Rebase of changes to sensor_hil_fixedwing branch.

apps/examples/control_demo/control_demo.cpp

@@ -108,7 +108,7 @@ int control_demo_main(int argc, char *argv[])
 		deamon_task = task_spawn("control_demo",
 					 SCHED_DEFAULT,
 					 SCHED_PRIORITY_MAX - 10,
-					 4096,
+					 5120,
 					 control_demo_thread_main,
 					 (argv) ? (const char **)&argv[2] : (const char **)NULL);
 		exit(0);

apps/examples/control_demo/params.c

@@ -6,31 +6,31 @@
 // 16 is max name length
 
 // gyro low pass filter
-PARAM_DEFINE_FLOAT(FWB_P_LP, 10.0f); // roll rate low pass cut freq
-PARAM_DEFINE_FLOAT(FWB_Q_LP, 10.0f); // pitch rate low pass cut freq
-PARAM_DEFINE_FLOAT(FWB_R_LP, 10.0f); // yaw rate low pass cut freq
+PARAM_DEFINE_FLOAT(FWB_P_LP, 300.0f); // roll rate low pass cut freq
+PARAM_DEFINE_FLOAT(FWB_Q_LP, 300.0f); // pitch rate low pass cut freq
+PARAM_DEFINE_FLOAT(FWB_R_LP, 300.0f); // yaw rate low pass cut freq
 
 // yaw washout
 PARAM_DEFINE_FLOAT(FWB_R_HP, 1.0f); // yaw rate high pass
 
 // stabilization mode
-PARAM_DEFINE_FLOAT(FWB_P2AIL, 0.1f); // roll rate 2 aileron
-PARAM_DEFINE_FLOAT(FWB_Q2ELV, 0.1f); // pitch rate 2 elevator
-PARAM_DEFINE_FLOAT(FWB_R2RDR, 0.1f); // yaw rate 2 rudder
+PARAM_DEFINE_FLOAT(FWB_P2AIL, 0.5f); // roll rate 2 aileron
+PARAM_DEFINE_FLOAT(FWB_Q2ELV, 0.5f); // pitch rate 2 elevator
+PARAM_DEFINE_FLOAT(FWB_R2RDR, 0.2f); // yaw rate 2 rudder
 
 //  psi -> phi -> p
-PARAM_DEFINE_FLOAT(FWB_PSI2PHI, 2.0f);      // heading 2 roll
-PARAM_DEFINE_FLOAT(FWB_PHI2P, 2.0f);        // roll to roll rate
-PARAM_DEFINE_FLOAT(FWB_PHI_LIM_MAX, 1.0f);  // roll limit
+PARAM_DEFINE_FLOAT(FWB_PSI2PHI, 0.5f);      // heading 2 roll
+PARAM_DEFINE_FLOAT(FWB_PHI2P, 1.0f);        // roll to roll rate
+PARAM_DEFINE_FLOAT(FWB_PHI_LIM_MAX, 0.5f);  // roll limit, 28 deg
 
 // velocity -> theta
-PARAM_DEFINE_FLOAT(FWB_V2THE_P, 0.5f);
+PARAM_DEFINE_FLOAT(FWB_V2THE_P, 0.2f);
 PARAM_DEFINE_FLOAT(FWB_V2THE_I, 0.0f);
 PARAM_DEFINE_FLOAT(FWB_V2THE_D, 0.0f);
 PARAM_DEFINE_FLOAT(FWB_V2THE_D_LP, 0.0f);
 PARAM_DEFINE_FLOAT(FWB_V2THE_I_MAX, 0.0f);
-PARAM_DEFINE_FLOAT(FWB_THE_MIN, -1.0f);
-PARAM_DEFINE_FLOAT(FWB_THE_MAX, 1.0f);
+PARAM_DEFINE_FLOAT(FWB_THE_MIN, -0.5f);
+PARAM_DEFINE_FLOAT(FWB_THE_MAX, 0.5f);
 
 
 // theta -> q
@@ -41,15 +41,15 @@ PARAM_DEFINE_FLOAT(FWB_THE2Q_D_LP, 0.0f);
 PARAM_DEFINE_FLOAT(FWB_THE2Q_I_MAX, 0.0f);
 
 //  h -> thr
-PARAM_DEFINE_FLOAT(FWB_H2THR_P, 0.005f);
-PARAM_DEFINE_FLOAT(FWB_H2THR_I, 0.001f);
-PARAM_DEFINE_FLOAT(FWB_H2THR_D, 0.01f);
-PARAM_DEFINE_FLOAT(FWB_H2THR_D_LP, 1.0f);
-PARAM_DEFINE_FLOAT(FWB_H2THR_I_MAX, 250.0f);
+PARAM_DEFINE_FLOAT(FWB_H2THR_P, 0.01f);
+PARAM_DEFINE_FLOAT(FWB_H2THR_I, 0.0f);
+PARAM_DEFINE_FLOAT(FWB_H2THR_D, 0.0f);
+PARAM_DEFINE_FLOAT(FWB_H2THR_D_LP, 0.0f);
+PARAM_DEFINE_FLOAT(FWB_H2THR_I_MAX, 0.0f);
 
 // crosstrack
-PARAM_DEFINE_FLOAT(FWB_XT2YAW_MAX, 1.0f);
-PARAM_DEFINE_FLOAT(FWB_XT2YAW, 0.01f);
+PARAM_DEFINE_FLOAT(FWB_XT2YAW_MAX, 1.57f); // 90 deg
+PARAM_DEFINE_FLOAT(FWB_XT2YAW, 0.002f);
 
 // speed command
 PARAM_DEFINE_FLOAT(FWB_V_MIN, 20.0f);
@@ -58,6 +58,6 @@ PARAM_DEFINE_FLOAT(FWB_V_MAX, 24.0f);
 
 // trim
 PARAM_DEFINE_FLOAT(FWB_TRIM_AIL, 0.0f);
-PARAM_DEFINE_FLOAT(FWB_TRIM_ELV, 0.0f);
+PARAM_DEFINE_FLOAT(FWB_TRIM_ELV, 0.005f);
 PARAM_DEFINE_FLOAT(FWB_TRIM_RDR, 0.0f);
-PARAM_DEFINE_FLOAT(FWB_TRIM_THR, 0.7f);
+PARAM_DEFINE_FLOAT(FWB_TRIM_THR, 0.81f);

apps/examples/kalman_demo/KalmanNav.cpp

@@ -42,10 +42,12 @@
 #include "KalmanNav.hpp"
 
 // constants
+// Titterton pg. 52
 static const float omega = 7.2921150e-5f; // earth rotation rate, rad/s
-static const float R = 6.371000e6f; // earth radius, m
-static const float RSq = 4.0589641e13f; // radius squared
-static const float g = 9.8f; // gravitational accel. m/s^2
+static const float R0 = 6378137.0f; // earth radius, m
+
+static const float RSq = 4.0680631591e+13; // radius squared
+static const float g = 9.806f; // gravitational accel. m/s^2, XXX should be calibrated
 
 KalmanNav::KalmanNav(SuperBlock *parent, const char *name) :
 	SuperBlock(parent, name),
@@ -57,15 +59,15 @@ KalmanNav::KalmanNav(SuperBlock *parent, const char *name) :
 	// attitude measurement ekf matrices
 	HAtt(6, 9),
 	RAtt(6, 6),
-	// gps measurement ekf matrices
-	HGps(6, 9),
-	RGps(6, 6),
+	// position measurement ekf matrices
+	HPos(5, 9),
+	RPos(5, 5),
 	// attitude representations
 	C_nb(),
 	q(),
 	// subscriptions
-	_sensors(&getSubscriptions(), ORB_ID(sensor_combined), 5), // limit to 200 Hz
-	_gps(&getSubscriptions(), ORB_ID(vehicle_gps_position), 1000), // limit to 1 Hz
+	_sensors(&getSubscriptions(), ORB_ID(sensor_combined), 1), // limit to 1000 Hz
+	_gps(&getSubscriptions(), ORB_ID(vehicle_gps_position), 100), // limit to 10 Hz
 	_param_update(&getSubscriptions(), ORB_ID(parameter_update), 1000), // limit to 1 Hz
 	// publications
 	_pos(&getPublications(), ORB_ID(vehicle_global_position)),
@@ -78,6 +80,9 @@ KalmanNav::KalmanNav(SuperBlock *parent, const char *name) :
 	_outTimeStamp(hrt_absolute_time()),
 	// frame count
 	_navFrames(0),
+	// miss counts
+	_missFast(0),
+	_missSlow(0),
 	// state
 	fN(0), fE(0), fD(0),
 	phi(0), theta(0), psi(0),
@@ -87,8 +92,8 @@ KalmanNav::KalmanNav(SuperBlock *parent, const char *name) :
 	_vGyro(this, "V_GYRO"),
 	_vAccel(this, "V_ACCEL"),
 	_rMag(this, "R_MAG"),
-	_rGpsV(this, "R_GPS_V"),
-	_rGpsGeo(this, "R_GPS_GEO"),
+	_rGpsVel(this, "R_GPS_VEL"),
+	_rGpsPos(this, "R_GPS_POS"),
 	_rGpsAlt(this, "R_GPS_ALT"),
 	_rAccel(this, "R_ACCEL")
 {
@@ -99,12 +104,21 @@ KalmanNav::KalmanNav(SuperBlock *parent, const char *name) :
 
 	// wait for gps lock
 	while (1) {
-		updateSubscriptions();
+		struct pollfd fds[1];
+		fds[0].fd = _gps.getHandle();
+		fds[0].events = POLLIN;
 
-		if (_gps.fix_type > 2) break;
+		// poll 10 seconds for new data
+		int ret = poll(fds, 1, 10000);
 
-		printf("[kalman_demo] waiting for gps lock\n");
-		usleep(1000000);
+		if (ret > 0)  {
+			updateSubscriptions();
+
+			if (_gps.fix_type > 2) break;
+
+		} else if (ret == 0) {
+			printf("[kalman_demo] waiting for gps lock\n");
+		}
 	}
 
 	// initial state
@@ -124,16 +138,12 @@ KalmanNav::KalmanNav(SuperBlock *parent, const char *name) :
 	// initialize dcm
 	C_nb = Dcm(q);
 
-	// initialize F to identity
-	F = Matrix::identity(9);
-
-	// HGps is constant
-	HGps(0, 3) = 1.0f;
-	HGps(1, 4) = 1.0f;
-	HGps(2, 5) = 1.0f;
-	HGps(3, 6) = 1.0f;
-	HGps(4, 7) = 1.0f;
-	HGps(5, 8) = 1.0f;
+	// HPos is constant
+	HPos(0, 3) = 1.0f;
+	HPos(1, 4) = 1.0f;
+	HPos(2, 6) = 1.0f;
+	HPos(3, 7) = 1.0f;
+	HPos(4, 8) = 1.0f;
 
 	// initialize all parameters
 	updateParams();
@@ -143,11 +153,13 @@ void KalmanNav::update()
 {
 	using namespace math;
 
-	struct pollfd fds[2];
+	struct pollfd fds[3];
 	fds[0].fd = _sensors.getHandle();
 	fds[0].events = POLLIN;
 	fds[1].fd = _param_update.getHandle();
 	fds[1].events = POLLIN;
+	fds[2].fd = _gps.getHandle();
+	fds[2].events = POLLIN;
 
 	// poll 20 milliseconds for new data
 	int ret = poll(fds, 2, 20);
@@ -158,53 +170,55 @@ void KalmanNav::update()
 		return;
 
 	} else if (ret == 0) { // timeout
-		// run anyway
-	} else if (ret > 0) {
-		// update params when requested
-		if (fds[1].revents & POLLIN) {
-			printf("updating params\n");
-			updateParams();
-		}
-
-		// if no new sensor data, return
-		if (!(fds[0].revents & POLLIN)) return;
+		return;
 	}
 
 	// get new timestamp
 	uint64_t newTimeStamp = hrt_absolute_time();
 
 	// check updated subscriptions
+	if (_param_update.updated()) updateParams();
+
 	bool gpsUpdate = _gps.updated();
+	bool sensorsUpdate = _sensors.updated();
 
 	// get new information from subscriptions
 	// this clears update flag
 	updateSubscriptions();
 
-	// count fast frames
-	_navFrames += 1;
+	// abort update if no new data
+	if (!(sensorsUpdate || gpsUpdate)) return;
 
 	// fast prediciton step
 	// note, using sensors timestamp so we can account
 	// for packet lag
 	float dtFast = (_sensors.timestamp - _fastTimeStamp) / 1.0e6f;
 	_fastTimeStamp = _sensors.timestamp;
-	predictFast(dtFast);
+
+	if (dtFast < 1.0f / 50) {
+		predictFast(dtFast);
+		// count fast frames
+		_navFrames += 1;
+
+	} else _missFast++;
 
 	// slow prediction step
 	float dtSlow = (_sensors.timestamp - _slowTimeStamp) / 1.0e6f;
 
-	if (dtSlow > 1.0f / 200) { // 200 Hz
+	if (dtSlow > 1.0f / 100) { // 100 Hz
 		_slowTimeStamp = _sensors.timestamp;
-		predictSlow(dtSlow);
+
+		if (dtSlow < 1.0f / 50) predictSlow(dtSlow);
+		else _missSlow ++;
 	}
 
 	// gps correction step
 	if (gpsUpdate) {
-		correctGps();
+		correctPos();
 	}
 
 	// attitude correction step
-	if (_sensors.timestamp - _attTimeStamp > 1e6 / 1) { // 1 Hz
+	if (_sensors.timestamp - _attTimeStamp > 1e6 / 20) { // 20 Hz
 		_attTimeStamp = _sensors.timestamp;
 		correctAtt();
 	}
@@ -218,8 +232,10 @@ void KalmanNav::update()
 	// output
 	if (newTimeStamp - _outTimeStamp > 1e6) { // 1 Hz
 		_outTimeStamp = newTimeStamp;
-		printf("nav: %4d Hz\n", _navFrames);
+		printf("nav: %4d Hz, misses fast: %4d slow: %4d\n", _navFrames, _missFast, _missSlow);
 		_navFrames = 0;
+		_missFast = 0;
+		_missSlow = 0;
 	}
 }
 
@@ -298,21 +314,28 @@ void KalmanNav::predictFast(float dt)
 	// trig
 	float sinL = sinf(lat);
 	float cosL = cosf(lat);
+	float cosLSing = cosf(lat);
+
+	if (fabsf(cosLSing) < 0.01f) cosLSing = 0.01f;
 
 	// position update
 	// neglects angular deflections in local gravity
 	// see Titerton pg. 70
-	float LDot = vN / (R + float(alt));
-	float lDot = vE / (cosL * (R + float(alt)));
-	float vNDot = fN - vE * (2 * omega +
-				 lDot) * sinL +
+	float R = R0 + float(alt);
+	float LDot = vN / R;
+	float lDot = vE / (cosLSing * R);
+	float rotRate = 2 * omega + lDot;
+	float vNDot = fN - vE * rotRate * sinL +
 		      vD * LDot;
-	float vDDot = fD - vE * (2 * omega + lDot) * cosL -
+	float vDDot = fD - vE * rotRate * cosL -
 		      vN * LDot + g;
-	float vEDot = fE + vN * (2 * omega + lDot) * sinL +
-		      vDDot * (2 * omega * cosL);
+	float vEDot = fE + vN * rotRate * sinL +
+		      vDDot * rotRate * cosL;
 
 	// rectangular integration
+	//printf("dt: %8.4f\n", double(dt));
+	//printf("fN: %8.4f, fE: %8.4f, fD: %8.4f\n", double(fN), double(fE), double(fD));
+	//printf("vN: %8.4f, vE: %8.4f, vD: %8.4f\n", double(vN), double(vE), double(vD));
 	vN += vNDot * dt;
 	vE += vEDot * dt;
 	vD += vDDot * dt;
@@ -331,87 +354,88 @@ void KalmanNav::predictSlow(float dt)
 	float cosLSq = cosL * cosL;
 	float tanL = tanf(lat);
 
+	// prepare for matrix
+	float R = R0 + float(alt);
+
 	// F Matrix
 	// Titterton pg. 291
-	//
-	// difference from Jacobian
-	// multiplity by dt for all elements
-	// add 1.0 to diagonal elements
 
-	F(0, 1) = (-(omega * sinL + vE * tanL / R)) * dt;
-	F(0, 2) = (vN / R) * dt;
-	F(0, 4) = (1.0f / R) * dt;
-	F(0, 6) = (-omega * sinL) * dt;
-	F(0, 8) = (-vE / RSq) * dt;
+	F(0, 1) = -(omega * sinL + vE * tanL / R);
+	F(0, 2) = vN / R;
+	F(0, 4) = 1.0f / R;
+	F(0, 6) = -omega * sinL;
+	F(0, 8) = -vE / RSq;
 
-	F(1, 0) = (omega * sinL + vE * tanL / R) * dt;
-	F(1, 2) = (omega * cosL + vE / R) * dt;
-	F(1, 3) = (-1.0f / R) * dt;
-	F(1, 8) = (vN / RSq) * dt;
+	F(1, 0) = omega * sinL + vE * tanL / R;
+	F(1, 2) = omega * cosL + vE / R;
+	F(1, 3) = -1.0f / R;
+	F(1, 8) = vN / RSq;
 
-	F(2, 0) = (-vN / R) * dt;
-	F(2, 1) = (-omega * cosL - vE / R) * dt;
-	F(2, 4) = (-tanL / R) * dt;
-	F(2, 6) = (-omega * cosL - vE / (R * cosLSq)) * dt;
-	F(2, 8) = (vE * tanL / RSq) * dt;
+	F(2, 0) = -vN / R;
+	F(2, 1) = -omega * cosL - vE / R;
+	F(2, 4) = -tanL / R;
+	F(2, 6) = -omega * cosL - vE / (R * cosLSq);
+	F(2, 8) = vE * tanL / RSq;
 
-	F(3, 1) = (-fD) * dt;
-	F(3, 2) = (fE) * dt;
-	F(3, 3) = 1.0f + (vD / R) * dt; // on diagonal
-	F(3, 4) = (-2 * (omega * sinL + vE * tanL / R)) * dt;
-	F(3, 5) = (vN / R) * dt;
-	F(3, 6) = (-vE * (2 * omega * cosL + vE / (R * cosLSq))) * dt;
-	F(3, 8) = ((vE * vE * tanL - vN * vD) / RSq) * dt;
+	F(3, 1) = -fD;
+	F(3, 2) = fE;
+	F(3, 3) = vD / R;
+	F(3, 4) = -2 * (omega * sinL + vE * tanL / R);
+	F(3, 5) = vN / R;
+	F(3, 6) = -vE * (2 * omega * cosL + vE / (R * cosLSq));
+	F(3, 8) = (vE * vE * tanL - vN * vD) / RSq;
 
-	F(4, 0) = (fD) * dt;
-	F(4, 2) = (-fN) * dt;
-	F(4, 3) = (2 * omega * sinL + vE * tanL / R) * dt;
-	F(4, 4) = 1.0f + ((vN * tanL + vD) / R) * dt; // on diagonal
-	F(4, 5) = (2 * omega * cosL + vE / R) * dt;
-	F(4, 6) = (2 * omega * (vN * cosL - vD * sinL) +
-		   vN * vE / (R * cosLSq)) * dt;
-	F(4, 8) = (-vE * (vN * tanL + vD) / RSq) * dt;
+	F(4, 0) = fD;
+	F(4, 2) = -fN;
+	F(4, 3) = 2 * omega * sinL + vE * tanL / R;
+	F(4, 4) = (vN * tanL + vD) / R;
+	F(4, 5) = 2 * omega * cosL + vE / R;
+	F(4, 6) = 2 * omega * (vN * cosL - vD * sinL) +
+		  vN * vE / (R * cosLSq);
+	F(4, 8) = -vE * (vN * tanL + vD) / RSq;
 
-	F(5, 0) = (-fE) * dt;
-	F(5, 1) = (fN) * dt;
-	F(5, 3) = (-2 * vN / R) * dt;
-	F(5, 4) = (-2 * (omega * cosL + vE / R)) * dt;
-	F(5, 6) = (2 * omega * vE * sinL) * dt;
-	F(5, 8) = ((vN * vN + vE * vE) / RSq) * dt;
+	F(5, 0) = -fE;
+	F(5, 1) = fN;
+	F(5, 3) = -2 * vN / R;
+	F(5, 4) = -2 * (omega * cosL + vE / R);
+	F(5, 6) = 2 * omega * vE * sinL;
+	F(5, 8) = (vN * vN + vE * vE) / RSq;
 
-	F(6, 3) = (1 / R) * dt;
-	F(6, 8) = (-vN / RSq) * dt;
+	F(6, 3) = 1 / R;
+	F(6, 8) = -vN / RSq;
 
-	F(7, 4) = (1 / (R * cosL)) * dt;
-	F(7, 6) = (vE * tanL / (R * cosL)) * dt;
-	F(7, 8) = (-vE / (cosL * RSq)) * dt;
+	F(7, 4) = 1 / (R * cosL);
+	F(7, 6) = vE * tanL / (R * cosL);
+	F(7, 8) = -vE / (cosL * RSq);
 
-	F(8, 5) = (-1) * dt;
+	F(8, 5) = -1;
 
 	// G Matrix
 	// Titterton pg. 291
-	G(0, 0) = -C_nb(0, 0) * dt;
-	G(0, 1) = -C_nb(0, 1) * dt;
-	G(0, 2) = -C_nb(0, 2) * dt;
-	G(1, 0) = -C_nb(1, 0) * dt;
-	G(1, 1) = -C_nb(1, 1) * dt;
-	G(1, 2) = -C_nb(1, 2) * dt;
-	G(2, 0) = -C_nb(2, 0) * dt;
-	G(2, 1) = -C_nb(2, 1) * dt;
-	G(2, 2) = -C_nb(2, 2) * dt;
+	G(0, 0) = -C_nb(0, 0);
+	G(0, 1) = -C_nb(0, 1);
+	G(0, 2) = -C_nb(0, 2);
+	G(1, 0) = -C_nb(1, 0);
+	G(1, 1) = -C_nb(1, 1);
+	G(1, 2) = -C_nb(1, 2);
+	G(2, 0) = -C_nb(2, 0);
+	G(2, 1) = -C_nb(2, 1);
+	G(2, 2) = -C_nb(2, 2);
 
-	G(3, 3) = C_nb(0, 0) * dt;
-	G(3, 4) = C_nb(0, 1) * dt;
-	G(3, 5) = C_nb(0, 2) * dt;
-	G(4, 3) = C_nb(1, 0) * dt;
-	G(4, 4) = C_nb(1, 1) * dt;
-	G(4, 5) = C_nb(1, 2) * dt;
-	G(5, 3) = C_nb(2, 0) * dt;
-	G(5, 4) = C_nb(2, 1) * dt;
-	G(5, 5) = C_nb(2, 2) * dt;
+	G(3, 3) = C_nb(0, 0);
+	G(3, 4) = C_nb(0, 1);
+	G(3, 5) = C_nb(0, 2);
+	G(4, 3) = C_nb(1, 0);
+	G(4, 4) = C_nb(1, 1);
+	G(4, 5) = C_nb(1, 2);
+	G(5, 3) = C_nb(2, 0);
+	G(5, 4) = C_nb(2, 1);
+	G(5, 5) = C_nb(2, 2);
 
-	// predict equations for kalman filter
-	P = F * P * F.transpose() + G * V * G.transpose();
+	// continuous predictioon equations
+	// for discrte time EKF
+	// http://en.wikipedia.org/wiki/Extended_Kalman_filter
+	P = P + (F * P + P * F.transpose() + G * V * G.transpose()) * dt;
 }
 
 void KalmanNav::correctAtt()
@@ -428,12 +452,11 @@ void KalmanNav::correctAtt()
 
 	// mag measurement
 	Vector3 zMag(_sensors.magnetometer_ga);
-	zMag = zMag.unit();
 
 	// mag predicted measurement
 	// choosing some typical magnetic field properties,
 	//  TODO dip/dec depend on lat/ lon/ time
-	static const float dip = 60.0f / M_RAD_TO_DEG_F; // dip, inclination with level
+	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
 	float bN = cosf(dip) * cosf(dec);
 	float bE = cosf(dip) * sinf(dec);
@@ -443,10 +466,29 @@ void KalmanNav::correctAtt()
 
 	// accel measurement
 	Vector3 zAccel(_sensors.accelerometer_m_s2);
+	float accelMag = zAccel.norm();
 	zAccel = zAccel.unit();
 
+	// ignore accel correction when accel mag not close to g
+	Matrix RAttAdjust = RAtt;
+
+	bool ignoreAccel = fabsf(accelMag - g) > 1.1f;
+
+	if (ignoreAccel) {
+		RAttAdjust(3, 3) = 1.0e10;
+		RAttAdjust(4, 4) = 1.0e10;
+		RAttAdjust(5, 5) = 1.0e10;
+
+	} else {
+		//printf("correcting attitude with accel\n");
+	}
+
+	// account for banked turn
+	// this would only work for fixed wing, so try to avoid
+	//Vector3 zCentrip = Vector3(0, cosf(phi), -sinf(phi))*g*tanf(phi);
+
 	// accel predicted measurement
-	Vector3 zAccelHat = (C_nb.transpose() * Vector3(0, 0, -1)).unit();
+	Vector3 zAccelHat = (C_nb.transpose() * Vector3(0, 0, -g) /*+ zCentrip*/).unit();
 
 	// combined measurement
 	Vector zAtt(6);
@@ -498,8 +540,9 @@ void KalmanNav::correctAtt()
 	HAtt(5, 1) = cosPhi * sinTheta;
 
 	// compute correction
+	// http://en.wikipedia.org/wiki/Extended_Kalman_filter
 	Vector y = zAtt - zAttHat; // residual
-	Matrix S = HAtt * P * HAtt.transpose() + RAtt; // residual covariance
+	Matrix S = HAtt * P * HAtt.transpose() + RAttAdjust; // residual covariance
 	Matrix K = P * HAtt.transpose() * S.inverse();
 	Vector xCorrect = K * y;
 
@@ -510,24 +553,34 @@ void KalmanNav::correctAtt()
 		if (isnan(val) || isinf(val)) {
 			// abort correction and return
 			printf("[kalman_demo] numerical failure in att correction\n");
+			// reset P matrix to identity
+			P = Matrix::identity(9) * 1.0f;
 			return;
 		}
 	}
 
 	// correct state
-	phi += xCorrect(PHI);
-	theta += xCorrect(THETA);
+	if (!ignoreAccel) {
+		phi += xCorrect(PHI);
+		theta += xCorrect(THETA);
+	}
+
 	psi += xCorrect(PSI);
 
+	// attitude also affects nav velocities
+	vN += xCorrect(VN);
+	vE += xCorrect(VE);
+	vD += xCorrect(VD);
+
 	// update state covariance
+	// http://en.wikipedia.org/wiki/Extended_Kalman_filter
 	P = P - K * HAtt * P;
 
 	// fault detection
 	float beta = y.dot(S.inverse() * y);
-	printf("attitude: beta = %8.4f\n", (double)beta);
 
 	if (beta > 10.0f) {
-		//printf("fault in attitude: beta = %8.4f\n", (double)beta);
+		printf("fault in attitude: beta = %8.4f\n", (double)beta);
 		//printf("y:\n"); y.print();
 	}
 
@@ -536,20 +589,32 @@ void KalmanNav::correctAtt()
 	q = Quaternion(EulerAngles(phi, theta, psi));
 }
 
-void KalmanNav::correctGps()
+void KalmanNav::correctPos()
 {
 	using namespace math;
 	Vector y(6);
 	y(0) = _gps.vel_n - vN;
 	y(1) = _gps.vel_e - vE;
-	y(2) = _gps.vel_d - vD;
-	y(3) = double(_gps.lat) / 1.0e7 / M_RAD_TO_DEG - lat;
-	y(4) = double(_gps.lon) / 1.0e7 / M_RAD_TO_DEG - lon;
-	y(5) = double(_gps.alt) / 1.0e3 - alt;
+	y(2) = double(_gps.lat) / 1.0e7 / M_RAD_TO_DEG - lat;
+	y(3) = double(_gps.lon) / 1.0e7 / M_RAD_TO_DEG - lon;
+	y(4) = double(_gps.alt) / 1.0e3 - alt;
+
+	// update gps noise
+	float cosLSing = cosf(lat);
+	float R = R0 + float(alt);
+
+	// prevent singularity
+	if (fabsf(cosLSing) < 0.01f) cosLSing = 0.01f;
+
+	float noiseLat = _rGpsPos.get() / R;
+	float noiseLon = _rGpsPos.get() / (R * cosLSing);
+	RPos(2, 2) = noiseLat * noiseLat;
+	RPos(3, 3) = noiseLon * noiseLon;
 
 	// compute correction
-	Matrix S = HGps * P * HGps.transpose() + RGps; // residual covariance
-	Matrix K = P * HGps.transpose() * S.inverse();
+	// http://en.wikipedia.org/wiki/Extended_Kalman_filter
+	Matrix S = HPos * P * HPos.transpose() + RPos; // residual covariance
+	Matrix K = P * HPos.transpose() * S.inverse();
 	Vector xCorrect = K * y;
 
 	// check correction is sane
@@ -566,6 +631,8 @@ void KalmanNav::correctGps()
 			setLatDegE7(_gps.lat);
 			setLonDegE7(_gps.lon);
 			setAltE3(_gps.alt);
+			// reset P matrix to identity
+			P = Matrix::identity(9) * 1.0f;
 			return;
 		}
 	}
@@ -579,14 +646,14 @@ void KalmanNav::correctGps()
 	alt += double(xCorrect(ALT));
 
 	// update state covariance
-	P = P - K * HGps * P;
+	// http://en.wikipedia.org/wiki/Extended_Kalman_filter
+	P = P - K * HPos * P;
 
 	// fault detetcion
 	float beta = y.dot(S.inverse() * y);
-	printf("gps: beta = %8.4f\n", (double)beta);
 
-	if (beta > 100.0f) {
-		//printf("fault in gps: beta = %8.4f\n", (double)beta);
+	if (beta > 10.0f) {
+		printf("fault in gps: beta = %8.4f\n", (double)beta);
 		//printf("y:\n"); y.print();
 	}
 }
@@ -597,6 +664,7 @@ void KalmanNav::updateParams()
 	using namespace control;
 	SuperBlock::updateParams();
 
+
 	// gyro noise
 	V(0, 0) = _vGyro.get();   // gyro x, rad/s
 	V(1, 1) = _vGyro.get();   // gyro y
@@ -608,20 +676,31 @@ void KalmanNav::updateParams()
 	V(5, 5) = _vAccel.get();   // accel z
 
 	// magnetometer noise
-	RAtt(0, 0) = _rMag.get(); // normalized direction
-	RAtt(1, 1) = _rMag.get();
-	RAtt(2, 2) = _rMag.get();
+	float noiseMagSq = _rMag.get() * _rMag.get();
+	RAtt(0, 0) = noiseMagSq; // normalized direction
+	RAtt(1, 1) = noiseMagSq;
+	RAtt(2, 2) = noiseMagSq;
 
 	// accelerometer noise
-	RAtt(3, 3) = _rAccel.get(); // normalized direction
-	RAtt(4, 4) = _rAccel.get();
-	RAtt(5, 5) = _rAccel.get();
+	float noiseAccelSq = _rAccel.get() * _rAccel.get();
+	RAtt(3, 3) = noiseAccelSq; // normalized direction
+	RAtt(4, 4) = noiseAccelSq;
+	RAtt(5, 5) = noiseAccelSq;
 
 	// gps noise
-	RGps(0, 0) = _rGpsV.get(); // vn, m/s
-	RGps(1, 1) = _rGpsV.get(); // ve
-	RGps(2, 2) = _rGpsV.get(); // vd
-	RGps(3, 3) = _rGpsGeo.get(); // L, rad
-	RGps(4, 4) = _rGpsGeo.get(); // l, rad
-	RGps(5, 5) = _rGpsAlt.get(); // h, m
+	float cosLSing = cosf(lat);
+	float R = R0 + float(alt);
+
+	// prevent singularity
+	if (fabsf(cosLSing) < 0.01f) cosLSing = 0.01f;
+
+	float noiseVel = _rGpsVel.get();
+	float noiseLat = _rGpsPos.get() / R;
+	float noiseLon = _rGpsPos.get() / (R * cosLSing);
+	float noiseAlt = _rGpsAlt.get();
+	RPos(0, 0) = noiseVel * noiseVel; // vn
+	RPos(1, 1) = noiseVel * noiseVel; // ve
+	RPos(2, 2) = noiseLat * noiseLat; // lat
+	RPos(3, 3) = noiseLon * noiseLon; // lon
+	RPos(4, 4) = noiseAlt * noiseAlt; // h
 }

apps/examples/kalman_demo/KalmanNav.hpp

@@ -60,6 +60,11 @@
 #include <poll.h>
 #include <unistd.h>
 
+/**
+ * Kalman filter navigation class
+ * http://en.wikipedia.org/wiki/Extended_Kalman_filter
+ * Discrete-time extended Kalman filter
+ */
 class KalmanNav : public control::SuperBlock
 {
 public:
@@ -70,7 +75,7 @@ public:
 	void predictFast(float dt);
 	void predictSlow(float dt);
 	void correctAtt();
-	void correctGps();
+	void correctPos();
 	virtual void updateParams();
 protected:
 	math::Matrix F;
@@ -79,8 +84,8 @@ protected:
 	math::Matrix V;
 	math::Matrix HAtt;
 	math::Matrix RAtt;
-	math::Matrix HGps;
-	math::Matrix RGps;
+	math::Matrix HPos;
+	math::Matrix RPos;
 	math::Dcm C_nb;
 	math::Quaternion q;
 	control::UOrbSubscription<sensor_combined_s> _sensors;
@@ -94,6 +99,8 @@ protected:
 	uint64_t _attTimeStamp;
 	uint64_t _outTimeStamp;
 	uint16_t _navFrames;
+	uint16_t _missFast;
+	uint16_t _missSlow;
 	float fN, fE, fD;
 	// states
 	enum {PHI = 0, THETA, PSI, VN, VE, VD, LAT, LON, ALT};
@@ -103,8 +110,8 @@ protected:
 	control::BlockParam<float> _vGyro;
 	control::BlockParam<float> _vAccel;
 	control::BlockParam<float> _rMag;
-	control::BlockParam<float> _rGpsV;
-	control::BlockParam<float> _rGpsGeo;
+	control::BlockParam<float> _rGpsVel;
+	control::BlockParam<float> _rGpsPos;
 	control::BlockParam<float> _rGpsAlt;
 	control::BlockParam<float> _rAccel;
 	int32_t getLatDegE7() { return int32_t(lat * 1.0e7 * M_RAD_TO_DEG); }

apps/examples/kalman_demo/params.c

@@ -3,8 +3,8 @@
 /*PARAM_DEFINE_FLOAT(NAME,0.0f);*/
 PARAM_DEFINE_FLOAT(KF_V_GYRO, 0.01f);
 PARAM_DEFINE_FLOAT(KF_V_ACCEL, 0.01f);
-PARAM_DEFINE_FLOAT(KF_R_MAG, 0.01f);
-PARAM_DEFINE_FLOAT(KF_R_GPS_V, 0.1f);
-PARAM_DEFINE_FLOAT(KF_R_GPS_GEO, 1.0e-7f);
-PARAM_DEFINE_FLOAT(KF_R_GPS_ALT, 10.0f);
-PARAM_DEFINE_FLOAT(KF_R_ACCEL, 0.01f);
+PARAM_DEFINE_FLOAT(KF_R_MAG, 1.0f);
+PARAM_DEFINE_FLOAT(KF_R_GPS_VEL, 1.0f);
+PARAM_DEFINE_FLOAT(KF_R_GPS_POS, 1.0f);
+PARAM_DEFINE_FLOAT(KF_R_GPS_ALT, 1.0f);
+PARAM_DEFINE_FLOAT(KF_R_ACCEL, 1.0f);