Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-02-23 16:23:56 -04:00
Code Issues Packages Projects Releases Wiki Activity

AP_NavEKF2: Update function header comments

Browse Source
This commit is contained in:
Paul Riseborough 2015-10-18 10:32:11 +11:00 committed by Andrew Tridgell
parent 6899767d28
commit 1b8a93ef0c
4 changed files with 50 additions and 14 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

12
libraries/AP_NavEKF2/AP_NavEKF2_AirDataFusion.cpp
View File

@ -26,7 +26,11 @@ extern const AP_HAL::HAL& hal;
* FUSE MEASURED_DATA *
********************************************************/
// fuse true airspeed measurements
/*
* Fuse true airspeed measurements using explicit algebraic equations generated with Matlab symbolic toolbox.
* The script file used to generate these and other equations in this filter can be found here:
* https://github.com/priseborough/InertialNav/blob/master/derivations/RotationVectorAttitudeParameterisation/GenerateNavFilterEquations.m
*/
void NavEKF2_core::FuseAirspeed()
{
// start performance timer
@ -273,7 +277,11 @@ void NavEKF2_core::SelectBetaFusion()
}
}
// fuse sythetic sideslip measurement of zero
/*
* Fuse sythetic sideslip measurement of zero using explicit algebraic equations generated with Matlab symbolic toolbox.
* The script file used to generate these and other equations in this filter can be found here:
* https://github.com/priseborough/InertialNav/blob/master/derivations/RotationVectorAttitudeParameterisation/GenerateNavFilterEquations.m
*/
void NavEKF2_core::FuseSideslip()
{
// start performance timer

13
libraries/AP_NavEKF2/AP_NavEKF2_MagFusion.cpp
View File

@ -148,8 +148,11 @@ void NavEKF2_core::SelectMagFusion()
perf_end(_perf_FuseMagnetometer);
}
// fuse magnetometer measurements and apply innovation consistency checks
// fuse each axis on consecutive time steps to spread computional load
/*
* Fuse magnetometer measurements using explicit algebraic equations generated with Matlab symbolic toolbox.
* The script file used to generate these and other equations in this filter can be found here:
* https://github.com/priseborough/InertialNav/blob/master/derivations/RotationVectorAttitudeParameterisation/GenerateNavFilterEquations.m
*/
void NavEKF2_core::FuseMagnetometer()
{
// declarations
@ -511,7 +514,11 @@ void NavEKF2_core::FuseMagnetometer()
}
// Fuse compass measurements usinga simple declination observation model that doesn't use magnetic field states
/*
* Fuse compass measurements using explicit algebraic equations generated with Matlab symbolic toolbox.
* The script file used to generate these and other equations in this filter can be found here:
* https://github.com/priseborough/InertialNav/blob/master/derivations/RotationVectorAttitudeParameterisation/GenerateNavFilterEquations.m
*/
void NavEKF2_core::fuseCompass()
{
float q0 = stateStruct.quat[0];

6
libraries/AP_NavEKF2/AP_NavEKF2_OptFlowFusion.cpp
View File

@ -275,8 +275,10 @@ void NavEKF2_core::EstimateTerrainOffset()
}
/*
Fuse angular motion compensated optical flow rates into the main filter.
Requires a valid terrain height estimate.
* Fuse angular motion compensated optical flow rates using explicit algebraic equations generated with Matlab symbolic toolbox.
* The script file used to generate these and other equations in this filter can be found here:
* https://github.com/priseborough/InertialNav/blob/master/derivations/RotationVectorAttitudeParameterisation/GenerateNavFilterEquations.m
* Requires a valid terrain height estimate.
*/
void NavEKF2_core::FuseOptFlow()
{

29
libraries/AP_NavEKF2/AP_NavEKF2_core.cpp
View File

@ -393,8 +393,13 @@ void NavEKF2_core::UpdateFilter()
perf_end(_perf_UpdateFilter);
}
// update the quaternion, velocity and position states using delayed IMU measurements
// because the EKF is running on a delayed time horizon
/*
* Update the quaternion, velocity and position states using delayed IMU measurements
* because the EKF is running on a delayed time horizon. Note that the quaternion is
* not used by the EKF equations, which instead estimate the error in the attitude of
* the vehicle when each observtion is fused. This attitude error is then used to correct
* the quaternion.
*/
void NavEKF2_core::UpdateStrapdownEquationsNED()
{
Vector3f delVelNav; // delta velocity vector
@ -525,8 +530,18 @@ void NavEKF2_core::calcOutputStates() {
while (imuStoreAccessIndex != fifoIndexNow);
}
// Propagate PVA solution forward from the fusion time horizon to the current time horizon
// using simple observer. This also applies an LPF to fusion corrections
/*
* Propagate PVA solution forward from the fusion time horizon to the current time horizon
* using simple observer which performs two functions:
* 1) Corrects for the delayed time horizon used by the EKF.
* 2) Applies a LPF to state corrections to prevent 'stepping' in states due to measurement
* fusion introducing unwanted noise into the control loops.
* The inspiration for using a complementary filter to correct for time delays in the EKF
* is based on the work by A Khosravian.
*
* “Recursive Attitude Estimation in the Presence of Multi-rate and Multi-delay Vector Measurements”
* A Khosravian, J Trumpf, R Mahony, T Hamel, Australian National University
*/
void NavEKF2_core::calcOutputStatesFast() {
// Calculate strapdown solution at the current time horizon
@ -619,7 +634,11 @@ void NavEKF2_core::calcOutputStatesFast() {
}
// calculate the predicted state covariance matrix
/*
* Calculate the predicted state covariance matrix using algebraic equations generated with Matlab symbolic toolbox.
* The script file used to generate these and otehr equations in this filter can be found here:
* https://github.com/priseborough/InertialNav/blob/master/derivations/RotationVectorAttitudeParameterisation/GenerateNavFilterEquations.m
*/
void NavEKF2_core::CovariancePrediction()
{
perf_begin(_perf_CovariancePrediction);