Merge branch 'master' into mc_mixer_fix

101 files changed, 6521 insertions, 3785 deletions

diff --git a/Images/aerocore.prototype b/Images/aerocore.prototype
new file mode 100644
index 000000000..8502a0864
--- /dev/null
+++ b/Images/aerocore.prototype
@@ -0,0 +1,12 @@
+{
+    "board_id": 19,
+    "magic": "AeroCore",
+    "description": "Firmware for the Gumstix AeroCore board",
+    "image": "",
+    "build_time": 0,
+    "summary": "AEROCORE",
+    "version": "0.1",
+    "image_size": 0,
+    "git_identity": "",
+    "board_revision": 0
+}
diff --git a/ROMFS/px4fmu_common/init.d/rc.fw_apps b/ROMFS/px4fmu_common/init.d/rc.fw_apps
index 429abc5ec..9aca3fc5f 100644
--- a/ROMFS/px4fmu_common/init.d/rc.fw_apps
+++ b/ROMFS/px4fmu_common/init.d/rc.fw_apps
@@ -6,7 +6,7 @@
 #
 # Start the attitude and position estimator
 #
-fw_att_pos_estimator start
+ekf_att_pos_estimator start
 
 #
 # Start attitude controller
diff --git a/ROMFS/px4fmu_common/init.d/rc.fw_defaults b/ROMFS/px4fmu_common/init.d/rc.fw_defaults
index 4cd73e23f..3a50fcf56 100644
--- a/ROMFS/px4fmu_common/init.d/rc.fw_defaults
+++ b/ROMFS/px4fmu_common/init.d/rc.fw_defaults
@@ -11,6 +11,4 @@ then
 	param set NAV_RTL_ALT 100
 	param set NAV_RTL_LAND_T -1
 	param set NAV_ACCEPT_RAD 50
-	param set RC_SCALE_ROLL 1
-	param set RC_SCALE_PITCH 1
 fi
\ No newline at end of file
diff --git a/ROMFS/px4fmu_common/init.d/rc.logging b/ROMFS/px4fmu_common/init.d/rc.logging
index 3469d5f5f..009e7431a 100644
--- a/ROMFS/px4fmu_common/init.d/rc.logging
+++ b/ROMFS/px4fmu_common/init.d/rc.logging
@@ -8,7 +8,7 @@ then
 	if ver hwcmp PX4FMU_V1
 	then
 		echo "Start sdlog2 at 50Hz"
-		sdlog2 start -r 50 -a -b 8 -t
+		sdlog2 start -r 50 -a -b 5 -t
 	else
 		echo "Start sdlog2 at 200Hz"
 		sdlog2 start -r 200 -a -b 16 -t
diff --git a/ROMFS/px4fmu_common/init.d/rc.mc_apps b/ROMFS/px4fmu_common/init.d/rc.mc_apps
index ed3939757..268eb9bba 100644
--- a/ROMFS/px4fmu_common/init.d/rc.mc_apps
+++ b/ROMFS/px4fmu_common/init.d/rc.mc_apps
@@ -5,6 +5,7 @@
 #
 
 attitude_estimator_ekf start
+#ekf_att_pos_estimator start
 position_estimator_inav start
 
 mc_att_control start
diff --git a/ROMFS/px4fmu_common/init.d/rc.mc_defaults b/ROMFS/px4fmu_common/init.d/rc.mc_defaults
index a8c6dc811..b6b0a5b4e 100644
--- a/ROMFS/px4fmu_common/init.d/rc.mc_defaults
+++ b/ROMFS/px4fmu_common/init.d/rc.mc_defaults
@@ -35,6 +35,12 @@ then
 	param set MPC_TILTMAX_AIR 45.0
 	param set MPC_TILTMAX_LND 15.0
 	param set MPC_LAND_SPEED 1.0
+
+	param set PE_VELNE_NOISE 0.5
+	param set PE_VELNE_NOISE 0.7
+	param set PE_POSNE_NOISE 0.5
+	param set PE_POSD_NOISE 1.0
+
 fi
 
 set PWM_RATE 400
diff --git a/ROMFS/px4fmu_common/init.d/rc.usb b/ROMFS/px4fmu_common/init.d/rc.usb
index b7b556945..76593881d 100644
--- a/ROMFS/px4fmu_common/init.d/rc.usb
+++ b/ROMFS/px4fmu_common/init.d/rc.usb
@@ -3,16 +3,20 @@
 # USB MAVLink start
 #
 
-echo "Starting MAVLink on this USB console"
-
 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
+usleep 100000
 mavlink stream -d /dev/ttyACM0 -s OPTICAL_FLOW -r 10
+usleep 100000
 mavlink stream -d /dev/ttyACM0 -s VFR_HUD -r 20
+usleep 100000
 mavlink stream -d /dev/ttyACM0 -s ATTITUDE -r 20
+usleep 100000
 mavlink stream -d /dev/ttyACM0 -s ATTITUDE_CONTROLS -r 30
-mavlink stream -d /dev/ttyACM0 -s SERVO_OUTPUT_RAW_0 -r 10
+usleep 100000
+mavlink stream -d /dev/ttyACM0 -s SERVO_OUTPUT_RAW_0 -r 20
+usleep 100000
 
 # Exit shell to make it available to MAVLink
 exit
diff --git a/makefiles/board_aerocore.mk b/makefiles/board_aerocore.mk
new file mode 100644
index 000000000..6f4b93266
--- /dev/null
+++ b/makefiles/board_aerocore.mk
@@ -0,0 +1,11 @@
+#
+# Board-specific definitions for the Gumstix AeroCore
+#
+
+#
+# Configure the toolchain
+#
+CONFIG_ARCH			 = CORTEXM4F
+CONFIG_BOARD			 = AEROCORE
+
+include $(PX4_MK_DIR)/toolchain_gnu-arm-eabi.mk
diff --git a/makefiles/config_aerocore_default.mk b/makefiles/config_aerocore_default.mk
new file mode 100644
index 000000000..e038500a6
--- /dev/null
+++ b/makefiles/config_aerocore_default.mk
@@ -0,0 +1,125 @@
+#
+# Makefile for the AeroCore *default* configuration
+#
+
+#
+# Use the configuration's ROMFS.
+#
+ROMFS_ROOT	 = $(PX4_BASE)/ROMFS/px4fmu_common
+
+#
+# Board support modules
+#
+MODULES		+= drivers/device
+MODULES		+= drivers/stm32
+MODULES		+= drivers/stm32/adc
+MODULES		+= drivers/stm32/tone_alarm
+MODULES		+= drivers/led
+MODULES		+= drivers/px4fmu
+MODULES		+= drivers/boards/aerocore
+MODULES		+= drivers/lsm303d
+MODULES		+= drivers/l3gd20
+MODULES		+= drivers/ms5611
+MODULES		+= drivers/gps
+MODULES		+= drivers/hil
+MODULES		+= modules/sensors
+
+#
+# System commands
+#
+MODULES		+= systemcmds/boardinfo
+MODULES		+= systemcmds/mixer
+MODULES		+= systemcmds/param
+MODULES		+= systemcmds/perf
+MODULES		+= systemcmds/preflight_check
+MODULES		+= systemcmds/pwm
+MODULES		+= systemcmds/esc_calib
+MODULES		+= systemcmds/reboot
+MODULES		+= systemcmds/top
+MODULES		+= systemcmds/config
+MODULES		+= systemcmds/nshterm
+MODULES		+= systemcmds/mtd
+MODULES		+= systemcmds/dumpfile
+
+#
+# General system control
+#
+MODULES		+= modules/commander
+MODULES		+= modules/navigator
+MODULES		+= modules/mavlink
+
+#
+# Estimation modules (EKF/ SO3 / other filters)
+#
+MODULES		+= modules/attitude_estimator_ekf
+MODULES		+= modules/attitude_estimator_so3
+MODULES		+= modules/fw_att_pos_estimator
+MODULES		+= modules/position_estimator_inav
+
+#
+# Vehicle Control
+#
+MODULES		+= modules/fw_pos_control_l1
+MODULES		+= modules/fw_att_control
+MODULES		+= modules/mc_att_control
+MODULES		+= modules/mc_pos_control
+
+#
+# Library modules
+#
+MODULES		+= modules/systemlib
+MODULES		+= modules/systemlib/mixer
+MODULES		+= modules/controllib
+MODULES		+= modules/uORB
+MODULES		+= modules/dataman
+
+#
+# Libraries
+#
+LIBRARIES	+= lib/mathlib/CMSIS
+MODULES		+= lib/mathlib
+MODULES		+= lib/mathlib/math/filter
+MODULES		+= lib/ecl
+MODULES		+= lib/external_lgpl
+MODULES		+= lib/geo
+MODULES		+= lib/conversion
+MODULES		+= lib/launchdetection
+
+#
+# Demo apps
+#
+#MODULES		+= examples/math_demo
+# Tutorial code from
+# https://pixhawk.ethz.ch/px4/dev/hello_sky
+MODULES		+= examples/px4_simple_app
+
+# Tutorial code from
+# https://pixhawk.ethz.ch/px4/dev/daemon
+#MODULES		+= examples/px4_daemon_app
+
+# Tutorial code from
+# https://pixhawk.ethz.ch/px4/dev/debug_values
+#MODULES		+= examples/px4_mavlink_debug
+
+# Tutorial code from
+# https://pixhawk.ethz.ch/px4/dev/example_fixedwing_control
+#MODULES			+= examples/fixedwing_control
+
+# Hardware test
+#MODULES			+= examples/hwtest
+
+#
+# Transitional support - add commands from the NuttX export archive.
+#
+# In general, these should move to modules over time.
+#
+# Each entry here is <command>.<priority>.<stacksize>.<entrypoint> but we use a helper macro
+# to make the table a bit more readable.
+#
+define _B
+	$(strip $1).$(or $(strip $2),SCHED_PRIORITY_DEFAULT).$(or $(strip $3),CONFIG_PTHREAD_STACK_DEFAULT).$(strip $4)
+endef
+
+BUILTIN_COMMANDS := \
+    $(call _B, hello, , 2048, hello_main) \
+    $(call _B, i2c, , 2048, i2c_main)
diff --git a/makefiles/config_px4fmu-v1_default.mk b/makefiles/config_px4fmu-v1_default.mk
index 61a417f30..810e008ae 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/fw_att_pos_estimator
+MODULES		+= modules/ekf_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 65ca24325..d6e88859c 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/fw_att_pos_estimator
+MODULES		+= modules/ekf_att_pos_estimator
 MODULES		+= modules/position_estimator_inav
 MODULES		+= examples/flow_position_estimator
 
diff --git a/nuttx-configs/aerocore/include/board.h b/nuttx-configs/aerocore/include/board.h
new file mode 100644
index 000000000..8705c1bc2
--- /dev/null
+++ b/nuttx-configs/aerocore/include/board.h
@@ -0,0 +1,263 @@
+/************************************************************************************
+ * configs/aerocore/include/board.h
+ * include/arch/board/board.h
+ *
+ *   Copyright (C) 2009 Gregory Nutt. All rights reserved.
+ *   Author: Gregory Nutt <gnutt@nuttx.org>
+ *
+ * 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 NuttX 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.
+ *
+ ************************************************************************************/
+
+#ifndef __ARCH_BOARD_BOARD_H
+#define __ARCH_BOARD_BOARD_H
+
+/************************************************************************************
+ * Included Files
+ ************************************************************************************/
+
+#include <nuttx/config.h>
+#ifndef __ASSEMBLY__
+# include <stdint.h>
+#endif
+
+#include <stm32.h>
+
+/************************************************************************************
+ * Definitions
+ ************************************************************************************/
+
+/* Clocking *************************************************************************/
+/* The AeroCore uses a 24MHz crystal connected to the HSE.
+ *
+ * This is the "standard" configuration as set up by arch/arm/src/stm32f40xx_rcc.c:
+ *   System Clock source           : PLL (HSE)
+ *   SYSCLK(Hz)                    : 168000000    Determined by PLL configuration
+ *   HCLK(Hz)                      : 168000000    (STM32_RCC_CFGR_HPRE)
+ *   AHB Prescaler                 : 1            (STM32_RCC_CFGR_HPRE)
+ *   APB1 Prescaler                : 4            (STM32_RCC_CFGR_PPRE1)
+ *   APB2 Prescaler                : 2            (STM32_RCC_CFGR_PPRE2)
+ *   HSE Frequency(Hz)             : 24000000     (STM32_BOARD_XTAL)
+ *   PLLM                          : 24           (STM32_PLLCFG_PLLM)
+ *   PLLN                          : 336          (STM32_PLLCFG_PLLN)
+ *   PLLP                          : 2            (STM32_PLLCFG_PLLP)
+ *   PLLQ                          : 7            (STM32_PLLCFG_PPQ)
+ *   Main regulator output voltage : Scale1 mode  Needed for high speed SYSCLK
+ *   Flash Latency(WS)             : 5
+ *   Prefetch Buffer               : OFF
+ *   Instruction cache             : ON
+ *   Data cache                    : ON
+ *   Require 48MHz for USB OTG FS, : Enabled
+ *   SDIO and RNG clock
+ */
+
+/* HSI - 16 MHz RC factory-trimmed
+ * LSI - 32 KHz RC
+ * HSE - On-board crystal frequency is 24MHz
+ * LSE - not installed
+ */
+
+#define STM32_BOARD_XTAL        24000000ul
+
+#define STM32_HSI_FREQUENCY     16000000ul
+#define STM32_LSI_FREQUENCY     32000
+#define STM32_HSE_FREQUENCY     STM32_BOARD_XTAL
+
+/* Main PLL Configuration.
+ *
+ * PLL source is HSE
+ * PLL_VCO = (STM32_HSE_FREQUENCY / PLLM) * PLLN
+ *         = (24,000,000 / 24) * 336
+ *         = 336,000,000
+ * SYSCLK  = PLL_VCO / PLLP
+ *         = 336,000,000 / 2 = 168,000,000
+ * USB OTG FS, SDIO and RNG Clock
+ *         =  PLL_VCO / PLLQ
+ *         = 48,000,000
+ */
+
+#define STM32_PLLCFG_PLLM       RCC_PLLCFG_PLLM(24)
+#define STM32_PLLCFG_PLLN       RCC_PLLCFG_PLLN(336)
+#define STM32_PLLCFG_PLLP       RCC_PLLCFG_PLLP_2
+#define STM32_PLLCFG_PLLQ       RCC_PLLCFG_PLLQ(7)
+
+#define STM32_SYSCLK_FREQUENCY  168000000ul
+
+/* AHB clock (HCLK) is SYSCLK (168MHz) */
+
+#define STM32_RCC_CFGR_HPRE     RCC_CFGR_HPRE_SYSCLK  /* HCLK  = SYSCLK / 1 */
+#define STM32_HCLK_FREQUENCY    STM32_SYSCLK_FREQUENCY
+#define STM32_BOARD_HCLK        STM32_HCLK_FREQUENCY  /* same as above, to satisfy compiler */
+
+/* APB1 clock (PCLK1) is HCLK/4 (42MHz) */
+
+#define STM32_RCC_CFGR_PPRE1    RCC_CFGR_PPRE1_HCLKd4     /* PCLK1 = HCLK / 4 */
+#define STM32_PCLK1_FREQUENCY   (STM32_HCLK_FREQUENCY/4)
+
+/* Timers driven from APB1 will be twice PCLK1 */
+
+#define STM32_APB1_TIM2_CLKIN   (2*STM32_PCLK1_FREQUENCY)
+#define STM32_APB1_TIM3_CLKIN   (2*STM32_PCLK1_FREQUENCY)
+#define STM32_APB1_TIM4_CLKIN   (2*STM32_PCLK1_FREQUENCY)
+#define STM32_APB1_TIM5_CLKIN   (2*STM32_PCLK1_FREQUENCY)
+#define STM32_APB1_TIM6_CLKIN   (2*STM32_PCLK1_FREQUENCY)
+#define STM32_APB1_TIM7_CLKIN   (2*STM32_PCLK1_FREQUENCY)
+#define STM32_APB1_TIM12_CLKIN  (2*STM32_PCLK1_FREQUENCY)
+#define STM32_APB1_TIM13_CLKIN  (2*STM32_PCLK1_FREQUENCY)
+#define STM32_APB1_TIM14_CLKIN  (2*STM32_PCLK1_FREQUENCY)
+
+/* APB2 clock (PCLK2) is HCLK/2 (84MHz) */
+
+#define STM32_RCC_CFGR_PPRE2    RCC_CFGR_PPRE2_HCLKd2     /* PCLK2 = HCLK / 2 */
+#define STM32_PCLK2_FREQUENCY   (STM32_HCLK_FREQUENCY/2)
+
+/* Timers driven from APB2 will be twice PCLK2 */
+
+#define STM32_APB2_TIM1_CLKIN   (2*STM32_PCLK2_FREQUENCY)
+#define STM32_APB2_TIM8_CLKIN   (2*STM32_PCLK2_FREQUENCY)
+#define STM32_APB2_TIM9_CLKIN   (2*STM32_PCLK2_FREQUENCY)
+#define STM32_APB2_TIM10_CLKIN  (2*STM32_PCLK2_FREQUENCY)
+#define STM32_APB2_TIM11_CLKIN  (2*STM32_PCLK2_FREQUENCY)
+
+/* Timer Frequencies, if APBx is set to 1, frequency is same to APBx
+ * otherwise frequency is 2xAPBx. 
+ * Note: TIM1,8 are on APB2, others on APB1
+ */
+
+#define STM32_TIM18_FREQUENCY   (2*STM32_PCLK2_FREQUENCY)
+#define STM32_TIM27_FREQUENCY   (2*STM32_PCLK1_FREQUENCY)
+
+/* Alternate function pin selections ************************************************/
+
+/*
+ * UARTs.
+ */
+/* USART1 on PB[6,7]: GPS */
+#define GPIO_USART1_RX	GPIO_USART1_RX_2
+#define GPIO_USART1_TX	GPIO_USART1_TX_2
+
+/* USART2 on PD[5,6]: J5 Breakout */
+#define GPIO_USART2_RX	GPIO_USART2_RX_2
+#define GPIO_USART2_TX	GPIO_USART2_TX_2
+#define GPIO_USART2_CTS	0 // unused
+#define GPIO_USART2_RTS	0 // unused
+
+/* USART3 on PD[8,9]: to DuoVero UART2 */
+#define GPIO_USART3_RX	GPIO_USART3_RX_3
+#define GPIO_USART3_TX	GPIO_USART3_TX_3
+#define GPIO_USART3_CTS	0 // unused
+#define GPIO_USART3_RTS	0 // unused
+
+/* UART7 on PE[78]: J7 Breakout */
+#define GPIO_UART7_RX	GPIO_UART7_RX_1
+#define GPIO_UART7_TX	GPIO_UART7_TX_1
+
+/*
+ * UART8 on PE[0-1]: System Console on Port C of USB (J7)
+ * No alternate pin config
+*/
+
+/* USART[1,6] require a RX DMA configuration */
+#define DMAMAP_USART1_RX DMAMAP_USART1_RX_2
+#define DMAMAP_USART6_RX DMAMAP_USART6_RX_2
+
+/*
+ * I2C
+ *
+ * The optional _GPIO configurations allow the I2C driver to manually
+ * reset the bus to clear stuck slaves.  They match the pin configuration,
+ * but are normally-high GPIOs.
+ */
+
+/* PB[10-11]: I2C2 is broken out on J9 header */
+#define GPIO_I2C2_SCL		GPIO_I2C2_SCL_1
+#define GPIO_I2C2_SDA		GPIO_I2C2_SDA_1
+#define GPIO_I2C2_SCL_GPIO	(GPIO_OUTPUT|GPIO_OPENDRAIN|GPIO_SPEED_50MHz|GPIO_OUTPUT_SET|GPIO_PORTB|GPIO_PIN10)
+#define GPIO_I2C2_SDA_GPIO	(GPIO_OUTPUT|GPIO_OPENDRAIN|GPIO_SPEED_50MHz|GPIO_OUTPUT_SET|GPIO_PORTB|GPIO_PIN11)
+
+/*
+ * SPI
+ */
+/* PA[4-7] SPI1 broken out on J12 */ 
+#define GPIO_SPI1_NSS	(GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_SET|GPIO_PORTA|GPIO_PIN4) /* should be GPIO_SPI1_NSS_2 but use as a GPIO */
+#define GPIO_SPI1_SCK	(GPIO_SPI1_SCK_1|GPIO_SPEED_50MHz)
+#define GPIO_SPI1_MISO	(GPIO_SPI1_MISO_1|GPIO_SPEED_50MHz)
+#define GPIO_SPI1_MOSI	(GPIO_SPI1_MOSI_1|GPIO_SPEED_50MHz)
+
+/* PB[12-15]: SPI2 connected to DuoVero SPI1 */
+#define GPIO_SPI2_NSS	(GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_SET|GPIO_PORTB|GPIO_PIN9) /* should be GPIO_SPI2_NSS_2 but use as a GPIO */
+#define GPIO_SPI2_SCK	(GPIO_SPI2_SCK_2|GPIO_SPEED_50MHz)
+#define GPIO_SPI2_MISO	(GPIO_SPI2_MISO_1|GPIO_SPEED_50MHz)
+#define GPIO_SPI2_MOSI	(GPIO_SPI2_MOSI_1|GPIO_SPEED_50MHz)
+
+/* PC[10-12]: SPI3 connected to onboard sensors */
+#define GPIO_SPI3_SCK	(GPIO_SPI3_SCK_2|GPIO_SPEED_50MHz)
+#define GPIO_SPI3_MISO	(GPIO_SPI3_MISO_2|GPIO_SPEED_50MHz)
+#define GPIO_SPI3_MOSI	(GPIO_SPI3_MOSI_2|GPIO_SPEED_50MHz)
+
+/* PE[11-14]: SPI4 connected to FRAM */
+#define GPIO_SPI4_NSS	(GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_SET|GPIO_PORTE|GPIO_PIN11) /* should be GPIO_SPI4_NSS_2 but use as a GPIO */
+#define GPIO_SPI4_SCK	(GPIO_SPI4_SCK_2|GPIO_SPEED_50MHz)
+#define GPIO_SPI4_MISO	(GPIO_SPI4_MISO_2|GPIO_SPEED_50MHz)
+#define GPIO_SPI4_MOSI	(GPIO_SPI4_MOSI_2|GPIO_SPEED_50MHz)
+
+/************************************************************************************
+ * Public Data
+ ************************************************************************************/
+#ifndef __ASSEMBLY__
+
+#undef EXTERN
+#if defined(__cplusplus)
+#define EXTERN extern "C"
+extern "C" {
+#else
+#define EXTERN extern
+#endif
+
+/************************************************************************************
+ * Public Function Prototypes
+ ************************************************************************************/
+/************************************************************************************
+ * Name: stm32_boardinitialize
+ *
+ * Description:
+ *   All STM32 architectures must provide the following entry point.  This entry point
+ *   is called early in the intitialization -- after all memory has been configured
+ *   and mapped but before any devices have been initialized.
+ *
+ ************************************************************************************/
+
+EXTERN void stm32_boardinitialize(void);
+
+#undef EXTERN
+#if defined(__cplusplus)
+}
+#endif
+
+#endif /* __ASSEMBLY__ */
+#endif  /* __ARCH_BOARD_BOARD_H */
diff --git a/src/modules/att_pos_estimator_ekf/params.c b/nuttx-configs/aerocore/include/nsh_romfsimg.h
index 4af5edead..15e4e7a8d 100644
--- a/src/modules/att_pos_estimator_ekf/params.c
+++ b/nuttx-configs/aerocore/include/nsh_romfsimg.h
@@ -1,6 +1,6 @@
 /****************************************************************************
  *
- *   Copyright (c) 2012, 2013 PX4 Development Team. All rights reserved.
+ *   Copyright (C) 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
@@ -31,19 +31,12 @@
  *
  ****************************************************************************/
 
-#include <systemlib/param/param.h>
+/**
+ * nsh_romfsetc.h
+ *
+ * This file is a stub for 'make export' purposes; the actual ROMFS
+ * must be supplied by the library client.
+ */
 
-/*PARAM_DEFINE_FLOAT(NAME,0.0f);*/
-PARAM_DEFINE_FLOAT(KF_V_GYRO, 0.008f);
-PARAM_DEFINE_FLOAT(KF_V_ACCEL, 1.0f);
-PARAM_DEFINE_FLOAT(KF_R_MAG, 0.8f);
-PARAM_DEFINE_FLOAT(KF_R_GPS_VEL, 0.5f);
-PARAM_DEFINE_FLOAT(KF_R_GPS_POS, 2.0f);
-PARAM_DEFINE_FLOAT(KF_R_GPS_ALT, 3.0f);
-PARAM_DEFINE_FLOAT(KF_R_PRESS_ALT, 0.1f);
-PARAM_DEFINE_FLOAT(KF_R_ACCEL, 1.0f);
-PARAM_DEFINE_FLOAT(KF_FAULT_POS, 10.0f);
-PARAM_DEFINE_FLOAT(KF_FAULT_ATT, 10.0f);
-PARAM_DEFINE_FLOAT(KF_ENV_G, 9.765f);
-PARAM_DEFINE_FLOAT(KF_ENV_MAG_DIP, 60.0f);
-PARAM_DEFINE_FLOAT(KF_ENV_MAG_DEC, 0.0f);
+extern unsigned char romfs_img[];
+extern unsigned int romfs_img_len;
diff --git a/nuttx-configs/aerocore/nsh/Make.defs b/nuttx-configs/aerocore/nsh/Make.defs
new file mode 100644
index 000000000..c7a1b71bb
--- /dev/null
+++ b/nuttx-configs/aerocore/nsh/Make.defs
@@ -0,0 +1,179 @@
+############################################################################
+# configs/aerocore/nsh/Make.defs
+#
+#   Copyright (C) 2011 Gregory Nutt. All rights reserved.
+#   Author: Gregory Nutt <gnutt@nuttx.org>
+#
+# 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 NuttX 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.
+#
+############################################################################
+
+include ${TOPDIR}/.config
+include ${TOPDIR}/tools/Config.mk
+
+#
+# We only support building with the ARM bare-metal toolchain from
+# https://launchpad.net/gcc-arm-embedded on Windows, Linux or Mac OS.
+#
+CONFIG_ARMV7M_TOOLCHAIN	:= GNU_EABI
+
+include ${TOPDIR}/arch/arm/src/armv7-m/Toolchain.defs
+
+CC			 = $(CROSSDEV)gcc
+CXX			 = $(CROSSDEV)g++
+CPP			 = $(CROSSDEV)gcc -E
+LD			 = $(CROSSDEV)ld
+AR			 = $(CROSSDEV)ar rcs
+NM			 = $(CROSSDEV)nm
+OBJCOPY			 = $(CROSSDEV)objcopy
+OBJDUMP			 = $(CROSSDEV)objdump
+
+MAXOPTIMIZATION		 = -O3
+ARCHCPUFLAGS		 = -mcpu=cortex-m4 \
+			   -mthumb \
+			   -march=armv7e-m \
+			   -mfpu=fpv4-sp-d16 \
+			   -mfloat-abi=hard
+
+
+# enable precise stack overflow tracking
+INSTRUMENTATIONDEFINES	 = -finstrument-functions \
+			   -ffixed-r10
+
+# pull in *just* libm from the toolchain ... this is grody
+LIBM			 = "${shell $(CC) $(ARCHCPUFLAGS) -print-file-name=libm.a}"
+EXTRA_LIBS		+= $(LIBM)
+
+# use our linker script
+LDSCRIPT		 = ld.script
+
+ifeq ($(WINTOOL),y)
+  # Windows-native toolchains
+  DIRLINK		 = $(TOPDIR)/tools/copydir.sh
+  DIRUNLINK		 = $(TOPDIR)/tools/unlink.sh
+  MKDEP			 = $(TOPDIR)/tools/mknulldeps.sh
+  ARCHINCLUDES		 = -I. -isystem "${shell cygpath -w $(TOPDIR)/include}"
+  ARCHXXINCLUDES	 = -I. -isystem "${shell cygpath -w $(TOPDIR)/include}" -isystem "${shell cygpath -w $(TOPDIR)/include/cxx}"
+  ARCHSCRIPT		 = -T "${shell cygpath -w $(TOPDIR)/configs/$(CONFIG_ARCH_BOARD)/scripts/$(LDSCRIPT)}"
+else
+  ifeq ($(PX4_WINTOOL),y)
+    # Windows-native toolchains (MSYS)
+    DIRLINK		 = $(TOPDIR)/tools/copydir.sh
+    DIRUNLINK		 = $(TOPDIR)/tools/unlink.sh
+    MKDEP		 = $(TOPDIR)/tools/mknulldeps.sh
+    ARCHINCLUDES	 = -I. -isystem $(TOPDIR)/include
+    ARCHXXINCLUDES	 = -I. -isystem $(TOPDIR)/include -isystem $(TOPDIR)/include/cxx
+    ARCHSCRIPT		 = -T$(TOPDIR)/configs/$(CONFIG_ARCH_BOARD)/scripts/$(LDSCRIPT)
+  else
+    # Linux/Cygwin-native toolchain 
+    MKDEP		 = $(TOPDIR)/tools/mkdeps.sh
+    ARCHINCLUDES	 = -I. -isystem $(TOPDIR)/include
+    ARCHXXINCLUDES	 = -I. -isystem $(TOPDIR)/include -isystem $(TOPDIR)/include/cxx
+    ARCHSCRIPT		 = -T$(TOPDIR)/configs/$(CONFIG_ARCH_BOARD)/scripts/$(LDSCRIPT)
+  endif
+endif
+
+# tool versions
+ARCHCCVERSION		 = ${shell $(CC) -v 2>&1 | sed -n '/^gcc version/p' | sed -e 's/^gcc version \([0-9\.]\)/\1/g' -e 's/[-\ ].*//g' -e '1q'}
+ARCHCCMAJOR		 = ${shell echo $(ARCHCCVERSION) | cut -d'.' -f1}
+
+# optimisation flags
+ARCHOPTIMIZATION	 = $(MAXOPTIMIZATION) \
+			   -fno-strict-aliasing \
+			   -fno-strength-reduce \
+			   -fomit-frame-pointer \
+   			   -funsafe-math-optimizations \
+   			   -fno-builtin-printf \
+   			   -ffunction-sections \
+   			   -fdata-sections
+
+ifeq ("${CONFIG_DEBUG_SYMBOLS}","y")
+ARCHOPTIMIZATION	+= -g
+endif
+
+ARCHCFLAGS		 = -std=gnu99
+ARCHCXXFLAGS		 = -fno-exceptions -fno-rtti -std=gnu++0x
+ARCHWARNINGS		 = -Wall \
+			   -Wextra \
+			   -Wdouble-promotion \
+			   -Wshadow \
+			   -Wfloat-equal \
+			   -Wframe-larger-than=1024 \
+			   -Wpointer-arith \
+			   -Wlogical-op \
+			   -Wmissing-declarations \
+			   -Wpacked \
+			   -Wno-unused-parameter
+#   -Wcast-qual  - generates spurious noreturn attribute warnings, try again later
+#   -Wconversion - would be nice, but too many "risky-but-safe" conversions in the code
+#   -Wcast-align - would help catch bad casts in some cases, but generates too many false positives
+
+ARCHCWARNINGS		 = $(ARCHWARNINGS) \
+			   -Wbad-function-cast \
+			   -Wstrict-prototypes \
+			   -Wold-style-declaration \
+			   -Wmissing-parameter-type \
+			   -Wmissing-prototypes \
+			   -Wnested-externs \
+			   -Wunsuffixed-float-constants
+ARCHWARNINGSXX		 = $(ARCHWARNINGS) \
+			   -Wno-psabi
+ARCHDEFINES		 =
+ARCHPICFLAGS		 = -fpic -msingle-pic-base -mpic-register=r10
+
+# this seems to be the only way to add linker flags
+EXTRA_LIBS		+= --warn-common \
+			   --gc-sections
+
+CFLAGS			 = $(ARCHCFLAGS) $(ARCHCWARNINGS) $(ARCHOPTIMIZATION) $(ARCHCPUFLAGS) $(ARCHINCLUDES) $(INSTRUMENTATIONDEFINES) $(ARCHDEFINES) $(EXTRADEFINES) -pipe -fno-common
+CPICFLAGS		 = $(ARCHPICFLAGS) $(CFLAGS)
+CXXFLAGS		 = $(ARCHCXXFLAGS) $(ARCHWARNINGSXX) $(ARCHOPTIMIZATION) $(ARCHCPUFLAGS) $(ARCHXXINCLUDES) $(INSTRUMENTATIONDEFINES) $(ARCHDEFINES) $(EXTRADEFINES) -pipe
+CXXPICFLAGS		 = $(ARCHPICFLAGS) $(CXXFLAGS)
+CPPFLAGS		 = $(ARCHINCLUDES) $(INSTRUMENTATIONDEFINES) $(ARCHDEFINES) $(EXTRADEFINES)
+AFLAGS			 = $(CFLAGS) -D__ASSEMBLY__
+
+NXFLATLDFLAGS1		 = -r -d -warn-common
+NXFLATLDFLAGS2		 = $(NXFLATLDFLAGS1) -T$(TOPDIR)/binfmt/libnxflat/gnu-nxflat.ld -no-check-sections
+LDNXFLATFLAGS		 = -e main -s 2048
+
+OBJEXT			 = .o
+LIBEXT			 = .a
+EXEEXT			 =
+
+
+# produce partially-linked $1 from files in $2
+define PRELINK
+	@echo "PRELINK: $1"
+	$(Q) $(LD) -Ur -o $1 $2 && $(OBJCOPY) --localize-hidden $1
+endef
+
+HOSTCC			 = gcc
+HOSTINCLUDES		 = -I.
+HOSTCFLAGS		 = -Wall -Wstrict-prototypes -Wshadow -g -pipe
+HOSTLDFLAGS		 =
+
diff --git a/src/modules/att_pos_estimator_ekf/module.mk b/nuttx-configs/aerocore/nsh/appconfig
index 8d4a40d95..2850dac06 100644
--- a/src/modules/att_pos_estimator_ekf/module.mk
+++ b/nuttx-configs/aerocore/nsh/appconfig
@@ -1,6 +1,8 @@
 ############################################################################
+# configs/aerocore/nsh/appconfig
 #
-#   Copyright (c) 2012, 2013 PX4 Development Team. All rights reserved.
+#   Copyright (C) 2011 Gregory Nutt. All rights reserved.
+#   Author: Gregory Nutt <gnutt@nuttx.org>
 #
 # Redistribution and use in source and binary forms, with or without
 # modification, are permitted provided that the following conditions
@@ -12,7 +14,7 @@
 #    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
+# 3. Neither the name NuttX nor the names of its contributors may be
 #    used to endorse or promote products derived from this software
 #    without specific prior written permission.
 #
@@ -31,12 +33,10 @@
 #
 ############################################################################
 
-#
-# Full attitude / position Extended Kalman Filter
-#
+# Path to example in apps/examples containing the user_start entry point
 
-MODULE_COMMAND	= att_pos_estimator_ekf
+CONFIGURED_APPS	+= examples/nsh
 
-SRCS		= kalman_main.cpp \
-		  KalmanNav.cpp \
-		  params.c
+# The NSH application library
+CONFIGURED_APPS += nshlib
+CONFIGURED_APPS += system/readline
diff --git a/nuttx-configs/aerocore/nsh/defconfig b/nuttx-configs/aerocore/nsh/defconfig
new file mode 100644
index 000000000..8d0bae7b9
--- /dev/null
+++ b/nuttx-configs/aerocore/nsh/defconfig
@@ -0,0 +1,950 @@
+#
+# Automatically generated file; DO NOT EDIT.
+# Nuttx/ Configuration
+#
+CONFIG_NUTTX_NEWCONFIG=y
+
+#
+# Build Setup
+#
+# CONFIG_EXPERIMENTAL is not set
+CONFIG_HOST_LINUX=y
+# CONFIG_HOST_OSX is not set
+# CONFIG_HOST_WINDOWS is not set
+# CONFIG_HOST_OTHER is not set
+
+#
+# Build Configuration
+#
+CONFIG_APPS_DIR="../apps"
+# CONFIG_BUILD_2PASS is not set
+
+#
+# Binary Output Formats
+#
+# CONFIG_RRLOAD_BINARY is not set
+# CONFIG_INTELHEX_BINARY is not set
+# CONFIG_MOTOROLA_SREC is not set
+CONFIG_RAW_BINARY=y
+
+#
+# Customize Header Files
+#
+# CONFIG_ARCH_STDBOOL_H is not set
+CONFIG_ARCH_MATH_H=y
+# CONFIG_ARCH_FLOAT_H is not set
+# CONFIG_ARCH_STDARG_H is not set
+
+#
+# Debug Options
+#
+# CONFIG_DEBUG is not set
+# CONFIG_DEBUG_SYMBOLS is not set
+
+#
+# System Type
+#
+# CONFIG_ARCH_8051 is not set
+CONFIG_ARCH_ARM=y
+# CONFIG_ARCH_AVR is not set
+# CONFIG_ARCH_HC is not set
+# CONFIG_ARCH_MIPS is not set
+# CONFIG_ARCH_RGMP is not set
+# CONFIG_ARCH_SH is not set
+# CONFIG_ARCH_SIM is not set
+# CONFIG_ARCH_X86 is not set
+# CONFIG_ARCH_Z16 is not set
+# CONFIG_ARCH_Z80 is not set
+CONFIG_ARCH="arm"
+
+#
+# ARM Options
+#
+# CONFIG_ARCH_CHIP_C5471 is not set
+# CONFIG_ARCH_CHIP_CALYPSO is not set
+# CONFIG_ARCH_CHIP_DM320 is not set
+# CONFIG_ARCH_CHIP_IMX is not set
+# CONFIG_ARCH_CHIP_KINETIS is not set
+# CONFIG_ARCH_CHIP_KL is not set
+# CONFIG_ARCH_CHIP_LM is not set
+# CONFIG_ARCH_CHIP_LPC17XX is not set
+# CONFIG_ARCH_CHIP_LPC214X is not set
+# CONFIG_ARCH_CHIP_LPC2378 is not set
+# CONFIG_ARCH_CHIP_LPC31XX is not set
+# CONFIG_ARCH_CHIP_LPC43XX is not set
+# CONFIG_ARCH_CHIP_NUC1XX is not set
+# CONFIG_ARCH_CHIP_SAM34 is not set
+CONFIG_ARCH_CHIP_STM32=y
+# CONFIG_ARCH_CHIP_STR71X is not set
+CONFIG_ARCH_CORTEXM4=y
+CONFIG_ARCH_FAMILY="armv7-m"
+CONFIG_ARCH_CHIP="stm32"
+CONFIG_ARMV7M_USEBASEPRI=y
+CONFIG_ARCH_HAVE_CMNVECTOR=y
+CONFIG_ARMV7M_CMNVECTOR=y
+CONFIG_ARCH_HAVE_FPU=y
+CONFIG_ARCH_FPU=y
+CONFIG_ARCH_HAVE_MPU=y
+# CONFIG_ARMV7M_MPU is not set
+
+#
+# ARMV7M Configuration Options
+#
+# CONFIG_ARMV7M_TOOLCHAIN_BUILDROOT is not set
+# CONFIG_ARMV7M_TOOLCHAIN_CODEREDL is not set
+# CONFIG_ARMV7M_TOOLCHAIN_CODESOURCERYL is not set
+CONFIG_ARMV7M_TOOLCHAIN_GNU_EABI=y
+CONFIG_ARMV7M_STACKCHECK=y
+CONFIG_SERIAL_TERMIOS=y
+
+#
+# STM32 Configuration Options
+#
+# CONFIG_ARCH_CHIP_STM32L151C6 is not set
+# CONFIG_ARCH_CHIP_STM32L151C8 is not set
+# CONFIG_ARCH_CHIP_STM32L151CB is not set
+# CONFIG_ARCH_CHIP_STM32L151R6 is not set
+# CONFIG_ARCH_CHIP_STM32L151R8 is not set
+# CONFIG_ARCH_CHIP_STM32L151RB is not set
+# CONFIG_ARCH_CHIP_STM32L151V6 is not set
+# CONFIG_ARCH_CHIP_STM32L151V8 is not set
+# CONFIG_ARCH_CHIP_STM32L151VB is not set
+# CONFIG_ARCH_CHIP_STM32L152C6 is not set
+# CONFIG_ARCH_CHIP_STM32L152C8 is not set
+# CONFIG_ARCH_CHIP_STM32L152CB is not set
+# CONFIG_ARCH_CHIP_STM32L152R6 is not set
+# CONFIG_ARCH_CHIP_STM32L152R8 is not set
+# CONFIG_ARCH_CHIP_STM32L152RB is not set
+# CONFIG_ARCH_CHIP_STM32L152V6 is not set
+# CONFIG_ARCH_CHIP_STM32L152V8 is not set
+# CONFIG_ARCH_CHIP_STM32L152VB is not set
+# CONFIG_ARCH_CHIP_STM32F100C8 is not set
+# CONFIG_ARCH_CHIP_STM32F100CB is not set
+# CONFIG_ARCH_CHIP_STM32F100R8 is not set
+# CONFIG_ARCH_CHIP_STM32F100RB is not set
+# CONFIG_ARCH_CHIP_STM32F100RC is not set
+# CONFIG_ARCH_CHIP_STM32F100RD is not set
+# CONFIG_ARCH_CHIP_STM32F100RE is not set
+# CONFIG_ARCH_CHIP_STM32F100V8 is not set
+# CONFIG_ARCH_CHIP_STM32F100VB is not set
+# CONFIG_ARCH_CHIP_STM32F100VC is not set
+# CONFIG_ARCH_CHIP_STM32F100VD is not set
+# CONFIG_ARCH_CHIP_STM32F100VE is not set
+# CONFIG_ARCH_CHIP_STM32F103C4 is not set
+# CONFIG_ARCH_CHIP_STM32F103C8 is not set
+# CONFIG_ARCH_CHIP_STM32F103RET6 is not set
+# CONFIG_ARCH_CHIP_STM32F103VCT6 is not set
+# CONFIG_ARCH_CHIP_STM32F103VET6 is not set
+# CONFIG_ARCH_CHIP_STM32F103ZET6 is not set
+# CONFIG_ARCH_CHIP_STM32F105VBT7 is not set
+# CONFIG_ARCH_CHIP_STM32F107VC is not set
+# CONFIG_ARCH_CHIP_STM32F207IG is not set
+# CONFIG_ARCH_CHIP_STM32F302CB is not set
+# CONFIG_ARCH_CHIP_STM32F302CC is not set
+# CONFIG_ARCH_CHIP_STM32F302RB is not set
+# CONFIG_ARCH_CHIP_STM32F302RC is not set
+# CONFIG_ARCH_CHIP_STM32F302VB is not set
+# CONFIG_ARCH_CHIP_STM32F302VC is not set
+# CONFIG_ARCH_CHIP_STM32F303CB is not set
+# CONFIG_ARCH_CHIP_STM32F303CC is not set
+# CONFIG_ARCH_CHIP_STM32F303RB is not set
+# CONFIG_ARCH_CHIP_STM32F303RC is not set
+# CONFIG_ARCH_CHIP_STM32F303VB is not set
+# CONFIG_ARCH_CHIP_STM32F303VC is not set
+# CONFIG_ARCH_CHIP_STM32F405RG is not set
+# CONFIG_ARCH_CHIP_STM32F405VG is not set
+# CONFIG_ARCH_CHIP_STM32F405ZG is not set
+# CONFIG_ARCH_CHIP_STM32F407VE is not set
+# CONFIG_ARCH_CHIP_STM32F407VG is not set
+# CONFIG_ARCH_CHIP_STM32F407ZE is not set
+# CONFIG_ARCH_CHIP_STM32F407ZG is not set
+# CONFIG_ARCH_CHIP_STM32F407IE is not set
+# CONFIG_ARCH_CHIP_STM32F407IG is not set
+CONFIG_ARCH_CHIP_STM32F427V=y
+# CONFIG_ARCH_CHIP_STM32F427Z is not set
+# CONFIG_ARCH_CHIP_STM32F427I is not set
+# CONFIG_STM32_STM32L15XX is not set
+# CONFIG_STM32_ENERGYLITE is not set
+# CONFIG_STM32_STM32F10XX is not set
+# CONFIG_STM32_VALUELINE is not set
+# CONFIG_STM32_CONNECTIVITYLINE is not set
+# CONFIG_STM32_PERFORMANCELINE is not set
+# CONFIG_STM32_HIGHDENSITY is not set
+# CONFIG_STM32_MEDIUMDENSITY is not set
+# CONFIG_STM32_LOWDENSITY is not set
+# CONFIG_STM32_STM32F20XX is not set
+# CONFIG_STM32_STM32F30XX is not set
+CONFIG_STM32_STM32F40XX=y
+CONFIG_STM32_STM32F427=y
+# CONFIG_STM32_DFU is not set
+
+#
+# STM32 Peripheral Support
+#
+CONFIG_STM32_ADC1=y
+# CONFIG_STM32_ADC2 is not set
+# CONFIG_STM32_ADC3 is not set
+CONFIG_STM32_BKPSRAM=y
+# CONFIG_STM32_CAN1 is not set
+# CONFIG_STM32_CAN2 is not set
+CONFIG_STM32_CCMDATARAM=y
+# CONFIG_STM32_CRC is not set
+# CONFIG_STM32_CRYP is not set
+CONFIG_STM32_DMA1=y
+CONFIG_STM32_DMA2=y
+# CONFIG_STM32_DAC1 is not set
+# CONFIG_STM32_DAC2 is not set
+# CONFIG_STM32_DCMI is not set
+# CONFIG_STM32_ETHMAC is not set
+# CONFIG_STM32_FSMC is not set
+# CONFIG_STM32_HASH is not set
+# CONFIG_STM32_I2C1 is not set
+CONFIG_STM32_I2C2=y
+# CONFIG_STM32_I2C3 is not set
+# CONFIG_STM32_OTGFS is not set
+# CONFIG_STM32_OTGHS is not set
+CONFIG_STM32_PWR=y
+# CONFIG_STM32_RNG is not set
+# CONFIG_STM32_SDIO is not set
+CONFIG_STM32_SPI1=y
+CONFIG_STM32_SPI2=y
+CONFIG_STM32_SPI3=y
+CONFIG_STM32_SPI4=y
+# CONFIG_STM32_SPI5 is not set
+# CONFIG_STM32_SPI6 is not set
+CONFIG_STM32_SYSCFG=y
+CONFIG_STM32_TIM1=y
+# CONFIG_STM32_TIM2 is not set
+CONFIG_STM32_TIM3=y
+CONFIG_STM32_TIM4=y
+CONFIG_STM32_TIM5=y
+# CONFIG_STM32_TIM6 is not set
+# CONFIG_STM32_TIM7 is not set
+# CONFIG_STM32_TIM8 is not set
+CONFIG_STM32_TIM9=y
+# CONFIG_STM32_TIM10 is not set
+# CONFIG_STM32_TIM11 is not set
+# CONFIG_STM32_TIM12 is not set
+# CONFIG_STM32_TIM13 is not set
+# CONFIG_STM32_TIM14 is not set
+CONFIG_STM32_USART1=y
+CONFIG_STM32_USART2=y
+CONFIG_STM32_USART3=y
+# CONFIG_STM32_UART4 is not set
+# CONFIG_STM32_UART5 is not set
+# CONFIG_STM32_USART6 is not set
+CONFIG_STM32_UART7=y
+CONFIG_STM32_UART8=y
+# CONFIG_STM32_IWDG is not set
+CONFIG_STM32_WWDG=y
+CONFIG_STM32_ADC=y
+CONFIG_STM32_SPI=y
+CONFIG_STM32_I2C=y
+
+#
+# Alternate Pin Mapping
+#
+CONFIG_STM32_FLASH_PREFETCH=y
+# CONFIG_STM32_JTAG_DISABLE is not set
+# CONFIG_STM32_JTAG_FULL_ENABLE is not set
+# CONFIG_STM32_JTAG_NOJNTRST_ENABLE is not set
+CONFIG_STM32_JTAG_SW_ENABLE=y
+CONFIG_STM32_DISABLE_IDLE_SLEEP_DURING_DEBUG=y
+# CONFIG_STM32_FORCEPOWER is not set
+# CONFIG_ARCH_BOARD_STM32_CUSTOM_CLOCKCONFIG is not set
+# CONFIG_STM32_CCMEXCLUDE is not set
+CONFIG_STM32_DMACAPABLE=y
+# CONFIG_STM32_TIM1_PWM is not set
+# CONFIG_STM32_TIM3_PWM is not set
+# CONFIG_STM32_TIM4_PWM is not set
+# CONFIG_STM32_TIM5_PWM is not set
+# CONFIG_STM32_TIM9_PWM is not set
+# CONFIG_STM32_TIM1_ADC is not set
+# CONFIG_STM32_TIM3_ADC is not set
+# CONFIG_STM32_TIM4_ADC is not set
+# CONFIG_STM32_TIM5_ADC is not set
+CONFIG_STM32_USART=y
+
+#
+# U[S]ART Configuration
+#
+# CONFIG_USART1_RS485 is not set
+CONFIG_USART1_RXDMA=y
+# CONFIG_USART2_RS485 is not set
+CONFIG_USART2_RXDMA=y
+# CONFIG_USART3_RS485 is not set
+CONFIG_USART3_RXDMA=y
+# CONFIG_UART7_RS485 is not set
+CONFIG_UART7_RXDMA=y
+# CONFIG_UART8_RS485 is not set
+CONFIG_UART8_RXDMA=y
+CONFIG_SERIAL_DISABLE_REORDERING=y
+CONFIG_STM32_USART_SINGLEWIRE=y
+
+#
+# SPI Configuration
+#
+# CONFIG_STM32_SPI_INTERRUPTS is not set
+# CONFIG_STM32_SPI_DMA is not set
+
+#
+# I2C Configuration
+#
+# CONFIG_STM32_I2C_DYNTIMEO is not set
+CONFIG_STM32_I2CTIMEOSEC=0
+CONFIG_STM32_I2CTIMEOMS=10
+CONFIG_STM32_I2CTIMEOTICKS=500
+# CONFIG_STM32_I2C_DUTY16_9 is not set
+
+#
+# USB Host Configuration
+#
+
+#
+# USB Device Configuration
+#
+
+#
+# External Memory Configuration
+#
+
+#
+# Architecture Options
+#
+# CONFIG_ARCH_NOINTC is not set
+# CONFIG_ARCH_VECNOTIRQ is not set
+CONFIG_ARCH_DMA=y
+CONFIG_ARCH_IRQPRIO=y
+# CONFIG_CUSTOM_STACK is not set
+# CONFIG_ADDRENV is not set
+CONFIG_ARCH_HAVE_VFORK=y
+CONFIG_ARCH_STACKDUMP=y
+# CONFIG_ENDIAN_BIG is not set
+# CONFIG_ARCH_HAVE_RAMFUNCS is not set
+CONFIG_ARCH_HAVE_RAMVECTORS=y
+# CONFIG_ARCH_RAMVECTORS is not set
+
+#
+# Board Settings
+#
+CONFIG_BOARD_LOOPSPERMSEC=16717
+# CONFIG_ARCH_CALIBRATION is not set
+CONFIG_DRAM_START=0x20000000
+CONFIG_DRAM_SIZE=262144
+CONFIG_ARCH_HAVE_INTERRUPTSTACK=y
+CONFIG_ARCH_INTERRUPTSTACK=4096
+
+#
+# Boot options
+#
+# CONFIG_BOOT_RUNFROMEXTSRAM is not set
+CONFIG_BOOT_RUNFROMFLASH=y
+# CONFIG_BOOT_RUNFROMISRAM is not set
+# CONFIG_BOOT_RUNFROMSDRAM is not set
+# CONFIG_BOOT_COPYTORAM is not set
+
+#
+# Board Selection
+#
+CONFIG_ARCH_BOARD_CUSTOM=y
+CONFIG_ARCH_BOARD=""
+
+#
+# Common Board Options
+#
+CONFIG_NSH_MMCSDMINOR=0
+
+#
+# Board-Specific Options
+#
+
+#
+# RTOS Features
+#
+# CONFIG_BOARD_INITIALIZE is not set
+CONFIG_MSEC_PER_TICK=1
+CONFIG_RR_INTERVAL=0
+CONFIG_SCHED_INSTRUMENTATION=y
+CONFIG_TASK_NAME_SIZE=24
+# CONFIG_SCHED_HAVE_PARENT is not set
+# CONFIG_JULIAN_TIME is not set
+CONFIG_START_YEAR=1970
+CONFIG_START_MONTH=1
+CONFIG_START_DAY=1
+CONFIG_DEV_CONSOLE=y
+# CONFIG_MUTEX_TYPES is not set
+CONFIG_PRIORITY_INHERITANCE=y
+CONFIG_SEM_PREALLOCHOLDERS=0
+CONFIG_SEM_NNESTPRIO=8
+# CONFIG_FDCLONE_DISABLE is not set
+CONFIG_FDCLONE_STDIO=y
+CONFIG_SDCLONE_DISABLE=y
+CONFIG_SCHED_WAITPID=y
+# CONFIG_SCHED_STARTHOOK is not set
+CONFIG_SCHED_ATEXIT=y
+CONFIG_SCHED_ATEXIT_MAX=1
+# CONFIG_SCHED_ONEXIT is not set
+CONFIG_USER_ENTRYPOINT="nsh_main"
+# CONFIG_DISABLE_OS_API is not set
+
+#
+# Signal Numbers
+#
+CONFIG_SIG_SIGUSR1=1
+CONFIG_SIG_SIGUSR2=2
+CONFIG_SIG_SIGALARM=3
+CONFIG_SIG_SIGCONDTIMEDOUT=16
+CONFIG_SIG_SIGWORK=4
+
+#
+# Sizes of configurable things (0 disables)
+#
+CONFIG_MAX_TASKS=32
+CONFIG_MAX_TASK_ARGS=10
+CONFIG_NPTHREAD_KEYS=4
+CONFIG_NFILE_DESCRIPTORS=36
+CONFIG_NFILE_STREAMS=8
+CONFIG_NAME_MAX=32
+CONFIG_PREALLOC_MQ_MSGS=4
+CONFIG_MQ_MAXMSGSIZE=32
+CONFIG_MAX_WDOGPARMS=2
+CONFIG_PREALLOC_WDOGS=50
+CONFIG_PREALLOC_TIMERS=50
+
+#
+# Stack and heap information
+#
+CONFIG_IDLETHREAD_STACKSIZE=6000
+CONFIG_USERMAIN_STACKSIZE=4096
+CONFIG_PTHREAD_STACK_MIN=512
+CONFIG_PTHREAD_STACK_DEFAULT=2048
+
+#
+# Device Drivers
+#
+# CONFIG_DISABLE_POLL is not set
+CONFIG_DEV_NULL=y
+# CONFIG_DEV_ZERO is not set
+# CONFIG_LOOP is not set
+# CONFIG_RAMDISK is not set
+# CONFIG_CAN is not set
+# CONFIG_PWM is not set
+CONFIG_I2C=y
+# CONFIG_I2C_SLAVE is not set
+CONFIG_I2C_TRANSFER=y
+# CONFIG_I2C_WRITEREAD is not set
+# CONFIG_I2C_POLLED is not set
+# CONFIG_I2C_TRACE is not set
+CONFIG_ARCH_HAVE_I2CRESET=y
+CONFIG_I2C_RESET=y
+CONFIG_SPI=y
+# CONFIG_SPI_OWNBUS is not set
+CONFIG_SPI_EXCHANGE=y
+# CONFIG_SPI_CMDDATA is not set
+# CONFIG_RTC is not set
+CONFIG_WATCHDOG=y
+# CONFIG_ANALOG is not set
+# CONFIG_AUDIO_DEVICES is not set
+# CONFIG_BCH is not set
+# CONFIG_INPUT is not set
+# CONFIG_LCD is not set
+# CONFIG_MMCSD is not set
+CONFIG_MTD=y
+
+#
+# MTD Configuration
+#
+CONFIG_MTD_PARTITION=y
+CONFIG_MTD_BYTE_WRITE=y
+
+#
+# MTD Device Drivers
+#
+# CONFIG_RAMMTD is not set
+# CONFIG_MTD_AT24XX is not set
+# CONFIG_MTD_AT45DB is not set
+# CONFIG_MTD_M25P is not set
+# CONFIG_MTD_SMART is not set
+CONFIG_MTD_RAMTRON=y
+CONFIG_RAMTRON_FUJITSU=y
+# CONFIG_MTD_SST25 is not set
+# CONFIG_MTD_SST39FV is not set
+# CONFIG_MTD_W25 is not set
+CONFIG_PIPES=y
+# CONFIG_PM is not set
+# CONFIG_POWER is not set
+# CONFIG_SENSORS is not set
+CONFIG_SERIAL=y
+# CONFIG_DEV_LOWCONSOLE is not set
+# CONFIG_16550_UART is not set
+CONFIG_ARCH_HAVE_UART7=y
+CONFIG_ARCH_HAVE_UART8=y
+CONFIG_ARCH_HAVE_USART1=y
+CONFIG_ARCH_HAVE_USART2=y
+CONFIG_ARCH_HAVE_USART3=y
+CONFIG_MCU_SERIAL=y
+CONFIG_STANDARD_SERIAL=y
+CONFIG_SERIAL_NPOLLWAITERS=2
+# CONFIG_USART1_SERIAL_CONSOLE is not set
+# CONFIG_USART2_SERIAL_CONSOLE is not set
+# CONFIG_USART3_SERIAL_CONSOLE is not set
+# CONFIG_UART7_SERIAL_CONSOLE is not set
+CONFIG_UART8_SERIAL_CONSOLE=y
+# CONFIG_NO_SERIAL_CONSOLE is not set
+
+#
+# USART1 Configuration
+#
+CONFIG_USART1_RXBUFSIZE=512
+CONFIG_USART1_TXBUFSIZE=512
+CONFIG_USART1_BAUD=115200
+CONFIG_USART1_BITS=8
+CONFIG_USART1_PARITY=0
+CONFIG_USART1_2STOP=0
+# CONFIG_USART1_IFLOWCONTROL is not set
+# CONFIG_USART1_OFLOWCONTROL is not set
+
+#
+# USART2 Configuration
+#
+CONFIG_USART2_RXBUFSIZE=512
+CONFIG_USART2_TXBUFSIZE=512
+CONFIG_USART2_BAUD=115200
+CONFIG_USART2_BITS=8
+CONFIG_USART2_PARITY=0
+CONFIG_USART2_2STOP=0
+# CONFIG_USART2_IFLOWCONTROL is not set
+# CONFIG_USART2_OFLOWCONTROL is not set
+
+#
+# USART3 Configuration
+#
+CONFIG_USART3_RXBUFSIZE=512
+CONFIG_USART3_TXBUFSIZE=512
+CONFIG_USART3_BAUD=115200
+CONFIG_USART3_BITS=8
+CONFIG_USART3_PARITY=0
+CONFIG_USART3_2STOP=0
+# CONFIG_USART3_IFLOWCONTROL is not set
+# CONFIG_USART3_OFLOWCONTROL is not set
+
+#
+# UART7 Configuration
+#
+CONFIG_UART7_RXBUFSIZE=512
+CONFIG_UART7_TXBUFSIZE=512
+CONFIG_UART7_BAUD=115200
+CONFIG_UART7_BITS=8
+CONFIG_UART7_PARITY=0
+CONFIG_UART7_2STOP=0
+# CONFIG_UART7_IFLOWCONTROL is not set
+# CONFIG_UART7_OFLOWCONTROL is not set
+
+#
+# UART8 Configuration
+#
+CONFIG_UART8_RXBUFSIZE=512
+CONFIG_UART8_TXBUFSIZE=512
+CONFIG_UART8_BAUD=115200
+CONFIG_UART8_BITS=8
+CONFIG_UART8_PARITY=0
+CONFIG_UART8_2STOP=0
+# CONFIG_UART8_IFLOWCONTROL is not set
+# CONFIG_UART8_OFLOWCONTROL is not set
+# CONFIG_SERIAL_IFLOWCONTROL is not set
+# CONFIG_SERIAL_OFLOWCONTROL is not set
+# CONFIG_USBDEV is not set
+# CONFIG_USBHOST is not set
+# CONFIG_WIRELESS is not set
+
+#
+# System Logging Device Options
+#
+
+#
+# System Logging
+#
+# CONFIG_RAMLOG is not set
+
+#
+# Networking Support
+#
+# CONFIG_NET is not set
+
+#
+# File Systems
+#
+
+#
+# File system configuration
+#
+# CONFIG_DISABLE_MOUNTPOINT is not set
+# CONFIG_FS_RAMMAP is not set
+CONFIG_FS_FAT=y
+CONFIG_FAT_LCNAMES=y
+CONFIG_FAT_LFN=y
+CONFIG_FAT_MAXFNAME=32
+CONFIG_FS_FATTIME=y
+CONFIG_FAT_DMAMEMORY=y
+CONFIG_FS_NXFFS=y
+CONFIG_NXFFS_PREALLOCATED=y
+CONFIG_NXFFS_ERASEDSTATE=0xff
+CONFIG_NXFFS_PACKTHRESHOLD=32
+CONFIG_NXFFS_MAXNAMLEN=32
+CONFIG_NXFFS_TAILTHRESHOLD=2048
+CONFIG_FS_ROMFS=y
+# CONFIG_FS_SMARTFS is not set
+CONFIG_FS_BINFS=y
+
+#
+# System Logging
+#
+# CONFIG_SYSLOG_ENABLE is not set
+# CONFIG_SYSLOG is not set
+
+#
+# Graphics Support
+#
+# CONFIG_NX is not set
+
+#
+# Memory Management
+#
+# CONFIG_MM_MULTIHEAP is not set
+# CONFIG_MM_SMALL is not set
+CONFIG_MM_REGIONS=2
+CONFIG_GRAN=y
+# CONFIG_GRAN_SINGLE is not set
+# CONFIG_GRAN_INTR is not set
+
+#
+# Audio Support
+#
+# CONFIG_AUDIO is not set
+
+#
+# Binary Formats
+#
+# CONFIG_BINFMT_DISABLE is not set
+# CONFIG_BINFMT_EXEPATH is not set
+# CONFIG_NXFLAT is not set
+# CONFIG_ELF is not set
+CONFIG_BUILTIN=y
+# CONFIG_PIC is not set
+# CONFIG_SYMTAB_ORDEREDBYNAME is not set
+
+#
+# Library Routines
+#
+
+#
+# Standard C Library Options
+#
+CONFIG_STDIO_BUFFER_SIZE=32
+CONFIG_STDIO_LINEBUFFER=y
+CONFIG_NUNGET_CHARS=2
+CONFIG_LIB_HOMEDIR="/"
+# CONFIG_NOPRINTF_FIELDWIDTH is not set
+CONFIG_LIBC_FLOATINGPOINT=y
+CONFIG_LIB_RAND_ORDER=1
+# CONFIG_EOL_IS_CR is not set
+# CONFIG_EOL_IS_LF is not set
+# CONFIG_EOL_IS_BOTH_CRLF is not set
+CONFIG_EOL_IS_EITHER_CRLF=y
+# CONFIG_LIBC_EXECFUNCS is not set
+CONFIG_POSIX_SPAWN_PROXY_STACKSIZE=1024
+CONFIG_TASK_SPAWN_DEFAULT_STACKSIZE=2048
+CONFIG_LIBC_STRERROR=y
+# CONFIG_LIBC_STRERROR_SHORT is not set
+# CONFIG_LIBC_PERROR_STDOUT is not set
+CONFIG_ARCH_LOWPUTC=y
+CONFIG_LIB_SENDFILE_BUFSIZE=512
+# CONFIG_ARCH_ROMGETC is not set
+CONFIG_ARCH_OPTIMIZED_FUNCTIONS=y
+CONFIG_ARCH_MEMCPY=y
+# CONFIG_ARCH_MEMCMP is not set
+# CONFIG_ARCH_MEMMOVE is not set
+# CONFIG_ARCH_MEMSET is not set
+# CONFIG_MEMSET_OPTSPEED is not set
+# CONFIG_ARCH_STRCHR is not set
+# CONFIG_ARCH_STRCMP is not set
+# CONFIG_ARCH_STRCPY is not set
+# CONFIG_ARCH_STRNCPY is not set
+# CONFIG_ARCH_STRLEN is not set
+# CONFIG_ARCH_STRNLEN is not set
+# CONFIG_ARCH_BZERO is not set
+
+#
+# Non-standard Library Support
+#
+CONFIG_SCHED_WORKQUEUE=y
+CONFIG_SCHED_HPWORK=y
+CONFIG_SCHED_WORKPRIORITY=192
+CONFIG_SCHED_WORKPERIOD=5000
+CONFIG_SCHED_WORKSTACKSIZE=2000
+CONFIG_SCHED_LPWORK=y
+CONFIG_SCHED_LPWORKPRIORITY=50
+CONFIG_SCHED_LPWORKPERIOD=50000
+CONFIG_SCHED_LPWORKSTACKSIZE=2000
+# CONFIG_LIB_KBDCODEC is not set
+# CONFIG_LIB_SLCDCODEC is not set
+
+#
+# Basic CXX Support
+#
+CONFIG_C99_BOOL8=y
+CONFIG_HAVE_CXX=y
+CONFIG_HAVE_CXXINITIALIZE=y
+# CONFIG_CXX_NEWLONG is not set
+
+#
+# uClibc++ Standard C++ Library
+#
+# CONFIG_UCLIBCXX is not set
+
+#
+# Application Configuration
+#
+
+#
+# Built-In Applications
+#
+CONFIG_BUILTIN_PROXY_STACKSIZE=1024
+
+#
+# Examples
+#
+# CONFIG_EXAMPLES_BUTTONS is not set
+# CONFIG_EXAMPLES_CAN is not set
+# CONFIG_EXAMPLES_COMPOSITE is not set
+# CONFIG_EXAMPLES_CXXTEST is not set
+# CONFIG_EXAMPLES_DHCPD is not set
+# CONFIG_EXAMPLES_ELF is not set
+# CONFIG_EXAMPLES_FTPC is not set
+# CONFIG_EXAMPLES_FTPD is not set
+CONFIG_EXAMPLES_HELLO=y
+# CONFIG_EXAMPLES_HELLOXX is not set
+# CONFIG_EXAMPLES_JSON is not set
+# CONFIG_EXAMPLES_HIDKBD is not set
+# CONFIG_EXAMPLES_KEYPADTEST is not set
+# CONFIG_EXAMPLES_IGMP is not set
+# CONFIG_EXAMPLES_LCDRW is not set
+# CONFIG_EXAMPLES_MM is not set
+# CONFIG_EXAMPLES_MODBUS is not set
+CONFIG_EXAMPLES_MOUNT=y
+# CONFIG_EXAMPLES_MTDPART is not set
+# CONFIG_EXAMPLES_NRF24L01TERM is not set
+CONFIG_EXAMPLES_NSH=y
+# CONFIG_EXAMPLES_NULL is not set
+# CONFIG_EXAMPLES_NX is not set
+# CONFIG_EXAMPLES_NXCONSOLE is not set
+# CONFIG_EXAMPLES_NXFFS is not set
+# CONFIG_EXAMPLES_NXFLAT is not set
+# CONFIG_EXAMPLES_NXHELLO is not set
+# CONFIG_EXAMPLES_NXIMAGE is not set
+# CONFIG_EXAMPLES_NXLINES is not set
+# CONFIG_EXAMPLES_NXTEXT is not set
+# CONFIG_EXAMPLES_OSTEST is not set
+# CONFIG_EXAMPLES_PASHELLO is not set
+# CONFIG_EXAMPLES_PIPE is not set
+# CONFIG_EXAMPLES_POSIXSPAWN is not set
+# CONFIG_EXAMPLES_QENCODER is not set
+# CONFIG_EXAMPLES_RGMP is not set
+# CONFIG_EXAMPLES_ROMFS is not set
+# CONFIG_EXAMPLES_SENDMAIL is not set
+# CONFIG_EXAMPLES_SERLOOP is not set
+# CONFIG_EXAMPLES_SLCD is not set
+# CONFIG_EXAMPLES_SMART_TEST is not set
+# CONFIG_EXAMPLES_SMART is not set
+# CONFIG_EXAMPLES_TCPECHO is not set
+# CONFIG_EXAMPLES_TELNETD is not set
+# CONFIG_EXAMPLES_THTTPD is not set
+# CONFIG_EXAMPLES_TIFF is not set
+# CONFIG_EXAMPLES_TOUCHSCREEN is not set
+# CONFIG_EXAMPLES_UDP is not set
+# CONFIG_EXAMPLES_UIP is not set
+# CONFIG_EXAMPLES_USBSERIAL is not set
+# CONFIG_EXAMPLES_USBMSC is not set
+# CONFIG_EXAMPLES_USBTERM is not set
+# CONFIG_EXAMPLES_WATCHDOG is not set
+
+#
+# Graphics Support
+#
+# CONFIG_TIFF is not set
+
+#
+# Interpreters
+#
+# CONFIG_INTERPRETERS_FICL is not set
+# CONFIG_INTERPRETERS_PCODE is not set
+
+#
+# Network Utilities
+#
+
+#
+# Networking Utilities
+#
+# CONFIG_NETUTILS_CODECS is not set
+# CONFIG_NETUTILS_DHCPC is not set
+# CONFIG_NETUTILS_DHCPD is not set
+# CONFIG_NETUTILS_FTPC is not set
+# CONFIG_NETUTILS_FTPD is not set
+# CONFIG_NETUTILS_JSON is not set
+# CONFIG_NETUTILS_RESOLV is not set
+# CONFIG_NETUTILS_SMTP is not set
+# CONFIG_NETUTILS_TELNETD is not set
+# CONFIG_NETUTILS_TFTPC is not set
+# CONFIG_NETUTILS_THTTPD is not set
+# CONFIG_NETUTILS_UIPLIB is not set
+# CONFIG_NETUTILS_WEBCLIENT is not set
+
+#
+# FreeModBus
+#
+# CONFIG_MODBUS is not set
+
+#
+# NSH Library
+#
+CONFIG_NSH_LIBRARY=y
+CONFIG_NSH_BUILTIN_APPS=y
+
+#
+# Disable Individual commands
+#
+# CONFIG_NSH_DISABLE_CAT is not set
+# CONFIG_NSH_DISABLE_CD is not set
+# CONFIG_NSH_DISABLE_CP is not set
+# CONFIG_NSH_DISABLE_DD is not set
+# CONFIG_NSH_DISABLE_ECHO is not set
+# CONFIG_NSH_DISABLE_EXEC is not set
+# CONFIG_NSH_DISABLE_EXIT is not set
+# CONFIG_NSH_DISABLE_FREE is not set
+# CONFIG_NSH_DISABLE_GET is not set
+# CONFIG_NSH_DISABLE_HELP is not set
+# CONFIG_NSH_DISABLE_HEXDUMP is not set
+# CONFIG_NSH_DISABLE_IFCONFIG is not set
+# CONFIG_NSH_DISABLE_KILL is not set
+# CONFIG_NSH_DISABLE_LOSETUP is not set
+# CONFIG_NSH_DISABLE_LS is not set
+# CONFIG_NSH_DISABLE_MB is not set
+# CONFIG_NSH_DISABLE_MKDIR is not set
+# CONFIG_NSH_DISABLE_MKFATFS is not set
+# CONFIG_NSH_DISABLE_MKFIFO is not set
+# CONFIG_NSH_DISABLE_MKRD is not set
+# CONFIG_NSH_DISABLE_MH is not set
+# CONFIG_NSH_DISABLE_MOUNT is not set
+# CONFIG_NSH_DISABLE_MW is not set
+# CONFIG_NSH_DISABLE_NSFMOUNT is not set
+# CONFIG_NSH_DISABLE_PS is not set
+# CONFIG_NSH_DISABLE_PING is not set
+# CONFIG_NSH_DISABLE_PUT is not set
+# CONFIG_NSH_DISABLE_PWD is not set
+# CONFIG_NSH_DISABLE_RM is not set
+# CONFIG_NSH_DISABLE_RMDIR is not set
+# CONFIG_NSH_DISABLE_SET is not set
+# CONFIG_NSH_DISABLE_SH is not set
+# CONFIG_NSH_DISABLE_SLEEP is not set
+# CONFIG_NSH_DISABLE_TEST is not set
+# CONFIG_NSH_DISABLE_UMOUNT is not set
+# CONFIG_NSH_DISABLE_UNSET is not set
+# CONFIG_NSH_DISABLE_USLEEP is not set
+# CONFIG_NSH_DISABLE_WGET is not set
+# CONFIG_NSH_DISABLE_XD is not set
+
+#
+# Configure Command Options
+#
+# CONFIG_NSH_CMDOPT_DF_H is not set
+CONFIG_NSH_CODECS_BUFSIZE=128
+CONFIG_NSH_FILEIOSIZE=512
+CONFIG_NSH_STRERROR=y
+CONFIG_NSH_LINELEN=128
+CONFIG_NSH_MAXARGUMENTS=12
+CONFIG_NSH_NESTDEPTH=8
+# CONFIG_NSH_DISABLESCRIPT is not set
+# CONFIG_NSH_DISABLEBG is not set
+CONFIG_NSH_ROMFSETC=y
+# CONFIG_NSH_ROMFSRC is not set
+CONFIG_NSH_ROMFSMOUNTPT="/etc"
+CONFIG_NSH_INITSCRIPT="init.d/rcS"
+CONFIG_NSH_ROMFSDEVNO=0
+CONFIG_NSH_ROMFSSECTSIZE=128
+CONFIG_NSH_ARCHROMFS=y
+CONFIG_NSH_FATDEVNO=1
+CONFIG_NSH_FATSECTSIZE=512
+CONFIG_NSH_FATNSECTORS=1024
+CONFIG_NSH_FATMOUNTPT="/tmp"
+CONFIG_NSH_CONSOLE=y
+
+#
+# USB Trace Support
+#
+# CONFIG_NSH_CONDEV is not set
+CONFIG_NSH_ARCHINIT=y
+
+#
+# NxWidgets/NxWM
+#
+
+#
+# System NSH Add-Ons
+#
+
+#
+# Custom Free Memory Command
+#
+# CONFIG_SYSTEM_FREE is not set
+
+#
+# I2C tool
+#
+CONFIG_SYSTEM_I2CTOOL=y
+CONFIG_I2CTOOL_MINBUS=0
+CONFIG_I2CTOOL_MAXBUS=3
+CONFIG_I2CTOOL_MINADDR=0x03
+CONFIG_I2CTOOL_MAXADDR=0x77
+CONFIG_I2CTOOL_MAXREGADDR=0xff
+CONFIG_I2CTOOL_DEFFREQ=4000000
+
+#
+# FLASH Program Installation
+#
+# CONFIG_SYSTEM_INSTALL is not set
+
+#
+# FLASH Erase-all Command
+#
+# CONFIG_SYSTEM_FLASH_ERASEALL is not set
+
+#
+# readline()
+#
+CONFIG_SYSTEM_READLINE=y
+CONFIG_READLINE_ECHO=y
+
+#
+# Power Off
+#
+# CONFIG_SYSTEM_POWEROFF is not set
+
+#
+# RAMTRON
+#
+# CONFIG_SYSTEM_RAMTRON is not set
+
+#
+# SD Card
+#
+# CONFIG_SYSTEM_SDCARD is not set
+
+#
+# Sysinfo
+#
+CONFIG_SYSTEM_SYSINFO=y
+
+#
+# USB Monitor
+#
diff --git a/nuttx-configs/aerocore/nsh/setenv.sh b/nuttx-configs/aerocore/nsh/setenv.sh
new file mode 100755
index 000000000..2327f38a1
--- /dev/null
+++ b/nuttx-configs/aerocore/nsh/setenv.sh
@@ -0,0 +1,67 @@
+#!/bin/bash
+# configs/aerocore/nsh/setenv.sh
+#
+#   Copyright (C) 2011 Gregory Nutt. All rights reserved.
+#   Author: Gregory Nutt <gnutt@nuttx.org>
+#
+# 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 NuttX 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.
+#
+
+if [ "$_" = "$0" ] ; then
+  echo "You must source this script, not run it!" 1>&2
+  exit 1
+fi
+
+WD=`pwd`
+if [ ! -x "setenv.sh" ]; then
+  echo "This script must be executed from the top-level NuttX build directory"
+  exit 1
+fi
+
+if [ -z "${PATH_ORIG}" ]; then
+  export PATH_ORIG="${PATH}"
+fi
+
+# This the Cygwin path to the location where I installed the RIDE
+# toolchain under windows.  You will also have to edit this if you install
+# the RIDE toolchain in any other location
+#export TOOLCHAIN_BIN="/cygdrive/c/Program Files (x86)/Raisonance/Ride/arm-gcc/bin"
+
+# This the Cygwin path to the location where I installed the CodeSourcery
+# toolchain under windows.  You will also have to edit this if you install
+# the CodeSourcery toolchain in any other location
+export TOOLCHAIN_BIN="/cygdrive/c/Program Files (x86)/CodeSourcery/Sourcery G++ Lite/bin"
+
+# This the Cygwin path to the location where I build the buildroot
+# toolchain.
+#export TOOLCHAIN_BIN="${WD}/../misc/buildroot/build_arm_nofpu/staging_dir/bin"
+
+# Add the path to the toolchain to the PATH varialble
+export PATH="${TOOLCHAIN_BIN}:/sbin:/usr/sbin:${PATH_ORIG}"
+
+echo "PATH : ${PATH}"
diff --git a/nuttx-configs/aerocore/scripts/ld.script b/nuttx-configs/aerocore/scripts/ld.script
new file mode 100644
index 000000000..968d3127f
--- /dev/null
+++ b/nuttx-configs/aerocore/scripts/ld.script
@@ -0,0 +1,150 @@
+/****************************************************************************
+ * configs/aerocore/common/ld.script
+ *
+ *   Copyright (C) 2011 Gregory Nutt. All rights reserved.
+ *   Author: Gregory Nutt <gnutt@nuttx.org>
+ *
+ * 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 NuttX 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.
+ *
+ ****************************************************************************/
+
+/* The STM32F427 has 2048Kb of FLASH beginning at address 0x0800:0000 and
+ * 256Kb of SRAM. SRAM is split up into three blocks:
+ *
+ * 1) 112Kb of SRAM beginning at address 0x2000:0000
+ * 2)  16Kb of SRAM beginning at address 0x2001:c000
+ * 3)  64Kb of SRAM beginning at address 0x2002:0000
+ * 4)  64Kb of TCM SRAM beginning at address 0x1000:0000
+ *
+ * When booting from FLASH, FLASH memory is aliased to address 0x0000:0000
+ * where the code expects to begin execution by jumping to the entry point in
+ * the 0x0800:0000 address range.
+ *
+ * The first 0x4000 of flash is reserved for the bootloader.
+ */
+
+MEMORY
+{
+    flash (rx)   : ORIGIN = 0x08004000, LENGTH = 2032K
+    sram (rwx)   : ORIGIN = 0x20000000, LENGTH = 192K
+    ccsram (rwx) : ORIGIN = 0x10000000, LENGTH = 64K
+}
+
+OUTPUT_ARCH(arm)
+
+ENTRY(__start)		/* treat __start as the anchor for dead code stripping */
+EXTERN(_vectors)	/* force the vectors to be included in the output */
+
+/* 
+ * Ensure that abort() is present in the final object.  The exception handling
+ * code pulled in by libgcc.a requires it (and that code cannot be easily avoided).
+ */
+EXTERN(abort)
+
+SECTIONS
+{
+	.text : {
+		_stext = ABSOLUTE(.);
+		*(.vectors)
+		*(.text .text.*)        
+		*(.fixup)
+		*(.gnu.warning)
+		*(.rodata .rodata.*)        
+		*(.gnu.linkonce.t.*)
+		*(.got)
+		*(.gcc_except_table)
+		*(.gnu.linkonce.r.*)
+		_etext = ABSOLUTE(.);
+
+		/* 
+		 * This is a hack to make the newlib libm __errno() call
+		 * use the NuttX get_errno_ptr() function.
+		 */
+		__errno = get_errno_ptr;
+	} > flash
+
+	/*
+	 * Init functions (static constructors and the like)
+	 */
+        .init_section : {
+                _sinit = ABSOLUTE(.);
+                KEEP(*(.init_array .init_array.*))
+                _einit = ABSOLUTE(.);
+        } > flash
+
+	/*
+	 * Construction data for parameters.
+	 */
+	__param ALIGN(4): {
+		__param_start = ABSOLUTE(.);
+		KEEP(*(__param*))
+		__param_end = ABSOLUTE(.);
+	} > flash
+
+	.ARM.extab : {
+		*(.ARM.extab*)
+	} > flash
+
+	__exidx_start = ABSOLUTE(.);
+	.ARM.exidx : {
+		*(.ARM.exidx*)
+	} > flash
+	__exidx_end = ABSOLUTE(.);
+
+	_eronly = ABSOLUTE(.);
+
+	.data : {
+		_sdata = ABSOLUTE(.);
+		*(.data .data.*)
+		*(.gnu.linkonce.d.*)
+		CONSTRUCTORS
+		_edata = ABSOLUTE(.);
+	} > sram AT > flash
+
+	.bss : {
+		_sbss = ABSOLUTE(.);
+		*(.bss .bss.*)
+		*(.gnu.linkonce.b.*)
+		*(COMMON)
+		_ebss = ABSOLUTE(.);
+	} > sram
+
+	/* Stabs debugging sections. */
+	.stab 0 : { *(.stab) }
+	.stabstr 0 : { *(.stabstr) }
+	.stab.excl 0 : { *(.stab.excl) }
+	.stab.exclstr 0 : { *(.stab.exclstr) }
+	.stab.index 0 : { *(.stab.index) }
+	.stab.indexstr 0 : { *(.stab.indexstr) }
+	.comment 0 : { *(.comment) }
+	.debug_abbrev 0 : { *(.debug_abbrev) }
+	.debug_info 0 : { *(.debug_info) }
+	.debug_line 0 : { *(.debug_line) }
+	.debug_pubnames 0 : { *(.debug_pubnames) }
+	.debug_aranges 0 : { *(.debug_aranges) }
+}
diff --git a/nuttx-configs/aerocore/src/Makefile b/nuttx-configs/aerocore/src/Makefile
new file mode 100644
index 000000000..859ba4ab2
--- /dev/null
+++ b/nuttx-configs/aerocore/src/Makefile
@@ -0,0 +1,84 @@
+############################################################################
+# configs/aerocore/src/Makefile
+#
+#   Copyright (C) 2011 Gregory Nutt. All rights reserved.
+#   Author: Gregory Nutt <gnutt@nuttx.org>
+#
+# 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 NuttX 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.
+#
+############################################################################
+
+-include $(TOPDIR)/Make.defs
+
+CFLAGS		+= -I$(TOPDIR)/sched
+
+ASRCS		= 
+AOBJS		= $(ASRCS:.S=$(OBJEXT))
+
+CSRCS		= empty.c
+COBJS		= $(CSRCS:.c=$(OBJEXT))
+
+SRCS		= $(ASRCS) $(CSRCS)
+OBJS		= $(AOBJS) $(COBJS)
+
+ARCH_SRCDIR	= $(TOPDIR)/arch/$(CONFIG_ARCH)/src
+ifeq ($(WINTOOL),y)
+  CFLAGS	+= -I "${shell cygpath -w $(ARCH_SRCDIR)/chip}" \
+  		   -I "${shell cygpath -w $(ARCH_SRCDIR)/common}" \
+  		   -I "${shell cygpath -w $(ARCH_SRCDIR)/armv7-m}"
+else
+  CFLAGS	+= -I$(ARCH_SRCDIR)/chip -I$(ARCH_SRCDIR)/common -I$(ARCH_SRCDIR)/armv7-m
+endif
+
+all: libboard$(LIBEXT)
+
+$(AOBJS): %$(OBJEXT): %.S
+	$(call ASSEMBLE, $<, $@)
+
+$(COBJS) $(LINKOBJS): %$(OBJEXT): %.c
+	$(call COMPILE, $<, $@)
+
+libboard$(LIBEXT): $(OBJS)
+	$(call ARCHIVE, $@, $(OBJS))
+
+.depend: Makefile $(SRCS)
+	$(Q) $(MKDEP) $(CC) -- $(CFLAGS) -- $(SRCS) >Make.dep
+	$(Q) touch $@
+
+depend: .depend
+
+clean:
+	$(call DELFILE, libboard$(LIBEXT))
+	$(call CLEAN)
+
+distclean: clean
+	$(call DELFILE, Make.dep)
+	$(call DELFILE, .depend)
+
+-include Make.dep
+
diff --git a/nuttx-configs/aerocore/src/empty.c b/nuttx-configs/aerocore/src/empty.c
new file mode 100644
index 000000000..ace900866
--- /dev/null
+++ b/nuttx-configs/aerocore/src/empty.c
@@ -0,0 +1,4 @@
+/*
+ * There are no source files here, but libboard.a can't be empty, so 
+ * we have this empty source file to keep it company.
+ */
diff --git a/src/drivers/boards/aerocore/aerocore_init.c b/src/drivers/boards/aerocore/aerocore_init.c
new file mode 100644
index 000000000..4e3ba2d7e
--- /dev/null
+++ b/src/drivers/boards/aerocore/aerocore_init.c
@@ -0,0 +1,282 @@
+/****************************************************************************
+ *
+ *   Copyright (C) 2012 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 aerocore_init.c
+ *
+ * AeroCore-specific early startup code.  This file implements the
+ * nsh_archinitialize() function that is called early by nsh during startup.
+ *
+ * Code here is run before the rcS script is invoked; it should start required
+ * subsystems and perform board-specific initialisation.
+ */
+
+/****************************************************************************
+ * Included Files
+ ****************************************************************************/
+
+#include <nuttx/config.h>
+
+#include <stdbool.h>
+#include <stdio.h>
+#include <debug.h>
+#include <errno.h>
+
+#include <nuttx/arch.h>
+#include <nuttx/spi.h>
+#include <nuttx/i2c.h>
+#include <nuttx/mmcsd.h>
+#include <nuttx/analog/adc.h>
+#include <nuttx/gran.h>
+
+#include <stm32.h>
+#include "board_config.h"
+#include <stm32_uart.h>
+
+#include <arch/board/board.h>
+
+#include <drivers/drv_hrt.h>
+#include <drivers/drv_led.h>
+
+#include <systemlib/cpuload.h>
+#include <systemlib/perf_counter.h>
+
+/****************************************************************************
+ * Pre-Processor Definitions
+ ****************************************************************************/
+
+/* Configuration ************************************************************/
+
+/* Debug ********************************************************************/
+
+#ifdef CONFIG_CPP_HAVE_VARARGS
+#  ifdef CONFIG_DEBUG
+#    define message(...) lowsyslog(__VA_ARGS__)
+#  else
+#    define message(...) printf(__VA_ARGS__)
+#  endif
+#else
+#  ifdef CONFIG_DEBUG
+#    define message lowsyslog
+#  else
+#    define message printf
+#  endif
+#endif
+
+/****************************************************************************
+ * Protected Functions
+ ****************************************************************************/
+
+#if defined(CONFIG_FAT_DMAMEMORY)
+# if !defined(CONFIG_GRAN) || !defined(CONFIG_FAT_DMAMEMORY)
+#  error microSD DMA support requires CONFIG_GRAN
+# endif
+
+static GRAN_HANDLE dma_allocator;
+
+/* 
+ * The DMA heap size constrains the total number of things that can be 
+ * ready to do DMA at a time.
+ *
+ * For example, FAT DMA depends on one sector-sized buffer per filesystem plus
+ * one sector-sized buffer per file. 
+ *
+ * We use a fundamental alignment / granule size of 64B; this is sufficient
+ * to guarantee alignment for the largest STM32 DMA burst (16 beats x 32bits).
+ */
+static uint8_t g_dma_heap[8192] __attribute__((aligned(64)));
+static perf_counter_t g_dma_perf;
+
+static void
+dma_alloc_init(void)
+{
+	dma_allocator = gran_initialize(g_dma_heap,
+					sizeof(g_dma_heap),
+					7,  /* 128B granule - must be > alignment (XXX bug?) */
+					6); /* 64B alignment */
+	if (dma_allocator == NULL) {
+		message("[boot] DMA allocator setup FAILED");
+	} else {
+		g_dma_perf = perf_alloc(PC_COUNT, "DMA allocations");
+	}
+}
+
+/****************************************************************************
+ * Public Functions
+ ****************************************************************************/
+
+/*
+ * DMA-aware allocator stubs for the FAT filesystem.
+ */
+
+__EXPORT void *fat_dma_alloc(size_t size);
+__EXPORT void fat_dma_free(FAR void *memory, size_t size);
+
+void *
+fat_dma_alloc(size_t size)
+{
+	perf_count(g_dma_perf);
+	return gran_alloc(dma_allocator, size);
+}
+
+void
+fat_dma_free(FAR void *memory, size_t size)
+{
+	gran_free(dma_allocator, memory, size);
+}
+
+#else
+
+# define dma_alloc_init()
+
+#endif
+
+/************************************************************************************
+ * Name: stm32_boardinitialize
+ *
+ * Description:
+ *   All STM32 architectures must provide the following entry point.  This entry point
+ *   is called early in the intitialization -- after all memory has been configured
+ *   and mapped but before any devices have been initialized.
+ *
+ ************************************************************************************/
+
+__EXPORT void
+stm32_boardinitialize(void)
+{
+	/* configure SPI interfaces */
+	stm32_spiinitialize();
+
+	/* configure LEDs */
+	up_ledinit();
+}
+
+/****************************************************************************
+ * Name: nsh_archinitialize
+ *
+ * Description:
+ *   Perform architecture specific initialization
+ *
+ ****************************************************************************/
+
+static struct spi_dev_s *spi3;
+static struct spi_dev_s *spi4;
+
+#include <math.h>
+
+#ifdef __cplusplus
+__EXPORT int matherr(struct __exception *e)
+{
+	return 1;
+}
+#else
+__EXPORT int matherr(struct exception *e)
+{
+	return 1;
+}
+#endif
+
+__EXPORT int nsh_archinitialize(void)
+{
+
+	/* configure ADC pins */
+	stm32_configgpio(GPIO_ADC1_IN10);	/* used by VBUS valid */
+	stm32_configgpio(GPIO_ADC1_IN11);	/* J1 breakout */
+	stm32_configgpio(GPIO_ADC1_IN12);	/* J1 breakout */
+	stm32_configgpio(GPIO_ADC1_IN13);	/* J1 breakout */
+
+	/* configure the high-resolution time/callout interface */
+	hrt_init();
+
+	/* configure the DMA allocator */
+	dma_alloc_init();
+
+	/* configure CPU load estimation */
+#ifdef CONFIG_SCHED_INSTRUMENTATION
+	cpuload_initialize_once();
+#endif
+
+	/* set up the serial DMA polling */
+	static struct hrt_call serial_dma_call;
+	struct timespec ts;
+
+	/*
+	 * Poll at 1ms intervals for received bytes that have not triggered
+	 * a DMA event.
+	 */
+	ts.tv_sec = 0;
+	ts.tv_nsec = 1000000;
+
+	hrt_call_every(&serial_dma_call,
+		       ts_to_abstime(&ts),
+		       ts_to_abstime(&ts),
+		       (hrt_callout)stm32_serial_dma_poll,
+		       NULL);
+
+	/* initial LED state */
+	drv_led_start();
+	led_off(LED_AMBER);
+
+	/* Configure Sensors on SPI bus #3 */
+	spi3 = up_spiinitialize(3);
+	if (!spi3) {
+		message("[boot] FAILED to initialize SPI port 3\n");
+		up_ledon(LED_AMBER);
+		return -ENODEV;
+	}
+	/* Default: 1MHz, 8 bits, Mode 3 */
+	SPI_SETFREQUENCY(spi3, 10000000);
+	SPI_SETBITS(spi3, 8);
+	SPI_SETMODE(spi3, SPIDEV_MODE3);
+	SPI_SELECT(spi3, PX4_SPIDEV_GYRO, false);
+	SPI_SELECT(spi3, PX4_SPIDEV_ACCEL_MAG, false);
+	SPI_SELECT(spi3, PX4_SPIDEV_BARO, false);
+	up_udelay(20);
+	message("[boot] Initialized SPI port 3 (SENSORS)\n");
+
+	/* Configure FRAM on SPI bus #4 */
+	spi4 = up_spiinitialize(4);
+	if (!spi4) {
+		message("[boot] FAILED to initialize SPI port 4\n");
+		up_ledon(LED_AMBER);
+		return -ENODEV;
+	}
+	/* Default: ~10MHz, 8 bits, Mode 3 */
+	SPI_SETFREQUENCY(spi4, 10 * 1000 * 1000);
+	SPI_SETBITS(spi4, 8);
+	SPI_SETMODE(spi4, SPIDEV_MODE0);
+	SPI_SELECT(spi4, SPIDEV_FLASH, false);
+	message("[boot] Initialized SPI port 4 (FRAM)\n");
+
+	return OK;
+}
diff --git a/src/modules/fw_att_pos_estimator/fw_att_pos_estimator_params.c b/src/drivers/boards/aerocore/aerocore_led.c
index 6138ef39c..e40d1730c 100644
--- a/src/modules/fw_att_pos_estimator/fw_att_pos_estimator_params.c
+++ b/src/drivers/boards/aerocore/aerocore_led.c
@@ -1,6 +1,6 @@
 /****************************************************************************
  *
- *   Copyright (c) 2013, 2014 PX4 Development Team. All rights reserved.
+ *   Copyright (c) 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
@@ -32,86 +32,90 @@
  ****************************************************************************/
 
 /**
- * @file fw_att_pos_estimator_params.c
+ * @file aerocore_led.c
  *
- * Parameters defined by the attitude and position estimator task
- *
- * @author Lorenz Meier <lm@inf.ethz.ch>
+ * AeroCore LED backend.
  */
 
 #include <nuttx/config.h>
 
-#include <systemlib/param/param.h>
+#include <stdbool.h>
+
+#include "stm32.h"
+#include "board_config.h"
+
+#include <arch/board/board.h>
 
 /*
- * Estimator parameters, accessible via MAVLink
- *
+ * Ideally we'd be able to get these from up_internal.h,
+ * but since we want to be able to disable the NuttX use
+ * of leds for system indication at will and there is no
+ * separate switch, we need to build independent of the
+ * CONFIG_ARCH_LEDS configuration switch.
  */
+__BEGIN_DECLS
+extern void led_init();
+extern void led_on(int led);
+extern void led_off(int led);
+extern void led_toggle(int led);
+__END_DECLS
 
-/**
- * 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);
+__EXPORT void led_init()
+{
+	stm32_configgpio(GPIO_LED0);
+	stm32_configgpio(GPIO_LED1);
+}
 
-/**
- * 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);
+__EXPORT void led_on(int led)
+{
+	switch (led) {
+	case 0:
+		stm32_gpiowrite(GPIO_LED0, true);
+		break;
 
-/**
- * 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);
+	case 1:
+		stm32_gpiowrite(GPIO_LED1, true);
+		break;
 
-/**
- * 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);
+	default:
+		warnx("LED ID not recognized\n");
+	}
+}
 
-/**
- * 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);
+__EXPORT void led_off(int led)
+{
+	switch (led) {
+	case 0:
+		stm32_gpiowrite(GPIO_LED0, false);
+		break;
 
-/**
- * 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);
+	case 1:
+		stm32_gpiowrite(GPIO_LED1, false);
+		break;
+
+	default:
+		warnx("LED ID not recognized\n");
+	}
+}
+
+__EXPORT void led_toggle(int led)
+{
+	switch (led) {
+	case 0:
+		if (stm32_gpioread(GPIO_LED0))
+			stm32_gpiowrite(GPIO_LED0, false);
+		else
+			stm32_gpiowrite(GPIO_LED0, true);
+		break;
+
+	case 1:
+		if (stm32_gpioread(GPIO_LED1))
+			stm32_gpiowrite(GPIO_LED1, false);
+		else
+			stm32_gpiowrite(GPIO_LED1, true);
+		break;
 
+	default:
+		warnx("LED ID not recognized\n");
+	}
+}
diff --git a/src/drivers/boards/aerocore/aerocore_pwm_servo.c b/src/drivers/boards/aerocore/aerocore_pwm_servo.c
new file mode 100644
index 000000000..251eaff7b
--- /dev/null
+++ b/src/drivers/boards/aerocore/aerocore_pwm_servo.c
@@ -0,0 +1,117 @@
+/****************************************************************************
+ *
+ *   Copyright (C) 2012 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 aerocore_pwm_servo.c
+ *
+ * Configuration data for the stm32 pwm_servo driver.
+ *
+ * Note that these arrays must always be fully-sized.
+ */
+
+#include <stdint.h>
+
+#include <stm32.h>
+#include <stm32_gpio.h>
+#include <stm32_tim.h>
+
+#include <drivers/stm32/drv_pwm_servo.h>
+#include <drivers/drv_pwm_output.h>
+
+#include "board_config.h"
+
+__EXPORT const struct pwm_servo_timer pwm_timers[PWM_SERVO_MAX_TIMERS] = {
+	{
+		.base = STM32_TIM1_BASE,
+		.clock_register = STM32_RCC_APB2ENR,
+		.clock_bit = RCC_APB2ENR_TIM1EN,
+		.clock_freq = STM32_APB2_TIM1_CLKIN
+	},
+	{
+		.base = STM32_TIM3_BASE,
+		.clock_register = STM32_RCC_APB1ENR,
+		.clock_bit = RCC_APB1ENR_TIM3EN,
+		.clock_freq = STM32_APB1_TIM3_CLKIN
+	}
+};
+
+__EXPORT const struct pwm_servo_channel pwm_channels[PWM_SERVO_MAX_CHANNELS] = {
+	{
+		.gpio = GPIO_TIM1_CH1OUT,
+		.timer_index = 0,
+		.timer_channel = 1,
+		.default_value = 1500,
+	},
+	{
+		.gpio = GPIO_TIM1_CH2OUT,
+		.timer_index = 0,
+		.timer_channel = 2,
+		.default_value = 1500,
+	},
+	{
+		.gpio = GPIO_TIM1_CH3OUT,
+		.timer_index = 0,
+		.timer_channel = 3,
+		.default_value = 1500,
+	},
+	{
+		.gpio = GPIO_TIM1_CH4OUT,
+		.timer_index = 0,
+		.timer_channel = 4,
+		.default_value = 1500,
+	},
+	{
+		.gpio = GPIO_TIM3_CH1OUT,
+		.timer_index = 1,
+		.timer_channel = 1,
+		.default_value = 1500,
+	},
+	{
+		.gpio = GPIO_TIM3_CH2OUT,
+		.timer_index = 1,
+		.timer_channel = 2,
+		.default_value = 1500,
+	},
+	{
+		.gpio = GPIO_TIM3_CH3OUT,
+		.timer_index = 1,
+		.timer_channel = 3,
+		.default_value = 1500,
+	},
+	{
+		.gpio = GPIO_TIM3_CH4OUT,
+		.timer_index = 1,
+		.timer_channel = 4,
+		.default_value = 1500,
+	}
+};
diff --git a/src/drivers/boards/aerocore/aerocore_spi.c b/src/drivers/boards/aerocore/aerocore_spi.c
new file mode 100644
index 000000000..e329bd9d1
--- /dev/null
+++ b/src/drivers/boards/aerocore/aerocore_spi.c
@@ -0,0 +1,183 @@
+/****************************************************************************
+ *
+ *   Copyright (C) 2012 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 aerocore_spi.c
+ *
+ * Board-specific SPI functions.
+ */
+
+/************************************************************************************
+ * Included Files
+ ************************************************************************************/
+
+#include <nuttx/config.h>
+
+#include <stdint.h>
+#include <stdbool.h>
+#include <debug.h>
+
+#include <nuttx/spi.h>
+#include <arch/board/board.h>
+
+#include <up_arch.h>
+#include <chip.h>
+#include <stm32.h>
+#include "board_config.h"
+
+/************************************************************************************
+ * Public Functions
+ ************************************************************************************/
+
+/************************************************************************************
+ * Name: stm32_spiinitialize
+ *
+ * Description:
+ *   Called to configure SPI chip select GPIO pins for the PX4FMU board.
+ *
+ ************************************************************************************/
+
+__EXPORT void weak_function stm32_spiinitialize(void)
+{
+#ifdef CONFIG_STM32_SPI1
+	stm32_configgpio(GPIO_SPI1_NSS);
+	stm32_gpiowrite(GPIO_SPI1_NSS, 1);
+#endif
+
+#ifdef CONFIG_STM32_SPI2
+	stm32_configgpio(GPIO_SPI2_NSS);
+	stm32_gpiowrite(GPIO_SPI2_NSS, 1);
+#endif
+
+#ifdef CONFIG_STM32_SPI3
+	stm32_configgpio(GPIO_SPI_CS_GYRO);
+	stm32_configgpio(GPIO_SPI_CS_ACCEL_MAG);
+	stm32_configgpio(GPIO_SPI_CS_BARO);
+
+	/* De-activate all peripherals,
+	 * required for some peripheral
+	 * state machines
+	 */
+	stm32_gpiowrite(GPIO_SPI_CS_GYRO, 1);
+	stm32_gpiowrite(GPIO_SPI_CS_ACCEL_MAG, 1);
+	stm32_gpiowrite(GPIO_SPI_CS_BARO, 1);
+
+	stm32_configgpio(GPIO_EXTI_GYRO_DRDY);
+	stm32_configgpio(GPIO_EXTI_MAG_DRDY);
+	stm32_configgpio(GPIO_EXTI_ACCEL_DRDY);
+#endif
+
+#ifdef CONFIG_STM32_SPI4
+	stm32_configgpio(GPIO_SPI4_NSS);
+	stm32_gpiowrite(GPIO_SPI4_NSS, 1);
+#endif
+}
+
+#ifdef CONFIG_STM32_SPI1
+__EXPORT void stm32_spi1select(FAR struct spi_dev_s *dev, enum spi_dev_e devid, bool selected)
+{
+	/* there is only one device broken-out so select it */
+	stm32_gpiowrite(GPIO_SPI1_NSS, !selected);
+}
+
+__EXPORT uint8_t stm32_spi1status(FAR struct spi_dev_s *dev, enum spi_dev_e devid)
+{
+	return SPI_STATUS_PRESENT;
+}
+#endif
+
+#ifdef CONFIG_STM32_SPI2
+__EXPORT void stm32_spi2select(FAR struct spi_dev_s *dev, enum spi_dev_e devid, bool selected)
+{
+	/* there is only one device broken-out so select it */
+	stm32_gpiowrite(GPIO_SPI2_NSS, !selected);
+}
+
+__EXPORT uint8_t stm32_spi2status(FAR struct spi_dev_s *dev, enum spi_dev_e devid)
+{
+	return SPI_STATUS_PRESENT;
+}
+#endif
+
+#ifdef CONFIG_STM32_SPI3
+__EXPORT void stm32_spi3select(FAR struct spi_dev_s *dev, enum spi_dev_e devid, bool selected)
+{
+	/* SPI select is active low, so write !selected to select the device */
+
+	switch (devid) {
+	case PX4_SPIDEV_GYRO:
+		/* Making sure the other peripherals are not selected */
+		stm32_gpiowrite(GPIO_SPI_CS_GYRO, !selected);
+		stm32_gpiowrite(GPIO_SPI_CS_ACCEL_MAG, 1);
+		stm32_gpiowrite(GPIO_SPI_CS_BARO, 1);
+		break;
+
+	case PX4_SPIDEV_ACCEL_MAG:
+		/* Making sure the other peripherals are not selected */
+		stm32_gpiowrite(GPIO_SPI_CS_GYRO, 1);
+		stm32_gpiowrite(GPIO_SPI_CS_ACCEL_MAG, !selected);
+		stm32_gpiowrite(GPIO_SPI_CS_BARO, 1);
+		break;
+
+	case PX4_SPIDEV_BARO:
+		/* Making sure the other peripherals are not selected */
+		stm32_gpiowrite(GPIO_SPI_CS_GYRO, 1);
+		stm32_gpiowrite(GPIO_SPI_CS_ACCEL_MAG, 1);
+		stm32_gpiowrite(GPIO_SPI_CS_BARO, !selected);
+		break;
+
+	default:
+		break;
+	}
+}
+
+__EXPORT uint8_t stm32_spi3status(FAR struct spi_dev_s *dev, enum spi_dev_e devid)
+{
+	return SPI_STATUS_PRESENT;
+}
+#endif
+
+
+#ifdef CONFIG_STM32_SPI4
+__EXPORT void stm32_spi4select(FAR struct spi_dev_s *dev, enum spi_dev_e devid, bool selected)
+{
+	/* there can only be one device on this bus, so always select it */
+	stm32_gpiowrite(GPIO_SPI4_NSS, !selected);
+}
+
+__EXPORT uint8_t stm32_spi4status(FAR struct spi_dev_s *dev, enum spi_dev_e devid)
+{
+	/* FRAM is always present */
+	return SPI_STATUS_PRESENT;
+}
+#endif
diff --git a/src/drivers/boards/aerocore/board_config.h b/src/drivers/boards/aerocore/board_config.h
new file mode 100644
index 000000000..70142a314
--- /dev/null
+++ b/src/drivers/boards/aerocore/board_config.h
@@ -0,0 +1,176 @@
+/****************************************************************************
+ *
+ *   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 board_config.h
+ *
+ * AeroCore internal definitions
+ */
+
+#pragma once
+
+/****************************************************************************************************
+ * Included Files
+ ****************************************************************************************************/
+
+#include <nuttx/config.h>
+#include <nuttx/compiler.h>
+#include <stdint.h>
+
+__BEGIN_DECLS
+
+/* these headers are not C++ safe */
+#include <stm32.h>
+#include <arch/board/board.h>
+
+#define UDID_START		0x1FFF7A10
+ 
+/****************************************************************************************************
+ * Definitions
+ ****************************************************************************************************/
+
+/* LEDs */
+#define GPIO_LED0	(GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_SET|GPIO_PORTE|GPIO_PIN9)
+#define GPIO_LED1	(GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_SET|GPIO_PORTE|GPIO_PIN10)
+
+/* Gyro */
+#define GPIO_EXTI_GYRO_DRDY	(GPIO_INPUT|GPIO_FLOAT|GPIO_EXTI|GPIO_PORTD|GPIO_PIN0)
+#define SENSOR_BOARD_ROTATION_DEFAULT	3 /* SENSOR_BOARD_ROTATION_270_DEG */
+
+/* Accel & Mag */
+#define GPIO_EXTI_MAG_DRDY	(GPIO_INPUT|GPIO_FLOAT|GPIO_EXTI|GPIO_PORTD|GPIO_PIN1)
+#define GPIO_EXTI_ACCEL_DRDY	(GPIO_INPUT|GPIO_FLOAT|GPIO_EXTI|GPIO_PORTD|GPIO_PIN2)
+
+/* GPS */
+#define GPIO_GPS_NRESET		(GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_OUTPUT_SET|GPIO_PORTC|GPIO_PIN5)
+#define GPIO_GPS_TIMEPULSE	(GPIO_INPUT|GPIO_FLOAT|GPIO_PORTC|GPIO_PIN4)
+#define GPS_DEFAULT_UART_PORT	"/dev/ttyS0"
+
+/* SPI3--Sensors */
+#define PX4_SPI_BUS_SENSORS	3
+#define GPIO_SPI_CS_ACCEL_MAG	(GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_SET|GPIO_PORTE|GPIO_PIN2)
+#define GPIO_SPI_CS_GYRO	(GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_SET|GPIO_PORTE|GPIO_PIN3)
+#define GPIO_SPI_CS_BARO	(GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_SET|GPIO_PORTE|GPIO_PIN4)
+
+/* Nominal chip selects for devices on SPI bus #3 */
+#define PX4_SPIDEV_ACCEL_MAG	0
+#define PX4_SPIDEV_GYRO		1
+#define PX4_SPIDEV_BARO		2
+
+/* User GPIOs broken out on J11 */
+#define GPIO_GPIO0_INPUT	(GPIO_INPUT|GPIO_PULLUP|GPIO_PORTB|GPIO_PIN0)
+#define GPIO_GPIO1_INPUT	(GPIO_INPUT|GPIO_PULLUP|GPIO_PORTB|GPIO_PIN1)
+#define GPIO_GPIO3_INPUT	(GPIO_INPUT|GPIO_PULLUP|GPIO_PORTA|GPIO_PIN1)
+#define GPIO_GPIO4_INPUT	(GPIO_INPUT|GPIO_PULLUP|GPIO_PORTA|GPIO_PIN2)
+#define GPIO_GPIO5_INPUT	(GPIO_INPUT|GPIO_PULLUP|GPIO_PORTA|GPIO_PIN3)
+#define GPIO_GPIO6_INPUT	(GPIO_INPUT|GPIO_PULLUP|GPIO_PORTD|GPIO_PIN12)
+#define GPIO_GPIO7_INPUT	(GPIO_INPUT|GPIO_PULLUP|GPIO_PORTD|GPIO_PIN13)
+#define GPIO_GPIO8_INPUT	(GPIO_INPUT|GPIO_PULLUP|GPIO_PORTD|GPIO_PIN14)
+#define GPIO_GPIO9_INPUT	(GPIO_INPUT|GPIO_PULLUP|GPIO_PORTD|GPIO_PIN15)
+#define GPIO_GPIO10_INPUT	(GPIO_INPUT|GPIO_PULLUP|GPIO_PORTB|GPIO_PIN5)
+#define GPIO_GPIO11_INPUT	(GPIO_INPUT|GPIO_PULLUP|GPIO_PORTB|GPIO_PIN8)
+
+#define GPIO_GPIO0_OUTPUT	(GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTB|GPIO_PIN0)
+#define GPIO_GPIO1_OUTPUT	(GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTB|GPIO_PIN1)
+#define GPIO_GPIO3_OUTPUT	(GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTA|GPIO_PIN1)
+#define GPIO_GPIO4_OUTPUT	(GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTA|GPIO_PIN2)
+#define GPIO_GPIO5_OUTPUT	(GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTA|GPIO_PIN3)
+#define GPIO_GPIO6_OUTPUT	(GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTD|GPIO_PIN12)
+#define GPIO_GPIO7_OUTPUT	(GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTD|GPIO_PIN13)
+#define GPIO_GPIO8_OUTPUT	(GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTD|GPIO_PIN14)
+#define GPIO_GPIO9_OUTPUT	(GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTD|GPIO_PIN15)
+#define GPIO_GPIO10_OUTPUT	(GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTB|GPIO_PIN5)
+#define GPIO_GPIO11_OUTPUT	(GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTB|GPIO_PIN8)
+
+/* PWM
+ *
+ * Eight PWM outputs are configured.
+ *
+ * Pins:
+ *
+ * CH1 : PA8  : TIM1_CH1
+ * CH2 : PA9  : TIM1_CH2
+ * CH3 : PA10 : TIM1_CH3
+ * CH4 : PA11 : TIM1_CH4
+ * CH5 : PC6  : TIM3_CH1
+ * CH6 : PC7  : TIM3_CH2
+ * CH7 : PC8  : TIM3_CH3
+ * CH8 : PC9  : TIM3_CH4
+ */
+#define GPIO_TIM1_CH1OUT	GPIO_TIM1_CH1OUT_1
+#define GPIO_TIM1_CH2OUT	GPIO_TIM1_CH2OUT_1
+#define GPIO_TIM1_CH3OUT	GPIO_TIM1_CH3OUT_1
+#define GPIO_TIM1_CH4OUT	GPIO_TIM1_CH4OUT_1
+#define GPIO_TIM3_CH1OUT	GPIO_TIM3_CH1OUT_3
+#define GPIO_TIM3_CH2OUT	GPIO_TIM3_CH2OUT_3
+#define GPIO_TIM3_CH3OUT	GPIO_TIM3_CH3OUT_2
+#define GPIO_TIM3_CH4OUT	GPIO_TIM3_CH4OUT_2
+
+/* High-resolution timer */
+#define HRT_TIMER		8	/* use timer 8 for the HRT */
+#define HRT_TIMER_CHANNEL	1	/* use capture/compare channel */
+
+/* Tone Alarm (no onboard speaker )*/
+#define TONE_ALARM_TIMER	2	/* timer 2 */
+#define TONE_ALARM_CHANNEL	1	/* channel 1 */
+#define GPIO_TONE_ALARM_IDLE	(GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTA|GPIO_PIN0)
+#define GPIO_TONE_ALARM		(GPIO_ALT|GPIO_AF1|GPIO_SPEED_2MHz|GPIO_PUSHPULL|GPIO_PORTA|GPIO_PIN0)
+
+
+/****************************************************************************************************
+ * Public Types
+ ****************************************************************************************************/
+
+/****************************************************************************************************
+ * Public data
+ ****************************************************************************************************/
+
+#ifndef __ASSEMBLY__
+
+/****************************************************************************************************
+ * Public Functions
+ ****************************************************************************************************/
+
+/****************************************************************************************************
+ * Name: stm32_spiinitialize
+ *
+ * Description:
+ *   Called to configure SPI chip select GPIO pins for the PX4FMU board.
+ *
+ ****************************************************************************************************/
+
+extern void stm32_spiinitialize(void);
+
+#endif /* __ASSEMBLY__ */
+
+__END_DECLS
diff --git a/src/drivers/boards/aerocore/module.mk b/src/drivers/boards/aerocore/module.mk
new file mode 100644
index 000000000..b53fe0a29
--- /dev/null
+++ b/src/drivers/boards/aerocore/module.mk
@@ -0,0 +1,8 @@
+#
+# Board-specific startup code for the AeroCore
+#
+
+SRCS		 = aerocore_init.c \
+		   aerocore_pwm_servo.c \
+		   aerocore_spi.c \
+		   aerocore_led.c
diff --git a/src/drivers/boards/px4fmu-v1/board_config.h b/src/drivers/boards/px4fmu-v1/board_config.h
index 02c26b5c0..58273f2d2 100644
--- a/src/drivers/boards/px4fmu-v1/board_config.h
+++ b/src/drivers/boards/px4fmu-v1/board_config.h
@@ -85,6 +85,8 @@ __BEGIN_DECLS
 #define GPIO_SPI_CS_MPU		(GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_50MHz|GPIO_OUTPUT_SET|GPIO_PORTB|GPIO_PIN0)
 #define GPIO_SPI_CS_SDCARD	(GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_50MHz|GPIO_OUTPUT_SET|GPIO_PORTA|GPIO_PIN4)
 
+#define PX4_SPI_BUS_SENSORS	1
+
 /*
  * Use these in place of the spi_dev_e enumeration to
  * select a specific SPI device on SPI1
diff --git a/src/drivers/boards/px4fmu-v2/board_config.h b/src/drivers/boards/px4fmu-v2/board_config.h
index 7cfca7656..c2de1bfba 100644
--- a/src/drivers/boards/px4fmu-v2/board_config.h
+++ b/src/drivers/boards/px4fmu-v2/board_config.h
@@ -107,6 +107,8 @@ __BEGIN_DECLS
 #define GPIO_SPI_CS_FRAM	(GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_SET|GPIO_PORTD|GPIO_PIN10)
 #define GPIO_SPI_CS_MPU		(GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_SET|GPIO_PORTC|GPIO_PIN2)
 
+#define PX4_SPI_BUS_SENSORS	1
+
 /* Use these in place of the spi_dev_e enumeration to select a specific SPI device on SPI1 */
 #define PX4_SPIDEV_GYRO		1
 #define PX4_SPIDEV_ACCEL_MAG	2
diff --git a/src/drivers/drv_gpio.h b/src/drivers/drv_gpio.h
index f60964c2b..5acd0d343 100644
--- a/src/drivers/drv_gpio.h
+++ b/src/drivers/drv_gpio.h
@@ -94,6 +94,14 @@
 
 #endif
 
+#ifdef CONFIG_ARCH_BOARD_AEROCORE
+/*
+ * AeroCore GPIO numbers and configuration.
+ *
+ */
+# define PX4FMU_DEVICE_PATH	"/dev/px4fmu"
+#endif
+
 #ifdef CONFIG_ARCH_BOARD_PX4IO_V1
 /* no GPIO driver on the PX4IOv1 board */
 #endif
@@ -146,4 +154,4 @@
 
 #define GPIO_SENSOR_RAIL_RESET	GPIOC(13)
 
-#endif /* _DRV_GPIO_H */
\ No newline at end of file
+#endif /* _DRV_GPIO_H */
diff --git a/src/drivers/drv_gps.h b/src/drivers/drv_gps.h
index 06e3535b3..e14f4e00d 100644
--- a/src/drivers/drv_gps.h
+++ b/src/drivers/drv_gps.h
@@ -43,10 +43,14 @@
 #include <stdint.h>
 #include <sys/ioctl.h>
 
+#include "board_config.h"
+
 #include "drv_sensor.h"
 #include "drv_orb_dev.h"
 
+#ifndef GPS_DEFAULT_UART_PORT
 #define GPS_DEFAULT_UART_PORT "/dev/ttyS3"
+#endif
 
 #define GPS_DEVICE_PATH	"/dev/gps"
 
diff --git a/src/drivers/gps/gps.cpp b/src/drivers/gps/gps.cpp
index a902bdf2f..2d8d37298 100644
--- a/src/drivers/gps/gps.cpp
+++ b/src/drivers/gps/gps.cpp
@@ -63,6 +63,8 @@
 #include <uORB/uORB.h>
 #include <uORB/topics/vehicle_gps_position.h>
 
+#include <board_config.h>
+
 #include "ubx.h"
 #include "mtk.h"
 
@@ -276,14 +278,14 @@ GPS::task_main()
 			_report.timestamp_position = hrt_absolute_time();
 			_report.lat = (int32_t)47.378301e7f;
 			_report.lon = (int32_t)8.538777e7f;
-			_report.alt = (int32_t)400e3f;
+			_report.alt = (int32_t)1200e3f;
 			_report.timestamp_variance = hrt_absolute_time();
 			_report.s_variance_m_s = 10.0f;
 			_report.p_variance_m = 10.0f;
 			_report.c_variance_rad = 0.1f;
 			_report.fix_type = 3;
-			_report.eph_m = 3.0f;
-			_report.epv_m = 7.0f;
+			_report.eph_m = 0.9f;
+			_report.epv_m = 1.8f;
 			_report.timestamp_velocity = hrt_absolute_time();
 			_report.vel_n_m_s = 0.0f;
 			_report.vel_e_m_s = 0.0f;
@@ -421,7 +423,14 @@ GPS::task_main()
 void
 GPS::cmd_reset()
 {
-	//XXX add reset?
+#ifdef GPIO_GPS_NRESET
+	warnx("Toggling GPS reset pin");
+	stm32_configgpio(GPIO_GPS_NRESET);
+	stm32_gpiowrite(GPIO_GPS_NRESET, 0);
+	usleep(100);
+	stm32_gpiowrite(GPIO_GPS_NRESET, 1);
+	warnx("Toggled GPS reset pin");
+#endif
 }
 
 void
diff --git a/src/drivers/l3gd20/l3gd20.cpp b/src/drivers/l3gd20/l3gd20.cpp
index 90c3db9ae..37e72388b 100644
--- a/src/drivers/l3gd20/l3gd20.cpp
+++ b/src/drivers/l3gd20/l3gd20.cpp
@@ -34,6 +34,9 @@
 /**
  * @file l3gd20.cpp
  * Driver for the ST L3GD20 MEMS gyro connected via SPI.
+ *
+ * Note: With the exception of the self-test feature, the ST L3G4200D is
+ *       also supported by this driver.
  */
 
 #include <nuttx/config.h>
@@ -89,9 +92,11 @@ static const int ERROR = -1;
 #define ADDR_WHO_AM_I			0x0F
 #define WHO_I_AM_H 				0xD7
 #define WHO_I_AM				0xD4
+#define WHO_I_AM_L3G4200D		0xD3	/* for L3G4200D */
 
 #define ADDR_CTRL_REG1			0x20
 #define REG1_RATE_LP_MASK			0xF0 /* Mask to guard partial register update */
+
 /* keep lowpass low to avoid noise issues */
 #define RATE_95HZ_LP_25HZ		((0<<7) | (0<<6) | (0<<5) | (1<<4))
 #define RATE_190HZ_LP_25HZ		((0<<7) | (1<<6) | (0<<5) | (1<<4))
@@ -166,9 +171,14 @@ static const int ERROR = -1;
 #define FIFO_CTRL_BYPASS_TO_STREAM_MODE		(1<<7)
 
 #define L3GD20_DEFAULT_RATE			760
+#define L3G4200D_DEFAULT_RATE			800
 #define L3GD20_DEFAULT_RANGE_DPS		2000
 #define L3GD20_DEFAULT_FILTER_FREQ		30
 
+#ifndef SENSOR_BOARD_ROTATION_DEFAULT
+#define SENSOR_BOARD_ROTATION_DEFAULT		SENSOR_BOARD_ROTATION_270_DEG
+#endif
+
 extern "C" { __EXPORT int l3gd20_main(int argc, char *argv[]); }
 
 class L3GD20 : public device::SPI
@@ -216,6 +226,9 @@ private:
 	math::LowPassFilter2p	_gyro_filter_y;
 	math::LowPassFilter2p	_gyro_filter_z;
 
+	/* true if an L3G4200D is detected */
+	bool	_is_l3g4200d;
+
 	/**
 	 * Start automatic measurement.
 	 */
@@ -324,14 +337,15 @@ L3GD20::L3GD20(int bus, const char* path, spi_dev_e device) :
 	_gyro_topic(-1),
 	_class_instance(-1),
 	_current_rate(0),
-	_orientation(SENSOR_BOARD_ROTATION_270_DEG),
+	_orientation(SENSOR_BOARD_ROTATION_DEFAULT),
 	_read(0),
 	_sample_perf(perf_alloc(PC_ELAPSED, "l3gd20_read")),
 	_reschedules(perf_alloc(PC_COUNT, "l3gd20_reschedules")),
 	_errors(perf_alloc(PC_COUNT, "l3gd20_errors")),
 	_gyro_filter_x(L3GD20_DEFAULT_RATE, L3GD20_DEFAULT_FILTER_FREQ),
 	_gyro_filter_y(L3GD20_DEFAULT_RATE, L3GD20_DEFAULT_FILTER_FREQ),
-	_gyro_filter_z(L3GD20_DEFAULT_RATE, L3GD20_DEFAULT_FILTER_FREQ)
+	_gyro_filter_z(L3GD20_DEFAULT_RATE, L3GD20_DEFAULT_FILTER_FREQ),
+	_is_l3g4200d(false)
 {
 	// enable debug() calls
 	_debug_enabled = true;
@@ -413,14 +427,7 @@ L3GD20::probe()
 	/* verify that the device is attached and functioning, accept L3GD20 and L3GD20H */
 	if (read_reg(ADDR_WHO_AM_I) == WHO_I_AM) {
 
-		#ifdef CONFIG_ARCH_BOARD_PX4FMU_V1
-			_orientation = SENSOR_BOARD_ROTATION_270_DEG;
-		#elif CONFIG_ARCH_BOARD_PX4FMU_V2
-			_orientation = SENSOR_BOARD_ROTATION_270_DEG;
-		#else
-			#error This driver needs a board selection, either CONFIG_ARCH_BOARD_PX4FMU_V1 or CONFIG_ARCH_BOARD_PX4FMU_V2
-		#endif
-
+		_orientation = SENSOR_BOARD_ROTATION_DEFAULT;
 		success = true;
 	}
 
@@ -430,6 +437,13 @@ L3GD20::probe()
 		success = true;
 	}
 
+	/* Detect the L3G4200D used on AeroCore */
+	if (read_reg(ADDR_WHO_AM_I) == WHO_I_AM_L3G4200D) {
+		_is_l3g4200d = true;
+		_orientation = SENSOR_BOARD_ROTATION_DEFAULT;
+		success = true;
+	}
+
 	if (success)
 		return OK;
 
@@ -502,6 +516,9 @@ L3GD20::ioctl(struct file *filp, int cmd, unsigned long arg)
 				/* set default/max polling rate */
 			case SENSOR_POLLRATE_MAX:
 			case SENSOR_POLLRATE_DEFAULT:
+				if (_is_l3g4200d) {
+					return ioctl(filp, SENSORIOCSPOLLRATE, L3G4200D_DEFAULT_RATE);
+				}
 				return ioctl(filp, SENSORIOCSPOLLRATE, L3GD20_DEFAULT_RATE);
 
 				/* adjust to a legal polling interval in Hz */
@@ -683,23 +700,26 @@ L3GD20::set_samplerate(unsigned frequency)
 	uint8_t bits = REG1_POWER_NORMAL | REG1_Z_ENABLE | REG1_Y_ENABLE | REG1_X_ENABLE;
 
 	if (frequency == 0)
-		frequency = 760;
+		frequency = _is_l3g4200d ? 800 : 760;
 
-	/* use limits good for H or non-H models */
+	/*
+	 * Use limits good for H or non-H models. Rates are slightly different
+	 * for L3G4200D part but register settings are the same.
+	 */
 	if (frequency <= 100) {
-		_current_rate = 95;
+		_current_rate = _is_l3g4200d ? 100 : 95;
 		bits |= RATE_95HZ_LP_25HZ;
 
 	} else if (frequency <= 200) {
-		_current_rate = 190;
+		_current_rate = _is_l3g4200d ? 200 : 190;
 		bits |= RATE_190HZ_LP_50HZ;
 
 	} else if (frequency <= 400) {
-		_current_rate = 380;
+		_current_rate = _is_l3g4200d ? 400 : 380;
 		bits |= RATE_380HZ_LP_50HZ;
 
 	} else if (frequency <= 800) {
-		_current_rate = 760;
+		_current_rate = _is_l3g4200d ? 800 : 760;
 		bits |= RATE_760HZ_LP_50HZ;
 	} else {
 		return -EINVAL;
@@ -772,7 +792,7 @@ L3GD20::reset()
 	 * callback fast enough to not miss data. */
 	write_reg(ADDR_FIFO_CTRL_REG, FIFO_CTRL_BYPASS_MODE);
 
-	set_samplerate(0); // 760Hz
+	set_samplerate(0); // 760Hz or 800Hz
 	set_range(L3GD20_DEFAULT_RANGE_DPS);
 	set_driver_lowpass_filter(L3GD20_DEFAULT_RATE, L3GD20_DEFAULT_FILTER_FREQ);
 
@@ -971,7 +991,7 @@ start()
 		errx(0, "already started");
 
 	/* create the driver */
-	g_dev = new L3GD20(1 /* SPI bus 1 */, L3GD20_DEVICE_PATH, (spi_dev_e)PX4_SPIDEV_GYRO);
+	g_dev = new L3GD20(PX4_SPI_BUS_SENSORS, L3GD20_DEVICE_PATH, (spi_dev_e)PX4_SPIDEV_GYRO);
 
 	if (g_dev == nullptr)
 		goto fail;
diff --git a/src/drivers/lsm303d/lsm303d.cpp b/src/drivers/lsm303d/lsm303d.cpp
index 4dee7649b..4ca8b5e42 100644
--- a/src/drivers/lsm303d/lsm303d.cpp
+++ b/src/drivers/lsm303d/lsm303d.cpp
@@ -1793,7 +1793,7 @@ start()
 		errx(0, "already started");
 
 	/* create the driver */
-	g_dev = new LSM303D(1 /* SPI dev 1 */, LSM303D_DEVICE_PATH_ACCEL, (spi_dev_e)PX4_SPIDEV_ACCEL_MAG);
+	g_dev = new LSM303D(PX4_SPI_BUS_SENSORS, LSM303D_DEVICE_PATH_ACCEL, (spi_dev_e)PX4_SPIDEV_ACCEL_MAG);
 
 	if (g_dev == nullptr) {
 		warnx("failed instantiating LSM303D obj");
diff --git a/src/drivers/ms5611/ms5611_spi.cpp b/src/drivers/ms5611/ms5611_spi.cpp
index 26216e840..8759d16a1 100644
--- a/src/drivers/ms5611/ms5611_spi.cpp
+++ b/src/drivers/ms5611/ms5611_spi.cpp
@@ -117,7 +117,7 @@ private:
 device::Device *
 MS5611_spi_interface(ms5611::prom_u &prom_buf)
 {
-	return new MS5611_SPI(1 /* XXX MAGIC NUMBER */, (spi_dev_e)PX4_SPIDEV_BARO, prom_buf);
+	return new MS5611_SPI(PX4_SPI_BUS_SENSORS, (spi_dev_e)PX4_SPIDEV_BARO, prom_buf);
 }
 
 MS5611_SPI::MS5611_SPI(int bus, spi_dev_e device, ms5611::prom_u &prom_buf) :
diff --git a/src/drivers/px4fmu/fmu.cpp b/src/drivers/px4fmu/fmu.cpp
index 4d72ead9b..fd69cf795 100644
--- a/src/drivers/px4fmu/fmu.cpp
+++ b/src/drivers/px4fmu/fmu.cpp
@@ -92,6 +92,7 @@ public:
 		MODE_2PWM,
 		MODE_4PWM,
 		MODE_6PWM,
+		MODE_8PWM,
 	};
 	PX4FMU();
 	virtual ~PX4FMU();
@@ -113,6 +114,9 @@ private:
 #if defined(CONFIG_ARCH_BOARD_PX4FMU_V2)
 	static const unsigned _max_actuators = 6;
 #endif
+#if defined(CONFIG_ARCH_BOARD_AEROCORE)
+	static const unsigned _max_actuators = 8;
+#endif
 
 	Mode		_mode;
 	unsigned	_pwm_default_rate;
@@ -203,6 +207,20 @@ const PX4FMU::GPIOConfig PX4FMU::_gpio_tab[] = {
 	{GPIO_VDD_5V_HIPOWER_OC, 0,                       0},
 	{GPIO_VDD_5V_PERIPH_OC,  0,                       0},
 #endif
+#if defined(CONFIG_ARCH_BOARD_AEROCORE)
+	/* AeroCore breaks out User GPIOs on J11 */
+	{GPIO_GPIO0_INPUT,       GPIO_GPIO0_OUTPUT,       0},
+	{GPIO_GPIO1_INPUT,       GPIO_GPIO1_OUTPUT,       0},
+	{GPIO_GPIO3_INPUT,       GPIO_GPIO3_OUTPUT,       0},
+	{GPIO_GPIO4_INPUT,       GPIO_GPIO4_OUTPUT,       0},
+	{GPIO_GPIO5_INPUT,       GPIO_GPIO5_OUTPUT,       0},
+	{GPIO_GPIO6_INPUT,       GPIO_GPIO6_OUTPUT,       0},
+	{GPIO_GPIO7_INPUT,       GPIO_GPIO7_OUTPUT,       0},
+	{GPIO_GPIO8_INPUT,       GPIO_GPIO8_OUTPUT,       0},
+	{GPIO_GPIO9_INPUT,       GPIO_GPIO9_OUTPUT,       0},
+	{GPIO_GPIO10_INPUT,      GPIO_GPIO10_OUTPUT,      0},
+	{GPIO_GPIO11_INPUT,      GPIO_GPIO11_OUTPUT,      0},
+#endif
 };
 
 const unsigned PX4FMU::_ngpio = sizeof(PX4FMU::_gpio_tab) / sizeof(PX4FMU::_gpio_tab[0]);
@@ -382,6 +400,20 @@ PX4FMU::set_mode(Mode mode)
 
 		break;
 
+#ifdef CONFIG_ARCH_BOARD_AEROCORE
+	case MODE_8PWM: // AeroCore PWMs as 8 PWM outs
+		debug("MODE_8PWM");
+		/* default output rates */
+		_pwm_default_rate = 50;
+		_pwm_alt_rate = 50;
+		_pwm_alt_rate_channels = 0;
+
+		/* XXX magic numbers */
+		up_pwm_servo_init(0xff);
+		set_pwm_rate(_pwm_alt_rate_channels, _pwm_default_rate, _pwm_alt_rate);
+		break;
+#endif
+
 	case MODE_NONE:
 		debug("MODE_NONE");
 
@@ -602,6 +634,9 @@ PX4FMU::task_main()
 					num_outputs = 6;
 					break;
 
+				case MODE_8PWM:
+					num_outputs = 8;
+					break;
 				default:
 					num_outputs = 0;
 					break;
@@ -757,6 +792,9 @@ PX4FMU::ioctl(file *filp, int cmd, unsigned long arg)
 	case MODE_2PWM:
 	case MODE_4PWM:
 	case MODE_6PWM:
+#ifdef CONFIG_ARCH_BOARD_AEROCORE
+	case MODE_8PWM:
+#endif
 		ret = pwm_ioctl(filp, cmd, arg);
 		break;
 
@@ -986,6 +1024,15 @@ PX4FMU::pwm_ioctl(file *filp, int cmd, unsigned long arg)
 			break;
 		}
 
+#ifdef CONFIG_ARCH_BOARD_AEROCORE
+	case PWM_SERVO_SET(7):
+	case PWM_SERVO_SET(6):
+		if (_mode < MODE_8PWM) {
+			ret = -EINVAL;
+			break;
+		}
+#endif
+
 	case PWM_SERVO_SET(5):
 	case PWM_SERVO_SET(4):
 		if (_mode < MODE_6PWM) {
@@ -1013,6 +1060,15 @@ PX4FMU::pwm_ioctl(file *filp, int cmd, unsigned long arg)
 
 		break;
 
+#ifdef CONFIG_ARCH_BOARD_AEROCORE
+	case PWM_SERVO_GET(7):
+	case PWM_SERVO_GET(6):
+		if (_mode < MODE_8PWM) {
+			ret = -EINVAL;
+			break;
+		}
+#endif
+
 	case PWM_SERVO_GET(5):
 	case PWM_SERVO_GET(4):
 		if (_mode < MODE_6PWM) {
@@ -1040,12 +1096,22 @@ PX4FMU::pwm_ioctl(file *filp, int cmd, unsigned long arg)
 	case PWM_SERVO_GET_RATEGROUP(3):
 	case PWM_SERVO_GET_RATEGROUP(4):
 	case PWM_SERVO_GET_RATEGROUP(5):
+#ifdef CONFIG_ARCH_BOARD_AEROCORE
+	case PWM_SERVO_GET_RATEGROUP(6):
+	case PWM_SERVO_GET_RATEGROUP(7):
+#endif
 		*(uint32_t *)arg = up_pwm_servo_get_rate_group(cmd - PWM_SERVO_GET_RATEGROUP(0));
 		break;
 
 	case PWM_SERVO_GET_COUNT:
 	case MIXERIOCGETOUTPUTCOUNT:
 		switch (_mode) {
+#ifdef CONFIG_ARCH_BOARD_AEROCORE
+		case MODE_8PWM:
+			*(unsigned *)arg = 8;
+			break;
+#endif
+
 		case MODE_6PWM:
 			*(unsigned *)arg = 6;
 			break;
@@ -1091,6 +1157,11 @@ PX4FMU::pwm_ioctl(file *filp, int cmd, unsigned long arg)
 			set_mode(MODE_6PWM);
 			break;
 #endif
+#if defined(CONFIG_ARCH_BOARD_AEROCORE)
+		case 8:
+			set_mode(MODE_8PWM);
+			break;
+#endif
 
 		default:
 			ret = -EINVAL;
@@ -1181,10 +1252,17 @@ PX4FMU::write(file *filp, const char *buffer, size_t len)
 	unsigned count = len / 2;
 	uint16_t values[6];
 
+#ifdef CONFIG_ARCH_BOARD_AEROCORE
+	if (count > 8) {
+		// we have at most 8 outputs
+		count = 8;
+	}
+#else
 	if (count > 6) {
 		// we have at most 6 outputs
 		count = 6;
 	}
+#endif
 
 	// allow for misaligned values
 	memcpy(values, buffer, count * 2);
@@ -1459,6 +1537,9 @@ fmu_new_mode(PortMode new_mode)
 #if defined(CONFIG_ARCH_BOARD_PX4FMU_V2)
 		servo_mode = PX4FMU::MODE_6PWM;
 #endif
+#if defined(CONFIG_ARCH_BOARD_AEROCORE)
+		servo_mode = PX4FMU::MODE_8PWM;
+#endif
 		break;
 
 		/* mixed modes supported on v1 board only */
@@ -1776,7 +1857,7 @@ fmu_main(int argc, char *argv[])
 	fprintf(stderr, "FMU: unrecognised command %s, try:\n", verb);
 #if defined(CONFIG_ARCH_BOARD_PX4FMU_V1)
 	fprintf(stderr, "  mode_gpio, mode_serial, mode_pwm, mode_gpio_serial, mode_pwm_serial, mode_pwm_gpio, test\n");
-#elif defined(CONFIG_ARCH_BOARD_PX4FMU_V2)
+#elif defined(CONFIG_ARCH_BOARD_PX4FMU_V2) || defined(CONFIG_ARCH_BOARD_AEROCORE)
 	fprintf(stderr, "  mode_gpio, mode_pwm, test, sensor_reset [milliseconds]\n");
 #endif
 	exit(1);
diff --git a/src/drivers/px4fmu/module.mk b/src/drivers/px4fmu/module.mk
index 05bc7a5b3..eeb59e1a1 100644
--- a/src/drivers/px4fmu/module.mk
+++ b/src/drivers/px4fmu/module.mk
@@ -4,3 +4,5 @@
 
 MODULE_COMMAND	 = fmu
 SRCS		 = fmu.cpp
+
+MODULE_STACKSIZE = 1200
diff --git a/src/drivers/px4io/module.mk b/src/drivers/px4io/module.mk
index 2054faa12..c14f1f783 100644
--- a/src/drivers/px4io/module.mk
+++ b/src/drivers/px4io/module.mk
@@ -44,3 +44,5 @@ SRCS			= px4io.cpp \
 
 # XXX prune to just get UART registers
 INCLUDE_DIRS    += $(NUTTX_SRC)/arch/arm/src/stm32 $(NUTTX_SRC)/arch/arm/src/common
+
+MODULE_STACKSIZE = 1200
diff --git a/src/drivers/px4io/px4io.cpp b/src/drivers/px4io/px4io.cpp
index aec6dd3b7..3c3b59862 100644
--- a/src/drivers/px4io/px4io.cpp
+++ b/src/drivers/px4io/px4io.cpp
@@ -529,6 +529,11 @@ PX4IO::~PX4IO()
 	if (_interface != nullptr)
 		delete _interface;
 
+	/* deallocate perfs */
+	perf_free(_perf_update);
+	perf_free(_perf_write);
+	perf_free(_perf_chan_count);
+
 	g_dev = nullptr;
 }
 
@@ -1448,7 +1453,7 @@ PX4IO::io_get_raw_rc_input(rc_input_values &input_rc)
 	/* we don't have the status bits, so input_source has to be set elsewhere */
 	input_rc.input_source = RC_INPUT_SOURCE_UNKNOWN;
 
-	static const unsigned prolog = (PX4IO_P_RAW_RC_BASE - PX4IO_P_RAW_RC_COUNT);
+	const unsigned prolog = (PX4IO_P_RAW_RC_BASE - PX4IO_P_RAW_RC_COUNT);
 	uint16_t regs[RC_INPUT_MAX_CHANNELS + prolog];
 
 	/*
@@ -1456,8 +1461,6 @@ PX4IO::io_get_raw_rc_input(rc_input_values &input_rc)
 	 *
 	 * This should be the common case (9 channel R/C control being a reasonable upper bound).
 	 */
-	input_rc.timestamp_publication = hrt_absolute_time();
-
 	ret = io_reg_get(PX4IO_PAGE_RAW_RC_INPUT, PX4IO_P_RAW_RC_COUNT, &regs[0], prolog + 9);
 
 	if (ret != OK)
@@ -1469,23 +1472,38 @@ PX4IO::io_get_raw_rc_input(rc_input_values &input_rc)
 	 */
 	channel_count = regs[PX4IO_P_RAW_RC_COUNT];
 
-	if (channel_count != _rc_chan_count)
+	/* limit the channel count */
+	if (channel_count > RC_INPUT_MAX_CHANNELS) {
+		channel_count = RC_INPUT_MAX_CHANNELS;
+	}
+
+	/* count channel count changes to identify signal integrity issues */
+	if (channel_count != _rc_chan_count) {
 		perf_count(_perf_chan_count);
+	}
 
 	_rc_chan_count = channel_count;
 
+	input_rc.timestamp_publication = hrt_absolute_time();
+
 	input_rc.rc_ppm_frame_length = regs[PX4IO_P_RAW_RC_DATA];
 	input_rc.rssi = regs[PX4IO_P_RAW_RC_NRSSI];
 	input_rc.rc_failsafe = (regs[PX4IO_P_RAW_RC_FLAGS] & PX4IO_P_RAW_RC_FLAGS_FAILSAFE);
+	input_rc.rc_lost = !(regs[PX4IO_P_RAW_RC_FLAGS] & PX4IO_P_RAW_RC_FLAGS_RC_OK);
 	input_rc.rc_lost_frame_count = regs[PX4IO_P_RAW_LOST_FRAME_COUNT];
 	input_rc.rc_total_frame_count = regs[PX4IO_P_RAW_FRAME_COUNT];
+	input_rc.channel_count = channel_count;
 
 	/* rc_lost has to be set before the call to this function */
-	if (!input_rc.rc_lost && !input_rc.rc_failsafe)
+	if (!input_rc.rc_lost && !input_rc.rc_failsafe) {
 		_rc_last_valid = input_rc.timestamp_publication;
+	}
 
 	input_rc.timestamp_last_signal = _rc_last_valid;
 
+	/* FIELDS NOT SET HERE */
+	/* input_rc.input_source is set after this call XXX we might want to mirror the flags in the RC struct */
+
 	if (channel_count > 9) {
 		ret = io_reg_get(PX4IO_PAGE_RAW_RC_INPUT, PX4IO_P_RAW_RC_BASE + 9, &regs[prolog + 9], channel_count - 9);
 
@@ -1493,8 +1511,10 @@ PX4IO::io_get_raw_rc_input(rc_input_values &input_rc)
 			return ret;
 	}
 
-	input_rc.channel_count = channel_count;
-	memcpy(input_rc.values, &regs[prolog], channel_count * 2);
+	/* last thing set are the actual channel values as 16 bit values */
+	for (unsigned i = 0; i < channel_count; i++) {
+		input_rc.values[i] = regs[prolog + i];
+	}
 
 	return ret;
 }
diff --git a/src/drivers/stm32/adc/adc.cpp b/src/drivers/stm32/adc/adc.cpp
index 3a60d2cae..de13b8969 100644
--- a/src/drivers/stm32/adc/adc.cpp
+++ b/src/drivers/stm32/adc/adc.cpp
@@ -419,6 +419,10 @@ adc_main(int argc, char *argv[])
 		g_adc = new ADC((1 << 2) | (1 << 3) | (1 << 4) | 
 			(1 << 10) | (1 << 11) | (1 << 12) | (1 << 13) | (1 << 14) | (1 << 15));
 #endif
+#ifdef CONFIG_ARCH_BOARD_AEROCORE
+		/* XXX this hardcodes the default channel set for AeroCore - should be configurable */
+		g_adc = new ADC((1 << 10) | (1 << 11) | (1 << 12) | (1 << 13));
+#endif
 
 		if (g_adc == nullptr)
 			errx(1, "couldn't allocate the ADC driver");
diff --git a/src/drivers/stm32/drv_hrt.c b/src/drivers/stm32/drv_hrt.c
index 5bb550279..281f918d7 100644
--- a/src/drivers/stm32/drv_hrt.c
+++ b/src/drivers/stm32/drv_hrt.c
@@ -141,7 +141,7 @@
 # define HRT_TIMER_POWER_REG	STM32_RCC_APB1ENR
 # define HRT_TIMER_POWER_BIT	RCC_APB2ENR_TIM10EN
 # define HRT_TIMER_VECTOR	STM32_IRQ_TIM1UP
-# define HRT_TIMER_CLOCK	STM32_APB1_TIM10_CLKIN
+# define HRT_TIMER_CLOCK	STM32_APB2_TIM10_CLKIN
 # if CONFIG_STM32_TIM10
 #  error must not set CONFIG_STM32_TIM11=y and HRT_TIMER=10
 # endif
@@ -150,7 +150,7 @@
 # define HRT_TIMER_POWER_REG	STM32_RCC_APB1ENR
 # define HRT_TIMER_POWER_BIT	RCC_APB2ENR_TIM11EN
 # define HRT_TIMER_VECTOR	STM32_IRQ_TIM1TRGCOM
-# define HRT_TIMER_CLOCK	STM32_APB1_TIM11_CLKIN
+# define HRT_TIMER_CLOCK	STM32_APB2_TIM11_CLKIN
 # if CONFIG_STM32_TIM11
 #  error must not set CONFIG_STM32_TIM11=y and HRT_TIMER=11
 # endif
diff --git a/src/examples/fixedwing_control/module.mk b/src/examples/fixedwing_control/module.mk
index d2c48934f..a2a9eb113 100644
--- a/src/examples/fixedwing_control/module.mk
+++ b/src/examples/fixedwing_control/module.mk
@@ -39,3 +39,5 @@ MODULE_COMMAND	= ex_fixedwing_control
 
 SRCS		= main.c \
 		  params.c
+
+MODULE_STACKSIZE = 1200
diff --git a/src/examples/px4_daemon_app/module.mk b/src/examples/px4_daemon_app/module.mk
index 5f8aa73d5..fc4223142 100644
--- a/src/examples/px4_daemon_app/module.mk
+++ b/src/examples/px4_daemon_app/module.mk
@@ -38,3 +38,5 @@
 MODULE_COMMAND		= px4_daemon_app
 
 SRCS			= px4_daemon_app.c
+
+MODULE_STACKSIZE = 1200
diff --git a/src/examples/px4_daemon_app/px4_daemon_app.c b/src/examples/px4_daemon_app/px4_daemon_app.c
index 53f1b4a9a..3eaf14148 100644
--- a/src/examples/px4_daemon_app/px4_daemon_app.c
+++ b/src/examples/px4_daemon_app/px4_daemon_app.c
@@ -98,7 +98,7 @@ int px4_daemon_app_main(int argc, char *argv[])
 		daemon_task = task_spawn_cmd("daemon",
 					 SCHED_DEFAULT,
 					 SCHED_PRIORITY_DEFAULT,
-					 4096,
+					 2000,
 					 px4_daemon_thread_main,
 					 (argv) ? (const char **)&argv[2] : (const char **)NULL);
 		exit(0);
diff --git a/src/examples/px4_mavlink_debug/module.mk b/src/examples/px4_mavlink_debug/module.mk
index fefd61496..c7ef97fc4 100644
--- a/src/examples/px4_mavlink_debug/module.mk
+++ b/src/examples/px4_mavlink_debug/module.mk
@@ -37,4 +37,6 @@
 
 MODULE_COMMAND		= px4_mavlink_debug
 
-SRCS			= px4_mavlink_debug.c
\ No newline at end of file
+SRCS			= px4_mavlink_debug.c
+
+MODULE_STACKSIZE = 2000
diff --git a/src/lib/version/version.h b/src/lib/version/version.h
index af733aaf0..d8ccb6774 100644
--- a/src/lib/version/version.h
+++ b/src/lib/version/version.h
@@ -59,4 +59,8 @@
 #define	HW_ARCH "PX4FMU_V2"
 #endif
 
+#ifdef CONFIG_ARCH_BOARD_AEROCORE
+#define	HW_ARCH "AEROCORE"
+#endif
+
 #endif /* VERSION_H_ */
diff --git a/src/modules/att_pos_estimator_ekf/KalmanNav.cpp b/src/modules/att_pos_estimator_ekf/KalmanNav.cpp
deleted file mode 100644
index 5cf84542b..000000000
--- a/src/modules/att_pos_estimator_ekf/KalmanNav.cpp
+++ /dev/null
@@ -1,812 +0,0 @@
-/****************************************************************************
- *
- *   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 KalmanNav.cpp
- *
- * Kalman filter navigation code
- */
-
-#include <poll.h>
-
-#include "KalmanNav.hpp"
-#include <systemlib/err.h>
-#include <geo/geo.h>
-
-// constants
-// Titterton pg. 52
-static const float omega = 7.2921150e-5f; // earth rotation rate, rad/s
-static const float R0 = 6378137.0f; // earth radius, m
-static const float g0 = 9.806f; // standard gravitational accel. m/s^2
-static const int8_t ret_ok = 0; 		// no error in function
-static const int8_t ret_error = -1; 	// error occurred
-
-KalmanNav::KalmanNav(SuperBlock *parent, const char *name) :
-	SuperBlock(parent, name),
-	// subscriptions
-	_sensors(&getSubscriptions(), ORB_ID(sensor_combined), 5), // limit to 200 Hz
-	_gps(&getSubscriptions(), ORB_ID(vehicle_gps_position), 100), // limit to 10 Hz
-	_param_update(&getSubscriptions(), ORB_ID(parameter_update), 1000), // limit to 1 Hz
-	// publications
-	_pos(&getPublications(), ORB_ID(vehicle_global_position)),
-	_localPos(&getPublications(), ORB_ID(vehicle_local_position)),
-	_att(&getPublications(), ORB_ID(vehicle_attitude)),
-	// timestamps
-	_pubTimeStamp(hrt_absolute_time()),
-	_predictTimeStamp(hrt_absolute_time()),
-	_attTimeStamp(hrt_absolute_time()),
-	_outTimeStamp(hrt_absolute_time()),
-	// frame count
-	_navFrames(0),
-	// miss counts
-	_miss(0),
-	// accelerations
-	fN(0), fE(0), fD(0),
-	// state
-	phi(0), theta(0), psi(0),
-	vN(0), vE(0), vD(0),
-	lat(0), lon(0), alt(0),
-	lat0(0), lon0(0), alt0(0),
-	// parameters for ground station
-	_vGyro(this, "V_GYRO"),
-	_vAccel(this, "V_ACCEL"),
-	_rMag(this, "R_MAG"),
-	_rGpsVel(this, "R_GPS_VEL"),
-	_rGpsPos(this, "R_GPS_POS"),
-	_rGpsAlt(this, "R_GPS_ALT"),
-	_rPressAlt(this, "R_PRESS_ALT"),
-	_rAccel(this, "R_ACCEL"),
-	_magDip(this, "ENV_MAG_DIP"),
-	_magDec(this, "ENV_MAG_DEC"),
-	_g(this, "ENV_G"),
-	_faultPos(this, "FAULT_POS"),
-	_faultAtt(this, "FAULT_ATT"),
-	_attitudeInitialized(false),
-	_positionInitialized(false),
-	_attitudeInitCounter(0)
-{
-	using namespace math;
-
-	memset(&ref, 0, sizeof(ref));
-
-	F.zero();
-	G.zero();
-	V.zero();
-	HAtt.zero();
-	RAtt.zero();
-	HPos.zero();
-	RPos.zero();
-
-	// initial state covariance matrix
-	P0.identity();
-	P0 *= 0.01f;
-	P = P0;
-
-	// initial state
-	phi = 0.0f;
-	theta = 0.0f;
-	psi = 0.0f;
-	vN = 0.0f;
-	vE = 0.0f;
-	vD = 0.0f;
-	lat = 0.0f;
-	lon = 0.0f;
-	alt = 0.0f;
-
-	// initialize rotation quaternion with a single raw sensor measurement
-	_sensors.update();
-	q = init(
-		_sensors.accelerometer_m_s2[0],
-		_sensors.accelerometer_m_s2[1],
-		_sensors.accelerometer_m_s2[2],
-		_sensors.magnetometer_ga[0],
-		_sensors.magnetometer_ga[1],
-		_sensors.magnetometer_ga[2]);
-
-	// initialize dcm
-	C_nb = q.to_dcm();
-
-	// HPos is constant
-	HPos(0, 3) = 1.0f;
-	HPos(1, 4) = 1.0f;
-	HPos(2, 6) = 1.0e7f * M_RAD_TO_DEG_F;
-	HPos(3, 7) = 1.0e7f * M_RAD_TO_DEG_F;
-	HPos(4, 8) = 1.0f;
-	HPos(5, 8) = 1.0f;
-
-	// initialize all parameters
-	updateParams();
-}
-
-math::Quaternion KalmanNav::init(float ax, float ay, float az, float mx, float my, float mz)
-{
-    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);
-
-    float q0 = cosRoll * cosPitch * cosHeading + sinRoll * sinPitch * sinHeading;
-    float q1 = sinRoll * cosPitch * cosHeading - cosRoll * sinPitch * sinHeading;
-    float q2 = cosRoll * sinPitch * cosHeading + sinRoll * cosPitch * sinHeading;
-    float q3 = cosRoll * cosPitch * sinHeading - sinRoll * sinPitch * cosHeading;
-
-    return math::Quaternion(q0, q1, q2, q3);
-
-}
-
-void KalmanNav::update()
-{
-	using namespace math;
-
-	struct pollfd fds[1];
-	fds[0].fd = _sensors.getHandle();
-	fds[0].events = POLLIN;
-
-	// poll for new data
-	int ret = poll(fds, 1, 1000);
-
-	if (ret < 0) {
-		// XXX this is seriously bad - should be an emergency
-		return;
-
-	} else if (ret == 0) { // timeout
-		return;
-	}
-
-	// get new timestamp
-	uint64_t newTimeStamp = hrt_absolute_time();
-
-	// check updated subscriptions
-	if (_param_update.updated()) updateParams();
-
-	bool gpsUpdate = _gps.updated();
-	bool sensorsUpdate = _sensors.updated();
-
-	// get new information from subscriptions
-	// this clears update flag
-	updateSubscriptions();
-
-	// initialize attitude when sensors online
-	if (!_attitudeInitialized && sensorsUpdate) {
-		if (correctAtt() == ret_ok) _attitudeInitCounter++;
-
-		if (_attitudeInitCounter > 100) {
-			warnx("initialized EKF attitude");
-			warnx("phi: %8.4f, theta: %8.4f, psi: %8.4f",
-			       double(phi), double(theta), double(psi));
-			_attitudeInitialized = true;
-		}
-	}
-
-	// initialize position when gps received
-	if (!_positionInitialized &&
-	    _attitudeInitialized && // wait for attitude first
-	    gpsUpdate &&
-	    _gps.fix_type > 2
-	    //&& _gps.counter_pos_valid > 10
-	   ) {
-		vN = _gps.vel_n_m_s;
-		vE = _gps.vel_e_m_s;
-		vD = _gps.vel_d_m_s;
-		setLatDegE7(_gps.lat);
-		setLonDegE7(_gps.lon);
-		setAltE3(_gps.alt);
-		// set reference position for
-		// local position
-		lat0 = lat;
-		lon0 = lon;
-		alt0 = alt;
-		map_projection_init(&ref, lat0, lon0);
-		_positionInitialized = true;
-		warnx("initialized EKF state with GPS");
-		warnx("vN: %8.4f, vE: %8.4f, vD: %8.4f, lat: %8.4f, lon: %8.4f, alt: %8.4f",
-		       double(vN), double(vE), double(vD),
-		       lat, lon, double(alt));
-	}
-
-	// prediction step
-	// using sensors timestamp so we can account for packet lag
-	float dt = (_sensors.timestamp - _predictTimeStamp) / 1.0e6f;
-	//printf("dt: %15.10f\n", double(dt));
-	_predictTimeStamp = _sensors.timestamp;
-
-	// don't predict if time greater than a second
-	if (dt < 1.0f) {
-		predictState(dt);
-		predictStateCovariance(dt);
-		// count fast frames
-		_navFrames += 1;
-	}
-
-	// count times 100 Hz rate isn't met
-	if (dt > 0.01f) _miss++;
-
-	// gps correction step
-	if (_positionInitialized && gpsUpdate) {
-		correctPos();
-	}
-
-	// attitude correction step
-	if (_attitudeInitialized 								// initialized
-	    && sensorsUpdate 								// new data
-	    && _sensors.timestamp - _attTimeStamp > 1e6 / 50 	// 50 Hz
-	   ) {
-		_attTimeStamp = _sensors.timestamp;
-		correctAtt();
-	}
-
-	// publication
-	if (newTimeStamp - _pubTimeStamp > 1e6 / 50) { // 50 Hz
-		_pubTimeStamp = newTimeStamp;
-
-		updatePublications();
-	}
-
-	// output
-	if (newTimeStamp - _outTimeStamp > 10e6) { // 0.1 Hz
-		_outTimeStamp = newTimeStamp;
-		//printf("nav: %4d Hz, miss #: %4d\n",
-		//       _navFrames / 10, _miss / 10);
-		_navFrames = 0;
-		_miss = 0;
-	}
-}
-
-void KalmanNav::updatePublications()
-{
-	using namespace math;
-
-	// global position publication
-	_pos.timestamp = _pubTimeStamp;
-	_pos.time_gps_usec = _gps.timestamp_position;
-	_pos.lat = lat * M_RAD_TO_DEG;
-	_pos.lon = lon * M_RAD_TO_DEG;
-	_pos.alt = float(alt);
-	_pos.vel_n = vN;
-	_pos.vel_e = vE;
-	_pos.vel_d = vD;
-	_pos.yaw = psi;
-
-	// local position publication
-	float x;
-	float y;
-	bool landed = alt < (alt0 + 0.1); // XXX improve?
-	map_projection_project(&ref, lat, lon, &x, &y);
-	_localPos.timestamp = _pubTimeStamp;
-	_localPos.xy_valid = true;
-	_localPos.z_valid = true;
-	_localPos.v_xy_valid = true;
-	_localPos.v_z_valid = true;
-	_localPos.x = x;
-	_localPos.y = y;
-	_localPos.z = alt0 - alt;
-	_localPos.vx = vN;
-	_localPos.vy = vE;
-	_localPos.vz = vD;
-	_localPos.yaw = psi;
-	_localPos.xy_global = true;
-	_localPos.z_global = true;
-	_localPos.ref_timestamp = _pubTimeStamp;
-	_localPos.ref_lat = lat * M_RAD_TO_DEG;
-	_localPos.ref_lon = lon * M_RAD_TO_DEG;
-	_localPos.ref_alt = 0;
-	_localPos.landed = landed;
-
-	// attitude publication
-	_att.timestamp = _pubTimeStamp;
-	_att.roll = phi;
-	_att.pitch = theta;
-	_att.yaw = psi;
-	_att.rollspeed = _sensors.gyro_rad_s[0];
-	_att.pitchspeed = _sensors.gyro_rad_s[1];
-	_att.yawspeed = _sensors.gyro_rad_s[2];
-	// TODO, add gyro offsets to filter
-	_att.rate_offsets[0] = 0.0f;
-	_att.rate_offsets[1] = 0.0f;
-	_att.rate_offsets[2] = 0.0f;
-
-	for (int i = 0; i < 3; i++) for (int j = 0; j < 3; j++)
-			_att.R[i][j] = C_nb(i, j);
-
-	for (int i = 0; i < 4; i++) _att.q[i] = q(i);
-
-	_att.R_valid = true;
-	_att.q_valid = true;
-
-	// selectively update publications,
-	// do NOT call superblock do-all method
-	if (_positionInitialized) {
-		_pos.update();
-		_localPos.update();
-	}
-
-	if (_attitudeInitialized)
-		_att.update();
-}
-
-int KalmanNav::predictState(float dt)
-{
-	using namespace math;
-
-	// trig
-	float sinL = sinf(lat);
-	float cosL = cosf(lat);
-	float cosLSing = cosf(lat);
-
-	// prevent singularity
-	if (fabsf(cosLSing) < 0.01f) {
-		if (cosLSing > 0) cosLSing = 0.01;
-		else cosLSing = -0.01;
-	}
-
-	// attitude prediction
-	if (_attitudeInitialized) {
-		Vector<3> w(_sensors.gyro_rad_s);
-
-		// attitude
-		q = q + q.derivative(w) * dt;
-
-		// renormalize quaternion if needed
-		if (fabsf(q.length() - 1.0f) > 1e-4f) {
-			q.normalize();
-		}
-
-		// C_nb update
-		C_nb = q.to_dcm();
-
-		// euler update
-		Vector<3> euler = C_nb.to_euler();
-		phi = euler.data[0];
-		theta = euler.data[1];
-		psi = euler.data[2];
-
-		// specific acceleration in nav frame
-		Vector<3> accelB(_sensors.accelerometer_m_s2);
-		Vector<3> accelN = C_nb * accelB;
-		fN = accelN(0);
-		fE = accelN(1);
-		fD = accelN(2);
-	}
-
-	// position prediction
-	if (_positionInitialized) {
-		// neglects angular deflections in local gravity
-		// see Titerton pg. 70
-		float R = R0 + float(alt);
-		float LDot = vN / R;
-		float lDot = vE / (cosLSing * R);
-		float rotRate = 2 * omega + lDot;
-
-		// XXX position prediction using speed
-		float vNDot = fN - vE * rotRate * sinL +
-			      vD * LDot;
-		float vDDot = fD - vE * rotRate * cosL -
-			      vN * LDot + _g.get();
-		float vEDot = fE + vN * rotRate * sinL +
-			      vDDot * rotRate * cosL;
-
-		// rectangular integration
-		vN += vNDot * dt;
-		vE += vEDot * dt;
-		vD += vDDot * dt;
-		lat += double(LDot * dt);
-		lon += double(lDot * dt);
-		alt += double(-vD * dt);
-	}
-
-	return ret_ok;
-}
-
-int KalmanNav::predictStateCovariance(float dt)
-{
-	using namespace math;
-
-	// trig
-	float sinL = sinf(lat);
-	float cosL = cosf(lat);
-	float cosLSq = cosL * cosL;
-	float tanL = tanf(lat);
-
-	// prepare for matrix
-	float R = R0 + float(alt);
-	float RSq = R * R;
-
-	// F Matrix
-	// Titterton pg. 291
-
-	F(0, 1) = -(omega * sinL + vE * tanL / R);
-	F(0, 2) = vN / R;
-	F(0, 4) = 1.0f / R;
-	F(0, 6) = -omega * sinL;
-	F(0, 8) = -vE / RSq;
-
-	F(1, 0) = omega * sinL + vE * tanL / R;
-	F(1, 2) = omega * cosL + vE / R;
-	F(1, 3) = -1.0f / R;
-	F(1, 8) = vN / RSq;
-
-	F(2, 0) = -vN / R;
-	F(2, 1) = -omega * cosL - vE / R;
-	F(2, 4) = -tanL / R;
-	F(2, 6) = -omega * cosL - vE / (R * cosLSq);
-	F(2, 8) = vE * tanL / RSq;
-
-	F(3, 1) = -fD;
-	F(3, 2) = fE;
-	F(3, 3) = vD / R;
-	F(3, 4) = -2 * (omega * sinL + vE * tanL / R);
-	F(3, 5) = vN / R;
-	F(3, 6) = -vE * (2 * omega * cosL + vE / (R * cosLSq));
-	F(3, 8) = (vE * vE * tanL - vN * vD) / RSq;
-
-	F(4, 0) = fD;
-	F(4, 2) = -fN;
-	F(4, 3) = 2 * omega * sinL + vE * tanL / R;
-	F(4, 4) = (vN * tanL + vD) / R;
-	F(4, 5) = 2 * omega * cosL + vE / R;
-	F(4, 6) = 2 * omega * (vN * cosL - vD * sinL) +
-		  vN * vE / (R * cosLSq);
-	F(4, 8) = -vE * (vN * tanL + vD) / RSq;
-
-	F(5, 0) = -fE;
-	F(5, 1) = fN;
-	F(5, 3) = -2 * vN / R;
-	F(5, 4) = -2 * (omega * cosL + vE / R);
-	F(5, 6) = 2 * omega * vE * sinL;
-	F(5, 8) = (vN * vN + vE * vE) / RSq;
-
-	F(6, 3) = 1 / R;
-	F(6, 8) = -vN / RSq;
-
-	F(7, 4) = 1 / (R * cosL);
-	F(7, 6) = vE * tanL / (R * cosL);
-	F(7, 8) = -vE / (cosL * RSq);
-
-	F(8, 5) = -1;
-
-	// G Matrix
-	// Titterton pg. 291
-	G(0, 0) = -C_nb(0, 0);
-	G(0, 1) = -C_nb(0, 1);
-	G(0, 2) = -C_nb(0, 2);
-	G(1, 0) = -C_nb(1, 0);
-	G(1, 1) = -C_nb(1, 1);
-	G(1, 2) = -C_nb(1, 2);
-	G(2, 0) = -C_nb(2, 0);
-	G(2, 1) = -C_nb(2, 1);
-	G(2, 2) = -C_nb(2, 2);
-
-	G(3, 3) = C_nb(0, 0);
-	G(3, 4) = C_nb(0, 1);
-	G(3, 5) = C_nb(0, 2);
-	G(4, 3) = C_nb(1, 0);
-	G(4, 4) = C_nb(1, 1);
-	G(4, 5) = C_nb(1, 2);
-	G(5, 3) = C_nb(2, 0);
-	G(5, 4) = C_nb(2, 1);
-	G(5, 5) = C_nb(2, 2);
-
-	// continuous prediction equations
-	// for discrete time EKF
-	// http://en.wikipedia.org/wiki/Extended_Kalman_filter
-	P = P + (F * P + P * F.transposed() + G * V * G.transposed()) * dt;
-
-	return ret_ok;
-}
-
-int KalmanNav::correctAtt()
-{
-	using namespace math;
-
-	// trig
-	float cosPhi = cosf(phi);
-	float cosTheta = cosf(theta);
-	// float cosPsi = cosf(psi);
-	float sinPhi = sinf(phi);
-	float sinTheta = sinf(theta);
-	// float sinPsi = sinf(psi);
-
-	// mag predicted measurement
-	// choosing some typical magnetic field properties,
-	//  TODO dip/dec depend on lat/ lon/ time
-	//float dip = _magDip.get() / M_RAD_TO_DEG_F; // dip, inclination with level
-	float dec = _magDec.get() / M_RAD_TO_DEG_F; // declination, clockwise rotation from north
-
-	// compensate roll and pitch, but not yaw
-	// XXX take the vectors out of the C_nb matrix to avoid singularities
-	math::Matrix<3,3> C_rp;
-	C_rp.from_euler(phi, theta, 0.0f);//C_nb.transposed();
-
-	// mag measurement
-	Vector<3> magBody(_sensors.magnetometer_ga);
-
-	// transform to earth frame
-	Vector<3> magNav = C_rp * magBody;
-
-	// calculate error between estimate and measurement
-	// apply declination correction for true heading as well.
-	float yMag = -atan2f(magNav(1),magNav(0)) - psi - dec;
-	if (yMag > M_PI_F) yMag -= 2*M_PI_F;
-	if (yMag < -M_PI_F) yMag += 2*M_PI_F;
-
-	// accel measurement
-	Vector<3> zAccel(_sensors.accelerometer_m_s2);
-	float accelMag = zAccel.length();
-	zAccel.normalize();
-
-	// ignore accel correction when accel mag not close to g
-	Matrix<4,4> RAttAdjust = RAtt;
-
-	bool ignoreAccel = fabsf(accelMag - _g.get()) > 1.1f;
-
-	if (ignoreAccel) {
-		RAttAdjust(1, 1) = 1.0e10;
-		RAttAdjust(2, 2) = 1.0e10;
-		RAttAdjust(3, 3) = 1.0e10;
-
-	} else {
-		//printf("correcting attitude with accel\n");
-	}
-
-	// accel predicted measurement
-	Vector<3> zAccelHat = (C_nb.transposed() * Vector<3>(0, 0, -_g.get())).normalized();
-
-	// calculate residual
-	Vector<4> y(yMag, zAccel(0) - zAccelHat(0), zAccel(1) - zAccelHat(1), zAccel(2) - zAccelHat(2));
-
-	// HMag
-	HAtt(0, 2) = 1;
-
-	// HAccel
-	HAtt(1, 1) = cosTheta;
-	HAtt(2, 0) = -cosPhi * cosTheta;
-	HAtt(2, 1) = sinPhi * sinTheta;
-	HAtt(3, 0) = sinPhi * cosTheta;
-	HAtt(3, 1) = cosPhi * sinTheta;
-
-	// compute correction
-	// http://en.wikipedia.org/wiki/Extended_Kalman_filter
-	Matrix<4, 4> S = HAtt * P * HAtt.transposed() + RAttAdjust; // residual covariance
-	Matrix<9, 4> K = P * HAtt.transposed() * S.inversed();
-	Vector<9> xCorrect = K * y;
-
-	// check correciton is sane
-	for (size_t i = 0; i < xCorrect.get_size(); i++) {
-		float val = xCorrect(i);
-
-		if (isnan(val) || isinf(val)) {
-			// abort correction and return
-			warnx("numerical failure in att correction");
-			// reset P matrix to P0
-			P = P0;
-			return ret_error;
-		}
-	}
-
-	// correct state
-	if (!ignoreAccel) {
-		phi += xCorrect(PHI);
-		theta += xCorrect(THETA);
-	}
-
-	psi += xCorrect(PSI);
-
-	// attitude also affects nav velocities
-	if (_positionInitialized) {
-		vN += xCorrect(VN);
-		vE += xCorrect(VE);
-		vD += xCorrect(VD);
-	}
-
-	// update state covariance
-	// http://en.wikipedia.org/wiki/Extended_Kalman_filter
-	P = P - K * HAtt * P;
-
-	// fault detection
-	float beta = y * (S.inversed() * y);
-
-	if (beta > _faultAtt.get()) {
-		warnx("fault in attitude: beta = %8.4f", (double)beta);
-		warnx("y:"); y.print();
-	}
-
-	// update quaternions from euler
-	// angle correction
-	q.from_euler(phi, theta, psi);
-
-	return ret_ok;
-}
-
-int KalmanNav::correctPos()
-{
-	using namespace math;
-
-	// residual
-	Vector<6> y;
-	y(0) = _gps.vel_n_m_s - vN;
-	y(1) = _gps.vel_e_m_s - vE;
-	y(2) = double(_gps.lat) - double(lat) * 1.0e7 * M_RAD_TO_DEG;
-	y(3) = double(_gps.lon) - double(lon) * 1.0e7 * M_RAD_TO_DEG;
-	y(4) = _gps.alt / 1.0e3f - alt;
-	y(5) = _sensors.baro_alt_meter - alt;
-
-	// compute correction
-	// http://en.wikipedia.org/wiki/Extended_Kalman_filter
-	Matrix<6,6> S = HPos * P * HPos.transposed() + RPos; // residual covariance
-	Matrix<9,6> K = P * HPos.transposed() * S.inversed();
-	Vector<9> xCorrect = K * y;
-
-	// check correction is sane
-	for (size_t i = 0; i < xCorrect.get_size(); i++) {
-		float val = xCorrect(i);
-
-		if (!isfinite(val)) {
-			// abort correction and return
-			warnx("numerical failure in gps correction");
-			// fallback to GPS
-			vN = _gps.vel_n_m_s;
-			vE = _gps.vel_e_m_s;
-			vD = _gps.vel_d_m_s;
-			setLatDegE7(_gps.lat);
-			setLonDegE7(_gps.lon);
-			setAltE3(_gps.alt);
-			// reset P matrix to P0
-			P = P0;
-			return ret_error;
-		}
-	}
-
-	// correct state
-	vN += xCorrect(VN);
-	vE += xCorrect(VE);
-	vD += xCorrect(VD);
-	lat += double(xCorrect(LAT));
-	lon += double(xCorrect(LON));
-	alt += xCorrect(ALT);
-
-	// update state covariance
-	// http://en.wikipedia.org/wiki/Extended_Kalman_filter
-	P = P - K * HPos * P;
-
-	// fault detetcion
-	float beta = y * (S.inversed() * y);
-
-	static int counter = 0;
-	if (beta > _faultPos.get() && (counter % 10 == 0)) {
-		warnx("fault in gps: beta = %8.4f", (double)beta);
-		warnx("Y/N: vN: %8.4f, vE: %8.4f, lat: %8.4f, lon: %8.4f, alt: %8.4f, baro: %8.4f",
-		       double(y(0) / sqrtf(RPos(0, 0))),
-		       double(y(1) / sqrtf(RPos(1, 1))),
-		       double(y(2) / sqrtf(RPos(2, 2))),
-		       double(y(3) / sqrtf(RPos(3, 3))),
-		       double(y(4) / sqrtf(RPos(4, 4))),
-		       double(y(5) / sqrtf(RPos(5, 5))));
-	}
-	counter++;
-
-	return ret_ok;
-}
-
-void KalmanNav::updateParams()
-{
-	using namespace math;
-	using namespace control;
-	SuperBlock::updateParams();
-
-	// gyro noise
-	V(0, 0) = _vGyro.get();   // gyro x, rad/s
-	V(1, 1) = _vGyro.get();   // gyro y
-	V(2, 2) = _vGyro.get();   // gyro z
-
-	// accel noise
-	V(3, 3) = _vAccel.get();   // accel x, m/s^2
-	V(4, 4) = _vAccel.get();   // accel y
-	V(5, 5) = _vAccel.get();   // accel z
-
-	// magnetometer noise
-	float noiseMin = 1e-6f;
-	float noiseMagSq = _rMag.get() * _rMag.get();
-
-	if (noiseMagSq < noiseMin) noiseMagSq = noiseMin;
-
-	RAtt(0, 0) = noiseMagSq; // normalized direction
-
-	// accelerometer noise
-	float noiseAccelSq = _rAccel.get() * _rAccel.get();
-
-	// bound noise to prevent singularities
-	if (noiseAccelSq < noiseMin) noiseAccelSq = noiseMin;
-
-	RAtt(1, 1) = noiseAccelSq; // normalized direction
-	RAtt(2, 2) = noiseAccelSq;
-	RAtt(3, 3) = noiseAccelSq;
-
-	// gps noise
-	float R = R0 + float(alt);
-	float cosLSing = cosf(lat);
-
-	// prevent singularity
-	if (fabsf(cosLSing) < 0.01f) {
-		if (cosLSing > 0) cosLSing = 0.01;
-		else cosLSing = -0.01;
-	}
-
-	float noiseVel = _rGpsVel.get();
-	float noiseLatDegE7 = 1.0e7f * M_RAD_TO_DEG_F * _rGpsPos.get() / R;
-	float noiseLonDegE7 = noiseLatDegE7 / cosLSing;
-	float noiseGpsAlt = _rGpsAlt.get();
-	float noisePressAlt = _rPressAlt.get();
-
-	// bound noise to prevent singularities
-	if (noiseVel < noiseMin) noiseVel = noiseMin;
-
-	if (noiseLatDegE7 < noiseMin) noiseLatDegE7 = noiseMin;
-
-	if (noiseLonDegE7 < noiseMin) noiseLonDegE7 = noiseMin;
-
-	if (noiseGpsAlt < noiseMin) noiseGpsAlt = noiseMin;
-
-	if (noisePressAlt < noiseMin) noisePressAlt = noiseMin;
-
-	RPos(0, 0) = noiseVel * noiseVel; // vn
-	RPos(1, 1) = noiseVel * noiseVel; // ve
-	RPos(2, 2) = noiseLatDegE7 * noiseLatDegE7; // lat
-	RPos(3, 3) = noiseLonDegE7 * noiseLonDegE7; // lon
-	RPos(4, 4) = noiseGpsAlt * noiseGpsAlt; // h
-	RPos(5, 5) = noisePressAlt * noisePressAlt; // h
-	// XXX, note that RPos depends on lat, so updateParams should
-	// be called if lat changes significantly
-}
diff --git a/src/modules/att_pos_estimator_ekf/KalmanNav.hpp b/src/modules/att_pos_estimator_ekf/KalmanNav.hpp
deleted file mode 100644
index 24df153cb..000000000
--- a/src/modules/att_pos_estimator_ekf/KalmanNav.hpp
+++ /dev/null
@@ -1,194 +0,0 @@
-/****************************************************************************
- *
- *   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 KalmanNav.hpp
- *
- * kalman filter navigation code
- */
-
-#pragma once
-
-//#define MATRIX_ASSERT
-//#define VECTOR_ASSERT
-
-#include <nuttx/config.h>
-
-#include <mathlib/mathlib.h>
-#include <controllib/blocks.hpp>
-#include <controllib/block/BlockParam.hpp>
-#include <uORB/Subscription.hpp>
-#include <uORB/Publication.hpp>
-#include <lib/geo/geo.h>
-
-#include <uORB/topics/vehicle_attitude.h>
-#include <uORB/topics/vehicle_global_position.h>
-#include <uORB/topics/vehicle_local_position.h>
-#include <uORB/topics/sensor_combined.h>
-#include <uORB/topics/vehicle_gps_position.h>
-#include <uORB/topics/parameter_update.h>
-
-#include <drivers/drv_accel.h>
-#include <drivers/drv_gyro.h>
-#include <drivers/drv_mag.h>
-
-#include <drivers/drv_hrt.h>
-#include <poll.h>
-#include <unistd.h>
-
-/**
- * Kalman filter navigation class
- * http://en.wikipedia.org/wiki/Extended_Kalman_filter
- * Discrete-time extended Kalman filter
- */
-class KalmanNav : public control::SuperBlock
-{
-public:
-	/**
-	 * Constructor
-	 */
-	KalmanNav(SuperBlock *parent, const char *name);
-
-	/**
-	 * Deconstuctor
-	 */
-
-	virtual ~KalmanNav() {};
-
-	math::Quaternion init(float ax, float ay, float az, float mx, float my, float mz);
-
-	/**
-	 * The main callback function for the class
-	 */
-	void update();
-
-
-	/**
-	 * Publication update
-	 */
-	virtual void updatePublications();
-
-	/**
-	 * State prediction
-	 * Continuous, non-linear
-	 */
-	int predictState(float dt);
-
-	/**
-	 * State covariance prediction
-	 * Continuous, linear
-	 */
-	int predictStateCovariance(float dt);
-
-	/**
-	 * Attitude correction
-	 */
-	int correctAtt();
-
-	/**
-	 * Position correction
-	 */
-	int correctPos();
-
-	/**
-	 * Overloaded update parameters
-	 */
-	virtual void updateParams();
-protected:
-	// kalman filter
-	math::Matrix<9,9> F;             /**< Jacobian(f,x), where dx/dt = f(x,u) */
-	math::Matrix<9,6> G;             /**< noise shaping matrix for gyro/accel */
-	math::Matrix<9,9> P;             /**< state covariance matrix */
-	math::Matrix<9,9> P0;            /**< initial state covariance matrix */
-	math::Matrix<6,6> V;             /**< gyro/ accel noise matrix */
-	math::Matrix<4,9> HAtt;          /**< attitude measurement matrix */
-	math::Matrix<4,4> RAtt;          /**< attitude measurement noise matrix */
-	math::Matrix<6,9> HPos;          /**< position measurement jacobian matrix */
-	math::Matrix<6,6> RPos;          /**< position measurement noise matrix */
-	// attitude
-	math::Matrix<3,3> C_nb;             /**< direction cosine matrix from body to nav frame */
-	math::Quaternion q;         /**< quaternion from body to nav frame */
-	// subscriptions
-	uORB::Subscription<sensor_combined_s> _sensors;          /**< sensors sub. */
-	uORB::Subscription<vehicle_gps_position_s> _gps;         /**< gps sub. */
-	uORB::Subscription<parameter_update_s> _param_update;    /**< parameter update sub. */
-	// publications
-	uORB::Publication<vehicle_global_position_s> _pos;       /**< position pub. */
-	uORB::Publication<vehicle_local_position_s> _localPos;   /**< local position pub. */
-	uORB::Publication<vehicle_attitude_s> _att;              /**< attitude pub. */
-	// time stamps
-	uint64_t _pubTimeStamp;     /**< output data publication time stamp */
-	uint64_t _predictTimeStamp; /**< prediction time stamp */
-	uint64_t _attTimeStamp;     /**< attitude correction time stamp */
-	uint64_t _outTimeStamp;     /**< output time stamp */
-	// frame count
-	uint16_t _navFrames;        /**< navigation frames completed in output cycle */
-	// miss counts
-	uint16_t _miss;         	/**< number of times fast prediction loop missed */
-	// accelerations
-	float fN, fE, fD;           /**< navigation frame acceleration */
-	// states
-	enum {PHI = 0, THETA, PSI, VN, VE, VD, LAT, LON, ALT};  /**< state enumeration */
-	float phi, theta, psi;                  /**< 3-2-1 euler angles */
-	float vN, vE, vD;                       /**< navigation velocity, m/s */
-	double lat, lon;                   	/**< lat, lon radians */
-	// parameters
-	float alt;                   		/**< altitude, meters */
-	double lat0, lon0;                   	/**< reference latitude and longitude */
-	struct map_projection_reference_s ref;	/**< local projection reference */
-	float alt0;                   		/**<  refeerence altitude (ground height) */
-	control::BlockParamFloat _vGyro;      /**< gyro process noise */
-	control::BlockParamFloat _vAccel;     /**< accelerometer process noise  */
-	control::BlockParamFloat _rMag;       /**< magnetometer measurement noise  */
-	control::BlockParamFloat _rGpsVel;    /**< gps velocity measurement noise */
-	control::BlockParamFloat _rGpsPos;    /**< gps position measurement noise */
-	control::BlockParamFloat _rGpsAlt;    /**< gps altitude measurement noise */
-	control::BlockParamFloat _rPressAlt;  /**< press altitude measurement noise */
-	control::BlockParamFloat _rAccel;     /**< accelerometer measurement noise */
-	control::BlockParamFloat _magDip;     /**< magnetic inclination with level */
-	control::BlockParamFloat _magDec;     /**< magnetic declination, clockwise rotation */
-	control::BlockParamFloat _g;          /**< gravitational constant */
-	control::BlockParamFloat _faultPos;   /**< fault detection threshold for position */
-	control::BlockParamFloat _faultAtt;   /**< fault detection threshold for attitude */
-	// status
-	bool _attitudeInitialized;
-	bool _positionInitialized;
-	uint16_t _attitudeInitCounter;
-	// accessors
-	int32_t getLatDegE7() { return int32_t(lat * 1.0e7 * M_RAD_TO_DEG); }
-	void setLatDegE7(int32_t val) { lat = val / 1.0e7 / M_RAD_TO_DEG; }
-	int32_t getLonDegE7() { return int32_t(lon * 1.0e7 * M_RAD_TO_DEG); }
-	void setLonDegE7(int32_t val) { lon = val / 1.0e7 / M_RAD_TO_DEG; }
-	int32_t getAltE3() { return int32_t(alt * 1.0e3); }
-	void setAltE3(int32_t val) { alt = double(val) / 1.0e3; }
-};
diff --git a/src/modules/att_pos_estimator_ekf/kalman_main.cpp b/src/modules/att_pos_estimator_ekf/kalman_main.cpp
deleted file mode 100644
index 3d20d4d2d..000000000
--- a/src/modules/att_pos_estimator_ekf/kalman_main.cpp
+++ /dev/null
@@ -1,157 +0,0 @@
-/****************************************************************************
- *
- *   Copyright (c) 2012, 2013 PX4 Development Team. All rights reserved.
- *   Author: 	James Goppert
- *
- * 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 kalman_main.cpp
- * Combined attitude / position estimator.
- *
- * @author James Goppert
- */
-
-#include <nuttx/config.h>
-#include <unistd.h>
-#include <stdio.h>
-#include <stdlib.h>
-#include <string.h>
-#include <systemlib/systemlib.h>
-#include <systemlib/param/param.h>
-#include <systemlib/err.h>
-#include <drivers/drv_hrt.h>
-#include <math.h>
-#include "KalmanNav.hpp"
-
-static bool thread_should_exit = false;     /**< Deamon exit flag */
-static bool thread_running = false;     /**< Deamon status flag */
-static int daemon_task;             /**< Handle of deamon task / thread */
-
-/**
- * Deamon management function.
- */
-extern "C" __EXPORT int att_pos_estimator_ekf_main(int argc, char *argv[]);
-
-/**
- * Mainloop of deamon.
- */
-int kalman_demo_thread_main(int argc, char *argv[]);
-
-/**
- * Print the correct usage.
- */
-static void usage(const char *reason);
-
-static void
-usage(const char *reason)
-{
-	if (reason)
-		fprintf(stderr, "%s\n", reason);
-
-	warnx("usage: att_pos_estimator_ekf {start|stop|status} [-p <additional params>]");
-	exit(1);
-}
-
-/**
- * The deamon app only briefly exists to start
- * the background job. The stack size assigned in the
- * Makefile does only apply to this management task.
- *
- * The actual stack size should be set in the call
- * to task_create().
- */
-int att_pos_estimator_ekf_main(int argc, char *argv[])
-{
-
-	if (argc < 1)
-		usage("missing command");
-
-	if (!strcmp(argv[1], "start")) {
-
-		if (thread_running) {
-			warnx("already running");
-			/* this is not an error */
-			exit(0);
-		}
-
-		thread_should_exit = false;
-
-		daemon_task = task_spawn_cmd("att_pos_estimator_ekf",
-					 SCHED_DEFAULT,
-					 SCHED_PRIORITY_MAX - 30,
-					 8192,
-					 kalman_demo_thread_main,
-					 (argv) ? (const char **)&argv[2] : (const char **)NULL);
-		exit(0);
-	}
-
-	if (!strcmp(argv[1], "stop")) {
-		thread_should_exit = true;
-		exit(0);
-	}
-
-	if (!strcmp(argv[1], "status")) {
-		if (thread_running) {
-			warnx("is running\n");
-			exit(0);
-
-		} else {
-			warnx("not started\n");
-			exit(1);
-		}
-
-	}
-
-	usage("unrecognized command");
-	exit(1);
-}
-
-int kalman_demo_thread_main(int argc, char *argv[])
-{
-
-	warnx("starting");
-
-	using namespace math;
-
-	thread_running = true;
-
-	KalmanNav nav(NULL, "KF");
-
-	while (!thread_should_exit) {
-		nav.update();
-	}
-
-	warnx("exiting.");
-
-	thread_running = false;
-
-	return 0;
-}
diff --git a/src/modules/attitude_estimator_ekf/attitude_estimator_ekf_main.cpp b/src/modules/attitude_estimator_ekf/attitude_estimator_ekf_main.cpp
index c61b6ff3f..2ec889efe 100755
--- a/src/modules/attitude_estimator_ekf/attitude_estimator_ekf_main.cpp
+++ b/src/modules/attitude_estimator_ekf/attitude_estimator_ekf_main.cpp
@@ -1,8 +1,6 @@
 /****************************************************************************
  *
- *   Copyright (C) 2012 PX4 Development Team. All rights reserved.
- *   Author: Tobias Naegeli <naegelit@student.ethz.ch>
- *           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
@@ -34,9 +32,12 @@
  ****************************************************************************/
 
 /*
- * @file attitude_estimator_ekf_main.c
+ * @file attitude_estimator_ekf_main.cpp
  *
  * Extended Kalman Filter for Attitude Estimation.
+ *
+ * @author Tobias Naegeli <naegelit@student.ethz.ch>
+ * @author Lorenz Meier <lm@inf.ethz.ch>
  */
 
 #include <nuttx/config.h>
@@ -111,7 +112,7 @@ usage(const char *reason)
  * Makefile does only apply to this management task.
  *
  * The actual stack size should be set in the call
- * to task_create().
+ * to task_spawn_cmd().
  */
 int attitude_estimator_ekf_main(int argc, char *argv[])
 {
diff --git a/src/modules/attitude_estimator_ekf/module.mk b/src/modules/attitude_estimator_ekf/module.mk
index d98647f99..99d0c5bf2 100644
--- a/src/modules/attitude_estimator_ekf/module.mk
+++ b/src/modules/attitude_estimator_ekf/module.mk
@@ -50,3 +50,5 @@ SRCS		 = attitude_estimator_ekf_main.cpp \
 		   codegen/rtGetNaN.c \
 		   codegen/norm.c \
 		   codegen/cross.c
+
+MODULE_STACKSIZE = 1200
diff --git a/src/modules/attitude_estimator_so3/attitude_estimator_so3_main.cpp b/src/modules/attitude_estimator_so3/attitude_estimator_so3_main.cpp
index 3653defa6..e55b01160 100755
--- a/src/modules/attitude_estimator_so3/attitude_estimator_so3_main.cpp
+++ b/src/modules/attitude_estimator_so3/attitude_estimator_so3_main.cpp
@@ -1,8 +1,6 @@
 /****************************************************************************
  *
- *   Copyright (C) 2013 PX4 Development Team. All rights reserved.
- *   Author: Hyon Lim <limhyon@gmail.com>
- *           Anton Babushkin <anton.babushkin@me.com>
+ *   Copyright (c) 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
@@ -36,6 +34,9 @@
  /*
  * @file attitude_estimator_so3_main.cpp
  *
+ * @author Hyon Lim <limhyon@gmail.com>
+ * @author Anton Babushkin <anton.babushkin@me.com>
+ *
  * Implementation of nonlinear complementary filters on the SO(3).
  * This code performs attitude estimation by using accelerometer, gyroscopes and magnetometer.
  * Result is provided as quaternion, 1-2-3 Euler angle and rotation matrix.
@@ -131,7 +132,7 @@ usage(const char *reason)
  * Makefile does only apply to this management task.
  *
  * The actual stack size should be set in the call
- * to task_create().
+ * to task_spawn_cmd().
  */
 int attitude_estimator_so3_main(int argc, char *argv[])
 {
diff --git a/src/modules/attitude_estimator_so3/module.mk b/src/modules/attitude_estimator_so3/module.mk
index e29bb16a6..f52715abb 100644
--- a/src/modules/attitude_estimator_so3/module.mk
+++ b/src/modules/attitude_estimator_so3/module.mk
@@ -6,3 +6,5 @@ MODULE_COMMAND	 = attitude_estimator_so3
 
 SRCS		 = attitude_estimator_so3_main.cpp \
 		   attitude_estimator_so3_params.c
+
+MODULE_STACKSIZE = 1200
diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp
index 17d3d3dcd..13da27dcd 100644
--- a/src/modules/commander/commander.cpp
+++ b/src/modules/commander/commander.cpp
@@ -249,7 +249,7 @@ int commander_main(int argc, char *argv[])
 		daemon_task = task_spawn_cmd("commander",
 					     SCHED_DEFAULT,
 					     SCHED_PRIORITY_MAX - 40,
-					     3000,
+					     2950,
 					     commander_thread_main,
 					     (argv) ? (const char **)&argv[2] : (const char **)NULL);
 
@@ -743,7 +743,7 @@ int commander_thread_main(int argc, char *argv[])
 
 	pthread_attr_t commander_low_prio_attr;
 	pthread_attr_init(&commander_low_prio_attr);
-	pthread_attr_setstacksize(&commander_low_prio_attr, 2992);
+	pthread_attr_setstacksize(&commander_low_prio_attr, 2900);
 
 	struct sched_param param;
 	(void)pthread_attr_getschedparam(&commander_low_prio_attr, &param);
diff --git a/src/modules/commander/commander_helper.cpp b/src/modules/commander/commander_helper.cpp
index 86f77cdb8..940a04aa1 100644
--- a/src/modules/commander/commander_helper.cpp
+++ b/src/modules/commander/commander_helper.cpp
@@ -206,7 +206,7 @@ int led_init()
 		return ERROR;
 	}
 
-	/* the blue LED is only available on FMUv1 but not FMUv2 */
+	/* the blue LED is only available on FMUv1 & AeroCore but not FMUv2 */
 	(void)ioctl(leds, LED_ON, LED_BLUE);
 
 	/* we consider the amber led mandatory */
diff --git a/src/modules/commander/mag_calibration.cpp b/src/modules/commander/mag_calibration.cpp
index 9296db6ed..0ead22f77 100644
--- a/src/modules/commander/mag_calibration.cpp
+++ b/src/modules/commander/mag_calibration.cpp
@@ -72,7 +72,7 @@ int do_mag_calibration(int mavlink_fd)
 	uint64_t calibration_interval = 45 * 1000 * 1000;
 
 	/* maximum 500 values */
-	const unsigned int calibration_maxcount = 500;
+	const unsigned int calibration_maxcount = 240;
 	unsigned int calibration_counter;
 
 	struct mag_scale mscale_null = {
@@ -121,6 +121,20 @@ int do_mag_calibration(int mavlink_fd)
 
 		if (x == NULL || y == NULL || z == NULL) {
 			mavlink_log_critical(mavlink_fd, "ERROR: out of memory");
+
+			/* clean up */
+			if (x != NULL) {
+				free(x);
+			}
+
+			if (y != NULL) {
+				free(y);
+			}
+
+			if (z != NULL) {
+				free(z);
+			}
+
 			res = ERROR;
 			return res;
 		}
diff --git a/src/modules/commander/module.mk b/src/modules/commander/module.mk
index 554dfcb08..27ca5c182 100644
--- a/src/modules/commander/module.mk
+++ b/src/modules/commander/module.mk
@@ -47,3 +47,7 @@ SRCS		 	= commander.cpp \
 			baro_calibration.cpp \
 			rc_calibration.cpp \
 			airspeed_calibration.cpp
+
+MODULE_STACKSIZE = 1200
+
+MAXOPTIMIZATION	 = -Os
diff --git a/src/modules/dataman/dataman.c b/src/modules/dataman/dataman.c
index 7505ba358..1a65313e8 100644
--- a/src/modules/dataman/dataman.c
+++ b/src/modules/dataman/dataman.c
@@ -1,10 +1,6 @@
 /****************************************************************************
  *
- *   Copyright (c) 2013 PX4 Development Team. All rights reserved.
- *   Author: 	Jean Cyr
- *              Lorenz Meier
- *              Julian Oes
- *              Thomas Gubler
+ *   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
@@ -37,6 +33,11 @@
 /**
  * @file dataman.c
  * DATAMANAGER driver.
+ *
+ * @author Jean Cyr
+ * @author Lorenz Meier
+ * @author Julian Oes
+ * @author Thomas Gubler
  */
 
 #include <nuttx/config.h>
@@ -62,7 +63,7 @@ __EXPORT ssize_t dm_write(dm_item_t  item, unsigned char index, dm_persitence_t
 __EXPORT int dm_clear(dm_item_t item);
 __EXPORT int dm_restart(dm_reset_reason restart_type);
 
-/* Types of function calls supported by the worker task */
+/** Types of function calls supported by the worker task */
 typedef enum {
 	dm_write_func = 0,
 	dm_read_func,
@@ -71,11 +72,12 @@ typedef enum {
 	dm_number_of_funcs
 } dm_function_t;
 
-/* Work task work item */
+/** Work task work item */
 typedef struct {
 	sq_entry_t link;	/**< list linkage */
 	sem_t wait_sem;
-	dm_function_t func;
+	unsigned char first;
+	unsigned char func;
 	ssize_t result;
 	union {
 		struct {
@@ -100,6 +102,8 @@ typedef struct {
 	};
 } work_q_item_t;
 
+const size_t k_work_item_allocation_chunk_size = 8;
+
 /* Usage statistics */
 static unsigned g_func_counts[dm_number_of_funcs];
 
@@ -177,9 +181,23 @@ create_work_item(void)
 
 	unlock_queue(&g_free_q);
 
-	/* If we there weren't any free items then obtain memory for a new one */
-	if (item == NULL)
-		item = (work_q_item_t *)malloc(sizeof(work_q_item_t));
+	/* If we there weren't any free items then obtain memory for a new ones */
+	if (item == NULL) {
+		item = (work_q_item_t *)malloc(k_work_item_allocation_chunk_size * sizeof(work_q_item_t));
+		if (item) {
+			item->first = 1;
+			lock_queue(&g_free_q);
+			for (int i = 1; i < k_work_item_allocation_chunk_size; i++) {
+				(item + i)->first = 0;
+				sq_addfirst(&(item + i)->link, &(g_free_q.q));
+			}
+			/* Update the queue size and potentially the maximum queue size */
+			g_free_q.size += k_work_item_allocation_chunk_size - 1;
+			if (g_free_q.size > g_free_q.max_size)
+				g_free_q.max_size = g_free_q.size;
+			unlock_queue(&g_free_q);
+		}
+	}
 
 	/* If we got one then lock the item*/
 	if (item)
@@ -411,7 +429,7 @@ _clear(dm_item_t item)
 	return result;
 }
 
-/* Tell the data manager about the type of the last reset */
+/** Tell the data manager about the type of the last reset */
 static int
 _restart(dm_reset_reason reason)
 {
@@ -480,7 +498,7 @@ _restart(dm_reset_reason reason)
 	return result;
 }
 
-/* write to the data manager file */
+/** Write to the data manager file */
 __EXPORT ssize_t
 dm_write(dm_item_t item, unsigned char index, dm_persitence_t persistence, const void *buf, size_t count)
 {
@@ -505,7 +523,7 @@ dm_write(dm_item_t item, unsigned char index, dm_persitence_t persistence, const
 	return (ssize_t)enqueue_work_item_and_wait_for_result(work);
 }
 
-/* Retrieve from the data manager file */
+/** Retrieve from the data manager file */
 __EXPORT ssize_t
 dm_read(dm_item_t item, unsigned char index, void *buf, size_t count)
 {
@@ -717,8 +735,8 @@ task_main(int argc, char *argv[])
 	for (;;) {
 		if ((work = (work_q_item_t *)sq_remfirst(&(g_free_q.q))) == NULL)
 			break;
-
-		free(work);
+		if (work->first)
+			free(work);
 	}
 
 	destroy_q(&g_work_q);
@@ -736,7 +754,7 @@ start(void)
 	sem_init(&g_init_sema, 1, 0);
 
 	/* start the worker thread */
-	if ((task = task_spawn_cmd("dataman", SCHED_DEFAULT, SCHED_PRIORITY_MAX - 5, 2048, task_main, NULL)) <= 0) {
+	if ((task = task_spawn_cmd("dataman", SCHED_DEFAULT, SCHED_PRIORITY_MAX - 5, 2000, task_main, NULL)) <= 0) {
 		warn("task start failed");
 		return -1;
 	}
diff --git a/src/modules/dataman/dataman.h b/src/modules/dataman/dataman.h
index 4382baeb5..1dfb26f73 100644
--- a/src/modules/dataman/dataman.h
+++ b/src/modules/dataman/dataman.h
@@ -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
@@ -46,7 +46,7 @@
 extern "C" {
 #endif
 
-	/* Types of items that the data manager can store */
+	/** Types of items that the data manager can store */
 	typedef enum {
 		DM_KEY_SAFE_POINTS = 0,		/* Safe points coordinates, safe point 0 is home point */
 		DM_KEY_FENCE_POINTS,		/* Fence vertex coordinates */
@@ -56,7 +56,7 @@ extern "C" {
 		DM_KEY_NUM_KEYS			/* Total number of item types defined */
 	} dm_item_t;
 
-	/* The maximum number of instances for each item type */
+	/** The maximum number of instances for each item type */
 	enum {
 		DM_KEY_SAFE_POINTS_MAX = 8,
 		DM_KEY_FENCE_POINTS_MAX = GEOFENCE_MAX_VERTICES,
@@ -65,24 +65,24 @@ extern "C" {
 		DM_KEY_WAYPOINTS_ONBOARD_MAX = NUM_MISSIONS_SUPPORTED
 	};
 
-	/* Data persistence levels */
+	/** Data persistence levels */
 	typedef enum {
 		DM_PERSIST_POWER_ON_RESET = 0,	/* Data survives all resets */
 		DM_PERSIST_IN_FLIGHT_RESET,     /* Data survives in-flight resets only */
 		DM_PERSIST_VOLATILE             /* Data does not survive resets */
 	} dm_persitence_t;
 
-	/* The reason for the last reset */
+	/** The reason for the last reset */
 	typedef enum {
 		DM_INIT_REASON_POWER_ON = 0,	/* Data survives resets */
 		DM_INIT_REASON_IN_FLIGHT,		/* Data survives in-flight resets only */
 		DM_INIT_REASON_VOLATILE			/* Data does not survive reset */
 	} dm_reset_reason;
 
-	/* Maximum size in bytes of a single item instance */
+	/** Maximum size in bytes of a single item instance */
 	#define DM_MAX_DATA_SIZE 124
 
-	/* Retrieve from the data manager store */
+	/** Retrieve from the data manager store */
 	__EXPORT ssize_t
 	dm_read(
 		dm_item_t item,			/* The item type to retrieve */
@@ -91,7 +91,7 @@ extern "C" {
 		size_t buflen			/* Length in bytes of data to retrieve */
 	);
 
-	/* write to the data manager store */
+	/** write to the data manager store */
 	__EXPORT ssize_t
 	dm_write(
 		dm_item_t  item,		/* The item type to store */
@@ -101,13 +101,13 @@ extern "C" {
 		size_t buflen			/* Length in bytes of data to retrieve */
 	);
 
-	/* Erase all items of this type */
+	/** Erase all items of this type */
 	__EXPORT int
 	dm_clear(
 		dm_item_t item			/* The item type to clear */
 		);
 
-	/* Tell the data manager about the type of the last reset */
+	/** Tell the data manager about the type of the last reset */
 	__EXPORT int
 	dm_restart(
 		dm_reset_reason restart_type	/* The last reset type */
diff --git a/src/modules/dataman/module.mk b/src/modules/dataman/module.mk
index 27670dd3f..234607b3d 100644
--- a/src/modules/dataman/module.mk
+++ b/src/modules/dataman/module.mk
@@ -38,3 +38,5 @@
 MODULE_COMMAND	= dataman
 
 SRCS		= dataman.c
+
+MODULE_STACKSIZE = 1200
diff --git a/src/modules/ekf_att_pos_estimator/estimator.cpp b/src/modules/ekf_att_pos_estimator/estimator.cpp
new file mode 100644
index 000000000..23ecd89ac
--- /dev/null
+++ b/src/modules/ekf_att_pos_estimator/estimator.cpp
@@ -0,0 +1,2609 @@
+#include "estimator.h"
+#include <string.h>
+#include <stdarg.h>
+
+// Define EKF_DEBUG here to enable the debug print calls
+// if the macro is not set, these will be completely
+// optimized out by the compiler.
+#define EKF_DEBUG
+
+#ifdef EKF_DEBUG
+#include <stdio.h>
+
+static void
+ekf_debug_print(const char *fmt, va_list args)
+{
+    fprintf(stderr, "%s: ", "[ekf]");
+    vfprintf(stderr, fmt, args);
+
+    fprintf(stderr, "\n");
+}
+
+static void
+ekf_debug(const char *fmt, ...)
+{
+    va_list args;
+
+    va_start(args, fmt);
+    ekf_debug_print(fmt, args);
+}
+
+#else
+
+static void ekf_debug(const char *fmt, ...) { while(0){} }
+#endif
+
+float Vector3f::length(void) const
+{
+    return sqrt(x*x + y*y + z*z);
+}
+
+void Vector3f::zero(void)
+{
+    x = 0.0f;
+    y = 0.0f;
+    z = 0.0f;
+}
+
+Mat3f::Mat3f() {
+    identity();
+}
+
+void Mat3f::identity() {
+    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()
+
+    /* NOTE: DO NOT initialize class members here. Use ZeroVariables()
+     * instead to allow clean in-air re-initialization.
+     */
+{
+
+    ZeroVariables();
+    InitialiseParameters();
+}
+
+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;
+    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 - states[13];
+
+// 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] = cosf(0.5f*rotationMag);
+        float rotScaler = (sinf(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 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;
+    float dvz_b;
+
+    // arrays
+    float processNoise[n_states];
+    float SF[15];
+    float SG[8];
+    float SQ[11];
+    float SPP[8] = {0};
+    float nextP[n_states][n_states];
+
+    // calculate covariance prediction process noise
+    for (uint8_t i= 0; i<=9;  i++) processNoise[i] = 1.0e-9f;
+    for (uint8_t i=10; i<=12; i++) processNoise[i] = dt * dAngBiasSigma;
+    // scale gyro bias noise when on ground to allow for faster bias estimation
+    for (uint8_t i=10; i<=12; i++) processNoise[i] = dt * dAngBiasSigma;
+    processNoise[13] = dVelBiasSigma;
+    for (uint8_t i=14; i<=15; i++) processNoise[i] = dt * windVelSigma;
+    for (uint8_t i=16; i<=18; i++) processNoise[i] = dt * magEarthSigma;
+    for (uint8_t i=19; i<=21; i++) processNoise[i] = dt * magBodySigma;
+    processNoise[22] = dt * sqrtf(sq(states[4]) + sq(states[5])) * gndHgtSigma;
+
+    // square all sigmas
+    for (unsigned i = 0; i < n_states; 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];
+    dvz_b =  states[13];
+    gyroProcessNoise = ConstrainFloat(gyroProcessNoise, 1e-3f, 5e-2f);
+    daxCov = sq(dt*gyroProcessNoise);
+    dayCov = sq(dt*gyroProcessNoise);
+    dazCov = sq(dt*gyroProcessNoise);
+    if (onGround) dazCov = dazCov * sq(yawVarScale);
+    accelProcessNoise = ConstrainFloat(accelProcessNoise, 5e-2, 1.0f);
+    dvxCov = sq(dt*accelProcessNoise);
+    dvyCov = sq(dt*accelProcessNoise);
+    dvzCov = sq(dt*accelProcessNoise);
+
+    // Predicted covariance calculation
+    SF[0] = dvz - dvz_b;
+    SF[1] = 2*q3*SF[0] + 2*dvx*q1 + 2*dvy*q2;
+    SF[2] = 2*dvx*q3 - 2*q1*SF[0] + 2*dvy*q0;
+    SF[3] = 2*q2*SF[0] + 2*dvx*q0 - 2*dvy*q3;
+    SF[4] = day/2 - day_b/2;
+    SF[5] = daz/2 - daz_b/2;
+    SF[6] = dax/2 - dax_b/2;
+    SF[7] = dax_b/2 - dax/2;
+    SF[8] = daz_b/2 - daz/2;
+    SF[9] = day_b/2 - day/2;
+    SF[10] = 2*q0*SF[0];
+    SF[11] = q1/2;
+    SF[12] = q2/2;
+    SF[13] = q3/2;
+    SF[14] = 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[10] + SF[14] - 2*dvx*q2;
+    SPP[1] = 2*q2*SF[0] + 2*dvx*q0 - 2*dvy*q3;
+    SPP[2] = 2*dvx*q3 - 2*q1*SF[0] + 2*dvy*q0;
+    SPP[3] = 2*q0*q1 - 2*q2*q3;
+    SPP[4] = 2*q0*q2 + 2*q1*q3;
+    SPP[5] = sq(q0) - sq(q1) - sq(q2) + sq(q3);
+    SPP[6] = SF[13];
+    SPP[7] = SF[12];
+
+    nextP[0][0] = P[0][0] + P[1][0]*SF[7] + P[2][0]*SF[9] + P[3][0]*SF[8] + P[10][0]*SF[11] + P[11][0]*SPP[7] + P[12][0]*SPP[6] + (daxCov*SQ[10])/4 + SF[7]*(P[0][1] + P[1][1]*SF[7] + P[2][1]*SF[9] + P[3][1]*SF[8] + P[10][1]*SF[11] + P[11][1]*SPP[7] + P[12][1]*SPP[6]) + SF[9]*(P[0][2] + P[1][2]*SF[7] + P[2][2]*SF[9] + P[3][2]*SF[8] + P[10][2]*SF[11] + P[11][2]*SPP[7] + P[12][2]*SPP[6]) + SF[8]*(P[0][3] + P[1][3]*SF[7] + P[2][3]*SF[9] + P[3][3]*SF[8] + P[10][3]*SF[11] + P[11][3]*SPP[7] + P[12][3]*SPP[6]) + SF[11]*(P[0][10] + P[1][10]*SF[7] + P[2][10]*SF[9] + P[3][10]*SF[8] + P[10][10]*SF[11] + P[11][10]*SPP[7] + P[12][10]*SPP[6]) + SPP[7]*(P[0][11] + P[1][11]*SF[7] + P[2][11]*SF[9] + P[3][11]*SF[8] + P[10][11]*SF[11] + P[11][11]*SPP[7] + P[12][11]*SPP[6]) + SPP[6]*(P[0][12] + P[1][12]*SF[7] + P[2][12]*SF[9] + P[3][12]*SF[8] + P[10][12]*SF[11] + P[11][12]*SPP[7] + P[12][12]*SPP[6]) + (dayCov*sq(q2))/4 + (dazCov*sq(q3))/4;
+    nextP[0][1] = P[0][1] + SQ[8] + P[1][1]*SF[7] + P[2][1]*SF[9] + P[3][1]*SF[8] + P[10][1]*SF[11] + P[11][1]*SPP[7] + P[12][1]*SPP[6] + SF[6]*(P[0][0] + P[1][0]*SF[7] + P[2][0]*SF[9] + P[3][0]*SF[8] + P[10][0]*SF[11] + P[11][0]*SPP[7] + P[12][0]*SPP[6]) + SF[5]*(P[0][2] + P[1][2]*SF[7] + P[2][2]*SF[9] + P[3][2]*SF[8] + P[10][2]*SF[11] + P[11][2]*SPP[7] + P[12][2]*SPP[6]) + SF[9]*(P[0][3] + P[1][3]*SF[7] + P[2][3]*SF[9] + P[3][3]*SF[8] + P[10][3]*SF[11] + P[11][3]*SPP[7] + P[12][3]*SPP[6]) + SPP[6]*(P[0][11] + P[1][11]*SF[7] + P[2][11]*SF[9] + P[3][11]*SF[8] + P[10][11]*SF[11] + P[11][11]*SPP[7] + P[12][11]*SPP[6]) - SPP[7]*(P[0][12] + P[1][12]*SF[7] + P[2][12]*SF[9] + P[3][12]*SF[8] + P[10][12]*SF[11] + P[11][12]*SPP[7] + P[12][12]*SPP[6]) - (q0*(P[0][10] + P[1][10]*SF[7] + P[2][10]*SF[9] + P[3][10]*SF[8] + P[10][10]*SF[11] + P[11][10]*SPP[7] + P[12][10]*SPP[6]))/2;
+    nextP[0][2] = P[0][2] + SQ[7] + P[1][2]*SF[7] + P[2][2]*SF[9] + P[3][2]*SF[8] + P[10][2]*SF[11] + P[11][2]*SPP[7] + P[12][2]*SPP[6] + SF[4]*(P[0][0] + P[1][0]*SF[7] + P[2][0]*SF[9] + P[3][0]*SF[8] + P[10][0]*SF[11] + P[11][0]*SPP[7] + P[12][0]*SPP[6]) + SF[8]*(P[0][1] + P[1][1]*SF[7] + P[2][1]*SF[9] + P[3][1]*SF[8] + P[10][1]*SF[11] + P[11][1]*SPP[7] + P[12][1]*SPP[6]) + SF[6]*(P[0][3] + P[1][3]*SF[7] + P[2][3]*SF[9] + P[3][3]*SF[8] + P[10][3]*SF[11] + P[11][3]*SPP[7] + P[12][3]*SPP[6]) + SF[11]*(P[0][12] + P[1][12]*SF[7] + P[2][12]*SF[9] + P[3][12]*SF[8] + P[10][12]*SF[11] + P[11][12]*SPP[7] + P[12][12]*SPP[6]) - SPP[6]*(P[0][10] + P[1][10]*SF[7] + P[2][10]*SF[9] + P[3][10]*SF[8] + P[10][10]*SF[11] + P[11][10]*SPP[7] + P[12][10]*SPP[6]) - (q0*(P[0][11] + P[1][11]*SF[7] + P[2][11]*SF[9] + P[3][11]*SF[8] + P[10][11]*SF[11] + P[11][11]*SPP[7] + P[12][11]*SPP[6]))/2;
+    nextP[0][3] = P[0][3] + SQ[6] + P[1][3]*SF[7] + P[2][3]*SF[9] + P[3][3]*SF[8] + P[10][3]*SF[11] + P[11][3]*SPP[7] + P[12][3]*SPP[6] + SF[5]*(P[0][0] + P[1][0]*SF[7] + P[2][0]*SF[9] + P[3][0]*SF[8] + P[10][0]*SF[11] + P[11][0]*SPP[7] + P[12][0]*SPP[6]) + SF[4]*(P[0][1] + P[1][1]*SF[7] + P[2][1]*SF[9] + P[3][1]*SF[8] + P[10][1]*SF[11] + P[11][1]*SPP[7] + P[12][1]*SPP[6]) + SF[7]*(P[0][2] + P[1][2]*SF[7] + P[2][2]*SF[9] + P[3][2]*SF[8] + P[10][2]*SF[11] + P[11][2]*SPP[7] + P[12][2]*SPP[6]) - SF[11]*(P[0][11] + P[1][11]*SF[7] + P[2][11]*SF[9] + P[3][11]*SF[8] + P[10][11]*SF[11] + P[11][11]*SPP[7] + P[12][11]*SPP[6]) + SPP[7]*(P[0][10] + P[1][10]*SF[7] + P[2][10]*SF[9] + P[3][10]*SF[8] + P[10][10]*SF[11] + P[11][10]*SPP[7] + P[12][10]*SPP[6]) - (q0*(P[0][12] + P[1][12]*SF[7] + P[2][12]*SF[9] + P[3][12]*SF[8] + P[10][12]*SF[11] + P[11][12]*SPP[7] + P[12][12]*SPP[6]))/2;
+    nextP[0][4] = P[0][4] + P[1][4]*SF[7] + P[2][4]*SF[9] + P[3][4]*SF[8] + P[10][4]*SF[11] + P[11][4]*SPP[7] + P[12][4]*SPP[6] + SF[3]*(P[0][0] + P[1][0]*SF[7] + P[2][0]*SF[9] + P[3][0]*SF[8] + P[10][0]*SF[11] + P[11][0]*SPP[7] + P[12][0]*SPP[6]) + SF[1]*(P[0][1] + P[1][1]*SF[7] + P[2][1]*SF[9] + P[3][1]*SF[8] + P[10][1]*SF[11] + P[11][1]*SPP[7] + P[12][1]*SPP[6]) + SPP[0]*(P[0][2] + P[1][2]*SF[7] + P[2][2]*SF[9] + P[3][2]*SF[8] + P[10][2]*SF[11] + P[11][2]*SPP[7] + P[12][2]*SPP[6]) - SPP[2]*(P[0][3] + P[1][3]*SF[7] + P[2][3]*SF[9] + P[3][3]*SF[8] + P[10][3]*SF[11] + P[11][3]*SPP[7] + P[12][3]*SPP[6]) - SPP[4]*(P[0][13] + P[1][13]*SF[7] + P[2][13]*SF[9] + P[3][13]*SF[8] + P[10][13]*SF[11] + P[11][13]*SPP[7] + P[12][13]*SPP[6]);
+    nextP[0][5] = P[0][5] + P[1][5]*SF[7] + P[2][5]*SF[9] + P[3][5]*SF[8] + P[10][5]*SF[11] + P[11][5]*SPP[7] + P[12][5]*SPP[6] + SF[2]*(P[0][0] + P[1][0]*SF[7] + P[2][0]*SF[9] + P[3][0]*SF[8] + P[10][0]*SF[11] + P[11][0]*SPP[7] + P[12][0]*SPP[6]) + SF[1]*(P[0][2] + P[1][2]*SF[7] + P[2][2]*SF[9] + P[3][2]*SF[8] + P[10][2]*SF[11] + P[11][2]*SPP[7] + P[12][2]*SPP[6]) + SF[3]*(P[0][3] + P[1][3]*SF[7] + P[2][3]*SF[9] + P[3][3]*SF[8] + P[10][3]*SF[11] + P[11][3]*SPP[7] + P[12][3]*SPP[6]) - SPP[0]*(P[0][1] + P[1][1]*SF[7] + P[2][1]*SF[9] + P[3][1]*SF[8] + P[10][1]*SF[11] + P[11][1]*SPP[7] + P[12][1]*SPP[6]) + SPP[3]*(P[0][13] + P[1][13]*SF[7] + P[2][13]*SF[9] + P[3][13]*SF[8] + P[10][13]*SF[11] + P[11][13]*SPP[7] + P[12][13]*SPP[6]);
+    nextP[0][6] = P[0][6] + P[1][6]*SF[7] + P[2][6]*SF[9] + P[3][6]*SF[8] + P[10][6]*SF[11] + P[11][6]*SPP[7] + P[12][6]*SPP[6] + SF[2]*(P[0][1] + P[1][1]*SF[7] + P[2][1]*SF[9] + P[3][1]*SF[8] + P[10][1]*SF[11] + P[11][1]*SPP[7] + P[12][1]*SPP[6]) + SF[1]*(P[0][3] + P[1][3]*SF[7] + P[2][3]*SF[9] + P[3][3]*SF[8] + P[10][3]*SF[11] + P[11][3]*SPP[7] + P[12][3]*SPP[6]) + SPP[0]*(P[0][0] + P[1][0]*SF[7] + P[2][0]*SF[9] + P[3][0]*SF[8] + P[10][0]*SF[11] + P[11][0]*SPP[7] + P[12][0]*SPP[6]) - SPP[1]*(P[0][2] + P[1][2]*SF[7] + P[2][2]*SF[9] + P[3][2]*SF[8] + P[10][2]*SF[11] + P[11][2]*SPP[7] + P[12][2]*SPP[6]) - (sq(q0) - sq(q1) - sq(q2) + sq(q3))*(P[0][13] + P[1][13]*SF[7] + P[2][13]*SF[9] + P[3][13]*SF[8] + P[10][13]*SF[11] + P[11][13]*SPP[7] + P[12][13]*SPP[6]);
+    nextP[0][7] = P[0][7] + P[1][7]*SF[7] + P[2][7]*SF[9] + P[3][7]*SF[8] + P[10][7]*SF[11] + P[11][7]*SPP[7] + P[12][7]*SPP[6] + dt*(P[0][4] + P[1][4]*SF[7] + P[2][4]*SF[9] + P[3][4]*SF[8] + P[10][4]*SF[11] + P[11][4]*SPP[7] + P[12][4]*SPP[6]);
+    nextP[0][8] = P[0][8] + P[1][8]*SF[7] + P[2][8]*SF[9] + P[3][8]*SF[8] + P[10][8]*SF[11] + P[11][8]*SPP[7] + P[12][8]*SPP[6] + dt*(P[0][5] + P[1][5]*SF[7] + P[2][5]*SF[9] + P[3][5]*SF[8] + P[10][5]*SF[11] + P[11][5]*SPP[7] + P[12][5]*SPP[6]);
+    nextP[0][9] = P[0][9] + P[1][9]*SF[7] + P[2][9]*SF[9] + P[3][9]*SF[8] + P[10][9]*SF[11] + P[11][9]*SPP[7] + P[12][9]*SPP[6] + dt*(P[0][6] + P[1][6]*SF[7] + P[2][6]*SF[9] + P[3][6]*SF[8] + P[10][6]*SF[11] + P[11][6]*SPP[7] + P[12][6]*SPP[6]);
+    nextP[0][10] = P[0][10] + P[1][10]*SF[7] + P[2][10]*SF[9] + P[3][10]*SF[8] + P[10][10]*SF[11] + P[11][10]*SPP[7] + P[12][10]*SPP[6];
+    nextP[0][11] = P[0][11] + P[1][11]*SF[7] + P[2][11]*SF[9] + P[3][11]*SF[8] + P[10][11]*SF[11] + P[11][11]*SPP[7] + P[12][11]*SPP[6];
+    nextP[0][12] = P[0][12] + P[1][12]*SF[7] + P[2][12]*SF[9] + P[3][12]*SF[8] + P[10][12]*SF[11] + P[11][12]*SPP[7] + P[12][12]*SPP[6];
+    nextP[0][13] = P[0][13] + P[1][13]*SF[7] + P[2][13]*SF[9] + P[3][13]*SF[8] + P[10][13]*SF[11] + P[11][13]*SPP[7] + P[12][13]*SPP[6];
+    nextP[0][14] = P[0][14] + P[1][14]*SF[7] + P[2][14]*SF[9] + P[3][14]*SF[8] + P[10][14]*SF[11] + P[11][14]*SPP[7] + P[12][14]*SPP[6];
+    nextP[0][15] = P[0][15] + P[1][15]*SF[7] + P[2][15]*SF[9] + P[3][15]*SF[8] + P[10][15]*SF[11] + P[11][15]*SPP[7] + P[12][15]*SPP[6];
+    nextP[0][16] = P[0][16] + P[1][16]*SF[7] + P[2][16]*SF[9] + P[3][16]*SF[8] + P[10][16]*SF[11] + P[11][16]*SPP[7] + P[12][16]*SPP[6];
+    nextP[0][17] = P[0][17] + P[1][17]*SF[7] + P[2][17]*SF[9] + P[3][17]*SF[8] + P[10][17]*SF[11] + P[11][17]*SPP[7] + P[12][17]*SPP[6];
+    nextP[0][18] = P[0][18] + P[1][18]*SF[7] + P[2][18]*SF[9] + P[3][18]*SF[8] + P[10][18]*SF[11] + P[11][18]*SPP[7] + P[12][18]*SPP[6];
+    nextP[0][19] = P[0][19] + P[1][19]*SF[7] + P[2][19]*SF[9] + P[3][19]*SF[8] + P[10][19]*SF[11] + P[11][19]*SPP[7] + P[12][19]*SPP[6];
+    nextP[0][20] = P[0][20] + P[1][20]*SF[7] + P[2][20]*SF[9] + P[3][20]*SF[8] + P[10][20]*SF[11] + P[11][20]*SPP[7] + P[12][20]*SPP[6];
+    nextP[0][21] = P[0][21] + P[1][21]*SF[7] + P[2][21]*SF[9] + P[3][21]*SF[8] + P[10][21]*SF[11] + P[11][21]*SPP[7] + P[12][21]*SPP[6];
+    nextP[0][22] = P[0][22] + P[1][22]*SF[7] + P[2][22]*SF[9] + P[3][22]*SF[8] + P[10][22]*SF[11] + P[11][22]*SPP[7] + P[12][22]*SPP[6];
+    nextP[1][0] = P[1][0] + SQ[8] + P[0][0]*SF[6] + P[2][0]*SF[5] + P[3][0]*SF[9] + P[11][0]*SPP[6] - P[12][0]*SPP[7] - (P[10][0]*q0)/2 + SF[7]*(P[1][1] + P[0][1]*SF[6] + P[2][1]*SF[5] + P[3][1]*SF[9] + P[11][1]*SPP[6] - P[12][1]*SPP[7] - (P[10][1]*q0)/2) + SF[9]*(P[1][2] + P[0][2]*SF[6] + P[2][2]*SF[5] + P[3][2]*SF[9] + P[11][2]*SPP[6] - P[12][2]*SPP[7] - (P[10][2]*q0)/2) + SF[8]*(P[1][3] + P[0][3]*SF[6] + P[2][3]*SF[5] + P[3][3]*SF[9] + P[11][3]*SPP[6] - P[12][3]*SPP[7] - (P[10][3]*q0)/2) + SF[11]*(P[1][10] + P[0][10]*SF[6] + P[2][10]*SF[5] + P[3][10]*SF[9] + P[11][10]*SPP[6] - P[12][10]*SPP[7] - (P[10][10]*q0)/2) + SPP[7]*(P[1][11] + P[0][11]*SF[6] + P[2][11]*SF[5] + P[3][11]*SF[9] + P[11][11]*SPP[6] - P[12][11]*SPP[7] - (P[10][11]*q0)/2) + SPP[6]*(P[1][12] + P[0][12]*SF[6] + P[2][12]*SF[5] + P[3][12]*SF[9] + P[11][12]*SPP[6] - P[12][12]*SPP[7] - (P[10][12]*q0)/2);
+    nextP[1][1] = P[1][1] + P[0][1]*SF[6] + P[2][1]*SF[5] + P[3][1]*SF[9] + P[11][1]*SPP[6] - P[12][1]*SPP[7] + daxCov*SQ[9] - (P[10][1]*q0)/2 + SF[6]*(P[1][0] + P[0][0]*SF[6] + P[2][0]*SF[5] + P[3][0]*SF[9] + P[11][0]*SPP[6] - P[12][0]*SPP[7] - (P[10][0]*q0)/2) + SF[5]*(P[1][2] + P[0][2]*SF[6] + P[2][2]*SF[5] + P[3][2]*SF[9] + P[11][2]*SPP[6] - P[12][2]*SPP[7] - (P[10][2]*q0)/2) + SF[9]*(P[1][3] + P[0][3]*SF[6] + P[2][3]*SF[5] + P[3][3]*SF[9] + P[11][3]*SPP[6] - P[12][3]*SPP[7] - (P[10][3]*q0)/2) + SPP[6]*(P[1][11] + P[0][11]*SF[6] + P[2][11]*SF[5] + P[3][11]*SF[9] + P[11][11]*SPP[6] - P[12][11]*SPP[7] - (P[10][11]*q0)/2) - SPP[7]*(P[1][12] + P[0][12]*SF[6] + P[2][12]*SF[5] + P[3][12]*SF[9] + P[11][12]*SPP[6] - P[12][12]*SPP[7] - (P[10][12]*q0)/2) + (dayCov*sq(q3))/4 + (dazCov*sq(q2))/4 - (q0*(P[1][10] + P[0][10]*SF[6] + P[2][10]*SF[5] + P[3][10]*SF[9] + P[11][10]*SPP[6] - P[12][10]*SPP[7] - (P[10][10]*q0)/2))/2;
+    nextP[1][2] = P[1][2] + SQ[5] + P[0][2]*SF[6] + P[2][2]*SF[5] + P[3][2]*SF[9] + P[11][2]*SPP[6] - P[12][2]*SPP[7] - (P[10][2]*q0)/2 + SF[4]*(P[1][0] + P[0][0]*SF[6] + P[2][0]*SF[5] + P[3][0]*SF[9] + P[11][0]*SPP[6] - P[12][0]*SPP[7] - (P[10][0]*q0)/2) + SF[8]*(P[1][1] + P[0][1]*SF[6] + P[2][1]*SF[5] + P[3][1]*SF[9] + P[11][1]*SPP[6] - P[12][1]*SPP[7] - (P[10][1]*q0)/2) + SF[6]*(P[1][3] + P[0][3]*SF[6] + P[2][3]*SF[5] + P[3][3]*SF[9] + P[11][3]*SPP[6] - P[12][3]*SPP[7] - (P[10][3]*q0)/2) + SF[11]*(P[1][12] + P[0][12]*SF[6] + P[2][12]*SF[5] + P[3][12]*SF[9] + P[11][12]*SPP[6] - P[12][12]*SPP[7] - (P[10][12]*q0)/2) - SPP[6]*(P[1][10] + P[0][10]*SF[6] + P[2][10]*SF[5] + P[3][10]*SF[9] + P[11][10]*SPP[6] - P[12][10]*SPP[7] - (P[10][10]*q0)/2) - (q0*(P[1][11] + P[0][11]*SF[6] + P[2][11]*SF[5] + P[3][11]*SF[9] + P[11][11]*SPP[6] - P[12][11]*SPP[7] - (P[10][11]*q0)/2))/2;
+    nextP[1][3] = P[1][3] + SQ[4] + P[0][3]*SF[6] + P[2][3]*SF[5] + P[3][3]*SF[9] + P[11][3]*SPP[6] - P[12][3]*SPP[7] - (P[10][3]*q0)/2 + SF[5]*(P[1][0] + P[0][0]*SF[6] + P[2][0]*SF[5] + P[3][0]*SF[9] + P[11][0]*SPP[6] - P[12][0]*SPP[7] - (P[10][0]*q0)/2) + SF[4]*(P[1][1] + P[0][1]*SF[6] + P[2][1]*SF[5] + P[3][1]*SF[9] + P[11][1]*SPP[6] - P[12][1]*SPP[7] - (P[10][1]*q0)/2) + SF[7]*(P[1][2] + P[0][2]*SF[6] + P[2][2]*SF[5] + P[3][2]*SF[9] + P[11][2]*SPP[6] - P[12][2]*SPP[7] - (P[10][2]*q0)/2) - SF[11]*(P[1][11] + P[0][11]*SF[6] + P[2][11]*SF[5] + P[3][11]*SF[9] + P[11][11]*SPP[6] - P[12][11]*SPP[7] - (P[10][11]*q0)/2) + SPP[7]*(P[1][10] + P[0][10]*SF[6] + P[2][10]*SF[5] + P[3][10]*SF[9] + P[11][10]*SPP[6] - P[12][10]*SPP[7] - (P[10][10]*q0)/2) - (q0*(P[1][12] + P[0][12]*SF[6] + P[2][12]*SF[5] + P[3][12]*SF[9] + P[11][12]*SPP[6] - P[12][12]*SPP[7] - (P[10][12]*q0)/2))/2;
+    nextP[1][4] = P[1][4] + P[0][4]*SF[6] + P[2][4]*SF[5] + P[3][4]*SF[9] + P[11][4]*SPP[6] - P[12][4]*SPP[7] - (P[10][4]*q0)/2 + SF[3]*(P[1][0] + P[0][0]*SF[6] + P[2][0]*SF[5] + P[3][0]*SF[9] + P[11][0]*SPP[6] - P[12][0]*SPP[7] - (P[10][0]*q0)/2) + SF[1]*(P[1][1] + P[0][1]*SF[6] + P[2][1]*SF[5] + P[3][1]*SF[9] + P[11][1]*SPP[6] - P[12][1]*SPP[7] - (P[10][1]*q0)/2) + SPP[0]*(P[1][2] + P[0][2]*SF[6] + P[2][2]*SF[5] + P[3][2]*SF[9] + P[11][2]*SPP[6] - P[12][2]*SPP[7] - (P[10][2]*q0)/2) - SPP[2]*(P[1][3] + P[0][3]*SF[6] + P[2][3]*SF[5] + P[3][3]*SF[9] + P[11][3]*SPP[6] - P[12][3]*SPP[7] - (P[10][3]*q0)/2) - SPP[4]*(P[1][13] + P[0][13]*SF[6] + P[2][13]*SF[5] + P[3][13]*SF[9] + P[11][13]*SPP[6] - P[12][13]*SPP[7] - (P[10][13]*q0)/2);
+    nextP[1][5] = P[1][5] + P[0][5]*SF[6] + P[2][5]*SF[5] + P[3][5]*SF[9] + P[11][5]*SPP[6] - P[12][5]*SPP[7] - (P[10][5]*q0)/2 + SF[2]*(P[1][0] + P[0][0]*SF[6] + P[2][0]*SF[5] + P[3][0]*SF[9] + P[11][0]*SPP[6] - P[12][0]*SPP[7] - (P[10][0]*q0)/2) + SF[1]*(P[1][2] + P[0][2]*SF[6] + P[2][2]*SF[5] + P[3][2]*SF[9] + P[11][2]*SPP[6] - P[12][2]*SPP[7] - (P[10][2]*q0)/2) + SF[3]*(P[1][3] + P[0][3]*SF[6] + P[2][3]*SF[5] + P[3][3]*SF[9] + P[11][3]*SPP[6] - P[12][3]*SPP[7] - (P[10][3]*q0)/2) - SPP[0]*(P[1][1] + P[0][1]*SF[6] + P[2][1]*SF[5] + P[3][1]*SF[9] + P[11][1]*SPP[6] - P[12][1]*SPP[7] - (P[10][1]*q0)/2) + SPP[3]*(P[1][13] + P[0][13]*SF[6] + P[2][13]*SF[5] + P[3][13]*SF[9] + P[11][13]*SPP[6] - P[12][13]*SPP[7] - (P[10][13]*q0)/2);
+    nextP[1][6] = P[1][6] + P[0][6]*SF[6] + P[2][6]*SF[5] + P[3][6]*SF[9] + P[11][6]*SPP[6] - P[12][6]*SPP[7] - (P[10][6]*q0)/2 + SF[2]*(P[1][1] + P[0][1]*SF[6] + P[2][1]*SF[5] + P[3][1]*SF[9] + P[11][1]*SPP[6] - P[12][1]*SPP[7] - (P[10][1]*q0)/2) + SF[1]*(P[1][3] + P[0][3]*SF[6] + P[2][3]*SF[5] + P[3][3]*SF[9] + P[11][3]*SPP[6] - P[12][3]*SPP[7] - (P[10][3]*q0)/2) + SPP[0]*(P[1][0] + P[0][0]*SF[6] + P[2][0]*SF[5] + P[3][0]*SF[9] + P[11][0]*SPP[6] - P[12][0]*SPP[7] - (P[10][0]*q0)/2) - SPP[1]*(P[1][2] + P[0][2]*SF[6] + P[2][2]*SF[5] + P[3][2]*SF[9] + P[11][2]*SPP[6] - P[12][2]*SPP[7] - (P[10][2]*q0)/2) - (sq(q0) - sq(q1) - sq(q2) + sq(q3))*(P[1][13] + P[0][13]*SF[6] + P[2][13]*SF[5] + P[3][13]*SF[9] + P[11][13]*SPP[6] - P[12][13]*SPP[7] - (P[10][13]*q0)/2);
+    nextP[1][7] = P[1][7] + P[0][7]*SF[6] + P[2][7]*SF[5] + P[3][7]*SF[9] + P[11][7]*SPP[6] - P[12][7]*SPP[7] - (P[10][7]*q0)/2 + dt*(P[1][4] + P[0][4]*SF[6] + P[2][4]*SF[5] + P[3][4]*SF[9] + P[11][4]*SPP[6] - P[12][4]*SPP[7] - (P[10][4]*q0)/2);
+    nextP[1][8] = P[1][8] + P[0][8]*SF[6] + P[2][8]*SF[5] + P[3][8]*SF[9] + P[11][8]*SPP[6] - P[12][8]*SPP[7] - (P[10][8]*q0)/2 + dt*(P[1][5] + P[0][5]*SF[6] + P[2][5]*SF[5] + P[3][5]*SF[9] + P[11][5]*SPP[6] - P[12][5]*SPP[7] - (P[10][5]*q0)/2);
+    nextP[1][9] = P[1][9] + P[0][9]*SF[6] + P[2][9]*SF[5] + P[3][9]*SF[9] + P[11][9]*SPP[6] - P[12][9]*SPP[7] - (P[10][9]*q0)/2 + dt*(P[1][6] + P[0][6]*SF[6] + P[2][6]*SF[5] + P[3][6]*SF[9] + P[11][6]*SPP[6] - P[12][6]*SPP[7] - (P[10][6]*q0)/2);
+    nextP[1][10] = P[1][10] + P[0][10]*SF[6] + P[2][10]*SF[5] + P[3][10]*SF[9] + P[11][10]*SPP[6] - P[12][10]*SPP[7] - (P[10][10]*q0)/2;
+    nextP[1][11] = P[1][11] + P[0][11]*SF[6] + P[2][11]*SF[5] + P[3][11]*SF[9] + P[11][11]*SPP[6] - P[12][11]*SPP[7] - (P[10][11]*q0)/2;
+    nextP[1][12] = P[1][12] + P[0][12]*SF[6] + P[2][12]*SF[5] + P[3][12]*SF[9] + P[11][12]*SPP[6] - P[12][12]*SPP[7] - (P[10][12]*q0)/2;
+    nextP[1][13] = P[1][13] + P[0][13]*SF[6] + P[2][13]*SF[5] + P[3][13]*SF[9] + P[11][13]*SPP[6] - P[12][13]*SPP[7] - (P[10][13]*q0)/2;
+    nextP[1][14] = P[1][14] + P[0][14]*SF[6] + P[2][14]*SF[5] + P[3][14]*SF[9] + P[11][14]*SPP[6] - P[12][14]*SPP[7] - (P[10][14]*q0)/2;
+    nextP[1][15] = P[1][15] + P[0][15]*SF[6] + P[2][15]*SF[5] + P[3][15]*SF[9] + P[11][15]*SPP[6] - P[12][15]*SPP[7] - (P[10][15]*q0)/2;
+    nextP[1][16] = P[1][16] + P[0][16]*SF[6] + P[2][16]*SF[5] + P[3][16]*SF[9] + P[11][16]*SPP[6] - P[12][16]*SPP[7] - (P[10][16]*q0)/2;
+    nextP[1][17] = P[1][17] + P[0][17]*SF[6] + P[2][17]*SF[5] + P[3][17]*SF[9] + P[11][17]*SPP[6] - P[12][17]*SPP[7] - (P[10][17]*q0)/2;
+    nextP[1][18] = P[1][18] + P[0][18]*SF[6] + P[2][18]*SF[5] + P[3][18]*SF[9] + P[11][18]*SPP[6] - P[12][18]*SPP[7] - (P[10][18]*q0)/2;
+    nextP[1][19] = P[1][19] + P[0][19]*SF[6] + P[2][19]*SF[5] + P[3][19]*SF[9] + P[11][19]*SPP[6] - P[12][19]*SPP[7] - (P[10][19]*q0)/2;
+    nextP[1][20] = P[1][20] + P[0][20]*SF[6] + P[2][20]*SF[5] + P[3][20]*SF[9] + P[11][20]*SPP[6] - P[12][20]*SPP[7] - (P[10][20]*q0)/2;
+    nextP[1][21] = P[1][21] + P[0][21]*SF[6] + P[2][21]*SF[5] + P[3][21]*SF[9] + P[11][21]*SPP[6] - P[12][21]*SPP[7] - (P[10][21]*q0)/2;
+    nextP[1][22] = P[1][22] + P[0][22]*SF[6] + P[2][22]*SF[5] + P[3][22]*SF[9] + P[11][22]*SPP[6] - P[12][22]*SPP[7] - (P[10][22]*q0)/2;
+    nextP[2][0] = P[2][0] + SQ[7] + P[0][0]*SF[4] + P[1][0]*SF[8] + P[3][0]*SF[6] + P[12][0]*SF[11] - P[10][0]*SPP[6] - (P[11][0]*q0)/2 + SF[7]*(P[2][1] + P[0][1]*SF[4] + P[1][1]*SF[8] + P[3][1]*SF[6] + P[12][1]*SF[11] - P[10][1]*SPP[6] - (P[11][1]*q0)/2) + SF[9]*(P[2][2] + P[0][2]*SF[4] + P[1][2]*SF[8] + P[3][2]*SF[6] + P[12][2]*SF[11] - P[10][2]*SPP[6] - (P[11][2]*q0)/2) + SF[8]*(P[2][3] + P[0][3]*SF[4] + P[1][3]*SF[8] + P[3][3]*SF[6] + P[12][3]*SF[11] - P[10][3]*SPP[6] - (P[11][3]*q0)/2) + SF[11]*(P[2][10] + P[0][10]*SF[4] + P[1][10]*SF[8] + P[3][10]*SF[6] + P[12][10]*SF[11] - P[10][10]*SPP[6] - (P[11][10]*q0)/2) + SPP[7]*(P[2][11] + P[0][11]*SF[4] + P[1][11]*SF[8] + P[3][11]*SF[6] + P[12][11]*SF[11] - P[10][11]*SPP[6] - (P[11][11]*q0)/2) + SPP[6]*(P[2][12] + P[0][12]*SF[4] + P[1][12]*SF[8] + P[3][12]*SF[6] + P[12][12]*SF[11] - P[10][12]*SPP[6] - (P[11][12]*q0)/2);
+    nextP[2][1] = P[2][1] + SQ[5] + P[0][1]*SF[4] + P[1][1]*SF[8] + P[3][1]*SF[6] + P[12][1]*SF[11] - P[10][1]*SPP[6] - (P[11][1]*q0)/2 + SF[6]*(P[2][0] + P[0][0]*SF[4] + P[1][0]*SF[8] + P[3][0]*SF[6] + P[12][0]*SF[11] - P[10][0]*SPP[6] - (P[11][0]*q0)/2) + SF[5]*(P[2][2] + P[0][2]*SF[4] + P[1][2]*SF[8] + P[3][2]*SF[6] + P[12][2]*SF[11] - P[10][2]*SPP[6] - (P[11][2]*q0)/2) + SF[9]*(P[2][3] + P[0][3]*SF[4] + P[1][3]*SF[8] + P[3][3]*SF[6] + P[12][3]*SF[11] - P[10][3]*SPP[6] - (P[11][3]*q0)/2) + SPP[6]*(P[2][11] + P[0][11]*SF[4] + P[1][11]*SF[8] + P[3][11]*SF[6] + P[12][11]*SF[11] - P[10][11]*SPP[6] - (P[11][11]*q0)/2) - SPP[7]*(P[2][12] + P[0][12]*SF[4] + P[1][12]*SF[8] + P[3][12]*SF[6] + P[12][12]*SF[11] - P[10][12]*SPP[6] - (P[11][12]*q0)/2) - (q0*(P[2][10] + P[0][10]*SF[4] + P[1][10]*SF[8] + P[3][10]*SF[6] + P[12][10]*SF[11] - P[10][10]*SPP[6] - (P[11][10]*q0)/2))/2;
+    nextP[2][2] = P[2][2] + P[0][2]*SF[4] + P[1][2]*SF[8] + P[3][2]*SF[6] + P[12][2]*SF[11] - P[10][2]*SPP[6] + dayCov*SQ[9] + (dazCov*SQ[10])/4 - (P[11][2]*q0)/2 + SF[4]*(P[2][0] + P[0][0]*SF[4] + P[1][0]*SF[8] + P[3][0]*SF[6] + P[12][0]*SF[11] - P[10][0]*SPP[6] - (P[11][0]*q0)/2) + SF[8]*(P[2][1] + P[0][1]*SF[4] + P[1][1]*SF[8] + P[3][1]*SF[6] + P[12][1]*SF[11] - P[10][1]*SPP[6] - (P[11][1]*q0)/2) + SF[6]*(P[2][3] + P[0][3]*SF[4] + P[1][3]*SF[8] + P[3][3]*SF[6] + P[12][3]*SF[11] - P[10][3]*SPP[6] - (P[11][3]*q0)/2) + SF[11]*(P[2][12] + P[0][12]*SF[4] + P[1][12]*SF[8] + P[3][12]*SF[6] + P[12][12]*SF[11] - P[10][12]*SPP[6] - (P[11][12]*q0)/2) - SPP[6]*(P[2][10] + P[0][10]*SF[4] + P[1][10]*SF[8] + P[3][10]*SF[6] + P[12][10]*SF[11] - P[10][10]*SPP[6] - (P[11][10]*q0)/2) + (daxCov*sq(q3))/4 - (q0*(P[2][11] + P[0][11]*SF[4] + P[1][11]*SF[8] + P[3][11]*SF[6] + P[12][11]*SF[11] - P[10][11]*SPP[6] - (P[11][11]*q0)/2))/2;
+    nextP[2][3] = P[2][3] + SQ[3] + P[0][3]*SF[4] + P[1][3]*SF[8] + P[3][3]*SF[6] + P[12][3]*SF[11] - P[10][3]*SPP[6] - (P[11][3]*q0)/2 + SF[5]*(P[2][0] + P[0][0]*SF[4] + P[1][0]*SF[8] + P[3][0]*SF[6] + P[12][0]*SF[11] - P[10][0]*SPP[6] - (P[11][0]*q0)/2) + SF[4]*(P[2][1] + P[0][1]*SF[4] + P[1][1]*SF[8] + P[3][1]*SF[6] + P[12][1]*SF[11] - P[10][1]*SPP[6] - (P[11][1]*q0)/2) + SF[7]*(P[2][2] + P[0][2]*SF[4] + P[1][2]*SF[8] + P[3][2]*SF[6] + P[12][2]*SF[11] - P[10][2]*SPP[6] - (P[11][2]*q0)/2) - SF[11]*(P[2][11] + P[0][11]*SF[4] + P[1][11]*SF[8] + P[3][11]*SF[6] + P[12][11]*SF[11] - P[10][11]*SPP[6] - (P[11][11]*q0)/2) + SPP[7]*(P[2][10] + P[0][10]*SF[4] + P[1][10]*SF[8] + P[3][10]*SF[6] + P[12][10]*SF[11] - P[10][10]*SPP[6] - (P[11][10]*q0)/2) - (q0*(P[2][12] + P[0][12]*SF[4] + P[1][12]*SF[8] + P[3][12]*SF[6] + P[12][12]*SF[11] - P[10][12]*SPP[6] - (P[11][12]*q0)/2))/2;
+    nextP[2][4] = P[2][4] + P[0][4]*SF[4] + P[1][4]*SF[8] + P[3][4]*SF[6] + P[12][4]*SF[11] - P[10][4]*SPP[6] - (P[11][4]*q0)/2 + SF[3]*(P[2][0] + P[0][0]*SF[4] + P[1][0]*SF[8] + P[3][0]*SF[6] + P[12][0]*SF[11] - P[10][0]*SPP[6] - (P[11][0]*q0)/2) + SF[1]*(P[2][1] + P[0][1]*SF[4] + P[1][1]*SF[8] + P[3][1]*SF[6] + P[12][1]*SF[11] - P[10][1]*SPP[6] - (P[11][1]*q0)/2) + SPP[0]*(P[2][2] + P[0][2]*SF[4] + P[1][2]*SF[8] + P[3][2]*SF[6] + P[12][2]*SF[11] - P[10][2]*SPP[6] - (P[11][2]*q0)/2) - SPP[2]*(P[2][3] + P[0][3]*SF[4] + P[1][3]*SF[8] + P[3][3]*SF[6] + P[12][3]*SF[11] - P[10][3]*SPP[6] - (P[11][3]*q0)/2) - SPP[4]*(P[2][13] + P[0][13]*SF[4] + P[1][13]*SF[8] + P[3][13]*SF[6] + P[12][13]*SF[11] - P[10][13]*SPP[6] - (P[11][13]*q0)/2);
+    nextP[2][5] = P[2][5] + P[0][5]*SF[4] + P[1][5]*SF[8] + P[3][5]*SF[6] + P[12][5]*SF[11] - P[10][5]*SPP[6] - (P[11][5]*q0)/2 + SF[2]*(P[2][0] + P[0][0]*SF[4] + P[1][0]*SF[8] + P[3][0]*SF[6] + P[12][0]*SF[11] - P[10][0]*SPP[6] - (P[11][0]*q0)/2) + SF[1]*(P[2][2] + P[0][2]*SF[4] + P[1][2]*SF[8] + P[3][2]*SF[6] + P[12][2]*SF[11] - P[10][2]*SPP[6] - (P[11][2]*q0)/2) + SF[3]*(P[2][3] + P[0][3]*SF[4] + P[1][3]*SF[8] + P[3][3]*SF[6] + P[12][3]*SF[11] - P[10][3]*SPP[6] - (P[11][3]*q0)/2) - SPP[0]*(P[2][1] + P[0][1]*SF[4] + P[1][1]*SF[8] + P[3][1]*SF[6] + P[12][1]*SF[11] - P[10][1]*SPP[6] - (P[11][1]*q0)/2) + SPP[3]*(P[2][13] + P[0][13]*SF[4] + P[1][13]*SF[8] + P[3][13]*SF[6] + P[12][13]*SF[11] - P[10][13]*SPP[6] - (P[11][13]*q0)/2);
+    nextP[2][6] = P[2][6] + P[0][6]*SF[4] + P[1][6]*SF[8] + P[3][6]*SF[6] + P[12][6]*SF[11] - P[10][6]*SPP[6] - (P[11][6]*q0)/2 + SF[2]*(P[2][1] + P[0][1]*SF[4] + P[1][1]*SF[8] + P[3][1]*SF[6] + P[12][1]*SF[11] - P[10][1]*SPP[6] - (P[11][1]*q0)/2) + SF[1]*(P[2][3] + P[0][3]*SF[4] + P[1][3]*SF[8] + P[3][3]*SF[6] + P[12][3]*SF[11] - P[10][3]*SPP[6] - (P[11][3]*q0)/2) + SPP[0]*(P[2][0] + P[0][0]*SF[4] + P[1][0]*SF[8] + P[3][0]*SF[6] + P[12][0]*SF[11] - P[10][0]*SPP[6] - (P[11][0]*q0)/2) - SPP[1]*(P[2][2] + P[0][2]*SF[4] + P[1][2]*SF[8] + P[3][2]*SF[6] + P[12][2]*SF[11] - P[10][2]*SPP[6] - (P[11][2]*q0)/2) - (sq(q0) - sq(q1) - sq(q2) + sq(q3))*(P[2][13] + P[0][13]*SF[4] + P[1][13]*SF[8] + P[3][13]*SF[6] + P[12][13]*SF[11] - P[10][13]*SPP[6] - (P[11][13]*q0)/2);
+    nextP[2][7] = P[2][7] + P[0][7]*SF[4] + P[1][7]*SF[8] + P[3][7]*SF[6] + P[12][7]*SF[11] - P[10][7]*SPP[6] - (P[11][7]*q0)/2 + dt*(P[2][4] + P[0][4]*SF[4] + P[1][4]*SF[8] + P[3][4]*SF[6] + P[12][4]*SF[11] - P[10][4]*SPP[6] - (P[11][4]*q0)/2);
+    nextP[2][8] = P[2][8] + P[0][8]*SF[4] + P[1][8]*SF[8] + P[3][8]*SF[6] + P[12][8]*SF[11] - P[10][8]*SPP[6] - (P[11][8]*q0)/2 + dt*(P[2][5] + P[0][5]*SF[4] + P[1][5]*SF[8] + P[3][5]*SF[6] + P[12][5]*SF[11] - P[10][5]*SPP[6] - (P[11][5]*q0)/2);
+    nextP[2][9] = P[2][9] + P[0][9]*SF[4] + P[1][9]*SF[8] + P[3][9]*SF[6] + P[12][9]*SF[11] - P[10][9]*SPP[6] - (P[11][9]*q0)/2 + dt*(P[2][6] + P[0][6]*SF[4] + P[1][6]*SF[8] + P[3][6]*SF[6] + P[12][6]*SF[11] - P[10][6]*SPP[6] - (P[11][6]*q0)/2);
+    nextP[2][10] = P[2][10] + P[0][10]*SF[4] + P[1][10]*SF[8] + P[3][10]*SF[6] + P[12][10]*SF[11] - P[10][10]*SPP[6] - (P[11][10]*q0)/2;
+    nextP[2][11] = P[2][11] + P[0][11]*SF[4] + P[1][11]*SF[8] + P[3][11]*SF[6] + P[12][11]*SF[11] - P[10][11]*SPP[6] - (P[11][11]*q0)/2;
+    nextP[2][12] = P[2][12] + P[0][12]*SF[4] + P[1][12]*SF[8] + P[3][12]*SF[6] + P[12][12]*SF[11] - P[10][12]*SPP[6] - (P[11][12]*q0)/2;
+    nextP[2][13] = P[2][13] + P[0][13]*SF[4] + P[1][13]*SF[8] + P[3][13]*SF[6] + P[12][13]*SF[11] - P[10][13]*SPP[6] - (P[11][13]*q0)/2;
+    nextP[2][14] = P[2][14] + P[0][14]*SF[4] + P[1][14]*SF[8] + P[3][14]*SF[6] + P[12][14]*SF[11] - P[10][14]*SPP[6] - (P[11][14]*q0)/2;
+    nextP[2][15] = P[2][15] + P[0][15]*SF[4] + P[1][15]*SF[8] + P[3][15]*SF[6] + P[12][15]*SF[11] - P[10][15]*SPP[6] - (P[11][15]*q0)/2;
+    nextP[2][16] = P[2][16] + P[0][16]*SF[4] + P[1][16]*SF[8] + P[3][16]*SF[6] + P[12][16]*SF[11] - P[10][16]*SPP[6] - (P[11][16]*q0)/2;
+    nextP[2][17] = P[2][17] + P[0][17]*SF[4] + P[1][17]*SF[8] + P[3][17]*SF[6] + P[12][17]*SF[11] - P[10][17]*SPP[6] - (P[11][17]*q0)/2;
+    nextP[2][18] = P[2][18] + P[0][18]*SF[4] + P[1][18]*SF[8] + P[3][18]*SF[6] + P[12][18]*SF[11] - P[10][18]*SPP[6] - (P[11][18]*q0)/2;
+    nextP[2][19] = P[2][19] + P[0][19]*SF[4] + P[1][19]*SF[8] + P[3][19]*SF[6] + P[12][19]*SF[11] - P[10][19]*SPP[6] - (P[11][19]*q0)/2;
+    nextP[2][20] = P[2][20] + P[0][20]*SF[4] + P[1][20]*SF[8] + P[3][20]*SF[6] + P[12][20]*SF[11] - P[10][20]*SPP[6] - (P[11][20]*q0)/2;
+    nextP[2][21] = P[2][21] + P[0][21]*SF[4] + P[1][21]*SF[8] + P[3][21]*SF[6] + P[12][21]*SF[11] - P[10][21]*SPP[6] - (P[11][21]*q0)/2;
+    nextP[2][22] = P[2][22] + P[0][22]*SF[4] + P[1][22]*SF[8] + P[3][22]*SF[6] + P[12][22]*SF[11] - P[10][22]*SPP[6] - (P[11][22]*q0)/2;
+    nextP[3][0] = P[3][0] + SQ[6] + P[0][0]*SF[5] + P[1][0]*SF[4] + P[2][0]*SF[7] - P[11][0]*SF[11] + P[10][0]*SPP[7] - (P[12][0]*q0)/2 + SF[7]*(P[3][1] + P[0][1]*SF[5] + P[1][1]*SF[4] + P[2][1]*SF[7] - P[11][1]*SF[11] + P[10][1]*SPP[7] - (P[12][1]*q0)/2) + SF[9]*(P[3][2] + P[0][2]*SF[5] + P[1][2]*SF[4] + P[2][2]*SF[7] - P[11][2]*SF[11] + P[10][2]*SPP[7] - (P[12][2]*q0)/2) + SF[8]*(P[3][3] + P[0][3]*SF[5] + P[1][3]*SF[4] + P[2][3]*SF[7] - P[11][3]*SF[11] + P[10][3]*SPP[7] - (P[12][3]*q0)/2) + SF[11]*(P[3][10] + P[0][10]*SF[5] + P[1][10]*SF[4] + P[2][10]*SF[7] - P[11][10]*SF[11] + P[10][10]*SPP[7] - (P[12][10]*q0)/2) + SPP[7]*(P[3][11] + P[0][11]*SF[5] + P[1][11]*SF[4] + P[2][11]*SF[7] - P[11][11]*SF[11] + P[10][11]*SPP[7] - (P[12][11]*q0)/2) + SPP[6]*(P[3][12] + P[0][12]*SF[5] + P[1][12]*SF[4] + P[2][12]*SF[7] - P[11][12]*SF[11] + P[10][12]*SPP[7] - (P[12][12]*q0)/2);
+    nextP[3][1] = P[3][1] + SQ[4] + P[0][1]*SF[5] + P[1][1]*SF[4] + P[2][1]*SF[7] - P[11][1]*SF[11] + P[10][1]*SPP[7] - (P[12][1]*q0)/2 + SF[6]*(P[3][0] + P[0][0]*SF[5] + P[1][0]*SF[4] + P[2][0]*SF[7] - P[11][0]*SF[11] + P[10][0]*SPP[7] - (P[12][0]*q0)/2) + SF[5]*(P[3][2] + P[0][2]*SF[5] + P[1][2]*SF[4] + P[2][2]*SF[7] - P[11][2]*SF[11] + P[10][2]*SPP[7] - (P[12][2]*q0)/2) + SF[9]*(P[3][3] + P[0][3]*SF[5] + P[1][3]*SF[4] + P[2][3]*SF[7] - P[11][3]*SF[11] + P[10][3]*SPP[7] - (P[12][3]*q0)/2) + SPP[6]*(P[3][11] + P[0][11]*SF[5] + P[1][11]*SF[4] + P[2][11]*SF[7] - P[11][11]*SF[11] + P[10][11]*SPP[7] - (P[12][11]*q0)/2) - SPP[7]*(P[3][12] + P[0][12]*SF[5] + P[1][12]*SF[4] + P[2][12]*SF[7] - P[11][12]*SF[11] + P[10][12]*SPP[7] - (P[12][12]*q0)/2) - (q0*(P[3][10] + P[0][10]*SF[5] + P[1][10]*SF[4] + P[2][10]*SF[7] - P[11][10]*SF[11] + P[10][10]*SPP[7] - (P[12][10]*q0)/2))/2;
+    nextP[3][2] = P[3][2] + SQ[3] + P[0][2]*SF[5] + P[1][2]*SF[4] + P[2][2]*SF[7] - P[11][2]*SF[11] + P[10][2]*SPP[7] - (P[12][2]*q0)/2 + SF[4]*(P[3][0] + P[0][0]*SF[5] + P[1][0]*SF[4] + P[2][0]*SF[7] - P[11][0]*SF[11] + P[10][0]*SPP[7] - (P[12][0]*q0)/2) + SF[8]*(P[3][1] + P[0][1]*SF[5] + P[1][1]*SF[4] + P[2][1]*SF[7] - P[11][1]*SF[11] + P[10][1]*SPP[7] - (P[12][1]*q0)/2) + SF[6]*(P[3][3] + P[0][3]*SF[5] + P[1][3]*SF[4] + P[2][3]*SF[7] - P[11][3]*SF[11] + P[10][3]*SPP[7] - (P[12][3]*q0)/2) + SF[11]*(P[3][12] + P[0][12]*SF[5] + P[1][12]*SF[4] + P[2][12]*SF[7] - P[11][12]*SF[11] + P[10][12]*SPP[7] - (P[12][12]*q0)/2) - SPP[6]*(P[3][10] + P[0][10]*SF[5] + P[1][10]*SF[4] + P[2][10]*SF[7] - P[11][10]*SF[11] + P[10][10]*SPP[7] - (P[12][10]*q0)/2) - (q0*(P[3][11] + P[0][11]*SF[5] + P[1][11]*SF[4] + P[2][11]*SF[7] - P[11][11]*SF[11] + P[10][11]*SPP[7] - (P[12][11]*q0)/2))/2;
+    nextP[3][3] = P[3][3] + P[0][3]*SF[5] + P[1][3]*SF[4] + P[2][3]*SF[7] - P[11][3]*SF[11] + P[10][3]*SPP[7] + (dayCov*SQ[10])/4 + dazCov*SQ[9] - (P[12][3]*q0)/2 + SF[5]*(P[3][0] + P[0][0]*SF[5] + P[1][0]*SF[4] + P[2][0]*SF[7] - P[11][0]*SF[11] + P[10][0]*SPP[7] - (P[12][0]*q0)/2) + SF[4]*(P[3][1] + P[0][1]*SF[5] + P[1][1]*SF[4] + P[2][1]*SF[7] - P[11][1]*SF[11] + P[10][1]*SPP[7] - (P[12][1]*q0)/2) + SF[7]*(P[3][2] + P[0][2]*SF[5] + P[1][2]*SF[4] + P[2][2]*SF[7] - P[11][2]*SF[11] + P[10][2]*SPP[7] - (P[12][2]*q0)/2) - SF[11]*(P[3][11] + P[0][11]*SF[5] + P[1][11]*SF[4] + P[2][11]*SF[7] - P[11][11]*SF[11] + P[10][11]*SPP[7] - (P[12][11]*q0)/2) + SPP[7]*(P[3][10] + P[0][10]*SF[5] + P[1][10]*SF[4] + P[2][10]*SF[7] - P[11][10]*SF[11] + P[10][10]*SPP[7] - (P[12][10]*q0)/2) + (daxCov*sq(q2))/4 - (q0*(P[3][12] + P[0][12]*SF[5] + P[1][12]*SF[4] + P[2][12]*SF[7] - P[11][12]*SF[11] + P[10][12]*SPP[7] - (P[12][12]*q0)/2))/2;
+    nextP[3][4] = P[3][4] + P[0][4]*SF[5] + P[1][4]*SF[4] + P[2][4]*SF[7] - P[11][4]*SF[11] + P[10][4]*SPP[7] - (P[12][4]*q0)/2 + SF[3]*(P[3][0] + P[0][0]*SF[5] + P[1][0]*SF[4] + P[2][0]*SF[7] - P[11][0]*SF[11] + P[10][0]*SPP[7] - (P[12][0]*q0)/2) + SF[1]*(P[3][1] + P[0][1]*SF[5] + P[1][1]*SF[4] + P[2][1]*SF[7] - P[11][1]*SF[11] + P[10][1]*SPP[7] - (P[12][1]*q0)/2) + SPP[0]*(P[3][2] + P[0][2]*SF[5] + P[1][2]*SF[4] + P[2][2]*SF[7] - P[11][2]*SF[11] + P[10][2]*SPP[7] - (P[12][2]*q0)/2) - SPP[2]*(P[3][3] + P[0][3]*SF[5] + P[1][3]*SF[4] + P[2][3]*SF[7] - P[11][3]*SF[11] + P[10][3]*SPP[7] - (P[12][3]*q0)/2) - SPP[4]*(P[3][13] + P[0][13]*SF[5] + P[1][13]*SF[4] + P[2][13]*SF[7] - P[11][13]*SF[11] + P[10][13]*SPP[7] - (P[12][13]*q0)/2);
+    nextP[3][5] = P[3][5] + P[0][5]*SF[5] + P[1][5]*SF[4] + P[2][5]*SF[7] - P[11][5]*SF[11] + P[10][5]*SPP[7] - (P[12][5]*q0)/2 + SF[2]*(P[3][0] + P[0][0]*SF[5] + P[1][0]*SF[4] + P[2][0]*SF[7] - P[11][0]*SF[11] + P[10][0]*SPP[7] - (P[12][0]*q0)/2) + SF[1]*(P[3][2] + P[0][2]*SF[5] + P[1][2]*SF[4] + P[2][2]*SF[7] - P[11][2]*SF[11] + P[10][2]*SPP[7] - (P[12][2]*q0)/2) + SF[3]*(P[3][3] + P[0][3]*SF[5] + P[1][3]*SF[4] + P[2][3]*SF[7] - P[11][3]*SF[11] + P[10][3]*SPP[7] - (P[12][3]*q0)/2) - SPP[0]*(P[3][1] + P[0][1]*SF[5] + P[1][1]*SF[4] + P[2][1]*SF[7] - P[11][1]*SF[11] + P[10][1]*SPP[7] - (P[12][1]*q0)/2) + SPP[3]*(P[3][13] + P[0][13]*SF[5] + P[1][13]*SF[4] + P[2][13]*SF[7] - P[11][13]*SF[11] + P[10][13]*SPP[7] - (P[12][13]*q0)/2);
+    nextP[3][6] = P[3][6] + P[0][6]*SF[5] + P[1][6]*SF[4] + P[2][6]*SF[7] - P[11][6]*SF[11] + P[10][6]*SPP[7] - (P[12][6]*q0)/2 + SF[2]*(P[3][1] + P[0][1]*SF[5] + P[1][1]*SF[4] + P[2][1]*SF[7] - P[11][1]*SF[11] + P[10][1]*SPP[7] - (P[12][1]*q0)/2) + SF[1]*(P[3][3] + P[0][3]*SF[5] + P[1][3]*SF[4] + P[2][3]*SF[7] - P[11][3]*SF[11] + P[10][3]*SPP[7] - (P[12][3]*q0)/2) + SPP[0]*(P[3][0] + P[0][0]*SF[5] + P[1][0]*SF[4] + P[2][0]*SF[7] - P[11][0]*SF[11] + P[10][0]*SPP[7] - (P[12][0]*q0)/2) - SPP[1]*(P[3][2] + P[0][2]*SF[5] + P[1][2]*SF[4] + P[2][2]*SF[7] - P[11][2]*SF[11] + P[10][2]*SPP[7] - (P[12][2]*q0)/2) - (sq(q0) - sq(q1) - sq(q2) + sq(q3))*(P[3][13] + P[0][13]*SF[5] + P[1][13]*SF[4] + P[2][13]*SF[7] - P[11][13]*SF[11] + P[10][13]*SPP[7] - (P[12][13]*q0)/2);
+    nextP[3][7] = P[3][7] + P[0][7]*SF[5] + P[1][7]*SF[4] + P[2][7]*SF[7] - P[11][7]*SF[11] + P[10][7]*SPP[7] - (P[12][7]*q0)/2 + dt*(P[3][4] + P[0][4]*SF[5] + P[1][4]*SF[4] + P[2][4]*SF[7] - P[11][4]*SF[11] + P[10][4]*SPP[7] - (P[12][4]*q0)/2);
+    nextP[3][8] = P[3][8] + P[0][8]*SF[5] + P[1][8]*SF[4] + P[2][8]*SF[7] - P[11][8]*SF[11] + P[10][8]*SPP[7] - (P[12][8]*q0)/2 + dt*(P[3][5] + P[0][5]*SF[5] + P[1][5]*SF[4] + P[2][5]*SF[7] - P[11][5]*SF[11] + P[10][5]*SPP[7] - (P[12][5]*q0)/2);
+    nextP[3][9] = P[3][9] + P[0][9]*SF[5] + P[1][9]*SF[4] + P[2][9]*SF[7] - P[11][9]*SF[11] + P[10][9]*SPP[7] - (P[12][9]*q0)/2 + dt*(P[3][6] + P[0][6]*SF[5] + P[1][6]*SF[4] + P[2][6]*SF[7] - P[11][6]*SF[11] + P[10][6]*SPP[7] - (P[12][6]*q0)/2);
+    nextP[3][10] = P[3][10] + P[0][10]*SF[5] + P[1][10]*SF[4] + P[2][10]*SF[7] - P[11][10]*SF[11] + P[10][10]*SPP[7] - (P[12][10]*q0)/2;
+    nextP[3][11] = P[3][11] + P[0][11]*SF[5] + P[1][11]*SF[4] + P[2][11]*SF[7] - P[11][11]*SF[11] + P[10][11]*SPP[7] - (P[12][11]*q0)/2;
+    nextP[3][12] = P[3][12] + P[0][12]*SF[5] + P[1][12]*SF[4] + P[2][12]*SF[7] - P[11][12]*SF[11] + P[10][12]*SPP[7] - (P[12][12]*q0)/2;
+    nextP[3][13] = P[3][13] + P[0][13]*SF[5] + P[1][13]*SF[4] + P[2][13]*SF[7] - P[11][13]*SF[11] + P[10][13]*SPP[7] - (P[12][13]*q0)/2;
+    nextP[3][14] = P[3][14] + P[0][14]*SF[5] + P[1][14]*SF[4] + P[2][14]*SF[7] - P[11][14]*SF[11] + P[10][14]*SPP[7] - (P[12][14]*q0)/2;
+    nextP[3][15] = P[3][15] + P[0][15]*SF[5] + P[1][15]*SF[4] + P[2][15]*SF[7] - P[11][15]*SF[11] + P[10][15]*SPP[7] - (P[12][15]*q0)/2;
+    nextP[3][16] = P[3][16] + P[0][16]*SF[5] + P[1][16]*SF[4] + P[2][16]*SF[7] - P[11][16]*SF[11] + P[10][16]*SPP[7] - (P[12][16]*q0)/2;
+    nextP[3][17] = P[3][17] + P[0][17]*SF[5] + P[1][17]*SF[4] + P[2][17]*SF[7] - P[11][17]*SF[11] + P[10][17]*SPP[7] - (P[12][17]*q0)/2;
+    nextP[3][18] = P[3][18] + P[0][18]*SF[5] + P[1][18]*SF[4] + P[2][18]*SF[7] - P[11][18]*SF[11] + P[10][18]*SPP[7] - (P[12][18]*q0)/2;
+    nextP[3][19] = P[3][19] + P[0][19]*SF[5] + P[1][19]*SF[4] + P[2][19]*SF[7] - P[11][19]*SF[11] + P[10][19]*SPP[7] - (P[12][19]*q0)/2;
+    nextP[3][20] = P[3][20] + P[0][20]*SF[5] + P[1][20]*SF[4] + P[2][20]*SF[7] - P[11][20]*SF[11] + P[10][20]*SPP[7] - (P[12][20]*q0)/2;
+    nextP[3][21] = P[3][21] + P[0][21]*SF[5] + P[1][21]*SF[4] + P[2][21]*SF[7] - P[11][21]*SF[11] + P[10][21]*SPP[7] - (P[12][21]*q0)/2;
+    nextP[3][22] = P[3][22] + P[0][22]*SF[5] + P[1][22]*SF[4] + P[2][22]*SF[7] - P[11][22]*SF[11] + P[10][22]*SPP[7] - (P[12][22]*q0)/2;
+    nextP[4][0] = P[4][0] + P[0][0]*SF[3] + P[1][0]*SF[1] + P[2][0]*SPP[0] - P[3][0]*SPP[2] - P[13][0]*SPP[4] + SF[7]*(P[4][1] + P[0][1]*SF[3] + P[1][1]*SF[1] + P[2][1]*SPP[0] - P[3][1]*SPP[2] - P[13][1]*SPP[4]) + SF[9]*(P[4][2] + P[0][2]*SF[3] + P[1][2]*SF[1] + P[2][2]*SPP[0] - P[3][2]*SPP[2] - P[13][2]*SPP[4]) + SF[8]*(P[4][3] + P[0][3]*SF[3] + P[1][3]*SF[1] + P[2][3]*SPP[0] - P[3][3]*SPP[2] - P[13][3]*SPP[4]) + SF[11]*(P[4][10] + P[0][10]*SF[3] + P[1][10]*SF[1] + P[2][10]*SPP[0] - P[3][10]*SPP[2] - P[13][10]*SPP[4]) + SPP[7]*(P[4][11] + P[0][11]*SF[3] + P[1][11]*SF[1] + P[2][11]*SPP[0] - P[3][11]*SPP[2] - P[13][11]*SPP[4]) + SPP[6]*(P[4][12] + P[0][12]*SF[3] + P[1][12]*SF[1] + P[2][12]*SPP[0] - P[3][12]*SPP[2] - P[13][12]*SPP[4]);
+    nextP[4][1] = P[4][1] + P[0][1]*SF[3] + P[1][1]*SF[1] + P[2][1]*SPP[0] - P[3][1]*SPP[2] - P[13][1]*SPP[4] + SF[6]*(P[4][0] + P[0][0]*SF[3] + P[1][0]*SF[1] + P[2][0]*SPP[0] - P[3][0]*SPP[2] - P[13][0]*SPP[4]) + SF[5]*(P[4][2] + P[0][2]*SF[3] + P[1][2]*SF[1] + P[2][2]*SPP[0] - P[3][2]*SPP[2] - P[13][2]*SPP[4]) + SF[9]*(P[4][3] + P[0][3]*SF[3] + P[1][3]*SF[1] + P[2][3]*SPP[0] - P[3][3]*SPP[2] - P[13][3]*SPP[4]) + SPP[6]*(P[4][11] + P[0][11]*SF[3] + P[1][11]*SF[1] + P[2][11]*SPP[0] - P[3][11]*SPP[2] - P[13][11]*SPP[4]) - SPP[7]*(P[4][12] + P[0][12]*SF[3] + P[1][12]*SF[1] + P[2][12]*SPP[0] - P[3][12]*SPP[2] - P[13][12]*SPP[4]) - (q0*(P[4][10] + P[0][10]*SF[3] + P[1][10]*SF[1] + P[2][10]*SPP[0] - P[3][10]*SPP[2] - P[13][10]*SPP[4]))/2;
+    nextP[4][2] = P[4][2] + P[0][2]*SF[3] + P[1][2]*SF[1] + P[2][2]*SPP[0] - P[3][2]*SPP[2] - P[13][2]*SPP[4] + SF[4]*(P[4][0] + P[0][0]*SF[3] + P[1][0]*SF[1] + P[2][0]*SPP[0] - P[3][0]*SPP[2] - P[13][0]*SPP[4]) + SF[8]*(P[4][1] + P[0][1]*SF[3] + P[1][1]*SF[1] + P[2][1]*SPP[0] - P[3][1]*SPP[2] - P[13][1]*SPP[4]) + SF[6]*(P[4][3] + P[0][3]*SF[3] + P[1][3]*SF[1] + P[2][3]*SPP[0] - P[3][3]*SPP[2] - P[13][3]*SPP[4]) + SF[11]*(P[4][12] + P[0][12]*SF[3] + P[1][12]*SF[1] + P[2][12]*SPP[0] - P[3][12]*SPP[2] - P[13][12]*SPP[4]) - SPP[6]*(P[4][10] + P[0][10]*SF[3] + P[1][10]*SF[1] + P[2][10]*SPP[0] - P[3][10]*SPP[2] - P[13][10]*SPP[4]) - (q0*(P[4][11] + P[0][11]*SF[3] + P[1][11]*SF[1] + P[2][11]*SPP[0] - P[3][11]*SPP[2] - P[13][11]*SPP[4]))/2;
+    nextP[4][3] = P[4][3] + P[0][3]*SF[3] + P[1][3]*SF[1] + P[2][3]*SPP[0] - P[3][3]*SPP[2] - P[13][3]*SPP[4] + SF[5]*(P[4][0] + P[0][0]*SF[3] + P[1][0]*SF[1] + P[2][0]*SPP[0] - P[3][0]*SPP[2] - P[13][0]*SPP[4]) + SF[4]*(P[4][1] + P[0][1]*SF[3] + P[1][1]*SF[1] + P[2][1]*SPP[0] - P[3][1]*SPP[2] - P[13][1]*SPP[4]) + SF[7]*(P[4][2] + P[0][2]*SF[3] + P[1][2]*SF[1] + P[2][2]*SPP[0] - P[3][2]*SPP[2] - P[13][2]*SPP[4]) - SF[11]*(P[4][11] + P[0][11]*SF[3] + P[1][11]*SF[1] + P[2][11]*SPP[0] - P[3][11]*SPP[2] - P[13][11]*SPP[4]) + SPP[7]*(P[4][10] + P[0][10]*SF[3] + P[1][10]*SF[1] + P[2][10]*SPP[0] - P[3][10]*SPP[2] - P[13][10]*SPP[4]) - (q0*(P[4][12] + P[0][12]*SF[3] + P[1][12]*SF[1] + P[2][12]*SPP[0] - P[3][12]*SPP[2] - P[13][12]*SPP[4]))/2;
+    nextP[4][4] = P[4][4] + P[0][4]*SF[3] + P[1][4]*SF[1] + P[2][4]*SPP[0] - P[3][4]*SPP[2] - P[13][4]*SPP[4] + dvyCov*sq(SG[7] - 2*q0*q3) + dvzCov*sq(SG[6] + 2*q0*q2) + SF[3]*(P[4][0] + P[0][0]*SF[3] + P[1][0]*SF[1] + P[2][0]*SPP[0] - P[3][0]*SPP[2] - P[13][0]*SPP[4]) + SF[1]*(P[4][1] + P[0][1]*SF[3] + P[1][1]*SF[1] + P[2][1]*SPP[0] - P[3][1]*SPP[2] - P[13][1]*SPP[4]) + SPP[0]*(P[4][2] + P[0][2]*SF[3] + P[1][2]*SF[1] + P[2][2]*SPP[0] - P[3][2]*SPP[2] - P[13][2]*SPP[4]) - SPP[2]*(P[4][3] + P[0][3]*SF[3] + P[1][3]*SF[1] + P[2][3]*SPP[0] - P[3][3]*SPP[2] - P[13][3]*SPP[4]) - SPP[4]*(P[4][13] + P[0][13]*SF[3] + P[1][13]*SF[1] + P[2][13]*SPP[0] - P[3][13]*SPP[2] - P[13][13]*SPP[4]) + dvxCov*sq(SG[1] + SG[2] - SG[3] - SG[4]);
+    nextP[4][5] = P[4][5] + SQ[2] + P[0][5]*SF[3] + P[1][5]*SF[1] + P[2][5]*SPP[0] - P[3][5]*SPP[2] - P[13][5]*SPP[4] + SF[2]*(P[4][0] + P[0][0]*SF[3] + P[1][0]*SF[1] + P[2][0]*SPP[0] - P[3][0]*SPP[2] - P[13][0]*SPP[4]) + SF[1]*(P[4][2] + P[0][2]*SF[3] + P[1][2]*SF[1] + P[2][2]*SPP[0] - P[3][2]*SPP[2] - P[13][2]*SPP[4]) + SF[3]*(P[4][3] + P[0][3]*SF[3] + P[1][3]*SF[1] + P[2][3]*SPP[0] - P[3][3]*SPP[2] - P[13][3]*SPP[4]) - SPP[0]*(P[4][1] + P[0][1]*SF[3] + P[1][1]*SF[1] + P[2][1]*SPP[0] - P[3][1]*SPP[2] - P[13][1]*SPP[4]) + SPP[3]*(P[4][13] + P[0][13]*SF[3] + P[1][13]*SF[1] + P[2][13]*SPP[0] - P[3][13]*SPP[2] - P[13][13]*SPP[4]);
+    nextP[4][6] = P[4][6] + SQ[1] + P[0][6]*SF[3] + P[1][6]*SF[1] + P[2][6]*SPP[0] - P[3][6]*SPP[2] - P[13][6]*SPP[4] + SF[2]*(P[4][1] + P[0][1]*SF[3] + P[1][1]*SF[1] + P[2][1]*SPP[0] - P[3][1]*SPP[2] - P[13][1]*SPP[4]) + SF[1]*(P[4][3] + P[0][3]*SF[3] + P[1][3]*SF[1] + P[2][3]*SPP[0] - P[3][3]*SPP[2] - P[13][3]*SPP[4]) + SPP[0]*(P[4][0] + P[0][0]*SF[3] + P[1][0]*SF[1] + P[2][0]*SPP[0] - P[3][0]*SPP[2] - P[13][0]*SPP[4]) - SPP[1]*(P[4][2] + P[0][2]*SF[3] + P[1][2]*SF[1] + P[2][2]*SPP[0] - P[3][2]*SPP[2] - P[13][2]*SPP[4]) - (sq(q0) - sq(q1) - sq(q2) + sq(q3))*(P[4][13] + P[0][13]*SF[3] + P[1][13]*SF[1] + P[2][13]*SPP[0] - P[3][13]*SPP[2] - P[13][13]*SPP[4]);
+    nextP[4][7] = P[4][7] + P[0][7]*SF[3] + P[1][7]*SF[1] + P[2][7]*SPP[0] - P[3][7]*SPP[2] - P[13][7]*SPP[4] + dt*(P[4][4] + P[0][4]*SF[3] + P[1][4]*SF[1] + P[2][4]*SPP[0] - P[3][4]*SPP[2] - P[13][4]*SPP[4]);
+    nextP[4][8] = P[4][8] + P[0][8]*SF[3] + P[1][8]*SF[1] + P[2][8]*SPP[0] - P[3][8]*SPP[2] - P[13][8]*SPP[4] + dt*(P[4][5] + P[0][5]*SF[3] + P[1][5]*SF[1] + P[2][5]*SPP[0] - P[3][5]*SPP[2] - P[13][5]*SPP[4]);
+    nextP[4][9] = P[4][9] + P[0][9]*SF[3] + P[1][9]*SF[1] + P[2][9]*SPP[0] - P[3][9]*SPP[2] - P[13][9]*SPP[4] + dt*(P[4][6] + P[0][6]*SF[3] + P[1][6]*SF[1] + P[2][6]*SPP[0] - P[3][6]*SPP[2] - P[13][6]*SPP[4]);
+    nextP[4][10] = P[4][10] + P[0][10]*SF[3] + P[1][10]*SF[1] + P[2][10]*SPP[0] - P[3][10]*SPP[2] - P[13][10]*SPP[4];
+    nextP[4][11] = P[4][11] + P[0][11]*SF[3] + P[1][11]*SF[1] + P[2][11]*SPP[0] - P[3][11]*SPP[2] - P[13][11]*SPP[4];
+    nextP[4][12] = P[4][12] + P[0][12]*SF[3] + P[1][12]*SF[1] + P[2][12]*SPP[0] - P[3][12]*SPP[2] - P[13][12]*SPP[4];
+    nextP[4][13] = P[4][13] + P[0][13]*SF[3] + P[1][13]*SF[1] + P[2][13]*SPP[0] - P[3][13]*SPP[2] - P[13][13]*SPP[4];
+    nextP[4][14] = P[4][14] + P[0][14]*SF[3] + P[1][14]*SF[1] + P[2][14]*SPP[0] - P[3][14]*SPP[2] - P[13][14]*SPP[4];
+    nextP[4][15] = P[4][15] + P[0][15]*SF[3] + P[1][15]*SF[1] + P[2][15]*SPP[0] - P[3][15]*SPP[2] - P[13][15]*SPP[4];
+    nextP[4][16] = P[4][16] + P[0][16]*SF[3] + P[1][16]*SF[1] + P[2][16]*SPP[0] - P[3][16]*SPP[2] - P[13][16]*SPP[4];
+    nextP[4][17] = P[4][17] + P[0][17]*SF[3] + P[1][17]*SF[1] + P[2][17]*SPP[0] - P[3][17]*SPP[2] - P[13][17]*SPP[4];
+    nextP[4][18] = P[4][18] + P[0][18]*SF[3] + P[1][18]*SF[1] + P[2][18]*SPP[0] - P[3][18]*SPP[2] - P[13][18]*SPP[4];
+    nextP[4][19] = P[4][19] + P[0][19]*SF[3] + P[1][19]*SF[1] + P[2][19]*SPP[0] - P[3][19]*SPP[2] - P[13][19]*SPP[4];
+    nextP[4][20] = P[4][20] + P[0][20]*SF[3] + P[1][20]*SF[1] + P[2][20]*SPP[0] - P[3][20]*SPP[2] - P[13][20]*SPP[4];
+    nextP[4][21] = P[4][21] + P[0][21]*SF[3] + P[1][21]*SF[1] + P[2][21]*SPP[0] - P[3][21]*SPP[2] - P[13][21]*SPP[4];
+    nextP[4][22] = P[4][22] + P[0][22]*SF[3] + P[1][22]*SF[1] + P[2][22]*SPP[0] - P[3][22]*SPP[2] - P[13][22]*SPP[4];
+    nextP[5][0] = P[5][0] + P[0][0]*SF[2] + P[2][0]*SF[1] + P[3][0]*SF[3] - P[1][0]*SPP[0] + P[13][0]*SPP[3] + SF[7]*(P[5][1] + P[0][1]*SF[2] + P[2][1]*SF[1] + P[3][1]*SF[3] - P[1][1]*SPP[0] + P[13][1]*SPP[3]) + SF[9]*(P[5][2] + P[0][2]*SF[2] + P[2][2]*SF[1] + P[3][2]*SF[3] - P[1][2]*SPP[0] + P[13][2]*SPP[3]) + SF[8]*(P[5][3] + P[0][3]*SF[2] + P[2][3]*SF[1] + P[3][3]*SF[3] - P[1][3]*SPP[0] + P[13][3]*SPP[3]) + SF[11]*(P[5][10] + P[0][10]*SF[2] + P[2][10]*SF[1] + P[3][10]*SF[3] - P[1][10]*SPP[0] + P[13][10]*SPP[3]) + SPP[7]*(P[5][11] + P[0][11]*SF[2] + P[2][11]*SF[1] + P[3][11]*SF[3] - P[1][11]*SPP[0] + P[13][11]*SPP[3]) + SPP[6]*(P[5][12] + P[0][12]*SF[2] + P[2][12]*SF[1] + P[3][12]*SF[3] - P[1][12]*SPP[0] + P[13][12]*SPP[3]);
+    nextP[5][1] = P[5][1] + P[0][1]*SF[2] + P[2][1]*SF[1] + P[3][1]*SF[3] - P[1][1]*SPP[0] + P[13][1]*SPP[3] + SF[6]*(P[5][0] + P[0][0]*SF[2] + P[2][0]*SF[1] + P[3][0]*SF[3] - P[1][0]*SPP[0] + P[13][0]*SPP[3]) + SF[5]*(P[5][2] + P[0][2]*SF[2] + P[2][2]*SF[1] + P[3][2]*SF[3] - P[1][2]*SPP[0] + P[13][2]*SPP[3]) + SF[9]*(P[5][3] + P[0][3]*SF[2] + P[2][3]*SF[1] + P[3][3]*SF[3] - P[1][3]*SPP[0] + P[13][3]*SPP[3]) + SPP[6]*(P[5][11] + P[0][11]*SF[2] + P[2][11]*SF[1] + P[3][11]*SF[3] - P[1][11]*SPP[0] + P[13][11]*SPP[3]) - SPP[7]*(P[5][12] + P[0][12]*SF[2] + P[2][12]*SF[1] + P[3][12]*SF[3] - P[1][12]*SPP[0] + P[13][12]*SPP[3]) - (q0*(P[5][10] + P[0][10]*SF[2] + P[2][10]*SF[1] + P[3][10]*SF[3] - P[1][10]*SPP[0] + P[13][10]*SPP[3]))/2;
+    nextP[5][2] = P[5][2] + P[0][2]*SF[2] + P[2][2]*SF[1] + P[3][2]*SF[3] - P[1][2]*SPP[0] + P[13][2]*SPP[3] + SF[4]*(P[5][0] + P[0][0]*SF[2] + P[2][0]*SF[1] + P[3][0]*SF[3] - P[1][0]*SPP[0] + P[13][0]*SPP[3]) + SF[8]*(P[5][1] + P[0][1]*SF[2] + P[2][1]*SF[1] + P[3][1]*SF[3] - P[1][1]*SPP[0] + P[13][1]*SPP[3]) + SF[6]*(P[5][3] + P[0][3]*SF[2] + P[2][3]*SF[1] + P[3][3]*SF[3] - P[1][3]*SPP[0] + P[13][3]*SPP[3]) + SF[11]*(P[5][12] + P[0][12]*SF[2] + P[2][12]*SF[1] + P[3][12]*SF[3] - P[1][12]*SPP[0] + P[13][12]*SPP[3]) - SPP[6]*(P[5][10] + P[0][10]*SF[2] + P[2][10]*SF[1] + P[3][10]*SF[3] - P[1][10]*SPP[0] + P[13][10]*SPP[3]) - (q0*(P[5][11] + P[0][11]*SF[2] + P[2][11]*SF[1] + P[3][11]*SF[3] - P[1][11]*SPP[0] + P[13][11]*SPP[3]))/2;
+    nextP[5][3] = P[5][3] + P[0][3]*SF[2] + P[2][3]*SF[1] + P[3][3]*SF[3] - P[1][3]*SPP[0] + P[13][3]*SPP[3] + SF[5]*(P[5][0] + P[0][0]*SF[2] + P[2][0]*SF[1] + P[3][0]*SF[3] - P[1][0]*SPP[0] + P[13][0]*SPP[3]) + SF[4]*(P[5][1] + P[0][1]*SF[2] + P[2][1]*SF[1] + P[3][1]*SF[3] - P[1][1]*SPP[0] + P[13][1]*SPP[3]) + SF[7]*(P[5][2] + P[0][2]*SF[2] + P[2][2]*SF[1] + P[3][2]*SF[3] - P[1][2]*SPP[0] + P[13][2]*SPP[3]) - SF[11]*(P[5][11] + P[0][11]*SF[2] + P[2][11]*SF[1] + P[3][11]*SF[3] - P[1][11]*SPP[0] + P[13][11]*SPP[3]) + SPP[7]*(P[5][10] + P[0][10]*SF[2] + P[2][10]*SF[1] + P[3][10]*SF[3] - P[1][10]*SPP[0] + P[13][10]*SPP[3]) - (q0*(P[5][12] + P[0][12]*SF[2] + P[2][12]*SF[1] + P[3][12]*SF[3] - P[1][12]*SPP[0] + P[13][12]*SPP[3]))/2;
+    nextP[5][4] = P[5][4] + SQ[2] + P[0][4]*SF[2] + P[2][4]*SF[1] + P[3][4]*SF[3] - P[1][4]*SPP[0] + P[13][4]*SPP[3] + SF[3]*(P[5][0] + P[0][0]*SF[2] + P[2][0]*SF[1] + P[3][0]*SF[3] - P[1][0]*SPP[0] + P[13][0]*SPP[3]) + SF[1]*(P[5][1] + P[0][1]*SF[2] + P[2][1]*SF[1] + P[3][1]*SF[3] - P[1][1]*SPP[0] + P[13][1]*SPP[3]) + SPP[0]*(P[5][2] + P[0][2]*SF[2] + P[2][2]*SF[1] + P[3][2]*SF[3] - P[1][2]*SPP[0] + P[13][2]*SPP[3]) - SPP[2]*(P[5][3] + P[0][3]*SF[2] + P[2][3]*SF[1] + P[3][3]*SF[3] - P[1][3]*SPP[0] + P[13][3]*SPP[3]) - SPP[4]*(P[5][13] + P[0][13]*SF[2] + P[2][13]*SF[1] + P[3][13]*SF[3] - P[1][13]*SPP[0] + P[13][13]*SPP[3]);
+    nextP[5][5] = P[5][5] + P[0][5]*SF[2] + P[2][5]*SF[1] + P[3][5]*SF[3] - P[1][5]*SPP[0] + P[13][5]*SPP[3] + dvxCov*sq(SG[7] + 2*q0*q3) + dvzCov*sq(SG[5] - 2*q0*q1) + SF[2]*(P[5][0] + P[0][0]*SF[2] + P[2][0]*SF[1] + P[3][0]*SF[3] - P[1][0]*SPP[0] + P[13][0]*SPP[3]) + SF[1]*(P[5][2] + P[0][2]*SF[2] + P[2][2]*SF[1] + P[3][2]*SF[3] - P[1][2]*SPP[0] + P[13][2]*SPP[3]) + SF[3]*(P[5][3] + P[0][3]*SF[2] + P[2][3]*SF[1] + P[3][3]*SF[3] - P[1][3]*SPP[0] + P[13][3]*SPP[3]) - SPP[0]*(P[5][1] + P[0][1]*SF[2] + P[2][1]*SF[1] + P[3][1]*SF[3] - P[1][1]*SPP[0] + P[13][1]*SPP[3]) + SPP[3]*(P[5][13] + P[0][13]*SF[2] + P[2][13]*SF[1] + P[3][13]*SF[3] - P[1][13]*SPP[0] + P[13][13]*SPP[3]) + dvyCov*sq(SG[1] - SG[2] + SG[3] - SG[4]);
+    nextP[5][6] = P[5][6] + SQ[0] + P[0][6]*SF[2] + P[2][6]*SF[1] + P[3][6]*SF[3] - P[1][6]*SPP[0] + P[13][6]*SPP[3] + SF[2]*(P[5][1] + P[0][1]*SF[2] + P[2][1]*SF[1] + P[3][1]*SF[3] - P[1][1]*SPP[0] + P[13][1]*SPP[3]) + SF[1]*(P[5][3] + P[0][3]*SF[2] + P[2][3]*SF[1] + P[3][3]*SF[3] - P[1][3]*SPP[0] + P[13][3]*SPP[3]) + SPP[0]*(P[5][0] + P[0][0]*SF[2] + P[2][0]*SF[1] + P[3][0]*SF[3] - P[1][0]*SPP[0] + P[13][0]*SPP[3]) - SPP[1]*(P[5][2] + P[0][2]*SF[2] + P[2][2]*SF[1] + P[3][2]*SF[3] - P[1][2]*SPP[0] + P[13][2]*SPP[3]) - (sq(q0) - sq(q1) - sq(q2) + sq(q3))*(P[5][13] + P[0][13]*SF[2] + P[2][13]*SF[1] + P[3][13]*SF[3] - P[1][13]*SPP[0] + P[13][13]*SPP[3]);
+    nextP[5][7] = P[5][7] + P[0][7]*SF[2] + P[2][7]*SF[1] + P[3][7]*SF[3] - P[1][7]*SPP[0] + P[13][7]*SPP[3] + dt*(P[5][4] + P[0][4]*SF[2] + P[2][4]*SF[1] + P[3][4]*SF[3] - P[1][4]*SPP[0] + P[13][4]*SPP[3]);
+    nextP[5][8] = P[5][8] + P[0][8]*SF[2] + P[2][8]*SF[1] + P[3][8]*SF[3] - P[1][8]*SPP[0] + P[13][8]*SPP[3] + dt*(P[5][5] + P[0][5]*SF[2] + P[2][5]*SF[1] + P[3][5]*SF[3] - P[1][5]*SPP[0] + P[13][5]*SPP[3]);
+    nextP[5][9] = P[5][9] + P[0][9]*SF[2] + P[2][9]*SF[1] + P[3][9]*SF[3] - P[1][9]*SPP[0] + P[13][9]*SPP[3] + dt*(P[5][6] + P[0][6]*SF[2] + P[2][6]*SF[1] + P[3][6]*SF[3] - P[1][6]*SPP[0] + P[13][6]*SPP[3]);
+    nextP[5][10] = P[5][10] + P[0][10]*SF[2] + P[2][10]*SF[1] + P[3][10]*SF[3] - P[1][10]*SPP[0] + P[13][10]*SPP[3];
+    nextP[5][11] = P[5][11] + P[0][11]*SF[2] + P[2][11]*SF[1] + P[3][11]*SF[3] - P[1][11]*SPP[0] + P[13][11]*SPP[3];
+    nextP[5][12] = P[5][12] + P[0][12]*SF[2] + P[2][12]*SF[1] + P[3][12]*SF[3] - P[1][12]*SPP[0] + P[13][12]*SPP[3];
+    nextP[5][13] = P[5][13] + P[0][13]*SF[2] + P[2][13]*SF[1] + P[3][13]*SF[3] - P[1][13]*SPP[0] + P[13][13]*SPP[3];
+    nextP[5][14] = P[5][14] + P[0][14]*SF[2] + P[2][14]*SF[1] + P[3][14]*SF[3] - P[1][14]*SPP[0] + P[13][14]*SPP[3];
+    nextP[5][15] = P[5][15] + P[0][15]*SF[2] + P[2][15]*SF[1] + P[3][15]*SF[3] - P[1][15]*SPP[0] + P[13][15]*SPP[3];
+    nextP[5][16] = P[5][16] + P[0][16]*SF[2] + P[2][16]*SF[1] + P[3][16]*SF[3] - P[1][16]*SPP[0] + P[13][16]*SPP[3];
+    nextP[5][17] = P[5][17] + P[0][17]*SF[2] + P[2][17]*SF[1] + P[3][17]*SF[3] - P[1][17]*SPP[0] + P[13][17]*SPP[3];
+    nextP[5][18] = P[5][18] + P[0][18]*SF[2] + P[2][18]*SF[1] + P[3][18]*SF[3] - P[1][18]*SPP[0] + P[13][18]*SPP[3];
+    nextP[5][19] = P[5][19] + P[0][19]*SF[2] + P[2][19]*SF[1] + P[3][19]*SF[3] - P[1][19]*SPP[0] + P[13][19]*SPP[3];
+    nextP[5][20] = P[5][20] + P[0][20]*SF[2] + P[2][20]*SF[1] + P[3][20]*SF[3] - P[1][20]*SPP[0] + P[13][20]*SPP[3];
+    nextP[5][21] = P[5][21] + P[0][21]*SF[2] + P[2][21]*SF[1] + P[3][21]*SF[3] - P[1][21]*SPP[0] + P[13][21]*SPP[3];
+    nextP[5][22] = P[5][22] + P[0][22]*SF[2] + P[2][22]*SF[1] + P[3][22]*SF[3] - P[1][22]*SPP[0] + P[13][22]*SPP[3];
+    nextP[6][0] = P[6][0] + P[1][0]*SF[2] + P[3][0]*SF[1] + P[0][0]*SPP[0] - P[2][0]*SPP[1] - P[13][0]*(sq(q0) - sq(q1) - sq(q2) + sq(q3)) + SF[7]*(P[6][1] + P[1][1]*SF[2] + P[3][1]*SF[1] + P[0][1]*SPP[0] - P[2][1]*SPP[1] - P[13][1]*(sq(q0) - sq(q1) - sq(q2) + sq(q3))) + SF[9]*(P[6][2] + P[1][2]*SF[2] + P[3][2]*SF[1] + P[0][2]*SPP[0] - P[2][2]*SPP[1] - P[13][2]*(sq(q0) - sq(q1) - sq(q2) + sq(q3))) + SF[8]*(P[6][3] + P[1][3]*SF[2] + P[3][3]*SF[1] + P[0][3]*SPP[0] - P[2][3]*SPP[1] - P[13][3]*(sq(q0) - sq(q1) - sq(q2) + sq(q3))) + SF[11]*(P[6][10] + P[1][10]*SF[2] + P[3][10]*SF[1] + P[0][10]*SPP[0] - P[2][10]*SPP[1] - P[13][10]*(sq(q0) - sq(q1) - sq(q2) + sq(q3))) + SPP[7]*(P[6][11] + P[1][11]*SF[2] + P[3][11]*SF[1] + P[0][11]*SPP[0] - P[2][11]*SPP[1] - P[13][11]*(sq(q0) - sq(q1) - sq(q2) + sq(q3))) + SPP[6]*(P[6][12] + P[1][12]*SF[2] + P[3][12]*SF[1] + P[0][12]*SPP[0] - P[2][12]*SPP[1] - P[13][12]*(sq(q0) - sq(q1) - sq(q2) + sq(q3)));
+    nextP[6][1] = P[6][1] + P[1][1]*SF[2] + P[3][1]*SF[1] + P[0][1]*SPP[0] - P[2][1]*SPP[1] - P[13][1]*(sq(q0) - sq(q1) - sq(q2) + sq(q3)) + SF[6]*(P[6][0] + P[1][0]*SF[2] + P[3][0]*SF[1] + P[0][0]*SPP[0] - P[2][0]*SPP[1] - P[13][0]*(sq(q0) - sq(q1) - sq(q2) + sq(q3))) + SF[5]*(P[6][2] + P[1][2]*SF[2] + P[3][2]*SF[1] + P[0][2]*SPP[0] - P[2][2]*SPP[1] - P[13][2]*(sq(q0) - sq(q1) - sq(q2) + sq(q3))) + SF[9]*(P[6][3] + P[1][3]*SF[2] + P[3][3]*SF[1] + P[0][3]*SPP[0] - P[2][3]*SPP[1] - P[13][3]*(sq(q0) - sq(q1) - sq(q2) + sq(q3))) + SPP[6]*(P[6][11] + P[1][11]*SF[2] + P[3][11]*SF[1] + P[0][11]*SPP[0] - P[2][11]*SPP[1] - P[13][11]*(sq(q0) - sq(q1) - sq(q2) + sq(q3))) - SPP[7]*(P[6][12] + P[1][12]*SF[2] + P[3][12]*SF[1] + P[0][12]*SPP[0] - P[2][12]*SPP[1] - P[13][12]*(sq(q0) - sq(q1) - sq(q2) + sq(q3))) - (q0*(P[6][10] + P[1][10]*SF[2] + P[3][10]*SF[1] + P[0][10]*SPP[0] - P[2][10]*SPP[1] - P[13][10]*(sq(q0) - sq(q1) - sq(q2) + sq(q3))))/2;
+    nextP[6][2] = P[6][2] + P[1][2]*SF[2] + P[3][2]*SF[1] + P[0][2]*SPP[0] - P[2][2]*SPP[1] - P[13][2]*(sq(q0) - sq(q1) - sq(q2) + sq(q3)) + SF[4]*(P[6][0] + P[1][0]*SF[2] + P[3][0]*SF[1] + P[0][0]*SPP[0] - P[2][0]*SPP[1] - P[13][0]*(sq(q0) - sq(q1) - sq(q2) + sq(q3))) + SF[8]*(P[6][1] + P[1][1]*SF[2] + P[3][1]*SF[1] + P[0][1]*SPP[0] - P[2][1]*SPP[1] - P[13][1]*(sq(q0) - sq(q1) - sq(q2) + sq(q3))) + SF[6]*(P[6][3] + P[1][3]*SF[2] + P[3][3]*SF[1] + P[0][3]*SPP[0] - P[2][3]*SPP[1] - P[13][3]*(sq(q0) - sq(q1) - sq(q2) + sq(q3))) + SF[11]*(P[6][12] + P[1][12]*SF[2] + P[3][12]*SF[1] + P[0][12]*SPP[0] - P[2][12]*SPP[1] - P[13][12]*(sq(q0) - sq(q1) - sq(q2) + sq(q3))) - SPP[6]*(P[6][10] + P[1][10]*SF[2] + P[3][10]*SF[1] + P[0][10]*SPP[0] - P[2][10]*SPP[1] - P[13][10]*(sq(q0) - sq(q1) - sq(q2) + sq(q3))) - (q0*(P[6][11] + P[1][11]*SF[2] + P[3][11]*SF[1] + P[0][11]*SPP[0] - P[2][11]*SPP[1] - P[13][11]*(sq(q0) - sq(q1) - sq(q2) + sq(q3))))/2;
+    nextP[6][3] = P[6][3] + P[1][3]*SF[2] + P[3][3]*SF[1] + P[0][3]*SPP[0] - P[2][3]*SPP[1] - P[13][3]*(sq(q0) - sq(q1) - sq(q2) + sq(q3)) + SF[5]*(P[6][0] + P[1][0]*SF[2] + P[3][0]*SF[1] + P[0][0]*SPP[0] - P[2][0]*SPP[1] - P[13][0]*(sq(q0) - sq(q1) - sq(q2) + sq(q3))) + SF[4]*(P[6][1] + P[1][1]*SF[2] + P[3][1]*SF[1] + P[0][1]*SPP[0] - P[2][1]*SPP[1] - P[13][1]*(sq(q0) - sq(q1) - sq(q2) + sq(q3))) + SF[7]*(P[6][2] + P[1][2]*SF[2] + P[3][2]*SF[1] + P[0][2]*SPP[0] - P[2][2]*SPP[1] - P[13][2]*(sq(q0) - sq(q1) - sq(q2) + sq(q3))) - SF[11]*(P[6][11] + P[1][11]*SF[2] + P[3][11]*SF[1] + P[0][11]*SPP[0] - P[2][11]*SPP[1] - P[13][11]*(sq(q0) - sq(q1) - sq(q2) + sq(q3))) + SPP[7]*(P[6][10] + P[1][10]*SF[2] + P[3][10]*SF[1] + P[0][10]*SPP[0] - P[2][10]*SPP[1] - P[13][10]*(sq(q0) - sq(q1) - sq(q2) + sq(q3))) - (q0*(P[6][12] + P[1][12]*SF[2] + P[3][12]*SF[1] + P[0][12]*SPP[0] - P[2][12]*SPP[1] - P[13][12]*(sq(q0) - sq(q1) - sq(q2) + sq(q3))))/2;
+    nextP[6][4] = P[6][4] + SQ[1] + P[1][4]*SF[2] + P[3][4]*SF[1] + P[0][4]*SPP[0] - P[2][4]*SPP[1] - P[13][4]*(sq(q0) - sq(q1) - sq(q2) + sq(q3)) + SF[3]*(P[6][0] + P[1][0]*SF[2] + P[3][0]*SF[1] + P[0][0]*SPP[0] - P[2][0]*SPP[1] - P[13][0]*(sq(q0) - sq(q1) - sq(q2) + sq(q3))) + SF[1]*(P[6][1] + P[1][1]*SF[2] + P[3][1]*SF[1] + P[0][1]*SPP[0] - P[2][1]*SPP[1] - P[13][1]*(sq(q0) - sq(q1) - sq(q2) + sq(q3))) + SPP[0]*(P[6][2] + P[1][2]*SF[2] + P[3][2]*SF[1] + P[0][2]*SPP[0] - P[2][2]*SPP[1] - P[13][2]*(sq(q0) - sq(q1) - sq(q2) + sq(q3))) - SPP[2]*(P[6][3] + P[1][3]*SF[2] + P[3][3]*SF[1] + P[0][3]*SPP[0] - P[2][3]*SPP[1] - P[13][3]*(sq(q0) - sq(q1) - sq(q2) + sq(q3))) - SPP[4]*(P[6][13] + P[1][13]*SF[2] + P[3][13]*SF[1] + P[0][13]*SPP[0] - P[2][13]*SPP[1] - P[13][13]*(sq(q0) - sq(q1) - sq(q2) + sq(q3)));
+    nextP[6][5] = P[6][5] + SQ[0] + P[1][5]*SF[2] + P[3][5]*SF[1] + P[0][5]*SPP[0] - P[2][5]*SPP[1] - P[13][5]*(sq(q0) - sq(q1) - sq(q2) + sq(q3)) + SF[2]*(P[6][0] + P[1][0]*SF[2] + P[3][0]*SF[1] + P[0][0]*SPP[0] - P[2][0]*SPP[1] - P[13][0]*(sq(q0) - sq(q1) - sq(q2) + sq(q3))) + SF[1]*(P[6][2] + P[1][2]*SF[2] + P[3][2]*SF[1] + P[0][2]*SPP[0] - P[2][2]*SPP[1] - P[13][2]*(sq(q0) - sq(q1) - sq(q2) + sq(q3))) + SF[3]*(P[6][3] + P[1][3]*SF[2] + P[3][3]*SF[1] + P[0][3]*SPP[0] - P[2][3]*SPP[1] - P[13][3]*(sq(q0) - sq(q1) - sq(q2) + sq(q3))) - SPP[0]*(P[6][1] + P[1][1]*SF[2] + P[3][1]*SF[1] + P[0][1]*SPP[0] - P[2][1]*SPP[1] - P[13][1]*(sq(q0) - sq(q1) - sq(q2) + sq(q3))) + SPP[3]*(P[6][13] + P[1][13]*SF[2] + P[3][13]*SF[1] + P[0][13]*SPP[0] - P[2][13]*SPP[1] - P[13][13]*(sq(q0) - sq(q1) - sq(q2) + sq(q3)));
+    nextP[6][6] = P[6][6] + P[1][6]*SF[2] + P[3][6]*SF[1] + P[0][6]*SPP[0] - P[2][6]*SPP[1] - P[13][6]*(sq(q0) - sq(q1) - sq(q2) + sq(q3)) + dvxCov*sq(SG[6] - 2*q0*q2) + dvyCov*sq(SG[5] + 2*q0*q1) - SPP[5]*(P[6][13] + P[1][13]*SF[2] + P[3][13]*SF[1] + P[0][13]*SPP[0] - P[2][13]*SPP[1] - P[13][13]*SPP[5]) + SF[2]*(P[6][1] + P[1][1]*SF[2] + P[3][1]*SF[1] + P[0][1]*SPP[0] - P[2][1]*SPP[1] - P[13][1]*(sq(q0) - sq(q1) - sq(q2) + sq(q3))) + SF[1]*(P[6][3] + P[1][3]*SF[2] + P[3][3]*SF[1] + P[0][3]*SPP[0] - P[2][3]*SPP[1] - P[13][3]*(sq(q0) - sq(q1) - sq(q2) + sq(q3))) + SPP[0]*(P[6][0] + P[1][0]*SF[2] + P[3][0]*SF[1] + P[0][0]*SPP[0] - P[2][0]*SPP[1] - P[13][0]*(sq(q0) - sq(q1) - sq(q2) + sq(q3))) - SPP[1]*(P[6][2] + P[1][2]*SF[2] + P[3][2]*SF[1] + P[0][2]*SPP[0] - P[2][2]*SPP[1] - P[13][2]*(sq(q0) - sq(q1) - sq(q2) + sq(q3))) + dvzCov*sq(SG[1] - SG[2] - SG[3] + SG[4]);
+    nextP[6][7] = P[6][7] + P[1][7]*SF[2] + P[3][7]*SF[1] + P[0][7]*SPP[0] - P[2][7]*SPP[1] - P[13][7]*SPP[5] + dt*(P[6][4] + P[1][4]*SF[2] + P[3][4]*SF[1] + P[0][4]*SPP[0] - P[2][4]*SPP[1] - P[13][4]*SPP[5]);
+    nextP[6][8] = P[6][8] + P[1][8]*SF[2] + P[3][8]*SF[1] + P[0][8]*SPP[0] - P[2][8]*SPP[1] - P[13][8]*SPP[5] + dt*(P[6][5] + P[1][5]*SF[2] + P[3][5]*SF[1] + P[0][5]*SPP[0] - P[2][5]*SPP[1] - P[13][5]*SPP[5]);
+    nextP[6][9] = P[6][9] + P[1][9]*SF[2] + P[3][9]*SF[1] + P[0][9]*SPP[0] - P[2][9]*SPP[1] - P[13][9]*SPP[5] + dt*(P[6][6] + P[1][6]*SF[2] + P[3][6]*SF[1] + P[0][6]*SPP[0] - P[2][6]*SPP[1] - P[13][6]*SPP[5]);
+    nextP[6][10] = P[6][10] + P[1][10]*SF[2] + P[3][10]*SF[1] + P[0][10]*SPP[0] - P[2][10]*SPP[1] - P[13][10]*SPP[5];
+    nextP[6][11] = P[6][11] + P[1][11]*SF[2] + P[3][11]*SF[1] + P[0][11]*SPP[0] - P[2][11]*SPP[1] - P[13][11]*SPP[5];
+    nextP[6][12] = P[6][12] + P[1][12]*SF[2] + P[3][12]*SF[1] + P[0][12]*SPP[0] - P[2][12]*SPP[1] - P[13][12]*SPP[5];
+    nextP[6][13] = P[6][13] + P[1][13]*SF[2] + P[3][13]*SF[1] + P[0][13]*SPP[0] - P[2][13]*SPP[1] - P[13][13]*SPP[5];
+    nextP[6][14] = P[6][14] + P[1][14]*SF[2] + P[3][14]*SF[1] + P[0][14]*SPP[0] - P[2][14]*SPP[1] - P[13][14]*SPP[5];
+    nextP[6][15] = P[6][15] + P[1][15]*SF[2] + P[3][15]*SF[1] + P[0][15]*SPP[0] - P[2][15]*SPP[1] - P[13][15]*SPP[5];
+    nextP[6][16] = P[6][16] + P[1][16]*SF[2] + P[3][16]*SF[1] + P[0][16]*SPP[0] - P[2][16]*SPP[1] - P[13][16]*SPP[5];
+    nextP[6][17] = P[6][17] + P[1][17]*SF[2] + P[3][17]*SF[1] + P[0][17]*SPP[0] - P[2][17]*SPP[1] - P[13][17]*SPP[5];
+    nextP[6][18] = P[6][18] + P[1][18]*SF[2] + P[3][18]*SF[1] + P[0][18]*SPP[0] - P[2][18]*SPP[1] - P[13][18]*SPP[5];
+    nextP[6][19] = P[6][19] + P[1][19]*SF[2] + P[3][19]*SF[1] + P[0][19]*SPP[0] - P[2][19]*SPP[1] - P[13][19]*SPP[5];
+    nextP[6][20] = P[6][20] + P[1][20]*SF[2] + P[3][20]*SF[1] + P[0][20]*SPP[0] - P[2][20]*SPP[1] - P[13][20]*SPP[5];
+    nextP[6][21] = P[6][21] + P[1][21]*SF[2] + P[3][21]*SF[1] + P[0][21]*SPP[0] - P[2][21]*SPP[1] - P[13][21]*SPP[5];
+    nextP[6][22] = P[6][22] + P[1][22]*SF[2] + P[3][22]*SF[1] + P[0][22]*SPP[0] - P[2][22]*SPP[1] - P[13][22]*SPP[5];
+    nextP[7][0] = P[7][0] + P[4][0]*dt + SF[7]*(P[7][1] + P[4][1]*dt) + SF[9]*(P[7][2] + P[4][2]*dt) + SF[8]*(P[7][3] + P[4][3]*dt) + SF[11]*(P[7][10] + P[4][10]*dt) + SPP[7]*(P[7][11] + P[4][11]*dt) + SPP[6]*(P[7][12] + P[4][12]*dt);
+    nextP[7][1] = P[7][1] + P[4][1]*dt + SF[6]*(P[7][0] + P[4][0]*dt) + SF[5]*(P[7][2] + P[4][2]*dt) + SF[9]*(P[7][3] + P[4][3]*dt) + SPP[6]*(P[7][11] + P[4][11]*dt) - SPP[7]*(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[4]*(P[7][0] + P[4][0]*dt) + SF[8]*(P[7][1] + P[4][1]*dt) + SF[6]*(P[7][3] + P[4][3]*dt) + SF[11]*(P[7][12] + P[4][12]*dt) - SPP[6]*(P[7][10] + P[4][10]*dt) - (q0*(P[7][11] + P[4][11]*dt))/2;
+    nextP[7][3] = P[7][3] + P[4][3]*dt + SF[5]*(P[7][0] + P[4][0]*dt) + SF[4]*(P[7][1] + P[4][1]*dt) + SF[7]*(P[7][2] + P[4][2]*dt) - SF[11]*(P[7][11] + P[4][11]*dt) + SPP[7]*(P[7][10] + P[4][10]*dt) - (q0*(P[7][12] + P[4][12]*dt))/2;
+    nextP[7][4] = P[7][4] + P[4][4]*dt + SF[1]*(P[7][1] + P[4][1]*dt) + SF[3]*(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) - SPP[4]*(P[7][13] + P[4][13]*dt);
+    nextP[7][5] = P[7][5] + P[4][5]*dt + SF[2]*(P[7][0] + P[4][0]*dt) + SF[1]*(P[7][2] + P[4][2]*dt) + SF[3]*(P[7][3] + P[4][3]*dt) - SPP[0]*(P[7][1] + P[4][1]*dt) + SPP[3]*(P[7][13] + P[4][13]*dt);
+    nextP[7][6] = P[7][6] + P[4][6]*dt + SF[2]*(P[7][1] + P[4][1]*dt) + SF[1]*(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) - SPP[5]*(P[7][13] + P[4][13]*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[7][21] = P[7][21] + P[4][21]*dt;
+    nextP[7][22] = P[7][22] + P[4][22]*dt;
+    nextP[8][0] = P[8][0] + P[5][0]*dt + SF[7]*(P[8][1] + P[5][1]*dt) + SF[9]*(P[8][2] + P[5][2]*dt) + SF[8]*(P[8][3] + P[5][3]*dt) + SF[11]*(P[8][10] + P[5][10]*dt) + SPP[7]*(P[8][11] + P[5][11]*dt) + SPP[6]*(P[8][12] + P[5][12]*dt);
+    nextP[8][1] = P[8][1] + P[5][1]*dt + SF[6]*(P[8][0] + P[5][0]*dt) + SF[5]*(P[8][2] + P[5][2]*dt) + SF[9]*(P[8][3] + P[5][3]*dt) + SPP[6]*(P[8][11] + P[5][11]*dt) - SPP[7]*(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[4]*(P[8][0] + P[5][0]*dt) + SF[8]*(P[8][1] + P[5][1]*dt) + SF[6]*(P[8][3] + P[5][3]*dt) + SF[11]*(P[8][12] + P[5][12]*dt) - SPP[6]*(P[8][10] + P[5][10]*dt) - (q0*(P[8][11] + P[5][11]*dt))/2;
+    nextP[8][3] = P[8][3] + P[5][3]*dt + SF[5]*(P[8][0] + P[5][0]*dt) + SF[4]*(P[8][1] + P[5][1]*dt) + SF[7]*(P[8][2] + P[5][2]*dt) - SF[11]*(P[8][11] + P[5][11]*dt) + SPP[7]*(P[8][10] + P[5][10]*dt) - (q0*(P[8][12] + P[5][12]*dt))/2;
+    nextP[8][4] = P[8][4] + P[5][4]*dt + SF[1]*(P[8][1] + P[5][1]*dt) + SF[3]*(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) - SPP[4]*(P[8][13] + P[5][13]*dt);
+    nextP[8][5] = P[8][5] + P[5][5]*dt + SF[2]*(P[8][0] + P[5][0]*dt) + SF[1]*(P[8][2] + P[5][2]*dt) + SF[3]*(P[8][3] + P[5][3]*dt) - SPP[0]*(P[8][1] + P[5][1]*dt) + SPP[3]*(P[8][13] + P[5][13]*dt);
+    nextP[8][6] = P[8][6] + P[5][6]*dt + SF[2]*(P[8][1] + P[5][1]*dt) + SF[1]*(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) - SPP[5]*(P[8][13] + P[5][13]*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[8][21] = P[8][21] + P[5][21]*dt;
+    nextP[8][22] = P[8][22] + P[5][22]*dt;
+    nextP[9][0] = P[9][0] + P[6][0]*dt + SF[7]*(P[9][1] + P[6][1]*dt) + SF[9]*(P[9][2] + P[6][2]*dt) + SF[8]*(P[9][3] + P[6][3]*dt) + SF[11]*(P[9][10] + P[6][10]*dt) + SPP[7]*(P[9][11] + P[6][11]*dt) + SPP[6]*(P[9][12] + P[6][12]*dt);
+    nextP[9][1] = P[9][1] + P[6][1]*dt + SF[6]*(P[9][0] + P[6][0]*dt) + SF[5]*(P[9][2] + P[6][2]*dt) + SF[9]*(P[9][3] + P[6][3]*dt) + SPP[6]*(P[9][11] + P[6][11]*dt) - SPP[7]*(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[4]*(P[9][0] + P[6][0]*dt) + SF[8]*(P[9][1] + P[6][1]*dt) + SF[6]*(P[9][3] + P[6][3]*dt) + SF[11]*(P[9][12] + P[6][12]*dt) - SPP[6]*(P[9][10] + P[6][10]*dt) - (q0*(P[9][11] + P[6][11]*dt))/2;
+    nextP[9][3] = P[9][3] + P[6][3]*dt + SF[5]*(P[9][0] + P[6][0]*dt) + SF[4]*(P[9][1] + P[6][1]*dt) + SF[7]*(P[9][2] + P[6][2]*dt) - SF[11]*(P[9][11] + P[6][11]*dt) + SPP[7]*(P[9][10] + P[6][10]*dt) - (q0*(P[9][12] + P[6][12]*dt))/2;
+    nextP[9][4] = P[9][4] + P[6][4]*dt + SF[1]*(P[9][1] + P[6][1]*dt) + SF[3]*(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) - SPP[4]*(P[9][13] + P[6][13]*dt);
+    nextP[9][5] = P[9][5] + P[6][5]*dt + SF[2]*(P[9][0] + P[6][0]*dt) + SF[1]*(P[9][2] + P[6][2]*dt) + SF[3]*(P[9][3] + P[6][3]*dt) - SPP[0]*(P[9][1] + P[6][1]*dt) + SPP[3]*(P[9][13] + P[6][13]*dt);
+    nextP[9][6] = P[9][6] + P[6][6]*dt + SF[2]*(P[9][1] + P[6][1]*dt) + SF[1]*(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) - SPP[5]*(P[9][13] + P[6][13]*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[9][21] = P[9][21] + P[6][21]*dt;
+    nextP[9][22] = P[9][22] + P[6][22]*dt;
+    nextP[10][0] = P[10][0] + P[10][1]*SF[7] + P[10][2]*SF[9] + P[10][3]*SF[8] + P[10][10]*SF[11] + P[10][11]*SPP[7] + P[10][12]*SPP[6];
+    nextP[10][1] = P[10][1] + P[10][0]*SF[6] + P[10][2]*SF[5] + P[10][3]*SF[9] + P[10][11]*SPP[6] - P[10][12]*SPP[7] - (P[10][10]*q0)/2;
+    nextP[10][2] = P[10][2] + P[10][0]*SF[4] + P[10][1]*SF[8] + P[10][3]*SF[6] + P[10][12]*SF[11] - P[10][10]*SPP[6] - (P[10][11]*q0)/2;
+    nextP[10][3] = P[10][3] + P[10][0]*SF[5] + P[10][1]*SF[4] + P[10][2]*SF[7] - P[10][11]*SF[11] + P[10][10]*SPP[7] - (P[10][12]*q0)/2;
+    nextP[10][4] = P[10][4] + P[10][1]*SF[1] + P[10][0]*SF[3] + P[10][2]*SPP[0] - P[10][3]*SPP[2] - P[10][13]*SPP[4];
+    nextP[10][5] = P[10][5] + P[10][0]*SF[2] + P[10][2]*SF[1] + P[10][3]*SF[3] - P[10][1]*SPP[0] + P[10][13]*SPP[3];
+    nextP[10][6] = P[10][6] + P[10][1]*SF[2] + P[10][3]*SF[1] + P[10][0]*SPP[0] - P[10][2]*SPP[1] - P[10][13]*SPP[5];
+    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[10][21] = P[10][21];
+    nextP[10][22] = P[10][22];
+    nextP[11][0] = P[11][0] + P[11][1]*SF[7] + P[11][2]*SF[9] + P[11][3]*SF[8] + P[11][10]*SF[11] + P[11][11]*SPP[7] + P[11][12]*SPP[6];
+    nextP[11][1] = P[11][1] + P[11][0]*SF[6] + P[11][2]*SF[5] + P[11][3]*SF[9] + P[11][11]*SPP[6] - P[11][12]*SPP[7] - (P[11][10]*q0)/2;
+    nextP[11][2] = P[11][2] + P[11][0]*SF[4] + P[11][1]*SF[8] + P[11][3]*SF[6] + P[11][12]*SF[11] - P[11][10]*SPP[6] - (P[11][11]*q0)/2;
+    nextP[11][3] = P[11][3] + P[11][0]*SF[5] + P[11][1]*SF[4] + P[11][2]*SF[7] - P[11][11]*SF[11] + P[11][10]*SPP[7] - (P[11][12]*q0)/2;
+    nextP[11][4] = P[11][4] + P[11][1]*SF[1] + P[11][0]*SF[3] + P[11][2]*SPP[0] - P[11][3]*SPP[2] - P[11][13]*SPP[4];
+    nextP[11][5] = P[11][5] + P[11][0]*SF[2] + P[11][2]*SF[1] + P[11][3]*SF[3] - P[11][1]*SPP[0] + P[11][13]*SPP[3];
+    nextP[11][6] = P[11][6] + P[11][1]*SF[2] + P[11][3]*SF[1] + P[11][0]*SPP[0] - P[11][2]*SPP[1] - P[11][13]*SPP[5];
+    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[11][21] = P[11][21];
+    nextP[11][22] = P[11][22];
+    nextP[12][0] = P[12][0] + P[12][1]*SF[7] + P[12][2]*SF[9] + P[12][3]*SF[8] + P[12][10]*SF[11] + P[12][11]*SPP[7] + P[12][12]*SPP[6];
+    nextP[12][1] = P[12][1] + P[12][0]*SF[6] + P[12][2]*SF[5] + P[12][3]*SF[9] + P[12][11]*SPP[6] - P[12][12]*SPP[7] - (P[12][10]*q0)/2;
+    nextP[12][2] = P[12][2] + P[12][0]*SF[4] + P[12][1]*SF[8] + P[12][3]*SF[6] + P[12][12]*SF[11] - P[12][10]*SPP[6] - (P[12][11]*q0)/2;
+    nextP[12][3] = P[12][3] + P[12][0]*SF[5] + P[12][1]*SF[4] + P[12][2]*SF[7] - P[12][11]*SF[11] + P[12][10]*SPP[7] - (P[12][12]*q0)/2;
+    nextP[12][4] = P[12][4] + P[12][1]*SF[1] + P[12][0]*SF[3] + P[12][2]*SPP[0] - P[12][3]*SPP[2] - P[12][13]*SPP[4];
+    nextP[12][5] = P[12][5] + P[12][0]*SF[2] + P[12][2]*SF[1] + P[12][3]*SF[3] - P[12][1]*SPP[0] + P[12][13]*SPP[3];
+    nextP[12][6] = P[12][6] + P[12][1]*SF[2] + P[12][3]*SF[1] + P[12][0]*SPP[0] - P[12][2]*SPP[1] - P[12][13]*SPP[5];
+    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[12][21] = P[12][21];
+    nextP[12][22] = P[12][22];
+    nextP[13][0] = P[13][0] + P[13][1]*SF[7] + P[13][2]*SF[9] + P[13][3]*SF[8] + P[13][10]*SF[11] + P[13][11]*SPP[7] + P[13][12]*SPP[6];
+    nextP[13][1] = P[13][1] + P[13][0]*SF[6] + P[13][2]*SF[5] + P[13][3]*SF[9] + P[13][11]*SPP[6] - P[13][12]*SPP[7] - (P[13][10]*q0)/2;
+    nextP[13][2] = P[13][2] + P[13][0]*SF[4] + P[13][1]*SF[8] + P[13][3]*SF[6] + P[13][12]*SF[11] - P[13][10]*SPP[6] - (P[13][11]*q0)/2;
+    nextP[13][3] = P[13][3] + P[13][0]*SF[5] + P[13][1]*SF[4] + P[13][2]*SF[7] - P[13][11]*SF[11] + P[13][10]*SPP[7] - (P[13][12]*q0)/2;
+    nextP[13][4] = P[13][4] + P[13][1]*SF[1] + P[13][0]*SF[3] + P[13][2]*SPP[0] - P[13][3]*SPP[2] - P[13][13]*SPP[4];
+    nextP[13][5] = P[13][5] + P[13][0]*SF[2] + P[13][2]*SF[1] + P[13][3]*SF[3] - P[13][1]*SPP[0] + P[13][13]*SPP[3];
+    nextP[13][6] = P[13][6] + P[13][1]*SF[2] + P[13][3]*SF[1] + P[13][0]*SPP[0] - P[13][2]*SPP[1] - P[13][13]*SPP[5];
+    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[13][21] = P[13][21];
+    nextP[13][22] = P[13][22];
+    nextP[14][0] = P[14][0] + P[14][1]*SF[7] + P[14][2]*SF[9] + P[14][3]*SF[8] + P[14][10]*SF[11] + P[14][11]*SPP[7] + P[14][12]*SPP[6];
+    nextP[14][1] = P[14][1] + P[14][0]*SF[6] + P[14][2]*SF[5] + P[14][3]*SF[9] + P[14][11]*SPP[6] - P[14][12]*SPP[7] - (P[14][10]*q0)/2;
+    nextP[14][2] = P[14][2] + P[14][0]*SF[4] + P[14][1]*SF[8] + P[14][3]*SF[6] + P[14][12]*SF[11] - P[14][10]*SPP[6] - (P[14][11]*q0)/2;
+    nextP[14][3] = P[14][3] + P[14][0]*SF[5] + P[14][1]*SF[4] + P[14][2]*SF[7] - P[14][11]*SF[11] + P[14][10]*SPP[7] - (P[14][12]*q0)/2;
+    nextP[14][4] = P[14][4] + P[14][1]*SF[1] + P[14][0]*SF[3] + P[14][2]*SPP[0] - P[14][3]*SPP[2] - P[14][13]*SPP[4];
+    nextP[14][5] = P[14][5] + P[14][0]*SF[2] + P[14][2]*SF[1] + P[14][3]*SF[3] - P[14][1]*SPP[0] + P[14][13]*SPP[3];
+    nextP[14][6] = P[14][6] + P[14][1]*SF[2] + P[14][3]*SF[1] + P[14][0]*SPP[0] - P[14][2]*SPP[1] - P[14][13]*SPP[5];
+    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[14][21] = P[14][21];
+    nextP[14][22] = P[14][22];
+    nextP[15][0] = P[15][0] + P[15][1]*SF[7] + P[15][2]*SF[9] + P[15][3]*SF[8] + P[15][10]*SF[11] + P[15][11]*SPP[7] + P[15][12]*SPP[6];
+    nextP[15][1] = P[15][1] + P[15][0]*SF[6] + P[15][2]*SF[5] + P[15][3]*SF[9] + P[15][11]*SPP[6] - P[15][12]*SPP[7] - (P[15][10]*q0)/2;
+    nextP[15][2] = P[15][2] + P[15][0]*SF[4] + P[15][1]*SF[8] + P[15][3]*SF[6] + P[15][12]*SF[11] - P[15][10]*SPP[6] - (P[15][11]*q0)/2;
+    nextP[15][3] = P[15][3] + P[15][0]*SF[5] + P[15][1]*SF[4] + P[15][2]*SF[7] - P[15][11]*SF[11] + P[15][10]*SPP[7] - (P[15][12]*q0)/2;
+    nextP[15][4] = P[15][4] + P[15][1]*SF[1] + P[15][0]*SF[3] + P[15][2]*SPP[0] - P[15][3]*SPP[2] - P[15][13]*SPP[4];
+    nextP[15][5] = P[15][5] + P[15][0]*SF[2] + P[15][2]*SF[1] + P[15][3]*SF[3] - P[15][1]*SPP[0] + P[15][13]*SPP[3];
+    nextP[15][6] = P[15][6] + P[15][1]*SF[2] + P[15][3]*SF[1] + P[15][0]*SPP[0] - P[15][2]*SPP[1] - P[15][13]*SPP[5];
+    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[15][21] = P[15][21];
+    nextP[15][22] = P[15][22];
+    nextP[16][0] = P[16][0] + P[16][1]*SF[7] + P[16][2]*SF[9] + P[16][3]*SF[8] + P[16][10]*SF[11] + P[16][11]*SPP[7] + P[16][12]*SPP[6];
+    nextP[16][1] = P[16][1] + P[16][0]*SF[6] + P[16][2]*SF[5] + P[16][3]*SF[9] + P[16][11]*SPP[6] - P[16][12]*SPP[7] - (P[16][10]*q0)/2;
+    nextP[16][2] = P[16][2] + P[16][0]*SF[4] + P[16][1]*SF[8] + P[16][3]*SF[6] + P[16][12]*SF[11] - P[16][10]*SPP[6] - (P[16][11]*q0)/2;
+    nextP[16][3] = P[16][3] + P[16][0]*SF[5] + P[16][1]*SF[4] + P[16][2]*SF[7] - P[16][11]*SF[11] + P[16][10]*SPP[7] - (P[16][12]*q0)/2;
+    nextP[16][4] = P[16][4] + P[16][1]*SF[1] + P[16][0]*SF[3] + P[16][2]*SPP[0] - P[16][3]*SPP[2] - P[16][13]*SPP[4];
+    nextP[16][5] = P[16][5] + P[16][0]*SF[2] + P[16][2]*SF[1] + P[16][3]*SF[3] - P[16][1]*SPP[0] + P[16][13]*SPP[3];
+    nextP[16][6] = P[16][6] + P[16][1]*SF[2] + P[16][3]*SF[1] + P[16][0]*SPP[0] - P[16][2]*SPP[1] - P[16][13]*SPP[5];
+    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[16][21] = P[16][21];
+    nextP[16][22] = P[16][22];
+    nextP[17][0] = P[17][0] + P[17][1]*SF[7] + P[17][2]*SF[9] + P[17][3]*SF[8] + P[17][10]*SF[11] + P[17][11]*SPP[7] + P[17][12]*SPP[6];
+    nextP[17][1] = P[17][1] + P[17][0]*SF[6] + P[17][2]*SF[5] + P[17][3]*SF[9] + P[17][11]*SPP[6] - P[17][12]*SPP[7] - (P[17][10]*q0)/2;
+    nextP[17][2] = P[17][2] + P[17][0]*SF[4] + P[17][1]*SF[8] + P[17][3]*SF[6] + P[17][12]*SF[11] - P[17][10]*SPP[6] - (P[17][11]*q0)/2;
+    nextP[17][3] = P[17][3] + P[17][0]*SF[5] + P[17][1]*SF[4] + P[17][2]*SF[7] - P[17][11]*SF[11] + P[17][10]*SPP[7] - (P[17][12]*q0)/2;
+    nextP[17][4] = P[17][4] + P[17][1]*SF[1] + P[17][0]*SF[3] + P[17][2]*SPP[0] - P[17][3]*SPP[2] - P[17][13]*SPP[4];
+    nextP[17][5] = P[17][5] + P[17][0]*SF[2] + P[17][2]*SF[1] + P[17][3]*SF[3] - P[17][1]*SPP[0] + P[17][13]*SPP[3];
+    nextP[17][6] = P[17][6] + P[17][1]*SF[2] + P[17][3]*SF[1] + P[17][0]*SPP[0] - P[17][2]*SPP[1] - P[17][13]*SPP[5];
+    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[17][21] = P[17][21];
+    nextP[17][22] = P[17][22];
+    nextP[18][0] = P[18][0] + P[18][1]*SF[7] + P[18][2]*SF[9] + P[18][3]*SF[8] + P[18][10]*SF[11] + P[18][11]*SPP[7] + P[18][12]*SPP[6];
+    nextP[18][1] = P[18][1] + P[18][0]*SF[6] + P[18][2]*SF[5] + P[18][3]*SF[9] + P[18][11]*SPP[6] - P[18][12]*SPP[7] - (P[18][10]*q0)/2;
+    nextP[18][2] = P[18][2] + P[18][0]*SF[4] + P[18][1]*SF[8] + P[18][3]*SF[6] + P[18][12]*SF[11] - P[18][10]*SPP[6] - (P[18][11]*q0)/2;
+    nextP[18][3] = P[18][3] + P[18][0]*SF[5] + P[18][1]*SF[4] + P[18][2]*SF[7] - P[18][11]*SF[11] + P[18][10]*SPP[7] - (P[18][12]*q0)/2;
+    nextP[18][4] = P[18][4] + P[18][1]*SF[1] + P[18][0]*SF[3] + P[18][2]*SPP[0] - P[18][3]*SPP[2] - P[18][13]*SPP[4];
+    nextP[18][5] = P[18][5] + P[18][0]*SF[2] + P[18][2]*SF[1] + P[18][3]*SF[3] - P[18][1]*SPP[0] + P[18][13]*SPP[3];
+    nextP[18][6] = P[18][6] + P[18][1]*SF[2] + P[18][3]*SF[1] + P[18][0]*SPP[0] - P[18][2]*SPP[1] - P[18][13]*SPP[5];
+    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[18][21] = P[18][21];
+    nextP[18][22] = P[18][22];
+    nextP[19][0] = P[19][0] + P[19][1]*SF[7] + P[19][2]*SF[9] + P[19][3]*SF[8] + P[19][10]*SF[11] + P[19][11]*SPP[7] + P[19][12]*SPP[6];
+    nextP[19][1] = P[19][1] + P[19][0]*SF[6] + P[19][2]*SF[5] + P[19][3]*SF[9] + P[19][11]*SPP[6] - P[19][12]*SPP[7] - (P[19][10]*q0)/2;
+    nextP[19][2] = P[19][2] + P[19][0]*SF[4] + P[19][1]*SF[8] + P[19][3]*SF[6] + P[19][12]*SF[11] - P[19][10]*SPP[6] - (P[19][11]*q0)/2;
+    nextP[19][3] = P[19][3] + P[19][0]*SF[5] + P[19][1]*SF[4] + P[19][2]*SF[7] - P[19][11]*SF[11] + P[19][10]*SPP[7] - (P[19][12]*q0)/2;
+    nextP[19][4] = P[19][4] + P[19][1]*SF[1] + P[19][0]*SF[3] + P[19][2]*SPP[0] - P[19][3]*SPP[2] - P[19][13]*SPP[4];
+    nextP[19][5] = P[19][5] + P[19][0]*SF[2] + P[19][2]*SF[1] + P[19][3]*SF[3] - P[19][1]*SPP[0] + P[19][13]*SPP[3];
+    nextP[19][6] = P[19][6] + P[19][1]*SF[2] + P[19][3]*SF[1] + P[19][0]*SPP[0] - P[19][2]*SPP[1] - P[19][13]*SPP[5];
+    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[19][21] = P[19][21];
+    nextP[19][22] = P[19][22];
+    nextP[20][0] = P[20][0] + P[20][1]*SF[7] + P[20][2]*SF[9] + P[20][3]*SF[8] + P[20][10]*SF[11] + P[20][11]*SPP[7] + P[20][12]*SPP[6];
+    nextP[20][1] = P[20][1] + P[20][0]*SF[6] + P[20][2]*SF[5] + P[20][3]*SF[9] + P[20][11]*SPP[6] - P[20][12]*SPP[7] - (P[20][10]*q0)/2;
+    nextP[20][2] = P[20][2] + P[20][0]*SF[4] + P[20][1]*SF[8] + P[20][3]*SF[6] + P[20][12]*SF[11] - P[20][10]*SPP[6] - (P[20][11]*q0)/2;
+    nextP[20][3] = P[20][3] + P[20][0]*SF[5] + P[20][1]*SF[4] + P[20][2]*SF[7] - P[20][11]*SF[11] + P[20][10]*SPP[7] - (P[20][12]*q0)/2;
+    nextP[20][4] = P[20][4] + P[20][1]*SF[1] + P[20][0]*SF[3] + P[20][2]*SPP[0] - P[20][3]*SPP[2] - P[20][13]*SPP[4];
+    nextP[20][5] = P[20][5] + P[20][0]*SF[2] + P[20][2]*SF[1] + P[20][3]*SF[3] - P[20][1]*SPP[0] + P[20][13]*SPP[3];
+    nextP[20][6] = P[20][6] + P[20][1]*SF[2] + P[20][3]*SF[1] + P[20][0]*SPP[0] - P[20][2]*SPP[1] - P[20][13]*SPP[5];
+    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];
+    nextP[20][21] = P[20][21];
+    nextP[20][22] = P[20][22];
+    nextP[21][0] = P[21][0] + P[21][1]*SF[7] + P[21][2]*SF[9] + P[21][3]*SF[8] + P[21][10]*SF[11] + P[21][11]*SPP[7] + P[21][12]*SPP[6];
+    nextP[21][1] = P[21][1] + P[21][0]*SF[6] + P[21][2]*SF[5] + P[21][3]*SF[9] + P[21][11]*SPP[6] - P[21][12]*SPP[7] - (P[21][10]*q0)/2;
+    nextP[21][2] = P[21][2] + P[21][0]*SF[4] + P[21][1]*SF[8] + P[21][3]*SF[6] + P[21][12]*SF[11] - P[21][10]*SPP[6] - (P[21][11]*q0)/2;
+    nextP[21][3] = P[21][3] + P[21][0]*SF[5] + P[21][1]*SF[4] + P[21][2]*SF[7] - P[21][11]*SF[11] + P[21][10]*SPP[7] - (P[21][12]*q0)/2;
+    nextP[21][4] = P[21][4] + P[21][1]*SF[1] + P[21][0]*SF[3] + P[21][2]*SPP[0] - P[21][3]*SPP[2] - P[21][13]*SPP[4];
+    nextP[21][5] = P[21][5] + P[21][0]*SF[2] + P[21][2]*SF[1] + P[21][3]*SF[3] - P[21][1]*SPP[0] + P[21][13]*SPP[3];
+    nextP[21][6] = P[21][6] + P[21][1]*SF[2] + P[21][3]*SF[1] + P[21][0]*SPP[0] - P[21][2]*SPP[1] - P[21][13]*SPP[5];
+    nextP[21][7] = P[21][7] + P[21][4]*dt;
+    nextP[21][8] = P[21][8] + P[21][5]*dt;
+    nextP[21][9] = P[21][9] + P[21][6]*dt;
+    nextP[21][10] = P[21][10];
+    nextP[21][11] = P[21][11];
+    nextP[21][12] = P[21][12];
+    nextP[21][13] = P[21][13];
+    nextP[21][14] = P[21][14];
+    nextP[21][15] = P[21][15];
+    nextP[21][16] = P[21][16];
+    nextP[21][17] = P[21][17];
+    nextP[21][18] = P[21][18];
+    nextP[21][19] = P[21][19];
+    nextP[21][20] = P[21][20];
+    nextP[21][21] = P[21][21];
+    nextP[21][22] = P[21][22];
+    nextP[22][0] = P[22][0] + P[22][1]*SF[7] + P[22][2]*SF[9] + P[22][3]*SF[8] + P[22][10]*SF[11] + P[22][11]*SPP[7] + P[22][12]*SPP[6];
+    nextP[22][1] = P[22][1] + P[22][0]*SF[6] + P[22][2]*SF[5] + P[22][3]*SF[9] + P[22][11]*SPP[6] - P[22][12]*SPP[7] - (P[22][10]*q0)/2;
+    nextP[22][2] = P[22][2] + P[22][0]*SF[4] + P[22][1]*SF[8] + P[22][3]*SF[6] + P[22][12]*SF[11] - P[22][10]*SPP[6] - (P[22][11]*q0)/2;
+    nextP[22][3] = P[22][3] + P[22][0]*SF[5] + P[22][1]*SF[4] + P[22][2]*SF[7] - P[22][11]*SF[11] + P[22][10]*SPP[7] - (P[22][12]*q0)/2;
+    nextP[22][4] = P[22][4] + P[22][1]*SF[1] + P[22][0]*SF[3] + P[22][2]*SPP[0] - P[22][3]*SPP[2] - P[22][13]*SPP[4];
+    nextP[22][5] = P[22][5] + P[22][0]*SF[2] + P[22][2]*SF[1] + P[22][3]*SF[3] - P[22][1]*SPP[0] + P[22][13]*SPP[3];
+    nextP[22][6] = P[22][6] + P[22][1]*SF[2] + P[22][3]*SF[1] + P[22][0]*SPP[0] - P[22][2]*SPP[1] - P[22][13]*SPP[5];
+    nextP[22][7] = P[22][7] + P[22][4]*dt;
+    nextP[22][8] = P[22][8] + P[22][5]*dt;
+    nextP[22][9] = P[22][9] + P[22][6]*dt;
+    nextP[22][10] = P[22][10];
+    nextP[22][11] = P[22][11];
+    nextP[22][12] = P[22][12];
+    nextP[22][13] = P[22][13];
+    nextP[22][14] = P[22][14];
+    nextP[22][15] = P[22][15];
+    nextP[22][16] = P[22][16];
+    nextP[22][17] = P[22][17];
+    nextP[22][18] = P[22][18];
+    nextP[22][19] = P[22][19];
+    nextP[22][20] = P[22][20];
+    nextP[22][21] = P[22][21];
+    nextP[22][22] = P[22][22];
+
+    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,16,21);
+        zeroCols(nextP,16,21);
+    }
+
+    // If on ground or not using airspeed sensing, inhibit wind velocity
+    // covariance growth.
+    if (onGround || !useAirspeed)
+    {
+        zeroRows(nextP,14,15);
+        zeroCols(nextP,14,15);
+    }
+
+    // If on ground, inhibit terrain offset updates by
+    // setting the coresponding covariance terms to zero.
+    if (onGround)
+    {
+        zeroRows(nextP,22,22);
+        zeroCols(nextP,22,22);
+    }
+
+    // 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 <= 13; 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 > 13) || (j > 13)) {
+                        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] = sq(vneSigma) + sq(velErr);
+        R_OBS[1] = R_OBS[0];
+        R_OBS[2] = sq(vdSigma) + sq(velErr);
+        R_OBS[3] = sq(posNeSigma) + sq(posErr);
+        R_OBS[4] = R_OBS[3];
+        R_OBS[5] = sq(posDSigma) + sq(posErr);
+
+        // 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 = 22;
+        }
+        else
+        {
+            indexLimit = 13;
+        }
+        // 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;
+                }
+
+                // Don't update Z accel bias state unless using a height observation (GPS velocities can be biased)
+                if (obsIndex != 5) {
+                    Kfusion[13] = 0;
+                }
+
+                // 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();
+
+}
+
+void AttPosEKF::FuseMagnetometer()
+{
+
+    float &q0 = magstate.q0;
+    float &q1 = magstate.q1;
+    float &q2 = magstate.q2;
+    float &q3 = magstate.q3;
+    float &magN = magstate.magN;
+    float &magE = magstate.magE;
+    float &magD = magstate.magD;
+    float &magXbias = magstate.magXbias;
+    float &magYbias = magstate.magYbias;
+    float &magZbias = magstate.magZbias;
+    unsigned &obsIndex = magstate.obsIndex;
+    Mat3f &DCM = magstate.DCM;
+    float *MagPred = &magstate.MagPred[0];
+    float &R_MAG = magstate.R_MAG;
+    float *SH_MAG = &magstate.SH_MAG[0];
+
+    float SK_MX[6];
+    float SK_MY[5];
+    float SK_MZ[6];
+    float H_MAG[n_states];
+    for (uint8_t i = 0; i < n_states; i++) {
+        H_MAG[i] = 0.0f;
+    }
+    unsigned indexLimit;
+
+// 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 = n_states;
+        }
+        else
+        {
+            indexLimit = 13 + 1;
+        }
+
+        // Sequential fusion of XYZ components to spread processing load across
+        // three prediction time steps.
+
+        // Calculate observation jacobians and Kalman gains
+        if (fuseMagData)
+        {
+            // Copy required states to local variable names
+            q0       = statesAtMagMeasTime[0];
+            q1       = statesAtMagMeasTime[1];
+            q2       = statesAtMagMeasTime[2];
+            q3       = statesAtMagMeasTime[3];
+            magN     = statesAtMagMeasTime[16];
+            magE     = statesAtMagMeasTime[17];
+            magD     = statesAtMagMeasTime[18];
+            magXbias = statesAtMagMeasTime[19];
+            magYbias = statesAtMagMeasTime[20];
+            magZbias = statesAtMagMeasTime[21];
+
+            // rotate predicted earth components into body axes and calculate
+            // predicted measurments
+            DCM.x.x = q0*q0 + q1*q1 - q2*q2 - q3*q3;
+            DCM.x.y = 2*(q1*q2 + q0*q3);
+            DCM.x.z = 2*(q1*q3-q0*q2);
+            DCM.y.x = 2*(q1*q2 - q0*q3);
+            DCM.y.y = q0*q0 - q1*q1 + q2*q2 - q3*q3;
+            DCM.y.z = 2*(q2*q3 + q0*q1);
+            DCM.z.x = 2*(q1*q3 + q0*q2);
+            DCM.z.y = 2*(q2*q3 - q0*q1);
+            DCM.z.z = q0*q0 - q1*q1 - q2*q2 + q3*q3;
+            MagPred[0] = DCM.x.x*magN + DCM.x.y*magE  + DCM.x.z*magD + magXbias;
+            MagPred[1] = DCM.y.x*magN + DCM.y.y*magE  + DCM.y.z*magD + magYbias;
+            MagPred[2] = DCM.z.x*magN + DCM.z.y*magE  + DCM.z.z*magD + magZbias;
+
+            // scale magnetometer observation error with total angular rate
+            R_MAG = sq(magMeasurementSigma) + 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 < n_states; 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[16] = SH_MAG[5] - SH_MAG[4] - SH_MAG[3] + SH_MAG[6];
+            H_MAG[17] = 2*q0*q3 + 2*q1*q2;
+            H_MAG[18] = 2*q1*q3 - 2*q0*q2;
+            H_MAG[19] = 1.0f;
+
+            // Calculate Kalman gain
+            float temp = (P[19][19] + R_MAG + P[1][19]*SH_MAG[0] + P[3][19]*SH_MAG[2] - P[16][19]*(SH_MAG[3] + SH_MAG[4] - SH_MAG[5] - SH_MAG[6]) - (2*magD*q0 - 2*magE*q1 + 2*magN*q2)*(P[19][2] + P[1][2]*SH_MAG[0] + P[3][2]*SH_MAG[2] - P[16][2]*(SH_MAG[3] + SH_MAG[4] - SH_MAG[5] - SH_MAG[6]) + P[17][2]*(2*q0*q3 + 2*q1*q2) - P[18][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[19][0] + P[1][0]*SH_MAG[0] + P[3][0]*SH_MAG[2] - P[16][0]*(SH_MAG[3] + SH_MAG[4] - SH_MAG[5] - SH_MAG[6]) + P[17][0]*(2*q0*q3 + 2*q1*q2) - P[18][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[19][1] + P[1][1]*SH_MAG[0] + P[3][1]*SH_MAG[2] - P[16][1]*(SH_MAG[3] + SH_MAG[4] - SH_MAG[5] - SH_MAG[6]) + P[17][1]*(2*q0*q3 + 2*q1*q2) - P[18][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[19][3] + P[1][3]*SH_MAG[0] + P[3][3]*SH_MAG[2] - P[16][3]*(SH_MAG[3] + SH_MAG[4] - SH_MAG[5] - SH_MAG[6]) + P[17][3]*(2*q0*q3 + 2*q1*q2) - P[18][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[19][16] + P[1][16]*SH_MAG[0] + P[3][16]*SH_MAG[2] - P[16][16]*(SH_MAG[3] + SH_MAG[4] - SH_MAG[5] - SH_MAG[6]) + P[17][16]*(2*q0*q3 + 2*q1*q2) - P[18][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)) + P[17][19]*(2*q0*q3 + 2*q1*q2) - P[18][19]*(2*q0*q2 - 2*q1*q3) - P[2][19]*(2*magD*q0 - 2*magE*q1 + 2*magN*q2) + (2*q0*q3 + 2*q1*q2)*(P[19][17] + P[1][17]*SH_MAG[0] + P[3][17]*SH_MAG[2] - P[16][17]*(SH_MAG[3] + SH_MAG[4] - SH_MAG[5] - SH_MAG[6]) + P[17][17]*(2*q0*q3 + 2*q1*q2) - P[18][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)) - (2*q0*q2 - 2*q1*q3)*(P[19][18] + P[1][18]*SH_MAG[0] + P[3][18]*SH_MAG[2] - P[16][18]*(SH_MAG[3] + SH_MAG[4] - SH_MAG[5] - SH_MAG[6]) + P[17][18]*(2*q0*q3 + 2*q1*q2) - P[18][18]*(2*q0*q2 - 2*q1*q3) - P[2][18]*(2*magD*q0 - 2*magE*q1 + 2*magN*q2) + P[0][18]*(SH_MAG[7] + SH_MAG[8] - 2*magD*q2)) + P[0][19]*(SH_MAG[7] + SH_MAG[8] - 2*magD*q2));
+            if (temp >= R_MAG) {
+                SK_MX[0] = 1.0f / temp;
+            } else {
+                // the calculation is badly conditioned, so we cannot perform fusion on this step
+                // we increase the state variances and try again next time
+                P[19][19] += 0.1f*R_MAG;
+                obsIndex = 1;
+                return;
+            }
+            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][19] + 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][16]*SK_MX[1] + P[0][17]*SK_MX[5] - P[0][18]*SK_MX[4]);
+            Kfusion[1] = SK_MX[0]*(P[1][19] + 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][16]*SK_MX[1] + P[1][17]*SK_MX[5] - P[1][18]*SK_MX[4]);
+            Kfusion[2] = SK_MX[0]*(P[2][19] + 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][16]*SK_MX[1] + P[2][17]*SK_MX[5] - P[2][18]*SK_MX[4]);
+            Kfusion[3] = SK_MX[0]*(P[3][19] + 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][16]*SK_MX[1] + P[3][17]*SK_MX[5] - P[3][18]*SK_MX[4]);
+            Kfusion[4] = SK_MX[0]*(P[4][19] + 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][16]*SK_MX[1] + P[4][17]*SK_MX[5] - P[4][18]*SK_MX[4]);
+            Kfusion[5] = SK_MX[0]*(P[5][19] + 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][16]*SK_MX[1] + P[5][17]*SK_MX[5] - P[5][18]*SK_MX[4]);
+            Kfusion[6] = SK_MX[0]*(P[6][19] + 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][16]*SK_MX[1] + P[6][17]*SK_MX[5] - P[6][18]*SK_MX[4]);
+            Kfusion[7] = SK_MX[0]*(P[7][19] + 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][16]*SK_MX[1] + P[7][17]*SK_MX[5] - P[7][18]*SK_MX[4]);
+            Kfusion[8] = SK_MX[0]*(P[8][19] + 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][16]*SK_MX[1] + P[8][17]*SK_MX[5] - P[8][18]*SK_MX[4]);
+            Kfusion[9] = SK_MX[0]*(P[9][19] + 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][16]*SK_MX[1] + P[9][17]*SK_MX[5] - P[9][18]*SK_MX[4]);
+            Kfusion[10] = SK_MX[0]*(P[10][19] + 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][16]*SK_MX[1] + P[10][17]*SK_MX[5] - P[10][18]*SK_MX[4]);
+            Kfusion[11] = SK_MX[0]*(P[11][19] + 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][16]*SK_MX[1] + P[11][17]*SK_MX[5] - P[11][18]*SK_MX[4]);
+            Kfusion[12] = SK_MX[0]*(P[12][19] + 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][16]*SK_MX[1] + P[12][17]*SK_MX[5] - P[12][18]*SK_MX[4]);
+            // Only height measurements are allowed to modify the Z delta velocity bias state. This improves the stability of the estimate
+            Kfusion[13] = 0.0f;//SK_MX[0]*(P[13][19] + 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][16]*SK_MX[1] + P[13][17]*SK_MX[5] - P[13][18]*SK_MX[4]);
+            Kfusion[14] = SK_MX[0]*(P[14][19] + 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][16]*SK_MX[1] + P[14][17]*SK_MX[5] - P[14][18]*SK_MX[4]);
+            Kfusion[15] = SK_MX[0]*(P[15][19] + 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][16]*SK_MX[1] + P[15][17]*SK_MX[5] - P[15][18]*SK_MX[4]);
+            Kfusion[16] = SK_MX[0]*(P[16][19] + 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][16]*SK_MX[1] + P[16][17]*SK_MX[5] - P[16][18]*SK_MX[4]);
+            Kfusion[17] = SK_MX[0]*(P[17][19] + 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][16]*SK_MX[1] + P[17][17]*SK_MX[5] - P[17][18]*SK_MX[4]);
+            Kfusion[18] = SK_MX[0]*(P[18][19] + 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][16]*SK_MX[1] + P[18][17]*SK_MX[5] - P[18][18]*SK_MX[4]);
+            Kfusion[19] = SK_MX[0]*(P[19][19] + 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][16]*SK_MX[1] + P[19][17]*SK_MX[5] - P[19][18]*SK_MX[4]);
+            Kfusion[20] = SK_MX[0]*(P[20][19] + 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][16]*SK_MX[1] + P[20][17]*SK_MX[5] - P[20][18]*SK_MX[4]);
+            Kfusion[21] = SK_MX[0]*(P[21][19] + P[21][1]*SH_MAG[0] + P[21][3]*SH_MAG[2] + P[21][0]*SK_MX[3] - P[21][2]*SK_MX[2] - P[21][16]*SK_MX[1] + P[21][17]*SK_MX[5] - P[21][18]*SK_MX[4]);
+            Kfusion[22] = SK_MX[0]*(P[22][19] + P[22][1]*SH_MAG[0] + P[22][3]*SH_MAG[2] + P[22][0]*SK_MX[3] - P[22][2]*SK_MX[2] - P[22][16]*SK_MX[1] + P[22][17]*SK_MX[5] - P[22][18]*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;
+            fuseMagData = false;
+        }
+        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[16] = 2*q1*q2 - 2*q0*q3;
+            H_MAG[17] = SH_MAG[4] - SH_MAG[3] - SH_MAG[5] + SH_MAG[6];
+            H_MAG[18] = 2*q0*q1 + 2*q2*q3;
+            H_MAG[20] = 1;
+
+            // Calculate Kalman gain
+            float temp = (P[20][20] + R_MAG + P[0][20]*SH_MAG[2] + P[1][20]*SH_MAG[1] + P[2][20]*SH_MAG[0] - P[17][20]*(SH_MAG[3] - SH_MAG[4] + SH_MAG[5] - SH_MAG[6]) - (2*q0*q3 - 2*q1*q2)*(P[20][16] + P[0][16]*SH_MAG[2] + P[1][16]*SH_MAG[1] + P[2][16]*SH_MAG[0] - P[17][16]*(SH_MAG[3] - SH_MAG[4] + SH_MAG[5] - SH_MAG[6]) - P[16][16]*(2*q0*q3 - 2*q1*q2) + P[18][16]*(2*q0*q1 + 2*q2*q3) - P[3][16]*(SH_MAG[7] + SH_MAG[8] - 2*magD*q2)) + (2*q0*q1 + 2*q2*q3)*(P[20][18] + P[0][18]*SH_MAG[2] + P[1][18]*SH_MAG[1] + P[2][18]*SH_MAG[0] - P[17][18]*(SH_MAG[3] - SH_MAG[4] + SH_MAG[5] - SH_MAG[6]) - P[16][18]*(2*q0*q3 - 2*q1*q2) + P[18][18]*(2*q0*q1 + 2*q2*q3) - P[3][18]*(SH_MAG[7] + SH_MAG[8] - 2*magD*q2)) - (SH_MAG[7] + SH_MAG[8] - 2*magD*q2)*(P[20][3] + P[0][3]*SH_MAG[2] + P[1][3]*SH_MAG[1] + P[2][3]*SH_MAG[0] - P[17][3]*(SH_MAG[3] - SH_MAG[4] + SH_MAG[5] - SH_MAG[6]) - P[16][3]*(2*q0*q3 - 2*q1*q2) + P[18][3]*(2*q0*q1 + 2*q2*q3) - P[3][3]*(SH_MAG[7] + SH_MAG[8] - 2*magD*q2)) - P[16][20]*(2*q0*q3 - 2*q1*q2) + P[18][20]*(2*q0*q1 + 2*q2*q3) + SH_MAG[2]*(P[20][0] + P[0][0]*SH_MAG[2] + P[1][0]*SH_MAG[1] + P[2][0]*SH_MAG[0] - P[17][0]*(SH_MAG[3] - SH_MAG[4] + SH_MAG[5] - SH_MAG[6]) - P[16][0]*(2*q0*q3 - 2*q1*q2) + P[18][0]*(2*q0*q1 + 2*q2*q3) - P[3][0]*(SH_MAG[7] + SH_MAG[8] - 2*magD*q2)) + SH_MAG[1]*(P[20][1] + P[0][1]*SH_MAG[2] + P[1][1]*SH_MAG[1] + P[2][1]*SH_MAG[0] - P[17][1]*(SH_MAG[3] - SH_MAG[4] + SH_MAG[5] - SH_MAG[6]) - P[16][1]*(2*q0*q3 - 2*q1*q2) + P[18][1]*(2*q0*q1 + 2*q2*q3) - P[3][1]*(SH_MAG[7] + SH_MAG[8] - 2*magD*q2)) + SH_MAG[0]*(P[20][2] + P[0][2]*SH_MAG[2] + P[1][2]*SH_MAG[1] + P[2][2]*SH_MAG[0] - P[17][2]*(SH_MAG[3] - SH_MAG[4] + SH_MAG[5] - SH_MAG[6]) - P[16][2]*(2*q0*q3 - 2*q1*q2) + P[18][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[20][17] + P[0][17]*SH_MAG[2] + P[1][17]*SH_MAG[1] + P[2][17]*SH_MAG[0] - P[17][17]*(SH_MAG[3] - SH_MAG[4] + SH_MAG[5] - SH_MAG[6]) - P[16][17]*(2*q0*q3 - 2*q1*q2) + P[18][17]*(2*q0*q1 + 2*q2*q3) - P[3][17]*(SH_MAG[7] + SH_MAG[8] - 2*magD*q2)) - P[3][20]*(SH_MAG[7] + SH_MAG[8] - 2*magD*q2));
+            if (temp >= R_MAG) {
+                SK_MY[0] = 1.0f / temp;
+            } else {
+                // the calculation is badly conditioned, so we cannot perform fusion on this step
+                // we increase the state variances and try again next time
+                P[20][20] += 0.1f*R_MAG;
+                obsIndex = 2;
+                return;
+            }
+            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][20] + 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][17]*SK_MY[1] - P[0][16]*SK_MY[3] + P[0][18]*SK_MY[4]);
+            Kfusion[1] = SK_MY[0]*(P[1][20] + 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][17]*SK_MY[1] - P[1][16]*SK_MY[3] + P[1][18]*SK_MY[4]);
+            Kfusion[2] = SK_MY[0]*(P[2][20] + 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][17]*SK_MY[1] - P[2][16]*SK_MY[3] + P[2][18]*SK_MY[4]);
+            Kfusion[3] = SK_MY[0]*(P[3][20] + 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][17]*SK_MY[1] - P[3][16]*SK_MY[3] + P[3][18]*SK_MY[4]);
+            Kfusion[4] = SK_MY[0]*(P[4][20] + 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][17]*SK_MY[1] - P[4][16]*SK_MY[3] + P[4][18]*SK_MY[4]);
+            Kfusion[5] = SK_MY[0]*(P[5][20] + 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][17]*SK_MY[1] - P[5][16]*SK_MY[3] + P[5][18]*SK_MY[4]);
+            Kfusion[6] = SK_MY[0]*(P[6][20] + 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][17]*SK_MY[1] - P[6][16]*SK_MY[3] + P[6][18]*SK_MY[4]);
+            Kfusion[7] = SK_MY[0]*(P[7][20] + 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][17]*SK_MY[1] - P[7][16]*SK_MY[3] + P[7][18]*SK_MY[4]);
+            Kfusion[8] = SK_MY[0]*(P[8][20] + 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][17]*SK_MY[1] - P[8][16]*SK_MY[3] + P[8][18]*SK_MY[4]);
+            Kfusion[9] = SK_MY[0]*(P[9][20] + 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][17]*SK_MY[1] - P[9][16]*SK_MY[3] + P[9][18]*SK_MY[4]);
+            Kfusion[10] = SK_MY[0]*(P[10][20] + 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][17]*SK_MY[1] - P[10][16]*SK_MY[3] + P[10][18]*SK_MY[4]);
+            Kfusion[11] = SK_MY[0]*(P[11][20] + 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][17]*SK_MY[1] - P[11][16]*SK_MY[3] + P[11][18]*SK_MY[4]);
+            Kfusion[12] = SK_MY[0]*(P[12][20] + 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][17]*SK_MY[1] - P[12][16]*SK_MY[3] + P[12][18]*SK_MY[4]);
+            // Only height measurements are allowed to modify the Z delta velocity bias state. This improves the stability of the estimate
+            Kfusion[13] = 0.0f;//SK_MY[0]*(P[13][20] + 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][17]*SK_MY[1] - P[13][16]*SK_MY[3] + P[13][18]*SK_MY[4]);
+            Kfusion[14] = SK_MY[0]*(P[14][20] + 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][17]*SK_MY[1] - P[14][16]*SK_MY[3] + P[14][18]*SK_MY[4]);
+            Kfusion[15] = SK_MY[0]*(P[15][20] + 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][17]*SK_MY[1] - P[15][16]*SK_MY[3] + P[15][18]*SK_MY[4]);
+            Kfusion[16] = SK_MY[0]*(P[16][20] + 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][17]*SK_MY[1] - P[16][16]*SK_MY[3] + P[16][18]*SK_MY[4]);
+            Kfusion[17] = SK_MY[0]*(P[17][20] + 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][17]*SK_MY[1] - P[17][16]*SK_MY[3] + P[17][18]*SK_MY[4]);
+            Kfusion[18] = SK_MY[0]*(P[18][20] + 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][17]*SK_MY[1] - P[18][16]*SK_MY[3] + P[18][18]*SK_MY[4]);
+            Kfusion[19] = SK_MY[0]*(P[19][20] + 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][17]*SK_MY[1] - P[19][16]*SK_MY[3] + P[19][18]*SK_MY[4]);
+            Kfusion[20] = SK_MY[0]*(P[20][20] + 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][17]*SK_MY[1] - P[20][16]*SK_MY[3] + P[20][18]*SK_MY[4]);
+            Kfusion[21] = SK_MY[0]*(P[21][20] + P[21][0]*SH_MAG[2] + P[21][1]*SH_MAG[1] + P[21][2]*SH_MAG[0] - P[21][3]*SK_MY[2] - P[21][17]*SK_MY[1] - P[21][16]*SK_MY[3] + P[21][18]*SK_MY[4]);
+            Kfusion[22] = SK_MY[0]*(P[22][20] + P[22][0]*SH_MAG[2] + P[22][1]*SH_MAG[1] + P[22][2]*SH_MAG[0] - P[22][3]*SK_MY[2] - P[22][17]*SK_MY[1] - P[22][16]*SK_MY[3] + P[22][18]*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 < n_states; 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[16] = 2*q0*q2 + 2*q1*q3;
+            H_MAG[17] = 2*q2*q3 - 2*q0*q1;
+            H_MAG[18] = SH_MAG[3] - SH_MAG[4] - SH_MAG[5] + SH_MAG[6];
+            H_MAG[21] = 1;
+
+            // Calculate Kalman gain
+            float temp = (P[21][21] + R_MAG + P[0][21]*SH_MAG[1] + P[3][21]*SH_MAG[0] + P[18][21]*(SH_MAG[3] - SH_MAG[4] - SH_MAG[5] + SH_MAG[6]) - (2*magD*q1 + 2*magE*q0 - 2*magN*q3)*(P[21][1] + P[0][1]*SH_MAG[1] + P[3][1]*SH_MAG[0] + P[18][1]*(SH_MAG[3] - SH_MAG[4] - SH_MAG[5] + SH_MAG[6]) + P[16][1]*(2*q0*q2 + 2*q1*q3) - P[17][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[21][2] + P[0][2]*SH_MAG[1] + P[3][2]*SH_MAG[0] + P[18][2]*(SH_MAG[3] - SH_MAG[4] - SH_MAG[5] + SH_MAG[6]) + P[16][2]*(2*q0*q2 + 2*q1*q3) - P[17][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[21][0] + P[0][0]*SH_MAG[1] + P[3][0]*SH_MAG[0] + P[18][0]*(SH_MAG[3] - SH_MAG[4] - SH_MAG[5] + SH_MAG[6]) + P[16][0]*(2*q0*q2 + 2*q1*q3) - P[17][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[21][3] + P[0][3]*SH_MAG[1] + P[3][3]*SH_MAG[0] + P[18][3]*(SH_MAG[3] - SH_MAG[4] - SH_MAG[5] + SH_MAG[6]) + P[16][3]*(2*q0*q2 + 2*q1*q3) - P[17][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[21][18] + P[0][18]*SH_MAG[1] + P[3][18]*SH_MAG[0] + P[18][18]*(SH_MAG[3] - SH_MAG[4] - SH_MAG[5] + SH_MAG[6]) + P[16][18]*(2*q0*q2 + 2*q1*q3) - P[17][18]*(2*q0*q1 - 2*q2*q3) - P[1][18]*(2*magD*q1 + 2*magE*q0 - 2*magN*q3) + P[2][18]*(SH_MAG[7] + SH_MAG[8] - 2*magD*q2)) + P[16][21]*(2*q0*q2 + 2*q1*q3) - P[17][21]*(2*q0*q1 - 2*q2*q3) - P[1][21]*(2*magD*q1 + 2*magE*q0 - 2*magN*q3) + (2*q0*q2 + 2*q1*q3)*(P[21][16] + P[0][16]*SH_MAG[1] + P[3][16]*SH_MAG[0] + P[18][16]*(SH_MAG[3] - SH_MAG[4] - SH_MAG[5] + SH_MAG[6]) + P[16][16]*(2*q0*q2 + 2*q1*q3) - P[17][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)) - (2*q0*q1 - 2*q2*q3)*(P[21][17] + P[0][17]*SH_MAG[1] + P[3][17]*SH_MAG[0] + P[18][17]*(SH_MAG[3] - SH_MAG[4] - SH_MAG[5] + SH_MAG[6]) + P[16][17]*(2*q0*q2 + 2*q1*q3) - P[17][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[2][21]*(SH_MAG[7] + SH_MAG[8] - 2*magD*q2));
+            if (temp >= R_MAG) {
+                SK_MZ[0] = 1.0f / temp;
+            } else {
+                // the calculation is badly conditioned, so we cannot perform fusion on this step
+                // we increase the state variances and try again next time
+                P[21][21] += 0.1f*R_MAG;
+                obsIndex = 3;
+                return;
+            }
+            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][21] + 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][18]*SK_MZ[1] + P[0][16]*SK_MZ[5] - P[0][17]*SK_MZ[4]);
+            Kfusion[1] = SK_MZ[0]*(P[1][21] + 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][18]*SK_MZ[1] + P[1][16]*SK_MZ[5] - P[1][17]*SK_MZ[4]);
+            Kfusion[2] = SK_MZ[0]*(P[2][21] + 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][18]*SK_MZ[1] + P[2][16]*SK_MZ[5] - P[2][17]*SK_MZ[4]);
+            Kfusion[3] = SK_MZ[0]*(P[3][21] + 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][18]*SK_MZ[1] + P[3][16]*SK_MZ[5] - P[3][17]*SK_MZ[4]);
+            Kfusion[4] = SK_MZ[0]*(P[4][21] + 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][18]*SK_MZ[1] + P[4][16]*SK_MZ[5] - P[4][17]*SK_MZ[4]);
+            Kfusion[5] = SK_MZ[0]*(P[5][21] + 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][18]*SK_MZ[1] + P[5][16]*SK_MZ[5] - P[5][17]*SK_MZ[4]);
+            Kfusion[6] = SK_MZ[0]*(P[6][21] + 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][18]*SK_MZ[1] + P[6][16]*SK_MZ[5] - P[6][17]*SK_MZ[4]);
+            Kfusion[7] = SK_MZ[0]*(P[7][21] + 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][18]*SK_MZ[1] + P[7][16]*SK_MZ[5] - P[7][17]*SK_MZ[4]);
+            Kfusion[8] = SK_MZ[0]*(P[8][21] + 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][18]*SK_MZ[1] + P[8][16]*SK_MZ[5] - P[8][17]*SK_MZ[4]);
+            Kfusion[9] = SK_MZ[0]*(P[9][21] + 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][18]*SK_MZ[1] + P[9][16]*SK_MZ[5] - P[9][17]*SK_MZ[4]);
+            Kfusion[10] = SK_MZ[0]*(P[10][21] + 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][18]*SK_MZ[1] + P[10][16]*SK_MZ[5] - P[10][17]*SK_MZ[4]);
+            Kfusion[11] = SK_MZ[0]*(P[11][21] + 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][18]*SK_MZ[1] + P[11][16]*SK_MZ[5] - P[11][17]*SK_MZ[4]);
+            Kfusion[12] = SK_MZ[0]*(P[12][21] + 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][18]*SK_MZ[1] + P[12][16]*SK_MZ[5] - P[12][17]*SK_MZ[4]);
+            // Only height measurements are allowed to modify the Z delta velocity bias state. This improves the stability of the estimate
+            Kfusion[13] = 0.0f;//SK_MZ[0]*(P[13][21] + 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][18]*SK_MZ[1] + P[13][16]*SK_MZ[5] - P[13][17]*SK_MZ[4]);
+            Kfusion[14] = SK_MZ[0]*(P[14][21] + 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][18]*SK_MZ[1] + P[14][16]*SK_MZ[5] - P[14][17]*SK_MZ[4]);
+            Kfusion[15] = SK_MZ[0]*(P[15][21] + 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][18]*SK_MZ[1] + P[15][16]*SK_MZ[5] - P[15][17]*SK_MZ[4]);
+            Kfusion[16] = SK_MZ[0]*(P[16][21] + 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][18]*SK_MZ[1] + P[16][16]*SK_MZ[5] - P[16][17]*SK_MZ[4]);
+            Kfusion[17] = SK_MZ[0]*(P[17][21] + 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][18]*SK_MZ[1] + P[17][16]*SK_MZ[5] - P[17][17]*SK_MZ[4]);
+            Kfusion[18] = SK_MZ[0]*(P[18][21] + 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][18]*SK_MZ[1] + P[18][16]*SK_MZ[5] - P[18][17]*SK_MZ[4]);
+            Kfusion[19] = SK_MZ[0]*(P[19][21] + 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][18]*SK_MZ[1] + P[19][16]*SK_MZ[5] - P[19][17]*SK_MZ[4]);
+            Kfusion[20] = SK_MZ[0]*(P[20][21] + 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][18]*SK_MZ[1] + P[20][16]*SK_MZ[5] - P[20][17]*SK_MZ[4]);
+            Kfusion[21] = SK_MZ[0]*(P[21][21] + P[21][0]*SH_MAG[1] + P[21][3]*SH_MAG[0] - P[21][1]*SK_MZ[2] + P[21][2]*SK_MZ[3] + P[21][18]*SK_MZ[1] + P[21][16]*SK_MZ[5] - P[21][17]*SK_MZ[4]);
+            Kfusion[22] = SK_MZ[0]*(P[22][21] + P[22][0]*SH_MAG[1] + P[22][3]*SH_MAG[0] - P[22][1]*SK_MZ[2] + P[22][2]*SK_MZ[3] + P[22][18]*SK_MZ[1] + P[22][16]*SK_MZ[5] - P[22][17]*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 <= 15; j++) KH[i][j] = 0.0f;
+                if (!onGround)
+                {
+                    for (uint8_t j = 16; j <= 21; j++)
+                    {
+                        KH[i][j] = Kfusion[i] * H_MAG[j];
+                    }
+                }
+                else
+                {
+                    for (uint8_t j = 16; j <= 21; 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 = 16; k<=21; 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;
+    float R_TAS = sq(airspeedMeasurementSigma);
+    float SH_TAS[3];
+    float SK_TAS;
+    float VtasPred;
+
+    // Copy required states to local variable names
+    vn = statesAtVtasMeasTime[4];
+    ve = statesAtVtasMeasTime[5];
+    vd = statesAtVtasMeasTime[6];
+    vwn = statesAtVtasMeasTime[14];
+    vwe = statesAtVtasMeasTime[15];
+
+    // 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[n_states];
+        for (uint8_t i = 0; i < n_states; 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[14] = -SH_TAS[2];
+        H_TAS[15] = -SH_TAS[1];
+
+        // Calculate Kalman gains
+        float temp = (R_TAS + SH_TAS[2]*(P[4][4]*SH_TAS[2] + P[5][4]*SH_TAS[1] - P[14][4]*SH_TAS[2] - P[15][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[14][5]*SH_TAS[2] - P[15][5]*SH_TAS[1] + P[6][5]*vd*SH_TAS[0]) - SH_TAS[2]*(P[4][14]*SH_TAS[2] + P[5][14]*SH_TAS[1] - P[14][14]*SH_TAS[2] - P[15][14]*SH_TAS[1] + P[6][14]*vd*SH_TAS[0]) - SH_TAS[1]*(P[4][15]*SH_TAS[2] + P[5][15]*SH_TAS[1] - P[14][15]*SH_TAS[2] - P[15][15]*SH_TAS[1] + P[6][15]*vd*SH_TAS[0]) + vd*SH_TAS[0]*(P[4][6]*SH_TAS[2] + P[5][6]*SH_TAS[1] - P[14][6]*SH_TAS[2] - P[15][6]*SH_TAS[1] + P[6][6]*vd*SH_TAS[0]));
+        if (temp >= R_TAS) {
+            SK_TAS = 1.0f / temp;
+        } else {
+            // the calculation is badly conditioned, so we cannot perform fusion on this step
+            // we increase the wind state variances and try again next time
+            P[14][14] += 0.05f*R_TAS;
+            P[15][15] += 0.05f*R_TAS;
+            return;
+        }
+        Kfusion[0] = SK_TAS*(P[0][4]*SH_TAS[2] - P[0][14]*SH_TAS[2] + P[0][5]*SH_TAS[1] - P[0][15]*SH_TAS[1] + P[0][6]*vd*SH_TAS[0]);
+        Kfusion[1] = SK_TAS*(P[1][4]*SH_TAS[2] - P[1][14]*SH_TAS[2] + P[1][5]*SH_TAS[1] - P[1][15]*SH_TAS[1] + P[1][6]*vd*SH_TAS[0]);
+        Kfusion[2] = SK_TAS*(P[2][4]*SH_TAS[2] - P[2][14]*SH_TAS[2] + P[2][5]*SH_TAS[1] - P[2][15]*SH_TAS[1] + P[2][6]*vd*SH_TAS[0]);
+        Kfusion[3] = SK_TAS*(P[3][4]*SH_TAS[2] - P[3][14]*SH_TAS[2] + P[3][5]*SH_TAS[1] - P[3][15]*SH_TAS[1] + P[3][6]*vd*SH_TAS[0]);
+        Kfusion[4] = SK_TAS*(P[4][4]*SH_TAS[2] - P[4][14]*SH_TAS[2] + P[4][5]*SH_TAS[1] - P[4][15]*SH_TAS[1] + P[4][6]*vd*SH_TAS[0]);
+        Kfusion[5] = SK_TAS*(P[5][4]*SH_TAS[2] - P[5][14]*SH_TAS[2] + P[5][5]*SH_TAS[1] - P[5][15]*SH_TAS[1] + P[5][6]*vd*SH_TAS[0]);
+        Kfusion[6] = SK_TAS*(P[6][4]*SH_TAS[2] - P[6][14]*SH_TAS[2] + P[6][5]*SH_TAS[1] - P[6][15]*SH_TAS[1] + P[6][6]*vd*SH_TAS[0]);
+        Kfusion[7] = SK_TAS*(P[7][4]*SH_TAS[2] - P[7][14]*SH_TAS[2] + P[7][5]*SH_TAS[1] - P[7][15]*SH_TAS[1] + P[7][6]*vd*SH_TAS[0]);
+        Kfusion[8] = SK_TAS*(P[8][4]*SH_TAS[2] - P[8][14]*SH_TAS[2] + P[8][5]*SH_TAS[1] - P[8][15]*SH_TAS[1] + P[8][6]*vd*SH_TAS[0]);
+        Kfusion[9] = SK_TAS*(P[9][4]*SH_TAS[2] - P[9][14]*SH_TAS[2] + P[9][5]*SH_TAS[1] - P[9][15]*SH_TAS[1] + P[9][6]*vd*SH_TAS[0]);
+        Kfusion[10] = SK_TAS*(P[10][4]*SH_TAS[2] - P[10][14]*SH_TAS[2] + P[10][5]*SH_TAS[1] - P[10][15]*SH_TAS[1] + P[10][6]*vd*SH_TAS[0]);
+        Kfusion[11] = SK_TAS*(P[11][4]*SH_TAS[2] - P[11][14]*SH_TAS[2] + P[11][5]*SH_TAS[1] - P[11][15]*SH_TAS[1] + P[11][6]*vd*SH_TAS[0]);
+        Kfusion[12] = SK_TAS*(P[12][4]*SH_TAS[2] - P[12][14]*SH_TAS[2] + P[12][5]*SH_TAS[1] - P[12][15]*SH_TAS[1] + P[12][6]*vd*SH_TAS[0]);
+        // Only height measurements are allowed to modify the Z delta velocity bias state. This improves the stability of the estimate
+        Kfusion[13] = 0.0f;//SK_TAS*(P[13][4]*SH_TAS[2] - P[13][14]*SH_TAS[2] + P[13][5]*SH_TAS[1] - P[13][15]*SH_TAS[1] + P[13][6]*vd*SH_TAS[0]);
+        Kfusion[14] = SK_TAS*(P[14][4]*SH_TAS[2] - P[14][14]*SH_TAS[2] + P[14][5]*SH_TAS[1] - P[14][15]*SH_TAS[1] + P[14][6]*vd*SH_TAS[0]);
+        Kfusion[15] = SK_TAS*(P[15][4]*SH_TAS[2] - P[15][14]*SH_TAS[2] + P[15][5]*SH_TAS[1] - P[15][15]*SH_TAS[1] + P[15][6]*vd*SH_TAS[0]);
+        Kfusion[16] = SK_TAS*(P[16][4]*SH_TAS[2] - P[16][14]*SH_TAS[2] + P[16][5]*SH_TAS[1] - P[16][15]*SH_TAS[1] + P[16][6]*vd*SH_TAS[0]);
+        Kfusion[17] = SK_TAS*(P[17][4]*SH_TAS[2] - P[17][14]*SH_TAS[2] + P[17][5]*SH_TAS[1] - P[17][15]*SH_TAS[1] + P[17][6]*vd*SH_TAS[0]);
+        Kfusion[18] = SK_TAS*(P[18][4]*SH_TAS[2] - P[18][14]*SH_TAS[2] + P[18][5]*SH_TAS[1] - P[18][15]*SH_TAS[1] + P[18][6]*vd*SH_TAS[0]);
+        Kfusion[19] = SK_TAS*(P[19][4]*SH_TAS[2] - P[19][14]*SH_TAS[2] + P[19][5]*SH_TAS[1] - P[19][15]*SH_TAS[1] + P[19][6]*vd*SH_TAS[0]);
+        Kfusion[20] = SK_TAS*(P[20][4]*SH_TAS[2] - P[20][14]*SH_TAS[2] + P[20][5]*SH_TAS[1] - P[20][15]*SH_TAS[1] + P[20][6]*vd*SH_TAS[0]);
+        Kfusion[21] = SK_TAS*(P[21][4]*SH_TAS[2] - P[21][14]*SH_TAS[2] + P[21][5]*SH_TAS[1] - P[21][15]*SH_TAS[1] + P[21][6]*vd*SH_TAS[0]);
+        Kfusion[22] = SK_TAS*(P[22][4]*SH_TAS[2] - P[22][14]*SH_TAS[2] + P[22][5]*SH_TAS[1] - P[22][15]*SH_TAS[1] + P[22][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 <= 22; 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 <= 22; 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 <= 13; j++) KH[i][j] = 0.0;
+                for (uint8_t j = 14; j <= 15; j++)
+                {
+                    KH[i][j] = Kfusion[i] * H_TAS[j];
+                }
+                for (uint8_t j = 16; j <= 22; j++) KH[i][j] = 0.0;
+            }
+            for (uint8_t i = 0; i <= 22; i++)
+            {
+                for (uint8_t j = 0; j <= 22; 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 = 14; k <= 15; k++)
+                    {
+                        KHP[i][j] = KHP[i][j] + KH[i][k] * P[k][j];
+                    }
+                }
+            }
+            for (uint8_t i = 0; i <= 22; i++)
+            {
+                for (uint8_t j = 0; j <= 22; 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::FuseRangeFinder()
+{
+
+    // Local variables
+    float rngPred;
+    float SH_RNG[5];
+    float H_RNG[23];
+    float SK_RNG[6];
+    float cosRngTilt;
+    const float R_RNG = 0.25f; // 0.5 m2 rangefinder measurement variance
+
+    // Copy required states to local variable names
+    float q0 = statesAtRngTime[0];
+    float q1 = statesAtRngTime[1];
+    float q2 = statesAtRngTime[2];
+    float q3 = statesAtRngTime[3];
+    float pd = statesAtRngTime[9];
+    float ptd = statesAtRngTime[22];
+
+    // Need to check that our range finder tilt angle is less than 30 degrees and we are using range finder data
+    SH_RNG[4] = sin(rngFinderPitch);
+    cosRngTilt = - Tbn.z.x * SH_RNG[4] + Tbn.z.z * cos(rngFinderPitch);
+    if (useRangeFinder && cosRngTilt > 0.87f)
+    {
+        // Calculate observation jacobian and Kalman gain ignoring all states other than the terrain offset
+        // This prevents the range finder measurement modifying any of the other filter states and significantly reduces computations
+        SH_RNG[0] = SH_RNG[4]*(2*q0*q2 - 2*q1*q3) - sq(q0) + sq(q1) + sq(q2) - sq(q3);
+        SH_RNG[1] = pd - ptd;
+        SH_RNG[2] = 1/sq(SH_RNG[0]);
+        SH_RNG[3] = 1/SH_RNG[0];
+        for (uint8_t i = 0; i < n_states; i++) {
+            H_RNG[i] = 0.0f;
+            Kfusion[i] = 0.0f;
+        }
+        H_RNG[22] = -SH_RNG[3];
+        SK_RNG[0] = 1/(R_RNG + SH_RNG[3]*(P[9][9]*SH_RNG[3] - P[22][9]*SH_RNG[3] + P[0][9]*SH_RNG[1]*SH_RNG[2]*(2*q0 - 2*q2*SH_RNG[4]) - P[1][9]*SH_RNG[1]*SH_RNG[2]*(2*q1 - 2*q3*SH_RNG[4]) - P[2][9]*SH_RNG[1]*SH_RNG[2]*(2*q2 + 2*q0*SH_RNG[4]) + P[3][9]*SH_RNG[1]*SH_RNG[2]*(2*q3 + 2*q1*SH_RNG[4])) - SH_RNG[3]*(P[9][22]*SH_RNG[3] - P[22][22]*SH_RNG[3] + P[0][22]*SH_RNG[1]*SH_RNG[2]*(2*q0 - 2*q2*SH_RNG[4]) - P[1][22]*SH_RNG[1]*SH_RNG[2]*(2*q1 - 2*q3*SH_RNG[4]) - P[2][22]*SH_RNG[1]*SH_RNG[2]*(2*q2 + 2*q0*SH_RNG[4]) + P[3][22]*SH_RNG[1]*SH_RNG[2]*(2*q3 + 2*q1*SH_RNG[4])) + SH_RNG[1]*SH_RNG[2]*(2*q0 - 2*q2*SH_RNG[4])*(P[9][0]*SH_RNG[3] - P[22][0]*SH_RNG[3] + P[0][0]*SH_RNG[1]*SH_RNG[2]*(2*q0 - 2*q2*SH_RNG[4]) - P[1][0]*SH_RNG[1]*SH_RNG[2]*(2*q1 - 2*q3*SH_RNG[4]) - P[2][0]*SH_RNG[1]*SH_RNG[2]*(2*q2 + 2*q0*SH_RNG[4]) + P[3][0]*SH_RNG[1]*SH_RNG[2]*(2*q3 + 2*q1*SH_RNG[4])) - SH_RNG[1]*SH_RNG[2]*(2*q1 - 2*q3*SH_RNG[4])*(P[9][1]*SH_RNG[3] - P[22][1]*SH_RNG[3] + P[0][1]*SH_RNG[1]*SH_RNG[2]*(2*q0 - 2*q2*SH_RNG[4]) - P[1][1]*SH_RNG[1]*SH_RNG[2]*(2*q1 - 2*q3*SH_RNG[4]) - P[2][1]*SH_RNG[1]*SH_RNG[2]*(2*q2 + 2*q0*SH_RNG[4]) + P[3][1]*SH_RNG[1]*SH_RNG[2]*(2*q3 + 2*q1*SH_RNG[4])) - SH_RNG[1]*SH_RNG[2]*(2*q2 + 2*q0*SH_RNG[4])*(P[9][2]*SH_RNG[3] - P[22][2]*SH_RNG[3] + P[0][2]*SH_RNG[1]*SH_RNG[2]*(2*q0 - 2*q2*SH_RNG[4]) - P[1][2]*SH_RNG[1]*SH_RNG[2]*(2*q1 - 2*q3*SH_RNG[4]) - P[2][2]*SH_RNG[1]*SH_RNG[2]*(2*q2 + 2*q0*SH_RNG[4]) + P[3][2]*SH_RNG[1]*SH_RNG[2]*(2*q3 + 2*q1*SH_RNG[4])) + SH_RNG[1]*SH_RNG[2]*(2*q3 + 2*q1*SH_RNG[4])*(P[9][3]*SH_RNG[3] - P[22][3]*SH_RNG[3] + P[0][3]*SH_RNG[1]*SH_RNG[2]*(2*q0 - 2*q2*SH_RNG[4]) - P[1][3]*SH_RNG[1]*SH_RNG[2]*(2*q1 - 2*q3*SH_RNG[4]) - P[2][3]*SH_RNG[1]*SH_RNG[2]*(2*q2 + 2*q0*SH_RNG[4]) + P[3][3]*SH_RNG[1]*SH_RNG[2]*(2*q3 + 2*q1*SH_RNG[4])));
+        SK_RNG[1] = 2*q1 - 2*q3*SH_RNG[4];
+        SK_RNG[2] = 2*q0 - 2*q2*SH_RNG[4];
+        SK_RNG[3] = 2*q3 + 2*q1*SH_RNG[4];
+        SK_RNG[4] = 2*q2 + 2*q0*SH_RNG[4];
+        SK_RNG[5] = SH_RNG[2];
+        Kfusion[22] = SK_RNG[0]*(P[22][9]*SH_RNG[3] - P[22][22]*SH_RNG[3] + P[22][0]*SH_RNG[1]*SK_RNG[2]*SK_RNG[5] - P[22][1]*SH_RNG[1]*SK_RNG[1]*SK_RNG[5] - P[22][2]*SH_RNG[1]*SK_RNG[4]*SK_RNG[5] + P[22][3]*SH_RNG[1]*SK_RNG[3]*SK_RNG[5]);
+
+        // Calculate the measurement innovation
+        rngPred = (ptd - pd)/cosRngTilt;
+        innovRng = rngPred - rngMea;
+        //printf("mea=%5.1f, pred=%5.1f, pd=%5.1f, ptd=%5.2f\n", rngMea, rngPred, pd, ptd);
+
+        // Check the innovation for consistency and don't fuse if > 5Sigma
+        if ((innovRng*innovRng*SK_RNG[0]) < 25)
+        {
+            // correct the state vector
+            states[22] = states[22] - Kfusion[22] * innovRng;
+
+            // correct the covariance P = (I - K*H)*P
+            P[22][22] = P[22][22] - Kfusion[22] * H_RNG[22] * P[22][22];
+            P[22][22] = ConstrainFloat(P[22][22], 0.0f, 10000.0f);
+        }
+    }
+
+}
+
+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, uint64_t msec)
+{
+    int ret = 0;
+
+    int64_t bestTimeDelta = 200;
+    unsigned bestStoreIndex = 0;
+    for (unsigned storeIndex = 0; storeIndex < data_buffer_size; storeIndex++)
+    {
+        // Work around a GCC compiler bug - we know 64bit support on ARM is
+        // sketchy in GCC.
+        uint64_t timeDelta;
+
+        if (msec > statetimeStamp[storeIndex]) {
+            timeDelta = msec - statetimeStamp[storeIndex];
+        } else {
+            timeDelta = statetimeStamp[storeIndex] - msec;
+        }
+
+        if (timeDelta < bestTimeDelta)
+        {
+            bestStoreIndex = storeIndex;
+            bestTimeDelta = timeDelta;
+        }
+    }
+    if (bestTimeDelta < 200) // only output stored state if < 200 msec retrieval error
+    {
+        for (unsigned 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 (unsigned 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], double lat, double lon, float hgt, double latRef, double 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 < 6.0f));
+    if (staticMode) {
+        staticMode = (!refSet || (GPSstatus < GPS_FIX_3D));
+    }
+}
+
+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.7f);
+    P[5][5]   = P[4][4];
+    P[6][6]   = sq(0.7f);
+    P[7][7]   = sq(15.0f);
+    P[8][8]   = P[7][7];
+    P[9][9]   = sq(5.0f);
+    P[10][10] = sq(0.1f*deg2rad*dtIMU);
+    P[11][11] = P[10][10];
+    P[12][12] = P[10][10];
+    P[13][13] = sq(0.2f*dtIMU);
+    P[14][14] = sq(8.0f);
+    P[15][14]  = P[14][14];
+    P[16][16] = sq(0.02f);
+    P[17][17] = P[16][16];
+    P[18][18] = P[16][16];
+    P[19][19] = sq(0.02f);
+    P[20][20] = P[19][19];
+    P[21][21] = P[19][19];
+    P[22][22] = sq(0.5f);
+}
+
+float AttPosEKF::ConstrainFloat(float val, float min, float max)
+{
+    float ret;
+    if (val > max) {
+        ret = max;
+        ekf_debug("> max: %8.4f, val: %8.4f", max, val);
+    } else if (val < min) {
+        ret = min;
+        ekf_debug("< min: %8.4f, val: %8.4f", min, val);
+    } else {
+        ret = val;
+    }
+
+    if (!isfinite(val)) {
+        ekf_debug("constrain: non-finite!");
+    }
+
+    return ret;
+}
+
+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: Delta Velocity bias - m/s (Z)
+    // 14-15: Wind Vector  - m/sec (North,East)
+    // 16-18: Earth Magnetic Field Vector - gauss (North, East, Down)
+    // 19-21: Body Magnetic Field Vector - gauss (X,Y,Z)
+    // 22: Terrain  offset - m
+
+    // Constrain quaternion variances
+    for (unsigned i = 0; i <= 3; i++) {
+        P[i][i] = ConstrainFloat(P[i][i], 0.0f, 1.0f);
+    }
+
+    // Constrain velocity variances
+    for (unsigned i = 4; i <= 6; i++) {
+        P[i][i] = ConstrainFloat(P[i][i], 0.0f, 1.0e3f);
+    }
+
+    // Constrain position variances
+    for (unsigned i = 7; i <= 9; i++) {
+        P[i][i] = ConstrainFloat(P[i][i], 0.0f, 1.0e6f);
+    }
+
+    // Constrain delta angle bias variances
+    for (unsigned i = 10; i <= 12; i++) {
+        P[i][i] = ConstrainFloat(P[i][i], 0.0f, sq(0.12f * dtIMU));
+    }
+
+    // Constrain delta velocity bias variance
+    P[13][13] = ConstrainFloat(P[13][13], 0.0f, sq(1.0f * dtIMU));
+
+    // Wind velocity variances
+    for (unsigned i = 14; i <= 15; i++) {
+        P[i][i] = ConstrainFloat(P[i][i], 0.0f, 1.0e3f);
+    }
+
+    // Earth magnetic field variances
+    for (unsigned i = 16; i <= 18; i++) {
+        P[i][i] = ConstrainFloat(P[i][i], 0.0f, 1.0f);
+    }
+
+    // Body magnetic field variances
+    for (unsigned i = 19; i <= 21; i++) {
+        P[i][i] = ConstrainFloat(P[i][i], 0.0f, 1.0f);
+    }
+
+    // Constrain terrain offset variance
+    P[22][22] = ConstrainFloat(P[22][22], 0.0f, 10000.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: Delta Velocity bias - m/s (Z)
+    // 14-15: Wind Vector  - m/sec (North,East)
+    // 16-18: Earth Magnetic Field Vector - gauss (North, East, Down)
+    // 19-21: Body Magnetic Field Vector - gauss (X,Y,Z)
+    // 22: Terrain  offset - m
+
+    // Constrain quaternion
+    for (unsigned i = 0; i <= 3; i++) {
+        states[i] = ConstrainFloat(states[i], -1.0f, 1.0f);
+    }
+
+    // Constrain velocities to what GPS can do for us
+    for (unsigned i = 4; i <= 6; i++) {
+        states[i] = ConstrainFloat(states[i], -5.0e2f, 5.0e2f);
+    }
+
+    // Constrain position to a reasonable vehicle range (in meters)
+    for (unsigned i = 7; i <= 8; 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 <= 12; i++) {
+        states[i] = ConstrainFloat(states[i], -0.12f * dtIMU, 0.12f * dtIMU);
+    }
+
+    // Constrain delta velocity bias
+    states[13] = ConstrainFloat(states[13], -1.0f * dtIMU, 1.0f * dtIMU);
+
+    // Wind velocity limits - assume 120 m/s max velocity
+    for (unsigned i = 14; i <= 15; i++) {
+        states[i] = ConstrainFloat(states[i], -120.0f, 120.0f);
+    }
+
+    // Earth magnetic field limits (in Gauss)
+    for (unsigned i = 16; 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 = 19; i <= 21; i++) {
+        states[i] = ConstrainFloat(states[i], -0.5f, 0.5f);
+    }
+
+    // Constrain terrain offset
+    states[22] = ConstrainFloat(states[22], -1000.0f, 1000.0f);
+
+}
+
+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 (unsigned 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 integrators
+    if (!isfinite(summedDelAng.x) || !isfinite(summedDelAng.y) || !isfinite(summedDelAng.z)) {
+        err_report->statesNaN = true;
+        ekf_debug("summedDelAng NaN: x: %f y: %f z: %f", (double)summedDelAng.x, (double)summedDelAng.y, (double)summedDelAng.z);
+        err = true;
+        goto out;
+    } // delta angles
+
+    if (!isfinite(correctedDelAng.x) || !isfinite(correctedDelAng.y) || !isfinite(correctedDelAng.z)) {
+        err_report->statesNaN = true;
+        ekf_debug("correctedDelAng NaN: x: %f y: %f z: %f", (double)correctedDelAng.x, (double)correctedDelAng.y, (double)correctedDelAng.z);
+        err = true;
+        goto out;
+    } // delta angles
+
+    if (!isfinite(summedDelVel.x) || !isfinite(summedDelVel.y) || !isfinite(summedDelVel.z)) {
+        err_report->statesNaN = true;
+        ekf_debug("summedDelVel NaN: x: %f y: %f z: %f", (double)summedDelVel.x, (double)summedDelVel.y, (double)summedDelVel.z);
+        err = true;
+        goto out;
+    } // delta velocities
+
+    // 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;
+                ekf_debug("KH NaN");
+                goto out;
+            } //  intermediate result used for covariance updates
+
+            if (!isfinite(KHP[i][j])) {
+
+                err_report->covarianceNaN = true;
+                err = true;
+                ekf_debug("KHP NaN");
+                goto out;
+            } // intermediate result used for covariance updates
+
+            if (!isfinite(P[i][j])) {
+
+                err_report->covarianceNaN = true;
+                err = true;
+                ekf_debug("P NaN");
+            } // covariance matrix
+        }
+
+        if (!isfinite(Kfusion[i])) {
+
+            err_report->kalmanGainsNaN = true;
+            ekf_debug("Kfusion NaN");
+            err = true;
+            goto out;
+        } // Kalman gains
+
+        if (!isfinite(states[i])) {
+
+            err_report->statesNaN = true;
+            ekf_debug("states NaN: i: %u val: %f", i, (double)states[i]);
+            err = true;
+            goto out;
+        } // state matrix
+    }
+
+out:
+    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)) {
+        ekf_debug("re-initializing dynamic");
+
+        InitializeDynamic(velNED, magDeclination);
+
+        return 1;
+    }
+
+    // Reset the filter if the IMU data is too old
+    if (dtIMU > 0.3f) {
+
+        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 declination, float *initQuat)
+{
+    float initialRoll, initialPitch;
+    float cosRoll, sinRoll, cosPitch, sinPitch;
+    float magX, magY;
+    float initialHdg, cosHeading, sinHeading;
+
+    initialRoll = atan2f(-ay, -az);
+    initialPitch = atan2f(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);
+    /* true heading is the mag heading minus declination */
+    initialHdg += declination;
+
+    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;
+
+    /* normalize */
+    float norm = sqrtf(initQuat[0]*initQuat[0] + initQuat[1]*initQuat[1] + initQuat[2]*initQuat[2] + initQuat[3]*initQuat[3]);
+
+    initQuat[0] /= norm;
+    initQuat[1] /= norm;
+    initQuat[2] /= norm;
+    initQuat[3] /= norm;
+}
+
+void AttPosEKF::InitializeDynamic(float (&initvelNED)[3], float declination)
+{
+
+    // Fill variables with valid data
+    velNED[0] = initvelNED[0];
+    velNED[1] = initvelNED[1];
+    velNED[2] = initvelNED[2];
+    magDeclination = declination;
+
+    // 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, declination, 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;
+    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;
+
+    magstate.q0 = initQuat[0];
+    magstate.q1 = initQuat[1];
+    magstate.q2 = initQuat[2];
+    magstate.q3 = initQuat[3];
+    magstate.magN = initMagNED.x;
+    magstate.magE = initMagNED.y;
+    magstate.magD = initMagNED.z;
+    magstate.magXbias = magBias.x;
+    magstate.magYbias = magBias.y;
+    magstate.magZbias = magBias.z;
+    magstate.R_MAG = sq(magMeasurementSigma);
+    magstate.DCM = DCM;
+
+    // write to state vector
+    for (uint8_t j=0; j<=3; j++) states[j] = initQuat[j]; // quaternions
+    for (uint8_t j=4; j<=6; j++) states[j] = initvelNED[j-4]; // velocities
+    for (uint8_t j=7; j<=15; j++) states[j] = 0.0f; // positions, dAngBias, dVelBias, windVel
+    states[16] = initMagNED.x; // Magnetic Field North
+    states[17] = initMagNED.y; // Magnetic Field East
+    states[18] = initMagNED.z; // Magnetic Field Down
+    states[19] = magBias.x; // Magnetic Field Bias X
+    states[20] = magBias.y; // Magnetic Field Bias Y
+    states[21] = magBias.z; // Magnetic Field Bias Z
+    states[22] = 0.0f; // terrain height
+
+    ResetVelocity();
+    ResetPosition();
+    ResetHeight();
+
+    statesInitialised = true;
+
+    // initialise the covariance matrix
+    CovarianceInit();
+
+    //Define Earth rotation vector in the NED navigation frame
+    calcEarthRateNED(earthRateNED, latRef);
+
+}
+
+void AttPosEKF::InitialiseFilter(float (&initvelNED)[3], double referenceLat, double referenceLon, float referenceHgt, float declination)
+{
+    // store initial lat,long and height
+    latRef = referenceLat;
+    lonRef = referenceLon;
+    hgtRef = referenceHgt;
+    refSet = true;
+
+    // we are at reference altitude, so measurement must be zero
+    hgtMea = 0.0f;
+
+    // the baro offset must be this difference now
+    baroHgtOffset = baroHgt - referenceHgt;
+
+    memset(&last_ekf_error, 0, sizeof(last_ekf_error));
+
+    InitializeDynamic(initvelNED, declination);
+}
+
+void AttPosEKF::ZeroVariables()
+{
+
+    // Initialize on-init initialized variables
+    fusionModeGPS = 0;
+    covSkipCount = 0;
+    statesInitialised = false;
+    fuseVelData = false;
+    fusePosData = false;
+    fuseHgtData = false;
+    fuseMagData = false;
+    fuseVtasData = false;
+    onGround = true;
+    staticMode = true;
+    useAirspeed = true;
+    useCompass = true;
+    useRangeFinder = true;
+    numericalProtection = true;
+    refSet = false;
+    storeIndex = 0;
+    gpsHgt = 0.0f;
+    baroHgt = 0.0f;
+    GPSstatus = 0;
+    VtasMeas = 0.0f;
+    magDeclination = 0.0f;
+
+    // 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
+    }
+
+    correctedDelAng.zero();
+    summedDelAng.zero();
+    summedDelVel.zero();
+
+    dAngIMU.zero();
+    dVelIMU.zero();
+
+    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(&magstate, 0, sizeof(magstate));
+    magstate.q0 = 1.0f;
+    magstate.DCM.identity();
+
+    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/ekf_att_pos_estimator/estimator.h
index e62f1a98a..e821089f2 100644
--- a/src/modules/fw_att_pos_estimator/estimator.h
+++ b/src/modules/ekf_att_pos_estimator/estimator.h
@@ -20,7 +20,7 @@ public:
     float z;
 
     float length(void) const;
-    Vector3f zero(void) const;
+    void zero(void);
 };
 
 class Mat3f
@@ -33,6 +33,7 @@ public:
 
     Mat3f();
 
+    void identity();
     Mat3f transpose(void) const;
 };
 
@@ -45,14 +46,9 @@ Vector3f operator*(Vector3f vecIn1, float sclIn1);
 
 void swap_var(float &d1, float &d2);
 
-const unsigned int n_states = 21;
+const unsigned int n_states = 23;
 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,
@@ -82,6 +78,88 @@ public:
     AttPosEKF();
     ~AttPosEKF();
 
+
+
+    /* ##############################################
+     *
+     *   M A I N    F I L T E R    P A R A M E T E R S
+     *
+     * ########################################### */
+
+    /*
+     * parameters are defined here and initialised in
+     * the InitialiseParameters() (which is just 20 lines down)
+     */
+
+    float covTimeStepMax; // maximum time allowed between covariance predictions
+    float covDelAngMax; // maximum delta angle between covariance predictions
+    float rngFinderPitch; // pitch angle of laser range finder in radians. Zero is aligned with the Z body axis. Positive is RH rotation about Y body axis.
+
+    float yawVarScale;
+    float windVelSigma;
+    float dAngBiasSigma;
+    float dVelBiasSigma;
+    float magEarthSigma;
+    float magBodySigma;
+    float gndHgtSigma;
+
+    float vneSigma;
+    float vdSigma;
+    float posNeSigma;
+    float posDSigma;
+    float magMeasurementSigma;
+    float airspeedMeasurementSigma;
+
+    float gyroProcessNoise;
+    float accelProcessNoise;
+
+    float EAS2TAS; // ratio f true to equivalent airspeed
+
+    void InitialiseParameters()
+    {
+        covTimeStepMax = 0.07f; // maximum time allowed between covariance predictions
+        covDelAngMax = 0.02f; // maximum delta angle between covariance predictions
+        rngFinderPitch = 0.0f; // pitch angle of laser range finder in radians. Zero is aligned with the Z body axis. Positive is RH rotation about Y body axis.
+        EAS2TAS = 1.0f;
+
+        yawVarScale = 1.0f;
+        windVelSigma = 0.1f;
+        dAngBiasSigma = 5.0e-7f;
+        dVelBiasSigma = 1e-4f;
+        magEarthSigma = 3.0e-4f;
+        magBodySigma  = 3.0e-4f;
+        gndHgtSigma  = 0.02f; // assume 2% terrain gradient 1-sigma
+
+        vneSigma = 0.2f;
+        vdSigma = 0.3f;
+        posNeSigma = 2.0f;
+        posDSigma = 2.0f;
+
+        magMeasurementSigma = 0.05;
+        airspeedMeasurementSigma = 1.4f;
+        gyroProcessNoise = 1.4544411e-2f;
+        accelProcessNoise = 0.5f;
+    }
+
+    struct {
+        unsigned obsIndex;
+        float MagPred[3];
+        float SH_MAG[9];
+        float q0;
+        float q1;
+        float q2;
+        float q3;
+        float magN;
+        float magE;
+        float magD;
+        float magXbias;
+        float magYbias;
+        float magZbias;
+        float R_MAG;
+        Mat3f DCM;
+    } magstate;
+
+
     // 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
@@ -96,6 +174,7 @@ public:
     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
+    float statesAtRngTime[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)
@@ -104,6 +183,10 @@ public:
     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)
+
+    Mat3f Tbn; // transformation matrix from body to NED coordinates
+    Mat3f Tnb; // transformation amtrix from NED to body coordinates
+
     Vector3f accel; // acceleration vector in XYZ body axes measured by the IMU (m/s^2)
     Vector3f dVelIMU;
     Vector3f dAngIMU;
@@ -115,26 +198,30 @@ public:
     float velNED[3]; // North, East, Down velocity obs (m/s)
     float posNE[2]; // North, East position obs (m)
     float hgtMea; //  measured height (m)
+    float baroHgtOffset;        ///< the baro (weather) offset from normalized altitude
+    float rngMea; // Ground distance
     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 innovRng; ///< Range finder innovation
     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 magDeclination;       ///< magnetic declination
+    double latRef; // WGS-84 latitude of reference point (rad)
+    double lonRef; // WGS-84 longitude of reference point (rad)
     float hgtRef; // WGS-84 height of reference point (m)
+    bool refSet;                ///< flag to indicate if the reference position has been set
     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
+    unsigned covSkipCount; // Number of state prediction frames (IMU daya updates to skip before doing the covariance prediction
 
     // GPS input data variables
     float gpsCourse;
     float gpsVelD;
-    float gpsLat;
-    float gpsLon;
+    double gpsLat;
+    double gpsLon;
     float gpsHgt;
     uint8_t GPSstatus;
 
@@ -148,11 +235,13 @@ public:
     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 fuseRngData;   ///< true when range data is 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
+    bool useRangeFinder;     ///< true when rangefinder is being used
 
     struct ekf_status_report current_ekf_state;
     struct ekf_status_report last_ekf_error;
@@ -172,6 +261,10 @@ void FuseMagnetometer();
 
 void FuseAirspeed();
 
+void FuseRangeFinder();
+
+void FuseOpticalFlow();
+
 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);
@@ -192,7 +285,7 @@ void StoreStates(uint64_t timestamp_ms);
  *         time-wise where valid states were updated and invalid remained at the old
  *         value.
  */
-int RecallStates(float statesForFusion[n_states], uint64_t msec);
+int RecallStates(float *statesForFusion, uint64_t msec);
 
 void ResetStoredStates();
 
@@ -206,7 +299,7 @@ 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 calcposNED(float (&posNED)[3], double lat, double lon, float hgt, double latRef, double lonRef, float hgtRef);
 
 static void calcLLH(float (&posNED)[3], float lat, float lon, float hgt, float latRef, float lonRef, float hgtRef);
 
@@ -218,7 +311,7 @@ void OnGroundCheck();
 
 void CovarianceInit();
 
-void InitialiseFilter(float (&initvelNED)[3]);
+void InitialiseFilter(float (&initvelNED)[3], double referenceLat, double referenceLon, float referenceHgt, float declination);
 
 float ConstrainFloat(float val, float min, float max);
 
@@ -243,7 +336,7 @@ 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]);
+void InitializeDynamic(float (&initvelNED)[3], float declination);
 
 protected:
 
@@ -251,7 +344,7 @@ bool FilterHealthy();
 
 void ResetHeight(void);
 
-void AttitudeInit(float ax, float ay, float az, float mx, float my, float mz, float *initQuat);
+void AttitudeInit(float ax, float ay, float az, float mx, float my, float mz, float declination, float *initQuat);
 
 };
 
diff --git a/src/modules/fw_att_pos_estimator/fw_att_pos_estimator_main.cpp b/src/modules/ekf_att_pos_estimator/fw_att_pos_estimator_main.cpp
index fb8abe95a..907f4c2e1 100644
--- a/src/modules/fw_att_pos_estimator/fw_att_pos_estimator_main.cpp
+++ b/src/modules/ekf_att_pos_estimator/fw_att_pos_estimator_main.cpp
@@ -73,6 +73,7 @@
 #include <uORB/topics/parameter_update.h>
 #include <uORB/topics/estimator_status.h>
 #include <uORB/topics/actuator_armed.h>
+#include <uORB/topics/home_position.h>
 #include <systemlib/param/param.h>
 #include <systemlib/err.h>
 #include <geo/geo.h>
@@ -90,20 +91,19 @@
  *
  * @ingroup apps
  */
-extern "C" __EXPORT int fw_att_pos_estimator_main(int argc, char *argv[]);
+extern "C" __EXPORT int ekf_att_pos_estimator_main(int argc, char *argv[]);
 
 __EXPORT uint32_t millis();
 
 static uint64_t last_run = 0;
 static uint64_t IMUmsec = 0;
+static const uint64_t FILTER_INIT_DELAY = 1 * 1000 * 1000;
 
 uint32_t millis()
 {
 	return IMUmsec;
 }
 
-static void print_status();
-
 class FixedwingEstimator
 {
 public:
@@ -152,6 +152,7 @@ private:
 	int 		_params_sub;			/**< notification of parameter updates */
 	int 		_manual_control_sub;		/**< notification of manual control updates */
 	int		_mission_sub;
+	int		_home_sub;			/**< home position as defined by commander / user */
 
 	orb_advert_t	_att_pub;			/**< vehicle attitude */
 	orb_advert_t	_global_pos_pub;		/**< global position */
@@ -191,7 +192,13 @@ private:
 	perf_counter_t	_perf_airspeed;			///<local performance counter for airspeed updates
 
 	bool						_initialized;
+	bool						_baro_init;
 	bool						_gps_initialized;
+	uint64_t					_gps_start_time;
+	uint64_t					_filter_start_time;
+	bool						_gyro_valid;
+	bool						_accel_valid;
+	bool						_mag_valid;
 
 	int						_mavlink_fd;
 
@@ -201,6 +208,19 @@ private:
 		int32_t	height_delay_ms;
 		int32_t	mag_delay_ms;
 		int32_t	tas_delay_ms;
+		float velne_noise;
+		float veld_noise;
+		float posne_noise;
+		float posd_noise;
+		float mag_noise;
+		float gyro_pnoise;
+		float acc_pnoise;
+		float gbias_pnoise;
+		float abias_pnoise;
+		float mage_pnoise;
+		float magb_pnoise;
+		float eas_noise;
+		float pos_stddev_threshold;
 	}		_parameters;			/**< local copies of interesting parameters */
 
 	struct {
@@ -209,6 +229,19 @@ private:
 		param_t	height_delay_ms;
 		param_t	mag_delay_ms;
 		param_t	tas_delay_ms;
+		param_t velne_noise;
+		param_t veld_noise;
+		param_t posne_noise;
+		param_t posd_noise;
+		param_t mag_noise;
+		param_t gyro_pnoise;
+		param_t acc_pnoise;
+		param_t gbias_pnoise;
+		param_t abias_pnoise;
+		param_t mage_pnoise;
+		param_t magb_pnoise;
+		param_t eas_noise;
+		param_t pos_stddev_threshold;
 	}		_parameter_handles;		/**< handles for interesting parameters */
 
 	AttPosEKF					*_ekf;
@@ -271,6 +304,8 @@ FixedwingEstimator::FixedwingEstimator() :
 	_vstatus_sub(-1),
 	_params_sub(-1),
 	_manual_control_sub(-1),
+	_mission_sub(-1),
+	_home_sub(-1),
 
 /* publications */
 	_att_pub(-1),
@@ -278,6 +313,25 @@ FixedwingEstimator::FixedwingEstimator() :
 	_local_pos_pub(-1),
 	_estimator_status_pub(-1),
 
+	_att({}),
+	_gyro({}),
+	_accel({}),
+	_mag({}),
+	_airspeed({}),
+	_baro({}),
+	_vstatus({}),
+	_global_pos({}),
+	_local_pos({}),
+	_gps({}),
+
+	_gyro_offsets({}),
+	_accel_offsets({}),
+	_mag_offsets({}),
+
+	#ifdef SENSOR_COMBINED_SUB
+	_sensor_combined({}),
+	#endif
+
 	_baro_ref(0.0f),
 	_baro_gps_offset(0.0f),
 
@@ -292,18 +346,35 @@ FixedwingEstimator::FixedwingEstimator() :
 
 /* states */
 	_initialized(false),
+	_baro_init(false),
 	_gps_initialized(false),
+	_gyro_valid(false),
+	_accel_valid(false),
+	_mag_valid(false),
 	_mavlink_fd(-1),
 	_ekf(nullptr)
 {
 
-	_mavlink_fd = open(MAVLINK_LOG_DEVICE, 0);
+	last_run = hrt_absolute_time();
 
 	_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");
+	_parameter_handles.velne_noise = param_find("PE_VELNE_NOISE");
+	_parameter_handles.veld_noise = param_find("PE_VELD_NOISE");
+	_parameter_handles.posne_noise = param_find("PE_POSNE_NOISE");
+	_parameter_handles.posd_noise = param_find("PE_POSD_NOISE");
+	_parameter_handles.mag_noise = param_find("PE_MAG_NOISE");
+	_parameter_handles.gyro_pnoise = param_find("PE_GYRO_PNOISE");
+	_parameter_handles.acc_pnoise = param_find("PE_ACC_PNOISE");
+	_parameter_handles.gbias_pnoise = param_find("PE_GBIAS_PNOISE");
+	_parameter_handles.abias_pnoise = param_find("PE_ABIAS_PNOISE");
+	_parameter_handles.mage_pnoise = param_find("PE_MAGE_PNOISE");
+	_parameter_handles.magb_pnoise = param_find("PE_MAGB_PNOISE");
+	_parameter_handles.eas_noise = param_find("PE_EAS_NOISE");
+	_parameter_handles.pos_stddev_threshold = param_find("PE_POSDEV_INIT");
 
 	/* fetch initial parameter values */
 	parameters_update();
@@ -317,6 +388,10 @@ FixedwingEstimator::FixedwingEstimator() :
 	if (fd > 0) {
 		res = ioctl(fd, GYROIOCGSCALE, (long unsigned int)&_gyro_offsets);
 		close(fd);
+
+		if (res) {
+			warnx("G SCALE FAIL");
+		}
 	}
 
 	fd = open(ACCEL_DEVICE_PATH, O_RDONLY);
@@ -324,6 +399,10 @@ FixedwingEstimator::FixedwingEstimator() :
 	if (fd > 0) {
 		res = ioctl(fd, ACCELIOCGSCALE, (long unsigned int)&_accel_offsets);
 		close(fd);
+
+		if (res) {
+			warnx("A SCALE FAIL");
+		}
 	}
 
 	fd = open(MAG_DEVICE_PATH, O_RDONLY);
@@ -331,6 +410,10 @@ FixedwingEstimator::FixedwingEstimator() :
 	if (fd > 0) {
 		res = ioctl(fd, MAGIOCGSCALE, (long unsigned int)&_mag_offsets);
 		close(fd);
+
+		if (res) {
+			warnx("M SCALE FAIL");
+		}
 	}
 }
 
@@ -368,6 +451,37 @@ FixedwingEstimator::parameters_update()
 	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));
+	param_get(_parameter_handles.velne_noise, &(_parameters.velne_noise));
+	param_get(_parameter_handles.veld_noise, &(_parameters.veld_noise));
+	param_get(_parameter_handles.posne_noise, &(_parameters.posne_noise));
+	param_get(_parameter_handles.posd_noise, &(_parameters.posd_noise));
+	param_get(_parameter_handles.mag_noise, &(_parameters.mag_noise));
+	param_get(_parameter_handles.gyro_pnoise, &(_parameters.gyro_pnoise));
+	param_get(_parameter_handles.acc_pnoise, &(_parameters.acc_pnoise));
+	param_get(_parameter_handles.gbias_pnoise, &(_parameters.gbias_pnoise));
+	param_get(_parameter_handles.abias_pnoise, &(_parameters.abias_pnoise));
+	param_get(_parameter_handles.mage_pnoise, &(_parameters.mage_pnoise));
+	param_get(_parameter_handles.magb_pnoise, &(_parameters.magb_pnoise));
+	param_get(_parameter_handles.eas_noise, &(_parameters.eas_noise));
+	param_get(_parameter_handles.pos_stddev_threshold, &(_parameters.pos_stddev_threshold));
+
+	if (_ekf) {
+		// _ekf->yawVarScale = 1.0f;
+		// _ekf->windVelSigma = 0.1f;
+		_ekf->dAngBiasSigma = _parameters.gbias_pnoise;
+		_ekf->dVelBiasSigma = _parameters.abias_pnoise;
+		_ekf->magEarthSigma = _parameters.mage_pnoise;
+		_ekf->magBodySigma  = _parameters.magb_pnoise;
+		// _ekf->gndHgtSigma  = 0.02f;
+		_ekf->vneSigma = _parameters.velne_noise;
+		_ekf->vdSigma = _parameters.veld_noise;
+		_ekf->posNeSigma = _parameters.posne_noise;
+		_ekf->posDSigma = _parameters.posd_noise;
+		_ekf->magMeasurementSigma = _parameters.mag_noise;
+		_ekf->gyroProcessNoise = _parameters.gyro_pnoise;
+        	_ekf->accelProcessNoise = _parameters.acc_pnoise;
+		_ekf->airspeedMeasurementSigma = _parameters.eas_noise;
+	}
 
 	return OK;
 }
@@ -392,13 +506,14 @@ 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()
 {
+	_mavlink_fd = open(MAVLINK_LOG_DEVICE, 0);
 
 	_ekf = new AttPosEKF();
+	float dt = 0.0f; // time lapsed since last covariance prediction
+	_filter_start_time = hrt_absolute_time();
 
 	if (!_ekf) {
 		errx(1, "failed allocating EKF filter - out of RAM!");
@@ -412,6 +527,7 @@ FixedwingEstimator::task_main()
 	_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));
+	_home_sub = orb_subscribe(ORB_ID(home_position));
 
 	/* rate limit vehicle status updates to 5Hz */
 	orb_set_interval(_vstatus_sub, 200);
@@ -431,26 +547,11 @@ FixedwingEstimator::task_main()
 	orb_set_interval(_sensor_combined_sub, 4);
 #endif
 
+	/* sets also parameters in the EKF object */
 	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;
+	Vector3f lastAccel = {0.0f, 0.0f, 0.0f};
 
 	/* wakeup source(s) */
 	struct pollfd fds[2];
@@ -466,9 +567,8 @@ FixedwingEstimator::task_main()
 	fds[1].events = POLLIN;
 #endif
 
-	hrt_abstime start_time = hrt_absolute_time();
-
 	bool newDataGps = false;
+	bool newHgtData = false;
 	bool newAdsData = false;
 	bool newDataMag = false;
 
@@ -503,19 +603,52 @@ FixedwingEstimator::task_main()
 		if (fds[1].revents & POLLIN) {
 
 			/* check vehicle status for changes to publication state */
+			bool prev_hil = (_vstatus.hil_state == HIL_STATE_ON);
 			vehicle_status_poll();
 
 			bool accel_updated;
 			bool mag_updated;
 
+			hrt_abstime last_sensor_timestamp;
+
 			perf_count(_perf_gyro);
 
+			/* Reset baro reference if switching to HIL, reset sensor states */
+			if (!prev_hil && (_vstatus.hil_state == HIL_STATE_ON)) {
+				/* system is in HIL now, wait for measurements to come in one last round */
+				usleep(60000);
+
+#ifndef SENSOR_COMBINED_SUB
+				orb_copy(ORB_ID(sensor_gyro), _gyro_sub, &_gyro);
+				orb_copy(ORB_ID(sensor_accel), _accel_sub, &_accel);
+				orb_copy(ORB_ID(sensor_mag), _mag_sub, &_mag);
+#else
+				/* now read all sensor publications to ensure all real sensor data is purged */
+				orb_copy(ORB_ID(sensor_combined), _sensor_combined_sub, &_sensor_combined);
+#endif
+
+				/* set sensors to de-initialized state */
+				_gyro_valid = false;
+				_accel_valid = false;
+				_mag_valid = false;
+
+				_baro_init = false;
+				_gps_initialized = false;
+				last_sensor_timestamp = hrt_absolute_time();
+				last_run = last_sensor_timestamp;
+
+				_ekf->ZeroVariables();
+				_ekf->dtIMU = 0.01f;
+				_filter_start_time = last_sensor_timestamp;
+
+				/* now skip this loop and get data on the next one, which will also re-init the filter */
+				continue;
+			}
+
 			/**
 			 *    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);
@@ -529,27 +662,46 @@ FixedwingEstimator::task_main()
 			}
 
 			last_sensor_timestamp = _gyro.timestamp;
-			_ekf.IMUmsec = _gyro.timestamp / 1e3f;
+			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)
+			if (!isfinite(deltaT) || 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 = _gyro.x;
-			_ekf->angRate.y = _gyro.y;
-			_ekf->angRate.z = _gyro.z;
+			if (isfinite(_gyro.x) &&
+				isfinite(_gyro.y) &&
+				isfinite(_gyro.z)) {
+				_ekf->angRate.x = _gyro.x;
+				_ekf->angRate.y = _gyro.y;
+				_ekf->angRate.z = _gyro.z;
+
+				if (!_gyro_valid) {
+					lastAngRate = _ekf->angRate;
+				}
+
+				_gyro_valid = true;
+			}
 
-			_ekf->accel.x = _accel.x;
-			_ekf->accel.y = _accel.y;
-			_ekf->accel.z = _accel.z;
+			if (accel_updated) {
+				_ekf->accel.x = _accel.x;
+				_ekf->accel.y = _accel.y;
+				_ekf->accel.z = _accel.z;
+
+				if (!_accel_valid) {
+					lastAccel = _ekf->accel;
+				}
+
+				_accel_valid = true;
+			}
 
 			_ekf->dAngIMU = 0.5f * (angRate + lastAngRate) * dtIMU;
 			_ekf->lastAngRate = angRate;
@@ -565,6 +717,8 @@ FixedwingEstimator::task_main()
 
 			if (last_accel != _sensor_combined.accelerometer_timestamp) {
 				accel_updated = true;
+			} else {
+				accel_updated = false;
 			}
 
 			last_accel = _sensor_combined.accelerometer_timestamp;
@@ -575,23 +729,43 @@ FixedwingEstimator::task_main()
 			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)
+			if (!isfinite(deltaT) || deltaT > 1.0f || deltaT < 0.000001f) {
 				deltaT = 0.01f;
+			}
+
+			last_run = _sensor_combined.timestamp;
 
 			// 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];
+			if (isfinite(_sensor_combined.gyro_rad_s[0]) &&
+				isfinite(_sensor_combined.gyro_rad_s[1]) &&
+				isfinite(_sensor_combined.gyro_rad_s[2])) {
+				_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];
+
+				if (!_gyro_valid) {
+					lastAngRate = _ekf->angRate;
+				}
+
+				_gyro_valid = true;
+			}
+
+			if (accel_updated) {
+				_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];
+
+				if (!_accel_valid) {
+					lastAccel = _ekf->accel;
+				}
 
-			_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];
+				_accel_valid = true;
+			}
 
 			_ekf->dAngIMU = 0.5f * (_ekf->angRate + lastAngRate) * _ekf->dtIMU;
 			lastAngRate = _ekf->angRate;
@@ -635,11 +809,13 @@ FixedwingEstimator::task_main()
 				perf_count(_perf_gps);
 
 				if (_gps.fix_type < 3) {
-					gps_updated = false;
 					newDataGps = false;
 
 				} else {
 
+					/* store time of valid GPS measurement */
+					_gps_start_time = hrt_absolute_time();
+
 					/* check if we had a GPS outage for a long time */
 					if (hrt_elapsed_time(&last_gps) > 5 * 1000 * 1000) {
 						_ekf->ResetPosition();
@@ -660,6 +836,21 @@ FixedwingEstimator::task_main()
 					_ekf->gpsLat = math::radians(_gps.lat / (double)1e7);
 					_ekf->gpsLon = math::radians(_gps.lon / (double)1e7) - M_PI;
 					_ekf->gpsHgt = _gps.alt / 1e3f;
+
+					// if (_gps.s_variance_m_s > 0.25f && _gps.s_variance_m_s < 100.0f * 100.0f) {
+					// 	_ekf->vneSigma = sqrtf(_gps.s_variance_m_s);
+					// } else {
+					// 	_ekf->vneSigma = _parameters.velne_noise;
+					// }
+
+					// if (_gps.p_variance_m > 0.25f && _gps.p_variance_m < 100.0f * 100.0f) {
+					// 	_ekf->posNeSigma = sqrtf(_gps.p_variance_m);
+					// } else {
+					// 	_ekf->posNeSigma = _parameters.posne_noise;
+					// }
+
+					// warnx("vel: %8.4f pos: %8.4f", _gps.s_variance_m_s, _gps.p_variance_m);
+
 					newDataGps = true;
 
 				}
@@ -672,10 +863,17 @@ FixedwingEstimator::task_main()
 			if (baro_updated) {
 				orb_copy(ORB_ID(sensor_baro), _baro_sub, &_baro);
 
-				_ekf->baroHgt = _baro.altitude - _baro_ref;
+				_ekf->baroHgt = _baro.altitude;
 
-				// Could use a blend of GPS and baro alt data if desired
-				_ekf->hgtMea = 1.0f * _ekf->baroHgt + 0.0f * _ekf->gpsHgt;
+				if (!_baro_init) {
+					_baro_ref = _baro.altitude;
+					_baro_init = true;
+					warnx("ALT REF INIT");
+				}
+
+				newHgtData = true;
+			} else {
+				newHgtData = false;
 			}
 
 #ifndef SENSOR_COMBINED_SUB
@@ -684,6 +882,8 @@ FixedwingEstimator::task_main()
 
 			if (mag_updated) {
 
+				_mag_valid = true;
+
 				perf_count(_perf_mag);
 
 #ifndef SENSOR_COMBINED_SUB
@@ -727,6 +927,8 @@ FixedwingEstimator::task_main()
 			 */
 			int check = _ekf->CheckAndBound();
 
+			const char* ekfname = "[ekf] ";
+
 			switch (check) {
 				case 0:
 					/* all ok */
@@ -735,26 +937,38 @@ FixedwingEstimator::task_main()
 				{
 					const char* str = "NaN in states, resetting";
 					warnx("%s", str);
-					mavlink_log_critical(_mavlink_fd, str);
+					mavlink_log_critical(_mavlink_fd, "%s%s", ekfname, str);
 					break;
 				}
 				case 2:
 				{
 					const char* str = "stale IMU data, resetting";
 					warnx("%s", str);
-					mavlink_log_critical(_mavlink_fd, str);
+					mavlink_log_critical(_mavlink_fd, "%s%s", ekfname, str);
 					break;
 				}
 				case 3:
 				{
-					const char* str = "switching dynamic / static state";
+					const char* str = "switching to dynamic state";
 					warnx("%s", str);
-					mavlink_log_critical(_mavlink_fd, str);
+					mavlink_log_info(_mavlink_fd, "%s%s", ekfname, str);
 					break;
 				}
+
+				default:
+				{
+					const char* str = "unknown reset condition";
+					warnx("%s", str);
+					mavlink_log_critical(_mavlink_fd, "%s%s", ekfname, str);
+				}
 			}
 
-			// If non-zero, we got a problem
+			// warn on fatal resets
+			if (check == 1) {
+				warnx("NUMERIC ERROR IN FILTER");
+			}
+
+			// If non-zero, we got a filter reset
 			if (check) {
 
 				struct ekf_status_report ekf_report;
@@ -770,7 +984,7 @@ FixedwingEstimator::task_main()
 				rep.kalman_gain_nan = ekf_report.kalmanGainsNaN;
 
 				// Copy all states or at least all that we can fit
-				int i = 0;
+				unsigned 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;
@@ -786,6 +1000,23 @@ FixedwingEstimator::task_main()
 				} else {
 					_estimator_status_pub = orb_advertise(ORB_ID(estimator_status), &rep);
 				}
+
+				/* set sensors to de-initialized state */
+				_gyro_valid = false;
+				_accel_valid = false;
+				_mag_valid = false;
+
+				_baro_init = false;
+				_gps_initialized = false;
+				last_sensor_timestamp = hrt_absolute_time();
+				last_run = last_sensor_timestamp;
+
+				_ekf->ZeroVariables();
+				_ekf->dtIMU = 0.01f;
+
+				// Let the system re-initialize itself
+				continue;
+
 			}
 
 
@@ -793,52 +1024,70 @@ FixedwingEstimator::task_main()
 			 *    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(&_filter_start_time) > FILTER_INIT_DELAY) && _baro_init && _gyro_valid && _accel_valid && _mag_valid) {
 
+				float initVelNED[3];
 
-			if (hrt_elapsed_time(&start_time) > 100000) {
+				if (!_gps_initialized && _gps.fix_type > 2 && _gps.eph_m < _parameters.pos_stddev_threshold && _gps.epv_m < _parameters.pos_stddev_threshold) {
 
-				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;
+					initVelNED[0] = _gps.vel_n_m_s;
+					initVelNED[1] = _gps.vel_e_m_s;
+					initVelNED[2] = _gps.vel_d_m_s;
 
-					double lat = _gps.lat * 1e-7;
-					double lon = _gps.lon * 1e-7;
-					float alt = _gps.alt * 1e-3;
+					// GPS is in scaled integers, convert
+					double lat = _gps.lat / 1.0e7;
+					double lon = _gps.lon / 1.0e7;
+					float gps_alt = _gps.alt / 1e3f;
 
-					_ekf->InitialiseFilter(_ekf->velNED);
+					// Set up height correctly
+					orb_copy(ORB_ID(sensor_baro), _baro_sub, &_baro);
+					_baro_gps_offset = _baro_ref - _baro.altitude;
+					_ekf->baroHgt = _baro.altitude;
+					_ekf->hgtMea = 1.0f * (_ekf->baroHgt - (_baro_ref));
+
+					// Set up position variables correctly
+					_ekf->GPSstatus = _gps.fix_type;
+
+					_ekf->gpsLat = math::radians(lat);
+					_ekf->gpsLon = math::radians(lon) - M_PI;
+					_ekf->gpsHgt = gps_alt;
+
+					// Look up mag declination based on current position
+					float declination = math::radians(get_mag_declination(lat, lon));
+
+					_ekf->InitialiseFilter(initVelNED, math::radians(lat), math::radians(lon) - M_PI, gps_alt, declination);
 
 					// Initialize projection
-					_local_pos.ref_lat = _gps.lat;
-					_local_pos.ref_lon = _gps.lon;
-					_local_pos.ref_alt = alt;
+					_local_pos.ref_lat = lat;
+					_local_pos.ref_lon = lon;
+					_local_pos.ref_alt = gps_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(&_pos_ref, lat, lon);
-					mavlink_log_info(_mavlink_fd, "[position estimator] init ref: lat=%.7f, lon=%.7f, alt=%.2f", lat, lon, alt);
+					mavlink_log_info(_mavlink_fd, "[ekf] ref: LA %.4f,LO %.4f,ALT %.2f", lat, lon, (double)gps_alt);
+					warnx("HOME/REF: LA %8.4f,LO %8.4f,ALT %8.2f V: %8.4f %8.4f %8.4f", lat, lon, (double)gps_alt,
+						(double)_ekf->velNED[0], (double)_ekf->velNED[1], (double)_ekf->velNED[2]);
+					warnx("BARO: %8.4f m / ref: %8.4f m / gps offs: %8.4f m", _ekf->baroHgt, _baro_ref, _baro_gps_offset);
+					warnx("GPS: eph: %8.4f, epv: %8.4f, declination: %8.4f", (double)_gps.eph_m, (double)_gps.epv_m, (double)math::degrees(declination));
 
 					_gps_initialized = true;
 
 				} else if (!_ekf->statesInitialised) {
-					_ekf->velNED[0] = 0.0f;
-					_ekf->velNED[1] = 0.0f;
-					_ekf->velNED[2] = 0.0f;
+
+					initVelNED[0] = 0.0f;
+					initVelNED[1] = 0.0f;
+					initVelNED[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);
+
+					_local_pos.ref_alt = _baro_ref;
+					_baro_gps_offset = 0.0f;
+
+					_ekf->InitialiseFilter(initVelNED, 0.0, 0.0, 0.0f, 0.0f);
 				}
 			}
 
@@ -872,10 +1121,10 @@ FixedwingEstimator::task_main()
 
 				// 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)) {
+				if ((dt >= (_ekf->covTimeStepMax - _ekf->dtIMU)) || (_ekf->summedDelAng.length() > _ekf->covDelAngMax)) {
 					_ekf->CovariancePrediction(dt);
-					_ekf->summedDelAng = _ekf->summedDelAng.zero();
-					_ekf->summedDelVel = _ekf->summedDelVel.zero();
+					_ekf->summedDelAng.zero();
+					_ekf->summedDelVel.zero();
 					dt = 0.0f;
 				}
 
@@ -926,9 +1175,9 @@ FixedwingEstimator::task_main()
 				_ekf->fusePosData = false;
 			}
 
-			if (newAdsData && _ekf->statesInitialised) {
+			if (newHgtData && _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->hgtMea = 1.0f * (_ekf->baroHgt - _baro_ref);
 				_ekf->fuseHgtData = true;
 				// recall states stored at time of measurement after adjusting for delays
 				_ekf->RecallStates(_ekf->statesAtHgtTime, (IMUmsec - _parameters.height_delay_ms));
@@ -1017,7 +1266,8 @@ FixedwingEstimator::task_main()
 				_local_pos.timestamp = last_sensor_timestamp;
 				_local_pos.x = _ekf->states[7];
 				_local_pos.y = _ekf->states[8];
-				_local_pos.z = _ekf->states[9];
+				// XXX need to announce change of Z reference somehow elegantly
+				_local_pos.z = _ekf->states[9] - _baro_gps_offset;
 
 				_local_pos.vx = _ekf->states[4];
 				_local_pos.vy = _ekf->states[5];
@@ -1050,6 +1300,8 @@ FixedwingEstimator::task_main()
 					_global_pos.lat = est_lat;
 					_global_pos.lon = est_lon;
 					_global_pos.time_gps_usec = _gps.time_gps_usec;
+					_global_pos.eph = _gps.eph_m;
+					_global_pos.epv = _gps.epv_m;
 				}
 
 				if (_local_pos.v_xy_valid) {
@@ -1060,8 +1312,8 @@ FixedwingEstimator::task_main()
 					_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);
+				/* local pos alt is negative, change sign and add alt offsets */
+				_global_pos.alt = _local_pos.ref_alt + _baro_gps_offset + (-_local_pos.z);
 
 				if (_local_pos.v_z_valid) {
 					_global_pos.vel_d = _local_pos.vz;
@@ -1102,7 +1354,7 @@ FixedwingEstimator::start()
 	ASSERT(_estimator_task == -1);
 
 	/* start the task */
-	_estimator_task = task_spawn_cmd("fw_att_pos_estimator",
+	_estimator_task = task_spawn_cmd("ekf_att_pos_estimator",
 					 SCHED_DEFAULT,
 					 SCHED_PRIORITY_MAX - 40,
 					 6000,
@@ -1136,7 +1388,8 @@ FixedwingEstimator::print_status()
 	// 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("dtIMU: %8.6f IMUmsec: %d\n", (double)_ekf->dtIMU, (int)IMUmsec);
+	printf("ref alt: %8.6f\n", (double)_local_pos.ref_alt);
 	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]);
@@ -1180,13 +1433,13 @@ int FixedwingEstimator::trip_nan() {
 		_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 #1 with NaN values");
+		_ekf->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 #2 with NaN values");
+		_ekf->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
@@ -1208,10 +1461,10 @@ int FixedwingEstimator::trip_nan() {
 	return ret;
 }
 
-int fw_att_pos_estimator_main(int argc, char *argv[])
+int ekf_att_pos_estimator_main(int argc, char *argv[])
 {
 	if (argc < 1)
-		errx(1, "usage: fw_att_pos_estimator {start|stop|status}");
+		errx(1, "usage: ekf_att_pos_estimator {start|stop|status}");
 
 	if (!strcmp(argv[1], "start")) {
 
diff --git a/src/modules/ekf_att_pos_estimator/fw_att_pos_estimator_params.c b/src/modules/ekf_att_pos_estimator/fw_att_pos_estimator_params.c
new file mode 100644
index 000000000..d2c6e1f15
--- /dev/null
+++ b/src/modules/ekf_att_pos_estimator/fw_att_pos_estimator_params.c
@@ -0,0 +1,271 @@
+/****************************************************************************
+ *
+ *   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);
+
+/**
+ * Airspeed measurement noise.
+ *
+ * Increasing this value will make the filter trust this sensor
+ * less and trust other sensors more.
+ *
+ * @min 0.5
+ * @max 5.0
+ * @group Position Estimator
+ */
+PARAM_DEFINE_FLOAT(PE_EAS_NOISE, 1.4f);
+
+/**
+ * Velocity measurement noise in north-east (horizontal) direction.
+ *
+ * Generic default: 0.3, multicopters: 0.5, ground vehicles: 0.5
+ *
+ * @min 0.05
+ * @max 5.0
+ * @group Position Estimator
+ */
+PARAM_DEFINE_FLOAT(PE_VELNE_NOISE, 0.3f);
+
+/**
+ * Velocity noise in down (vertical) direction
+ *
+ * Generic default: 0.5, multicopters: 0.7, ground vehicles: 0.7
+ *
+ * @min 0.05
+ * @max 5.0
+ * @group Position Estimator
+ */
+PARAM_DEFINE_FLOAT(PE_VELD_NOISE, 0.5f);
+
+/**
+ * Position noise in north-east (horizontal) direction
+ *
+ * Generic defaults: 0.5, multicopters: 0.5, ground vehicles: 0.5
+ *
+ * @min 0.1
+ * @max 10.0
+ * @group Position Estimator
+ */
+PARAM_DEFINE_FLOAT(PE_POSNE_NOISE, 0.5f);
+
+/**
+ * Position noise in down (vertical) direction
+ *
+ * Generic defaults: 0.5, multicopters: 1.0, ground vehicles: 1.0
+ *
+ * @min 0.1
+ * @max 10.0
+ * @group Position Estimator
+ */
+PARAM_DEFINE_FLOAT(PE_POSD_NOISE, 0.5f);
+
+/**
+ * Magnetometer measurement noise
+ *
+ * Generic defaults: 0.05, multicopters: 0.05, ground vehicles: 0.05
+ *
+ * @min 0.1
+ * @max 10.0
+ * @group Position Estimator
+ */
+PARAM_DEFINE_FLOAT(PE_MAG_NOISE, 0.05f);
+
+/**
+ * Gyro process noise
+ *
+ * Generic defaults: 0.015, multicopters: 0.015, ground vehicles: 0.015.
+ * This noise controls how much the filter trusts the gyro measurements.
+ * Increasing it makes the filter trust the gyro less and other sensors more.
+ *
+ * @min 0.001
+ * @max 0.05
+ * @group Position Estimator
+ */
+PARAM_DEFINE_FLOAT(PE_GYRO_PNOISE, 0.015f);
+
+/**
+ * Accelerometer process noise
+ *
+ * Generic defaults: 0.25, multicopters: 0.25, ground vehicles: 0.25.
+ * Increasing this value makes the filter trust the accelerometer less
+ * and other sensors more.
+ *
+ * @min 0.05
+ * @max 1.0
+ * @group Position Estimator
+ */
+PARAM_DEFINE_FLOAT(PE_ACC_PNOISE, 0.25f);
+
+/**
+ * Gyro bias estimate process noise
+ *
+ * Generic defaults: 1e-07f, multicopters: 1e-07f, ground vehicles: 1e-07f.
+ * Increasing this value will make the gyro bias converge faster but noisier.
+ *
+ * @min 0.0000001
+ * @max 0.00001
+ * @group Position Estimator
+ */
+PARAM_DEFINE_FLOAT(PE_GBIAS_PNOISE, 1e-07f);
+
+/**
+ * Accelerometer bias estimate process noise
+ *
+ * Generic defaults: 0.0001f, multicopters: 0.0001f, ground vehicles: 0.0001f.
+ * Increasing this value makes the bias estimation faster and noisier.
+ *
+ * @min 0.0001
+ * @max 0.001
+ * @group Position Estimator
+ */
+PARAM_DEFINE_FLOAT(PE_ABIAS_PNOISE, 0.0001f);
+
+/**
+ * Magnetometer earth frame offsets process noise
+ *
+ * Generic defaults: 0.0001, multicopters: 0.0001, ground vehicles: 0.0001.
+ * Increasing this value makes the magnetometer earth bias estimate converge
+ * faster but also noisier.
+ *
+ * @min 0.0001
+ * @max 0.01
+ * @group Position Estimator
+ */
+PARAM_DEFINE_FLOAT(PE_MAGE_PNOISE, 0.0003f);
+
+/**
+ * Magnetometer body frame offsets process noise
+ *
+ * Generic defaults: 0.0003, multicopters: 0.0003, ground vehicles: 0.0003.
+ * Increasing this value makes the magnetometer body bias estimate converge faster
+ * but also noisier.
+ *
+ * @min 0.0001
+ * @max 0.01
+ * @group Position Estimator
+ */
+PARAM_DEFINE_FLOAT(PE_MAGB_PNOISE, 0.0003f);
+
+/**
+ * Threshold for filter initialization.
+ *
+ * If the standard deviation of the GPS position estimate is below this threshold
+ * in meters, the filter will initialize.
+ *
+ * @min 0.3
+ * @max 10.0
+ * @group Position Estimator
+ */
+PARAM_DEFINE_FLOAT(PE_POSDEV_INIT, 5.0f);
diff --git a/src/modules/fw_att_pos_estimator/module.mk b/src/modules/ekf_att_pos_estimator/module.mk
index c992959e0..30955d0dd 100644
--- a/src/modules/fw_att_pos_estimator/module.mk
+++ b/src/modules/ekf_att_pos_estimator/module.mk
@@ -35,7 +35,7 @@
 # Main Attitude and Position Estimator for Fixed Wing Aircraft
 #
 
-MODULE_COMMAND	= fw_att_pos_estimator
+MODULE_COMMAND	= ekf_att_pos_estimator
 
 SRCS		= fw_att_pos_estimator_main.cpp \
 		  fw_att_pos_estimator_params.c \
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 1c411fa06..1f3e9f098 100644
--- a/src/modules/fw_att_control/fw_att_control_main.cpp
+++ b/src/modules/fw_att_control/fw_att_control_main.cpp
@@ -783,7 +783,7 @@ FixedwingAttitudeControl::task_main()
 					_actuators.control[1] = (isfinite(pitch_u)) ? pitch_u + _parameters.trim_pitch : _parameters.trim_pitch;
 					if (!isfinite(pitch_u)) {
 						warnx("pitch_u %.4f, _yaw_ctrl.get_desired_rate() %.4f, airspeed %.4f, airspeed_scaling %.4f, roll_sp %.4f, pitch_sp %.4f, _roll_ctrl.get_desired_rate() %.4f, _pitch_ctrl.get_desired_rate() %.4f att_sp.roll_body %.4f",
-								pitch_u, _yaw_ctrl.get_desired_rate(), airspeed, airspeed_scaling, roll_sp, pitch_sp, _roll_ctrl.get_desired_rate(), _pitch_ctrl.get_desired_rate(), _att_sp.roll_body);
+								(double)pitch_u, (double)_yaw_ctrl.get_desired_rate(), (double)airspeed, (double)airspeed_scaling, (double)roll_sp, (double)pitch_sp, (double)_roll_ctrl.get_desired_rate(), (double)_pitch_ctrl.get_desired_rate(), (double)_att_sp.roll_body);
 					}
 
 					float yaw_u = _yaw_ctrl.control_bodyrate(_att.roll, _att.pitch,
@@ -792,16 +792,16 @@ FixedwingAttitudeControl::task_main()
 							_parameters.airspeed_min, _parameters.airspeed_max, airspeed, airspeed_scaling, lock_integrator);
 					_actuators.control[2] = (isfinite(yaw_u)) ? yaw_u + _parameters.trim_yaw : _parameters.trim_yaw;
 					if (!isfinite(yaw_u)) {
-						warnx("yaw_u %.4f", yaw_u);
+						warnx("yaw_u %.4f", (double)yaw_u);
 					}
 
 					/* throttle passed through */
 					_actuators.control[3] = (isfinite(throttle_sp)) ? throttle_sp : 0.0f;
 					if (!isfinite(throttle_sp)) {
-						warnx("throttle_sp %.4f", throttle_sp);
+						warnx("throttle_sp %.4f", (double)throttle_sp);
 					}
 				} else {
-					warnx("Non-finite setpoint roll_sp: %.4f, pitch_sp %.4f", roll_sp, pitch_sp);
+					warnx("Non-finite setpoint roll_sp: %.4f, pitch_sp %.4f", (double)roll_sp, (double)pitch_sp);
 				}
 
 				/*
diff --git a/src/modules/fw_att_pos_estimator/estimator.cpp b/src/modules/fw_att_pos_estimator/estimator.cpp
deleted file mode 100644
index c31217393..000000000
--- a/src/modules/fw_att_pos_estimator/estimator.cpp
+++ /dev/null
@@ -1,2248 +0,0 @@
-#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/mavlink/mavlink_main.cpp b/src/modules/mavlink/mavlink_main.cpp
index dd88b0949..199e85305 100644
--- a/src/modules/mavlink/mavlink_main.cpp
+++ b/src/modules/mavlink/mavlink_main.cpp
@@ -189,9 +189,18 @@ mavlink_send_uart_bytes(mavlink_channel_t channel, const uint8_t *ch, int length
 	/* If the wait until transmit flag is on, only transmit after we've received messages.
 	   Otherwise, transmit all the time. */
 	if (instance->should_transmit()) {
-	   ssize_t ret = write(uart, ch, desired);
+
+		/* check if there is space in the buffer, let it overflow else */
+		if (!ioctl(uart, FIONWRITE, (unsigned long)&buf_free)) {
+
+			if (desired > buf_free) {
+				desired = buf_free;
+			}
+		}
+
+		ssize_t ret = write(uart, ch, desired);
 		if (ret != desired) {
-			// XXX do something here, but change to using FIONWRITE and OS buf size for detection
+			warnx("TX FAIL");
 		}
 	}
 
@@ -1523,6 +1532,8 @@ void Mavlink::mavlink_wpm_message_handler(const mavlink_message_t *msg)
 void
 Mavlink::mavlink_missionlib_send_message(mavlink_message_t *msg)
 {
+	uint8_t missionlib_msg_buf[MAVLINK_MAX_PACKET_LEN];
+
 	uint16_t len = mavlink_msg_to_send_buffer(missionlib_msg_buf, msg);
 
 	mavlink_send_uart_bytes(_channel, missionlib_msg_buf, len);
@@ -2193,7 +2204,7 @@ Mavlink::start(int argc, char *argv[])
 	task_spawn_cmd(buf,
 		       SCHED_DEFAULT,
 		       SCHED_PRIORITY_DEFAULT,
-		       2048,
+		       2000,
 		       (main_t)&Mavlink::start_helper,
 		       (const char **)argv);
 
diff --git a/src/modules/mavlink/mavlink_main.h b/src/modules/mavlink/mavlink_main.h
index 1bf51fd31..c7a7d32f8 100644
--- a/src/modules/mavlink/mavlink_main.h
+++ b/src/modules/mavlink/mavlink_main.h
@@ -237,7 +237,6 @@ private:
 
 	orb_advert_t	_mission_pub;
 	struct mission_s mission;
-	uint8_t missionlib_msg_buf[MAVLINK_MAX_PACKET_LEN];
 	MAVLINK_MODE _mode;
 
 	uint8_t _mavlink_wpm_comp_id;
diff --git a/src/modules/mavlink/module.mk b/src/modules/mavlink/module.mk
index dcca11977..f532e26fe 100644
--- a/src/modules/mavlink/module.mk
+++ b/src/modules/mavlink/module.mk
@@ -48,3 +48,5 @@ SRCS		 += mavlink_main.cpp \
 INCLUDE_DIRS	 += $(MAVLINK_SRC)/include/mavlink
 
 MAXOPTIMIZATION	 = -Os
+
+MODULE_STACKSIZE = 1024
diff --git a/src/modules/mc_att_control/mc_att_control_main.cpp b/src/modules/mc_att_control/mc_att_control_main.cpp
index a69153bf0..b23166a5e 100644
--- a/src/modules/mc_att_control/mc_att_control_main.cpp
+++ b/src/modules/mc_att_control/mc_att_control_main.cpp
@@ -1,9 +1,6 @@
 /****************************************************************************
  *
  *   Copyright (c) 2013, 2014 PX4 Development Team. All rights reserved.
- *   Author: @author Tobias Naegeli <naegelit@student.ethz.ch>
- *           @author Lorenz Meier <lm@inf.ethz.ch>
- *           @author Anton Babushkin <anton.babushkin@me.com>
  *
  * Redistribution and use in source and binary forms, with or without
  * modification, are permitted provided that the following conditions
@@ -35,9 +32,13 @@
  ****************************************************************************/
 
 /**
- * @file mc_att_control_main.c
+ * @file mc_att_control_main.cpp
  * Multicopter attitude controller.
  *
+ * @author Tobias Naegeli <naegelit@student.ethz.ch>
+ * @author Lorenz Meier <lm@inf.ethz.ch>
+ * @author Anton Babushkin <anton.babushkin@me.com>
+ *
  * The controller has two loops: P loop for angular error and PD loop for angular rate error.
  * Desired rotation calculated keeping in mind that yaw response is normally slower than roll/pitch.
  * For small deviations controller rotates copter to have shortest path of thrust vector and independently rotates around yaw,
@@ -506,6 +507,14 @@ MulticopterAttitudeControl::control_attitude(float dt)
 			/* move yaw setpoint */
 			yaw_sp_move_rate = _manual_control_sp.r * _params.man_yaw_max;
 			_v_att_sp.yaw_body = _wrap_pi(_v_att_sp.yaw_body + yaw_sp_move_rate * dt);
+			float yaw_offs_max = _params.man_yaw_max / _params.att_p(2);
+			float yaw_offs = _wrap_pi(_v_att_sp.yaw_body - _v_att.yaw);
+			if (yaw_offs < - yaw_offs_max) {
+				_v_att_sp.yaw_body = _wrap_pi(_v_att.yaw - yaw_offs_max);
+
+			} else if (yaw_offs > yaw_offs_max) {
+				_v_att_sp.yaw_body = _wrap_pi(_v_att.yaw + yaw_offs_max);
+			}
 			_v_att_sp.R_valid = false;
 			publish_att_sp = true;
 		}
@@ -817,7 +826,7 @@ MulticopterAttitudeControl::start()
 	_control_task = task_spawn_cmd("mc_att_control",
 				       SCHED_DEFAULT,
 				       SCHED_PRIORITY_MAX - 5,
-				       2048,
+				       2000,
 				       (main_t)&MulticopterAttitudeControl::task_main_trampoline,
 				       nullptr);
 
diff --git a/src/modules/mc_att_control/mc_att_control_params.c b/src/modules/mc_att_control/mc_att_control_params.c
index 19134c7b4..ad38a0a03 100644
--- a/src/modules/mc_att_control/mc_att_control_params.c
+++ b/src/modules/mc_att_control/mc_att_control_params.c
@@ -1,9 +1,6 @@
 /****************************************************************************
  *
  *   Copyright (c) 2013, 2014 PX4 Development Team. All rights reserved.
- *   Author: @author Tobias Naegeli <naegelit@student.ethz.ch>
- *   	     @author Lorenz Meier <lm@inf.ethz.ch>
- *           @author Anton Babushkin <anton.babushkin@me.com>
  *
  * Redistribution and use in source and binary forms, with or without
  * modification, are permitted provided that the following conditions
@@ -37,6 +34,10 @@
 /**
  * @file mc_att_control_params.c
  * Parameters for multicopter attitude controller.
+ *
+ * @author Tobias Naegeli <naegelit@student.ethz.ch>
+ * @author Lorenz Meier <lm@inf.ethz.ch>
+ * @author Anton Babushkin <anton.babushkin@me.com>
  */
 
 #include <systemlib/param/param.h>
diff --git a/src/modules/mc_pos_control/mc_pos_control_main.cpp b/src/modules/mc_pos_control/mc_pos_control_main.cpp
index 6e611d4ab..09960d106 100644
--- a/src/modules/mc_pos_control/mc_pos_control_main.cpp
+++ b/src/modules/mc_pos_control/mc_pos_control_main.cpp
@@ -1062,7 +1062,7 @@ MulticopterPositionControl::start()
 	_control_task = task_spawn_cmd("mc_pos_control",
 				       SCHED_DEFAULT,
 				       SCHED_PRIORITY_MAX - 5,
-				       2048,
+				       2000,
 				       (main_t)&MulticopterPositionControl::task_main_trampoline,
 				       nullptr);
 
diff --git a/src/modules/mc_pos_control/mc_pos_control_params.c b/src/modules/mc_pos_control/mc_pos_control_params.c
index dacdd46f0..05ab5769b 100644
--- a/src/modules/mc_pos_control/mc_pos_control_params.c
+++ b/src/modules/mc_pos_control/mc_pos_control_params.c
@@ -35,6 +35,8 @@
 /**
  * @file mc_pos_control_params.c
  * Multicopter position controller parameters.
+ *
+ * @author Anton Babushkin <anton.babushkin@me.com>
  */
 
 #include <systemlib/param/param.h>
diff --git a/src/modules/navigator/geofence.cpp b/src/modules/navigator/geofence.cpp
index f452a85f7..bc8dbca50 100644
--- a/src/modules/navigator/geofence.cpp
+++ b/src/modules/navigator/geofence.cpp
@@ -1,8 +1,6 @@
 /****************************************************************************
  *
  *   Copyright (c) 2013 PX4 Development Team. All rights reserved.
- *   Author: @author Jean Cyr <jean.m.cyr@gmail.com>
- *           @author Thomas Gubler <thomasgubler@gmail.com>
  *
  * Redistribution and use in source and binary forms, with or without
  * modification, are permitted provided that the following conditions
@@ -35,6 +33,9 @@
 /**
  * @file geofence.cpp
  * Provides functions for handling the geofence
+ *
+ * @author Jean Cyr <jean.m.cyr@gmail.com>
+ * @author Thomas Gubler <thomasgubler@gmail.com>
  */
 #include "geofence.h"
 
diff --git a/src/modules/navigator/geofence.h b/src/modules/navigator/geofence.h
index 9628b7271..2eb126ab5 100644
--- a/src/modules/navigator/geofence.h
+++ b/src/modules/navigator/geofence.h
@@ -1,8 +1,6 @@
 /****************************************************************************
  *
  *   Copyright (c) 2013 PX4 Development Team. All rights reserved.
- *   Author: @author Jean Cyr <jean.m.cyr@gmail.com>
- *           @author Thomas Gubler <thomasgubler@gmail.com>
  *
  * Redistribution and use in source and binary forms, with or without
  * modification, are permitted provided that the following conditions
@@ -35,6 +33,9 @@
 /**
  * @file geofence.h
  * Provides functions for handling the geofence
+ *
+ * @author Jean Cyr <jean.m.cyr@gmail.com>
+ * @author Thomas Gubler <thomasgubler@gmail.com>
  */
 
 #ifndef GEOFENCE_H_
diff --git a/src/modules/navigator/geofence_params.c b/src/modules/navigator/geofence_params.c
index 5831a0ca9..653b1ad84 100644
--- a/src/modules/navigator/geofence_params.c
+++ b/src/modules/navigator/geofence_params.c
@@ -1,7 +1,6 @@
 /****************************************************************************
  *
  *   Copyright (c) 2013 PX4 Development Team. All rights reserved.
- *   Author: Lorenz Meier <lm@inf.ethz.ch>
  *
  * Redistribution and use in source and binary forms, with or without
  * modification, are permitted provided that the following conditions
diff --git a/src/modules/navigator/mission_feasibility_checker.cpp b/src/modules/navigator/mission_feasibility_checker.cpp
index 646ab6ab9..2f45f2267 100644
--- a/src/modules/navigator/mission_feasibility_checker.cpp
+++ b/src/modules/navigator/mission_feasibility_checker.cpp
@@ -1,8 +1,6 @@
 /****************************************************************************
  *
  *   Copyright (c) 2013 PX4 Development Team. All rights reserved.
- *   Author: @author Lorenz Meier <lm@inf.ethz.ch>
- *           @author Thomas Gubler <thomasgubler@student.ethz.ch>
  *
  * Redistribution and use in source and binary forms, with or without
  * modification, are permitted provided that the following conditions
@@ -35,6 +33,9 @@
 /**
  * @file mission_feasibility_checker.cpp
  * Provides checks if mission is feasible given the navigation capabilities
+ *
+ * @author Lorenz Meier <lm@inf.ethz.ch>
+ * @author Thomas Gubler <thomasgubler@student.ethz.ch>
  */
 
 #include "mission_feasibility_checker.h"
diff --git a/src/modules/navigator/mission_feasibility_checker.h b/src/modules/navigator/mission_feasibility_checker.h
index 7a0b2a296..f98e28462 100644
--- a/src/modules/navigator/mission_feasibility_checker.h
+++ b/src/modules/navigator/mission_feasibility_checker.h
@@ -1,8 +1,6 @@
 /****************************************************************************
  *
  *   Copyright (c) 2013 PX4 Development Team. All rights reserved.
- *   Author: @author Lorenz Meier <lm@inf.ethz.ch>
- *           @author Thomas Gubler <thomasgubler@student.ethz.ch>
  *
  * Redistribution and use in source and binary forms, with or without
  * modification, are permitted provided that the following conditions
@@ -35,7 +33,11 @@
 /**
  * @file mission_feasibility_checker.h
  * Provides checks if mission is feasible given the navigation capabilities
+ *
+ * @author Lorenz Meier <lm@inf.ethz.ch>
+ * @author Thomas Gubler <thomasgubler@student.ethz.ch>
  */
+
 #ifndef MISSION_FEASIBILITY_CHECKER_H_
 #define MISSION_FEASIBILITY_CHECKER_H_
 
diff --git a/src/modules/navigator/module.mk b/src/modules/navigator/module.mk
index 55f8a4caa..6ea9dec2b 100644
--- a/src/modules/navigator/module.mk
+++ b/src/modules/navigator/module.mk
@@ -45,3 +45,5 @@ SRCS		= navigator_main.cpp \
 		  geofence_params.c
 
 INCLUDE_DIRS	 += $(MAVLINK_SRC)/include/mavlink
+
+MODULE_STACKSIZE = 1200
diff --git a/src/modules/navigator/navigator_main.cpp b/src/modules/navigator/navigator_main.cpp
index 1a45e1345..87c893079 100644
--- a/src/modules/navigator/navigator_main.cpp
+++ b/src/modules/navigator/navigator_main.cpp
@@ -1,10 +1,6 @@
 /****************************************************************************
  *
  *   Copyright (c) 2013, 2014 PX4 Development Team. All rights reserved.
- *   Author: @author Lorenz Meier <lm@inf.ethz.ch>
- *           @author Jean Cyr <jean.m.cyr@gmail.com>
- *           @author Julian Oes <joes@student.ethz.ch>
- *           @author Anton Babushkin <anton.babushkin@me.com>
  *
  * Redistribution and use in source and binary forms, with or without
  * modification, are permitted provided that the following conditions
@@ -35,7 +31,7 @@
  *
  ****************************************************************************/
 /**
- * @file navigator_main.c
+ * @file navigator_main.cpp
  * Implementation of the main navigation state machine.
  *
  * Handles missions, geo fencing and failsafe navigation behavior.
@@ -852,7 +848,7 @@ Navigator::start()
 	_navigator_task = task_spawn_cmd("navigator",
 					 SCHED_DEFAULT,
 					 SCHED_PRIORITY_MAX - 5,
-					 2048,
+					 2000,
 					 (main_t)&Navigator::task_main_trampoline,
 					 nullptr);
 
diff --git a/src/modules/navigator/navigator_mission.cpp b/src/modules/navigator/navigator_mission.cpp
index 72dddebfe..49fc62785 100644
--- a/src/modules/navigator/navigator_mission.cpp
+++ b/src/modules/navigator/navigator_mission.cpp
@@ -1,7 +1,6 @@
 /****************************************************************************
  *
  *   Copyright (c) 2013 PX4 Development Team. All rights reserved.
- *   Author: @author Julian Oes <joes@student.ethz.ch>
  *
  * Redistribution and use in source and binary forms, with or without
  * modification, are permitted provided that the following conditions
@@ -34,6 +33,8 @@
 /**
  * @file navigator_mission.cpp
  * Helper class to access missions
+ *
+ * @author Julian Oes <joes@student.ethz.ch>
  */
 
 #include <string.h>
diff --git a/src/modules/navigator/navigator_mission.h b/src/modules/navigator/navigator_mission.h
index 2bd4da82e..b0f88e016 100644
--- a/src/modules/navigator/navigator_mission.h
+++ b/src/modules/navigator/navigator_mission.h
@@ -1,7 +1,6 @@
 /****************************************************************************
  *
  *   Copyright (c) 2013 PX4 Development Team. All rights reserved.
- *   Author: @author Julian Oes <joes@student.ethz.ch>
  *
  * Redistribution and use in source and binary forms, with or without
  * modification, are permitted provided that the following conditions
@@ -34,6 +33,8 @@
 /**
  * @file navigator_mission.h
  * Helper class to access missions
+ *
+ * @author Julian Oes <joes@student.ethz.ch>
  */
 
 #ifndef NAVIGATOR_MISSION_H
diff --git a/src/modules/navigator/navigator_params.c b/src/modules/navigator/navigator_params.c
index 9ef359c6d..5139283b6 100644
--- a/src/modules/navigator/navigator_params.c
+++ b/src/modules/navigator/navigator_params.c
@@ -1,9 +1,6 @@
 /****************************************************************************
  *
  *   Copyright (c) 2013 PX4 Development Team. All rights reserved.
- *   Author: @author Lorenz Meier <lm@inf.ethz.ch>
- *           @author Julian Oes <joes@student.ethz.ch>
- *           @author Anton Babushkin <anton.babushkin@me.com>
  *
  * Redistribution and use in source and binary forms, with or without
  * modification, are permitted provided that the following conditions
diff --git a/src/modules/navigator/navigator_state.h b/src/modules/navigator/navigator_state.h
index 6a1475c9b..476f93414 100644
--- a/src/modules/navigator/navigator_state.h
+++ b/src/modules/navigator/navigator_state.h
@@ -1,8 +1,42 @@
-/*
- * navigator_state.h
+/****************************************************************************
  *
- *  Created on: 27.01.2014
- *      Author: ton
+ *   Copyright (c) 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 navigator_state.h
+ *
+ * Navigator state
+ *
+ * @author Anton Babushkin <anton.babushkin@me.com>
  */
 
 #ifndef NAVIGATOR_STATE_H_
diff --git a/src/modules/px4iofirmware/controls.c b/src/modules/px4iofirmware/controls.c
index 56c5aa005..62e6b12cb 100644
--- a/src/modules/px4iofirmware/controls.c
+++ b/src/modules/px4iofirmware/controls.c
@@ -291,6 +291,7 @@ controls_tick() {
 
 		/* set RC OK flag, as we got an update */
 		r_status_flags |= PX4IO_P_STATUS_FLAGS_RC_OK;
+		r_raw_rc_flags |= PX4IO_P_RAW_RC_FLAGS_RC_OK;
 
 		/* if we have enough channels (5) to control the vehicle, the mapping is ok */
 		if (assigned_channels > 4) {
@@ -336,6 +337,9 @@ controls_tick() {
 			PX4IO_P_STATUS_FLAGS_OVERRIDE |
 			PX4IO_P_STATUS_FLAGS_RC_OK);
 
+		/* flag raw RC as lost */
+		r_raw_rc_flags &= ~(PX4IO_P_RAW_RC_FLAGS_RC_OK);
+
 		/* Mark all channels as invalid, as we just lost the RX */
 		r_rc_valid = 0;
 
@@ -405,8 +409,9 @@ ppm_input(uint16_t *values, uint16_t *num_values, uint16_t *frame_len)
 		if (*num_values > PX4IO_RC_INPUT_CHANNELS)
 			*num_values = PX4IO_RC_INPUT_CHANNELS;
 
-		for (unsigned i = 0; i < *num_values; i++)
+		for (unsigned i = 0; i < *num_values; i++) {
 			values[i] = ppm_buffer[i];
+		}
 
 		/* clear validity */
 		ppm_last_valid_decode = 0;
diff --git a/src/modules/px4iofirmware/protocol.h b/src/modules/px4iofirmware/protocol.h
index 6c20d6006..91975f2a0 100644
--- a/src/modules/px4iofirmware/protocol.h
+++ b/src/modules/px4iofirmware/protocol.h
@@ -142,6 +142,7 @@
 #define PX4IO_P_RAW_RC_FLAGS_FAILSAFE		(1 << 1) /* receiver is in failsafe mode */
 #define PX4IO_P_RAW_RC_FLAGS_RC_DSM11		(1 << 2) /* DSM decoding is 11 bit mode */
 #define PX4IO_P_RAW_RC_FLAGS_MAPPING_OK		(1 << 3) /* Channel mapping is ok */
+#define PX4IO_P_RAW_RC_FLAGS_RC_OK		(1 << 4) /* RC reception ok */
 
 #define PX4IO_P_RAW_RC_NRSSI			2	/* [2] Normalized RSSI value, 0: no reception, 255: perfect reception */
 #define PX4IO_P_RAW_RC_DATA			3	/* [1] + [2] Details about the RC source (PPM frame length, Spektrum protocol type) */
diff --git a/src/modules/sdlog2/module.mk b/src/modules/sdlog2/module.mk
index f53129688..a28d43e72 100644
--- a/src/modules/sdlog2/module.mk
+++ b/src/modules/sdlog2/module.mk
@@ -41,3 +41,5 @@ MODULE_PRIORITY = "SCHED_PRIORITY_MAX-30"
 
 SRCS = sdlog2.c \
        logbuffer.c
+
+MODULE_STACKSIZE = 1200
diff --git a/src/modules/sensors/module.mk b/src/modules/sensors/module.mk
index aa538fd6b..5b1bc5e86 100644
--- a/src/modules/sensors/module.mk
+++ b/src/modules/sensors/module.mk
@@ -40,3 +40,5 @@ MODULE_PRIORITY	= "SCHED_PRIORITY_MAX-5"
 
 SRCS		= sensors.cpp \
 		  sensor_params.c
+
+MODULE_STACKSIZE = 1200
diff --git a/src/modules/sensors/sensor_params.c b/src/modules/sensors/sensor_params.c
index 07be3560a..c021e1fde 100644
--- a/src/modules/sensors/sensor_params.c
+++ b/src/modules/sensors/sensor_params.c
@@ -488,6 +488,15 @@ PARAM_DEFINE_INT32(BAT_V_SCALE_IO, 10000);
  * @group Battery Calibration
  */
 PARAM_DEFINE_FLOAT(BAT_V_SCALING, 0.0082f);
+#elif CONFIG_ARCH_BOARD_AEROCORE
+/**
+ * Scaling factor for battery voltage sensor on AeroCore.
+ *
+ * For R70 = 133K, R71 = 10K --> scale = 1.8 * 143 / (4096*10) = 0.0063
+ *
+ * @group Battery Calibration
+ */
+PARAM_DEFINE_FLOAT(BAT_V_SCALING, 0.0063f);
 #else
 /**
  * Scaling factor for battery voltage sensor on FMU v1.
diff --git a/src/modules/sensors/sensors.cpp b/src/modules/sensors/sensors.cpp
index 18bf97f8d..c5954d02a 100644
--- a/src/modules/sensors/sensors.cpp
+++ b/src/modules/sensors/sensors.cpp
@@ -126,6 +126,12 @@
 #define ADC_AIRSPEED_VOLTAGE_CHANNEL	15
 #endif
 
+#ifdef CONFIG_ARCH_BOARD_AEROCORE
+#define ADC_BATTERY_VOLTAGE_CHANNEL	10
+#define ADC_BATTERY_CURRENT_CHANNEL	-1
+#define ADC_AIRSPEED_VOLTAGE_CHANNEL	-1
+#endif
+
 #define BATT_V_LOWPASS 0.001f
 #define BATT_V_IGNORE_THRESHOLD 3.5f
 
@@ -797,7 +803,7 @@ Sensors::accel_init()
 		/* set the driver to poll at 1000Hz */
 		ioctl(fd, SENSORIOCSPOLLRATE, 1000);
 
-#elif CONFIG_ARCH_BOARD_PX4FMU_V2
+#elif CONFIG_ARCH_BOARD_PX4FMU_V2 || CONFIG_ARCH_BOARD_AEROCORE
 
 		/* set the accel internal sampling rate up to at leat 800Hz */
 		ioctl(fd, ACCELIOCSSAMPLERATE, 800);
@@ -806,7 +812,7 @@ Sensors::accel_init()
 		ioctl(fd, SENSORIOCSPOLLRATE, 800);
 
 #else
-#error Need a board configuration, either CONFIG_ARCH_BOARD_PX4FMU_V1 or CONFIG_ARCH_BOARD_PX4FMU_V2
+#error Need a board configuration, either CONFIG_ARCH_BOARD_PX4FMU_V1, CONFIG_ARCH_BOARD_PX4FMU_V2 or CONFIG_ARCH_BOARD_AEROCORE
 
 #endif
 
@@ -1669,7 +1675,7 @@ Sensors::start()
 	_sensors_task = task_spawn_cmd("sensors_task",
 				       SCHED_DEFAULT,
 				       SCHED_PRIORITY_MAX - 5,
-				       2048,
+				       2000,
 				       (main_t)&Sensors::task_main_trampoline,
 				       nullptr);
 
diff --git a/src/modules/systemlib/pid/pid.c b/src/modules/systemlib/pid/pid.c
index 6a4e9392a..45f218a5b 100644
--- a/src/modules/systemlib/pid/pid.c
+++ b/src/modules/systemlib/pid/pid.c
@@ -53,7 +53,7 @@
 
 #define SIGMA 0.000001f
 
-__EXPORT void pid_init(PID_t *pid, uint8_t mode, float dt_min)
+__EXPORT void pid_init(PID_t *pid, pid_mode_t mode, float dt_min)
 {
 	pid->mode = mode;
 	pid->dt_min = dt_min;
diff --git a/src/systemcmds/mtd/24xxxx_mtd.c b/src/systemcmds/mtd/24xxxx_mtd.c
index e34be44e3..a3f9dffff 100644
--- a/src/systemcmds/mtd/24xxxx_mtd.c
+++ b/src/systemcmds/mtd/24xxxx_mtd.c
@@ -142,12 +142,9 @@ struct at24c_dev_s {
 	uint16_t              pagesize; /* 32, 63 */
 	uint16_t              npages;   /* 128, 256, 512, 1024 */
 
-	perf_counter_t        perf_reads;
-	perf_counter_t        perf_writes;
-	perf_counter_t        perf_resets;
-	perf_counter_t        perf_read_retries;
-	perf_counter_t        perf_read_errors;
-	perf_counter_t        perf_write_errors;
+	perf_counter_t        perf_transfers;
+	perf_counter_t        perf_resets_retries;
+	perf_counter_t        perf_errors;
 };
 
 /************************************************************************************
@@ -298,9 +295,9 @@ static ssize_t at24c_bread(FAR struct mtd_dev_s *dev, off_t startblock,
 
 		for (;;) {
 
-			perf_begin(priv->perf_reads);
+			perf_begin(priv->perf_transfers);
 			ret = I2C_TRANSFER(priv->dev, &msgv[0], 2);
-			perf_end(priv->perf_reads);
+			perf_end(priv->perf_transfers);
 
 			if (ret >= 0)
 				break;
@@ -314,10 +311,10 @@ static ssize_t at24c_bread(FAR struct mtd_dev_s *dev, off_t startblock,
 			 * XXX maybe do special first-read handling with optional
 			 * bus reset as well?
 			 */
-			perf_count(priv->perf_read_retries);
+			perf_count(priv->perf_resets_retries);
 
 			if (--tries == 0) {
-				perf_count(priv->perf_read_errors);
+				perf_count(priv->perf_errors);
 				return ERROR;
 			}
 		}
@@ -383,9 +380,9 @@ static ssize_t at24c_bwrite(FAR struct mtd_dev_s *dev, off_t startblock, size_t
 
 		for (;;) {
 
-			perf_begin(priv->perf_writes);
+			perf_begin(priv->perf_transfers);
 			ret = I2C_TRANSFER(priv->dev, &msgv[0], 1);
-			perf_end(priv->perf_writes);
+			perf_end(priv->perf_transfers);
 
 			if (ret >= 0)
 				break;
@@ -397,7 +394,7 @@ static ssize_t at24c_bwrite(FAR struct mtd_dev_s *dev, off_t startblock, size_t
 			 * poll for write completion.
 			 */
 			if (--tries == 0) {
-				perf_count(priv->perf_write_errors);
+				perf_count(priv->perf_errors);
 				return ERROR;
 			}
 		}
@@ -521,12 +518,9 @@ FAR struct mtd_dev_s *at24c_initialize(FAR struct i2c_dev_s *dev) {
 		priv->mtd.ioctl  = at24c_ioctl;
 		priv->dev        = dev;
 
-		priv->perf_reads = perf_alloc(PC_ELAPSED, "EEPROM read");
-		priv->perf_writes = perf_alloc(PC_ELAPSED, "EEPROM write");
-		priv->perf_resets = perf_alloc(PC_COUNT, "EEPROM reset");
-		priv->perf_read_retries = perf_alloc(PC_COUNT, "EEPROM read retries");
-		priv->perf_read_errors = perf_alloc(PC_COUNT, "EEPROM read errors");
-		priv->perf_write_errors = perf_alloc(PC_COUNT, "EEPROM write errors");
+		priv->perf_transfers = perf_alloc(PC_ELAPSED, "eeprom_trans");
+		priv->perf_resets_retries = perf_alloc(PC_COUNT, "eeprom_rst");
+		priv->perf_errors = perf_alloc(PC_COUNT, "eeprom_errs");
 	}
 
 	/* attempt to read to validate device is present */
@@ -548,9 +542,9 @@ FAR struct mtd_dev_s *at24c_initialize(FAR struct i2c_dev_s *dev) {
 		}
 	};
 
-	perf_begin(priv->perf_reads);
+	perf_begin(priv->perf_transfers);
 	int ret = I2C_TRANSFER(priv->dev, &msgv[0], 2);
-	perf_end(priv->perf_reads);
+	perf_end(priv->perf_transfers);
 
 	if (ret < 0) {
 		return NULL;
diff --git a/src/systemcmds/mtd/mtd.c b/src/systemcmds/mtd/mtd.c
index 0a88d40f3..a57eaafe7 100644
--- a/src/systemcmds/mtd/mtd.c
+++ b/src/systemcmds/mtd/mtd.c
@@ -160,7 +160,11 @@ static void
 ramtron_attach(void)
 {
 	/* find the right spi */
+#ifdef CONFIG_ARCH_BOARD_AEROCORE
+	struct spi_dev_s *spi = up_spiinitialize(4);
+#else
 	struct spi_dev_s *spi = up_spiinitialize(2);
+#endif
 	/* this resets the spi bus, set correct bus speed again */
 	SPI_SETFREQUENCY(spi, 10 * 1000 * 1000);
 	SPI_SETBITS(spi, 8);
diff --git a/src/systemcmds/nshterm/module.mk b/src/systemcmds/nshterm/module.mk
index a48588535..fe79ec3b7 100644
--- a/src/systemcmds/nshterm/module.mk
+++ b/src/systemcmds/nshterm/module.mk
@@ -38,4 +38,4 @@
 MODULE_COMMAND	 = nshterm
 SRCS		 = nshterm.c
 
-MODULE_STACKSIZE = 3000
+MODULE_STACKSIZE = 1500
diff --git a/src/systemcmds/param/module.mk b/src/systemcmds/param/module.mk
index 63f15ad28..f716eb71e 100644
--- a/src/systemcmds/param/module.mk
+++ b/src/systemcmds/param/module.mk
@@ -38,7 +38,8 @@
 MODULE_COMMAND	 = param
 SRCS		 = param.c
 
-MODULE_STACKSIZE = 4096
+# Note: measurements yielded a max of 900 bytes used.
+MODULE_STACKSIZE = 1800
 
 MAXOPTIMIZATION	 = -Os
 
diff --git a/src/systemcmds/param/param.c b/src/systemcmds/param/param.c
index 984d19bd9..d92ee88ef 100644
--- a/src/systemcmds/param/param.c
+++ b/src/systemcmds/param/param.c
@@ -61,7 +61,7 @@ static void	do_load(const char* param_file_name);
 static void	do_import(const char* param_file_name);
 static void	do_show(const char* search_string);
 static void	do_show_print(void *arg, param_t param);
-static void	do_set(const char* name, const char* val);
+static void	do_set(const char* name, const char* val, bool fail_on_not_found);
 static void	do_compare(const char* name, const char* vals[], unsigned comparisons);
 static void	do_reset();
 
@@ -117,10 +117,17 @@ param_main(int argc, char *argv[])
 		}
 
 		if (!strcmp(argv[1], "set")) {
-			if (argc >= 4) {
-				do_set(argv[2], argv[3]);
+			if (argc >= 5) {
+
+				/* if the fail switch is provided, fails the command if not found */
+				bool fail = !strcmp(argv[4], "fail");
+
+				do_set(argv[2], argv[3], fail);
+
+			} else if (argc >= 4) {
+				do_set(argv[2], argv[3], false);
 			} else {
-				errx(1, "not enough arguments.\nTry 'param set PARAM_NAME 3'");
+				errx(1, "not enough arguments.\nTry 'param set PARAM_NAME 3 [fail]'");
 			}
 		}
 
@@ -282,7 +289,7 @@ do_show_print(void *arg, param_t param)
 }
 
 static void
-do_set(const char* name, const char* val)
+do_set(const char* name, const char* val, bool fail_on_not_found)
 {
 	int32_t i;
 	float f;
@@ -290,8 +297,8 @@ do_set(const char* name, const char* val)
 
 	/* set nothing if parameter cannot be found */
 	if (param == PARAM_INVALID) {
-		/* param not found */
-		errx(1, "Error: Parameter %s not found.", name);
+		/* param not found - fail silenty in scripts as it prevents booting */
+		errx(((fail_on_not_found) ? 1 : 0), "Error: Parameter %s not found.", name);
 	}
 
 	printf("%c %s: ",
diff --git a/src/systemcmds/perf/module.mk b/src/systemcmds/perf/module.mk
index 77952842b..ec39a7a85 100644
--- a/src/systemcmds/perf/module.mk
+++ b/src/systemcmds/perf/module.mk
@@ -39,3 +39,5 @@ MODULE_COMMAND	 = perf
 SRCS		 = perf.c
 
 MAXOPTIMIZATION	 = -Os
+
+MODULE_STACKSIZE = 1800
diff --git a/src/systemcmds/preflight_check/module.mk b/src/systemcmds/preflight_check/module.mk
index 7c3c88783..0cb2a4cd0 100644
--- a/src/systemcmds/preflight_check/module.mk
+++ b/src/systemcmds/preflight_check/module.mk
@@ -40,3 +40,5 @@ MODULE_COMMAND	 = preflight_check
 SRCS		 = preflight_check.c
 
 MAXOPTIMIZATION	 = -Os
+
+MODULE_STACKSIZE = 1800
diff --git a/src/systemcmds/pwm/module.mk b/src/systemcmds/pwm/module.mk
index 4a23bba90..13a24150f 100644
--- a/src/systemcmds/pwm/module.mk
+++ b/src/systemcmds/pwm/module.mk
@@ -38,4 +38,4 @@
 MODULE_COMMAND	 = pwm
 SRCS		 = pwm.c
 
-MODULE_STACKSIZE = 4096
+MODULE_STACKSIZE = 1800
diff --git a/src/systemcmds/reboot/module.mk b/src/systemcmds/reboot/module.mk
index 19f64af54..edf9d8b37 100644
--- a/src/systemcmds/reboot/module.mk
+++ b/src/systemcmds/reboot/module.mk
@@ -39,3 +39,5 @@ MODULE_COMMAND	 = reboot
 SRCS		 = reboot.c
 
 MAXOPTIMIZATION	 = -Os
+
+MODULE_STACKSIZE = 800
diff --git a/src/systemcmds/top/module.mk b/src/systemcmds/top/module.mk
index 9239aafc3..548a09f85 100644
--- a/src/systemcmds/top/module.mk
+++ b/src/systemcmds/top/module.mk
@@ -38,7 +38,7 @@
 MODULE_COMMAND	 = top
 SRCS		 = top.c
 
-MODULE_STACKSIZE = 3000
+MODULE_STACKSIZE = 1700
 
 MAXOPTIMIZATION	 = -Os