AP_NavEKF2: Update function header comments

libraries/AP_NavEKF2/AP_NavEKF2_AirDataFusion.cpp

@@ -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
@@ -461,4 +469,4 @@ void NavEKF2_core::FuseSideslip()
 ********************************************************/
 
 
-#endif // HAL_CPU_CLASS
+#endif // HAL_CPU_CLASS

libraries/AP_NavEKF2/AP_NavEKF2_MagFusion.cpp

@@ -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];
@@ -675,4 +682,4 @@ void NavEKF2_core::alignMagStateDeclination()
 }
 
 
-#endif // HAL_CPU_CLASS
+#endif // HAL_CPU_CLASS

libraries/AP_NavEKF2/AP_NavEKF2_OptFlowFusion.cpp

@@ -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()
 {

libraries/AP_NavEKF2/AP_NavEKF2_core.cpp

@@ -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);