diff --git a/Makefile b/Makefile
index 438f697a4..200a436b8 100644
--- a/Makefile
+++ b/Makefile
@@ -432,7 +432,7 @@ all_$(1)_clean: $$(addsuffix _clean, $$(filter bl_$(1), $$(BL_TARGETS)))
 all_$(1)_clean: $$(addsuffix _clean, $$(filter bu_$(1), $$(BU_TARGETS)))
 endef
 
-ALL_BOARDS := openpilot ahrs coptercontrol pipxtreme ins
+ALL_BOARDS := openpilot ahrs coptercontrol pipxtreme ins revolution
 
 # Friendly names of each board (used to find source tree)
 openpilot_friendly     := OpenPilot
@@ -440,6 +440,7 @@ coptercontrol_friendly := CopterControl
 pipxtreme_friendly     := PipXtreme
 ins_friendly           := INS
 ahrs_friendly          := AHRS
+revolution_friendly    := REVOLUTION
 
 # Start out assuming that we'll build fw, bl and bu for all boards
 FW_BOARDS  := $(ALL_BOARDS)
diff --git a/flight/Revolution/Makefile b/flight/Revolution/Makefile
new file mode 100644
index 000000000..be4dc47a4
--- /dev/null
+++ b/flight/Revolution/Makefile
@@ -0,0 +1,440 @@
+#####
+# Project: OpenPilot INS
+#
+#
+# Makefile for OpenPilot INS project
+#
+# The OpenPilot Team, http://www.openpilot.org, Copyright (C) 2009.
+#
+#
+# This program is free software; you can redistribute it and/or modify
+# it under the terms of the GNU General Public License as published by
+# the Free Software Foundation; either version 3 of the License, or
+# (at your option) any later version.
+#
+# This program is distributed in the hope that it will be useful, but
+# WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY
+# or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License
+# for more details.
+#
+# You should have received a copy of the GNU General Public License along
+# with this program; if not, write to the Free Software Foundation, Inc.,
+# 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
+#####
+
+WHEREAMI := $(dir $(lastword $(MAKEFILE_LIST)))
+TOP      := $(realpath $(WHEREAMI)/../../)
+include $(TOP)/make/firmware-defs.mk
+include $(TOP)/make/boards/$(BOARD_NAME)/board-info.mk
+
+# Target file name (without extension).
+TARGET := fw_$(BOARD_NAME)
+
+# Directory for output files (lst, obj, dep, elf, sym, map, hex, bin etc.)
+OUTDIR := $(TOP)/build/$(TARGET)
+
+# Set developer code and compile options
+# Set to YES for debugging
+DEBUG ?= NO
+
+# Set to YES when using Code Sourcery toolchain
+CODE_SOURCERY ?= NO
+
+ifeq ($(CODE_SOURCERY), YES)
+REMOVE_CMD = cs-rm
+else
+REMOVE_CMD = rm
+endif
+
+FLASH_TOOL = OPENOCD
+
+# Paths
+INS = ./
+INSINC = $(INS)/inc
+PIOS = ../PiOS
+PIOSINC = $(PIOS)/inc
+FLIGHTLIB = ../Libraries
+FLIGHTLIBINC = ../Libraries/inc
+PIOSSTM32F2XX = $(PIOS)/STM32F2xx
+PIOSCOMMON = $(PIOS)/Common
+PIOSBOARDS = $(PIOS)/Boards
+APPLIBDIR = $(PIOSSTM32F2XX)/Libraries
+STMLIBDIR = $(APPLIBDIR)
+STMSPDDIR = $(STMLIBDIR)/STM32F2xx_StdPeriph_Driver
+STMSPDSRCDIR = $(STMSPDDIR)/src
+STMSPDINCDIR = $(STMSPDDIR)/inc
+CMSISDIR  = $(STMLIBDIR)/CMSIS/Core/CM3
+OPDIR = ../OpenPilot
+OPUAVOBJ = ../UAVObjects
+OPUAVOBJINC = $(OPUAVOBJ)/inc
+OPSYSINC = $(OPDIR)/System/inc
+BOOT = ../Bootloaders/INS
+BOOTINC = $(BOOT)/inc
+
+OPUAVSYNTHDIR = $(OUTDIR)/../uavobject-synthetics/flight
+
+# List C source files here. (C dependencies are automatically generated.)
+# use file-extension c for "c-only"-files
+
+## INS:
+SRC = ins.c
+SRC += pios_board.c
+SRC += insgps13state.c
+SRC += insgps_helper.c
+SRC += $(FLIGHTLIB)/CoordinateConversions.c
+SRC += $(FLIGHTLIB)/fifo_buffer.c
+SRC += $(FLIGHTLIB)/ahrs_spi_comm.c
+SRC += $(FLIGHTLIB)/ahrs_comm_objects.c
+SRC += $(FLIGHTLIB)/NMEA.c
+SRC += $(BOOT)/ahrs_spi_program_slave.c
+SRC += $(BOOT)/ahrs_slave_test.c
+SRC += $(BOOT)/ahrs_spi_program.c
+
+## PIOS Hardware (STM32F2xx)
+SRC += $(PIOSSTM32F2XX)/pios_sys.c
+SRC += $(PIOSSTM32F2XX)/pios_led.c
+SRC += $(PIOSSTM32F2XX)/pios_delay.c
+SRC += $(PIOSSTM32F2XX)/pios_usart.c
+SRC += $(PIOSSTM32F2XX)/pios_irq.c
+SRC += $(PIOSSTM32F2XX)/pios_i2c.c
+#SRC += $(PIOSSTM32F2XX)/pios_debug.c
+#SRC += $(PIOSSTM32F2XX)/pios_gpio.c
+SRC += $(PIOSSTM32F2XX)/pios_spi.c
+#SRC += $(PIOSSTM32F2XX)/pios_exti.c
+SRC += $(PIOSSTM32F2XX)/pios_iap.c
+SRC += $(PIOSSTM32F2XX)/pios_bma180.c
+SRC += $(PIOSSTM32F2XX)/pios_hmc5883.c
+SRC += $(PIOSSTM32F2XX)/pios_bmp085.c
+SRC += $(PIOSSTM32F2XX)/pios_imu3000.c
+
+## PIOS Hardware (Common)
+SRC += $(PIOSCOMMON)/pios_com.c
+SRC += $(PIOSCOMMON)/printf-stdarg.c
+
+SRC += $(PIOSCOMMON)/pios_bl_helper.c
+
+## CMSIS for STM32
+SRC += $(CMSISDIR)/core_cm3.c
+SRC += $(CMSISDIR)/system_stm32f4xx.c
+
+## Used parts of the STM-Library
+SRC += $(STMSPDSRCDIR)/stm32f2xx_adc.c
+SRC += $(STMSPDSRCDIR)/stm32f2xx_crc.c
+SRC += $(STMSPDSRCDIR)/stm32f2xx_dac.c
+SRC += $(STMSPDSRCDIR)/stm32f2xx_dma.c
+SRC += $(STMSPDSRCDIR)/stm32f2xx_exti.c
+SRC += $(STMSPDSRCDIR)/stm32f2xx_flash.c
+SRC += $(STMSPDSRCDIR)/stm32f2xx_gpio.c
+SRC += $(STMSPDSRCDIR)/stm32f2xx_i2c.c
+SRC += $(STMSPDSRCDIR)/stm32f2xx_pwr.c
+SRC += $(STMSPDSRCDIR)/stm32f2xx_rcc.c
+SRC += $(STMSPDSRCDIR)/stm32f2xx_rtc.c
+SRC += $(STMSPDSRCDIR)/stm32f2xx_spi.c
+SRC += $(STMSPDSRCDIR)/stm32f2xx_syscfg.c
+SRC += $(STMSPDSRCDIR)/stm32f2xx_tim.c
+SRC += $(STMSPDSRCDIR)/stm32f2xx_usart.c
+SRC += $(STMSPDSRCDIR)/misc.c
+# List C source files here which must be compiled in ARM-Mode (no -mthumb).
+# use file-extension c for "c-only"-files
+## just for testing, timer.c could be compiled in thumb-mode too
+SRCARM =
+
+# List C++ source files here.
+# use file-extension .cpp for C++-files (not .C)
+CPPSRC =
+
+# List C++ source files here which must be compiled in ARM-Mode.
+# use file-extension .cpp for C++-files (not .C)
+#CPPSRCARM = $(TARGET).cpp
+CPPSRCARM =
+
+# List Assembler source files here.
+# Make them always end in a capital .S. Files ending in a lowercase .s
+# will not be considered source files but generated files (assembler
+# output from the compiler), and will be deleted upon "make clean"!
+# Even though the DOS/Win* filesystem matches both .s and .S the same,
+# it will preserve the spelling of the filenames, and gcc itself does
+# care about how the name is spelled on its command-line.
+ASRC = $(PIOSSTM32F2XX)/startup_stm32f2xx.S
+
+# List Assembler source files here which must be assembled in ARM-Mode..
+ASRCARM =
+
+# List any extra directories to look for include files here.
+#    Each directory must be seperated by a space.
+EXTRAINCDIRS  += $(PIOS)
+EXTRAINCDIRS  += $(PIOSINC)
+EXTRAINCDIRS  += $(FLIGHTLIBINC)
+EXTRAINCDIRS  += $(PIOSSTM32F2XX)
+EXTRAINCDIRS  += $(PIOSCOMMON)
+EXTRAINCDIRS  += $(PIOSBOARDS)
+EXTRAINCDIRS  += $(STMSPDINCDIR)
+EXTRAINCDIRS  += $(CMSISDIR)
+EXTRAINCDIRS  += $(INSINC)
+EXTRAINCDIRS  += $(OPUAVSYNTHDIR)
+EXTRAINCDIRS  += $(BOOTINC)
+
+# List any extra directories to look for library files here.
+# Also add directories where the linker should search for
+# includes from linker-script to the list
+#     Each directory must be seperated by a space.
+EXTRA_LIBDIRS =
+
+# Extra Libraries
+#    Each library-name must be seperated by a space.
+#    i.e. to link with libxyz.a, libabc.a and libefsl.a:
+#    EXTRA_LIBS = xyz abc efsl
+# for newlib-lpc (file: libnewlibc-lpc.a):
+#    EXTRA_LIBS = newlib-lpc
+EXTRA_LIBS =
+
+# Path to Linker-Scripts
+LINKERSCRIPTPATH = $(PIOSSTM32F2XX)
+
+# Optimization level, can be [0, 1, 2, 3, s].
+# 0 = turn off optimization. s = optimize for size.
+# (Note: 3 is not always the best optimization level. See avr-libc FAQ.)
+
+ifeq ($(DEBUG),YES)
+CFLAGS += -O0
+CFLAGS += -DGENERAL_COV
+CFLAGS += -finstrument-functions -ffixed-r10
+else
+CFLAGS += -Os
+endif
+
+
+   
+# common architecture-specific flags from the device-specific library makefile
+CFLAGS += $(ARCHFLAGS)
+
+
+CFLAGS += -DDIAGNOSTICS
+
+# This is not the best place for these.  Really should abstract out
+# to the board file or something
+CFLAGS += -DHSE_VALUE=$(OSCILLATOR_FREQ)
+CFLAGS += -DSTM32F2XX
+CFLAGS += -DMEM_SIZE=1024000000
+
+# Output format. (can be ihex or binary or both)
+#  binary to create a load-image in raw-binary format i.e. for SAM-BA,
+#  ihex to create a load-image in Intel hex format
+#LOADFORMAT = ihex
+#LOADFORMAT = binary
+LOADFORMAT = both
+
+# Debugging format.
+DEBUGF = dwarf-2
+
+# Place project-specific -D (define) and/or
+# -U options for C here.
+CDEFS = -DSTM32F10X_$(MODEL)
+CDEFS += -DUSE_STDPERIPH_DRIVER
+CDEFS += -DUSE_$(BOARD)
+CDEFS += -DIN_AHRS
+
+# Place project-specific -D and/or -U options for
+# Assembler with preprocessor here.
+#ADEFS = -DUSE_IRQ_ASM_WRAPPER
+ADEFS = -D__ASSEMBLY__
+
+# Compiler flag to set the C Standard level.
+# c89   - "ANSI" C
+# gnu89 - c89 plus GCC extensions
+# c99   - ISO C99 standard (not yet fully implemented)
+# gnu99 - c99 plus GCC extensions
+CSTANDARD = -std=gnu99
+
+#-----
+
+# Compiler flags.
+
+#  -g*:          generate debugging information
+#  -O*:          optimization level
+#  -f...:        tuning, see GCC manual and avr-libc documentation
+#  -Wall...:     warning level
+#  -Wa,...:      tell GCC to pass this to the assembler.
+#    -adhlns...: create assembler listing
+#
+# Flags for C and C++ (arm-elf-gcc/arm-elf-g++)
+
+CFLAGS += -g$(DEBUGF)
+
+CFLAGS += -ffast-math
+
+CFLAGS += -mcpu=$(MCU)
+CFLAGS += $(CDEFS)
+CFLAGS += $(patsubst %,-I%,$(EXTRAINCDIRS)) -I.
+
+CFLAGS += -mapcs-frame
+CFLAGS += -fomit-frame-pointer
+ifeq ($(CODE_SOURCERY), YES)
+CFLAGS += -fpromote-loop-indices
+endif
+
+#CFLAGS += -Wall
+#CFLAGS += -Werror
+CFLAGS += -Wa,-adhlns=$(addprefix $(OUTDIR)/, $(notdir $(addsuffix .lst, $(basename $<))))
+# Compiler flags to generate dependency files:
+CFLAGS += -MD -MP -MF $(OUTDIR)/dep/$(@F).d
+
+# flags only for C
+#CONLYFLAGS += -Wnested-externs
+CONLYFLAGS += $(CSTANDARD)
+
+# Assembler flags.
+#  -Wa,...:    tell GCC to pass this to the assembler.
+#  -ahlns:     create listing
+ASFLAGS  = $(ARCHFLAGS) -mthumb -I. -x assembler-with-cpp
+ASFLAGS += $(ADEFS)
+ASFLAGS += -Wa,-adhlns=$(addprefix $(OUTDIR)/, $(notdir $(addsuffix .lst, $(basename $<))))
+ASFLAGS += $(patsubst %,-I%,$(EXTRAINCDIRS))
+
+MATH_LIB = -lm
+
+# Linker flags.
+#  -Wl,...:     tell GCC to pass this to linker.
+#    -Map:      create map file
+#    --cref:    add cross reference to  map file
+LDFLAGS = -nostartfiles -Wl,-Map=$(OUTDIR)/$(TARGET).map,--cref,--gc-sections
+LDFLAGS += $(patsubst %,-L%,$(EXTRA_LIBDIRS))
+LDFLAGS += -lc
+LDFLAGS += $(patsubst %,-l%,$(EXTRA_LIBS))
+LDFLAGS += $(MATH_LIB)
+LDFLAGS += -lc -lgcc
+
+# Set linker-script name depending on selected submodel name
+LDFLAGS += -T$(LINKERSCRIPTPATH)/link_$(BOARD)_memory.ld
+LDFLAGS += -T$(LINKERSCRIPTPATH)/link_$(BOARD)_sections.ld
+
+# Define programs and commands.
+REMOVE  = $(REMOVE_CMD) -f
+
+# List of all source files.
+ALLSRC     = $(ASRCARM) $(ASRC) $(SRCARM) $(SRC) $(CPPSRCARM) $(CPPSRC)
+# List of all source files without directory and file-extension.
+ALLSRCBASE = $(notdir $(basename $(ALLSRC)))
+
+# Define all object files.
+ALLOBJ     = $(addprefix $(OUTDIR)/, $(addsuffix .o, $(ALLSRCBASE)))
+
+# Define all listing files (used for make clean).
+LSTFILES   = $(addprefix $(OUTDIR)/, $(addsuffix .lst, $(ALLSRCBASE)))
+# Define all depedency-files (used for make clean).
+DEPFILES   = $(addprefix $(OUTDIR)/dep/, $(addsuffix .o.d, $(ALLSRCBASE)))
+
+# Default target.
+all: gccversion build
+
+ifeq ($(LOADFORMAT),ihex)
+build: elf hex lss sym
+else
+ifeq ($(LOADFORMAT),binary)
+build: elf bin lss sym
+else
+ifeq ($(LOADFORMAT),both)
+build: elf hex bin lss sym
+else
+$(error "$(MSG_FORMATERROR) $(FORMAT)")
+endif
+endif
+endif
+
+# Link: create ELF output file from object files.
+$(eval $(call LINK_TEMPLATE, $(OUTDIR)/$(TARGET).elf, $(ALLOBJ)))
+
+# Assemble: create object files from assembler source files.
+$(foreach src, $(ASRC), $(eval $(call ASSEMBLE_TEMPLATE, $(src))))
+
+# Assemble: create object files from assembler source files. ARM-only
+$(foreach src, $(ASRCARM), $(eval $(call ASSEMBLE_ARM_TEMPLATE, $(src))))
+
+# Compile: create object files from C source files.
+$(foreach src, $(SRC), $(eval $(call COMPILE_C_TEMPLATE, $(src))))
+
+# Compile: create object files from C source files. ARM-only
+$(foreach src, $(SRCARM), $(eval $(call COMPILE_C_ARM_TEMPLATE, $(src))))
+
+# Compile: create object files from C++ source files.
+$(foreach src, $(CPPSRC), $(eval $(call COMPILE_CPP_TEMPLATE, $(src))))
+
+# Compile: create object files from C++ source files. ARM-only
+$(foreach src, $(CPPSRCARM), $(eval $(call COMPILE_CPP_ARM_TEMPLATE, $(src))))
+
+# Compile: create assembler files from C source files. ARM/Thumb
+$(eval $(call PARTIAL_COMPILE_TEMPLATE, SRC))
+
+# Compile: create assembler files from C source files. ARM only
+$(eval $(call PARTIAL_COMPILE_ARM_TEMPLATE, SRCARM))
+
+$(OUTDIR)/$(TARGET).bin.o: $(OUTDIR)/$(TARGET).bin
+
+$(eval $(call OPFW_TEMPLATE,$(OUTDIR)/$(TARGET).bin,$(BOARD_TYPE),$(BOARD_REVISION)))
+
+# Add jtag targets (program and wipe)
+$(eval $(call JTAG_TEMPLATE,$(OUTDIR)/$(TARGET).bin,$(FW_BANK_BASE),$(FW_BANK_SIZE),$(OPENOCD_CONFIG)))
+
+.PHONY: elf lss sym hex bin bino opfw
+elf: $(OUTDIR)/$(TARGET).elf
+lss: $(OUTDIR)/$(TARGET).lss
+sym: $(OUTDIR)/$(TARGET).sym
+hex: $(OUTDIR)/$(TARGET).hex
+bin: $(OUTDIR)/$(TARGET).bin
+bino: $(OUTDIR)/$(TARGET).bin.o
+opfw: $(OUTDIR)/$(TARGET).opfw
+
+# Display sizes of sections.
+$(eval $(call SIZE_TEMPLATE, $(OUTDIR)/$(TARGET).elf))
+
+# Generate Doxygen documents
+docs:
+	doxygen  $(DOXYGENDIR)/doxygen.cfg
+
+# Install: install binary file with prefix/suffix into install directory
+install: $(OUTDIR)/$(TARGET).opfw
+ifneq ($(INSTALL_DIR),)
+	@echo $(MSG_INSTALLING) $(call toprel, $<)
+	$(V1) mkdir -p $(INSTALL_DIR)
+	$(V1) $(INSTALL) $< $(INSTALL_DIR)/$(INSTALL_PFX)$(TARGET)$(INSTALL_SFX).opfw
+else
+	$(error INSTALL_DIR must be specified for $@)
+endif
+
+# Target: clean project.
+clean: clean_list
+
+clean_list :
+	@echo $(MSG_CLEANING)
+	$(V1) $(REMOVE) $(OUTDIR)/$(TARGET).map
+	$(V1) $(REMOVE) $(OUTDIR)/$(TARGET).elf
+	$(V1) $(REMOVE) $(OUTDIR)/$(TARGET).hex
+	$(V1) $(REMOVE) $(OUTDIR)/$(TARGET).bin
+	$(V1) $(REMOVE) $(OUTDIR)/$(TARGET).sym
+	$(V1) $(REMOVE) $(OUTDIR)/$(TARGET).lss
+	$(V1) $(REMOVE) $(OUTDIR)/$(TARGET).bin.o
+	$(V1) $(REMOVE) $(ALLOBJ)
+	$(V1) $(REMOVE) $(LSTFILES)
+	$(V1) $(REMOVE) $(DEPFILES)
+	$(V1) $(REMOVE) $(SRC:.c=.s)
+	$(V1) $(REMOVE) $(SRCARM:.c=.s)
+	$(V1) $(REMOVE) $(CPPSRC:.cpp=.s)
+	$(V1) $(REMOVE) $(CPPSRCARM:.cpp=.s)
+
+# Create output files directory
+# all known MS Windows OS define the ComSpec environment variable
+ifdef ComSpec
+$(shell md $(subst /,\\,$(OUTDIR)) 2>NUL)
+else
+$(shell mkdir -p $(OUTDIR) 2>/dev/null)
+endif
+
+# Include the dependency files.
+ifdef ComSpec
+-include $(shell md $(subst /,\\,$(OUTDIR))\dep 2>NUL) $(wildcard $(OUTDIR)/dep/*)
+else
+-include $(shell mkdir $(OUTDIR) 2>/dev/null) $(shell mkdir $(OUTDIR)/dep 2>/dev/null) $(wildcard $(OUTDIR)/dep/*)
+endif
+
+# Listing of phony targets.
+.PHONY : all build clean clean_list install
diff --git a/flight/Revolution/cm3_fault_handlers.c b/flight/Revolution/cm3_fault_handlers.c
new file mode 100644
index 000000000..af26e2ad9
--- /dev/null
+++ b/flight/Revolution/cm3_fault_handlers.c
@@ -0,0 +1,87 @@
+/*
+ * cm3_fault_handlers.c
+ *
+ *  Created on: Apr 24, 2011
+ *      Author: msmith
+ */
+
+#include <stdint.h>
+#include "dcc_stdio.h"
+#ifdef STM32F4XX
+# include "stm32f4xx.h"
+#endif
+#ifdef STM32F2XX
+# include "stm32f2xx.h"
+#endif
+
+#define FAULT_TRAMPOLINE(_vec)										\
+__attribute__((naked, no_instrument_function))						\
+void																\
+_vec##_Handler(void)												\
+{																	\
+     __asm(  ".syntax unified\n"									\
+                     "MOVS   R0, #4  \n"							\
+                     "MOV    R1, LR  \n"							\
+                     "TST    R0, R1  \n"							\
+                     "BEQ    1f    \n"								\
+                     "MRS    R0, PSP \n"							\
+                     "B      " #_vec "_Handler2      \n"			\
+             "1:  \n"												\
+                     "MRS    R0, MSP \n"							\
+                     "B      " #_vec "_Handler2      \n"			\
+             ".syntax divided\n");									\
+}																	\
+struct hack
+
+struct cm3_frame {
+	uint32_t	r0;
+	uint32_t	r1;
+	uint32_t	r2;
+	uint32_t	r3;
+	uint32_t	r12;
+	uint32_t	lr;
+	uint32_t	pc;
+	uint32_t	psr;
+};
+
+FAULT_TRAMPOLINE(HardFault);
+FAULT_TRAMPOLINE(BusFault);
+FAULT_TRAMPOLINE(UsageFault);
+
+/* this is a hackaround to avoid an issue where dereferencing SCB seems to result in bad codegen and a link error */
+#define SCB_REG(_reg)	(*(uint32_t *)&(SCB->_reg))
+
+void
+HardFault_Handler2(struct cm3_frame *frame)
+{
+	dbg_write_str("\nHARD FAULT");
+	dbg_write_hex32(frame->pc);
+	dbg_write_char('\n');
+	dbg_write_hex32(SCB_REG(HFSR));
+	dbg_write_char('\n');
+	for (;;);
+}
+
+void
+BusFault_Handler2(struct cm3_frame *frame)
+{
+	dbg_write_str("\nBUS FAULT");
+	dbg_write_hex32(frame->pc);
+	dbg_write_char('\n');
+	dbg_write_hex32(SCB_REG(CFSR));
+	dbg_write_char('\n');
+	dbg_write_hex32(SCB_REG(BFAR));
+	dbg_write_char('\n');
+	for (;;);
+}
+
+void
+UsageFault_Handler2(struct cm3_frame *frame)
+{
+	dbg_write_str("\nUSAGE FAULT");
+	dbg_write_hex32(frame->pc);
+	dbg_write_char('\n');
+	dbg_write_hex32(SCB_REG(CFSR));
+	dbg_write_char('\n');
+	for (;;);
+}
diff --git a/flight/Revolution/inc/ahrs_timer.h b/flight/Revolution/inc/ahrs_timer.h
new file mode 100644
index 000000000..aaddf5769
--- /dev/null
+++ b/flight/Revolution/inc/ahrs_timer.h
@@ -0,0 +1,45 @@
+/**
+ ******************************************************************************
+ * @addtogroup AHRS AHRS Control
+ * @brief The AHRS Modules perform
+ *
+ * @{ 
+ * @addtogroup AHRS_TIMER
+ * @brief Sets up a simple timer that can be polled to estimate idle time
+ * @{ 
+ *
+ *
+ * @file       ahrs.c
+ * @author     The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010.
+ * @brief      INSGPS Test Program
+ * @see        The GNU Public License (GPL) Version 3
+ * 
+ *****************************************************************************/
+/* 
+ * This program is free software; you can redistribute it and/or modify 
+ * it under the terms of the GNU General Public License as published by 
+ * the Free Software Foundation; either version 3 of the License, or 
+ * (at your option) any later version.
+ * 
+ * This program is distributed in the hope that it will be useful, but 
+ * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY 
+ * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License 
+ * for more details.
+ * 
+ * You should have received a copy of the GNU General Public License along 
+ * with this program; if not, write to the Free Software Foundation, Inc., 
+ * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
+ */
+
+#ifndef AHRS_TIMER
+#define AHRS_TIMER
+
+#include <pios.h>
+
+#define TIMER_RATE (8e6 / 128)
+
+void timer_start();
+uint32_t timer_count();
+uint32_t timer_rate();
+
+#endif
diff --git a/flight/Revolution/inc/ins.h b/flight/Revolution/inc/ins.h
new file mode 100644
index 000000000..4dcf03601
--- /dev/null
+++ b/flight/Revolution/inc/ins.h
@@ -0,0 +1,117 @@
+/**
+ ******************************************************************************
+ * @addtogroup INS INS
+
+ * @brief The main INS headers
+ *
+ * @{
+ * @addtogroup INS_Main
+ * @brief INS headers
+ * @{
+ *
+ * @file       ins.h
+ * @author     David "Buzz" Carlson (buzz@chebuzz.com)
+ * 				The OpenPilot Team, http://www.openpilot.org Copyright (C) 2011.
+ * @brief      INS Headers
+ * @see        The GNU Public License (GPL) Version 3
+ *
+ *****************************************************************************/
+/* 
+ * This program is free software; you can redistribute it and/or modify 
+ * it under the terms of the GNU General Public License as published by 
+ * the Free Software Foundation; either version 3 of the License, or 
+ * (at your option) any later version.
+ * 
+ * This program is distributed in the hope that it will be useful, but 
+ * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY 
+ * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License 
+ * for more details.
+ * 
+ * You should have received a copy of the GNU General Public License along 
+ * with this program; if not, write to the Free Software Foundation, Inc., 
+ * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
+ */
+
+#ifndef INS_H
+#define INS_H
+
+/* PIOS Includes */
+#include <pios.h>
+
+struct mag_sensor {
+	uint8_t id[4];
+	uint8_t updated;
+	struct {
+		int16_t axis[3];
+	} raw;
+	struct {
+		float axis[3];
+	} scaled;
+	struct {
+		float bias[3];
+		float scale[3];
+		float variance[3];
+	} calibration;
+};
+
+//! Contains the data from the accelerometer
+struct accel_sensor {
+	struct {
+		float x;
+		float y;
+		float z;
+	} filtered;
+	struct {
+		float bias[3];
+		float scale[3];
+		float variance[3];
+	} calibration;
+	float temperature;
+};
+
+//! Contains the data from the gyro
+struct gyro_sensor {
+	struct {
+		float x;
+		float y;
+		float z;
+	} filtered;
+	struct {
+		float bias[3];
+		float scale[3];
+		float variance[3];
+	} calibration;
+	float temperature;
+};
+
+//! Conains the current estimate of the attitude
+struct attitude_solution {
+	struct {
+		float q1;
+		float q2;
+		float q3;
+		float q4;
+	} quaternion;
+};
+
+//! Contains data from the altitude sensor
+struct altitude_sensor {
+	float altitude;
+	bool updated;
+};
+
+//! Contains data from the GPS (via the SPI link)
+struct gps_sensor {
+	float NED[3];
+	float heading;
+	float groundspeed;
+	float quality;
+	bool updated;
+};
+
+#endif /* INS_H */
+
+/**
+ * @}
+ * @}
+ */
diff --git a/flight/Revolution/inc/ins_fsm.h b/flight/Revolution/inc/ins_fsm.h
new file mode 100644
index 000000000..dd9d24b21
--- /dev/null
+++ b/flight/Revolution/inc/ins_fsm.h
@@ -0,0 +1,94 @@
+ /**
+ ******************************************************************************
+ *
+ * @file       ahrs_fsm.h
+ * @author     The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010.
+ * @brief     
+ * @see        The GNU Public License (GPL) Version 3
+ *
+ *****************************************************************************/
+/*
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 3 of the License, or
+ * (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful, but
+ * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY
+ * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License
+ * for more details.
+ *
+ * You should have received a copy of the GNU General Public License along
+ * with this program; if not, write to the Free Software Foundation, Inc.,
+ * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
+ */
+
+#ifndef AHRS_FSM_H
+#define AHRS_FSM_H
+
+#include "pios_opahrs_proto.h"
+
+enum lfsm_state {
+	LFSM_STATE_FAULTED = 0,	/* Must be zero so undefined transitions land here */
+	LFSM_STATE_STOPPED,
+	LFSM_STATE_STOPPING,
+	LFSM_STATE_INACTIVE,
+	LFSM_STATE_USER_BUSY,
+	LFSM_STATE_USER_BUSY_RX_PENDING,
+	LFSM_STATE_USER_BUSY_TX_PENDING,
+	LFSM_STATE_USER_BUSY_RXTX_PENDING,
+	LFSM_STATE_USER_RX_PENDING,
+	LFSM_STATE_USER_TX_PENDING,
+	LFSM_STATE_USER_RXTX_PENDING,
+	LFSM_STATE_USER_RX_ACTIVE,
+	LFSM_STATE_USER_TX_ACTIVE,
+	LFSM_STATE_USER_RXTX_ACTIVE,
+
+	LFSM_STATE_NUM_STATES	/* Must be last */
+};
+
+enum lfsm_event {
+	LFSM_EVENT_INIT_LINK,
+	LFSM_EVENT_STOP,
+	LFSM_EVENT_USER_SET_RX,
+	LFSM_EVENT_USER_SET_TX,
+	LFSM_EVENT_USER_DONE,
+	LFSM_EVENT_RX_LINK,
+	LFSM_EVENT_RX_USER,
+	LFSM_EVENT_RX_UNKNOWN,
+
+	LFSM_EVENT_NUM_EVENTS	/* Must be last */
+};
+
+struct lfsm_link_stats {
+	uint32_t rx_badcrc;
+	uint32_t rx_badmagic_head;
+	uint32_t rx_badmagic_tail;
+	uint32_t rx_link;
+	uint32_t rx_user;
+	uint32_t tx_user;
+	uint32_t rx_badtype;
+	uint32_t rx_badver;
+};
+
+extern void lfsm_init(void);
+extern void lfsm_inject_event(enum lfsm_event event);
+
+extern void lfsm_irq_callback(uint8_t crc_ok, uint8_t crc_val);
+
+extern void lfsm_get_link_stats(struct lfsm_link_stats *stats);
+extern enum lfsm_state lfsm_get_state(void);
+
+extern void lfsm_set_link_proto_v0(struct opahrs_msg_v0 *link_tx,
+				   struct opahrs_msg_v0 *link_rx);
+extern void lfsm_user_set_rx_v0(struct opahrs_msg_v0 *user_rx);
+extern void lfsm_user_set_tx_v0(struct opahrs_msg_v0 *user_tx);
+
+extern void lfsm_set_link_proto_v1(struct opahrs_msg_v1 *link_tx,
+				   struct opahrs_msg_v1 *link_rx);
+extern void lfsm_user_set_rx_v1(struct opahrs_msg_v1 *user_rx);
+extern void lfsm_user_set_tx_v1(struct opahrs_msg_v1 *user_tx);
+
+extern void lfsm_user_done(void);
+
+#endif /* AHRS_FSM_H */
diff --git a/flight/Revolution/inc/insgps.h b/flight/Revolution/inc/insgps.h
new file mode 100644
index 000000000..bf4faae4b
--- /dev/null
+++ b/flight/Revolution/inc/insgps.h
@@ -0,0 +1,93 @@
+/**
+ ******************************************************************************
+ * @addtogroup AHRS 
+ * @{
+ * @addtogroup INSGPS
+ * @{
+ * @brief INSGPS is a joint attitude and position estimation EKF
+ *
+ * @file       insgps.h
+ * @author     The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010.
+ * @brief      Include file of the INSGPS exposed functionality.
+ *
+ * @see        The GNU Public License (GPL) Version 3
+ *
+ *****************************************************************************/
+/*
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 3 of the License, or
+ * (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful, but
+ * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY
+ * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License
+ * for more details.
+ *
+ * You should have received a copy of the GNU General Public License along
+ * with this program; if not, write to the Free Software Foundation, Inc.,
+ * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
+ */
+
+#ifndef INSGPS_H_
+#define INSGPS_H_
+
+#include "stdint.h"
+
+/**
+  * @addtogroup Constants
+  * @{
+  */
+#define POS_SENSORS 0x007
+#define HORIZ_SENSORS 0x018
+#define VERT_SENSORS  0x020
+#define MAG_SENSORS 0x1C0
+#define BARO_SENSOR 0x200
+
+#define FULL_SENSORS 0x3FF
+
+/**
+  * @}
+  */
+
+//  Exposed Function Prototypes
+void INSGPSInit();
+void INSStatePrediction(float gyro_data[3], float accel_data[3], float dT);
+void INSCovariancePrediction(float dT);
+void INSCorrection(float mag_data[3], float Pos[3], float Vel[3], float BaroAlt, uint16_t SensorsUsed);
+
+void INSResetP(float PDiag[13]);
+void INSSetState(float pos[3], float vel[3], float q[4], float gyro_bias[3], float accel_bias[3]);
+void INSSetPosVelVar(float PosVar, float VelVar);
+void INSSetGyroBias(float gyro_bias[3]);
+void INSSetAccelVar(float accel_var[3]);
+void INSSetGyroVar(float gyro_var[3]);
+void INSSetMagNorth(float B[3]);
+void INSSetMagVar(float scaled_mag_var[3]);
+void INSPosVelReset(float pos[3], float vel[3]);
+
+void MagCorrection(float mag_data[3]);
+void MagVelBaroCorrection(float mag_data[3], float Vel[3], float BaroAlt);
+void FullCorrection(float mag_data[3], float Pos[3], float Vel[3],
+		    float BaroAlt);
+void GpsBaroCorrection(float Pos[3], float Vel[3], float BaroAlt);
+void GpsMagCorrection(float mag_data[3], float Pos[3], float Vel[2]);
+void VelBaroCorrection(float Vel[3], float BaroAlt);
+
+uint16_t ins_get_num_states();
+
+//  Nav structure containing current solution
+struct NavStruct {
+	float Pos[3];		// Position in meters and relative to a local NED frame
+	float Vel[3];		// Velocity in meters and in NED
+	float q[4];		// unit quaternion rotation relative to NED
+	float gyro_bias[3];
+	float accel_bias[3];
+} Nav;
+
+/**
+ * @}
+ * @}
+ */
+
+#endif /* EKF_H_ */
diff --git a/flight/Revolution/inc/insgps_helper.h b/flight/Revolution/inc/insgps_helper.h
new file mode 100644
index 000000000..cf414b435
--- /dev/null
+++ b/flight/Revolution/inc/insgps_helper.h
@@ -0,0 +1,3 @@
+void ins_init_algorithm();
+void ins_outdoor_update();
+void ins_indoor_update();
diff --git a/flight/Revolution/inc/pios_config.h b/flight/Revolution/inc/pios_config.h
new file mode 100644
index 000000000..2ce0d71db
--- /dev/null
+++ b/flight/Revolution/inc/pios_config.h
@@ -0,0 +1,69 @@
+/**
+ ******************************************************************************
+ * @addtogroup INS INS
+
+ * @brief The INS configuration
+ *
+ * @{
+ * @addtogroup INS_Main
+ * @brief INS configuration
+ * @{
+ *
+ * @file       pios_config.h  
+ * @author     David "Buzz" Carlson (buzz@chebuzz.com)
+ * 				The OpenPilot Team, http://www.openpilot.org Copyright (C) 2011.
+ * @brief      PiOS configuration header. 
+ *                 - Central compile time config for the project.
+ * @see        The GNU Public License (GPL) Version 3
+ *
+ *****************************************************************************/
+/* 
+ * This program is free software; you can redistribute it and/or modify 
+ * it under the terms of the GNU General Public License as published by 
+ * the Free Software Foundation; either version 3 of the License, or 
+ * (at your option) any later version.
+ * 
+ * This program is distributed in the hope that it will be useful, but 
+ * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY 
+ * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License 
+ * for more details.
+ * 
+ * You should have received a copy of the GNU General Public License along 
+ * with this program; if not, write to the Free Software Foundation, Inc., 
+ * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
+ */
+
+#ifndef PIOS_CONFIG_H
+#define PIOS_CONFIG_H
+
+/* Enable/Disable PiOS Modules */
+#define PIOS_INCLUDE_DELAY
+#define PIOS_INCLUDE_I2C
+#define PIOS_INCLUDE_IRQ
+#define PIOS_INCLUDE_LED
+#define PIOS_INCLUDE_SPI
+#define PIOS_INCLUDE_SYS
+#define PIOS_INCLUDE_USART
+#define PIOS_INCLUDE_COM
+#define PIOS_INCLUDE_COM_AUX
+#define PIOS_INCLUDE_GPS
+#define PIOS_INCLUDE_BMA180
+#define PIOS_INCLUDE_HMC5883
+#define PIOS_INCLUDE_BMP085
+#define PIOS_INCLUDE_IMU3000
+#define PIOS_INCLUDE_GPIO
+#define PIOS_INCLUDE_EXTI
+
+#define PIOS_INCLUDE_BMA180
+
+/* COM Module */
+#define GPS_BAUDRATE			19200
+#define AUXUART_ENABLED			0
+#define AUXUART_BAUDRATE		19200
+
+#endif /* PIOS_CONFIG_H */
+
+/**
+ * @}
+ * @}
+ */
diff --git a/flight/Revolution/ins.c b/flight/Revolution/ins.c
new file mode 100644
index 000000000..2f47d8519
--- /dev/null
+++ b/flight/Revolution/ins.c
@@ -0,0 +1,903 @@
+/**
+ ******************************************************************************
+ * @addtogroup INS INS
+
+ * @brief The INS Modules perform
+ *
+ * @{
+ * @addtogroup INS_Main
+ * @brief Main function which does the hardware dependent stuff
+ * @{
+ *
+ *
+ * @file       ins.c
+ * @author     The OpenPilot Team, http://www.openpilot.org Copyright (C) 2011.
+ * @brief      INSGPS Test Program
+ * @see        The GNU Public License (GPL) Version 3
+ *
+ *****************************************************************************/
+/*
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 3 of the License, or
+ * (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful, but
+ * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY
+ * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License
+ * for more details.
+ *
+ * You should have received a copy of the GNU General Public License along
+ * with this program; if not, write to the Free Software Foundation, Inc.,
+ * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
+ */
+
+/* 
+ TODO:
+  BMP085 - Pressure
+  IMU3000 interrupt
+  BMA180 interrupt
+ */
+
+#define TYPICAL_PERIOD 3300
+#define timer_rate() 100000
+#define timer_count() 1
+/* OpenPilot Includes */
+#include "ins.h"
+#include "pios.h"
+#include "ahrs_spi_comm.h"
+#include "insgps.h"
+#include "CoordinateConversions.h"
+#include "NMEA.h"
+#include <stdbool.h>
+#include "fifo_buffer.h"
+#include "insgps_helper.h"
+
+#define DEG_TO_RAD         (M_PI / 180.0)
+#define RAD_TO_DEG         (180.0 / M_PI)
+
+#define INSGPS_MAGLEN      1000
+#define INSGPS_MAGTOL      0.5 /* error in magnetic vector length to use  */
+
+#define GYRO_OOB(x) ((x > (1000 * DEG_TO_RAD)) || (x < (-1000 * DEG_TO_RAD)))
+#define ACCEL_OOB(x) (((x > 12*9.81) || (x < -12*9.81)))
+#define ISNAN(x) (x != x)
+// down-sampled data index
+
+volatile int8_t ahrs_algorithm;
+
+/* Data accessors */
+void get_gps_data();
+void get_mag_data();
+void get_baro_data();
+void get_accel_gyro_data();
+
+void reset_values();
+void measure_noise(void);
+void zero_gyros(bool update_settings);
+
+/* Communication functions */
+//void send_calibration(void);
+void send_attitude(void);
+void send_velocity(void);
+void homelocation_callback(AhrsObjHandle obj);
+//void calibration_callback(AhrsObjHandle obj);
+void settings_callback(AhrsObjHandle obj);
+void affine_rotate(float scale[3][4], float rotation[3]);
+void calibration(float result[3], float scale[3][4], float arg[3]);
+
+extern void PIOS_Board_Init(void);
+void panic(uint32_t blinks);
+static void print_ekf_binary(bool ekf);
+void simple_update();
+
+/* Bootloader related functions and var*/
+void firmwareiapobj_callback(AhrsObjHandle obj);
+volatile uint8_t reset_count=0;
+
+/**
+ * @addtogroup INS_Global_Data INS Global Data
+ * @{
+ * Public data.  Used by both EKF and the sender
+ */
+
+//! Contains the data from the mag sensor chip
+struct mag_sensor mag_data;
+
+//! Contains the data from the accelerometer
+struct accel_sensor  accel_data;
+
+//! Contains the data from the gyro
+struct gyro_sensor gyro_data;
+
+//! Conains the current estimate of the attitude
+struct attitude_solution attitude_data;
+
+//! Contains data from the altitude sensor
+struct altitude_sensor altitude_data;
+
+//! Contains data from the GPS (via the SPI link)
+struct gps_sensor gps_data;
+
+static float mag_len = 0;
+
+typedef enum { INS_IDLE, INS_DATA_READY, INS_PROCESSING } states;
+
+/**
+ * @}
+ */
+
+/**
+ * @brief INS Main function
+ */
+
+uint32_t total_conversion_blocks;
+static bool bias_corrected_raw;
+
+float pressure, altitude;
+
+int32_t dr;
+
+static volatile bool init_algorithm = false;
+static bool zeroed_gyros = false;
+int32_t sclk, sclk_prev;
+int32_t sclk_count;
+uint32_t loop_time;
+int main()
+{	
+//	*(volatile unsigned long *)0xE000ED88 |= (0xf << 20);
+
+	PIOS_Board_Init();
+	PIOS_LED_Off(LED1);
+	PIOS_LED_On(LED2);
+		
+	// Sensors need a second to start
+	PIOS_DELAY_WaitmS(100);
+
+	ahrs_algorithm = INSSETTINGS_ALGORITHM_SIMPLE;
+	reset_values();	
+	gps_data.quality = -1;
+#if 0
+	// Sensor test
+	if(PIOS_IMU3000_Test() != 0)
+		panic(1);
+	
+	if(PIOS_BMA180_Test() != 0)
+		panic(2);
+	
+	if(PIOS_HMC5883_Test() != 0)
+		panic(3);	
+	
+	if(PIOS_BMP085_Test() != 0)
+		panic(4); 	
+#endif
+
+	PIOS_LED_On(LED1);
+	PIOS_LED_Off(LED2);
+	
+	// Kickstart BMP085 measurements until driver improved
+	PIOS_BMP085_StartADC(TemperatureConv);
+
+	// Flash warning light while trying to connect
+	uint32_t time_val1 = PIOS_DELAY_GetRaw();
+	uint32_t time_val2;
+	uint32_t ms_count = 0;
+	while(!AhrsLinkReady()) {
+		AhrsPoll();
+		if(PIOS_DELAY_DiffuS(time_val1) > 1000) {
+			ms_count += 1;
+			time_val1 = PIOS_DELAY_GetRaw();
+		}
+		if(ms_count > 100) {
+			PIOS_LED_Toggle(LED2);
+			ms_count = 0;
+		}
+	}
+	PIOS_LED_Off(LED2);
+	
+	/* we didn't connect the callbacks before because we have to wait
+	 for all data to be up to date before doing anything*/
+	InsSettingsConnectCallback(settings_callback);
+	HomeLocationConnectCallback(homelocation_callback);
+	//FirmwareIAPObjConnectCallback(firmwareiapobj_callback);
+	
+	for(uint32_t i = 0; i < 200; i++) {
+		get_accel_gyro_data();   // This function blocks till data avilable
+		get_mag_data();
+		get_baro_data();
+		PIOS_DELAY_WaituS(TYPICAL_PERIOD);
+	}
+
+	settings_callback(InsSettingsHandle());
+	ins_init_algorithm(); 
+	
+	/******************* Main EKF loop ****************************/
+	while(1) {
+		AhrsPoll();
+		InsStatusData status;
+		InsStatusGet(&status);
+		
+		// Alive signal
+		if ((total_conversion_blocks++ % 100) == 0)
+			PIOS_LED_Toggle(LED1);
+		
+		loop_time = PIOS_DELAY_DiffuS(time_val1);
+		time_val1 = PIOS_DELAY_GetRaw();
+		
+		get_accel_gyro_data();   // This function blocks till data avilable
+		get_mag_data();
+		get_baro_data();
+		get_gps_data();
+		
+		status.IdleTimePerCycle = PIOS_DELAY_DiffuS(time_val1);
+		
+		if(ISNAN(accel_data.filtered.x + accel_data.filtered.y + accel_data.filtered.z) ||
+		   ISNAN(gyro_data.filtered.x + gyro_data.filtered.y + gyro_data.filtered.z) ||
+		   ACCEL_OOB(accel_data.filtered.x) || ACCEL_OOB(accel_data.filtered.y) || ACCEL_OOB(accel_data.filtered.z) ||
+		   GYRO_OOB(gyro_data.filtered.x) || GYRO_OOB(gyro_data.filtered.y) || GYRO_OOB(gyro_data.filtered.z)) {
+			// If any values are NaN or huge don't update
+			//TODO: add field to ahrs status to track number of these events
+			continue;
+		}
+		
+		if(total_conversion_blocks <= 100 && !zeroed_gyros) {
+			// TODO: Replace this with real init
+			zero_gyros(total_conversion_blocks == 100);
+			if(total_conversion_blocks == 100)
+				zeroed_gyros = true;
+			PIOS_DELAY_WaituS(TYPICAL_PERIOD);
+
+			float zeros[3] = {0,0,0};
+			INSSetGyroBias(zeros);
+
+			continue;
+		}
+		/* If algorithm changed reinit.  This could go in callback but wouldn't be synchronous */
+		if (init_algorithm) {
+			ins_init_algorithm(); 
+			init_algorithm = false;
+		}
+		
+		time_val2 = PIOS_DELAY_GetRaw();
+		
+		print_ekf_binary(true);
+		
+		switch(ahrs_algorithm) {
+			case INSSETTINGS_ALGORITHM_SIMPLE:
+				simple_update();
+				break;
+			case INSSETTINGS_ALGORITHM_INSGPS_OUTDOOR:
+				ins_outdoor_update();
+				break;
+			case INSSETTINGS_ALGORITHM_INSGPS_INDOOR:
+			case INSSETTINGS_ALGORITHM_INSGPS_INDOOR_NOMAG:
+				ins_indoor_update();
+				break;
+			case INSSETTINGS_ALGORITHM_CALIBRATION:
+				measure_noise();
+				break;
+			case INSSETTINGS_ALGORITHM_SENSORRAW:
+				print_ekf_binary(false);
+				// Run at standard rate
+				while(PIOS_DELAY_DiffuS(time_val1) < TYPICAL_PERIOD);
+				break;
+			case INSSETTINGS_ALGORITHM_ZEROGYROS:
+				zero_gyros(false);
+				// Run at standard rate
+				while(PIOS_DELAY_DiffuS(time_val1) < TYPICAL_PERIOD);
+				break;
+		}
+		
+		status.RunningTimePerCycle = PIOS_DELAY_DiffuS(time_val2);
+		InsStatusSet(&status);
+	}
+	
+	return 0;
+}
+
+
+
+/**
+ * @brief Simple update using just mag and accel.  Yaw biased and big attitude changes.
+ */
+void simple_update() {
+	float q[4];
+	float rpy[3];
+	/***************** SIMPLE ATTITUDE FROM NORTH AND ACCEL ************/
+	/* Very simple computation of the heading and attitude from accel. */
+	rpy[2] = atan2((mag_data.raw.axis[1]), (-1 * mag_data.raw.axis[0])) * RAD_TO_DEG;
+	rpy[1] = atan2(accel_data.filtered.x, accel_data.filtered.z) * RAD_TO_DEG;
+	rpy[0] = atan2(accel_data.filtered.y, accel_data.filtered.z) * RAD_TO_DEG;
+
+	RPY2Quaternion(rpy, q);
+	attitude_data.quaternion.q1 = q[0];
+	attitude_data.quaternion.q2 = q[1];
+	attitude_data.quaternion.q3 = q[2];
+	attitude_data.quaternion.q4 = q[3];
+	send_attitude();
+}
+
+/**
+ * @brief Output all the important inputs and states of the ekf through serial port
+ */
+static void print_ekf_binary(bool ekf)
+{
+	static uint32_t timeval;
+	uint16_t delay;
+	delay = PIOS_DELAY_DiffuS(timeval);
+	timeval = PIOS_DELAY_GetRaw();
+	
+	PIOS_DELAY_WaituS(500);
+	
+	uint8_t framing[] = { 0xff, 0x00, 0xc3, 0x7d };
+	// Dump raw buffer
+	
+	PIOS_COM_SendBuffer(PIOS_COM_AUX, &framing[0], sizeof(framing));
+	PIOS_COM_SendBuffer(PIOS_COM_AUX, (uint8_t *) & total_conversion_blocks, sizeof(total_conversion_blocks));
+	PIOS_COM_SendBuffer(PIOS_COM_AUX, (uint8_t *) & accel_data.filtered.x, 4*3);
+	PIOS_COM_SendBuffer(PIOS_COM_AUX, (uint8_t *) & gyro_data.filtered.x, 4*3);
+	PIOS_COM_SendBuffer(PIOS_COM_AUX, (uint8_t *) & mag_data.updated, 1);
+	PIOS_COM_SendBuffer(PIOS_COM_AUX, (uint8_t *) & mag_data.scaled.axis, 3*4);
+	PIOS_COM_SendBuffer(PIOS_COM_AUX, (uint8_t *) & altitude_data.updated, 1);
+	PIOS_COM_SendBuffer(PIOS_COM_AUX, (uint8_t *) & altitude_data.altitude, 4);
+	PIOS_COM_SendBuffer(PIOS_COM_AUX, (uint8_t *) &gyro_data.temperature, 4);
+	PIOS_COM_SendBuffer(PIOS_COM_AUX, (uint8_t *) &accel_data.temperature, 4);
+	PIOS_COM_SendBuffer(PIOS_COM_AUX, (uint8_t *) &delay, 2);
+	PIOS_COM_SendBuffer(PIOS_COM_AUX, (uint8_t *) & gps_data, sizeof(gps_data));
+
+	if(ekf)
+		PIOS_COM_SendBuffer(PIOS_COM_AUX, (uint8_t *) & Nav, sizeof(Nav));                                             // X (86:149)
+	else {
+		mag_data.updated = 0;
+		altitude_data.updated = 0;
+		gps_data.updated = 0;
+	}
+}
+
+void panic(uint32_t blinks) 
+{
+	int blinked = 0;
+	while(1) {
+		PIOS_LED_On(LED2);
+		PIOS_DELAY_WaitmS(200);
+		PIOS_LED_Off(LED2);
+		PIOS_DELAY_WaitmS(200);
+
+		blinked++;
+		if(blinked >= blinks) {
+			blinked = 0;
+			PIOS_DELAY_WaitmS(1000);
+		}			
+	}
+}
+
+/**
+ * @brief Get the accel and gyro data from whichever source when available
+ *
+ * This function will act as the HAL for the new INS sensors
+ */
+ 
+uint32_t accel_samples;
+uint32_t gyro_samples;
+struct pios_bma180_data accel;
+struct pios_imu3000_data gyro;
+AttitudeRawData raw;
+int32_t accel_accum[3] = {0, 0, 0};
+int32_t gyro_accum[3] = {0,0,0};
+float scaling;
+
+void get_accel_gyro_data()
+{
+	int32_t read_good;
+	int32_t count;
+	
+	for (int i = 0; i < 3; i++) {
+		accel_accum[i] = 0;
+		gyro_accum[i] = 0;
+	}
+	accel_samples = 0;
+	gyro_samples = 0;
+	
+	// Make sure we get one sample
+	count = 0;
+	while((read_good = PIOS_BMA180_ReadFifo(&accel)) != 0);
+	while(read_good == 0) {	
+		count++;
+
+		accel_accum[0] += accel.x;
+		accel_accum[1] += accel.y;
+		accel_accum[2] += accel.z;
+		
+		read_good = PIOS_BMA180_ReadFifo(&accel);
+	}
+	accel_samples = count;
+
+	// Make sure we get one sample
+	count = 0;
+	while((read_good = PIOS_IMU3000_ReadFifo(&gyro)) != 0);
+	while(read_good == 0) {
+		count++;
+
+		gyro_accum[0] += gyro.x;
+		gyro_accum[1] += gyro.y;
+		gyro_accum[2] += gyro.z;
+		
+		read_good = PIOS_IMU3000_ReadFifo(&gyro);
+	}
+	gyro_samples = count;
+
+	// Not the swaping of channel orders
+	scaling = PIOS_BMA180_GetScale() / accel_samples;
+	accel_data.filtered.x = accel_accum[0] * scaling * accel_data.calibration.scale[0] + accel_data.calibration.bias[0];
+	accel_data.filtered.y = -accel_accum[1] * scaling * accel_data.calibration.scale[1] + accel_data.calibration.bias[1];
+	accel_data.filtered.z = -accel_accum[2] * scaling * accel_data.calibration.scale[2] + accel_data.calibration.bias[2];
+
+	scaling = PIOS_IMU3000_GetScale() / gyro_samples;
+	gyro_data.filtered.x = -((float) gyro_accum[1]) * scaling * gyro_data.calibration.scale[0] + gyro_data.calibration.bias[0];
+	gyro_data.filtered.y = -((float) gyro_accum[0]) * scaling * gyro_data.calibration.scale[1] + gyro_data.calibration.bias[1];
+	gyro_data.filtered.z = -((float) gyro_accum[2]) * scaling * gyro_data.calibration.scale[2] + gyro_data.calibration.bias[2];
+	
+	raw.accels[0] = accel_data.filtered.x;
+	raw.accels[1] = accel_data.filtered.y;
+	raw.accels[2] = accel_data.filtered.z;
+	raw.gyros[0] = gyro_data.filtered.x * RAD_TO_DEG;
+	raw.gyros[1] = gyro_data.filtered.y * RAD_TO_DEG;
+	raw.gyros[2] = gyro_data.filtered.z * RAD_TO_DEG;
+
+	// From data sheet 35 deg C corresponds to -13200, and 280 LSB per C
+	gyro_data.temperature = 35.0f + ((float) gyro.temperature + 13200) / 280;
+	// From the data sheet 25 deg C corresponds to 2 and 2 LSB per C
+	accel_data.temperature = 25.0f + ((float) accel.temperature - 2) / 2;
+	
+	if (bias_corrected_raw)
+	{
+		raw.gyros[0] -= Nav.gyro_bias[0] * RAD_TO_DEG;
+		raw.gyros[1] -= Nav.gyro_bias[1] * RAD_TO_DEG;
+		raw.gyros[2] -= Nav.gyro_bias[2] * RAD_TO_DEG;
+		
+		raw.accels[0] -= Nav.accel_bias[0];
+		raw.accels[1] -= Nav.accel_bias[1];
+		raw.accels[2] -= Nav.accel_bias[2];
+	}
+	
+	raw.temperature[ATTITUDERAW_TEMPERATURE_GYRO] = gyro_data.temperature;
+	raw.temperature[ATTITUDERAW_TEMPERATURE_ACCEL] = accel_data.temperature;
+	
+	raw.magnetometers[0] = mag_data.scaled.axis[0];
+	raw.magnetometers[1] = mag_data.scaled.axis[1];
+	raw.magnetometers[2] = mag_data.scaled.axis[2];
+	AttitudeRawSet(&raw);
+}
+
+/**
+ * @brief Get the mag data from the I2C sensor and load into structure
+ * @return none
+ *
+ * This function also considers if the home location is set and has a valid
+ * magnetic field before updating the mag data to prevent data being used that
+ * cannot be interpreted.  In addition the mag data is not used for the first
+ * five seconds to allow the filter to start to converge
+ */
+void get_mag_data()
+{
+	// Get magnetic readings
+	// For now don't use mags until the magnetic field is set AND until 5 seconds
+	// after initialization otherwise it seems to have problems
+	// TODO: Follow up this initialization issue
+	HomeLocationData home;
+	HomeLocationGet(&home);
+	if (PIOS_HMC5883_NewDataAvailable()) {
+		PIOS_HMC5883_ReadMag(mag_data.raw.axis);
+
+		mag_data.scaled.axis[0] = -(mag_data.raw.axis[0] * mag_data.calibration.scale[0]) + mag_data.calibration.bias[0];
+		mag_data.scaled.axis[1] = -(mag_data.raw.axis[1] * mag_data.calibration.scale[1]) + mag_data.calibration.bias[1];
+		mag_data.scaled.axis[2] = -(mag_data.raw.axis[2] * mag_data.calibration.scale[2]) + mag_data.calibration.bias[2];
+
+		mag_data.updated =  true;
+	}
+}
+
+/**
+ * @brief Get the barometer data
+ * @return none
+ */
+uint32_t baro_conversions = 0;
+void get_baro_data() 
+{
+	int32_t retval = PIOS_BMP085_ReadADC();
+	if (retval == 0) { // Conversion completed 
+		pressure = PIOS_BMP085_GetPressure();
+		altitude = 44330.0 * (1.0 - powf(pressure / BMP085_P0, (1.0 / 5.255)));
+		
+		BaroAltitudeData data;
+		BaroAltitudeGet(&data);
+		data.Altitude = altitude;
+		data.Pressure = pressure  / 1000.0f;
+		data.Temperature = PIOS_BMP085_GetTemperature() / 10.0f;  // Convert to deg C
+		BaroAltitudeSet(&data);
+		
+		if((baro_conversions++) % 2)
+			PIOS_BMP085_StartADC(PressureConv);
+		else
+			PIOS_BMP085_StartADC(TemperatureConv);
+			
+		altitude_data.altitude = data.Altitude;
+		altitude_data.updated = true;
+	}
+}
+
+/**
+ * @brief Process any data coming in the gps port
+ */
+void get_gps_data() 
+{
+	uint8_t c;
+	static bool start_flag = false;
+	static bool found_cr = false;
+	static char gps_rx_buffer[NMEA_MAX_PACKET_LENGTH];
+	static uint32_t rx_count = 0;
+	static uint32_t numChecksumErrors = 0;
+	static uint32_t numParsingErrors = 0;
+	static uint32_t numOverflowErrors = 0;
+	static uint32_t numUpdates = 0;
+	while(PIOS_COM_ReceiveBuffer(pios_com_gps_id, &c, 1, 0) == 1) 
+	{
+		// Echo data back out aux port
+		//PIOS_COM_SendBufferNonBlocking(pios_com_aux_id, &c, 1);
+		
+		// detect start while acquiring stream
+		if (!start_flag && (c == '$'))
+		{
+			start_flag = true;
+			found_cr = false;
+			rx_count = 0;
+		}
+		else if (!start_flag)
+			continue;
+		
+		if (rx_count >= NMEA_MAX_PACKET_LENGTH)
+		{
+			// The buffer is already full and we haven't found a valid NMEA sentence.
+			// Flush the buffer and note the overflow event.
+			start_flag = false;
+			found_cr = false;
+			rx_count = 0;
+			numOverflowErrors++;
+		}
+		else
+		{
+			gps_rx_buffer[rx_count] = c;
+			rx_count++;
+		}
+		
+		// look for ending '\r\n' sequence
+		if (!found_cr && (c == '\r') )
+			found_cr = true;
+		else if (found_cr && (c != '\n') )
+				found_cr = false;  // false end flag
+		else if (found_cr && (c == '\n') )
+		{
+			// The NMEA functions require a zero-terminated string
+			// As we detected \r\n, the string as for sure 2 bytes long, we will also strip the \r\n
+			gps_rx_buffer[rx_count-2] = 0;
+			
+			// prepare to parse next sentence
+			start_flag = false;
+			found_cr = false;
+			rx_count = 0;
+			// Our rxBuffer must look like this now:
+			//   [0]           = '$'
+			//   ...           = zero or more bytes of sentence payload
+			//   [end_pos - 1] = '\r'
+			//   [end_pos]     = '\n'
+			//
+			// Prepare to consume the sentence from the buffer
+			
+			// Validate the checksum over the sentence
+			if (!NMEA_checksum(&gps_rx_buffer[1]))
+			{	// Invalid checksum.  May indicate dropped characters on Rx.
+				//PIOS_DEBUG_PinHigh(2);
+				++numChecksumErrors;
+				//PIOS_DEBUG_PinLow(2);
+			}
+			else
+			{	// Valid checksum, use this packet to update the GPS position
+				if (!NMEA_update_position(&gps_rx_buffer[1])) {
+					//PIOS_DEBUG_PinHigh(2);
+					++numParsingErrors;
+					//PIOS_DEBUG_PinLow(2);
+				}
+				else {
+					++numUpdates;
+					
+					GPSPositionData pos;
+					GPSPositionGet(&pos);
+					HomeLocationData home;
+					HomeLocationGet(&home);
+					
+					// convert from cm back to meters
+					double LLA[3] = {(double) pos.Latitude / 1e7, (double) pos.Longitude / 1e7, (double) (pos.GeoidSeparation + pos.Altitude)};
+					// put in local NED frame
+					double ECEF[3] = {(double) (home.ECEF[0] / 100), (double) (home.ECEF[1] / 100), (double) (home.ECEF[2] / 100)};
+					LLA2Base(LLA, ECEF, (float (*)[3]) home.RNE, gps_data.NED);
+					
+					gps_data.heading = pos.Heading;
+					gps_data.groundspeed = pos.Groundspeed;
+					gps_data.quality = pos.Satellites;
+					gps_data.updated = true;
+					
+					const uint32_t INSGPS_GPS_MINSAT = 6;
+					const float INSGPS_GPS_MINPDOP = 4;
+					
+					// if poor don't use this update
+					if((ahrs_algorithm != INSSETTINGS_ALGORITHM_INSGPS_OUTDOOR) ||
+					   (pos.Satellites < INSGPS_GPS_MINSAT) || 
+					   (pos.PDOP >= INSGPS_GPS_MINPDOP) || 
+					   (home.Set == HOMELOCATION_SET_FALSE) ||
+					   ((home.ECEF[0] == 0) && (home.ECEF[1] == 0) && (home.ECEF[2] == 0)))
+					{
+						gps_data.updated = false;
+					}	
+				}
+			}
+		}
+	}
+	
+}
+
+/**
+ * @brief Assumes board is not moving computes biases and variances of sensors
+ * @returns None
+ *
+ * All data is stored in global structures.  This function should be called from OP when
+ * aircraft is in stable state and then the data stored to SD card.
+ *
+ * After this function the bias for each sensor will be the mean value.  This doesn't make
+ * sense for the z accel so make sure 6 point calibration is also run and those values set
+ * after these read.
+ */
+#define NMEAN  500
+#define NVAR   1000
+#define CHANNELS 6
+static uint32_t calibrate_count = 0;
+float f_means[CHANNELS];
+float f_var[CHANNELS] = {0, 0, 0, 0, 0, 0};
+void measure_noise()
+{
+	uint32_t i;
+
+	float data[CHANNELS] = {accel_data.filtered.x, 
+		accel_data.filtered.y,
+		accel_data.filtered.z, 
+		gyro_data.filtered.x,
+		gyro_data.filtered.y,
+		gyro_data.filtered.z
+	};
+	
+	// First step, zero all sufficient statistics
+	if(calibrate_count == 0) {
+		for (i = 0; i < CHANNELS; i++) {
+			f_means[i] = 0;
+			f_var[i] = 0;
+		}
+	}
+	
+	// Accumulate for an estimate of mean
+	if(calibrate_count < NMEAN)
+		for (i = 0; i < CHANNELS; i++)
+			f_means[i] += data[i];
+	
+	if(calibrate_count == NMEAN)
+		for (i = 0; i < CHANNELS; i++)
+			f_means[i] /= (float) NMEAN;
+	
+	// Accumulate for estimate of variance.  This needs to be done
+	// sequentially because storing second moment would go out of 
+	// float precision
+	if(calibrate_count >= NMEAN && calibrate_count < (NMEAN + NVAR))
+		for (i = 0; i < CHANNELS; i++)
+			f_var[i] += pow(f_means[i] - data[i],2);
+	
+	if(calibrate_count == (NMEAN + NVAR)) {
+		for (i = 0; i < CHANNELS; i++)
+			f_var[i] /= (float) (NVAR - 1);
+
+		calibrate_count = 0;
+
+		InsSettingsData settings;
+		InsSettingsGet(&settings);
+		
+		settings.Algorithm = INSSETTINGS_ALGORITHM_NONE;
+		settings.accel_var[0] = f_var[0];
+		settings.accel_var[1] = f_var[1];
+		settings.accel_var[2] = f_var[2];
+		settings.gyro_var[0] = f_var[3];
+		settings.gyro_var[1] = f_var[4];
+		settings.gyro_var[2] = f_var[5];
+		
+		InsSettingsSet(&settings);
+		settings_callback(InsSettingsHandle());
+	} else {
+		PIOS_DELAY_WaituS(TYPICAL_PERIOD);
+		calibrate_count++;
+	}
+}
+
+void zero_gyros(bool update_settings)
+{
+	const float rate = 1e-2;
+	gyro_data.calibration.bias[0] += -gyro_data.filtered.x * rate;
+	gyro_data.calibration.bias[1] += -gyro_data.filtered.y * rate;
+	gyro_data.calibration.bias[2] += -gyro_data.filtered.z * rate;
+	
+	if(update_settings) {
+		InsSettingsData settings;
+		InsSettingsGet(&settings);
+		settings.gyro_bias[INSSETTINGS_GYRO_BIAS_X] = gyro_data.calibration.bias[0];
+		settings.gyro_bias[INSSETTINGS_GYRO_BIAS_Y] = gyro_data.calibration.bias[1];
+		settings.gyro_bias[INSSETTINGS_GYRO_BIAS_Z] = gyro_data.calibration.bias[2];
+		InsSettingsSet(&settings);
+	}
+}
+
+/**
+ * @brief Populate fields with initial values
+ */
+void reset_values()
+{
+	accel_data.calibration.scale[0] = 1;
+	accel_data.calibration.scale[1] = 1;
+	accel_data.calibration.scale[2] = 1;
+	accel_data.calibration.bias[0] = 0;
+	accel_data.calibration.bias[1] = 0;
+	accel_data.calibration.bias[2] = 0;
+	accel_data.calibration.variance[0] = 1;
+	accel_data.calibration.variance[1] = 1;
+	accel_data.calibration.variance[2] = 1;
+
+	gyro_data.calibration.scale[0] = 1;
+	gyro_data.calibration.scale[1] = 1;
+	gyro_data.calibration.scale[2] = 1;
+	gyro_data.calibration.bias[0] = 0;
+	gyro_data.calibration.bias[1] = 0;
+	gyro_data.calibration.bias[2] = 0;
+	gyro_data.calibration.variance[0] = 1;
+	gyro_data.calibration.variance[1] = 1;
+	gyro_data.calibration.variance[2] = 1;
+	
+	mag_data.calibration.scale[0] = 1;
+	mag_data.calibration.scale[1] = 1;
+	mag_data.calibration.scale[2] = 1;
+	mag_data.calibration.bias[0] = 0;
+	mag_data.calibration.bias[1] = 0;
+	mag_data.calibration.bias[2] = 0;
+	mag_data.calibration.variance[0] = 50;
+	mag_data.calibration.variance[1] = 50;
+	mag_data.calibration.variance[2] = 50;
+	
+	ahrs_algorithm = INSSETTINGS_ALGORITHM_NONE;
+}
+
+void send_attitude(void)
+{
+	AttitudeActualData attitude;
+	AttitudeActualGet(&attitude);
+	attitude.q1 = attitude_data.quaternion.q1;
+	attitude.q2 = attitude_data.quaternion.q2;
+	attitude.q3 = attitude_data.quaternion.q3;
+	attitude.q4 = attitude_data.quaternion.q4;
+	float rpy[3];
+	Quaternion2RPY(&attitude_data.quaternion.q1, rpy);
+	attitude.Roll = rpy[0];
+	attitude.Pitch = rpy[1];
+	attitude.Yaw = rpy[2];
+	AttitudeActualSet(&attitude);
+}
+
+void send_velocity(void)
+{
+	VelocityActualData velocityActual;
+	VelocityActualGet(&velocityActual);
+
+	// convert into cm
+	velocityActual.North = Nav.Vel[0] * 100;
+	velocityActual.East = Nav.Vel[1] * 100;
+	velocityActual.Down = Nav.Vel[2] * 100;
+
+	VelocityActualSet(&velocityActual);
+}
+
+int callback_count = 0;
+void settings_callback(AhrsObjHandle obj)
+{
+	callback_count ++;
+	InsSettingsData settings;
+	InsSettingsGet(&settings);
+
+	init_algorithm = ahrs_algorithm != settings.Algorithm;
+	ahrs_algorithm = settings.Algorithm;
+	bias_corrected_raw = settings.BiasCorrectedRaw == INSSETTINGS_BIASCORRECTEDRAW_TRUE;
+
+	accel_data.calibration.scale[0] = settings.accel_scale[INSSETTINGS_ACCEL_SCALE_X];
+	accel_data.calibration.scale[1] = settings.accel_scale[INSSETTINGS_ACCEL_SCALE_Y];
+	accel_data.calibration.scale[2] = settings.accel_scale[INSSETTINGS_ACCEL_SCALE_Z];
+	accel_data.calibration.bias[0] = settings.accel_bias[INSSETTINGS_ACCEL_BIAS_X];
+	accel_data.calibration.bias[1] = settings.accel_bias[INSSETTINGS_ACCEL_BIAS_Y];
+	accel_data.calibration.bias[2] = settings.accel_bias[INSSETTINGS_ACCEL_BIAS_Z];
+	accel_data.calibration.variance[0] = settings.accel_var[INSSETTINGS_ACCEL_VAR_X];
+	accel_data.calibration.variance[1] = settings.accel_var[INSSETTINGS_ACCEL_VAR_Y];
+	accel_data.calibration.variance[2] = settings.accel_var[INSSETTINGS_ACCEL_VAR_Z];
+	
+	gyro_data.calibration.scale[0] = settings.gyro_scale[INSSETTINGS_GYRO_SCALE_X];
+	gyro_data.calibration.scale[1] = settings.gyro_scale[INSSETTINGS_GYRO_SCALE_Y];
+	gyro_data.calibration.scale[2] = settings.gyro_scale[INSSETTINGS_GYRO_SCALE_Z];
+	gyro_data.calibration.bias[0] = settings.gyro_bias[INSSETTINGS_GYRO_BIAS_X];
+	gyro_data.calibration.bias[1] = settings.gyro_bias[INSSETTINGS_GYRO_BIAS_Y];
+	gyro_data.calibration.bias[2] = settings.gyro_bias[INSSETTINGS_GYRO_BIAS_Z];
+	gyro_data.calibration.variance[0] = settings.gyro_var[INSSETTINGS_GYRO_VAR_X];
+	gyro_data.calibration.variance[1] = settings.gyro_var[INSSETTINGS_GYRO_VAR_Y];
+	gyro_data.calibration.variance[2] = settings.gyro_var[INSSETTINGS_GYRO_VAR_Z];
+	
+	mag_data.calibration.scale[0] = settings.mag_scale[INSSETTINGS_MAG_SCALE_X];
+	mag_data.calibration.scale[1] = settings.mag_scale[INSSETTINGS_MAG_SCALE_Y];
+	mag_data.calibration.scale[2] = settings.mag_scale[INSSETTINGS_MAG_SCALE_Z];
+	mag_data.calibration.bias[0] = settings.mag_bias[INSSETTINGS_MAG_BIAS_X];
+	mag_data.calibration.bias[1] = settings.mag_bias[INSSETTINGS_MAG_BIAS_Y];
+	mag_data.calibration.bias[2] = settings.mag_bias[INSSETTINGS_MAG_BIAS_Z];
+	mag_data.calibration.variance[0] = settings.mag_var[INSSETTINGS_MAG_VAR_X];
+	mag_data.calibration.variance[1] = settings.mag_var[INSSETTINGS_MAG_VAR_Y];
+	mag_data.calibration.variance[2] = settings.mag_var[INSSETTINGS_MAG_VAR_Z];
+}
+
+void homelocation_callback(AhrsObjHandle obj)
+{
+	HomeLocationData data;
+	HomeLocationGet(&data);
+
+	mag_len = sqrt(pow(data.Be[0],2) + pow(data.Be[1],2) + pow(data.Be[2],2));
+	float Be[3] = {data.Be[0] / mag_len, data.Be[1] / mag_len, data.Be[2] / mag_len};
+
+	INSSetMagNorth(Be);
+	
+	init_algorithm = true;
+}
+
+void firmwareiapobj_callback(AhrsObjHandle obj) 
+{
+#if 0
+	const struct pios_board_info * bdinfo = &pios_board_info_blob;
+	
+	FirmwareIAPObjData firmwareIAPObj;
+	FirmwareIAPObjGet(&firmwareIAPObj);
+	if(firmwareIAPObj.ArmReset==0)
+		reset_count=0;
+	if(firmwareIAPObj.ArmReset==1)
+	{
+		
+		if((firmwareIAPObj.BoardType==bdinfo->board_type) || (firmwareIAPObj.BoardType==0xFF))
+		{
+			
+			++reset_count;
+			if(reset_count>2)
+			{
+				PIOS_IAP_SetRequest1();
+				PIOS_IAP_SetRequest2();
+				PIOS_SYS_Reset();
+			}
+		}
+	}
+	else if(firmwareIAPObj.BoardType==bdinfo->board_type && firmwareIAPObj.crc!=PIOS_BL_HELPER_CRC_Memory_Calc())
+	{
+		PIOS_BL_HELPER_FLASH_Read_Description(firmwareIAPObj.Description,bdinfo->desc_size);
+		firmwareIAPObj.crc=PIOS_BL_HELPER_CRC_Memory_Calc();
+		firmwareIAPObj.BoardRevision=bdinfo->board_rev;
+		FirmwareIAPObjSet(&firmwareIAPObj);
+	}
+#endif
+}
+
+
+/**
+ * @}
+ */
+
diff --git a/flight/Revolution/insgps13state.c b/flight/Revolution/insgps13state.c
new file mode 100644
index 000000000..e2868bbd6
--- /dev/null
+++ b/flight/Revolution/insgps13state.c
@@ -0,0 +1,1660 @@
+/**
+ ******************************************************************************
+ * @addtogroup AHRS 
+ * @{
+ * @addtogroup INSGPS
+ * @{
+ * @brief INSGPS is a joint attitude and position estimation EKF
+ *
+ * @file       insgps.c
+ * @author     The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010.
+ * @brief      An INS/GPS algorithm implemented with an EKF.
+ *
+ * @see        The GNU Public License (GPL) Version 3
+ *
+ *****************************************************************************/
+/*
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 3 of the License, or
+ * (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful, but
+ * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY
+ * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License
+ * for more details.
+ *
+ * You should have received a copy of the GNU General Public License along
+ * with this program; if not, write to the Free Software Foundation, Inc.,
+ * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
+ */
+
+#include "insgps.h"
+#include <math.h>
+#include <stdint.h>
+
+// constants/macros/typdefs
+#define NUMX 13			// number of states, X is the state vector
+#define NUMW 9			// number of plant noise inputs, w is disturbance noise vector
+#define NUMV 10			// number of measurements, v is the measurement noise vector
+#define NUMU 6			// number of deterministic inputs, U is the input vector
+
+#if defined(GENERAL_COV)
+// This might trick people so I have a note here.  There is a slower but bigger version of the 
+// code here but won't fit when debugging disabled (requires -Os)
+#define COVARIANCE_PREDICTION_GENERAL
+#endif
+
+// Private functions
+void CovariancePrediction(float F[NUMX][NUMX], float G[NUMX][NUMW],
+			  float Q[NUMW], float dT, float P[NUMX][NUMX]);
+void SerialUpdate(float H[NUMV][NUMX], float R[NUMV], float Z[NUMV],
+		  float Y[NUMV], float P[NUMX][NUMX], float X[NUMX],
+		  uint16_t SensorsUsed);
+void RungeKutta(float X[NUMX], float U[NUMU], float dT);
+void StateEq(float X[NUMX], float U[NUMU], float Xdot[NUMX]);
+void LinearizeFG(float X[NUMX], float U[NUMU], float F[NUMX][NUMX],
+		 float G[NUMX][NUMW]);
+void MeasurementEq(float X[NUMX], float Be[3], float Y[NUMV]);
+void LinearizeH(float X[NUMX], float Be[3], float H[NUMV][NUMX]);
+
+// Private variables
+float F[NUMX][NUMX], G[NUMX][NUMW], H[NUMV][NUMX];	// linearized system matrices
+													// global to init to zero and maintain zero elements
+float Be[3];			// local magnetic unit vector in NED frame
+float P[NUMX][NUMX], X[NUMX];	// covariance matrix and state vector
+float Q[NUMW], R[NUMV];		// input noise and measurement noise variances
+float K[NUMX][NUMV];		// feedback gain matrix
+
+//  *************  Exposed Functions ****************
+//  *************************************************
+
+uint16_t ins_get_num_states() 
+{
+	return NUMX;
+}
+
+void INSGPSInit()		//pretty much just a place holder for now
+{
+	Be[0] = 1;
+	Be[1] = 0;
+	Be[2] = 0;		// local magnetic unit vector
+
+	for (int i = 0; i < NUMX; i++) {
+		for (int j = 0; j < NUMX; j++) {
+			P[i][j] = 0; // zero all terms
+			F[i][j] = 0;
+		}
+		
+		for (int j = 0; j < NUMW; j++)
+			G[i][j] = 0;
+			
+		for (int j = 0; j < NUMV; j++) {
+			H[j][i] = 0;
+			K[i][j] = 0;
+		}
+			
+		X[i] = 0;
+	}
+	for (int i = 0; i < NUMW; i++)
+		Q[i] = 0;
+	for (int i = 0; i < NUMV; i++) 
+		R[i] = 0;
+
+	
+	P[0][0] = P[1][1] = P[2][2] = 25;	// initial position variance (m^2)
+	P[3][3] = P[4][4] = P[5][5] = 5;	// initial velocity variance (m/s)^2
+	P[6][6] = P[7][7] = P[8][8] = P[9][9] = 1e-5;	// initial quaternion variance
+	P[10][10] = P[11][11] = P[12][12] = 1e-9;	// initial gyro bias variance (rad/s)^2
+
+	X[0] = X[1] = X[2] = X[3] = X[4] = X[5] = 0;	// initial pos and vel (m)
+	X[6] = 1;
+	X[7] = X[8] = X[9] = 0;	// initial quaternion (level and North) (m/s)
+	X[10] = X[11] = X[12] = 0;	// initial gyro bias (rad/s)
+
+	Q[0] = Q[1] = Q[2] = 50e-8;	// gyro noise variance (rad/s)^2
+	Q[3] = Q[4] = Q[5] = 0.01;	// accelerometer noise variance (m/s^2)^2
+	Q[6] = Q[7] = Q[8] = 2e-15;	// gyro bias random walk variance (rad/s^2)^2
+
+	R[0] = R[1] = 0.004;	// High freq GPS horizontal position noise variance (m^2)
+	R[2] = 0.036;		// High freq GPS vertical position noise variance (m^2)
+	R[3] = R[4] = 0.004;	// High freq GPS horizontal velocity noise variance (m/s)^2
+	R[5] = 100;		// High freq GPS vertical velocity noise variance (m/s)^2
+	R[6] = R[7] = R[8] = 0.005;	// magnetometer unit vector noise variance
+	R[9] = .05;		// High freq altimeter noise variance (m^2)
+}
+
+void INSResetP(float PDiag[NUMX])
+{
+	uint8_t i,j;
+
+	// if PDiag[i] nonzero then clear row and column and set diagonal element
+	for (i=0;i<NUMX;i++){
+		if (PDiag != 0){
+			for (j=0;j<NUMX;j++)
+				P[i][j]=P[j][i]=0;
+			P[i][i]=PDiag[i];
+		}
+	}
+}
+
+void INSSetState(float pos[3], float vel[3], float q[4], float gyro_bias[3], float accel_bias[3])
+{
+	/* Note: accel_bias not used in 13 state INS */
+	X[0] = pos[0];
+	X[1] = pos[1];
+	X[2] = pos[2];
+	X[3] = vel[0];
+	X[4] = vel[1];
+	X[5] = vel[2];
+	X[6] = q[0];
+	X[7] = q[1];
+	X[8] = q[2];
+	X[9] = q[3];
+	X[10] = gyro_bias[0];
+	X[11] = gyro_bias[1];
+	X[12] = gyro_bias[2];
+}
+
+void INSPosVelReset(float pos[3], float vel[3]) 
+{
+	for (int i = 0; i < 6; i++) {
+		for(int j = i; j < NUMX; j++) {
+			P[i][j] = 0;  // zero the first 6 rows and columns
+			P[j][i] = 0; 
+		}
+	}
+	
+	P[0][0] = P[1][1] = P[2][2] = 25;	// initial position variance (m^2)
+	P[3][3] = P[4][4] = P[5][5] = 5;	// initial velocity variance (m/s)^2
+	
+	X[0] = pos[0];
+	X[1] = pos[1];
+	X[2] = pos[2];
+	X[3] = vel[0];
+	X[4] = vel[1];
+	X[5] = vel[2];	
+}
+
+void INSSetPosVelVar(float PosVar, float VelVar)
+{
+	R[0] = PosVar;
+	R[1] = PosVar;
+	R[2] = PosVar;
+	R[3] = VelVar;
+	R[4] = VelVar;
+//    R[5] = PosVar;  // Don't change vertical velocity, not measured
+}
+
+void INSSetGyroBias(float gyro_bias[3])
+{
+	X[10] = gyro_bias[0];
+	X[11] = gyro_bias[1];
+	X[12] = gyro_bias[2];
+}
+
+void INSSetAccelVar(float accel_var[3])
+{
+	Q[3] = accel_var[0];
+	Q[4] = accel_var[1];
+	Q[5] = accel_var[2];
+}
+
+void INSSetGyroVar(float gyro_var[3])
+{
+	Q[0] = gyro_var[0];
+	Q[1] = gyro_var[1];
+	Q[2] = gyro_var[2];
+}
+
+void INSSetMagVar(float scaled_mag_var[3])
+{
+	R[6] = scaled_mag_var[0];
+	R[7] = scaled_mag_var[1];
+	R[8] = scaled_mag_var[2];
+}
+
+void INSSetMagNorth(float B[3])
+{
+	Be[0] = B[0];
+	Be[1] = B[1];
+	Be[2] = B[2];
+}
+
+void INSStatePrediction(float gyro_data[3], float accel_data[3], float dT)
+{
+	float U[6];
+	float qmag;
+
+	// rate gyro inputs in units of rad/s
+	U[0] = gyro_data[0];
+	U[1] = gyro_data[1];
+	U[2] = gyro_data[2];
+
+	// accelerometer inputs in units of m/s
+	U[3] = accel_data[0];
+	U[4] = accel_data[1];
+	U[5] = accel_data[2];
+
+	// EKF prediction step
+	LinearizeFG(X, U, F, G);
+	RungeKutta(X, U, dT);
+	qmag = sqrt(X[6] * X[6] + X[7] * X[7] + X[8] * X[8] + X[9] * X[9]);
+	X[6] /= qmag;
+	X[7] /= qmag;
+	X[8] /= qmag;
+	X[9] /= qmag;
+	//CovariancePrediction(F,G,Q,dT,P);
+
+	// Update Nav solution structure
+	Nav.Pos[0] = X[0];
+	Nav.Pos[1] = X[1];
+	Nav.Pos[2] = X[2];
+	Nav.Vel[0] = X[3];
+	Nav.Vel[1] = X[4];
+	Nav.Vel[2] = X[5];
+	Nav.q[0] = X[6];
+	Nav.q[1] = X[7];
+	Nav.q[2] = X[8];
+	Nav.q[3] = X[9];
+	Nav.gyro_bias[0] = X[10];
+	Nav.gyro_bias[1] = X[11];
+	Nav.gyro_bias[2] = X[12];	
+}
+
+void INSCovariancePrediction(float dT)
+{
+	CovariancePrediction(F, G, Q, dT, P);
+}
+
+float zeros[3] = { 0, 0, 0 };
+
+void MagCorrection(float mag_data[3])
+{
+	INSCorrection(mag_data, zeros, zeros, zeros[0], MAG_SENSORS);
+}
+
+void MagVelBaroCorrection(float mag_data[3], float Vel[3], float BaroAlt)
+{
+	INSCorrection(mag_data, zeros, Vel, BaroAlt,
+		      MAG_SENSORS | HORIZ_SENSORS | VERT_SENSORS |
+		      BARO_SENSOR);
+}
+
+void GpsBaroCorrection(float Pos[3], float Vel[3], float BaroAlt)
+{
+	INSCorrection(zeros, Pos, Vel, BaroAlt,
+		      HORIZ_SENSORS | VERT_SENSORS | BARO_SENSOR);
+}
+
+void FullCorrection(float mag_data[3], float Pos[3], float Vel[3],
+		    float BaroAlt)
+{
+	INSCorrection(mag_data, Pos, Vel, BaroAlt, FULL_SENSORS);
+}
+
+void GpsMagCorrection(float mag_data[3], float Pos[3], float Vel[3])
+{
+	INSCorrection(mag_data, Pos, Vel, zeros[0],
+		      POS_SENSORS | HORIZ_SENSORS | MAG_SENSORS);
+}
+
+void VelBaroCorrection(float Vel[3], float BaroAlt)
+{
+	INSCorrection(zeros, zeros, Vel, BaroAlt,
+		      HORIZ_SENSORS | VERT_SENSORS | BARO_SENSOR);
+}
+
+void INSCorrection(float mag_data[3], float Pos[3], float Vel[3],
+		   float BaroAlt, uint16_t SensorsUsed)
+{
+	float Z[10], Y[10];
+	float Bmag, qmag;
+
+	// GPS Position in meters and in local NED frame
+	Z[0] = Pos[0];
+	Z[1] = Pos[1];
+	Z[2] = Pos[2];
+
+	// GPS Velocity in meters and in local NED frame
+	Z[3] = Vel[0];
+	Z[4] = Vel[1];
+	Z[5] = Vel[2];
+
+	// magnetometer data in any units (use unit vector) and in body frame
+	Bmag =
+	    sqrt(mag_data[0] * mag_data[0] + mag_data[1] * mag_data[1] +
+		 mag_data[2] * mag_data[2]);
+	Z[6] = mag_data[0] / Bmag;
+	Z[7] = mag_data[1] / Bmag;
+	Z[8] = mag_data[2] / Bmag;
+
+	// barometric altimeter in meters and in local NED frame
+	Z[9] = BaroAlt;
+
+	// EKF correction step
+	LinearizeH(X, Be, H);
+	MeasurementEq(X, Be, Y);
+	SerialUpdate(H, R, Z, Y, P, X, SensorsUsed);
+	qmag = sqrt(X[6] * X[6] + X[7] * X[7] + X[8] * X[8] + X[9] * X[9]);
+	X[6] /= qmag;
+	X[7] /= qmag;
+	X[8] /= qmag;
+	X[9] /= qmag;
+
+	// Update Nav solution structure
+	Nav.Pos[0] = X[0];
+	Nav.Pos[1] = X[1];
+	Nav.Pos[2] = X[2];
+	Nav.Vel[0] = X[3];
+	Nav.Vel[1] = X[4];
+	Nav.Vel[2] = X[5];
+	Nav.q[0] = X[6];
+	Nav.q[1] = X[7];
+	Nav.q[2] = X[8];
+	Nav.q[3] = X[9];
+}
+
+//  *************  CovariancePrediction *************
+//  Does the prediction step of the Kalman filter for the covariance matrix
+//  Output, Pnew, overwrites P, the input covariance
+//  Pnew = (I+F*T)*P*(I+F*T)' + T^2*G*Q*G'
+//  Q is the discrete time covariance of process noise
+//  Q is vector of the diagonal for a square matrix with
+//    dimensions equal to the number of disturbance noise variables
+//  The General Method is very inefficient,not taking advantage of the sparse F and G
+//  The first Method is very specific to this implementation
+//  ************************************************
+
+#ifdef COVARIANCE_PREDICTION_GENERAL
+
+void CovariancePrediction(float F[NUMX][NUMX], float G[NUMX][NUMW],
+			  float Q[NUMW], float dT, float P[NUMX][NUMX])
+{
+	float Dummy[NUMX][NUMX], dTsq;
+	uint8_t i, j, k;
+
+	//  Pnew = (I+F*T)*P*(I+F*T)' + T^2*G*Q*G' = T^2[(P/T + F*P)*(I/T + F') + G*Q*G')]
+
+	dTsq = dT * dT;
+
+	for (i = 0; i < NUMX; i++)	// Calculate Dummy = (P/T +F*P)
+		for (j = 0; j < NUMX; j++) {
+			Dummy[i][j] = P[i][j] / dT;
+			for (k = 0; k < NUMX; k++)
+				Dummy[i][j] += F[i][k] * P[k][j];
+		}
+	for (i = 0; i < NUMX; i++)	// Calculate Pnew = Dummy/T + Dummy*F' + G*Qw*G'
+		for (j = i; j < NUMX; j++) {	// Use symmetry, ie only find upper triangular
+			P[i][j] = Dummy[i][j] / dT;
+			for (k = 0; k < NUMX; k++)
+				P[i][j] += Dummy[i][k] * F[j][k];	// P = Dummy/T + Dummy*F'
+			for (k = 0; k < NUMW; k++)
+				P[i][j] += Q[k] * G[i][k] * G[j][k];	// P = Dummy/T + Dummy*F' + G*Q*G'
+			P[j][i] = P[i][j] = P[i][j] * dTsq;	// Pnew = T^2*P and fill in lower triangular;
+		}
+}
+
+#else
+
+void CovariancePrediction(float F[NUMX][NUMX], float G[NUMX][NUMW],
+			  float Q[NUMW], float dT, float P[NUMX][NUMX])
+{
+	float D[NUMX][NUMX], T, Tsq;
+	uint8_t i, j;
+
+	//  Pnew = (I+F*T)*P*(I+F*T)' + T^2*G*Q*G' = scalar expansion from symbolic manipulator
+
+	T = dT;
+	Tsq = dT * dT;
+
+	for (i = 0; i < NUMX; i++)	// Create a copy of the upper triangular of P
+		for (j = i; j < NUMX; j++)
+			D[i][j] = P[i][j];
+
+	// Brute force calculation of the elements of P
+	P[0][0] = D[3][3] * Tsq + (2 * D[0][3]) * T + D[0][0];
+	P[0][1] = P[1][0] =
+	    D[3][4] * Tsq + (D[0][4] + D[1][3]) * T + D[0][1];
+	P[0][2] = P[2][0] =
+	    D[3][5] * Tsq + (D[0][5] + D[2][3]) * T + D[0][2];
+	P[0][3] = P[3][0] =
+	    (F[3][6] * D[3][6] + F[3][7] * D[3][7] + F[3][8] * D[3][8] +
+	     F[3][9] * D[3][9]) * Tsq + (D[3][3] + F[3][6] * D[0][6] +
+					 F[3][7] * D[0][7] +
+					 F[3][8] * D[0][8] +
+					 F[3][9] * D[0][9]) * T + D[0][3];
+	P[0][4] = P[4][0] =
+	    (F[4][6] * D[3][6] + F[4][7] * D[3][7] + F[4][8] * D[3][8] +
+	     F[4][9] * D[3][9]) * Tsq + (D[3][4] + F[4][6] * D[0][6] +
+					 F[4][7] * D[0][7] +
+					 F[4][8] * D[0][8] +
+					 F[4][9] * D[0][9]) * T + D[0][4];
+	P[0][5] = P[5][0] =
+	    (F[5][6] * D[3][6] + F[5][7] * D[3][7] + F[5][8] * D[3][8] +
+	     F[5][9] * D[3][9]) * Tsq + (D[3][5] + F[5][6] * D[0][6] +
+					 F[5][7] * D[0][7] +
+					 F[5][8] * D[0][8] +
+					 F[5][9] * D[0][9]) * T + D[0][5];
+	P[0][6] = P[6][0] =
+	    (F[6][7] * D[3][7] + F[6][8] * D[3][8] + F[6][9] * D[3][9] +
+	     F[6][10] * D[3][10] + F[6][11] * D[3][11] +
+	     F[6][12] * D[3][12]) * Tsq + (D[3][6] + F[6][7] * D[0][7] +
+					   F[6][8] * D[0][8] +
+					   F[6][9] * D[0][9] +
+					   F[6][10] * D[0][10] +
+					   F[6][11] * D[0][11] +
+					   F[6][12] * D[0][12]) * T +
+	    D[0][6];
+	P[0][7] = P[7][0] =
+	    (F[7][6] * D[3][6] + F[7][8] * D[3][8] + F[7][9] * D[3][9] +
+	     F[7][10] * D[3][10] + F[7][11] * D[3][11] +
+	     F[7][12] * D[3][12]) * Tsq + (D[3][7] + F[7][6] * D[0][6] +
+					   F[7][8] * D[0][8] +
+					   F[7][9] * D[0][9] +
+					   F[7][10] * D[0][10] +
+					   F[7][11] * D[0][11] +
+					   F[7][12] * D[0][12]) * T +
+	    D[0][7];
+	P[0][8] = P[8][0] =
+	    (F[8][6] * D[3][6] + F[8][7] * D[3][7] + F[8][9] * D[3][9] +
+	     F[8][10] * D[3][10] + F[8][11] * D[3][11] +
+	     F[8][12] * D[3][12]) * Tsq + (D[3][8] + F[8][6] * D[0][6] +
+					   F[8][7] * D[0][7] +
+					   F[8][9] * D[0][9] +
+					   F[8][10] * D[0][10] +
+					   F[8][11] * D[0][11] +
+					   F[8][12] * D[0][12]) * T +
+	    D[0][8];
+	P[0][9] = P[9][0] =
+	    (F[9][6] * D[3][6] + F[9][7] * D[3][7] + F[9][8] * D[3][8] +
+	     F[9][10] * D[3][10] + F[9][11] * D[3][11] +
+	     F[9][12] * D[3][12]) * Tsq + (D[3][9] + F[9][6] * D[0][6] +
+					   F[9][7] * D[0][7] +
+					   F[9][8] * D[0][8] +
+					   F[9][10] * D[0][10] +
+					   F[9][11] * D[0][11] +
+					   F[9][12] * D[0][12]) * T +
+	    D[0][9];
+	P[0][10] = P[10][0] = D[3][10] * T + D[0][10];
+	P[0][11] = P[11][0] = D[3][11] * T + D[0][11];
+	P[0][12] = P[12][0] = D[3][12] * T + D[0][12];
+	P[1][1] = D[4][4] * Tsq + (2 * D[1][4]) * T + D[1][1];
+	P[1][2] = P[2][1] =
+	    D[4][5] * Tsq + (D[1][5] + D[2][4]) * T + D[1][2];
+	P[1][3] = P[3][1] =
+	    (F[3][6] * D[4][6] + F[3][7] * D[4][7] + F[3][8] * D[4][8] +
+	     F[3][9] * D[4][9]) * Tsq + (D[3][4] + F[3][6] * D[1][6] +
+					 F[3][7] * D[1][7] +
+					 F[3][8] * D[1][8] +
+					 F[3][9] * D[1][9]) * T + D[1][3];
+	P[1][4] = P[4][1] =
+	    (F[4][6] * D[4][6] + F[4][7] * D[4][7] + F[4][8] * D[4][8] +
+	     F[4][9] * D[4][9]) * Tsq + (D[4][4] + F[4][6] * D[1][6] +
+					 F[4][7] * D[1][7] +
+					 F[4][8] * D[1][8] +
+					 F[4][9] * D[1][9]) * T + D[1][4];
+	P[1][5] = P[5][1] =
+	    (F[5][6] * D[4][6] + F[5][7] * D[4][7] + F[5][8] * D[4][8] +
+	     F[5][9] * D[4][9]) * Tsq + (D[4][5] + F[5][6] * D[1][6] +
+					 F[5][7] * D[1][7] +
+					 F[5][8] * D[1][8] +
+					 F[5][9] * D[1][9]) * T + D[1][5];
+	P[1][6] = P[6][1] =
+	    (F[6][7] * D[4][7] + F[6][8] * D[4][8] + F[6][9] * D[4][9] +
+	     F[6][10] * D[4][10] + F[6][11] * D[4][11] +
+	     F[6][12] * D[4][12]) * Tsq + (D[4][6] + F[6][7] * D[1][7] +
+					   F[6][8] * D[1][8] +
+					   F[6][9] * D[1][9] +
+					   F[6][10] * D[1][10] +
+					   F[6][11] * D[1][11] +
+					   F[6][12] * D[1][12]) * T +
+	    D[1][6];
+	P[1][7] = P[7][1] =
+	    (F[7][6] * D[4][6] + F[7][8] * D[4][8] + F[7][9] * D[4][9] +
+	     F[7][10] * D[4][10] + F[7][11] * D[4][11] +
+	     F[7][12] * D[4][12]) * Tsq + (D[4][7] + F[7][6] * D[1][6] +
+					   F[7][8] * D[1][8] +
+					   F[7][9] * D[1][9] +
+					   F[7][10] * D[1][10] +
+					   F[7][11] * D[1][11] +
+					   F[7][12] * D[1][12]) * T +
+	    D[1][7];
+	P[1][8] = P[8][1] =
+	    (F[8][6] * D[4][6] + F[8][7] * D[4][7] + F[8][9] * D[4][9] +
+	     F[8][10] * D[4][10] + F[8][11] * D[4][11] +
+	     F[8][12] * D[4][12]) * Tsq + (D[4][8] + F[8][6] * D[1][6] +
+					   F[8][7] * D[1][7] +
+					   F[8][9] * D[1][9] +
+					   F[8][10] * D[1][10] +
+					   F[8][11] * D[1][11] +
+					   F[8][12] * D[1][12]) * T +
+	    D[1][8];
+	P[1][9] = P[9][1] =
+	    (F[9][6] * D[4][6] + F[9][7] * D[4][7] + F[9][8] * D[4][8] +
+	     F[9][10] * D[4][10] + F[9][11] * D[4][11] +
+	     F[9][12] * D[4][12]) * Tsq + (D[4][9] + F[9][6] * D[1][6] +
+					   F[9][7] * D[1][7] +
+					   F[9][8] * D[1][8] +
+					   F[9][10] * D[1][10] +
+					   F[9][11] * D[1][11] +
+					   F[9][12] * D[1][12]) * T +
+	    D[1][9];
+	P[1][10] = P[10][1] = D[4][10] * T + D[1][10];
+	P[1][11] = P[11][1] = D[4][11] * T + D[1][11];
+	P[1][12] = P[12][1] = D[4][12] * T + D[1][12];
+	P[2][2] = D[5][5] * Tsq + (2 * D[2][5]) * T + D[2][2];
+	P[2][3] = P[3][2] =
+	    (F[3][6] * D[5][6] + F[3][7] * D[5][7] + F[3][8] * D[5][8] +
+	     F[3][9] * D[5][9]) * Tsq + (D[3][5] + F[3][6] * D[2][6] +
+					 F[3][7] * D[2][7] +
+					 F[3][8] * D[2][8] +
+					 F[3][9] * D[2][9]) * T + D[2][3];
+	P[2][4] = P[4][2] =
+	    (F[4][6] * D[5][6] + F[4][7] * D[5][7] + F[4][8] * D[5][8] +
+	     F[4][9] * D[5][9]) * Tsq + (D[4][5] + F[4][6] * D[2][6] +
+					 F[4][7] * D[2][7] +
+					 F[4][8] * D[2][8] +
+					 F[4][9] * D[2][9]) * T + D[2][4];
+	P[2][5] = P[5][2] =
+	    (F[5][6] * D[5][6] + F[5][7] * D[5][7] + F[5][8] * D[5][8] +
+	     F[5][9] * D[5][9]) * Tsq + (D[5][5] + F[5][6] * D[2][6] +
+					 F[5][7] * D[2][7] +
+					 F[5][8] * D[2][8] +
+					 F[5][9] * D[2][9]) * T + D[2][5];
+	P[2][6] = P[6][2] =
+	    (F[6][7] * D[5][7] + F[6][8] * D[5][8] + F[6][9] * D[5][9] +
+	     F[6][10] * D[5][10] + F[6][11] * D[5][11] +
+	     F[6][12] * D[5][12]) * Tsq + (D[5][6] + F[6][7] * D[2][7] +
+					   F[6][8] * D[2][8] +
+					   F[6][9] * D[2][9] +
+					   F[6][10] * D[2][10] +
+					   F[6][11] * D[2][11] +
+					   F[6][12] * D[2][12]) * T +
+	    D[2][6];
+	P[2][7] = P[7][2] =
+	    (F[7][6] * D[5][6] + F[7][8] * D[5][8] + F[7][9] * D[5][9] +
+	     F[7][10] * D[5][10] + F[7][11] * D[5][11] +
+	     F[7][12] * D[5][12]) * Tsq + (D[5][7] + F[7][6] * D[2][6] +
+					   F[7][8] * D[2][8] +
+					   F[7][9] * D[2][9] +
+					   F[7][10] * D[2][10] +
+					   F[7][11] * D[2][11] +
+					   F[7][12] * D[2][12]) * T +
+	    D[2][7];
+	P[2][8] = P[8][2] =
+	    (F[8][6] * D[5][6] + F[8][7] * D[5][7] + F[8][9] * D[5][9] +
+	     F[8][10] * D[5][10] + F[8][11] * D[5][11] +
+	     F[8][12] * D[5][12]) * Tsq + (D[5][8] + F[8][6] * D[2][6] +
+					   F[8][7] * D[2][7] +
+					   F[8][9] * D[2][9] +
+					   F[8][10] * D[2][10] +
+					   F[8][11] * D[2][11] +
+					   F[8][12] * D[2][12]) * T +
+	    D[2][8];
+	P[2][9] = P[9][2] =
+	    (F[9][6] * D[5][6] + F[9][7] * D[5][7] + F[9][8] * D[5][8] +
+	     F[9][10] * D[5][10] + F[9][11] * D[5][11] +
+	     F[9][12] * D[5][12]) * Tsq + (D[5][9] + F[9][6] * D[2][6] +
+					   F[9][7] * D[2][7] +
+					   F[9][8] * D[2][8] +
+					   F[9][10] * D[2][10] +
+					   F[9][11] * D[2][11] +
+					   F[9][12] * D[2][12]) * T +
+	    D[2][9];
+	P[2][10] = P[10][2] = D[5][10] * T + D[2][10];
+	P[2][11] = P[11][2] = D[5][11] * T + D[2][11];
+	P[2][12] = P[12][2] = D[5][12] * T + D[2][12];
+	P[3][3] =
+	    (Q[3] * G[3][3] * G[3][3] + Q[4] * G[3][4] * G[3][4] +
+	     Q[5] * G[3][5] * G[3][5] + F[3][9] * (F[3][9] * D[9][9] +
+						   F[3][6] * D[6][9] +
+						   F[3][7] * D[7][9] +
+						   F[3][8] * D[8][9]) +
+	     F[3][6] * (F[3][6] * D[6][6] + F[3][7] * D[6][7] +
+			F[3][8] * D[6][8] + F[3][9] * D[6][9]) +
+	     F[3][7] * (F[3][6] * D[6][7] + F[3][7] * D[7][7] +
+			F[3][8] * D[7][8] + F[3][9] * D[7][9]) +
+	     F[3][8] * (F[3][6] * D[6][8] + F[3][7] * D[7][8] +
+			F[3][8] * D[8][8] + F[3][9] * D[8][9])) * Tsq +
+	    (2 * F[3][6] * D[3][6] + 2 * F[3][7] * D[3][7] +
+	     2 * F[3][8] * D[3][8] + 2 * F[3][9] * D[3][9]) * T + D[3][3];
+	P[3][4] = P[4][3] =
+	    (F[4][9] *
+	     (F[3][9] * D[9][9] + F[3][6] * D[6][9] + F[3][7] * D[7][9] +
+	      F[3][8] * D[8][9]) + F[4][6] * (F[3][6] * D[6][6] +
+					      F[3][7] * D[6][7] +
+					      F[3][8] * D[6][8] +
+					      F[3][9] * D[6][9]) +
+	     F[4][7] * (F[3][6] * D[6][7] + F[3][7] * D[7][7] +
+			F[3][8] * D[7][8] + F[3][9] * D[7][9]) +
+	     F[4][8] * (F[3][6] * D[6][8] + F[3][7] * D[7][8] +
+			F[3][8] * D[8][8] + F[3][9] * D[8][9]) +
+	     G[3][3] * G[4][3] * Q[3] + G[3][4] * G[4][4] * Q[4] +
+	     G[3][5] * G[4][5] * Q[5]) * Tsq + (F[3][6] * D[4][6] +
+						F[4][6] * D[3][6] +
+						F[3][7] * D[4][7] +
+						F[4][7] * D[3][7] +
+						F[3][8] * D[4][8] +
+						F[4][8] * D[3][8] +
+						F[3][9] * D[4][9] +
+						F[4][9] * D[3][9]) * T +
+	    D[3][4];
+	P[3][5] = P[5][3] =
+	    (F[5][9] *
+	     (F[3][9] * D[9][9] + F[3][6] * D[6][9] + F[3][7] * D[7][9] +
+	      F[3][8] * D[8][9]) + F[5][6] * (F[3][6] * D[6][6] +
+					      F[3][7] * D[6][7] +
+					      F[3][8] * D[6][8] +
+					      F[3][9] * D[6][9]) +
+	     F[5][7] * (F[3][6] * D[6][7] + F[3][7] * D[7][7] +
+			F[3][8] * D[7][8] + F[3][9] * D[7][9]) +
+	     F[5][8] * (F[3][6] * D[6][8] + F[3][7] * D[7][8] +
+			F[3][8] * D[8][8] + F[3][9] * D[8][9]) +
+	     G[3][3] * G[5][3] * Q[3] + G[3][4] * G[5][4] * Q[4] +
+	     G[3][5] * G[5][5] * Q[5]) * Tsq + (F[3][6] * D[5][6] +
+						F[5][6] * D[3][6] +
+						F[3][7] * D[5][7] +
+						F[5][7] * D[3][7] +
+						F[3][8] * D[5][8] +
+						F[5][8] * D[3][8] +
+						F[3][9] * D[5][9] +
+						F[5][9] * D[3][9]) * T +
+	    D[3][5];
+	P[3][6] = P[6][3] =
+	    (F[6][9] *
+	     (F[3][9] * D[9][9] + F[3][6] * D[6][9] + F[3][7] * D[7][9] +
+	      F[3][8] * D[8][9]) + F[6][10] * (F[3][9] * D[9][10] +
+					       F[3][6] * D[6][10] +
+					       F[3][7] * D[7][10] +
+					       F[3][8] * D[8][10]) +
+	     F[6][11] * (F[3][9] * D[9][11] + F[3][6] * D[6][11] +
+			 F[3][7] * D[7][11] + F[3][8] * D[8][11]) +
+	     F[6][12] * (F[3][9] * D[9][12] + F[3][6] * D[6][12] +
+			 F[3][7] * D[7][12] + F[3][8] * D[8][12]) +
+	     F[6][7] * (F[3][6] * D[6][7] + F[3][7] * D[7][7] +
+			F[3][8] * D[7][8] + F[3][9] * D[7][9]) +
+	     F[6][8] * (F[3][6] * D[6][8] + F[3][7] * D[7][8] +
+			F[3][8] * D[8][8] + F[3][9] * D[8][9])) * Tsq +
+	    (F[3][6] * D[6][6] + F[3][7] * D[6][7] + F[6][7] * D[3][7] +
+	     F[3][8] * D[6][8] + F[6][8] * D[3][8] + F[3][9] * D[6][9] +
+	     F[6][9] * D[3][9] + F[6][10] * D[3][10] +
+	     F[6][11] * D[3][11] + F[6][12] * D[3][12]) * T + D[3][6];
+	P[3][7] = P[7][3] =
+	    (F[7][9] *
+	     (F[3][9] * D[9][9] + F[3][6] * D[6][9] + F[3][7] * D[7][9] +
+	      F[3][8] * D[8][9]) + F[7][10] * (F[3][9] * D[9][10] +
+					       F[3][6] * D[6][10] +
+					       F[3][7] * D[7][10] +
+					       F[3][8] * D[8][10]) +
+	     F[7][11] * (F[3][9] * D[9][11] + F[3][6] * D[6][11] +
+			 F[3][7] * D[7][11] + F[3][8] * D[8][11]) +
+	     F[7][12] * (F[3][9] * D[9][12] + F[3][6] * D[6][12] +
+			 F[3][7] * D[7][12] + F[3][8] * D[8][12]) +
+	     F[7][6] * (F[3][6] * D[6][6] + F[3][7] * D[6][7] +
+			F[3][8] * D[6][8] + F[3][9] * D[6][9]) +
+	     F[7][8] * (F[3][6] * D[6][8] + F[3][7] * D[7][8] +
+			F[3][8] * D[8][8] + F[3][9] * D[8][9])) * Tsq +
+	    (F[3][6] * D[6][7] + F[7][6] * D[3][6] + F[3][7] * D[7][7] +
+	     F[3][8] * D[7][8] + F[7][8] * D[3][8] + F[3][9] * D[7][9] +
+	     F[7][9] * D[3][9] + F[7][10] * D[3][10] +
+	     F[7][11] * D[3][11] + F[7][12] * D[3][12]) * T + D[3][7];
+	P[3][8] = P[8][3] =
+	    (F[8][9] *
+	     (F[3][9] * D[9][9] + F[3][6] * D[6][9] + F[3][7] * D[7][9] +
+	      F[3][8] * D[8][9]) + F[8][10] * (F[3][9] * D[9][10] +
+					       F[3][6] * D[6][10] +
+					       F[3][7] * D[7][10] +
+					       F[3][8] * D[8][10]) +
+	     F[8][11] * (F[3][9] * D[9][11] + F[3][6] * D[6][11] +
+			 F[3][7] * D[7][11] + F[3][8] * D[8][11]) +
+	     F[8][12] * (F[3][9] * D[9][12] + F[3][6] * D[6][12] +
+			 F[3][7] * D[7][12] + F[3][8] * D[8][12]) +
+	     F[8][6] * (F[3][6] * D[6][6] + F[3][7] * D[6][7] +
+			F[3][8] * D[6][8] + F[3][9] * D[6][9]) +
+	     F[8][7] * (F[3][6] * D[6][7] + F[3][7] * D[7][7] +
+			F[3][8] * D[7][8] + F[3][9] * D[7][9])) * Tsq +
+	    (F[3][6] * D[6][8] + F[3][7] * D[7][8] + F[8][6] * D[3][6] +
+	     F[8][7] * D[3][7] + F[3][8] * D[8][8] + F[3][9] * D[8][9] +
+	     F[8][9] * D[3][9] + F[8][10] * D[3][10] +
+	     F[8][11] * D[3][11] + F[8][12] * D[3][12]) * T + D[3][8];
+	P[3][9] = P[9][3] =
+	    (F[9][10] *
+	     (F[3][9] * D[9][10] + F[3][6] * D[6][10] +
+	      F[3][7] * D[7][10] + F[3][8] * D[8][10]) +
+	     F[9][11] * (F[3][9] * D[9][11] + F[3][6] * D[6][11] +
+			 F[3][7] * D[7][11] + F[3][8] * D[8][11]) +
+	     F[9][12] * (F[3][9] * D[9][12] + F[3][6] * D[6][12] +
+			 F[3][7] * D[7][12] + F[3][8] * D[8][12]) +
+	     F[9][6] * (F[3][6] * D[6][6] + F[3][7] * D[6][7] +
+			F[3][8] * D[6][8] + F[3][9] * D[6][9]) +
+	     F[9][7] * (F[3][6] * D[6][7] + F[3][7] * D[7][7] +
+			F[3][8] * D[7][8] + F[3][9] * D[7][9]) +
+	     F[9][8] * (F[3][6] * D[6][8] + F[3][7] * D[7][8] +
+			F[3][8] * D[8][8] + F[3][9] * D[8][9])) * Tsq +
+	    (F[9][6] * D[3][6] + F[9][7] * D[3][7] + F[9][8] * D[3][8] +
+	     F[3][9] * D[9][9] + F[9][10] * D[3][10] +
+	     F[9][11] * D[3][11] + F[9][12] * D[3][12] +
+	     F[3][6] * D[6][9] + F[3][7] * D[7][9] +
+	     F[3][8] * D[8][9]) * T + D[3][9];
+	P[3][10] = P[10][3] =
+	    (F[3][9] * D[9][10] + F[3][6] * D[6][10] + F[3][7] * D[7][10] +
+	     F[3][8] * D[8][10]) * T + D[3][10];
+	P[3][11] = P[11][3] =
+	    (F[3][9] * D[9][11] + F[3][6] * D[6][11] + F[3][7] * D[7][11] +
+	     F[3][8] * D[8][11]) * T + D[3][11];
+	P[3][12] = P[12][3] =
+	    (F[3][9] * D[9][12] + F[3][6] * D[6][12] + F[3][7] * D[7][12] +
+	     F[3][8] * D[8][12]) * T + D[3][12];
+	P[4][4] =
+	    (Q[3] * G[4][3] * G[4][3] + Q[4] * G[4][4] * G[4][4] +
+	     Q[5] * G[4][5] * G[4][5] + F[4][9] * (F[4][9] * D[9][9] +
+						   F[4][6] * D[6][9] +
+						   F[4][7] * D[7][9] +
+						   F[4][8] * D[8][9]) +
+	     F[4][6] * (F[4][6] * D[6][6] + F[4][7] * D[6][7] +
+			F[4][8] * D[6][8] + F[4][9] * D[6][9]) +
+	     F[4][7] * (F[4][6] * D[6][7] + F[4][7] * D[7][7] +
+			F[4][8] * D[7][8] + F[4][9] * D[7][9]) +
+	     F[4][8] * (F[4][6] * D[6][8] + F[4][7] * D[7][8] +
+			F[4][8] * D[8][8] + F[4][9] * D[8][9])) * Tsq +
+	    (2 * F[4][6] * D[4][6] + 2 * F[4][7] * D[4][7] +
+	     2 * F[4][8] * D[4][8] + 2 * F[4][9] * D[4][9]) * T + D[4][4];
+	P[4][5] = P[5][4] =
+	    (F[5][9] *
+	     (F[4][9] * D[9][9] + F[4][6] * D[6][9] + F[4][7] * D[7][9] +
+	      F[4][8] * D[8][9]) + F[5][6] * (F[4][6] * D[6][6] +
+					      F[4][7] * D[6][7] +
+					      F[4][8] * D[6][8] +
+					      F[4][9] * D[6][9]) +
+	     F[5][7] * (F[4][6] * D[6][7] + F[4][7] * D[7][7] +
+			F[4][8] * D[7][8] + F[4][9] * D[7][9]) +
+	     F[5][8] * (F[4][6] * D[6][8] + F[4][7] * D[7][8] +
+			F[4][8] * D[8][8] + F[4][9] * D[8][9]) +
+	     G[4][3] * G[5][3] * Q[3] + G[4][4] * G[5][4] * Q[4] +
+	     G[4][5] * G[5][5] * Q[5]) * Tsq + (F[4][6] * D[5][6] +
+						F[5][6] * D[4][6] +
+						F[4][7] * D[5][7] +
+						F[5][7] * D[4][7] +
+						F[4][8] * D[5][8] +
+						F[5][8] * D[4][8] +
+						F[4][9] * D[5][9] +
+						F[5][9] * D[4][9]) * T +
+	    D[4][5];
+	P[4][6] = P[6][4] =
+	    (F[6][9] *
+	     (F[4][9] * D[9][9] + F[4][6] * D[6][9] + F[4][7] * D[7][9] +
+	      F[4][8] * D[8][9]) + F[6][10] * (F[4][9] * D[9][10] +
+					       F[4][6] * D[6][10] +
+					       F[4][7] * D[7][10] +
+					       F[4][8] * D[8][10]) +
+	     F[6][11] * (F[4][9] * D[9][11] + F[4][6] * D[6][11] +
+			 F[4][7] * D[7][11] + F[4][8] * D[8][11]) +
+	     F[6][12] * (F[4][9] * D[9][12] + F[4][6] * D[6][12] +
+			 F[4][7] * D[7][12] + F[4][8] * D[8][12]) +
+	     F[6][7] * (F[4][6] * D[6][7] + F[4][7] * D[7][7] +
+			F[4][8] * D[7][8] + F[4][9] * D[7][9]) +
+	     F[6][8] * (F[4][6] * D[6][8] + F[4][7] * D[7][8] +
+			F[4][8] * D[8][8] + F[4][9] * D[8][9])) * Tsq +
+	    (F[4][6] * D[6][6] + F[4][7] * D[6][7] + F[6][7] * D[4][7] +
+	     F[4][8] * D[6][8] + F[6][8] * D[4][8] + F[4][9] * D[6][9] +
+	     F[6][9] * D[4][9] + F[6][10] * D[4][10] +
+	     F[6][11] * D[4][11] + F[6][12] * D[4][12]) * T + D[4][6];
+	P[4][7] = P[7][4] =
+	    (F[7][9] *
+	     (F[4][9] * D[9][9] + F[4][6] * D[6][9] + F[4][7] * D[7][9] +
+	      F[4][8] * D[8][9]) + F[7][10] * (F[4][9] * D[9][10] +
+					       F[4][6] * D[6][10] +
+					       F[4][7] * D[7][10] +
+					       F[4][8] * D[8][10]) +
+	     F[7][11] * (F[4][9] * D[9][11] + F[4][6] * D[6][11] +
+			 F[4][7] * D[7][11] + F[4][8] * D[8][11]) +
+	     F[7][12] * (F[4][9] * D[9][12] + F[4][6] * D[6][12] +
+			 F[4][7] * D[7][12] + F[4][8] * D[8][12]) +
+	     F[7][6] * (F[4][6] * D[6][6] + F[4][7] * D[6][7] +
+			F[4][8] * D[6][8] + F[4][9] * D[6][9]) +
+	     F[7][8] * (F[4][6] * D[6][8] + F[4][7] * D[7][8] +
+			F[4][8] * D[8][8] + F[4][9] * D[8][9])) * Tsq +
+	    (F[4][6] * D[6][7] + F[7][6] * D[4][6] + F[4][7] * D[7][7] +
+	     F[4][8] * D[7][8] + F[7][8] * D[4][8] + F[4][9] * D[7][9] +
+	     F[7][9] * D[4][9] + F[7][10] * D[4][10] +
+	     F[7][11] * D[4][11] + F[7][12] * D[4][12]) * T + D[4][7];
+	P[4][8] = P[8][4] =
+	    (F[8][9] *
+	     (F[4][9] * D[9][9] + F[4][6] * D[6][9] + F[4][7] * D[7][9] +
+	      F[4][8] * D[8][9]) + F[8][10] * (F[4][9] * D[9][10] +
+					       F[4][6] * D[6][10] +
+					       F[4][7] * D[7][10] +
+					       F[4][8] * D[8][10]) +
+	     F[8][11] * (F[4][9] * D[9][11] + F[4][6] * D[6][11] +
+			 F[4][7] * D[7][11] + F[4][8] * D[8][11]) +
+	     F[8][12] * (F[4][9] * D[9][12] + F[4][6] * D[6][12] +
+			 F[4][7] * D[7][12] + F[4][8] * D[8][12]) +
+	     F[8][6] * (F[4][6] * D[6][6] + F[4][7] * D[6][7] +
+			F[4][8] * D[6][8] + F[4][9] * D[6][9]) +
+	     F[8][7] * (F[4][6] * D[6][7] + F[4][7] * D[7][7] +
+			F[4][8] * D[7][8] + F[4][9] * D[7][9])) * Tsq +
+	    (F[4][6] * D[6][8] + F[4][7] * D[7][8] + F[8][6] * D[4][6] +
+	     F[8][7] * D[4][7] + F[4][8] * D[8][8] + F[4][9] * D[8][9] +
+	     F[8][9] * D[4][9] + F[8][10] * D[4][10] +
+	     F[8][11] * D[4][11] + F[8][12] * D[4][12]) * T + D[4][8];
+	P[4][9] = P[9][4] =
+	    (F[9][10] *
+	     (F[4][9] * D[9][10] + F[4][6] * D[6][10] +
+	      F[4][7] * D[7][10] + F[4][8] * D[8][10]) +
+	     F[9][11] * (F[4][9] * D[9][11] + F[4][6] * D[6][11] +
+			 F[4][7] * D[7][11] + F[4][8] * D[8][11]) +
+	     F[9][12] * (F[4][9] * D[9][12] + F[4][6] * D[6][12] +
+			 F[4][7] * D[7][12] + F[4][8] * D[8][12]) +
+	     F[9][6] * (F[4][6] * D[6][6] + F[4][7] * D[6][7] +
+			F[4][8] * D[6][8] + F[4][9] * D[6][9]) +
+	     F[9][7] * (F[4][6] * D[6][7] + F[4][7] * D[7][7] +
+			F[4][8] * D[7][8] + F[4][9] * D[7][9]) +
+	     F[9][8] * (F[4][6] * D[6][8] + F[4][7] * D[7][8] +
+			F[4][8] * D[8][8] + F[4][9] * D[8][9])) * Tsq +
+	    (F[9][6] * D[4][6] + F[9][7] * D[4][7] + F[9][8] * D[4][8] +
+	     F[4][9] * D[9][9] + F[9][10] * D[4][10] +
+	     F[9][11] * D[4][11] + F[9][12] * D[4][12] +
+	     F[4][6] * D[6][9] + F[4][7] * D[7][9] +
+	     F[4][8] * D[8][9]) * T + D[4][9];
+	P[4][10] = P[10][4] =
+	    (F[4][9] * D[9][10] + F[4][6] * D[6][10] + F[4][7] * D[7][10] +
+	     F[4][8] * D[8][10]) * T + D[4][10];
+	P[4][11] = P[11][4] =
+	    (F[4][9] * D[9][11] + F[4][6] * D[6][11] + F[4][7] * D[7][11] +
+	     F[4][8] * D[8][11]) * T + D[4][11];
+	P[4][12] = P[12][4] =
+	    (F[4][9] * D[9][12] + F[4][6] * D[6][12] + F[4][7] * D[7][12] +
+	     F[4][8] * D[8][12]) * T + D[4][12];
+	P[5][5] =
+	    (Q[3] * G[5][3] * G[5][3] + Q[4] * G[5][4] * G[5][4] +
+	     Q[5] * G[5][5] * G[5][5] + F[5][9] * (F[5][9] * D[9][9] +
+						   F[5][6] * D[6][9] +
+						   F[5][7] * D[7][9] +
+						   F[5][8] * D[8][9]) +
+	     F[5][6] * (F[5][6] * D[6][6] + F[5][7] * D[6][7] +
+			F[5][8] * D[6][8] + F[5][9] * D[6][9]) +
+	     F[5][7] * (F[5][6] * D[6][7] + F[5][7] * D[7][7] +
+			F[5][8] * D[7][8] + F[5][9] * D[7][9]) +
+	     F[5][8] * (F[5][6] * D[6][8] + F[5][7] * D[7][8] +
+			F[5][8] * D[8][8] + F[5][9] * D[8][9])) * Tsq +
+	    (2 * F[5][6] * D[5][6] + 2 * F[5][7] * D[5][7] +
+	     2 * F[5][8] * D[5][8] + 2 * F[5][9] * D[5][9]) * T + D[5][5];
+	P[5][6] = P[6][5] =
+	    (F[6][9] *
+	     (F[5][9] * D[9][9] + F[5][6] * D[6][9] + F[5][7] * D[7][9] +
+	      F[5][8] * D[8][9]) + F[6][10] * (F[5][9] * D[9][10] +
+					       F[5][6] * D[6][10] +
+					       F[5][7] * D[7][10] +
+					       F[5][8] * D[8][10]) +
+	     F[6][11] * (F[5][9] * D[9][11] + F[5][6] * D[6][11] +
+			 F[5][7] * D[7][11] + F[5][8] * D[8][11]) +
+	     F[6][12] * (F[5][9] * D[9][12] + F[5][6] * D[6][12] +
+			 F[5][7] * D[7][12] + F[5][8] * D[8][12]) +
+	     F[6][7] * (F[5][6] * D[6][7] + F[5][7] * D[7][7] +
+			F[5][8] * D[7][8] + F[5][9] * D[7][9]) +
+	     F[6][8] * (F[5][6] * D[6][8] + F[5][7] * D[7][8] +
+			F[5][8] * D[8][8] + F[5][9] * D[8][9])) * Tsq +
+	    (F[5][6] * D[6][6] + F[5][7] * D[6][7] + F[6][7] * D[5][7] +
+	     F[5][8] * D[6][8] + F[6][8] * D[5][8] + F[5][9] * D[6][9] +
+	     F[6][9] * D[5][9] + F[6][10] * D[5][10] +
+	     F[6][11] * D[5][11] + F[6][12] * D[5][12]) * T + D[5][6];
+	P[5][7] = P[7][5] =
+	    (F[7][9] *
+	     (F[5][9] * D[9][9] + F[5][6] * D[6][9] + F[5][7] * D[7][9] +
+	      F[5][8] * D[8][9]) + F[7][10] * (F[5][9] * D[9][10] +
+					       F[5][6] * D[6][10] +
+					       F[5][7] * D[7][10] +
+					       F[5][8] * D[8][10]) +
+	     F[7][11] * (F[5][9] * D[9][11] + F[5][6] * D[6][11] +
+			 F[5][7] * D[7][11] + F[5][8] * D[8][11]) +
+	     F[7][12] * (F[5][9] * D[9][12] + F[5][6] * D[6][12] +
+			 F[5][7] * D[7][12] + F[5][8] * D[8][12]) +
+	     F[7][6] * (F[5][6] * D[6][6] + F[5][7] * D[6][7] +
+			F[5][8] * D[6][8] + F[5][9] * D[6][9]) +
+	     F[7][8] * (F[5][6] * D[6][8] + F[5][7] * D[7][8] +
+			F[5][8] * D[8][8] + F[5][9] * D[8][9])) * Tsq +
+	    (F[5][6] * D[6][7] + F[7][6] * D[5][6] + F[5][7] * D[7][7] +
+	     F[5][8] * D[7][8] + F[7][8] * D[5][8] + F[5][9] * D[7][9] +
+	     F[7][9] * D[5][9] + F[7][10] * D[5][10] +
+	     F[7][11] * D[5][11] + F[7][12] * D[5][12]) * T + D[5][7];
+	P[5][8] = P[8][5] =
+	    (F[8][9] *
+	     (F[5][9] * D[9][9] + F[5][6] * D[6][9] + F[5][7] * D[7][9] +
+	      F[5][8] * D[8][9]) + F[8][10] * (F[5][9] * D[9][10] +
+					       F[5][6] * D[6][10] +
+					       F[5][7] * D[7][10] +
+					       F[5][8] * D[8][10]) +
+	     F[8][11] * (F[5][9] * D[9][11] + F[5][6] * D[6][11] +
+			 F[5][7] * D[7][11] + F[5][8] * D[8][11]) +
+	     F[8][12] * (F[5][9] * D[9][12] + F[5][6] * D[6][12] +
+			 F[5][7] * D[7][12] + F[5][8] * D[8][12]) +
+	     F[8][6] * (F[5][6] * D[6][6] + F[5][7] * D[6][7] +
+			F[5][8] * D[6][8] + F[5][9] * D[6][9]) +
+	     F[8][7] * (F[5][6] * D[6][7] + F[5][7] * D[7][7] +
+			F[5][8] * D[7][8] + F[5][9] * D[7][9])) * Tsq +
+	    (F[5][6] * D[6][8] + F[5][7] * D[7][8] + F[8][6] * D[5][6] +
+	     F[8][7] * D[5][7] + F[5][8] * D[8][8] + F[5][9] * D[8][9] +
+	     F[8][9] * D[5][9] + F[8][10] * D[5][10] +
+	     F[8][11] * D[5][11] + F[8][12] * D[5][12]) * T + D[5][8];
+	P[5][9] = P[9][5] =
+	    (F[9][10] *
+	     (F[5][9] * D[9][10] + F[5][6] * D[6][10] +
+	      F[5][7] * D[7][10] + F[5][8] * D[8][10]) +
+	     F[9][11] * (F[5][9] * D[9][11] + F[5][6] * D[6][11] +
+			 F[5][7] * D[7][11] + F[5][8] * D[8][11]) +
+	     F[9][12] * (F[5][9] * D[9][12] + F[5][6] * D[6][12] +
+			 F[5][7] * D[7][12] + F[5][8] * D[8][12]) +
+	     F[9][6] * (F[5][6] * D[6][6] + F[5][7] * D[6][7] +
+			F[5][8] * D[6][8] + F[5][9] * D[6][9]) +
+	     F[9][7] * (F[5][6] * D[6][7] + F[5][7] * D[7][7] +
+			F[5][8] * D[7][8] + F[5][9] * D[7][9]) +
+	     F[9][8] * (F[5][6] * D[6][8] + F[5][7] * D[7][8] +
+			F[5][8] * D[8][8] + F[5][9] * D[8][9])) * Tsq +
+	    (F[9][6] * D[5][6] + F[9][7] * D[5][7] + F[9][8] * D[5][8] +
+	     F[5][9] * D[9][9] + F[9][10] * D[5][10] +
+	     F[9][11] * D[5][11] + F[9][12] * D[5][12] +
+	     F[5][6] * D[6][9] + F[5][7] * D[7][9] +
+	     F[5][8] * D[8][9]) * T + D[5][9];
+	P[5][10] = P[10][5] =
+	    (F[5][9] * D[9][10] + F[5][6] * D[6][10] + F[5][7] * D[7][10] +
+	     F[5][8] * D[8][10]) * T + D[5][10];
+	P[5][11] = P[11][5] =
+	    (F[5][9] * D[9][11] + F[5][6] * D[6][11] + F[5][7] * D[7][11] +
+	     F[5][8] * D[8][11]) * T + D[5][11];
+	P[5][12] = P[12][5] =
+	    (F[5][9] * D[9][12] + F[5][6] * D[6][12] + F[5][7] * D[7][12] +
+	     F[5][8] * D[8][12]) * T + D[5][12];
+	P[6][6] =
+	    (Q[0] * G[6][0] * G[6][0] + Q[1] * G[6][1] * G[6][1] +
+	     Q[2] * G[6][2] * G[6][2] + F[6][9] * (F[6][9] * D[9][9] +
+						   F[6][10] * D[9][10] +
+						   F[6][11] * D[9][11] +
+						   F[6][12] * D[9][12] +
+						   F[6][7] * D[7][9] +
+						   F[6][8] * D[8][9]) +
+	     F[6][10] * (F[6][9] * D[9][10] + F[6][10] * D[10][10] +
+			 F[6][11] * D[10][11] + F[6][12] * D[10][12] +
+			 F[6][7] * D[7][10] + F[6][8] * D[8][10]) +
+	     F[6][11] * (F[6][9] * D[9][11] + F[6][10] * D[10][11] +
+			 F[6][11] * D[11][11] + F[6][12] * D[11][12] +
+			 F[6][7] * D[7][11] + F[6][8] * D[8][11]) +
+	     F[6][12] * (F[6][9] * D[9][12] + F[6][10] * D[10][12] +
+			 F[6][11] * D[11][12] + F[6][12] * D[12][12] +
+			 F[6][7] * D[7][12] + F[6][8] * D[8][12]) +
+	     F[6][7] * (F[6][7] * D[7][7] + F[6][8] * D[7][8] +
+			F[6][9] * D[7][9] + F[6][10] * D[7][10] +
+			F[6][11] * D[7][11] + F[6][12] * D[7][12]) +
+	     F[6][8] * (F[6][7] * D[7][8] + F[6][8] * D[8][8] +
+			F[6][9] * D[8][9] + F[6][10] * D[8][10] +
+			F[6][11] * D[8][11] + F[6][12] * D[8][12])) * Tsq +
+	    (2 * F[6][7] * D[6][7] + 2 * F[6][8] * D[6][8] +
+	     2 * F[6][9] * D[6][9] + 2 * F[6][10] * D[6][10] +
+	     2 * F[6][11] * D[6][11] + 2 * F[6][12] * D[6][12]) * T +
+	    D[6][6];
+	P[6][7] = P[7][6] =
+	    (F[7][9] *
+	     (F[6][9] * D[9][9] + F[6][10] * D[9][10] +
+	      F[6][11] * D[9][11] + F[6][12] * D[9][12] +
+	      F[6][7] * D[7][9] + F[6][8] * D[8][9]) +
+	     F[7][10] * (F[6][9] * D[9][10] + F[6][10] * D[10][10] +
+			 F[6][11] * D[10][11] + F[6][12] * D[10][12] +
+			 F[6][7] * D[7][10] + F[6][8] * D[8][10]) +
+	     F[7][11] * (F[6][9] * D[9][11] + F[6][10] * D[10][11] +
+			 F[6][11] * D[11][11] + F[6][12] * D[11][12] +
+			 F[6][7] * D[7][11] + F[6][8] * D[8][11]) +
+	     F[7][12] * (F[6][9] * D[9][12] + F[6][10] * D[10][12] +
+			 F[6][11] * D[11][12] + F[6][12] * D[12][12] +
+			 F[6][7] * D[7][12] + F[6][8] * D[8][12]) +
+	     F[7][6] * (F[6][7] * D[6][7] + F[6][8] * D[6][8] +
+			F[6][9] * D[6][9] + F[6][10] * D[6][10] +
+			F[6][11] * D[6][11] + F[6][12] * D[6][12]) +
+	     F[7][8] * (F[6][7] * D[7][8] + F[6][8] * D[8][8] +
+			F[6][9] * D[8][9] + F[6][10] * D[8][10] +
+			F[6][11] * D[8][11] + F[6][12] * D[8][12]) +
+	     G[6][0] * G[7][0] * Q[0] + G[6][1] * G[7][1] * Q[1] +
+	     G[6][2] * G[7][2] * Q[2]) * Tsq + (F[7][6] * D[6][6] +
+						F[6][7] * D[7][7] +
+						F[6][8] * D[7][8] +
+						F[7][8] * D[6][8] +
+						F[6][9] * D[7][9] +
+						F[7][9] * D[6][9] +
+						F[6][10] * D[7][10] +
+						F[7][10] * D[6][10] +
+						F[6][11] * D[7][11] +
+						F[7][11] * D[6][11] +
+						F[6][12] * D[7][12] +
+						F[7][12] * D[6][12]) * T +
+	    D[6][7];
+	P[6][8] = P[8][6] =
+	    (F[8][9] *
+	     (F[6][9] * D[9][9] + F[6][10] * D[9][10] +
+	      F[6][11] * D[9][11] + F[6][12] * D[9][12] +
+	      F[6][7] * D[7][9] + F[6][8] * D[8][9]) +
+	     F[8][10] * (F[6][9] * D[9][10] + F[6][10] * D[10][10] +
+			 F[6][11] * D[10][11] + F[6][12] * D[10][12] +
+			 F[6][7] * D[7][10] + F[6][8] * D[8][10]) +
+	     F[8][11] * (F[6][9] * D[9][11] + F[6][10] * D[10][11] +
+			 F[6][11] * D[11][11] + F[6][12] * D[11][12] +
+			 F[6][7] * D[7][11] + F[6][8] * D[8][11]) +
+	     F[8][12] * (F[6][9] * D[9][12] + F[6][10] * D[10][12] +
+			 F[6][11] * D[11][12] + F[6][12] * D[12][12] +
+			 F[6][7] * D[7][12] + F[6][8] * D[8][12]) +
+	     F[8][6] * (F[6][7] * D[6][7] + F[6][8] * D[6][8] +
+			F[6][9] * D[6][9] + F[6][10] * D[6][10] +
+			F[6][11] * D[6][11] + F[6][12] * D[6][12]) +
+	     F[8][7] * (F[6][7] * D[7][7] + F[6][8] * D[7][8] +
+			F[6][9] * D[7][9] + F[6][10] * D[7][10] +
+			F[6][11] * D[7][11] + F[6][12] * D[7][12]) +
+	     G[6][0] * G[8][0] * Q[0] + G[6][1] * G[8][1] * Q[1] +
+	     G[6][2] * G[8][2] * Q[2]) * Tsq + (F[6][7] * D[7][8] +
+						F[8][6] * D[6][6] +
+						F[8][7] * D[6][7] +
+						F[6][8] * D[8][8] +
+						F[6][9] * D[8][9] +
+						F[8][9] * D[6][9] +
+						F[6][10] * D[8][10] +
+						F[8][10] * D[6][10] +
+						F[6][11] * D[8][11] +
+						F[8][11] * D[6][11] +
+						F[6][12] * D[8][12] +
+						F[8][12] * D[6][12]) * T +
+	    D[6][8];
+	P[6][9] = P[9][6] =
+	    (F[9][10] *
+	     (F[6][9] * D[9][10] + F[6][10] * D[10][10] +
+	      F[6][11] * D[10][11] + F[6][12] * D[10][12] +
+	      F[6][7] * D[7][10] + F[6][8] * D[8][10]) +
+	     F[9][11] * (F[6][9] * D[9][11] + F[6][10] * D[10][11] +
+			 F[6][11] * D[11][11] + F[6][12] * D[11][12] +
+			 F[6][7] * D[7][11] + F[6][8] * D[8][11]) +
+	     F[9][12] * (F[6][9] * D[9][12] + F[6][10] * D[10][12] +
+			 F[6][11] * D[11][12] + F[6][12] * D[12][12] +
+			 F[6][7] * D[7][12] + F[6][8] * D[8][12]) +
+	     F[9][6] * (F[6][7] * D[6][7] + F[6][8] * D[6][8] +
+			F[6][9] * D[6][9] + F[6][10] * D[6][10] +
+			F[6][11] * D[6][11] + F[6][12] * D[6][12]) +
+	     F[9][7] * (F[6][7] * D[7][7] + F[6][8] * D[7][8] +
+			F[6][9] * D[7][9] + F[6][10] * D[7][10] +
+			F[6][11] * D[7][11] + F[6][12] * D[7][12]) +
+	     F[9][8] * (F[6][7] * D[7][8] + F[6][8] * D[8][8] +
+			F[6][9] * D[8][9] + F[6][10] * D[8][10] +
+			F[6][11] * D[8][11] + F[6][12] * D[8][12]) +
+	     G[9][0] * G[6][0] * Q[0] + G[9][1] * G[6][1] * Q[1] +
+	     G[9][2] * G[6][2] * Q[2]) * Tsq + (F[9][6] * D[6][6] +
+						F[9][7] * D[6][7] +
+						F[9][8] * D[6][8] +
+						F[6][9] * D[9][9] +
+						F[9][10] * D[6][10] +
+						F[6][10] * D[9][10] +
+						F[9][11] * D[6][11] +
+						F[6][11] * D[9][11] +
+						F[9][12] * D[6][12] +
+						F[6][12] * D[9][12] +
+						F[6][7] * D[7][9] +
+						F[6][8] * D[8][9]) * T +
+	    D[6][9];
+	P[6][10] = P[10][6] =
+	    (F[6][9] * D[9][10] + F[6][10] * D[10][10] +
+	     F[6][11] * D[10][11] + F[6][12] * D[10][12] +
+	     F[6][7] * D[7][10] + F[6][8] * D[8][10]) * T + D[6][10];
+	P[6][11] = P[11][6] =
+	    (F[6][9] * D[9][11] + F[6][10] * D[10][11] +
+	     F[6][11] * D[11][11] + F[6][12] * D[11][12] +
+	     F[6][7] * D[7][11] + F[6][8] * D[8][11]) * T + D[6][11];
+	P[6][12] = P[12][6] =
+	    (F[6][9] * D[9][12] + F[6][10] * D[10][12] +
+	     F[6][11] * D[11][12] + F[6][12] * D[12][12] +
+	     F[6][7] * D[7][12] + F[6][8] * D[8][12]) * T + D[6][12];
+	P[7][7] =
+	    (Q[0] * G[7][0] * G[7][0] + Q[1] * G[7][1] * G[7][1] +
+	     Q[2] * G[7][2] * G[7][2] + F[7][9] * (F[7][9] * D[9][9] +
+						   F[7][10] * D[9][10] +
+						   F[7][11] * D[9][11] +
+						   F[7][12] * D[9][12] +
+						   F[7][6] * D[6][9] +
+						   F[7][8] * D[8][9]) +
+	     F[7][10] * (F[7][9] * D[9][10] + F[7][10] * D[10][10] +
+			 F[7][11] * D[10][11] + F[7][12] * D[10][12] +
+			 F[7][6] * D[6][10] + F[7][8] * D[8][10]) +
+	     F[7][11] * (F[7][9] * D[9][11] + F[7][10] * D[10][11] +
+			 F[7][11] * D[11][11] + F[7][12] * D[11][12] +
+			 F[7][6] * D[6][11] + F[7][8] * D[8][11]) +
+	     F[7][12] * (F[7][9] * D[9][12] + F[7][10] * D[10][12] +
+			 F[7][11] * D[11][12] + F[7][12] * D[12][12] +
+			 F[7][6] * D[6][12] + F[7][8] * D[8][12]) +
+	     F[7][6] * (F[7][6] * D[6][6] + F[7][8] * D[6][8] +
+			F[7][9] * D[6][9] + F[7][10] * D[6][10] +
+			F[7][11] * D[6][11] + F[7][12] * D[6][12]) +
+	     F[7][8] * (F[7][6] * D[6][8] + F[7][8] * D[8][8] +
+			F[7][9] * D[8][9] + F[7][10] * D[8][10] +
+			F[7][11] * D[8][11] + F[7][12] * D[8][12])) * Tsq +
+	    (2 * F[7][6] * D[6][7] + 2 * F[7][8] * D[7][8] +
+	     2 * F[7][9] * D[7][9] + 2 * F[7][10] * D[7][10] +
+	     2 * F[7][11] * D[7][11] + 2 * F[7][12] * D[7][12]) * T +
+	    D[7][7];
+	P[7][8] = P[8][7] =
+	    (F[8][9] *
+	     (F[7][9] * D[9][9] + F[7][10] * D[9][10] +
+	      F[7][11] * D[9][11] + F[7][12] * D[9][12] +
+	      F[7][6] * D[6][9] + F[7][8] * D[8][9]) +
+	     F[8][10] * (F[7][9] * D[9][10] + F[7][10] * D[10][10] +
+			 F[7][11] * D[10][11] + F[7][12] * D[10][12] +
+			 F[7][6] * D[6][10] + F[7][8] * D[8][10]) +
+	     F[8][11] * (F[7][9] * D[9][11] + F[7][10] * D[10][11] +
+			 F[7][11] * D[11][11] + F[7][12] * D[11][12] +
+			 F[7][6] * D[6][11] + F[7][8] * D[8][11]) +
+	     F[8][12] * (F[7][9] * D[9][12] + F[7][10] * D[10][12] +
+			 F[7][11] * D[11][12] + F[7][12] * D[12][12] +
+			 F[7][6] * D[6][12] + F[7][8] * D[8][12]) +
+	     F[8][6] * (F[7][6] * D[6][6] + F[7][8] * D[6][8] +
+			F[7][9] * D[6][9] + F[7][10] * D[6][10] +
+			F[7][11] * D[6][11] + F[7][12] * D[6][12]) +
+	     F[8][7] * (F[7][6] * D[6][7] + F[7][8] * D[7][8] +
+			F[7][9] * D[7][9] + F[7][10] * D[7][10] +
+			F[7][11] * D[7][11] + F[7][12] * D[7][12]) +
+	     G[7][0] * G[8][0] * Q[0] + G[7][1] * G[8][1] * Q[1] +
+	     G[7][2] * G[8][2] * Q[2]) * Tsq + (F[7][6] * D[6][8] +
+						F[8][6] * D[6][7] +
+						F[8][7] * D[7][7] +
+						F[7][8] * D[8][8] +
+						F[7][9] * D[8][9] +
+						F[8][9] * D[7][9] +
+						F[7][10] * D[8][10] +
+						F[8][10] * D[7][10] +
+						F[7][11] * D[8][11] +
+						F[8][11] * D[7][11] +
+						F[7][12] * D[8][12] +
+						F[8][12] * D[7][12]) * T +
+	    D[7][8];
+	P[7][9] = P[9][7] =
+	    (F[9][10] *
+	     (F[7][9] * D[9][10] + F[7][10] * D[10][10] +
+	      F[7][11] * D[10][11] + F[7][12] * D[10][12] +
+	      F[7][6] * D[6][10] + F[7][8] * D[8][10]) +
+	     F[9][11] * (F[7][9] * D[9][11] + F[7][10] * D[10][11] +
+			 F[7][11] * D[11][11] + F[7][12] * D[11][12] +
+			 F[7][6] * D[6][11] + F[7][8] * D[8][11]) +
+	     F[9][12] * (F[7][9] * D[9][12] + F[7][10] * D[10][12] +
+			 F[7][11] * D[11][12] + F[7][12] * D[12][12] +
+			 F[7][6] * D[6][12] + F[7][8] * D[8][12]) +
+	     F[9][6] * (F[7][6] * D[6][6] + F[7][8] * D[6][8] +
+			F[7][9] * D[6][9] + F[7][10] * D[6][10] +
+			F[7][11] * D[6][11] + F[7][12] * D[6][12]) +
+	     F[9][7] * (F[7][6] * D[6][7] + F[7][8] * D[7][8] +
+			F[7][9] * D[7][9] + F[7][10] * D[7][10] +
+			F[7][11] * D[7][11] + F[7][12] * D[7][12]) +
+	     F[9][8] * (F[7][6] * D[6][8] + F[7][8] * D[8][8] +
+			F[7][9] * D[8][9] + F[7][10] * D[8][10] +
+			F[7][11] * D[8][11] + F[7][12] * D[8][12]) +
+	     G[9][0] * G[7][0] * Q[0] + G[9][1] * G[7][1] * Q[1] +
+	     G[9][2] * G[7][2] * Q[2]) * Tsq + (F[9][6] * D[6][7] +
+						F[9][7] * D[7][7] +
+						F[9][8] * D[7][8] +
+						F[7][9] * D[9][9] +
+						F[9][10] * D[7][10] +
+						F[7][10] * D[9][10] +
+						F[9][11] * D[7][11] +
+						F[7][11] * D[9][11] +
+						F[9][12] * D[7][12] +
+						F[7][12] * D[9][12] +
+						F[7][6] * D[6][9] +
+						F[7][8] * D[8][9]) * T +
+	    D[7][9];
+	P[7][10] = P[10][7] =
+	    (F[7][9] * D[9][10] + F[7][10] * D[10][10] +
+	     F[7][11] * D[10][11] + F[7][12] * D[10][12] +
+	     F[7][6] * D[6][10] + F[7][8] * D[8][10]) * T + D[7][10];
+	P[7][11] = P[11][7] =
+	    (F[7][9] * D[9][11] + F[7][10] * D[10][11] +
+	     F[7][11] * D[11][11] + F[7][12] * D[11][12] +
+	     F[7][6] * D[6][11] + F[7][8] * D[8][11]) * T + D[7][11];
+	P[7][12] = P[12][7] =
+	    (F[7][9] * D[9][12] + F[7][10] * D[10][12] +
+	     F[7][11] * D[11][12] + F[7][12] * D[12][12] +
+	     F[7][6] * D[6][12] + F[7][8] * D[8][12]) * T + D[7][12];
+	P[8][8] =
+	    (Q[0] * G[8][0] * G[8][0] + Q[1] * G[8][1] * G[8][1] +
+	     Q[2] * G[8][2] * G[8][2] + F[8][9] * (F[8][9] * D[9][9] +
+						   F[8][10] * D[9][10] +
+						   F[8][11] * D[9][11] +
+						   F[8][12] * D[9][12] +
+						   F[8][6] * D[6][9] +
+						   F[8][7] * D[7][9]) +
+	     F[8][10] * (F[8][9] * D[9][10] + F[8][10] * D[10][10] +
+			 F[8][11] * D[10][11] + F[8][12] * D[10][12] +
+			 F[8][6] * D[6][10] + F[8][7] * D[7][10]) +
+	     F[8][11] * (F[8][9] * D[9][11] + F[8][10] * D[10][11] +
+			 F[8][11] * D[11][11] + F[8][12] * D[11][12] +
+			 F[8][6] * D[6][11] + F[8][7] * D[7][11]) +
+	     F[8][12] * (F[8][9] * D[9][12] + F[8][10] * D[10][12] +
+			 F[8][11] * D[11][12] + F[8][12] * D[12][12] +
+			 F[8][6] * D[6][12] + F[8][7] * D[7][12]) +
+	     F[8][6] * (F[8][6] * D[6][6] + F[8][7] * D[6][7] +
+			F[8][9] * D[6][9] + F[8][10] * D[6][10] +
+			F[8][11] * D[6][11] + F[8][12] * D[6][12]) +
+	     F[8][7] * (F[8][6] * D[6][7] + F[8][7] * D[7][7] +
+			F[8][9] * D[7][9] + F[8][10] * D[7][10] +
+			F[8][11] * D[7][11] + F[8][12] * D[7][12])) * Tsq +
+	    (2 * F[8][6] * D[6][8] + 2 * F[8][7] * D[7][8] +
+	     2 * F[8][9] * D[8][9] + 2 * F[8][10] * D[8][10] +
+	     2 * F[8][11] * D[8][11] + 2 * F[8][12] * D[8][12]) * T +
+	    D[8][8];
+	P[8][9] = P[9][8] =
+	    (F[9][10] *
+	     (F[8][9] * D[9][10] + F[8][10] * D[10][10] +
+	      F[8][11] * D[10][11] + F[8][12] * D[10][12] +
+	      F[8][6] * D[6][10] + F[8][7] * D[7][10]) +
+	     F[9][11] * (F[8][9] * D[9][11] + F[8][10] * D[10][11] +
+			 F[8][11] * D[11][11] + F[8][12] * D[11][12] +
+			 F[8][6] * D[6][11] + F[8][7] * D[7][11]) +
+	     F[9][12] * (F[8][9] * D[9][12] + F[8][10] * D[10][12] +
+			 F[8][11] * D[11][12] + F[8][12] * D[12][12] +
+			 F[8][6] * D[6][12] + F[8][7] * D[7][12]) +
+	     F[9][6] * (F[8][6] * D[6][6] + F[8][7] * D[6][7] +
+			F[8][9] * D[6][9] + F[8][10] * D[6][10] +
+			F[8][11] * D[6][11] + F[8][12] * D[6][12]) +
+	     F[9][7] * (F[8][6] * D[6][7] + F[8][7] * D[7][7] +
+			F[8][9] * D[7][9] + F[8][10] * D[7][10] +
+			F[8][11] * D[7][11] + F[8][12] * D[7][12]) +
+	     F[9][8] * (F[8][6] * D[6][8] + F[8][7] * D[7][8] +
+			F[8][9] * D[8][9] + F[8][10] * D[8][10] +
+			F[8][11] * D[8][11] + F[8][12] * D[8][12]) +
+	     G[9][0] * G[8][0] * Q[0] + G[9][1] * G[8][1] * Q[1] +
+	     G[9][2] * G[8][2] * Q[2]) * Tsq + (F[9][6] * D[6][8] +
+						F[9][7] * D[7][8] +
+						F[9][8] * D[8][8] +
+						F[8][9] * D[9][9] +
+						F[9][10] * D[8][10] +
+						F[8][10] * D[9][10] +
+						F[9][11] * D[8][11] +
+						F[8][11] * D[9][11] +
+						F[9][12] * D[8][12] +
+						F[8][12] * D[9][12] +
+						F[8][6] * D[6][9] +
+						F[8][7] * D[7][9]) * T +
+	    D[8][9];
+	P[8][10] = P[10][8] =
+	    (F[8][9] * D[9][10] + F[8][10] * D[10][10] +
+	     F[8][11] * D[10][11] + F[8][12] * D[10][12] +
+	     F[8][6] * D[6][10] + F[8][7] * D[7][10]) * T + D[8][10];
+	P[8][11] = P[11][8] =
+	    (F[8][9] * D[9][11] + F[8][10] * D[10][11] +
+	     F[8][11] * D[11][11] + F[8][12] * D[11][12] +
+	     F[8][6] * D[6][11] + F[8][7] * D[7][11]) * T + D[8][11];
+	P[8][12] = P[12][8] =
+	    (F[8][9] * D[9][12] + F[8][10] * D[10][12] +
+	     F[8][11] * D[11][12] + F[8][12] * D[12][12] +
+	     F[8][6] * D[6][12] + F[8][7] * D[7][12]) * T + D[8][12];
+	P[9][9] =
+	    (Q[0] * G[9][0] * G[9][0] + Q[1] * G[9][1] * G[9][1] +
+	     Q[2] * G[9][2] * G[9][2] + F[9][10] * (F[9][10] * D[10][10] +
+						    F[9][11] * D[10][11] +
+						    F[9][12] * D[10][12] +
+						    F[9][6] * D[6][10] +
+						    F[9][7] * D[7][10] +
+						    F[9][8] * D[8][10]) +
+	     F[9][11] * (F[9][10] * D[10][11] + F[9][11] * D[11][11] +
+			 F[9][12] * D[11][12] + F[9][6] * D[6][11] +
+			 F[9][7] * D[7][11] + F[9][8] * D[8][11]) +
+	     F[9][12] * (F[9][10] * D[10][12] + F[9][11] * D[11][12] +
+			 F[9][12] * D[12][12] + F[9][6] * D[6][12] +
+			 F[9][7] * D[7][12] + F[9][8] * D[8][12]) +
+	     F[9][6] * (F[9][6] * D[6][6] + F[9][7] * D[6][7] +
+			F[9][8] * D[6][8] + F[9][10] * D[6][10] +
+			F[9][11] * D[6][11] + F[9][12] * D[6][12]) +
+	     F[9][7] * (F[9][6] * D[6][7] + F[9][7] * D[7][7] +
+			F[9][8] * D[7][8] + F[9][10] * D[7][10] +
+			F[9][11] * D[7][11] + F[9][12] * D[7][12]) +
+	     F[9][8] * (F[9][6] * D[6][8] + F[9][7] * D[7][8] +
+			F[9][8] * D[8][8] + F[9][10] * D[8][10] +
+			F[9][11] * D[8][11] + F[9][12] * D[8][12])) * Tsq +
+	    (2 * F[9][10] * D[9][10] + 2 * F[9][11] * D[9][11] +
+	     2 * F[9][12] * D[9][12] + 2 * F[9][6] * D[6][9] +
+	     2 * F[9][7] * D[7][9] + 2 * F[9][8] * D[8][9]) * T + D[9][9];
+	P[9][10] = P[10][9] =
+	    (F[9][10] * D[10][10] + F[9][11] * D[10][11] +
+	     F[9][12] * D[10][12] + F[9][6] * D[6][10] +
+	     F[9][7] * D[7][10] + F[9][8] * D[8][10]) * T + D[9][10];
+	P[9][11] = P[11][9] =
+	    (F[9][10] * D[10][11] + F[9][11] * D[11][11] +
+	     F[9][12] * D[11][12] + F[9][6] * D[6][11] +
+	     F[9][7] * D[7][11] + F[9][8] * D[8][11]) * T + D[9][11];
+	P[9][12] = P[12][9] =
+	    (F[9][10] * D[10][12] + F[9][11] * D[11][12] +
+	     F[9][12] * D[12][12] + F[9][6] * D[6][12] +
+	     F[9][7] * D[7][12] + F[9][8] * D[8][12]) * T + D[9][12];
+	P[10][10] = Q[6] * Tsq + D[10][10];
+	P[10][11] = P[11][10] = D[10][11];
+	P[10][12] = P[12][10] = D[10][12];
+	P[11][11] = Q[7] * Tsq + D[11][11];
+	P[11][12] = P[12][11] = D[11][12];
+	P[12][12] = Q[8] * Tsq + D[12][12];
+}
+#endif
+
+//  *************  SerialUpdate *******************
+//  Does the update step of the Kalman filter for the covariance and estimate
+//  Outputs are Xnew & Pnew, and are written over P and X
+//  Z is actual measurement, Y is predicted measurement
+//  Xnew = X + K*(Z-Y), Pnew=(I-K*H)*P,
+//    where K=P*H'*inv[H*P*H'+R]
+//  NOTE the algorithm assumes R (measurement covariance matrix) is diagonal
+//    i.e. the measurment noises are uncorrelated.
+//  It therefore uses a serial update that requires no matrix inversion by
+//    processing the measurements one at a time.
+//  Algorithm - see Grewal and Andrews, "Kalman Filtering,2nd Ed" p.121 & p.253
+//            - or see Simon, "Optimal State Estimation," 1st Ed, p.150
+//  The SensorsUsed variable is a bitwise mask indicating which sensors
+//     should be used in the update.
+//  ************************************************
+
+void SerialUpdate(float H[NUMV][NUMX], float R[NUMV], float Z[NUMV],
+		  float Y[NUMV], float P[NUMX][NUMX], float X[NUMX],
+		  uint16_t SensorsUsed)
+{
+	float HP[NUMX], HPHR, Error;
+	uint8_t i, j, k, m;
+
+	for (m = 0; m < NUMV; m++) {
+
+		if (SensorsUsed & (0x01 << m)) {	// use this sensor for update
+
+			for (j = 0; j < NUMX; j++) {	// Find Hp = H*P
+				HP[j] = 0;
+				for (k = 0; k < NUMX; k++)
+					HP[j] += H[m][k] * P[k][j];
+			}
+			HPHR = R[m];	// Find  HPHR = H*P*H' + R
+			for (k = 0; k < NUMX; k++)
+				HPHR += HP[k] * H[m][k];
+
+			for (k = 0; k < NUMX; k++)
+				K[k][m] = HP[k] / HPHR;	// find K = HP/HPHR
+
+			for (i = 0; i < NUMX; i++) {	// Find P(m)= P(m-1) + K*HP
+				for (j = i; j < NUMX; j++)
+					P[i][j] = P[j][i] =
+					    P[i][j] - K[i][m] * HP[j];
+			}
+
+			Error = Z[m] - Y[m];
+			for (i = 0; i < NUMX; i++)	// Find X(m)= X(m-1) + K*Error
+				X[i] = X[i] + K[i][m] * Error;
+
+		}
+	}
+}
+
+//  *************  RungeKutta **********************
+//  Does a 4th order Runge Kutta numerical integration step
+//  Output, Xnew, is written over X
+//  NOTE the algorithm assumes time invariant state equations and
+//    constant inputs over integration step
+//  ************************************************
+
+void RungeKutta(float X[NUMX], float U[NUMU], float dT)
+{
+
+	float dT2 =
+	    dT / 2, K1[NUMX], K2[NUMX], K3[NUMX], K4[NUMX], Xlast[NUMX];
+	uint8_t i;
+
+	for (i = 0; i < NUMX; i++)
+		Xlast[i] = X[i];	// make a working copy
+
+	StateEq(X, U, K1);	// k1 = f(x,u)
+	for (i = 0; i < NUMX; i++)
+		X[i] = Xlast[i] + dT2 * K1[i];
+	StateEq(X, U, K2);	// k2 = f(x+0.5*dT*k1,u)
+	for (i = 0; i < NUMX; i++)
+		X[i] = Xlast[i] + dT2 * K2[i];
+	StateEq(X, U, K3);	// k3 = f(x+0.5*dT*k2,u)
+	for (i = 0; i < NUMX; i++)
+		X[i] = Xlast[i] + dT * K3[i];
+	StateEq(X, U, K4);	// k4 = f(x+dT*k3,u)
+
+	// Xnew  = X + dT*(k1+2*k2+2*k3+k4)/6
+	for (i = 0; i < NUMX; i++)
+		X[i] =
+		    Xlast[i] + dT * (K1[i] + 2 * K2[i] + 2 * K3[i] +
+				     K4[i]) / 6;
+}
+
+//  *************  Model Specific Stuff  ***************************
+//  ***  StateEq, MeasurementEq, LinerizeFG, and LinearizeH ********
+//
+//  State Variables = [Pos Vel Quaternion GyroBias NO-AccelBias]
+//  Deterministic Inputs = [AngularVel Accel]
+//  Disturbance Noise = [GyroNoise AccelNoise GyroRandomWalkNoise NO-AccelRandomWalkNoise]
+//
+//  Measurement Variables = [Pos Vel BodyFrameMagField Altimeter]
+//  Inputs to Measurement = [EarthFrameMagField]
+//
+//  Notes: Pos and Vel in earth frame
+//  AngularVel and Accel in body frame
+//  MagFields are unit vectors
+//  Xdot is output of StateEq()
+//  F and G are outputs of LinearizeFG(), all elements not set should be zero
+//  y is output of OutputEq()
+//  H is output of LinearizeH(), all elements not set should be zero
+//  ************************************************
+
+void StateEq(float X[NUMX], float U[NUMU], float Xdot[NUMX])
+{
+	float ax, ay, az, wx, wy, wz, q0, q1, q2, q3;
+
+	// ax=U[3]-X[13]; ay=U[4]-X[14]; az=U[5]-X[15];  // subtract the biases on accels
+	ax = U[3];
+	ay = U[4];
+	az = U[5];		// NO BIAS STATES ON ACCELS
+	wx = U[0] - X[10];
+	wy = U[1] - X[11];
+	wz = U[2] - X[12];	// subtract the biases on gyros
+	q0 = X[6];
+	q1 = X[7];
+	q2 = X[8];
+	q3 = X[9];
+
+	// Pdot = V
+	Xdot[0] = X[3];
+	Xdot[1] = X[4];
+	Xdot[2] = X[5];
+
+	// Vdot = Reb*a
+	Xdot[3] =
+	    (q0 * q0 + q1 * q1 - q2 * q2 - q3 * q3) * ax + 2 * (q1 * q2 -
+								q0 * q3) *
+	    ay + 2 * (q1 * q3 + q0 * q2) * az;
+	Xdot[4] =
+	    2 * (q1 * q2 + q0 * q3) * ax + (q0 * q0 - q1 * q1 + q2 * q2 -
+					    q3 * q3) * ay + 2 * (q2 * q3 -
+								 q0 * q1) *
+	    az;
+	Xdot[5] =
+	    2 * (q1 * q3 - q0 * q2) * ax + 2 * (q2 * q3 + q0 * q1) * ay +
+	    (q0 * q0 - q1 * q1 - q2 * q2 + q3 * q3) * az + 9.81;
+
+	// qdot = Q*w
+	Xdot[6] = (-q1 * wx - q2 * wy - q3 * wz) / 2;
+	Xdot[7] = (q0 * wx - q3 * wy + q2 * wz) / 2;
+	Xdot[8] = (q3 * wx + q0 * wy - q1 * wz) / 2;
+	Xdot[9] = (-q2 * wx + q1 * wy + q0 * wz) / 2;
+
+	// best guess is that bias stays constant
+	Xdot[10] = Xdot[11] = Xdot[12] = 0;
+}
+
+void LinearizeFG(float X[NUMX], float U[NUMU], float F[NUMX][NUMX],
+		 float G[NUMX][NUMW])
+{
+	float ax, ay, az, wx, wy, wz, q0, q1, q2, q3;
+
+	// ax=U[3]-X[13]; ay=U[4]-X[14]; az=U[5]-X[15];  // subtract the biases on accels
+	ax = U[3];
+	ay = U[4];
+	az = U[5];		// NO BIAS STATES ON ACCELS
+	wx = U[0] - X[10];
+	wy = U[1] - X[11];
+	wz = U[2] - X[12];	// subtract the biases on gyros
+	q0 = X[6];
+	q1 = X[7];
+	q2 = X[8];
+	q3 = X[9];
+
+	// Pdot = V
+	F[0][3] = F[1][4] = F[2][5] = 1;
+
+	// dVdot/dq
+	F[3][6] = 2 * (q0 * ax - q3 * ay + q2 * az);
+	F[3][7] = 2 * (q1 * ax + q2 * ay + q3 * az);
+	F[3][8] = 2 * (-q2 * ax + q1 * ay + q0 * az);
+	F[3][9] = 2 * (-q3 * ax - q0 * ay + q1 * az);
+	F[4][6] = 2 * (q3 * ax + q0 * ay - q1 * az);
+	F[4][7] = 2 * (q2 * ax - q1 * ay - q0 * az);
+	F[4][8] = 2 * (q1 * ax + q2 * ay + q3 * az);
+	F[4][9] = 2 * (q0 * ax - q3 * ay + q2 * az);
+	F[5][6] = 2 * (-q2 * ax + q1 * ay + q0 * az);
+	F[5][7] = 2 * (q3 * ax + q0 * ay - q1 * az);
+	F[5][8] = 2 * (-q0 * ax + q3 * ay - q2 * az);
+	F[5][9] = 2 * (q1 * ax + q2 * ay + q3 * az);
+
+	// dVdot/dabias & dVdot/dna  - NO BIAS STATES ON ACCELS - S0 REPEAT FOR G BELOW
+	// F[3][13]=G[3][3]=-q0*q0-q1*q1+q2*q2+q3*q3; F[3][14]=G[3][4]=2*(-q1*q2+q0*q3);         F[3][15]=G[3][5]=-2*(q1*q3+q0*q2);
+	// F[4][13]=G[4][3]=-2*(q1*q2+q0*q3);         F[4][14]=G[4][4]=-q0*q0+q1*q1-q2*q2+q3*q3; F[4][15]=G[4][5]=2*(-q2*q3+q0*q1);
+	// F[5][13]=G[5][3]=2*(-q1*q3+q0*q2);         F[5][14]=G[5][4]=-2*(q2*q3+q0*q1);         F[5][15]=G[5][5]=-q0*q0+q1*q1+q2*q2-q3*q3;
+
+	// dqdot/dq
+	F[6][6] = 0;
+	F[6][7] = -wx / 2;
+	F[6][8] = -wy / 2;
+	F[6][9] = -wz / 2;
+	F[7][6] = wx / 2;
+	F[7][7] = 0;
+	F[7][8] = wz / 2;
+	F[7][9] = -wy / 2;
+	F[8][6] = wy / 2;
+	F[8][7] = -wz / 2;
+	F[8][8] = 0;
+	F[8][9] = wx / 2;
+	F[9][6] = wz / 2;
+	F[9][7] = wy / 2;
+	F[9][8] = -wx / 2;
+	F[9][9] = 0;
+
+	// dqdot/dwbias
+	F[6][10] = q1 / 2;
+	F[6][11] = q2 / 2;
+	F[6][12] = q3 / 2;
+	F[7][10] = -q0 / 2;
+	F[7][11] = q3 / 2;
+	F[7][12] = -q2 / 2;
+	F[8][10] = -q3 / 2;
+	F[8][11] = -q0 / 2;
+	F[8][12] = q1 / 2;
+	F[9][10] = q2 / 2;
+	F[9][11] = -q1 / 2;
+	F[9][12] = -q0 / 2;
+
+	// dVdot/dna  - NO BIAS STATES ON ACCELS - S0 REPEAT FOR G HERE
+	G[3][3] = -q0 * q0 - q1 * q1 + q2 * q2 + q3 * q3;
+	G[3][4] = 2 * (-q1 * q2 + q0 * q3);
+	G[3][5] = -2 * (q1 * q3 + q0 * q2);
+	G[4][3] = -2 * (q1 * q2 + q0 * q3);
+	G[4][4] = -q0 * q0 + q1 * q1 - q2 * q2 + q3 * q3;
+	G[4][5] = 2 * (-q2 * q3 + q0 * q1);
+	G[5][3] = 2 * (-q1 * q3 + q0 * q2);
+	G[5][4] = -2 * (q2 * q3 + q0 * q1);
+	G[5][5] = -q0 * q0 + q1 * q1 + q2 * q2 - q3 * q3;
+
+	// dqdot/dnw
+	G[6][0] = q1 / 2;
+	G[6][1] = q2 / 2;
+	G[6][2] = q3 / 2;
+	G[7][0] = -q0 / 2;
+	G[7][1] = q3 / 2;
+	G[7][2] = -q2 / 2;
+	G[8][0] = -q3 / 2;
+	G[8][1] = -q0 / 2;
+	G[8][2] = q1 / 2;
+	G[9][0] = q2 / 2;
+	G[9][1] = -q1 / 2;
+	G[9][2] = -q0 / 2;
+
+	// dwbias = random walk noise
+	G[10][6] = G[11][7] = G[12][8] = 1;
+	// dabias = random walk noise
+	// G[13][9]=G[14][10]=G[15][11]=1;  // NO BIAS STATES ON ACCELS
+}
+
+void MeasurementEq(float X[NUMX], float Be[3], float Y[NUMV])
+{
+	float q0, q1, q2, q3;
+
+	q0 = X[6];
+	q1 = X[7];
+	q2 = X[8];
+	q3 = X[9];
+
+	// first six outputs are P and V
+	Y[0] = X[0];
+	Y[1] = X[1];
+	Y[2] = X[2];
+	Y[3] = X[3];
+	Y[4] = X[4];
+	Y[5] = X[5];
+
+	// Bb=Rbe*Be
+	Y[6] =
+	    (q0 * q0 + q1 * q1 - q2 * q2 - q3 * q3) * Be[0] +
+	    2 * (q1 * q2 + q0 * q3) * Be[1] + 2 * (q1 * q3 -
+						   q0 * q2) * Be[2];
+	Y[7] =
+	    2 * (q1 * q2 - q0 * q3) * Be[0] + (q0 * q0 - q1 * q1 +
+					       q2 * q2 - q3 * q3) * Be[1] +
+	    2 * (q2 * q3 + q0 * q1) * Be[2];
+	Y[8] =
+	    2 * (q1 * q3 + q0 * q2) * Be[0] + 2 * (q2 * q3 -
+						   q0 * q1) * Be[1] +
+	    (q0 * q0 - q1 * q1 - q2 * q2 + q3 * q3) * Be[2];
+
+	// Alt = -Pz
+	Y[9] = -X[2];
+}
+
+void LinearizeH(float X[NUMX], float Be[3], float H[NUMV][NUMX])
+{
+	float q0, q1, q2, q3;
+
+	q0 = X[6];
+	q1 = X[7];
+	q2 = X[8];
+	q3 = X[9];
+
+	// dP/dP=I;
+	H[0][0] = H[1][1] = H[2][2] = 1;
+	// dV/dV=I;
+	H[3][3] = H[4][4] = H[5][5] = 1;
+
+	// dBb/dq
+	H[6][6] = 2 * (q0 * Be[0] + q3 * Be[1] - q2 * Be[2]);
+	H[6][7] = 2 * (q1 * Be[0] + q2 * Be[1] + q3 * Be[2]);
+	H[6][8] = 2 * (-q2 * Be[0] + q1 * Be[1] - q0 * Be[2]);
+	H[6][9] = 2 * (-q3 * Be[0] + q0 * Be[1] + q1 * Be[2]);
+	H[7][6] = 2 * (-q3 * Be[0] + q0 * Be[1] + q1 * Be[2]);
+	H[7][7] = 2 * (q2 * Be[0] - q1 * Be[1] + q0 * Be[2]);
+	H[7][8] = 2 * (q1 * Be[0] + q2 * Be[1] + q3 * Be[2]);
+	H[7][9] = 2 * (-q0 * Be[0] - q3 * Be[1] + q2 * Be[2]);
+	H[8][6] = 2 * (q2 * Be[0] - q1 * Be[1] + q0 * Be[2]);
+	H[8][7] = 2 * (q3 * Be[0] - q0 * Be[1] - q1 * Be[2]);
+	H[8][8] = 2 * (q0 * Be[0] + q3 * Be[1] - q2 * Be[2]);
+	H[8][9] = 2 * (q1 * Be[0] + q2 * Be[1] + q3 * Be[2]);
+
+	// dAlt/dPz = -1
+	H[9][2] = -1;
+}
+
+/**
+ * @}
+ * @}
+ */
diff --git a/flight/Revolution/insgps16state.c b/flight/Revolution/insgps16state.c
new file mode 100755
index 000000000..d1384b213
--- /dev/null
+++ b/flight/Revolution/insgps16state.c
@@ -0,0 +1,866 @@
+/**
+ ******************************************************************************
+ * @addtogroup AHRS 
+ * @{
+ * @addtogroup INSGPS
+ * @{
+ * @brief INSGPS is a joint attitude and position estimation EKF
+ *
+ * @file       insgps.c
+ * @author     The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010.
+ * @brief      An INS/GPS algorithm implemented with an EKF.
+ *
+ * @see        The GNU Public License (GPL) Version 3
+ *
+ *****************************************************************************/
+/*
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 3 of the License, or
+ * (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful, but
+ * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY
+ * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License
+ * for more details.
+ *
+ * You should have received a copy of the GNU General Public License along
+ * with this program; if not, write to the Free Software Foundation, Inc.,
+ * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
+ */
+
+#include "insgps.h"
+#include <math.h>
+#include <stdint.h>
+
+// constants/macros/typdefs
+#define NUMX 16			// number of states, X is the state vector
+#define NUMW 12			// number of plant noise inputs, w is disturbance noise vector
+#define NUMV 10			// number of measurements, v is the measurement noise vector
+#define NUMU 6			// number of deterministic inputs, U is the input vector
+
+#if defined(GENERAL_COV)
+// This might trick people so I have a note here.  There is a slower but bigger version of the 
+// code here but won't fit when debugging disabled (requires -Os)
+#define COVARIANCE_PREDICTION_GENERAL
+#endif
+
+// Private functions
+void CovariancePrediction(float F[NUMX][NUMX], float G[NUMX][NUMW],
+			  float Q[NUMW], float dT, float P[NUMX][NUMX]);
+void SerialUpdate(float H[NUMV][NUMX], float R[NUMV], float Z[NUMV],
+		  float Y[NUMV], float P[NUMX][NUMX], float X[NUMX],
+		  uint16_t SensorsUsed);
+void RungeKutta(float X[NUMX], float U[NUMU], float dT);
+void StateEq(float X[NUMX], float U[NUMU], float Xdot[NUMX]);
+void LinearizeFG(float X[NUMX], float U[NUMU], float F[NUMX][NUMX],
+		 float G[NUMX][NUMW]);
+void MeasurementEq(float X[NUMX], float Be[3], float Y[NUMV]);
+void LinearizeH(float X[NUMX], float Be[3], float H[NUMV][NUMX]);
+
+// Private variables
+float F[NUMX][NUMX], G[NUMX][NUMW], H[NUMV][NUMX];	// linearized system matrices
+													// global to init to zero and maintain zero elements
+float Be[3];			// local magnetic unit vector in NED frame
+float P[NUMX][NUMX], X[NUMX];	// covariance matrix and state vector
+float Q[NUMW], R[NUMV];		// input noise and measurement noise variances
+float K[NUMX][NUMV];		// feedback gain matrix
+
+//  *************  Exposed Functions ****************
+//  *************************************************
+
+uint16_t ins_get_num_states() 
+{
+	return NUMX;
+}
+
+void INSGPSInit()		//pretty much just a place holder for now
+{
+	Be[0] = 1;
+	Be[1] = 0;
+	Be[2] = 0;		// local magnetic unit vector
+
+	for (int i = 0; i < NUMX; i++) {
+		for (int j = 0; j < NUMX; j++) {
+			P[i][j] = 0; // zero all terms
+		}
+	}
+	
+	P[0][0] = P[1][1] = P[2][2] = 25;	// initial position variance (m^2)
+	P[3][3] = P[4][4] = P[5][5] = 5;	// initial velocity variance (m/s)^2
+	P[6][6] = P[7][7] = P[8][8] = P[9][9] = 1e-5;	// initial quaternion variance
+	P[10][10] = P[11][11] = P[12][12] = 1e-5;	// initial gyro bias variance (rad/s)^2
+
+	X[0] = X[1] = X[2] = X[3] = X[4] = X[5] = 0;	// initial pos and vel (m)
+	X[6] = 1;
+	X[7] = X[8] = X[9] = 0;	// initial quaternion (level and North) (m/s)
+	X[10] = X[11] = X[12] = 0;	// initial gyro bias (rad/s)
+
+	Q[0] = Q[1] = Q[2] = 50e-8;	// gyro noise variance (rad/s)^2
+	Q[3] = Q[4] = Q[5] = 0.01;	// accelerometer noise variance (m/s^2)^2
+	Q[6] = Q[7] = Q[8] = 2e-9;	// gyro bias random walk variance (rad/s^2)^2
+	Q[9] = Q[10] = Q[11] = 2e-20;	// accel bias random walk variance (m/s^3)^2
+
+	R[0] = R[1] = 0.004;	// High freq GPS horizontal position noise variance (m^2)
+	R[2] = 0.036;		// High freq GPS vertical position noise variance (m^2)
+	R[3] = R[4] = 0.004;	// High freq GPS horizontal velocity noise variance (m/s)^2
+	R[5] = 100;		// High freq GPS vertical velocity noise variance (m/s)^2
+	R[6] = R[7] = R[8] = 0.005;	// magnetometer unit vector noise variance
+	R[9] = .05;		// High freq altimeter noise variance (m^2)
+}
+
+void INSResetP(float PDiag[NUMX])
+{
+	uint8_t i,j;
+
+	// if PDiag[i] nonzero then clear row and column and set diagonal element
+	for (i=0;i<NUMX;i++){
+		if (PDiag != 0){
+			for (j=0;j<NUMX;j++)
+				P[i][j]=P[j][i]=0;
+			P[i][i]=PDiag[i];
+		}
+	}
+}
+
+void INSSetState(float pos[3], float vel[3], float q[4], float gyro_bias[3], float accel_bias[3])
+{
+	Nav.Pos[0] = X[0] = pos[0];
+	Nav.Pos[1] = X[1] = pos[1];
+	Nav.Pos[2] = X[2] = pos[2];
+	Nav.Vel[0] = X[3] = vel[0];
+	Nav.Vel[1] = X[4] = vel[1];
+	Nav.Vel[2] = X[5] = vel[2];
+	Nav.q[0] = X[6] = q[0];
+	Nav.q[1] = X[7] = q[1];
+	Nav.q[2] = X[8] = q[2];
+	Nav.q[3] = X[9] = q[3];
+	Nav.gyro_bias[0] = X[10] = gyro_bias[0];
+	Nav.gyro_bias[1] = X[11] = gyro_bias[1];
+	Nav.gyro_bias[2] = X[12] = gyro_bias[2];
+	Nav.accel_bias[0] = X[13] = accel_bias[0];
+	Nav.accel_bias[1] = X[14] = accel_bias[1];
+	Nav.accel_bias[2] = X[15] = accel_bias[2];
+}
+
+void INSPosVelReset(float pos[3], float vel[3]) 
+{
+	for (int i = 0; i < 6; i++) {
+		for(int j = i; j < NUMX; j++) {
+			P[i][j] = 0;  // zero the first 6 rows and columns
+			P[j][i] = 0; 
+		}
+	}
+	
+	P[0][0] = P[1][1] = P[2][2] = 25;	// initial position variance (m^2)
+	P[3][3] = P[4][4] = P[5][5] = 5;	// initial velocity variance (m/s)^2
+	
+	X[0] = pos[0];
+	X[1] = pos[1];
+	X[2] = pos[2];
+	X[3] = vel[0];
+	X[4] = vel[1];
+	X[5] = vel[2];	
+}
+
+void INSSetPosVelVar(float PosVar, float VelVar)
+{
+	R[0] = PosVar;
+	R[1] = PosVar;
+	R[2] = PosVar;
+	R[3] = VelVar;
+	R[4] = VelVar;
+//    R[5] = PosVar;  // Don't change vertical velocity, not measured
+}
+
+void INSSetGyroBias(float gyro_bias[3])
+{
+	X[10] = gyro_bias[0];
+	X[11] = gyro_bias[1];
+	X[12] = gyro_bias[2];
+}
+
+void INSSetAccelVar(float accel_var[3])
+{
+	Q[3] = accel_var[0];
+	Q[4] = accel_var[1];
+	Q[5] = accel_var[2];
+}
+
+void INSSetGyroVar(float gyro_var[3])
+{
+	Q[0] = gyro_var[0];
+	Q[1] = gyro_var[1];
+	Q[2] = gyro_var[2];
+}
+
+void INSSetMagVar(float scaled_mag_var[3])
+{
+	R[6] = scaled_mag_var[0];
+	R[7] = scaled_mag_var[1];
+	R[8] = scaled_mag_var[2];
+}
+
+void INSSetMagNorth(float B[3])
+{
+	Be[0] = B[0];
+	Be[1] = B[1];
+	Be[2] = B[2];
+}
+
+void INSStatePrediction(float gyro_data[3], float accel_data[3], float dT)
+{
+	float U[6];
+	float qmag;
+
+	// rate gyro inputs in units of rad/s
+	U[0] = gyro_data[0];
+	U[1] = gyro_data[1];
+	U[2] = gyro_data[2];
+
+	// accelerometer inputs in units of m/s
+	U[3] = accel_data[0];
+	U[4] = accel_data[1];
+	U[5] = accel_data[2];
+
+	// EKF prediction step
+	LinearizeFG(X, U, F, G);
+	RungeKutta(X, U, dT);
+	qmag = sqrt(X[6] * X[6] + X[7] * X[7] + X[8] * X[8] + X[9] * X[9]);
+	X[6] /= qmag;
+	X[7] /= qmag;
+	X[8] /= qmag;
+	X[9] /= qmag;
+	//CovariancePrediction(F,G,Q,dT,P);
+
+	// Update Nav solution structure
+	Nav.Pos[0] = X[0];
+	Nav.Pos[1] = X[1];
+	Nav.Pos[2] = X[2];
+	Nav.Vel[0] = X[3];
+	Nav.Vel[1] = X[4];
+	Nav.Vel[2] = X[5];
+	Nav.q[0] = X[6];
+	Nav.q[1] = X[7];
+	Nav.q[2] = X[8];
+	Nav.q[3] = X[9];
+	Nav.gyro_bias[0] = X[10];
+	Nav.gyro_bias[1] = X[11];
+	Nav.gyro_bias[2] = X[12];	
+}
+
+void INSCovariancePrediction(float dT)
+{
+	CovariancePrediction(F, G, Q, dT, P);
+}
+
+float zeros[3] = { 0, 0, 0 };
+
+void MagCorrection(float mag_data[3])
+{
+	INSCorrection(mag_data, zeros, zeros, zeros[0], MAG_SENSORS);
+}
+
+void MagVelBaroCorrection(float mag_data[3], float Vel[3], float BaroAlt)
+{
+	INSCorrection(mag_data, zeros, Vel, BaroAlt,
+		      MAG_SENSORS | HORIZ_SENSORS | VERT_SENSORS |
+		      BARO_SENSOR);
+}
+
+void GpsBaroCorrection(float Pos[3], float Vel[3], float BaroAlt)
+{
+	INSCorrection(zeros, Pos, Vel, BaroAlt,
+		      HORIZ_SENSORS | VERT_SENSORS | BARO_SENSOR);
+}
+
+void FullCorrection(float mag_data[3], float Pos[3], float Vel[3],
+		    float BaroAlt)
+{
+	INSCorrection(mag_data, Pos, Vel, BaroAlt, FULL_SENSORS);
+}
+
+void GpsMagCorrection(float mag_data[3], float Pos[3], float Vel[3])
+{
+	INSCorrection(mag_data, Pos, Vel, zeros[0],
+		      POS_SENSORS | HORIZ_SENSORS | MAG_SENSORS);
+}
+
+void VelBaroCorrection(float Vel[3], float BaroAlt)
+{
+	INSCorrection(zeros, zeros, Vel, BaroAlt,
+		      HORIZ_SENSORS | VERT_SENSORS | BARO_SENSOR);
+}
+
+void INSCorrection(float mag_data[3], float Pos[3], float Vel[3],
+		   float BaroAlt, uint16_t SensorsUsed)
+{
+	float Z[10], Y[10];
+	float Bmag, qmag;
+
+	// GPS Position in meters and in local NED frame
+	Z[0] = Pos[0];
+	Z[1] = Pos[1];
+	Z[2] = Pos[2];
+
+	// GPS Velocity in meters and in local NED frame
+	Z[3] = Vel[0];
+	Z[4] = Vel[1];
+	Z[5] = Vel[2];
+
+	// magnetometer data in any units (use unit vector) and in body frame
+	Bmag =
+	    sqrt(mag_data[0] * mag_data[0] + mag_data[1] * mag_data[1] +
+		 mag_data[2] * mag_data[2]);
+	Z[6] = mag_data[0] / Bmag;
+	Z[7] = mag_data[1] / Bmag;
+	Z[8] = mag_data[2] / Bmag;
+
+	// barometric altimeter in meters and in local NED frame
+	Z[9] = BaroAlt;
+
+	// EKF correction step
+	LinearizeH(X, Be, H);
+	MeasurementEq(X, Be, Y);
+	SerialUpdate(H, R, Z, Y, P, X, SensorsUsed);
+	qmag = sqrt(X[6] * X[6] + X[7] * X[7] + X[8] * X[8] + X[9] * X[9]);
+	X[6] /= qmag;
+	X[7] /= qmag;
+	X[8] /= qmag;
+	X[9] /= qmag;
+
+	// Update Nav solution structure
+	Nav.Pos[0] = X[0];
+	Nav.Pos[1] = X[1];
+	Nav.Pos[2] = X[2];
+	Nav.Vel[0] = X[3];
+	Nav.Vel[1] = X[4];
+	Nav.Vel[2] = X[5];
+	Nav.q[0] = X[6];
+	Nav.q[1] = X[7];
+	Nav.q[2] = X[8];
+	Nav.q[3] = X[9];
+	Nav.gyro_bias[0] = X[10];
+	Nav.gyro_bias[1] = X[11];
+	Nav.gyro_bias[2] = X[12];	
+	Nav.accel_bias[0] = X[13];
+	Nav.accel_bias[1] = X[14];
+	Nav.accel_bias[2] = X[15];
+}
+
+//  *************  CovariancePrediction *************
+//  Does the prediction step of the Kalman filter for the covariance matrix
+//  Output, Pnew, overwrites P, the input covariance
+//  Pnew = (I+F*T)*P*(I+F*T)' + T^2*G*Q*G'
+//  Q is the discrete time covariance of process noise
+//  Q is vector of the diagonal for a square matrix with
+//    dimensions equal to the number of disturbance noise variables
+//  The General Method is very inefficient,not taking advantage of the sparse F and G
+//  The first Method is very specific to this implementation
+//  ************************************************
+
+#ifdef COVARIANCE_PREDICTION_GENERAL
+
+void CovariancePrediction(float F[NUMX][NUMX], float G[NUMX][NUMW],
+			  float Q[NUMW], float dT, float P[NUMX][NUMX])
+{
+	float Dummy[NUMX][NUMX], dTsq;
+	uint8_t i, j, k;
+
+	//  Pnew = (I+F*T)*P*(I+F*T)' + T^2*G*Q*G' = T^2[(P/T + F*P)*(I/T + F') + G*Q*G')]
+
+	dTsq = dT * dT;
+
+	for (i = 0; i < NUMX; i++)	// Calculate Dummy = (P/T +F*P)
+		for (j = 0; j < NUMX; j++) {
+			Dummy[i][j] = P[i][j] / dT;
+			for (k = 0; k < NUMX; k++)
+				Dummy[i][j] += F[i][k] * P[k][j];
+		}
+	for (i = 0; i < NUMX; i++)	// Calculate Pnew = Dummy/T + Dummy*F' + G*Qw*G'
+		for (j = i; j < NUMX; j++) {	// Use symmetry, ie only find upper triangular
+			P[i][j] = Dummy[i][j] / dT;
+			for (k = 0; k < NUMX; k++)
+				P[i][j] += Dummy[i][k] * F[j][k];	// P = Dummy/T + Dummy*F'
+			for (k = 0; k < NUMW; k++)
+				P[i][j] += Q[k] * G[i][k] * G[j][k];	// P = Dummy/T + Dummy*F' + G*Q*G'
+			P[j][i] = P[i][j] = P[i][j] * dTsq;	// Pnew = T^2*P and fill in lower triangular;
+		}
+}
+
+#else
+
+void CovariancePrediction(float F[NUMX][NUMX], float G[NUMX][NUMW],
+			  float Q[NUMW], float dT, float P[NUMX][NUMX])
+{
+	float D[NUMX][NUMX], T, Tsq;
+	uint8_t i, j;
+
+	//  Pnew = (I+F*T)*P*(I+F*T)' + T^2*G*Q*G' = scalar expansion from symbolic manipulator
+
+	T = dT;
+	Tsq = dT * dT;
+
+	for (i = 0; i < NUMX; i++)	// Create a copy of the upper triangular of P
+		for (j = i; j < NUMX; j++)
+			D[i][j] = P[i][j];
+
+	// Brute force calculation of the elements of P
+	P[0][0] = D[3][3]*Tsq + (2*D[0][3])*T + D[0][0];
+	P[0][1] = P[1][0] = D[3][4]*Tsq + (D[0][4] + D[1][3])*T + D[0][1];
+	P[0][2] = P[2][0] = D[3][5]*Tsq + (D[0][5] + D[2][3])*T + D[0][2];
+	P[0][3] = P[3][0] = (F[3][6]*D[3][6] + F[3][7]*D[3][7] + F[3][8]*D[3][8] + F[3][9]*D[3][9] + F[3][13]*D[3][13] + F[3][14]*D[3][14] + F[3][15]*D[3][15])*Tsq + (D[3][3] + F[3][6]*D[0][6] + F[3][7]*D[0][7] + F[3][8]*D[0][8] + F[3][9]*D[0][9] + F[3][13]*D[0][13] + F[3][14]*D[0][14] + F[3][15]*D[0][15])*T + D[0][3];
+	P[0][4] = P[4][0] = (F[4][6]*D[3][6] + F[4][7]*D[3][7] + F[4][8]*D[3][8] + F[4][9]*D[3][9] + F[4][13]*D[3][13] + F[4][14]*D[3][14] + F[4][15]*D[3][15])*Tsq + (D[3][4] + F[4][6]*D[0][6] + F[4][7]*D[0][7] + F[4][8]*D[0][8] + F[4][9]*D[0][9] + F[4][13]*D[0][13] + F[4][14]*D[0][14] + F[4][15]*D[0][15])*T + D[0][4];
+	P[0][5] = P[5][0] = (F[5][6]*D[3][6] + F[5][7]*D[3][7] + F[5][8]*D[3][8] + F[5][9]*D[3][9] + F[5][13]*D[3][13] + F[5][14]*D[3][14] + F[5][15]*D[3][15])*Tsq + (D[3][5] + F[5][6]*D[0][6] + F[5][7]*D[0][7] + F[5][8]*D[0][8] + F[5][9]*D[0][9] + F[5][13]*D[0][13] + F[5][14]*D[0][14] + F[5][15]*D[0][15])*T + D[0][5];
+	P[0][6] = P[6][0] = (F[6][7]*D[3][7] + F[6][8]*D[3][8] + F[6][9]*D[3][9] + F[6][10]*D[3][10] + F[6][11]*D[3][11] + F[6][12]*D[3][12])*Tsq + (D[3][6] + F[6][7]*D[0][7] + F[6][8]*D[0][8] + F[6][9]*D[0][9] + F[6][10]*D[0][10] + F[6][11]*D[0][11] + F[6][12]*D[0][12])*T + D[0][6];
+	P[0][7] = P[7][0] = (F[7][6]*D[3][6] + F[7][8]*D[3][8] + F[7][9]*D[3][9] + F[7][10]*D[3][10] + F[7][11]*D[3][11] + F[7][12]*D[3][12])*Tsq + (D[3][7] + F[7][6]*D[0][6] + F[7][8]*D[0][8] + F[7][9]*D[0][9] + F[7][10]*D[0][10] + F[7][11]*D[0][11] + F[7][12]*D[0][12])*T + D[0][7];
+	P[0][8] = P[8][0] = (F[8][6]*D[3][6] + F[8][7]*D[3][7] + F[8][9]*D[3][9] + F[8][10]*D[3][10] + F[8][11]*D[3][11] + F[8][12]*D[3][12])*Tsq + (D[3][8] + F[8][6]*D[0][6] + F[8][7]*D[0][7] + F[8][9]*D[0][9] + F[8][10]*D[0][10] + F[8][11]*D[0][11] + F[8][12]*D[0][12])*T + D[0][8];
+	P[0][9] = P[9][0] = (F[9][6]*D[3][6] + F[9][7]*D[3][7] + F[9][8]*D[3][8] + F[9][10]*D[3][10] + F[9][11]*D[3][11] + F[9][12]*D[3][12])*Tsq + (D[3][9] + F[9][6]*D[0][6] + F[9][7]*D[0][7] + F[9][8]*D[0][8] + F[9][10]*D[0][10] + F[9][11]*D[0][11] + F[9][12]*D[0][12])*T + D[0][9];
+	P[0][10] = P[10][0] = D[3][10]*T + D[0][10];
+	P[0][11] = P[11][0] = D[3][11]*T + D[0][11];
+	P[0][12] = P[12][0] = D[3][12]*T + D[0][12];
+	P[0][13] = P[13][0] = D[3][13]*T + D[0][13];
+	P[0][14] = P[14][0] = D[3][14]*T + D[0][14];
+	P[0][15] = P[15][0] = D[3][15]*T + D[0][15];
+	P[1][1] = D[4][4]*Tsq + (2*D[1][4])*T + D[1][1];
+	P[1][2] = P[2][1] = D[4][5]*Tsq + (D[1][5] + D[2][4])*T + D[1][2];
+	P[1][3] = P[3][1] = (F[3][6]*D[4][6] + F[3][7]*D[4][7] + F[3][8]*D[4][8] + F[3][9]*D[4][9] + F[3][13]*D[4][13] + F[3][14]*D[4][14] + F[3][15]*D[4][15])*Tsq + (D[3][4] + F[3][6]*D[1][6] + F[3][7]*D[1][7] + F[3][8]*D[1][8] + F[3][9]*D[1][9] + F[3][13]*D[1][13] + F[3][14]*D[1][14] + F[3][15]*D[1][15])*T + D[1][3];
+	P[1][4] = P[4][1] = (F[4][6]*D[4][6] + F[4][7]*D[4][7] + F[4][8]*D[4][8] + F[4][9]*D[4][9] + F[4][13]*D[4][13] + F[4][14]*D[4][14] + F[4][15]*D[4][15])*Tsq + (D[4][4] + F[4][6]*D[1][6] + F[4][7]*D[1][7] + F[4][8]*D[1][8] + F[4][9]*D[1][9] + F[4][13]*D[1][13] + F[4][14]*D[1][14] + F[4][15]*D[1][15])*T + D[1][4];
+	P[1][5] = P[5][1] = (F[5][6]*D[4][6] + F[5][7]*D[4][7] + F[5][8]*D[4][8] + F[5][9]*D[4][9] + F[5][13]*D[4][13] + F[5][14]*D[4][14] + F[5][15]*D[4][15])*Tsq + (D[4][5] + F[5][6]*D[1][6] + F[5][7]*D[1][7] + F[5][8]*D[1][8] + F[5][9]*D[1][9] + F[5][13]*D[1][13] + F[5][14]*D[1][14] + F[5][15]*D[1][15])*T + D[1][5];
+	P[1][6] = P[6][1] = (F[6][7]*D[4][7] + F[6][8]*D[4][8] + F[6][9]*D[4][9] + F[6][10]*D[4][10] + F[6][11]*D[4][11] + F[6][12]*D[4][12])*Tsq + (D[4][6] + F[6][7]*D[1][7] + F[6][8]*D[1][8] + F[6][9]*D[1][9] + F[6][10]*D[1][10] + F[6][11]*D[1][11] + F[6][12]*D[1][12])*T + D[1][6];
+	P[1][7] = P[7][1] = (F[7][6]*D[4][6] + F[7][8]*D[4][8] + F[7][9]*D[4][9] + F[7][10]*D[4][10] + F[7][11]*D[4][11] + F[7][12]*D[4][12])*Tsq + (D[4][7] + F[7][6]*D[1][6] + F[7][8]*D[1][8] + F[7][9]*D[1][9] + F[7][10]*D[1][10] + F[7][11]*D[1][11] + F[7][12]*D[1][12])*T + D[1][7];
+	P[1][8] = P[8][1] = (F[8][6]*D[4][6] + F[8][7]*D[4][7] + F[8][9]*D[4][9] + F[8][10]*D[4][10] + F[8][11]*D[4][11] + F[8][12]*D[4][12])*Tsq + (D[4][8] + F[8][6]*D[1][6] + F[8][7]*D[1][7] + F[8][9]*D[1][9] + F[8][10]*D[1][10] + F[8][11]*D[1][11] + F[8][12]*D[1][12])*T + D[1][8];
+	P[1][9] = P[9][1] = (F[9][6]*D[4][6] + F[9][7]*D[4][7] + F[9][8]*D[4][8] + F[9][10]*D[4][10] + F[9][11]*D[4][11] + F[9][12]*D[4][12])*Tsq + (D[4][9] + F[9][6]*D[1][6] + F[9][7]*D[1][7] + F[9][8]*D[1][8] + F[9][10]*D[1][10] + F[9][11]*D[1][11] + F[9][12]*D[1][12])*T + D[1][9];
+	P[1][10] = P[10][1] = D[4][10]*T + D[1][10];
+	P[1][11] = P[11][1] = D[4][11]*T + D[1][11];
+	P[1][12] = P[12][1] = D[4][12]*T + D[1][12];
+	P[1][13] = P[13][1] = D[4][13]*T + D[1][13];
+	P[1][14] = P[14][1] = D[4][14]*T + D[1][14];
+	P[1][15] = P[15][1] = D[4][15]*T + D[1][15];
+	P[2][2] = D[5][5]*Tsq + (2*D[2][5])*T + D[2][2];
+	P[2][3] = P[3][2] = (F[3][6]*D[5][6] + F[3][7]*D[5][7] + F[3][8]*D[5][8] + F[3][9]*D[5][9] + F[3][13]*D[5][13] + F[3][14]*D[5][14] + F[3][15]*D[5][15])*Tsq + (D[3][5] + F[3][6]*D[2][6] + F[3][7]*D[2][7] + F[3][8]*D[2][8] + F[3][9]*D[2][9] + F[3][13]*D[2][13] + F[3][14]*D[2][14] + F[3][15]*D[2][15])*T + D[2][3];
+	P[2][4] = P[4][2] = (F[4][6]*D[5][6] + F[4][7]*D[5][7] + F[4][8]*D[5][8] + F[4][9]*D[5][9] + F[4][13]*D[5][13] + F[4][14]*D[5][14] + F[4][15]*D[5][15])*Tsq + (D[4][5] + F[4][6]*D[2][6] + F[4][7]*D[2][7] + F[4][8]*D[2][8] + F[4][9]*D[2][9] + F[4][13]*D[2][13] + F[4][14]*D[2][14] + F[4][15]*D[2][15])*T + D[2][4];
+	P[2][5] = P[5][2] = (F[5][6]*D[5][6] + F[5][7]*D[5][7] + F[5][8]*D[5][8] + F[5][9]*D[5][9] + F[5][13]*D[5][13] + F[5][14]*D[5][14] + F[5][15]*D[5][15])*Tsq + (D[5][5] + F[5][6]*D[2][6] + F[5][7]*D[2][7] + F[5][8]*D[2][8] + F[5][9]*D[2][9] + F[5][13]*D[2][13] + F[5][14]*D[2][14] + F[5][15]*D[2][15])*T + D[2][5];
+	P[2][6] = P[6][2] = (F[6][7]*D[5][7] + F[6][8]*D[5][8] + F[6][9]*D[5][9] + F[6][10]*D[5][10] + F[6][11]*D[5][11] + F[6][12]*D[5][12])*Tsq + (D[5][6] + F[6][7]*D[2][7] + F[6][8]*D[2][8] + F[6][9]*D[2][9] + F[6][10]*D[2][10] + F[6][11]*D[2][11] + F[6][12]*D[2][12])*T + D[2][6];
+	P[2][7] = P[7][2] = (F[7][6]*D[5][6] + F[7][8]*D[5][8] + F[7][9]*D[5][9] + F[7][10]*D[5][10] + F[7][11]*D[5][11] + F[7][12]*D[5][12])*Tsq + (D[5][7] + F[7][6]*D[2][6] + F[7][8]*D[2][8] + F[7][9]*D[2][9] + F[7][10]*D[2][10] + F[7][11]*D[2][11] + F[7][12]*D[2][12])*T + D[2][7];
+	P[2][8] = P[8][2] = (F[8][6]*D[5][6] + F[8][7]*D[5][7] + F[8][9]*D[5][9] + F[8][10]*D[5][10] + F[8][11]*D[5][11] + F[8][12]*D[5][12])*Tsq + (D[5][8] + F[8][6]*D[2][6] + F[8][7]*D[2][7] + F[8][9]*D[2][9] + F[8][10]*D[2][10] + F[8][11]*D[2][11] + F[8][12]*D[2][12])*T + D[2][8];
+	P[2][9] = P[9][2] = (F[9][6]*D[5][6] + F[9][7]*D[5][7] + F[9][8]*D[5][8] + F[9][10]*D[5][10] + F[9][11]*D[5][11] + F[9][12]*D[5][12])*Tsq + (D[5][9] + F[9][6]*D[2][6] + F[9][7]*D[2][7] + F[9][8]*D[2][8] + F[9][10]*D[2][10] + F[9][11]*D[2][11] + F[9][12]*D[2][12])*T + D[2][9];
+	P[2][10] = P[10][2] = D[5][10]*T + D[2][10];
+	P[2][11] = P[11][2] = D[5][11]*T + D[2][11];
+	P[2][12] = P[12][2] = D[5][12]*T + D[2][12];
+	P[2][13] = P[13][2] = D[5][13]*T + D[2][13];
+	P[2][14] = P[14][2] = D[5][14]*T + D[2][14];
+	P[2][15] = P[15][2] = D[5][15]*T + D[2][15];
+	P[3][3] = (Q[3]*G[3][3]*G[3][3] + Q[4]*G[3][4]*G[3][4] + Q[5]*G[3][5]*G[3][5] + F[3][9]*(F[3][9]*D[9][9] + F[3][13]*D[9][13] + F[3][14]*D[9][14] + F[3][15]*D[9][15] + F[3][6]*D[6][9] + F[3][7]*D[7][9] + F[3][8]*D[8][9]) + F[3][13]*(F[3][9]*D[9][13] + F[3][13]*D[13][13] + F[3][14]*D[13][14] + F[3][15]*D[13][15] + F[3][6]*D[6][13] + F[3][7]*D[7][13] + F[3][8]*D[8][13]) + F[3][14]*(F[3][9]*D[9][14] + F[3][13]*D[13][14] + F[3][14]*D[14][14] + F[3][15]*D[14][15] + F[3][6]*D[6][14] + F[3][7]*D[7][14] + F[3][8]*D[8][14]) + F[3][15]*(F[3][9]*D[9][15] + F[3][13]*D[13][15] + F[3][14]*D[14][15] + F[3][15]*D[14][15] + F[3][6]*D[6][15] + F[3][7]*D[7][15] + F[3][8]*D[8][15]) + F[3][6]*(F[3][6]*D[6][6] + F[3][7]*D[6][7] + F[3][8]*D[6][8] + F[3][9]*D[6][9] + F[3][13]*D[6][13] + F[3][14]*D[6][14] + F[3][15]*D[6][15]) + F[3][7]*(F[3][6]*D[6][7] + F[3][7]*D[7][7] + F[3][8]*D[7][8] + F[3][9]*D[7][9] + F[3][13]*D[7][13] + F[3][14]*D[7][14] + F[3][15]*D[7][15]) + F[3][8]*(F[3][6]*D[6][8] + F[3][7]*D[7][8] + F[3][8]*D[8][8] + F[3][9]*D[8][9] + F[3][13]*D[8][13] + F[3][14]*D[8][14] + F[3][15]*D[8][15]))*Tsq + (2*F[3][6]*D[3][6] + 2*F[3][7]*D[3][7] + 2*F[3][8]*D[3][8] + 2*F[3][9]*D[3][9] + 2*F[3][13]*D[3][13] + 2*F[3][14]*D[3][14] + 2*F[3][15]*D[3][15])*T + D[3][3];
+	P[3][4] = P[4][3] = (F[4][9]*(F[3][9]*D[9][9] + F[3][13]*D[9][13] + F[3][14]*D[9][14] + F[3][15]*D[9][15] + F[3][6]*D[6][9] + F[3][7]*D[7][9] + F[3][8]*D[8][9]) + F[4][13]*(F[3][9]*D[9][13] + F[3][13]*D[13][13] + F[3][14]*D[13][14] + F[3][15]*D[13][15] + F[3][6]*D[6][13] + F[3][7]*D[7][13] + F[3][8]*D[8][13]) + F[4][14]*(F[3][9]*D[9][14] + F[3][13]*D[13][14] + F[3][14]*D[14][14] + F[3][15]*D[14][15] + F[3][6]*D[6][14] + F[3][7]*D[7][14] + F[3][8]*D[8][14]) + F[4][15]*(F[3][9]*D[9][15] + F[3][13]*D[13][15] + F[3][14]*D[14][15] + F[3][15]*D[14][15] + F[3][6]*D[6][15] + F[3][7]*D[7][15] + F[3][8]*D[8][15]) + F[4][6]*(F[3][6]*D[6][6] + F[3][7]*D[6][7] + F[3][8]*D[6][8] + F[3][9]*D[6][9] + F[3][13]*D[6][13] + F[3][14]*D[6][14] + F[3][15]*D[6][15]) + F[4][7]*(F[3][6]*D[6][7] + F[3][7]*D[7][7] + F[3][8]*D[7][8] + F[3][9]*D[7][9] + F[3][13]*D[7][13] + F[3][14]*D[7][14] + F[3][15]*D[7][15]) + F[4][8]*(F[3][6]*D[6][8] + F[3][7]*D[7][8] + F[3][8]*D[8][8] + F[3][9]*D[8][9] + F[3][13]*D[8][13] + F[3][14]*D[8][14] + F[3][15]*D[8][15]) + G[3][3]*G[4][3]*Q[3] + G[3][4]*G[4][4]*Q[4] + G[3][5]*G[4][5]*Q[5])*Tsq + (F[3][6]*D[4][6] + F[4][6]*D[3][6] + F[3][7]*D[4][7] + F[4][7]*D[3][7] + F[3][8]*D[4][8] + F[4][8]*D[3][8] + F[3][9]*D[4][9] + F[4][9]*D[3][9] + F[3][13]*D[4][13] + F[4][13]*D[3][13] + F[3][14]*D[4][14] + F[4][14]*D[3][14] + F[3][15]*D[4][15] + F[4][15]*D[3][15])*T + D[3][4];
+	P[3][5] = P[5][3] = (F[5][9]*(F[3][9]*D[9][9] + F[3][13]*D[9][13] + F[3][14]*D[9][14] + F[3][15]*D[9][15] + F[3][6]*D[6][9] + F[3][7]*D[7][9] + F[3][8]*D[8][9]) + F[5][13]*(F[3][9]*D[9][13] + F[3][13]*D[13][13] + F[3][14]*D[13][14] + F[3][15]*D[13][15] + F[3][6]*D[6][13] + F[3][7]*D[7][13] + F[3][8]*D[8][13]) + F[5][14]*(F[3][9]*D[9][14] + F[3][13]*D[13][14] + F[3][14]*D[14][14] + F[3][15]*D[14][15] + F[3][6]*D[6][14] + F[3][7]*D[7][14] + F[3][8]*D[8][14]) + F[5][15]*(F[3][9]*D[9][15] + F[3][13]*D[13][15] + F[3][14]*D[14][15] + F[3][15]*D[14][15] + F[3][6]*D[6][15] + F[3][7]*D[7][15] + F[3][8]*D[8][15]) + F[5][6]*(F[3][6]*D[6][6] + F[3][7]*D[6][7] + F[3][8]*D[6][8] + F[3][9]*D[6][9] + F[3][13]*D[6][13] + F[3][14]*D[6][14] + F[3][15]*D[6][15]) + F[5][7]*(F[3][6]*D[6][7] + F[3][7]*D[7][7] + F[3][8]*D[7][8] + F[3][9]*D[7][9] + F[3][13]*D[7][13] + F[3][14]*D[7][14] + F[3][15]*D[7][15]) + F[5][8]*(F[3][6]*D[6][8] + F[3][7]*D[7][8] + F[3][8]*D[8][8] + F[3][9]*D[8][9] + F[3][13]*D[8][13] + F[3][14]*D[8][14] + F[3][15]*D[8][15]) + G[3][3]*G[5][3]*Q[3] + G[3][4]*G[5][4]*Q[4] + G[3][5]*G[5][5]*Q[5])*Tsq + (F[3][6]*D[5][6] + F[5][6]*D[3][6] + F[3][7]*D[5][7] + F[5][7]*D[3][7] + F[3][8]*D[5][8] + F[5][8]*D[3][8] + F[3][9]*D[5][9] + F[5][9]*D[3][9] + F[3][13]*D[5][13] + F[5][13]*D[3][13] + F[3][14]*D[5][14] + F[5][14]*D[3][14] + F[3][15]*D[5][15] + F[5][15]*D[3][15])*T + D[3][5];
+	P[3][6] = P[6][3] = (F[6][9]*(F[3][9]*D[9][9] + F[3][13]*D[9][13] + F[3][14]*D[9][14] + F[3][15]*D[9][15] + F[3][6]*D[6][9] + F[3][7]*D[7][9] + F[3][8]*D[8][9]) + F[6][10]*(F[3][9]*D[9][10] + F[3][13]*D[10][13] + F[3][14]*D[10][14] + F[3][15]*D[10][15] + F[3][6]*D[6][10] + F[3][7]*D[7][10] + F[3][8]*D[8][10]) + F[6][11]*(F[3][9]*D[9][11] + F[3][13]*D[11][13] + F[3][14]*D[11][14] + F[3][15]*D[11][15] + F[3][6]*D[6][11] + F[3][7]*D[7][11] + F[3][8]*D[8][11]) + F[6][12]*(F[3][9]*D[9][12] + F[3][13]*D[12][13] + F[3][14]*D[12][14] + F[3][15]*D[12][15] + F[3][6]*D[6][12] + F[3][7]*D[7][12] + F[3][8]*D[8][12]) + F[6][7]*(F[3][6]*D[6][7] + F[3][7]*D[7][7] + F[3][8]*D[7][8] + F[3][9]*D[7][9] + F[3][13]*D[7][13] + F[3][14]*D[7][14] + F[3][15]*D[7][15]) + F[6][8]*(F[3][6]*D[6][8] + F[3][7]*D[7][8] + F[3][8]*D[8][8] + F[3][9]*D[8][9] + F[3][13]*D[8][13] + F[3][14]*D[8][14] + F[3][15]*D[8][15]))*Tsq + (F[3][6]*D[6][6] + F[3][7]*D[6][7] + F[6][7]*D[3][7] + F[3][8]*D[6][8] + F[6][8]*D[3][8] + F[3][9]*D[6][9] + F[6][9]*D[3][9] + F[6][10]*D[3][10] + F[6][11]*D[3][11] + F[6][12]*D[3][12] + F[3][13]*D[6][13] + F[3][14]*D[6][14] + F[3][15]*D[6][15])*T + D[3][6];
+	P[3][7] = P[7][3] = (F[7][9]*(F[3][9]*D[9][9] + F[3][13]*D[9][13] + F[3][14]*D[9][14] + F[3][15]*D[9][15] + F[3][6]*D[6][9] + F[3][7]*D[7][9] + F[3][8]*D[8][9]) + F[7][10]*(F[3][9]*D[9][10] + F[3][13]*D[10][13] + F[3][14]*D[10][14] + F[3][15]*D[10][15] + F[3][6]*D[6][10] + F[3][7]*D[7][10] + F[3][8]*D[8][10]) + F[7][11]*(F[3][9]*D[9][11] + F[3][13]*D[11][13] + F[3][14]*D[11][14] + F[3][15]*D[11][15] + F[3][6]*D[6][11] + F[3][7]*D[7][11] + F[3][8]*D[8][11]) + F[7][12]*(F[3][9]*D[9][12] + F[3][13]*D[12][13] + F[3][14]*D[12][14] + F[3][15]*D[12][15] + F[3][6]*D[6][12] + F[3][7]*D[7][12] + F[3][8]*D[8][12]) + F[7][6]*(F[3][6]*D[6][6] + F[3][7]*D[6][7] + F[3][8]*D[6][8] + F[3][9]*D[6][9] + F[3][13]*D[6][13] + F[3][14]*D[6][14] + F[3][15]*D[6][15]) + F[7][8]*(F[3][6]*D[6][8] + F[3][7]*D[7][8] + F[3][8]*D[8][8] + F[3][9]*D[8][9] + F[3][13]*D[8][13] + F[3][14]*D[8][14] + F[3][15]*D[8][15]))*Tsq + (F[3][6]*D[6][7] + F[7][6]*D[3][6] + F[3][7]*D[7][7] + F[3][8]*D[7][8] + F[7][8]*D[3][8] + F[3][9]*D[7][9] + F[7][9]*D[3][9] + F[7][10]*D[3][10] + F[7][11]*D[3][11] + F[7][12]*D[3][12] + F[3][13]*D[7][13] + F[3][14]*D[7][14] + F[3][15]*D[7][15])*T + D[3][7];
+	P[3][8] = P[8][3] = (F[8][9]*(F[3][9]*D[9][9] + F[3][13]*D[9][13] + F[3][14]*D[9][14] + F[3][15]*D[9][15] + F[3][6]*D[6][9] + F[3][7]*D[7][9] + F[3][8]*D[8][9]) + F[8][10]*(F[3][9]*D[9][10] + F[3][13]*D[10][13] + F[3][14]*D[10][14] + F[3][15]*D[10][15] + F[3][6]*D[6][10] + F[3][7]*D[7][10] + F[3][8]*D[8][10]) + F[8][11]*(F[3][9]*D[9][11] + F[3][13]*D[11][13] + F[3][14]*D[11][14] + F[3][15]*D[11][15] + F[3][6]*D[6][11] + F[3][7]*D[7][11] + F[3][8]*D[8][11]) + F[8][12]*(F[3][9]*D[9][12] + F[3][13]*D[12][13] + F[3][14]*D[12][14] + F[3][15]*D[12][15] + F[3][6]*D[6][12] + F[3][7]*D[7][12] + F[3][8]*D[8][12]) + F[8][6]*(F[3][6]*D[6][6] + F[3][7]*D[6][7] + F[3][8]*D[6][8] + F[3][9]*D[6][9] + F[3][13]*D[6][13] + F[3][14]*D[6][14] + F[3][15]*D[6][15]) + F[8][7]*(F[3][6]*D[6][7] + F[3][7]*D[7][7] + F[3][8]*D[7][8] + F[3][9]*D[7][9] + F[3][13]*D[7][13] + F[3][14]*D[7][14] + F[3][15]*D[7][15]))*Tsq + (F[3][6]*D[6][8] + F[3][7]*D[7][8] + F[8][6]*D[3][6] + F[8][7]*D[3][7] + F[3][8]*D[8][8] + F[3][9]*D[8][9] + F[8][9]*D[3][9] + F[8][10]*D[3][10] + F[8][11]*D[3][11] + F[8][12]*D[3][12] + F[3][13]*D[8][13] + F[3][14]*D[8][14] + F[3][15]*D[8][15])*T + D[3][8];
+	P[3][9] = P[9][3] = (F[9][10]*(F[3][9]*D[9][10] + F[3][13]*D[10][13] + F[3][14]*D[10][14] + F[3][15]*D[10][15] + F[3][6]*D[6][10] + F[3][7]*D[7][10] + F[3][8]*D[8][10]) + F[9][11]*(F[3][9]*D[9][11] + F[3][13]*D[11][13] + F[3][14]*D[11][14] + F[3][15]*D[11][15] + F[3][6]*D[6][11] + F[3][7]*D[7][11] + F[3][8]*D[8][11]) + F[9][12]*(F[3][9]*D[9][12] + F[3][13]*D[12][13] + F[3][14]*D[12][14] + F[3][15]*D[12][15] + F[3][6]*D[6][12] + F[3][7]*D[7][12] + F[3][8]*D[8][12]) + F[9][6]*(F[3][6]*D[6][6] + F[3][7]*D[6][7] + F[3][8]*D[6][8] + F[3][9]*D[6][9] + F[3][13]*D[6][13] + F[3][14]*D[6][14] + F[3][15]*D[6][15]) + F[9][7]*(F[3][6]*D[6][7] + F[3][7]*D[7][7] + F[3][8]*D[7][8] + F[3][9]*D[7][9] + F[3][13]*D[7][13] + F[3][14]*D[7][14] + F[3][15]*D[7][15]) + F[9][8]*(F[3][6]*D[6][8] + F[3][7]*D[7][8] + F[3][8]*D[8][8] + F[3][9]*D[8][9] + F[3][13]*D[8][13] + F[3][14]*D[8][14] + F[3][15]*D[8][15]))*Tsq + (F[9][6]*D[3][6] + F[9][7]*D[3][7] + F[9][8]*D[3][8] + F[3][9]*D[9][9] + F[9][10]*D[3][10] + F[9][11]*D[3][11] + F[9][12]*D[3][12] + F[3][13]*D[9][13] + F[3][14]*D[9][14] + F[3][15]*D[9][15] + F[3][6]*D[6][9] + F[3][7]*D[7][9] + F[3][8]*D[8][9])*T + D[3][9];
+	P[3][10] = P[10][3] = (F[3][9]*D[9][10] + F[3][13]*D[10][13] + F[3][14]*D[10][14] + F[3][15]*D[10][15] + F[3][6]*D[6][10] + F[3][7]*D[7][10] + F[3][8]*D[8][10])*T + D[3][10];
+	P[3][11] = P[11][3] = (F[3][9]*D[9][11] + F[3][13]*D[11][13] + F[3][14]*D[11][14] + F[3][15]*D[11][15] + F[3][6]*D[6][11] + F[3][7]*D[7][11] + F[3][8]*D[8][11])*T + D[3][11];
+	P[3][12] = P[12][3] = (F[3][9]*D[9][12] + F[3][13]*D[12][13] + F[3][14]*D[12][14] + F[3][15]*D[12][15] + F[3][6]*D[6][12] + F[3][7]*D[7][12] + F[3][8]*D[8][12])*T + D[3][12];
+	P[3][13] = P[13][3] = (F[3][9]*D[9][13] + F[3][13]*D[13][13] + F[3][14]*D[13][14] + F[3][15]*D[13][15] + F[3][6]*D[6][13] + F[3][7]*D[7][13] + F[3][8]*D[8][13])*T + D[3][13];
+	P[3][14] = P[14][3] = (F[3][9]*D[9][14] + F[3][13]*D[13][14] + F[3][14]*D[14][14] + F[3][15]*D[14][15] + F[3][6]*D[6][14] + F[3][7]*D[7][14] + F[3][8]*D[8][14])*T + D[3][14];
+	P[3][15] = P[15][3] = (F[3][9]*D[9][15] + F[3][13]*D[13][15] + F[3][14]*D[14][15] + F[3][15]*D[14][15] + F[3][6]*D[6][15] + F[3][7]*D[7][15] + F[3][8]*D[8][15])*T + D[3][15];
+	P[4][4] = (Q[3]*G[4][3]*G[4][3] + Q[4]*G[4][4]*G[4][4] + Q[5]*G[4][5]*G[4][5] + F[4][9]*(F[4][9]*D[9][9] + F[4][13]*D[9][13] + F[4][14]*D[9][14] + F[4][15]*D[9][15] + F[4][6]*D[6][9] + F[4][7]*D[7][9] + F[4][8]*D[8][9]) + F[4][13]*(F[4][9]*D[9][13] + F[4][13]*D[13][13] + F[4][14]*D[13][14] + F[4][15]*D[13][15] + F[4][6]*D[6][13] + F[4][7]*D[7][13] + F[4][8]*D[8][13]) + F[4][14]*(F[4][9]*D[9][14] + F[4][13]*D[13][14] + F[4][14]*D[14][14] + F[4][15]*D[14][15] + F[4][6]*D[6][14] + F[4][7]*D[7][14] + F[4][8]*D[8][14]) + F[4][15]*(F[4][9]*D[9][15] + F[4][13]*D[13][15] + F[4][14]*D[14][15] + F[4][15]*D[14][15] + F[4][6]*D[6][15] + F[4][7]*D[7][15] + F[4][8]*D[8][15]) + F[4][6]*(F[4][6]*D[6][6] + F[4][7]*D[6][7] + F[4][8]*D[6][8] + F[4][9]*D[6][9] + F[4][13]*D[6][13] + F[4][14]*D[6][14] + F[4][15]*D[6][15]) + F[4][7]*(F[4][6]*D[6][7] + F[4][7]*D[7][7] + F[4][8]*D[7][8] + F[4][9]*D[7][9] + F[4][13]*D[7][13] + F[4][14]*D[7][14] + F[4][15]*D[7][15]) + F[4][8]*(F[4][6]*D[6][8] + F[4][7]*D[7][8] + F[4][8]*D[8][8] + F[4][9]*D[8][9] + F[4][13]*D[8][13] + F[4][14]*D[8][14] + F[4][15]*D[8][15]))*Tsq + (2*F[4][6]*D[4][6] + 2*F[4][7]*D[4][7] + 2*F[4][8]*D[4][8] + 2*F[4][9]*D[4][9] + 2*F[4][13]*D[4][13] + 2*F[4][14]*D[4][14] + 2*F[4][15]*D[4][15])*T + D[4][4];
+	P[4][5] = P[5][4] = (F[5][9]*(F[4][9]*D[9][9] + F[4][13]*D[9][13] + F[4][14]*D[9][14] + F[4][15]*D[9][15] + F[4][6]*D[6][9] + F[4][7]*D[7][9] + F[4][8]*D[8][9]) + F[5][13]*(F[4][9]*D[9][13] + F[4][13]*D[13][13] + F[4][14]*D[13][14] + F[4][15]*D[13][15] + F[4][6]*D[6][13] + F[4][7]*D[7][13] + F[4][8]*D[8][13]) + F[5][14]*(F[4][9]*D[9][14] + F[4][13]*D[13][14] + F[4][14]*D[14][14] + F[4][15]*D[14][15] + F[4][6]*D[6][14] + F[4][7]*D[7][14] + F[4][8]*D[8][14]) + F[5][15]*(F[4][9]*D[9][15] + F[4][13]*D[13][15] + F[4][14]*D[14][15] + F[4][15]*D[14][15] + F[4][6]*D[6][15] + F[4][7]*D[7][15] + F[4][8]*D[8][15]) + F[5][6]*(F[4][6]*D[6][6] + F[4][7]*D[6][7] + F[4][8]*D[6][8] + F[4][9]*D[6][9] + F[4][13]*D[6][13] + F[4][14]*D[6][14] + F[4][15]*D[6][15]) + F[5][7]*(F[4][6]*D[6][7] + F[4][7]*D[7][7] + F[4][8]*D[7][8] + F[4][9]*D[7][9] + F[4][13]*D[7][13] + F[4][14]*D[7][14] + F[4][15]*D[7][15]) + F[5][8]*(F[4][6]*D[6][8] + F[4][7]*D[7][8] + F[4][8]*D[8][8] + F[4][9]*D[8][9] + F[4][13]*D[8][13] + F[4][14]*D[8][14] + F[4][15]*D[8][15]) + G[4][3]*G[5][3]*Q[3] + G[4][4]*G[5][4]*Q[4] + G[4][5]*G[5][5]*Q[5])*Tsq + (F[4][6]*D[5][6] + F[5][6]*D[4][6] + F[4][7]*D[5][7] + F[5][7]*D[4][7] + F[4][8]*D[5][8] + F[5][8]*D[4][8] + F[4][9]*D[5][9] + F[5][9]*D[4][9] + F[4][13]*D[5][13] + F[5][13]*D[4][13] + F[4][14]*D[5][14] + F[5][14]*D[4][14] + F[4][15]*D[5][15] + F[5][15]*D[4][15])*T + D[4][5];
+	P[4][6] = P[6][4] = (F[6][9]*(F[4][9]*D[9][9] + F[4][13]*D[9][13] + F[4][14]*D[9][14] + F[4][15]*D[9][15] + F[4][6]*D[6][9] + F[4][7]*D[7][9] + F[4][8]*D[8][9]) + F[6][10]*(F[4][9]*D[9][10] + F[4][13]*D[10][13] + F[4][14]*D[10][14] + F[4][15]*D[10][15] + F[4][6]*D[6][10] + F[4][7]*D[7][10] + F[4][8]*D[8][10]) + F[6][11]*(F[4][9]*D[9][11] + F[4][13]*D[11][13] + F[4][14]*D[11][14] + F[4][15]*D[11][15] + F[4][6]*D[6][11] + F[4][7]*D[7][11] + F[4][8]*D[8][11]) + F[6][12]*(F[4][9]*D[9][12] + F[4][13]*D[12][13] + F[4][14]*D[12][14] + F[4][15]*D[12][15] + F[4][6]*D[6][12] + F[4][7]*D[7][12] + F[4][8]*D[8][12]) + F[6][7]*(F[4][6]*D[6][7] + F[4][7]*D[7][7] + F[4][8]*D[7][8] + F[4][9]*D[7][9] + F[4][13]*D[7][13] + F[4][14]*D[7][14] + F[4][15]*D[7][15]) + F[6][8]*(F[4][6]*D[6][8] + F[4][7]*D[7][8] + F[4][8]*D[8][8] + F[4][9]*D[8][9] + F[4][13]*D[8][13] + F[4][14]*D[8][14] + F[4][15]*D[8][15]))*Tsq + (F[4][6]*D[6][6] + F[4][7]*D[6][7] + F[6][7]*D[4][7] + F[4][8]*D[6][8] + F[6][8]*D[4][8] + F[4][9]*D[6][9] + F[6][9]*D[4][9] + F[6][10]*D[4][10] + F[6][11]*D[4][11] + F[6][12]*D[4][12] + F[4][13]*D[6][13] + F[4][14]*D[6][14] + F[4][15]*D[6][15])*T + D[4][6];
+	P[4][7] = P[7][4] = (F[7][9]*(F[4][9]*D[9][9] + F[4][13]*D[9][13] + F[4][14]*D[9][14] + F[4][15]*D[9][15] + F[4][6]*D[6][9] + F[4][7]*D[7][9] + F[4][8]*D[8][9]) + F[7][10]*(F[4][9]*D[9][10] + F[4][13]*D[10][13] + F[4][14]*D[10][14] + F[4][15]*D[10][15] + F[4][6]*D[6][10] + F[4][7]*D[7][10] + F[4][8]*D[8][10]) + F[7][11]*(F[4][9]*D[9][11] + F[4][13]*D[11][13] + F[4][14]*D[11][14] + F[4][15]*D[11][15] + F[4][6]*D[6][11] + F[4][7]*D[7][11] + F[4][8]*D[8][11]) + F[7][12]*(F[4][9]*D[9][12] + F[4][13]*D[12][13] + F[4][14]*D[12][14] + F[4][15]*D[12][15] + F[4][6]*D[6][12] + F[4][7]*D[7][12] + F[4][8]*D[8][12]) + F[7][6]*(F[4][6]*D[6][6] + F[4][7]*D[6][7] + F[4][8]*D[6][8] + F[4][9]*D[6][9] + F[4][13]*D[6][13] + F[4][14]*D[6][14] + F[4][15]*D[6][15]) + F[7][8]*(F[4][6]*D[6][8] + F[4][7]*D[7][8] + F[4][8]*D[8][8] + F[4][9]*D[8][9] + F[4][13]*D[8][13] + F[4][14]*D[8][14] + F[4][15]*D[8][15]))*Tsq + (F[4][6]*D[6][7] + F[7][6]*D[4][6] + F[4][7]*D[7][7] + F[4][8]*D[7][8] + F[7][8]*D[4][8] + F[4][9]*D[7][9] + F[7][9]*D[4][9] + F[7][10]*D[4][10] + F[7][11]*D[4][11] + F[7][12]*D[4][12] + F[4][13]*D[7][13] + F[4][14]*D[7][14] + F[4][15]*D[7][15])*T + D[4][7];
+	P[4][8] = P[8][4] = (F[8][9]*(F[4][9]*D[9][9] + F[4][13]*D[9][13] + F[4][14]*D[9][14] + F[4][15]*D[9][15] + F[4][6]*D[6][9] + F[4][7]*D[7][9] + F[4][8]*D[8][9]) + F[8][10]*(F[4][9]*D[9][10] + F[4][13]*D[10][13] + F[4][14]*D[10][14] + F[4][15]*D[10][15] + F[4][6]*D[6][10] + F[4][7]*D[7][10] + F[4][8]*D[8][10]) + F[8][11]*(F[4][9]*D[9][11] + F[4][13]*D[11][13] + F[4][14]*D[11][14] + F[4][15]*D[11][15] + F[4][6]*D[6][11] + F[4][7]*D[7][11] + F[4][8]*D[8][11]) + F[8][12]*(F[4][9]*D[9][12] + F[4][13]*D[12][13] + F[4][14]*D[12][14] + F[4][15]*D[12][15] + F[4][6]*D[6][12] + F[4][7]*D[7][12] + F[4][8]*D[8][12]) + F[8][6]*(F[4][6]*D[6][6] + F[4][7]*D[6][7] + F[4][8]*D[6][8] + F[4][9]*D[6][9] + F[4][13]*D[6][13] + F[4][14]*D[6][14] + F[4][15]*D[6][15]) + F[8][7]*(F[4][6]*D[6][7] + F[4][7]*D[7][7] + F[4][8]*D[7][8] + F[4][9]*D[7][9] + F[4][13]*D[7][13] + F[4][14]*D[7][14] + F[4][15]*D[7][15]))*Tsq + (F[4][6]*D[6][8] + F[4][7]*D[7][8] + F[8][6]*D[4][6] + F[8][7]*D[4][7] + F[4][8]*D[8][8] + F[4][9]*D[8][9] + F[8][9]*D[4][9] + F[8][10]*D[4][10] + F[8][11]*D[4][11] + F[8][12]*D[4][12] + F[4][13]*D[8][13] + F[4][14]*D[8][14] + F[4][15]*D[8][15])*T + D[4][8];
+	P[4][9] = P[9][4] = (F[9][10]*(F[4][9]*D[9][10] + F[4][13]*D[10][13] + F[4][14]*D[10][14] + F[4][15]*D[10][15] + F[4][6]*D[6][10] + F[4][7]*D[7][10] + F[4][8]*D[8][10]) + F[9][11]*(F[4][9]*D[9][11] + F[4][13]*D[11][13] + F[4][14]*D[11][14] + F[4][15]*D[11][15] + F[4][6]*D[6][11] + F[4][7]*D[7][11] + F[4][8]*D[8][11]) + F[9][12]*(F[4][9]*D[9][12] + F[4][13]*D[12][13] + F[4][14]*D[12][14] + F[4][15]*D[12][15] + F[4][6]*D[6][12] + F[4][7]*D[7][12] + F[4][8]*D[8][12]) + F[9][6]*(F[4][6]*D[6][6] + F[4][7]*D[6][7] + F[4][8]*D[6][8] + F[4][9]*D[6][9] + F[4][13]*D[6][13] + F[4][14]*D[6][14] + F[4][15]*D[6][15]) + F[9][7]*(F[4][6]*D[6][7] + F[4][7]*D[7][7] + F[4][8]*D[7][8] + F[4][9]*D[7][9] + F[4][13]*D[7][13] + F[4][14]*D[7][14] + F[4][15]*D[7][15]) + F[9][8]*(F[4][6]*D[6][8] + F[4][7]*D[7][8] + F[4][8]*D[8][8] + F[4][9]*D[8][9] + F[4][13]*D[8][13] + F[4][14]*D[8][14] + F[4][15]*D[8][15]))*Tsq + (F[9][6]*D[4][6] + F[9][7]*D[4][7] + F[9][8]*D[4][8] + F[4][9]*D[9][9] + F[9][10]*D[4][10] + F[9][11]*D[4][11] + F[9][12]*D[4][12] + F[4][13]*D[9][13] + F[4][14]*D[9][14] + F[4][15]*D[9][15] + F[4][6]*D[6][9] + F[4][7]*D[7][9] + F[4][8]*D[8][9])*T + D[4][9];
+	P[4][10] = P[10][4] = (F[4][9]*D[9][10] + F[4][13]*D[10][13] + F[4][14]*D[10][14] + F[4][15]*D[10][15] + F[4][6]*D[6][10] + F[4][7]*D[7][10] + F[4][8]*D[8][10])*T + D[4][10];
+	P[4][11] = P[11][4] = (F[4][9]*D[9][11] + F[4][13]*D[11][13] + F[4][14]*D[11][14] + F[4][15]*D[11][15] + F[4][6]*D[6][11] + F[4][7]*D[7][11] + F[4][8]*D[8][11])*T + D[4][11];
+	P[4][12] = P[12][4] = (F[4][9]*D[9][12] + F[4][13]*D[12][13] + F[4][14]*D[12][14] + F[4][15]*D[12][15] + F[4][6]*D[6][12] + F[4][7]*D[7][12] + F[4][8]*D[8][12])*T + D[4][12];
+	P[4][13] = P[13][4] = (F[4][9]*D[9][13] + F[4][13]*D[13][13] + F[4][14]*D[13][14] + F[4][15]*D[13][15] + F[4][6]*D[6][13] + F[4][7]*D[7][13] + F[4][8]*D[8][13])*T + D[4][13];
+	P[4][14] = P[14][4] = (F[4][9]*D[9][14] + F[4][13]*D[13][14] + F[4][14]*D[14][14] + F[4][15]*D[14][15] + F[4][6]*D[6][14] + F[4][7]*D[7][14] + F[4][8]*D[8][14])*T + D[4][14];
+	P[4][15] = P[15][4] = (F[4][9]*D[9][15] + F[4][13]*D[13][15] + F[4][14]*D[14][15] + F[4][15]*D[14][15] + F[4][6]*D[6][15] + F[4][7]*D[7][15] + F[4][8]*D[8][15])*T + D[4][15];
+	P[5][5] = (Q[3]*G[5][3]*G[5][3] + Q[4]*G[5][4]*G[5][4] + Q[5]*G[5][5]*G[5][5] + F[5][9]*(F[5][9]*D[9][9] + F[5][13]*D[9][13] + F[5][14]*D[9][14] + F[5][15]*D[9][15] + F[5][6]*D[6][9] + F[5][7]*D[7][9] + F[5][8]*D[8][9]) + F[5][13]*(F[5][9]*D[9][13] + F[5][13]*D[13][13] + F[5][14]*D[13][14] + F[5][15]*D[13][15] + F[5][6]*D[6][13] + F[5][7]*D[7][13] + F[5][8]*D[8][13]) + F[5][14]*(F[5][9]*D[9][14] + F[5][13]*D[13][14] + F[5][14]*D[14][14] + F[5][15]*D[14][15] + F[5][6]*D[6][14] + F[5][7]*D[7][14] + F[5][8]*D[8][14]) + F[5][15]*(F[5][9]*D[9][15] + F[5][13]*D[13][15] + F[5][14]*D[14][15] + F[5][15]*D[14][15] + F[5][6]*D[6][15] + F[5][7]*D[7][15] + F[5][8]*D[8][15]) + F[5][6]*(F[5][6]*D[6][6] + F[5][7]*D[6][7] + F[5][8]*D[6][8] + F[5][9]*D[6][9] + F[5][13]*D[6][13] + F[5][14]*D[6][14] + F[5][15]*D[6][15]) + F[5][7]*(F[5][6]*D[6][7] + F[5][7]*D[7][7] + F[5][8]*D[7][8] + F[5][9]*D[7][9] + F[5][13]*D[7][13] + F[5][14]*D[7][14] + F[5][15]*D[7][15]) + F[5][8]*(F[5][6]*D[6][8] + F[5][7]*D[7][8] + F[5][8]*D[8][8] + F[5][9]*D[8][9] + F[5][13]*D[8][13] + F[5][14]*D[8][14] + F[5][15]*D[8][15]))*Tsq + (2*F[5][6]*D[5][6] + 2*F[5][7]*D[5][7] + 2*F[5][8]*D[5][8] + 2*F[5][9]*D[5][9] + 2*F[5][13]*D[5][13] + 2*F[5][14]*D[5][14] + 2*F[5][15]*D[5][15])*T + D[5][5];
+	P[5][6] = P[6][5] = (F[6][9]*(F[5][9]*D[9][9] + F[5][13]*D[9][13] + F[5][14]*D[9][14] + F[5][15]*D[9][15] + F[5][6]*D[6][9] + F[5][7]*D[7][9] + F[5][8]*D[8][9]) + F[6][10]*(F[5][9]*D[9][10] + F[5][13]*D[10][13] + F[5][14]*D[10][14] + F[5][15]*D[10][15] + F[5][6]*D[6][10] + F[5][7]*D[7][10] + F[5][8]*D[8][10]) + F[6][11]*(F[5][9]*D[9][11] + F[5][13]*D[11][13] + F[5][14]*D[11][14] + F[5][15]*D[11][15] + F[5][6]*D[6][11] + F[5][7]*D[7][11] + F[5][8]*D[8][11]) + F[6][12]*(F[5][9]*D[9][12] + F[5][13]*D[12][13] + F[5][14]*D[12][14] + F[5][15]*D[12][15] + F[5][6]*D[6][12] + F[5][7]*D[7][12] + F[5][8]*D[8][12]) + F[6][7]*(F[5][6]*D[6][7] + F[5][7]*D[7][7] + F[5][8]*D[7][8] + F[5][9]*D[7][9] + F[5][13]*D[7][13] + F[5][14]*D[7][14] + F[5][15]*D[7][15]) + F[6][8]*(F[5][6]*D[6][8] + F[5][7]*D[7][8] + F[5][8]*D[8][8] + F[5][9]*D[8][9] + F[5][13]*D[8][13] + F[5][14]*D[8][14] + F[5][15]*D[8][15]))*Tsq + (F[5][6]*D[6][6] + F[5][7]*D[6][7] + F[6][7]*D[5][7] + F[5][8]*D[6][8] + F[6][8]*D[5][8] + F[5][9]*D[6][9] + F[6][9]*D[5][9] + F[6][10]*D[5][10] + F[6][11]*D[5][11] + F[6][12]*D[5][12] + F[5][13]*D[6][13] + F[5][14]*D[6][14] + F[5][15]*D[6][15])*T + D[5][6];
+	P[5][7] = P[7][5] = (F[7][9]*(F[5][9]*D[9][9] + F[5][13]*D[9][13] + F[5][14]*D[9][14] + F[5][15]*D[9][15] + F[5][6]*D[6][9] + F[5][7]*D[7][9] + F[5][8]*D[8][9]) + F[7][10]*(F[5][9]*D[9][10] + F[5][13]*D[10][13] + F[5][14]*D[10][14] + F[5][15]*D[10][15] + F[5][6]*D[6][10] + F[5][7]*D[7][10] + F[5][8]*D[8][10]) + F[7][11]*(F[5][9]*D[9][11] + F[5][13]*D[11][13] + F[5][14]*D[11][14] + F[5][15]*D[11][15] + F[5][6]*D[6][11] + F[5][7]*D[7][11] + F[5][8]*D[8][11]) + F[7][12]*(F[5][9]*D[9][12] + F[5][13]*D[12][13] + F[5][14]*D[12][14] + F[5][15]*D[12][15] + F[5][6]*D[6][12] + F[5][7]*D[7][12] + F[5][8]*D[8][12]) + F[7][6]*(F[5][6]*D[6][6] + F[5][7]*D[6][7] + F[5][8]*D[6][8] + F[5][9]*D[6][9] + F[5][13]*D[6][13] + F[5][14]*D[6][14] + F[5][15]*D[6][15]) + F[7][8]*(F[5][6]*D[6][8] + F[5][7]*D[7][8] + F[5][8]*D[8][8] + F[5][9]*D[8][9] + F[5][13]*D[8][13] + F[5][14]*D[8][14] + F[5][15]*D[8][15]))*Tsq + (F[5][6]*D[6][7] + F[7][6]*D[5][6] + F[5][7]*D[7][7] + F[5][8]*D[7][8] + F[7][8]*D[5][8] + F[5][9]*D[7][9] + F[7][9]*D[5][9] + F[7][10]*D[5][10] + F[7][11]*D[5][11] + F[7][12]*D[5][12] + F[5][13]*D[7][13] + F[5][14]*D[7][14] + F[5][15]*D[7][15])*T + D[5][7];
+	P[5][8] = P[8][5] = (F[8][9]*(F[5][9]*D[9][9] + F[5][13]*D[9][13] + F[5][14]*D[9][14] + F[5][15]*D[9][15] + F[5][6]*D[6][9] + F[5][7]*D[7][9] + F[5][8]*D[8][9]) + F[8][10]*(F[5][9]*D[9][10] + F[5][13]*D[10][13] + F[5][14]*D[10][14] + F[5][15]*D[10][15] + F[5][6]*D[6][10] + F[5][7]*D[7][10] + F[5][8]*D[8][10]) + F[8][11]*(F[5][9]*D[9][11] + F[5][13]*D[11][13] + F[5][14]*D[11][14] + F[5][15]*D[11][15] + F[5][6]*D[6][11] + F[5][7]*D[7][11] + F[5][8]*D[8][11]) + F[8][12]*(F[5][9]*D[9][12] + F[5][13]*D[12][13] + F[5][14]*D[12][14] + F[5][15]*D[12][15] + F[5][6]*D[6][12] + F[5][7]*D[7][12] + F[5][8]*D[8][12]) + F[8][6]*(F[5][6]*D[6][6] + F[5][7]*D[6][7] + F[5][8]*D[6][8] + F[5][9]*D[6][9] + F[5][13]*D[6][13] + F[5][14]*D[6][14] + F[5][15]*D[6][15]) + F[8][7]*(F[5][6]*D[6][7] + F[5][7]*D[7][7] + F[5][8]*D[7][8] + F[5][9]*D[7][9] + F[5][13]*D[7][13] + F[5][14]*D[7][14] + F[5][15]*D[7][15]))*Tsq + (F[5][6]*D[6][8] + F[5][7]*D[7][8] + F[8][6]*D[5][6] + F[8][7]*D[5][7] + F[5][8]*D[8][8] + F[5][9]*D[8][9] + F[8][9]*D[5][9] + F[8][10]*D[5][10] + F[8][11]*D[5][11] + F[8][12]*D[5][12] + F[5][13]*D[8][13] + F[5][14]*D[8][14] + F[5][15]*D[8][15])*T + D[5][8];
+	P[5][9] = P[9][5] = (F[9][10]*(F[5][9]*D[9][10] + F[5][13]*D[10][13] + F[5][14]*D[10][14] + F[5][15]*D[10][15] + F[5][6]*D[6][10] + F[5][7]*D[7][10] + F[5][8]*D[8][10]) + F[9][11]*(F[5][9]*D[9][11] + F[5][13]*D[11][13] + F[5][14]*D[11][14] + F[5][15]*D[11][15] + F[5][6]*D[6][11] + F[5][7]*D[7][11] + F[5][8]*D[8][11]) + F[9][12]*(F[5][9]*D[9][12] + F[5][13]*D[12][13] + F[5][14]*D[12][14] + F[5][15]*D[12][15] + F[5][6]*D[6][12] + F[5][7]*D[7][12] + F[5][8]*D[8][12]) + F[9][6]*(F[5][6]*D[6][6] + F[5][7]*D[6][7] + F[5][8]*D[6][8] + F[5][9]*D[6][9] + F[5][13]*D[6][13] + F[5][14]*D[6][14] + F[5][15]*D[6][15]) + F[9][7]*(F[5][6]*D[6][7] + F[5][7]*D[7][7] + F[5][8]*D[7][8] + F[5][9]*D[7][9] + F[5][13]*D[7][13] + F[5][14]*D[7][14] + F[5][15]*D[7][15]) + F[9][8]*(F[5][6]*D[6][8] + F[5][7]*D[7][8] + F[5][8]*D[8][8] + F[5][9]*D[8][9] + F[5][13]*D[8][13] + F[5][14]*D[8][14] + F[5][15]*D[8][15]))*Tsq + (F[9][6]*D[5][6] + F[9][7]*D[5][7] + F[9][8]*D[5][8] + F[5][9]*D[9][9] + F[9][10]*D[5][10] + F[9][11]*D[5][11] + F[9][12]*D[5][12] + F[5][13]*D[9][13] + F[5][14]*D[9][14] + F[5][15]*D[9][15] + F[5][6]*D[6][9] + F[5][7]*D[7][9] + F[5][8]*D[8][9])*T + D[5][9];
+	P[5][10] = P[10][5] = (F[5][9]*D[9][10] + F[5][13]*D[10][13] + F[5][14]*D[10][14] + F[5][15]*D[10][15] + F[5][6]*D[6][10] + F[5][7]*D[7][10] + F[5][8]*D[8][10])*T + D[5][10];
+	P[5][11] = P[11][5] = (F[5][9]*D[9][11] + F[5][13]*D[11][13] + F[5][14]*D[11][14] + F[5][15]*D[11][15] + F[5][6]*D[6][11] + F[5][7]*D[7][11] + F[5][8]*D[8][11])*T + D[5][11];
+	P[5][12] = P[12][5] = (F[5][9]*D[9][12] + F[5][13]*D[12][13] + F[5][14]*D[12][14] + F[5][15]*D[12][15] + F[5][6]*D[6][12] + F[5][7]*D[7][12] + F[5][8]*D[8][12])*T + D[5][12];
+	P[5][13] = P[13][5] = (F[5][9]*D[9][13] + F[5][13]*D[13][13] + F[5][14]*D[13][14] + F[5][15]*D[13][15] + F[5][6]*D[6][13] + F[5][7]*D[7][13] + F[5][8]*D[8][13])*T + D[5][13];
+	P[5][14] = P[14][5] = (F[5][9]*D[9][14] + F[5][13]*D[13][14] + F[5][14]*D[14][14] + F[5][15]*D[14][15] + F[5][6]*D[6][14] + F[5][7]*D[7][14] + F[5][8]*D[8][14])*T + D[5][14];
+	P[5][15] = P[15][5] = (F[5][9]*D[9][15] + F[5][13]*D[13][15] + F[5][14]*D[14][15] + F[5][15]*D[14][15] + F[5][6]*D[6][15] + F[5][7]*D[7][15] + F[5][8]*D[8][15])*T + D[5][15];
+	P[6][6] = (Q[0]*G[6][0]*G[6][0] + Q[1]*G[6][1]*G[6][1] + Q[2]*G[6][2]*G[6][2] + F[6][9]*(F[6][9]*D[9][9] + F[6][10]*D[9][10] + F[6][11]*D[9][11] + F[6][12]*D[9][12] + F[6][7]*D[7][9] + F[6][8]*D[8][9]) + F[6][10]*(F[6][9]*D[9][10] + F[6][10]*D[10][10] + F[6][11]*D[10][11] + F[6][12]*D[10][12] + F[6][7]*D[7][10] + F[6][8]*D[8][10]) + F[6][11]*(F[6][9]*D[9][11] + F[6][10]*D[10][11] + F[6][11]*D[11][11] + F[6][12]*D[11][12] + F[6][7]*D[7][11] + F[6][8]*D[8][11]) + F[6][12]*(F[6][9]*D[9][12] + F[6][10]*D[10][12] + F[6][11]*D[11][12] + F[6][12]*D[12][12] + F[6][7]*D[7][12] + F[6][8]*D[8][12]) + F[6][7]*(F[6][7]*D[7][7] + F[6][8]*D[7][8] + F[6][9]*D[7][9] + F[6][10]*D[7][10] + F[6][11]*D[7][11] + F[6][12]*D[7][12]) + F[6][8]*(F[6][7]*D[7][8] + F[6][8]*D[8][8] + F[6][9]*D[8][9] + F[6][10]*D[8][10] + F[6][11]*D[8][11] + F[6][12]*D[8][12]))*Tsq + (2*F[6][7]*D[6][7] + 2*F[6][8]*D[6][8] + 2*F[6][9]*D[6][9] + 2*F[6][10]*D[6][10] + 2*F[6][11]*D[6][11] + 2*F[6][12]*D[6][12])*T + D[6][6];
+	P[6][7] = P[7][6] = (F[7][9]*(F[6][9]*D[9][9] + F[6][10]*D[9][10] + F[6][11]*D[9][11] + F[6][12]*D[9][12] + F[6][7]*D[7][9] + F[6][8]*D[8][9]) + F[7][10]*(F[6][9]*D[9][10] + F[6][10]*D[10][10] + F[6][11]*D[10][11] + F[6][12]*D[10][12] + F[6][7]*D[7][10] + F[6][8]*D[8][10]) + F[7][11]*(F[6][9]*D[9][11] + F[6][10]*D[10][11] + F[6][11]*D[11][11] + F[6][12]*D[11][12] + F[6][7]*D[7][11] + F[6][8]*D[8][11]) + F[7][12]*(F[6][9]*D[9][12] + F[6][10]*D[10][12] + F[6][11]*D[11][12] + F[6][12]*D[12][12] + F[6][7]*D[7][12] + F[6][8]*D[8][12]) + F[7][6]*(F[6][7]*D[6][7] + F[6][8]*D[6][8] + F[6][9]*D[6][9] + F[6][10]*D[6][10] + F[6][11]*D[6][11] + F[6][12]*D[6][12]) + F[7][8]*(F[6][7]*D[7][8] + F[6][8]*D[8][8] + F[6][9]*D[8][9] + F[6][10]*D[8][10] + F[6][11]*D[8][11] + F[6][12]*D[8][12]) + G[6][0]*G[7][0]*Q[0] + G[6][1]*G[7][1]*Q[1] + G[6][2]*G[7][2]*Q[2])*Tsq + (F[7][6]*D[6][6] + F[6][7]*D[7][7] + F[6][8]*D[7][8] + F[7][8]*D[6][8] + F[6][9]*D[7][9] + F[7][9]*D[6][9] + F[6][10]*D[7][10] + F[7][10]*D[6][10] + F[6][11]*D[7][11] + F[7][11]*D[6][11] + F[6][12]*D[7][12] + F[7][12]*D[6][12])*T + D[6][7];
+	P[6][8] = P[8][6] = (F[8][9]*(F[6][9]*D[9][9] + F[6][10]*D[9][10] + F[6][11]*D[9][11] + F[6][12]*D[9][12] + F[6][7]*D[7][9] + F[6][8]*D[8][9]) + F[8][10]*(F[6][9]*D[9][10] + F[6][10]*D[10][10] + F[6][11]*D[10][11] + F[6][12]*D[10][12] + F[6][7]*D[7][10] + F[6][8]*D[8][10]) + F[8][11]*(F[6][9]*D[9][11] + F[6][10]*D[10][11] + F[6][11]*D[11][11] + F[6][12]*D[11][12] + F[6][7]*D[7][11] + F[6][8]*D[8][11]) + F[8][12]*(F[6][9]*D[9][12] + F[6][10]*D[10][12] + F[6][11]*D[11][12] + F[6][12]*D[12][12] + F[6][7]*D[7][12] + F[6][8]*D[8][12]) + F[8][6]*(F[6][7]*D[6][7] + F[6][8]*D[6][8] + F[6][9]*D[6][9] + F[6][10]*D[6][10] + F[6][11]*D[6][11] + F[6][12]*D[6][12]) + F[8][7]*(F[6][7]*D[7][7] + F[6][8]*D[7][8] + F[6][9]*D[7][9] + F[6][10]*D[7][10] + F[6][11]*D[7][11] + F[6][12]*D[7][12]) + G[6][0]*G[8][0]*Q[0] + G[6][1]*G[8][1]*Q[1] + G[6][2]*G[8][2]*Q[2])*Tsq + (F[6][7]*D[7][8] + F[8][6]*D[6][6] + F[8][7]*D[6][7] + F[6][8]*D[8][8] + F[6][9]*D[8][9] + F[8][9]*D[6][9] + F[6][10]*D[8][10] + F[8][10]*D[6][10] + F[6][11]*D[8][11] + F[8][11]*D[6][11] + F[6][12]*D[8][12] + F[8][12]*D[6][12])*T + D[6][8];
+	P[6][9] = P[9][6] = (F[9][10]*(F[6][9]*D[9][10] + F[6][10]*D[10][10] + F[6][11]*D[10][11] + F[6][12]*D[10][12] + F[6][7]*D[7][10] + F[6][8]*D[8][10]) + F[9][11]*(F[6][9]*D[9][11] + F[6][10]*D[10][11] + F[6][11]*D[11][11] + F[6][12]*D[11][12] + F[6][7]*D[7][11] + F[6][8]*D[8][11]) + F[9][12]*(F[6][9]*D[9][12] + F[6][10]*D[10][12] + F[6][11]*D[11][12] + F[6][12]*D[12][12] + F[6][7]*D[7][12] + F[6][8]*D[8][12]) + F[9][6]*(F[6][7]*D[6][7] + F[6][8]*D[6][8] + F[6][9]*D[6][9] + F[6][10]*D[6][10] + F[6][11]*D[6][11] + F[6][12]*D[6][12]) + F[9][7]*(F[6][7]*D[7][7] + F[6][8]*D[7][8] + F[6][9]*D[7][9] + F[6][10]*D[7][10] + F[6][11]*D[7][11] + F[6][12]*D[7][12]) + F[9][8]*(F[6][7]*D[7][8] + F[6][8]*D[8][8] + F[6][9]*D[8][9] + F[6][10]*D[8][10] + F[6][11]*D[8][11] + F[6][12]*D[8][12]) + G[9][0]*G[6][0]*Q[0] + G[9][1]*G[6][1]*Q[1] + G[9][2]*G[6][2]*Q[2])*Tsq + (F[9][6]*D[6][6] + F[9][7]*D[6][7] + F[9][8]*D[6][8] + F[6][9]*D[9][9] + F[9][10]*D[6][10] + F[6][10]*D[9][10] + F[9][11]*D[6][11] + F[6][11]*D[9][11] + F[9][12]*D[6][12] + F[6][12]*D[9][12] + F[6][7]*D[7][9] + F[6][8]*D[8][9])*T + D[6][9];
+	P[6][10] = P[10][6] = (F[6][9]*D[9][10] + F[6][10]*D[10][10] + F[6][11]*D[10][11] + F[6][12]*D[10][12] + F[6][7]*D[7][10] + F[6][8]*D[8][10])*T + D[6][10];
+	P[6][11] = P[11][6] = (F[6][9]*D[9][11] + F[6][10]*D[10][11] + F[6][11]*D[11][11] + F[6][12]*D[11][12] + F[6][7]*D[7][11] + F[6][8]*D[8][11])*T + D[6][11];
+	P[6][12] = P[12][6] = (F[6][9]*D[9][12] + F[6][10]*D[10][12] + F[6][11]*D[11][12] + F[6][12]*D[12][12] + F[6][7]*D[7][12] + F[6][8]*D[8][12])*T + D[6][12];
+	P[6][13] = P[13][6] = (F[6][9]*D[9][13] + F[6][10]*D[10][13] + F[6][11]*D[11][13] + F[6][12]*D[12][13] + F[6][7]*D[7][13] + F[6][8]*D[8][13])*T + D[6][13];
+	P[6][14] = P[14][6] = (F[6][9]*D[9][14] + F[6][10]*D[10][14] + F[6][11]*D[11][14] + F[6][12]*D[12][14] + F[6][7]*D[7][14] + F[6][8]*D[8][14])*T + D[6][14];
+	P[6][15] = P[15][6] = (F[6][9]*D[9][15] + F[6][10]*D[10][15] + F[6][11]*D[11][15] + F[6][12]*D[12][15] + F[6][7]*D[7][15] + F[6][8]*D[8][15])*T + D[6][15];
+	P[7][7] = (Q[0]*G[7][0]*G[7][0] + Q[1]*G[7][1]*G[7][1] + Q[2]*G[7][2]*G[7][2] + F[7][9]*(F[7][9]*D[9][9] + F[7][10]*D[9][10] + F[7][11]*D[9][11] + F[7][12]*D[9][12] + F[7][6]*D[6][9] + F[7][8]*D[8][9]) + F[7][10]*(F[7][9]*D[9][10] + F[7][10]*D[10][10] + F[7][11]*D[10][11] + F[7][12]*D[10][12] + F[7][6]*D[6][10] + F[7][8]*D[8][10]) + F[7][11]*(F[7][9]*D[9][11] + F[7][10]*D[10][11] + F[7][11]*D[11][11] + F[7][12]*D[11][12] + F[7][6]*D[6][11] + F[7][8]*D[8][11]) + F[7][12]*(F[7][9]*D[9][12] + F[7][10]*D[10][12] + F[7][11]*D[11][12] + F[7][12]*D[12][12] + F[7][6]*D[6][12] + F[7][8]*D[8][12]) + F[7][6]*(F[7][6]*D[6][6] + F[7][8]*D[6][8] + F[7][9]*D[6][9] + F[7][10]*D[6][10] + F[7][11]*D[6][11] + F[7][12]*D[6][12]) + F[7][8]*(F[7][6]*D[6][8] + F[7][8]*D[8][8] + F[7][9]*D[8][9] + F[7][10]*D[8][10] + F[7][11]*D[8][11] + F[7][12]*D[8][12]))*Tsq + (2*F[7][6]*D[6][7] + 2*F[7][8]*D[7][8] + 2*F[7][9]*D[7][9] + 2*F[7][10]*D[7][10] + 2*F[7][11]*D[7][11] + 2*F[7][12]*D[7][12])*T + D[7][7];
+	P[7][8] = P[8][7] = (F[8][9]*(F[7][9]*D[9][9] + F[7][10]*D[9][10] + F[7][11]*D[9][11] + F[7][12]*D[9][12] + F[7][6]*D[6][9] + F[7][8]*D[8][9]) + F[8][10]*(F[7][9]*D[9][10] + F[7][10]*D[10][10] + F[7][11]*D[10][11] + F[7][12]*D[10][12] + F[7][6]*D[6][10] + F[7][8]*D[8][10]) + F[8][11]*(F[7][9]*D[9][11] + F[7][10]*D[10][11] + F[7][11]*D[11][11] + F[7][12]*D[11][12] + F[7][6]*D[6][11] + F[7][8]*D[8][11]) + F[8][12]*(F[7][9]*D[9][12] + F[7][10]*D[10][12] + F[7][11]*D[11][12] + F[7][12]*D[12][12] + F[7][6]*D[6][12] + F[7][8]*D[8][12]) + F[8][6]*(F[7][6]*D[6][6] + F[7][8]*D[6][8] + F[7][9]*D[6][9] + F[7][10]*D[6][10] + F[7][11]*D[6][11] + F[7][12]*D[6][12]) + F[8][7]*(F[7][6]*D[6][7] + F[7][8]*D[7][8] + F[7][9]*D[7][9] + F[7][10]*D[7][10] + F[7][11]*D[7][11] + F[7][12]*D[7][12]) + G[7][0]*G[8][0]*Q[0] + G[7][1]*G[8][1]*Q[1] + G[7][2]*G[8][2]*Q[2])*Tsq + (F[7][6]*D[6][8] + F[8][6]*D[6][7] + F[8][7]*D[7][7] + F[7][8]*D[8][8] + F[7][9]*D[8][9] + F[8][9]*D[7][9] + F[7][10]*D[8][10] + F[8][10]*D[7][10] + F[7][11]*D[8][11] + F[8][11]*D[7][11] + F[7][12]*D[8][12] + F[8][12]*D[7][12])*T + D[7][8];
+	P[7][9] = P[9][7] = (F[9][10]*(F[7][9]*D[9][10] + F[7][10]*D[10][10] + F[7][11]*D[10][11] + F[7][12]*D[10][12] + F[7][6]*D[6][10] + F[7][8]*D[8][10]) + F[9][11]*(F[7][9]*D[9][11] + F[7][10]*D[10][11] + F[7][11]*D[11][11] + F[7][12]*D[11][12] + F[7][6]*D[6][11] + F[7][8]*D[8][11]) + F[9][12]*(F[7][9]*D[9][12] + F[7][10]*D[10][12] + F[7][11]*D[11][12] + F[7][12]*D[12][12] + F[7][6]*D[6][12] + F[7][8]*D[8][12]) + F[9][6]*(F[7][6]*D[6][6] + F[7][8]*D[6][8] + F[7][9]*D[6][9] + F[7][10]*D[6][10] + F[7][11]*D[6][11] + F[7][12]*D[6][12]) + F[9][7]*(F[7][6]*D[6][7] + F[7][8]*D[7][8] + F[7][9]*D[7][9] + F[7][10]*D[7][10] + F[7][11]*D[7][11] + F[7][12]*D[7][12]) + F[9][8]*(F[7][6]*D[6][8] + F[7][8]*D[8][8] + F[7][9]*D[8][9] + F[7][10]*D[8][10] + F[7][11]*D[8][11] + F[7][12]*D[8][12]) + G[9][0]*G[7][0]*Q[0] + G[9][1]*G[7][1]*Q[1] + G[9][2]*G[7][2]*Q[2])*Tsq + (F[9][6]*D[6][7] + F[9][7]*D[7][7] + F[9][8]*D[7][8] + F[7][9]*D[9][9] + F[9][10]*D[7][10] + F[7][10]*D[9][10] + F[9][11]*D[7][11] + F[7][11]*D[9][11] + F[9][12]*D[7][12] + F[7][12]*D[9][12] + F[7][6]*D[6][9] + F[7][8]*D[8][9])*T + D[7][9];
+	P[7][10] = P[10][7] = (F[7][9]*D[9][10] + F[7][10]*D[10][10] + F[7][11]*D[10][11] + F[7][12]*D[10][12] + F[7][6]*D[6][10] + F[7][8]*D[8][10])*T + D[7][10];
+	P[7][11] = P[11][7] = (F[7][9]*D[9][11] + F[7][10]*D[10][11] + F[7][11]*D[11][11] + F[7][12]*D[11][12] + F[7][6]*D[6][11] + F[7][8]*D[8][11])*T + D[7][11];
+	P[7][12] = P[12][7] = (F[7][9]*D[9][12] + F[7][10]*D[10][12] + F[7][11]*D[11][12] + F[7][12]*D[12][12] + F[7][6]*D[6][12] + F[7][8]*D[8][12])*T + D[7][12];
+	P[7][13] = P[13][7] = (F[7][9]*D[9][13] + F[7][10]*D[10][13] + F[7][11]*D[11][13] + F[7][12]*D[12][13] + F[7][6]*D[6][13] + F[7][8]*D[8][13])*T + D[7][13];
+	P[7][14] = P[14][7] = (F[7][9]*D[9][14] + F[7][10]*D[10][14] + F[7][11]*D[11][14] + F[7][12]*D[12][14] + F[7][6]*D[6][14] + F[7][8]*D[8][14])*T + D[7][14];
+	P[7][15] = P[15][7] = (F[7][9]*D[9][15] + F[7][10]*D[10][15] + F[7][11]*D[11][15] + F[7][12]*D[12][15] + F[7][6]*D[6][15] + F[7][8]*D[8][15])*T + D[7][15];
+	P[8][8] = (Q[0]*G[8][0]*G[8][0] + Q[1]*G[8][1]*G[8][1] + Q[2]*G[8][2]*G[8][2] + F[8][9]*(F[8][9]*D[9][9] + F[8][10]*D[9][10] + F[8][11]*D[9][11] + F[8][12]*D[9][12] + F[8][6]*D[6][9] + F[8][7]*D[7][9]) + F[8][10]*(F[8][9]*D[9][10] + F[8][10]*D[10][10] + F[8][11]*D[10][11] + F[8][12]*D[10][12] + F[8][6]*D[6][10] + F[8][7]*D[7][10]) + F[8][11]*(F[8][9]*D[9][11] + F[8][10]*D[10][11] + F[8][11]*D[11][11] + F[8][12]*D[11][12] + F[8][6]*D[6][11] + F[8][7]*D[7][11]) + F[8][12]*(F[8][9]*D[9][12] + F[8][10]*D[10][12] + F[8][11]*D[11][12] + F[8][12]*D[12][12] + F[8][6]*D[6][12] + F[8][7]*D[7][12]) + F[8][6]*(F[8][6]*D[6][6] + F[8][7]*D[6][7] + F[8][9]*D[6][9] + F[8][10]*D[6][10] + F[8][11]*D[6][11] + F[8][12]*D[6][12]) + F[8][7]*(F[8][6]*D[6][7] + F[8][7]*D[7][7] + F[8][9]*D[7][9] + F[8][10]*D[7][10] + F[8][11]*D[7][11] + F[8][12]*D[7][12]))*Tsq + (2*F[8][6]*D[6][8] + 2*F[8][7]*D[7][8] + 2*F[8][9]*D[8][9] + 2*F[8][10]*D[8][10] + 2*F[8][11]*D[8][11] + 2*F[8][12]*D[8][12])*T + D[8][8];
+	P[8][9] = P[9][8] = (F[9][10]*(F[8][9]*D[9][10] + F[8][10]*D[10][10] + F[8][11]*D[10][11] + F[8][12]*D[10][12] + F[8][6]*D[6][10] + F[8][7]*D[7][10]) + F[9][11]*(F[8][9]*D[9][11] + F[8][10]*D[10][11] + F[8][11]*D[11][11] + F[8][12]*D[11][12] + F[8][6]*D[6][11] + F[8][7]*D[7][11]) + F[9][12]*(F[8][9]*D[9][12] + F[8][10]*D[10][12] + F[8][11]*D[11][12] + F[8][12]*D[12][12] + F[8][6]*D[6][12] + F[8][7]*D[7][12]) + F[9][6]*(F[8][6]*D[6][6] + F[8][7]*D[6][7] + F[8][9]*D[6][9] + F[8][10]*D[6][10] + F[8][11]*D[6][11] + F[8][12]*D[6][12]) + F[9][7]*(F[8][6]*D[6][7] + F[8][7]*D[7][7] + F[8][9]*D[7][9] + F[8][10]*D[7][10] + F[8][11]*D[7][11] + F[8][12]*D[7][12]) + F[9][8]*(F[8][6]*D[6][8] + F[8][7]*D[7][8] + F[8][9]*D[8][9] + F[8][10]*D[8][10] + F[8][11]*D[8][11] + F[8][12]*D[8][12]) + G[9][0]*G[8][0]*Q[0] + G[9][1]*G[8][1]*Q[1] + G[9][2]*G[8][2]*Q[2])*Tsq + (F[9][6]*D[6][8] + F[9][7]*D[7][8] + F[9][8]*D[8][8] + F[8][9]*D[9][9] + F[9][10]*D[8][10] + F[8][10]*D[9][10] + F[9][11]*D[8][11] + F[8][11]*D[9][11] + F[9][12]*D[8][12] + F[8][12]*D[9][12] + F[8][6]*D[6][9] + F[8][7]*D[7][9])*T + D[8][9];
+	P[8][10] = P[10][8] = (F[8][9]*D[9][10] + F[8][10]*D[10][10] + F[8][11]*D[10][11] + F[8][12]*D[10][12] + F[8][6]*D[6][10] + F[8][7]*D[7][10])*T + D[8][10];
+	P[8][11] = P[11][8] = (F[8][9]*D[9][11] + F[8][10]*D[10][11] + F[8][11]*D[11][11] + F[8][12]*D[11][12] + F[8][6]*D[6][11] + F[8][7]*D[7][11])*T + D[8][11];
+	P[8][12] = P[12][8] = (F[8][9]*D[9][12] + F[8][10]*D[10][12] + F[8][11]*D[11][12] + F[8][12]*D[12][12] + F[8][6]*D[6][12] + F[8][7]*D[7][12])*T + D[8][12];
+	P[8][13] = P[13][8] = (F[8][9]*D[9][13] + F[8][10]*D[10][13] + F[8][11]*D[11][13] + F[8][12]*D[12][13] + F[8][6]*D[6][13] + F[8][7]*D[7][13])*T + D[8][13];
+	P[8][14] = P[14][8] = (F[8][9]*D[9][14] + F[8][10]*D[10][14] + F[8][11]*D[11][14] + F[8][12]*D[12][14] + F[8][6]*D[6][14] + F[8][7]*D[7][14])*T + D[8][14];
+	P[8][15] = P[15][8] = (F[8][9]*D[9][15] + F[8][10]*D[10][15] + F[8][11]*D[11][15] + F[8][12]*D[12][15] + F[8][6]*D[6][15] + F[8][7]*D[7][15])*T + D[8][15];
+	P[9][9] = (Q[0]*G[9][0]*G[9][0] + Q[1]*G[9][1]*G[9][1] + Q[2]*G[9][2]*G[9][2] + F[9][10]*(F[9][10]*D[10][10] + F[9][11]*D[10][11] + F[9][12]*D[10][12] + F[9][6]*D[6][10] + F[9][7]*D[7][10] + F[9][8]*D[8][10]) + F[9][11]*(F[9][10]*D[10][11] + F[9][11]*D[11][11] + F[9][12]*D[11][12] + F[9][6]*D[6][11] + F[9][7]*D[7][11] + F[9][8]*D[8][11]) + F[9][12]*(F[9][10]*D[10][12] + F[9][11]*D[11][12] + F[9][12]*D[12][12] + F[9][6]*D[6][12] + F[9][7]*D[7][12] + F[9][8]*D[8][12]) + F[9][6]*(F[9][6]*D[6][6] + F[9][7]*D[6][7] + F[9][8]*D[6][8] + F[9][10]*D[6][10] + F[9][11]*D[6][11] + F[9][12]*D[6][12]) + F[9][7]*(F[9][6]*D[6][7] + F[9][7]*D[7][7] + F[9][8]*D[7][8] + F[9][10]*D[7][10] + F[9][11]*D[7][11] + F[9][12]*D[7][12]) + F[9][8]*(F[9][6]*D[6][8] + F[9][7]*D[7][8] + F[9][8]*D[8][8] + F[9][10]*D[8][10] + F[9][11]*D[8][11] + F[9][12]*D[8][12]))*Tsq + (2*F[9][10]*D[9][10] + 2*F[9][11]*D[9][11] + 2*F[9][12]*D[9][12] + 2*F[9][6]*D[6][9] + 2*F[9][7]*D[7][9] + 2*F[9][8]*D[8][9])*T + D[9][9];
+	P[9][10] = P[10][9] = (F[9][10]*D[10][10] + F[9][11]*D[10][11] + F[9][12]*D[10][12] + F[9][6]*D[6][10] + F[9][7]*D[7][10] + F[9][8]*D[8][10])*T + D[9][10];
+	P[9][11] = P[11][9] = (F[9][10]*D[10][11] + F[9][11]*D[11][11] + F[9][12]*D[11][12] + F[9][6]*D[6][11] + F[9][7]*D[7][11] + F[9][8]*D[8][11])*T + D[9][11];
+	P[9][12] = P[12][9] = (F[9][10]*D[10][12] + F[9][11]*D[11][12] + F[9][12]*D[12][12] + F[9][6]*D[6][12] + F[9][7]*D[7][12] + F[9][8]*D[8][12])*T + D[9][12];
+	P[9][13] = P[13][9] = (F[9][10]*D[10][13] + F[9][11]*D[11][13] + F[9][12]*D[12][13] + F[9][6]*D[6][13] + F[9][7]*D[7][13] + F[9][8]*D[8][13])*T + D[9][13];
+	P[9][14] = P[14][9] = (F[9][10]*D[10][14] + F[9][11]*D[11][14] + F[9][12]*D[12][14] + F[9][6]*D[6][14] + F[9][7]*D[7][14] + F[9][8]*D[8][14])*T + D[9][14];
+	P[9][15] = P[15][9] = (F[9][10]*D[10][15] + F[9][11]*D[11][15] + F[9][12]*D[12][15] + F[9][6]*D[6][15] + F[9][7]*D[7][15] + F[9][8]*D[8][15])*T + D[9][15];
+	P[10][10] = Q[6]*Tsq + D[10][10];
+	P[10][11] = P[11][10] = D[10][11];
+	P[10][12] = P[12][10] = D[10][12];
+	P[10][13] = P[13][10] = D[10][13];
+	P[10][14] = P[14][10] = D[10][14];
+	P[10][15] = P[15][10] = D[10][15];
+	P[11][11] = Q[7]*Tsq + D[11][11];
+	P[11][12] = P[12][11] = D[11][12];
+	P[11][13] = P[13][11] = D[11][13];
+	P[11][14] = P[14][11] = D[11][14];
+	P[11][15] = P[15][11] = D[11][15];
+	P[12][12] = Q[8]*Tsq + D[12][12];
+	P[12][13] = P[13][12] = D[12][13];
+	P[12][14] = P[14][12] = D[12][14];
+	P[12][15] = P[15][12] = D[12][15];
+	P[13][13] = Q[9]*Tsq + D[13][13];
+	P[13][14] = P[14][13] = D[13][14];
+	P[13][15] = P[15][13] = D[13][15];
+	P[14][14] = Q[10]*Tsq + D[14][14];
+	P[14][15] = P[15][14] = D[14][15];
+	P[15][15] = Q[11]*Tsq + D[14][15];
+}
+#endif
+
+//  *************  SerialUpdate *******************
+//  Does the update step of the Kalman filter for the covariance and estimate
+//  Outputs are Xnew & Pnew, and are written over P and X
+//  Z is actual measurement, Y is predicted measurement
+//  Xnew = X + K*(Z-Y), Pnew=(I-K*H)*P,
+//    where K=P*H'*inv[H*P*H'+R]
+//  NOTE the algorithm assumes R (measurement covariance matrix) is diagonal
+//    i.e. the measurment noises are uncorrelated.
+//  It therefore uses a serial update that requires no matrix inversion by
+//    processing the measurements one at a time.
+//  Algorithm - see Grewal and Andrews, "Kalman Filtering,2nd Ed" p.121 & p.253
+//            - or see Simon, "Optimal State Estimation," 1st Ed, p.150
+//  The SensorsUsed variable is a bitwise mask indicating which sensors
+//     should be used in the update.
+//  ************************************************
+
+void SerialUpdate(float H[NUMV][NUMX], float R[NUMV], float Z[NUMV],
+		  float Y[NUMV], float P[NUMX][NUMX], float X[NUMX],
+		  uint16_t SensorsUsed)
+{
+	float HP[NUMX], HPHR, Error;
+	uint8_t i, j, k, m;
+
+	for (m = 0; m < NUMV; m++) {
+
+		if (SensorsUsed & (0x01 << m)) {	// use this sensor for update
+
+			for (j = 0; j < NUMX; j++) {	// Find Hp = H*P
+				HP[j] = 0;
+				for (k = 0; k < NUMX; k++)
+					HP[j] += H[m][k] * P[k][j];
+			}
+			HPHR = R[m];	// Find  HPHR = H*P*H' + R
+			for (k = 0; k < NUMX; k++)
+				HPHR += HP[k] * H[m][k];
+
+			for (k = 0; k < NUMX; k++)
+				K[k][m] = HP[k] / HPHR;	// find K = HP/HPHR
+
+			for (i = 0; i < NUMX; i++) {	// Find P(m)= P(m-1) + K*HP
+				for (j = i; j < NUMX; j++)
+					P[i][j] = P[j][i] =
+					    P[i][j] - K[i][m] * HP[j];
+			}
+
+			Error = Z[m] - Y[m];
+			for (i = 0; i < NUMX; i++)	// Find X(m)= X(m-1) + K*Error
+				X[i] = X[i] + K[i][m] * Error;
+
+		}
+	}
+}
+
+//  *************  RungeKutta **********************
+//  Does a 4th order Runge Kutta numerical integration step
+//  Output, Xnew, is written over X
+//  NOTE the algorithm assumes time invariant state equations and
+//    constant inputs over integration step
+//  ************************************************
+
+void RungeKutta(float X[NUMX], float U[NUMU], float dT)
+{
+
+	float dT2 =
+	    dT / 2, K1[NUMX], K2[NUMX], K3[NUMX], K4[NUMX], Xlast[NUMX];
+	uint8_t i;
+
+	for (i = 0; i < NUMX; i++)
+		Xlast[i] = X[i];	// make a working copy
+
+	StateEq(X, U, K1);	// k1 = f(x,u)
+	for (i = 0; i < NUMX; i++)
+		X[i] = Xlast[i] + dT2 * K1[i];
+	StateEq(X, U, K2);	// k2 = f(x+0.5*dT*k1,u)
+	for (i = 0; i < NUMX; i++)
+		X[i] = Xlast[i] + dT2 * K2[i];
+	StateEq(X, U, K3);	// k3 = f(x+0.5*dT*k2,u)
+	for (i = 0; i < NUMX; i++)
+		X[i] = Xlast[i] + dT * K3[i];
+	StateEq(X, U, K4);	// k4 = f(x+dT*k3,u)
+
+	// Xnew  = X + dT*(k1+2*k2+2*k3+k4)/6
+	for (i = 0; i < NUMX; i++)
+		X[i] =
+		    Xlast[i] + dT * (K1[i] + 2 * K2[i] + 2 * K3[i] +
+				     K4[i]) / 6;
+}
+
+//  *************  Model Specific Stuff  ***************************
+//  ***  StateEq, MeasurementEq, LinerizeFG, and LinearizeH ********
+//
+//  State Variables = [Pos Vel Quaternion GyroBias AccelBias]
+//  Deterministic Inputs = [AngularVel Accel]
+//  Disturbance Noise = [GyroNoise AccelNoise GyroRandomWalkNoise AccelRandomWalkNoise]
+//
+//  Measurement Variables = [Pos Vel BodyFrameMagField Altimeter]
+//  Inputs to Measurement = [EarthFrameMagField]
+//
+//  Notes: Pos and Vel in earth frame
+//  AngularVel and Accel in body frame
+//  MagFields are unit vectors
+//  Xdot is output of StateEq()
+//  F and G are outputs of LinearizeFG(), all elements not set should be zero
+//  y is output of OutputEq()
+//  H is output of LinearizeH(), all elements not set should be zero
+//  ************************************************
+
+void StateEq(float X[NUMX], float U[NUMU], float Xdot[NUMX])
+{
+	float ax, ay, az, wx, wy, wz, q0, q1, q2, q3;
+
+	ax=U[3]-X[13];
+	ay=U[4]-X[14];
+	az=U[5]-X[15];  // subtract the biases on accels
+	wx = U[0] - X[10];
+	wy = U[1] - X[11];
+	wz = U[2] - X[12];	// subtract the biases on gyros
+	q0 = X[6];
+	q1 = X[7];
+	q2 = X[8];
+	q3 = X[9];
+
+	// Pdot = V
+	Xdot[0] = X[3];
+	Xdot[1] = X[4];
+	Xdot[2] = X[5];
+
+	// Vdot = Reb*a
+	Xdot[3] =
+	    (q0 * q0 + q1 * q1 - q2 * q2 - q3 * q3) * ax + 2 * (q1 * q2 -
+								q0 * q3) *
+	    ay + 2 * (q1 * q3 + q0 * q2) * az;
+	Xdot[4] =
+	    2 * (q1 * q2 + q0 * q3) * ax + (q0 * q0 - q1 * q1 + q2 * q2 -
+					    q3 * q3) * ay + 2 * (q2 * q3 -
+								 q0 * q1) *
+	    az;
+	Xdot[5] =
+	    2 * (q1 * q3 - q0 * q2) * ax + 2 * (q2 * q3 + q0 * q1) * ay +
+	    (q0 * q0 - q1 * q1 - q2 * q2 + q3 * q3) * az + 9.81;
+
+	// qdot = Q*w
+	Xdot[6] = (-q1 * wx - q2 * wy - q3 * wz) / 2;
+	Xdot[7] = (q0 * wx - q3 * wy + q2 * wz) / 2;
+	Xdot[8] = (q3 * wx + q0 * wy - q1 * wz) / 2;
+	Xdot[9] = (-q2 * wx + q1 * wy + q0 * wz) / 2;
+
+	// best guess is that bias stays constant
+	Xdot[10] = Xdot[11] = Xdot[12] = 0;
+	Xdot[13] = Xdot[14] = Xdot[15] = 0;
+}
+
+void LinearizeFG(float X[NUMX], float U[NUMU], float F[NUMX][NUMX],
+		 float G[NUMX][NUMW])
+{
+	float ax, ay, az, wx, wy, wz, q0, q1, q2, q3;
+
+	// ax=U[3]-X[13]; ay=U[4]-X[14]; az=U[5]-X[15];  // subtract the biases on accels
+	ax = U[3];
+	ay = U[4];
+	az = U[5];		// NO BIAS STATES ON ACCELS
+	wx = U[0] - X[10];
+	wy = U[1] - X[11];
+	wz = U[2] - X[12];	// subtract the biases on gyros
+	q0 = X[6];
+	q1 = X[7];
+	q2 = X[8];
+	q3 = X[9];
+
+	// Pdot = V
+	F[0][3] = F[1][4] = F[2][5] = 1;
+
+	// dVdot/dq
+	F[3][6] = 2 * (q0 * ax - q3 * ay + q2 * az);
+	F[3][7] = 2 * (q1 * ax + q2 * ay + q3 * az);
+	F[3][8] = 2 * (-q2 * ax + q1 * ay + q0 * az);
+	F[3][9] = 2 * (-q3 * ax - q0 * ay + q1 * az);
+	F[4][6] = 2 * (q3 * ax + q0 * ay - q1 * az);
+	F[4][7] = 2 * (q2 * ax - q1 * ay - q0 * az);
+	F[4][8] = 2 * (q1 * ax + q2 * ay + q3 * az);
+	F[4][9] = 2 * (q0 * ax - q3 * ay + q2 * az);
+	F[5][6] = 2 * (-q2 * ax + q1 * ay + q0 * az);
+	F[5][7] = 2 * (q3 * ax + q0 * ay - q1 * az);
+	F[5][8] = 2 * (-q0 * ax + q3 * ay - q2 * az);
+	F[5][9] = 2 * (q1 * ax + q2 * ay + q3 * az);
+
+	// dVdot/dabias & dVdot/dna
+	F[3][13]=G[3][3]=-q0*q0-q1*q1+q2*q2+q3*q3; F[3][14]=G[3][4]=2*(-q1*q2+q0*q3);         F[3][15]=G[3][5]=-2*(q1*q3+q0*q2);
+	F[4][13]=G[4][3]=-2*(q1*q2+q0*q3);         F[4][14]=G[4][4]=-q0*q0+q1*q1-q2*q2+q3*q3; F[4][15]=G[4][5]=2*(-q2*q3+q0*q1);
+	F[5][13]=G[5][3]=2*(-q1*q3+q0*q2);         F[5][14]=G[5][4]=-2*(q2*q3+q0*q1);         F[5][15]=G[5][5]=-q0*q0+q1*q1+q2*q2-q3*q3;
+
+	// dqdot/dq
+	F[6][6] = 0;
+	F[6][7] = -wx / 2;
+	F[6][8] = -wy / 2;
+	F[6][9] = -wz / 2;
+	F[7][6] = wx / 2;
+	F[7][7] = 0;
+	F[7][8] = wz / 2;
+	F[7][9] = -wy / 2;
+	F[8][6] = wy / 2;
+	F[8][7] = -wz / 2;
+	F[8][8] = 0;
+	F[8][9] = wx / 2;
+	F[9][6] = wz / 2;
+	F[9][7] = wy / 2;
+	F[9][8] = -wx / 2;
+	F[9][9] = 0;
+
+	// dqdot/dwbias
+	F[6][10] = q1 / 2;
+	F[6][11] = q2 / 2;
+	F[6][12] = q3 / 2;
+	F[7][10] = -q0 / 2;
+	F[7][11] = q3 / 2;
+	F[7][12] = -q2 / 2;
+	F[8][10] = -q3 / 2;
+	F[8][11] = -q0 / 2;
+	F[8][12] = q1 / 2;
+	F[9][10] = q2 / 2;
+	F[9][11] = -q1 / 2;
+	F[9][12] = -q0 / 2;
+
+	// dVdot/dna  - WITH BIAS STATES ON ACCELS - THIS DONE ABOVE
+	//G[3][3]=-q0*q0-q1*q1+q2*q2+q3*q3; G[3][4]=2*(-q1*q2+q0*q3);         G[3][5]=-2*(q1*q3+q0*q2);
+	//G[4][3]=-2*(q1*q2+q0*q3);         G[4][4]=-q0*q0+q1*q1-q2*q2+q3*q3; G[4][5]=2*(-q2*q3+q0*q1);
+	//G[5][3]=2*(-q1*q3+q0*q2);         G[5][4]=-2*(q2*q3+q0*q1);         G[5][5]=-q0*q0+q1*q1+q2*q2-q3*q3;
+
+	// dqdot/dnw
+	G[6][0] = q1 / 2;
+	G[6][1] = q2 / 2;
+	G[6][2] = q3 / 2;
+	G[7][0] = -q0 / 2;
+	G[7][1] = q3 / 2;
+	G[7][2] = -q2 / 2;
+	G[8][0] = -q3 / 2;
+	G[8][1] = -q0 / 2;
+	G[8][2] = q1 / 2;
+	G[9][0] = q2 / 2;
+	G[9][1] = -q1 / 2;
+	G[9][2] = -q0 / 2;
+
+	// dwbias = random walk noise
+	G[10][6] = G[11][7] = G[12][8] = 1;
+	// dabias = random walk noise
+	G[13][9] = G[14][10] = G[15][11] = 1;
+}
+
+void MeasurementEq(float X[NUMX], float Be[3], float Y[NUMV])
+{
+	float q0, q1, q2, q3;
+
+	q0 = X[6];
+	q1 = X[7];
+	q2 = X[8];
+	q3 = X[9];
+
+	// first six outputs are P and V
+	Y[0] = X[0];
+	Y[1] = X[1];
+	Y[2] = X[2];
+	Y[3] = X[3];
+	Y[4] = X[4];
+	Y[5] = X[5];
+
+	// Bb=Rbe*Be
+	Y[6] =
+	    (q0 * q0 + q1 * q1 - q2 * q2 - q3 * q3) * Be[0] +
+	    2 * (q1 * q2 + q0 * q3) * Be[1] + 2 * (q1 * q3 -
+						   q0 * q2) * Be[2];
+	Y[7] =
+	    2 * (q1 * q2 - q0 * q3) * Be[0] + (q0 * q0 - q1 * q1 +
+					       q2 * q2 - q3 * q3) * Be[1] +
+	    2 * (q2 * q3 + q0 * q1) * Be[2];
+	Y[8] =
+	    2 * (q1 * q3 + q0 * q2) * Be[0] + 2 * (q2 * q3 -
+						   q0 * q1) * Be[1] +
+	    (q0 * q0 - q1 * q1 - q2 * q2 + q3 * q3) * Be[2];
+
+	// Alt = -Pz
+	Y[9] = -X[2];
+}
+
+void LinearizeH(float X[NUMX], float Be[3], float H[NUMV][NUMX])
+{
+	float q0, q1, q2, q3;
+
+	q0 = X[6];
+	q1 = X[7];
+	q2 = X[8];
+	q3 = X[9];
+
+	// dP/dP=I;
+	H[0][0] = H[1][1] = H[2][2] = 1;
+	// dV/dV=I;
+	H[3][3] = H[4][4] = H[5][5] = 1;
+
+	// dBb/dq
+	H[6][6] = 2 * (q0 * Be[0] + q3 * Be[1] - q2 * Be[2]);
+	H[6][7] = 2 * (q1 * Be[0] + q2 * Be[1] + q3 * Be[2]);
+	H[6][8] = 2 * (-q2 * Be[0] + q1 * Be[1] - q0 * Be[2]);
+	H[6][9] = 2 * (-q3 * Be[0] + q0 * Be[1] + q1 * Be[2]);
+	H[7][6] = 2 * (-q3 * Be[0] + q0 * Be[1] + q1 * Be[2]);
+	H[7][7] = 2 * (q2 * Be[0] - q1 * Be[1] + q0 * Be[2]);
+	H[7][8] = 2 * (q1 * Be[0] + q2 * Be[1] + q3 * Be[2]);
+	H[7][9] = 2 * (-q0 * Be[0] - q3 * Be[1] + q2 * Be[2]);
+	H[8][6] = 2 * (q2 * Be[0] - q1 * Be[1] + q0 * Be[2]);
+	H[8][7] = 2 * (q3 * Be[0] - q0 * Be[1] - q1 * Be[2]);
+	H[8][8] = 2 * (q0 * Be[0] + q3 * Be[1] - q2 * Be[2]);
+	H[8][9] = 2 * (q1 * Be[0] + q2 * Be[1] + q3 * Be[2]);
+
+	// dAlt/dPz = -1
+	H[9][2] = -1;
+}
+
+/**
+ * @}
+ * @}
+ */
diff --git a/flight/Revolution/insgps_helper.c b/flight/Revolution/insgps_helper.c
new file mode 100644
index 000000000..901e9db3f
--- /dev/null
+++ b/flight/Revolution/insgps_helper.c
@@ -0,0 +1,328 @@
+
+#include "ins.h"
+#include "pios.h"
+#include "ahrs_spi_comm.h"
+#include "insgps.h"
+#include "CoordinateConversions.h"
+
+#define DEG_TO_RAD         (M_PI / 180.0)
+#define RAD_TO_DEG         (180.0 / M_PI)
+
+#define INSGPS_GPS_TIMEOUT 2   /* 2 seconds triggers reinit of position */
+#define INSGPS_GPS_MINSAT  6   /* 2 seconds triggers reinit of position */
+#define INSGPS_GPS_MINPDOP 3.5 /* minimum PDOP for postition updates    */
+ /* If GPS is more than this distance on any dimension then wait a few updates */
+ /* and reinitialize there */
+#define INSGPS_GPS_FAR     10 
+
+//! Contains the data from the mag sensor chip
+extern struct mag_sensor mag_data;
+
+//! Contains the data from the accelerometer
+extern struct accel_sensor  accel_data;
+
+//! Contains the data from the gyro
+extern struct gyro_sensor gyro_data;
+
+//! Conains the current estimate of the attitude
+extern struct attitude_solution attitude_data;
+
+//! Contains data from the altitude sensor
+extern struct altitude_sensor altitude_data;
+
+//! Contains data from the GPS (via the SPI link)
+extern struct gps_sensor gps_data;
+
+//! Offset correction of barometric alt, to match gps data
+static float baro_offset = 0;
+
+extern void send_calibration(void);
+extern void send_attitude(void);
+extern void send_velocity(void);
+extern void send_position(void);
+extern volatile int8_t ahrs_algorithm;
+extern void get_accel_gyro_data();
+extern void get_mag_data();
+
+/* INS functions */
+/**
+ * @brief Update the EKF when in outdoor mode.  The primary difference is using the GPS values.
+ */
+uint32_t total_far_count = 0;
+uint32_t relocated = 0;
+void ins_outdoor_update()
+{
+	static uint32_t ins_last_time;
+	float gyro[3], accel[3], vel[3];
+	float dT;
+	uint16_t sensors;
+	static uint32_t gps_far_count = 0;
+	
+	dT = PIOS_DELAY_DiffuS(ins_last_time) / 1e6;
+	ins_last_time = PIOS_DELAY_GetRaw();
+	
+	// This should only happen at start up or at mode switches
+	if(dT > 0.01)
+		dT = 0.01;
+		
+	// format data for INS algo
+	gyro[0] = gyro_data.filtered.x;
+	gyro[1] = gyro_data.filtered.y;
+	gyro[2] = gyro_data.filtered.z;
+	accel[0] = accel_data.filtered.x,
+	accel[1] = accel_data.filtered.y,
+	accel[2] = accel_data.filtered.z,
+	
+	INSStatePrediction(gyro, accel, dT);
+	attitude_data.quaternion.q1 = Nav.q[0];
+	attitude_data.quaternion.q2 = Nav.q[1];
+	attitude_data.quaternion.q3 = Nav.q[2];
+	attitude_data.quaternion.q4 = Nav.q[3];
+	send_attitude();  // get message out quickly
+	INSCovariancePrediction(dT);
+
+	PositionActualData positionActual;
+	PositionActualGet(&positionActual);
+	positionActual.North = Nav.Pos[0];
+	positionActual.East = Nav.Pos[1];
+	positionActual.Down = Nav.Pos[2];
+	PositionActualSet(&positionActual);
+	
+	VelocityActualData velocityActual;
+	VelocityActualGet(&velocityActual);
+	velocityActual.North = Nav.Vel[0];
+	velocityActual.East = Nav.Vel[1];
+	velocityActual.Down = Nav.Vel[2];
+	VelocityActualSet(&velocityActual);
+
+	sensors = 0;
+	
+	if (gps_data.updated)
+	{
+		vel[0] = gps_data.groundspeed * cos(gps_data.heading * DEG_TO_RAD);
+		vel[1] = gps_data.groundspeed * sin(gps_data.heading * DEG_TO_RAD);
+		vel[2] = 0;
+		
+		if (abs(gps_data.NED[0] - Nav.Pos[0]) > INSGPS_GPS_FAR ||
+		    abs(gps_data.NED[1] - Nav.Pos[1]) > INSGPS_GPS_FAR ||
+		    abs(gps_data.NED[2] - Nav.Pos[2]) > INSGPS_GPS_FAR ||
+		    abs(vel[0] - Nav.Vel[0]) > INSGPS_GPS_FAR ||
+		    abs(vel[1] - Nav.Vel[1]) > INSGPS_GPS_FAR) {
+			gps_far_count++;
+			total_far_count++;
+			gps_data.updated = false;
+			
+			if(gps_far_count > 30) {
+				INSPosVelReset(gps_data.NED,vel);
+				relocated++;
+			}
+		}
+		else {			
+			sensors |= HORIZ_SENSORS | POS_SENSORS;
+			
+			/*
+			 * When using gps need to make sure that barometer is brought into NED frame
+			 * we should try and see if the altitude from the home location is good enough
+			 * to use for the offset but for now starting with this conservative filter
+			 */
+			if(fabs(gps_data.NED[2] + (altitude_data.altitude - baro_offset)) > 10) {
+				baro_offset = gps_data.NED[2] + altitude_data.altitude;
+			} else {
+				/* IIR filter with 100 second or so tau to keep them crudely in the same frame */
+				baro_offset = baro_offset * 0.999 + (gps_data.NED[2] + altitude_data.altitude) * 0.001;
+			}
+			gps_data.updated = false;
+		}
+	}
+	
+	if(mag_data.updated) {
+		sensors |= MAG_SENSORS;
+		mag_data.updated = false;
+	}
+	
+	if(altitude_data.updated) {
+		sensors |= BARO_SENSOR;
+		altitude_data.updated = false;
+	}
+	
+	/*
+	 * TODO: Need to add a general sanity check for all the inputs to make sure their kosher
+	 * although probably should occur within INS itself
+	 */
+	INSCorrection(mag_data.scaled.axis, gps_data.NED, vel, altitude_data.altitude - baro_offset, sensors);
+	
+	if(fabs(Nav.gyro_bias[0]) > 0.1 || fabs(Nav.gyro_bias[1]) > 0.1 || fabs(Nav.gyro_bias[2]) > 0.1) {
+		float zeros[3] = {0,0,0};
+		INSSetGyroBias(zeros);
+	}
+}
+
+/**
+ * @brief Update the EKF when in indoor mode
+ */
+void ins_indoor_update()
+{
+	static uint32_t updated_without_gps = 0;
+	
+	float gyro[3], accel[3];
+	float zeros[3] = {0, 0, 0};
+	static uint32_t ins_last_time = 0;
+	uint16_t sensors = 0;
+	float dT;
+
+	dT = PIOS_DELAY_DiffuS(ins_last_time) / 1e6;
+	ins_last_time = PIOS_DELAY_GetRaw();
+	
+	// This should only happen at start up or at mode switches
+	if(dT > 0.01)
+		dT = 0.01;
+
+	// format data for INS algo
+	gyro[0] = gyro_data.filtered.x;
+	gyro[1] = gyro_data.filtered.y;
+	gyro[2] = gyro_data.filtered.z;
+	accel[0] = accel_data.filtered.x,
+	accel[1] = accel_data.filtered.y,
+	accel[2] = accel_data.filtered.z,
+	
+	INSStatePrediction(gyro, accel, dT);
+	attitude_data.quaternion.q1 = Nav.q[0];
+	attitude_data.quaternion.q2 = Nav.q[1];
+	attitude_data.quaternion.q3 = Nav.q[2];
+	attitude_data.quaternion.q4 = Nav.q[3];
+	send_attitude();  // get message out quickly
+	INSCovariancePrediction(dT);
+	
+	/* Indoors, update with zero position and velocity and high covariance */
+	sensors = HORIZ_SENSORS | VERT_SENSORS;
+	
+	if(mag_data.updated && (ahrs_algorithm == INSSETTINGS_ALGORITHM_INSGPS_INDOOR)) {
+		sensors |= MAG_SENSORS;
+		mag_data.updated = false;
+	}
+	
+	if(altitude_data.updated) {
+		sensors |= BARO_SENSOR;
+		altitude_data.updated = false;
+	}
+	
+	if(gps_data.updated) {
+		PositionActualData positionActual;
+		PositionActualGet(&positionActual);
+		positionActual.North = gps_data.NED[0];
+		positionActual.East = gps_data.NED[1];
+		positionActual.Down = Nav.Pos[2];
+		PositionActualSet(&positionActual);
+		
+		VelocityActualData velocityActual;
+		VelocityActualGet(&velocityActual);
+		velocityActual.North = gps_data.groundspeed * cos(gps_data.heading * DEG_TO_RAD);
+		velocityActual.East = gps_data.groundspeed * sin(gps_data.heading * DEG_TO_RAD);
+		velocityActual.Down = Nav.Vel[2];
+		VelocityActualSet(&velocityActual);
+		
+		updated_without_gps = 0;
+		gps_data.updated = false;
+	} else {
+		PositionActualData positionActual;
+		PositionActualGet(&positionActual);
+
+		VelocityActualData velocityActual;
+		VelocityActualGet(&velocityActual);
+
+		positionActual.Down = Nav.Pos[2];
+		velocityActual.Down = Nav.Vel[2];
+
+		if(updated_without_gps > 500) {
+			// After 2-3 seconds without a GPS update set velocity estimate to NAN
+			positionActual.North = NAN;
+			positionActual.East = NAN;
+			velocityActual.North = NAN;
+			velocityActual.East = NAN;
+		} else
+			updated_without_gps++;
+
+		PositionActualSet(&positionActual);
+		VelocityActualSet(&velocityActual);
+	}
+	
+	/*
+	 * TODO: Need to add a general sanity check for all the inputs to make sure their kosher
+	 * although probably should occur within INS itself
+	 */
+	INSCorrection(mag_data.scaled.axis, zeros, zeros, altitude_data.altitude, sensors);
+	
+	if(fabs(Nav.gyro_bias[0]) > 0.1 || fabs(Nav.gyro_bias[1]) > 0.1 || fabs(Nav.gyro_bias[2]) > 0.1) {
+		float zeros[3] = {0,0,0};
+		INSSetGyroBias(zeros);
+	}
+
+}
+
+/**
+ * @brief Initialize the EKF assuming stationary
+ */
+bool inited = false;
+float init_q[4];
+void ins_init_algorithm()
+{
+	inited = true;
+	float Rbe[3][3], q[4], accels[3], rpy[3], mag;
+	float ge[3]={0,0,-9.81}, zeros[3]={0,0,0}, Pdiag[16]={25,25,25,5,5,5,1e-5,1e-5,1e-5,1e-5,1e-5,1e-5,1e-5,1e-4,1e-4,1e-4};
+	bool using_mags, using_gps;
+	
+	INSGPSInit();
+	
+	HomeLocationData home;
+	HomeLocationGet(&home);
+	
+	accels[0]=accel_data.filtered.x;
+	accels[1]=accel_data.filtered.y;
+	accels[2]=accel_data.filtered.z;
+	
+	using_mags = (ahrs_algorithm == INSSETTINGS_ALGORITHM_INSGPS_OUTDOOR) || (ahrs_algorithm == INSSETTINGS_ALGORITHM_INSGPS_INDOOR);
+	using_mags &= (home.Be[0] != 0) || (home.Be[1] != 0) || (home.Be[2] != 0);  /* only use mags when valid home location */
+	
+	using_gps = (ahrs_algorithm == INSSETTINGS_ALGORITHM_INSGPS_OUTDOOR) && (gps_data.quality >= INSGPS_GPS_MINSAT);
+	
+	/* Block till a data update */
+	get_accel_gyro_data();
+	
+	/* Ensure we get mag data in a timely manner */
+	uint16_t fail_count = 50; // 50 at 200 Hz is up to 0.25 sec
+	while(using_mags && !mag_data.updated && fail_count--) {
+		get_mag_data();
+		get_accel_gyro_data();
+		AhrsPoll();
+		PIOS_DELAY_WaituS(2000);
+	}
+	using_mags &= mag_data.updated;
+	
+	if (using_mags) {		
+		RotFrom2Vectors(accels, ge, mag_data.scaled.axis, home.Be, Rbe);
+		R2Quaternion(Rbe,q);
+		if (using_gps)
+			INSSetState(gps_data.NED, zeros, q, zeros, zeros);
+		else
+			INSSetState(zeros, zeros, q, zeros, zeros);
+	} else {
+		// assume yaw = 0
+		mag = VectorMagnitude(accels);
+		rpy[1] = asinf(-accels[0]/mag);
+		rpy[0] = atan2(accels[1]/mag,accels[2]/mag);
+		rpy[2] = 0;
+		RPY2Quaternion(rpy,init_q);
+		if (using_gps)
+			INSSetState(gps_data.NED, zeros, init_q, zeros, zeros);
+		else {
+			for (uint32_t i = 0; i < 5; i++) {
+				INSSetState(zeros, zeros, init_q, zeros, zeros);
+				ins_indoor_update();
+			}
+		}
+	}
+	
+	INSResetP(Pdiag);
+	
+	// TODO: include initial estimate of gyro bias?
+}
diff --git a/flight/Revolution/pios_board.c b/flight/Revolution/pios_board.c
new file mode 100644
index 000000000..ed0a3e0c2
--- /dev/null
+++ b/flight/Revolution/pios_board.c
@@ -0,0 +1,794 @@
+/**
+ ******************************************************************************
+ * @addtogroup PIOS PIOS Core hardware abstraction layer
+ * @{
+ * @addtogroup PIOS_HMC5883 HMC5883 Functions
+ * @brief Deals with the hardware interface to the magnetometers
+ * @{
+ *
+ * @file       pios_board.c
+ * @author     David "Buzz" Carlson (buzz@chebuzz.com)
+ * 				The OpenPilot Team, http://www.openpilot.org Copyright (C) 2011.
+ * @brief      Defines board specific static initializers for hardware for the INS board.
+ * @see        The GNU Public License (GPL) Version 3
+ *
+ *****************************************************************************/
+/* 
+ * This program is free software; you can redistribute it and/or modify 
+ * it under the terms of the GNU General Public License as published by 
+ * the Free Software Foundation; either version 3 of the License, or 
+ * (at your option) any later version.
+ * 
+ * This program is distributed in the hope that it will be useful, but 
+ * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY 
+ * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License 
+ * for more details.
+ * 
+ * You should have received a copy of the GNU General Public License along 
+ * with this program; if not, write to the Free Software Foundation, Inc., 
+ * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
+ */
+
+#include <pios.h>
+
+#if defined(PIOS_INCLUDE_SPI)
+
+#include <pios_spi_priv.h>
+
+/* SPI2 Interface
+ *      - Used for mainboard communications
+ *
+ * NOTE: Leave this declared as const data so that it ends up in the 
+ * .rodata section (ie. Flash) rather than in the .bss section (RAM).
+ */
+void PIOS_SPI_op_irq_handler(void);
+void DMA1_Stream3_IRQHandler(void) __attribute__((alias("PIOS_SPI_op_irq_handler")));
+void DMA1_Stream4_IRQHandler(void) __attribute__((alias("PIOS_SPI_op_irq_handler")));
+static const struct pios_spi_cfg pios_spi_op_cfg = {
+	.regs = SPI2,
+	.remap = GPIO_AF_SPI2,
+	.init = {
+		.SPI_Mode = SPI_Mode_Slave,
+		.SPI_Direction = SPI_Direction_2Lines_FullDuplex,
+		.SPI_DataSize = SPI_DataSize_8b,
+		.SPI_NSS = SPI_NSS_Hard,
+		.SPI_FirstBit = SPI_FirstBit_MSB,
+		.SPI_CRCPolynomial = 7,
+		.SPI_CPOL = SPI_CPOL_High,
+		.SPI_CPHA = SPI_CPHA_2Edge,
+	},
+	.use_crc = true,
+	.dma = {
+		.irq = {
+			// Note this is the stream ID that triggers interrupts (in this case RX)
+			.flags = (DMA_IT_TCIF3 | DMA_IT_TEIF3 | DMA_IT_HTIF3),
+			.init = {
+				.NVIC_IRQChannel = DMA1_Stream3_IRQn,
+				.NVIC_IRQChannelPreemptionPriority = PIOS_IRQ_PRIO_HIGH,
+				.NVIC_IRQChannelSubPriority = 0,
+				.NVIC_IRQChannelCmd = ENABLE,
+			},
+		},
+		
+		.rx = {
+			.channel = DMA1_Stream3,
+			.init = {
+				.DMA_Channel            = DMA_Channel_0,
+				.DMA_PeripheralBaseAddr = (uint32_t) & (SPI2->DR),
+				.DMA_DIR                = DMA_DIR_PeripheralToMemory,
+				.DMA_PeripheralInc      = DMA_PeripheralInc_Disable,
+				.DMA_MemoryInc          = DMA_MemoryInc_Enable,
+				.DMA_PeripheralDataSize = DMA_PeripheralDataSize_Byte,
+				.DMA_MemoryDataSize     = DMA_MemoryDataSize_Byte,
+				.DMA_Mode               = DMA_Mode_Normal,
+				.DMA_Priority           = DMA_Priority_Medium,
+				//TODO: Enable FIFO
+				.DMA_FIFOMode           = DMA_FIFOMode_Disable,
+                .DMA_FIFOThreshold      = DMA_FIFOThreshold_Full,
+                .DMA_MemoryBurst        = DMA_MemoryBurst_Single,
+                .DMA_PeripheralBurst    = DMA_PeripheralBurst_Single,
+			},
+		},
+		.tx = {
+			.channel = DMA1_Stream4,
+			.init = {
+				.DMA_Channel            = DMA_Channel_0,
+				.DMA_PeripheralBaseAddr = (uint32_t) & (SPI2->DR),
+				.DMA_DIR                = DMA_DIR_MemoryToPeripheral,
+				.DMA_PeripheralInc      = DMA_PeripheralInc_Disable,
+				.DMA_MemoryInc          = DMA_MemoryInc_Enable,
+				.DMA_PeripheralDataSize = DMA_PeripheralDataSize_Byte,
+				.DMA_MemoryDataSize     = DMA_MemoryDataSize_Byte,
+				.DMA_Mode               = DMA_Mode_Normal,
+				.DMA_Priority           = DMA_Priority_Medium,
+				.DMA_FIFOMode           = DMA_FIFOMode_Disable,
+                .DMA_FIFOThreshold      = DMA_FIFOThreshold_Full,
+                .DMA_MemoryBurst        = DMA_MemoryBurst_Single,
+                .DMA_PeripheralBurst    = DMA_PeripheralBurst_Single,
+			},
+		},
+	},
+	.sclk = {
+		.gpio = GPIOB,
+		.init = {
+			.GPIO_Pin = GPIO_Pin_13,
+			.GPIO_Speed = GPIO_Speed_100MHz,
+			.GPIO_Mode = GPIO_Mode_AF,
+			.GPIO_OType = GPIO_OType_PP,
+			.GPIO_PuPd = GPIO_PuPd_NOPULL
+		},
+	},
+	.miso = {
+		.gpio = GPIOB,
+		.init = {
+			.GPIO_Pin = GPIO_Pin_14,
+			.GPIO_Speed = GPIO_Speed_50MHz,
+			.GPIO_Mode = GPIO_Mode_AF,
+			.GPIO_OType = GPIO_OType_PP,
+			.GPIO_PuPd = GPIO_PuPd_NOPULL
+		},
+	},
+	.mosi = {
+		.gpio = GPIOB,
+		.init = {
+			.GPIO_Pin = GPIO_Pin_15,
+			.GPIO_Speed = GPIO_Speed_50MHz,
+			.GPIO_Mode = GPIO_Mode_AF,
+			.GPIO_OType = GPIO_OType_PP,
+			.GPIO_PuPd = GPIO_PuPd_NOPULL
+		},
+	},
+	.slave_count = 1,
+	.ssel = { {
+		.gpio = GPIOB,
+		.init = {
+			.GPIO_Pin = GPIO_Pin_12,
+			.GPIO_Speed = GPIO_Speed_50MHz,
+			.GPIO_Mode = GPIO_Mode_IN,
+			.GPIO_OType = GPIO_OType_PP,
+			.GPIO_PuPd = GPIO_PuPd_UP
+		},
+	} },
+};
+
+uint32_t pios_spi_op_id;
+void PIOS_SPI_op_irq_handler(void)
+{
+	/* Call into the generic code to handle the IRQ for this specific device */
+	PIOS_SPI_IRQ_Handler(pios_spi_op_id);
+}
+
+/* SPI1 Interface
+ *    - Used for BMA180 accelerometer
+ */
+void PIOS_SPI_accel_irq_handler(void);
+void DMA2_Stream0_IRQHandler(void) __attribute__((alias("PIOS_SPI_accel_irq_handler")));
+void DMA2_Stream3_IRQHandler(void) __attribute__((alias("PIOS_SPI_accel_irq_handler")));
+static const struct pios_spi_cfg pios_spi_accel_cfg = {
+	.regs = SPI1,
+	.remap = GPIO_AF_SPI1,
+	.init   = {
+		.SPI_Mode              = SPI_Mode_Master,
+		.SPI_Direction         = SPI_Direction_2Lines_FullDuplex,
+		.SPI_DataSize          = SPI_DataSize_8b,
+		.SPI_NSS               = SPI_NSS_Soft,
+		.SPI_FirstBit          = SPI_FirstBit_MSB,
+		.SPI_CRCPolynomial     = 7,
+		.SPI_CPOL              = SPI_CPOL_High,
+		.SPI_CPHA              = SPI_CPHA_2Edge,
+		.SPI_BaudRatePrescaler = SPI_BaudRatePrescaler_4,
+	},
+	.use_crc = false,
+	.dma = {
+		.irq = {
+			.flags   = (DMA_IT_TCIF0 | DMA_IT_TEIF0 | DMA_IT_HTIF0),
+			.init    = {
+				.NVIC_IRQChannel                   = DMA2_Stream0_IRQn,
+				.NVIC_IRQChannelPreemptionPriority = PIOS_IRQ_PRIO_HIGH,
+				.NVIC_IRQChannelSubPriority        = 0,
+				.NVIC_IRQChannelCmd                = ENABLE,
+			},
+		},
+		
+		.rx = {
+			.channel = DMA2_Stream0,
+			.init    = {
+                .DMA_Channel            = DMA_Channel_3,
+				.DMA_PeripheralBaseAddr = (uint32_t)&(SPI1->DR),
+				.DMA_DIR                = DMA_DIR_PeripheralToMemory,
+				.DMA_PeripheralInc      = DMA_PeripheralInc_Disable,
+				.DMA_MemoryInc          = DMA_MemoryInc_Enable,
+				.DMA_PeripheralDataSize = DMA_PeripheralDataSize_Byte,
+				.DMA_MemoryDataSize     = DMA_MemoryDataSize_Byte,
+				.DMA_Mode               = DMA_Mode_Normal,
+				.DMA_Priority           = DMA_Priority_Medium,
+				.DMA_FIFOMode           = DMA_FIFOMode_Disable,
+                /* .DMA_FIFOThreshold */
+                .DMA_MemoryBurst        = DMA_MemoryBurst_Single,
+                .DMA_PeripheralBurst    = DMA_PeripheralBurst_Single,
+			},
+		},
+		.tx = {
+			.channel = DMA2_Stream3,
+			.init    = {
+                .DMA_Channel            = DMA_Channel_3,
+				.DMA_PeripheralBaseAddr = (uint32_t)&(SPI1->DR),
+				.DMA_DIR                = DMA_DIR_MemoryToPeripheral,
+				.DMA_PeripheralInc      = DMA_PeripheralInc_Disable,
+				.DMA_MemoryInc          = DMA_MemoryInc_Enable,
+				.DMA_PeripheralDataSize = DMA_PeripheralDataSize_Byte,
+				.DMA_MemoryDataSize     = DMA_MemoryDataSize_Byte,
+				.DMA_Mode               = DMA_Mode_Normal,
+				.DMA_Priority           = DMA_Priority_High,
+				.DMA_FIFOMode           = DMA_FIFOMode_Disable,
+                /* .DMA_FIFOThreshold */
+                .DMA_MemoryBurst        = DMA_MemoryBurst_Single,
+                .DMA_PeripheralBurst    = DMA_PeripheralBurst_Single,
+			},
+		},
+	},
+	.sclk = {
+		.gpio = GPIOA,
+		.init = {
+			.GPIO_Pin   = GPIO_Pin_5,
+			.GPIO_Speed = GPIO_Speed_100MHz,
+			.GPIO_Mode  = GPIO_Mode_AF,
+			.GPIO_OType = GPIO_OType_PP,
+			.GPIO_PuPd = GPIO_PuPd_UP
+		},
+	},
+	.miso = {
+		.gpio = GPIOA,
+		.init = {
+			.GPIO_Pin   = GPIO_Pin_6,
+			.GPIO_Speed = GPIO_Speed_50MHz,
+			.GPIO_Mode  = GPIO_Mode_AF,
+			.GPIO_OType = GPIO_OType_PP,
+			.GPIO_PuPd = GPIO_PuPd_UP
+		},
+	},
+	.mosi = {
+		.gpio = GPIOA,
+		.init = {
+			.GPIO_Pin   = GPIO_Pin_7,
+			.GPIO_Speed = GPIO_Speed_50MHz,
+			.GPIO_Mode  = GPIO_Mode_AF,
+			.GPIO_OType = GPIO_OType_PP,
+			.GPIO_PuPd = GPIO_PuPd_UP
+		},
+	},
+	.slave_count = 1,
+	.ssel = { {
+		.gpio = GPIOA,
+		.init = {
+			.GPIO_Pin   = GPIO_Pin_4,
+			.GPIO_Speed = GPIO_Speed_50MHz,
+			.GPIO_Mode  = GPIO_Mode_OUT,
+			.GPIO_OType = GPIO_OType_PP,
+			.GPIO_PuPd = GPIO_PuPd_UP
+		},
+	} },
+};
+
+static uint32_t pios_spi_accel_id;
+void PIOS_SPI_accel_irq_handler(void)
+{
+	/* Call into the generic code to handle the IRQ for this specific device */
+	PIOS_SPI_IRQ_Handler(pios_spi_accel_id);
+}
+
+#endif /* PIOS_INCLUDE_SPI */
+
+
+
+#if defined(PIOS_INCLUDE_GPS)
+#include <pios_usart_priv.h>
+
+/*
+ * GPS USART
+ */
+static const struct pios_usart_cfg pios_usart_gps_cfg = {
+	.regs = USART1,
+	.remap = GPIO_AF_USART1,
+	.init = {
+		.USART_BaudRate = 57600,
+		.USART_WordLength = USART_WordLength_8b,
+		.USART_Parity = USART_Parity_No,
+		.USART_StopBits = USART_StopBits_1,
+		.USART_HardwareFlowControl =
+		USART_HardwareFlowControl_None,
+		.USART_Mode = USART_Mode_Rx | USART_Mode_Tx,
+	},
+	.irq = {
+		.init = {
+			.NVIC_IRQChannel = USART1_IRQn,
+			.NVIC_IRQChannelPreemptionPriority = PIOS_IRQ_PRIO_MID,
+			.NVIC_IRQChannelSubPriority = 0,
+			.NVIC_IRQChannelCmd = ENABLE,
+		},
+	},
+	.rx = {
+		.gpio = GPIOA,
+		.init = {
+			.GPIO_Pin   = GPIO_Pin_10,
+			.GPIO_Speed = GPIO_Speed_2MHz,
+			.GPIO_Mode  = GPIO_Mode_AF,
+			.GPIO_OType = GPIO_OType_PP,
+			.GPIO_PuPd  = GPIO_PuPd_UP
+		},
+	},
+	.tx = {
+		.gpio = GPIOA,
+		.init = {
+			.GPIO_Pin   = GPIO_Pin_9,
+			.GPIO_Speed = GPIO_Speed_2MHz,
+			.GPIO_Mode  = GPIO_Mode_AF,
+			.GPIO_OType = GPIO_OType_PP,
+			.GPIO_PuPd  = GPIO_PuPd_UP
+		},
+	},
+};
+
+#define PIOS_COM_AUX_TX_BUF_LEN 150
+static uint8_t pios_com_aux_tx_buffer[PIOS_COM_AUX_TX_BUF_LEN];
+#define PIOS_COM_AUX_RX_BUF_LEN 10
+static uint8_t pios_com_aux_rx_buffer[PIOS_COM_AUX_RX_BUF_LEN];
+
+#endif /* PIOS_INCLUDE_GPS */
+
+#ifdef PIOS_INCLUDE_COM_AUX
+/*
+ * AUX USART
+ */
+static const struct pios_usart_cfg pios_usart_aux_cfg = {
+	.regs = UART4,
+	.remap = GPIO_AF_UART4,
+	.init = {
+		.USART_BaudRate = 230400,
+		.USART_WordLength = USART_WordLength_8b,
+		.USART_Parity = USART_Parity_No,
+		.USART_StopBits = USART_StopBits_1,
+		.USART_HardwareFlowControl =
+		USART_HardwareFlowControl_None,
+		.USART_Mode = USART_Mode_Rx | USART_Mode_Tx,
+	},
+	.irq = {
+		.init = {
+			.NVIC_IRQChannel = UART4_IRQn,
+			.NVIC_IRQChannelPreemptionPriority = PIOS_IRQ_PRIO_HIGH,
+			.NVIC_IRQChannelSubPriority = 0,
+			.NVIC_IRQChannelCmd = ENABLE,
+		},
+	},
+	.rx = {
+		.gpio = GPIOC,
+		.init = {
+			.GPIO_Pin = GPIO_Pin_11,
+			.GPIO_Speed = GPIO_Speed_2MHz,
+			.GPIO_Mode  = GPIO_Mode_AF,
+			.GPIO_OType = GPIO_OType_PP,
+			.GPIO_PuPd  = GPIO_PuPd_UP
+		},
+	},
+	.tx = {
+		.gpio = GPIOC,
+		.init = {
+			.GPIO_Pin = GPIO_Pin_10,
+			.GPIO_Speed = GPIO_Speed_2MHz,
+			.GPIO_Mode  = GPIO_Mode_AF,
+			.GPIO_OType = GPIO_OType_PP,
+			.GPIO_PuPd  = GPIO_PuPd_UP
+		},
+	},
+};
+
+#define PIOS_COM_GPS_TX_BUF_LEN 10
+static uint8_t pios_com_gps_tx_buffer[PIOS_COM_GPS_TX_BUF_LEN];
+#define PIOS_COM_GPS_RX_BUF_LEN 192
+static uint8_t pios_com_gps_rx_buffer[PIOS_COM_GPS_RX_BUF_LEN];
+
+#endif /* PIOS_COM_AUX */
+
+
+#if defined(PIOS_INCLUDE_COM)
+
+#include <pios_com_priv.h>
+
+#endif /* PIOS_INCLUDE_COM */
+
+#if defined(PIOS_INCLUDE_I2C)
+
+#include <pios_i2c_priv.h>
+
+/*
+ * I2C Adapters
+ */
+void PIOS_I2C_pres_mag_adapter_ev_irq_handler(void);
+void PIOS_I2C_pres_mag_adapter_er_irq_handler(void);
+void I2C1_EV_IRQHandler()
+__attribute__ ((alias("PIOS_I2C_pres_mag_adapter_ev_irq_handler")));
+void I2C1_ER_IRQHandler()
+__attribute__ ((alias("PIOS_I2C_pres_mag_adapter_er_irq_handler")));
+
+static const struct pios_i2c_adapter_cfg pios_i2c_pres_mag_adapter_cfg = {
+	.regs = I2C1,
+	.remap = GPIO_AF_I2C1,
+	.init = {
+		.I2C_Mode = I2C_Mode_I2C,
+		.I2C_OwnAddress1 = 0,
+		.I2C_Ack = I2C_Ack_Enable,
+		.I2C_AcknowledgedAddress = I2C_AcknowledgedAddress_7bit,
+		.I2C_DutyCycle = I2C_DutyCycle_2,
+		.I2C_ClockSpeed = 400000,	/* bits/s */
+	},
+	.transfer_timeout_ms = 50,
+	.scl = {
+		.gpio = GPIOB,
+		.init = {
+			.GPIO_Pin = GPIO_Pin_6,
+            .GPIO_Mode  = GPIO_Mode_AF,
+            .GPIO_Speed = GPIO_Speed_50MHz,
+            .GPIO_OType = GPIO_OType_OD,
+            .GPIO_PuPd  = GPIO_PuPd_NOPULL,
+		},
+	},
+	.sda = {
+		.gpio = GPIOB,
+		.init = {
+			.GPIO_Pin = GPIO_Pin_7,
+            .GPIO_Mode  = GPIO_Mode_AF,
+            .GPIO_Speed = GPIO_Speed_50MHz,
+            .GPIO_OType = GPIO_OType_OD,
+            .GPIO_PuPd  = GPIO_PuPd_NOPULL,
+		},
+	},
+	.event = {
+		.flags = 0,	/* FIXME: check this */
+		.init = {
+			.NVIC_IRQChannel = I2C1_EV_IRQn,
+			.NVIC_IRQChannelPreemptionPriority = PIOS_IRQ_PRIO_HIGHEST,
+			.NVIC_IRQChannelSubPriority = 0,
+			.NVIC_IRQChannelCmd = ENABLE,
+		},
+	},
+	.error = {
+		.flags = 0,	/* FIXME: check this */
+		.init = {
+			.NVIC_IRQChannel = I2C1_ER_IRQn,
+			.NVIC_IRQChannelPreemptionPriority = PIOS_IRQ_PRIO_HIGHEST,
+			.NVIC_IRQChannelSubPriority = 0,
+			.NVIC_IRQChannelCmd = ENABLE,
+		},
+	},
+};
+
+uint32_t pios_i2c_pres_mag_adapter_id;
+void PIOS_I2C_pres_mag_adapter_ev_irq_handler(void)
+{
+	/* Call into the generic code to handle the IRQ for this specific device */
+	PIOS_I2C_EV_IRQ_Handler(pios_i2c_pres_mag_adapter_id);
+}
+
+void PIOS_I2C_pres_mag_adapter_er_irq_handler(void)
+{
+	/* Call into the generic code to handle the IRQ for this specific device */
+	PIOS_I2C_ER_IRQ_Handler(pios_i2c_pres_mag_adapter_id);
+}
+
+
+void PIOS_I2C_gyro_adapter_ev_irq_handler(void);
+void PIOS_I2C_gyro_adapter_er_irq_handler(void);
+void I2C2_EV_IRQHandler() __attribute__ ((alias ("PIOS_I2C_gyro_adapter_ev_irq_handler")));
+void I2C2_ER_IRQHandler() __attribute__ ((alias ("PIOS_I2C_gyro_adapter_er_irq_handler")));
+
+static const struct pios_i2c_adapter_cfg pios_i2c_gyro_adapter_cfg = {
+	.regs = I2C2,
+	.remap = GPIO_AF_I2C2,
+	.init = {
+		.I2C_Mode                = I2C_Mode_I2C,
+		.I2C_OwnAddress1         = 0,
+		.I2C_Ack                 = I2C_Ack_Enable,
+		.I2C_AcknowledgedAddress = I2C_AcknowledgedAddress_7bit,
+		.I2C_DutyCycle           = I2C_DutyCycle_2,
+		.I2C_ClockSpeed          = 400000,	/* bits/s */
+	},
+	.transfer_timeout_ms = 50,
+	.scl = {
+		.gpio = GPIOB,
+		.init = {
+			.GPIO_Pin   = GPIO_Pin_10,
+            .GPIO_Mode  = GPIO_Mode_AF,
+            .GPIO_Speed = GPIO_Speed_50MHz,
+            .GPIO_OType = GPIO_OType_OD,
+            .GPIO_PuPd  = GPIO_PuPd_NOPULL,
+		},
+	},
+	.sda = {
+		.gpio = GPIOB,
+		.init = {
+			.GPIO_Pin   = GPIO_Pin_11,
+            .GPIO_Mode  = GPIO_Mode_AF,
+            .GPIO_Speed = GPIO_Speed_50MHz,
+            .GPIO_OType = GPIO_OType_OD,
+            .GPIO_PuPd  = GPIO_PuPd_NOPULL,
+		},
+	},
+	.event = {
+		.flags   = 0,		/* FIXME: check this */
+		.init = {
+			.NVIC_IRQChannel                   = I2C2_EV_IRQn,
+			.NVIC_IRQChannelPreemptionPriority = PIOS_IRQ_PRIO_HIGHEST,
+			.NVIC_IRQChannelSubPriority        = 0,
+			.NVIC_IRQChannelCmd                = ENABLE,
+		},
+	},
+	.error = {
+		.flags   = 0,		/* FIXME: check this */
+		.init = {
+			.NVIC_IRQChannel                   = I2C2_ER_IRQn,
+			.NVIC_IRQChannelPreemptionPriority = PIOS_IRQ_PRIO_HIGHEST,
+			.NVIC_IRQChannelSubPriority        = 0,
+			.NVIC_IRQChannelCmd                = ENABLE,
+		},
+	},
+};
+
+uint32_t pios_i2c_gyro_adapter_id;
+void PIOS_I2C_gyro_adapter_ev_irq_handler(void)
+{
+	/* Call into the generic code to handle the IRQ for this specific device */
+	PIOS_I2C_EV_IRQ_Handler(pios_i2c_gyro_adapter_id);
+}
+
+void PIOS_I2C_gyro_adapter_er_irq_handler(void)
+{
+	/* Call into the generic code to handle the IRQ for this specific device */
+	PIOS_I2C_ER_IRQ_Handler(pios_i2c_gyro_adapter_id);
+}
+
+#endif /* PIOS_INCLUDE_I2C */
+
+
+extern const struct pios_com_driver pios_usart_com_driver;
+
+uint32_t pios_com_aux_id;
+uint32_t pios_com_gps_id;
+
+
+/**
+ * Sensor configurations 
+ */
+#include "pios_hmc5883.h"
+static const struct pios_hmc5883_cfg pios_hmc5883_cfg = {
+	.drdy = {
+		.gpio = GPIOB,
+		.init = {
+			.GPIO_Pin = GPIO_Pin_8,
+			.GPIO_Speed = GPIO_Speed_100MHz,
+			.GPIO_Mode = GPIO_Mode_IN,
+			.GPIO_OType = GPIO_OType_OD,
+			.GPIO_PuPd = GPIO_PuPd_NOPULL,
+		},
+	},
+	.eoc_exti = {
+		.pin_source = EXTI_PinSource8,
+		.port_source = EXTI_PortSourceGPIOB,
+		.init = {
+			.EXTI_Line = EXTI_Line8, // matches above GPIO pin
+			.EXTI_Mode = EXTI_Mode_Interrupt,
+			.EXTI_Trigger = EXTI_Trigger_Rising,
+			.EXTI_LineCmd = ENABLE,
+		},
+	},
+	.eoc_irq = {
+		.init = {
+			.NVIC_IRQChannel = EXTI9_5_IRQn,
+			.NVIC_IRQChannelPreemptionPriority = PIOS_IRQ_PRIO_LOW,
+			.NVIC_IRQChannelSubPriority = 0,
+			.NVIC_IRQChannelCmd = ENABLE,
+		},
+	},
+	.M_ODR = PIOS_HMC5883_ODR_75,
+	.Meas_Conf = PIOS_HMC5883_MEASCONF_NORMAL,
+	.Gain = PIOS_HMC5883_GAIN_1_9,
+	.Mode = PIOS_HMC5883_MODE_CONTINUOUS,
+
+};
+
+#include "pios_bma180.h"
+static const struct pios_bma180_cfg pios_bma180_cfg = {
+	.drdy = {
+		.gpio = GPIOC,
+		.init = {
+			.GPIO_Pin = GPIO_Pin_4,
+			.GPIO_Speed = GPIO_Speed_100MHz,
+			.GPIO_Mode = GPIO_Mode_IN,
+			.GPIO_OType = GPIO_OType_OD,
+			.GPIO_PuPd = GPIO_PuPd_NOPULL,
+		},
+	},
+	.eoc_exti = {
+		.pin_source = EXTI_PinSource4,
+		.port_source = EXTI_PortSourceGPIOC,
+		.init = {
+			.EXTI_Line = EXTI_Line4, // matches above GPIO pin
+			.EXTI_Mode = EXTI_Mode_Interrupt,
+			.EXTI_Trigger = EXTI_Trigger_Rising,
+			.EXTI_LineCmd = ENABLE,
+		},
+	},
+	.eoc_irq = {
+		.init = {
+			.NVIC_IRQChannel = EXTI4_IRQn,
+			.NVIC_IRQChannelPreemptionPriority = PIOS_IRQ_PRIO_LOW,
+			.NVIC_IRQChannelSubPriority = 0,
+			.NVIC_IRQChannelCmd = ENABLE,
+		},
+	},
+};
+
+#include "pios_imu3000.h"
+static const struct pios_imu3000_cfg pios_imu3000_cfg = {
+	.drdy = {
+		.gpio = GPIOB,
+		.init = {
+			.GPIO_Pin = GPIO_Pin_1,
+			.GPIO_Speed = GPIO_Speed_100MHz,
+			.GPIO_Mode = GPIO_Mode_IN,
+			.GPIO_OType = GPIO_OType_OD,
+			.GPIO_PuPd = GPIO_PuPd_NOPULL,
+		},
+	},
+	.eoc_exti = {
+		.pin_source = EXTI_PinSource1,
+		.port_source = EXTI_PortSourceGPIOB,
+		.init = {
+			.EXTI_Line = EXTI_Line1, // matches above GPIO pin
+			.EXTI_Mode = EXTI_Mode_Interrupt,
+			.EXTI_Trigger = EXTI_Trigger_Rising,
+			.EXTI_LineCmd = ENABLE,
+		},
+	},
+	.eoc_irq = {
+		.init = {
+			.NVIC_IRQChannel = EXTI1_IRQn,
+			.NVIC_IRQChannelPreemptionPriority = PIOS_IRQ_PRIO_HIGH,
+			.NVIC_IRQChannelSubPriority = 0,
+			.NVIC_IRQChannelCmd = ENABLE,
+		},
+	},
+	.Fifo_store = PIOS_IMU3000_FIFO_TEMP_OUT | PIOS_IMU3000_FIFO_GYRO_X_OUT | PIOS_IMU3000_FIFO_GYRO_Y_OUT 
+	| PIOS_IMU3000_FIFO_GYRO_Z_OUT | PIOS_IMU3000_FIFO_FOOTER,
+	// Clock at 8 khz, downsampled by 4 for 2khz
+	.Smpl_rate_div = 7, 
+	.Interrupt_cfg = PIOS_IMU3000_INT_DATA_RDY | PIOS_IMU3000_INT_CLR_ANYRD,
+	.User_ctl = PIOS_IMU3000_USERCTL_FIFO_EN,
+	.Pwr_mgmt_clk = PIOS_IMU3000_PWRMGMT_PLL_X_CLK,
+	.range = PIOS_IMU3000_SCALE_500_DEG,
+	.filter = PIOS_IMU3000_LOWPASS_256_HZ
+
+};
+
+#include "pios_bmp085.h"
+static const struct pios_bmp085_cfg pios_bmp085_cfg = {
+	.drdy = {
+		.gpio = GPIOC,
+		.init = {
+			.GPIO_Pin = GPIO_Pin_2,
+			.GPIO_Speed = GPIO_Speed_100MHz,
+			.GPIO_Mode = GPIO_Mode_IN,
+			.GPIO_OType = GPIO_OType_OD,
+			.GPIO_PuPd = GPIO_PuPd_NOPULL,
+		},
+	},
+	.eoc_exti = {
+		.pin_source = EXTI_PinSource2,
+		.port_source = EXTI_PortSourceGPIOC,
+		.init = {
+			.EXTI_Line = EXTI_Line2, // matches above GPIO pin
+			.EXTI_Mode = EXTI_Mode_Interrupt,
+			.EXTI_Trigger = EXTI_Trigger_Rising,
+			.EXTI_LineCmd = ENABLE,
+		},
+	},
+	.eoc_irq = {
+		.init = {
+			.NVIC_IRQChannel = EXTI2_IRQn,
+			.NVIC_IRQChannelPreemptionPriority = PIOS_IRQ_PRIO_LOW,
+			.NVIC_IRQChannelSubPriority = 0,
+			.NVIC_IRQChannelCmd = ENABLE,
+		},
+	},
+	.xclr = {
+		.gpio = GPIOC,
+		.init = {
+			.GPIO_Pin = GPIO_Pin_1,
+			.GPIO_Speed = GPIO_Speed_100MHz,
+			.GPIO_Mode = GPIO_Mode_OUT,
+			.GPIO_OType = GPIO_OType_PP,
+			.GPIO_PuPd = GPIO_PuPd_NOPULL,
+		},
+	},
+	.oversampling = 3,
+};
+
+/**
+ * PIOS_Board_Init()
+ * initializes all the core subsystems on this specific hardware
+ * called from System/openpilot.c
+ */
+void PIOS_Board_Init(void) {
+	/* Brings up System using CMSIS functions, enables the LEDs. */
+	PIOS_SYS_Init();
+	
+	/* Delay system */
+	PIOS_DELAY_Init();
+
+#ifdef PIOS_DEBUG_ENABLE_DEBUG_PINS
+	PIOS_DEBUG_Init(&pios_tim_servo_all_channels, NELEMENTS(pios_tim_servo_all_channels));
+#endif	/* PIOS_DEBUG_ENABLE_DEBUG_PINS */
+	
+	
+	/* IAP System Setup */
+	PIOS_IAP_Init();
+	
+#if defined(PIOS_INCLUDE_COM)
+#if defined(PIOS_INCLUDE_GPS)
+	uint32_t pios_usart_gps_id;
+	if (PIOS_USART_Init(&pios_usart_gps_id, &pios_usart_gps_cfg)) {
+		PIOS_DEBUG_Assert(0);
+	}
+	if (PIOS_COM_Init(&pios_com_gps_id, &pios_usart_com_driver, pios_usart_gps_id,
+					  pios_com_gps_rx_buffer, sizeof(pios_com_gps_rx_buffer),
+					  pios_com_gps_tx_buffer, sizeof(pios_com_gps_tx_buffer))) {
+		PIOS_DEBUG_Assert(0);
+	}
+#endif	/* PIOS_INCLUDE_GPS */
+	
+#if defined(PIOS_INCLUDE_COM_AUX)
+	uint32_t pios_usart_aux_id;
+
+    if (PIOS_USART_Init(&pios_usart_aux_id, &pios_usart_aux_cfg)) {
+		PIOS_DEBUG_Assert(0);
+	}
+	if (PIOS_COM_Init(&pios_com_aux_id, &pios_usart_com_driver, pios_usart_aux_id,
+					  pios_com_aux_rx_buffer, sizeof(pios_com_aux_rx_buffer),
+					  pios_com_aux_tx_buffer, sizeof(pios_com_aux_tx_buffer))) {
+		PIOS_DEBUG_Assert(0);
+	}
+#endif	/* PIOS_INCLUDE_COM_AUX */
+#endif	/* PIOS_INCLUDE_COM */
+	
+	if (PIOS_I2C_Init(&pios_i2c_pres_mag_adapter_id, &pios_i2c_pres_mag_adapter_cfg)) {
+		PIOS_DEBUG_Assert(0);
+	}
+
+	if (PIOS_I2C_Init(&pios_i2c_gyro_adapter_id, &pios_i2c_gyro_adapter_cfg)) {
+		PIOS_DEBUG_Assert(0);
+	}
+	/* Set up the SPI interface to the accelerometer*/
+	if (PIOS_SPI_Init(&pios_spi_accel_id, &pios_spi_accel_cfg)) {
+		PIOS_DEBUG_Assert(0);
+	}
+	PIOS_BMA180_Attach(pios_spi_accel_id);
+	PIOS_BMA180_Init(&pios_bma180_cfg);
+	PIOS_IMU3000_Init(&pios_imu3000_cfg);
+	PIOS_HMC5883_Init(&pios_hmc5883_cfg);
+	PIOS_BMP085_Init(&pios_bmp085_cfg);
+	
+	
+	/* Set up the SPI interface to the OP board */
+#include "ahrs_spi_comm.h"
+	AhrsInitComms();
+	if (PIOS_SPI_Init(&pios_spi_op_id, &pios_spi_op_cfg)) {
+		PIOS_DEBUG_Assert(0);
+	}
+	
+	AhrsConnect(pios_spi_op_id);
+}
+
+/**
+ * @}
+ * @}
+ */
+
diff --git a/flight/Revolution/test.c b/flight/Revolution/test.c
new file mode 100644
index 000000000..27899741a
--- /dev/null
+++ b/flight/Revolution/test.c
@@ -0,0 +1,173 @@
+/**
+ ******************************************************************************
+ * @addtogroup INS INS
+
+ * @brief The INS Modules perform
+ *
+ * @{
+ * @addtogroup INS_Main
+ * @brief Main function which does the hardware dependent stuff
+ * @{
+ *
+ *
+ * @file       ins.c
+ * @author     David "Buzz" Carlson (buzz@chebuzz.com)
+ * 				The OpenPilot Team, http://www.openpilot.org Copyright (C) 2011.
+ * @brief      INSGPS Test Program
+ * @see        The GNU Public License (GPL) Version 3
+ *
+ *****************************************************************************/
+/*
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 3 of the License, or
+ * (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful, but
+ * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY
+ * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License
+ * for more details.
+ *
+ * You should have received a copy of the GNU General Public License along
+ * with this program; if not, write to the Free Software Foundation, Inc.,
+ * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
+ */
+
+/* OpenPilot Includes */
+#include "ins.h"
+#include "pios.h"
+#include "ahrs_spi_comm.h"
+#include <stdbool.h>
+#include "fifo_buffer.h"
+
+void reset_values();
+
+/**
+ * @addtogroup INS_Global_Data INS Global Data
+ * @{
+ * Public data.  Used by both EKF and the sender
+ */
+
+//! Contains the data from the mag sensor chip
+struct mag_sensor mag_data;
+
+//! Contains the data from the accelerometer
+struct accel_sensor  accel_data;
+
+//! Contains the data from the gyro
+struct gyro_sensor gyro_data;
+
+//! Conains the current estimate of the attitude
+struct attitude_solution attitude_data;
+
+//! Contains data from the altitude sensor
+struct altitude_sensor altitude_data;
+
+//! Contains data from the GPS (via the SPI link)
+struct gps_sensor gps_data;
+
+//! Offset correction of barometric alt, to match gps data
+//static float baro_offset = 0;
+
+//static float mag_len = 0;
+
+typedef enum { INS_IDLE, INS_DATA_READY, INS_PROCESSING } states;
+volatile int32_t ekf_too_slow;
+volatile int32_t total_conversion_blocks;
+
+/**
+ * @}
+ */
+
+/* INS functions */
+void blink(int led, int times)
+{
+	for(int i=0; i<times; i++)
+	{
+		PIOS_LED_Toggle(led);
+		PIOS_DELAY_WaitmS(250);
+		PIOS_LED_Toggle(led);
+		PIOS_DELAY_WaitmS(250);
+	}
+}
+
+
+void test_accel()
+{
+	if(PIOS_BMA180_Test() == 0)
+		blink(LED1, 1);
+	else
+		blink(LED2, 1);
+}
+
+#if defined (PIOS_INCLUDE_HMC5883)
+void test_mag()
+{
+	if(PIOS_HMC5883_Test() == 0)
+		blink(LED1, 2);
+	else
+		blink(LED2, 2);
+}
+#endif
+
+#if defined (PIOS_INCLUDE_BMP085)
+void test_pressure()
+{
+	if(PIOS_BMP085_Test() == 0)
+		blink(LED1, 3);
+	else
+		blink(LED2, 3);
+}
+#endif
+
+#if defined (PIOS_INCLUDE_IMU3000)
+void test_imu()
+{
+	if(PIOS_IMU3000_Test() == 0)
+		blink(LED1, 4);
+	else
+		blink(LED2, 4);
+}
+#endif
+
+
+extern void PIOS_Board_Init(void);
+struct pios_bma180_data bma180_data;
+
+/**
+ * @brief INS Main function
+ */
+int main()
+{
+
+	PIOS_Board_Init();
+
+	while (1)
+	{
+		test_accel();
+		PIOS_DELAY_WaitmS(1000);
+
+#if defined (PIOS_INCLUDE_HMC5883)
+		test_mag();
+		PIOS_DELAY_WaitmS(1000);
+#endif
+
+#if defined (PIOS_INCLUDE_BMP085)
+		test_pressure();
+		PIOS_DELAY_WaitmS(1000);
+#endif
+
+#if defined (PIOS_INCLUDE_IMU3000)
+		test_imu();
+		PIOS_DELAY_WaitmS(1000);
+#endif
+		PIOS_DELAY_WaitmS(3000);
+	}
+
+}
+
+
+/**
+ * @}
+ */
+
diff --git a/make/boards/esc/board-info.mk b/make/boards/esc/board-info.mk
new file mode 100755
index 000000000..00c5f6bc0
--- /dev/null
+++ b/make/boards/esc/board-info.mk
@@ -0,0 +1,20 @@
+BOARD_TYPE          := 0x08
+BOARD_REVISION      := 0x01
+BOOTLOADER_VERSION  := 0x01
+HW_TYPE             := 0x01
+
+MCU                 := cortex-m3
+CHIP                := STM32F103CBT
+BOARD               := STM32F103CB_ESC_Rev1
+MODEL               := MD
+MODEL_SUFFIX        :=
+
+OPENOCD_CONFIG      := stm32f1x.cfg 
+
+# Note: These must match the values in link_$(BOARD)_memory.ld
+BL_BANK_BASE        := 0x08000000  # Start of bootloader flash
+BL_BANK_SIZE        := 0x00003000  # Should include BD_INFO region
+FW_BANK_BASE        := 0x08003000  # Start of firmware flash
+FW_BANK_SIZE        := 0x0001D000  # Should include FW_DESC_SIZE
+
+FW_DESC_SIZE        := 0x00000064
diff --git a/make/boards/revolution/board-info.mk b/make/boards/revolution/board-info.mk
new file mode 100644
index 000000000..d36e054cc
--- /dev/null
+++ b/make/boards/revolution/board-info.mk
@@ -0,0 +1,23 @@
+BOARD_TYPE          := 0x05
+BOARD_REVISION      := 0x01
+BOOTLOADER_VERSION  := 0x00
+HW_TYPE             := 0x00
+
+MCU                 := cortex-m4
+CHIP                := STM32F205RGT
+BOARD               := STM32F2xx_INS
+MODEL               := HD
+MODEL_SUFFIX        := 
+
+OPENOCD_CONFIG      := stm32f4xx.cfg
+
+# Note: These must match the values in link_$(BOARD)_memory.ld
+BL_BANK_BASE        := 0x08000000  # Start of bootloader flash
+BL_BANK_SIZE        := 0x00008000  # Should include BD_INFO region
+FW_BANK_BASE        := 0x08008000  # Start of firmware flash
+FW_BANK_SIZE        := 0x0001E000  # Should include FW_DESC_SIZE
+
+FW_DESC_SIZE        := 0x00000064
+
+OSCILLATOR_FREQ     := 8000000
+SYSCLK_FREQ         := 168000000