Added pressure alt measurement to kalman demo.

3 files changed, 18 insertions, 8 deletions

diff --git a/apps/examples/kalman_demo/KalmanNav.cpp b/apps/examples/kalman_demo/KalmanNav.cpp
index 955e77b3e..6df454a55 100644
--- a/apps/examples/kalman_demo/KalmanNav.cpp
+++ b/apps/examples/kalman_demo/KalmanNav.cpp
@@ -61,8 +61,8 @@ KalmanNav::KalmanNav(SuperBlock *parent, const char *name) :
 	HAtt(6, 9),
 	RAtt(6, 6),
 	// position measurement ekf matrices
-	HPos(5, 9),
-	RPos(5, 5),
+	HPos(6, 9),
+	RPos(6, 6),
 	// attitude representations
 	C_nb(),
 	q(),
@@ -95,6 +95,7 @@ KalmanNav::KalmanNav(SuperBlock *parent, const char *name) :
 	_rGpsVel(this, "R_GPS_VEL"),
 	_rGpsPos(this, "R_GPS_POS"),
 	_rGpsAlt(this, "R_GPS_ALT"),
+	_rPressAlt(this, "R_PRESS_ALT"),
 	_rAccel(this, "R_ACCEL"),
 	_magDip(this, "ENV_MAG_DIP"),
 	_magDec(this, "ENV_MAG_DEC"),
@@ -134,6 +135,7 @@ KalmanNav::KalmanNav(SuperBlock *parent, const char *name) :
 	HPos(2, 6) = 1.0e7f * M_RAD_TO_DEG_F;
 	HPos(3, 7) = 1.0e7f * M_RAD_TO_DEG_F;
 	HPos(4, 8) = 1.0f;
+	HPos(5, 8) = 1.0f;
 
 	// initialize all parameters
 	updateParams();
@@ -192,7 +194,7 @@ void KalmanNav::update()
 	    gpsUpdate &&
 	    _gps.fix_type > 2
 	    //&& _gps.counter_pos_valid > 10
-	    ) {
+	   ) {
 		vN = _gps.vel_n_m_s;
 		vE = _gps.vel_e_m_s;
 		vD = _gps.vel_d_m_s;
@@ -630,12 +632,13 @@ int KalmanNav::correctPos()
 	using namespace math;
 
 	// residual
-	Vector y(5);
+	Vector y(6);
 	y(0) = _gps.vel_n_m_s - vN;
 	y(1) = _gps.vel_e_m_s - vE;
 	y(2) = double(_gps.lat) - lat * 1.0e7 * M_RAD_TO_DEG;
 	y(3) = double(_gps.lon) - lon * 1.0e7 * M_RAD_TO_DEG;
 	y(4) = double(_gps.alt) / 1.0e3 - alt;
+	y(5) = double(_sensors.baro_alt_meter) - alt;
 
 	// compute correction
 	// http://en.wikipedia.org/wiki/Extended_Kalman_filter
@@ -685,7 +688,8 @@ int KalmanNav::correctPos()
 		       double(y(1) / sqrtf(RPos(1, 1))),
 		       double(y(2) / sqrtf(RPos(2, 2))),
 		       double(y(3) / sqrtf(RPos(3, 3))),
-		       double(y(4) / sqrtf(RPos(4, 4))));
+		       double(y(4) / sqrtf(RPos(4, 4))),
+		       double(y(5) / sqrtf(RPos(5, 5))));
 	}
 
 	return ret_ok;
@@ -740,7 +744,8 @@ void KalmanNav::updateParams()
 	float noiseVel = _rGpsVel.get();
 	float noiseLatDegE7 = 1.0e7f * M_RAD_TO_DEG_F * _rGpsPos.get() / R;
 	float noiseLonDegE7 = noiseLatDegE7 / cosLSing;
-	float noiseAlt = _rGpsAlt.get();
+	float noiseGpsAlt = _rGpsAlt.get();
+	float noisePressAlt = _rPressAlt.get();
 
 	// bound noise to prevent singularities
 	if (noiseVel < noiseMin) noiseVel = noiseMin;
@@ -749,13 +754,16 @@ void KalmanNav::updateParams()
 
 	if (noiseLonDegE7 < noiseMin) noiseLonDegE7 = noiseMin;
 
-	if (noiseAlt < noiseMin) noiseAlt = noiseMin;
+	if (noiseGpsAlt < noiseMin) noiseGpsAlt = noiseMin;
+
+	if (noisePressAlt < noiseMin) noisePressAlt = noiseMin;
 
 	RPos(0, 0) = noiseVel * noiseVel; // vn
 	RPos(1, 1) = noiseVel * noiseVel; // ve
 	RPos(2, 2) = noiseLatDegE7 * noiseLatDegE7; // lat
 	RPos(3, 3) = noiseLonDegE7 * noiseLonDegE7; // lon
-	RPos(4, 4) = noiseAlt * noiseAlt; // h
+	RPos(4, 4) = noiseGpsAlt * noiseGpsAlt; // h
+	RPos(5, 5) = noisePressAlt * noisePressAlt; // h
 	// XXX, note that RPos depends on lat, so updateParams should
 	// be called if lat changes significantly
 }
diff --git a/apps/examples/kalman_demo/KalmanNav.hpp b/apps/examples/kalman_demo/KalmanNav.hpp
index 7709ac697..c2bf18115 100644
--- a/apps/examples/kalman_demo/KalmanNav.hpp
+++ b/apps/examples/kalman_demo/KalmanNav.hpp
@@ -159,6 +159,7 @@ protected:
 	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> _rPressAlt;  /**< press 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 */
diff --git a/apps/examples/kalman_demo/params.c b/apps/examples/kalman_demo/params.c
index 3bfe01261..91e80a5ea 100644
--- a/apps/examples/kalman_demo/params.c
+++ b/apps/examples/kalman_demo/params.c
@@ -7,6 +7,7 @@ PARAM_DEFINE_FLOAT(KF_R_MAG, 1.0f);
 PARAM_DEFINE_FLOAT(KF_R_GPS_VEL, 1.0f);
 PARAM_DEFINE_FLOAT(KF_R_GPS_POS, 5.0f);
 PARAM_DEFINE_FLOAT(KF_R_GPS_ALT, 5.0f);
+PARAM_DEFINE_FLOAT(KF_R_PRESS_ALT, 0.1f);
 PARAM_DEFINE_FLOAT(KF_R_ACCEL, 1.0f);
 PARAM_DEFINE_FLOAT(KF_FAULT_POS, 10.0f);
 PARAM_DEFINE_FLOAT(KF_FAULT_ATT, 10.0f);