Merge branch 'master' into fmu_mixer

67 files changed, 4896 insertions, 510 deletions

diff --git a/ROMFS/px4fmu_common/init.d/1000_rc_fw_easystar.hil b/ROMFS/px4fmu_common/init.d/1000_rc_fw_easystar.hil
index 36194ad68..d114fe21a 100644
--- a/ROMFS/px4fmu_common/init.d/1000_rc_fw_easystar.hil
+++ b/ROMFS/px4fmu_common/init.d/1000_rc_fw_easystar.hil
@@ -7,7 +7,7 @@
 
 sh /etc/init.d/rc.fw_defaults
 
-echo "HIL Rascal 110 starting.."
+echo "X Plane HIL starting.."
 
 set HIL yes
 
diff --git a/ROMFS/px4fmu_common/init.d/3033_wingwing b/ROMFS/px4fmu_common/init.d/3033_wingwing
index 6e1a531cf..fff4b58df 100644
--- a/ROMFS/px4fmu_common/init.d/3033_wingwing
+++ b/ROMFS/px4fmu_common/init.d/3033_wingwing
@@ -33,3 +33,6 @@ then
 fi
 
 set MIXER FMU_Q
+# Provide ESC a constant 1000 us pulse
+set PWM_OUTPUTS 4
+set PWM_DISARMED 1000
diff --git a/ROMFS/px4fmu_common/init.d/rc.fw_apps b/ROMFS/px4fmu_common/init.d/rc.fw_apps
index 72327c554..429abc5ec 100644
--- a/ROMFS/px4fmu_common/init.d/rc.fw_apps
+++ b/ROMFS/px4fmu_common/init.d/rc.fw_apps
@@ -3,6 +3,13 @@
 # Standard apps for fixed wing
 #
 
-att_pos_estimator_ekf start
+#
+# Start the attitude and position estimator
+#
+fw_att_pos_estimator start
+
+#
+# Start attitude controller
+#
 fw_att_control start
 fw_pos_control_l1 start
diff --git a/ROMFS/px4fmu_common/init.d/rc.usb b/ROMFS/px4fmu_common/init.d/rc.usb
index 9f80219b1..b7b556945 100644
--- a/ROMFS/px4fmu_common/init.d/rc.usb
+++ b/ROMFS/px4fmu_common/init.d/rc.usb
@@ -9,6 +9,8 @@ mavlink start -r 10000 -d /dev/ttyACM0
 # Enable a number of interesting streams we want via USB
 mavlink stream -d /dev/ttyACM0 -s NAMED_VALUE_FLOAT -r 10
 mavlink stream -d /dev/ttyACM0 -s OPTICAL_FLOW -r 10
+mavlink stream -d /dev/ttyACM0 -s VFR_HUD -r 20
+mavlink stream -d /dev/ttyACM0 -s ATTITUDE -r 20
 mavlink stream -d /dev/ttyACM0 -s ATTITUDE_CONTROLS -r 30
 mavlink stream -d /dev/ttyACM0 -s SERVO_OUTPUT_RAW_0 -r 10
 
diff --git a/Tools/px_uploader.py b/Tools/px_uploader.py
index e4a8b3c05..985e6ffd9 100755
--- a/Tools/px_uploader.py
+++ b/Tools/px_uploader.py
@@ -389,18 +389,22 @@ class uploader(object):
                     self.otp_pid = self.otp[12:8:-1]
                     self.otp_coa = self.otp[32:160]
                     # show user:
-                    print("type: " + self.otp_id.decode('Latin-1'))
-                    print("idtype: " + binascii.b2a_qp(self.otp_idtype).decode('Latin-1'))
-                    print("vid: " + binascii.hexlify(self.otp_vid).decode('Latin-1'))
-                    print("pid: "+ binascii.hexlify(self.otp_pid).decode('Latin-1'))
-                    print("coa: "+ binascii.b2a_base64(self.otp_coa).decode('Latin-1'))
-                    print("sn: ", end='')
-                    for byte in range(0,12,4):
-                        x = self.__getSN(byte)
-                        x = x[::-1]  # reverse the bytes
-                        self.sn  = self.sn + x
-                        print(binascii.hexlify(x).decode('Latin-1'), end='') # show user
-                    print('')
+                    try:
+                            print("type: " + self.otp_id.decode('Latin-1'))
+                            print("idtype: " + binascii.b2a_qp(self.otp_idtype).decode('Latin-1'))
+                            print("vid: " + binascii.hexlify(self.otp_vid).decode('Latin-1'))
+                            print("pid: "+ binascii.hexlify(self.otp_pid).decode('Latin-1'))
+                            print("coa: "+ binascii.b2a_base64(self.otp_coa).decode('Latin-1'))
+                            print("sn: ", end='')
+                            for byte in range(0,12,4):
+                                    x = self.__getSN(byte)
+                                    x = x[::-1]  # reverse the bytes
+                                    self.sn  = self.sn + x
+                                    print(binascii.hexlify(x).decode('Latin-1'), end='') # show user
+                            print('')
+                    except Exception:
+                            # ignore bad character encodings
+                            pass
                 print("erase...")
                 self.__erase()
 
diff --git a/makefiles/config_px4fmu-v1_default.mk b/makefiles/config_px4fmu-v1_default.mk
index 651c2ac38..532e978d0 100644
--- a/makefiles/config_px4fmu-v1_default.mk
+++ b/makefiles/config_px4fmu-v1_default.mk
@@ -70,7 +70,7 @@ MODULES		+= modules/gpio_led
 # Estimation modules (EKF/ SO3 / other filters)
 #
 MODULES		+= modules/attitude_estimator_ekf
-MODULES		+= modules/att_pos_estimator_ekf
+MODULES		+= modules/fw_att_pos_estimator
 MODULES		+= modules/position_estimator_inav
 #MODULES		+= examples/flow_position_estimator
 
diff --git a/makefiles/config_px4fmu-v2_default.mk b/makefiles/config_px4fmu-v2_default.mk
index 5bed190d9..e13421acc 100644
--- a/makefiles/config_px4fmu-v2_default.mk
+++ b/makefiles/config_px4fmu-v2_default.mk
@@ -79,7 +79,7 @@ MODULES		+= modules/gpio_led
 #
 MODULES		+= modules/attitude_estimator_ekf
 MODULES		+= modules/attitude_estimator_so3
-MODULES		+= modules/att_pos_estimator_ekf
+MODULES		+= modules/fw_att_pos_estimator
 MODULES		+= modules/position_estimator_inav
 MODULES		+= examples/flow_position_estimator
 
diff --git a/src/drivers/drv_tone_alarm.h b/src/drivers/drv_tone_alarm.h
index cb5fd53a5..147d7123a 100644
--- a/src/drivers/drv_tone_alarm.h
+++ b/src/drivers/drv_tone_alarm.h
@@ -147,6 +147,7 @@ enum {
 	TONE_BATTERY_WARNING_SLOW_TUNE,
 	TONE_BATTERY_WARNING_FAST_TUNE,
 	TONE_GPS_WARNING_TUNE,
+	TONE_ARMING_FAILURE_TUNE,
 	TONE_NUMBER_OF_TUNES
 };
 
diff --git a/src/drivers/ets_airspeed/ets_airspeed.cpp b/src/drivers/ets_airspeed/ets_airspeed.cpp
index d27ab9727..d873a1132 100644
--- a/src/drivers/ets_airspeed/ets_airspeed.cpp
+++ b/src/drivers/ets_airspeed/ets_airspeed.cpp
@@ -176,11 +176,14 @@ ETSAirspeed::collect()
 		_max_differential_pressure_pa = diff_pres_pa;
 	}
 
-	// XXX we may want to smooth out the readings to remove noise.
 	differential_pressure_s report;
 	report.timestamp = hrt_absolute_time();
         report.error_count = perf_event_count(_comms_errors);
 	report.differential_pressure_pa = (float)diff_pres_pa;
+
+	// XXX we may want to smooth out the readings to remove noise.
+	report.differential_pressure_filtered_pa = (float)diff_pres_pa;
+	report.temperature = -1000.0f;
 	report.voltage = 0;
 	report.max_differential_pressure_pa = _max_differential_pressure_pa;
 
diff --git a/src/drivers/hmc5883/hmc5883.cpp b/src/drivers/hmc5883/hmc5883.cpp
index 4c85c0cda..cb97354ec 100644
--- a/src/drivers/hmc5883/hmc5883.cpp
+++ b/src/drivers/hmc5883/hmc5883.cpp
@@ -169,6 +169,8 @@ private:
 
 	int			_bus;			/**< the bus the device is connected to */
 
+	struct mag_report	_last_report;           /**< used for info() */
+
 	/**
 	 * Test whether the device supported by the driver is present at a
 	 * specific address.
@@ -870,6 +872,8 @@ HMC5883::collect()
 		}
 	}
 
+	_last_report = new_report;
+
 	/* post a report to the ring */
 	if (_reports->force(&new_report)) {
 		perf_count(_buffer_overflows);
@@ -1042,31 +1046,28 @@ int HMC5883::calibrate(struct file *filp, unsigned enable)
 
 	warnx("axes scaling: %.6f  %.6f  %.6f", (double)scaling[0], (double)scaling[1], (double)scaling[2]);
 
-	/* set back to normal mode */
-	/* Set to 1.1 Gauss */
-	if (OK != ::ioctl(fd, MAGIOCSRANGE, 1)) {
-		warnx("failed to set 1.1 Ga range");
-		goto out;
-	}
-
-	if (OK != ::ioctl(fd, MAGIOCEXSTRAP, 0)) {
-		warnx("failed to disable sensor calibration mode");
-		goto out;
-	}
-
 	/* set scaling in device */
 	mscale_previous.x_scale = scaling[0];
 	mscale_previous.y_scale = scaling[1];
 	mscale_previous.z_scale = scaling[2];
 
+	ret = OK;
+
+out:
+
 	if (OK != ioctl(filp, MAGIOCSSCALE, (long unsigned int)&mscale_previous)) {
 		warn("WARNING: failed to set new scale / offsets for mag");
-		goto out;
 	}
 
-	ret = OK;
+	/* set back to normal mode */
+	/* Set to 1.1 Gauss */
+	if (OK != ::ioctl(fd, MAGIOCSRANGE, 1)) {
+		warnx("failed to set 1.1 Ga range");
+	}
 
-out:
+	if (OK != ::ioctl(fd, MAGIOCEXSTRAP, 0)) {
+		warnx("failed to disable sensor calibration mode");
+	}
 
 	if (ret == OK) {
 		if (!check_scale()) {
@@ -1221,6 +1222,7 @@ HMC5883::print_info()
 	perf_print_counter(_comms_errors);
 	perf_print_counter(_buffer_overflows);
 	printf("poll interval:  %u ticks\n", _measure_ticks);
+	printf("output  (%.2f %.2f %.2f)\n", (double)_last_report.x, (double)_last_report.y, (double)_last_report.z);
 	printf("offsets (%.2f %.2f %.2f)\n", (double)_scale.x_offset, (double)_scale.y_offset, (double)_scale.z_offset);
 	printf("scaling (%.2f %.2f %.2f) 1/range_scale %.2f range_ga %.2f\n", 
 	       (double)_scale.x_scale, (double)_scale.y_scale, (double)_scale.z_scale,
diff --git a/src/drivers/meas_airspeed/meas_airspeed.cpp b/src/drivers/meas_airspeed/meas_airspeed.cpp
index 2c3efdc35..1ad383ee0 100644
--- a/src/drivers/meas_airspeed/meas_airspeed.cpp
+++ b/src/drivers/meas_airspeed/meas_airspeed.cpp
@@ -87,6 +87,7 @@
 #include <uORB/uORB.h>
 #include <uORB/topics/differential_pressure.h>
 #include <uORB/topics/subsystem_info.h>
+#include <uORB/topics/system_power.h>
 
 #include <drivers/airspeed/airspeed.h>
 
@@ -121,6 +122,14 @@ protected:
 	virtual int	collect();
 
 	math::LowPassFilter2p	_filter;
+
+	/**
+	 * Correct for 5V rail voltage variations
+	 */
+	void voltage_correction(float &diff_pres_pa, float &temperature);
+
+	int _t_system_power;
+	struct system_power_s system_power;
 };
 
 /*
@@ -129,10 +138,11 @@ protected:
 extern "C" __EXPORT int meas_airspeed_main(int argc, char *argv[]);
 
 MEASAirspeed::MEASAirspeed(int bus, int address, const char *path) : Airspeed(bus, address,
-			CONVERSION_INTERVAL, path),
-			_filter(MEAS_RATE, MEAS_DRIVER_FILTER_FREQ)
+	CONVERSION_INTERVAL, path),
+	_filter(MEAS_RATE, MEAS_DRIVER_FILTER_FREQ),
+	_t_system_power(-1)
 {
-
+	memset(&system_power, 0, sizeof(system_power));
 }
 
 int
@@ -194,19 +204,48 @@ MEASAirspeed::collect()
 	dp_raw = 0x3FFF & dp_raw;
 	dT_raw = (val[2] << 8) + val[3];
 	dT_raw = (0xFFE0 & dT_raw) >> 5;
-	float temperature = ((200 * dT_raw) / 2047) - 50;
+	float temperature = ((200.0f * dT_raw) / 2047) - 50;
 
-	/* calculate differential pressure. As its centered around 8000
-	 * and can go positive or negative, enforce absolute value
-	*/
+	// Calculate differential pressure. As its centered around 8000
+	// and can go positive or negative
 	const float P_min = -1.0f;
 	const float P_max = 1.0f;
-	float diff_press_pa = fabsf((((float)dp_raw - 0.1f * 16383.0f) * (P_max - P_min) / (0.8f * 16383.0f) + P_min) * 6894.8f) - _diff_pres_offset;
+	const float PSI_to_Pa = 6894.757f;
+	/*
+	  this equation is an inversion of the equation in the
+	  pressure transfer function figure on page 4 of the datasheet
 
-	if (diff_press_pa < 0.0f) {
-		diff_press_pa = 0.0f;
-	}
+	  We negate the result so that positive differential pressures
+	  are generated when the bottom port is used as the static
+	  port on the pitot and top port is used as the dynamic port
+	 */
+	float diff_press_PSI = -((dp_raw - 0.1f*16383) * (P_max-P_min)/(0.8f*16383) + P_min);
+	float diff_press_pa_raw = diff_press_PSI * PSI_to_Pa;
 
+        // correct for 5V rail voltage if possible
+        voltage_correction(diff_press_pa_raw, temperature);
+
+	float diff_press_pa = fabsf(diff_press_pa_raw - _diff_pres_offset);
+	
+	/*
+	  note that we return both the absolute value with offset
+	  applied and a raw value without the offset applied. This
+	  makes it possible for higher level code to detect if the
+	  user has the tubes connected backwards, and also makes it
+	  possible to correctly use offsets calculated by a higher
+	  level airspeed driver.
+
+	  With the above calculation the MS4525 sensor will produce a
+	  positive number when the top port is used as a dynamic port
+	  and bottom port is used as the static port
+
+	  Also note that the _diff_pres_offset is applied before the
+	  fabsf() not afterwards. It needs to be done this way to
+	  prevent a bias at low speeds, but this also means that when
+	  setting a offset you must set it based on the raw value, not
+	  the offset value
+	 */
+	
 	struct differential_pressure_s report;
 
 	/* track maximum differential pressure measured (so we can work out top speed). */
@@ -219,6 +258,13 @@ MEASAirspeed::collect()
 	report.temperature = temperature;
 	report.differential_pressure_pa = diff_press_pa;
 	report.differential_pressure_filtered_pa =  _filter.apply(diff_press_pa);
+
+	/* the dynamics of the filter can make it overshoot into the negative range */
+	if (report.differential_pressure_filtered_pa < 0.0f) {
+		report.differential_pressure_filtered_pa = _filter.reset(diff_press_pa);
+	}
+
+	report.differential_pressure_raw_pa = diff_press_pa_raw;
 	report.voltage = 0;
 	report.max_differential_pressure_pa = _max_differential_pressure_pa;
 
@@ -288,6 +334,62 @@ MEASAirspeed::cycle()
 }
 
 /**
+   correct for 5V rail voltage if the system_power ORB topic is
+   available
+   
+   See http://uav.tridgell.net/MS4525/MS4525-offset.png for a graph of
+   offset versus voltage for 3 sensors
+ */
+void
+MEASAirspeed::voltage_correction(float &diff_press_pa, float &temperature)
+{
+#ifdef CONFIG_ARCH_BOARD_PX4FMU_V2
+	if (_t_system_power == -1) {
+		_t_system_power = orb_subscribe(ORB_ID(system_power));
+	}
+	if (_t_system_power == -1) {
+		// not available
+		return;
+	}
+	bool updated = false;
+	orb_check(_t_system_power, &updated);
+	if (updated) {
+		orb_copy(ORB_ID(system_power), _t_system_power, &system_power);
+	}
+	if (system_power.voltage5V_v < 3.0f || system_power.voltage5V_v > 6.0f) {
+		// not valid, skip correction
+		return;
+	}
+
+	const float slope = 65.0f;
+	/*
+	  apply a piecewise linear correction, flattening at 0.5V from 5V
+	 */
+	float voltage_diff = system_power.voltage5V_v - 5.0f;
+	if (voltage_diff > 0.5f) {
+		voltage_diff = 0.5f;
+	}
+	if (voltage_diff < -0.5f) {
+		voltage_diff = -0.5f;
+	}
+	diff_press_pa -= voltage_diff * slope;
+
+	/*
+	  the temperature masurement varies as well
+	 */
+	const float temp_slope = 0.887f;
+	voltage_diff = system_power.voltage5V_v - 5.0f;
+	if (voltage_diff > 0.5f) {
+		voltage_diff = 0.5f;
+	}
+	if (voltage_diff < -1.0f) {
+		voltage_diff = -1.0f;
+	}
+	temperature -= voltage_diff * temp_slope;	
+#endif // CONFIG_ARCH_BOARD_PX4FMU_V2
+}
+
+/**
  * Local functions in support of the shell command.
  */
 namespace meas_airspeed
@@ -409,7 +511,7 @@ test()
 	}
 
 	warnx("single read");
-	warnx("diff pressure: %8.4f pa", (double)report.differential_pressure_pa);
+	warnx("diff pressure: %d pa", (int)report.differential_pressure_pa);
 
 	/* start the sensor polling at 2Hz */
 	if (OK != ioctl(fd, SENSORIOCSPOLLRATE, 2)) {
@@ -437,7 +539,7 @@ test()
 		}
 
 		warnx("periodic read %u", i);
-		warnx("diff pressure: %8.4f pa", (double)report.differential_pressure_pa);
+		warnx("diff pressure: %d pa", (int)report.differential_pressure_pa);
 		warnx("temperature: %d C (0x%02x)", (int)report.temperature, (unsigned) report.temperature);
 	}
 
diff --git a/src/drivers/px4io/px4io.cpp b/src/drivers/px4io/px4io.cpp
index 82f3ba044..e318e206a 100644
--- a/src/drivers/px4io/px4io.cpp
+++ b/src/drivers/px4io/px4io.cpp
@@ -944,8 +944,23 @@ PX4IO::task_main()
 				int pret = io_reg_set(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_VBATT_SCALE, &scaling, 1);
 
 				if (pret != OK) {
-					log("voltage scaling upload failed");
+					log("vscale upload failed");
 				}
+
+				/* send RC throttle failsafe value to IO */
+				int32_t failsafe_param_val;
+				param_t failsafe_param = param_find("RC_FAILS_THR");
+
+				if (failsafe_param > 0) {
+
+					param_get(failsafe_param, &failsafe_param_val);
+					uint16_t failsafe_thr = failsafe_param_val;
+					pret = io_reg_set(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_RC_THR_FAILSAFE_US, &failsafe_thr, 1);
+					if (pret != OK) {
+						log("failsafe upload failed");
+					}
+				}
+
 			}
 
 		}
@@ -1479,10 +1494,11 @@ PX4IO::io_publish_raw_rc()
 	} else {
 		rc_val.input_source = RC_INPUT_SOURCE_UNKNOWN;
 
-		/* we do not know the RC input, only publish if RC OK flag is set */
-		/* if no raw RC, just don't publish */
-		if (!(_status & PX4IO_P_STATUS_FLAGS_RC_OK))
+		/* only keep publishing RC input if we ever got a valid input */
+		if (_rc_last_valid == 0) {
+			/* we have never seen valid RC signals, abort */
 			return OK;
+		}
 	}
 
 	/* lazily advertise on first publication */
diff --git a/src/drivers/sf0x/sf0x.cpp b/src/drivers/sf0x/sf0x.cpp
index 70cd1ab1e..a0cf98340 100644
--- a/src/drivers/sf0x/sf0x.cpp
+++ b/src/drivers/sf0x/sf0x.cpp
@@ -124,6 +124,8 @@ private:
 
 	orb_advert_t			_range_finder_topic;
 
+	unsigned			_consecutive_fail_count;
+
 	perf_counter_t			_sample_perf;
 	perf_counter_t			_comms_errors;
 	perf_counter_t			_buffer_overflows;
@@ -186,6 +188,7 @@ SF0X::SF0X(const char *port) :
 	_linebuf_index(0),
 	_last_read(0),
 	_range_finder_topic(-1),
+	_consecutive_fail_count(0),
 	_sample_perf(perf_alloc(PC_ELAPSED, "sf0x_read")),
 	_comms_errors(perf_alloc(PC_COUNT, "sf0x_comms_errors")),
 	_buffer_overflows(perf_alloc(PC_COUNT, "sf0x_buffer_overflows"))
@@ -276,34 +279,11 @@ SF0X::init()
 		warnx("advert err");
 	}
 
-	/* attempt to get a measurement 5 times */
-	while (ret != OK && i < 5) {
-
-		if (measure()) {
-			ret = ERROR;
-			_sensor_ok = false;
-		}
-
-		usleep(100000);
-
-		if (collect()) {
-			ret = ERROR;
-			_sensor_ok = false;
-
-		} else {
-			ret = OK;
-			/* sensor is ok, but we don't really know if it is within range */
-			_sensor_ok = true;
-		}
-
-		i++;
-	}
-
 	/* close the fd */
 	::close(_fd);
 	_fd = -1;
 out:
-	return ret;
+	return OK;
 }
 
 int
@@ -376,6 +356,7 @@ SF0X::ioctl(struct file *filp, int cmd, unsigned long arg)
 
 			/* adjust to a legal polling interval in Hz */
 			default: {
+
 					/* do we need to start internal polling? */
 					bool want_start = (_measure_ticks == 0);
 
@@ -544,10 +525,16 @@ SF0X::collect()
 
 	if (read_elapsed > (SF0X_CONVERSION_INTERVAL * 2)) {
 		_linebuf_index = 0;
+	} else if (_linebuf_index > 0) {
+		/* increment to next read position */
+		_linebuf_index++;
 	}
 
+	/* the buffer for read chars is buflen minus null termination */
+	unsigned readlen = sizeof(_linebuf) - 1;
+
 	/* read from the sensor (uart buffer) */
-	ret = ::read(_fd, &_linebuf[_linebuf_index], sizeof(_linebuf) - _linebuf_index);
+	ret = ::read(_fd, &_linebuf[_linebuf_index], readlen - _linebuf_index);
 
 	if (ret < 0) {
 		_linebuf[sizeof(_linebuf) - 1] = '\0';
@@ -562,19 +549,30 @@ SF0X::collect()
 		} else {
 			return -EAGAIN;
 		}
+	} else if (ret == 0) {
+		return -EAGAIN;
 	}
 
-	_linebuf_index += ret;
-
-	if (_linebuf_index >= sizeof(_linebuf)) {
-		_linebuf_index = 0;
-	}
+	/* we did increment the index to the next position already, so just add the additional fields */
+	_linebuf_index += (ret - 1);
 
 	_last_read = hrt_absolute_time();
 
-	if (_linebuf[_linebuf_index - 2] != '\r' || _linebuf[_linebuf_index - 1] != '\n') {
-		/* incomplete read, reschedule ourselves */
+	if (_linebuf_index < 1) {
+		/* we need at least the two end bytes to make sense of this string */
 		return -EAGAIN;
+
+	} else if (_linebuf[_linebuf_index - 1] != '\r' || _linebuf[_linebuf_index] != '\n') {
+
+		if (_linebuf_index >= readlen - 1) {
+			/* we have a full buffer, but no line ending - abort */
+			_linebuf_index = 0;
+			perf_count(_comms_errors);
+			return -ENOMEM;
+		} else {
+			/* incomplete read, reschedule ourselves */
+			return -EAGAIN;
+		}
 	}
 
 	char *end;
@@ -582,22 +580,56 @@ SF0X::collect()
 	bool valid;
 
 	/* enforce line ending */
-	_linebuf[sizeof(_linebuf) - 1] = '\0';
+	unsigned lend = (_linebuf_index < (sizeof(_linebuf) - 1)) ? _linebuf_index : (sizeof(_linebuf) - 1);
+
+	_linebuf[lend] = '\0';
 
 	if (_linebuf[0] == '-' && _linebuf[1] == '-' && _linebuf[2] == '.') {
 		si_units = -1.0f;
 		valid = false;
 
 	} else {
-		si_units = strtod(_linebuf, &end);
-		valid = true;
+
+		/* we need to find a dot in the string, as we're missing the meters part else */
+		valid = false;
+
+		/* wipe out partially read content from last cycle(s), check for dot */
+		for (int i = 0; i < (lend - 2); i++) {
+			if (_linebuf[i] == '\n') {
+				char buf[sizeof(_linebuf)];
+				memcpy(buf, &_linebuf[i+1], (lend + 1) - (i + 1));
+				memcpy(_linebuf, buf, (lend + 1) - (i + 1));
+			}
+
+			if (_linebuf[i] == '.') {
+				valid = true;
+			}
+		}
+
+		if (valid) {
+			si_units = strtod(_linebuf, &end);
+
+			/* we require at least 3 characters for a valid number */
+			if (end > _linebuf + 3) {
+				valid = true;
+			} else {
+				si_units = -1.0f;
+				valid = false;
+			}
+		}
 	}
 
-	debug("val (float): %8.4f, raw: %s\n", si_units, _linebuf);
+	debug("val (float): %8.4f, raw: %s, valid: %s\n", si_units, _linebuf, ((valid) ? "OK" : "NO"));
 
-	/* done with this chunk, resetting */
+	/* done with this chunk, resetting - even if invalid */
 	_linebuf_index = 0;
 
+	/* if its invalid, there is no reason to forward the value */
+	if (!valid) {
+		perf_count(_comms_errors);
+		return -EINVAL;
+	}
+
 	struct range_finder_report report;
 
 	/* this should be fairly close to the end of the measurement, so the best approximation of the time */
@@ -689,10 +721,19 @@ SF0X::cycle()
 		}
 
 		if (OK != collect_ret) {
-			log("collection error");
+
+			/* we know the sensor needs about four seconds to initialize */
+			if (hrt_absolute_time() > 5 * 1000 * 1000LL && _consecutive_fail_count < 5) {
+				log("collection error #%u", _consecutive_fail_count);
+			}
+			_consecutive_fail_count++;
+
 			/* restart the measurement state machine */
 			start();
 			return;
+		} else {
+			/* apparently success */
+			_consecutive_fail_count = 0;
 		}
 
 		/* next phase is measurement */
@@ -848,10 +889,10 @@ test()
 	}
 
 	warnx("single read");
-	warnx("measurement: %0.2f m", (double)report.distance);
-	warnx("time:        %lld", report.timestamp);
+	warnx("val:  %0.2f m", (double)report.distance);
+	warnx("time: %lld", report.timestamp);
 
-	/* start the sensor polling at 2Hz */
+	/* start the sensor polling at 2 Hz rate */
 	if (OK != ioctl(fd, SENSORIOCSPOLLRATE, 2)) {
 		errx(1, "failed to set 2Hz poll rate");
 	}
@@ -866,24 +907,26 @@ test()
 		int ret = poll(&fds, 1, 2000);
 
 		if (ret != 1) {
-			errx(1, "timed out waiting for sensor data");
+			warnx("timed out");
+			break;
 		}
 
 		/* now go get it */
 		sz = read(fd, &report, sizeof(report));
 
 		if (sz != sizeof(report)) {
-			err(1, "periodic read failed");
+			warnx("read failed: got %d vs exp. %d", sz, sizeof(report));
+			break;
 		}
 
-		warnx("periodic read %u", i);
-		warnx("measurement: %0.3f", (double)report.distance);
-		warnx("time:        %lld", report.timestamp);
+		warnx("read #%u", i);
+		warnx("val:  %0.3f m", (double)report.distance);
+		warnx("time: %lld", report.timestamp);
 	}
 
-	/* reset the sensor polling to default rate */
+	/* reset the sensor polling to the default rate */
 	if (OK != ioctl(fd, SENSORIOCSPOLLRATE, SENSOR_POLLRATE_DEFAULT)) {
-		errx(1, "failed to set default poll rate");
+		errx(1, "ERR: DEF RATE");
 	}
 
 	errx(0, "PASS");
diff --git a/src/drivers/stm32/adc/adc.cpp b/src/drivers/stm32/adc/adc.cpp
index 00e46d6b8..3a60d2cae 100644
--- a/src/drivers/stm32/adc/adc.cpp
+++ b/src/drivers/stm32/adc/adc.cpp
@@ -41,6 +41,7 @@
  */
 
 #include <nuttx/config.h>
+#include <board_config.h>
 #include <drivers/device/device.h>
 
 #include <sys/types.h>
@@ -64,6 +65,8 @@
 #include <systemlib/err.h>
 #include <systemlib/perf_counter.h>
 
+#include <uORB/topics/system_power.h>
+
 /*
  * Register accessors.
  * For now, no reason not to just use ADC1.
@@ -119,6 +122,8 @@ private:
 	unsigned		_channel_count;
 	adc_msg_s		*_samples;		/**< sample buffer */
 
+	orb_advert_t		_to_system_power;
+
 	/** work trampoline */
 	static void		_tick_trampoline(void *arg);
 
@@ -134,13 +139,16 @@ private:
 	 */
 	uint16_t		_sample(unsigned channel);
 
+	// update system_power ORB topic, only on FMUv2
+	void update_system_power(void);
 };
 
 ADC::ADC(uint32_t channels) :
 	CDev("adc", ADC_DEVICE_PATH),
 	_sample_perf(perf_alloc(PC_ELAPSED, "ADC samples")),
 	_channel_count(0),
-	_samples(nullptr)
+	_samples(nullptr),
+	_to_system_power(0)
 {
 	_debug_enabled = true;
 
@@ -290,6 +298,43 @@ ADC::_tick()
 	/* scan the channel set and sample each */
 	for (unsigned i = 0; i < _channel_count; i++)
 		_samples[i].am_data = _sample(_samples[i].am_channel);
+	update_system_power();
+}
+
+void
+ADC::update_system_power(void)
+{
+#ifdef CONFIG_ARCH_BOARD_PX4FMU_V2
+	system_power_s system_power;
+	system_power.timestamp = hrt_absolute_time();
+
+	system_power.voltage5V_v = 0;
+	for (unsigned i = 0; i < _channel_count; i++) {
+		if (_samples[i].am_channel == 4) {
+			// it is 2:1 scaled
+			system_power.voltage5V_v = _samples[i].am_data * (6.6f / 4096);
+		}
+	}
+
+	// these are not ADC related, but it is convenient to
+	// publish these to the same topic
+	system_power.usb_connected = stm32_gpioread(GPIO_OTGFS_VBUS);
+
+	// note that the valid pins are active low
+	system_power.brick_valid   = !stm32_gpioread(GPIO_VDD_BRICK_VALID);
+	system_power.servo_valid   = !stm32_gpioread(GPIO_VDD_SERVO_VALID);
+
+	// OC pins are active low
+	system_power.periph_5V_OC  = !stm32_gpioread(GPIO_VDD_5V_PERIPH_OC);
+	system_power.hipower_5V_OC = !stm32_gpioread(GPIO_VDD_5V_HIPOWER_OC);
+
+	/* lazily publish */
+	if (_to_system_power > 0) {
+		orb_publish(ORB_ID(system_power), _to_system_power, &system_power);
+	} else {
+		_to_system_power = orb_advertise(ORB_ID(system_power), &system_power);
+	}
+#endif // CONFIG_ARCH_BOARD_PX4FMU_V2
 }
 
 uint16_t
@@ -341,7 +386,7 @@ test(void)
 		err(1, "can't open ADC device");
 
 	for (unsigned i = 0; i < 50; i++) {
-		adc_msg_s data[10];
+		adc_msg_s data[12];
 		ssize_t count = read(fd, data, sizeof(data));
 
 		if (count < 0)
diff --git a/src/drivers/stm32/tone_alarm/tone_alarm.cpp b/src/drivers/stm32/tone_alarm/tone_alarm.cpp
index f36f2091e..8ed0de58c 100644
--- a/src/drivers/stm32/tone_alarm/tone_alarm.cpp
+++ b/src/drivers/stm32/tone_alarm/tone_alarm.cpp
@@ -334,6 +334,7 @@ ToneAlarm::ToneAlarm() :
 	_default_tunes[TONE_BATTERY_WARNING_SLOW_TUNE] = "MBNT100a8";					//battery warning slow
 	_default_tunes[TONE_BATTERY_WARNING_FAST_TUNE] = "MBNT255a8a8a8a8a8a8a8a8a8a8a8a8a8a8a8a8";	//battery warning fast
 	_default_tunes[TONE_GPS_WARNING_TUNE] = "MFT255L4AAAL1F#";					//gps warning slow
+	_default_tunes[TONE_ARMING_FAILURE_TUNE] = "MFT255L4<<<BAP";
 
 	_tune_names[TONE_STARTUP_TUNE] = "startup";			// startup tune
 	_tune_names[TONE_ERROR_TUNE] = "error";				// ERROR tone
@@ -344,6 +345,7 @@ ToneAlarm::ToneAlarm() :
 	_tune_names[TONE_BATTERY_WARNING_SLOW_TUNE] = "slow_bat";	// battery warning slow
 	_tune_names[TONE_BATTERY_WARNING_FAST_TUNE] = "fast_bat";	// battery warning fast
 	_tune_names[TONE_GPS_WARNING_TUNE] = "gps_warning";	            // gps warning
+	_tune_names[TONE_ARMING_FAILURE_TUNE] = "arming_failure";            //fail to arm
 }
 
 ToneAlarm::~ToneAlarm()
diff --git a/src/lib/geo/geo.c b/src/lib/geo/geo.c
index 59c04f0e3..f72dc607c 100644
--- a/src/lib/geo/geo.c
+++ b/src/lib/geo/geo.c
@@ -197,7 +197,6 @@ __EXPORT float get_bearing_to_next_waypoint(double lat_now, double lon_now, doub
 	double lat_next_rad = lat_next * M_DEG_TO_RAD;
 	double lon_next_rad = lon_next * M_DEG_TO_RAD;
 
-	double d_lat = lat_next_rad - lat_now_rad;
 	double d_lon = lon_next_rad - lon_now_rad;
 
 	/* conscious mix of double and float trig function to maximize speed and efficiency */
diff --git a/src/lib/mathlib/math/filter/LowPassFilter2p.cpp b/src/lib/mathlib/math/filter/LowPassFilter2p.cpp
index 3699d9bce..6f640c9f9 100644
--- a/src/lib/mathlib/math/filter/LowPassFilter2p.cpp
+++ b/src/lib/mathlib/math/filter/LowPassFilter2p.cpp
@@ -69,7 +69,7 @@ float LowPassFilter2p::apply(float sample)
     // do the filtering
     float delay_element_0 = sample - _delay_element_1 * _a1 - _delay_element_2 * _a2;
     if (isnan(delay_element_0) || isinf(delay_element_0)) {
-        // don't allow bad values to propogate via the filter
+        // don't allow bad values to propagate via the filter
         delay_element_0 = sample;
     }
     float output = delay_element_0 * _b0 + _delay_element_1 * _b1 + _delay_element_2 * _b2;
@@ -81,5 +81,10 @@ float LowPassFilter2p::apply(float sample)
     return output;
 }
 
+float LowPassFilter2p::reset(float sample) {
+    _delay_element_1 = _delay_element_2 = sample;
+    return apply(sample);
+}
+
 } // namespace math
 
diff --git a/src/lib/mathlib/math/filter/LowPassFilter2p.hpp b/src/lib/mathlib/math/filter/LowPassFilter2p.hpp
index 208ec98d4..74cd5d78c 100644
--- a/src/lib/mathlib/math/filter/LowPassFilter2p.hpp
+++ b/src/lib/mathlib/math/filter/LowPassFilter2p.hpp
@@ -52,18 +52,30 @@ public:
         _delay_element_1 = _delay_element_2 = 0;
     }
 
-    // change parameters
+    /**
+     * Change filter parameters
+     */
     void set_cutoff_frequency(float sample_freq, float cutoff_freq);
 
-    // apply - Add a new raw value to the filter 
-    // and retrieve the filtered result
+    /**
+     * Add a new raw value to the filter
+     *
+     * @return retrieve the filtered result
+     */
     float apply(float sample);
 
-    // return the cutoff frequency
+    /**
+     * Return the cutoff frequency
+     */
     float get_cutoff_freq(void) const {
         return _cutoff_freq;
     }
 
+    /**
+     * Reset the filter state to this value
+     */
+    float reset(float sample);
+
 private:
     float           _cutoff_freq; 
     float           _a1;
diff --git a/src/modules/commander/airspeed_calibration.cpp b/src/modules/commander/airspeed_calibration.cpp
index 6039d92a7..c8c7a42e7 100644
--- a/src/modules/commander/airspeed_calibration.cpp
+++ b/src/modules/commander/airspeed_calibration.cpp
@@ -1,6 +1,6 @@
 /****************************************************************************
  *
- *   Copyright (C) 2013 PX4 Development Team. All rights reserved.
+ *   Copyright (c) 2013, 2014 PX4 Development Team. All rights reserved.
  *
  * Redistribution and use in source and binary forms, with or without
  * modification, are permitted provided that the following conditions
@@ -64,9 +64,9 @@ int do_airspeed_calibration(int mavlink_fd)
 {
 	/* give directions */
 	mavlink_log_info(mavlink_fd, CAL_STARTED_MSG, sensor_name);
-	mavlink_log_info(mavlink_fd, "don't move system");
+	mavlink_log_info(mavlink_fd, "ensure airspeed sensor is not registering wind");
 
-	const int calibration_count = 2500;
+	const int calibration_count = 2000;
 
 	int diff_pres_sub = orb_subscribe(ORB_ID(differential_pressure));
 	struct differential_pressure_s diff_pres;
@@ -85,13 +85,15 @@ int do_airspeed_calibration(int mavlink_fd)
 	if (fd > 0) {
 		if (OK == ioctl(fd, AIRSPEEDIOCSSCALE, (long unsigned int)&airscale)) {
 			paramreset_successful = true;
+		} else {
+			mavlink_log_critical(mavlink_fd, "airspeed offset zero failed");
 		}
 		close(fd);
 	}
 
 	if (!paramreset_successful) {
-		warn("WARNING: failed to set scale / offsets for airspeed sensor");
-		mavlink_log_critical(mavlink_fd, "could not reset dpress sensor");
+		warn("FAILED to set scale / offsets for airspeed");
+		mavlink_log_critical(mavlink_fd, "dpress reset failed");
 		mavlink_log_info(mavlink_fd, CAL_FAILED_MSG, sensor_name);
 		return ERROR;
 	}
@@ -107,7 +109,7 @@ int do_airspeed_calibration(int mavlink_fd)
 
 		if (poll_ret) {
 			orb_copy(ORB_ID(differential_pressure), diff_pres_sub, &diff_pres);
-			diff_pres_offset += diff_pres.differential_pressure_pa;
+			diff_pres_offset += diff_pres.differential_pressure_raw_pa;
 			calibration_counter++;
 
 			if (calibration_counter % (calibration_count / 20) == 0)
diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp
index cf7ba757e..3720b5a3b 100644
--- a/src/modules/commander/commander.cpp
+++ b/src/modules/commander/commander.cpp
@@ -118,8 +118,7 @@ extern struct system_load_s system_load;
 #define STICK_ON_OFF_COUNTER_LIMIT (STICK_ON_OFF_HYSTERESIS_TIME_MS*COMMANDER_MONITORING_LOOPSPERMSEC)
 
 #define POSITION_TIMEOUT 1000000 /**< consider the local or global position estimate invalid after 1s */
-#define RC_TIMEOUT 100000
-#define RC_TIMEOUT_HIL 500000
+#define RC_TIMEOUT 500000
 #define DIFFPRESS_TIMEOUT 2000000
 
 #define PRINT_INTERVAL	5000000
@@ -1109,16 +1108,8 @@ int commander_thread_main(int argc, char *argv[])
 			}
 		}
 
-
-		/*
-		 * XXX workaround:
-		 * Prevent RC loss in HIL when sensors.cpp is only publishing sp_man at a low rate (e.g. 30Hz)
-		 * which can trigger RC loss if the computer/simulator lags.
-		 */
-		uint64_t rc_timeout = status.hil_state == HIL_STATE_ON ? RC_TIMEOUT_HIL : RC_TIMEOUT;
-
 		/* start RC input check */
-		if (!status.rc_input_blocked && sp_man.timestamp != 0 && hrt_absolute_time() < sp_man.timestamp + rc_timeout) {
+		if (!status.rc_input_blocked && sp_man.timestamp != 0 && hrt_absolute_time() < sp_man.timestamp + RC_TIMEOUT) {
 			/* handle the case where RC signal was regained */
 			if (!status.rc_signal_found_once) {
 				status.rc_signal_found_once = true;
diff --git a/src/modules/fw_att_control/fw_att_control_main.cpp b/src/modules/fw_att_control/fw_att_control_main.cpp
index f139c25f4..2f84dc963 100644
--- a/src/modules/fw_att_control/fw_att_control_main.cpp
+++ b/src/modules/fw_att_control/fw_att_control_main.cpp
@@ -660,18 +660,24 @@ FixedwingAttitudeControl::task_main()
 
 				float airspeed;
 
-				/* if airspeed is smaller than min, the sensor is not giving good readings */
-				if ((_airspeed.indicated_airspeed_m_s < 0.5f * _parameters.airspeed_min) ||
-				    !isfinite(_airspeed.indicated_airspeed_m_s) ||
+				/* if airspeed is not updating, we assume the normal average speed */
+				if (!isfinite(_airspeed.true_airspeed_m_s) ||
 				    hrt_elapsed_time(&_airspeed.timestamp) > 1e6) {
 					airspeed = _parameters.airspeed_trim;
 
 				} else {
-					airspeed = _airspeed.indicated_airspeed_m_s;
+					airspeed = _airspeed.true_airspeed_m_s;
 				}
 
-				float airspeed_scaling = _parameters.airspeed_trim / airspeed;
-				//warnx("aspd scale: %6.2f act scale: %6.2f", airspeed_scaling, actuator_scaling);
+				/*
+				 * For scaling our actuators using anything less than the min (close to stall)
+				 * speed doesn't make any sense - its the strongest reasonable deflection we
+				 * want to do in flight and its the baseline a human pilot would choose.
+				 *
+				 * Forcing the scaling to this value allows reasonable handheld tests.
+				 */
+
+				float airspeed_scaling = _parameters.airspeed_trim / ((airspeed < _parameters.airspeed_min) ? _parameters.airspeed_min : airspeed);
 
 				float roll_sp = _parameters.rollsp_offset_rad;
 				float pitch_sp = _parameters.pitchsp_offset_rad;
diff --git a/src/modules/fw_att_pos_estimator/estimator.cpp b/src/modules/fw_att_pos_estimator/estimator.cpp
new file mode 100644
index 000000000..c31217393
--- /dev/null
+++ b/src/modules/fw_att_pos_estimator/estimator.cpp
@@ -0,0 +1,2248 @@
+#include "estimator.h"
+
+#include <string.h>
+
+float Vector3f::length(void) const
+{
+    return sqrt(x*x + y*y + z*z);
+}
+
+Vector3f Vector3f::zero(void) const
+{
+    Vector3f ret = *this;
+    ret.x = 0.0;
+    ret.y = 0.0;
+    ret.z = 0.0;
+    return ret;
+}
+
+Mat3f::Mat3f() {
+    x.x = 1.0f;
+    x.y = 0.0f;
+    x.z = 0.0f;
+
+    y.x = 0.0f;
+    y.y = 1.0f;
+    y.z = 0.0f;
+
+    z.x = 0.0f;
+    z.y = 0.0f;
+    z.z = 1.0f;
+}
+
+Mat3f Mat3f::transpose(void) const
+{
+    Mat3f ret = *this;
+    swap_var(ret.x.y, ret.y.x);
+    swap_var(ret.x.z, ret.z.x);
+    swap_var(ret.y.z, ret.z.y);
+    return ret;
+}
+
+// overload + operator to provide a vector addition
+Vector3f operator+( Vector3f vecIn1, Vector3f vecIn2)
+{
+    Vector3f vecOut;
+    vecOut.x = vecIn1.x + vecIn2.x;
+    vecOut.y = vecIn1.y + vecIn2.y;
+    vecOut.z = vecIn1.z + vecIn2.z;
+    return vecOut;
+}
+
+// overload - operator to provide a vector subtraction
+Vector3f operator-( Vector3f vecIn1, Vector3f vecIn2)
+{
+    Vector3f vecOut;
+    vecOut.x = vecIn1.x - vecIn2.x;
+    vecOut.y = vecIn1.y - vecIn2.y;
+    vecOut.z = vecIn1.z - vecIn2.z;
+    return vecOut;
+}
+
+// overload * operator to provide a matrix vector product
+Vector3f operator*( Mat3f matIn, Vector3f vecIn)
+{
+    Vector3f vecOut;
+    vecOut.x = matIn.x.x*vecIn.x + matIn.x.y*vecIn.y + matIn.x.z*vecIn.z;
+    vecOut.y = matIn.y.x*vecIn.x + matIn.y.y*vecIn.y + matIn.y.z*vecIn.z;
+    vecOut.z = matIn.x.x*vecIn.x + matIn.z.y*vecIn.y + matIn.z.z*vecIn.z;
+    return vecOut;
+}
+
+// overload % operator to provide a vector cross product
+Vector3f operator%( Vector3f vecIn1, Vector3f vecIn2)
+{
+    Vector3f vecOut;
+    vecOut.x = vecIn1.y*vecIn2.z - vecIn1.z*vecIn2.y;
+    vecOut.y = vecIn1.z*vecIn2.x - vecIn1.x*vecIn2.z;
+    vecOut.z = vecIn1.x*vecIn2.y - vecIn1.y*vecIn2.x;
+    return vecOut;
+}
+
+// overload * operator to provide a vector scaler product
+Vector3f operator*(Vector3f vecIn1, float sclIn1)
+{
+    Vector3f vecOut;
+    vecOut.x = vecIn1.x * sclIn1;
+    vecOut.y = vecIn1.y * sclIn1;
+    vecOut.z = vecIn1.z * sclIn1;
+    return vecOut;
+}
+
+// overload * operator to provide a vector scaler product
+Vector3f operator*(float sclIn1, Vector3f vecIn1)
+{
+    Vector3f vecOut;
+    vecOut.x = vecIn1.x * sclIn1;
+    vecOut.y = vecIn1.y * sclIn1;
+    vecOut.z = vecIn1.z * sclIn1;
+    return vecOut;
+}
+
+void swap_var(float &d1, float &d2)
+{
+    float tmp = d1;
+    d1 = d2;
+    d2 = tmp;
+}
+
+AttPosEKF::AttPosEKF() :
+    fusionModeGPS(0),
+    covSkipCount(0),
+    EAS2TAS(1.0f),
+    statesInitialised(false),
+    fuseVelData(false),
+    fusePosData(false),
+    fuseHgtData(false),
+    fuseMagData(false),
+    fuseVtasData(false),
+    onGround(true),
+    staticMode(true),
+    useAirspeed(true),
+    useCompass(true),
+    numericalProtection(true),
+    storeIndex(0)
+{
+
+}
+
+AttPosEKF::~AttPosEKF()
+{
+}
+
+void AttPosEKF::UpdateStrapdownEquationsNED()
+{
+    Vector3f delVelNav;
+    float q00;
+    float q11;
+    float q22;
+    float q33;
+    float q01;
+    float q02;
+    float q03;
+    float q12;
+    float q13;
+    float q23;
+    Mat3f Tbn;
+    Mat3f Tnb;
+    float rotationMag;
+    float qUpdated[4];
+    float quatMag;
+    float deltaQuat[4];
+    const Vector3f gravityNED = {0.0,0.0,GRAVITY_MSS};
+
+// Remove sensor bias errors
+    correctedDelAng.x = dAngIMU.x - states[10];
+    correctedDelAng.y = dAngIMU.y - states[11];
+    correctedDelAng.z = dAngIMU.z - states[12];
+    dVelIMU.x = dVelIMU.x;
+    dVelIMU.y = dVelIMU.y;
+    dVelIMU.z = dVelIMU.z;
+
+// Save current measurements
+    Vector3f  prevDelAng = correctedDelAng;
+
+// Apply corrections for earths rotation rate and coning errors
+// * and + operators have been overloaded
+    correctedDelAng   = correctedDelAng - Tnb*earthRateNED*dtIMU + 8.333333333333333e-2f*(prevDelAng % correctedDelAng);
+
+// Convert the rotation vector to its equivalent quaternion
+    rotationMag = correctedDelAng.length();
+    if (rotationMag < 1e-12f)
+    {
+        deltaQuat[0] = 1.0;
+        deltaQuat[1] = 0.0;
+        deltaQuat[2] = 0.0;
+        deltaQuat[3] = 0.0;
+    }
+    else
+    {
+        deltaQuat[0] = cos(0.5f*rotationMag);
+        float rotScaler = (sin(0.5f*rotationMag))/rotationMag;
+        deltaQuat[1] = correctedDelAng.x*rotScaler;
+        deltaQuat[2] = correctedDelAng.y*rotScaler;
+        deltaQuat[3] = correctedDelAng.z*rotScaler;
+    }
+
+// Update the quaternions by rotating from the previous attitude through
+// the delta angle rotation quaternion
+    qUpdated[0] = states[0]*deltaQuat[0] - states[1]*deltaQuat[1] - states[2]*deltaQuat[2] - states[3]*deltaQuat[3];
+    qUpdated[1] = states[0]*deltaQuat[1] + states[1]*deltaQuat[0] + states[2]*deltaQuat[3] - states[3]*deltaQuat[2];
+    qUpdated[2] = states[0]*deltaQuat[2] + states[2]*deltaQuat[0] + states[3]*deltaQuat[1] - states[1]*deltaQuat[3];
+    qUpdated[3] = states[0]*deltaQuat[3] + states[3]*deltaQuat[0] + states[1]*deltaQuat[2] - states[2]*deltaQuat[1];
+
+// Normalise the quaternions and update the quaternion states
+    quatMag = sqrtf(sq(qUpdated[0]) + sq(qUpdated[1]) + sq(qUpdated[2]) + sq(qUpdated[3]));
+    if (quatMag > 1e-16f)
+    {
+        float quatMagInv = 1.0f/quatMag;
+        states[0] = quatMagInv*qUpdated[0];
+        states[1] = quatMagInv*qUpdated[1];
+        states[2] = quatMagInv*qUpdated[2];
+        states[3] = quatMagInv*qUpdated[3];
+    }
+
+// Calculate the body to nav cosine matrix
+    q00 = sq(states[0]);
+    q11 = sq(states[1]);
+    q22 = sq(states[2]);
+    q33 = sq(states[3]);
+    q01 =  states[0]*states[1];
+    q02 =  states[0]*states[2];
+    q03 =  states[0]*states[3];
+    q12 =  states[1]*states[2];
+    q13 =  states[1]*states[3];
+    q23 =  states[2]*states[3];
+
+    Tbn.x.x = q00 + q11 - q22 - q33;
+    Tbn.y.y = q00 - q11 + q22 - q33;
+    Tbn.z.z = q00 - q11 - q22 + q33;
+    Tbn.x.y = 2*(q12 - q03);
+    Tbn.x.z = 2*(q13 + q02);
+    Tbn.y.x = 2*(q12 + q03);
+    Tbn.y.z = 2*(q23 - q01);
+    Tbn.z.x = 2*(q13 - q02);
+    Tbn.z.y = 2*(q23 + q01);
+
+    Tnb = Tbn.transpose();
+
+// transform body delta velocities to delta velocities in the nav frame
+// * and + operators have been overloaded
+    //delVelNav = Tbn*dVelIMU + gravityNED*dtIMU;
+    delVelNav.x = Tbn.x.x*dVelIMU.x + Tbn.x.y*dVelIMU.y + Tbn.x.z*dVelIMU.z + gravityNED.x*dtIMU;
+    delVelNav.y = Tbn.y.x*dVelIMU.x + Tbn.y.y*dVelIMU.y + Tbn.y.z*dVelIMU.z + gravityNED.y*dtIMU;
+    delVelNav.z = Tbn.z.x*dVelIMU.x + Tbn.z.y*dVelIMU.y + Tbn.z.z*dVelIMU.z + gravityNED.z*dtIMU;
+
+// calculate the magnitude of the nav acceleration (required for GPS
+// variance estimation)
+    accNavMag = delVelNav.length()/dtIMU;
+
+// If calculating position save previous velocity
+    float lastVelocity[3];
+    lastVelocity[0] = states[4];
+    lastVelocity[1] = states[5];
+    lastVelocity[2] = states[6];
+
+// Sum delta velocities to get velocity
+    states[4] = states[4] + delVelNav.x;
+    states[5] = states[5] + delVelNav.y;
+    states[6] = states[6] + delVelNav.z;
+
+// If calculating postions, do a trapezoidal integration for position
+    states[7] = states[7] + 0.5f*(states[4] + lastVelocity[0])*dtIMU;
+    states[8] = states[8] + 0.5f*(states[5] + lastVelocity[1])*dtIMU;
+    states[9] = states[9] + 0.5f*(states[6] + lastVelocity[2])*dtIMU;
+
+    // Constrain states (to protect against filter divergence)
+    ConstrainStates();
+}
+
+void AttPosEKF::CovariancePrediction(float dt)
+{
+    // scalars
+    float windVelSigma;
+    float dAngBiasSigma;
+    // float dVelBiasSigma;
+    float magEarthSigma;
+    float magBodySigma;
+    float daxCov;
+    float dayCov;
+    float dazCov;
+    float dvxCov;
+    float dvyCov;
+    float dvzCov;
+    float dvx;
+    float dvy;
+    float dvz;
+    float dax;
+    float day;
+    float daz;
+    float q0;
+    float q1;
+    float q2;
+    float q3;
+    float dax_b;
+    float day_b;
+    float daz_b;
+
+    // arrays
+    float processNoise[21];
+    float SF[14];
+    float SG[8];
+    float SQ[11];
+    float SPP[13] = {0};
+    float nextP[21][21];
+
+    // calculate covariance prediction process noise
+    const float yawVarScale = 1.0f;
+    windVelSigma = dt*0.1f;
+    dAngBiasSigma = dt*5.0e-7f;
+    magEarthSigma = dt*3.0e-4f;
+    magBodySigma  = dt*3.0e-4f;
+    for (uint8_t i= 0; i<=9; i++) processNoise[i]  = 1.0e-9f;
+    for (uint8_t i=10; i<=12; i++) processNoise[i] = dAngBiasSigma;
+    if (onGround) processNoise[12] = dAngBiasSigma * yawVarScale;
+    for (uint8_t i=13; i<=14; i++) processNoise[i] = windVelSigma;
+    for (uint8_t i=15; i<=17; i++) processNoise[i] = magEarthSigma;
+    for (uint8_t i=18; i<=20; i++) processNoise[i] = magBodySigma;
+    for (uint8_t i= 0; i<=20; i++) processNoise[i] = sq(processNoise[i]);
+
+    // set variables used to calculate covariance growth
+    dvx = summedDelVel.x;
+    dvy = summedDelVel.y;
+    dvz = summedDelVel.z;
+    dax = summedDelAng.x;
+    day = summedDelAng.y;
+    daz = summedDelAng.z;
+    q0 = states[0];
+    q1 = states[1];
+    q2 = states[2];
+    q3 = states[3];
+    dax_b = states[10];
+    day_b = states[11];
+    daz_b = states[12];
+    daxCov = sq(dt*1.4544411e-2f);
+    dayCov = sq(dt*1.4544411e-2f);
+    dazCov = sq(dt*1.4544411e-2f);
+    if (onGround) dazCov = dazCov * sq(yawVarScale);
+    dvxCov = sq(dt*0.5f);
+    dvyCov = sq(dt*0.5f);
+    dvzCov = sq(dt*0.5f);
+
+    // Predicted covariance calculation
+    SF[0] = 2*dvx*q1 + 2*dvy*q2 + 2*dvz*q3;
+    SF[1] = 2*dvx*q3 + 2*dvy*q0 - 2*dvz*q1;
+    SF[2] = 2*dvx*q0 - 2*dvy*q3 + 2*dvz*q2;
+    SF[3] = day/2 - day_b/2;
+    SF[4] = daz/2 - daz_b/2;
+    SF[5] = dax/2 - dax_b/2;
+    SF[6] = dax_b/2 - dax/2;
+    SF[7] = daz_b/2 - daz/2;
+    SF[8] = day_b/2 - day/2;
+    SF[9] = q1/2;
+    SF[10] = q2/2;
+    SF[11] = q3/2;
+    SF[12] = 2*dvz*q0;
+    SF[13] = 2*dvy*q1;
+
+    SG[0] = q0/2;
+    SG[1] = sq(q3);
+    SG[2] = sq(q2);
+    SG[3] = sq(q1);
+    SG[4] = sq(q0);
+    SG[5] = 2*q2*q3;
+    SG[6] = 2*q1*q3;
+    SG[7] = 2*q1*q2;
+
+    SQ[0] = dvzCov*(SG[5] - 2*q0*q1)*(SG[1] - SG[2] - SG[3] + SG[4]) - dvyCov*(SG[5] + 2*q0*q1)*(SG[1] - SG[2] + SG[3] - SG[4]) + dvxCov*(SG[6] - 2*q0*q2)*(SG[7] + 2*q0*q3);
+    SQ[1] = dvzCov*(SG[6] + 2*q0*q2)*(SG[1] - SG[2] - SG[3] + SG[4]) - dvxCov*(SG[6] - 2*q0*q2)*(SG[1] + SG[2] - SG[3] - SG[4]) + dvyCov*(SG[5] + 2*q0*q1)*(SG[7] - 2*q0*q3);
+    SQ[2] = dvzCov*(SG[5] - 2*q0*q1)*(SG[6] + 2*q0*q2) - dvyCov*(SG[7] - 2*q0*q3)*(SG[1] - SG[2] + SG[3] - SG[4]) - dvxCov*(SG[7] + 2*q0*q3)*(SG[1] + SG[2] - SG[3] - SG[4]);
+    SQ[3] = (dayCov*q1*SG[0])/2 - (dazCov*q1*SG[0])/2 - (daxCov*q2*q3)/4;
+    SQ[4] = (dazCov*q2*SG[0])/2 - (daxCov*q2*SG[0])/2 - (dayCov*q1*q3)/4;
+    SQ[5] = (daxCov*q3*SG[0])/2 - (dayCov*q3*SG[0])/2 - (dazCov*q1*q2)/4;
+    SQ[6] = (daxCov*q1*q2)/4 - (dazCov*q3*SG[0])/2 - (dayCov*q1*q2)/4;
+    SQ[7] = (dazCov*q1*q3)/4 - (daxCov*q1*q3)/4 - (dayCov*q2*SG[0])/2;
+    SQ[8] = (dayCov*q2*q3)/4 - (daxCov*q1*SG[0])/2 - (dazCov*q2*q3)/4;
+    SQ[9] = sq(SG[0]);
+    SQ[10] = sq(q1);
+
+    SPP[0] = SF[12] + SF[13] - 2*dvx*q2;
+    SPP[1] = 2*dvx*q0 - 2*dvy*q3 + 2*dvz*q2;
+    SPP[2] = 2*dvx*q3 + 2*dvy*q0 - 2*dvz*q1;
+    SPP[3] = SF[11];
+    SPP[4] = SF[10];
+    SPP[5] = SF[9];
+    SPP[6] = SF[7];
+    SPP[7] = SF[8];
+
+    nextP[0][0] = P[0][0] + P[1][0]*SF[6] + P[2][0]*SPP[7] + P[3][0]*SPP[6] + P[10][0]*SPP[5] + P[11][0]*SPP[4] + P[12][0]*SPP[3] + (daxCov*SQ[10])/4 + SF[6]*(P[0][1] + P[1][1]*SF[6] + P[2][1]*SPP[7] + P[3][1]*SPP[6] + P[10][1]*SPP[5] + P[11][1]*SPP[4] + P[12][1]*SPP[3]) + SPP[7]*(P[0][2] + P[1][2]*SF[6] + P[2][2]*SPP[7] + P[3][2]*SPP[6] + P[10][2]*SPP[5] + P[11][2]*SPP[4] + P[12][2]*SPP[3]) + SPP[6]*(P[0][3] + P[1][3]*SF[6] + P[2][3]*SPP[7] + P[3][3]*SPP[6] + P[10][3]*SPP[5] + P[11][3]*SPP[4] + P[12][3]*SPP[3]) + SPP[5]*(P[0][10] + P[1][10]*SF[6] + P[2][10]*SPP[7] + P[3][10]*SPP[6] + P[10][10]*SPP[5] + P[11][10]*SPP[4] + P[12][10]*SPP[3]) + SPP[4]*(P[0][11] + P[1][11]*SF[6] + P[2][11]*SPP[7] + P[3][11]*SPP[6] + P[10][11]*SPP[5] + P[11][11]*SPP[4] + P[12][11]*SPP[3]) + SPP[3]*(P[0][12] + P[1][12]*SF[6] + P[2][12]*SPP[7] + P[3][12]*SPP[6] + P[10][12]*SPP[5] + P[11][12]*SPP[4] + P[12][12]*SPP[3]) + (dayCov*sq(q2))/4 + (dazCov*sq(q3))/4;
+    nextP[0][1] = P[0][1] + SQ[8] + P[1][1]*SF[6] + P[2][1]*SPP[7] + P[3][1]*SPP[6] + P[10][1]*SPP[5] + P[11][1]*SPP[4] + P[12][1]*SPP[3] + SF[5]*(P[0][0] + P[1][0]*SF[6] + P[2][0]*SPP[7] + P[3][0]*SPP[6] + P[10][0]*SPP[5] + P[11][0]*SPP[4] + P[12][0]*SPP[3]) + SF[4]*(P[0][2] + P[1][2]*SF[6] + P[2][2]*SPP[7] + P[3][2]*SPP[6] + P[10][2]*SPP[5] + P[11][2]*SPP[4] + P[12][2]*SPP[3]) + SPP[7]*(P[0][3] + P[1][3]*SF[6] + P[2][3]*SPP[7] + P[3][3]*SPP[6] + P[10][3]*SPP[5] + P[11][3]*SPP[4] + P[12][3]*SPP[3]) + SPP[3]*(P[0][11] + P[1][11]*SF[6] + P[2][11]*SPP[7] + P[3][11]*SPP[6] + P[10][11]*SPP[5] + P[11][11]*SPP[4] + P[12][11]*SPP[3]) - SPP[4]*(P[0][12] + P[1][12]*SF[6] + P[2][12]*SPP[7] + P[3][12]*SPP[6] + P[10][12]*SPP[5] + P[11][12]*SPP[4] + P[12][12]*SPP[3]) - (q0*(P[0][10] + P[1][10]*SF[6] + P[2][10]*SPP[7] + P[3][10]*SPP[6] + P[10][10]*SPP[5] + P[11][10]*SPP[4] + P[12][10]*SPP[3]))/2;
+    nextP[0][2] = P[0][2] + SQ[7] + P[1][2]*SF[6] + P[2][2]*SPP[7] + P[3][2]*SPP[6] + P[10][2]*SPP[5] + P[11][2]*SPP[4] + P[12][2]*SPP[3] + SF[3]*(P[0][0] + P[1][0]*SF[6] + P[2][0]*SPP[7] + P[3][0]*SPP[6] + P[10][0]*SPP[5] + P[11][0]*SPP[4] + P[12][0]*SPP[3]) + SF[5]*(P[0][3] + P[1][3]*SF[6] + P[2][3]*SPP[7] + P[3][3]*SPP[6] + P[10][3]*SPP[5] + P[11][3]*SPP[4] + P[12][3]*SPP[3]) + SPP[6]*(P[0][1] + P[1][1]*SF[6] + P[2][1]*SPP[7] + P[3][1]*SPP[6] + P[10][1]*SPP[5] + P[11][1]*SPP[4] + P[12][1]*SPP[3]) - SPP[3]*(P[0][10] + P[1][10]*SF[6] + P[2][10]*SPP[7] + P[3][10]*SPP[6] + P[10][10]*SPP[5] + P[11][10]*SPP[4] + P[12][10]*SPP[3]) + SPP[5]*(P[0][12] + P[1][12]*SF[6] + P[2][12]*SPP[7] + P[3][12]*SPP[6] + P[10][12]*SPP[5] + P[11][12]*SPP[4] + P[12][12]*SPP[3]) - (q0*(P[0][11] + P[1][11]*SF[6] + P[2][11]*SPP[7] + P[3][11]*SPP[6] + P[10][11]*SPP[5] + P[11][11]*SPP[4] + P[12][11]*SPP[3]))/2;
+    nextP[0][3] = P[0][3] + SQ[6] + P[1][3]*SF[6] + P[2][3]*SPP[7] + P[3][3]*SPP[6] + P[10][3]*SPP[5] + P[11][3]*SPP[4] + P[12][3]*SPP[3] + SF[4]*(P[0][0] + P[1][0]*SF[6] + P[2][0]*SPP[7] + P[3][0]*SPP[6] + P[10][0]*SPP[5] + P[11][0]*SPP[4] + P[12][0]*SPP[3]) + SF[3]*(P[0][1] + P[1][1]*SF[6] + P[2][1]*SPP[7] + P[3][1]*SPP[6] + P[10][1]*SPP[5] + P[11][1]*SPP[4] + P[12][1]*SPP[3]) + SF[6]*(P[0][2] + P[1][2]*SF[6] + P[2][2]*SPP[7] + P[3][2]*SPP[6] + P[10][2]*SPP[5] + P[11][2]*SPP[4] + P[12][2]*SPP[3]) + SPP[4]*(P[0][10] + P[1][10]*SF[6] + P[2][10]*SPP[7] + P[3][10]*SPP[6] + P[10][10]*SPP[5] + P[11][10]*SPP[4] + P[12][10]*SPP[3]) - SPP[5]*(P[0][11] + P[1][11]*SF[6] + P[2][11]*SPP[7] + P[3][11]*SPP[6] + P[10][11]*SPP[5] + P[11][11]*SPP[4] + P[12][11]*SPP[3]) - (q0*(P[0][12] + P[1][12]*SF[6] + P[2][12]*SPP[7] + P[3][12]*SPP[6] + P[10][12]*SPP[5] + P[11][12]*SPP[4] + P[12][12]*SPP[3]))/2;
+    nextP[0][4] = P[0][4] + P[1][4]*SF[6] + P[2][4]*SPP[7] + P[3][4]*SPP[6] + P[10][4]*SPP[5] + P[11][4]*SPP[4] + P[12][4]*SPP[3] + SF[2]*(P[0][0] + P[1][0]*SF[6] + P[2][0]*SPP[7] + P[3][0]*SPP[6] + P[10][0]*SPP[5] + P[11][0]*SPP[4] + P[12][0]*SPP[3]) + SF[0]*(P[0][1] + P[1][1]*SF[6] + P[2][1]*SPP[7] + P[3][1]*SPP[6] + P[10][1]*SPP[5] + P[11][1]*SPP[4] + P[12][1]*SPP[3]) + SPP[0]*(P[0][2] + P[1][2]*SF[6] + P[2][2]*SPP[7] + P[3][2]*SPP[6] + P[10][2]*SPP[5] + P[11][2]*SPP[4] + P[12][2]*SPP[3]) - SPP[2]*(P[0][3] + P[1][3]*SF[6] + P[2][3]*SPP[7] + P[3][3]*SPP[6] + P[10][3]*SPP[5] + P[11][3]*SPP[4] + P[12][3]*SPP[3]);
+    nextP[0][5] = P[0][5] + P[1][5]*SF[6] + P[2][5]*SPP[7] + P[3][5]*SPP[6] + P[10][5]*SPP[5] + P[11][5]*SPP[4] + P[12][5]*SPP[3] + SF[1]*(P[0][0] + P[1][0]*SF[6] + P[2][0]*SPP[7] + P[3][0]*SPP[6] + P[10][0]*SPP[5] + P[11][0]*SPP[4] + P[12][0]*SPP[3]) + SF[0]*(P[0][2] + P[1][2]*SF[6] + P[2][2]*SPP[7] + P[3][2]*SPP[6] + P[10][2]*SPP[5] + P[11][2]*SPP[4] + P[12][2]*SPP[3]) + SF[2]*(P[0][3] + P[1][3]*SF[6] + P[2][3]*SPP[7] + P[3][3]*SPP[6] + P[10][3]*SPP[5] + P[11][3]*SPP[4] + P[12][3]*SPP[3]) - SPP[0]*(P[0][1] + P[1][1]*SF[6] + P[2][1]*SPP[7] + P[3][1]*SPP[6] + P[10][1]*SPP[5] + P[11][1]*SPP[4] + P[12][1]*SPP[3]);
+    nextP[0][6] = P[0][6] + P[1][6]*SF[6] + P[2][6]*SPP[7] + P[3][6]*SPP[6] + P[10][6]*SPP[5] + P[11][6]*SPP[4] + P[12][6]*SPP[3] + SF[1]*(P[0][1] + P[1][1]*SF[6] + P[2][1]*SPP[7] + P[3][1]*SPP[6] + P[10][1]*SPP[5] + P[11][1]*SPP[4] + P[12][1]*SPP[3]) + SF[0]*(P[0][3] + P[1][3]*SF[6] + P[2][3]*SPP[7] + P[3][3]*SPP[6] + P[10][3]*SPP[5] + P[11][3]*SPP[4] + P[12][3]*SPP[3]) + SPP[0]*(P[0][0] + P[1][0]*SF[6] + P[2][0]*SPP[7] + P[3][0]*SPP[6] + P[10][0]*SPP[5] + P[11][0]*SPP[4] + P[12][0]*SPP[3]) - SPP[1]*(P[0][2] + P[1][2]*SF[6] + P[2][2]*SPP[7] + P[3][2]*SPP[6] + P[10][2]*SPP[5] + P[11][2]*SPP[4] + P[12][2]*SPP[3]);
+    nextP[0][7] = P[0][7] + P[1][7]*SF[6] + P[2][7]*SPP[7] + P[3][7]*SPP[6] + P[10][7]*SPP[5] + P[11][7]*SPP[4] + P[12][7]*SPP[3] + dt*(P[0][4] + P[1][4]*SF[6] + P[2][4]*SPP[7] + P[3][4]*SPP[6] + P[10][4]*SPP[5] + P[11][4]*SPP[4] + P[12][4]*SPP[3]);
+    nextP[0][8] = P[0][8] + P[1][8]*SF[6] + P[2][8]*SPP[7] + P[3][8]*SPP[6] + P[10][8]*SPP[5] + P[11][8]*SPP[4] + P[12][8]*SPP[3] + dt*(P[0][5] + P[1][5]*SF[6] + P[2][5]*SPP[7] + P[3][5]*SPP[6] + P[10][5]*SPP[5] + P[11][5]*SPP[4] + P[12][5]*SPP[3]);
+    nextP[0][9] = P[0][9] + P[1][9]*SF[6] + P[2][9]*SPP[7] + P[3][9]*SPP[6] + P[10][9]*SPP[5] + P[11][9]*SPP[4] + P[12][9]*SPP[3] + dt*(P[0][6] + P[1][6]*SF[6] + P[2][6]*SPP[7] + P[3][6]*SPP[6] + P[10][6]*SPP[5] + P[11][6]*SPP[4] + P[12][6]*SPP[3]);
+    nextP[0][10] = P[0][10] + P[1][10]*SF[6] + P[2][10]*SPP[7] + P[3][10]*SPP[6] + P[10][10]*SPP[5] + P[11][10]*SPP[4] + P[12][10]*SPP[3];
+    nextP[0][11] = P[0][11] + P[1][11]*SF[6] + P[2][11]*SPP[7] + P[3][11]*SPP[6] + P[10][11]*SPP[5] + P[11][11]*SPP[4] + P[12][11]*SPP[3];
+    nextP[0][12] = P[0][12] + P[1][12]*SF[6] + P[2][12]*SPP[7] + P[3][12]*SPP[6] + P[10][12]*SPP[5] + P[11][12]*SPP[4] + P[12][12]*SPP[3];
+    nextP[0][13] = P[0][13] + P[1][13]*SF[6] + P[2][13]*SPP[7] + P[3][13]*SPP[6] + P[10][13]*SPP[5] + P[11][13]*SPP[4] + P[12][13]*SPP[3];
+    nextP[0][14] = P[0][14] + P[1][14]*SF[6] + P[2][14]*SPP[7] + P[3][14]*SPP[6] + P[10][14]*SPP[5] + P[11][14]*SPP[4] + P[12][14]*SPP[3];
+    nextP[0][15] = P[0][15] + P[1][15]*SF[6] + P[2][15]*SPP[7] + P[3][15]*SPP[6] + P[10][15]*SPP[5] + P[11][15]*SPP[4] + P[12][15]*SPP[3];
+    nextP[0][16] = P[0][16] + P[1][16]*SF[6] + P[2][16]*SPP[7] + P[3][16]*SPP[6] + P[10][16]*SPP[5] + P[11][16]*SPP[4] + P[12][16]*SPP[3];
+    nextP[0][17] = P[0][17] + P[1][17]*SF[6] + P[2][17]*SPP[7] + P[3][17]*SPP[6] + P[10][17]*SPP[5] + P[11][17]*SPP[4] + P[12][17]*SPP[3];
+    nextP[0][18] = P[0][18] + P[1][18]*SF[6] + P[2][18]*SPP[7] + P[3][18]*SPP[6] + P[10][18]*SPP[5] + P[11][18]*SPP[4] + P[12][18]*SPP[3];
+    nextP[0][19] = P[0][19] + P[1][19]*SF[6] + P[2][19]*SPP[7] + P[3][19]*SPP[6] + P[10][19]*SPP[5] + P[11][19]*SPP[4] + P[12][19]*SPP[3];
+    nextP[0][20] = P[0][20] + P[1][20]*SF[6] + P[2][20]*SPP[7] + P[3][20]*SPP[6] + P[10][20]*SPP[5] + P[11][20]*SPP[4] + P[12][20]*SPP[3];
+    nextP[1][0] = P[1][0] + SQ[8] + P[0][0]*SF[5] + P[2][0]*SF[4] + P[3][0]*SPP[7] + P[11][0]*SPP[3] - P[12][0]*SPP[4] - (P[10][0]*q0)/2 + SF[6]*(P[1][1] + P[0][1]*SF[5] + P[2][1]*SF[4] + P[3][1]*SPP[7] + P[11][1]*SPP[3] - P[12][1]*SPP[4] - (P[10][1]*q0)/2) + SPP[7]*(P[1][2] + P[0][2]*SF[5] + P[2][2]*SF[4] + P[3][2]*SPP[7] + P[11][2]*SPP[3] - P[12][2]*SPP[4] - (P[10][2]*q0)/2) + SPP[6]*(P[1][3] + P[0][3]*SF[5] + P[2][3]*SF[4] + P[3][3]*SPP[7] + P[11][3]*SPP[3] - P[12][3]*SPP[4] - (P[10][3]*q0)/2) + SPP[5]*(P[1][10] + P[0][10]*SF[5] + P[2][10]*SF[4] + P[3][10]*SPP[7] + P[11][10]*SPP[3] - P[12][10]*SPP[4] - (P[10][10]*q0)/2) + SPP[4]*(P[1][11] + P[0][11]*SF[5] + P[2][11]*SF[4] + P[3][11]*SPP[7] + P[11][11]*SPP[3] - P[12][11]*SPP[4] - (P[10][11]*q0)/2) + SPP[3]*(P[1][12] + P[0][12]*SF[5] + P[2][12]*SF[4] + P[3][12]*SPP[7] + P[11][12]*SPP[3] - P[12][12]*SPP[4] - (P[10][12]*q0)/2);
+    nextP[1][1] = P[1][1] + P[0][1]*SF[5] + P[2][1]*SF[4] + P[3][1]*SPP[7] + P[11][1]*SPP[3] - P[12][1]*SPP[4] + daxCov*SQ[9] - (P[10][1]*q0)/2 + SF[5]*(P[1][0] + P[0][0]*SF[5] + P[2][0]*SF[4] + P[3][0]*SPP[7] + P[11][0]*SPP[3] - P[12][0]*SPP[4] - (P[10][0]*q0)/2) + SF[4]*(P[1][2] + P[0][2]*SF[5] + P[2][2]*SF[4] + P[3][2]*SPP[7] + P[11][2]*SPP[3] - P[12][2]*SPP[4] - (P[10][2]*q0)/2) + SPP[7]*(P[1][3] + P[0][3]*SF[5] + P[2][3]*SF[4] + P[3][3]*SPP[7] + P[11][3]*SPP[3] - P[12][3]*SPP[4] - (P[10][3]*q0)/2) + SPP[3]*(P[1][11] + P[0][11]*SF[5] + P[2][11]*SF[4] + P[3][11]*SPP[7] + P[11][11]*SPP[3] - P[12][11]*SPP[4] - (P[10][11]*q0)/2) - SPP[4]*(P[1][12] + P[0][12]*SF[5] + P[2][12]*SF[4] + P[3][12]*SPP[7] + P[11][12]*SPP[3] - P[12][12]*SPP[4] - (P[10][12]*q0)/2) + (dayCov*sq(q3))/4 + (dazCov*sq(q2))/4 - (q0*(P[1][10] + P[0][10]*SF[5] + P[2][10]*SF[4] + P[3][10]*SPP[7] + P[11][10]*SPP[3] - P[12][10]*SPP[4] - (P[10][10]*q0)/2))/2;
+    nextP[1][2] = P[1][2] + SQ[5] + P[0][2]*SF[5] + P[2][2]*SF[4] + P[3][2]*SPP[7] + P[11][2]*SPP[3] - P[12][2]*SPP[4] - (P[10][2]*q0)/2 + SF[3]*(P[1][0] + P[0][0]*SF[5] + P[2][0]*SF[4] + P[3][0]*SPP[7] + P[11][0]*SPP[3] - P[12][0]*SPP[4] - (P[10][0]*q0)/2) + SF[5]*(P[1][3] + P[0][3]*SF[5] + P[2][3]*SF[4] + P[3][3]*SPP[7] + P[11][3]*SPP[3] - P[12][3]*SPP[4] - (P[10][3]*q0)/2) + SPP[6]*(P[1][1] + P[0][1]*SF[5] + P[2][1]*SF[4] + P[3][1]*SPP[7] + P[11][1]*SPP[3] - P[12][1]*SPP[4] - (P[10][1]*q0)/2) - SPP[3]*(P[1][10] + P[0][10]*SF[5] + P[2][10]*SF[4] + P[3][10]*SPP[7] + P[11][10]*SPP[3] - P[12][10]*SPP[4] - (P[10][10]*q0)/2) + SPP[5]*(P[1][12] + P[0][12]*SF[5] + P[2][12]*SF[4] + P[3][12]*SPP[7] + P[11][12]*SPP[3] - P[12][12]*SPP[4] - (P[10][12]*q0)/2) - (q0*(P[1][11] + P[0][11]*SF[5] + P[2][11]*SF[4] + P[3][11]*SPP[7] + P[11][11]*SPP[3] - P[12][11]*SPP[4] - (P[10][11]*q0)/2))/2;
+    nextP[1][3] = P[1][3] + SQ[4] + P[0][3]*SF[5] + P[2][3]*SF[4] + P[3][3]*SPP[7] + P[11][3]*SPP[3] - P[12][3]*SPP[4] - (P[10][3]*q0)/2 + SF[4]*(P[1][0] + P[0][0]*SF[5] + P[2][0]*SF[4] + P[3][0]*SPP[7] + P[11][0]*SPP[3] - P[12][0]*SPP[4] - (P[10][0]*q0)/2) + SF[3]*(P[1][1] + P[0][1]*SF[5] + P[2][1]*SF[4] + P[3][1]*SPP[7] + P[11][1]*SPP[3] - P[12][1]*SPP[4] - (P[10][1]*q0)/2) + SF[6]*(P[1][2] + P[0][2]*SF[5] + P[2][2]*SF[4] + P[3][2]*SPP[7] + P[11][2]*SPP[3] - P[12][2]*SPP[4] - (P[10][2]*q0)/2) + SPP[4]*(P[1][10] + P[0][10]*SF[5] + P[2][10]*SF[4] + P[3][10]*SPP[7] + P[11][10]*SPP[3] - P[12][10]*SPP[4] - (P[10][10]*q0)/2) - SPP[5]*(P[1][11] + P[0][11]*SF[5] + P[2][11]*SF[4] + P[3][11]*SPP[7] + P[11][11]*SPP[3] - P[12][11]*SPP[4] - (P[10][11]*q0)/2) - (q0*(P[1][12] + P[0][12]*SF[5] + P[2][12]*SF[4] + P[3][12]*SPP[7] + P[11][12]*SPP[3] - P[12][12]*SPP[4] - (P[10][12]*q0)/2))/2;
+    nextP[1][4] = P[1][4] + P[0][4]*SF[5] + P[2][4]*SF[4] + P[3][4]*SPP[7] + P[11][4]*SPP[3] - P[12][4]*SPP[4] - (P[10][4]*q0)/2 + SF[2]*(P[1][0] + P[0][0]*SF[5] + P[2][0]*SF[4] + P[3][0]*SPP[7] + P[11][0]*SPP[3] - P[12][0]*SPP[4] - (P[10][0]*q0)/2) + SF[0]*(P[1][1] + P[0][1]*SF[5] + P[2][1]*SF[4] + P[3][1]*SPP[7] + P[11][1]*SPP[3] - P[12][1]*SPP[4] - (P[10][1]*q0)/2) + SPP[0]*(P[1][2] + P[0][2]*SF[5] + P[2][2]*SF[4] + P[3][2]*SPP[7] + P[11][2]*SPP[3] - P[12][2]*SPP[4] - (P[10][2]*q0)/2) - SPP[2]*(P[1][3] + P[0][3]*SF[5] + P[2][3]*SF[4] + P[3][3]*SPP[7] + P[11][3]*SPP[3] - P[12][3]*SPP[4] - (P[10][3]*q0)/2);
+    nextP[1][5] = P[1][5] + P[0][5]*SF[5] + P[2][5]*SF[4] + P[3][5]*SPP[7] + P[11][5]*SPP[3] - P[12][5]*SPP[4] - (P[10][5]*q0)/2 + SF[1]*(P[1][0] + P[0][0]*SF[5] + P[2][0]*SF[4] + P[3][0]*SPP[7] + P[11][0]*SPP[3] - P[12][0]*SPP[4] - (P[10][0]*q0)/2) + SF[0]*(P[1][2] + P[0][2]*SF[5] + P[2][2]*SF[4] + P[3][2]*SPP[7] + P[11][2]*SPP[3] - P[12][2]*SPP[4] - (P[10][2]*q0)/2) + SF[2]*(P[1][3] + P[0][3]*SF[5] + P[2][3]*SF[4] + P[3][3]*SPP[7] + P[11][3]*SPP[3] - P[12][3]*SPP[4] - (P[10][3]*q0)/2) - SPP[0]*(P[1][1] + P[0][1]*SF[5] + P[2][1]*SF[4] + P[3][1]*SPP[7] + P[11][1]*SPP[3] - P[12][1]*SPP[4] - (P[10][1]*q0)/2);
+    nextP[1][6] = P[1][6] + P[0][6]*SF[5] + P[2][6]*SF[4] + P[3][6]*SPP[7] + P[11][6]*SPP[3] - P[12][6]*SPP[4] - (P[10][6]*q0)/2 + SF[1]*(P[1][1] + P[0][1]*SF[5] + P[2][1]*SF[4] + P[3][1]*SPP[7] + P[11][1]*SPP[3] - P[12][1]*SPP[4] - (P[10][1]*q0)/2) + SF[0]*(P[1][3] + P[0][3]*SF[5] + P[2][3]*SF[4] + P[3][3]*SPP[7] + P[11][3]*SPP[3] - P[12][3]*SPP[4] - (P[10][3]*q0)/2) + SPP[0]*(P[1][0] + P[0][0]*SF[5] + P[2][0]*SF[4] + P[3][0]*SPP[7] + P[11][0]*SPP[3] - P[12][0]*SPP[4] - (P[10][0]*q0)/2) - SPP[1]*(P[1][2] + P[0][2]*SF[5] + P[2][2]*SF[4] + P[3][2]*SPP[7] + P[11][2]*SPP[3] - P[12][2]*SPP[4] - (P[10][2]*q0)/2);
+    nextP[1][7] = P[1][7] + P[0][7]*SF[5] + P[2][7]*SF[4] + P[3][7]*SPP[7] + P[11][7]*SPP[3] - P[12][7]*SPP[4] - (P[10][7]*q0)/2 + dt*(P[1][4] + P[0][4]*SF[5] + P[2][4]*SF[4] + P[3][4]*SPP[7] + P[11][4]*SPP[3] - P[12][4]*SPP[4] - (P[10][4]*q0)/2);
+    nextP[1][8] = P[1][8] + P[0][8]*SF[5] + P[2][8]*SF[4] + P[3][8]*SPP[7] + P[11][8]*SPP[3] - P[12][8]*SPP[4] - (P[10][8]*q0)/2 + dt*(P[1][5] + P[0][5]*SF[5] + P[2][5]*SF[4] + P[3][5]*SPP[7] + P[11][5]*SPP[3] - P[12][5]*SPP[4] - (P[10][5]*q0)/2);
+    nextP[1][9] = P[1][9] + P[0][9]*SF[5] + P[2][9]*SF[4] + P[3][9]*SPP[7] + P[11][9]*SPP[3] - P[12][9]*SPP[4] - (P[10][9]*q0)/2 + dt*(P[1][6] + P[0][6]*SF[5] + P[2][6]*SF[4] + P[3][6]*SPP[7] + P[11][6]*SPP[3] - P[12][6]*SPP[4] - (P[10][6]*q0)/2);
+    nextP[1][10] = P[1][10] + P[0][10]*SF[5] + P[2][10]*SF[4] + P[3][10]*SPP[7] + P[11][10]*SPP[3] - P[12][10]*SPP[4] - (P[10][10]*q0)/2;
+    nextP[1][11] = P[1][11] + P[0][11]*SF[5] + P[2][11]*SF[4] + P[3][11]*SPP[7] + P[11][11]*SPP[3] - P[12][11]*SPP[4] - (P[10][11]*q0)/2;
+    nextP[1][12] = P[1][12] + P[0][12]*SF[5] + P[2][12]*SF[4] + P[3][12]*SPP[7] + P[11][12]*SPP[3] - P[12][12]*SPP[4] - (P[10][12]*q0)/2;
+    nextP[1][13] = P[1][13] + P[0][13]*SF[5] + P[2][13]*SF[4] + P[3][13]*SPP[7] + P[11][13]*SPP[3] - P[12][13]*SPP[4] - (P[10][13]*q0)/2;
+    nextP[1][14] = P[1][14] + P[0][14]*SF[5] + P[2][14]*SF[4] + P[3][14]*SPP[7] + P[11][14]*SPP[3] - P[12][14]*SPP[4] - (P[10][14]*q0)/2;
+    nextP[1][15] = P[1][15] + P[0][15]*SF[5] + P[2][15]*SF[4] + P[3][15]*SPP[7] + P[11][15]*SPP[3] - P[12][15]*SPP[4] - (P[10][15]*q0)/2;
+    nextP[1][16] = P[1][16] + P[0][16]*SF[5] + P[2][16]*SF[4] + P[3][16]*SPP[7] + P[11][16]*SPP[3] - P[12][16]*SPP[4] - (P[10][16]*q0)/2;
+    nextP[1][17] = P[1][17] + P[0][17]*SF[5] + P[2][17]*SF[4] + P[3][17]*SPP[7] + P[11][17]*SPP[3] - P[12][17]*SPP[4] - (P[10][17]*q0)/2;
+    nextP[1][18] = P[1][18] + P[0][18]*SF[5] + P[2][18]*SF[4] + P[3][18]*SPP[7] + P[11][18]*SPP[3] - P[12][18]*SPP[4] - (P[10][18]*q0)/2;
+    nextP[1][19] = P[1][19] + P[0][19]*SF[5] + P[2][19]*SF[4] + P[3][19]*SPP[7] + P[11][19]*SPP[3] - P[12][19]*SPP[4] - (P[10][19]*q0)/2;
+    nextP[1][20] = P[1][20] + P[0][20]*SF[5] + P[2][20]*SF[4] + P[3][20]*SPP[7] + P[11][20]*SPP[3] - P[12][20]*SPP[4] - (P[10][20]*q0)/2;
+    nextP[2][0] = P[2][0] + SQ[7] + P[0][0]*SF[3] + P[3][0]*SF[5] + P[1][0]*SPP[6] - P[10][0]*SPP[3] + P[12][0]*SPP[5] - (P[11][0]*q0)/2 + SF[6]*(P[2][1] + P[0][1]*SF[3] + P[3][1]*SF[5] + P[1][1]*SPP[6] - P[10][1]*SPP[3] + P[12][1]*SPP[5] - (P[11][1]*q0)/2) + SPP[7]*(P[2][2] + P[0][2]*SF[3] + P[3][2]*SF[5] + P[1][2]*SPP[6] - P[10][2]*SPP[3] + P[12][2]*SPP[5] - (P[11][2]*q0)/2) + SPP[6]*(P[2][3] + P[0][3]*SF[3] + P[3][3]*SF[5] + P[1][3]*SPP[6] - P[10][3]*SPP[3] + P[12][3]*SPP[5] - (P[11][3]*q0)/2) + SPP[5]*(P[2][10] + P[0][10]*SF[3] + P[3][10]*SF[5] + P[1][10]*SPP[6] - P[10][10]*SPP[3] + P[12][10]*SPP[5] - (P[11][10]*q0)/2) + SPP[4]*(P[2][11] + P[0][11]*SF[3] + P[3][11]*SF[5] + P[1][11]*SPP[6] - P[10][11]*SPP[3] + P[12][11]*SPP[5] - (P[11][11]*q0)/2) + SPP[3]*(P[2][12] + P[0][12]*SF[3] + P[3][12]*SF[5] + P[1][12]*SPP[6] - P[10][12]*SPP[3] + P[12][12]*SPP[5] - (P[11][12]*q0)/2);
+    nextP[2][1] = P[2][1] + SQ[5] + P[0][1]*SF[3] + P[3][1]*SF[5] + P[1][1]*SPP[6] - P[10][1]*SPP[3] + P[12][1]*SPP[5] - (P[11][1]*q0)/2 + SF[5]*(P[2][0] + P[0][0]*SF[3] + P[3][0]*SF[5] + P[1][0]*SPP[6] - P[10][0]*SPP[3] + P[12][0]*SPP[5] - (P[11][0]*q0)/2) + SF[4]*(P[2][2] + P[0][2]*SF[3] + P[3][2]*SF[5] + P[1][2]*SPP[6] - P[10][2]*SPP[3] + P[12][2]*SPP[5] - (P[11][2]*q0)/2) + SPP[7]*(P[2][3] + P[0][3]*SF[3] + P[3][3]*SF[5] + P[1][3]*SPP[6] - P[10][3]*SPP[3] + P[12][3]*SPP[5] - (P[11][3]*q0)/2) + SPP[3]*(P[2][11] + P[0][11]*SF[3] + P[3][11]*SF[5] + P[1][11]*SPP[6] - P[10][11]*SPP[3] + P[12][11]*SPP[5] - (P[11][11]*q0)/2) - SPP[4]*(P[2][12] + P[0][12]*SF[3] + P[3][12]*SF[5] + P[1][12]*SPP[6] - P[10][12]*SPP[3] + P[12][12]*SPP[5] - (P[11][12]*q0)/2) - (q0*(P[2][10] + P[0][10]*SF[3] + P[3][10]*SF[5] + P[1][10]*SPP[6] - P[10][10]*SPP[3] + P[12][10]*SPP[5] - (P[11][10]*q0)/2))/2;
+    nextP[2][2] = P[2][2] + P[0][2]*SF[3] + P[3][2]*SF[5] + P[1][2]*SPP[6] - P[10][2]*SPP[3] + P[12][2]*SPP[5] + dayCov*SQ[9] + (dazCov*SQ[10])/4 - (P[11][2]*q0)/2 + SF[3]*(P[2][0] + P[0][0]*SF[3] + P[3][0]*SF[5] + P[1][0]*SPP[6] - P[10][0]*SPP[3] + P[12][0]*SPP[5] - (P[11][0]*q0)/2) + SF[5]*(P[2][3] + P[0][3]*SF[3] + P[3][3]*SF[5] + P[1][3]*SPP[6] - P[10][3]*SPP[3] + P[12][3]*SPP[5] - (P[11][3]*q0)/2) + SPP[6]*(P[2][1] + P[0][1]*SF[3] + P[3][1]*SF[5] + P[1][1]*SPP[6] - P[10][1]*SPP[3] + P[12][1]*SPP[5] - (P[11][1]*q0)/2) - SPP[3]*(P[2][10] + P[0][10]*SF[3] + P[3][10]*SF[5] + P[1][10]*SPP[6] - P[10][10]*SPP[3] + P[12][10]*SPP[5] - (P[11][10]*q0)/2) + SPP[5]*(P[2][12] + P[0][12]*SF[3] + P[3][12]*SF[5] + P[1][12]*SPP[6] - P[10][12]*SPP[3] + P[12][12]*SPP[5] - (P[11][12]*q0)/2) + (daxCov*sq(q3))/4 - (q0*(P[2][11] + P[0][11]*SF[3] + P[3][11]*SF[5] + P[1][11]*SPP[6] - P[10][11]*SPP[3] + P[12][11]*SPP[5] - (P[11][11]*q0)/2))/2;
+    nextP[2][3] = P[2][3] + SQ[3] + P[0][3]*SF[3] + P[3][3]*SF[5] + P[1][3]*SPP[6] - P[10][3]*SPP[3] + P[12][3]*SPP[5] - (P[11][3]*q0)/2 + SF[4]*(P[2][0] + P[0][0]*SF[3] + P[3][0]*SF[5] + P[1][0]*SPP[6] - P[10][0]*SPP[3] + P[12][0]*SPP[5] - (P[11][0]*q0)/2) + SF[3]*(P[2][1] + P[0][1]*SF[3] + P[3][1]*SF[5] + P[1][1]*SPP[6] - P[10][1]*SPP[3] + P[12][1]*SPP[5] - (P[11][1]*q0)/2) + SF[6]*(P[2][2] + P[0][2]*SF[3] + P[3][2]*SF[5] + P[1][2]*SPP[6] - P[10][2]*SPP[3] + P[12][2]*SPP[5] - (P[11][2]*q0)/2) + SPP[4]*(P[2][10] + P[0][10]*SF[3] + P[3][10]*SF[5] + P[1][10]*SPP[6] - P[10][10]*SPP[3] + P[12][10]*SPP[5] - (P[11][10]*q0)/2) - SPP[5]*(P[2][11] + P[0][11]*SF[3] + P[3][11]*SF[5] + P[1][11]*SPP[6] - P[10][11]*SPP[3] + P[12][11]*SPP[5] - (P[11][11]*q0)/2) - (q0*(P[2][12] + P[0][12]*SF[3] + P[3][12]*SF[5] + P[1][12]*SPP[6] - P[10][12]*SPP[3] + P[12][12]*SPP[5] - (P[11][12]*q0)/2))/2;
+    nextP[2][4] = P[2][4] + P[0][4]*SF[3] + P[3][4]*SF[5] + P[1][4]*SPP[6] - P[10][4]*SPP[3] + P[12][4]*SPP[5] - (P[11][4]*q0)/2 + SF[2]*(P[2][0] + P[0][0]*SF[3] + P[3][0]*SF[5] + P[1][0]*SPP[6] - P[10][0]*SPP[3] + P[12][0]*SPP[5] - (P[11][0]*q0)/2) + SF[0]*(P[2][1] + P[0][1]*SF[3] + P[3][1]*SF[5] + P[1][1]*SPP[6] - P[10][1]*SPP[3] + P[12][1]*SPP[5] - (P[11][1]*q0)/2) + SPP[0]*(P[2][2] + P[0][2]*SF[3] + P[3][2]*SF[5] + P[1][2]*SPP[6] - P[10][2]*SPP[3] + P[12][2]*SPP[5] - (P[11][2]*q0)/2) - SPP[2]*(P[2][3] + P[0][3]*SF[3] + P[3][3]*SF[5] + P[1][3]*SPP[6] - P[10][3]*SPP[3] + P[12][3]*SPP[5] - (P[11][3]*q0)/2);
+    nextP[2][5] = P[2][5] + P[0][5]*SF[3] + P[3][5]*SF[5] + P[1][5]*SPP[6] - P[10][5]*SPP[3] + P[12][5]*SPP[5] - (P[11][5]*q0)/2 + SF[1]*(P[2][0] + P[0][0]*SF[3] + P[3][0]*SF[5] + P[1][0]*SPP[6] - P[10][0]*SPP[3] + P[12][0]*SPP[5] - (P[11][0]*q0)/2) + SF[0]*(P[2][2] + P[0][2]*SF[3] + P[3][2]*SF[5] + P[1][2]*SPP[6] - P[10][2]*SPP[3] + P[12][2]*SPP[5] - (P[11][2]*q0)/2) + SF[2]*(P[2][3] + P[0][3]*SF[3] + P[3][3]*SF[5] + P[1][3]*SPP[6] - P[10][3]*SPP[3] + P[12][3]*SPP[5] - (P[11][3]*q0)/2) - SPP[0]*(P[2][1] + P[0][1]*SF[3] + P[3][1]*SF[5] + P[1][1]*SPP[6] - P[10][1]*SPP[3] + P[12][1]*SPP[5] - (P[11][1]*q0)/2);
+    nextP[2][6] = P[2][6] + P[0][6]*SF[3] + P[3][6]*SF[5] + P[1][6]*SPP[6] - P[10][6]*SPP[3] + P[12][6]*SPP[5] - (P[11][6]*q0)/2 + SF[1]*(P[2][1] + P[0][1]*SF[3] + P[3][1]*SF[5] + P[1][1]*SPP[6] - P[10][1]*SPP[3] + P[12][1]*SPP[5] - (P[11][1]*q0)/2) + SF[0]*(P[2][3] + P[0][3]*SF[3] + P[3][3]*SF[5] + P[1][3]*SPP[6] - P[10][3]*SPP[3] + P[12][3]*SPP[5] - (P[11][3]*q0)/2) + SPP[0]*(P[2][0] + P[0][0]*SF[3] + P[3][0]*SF[5] + P[1][0]*SPP[6] - P[10][0]*SPP[3] + P[12][0]*SPP[5] - (P[11][0]*q0)/2) - SPP[1]*(P[2][2] + P[0][2]*SF[3] + P[3][2]*SF[5] + P[1][2]*SPP[6] - P[10][2]*SPP[3] + P[12][2]*SPP[5] - (P[11][2]*q0)/2);
+    nextP[2][7] = P[2][7] + P[0][7]*SF[3] + P[3][7]*SF[5] + P[1][7]*SPP[6] - P[10][7]*SPP[3] + P[12][7]*SPP[5] - (P[11][7]*q0)/2 + dt*(P[2][4] + P[0][4]*SF[3] + P[3][4]*SF[5] + P[1][4]*SPP[6] - P[10][4]*SPP[3] + P[12][4]*SPP[5] - (P[11][4]*q0)/2);
+    nextP[2][8] = P[2][8] + P[0][8]*SF[3] + P[3][8]*SF[5] + P[1][8]*SPP[6] - P[10][8]*SPP[3] + P[12][8]*SPP[5] - (P[11][8]*q0)/2 + dt*(P[2][5] + P[0][5]*SF[3] + P[3][5]*SF[5] + P[1][5]*SPP[6] - P[10][5]*SPP[3] + P[12][5]*SPP[5] - (P[11][5]*q0)/2);
+    nextP[2][9] = P[2][9] + P[0][9]*SF[3] + P[3][9]*SF[5] + P[1][9]*SPP[6] - P[10][9]*SPP[3] + P[12][9]*SPP[5] - (P[11][9]*q0)/2 + dt*(P[2][6] + P[0][6]*SF[3] + P[3][6]*SF[5] + P[1][6]*SPP[6] - P[10][6]*SPP[3] + P[12][6]*SPP[5] - (P[11][6]*q0)/2);
+    nextP[2][10] = P[2][10] + P[0][10]*SF[3] + P[3][10]*SF[5] + P[1][10]*SPP[6] - P[10][10]*SPP[3] + P[12][10]*SPP[5] - (P[11][10]*q0)/2;
+    nextP[2][11] = P[2][11] + P[0][11]*SF[3] + P[3][11]*SF[5] + P[1][11]*SPP[6] - P[10][11]*SPP[3] + P[12][11]*SPP[5] - (P[11][11]*q0)/2;
+    nextP[2][12] = P[2][12] + P[0][12]*SF[3] + P[3][12]*SF[5] + P[1][12]*SPP[6] - P[10][12]*SPP[3] + P[12][12]*SPP[5] - (P[11][12]*q0)/2;
+    nextP[2][13] = P[2][13] + P[0][13]*SF[3] + P[3][13]*SF[5] + P[1][13]*SPP[6] - P[10][13]*SPP[3] + P[12][13]*SPP[5] - (P[11][13]*q0)/2;
+    nextP[2][14] = P[2][14] + P[0][14]*SF[3] + P[3][14]*SF[5] + P[1][14]*SPP[6] - P[10][14]*SPP[3] + P[12][14]*SPP[5] - (P[11][14]*q0)/2;
+    nextP[2][15] = P[2][15] + P[0][15]*SF[3] + P[3][15]*SF[5] + P[1][15]*SPP[6] - P[10][15]*SPP[3] + P[12][15]*SPP[5] - (P[11][15]*q0)/2;
+    nextP[2][16] = P[2][16] + P[0][16]*SF[3] + P[3][16]*SF[5] + P[1][16]*SPP[6] - P[10][16]*SPP[3] + P[12][16]*SPP[5] - (P[11][16]*q0)/2;
+    nextP[2][17] = P[2][17] + P[0][17]*SF[3] + P[3][17]*SF[5] + P[1][17]*SPP[6] - P[10][17]*SPP[3] + P[12][17]*SPP[5] - (P[11][17]*q0)/2;
+    nextP[2][18] = P[2][18] + P[0][18]*SF[3] + P[3][18]*SF[5] + P[1][18]*SPP[6] - P[10][18]*SPP[3] + P[12][18]*SPP[5] - (P[11][18]*q0)/2;
+    nextP[2][19] = P[2][19] + P[0][19]*SF[3] + P[3][19]*SF[5] + P[1][19]*SPP[6] - P[10][19]*SPP[3] + P[12][19]*SPP[5] - (P[11][19]*q0)/2;
+    nextP[2][20] = P[2][20] + P[0][20]*SF[3] + P[3][20]*SF[5] + P[1][20]*SPP[6] - P[10][20]*SPP[3] + P[12][20]*SPP[5] - (P[11][20]*q0)/2;
+    nextP[3][0] = P[3][0] + SQ[6] + P[0][0]*SF[4] + P[1][0]*SF[3] + P[2][0]*SF[6] + P[10][0]*SPP[4] - P[11][0]*SPP[5] - (P[12][0]*q0)/2 + SF[6]*(P[3][1] + P[0][1]*SF[4] + P[1][1]*SF[3] + P[2][1]*SF[6] + P[10][1]*SPP[4] - P[11][1]*SPP[5] - (P[12][1]*q0)/2) + SPP[7]*(P[3][2] + P[0][2]*SF[4] + P[1][2]*SF[3] + P[2][2]*SF[6] + P[10][2]*SPP[4] - P[11][2]*SPP[5] - (P[12][2]*q0)/2) + SPP[6]*(P[3][3] + P[0][3]*SF[4] + P[1][3]*SF[3] + P[2][3]*SF[6] + P[10][3]*SPP[4] - P[11][3]*SPP[5] - (P[12][3]*q0)/2) + SPP[5]*(P[3][10] + P[0][10]*SF[4] + P[1][10]*SF[3] + P[2][10]*SF[6] + P[10][10]*SPP[4] - P[11][10]*SPP[5] - (P[12][10]*q0)/2) + SPP[4]*(P[3][11] + P[0][11]*SF[4] + P[1][11]*SF[3] + P[2][11]*SF[6] + P[10][11]*SPP[4] - P[11][11]*SPP[5] - (P[12][11]*q0)/2) + SPP[3]*(P[3][12] + P[0][12]*SF[4] + P[1][12]*SF[3] + P[2][12]*SF[6] + P[10][12]*SPP[4] - P[11][12]*SPP[5] - (P[12][12]*q0)/2);
+    nextP[3][1] = P[3][1] + SQ[4] + P[0][1]*SF[4] + P[1][1]*SF[3] + P[2][1]*SF[6] + P[10][1]*SPP[4] - P[11][1]*SPP[5] - (P[12][1]*q0)/2 + SF[5]*(P[3][0] + P[0][0]*SF[4] + P[1][0]*SF[3] + P[2][0]*SF[6] + P[10][0]*SPP[4] - P[11][0]*SPP[5] - (P[12][0]*q0)/2) + SF[4]*(P[3][2] + P[0][2]*SF[4] + P[1][2]*SF[3] + P[2][2]*SF[6] + P[10][2]*SPP[4] - P[11][2]*SPP[5] - (P[12][2]*q0)/2) + SPP[7]*(P[3][3] + P[0][3]*SF[4] + P[1][3]*SF[3] + P[2][3]*SF[6] + P[10][3]*SPP[4] - P[11][3]*SPP[5] - (P[12][3]*q0)/2) + SPP[3]*(P[3][11] + P[0][11]*SF[4] + P[1][11]*SF[3] + P[2][11]*SF[6] + P[10][11]*SPP[4] - P[11][11]*SPP[5] - (P[12][11]*q0)/2) - SPP[4]*(P[3][12] + P[0][12]*SF[4] + P[1][12]*SF[3] + P[2][12]*SF[6] + P[10][12]*SPP[4] - P[11][12]*SPP[5] - (P[12][12]*q0)/2) - (q0*(P[3][10] + P[0][10]*SF[4] + P[1][10]*SF[3] + P[2][10]*SF[6] + P[10][10]*SPP[4] - P[11][10]*SPP[5] - (P[12][10]*q0)/2))/2;
+    nextP[3][2] = P[3][2] + SQ[3] + P[0][2]*SF[4] + P[1][2]*SF[3] + P[2][2]*SF[6] + P[10][2]*SPP[4] - P[11][2]*SPP[5] - (P[12][2]*q0)/2 + SF[3]*(P[3][0] + P[0][0]*SF[4] + P[1][0]*SF[3] + P[2][0]*SF[6] + P[10][0]*SPP[4] - P[11][0]*SPP[5] - (P[12][0]*q0)/2) + SF[5]*(P[3][3] + P[0][3]*SF[4] + P[1][3]*SF[3] + P[2][3]*SF[6] + P[10][3]*SPP[4] - P[11][3]*SPP[5] - (P[12][3]*q0)/2) + SPP[6]*(P[3][1] + P[0][1]*SF[4] + P[1][1]*SF[3] + P[2][1]*SF[6] + P[10][1]*SPP[4] - P[11][1]*SPP[5] - (P[12][1]*q0)/2) - SPP[3]*(P[3][10] + P[0][10]*SF[4] + P[1][10]*SF[3] + P[2][10]*SF[6] + P[10][10]*SPP[4] - P[11][10]*SPP[5] - (P[12][10]*q0)/2) + SPP[5]*(P[3][12] + P[0][12]*SF[4] + P[1][12]*SF[3] + P[2][12]*SF[6] + P[10][12]*SPP[4] - P[11][12]*SPP[5] - (P[12][12]*q0)/2) - (q0*(P[3][11] + P[0][11]*SF[4] + P[1][11]*SF[3] + P[2][11]*SF[6] + P[10][11]*SPP[4] - P[11][11]*SPP[5] - (P[12][11]*q0)/2))/2;
+    nextP[3][3] = P[3][3] + P[0][3]*SF[4] + P[1][3]*SF[3] + P[2][3]*SF[6] + P[10][3]*SPP[4] - P[11][3]*SPP[5] + (dayCov*SQ[10])/4 + dazCov*SQ[9] - (P[12][3]*q0)/2 + SF[4]*(P[3][0] + P[0][0]*SF[4] + P[1][0]*SF[3] + P[2][0]*SF[6] + P[10][0]*SPP[4] - P[11][0]*SPP[5] - (P[12][0]*q0)/2) + SF[3]*(P[3][1] + P[0][1]*SF[4] + P[1][1]*SF[3] + P[2][1]*SF[6] + P[10][1]*SPP[4] - P[11][1]*SPP[5] - (P[12][1]*q0)/2) + SF[6]*(P[3][2] + P[0][2]*SF[4] + P[1][2]*SF[3] + P[2][2]*SF[6] + P[10][2]*SPP[4] - P[11][2]*SPP[5] - (P[12][2]*q0)/2) + SPP[4]*(P[3][10] + P[0][10]*SF[4] + P[1][10]*SF[3] + P[2][10]*SF[6] + P[10][10]*SPP[4] - P[11][10]*SPP[5] - (P[12][10]*q0)/2) - SPP[5]*(P[3][11] + P[0][11]*SF[4] + P[1][11]*SF[3] + P[2][11]*SF[6] + P[10][11]*SPP[4] - P[11][11]*SPP[5] - (P[12][11]*q0)/2) + (daxCov*sq(q2))/4 - (q0*(P[3][12] + P[0][12]*SF[4] + P[1][12]*SF[3] + P[2][12]*SF[6] + P[10][12]*SPP[4] - P[11][12]*SPP[5] - (P[12][12]*q0)/2))/2;
+    nextP[3][4] = P[3][4] + P[0][4]*SF[4] + P[1][4]*SF[3] + P[2][4]*SF[6] + P[10][4]*SPP[4] - P[11][4]*SPP[5] - (P[12][4]*q0)/2 + SF[2]*(P[3][0] + P[0][0]*SF[4] + P[1][0]*SF[3] + P[2][0]*SF[6] + P[10][0]*SPP[4] - P[11][0]*SPP[5] - (P[12][0]*q0)/2) + SF[0]*(P[3][1] + P[0][1]*SF[4] + P[1][1]*SF[3] + P[2][1]*SF[6] + P[10][1]*SPP[4] - P[11][1]*SPP[5] - (P[12][1]*q0)/2) + SPP[0]*(P[3][2] + P[0][2]*SF[4] + P[1][2]*SF[3] + P[2][2]*SF[6] + P[10][2]*SPP[4] - P[11][2]*SPP[5] - (P[12][2]*q0)/2) - SPP[2]*(P[3][3] + P[0][3]*SF[4] + P[1][3]*SF[3] + P[2][3]*SF[6] + P[10][3]*SPP[4] - P[11][3]*SPP[5] - (P[12][3]*q0)/2);
+    nextP[3][5] = P[3][5] + P[0][5]*SF[4] + P[1][5]*SF[3] + P[2][5]*SF[6] + P[10][5]*SPP[4] - P[11][5]*SPP[5] - (P[12][5]*q0)/2 + SF[1]*(P[3][0] + P[0][0]*SF[4] + P[1][0]*SF[3] + P[2][0]*SF[6] + P[10][0]*SPP[4] - P[11][0]*SPP[5] - (P[12][0]*q0)/2) + SF[0]*(P[3][2] + P[0][2]*SF[4] + P[1][2]*SF[3] + P[2][2]*SF[6] + P[10][2]*SPP[4] - P[11][2]*SPP[5] - (P[12][2]*q0)/2) + SF[2]*(P[3][3] + P[0][3]*SF[4] + P[1][3]*SF[3] + P[2][3]*SF[6] + P[10][3]*SPP[4] - P[11][3]*SPP[5] - (P[12][3]*q0)/2) - SPP[0]*(P[3][1] + P[0][1]*SF[4] + P[1][1]*SF[3] + P[2][1]*SF[6] + P[10][1]*SPP[4] - P[11][1]*SPP[5] - (P[12][1]*q0)/2);
+    nextP[3][6] = P[3][6] + P[0][6]*SF[4] + P[1][6]*SF[3] + P[2][6]*SF[6] + P[10][6]*SPP[4] - P[11][6]*SPP[5] - (P[12][6]*q0)/2 + SF[1]*(P[3][1] + P[0][1]*SF[4] + P[1][1]*SF[3] + P[2][1]*SF[6] + P[10][1]*SPP[4] - P[11][1]*SPP[5] - (P[12][1]*q0)/2) + SF[0]*(P[3][3] + P[0][3]*SF[4] + P[1][3]*SF[3] + P[2][3]*SF[6] + P[10][3]*SPP[4] - P[11][3]*SPP[5] - (P[12][3]*q0)/2) + SPP[0]*(P[3][0] + P[0][0]*SF[4] + P[1][0]*SF[3] + P[2][0]*SF[6] + P[10][0]*SPP[4] - P[11][0]*SPP[5] - (P[12][0]*q0)/2) - SPP[1]*(P[3][2] + P[0][2]*SF[4] + P[1][2]*SF[3] + P[2][2]*SF[6] + P[10][2]*SPP[4] - P[11][2]*SPP[5] - (P[12][2]*q0)/2);
+    nextP[3][7] = P[3][7] + P[0][7]*SF[4] + P[1][7]*SF[3] + P[2][7]*SF[6] + P[10][7]*SPP[4] - P[11][7]*SPP[5] - (P[12][7]*q0)/2 + dt*(P[3][4] + P[0][4]*SF[4] + P[1][4]*SF[3] + P[2][4]*SF[6] + P[10][4]*SPP[4] - P[11][4]*SPP[5] - (P[12][4]*q0)/2);
+    nextP[3][8] = P[3][8] + P[0][8]*SF[4] + P[1][8]*SF[3] + P[2][8]*SF[6] + P[10][8]*SPP[4] - P[11][8]*SPP[5] - (P[12][8]*q0)/2 + dt*(P[3][5] + P[0][5]*SF[4] + P[1][5]*SF[3] + P[2][5]*SF[6] + P[10][5]*SPP[4] - P[11][5]*SPP[5] - (P[12][5]*q0)/2);
+    nextP[3][9] = P[3][9] + P[0][9]*SF[4] + P[1][9]*SF[3] + P[2][9]*SF[6] + P[10][9]*SPP[4] - P[11][9]*SPP[5] - (P[12][9]*q0)/2 + dt*(P[3][6] + P[0][6]*SF[4] + P[1][6]*SF[3] + P[2][6]*SF[6] + P[10][6]*SPP[4] - P[11][6]*SPP[5] - (P[12][6]*q0)/2);
+    nextP[3][10] = P[3][10] + P[0][10]*SF[4] + P[1][10]*SF[3] + P[2][10]*SF[6] + P[10][10]*SPP[4] - P[11][10]*SPP[5] - (P[12][10]*q0)/2;
+    nextP[3][11] = P[3][11] + P[0][11]*SF[4] + P[1][11]*SF[3] + P[2][11]*SF[6] + P[10][11]*SPP[4] - P[11][11]*SPP[5] - (P[12][11]*q0)/2;
+    nextP[3][12] = P[3][12] + P[0][12]*SF[4] + P[1][12]*SF[3] + P[2][12]*SF[6] + P[10][12]*SPP[4] - P[11][12]*SPP[5] - (P[12][12]*q0)/2;
+    nextP[3][13] = P[3][13] + P[0][13]*SF[4] + P[1][13]*SF[3] + P[2][13]*SF[6] + P[10][13]*SPP[4] - P[11][13]*SPP[5] - (P[12][13]*q0)/2;
+    nextP[3][14] = P[3][14] + P[0][14]*SF[4] + P[1][14]*SF[3] + P[2][14]*SF[6] + P[10][14]*SPP[4] - P[11][14]*SPP[5] - (P[12][14]*q0)/2;
+    nextP[3][15] = P[3][15] + P[0][15]*SF[4] + P[1][15]*SF[3] + P[2][15]*SF[6] + P[10][15]*SPP[4] - P[11][15]*SPP[5] - (P[12][15]*q0)/2;
+    nextP[3][16] = P[3][16] + P[0][16]*SF[4] + P[1][16]*SF[3] + P[2][16]*SF[6] + P[10][16]*SPP[4] - P[11][16]*SPP[5] - (P[12][16]*q0)/2;
+    nextP[3][17] = P[3][17] + P[0][17]*SF[4] + P[1][17]*SF[3] + P[2][17]*SF[6] + P[10][17]*SPP[4] - P[11][17]*SPP[5] - (P[12][17]*q0)/2;
+    nextP[3][18] = P[3][18] + P[0][18]*SF[4] + P[1][18]*SF[3] + P[2][18]*SF[6] + P[10][18]*SPP[4] - P[11][18]*SPP[5] - (P[12][18]*q0)/2;
+    nextP[3][19] = P[3][19] + P[0][19]*SF[4] + P[1][19]*SF[3] + P[2][19]*SF[6] + P[10][19]*SPP[4] - P[11][19]*SPP[5] - (P[12][19]*q0)/2;
+    nextP[3][20] = P[3][20] + P[0][20]*SF[4] + P[1][20]*SF[3] + P[2][20]*SF[6] + P[10][20]*SPP[4] - P[11][20]*SPP[5] - (P[12][20]*q0)/2;
+    nextP[4][0] = P[4][0] + P[0][0]*SF[2] + P[1][0]*SF[0] + P[2][0]*SPP[0] - P[3][0]*SPP[2] + SF[6]*(P[4][1] + P[0][1]*SF[2] + P[1][1]*SF[0] + P[2][1]*SPP[0] - P[3][1]*SPP[2]) + SPP[7]*(P[4][2] + P[0][2]*SF[2] + P[1][2]*SF[0] + P[2][2]*SPP[0] - P[3][2]*SPP[2]) + SPP[6]*(P[4][3] + P[0][3]*SF[2] + P[1][3]*SF[0] + P[2][3]*SPP[0] - P[3][3]*SPP[2]) + SPP[5]*(P[4][10] + P[0][10]*SF[2] + P[1][10]*SF[0] + P[2][10]*SPP[0] - P[3][10]*SPP[2]) + SPP[4]*(P[4][11] + P[0][11]*SF[2] + P[1][11]*SF[0] + P[2][11]*SPP[0] - P[3][11]*SPP[2]) + SPP[3]*(P[4][12] + P[0][12]*SF[2] + P[1][12]*SF[0] + P[2][12]*SPP[0] - P[3][12]*SPP[2]);
+    nextP[4][1] = P[4][1] + P[0][1]*SF[2] + P[1][1]*SF[0] + P[2][1]*SPP[0] - P[3][1]*SPP[2] + SF[5]*(P[4][0] + P[0][0]*SF[2] + P[1][0]*SF[0] + P[2][0]*SPP[0] - P[3][0]*SPP[2]) + SF[4]*(P[4][2] + P[0][2]*SF[2] + P[1][2]*SF[0] + P[2][2]*SPP[0] - P[3][2]*SPP[2]) + SPP[7]*(P[4][3] + P[0][3]*SF[2] + P[1][3]*SF[0] + P[2][3]*SPP[0] - P[3][3]*SPP[2]) + SPP[3]*(P[4][11] + P[0][11]*SF[2] + P[1][11]*SF[0] + P[2][11]*SPP[0] - P[3][11]*SPP[2]) - SPP[4]*(P[4][12] + P[0][12]*SF[2] + P[1][12]*SF[0] + P[2][12]*SPP[0] - P[3][12]*SPP[2]) - (q0*(P[4][10] + P[0][10]*SF[2] + P[1][10]*SF[0] + P[2][10]*SPP[0] - P[3][10]*SPP[2]))/2;
+    nextP[4][2] = P[4][2] + P[0][2]*SF[2] + P[1][2]*SF[0] + P[2][2]*SPP[0] - P[3][2]*SPP[2] + SF[3]*(P[4][0] + P[0][0]*SF[2] + P[1][0]*SF[0] + P[2][0]*SPP[0] - P[3][0]*SPP[2]) + SF[5]*(P[4][3] + P[0][3]*SF[2] + P[1][3]*SF[0] + P[2][3]*SPP[0] - P[3][3]*SPP[2]) + SPP[6]*(P[4][1] + P[0][1]*SF[2] + P[1][1]*SF[0] + P[2][1]*SPP[0] - P[3][1]*SPP[2]) - SPP[3]*(P[4][10] + P[0][10]*SF[2] + P[1][10]*SF[0] + P[2][10]*SPP[0] - P[3][10]*SPP[2]) + SPP[5]*(P[4][12] + P[0][12]*SF[2] + P[1][12]*SF[0] + P[2][12]*SPP[0] - P[3][12]*SPP[2]) - (q0*(P[4][11] + P[0][11]*SF[2] + P[1][11]*SF[0] + P[2][11]*SPP[0] - P[3][11]*SPP[2]))/2;
+    nextP[4][3] = P[4][3] + P[0][3]*SF[2] + P[1][3]*SF[0] + P[2][3]*SPP[0] - P[3][3]*SPP[2] + SF[4]*(P[4][0] + P[0][0]*SF[2] + P[1][0]*SF[0] + P[2][0]*SPP[0] - P[3][0]*SPP[2]) + SF[3]*(P[4][1] + P[0][1]*SF[2] + P[1][1]*SF[0] + P[2][1]*SPP[0] - P[3][1]*SPP[2]) + SF[6]*(P[4][2] + P[0][2]*SF[2] + P[1][2]*SF[0] + P[2][2]*SPP[0] - P[3][2]*SPP[2]) + SPP[4]*(P[4][10] + P[0][10]*SF[2] + P[1][10]*SF[0] + P[2][10]*SPP[0] - P[3][10]*SPP[2]) - SPP[5]*(P[4][11] + P[0][11]*SF[2] + P[1][11]*SF[0] + P[2][11]*SPP[0] - P[3][11]*SPP[2]) - (q0*(P[4][12] + P[0][12]*SF[2] + P[1][12]*SF[0] + P[2][12]*SPP[0] - P[3][12]*SPP[2]))/2;
+    nextP[4][4] = P[4][4] + P[0][4]*SF[2] + P[1][4]*SF[0] + P[2][4]*SPP[0] - P[3][4]*SPP[2] + dvyCov*sq(SG[7] - 2*q0*q3) + dvzCov*sq(SG[6] + 2*q0*q2) + SF[2]*(P[4][0] + P[0][0]*SF[2] + P[1][0]*SF[0] + P[2][0]*SPP[0] - P[3][0]*SPP[2]) + SF[0]*(P[4][1] + P[0][1]*SF[2] + P[1][1]*SF[0] + P[2][1]*SPP[0] - P[3][1]*SPP[2]) + SPP[0]*(P[4][2] + P[0][2]*SF[2] + P[1][2]*SF[0] + P[2][2]*SPP[0] - P[3][2]*SPP[2]) - SPP[2]*(P[4][3] + P[0][3]*SF[2] + P[1][3]*SF[0] + P[2][3]*SPP[0] - P[3][3]*SPP[2]) + dvxCov*sq(SG[1] + SG[2] - SG[3] - SG[4]);
+    nextP[4][5] = P[4][5] + SQ[2] + P[0][5]*SF[2] + P[1][5]*SF[0] + P[2][5]*SPP[0] - P[3][5]*SPP[2] + SF[1]*(P[4][0] + P[0][0]*SF[2] + P[1][0]*SF[0] + P[2][0]*SPP[0] - P[3][0]*SPP[2]) + SF[0]*(P[4][2] + P[0][2]*SF[2] + P[1][2]*SF[0] + P[2][2]*SPP[0] - P[3][2]*SPP[2]) + SF[2]*(P[4][3] + P[0][3]*SF[2] + P[1][3]*SF[0] + P[2][3]*SPP[0] - P[3][3]*SPP[2]) - SPP[0]*(P[4][1] + P[0][1]*SF[2] + P[1][1]*SF[0] + P[2][1]*SPP[0] - P[3][1]*SPP[2]);
+    nextP[4][6] = P[4][6] + SQ[1] + P[0][6]*SF[2] + P[1][6]*SF[0] + P[2][6]*SPP[0] - P[3][6]*SPP[2] + SF[1]*(P[4][1] + P[0][1]*SF[2] + P[1][1]*SF[0] + P[2][1]*SPP[0] - P[3][1]*SPP[2]) + SF[0]*(P[4][3] + P[0][3]*SF[2] + P[1][3]*SF[0] + P[2][3]*SPP[0] - P[3][3]*SPP[2]) + SPP[0]*(P[4][0] + P[0][0]*SF[2] + P[1][0]*SF[0] + P[2][0]*SPP[0] - P[3][0]*SPP[2]) - SPP[1]*(P[4][2] + P[0][2]*SF[2] + P[1][2]*SF[0] + P[2][2]*SPP[0] - P[3][2]*SPP[2]);
+    nextP[4][7] = P[4][7] + P[0][7]*SF[2] + P[1][7]*SF[0] + P[2][7]*SPP[0] - P[3][7]*SPP[2] + dt*(P[4][4] + P[0][4]*SF[2] + P[1][4]*SF[0] + P[2][4]*SPP[0] - P[3][4]*SPP[2]);
+    nextP[4][8] = P[4][8] + P[0][8]*SF[2] + P[1][8]*SF[0] + P[2][8]*SPP[0] - P[3][8]*SPP[2] + dt*(P[4][5] + P[0][5]*SF[2] + P[1][5]*SF[0] + P[2][5]*SPP[0] - P[3][5]*SPP[2]);
+    nextP[4][9] = P[4][9] + P[0][9]*SF[2] + P[1][9]*SF[0] + P[2][9]*SPP[0] - P[3][9]*SPP[2] + dt*(P[4][6] + P[0][6]*SF[2] + P[1][6]*SF[0] + P[2][6]*SPP[0] - P[3][6]*SPP[2]);
+    nextP[4][10] = P[4][10] + P[0][10]*SF[2] + P[1][10]*SF[0] + P[2][10]*SPP[0] - P[3][10]*SPP[2];
+    nextP[4][11] = P[4][11] + P[0][11]*SF[2] + P[1][11]*SF[0] + P[2][11]*SPP[0] - P[3][11]*SPP[2];
+    nextP[4][12] = P[4][12] + P[0][12]*SF[2] + P[1][12]*SF[0] + P[2][12]*SPP[0] - P[3][12]*SPP[2];
+    nextP[4][13] = P[4][13] + P[0][13]*SF[2] + P[1][13]*SF[0] + P[2][13]*SPP[0] - P[3][13]*SPP[2];
+    nextP[4][14] = P[4][14] + P[0][14]*SF[2] + P[1][14]*SF[0] + P[2][14]*SPP[0] - P[3][14]*SPP[2];
+    nextP[4][15] = P[4][15] + P[0][15]*SF[2] + P[1][15]*SF[0] + P[2][15]*SPP[0] - P[3][15]*SPP[2];
+    nextP[4][16] = P[4][16] + P[0][16]*SF[2] + P[1][16]*SF[0] + P[2][16]*SPP[0] - P[3][16]*SPP[2];
+    nextP[4][17] = P[4][17] + P[0][17]*SF[2] + P[1][17]*SF[0] + P[2][17]*SPP[0] - P[3][17]*SPP[2];
+    nextP[4][18] = P[4][18] + P[0][18]*SF[2] + P[1][18]*SF[0] + P[2][18]*SPP[0] - P[3][18]*SPP[2];
+    nextP[4][19] = P[4][19] + P[0][19]*SF[2] + P[1][19]*SF[0] + P[2][19]*SPP[0] - P[3][19]*SPP[2];
+    nextP[4][20] = P[4][20] + P[0][20]*SF[2] + P[1][20]*SF[0] + P[2][20]*SPP[0] - P[3][20]*SPP[2];
+    nextP[5][0] = P[5][0] + P[0][0]*SF[1] + P[2][0]*SF[0] + P[3][0]*SF[2] - P[1][0]*SPP[0] + SF[6]*(P[5][1] + P[0][1]*SF[1] + P[2][1]*SF[0] + P[3][1]*SF[2] - P[1][1]*SPP[0]) + SPP[7]*(P[5][2] + P[0][2]*SF[1] + P[2][2]*SF[0] + P[3][2]*SF[2] - P[1][2]*SPP[0]) + SPP[6]*(P[5][3] + P[0][3]*SF[1] + P[2][3]*SF[0] + P[3][3]*SF[2] - P[1][3]*SPP[0]) + SPP[5]*(P[5][10] + P[0][10]*SF[1] + P[2][10]*SF[0] + P[3][10]*SF[2] - P[1][10]*SPP[0]) + SPP[4]*(P[5][11] + P[0][11]*SF[1] + P[2][11]*SF[0] + P[3][11]*SF[2] - P[1][11]*SPP[0]) + SPP[3]*(P[5][12] + P[0][12]*SF[1] + P[2][12]*SF[0] + P[3][12]*SF[2] - P[1][12]*SPP[0]);
+    nextP[5][1] = P[5][1] + P[0][1]*SF[1] + P[2][1]*SF[0] + P[3][1]*SF[2] - P[1][1]*SPP[0] + SF[5]*(P[5][0] + P[0][0]*SF[1] + P[2][0]*SF[0] + P[3][0]*SF[2] - P[1][0]*SPP[0]) + SF[4]*(P[5][2] + P[0][2]*SF[1] + P[2][2]*SF[0] + P[3][2]*SF[2] - P[1][2]*SPP[0]) + SPP[7]*(P[5][3] + P[0][3]*SF[1] + P[2][3]*SF[0] + P[3][3]*SF[2] - P[1][3]*SPP[0]) + SPP[3]*(P[5][11] + P[0][11]*SF[1] + P[2][11]*SF[0] + P[3][11]*SF[2] - P[1][11]*SPP[0]) - SPP[4]*(P[5][12] + P[0][12]*SF[1] + P[2][12]*SF[0] + P[3][12]*SF[2] - P[1][12]*SPP[0]) - (q0*(P[5][10] + P[0][10]*SF[1] + P[2][10]*SF[0] + P[3][10]*SF[2] - P[1][10]*SPP[0]))/2;
+    nextP[5][2] = P[5][2] + P[0][2]*SF[1] + P[2][2]*SF[0] + P[3][2]*SF[2] - P[1][2]*SPP[0] + SF[3]*(P[5][0] + P[0][0]*SF[1] + P[2][0]*SF[0] + P[3][0]*SF[2] - P[1][0]*SPP[0]) + SF[5]*(P[5][3] + P[0][3]*SF[1] + P[2][3]*SF[0] + P[3][3]*SF[2] - P[1][3]*SPP[0]) + SPP[6]*(P[5][1] + P[0][1]*SF[1] + P[2][1]*SF[0] + P[3][1]*SF[2] - P[1][1]*SPP[0]) - SPP[3]*(P[5][10] + P[0][10]*SF[1] + P[2][10]*SF[0] + P[3][10]*SF[2] - P[1][10]*SPP[0]) + SPP[5]*(P[5][12] + P[0][12]*SF[1] + P[2][12]*SF[0] + P[3][12]*SF[2] - P[1][12]*SPP[0]) - (q0*(P[5][11] + P[0][11]*SF[1] + P[2][11]*SF[0] + P[3][11]*SF[2] - P[1][11]*SPP[0]))/2;
+    nextP[5][3] = P[5][3] + P[0][3]*SF[1] + P[2][3]*SF[0] + P[3][3]*SF[2] - P[1][3]*SPP[0] + SF[4]*(P[5][0] + P[0][0]*SF[1] + P[2][0]*SF[0] + P[3][0]*SF[2] - P[1][0]*SPP[0]) + SF[3]*(P[5][1] + P[0][1]*SF[1] + P[2][1]*SF[0] + P[3][1]*SF[2] - P[1][1]*SPP[0]) + SF[6]*(P[5][2] + P[0][2]*SF[1] + P[2][2]*SF[0] + P[3][2]*SF[2] - P[1][2]*SPP[0]) + SPP[4]*(P[5][10] + P[0][10]*SF[1] + P[2][10]*SF[0] + P[3][10]*SF[2] - P[1][10]*SPP[0]) - SPP[5]*(P[5][11] + P[0][11]*SF[1] + P[2][11]*SF[0] + P[3][11]*SF[2] - P[1][11]*SPP[0]) - (q0*(P[5][12] + P[0][12]*SF[1] + P[2][12]*SF[0] + P[3][12]*SF[2] - P[1][12]*SPP[0]))/2;
+    nextP[5][4] = P[5][4] + SQ[2] + P[0][4]*SF[1] + P[2][4]*SF[0] + P[3][4]*SF[2] - P[1][4]*SPP[0] + SF[2]*(P[5][0] + P[0][0]*SF[1] + P[2][0]*SF[0] + P[3][0]*SF[2] - P[1][0]*SPP[0]) + SF[0]*(P[5][1] + P[0][1]*SF[1] + P[2][1]*SF[0] + P[3][1]*SF[2] - P[1][1]*SPP[0]) + SPP[0]*(P[5][2] + P[0][2]*SF[1] + P[2][2]*SF[0] + P[3][2]*SF[2] - P[1][2]*SPP[0]) - SPP[2]*(P[5][3] + P[0][3]*SF[1] + P[2][3]*SF[0] + P[3][3]*SF[2] - P[1][3]*SPP[0]);
+    nextP[5][5] = P[5][5] + P[0][5]*SF[1] + P[2][5]*SF[0] + P[3][5]*SF[2] - P[1][5]*SPP[0] + dvxCov*sq(SG[7] + 2*q0*q3) + dvzCov*sq(SG[5] - 2*q0*q1) + SF[1]*(P[5][0] + P[0][0]*SF[1] + P[2][0]*SF[0] + P[3][0]*SF[2] - P[1][0]*SPP[0]) + SF[0]*(P[5][2] + P[0][2]*SF[1] + P[2][2]*SF[0] + P[3][2]*SF[2] - P[1][2]*SPP[0]) + SF[2]*(P[5][3] + P[0][3]*SF[1] + P[2][3]*SF[0] + P[3][3]*SF[2] - P[1][3]*SPP[0]) - SPP[0]*(P[5][1] + P[0][1]*SF[1] + P[2][1]*SF[0] + P[3][1]*SF[2] - P[1][1]*SPP[0]) + dvyCov*sq(SG[1] - SG[2] + SG[3] - SG[4]);
+    nextP[5][6] = P[5][6] + SQ[0] + P[0][6]*SF[1] + P[2][6]*SF[0] + P[3][6]*SF[2] - P[1][6]*SPP[0] + SF[1]*(P[5][1] + P[0][1]*SF[1] + P[2][1]*SF[0] + P[3][1]*SF[2] - P[1][1]*SPP[0]) + SF[0]*(P[5][3] + P[0][3]*SF[1] + P[2][3]*SF[0] + P[3][3]*SF[2] - P[1][3]*SPP[0]) + SPP[0]*(P[5][0] + P[0][0]*SF[1] + P[2][0]*SF[0] + P[3][0]*SF[2] - P[1][0]*SPP[0]) - SPP[1]*(P[5][2] + P[0][2]*SF[1] + P[2][2]*SF[0] + P[3][2]*SF[2] - P[1][2]*SPP[0]);
+    nextP[5][7] = P[5][7] + P[0][7]*SF[1] + P[2][7]*SF[0] + P[3][7]*SF[2] - P[1][7]*SPP[0] + dt*(P[5][4] + P[0][4]*SF[1] + P[2][4]*SF[0] + P[3][4]*SF[2] - P[1][4]*SPP[0]);
+    nextP[5][8] = P[5][8] + P[0][8]*SF[1] + P[2][8]*SF[0] + P[3][8]*SF[2] - P[1][8]*SPP[0] + dt*(P[5][5] + P[0][5]*SF[1] + P[2][5]*SF[0] + P[3][5]*SF[2] - P[1][5]*SPP[0]);
+    nextP[5][9] = P[5][9] + P[0][9]*SF[1] + P[2][9]*SF[0] + P[3][9]*SF[2] - P[1][9]*SPP[0] + dt*(P[5][6] + P[0][6]*SF[1] + P[2][6]*SF[0] + P[3][6]*SF[2] - P[1][6]*SPP[0]);
+    nextP[5][10] = P[5][10] + P[0][10]*SF[1] + P[2][10]*SF[0] + P[3][10]*SF[2] - P[1][10]*SPP[0];
+    nextP[5][11] = P[5][11] + P[0][11]*SF[1] + P[2][11]*SF[0] + P[3][11]*SF[2] - P[1][11]*SPP[0];
+    nextP[5][12] = P[5][12] + P[0][12]*SF[1] + P[2][12]*SF[0] + P[3][12]*SF[2] - P[1][12]*SPP[0];
+    nextP[5][13] = P[5][13] + P[0][13]*SF[1] + P[2][13]*SF[0] + P[3][13]*SF[2] - P[1][13]*SPP[0];
+    nextP[5][14] = P[5][14] + P[0][14]*SF[1] + P[2][14]*SF[0] + P[3][14]*SF[2] - P[1][14]*SPP[0];
+    nextP[5][15] = P[5][15] + P[0][15]*SF[1] + P[2][15]*SF[0] + P[3][15]*SF[2] - P[1][15]*SPP[0];
+    nextP[5][16] = P[5][16] + P[0][16]*SF[1] + P[2][16]*SF[0] + P[3][16]*SF[2] - P[1][16]*SPP[0];
+    nextP[5][17] = P[5][17] + P[0][17]*SF[1] + P[2][17]*SF[0] + P[3][17]*SF[2] - P[1][17]*SPP[0];
+    nextP[5][18] = P[5][18] + P[0][18]*SF[1] + P[2][18]*SF[0] + P[3][18]*SF[2] - P[1][18]*SPP[0];
+    nextP[5][19] = P[5][19] + P[0][19]*SF[1] + P[2][19]*SF[0] + P[3][19]*SF[2] - P[1][19]*SPP[0];
+    nextP[5][20] = P[5][20] + P[0][20]*SF[1] + P[2][20]*SF[0] + P[3][20]*SF[2] - P[1][20]*SPP[0];
+    nextP[6][0] = P[6][0] + P[1][0]*SF[1] + P[3][0]*SF[0] + P[0][0]*SPP[0] - P[2][0]*SPP[1] + SF[6]*(P[6][1] + P[1][1]*SF[1] + P[3][1]*SF[0] + P[0][1]*SPP[0] - P[2][1]*SPP[1]) + SPP[7]*(P[6][2] + P[1][2]*SF[1] + P[3][2]*SF[0] + P[0][2]*SPP[0] - P[2][2]*SPP[1]) + SPP[6]*(P[6][3] + P[1][3]*SF[1] + P[3][3]*SF[0] + P[0][3]*SPP[0] - P[2][3]*SPP[1]) + SPP[5]*(P[6][10] + P[1][10]*SF[1] + P[3][10]*SF[0] + P[0][10]*SPP[0] - P[2][10]*SPP[1]) + SPP[4]*(P[6][11] + P[1][11]*SF[1] + P[3][11]*SF[0] + P[0][11]*SPP[0] - P[2][11]*SPP[1]) + SPP[3]*(P[6][12] + P[1][12]*SF[1] + P[3][12]*SF[0] + P[0][12]*SPP[0] - P[2][12]*SPP[1]);
+    nextP[6][1] = P[6][1] + P[1][1]*SF[1] + P[3][1]*SF[0] + P[0][1]*SPP[0] - P[2][1]*SPP[1] + SF[5]*(P[6][0] + P[1][0]*SF[1] + P[3][0]*SF[0] + P[0][0]*SPP[0] - P[2][0]*SPP[1]) + SF[4]*(P[6][2] + P[1][2]*SF[1] + P[3][2]*SF[0] + P[0][2]*SPP[0] - P[2][2]*SPP[1]) + SPP[7]*(P[6][3] + P[1][3]*SF[1] + P[3][3]*SF[0] + P[0][3]*SPP[0] - P[2][3]*SPP[1]) + SPP[3]*(P[6][11] + P[1][11]*SF[1] + P[3][11]*SF[0] + P[0][11]*SPP[0] - P[2][11]*SPP[1]) - SPP[4]*(P[6][12] + P[1][12]*SF[1] + P[3][12]*SF[0] + P[0][12]*SPP[0] - P[2][12]*SPP[1]) - (q0*(P[6][10] + P[1][10]*SF[1] + P[3][10]*SF[0] + P[0][10]*SPP[0] - P[2][10]*SPP[1]))/2;
+    nextP[6][2] = P[6][2] + P[1][2]*SF[1] + P[3][2]*SF[0] + P[0][2]*SPP[0] - P[2][2]*SPP[1] + SF[3]*(P[6][0] + P[1][0]*SF[1] + P[3][0]*SF[0] + P[0][0]*SPP[0] - P[2][0]*SPP[1]) + SF[5]*(P[6][3] + P[1][3]*SF[1] + P[3][3]*SF[0] + P[0][3]*SPP[0] - P[2][3]*SPP[1]) + SPP[6]*(P[6][1] + P[1][1]*SF[1] + P[3][1]*SF[0] + P[0][1]*SPP[0] - P[2][1]*SPP[1]) - SPP[3]*(P[6][10] + P[1][10]*SF[1] + P[3][10]*SF[0] + P[0][10]*SPP[0] - P[2][10]*SPP[1]) + SPP[5]*(P[6][12] + P[1][12]*SF[1] + P[3][12]*SF[0] + P[0][12]*SPP[0] - P[2][12]*SPP[1]) - (q0*(P[6][11] + P[1][11]*SF[1] + P[3][11]*SF[0] + P[0][11]*SPP[0] - P[2][11]*SPP[1]))/2;
+    nextP[6][3] = P[6][3] + P[1][3]*SF[1] + P[3][3]*SF[0] + P[0][3]*SPP[0] - P[2][3]*SPP[1] + SF[4]*(P[6][0] + P[1][0]*SF[1] + P[3][0]*SF[0] + P[0][0]*SPP[0] - P[2][0]*SPP[1]) + SF[3]*(P[6][1] + P[1][1]*SF[1] + P[3][1]*SF[0] + P[0][1]*SPP[0] - P[2][1]*SPP[1]) + SF[6]*(P[6][2] + P[1][2]*SF[1] + P[3][2]*SF[0] + P[0][2]*SPP[0] - P[2][2]*SPP[1]) + SPP[4]*(P[6][10] + P[1][10]*SF[1] + P[3][10]*SF[0] + P[0][10]*SPP[0] - P[2][10]*SPP[1]) - SPP[5]*(P[6][11] + P[1][11]*SF[1] + P[3][11]*SF[0] + P[0][11]*SPP[0] - P[2][11]*SPP[1]) - (q0*(P[6][12] + P[1][12]*SF[1] + P[3][12]*SF[0] + P[0][12]*SPP[0] - P[2][12]*SPP[1]))/2;
+    nextP[6][4] = P[6][4] + SQ[1] + P[1][4]*SF[1] + P[3][4]*SF[0] + P[0][4]*SPP[0] - P[2][4]*SPP[1] + SF[2]*(P[6][0] + P[1][0]*SF[1] + P[3][0]*SF[0] + P[0][0]*SPP[0] - P[2][0]*SPP[1]) + SF[0]*(P[6][1] + P[1][1]*SF[1] + P[3][1]*SF[0] + P[0][1]*SPP[0] - P[2][1]*SPP[1]) + SPP[0]*(P[6][2] + P[1][2]*SF[1] + P[3][2]*SF[0] + P[0][2]*SPP[0] - P[2][2]*SPP[1]) - SPP[2]*(P[6][3] + P[1][3]*SF[1] + P[3][3]*SF[0] + P[0][3]*SPP[0] - P[2][3]*SPP[1]);
+    nextP[6][5] = P[6][5] + SQ[0] + P[1][5]*SF[1] + P[3][5]*SF[0] + P[0][5]*SPP[0] - P[2][5]*SPP[1] + SF[1]*(P[6][0] + P[1][0]*SF[1] + P[3][0]*SF[0] + P[0][0]*SPP[0] - P[2][0]*SPP[1]) + SF[0]*(P[6][2] + P[1][2]*SF[1] + P[3][2]*SF[0] + P[0][2]*SPP[0] - P[2][2]*SPP[1]) + SF[2]*(P[6][3] + P[1][3]*SF[1] + P[3][3]*SF[0] + P[0][3]*SPP[0] - P[2][3]*SPP[1]) - SPP[0]*(P[6][1] + P[1][1]*SF[1] + P[3][1]*SF[0] + P[0][1]*SPP[0] - P[2][1]*SPP[1]);
+    nextP[6][6] = P[6][6] + P[1][6]*SF[1] + P[3][6]*SF[0] + P[0][6]*SPP[0] - P[2][6]*SPP[1] + dvxCov*sq(SG[6] - 2*q0*q2) + dvyCov*sq(SG[5] + 2*q0*q1) + SF[1]*(P[6][1] + P[1][1]*SF[1] + P[3][1]*SF[0] + P[0][1]*SPP[0] - P[2][1]*SPP[1]) + SF[0]*(P[6][3] + P[1][3]*SF[1] + P[3][3]*SF[0] + P[0][3]*SPP[0] - P[2][3]*SPP[1]) + SPP[0]*(P[6][0] + P[1][0]*SF[1] + P[3][0]*SF[0] + P[0][0]*SPP[0] - P[2][0]*SPP[1]) - SPP[1]*(P[6][2] + P[1][2]*SF[1] + P[3][2]*SF[0] + P[0][2]*SPP[0] - P[2][2]*SPP[1]) + dvzCov*sq(SG[1] - SG[2] - SG[3] + SG[4]);
+    nextP[6][7] = P[6][7] + P[1][7]*SF[1] + P[3][7]*SF[0] + P[0][7]*SPP[0] - P[2][7]*SPP[1] + dt*(P[6][4] + P[1][4]*SF[1] + P[3][4]*SF[0] + P[0][4]*SPP[0] - P[2][4]*SPP[1]);
+    nextP[6][8] = P[6][8] + P[1][8]*SF[1] + P[3][8]*SF[0] + P[0][8]*SPP[0] - P[2][8]*SPP[1] + dt*(P[6][5] + P[1][5]*SF[1] + P[3][5]*SF[0] + P[0][5]*SPP[0] - P[2][5]*SPP[1]);
+    nextP[6][9] = P[6][9] + P[1][9]*SF[1] + P[3][9]*SF[0] + P[0][9]*SPP[0] - P[2][9]*SPP[1] + dt*(P[6][6] + P[1][6]*SF[1] + P[3][6]*SF[0] + P[0][6]*SPP[0] - P[2][6]*SPP[1]);
+    nextP[6][10] = P[6][10] + P[1][10]*SF[1] + P[3][10]*SF[0] + P[0][10]*SPP[0] - P[2][10]*SPP[1];
+    nextP[6][11] = P[6][11] + P[1][11]*SF[1] + P[3][11]*SF[0] + P[0][11]*SPP[0] - P[2][11]*SPP[1];
+    nextP[6][12] = P[6][12] + P[1][12]*SF[1] + P[3][12]*SF[0] + P[0][12]*SPP[0] - P[2][12]*SPP[1];
+    nextP[6][13] = P[6][13] + P[1][13]*SF[1] + P[3][13]*SF[0] + P[0][13]*SPP[0] - P[2][13]*SPP[1];
+    nextP[6][14] = P[6][14] + P[1][14]*SF[1] + P[3][14]*SF[0] + P[0][14]*SPP[0] - P[2][14]*SPP[1];
+    nextP[6][15] = P[6][15] + P[1][15]*SF[1] + P[3][15]*SF[0] + P[0][15]*SPP[0] - P[2][15]*SPP[1];
+    nextP[6][16] = P[6][16] + P[1][16]*SF[1] + P[3][16]*SF[0] + P[0][16]*SPP[0] - P[2][16]*SPP[1];
+    nextP[6][17] = P[6][17] + P[1][17]*SF[1] + P[3][17]*SF[0] + P[0][17]*SPP[0] - P[2][17]*SPP[1];
+    nextP[6][18] = P[6][18] + P[1][18]*SF[1] + P[3][18]*SF[0] + P[0][18]*SPP[0] - P[2][18]*SPP[1];
+    nextP[6][19] = P[6][19] + P[1][19]*SF[1] + P[3][19]*SF[0] + P[0][19]*SPP[0] - P[2][19]*SPP[1];
+    nextP[6][20] = P[6][20] + P[1][20]*SF[1] + P[3][20]*SF[0] + P[0][20]*SPP[0] - P[2][20]*SPP[1];
+    nextP[7][0] = P[7][0] + P[4][0]*dt + SF[6]*(P[7][1] + P[4][1]*dt) + SPP[7]*(P[7][2] + P[4][2]*dt) + SPP[6]*(P[7][3] + P[4][3]*dt) + SPP[5]*(P[7][10] + P[4][10]*dt) + SPP[4]*(P[7][11] + P[4][11]*dt) + SPP[3]*(P[7][12] + P[4][12]*dt);
+    nextP[7][1] = P[7][1] + P[4][1]*dt + SF[5]*(P[7][0] + P[4][0]*dt) + SF[4]*(P[7][2] + P[4][2]*dt) + SPP[7]*(P[7][3] + P[4][3]*dt) + SPP[3]*(P[7][11] + P[4][11]*dt) - SPP[4]*(P[7][12] + P[4][12]*dt) - (q0*(P[7][10] + P[4][10]*dt))/2;
+    nextP[7][2] = P[7][2] + P[4][2]*dt + SF[3]*(P[7][0] + P[4][0]*dt) + SF[5]*(P[7][3] + P[4][3]*dt) + SPP[6]*(P[7][1] + P[4][1]*dt) - SPP[3]*(P[7][10] + P[4][10]*dt) + SPP[5]*(P[7][12] + P[4][12]*dt) - (q0*(P[7][11] + P[4][11]*dt))/2;
+    nextP[7][3] = P[7][3] + P[4][3]*dt + SF[4]*(P[7][0] + P[4][0]*dt) + SF[3]*(P[7][1] + P[4][1]*dt) + SF[6]*(P[7][2] + P[4][2]*dt) + SPP[4]*(P[7][10] + P[4][10]*dt) - SPP[5]*(P[7][11] + P[4][11]*dt) - (q0*(P[7][12] + P[4][12]*dt))/2;
+    nextP[7][4] = P[7][4] + P[4][4]*dt + SF[0]*(P[7][1] + P[4][1]*dt) + SF[2]*(P[7][0] + P[4][0]*dt) + SPP[0]*(P[7][2] + P[4][2]*dt) - SPP[2]*(P[7][3] + P[4][3]*dt);
+    nextP[7][5] = P[7][5] + P[4][5]*dt + SF[1]*(P[7][0] + P[4][0]*dt) + SF[0]*(P[7][2] + P[4][2]*dt) + SF[2]*(P[7][3] + P[4][3]*dt) - SPP[0]*(P[7][1] + P[4][1]*dt);
+    nextP[7][6] = P[7][6] + P[4][6]*dt + SF[1]*(P[7][1] + P[4][1]*dt) + SF[0]*(P[7][3] + P[4][3]*dt) + SPP[0]*(P[7][0] + P[4][0]*dt) - SPP[1]*(P[7][2] + P[4][2]*dt);
+    nextP[7][7] = P[7][7] + P[4][7]*dt + dt*(P[7][4] + P[4][4]*dt);
+    nextP[7][8] = P[7][8] + P[4][8]*dt + dt*(P[7][5] + P[4][5]*dt);
+    nextP[7][9] = P[7][9] + P[4][9]*dt + dt*(P[7][6] + P[4][6]*dt);
+    nextP[7][10] = P[7][10] + P[4][10]*dt;
+    nextP[7][11] = P[7][11] + P[4][11]*dt;
+    nextP[7][12] = P[7][12] + P[4][12]*dt;
+    nextP[7][13] = P[7][13] + P[4][13]*dt;
+    nextP[7][14] = P[7][14] + P[4][14]*dt;
+    nextP[7][15] = P[7][15] + P[4][15]*dt;
+    nextP[7][16] = P[7][16] + P[4][16]*dt;
+    nextP[7][17] = P[7][17] + P[4][17]*dt;
+    nextP[7][18] = P[7][18] + P[4][18]*dt;
+    nextP[7][19] = P[7][19] + P[4][19]*dt;
+    nextP[7][20] = P[7][20] + P[4][20]*dt;
+    nextP[8][0] = P[8][0] + P[5][0]*dt + SF[6]*(P[8][1] + P[5][1]*dt) + SPP[7]*(P[8][2] + P[5][2]*dt) + SPP[6]*(P[8][3] + P[5][3]*dt) + SPP[5]*(P[8][10] + P[5][10]*dt) + SPP[4]*(P[8][11] + P[5][11]*dt) + SPP[3]*(P[8][12] + P[5][12]*dt);
+    nextP[8][1] = P[8][1] + P[5][1]*dt + SF[5]*(P[8][0] + P[5][0]*dt) + SF[4]*(P[8][2] + P[5][2]*dt) + SPP[7]*(P[8][3] + P[5][3]*dt) + SPP[3]*(P[8][11] + P[5][11]*dt) - SPP[4]*(P[8][12] + P[5][12]*dt) - (q0*(P[8][10] + P[5][10]*dt))/2;
+    nextP[8][2] = P[8][2] + P[5][2]*dt + SF[3]*(P[8][0] + P[5][0]*dt) + SF[5]*(P[8][3] + P[5][3]*dt) + SPP[6]*(P[8][1] + P[5][1]*dt) - SPP[3]*(P[8][10] + P[5][10]*dt) + SPP[5]*(P[8][12] + P[5][12]*dt) - (q0*(P[8][11] + P[5][11]*dt))/2;
+    nextP[8][3] = P[8][3] + P[5][3]*dt + SF[4]*(P[8][0] + P[5][0]*dt) + SF[3]*(P[8][1] + P[5][1]*dt) + SF[6]*(P[8][2] + P[5][2]*dt) + SPP[4]*(P[8][10] + P[5][10]*dt) - SPP[5]*(P[8][11] + P[5][11]*dt) - (q0*(P[8][12] + P[5][12]*dt))/2;
+    nextP[8][4] = P[8][4] + P[5][4]*dt + SF[0]*(P[8][1] + P[5][1]*dt) + SF[2]*(P[8][0] + P[5][0]*dt) + SPP[0]*(P[8][2] + P[5][2]*dt) - SPP[2]*(P[8][3] + P[5][3]*dt);
+    nextP[8][5] = P[8][5] + P[5][5]*dt + SF[1]*(P[8][0] + P[5][0]*dt) + SF[0]*(P[8][2] + P[5][2]*dt) + SF[2]*(P[8][3] + P[5][3]*dt) - SPP[0]*(P[8][1] + P[5][1]*dt);
+    nextP[8][6] = P[8][6] + P[5][6]*dt + SF[1]*(P[8][1] + P[5][1]*dt) + SF[0]*(P[8][3] + P[5][3]*dt) + SPP[0]*(P[8][0] + P[5][0]*dt) - SPP[1]*(P[8][2] + P[5][2]*dt);
+    nextP[8][7] = P[8][7] + P[5][7]*dt + dt*(P[8][4] + P[5][4]*dt);
+    nextP[8][8] = P[8][8] + P[5][8]*dt + dt*(P[8][5] + P[5][5]*dt);
+    nextP[8][9] = P[8][9] + P[5][9]*dt + dt*(P[8][6] + P[5][6]*dt);
+    nextP[8][10] = P[8][10] + P[5][10]*dt;
+    nextP[8][11] = P[8][11] + P[5][11]*dt;
+    nextP[8][12] = P[8][12] + P[5][12]*dt;
+    nextP[8][13] = P[8][13] + P[5][13]*dt;
+    nextP[8][14] = P[8][14] + P[5][14]*dt;
+    nextP[8][15] = P[8][15] + P[5][15]*dt;
+    nextP[8][16] = P[8][16] + P[5][16]*dt;
+    nextP[8][17] = P[8][17] + P[5][17]*dt;
+    nextP[8][18] = P[8][18] + P[5][18]*dt;
+    nextP[8][19] = P[8][19] + P[5][19]*dt;
+    nextP[8][20] = P[8][20] + P[5][20]*dt;
+    nextP[9][0] = P[9][0] + P[6][0]*dt + SF[6]*(P[9][1] + P[6][1]*dt) + SPP[7]*(P[9][2] + P[6][2]*dt) + SPP[6]*(P[9][3] + P[6][3]*dt) + SPP[5]*(P[9][10] + P[6][10]*dt) + SPP[4]*(P[9][11] + P[6][11]*dt) + SPP[3]*(P[9][12] + P[6][12]*dt);
+    nextP[9][1] = P[9][1] + P[6][1]*dt + SF[5]*(P[9][0] + P[6][0]*dt) + SF[4]*(P[9][2] + P[6][2]*dt) + SPP[7]*(P[9][3] + P[6][3]*dt) + SPP[3]*(P[9][11] + P[6][11]*dt) - SPP[4]*(P[9][12] + P[6][12]*dt) - (q0*(P[9][10] + P[6][10]*dt))/2;
+    nextP[9][2] = P[9][2] + P[6][2]*dt + SF[3]*(P[9][0] + P[6][0]*dt) + SF[5]*(P[9][3] + P[6][3]*dt) + SPP[6]*(P[9][1] + P[6][1]*dt) - SPP[3]*(P[9][10] + P[6][10]*dt) + SPP[5]*(P[9][12] + P[6][12]*dt) - (q0*(P[9][11] + P[6][11]*dt))/2;
+    nextP[9][3] = P[9][3] + P[6][3]*dt + SF[4]*(P[9][0] + P[6][0]*dt) + SF[3]*(P[9][1] + P[6][1]*dt) + SF[6]*(P[9][2] + P[6][2]*dt) + SPP[4]*(P[9][10] + P[6][10]*dt) - SPP[5]*(P[9][11] + P[6][11]*dt) - (q0*(P[9][12] + P[6][12]*dt))/2;
+    nextP[9][4] = P[9][4] + P[6][4]*dt + SF[0]*(P[9][1] + P[6][1]*dt) + SF[2]*(P[9][0] + P[6][0]*dt) + SPP[0]*(P[9][2] + P[6][2]*dt) - SPP[2]*(P[9][3] + P[6][3]*dt);
+    nextP[9][5] = P[9][5] + P[6][5]*dt + SF[1]*(P[9][0] + P[6][0]*dt) + SF[0]*(P[9][2] + P[6][2]*dt) + SF[2]*(P[9][3] + P[6][3]*dt) - SPP[0]*(P[9][1] + P[6][1]*dt);
+    nextP[9][6] = P[9][6] + P[6][6]*dt + SF[1]*(P[9][1] + P[6][1]*dt) + SF[0]*(P[9][3] + P[6][3]*dt) + SPP[0]*(P[9][0] + P[6][0]*dt) - SPP[1]*(P[9][2] + P[6][2]*dt);
+    nextP[9][7] = P[9][7] + P[6][7]*dt + dt*(P[9][4] + P[6][4]*dt);
+    nextP[9][8] = P[9][8] + P[6][8]*dt + dt*(P[9][5] + P[6][5]*dt);
+    nextP[9][9] = P[9][9] + P[6][9]*dt + dt*(P[9][6] + P[6][6]*dt);
+    nextP[9][10] = P[9][10] + P[6][10]*dt;
+    nextP[9][11] = P[9][11] + P[6][11]*dt;
+    nextP[9][12] = P[9][12] + P[6][12]*dt;
+    nextP[9][13] = P[9][13] + P[6][13]*dt;
+    nextP[9][14] = P[9][14] + P[6][14]*dt;
+    nextP[9][15] = P[9][15] + P[6][15]*dt;
+    nextP[9][16] = P[9][16] + P[6][16]*dt;
+    nextP[9][17] = P[9][17] + P[6][17]*dt;
+    nextP[9][18] = P[9][18] + P[6][18]*dt;
+    nextP[9][19] = P[9][19] + P[6][19]*dt;
+    nextP[9][20] = P[9][20] + P[6][20]*dt;
+    nextP[10][0] = P[10][0] + P[10][1]*SF[6] + P[10][2]*SPP[7] + P[10][3]*SPP[6] + P[10][10]*SPP[5] + P[10][11]*SPP[4] + P[10][12]*SPP[3];
+    nextP[10][1] = P[10][1] + P[10][0]*SF[5] + P[10][2]*SF[4] + P[10][3]*SPP[7] + P[10][11]*SPP[3] - P[10][12]*SPP[4] - (P[10][10]*q0)/2;
+    nextP[10][2] = P[10][2] + P[10][0]*SF[3] + P[10][3]*SF[5] + P[10][1]*SPP[6] - P[10][10]*SPP[3] + P[10][12]*SPP[5] - (P[10][11]*q0)/2;
+    nextP[10][3] = P[10][3] + P[10][0]*SF[4] + P[10][1]*SF[3] + P[10][2]*SF[6] + P[10][10]*SPP[4] - P[10][11]*SPP[5] - (P[10][12]*q0)/2;
+    nextP[10][4] = P[10][4] + P[10][1]*SF[0] + P[10][0]*SF[2] + P[10][2]*SPP[0] - P[10][3]*SPP[2];
+    nextP[10][5] = P[10][5] + P[10][0]*SF[1] + P[10][2]*SF[0] + P[10][3]*SF[2] - P[10][1]*SPP[0];
+    nextP[10][6] = P[10][6] + P[10][1]*SF[1] + P[10][3]*SF[0] + P[10][0]*SPP[0] - P[10][2]*SPP[1];
+    nextP[10][7] = P[10][7] + P[10][4]*dt;
+    nextP[10][8] = P[10][8] + P[10][5]*dt;
+    nextP[10][9] = P[10][9] + P[10][6]*dt;
+    nextP[10][10] = P[10][10];
+    nextP[10][11] = P[10][11];
+    nextP[10][12] = P[10][12];
+    nextP[10][13] = P[10][13];
+    nextP[10][14] = P[10][14];
+    nextP[10][15] = P[10][15];
+    nextP[10][16] = P[10][16];
+    nextP[10][17] = P[10][17];
+    nextP[10][18] = P[10][18];
+    nextP[10][19] = P[10][19];
+    nextP[10][20] = P[10][20];
+    nextP[11][0] = P[11][0] + P[11][1]*SF[6] + P[11][2]*SPP[7] + P[11][3]*SPP[6] + P[11][10]*SPP[5] + P[11][11]*SPP[4] + P[11][12]*SPP[3];
+    nextP[11][1] = P[11][1] + P[11][0]*SF[5] + P[11][2]*SF[4] + P[11][3]*SPP[7] + P[11][11]*SPP[3] - P[11][12]*SPP[4] - (P[11][10]*q0)/2;
+    nextP[11][2] = P[11][2] + P[11][0]*SF[3] + P[11][3]*SF[5] + P[11][1]*SPP[6] - P[11][10]*SPP[3] + P[11][12]*SPP[5] - (P[11][11]*q0)/2;
+    nextP[11][3] = P[11][3] + P[11][0]*SF[4] + P[11][1]*SF[3] + P[11][2]*SF[6] + P[11][10]*SPP[4] - P[11][11]*SPP[5] - (P[11][12]*q0)/2;
+    nextP[11][4] = P[11][4] + P[11][1]*SF[0] + P[11][0]*SF[2] + P[11][2]*SPP[0] - P[11][3]*SPP[2];
+    nextP[11][5] = P[11][5] + P[11][0]*SF[1] + P[11][2]*SF[0] + P[11][3]*SF[2] - P[11][1]*SPP[0];
+    nextP[11][6] = P[11][6] + P[11][1]*SF[1] + P[11][3]*SF[0] + P[11][0]*SPP[0] - P[11][2]*SPP[1];
+    nextP[11][7] = P[11][7] + P[11][4]*dt;
+    nextP[11][8] = P[11][8] + P[11][5]*dt;
+    nextP[11][9] = P[11][9] + P[11][6]*dt;
+    nextP[11][10] = P[11][10];
+    nextP[11][11] = P[11][11];
+    nextP[11][12] = P[11][12];
+    nextP[11][13] = P[11][13];
+    nextP[11][14] = P[11][14];
+    nextP[11][15] = P[11][15];
+    nextP[11][16] = P[11][16];
+    nextP[11][17] = P[11][17];
+    nextP[11][18] = P[11][18];
+    nextP[11][19] = P[11][19];
+    nextP[11][20] = P[11][20];
+    nextP[12][0] = P[12][0] + P[12][1]*SF[6] + P[12][2]*SPP[7] + P[12][3]*SPP[6] + P[12][10]*SPP[5] + P[12][11]*SPP[4] + P[12][12]*SPP[3];
+    nextP[12][1] = P[12][1] + P[12][0]*SF[5] + P[12][2]*SF[4] + P[12][3]*SPP[7] + P[12][11]*SPP[3] - P[12][12]*SPP[4] - (P[12][10]*q0)/2;
+    nextP[12][2] = P[12][2] + P[12][0]*SF[3] + P[12][3]*SF[5] + P[12][1]*SPP[6] - P[12][10]*SPP[3] + P[12][12]*SPP[5] - (P[12][11]*q0)/2;
+    nextP[12][3] = P[12][3] + P[12][0]*SF[4] + P[12][1]*SF[3] + P[12][2]*SF[6] + P[12][10]*SPP[4] - P[12][11]*SPP[5] - (P[12][12]*q0)/2;
+    nextP[12][4] = P[12][4] + P[12][1]*SF[0] + P[12][0]*SF[2] + P[12][2]*SPP[0] - P[12][3]*SPP[2];
+    nextP[12][5] = P[12][5] + P[12][0]*SF[1] + P[12][2]*SF[0] + P[12][3]*SF[2] - P[12][1]*SPP[0];
+    nextP[12][6] = P[12][6] + P[12][1]*SF[1] + P[12][3]*SF[0] + P[12][0]*SPP[0] - P[12][2]*SPP[1];
+    nextP[12][7] = P[12][7] + P[12][4]*dt;
+    nextP[12][8] = P[12][8] + P[12][5]*dt;
+    nextP[12][9] = P[12][9] + P[12][6]*dt;
+    nextP[12][10] = P[12][10];
+    nextP[12][11] = P[12][11];
+    nextP[12][12] = P[12][12];
+    nextP[12][13] = P[12][13];
+    nextP[12][14] = P[12][14];
+    nextP[12][15] = P[12][15];
+    nextP[12][16] = P[12][16];
+    nextP[12][17] = P[12][17];
+    nextP[12][18] = P[12][18];
+    nextP[12][19] = P[12][19];
+    nextP[12][20] = P[12][20];
+    nextP[13][0] = P[13][0] + P[13][1]*SF[6] + P[13][2]*SPP[7] + P[13][3]*SPP[6] + P[13][10]*SPP[5] + P[13][11]*SPP[4] + P[13][12]*SPP[3];
+    nextP[13][1] = P[13][1] + P[13][0]*SF[5] + P[13][2]*SF[4] + P[13][3]*SPP[7] + P[13][11]*SPP[3] - P[13][12]*SPP[4] - (P[13][10]*q0)/2;
+    nextP[13][2] = P[13][2] + P[13][0]*SF[3] + P[13][3]*SF[5] + P[13][1]*SPP[6] - P[13][10]*SPP[3] + P[13][12]*SPP[5] - (P[13][11]*q0)/2;
+    nextP[13][3] = P[13][3] + P[13][0]*SF[4] + P[13][1]*SF[3] + P[13][2]*SF[6] + P[13][10]*SPP[4] - P[13][11]*SPP[5] - (P[13][12]*q0)/2;
+    nextP[13][4] = P[13][4] + P[13][1]*SF[0] + P[13][0]*SF[2] + P[13][2]*SPP[0] - P[13][3]*SPP[2];
+    nextP[13][5] = P[13][5] + P[13][0]*SF[1] + P[13][2]*SF[0] + P[13][3]*SF[2] - P[13][1]*SPP[0];
+    nextP[13][6] = P[13][6] + P[13][1]*SF[1] + P[13][3]*SF[0] + P[13][0]*SPP[0] - P[13][2]*SPP[1];
+    nextP[13][7] = P[13][7] + P[13][4]*dt;
+    nextP[13][8] = P[13][8] + P[13][5]*dt;
+    nextP[13][9] = P[13][9] + P[13][6]*dt;
+    nextP[13][10] = P[13][10];
+    nextP[13][11] = P[13][11];
+    nextP[13][12] = P[13][12];
+    nextP[13][13] = P[13][13];
+    nextP[13][14] = P[13][14];
+    nextP[13][15] = P[13][15];
+    nextP[13][16] = P[13][16];
+    nextP[13][17] = P[13][17];
+    nextP[13][18] = P[13][18];
+    nextP[13][19] = P[13][19];
+    nextP[13][20] = P[13][20];
+    nextP[14][0] = P[14][0] + P[14][1]*SF[6] + P[14][2]*SPP[7] + P[14][3]*SPP[6] + P[14][10]*SPP[5] + P[14][11]*SPP[4] + P[14][12]*SPP[3];
+    nextP[14][1] = P[14][1] + P[14][0]*SF[5] + P[14][2]*SF[4] + P[14][3]*SPP[7] + P[14][11]*SPP[3] - P[14][12]*SPP[4] - (P[14][10]*q0)/2;
+    nextP[14][2] = P[14][2] + P[14][0]*SF[3] + P[14][3]*SF[5] + P[14][1]*SPP[6] - P[14][10]*SPP[3] + P[14][12]*SPP[5] - (P[14][11]*q0)/2;
+    nextP[14][3] = P[14][3] + P[14][0]*SF[4] + P[14][1]*SF[3] + P[14][2]*SF[6] + P[14][10]*SPP[4] - P[14][11]*SPP[5] - (P[14][12]*q0)/2;
+    nextP[14][4] = P[14][4] + P[14][1]*SF[0] + P[14][0]*SF[2] + P[14][2]*SPP[0] - P[14][3]*SPP[2];
+    nextP[14][5] = P[14][5] + P[14][0]*SF[1] + P[14][2]*SF[0] + P[14][3]*SF[2] - P[14][1]*SPP[0];
+    nextP[14][6] = P[14][6] + P[14][1]*SF[1] + P[14][3]*SF[0] + P[14][0]*SPP[0] - P[14][2]*SPP[1];
+    nextP[14][7] = P[14][7] + P[14][4]*dt;
+    nextP[14][8] = P[14][8] + P[14][5]*dt;
+    nextP[14][9] = P[14][9] + P[14][6]*dt;
+    nextP[14][10] = P[14][10];
+    nextP[14][11] = P[14][11];
+    nextP[14][12] = P[14][12];
+    nextP[14][13] = P[14][13];
+    nextP[14][14] = P[14][14];
+    nextP[14][15] = P[14][15];
+    nextP[14][16] = P[14][16];
+    nextP[14][17] = P[14][17];
+    nextP[14][18] = P[14][18];
+    nextP[14][19] = P[14][19];
+    nextP[14][20] = P[14][20];
+    nextP[15][0] = P[15][0] + P[15][1]*SF[6] + P[15][2]*SPP[7] + P[15][3]*SPP[6] + P[15][10]*SPP[5] + P[15][11]*SPP[4] + P[15][12]*SPP[3];
+    nextP[15][1] = P[15][1] + P[15][0]*SF[5] + P[15][2]*SF[4] + P[15][3]*SPP[7] + P[15][11]*SPP[3] - P[15][12]*SPP[4] - (P[15][10]*q0)/2;
+    nextP[15][2] = P[15][2] + P[15][0]*SF[3] + P[15][3]*SF[5] + P[15][1]*SPP[6] - P[15][10]*SPP[3] + P[15][12]*SPP[5] - (P[15][11]*q0)/2;
+    nextP[15][3] = P[15][3] + P[15][0]*SF[4] + P[15][1]*SF[3] + P[15][2]*SF[6] + P[15][10]*SPP[4] - P[15][11]*SPP[5] - (P[15][12]*q0)/2;
+    nextP[15][4] = P[15][4] + P[15][1]*SF[0] + P[15][0]*SF[2] + P[15][2]*SPP[0] - P[15][3]*SPP[2];
+    nextP[15][5] = P[15][5] + P[15][0]*SF[1] + P[15][2]*SF[0] + P[15][3]*SF[2] - P[15][1]*SPP[0];
+    nextP[15][6] = P[15][6] + P[15][1]*SF[1] + P[15][3]*SF[0] + P[15][0]*SPP[0] - P[15][2]*SPP[1];
+    nextP[15][7] = P[15][7] + P[15][4]*dt;
+    nextP[15][8] = P[15][8] + P[15][5]*dt;
+    nextP[15][9] = P[15][9] + P[15][6]*dt;
+    nextP[15][10] = P[15][10];
+    nextP[15][11] = P[15][11];
+    nextP[15][12] = P[15][12];
+    nextP[15][13] = P[15][13];
+    nextP[15][14] = P[15][14];
+    nextP[15][15] = P[15][15];
+    nextP[15][16] = P[15][16];
+    nextP[15][17] = P[15][17];
+    nextP[15][18] = P[15][18];
+    nextP[15][19] = P[15][19];
+    nextP[15][20] = P[15][20];
+    nextP[16][0] = P[16][0] + P[16][1]*SF[6] + P[16][2]*SPP[7] + P[16][3]*SPP[6] + P[16][10]*SPP[5] + P[16][11]*SPP[4] + P[16][12]*SPP[3];
+    nextP[16][1] = P[16][1] + P[16][0]*SF[5] + P[16][2]*SF[4] + P[16][3]*SPP[7] + P[16][11]*SPP[3] - P[16][12]*SPP[4] - (P[16][10]*q0)/2;
+    nextP[16][2] = P[16][2] + P[16][0]*SF[3] + P[16][3]*SF[5] + P[16][1]*SPP[6] - P[16][10]*SPP[3] + P[16][12]*SPP[5] - (P[16][11]*q0)/2;
+    nextP[16][3] = P[16][3] + P[16][0]*SF[4] + P[16][1]*SF[3] + P[16][2]*SF[6] + P[16][10]*SPP[4] - P[16][11]*SPP[5] - (P[16][12]*q0)/2;
+    nextP[16][4] = P[16][4] + P[16][1]*SF[0] + P[16][0]*SF[2] + P[16][2]*SPP[0] - P[16][3]*SPP[2];
+    nextP[16][5] = P[16][5] + P[16][0]*SF[1] + P[16][2]*SF[0] + P[16][3]*SF[2] - P[16][1]*SPP[0];
+    nextP[16][6] = P[16][6] + P[16][1]*SF[1] + P[16][3]*SF[0] + P[16][0]*SPP[0] - P[16][2]*SPP[1];
+    nextP[16][7] = P[16][7] + P[16][4]*dt;
+    nextP[16][8] = P[16][8] + P[16][5]*dt;
+    nextP[16][9] = P[16][9] + P[16][6]*dt;
+    nextP[16][10] = P[16][10];
+    nextP[16][11] = P[16][11];
+    nextP[16][12] = P[16][12];
+    nextP[16][13] = P[16][13];
+    nextP[16][14] = P[16][14];
+    nextP[16][15] = P[16][15];
+    nextP[16][16] = P[16][16];
+    nextP[16][17] = P[16][17];
+    nextP[16][18] = P[16][18];
+    nextP[16][19] = P[16][19];
+    nextP[16][20] = P[16][20];
+    nextP[17][0] = P[17][0] + P[17][1]*SF[6] + P[17][2]*SPP[7] + P[17][3]*SPP[6] + P[17][10]*SPP[5] + P[17][11]*SPP[4] + P[17][12]*SPP[3];
+    nextP[17][1] = P[17][1] + P[17][0]*SF[5] + P[17][2]*SF[4] + P[17][3]*SPP[7] + P[17][11]*SPP[3] - P[17][12]*SPP[4] - (P[17][10]*q0)/2;
+    nextP[17][2] = P[17][2] + P[17][0]*SF[3] + P[17][3]*SF[5] + P[17][1]*SPP[6] - P[17][10]*SPP[3] + P[17][12]*SPP[5] - (P[17][11]*q0)/2;
+    nextP[17][3] = P[17][3] + P[17][0]*SF[4] + P[17][1]*SF[3] + P[17][2]*SF[6] + P[17][10]*SPP[4] - P[17][11]*SPP[5] - (P[17][12]*q0)/2;
+    nextP[17][4] = P[17][4] + P[17][1]*SF[0] + P[17][0]*SF[2] + P[17][2]*SPP[0] - P[17][3]*SPP[2];
+    nextP[17][5] = P[17][5] + P[17][0]*SF[1] + P[17][2]*SF[0] + P[17][3]*SF[2] - P[17][1]*SPP[0];
+    nextP[17][6] = P[17][6] + P[17][1]*SF[1] + P[17][3]*SF[0] + P[17][0]*SPP[0] - P[17][2]*SPP[1];
+    nextP[17][7] = P[17][7] + P[17][4]*dt;
+    nextP[17][8] = P[17][8] + P[17][5]*dt;
+    nextP[17][9] = P[17][9] + P[17][6]*dt;
+    nextP[17][10] = P[17][10];
+    nextP[17][11] = P[17][11];
+    nextP[17][12] = P[17][12];
+    nextP[17][13] = P[17][13];
+    nextP[17][14] = P[17][14];
+    nextP[17][15] = P[17][15];
+    nextP[17][16] = P[17][16];
+    nextP[17][17] = P[17][17];
+    nextP[17][18] = P[17][18];
+    nextP[17][19] = P[17][19];
+    nextP[17][20] = P[17][20];
+    nextP[18][0] = P[18][0] + P[18][1]*SF[6] + P[18][2]*SPP[7] + P[18][3]*SPP[6] + P[18][10]*SPP[5] + P[18][11]*SPP[4] + P[18][12]*SPP[3];
+    nextP[18][1] = P[18][1] + P[18][0]*SF[5] + P[18][2]*SF[4] + P[18][3]*SPP[7] + P[18][11]*SPP[3] - P[18][12]*SPP[4] - (P[18][10]*q0)/2;
+    nextP[18][2] = P[18][2] + P[18][0]*SF[3] + P[18][3]*SF[5] + P[18][1]*SPP[6] - P[18][10]*SPP[3] + P[18][12]*SPP[5] - (P[18][11]*q0)/2;
+    nextP[18][3] = P[18][3] + P[18][0]*SF[4] + P[18][1]*SF[3] + P[18][2]*SF[6] + P[18][10]*SPP[4] - P[18][11]*SPP[5] - (P[18][12]*q0)/2;
+    nextP[18][4] = P[18][4] + P[18][1]*SF[0] + P[18][0]*SF[2] + P[18][2]*SPP[0] - P[18][3]*SPP[2];
+    nextP[18][5] = P[18][5] + P[18][0]*SF[1] + P[18][2]*SF[0] + P[18][3]*SF[2] - P[18][1]*SPP[0];
+    nextP[18][6] = P[18][6] + P[18][1]*SF[1] + P[18][3]*SF[0] + P[18][0]*SPP[0] - P[18][2]*SPP[1];
+    nextP[18][7] = P[18][7] + P[18][4]*dt;
+    nextP[18][8] = P[18][8] + P[18][5]*dt;
+    nextP[18][9] = P[18][9] + P[18][6]*dt;
+    nextP[18][10] = P[18][10];
+    nextP[18][11] = P[18][11];
+    nextP[18][12] = P[18][12];
+    nextP[18][13] = P[18][13];
+    nextP[18][14] = P[18][14];
+    nextP[18][15] = P[18][15];
+    nextP[18][16] = P[18][16];
+    nextP[18][17] = P[18][17];
+    nextP[18][18] = P[18][18];
+    nextP[18][19] = P[18][19];
+    nextP[18][20] = P[18][20];
+    nextP[19][0] = P[19][0] + P[19][1]*SF[6] + P[19][2]*SPP[7] + P[19][3]*SPP[6] + P[19][10]*SPP[5] + P[19][11]*SPP[4] + P[19][12]*SPP[3];
+    nextP[19][1] = P[19][1] + P[19][0]*SF[5] + P[19][2]*SF[4] + P[19][3]*SPP[7] + P[19][11]*SPP[3] - P[19][12]*SPP[4] - (P[19][10]*q0)/2;
+    nextP[19][2] = P[19][2] + P[19][0]*SF[3] + P[19][3]*SF[5] + P[19][1]*SPP[6] - P[19][10]*SPP[3] + P[19][12]*SPP[5] - (P[19][11]*q0)/2;
+    nextP[19][3] = P[19][3] + P[19][0]*SF[4] + P[19][1]*SF[3] + P[19][2]*SF[6] + P[19][10]*SPP[4] - P[19][11]*SPP[5] - (P[19][12]*q0)/2;
+    nextP[19][4] = P[19][4] + P[19][1]*SF[0] + P[19][0]*SF[2] + P[19][2]*SPP[0] - P[19][3]*SPP[2];
+    nextP[19][5] = P[19][5] + P[19][0]*SF[1] + P[19][2]*SF[0] + P[19][3]*SF[2] - P[19][1]*SPP[0];
+    nextP[19][6] = P[19][6] + P[19][1]*SF[1] + P[19][3]*SF[0] + P[19][0]*SPP[0] - P[19][2]*SPP[1];
+    nextP[19][7] = P[19][7] + P[19][4]*dt;
+    nextP[19][8] = P[19][8] + P[19][5]*dt;
+    nextP[19][9] = P[19][9] + P[19][6]*dt;
+    nextP[19][10] = P[19][10];
+    nextP[19][11] = P[19][11];
+    nextP[19][12] = P[19][12];
+    nextP[19][13] = P[19][13];
+    nextP[19][14] = P[19][14];
+    nextP[19][15] = P[19][15];
+    nextP[19][16] = P[19][16];
+    nextP[19][17] = P[19][17];
+    nextP[19][18] = P[19][18];
+    nextP[19][19] = P[19][19];
+    nextP[19][20] = P[19][20];
+    nextP[20][0] = P[20][0] + P[20][1]*SF[6] + P[20][2]*SPP[7] + P[20][3]*SPP[6] + P[20][10]*SPP[5] + P[20][11]*SPP[4] + P[20][12]*SPP[3];
+    nextP[20][1] = P[20][1] + P[20][0]*SF[5] + P[20][2]*SF[4] + P[20][3]*SPP[7] + P[20][11]*SPP[3] - P[20][12]*SPP[4] - (P[20][10]*q0)/2;
+    nextP[20][2] = P[20][2] + P[20][0]*SF[3] + P[20][3]*SF[5] + P[20][1]*SPP[6] - P[20][10]*SPP[3] + P[20][12]*SPP[5] - (P[20][11]*q0)/2;
+    nextP[20][3] = P[20][3] + P[20][0]*SF[4] + P[20][1]*SF[3] + P[20][2]*SF[6] + P[20][10]*SPP[4] - P[20][11]*SPP[5] - (P[20][12]*q0)/2;
+    nextP[20][4] = P[20][4] + P[20][1]*SF[0] + P[20][0]*SF[2] + P[20][2]*SPP[0] - P[20][3]*SPP[2];
+    nextP[20][5] = P[20][5] + P[20][0]*SF[1] + P[20][2]*SF[0] + P[20][3]*SF[2] - P[20][1]*SPP[0];
+    nextP[20][6] = P[20][6] + P[20][1]*SF[1] + P[20][3]*SF[0] + P[20][0]*SPP[0] - P[20][2]*SPP[1];
+    nextP[20][7] = P[20][7] + P[20][4]*dt;
+    nextP[20][8] = P[20][8] + P[20][5]*dt;
+    nextP[20][9] = P[20][9] + P[20][6]*dt;
+    nextP[20][10] = P[20][10];
+    nextP[20][11] = P[20][11];
+    nextP[20][12] = P[20][12];
+    nextP[20][13] = P[20][13];
+    nextP[20][14] = P[20][14];
+    nextP[20][15] = P[20][15];
+    nextP[20][16] = P[20][16];
+    nextP[20][17] = P[20][17];
+    nextP[20][18] = P[20][18];
+    nextP[20][19] = P[20][19];
+    nextP[20][20] = P[20][20];
+
+    for (unsigned i = 0; i < n_states; i++)
+    {
+        nextP[i][i] = nextP[i][i] + processNoise[i];
+    }
+
+    // If on ground or no magnetometer fitted, inhibit magnetometer bias updates by
+    // setting the coresponding covariance terms to zero.
+    if (onGround || !useCompass)
+    {
+        zeroRows(nextP,15,20);
+        zeroCols(nextP,15,20);
+    }
+
+    // If on ground or not using airspeed sensing, inhibit wind velocity
+    // covariance growth.
+    if (onGround || !useAirspeed)
+    {
+        zeroRows(nextP,13,14);
+        zeroCols(nextP,13,14);
+    }
+
+    // If the total position variance exceds 1E6 (1000m), then stop covariance
+    // growth by setting the predicted to the previous values
+    // This prevent an ill conditioned matrix from occurring for long periods
+    // without GPS
+    if ((P[7][7] + P[8][8]) > 1E6f)
+    {
+        for (uint8_t i=7; i<=8; i++)
+        {
+            for (unsigned j = 0; j < n_states; j++)
+            {
+                nextP[i][j] = P[i][j];
+                nextP[j][i] = P[j][i];
+            }
+        }
+    }
+
+    if (onGround || staticMode) {
+        // copy the portion of the variances we want to
+        // propagate
+        for (unsigned i = 0; i < 14; i++) {
+            P[i][i] = nextP[i][i];
+
+            // force symmetry for observable states
+            // force zero for non-observable states
+            for (unsigned i = 1; i < n_states; i++)
+            {
+                for (uint8_t j = 0; j < i; j++)
+                {
+                    if ((i > 12) || (j > 12)) {
+                        P[i][j] = 0.0f;
+                    } else {
+                        P[i][j] = 0.5f * (nextP[i][j] + nextP[j][i]);
+                    }
+                    P[j][i] = P[i][j];
+                }
+            }
+        }
+
+    } else {
+
+        // Copy covariance
+        for (unsigned i = 0; i < n_states; i++) {
+            P[i][i] = nextP[i][i];
+        }
+
+        // force symmetry for observable states
+        for (unsigned i = 1; i < n_states; i++)
+        {
+            for (uint8_t j = 0; j < i; j++)
+            {
+                P[i][j] = 0.5f * (nextP[i][j] + nextP[j][i]);
+                P[j][i] = P[i][j];
+            }
+        }
+
+    }
+
+    ConstrainVariances();
+}
+
+void AttPosEKF::FuseVelposNED()
+{
+
+// declare variables used by fault isolation logic
+    uint32_t gpsRetryTime = 30000; // time in msec before GPS fusion will be retried following innovation consistency failure
+    uint32_t gpsRetryTimeNoTAS = 5000; // retry time if no TAS measurement available
+    uint32_t hgtRetryTime = 5000; // height measurement retry time
+    uint32_t horizRetryTime;
+
+// declare variables used to check measurement errors
+    float velInnov[3] = {0.0f,0.0f,0.0f};
+    float posInnov[2] = {0.0f,0.0f};
+    float hgtInnov = 0.0f;
+
+// declare variables used to control access to arrays
+    bool fuseData[6] = {false,false,false,false,false,false};
+    uint8_t stateIndex;
+    uint8_t obsIndex;
+    uint8_t indexLimit;
+
+// declare variables used by state and covariance update calculations
+    float velErr;
+    float posErr;
+    float R_OBS[6];
+    float observation[6];
+    float SK;
+    float quatMag;
+
+// Perform sequential fusion of GPS measurements. This assumes that the
+// errors in the different velocity and position components are
+// uncorrelated which is not true, however in the absence of covariance
+// data from the GPS receiver it is the only assumption we can make
+// so we might as well take advantage of the computational efficiencies
+// associated with sequential fusion
+    if (fuseVelData || fusePosData || fuseHgtData)
+    {
+        // set the GPS data timeout depending on whether airspeed data is present
+        if (useAirspeed) horizRetryTime = gpsRetryTime;
+        else horizRetryTime = gpsRetryTimeNoTAS;
+
+        // Form the observation vector
+        for (uint8_t i=0; i<=2; i++) observation[i] = velNED[i];
+        for (uint8_t i=3; i<=4; i++) observation[i] = posNE[i-3];
+        observation[5] = -(hgtMea);
+
+        // Estimate the GPS Velocity, GPS horiz position and height measurement variances.
+        velErr = 0.2f*accNavMag; // additional error in GPS velocities caused by manoeuvring
+        posErr = 0.2f*accNavMag; // additional error in GPS position caused by manoeuvring
+        R_OBS[0] = 0.04f + sq(velErr);
+        R_OBS[1] = R_OBS[0];
+        R_OBS[2] = 0.08f + sq(velErr);
+        R_OBS[3] = R_OBS[2];
+        R_OBS[4] = 4.0f  + sq(posErr);
+        R_OBS[5] = 4.0f;
+
+        // Set innovation variances to zero default
+        for (uint8_t i = 0; i<=5; i++)
+        {
+            varInnovVelPos[i] = 0.0f;
+        }
+        // calculate innovations and check GPS data validity using an innovation consistency check
+        if (fuseVelData)
+        {
+            // test velocity measurements
+            uint8_t imax = 2;
+            if (fusionModeGPS == 1) imax = 1;
+            for (uint8_t i = 0; i<=imax; i++)
+            {
+                velInnov[i] = statesAtVelTime[i+4] - velNED[i];
+                stateIndex = 4 + i;
+                varInnovVelPos[i] = P[stateIndex][stateIndex] + R_OBS[i];
+            }
+            // apply a 5-sigma threshold
+            current_ekf_state.velHealth = (sq(velInnov[0]) + sq(velInnov[1]) + sq(velInnov[2])) < 25.0f * (varInnovVelPos[0] + varInnovVelPos[1] + varInnovVelPos[2]);
+            current_ekf_state.velTimeout = (millis() - current_ekf_state.velFailTime) > horizRetryTime;
+            if (current_ekf_state.velHealth || current_ekf_state.velTimeout)
+            {
+                current_ekf_state.velHealth = true;
+                current_ekf_state.velFailTime = millis();
+            }
+            else
+            {
+                current_ekf_state.velHealth = false;
+            }
+        }
+        if (fusePosData)
+        {
+            // test horizontal position measurements
+            posInnov[0] = statesAtPosTime[7] - posNE[0];
+            posInnov[1] = statesAtPosTime[8] - posNE[1];
+            varInnovVelPos[3] = P[7][7] + R_OBS[3];
+            varInnovVelPos[4] = P[8][8] + R_OBS[4];
+            // apply a 10-sigma threshold
+            current_ekf_state.posHealth = (sq(posInnov[0]) + sq(posInnov[1])) < 100.0f*(varInnovVelPos[3] + varInnovVelPos[4]);
+            current_ekf_state.posTimeout = (millis() - current_ekf_state.posFailTime) > horizRetryTime;
+            if (current_ekf_state.posHealth || current_ekf_state.posTimeout)
+            {
+                current_ekf_state.posHealth = true;
+                current_ekf_state.posFailTime = millis();
+            }
+            else
+            {
+                current_ekf_state.posHealth = false;
+            }
+        }
+        // test height measurements
+        if (fuseHgtData)
+        {
+            hgtInnov = statesAtHgtTime[9] + hgtMea;
+            varInnovVelPos[5] = P[9][9] + R_OBS[5];
+            // apply a 10-sigma threshold
+            current_ekf_state.hgtHealth = sq(hgtInnov) < 100.0f*varInnovVelPos[5];
+            current_ekf_state.hgtTimeout = (millis() - current_ekf_state.hgtFailTime) > hgtRetryTime;
+            if (current_ekf_state.hgtHealth || current_ekf_state.hgtTimeout)
+            {
+                current_ekf_state.hgtHealth = true;
+                current_ekf_state.hgtFailTime = millis();
+            }
+            else
+            {
+                current_ekf_state.hgtHealth = false;
+            }
+        }
+        // Set range for sequential fusion of velocity and position measurements depending
+        // on which data is available and its health
+        if (fuseVelData && fusionModeGPS == 0 && current_ekf_state.velHealth)
+        {
+            fuseData[0] = true;
+            fuseData[1] = true;
+            fuseData[2] = true;
+        }
+        if (fuseVelData && fusionModeGPS == 1 && current_ekf_state.velHealth)
+        {
+            fuseData[0] = true;
+            fuseData[1] = true;
+        }
+        if (fusePosData && fusionModeGPS <= 2 && current_ekf_state.posHealth)
+        {
+            fuseData[3] = true;
+            fuseData[4] = true;
+        }
+        if (fuseHgtData && current_ekf_state.hgtHealth)
+        {
+            fuseData[5] = true;
+        }
+        // Limit range of states modified when on ground
+        if(!onGround)
+        {
+            indexLimit = 20;
+        }
+        else
+        {
+            indexLimit = 12;
+        }
+        // Fuse measurements sequentially
+        for (obsIndex=0; obsIndex<=5; obsIndex++)
+        {
+            if (fuseData[obsIndex])
+            {
+                stateIndex = 4 + obsIndex;
+                // Calculate the measurement innovation, using states from a
+                // different time coordinate if fusing height data
+                if (obsIndex >= 0 && obsIndex <= 2)
+                {
+                    innovVelPos[obsIndex] = statesAtVelTime[stateIndex] - observation[obsIndex];
+                }
+                else if (obsIndex == 3 || obsIndex == 4)
+                {
+                    innovVelPos[obsIndex] = statesAtPosTime[stateIndex] - observation[obsIndex];
+                }
+                else if (obsIndex == 5)
+                {
+                    innovVelPos[obsIndex] = statesAtHgtTime[stateIndex] - observation[obsIndex];
+                }
+                // Calculate the Kalman Gain
+                // Calculate innovation variances - also used for data logging
+                varInnovVelPos[obsIndex] = P[stateIndex][stateIndex] + R_OBS[obsIndex];
+                SK = 1.0/varInnovVelPos[obsIndex];
+                for (uint8_t i= 0; i<=indexLimit; i++)
+                {
+                    Kfusion[i] = P[i][stateIndex]*SK;
+                }
+                // Calculate state corrections and re-normalise the quaternions
+                for (uint8_t i = 0; i<=indexLimit; i++)
+                {
+                    states[i] = states[i] - Kfusion[i] * innovVelPos[obsIndex];
+                }
+                quatMag = sqrt(states[0]*states[0] + states[1]*states[1] + states[2]*states[2] + states[3]*states[3]);
+                if (quatMag > 1e-12f) // divide by  0 protection
+                {
+                    for (uint8_t i = 0; i<=3; i++)
+                    {
+                        states[i] = states[i] / quatMag;
+                    }
+                }
+                // Update the covariance - take advantage of direct observation of a
+                // single state at index = stateIndex to reduce computations
+                // Optimised implementation of standard equation P = (I - K*H)*P;
+                for (uint8_t i= 0; i<=indexLimit; i++)
+                {
+                    for (uint8_t j= 0; j<=indexLimit; j++)
+                    {
+                        KHP[i][j] = Kfusion[i] * P[stateIndex][j];
+                    }
+                }
+                for (uint8_t i= 0; i<=indexLimit; i++)
+                {
+                    for (uint8_t j= 0; j<=indexLimit; j++)
+                    {
+                        P[i][j] = P[i][j] - KHP[i][j];
+                    }
+                }
+            }
+        }
+    }
+
+    ForceSymmetry();
+    ConstrainVariances();
+
+    //printf("velh: %s, posh: %s, hgth: %s\n", ((velHealth) ? "OK" : "FAIL"), ((posHealth) ? "OK" : "FAIL"), ((hgtHealth) ? "OK" : "FAIL"));
+}
+
+void AttPosEKF::FuseMagnetometer()
+{
+    uint8_t obsIndex;
+    uint8_t indexLimit;
+    float DCM[3][3] =
+    {
+        {1.0f,0.0f,0.0f} ,
+        {0.0f,1.0f,0.0f} ,
+        {0.0f,0.0f,1.0f}
+    };
+    float MagPred[3] = {0.0f,0.0f,0.0f};
+    float SK_MX[6];
+    float SK_MY[5];
+    float SK_MZ[6];
+    float SH_MAG[9] = {0.0f,0.0f,0.0f,0.0f,0.0f,0.0f,0.0f,0.0f,0.0f};
+
+// Perform sequential fusion of Magnetometer measurements.
+// This assumes that the errors in the different components are
+// uncorrelated which is not true, however in the absence of covariance
+// data fit is the only assumption we can make
+// so we might as well take advantage of the computational efficiencies
+// associated with sequential fusion
+    if (useCompass && (fuseMagData || obsIndex == 1 || obsIndex == 2))
+    {
+        // Limit range of states modified when on ground
+        if(!onGround)
+        {
+            indexLimit = 20;
+        }
+        else
+        {
+            indexLimit = 12;
+        }
+
+        static float q0 = 0.0f;
+        static float q1 = 0.0f;
+        static float q2 = 0.0f;
+        static float q3 = 1.0f;
+        static float magN = 0.4f;
+        static float magE = 0.0f;
+        static float magD = 0.3f;
+
+        static float R_MAG = 0.0025f;
+
+        float H_MAG[21] = {0.0f,0.0f,0.0f,0.0f,0.0f,0.0f,0.0f,0.0f,0.0f,0.0f,0.0f,0.0f,0.0f,0.0f,0.0f,0.0f,0.0f,0.0f,0.0f,0.0f,0.0f};
+
+        // Sequential fusion of XYZ components to spread processing load across
+        // three prediction time steps.
+
+        // Calculate observation jacobians and Kalman gains
+        if (fuseMagData)
+        {
+            static float magXbias = 0.0f;
+            static float magYbias = 0.0f;
+            static float magZbias = 0.0f;
+
+            // Copy required states to local variable names
+            q0       = statesAtMagMeasTime[0];
+            q1       = statesAtMagMeasTime[1];
+            q2       = statesAtMagMeasTime[2];
+            q3       = statesAtMagMeasTime[3];
+            magN     = statesAtMagMeasTime[15];
+            magE     = statesAtMagMeasTime[16];
+            magD     = statesAtMagMeasTime[17];
+            magXbias = statesAtMagMeasTime[18];
+            magYbias = statesAtMagMeasTime[19];
+            magZbias = statesAtMagMeasTime[20];
+
+            // rotate predicted earth components into body axes and calculate
+            // predicted measurments
+            DCM[0][0] = q0*q0 + q1*q1 - q2*q2 - q3*q3;
+            DCM[0][1] = 2*(q1*q2 + q0*q3);
+            DCM[0][2] = 2*(q1*q3-q0*q2);
+            DCM[1][0] = 2*(q1*q2 - q0*q3);
+            DCM[1][1] = q0*q0 - q1*q1 + q2*q2 - q3*q3;
+            DCM[1][2] = 2*(q2*q3 + q0*q1);
+            DCM[2][0] = 2*(q1*q3 + q0*q2);
+            DCM[2][1] = 2*(q2*q3 - q0*q1);
+            DCM[2][2] = q0*q0 - q1*q1 - q2*q2 + q3*q3;
+            MagPred[0] = DCM[0][0]*magN + DCM[0][1]*magE  + DCM[0][2]*magD + magXbias;
+            MagPred[1] = DCM[1][0]*magN + DCM[1][1]*magE  + DCM[1][2]*magD + magYbias;
+            MagPred[2] = DCM[2][0]*magN + DCM[2][1]*magE  + DCM[2][2]*magD + magZbias;
+
+            // scale magnetometer observation error with total angular rate
+            R_MAG = 0.0025f + sq(0.05f*dAngIMU.length()/dtIMU);
+
+            // Calculate observation jacobians
+            SH_MAG[0] = 2*magD*q3 + 2*magE*q2 + 2*magN*q1;
+            SH_MAG[1] = 2*magD*q0 - 2*magE*q1 + 2*magN*q2;
+            SH_MAG[2] = 2*magD*q1 + 2*magE*q0 - 2*magN*q3;
+            SH_MAG[3] = sq(q3);
+            SH_MAG[4] = sq(q2);
+            SH_MAG[5] = sq(q1);
+            SH_MAG[6] = sq(q0);
+            SH_MAG[7] = 2*magN*q0;
+            SH_MAG[8] = 2*magE*q3;
+
+            for (uint8_t i=0; i<=20; i++) H_MAG[i] = 0;
+            H_MAG[0] = SH_MAG[7] + SH_MAG[8] - 2*magD*q2;
+            H_MAG[1] = SH_MAG[0];
+            H_MAG[2] = 2*magE*q1 - 2*magD*q0 - 2*magN*q2;
+            H_MAG[3] = SH_MAG[2];
+            H_MAG[15] = SH_MAG[5] - SH_MAG[4] - SH_MAG[3] + SH_MAG[6];
+            H_MAG[16] = 2*q0*q3 + 2*q1*q2;
+            H_MAG[17] = 2*q1*q3 - 2*q0*q2;
+            H_MAG[18] = 1.0f;
+
+            // Calculate Kalman gain
+            SK_MX[0] = 1/(P[18][18] + R_MAG + P[1][18]*SH_MAG[0] + P[3][18]*SH_MAG[2] - P[15][18]*(SH_MAG[3] + SH_MAG[4] - SH_MAG[5] - SH_MAG[6]) - (2*magD*q0 - 2*magE*q1 + 2*magN*q2)*(P[18][2] + P[1][2]*SH_MAG[0] + P[3][2]*SH_MAG[2] - P[15][2]*(SH_MAG[3] + SH_MAG[4] - SH_MAG[5] - SH_MAG[6]) + P[16][2]*(2*q0*q3 + 2*q1*q2) - P[17][2]*(2*q0*q2 - 2*q1*q3) - P[2][2]*(2*magD*q0 - 2*magE*q1 + 2*magN*q2) + P[0][2]*(SH_MAG[7] + SH_MAG[8] - 2*magD*q2)) + (SH_MAG[7] + SH_MAG[8] - 2*magD*q2)*(P[18][0] + P[1][0]*SH_MAG[0] + P[3][0]*SH_MAG[2] - P[15][0]*(SH_MAG[3] + SH_MAG[4] - SH_MAG[5] - SH_MAG[6]) + P[16][0]*(2*q0*q3 + 2*q1*q2) - P[17][0]*(2*q0*q2 - 2*q1*q3) - P[2][0]*(2*magD*q0 - 2*magE*q1 + 2*magN*q2) + P[0][0]*(SH_MAG[7] + SH_MAG[8] - 2*magD*q2)) + SH_MAG[0]*(P[18][1] + P[1][1]*SH_MAG[0] + P[3][1]*SH_MAG[2] - P[15][1]*(SH_MAG[3] + SH_MAG[4] - SH_MAG[5] - SH_MAG[6]) + P[16][1]*(2*q0*q3 + 2*q1*q2) - P[17][1]*(2*q0*q2 - 2*q1*q3) - P[2][1]*(2*magD*q0 - 2*magE*q1 + 2*magN*q2) + P[0][1]*(SH_MAG[7] + SH_MAG[8] - 2*magD*q2)) + SH_MAG[2]*(P[18][3] + P[1][3]*SH_MAG[0] + P[3][3]*SH_MAG[2] - P[15][3]*(SH_MAG[3] + SH_MAG[4] - SH_MAG[5] - SH_MAG[6]) + P[16][3]*(2*q0*q3 + 2*q1*q2) - P[17][3]*(2*q0*q2 - 2*q1*q3) - P[2][3]*(2*magD*q0 - 2*magE*q1 + 2*magN*q2) + P[0][3]*(SH_MAG[7] + SH_MAG[8] - 2*magD*q2)) - (SH_MAG[3] + SH_MAG[4] - SH_MAG[5] - SH_MAG[6])*(P[18][15] + P[1][15]*SH_MAG[0] + P[3][15]*SH_MAG[2] - P[15][15]*(SH_MAG[3] + SH_MAG[4] - SH_MAG[5] - SH_MAG[6]) + P[16][15]*(2*q0*q3 + 2*q1*q2) - P[17][15]*(2*q0*q2 - 2*q1*q3) - P[2][15]*(2*magD*q0 - 2*magE*q1 + 2*magN*q2) + P[0][15]*(SH_MAG[7] + SH_MAG[8] - 2*magD*q2)) + P[16][18]*(2*q0*q3 + 2*q1*q2) - P[17][18]*(2*q0*q2 - 2*q1*q3) - P[2][18]*(2*magD*q0 - 2*magE*q1 + 2*magN*q2) + (2*q0*q3 + 2*q1*q2)*(P[18][16] + P[1][16]*SH_MAG[0] + P[3][16]*SH_MAG[2] - P[15][16]*(SH_MAG[3] + SH_MAG[4] - SH_MAG[5] - SH_MAG[6]) + P[16][16]*(2*q0*q3 + 2*q1*q2) - P[17][16]*(2*q0*q2 - 2*q1*q3) - P[2][16]*(2*magD*q0 - 2*magE*q1 + 2*magN*q2) + P[0][16]*(SH_MAG[7] + SH_MAG[8] - 2*magD*q2)) - (2*q0*q2 - 2*q1*q3)*(P[18][17] + P[1][17]*SH_MAG[0] + P[3][17]*SH_MAG[2] - P[15][17]*(SH_MAG[3] + SH_MAG[4] - SH_MAG[5] - SH_MAG[6]) + P[16][17]*(2*q0*q3 + 2*q1*q2) - P[17][17]*(2*q0*q2 - 2*q1*q3) - P[2][17]*(2*magD*q0 - 2*magE*q1 + 2*magN*q2) + P[0][17]*(SH_MAG[7] + SH_MAG[8] - 2*magD*q2)) + P[0][18]*(SH_MAG[7] + SH_MAG[8] - 2*magD*q2));
+            SK_MX[1] = SH_MAG[3] + SH_MAG[4] - SH_MAG[5] - SH_MAG[6];
+            SK_MX[2] = 2*magD*q0 - 2*magE*q1 + 2*magN*q2;
+            SK_MX[3] = SH_MAG[7] + SH_MAG[8] - 2*magD*q2;
+            SK_MX[4] = 2*q0*q2 - 2*q1*q3;
+            SK_MX[5] = 2*q0*q3 + 2*q1*q2;
+            Kfusion[0] = SK_MX[0]*(P[0][18] + P[0][1]*SH_MAG[0] + P[0][3]*SH_MAG[2] + P[0][0]*SK_MX[3] - P[0][2]*SK_MX[2] - P[0][15]*SK_MX[1] + P[0][16]*SK_MX[5] - P[0][17]*SK_MX[4]);
+            Kfusion[1] = SK_MX[0]*(P[1][18] + P[1][1]*SH_MAG[0] + P[1][3]*SH_MAG[2] + P[1][0]*SK_MX[3] - P[1][2]*SK_MX[2] - P[1][15]*SK_MX[1] + P[1][16]*SK_MX[5] - P[1][17]*SK_MX[4]);
+            Kfusion[2] = SK_MX[0]*(P[2][18] + P[2][1]*SH_MAG[0] + P[2][3]*SH_MAG[2] + P[2][0]*SK_MX[3] - P[2][2]*SK_MX[2] - P[2][15]*SK_MX[1] + P[2][16]*SK_MX[5] - P[2][17]*SK_MX[4]);
+            Kfusion[3] = SK_MX[0]*(P[3][18] + P[3][1]*SH_MAG[0] + P[3][3]*SH_MAG[2] + P[3][0]*SK_MX[3] - P[3][2]*SK_MX[2] - P[3][15]*SK_MX[1] + P[3][16]*SK_MX[5] - P[3][17]*SK_MX[4]);
+            Kfusion[4] = SK_MX[0]*(P[4][18] + P[4][1]*SH_MAG[0] + P[4][3]*SH_MAG[2] + P[4][0]*SK_MX[3] - P[4][2]*SK_MX[2] - P[4][15]*SK_MX[1] + P[4][16]*SK_MX[5] - P[4][17]*SK_MX[4]);
+            Kfusion[5] = SK_MX[0]*(P[5][18] + P[5][1]*SH_MAG[0] + P[5][3]*SH_MAG[2] + P[5][0]*SK_MX[3] - P[5][2]*SK_MX[2] - P[5][15]*SK_MX[1] + P[5][16]*SK_MX[5] - P[5][17]*SK_MX[4]);
+            Kfusion[6] = SK_MX[0]*(P[6][18] + P[6][1]*SH_MAG[0] + P[6][3]*SH_MAG[2] + P[6][0]*SK_MX[3] - P[6][2]*SK_MX[2] - P[6][15]*SK_MX[1] + P[6][16]*SK_MX[5] - P[6][17]*SK_MX[4]);
+            Kfusion[7] = SK_MX[0]*(P[7][18] + P[7][1]*SH_MAG[0] + P[7][3]*SH_MAG[2] + P[7][0]*SK_MX[3] - P[7][2]*SK_MX[2] - P[7][15]*SK_MX[1] + P[7][16]*SK_MX[5] - P[7][17]*SK_MX[4]);
+            Kfusion[8] = SK_MX[0]*(P[8][18] + P[8][1]*SH_MAG[0] + P[8][3]*SH_MAG[2] + P[8][0]*SK_MX[3] - P[8][2]*SK_MX[2] - P[8][15]*SK_MX[1] + P[8][16]*SK_MX[5] - P[8][17]*SK_MX[4]);
+            Kfusion[9] = SK_MX[0]*(P[9][18] + P[9][1]*SH_MAG[0] + P[9][3]*SH_MAG[2] + P[9][0]*SK_MX[3] - P[9][2]*SK_MX[2] - P[9][15]*SK_MX[1] + P[9][16]*SK_MX[5] - P[9][17]*SK_MX[4]);
+            Kfusion[10] = SK_MX[0]*(P[10][18] + P[10][1]*SH_MAG[0] + P[10][3]*SH_MAG[2] + P[10][0]*SK_MX[3] - P[10][2]*SK_MX[2] - P[10][15]*SK_MX[1] + P[10][16]*SK_MX[5] - P[10][17]*SK_MX[4]);
+            Kfusion[11] = SK_MX[0]*(P[11][18] + P[11][1]*SH_MAG[0] + P[11][3]*SH_MAG[2] + P[11][0]*SK_MX[3] - P[11][2]*SK_MX[2] - P[11][15]*SK_MX[1] + P[11][16]*SK_MX[5] - P[11][17]*SK_MX[4]);
+            Kfusion[12] = SK_MX[0]*(P[12][18] + P[12][1]*SH_MAG[0] + P[12][3]*SH_MAG[2] + P[12][0]*SK_MX[3] - P[12][2]*SK_MX[2] - P[12][15]*SK_MX[1] + P[12][16]*SK_MX[5] - P[12][17]*SK_MX[4]);
+            Kfusion[13] = SK_MX[0]*(P[13][18] + P[13][1]*SH_MAG[0] + P[13][3]*SH_MAG[2] + P[13][0]*SK_MX[3] - P[13][2]*SK_MX[2] - P[13][15]*SK_MX[1] + P[13][16]*SK_MX[5] - P[13][17]*SK_MX[4]);
+            Kfusion[14] = SK_MX[0]*(P[14][18] + P[14][1]*SH_MAG[0] + P[14][3]*SH_MAG[2] + P[14][0]*SK_MX[3] - P[14][2]*SK_MX[2] - P[14][15]*SK_MX[1] + P[14][16]*SK_MX[5] - P[14][17]*SK_MX[4]);
+            Kfusion[15] = SK_MX[0]*(P[15][18] + P[15][1]*SH_MAG[0] + P[15][3]*SH_MAG[2] + P[15][0]*SK_MX[3] - P[15][2]*SK_MX[2] - P[15][15]*SK_MX[1] + P[15][16]*SK_MX[5] - P[15][17]*SK_MX[4]);
+            Kfusion[16] = SK_MX[0]*(P[16][18] + P[16][1]*SH_MAG[0] + P[16][3]*SH_MAG[2] + P[16][0]*SK_MX[3] - P[16][2]*SK_MX[2] - P[16][15]*SK_MX[1] + P[16][16]*SK_MX[5] - P[16][17]*SK_MX[4]);
+            Kfusion[17] = SK_MX[0]*(P[17][18] + P[17][1]*SH_MAG[0] + P[17][3]*SH_MAG[2] + P[17][0]*SK_MX[3] - P[17][2]*SK_MX[2] - P[17][15]*SK_MX[1] + P[17][16]*SK_MX[5] - P[17][17]*SK_MX[4]);
+            Kfusion[18] = SK_MX[0]*(P[18][18] + P[18][1]*SH_MAG[0] + P[18][3]*SH_MAG[2] + P[18][0]*SK_MX[3] - P[18][2]*SK_MX[2] - P[18][15]*SK_MX[1] + P[18][16]*SK_MX[5] - P[18][17]*SK_MX[4]);
+            Kfusion[19] = SK_MX[0]*(P[19][18] + P[19][1]*SH_MAG[0] + P[19][3]*SH_MAG[2] + P[19][0]*SK_MX[3] - P[19][2]*SK_MX[2] - P[19][15]*SK_MX[1] + P[19][16]*SK_MX[5] - P[19][17]*SK_MX[4]);
+            Kfusion[20] = SK_MX[0]*(P[20][18] + P[20][1]*SH_MAG[0] + P[20][3]*SH_MAG[2] + P[20][0]*SK_MX[3] - P[20][2]*SK_MX[2] - P[20][15]*SK_MX[1] + P[20][16]*SK_MX[5] - P[20][17]*SK_MX[4]);
+            varInnovMag[0] = 1.0f/SK_MX[0];
+            innovMag[0] = MagPred[0] - magData.x;
+
+            // reset the observation index to 0 (we start by fusing the X
+            // measurement)
+            obsIndex = 0;
+        }
+        else if (obsIndex == 1) // we are now fusing the Y measurement
+        {
+            // Calculate observation jacobians
+            for (unsigned int i=0; i<n_states; i++) H_MAG[i] = 0;
+            H_MAG[0] = SH_MAG[2];
+            H_MAG[1] = SH_MAG[1];
+            H_MAG[2] = SH_MAG[0];
+            H_MAG[3] = 2*magD*q2 - SH_MAG[8] - SH_MAG[7];
+            H_MAG[15] = 2*q1*q2 - 2*q0*q3;
+            H_MAG[16] = SH_MAG[4] - SH_MAG[3] - SH_MAG[5] + SH_MAG[6];
+            H_MAG[17] = 2*q0*q1 + 2*q2*q3;
+            H_MAG[19] = 1;
+
+            // Calculate Kalman gain
+            SK_MY[0] = 1/(P[19][19] + R_MAG + P[0][19]*SH_MAG[2] + P[1][19]*SH_MAG[1] + P[2][19]*SH_MAG[0] - P[16][19]*(SH_MAG[3] - SH_MAG[4] + SH_MAG[5] - SH_MAG[6]) - (2*q0*q3 - 2*q1*q2)*(P[19][15] + P[0][15]*SH_MAG[2] + P[1][15]*SH_MAG[1] + P[2][15]*SH_MAG[0] - P[16][15]*(SH_MAG[3] - SH_MAG[4] + SH_MAG[5] - SH_MAG[6]) - P[15][15]*(2*q0*q3 - 2*q1*q2) + P[17][15]*(2*q0*q1 + 2*q2*q3) - P[3][15]*(SH_MAG[7] + SH_MAG[8] - 2*magD*q2)) + (2*q0*q1 + 2*q2*q3)*(P[19][17] + P[0][17]*SH_MAG[2] + P[1][17]*SH_MAG[1] + P[2][17]*SH_MAG[0] - P[16][17]*(SH_MAG[3] - SH_MAG[4] + SH_MAG[5] - SH_MAG[6]) - P[15][17]*(2*q0*q3 - 2*q1*q2) + P[17][17]*(2*q0*q1 + 2*q2*q3) - P[3][17]*(SH_MAG[7] + SH_MAG[8] - 2*magD*q2)) - (SH_MAG[7] + SH_MAG[8] - 2*magD*q2)*(P[19][3] + P[0][3]*SH_MAG[2] + P[1][3]*SH_MAG[1] + P[2][3]*SH_MAG[0] - P[16][3]*(SH_MAG[3] - SH_MAG[4] + SH_MAG[5] - SH_MAG[6]) - P[15][3]*(2*q0*q3 - 2*q1*q2) + P[17][3]*(2*q0*q1 + 2*q2*q3) - P[3][3]*(SH_MAG[7] + SH_MAG[8] - 2*magD*q2)) - P[15][19]*(2*q0*q3 - 2*q1*q2) + P[17][19]*(2*q0*q1 + 2*q2*q3) + SH_MAG[2]*(P[19][0] + P[0][0]*SH_MAG[2] + P[1][0]*SH_MAG[1] + P[2][0]*SH_MAG[0] - P[16][0]*(SH_MAG[3] - SH_MAG[4] + SH_MAG[5] - SH_MAG[6]) - P[15][0]*(2*q0*q3 - 2*q1*q2) + P[17][0]*(2*q0*q1 + 2*q2*q3) - P[3][0]*(SH_MAG[7] + SH_MAG[8] - 2*magD*q2)) + SH_MAG[1]*(P[19][1] + P[0][1]*SH_MAG[2] + P[1][1]*SH_MAG[1] + P[2][1]*SH_MAG[0] - P[16][1]*(SH_MAG[3] - SH_MAG[4] + SH_MAG[5] - SH_MAG[6]) - P[15][1]*(2*q0*q3 - 2*q1*q2) + P[17][1]*(2*q0*q1 + 2*q2*q3) - P[3][1]*(SH_MAG[7] + SH_MAG[8] - 2*magD*q2)) + SH_MAG[0]*(P[19][2] + P[0][2]*SH_MAG[2] + P[1][2]*SH_MAG[1] + P[2][2]*SH_MAG[0] - P[16][2]*(SH_MAG[3] - SH_MAG[4] + SH_MAG[5] - SH_MAG[6]) - P[15][2]*(2*q0*q3 - 2*q1*q2) + P[17][2]*(2*q0*q1 + 2*q2*q3) - P[3][2]*(SH_MAG[7] + SH_MAG[8] - 2*magD*q2)) - (SH_MAG[3] - SH_MAG[4] + SH_MAG[5] - SH_MAG[6])*(P[19][16] + P[0][16]*SH_MAG[2] + P[1][16]*SH_MAG[1] + P[2][16]*SH_MAG[0] - P[16][16]*(SH_MAG[3] - SH_MAG[4] + SH_MAG[5] - SH_MAG[6]) - P[15][16]*(2*q0*q3 - 2*q1*q2) + P[17][16]*(2*q0*q1 + 2*q2*q3) - P[3][16]*(SH_MAG[7] + SH_MAG[8] - 2*magD*q2)) - P[3][19]*(SH_MAG[7] + SH_MAG[8] - 2*magD*q2));
+            SK_MY[1] = SH_MAG[3] - SH_MAG[4] + SH_MAG[5] - SH_MAG[6];
+            SK_MY[2] = SH_MAG[7] + SH_MAG[8] - 2*magD*q2;
+            SK_MY[3] = 2*q0*q3 - 2*q1*q2;
+            SK_MY[4] = 2*q0*q1 + 2*q2*q3;
+            Kfusion[0] = SK_MY[0]*(P[0][19] + P[0][0]*SH_MAG[2] + P[0][1]*SH_MAG[1] + P[0][2]*SH_MAG[0] - P[0][3]*SK_MY[2] - P[0][16]*SK_MY[1] - P[0][15]*SK_MY[3] + P[0][17]*SK_MY[4]);
+            Kfusion[1] = SK_MY[0]*(P[1][19] + P[1][0]*SH_MAG[2] + P[1][1]*SH_MAG[1] + P[1][2]*SH_MAG[0] - P[1][3]*SK_MY[2] - P[1][16]*SK_MY[1] - P[1][15]*SK_MY[3] + P[1][17]*SK_MY[4]);
+            Kfusion[2] = SK_MY[0]*(P[2][19] + P[2][0]*SH_MAG[2] + P[2][1]*SH_MAG[1] + P[2][2]*SH_MAG[0] - P[2][3]*SK_MY[2] - P[2][16]*SK_MY[1] - P[2][15]*SK_MY[3] + P[2][17]*SK_MY[4]);
+            Kfusion[3] = SK_MY[0]*(P[3][19] + P[3][0]*SH_MAG[2] + P[3][1]*SH_MAG[1] + P[3][2]*SH_MAG[0] - P[3][3]*SK_MY[2] - P[3][16]*SK_MY[1] - P[3][15]*SK_MY[3] + P[3][17]*SK_MY[4]);
+            Kfusion[4] = SK_MY[0]*(P[4][19] + P[4][0]*SH_MAG[2] + P[4][1]*SH_MAG[1] + P[4][2]*SH_MAG[0] - P[4][3]*SK_MY[2] - P[4][16]*SK_MY[1] - P[4][15]*SK_MY[3] + P[4][17]*SK_MY[4]);
+            Kfusion[5] = SK_MY[0]*(P[5][19] + P[5][0]*SH_MAG[2] + P[5][1]*SH_MAG[1] + P[5][2]*SH_MAG[0] - P[5][3]*SK_MY[2] - P[5][16]*SK_MY[1] - P[5][15]*SK_MY[3] + P[5][17]*SK_MY[4]);
+            Kfusion[6] = SK_MY[0]*(P[6][19] + P[6][0]*SH_MAG[2] + P[6][1]*SH_MAG[1] + P[6][2]*SH_MAG[0] - P[6][3]*SK_MY[2] - P[6][16]*SK_MY[1] - P[6][15]*SK_MY[3] + P[6][17]*SK_MY[4]);
+            Kfusion[7] = SK_MY[0]*(P[7][19] + P[7][0]*SH_MAG[2] + P[7][1]*SH_MAG[1] + P[7][2]*SH_MAG[0] - P[7][3]*SK_MY[2] - P[7][16]*SK_MY[1] - P[7][15]*SK_MY[3] + P[7][17]*SK_MY[4]);
+            Kfusion[8] = SK_MY[0]*(P[8][19] + P[8][0]*SH_MAG[2] + P[8][1]*SH_MAG[1] + P[8][2]*SH_MAG[0] - P[8][3]*SK_MY[2] - P[8][16]*SK_MY[1] - P[8][15]*SK_MY[3] + P[8][17]*SK_MY[4]);
+            Kfusion[9] = SK_MY[0]*(P[9][19] + P[9][0]*SH_MAG[2] + P[9][1]*SH_MAG[1] + P[9][2]*SH_MAG[0] - P[9][3]*SK_MY[2] - P[9][16]*SK_MY[1] - P[9][15]*SK_MY[3] + P[9][17]*SK_MY[4]);
+            Kfusion[10] = SK_MY[0]*(P[10][19] + P[10][0]*SH_MAG[2] + P[10][1]*SH_MAG[1] + P[10][2]*SH_MAG[0] - P[10][3]*SK_MY[2] - P[10][16]*SK_MY[1] - P[10][15]*SK_MY[3] + P[10][17]*SK_MY[4]);
+            Kfusion[11] = SK_MY[0]*(P[11][19] + P[11][0]*SH_MAG[2] + P[11][1]*SH_MAG[1] + P[11][2]*SH_MAG[0] - P[11][3]*SK_MY[2] - P[11][16]*SK_MY[1] - P[11][15]*SK_MY[3] + P[11][17]*SK_MY[4]);
+            Kfusion[12] = SK_MY[0]*(P[12][19] + P[12][0]*SH_MAG[2] + P[12][1]*SH_MAG[1] + P[12][2]*SH_MAG[0] - P[12][3]*SK_MY[2] - P[12][16]*SK_MY[1] - P[12][15]*SK_MY[3] + P[12][17]*SK_MY[4]);
+            Kfusion[13] = SK_MY[0]*(P[13][19] + P[13][0]*SH_MAG[2] + P[13][1]*SH_MAG[1] + P[13][2]*SH_MAG[0] - P[13][3]*SK_MY[2] - P[13][16]*SK_MY[1] - P[13][15]*SK_MY[3] + P[13][17]*SK_MY[4]);
+            Kfusion[14] = SK_MY[0]*(P[14][19] + P[14][0]*SH_MAG[2] + P[14][1]*SH_MAG[1] + P[14][2]*SH_MAG[0] - P[14][3]*SK_MY[2] - P[14][16]*SK_MY[1] - P[14][15]*SK_MY[3] + P[14][17]*SK_MY[4]);
+            Kfusion[15] = SK_MY[0]*(P[15][19] + P[15][0]*SH_MAG[2] + P[15][1]*SH_MAG[1] + P[15][2]*SH_MAG[0] - P[15][3]*SK_MY[2] - P[15][16]*SK_MY[1] - P[15][15]*SK_MY[3] + P[15][17]*SK_MY[4]);
+            Kfusion[16] = SK_MY[0]*(P[16][19] + P[16][0]*SH_MAG[2] + P[16][1]*SH_MAG[1] + P[16][2]*SH_MAG[0] - P[16][3]*SK_MY[2] - P[16][16]*SK_MY[1] - P[16][15]*SK_MY[3] + P[16][17]*SK_MY[4]);
+            Kfusion[17] = SK_MY[0]*(P[17][19] + P[17][0]*SH_MAG[2] + P[17][1]*SH_MAG[1] + P[17][2]*SH_MAG[0] - P[17][3]*SK_MY[2] - P[17][16]*SK_MY[1] - P[17][15]*SK_MY[3] + P[17][17]*SK_MY[4]);
+            Kfusion[18] = SK_MY[0]*(P[18][19] + P[18][0]*SH_MAG[2] + P[18][1]*SH_MAG[1] + P[18][2]*SH_MAG[0] - P[18][3]*SK_MY[2] - P[18][16]*SK_MY[1] - P[18][15]*SK_MY[3] + P[18][17]*SK_MY[4]);
+            Kfusion[19] = SK_MY[0]*(P[19][19] + P[19][0]*SH_MAG[2] + P[19][1]*SH_MAG[1] + P[19][2]*SH_MAG[0] - P[19][3]*SK_MY[2] - P[19][16]*SK_MY[1] - P[19][15]*SK_MY[3] + P[19][17]*SK_MY[4]);
+            Kfusion[20] = SK_MY[0]*(P[20][19] + P[20][0]*SH_MAG[2] + P[20][1]*SH_MAG[1] + P[20][2]*SH_MAG[0] - P[20][3]*SK_MY[2] - P[20][16]*SK_MY[1] - P[20][15]*SK_MY[3] + P[20][17]*SK_MY[4]);
+            varInnovMag[1] = 1.0f/SK_MY[0];
+            innovMag[1] = MagPred[1] - magData.y;
+        }
+        else if (obsIndex == 2) // we are now fusing the Z measurement
+        {
+            // Calculate observation jacobians
+            for (uint8_t i=0; i<=20; i++) H_MAG[i] = 0;
+            H_MAG[0] = SH_MAG[1];
+            H_MAG[1] = 2*magN*q3 - 2*magE*q0 - 2*magD*q1;
+            H_MAG[2] = SH_MAG[7] + SH_MAG[8] - 2*magD*q2;
+            H_MAG[3] = SH_MAG[0];
+            H_MAG[15] = 2*q0*q2 + 2*q1*q3;
+            H_MAG[16] = 2*q2*q3 - 2*q0*q1;
+            H_MAG[17] = SH_MAG[3] - SH_MAG[4] - SH_MAG[5] + SH_MAG[6];
+            H_MAG[20] = 1;
+
+            // Calculate Kalman gain
+            SK_MZ[0] = 1/(P[20][20] + R_MAG + P[0][20]*SH_MAG[1] + P[3][20]*SH_MAG[0] + P[17][20]*(SH_MAG[3] - SH_MAG[4] - SH_MAG[5] + SH_MAG[6]) - (2*magD*q1 + 2*magE*q0 - 2*magN*q3)*(P[20][1] + P[0][1]*SH_MAG[1] + P[3][1]*SH_MAG[0] + P[17][1]*(SH_MAG[3] - SH_MAG[4] - SH_MAG[5] + SH_MAG[6]) + P[15][1]*(2*q0*q2 + 2*q1*q3) - P[16][1]*(2*q0*q1 - 2*q2*q3) - P[1][1]*(2*magD*q1 + 2*magE*q0 - 2*magN*q3) + P[2][1]*(SH_MAG[7] + SH_MAG[8] - 2*magD*q2)) + (SH_MAG[7] + SH_MAG[8] - 2*magD*q2)*(P[20][2] + P[0][2]*SH_MAG[1] + P[3][2]*SH_MAG[0] + P[17][2]*(SH_MAG[3] - SH_MAG[4] - SH_MAG[5] + SH_MAG[6]) + P[15][2]*(2*q0*q2 + 2*q1*q3) - P[16][2]*(2*q0*q1 - 2*q2*q3) - P[1][2]*(2*magD*q1 + 2*magE*q0 - 2*magN*q3) + P[2][2]*(SH_MAG[7] + SH_MAG[8] - 2*magD*q2)) + SH_MAG[1]*(P[20][0] + P[0][0]*SH_MAG[1] + P[3][0]*SH_MAG[0] + P[17][0]*(SH_MAG[3] - SH_MAG[4] - SH_MAG[5] + SH_MAG[6]) + P[15][0]*(2*q0*q2 + 2*q1*q3) - P[16][0]*(2*q0*q1 - 2*q2*q3) - P[1][0]*(2*magD*q1 + 2*magE*q0 - 2*magN*q3) + P[2][0]*(SH_MAG[7] + SH_MAG[8] - 2*magD*q2)) + SH_MAG[0]*(P[20][3] + P[0][3]*SH_MAG[1] + P[3][3]*SH_MAG[0] + P[17][3]*(SH_MAG[3] - SH_MAG[4] - SH_MAG[5] + SH_MAG[6]) + P[15][3]*(2*q0*q2 + 2*q1*q3) - P[16][3]*(2*q0*q1 - 2*q2*q3) - P[1][3]*(2*magD*q1 + 2*magE*q0 - 2*magN*q3) + P[2][3]*(SH_MAG[7] + SH_MAG[8] - 2*magD*q2)) + (SH_MAG[3] - SH_MAG[4] - SH_MAG[5] + SH_MAG[6])*(P[20][17] + P[0][17]*SH_MAG[1] + P[3][17]*SH_MAG[0] + P[17][17]*(SH_MAG[3] - SH_MAG[4] - SH_MAG[5] + SH_MAG[6]) + P[15][17]*(2*q0*q2 + 2*q1*q3) - P[16][17]*(2*q0*q1 - 2*q2*q3) - P[1][17]*(2*magD*q1 + 2*magE*q0 - 2*magN*q3) + P[2][17]*(SH_MAG[7] + SH_MAG[8] - 2*magD*q2)) + P[15][20]*(2*q0*q2 + 2*q1*q3) - P[16][20]*(2*q0*q1 - 2*q2*q3) - P[1][20]*(2*magD*q1 + 2*magE*q0 - 2*magN*q3) + (2*q0*q2 + 2*q1*q3)*(P[20][15] + P[0][15]*SH_MAG[1] + P[3][15]*SH_MAG[0] + P[17][15]*(SH_MAG[3] - SH_MAG[4] - SH_MAG[5] + SH_MAG[6]) + P[15][15]*(2*q0*q2 + 2*q1*q3) - P[16][15]*(2*q0*q1 - 2*q2*q3) - P[1][15]*(2*magD*q1 + 2*magE*q0 - 2*magN*q3) + P[2][15]*(SH_MAG[7] + SH_MAG[8] - 2*magD*q2)) - (2*q0*q1 - 2*q2*q3)*(P[20][16] + P[0][16]*SH_MAG[1] + P[3][16]*SH_MAG[0] + P[17][16]*(SH_MAG[3] - SH_MAG[4] - SH_MAG[5] + SH_MAG[6]) + P[15][16]*(2*q0*q2 + 2*q1*q3) - P[16][16]*(2*q0*q1 - 2*q2*q3) - P[1][16]*(2*magD*q1 + 2*magE*q0 - 2*magN*q3) + P[2][16]*(SH_MAG[7] + SH_MAG[8] - 2*magD*q2)) + P[2][20]*(SH_MAG[7] + SH_MAG[8] - 2*magD*q2));
+            SK_MZ[1] = SH_MAG[3] - SH_MAG[4] - SH_MAG[5] + SH_MAG[6];
+            SK_MZ[2] = 2*magD*q1 + 2*magE*q0 - 2*magN*q3;
+            SK_MZ[3] = SH_MAG[7] + SH_MAG[8] - 2*magD*q2;
+            SK_MZ[4] = 2*q0*q1 - 2*q2*q3;
+            SK_MZ[5] = 2*q0*q2 + 2*q1*q3;
+            Kfusion[0] = SK_MZ[0]*(P[0][20] + P[0][0]*SH_MAG[1] + P[0][3]*SH_MAG[0] - P[0][1]*SK_MZ[2] + P[0][2]*SK_MZ[3] + P[0][17]*SK_MZ[1] + P[0][15]*SK_MZ[5] - P[0][16]*SK_MZ[4]);
+            Kfusion[1] = SK_MZ[0]*(P[1][20] + P[1][0]*SH_MAG[1] + P[1][3]*SH_MAG[0] - P[1][1]*SK_MZ[2] + P[1][2]*SK_MZ[3] + P[1][17]*SK_MZ[1] + P[1][15]*SK_MZ[5] - P[1][16]*SK_MZ[4]);
+            Kfusion[2] = SK_MZ[0]*(P[2][20] + P[2][0]*SH_MAG[1] + P[2][3]*SH_MAG[0] - P[2][1]*SK_MZ[2] + P[2][2]*SK_MZ[3] + P[2][17]*SK_MZ[1] + P[2][15]*SK_MZ[5] - P[2][16]*SK_MZ[4]);
+            Kfusion[3] = SK_MZ[0]*(P[3][20] + P[3][0]*SH_MAG[1] + P[3][3]*SH_MAG[0] - P[3][1]*SK_MZ[2] + P[3][2]*SK_MZ[3] + P[3][17]*SK_MZ[1] + P[3][15]*SK_MZ[5] - P[3][16]*SK_MZ[4]);
+            Kfusion[4] = SK_MZ[0]*(P[4][20] + P[4][0]*SH_MAG[1] + P[4][3]*SH_MAG[0] - P[4][1]*SK_MZ[2] + P[4][2]*SK_MZ[3] + P[4][17]*SK_MZ[1] + P[4][15]*SK_MZ[5] - P[4][16]*SK_MZ[4]);
+            Kfusion[5] = SK_MZ[0]*(P[5][20] + P[5][0]*SH_MAG[1] + P[5][3]*SH_MAG[0] - P[5][1]*SK_MZ[2] + P[5][2]*SK_MZ[3] + P[5][17]*SK_MZ[1] + P[5][15]*SK_MZ[5] - P[5][16]*SK_MZ[4]);
+            Kfusion[6] = SK_MZ[0]*(P[6][20] + P[6][0]*SH_MAG[1] + P[6][3]*SH_MAG[0] - P[6][1]*SK_MZ[2] + P[6][2]*SK_MZ[3] + P[6][17]*SK_MZ[1] + P[6][15]*SK_MZ[5] - P[6][16]*SK_MZ[4]);
+            Kfusion[7] = SK_MZ[0]*(P[7][20] + P[7][0]*SH_MAG[1] + P[7][3]*SH_MAG[0] - P[7][1]*SK_MZ[2] + P[7][2]*SK_MZ[3] + P[7][17]*SK_MZ[1] + P[7][15]*SK_MZ[5] - P[7][16]*SK_MZ[4]);
+            Kfusion[8] = SK_MZ[0]*(P[8][20] + P[8][0]*SH_MAG[1] + P[8][3]*SH_MAG[0] - P[8][1]*SK_MZ[2] + P[8][2]*SK_MZ[3] + P[8][17]*SK_MZ[1] + P[8][15]*SK_MZ[5] - P[8][16]*SK_MZ[4]);
+            Kfusion[9] = SK_MZ[0]*(P[9][20] + P[9][0]*SH_MAG[1] + P[9][3]*SH_MAG[0] - P[9][1]*SK_MZ[2] + P[9][2]*SK_MZ[3] + P[9][17]*SK_MZ[1] + P[9][15]*SK_MZ[5] - P[9][16]*SK_MZ[4]);
+            Kfusion[10] = SK_MZ[0]*(P[10][20] + P[10][0]*SH_MAG[1] + P[10][3]*SH_MAG[0] - P[10][1]*SK_MZ[2] + P[10][2]*SK_MZ[3] + P[10][17]*SK_MZ[1] + P[10][15]*SK_MZ[5] - P[10][16]*SK_MZ[4]);
+            Kfusion[11] = SK_MZ[0]*(P[11][20] + P[11][0]*SH_MAG[1] + P[11][3]*SH_MAG[0] - P[11][1]*SK_MZ[2] + P[11][2]*SK_MZ[3] + P[11][17]*SK_MZ[1] + P[11][15]*SK_MZ[5] - P[11][16]*SK_MZ[4]);
+            Kfusion[12] = SK_MZ[0]*(P[12][20] + P[12][0]*SH_MAG[1] + P[12][3]*SH_MAG[0] - P[12][1]*SK_MZ[2] + P[12][2]*SK_MZ[3] + P[12][17]*SK_MZ[1] + P[12][15]*SK_MZ[5] - P[12][16]*SK_MZ[4]);
+            Kfusion[13] = SK_MZ[0]*(P[13][20] + P[13][0]*SH_MAG[1] + P[13][3]*SH_MAG[0] - P[13][1]*SK_MZ[2] + P[13][2]*SK_MZ[3] + P[13][17]*SK_MZ[1] + P[13][15]*SK_MZ[5] - P[13][16]*SK_MZ[4]);
+            Kfusion[14] = SK_MZ[0]*(P[14][20] + P[14][0]*SH_MAG[1] + P[14][3]*SH_MAG[0] - P[14][1]*SK_MZ[2] + P[14][2]*SK_MZ[3] + P[14][17]*SK_MZ[1] + P[14][15]*SK_MZ[5] - P[14][16]*SK_MZ[4]);
+            Kfusion[15] = SK_MZ[0]*(P[15][20] + P[15][0]*SH_MAG[1] + P[15][3]*SH_MAG[0] - P[15][1]*SK_MZ[2] + P[15][2]*SK_MZ[3] + P[15][17]*SK_MZ[1] + P[15][15]*SK_MZ[5] - P[15][16]*SK_MZ[4]);
+            Kfusion[16] = SK_MZ[0]*(P[16][20] + P[16][0]*SH_MAG[1] + P[16][3]*SH_MAG[0] - P[16][1]*SK_MZ[2] + P[16][2]*SK_MZ[3] + P[16][17]*SK_MZ[1] + P[16][15]*SK_MZ[5] - P[16][16]*SK_MZ[4]);
+            Kfusion[17] = SK_MZ[0]*(P[17][20] + P[17][0]*SH_MAG[1] + P[17][3]*SH_MAG[0] - P[17][1]*SK_MZ[2] + P[17][2]*SK_MZ[3] + P[17][17]*SK_MZ[1] + P[17][15]*SK_MZ[5] - P[17][16]*SK_MZ[4]);
+            Kfusion[18] = SK_MZ[0]*(P[18][20] + P[18][0]*SH_MAG[1] + P[18][3]*SH_MAG[0] - P[18][1]*SK_MZ[2] + P[18][2]*SK_MZ[3] + P[18][17]*SK_MZ[1] + P[18][15]*SK_MZ[5] - P[18][16]*SK_MZ[4]);
+            Kfusion[19] = SK_MZ[0]*(P[19][20] + P[19][0]*SH_MAG[1] + P[19][3]*SH_MAG[0] - P[19][1]*SK_MZ[2] + P[19][2]*SK_MZ[3] + P[19][17]*SK_MZ[1] + P[19][15]*SK_MZ[5] - P[19][16]*SK_MZ[4]);
+            Kfusion[20] = SK_MZ[0]*(P[20][20] + P[20][0]*SH_MAG[1] + P[20][3]*SH_MAG[0] - P[20][1]*SK_MZ[2] + P[20][2]*SK_MZ[3] + P[20][17]*SK_MZ[1] + P[20][15]*SK_MZ[5] - P[20][16]*SK_MZ[4]);
+            varInnovMag[2] = 1.0f/SK_MZ[0];
+            innovMag[2] = MagPred[2] - magData.z;
+
+        }
+
+        // Check the innovation for consistency and don't fuse if > 5Sigma
+        if ((innovMag[obsIndex]*innovMag[obsIndex]/varInnovMag[obsIndex]) < 25.0)
+        {
+            // correct the state vector
+            for (uint8_t j= 0; j<=indexLimit; j++)
+            {
+                states[j] = states[j] - Kfusion[j] * innovMag[obsIndex];
+            }
+            // normalise the quaternion states
+            float quatMag = sqrt(states[0]*states[0] + states[1]*states[1] + states[2]*states[2] + states[3]*states[3]);
+            if (quatMag > 1e-12)
+            {
+                for (uint8_t j= 0; j<=3; j++)
+                {
+                    float quatMagInv = 1.0f/quatMag;
+                    states[j] = states[j] * quatMagInv;
+                }
+            }
+            // correct the covariance P = (I - K*H)*P
+            // take advantage of the empty columns in KH to reduce the
+            // number of operations
+            for (uint8_t i = 0; i<=indexLimit; i++)
+            {
+                for (uint8_t j = 0; j<=3; j++)
+                {
+                    KH[i][j] = Kfusion[i] * H_MAG[j];
+                }
+                for (uint8_t j = 4; j<=17; j++) KH[i][j] = 0.0f;
+                if (!onGround)
+                {
+                    for (uint8_t j = 15; j<=20; j++)
+                    {
+                        KH[i][j] = Kfusion[i] * H_MAG[j];
+                    }
+                }
+                else
+                {
+                    for (uint8_t j = 15; j<=20; j++)
+                    {
+                        KH[i][j] = 0.0f;
+                    }
+                }
+            }
+            for (uint8_t i = 0; i<=indexLimit; i++)
+            {
+                for (uint8_t j = 0; j<=indexLimit; j++)
+                {
+                    KHP[i][j] = 0.0f;
+                    for (uint8_t k = 0; k<=3; k++)
+                    {
+                        KHP[i][j] = KHP[i][j] + KH[i][k] * P[k][j];
+                    }
+                    if (!onGround)
+                    {
+                        for (uint8_t k = 15; k<=20; k++)
+                        {
+                            KHP[i][j] = KHP[i][j] + KH[i][k] * P[k][j];
+                        }
+                    }
+                }
+            }
+        }
+        for (uint8_t i = 0; i<=indexLimit; i++)
+        {
+            for (uint8_t j = 0; j<=indexLimit; j++)
+            {
+                P[i][j] = P[i][j] - KHP[i][j];
+            }
+        }
+    }
+    obsIndex = obsIndex + 1;
+
+    ForceSymmetry();
+    ConstrainVariances();
+}
+
+void AttPosEKF::FuseAirspeed()
+{
+    float vn;
+    float ve;
+    float vd;
+    float vwn;
+    float vwe;
+    const float R_TAS = 2.0f;
+    float SH_TAS[3];
+    float Kfusion[21];
+    float VtasPred;
+
+    // Copy required states to local variable names
+    vn = statesAtVtasMeasTime[4];
+    ve = statesAtVtasMeasTime[5];
+    vd = statesAtVtasMeasTime[6];
+    vwn = statesAtVtasMeasTime[13];
+    vwe = statesAtVtasMeasTime[14];
+
+    // Need to check that it is flying before fusing airspeed data
+    // Calculate the predicted airspeed
+    VtasPred = sqrtf((ve - vwe)*(ve - vwe) + (vn - vwn)*(vn - vwn) + vd*vd);
+    // Perform fusion of True Airspeed measurement
+    if (useAirspeed && fuseVtasData && (VtasPred > 1.0f) && (VtasMeas > 8.0f))
+    {
+        // Calculate observation jacobians
+        SH_TAS[0] = 1/(sqrt(sq(ve - vwe) + sq(vn - vwn) + sq(vd)));
+        SH_TAS[1] = (SH_TAS[0]*(2.0f*ve - 2*vwe))/2.0f;
+        SH_TAS[2] = (SH_TAS[0]*(2.0f*vn - 2*vwn))/2.0f;
+        
+        float H_TAS[21];
+        for (uint8_t i=0; i<=20; i++) H_TAS[i] = 0.0f;
+        H_TAS[4] = SH_TAS[2];
+        H_TAS[5] = SH_TAS[1];
+        H_TAS[6] = vd*SH_TAS[0];
+        H_TAS[13] = -SH_TAS[2];
+        H_TAS[14] = -SH_TAS[1];
+
+        // Calculate Kalman gains
+        float SK_TAS = 1.0f/(R_TAS + SH_TAS[2]*(P[4][4]*SH_TAS[2] + P[5][4]*SH_TAS[1] - P[13][4]*SH_TAS[2] - P[14][4]*SH_TAS[1] + P[6][4]*vd*SH_TAS[0]) + SH_TAS[1]*(P[4][5]*SH_TAS[2] + P[5][5]*SH_TAS[1] - P[13][5]*SH_TAS[2] - P[14][5]*SH_TAS[1] + P[6][5]*vd*SH_TAS[0]) - SH_TAS[2]*(P[4][13]*SH_TAS[2] + P[5][13]*SH_TAS[1] - P[13][13]*SH_TAS[2] - P[14][13]*SH_TAS[1] + P[6][13]*vd*SH_TAS[0]) - SH_TAS[1]*(P[4][14]*SH_TAS[2] + P[5][14]*SH_TAS[1] - P[13][14]*SH_TAS[2] - P[14][14]*SH_TAS[1] + P[6][14]*vd*SH_TAS[0]) + vd*SH_TAS[0]*(P[4][6]*SH_TAS[2] + P[5][6]*SH_TAS[1] - P[13][6]*SH_TAS[2] - P[14][6]*SH_TAS[1] + P[6][6]*vd*SH_TAS[0]));
+        Kfusion[0] = SK_TAS*(P[0][4]*SH_TAS[2] - P[0][13]*SH_TAS[2] + P[0][5]*SH_TAS[1] - P[0][14]*SH_TAS[1] + P[0][6]*vd*SH_TAS[0]);
+        Kfusion[1] = SK_TAS*(P[1][4]*SH_TAS[2] - P[1][13]*SH_TAS[2] + P[1][5]*SH_TAS[1] - P[1][14]*SH_TAS[1] + P[1][6]*vd*SH_TAS[0]);
+        Kfusion[2] = SK_TAS*(P[2][4]*SH_TAS[2] - P[2][13]*SH_TAS[2] + P[2][5]*SH_TAS[1] - P[2][14]*SH_TAS[1] + P[2][6]*vd*SH_TAS[0]);
+        Kfusion[3] = SK_TAS*(P[3][4]*SH_TAS[2] - P[3][13]*SH_TAS[2] + P[3][5]*SH_TAS[1] - P[3][14]*SH_TAS[1] + P[3][6]*vd*SH_TAS[0]);
+        Kfusion[4] = SK_TAS*(P[4][4]*SH_TAS[2] - P[4][13]*SH_TAS[2] + P[4][5]*SH_TAS[1] - P[4][14]*SH_TAS[1] + P[4][6]*vd*SH_TAS[0]);
+        Kfusion[5] = SK_TAS*(P[5][4]*SH_TAS[2] - P[5][13]*SH_TAS[2] + P[5][5]*SH_TAS[1] - P[5][14]*SH_TAS[1] + P[5][6]*vd*SH_TAS[0]);
+        Kfusion[6] = SK_TAS*(P[6][4]*SH_TAS[2] - P[6][13]*SH_TAS[2] + P[6][5]*SH_TAS[1] - P[6][14]*SH_TAS[1] + P[6][6]*vd*SH_TAS[0]);
+        Kfusion[7] = SK_TAS*(P[7][4]*SH_TAS[2] - P[7][13]*SH_TAS[2] + P[7][5]*SH_TAS[1] - P[7][14]*SH_TAS[1] + P[7][6]*vd*SH_TAS[0]);
+        Kfusion[8] = SK_TAS*(P[8][4]*SH_TAS[2] - P[8][13]*SH_TAS[2] + P[8][5]*SH_TAS[1] - P[8][14]*SH_TAS[1] + P[8][6]*vd*SH_TAS[0]);
+        Kfusion[9] = SK_TAS*(P[9][4]*SH_TAS[2] - P[9][13]*SH_TAS[2] + P[9][5]*SH_TAS[1] - P[9][14]*SH_TAS[1] + P[9][6]*vd*SH_TAS[0]);
+        Kfusion[10] = SK_TAS*(P[10][4]*SH_TAS[2] - P[10][13]*SH_TAS[2] + P[10][5]*SH_TAS[1] - P[10][14]*SH_TAS[1] + P[10][6]*vd*SH_TAS[0]);
+        Kfusion[11] = SK_TAS*(P[11][4]*SH_TAS[2] - P[11][13]*SH_TAS[2] + P[11][5]*SH_TAS[1] - P[11][14]*SH_TAS[1] + P[11][6]*vd*SH_TAS[0]);
+        Kfusion[12] = SK_TAS*(P[12][4]*SH_TAS[2] - P[12][13]*SH_TAS[2] + P[12][5]*SH_TAS[1] - P[12][14]*SH_TAS[1] + P[12][6]*vd*SH_TAS[0]);
+        Kfusion[13] = SK_TAS*(P[13][4]*SH_TAS[2] - P[13][13]*SH_TAS[2] + P[13][5]*SH_TAS[1] - P[13][14]*SH_TAS[1] + P[13][6]*vd*SH_TAS[0]);
+        Kfusion[14] = SK_TAS*(P[14][4]*SH_TAS[2] - P[14][13]*SH_TAS[2] + P[14][5]*SH_TAS[1] - P[14][14]*SH_TAS[1] + P[14][6]*vd*SH_TAS[0]);
+        Kfusion[15] = SK_TAS*(P[15][4]*SH_TAS[2] - P[15][13]*SH_TAS[2] + P[15][5]*SH_TAS[1] - P[15][14]*SH_TAS[1] + P[15][6]*vd*SH_TAS[0]);
+        Kfusion[16] = SK_TAS*(P[16][4]*SH_TAS[2] - P[16][13]*SH_TAS[2] + P[16][5]*SH_TAS[1] - P[16][14]*SH_TAS[1] + P[16][6]*vd*SH_TAS[0]);
+        Kfusion[17] = SK_TAS*(P[17][4]*SH_TAS[2] - P[17][13]*SH_TAS[2] + P[17][5]*SH_TAS[1] - P[17][14]*SH_TAS[1] + P[17][6]*vd*SH_TAS[0]);
+        Kfusion[18] = SK_TAS*(P[18][4]*SH_TAS[2] - P[18][13]*SH_TAS[2] + P[18][5]*SH_TAS[1] - P[18][14]*SH_TAS[1] + P[18][6]*vd*SH_TAS[0]);
+        Kfusion[19] = SK_TAS*(P[19][4]*SH_TAS[2] - P[19][13]*SH_TAS[2] + P[19][5]*SH_TAS[1] - P[19][14]*SH_TAS[1] + P[19][6]*vd*SH_TAS[0]);
+        Kfusion[20] = SK_TAS*(P[20][4]*SH_TAS[2] - P[20][13]*SH_TAS[2] + P[20][5]*SH_TAS[1] - P[20][14]*SH_TAS[1] + P[20][6]*vd*SH_TAS[0]);
+        varInnovVtas = 1.0f/SK_TAS;
+
+        // Calculate the measurement innovation
+        innovVtas = VtasPred - VtasMeas;
+        // Check the innovation for consistency and don't fuse if > 5Sigma
+        if ((innovVtas*innovVtas*SK_TAS) < 25.0)
+        {
+            // correct the state vector
+            for (uint8_t j=0; j<=20; j++)
+            {
+                states[j] = states[j] - Kfusion[j] * innovVtas;
+            }
+            // normalise the quaternion states
+            float quatMag = sqrt(states[0]*states[0] + states[1]*states[1] + states[2]*states[2] + states[3]*states[3]);
+            if (quatMag > 1e-12f)
+            {
+                for (uint8_t j= 0; j<=3; j++)
+                {
+                    float quatMagInv = 1.0f/quatMag;
+                    states[j] = states[j] * quatMagInv;
+                }
+            }
+            // correct the covariance P = (I - K*H)*P
+            // take advantage of the empty columns in H to reduce the
+            // number of operations
+            for (uint8_t i = 0; i<=20; i++)
+            {
+                for (uint8_t j = 0; j<=3; j++) KH[i][j] = 0.0;
+                for (uint8_t j = 4; j<=6; j++)
+                {
+                    KH[i][j] = Kfusion[i] * H_TAS[j];
+                }
+                for (uint8_t j = 7; j<=12; j++) KH[i][j] = 0.0;
+                for (uint8_t j = 13; j<=14; j++)
+                {
+                    KH[i][j] = Kfusion[i] * H_TAS[j];
+                }
+                for (uint8_t j = 15; j<=20; j++) KH[i][j] = 0.0;
+            }
+            for (uint8_t i = 0; i<=20; i++)
+            {
+                for (uint8_t j = 0; j<=20; j++)
+                {
+                    KHP[i][j] = 0.0;
+                    for (uint8_t k = 4; k<=6; k++)
+                    {
+                        KHP[i][j] = KHP[i][j] + KH[i][k] * P[k][j];
+                    }
+                    for (uint8_t k = 13; k<=14; k++)
+                    {
+                        KHP[i][j] = KHP[i][j] + KH[i][k] * P[k][j];
+                    }
+                }
+            }
+            for (uint8_t i = 0; i<=20; i++)
+            {
+                for (uint8_t j = 0; j<=20; j++)
+                {
+                    P[i][j] = P[i][j] - KHP[i][j];
+                }
+            }
+        }
+    }
+
+    ForceSymmetry();
+    ConstrainVariances();
+}
+
+void AttPosEKF::zeroRows(float (&covMat)[n_states][n_states], uint8_t first, uint8_t last)
+{
+    uint8_t row;
+    uint8_t col;
+    for (row=first; row<=last; row++)
+    {
+        for (col=0; col<n_states; col++)
+        {
+            covMat[row][col] = 0.0;
+        }
+    }
+}
+
+void AttPosEKF::zeroCols(float (&covMat)[n_states][n_states], uint8_t first, uint8_t last)
+{
+    uint8_t row;
+    uint8_t col;
+    for (col=first; col<=last; col++)
+    {
+        for (row=0; row < n_states; row++)
+        {
+            covMat[row][col] = 0.0;
+        }
+    }
+}
+
+float AttPosEKF::sq(float valIn)
+{
+    return valIn*valIn;
+}
+
+// Store states in a history array along with time stamp
+void AttPosEKF::StoreStates(uint64_t timestamp_ms)
+{
+    for (unsigned i=0; i<n_states; i++)
+        storedStates[i][storeIndex] = states[i];
+    statetimeStamp[storeIndex] = timestamp_ms;
+    storeIndex++;
+    if (storeIndex == data_buffer_size)
+        storeIndex = 0;
+}
+
+void AttPosEKF::ResetStoredStates()
+{
+    // reset all stored states
+    memset(&storedStates[0][0], 0, sizeof(storedStates));
+    memset(&statetimeStamp[0], 0, sizeof(statetimeStamp));
+
+    // reset store index to first
+    storeIndex = 0;
+
+    // overwrite all existing states
+    for (unsigned i = 0; i < n_states; i++) {
+        storedStates[i][storeIndex] = states[i];
+    }
+
+    statetimeStamp[storeIndex] = millis();
+
+    // increment to next storage index
+    storeIndex++;
+}
+
+// Output the state vector stored at the time that best matches that specified by msec
+int AttPosEKF::RecallStates(float statesForFusion[n_states], uint64_t msec)
+{
+    int ret = 0;
+
+    // int64_t bestTimeDelta = 200;
+    // unsigned bestStoreIndex = 0;
+    // for (unsigned storeIndex = 0; storeIndex < data_buffer_size; storeIndex++)
+    // {
+    //     // The time delta can also end up as negative number,
+    //     // since we might compare future to past or past to future
+    //     // therefore cast to int64.
+    //     int64_t timeDelta = (int64_t)msec - (int64_t)statetimeStamp[storeIndex];
+    //     if (timeDelta < 0) {
+    //         timeDelta = -timeDelta;
+    //     }
+
+    //     if (timeDelta < bestTimeDelta)
+    //     {
+    //         bestStoreIndex = storeIndex;
+    //         bestTimeDelta = timeDelta;
+    //     }
+    // }
+    // if (bestTimeDelta < 200) // only output stored state if < 200 msec retrieval error
+    // {
+    //     for (uint8_t i=0; i < n_states; i++) {
+    //         if (isfinite(storedStates[i][bestStoreIndex])) {
+    //             statesForFusion[i] = storedStates[i][bestStoreIndex];
+    //         } else if (isfinite(states[i])) {
+    //             statesForFusion[i] = states[i];
+    //         } else {
+    //             // There is not much we can do here, except reporting the error we just
+    //             // found.
+    //             ret++;
+    //         }
+    // }
+    // else // otherwise output current state
+    // {
+        for (uint8_t i=0; i < n_states; i++) {
+            if (isfinite(states[i])) {
+                statesForFusion[i] = states[i];
+            } else {
+                ret++;
+            }
+        }
+    // }
+
+    return ret;
+}
+
+void AttPosEKF::quat2Tnb(Mat3f &Tnb, const float (&quat)[4])
+{
+    // Calculate the nav to body cosine matrix
+    float q00 = sq(quat[0]);
+    float q11 = sq(quat[1]);
+    float q22 = sq(quat[2]);
+    float q33 = sq(quat[3]);
+    float q01 =  quat[0]*quat[1];
+    float q02 =  quat[0]*quat[2];
+    float q03 =  quat[0]*quat[3];
+    float q12 =  quat[1]*quat[2];
+    float q13 =  quat[1]*quat[3];
+    float q23 =  quat[2]*quat[3];
+
+    Tnb.x.x = q00 + q11 - q22 - q33;
+    Tnb.y.y = q00 - q11 + q22 - q33;
+    Tnb.z.z = q00 - q11 - q22 + q33;
+    Tnb.y.x = 2*(q12 - q03);
+    Tnb.z.x = 2*(q13 + q02);
+    Tnb.x.y = 2*(q12 + q03);
+    Tnb.z.y = 2*(q23 - q01);
+    Tnb.x.z = 2*(q13 - q02);
+    Tnb.y.z = 2*(q23 + q01);
+}
+
+void AttPosEKF::quat2Tbn(Mat3f &Tbn, const float (&quat)[4])
+{
+    // Calculate the body to nav cosine matrix
+    float q00 = sq(quat[0]);
+    float q11 = sq(quat[1]);
+    float q22 = sq(quat[2]);
+    float q33 = sq(quat[3]);
+    float q01 =  quat[0]*quat[1];
+    float q02 =  quat[0]*quat[2];
+    float q03 =  quat[0]*quat[3];
+    float q12 =  quat[1]*quat[2];
+    float q13 =  quat[1]*quat[3];
+    float q23 =  quat[2]*quat[3];
+
+    Tbn.x.x = q00 + q11 - q22 - q33;
+    Tbn.y.y = q00 - q11 + q22 - q33;
+    Tbn.z.z = q00 - q11 - q22 + q33;
+    Tbn.x.y = 2*(q12 - q03);
+    Tbn.x.z = 2*(q13 + q02);
+    Tbn.y.x = 2*(q12 + q03);
+    Tbn.y.z = 2*(q23 - q01);
+    Tbn.z.x = 2*(q13 - q02);
+    Tbn.z.y = 2*(q23 + q01);
+}
+
+void AttPosEKF::eul2quat(float (&quat)[4], const float (&eul)[3])
+{
+    float u1 = cos(0.5f*eul[0]);
+    float u2 = cos(0.5f*eul[1]);
+    float u3 = cos(0.5f*eul[2]);
+    float u4 = sin(0.5f*eul[0]);
+    float u5 = sin(0.5f*eul[1]);
+    float u6 = sin(0.5f*eul[2]);
+    quat[0] = u1*u2*u3+u4*u5*u6;
+    quat[1] = u4*u2*u3-u1*u5*u6;
+    quat[2] = u1*u5*u3+u4*u2*u6;
+    quat[3] = u1*u2*u6-u4*u5*u3;
+}
+
+void AttPosEKF::quat2eul(float (&y)[3], const float (&u)[4])
+{
+    y[0] = atan2f((2.0f*(u[2]*u[3]+u[0]*u[1])) , (u[0]*u[0]-u[1]*u[1]-u[2]*u[2]+u[3]*u[3]));
+    y[1] = -asinf(2.0f*(u[1]*u[3]-u[0]*u[2]));
+    y[2] = atan2f((2.0f*(u[1]*u[2]+u[0]*u[3])) , (u[0]*u[0]+u[1]*u[1]-u[2]*u[2]-u[3]*u[3]));
+}
+
+void AttPosEKF::calcvelNED(float (&velNED)[3], float gpsCourse, float gpsGndSpd, float gpsVelD)
+{
+    velNED[0] = gpsGndSpd*cosf(gpsCourse);
+    velNED[1] = gpsGndSpd*sinf(gpsCourse);
+    velNED[2] = gpsVelD;
+}
+
+void AttPosEKF::calcposNED(float (&posNED)[3], float lat, float lon, float hgt, float latRef, float lonRef, float hgtRef)
+{
+    posNED[0] = earthRadius * (lat - latRef);
+    posNED[1] = earthRadius * cos(latRef) * (lon - lonRef);
+    posNED[2] = -(hgt - hgtRef);
+}
+
+void AttPosEKF::calcLLH(float (&posNED)[3], float lat, float lon, float hgt, float latRef, float lonRef, float hgtRef)
+{
+    lat = latRef + posNED[0] * earthRadiusInv;
+    lon = lonRef + posNED[1] * earthRadiusInv / cos(latRef);
+    hgt = hgtRef - posNED[2];
+}
+
+void AttPosEKF::OnGroundCheck()
+{
+    onGround = (((sq(velNED[0]) + sq(velNED[1]) + sq(velNED[2])) < 4.0f) && (VtasMeas < 8.0f));
+    if (staticMode) {
+        staticMode = !(GPSstatus > GPS_FIX_2D);
+    }
+}
+
+void AttPosEKF::calcEarthRateNED(Vector3f &omega, float latitude)
+{
+    //Define Earth rotation vector in the NED navigation frame
+    omega.x  = earthRate*cosf(latitude);
+    omega.y  = 0.0f;
+    omega.z  = -earthRate*sinf(latitude);
+}
+
+void AttPosEKF::CovarianceInit()
+{
+    // Calculate the initial covariance matrix P
+    P[0][0]   = 0.25f * sq(1.0f*deg2rad);
+    P[1][1]   = 0.25f * sq(1.0f*deg2rad);
+    P[2][2]   = 0.25f * sq(1.0f*deg2rad);
+    P[3][3]   = 0.25f * sq(10.0f*deg2rad);
+    P[4][4]   = sq(0.7);
+    P[5][5]   = P[4][4];
+    P[6][6]   = sq(0.7);
+    P[7][7]   = sq(15.0);
+    P[8][8]   = P[7][7];
+    P[9][9]   = sq(5.0);
+    P[10][10] = sq(0.1*deg2rad*dtIMU);
+    P[11][11] = P[10][10];
+    P[12][12] = P[10][10];
+    P[13][13] = sq(8.0f);
+    P[14][4]  = P[13][13];
+    P[15][15] = sq(0.02f);
+    P[16][16] = P[15][15];
+    P[17][17] = P[15][15];
+    P[18][18] = sq(0.02f);
+    P[19][19] = P[18][18];
+    P[20][20] = P[18][18];
+}
+
+float AttPosEKF::ConstrainFloat(float val, float min, float max)
+{
+    return (val > max) ? max : ((val < min) ? min : val);
+}
+
+void AttPosEKF::ConstrainVariances()
+{
+    if (!numericalProtection) {
+        return;
+    }
+
+    // State vector:
+    // 0-3: quaternions (q0, q1, q2, q3)
+    // 4-6: Velocity - m/sec (North, East, Down)
+    // 7-9: Position - m (North, East, Down)
+    // 10-12: Delta Angle bias - rad (X,Y,Z)
+    // 13-14: Wind Vector  - m/sec (North,East)
+    // 15-17: Earth Magnetic Field Vector - gauss (North, East, Down)
+    // 18-20: Body Magnetic Field Vector - gauss (X,Y,Z)
+
+    // Constrain quaternion variances
+    for (unsigned i = 0; i < 4; i++) {
+        P[i][i] = ConstrainFloat(P[i][i], 0.0f, 1.0f);
+    }
+
+    // Constrain velocitie variances
+    for (unsigned i = 4; i < 7; i++) {
+        P[i][i] = ConstrainFloat(P[i][i], 0.0f, 1.0e3f);
+    }
+
+    // Constrain position variances
+    for (unsigned i = 7; i < 10; i++) {
+        P[i][i] = ConstrainFloat(P[i][i], 0.0f, 1.0e6f);
+    }
+
+    // Angle bias variances
+    for (unsigned i = 10; i < 13; i++) {
+        P[i][i] = ConstrainFloat(P[i][i], 0.0f, sq(0.175f * dtIMU));
+    }
+
+    // Wind velocity variances
+    for (unsigned i = 13; i < 15; i++) {
+        P[i][i] = ConstrainFloat(P[i][i], 0.0f, 1.0e3f);
+    }
+
+    // Earth magnetic field variances
+    for (unsigned i = 15; i < 18; i++) {
+        P[i][i] = ConstrainFloat(P[i][i], 0.0f, 1.0f);
+    }
+
+    // Body magnetic field variances
+    for (unsigned i = 18; i < 21; i++) {
+        P[i][i] = ConstrainFloat(P[i][i], 0.0f, 1.0f);
+    }
+
+}
+
+void AttPosEKF::ConstrainStates()
+{
+    if (!numericalProtection) {
+        return;
+    }
+
+    // State vector:
+    // 0-3: quaternions (q0, q1, q2, q3)
+    // 4-6: Velocity - m/sec (North, East, Down)
+    // 7-9: Position - m (North, East, Down)
+    // 10-12: Delta Angle bias - rad (X,Y,Z)
+    // 13-14: Wind Vector  - m/sec (North,East)
+    // 15-17: Earth Magnetic Field Vector - gauss (North, East, Down)
+    // 18-20: Body Magnetic Field Vector - gauss (X,Y,Z)
+
+
+    // Constrain quaternion
+    for (unsigned i = 0; i < 4; i++) {
+        states[i] = ConstrainFloat(states[i], -1.0f, 1.0f);
+    }
+
+    // Constrain velocities to what GPS can do for us
+    for (unsigned i = 4; i < 7; i++) {
+        states[i] = ConstrainFloat(states[i], -5.0e2f, 5.0e2f);
+    }
+
+    // Constrain position to a reasonable vehicle range (in meters)
+    for (unsigned i = 7; i < 9; i++) {
+        states[i] = ConstrainFloat(states[i], -1.0e6f, 1.0e6f);
+    }
+
+    // Constrain altitude
+    states[9] = ConstrainFloat(states[9], -4.0e4f, 1.0e4f);
+
+    // Angle bias limit - set to 8 degrees / sec
+    for (unsigned i = 10; i < 13; i++) {
+        states[i] = ConstrainFloat(states[i], -0.12f * dtIMU, 0.12f * dtIMU);
+    }
+
+    // Wind velocity limits - assume 120 m/s max velocity
+    for (unsigned i = 13; i < 15; i++) {
+        states[i] = ConstrainFloat(states[i], -120.0f, 120.0f);
+    }
+
+    // Earth magnetic field limits (in Gauss)
+    for (unsigned i = 15; i < 18; i++) {
+        states[i] = ConstrainFloat(states[i], -1.0f, 1.0f);
+    }
+
+    // Body magnetic field variances (in Gauss).
+    // the max offset should be in this range.
+    for (unsigned i = 18; i < 21; i++) {
+        states[i] = ConstrainFloat(states[i], -0.5f, 0.5f);
+    }
+
+}
+
+void AttPosEKF::ForceSymmetry()
+{
+    if (!numericalProtection) {
+        return;
+    }
+
+    // Force symmetry on the covariance matrix to prevent ill-conditioning
+    // of the matrix which would cause the filter to blow-up
+    for (unsigned i = 1; i < n_states; i++)
+    {
+        for (uint8_t j = 0; j < i; j++)
+        {
+            P[i][j] = 0.5f * (P[i][j] + P[j][i]);
+            P[j][i] = P[i][j];
+        }
+    }
+}
+
+bool AttPosEKF::FilterHealthy()
+{
+    if (!statesInitialised) {
+        return false;
+    }
+
+    // XXX Check state vector for NaNs and ill-conditioning
+
+    // Check if any of the major inputs timed out
+    if (current_ekf_state.posTimeout || current_ekf_state.velTimeout || current_ekf_state.hgtTimeout) {
+        return false;
+    }
+
+    // Nothing fired, return ok.
+    return true;
+}
+
+/**
+ * Reset the filter position states
+ *
+ * This resets the position to the last GPS measurement
+ * or to zero in case of static position.
+ */
+void AttPosEKF::ResetPosition(void)
+{
+    if (staticMode) {
+        states[7] = 0;
+        states[8] = 0;
+    } else if (GPSstatus >= GPS_FIX_3D) {
+
+        // reset the states from the GPS measurements
+        states[7] = posNE[0];
+        states[8] = posNE[1];
+    }
+}
+
+/**
+ * Reset the height state.
+ *
+ * This resets the height state with the last altitude measurements
+ */
+void AttPosEKF::ResetHeight(void)
+{
+    // write to the state vector
+    states[9]   = -hgtMea;
+}
+
+/**
+ * Reset the velocity state.
+ */
+void AttPosEKF::ResetVelocity(void)
+{
+    if (staticMode) {
+        states[4] = 0.0f;
+        states[5] = 0.0f;
+        states[6] = 0.0f;
+    } else if (GPSstatus >= GPS_FIX_3D) {
+
+        states[4]  = velNED[0]; // north velocity from last reading
+        states[5]  = velNED[1]; // east velocity from last reading
+        states[6]  = velNED[2]; // down velocity from last reading
+    }
+}
+
+
+void AttPosEKF::FillErrorReport(struct ekf_status_report *err)
+{
+    for (int i = 0; i < n_states; i++)
+    {
+        err->states[i] = states[i];
+    }
+
+    err->velHealth = current_ekf_state.velHealth;
+    err->posHealth = current_ekf_state.posHealth;
+    err->hgtHealth = current_ekf_state.hgtHealth;
+    err->velTimeout = current_ekf_state.velTimeout;
+    err->posTimeout = current_ekf_state.posTimeout;
+    err->hgtTimeout = current_ekf_state.hgtTimeout;
+}
+
+bool AttPosEKF::StatesNaN(struct ekf_status_report *err_report) {
+    bool err = false;
+
+    // check all states and covariance matrices
+    for (unsigned i = 0; i < n_states; i++) {
+        for (unsigned j = 0; j < n_states; j++) {
+            if (!isfinite(KH[i][j])) {
+
+                err_report->covarianceNaN = true;
+                err = true;
+            } //  intermediate result used for covariance updates
+            if (!isfinite(KHP[i][j])) {
+
+                err_report->covarianceNaN = true;
+                err = true;
+            } // intermediate result used for covariance updates
+            if (!isfinite(P[i][j])) {
+
+                err_report->covarianceNaN = true;
+                err = true;
+            } // covariance matrix
+        }
+
+        if (!isfinite(Kfusion[i])) {
+
+            err_report->kalmanGainsNaN = true;
+            err = true;
+        } // Kalman gains
+
+        if (!isfinite(states[i])) {
+
+            err_report->statesNaN = true;
+            err = true;
+        } // state matrix
+    }
+
+    if (err) {
+        FillErrorReport(err_report);
+    }
+
+    return err;
+
+}
+
+/**
+ * Check the filter inputs and bound its operational state
+ *
+ * This check will reset the filter states if required
+ * due to a failure of consistency or timeout checks.
+ * it should be run after the measurement data has been
+ * updated, but before any of the fusion steps are
+ * executed.
+ */
+int AttPosEKF::CheckAndBound()
+{
+
+    // Store the old filter state
+    bool currStaticMode = staticMode;
+
+    // Reset the filter if the states went NaN
+    if (StatesNaN(&last_ekf_error)) {
+
+        InitializeDynamic(velNED);
+
+        return 1;
+    }
+
+    // Reset the filter if the IMU data is too old
+    if (dtIMU > 0.2f) {
+
+        ResetVelocity();
+        ResetPosition();
+        ResetHeight();
+        ResetStoredStates();
+
+        // that's all we can do here, return
+        return 2;
+    }
+
+    // Check if we're on ground - this also sets static mode.
+    OnGroundCheck();
+
+    // Check if we switched between states
+    if (currStaticMode != staticMode) {
+        ResetVelocity();
+        ResetPosition();
+        ResetHeight();
+        ResetStoredStates();
+
+        return 3;
+    }
+
+    return 0;
+}
+
+void AttPosEKF::AttitudeInit(float ax, float ay, float az, float mx, float my, float mz, float *initQuat)
+{
+    float initialRoll, initialPitch;
+    float cosRoll, sinRoll, cosPitch, sinPitch;
+    float magX, magY;
+    float initialHdg, cosHeading, sinHeading;
+
+    initialRoll = atan2(-ay, -az);
+    initialPitch = atan2(ax, -az);
+
+    cosRoll = cosf(initialRoll);
+    sinRoll = sinf(initialRoll);
+    cosPitch = cosf(initialPitch);
+    sinPitch = sinf(initialPitch);
+
+    magX = mx * cosPitch + my * sinRoll * sinPitch + mz * cosRoll * sinPitch;
+
+    magY = my * cosRoll - mz * sinRoll;
+
+    initialHdg = atan2f(-magY, magX);
+
+    cosRoll = cosf(initialRoll * 0.5f);
+    sinRoll = sinf(initialRoll * 0.5f);
+
+    cosPitch = cosf(initialPitch * 0.5f);
+    sinPitch = sinf(initialPitch * 0.5f);
+
+    cosHeading = cosf(initialHdg * 0.5f);
+    sinHeading = sinf(initialHdg * 0.5f);
+
+    initQuat[0] = cosRoll * cosPitch * cosHeading + sinRoll * sinPitch * sinHeading;
+    initQuat[1] = sinRoll * cosPitch * cosHeading - cosRoll * sinPitch * sinHeading;
+    initQuat[2] = cosRoll * sinPitch * cosHeading + sinRoll * cosPitch * sinHeading;
+    initQuat[3] = cosRoll * cosPitch * sinHeading - sinRoll * sinPitch * cosHeading;
+}
+
+void AttPosEKF::InitializeDynamic(float (&initvelNED)[3])
+{
+
+    // Clear the init flag
+    statesInitialised = false;
+
+    ZeroVariables();
+
+    // Calculate initial filter quaternion states from raw measurements
+    float initQuat[4];
+    Vector3f initMagXYZ;
+    initMagXYZ   = magData - magBias;
+    AttitudeInit(accel.x, accel.y, accel.z, initMagXYZ.x, initMagXYZ.y, initMagXYZ.z, initQuat);
+
+    // Calculate initial Tbn matrix and rotate Mag measurements into NED
+    // to set initial NED magnetic field states
+    Mat3f DCM;
+    quat2Tbn(DCM, initQuat);
+    Vector3f initMagNED;
+    initMagXYZ   = magData - magBias;
+    initMagNED.x = DCM.x.x*initMagXYZ.x + DCM.x.y*initMagXYZ.y + DCM.x.z*initMagXYZ.z;
+    initMagNED.y = DCM.y.x*initMagXYZ.x + DCM.y.y*initMagXYZ.y + DCM.y.z*initMagXYZ.z;
+    initMagNED.z = DCM.z.x*initMagXYZ.x + DCM.z.y*initMagXYZ.y + DCM.z.z*initMagXYZ.z;
+
+
+
+    // write to state vector
+    for (uint8_t j=0; j<=3; j++) states[j] = initQuat[j]; // quaternions
+    for (uint8_t j=0; j<=2; j++) states[j+4] = initvelNED[j]; // velocities
+    for (uint8_t j=0; j<=7; j++) states[j+7] = 0.0f; // positiions, dAngBias, windVel
+    states[15] = initMagNED.x; // Magnetic Field North
+    states[16] = initMagNED.y; // Magnetic Field East
+    states[17] = initMagNED.z; // Magnetic Field Down
+    states[18] = magBias.x; // Magnetic Field Bias X
+    states[19] = magBias.y; // Magnetic Field Bias Y
+    states[20] = magBias.z; // Magnetic Field Bias Z
+
+    statesInitialised = true;
+
+    // initialise the covariance matrix
+    CovarianceInit();
+
+    //Define Earth rotation vector in the NED navigation frame
+    calcEarthRateNED(earthRateNED, latRef);
+
+    //Initialise summed variables used by covariance prediction
+    summedDelAng.x = 0.0f;
+    summedDelAng.y = 0.0f;
+    summedDelAng.z = 0.0f;
+    summedDelVel.x = 0.0f;
+    summedDelVel.y = 0.0f;
+    summedDelVel.z = 0.0f;
+}
+
+void AttPosEKF::InitialiseFilter(float (&initvelNED)[3])
+{
+    //store initial lat,long and height
+    latRef = gpsLat;
+    lonRef = gpsLon;
+    hgtRef = gpsHgt;
+
+    memset(&last_ekf_error, 0, sizeof(last_ekf_error));
+
+    InitializeDynamic(initvelNED);
+}
+
+void AttPosEKF::ZeroVariables()
+{
+    // Do the data structure init
+    for (unsigned i = 0; i < n_states; i++) {
+        for (unsigned j = 0; j < n_states; j++) {
+            KH[i][j] = 0.0f; //  intermediate result used for covariance updates
+            KHP[i][j] = 0.0f; // intermediate result used for covariance updates
+            P[i][j] = 0.0f; // covariance matrix
+        }
+
+        Kfusion[i] = 0.0f; // Kalman gains
+        states[i] = 0.0f; // state matrix
+    }
+
+    for (unsigned i = 0; i < data_buffer_size; i++) {
+
+        for (unsigned j = 0; j < n_states; j++) {
+            storedStates[j][i] = 0.0f;
+        }
+
+        statetimeStamp[i] = 0;
+    }
+
+    memset(&current_ekf_state, 0, sizeof(current_ekf_state));
+}
+
+void AttPosEKF::GetFilterState(struct ekf_status_report *state)
+{
+    memcpy(state, &current_ekf_state, sizeof(state));
+}
+
+void AttPosEKF::GetLastErrorState(struct ekf_status_report *last_error)
+{
+    memcpy(last_error, &last_ekf_error, sizeof(last_error));
+}
diff --git a/src/modules/fw_att_pos_estimator/estimator.h b/src/modules/fw_att_pos_estimator/estimator.h
new file mode 100644
index 000000000..e62f1a98a
--- /dev/null
+++ b/src/modules/fw_att_pos_estimator/estimator.h
@@ -0,0 +1,259 @@
+#include <math.h>
+#include <stdint.h>
+
+#pragma once
+
+#define GRAVITY_MSS 9.80665f
+#define deg2rad 0.017453292f
+#define rad2deg 57.295780f
+#define pi 3.141592657f
+#define earthRate 0.000072921f
+#define earthRadius 6378145.0f
+#define earthRadiusInv  1.5678540e-7f
+
+class Vector3f
+{
+private:
+public:
+    float x;
+    float y;
+    float z;
+
+    float length(void) const;
+    Vector3f zero(void) const;
+};
+
+class Mat3f
+{
+private:
+public:
+    Vector3f x;
+    Vector3f y;
+    Vector3f z;
+
+    Mat3f();
+
+    Mat3f transpose(void) const;
+};
+
+Vector3f operator*(float sclIn1, Vector3f vecIn1);
+Vector3f operator+( Vector3f vecIn1, Vector3f vecIn2);
+Vector3f operator-( Vector3f vecIn1, Vector3f vecIn2);
+Vector3f operator*( Mat3f matIn, Vector3f vecIn);
+Vector3f operator%( Vector3f vecIn1, Vector3f vecIn2);
+Vector3f operator*(Vector3f vecIn1, float sclIn1);
+
+void swap_var(float &d1, float &d2);
+
+const unsigned int n_states = 21;
+const unsigned int data_buffer_size = 50;
+
+const float covTimeStepMax = 0.07f; // maximum time allowed between covariance predictions
+const float covDelAngMax = 0.02f; // maximum delta angle between covariance predictions
+
+// extern bool staticMode;
+
+enum GPS_FIX {
+    GPS_FIX_NOFIX = 0,
+    GPS_FIX_2D = 2,
+    GPS_FIX_3D = 3
+};
+
+struct ekf_status_report {
+    bool velHealth;
+    bool posHealth;
+    bool hgtHealth;
+    bool velTimeout;
+    bool posTimeout;
+    bool hgtTimeout;
+    uint32_t velFailTime;
+    uint32_t posFailTime;
+    uint32_t hgtFailTime;
+    float states[n_states];
+    bool statesNaN;
+    bool covarianceNaN;
+    bool kalmanGainsNaN;
+};
+
+class AttPosEKF {
+
+public:
+
+    AttPosEKF();
+    ~AttPosEKF();
+
+    // Global variables
+    float KH[n_states][n_states]; //  intermediate result used for covariance updates
+    float KHP[n_states][n_states]; // intermediate result used for covariance updates
+    float P[n_states][n_states]; // covariance matrix
+    float Kfusion[n_states]; // Kalman gains
+    float states[n_states]; // state matrix
+    float storedStates[n_states][data_buffer_size]; // state vectors stored for the last 50 time steps
+    uint32_t statetimeStamp[data_buffer_size]; // time stamp for each state vector stored
+
+    float statesAtVelTime[n_states]; // States at the effective measurement time for posNE and velNED measurements
+    float statesAtPosTime[n_states]; // States at the effective measurement time for posNE and velNED measurements
+    float statesAtHgtTime[n_states]; // States at the effective measurement time for the hgtMea measurement
+    float statesAtMagMeasTime[n_states]; // filter satates at the effective measurement time
+    float statesAtVtasMeasTime[n_states]; // filter states at the effective measurement time
+
+    Vector3f correctedDelAng; // delta angles about the xyz body axes corrected for errors (rad)
+    Vector3f correctedDelVel; // delta velocities along the XYZ body axes corrected for errors (m/s)
+    Vector3f summedDelAng; // summed delta angles about the xyz body axes corrected for errors (rad)
+    Vector3f summedDelVel; // summed delta velocities along the XYZ body axes corrected for errors (m/s)
+    float accNavMag; // magnitude of navigation accel (- used to adjust GPS obs variance (m/s^2)
+    Vector3f earthRateNED; // earths angular rate vector in NED (rad/s)
+    Vector3f angRate; // angular rate vector in XYZ body axes measured by the IMU (rad/s)
+    Vector3f accel; // acceleration vector in XYZ body axes measured by the IMU (m/s^2)
+    Vector3f dVelIMU;
+    Vector3f dAngIMU;
+    float dtIMU; // time lapsed since the last IMU measurement or covariance update (sec)
+    uint8_t fusionModeGPS; // 0 = GPS outputs 3D velocity, 1 = GPS outputs 2D velocity, 2 = GPS outputs no velocity
+    float innovVelPos[6]; // innovation output
+    float varInnovVelPos[6]; // innovation variance output
+
+    float velNED[3]; // North, East, Down velocity obs (m/s)
+    float posNE[2]; // North, East position obs (m)
+    float hgtMea; //  measured height (m)
+    float posNED[3]; // North, East Down position (m)
+
+    float innovMag[3]; // innovation output
+    float varInnovMag[3]; // innovation variance output
+    Vector3f magData; // magnetometer flux radings in X,Y,Z body axes
+    float innovVtas; // innovation output
+    float varInnovVtas; // innovation variance output
+    float VtasMeas; // true airspeed measurement (m/s)
+    float latRef; // WGS-84 latitude of reference point (rad)
+    float lonRef; // WGS-84 longitude of reference point (rad)
+    float hgtRef; // WGS-84 height of reference point (m)
+    Vector3f magBias; // states representing magnetometer bias vector in XYZ body axes
+    uint8_t covSkipCount; // Number of state prediction frames (IMU daya updates to skip before doing the covariance prediction
+    float EAS2TAS; // ratio f true to equivalent airspeed
+
+    // GPS input data variables
+    float gpsCourse;
+    float gpsVelD;
+    float gpsLat;
+    float gpsLon;
+    float gpsHgt;
+    uint8_t GPSstatus;
+
+    // Baro input
+    float baroHgt;
+
+    bool statesInitialised;
+
+    bool fuseVelData; // this boolean causes the posNE and velNED obs to be fused
+    bool fusePosData; // this boolean causes the posNE and velNED obs to be fused
+    bool fuseHgtData; // this boolean causes the hgtMea obs to be fused
+    bool fuseMagData; // boolean true when magnetometer data is to be fused
+    bool fuseVtasData; // boolean true when airspeed data is to be fused
+
+    bool onGround;    ///< boolean true when the flight vehicle is on the ground (not flying)
+    bool staticMode;    ///< boolean true if no position feedback is fused
+    bool useAirspeed;    ///< boolean true if airspeed data is being used
+    bool useCompass;    ///< boolean true if magnetometer data is being used
+
+    struct ekf_status_report current_ekf_state;
+    struct ekf_status_report last_ekf_error;
+
+    bool numericalProtection;
+
+    unsigned storeIndex;
+
+
+void  UpdateStrapdownEquationsNED();
+
+void CovariancePrediction(float dt);
+
+void FuseVelposNED();
+
+void FuseMagnetometer();
+
+void FuseAirspeed();
+
+void zeroRows(float (&covMat)[n_states][n_states], uint8_t first, uint8_t last);
+
+void zeroCols(float (&covMat)[n_states][n_states], uint8_t first, uint8_t last);
+
+void quatNorm(float (&quatOut)[4], const float quatIn[4]);
+
+// store staes along with system time stamp in msces
+void StoreStates(uint64_t timestamp_ms);
+
+/**
+ * Recall the state vector.
+ *
+ * Recalls the vector stored at closest time to the one specified by msec
+ *
+ * @return zero on success, integer indicating the number of invalid states on failure.
+ *         Does only copy valid states, if the statesForFusion vector was initialized
+ *         correctly by the caller, the result can be safely used, but is a mixture
+ *         time-wise where valid states were updated and invalid remained at the old
+ *         value.
+ */
+int RecallStates(float statesForFusion[n_states], uint64_t msec);
+
+void ResetStoredStates();
+
+void quat2Tbn(Mat3f &Tbn, const float (&quat)[4]);
+
+void calcEarthRateNED(Vector3f &omega, float latitude);
+
+static void eul2quat(float (&quat)[4], const float (&eul)[3]);
+
+static void quat2eul(float (&eul)[3], const float (&quat)[4]);
+
+static void calcvelNED(float (&velNED)[3], float gpsCourse, float gpsGndSpd, float gpsVelD);
+
+static void calcposNED(float (&posNED)[3], float lat, float lon, float hgt, float latRef, float lonRef, float hgtRef);
+
+static void calcLLH(float (&posNED)[3], float lat, float lon, float hgt, float latRef, float lonRef, float hgtRef);
+
+static void quat2Tnb(Mat3f &Tnb, const float (&quat)[4]);
+
+static float sq(float valIn);
+
+void OnGroundCheck();
+
+void CovarianceInit();
+
+void InitialiseFilter(float (&initvelNED)[3]);
+
+float ConstrainFloat(float val, float min, float max);
+
+void ConstrainVariances();
+
+void ConstrainStates();
+
+void ForceSymmetry();
+
+int CheckAndBound();
+
+void ResetPosition();
+
+void ResetVelocity();
+
+void ZeroVariables();
+
+void GetFilterState(struct ekf_status_report *state);
+
+void GetLastErrorState(struct ekf_status_report *last_error);
+
+bool StatesNaN(struct ekf_status_report *err_report);
+void FillErrorReport(struct ekf_status_report *err);
+
+void InitializeDynamic(float (&initvelNED)[3]);
+
+protected:
+
+bool FilterHealthy();
+
+void ResetHeight(void);
+
+void AttitudeInit(float ax, float ay, float az, float mx, float my, float mz, float *initQuat);
+
+};
+
+uint32_t millis();
+
diff --git a/src/modules/fw_att_pos_estimator/fw_att_pos_estimator_main.cpp b/src/modules/fw_att_pos_estimator/fw_att_pos_estimator_main.cpp
new file mode 100644
index 000000000..840cd585e
--- /dev/null
+++ b/src/modules/fw_att_pos_estimator/fw_att_pos_estimator_main.cpp
@@ -0,0 +1,1273 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2013, 2014 PX4 Development Team. All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in
+ *    the documentation and/or other materials provided with the
+ *    distribution.
+ * 3. Neither the name PX4 nor the names of its contributors may be
+ *    used to endorse or promote products derived from this software
+ *    without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ ****************************************************************************/
+
+/**
+ * @file fw_att_pos_estimator_main.cpp
+ * Implementation of the attitude and position estimator.
+ *
+ * @author Paul Riseborough <p_riseborough@live.com.au>
+ * @author Lorenz Meier <lm@inf.ethz.ch>
+ */
+
+#include <nuttx/config.h>
+#include <stdio.h>
+#include <stdlib.h>
+#include <string.h>
+#include <unistd.h>
+#include <fcntl.h>
+#include <errno.h>
+#include <math.h>
+#include <poll.h>
+#include <time.h>
+#include <drivers/drv_hrt.h>
+
+#define SENSOR_COMBINED_SUB
+
+
+#include <drivers/drv_gyro.h>
+#include <drivers/drv_accel.h>
+#include <drivers/drv_mag.h>
+#include <drivers/drv_baro.h>
+#ifdef SENSOR_COMBINED_SUB
+#include <uORB/topics/sensor_combined.h>
+#endif
+#include <arch/board/board.h>
+#include <uORB/uORB.h>
+#include <uORB/topics/airspeed.h>
+#include <uORB/topics/vehicle_global_position.h>
+#include <uORB/topics/vehicle_local_position.h>
+#include <uORB/topics/vehicle_gps_position.h>
+#include <uORB/topics/vehicle_attitude.h>
+#include <uORB/topics/actuator_controls.h>
+#include <uORB/topics/vehicle_status.h>
+#include <uORB/topics/parameter_update.h>
+#include <uORB/topics/estimator_status.h>
+#include <uORB/topics/actuator_armed.h>
+#include <systemlib/param/param.h>
+#include <systemlib/err.h>
+#include <geo/geo.h>
+#include <systemlib/perf_counter.h>
+#include <systemlib/systemlib.h>
+#include <mathlib/mathlib.h>
+#include <mavlink/mavlink_log.h>
+
+#include "estimator.h"
+
+
+
+/**
+ * estimator app start / stop handling function
+ *
+ * @ingroup apps
+ */
+extern "C" __EXPORT int fw_att_pos_estimator_main(int argc, char *argv[]);
+
+__EXPORT uint32_t millis();
+
+static uint64_t last_run = 0;
+static uint64_t IMUmsec = 0;
+
+uint32_t millis()
+{
+	return IMUmsec;
+}
+
+static void print_status();
+
+class FixedwingEstimator
+{
+public:
+	/**
+	 * Constructor
+	 */
+	FixedwingEstimator();
+
+	/**
+	 * Destructor, also kills the sensors task.
+	 */
+	~FixedwingEstimator();
+
+	/**
+	 * Start the sensors task.
+	 *
+	 * @return		OK on success.
+	 */
+	int		start();
+
+	/**
+	 * Print the current status.
+	 */
+	void		print_status();
+
+	/**
+	 * Trip the filter by feeding it NaN values.
+	 */
+	int		trip_nan();
+
+private:
+
+	bool		_task_should_exit;		/**< if true, sensor task should exit */
+	int		_estimator_task;			/**< task handle for sensor task */
+#ifndef SENSOR_COMBINED_SUB
+	int		_gyro_sub;			/**< gyro sensor subscription */
+	int		_accel_sub;			/**< accel sensor subscription */
+	int		_mag_sub;			/**< mag sensor subscription */
+#else
+	int		_sensor_combined_sub;
+#endif
+	int		_airspeed_sub;			/**< airspeed subscription */
+	int		_baro_sub;			/**< barometer subscription */
+	int		_gps_sub;			/**< GPS subscription */
+	int		_vstatus_sub;			/**< vehicle status subscription */
+	int 		_params_sub;			/**< notification of parameter updates */
+	int 		_manual_control_sub;		/**< notification of manual control updates */
+	int		_mission_sub;
+
+	orb_advert_t	_att_pub;			/**< vehicle attitude */
+	orb_advert_t	_global_pos_pub;		/**< global position */
+	orb_advert_t	_local_pos_pub;			/**< position in local frame */
+	orb_advert_t	_estimator_status_pub;		/**< status of the estimator */
+
+	struct vehicle_attitude_s			_att;			/**< vehicle attitude */
+	struct gyro_report				_gyro;
+	struct accel_report				_accel;
+	struct mag_report				_mag;
+	struct airspeed_s				_airspeed;		/**< airspeed */
+	struct baro_report				_baro;			/**< baro readings */
+	struct vehicle_status_s				_vstatus;		/**< vehicle status */
+	struct vehicle_global_position_s		_global_pos;		/**< global vehicle position */
+	struct vehicle_local_position_s			_local_pos;		/**< local vehicle position */
+	struct vehicle_gps_position_s			_gps;			/**< GPS position */
+
+	struct gyro_scale				_gyro_offsets;
+	struct accel_scale				_accel_offsets;
+	struct mag_scale				_mag_offsets;
+
+#ifdef SENSOR_COMBINED_SUB
+	struct sensor_combined_s			_sensor_combined;
+#endif
+
+	float						_baro_ref;		/**< barometer reference altitude */
+	float						_baro_gps_offset;	/**< offset between GPS and baro */
+
+	perf_counter_t	_loop_perf;			/**< loop performance counter */
+	perf_counter_t	_perf_gyro;			///<local performance counter for gyro updates
+	perf_counter_t	_perf_accel;			///<local performance counter for accel updates
+	perf_counter_t	_perf_mag;			///<local performance counter for mag updates
+	perf_counter_t	_perf_gps;			///<local performance counter for gps updates
+	perf_counter_t	_perf_baro;			///<local performance counter for baro updates
+	perf_counter_t	_perf_airspeed;			///<local performance counter for airspeed updates
+
+	bool						_initialized;
+	bool						_gps_initialized;
+
+	int						_mavlink_fd;
+
+	struct {
+		int32_t	vel_delay_ms;
+		int32_t	pos_delay_ms;
+		int32_t	height_delay_ms;
+		int32_t	mag_delay_ms;
+		int32_t	tas_delay_ms;
+	}		_parameters;			/**< local copies of interesting parameters */
+
+	struct {
+		param_t vel_delay_ms;
+		param_t	pos_delay_ms;
+		param_t	height_delay_ms;
+		param_t	mag_delay_ms;
+		param_t	tas_delay_ms;
+	}		_parameter_handles;		/**< handles for interesting parameters */
+
+	AttPosEKF					*_ekf;
+
+	/**
+	 * Update our local parameter cache.
+	 */
+	int		parameters_update();
+
+	/**
+	 * Update control outputs
+	 *
+	 */
+	void		control_update();
+
+	/**
+	 * Check for changes in vehicle status.
+	 */
+	void		vehicle_status_poll();
+
+	/**
+	 * Shim for calling task_main from task_create.
+	 */
+	static void	task_main_trampoline(int argc, char *argv[]);
+
+	/**
+	 * Main sensor collection task.
+	 */
+	void		task_main() __attribute__((noreturn));
+};
+
+namespace estimator
+{
+
+/* oddly, ERROR is not defined for c++ */
+#ifdef ERROR
+# undef ERROR
+#endif
+static const int ERROR = -1;
+
+FixedwingEstimator	*g_estimator;
+}
+
+FixedwingEstimator::FixedwingEstimator() :
+
+	_task_should_exit(false),
+	_estimator_task(-1),
+
+/* subscriptions */
+#ifndef SENSOR_COMBINED_SUB
+	_gyro_sub(-1),
+	_accel_sub(-1),
+	_mag_sub(-1),
+#else
+	_sensor_combined_sub(-1),
+#endif
+	_airspeed_sub(-1),
+	_baro_sub(-1),
+	_gps_sub(-1),
+	_vstatus_sub(-1),
+	_params_sub(-1),
+	_manual_control_sub(-1),
+
+/* publications */
+	_att_pub(-1),
+	_global_pos_pub(-1),
+	_local_pos_pub(-1),
+	_estimator_status_pub(-1),
+
+	_baro_ref(0.0f),
+	_baro_gps_offset(0.0f),
+
+/* performance counters */
+	_loop_perf(perf_alloc(PC_COUNT, "fw_att_pos_estimator")),
+	_perf_gyro(perf_alloc(PC_COUNT, "fw_ekf_gyro_upd")),
+	_perf_accel(perf_alloc(PC_COUNT, "fw_ekf_accel_upd")),
+	_perf_mag(perf_alloc(PC_COUNT, "fw_ekf_mag_upd")),
+	_perf_gps(perf_alloc(PC_COUNT, "fw_ekf_gps_upd")),
+	_perf_baro(perf_alloc(PC_COUNT, "fw_ekf_baro_upd")),
+	_perf_airspeed(perf_alloc(PC_COUNT, "fw_ekf_aspd_upd")),
+
+/* states */
+	_initialized(false),
+	_gps_initialized(false),
+	_mavlink_fd(-1),
+	_ekf(nullptr)
+{
+
+	_mavlink_fd = open(MAVLINK_LOG_DEVICE, 0);
+
+	_parameter_handles.vel_delay_ms = param_find("PE_VEL_DELAY_MS");
+	_parameter_handles.pos_delay_ms = param_find("PE_POS_DELAY_MS");
+	_parameter_handles.height_delay_ms = param_find("PE_HGT_DELAY_MS");
+	_parameter_handles.mag_delay_ms = param_find("PE_MAG_DELAY_MS");
+	_parameter_handles.tas_delay_ms = param_find("PE_TAS_DELAY_MS");
+
+	/* fetch initial parameter values */
+	parameters_update();
+
+	/* get offsets */
+
+	int fd, res;
+
+	fd = open(GYRO_DEVICE_PATH, O_RDONLY);
+
+	if (fd > 0) {
+		res = ioctl(fd, GYROIOCGSCALE, (long unsigned int)&_gyro_offsets);
+		close(fd);
+	}
+
+	fd = open(ACCEL_DEVICE_PATH, O_RDONLY);
+
+	if (fd > 0) {
+		res = ioctl(fd, ACCELIOCGSCALE, (long unsigned int)&_accel_offsets);
+		close(fd);
+	}
+
+	fd = open(MAG_DEVICE_PATH, O_RDONLY);
+
+	if (fd > 0) {
+		res = ioctl(fd, MAGIOCGSCALE, (long unsigned int)&_mag_offsets);
+		close(fd);
+	}
+}
+
+FixedwingEstimator::~FixedwingEstimator()
+{
+	if (_estimator_task != -1) {
+
+		/* task wakes up every 100ms or so at the longest */
+		_task_should_exit = true;
+
+		/* wait for a second for the task to quit at our request */
+		unsigned i = 0;
+
+		do {
+			/* wait 20ms */
+			usleep(20000);
+
+			/* if we have given up, kill it */
+			if (++i > 50) {
+				task_delete(_estimator_task);
+				break;
+			}
+		} while (_estimator_task != -1);
+	}
+
+	estimator::g_estimator = nullptr;
+}
+
+int
+FixedwingEstimator::parameters_update()
+{
+
+	param_get(_parameter_handles.vel_delay_ms, &(_parameters.vel_delay_ms));
+	param_get(_parameter_handles.pos_delay_ms, &(_parameters.pos_delay_ms));
+	param_get(_parameter_handles.height_delay_ms, &(_parameters.height_delay_ms));
+	param_get(_parameter_handles.mag_delay_ms, &(_parameters.mag_delay_ms));
+	param_get(_parameter_handles.tas_delay_ms, &(_parameters.tas_delay_ms));
+
+	return OK;
+}
+
+void
+FixedwingEstimator::vehicle_status_poll()
+{
+	bool vstatus_updated;
+
+	/* Check HIL state if vehicle status has changed */
+	orb_check(_vstatus_sub, &vstatus_updated);
+
+	if (vstatus_updated) {
+
+		orb_copy(ORB_ID(vehicle_status), _vstatus_sub, &_vstatus);
+	}
+}
+
+void
+FixedwingEstimator::task_main_trampoline(int argc, char *argv[])
+{
+	estimator::g_estimator->task_main();
+}
+
+float dt = 0.0f; // time lapsed since last covariance prediction
+
+void
+FixedwingEstimator::task_main()
+{
+
+	_ekf = new AttPosEKF();
+
+	if (!_ekf) {
+		errx(1, "failed allocating EKF filter - out of RAM!");
+	}
+
+	/*
+	 * do subscriptions
+	 */
+	_baro_sub = orb_subscribe(ORB_ID(sensor_baro));
+	_airspeed_sub = orb_subscribe(ORB_ID(airspeed));
+	_gps_sub = orb_subscribe(ORB_ID(vehicle_gps_position));
+	_vstatus_sub = orb_subscribe(ORB_ID(vehicle_status));
+	_params_sub = orb_subscribe(ORB_ID(parameter_update));
+
+	/* rate limit vehicle status updates to 5Hz */
+	orb_set_interval(_vstatus_sub, 200);
+
+#ifndef SENSOR_COMBINED_SUB
+
+	_gyro_sub = orb_subscribe(ORB_ID(sensor_gyro));
+	_accel_sub = orb_subscribe(ORB_ID(sensor_accel));
+	_mag_sub = orb_subscribe(ORB_ID(sensor_mag));
+
+	/* rate limit gyro updates to 50 Hz */
+	/* XXX remove this!, BUT increase the data buffer size! */
+	orb_set_interval(_gyro_sub, 4);
+#else
+	_sensor_combined_sub = orb_subscribe(ORB_ID(sensor_combined));
+	/* XXX remove this!, BUT increase the data buffer size! */
+	orb_set_interval(_sensor_combined_sub, 4);
+#endif
+
+	parameters_update();
+
+	/* set initial filter state */
+	_ekf->fuseVelData = false;
+	_ekf->fusePosData = false;
+	_ekf->fuseHgtData = false;
+	_ekf->fuseMagData = false;
+	_ekf->fuseVtasData = false;
+	_ekf->statesInitialised = false;
+
+	/* initialize measurement data */
+	_ekf->VtasMeas = 0.0f;
+	Vector3f lastAngRate = {0.0f, 0.0f, 0.0f};
+	Vector3f lastAccel = {0.0f, 0.0f, -9.81f};
+	_ekf->dVelIMU.x = 0.0f;
+	_ekf->dVelIMU.y = 0.0f;
+	_ekf->dVelIMU.z = 0.0f;
+	_ekf->dAngIMU.x = 0.0f;
+	_ekf->dAngIMU.y = 0.0f;
+	_ekf->dAngIMU.z = 0.0f;
+
+	/* wakeup source(s) */
+	struct pollfd fds[2];
+
+	/* Setup of loop */
+	fds[0].fd = _params_sub;
+	fds[0].events = POLLIN;
+#ifndef SENSOR_COMBINED_SUB
+	fds[1].fd = _gyro_sub;
+	fds[1].events = POLLIN;
+#else
+	fds[1].fd = _sensor_combined_sub;
+	fds[1].events = POLLIN;
+#endif
+
+	hrt_abstime start_time = hrt_absolute_time();
+
+	bool newDataGps = false;
+	bool newAdsData = false;
+	bool newDataMag = false;
+
+	while (!_task_should_exit) {
+
+		/* wait for up to 500ms for data */
+		int pret = poll(&fds[0], (sizeof(fds) / sizeof(fds[0])), 100);
+
+		/* timed out - periodic check for _task_should_exit, etc. */
+		if (pret == 0)
+			continue;
+
+		/* this is undesirable but not much we can do - might want to flag unhappy status */
+		if (pret < 0) {
+			warn("poll error %d, %d", pret, errno);
+			continue;
+		}
+
+		perf_begin(_loop_perf);
+
+		/* only update parameters if they changed */
+		if (fds[0].revents & POLLIN) {
+			/* read from param to clear updated flag */
+			struct parameter_update_s update;
+			orb_copy(ORB_ID(parameter_update), _params_sub, &update);
+
+			/* update parameters from storage */
+			parameters_update();
+		}
+
+		/* only run estimator if gyro updated */
+		if (fds[1].revents & POLLIN) {
+
+			/* check vehicle status for changes to publication state */
+			vehicle_status_poll();
+
+			bool accel_updated;
+			bool mag_updated;
+
+			perf_count(_perf_gyro);
+
+			/**
+			 *    PART ONE: COLLECT ALL DATA
+			 **/
+
+			hrt_abstime last_sensor_timestamp;
+
+			/* load local copies */
+#ifndef SENSOR_COMBINED_SUB
+			orb_copy(ORB_ID(sensor_gyro), _gyro_sub, &_gyro);
+
+
+			orb_check(_accel_sub, &accel_updated);
+
+			if (accel_updated) {
+				perf_count(_perf_accel);
+				orb_copy(ORB_ID(sensor_accel), _accel_sub, &_accel);
+			}
+
+			last_sensor_timestamp = _gyro.timestamp;
+			_ekf.IMUmsec = _gyro.timestamp / 1e3f;
+
+			float deltaT = (_gyro.timestamp - last_run) / 1e6f;
+			last_run = _gyro.timestamp;
+
+			/* guard against too large deltaT's */
+			if (deltaT > 1.0f)
+				deltaT = 0.01f;
+
+
+			// Always store data, independent of init status
+			/* fill in last data set */
+			_ekf->dtIMU = deltaT;
+
+			_ekf->angRate.x = _gyro.x;
+			_ekf->angRate.y = _gyro.y;
+			_ekf->angRate.z = _gyro.z;
+
+			_ekf->accel.x = _accel.x;
+			_ekf->accel.y = _accel.y;
+			_ekf->accel.z = _accel.z;
+
+			_ekf->dAngIMU = 0.5f * (angRate + lastAngRate) * dtIMU;
+			_ekf->lastAngRate = angRate;
+			_ekf->dVelIMU = 0.5f * (accel + lastAccel) * dtIMU;
+			_ekf->lastAccel = accel;
+
+
+#else
+			orb_copy(ORB_ID(sensor_combined), _sensor_combined_sub, &_sensor_combined);
+
+			static hrt_abstime last_accel = 0;
+			static hrt_abstime last_mag = 0;
+
+			if (last_accel != _sensor_combined.accelerometer_timestamp) {
+				accel_updated = true;
+			}
+
+			last_accel = _sensor_combined.accelerometer_timestamp;
+
+
+			// Copy gyro and accel
+			last_sensor_timestamp = _sensor_combined.timestamp;
+			IMUmsec = _sensor_combined.timestamp / 1e3f;
+
+			float deltaT = (_sensor_combined.timestamp - last_run) / 1e6f;
+			last_run = _sensor_combined.timestamp;
+
+			/* guard against too large deltaT's */
+			if (deltaT > 1.0f || deltaT < 0.000001f)
+				deltaT = 0.01f;
+
+			// Always store data, independent of init status
+			/* fill in last data set */
+			_ekf->dtIMU = deltaT;
+
+			_ekf->angRate.x = _sensor_combined.gyro_rad_s[0];
+			_ekf->angRate.y = _sensor_combined.gyro_rad_s[1];
+			_ekf->angRate.z = _sensor_combined.gyro_rad_s[2];
+
+			_ekf->accel.x = _sensor_combined.accelerometer_m_s2[0];
+			_ekf->accel.y = _sensor_combined.accelerometer_m_s2[1];
+			_ekf->accel.z = _sensor_combined.accelerometer_m_s2[2];
+
+			_ekf->dAngIMU = 0.5f * (_ekf->angRate + lastAngRate) * _ekf->dtIMU;
+			lastAngRate = _ekf->angRate;
+			_ekf->dVelIMU = 0.5f * (_ekf->accel + lastAccel) * _ekf->dtIMU;
+			lastAccel = _ekf->accel;
+
+			if (last_mag != _sensor_combined.magnetometer_timestamp) {
+				mag_updated = true;
+				newDataMag = true;
+
+			} else {
+				newDataMag = false;
+			}
+
+			last_mag = _sensor_combined.magnetometer_timestamp;
+
+#endif
+
+			bool airspeed_updated;
+			orb_check(_airspeed_sub, &airspeed_updated);
+
+			if (airspeed_updated) {
+				orb_copy(ORB_ID(airspeed), _airspeed_sub, &_airspeed);
+				perf_count(_perf_airspeed);
+
+				_ekf->VtasMeas = _airspeed.true_airspeed_m_s;
+				newAdsData = true;
+
+			} else {
+				newAdsData = false;
+			}
+
+			bool gps_updated;
+			orb_check(_gps_sub, &gps_updated);
+
+			if (gps_updated) {
+
+				uint64_t last_gps = _gps.timestamp_position;
+
+				orb_copy(ORB_ID(vehicle_gps_position), _gps_sub, &_gps);
+				perf_count(_perf_gps);
+
+				if (_gps.fix_type < 3) {
+					gps_updated = false;
+					newDataGps = false;
+
+				} else {
+
+					/* check if we had a GPS outage for a long time */
+					if (hrt_elapsed_time(&last_gps) > 5 * 1000 * 1000) {
+						_ekf->ResetPosition();
+						_ekf->ResetVelocity();
+						_ekf->ResetStoredStates();
+					}
+
+					/* fuse GPS updates */
+
+					//_gps.timestamp / 1e3;
+					_ekf->GPSstatus = _gps.fix_type;
+					_ekf->velNED[0] = _gps.vel_n_m_s;
+					_ekf->velNED[1] = _gps.vel_e_m_s;
+					_ekf->velNED[2] = _gps.vel_d_m_s;
+
+					// warnx("GPS updated: status: %d, vel: %8.4f %8.4f %8.4f", (int)GPSstatus, velNED[0], velNED[1], velNED[2]);
+
+					_ekf->gpsLat = math::radians(_gps.lat / (double)1e7);
+					_ekf->gpsLon = math::radians(_gps.lon / (double)1e7) - M_PI;
+					_ekf->gpsHgt = _gps.alt / 1e3f;
+					newDataGps = true;
+
+				}
+
+			}
+
+			bool baro_updated;
+			orb_check(_baro_sub, &baro_updated);
+
+			if (baro_updated) {
+				orb_copy(ORB_ID(sensor_baro), _baro_sub, &_baro);
+
+				_ekf->baroHgt = _baro.altitude - _baro_ref;
+
+				// Could use a blend of GPS and baro alt data if desired
+				_ekf->hgtMea = 1.0f * _ekf->baroHgt + 0.0f * _ekf->gpsHgt;
+			}
+
+#ifndef SENSOR_COMBINED_SUB
+			orb_check(_mag_sub, &mag_updated);
+#endif
+
+			if (mag_updated) {
+
+				perf_count(_perf_mag);
+
+#ifndef SENSOR_COMBINED_SUB
+				orb_copy(ORB_ID(sensor_mag), _mag_sub, &_mag);
+
+				// XXX we compensate the offsets upfront - should be close to zero.
+				// 0.001f
+				_ekf->magData.x = _mag.x;
+				_ekf->magBias.x = 0.000001f; // _mag_offsets.x_offset
+
+				_ekf->magData.y = _mag.y;
+				_ekf->magBias.y = 0.000001f; // _mag_offsets.y_offset
+
+				_ekf->magData.z = _mag.z;
+				_ekf->magBias.z = 0.000001f; // _mag_offsets.y_offset
+
+#else
+
+				// XXX we compensate the offsets upfront - should be close to zero.
+				// 0.001f
+				_ekf->magData.x = _sensor_combined.magnetometer_ga[0];
+				_ekf->magBias.x = 0.000001f; // _mag_offsets.x_offset
+
+				_ekf->magData.y = _sensor_combined.magnetometer_ga[1];
+				_ekf->magBias.y = 0.000001f; // _mag_offsets.y_offset
+
+				_ekf->magData.z = _sensor_combined.magnetometer_ga[2];
+				_ekf->magBias.z = 0.000001f; // _mag_offsets.y_offset
+
+#endif
+
+				newDataMag = true;
+
+			} else {
+				newDataMag = false;
+			}
+
+
+			/**
+			 *    CHECK IF THE INPUT DATA IS SANE
+			 */
+			int check = _ekf->CheckAndBound();
+
+			switch (check) {
+				case 0:
+					/* all ok */
+					break;
+				case 1:
+				{
+					const char* str = "NaN in states, resetting";
+					warnx(str);
+					mavlink_log_critical(_mavlink_fd, str);
+					break;
+				}
+				case 2:
+				{
+					const char* str = "stale IMU data, resetting";
+					warnx(str);
+					mavlink_log_critical(_mavlink_fd, str);
+					break;
+				}
+				case 3:
+				{
+					const char* str = "switching dynamic / static state";
+					warnx(str);
+					mavlink_log_critical(_mavlink_fd, str);
+					break;
+				}
+			}
+
+			// If non-zero, we got a problem
+			if (check) {
+
+				struct ekf_status_report ekf_report;
+
+				_ekf->GetLastErrorState(&ekf_report);
+
+				struct estimator_status_report rep;
+				memset(&rep, 0, sizeof(rep));
+				rep.timestamp = hrt_absolute_time();
+
+				rep.states_nan = ekf_report.statesNaN;
+				rep.covariance_nan = ekf_report.covarianceNaN;
+				rep.kalman_gain_nan = ekf_report.kalmanGainsNaN;
+
+				// Copy all states or at least all that we can fit
+				int i = 0;
+				unsigned ekf_n_states = (sizeof(ekf_report.states) / sizeof(ekf_report.states[0]));
+				unsigned max_states = (sizeof(rep.states) / sizeof(rep.states[0]));
+				rep.n_states = (ekf_n_states < max_states) ? ekf_n_states : max_states;
+
+				while ((i < ekf_n_states) && (i < max_states)) {
+
+					rep.states[i] = ekf_report.states[i];
+					i++;
+				}
+
+				if (_estimator_status_pub > 0) {
+					orb_publish(ORB_ID(estimator_status), _estimator_status_pub, &rep);
+				} else {
+					_estimator_status_pub = orb_advertise(ORB_ID(estimator_status), &rep);
+				}
+			}
+
+
+			/**
+			 *    PART TWO: EXECUTE THE FILTER
+			 **/
+
+			// Wait long enough to ensure all sensors updated once
+			// XXX we rather want to check all updated
+
+
+			if (hrt_elapsed_time(&start_time) > 100000) {
+
+				if (!_gps_initialized && (_ekf->GPSstatus == 3)) {
+					_ekf->velNED[0] = _gps.vel_n_m_s;
+					_ekf->velNED[1] = _gps.vel_e_m_s;
+					_ekf->velNED[2] = _gps.vel_d_m_s;
+
+					double lat = _gps.lat * 1e-7;
+					double lon = _gps.lon * 1e-7;
+					float alt = _gps.alt * 1e-3;
+
+					_ekf->InitialiseFilter(_ekf->velNED);
+
+					// Initialize projection
+					_local_pos.ref_lat = _gps.lat;
+					_local_pos.ref_lon = _gps.lon;
+					_local_pos.ref_alt = alt;
+					_local_pos.ref_timestamp = _gps.timestamp_position;
+
+					// Store 
+					orb_copy(ORB_ID(sensor_baro), _baro_sub, &_baro);
+					_baro_ref = _baro.altitude;
+					_ekf->baroHgt = _baro.altitude - _baro_ref;
+					_baro_gps_offset = _baro_ref - _local_pos.ref_alt;
+
+					// XXX this is not multithreading safe
+					map_projection_init(lat, lon);
+					mavlink_log_info(_mavlink_fd, "[position estimator] init ref: lat=%.7f, lon=%.7f, alt=%.2f", lat, lon, alt);
+
+					_gps_initialized = true;
+
+				} else if (!_ekf->statesInitialised) {
+					_ekf->velNED[0] = 0.0f;
+					_ekf->velNED[1] = 0.0f;
+					_ekf->velNED[2] = 0.0f;
+					_ekf->posNED[0] = 0.0f;
+					_ekf->posNED[1] = 0.0f;
+					_ekf->posNED[2] = 0.0f;
+
+					_ekf->posNE[0] = _ekf->posNED[0];
+					_ekf->posNE[1] = _ekf->posNED[1];
+					_ekf->InitialiseFilter(_ekf->velNED);
+				}
+			}
+
+			// If valid IMU data and states initialised, predict states and covariances
+			if (_ekf->statesInitialised) {
+				// Run the strapdown INS equations every IMU update
+				_ekf->UpdateStrapdownEquationsNED();
+#if 0
+				// debug code - could be tunred into a filter mnitoring/watchdog function
+				float tempQuat[4];
+
+				for (uint8_t j = 0; j <= 3; j++) tempQuat[j] = states[j];
+
+				quat2eul(eulerEst, tempQuat);
+
+				for (uint8_t j = 0; j <= 2; j++) eulerDif[j] = eulerEst[j] - ahrsEul[j];
+
+				if (eulerDif[2] > pi) eulerDif[2] -= 2 * pi;
+
+				if (eulerDif[2] < -pi) eulerDif[2] += 2 * pi;
+
+#endif
+				// store the predicted states for subsequent use by measurement fusion
+				_ekf->StoreStates(IMUmsec);
+				// Check if on ground - status is used by covariance prediction
+				_ekf->OnGroundCheck();
+				// sum delta angles and time used by covariance prediction
+				_ekf->summedDelAng = _ekf->summedDelAng + _ekf->correctedDelAng;
+				_ekf->summedDelVel = _ekf->summedDelVel + _ekf->dVelIMU;
+				dt += _ekf->dtIMU;
+
+				// perform a covariance prediction if the total delta angle has exceeded the limit
+				// or the time limit will be exceeded at the next IMU update
+				if ((dt >= (covTimeStepMax - _ekf->dtIMU)) || (_ekf->summedDelAng.length() > covDelAngMax)) {
+					_ekf->CovariancePrediction(dt);
+					_ekf->summedDelAng = _ekf->summedDelAng.zero();
+					_ekf->summedDelVel = _ekf->summedDelVel.zero();
+					dt = 0.0f;
+				}
+
+				_initialized = true;
+			}
+
+			// Fuse GPS Measurements
+			if (newDataGps && _gps_initialized) {
+				// Convert GPS measurements to Pos NE, hgt and Vel NED
+				_ekf->velNED[0] = _gps.vel_n_m_s;
+				_ekf->velNED[1] = _gps.vel_e_m_s;
+				_ekf->velNED[2] = _gps.vel_d_m_s;
+				_ekf->calcposNED(_ekf->posNED, _ekf->gpsLat, _ekf->gpsLon, _ekf->gpsHgt, _ekf->latRef, _ekf->lonRef, _ekf->hgtRef);
+
+				_ekf->posNE[0] = _ekf->posNED[0];
+				_ekf->posNE[1] = _ekf->posNED[1];
+				// set fusion flags
+				_ekf->fuseVelData = true;
+				_ekf->fusePosData = true;
+				// recall states stored at time of measurement after adjusting for delays
+				_ekf->RecallStates(_ekf->statesAtVelTime, (IMUmsec - _parameters.vel_delay_ms));
+				_ekf->RecallStates(_ekf->statesAtPosTime, (IMUmsec - _parameters.pos_delay_ms));
+				// run the fusion step
+				_ekf->FuseVelposNED();
+
+			} else if (_ekf->statesInitialised) {
+				// Convert GPS measurements to Pos NE, hgt and Vel NED
+				_ekf->velNED[0] = 0.0f;
+				_ekf->velNED[1] = 0.0f;
+				_ekf->velNED[2] = 0.0f;
+				_ekf->posNED[0] = 0.0f;
+				_ekf->posNED[1] = 0.0f;
+				_ekf->posNED[2] = 0.0f;
+
+				_ekf->posNE[0] = _ekf->posNED[0];
+				_ekf->posNE[1] = _ekf->posNED[1];
+				// set fusion flags
+				_ekf->fuseVelData = true;
+				_ekf->fusePosData = true;
+				// recall states stored at time of measurement after adjusting for delays
+				_ekf->RecallStates(_ekf->statesAtVelTime, (IMUmsec - _parameters.vel_delay_ms));
+				_ekf->RecallStates(_ekf->statesAtPosTime, (IMUmsec - _parameters.pos_delay_ms));
+				// run the fusion step
+				_ekf->FuseVelposNED();
+
+			} else {
+				_ekf->fuseVelData = false;
+				_ekf->fusePosData = false;
+			}
+
+			if (newAdsData && _ekf->statesInitialised) {
+				// Could use a blend of GPS and baro alt data if desired
+				_ekf->hgtMea = 1.0f * _ekf->baroHgt + 0.0f * _ekf->gpsHgt;
+				_ekf->fuseHgtData = true;
+				// recall states stored at time of measurement after adjusting for delays
+				_ekf->RecallStates(_ekf->statesAtHgtTime, (IMUmsec - _parameters.height_delay_ms));
+				// run the fusion step
+				_ekf->FuseVelposNED();
+
+			} else {
+				_ekf->fuseHgtData = false;
+			}
+
+			// Fuse Magnetometer Measurements
+			if (newDataMag && _ekf->statesInitialised) {
+				_ekf->fuseMagData = true;
+				_ekf->RecallStates(_ekf->statesAtMagMeasTime, (IMUmsec - _parameters.mag_delay_ms)); // Assume 50 msec avg delay for magnetometer data
+
+			} else {
+				_ekf->fuseMagData = false;
+			}
+
+			if (_ekf->statesInitialised) _ekf->FuseMagnetometer();
+
+			// Fuse Airspeed Measurements
+			if (newAdsData && _ekf->statesInitialised && _ekf->VtasMeas > 8.0f) {
+				_ekf->fuseVtasData = true;
+				_ekf->RecallStates(_ekf->statesAtVtasMeasTime, (IMUmsec - _parameters.tas_delay_ms)); // assume 100 msec avg delay for airspeed data
+				_ekf->FuseAirspeed();
+
+			} else {
+				_ekf->fuseVtasData = false;
+			}
+
+			// Publish results
+			if (_initialized) {
+
+
+
+				// State vector:
+				// 0-3: quaternions (q0, q1, q2, q3)
+				// 4-6: Velocity - m/sec (North, East, Down)
+				// 7-9: Position - m (North, East, Down)
+				// 10-12: Delta Angle bias - rad (X,Y,Z)
+				// 13-14: Wind Vector  - m/sec (North,East)
+				// 15-17: Earth Magnetic Field Vector - milligauss (North, East, Down)
+				// 18-20: Body Magnetic Field Vector - milligauss (X,Y,Z)
+
+				math::Quaternion q(_ekf->states[0], _ekf->states[1], _ekf->states[2], _ekf->states[3]);
+				math::Matrix<3, 3> R = q.to_dcm();
+				math::Vector<3> euler = R.to_euler();
+
+				for (int i = 0; i < 3; i++) for (int j = 0; j < 3; j++)
+						_att.R[i][j] = R(i, j);
+
+				_att.timestamp = last_sensor_timestamp;
+				_att.q[0] = _ekf->states[0];
+				_att.q[1] = _ekf->states[1];
+				_att.q[2] = _ekf->states[2];
+				_att.q[3] = _ekf->states[3];
+				_att.q_valid = true;
+				_att.R_valid = true;
+
+				_att.timestamp = last_sensor_timestamp;
+				_att.roll = euler(0);
+				_att.pitch = euler(1);
+				_att.yaw = euler(2);
+
+				_att.rollspeed = _ekf->angRate.x - _ekf->states[10];
+				_att.pitchspeed = _ekf->angRate.y - _ekf->states[11];
+				_att.yawspeed = _ekf->angRate.z - _ekf->states[12];
+				// gyro offsets
+				_att.rate_offsets[0] = _ekf->states[10];
+				_att.rate_offsets[1] = _ekf->states[11];
+				_att.rate_offsets[2] = _ekf->states[12];
+
+				/* lazily publish the attitude only once available */
+				if (_att_pub > 0) {
+					/* publish the attitude setpoint */
+					orb_publish(ORB_ID(vehicle_attitude), _att_pub, &_att);
+
+				} else {
+					/* advertise and publish */
+					_att_pub = orb_advertise(ORB_ID(vehicle_attitude), &_att);
+				}
+			}
+
+			if (_gps_initialized) {
+				_local_pos.timestamp = last_sensor_timestamp;
+				_local_pos.x = _ekf->states[7];
+				_local_pos.y = _ekf->states[8];
+				_local_pos.z = _ekf->states[9];
+
+				_local_pos.vx = _ekf->states[4];
+				_local_pos.vy = _ekf->states[5];
+				_local_pos.vz = _ekf->states[6];
+
+				_local_pos.xy_valid = _gps_initialized;
+				_local_pos.z_valid = true;
+				_local_pos.v_xy_valid = _gps_initialized;
+				_local_pos.v_z_valid = true;
+				_local_pos.xy_global = true;
+
+				_local_pos.z_global = false;
+				_local_pos.yaw = _att.yaw;
+
+				/* lazily publish the local position only once available */
+				if (_local_pos_pub > 0) {
+					/* publish the attitude setpoint */
+					orb_publish(ORB_ID(vehicle_local_position), _local_pos_pub, &_local_pos);
+
+				} else {
+					/* advertise and publish */
+					_local_pos_pub = orb_advertise(ORB_ID(vehicle_local_position), &_local_pos);
+				}
+
+				_global_pos.timestamp = _local_pos.timestamp;
+
+				_global_pos.baro_valid = true;
+				_global_pos.global_valid = true;
+
+				if (_local_pos.xy_global) {
+					double est_lat, est_lon;
+					map_projection_reproject(_local_pos.x, _local_pos.y, &est_lat, &est_lon);
+					_global_pos.lat = est_lat;
+					_global_pos.lon = est_lon;
+					_global_pos.time_gps_usec = _gps.time_gps_usec;
+				}
+
+				/* set valid values even if position is not valid */
+				if (_local_pos.v_xy_valid) {
+					_global_pos.vel_n = _local_pos.vx;
+					_global_pos.vel_e = _local_pos.vy;
+				} else {
+					_global_pos.vel_n = 0.0f;
+					_global_pos.vel_e = 0.0f;
+				}
+
+				/* local pos alt is negative, change sign and add alt offset */
+				_global_pos.alt = _local_pos.ref_alt + (-_local_pos.z);
+
+				if (_local_pos.z_valid) {
+					_global_pos.baro_alt = _local_pos.ref_alt - _baro_gps_offset - _local_pos.z;
+				}
+
+				if (_local_pos.v_z_valid) {
+					_global_pos.vel_d = _local_pos.vz;
+				}
+
+				_global_pos.yaw = _local_pos.yaw;
+
+				_global_pos.timestamp = _local_pos.timestamp;
+
+				/* lazily publish the global position only once available */
+				if (_global_pos_pub > 0) {
+					/* publish the attitude setpoint */
+					orb_publish(ORB_ID(vehicle_global_position), _global_pos_pub, &_global_pos);
+
+				} else {
+					/* advertise and publish */
+					_global_pos_pub = orb_advertise(ORB_ID(vehicle_global_position), &_global_pos);
+				}
+			}
+
+		}
+
+		perf_end(_loop_perf);
+	}
+
+	warnx("exiting.\n");
+
+	_estimator_task = -1;
+	_exit(0);
+}
+
+int
+FixedwingEstimator::start()
+{
+	ASSERT(_estimator_task == -1);
+
+	/* start the task */
+	_estimator_task = task_spawn_cmd("fw_att_pos_estimator",
+					 SCHED_DEFAULT,
+					 SCHED_PRIORITY_MAX - 40,
+					 6000,
+					 (main_t)&FixedwingEstimator::task_main_trampoline,
+					 nullptr);
+
+	if (_estimator_task < 0) {
+		warn("task start failed");
+		return -errno;
+	}
+
+	return OK;
+}
+
+void
+FixedwingEstimator::print_status()
+{
+	math::Quaternion q(_ekf->states[0], _ekf->states[1], _ekf->states[2], _ekf->states[3]);
+	math::Matrix<3, 3> R = q.to_dcm();
+	math::Vector<3> euler = R.to_euler();
+
+	printf("attitude: roll: %8.4f, pitch %8.4f, yaw: %8.4f degrees\n",
+	       (double)math::degrees(euler(0)), (double)math::degrees(euler(1)), (double)math::degrees(euler(2)));
+
+	// State vector:
+	// 0-3: quaternions (q0, q1, q2, q3)
+	// 4-6: Velocity - m/sec (North, East, Down)
+	// 7-9: Position - m (North, East, Down)
+	// 10-12: Delta Angle bias - rad (X,Y,Z)
+	// 13-14: Wind Vector  - m/sec (North,East)
+	// 15-17: Earth Magnetic Field Vector - gauss (North, East, Down)
+	// 18-20: Body Magnetic Field Vector - gauss (X,Y,Z)
+
+	printf("dtIMU: %8.6f dt: %8.6f IMUmsec: %d\n", _ekf->dtIMU, dt, (int)IMUmsec);
+	printf("dvel: %8.6f %8.6f %8.6f accel: %8.6f %8.6f %8.6f\n", (double)_ekf->dVelIMU.x, (double)_ekf->dVelIMU.y, (double)_ekf->dVelIMU.z, (double)_ekf->accel.x, (double)_ekf->accel.y, (double)_ekf->accel.z);
+	printf("dang: %8.4f %8.4f %8.4f dang corr: %8.4f %8.4f %8.4f\n" , (double)_ekf->dAngIMU.x, (double)_ekf->dAngIMU.y, (double)_ekf->dAngIMU.z, (double)_ekf->correctedDelAng.x, (double)_ekf->correctedDelAng.y, (double)_ekf->correctedDelAng.z);
+	printf("states (quat)        [1-4]: %8.4f, %8.4f, %8.4f, %8.4f\n", (double)_ekf->states[0], (double)_ekf->states[1], (double)_ekf->states[2], (double)_ekf->states[3]);
+	printf("states (vel m/s)     [5-7]: %8.4f, %8.4f, %8.4f\n", (double)_ekf->states[4], (double)_ekf->states[5], (double)_ekf->states[6]);
+	printf("states (pos m)      [8-10]: %8.4f, %8.4f, %8.4f\n", (double)_ekf->states[7], (double)_ekf->states[8], (double)_ekf->states[9]);
+	printf("states (delta ang) [11-13]: %8.4f, %8.4f, %8.4f\n", (double)_ekf->states[10], (double)_ekf->states[11], (double)_ekf->states[12]);
+	printf("states (wind)      [14-15]: %8.4f, %8.4f\n", (double)_ekf->states[13], (double)_ekf->states[14]);
+	printf("states (earth mag) [16-18]: %8.4f, %8.4f, %8.4f\n", (double)_ekf->states[15], (double)_ekf->states[16], (double)_ekf->states[17]);
+	printf("states (body mag)  [19-21]: %8.4f, %8.4f, %8.4f\n", (double)_ekf->states[18], (double)_ekf->states[19], (double)_ekf->states[20]);
+	printf("states: %s %s %s %s %s %s %s %s %s %s\n",
+	       (_ekf->statesInitialised) ? "INITIALIZED" : "NON_INIT",
+	       (_ekf->onGround) ? "ON_GROUND" : "AIRBORNE",
+	       (_ekf->fuseVelData) ? "FUSE_VEL" : "INH_VEL",
+	       (_ekf->fusePosData) ? "FUSE_POS" : "INH_POS",
+	       (_ekf->fuseHgtData) ? "FUSE_HGT" : "INH_HGT",
+	       (_ekf->fuseMagData) ? "FUSE_MAG" : "INH_MAG",
+	       (_ekf->fuseVtasData) ? "FUSE_VTAS" : "INH_VTAS",
+	       (_ekf->useAirspeed) ? "USE_AIRSPD" : "IGN_AIRSPD",
+	       (_ekf->useCompass) ? "USE_COMPASS" : "IGN_COMPASS",
+	       (_ekf->staticMode) ? "STATIC_MODE" : "DYNAMIC_MODE");
+}
+
+int FixedwingEstimator::trip_nan() {
+
+	int ret = 0;
+
+	// If system is not armed, inject a NaN value into the filter
+	int armed_sub = orb_subscribe(ORB_ID(actuator_armed));
+
+	struct actuator_armed_s armed;
+	orb_copy(ORB_ID(actuator_armed), armed_sub, &armed);
+
+	if (armed.armed) {
+		warnx("ACTUATORS ARMED! NOT TRIPPING SYSTEM");
+		ret = 1;
+	} else {
+
+		float nan_val = 0.0f / 0.0f;
+
+		warnx("system not armed, tripping state vector with NaN values");
+		_ekf->states[5] = nan_val;
+		usleep(100000);
+
+		// warnx("tripping covariance #1 with NaN values");
+		// KH[2][2] = nan_val; //  intermediate result used for covariance updates
+		// usleep(100000);
+
+		// warnx("tripping covariance #2 with NaN values");
+		// KHP[5][5] = nan_val; // intermediate result used for covariance updates
+		// usleep(100000);
+
+		warnx("tripping covariance #3 with NaN values");
+		_ekf->P[3][3] = nan_val; // covariance matrix
+		usleep(100000);
+
+		warnx("tripping Kalman gains with NaN values");
+		_ekf->Kfusion[0] = nan_val; // Kalman gains
+		usleep(100000);
+
+		warnx("tripping stored states[0] with NaN values");
+		_ekf->storedStates[0][0] = nan_val;
+		usleep(100000);
+
+		warnx("\nDONE - FILTER STATE:");
+		print_status();
+	}
+
+	close(armed_sub);
+	return ret;
+}
+
+int fw_att_pos_estimator_main(int argc, char *argv[])
+{
+	if (argc < 1)
+		errx(1, "usage: fw_att_pos_estimator {start|stop|status}");
+
+	if (!strcmp(argv[1], "start")) {
+
+		if (estimator::g_estimator != nullptr)
+			errx(1, "already running");
+
+		estimator::g_estimator = new FixedwingEstimator;
+
+		if (estimator::g_estimator == nullptr)
+			errx(1, "alloc failed");
+
+		if (OK != estimator::g_estimator->start()) {
+			delete estimator::g_estimator;
+			estimator::g_estimator = nullptr;
+			err(1, "start failed");
+		}
+
+		exit(0);
+	}
+
+	if (!strcmp(argv[1], "stop")) {
+		if (estimator::g_estimator == nullptr)
+			errx(1, "not running");
+
+		delete estimator::g_estimator;
+		estimator::g_estimator = nullptr;
+		exit(0);
+	}
+
+	if (!strcmp(argv[1], "status")) {
+		if (estimator::g_estimator) {
+			warnx("running");
+
+			estimator::g_estimator->print_status();
+
+			exit(0);
+
+		} else {
+			errx(1, "not running");
+		}
+	}
+
+	if (!strcmp(argv[1], "trip")) {
+		if (estimator::g_estimator) {
+			int ret = estimator::g_estimator->trip_nan();
+
+			exit(ret);
+
+		} else {
+			errx(1, "not running");
+		}
+	}
+
+	warnx("unrecognized command");
+	return 1;
+}
diff --git a/src/modules/fw_att_pos_estimator/fw_att_pos_estimator_params.c b/src/modules/fw_att_pos_estimator/fw_att_pos_estimator_params.c
new file mode 100644
index 000000000..6138ef39c
--- /dev/null
+++ b/src/modules/fw_att_pos_estimator/fw_att_pos_estimator_params.c
@@ -0,0 +1,117 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2013, 2014 PX4 Development Team. All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in
+ *    the documentation and/or other materials provided with the
+ *    distribution.
+ * 3. Neither the name PX4 nor the names of its contributors may be
+ *    used to endorse or promote products derived from this software
+ *    without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ ****************************************************************************/
+
+/**
+ * @file fw_att_pos_estimator_params.c
+ *
+ * Parameters defined by the attitude and position estimator task
+ *
+ * @author Lorenz Meier <lm@inf.ethz.ch>
+ */
+
+#include <nuttx/config.h>
+
+#include <systemlib/param/param.h>
+
+/*
+ * Estimator parameters, accessible via MAVLink
+ *
+ */
+
+/**
+ * Velocity estimate delay
+ *
+ * The delay in milliseconds of the velocity estimate from GPS.
+ *
+ * @min 0
+ * @max 1000
+ * @group Position Estimator
+ */
+PARAM_DEFINE_INT32(PE_VEL_DELAY_MS, 230);
+
+/**
+ * Position estimate delay
+ *
+ * The delay in milliseconds of the position estimate from GPS.
+ *
+ * @min 0
+ * @max 1000
+ * @group Position Estimator
+ */
+PARAM_DEFINE_INT32(PE_POS_DELAY_MS, 210);
+
+/**
+ * Height estimate delay
+ *
+ * The delay in milliseconds of the height estimate from the barometer.
+ *
+ * @min 0
+ * @max 1000
+ * @group Position Estimator
+ */
+PARAM_DEFINE_INT32(PE_HGT_DELAY_MS, 350);
+
+/**
+ * Mag estimate delay
+ *
+ * The delay in milliseconds of the magnetic field estimate from
+ * the magnetometer.
+ *
+ * @min 0
+ * @max 1000
+ * @group Position Estimator
+ */
+PARAM_DEFINE_INT32(PE_MAG_DELAY_MS, 30);
+
+/**
+ * True airspeeed estimate delay
+ *
+ * The delay in milliseconds of the airspeed estimate.
+ *
+ * @min 0
+ * @max 1000
+ * @group Position Estimator
+ */
+PARAM_DEFINE_INT32(PE_TAS_DELAY_MS, 210);
+
+/**
+ * GPS vs. barometric altitude update weight
+ *
+ * RE-CHECK this.
+ *
+ * @min 0.0
+ * @max 1.0
+ * @group Position Estimator
+ */
+PARAM_DEFINE_FLOAT(PE_GPS_ALT_WGT, 0.9f);
+
diff --git a/src/modules/fw_att_pos_estimator/module.mk b/src/modules/fw_att_pos_estimator/module.mk
new file mode 100644
index 000000000..c992959e0
--- /dev/null
+++ b/src/modules/fw_att_pos_estimator/module.mk
@@ -0,0 +1,42 @@
+############################################################################
+#
+#   Copyright (c) 2013, 2014 PX4 Development Team. All rights reserved.
+#
+# Redistribution and use in source and binary forms, with or without
+# modification, are permitted provided that the following conditions
+# are met:
+#
+# 1. Redistributions of source code must retain the above copyright
+#    notice, this list of conditions and the following disclaimer.
+# 2. Redistributions in binary form must reproduce the above copyright
+#    notice, this list of conditions and the following disclaimer in
+#    the documentation and/or other materials provided with the
+#    distribution.
+# 3. Neither the name PX4 nor the names of its contributors may be
+#    used to endorse or promote products derived from this software
+#    without specific prior written permission.
+#
+# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+# OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+# AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+# POSSIBILITY OF SUCH DAMAGE.
+#
+############################################################################
+
+#
+# Main Attitude and Position Estimator for Fixed Wing Aircraft
+#
+
+MODULE_COMMAND	= fw_att_pos_estimator
+
+SRCS		= fw_att_pos_estimator_main.cpp \
+		  fw_att_pos_estimator_params.c \
+		  estimator.cpp
diff --git a/src/modules/mavlink/mavlink_orb_subscription.cpp b/src/modules/mavlink/mavlink_orb_subscription.cpp
index 4de722832..d432edd2b 100644
--- a/src/modules/mavlink/mavlink_orb_subscription.cpp
+++ b/src/modules/mavlink/mavlink_orb_subscription.cpp
@@ -88,6 +88,7 @@ MavlinkOrbSubscription::update(const hrt_abstime t)
 
 		if (_updated) {
 			orb_copy(_topic, _fd, _data);
+			_published = true;
 			return true;
 		}
 	}
diff --git a/src/modules/mavlink/mavlink_receiver.cpp b/src/modules/mavlink/mavlink_receiver.cpp
index d7e300670..3ec40ee0a 100644
--- a/src/modules/mavlink/mavlink_receiver.cpp
+++ b/src/modules/mavlink/mavlink_receiver.cpp
@@ -222,12 +222,10 @@ MavlinkReceiver::handle_message_command_long(mavlink_message_t *msg)
 			vcmd.source_component = msg->compid;
 			vcmd.confirmation =  cmd_mavlink.confirmation;
 
-			/* check if topic is advertised */
-			if (_cmd_pub <= 0) {
+			if (_cmd_pub < 0) {
 				_cmd_pub = orb_advertise(ORB_ID(vehicle_command), &vcmd);
 
 			} else {
-				/* publish */
 				orb_publish(ORB_ID(vehicle_command), _cmd_pub, &vcmd);
 			}
 		}
@@ -253,7 +251,7 @@ MavlinkReceiver::handle_message_optical_flow(mavlink_message_t *msg)
 	f.quality = flow.quality;
 	f.sensor_id = flow.sensor_id;
 
-	if (_flow_pub <= 0) {
+	if (_flow_pub < 0) {
 		_flow_pub = orb_advertise(ORB_ID(optical_flow), &f);
 
 	} else {
@@ -287,7 +285,7 @@ MavlinkReceiver::handle_message_set_mode(mavlink_message_t *msg)
 	vcmd.source_component = msg->compid;
 	vcmd.confirmation = 1;
 
-	if (_cmd_pub <= 0) {
+	if (_cmd_pub < 0) {
 		_cmd_pub = orb_advertise(ORB_ID(vehicle_command), &vcmd);
 
 	} else {
@@ -312,7 +310,7 @@ MavlinkReceiver::handle_message_vicon_position_estimate(mavlink_message_t *msg)
 	vicon_position.pitch = pos.pitch;
 	vicon_position.yaw = pos.yaw;
 
-	if (_vicon_position_pub <= 0) {
+	if (_vicon_position_pub < 0) {
 		_vicon_position_pub = orb_advertise(ORB_ID(vehicle_vicon_position), &vicon_position);
 
 	} else {
@@ -373,7 +371,7 @@ MavlinkReceiver::handle_message_quad_swarm_roll_pitch_yaw_thrust(mavlink_message
 
 		offboard_control_sp.timestamp = hrt_absolute_time();
 
-		if (_offboard_control_sp_pub <= 0) {
+		if (_offboard_control_sp_pub < 0) {
 			_offboard_control_sp_pub = orb_advertise(ORB_ID(offboard_control_setpoint), &offboard_control_sp);
 
 		} else {
@@ -401,7 +399,7 @@ MavlinkReceiver::handle_message_radio_status(mavlink_message_t *msg)
 	tstatus.rxerrors = rstatus.rxerrors;
 	tstatus.fixed = rstatus.fixed;
 
-	if (_telemetry_status_pub <= 0) {
+	if (_telemetry_status_pub < 0) {
 		_telemetry_status_pub = orb_advertise(ORB_ID(telemetry_status), &tstatus);
 
 	} else {
@@ -428,7 +426,7 @@ MavlinkReceiver::handle_message_manual_control(mavlink_message_t *msg)
 		rc.chan[2].scaled = man.r / 1000.0f;
 		rc.chan[3].scaled = man.z / 1000.0f;
 
-		if (_rc_pub == 0) {
+		if (_rc_pub < 0) {
 			_rc_pub = orb_advertise(ORB_ID(rc_channels), &rc);
 
 		} else {
@@ -450,7 +448,7 @@ MavlinkReceiver::handle_message_manual_control(mavlink_message_t *msg)
 		manual.yaw = man.r / 1000.0f;
 		manual.throttle = man.z / 1000.0f;
 
-		if (_manual_pub == 0) {
+		if (_manual_pub < 0) {
 			_manual_pub = orb_advertise(ORB_ID(manual_control_setpoint), &manual);
 
 		} else {
@@ -619,11 +617,11 @@ MavlinkReceiver::handle_message_hil_sensor(mavlink_message_t *msg)
 		hil_sensors.differential_pressure_timestamp = timestamp;
 
 		/* publish combined sensor topic */
-		if (_sensors_pub > 0) {
-			orb_publish(ORB_ID(sensor_combined), _sensors_pub, &hil_sensors);
+		if (_sensors_pub < 0) {
+			_sensors_pub = orb_advertise(ORB_ID(sensor_combined), &hil_sensors);
 
 		} else {
-			_sensors_pub = orb_advertise(ORB_ID(sensor_combined), &hil_sensors);
+			orb_publish(ORB_ID(sensor_combined), _sensors_pub, &hil_sensors);
 		}
 	}
 
@@ -638,11 +636,11 @@ MavlinkReceiver::handle_message_hil_sensor(mavlink_message_t *msg)
 		hil_battery_status.current_a = 10.0f;
 		hil_battery_status.discharged_mah = -1.0f;
 
-		if (_battery_pub > 0) {
-			orb_publish(ORB_ID(battery_status), _battery_pub, &hil_battery_status);
+		if (_battery_pub < 0) {
+			_battery_pub = orb_advertise(ORB_ID(battery_status), &hil_battery_status);
 
 		} else {
-			_battery_pub = orb_advertise(ORB_ID(battery_status), &hil_battery_status);
+			orb_publish(ORB_ID(battery_status), _battery_pub, &hil_battery_status);
 		}
 	}
 
@@ -694,11 +692,11 @@ MavlinkReceiver::handle_message_hil_gps(mavlink_message_t *msg)
 	hil_gps.fix_type = gps.fix_type;
 	hil_gps.satellites_visible = gps.satellites_visible;
 
-	if (_gps_pub > 0) {
-		orb_publish(ORB_ID(vehicle_gps_position), _gps_pub, &hil_gps);
+	if (_gps_pub < 0) {
+		_gps_pub = orb_advertise(ORB_ID(vehicle_gps_position), &hil_gps);
 
 	} else {
-		_gps_pub = orb_advertise(ORB_ID(vehicle_gps_position), &hil_gps);
+		orb_publish(ORB_ID(vehicle_gps_position), _gps_pub, &hil_gps);
 	}
 }
 
@@ -752,11 +750,11 @@ MavlinkReceiver::handle_message_hil_state_quaternion(mavlink_message_t *msg)
 		hil_attitude.pitchspeed = hil_state.pitchspeed;
 		hil_attitude.yawspeed = hil_state.yawspeed;
 
-		if (_attitude_pub > 0) {
-			orb_publish(ORB_ID(vehicle_attitude), _attitude_pub, &hil_attitude);
+		if (_attitude_pub < 0) {
+			_attitude_pub = orb_advertise(ORB_ID(vehicle_attitude), &hil_attitude);
 
 		} else {
-			_attitude_pub = orb_advertise(ORB_ID(vehicle_attitude), &hil_attitude);
+			orb_publish(ORB_ID(vehicle_attitude), _attitude_pub, &hil_attitude);
 		}
 	}
 
@@ -775,11 +773,11 @@ MavlinkReceiver::handle_message_hil_state_quaternion(mavlink_message_t *msg)
 		hil_global_pos.vel_d = hil_state.vz / 100.0f;
 		hil_global_pos.yaw = hil_attitude.yaw;
 
-		if (_global_pos_pub > 0) {
-			orb_publish(ORB_ID(vehicle_global_position), _global_pos_pub, &hil_global_pos);
+		if (_global_pos_pub < 0) {
+			_global_pos_pub = orb_advertise(ORB_ID(vehicle_global_position), &hil_global_pos);
 
 		} else {
-			_global_pos_pub = orb_advertise(ORB_ID(vehicle_global_position), &hil_global_pos);
+			orb_publish(ORB_ID(vehicle_global_position), _global_pos_pub, &hil_global_pos);
 		}
 	}
 
@@ -816,11 +814,11 @@ MavlinkReceiver::handle_message_hil_state_quaternion(mavlink_message_t *msg)
 		bool landed = (float)(hil_state.alt) / 1000.0f < (_hil_local_alt0 + 0.1f); // XXX improve?
 		hil_local_pos.landed = landed;
 
-		if (_local_pos_pub > 0) {
-			orb_publish(ORB_ID(vehicle_local_position), _local_pos_pub, &hil_local_pos);
+		if (_local_pos_pub < 0) {
+			_local_pos_pub = orb_advertise(ORB_ID(vehicle_local_position), &hil_local_pos);
 
 		} else {
-			_local_pos_pub = orb_advertise(ORB_ID(vehicle_local_position), &hil_local_pos);
+			orb_publish(ORB_ID(vehicle_local_position), _local_pos_pub, &hil_local_pos);
 		}
 	}
 
@@ -857,11 +855,11 @@ MavlinkReceiver::handle_message_hil_state_quaternion(mavlink_message_t *msg)
 		hil_battery_status.current_a = 10.0f;
 		hil_battery_status.discharged_mah = -1.0f;
 
-		if (_battery_pub > 0) {
-			orb_publish(ORB_ID(battery_status), _battery_pub, &hil_battery_status);
+		if (_battery_pub < 0) {
+			_battery_pub = orb_advertise(ORB_ID(battery_status), &hil_battery_status);
 
 		} else {
-			_battery_pub = orb_advertise(ORB_ID(battery_status), &hil_battery_status);
+			orb_publish(ORB_ID(battery_status), _battery_pub, &hil_battery_status);
 		}
 	}
 }
diff --git a/src/modules/mavlink/mavlink_receiver.h b/src/modules/mavlink/mavlink_receiver.h
index 0a5a1b5c7..8ccb2a035 100644
--- a/src/modules/mavlink/mavlink_receiver.h
+++ b/src/modules/mavlink/mavlink_receiver.h
@@ -120,7 +120,6 @@ private:
 
 	mavlink_status_t status;
 	struct vehicle_local_position_s hil_local_pos;
-	int _manual_sub;
 	orb_advert_t _global_pos_pub;
 	orb_advert_t _local_pos_pub;
 	orb_advert_t _attitude_pub;
@@ -139,6 +138,7 @@ private:
 	orb_advert_t _telemetry_status_pub;
 	orb_advert_t _rc_pub;
 	orb_advert_t _manual_pub;
+	int _manual_sub;
 	int _hil_frames;
 	uint64_t _old_timestamp;
 	bool _hil_local_proj_inited;
diff --git a/src/modules/navigator/navigator_main.cpp b/src/modules/navigator/navigator_main.cpp
index c45cafc1b..5a94b6671 100644
--- a/src/modules/navigator/navigator_main.cpp
+++ b/src/modules/navigator/navigator_main.cpp
@@ -1539,7 +1539,7 @@ Navigator::check_mission_item_reached()
 			/* check yaw if defined only for rotary wing except takeoff */
 			float yaw_err = _wrap_pi(_mission_item.yaw - _global_pos.yaw);
 
-			if (fabsf(yaw_err) < 0.05f) { /* XXX get rid of magic number */
+			if (fabsf(yaw_err) < 0.2f) { /* XXX get rid of magic number */
 				_waypoint_yaw_reached = true;
 			}
 
diff --git a/src/modules/px4iofirmware/controls.c b/src/modules/px4iofirmware/controls.c
index 941500f0d..56c5aa005 100644
--- a/src/modules/px4iofirmware/controls.c
+++ b/src/modules/px4iofirmware/controls.c
@@ -134,8 +134,6 @@ controls_tick() {
 
 	perf_begin(c_gather_sbus);
 
-	bool sbus_status = (r_status_flags & PX4IO_P_STATUS_FLAGS_RC_SBUS);
-
 	bool sbus_failsafe, sbus_frame_drop;
 	bool sbus_updated = sbus_input(r_raw_rc_values, &r_raw_rc_count, &sbus_failsafe, &sbus_frame_drop, PX4IO_RC_INPUT_CHANNELS);
 
@@ -201,94 +199,104 @@ controls_tick() {
 		/* update RC-received timestamp */
 		system_state.rc_channels_timestamp_received = hrt_absolute_time();
 
-		/* do not command anything in failsafe, kick in the RC loss counter */
-		if (!(r_raw_rc_flags & PX4IO_P_RAW_RC_FLAGS_FAILSAFE)) {
-
-			/* update RC-received timestamp */
-			system_state.rc_channels_timestamp_valid = system_state.rc_channels_timestamp_received;
-
-			/* map raw inputs to mapped inputs */
-			/* XXX mapping should be atomic relative to protocol */
-			for (unsigned i = 0; i < r_raw_rc_count; i++) {
-
-				/* map the input channel */
-				uint16_t *conf = &r_page_rc_input_config[i * PX4IO_P_RC_CONFIG_STRIDE];
-
-				if (conf[PX4IO_P_RC_CONFIG_OPTIONS] & PX4IO_P_RC_CONFIG_OPTIONS_ENABLED) {
-
-					uint16_t raw = r_raw_rc_values[i];
-
-					int16_t scaled;
-
-					/*
-					 * 1) Constrain to min/max values, as later processing depends on bounds.
-					 */
-					if (raw < conf[PX4IO_P_RC_CONFIG_MIN])
-						raw = conf[PX4IO_P_RC_CONFIG_MIN];
-					if (raw > conf[PX4IO_P_RC_CONFIG_MAX])
-						raw = conf[PX4IO_P_RC_CONFIG_MAX];
-
-					/*
-					 * 2) Scale around the mid point differently for lower and upper range.
-					 *
-					 * This is necessary as they don't share the same endpoints and slope.
-					 *
-					 * First normalize to 0..1 range with correct sign (below or above center),
-					 * then scale to 20000 range (if center is an actual center, -10000..10000,
-					 * if parameters only support half range, scale to 10000 range, e.g. if
-					 * center == min 0..10000, if center == max -10000..0).
-					 *
-					 * As the min and max bounds were enforced in step 1), division by zero
-					 * cannot occur, as for the case of center == min or center == max the if
-					 * statement is mutually exclusive with the arithmetic NaN case.
-					 *
-					 * DO NOT REMOVE OR ALTER STEP 1!
-					 */
-					if (raw > (conf[PX4IO_P_RC_CONFIG_CENTER] + conf[PX4IO_P_RC_CONFIG_DEADZONE])) {
-						scaled = 10000.0f * ((raw - conf[PX4IO_P_RC_CONFIG_CENTER] - conf[PX4IO_P_RC_CONFIG_DEADZONE]) / (float)(conf[PX4IO_P_RC_CONFIG_MAX] - conf[PX4IO_P_RC_CONFIG_CENTER] - conf[PX4IO_P_RC_CONFIG_DEADZONE]));
-
-					} else if (raw < (conf[PX4IO_P_RC_CONFIG_CENTER] - conf[PX4IO_P_RC_CONFIG_DEADZONE])) {
-						scaled = 10000.0f * ((raw - conf[PX4IO_P_RC_CONFIG_CENTER] + conf[PX4IO_P_RC_CONFIG_DEADZONE]) / (float)(conf[PX4IO_P_RC_CONFIG_CENTER] - conf[PX4IO_P_RC_CONFIG_DEADZONE] - conf[PX4IO_P_RC_CONFIG_MIN]));
-
-					} else {
-						/* in the configured dead zone, output zero */
-						scaled = 0;
-					}
-
-					/* invert channel if requested */
-					if (conf[PX4IO_P_RC_CONFIG_OPTIONS] & PX4IO_P_RC_CONFIG_OPTIONS_REVERSE)
-						scaled = -scaled;
+		/* update RC-received timestamp */
+		system_state.rc_channels_timestamp_valid = system_state.rc_channels_timestamp_received;
+
+		/* map raw inputs to mapped inputs */
+		/* XXX mapping should be atomic relative to protocol */
+		for (unsigned i = 0; i < r_raw_rc_count; i++) {
+
+			/* map the input channel */
+			uint16_t *conf = &r_page_rc_input_config[i * PX4IO_P_RC_CONFIG_STRIDE];
+
+			if (conf[PX4IO_P_RC_CONFIG_OPTIONS] & PX4IO_P_RC_CONFIG_OPTIONS_ENABLED) {
+
+				uint16_t raw = r_raw_rc_values[i];
+
+				int16_t scaled;
+
+				/*
+				 * 1) Constrain to min/max values, as later processing depends on bounds.
+				 */
+				if (raw < conf[PX4IO_P_RC_CONFIG_MIN])
+					raw = conf[PX4IO_P_RC_CONFIG_MIN];
+				if (raw > conf[PX4IO_P_RC_CONFIG_MAX])
+					raw = conf[PX4IO_P_RC_CONFIG_MAX];
+
+				/*
+				 * 2) Scale around the mid point differently for lower and upper range.
+				 *
+				 * This is necessary as they don't share the same endpoints and slope.
+				 *
+				 * First normalize to 0..1 range with correct sign (below or above center),
+				 * then scale to 20000 range (if center is an actual center, -10000..10000,
+				 * if parameters only support half range, scale to 10000 range, e.g. if
+				 * center == min 0..10000, if center == max -10000..0).
+				 *
+				 * As the min and max bounds were enforced in step 1), division by zero
+				 * cannot occur, as for the case of center == min or center == max the if
+				 * statement is mutually exclusive with the arithmetic NaN case.
+				 *
+				 * DO NOT REMOVE OR ALTER STEP 1!
+				 */
+				if (raw > (conf[PX4IO_P_RC_CONFIG_CENTER] + conf[PX4IO_P_RC_CONFIG_DEADZONE])) {
+					scaled = 10000.0f * ((raw - conf[PX4IO_P_RC_CONFIG_CENTER] - conf[PX4IO_P_RC_CONFIG_DEADZONE]) / (float)(conf[PX4IO_P_RC_CONFIG_MAX] - conf[PX4IO_P_RC_CONFIG_CENTER] - conf[PX4IO_P_RC_CONFIG_DEADZONE]));
+
+				} else if (raw < (conf[PX4IO_P_RC_CONFIG_CENTER] - conf[PX4IO_P_RC_CONFIG_DEADZONE])) {
+					scaled = 10000.0f * ((raw - conf[PX4IO_P_RC_CONFIG_CENTER] + conf[PX4IO_P_RC_CONFIG_DEADZONE]) / (float)(conf[PX4IO_P_RC_CONFIG_CENTER] - conf[PX4IO_P_RC_CONFIG_DEADZONE] - conf[PX4IO_P_RC_CONFIG_MIN]));
+
+				} else {
+					/* in the configured dead zone, output zero */
+					scaled = 0;
+				}
 
-					/* and update the scaled/mapped version */
-					unsigned mapped = conf[PX4IO_P_RC_CONFIG_ASSIGNMENT];
-					if (mapped < PX4IO_CONTROL_CHANNELS) {
+				/* invert channel if requested */
+				if (conf[PX4IO_P_RC_CONFIG_OPTIONS] & PX4IO_P_RC_CONFIG_OPTIONS_REVERSE) {
+					scaled = -scaled;
+				}
 
-						/* invert channel if pitch - pulling the lever down means pitching up by convention */
-						if (mapped == 1) /* roll, pitch, yaw, throttle, override is the standard order */
-							scaled = -scaled;
+				/* and update the scaled/mapped version */
+				unsigned mapped = conf[PX4IO_P_RC_CONFIG_ASSIGNMENT];
+				if (mapped < PX4IO_CONTROL_CHANNELS) {
 
-						r_rc_values[mapped] = SIGNED_TO_REG(scaled);
-						assigned_channels |= (1 << mapped);
+					/* invert channel if pitch - pulling the lever down means pitching up by convention */
+					if (mapped == 1) {
+						/* roll, pitch, yaw, throttle, override is the standard order */
+						scaled = -scaled;
+					}
 
+					if (mapped == 3 && r_setup_rc_thr_failsafe) {
+						/* throttle failsafe detection */
+						if (((raw < conf[PX4IO_P_RC_CONFIG_MIN]) && (raw < r_setup_rc_thr_failsafe)) ||
+						    ((raw > conf[PX4IO_P_RC_CONFIG_MAX]) && (raw > r_setup_rc_thr_failsafe))) {
+							r_raw_rc_flags |= PX4IO_P_RAW_RC_FLAGS_FAILSAFE;
+						} else {
+							r_raw_rc_flags &= ~(PX4IO_P_RAW_RC_FLAGS_FAILSAFE);
+						}
 					}
+
+					r_rc_values[mapped] = SIGNED_TO_REG(scaled);
+					assigned_channels |= (1 << mapped);
+
 				}
 			}
+		}
 
-			/* set un-assigned controls to zero */
-			for (unsigned i = 0; i < PX4IO_CONTROL_CHANNELS; i++) {
-				if (!(assigned_channels & (1 << i)))
-					r_rc_values[i] = 0;
+		/* set un-assigned controls to zero */
+		for (unsigned i = 0; i < PX4IO_CONTROL_CHANNELS; i++) {
+			if (!(assigned_channels & (1 << i))) {
+				r_rc_values[i] = 0;
 			}
+		}
 
-			/* set RC OK flag, as we got an update */
-			r_status_flags |= PX4IO_P_STATUS_FLAGS_RC_OK;
+		/* set RC OK flag, as we got an update */
+		r_status_flags |= PX4IO_P_STATUS_FLAGS_RC_OK;
 
-			/* if we have enough channels (5) to control the vehicle, the mapping is ok */
-			if (assigned_channels > 4) {
-				r_raw_rc_flags |= PX4IO_P_RAW_RC_FLAGS_MAPPING_OK;
-			} else {
-				r_raw_rc_flags &= ~(PX4IO_P_RAW_RC_FLAGS_MAPPING_OK);
-			}
+		/* if we have enough channels (5) to control the vehicle, the mapping is ok */
+		if (assigned_channels > 4) {
+			r_raw_rc_flags |= PX4IO_P_RAW_RC_FLAGS_MAPPING_OK;
+		} else {
+			r_raw_rc_flags &= ~(PX4IO_P_RAW_RC_FLAGS_MAPPING_OK);
 		}
 
 		/*
@@ -316,8 +324,13 @@ controls_tick() {
 	 * Handle losing RC input
 	 */
 
-	/* this kicks in if the receiver is gone or the system went to failsafe */
-	if (rc_input_lost || (r_raw_rc_flags & PX4IO_P_RAW_RC_FLAGS_FAILSAFE)) {
+	/* if we are in failsafe, clear the override flag */
+	if (r_raw_rc_flags & PX4IO_P_RAW_RC_FLAGS_FAILSAFE) {
+		r_status_flags &= ~(PX4IO_P_STATUS_FLAGS_OVERRIDE);
+	}
+
+	/* this kicks in if the receiver is gone, but there is not on failsafe (indicated by separate flag) */
+	if (rc_input_lost) {
 		/* Clear the RC input status flag, clear manual override flag */
 		r_status_flags &= ~(
 			PX4IO_P_STATUS_FLAGS_OVERRIDE |
@@ -326,27 +339,24 @@ controls_tick() {
 		/* Mark all channels as invalid, as we just lost the RX */
 		r_rc_valid = 0;
 
+		/* Set raw channel count to zero */
+		r_raw_rc_count = 0;
+
 		/* Set the RC_LOST alarm */
 		r_status_alarms |= PX4IO_P_STATUS_ALARMS_RC_LOST;
 	}
 
-	/* this kicks in if the receiver is completely gone */
-	if (rc_input_lost) {
-
-		/* Set channel count to zero */
-		r_raw_rc_count = 0;
-	}
-
 	/*
 	 * Check for manual override.
 	 *
 	 * The PX4IO_P_SETUP_ARMING_MANUAL_OVERRIDE_OK flag must be set, and we
-	 * must have R/C input.
+	 * must have R/C input (NO FAILSAFE!).
 	 * Override is enabled if either the hardcoded channel / value combination
 	 * is selected, or the AP has requested it.
 	 */
 	if ((r_setup_arming & PX4IO_P_SETUP_ARMING_MANUAL_OVERRIDE_OK) && 
-		(r_status_flags & PX4IO_P_STATUS_FLAGS_RC_OK)) {
+		(r_status_flags & PX4IO_P_STATUS_FLAGS_RC_OK) &&
+		!(r_raw_rc_flags & PX4IO_P_RAW_RC_FLAGS_FAILSAFE)) {
 
 		bool override = false;
 
@@ -369,10 +379,10 @@ controls_tick() {
 				mixer_tick();
 
 		} else {
-			r_status_flags &= ~PX4IO_P_STATUS_FLAGS_OVERRIDE;
+			r_status_flags &= ~(PX4IO_P_STATUS_FLAGS_OVERRIDE);
 		}
 	} else {
-		r_status_flags &= ~PX4IO_P_STATUS_FLAGS_OVERRIDE;
+		r_status_flags &= ~(PX4IO_P_STATUS_FLAGS_OVERRIDE);
 	}
 }
 
diff --git a/src/modules/px4iofirmware/protocol.h b/src/modules/px4iofirmware/protocol.h
index d48c6c529..a978d483a 100644
--- a/src/modules/px4iofirmware/protocol.h
+++ b/src/modules/px4iofirmware/protocol.h
@@ -164,10 +164,10 @@
 /* setup page */
 #define PX4IO_PAGE_SETUP		50
 #define PX4IO_P_SETUP_FEATURES			0
-#define PX4IO_P_SETUP_FEATURES_SBUS1_OUT	(1 << 0) /* enable S.Bus v1 output */
-#define PX4IO_P_SETUP_FEATURES_SBUS2_OUT	(1 << 1) /* enable S.Bus v2 output */
-#define PX4IO_P_SETUP_FEATURES_PWM_RSSI		(1 << 2) /* enable PWM RSSI parsing */
-#define PX4IO_P_SETUP_FEATURES_ADC_RSSI		(1 << 3) /* enable ADC RSSI parsing */
+#define PX4IO_P_SETUP_FEATURES_SBUS1_OUT	(1 << 0) /**< enable S.Bus v1 output */
+#define PX4IO_P_SETUP_FEATURES_SBUS2_OUT	(1 << 1) /**< enable S.Bus v2 output */
+#define PX4IO_P_SETUP_FEATURES_PWM_RSSI		(1 << 2) /**< enable PWM RSSI parsing */
+#define PX4IO_P_SETUP_FEATURES_ADC_RSSI		(1 << 3) /**< enable ADC RSSI parsing */
 
 #define PX4IO_P_SETUP_ARMING			1	 /* arming controls */
 #define PX4IO_P_SETUP_ARMING_IO_ARM_OK		(1 << 0) /* OK to arm the IO side */
@@ -201,13 +201,15 @@ enum {							/* DSM bind states */
 	dsm_bind_send_pulses,
 	dsm_bind_reinit_uart
 };
- 					     /*	8 */
-#define PX4IO_P_SETUP_SET_DEBUG			9		/* debug level for IO board */
+						/* 8 */
+#define PX4IO_P_SETUP_SET_DEBUG			9	/* debug level for IO board */
 
-#define PX4IO_P_SETUP_REBOOT_BL		       10	/* reboot IO into bootloader */
-#define PX4IO_REBOOT_BL_MAGIC               14662       /* required argument for reboot (random) */
+#define PX4IO_P_SETUP_REBOOT_BL			10	/* reboot IO into bootloader */
+#define PX4IO_REBOOT_BL_MAGIC			14662	/* required argument for reboot (random) */
 
-#define PX4IO_P_SETUP_CRC		       11	/* get CRC of IO firmware */
+#define PX4IO_P_SETUP_CRC			11	/* get CRC of IO firmware */
+						/* 12 occupied by CRC */
+#define PX4IO_P_SETUP_RC_THR_FAILSAFE_US	13	/**< the throttle failsafe pulse length in microseconds */
 
 /* autopilot control values, -10000..10000 */
 #define PX4IO_PAGE_CONTROLS			51		/**< actuator control groups, one after the other, 8 wide */
@@ -217,10 +219,10 @@ enum {							/* DSM bind states */
 #define PX4IO_P_CONTROLS_GROUP_3		(PX4IO_PROTOCOL_MAX_CONTROL_COUNT * 3)	/**< 0..PX4IO_PROTOCOL_MAX_CONTROL_COUNT - 1 */
 
 #define PX4IO_P_CONTROLS_GROUP_VALID		64
-#define PX4IO_P_CONTROLS_GROUP_VALID_GROUP0	(1 << 0) /* group 0 is valid / received */
-#define PX4IO_P_CONTROLS_GROUP_VALID_GROUP1	(1 << 1) /* group 1 is valid / received */
-#define PX4IO_P_CONTROLS_GROUP_VALID_GROUP2	(1 << 2) /* group 2 is valid / received */
-#define PX4IO_P_CONTROLS_GROUP_VALID_GROUP3	(1 << 3) /* group 3 is valid / received */
+#define PX4IO_P_CONTROLS_GROUP_VALID_GROUP0	(1 << 0) /**< group 0 is valid / received */
+#define PX4IO_P_CONTROLS_GROUP_VALID_GROUP1	(1 << 1) /**< group 1 is valid / received */
+#define PX4IO_P_CONTROLS_GROUP_VALID_GROUP2	(1 << 2) /**< group 2 is valid / received */
+#define PX4IO_P_CONTROLS_GROUP_VALID_GROUP3	(1 << 3) /**< group 3 is valid / received */
 
 /* raw text load to the mixer parser - ignores offset */
 #define PX4IO_PAGE_MIXERLOAD			52
diff --git a/src/modules/px4iofirmware/px4io.h b/src/modules/px4iofirmware/px4io.h
index 54c5663a5..4db948484 100644
--- a/src/modules/px4iofirmware/px4io.h
+++ b/src/modules/px4iofirmware/px4io.h
@@ -108,6 +108,7 @@ extern uint16_t			r_page_servo_disarmed[];	/* PX4IO_PAGE_DISARMED_PWM */
 #ifdef CONFIG_ARCH_BOARD_PX4IO_V1
 #define r_setup_relays		r_page_setup[PX4IO_P_SETUP_RELAYS]
 #endif
+#define r_setup_rc_thr_failsafe	r_page_setup[PX4IO_P_SETUP_RC_THR_FAILSAFE_US]
 
 #define r_control_values	(&r_page_controls[0])
 
diff --git a/src/modules/px4iofirmware/registers.c b/src/modules/px4iofirmware/registers.c
index 97d25bbfa..6752e7b4b 100644
--- a/src/modules/px4iofirmware/registers.c
+++ b/src/modules/px4iofirmware/registers.c
@@ -169,6 +169,7 @@ volatile uint16_t	r_page_setup[] =
 	[PX4IO_P_SETUP_SET_DEBUG]		= 0,
 	[PX4IO_P_SETUP_REBOOT_BL]		= 0,
 	[PX4IO_P_SETUP_CRC ... (PX4IO_P_SETUP_CRC+1)] = 0,
+	[PX4IO_P_SETUP_RC_THR_FAILSAFE_US] = 0,
 };
 
 #ifdef CONFIG_ARCH_BOARD_PX4IO_V2
diff --git a/src/modules/sdlog2/sdlog2.c b/src/modules/sdlog2/sdlog2.c
index ad709d27d..e62b0fafc 100644
--- a/src/modules/sdlog2/sdlog2.c
+++ b/src/modules/sdlog2/sdlog2.c
@@ -83,6 +83,9 @@
 #include <uORB/topics/rc_channels.h>
 #include <uORB/topics/esc_status.h>
 #include <uORB/topics/telemetry_status.h>
+#include <uORB/topics/estimator_status.h>
+#include <uORB/topics/system_power.h>
+#include <uORB/topics/servorail_status.h>
 
 #include <systemlib/systemlib.h>
 #include <systemlib/param/param.h>
@@ -794,6 +797,9 @@ int sdlog2_thread_main(int argc, char *argv[])
 		struct battery_status_s battery;
 		struct telemetry_status_s telemetry;
 		struct range_finder_report range_finder;
+		struct estimator_status_report estimator_status;
+		struct system_power_s system_power;
+		struct servorail_status_s servorail_status;
 	} buf;
 
 	memset(&buf, 0, sizeof(buf));
@@ -825,6 +831,8 @@ int sdlog2_thread_main(int argc, char *argv[])
 			struct log_BATT_s log_BATT;
 			struct log_DIST_s log_DIST;
 			struct log_TELE_s log_TELE;
+			struct log_ESTM_s log_ESTM;
+			struct log_PWR_s log_PWR;
 		} body;
 	} log_msg = {
 		LOG_PACKET_HEADER_INIT(0)
@@ -855,6 +863,9 @@ int sdlog2_thread_main(int argc, char *argv[])
 		int battery_sub;
 		int telemetry_sub;
 		int range_finder_sub;
+		int estimator_status_sub;
+		int system_power_sub;
+		int servorail_status_sub;
 	} subs;
 
 	subs.cmd_sub = orb_subscribe(ORB_ID(vehicle_command));
@@ -879,6 +890,9 @@ int sdlog2_thread_main(int argc, char *argv[])
 	subs.battery_sub = orb_subscribe(ORB_ID(battery_status));
 	subs.telemetry_sub = orb_subscribe(ORB_ID(telemetry_status));
 	subs.range_finder_sub = orb_subscribe(ORB_ID(sensor_range_finder));
+	subs.estimator_status_sub = orb_subscribe(ORB_ID(estimator_status));
+	subs.system_power_sub = orb_subscribe(ORB_ID(system_power));
+	subs.servorail_status_sub = orb_subscribe(ORB_ID(servorail_status));
 
 	thread_running = true;
 
@@ -1179,6 +1193,7 @@ int sdlog2_thread_main(int argc, char *argv[])
 			log_msg.msg_type = LOG_AIRS_MSG;
 			log_msg.body.log_AIRS.indicated_airspeed = buf.airspeed.indicated_airspeed_m_s;
 			log_msg.body.log_AIRS.true_airspeed = buf.airspeed.true_airspeed_m_s;
+			log_msg.body.log_AIRS.air_temperature_celsius = buf.airspeed.air_temperature_celsius;
 			LOGBUFFER_WRITE_AND_COUNT(AIRS);
 		}
 
@@ -1221,6 +1236,24 @@ int sdlog2_thread_main(int argc, char *argv[])
 			LOGBUFFER_WRITE_AND_COUNT(BATT);
 		}
 
+		/* --- SYSTEM POWER RAILS --- */
+		if (copy_if_updated(ORB_ID(system_power), subs.system_power_sub, &buf.system_power)) {
+			log_msg.msg_type = LOG_PWR_MSG;
+			log_msg.body.log_PWR.peripherals_5v = buf.system_power.voltage5V_v;
+			log_msg.body.log_PWR.usb_ok = buf.system_power.usb_connected;
+			log_msg.body.log_PWR.brick_ok = buf.system_power.brick_valid;
+			log_msg.body.log_PWR.servo_ok = buf.system_power.servo_valid;
+			log_msg.body.log_PWR.low_power_rail_overcurrent = buf.system_power.periph_5V_OC;
+			log_msg.body.log_PWR.high_power_rail_overcurrent = buf.system_power.hipower_5V_OC;
+
+			/* copy servo rail status topic here too */
+			orb_copy(ORB_ID(servorail_status), subs.servorail_status_sub, &buf.servorail_status);
+			log_msg.body.log_PWR.servo_rail_5v = buf.servorail_status.voltage_v;
+			log_msg.body.log_PWR.servo_rssi = buf.servorail_status.rssi_v;
+
+			LOGBUFFER_WRITE_AND_COUNT(PWR);
+		}
+
 		/* --- TELEMETRY --- */
 		if (copy_if_updated(ORB_ID(telemetry_status), subs.telemetry_sub, &buf.telemetry)) {
 			log_msg.msg_type = LOG_TELE_MSG;
@@ -1243,6 +1276,19 @@ int sdlog2_thread_main(int argc, char *argv[])
 			LOGBUFFER_WRITE_AND_COUNT(DIST);
 		}
 
+		/* --- ESTIMATOR STATUS --- */
+		if (copy_if_updated(ORB_ID(estimator_status), subs.estimator_status_sub, &buf.estimator_status)) {
+			log_msg.msg_type = LOG_ESTM_MSG;
+			unsigned maxcopy = (sizeof(buf.estimator_status.states) < sizeof(log_msg.body.log_ESTM.s)) ? sizeof(buf.estimator_status.states) : sizeof(log_msg.body.log_ESTM.s);
+			memset(&(log_msg.body.log_ESTM.s), 0, sizeof(log_msg.body.log_ESTM.s));
+			memcpy(&(log_msg.body.log_ESTM.s), buf.estimator_status.states, maxcopy);
+			log_msg.body.log_ESTM.n_states = buf.estimator_status.n_states;
+			log_msg.body.log_ESTM.states_nan = buf.estimator_status.states_nan;
+			log_msg.body.log_ESTM.covariance_nan = buf.estimator_status.covariance_nan;
+			log_msg.body.log_ESTM.kalman_gain_nan = buf.estimator_status.kalman_gain_nan;
+			LOGBUFFER_WRITE_AND_COUNT(ESTM);
+		}
+
 		/* signal the other thread new data, but not yet unlock */
 		if (logbuffer_count(&lb) > MIN_BYTES_TO_WRITE) {
 			/* only request write if several packets can be written at once */
diff --git a/src/modules/sdlog2/sdlog2_messages.h b/src/modules/sdlog2/sdlog2_messages.h
index fe500ad5f..a1a5856bc 100644
--- a/src/modules/sdlog2/sdlog2_messages.h
+++ b/src/modules/sdlog2/sdlog2_messages.h
@@ -174,6 +174,7 @@ struct log_OUT0_s {
 struct log_AIRS_s {
 	float indicated_airspeed;
 	float true_airspeed;
+	float air_temperature_celsius;
 };
 
 /* --- ARSP - ATTITUDE RATE SET POINT --- */
@@ -277,6 +278,29 @@ struct log_TELE_s {
 	uint8_t txbuf;
 };
 
+/* --- ESTM - ESTIMATOR STATUS --- */
+#define LOG_ESTM_MSG 23
+struct log_ESTM_s {
+	float s[10];
+	uint8_t n_states;
+	uint8_t states_nan;
+	uint8_t covariance_nan;
+	uint8_t kalman_gain_nan;
+};
+
+/* --- PWR - ONBOARD POWER SYSTEM --- */
+#define LOG_PWR_MSG 24
+struct log_PWR_s {
+	float peripherals_5v;
+	float servo_rail_5v;
+	float servo_rssi;
+	uint8_t usb_ok;
+	uint8_t brick_ok;
+	uint8_t servo_ok;
+	uint8_t low_power_rail_overcurrent;
+	uint8_t high_power_rail_overcurrent;
+};
+
 /********** SYSTEM MESSAGES, ID > 0x80 **********/
 
 /* --- TIME - TIME STAMP --- */
@@ -315,7 +339,7 @@ static const struct log_format_s log_formats[] = {
 	LOG_FORMAT(STAT, "BBfBB", "MainState,ArmState,BatRem,BatWarn,Landed"),
 	LOG_FORMAT(RC, "ffffffffB", "Ch0,Ch1,Ch2,Ch3,Ch4,Ch5,Ch6,Ch7,Count"),
 	LOG_FORMAT(OUT0, "ffffffff", "Out0,Out1,Out2,Out3,Out4,Out5,Out6,Out7"),
-	LOG_FORMAT(AIRS, "ff", "IndSpeed,TrueSpeed"),
+	LOG_FORMAT(AIRS, "fff", "IndSpeed,TrueSpeed,AirTemp"),
 	LOG_FORMAT(ARSP, "fff", "RollRateSP,PitchRateSP,YawRateSP"),
 	LOG_FORMAT(FLOW, "hhfffBB", "RawX,RawY,CompX,CompY,Dist,Q,SensID"),
 	LOG_FORMAT(GPOS, "LLfffffB", "Lat,Lon,Alt,VelN,VelE,VelD,BaroAlt,Flags"),
@@ -325,14 +349,16 @@ static const struct log_format_s log_formats[] = {
 	LOG_FORMAT(BATT, "ffff", "V,VFilt,C,Discharged"),
 	LOG_FORMAT(DIST, "ffB", "Bottom,BottomRate,Flags"),
 	LOG_FORMAT(TELE, "BBBBHHB", "RSSI,RemRSSI,Noise,RemNoise,RXErr,Fixed,TXBuf"),
+	LOG_FORMAT(ESTM, "ffffffffffBBBB", "s0,s1,s2,s3,s4,s5,s6,s7,s8,s9,n_states,states_nan,cov_nan,kgain_nan"),
+	LOG_FORMAT(PWR, "fffBBBBB", "Periph_5V,Servo_5V,RSSI,USB_OK,BRICK_OK,SRV_OK,PERIPH_OC,HIPWR_OC"),
 
 	/* system-level messages, ID >= 0x80 */
-	// FMT: don't write format of format message, it's useless
+	/* FMT: don't write format of format message, it's useless */
 	LOG_FORMAT(TIME, "Q", "StartTime"),
 	LOG_FORMAT(VER, "NZ", "Arch,FwGit"),
-	LOG_FORMAT(PARM, "Nf", "Name,Value"),
+	LOG_FORMAT(PARM, "Nf", "Name,Value")
 };
 
-static const int log_formats_num = sizeof(log_formats) / sizeof(struct log_format_s);
+static const unsigned log_formats_num = sizeof(log_formats) / sizeof(log_formats[0]);
 
 #endif /* SDLOG2_MESSAGES_H_ */
diff --git a/src/modules/sensors/sensor_params.c b/src/modules/sensors/sensor_params.c
index 288c6e00a..14f7ac812 100644
--- a/src/modules/sensors/sensor_params.c
+++ b/src/modules/sensors/sensor_params.c
@@ -668,33 +668,11 @@ PARAM_DEFINE_FLOAT(RC_SCALE_PITCH, 0.6f);
  */
 PARAM_DEFINE_FLOAT(RC_SCALE_YAW, 2.0f);
 
-
-/**
- * Failsafe channel mapping.
- *
- * @min 0
- * @max 18
- * @group Radio Calibration
- */
-PARAM_DEFINE_INT32(RC_FS_CH, 0);
-
-/**
- * Failsafe channel mode.
- *
- * 0 = too low means signal loss,
- * 1 = too high means signal loss
- *
- * @min 0
- * @max 1
- * @group Radio Calibration
- */
-PARAM_DEFINE_INT32(RC_FS_MODE, 0);
-
 /**
  * Failsafe channel PWM threshold.
  *
- * @min 0
- * @max 1
+ * @min 800
+ * @max 2200
  * @group Radio Calibration
  */
-PARAM_DEFINE_FLOAT(RC_FS_THR, 800);
+PARAM_DEFINE_INT32(RC_FAILS_THR, 0);
diff --git a/src/modules/sensors/sensors.cpp b/src/modules/sensors/sensors.cpp
index 91a8d5670..4083b8b4f 100644
--- a/src/modules/sensors/sensors.cpp
+++ b/src/modules/sensors/sensors.cpp
@@ -36,6 +36,8 @@
  * Sensor readout process.
  *
  * @author Lorenz Meier <lm@inf.ethz.ch>
+ * @author Julian Oes <joes@student.ethz.ch>
+ * @author Thomas Gubler <thomasgubler@student.ethz.ch>
  */
 
 #include <nuttx/config.h>
@@ -261,9 +263,7 @@ private:
 		float rc_scale_yaw;
 		float rc_scale_flaps;
 
-		int rc_fs_ch;
-		int rc_fs_mode;
-		float rc_fs_thr;
+		int32_t rc_fs_thr;
 
 		float battery_voltage_scaling;
 		float battery_current_scaling;
@@ -311,8 +311,6 @@ private:
 		param_t rc_scale_yaw;
 		param_t rc_scale_flaps;
 
-		param_t rc_fs_ch;
-		param_t rc_fs_mode;
 		param_t rc_fs_thr;
 
 		param_t battery_voltage_scaling;
@@ -529,9 +527,7 @@ Sensors::Sensors() :
 	_parameter_handles.rc_scale_flaps = param_find("RC_SCALE_FLAPS");
 
 	/* RC failsafe */
-	_parameter_handles.rc_fs_ch = param_find("RC_FS_CH");
-	_parameter_handles.rc_fs_mode = param_find("RC_FS_MODE");
-	_parameter_handles.rc_fs_thr = param_find("RC_FS_THR");
+	_parameter_handles.rc_fs_thr = param_find("RC_FAILS_THR");
 
 	/* gyro offsets */
 	_parameter_handles.gyro_offset[0] = param_find("SENS_GYRO_XOFF");
@@ -687,8 +683,6 @@ Sensors::parameters_update()
 	param_get(_parameter_handles.rc_scale_pitch, &(_parameters.rc_scale_pitch));
 	param_get(_parameter_handles.rc_scale_yaw, &(_parameters.rc_scale_yaw));
 	param_get(_parameter_handles.rc_scale_flaps, &(_parameters.rc_scale_flaps));
-	param_get(_parameter_handles.rc_fs_ch, &(_parameters.rc_fs_ch));
-	param_get(_parameter_handles.rc_fs_mode, &(_parameters.rc_fs_mode));
 	param_get(_parameter_handles.rc_fs_thr, &(_parameters.rc_fs_thr));
 
 	/* update RC function mappings */
@@ -1031,10 +1025,13 @@ Sensors::diff_pres_poll(struct sensor_combined_s &raw)
 		raw.differential_pressure_timestamp = _diff_pres.timestamp;
 		raw.differential_pressure_filtered_pa = _diff_pres.differential_pressure_filtered_pa;
 
+		float air_temperature_celsius = (_diff_pres.temperature > -300.0f) ? _diff_pres.temperature : (raw.baro_temp_celcius - PCB_TEMP_ESTIMATE_DEG);
+
 		_airspeed.timestamp = _diff_pres.timestamp;
 		_airspeed.indicated_airspeed_m_s = calc_indicated_airspeed(_diff_pres.differential_pressure_filtered_pa);
 		_airspeed.true_airspeed_m_s = calc_true_airspeed(_diff_pres.differential_pressure_filtered_pa + raw.baro_pres_mbar * 1e2f,
-					      raw.baro_pres_mbar * 1e2f, raw.baro_temp_celcius - PCB_TEMP_ESTIMATE_DEG);
+					      raw.baro_pres_mbar * 1e2f, air_temperature_celsius);
+		_airspeed.air_temperature_celsius = air_temperature_celsius;
 
 		/* announce the airspeed if needed, just publish else */
 		if (_airspeed_pub > 0) {
@@ -1171,16 +1168,19 @@ Sensors::adc_poll(struct sensor_combined_s &raw)
 	hrt_abstime t = hrt_absolute_time();
 	/* rate limit to 100 Hz */
 	if (t - _last_adc >= 10000) {
-		/* make space for a maximum of eight channels */
-		struct adc_msg_s buf_adc[8];
+		/* make space for a maximum of twelve channels (to ensure reading all channels at once) */
+		struct adc_msg_s buf_adc[12];
 		/* read all channels available */
 		int ret = read(_fd_adc, &buf_adc, sizeof(buf_adc));
 
 		if (ret >= (int)sizeof(buf_adc[0])) {
-			for (unsigned i = 0; i < sizeof(buf_adc) / sizeof(buf_adc[0]); i++) {
+
+			/* Read add channels we got */
+			for (unsigned i = 0; i < ret / sizeof(buf_adc[0]); i++) {
 				/* Save raw voltage values */
-				if (i < (sizeof(raw.adc_voltage_v)) / sizeof(raw.adc_voltage_v[0])) {
+				if (i < (sizeof(raw.adc_voltage_v) / sizeof(raw.adc_voltage_v[0]))) {
 					raw.adc_voltage_v[i] = buf_adc[i].am_data / (4096.0f / 3.3f);
+					raw.adc_mapping[i] = buf_adc[i].am_channel;
 				}
 
 				/* look for specific channels and process the raw voltage to measurement data */
@@ -1239,6 +1239,8 @@ Sensors::adc_poll(struct sensor_combined_s &raw)
 
 						_diff_pres.timestamp = t;
 						_diff_pres.differential_pressure_pa = diff_pres_pa;
+						_diff_pres.differential_pressure_filtered_pa = diff_pres_pa;
+						_diff_pres.temperature = -1000.0f;
 						_diff_pres.voltage = voltage;
 
 						/* announce the airspeed if needed, just publish else */
@@ -1303,19 +1305,15 @@ Sensors::rc_poll()
 		manual_control.aux5 = NAN;
 
 		/* require at least four channels to consider the signal valid */
-		if (rc_input.channel_count < 4)
+		if (rc_input.channel_count < 4) {
 			return;
+		}
 
-		/* failsafe check */
-		if (_parameters.rc_fs_ch != 0) {
-			if (_parameters.rc_fs_mode == 0) {
-				if (rc_input.values[_parameters.rc_fs_ch - 1] < _parameters.rc_fs_thr)
-					return;
-
-			} else if (_parameters.rc_fs_mode == 1) {
-				if (rc_input.values[_parameters.rc_fs_ch - 1] > _parameters.rc_fs_thr)
-					return;
-			}
+		/* check for failsafe */
+		if ((rc_input.rc_failsafe) || ((_parameters.rc_fs_thr != 0) && (((rc_input.values[_rc.function[THROTTLE]] < _parameters.min[_rc.function[THROTTLE]]) && (rc_input.values[_rc.function[THROTTLE]] < _parameters.rc_fs_thr))
+			|| ((rc_input.values[_rc.function[THROTTLE]] > _parameters.max[_rc.function[THROTTLE]]) && (rc_input.values[_rc.function[THROTTLE]] > _parameters.rc_fs_thr))))) {
+			/* do not publish manual control setpoints when there are none */
+			return;
 		}
 
 		unsigned channel_limit = rc_input.channel_count;
diff --git a/src/modules/uORB/objects_common.cpp b/src/modules/uORB/objects_common.cpp
index fb24de8d1..c8a589bb7 100644
--- a/src/modules/uORB/objects_common.cpp
+++ b/src/modules/uORB/objects_common.cpp
@@ -90,6 +90,9 @@ ORB_DEFINE(battery_status, struct battery_status_s);
 #include "topics/servorail_status.h"
 ORB_DEFINE(servorail_status, struct servorail_status_s);
 
+#include "topics/system_power.h"
+ORB_DEFINE(system_power, struct system_power_s);
+
 #include "topics/vehicle_global_position.h"
 ORB_DEFINE(vehicle_global_position, struct vehicle_global_position_s);
 
@@ -193,3 +196,6 @@ ORB_DEFINE(esc_status, struct esc_status_s);
 
 #include "topics/encoders.h"
 ORB_DEFINE(encoders, struct encoders_s);
+
+#include "topics/estimator_status.h"
+ORB_DEFINE(estimator_status, struct estimator_status_report);
diff --git a/src/modules/uORB/topics/airspeed.h b/src/modules/uORB/topics/airspeed.h
index a3da3758f..d2ee754cd 100644
--- a/src/modules/uORB/topics/airspeed.h
+++ b/src/modules/uORB/topics/airspeed.h
@@ -52,9 +52,10 @@
  * Airspeed
  */
 struct airspeed_s {
-	uint64_t	timestamp;					/**< microseconds since system boot, needed to integrate */
+	uint64_t	timestamp;			/**< microseconds since system boot, needed to integrate */
 	float		indicated_airspeed_m_s;		/**< indicated airspeed in meters per second, -1 if unknown	 */
-	float		true_airspeed_m_s;			/**< true airspeed in meters per second, -1 if unknown */
+	float		true_airspeed_m_s;		/**< true airspeed in meters per second, -1 if unknown */
+	float		air_temperature_celsius;	/**< air temperature in degrees celsius, -1000 if unknown */
 };
 
 /**
diff --git a/src/modules/uORB/topics/differential_pressure.h b/src/modules/uORB/topics/differential_pressure.h
index 3592c023c..01e14cda9 100644
--- a/src/modules/uORB/topics/differential_pressure.h
+++ b/src/modules/uORB/topics/differential_pressure.h
@@ -52,13 +52,14 @@
  * Differential pressure.
  */
 struct differential_pressure_s {
-	uint64_t	timestamp;			/**< microseconds since system boot, needed to integrate */
-	uint64_t	error_count;
+	uint64_t	timestamp;			/**< Microseconds since system boot, needed to integrate */
+	uint64_t	error_count;			/**< Number of errors detected by driver */
 	float	differential_pressure_pa;		/**< Differential pressure reading */
+	float	differential_pressure_raw_pa;		/**< Raw differential pressure reading (may be negative) */
 	float	differential_pressure_filtered_pa;	/**< Low pass filtered differential pressure reading */
 	float	max_differential_pressure_pa;		/**< Maximum differential pressure reading */
-	float		voltage;			/**< Voltage from analog airspeed sensors (voltage divider already compensated) */
-	float		temperature;			/**< Temperature provided by sensor */
+	float	voltage;				/**< Voltage from analog airspeed sensors (voltage divider already compensated) */
+	float	temperature;				/**< Temperature provided by sensor, -1000.0f if unknown */
 
 };
 
diff --git a/src/modules/uORB/topics/esc_status.h b/src/modules/uORB/topics/esc_status.h
index 11332d7a7..628824efa 100644
--- a/src/modules/uORB/topics/esc_status.h
+++ b/src/modules/uORB/topics/esc_status.h
@@ -60,7 +60,7 @@
 enum ESC_VENDOR {
 	ESC_VENDOR_GENERIC = 0,					/**< generic ESC */
 	ESC_VENDOR_MIKROKOPTER,					/**< Mikrokopter */
-	ESC_VENDOR_GRAUPNER_HOTT				/**< Graupner HoTT ESC */	
+	ESC_VENDOR_GRAUPNER_HOTT				/**< Graupner HoTT ESC */
 };
 
 enum ESC_CONNECTION_TYPE {
@@ -79,16 +79,15 @@ enum ESC_CONNECTION_TYPE {
 /**
  * Electronic speed controller status.
  */
-struct esc_status_s
-{
+struct esc_status_s {
 	/* use of a counter and timestamp recommended (but not necessary) */
 
 	uint16_t counter;   		/**< incremented by the writing thread everytime new data is stored */
 	uint64_t timestamp; 		/**< in microseconds since system start, is set whenever the writing thread stores new data */
-	
+
 	uint8_t esc_count;			/**< number of connected ESCs */
 	enum ESC_CONNECTION_TYPE esc_connectiontype;	/**< how ESCs connected to the system */
-	
+
 	struct {
 		uint16_t esc_address;			/**< Address of current ESC (in most cases 1-8 / must be set by driver) */
 		enum ESC_VENDOR esc_vendor;		/**< Vendor of current ESC */
diff --git a/src/modules/uORB/topics/estimator_status.h b/src/modules/uORB/topics/estimator_status.h
new file mode 100644
index 000000000..5530cdb21
--- /dev/null
+++ b/src/modules/uORB/topics/estimator_status.h
@@ -0,0 +1,80 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2014 PX4 Development Team. All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in
+ *    the documentation and/or other materials provided with the
+ *    distribution.
+ * 3. Neither the name PX4 nor the names of its contributors may be
+ *    used to endorse or promote products derived from this software
+ *    without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ ****************************************************************************/
+
+/**
+ * @file estimator_status.h
+ * Definition of the estimator_status_report uORB topic.
+ *
+ * @author Lorenz Meier <lm@inf.ethz.ch>
+ */
+
+#ifndef ESTIMATOR_STATUS_H_
+#define ESTIMATOR_STATUS_H_
+
+#include <stdint.h>
+#include <stdbool.h>
+#include "../uORB.h"
+
+/**
+ * @addtogroup topics
+ * @{
+ */
+
+/**
+ * Estimator status report.
+ *
+ * This is a generic status report struct which allows any of the onboard estimators
+ * to write the internal state to the system log.
+ *
+ */
+struct estimator_status_report {
+
+	/* NOTE: Ordering of fields optimized to align to 32 bit / 4 bytes - change with consideration only   */
+
+	uint64_t timestamp;			/**< Timestamp in microseconds since boot */
+	float states[32];			/**< Internal filter states */
+	float n_states;				/**< Number of states effectively used */
+	bool states_nan;			/**< If set to true, one of the states is NaN */
+	bool covariance_nan;			/**< If set to true, the covariance matrix went NaN */
+	bool kalman_gain_nan;			/**< If set to true, the Kalman gain matrix went NaN */
+
+};
+
+/**
+ * @}
+ */
+
+/* register this as object request broker structure */
+ORB_DECLARE(estimator_status);
+
+#endif
diff --git a/src/modules/uORB/topics/filtered_bottom_flow.h b/src/modules/uORB/topics/filtered_bottom_flow.h
index ab6de2613..c5d545542 100644
--- a/src/modules/uORB/topics/filtered_bottom_flow.h
+++ b/src/modules/uORB/topics/filtered_bottom_flow.h
@@ -53,8 +53,7 @@
 /**
  * Filtered bottom flow in bodyframe.
  */
-struct filtered_bottom_flow_s
-{
+struct filtered_bottom_flow_s {
 	uint64_t timestamp;		/**< time of this estimate, in microseconds since system start */
 
 	float sumx;				/**< Integrated bodyframe x flow in meters					   */
diff --git a/src/modules/uORB/topics/mission.h b/src/modules/uORB/topics/mission.h
index 194e2ed0c..9594c4c6a 100644
--- a/src/modules/uORB/topics/mission.h
+++ b/src/modules/uORB/topics/mission.h
@@ -77,8 +77,7 @@ enum ORIGIN {
  * This is the position the MAV is heading towards. If it of type loiter,
  * the MAV is circling around it with the given loiter radius in meters.
  */
-struct mission_item_s
-{
+struct mission_item_s {
 	bool altitude_is_relative;	/**< true if altitude is relative from start point	*/
 	double lat;			/**< latitude in degrees				*/
 	double lon;			/**< longitude in degrees				*/
diff --git a/src/modules/uORB/topics/navigation_capabilities.h b/src/modules/uORB/topics/navigation_capabilities.h
index 391756f99..7a5ae9891 100644
--- a/src/modules/uORB/topics/navigation_capabilities.h
+++ b/src/modules/uORB/topics/navigation_capabilities.h
@@ -52,12 +52,12 @@
  * Airspeed
  */
 struct navigation_capabilities_s {
-    float turn_distance;    /**< the optimal distance to a waypoint to switch to the next */
+	float turn_distance;		/**< the optimal distance to a waypoint to switch to the next */
 
-    /* Landing parameters: see fw_pos_control_l1/landingslope.h */
-    float landing_horizontal_slope_displacement;
-    float landing_slope_angle_rad;
-    float landing_flare_length;
+	/* Landing parameters: see fw_pos_control_l1/landingslope.h */
+	float landing_horizontal_slope_displacement;
+	float landing_slope_angle_rad;
+	float landing_flare_length;
 };
 
 /**
diff --git a/src/modules/uORB/topics/offboard_control_setpoint.h b/src/modules/uORB/topics/offboard_control_setpoint.h
index 7901b930a..68d3e494b 100644
--- a/src/modules/uORB/topics/offboard_control_setpoint.h
+++ b/src/modules/uORB/topics/offboard_control_setpoint.h
@@ -45,12 +45,11 @@
 
 /**
  * Off-board control inputs.
- * 
+ *
  * Typically sent by a ground control station / joystick or by
  * some off-board controller via C or SIMULINK.
  */
-enum OFFBOARD_CONTROL_MODE
-{
+enum OFFBOARD_CONTROL_MODE {
 	OFFBOARD_CONTROL_MODE_DIRECT = 0,
 	OFFBOARD_CONTROL_MODE_DIRECT_RATES = 1,
 	OFFBOARD_CONTROL_MODE_DIRECT_ATTITUDE = 2,
diff --git a/src/modules/uORB/topics/position_setpoint_triplet.h b/src/modules/uORB/topics/position_setpoint_triplet.h
index cf1ddfc38..34aaa30dd 100644
--- a/src/modules/uORB/topics/position_setpoint_triplet.h
+++ b/src/modules/uORB/topics/position_setpoint_triplet.h
@@ -54,24 +54,24 @@
 
 enum SETPOINT_TYPE
 {
-	SETPOINT_TYPE_NORMAL = 0,	/**< normal setpoint */
-	SETPOINT_TYPE_LOITER,		/**< loiter setpoint */
-	SETPOINT_TYPE_TAKEOFF,		/**< takeoff setpoint */
+	SETPOINT_TYPE_NORMAL = 0,		/**< normal setpoint */
+	SETPOINT_TYPE_LOITER,			/**< loiter setpoint */
+	SETPOINT_TYPE_TAKEOFF,			/**< takeoff setpoint */
 	SETPOINT_TYPE_LAND,			/**< land setpoint, altitude must be ignored, vehicle must descend until landing */
 	SETPOINT_TYPE_IDLE,			/**< do nothing, switch off motors or keep at idle speed (MC) */
 };
 
 struct position_setpoint_s
 {
-	bool valid;					/**< true if setpoint is valid */
+	bool valid;			/**< true if setpoint is valid */
 	enum SETPOINT_TYPE type;	/**< setpoint type to adjust behavior of position controller */
-	double lat;					/**< latitude, in deg */
-	double lon;					/**< longitude, in deg */
-	float alt;					/**< altitude AMSL, in m */
-	float yaw;					/**< yaw (only for multirotors), in rad [-PI..PI), NaN = hold current yaw */
+	double lat;			/**< latitude, in deg */
+	double lon;			/**< longitude, in deg */
+	float alt;			/**< altitude AMSL, in m */
+	float yaw;			/**< yaw (only for multirotors), in rad [-PI..PI), NaN = hold current yaw */
 	float loiter_radius;		/**< loiter radius (only for fixed wing), in m */
 	int8_t loiter_direction;	/**< loiter direction: 1 = CW, -1 = CCW */
-	float pitch_min;			/**< minimal pitch angle for fixed wing takeoff waypoints */
+	float pitch_min;		/**< minimal pitch angle for fixed wing takeoff waypoints */
 };
 
 /**
@@ -85,7 +85,7 @@ struct position_setpoint_triplet_s
 	struct position_setpoint_s current;
 	struct position_setpoint_s next;
 
-	nav_state_t nav_state;		/**< navigation state */
+	nav_state_t nav_state;			/**< navigation state */
 };
 
 /**
diff --git a/src/modules/uORB/topics/rc_channels.h b/src/modules/uORB/topics/rc_channels.h
index 086b2ef15..6eb20efd1 100644
--- a/src/modules/uORB/topics/rc_channels.h
+++ b/src/modules/uORB/topics/rc_channels.h
@@ -52,29 +52,28 @@
  */
 #define RC_CHANNELS_MAPPED_MAX   15
 
-/** 
+/**
  * This defines the mapping of the RC functions.
  * The value assigned to the specific function corresponds to the entry of
  * the channel array chan[].
  */
-enum RC_CHANNELS_FUNCTION
-{
-  THROTTLE = 0,
-  ROLL     = 1,
-  PITCH    = 2,
-  YAW      = 3,
-  MODE = 4,
-  RETURN = 5,
-  ASSISTED = 6,
-  MISSION = 7,
-  OFFBOARD_MODE = 8,
-  FLAPS   = 9,
-  AUX_1   = 10,
-  AUX_2   = 11,
-  AUX_3   = 12,
-  AUX_4   = 13,
-  AUX_5   = 14,
-  RC_CHANNELS_FUNCTION_MAX /**< indicates the number of functions. There can be more functions than RC channels. */
+enum RC_CHANNELS_FUNCTION {
+	THROTTLE = 0,
+	ROLL     = 1,
+	PITCH    = 2,
+	YAW      = 3,
+	MODE = 4,
+	RETURN = 5,
+	ASSISTED = 6,
+	MISSION = 7,
+	OFFBOARD_MODE = 8,
+	FLAPS   = 9,
+	AUX_1   = 10,
+	AUX_2   = 11,
+	AUX_3   = 12,
+	AUX_4   = 13,
+	AUX_5   = 14,
+	RC_CHANNELS_FUNCTION_MAX /**< indicates the number of functions. There can be more functions than RC channels. */
 };
 
 /**
@@ -85,16 +84,16 @@ enum RC_CHANNELS_FUNCTION
 struct rc_channels_s {
 
 	uint64_t timestamp;                 /**< In microseconds since boot time. */
-  uint64_t timestamp_last_valid;      /**< timestamp of last valid RC signal. */
-  struct {
-    float scaled;                     /**< Scaled to -1..1 (throttle: 0..1) */
-  } chan[RC_CHANNELS_MAPPED_MAX];
-  uint8_t chan_count;                 /**< number of valid channels */
+	uint64_t timestamp_last_valid;      /**< timestamp of last valid RC signal. */
+	struct {
+		float scaled;                     /**< Scaled to -1..1 (throttle: 0..1) */
+	} chan[RC_CHANNELS_MAPPED_MAX];
+	uint8_t chan_count;                 /**< number of valid channels */
 
-  /*String array to store the names of the functions*/
-  char function_name[RC_CHANNELS_FUNCTION_MAX][20];
-  int8_t function[RC_CHANNELS_FUNCTION_MAX];
-  uint8_t rssi;                       /**< Overall receive signal strength */
+	/*String array to store the names of the functions*/
+	char function_name[RC_CHANNELS_FUNCTION_MAX][20];
+	int8_t function[RC_CHANNELS_FUNCTION_MAX];
+	uint8_t rssi;                       /**< Overall receive signal strength */
 }; /**< radio control channels. */
 
 /**
diff --git a/src/modules/uORB/topics/sensor_combined.h b/src/modules/uORB/topics/sensor_combined.h
index cc25a83c3..fa3367de9 100644
--- a/src/modules/uORB/topics/sensor_combined.h
+++ b/src/modules/uORB/topics/sensor_combined.h
@@ -98,7 +98,8 @@ struct sensor_combined_s {
 	float baro_pres_mbar;			/**< Barometric pressure, already temp. comp.     */
 	float baro_alt_meter;			/**< Altitude, already temp. comp.                */
 	float baro_temp_celcius;		/**< Temperature in degrees celsius               */
-	float adc_voltage_v[4];			/**< ADC voltages of ADC Chan 10/11/12/13 or -1      */
+	float adc_voltage_v[10];		/**< ADC voltages of ADC Chan 10/11/12/13 or -1      */
+	unsigned adc_mapping[10];		/**< Channel indices of each of these values */
 	float mcu_temp_celcius;			/**< Internal temperature measurement of MCU */
 	uint64_t baro_timestamp;		/**< Barometer timestamp        */
 
diff --git a/src/modules/uORB/topics/subsystem_info.h b/src/modules/uORB/topics/subsystem_info.h
index cfe0bf69e..d3a7ce10d 100644
--- a/src/modules/uORB/topics/subsystem_info.h
+++ b/src/modules/uORB/topics/subsystem_info.h
@@ -50,8 +50,7 @@
 #include <stdbool.h>
 #include "../uORB.h"
 
-enum SUBSYSTEM_TYPE
-{
+enum SUBSYSTEM_TYPE {
 	SUBSYSTEM_TYPE_GYRO = 1,
 	SUBSYSTEM_TYPE_ACC = 2,
 	SUBSYSTEM_TYPE_MAG = 4,
diff --git a/src/modules/uORB/topics/system_power.h b/src/modules/uORB/topics/system_power.h
new file mode 100644
index 000000000..7763b6004
--- /dev/null
+++ b/src/modules/uORB/topics/system_power.h
@@ -0,0 +1,71 @@
+/****************************************************************************
+ *
+ *   Copyright (C) 2012-2013 PX4 Development Team. All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in
+ *    the documentation and/or other materials provided with the
+ *    distribution.
+ * 3. Neither the name PX4 nor the names of its contributors may be
+ *    used to endorse or promote products derived from this software
+ *    without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ ****************************************************************************/
+
+/**
+ * @file system_power.h
+ *
+ * Definition of the system_power voltage and power status uORB topic.
+ */
+
+#ifndef SYSTEM_POWER_H_
+#define SYSTEM_POWER_H_
+
+#include "../uORB.h"
+#include <stdint.h>
+
+/**
+ * @addtogroup topics
+ * @{
+ */
+
+/**
+ * voltage and power supply status
+ */
+struct system_power_s {
+	uint64_t	timestamp;		/**< microseconds since system boot */
+	float   	voltage5V_v;		/**< peripheral 5V rail voltage */
+	uint8_t         usb_connected:1;        /**< USB is connected when 1 */
+	uint8_t         brick_valid:1;          /**< brick power is good when 1 */
+	uint8_t         servo_valid:1;          /**< servo power is good when 1 */
+	uint8_t		periph_5V_OC:1;         /**< peripheral overcurrent when 1 */
+	uint8_t		hipower_5V_OC:1;        /**< hi power peripheral overcurrent when 1 */
+};
+
+/**
+ * @}
+ */
+
+/* register this as object request broker structure */
+ORB_DECLARE(system_power);
+
+#endif
diff --git a/src/modules/uORB/topics/telemetry_status.h b/src/modules/uORB/topics/telemetry_status.h
index 5192d4d58..76693c46e 100644
--- a/src/modules/uORB/topics/telemetry_status.h
+++ b/src/modules/uORB/topics/telemetry_status.h
@@ -44,10 +44,10 @@
 #include "../uORB.h"
 
 enum TELEMETRY_STATUS_RADIO_TYPE {
-    TELEMETRY_STATUS_RADIO_TYPE_GENERIC = 0,
-    TELEMETRY_STATUS_RADIO_TYPE_3DR_RADIO,
-    TELEMETRY_STATUS_RADIO_TYPE_UBIQUITY_BULLET,
-    TELEMETRY_STATUS_RADIO_TYPE_WIRE
+	TELEMETRY_STATUS_RADIO_TYPE_GENERIC = 0,
+	TELEMETRY_STATUS_RADIO_TYPE_3DR_RADIO,
+	TELEMETRY_STATUS_RADIO_TYPE_UBIQUITY_BULLET,
+	TELEMETRY_STATUS_RADIO_TYPE_WIRE
 };
 
 /**
@@ -57,14 +57,14 @@ enum TELEMETRY_STATUS_RADIO_TYPE {
 
 struct telemetry_status_s {
 	uint64_t timestamp;
-    enum TELEMETRY_STATUS_RADIO_TYPE type;  /**< type of the radio hardware     */
-    uint8_t rssi;              /**< local signal strength                      */
-    uint8_t remote_rssi;       /**< remote signal strength                     */
-    uint16_t rxerrors;          /**< receive errors                             */
-    uint16_t fixed;             /**< count of error corrected packets           */
-    uint8_t noise;              /**< background noise level                     */
-    uint8_t remote_noise;       /**< remote background noise level              */
-    uint8_t txbuf;              /**< how full the tx buffer is as a percentage  */
+	enum TELEMETRY_STATUS_RADIO_TYPE type;	/**< type of the radio hardware     */
+	uint8_t rssi;				/**< local signal strength                      */
+	uint8_t remote_rssi;			/**< remote signal strength                     */
+	uint16_t rxerrors;			/**< receive errors                             */
+	uint16_t fixed;				/**< count of error corrected packets           */
+	uint8_t noise;				/**< background noise level                     */
+	uint8_t remote_noise;			/**< remote background noise level              */
+	uint8_t txbuf;				/**< how full the tx buffer is as a percentage  */
 };
 
 /**
diff --git a/src/modules/uORB/topics/vehicle_attitude_setpoint.h b/src/modules/uORB/topics/vehicle_attitude_setpoint.h
index 7596f944f..d526a8ff2 100644
--- a/src/modules/uORB/topics/vehicle_attitude_setpoint.h
+++ b/src/modules/uORB/topics/vehicle_attitude_setpoint.h
@@ -52,8 +52,7 @@
 /**
  * vehicle attitude setpoint.
  */
-struct vehicle_attitude_setpoint_s
-{
+struct vehicle_attitude_setpoint_s {
 	uint64_t timestamp;		/**< in microseconds since system start, is set whenever the writing thread stores new data */
 
 	float roll_body;				/**< body angle in NED frame		*/
diff --git a/src/modules/uORB/topics/vehicle_bodyframe_speed_setpoint.h b/src/modules/uORB/topics/vehicle_bodyframe_speed_setpoint.h
index fbfab09f3..fd1ade671 100644
--- a/src/modules/uORB/topics/vehicle_bodyframe_speed_setpoint.h
+++ b/src/modules/uORB/topics/vehicle_bodyframe_speed_setpoint.h
@@ -48,8 +48,7 @@
  * @{
  */
 
-struct vehicle_bodyframe_speed_setpoint_s
-{
+struct vehicle_bodyframe_speed_setpoint_s {
 	uint64_t timestamp;		/**< in microseconds since system start, is set whenever the writing thread stores new data */
 
 	float vx;		/**< in m/s				  		*/
diff --git a/src/modules/uORB/topics/vehicle_command.h b/src/modules/uORB/topics/vehicle_command.h
index e6e4743c6..c21a29b13 100644
--- a/src/modules/uORB/topics/vehicle_command.h
+++ b/src/modules/uORB/topics/vehicle_command.h
@@ -51,43 +51,42 @@
  * Should contain all commands from MAVLink's VEHICLE_CMD ENUM,
  * but can contain additional ones.
  */
-enum VEHICLE_CMD
-{
-	VEHICLE_CMD_NAV_WAYPOINT=16, /* Navigate to MISSION. |Hold time in decimal seconds. (ignored by fixed wing, time to stay at MISSION for rotary wing)| Acceptance radius in meters (if the sphere with this radius is hit, the MISSION counts as reached)| 0 to pass through the WP, if > 0 radius in meters to pass by WP. Positive value for clockwise orbit, negative value for counter-clockwise orbit. Allows trajectory control.| Desired yaw angle at MISSION (rotary wing)| Latitude| Longitude| Altitude|  */
-	VEHICLE_CMD_NAV_LOITER_UNLIM=17, /* Loiter around this MISSION an unlimited amount of time |Empty| Empty| Radius around MISSION, in meters. If positive loiter clockwise, else counter-clockwise| Desired yaw angle.| Latitude| Longitude| Altitude|  */
-	VEHICLE_CMD_NAV_LOITER_TURNS=18, /* Loiter around this MISSION for X turns |Turns| Empty| Radius around MISSION, in meters. If positive loiter clockwise, else counter-clockwise| Desired yaw angle.| Latitude| Longitude| Altitude|  */
-	VEHICLE_CMD_NAV_LOITER_TIME=19, /* Loiter around this MISSION for X seconds |Seconds (decimal)| Empty| Radius around MISSION, in meters. If positive loiter clockwise, else counter-clockwise| Desired yaw angle.| Latitude| Longitude| Altitude|  */
-	VEHICLE_CMD_NAV_RETURN_TO_LAUNCH=20, /* Return to launch location |Empty| Empty| Empty| Empty| Empty| Empty| Empty|  */
-	VEHICLE_CMD_NAV_LAND=21, /* Land at location |Empty| Empty| Empty| Desired yaw angle.| Latitude| Longitude| Altitude|  */
-	VEHICLE_CMD_NAV_TAKEOFF=22, /* Takeoff from ground / hand |Minimum pitch (if airspeed sensor present), desired pitch without sensor| Empty| Empty| Yaw angle (if magnetometer present), ignored without magnetometer| Latitude| Longitude| Altitude|  */
-	VEHICLE_CMD_NAV_ROI=80, /* Sets the region of interest (ROI) for a sensor set or the vehicle itself. This can then be used by the vehicles control system to control the vehicle attitude and the attitude of various sensors such as cameras. |Region of intereset mode. (see MAV_ROI enum)| MISSION index/ target ID. (see MAV_ROI enum)| ROI index (allows a vehicle to manage multiple ROI's)| Empty| x the location of the fixed ROI (see MAV_FRAME)| y| z|  */
-	VEHICLE_CMD_NAV_PATHPLANNING=81, /* Control autonomous path planning on the MAV. |0: Disable local obstacle avoidance / local path planning (without resetting map), 1: Enable local path planning, 2: Enable and reset local path planning| 0: Disable full path planning (without resetting map), 1: Enable, 2: Enable and reset map/occupancy grid, 3: Enable and reset planned route, but not occupancy grid| Empty| Yaw angle at goal, in compass degrees, [0..360]| Latitude/X of goal| Longitude/Y of goal| Altitude/Z of goal|  */
-	VEHICLE_CMD_NAV_LAST=95, /* NOP - This command is only used to mark the upper limit of the NAV/ACTION commands in the enumeration |Empty| Empty| Empty| Empty| Empty| Empty| Empty|  */
-	VEHICLE_CMD_CONDITION_DELAY=112, /* Delay mission state machine. |Delay in seconds (decimal)| Empty| Empty| Empty| Empty| Empty| Empty|  */
-	VEHICLE_CMD_CONDITION_CHANGE_ALT=113, /* Ascend/descend at rate.  Delay mission state machine until desired altitude reached. |Descent / Ascend rate (m/s)| Empty| Empty| Empty| Empty| Empty| Finish Altitude|  */
-	VEHICLE_CMD_CONDITION_DISTANCE=114, /* Delay mission state machine until within desired distance of next NAV point. |Distance (meters)| Empty| Empty| Empty| Empty| Empty| Empty|  */
-	VEHICLE_CMD_CONDITION_YAW=115, /* Reach a certain target angle. |target angle: [0-360], 0 is north| speed during yaw change:[deg per second]| direction: negative: counter clockwise, positive: clockwise [-1,1]| relative offset or absolute angle: [ 1,0]| Empty| Empty| Empty|  */
-	VEHICLE_CMD_CONDITION_LAST=159, /* NOP - This command is only used to mark the upper limit of the CONDITION commands in the enumeration |Empty| Empty| Empty| Empty| Empty| Empty| Empty|  */
-	VEHICLE_CMD_DO_SET_MODE=176, /* Set system mode. |Mode, as defined by ENUM MAV_MODE| Empty| Empty| Empty| Empty| Empty| Empty|  */
-	VEHICLE_CMD_DO_JUMP=177, /* Jump to the desired command in the mission list.  Repeat this action only the specified number of times |Sequence number| Repeat count| Empty| Empty| Empty| Empty| Empty|  */
-	VEHICLE_CMD_DO_CHANGE_SPEED=178, /* Change speed and/or throttle set points. |Speed type (0=Airspeed, 1=Ground Speed)| Speed  (m/s, -1 indicates no change)| Throttle  ( Percent, -1 indicates no change)| Empty| Empty| Empty| Empty|  */
-	VEHICLE_CMD_DO_SET_HOME=179, /* Changes the home location either to the current location or a specified location. |Use current (1=use current location, 0=use specified location)| Empty| Empty| Empty| Latitude| Longitude| Altitude|  */
-	VEHICLE_CMD_DO_SET_PARAMETER=180, /* Set a system parameter.  Caution!  Use of this command requires knowledge of the numeric enumeration value of the parameter. |Parameter number| Parameter value| Empty| Empty| Empty| Empty| Empty|  */
-	VEHICLE_CMD_DO_SET_RELAY=181, /* Set a relay to a condition. |Relay number| Setting (1=on, 0=off, others possible depending on system hardware)| Empty| Empty| Empty| Empty| Empty|  */
-	VEHICLE_CMD_DO_REPEAT_RELAY=182, /* Cycle a relay on and off for a desired number of cyles with a desired period. |Relay number| Cycle count| Cycle time (seconds, decimal)| Empty| Empty| Empty| Empty|  */
-	VEHICLE_CMD_DO_SET_SERVO=183, /* Set a servo to a desired PWM value. |Servo number| PWM (microseconds, 1000 to 2000 typical)| Empty| Empty| Empty| Empty| Empty|  */
-	VEHICLE_CMD_DO_REPEAT_SERVO=184, /* Cycle a between its nominal setting and a desired PWM for a desired number of cycles with a desired period. |Servo number| PWM (microseconds, 1000 to 2000 typical)| Cycle count| Cycle time (seconds)| Empty| Empty| Empty|  */
-	VEHICLE_CMD_DO_CONTROL_VIDEO=200, /* Control onboard camera system. |Camera ID (-1 for all)| Transmission: 0: disabled, 1: enabled compressed, 2: enabled raw| Transmission mode: 0: video stream, >0: single images every n seconds (decimal)| Recording: 0: disabled, 1: enabled compressed, 2: enabled raw| Empty| Empty| Empty|  */
-	VEHICLE_CMD_DO_LAST=240, /* NOP - This command is only used to mark the upper limit of the DO commands in the enumeration |Empty| Empty| Empty| Empty| Empty| Empty| Empty|  */
-	VEHICLE_CMD_PREFLIGHT_CALIBRATION=241, /* Trigger calibration. This command will be only accepted if in pre-flight mode. |Gyro calibration: 0: no, 1: yes| Magnetometer calibration: 0: no, 1: yes| Ground pressure: 0: no, 1: yes| Radio calibration: 0: no, 1: yes| Accelerometer calibration: 0: no, 1: yes| Empty| Empty|  */
-	VEHICLE_CMD_PREFLIGHT_SET_SENSOR_OFFSETS=242, /* Set sensor offsets. This command will be only accepted if in pre-flight mode. |Sensor to adjust the offsets for: 0: gyros, 1: accelerometer, 2: magnetometer, 3: barometer, 4: optical flow| X axis offset (or generic dimension 1), in the sensor's raw units| Y axis offset (or generic dimension 2), in the sensor's raw units| Z axis offset (or generic dimension 3), in the sensor's raw units| Generic dimension 4, in the sensor's raw units| Generic dimension 5, in the sensor's raw units| Generic dimension 6, in the sensor's raw units|  */
-	VEHICLE_CMD_PREFLIGHT_STORAGE=245, /* Request storage of different parameter values and logs. This command will be only accepted if in pre-flight mode. |Parameter storage: 0: READ FROM FLASH/EEPROM, 1: WRITE CURRENT TO FLASH/EEPROM| Mission storage: 0: READ FROM FLASH/EEPROM, 1: WRITE CURRENT TO FLASH/EEPROM| Reserved| Reserved| Empty| Empty| Empty|  */
-	VEHICLE_CMD_PREFLIGHT_REBOOT_SHUTDOWN=246, /* Request the reboot or shutdown of system components. |0: Do nothing for autopilot, 1: Reboot autopilot, 2: Shutdown autopilot.| 0: Do nothing for onboard computer, 1: Reboot onboard computer, 2: Shutdown onboard computer.| Reserved| Reserved| Empty| Empty| Empty|  */
-	VEHICLE_CMD_OVERRIDE_GOTO=252, /* Hold / continue the current action |MAV_GOTO_DO_HOLD: hold MAV_GOTO_DO_CONTINUE: continue with next item in mission plan| MAV_GOTO_HOLD_AT_CURRENT_POSITION: Hold at current position MAV_GOTO_HOLD_AT_SPECIFIED_POSITION: hold at specified position| MAV_FRAME coordinate frame of hold point| Desired yaw angle in degrees| Latitude / X position| Longitude / Y position| Altitude / Z position|  */
-	VEHICLE_CMD_MISSION_START=300, /* start running a mission |first_item: the first mission item to run| last_item:  the last mission item to run (after this item is run, the mission ends)|  */
-	VEHICLE_CMD_COMPONENT_ARM_DISARM=400, /* Arms / Disarms a component |1 to arm, 0 to disarm|  */
-	VEHICLE_CMD_START_RX_PAIR=500, /* Starts receiver pairing |0:Spektrum| 0:Spektrum DSM2, 1:Spektrum DSMX|  */
-	VEHICLE_CMD_ENUM_END=501, /*  | */
+enum VEHICLE_CMD {
+	VEHICLE_CMD_NAV_WAYPOINT = 16, /* Navigate to MISSION. |Hold time in decimal seconds. (ignored by fixed wing, time to stay at MISSION for rotary wing)| Acceptance radius in meters (if the sphere with this radius is hit, the MISSION counts as reached)| 0 to pass through the WP, if > 0 radius in meters to pass by WP. Positive value for clockwise orbit, negative value for counter-clockwise orbit. Allows trajectory control.| Desired yaw angle at MISSION (rotary wing)| Latitude| Longitude| Altitude|  */
+	VEHICLE_CMD_NAV_LOITER_UNLIM = 17, /* Loiter around this MISSION an unlimited amount of time |Empty| Empty| Radius around MISSION, in meters. If positive loiter clockwise, else counter-clockwise| Desired yaw angle.| Latitude| Longitude| Altitude|  */
+	VEHICLE_CMD_NAV_LOITER_TURNS = 18, /* Loiter around this MISSION for X turns |Turns| Empty| Radius around MISSION, in meters. If positive loiter clockwise, else counter-clockwise| Desired yaw angle.| Latitude| Longitude| Altitude|  */
+	VEHICLE_CMD_NAV_LOITER_TIME = 19, /* Loiter around this MISSION for X seconds |Seconds (decimal)| Empty| Radius around MISSION, in meters. If positive loiter clockwise, else counter-clockwise| Desired yaw angle.| Latitude| Longitude| Altitude|  */
+	VEHICLE_CMD_NAV_RETURN_TO_LAUNCH = 20, /* Return to launch location |Empty| Empty| Empty| Empty| Empty| Empty| Empty|  */
+	VEHICLE_CMD_NAV_LAND = 21, /* Land at location |Empty| Empty| Empty| Desired yaw angle.| Latitude| Longitude| Altitude|  */
+	VEHICLE_CMD_NAV_TAKEOFF = 22, /* Takeoff from ground / hand |Minimum pitch (if airspeed sensor present), desired pitch without sensor| Empty| Empty| Yaw angle (if magnetometer present), ignored without magnetometer| Latitude| Longitude| Altitude|  */
+	VEHICLE_CMD_NAV_ROI = 80, /* Sets the region of interest (ROI) for a sensor set or the vehicle itself. This can then be used by the vehicles control system to control the vehicle attitude and the attitude of various sensors such as cameras. |Region of intereset mode. (see MAV_ROI enum)| MISSION index/ target ID. (see MAV_ROI enum)| ROI index (allows a vehicle to manage multiple ROI's)| Empty| x the location of the fixed ROI (see MAV_FRAME)| y| z|  */
+	VEHICLE_CMD_NAV_PATHPLANNING = 81, /* Control autonomous path planning on the MAV. |0: Disable local obstacle avoidance / local path planning (without resetting map), 1: Enable local path planning, 2: Enable and reset local path planning| 0: Disable full path planning (without resetting map), 1: Enable, 2: Enable and reset map/occupancy grid, 3: Enable and reset planned route, but not occupancy grid| Empty| Yaw angle at goal, in compass degrees, [0..360]| Latitude/X of goal| Longitude/Y of goal| Altitude/Z of goal|  */
+	VEHICLE_CMD_NAV_LAST = 95, /* NOP - This command is only used to mark the upper limit of the NAV/ACTION commands in the enumeration |Empty| Empty| Empty| Empty| Empty| Empty| Empty|  */
+	VEHICLE_CMD_CONDITION_DELAY = 112, /* Delay mission state machine. |Delay in seconds (decimal)| Empty| Empty| Empty| Empty| Empty| Empty|  */
+	VEHICLE_CMD_CONDITION_CHANGE_ALT = 113, /* Ascend/descend at rate.  Delay mission state machine until desired altitude reached. |Descent / Ascend rate (m/s)| Empty| Empty| Empty| Empty| Empty| Finish Altitude|  */
+	VEHICLE_CMD_CONDITION_DISTANCE = 114, /* Delay mission state machine until within desired distance of next NAV point. |Distance (meters)| Empty| Empty| Empty| Empty| Empty| Empty|  */
+	VEHICLE_CMD_CONDITION_YAW = 115, /* Reach a certain target angle. |target angle: [0-360], 0 is north| speed during yaw change:[deg per second]| direction: negative: counter clockwise, positive: clockwise [-1,1]| relative offset or absolute angle: [ 1,0]| Empty| Empty| Empty|  */
+	VEHICLE_CMD_CONDITION_LAST = 159, /* NOP - This command is only used to mark the upper limit of the CONDITION commands in the enumeration |Empty| Empty| Empty| Empty| Empty| Empty| Empty|  */
+	VEHICLE_CMD_DO_SET_MODE = 176, /* Set system mode. |Mode, as defined by ENUM MAV_MODE| Empty| Empty| Empty| Empty| Empty| Empty|  */
+	VEHICLE_CMD_DO_JUMP = 177, /* Jump to the desired command in the mission list.  Repeat this action only the specified number of times |Sequence number| Repeat count| Empty| Empty| Empty| Empty| Empty|  */
+	VEHICLE_CMD_DO_CHANGE_SPEED = 178, /* Change speed and/or throttle set points. |Speed type (0=Airspeed, 1=Ground Speed)| Speed  (m/s, -1 indicates no change)| Throttle  ( Percent, -1 indicates no change)| Empty| Empty| Empty| Empty|  */
+	VEHICLE_CMD_DO_SET_HOME = 179, /* Changes the home location either to the current location or a specified location. |Use current (1=use current location, 0=use specified location)| Empty| Empty| Empty| Latitude| Longitude| Altitude|  */
+	VEHICLE_CMD_DO_SET_PARAMETER = 180, /* Set a system parameter.  Caution!  Use of this command requires knowledge of the numeric enumeration value of the parameter. |Parameter number| Parameter value| Empty| Empty| Empty| Empty| Empty|  */
+	VEHICLE_CMD_DO_SET_RELAY = 181, /* Set a relay to a condition. |Relay number| Setting (1=on, 0=off, others possible depending on system hardware)| Empty| Empty| Empty| Empty| Empty|  */
+	VEHICLE_CMD_DO_REPEAT_RELAY = 182, /* Cycle a relay on and off for a desired number of cyles with a desired period. |Relay number| Cycle count| Cycle time (seconds, decimal)| Empty| Empty| Empty| Empty|  */
+	VEHICLE_CMD_DO_SET_SERVO = 183, /* Set a servo to a desired PWM value. |Servo number| PWM (microseconds, 1000 to 2000 typical)| Empty| Empty| Empty| Empty| Empty|  */
+	VEHICLE_CMD_DO_REPEAT_SERVO = 184, /* Cycle a between its nominal setting and a desired PWM for a desired number of cycles with a desired period. |Servo number| PWM (microseconds, 1000 to 2000 typical)| Cycle count| Cycle time (seconds)| Empty| Empty| Empty|  */
+	VEHICLE_CMD_DO_CONTROL_VIDEO = 200, /* Control onboard camera system. |Camera ID (-1 for all)| Transmission: 0: disabled, 1: enabled compressed, 2: enabled raw| Transmission mode: 0: video stream, >0: single images every n seconds (decimal)| Recording: 0: disabled, 1: enabled compressed, 2: enabled raw| Empty| Empty| Empty|  */
+	VEHICLE_CMD_DO_LAST = 240, /* NOP - This command is only used to mark the upper limit of the DO commands in the enumeration |Empty| Empty| Empty| Empty| Empty| Empty| Empty|  */
+	VEHICLE_CMD_PREFLIGHT_CALIBRATION = 241, /* Trigger calibration. This command will be only accepted if in pre-flight mode. |Gyro calibration: 0: no, 1: yes| Magnetometer calibration: 0: no, 1: yes| Ground pressure: 0: no, 1: yes| Radio calibration: 0: no, 1: yes| Accelerometer calibration: 0: no, 1: yes| Empty| Empty|  */
+	VEHICLE_CMD_PREFLIGHT_SET_SENSOR_OFFSETS = 242, /* Set sensor offsets. This command will be only accepted if in pre-flight mode. |Sensor to adjust the offsets for: 0: gyros, 1: accelerometer, 2: magnetometer, 3: barometer, 4: optical flow| X axis offset (or generic dimension 1), in the sensor's raw units| Y axis offset (or generic dimension 2), in the sensor's raw units| Z axis offset (or generic dimension 3), in the sensor's raw units| Generic dimension 4, in the sensor's raw units| Generic dimension 5, in the sensor's raw units| Generic dimension 6, in the sensor's raw units|  */
+	VEHICLE_CMD_PREFLIGHT_STORAGE = 245, /* Request storage of different parameter values and logs. This command will be only accepted if in pre-flight mode. |Parameter storage: 0: READ FROM FLASH/EEPROM, 1: WRITE CURRENT TO FLASH/EEPROM| Mission storage: 0: READ FROM FLASH/EEPROM, 1: WRITE CURRENT TO FLASH/EEPROM| Reserved| Reserved| Empty| Empty| Empty|  */
+	VEHICLE_CMD_PREFLIGHT_REBOOT_SHUTDOWN = 246, /* Request the reboot or shutdown of system components. |0: Do nothing for autopilot, 1: Reboot autopilot, 2: Shutdown autopilot.| 0: Do nothing for onboard computer, 1: Reboot onboard computer, 2: Shutdown onboard computer.| Reserved| Reserved| Empty| Empty| Empty|  */
+	VEHICLE_CMD_OVERRIDE_GOTO = 252, /* Hold / continue the current action |MAV_GOTO_DO_HOLD: hold MAV_GOTO_DO_CONTINUE: continue with next item in mission plan| MAV_GOTO_HOLD_AT_CURRENT_POSITION: Hold at current position MAV_GOTO_HOLD_AT_SPECIFIED_POSITION: hold at specified position| MAV_FRAME coordinate frame of hold point| Desired yaw angle in degrees| Latitude / X position| Longitude / Y position| Altitude / Z position|  */
+	VEHICLE_CMD_MISSION_START = 300, /* start running a mission |first_item: the first mission item to run| last_item:  the last mission item to run (after this item is run, the mission ends)|  */
+	VEHICLE_CMD_COMPONENT_ARM_DISARM = 400, /* Arms / Disarms a component |1 to arm, 0 to disarm|  */
+	VEHICLE_CMD_START_RX_PAIR = 500, /* Starts receiver pairing |0:Spektrum| 0:Spektrum DSM2, 1:Spektrum DSMX|  */
+	VEHICLE_CMD_ENUM_END = 501, /*  | */
 };
 
 /**
@@ -96,14 +95,13 @@ enum VEHICLE_CMD
  * Should contain all of MAVLink's VEHICLE_CMD_RESULT values
  * but can contain additional ones.
  */
-enum VEHICLE_CMD_RESULT
-{
-	VEHICLE_CMD_RESULT_ACCEPTED=0, /* Command ACCEPTED and EXECUTED | */
-	VEHICLE_CMD_RESULT_TEMPORARILY_REJECTED=1, /* Command TEMPORARY REJECTED/DENIED | */
-	VEHICLE_CMD_RESULT_DENIED=2, /* Command PERMANENTLY DENIED | */
-	VEHICLE_CMD_RESULT_UNSUPPORTED=3, /* Command UNKNOWN/UNSUPPORTED | */
-	VEHICLE_CMD_RESULT_FAILED=4, /* Command executed, but failed | */
-	VEHICLE_CMD_RESULT_ENUM_END=5, /*  | */
+enum VEHICLE_CMD_RESULT {
+	VEHICLE_CMD_RESULT_ACCEPTED = 0, /* Command ACCEPTED and EXECUTED | */
+	VEHICLE_CMD_RESULT_TEMPORARILY_REJECTED = 1, /* Command TEMPORARY REJECTED/DENIED | */
+	VEHICLE_CMD_RESULT_DENIED = 2, /* Command PERMANENTLY DENIED | */
+	VEHICLE_CMD_RESULT_UNSUPPORTED = 3, /* Command UNKNOWN/UNSUPPORTED | */
+	VEHICLE_CMD_RESULT_FAILED = 4, /* Command executed, but failed | */
+	VEHICLE_CMD_RESULT_ENUM_END = 5, /*  | */
 };
 
 /**
@@ -111,8 +109,7 @@ enum VEHICLE_CMD_RESULT
  * @{
  */
 
-struct vehicle_command_s
-{
+struct vehicle_command_s {
 	float param1;			/**< Parameter 1, as defined by MAVLink VEHICLE_CMD enum.   */
 	float param2;			/**< Parameter 2, as defined by MAVLink VEHICLE_CMD enum.   */
 	float param3;			/**< Parameter 3, as defined by MAVLink VEHICLE_CMD enum.   */
diff --git a/src/modules/uORB/topics/vehicle_control_debug.h b/src/modules/uORB/topics/vehicle_control_debug.h
index 6184284a4..2a45eaccc 100644
--- a/src/modules/uORB/topics/vehicle_control_debug.h
+++ b/src/modules/uORB/topics/vehicle_control_debug.h
@@ -47,8 +47,7 @@
  * @addtogroup topics
  * @{
  */
-struct vehicle_control_debug_s
-{
+struct vehicle_control_debug_s {
 	uint64_t timestamp; /**< in microseconds since system start */
 
 	float roll_p;		/**< roll P control part		*/
@@ -77,9 +76,9 @@ struct vehicle_control_debug_s
 
 }; /**< vehicle_control_debug */
 
- /**
- * @}
- */
+/**
+* @}
+*/
 
 /* register this as object request broker structure */
 ORB_DECLARE(vehicle_control_debug);
diff --git a/src/modules/uORB/topics/vehicle_control_mode.h b/src/modules/uORB/topics/vehicle_control_mode.h
index 7cbb37cd8..ea554006f 100644
--- a/src/modules/uORB/topics/vehicle_control_mode.h
+++ b/src/modules/uORB/topics/vehicle_control_mode.h
@@ -1,10 +1,6 @@
 /****************************************************************************
  *
- *   Copyright (C) 2012 PX4 Development Team. All rights reserved.
- *   Author: @author Lorenz Meier <lm@inf.ethz.ch>
- *           @author Petri Tanskanen <petri.tanskanen@inf.ethz.ch>
- *           @author Thomas Gubler <thomasgubler@student.ethz.ch>
- *           @author Julian Oes <joes@student.ethz.ch>
+ *   Copyright (c) 2012-2014 PX4 Development Team. All rights reserved.
  *
  * Redistribution and use in source and binary forms, with or without
  * modification, are permitted provided that the following conditions
@@ -38,8 +34,13 @@
 /**
  * @file vehicle_control_mode.h
  * Definition of the vehicle_control_mode uORB topic.
- * 
+ *
  * All control apps should depend their actions based on the flags set here.
+ *
+ * @author Lorenz Meier <lm@inf.ethz.ch>
+ * @author Petri Tanskanen <petri.tanskanen@inf.ethz.ch>
+ * @author Thomas Gubler <thomasgubler@student.ethz.ch>
+ * @author Julian Oes <joes@student.ethz.ch>
  */
 
 #ifndef VEHICLE_CONTROL_MODE
@@ -61,8 +62,7 @@
  * Encodes the complete system state and is set by the commander app.
  */
 
-struct vehicle_control_mode_s
-{
+struct vehicle_control_mode_s {
 	uint64_t timestamp; /**< in microseconds since system start, is set whenever the writing thread stores new data */
 
 	bool flag_armed;
diff --git a/src/modules/uORB/topics/vehicle_global_position.h b/src/modules/uORB/topics/vehicle_global_position.h
index ff9e98e1c..adcd028f0 100644
--- a/src/modules/uORB/topics/vehicle_global_position.h
+++ b/src/modules/uORB/topics/vehicle_global_position.h
@@ -1,9 +1,6 @@
 /****************************************************************************
  *
- *   Copyright (C) 2012 PX4 Development Team. All rights reserved.
- *   Author: @author Thomas Gubler <thomasgubler@student.ethz.ch>
- *           @author Julian Oes <joes@student.ethz.ch>
- *           @author Lorenz Meier <lm@inf.ethz.ch>
+ *   Copyright (c) 2012-2014 PX4 Development Team. All rights reserved.
  *
  * Redistribution and use in source and binary forms, with or without
  * modification, are permitted provided that the following conditions
@@ -37,6 +34,10 @@
 /**
  * @file vehicle_global_position.h
  * Definition of the global fused WGS84 position uORB topic.
+ *
+ * @author Thomas Gubler <thomasgubler@student.ethz.ch>
+ * @author Julian Oes <joes@student.ethz.ch>
+ * @author Lorenz Meier <lm@inf.ethz.ch>
  */
 
 #ifndef VEHICLE_GLOBAL_POSITION_T_H_
@@ -59,8 +60,7 @@
  * estimator, which will take more sources of information into account than just GPS,
  * e.g. control inputs of the vehicle in a Kalman-filter implementation.
  */
-struct vehicle_global_position_s
-{
+struct vehicle_global_position_s {
 	uint64_t timestamp;		/**< Time of this estimate, in microseconds since system start */
 
 	bool global_valid;		/**< true if position satisfies validity criteria of estimator */
diff --git a/src/modules/uORB/topics/vehicle_global_velocity_setpoint.h b/src/modules/uORB/topics/vehicle_global_velocity_setpoint.h
index 73961cdfe..5dac877d0 100644
--- a/src/modules/uORB/topics/vehicle_global_velocity_setpoint.h
+++ b/src/modules/uORB/topics/vehicle_global_velocity_setpoint.h
@@ -47,8 +47,7 @@
  * @{
  */
 
-struct vehicle_global_velocity_setpoint_s
-{
+struct vehicle_global_velocity_setpoint_s {
 	float vx;		/**< in m/s NED			  		*/
 	float vy;		/**< in m/s NED			  		*/
 	float vz;		/**< in m/s NED			  		*/
diff --git a/src/modules/uORB/topics/vehicle_gps_position.h b/src/modules/uORB/topics/vehicle_gps_position.h
index 1639a08c2..794c3f8bc 100644
--- a/src/modules/uORB/topics/vehicle_gps_position.h
+++ b/src/modules/uORB/topics/vehicle_gps_position.h
@@ -53,13 +53,12 @@
 /**
  * GPS position in WGS84 coordinates.
  */
-struct vehicle_gps_position_s
-{
+struct vehicle_gps_position_s {
 	uint64_t timestamp_position;			/**< Timestamp for position information */
 	int32_t lat;					/**< Latitude in 1E-7 degrees */
 	int32_t lon;					/**< Longitude in 1E-7 degrees */
 	int32_t alt;					/**< Altitude in 1E-3 meters (millimeters) above MSL  */
-	
+
 	uint64_t timestamp_variance;
 	float s_variance_m_s;				/**< speed accuracy estimate m/s */
 	float p_variance_m;				/**< position accuracy estimate m */
diff --git a/src/modules/uORB/topics/vehicle_local_position.h b/src/modules/uORB/topics/vehicle_local_position.h
index d567f2e02..db9637cd9 100644
--- a/src/modules/uORB/topics/vehicle_local_position.h
+++ b/src/modules/uORB/topics/vehicle_local_position.h
@@ -52,8 +52,7 @@
 /**
  * Fused local position in NED.
  */
-struct vehicle_local_position_s
-{
+struct vehicle_local_position_s {
 	uint64_t timestamp;		/**< Time of this estimate, in microseconds since system start */
 	bool xy_valid;			/**< true if x and y are valid */
 	bool z_valid;			/**< true if z is valid */
diff --git a/src/modules/uORB/topics/vehicle_local_position_setpoint.h b/src/modules/uORB/topics/vehicle_local_position_setpoint.h
index d24d81e3a..8988a0330 100644
--- a/src/modules/uORB/topics/vehicle_local_position_setpoint.h
+++ b/src/modules/uORB/topics/vehicle_local_position_setpoint.h
@@ -49,8 +49,7 @@
  * @{
  */
 
-struct vehicle_local_position_setpoint_s
-{
+struct vehicle_local_position_setpoint_s {
 	float x;		/**< in meters NED			  		*/
 	float y;		/**< in meters NED			  		*/
 	float z;		/**< in meters NED			  		*/
diff --git a/src/modules/uORB/topics/vehicle_rates_setpoint.h b/src/modules/uORB/topics/vehicle_rates_setpoint.h
index 46e62c4b7..9f8b412a7 100644
--- a/src/modules/uORB/topics/vehicle_rates_setpoint.h
+++ b/src/modules/uORB/topics/vehicle_rates_setpoint.h
@@ -47,8 +47,7 @@
  * @addtogroup topics
  * @{
  */
-struct vehicle_rates_setpoint_s
-{
+struct vehicle_rates_setpoint_s {
 	uint64_t timestamp; /**< in microseconds since system start */
 
 	float roll;	/**< body angular rates in NED frame		*/
@@ -58,9 +57,9 @@ struct vehicle_rates_setpoint_s
 
 }; /**< vehicle_rates_setpoint */
 
- /**
- * @}
- */
+/**
+* @}
+*/
 
 /* register this as object request broker structure */
 ORB_DECLARE(vehicle_rates_setpoint);
diff --git a/src/modules/uORB/topics/vehicle_status.h b/src/modules/uORB/topics/vehicle_status.h
index 5d40554fd..56be4d241 100644
--- a/src/modules/uORB/topics/vehicle_status.h
+++ b/src/modules/uORB/topics/vehicle_status.h
@@ -131,31 +131,31 @@ enum VEHICLE_MODE_FLAG {
  * Should match 1:1 MAVLink's MAV_TYPE ENUM
  */
 enum VEHICLE_TYPE {
-	VEHICLE_TYPE_GENERIC=0, /* Generic micro air vehicle. | */
-	VEHICLE_TYPE_FIXED_WING=1, /* Fixed wing aircraft. | */
-	VEHICLE_TYPE_QUADROTOR=2, /* Quadrotor | */
-	VEHICLE_TYPE_COAXIAL=3, /* Coaxial helicopter | */
-	VEHICLE_TYPE_HELICOPTER=4, /* Normal helicopter with tail rotor. | */
-	VEHICLE_TYPE_ANTENNA_TRACKER=5, /* Ground installation | */
-	VEHICLE_TYPE_GCS=6, /* Operator control unit / ground control station | */
-	VEHICLE_TYPE_AIRSHIP=7, /* Airship, controlled | */
-	VEHICLE_TYPE_FREE_BALLOON=8, /* Free balloon, uncontrolled | */
-	VEHICLE_TYPE_ROCKET=9, /* Rocket | */
-	VEHICLE_TYPE_GROUND_ROVER=10, /* Ground rover | */
-	VEHICLE_TYPE_SURFACE_BOAT=11, /* Surface vessel, boat, ship | */
-	VEHICLE_TYPE_SUBMARINE=12, /* Submarine | */
-	VEHICLE_TYPE_HEXAROTOR=13, /* Hexarotor | */
-	VEHICLE_TYPE_OCTOROTOR=14, /* Octorotor | */
-	VEHICLE_TYPE_TRICOPTER=15, /* Octorotor | */
-	VEHICLE_TYPE_FLAPPING_WING=16, /* Flapping wing | */
-	VEHICLE_TYPE_KITE=17, /* Kite | */
-	VEHICLE_TYPE_ENUM_END=18, /*  | */
+	VEHICLE_TYPE_GENERIC = 0, /* Generic micro air vehicle. | */
+	VEHICLE_TYPE_FIXED_WING = 1, /* Fixed wing aircraft. | */
+	VEHICLE_TYPE_QUADROTOR = 2, /* Quadrotor | */
+	VEHICLE_TYPE_COAXIAL = 3, /* Coaxial helicopter | */
+	VEHICLE_TYPE_HELICOPTER = 4, /* Normal helicopter with tail rotor. | */
+	VEHICLE_TYPE_ANTENNA_TRACKER = 5, /* Ground installation | */
+	VEHICLE_TYPE_GCS = 6, /* Operator control unit / ground control station | */
+	VEHICLE_TYPE_AIRSHIP = 7, /* Airship, controlled | */
+	VEHICLE_TYPE_FREE_BALLOON = 8, /* Free balloon, uncontrolled | */
+	VEHICLE_TYPE_ROCKET = 9, /* Rocket | */
+	VEHICLE_TYPE_GROUND_ROVER = 10, /* Ground rover | */
+	VEHICLE_TYPE_SURFACE_BOAT = 11, /* Surface vessel, boat, ship | */
+	VEHICLE_TYPE_SUBMARINE = 12, /* Submarine | */
+	VEHICLE_TYPE_HEXAROTOR = 13, /* Hexarotor | */
+	VEHICLE_TYPE_OCTOROTOR = 14, /* Octorotor | */
+	VEHICLE_TYPE_TRICOPTER = 15, /* Octorotor | */
+	VEHICLE_TYPE_FLAPPING_WING = 16, /* Flapping wing | */
+	VEHICLE_TYPE_KITE = 17, /* Kite | */
+	VEHICLE_TYPE_ENUM_END = 18, /*  | */
 };
 
 enum VEHICLE_BATTERY_WARNING {
-    VEHICLE_BATTERY_WARNING_NONE = 0,	/**< no battery low voltage warning active */
-    VEHICLE_BATTERY_WARNING_LOW,	/**< warning of low voltage */
-    VEHICLE_BATTERY_WARNING_CRITICAL	/**< alerting of critical voltage */
+	VEHICLE_BATTERY_WARNING_NONE = 0,	/**< no battery low voltage warning active */
+	VEHICLE_BATTERY_WARNING_LOW,	/**< warning of low voltage */
+	VEHICLE_BATTERY_WARNING_CRITICAL	/**< alerting of critical voltage */
 };
 
 /**
@@ -168,8 +168,7 @@ enum VEHICLE_BATTERY_WARNING {
  *
  * Encodes the complete system state and is set by the commander app.
  */
-struct vehicle_status_s
-{
+struct vehicle_status_s {
 	/* use of a counter and timestamp recommended (but not necessary) */
 
 	uint16_t counter;   /**< incremented by the writing thread everytime new data is stored */
@@ -219,7 +218,7 @@ struct vehicle_status_s
 	uint32_t onboard_control_sensors_present;
 	uint32_t onboard_control_sensors_enabled;
 	uint32_t onboard_control_sensors_health;
-	
+
 	float load;					/**< processor load from 0 to 1 */
 	float battery_voltage;
 	float battery_current;
diff --git a/src/modules/uORB/topics/vehicle_vicon_position.h b/src/modules/uORB/topics/vehicle_vicon_position.h
index 0822fa89a..e19a34a5d 100644
--- a/src/modules/uORB/topics/vehicle_vicon_position.h
+++ b/src/modules/uORB/topics/vehicle_vicon_position.h
@@ -52,8 +52,7 @@
 /**
  * Fused local position in NED.
  */
-struct vehicle_vicon_position_s
-{
+struct vehicle_vicon_position_s {
 	uint64_t timestamp;			/**< time of this estimate, in microseconds since system start */
 	bool valid;				/**< true if position satisfies validity criteria of estimator */
 
diff --git a/src/systemcmds/tests/test_adc.c b/src/systemcmds/tests/test_adc.c
index 030ac6e23..03391b851 100644
--- a/src/systemcmds/tests/test_adc.c
+++ b/src/systemcmds/tests/test_adc.c
@@ -66,8 +66,8 @@ int test_adc(int argc, char *argv[])
 	}
 
 	for (unsigned i = 0; i < 5; i++) {
-		/* make space for a maximum of eight channels */
-		struct adc_msg_s data[8];
+		/* make space for a maximum of twelve channels */
+		struct adc_msg_s data[12];
 		/* read all channels available */
 		ssize_t count = read(fd, data, sizeof(data));