Adding comments to ekf.

2 files changed, 102 insertions, 54 deletions

diff --git a/apps/examples/kalman_demo/KalmanNav.cpp b/apps/examples/kalman_demo/KalmanNav.cpp
index a735de406..5f71da58e 100644
--- a/apps/examples/kalman_demo/KalmanNav.cpp
+++ b/apps/examples/kalman_demo/KalmanNav.cpp
@@ -82,8 +82,9 @@ KalmanNav::KalmanNav(SuperBlock *parent, const char *name) :
 	// miss counts
 	_missFast(0),
 	_missSlow(0),
-	// state
+	// accelerations
 	fN(0), fE(0), fD(0),
+	// state
 	phi(0), theta(0), psi(0),
 	vN(0), vE(0), vD(0),
 	lat(0), lon(0), alt(0),
@@ -94,13 +95,15 @@ KalmanNav::KalmanNav(SuperBlock *parent, const char *name) :
 	_rGpsVel(this, "R_GPS_VEL"),
 	_rGpsPos(this, "R_GPS_POS"),
 	_rGpsAlt(this, "R_GPS_ALT"),
-	_rAccel(this, "R_ACCEL")
+	_rAccel(this, "R_ACCEL"),
+	_magDip(this, "MAG_DIP"),
+	_magDec(this, "MAG_DEC")
 {
 	using namespace math;
 
 	// initial state covariance matrix
 	P0 = Matrix::identity(9) * 1.0f;
-    P = P0;
+	P = P0;
 
 	// wait for gps lock
 	while (1) {
@@ -132,12 +135,12 @@ KalmanNav::KalmanNav(SuperBlock *parent, const char *name) :
 	setLonDegE7(_gps.lon);
 	setAltE3(_gps.alt);
 
-    printf("[kalman_demo] initializing EKF state with GPS\n");
-    printf("phi: %8.4f, theta: %8.4f, psi: %8.4f\n",
-            double(phi), double(theta), double(psi));
-    printf("vN: %8.4f, vE: %8.4f, vD: %8.4f, lat: %8.4f, lon: %8.4f, alt: %8.4f\n",
-            double(vN), double(vE), double(vD),
-            lat, lon, alt);
+	printf("[kalman_demo] initializing EKF state with GPS\n");
+	printf("phi: %8.4f, theta: %8.4f, psi: %8.4f\n",
+	       double(phi), double(theta), double(psi));
+	printf("vN: %8.4f, vE: %8.4f, vD: %8.4f, lat: %8.4f, lon: %8.4f, alt: %8.4f\n",
+	       double(vN), double(vE), double(vD),
+	       lat, lon, alt);
 
 	// initialize quaternions
 	q = Quaternion(EulerAngles(phi, theta, psi));
@@ -342,10 +345,10 @@ void KalmanNav::predictFast(float dt)
 		      vN * LDot + g;
 	float vEDot = fE + vN * rotRate * sinL +
 		      vDDot * rotRate * cosL;
-    printf("vNDot: %8.4f, vEDot: %8.4f, vDDot: %8.4f\n",
-            double(vNDot), double(vEDot), double(vDDot));
-    printf("LDot: %8.4f, lDot: %8.4f, rotRate: %8.4f\n",
-            double(LDot), double(lDot), double(rotRate));
+	printf("vNDot: %8.4f, vEDot: %8.4f, vDDot: %8.4f\n",
+	       double(vNDot), double(vEDot), double(vDDot));
+	printf("LDot: %8.4f, lDot: %8.4f, rotRate: %8.4f\n",
+	       double(LDot), double(lDot), double(rotRate));
 
 	// rectangular integration
 	//printf("dt: %8.4f\n", double(dt));
@@ -371,7 +374,7 @@ void KalmanNav::predictSlow(float dt)
 
 	// prepare for matrix
 	float R = R0 + float(alt);
-    float RSq = R*R;
+	float RSq = R * R;
 
 	// F Matrix
 	// Titterton pg. 291
@@ -472,8 +475,8 @@ void KalmanNav::correctAtt()
 	// mag predicted measurement
 	// choosing some typical magnetic field properties,
 	//  TODO dip/dec depend on lat/ lon/ time
-	static const float dip = 0.0f / M_RAD_TO_DEG_F; // dip, inclination with level
-	static const float dec = 0.0f / M_RAD_TO_DEG_F; // declination, clockwise rotation from north
+	static const float dip = _magDip.get() / M_RAD_TO_DEG_F; // dip, inclination with level
+	static const float dec = _magDec.get() / M_RAD_TO_DEG_F; // declination, clockwise rotation from north
 	float bN = cosf(dip) * cosf(dec);
 	float bE = cosf(dip) * sinf(dec);
 	float bD = sinf(dip);
@@ -593,7 +596,7 @@ void KalmanNav::correctAtt()
 	P = P - K * HAtt * P;
 
 	// fault detection
-	float beta = y.dot(S.inverse()*y);
+	float beta = y.dot(S.inverse() * y);
 
 	if (beta > 1000.0f) {
 		printf("fault in attitude: beta = %8.4f\n", (double)beta);
@@ -671,7 +674,7 @@ void KalmanNav::correctPos()
 	P = P - K * HPos * P;
 
 	// fault detetcion
-	float beta = y.dot(S.inverse()*y);
+	float beta = y.dot(S.inverse() * y);
 
 	if (beta > 1000.0f) {
 		printf("fault in gps: beta = %8.4f\n", (double)beta);
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); }