Merge pull request #171 from PX4/fault_detection

Attitude / position estimation and controller improvements

7 files changed, 371 insertions, 248 deletions

diff --git a/apps/commander/commander.c b/apps/commander/commander.c
index 103c53dc6..7654b3426 100644
--- a/apps/commander/commander.c
+++ b/apps/commander/commander.c
@@ -1850,15 +1850,16 @@ int commander_thread_main(int argc, char *argv[])
 					update_state_machine_mode_manual(stat_pub, &current_status, mavlink_fd);
 
 				} else if (sp_man.manual_override_switch < -STICK_ON_OFF_LIMIT) {
-					/* check auto mode switch for correct mode */
-					if (sp_man.auto_mode_switch > STICK_ON_OFF_LIMIT) {
-						/* enable guided mode */
-						update_state_machine_mode_guided(stat_pub, &current_status, mavlink_fd);
+					// /* check auto mode switch for correct mode */
+					// if (sp_man.auto_mode_switch > STICK_ON_OFF_LIMIT) {
+					// 	/* enable guided mode */
+					// 	update_state_machine_mode_guided(stat_pub, &current_status, mavlink_fd);
 
-					} else if (sp_man.auto_mode_switch < -STICK_ON_OFF_LIMIT) {
+					// } else if (sp_man.auto_mode_switch < -STICK_ON_OFF_LIMIT) {
+						// XXX hardcode to auto for now
 						update_state_machine_mode_auto(stat_pub, &current_status, mavlink_fd);
 
-					}
+					// }
 
 				} else {
 					/* center stick position, set SAS for all vehicle types */
diff --git a/apps/examples/control_demo/params.c b/apps/examples/control_demo/params.c
index 8f923f5a1..6525b70f5 100644
--- a/apps/examples/control_demo/params.c
+++ b/apps/examples/control_demo/params.c
@@ -14,50 +14,50 @@ PARAM_DEFINE_FLOAT(FWB_R_LP, 300.0f); // yaw rate low pass cut freq
 PARAM_DEFINE_FLOAT(FWB_R_HP, 1.0f); // yaw rate high pass
 
 // stabilization mode
-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
+PARAM_DEFINE_FLOAT(FWB_P2AIL, 0.3f); // 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
 
 //  psi -> phi -> p
 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
+PARAM_DEFINE_FLOAT(FWB_PHI_LIM_MAX, 0.3f);  // roll limit, 28 deg
 
 // velocity -> theta
-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, -0.5f);
-PARAM_DEFINE_FLOAT(FWB_THE_MAX, 0.5f);
+PARAM_DEFINE_FLOAT(FWB_V2THE_P, 1.0f);      // velocity to pitch angle PID, prop gain
+PARAM_DEFINE_FLOAT(FWB_V2THE_I, 0.0f);      // integral gain
+PARAM_DEFINE_FLOAT(FWB_V2THE_D, 0.0f);      // derivative gain
+PARAM_DEFINE_FLOAT(FWB_V2THE_D_LP, 0.0f);   // derivative low-pass
+PARAM_DEFINE_FLOAT(FWB_V2THE_I_MAX, 0.0f);  // integrator wind up guard
+PARAM_DEFINE_FLOAT(FWB_THE_MIN, -0.5f);     // the max commanded pitch angle
+PARAM_DEFINE_FLOAT(FWB_THE_MAX, 0.5f);      // the min commanded pitch angle
 
 
 // theta -> q
-PARAM_DEFINE_FLOAT(FWB_THE2Q_P, 1.0f);
+PARAM_DEFINE_FLOAT(FWB_THE2Q_P, 1.0f);      // pitch angle to pitch-rate PID
 PARAM_DEFINE_FLOAT(FWB_THE2Q_I, 0.0f);
 PARAM_DEFINE_FLOAT(FWB_THE2Q_D, 0.0f);
 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.01f);
+PARAM_DEFINE_FLOAT(FWB_H2THR_P, 0.01f);     // altitude to throttle PID
 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.57f); // 90 deg
-PARAM_DEFINE_FLOAT(FWB_XT2YAW, 0.002f);
+PARAM_DEFINE_FLOAT(FWB_XT2YAW_MAX, 1.57f);  // cross-track to yaw angle limit 90 deg
+PARAM_DEFINE_FLOAT(FWB_XT2YAW, 0.005f);     // cross-track to yaw angle gain
 
 // speed command
-PARAM_DEFINE_FLOAT(FWB_V_MIN, 20.0f);
-PARAM_DEFINE_FLOAT(FWB_V_CMD, 22.0f);
-PARAM_DEFINE_FLOAT(FWB_V_MAX, 24.0f);
+PARAM_DEFINE_FLOAT(FWB_V_MIN, 20.0f);       // minimum commanded velocity
+PARAM_DEFINE_FLOAT(FWB_V_CMD, 22.0f);       // commanded velocity
+PARAM_DEFINE_FLOAT(FWB_V_MAX, 24.0f);       // maximum commanded velocity
 
 // trim
-PARAM_DEFINE_FLOAT(FWB_TRIM_AIL, 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.81f);
+PARAM_DEFINE_FLOAT(FWB_TRIM_AIL, 0.0f);     // trim aileron, normalized (-1,1)
+PARAM_DEFINE_FLOAT(FWB_TRIM_ELV, 0.005f);   // trim elevator (-1,1)
+PARAM_DEFINE_FLOAT(FWB_TRIM_RDR, 0.0f);     // trim rudder (-1,1)
+PARAM_DEFINE_FLOAT(FWB_TRIM_THR, 0.8f);     // trim throttle (0,1)
diff --git a/apps/examples/kalman_demo/KalmanNav.cpp b/apps/examples/kalman_demo/KalmanNav.cpp
index 240564b56..7e89dffb2 100644
--- a/apps/examples/kalman_demo/KalmanNav.cpp
+++ b/apps/examples/kalman_demo/KalmanNav.cpp
@@ -45,9 +45,9 @@
 // Titterton pg. 52
 static const float omega = 7.2921150e-5f; // earth rotation rate, rad/s
 static const float R0 = 6378137.0f; // earth radius, m
-
-static const float RSq = 4.0680631591e+13; // earth radius squared
-static const float g = 9.806f; // gravitational accel. m/s^2, XXX should be calibrated
+static const float g0 = 9.806f; // standard gravitational accel. m/s^2
+static const int8_t ret_ok = 0; 		// no error in function
+static const int8_t ret_error = -1; 	// error occurred
 
 KalmanNav::KalmanNav(SuperBlock *parent, const char *name) :
 	SuperBlock(parent, name),
@@ -55,6 +55,7 @@ KalmanNav::KalmanNav(SuperBlock *parent, const char *name) :
 	F(9, 9),
 	G(9, 6),
 	P(9, 9),
+	P0(9, 9),
 	V(6, 6),
 	// attitude measurement ekf matrices
 	HAtt(6, 9),
@@ -66,7 +67,7 @@ KalmanNav::KalmanNav(SuperBlock *parent, const char *name) :
 	C_nb(),
 	q(),
 	// subscriptions
-	_sensors(&getSubscriptions(), ORB_ID(sensor_combined), 1), // limit to 1000 Hz
+	_sensors(&getSubscriptions(), ORB_ID(sensor_combined), 5), // limit to 200 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
@@ -74,17 +75,16 @@ KalmanNav::KalmanNav(SuperBlock *parent, const char *name) :
 	_att(&getPublications(), ORB_ID(vehicle_attitude)),
 	// timestamps
 	_pubTimeStamp(hrt_absolute_time()),
-	_fastTimeStamp(hrt_absolute_time()),
-	_slowTimeStamp(hrt_absolute_time()),
+	_predictTimeStamp(hrt_absolute_time()),
 	_attTimeStamp(hrt_absolute_time()),
 	_outTimeStamp(hrt_absolute_time()),
 	// frame count
 	_navFrames(0),
 	// miss counts
-	_missFast(0),
-	_missSlow(0),
-	// state
+	_miss(0),
+	// 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),
@@ -95,42 +95,32 @@ 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, "ENV_MAG_DIP"),
+	_magDec(this, "ENV_MAG_DEC"),
+	_g(this, "ENV_G"),
+	_faultPos(this, "FAULT_POS"),
+	_faultAtt(this, "FAULT_ATT"),
+	_attitudeInitialized(false),
+	_positionInitialized(false),
+	_attitudeInitCounter(0)
 {
 	using namespace math;
 
 	// initial state covariance matrix
-	P = Matrix::identity(9) * 1.0f;
-
-	// wait for gps lock
-	while (1) {
-		struct pollfd fds[1];
-		fds[0].fd = _gps.getHandle();
-		fds[0].events = POLLIN;
-
-		// poll 10 seconds for new data
-		int ret = poll(fds, 1, 10000);
-
-		if (ret > 0)  {
-			updateSubscriptions();
-
-			if (_gps.fix_type > 2) break;
-
-		} else if (ret == 0) {
-			printf("[kalman_demo] waiting for gps lock\n");
-		}
-	}
+	P0 = Matrix::identity(9) * 0.01f;
+	P = P0;
 
 	// initial state
 	phi = 0.0f;
 	theta = 0.0f;
 	psi = 0.0f;
-	vN = _gps.vel_n;
-	vE = _gps.vel_e;
-	vD = _gps.vel_d;
-	setLatDegE7(_gps.lat);
-	setLonDegE7(_gps.lon);
-	setAltE3(_gps.alt);
+	vN = 0.0f;
+	vE = 0.0f;
+	vD = 0.0f;
+	lat = 0.0f;
+	lon = 0.0f;
+	alt = 0.0f;
 
 	// initialize quaternions
 	q = Quaternion(EulerAngles(phi, theta, psi));
@@ -141,8 +131,8 @@ KalmanNav::KalmanNav(SuperBlock *parent, const char *name) :
 	// HPos is constant
 	HPos(0, 3) = 1.0f;
 	HPos(1, 4) = 1.0f;
-	HPos(2, 6) = 1.0f;
-	HPos(3, 7) = 1.0f;
+	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;
 
 	// initialize all parameters
@@ -153,18 +143,13 @@ void KalmanNav::update()
 {
 	using namespace math;
 
-	struct pollfd fds[3];
+	struct pollfd fds[1];
 	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);
+	// poll for new data
+	int ret = poll(fds, 1, 1000);
 
-	// check return value
 	if (ret < 0) {
 		// XXX this is seriously bad - should be an emergency
 		return;
@@ -186,39 +171,67 @@ void KalmanNav::update()
 	// this clears update flag
 	updateSubscriptions();
 
-	// abort update if no new data
-	if (!(sensorsUpdate || gpsUpdate)) return;
+	// initialize attitude when sensors online
+	if (!_attitudeInitialized && sensorsUpdate &&
+	    _sensors.accelerometer_counter > 10 &&
+	    _sensors.gyro_counter > 10 &&
+	    _sensors.magnetometer_counter > 10) {
+		if (correctAtt() == ret_ok) _attitudeInitCounter++;
+
+		if (_attitudeInitCounter > 100) {
+			printf("[kalman_demo] initialized EKF attitude\n");
+			printf("phi: %8.4f, theta: %8.4f, psi: %8.4f\n",
+			       double(phi), double(theta), double(psi));
+			_attitudeInitialized = true;
+		}
+	}
+
+	// initialize position when gps received
+	if (!_positionInitialized &&
+	    _attitudeInitialized && // wait for attitude first
+	    gpsUpdate &&
+	    _gps.fix_type > 2 &&
+	    _gps.counter_pos_valid > 10) {
+		vN = _gps.vel_n;
+		vE = _gps.vel_e;
+		vD = _gps.vel_d;
+		setLatDegE7(_gps.lat);
+		setLonDegE7(_gps.lon);
+		setAltE3(_gps.alt);
+		_positionInitialized = true;
+		printf("[kalman_demo] initialized EKF state with GPS\n");
+		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);
+	}
 
-	// fast prediciton step
-	// note, using sensors timestamp so we can account
-	// for packet lag
-	float dtFast = (_sensors.timestamp - _fastTimeStamp) / 1.0e6f;
-	_fastTimeStamp = _sensors.timestamp;
+	// prediciton step
+	// using sensors timestamp so we can account for packet lag
+	float dt = (_sensors.timestamp - _predictTimeStamp) / 1.0e6f;
+	//printf("dt: %15.10f\n", double(dt));
+	_predictTimeStamp = _sensors.timestamp;
 
-	if (dtFast < 1.0f / 50) {
-		predictFast(dtFast);
+	// don't predict if time greater than a second
+	if (dt < 1.0f) {
+		predictState(dt);
+		predictStateCovariance(dt);
 		// count fast frames
 		_navFrames += 1;
-
-	} else _missFast++;
-
-	// slow prediction step
-	float dtSlow = (_sensors.timestamp - _slowTimeStamp) / 1.0e6f;
-
-	if (dtSlow > 1.0f / 100) { // 100 Hz
-		_slowTimeStamp = _sensors.timestamp;
-
-		if (dtSlow < 1.0f / 50) predictSlow(dtSlow);
-		else _missSlow ++;
 	}
 
+	// count times 100 Hz rate isn't met
+	if (dt > 0.01f) _miss++;
+
 	// gps correction step
-	if (gpsUpdate) {
+	if (_positionInitialized && gpsUpdate) {
 		correctPos();
 	}
 
 	// attitude correction step
-	if (_sensors.timestamp - _attTimeStamp > 1e6 / 20) { // 20 Hz
+	if (_attitudeInitialized 								// initialized
+	    && sensorsUpdate 								// new data
+	    && _sensors.timestamp - _attTimeStamp > 1e6 / 20 	// 20 Hz
+	   ) {
 		_attTimeStamp = _sensors.timestamp;
 		correctAtt();
 	}
@@ -226,16 +239,17 @@ void KalmanNav::update()
 	// publication
 	if (newTimeStamp - _pubTimeStamp > 1e6 / 50) { // 50 Hz
 		_pubTimeStamp = newTimeStamp;
+
 		updatePublications();
 	}
 
 	// output
-	if (newTimeStamp - _outTimeStamp > 1e6) { // 1 Hz
+	if (newTimeStamp - _outTimeStamp > 10e6) { // 0.1 Hz
 		_outTimeStamp = newTimeStamp;
-		printf("nav: %4d Hz, misses fast: %4d slow: %4d\n", _navFrames, _missFast, _missSlow);
+		printf("nav: %4d Hz, miss #: %4d\n",
+		       _navFrames / 10, _miss / 10);
 		_navFrames = 0;
-		_missFast = 0;
-		_missSlow = 0;
+		_miss = 0;
 	}
 }
 
@@ -278,73 +292,87 @@ void KalmanNav::updatePublications()
 	_att.q_valid = true;
 	_att.counter = _navFrames;
 
-	// update publications
-	SuperBlock::updatePublications();
+	// selectively update publications,
+	// do NOT call superblock do-all method
+	if (_positionInitialized)
+		_pos.update();
+
+	if (_attitudeInitialized)
+		_att.update();
 }
 
-void KalmanNav::predictFast(float dt)
+int KalmanNav::predictState(float dt)
 {
 	using namespace math;
-	Vector3 w(_sensors.gyro_rad_s);
 
-	// attitude
-	q = q + q.derivative(w) * dt;
+	// trig
+	float sinL = sinf(lat);
+	float cosL = cosf(lat);
+	float cosLSing = cosf(lat);
 
-	// renormalize quaternion if needed
-	if (fabsf(q.norm() - 1.0f) > 1e-4f) {
-		q = q.unit();
+	// prevent singularity
+	if (fabsf(cosLSing) < 0.01f) {
+		if (cosLSing > 0) cosLSing = 0.01;
+		else cosLSing = -0.01;
 	}
 
-	// C_nb update
-	C_nb = Dcm(q);
+	// attitude prediction
+	if (_attitudeInitialized) {
+		Vector3 w(_sensors.gyro_rad_s);
 
-	// euler update
-	EulerAngles euler(C_nb);
-	phi = euler.getPhi();
-	theta = euler.getTheta();
-	psi = euler.getPsi();
+		// attitude
+		q = q + q.derivative(w) * dt;
 
-	// specific acceleration in nav frame
-	Vector3 accelB(_sensors.accelerometer_m_s2);
-	Vector3 accelN = C_nb * accelB;
-	fN = accelN(0);
-	fE = accelN(1);
-	fD = accelN(2);
+		// renormalize quaternion if needed
+		if (fabsf(q.norm() - 1.0f) > 1e-4f) {
+			q = q.unit();
+		}
 
-	// trig
-	float sinL = sinf(lat);
-	float cosL = cosf(lat);
-	float cosLSing = cosf(lat);
+		// C_nb update
+		C_nb = Dcm(q);
+
+		// euler update
+		EulerAngles euler(C_nb);
+		phi = euler.getPhi();
+		theta = euler.getTheta();
+		psi = euler.getPsi();
+
+		// specific acceleration in nav frame
+		Vector3 accelB(_sensors.accelerometer_m_s2);
+		Vector3 accelN = C_nb * accelB;
+		fN = accelN(0);
+		fE = accelN(1);
+		fD = accelN(2);
+	}
 
-	if (fabsf(cosLSing) < 0.01f) cosLSing = 0.01f;
+	// position prediction
+	if (_positionInitialized) {
+		// neglects angular deflections in local gravity
+		// see Titerton pg. 70
+		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 * rotRate * cosL -
+			      vN * LDot + _g.get();
+		float vEDot = fE + vN * rotRate * sinL +
+			      vDDot * rotRate * cosL;
+
+		// rectangular integration
+		vN += vNDot * dt;
+		vE += vEDot * dt;
+		vD += vDDot * dt;
+		lat += double(LDot * dt);
+		lon += double(lDot * dt);
+		alt += double(-vD * dt);
+	}
 
-	// position update
-	// neglects angular deflections in local gravity
-	// see Titerton pg. 70
-	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 * rotRate * cosL -
-		      vN * LDot + g;
-	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;
-	lat += double(LDot * dt);
-	lon += double(lDot * dt);
-	alt += double(-vD * dt);
+	return ret_ok;
 }
 
-void KalmanNav::predictSlow(float dt)
+int KalmanNav::predictStateCovariance(float dt)
 {
 	using namespace math;
 
@@ -356,6 +384,7 @@ void KalmanNav::predictSlow(float dt)
 
 	// prepare for matrix
 	float R = R0 + float(alt);
+	float RSq = R * R;
 
 	// F Matrix
 	// Titterton pg. 291
@@ -436,9 +465,11 @@ void KalmanNav::predictSlow(float dt)
 	// for discrte time EKF
 	// http://en.wikipedia.org/wiki/Extended_Kalman_filter
 	P = P + (F * P + P * F.transpose() + G * V * G.transpose()) * dt;
+
+	return ret_ok;
 }
 
-void KalmanNav::correctAtt()
+int KalmanNav::correctAtt()
 {
 	using namespace math;
 
@@ -452,12 +483,14 @@ void KalmanNav::correctAtt()
 
 	// mag measurement
 	Vector3 zMag(_sensors.magnetometer_ga);
+	//float magNorm = zMag.norm();
+	zMag = zMag.unit();
 
 	// 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
+	float dip = _magDip.get() / M_RAD_TO_DEG_F; // dip, inclination with level
+	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);
@@ -472,7 +505,7 @@ void KalmanNav::correctAtt()
 	// ignore accel correction when accel mag not close to g
 	Matrix RAttAdjust = RAtt;
 
-	bool ignoreAccel = fabsf(accelMag - g) > 1.1f;
+	bool ignoreAccel = fabsf(accelMag - _g.get()) > 1.1f;
 
 	if (ignoreAccel) {
 		RAttAdjust(3, 3) = 1.0e10;
@@ -483,12 +516,8 @@ void KalmanNav::correctAtt()
 		//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, -g) /*+ zCentrip*/).unit();
+	Vector3 zAccelHat = (C_nb.transpose() * Vector3(0, 0, -_g.get())).unit();
 
 	// combined measurement
 	Vector zAtt(6);
@@ -553,9 +582,9 @@ 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;
+			// reset P matrix to P0
+			P = P0;
+			return ret_error;
 		}
 	}
 
@@ -568,9 +597,11 @@ void KalmanNav::correctAtt()
 	psi += xCorrect(PSI);
 
 	// attitude also affects nav velocities
-	vN += xCorrect(VN);
-	vE += xCorrect(VE);
-	vD += xCorrect(VD);
+	if (_positionInitialized) {
+		vN += xCorrect(VN);
+		vE += xCorrect(VE);
+		vD += xCorrect(VD);
+	}
 
 	// update state covariance
 	// http://en.wikipedia.org/wiki/Extended_Kalman_filter
@@ -579,38 +610,33 @@ void KalmanNav::correctAtt()
 	// fault detection
 	float beta = y.dot(S.inverse() * y);
 
-	if (beta > 1000.0f) {
+	if (beta > _faultAtt.get()) {
 		printf("fault in attitude: beta = %8.4f\n", (double)beta);
-		//printf("y:\n"); y.print();
+		printf("y:\n"); y.print();
+		printf("zMagHat:\n"); zMagHat.print();
+		printf("zMag:\n"); zMag.print();
+		printf("bNav:\n"); bNav.print();
 	}
 
 	// update quaternions from euler
 	// angle correction
 	q = Quaternion(EulerAngles(phi, theta, psi));
+
+	return ret_ok;
 }
 
-void KalmanNav::correctPos()
+int KalmanNav::correctPos()
 {
 	using namespace math;
-	Vector y(6);
+
+	// residual
+	Vector y(5);
 	y(0) = _gps.vel_n - vN;
 	y(1) = _gps.vel_e - vE;
-	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(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;
 
-	// 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
 	// http://en.wikipedia.org/wiki/Extended_Kalman_filter
 	Matrix S = HPos * P * HPos.transpose() + RPos; // residual covariance
@@ -631,9 +657,9 @@ void KalmanNav::correctPos()
 			setLatDegE7(_gps.lat);
 			setLonDegE7(_gps.lon);
 			setAltE3(_gps.alt);
-			// reset P matrix to identity
-			P = Matrix::identity(9) * 1.0f;
-			return;
+			// reset P matrix to P0
+			P = P0;
+			return ret_error;
 		}
 	}
 
@@ -652,10 +678,17 @@ void KalmanNav::correctPos()
 	// fault detetcion
 	float beta = y.dot(S.inverse() * y);
 
-	if (beta > 1000.0f) {
+	if (beta > _faultPos.get()) {
 		printf("fault in gps: beta = %8.4f\n", (double)beta);
-		//printf("y:\n"); y.print();
+		printf("Y/N: vN: %8.4f, vE: %8.4f, lat: %8.4f, lon: %8.4f, alt: %8.4f\n",
+		       double(y(0) / sqrtf(RPos(0, 0))),
+		       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))));
 	}
+
+	return ret_ok;
 }
 
 void KalmanNav::updateParams()
@@ -664,7 +697,6 @@ 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
@@ -676,31 +708,54 @@ void KalmanNav::updateParams()
 	V(5, 5) = _vAccel.get();   // accel z
 
 	// magnetometer noise
+	float noiseMin = 1e-6f;
 	float noiseMagSq = _rMag.get() * _rMag.get();
+
+	if (noiseMagSq < noiseMin) noiseMagSq = noiseMin;
+
 	RAtt(0, 0) = noiseMagSq; // normalized direction
 	RAtt(1, 1) = noiseMagSq;
 	RAtt(2, 2) = noiseMagSq;
 
 	// accelerometer noise
 	float noiseAccelSq = _rAccel.get() * _rAccel.get();
+
+	// bound noise to prevent singularities
+	if (noiseAccelSq < noiseMin) noiseAccelSq = noiseMin;
+
 	RAtt(3, 3) = noiseAccelSq; // normalized direction
 	RAtt(4, 4) = noiseAccelSq;
 	RAtt(5, 5) = noiseAccelSq;
 
 	// gps noise
-	float cosLSing = cosf(lat);
 	float R = R0 + float(alt);
+	float cosLSing = cosf(lat);
 
 	// prevent singularity
-	if (fabsf(cosLSing) < 0.01f) cosLSing = 0.01f;
+	if (fabsf(cosLSing) < 0.01f) {
+		if (cosLSing > 0) cosLSing = 0.01;
+		else cosLSing = -0.01;
+	}
 
 	float noiseVel = _rGpsVel.get();
-	float noiseLat = _rGpsPos.get() / R;
-	float noiseLon = _rGpsPos.get() / (R * cosLSing);
+	float noiseLatDegE7 = 1.0e7f * M_RAD_TO_DEG_F * _rGpsPos.get() / R;
+	float noiseLonDegE7 = noiseLatDegE7 / cosLSing;
 	float noiseAlt = _rGpsAlt.get();
+
+	// bound noise to prevent singularities
+	if (noiseVel < noiseMin) noiseVel = noiseMin;
+
+	if (noiseLatDegE7 < noiseMin) noiseLatDegE7 = noiseMin;
+
+	if (noiseLonDegE7 < noiseMin) noiseLonDegE7 = noiseMin;
+
+	if (noiseAlt < noiseMin) noiseAlt = noiseMin;
+
 	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(2, 2) = noiseLatDegE7 * noiseLatDegE7; // lat
+	RPos(3, 3) = noiseLonDegE7 * noiseLonDegE7; // lon
 	RPos(4, 4) = noiseAlt * noiseAlt; // 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 1ea46d40f..7709ac697 100644
--- a/apps/examples/kalman_demo/KalmanNav.hpp
+++ b/apps/examples/kalman_demo/KalmanNav.hpp
@@ -68,52 +68,108 @@
 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();
+
+
+	/**
+	 * Publication update
+	 */
 	virtual void updatePublications();
-	void predictFast(float dt);
-	void predictSlow(float dt);
-	void correctAtt();
-	void correctPos();
+
+	/**
+	 * State prediction
+	 * Continuous, non-linear
+	 */
+	int predictState(float dt);
+
+	/**
+	 * State covariance prediction
+	 * Continuous, linear
+	 */
+	int predictStateCovariance(float dt);
+
+	/**
+	 * Attitude correction
+	 */
+	int correctAtt();
+
+	/**
+	 * Position correction
+	 */
+	int correctPos();
+
+	/**
+	 * Overloaded update parameters
+	 */
 	virtual void updateParams();
 protected:
-	math::Matrix F;
-	math::Matrix G;
-	math::Matrix P;
-	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 _predictTimeStamp; /**< 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 _miss;         	/**< number of times fast 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 */
+	control::BlockParam<float> _g;          /**< gravitational constant */
+	control::BlockParam<float> _faultPos;   /**< fault detection threshold for position */
+	control::BlockParam<float> _faultAtt;   /**< fault detection threshold for attitude */
+	// status
+	bool _attitudeInitialized;
+	bool _positionInitialized;
+	uint16_t _attitudeInitCounter;
+	// 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); }
diff --git a/apps/examples/kalman_demo/params.c b/apps/examples/kalman_demo/params.c
index 03cdca5ed..3bfe01261 100644
--- a/apps/examples/kalman_demo/params.c
+++ b/apps/examples/kalman_demo/params.c
@@ -1,10 +1,16 @@
 #include <systemlib/param/param.h>
 
 /*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_V_GYRO, 1.0f);
+PARAM_DEFINE_FLOAT(KF_V_ACCEL, 1.0f);
 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_GPS_POS, 5.0f);
+PARAM_DEFINE_FLOAT(KF_R_GPS_ALT, 5.0f);
 PARAM_DEFINE_FLOAT(KF_R_ACCEL, 1.0f);
+PARAM_DEFINE_FLOAT(KF_FAULT_POS, 10.0f);
+PARAM_DEFINE_FLOAT(KF_FAULT_ATT, 10.0f);
+PARAM_DEFINE_FLOAT(KF_ENV_G, 9.765f);
+PARAM_DEFINE_FLOAT(KF_ENV_MAG_DIP, 60.0f);
+PARAM_DEFINE_FLOAT(KF_ENV_MAG_DEC, 0.0f);
+
diff --git a/apps/mavlink/mavlink_receiver.c b/apps/mavlink/mavlink_receiver.c
index fa63c419f..9491ce7e3 100644
--- a/apps/mavlink/mavlink_receiver.c
+++ b/apps/mavlink/mavlink_receiver.c
@@ -318,9 +318,11 @@ handle_message(mavlink_message_t *msg)
 			static uint16_t hil_frames = 0;
 			static uint64_t old_timestamp = 0;
 
+			/* sensors general */
+			hil_sensors.timestamp = imu.time_usec;
+
 			/* hil gyro */
 			static const float mrad2rad = 1.0e-3f;
-			hil_sensors.timestamp = timestamp;
 			hil_sensors.gyro_counter = hil_counter;
 			hil_sensors.gyro_raw[0] = imu.xgyro;
 			hil_sensors.gyro_raw[1] = imu.ygyro;
@@ -367,8 +369,8 @@ handle_message(mavlink_message_t *msg)
 			hil_frames += 1 ;
 
 			// output
-			if ((timestamp - old_timestamp) > 1000000) {
-				printf("receiving hil imu at %d hz\n", hil_frames);
+			if ((timestamp - old_timestamp) > 10000000) {
+				printf("receiving hil imu at %d hz\n", hil_frames/10);
 				old_timestamp = timestamp;
 				hil_frames = 0;
 			}
@@ -412,8 +414,8 @@ handle_message(mavlink_message_t *msg)
 			hil_frames += 1 ;
 
 			// output
-			if ((timestamp - old_timestamp) > 1000000) {
-				printf("receiving hil gps at %d hz\n", hil_frames);
+			if ((timestamp - old_timestamp) > 10000000) {
+				printf("receiving hil gps at %d hz\n", hil_frames/10);
 				old_timestamp = timestamp;
 				hil_frames = 0;
 			}
@@ -429,6 +431,9 @@ handle_message(mavlink_message_t *msg)
 			static uint16_t hil_frames = 0;
 			static uint64_t old_timestamp = 0;
 
+			/* sensors general */
+			hil_sensors.timestamp = press.time_usec;
+
 			/* baro */
 			/* TODO, set ground_press/ temp during calib */
 			static const float ground_press = 1013.25f; // mbar
@@ -454,8 +459,8 @@ handle_message(mavlink_message_t *msg)
 			hil_frames += 1 ;
 
 			// output
-			if ((timestamp - old_timestamp) > 1000000) {
-				printf("receiving hil pressure at %d hz\n", hil_frames);
+			if ((timestamp - old_timestamp) > 10000000) {
+				printf("receiving hil pressure at %d hz\n", hil_frames/10);
 				old_timestamp = timestamp;
 				hil_frames = 0;
 			}
diff --git a/apps/mavlink/orb_listener.c b/apps/mavlink/orb_listener.c
index 8920714ef..ec5149745 100644
--- a/apps/mavlink/orb_listener.c
+++ b/apps/mavlink/orb_listener.c
@@ -718,7 +718,7 @@ uorb_receive_start(void)
 
 	/* --- GLOBAL POS VALUE --- */
 	mavlink_subs.global_pos_sub = orb_subscribe(ORB_ID(vehicle_global_position));
-	orb_set_interval(mavlink_subs.global_pos_sub, 1000);	/* 1Hz active updates */
+	orb_set_interval(mavlink_subs.global_pos_sub, 100);	/* 10 Hz active updates */
 
 	/* --- LOCAL POS VALUE --- */
 	mavlink_subs.local_pos_sub = orb_subscribe(ORB_ID(vehicle_local_position));