Adding comments to ekf.

1 files changed, 81 insertions, 36 deletions

diff --git a/apps/examples/kalman_demo/KalmanNav.hpp b/apps/examples/kalman_demo/KalmanNav.hpp
index c074cb7c3..af4588e20 100644
--- a/apps/examples/kalman_demo/KalmanNav.hpp
+++ b/apps/examples/kalman_demo/KalmanNav.hpp
@@ -68,53 +68,98 @@
 class KalmanNav : public control::SuperBlock
 {
 public:
+	/**
+	 * Constructor
+	 */
 	KalmanNav(SuperBlock *parent, const char *name);
+
+	/**
+	 * Deconstuctor
+	 */
+
 	virtual ~KalmanNav() {};
+	/**
+	 * The main callback function for the class
+	 */
 	void update();
+
+	/**
+	 * Fast prediction
+	 * Contains: state integration
+	 */
 	virtual void updatePublications();
 	void predictFast(float dt);
+
+	/**
+	 * Slow prediction
+	 * Contains: covariance integration
+	 */
 	void predictSlow(float dt);
+
+	/**
+	 * Attitude correction
+	 */
 	void correctAtt();
+
+	/**
+	 * Position correction
+	 */
 	void correctPos();
+
+	/**
+	 * Overloaded update parameters
+	 */
 	virtual void updateParams();
 protected:
-	math::Matrix F;
-	math::Matrix G;
-	math::Matrix P;
-	math::Matrix P0;
-	math::Matrix V;
-	math::Matrix HAtt;
-	math::Matrix RAtt;
-	math::Matrix HPos;
-	math::Matrix RPos;
-	math::Dcm C_nb;
-	math::Quaternion q;
-	control::UOrbSubscription<sensor_combined_s> _sensors;
-	control::UOrbSubscription<vehicle_gps_position_s> _gps;
-	control::UOrbSubscription<parameter_update_s> _param_update;
-	control::UOrbPublication<vehicle_global_position_s> _pos;
-	control::UOrbPublication<vehicle_attitude_s> _att;
-	uint64_t _pubTimeStamp;
-	uint64_t _fastTimeStamp;
-	uint64_t _slowTimeStamp;
-	uint64_t _attTimeStamp;
-	uint64_t _outTimeStamp;
-	uint16_t _navFrames;
-	uint16_t _missFast;
-	uint16_t _missSlow;
-	float fN, fE, fD;
+	// kalman filter
+	math::Matrix F;             /**< Jacobian(f,x), where dx/dt = f(x,u) */
+	math::Matrix G;             /**< noise shaping matrix for gyro/accel */
+	math::Matrix P;             /**< state covariance matrix */
+	math::Matrix P0;            /**< initial state covariance matrix */
+	math::Matrix V;             /**< gyro/ accel noise matrix */
+	math::Matrix HAtt;          /**< attitude measurement matrix */
+	math::Matrix RAtt;          /**< attitude measurement noise matrix */
+	math::Matrix HPos;          /**< position measurement jacobian matrix */
+	math::Matrix RPos;          /**< position measurement noise matrix */
+	// attitude
+	math::Dcm C_nb;             /**< direction cosine matrix from body to nav frame */
+	math::Quaternion q;         /**< quaternion from body to nav frame */
+	// subscriptions
+	control::UOrbSubscription<sensor_combined_s> _sensors;          /**< sensors sub. */
+	control::UOrbSubscription<vehicle_gps_position_s> _gps;         /**< gps sub. */
+	control::UOrbSubscription<parameter_update_s> _param_update;    /**< parameter update sub. */
+	// publications
+	control::UOrbPublication<vehicle_global_position_s> _pos;       /**< position pub. */
+	control::UOrbPublication<vehicle_attitude_s> _att;              /**< attitude pub. */
+	// time stamps
+	uint64_t _pubTimeStamp;     /**< output data publication time stamp */
+	uint64_t _fastTimeStamp;    /**< fast prediction time stamp */
+	uint64_t _slowTimeStamp;    /**< slow prediction time stamp */
+	uint64_t _attTimeStamp;     /**< attitude correction time stamp */
+	uint64_t _outTimeStamp;     /**< output time stamp */
+	// frame count
+	uint16_t _navFrames;        /**< navigation frames completed in output cycle */
+	// miss counts
+	uint16_t _missFast;         /**< number of times fast prediction loop missed */
+	uint16_t _missSlow;         /**< number of times slow prediction loop missed */
+	// accelerations
+	float fN, fE, fD;           /**< navigation frame acceleration */
 	// states
-	enum {PHI = 0, THETA, PSI, VN, VE, VD, LAT, LON, ALT};
-	float phi, theta, psi;
-	float vN, vE, vD;
-	double lat, lon, alt;
-	control::BlockParam<float> _vGyro;
-	control::BlockParam<float> _vAccel;
-	control::BlockParam<float> _rMag;
-	control::BlockParam<float> _rGpsVel;
-	control::BlockParam<float> _rGpsPos;
-	control::BlockParam<float> _rGpsAlt;
-	control::BlockParam<float> _rAccel;
+	enum {PHI = 0, THETA, PSI, VN, VE, VD, LAT, LON, ALT};  /**< state enumeration */
+	float phi, theta, psi;                  /**< 3-2-1 euler angles */
+	float vN, vE, vD;                       /**< navigation velocity, m/s */
+	double lat, lon, alt;                   /**< lat, lon, alt, radians */
+	// parameters
+	control::BlockParam<float> _vGyro;      /**< gyro process noise */
+	control::BlockParam<float> _vAccel;     /**< accelerometer process noise  */
+	control::BlockParam<float> _rMag;       /**< magnetometer measurement noise  */
+	control::BlockParam<float> _rGpsVel;    /**< gps velocity measurement noise */
+	control::BlockParam<float> _rGpsPos;    /**< gps position measurement noise */
+	control::BlockParam<float> _rGpsAlt;    /**< gps altitude measurement noise */
+	control::BlockParam<float> _rAccel;     /**< accelerometer measurement noise */
+	control::BlockParam<float> _magDip;     /**< magnetic inclination with level */
+	control::BlockParam<float> _magDec;     /**< magnetic declination, clockwise rotation */
+	// accessors
 	int32_t getLatDegE7() { return int32_t(lat * 1.0e7 * M_RAD_TO_DEG); }
 	void setLatDegE7(int32_t val) { lat = val / 1.0e7 / M_RAD_TO_DEG; }
 	int32_t getLonDegE7() { return int32_t(lon * 1.0e7 * M_RAD_TO_DEG); }