Merge remote-tracking branch 'upstream/master'

170 files changed, 8929 insertions, 6939 deletions

diff --git a/src/modules/attitude_estimator_ekf/AttitudeEKF.m b/src/modules/attitude_estimator_ekf/AttitudeEKF.m
new file mode 100644
index 000000000..fea1a773e
--- /dev/null
+++ b/src/modules/attitude_estimator_ekf/AttitudeEKF.m
@@ -0,0 +1,298 @@
+function [xa_apo,Pa_apo,Rot_matrix,eulerAngles,debugOutput]...
+    = AttitudeEKF(approx_prediction,use_inertia_matrix,zFlag,dt,z,q_rotSpeed,q_rotAcc,q_acc,q_mag,r_gyro,r_accel,r_mag,J)
+
+
+%LQG Postion Estimator and Controller
+% Observer:
+%        x[n|n]   = x[n|n-1] + M(y[n] - Cx[n|n-1] - Du[n])
+%        x[n+1|n] = Ax[n|n] + Bu[n]
+%
+% $Author: Tobias Naegeli $    $Date: 2014 $    $Revision: 3 $
+%
+%
+% Arguments:
+% approx_prediction: if 1 then the exponential map is approximated with a
+% first order taylor approximation. has at the moment not a big influence
+% (just 1st or 2nd order approximation) we should change it to rodriquez
+% approximation.
+% use_inertia_matrix: set to true if you have the inertia matrix J for your
+% quadrotor
+% xa_apo_k: old state vectotr
+% zFlag: if sensor measurement is available [gyro, acc, mag]
+% dt: dt in s
+% z: measurements [gyro, acc, mag]
+% q_rotSpeed: process noise gyro
+% q_rotAcc: process noise gyro acceleration
+% q_acc: process noise acceleration
+% q_mag: process noise magnetometer
+% r_gyro: measurement noise gyro
+% r_accel: measurement noise accel
+% r_mag: measurement noise mag
+% J: moment of inertia matrix
+
+
+% Output:
+% xa_apo: updated state vectotr
+% Pa_apo: updated state covariance matrix
+% Rot_matrix: rotation matrix
+% eulerAngles: euler angles
+% debugOutput: not used
+
+
+%% model specific parameters
+
+
+
+% compute once the inverse of the Inertia
+persistent Ji;
+if isempty(Ji)
+    Ji=single(inv(J));
+end
+
+%% init
+persistent x_apo
+if(isempty(x_apo))
+    gyro_init=single([0;0;0]);
+    gyro_acc_init=single([0;0;0]);
+    acc_init=single([0;0;-9.81]);
+    mag_init=single([1;0;0]);
+    x_apo=single([gyro_init;gyro_acc_init;acc_init;mag_init]);
+    
+end
+
+persistent P_apo
+if(isempty(P_apo))
+    %     P_apo = single(eye(NSTATES) * 1000);
+    P_apo = single(200*ones(12));
+end
+
+debugOutput = single(zeros(4,1));
+
+%% copy the states
+wx=  x_apo(1);   % x  body angular rate
+wy=  x_apo(2);   % y  body angular rate
+wz=  x_apo(3);   % z  body angular rate
+
+wax=  x_apo(4);  % x  body angular acceleration
+way=  x_apo(5);  % y  body angular acceleration
+waz=  x_apo(6);  % z  body angular acceleration
+
+zex=  x_apo(7);  % x  component gravity vector
+zey=  x_apo(8);  % y  component gravity vector
+zez=  x_apo(9);  % z  component gravity vector
+
+mux=  x_apo(10); % x  component magnetic field vector
+muy=  x_apo(11); % y  component magnetic field vector
+muz=  x_apo(12); % z  component magnetic field vector
+
+
+
+
+%% prediction section
+% compute the apriori state estimate from the previous aposteriori estimate
+%body angular accelerations
+if (use_inertia_matrix==1)
+    wak =[wax;way;waz]+Ji*(-cross([wax;way;waz],J*[wax;way;waz]))*dt;
+else
+    wak =[wax;way;waz];
+end
+
+%body angular rates
+wk =[wx;  wy; wz] + dt*wak;
+
+%derivative of the prediction rotation matrix
+O=[0,-wz,wy;wz,0,-wx;-wy,wx,0]';
+
+%prediction of the earth z vector
+if (approx_prediction==1)
+    %e^(Odt)=I+dt*O+dt^2/2!O^2
+    % so we do a first order approximation of the exponential map
+    zek =(O*dt+single(eye(3)))*[zex;zey;zez];
+    
+else
+    zek =(single(eye(3))+O*dt+dt^2/2*O^2)*[zex;zey;zez];
+    %zek =expm2(O*dt)*[zex;zey;zez]; not working because use double
+    %precision
+end
+
+
+
+%prediction of the magnetic vector
+if (approx_prediction==1)
+    %e^(Odt)=I+dt*O+dt^2/2!O^2
+    % so we do a first order approximation of the exponential map
+    muk =(O*dt+single(eye(3)))*[mux;muy;muz];
+else
+     muk =(single(eye(3))+O*dt+dt^2/2*O^2)*[mux;muy;muz];
+    %muk =expm2(O*dt)*[mux;muy;muz]; not working because use double
+    %precision
+end
+
+x_apr=[wk;wak;zek;muk];
+
+% compute the apriori error covariance estimate from the previous
+%aposteriori estimate
+
+EZ=[0,zez,-zey;
+    -zez,0,zex;
+    zey,-zex,0]';
+MA=[0,muz,-muy;
+    -muz,0,mux;
+    muy,-mux,0]';
+
+E=single(eye(3));
+Z=single(zeros(3));
+
+A_lin=[ Z,  E,  Z,  Z
+    Z,  Z,  Z,  Z
+    EZ, Z,  O,  Z
+    MA, Z,  Z,  O];
+
+A_lin=eye(12)+A_lin*dt;
+
+%process covariance matrix
+
+persistent Q
+if (isempty(Q))
+    Q=diag([ q_rotSpeed,q_rotSpeed,q_rotSpeed,...
+        q_rotAcc,q_rotAcc,q_rotAcc,...
+        q_acc,q_acc,q_acc,...
+        q_mag,q_mag,q_mag]);
+end
+
+P_apr=A_lin*P_apo*A_lin'+Q;
+
+
+%% update
+if zFlag(1)==1&&zFlag(2)==1&&zFlag(3)==1
+    
+%     R=[r_gyro,0,0,0,0,0,0,0,0;
+%         0,r_gyro,0,0,0,0,0,0,0;
+%         0,0,r_gyro,0,0,0,0,0,0;
+%         0,0,0,r_accel,0,0,0,0,0;
+%         0,0,0,0,r_accel,0,0,0,0;
+%         0,0,0,0,0,r_accel,0,0,0;
+%         0,0,0,0,0,0,r_mag,0,0;
+%         0,0,0,0,0,0,0,r_mag,0;
+%         0,0,0,0,0,0,0,0,r_mag];
+     R_v=[r_gyro,r_gyro,r_gyro,r_accel,r_accel,r_accel,r_mag,r_mag,r_mag];
+    %observation matrix
+    %[zw;ze;zmk];
+    H_k=[  E,     Z,      Z,    Z;
+        Z,     Z,      E,    Z;
+        Z,     Z,      Z,    E];
+    
+    y_k=z(1:9)-H_k*x_apr;
+    
+    
+    %S_k=H_k*P_apr*H_k'+R;
+     S_k=H_k*P_apr*H_k';
+     S_k(1:9+1:end) = S_k(1:9+1:end) + R_v;
+    K_k=(P_apr*H_k'/(S_k));
+    
+    
+    x_apo=x_apr+K_k*y_k;
+    P_apo=(eye(12)-K_k*H_k)*P_apr;
+else
+    if zFlag(1)==1&&zFlag(2)==0&&zFlag(3)==0
+        
+        R=[r_gyro,0,0;
+            0,r_gyro,0;
+            0,0,r_gyro];
+        R_v=[r_gyro,r_gyro,r_gyro];
+        %observation matrix
+        
+        H_k=[  E,     Z,      Z,    Z];
+        
+        y_k=z(1:3)-H_k(1:3,1:12)*x_apr;
+        
+       % S_k=H_k(1:3,1:12)*P_apr*H_k(1:3,1:12)'+R(1:3,1:3);
+        S_k=H_k(1:3,1:12)*P_apr*H_k(1:3,1:12)';
+        S_k(1:3+1:end) = S_k(1:3+1:end) + R_v;
+        K_k=(P_apr*H_k(1:3,1:12)'/(S_k));
+        
+        
+        x_apo=x_apr+K_k*y_k;
+        P_apo=(eye(12)-K_k*H_k(1:3,1:12))*P_apr;
+    else
+        if  zFlag(1)==1&&zFlag(2)==1&&zFlag(3)==0
+            
+%             R=[r_gyro,0,0,0,0,0;
+%                 0,r_gyro,0,0,0,0;
+%                 0,0,r_gyro,0,0,0;
+%                 0,0,0,r_accel,0,0;
+%                 0,0,0,0,r_accel,0;
+%                 0,0,0,0,0,r_accel];
+            
+            R_v=[r_gyro,r_gyro,r_gyro,r_accel,r_accel,r_accel];
+            %observation matrix
+            
+            H_k=[  E,     Z,      Z,    Z;
+                Z,     Z,      E,    Z];
+            
+            y_k=z(1:6)-H_k(1:6,1:12)*x_apr;
+            
+           % S_k=H_k(1:6,1:12)*P_apr*H_k(1:6,1:12)'+R(1:6,1:6);
+            S_k=H_k(1:6,1:12)*P_apr*H_k(1:6,1:12)';
+            S_k(1:6+1:end) = S_k(1:6+1:end) + R_v;
+            K_k=(P_apr*H_k(1:6,1:12)'/(S_k));
+            
+            
+            x_apo=x_apr+K_k*y_k;
+            P_apo=(eye(12)-K_k*H_k(1:6,1:12))*P_apr;
+        else
+            if  zFlag(1)==1&&zFlag(2)==0&&zFlag(3)==1
+%                 R=[r_gyro,0,0,0,0,0;
+%                     0,r_gyro,0,0,0,0;
+%                     0,0,r_gyro,0,0,0;
+%                     0,0,0,r_mag,0,0;
+%                     0,0,0,0,r_mag,0;
+%                     0,0,0,0,0,r_mag];
+                  R_v=[r_gyro,r_gyro,r_gyro,r_mag,r_mag,r_mag];
+                %observation matrix
+                
+                H_k=[  E,     Z,      Z,    Z;
+                    Z,     Z,      Z,    E];
+                
+                y_k=[z(1:3);z(7:9)]-H_k(1:6,1:12)*x_apr;
+                
+                %S_k=H_k(1:6,1:12)*P_apr*H_k(1:6,1:12)'+R(1:6,1:6);
+                S_k=H_k(1:6,1:12)*P_apr*H_k(1:6,1:12)';
+                S_k(1:6+1:end) = S_k(1:6+1:end) + R_v;
+                K_k=(P_apr*H_k(1:6,1:12)'/(S_k));
+                
+                
+                x_apo=x_apr+K_k*y_k;
+                P_apo=(eye(12)-K_k*H_k(1:6,1:12))*P_apr;
+            else
+                x_apo=x_apr;
+                P_apo=P_apr;
+            end
+        end
+    end
+end
+
+
+
+%% euler anglels extraction
+z_n_b = -x_apo(7:9)./norm(x_apo(7:9));
+m_n_b = x_apo(10:12)./norm(x_apo(10:12));
+
+y_n_b=cross(z_n_b,m_n_b);
+y_n_b=y_n_b./norm(y_n_b);
+
+x_n_b=(cross(y_n_b,z_n_b));
+x_n_b=x_n_b./norm(x_n_b);
+
+
+xa_apo=x_apo;
+Pa_apo=P_apo;
+% rotation matrix from earth to body system
+Rot_matrix=[x_n_b,y_n_b,z_n_b];
+
+
+phi=atan2(Rot_matrix(2,3),Rot_matrix(3,3));
+theta=-asin(Rot_matrix(1,3));
+psi=atan2(Rot_matrix(1,2),Rot_matrix(1,1));
+eulerAngles=[phi;theta;psi];
+
diff --git a/src/modules/attitude_estimator_ekf/attitudeKalmanfilter.prj b/src/modules/attitude_estimator_ekf/attitudeKalmanfilter.prj
new file mode 100644
index 000000000..9ea520346
--- /dev/null
+++ b/src/modules/attitude_estimator_ekf/attitudeKalmanfilter.prj
@@ -0,0 +1,502 @@
+<?xml version="1.0" encoding="UTF-8"?>

+<deployment-project plugin="plugin.matlabcoder" plugin-version="R2013a">

+  <configuration target="target.matlab.coder" target-name="MEX, C, and C++ Code Generation" name="attitudeKalmanfilter" location="/home/thomasgubler/src/Firmware/src/modules/attitude_estimator_ekf" file="/home/thomasgubler/src/Firmware/src/modules/attitude_estimator_ekf/attitudeKalmanfilter.prj" build-checksum="1213478164">

+    <profile key="profile.mex">

+      <param.MergeInstrumentationResults>false</param.MergeInstrumentationResults>

+      <param.BuiltInstrumentedMex>false</param.BuiltInstrumentedMex>

+      <param.RanInstrumentedMex>false</param.RanInstrumentedMex>

+      <param.WorkingFolder>option.WorkingFolder.Project</param.WorkingFolder>

+      <param.SpecifiedWorkingFolder />

+      <param.BuildFolder>option.BuildFolder.Project</param.BuildFolder>

+      <param.SpecifiedBuildFolder />

+      <param.SearchPaths />

+      <param.ResponsivenessChecks>true</param.ResponsivenessChecks>

+      <param.ExtrinsicCalls>true</param.ExtrinsicCalls>

+      <param.IntegrityChecks>true</param.IntegrityChecks>

+      <param.SaturateOnIntegerOverflow>true</param.SaturateOnIntegerOverflow>

+      <param.GlobalDataSyncMethod>option.GlobalDataSyncMethod.SyncAlways</param.GlobalDataSyncMethod>

+      <param.EnableVariableSizing>true</param.EnableVariableSizing>

+      <param.DynamicMemoryAllocation>option.DynamicMemoryAllocation.Threshold</param.DynamicMemoryAllocation>

+      <param.DynamicMemoryAllocationThreshold>65536</param.DynamicMemoryAllocationThreshold>

+      <param.StackUsageMax>200000</param.StackUsageMax>

+      <param.FilePartitionMethod>option.FilePartitionMethod.MapMFileToCFile</param.FilePartitionMethod>

+      <param.GenerateComments>true</param.GenerateComments>

+      <param.MATLABSourceComments>false</param.MATLABSourceComments>

+      <param.ReservedNameArray />

+      <param.EnableScreener>true</param.EnableScreener>

+      <param.EnableDebugging>false</param.EnableDebugging>

+      <param.GenerateReport>true</param.GenerateReport>

+      <param.LaunchReport>false</param.LaunchReport>

+      <param.CustomSourceCode />

+      <param.CustomHeaderCode />

+      <param.CustomInitializer />

+      <param.CustomTerminator />

+      <param.CustomInclude />

+      <param.CustomSource />

+      <param.CustomLibrary />

+      <param.PostCodeGenCommand />

+      <param.ProposeFixedPointDataTypes>true</param.ProposeFixedPointDataTypes>

+      <param.mex.GenCodeOnly>false</param.mex.GenCodeOnly>

+      <param.ConstantFoldingTimeout>40000</param.ConstantFoldingTimeout>

+      <param.RecursionLimit>100</param.RecursionLimit>

+      <param.TargetLang>option.TargetLang.C</param.TargetLang>

+      <param.EchoExpressions>true</param.EchoExpressions>

+      <param.InlineThreshold>10</param.InlineThreshold>

+      <param.InlineThresholdMax>200</param.InlineThresholdMax>

+      <param.InlineStackLimit>4000</param.InlineStackLimit>

+      <param.EnableMemcpy>true</param.EnableMemcpy>

+      <param.MemcpyThreshold>64</param.MemcpyThreshold>

+      <param.EnableOpenMP>true</param.EnableOpenMP>

+      <param.InitFltsAndDblsToZero>true</param.InitFltsAndDblsToZero>

+      <param.ConstantInputs>option.ConstantInputs.CheckValues</param.ConstantInputs>

+      <unset>

+        <param.MergeInstrumentationResults />

+        <param.BuiltInstrumentedMex />

+        <param.RanInstrumentedMex />

+        <param.WorkingFolder />

+        <param.SpecifiedWorkingFolder />

+        <param.BuildFolder />

+        <param.SpecifiedBuildFolder />

+        <param.SearchPaths />

+        <param.ResponsivenessChecks />

+        <param.ExtrinsicCalls />

+        <param.IntegrityChecks />

+        <param.SaturateOnIntegerOverflow />

+        <param.GlobalDataSyncMethod />

+        <param.EnableVariableSizing />

+        <param.DynamicMemoryAllocation />

+        <param.DynamicMemoryAllocationThreshold />

+        <param.StackUsageMax />

+        <param.FilePartitionMethod />

+        <param.GenerateComments />

+        <param.MATLABSourceComments />

+        <param.ReservedNameArray />

+        <param.EnableScreener />

+        <param.EnableDebugging />

+        <param.GenerateReport />

+        <param.LaunchReport />

+        <param.CustomSourceCode />

+        <param.CustomHeaderCode />

+        <param.CustomInitializer />

+        <param.CustomTerminator />

+        <param.CustomInclude />

+        <param.CustomSource />

+        <param.CustomLibrary />

+        <param.PostCodeGenCommand />

+        <param.ProposeFixedPointDataTypes />

+        <param.mex.GenCodeOnly />

+        <param.ConstantFoldingTimeout />

+        <param.RecursionLimit />

+        <param.TargetLang />

+        <param.EchoExpressions />

+        <param.InlineThreshold />

+        <param.InlineThresholdMax />

+        <param.InlineStackLimit />

+        <param.EnableMemcpy />

+        <param.MemcpyThreshold />

+        <param.EnableOpenMP />

+        <param.InitFltsAndDblsToZero />

+        <param.ConstantInputs />

+      </unset>

+    </profile>

+    <profile key="profile.c">

+      <param.grt.GenCodeOnly>true</param.grt.GenCodeOnly>

+      <param.WorkingFolder>option.WorkingFolder.Project</param.WorkingFolder>

+      <param.SpecifiedWorkingFolder />

+      <param.BuildFolder>option.BuildFolder.Specified</param.BuildFolder>

+      <param.SpecifiedBuildFolder>codegen</param.SpecifiedBuildFolder>

+      <param.SearchPaths />

+      <param.SaturateOnIntegerOverflow>true</param.SaturateOnIntegerOverflow>

+      <param.PurelyIntegerCode>false</param.PurelyIntegerCode>

+      <param.SupportNonFinite>false</param.SupportNonFinite>

+      <param.EnableVariableSizing>false</param.EnableVariableSizing>

+      <param.DynamicMemoryAllocation>option.DynamicMemoryAllocation.Threshold</param.DynamicMemoryAllocation>

+      <param.DynamicMemoryAllocationThreshold>65536</param.DynamicMemoryAllocationThreshold>

+      <param.StackUsageMax>4000</param.StackUsageMax>

+      <param.MultiInstanceCode>false</param.MultiInstanceCode>

+      <param.FilePartitionMethod>option.FilePartitionMethod.SingleFile</param.FilePartitionMethod>

+      <param.GenerateComments>true</param.GenerateComments>

+      <param.MATLABSourceComments>true</param.MATLABSourceComments>

+      <param.MATLABFcnDesc>false</param.MATLABFcnDesc>

+      <param.DataTypeReplacement>option.DataTypeReplacement.CBuiltIn</param.DataTypeReplacement>

+      <param.ConvertIfToSwitch>false</param.ConvertIfToSwitch>

+      <param.PreserveExternInFcnDecls>true</param.PreserveExternInFcnDecls>

+      <param.ParenthesesLevel>option.ParenthesesLevel.Nominal</param.ParenthesesLevel>

+      <param.MaxIdLength>31</param.MaxIdLength>

+      <param.CustomSymbolStrGlobalVar>$M$N</param.CustomSymbolStrGlobalVar>

+      <param.CustomSymbolStrType>$M$N</param.CustomSymbolStrType>

+      <param.CustomSymbolStrField>$M$N</param.CustomSymbolStrField>

+      <param.CustomSymbolStrFcn>$M$N</param.CustomSymbolStrFcn>

+      <param.CustomSymbolStrTmpVar>$M$N</param.CustomSymbolStrTmpVar>

+      <param.CustomSymbolStrMacro>$M$N</param.CustomSymbolStrMacro>

+      <param.CustomSymbolStrEMXArray>emxArray_$M$N</param.CustomSymbolStrEMXArray>

+      <param.CustomSymbolStrEMXArrayFcn>emx$M$N</param.CustomSymbolStrEMXArrayFcn>

+      <param.ReservedNameArray />

+      <param.EnableScreener>true</param.EnableScreener>

+      <param.Verbose>false</param.Verbose>

+      <param.GenerateReport>true</param.GenerateReport>

+      <param.GenerateCodeMetricsReport>true</param.GenerateCodeMetricsReport>

+      <param.GenerateCodeReplacementReport>false</param.GenerateCodeReplacementReport>

+      <param.LaunchReport>true</param.LaunchReport>

+      <param.CustomSourceCode />

+      <param.CustomHeaderCode />

+      <param.CustomInitializer />

+      <param.CustomTerminator />

+      <param.CustomInclude />

+      <param.CustomSource />

+      <param.CustomLibrary />

+      <param.PostCodeGenCommand />

+      <param.CodeReplacementLibrary>C89/C90 (ANSI)</param.CodeReplacementLibrary>

+      <param.SameHardware>true</param.SameHardware>

+      <param.HardwareVendor.Production>ARM Compatible</param.HardwareVendor.Production>

+      <param.HardwareType.Production>ARM Cortex</param.HardwareType.Production>

+      <var.instance.enabled.Production>true</var.instance.enabled.Production>

+      <param.HardwareSizeChar.Production>8</param.HardwareSizeChar.Production>

+      <param.HardwareSizeShort.Production>16</param.HardwareSizeShort.Production>

+      <param.HardwareSizeInt.Production>32</param.HardwareSizeInt.Production>

+      <param.HardwareSizeLong.Production>32</param.HardwareSizeLong.Production>

+      <param.HardwareSizeLongLong.Production>64</param.HardwareSizeLongLong.Production>

+      <param.HardwareSizeFloat.Production>32</param.HardwareSizeFloat.Production>

+      <param.HardwareSizeDouble.Production>64</param.HardwareSizeDouble.Production>

+      <param.HardwareSizeWord.Production>32</param.HardwareSizeWord.Production>

+      <param.HardwareSizePointer.Production>32</param.HardwareSizePointer.Production>

+      <param.HardwareEndianness.Production>option.HardwareEndianness.Little</param.HardwareEndianness.Production>

+      <param.HardwareArithmeticRightShift.Production>true</param.HardwareArithmeticRightShift.Production>

+      <param.HardwareLongLongMode.Production>false</param.HardwareLongLongMode.Production>

+      <param.HardwareAtomicIntegerSize.Production>option.HardwareAtomicIntegerSize.Long</param.HardwareAtomicIntegerSize.Production>

+      <param.HardwareAtomicFloatSize.Production>option.HardwareAtomicFloatSize.Double</param.HardwareAtomicFloatSize.Production>

+      <param.HardwareDivisionRounding.Production>option.HardwareDivisionRounding.Undefined</param.HardwareDivisionRounding.Production>

+      <param.HardwareVendor.Target>Generic</param.HardwareVendor.Target>

+      <param.HardwareType.Target>MATLAB Host Computer</param.HardwareType.Target>

+      <var.instance.enabled.Target>false</var.instance.enabled.Target>

+      <param.HardwareSizeChar.Target>8</param.HardwareSizeChar.Target>

+      <param.HardwareSizeShort.Target>16</param.HardwareSizeShort.Target>

+      <param.HardwareSizeInt.Target>32</param.HardwareSizeInt.Target>

+      <param.HardwareSizeLong.Target>64</param.HardwareSizeLong.Target>

+      <param.HardwareSizeLongLong.Target>64</param.HardwareSizeLongLong.Target>

+      <param.HardwareSizeFloat.Target>32</param.HardwareSizeFloat.Target>

+      <param.HardwareSizeDouble.Target>64</param.HardwareSizeDouble.Target>

+      <param.HardwareSizeWord.Target>64</param.HardwareSizeWord.Target>

+      <param.HardwareSizePointer.Target>64</param.HardwareSizePointer.Target>

+      <param.HardwareEndianness.Target>option.HardwareEndianness.Little</param.HardwareEndianness.Target>

+      <param.HardwareArithmeticRightShift.Target>true</param.HardwareArithmeticRightShift.Target>

+      <param.HardwareLongLongMode.Target>true</param.HardwareLongLongMode.Target>

+      <param.HardwareAtomicIntegerSize.Target>option.HardwareAtomicIntegerSize.Char</param.HardwareAtomicIntegerSize.Target>

+      <param.HardwareAtomicFloatSize.Target>option.HardwareAtomicFloatSize.None</param.HardwareAtomicFloatSize.Target>

+      <param.HardwareDivisionRounding.Target>option.HardwareDivisionRounding.Zero</param.HardwareDivisionRounding.Target>

+      <param.Toolchain>Automatically locate an installed toolchain</param.Toolchain>

+      <param.BuildConfiguration>Faster Builds</param.BuildConfiguration>

+      <param.CustomToolchainOptions />

+      <param.ConstantFoldingTimeout>40000</param.ConstantFoldingTimeout>

+      <param.RecursionLimit>100</param.RecursionLimit>

+      <param.IncludeTerminateFcn>true</param.IncludeTerminateFcn>

+      <param.TargetLang>option.TargetLang.C</param.TargetLang>

+      <param.CCompilerOptimization>option.CCompilerOptimization.On</param.CCompilerOptimization>

+      <param.CCompilerCustomOptimizations />

+      <param.GenerateMakefile>true</param.GenerateMakefile>

+      <param.BuildToolEnable>false</param.BuildToolEnable>

+      <param.MakeCommand>make_rtw</param.MakeCommand>

+      <param.TemplateMakefile>default_tmf</param.TemplateMakefile>

+      <param.BuildToolConfiguration />

+      <param.InlineThreshold>10</param.InlineThreshold>

+      <param.InlineThresholdMax>200</param.InlineThresholdMax>

+      <param.InlineStackLimit>4000</param.InlineStackLimit>

+      <param.EnableMemcpy>true</param.EnableMemcpy>

+      <param.MemcpyThreshold>64</param.MemcpyThreshold>

+      <param.EnableOpenMP>true</param.EnableOpenMP>

+      <param.InitFltsAndDblsToZero>true</param.InitFltsAndDblsToZero>

+      <param.PassStructByReference>false</param.PassStructByReference>

+      <param.UseECoderFeatures>true</param.UseECoderFeatures>

+      <unset>

+        <param.WorkingFolder />

+        <param.SpecifiedWorkingFolder />

+        <param.SearchPaths />

+        <param.SaturateOnIntegerOverflow />

+        <param.PurelyIntegerCode />

+        <param.DynamicMemoryAllocation />

+        <param.DynamicMemoryAllocationThreshold />

+        <param.MultiInstanceCode />

+        <param.GenerateComments />

+        <param.MATLABFcnDesc />

+        <param.DataTypeReplacement />

+        <param.ConvertIfToSwitch />

+        <param.PreserveExternInFcnDecls />

+        <param.ParenthesesLevel />

+        <param.MaxIdLength />

+        <param.CustomSymbolStrGlobalVar />

+        <param.CustomSymbolStrType />

+        <param.CustomSymbolStrField />

+        <param.CustomSymbolStrFcn />

+        <param.CustomSymbolStrTmpVar />

+        <param.CustomSymbolStrMacro />

+        <param.CustomSymbolStrEMXArray />

+        <param.CustomSymbolStrEMXArrayFcn />

+        <param.ReservedNameArray />

+        <param.EnableScreener />

+        <param.Verbose />

+        <param.GenerateReport />

+        <param.GenerateCodeMetricsReport />

+        <param.GenerateCodeReplacementReport />

+        <param.CustomInclude />

+        <param.CustomSource />

+        <param.CustomLibrary />

+        <param.SameHardware />

+        <var.instance.enabled.Production />

+        <param.HardwareSizeChar.Production />

+        <param.HardwareSizeShort.Production />

+        <param.HardwareSizeInt.Production />

+        <param.HardwareSizeLong.Production />

+        <param.HardwareSizeLongLong.Production />

+        <param.HardwareSizeFloat.Production />

+        <param.HardwareSizeDouble.Production />

+        <param.HardwareSizeWord.Production />

+        <param.HardwareSizePointer.Production />

+        <param.HardwareEndianness.Production />

+        <param.HardwareLongLongMode.Production />

+        <param.HardwareDivisionRounding.Production />

+        <var.instance.enabled.Target />

+        <param.HardwareSizeChar.Target />

+        <param.HardwareSizeShort.Target />

+        <param.HardwareSizeInt.Target />

+        <param.HardwareSizeLongLong.Target />

+        <param.HardwareSizeFloat.Target />

+        <param.HardwareSizeDouble.Target />

+        <param.HardwareEndianness.Target />

+        <param.HardwareAtomicFloatSize.Target />

+        <param.CustomToolchainOptions />

+        <param.ConstantFoldingTimeout />

+        <param.RecursionLimit />

+        <param.IncludeTerminateFcn />

+        <param.TargetLang />

+        <param.CCompilerCustomOptimizations />

+        <param.GenerateMakefile />

+        <param.BuildToolEnable />

+        <param.MakeCommand />

+        <param.TemplateMakefile />

+        <param.BuildToolConfiguration />

+        <param.InlineThreshold />

+        <param.InlineThresholdMax />

+        <param.InlineStackLimit />

+        <param.EnableMemcpy />

+        <param.MemcpyThreshold />

+        <param.EnableOpenMP />

+        <param.InitFltsAndDblsToZero />

+        <param.UseECoderFeatures />

+      </unset>

+    </profile>

+    <param.outputfile>/opt/matlab/r2013b/bin/codegen/codegen/lib/AttitudeEKF/AttitudeEKF.a</param.outputfile>

+    <param.version>R2012a</param.version>

+    <param.HasECoderFeatures>true</param.HasECoderFeatures>

+    <param.mex.mainhtml>t:\private\desktop-dinfk-xp\Attitude_Kalmanfilter\codegen\mex\attitudeKalmanfilter\html\index.html</param.mex.mainhtml>

+    <param.grt.mainhtml>/home/thomasgubler/src/Firmware/src/modules/attitude_estimator_ekf/codegen/html/index.html</param.grt.mainhtml>

+    <param.CallGeneratedCodeFromTest>true</param.CallGeneratedCodeFromTest>

+    <param.VerificationMode>option.VerificationMode.None</param.VerificationMode>

+    <param.SILDebugging>false</param.SILDebugging>

+    <param.DefaultTestFile>${PROJECT_ROOT}/AttitudeEKF_Test.m</param.DefaultTestFile>

+    <param.AutoInferDefaultFile>${PROJECT_ROOT}/AttitudeEKF_Test.m</param.AutoInferDefaultFile>

+    <param.AutoInferUseVariableSize>false</param.AutoInferUseVariableSize>

+    <param.AutoInferUseUnboundedSize>false</param.AutoInferUseUnboundedSize>

+    <param.AutoInferVariableSizeThreshold>1024</param.AutoInferVariableSizeThreshold>

+    <param.AutoInferUnboundedSizeThreshold>2048</param.AutoInferUnboundedSizeThreshold>

+    <param.mex.outputfile>AttitudeEKF_mex</param.mex.outputfile>

+    <param.grt.outputfile>AttitudeEKF</param.grt.outputfile>

+    <param.artifact>option.target.artifact.lib</param.artifact>

+    <param.FixedPointTypeProposalMode>option.FixedPointTypeProposalMode.ProposeFractionLengths</param.FixedPointTypeProposalMode>

+    <param.DefaultProposedFixedPointType>numerictype([],16,12)</param.DefaultProposedFixedPointType>

+    <param.MinMaxSafetyMargin>0</param.MinMaxSafetyMargin>

+    <param.OptimizeWholeNumbers>true</param.OptimizeWholeNumbers>

+    <param.LaunchInstrumentationReport>false</param.LaunchInstrumentationReport>

+    <param.OpenInstrumentationReportInBrowser>false</param.OpenInstrumentationReportInBrowser>

+    <param.CreatePrintableInstrumentationReport>false</param.CreatePrintableInstrumentationReport>

+    <param.EnableAutoExtrinsicCalls>true</param.EnableAutoExtrinsicCalls>

+    <param.UsePreconditions>false</param.UsePreconditions>

+    <param.FeatureFlags />

+    <param.FixedPointMode>option.FixedPointMode.None</param.FixedPointMode>

+    <param.AutoScaleLoopIndexVariables>false</param.AutoScaleLoopIndexVariables>

+    <param.ComputedFixedPointData />

+    <param.UserFixedPointData />

+    <param.DefaultWordLength>16</param.DefaultWordLength>

+    <param.DefaultFractionLength>4</param.DefaultFractionLength>

+    <param.FixedPointSafetyMargin>0</param.FixedPointSafetyMargin>

+    <param.FixedPointFimath>fimath('RoundingMethod', 'Floor', 'OverflowAction', 'Wrap', 'ProductMode', 'FullPrecision', 'MaxProductWordLength', 128, 'SumMode', 'FullPrecision', 'MaxSumWordLength', 128)</param.FixedPointFimath>

+    <param.FixedPointTypeSource>option.FixedPointTypeSource.SimAndDerived</param.FixedPointTypeSource>

+    <param.StaticAnalysisTimeout />

+    <param.StaticAnalysisGlobalRangesOnly>false</param.StaticAnalysisGlobalRangesOnly>

+    <param.LogAllIOValues>false</param.LogAllIOValues>

+    <param.LogHistogram>false</param.LogHistogram>

+    <param.ShowCoverage>true</param.ShowCoverage>

+    <param.ExcludedFixedPointVerificationFiles />

+    <param.ExcludedFixedPointSimulationFiles />

+    <param.InstrumentedBuildChecksum />

+    <param.FixedPointStaticAnalysisChecksum />

+    <param.InstrumentedMexFile />

+    <param.FixedPointValidationChecksum />

+    <param.FixedPointSourceCodeChecksum />

+    <param.FixedPointFunctionReplacements />

+    <param.OptimizeWholeNumbers>true</param.OptimizeWholeNumbers>

+    <param.GeneratedFixedPointFileSuffix>_fixpt</param.GeneratedFixedPointFileSuffix>

+    <param.DefaultFixedPointSignedness>option.DefaultFixedPointSignedness.Automatic</param.DefaultFixedPointSignedness>

+    <unset>

+      <param.outputfile />

+      <param.version />

+      <param.HasECoderFeatures />

+      <param.CallGeneratedCodeFromTest />

+      <param.VerificationMode />

+      <param.SILDebugging />

+      <param.AutoInferUseVariableSize />

+      <param.AutoInferUseUnboundedSize />

+      <param.AutoInferVariableSizeThreshold />

+      <param.AutoInferUnboundedSizeThreshold />

+      <param.mex.outputfile />

+      <param.grt.outputfile />

+      <param.FixedPointTypeProposalMode />

+      <param.DefaultProposedFixedPointType />

+      <param.MinMaxSafetyMargin />

+      <param.OptimizeWholeNumbers />

+      <param.LaunchInstrumentationReport />

+      <param.OpenInstrumentationReportInBrowser />

+      <param.CreatePrintableInstrumentationReport />

+      <param.EnableAutoExtrinsicCalls />

+      <param.UsePreconditions />

+      <param.FeatureFlags />

+      <param.FixedPointMode />

+      <param.AutoScaleLoopIndexVariables />

+      <param.ComputedFixedPointData />

+      <param.UserFixedPointData />

+      <param.DefaultWordLength />

+      <param.DefaultFractionLength />

+      <param.FixedPointSafetyMargin />

+      <param.FixedPointFimath />

+      <param.FixedPointTypeSource />

+      <param.StaticAnalysisTimeout />

+      <param.StaticAnalysisGlobalRangesOnly />

+      <param.LogAllIOValues />

+      <param.LogHistogram />

+      <param.ShowCoverage />

+      <param.ExcludedFixedPointVerificationFiles />

+      <param.ExcludedFixedPointSimulationFiles />

+      <param.InstrumentedBuildChecksum />

+      <param.FixedPointStaticAnalysisChecksum />

+      <param.InstrumentedMexFile />

+      <param.FixedPointValidationChecksum />

+      <param.FixedPointSourceCodeChecksum />

+      <param.FixedPointFunctionReplacements />

+      <param.GeneratedFixedPointFileSuffix />

+      <param.DefaultFixedPointSignedness />

+    </unset>

+    <fileset.entrypoints>

+      <file value="${PROJECT_ROOT}/AttitudeEKF.m" custom-data-expanded="true">

+        <Inputs fileName="AttitudeEKF.m" functionName="AttitudeEKF">

+          <Input Name="approx_prediction">

+            <Class>uint8</Class>

+            <UserDefined>false</UserDefined>

+            <Size>1 x 1</Size>

+            <Complex>false</Complex>

+          </Input>

+          <Input Name="use_inertia_matrix">

+            <Class>uint8</Class>

+            <UserDefined>false</UserDefined>

+            <Size>1 x 1</Size>

+            <Complex>false</Complex>

+          </Input>

+          <Input Name="zFlag">

+            <Class>uint8</Class>

+            <UserDefined>false</UserDefined>

+            <Size>3 x 1</Size>

+            <Complex>false</Complex>

+          </Input>

+          <Input Name="dt">

+            <Class>single</Class>

+            <UserDefined>false</UserDefined>

+            <Size>1 x 1</Size>

+            <Complex>false</Complex>

+          </Input>

+          <Input Name="z">

+            <Class>single</Class>

+            <UserDefined>false</UserDefined>

+            <Size>9 x 1</Size>

+            <Complex>false</Complex>

+          </Input>

+          <Input Name="q_rotSpeed">

+            <Class>single</Class>

+            <UserDefined>false</UserDefined>

+            <Size>1 x 1</Size>

+            <Complex>false</Complex>

+          </Input>

+          <Input Name="q_rotAcc">

+            <Class>single</Class>

+            <UserDefined>false</UserDefined>

+            <Size>1 x 1</Size>

+            <Complex>false</Complex>

+          </Input>

+          <Input Name="q_acc">

+            <Class>single</Class>

+            <UserDefined>false</UserDefined>

+            <Size>1 x 1</Size>

+            <Complex>false</Complex>

+          </Input>

+          <Input Name="q_mag">

+            <Class>single</Class>

+            <UserDefined>false</UserDefined>

+            <Size>1 x 1</Size>

+            <Complex>false</Complex>

+          </Input>

+          <Input Name="r_gyro">

+            <Class>single</Class>

+            <UserDefined>false</UserDefined>

+            <Size>1 x 1</Size>

+            <Complex>false</Complex>

+          </Input>

+          <Input Name="r_accel">

+            <Class>single</Class>

+            <UserDefined>false</UserDefined>

+            <Size>1 x 1</Size>

+            <Complex>false</Complex>

+          </Input>

+          <Input Name="r_mag">

+            <Class>single</Class>

+            <UserDefined>false</UserDefined>

+            <Size>1 x 1</Size>

+            <Complex>false</Complex>

+          </Input>

+          <Input Name="J">

+            <Class>single</Class>

+            <UserDefined>false</UserDefined>

+            <Size>3 x 3</Size>

+            <Complex>false</Complex>

+          </Input>

+        </Inputs>

+      </file>

+    </fileset.entrypoints>

+    <fileset.testbench>

+      <file>${PROJECT_ROOT}/AttitudeEKF_Test.m</file>

+    </fileset.testbench>

+    <fileset.package />

+    <build-deliverables>

+      <file name="AttitudeEKF.a" location="${MATLAB_ROOT}/bin/codegen/codegen/lib/AttitudeEKF" optional="false">/opt/matlab/r2013b/bin/codegen/codegen/lib/AttitudeEKF/AttitudeEKF.a</file>

+    </build-deliverables>

+    <workflow />

+    <matlab>

+      <root>/opt/matlab/r2013b</root>

+      <toolboxes>

+        <toolbox name="fixedpoint" />

+      </toolboxes>

+    </matlab>

+    <platform>

+      <unix>true</unix>

+      <mac>false</mac>

+      <windows>false</windows>

+      <win2k>false</win2k>

+      <winxp>false</winxp>

+      <vista>false</vista>

+      <linux>true</linux>

+      <solaris>false</solaris>

+      <osver>3.16.1-1-ARCH</osver>

+      <os32>false</os32>

+      <os64>true</os64>

+      <arch>glnxa64</arch>

+      <matlab>true</matlab>

+    </platform>

+  </configuration>

+</deployment-project>

+

diff --git a/src/modules/attitude_estimator_ekf/attitude_estimator_ekf_main.cpp b/src/modules/attitude_estimator_ekf/attitude_estimator_ekf_main.cpp
index 35dc39ec6..b0086676a 100755
--- a/src/modules/attitude_estimator_ekf/attitude_estimator_ekf_main.cpp
+++ b/src/modules/attitude_estimator_ekf/attitude_estimator_ekf_main.cpp
@@ -38,6 +38,7 @@
  *
  * @author Tobias Naegeli <naegelit@student.ethz.ch>
  * @author Lorenz Meier <lm@inf.ethz.ch>
+ * @author Thomas Gubler <thomasgubler@gmail.com>
  */
 
 #include <nuttx/config.h>
@@ -62,6 +63,7 @@
 #include <uORB/topics/vehicle_gps_position.h>
 #include <uORB/topics/vehicle_global_position.h>
 #include <uORB/topics/parameter_update.h>
+#include <uORB/topics/vision_position_estimate.h>
 #include <drivers/drv_hrt.h>
 
 #include <lib/mathlib/mathlib.h>
@@ -74,8 +76,7 @@
 #ifdef __cplusplus
 extern "C" {
 #endif
-#include "codegen/attitudeKalmanfilter_initialize.h"
-#include "codegen/attitudeKalmanfilter.h"
+#include "codegen/AttitudeEKF.h"
 #include "attitude_estimator_ekf_params.h"
 #ifdef __cplusplus
 }
@@ -132,9 +133,9 @@ int attitude_estimator_ekf_main(int argc, char *argv[])
 		attitude_estimator_ekf_task = task_spawn_cmd("attitude_estimator_ekf",
 					      SCHED_DEFAULT,
 					      SCHED_PRIORITY_MAX - 5,
-					      14000,
+					      7700,
 					      attitude_estimator_ekf_thread_main,
-					      (argv) ? (const char **)&argv[2] : (const char **)NULL);
+					      (argv) ? (char * const *)&argv[2] : (char * const *)NULL);
 		exit(0);
 	}
 
@@ -206,14 +207,11 @@ const unsigned int loop_interval_alarm = 6500;	// loop interval in microseconds
 			      0,  0,  1.f
 			     };		/**< init: identity matrix */
 
-	// print text
-	printf("Extended Kalman Filter Attitude Estimator initialized..\n\n");
-	fflush(stdout);
-
+	float debugOutput[4] = { 0.0f };
 	int overloadcounter = 19;
 
 	/* Initialize filter */
-	attitudeKalmanfilter_initialize();
+	AttitudeEKF_initialize();
 
 	/* store start time to guard against too slow update rates */
 	uint64_t last_run = hrt_absolute_time();
@@ -222,6 +220,8 @@ const unsigned int loop_interval_alarm = 6500;	// loop interval in microseconds
 	memset(&raw, 0, sizeof(raw));
 	struct vehicle_gps_position_s gps;
 	memset(&gps, 0, sizeof(gps));
+	gps.eph = 100000;
+	gps.epv = 100000;
 	struct vehicle_global_position_s global_pos;
 	memset(&global_pos, 0, sizeof(global_pos));
 	struct vehicle_attitude_s att;
@@ -260,9 +260,12 @@ const unsigned int loop_interval_alarm = 6500;	// loop interval in microseconds
 	/* subscribe to param changes */
 	int sub_params = orb_subscribe(ORB_ID(parameter_update));
 
-	/* subscribe to control mode*/
+	/* subscribe to control mode */
 	int sub_control_mode = orb_subscribe(ORB_ID(vehicle_control_mode));
 
+	/* subscribe to vision estimate */
+	int vision_sub = orb_subscribe(ORB_ID(vision_position_estimate));
+
 	/* advertise attitude */
 	orb_advert_t pub_att = orb_advertise(ORB_ID(vehicle_attitude), &att);
 
@@ -270,16 +273,13 @@ const unsigned int loop_interval_alarm = 6500;	// loop interval in microseconds
 
 	thread_running = true;
 
-	/* advertise debug value */
-	// struct debug_key_value_s dbg = { .key = "", .value = 0.0f };
-	// orb_advert_t pub_dbg = -1;
-
 	/* keep track of sensor updates */
 	uint64_t sensor_last_timestamp[3] = {0, 0, 0};
 
 	struct attitude_estimator_ekf_params ekf_params;
+	memset(&ekf_params, 0, sizeof(ekf_params));
 
-	struct attitude_estimator_ekf_param_handles ekf_param_handles;
+	struct attitude_estimator_ekf_param_handles ekf_param_handles = { 0 };
 
 	/* initialize parameter handles */
 	parameters_init(&ekf_param_handles);
@@ -295,6 +295,8 @@ const unsigned int loop_interval_alarm = 6500;	// loop interval in microseconds
 	math::Matrix<3, 3> R_decl;
 	R_decl.identity();
 
+	struct vision_position_estimate vision {};
+
 	/* register the perf counter */
 	perf_counter_t ekf_loop_perf = perf_alloc(PC_ELAPSED, "attitude_estimator_ekf");
 
@@ -315,8 +317,7 @@ const unsigned int loop_interval_alarm = 6500;	// loop interval in microseconds
 			orb_copy(ORB_ID(vehicle_control_mode), sub_control_mode, &control_mode);
 
 			if (!control_mode.flag_system_hil_enabled) {
-				fprintf(stderr,
-					"[att ekf] WARNING: Not getting sensors - sensor app running?\n");
+				warnx("WARNING: Not getting sensors - sensor app running?");
 			}
 
 		} else {
@@ -451,9 +452,30 @@ const unsigned int loop_interval_alarm = 6500;	// loop interval in microseconds
 						sensor_last_timestamp[2] = raw.magnetometer_timestamp;
 					}
 
-					z_k[6] = raw.magnetometer_ga[0];
-					z_k[7] = raw.magnetometer_ga[1];
-					z_k[8] = raw.magnetometer_ga[2];
+					bool vision_updated = false;
+					orb_check(vision_sub, &vision_updated);
+
+					if (vision_updated) {
+						orb_copy(ORB_ID(vision_position_estimate), vision_sub, &vision);
+					}
+
+					if (vision.timestamp_boot > 0 && (hrt_elapsed_time(&vision.timestamp_boot) < 500000)) {
+
+						math::Quaternion q(vision.q);
+						math::Matrix<3, 3> Rvis = q.to_dcm();
+
+						math::Vector<3> v(1.0f, 0.0f, 0.4f);
+
+						math::Vector<3> vn = Rvis * v;
+
+						z_k[6] = vn(0);
+						z_k[7] = vn(1);
+						z_k[8] = vn(2);
+					} else {
+						z_k[6] = raw.magnetometer_ga[0];
+						z_k[7] = raw.magnetometer_ga[1];
+						z_k[8] = raw.magnetometer_ga[2];
+					}
 
 					uint64_t now = hrt_absolute_time();
 					unsigned int time_elapsed = now - last_run;
@@ -508,8 +530,25 @@ const unsigned int loop_interval_alarm = 6500;	// loop interval in microseconds
 						continue;
 					}
 
-					attitudeKalmanfilter(update_vect, dt, z_k, x_aposteriori_k, P_aposteriori_k, ekf_params.q, ekf_params.r,
-							     euler, Rot_matrix, x_aposteriori, P_aposteriori);
+					/* Call the estimator */
+					AttitudeEKF(false, // approx_prediction
+							(unsigned char)ekf_params.use_moment_inertia,
+							update_vect,
+							dt,
+							z_k,
+							ekf_params.q[0], // q_rotSpeed,
+							ekf_params.q[1], // q_rotAcc
+							ekf_params.q[2], // q_acc
+							ekf_params.q[3], // q_mag
+							ekf_params.r[0], // r_gyro
+							ekf_params.r[1], // r_accel
+							ekf_params.r[2], // r_mag
+							ekf_params.moment_inertia_J,
+							x_aposteriori,
+							P_aposteriori,
+							Rot_matrix,
+							euler,
+							debugOutput);
 
 					/* swap values for next iteration, check for fatal inputs */
 					if (isfinite(euler[0]) && isfinite(euler[1]) && isfinite(euler[2])) {
diff --git a/src/modules/attitude_estimator_ekf/attitude_estimator_ekf_params.c b/src/modules/attitude_estimator_ekf/attitude_estimator_ekf_params.c
index bc0e3b93a..5637ec102 100755
--- a/src/modules/attitude_estimator_ekf/attitude_estimator_ekf_params.c
+++ b/src/modules/attitude_estimator_ekf/attitude_estimator_ekf_params.c
@@ -44,28 +44,96 @@
 
 /* Extended Kalman Filter covariances */
 
-/* gyro process noise */
+
+/**
+ * Body angular rate process noise
+ *
+ * @group attitude_ekf
+ */
 PARAM_DEFINE_FLOAT(EKF_ATT_V3_Q0, 1e-4f);
+
+/**
+ * Body angular acceleration process noise
+ *
+ * @group attitude_ekf
+ */
 PARAM_DEFINE_FLOAT(EKF_ATT_V3_Q1, 0.08f);
+
+/**
+ * Acceleration process noise
+ *
+ * @group attitude_ekf
+ */
 PARAM_DEFINE_FLOAT(EKF_ATT_V3_Q2, 0.009f);
-/* gyro offsets process noise */
+
+/**
+ * Magnet field vector process noise
+ *
+ * @group attitude_ekf
+ */
 PARAM_DEFINE_FLOAT(EKF_ATT_V3_Q3, 0.005f);
-PARAM_DEFINE_FLOAT(EKF_ATT_V3_Q4, 0.0f);
 
-/* gyro measurement noise */
+/**
+ * Gyro measurement noise
+ *
+ * @group attitude_ekf
+ */
 PARAM_DEFINE_FLOAT(EKF_ATT_V4_R0, 0.0008f);
-/* accel measurement noise */
+
+/**
+ * Accel measurement noise
+ *
+ * @group attitude_ekf
+ */
 PARAM_DEFINE_FLOAT(EKF_ATT_V4_R1, 10000.0f);
-/* mag measurement noise */
+
+/**
+ * Mag measurement noise
+ *
+ * @group attitude_ekf
+ */
 PARAM_DEFINE_FLOAT(EKF_ATT_V4_R2, 100.0f);
-/* offset estimation - UNUSED */
-PARAM_DEFINE_FLOAT(EKF_ATT_V4_R3, 0.0f);
 
 /* magnetic declination, in degrees */
 PARAM_DEFINE_FLOAT(ATT_MAG_DECL, 0.0f);
 
 PARAM_DEFINE_INT32(ATT_ACC_COMP, 2);
 
+/**
+ * Moment of inertia matrix diagonal entry (1, 1)
+ *
+ * @group attitude_ekf
+ * @unit kg*m^2
+ */
+PARAM_DEFINE_FLOAT(ATT_J11, 0.0018);
+
+/**
+ * Moment of inertia matrix diagonal entry (2, 2)
+ *
+ * @group attitude_ekf
+ * @unit kg*m^2
+ */
+PARAM_DEFINE_FLOAT(ATT_J22, 0.0018);
+
+/**
+ * Moment of inertia matrix diagonal entry (3, 3)
+ *
+ * @group attitude_ekf
+ * @unit kg*m^2
+ */
+PARAM_DEFINE_FLOAT(ATT_J33, 0.0037);
+
+/**
+ * Moment of inertia enabled in estimator
+ *
+ * If set to != 0 the moment of inertia will be used in the estimator
+ *
+ * @group attitude_ekf
+ * @min 0
+ * @max 1
+ */
+PARAM_DEFINE_INT32(ATT_J_EN, 0);
+
 int parameters_init(struct attitude_estimator_ekf_param_handles *h)
 {
 	/* PID parameters */
@@ -73,17 +141,20 @@ int parameters_init(struct attitude_estimator_ekf_param_handles *h)
 	h->q1 	=	param_find("EKF_ATT_V3_Q1");
 	h->q2 	=	param_find("EKF_ATT_V3_Q2");
 	h->q3 	=	param_find("EKF_ATT_V3_Q3");
-	h->q4 	=	param_find("EKF_ATT_V3_Q4");
 
 	h->r0 	=	param_find("EKF_ATT_V4_R0");
 	h->r1 	=	param_find("EKF_ATT_V4_R1");
 	h->r2 	=	param_find("EKF_ATT_V4_R2");
-	h->r3 	=	param_find("EKF_ATT_V4_R3");
 
 	h->mag_decl   =	param_find("ATT_MAG_DECL");
 
 	h->acc_comp   =	param_find("ATT_ACC_COMP");
 
+	h->moment_inertia_J[0]  =   param_find("ATT_J11");
+	h->moment_inertia_J[1]  =   param_find("ATT_J22");
+	h->moment_inertia_J[2]  =   param_find("ATT_J33");
+	h->use_moment_inertia	=   param_find("ATT_J_EN");
+
 	return OK;
 }
 
@@ -93,17 +164,20 @@ int parameters_update(const struct attitude_estimator_ekf_param_handles *h, stru
 	param_get(h->q1, &(p->q[1]));
 	param_get(h->q2, &(p->q[2]));
 	param_get(h->q3, &(p->q[3]));
-	param_get(h->q4, &(p->q[4]));
 
 	param_get(h->r0, &(p->r[0]));
 	param_get(h->r1, &(p->r[1]));
 	param_get(h->r2, &(p->r[2]));
-	param_get(h->r3, &(p->r[3]));
 
 	param_get(h->mag_decl, &(p->mag_decl));
 	p->mag_decl *= M_PI_F / 180.0f;
 
 	param_get(h->acc_comp, &(p->acc_comp));
 
+	for (int i = 0; i < 3; i++) {
+		param_get(h->moment_inertia_J[i], &(p->moment_inertia_J[3 * i + i]));
+	}
+	param_get(h->use_moment_inertia, &(p->use_moment_inertia));
+
 	return OK;
 }
diff --git a/src/modules/attitude_estimator_ekf/attitude_estimator_ekf_params.h b/src/modules/attitude_estimator_ekf/attitude_estimator_ekf_params.h
index 5985541ca..5d3b6b244 100755
--- a/src/modules/attitude_estimator_ekf/attitude_estimator_ekf_params.h
+++ b/src/modules/attitude_estimator_ekf/attitude_estimator_ekf_params.h
@@ -42,8 +42,10 @@
 #include <systemlib/param/param.h>
 
 struct attitude_estimator_ekf_params {
-	float r[9];
-	float q[12];
+	float r[3];
+	float q[4];
+	float moment_inertia_J[9];
+	int32_t use_moment_inertia;
 	float roll_off;
 	float pitch_off;
 	float yaw_off;
@@ -52,8 +54,10 @@ struct attitude_estimator_ekf_params {
 };
 
 struct attitude_estimator_ekf_param_handles {
-	param_t r0, r1, r2, r3;
-	param_t q0, q1, q2, q3, q4;
+	param_t r0, r1, r2;
+	param_t q0, q1, q2, q3;
+	param_t moment_inertia_J[3]; /**< diagonal entries of the matrix */
+	param_t use_moment_inertia;
 	param_t mag_decl;
 	param_t acc_comp;
 };
diff --git a/src/modules/attitude_estimator_ekf/codegen/AttitudeEKF.c b/src/modules/attitude_estimator_ekf/codegen/AttitudeEKF.c
new file mode 100644
index 000000000..68db382cf
--- /dev/null
+++ b/src/modules/attitude_estimator_ekf/codegen/AttitudeEKF.c
@@ -0,0 +1,1456 @@
+/*
+ * AttitudeEKF.c
+ *
+ * Code generation for function 'AttitudeEKF'
+ *
+ * C source code generated on: Thu Aug 21 11:17:28 2014
+ *
+ */
+
+/* Include files */
+#include "AttitudeEKF.h"
+
+/* Variable Definitions */
+static float Ji[9];
+static boolean_T Ji_not_empty;
+static float x_apo[12];
+static float P_apo[144];
+static float Q[144];
+static boolean_T Q_not_empty;
+
+/* Function Declarations */
+static void AttitudeEKF_init(void);
+static void b_mrdivide(const float A[72], const float B[36], float y[72]);
+static void inv(const float x[9], float y[9]);
+static void mrdivide(const float A[108], const float B[81], float y[108]);
+static float norm(const float x[3]);
+
+/* Function Definitions */
+static void AttitudeEKF_init(void)
+{
+  int i;
+  static const float fv5[12] = { 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F,
+    -9.81F, 1.0F, 0.0F, 0.0F };
+
+  for (i = 0; i < 12; i++) {
+    x_apo[i] = fv5[i];
+  }
+
+  for (i = 0; i < 144; i++) {
+    P_apo[i] = 200.0F;
+  }
+}
+
+/*
+ *
+ */
+static void b_mrdivide(const float A[72], const float B[36], float y[72])
+{
+  float b_A[36];
+  signed char ipiv[6];
+  int i1;
+  int iy;
+  int j;
+  int c;
+  int ix;
+  float temp;
+  int k;
+  float s;
+  int jy;
+  int ijA;
+  float Y[72];
+  for (i1 = 0; i1 < 6; i1++) {
+    for (iy = 0; iy < 6; iy++) {
+      b_A[iy + 6 * i1] = B[i1 + 6 * iy];
+    }
+
+    ipiv[i1] = (signed char)(1 + i1);
+  }
+
+  for (j = 0; j < 5; j++) {
+    c = j * 7;
+    iy = 0;
+    ix = c;
+    temp = (real32_T)fabs(b_A[c]);
+    for (k = 2; k <= 6 - j; k++) {
+      ix++;
+      s = (real32_T)fabs(b_A[ix]);
+      if (s > temp) {
+        iy = k - 1;
+        temp = s;
+      }
+    }
+
+    if (b_A[c + iy] != 0.0F) {
+      if (iy != 0) {
+        ipiv[j] = (signed char)((j + iy) + 1);
+        ix = j;
+        iy += j;
+        for (k = 0; k < 6; k++) {
+          temp = b_A[ix];
+          b_A[ix] = b_A[iy];
+          b_A[iy] = temp;
+          ix += 6;
+          iy += 6;
+        }
+      }
+
+      i1 = (c - j) + 6;
+      for (jy = c + 1; jy + 1 <= i1; jy++) {
+        b_A[jy] /= b_A[c];
+      }
+    }
+
+    iy = c;
+    jy = c + 6;
+    for (k = 1; k <= 5 - j; k++) {
+      temp = b_A[jy];
+      if (b_A[jy] != 0.0F) {
+        ix = c + 1;
+        i1 = (iy - j) + 12;
+        for (ijA = 7 + iy; ijA + 1 <= i1; ijA++) {
+          b_A[ijA] += b_A[ix] * -temp;
+          ix++;
+        }
+      }
+
+      jy += 6;
+      iy += 6;
+    }
+  }
+
+  for (i1 = 0; i1 < 12; i1++) {
+    for (iy = 0; iy < 6; iy++) {
+      Y[iy + 6 * i1] = A[i1 + 12 * iy];
+    }
+  }
+
+  for (jy = 0; jy < 6; jy++) {
+    if (ipiv[jy] != jy + 1) {
+      for (j = 0; j < 12; j++) {
+        temp = Y[jy + 6 * j];
+        Y[jy + 6 * j] = Y[(ipiv[jy] + 6 * j) - 1];
+        Y[(ipiv[jy] + 6 * j) - 1] = temp;
+      }
+    }
+  }
+
+  for (j = 0; j < 12; j++) {
+    c = 6 * j;
+    for (k = 0; k < 6; k++) {
+      iy = 6 * k;
+      if (Y[k + c] != 0.0F) {
+        for (jy = k + 2; jy < 7; jy++) {
+          Y[(jy + c) - 1] -= Y[k + c] * b_A[(jy + iy) - 1];
+        }
+      }
+    }
+  }
+
+  for (j = 0; j < 12; j++) {
+    c = 6 * j;
+    for (k = 5; k > -1; k += -1) {
+      iy = 6 * k;
+      if (Y[k + c] != 0.0F) {
+        Y[k + c] /= b_A[k + iy];
+        for (jy = 0; jy + 1 <= k; jy++) {
+          Y[jy + c] -= Y[k + c] * b_A[jy + iy];
+        }
+      }
+    }
+  }
+
+  for (i1 = 0; i1 < 6; i1++) {
+    for (iy = 0; iy < 12; iy++) {
+      y[iy + 12 * i1] = Y[i1 + 6 * iy];
+    }
+  }
+}
+
+/*
+ *
+ */
+static void inv(const float x[9], float y[9])
+{
+  float b_x[9];
+  int p1;
+  int p2;
+  int p3;
+  float absx11;
+  float absx21;
+  float absx31;
+  int itmp;
+  for (p1 = 0; p1 < 9; p1++) {
+    b_x[p1] = x[p1];
+  }
+
+  p1 = 0;
+  p2 = 3;
+  p3 = 6;
+  absx11 = (real32_T)fabs(x[0]);
+  absx21 = (real32_T)fabs(x[1]);
+  absx31 = (real32_T)fabs(x[2]);
+  if ((absx21 > absx11) && (absx21 > absx31)) {
+    p1 = 3;
+    p2 = 0;
+    b_x[0] = x[1];
+    b_x[1] = x[0];
+    b_x[3] = x[4];
+    b_x[4] = x[3];
+    b_x[6] = x[7];
+    b_x[7] = x[6];
+  } else {
+    if (absx31 > absx11) {
+      p1 = 6;
+      p3 = 0;
+      b_x[0] = x[2];
+      b_x[2] = x[0];
+      b_x[3] = x[5];
+      b_x[5] = x[3];
+      b_x[6] = x[8];
+      b_x[8] = x[6];
+    }
+  }
+
+  absx11 = b_x[1] / b_x[0];
+  b_x[1] /= b_x[0];
+  absx21 = b_x[2] / b_x[0];
+  b_x[2] /= b_x[0];
+  b_x[4] -= absx11 * b_x[3];
+  b_x[5] -= absx21 * b_x[3];
+  b_x[7] -= absx11 * b_x[6];
+  b_x[8] -= absx21 * b_x[6];
+  if ((real32_T)fabs(b_x[5]) > (real32_T)fabs(b_x[4])) {
+    itmp = p2;
+    p2 = p3;
+    p3 = itmp;
+    b_x[1] = absx21;
+    b_x[2] = absx11;
+    absx11 = b_x[4];
+    b_x[4] = b_x[5];
+    b_x[5] = absx11;
+    absx11 = b_x[7];
+    b_x[7] = b_x[8];
+    b_x[8] = absx11;
+  }
+
+  absx11 = b_x[5] / b_x[4];
+  b_x[5] /= b_x[4];
+  b_x[8] -= absx11 * b_x[7];
+  absx11 = (b_x[5] * b_x[1] - b_x[2]) / b_x[8];
+  absx21 = -(b_x[1] + b_x[7] * absx11) / b_x[4];
+  y[p1] = ((1.0F - b_x[3] * absx21) - b_x[6] * absx11) / b_x[0];
+  y[p1 + 1] = absx21;
+  y[p1 + 2] = absx11;
+  absx11 = -b_x[5] / b_x[8];
+  absx21 = (1.0F - b_x[7] * absx11) / b_x[4];
+  y[p2] = -(b_x[3] * absx21 + b_x[6] * absx11) / b_x[0];
+  y[p2 + 1] = absx21;
+  y[p2 + 2] = absx11;
+  absx11 = 1.0F / b_x[8];
+  absx21 = -b_x[7] * absx11 / b_x[4];
+  y[p3] = -(b_x[3] * absx21 + b_x[6] * absx11) / b_x[0];
+  y[p3 + 1] = absx21;
+  y[p3 + 2] = absx11;
+}
+
+/*
+ *
+ */
+static void mrdivide(const float A[108], const float B[81], float y[108])
+{
+  float b_A[81];
+  signed char ipiv[9];
+  int i0;
+  int iy;
+  int j;
+  int c;
+  int ix;
+  float temp;
+  int k;
+  float s;
+  int jy;
+  int ijA;
+  float Y[108];
+  for (i0 = 0; i0 < 9; i0++) {
+    for (iy = 0; iy < 9; iy++) {
+      b_A[iy + 9 * i0] = B[i0 + 9 * iy];
+    }
+
+    ipiv[i0] = (signed char)(1 + i0);
+  }
+
+  for (j = 0; j < 8; j++) {
+    c = j * 10;
+    iy = 0;
+    ix = c;
+    temp = (real32_T)fabs(b_A[c]);
+    for (k = 2; k <= 9 - j; k++) {
+      ix++;
+      s = (real32_T)fabs(b_A[ix]);
+      if (s > temp) {
+        iy = k - 1;
+        temp = s;
+      }
+    }
+
+    if (b_A[c + iy] != 0.0F) {
+      if (iy != 0) {
+        ipiv[j] = (signed char)((j + iy) + 1);
+        ix = j;
+        iy += j;
+        for (k = 0; k < 9; k++) {
+          temp = b_A[ix];
+          b_A[ix] = b_A[iy];
+          b_A[iy] = temp;
+          ix += 9;
+          iy += 9;
+        }
+      }
+
+      i0 = (c - j) + 9;
+      for (jy = c + 1; jy + 1 <= i0; jy++) {
+        b_A[jy] /= b_A[c];
+      }
+    }
+
+    iy = c;
+    jy = c + 9;
+    for (k = 1; k <= 8 - j; k++) {
+      temp = b_A[jy];
+      if (b_A[jy] != 0.0F) {
+        ix = c + 1;
+        i0 = (iy - j) + 18;
+        for (ijA = 10 + iy; ijA + 1 <= i0; ijA++) {
+          b_A[ijA] += b_A[ix] * -temp;
+          ix++;
+        }
+      }
+
+      jy += 9;
+      iy += 9;
+    }
+  }
+
+  for (i0 = 0; i0 < 12; i0++) {
+    for (iy = 0; iy < 9; iy++) {
+      Y[iy + 9 * i0] = A[i0 + 12 * iy];
+    }
+  }
+
+  for (jy = 0; jy < 9; jy++) {
+    if (ipiv[jy] != jy + 1) {
+      for (j = 0; j < 12; j++) {
+        temp = Y[jy + 9 * j];
+        Y[jy + 9 * j] = Y[(ipiv[jy] + 9 * j) - 1];
+        Y[(ipiv[jy] + 9 * j) - 1] = temp;
+      }
+    }
+  }
+
+  for (j = 0; j < 12; j++) {
+    c = 9 * j;
+    for (k = 0; k < 9; k++) {
+      iy = 9 * k;
+      if (Y[k + c] != 0.0F) {
+        for (jy = k + 2; jy < 10; jy++) {
+          Y[(jy + c) - 1] -= Y[k + c] * b_A[(jy + iy) - 1];
+        }
+      }
+    }
+  }
+
+  for (j = 0; j < 12; j++) {
+    c = 9 * j;
+    for (k = 8; k > -1; k += -1) {
+      iy = 9 * k;
+      if (Y[k + c] != 0.0F) {
+        Y[k + c] /= b_A[k + iy];
+        for (jy = 0; jy + 1 <= k; jy++) {
+          Y[jy + c] -= Y[k + c] * b_A[jy + iy];
+        }
+      }
+    }
+  }
+
+  for (i0 = 0; i0 < 9; i0++) {
+    for (iy = 0; iy < 12; iy++) {
+      y[iy + 12 * i0] = Y[i0 + 9 * iy];
+    }
+  }
+}
+
+/*
+ *
+ */
+static float norm(const float x[3])
+{
+  float y;
+  float scale;
+  int k;
+  float absxk;
+  float t;
+  y = 0.0F;
+  scale = 1.17549435E-38F;
+  for (k = 0; k < 3; k++) {
+    absxk = (real32_T)fabs(x[k]);
+    if (absxk > scale) {
+      t = scale / absxk;
+      y = 1.0F + y * t * t;
+      scale = absxk;
+    } else {
+      t = absxk / scale;
+      y += t * t;
+    }
+  }
+
+  return scale * (real32_T)sqrt(y);
+}
+
+/*
+ * function [xa_apo,Pa_apo,Rot_matrix,eulerAngles,debugOutput]...
+ *     = AttitudeEKF(approx_prediction,use_inertia_matrix,zFlag,dt,z,q_rotSpeed,q_rotAcc,q_acc,q_mag,r_gyro,r_accel,r_mag,J)
+ */
+void AttitudeEKF(unsigned char approx_prediction, unsigned char
+                 use_inertia_matrix, const unsigned char zFlag[3], float dt,
+                 const float z[9], float q_rotSpeed, float q_rotAcc, float q_acc,
+                 float q_mag, float r_gyro, float r_accel, float r_mag, const
+                 float J[9], float xa_apo[12], float Pa_apo[144], float
+                 Rot_matrix[9], float eulerAngles[3], float debugOutput[4])
+{
+  int i;
+  float fv0[3];
+  int r2;
+  float zek[3];
+  float muk[3];
+  float b_muk[3];
+  float c_muk[3];
+  float fv1[3];
+  float wak[3];
+  float O[9];
+  float b_O[9];
+  static const signed char iv0[9] = { 1, 0, 0, 0, 1, 0, 0, 0, 1 };
+
+  float fv2[3];
+  float maxval;
+  int r1;
+  float fv3[9];
+  float fv4[3];
+  float x_apr[12];
+  signed char I[144];
+  static float A_lin[144];
+  static const signed char iv1[36] = { 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 1,
+    0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0 };
+
+  static float b_A_lin[144];
+  float v[12];
+  static float P_apr[144];
+  float a[108];
+  static const signed char b_a[108] = { 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0,
+    0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0,
+    0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0,
+    0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0,
+    0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1 };
+
+  float S_k[81];
+  static const signed char b[108] = { 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1,
+    0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0,
+    0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0,
+    0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0,
+    0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1 };
+
+  float b_r_gyro[9];
+  float K_k[108];
+  float b_S_k[36];
+  static const signed char c_a[36] = { 1, 0, 0, 0, 1, 0, 0, 0, 1, 0, 0, 0, 0, 0,
+    0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0 };
+
+  static const signed char b_b[36] = { 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1,
+    0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0 };
+
+  float c_r_gyro[3];
+  float B[36];
+  int r3;
+  float a21;
+  float Y[36];
+  float d_a[72];
+  static const signed char e_a[72] = { 1, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0,
+    1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1,
+    0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0,
+    0, 0, 0, 0, 0, 0 };
+
+  static const signed char c_b[72] = { 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1,
+    0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0,
+    0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0,
+    0, 0, 1, 0, 0, 0 };
+
+  float d_r_gyro[6];
+  float c_S_k[6];
+  float b_K_k[72];
+  static const signed char f_a[72] = { 1, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0,
+    1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0,
+    0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 1, 0,
+    0, 0, 0, 0, 0, 1 };
+
+  static const signed char d_b[72] = { 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1,
+    0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0,
+    0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0,
+    0, 0, 0, 0, 0, 1 };
+
+  float b_z[6];
+
+  /* LQG Postion Estimator and Controller */
+  /*  Observer: */
+  /*         x[n|n]   = x[n|n-1] + M(y[n] - Cx[n|n-1] - Du[n]) */
+  /*         x[n+1|n] = Ax[n|n] + Bu[n] */
+  /*  */
+  /*  $Author: Tobias Naegeli $    $Date: 2014 $    $Revision: 3 $ */
+  /*  */
+  /*  */
+  /*  Arguments: */
+  /*  approx_prediction: if 1 then the exponential map is approximated with a */
+  /*  first order taylor approximation. has at the moment not a big influence */
+  /*  (just 1st or 2nd order approximation) we should change it to rodriquez */
+  /*  approximation. */
+  /*  use_inertia_matrix: set to true if you have the inertia matrix J for your */
+  /*  quadrotor */
+  /*  xa_apo_k: old state vectotr */
+  /*  zFlag: if sensor measurement is available [gyro, acc, mag] */
+  /*  dt: dt in s */
+  /*  z: measurements [gyro, acc, mag] */
+  /*  q_rotSpeed: process noise gyro */
+  /*  q_rotAcc: process noise gyro acceleration */
+  /*  q_acc: process noise acceleration */
+  /*  q_mag: process noise magnetometer */
+  /*  r_gyro: measurement noise gyro */
+  /*  r_accel: measurement noise accel */
+  /*  r_mag: measurement noise mag */
+  /*  J: moment of inertia matrix */
+  /*  Output: */
+  /*  xa_apo: updated state vectotr */
+  /*  Pa_apo: updated state covariance matrix */
+  /*  Rot_matrix: rotation matrix */
+  /*  eulerAngles: euler angles */
+  /*  debugOutput: not used */
+  /* % model specific parameters */
+  /*  compute once the inverse of the Inertia */
+  /* 'AttitudeEKF:48' if isempty(Ji) */
+  if (!Ji_not_empty) {
+    /* 'AttitudeEKF:49' Ji=single(inv(J)); */
+    inv(J, Ji);
+    Ji_not_empty = TRUE;
+  }
+
+  /* % init */
+  /* 'AttitudeEKF:54' if(isempty(x_apo)) */
+  /* 'AttitudeEKF:64' if(isempty(P_apo)) */
+  /* 'AttitudeEKF:69' debugOutput = single(zeros(4,1)); */
+  for (i = 0; i < 4; i++) {
+    debugOutput[i] = 0.0F;
+  }
+
+  /* % copy the states */
+  /* 'AttitudeEKF:72' wx=  x_apo(1); */
+  /*  x  body angular rate */
+  /* 'AttitudeEKF:73' wy=  x_apo(2); */
+  /*  y  body angular rate */
+  /* 'AttitudeEKF:74' wz=  x_apo(3); */
+  /*  z  body angular rate */
+  /* 'AttitudeEKF:76' wax=  x_apo(4); */
+  /*  x  body angular acceleration */
+  /* 'AttitudeEKF:77' way=  x_apo(5); */
+  /*  y  body angular acceleration */
+  /* 'AttitudeEKF:78' waz=  x_apo(6); */
+  /*  z  body angular acceleration */
+  /* 'AttitudeEKF:80' zex=  x_apo(7); */
+  /*  x  component gravity vector */
+  /* 'AttitudeEKF:81' zey=  x_apo(8); */
+  /*  y  component gravity vector */
+  /* 'AttitudeEKF:82' zez=  x_apo(9); */
+  /*  z  component gravity vector */
+  /* 'AttitudeEKF:84' mux=  x_apo(10); */
+  /*  x  component magnetic field vector */
+  /* 'AttitudeEKF:85' muy=  x_apo(11); */
+  /*  y  component magnetic field vector */
+  /* 'AttitudeEKF:86' muz=  x_apo(12); */
+  /*  z  component magnetic field vector */
+  /* % prediction section */
+  /*  compute the apriori state estimate from the previous aposteriori estimate */
+  /* body angular accelerations */
+  /* 'AttitudeEKF:94' if (use_inertia_matrix==1) */
+  if (use_inertia_matrix == 1) {
+    /* 'AttitudeEKF:95' wak =[wax;way;waz]+Ji*(-cross([wax;way;waz],J*[wax;way;waz]))*dt; */
+    fv0[0] = x_apo[3];
+    fv0[1] = x_apo[4];
+    fv0[2] = x_apo[5];
+    for (r2 = 0; r2 < 3; r2++) {
+      zek[r2] = 0.0F;
+      for (i = 0; i < 3; i++) {
+        zek[r2] += J[r2 + 3 * i] * fv0[i];
+      }
+    }
+
+    muk[0] = x_apo[3];
+    muk[1] = x_apo[4];
+    muk[2] = x_apo[5];
+    b_muk[0] = x_apo[4] * zek[2] - x_apo[5] * zek[1];
+    b_muk[1] = x_apo[5] * zek[0] - x_apo[3] * zek[2];
+    b_muk[2] = x_apo[3] * zek[1] - x_apo[4] * zek[0];
+    for (r2 = 0; r2 < 3; r2++) {
+      c_muk[r2] = -b_muk[r2];
+    }
+
+    fv1[0] = x_apo[3];
+    fv1[1] = x_apo[4];
+    fv1[2] = x_apo[5];
+    for (r2 = 0; r2 < 3; r2++) {
+      fv0[r2] = 0.0F;
+      for (i = 0; i < 3; i++) {
+        fv0[r2] += Ji[r2 + 3 * i] * c_muk[i];
+      }
+
+      wak[r2] = fv1[r2] + fv0[r2] * dt;
+    }
+  } else {
+    /* 'AttitudeEKF:96' else */
+    /* 'AttitudeEKF:97' wak =[wax;way;waz]; */
+    wak[0] = x_apo[3];
+    wak[1] = x_apo[4];
+    wak[2] = x_apo[5];
+  }
+
+  /* body angular rates */
+  /* 'AttitudeEKF:101' wk =[wx;  wy; wz] + dt*wak; */
+  /* derivative of the prediction rotation matrix */
+  /* 'AttitudeEKF:104' O=[0,-wz,wy;wz,0,-wx;-wy,wx,0]'; */
+  O[0] = 0.0F;
+  O[1] = -x_apo[2];
+  O[2] = x_apo[1];
+  O[3] = x_apo[2];
+  O[4] = 0.0F;
+  O[5] = -x_apo[0];
+  O[6] = -x_apo[1];
+  O[7] = x_apo[0];
+  O[8] = 0.0F;
+
+  /* prediction of the earth z vector */
+  /* 'AttitudeEKF:107' if (approx_prediction==1) */
+  if (approx_prediction == 1) {
+    /* e^(Odt)=I+dt*O+dt^2/2!O^2 */
+    /*  so we do a first order approximation of the exponential map */
+    /* 'AttitudeEKF:110' zek =(O*dt+single(eye(3)))*[zex;zey;zez]; */
+    for (r2 = 0; r2 < 3; r2++) {
+      for (i = 0; i < 3; i++) {
+        b_O[i + 3 * r2] = O[i + 3 * r2] * dt + (float)iv0[i + 3 * r2];
+      }
+    }
+
+    fv2[0] = x_apo[6];
+    fv2[1] = x_apo[7];
+    fv2[2] = x_apo[8];
+    for (r2 = 0; r2 < 3; r2++) {
+      zek[r2] = 0.0F;
+      for (i = 0; i < 3; i++) {
+        zek[r2] += b_O[r2 + 3 * i] * fv2[i];
+      }
+    }
+  } else {
+    /* 'AttitudeEKF:112' else */
+    /* 'AttitudeEKF:113' zek =(single(eye(3))+O*dt+dt^2/2*O^2)*[zex;zey;zez]; */
+    maxval = dt * dt / 2.0F;
+    for (r2 = 0; r2 < 3; r2++) {
+      for (i = 0; i < 3; i++) {
+        b_O[r2 + 3 * i] = 0.0F;
+        for (r1 = 0; r1 < 3; r1++) {
+          b_O[r2 + 3 * i] += O[r2 + 3 * r1] * O[r1 + 3 * i];
+        }
+      }
+    }
+
+    for (r2 = 0; r2 < 3; r2++) {
+      for (i = 0; i < 3; i++) {
+        fv3[i + 3 * r2] = ((float)iv0[i + 3 * r2] + O[i + 3 * r2] * dt) + maxval
+          * b_O[i + 3 * r2];
+      }
+    }
+
+    fv2[0] = x_apo[6];
+    fv2[1] = x_apo[7];
+    fv2[2] = x_apo[8];
+    for (r2 = 0; r2 < 3; r2++) {
+      zek[r2] = 0.0F;
+      for (i = 0; i < 3; i++) {
+        zek[r2] += fv3[r2 + 3 * i] * fv2[i];
+      }
+    }
+
+    /* zek =expm2(O*dt)*[zex;zey;zez]; not working because use double */
+    /* precision */
+  }
+
+  /* prediction of the magnetic vector */
+  /* 'AttitudeEKF:121' if (approx_prediction==1) */
+  if (approx_prediction == 1) {
+    /* e^(Odt)=I+dt*O+dt^2/2!O^2 */
+    /*  so we do a first order approximation of the exponential map */
+    /* 'AttitudeEKF:124' muk =(O*dt+single(eye(3)))*[mux;muy;muz]; */
+    for (r2 = 0; r2 < 3; r2++) {
+      for (i = 0; i < 3; i++) {
+        b_O[i + 3 * r2] = O[i + 3 * r2] * dt + (float)iv0[i + 3 * r2];
+      }
+    }
+
+    fv4[0] = x_apo[9];
+    fv4[1] = x_apo[10];
+    fv4[2] = x_apo[11];
+    for (r2 = 0; r2 < 3; r2++) {
+      muk[r2] = 0.0F;
+      for (i = 0; i < 3; i++) {
+        muk[r2] += b_O[r2 + 3 * i] * fv4[i];
+      }
+    }
+  } else {
+    /* 'AttitudeEKF:125' else */
+    /* 'AttitudeEKF:126' muk =(single(eye(3))+O*dt+dt^2/2*O^2)*[mux;muy;muz]; */
+    maxval = dt * dt / 2.0F;
+    for (r2 = 0; r2 < 3; r2++) {
+      for (i = 0; i < 3; i++) {
+        b_O[r2 + 3 * i] = 0.0F;
+        for (r1 = 0; r1 < 3; r1++) {
+          b_O[r2 + 3 * i] += O[r2 + 3 * r1] * O[r1 + 3 * i];
+        }
+      }
+    }
+
+    for (r2 = 0; r2 < 3; r2++) {
+      for (i = 0; i < 3; i++) {
+        fv3[i + 3 * r2] = ((float)iv0[i + 3 * r2] + O[i + 3 * r2] * dt) + maxval
+          * b_O[i + 3 * r2];
+      }
+    }
+
+    fv4[0] = x_apo[9];
+    fv4[1] = x_apo[10];
+    fv4[2] = x_apo[11];
+    for (r2 = 0; r2 < 3; r2++) {
+      muk[r2] = 0.0F;
+      for (i = 0; i < 3; i++) {
+        muk[r2] += fv3[r2 + 3 * i] * fv4[i];
+      }
+    }
+
+    /* muk =expm2(O*dt)*[mux;muy;muz]; not working because use double */
+    /* precision */
+  }
+
+  /* 'AttitudeEKF:131' x_apr=[wk;wak;zek;muk]; */
+  x_apr[0] = x_apo[0] + dt * wak[0];
+  x_apr[1] = x_apo[1] + dt * wak[1];
+  x_apr[2] = x_apo[2] + dt * wak[2];
+  for (i = 0; i < 3; i++) {
+    x_apr[i + 3] = wak[i];
+  }
+
+  for (i = 0; i < 3; i++) {
+    x_apr[i + 6] = zek[i];
+  }
+
+  for (i = 0; i < 3; i++) {
+    x_apr[i + 9] = muk[i];
+  }
+
+  /*  compute the apriori error covariance estimate from the previous */
+  /* aposteriori estimate */
+  /* 'AttitudeEKF:136' EZ=[0,zez,-zey; */
+  /* 'AttitudeEKF:137'     -zez,0,zex; */
+  /* 'AttitudeEKF:138'     zey,-zex,0]'; */
+  /* 'AttitudeEKF:139' MA=[0,muz,-muy; */
+  /* 'AttitudeEKF:140'     -muz,0,mux; */
+  /* 'AttitudeEKF:141'     muy,-mux,0]'; */
+  /* 'AttitudeEKF:143' E=single(eye(3)); */
+  /* 'AttitudeEKF:144' Z=single(zeros(3)); */
+  /* 'AttitudeEKF:146' A_lin=[ Z,  E,  Z,  Z */
+  /* 'AttitudeEKF:147'     Z,  Z,  Z,  Z */
+  /* 'AttitudeEKF:148'     EZ, Z,  O,  Z */
+  /* 'AttitudeEKF:149'     MA, Z,  Z,  O]; */
+  /* 'AttitudeEKF:151' A_lin=eye(12)+A_lin*dt; */
+  memset(&I[0], 0, 144U * sizeof(signed char));
+  for (i = 0; i < 12; i++) {
+    I[i + 12 * i] = 1;
+    for (r2 = 0; r2 < 3; r2++) {
+      A_lin[r2 + 12 * i] = iv1[r2 + 3 * i];
+    }
+
+    for (r2 = 0; r2 < 3; r2++) {
+      A_lin[(r2 + 12 * i) + 3] = 0.0F;
+    }
+  }
+
+  A_lin[6] = 0.0F;
+  A_lin[7] = x_apo[8];
+  A_lin[8] = -x_apo[7];
+  A_lin[18] = -x_apo[8];
+  A_lin[19] = 0.0F;
+  A_lin[20] = x_apo[6];
+  A_lin[30] = x_apo[7];
+  A_lin[31] = -x_apo[6];
+  A_lin[32] = 0.0F;
+  for (r2 = 0; r2 < 3; r2++) {
+    for (i = 0; i < 3; i++) {
+      A_lin[(i + 12 * (r2 + 3)) + 6] = 0.0F;
+    }
+  }
+
+  for (r2 = 0; r2 < 3; r2++) {
+    for (i = 0; i < 3; i++) {
+      A_lin[(i + 12 * (r2 + 6)) + 6] = O[i + 3 * r2];
+    }
+  }
+
+  for (r2 = 0; r2 < 3; r2++) {
+    for (i = 0; i < 3; i++) {
+      A_lin[(i + 12 * (r2 + 9)) + 6] = 0.0F;
+    }
+  }
+
+  A_lin[9] = 0.0F;
+  A_lin[10] = x_apo[11];
+  A_lin[11] = -x_apo[10];
+  A_lin[21] = -x_apo[11];
+  A_lin[22] = 0.0F;
+  A_lin[23] = x_apo[9];
+  A_lin[33] = x_apo[10];
+  A_lin[34] = -x_apo[9];
+  A_lin[35] = 0.0F;
+  for (r2 = 0; r2 < 3; r2++) {
+    for (i = 0; i < 3; i++) {
+      A_lin[(i + 12 * (r2 + 3)) + 9] = 0.0F;
+    }
+  }
+
+  for (r2 = 0; r2 < 3; r2++) {
+    for (i = 0; i < 3; i++) {
+      A_lin[(i + 12 * (r2 + 6)) + 9] = 0.0F;
+    }
+  }
+
+  for (r2 = 0; r2 < 3; r2++) {
+    for (i = 0; i < 3; i++) {
+      A_lin[(i + 12 * (r2 + 9)) + 9] = O[i + 3 * r2];
+    }
+  }
+
+  for (r2 = 0; r2 < 12; r2++) {
+    for (i = 0; i < 12; i++) {
+      b_A_lin[i + 12 * r2] = (float)I[i + 12 * r2] + A_lin[i + 12 * r2] * dt;
+    }
+  }
+
+  /* process covariance matrix */
+  /* 'AttitudeEKF:156' if (isempty(Q)) */
+  if (!Q_not_empty) {
+    /* 'AttitudeEKF:157' Q=diag([ q_rotSpeed,q_rotSpeed,q_rotSpeed,... */
+    /* 'AttitudeEKF:158'         q_rotAcc,q_rotAcc,q_rotAcc,... */
+    /* 'AttitudeEKF:159'         q_acc,q_acc,q_acc,... */
+    /* 'AttitudeEKF:160'         q_mag,q_mag,q_mag]); */
+    v[0] = q_rotSpeed;
+    v[1] = q_rotSpeed;
+    v[2] = q_rotSpeed;
+    v[3] = q_rotAcc;
+    v[4] = q_rotAcc;
+    v[5] = q_rotAcc;
+    v[6] = q_acc;
+    v[7] = q_acc;
+    v[8] = q_acc;
+    v[9] = q_mag;
+    v[10] = q_mag;
+    v[11] = q_mag;
+    memset(&Q[0], 0, 144U * sizeof(float));
+    for (i = 0; i < 12; i++) {
+      Q[i + 12 * i] = v[i];
+    }
+
+    Q_not_empty = TRUE;
+  }
+
+  /* 'AttitudeEKF:163' P_apr=A_lin*P_apo*A_lin'+Q; */
+  for (r2 = 0; r2 < 12; r2++) {
+    for (i = 0; i < 12; i++) {
+      A_lin[r2 + 12 * i] = 0.0F;
+      for (r1 = 0; r1 < 12; r1++) {
+        A_lin[r2 + 12 * i] += b_A_lin[r2 + 12 * r1] * P_apo[r1 + 12 * i];
+      }
+    }
+  }
+
+  for (r2 = 0; r2 < 12; r2++) {
+    for (i = 0; i < 12; i++) {
+      maxval = 0.0F;
+      for (r1 = 0; r1 < 12; r1++) {
+        maxval += A_lin[r2 + 12 * r1] * b_A_lin[i + 12 * r1];
+      }
+
+      P_apr[r2 + 12 * i] = maxval + Q[r2 + 12 * i];
+    }
+  }
+
+  /* % update */
+  /* 'AttitudeEKF:167' if zFlag(1)==1&&zFlag(2)==1&&zFlag(3)==1 */
+  if ((zFlag[0] == 1) && (zFlag[1] == 1) && (zFlag[2] == 1)) {
+    /*      R=[r_gyro,0,0,0,0,0,0,0,0; */
+    /*          0,r_gyro,0,0,0,0,0,0,0; */
+    /*          0,0,r_gyro,0,0,0,0,0,0; */
+    /*          0,0,0,r_accel,0,0,0,0,0; */
+    /*          0,0,0,0,r_accel,0,0,0,0; */
+    /*          0,0,0,0,0,r_accel,0,0,0; */
+    /*          0,0,0,0,0,0,r_mag,0,0; */
+    /*          0,0,0,0,0,0,0,r_mag,0; */
+    /*          0,0,0,0,0,0,0,0,r_mag]; */
+    /* 'AttitudeEKF:178' R_v=[r_gyro,r_gyro,r_gyro,r_accel,r_accel,r_accel,r_mag,r_mag,r_mag]; */
+    /* observation matrix */
+    /* [zw;ze;zmk]; */
+    /* 'AttitudeEKF:181' H_k=[  E,     Z,      Z,    Z; */
+    /* 'AttitudeEKF:182'         Z,     Z,      E,    Z; */
+    /* 'AttitudeEKF:183'         Z,     Z,      Z,    E]; */
+    /* 'AttitudeEKF:185' y_k=z(1:9)-H_k*x_apr; */
+    /* S_k=H_k*P_apr*H_k'+R; */
+    /* 'AttitudeEKF:189' S_k=H_k*P_apr*H_k'; */
+    for (r2 = 0; r2 < 9; r2++) {
+      for (i = 0; i < 12; i++) {
+        a[r2 + 9 * i] = 0.0F;
+        for (r1 = 0; r1 < 12; r1++) {
+          a[r2 + 9 * i] += (float)b_a[r2 + 9 * r1] * P_apr[r1 + 12 * i];
+        }
+      }
+
+      for (i = 0; i < 9; i++) {
+        S_k[r2 + 9 * i] = 0.0F;
+        for (r1 = 0; r1 < 12; r1++) {
+          S_k[r2 + 9 * i] += a[r2 + 9 * r1] * (float)b[r1 + 12 * i];
+        }
+      }
+    }
+
+    /* 'AttitudeEKF:190' S_k(1:9+1:end) = S_k(1:9+1:end) + R_v; */
+    b_r_gyro[0] = r_gyro;
+    b_r_gyro[1] = r_gyro;
+    b_r_gyro[2] = r_gyro;
+    b_r_gyro[3] = r_accel;
+    b_r_gyro[4] = r_accel;
+    b_r_gyro[5] = r_accel;
+    b_r_gyro[6] = r_mag;
+    b_r_gyro[7] = r_mag;
+    b_r_gyro[8] = r_mag;
+    for (r2 = 0; r2 < 9; r2++) {
+      b_O[r2] = S_k[10 * r2] + b_r_gyro[r2];
+    }
+
+    for (r2 = 0; r2 < 9; r2++) {
+      S_k[10 * r2] = b_O[r2];
+    }
+
+    /* 'AttitudeEKF:191' K_k=(P_apr*H_k'/(S_k)); */
+    for (r2 = 0; r2 < 12; r2++) {
+      for (i = 0; i < 9; i++) {
+        a[r2 + 12 * i] = 0.0F;
+        for (r1 = 0; r1 < 12; r1++) {
+          a[r2 + 12 * i] += P_apr[r2 + 12 * r1] * (float)b[r1 + 12 * i];
+        }
+      }
+    }
+
+    mrdivide(a, S_k, K_k);
+
+    /* 'AttitudeEKF:194' x_apo=x_apr+K_k*y_k; */
+    for (r2 = 0; r2 < 9; r2++) {
+      maxval = 0.0F;
+      for (i = 0; i < 12; i++) {
+        maxval += (float)b_a[r2 + 9 * i] * x_apr[i];
+      }
+
+      b_O[r2] = z[r2] - maxval;
+    }
+
+    for (r2 = 0; r2 < 12; r2++) {
+      maxval = 0.0F;
+      for (i = 0; i < 9; i++) {
+        maxval += K_k[r2 + 12 * i] * b_O[i];
+      }
+
+      x_apo[r2] = x_apr[r2] + maxval;
+    }
+
+    /* 'AttitudeEKF:195' P_apo=(eye(12)-K_k*H_k)*P_apr; */
+    memset(&I[0], 0, 144U * sizeof(signed char));
+    for (i = 0; i < 12; i++) {
+      I[i + 12 * i] = 1;
+    }
+
+    for (r2 = 0; r2 < 12; r2++) {
+      for (i = 0; i < 12; i++) {
+        maxval = 0.0F;
+        for (r1 = 0; r1 < 9; r1++) {
+          maxval += K_k[r2 + 12 * r1] * (float)b_a[r1 + 9 * i];
+        }
+
+        A_lin[r2 + 12 * i] = (float)I[r2 + 12 * i] - maxval;
+      }
+    }
+
+    for (r2 = 0; r2 < 12; r2++) {
+      for (i = 0; i < 12; i++) {
+        P_apo[r2 + 12 * i] = 0.0F;
+        for (r1 = 0; r1 < 12; r1++) {
+          P_apo[r2 + 12 * i] += A_lin[r2 + 12 * r1] * P_apr[r1 + 12 * i];
+        }
+      }
+    }
+  } else {
+    /* 'AttitudeEKF:196' else */
+    /* 'AttitudeEKF:197' if zFlag(1)==1&&zFlag(2)==0&&zFlag(3)==0 */
+    if ((zFlag[0] == 1) && (zFlag[1] == 0) && (zFlag[2] == 0)) {
+      /* 'AttitudeEKF:199' R=[r_gyro,0,0; */
+      /* 'AttitudeEKF:200'             0,r_gyro,0; */
+      /* 'AttitudeEKF:201'             0,0,r_gyro]; */
+      /* 'AttitudeEKF:202' R_v=[r_gyro,r_gyro,r_gyro]; */
+      /* observation matrix */
+      /* 'AttitudeEKF:205' H_k=[  E,     Z,      Z,    Z]; */
+      /* 'AttitudeEKF:207' y_k=z(1:3)-H_k(1:3,1:12)*x_apr; */
+      /*  S_k=H_k(1:3,1:12)*P_apr*H_k(1:3,1:12)'+R(1:3,1:3); */
+      /* 'AttitudeEKF:210' S_k=H_k(1:3,1:12)*P_apr*H_k(1:3,1:12)'; */
+      for (r2 = 0; r2 < 3; r2++) {
+        for (i = 0; i < 12; i++) {
+          b_S_k[r2 + 3 * i] = 0.0F;
+          for (r1 = 0; r1 < 12; r1++) {
+            b_S_k[r2 + 3 * i] += (float)c_a[r2 + 3 * r1] * P_apr[r1 + 12 * i];
+          }
+        }
+
+        for (i = 0; i < 3; i++) {
+          O[r2 + 3 * i] = 0.0F;
+          for (r1 = 0; r1 < 12; r1++) {
+            O[r2 + 3 * i] += b_S_k[r2 + 3 * r1] * (float)b_b[r1 + 12 * i];
+          }
+        }
+      }
+
+      /* 'AttitudeEKF:211' S_k(1:3+1:end) = S_k(1:3+1:end) + R_v; */
+      c_r_gyro[0] = r_gyro;
+      c_r_gyro[1] = r_gyro;
+      c_r_gyro[2] = r_gyro;
+      for (r2 = 0; r2 < 3; r2++) {
+        b_muk[r2] = O[r2 << 2] + c_r_gyro[r2];
+      }
+
+      for (r2 = 0; r2 < 3; r2++) {
+        O[r2 << 2] = b_muk[r2];
+      }
+
+      /* 'AttitudeEKF:212' K_k=(P_apr*H_k(1:3,1:12)'/(S_k)); */
+      for (r2 = 0; r2 < 3; r2++) {
+        for (i = 0; i < 3; i++) {
+          b_O[i + 3 * r2] = O[r2 + 3 * i];
+        }
+
+        for (i = 0; i < 12; i++) {
+          B[r2 + 3 * i] = 0.0F;
+          for (r1 = 0; r1 < 12; r1++) {
+            B[r2 + 3 * i] += P_apr[i + 12 * r1] * (float)b_b[r1 + 12 * r2];
+          }
+        }
+      }
+
+      r1 = 0;
+      r2 = 1;
+      r3 = 2;
+      maxval = (real32_T)fabs(b_O[0]);
+      a21 = (real32_T)fabs(b_O[1]);
+      if (a21 > maxval) {
+        maxval = a21;
+        r1 = 1;
+        r2 = 0;
+      }
+
+      if ((real32_T)fabs(b_O[2]) > maxval) {
+        r1 = 2;
+        r2 = 1;
+        r3 = 0;
+      }
+
+      b_O[r2] /= b_O[r1];
+      b_O[r3] /= b_O[r1];
+      b_O[3 + r2] -= b_O[r2] * b_O[3 + r1];
+      b_O[3 + r3] -= b_O[r3] * b_O[3 + r1];
+      b_O[6 + r2] -= b_O[r2] * b_O[6 + r1];
+      b_O[6 + r3] -= b_O[r3] * b_O[6 + r1];
+      if ((real32_T)fabs(b_O[3 + r3]) > (real32_T)fabs(b_O[3 + r2])) {
+        i = r2;
+        r2 = r3;
+        r3 = i;
+      }
+
+      b_O[3 + r3] /= b_O[3 + r2];
+      b_O[6 + r3] -= b_O[3 + r3] * b_O[6 + r2];
+      for (i = 0; i < 12; i++) {
+        Y[3 * i] = B[r1 + 3 * i];
+        Y[1 + 3 * i] = B[r2 + 3 * i] - Y[3 * i] * b_O[r2];
+        Y[2 + 3 * i] = (B[r3 + 3 * i] - Y[3 * i] * b_O[r3]) - Y[1 + 3 * i] *
+          b_O[3 + r3];
+        Y[2 + 3 * i] /= b_O[6 + r3];
+        Y[3 * i] -= Y[2 + 3 * i] * b_O[6 + r1];
+        Y[1 + 3 * i] -= Y[2 + 3 * i] * b_O[6 + r2];
+        Y[1 + 3 * i] /= b_O[3 + r2];
+        Y[3 * i] -= Y[1 + 3 * i] * b_O[3 + r1];
+        Y[3 * i] /= b_O[r1];
+      }
+
+      for (r2 = 0; r2 < 3; r2++) {
+        for (i = 0; i < 12; i++) {
+          b_S_k[i + 12 * r2] = Y[r2 + 3 * i];
+        }
+      }
+
+      /* 'AttitudeEKF:215' x_apo=x_apr+K_k*y_k; */
+      for (r2 = 0; r2 < 3; r2++) {
+        maxval = 0.0F;
+        for (i = 0; i < 12; i++) {
+          maxval += (float)c_a[r2 + 3 * i] * x_apr[i];
+        }
+
+        b_muk[r2] = z[r2] - maxval;
+      }
+
+      for (r2 = 0; r2 < 12; r2++) {
+        maxval = 0.0F;
+        for (i = 0; i < 3; i++) {
+          maxval += b_S_k[r2 + 12 * i] * b_muk[i];
+        }
+
+        x_apo[r2] = x_apr[r2] + maxval;
+      }
+
+      /* 'AttitudeEKF:216' P_apo=(eye(12)-K_k*H_k(1:3,1:12))*P_apr; */
+      memset(&I[0], 0, 144U * sizeof(signed char));
+      for (i = 0; i < 12; i++) {
+        I[i + 12 * i] = 1;
+      }
+
+      for (r2 = 0; r2 < 12; r2++) {
+        for (i = 0; i < 12; i++) {
+          maxval = 0.0F;
+          for (r1 = 0; r1 < 3; r1++) {
+            maxval += b_S_k[r2 + 12 * r1] * (float)c_a[r1 + 3 * i];
+          }
+
+          A_lin[r2 + 12 * i] = (float)I[r2 + 12 * i] - maxval;
+        }
+      }
+
+      for (r2 = 0; r2 < 12; r2++) {
+        for (i = 0; i < 12; i++) {
+          P_apo[r2 + 12 * i] = 0.0F;
+          for (r1 = 0; r1 < 12; r1++) {
+            P_apo[r2 + 12 * i] += A_lin[r2 + 12 * r1] * P_apr[r1 + 12 * i];
+          }
+        }
+      }
+    } else {
+      /* 'AttitudeEKF:217' else */
+      /* 'AttitudeEKF:218' if  zFlag(1)==1&&zFlag(2)==1&&zFlag(3)==0 */
+      if ((zFlag[0] == 1) && (zFlag[1] == 1) && (zFlag[2] == 0)) {
+        /*              R=[r_gyro,0,0,0,0,0; */
+        /*                  0,r_gyro,0,0,0,0; */
+        /*                  0,0,r_gyro,0,0,0; */
+        /*                  0,0,0,r_accel,0,0; */
+        /*                  0,0,0,0,r_accel,0; */
+        /*                  0,0,0,0,0,r_accel]; */
+        /* 'AttitudeEKF:227' R_v=[r_gyro,r_gyro,r_gyro,r_accel,r_accel,r_accel]; */
+        /* observation matrix */
+        /* 'AttitudeEKF:230' H_k=[  E,     Z,      Z,    Z; */
+        /* 'AttitudeEKF:231'                 Z,     Z,      E,    Z]; */
+        /* 'AttitudeEKF:233' y_k=z(1:6)-H_k(1:6,1:12)*x_apr; */
+        /*  S_k=H_k(1:6,1:12)*P_apr*H_k(1:6,1:12)'+R(1:6,1:6); */
+        /* 'AttitudeEKF:236' S_k=H_k(1:6,1:12)*P_apr*H_k(1:6,1:12)'; */
+        for (r2 = 0; r2 < 6; r2++) {
+          for (i = 0; i < 12; i++) {
+            d_a[r2 + 6 * i] = 0.0F;
+            for (r1 = 0; r1 < 12; r1++) {
+              d_a[r2 + 6 * i] += (float)e_a[r2 + 6 * r1] * P_apr[r1 + 12 * i];
+            }
+          }
+
+          for (i = 0; i < 6; i++) {
+            b_S_k[r2 + 6 * i] = 0.0F;
+            for (r1 = 0; r1 < 12; r1++) {
+              b_S_k[r2 + 6 * i] += d_a[r2 + 6 * r1] * (float)c_b[r1 + 12 * i];
+            }
+          }
+        }
+
+        /* 'AttitudeEKF:237' S_k(1:6+1:end) = S_k(1:6+1:end) + R_v; */
+        d_r_gyro[0] = r_gyro;
+        d_r_gyro[1] = r_gyro;
+        d_r_gyro[2] = r_gyro;
+        d_r_gyro[3] = r_accel;
+        d_r_gyro[4] = r_accel;
+        d_r_gyro[5] = r_accel;
+        for (r2 = 0; r2 < 6; r2++) {
+          c_S_k[r2] = b_S_k[7 * r2] + d_r_gyro[r2];
+        }
+
+        for (r2 = 0; r2 < 6; r2++) {
+          b_S_k[7 * r2] = c_S_k[r2];
+        }
+
+        /* 'AttitudeEKF:238' K_k=(P_apr*H_k(1:6,1:12)'/(S_k)); */
+        for (r2 = 0; r2 < 12; r2++) {
+          for (i = 0; i < 6; i++) {
+            d_a[r2 + 12 * i] = 0.0F;
+            for (r1 = 0; r1 < 12; r1++) {
+              d_a[r2 + 12 * i] += P_apr[r2 + 12 * r1] * (float)c_b[r1 + 12 * i];
+            }
+          }
+        }
+
+        b_mrdivide(d_a, b_S_k, b_K_k);
+
+        /* 'AttitudeEKF:241' x_apo=x_apr+K_k*y_k; */
+        for (r2 = 0; r2 < 6; r2++) {
+          maxval = 0.0F;
+          for (i = 0; i < 12; i++) {
+            maxval += (float)e_a[r2 + 6 * i] * x_apr[i];
+          }
+
+          d_r_gyro[r2] = z[r2] - maxval;
+        }
+
+        for (r2 = 0; r2 < 12; r2++) {
+          maxval = 0.0F;
+          for (i = 0; i < 6; i++) {
+            maxval += b_K_k[r2 + 12 * i] * d_r_gyro[i];
+          }
+
+          x_apo[r2] = x_apr[r2] + maxval;
+        }
+
+        /* 'AttitudeEKF:242' P_apo=(eye(12)-K_k*H_k(1:6,1:12))*P_apr; */
+        memset(&I[0], 0, 144U * sizeof(signed char));
+        for (i = 0; i < 12; i++) {
+          I[i + 12 * i] = 1;
+        }
+
+        for (r2 = 0; r2 < 12; r2++) {
+          for (i = 0; i < 12; i++) {
+            maxval = 0.0F;
+            for (r1 = 0; r1 < 6; r1++) {
+              maxval += b_K_k[r2 + 12 * r1] * (float)e_a[r1 + 6 * i];
+            }
+
+            A_lin[r2 + 12 * i] = (float)I[r2 + 12 * i] - maxval;
+          }
+        }
+
+        for (r2 = 0; r2 < 12; r2++) {
+          for (i = 0; i < 12; i++) {
+            P_apo[r2 + 12 * i] = 0.0F;
+            for (r1 = 0; r1 < 12; r1++) {
+              P_apo[r2 + 12 * i] += A_lin[r2 + 12 * r1] * P_apr[r1 + 12 * i];
+            }
+          }
+        }
+      } else {
+        /* 'AttitudeEKF:243' else */
+        /* 'AttitudeEKF:244' if  zFlag(1)==1&&zFlag(2)==0&&zFlag(3)==1 */
+        if ((zFlag[0] == 1) && (zFlag[1] == 0) && (zFlag[2] == 1)) {
+          /*                  R=[r_gyro,0,0,0,0,0; */
+          /*                      0,r_gyro,0,0,0,0; */
+          /*                      0,0,r_gyro,0,0,0; */
+          /*                      0,0,0,r_mag,0,0; */
+          /*                      0,0,0,0,r_mag,0; */
+          /*                      0,0,0,0,0,r_mag]; */
+          /* 'AttitudeEKF:251' R_v=[r_gyro,r_gyro,r_gyro,r_mag,r_mag,r_mag]; */
+          /* observation matrix */
+          /* 'AttitudeEKF:254' H_k=[  E,     Z,      Z,    Z; */
+          /* 'AttitudeEKF:255'                     Z,     Z,      Z,    E]; */
+          /* 'AttitudeEKF:257' y_k=[z(1:3);z(7:9)]-H_k(1:6,1:12)*x_apr; */
+          /* S_k=H_k(1:6,1:12)*P_apr*H_k(1:6,1:12)'+R(1:6,1:6); */
+          /* 'AttitudeEKF:260' S_k=H_k(1:6,1:12)*P_apr*H_k(1:6,1:12)'; */
+          for (r2 = 0; r2 < 6; r2++) {
+            for (i = 0; i < 12; i++) {
+              d_a[r2 + 6 * i] = 0.0F;
+              for (r1 = 0; r1 < 12; r1++) {
+                d_a[r2 + 6 * i] += (float)f_a[r2 + 6 * r1] * P_apr[r1 + 12 * i];
+              }
+            }
+
+            for (i = 0; i < 6; i++) {
+              b_S_k[r2 + 6 * i] = 0.0F;
+              for (r1 = 0; r1 < 12; r1++) {
+                b_S_k[r2 + 6 * i] += d_a[r2 + 6 * r1] * (float)d_b[r1 + 12 * i];
+              }
+            }
+          }
+
+          /* 'AttitudeEKF:261' S_k(1:6+1:end) = S_k(1:6+1:end) + R_v; */
+          d_r_gyro[0] = r_gyro;
+          d_r_gyro[1] = r_gyro;
+          d_r_gyro[2] = r_gyro;
+          d_r_gyro[3] = r_mag;
+          d_r_gyro[4] = r_mag;
+          d_r_gyro[5] = r_mag;
+          for (r2 = 0; r2 < 6; r2++) {
+            c_S_k[r2] = b_S_k[7 * r2] + d_r_gyro[r2];
+          }
+
+          for (r2 = 0; r2 < 6; r2++) {
+            b_S_k[7 * r2] = c_S_k[r2];
+          }
+
+          /* 'AttitudeEKF:262' K_k=(P_apr*H_k(1:6,1:12)'/(S_k)); */
+          for (r2 = 0; r2 < 12; r2++) {
+            for (i = 0; i < 6; i++) {
+              d_a[r2 + 12 * i] = 0.0F;
+              for (r1 = 0; r1 < 12; r1++) {
+                d_a[r2 + 12 * i] += P_apr[r2 + 12 * r1] * (float)d_b[r1 + 12 * i];
+              }
+            }
+          }
+
+          b_mrdivide(d_a, b_S_k, b_K_k);
+
+          /* 'AttitudeEKF:265' x_apo=x_apr+K_k*y_k; */
+          for (r2 = 0; r2 < 3; r2++) {
+            d_r_gyro[r2] = z[r2];
+          }
+
+          for (r2 = 0; r2 < 3; r2++) {
+            d_r_gyro[r2 + 3] = z[6 + r2];
+          }
+
+          for (r2 = 0; r2 < 6; r2++) {
+            c_S_k[r2] = 0.0F;
+            for (i = 0; i < 12; i++) {
+              c_S_k[r2] += (float)f_a[r2 + 6 * i] * x_apr[i];
+            }
+
+            b_z[r2] = d_r_gyro[r2] - c_S_k[r2];
+          }
+
+          for (r2 = 0; r2 < 12; r2++) {
+            maxval = 0.0F;
+            for (i = 0; i < 6; i++) {
+              maxval += b_K_k[r2 + 12 * i] * b_z[i];
+            }
+
+            x_apo[r2] = x_apr[r2] + maxval;
+          }
+
+          /* 'AttitudeEKF:266' P_apo=(eye(12)-K_k*H_k(1:6,1:12))*P_apr; */
+          memset(&I[0], 0, 144U * sizeof(signed char));
+          for (i = 0; i < 12; i++) {
+            I[i + 12 * i] = 1;
+          }
+
+          for (r2 = 0; r2 < 12; r2++) {
+            for (i = 0; i < 12; i++) {
+              maxval = 0.0F;
+              for (r1 = 0; r1 < 6; r1++) {
+                maxval += b_K_k[r2 + 12 * r1] * (float)f_a[r1 + 6 * i];
+              }
+
+              A_lin[r2 + 12 * i] = (float)I[r2 + 12 * i] - maxval;
+            }
+          }
+
+          for (r2 = 0; r2 < 12; r2++) {
+            for (i = 0; i < 12; i++) {
+              P_apo[r2 + 12 * i] = 0.0F;
+              for (r1 = 0; r1 < 12; r1++) {
+                P_apo[r2 + 12 * i] += A_lin[r2 + 12 * r1] * P_apr[r1 + 12 * i];
+              }
+            }
+          }
+        } else {
+          /* 'AttitudeEKF:267' else */
+          /* 'AttitudeEKF:268' x_apo=x_apr; */
+          for (i = 0; i < 12; i++) {
+            x_apo[i] = x_apr[i];
+          }
+
+          /* 'AttitudeEKF:269' P_apo=P_apr; */
+          memcpy(&P_apo[0], &P_apr[0], 144U * sizeof(float));
+        }
+      }
+    }
+  }
+
+  /* % euler anglels extraction */
+  /* 'AttitudeEKF:278' z_n_b = -x_apo(7:9)./norm(x_apo(7:9)); */
+  maxval = norm(*(float (*)[3])&x_apo[6]);
+  a21 = norm(*(float (*)[3])&x_apo[9]);
+  for (i = 0; i < 3; i++) {
+    /* 'AttitudeEKF:279' m_n_b = x_apo(10:12)./norm(x_apo(10:12)); */
+    muk[i] = -x_apo[i + 6] / maxval;
+    zek[i] = x_apo[i + 9] / a21;
+  }
+
+  /* 'AttitudeEKF:281' y_n_b=cross(z_n_b,m_n_b); */
+  wak[0] = muk[1] * zek[2] - muk[2] * zek[1];
+  wak[1] = muk[2] * zek[0] - muk[0] * zek[2];
+  wak[2] = muk[0] * zek[1] - muk[1] * zek[0];
+
+  /* 'AttitudeEKF:282' y_n_b=y_n_b./norm(y_n_b); */
+  maxval = norm(wak);
+  for (r2 = 0; r2 < 3; r2++) {
+    wak[r2] /= maxval;
+  }
+
+  /* 'AttitudeEKF:284' x_n_b=(cross(y_n_b,z_n_b)); */
+  zek[0] = wak[1] * muk[2] - wak[2] * muk[1];
+  zek[1] = wak[2] * muk[0] - wak[0] * muk[2];
+  zek[2] = wak[0] * muk[1] - wak[1] * muk[0];
+
+  /* 'AttitudeEKF:285' x_n_b=x_n_b./norm(x_n_b); */
+  maxval = norm(zek);
+  for (r2 = 0; r2 < 3; r2++) {
+    zek[r2] /= maxval;
+  }
+
+  /* 'AttitudeEKF:288' xa_apo=x_apo; */
+  for (i = 0; i < 12; i++) {
+    xa_apo[i] = x_apo[i];
+  }
+
+  /* 'AttitudeEKF:289' Pa_apo=P_apo; */
+  memcpy(&Pa_apo[0], &P_apo[0], 144U * sizeof(float));
+
+  /*  rotation matrix from earth to body system */
+  /* 'AttitudeEKF:291' Rot_matrix=[x_n_b,y_n_b,z_n_b]; */
+  for (r2 = 0; r2 < 3; r2++) {
+    Rot_matrix[r2] = zek[r2];
+    Rot_matrix[3 + r2] = wak[r2];
+    Rot_matrix[6 + r2] = muk[r2];
+  }
+
+  /* 'AttitudeEKF:294' phi=atan2(Rot_matrix(2,3),Rot_matrix(3,3)); */
+  /* 'AttitudeEKF:295' theta=-asin(Rot_matrix(1,3)); */
+  /* 'AttitudeEKF:296' psi=atan2(Rot_matrix(1,2),Rot_matrix(1,1)); */
+  /* 'AttitudeEKF:297' eulerAngles=[phi;theta;psi]; */
+  eulerAngles[0] = (real32_T)atan2(Rot_matrix[7], Rot_matrix[8]);
+  eulerAngles[1] = -(real32_T)asin(Rot_matrix[6]);
+  eulerAngles[2] = (real32_T)atan2(Rot_matrix[3], Rot_matrix[0]);
+}
+
+void AttitudeEKF_initialize(void)
+{
+  Q_not_empty = FALSE;
+  Ji_not_empty = FALSE;
+  AttitudeEKF_init();
+}
+
+void AttitudeEKF_terminate(void)
+{
+  /* (no terminate code required) */
+}
+
+/* End of code generation (AttitudeEKF.c) */
diff --git a/src/modules/attitude_estimator_ekf/codegen/AttitudeEKF.h b/src/modules/attitude_estimator_ekf/codegen/AttitudeEKF.h
new file mode 100644
index 000000000..7094da1da
--- /dev/null
+++ b/src/modules/attitude_estimator_ekf/codegen/AttitudeEKF.h
@@ -0,0 +1,26 @@
+/*
+ * AttitudeEKF.h
+ *
+ * Code generation for function 'AttitudeEKF'
+ *
+ * C source code generated on: Thu Aug 21 11:17:28 2014
+ *
+ */
+
+#ifndef __ATTITUDEEKF_H__
+#define __ATTITUDEEKF_H__
+/* Include files */
+#include <math.h>
+#include <stddef.h>
+#include <stdlib.h>
+#include <string.h>
+
+#include "rtwtypes.h"
+#include "AttitudeEKF_types.h"
+
+/* Function Declarations */
+extern void AttitudeEKF(unsigned char approx_prediction, unsigned char use_inertia_matrix, const unsigned char zFlag[3], float dt, const float z[9], float q_rotSpeed, float q_rotAcc, float q_acc, float q_mag, float r_gyro, float r_accel, float r_mag, const float J[9], float xa_apo[12], float Pa_apo[144], float Rot_matrix[9], float eulerAngles[3], float debugOutput[4]);
+extern void AttitudeEKF_initialize(void);
+extern void AttitudeEKF_terminate(void);
+#endif
+/* End of code generation (AttitudeEKF.h) */
diff --git a/src/modules/attitude_estimator_ekf/codegen/AttitudeEKF_types.h b/src/modules/attitude_estimator_ekf/codegen/AttitudeEKF_types.h
new file mode 100644
index 000000000..3f7522ffa
--- /dev/null
+++ b/src/modules/attitude_estimator_ekf/codegen/AttitudeEKF_types.h
@@ -0,0 +1,17 @@
+/*
+ * AttitudeEKF_types.h
+ *
+ * Code generation for function 'AttitudeEKF'
+ *
+ * C source code generated on: Thu Aug 21 11:17:28 2014
+ *
+ */
+
+#ifndef __ATTITUDEEKF_TYPES_H__
+#define __ATTITUDEEKF_TYPES_H__
+
+/* Include files */
+#include "rtwtypes.h"
+
+#endif
+/* End of code generation (AttitudeEKF_types.h) */
diff --git a/src/modules/attitude_estimator_ekf/codegen/attitudeKalmanfilter.c b/src/modules/attitude_estimator_ekf/codegen/attitudeKalmanfilter.c
deleted file mode 100755
index 9e97ad11a..000000000
--- a/src/modules/attitude_estimator_ekf/codegen/attitudeKalmanfilter.c
+++ /dev/null
@@ -1,1148 +0,0 @@
-/*

- * attitudeKalmanfilter.c

- *

- * Code generation for function 'attitudeKalmanfilter'

- *

- * C source code generated on: Sat Jan 19 15:25:29 2013

- *

- */

-

-/* Include files */

-#include "rt_nonfinite.h"

-#include "attitudeKalmanfilter.h"

-#include "rdivide.h"

-#include "norm.h"

-#include "cross.h"

-#include "eye.h"

-#include "mrdivide.h"

-

-/* Type Definitions */

-

-/* Named Constants */

-

-/* Variable Declarations */

-

-/* Variable Definitions */

-

-/* Function Declarations */

-static real32_T rt_atan2f_snf(real32_T u0, real32_T u1);

-

-/* Function Definitions */

-static real32_T rt_atan2f_snf(real32_T u0, real32_T u1)

-{

-  real32_T y;

-  int32_T b_u0;

-  int32_T b_u1;

-  if (rtIsNaNF(u0) || rtIsNaNF(u1)) {

-    y = ((real32_T)rtNaN);

-  } else if (rtIsInfF(u0) && rtIsInfF(u1)) {

-    if (u0 > 0.0F) {

-      b_u0 = 1;

-    } else {

-      b_u0 = -1;

-    }

-

-    if (u1 > 0.0F) {

-      b_u1 = 1;

-    } else {

-      b_u1 = -1;

-    }

-

-    y = (real32_T)atan2((real32_T)b_u0, (real32_T)b_u1);

-  } else if (u1 == 0.0F) {

-    if (u0 > 0.0F) {

-      y = RT_PIF / 2.0F;

-    } else if (u0 < 0.0F) {

-      y = -(RT_PIF / 2.0F);

-    } else {

-      y = 0.0F;

-    }

-  } else {

-    y = (real32_T)atan2(u0, u1);

-  }

-

-  return y;

-}

-

-/*

- * function [eulerAngles,Rot_matrix,x_aposteriori,P_aposteriori] = attitudeKalmanfilter_wo(updateVect,dt,z,x_aposteriori_k,P_aposteriori_k,q,r)

- */

-void attitudeKalmanfilter(const uint8_T updateVect[3], real32_T dt, const

-  real32_T z[9], const real32_T x_aposteriori_k[12], const real32_T

-  P_aposteriori_k[144], const real32_T q[12], real32_T r[9], real32_T

-  eulerAngles[3], real32_T Rot_matrix[9], real32_T x_aposteriori[12], real32_T

-  P_aposteriori[144])

-{

-  real32_T wak[3];

-  real32_T O[9];

-  real_T dv0[9];

-  real32_T a[9];

-  int32_T i;

-  real32_T b_a[9];

-  real32_T x_n_b[3];

-  real32_T b_x_aposteriori_k[3];

-  real32_T z_n_b[3];

-  real32_T c_a[3];

-  real32_T d_a[3];

-  int32_T i0;

-  real32_T x_apriori[12];

-  real_T dv1[144];

-  real32_T A_lin[144];

-  static const int8_T iv0[36] = { 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 1, 0, 0,

-    0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0 };

-

-  real32_T b_A_lin[144];

-  real32_T b_q[144];

-  real32_T c_A_lin[144];

-  real32_T d_A_lin[144];

-  real32_T e_A_lin[144];

-  int32_T i1;

-  real32_T P_apriori[144];

-  real32_T b_P_apriori[108];

-  static const int8_T iv1[108] = { 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 0,

-    0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0,

-    0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0,

-    0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0,

-    0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1 };

-

-  real32_T K_k[108];

-  real32_T fv0[81];

-  static const int8_T iv2[108] = { 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0,

-    0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0,

-    0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0,

-    1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0,

-    0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1 };

-

-  real32_T b_r[81];

-  real32_T fv1[81];

-  real32_T f0;

-  real32_T c_P_apriori[36];

-  static const int8_T iv3[36] = { 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0,

-    0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0 };

-

-  real32_T fv2[36];

-  static const int8_T iv4[36] = { 1, 0, 0, 0, 1, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0,

-    0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0 };

-

-  real32_T c_r[9];

-  real32_T b_K_k[36];

-  real32_T d_P_apriori[72];

-  static const int8_T iv5[72] = { 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0,

-    0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0,

-    1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0,

-    1, 0, 0, 0 };

-

-  real32_T c_K_k[72];

-  static const int8_T iv6[72] = { 1, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 1, 0,

-    0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0,

-    0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0,

-    0, 0, 0, 0 };

-

-  real32_T b_z[6];

-  static const int8_T iv7[72] = { 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0,

-    0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0,

-    0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0,

-    0, 0, 0, 1 };

-

-  static const int8_T iv8[72] = { 1, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 1, 0,

-    0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0,

-    0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0,

-    0, 0, 0, 1 };

-

-  real32_T fv3[6];

-  real32_T c_z[6];

-

-  /*  Extended Attitude Kalmanfilter */

-  /*  */

-  /*  state vector x has the following entries [ax,ay,az||mx,my,mz||wox,woy,woz||wx,wy,wz]' */

-  /*  measurement vector z has the following entries [ax,ay,az||mx,my,mz||wmx,wmy,wmz]' */

-  /*  knownConst has the following entries [PrvaA,PrvarM,PrvarWO,PrvarW||MsvarA,MsvarM,MsvarW] */

-  /*  */

-  /*  [x_aposteriori,P_aposteriori] = AttKalman(dt,z_k,x_aposteriori_k,P_aposteriori_k,knownConst) */

-  /*  */

-  /*  Example.... */

-  /*  */

-  /*  $Author: Tobias Naegeli $    $Date: 2012 $    $Revision: 1 $ */

-  /* coder.varsize('udpIndVect', [9,1], [1,0]) */

-  /* udpIndVect=find(updVect); */

-  /* process and measurement noise covariance matrix */

-  /* Q = diag(q.^2*dt); */

-  /* observation matrix */

-  /* 'attitudeKalmanfilter:33' wx=  x_aposteriori_k(1); */

-  /* 'attitudeKalmanfilter:34' wy=  x_aposteriori_k(2); */

-  /* 'attitudeKalmanfilter:35' wz=  x_aposteriori_k(3); */

-  /* 'attitudeKalmanfilter:37' wax=  x_aposteriori_k(4); */

-  /* 'attitudeKalmanfilter:38' way=  x_aposteriori_k(5); */

-  /* 'attitudeKalmanfilter:39' waz=  x_aposteriori_k(6); */

-  /* 'attitudeKalmanfilter:41' zex=  x_aposteriori_k(7); */

-  /* 'attitudeKalmanfilter:42' zey=  x_aposteriori_k(8); */

-  /* 'attitudeKalmanfilter:43' zez=  x_aposteriori_k(9); */

-  /* 'attitudeKalmanfilter:45' mux=  x_aposteriori_k(10); */

-  /* 'attitudeKalmanfilter:46' muy=  x_aposteriori_k(11); */

-  /* 'attitudeKalmanfilter:47' muz=  x_aposteriori_k(12); */

-  /* % prediction section */

-  /* body angular accelerations */

-  /* 'attitudeKalmanfilter:51' wak =[wax;way;waz]; */

-  wak[0] = x_aposteriori_k[3];

-  wak[1] = x_aposteriori_k[4];

-  wak[2] = x_aposteriori_k[5];

-

-  /* body angular rates */

-  /* 'attitudeKalmanfilter:54' wk =[wx;  wy; wz] + dt*wak; */

-  /* derivative of the prediction rotation matrix */

-  /* 'attitudeKalmanfilter:57' O=[0,-wz,wy;wz,0,-wx;-wy,wx,0]'; */

-  O[0] = 0.0F;

-  O[1] = -x_aposteriori_k[2];

-  O[2] = x_aposteriori_k[1];

-  O[3] = x_aposteriori_k[2];

-  O[4] = 0.0F;

-  O[5] = -x_aposteriori_k[0];

-  O[6] = -x_aposteriori_k[1];

-  O[7] = x_aposteriori_k[0];

-  O[8] = 0.0F;

-

-  /* prediction of the earth z vector */

-  /* 'attitudeKalmanfilter:60' zek =(eye(3)+O*dt)*[zex;zey;zez]; */

-  eye(dv0);

-  for (i = 0; i < 9; i++) {

-    a[i] = (real32_T)dv0[i] + O[i] * dt;

-  }

-

-  /* prediction of the magnetic vector */

-  /* 'attitudeKalmanfilter:63' muk =(eye(3)+O*dt)*[mux;muy;muz]; */

-  eye(dv0);

-  for (i = 0; i < 9; i++) {

-    b_a[i] = (real32_T)dv0[i] + O[i] * dt;

-  }

-

-  /* 'attitudeKalmanfilter:65' EZ=[0,zez,-zey; */

-  /* 'attitudeKalmanfilter:66'     -zez,0,zex; */

-  /* 'attitudeKalmanfilter:67'     zey,-zex,0]'; */

-  /* 'attitudeKalmanfilter:68' MA=[0,muz,-muy; */

-  /* 'attitudeKalmanfilter:69'     -muz,0,mux; */

-  /* 'attitudeKalmanfilter:70'     zey,-mux,0]'; */

-  /* 'attitudeKalmanfilter:74' E=eye(3); */

-  /* 'attitudeKalmanfilter:76' Z=zeros(3); */

-  /* 'attitudeKalmanfilter:77' x_apriori=[wk;wak;zek;muk]; */

-  x_n_b[0] = x_aposteriori_k[0];

-  x_n_b[1] = x_aposteriori_k[1];

-  x_n_b[2] = x_aposteriori_k[2];

-  b_x_aposteriori_k[0] = x_aposteriori_k[6];

-  b_x_aposteriori_k[1] = x_aposteriori_k[7];

-  b_x_aposteriori_k[2] = x_aposteriori_k[8];

-  z_n_b[0] = x_aposteriori_k[9];

-  z_n_b[1] = x_aposteriori_k[10];

-  z_n_b[2] = x_aposteriori_k[11];

-  for (i = 0; i < 3; i++) {

-    c_a[i] = 0.0F;

-    for (i0 = 0; i0 < 3; i0++) {

-      c_a[i] += a[i + 3 * i0] * b_x_aposteriori_k[i0];

-    }

-

-    d_a[i] = 0.0F;

-    for (i0 = 0; i0 < 3; i0++) {

-      d_a[i] += b_a[i + 3 * i0] * z_n_b[i0];

-    }

-

-    x_apriori[i] = x_n_b[i] + dt * wak[i];

-  }

-

-  for (i = 0; i < 3; i++) {

-    x_apriori[i + 3] = wak[i];

-  }

-

-  for (i = 0; i < 3; i++) {

-    x_apriori[i + 6] = c_a[i];

-  }

-

-  for (i = 0; i < 3; i++) {

-    x_apriori[i + 9] = d_a[i];

-  }

-

-  /* 'attitudeKalmanfilter:81' A_lin=[ Z,  E,  Z,  Z */

-  /* 'attitudeKalmanfilter:82'     Z,  Z,  Z,  Z */

-  /* 'attitudeKalmanfilter:83'     EZ, Z,  O,  Z */

-  /* 'attitudeKalmanfilter:84'     MA, Z,  Z,  O]; */

-  /* 'attitudeKalmanfilter:86' A_lin=eye(12)+A_lin*dt; */

-  b_eye(dv1);

-  for (i = 0; i < 12; i++) {

-    for (i0 = 0; i0 < 3; i0++) {

-      A_lin[i0 + 12 * i] = (real32_T)iv0[i0 + 3 * i];

-    }

-

-    for (i0 = 0; i0 < 3; i0++) {

-      A_lin[(i0 + 12 * i) + 3] = 0.0F;

-    }

-  }

-

-  A_lin[6] = 0.0F;

-  A_lin[7] = x_aposteriori_k[8];

-  A_lin[8] = -x_aposteriori_k[7];

-  A_lin[18] = -x_aposteriori_k[8];

-  A_lin[19] = 0.0F;

-  A_lin[20] = x_aposteriori_k[6];

-  A_lin[30] = x_aposteriori_k[7];

-  A_lin[31] = -x_aposteriori_k[6];

-  A_lin[32] = 0.0F;

-  for (i = 0; i < 3; i++) {

-    for (i0 = 0; i0 < 3; i0++) {

-      A_lin[(i0 + 12 * (i + 3)) + 6] = 0.0F;

-    }

-  }

-

-  for (i = 0; i < 3; i++) {

-    for (i0 = 0; i0 < 3; i0++) {

-      A_lin[(i0 + 12 * (i + 6)) + 6] = O[i0 + 3 * i];

-    }

-  }

-

-  for (i = 0; i < 3; i++) {

-    for (i0 = 0; i0 < 3; i0++) {

-      A_lin[(i0 + 12 * (i + 9)) + 6] = 0.0F;

-    }

-  }

-

-  A_lin[9] = 0.0F;

-  A_lin[10] = x_aposteriori_k[11];

-  A_lin[11] = -x_aposteriori_k[10];

-  A_lin[21] = -x_aposteriori_k[11];

-  A_lin[22] = 0.0F;

-  A_lin[23] = x_aposteriori_k[9];

-  A_lin[33] = x_aposteriori_k[7];

-  A_lin[34] = -x_aposteriori_k[9];

-  A_lin[35] = 0.0F;

-  for (i = 0; i < 3; i++) {

-    for (i0 = 0; i0 < 3; i0++) {

-      A_lin[(i0 + 12 * (i + 3)) + 9] = 0.0F;

-    }

-  }

-

-  for (i = 0; i < 3; i++) {

-    for (i0 = 0; i0 < 3; i0++) {

-      A_lin[(i0 + 12 * (i + 6)) + 9] = 0.0F;

-    }

-  }

-

-  for (i = 0; i < 3; i++) {

-    for (i0 = 0; i0 < 3; i0++) {

-      A_lin[(i0 + 12 * (i + 9)) + 9] = O[i0 + 3 * i];

-    }

-  }

-

-  for (i = 0; i < 12; i++) {

-    for (i0 = 0; i0 < 12; i0++) {

-      b_A_lin[i0 + 12 * i] = (real32_T)dv1[i0 + 12 * i] + A_lin[i0 + 12 * i] *

-        dt;

-    }

-  }

-

-  /* 'attitudeKalmanfilter:88' Qtemp=[ q(1),     0,      0,      0,      0,      0,      0,      0,      0,      0,      0,      0; */

-  /* 'attitudeKalmanfilter:89'         0,     q(1),      0,      0,      0,      0,      0,      0,      0,      0,      0,      0; */

-  /* 'attitudeKalmanfilter:90'         0,     0,      q(1),      0,      0,      0,      0,      0,      0,      0,      0,      0; */

-  /* 'attitudeKalmanfilter:91'         0,     0,      0,      q(2),   0,      0,     0,      0,      0,      0,      0,      0; */

-  /* 'attitudeKalmanfilter:92'         0,     0,      0,      0,      q(2),   0,     0,      0,      0,      0,      0,      0; */

-  /* 'attitudeKalmanfilter:93'         0,     0,      0,      0,      0,      q(2),   0,      0,      0,      0,      0,      0; */

-  /* 'attitudeKalmanfilter:94'         0,     0,      0,      0,      0,      0,      q(3),   0,      0,      0,      0,      0; */

-  /* 'attitudeKalmanfilter:95'         0,     0,      0,      0,      0,      0,      0,      q(3),   0,      0,      0,      0; */

-  /* 'attitudeKalmanfilter:96'         0,     0,      0,      0,      0,      0,      0,      0,      q(3),   0,      0,      0; */

-  /* 'attitudeKalmanfilter:97'         0,     0,      0,      0,      0,      0,      0,      0,      0,      q(4),   0,      0; */

-  /* 'attitudeKalmanfilter:98'         0,     0,      0,      0,      0,      0,      0,      0,      0,      0,      q(4),   0; */

-  /* 'attitudeKalmanfilter:99'         0,     0,      0,      0,      0,      0,      0,      0,      0,      0,      0,      q(4)]; */

-  /* 'attitudeKalmanfilter:103' Q=A_lin*Qtemp*A_lin'; */

-  /* 'attitudeKalmanfilter:106' P_apriori=A_lin*P_aposteriori_k*A_lin'+Q; */

-  b_q[0] = q[0];

-  b_q[12] = 0.0F;

-  b_q[24] = 0.0F;

-  b_q[36] = 0.0F;

-  b_q[48] = 0.0F;

-  b_q[60] = 0.0F;

-  b_q[72] = 0.0F;

-  b_q[84] = 0.0F;

-  b_q[96] = 0.0F;

-  b_q[108] = 0.0F;

-  b_q[120] = 0.0F;

-  b_q[132] = 0.0F;

-  b_q[1] = 0.0F;

-  b_q[13] = q[0];

-  b_q[25] = 0.0F;

-  b_q[37] = 0.0F;

-  b_q[49] = 0.0F;

-  b_q[61] = 0.0F;

-  b_q[73] = 0.0F;

-  b_q[85] = 0.0F;

-  b_q[97] = 0.0F;

-  b_q[109] = 0.0F;

-  b_q[121] = 0.0F;

-  b_q[133] = 0.0F;

-  b_q[2] = 0.0F;

-  b_q[14] = 0.0F;

-  b_q[26] = q[0];

-  b_q[38] = 0.0F;

-  b_q[50] = 0.0F;

-  b_q[62] = 0.0F;

-  b_q[74] = 0.0F;

-  b_q[86] = 0.0F;

-  b_q[98] = 0.0F;

-  b_q[110] = 0.0F;

-  b_q[122] = 0.0F;

-  b_q[134] = 0.0F;

-  b_q[3] = 0.0F;

-  b_q[15] = 0.0F;

-  b_q[27] = 0.0F;

-  b_q[39] = q[1];

-  b_q[51] = 0.0F;

-  b_q[63] = 0.0F;

-  b_q[75] = 0.0F;

-  b_q[87] = 0.0F;

-  b_q[99] = 0.0F;

-  b_q[111] = 0.0F;

-  b_q[123] = 0.0F;

-  b_q[135] = 0.0F;

-  b_q[4] = 0.0F;

-  b_q[16] = 0.0F;

-  b_q[28] = 0.0F;

-  b_q[40] = 0.0F;

-  b_q[52] = q[1];

-  b_q[64] = 0.0F;

-  b_q[76] = 0.0F;

-  b_q[88] = 0.0F;

-  b_q[100] = 0.0F;

-  b_q[112] = 0.0F;

-  b_q[124] = 0.0F;

-  b_q[136] = 0.0F;

-  b_q[5] = 0.0F;

-  b_q[17] = 0.0F;

-  b_q[29] = 0.0F;

-  b_q[41] = 0.0F;

-  b_q[53] = 0.0F;

-  b_q[65] = q[1];

-  b_q[77] = 0.0F;

-  b_q[89] = 0.0F;

-  b_q[101] = 0.0F;

-  b_q[113] = 0.0F;

-  b_q[125] = 0.0F;

-  b_q[137] = 0.0F;

-  b_q[6] = 0.0F;

-  b_q[18] = 0.0F;

-  b_q[30] = 0.0F;

-  b_q[42] = 0.0F;

-  b_q[54] = 0.0F;

-  b_q[66] = 0.0F;

-  b_q[78] = q[2];

-  b_q[90] = 0.0F;

-  b_q[102] = 0.0F;

-  b_q[114] = 0.0F;

-  b_q[126] = 0.0F;

-  b_q[138] = 0.0F;

-  b_q[7] = 0.0F;

-  b_q[19] = 0.0F;

-  b_q[31] = 0.0F;

-  b_q[43] = 0.0F;

-  b_q[55] = 0.0F;

-  b_q[67] = 0.0F;

-  b_q[79] = 0.0F;

-  b_q[91] = q[2];

-  b_q[103] = 0.0F;

-  b_q[115] = 0.0F;

-  b_q[127] = 0.0F;

-  b_q[139] = 0.0F;

-  b_q[8] = 0.0F;

-  b_q[20] = 0.0F;

-  b_q[32] = 0.0F;

-  b_q[44] = 0.0F;

-  b_q[56] = 0.0F;

-  b_q[68] = 0.0F;

-  b_q[80] = 0.0F;

-  b_q[92] = 0.0F;

-  b_q[104] = q[2];

-  b_q[116] = 0.0F;

-  b_q[128] = 0.0F;

-  b_q[140] = 0.0F;

-  b_q[9] = 0.0F;

-  b_q[21] = 0.0F;

-  b_q[33] = 0.0F;

-  b_q[45] = 0.0F;

-  b_q[57] = 0.0F;

-  b_q[69] = 0.0F;

-  b_q[81] = 0.0F;

-  b_q[93] = 0.0F;

-  b_q[105] = 0.0F;

-  b_q[117] = q[3];

-  b_q[129] = 0.0F;

-  b_q[141] = 0.0F;

-  b_q[10] = 0.0F;

-  b_q[22] = 0.0F;

-  b_q[34] = 0.0F;

-  b_q[46] = 0.0F;

-  b_q[58] = 0.0F;

-  b_q[70] = 0.0F;

-  b_q[82] = 0.0F;

-  b_q[94] = 0.0F;

-  b_q[106] = 0.0F;

-  b_q[118] = 0.0F;

-  b_q[130] = q[3];

-  b_q[142] = 0.0F;

-  b_q[11] = 0.0F;

-  b_q[23] = 0.0F;

-  b_q[35] = 0.0F;

-  b_q[47] = 0.0F;

-  b_q[59] = 0.0F;

-  b_q[71] = 0.0F;

-  b_q[83] = 0.0F;

-  b_q[95] = 0.0F;

-  b_q[107] = 0.0F;

-  b_q[119] = 0.0F;

-  b_q[131] = 0.0F;

-  b_q[143] = q[3];

-  for (i = 0; i < 12; i++) {

-    for (i0 = 0; i0 < 12; i0++) {

-      A_lin[i + 12 * i0] = 0.0F;

-      for (i1 = 0; i1 < 12; i1++) {

-        A_lin[i + 12 * i0] += b_A_lin[i + 12 * i1] * P_aposteriori_k[i1 + 12 *

-          i0];

-      }

-

-      c_A_lin[i + 12 * i0] = 0.0F;

-      for (i1 = 0; i1 < 12; i1++) {

-        c_A_lin[i + 12 * i0] += b_A_lin[i + 12 * i1] * b_q[i1 + 12 * i0];

-      }

-    }

-

-    for (i0 = 0; i0 < 12; i0++) {

-      d_A_lin[i + 12 * i0] = 0.0F;

-      for (i1 = 0; i1 < 12; i1++) {

-        d_A_lin[i + 12 * i0] += A_lin[i + 12 * i1] * b_A_lin[i0 + 12 * i1];

-      }

-

-      e_A_lin[i + 12 * i0] = 0.0F;

-      for (i1 = 0; i1 < 12; i1++) {

-        e_A_lin[i + 12 * i0] += c_A_lin[i + 12 * i1] * b_A_lin[i0 + 12 * i1];

-      }

-    }

-  }

-

-  for (i = 0; i < 12; i++) {

-    for (i0 = 0; i0 < 12; i0++) {

-      P_apriori[i0 + 12 * i] = d_A_lin[i0 + 12 * i] + e_A_lin[i0 + 12 * i];

-    }

-  }

-

-  /* % update */

-  /* 'attitudeKalmanfilter:110' if updateVect(1)==1&&updateVect(2)==1&&updateVect(3)==1 */

-  if ((updateVect[0] == 1) && (updateVect[1] == 1) && (updateVect[2] == 1)) {

-    /* 'attitudeKalmanfilter:111' if z(6)<4 || z(5)>15 */

-    if ((z[5] < 4.0F) || (z[4] > 15.0F)) {

-      /* 'attitudeKalmanfilter:112' r(2)=10000; */

-      r[1] = 10000.0F;

-    }

-

-    /* 'attitudeKalmanfilter:114' R=[r(1),0,0,0,0,0,0,0,0; */

-    /* 'attitudeKalmanfilter:115'         0,r(1),0,0,0,0,0,0,0; */

-    /* 'attitudeKalmanfilter:116'         0,0,r(1),0,0,0,0,0,0; */

-    /* 'attitudeKalmanfilter:117'         0,0,0,r(2),0,0,0,0,0; */

-    /* 'attitudeKalmanfilter:118'         0,0,0,0,r(2),0,0,0,0; */

-    /* 'attitudeKalmanfilter:119'         0,0,0,0,0,r(2),0,0,0; */

-    /* 'attitudeKalmanfilter:120'         0,0,0,0,0,0,r(3),0,0; */

-    /* 'attitudeKalmanfilter:121'         0,0,0,0,0,0,0,r(3),0; */

-    /* 'attitudeKalmanfilter:122'         0,0,0,0,0,0,0,0,r(3)]; */

-    /* observation matrix */

-    /* [zw;ze;zmk]; */

-    /* 'attitudeKalmanfilter:125' H_k=[  E,     Z,      Z,    Z; */

-    /* 'attitudeKalmanfilter:126'         Z,     Z,      E,    Z; */

-    /* 'attitudeKalmanfilter:127'         Z,     Z,      Z,    E]; */

-    /* 'attitudeKalmanfilter:129' y_k=z(1:9)-H_k*x_apriori; */

-    /* 'attitudeKalmanfilter:132' S_k=H_k*P_apriori*H_k'+R; */

-    /* 'attitudeKalmanfilter:133' K_k=(P_apriori*H_k'/(S_k)); */

-    for (i = 0; i < 12; i++) {

-      for (i0 = 0; i0 < 9; i0++) {

-        b_P_apriori[i + 12 * i0] = 0.0F;

-        for (i1 = 0; i1 < 12; i1++) {

-          b_P_apriori[i + 12 * i0] += P_apriori[i + 12 * i1] * (real32_T)iv1[i1

-            + 12 * i0];

-        }

-      }

-    }

-

-    for (i = 0; i < 9; i++) {

-      for (i0 = 0; i0 < 12; i0++) {

-        K_k[i + 9 * i0] = 0.0F;

-        for (i1 = 0; i1 < 12; i1++) {

-          K_k[i + 9 * i0] += (real32_T)iv2[i + 9 * i1] * P_apriori[i1 + 12 * i0];

-        }

-      }

-

-      for (i0 = 0; i0 < 9; i0++) {

-        fv0[i + 9 * i0] = 0.0F;

-        for (i1 = 0; i1 < 12; i1++) {

-          fv0[i + 9 * i0] += K_k[i + 9 * i1] * (real32_T)iv1[i1 + 12 * i0];

-        }

-      }

-    }

-

-    b_r[0] = r[0];

-    b_r[9] = 0.0F;

-    b_r[18] = 0.0F;

-    b_r[27] = 0.0F;

-    b_r[36] = 0.0F;

-    b_r[45] = 0.0F;

-    b_r[54] = 0.0F;

-    b_r[63] = 0.0F;

-    b_r[72] = 0.0F;

-    b_r[1] = 0.0F;

-    b_r[10] = r[0];

-    b_r[19] = 0.0F;

-    b_r[28] = 0.0F;

-    b_r[37] = 0.0F;

-    b_r[46] = 0.0F;

-    b_r[55] = 0.0F;

-    b_r[64] = 0.0F;

-    b_r[73] = 0.0F;

-    b_r[2] = 0.0F;

-    b_r[11] = 0.0F;

-    b_r[20] = r[0];

-    b_r[29] = 0.0F;

-    b_r[38] = 0.0F;

-    b_r[47] = 0.0F;

-    b_r[56] = 0.0F;

-    b_r[65] = 0.0F;

-    b_r[74] = 0.0F;

-    b_r[3] = 0.0F;

-    b_r[12] = 0.0F;

-    b_r[21] = 0.0F;

-    b_r[30] = r[1];

-    b_r[39] = 0.0F;

-    b_r[48] = 0.0F;

-    b_r[57] = 0.0F;

-    b_r[66] = 0.0F;

-    b_r[75] = 0.0F;

-    b_r[4] = 0.0F;

-    b_r[13] = 0.0F;

-    b_r[22] = 0.0F;

-    b_r[31] = 0.0F;

-    b_r[40] = r[1];

-    b_r[49] = 0.0F;

-    b_r[58] = 0.0F;

-    b_r[67] = 0.0F;

-    b_r[76] = 0.0F;

-    b_r[5] = 0.0F;

-    b_r[14] = 0.0F;

-    b_r[23] = 0.0F;

-    b_r[32] = 0.0F;

-    b_r[41] = 0.0F;

-    b_r[50] = r[1];

-    b_r[59] = 0.0F;

-    b_r[68] = 0.0F;

-    b_r[77] = 0.0F;

-    b_r[6] = 0.0F;

-    b_r[15] = 0.0F;

-    b_r[24] = 0.0F;

-    b_r[33] = 0.0F;

-    b_r[42] = 0.0F;

-    b_r[51] = 0.0F;

-    b_r[60] = r[2];

-    b_r[69] = 0.0F;

-    b_r[78] = 0.0F;

-    b_r[7] = 0.0F;

-    b_r[16] = 0.0F;

-    b_r[25] = 0.0F;

-    b_r[34] = 0.0F;

-    b_r[43] = 0.0F;

-    b_r[52] = 0.0F;

-    b_r[61] = 0.0F;

-    b_r[70] = r[2];

-    b_r[79] = 0.0F;

-    b_r[8] = 0.0F;

-    b_r[17] = 0.0F;

-    b_r[26] = 0.0F;

-    b_r[35] = 0.0F;

-    b_r[44] = 0.0F;

-    b_r[53] = 0.0F;

-    b_r[62] = 0.0F;

-    b_r[71] = 0.0F;

-    b_r[80] = r[2];

-    for (i = 0; i < 9; i++) {

-      for (i0 = 0; i0 < 9; i0++) {

-        fv1[i0 + 9 * i] = fv0[i0 + 9 * i] + b_r[i0 + 9 * i];

-      }

-    }

-

-    mrdivide(b_P_apriori, fv1, K_k);

-

-    /* 'attitudeKalmanfilter:136' x_aposteriori=x_apriori+K_k*y_k; */

-    for (i = 0; i < 9; i++) {

-      f0 = 0.0F;

-      for (i0 = 0; i0 < 12; i0++) {

-        f0 += (real32_T)iv2[i + 9 * i0] * x_apriori[i0];

-      }

-

-      O[i] = z[i] - f0;

-    }

-

-    for (i = 0; i < 12; i++) {

-      f0 = 0.0F;

-      for (i0 = 0; i0 < 9; i0++) {

-        f0 += K_k[i + 12 * i0] * O[i0];

-      }

-

-      x_aposteriori[i] = x_apriori[i] + f0;

-    }

-

-    /* 'attitudeKalmanfilter:137' P_aposteriori=(eye(12)-K_k*H_k)*P_apriori; */

-    b_eye(dv1);

-    for (i = 0; i < 12; i++) {

-      for (i0 = 0; i0 < 12; i0++) {

-        f0 = 0.0F;

-        for (i1 = 0; i1 < 9; i1++) {

-          f0 += K_k[i + 12 * i1] * (real32_T)iv2[i1 + 9 * i0];

-        }

-

-        b_A_lin[i + 12 * i0] = (real32_T)dv1[i + 12 * i0] - f0;

-      }

-    }

-

-    for (i = 0; i < 12; i++) {

-      for (i0 = 0; i0 < 12; i0++) {

-        P_aposteriori[i + 12 * i0] = 0.0F;

-        for (i1 = 0; i1 < 12; i1++) {

-          P_aposteriori[i + 12 * i0] += b_A_lin[i + 12 * i1] * P_apriori[i1 + 12

-            * i0];

-        }

-      }

-    }

-  } else {

-    /* 'attitudeKalmanfilter:138' else */

-    /* 'attitudeKalmanfilter:139' if updateVect(1)==1&&updateVect(2)==0&&updateVect(3)==0 */

-    if ((updateVect[0] == 1) && (updateVect[1] == 0) && (updateVect[2] == 0)) {

-      /* 'attitudeKalmanfilter:141' R=[r(1),0,0; */

-      /* 'attitudeKalmanfilter:142'             0,r(1),0; */

-      /* 'attitudeKalmanfilter:143'             0,0,r(1)]; */

-      /* observation matrix */

-      /* 'attitudeKalmanfilter:146' H_k=[  E,     Z,      Z,    Z]; */

-      /* 'attitudeKalmanfilter:148' y_k=z(1:3)-H_k(1:3,1:12)*x_apriori; */

-      /* 'attitudeKalmanfilter:150' S_k=H_k(1:3,1:12)*P_apriori*H_k(1:3,1:12)'+R(1:3,1:3); */

-      /* 'attitudeKalmanfilter:151' K_k=(P_apriori*H_k(1:3,1:12)'/(S_k)); */

-      for (i = 0; i < 12; i++) {

-        for (i0 = 0; i0 < 3; i0++) {

-          c_P_apriori[i + 12 * i0] = 0.0F;

-          for (i1 = 0; i1 < 12; i1++) {

-            c_P_apriori[i + 12 * i0] += P_apriori[i + 12 * i1] * (real32_T)

-              iv3[i1 + 12 * i0];

-          }

-        }

-      }

-

-      for (i = 0; i < 3; i++) {

-        for (i0 = 0; i0 < 12; i0++) {

-          fv2[i + 3 * i0] = 0.0F;

-          for (i1 = 0; i1 < 12; i1++) {

-            fv2[i + 3 * i0] += (real32_T)iv4[i + 3 * i1] * P_apriori[i1 + 12 *

-              i0];

-          }

-        }

-

-        for (i0 = 0; i0 < 3; i0++) {

-          O[i + 3 * i0] = 0.0F;

-          for (i1 = 0; i1 < 12; i1++) {

-            O[i + 3 * i0] += fv2[i + 3 * i1] * (real32_T)iv3[i1 + 12 * i0];

-          }

-        }

-      }

-

-      c_r[0] = r[0];

-      c_r[3] = 0.0F;

-      c_r[6] = 0.0F;

-      c_r[1] = 0.0F;

-      c_r[4] = r[0];

-      c_r[7] = 0.0F;

-      c_r[2] = 0.0F;

-      c_r[5] = 0.0F;

-      c_r[8] = r[0];

-      for (i = 0; i < 3; i++) {

-        for (i0 = 0; i0 < 3; i0++) {

-          a[i0 + 3 * i] = O[i0 + 3 * i] + c_r[i0 + 3 * i];

-        }

-      }

-

-      b_mrdivide(c_P_apriori, a, b_K_k);

-

-      /* 'attitudeKalmanfilter:154' x_aposteriori=x_apriori+K_k*y_k; */

-      for (i = 0; i < 3; i++) {

-        f0 = 0.0F;

-        for (i0 = 0; i0 < 12; i0++) {

-          f0 += (real32_T)iv4[i + 3 * i0] * x_apriori[i0];

-        }

-

-        x_n_b[i] = z[i] - f0;

-      }

-

-      for (i = 0; i < 12; i++) {

-        f0 = 0.0F;

-        for (i0 = 0; i0 < 3; i0++) {

-          f0 += b_K_k[i + 12 * i0] * x_n_b[i0];

-        }

-

-        x_aposteriori[i] = x_apriori[i] + f0;

-      }

-

-      /* 'attitudeKalmanfilter:155' P_aposteriori=(eye(12)-K_k*H_k(1:3,1:12))*P_apriori; */

-      b_eye(dv1);

-      for (i = 0; i < 12; i++) {

-        for (i0 = 0; i0 < 12; i0++) {

-          f0 = 0.0F;

-          for (i1 = 0; i1 < 3; i1++) {

-            f0 += b_K_k[i + 12 * i1] * (real32_T)iv4[i1 + 3 * i0];

-          }

-

-          b_A_lin[i + 12 * i0] = (real32_T)dv1[i + 12 * i0] - f0;

-        }

-      }

-

-      for (i = 0; i < 12; i++) {

-        for (i0 = 0; i0 < 12; i0++) {

-          P_aposteriori[i + 12 * i0] = 0.0F;

-          for (i1 = 0; i1 < 12; i1++) {

-            P_aposteriori[i + 12 * i0] += b_A_lin[i + 12 * i1] * P_apriori[i1 +

-              12 * i0];

-          }

-        }

-      }

-    } else {

-      /* 'attitudeKalmanfilter:156' else */

-      /* 'attitudeKalmanfilter:157' if  updateVect(1)==1&&updateVect(2)==1&&updateVect(3)==0 */

-      if ((updateVect[0] == 1) && (updateVect[1] == 1) && (updateVect[2] == 0))

-      {

-        /* 'attitudeKalmanfilter:158' if z(6)<4 || z(5)>15 */

-        if ((z[5] < 4.0F) || (z[4] > 15.0F)) {

-          /* 'attitudeKalmanfilter:159' r(2)=10000; */

-          r[1] = 10000.0F;

-        }

-

-        /* 'attitudeKalmanfilter:162' R=[r(1),0,0,0,0,0; */

-        /* 'attitudeKalmanfilter:163'                 0,r(1),0,0,0,0; */

-        /* 'attitudeKalmanfilter:164'                 0,0,r(1),0,0,0; */

-        /* 'attitudeKalmanfilter:165'                 0,0,0,r(2),0,0; */

-        /* 'attitudeKalmanfilter:166'                 0,0,0,0,r(2),0; */

-        /* 'attitudeKalmanfilter:167'                 0,0,0,0,0,r(2)]; */

-        /* observation matrix */

-        /* 'attitudeKalmanfilter:170' H_k=[  E,     Z,      Z,    Z; */

-        /* 'attitudeKalmanfilter:171'                 Z,     Z,      E,    Z]; */

-        /* 'attitudeKalmanfilter:173' y_k=z(1:6)-H_k(1:6,1:12)*x_apriori; */

-        /* 'attitudeKalmanfilter:175' S_k=H_k(1:6,1:12)*P_apriori*H_k(1:6,1:12)'+R(1:6,1:6); */

-        /* 'attitudeKalmanfilter:176' K_k=(P_apriori*H_k(1:6,1:12)'/(S_k)); */

-        for (i = 0; i < 12; i++) {

-          for (i0 = 0; i0 < 6; i0++) {

-            d_P_apriori[i + 12 * i0] = 0.0F;

-            for (i1 = 0; i1 < 12; i1++) {

-              d_P_apriori[i + 12 * i0] += P_apriori[i + 12 * i1] * (real32_T)

-                iv5[i1 + 12 * i0];

-            }

-          }

-        }

-

-        for (i = 0; i < 6; i++) {

-          for (i0 = 0; i0 < 12; i0++) {

-            c_K_k[i + 6 * i0] = 0.0F;

-            for (i1 = 0; i1 < 12; i1++) {

-              c_K_k[i + 6 * i0] += (real32_T)iv6[i + 6 * i1] * P_apriori[i1 + 12

-                * i0];

-            }

-          }

-

-          for (i0 = 0; i0 < 6; i0++) {

-            fv2[i + 6 * i0] = 0.0F;

-            for (i1 = 0; i1 < 12; i1++) {

-              fv2[i + 6 * i0] += c_K_k[i + 6 * i1] * (real32_T)iv5[i1 + 12 * i0];

-            }

-          }

-        }

-

-        b_K_k[0] = r[0];

-        b_K_k[6] = 0.0F;

-        b_K_k[12] = 0.0F;

-        b_K_k[18] = 0.0F;

-        b_K_k[24] = 0.0F;

-        b_K_k[30] = 0.0F;

-        b_K_k[1] = 0.0F;

-        b_K_k[7] = r[0];

-        b_K_k[13] = 0.0F;

-        b_K_k[19] = 0.0F;

-        b_K_k[25] = 0.0F;

-        b_K_k[31] = 0.0F;

-        b_K_k[2] = 0.0F;

-        b_K_k[8] = 0.0F;

-        b_K_k[14] = r[0];

-        b_K_k[20] = 0.0F;

-        b_K_k[26] = 0.0F;

-        b_K_k[32] = 0.0F;

-        b_K_k[3] = 0.0F;

-        b_K_k[9] = 0.0F;

-        b_K_k[15] = 0.0F;

-        b_K_k[21] = r[1];

-        b_K_k[27] = 0.0F;

-        b_K_k[33] = 0.0F;

-        b_K_k[4] = 0.0F;

-        b_K_k[10] = 0.0F;

-        b_K_k[16] = 0.0F;

-        b_K_k[22] = 0.0F;

-        b_K_k[28] = r[1];

-        b_K_k[34] = 0.0F;

-        b_K_k[5] = 0.0F;

-        b_K_k[11] = 0.0F;

-        b_K_k[17] = 0.0F;

-        b_K_k[23] = 0.0F;

-        b_K_k[29] = 0.0F;

-        b_K_k[35] = r[1];

-        for (i = 0; i < 6; i++) {

-          for (i0 = 0; i0 < 6; i0++) {

-            c_P_apriori[i0 + 6 * i] = fv2[i0 + 6 * i] + b_K_k[i0 + 6 * i];

-          }

-        }

-

-        c_mrdivide(d_P_apriori, c_P_apriori, c_K_k);

-

-        /* 'attitudeKalmanfilter:179' x_aposteriori=x_apriori+K_k*y_k; */

-        for (i = 0; i < 6; i++) {

-          f0 = 0.0F;

-          for (i0 = 0; i0 < 12; i0++) {

-            f0 += (real32_T)iv6[i + 6 * i0] * x_apriori[i0];

-          }

-

-          b_z[i] = z[i] - f0;

-        }

-

-        for (i = 0; i < 12; i++) {

-          f0 = 0.0F;

-          for (i0 = 0; i0 < 6; i0++) {

-            f0 += c_K_k[i + 12 * i0] * b_z[i0];

-          }

-

-          x_aposteriori[i] = x_apriori[i] + f0;

-        }

-

-        /* 'attitudeKalmanfilter:180' P_aposteriori=(eye(12)-K_k*H_k(1:6,1:12))*P_apriori; */

-        b_eye(dv1);

-        for (i = 0; i < 12; i++) {

-          for (i0 = 0; i0 < 12; i0++) {

-            f0 = 0.0F;

-            for (i1 = 0; i1 < 6; i1++) {

-              f0 += c_K_k[i + 12 * i1] * (real32_T)iv6[i1 + 6 * i0];

-            }

-

-            b_A_lin[i + 12 * i0] = (real32_T)dv1[i + 12 * i0] - f0;

-          }

-        }

-

-        for (i = 0; i < 12; i++) {

-          for (i0 = 0; i0 < 12; i0++) {

-            P_aposteriori[i + 12 * i0] = 0.0F;

-            for (i1 = 0; i1 < 12; i1++) {

-              P_aposteriori[i + 12 * i0] += b_A_lin[i + 12 * i1] * P_apriori[i1

-                + 12 * i0];

-            }

-          }

-        }

-      } else {

-        /* 'attitudeKalmanfilter:181' else */

-        /* 'attitudeKalmanfilter:182' if  updateVect(1)==1&&updateVect(2)==0&&updateVect(3)==1 */

-        if ((updateVect[0] == 1) && (updateVect[1] == 0) && (updateVect[2] == 1))

-        {

-          /* 'attitudeKalmanfilter:183' R=[r(1),0,0,0,0,0; */

-          /* 'attitudeKalmanfilter:184'                     0,r(1),0,0,0,0; */

-          /* 'attitudeKalmanfilter:185'                     0,0,r(1),0,0,0; */

-          /* 'attitudeKalmanfilter:186'                     0,0,0,r(3),0,0; */

-          /* 'attitudeKalmanfilter:187'                     0,0,0,0,r(3),0; */

-          /* 'attitudeKalmanfilter:188'                     0,0,0,0,0,r(3)]; */

-          /* observation matrix */

-          /* 'attitudeKalmanfilter:191' H_k=[  E,     Z,      Z,    Z; */

-          /* 'attitudeKalmanfilter:192'                     Z,     Z,      Z,    E]; */

-          /* 'attitudeKalmanfilter:194' y_k=[z(1:3);z(7:9)]-H_k(1:6,1:12)*x_apriori; */

-          /* 'attitudeKalmanfilter:196' S_k=H_k(1:6,1:12)*P_apriori*H_k(1:6,1:12)'+R(1:6,1:6); */

-          /* 'attitudeKalmanfilter:197' K_k=(P_apriori*H_k(1:6,1:12)'/(S_k)); */

-          for (i = 0; i < 12; i++) {

-            for (i0 = 0; i0 < 6; i0++) {

-              d_P_apriori[i + 12 * i0] = 0.0F;

-              for (i1 = 0; i1 < 12; i1++) {

-                d_P_apriori[i + 12 * i0] += P_apriori[i + 12 * i1] * (real32_T)

-                  iv7[i1 + 12 * i0];

-              }

-            }

-          }

-

-          for (i = 0; i < 6; i++) {

-            for (i0 = 0; i0 < 12; i0++) {

-              c_K_k[i + 6 * i0] = 0.0F;

-              for (i1 = 0; i1 < 12; i1++) {

-                c_K_k[i + 6 * i0] += (real32_T)iv8[i + 6 * i1] * P_apriori[i1 +

-                  12 * i0];

-              }

-            }

-

-            for (i0 = 0; i0 < 6; i0++) {

-              fv2[i + 6 * i0] = 0.0F;

-              for (i1 = 0; i1 < 12; i1++) {

-                fv2[i + 6 * i0] += c_K_k[i + 6 * i1] * (real32_T)iv7[i1 + 12 *

-                  i0];

-              }

-            }

-          }

-

-          b_K_k[0] = r[0];

-          b_K_k[6] = 0.0F;

-          b_K_k[12] = 0.0F;

-          b_K_k[18] = 0.0F;

-          b_K_k[24] = 0.0F;

-          b_K_k[30] = 0.0F;

-          b_K_k[1] = 0.0F;

-          b_K_k[7] = r[0];

-          b_K_k[13] = 0.0F;

-          b_K_k[19] = 0.0F;

-          b_K_k[25] = 0.0F;

-          b_K_k[31] = 0.0F;

-          b_K_k[2] = 0.0F;

-          b_K_k[8] = 0.0F;

-          b_K_k[14] = r[0];

-          b_K_k[20] = 0.0F;

-          b_K_k[26] = 0.0F;

-          b_K_k[32] = 0.0F;

-          b_K_k[3] = 0.0F;

-          b_K_k[9] = 0.0F;

-          b_K_k[15] = 0.0F;

-          b_K_k[21] = r[2];

-          b_K_k[27] = 0.0F;

-          b_K_k[33] = 0.0F;

-          b_K_k[4] = 0.0F;

-          b_K_k[10] = 0.0F;

-          b_K_k[16] = 0.0F;

-          b_K_k[22] = 0.0F;

-          b_K_k[28] = r[2];

-          b_K_k[34] = 0.0F;

-          b_K_k[5] = 0.0F;

-          b_K_k[11] = 0.0F;

-          b_K_k[17] = 0.0F;

-          b_K_k[23] = 0.0F;

-          b_K_k[29] = 0.0F;

-          b_K_k[35] = r[2];

-          for (i = 0; i < 6; i++) {

-            for (i0 = 0; i0 < 6; i0++) {

-              c_P_apriori[i0 + 6 * i] = fv2[i0 + 6 * i] + b_K_k[i0 + 6 * i];

-            }

-          }

-

-          c_mrdivide(d_P_apriori, c_P_apriori, c_K_k);

-

-          /* 'attitudeKalmanfilter:200' x_aposteriori=x_apriori+K_k*y_k; */

-          for (i = 0; i < 3; i++) {

-            b_z[i] = z[i];

-          }

-

-          for (i = 0; i < 3; i++) {

-            b_z[i + 3] = z[i + 6];

-          }

-

-          for (i = 0; i < 6; i++) {

-            fv3[i] = 0.0F;

-            for (i0 = 0; i0 < 12; i0++) {

-              fv3[i] += (real32_T)iv8[i + 6 * i0] * x_apriori[i0];

-            }

-

-            c_z[i] = b_z[i] - fv3[i];

-          }

-

-          for (i = 0; i < 12; i++) {

-            f0 = 0.0F;

-            for (i0 = 0; i0 < 6; i0++) {

-              f0 += c_K_k[i + 12 * i0] * c_z[i0];

-            }

-

-            x_aposteriori[i] = x_apriori[i] + f0;

-          }

-

-          /* 'attitudeKalmanfilter:201' P_aposteriori=(eye(12)-K_k*H_k(1:6,1:12))*P_apriori; */

-          b_eye(dv1);

-          for (i = 0; i < 12; i++) {

-            for (i0 = 0; i0 < 12; i0++) {

-              f0 = 0.0F;

-              for (i1 = 0; i1 < 6; i1++) {

-                f0 += c_K_k[i + 12 * i1] * (real32_T)iv8[i1 + 6 * i0];

-              }

-

-              b_A_lin[i + 12 * i0] = (real32_T)dv1[i + 12 * i0] - f0;

-            }

-          }

-

-          for (i = 0; i < 12; i++) {

-            for (i0 = 0; i0 < 12; i0++) {

-              P_aposteriori[i + 12 * i0] = 0.0F;

-              for (i1 = 0; i1 < 12; i1++) {

-                P_aposteriori[i + 12 * i0] += b_A_lin[i + 12 * i1] *

-                  P_apriori[i1 + 12 * i0];

-              }

-            }

-          }

-        } else {

-          /* 'attitudeKalmanfilter:202' else */

-          /* 'attitudeKalmanfilter:203' x_aposteriori=x_apriori; */

-          for (i = 0; i < 12; i++) {

-            x_aposteriori[i] = x_apriori[i];

-          }

-

-          /* 'attitudeKalmanfilter:204' P_aposteriori=P_apriori; */

-          memcpy(&P_aposteriori[0], &P_apriori[0], 144U * sizeof(real32_T));

-        }

-      }

-    }

-  }

-

-  /* % euler anglels extraction */

-  /* 'attitudeKalmanfilter:213' z_n_b = -x_aposteriori(7:9)./norm(x_aposteriori(7:9)); */

-  for (i = 0; i < 3; i++) {

-    x_n_b[i] = -x_aposteriori[i + 6];

-  }

-

-  rdivide(x_n_b, norm(*(real32_T (*)[3])&x_aposteriori[6]), z_n_b);

-

-  /* 'attitudeKalmanfilter:214' m_n_b = x_aposteriori(10:12)./norm(x_aposteriori(10:12)); */

-  rdivide(*(real32_T (*)[3])&x_aposteriori[9], norm(*(real32_T (*)[3])&

-           x_aposteriori[9]), wak);

-

-  /* 'attitudeKalmanfilter:216' y_n_b=cross(z_n_b,m_n_b); */

-  for (i = 0; i < 3; i++) {

-    x_n_b[i] = wak[i];

-  }

-

-  cross(z_n_b, x_n_b, wak);

-

-  /* 'attitudeKalmanfilter:217' y_n_b=y_n_b./norm(y_n_b); */

-  for (i = 0; i < 3; i++) {

-    x_n_b[i] = wak[i];

-  }

-

-  rdivide(x_n_b, norm(wak), wak);

-

-  /* 'attitudeKalmanfilter:219' x_n_b=(cross(y_n_b,z_n_b)); */

-  cross(wak, z_n_b, x_n_b);

-

-  /* 'attitudeKalmanfilter:220' x_n_b=x_n_b./norm(x_n_b); */

-  for (i = 0; i < 3; i++) {

-    b_x_aposteriori_k[i] = x_n_b[i];

-  }

-

-  rdivide(b_x_aposteriori_k, norm(x_n_b), x_n_b);

-

-  /* 'attitudeKalmanfilter:226' Rot_matrix=[x_n_b,y_n_b,z_n_b]; */

-  for (i = 0; i < 3; i++) {

-    Rot_matrix[i] = x_n_b[i];

-    Rot_matrix[3 + i] = wak[i];

-    Rot_matrix[6 + i] = z_n_b[i];

-  }

-

-  /* 'attitudeKalmanfilter:230' phi=atan2(Rot_matrix(2,3),Rot_matrix(3,3)); */

-  /* 'attitudeKalmanfilter:231' theta=-asin(Rot_matrix(1,3)); */

-  /* 'attitudeKalmanfilter:232' psi=atan2(Rot_matrix(1,2),Rot_matrix(1,1)); */

-  /* 'attitudeKalmanfilter:233' eulerAngles=[phi;theta;psi]; */

-  eulerAngles[0] = rt_atan2f_snf(Rot_matrix[7], Rot_matrix[8]);

-  eulerAngles[1] = -(real32_T)asin(Rot_matrix[6]);

-  eulerAngles[2] = rt_atan2f_snf(Rot_matrix[3], Rot_matrix[0]);

-}

-

-/* End of code generation (attitudeKalmanfilter.c) */

diff --git a/src/modules/attitude_estimator_ekf/codegen/attitudeKalmanfilter.h b/src/modules/attitude_estimator_ekf/codegen/attitudeKalmanfilter.h
deleted file mode 100755
index afa63c1a9..000000000
--- a/src/modules/attitude_estimator_ekf/codegen/attitudeKalmanfilter.h
+++ /dev/null
@@ -1,34 +0,0 @@
-/*

- * attitudeKalmanfilter.h

- *

- * Code generation for function 'attitudeKalmanfilter'

- *

- * C source code generated on: Sat Jan 19 15:25:29 2013

- *

- */

-

-#ifndef __ATTITUDEKALMANFILTER_H__

-#define __ATTITUDEKALMANFILTER_H__

-/* Include files */

-#include <math.h>

-#include <stddef.h>

-#include <stdlib.h>

-#include <string.h>

-#include "rt_defines.h"

-#include "rt_nonfinite.h"

-

-#include "rtwtypes.h"

-#include "attitudeKalmanfilter_types.h"

-

-/* Type Definitions */

-

-/* Named Constants */

-

-/* Variable Declarations */

-

-/* Variable Definitions */

-

-/* Function Declarations */

-extern void attitudeKalmanfilter(const uint8_T updateVect[3], real32_T dt, const real32_T z[9], const real32_T x_aposteriori_k[12], const real32_T P_aposteriori_k[144], const real32_T q[12], real32_T r[9], real32_T eulerAngles[3], real32_T Rot_matrix[9], real32_T x_aposteriori[12], real32_T P_aposteriori[144]);

-#endif

-/* End of code generation (attitudeKalmanfilter.h) */

diff --git a/src/modules/attitude_estimator_ekf/codegen/attitudeKalmanfilter_initialize.c b/src/modules/attitude_estimator_ekf/codegen/attitudeKalmanfilter_initialize.c
deleted file mode 100755
index 7d620d7fa..000000000
--- a/src/modules/attitude_estimator_ekf/codegen/attitudeKalmanfilter_initialize.c
+++ /dev/null
@@ -1,31 +0,0 @@
-/*

- * attitudeKalmanfilter_initialize.c

- *

- * Code generation for function 'attitudeKalmanfilter_initialize'

- *

- * C source code generated on: Sat Jan 19 15:25:29 2013

- *

- */

-

-/* Include files */

-#include "rt_nonfinite.h"

-#include "attitudeKalmanfilter.h"

-#include "attitudeKalmanfilter_initialize.h"

-

-/* Type Definitions */

-

-/* Named Constants */

-

-/* Variable Declarations */

-

-/* Variable Definitions */

-

-/* Function Declarations */

-

-/* Function Definitions */

-void attitudeKalmanfilter_initialize(void)

-{

-  rt_InitInfAndNaN(8U);

-}

-

-/* End of code generation (attitudeKalmanfilter_initialize.c) */

diff --git a/src/modules/attitude_estimator_ekf/codegen/attitudeKalmanfilter_initialize.h b/src/modules/attitude_estimator_ekf/codegen/attitudeKalmanfilter_initialize.h
deleted file mode 100755
index 8b599be66..000000000
--- a/src/modules/attitude_estimator_ekf/codegen/attitudeKalmanfilter_initialize.h
+++ /dev/null
@@ -1,34 +0,0 @@
-/*

- * attitudeKalmanfilter_initialize.h

- *

- * Code generation for function 'attitudeKalmanfilter_initialize'

- *

- * C source code generated on: Sat Jan 19 15:25:29 2013

- *

- */

-

-#ifndef __ATTITUDEKALMANFILTER_INITIALIZE_H__

-#define __ATTITUDEKALMANFILTER_INITIALIZE_H__

-/* Include files */

-#include <math.h>

-#include <stddef.h>

-#include <stdlib.h>

-#include <string.h>

-#include "rt_defines.h"

-#include "rt_nonfinite.h"

-

-#include "rtwtypes.h"

-#include "attitudeKalmanfilter_types.h"

-

-/* Type Definitions */

-

-/* Named Constants */

-

-/* Variable Declarations */

-

-/* Variable Definitions */

-

-/* Function Declarations */

-extern void attitudeKalmanfilter_initialize(void);

-#endif

-/* End of code generation (attitudeKalmanfilter_initialize.h) */

diff --git a/src/modules/attitude_estimator_ekf/codegen/attitudeKalmanfilter_terminate.c b/src/modules/attitude_estimator_ekf/codegen/attitudeKalmanfilter_terminate.c
deleted file mode 100755
index 7f9727419..000000000
--- a/src/modules/attitude_estimator_ekf/codegen/attitudeKalmanfilter_terminate.c
+++ /dev/null
@@ -1,31 +0,0 @@
-/*

- * attitudeKalmanfilter_terminate.c

- *

- * Code generation for function 'attitudeKalmanfilter_terminate'

- *

- * C source code generated on: Sat Jan 19 15:25:29 2013

- *

- */

-

-/* Include files */

-#include "rt_nonfinite.h"

-#include "attitudeKalmanfilter.h"

-#include "attitudeKalmanfilter_terminate.h"

-

-/* Type Definitions */

-

-/* Named Constants */

-

-/* Variable Declarations */

-

-/* Variable Definitions */

-

-/* Function Declarations */

-

-/* Function Definitions */

-void attitudeKalmanfilter_terminate(void)

-{

-  /* (no terminate code required) */

-}

-

-/* End of code generation (attitudeKalmanfilter_terminate.c) */

diff --git a/src/modules/attitude_estimator_ekf/codegen/attitudeKalmanfilter_terminate.h b/src/modules/attitude_estimator_ekf/codegen/attitudeKalmanfilter_terminate.h
deleted file mode 100755
index da84a5024..000000000
--- a/src/modules/attitude_estimator_ekf/codegen/attitudeKalmanfilter_terminate.h
+++ /dev/null
@@ -1,34 +0,0 @@
-/*

- * attitudeKalmanfilter_terminate.h

- *

- * Code generation for function 'attitudeKalmanfilter_terminate'

- *

- * C source code generated on: Sat Jan 19 15:25:29 2013

- *

- */

-

-#ifndef __ATTITUDEKALMANFILTER_TERMINATE_H__

-#define __ATTITUDEKALMANFILTER_TERMINATE_H__

-/* Include files */

-#include <math.h>

-#include <stddef.h>

-#include <stdlib.h>

-#include <string.h>

-#include "rt_defines.h"

-#include "rt_nonfinite.h"

-

-#include "rtwtypes.h"

-#include "attitudeKalmanfilter_types.h"

-

-/* Type Definitions */

-

-/* Named Constants */

-

-/* Variable Declarations */

-

-/* Variable Definitions */

-

-/* Function Declarations */

-extern void attitudeKalmanfilter_terminate(void);

-#endif

-/* End of code generation (attitudeKalmanfilter_terminate.h) */

diff --git a/src/modules/attitude_estimator_ekf/codegen/attitudeKalmanfilter_types.h b/src/modules/attitude_estimator_ekf/codegen/attitudeKalmanfilter_types.h
deleted file mode 100755
index 30fd1e75e..000000000
--- a/src/modules/attitude_estimator_ekf/codegen/attitudeKalmanfilter_types.h
+++ /dev/null
@@ -1,16 +0,0 @@
-/*

- * attitudeKalmanfilter_types.h

- *

- * Code generation for function 'attitudeKalmanfilter'

- *

- * C source code generated on: Sat Jan 19 15:25:29 2013

- *

- */

-

-#ifndef __ATTITUDEKALMANFILTER_TYPES_H__

-#define __ATTITUDEKALMANFILTER_TYPES_H__

-

-/* Type Definitions */

-

-#endif

-/* End of code generation (attitudeKalmanfilter_types.h) */

diff --git a/src/modules/attitude_estimator_ekf/codegen/cross.c b/src/modules/attitude_estimator_ekf/codegen/cross.c
deleted file mode 100755
index 84ada9002..000000000
--- a/src/modules/attitude_estimator_ekf/codegen/cross.c
+++ /dev/null
@@ -1,37 +0,0 @@
-/*

- * cross.c

- *

- * Code generation for function 'cross'

- *

- * C source code generated on: Sat Jan 19 15:25:29 2013

- *

- */

-

-/* Include files */

-#include "rt_nonfinite.h"

-#include "attitudeKalmanfilter.h"

-#include "cross.h"

-

-/* Type Definitions */

-

-/* Named Constants */

-

-/* Variable Declarations */

-

-/* Variable Definitions */

-

-/* Function Declarations */

-

-/* Function Definitions */

-

-/*

- *

- */

-void cross(const real32_T a[3], const real32_T b[3], real32_T c[3])

-{

-  c[0] = a[1] * b[2] - a[2] * b[1];

-  c[1] = a[2] * b[0] - a[0] * b[2];

-  c[2] = a[0] * b[1] - a[1] * b[0];

-}

-

-/* End of code generation (cross.c) */

diff --git a/src/modules/attitude_estimator_ekf/codegen/cross.h b/src/modules/attitude_estimator_ekf/codegen/cross.h
deleted file mode 100755
index e727f0684..000000000
--- a/src/modules/attitude_estimator_ekf/codegen/cross.h
+++ /dev/null
@@ -1,34 +0,0 @@
-/*

- * cross.h

- *

- * Code generation for function 'cross'

- *

- * C source code generated on: Sat Jan 19 15:25:29 2013

- *

- */

-

-#ifndef __CROSS_H__

-#define __CROSS_H__

-/* Include files */

-#include <math.h>

-#include <stddef.h>

-#include <stdlib.h>

-#include <string.h>

-#include "rt_defines.h"

-#include "rt_nonfinite.h"

-

-#include "rtwtypes.h"

-#include "attitudeKalmanfilter_types.h"

-

-/* Type Definitions */

-

-/* Named Constants */

-

-/* Variable Declarations */

-

-/* Variable Definitions */

-

-/* Function Declarations */

-extern void cross(const real32_T a[3], const real32_T b[3], real32_T c[3]);

-#endif

-/* End of code generation (cross.h) */

diff --git a/src/modules/attitude_estimator_ekf/codegen/eye.c b/src/modules/attitude_estimator_ekf/codegen/eye.c
deleted file mode 100755
index b89ab58ef..000000000
--- a/src/modules/attitude_estimator_ekf/codegen/eye.c
+++ /dev/null
@@ -1,51 +0,0 @@
-/*

- * eye.c

- *

- * Code generation for function 'eye'

- *

- * C source code generated on: Sat Jan 19 15:25:29 2013

- *

- */

-

-/* Include files */

-#include "rt_nonfinite.h"

-#include "attitudeKalmanfilter.h"

-#include "eye.h"

-

-/* Type Definitions */

-

-/* Named Constants */

-

-/* Variable Declarations */

-

-/* Variable Definitions */

-

-/* Function Declarations */

-

-/* Function Definitions */

-

-/*

- *

- */

-void b_eye(real_T I[144])

-{

-  int32_T i;

-  memset(&I[0], 0, 144U * sizeof(real_T));

-  for (i = 0; i < 12; i++) {

-    I[i + 12 * i] = 1.0;

-  }

-}

-

-/*

- *

- */

-void eye(real_T I[9])

-{

-  int32_T i;

-  memset(&I[0], 0, 9U * sizeof(real_T));

-  for (i = 0; i < 3; i++) {

-    I[i + 3 * i] = 1.0;

-  }

-}

-

-/* End of code generation (eye.c) */

diff --git a/src/modules/attitude_estimator_ekf/codegen/eye.h b/src/modules/attitude_estimator_ekf/codegen/eye.h
deleted file mode 100755
index 94fbe7671..000000000
--- a/src/modules/attitude_estimator_ekf/codegen/eye.h
+++ /dev/null
@@ -1,35 +0,0 @@
-/*

- * eye.h

- *

- * Code generation for function 'eye'

- *

- * C source code generated on: Sat Jan 19 15:25:29 2013

- *

- */

-

-#ifndef __EYE_H__

-#define __EYE_H__

-/* Include files */

-#include <math.h>

-#include <stddef.h>

-#include <stdlib.h>

-#include <string.h>

-#include "rt_defines.h"

-#include "rt_nonfinite.h"

-

-#include "rtwtypes.h"

-#include "attitudeKalmanfilter_types.h"

-

-/* Type Definitions */

-

-/* Named Constants */

-

-/* Variable Declarations */

-

-/* Variable Definitions */

-

-/* Function Declarations */

-extern void b_eye(real_T I[144]);

-extern void eye(real_T I[9]);

-#endif

-/* End of code generation (eye.h) */

diff --git a/src/modules/attitude_estimator_ekf/codegen/mrdivide.c b/src/modules/attitude_estimator_ekf/codegen/mrdivide.c
deleted file mode 100755
index a810f22e4..000000000
--- a/src/modules/attitude_estimator_ekf/codegen/mrdivide.c
+++ /dev/null
@@ -1,357 +0,0 @@
-/*

- * mrdivide.c

- *

- * Code generation for function 'mrdivide'

- *

- * C source code generated on: Sat Jan 19 15:25:29 2013

- *

- */

-

-/* Include files */

-#include "rt_nonfinite.h"

-#include "attitudeKalmanfilter.h"

-#include "mrdivide.h"

-

-/* Type Definitions */

-

-/* Named Constants */

-

-/* Variable Declarations */

-

-/* Variable Definitions */

-

-/* Function Declarations */

-

-/* Function Definitions */

-

-/*

- *

- */

-void b_mrdivide(const real32_T A[36], const real32_T B[9], real32_T y[36])

-{

-  real32_T b_A[9];

-  int32_T rtemp;

-  int32_T k;

-  real32_T b_B[36];

-  int32_T r1;

-  int32_T r2;

-  int32_T r3;

-  real32_T maxval;

-  real32_T a21;

-  real32_T Y[36];

-  for (rtemp = 0; rtemp < 3; rtemp++) {

-    for (k = 0; k < 3; k++) {

-      b_A[k + 3 * rtemp] = B[rtemp + 3 * k];

-    }

-  }

-

-  for (rtemp = 0; rtemp < 12; rtemp++) {

-    for (k = 0; k < 3; k++) {

-      b_B[k + 3 * rtemp] = A[rtemp + 12 * k];

-    }

-  }

-

-  r1 = 0;

-  r2 = 1;

-  r3 = 2;

-  maxval = (real32_T)fabs(b_A[0]);

-  a21 = (real32_T)fabs(b_A[1]);

-  if (a21 > maxval) {

-    maxval = a21;

-    r1 = 1;

-    r2 = 0;

-  }

-

-  if ((real32_T)fabs(b_A[2]) > maxval) {

-    r1 = 2;

-    r2 = 1;

-    r3 = 0;

-  }

-

-  b_A[r2] /= b_A[r1];

-  b_A[r3] /= b_A[r1];

-  b_A[3 + r2] -= b_A[r2] * b_A[3 + r1];

-  b_A[3 + r3] -= b_A[r3] * b_A[3 + r1];

-  b_A[6 + r2] -= b_A[r2] * b_A[6 + r1];

-  b_A[6 + r3] -= b_A[r3] * b_A[6 + r1];

-  if ((real32_T)fabs(b_A[3 + r3]) > (real32_T)fabs(b_A[3 + r2])) {

-    rtemp = r2;

-    r2 = r3;

-    r3 = rtemp;

-  }

-

-  b_A[3 + r3] /= b_A[3 + r2];

-  b_A[6 + r3] -= b_A[3 + r3] * b_A[6 + r2];

-  for (k = 0; k < 12; k++) {

-    Y[3 * k] = b_B[r1 + 3 * k];

-    Y[1 + 3 * k] = b_B[r2 + 3 * k] - Y[3 * k] * b_A[r2];

-    Y[2 + 3 * k] = (b_B[r3 + 3 * k] - Y[3 * k] * b_A[r3]) - Y[1 + 3 * k] * b_A[3

-      + r3];

-    Y[2 + 3 * k] /= b_A[6 + r3];

-    Y[3 * k] -= Y[2 + 3 * k] * b_A[6 + r1];

-    Y[1 + 3 * k] -= Y[2 + 3 * k] * b_A[6 + r2];

-    Y[1 + 3 * k] /= b_A[3 + r2];

-    Y[3 * k] -= Y[1 + 3 * k] * b_A[3 + r1];

-    Y[3 * k] /= b_A[r1];

-  }

-

-  for (rtemp = 0; rtemp < 3; rtemp++) {

-    for (k = 0; k < 12; k++) {

-      y[k + 12 * rtemp] = Y[rtemp + 3 * k];

-    }

-  }

-}

-

-/*

- *

- */

-void c_mrdivide(const real32_T A[72], const real32_T B[36], real32_T y[72])

-{

-  real32_T b_A[36];

-  int8_T ipiv[6];

-  int32_T i3;

-  int32_T iy;

-  int32_T j;

-  int32_T c;

-  int32_T ix;

-  real32_T temp;

-  int32_T k;

-  real32_T s;

-  int32_T jy;

-  int32_T ijA;

-  real32_T Y[72];

-  for (i3 = 0; i3 < 6; i3++) {

-    for (iy = 0; iy < 6; iy++) {

-      b_A[iy + 6 * i3] = B[i3 + 6 * iy];

-    }

-

-    ipiv[i3] = (int8_T)(1 + i3);

-  }

-

-  for (j = 0; j < 5; j++) {

-    c = j * 7;

-    iy = 0;

-    ix = c;

-    temp = (real32_T)fabs(b_A[c]);

-    for (k = 2; k <= 6 - j; k++) {

-      ix++;

-      s = (real32_T)fabs(b_A[ix]);

-      if (s > temp) {

-        iy = k - 1;

-        temp = s;

-      }

-    }

-

-    if (b_A[c + iy] != 0.0F) {

-      if (iy != 0) {

-        ipiv[j] = (int8_T)((j + iy) + 1);

-        ix = j;

-        iy += j;

-        for (k = 0; k < 6; k++) {

-          temp = b_A[ix];

-          b_A[ix] = b_A[iy];

-          b_A[iy] = temp;

-          ix += 6;

-          iy += 6;

-        }

-      }

-

-      i3 = (c - j) + 6;

-      for (jy = c + 1; jy + 1 <= i3; jy++) {

-        b_A[jy] /= b_A[c];

-      }

-    }

-

-    iy = c;

-    jy = c + 6;

-    for (k = 1; k <= 5 - j; k++) {

-      temp = b_A[jy];

-      if (b_A[jy] != 0.0F) {

-        ix = c + 1;

-        i3 = (iy - j) + 12;

-        for (ijA = 7 + iy; ijA + 1 <= i3; ijA++) {

-          b_A[ijA] += b_A[ix] * -temp;

-          ix++;

-        }

-      }

-

-      jy += 6;

-      iy += 6;

-    }

-  }

-

-  for (i3 = 0; i3 < 12; i3++) {

-    for (iy = 0; iy < 6; iy++) {

-      Y[iy + 6 * i3] = A[i3 + 12 * iy];

-    }

-  }

-

-  for (jy = 0; jy < 6; jy++) {

-    if (ipiv[jy] != jy + 1) {

-      for (j = 0; j < 12; j++) {

-        temp = Y[jy + 6 * j];

-        Y[jy + 6 * j] = Y[(ipiv[jy] + 6 * j) - 1];

-        Y[(ipiv[jy] + 6 * j) - 1] = temp;

-      }

-    }

-  }

-

-  for (j = 0; j < 12; j++) {

-    c = 6 * j;

-    for (k = 0; k < 6; k++) {

-      iy = 6 * k;

-      if (Y[k + c] != 0.0F) {

-        for (jy = k + 2; jy < 7; jy++) {

-          Y[(jy + c) - 1] -= Y[k + c] * b_A[(jy + iy) - 1];

-        }

-      }

-    }

-  }

-

-  for (j = 0; j < 12; j++) {

-    c = 6 * j;

-    for (k = 5; k > -1; k += -1) {

-      iy = 6 * k;

-      if (Y[k + c] != 0.0F) {

-        Y[k + c] /= b_A[k + iy];

-        for (jy = 0; jy + 1 <= k; jy++) {

-          Y[jy + c] -= Y[k + c] * b_A[jy + iy];

-        }

-      }

-    }

-  }

-

-  for (i3 = 0; i3 < 6; i3++) {

-    for (iy = 0; iy < 12; iy++) {

-      y[iy + 12 * i3] = Y[i3 + 6 * iy];

-    }

-  }

-}

-

-/*

- *

- */

-void mrdivide(const real32_T A[108], const real32_T B[81], real32_T y[108])

-{

-  real32_T b_A[81];

-  int8_T ipiv[9];

-  int32_T i2;

-  int32_T iy;

-  int32_T j;

-  int32_T c;

-  int32_T ix;

-  real32_T temp;

-  int32_T k;

-  real32_T s;

-  int32_T jy;

-  int32_T ijA;

-  real32_T Y[108];

-  for (i2 = 0; i2 < 9; i2++) {

-    for (iy = 0; iy < 9; iy++) {

-      b_A[iy + 9 * i2] = B[i2 + 9 * iy];

-    }

-

-    ipiv[i2] = (int8_T)(1 + i2);

-  }

-

-  for (j = 0; j < 8; j++) {

-    c = j * 10;

-    iy = 0;

-    ix = c;

-    temp = (real32_T)fabs(b_A[c]);

-    for (k = 2; k <= 9 - j; k++) {

-      ix++;

-      s = (real32_T)fabs(b_A[ix]);

-      if (s > temp) {

-        iy = k - 1;

-        temp = s;

-      }

-    }

-

-    if (b_A[c + iy] != 0.0F) {

-      if (iy != 0) {

-        ipiv[j] = (int8_T)((j + iy) + 1);

-        ix = j;

-        iy += j;

-        for (k = 0; k < 9; k++) {

-          temp = b_A[ix];

-          b_A[ix] = b_A[iy];

-          b_A[iy] = temp;

-          ix += 9;

-          iy += 9;

-        }

-      }

-

-      i2 = (c - j) + 9;

-      for (jy = c + 1; jy + 1 <= i2; jy++) {

-        b_A[jy] /= b_A[c];

-      }

-    }

-

-    iy = c;

-    jy = c + 9;

-    for (k = 1; k <= 8 - j; k++) {

-      temp = b_A[jy];

-      if (b_A[jy] != 0.0F) {

-        ix = c + 1;

-        i2 = (iy - j) + 18;

-        for (ijA = 10 + iy; ijA + 1 <= i2; ijA++) {

-          b_A[ijA] += b_A[ix] * -temp;

-          ix++;

-        }

-      }

-

-      jy += 9;

-      iy += 9;

-    }

-  }

-

-  for (i2 = 0; i2 < 12; i2++) {

-    for (iy = 0; iy < 9; iy++) {

-      Y[iy + 9 * i2] = A[i2 + 12 * iy];

-    }

-  }

-

-  for (jy = 0; jy < 9; jy++) {

-    if (ipiv[jy] != jy + 1) {

-      for (j = 0; j < 12; j++) {

-        temp = Y[jy + 9 * j];

-        Y[jy + 9 * j] = Y[(ipiv[jy] + 9 * j) - 1];

-        Y[(ipiv[jy] + 9 * j) - 1] = temp;

-      }

-    }

-  }

-

-  for (j = 0; j < 12; j++) {

-    c = 9 * j;

-    for (k = 0; k < 9; k++) {

-      iy = 9 * k;

-      if (Y[k + c] != 0.0F) {

-        for (jy = k + 2; jy < 10; jy++) {

-          Y[(jy + c) - 1] -= Y[k + c] * b_A[(jy + iy) - 1];

-        }

-      }

-    }

-  }

-

-  for (j = 0; j < 12; j++) {

-    c = 9 * j;

-    for (k = 8; k > -1; k += -1) {

-      iy = 9 * k;

-      if (Y[k + c] != 0.0F) {

-        Y[k + c] /= b_A[k + iy];

-        for (jy = 0; jy + 1 <= k; jy++) {

-          Y[jy + c] -= Y[k + c] * b_A[jy + iy];

-        }

-      }

-    }

-  }

-

-  for (i2 = 0; i2 < 9; i2++) {

-    for (iy = 0; iy < 12; iy++) {

-      y[iy + 12 * i2] = Y[i2 + 9 * iy];

-    }

-  }

-}

-

-/* End of code generation (mrdivide.c) */

diff --git a/src/modules/attitude_estimator_ekf/codegen/mrdivide.h b/src/modules/attitude_estimator_ekf/codegen/mrdivide.h
deleted file mode 100755
index 2d3b0d51f..000000000
--- a/src/modules/attitude_estimator_ekf/codegen/mrdivide.h
+++ /dev/null
@@ -1,36 +0,0 @@
-/*

- * mrdivide.h

- *

- * Code generation for function 'mrdivide'

- *

- * C source code generated on: Sat Jan 19 15:25:29 2013

- *

- */

-

-#ifndef __MRDIVIDE_H__

-#define __MRDIVIDE_H__

-/* Include files */

-#include <math.h>

-#include <stddef.h>

-#include <stdlib.h>

-#include <string.h>

-#include "rt_defines.h"

-#include "rt_nonfinite.h"

-

-#include "rtwtypes.h"

-#include "attitudeKalmanfilter_types.h"

-

-/* Type Definitions */

-

-/* Named Constants */

-

-/* Variable Declarations */

-

-/* Variable Definitions */

-

-/* Function Declarations */

-extern void b_mrdivide(const real32_T A[36], const real32_T B[9], real32_T y[36]);

-extern void c_mrdivide(const real32_T A[72], const real32_T B[36], real32_T y[72]);

-extern void mrdivide(const real32_T A[108], const real32_T B[81], real32_T y[108]);

-#endif

-/* End of code generation (mrdivide.h) */

diff --git a/src/modules/attitude_estimator_ekf/codegen/norm.c b/src/modules/attitude_estimator_ekf/codegen/norm.c
deleted file mode 100755
index 0c418cc7b..000000000
--- a/src/modules/attitude_estimator_ekf/codegen/norm.c
+++ /dev/null
@@ -1,54 +0,0 @@
-/*

- * norm.c

- *

- * Code generation for function 'norm'

- *

- * C source code generated on: Sat Jan 19 15:25:29 2013

- *

- */

-

-/* Include files */

-#include "rt_nonfinite.h"

-#include "attitudeKalmanfilter.h"

-#include "norm.h"

-

-/* Type Definitions */

-

-/* Named Constants */

-

-/* Variable Declarations */

-

-/* Variable Definitions */

-

-/* Function Declarations */

-

-/* Function Definitions */

-

-/*

- *

- */

-real32_T norm(const real32_T x[3])

-{

-  real32_T y;

-  real32_T scale;

-  int32_T k;

-  real32_T absxk;

-  real32_T t;

-  y = 0.0F;

-  scale = 1.17549435E-38F;

-  for (k = 0; k < 3; k++) {

-    absxk = (real32_T)fabs(x[k]);

-    if (absxk > scale) {

-      t = scale / absxk;

-      y = 1.0F + y * t * t;

-      scale = absxk;

-    } else {

-      t = absxk / scale;

-      y += t * t;

-    }

-  }

-

-  return scale * (real32_T)sqrt(y);

-}

-

-/* End of code generation (norm.c) */

diff --git a/src/modules/attitude_estimator_ekf/codegen/norm.h b/src/modules/attitude_estimator_ekf/codegen/norm.h
deleted file mode 100755
index 60cf77b57..000000000
--- a/src/modules/attitude_estimator_ekf/codegen/norm.h
+++ /dev/null
@@ -1,34 +0,0 @@
-/*

- * norm.h

- *

- * Code generation for function 'norm'

- *

- * C source code generated on: Sat Jan 19 15:25:29 2013

- *

- */

-

-#ifndef __NORM_H__

-#define __NORM_H__

-/* Include files */

-#include <math.h>

-#include <stddef.h>

-#include <stdlib.h>

-#include <string.h>

-#include "rt_defines.h"

-#include "rt_nonfinite.h"

-

-#include "rtwtypes.h"

-#include "attitudeKalmanfilter_types.h"

-

-/* Type Definitions */

-

-/* Named Constants */

-

-/* Variable Declarations */

-

-/* Variable Definitions */

-

-/* Function Declarations */

-extern real32_T norm(const real32_T x[3]);

-#endif

-/* End of code generation (norm.h) */

diff --git a/src/modules/attitude_estimator_ekf/codegen/rdivide.c b/src/modules/attitude_estimator_ekf/codegen/rdivide.c
deleted file mode 100755
index d035dae5e..000000000
--- a/src/modules/attitude_estimator_ekf/codegen/rdivide.c
+++ /dev/null
@@ -1,38 +0,0 @@
-/*

- * rdivide.c

- *

- * Code generation for function 'rdivide'

- *

- * C source code generated on: Sat Jan 19 15:25:29 2013

- *

- */

-

-/* Include files */

-#include "rt_nonfinite.h"

-#include "attitudeKalmanfilter.h"

-#include "rdivide.h"

-

-/* Type Definitions */

-

-/* Named Constants */

-

-/* Variable Declarations */

-

-/* Variable Definitions */

-

-/* Function Declarations */

-

-/* Function Definitions */

-

-/*

- *

- */

-void rdivide(const real32_T x[3], real32_T y, real32_T z[3])

-{

-  int32_T i;

-  for (i = 0; i < 3; i++) {

-    z[i] = x[i] / y;

-  }

-}

-

-/* End of code generation (rdivide.c) */

diff --git a/src/modules/attitude_estimator_ekf/codegen/rdivide.h b/src/modules/attitude_estimator_ekf/codegen/rdivide.h
deleted file mode 100755
index 4bbebebe2..000000000
--- a/src/modules/attitude_estimator_ekf/codegen/rdivide.h
+++ /dev/null
@@ -1,34 +0,0 @@
-/*

- * rdivide.h

- *

- * Code generation for function 'rdivide'

- *

- * C source code generated on: Sat Jan 19 15:25:29 2013

- *

- */

-

-#ifndef __RDIVIDE_H__

-#define __RDIVIDE_H__

-/* Include files */

-#include <math.h>

-#include <stddef.h>

-#include <stdlib.h>

-#include <string.h>

-#include "rt_defines.h"

-#include "rt_nonfinite.h"

-

-#include "rtwtypes.h"

-#include "attitudeKalmanfilter_types.h"

-

-/* Type Definitions */

-

-/* Named Constants */

-

-/* Variable Declarations */

-

-/* Variable Definitions */

-

-/* Function Declarations */

-extern void rdivide(const real32_T x[3], real32_T y, real32_T z[3]);

-#endif

-/* End of code generation (rdivide.h) */

diff --git a/src/modules/attitude_estimator_ekf/codegen/rtGetInf.c b/src/modules/attitude_estimator_ekf/codegen/rtGetInf.c
deleted file mode 100755
index 34164d104..000000000
--- a/src/modules/attitude_estimator_ekf/codegen/rtGetInf.c
+++ /dev/null
@@ -1,139 +0,0 @@
-/*

- * rtGetInf.c

- *

- * Code generation for function 'attitudeKalmanfilter'

- *

- * C source code generated on: Sat Jan 19 15:25:29 2013

- *

- */

-

-/*

- * Abstract:

- *       MATLAB for code generation function to initialize non-finite, Inf and MinusInf

- */

-#include "rtGetInf.h"

-#define NumBitsPerChar	8U

-

-/* Function: rtGetInf ==================================================

- * Abstract:

- * Initialize rtInf needed by the generated code.

- * Inf is initialized as non-signaling. Assumes IEEE.

- */

-real_T rtGetInf(void)

-{

-  size_t bitsPerReal = sizeof(real_T) * (NumBitsPerChar);

-  real_T inf = 0.0;

-  if (bitsPerReal == 32U) {

-    inf = rtGetInfF();

-  } else {

-    uint16_T one = 1U;

-    enum {

-      LittleEndian,

-      BigEndian

-    } machByteOrder = (*((uint8_T *) &one) == 1U) ? LittleEndian : BigEndian;

-    switch (machByteOrder) {

-     case LittleEndian:

-      {

-        union {

-          LittleEndianIEEEDouble bitVal;

-          real_T fltVal;

-        } tmpVal;

-

-        tmpVal.bitVal.words.wordH = 0x7FF00000U;

-        tmpVal.bitVal.words.wordL = 0x00000000U;

-        inf = tmpVal.fltVal;

-        break;

-      }

-

-     case BigEndian:

-      {

-        union {

-          BigEndianIEEEDouble bitVal;

-          real_T fltVal;

-        } tmpVal;

-

-        tmpVal.bitVal.words.wordH = 0x7FF00000U;

-        tmpVal.bitVal.words.wordL = 0x00000000U;

-        inf = tmpVal.fltVal;

-        break;

-      }

-    }

-  }

-

-  return inf;

-}

-

-/* Function: rtGetInfF ==================================================

- * Abstract:

- * Initialize rtInfF needed by the generated code.

- * Inf is initialized as non-signaling. Assumes IEEE.

- */

-real32_T rtGetInfF(void)

-{

-  IEEESingle infF;

-  infF.wordL.wordLuint = 0x7F800000U;

-  return infF.wordL.wordLreal;

-}

-

-/* Function: rtGetMinusInf ==================================================

- * Abstract:

- * Initialize rtMinusInf needed by the generated code.

- * Inf is initialized as non-signaling. Assumes IEEE.

- */

-real_T rtGetMinusInf(void)

-{

-  size_t bitsPerReal = sizeof(real_T) * (NumBitsPerChar);

-  real_T minf = 0.0;

-  if (bitsPerReal == 32U) {

-    minf = rtGetMinusInfF();

-  } else {

-    uint16_T one = 1U;

-    enum {

-      LittleEndian,

-      BigEndian

-    } machByteOrder = (*((uint8_T *) &one) == 1U) ? LittleEndian : BigEndian;

-    switch (machByteOrder) {

-     case LittleEndian:

-      {

-        union {

-          LittleEndianIEEEDouble bitVal;

-          real_T fltVal;

-        } tmpVal;

-

-        tmpVal.bitVal.words.wordH = 0xFFF00000U;

-        tmpVal.bitVal.words.wordL = 0x00000000U;

-        minf = tmpVal.fltVal;

-        break;

-      }

-

-     case BigEndian:

-      {

-        union {

-          BigEndianIEEEDouble bitVal;

-          real_T fltVal;

-        } tmpVal;

-

-        tmpVal.bitVal.words.wordH = 0xFFF00000U;

-        tmpVal.bitVal.words.wordL = 0x00000000U;

-        minf = tmpVal.fltVal;

-        break;

-      }

-    }

-  }

-

-  return minf;

-}

-

-/* Function: rtGetMinusInfF ==================================================

- * Abstract:

- * Initialize rtMinusInfF needed by the generated code.

- * Inf is initialized as non-signaling. Assumes IEEE.

- */

-real32_T rtGetMinusInfF(void)

-{

-  IEEESingle minfF;

-  minfF.wordL.wordLuint = 0xFF800000U;

-  return minfF.wordL.wordLreal;

-}

-

-/* End of code generation (rtGetInf.c) */

diff --git a/src/modules/attitude_estimator_ekf/codegen/rtGetInf.h b/src/modules/attitude_estimator_ekf/codegen/rtGetInf.h
deleted file mode 100755
index 145373cd0..000000000
--- a/src/modules/attitude_estimator_ekf/codegen/rtGetInf.h
+++ /dev/null
@@ -1,23 +0,0 @@
-/*

- * rtGetInf.h

- *

- * Code generation for function 'attitudeKalmanfilter'

- *

- * C source code generated on: Sat Jan 19 15:25:29 2013

- *

- */

-

-#ifndef __RTGETINF_H__

-#define __RTGETINF_H__

-

-#include <stddef.h>

-#include "rtwtypes.h"

-#include "rt_nonfinite.h"

-

-extern real_T rtGetInf(void);

-extern real32_T rtGetInfF(void);

-extern real_T rtGetMinusInf(void);

-extern real32_T rtGetMinusInfF(void);

-

-#endif

-/* End of code generation (rtGetInf.h) */

diff --git a/src/modules/attitude_estimator_ekf/codegen/rtGetNaN.c b/src/modules/attitude_estimator_ekf/codegen/rtGetNaN.c
deleted file mode 100755
index d84ca9573..000000000
--- a/src/modules/attitude_estimator_ekf/codegen/rtGetNaN.c
+++ /dev/null
@@ -1,96 +0,0 @@
-/*

- * rtGetNaN.c

- *

- * Code generation for function 'attitudeKalmanfilter'

- *

- * C source code generated on: Sat Jan 19 15:25:29 2013

- *

- */

-

-/*

- * Abstract:

- *       MATLAB for code generation function to initialize non-finite, NaN

- */

-#include "rtGetNaN.h"

-#define NumBitsPerChar	8U

-

-/* Function: rtGetNaN ==================================================

- * Abstract:

- * Initialize rtNaN needed by the generated code.

- * NaN is initialized as non-signaling. Assumes IEEE.

- */

-real_T rtGetNaN(void)

-{

-  size_t bitsPerReal = sizeof(real_T) * (NumBitsPerChar);

-  real_T nan = 0.0;

-  if (bitsPerReal == 32U) {

-    nan = rtGetNaNF();

-  } else {

-  uint16_T one = 1U;

-  enum {

-    LittleEndian,

-    BigEndian

-  } machByteOrder = (*((uint8_T *) &one) == 1U) ? LittleEndian : BigEndian;

-  switch (machByteOrder) {

-   case LittleEndian:

-    {

-      union {

-        LittleEndianIEEEDouble bitVal;

-        real_T fltVal;

-      } tmpVal;

-

-      tmpVal.bitVal.words.wordH = 0xFFF80000U;

-      tmpVal.bitVal.words.wordL = 0x00000000U;

-      nan = tmpVal.fltVal;

-      break;

-    }

-

-   case BigEndian:

-    {

-        union {

-          BigEndianIEEEDouble bitVal;

-          real_T fltVal;

-        } tmpVal;

-

-        tmpVal.bitVal.words.wordH = 0x7FFFFFFFU;

-        tmpVal.bitVal.words.wordL = 0xFFFFFFFFU;

-        nan = tmpVal.fltVal;

-        break;

-      }

-    }

-  }

-

-  return nan;

-}

-

-/* Function: rtGetNaNF ==================================================

- * Abstract:

- * Initialize rtNaNF needed by the generated code.

- * NaN is initialized as non-signaling. Assumes IEEE.

- */

-real32_T rtGetNaNF(void)

-{

-  IEEESingle nanF = { { 0 } };

-  uint16_T one = 1U;

-  enum {

-    LittleEndian,

-    BigEndian

-  } machByteOrder = (*((uint8_T *) &one) == 1U) ? LittleEndian : BigEndian;

-  switch (machByteOrder) {

-   case LittleEndian:

-    {

-      nanF.wordL.wordLuint = 0xFFC00000U;

-      break;

-    }

-

-   case BigEndian:

-    {

-      nanF.wordL.wordLuint = 0x7FFFFFFFU;

-      break;

-    }

-  }

-

-  return nanF.wordL.wordLreal;

-}

-

-/* End of code generation (rtGetNaN.c) */

diff --git a/src/modules/attitude_estimator_ekf/codegen/rtGetNaN.h b/src/modules/attitude_estimator_ekf/codegen/rtGetNaN.h
deleted file mode 100755
index 65fdaa96f..000000000
--- a/src/modules/attitude_estimator_ekf/codegen/rtGetNaN.h
+++ /dev/null
@@ -1,21 +0,0 @@
-/*

- * rtGetNaN.h

- *

- * Code generation for function 'attitudeKalmanfilter'

- *

- * C source code generated on: Sat Jan 19 15:25:29 2013

- *

- */

-

-#ifndef __RTGETNAN_H__

-#define __RTGETNAN_H__

-

-#include <stddef.h>

-#include "rtwtypes.h"

-#include "rt_nonfinite.h"

-

-extern real_T rtGetNaN(void);

-extern real32_T rtGetNaNF(void);

-

-#endif

-/* End of code generation (rtGetNaN.h) */

diff --git a/src/modules/attitude_estimator_ekf/codegen/rt_defines.h b/src/modules/attitude_estimator_ekf/codegen/rt_defines.h
deleted file mode 100755
index 356498363..000000000
--- a/src/modules/attitude_estimator_ekf/codegen/rt_defines.h
+++ /dev/null
@@ -1,24 +0,0 @@
-/*

- * rt_defines.h

- *

- * Code generation for function 'attitudeKalmanfilter'

- *

- * C source code generated on: Sat Jan 19 15:25:29 2013

- *

- */

-

-#ifndef __RT_DEFINES_H__

-#define __RT_DEFINES_H__

-

-#include <stdlib.h>

-

-#define RT_PI	3.14159265358979323846

-#define RT_PIF	3.1415927F

-#define RT_LN_10	2.30258509299404568402

-#define RT_LN_10F	2.3025851F

-#define RT_LOG10E	0.43429448190325182765

-#define RT_LOG10EF	0.43429449F

-#define RT_E	2.7182818284590452354

-#define RT_EF	2.7182817F

-#endif

-/* End of code generation (rt_defines.h) */

diff --git a/src/modules/attitude_estimator_ekf/codegen/rt_nonfinite.c b/src/modules/attitude_estimator_ekf/codegen/rt_nonfinite.c
deleted file mode 100755
index 303d1d9d2..000000000
--- a/src/modules/attitude_estimator_ekf/codegen/rt_nonfinite.c
+++ /dev/null
@@ -1,87 +0,0 @@
-/*

- * rt_nonfinite.c

- *

- * Code generation for function 'attitudeKalmanfilter'

- *

- * C source code generated on: Sat Jan 19 15:25:29 2013

- *

- */

-

-/*

- * Abstract:

- *      MATLAB for code generation function to initialize non-finites,

- *      (Inf, NaN and -Inf).

- */

-#include "rt_nonfinite.h"

-#include "rtGetNaN.h"

-#include "rtGetInf.h"

-

-real_T rtInf;

-real_T rtMinusInf;

-real_T rtNaN;

-real32_T rtInfF;

-real32_T rtMinusInfF;

-real32_T rtNaNF;

-

-/* Function: rt_InitInfAndNaN ==================================================

- * Abstract:

- * Initialize the rtInf, rtMinusInf, and rtNaN needed by the

- * generated code. NaN is initialized as non-signaling. Assumes IEEE.

- */

-void rt_InitInfAndNaN(size_t realSize)

-{

-  (void) (realSize);

-  rtNaN = rtGetNaN();

-  rtNaNF = rtGetNaNF();

-  rtInf = rtGetInf();

-  rtInfF = rtGetInfF();

-  rtMinusInf = rtGetMinusInf();

-  rtMinusInfF = rtGetMinusInfF();

-}

-

-/* Function: rtIsInf ==================================================

- * Abstract:

- * Test if value is infinite

- */

-boolean_T rtIsInf(real_T value)

-{

-  return ((value==rtInf || value==rtMinusInf) ? 1U : 0U);

-}

-

-/* Function: rtIsInfF =================================================

- * Abstract:

- * Test if single-precision value is infinite

- */

-boolean_T rtIsInfF(real32_T value)

-{

-  return(((value)==rtInfF || (value)==rtMinusInfF) ? 1U : 0U);

-}

-

-/* Function: rtIsNaN ==================================================

- * Abstract:

- * Test if value is not a number

- */

-boolean_T rtIsNaN(real_T value)

-{

-#if defined(_MSC_VER) && (_MSC_VER <= 1200)

-  return _isnan(value)? TRUE:FALSE;

-#else

-  return (value!=value)? 1U:0U;

-#endif

-}

-

-/* Function: rtIsNaNF =================================================

- * Abstract:

- * Test if single-precision value is not a number

- */

-boolean_T rtIsNaNF(real32_T value)

-{

-#if defined(_MSC_VER) && (_MSC_VER <= 1200)

-  return _isnan((real_T)value)? true:false;

-#else

-  return (value!=value)? 1U:0U;

-#endif

-}

-

-

-/* End of code generation (rt_nonfinite.c) */

diff --git a/src/modules/attitude_estimator_ekf/codegen/rt_nonfinite.h b/src/modules/attitude_estimator_ekf/codegen/rt_nonfinite.h
deleted file mode 100755
index bd56b30d9..000000000
--- a/src/modules/attitude_estimator_ekf/codegen/rt_nonfinite.h
+++ /dev/null
@@ -1,53 +0,0 @@
-/*

- * rt_nonfinite.h

- *

- * Code generation for function 'attitudeKalmanfilter'

- *

- * C source code generated on: Sat Jan 19 15:25:29 2013

- *

- */

-

-#ifndef __RT_NONFINITE_H__

-#define __RT_NONFINITE_H__

-

-#if defined(_MSC_VER) && (_MSC_VER <= 1200)

-#include <float.h>

-#endif

-#include <stddef.h>

-#include "rtwtypes.h"

-

-extern real_T rtInf;

-extern real_T rtMinusInf;

-extern real_T rtNaN;

-extern real32_T rtInfF;

-extern real32_T rtMinusInfF;

-extern real32_T rtNaNF;

-extern void rt_InitInfAndNaN(size_t realSize);

-extern boolean_T rtIsInf(real_T value);

-extern boolean_T rtIsInfF(real32_T value);

-extern boolean_T rtIsNaN(real_T value);

-extern boolean_T rtIsNaNF(real32_T value);

-

-typedef struct {

-  struct {

-    uint32_T wordH;

-    uint32_T wordL;

-  } words;

-} BigEndianIEEEDouble;

-

-typedef struct {

-  struct {

-    uint32_T wordL;

-    uint32_T wordH;

-  } words;

-} LittleEndianIEEEDouble;

-

-typedef struct {

-  union {

-    real32_T wordLreal;

-    uint32_T wordLuint;

-  } wordL;

-} IEEESingle;

-

-#endif

-/* End of code generation (rt_nonfinite.h) */

diff --git a/src/modules/attitude_estimator_ekf/codegen/rtwtypes.h b/src/modules/attitude_estimator_ekf/codegen/rtwtypes.h
index 9a5c96267..b5a02a7a6 100755..100644
--- a/src/modules/attitude_estimator_ekf/codegen/rtwtypes.h
+++ b/src/modules/attitude_estimator_ekf/codegen/rtwtypes.h
@@ -1,159 +1,160 @@
-/*

- * rtwtypes.h

- *

- * Code generation for function 'attitudeKalmanfilter'

- *

- * C source code generated on: Sat Jan 19 15:25:29 2013

- *

- */

-

-#ifndef __RTWTYPES_H__

-#define __RTWTYPES_H__

-#ifndef TRUE

-# define TRUE (1U)

-#endif

-#ifndef FALSE

-# define FALSE (0U)

-#endif

-#ifndef __TMWTYPES__

-#define __TMWTYPES__

-

-#include <limits.h>

-

-/*=======================================================================* 

- * Target hardware information

- *   Device type: Generic->MATLAB Host Computer

- *   Number of bits:     char:   8    short:   16    int:  32

- *                       long:  32      native word size:  32

- *   Byte ordering: LittleEndian

- *   Signed integer division rounds to: Zero

- *   Shift right on a signed integer as arithmetic shift: on

- *=======================================================================*/

-

-/*=======================================================================* 

- * Fixed width word size data types:                                     * 

- *   int8_T, int16_T, int32_T     - signed 8, 16, or 32 bit integers     * 

- *   uint8_T, uint16_T, uint32_T  - unsigned 8, 16, or 32 bit integers   * 

- *   real32_T, real64_T           - 32 and 64 bit floating point numbers * 

- *=======================================================================*/

-

-typedef signed char int8_T;

-typedef unsigned char uint8_T;

-typedef short int16_T;

-typedef unsigned short uint16_T;

-typedef int int32_T;

-typedef unsigned int uint32_T;

-typedef float real32_T;

-typedef double real64_T;

-

-/*===========================================================================* 

- * Generic type definitions: real_T, time_T, boolean_T, int_T, uint_T,       * 

- *                           ulong_T, char_T and byte_T.                     * 

- *===========================================================================*/

-

-typedef double real_T;

-typedef double time_T;

-typedef unsigned char boolean_T;

-typedef int int_T;

-typedef unsigned int uint_T;

-typedef unsigned long ulong_T;

-typedef char char_T;

-typedef char_T byte_T;

-

-/*===========================================================================* 

- * Complex number type definitions                                           * 

- *===========================================================================*/

-#define CREAL_T	

-   typedef struct {  

-     real32_T re;  

-     real32_T im;  

-   } creal32_T;  

-

-   typedef struct {  

-     real64_T re;  

-     real64_T im;  

-   } creal64_T;  

-

-   typedef struct {  

-     real_T re;  

-     real_T im;  

-   } creal_T;  

-

-   typedef struct {  

-     int8_T re;  

-     int8_T im;  

-   } cint8_T;  

-

-   typedef struct {  

-     uint8_T re;  

-     uint8_T im;  

-   } cuint8_T;  

-

-   typedef struct {  

-     int16_T re;  

-     int16_T im;  

-   } cint16_T;  

-

-   typedef struct {  

-     uint16_T re;  

-     uint16_T im;  

-   } cuint16_T;  

-

-   typedef struct {  

-     int32_T re;  

-     int32_T im;  

-   } cint32_T;  

-

-   typedef struct {  

-     uint32_T re;  

-     uint32_T im;  

-   } cuint32_T;  

-

-

-/*=======================================================================* 

- * Min and Max:                                                          * 

- *   int8_T, int16_T, int32_T     - signed 8, 16, or 32 bit integers     * 

- *   uint8_T, uint16_T, uint32_T  - unsigned 8, 16, or 32 bit integers   * 

- *=======================================================================*/

-

-#define MAX_int8_T  	((int8_T)(127))

-#define MIN_int8_T  	((int8_T)(-128))

-#define MAX_uint8_T 	((uint8_T)(255))

-#define MIN_uint8_T 	((uint8_T)(0))

-#define MAX_int16_T 	((int16_T)(32767))

-#define MIN_int16_T 	((int16_T)(-32768))

-#define MAX_uint16_T	((uint16_T)(65535))

-#define MIN_uint16_T	((uint16_T)(0))

-#define MAX_int32_T 	((int32_T)(2147483647))

-#define MIN_int32_T 	((int32_T)(-2147483647-1))

-#define MAX_uint32_T	((uint32_T)(0xFFFFFFFFU))

-#define MIN_uint32_T	((uint32_T)(0))

-

-/* Logical type definitions */

-#if !defined(__cplusplus) && !defined(__true_false_are_keywords)

-#  ifndef false

-#   define false (0U)

-#  endif

-#  ifndef true

-#   define true (1U)

-#  endif

-#endif

-

-/*

- * MATLAB for code generation assumes the code is compiled on a target using a 2's compliment representation

- * for signed integer values.

- */

-#if ((SCHAR_MIN + 1) != -SCHAR_MAX)

-#error "This code must be compiled using a 2's complement representation for signed integer values"

-#endif

-

-/*

- * Maximum length of a MATLAB identifier (function/variable)

- * including the null-termination character. Referenced by

- * rt_logging.c and rt_matrx.c.

- */

-#define TMW_NAME_LENGTH_MAX	64

-

-#endif

-#endif

-/* End of code generation (rtwtypes.h) */

+/*
+ * rtwtypes.h
+ *
+ * Code generation for function 'AttitudeEKF'
+ *
+ * C source code generated on: Thu Aug 21 11:17:28 2014
+ *
+ */
+
+#ifndef __RTWTYPES_H__
+#define __RTWTYPES_H__
+#ifndef TRUE
+# define TRUE (1U)
+#endif
+#ifndef FALSE
+# define FALSE (0U)
+#endif
+#ifndef __TMWTYPES__
+#define __TMWTYPES__
+
+#include <limits.h>
+
+/*=======================================================================* 
+ * Target hardware information
+ *   Device type: ARM Compatible->ARM Cortex
+ *   Number of bits:     char:   8    short:   16    int:  32
+ *                       long:  32
+ *                       native word size:  32
+ *   Byte ordering: LittleEndian
+ *   Signed integer division rounds to: Undefined
+ *   Shift right on a signed integer as arithmetic shift: on
+ *=======================================================================*/
+
+/*=======================================================================* 
+ * Fixed width word size data types:                                     * 
+ *   int8_T, int16_T, int32_T     - signed 8, 16, or 32 bit integers     * 
+ *   uint8_T, uint16_T, uint32_T  - unsigned 8, 16, or 32 bit integers   * 
+ *   real32_T, real64_T           - 32 and 64 bit floating point numbers * 
+ *=======================================================================*/
+
+typedef signed char int8_T;
+typedef unsigned char uint8_T;
+typedef short int16_T;
+typedef unsigned short uint16_T;
+typedef int int32_T;
+typedef unsigned int uint32_T;
+typedef float real32_T;
+typedef double real64_T;
+
+/*===========================================================================* 
+ * Generic type definitions: real_T, time_T, boolean_T, int_T, uint_T,       * 
+ *                           ulong_T, char_T and byte_T.                     * 
+ *===========================================================================*/
+
+typedef double real_T;
+typedef double time_T;
+typedef unsigned char boolean_T;
+typedef int int_T;
+typedef unsigned int uint_T;
+typedef unsigned long ulong_T;
+typedef char char_T;
+typedef char_T byte_T;
+
+/*===========================================================================* 
+ * Complex number type definitions                                           * 
+ *===========================================================================*/
+#define CREAL_T	
+   typedef struct {  
+      real32_T re;  
+      real32_T im;  
+   } creal32_T;  
+
+   typedef struct {  
+      real64_T re;  
+      real64_T im;  
+   } creal64_T;  
+
+   typedef struct {  
+      real_T re;  
+      real_T im;  
+   } creal_T;  
+
+   typedef struct {  
+      int8_T re;  
+      int8_T im;  
+   } cint8_T;  
+
+   typedef struct {  
+      uint8_T re;  
+      uint8_T im;  
+   } cuint8_T;  
+
+   typedef struct {  
+      int16_T re;  
+      int16_T im;  
+   } cint16_T;  
+
+   typedef struct {  
+      uint16_T re;  
+      uint16_T im;  
+   } cuint16_T;  
+
+   typedef struct {  
+      int32_T re;  
+      int32_T im;  
+   } cint32_T;  
+
+   typedef struct {  
+      uint32_T re;  
+      uint32_T im;  
+   } cuint32_T;  
+
+
+/*=======================================================================* 
+ * Min and Max:                                                          * 
+ *   int8_T, int16_T, int32_T     - signed 8, 16, or 32 bit integers     * 
+ *   uint8_T, uint16_T, uint32_T  - unsigned 8, 16, or 32 bit integers   * 
+ *=======================================================================*/
+
+#define MAX_int8_T  	((int8_T)(127))
+#define MIN_int8_T  	((int8_T)(-128))
+#define MAX_uint8_T 	((uint8_T)(255))
+#define MIN_uint8_T 	((uint8_T)(0))
+#define MAX_int16_T 	((int16_T)(32767))
+#define MIN_int16_T 	((int16_T)(-32768))
+#define MAX_uint16_T	((uint16_T)(65535))
+#define MIN_uint16_T	((uint16_T)(0))
+#define MAX_int32_T 	((int32_T)(2147483647))
+#define MIN_int32_T 	((int32_T)(-2147483647-1))
+#define MAX_uint32_T	((uint32_T)(0xFFFFFFFFU))
+#define MIN_uint32_T	((uint32_T)(0))
+
+/* Logical type definitions */
+#if !defined(__cplusplus) && !defined(__true_false_are_keywords)
+#  ifndef false
+#   define false (0U)
+#  endif
+#  ifndef true
+#   define true (1U)
+#  endif
+#endif
+
+/*
+ * MATLAB for code generation assumes the code is compiled on a target using a 2's compliment representation
+ * for signed integer values.
+ */
+#if ((SCHAR_MIN + 1) != -SCHAR_MAX)
+#error "This code must be compiled using a 2's complement representation for signed integer values"
+#endif
+
+/*
+ * Maximum length of a MATLAB identifier (function/variable)
+ * including the null-termination character. Referenced by
+ * rt_logging.c and rt_matrx.c.
+ */
+#define TMW_NAME_LENGTH_MAX	64
+
+#endif
+#endif
+/* End of code generation (rtwtypes.h) */
diff --git a/src/modules/attitude_estimator_ekf/module.mk b/src/modules/attitude_estimator_ekf/module.mk
index 99d0c5bf2..d158d7a49 100644
--- a/src/modules/attitude_estimator_ekf/module.mk
+++ b/src/modules/attitude_estimator_ekf/module.mk
@@ -39,16 +39,10 @@ MODULE_COMMAND	 = attitude_estimator_ekf
 
 SRCS		 = attitude_estimator_ekf_main.cpp \
 		   attitude_estimator_ekf_params.c \
-		   codegen/eye.c \
-		   codegen/attitudeKalmanfilter.c \
-		   codegen/mrdivide.c \
-		   codegen/rdivide.c \
-		   codegen/attitudeKalmanfilter_initialize.c \
-		   codegen/attitudeKalmanfilter_terminate.c \
-		   codegen/rt_nonfinite.c \
-		   codegen/rtGetInf.c \
-		   codegen/rtGetNaN.c \
-		   codegen/norm.c \
-		   codegen/cross.c
+		   codegen/AttitudeEKF.c
 
 MODULE_STACKSIZE = 1200
+
+EXTRACFLAGS = -Wno-float-equal -Wframe-larger-than=3700
+
+EXTRACXXFLAGS = -Wframe-larger-than=2400
diff --git a/src/modules/attitude_estimator_so3/attitude_estimator_so3_main.cpp b/src/modules/attitude_estimator_so3/attitude_estimator_so3_main.cpp
index e49027e47..9414225ca 100755
--- a/src/modules/attitude_estimator_so3/attitude_estimator_so3_main.cpp
+++ b/src/modules/attitude_estimator_so3/attitude_estimator_so3_main.cpp
@@ -153,7 +153,7 @@ int attitude_estimator_so3_main(int argc, char *argv[])
 					      SCHED_PRIORITY_MAX - 5,
 					      14000,
 					      attitude_estimator_so3_thread_main,
-					      (argv) ? (const char **)&argv[2] : (const char **)NULL);
+					      (argv) ? (char * const *)&argv[2] : (char * const *)NULL);
 		exit(0);
 	}
 
diff --git a/src/modules/attitude_estimator_so3/module.mk b/src/modules/attitude_estimator_so3/module.mk
index f52715abb..7b2e206cc 100644
--- a/src/modules/attitude_estimator_so3/module.mk
+++ b/src/modules/attitude_estimator_so3/module.mk
@@ -8,3 +8,5 @@ SRCS		 = attitude_estimator_so3_main.cpp \
 		   attitude_estimator_so3_params.c
 
 MODULE_STACKSIZE = 1200
+
+EXTRACXXFLAGS = -Wno-float-equal
diff --git a/src/modules/bottle_drop/bottle_drop.cpp b/src/modules/bottle_drop/bottle_drop.cpp
index e0bcbc6e9..4580b338d 100644
--- a/src/modules/bottle_drop/bottle_drop.cpp
+++ b/src/modules/bottle_drop/bottle_drop.cpp
@@ -523,6 +523,9 @@ BottleDrop::task_main()
 			}
 
 			switch (_drop_state) {
+				case DROP_STATE_INIT:
+					// do nothing
+					break;
 
 				case DROP_STATE_TARGET_VALID:
 				{
@@ -689,6 +692,10 @@ BottleDrop::task_main()
 						orb_publish(ORB_ID(onboard_mission), _onboard_mission_pub, &_onboard_mission);
 					}
 					break;
+
+				case DROP_STATE_BAY_CLOSED:
+					// do nothing
+					break;
 			}
 
 			counter++;
diff --git a/src/modules/commander/accelerometer_calibration.cpp b/src/modules/commander/accelerometer_calibration.cpp
index 0cb41489f..13ab966ab 100644
--- a/src/modules/commander/accelerometer_calibration.cpp
+++ b/src/modules/commander/accelerometer_calibration.cpp
@@ -159,6 +159,7 @@ int calculate_calibration_values(float accel_ref[6][3], float accel_T[3][3], flo
 int do_accel_calibration(int mavlink_fd)
 {
 	int fd;
+	int32_t device_id;
 
 	mavlink_log_info(mavlink_fd, CAL_STARTED_MSG, sensor_name);
 
@@ -180,6 +181,9 @@ int do_accel_calibration(int mavlink_fd)
 
 	/* reset all offsets to zero and all scales to one */
 	fd = open(ACCEL_DEVICE_PATH, 0);
+
+	device_id = ioctl(fd, DEVIOCGDEVICEID, 0);
+
 	res = ioctl(fd, ACCELIOCSSCALE, (long unsigned int)&accel_scale);
 	close(fd);
 
@@ -226,6 +230,10 @@ int do_accel_calibration(int mavlink_fd)
 			mavlink_log_critical(mavlink_fd, CAL_FAILED_SET_PARAMS_MSG);
 			res = ERROR;
 		}
+
+		if (param_set(param_find("SENS_ACC_ID"), &(device_id))) {
+				res = ERROR;
+		}
 	}
 
 	if (res == OK) {
@@ -263,7 +271,7 @@ int do_accel_calibration_measurements(int mavlink_fd, float accel_offs[3], float
 	const int samples_num = 2500;
 	float accel_ref[6][3];
 	bool data_collected[6] = { false, false, false, false, false, false };
-	const char *orientation_strs[6] = { "front", "back", "left", "right", "top", "bottom" };
+	const char *orientation_strs[6] = { "back", "front", "left", "right", "up", "down" };
 
 	int sensor_combined_sub = orb_subscribe(ORB_ID(sensor_combined));
 
@@ -294,12 +302,12 @@ int do_accel_calibration_measurements(int mavlink_fd, float accel_offs[3], float
 
 		/* inform user which axes are still needed */
 		mavlink_log_info(mavlink_fd, "pending: %s%s%s%s%s%s",
-				 (!data_collected[0]) ? "front " : "",
-				 (!data_collected[1]) ? "back " : "",
+				 (!data_collected[5]) ? "down " : "",
+				 (!data_collected[0]) ? "back " : "",
+				 (!data_collected[1]) ? "front " : "",
 				 (!data_collected[2]) ? "left " : "",
 				 (!data_collected[3]) ? "right " : "",
-				 (!data_collected[4]) ? "up " : "",
-				 (!data_collected[5]) ? "down " : "");
+				 (!data_collected[4]) ? "up " : "");
 
 		/* allow user enough time to read the message */
 		sleep(3);
diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp
index ec173c12b..247a2c5b8 100644
--- a/src/modules/commander/commander.cpp
+++ b/src/modules/commander/commander.cpp
@@ -81,7 +81,10 @@
 #include <uORB/topics/system_power.h>
 #include <uORB/topics/mission.h>
 #include <uORB/topics/mission_result.h>
+#include <uORB/topics/geofence_result.h>
 #include <uORB/topics/telemetry_status.h>
+#include <uORB/topics/vtol_vehicle_status.h>
+ #include <uORB/topics/vehicle_land_detected.h>
 
 #include <drivers/drv_led.h>
 #include <drivers/drv_hrt.h>
@@ -149,6 +152,9 @@ enum MAV_MODE_FLAG {
 /* Mavlink file descriptors */
 static int mavlink_fd = 0;
 
+/* Syste autostart ID */
+static int autostart_id;
+
 /* flags */
 static bool commander_initialized = false;
 static volatile bool thread_should_exit = false;		/**< daemon exit flag */
@@ -236,6 +242,13 @@ transition_result_t check_navigation_state_machine(struct vehicle_status_s *stat
 transition_result_t arm_disarm(bool arm, const int mavlink_fd, const char *armedBy);
 
 /**
+* @brief This function initializes the home position of the vehicle. This happens first time we get a good GPS fix and each
+*		 time the vehicle is armed with a good GPS fix.
+**/
+static void commander_set_home_position(orb_advert_t &homePub, home_position_s &home,
+					const vehicle_local_position_s &localPosition, const vehicle_global_position_s &globalPosition);
+
+/**
  * Loop that runs at a lower rate and priority for calibration and parameter tasks.
  */
 void *commander_low_prio_loop(void *arg);
@@ -263,7 +276,7 @@ int commander_main(int argc, char *argv[])
 					     SCHED_PRIORITY_MAX - 40,
 					     3200,
 					     commander_thread_main,
-					     (argv) ? (const char **)&argv[2] : (const char **)NULL);
+					     (argv) ? (char * const *)&argv[2] : (char * const *)NULL);
 
 		while (!thread_running) {
 			usleep(200);
@@ -310,12 +323,16 @@ int commander_main(int argc, char *argv[])
 	}
 
 	if (!strcmp(argv[1], "arm")) {
-		arm_disarm(true, mavlink_fd, "command line");
+		int mavlink_fd_local = open(MAVLINK_LOG_DEVICE, 0);
+		arm_disarm(true, mavlink_fd_local, "command line");
+		close(mavlink_fd_local);
 		exit(0);
 	}
 
 	if (!strcmp(argv[1], "disarm")) {
-		arm_disarm(false, mavlink_fd, "command line");
+		int mavlink_fd_local = open(MAVLINK_LOG_DEVICE, 0);
+		arm_disarm(false, mavlink_fd_local, "command line");
+		close(mavlink_fd_local);
 		exit(0);
 	}
 
@@ -551,7 +568,7 @@ bool handle_command(struct vehicle_status_s *status_local, const struct safety_s
 		}
 		break;
 
-	/* Flight termination */
+		/* Flight termination */
 	case VEHICLE_CMD_DO_FLIGHTTERMINATION: {
 			if (cmd->param1 > 0.5f) {
 				//XXX update state machine?
@@ -707,6 +724,53 @@ bool handle_command(struct vehicle_status_s *status_local, const struct safety_s
 	return true;
 }
 
+/**
+* @brief This function initializes the home position of the vehicle. This happens first time we get a good GPS fix and each
+*		 time the vehicle is armed with a good GPS fix.
+**/
+static void commander_set_home_position(orb_advert_t &homePub, home_position_s &home,
+					const vehicle_local_position_s &localPosition, const vehicle_global_position_s &globalPosition)
+{
+	//Need global position fix to be able to set home
+	if (!status.condition_global_position_valid) {
+		return;
+	}
+
+	//Ensure that the GPS accuracy is good enough for intializing home
+	if (globalPosition.eph > eph_threshold || globalPosition.epv > epv_threshold) {
+		return;
+	}
+
+	//Set home position
+	home.timestamp = hrt_absolute_time();
+	home.lat = globalPosition.lat;
+	home.lon = globalPosition.lon;
+	home.alt = globalPosition.alt;
+
+	home.x = localPosition.x;
+	home.y = localPosition.y;
+	home.z = localPosition.z;
+
+	warnx("home: lat = %.7f, lon = %.7f, alt = %.2f ", home.lat, home.lon, (double)home.alt);
+	mavlink_log_info(mavlink_fd, "home: %.7f, %.7f, %.2f", home.lat, home.lon, (double)home.alt);
+
+	/* announce new home position */
+	if (homePub > 0) {
+		orb_publish(ORB_ID(home_position), homePub, &home);
+
+	} else {
+		homePub = orb_advertise(ORB_ID(home_position), &home);
+	}
+
+	//Play tune first time we initialize HOME
+	if (!status.condition_home_position_valid) {
+		tune_positive(true);
+	}
+
+	/* mark home position as set */
+	status.condition_home_position_valid = true;
+}
+
 int commander_thread_main(int argc, char *argv[])
 {
 	/* not yet initialized */
@@ -728,9 +792,7 @@ int commander_thread_main(int argc, char *argv[])
 	param_t _param_ef_throttle_thres = param_find("COM_EF_THROT");
 	param_t _param_ef_current2throttle_thres = param_find("COM_EF_C2T");
 	param_t _param_ef_time_thres = param_find("COM_EF_TIME");
-
-	/* welcome user */
-	warnx("starting");
+	param_t _param_autostart_id = param_find("SYS_AUTOSTART");
 
 	const char *main_states_str[MAIN_STATE_MAX];
 	main_states_str[MAIN_STATE_MANUAL]			= "MANUAL";
@@ -897,7 +959,6 @@ int commander_thread_main(int argc, char *argv[])
 	bool critical_battery_voltage_actions_done = false;
 
 	hrt_abstime last_idle_time = 0;
-	hrt_abstime start_time = 0;
 
 	bool status_changed = true;
 	bool param_init_forced = true;
@@ -917,6 +978,11 @@ int commander_thread_main(int argc, char *argv[])
 	struct mission_result_s mission_result;
 	memset(&mission_result, 0, sizeof(mission_result));
 
+	/* Subscribe to geofence result topic */
+	int geofence_result_sub = orb_subscribe(ORB_ID(geofence_result));
+	struct geofence_result_s geofence_result;
+	memset(&geofence_result, 0, sizeof(geofence_result));
+
 	/* Subscribe to manual control data */
 	int sp_man_sub = orb_subscribe(ORB_ID(manual_control_setpoint));
 	struct manual_control_setpoint_s sp_man;
@@ -952,6 +1018,11 @@ int commander_thread_main(int argc, char *argv[])
 	struct vehicle_local_position_s local_position;
 	memset(&local_position, 0, sizeof(local_position));
 
+	/* Subscribe to land detector */
+	int land_detector_sub = orb_subscribe(ORB_ID(vehicle_land_detected));
+	struct vehicle_land_detected_s land_detector;
+	memset(&land_detector, 0, sizeof(land_detector));
+
 	/*
 	 * The home position is set based on GPS only, to prevent a dependency between
 	 * position estimator and commander. RAW GPS is more than good enough for a
@@ -1010,13 +1081,20 @@ int commander_thread_main(int argc, char *argv[])
 	struct actuator_controls_s actuator_controls;
 	memset(&actuator_controls, 0, sizeof(actuator_controls));
 
+	/* Subscribe to vtol vehicle status topic */
+	int vtol_vehicle_status_sub = orb_subscribe(ORB_ID(vtol_vehicle_status));
+	struct vtol_vehicle_status_s vtol_status;
+	memset(&vtol_status, 0, sizeof(vtol_status));
+	vtol_status.vtol_in_rw_mode = true;		//default for vtol is rotary wing
+
+
 	control_status_leds(&status, &armed, true);
 
 	/* now initialized */
 	commander_initialized = true;
 	thread_running = true;
 
-	start_time = hrt_absolute_time();
+	const hrt_abstime commander_boot_timestamp = hrt_absolute_time();
 
 	transition_result_t arming_ret;
 
@@ -1066,13 +1144,20 @@ int commander_thread_main(int argc, char *argv[])
 				    status.system_type == VEHICLE_TYPE_TRICOPTER ||
 				    status.system_type == VEHICLE_TYPE_QUADROTOR ||
 				    status.system_type == VEHICLE_TYPE_HEXAROTOR ||
-				    status.system_type == VEHICLE_TYPE_OCTOROTOR) {
+				    status.system_type == VEHICLE_TYPE_OCTOROTOR ||
+				    (status.system_type == VEHICLE_TYPE_VTOL_DUOROTOR && vtol_status.vtol_in_rw_mode) ||
+				    (status.system_type == VEHICLE_TYPE_VTOL_QUADROTOR && vtol_status.vtol_in_rw_mode)) {
+
 					status.is_rotary_wing = true;
 
 				} else {
 					status.is_rotary_wing = false;
 				}
 
+				/* set vehicle_status.is_vtol flag */
+				status.is_vtol = (status.system_type == VEHICLE_TYPE_VTOL_DUOROTOR) ||
+						 (status.system_type == VEHICLE_TYPE_VTOL_QUADROTOR);
+
 				/* check and update system / component ID */
 				param_get(_param_system_id, &(status.system_id));
 				param_get(_param_component_id, &(status.component_id));
@@ -1094,6 +1179,8 @@ int commander_thread_main(int argc, char *argv[])
 
 			/* navigation parameters */
 			param_get(_param_takeoff_alt, &takeoff_alt);
+
+			/* Safety parameters */
 			param_get(_param_enable_parachute, &parachute_enabled);
 			param_get(_param_enable_datalink_loss, &datalink_loss_enabled);
 			param_get(_param_datalink_loss_timeout, &datalink_loss_timeout);
@@ -1102,6 +1189,9 @@ int commander_thread_main(int argc, char *argv[])
 			param_get(_param_ef_throttle_thres, &ef_throttle_thres);
 			param_get(_param_ef_current2throttle_thres, &ef_current2throttle_thres);
 			param_get(_param_ef_time_thres, &ef_time_thres);
+
+			/* Autostart id */
+			param_get(_param_autostart_id, &autostart_id);
 		}
 
 		orb_check(sp_man_sub, &updated);
@@ -1212,6 +1302,7 @@ int commander_thread_main(int argc, char *argv[])
 		orb_check(safety_sub, &updated);
 
 		if (updated) {
+			bool previous_safety_off = safety.safety_off;
 			orb_copy(ORB_ID(safety), safety_sub, &safety);
 
 			/* disarm if safety is now on and still armed */
@@ -1225,6 +1316,33 @@ int commander_thread_main(int argc, char *argv[])
 					arming_state_changed = true;
 				}
 			}
+
+			//Notify the user if the status of the safety switch changes
+			if (safety.safety_switch_available && previous_safety_off != safety.safety_off) {
+
+				if (safety.safety_off) {
+					set_tune(TONE_NOTIFY_POSITIVE_TUNE);
+
+				} else {
+					tune_neutral(true);
+				}
+
+				status_changed = true;
+			}
+		}
+
+		/* update vtol vehicle status*/
+		orb_check(vtol_vehicle_status_sub, &updated);
+
+		if (updated) {
+			/* vtol status changed */
+			orb_copy(ORB_ID(vtol_vehicle_status), vtol_vehicle_status_sub, &vtol_status);
+			status.vtol_fw_permanent_stab = vtol_status.fw_permanent_stab;
+
+			/* Make sure that this is only adjusted if vehicle realy is of type vtol*/
+			if ((status.system_type == VEHICLE_TYPE_VTOL_DUOROTOR) || (status.system_type == VEHICLE_TYPE_VTOL_QUADROTOR)) {
+				status.is_rotary_wing = vtol_status.vtol_in_rw_mode;
+			}
 		}
 
 		/* update global position estimate */
@@ -1267,34 +1385,6 @@ int commander_thread_main(int argc, char *argv[])
 		check_valid(global_position.timestamp, POSITION_TIMEOUT, eph_good, &(status.condition_global_position_valid),
 			    &status_changed);
 
-		/* update home position */
-		if (!status.condition_home_position_valid && status.condition_global_position_valid && !armed.armed &&
-		    (global_position.eph < eph_threshold) && (global_position.epv < epv_threshold)) {
-
-			home.lat = global_position.lat;
-			home.lon = global_position.lon;
-			home.alt = global_position.alt;
-
-			home.x = local_position.x;
-			home.y = local_position.y;
-			home.z = local_position.z;
-
-			warnx("home: lat = %.7f, lon = %.7f, alt = %.2f ", home.lat, home.lon, (double)home.alt);
-			mavlink_log_info(mavlink_fd, "[cmd] home: %.7f, %.7f, %.2f", home.lat, home.lon, (double)home.alt);
-
-			/* announce new home position */
-			if (home_pub > 0) {
-				orb_publish(ORB_ID(home_position), home_pub, &home);
-
-			} else {
-				home_pub = orb_advertise(ORB_ID(home_position), &home);
-			}
-
-			/* mark home position as set */
-			status.condition_home_position_valid = true;
-			tune_positive(true);
-		}
-
 		/* update condition_local_position_valid and condition_local_altitude_valid */
 		/* hysteresis for EPH */
 		bool local_eph_good;
@@ -1321,9 +1411,15 @@ int commander_thread_main(int argc, char *argv[])
 		check_valid(local_position.timestamp, POSITION_TIMEOUT, local_position.z_valid,
 			    &(status.condition_local_altitude_valid), &status_changed);
 
+		/* Update land detector */
+		orb_check(land_detector_sub, &updated);
+		if(updated) {
+			orb_copy(ORB_ID(vehicle_land_detected), land_detector_sub, &land_detector);
+		}
+
 		if (status.condition_local_altitude_valid) {
-			if (status.condition_landed != local_position.landed) {
-				status.condition_landed = local_position.landed;
+			if (status.condition_landed != land_detector.landed) {
+				status.condition_landed = land_detector.landed;
 				status_changed = true;
 
 				if (status.condition_landed) {
@@ -1343,7 +1439,7 @@ int commander_thread_main(int argc, char *argv[])
 			orb_copy(ORB_ID_VEHICLE_ATTITUDE_CONTROLS, actuator_controls_sub, &actuator_controls);
 
 			/* only consider battery voltage if system has been running 2s and battery voltage is valid */
-			if (hrt_absolute_time() > start_time + 2000000 && battery.voltage_filtered_v > 0.0f) {
+			if (hrt_absolute_time() > commander_boot_timestamp + 2000000 && battery.voltage_filtered_v > 0.0f) {
 				status.battery_voltage = battery.voltage_filtered_v;
 				status.battery_current = battery.current_a;
 				status.condition_battery_voltage_valid = true;
@@ -1361,7 +1457,7 @@ int commander_thread_main(int argc, char *argv[])
 		if (updated) {
 			orb_copy(ORB_ID(subsystem_info), subsys_sub, &info);
 
-			warnx("subsystem changed: %d\n", (int)info.subsystem_type);
+			//warnx("subsystem changed: %d\n", (int)info.subsystem_type);
 
 			/* mark / unmark as present */
 			if (info.present) {
@@ -1512,27 +1608,34 @@ int commander_thread_main(int argc, char *argv[])
 
 		if (updated) {
 			orb_copy(ORB_ID(mission_result), mission_result_sub, &mission_result);
+		}
 
-			/* Check for geofence violation */
-			if (armed.armed && (mission_result.geofence_violated || mission_result.flight_termination)) {
-				//XXX: make this configurable to select different actions (e.g. navigation modes)
-				/* this will only trigger if geofence is activated via param and a geofence file is present, also there is a circuit breaker to disable the actual flight termination in the px4io driver */
-				armed.force_failsafe = true;
-				status_changed = true;
-				static bool flight_termination_printed = false;
-
-				if (!flight_termination_printed) {
-					warnx("Flight termination because of navigator request or geofence");
-					mavlink_log_critical(mavlink_fd, "GF violation: flight termination");
-					flight_termination_printed = true;
-				}
+		/* start geofence result check */
+		orb_check(geofence_result_sub, &updated);
 
-				if (counter % (1000000 / COMMANDER_MONITORING_INTERVAL) == 0) {
-					mavlink_log_critical(mavlink_fd, "GF violation: flight termination");
-				}
-			} // no reset is done here on purpose, on geofence violation we want to stay in flighttermination
+		if (updated) {
+			orb_copy(ORB_ID(geofence_result), geofence_result_sub, &geofence_result);
 		}
 
+		/* Check for geofence violation */
+		if (armed.armed && (geofence_result.geofence_violated || mission_result.flight_termination)) {
+			//XXX: make this configurable to select different actions (e.g. navigation modes)
+			/* this will only trigger if geofence is activated via param and a geofence file is present, also there is a circuit breaker to disable the actual flight termination in the px4io driver */
+			armed.force_failsafe = true;
+			status_changed = true;
+			static bool flight_termination_printed = false;
+
+			if (!flight_termination_printed) {
+				warnx("Flight termination because of navigator request or geofence");
+				mavlink_log_critical(mavlink_fd, "GF violation: flight termination");
+				flight_termination_printed = true;
+			}
+
+			if (counter % (1000000 / COMMANDER_MONITORING_INTERVAL) == 0) {
+				mavlink_log_critical(mavlink_fd, "GF violation: flight termination");
+			}
+		} // no reset is done here on purpose, on geofence violation we want to stay in flighttermination
+
 		/* RC input check */
 		if (!status.rc_input_blocked && sp_man.timestamp != 0 &&
 		    hrt_absolute_time() < sp_man.timestamp + (uint64_t)(rc_loss_timeout * 1e6f)) {
@@ -1544,7 +1647,8 @@ int commander_thread_main(int argc, char *argv[])
 
 			} else {
 				if (status.rc_signal_lost) {
-					mavlink_log_critical(mavlink_fd, "RC signal regained");
+					mavlink_log_critical(mavlink_fd, "RC SIGNAL REGAINED after %llums",
+							     (hrt_absolute_time() - status.rc_signal_lost_timestamp) / 1000);
 					status_changed = true;
 				}
 			}
@@ -1645,8 +1749,9 @@ int commander_thread_main(int argc, char *argv[])
 
 		} else {
 			if (!status.rc_signal_lost) {
-				mavlink_log_critical(mavlink_fd, "RC SIGNAL LOST");
+				mavlink_log_critical(mavlink_fd, "RC SIGNAL LOST (at t=%llums)", hrt_absolute_time() / 1000);
 				status.rc_signal_lost = true;
+				status.rc_signal_lost_timestamp = sp_man.timestamp;
 				status_changed = true;
 			}
 		}
@@ -1663,7 +1768,7 @@ int commander_thread_main(int argc, char *argv[])
 				if (telemetry_lost[i] &&
 				    hrt_elapsed_time(&telemetry_last_dl_loss[i]) > datalink_regain_timeout * 1e6) {
 
-					mavlink_log_critical(mavlink_fd, "data link %i regained", i);
+					mavlink_log_info(mavlink_fd, "data link %i regained", i);
 					telemetry_lost[i] = false;
 					have_link = true;
 
@@ -1677,7 +1782,7 @@ int commander_thread_main(int argc, char *argv[])
 				telemetry_last_dl_loss[i]  = hrt_absolute_time();
 
 				if (!telemetry_lost[i]) {
-					mavlink_log_critical(mavlink_fd, "data link %i lost", i);
+					mavlink_log_info(mavlink_fd, "data link %i lost", i);
 					telemetry_lost[i] = true;
 				}
 			}
@@ -1692,7 +1797,7 @@ int commander_thread_main(int argc, char *argv[])
 
 		} else {
 			if (!status.data_link_lost) {
-				mavlink_log_critical(mavlink_fd, "ALL DATA LINKS LOST");
+				mavlink_log_info(mavlink_fd, "ALL DATA LINKS LOST");
 				status.data_link_lost = true;
 				status.data_link_lost_counter++;
 				status_changed = true;
@@ -1793,42 +1898,23 @@ int commander_thread_main(int argc, char *argv[])
 			}
 		}
 
+		//Get current timestamp
+		const hrt_abstime now = hrt_absolute_time();
 
-		hrt_abstime t1 = hrt_absolute_time();
+		//First time home position update
+		if (!status.condition_home_position_valid) {
+			commander_set_home_position(home_pub, home, local_position, global_position);
+		}
+
+		/* update home position on arming if at least 2s from commander start spent to avoid setting home on in-air restart */
+		else if (arming_state_changed && armed.armed && !was_armed && now > commander_boot_timestamp + 2000000) {
+			commander_set_home_position(home_pub, home, local_position, global_position);
+		}
 
 		/* print new state */
 		if (arming_state_changed) {
 			status_changed = true;
 			mavlink_log_info(mavlink_fd, "[cmd] arming state: %s", arming_states_str[status.arming_state]);
-
-			/* update home position on arming if at least 2s from commander start spent to avoid setting home on in-air restart */
-			if (armed.armed && !was_armed && hrt_absolute_time() > start_time + 2000000 && status.condition_global_position_valid &&
-			    (global_position.eph < eph_threshold) && (global_position.epv < epv_threshold)) {
-
-				// TODO remove code duplication
-				home.lat = global_position.lat;
-				home.lon = global_position.lon;
-				home.alt = global_position.alt;
-
-				home.x = local_position.x;
-				home.y = local_position.y;
-				home.z = local_position.z;
-
-				warnx("home: lat = %.7f, lon = %.7f, alt = %.2f ", home.lat, home.lon, (double)home.alt);
-				mavlink_log_info(mavlink_fd, "home: %.7f, %.7f, %.2f", home.lat, home.lon, (double)home.alt);
-
-				/* announce new home position */
-				if (home_pub > 0) {
-					orb_publish(ORB_ID(home_position), home_pub, &home);
-
-				} else {
-					home_pub = orb_advertise(ORB_ID(home_position), &home);
-				}
-
-				/* mark home position as set */
-				status.condition_home_position_valid = true;
-			}
-
 			arming_state_changed = false;
 		}
 
@@ -1849,11 +1935,14 @@ int commander_thread_main(int argc, char *argv[])
 
 		if (status.failsafe != failsafe_old) {
 			status_changed = true;
+
 			if (status.failsafe) {
 				mavlink_log_critical(mavlink_fd, "failsafe mode on");
+
 			} else {
 				mavlink_log_critical(mavlink_fd, "failsafe mode off");
 			}
+
 			failsafe_old = status.failsafe;
 		}
 
@@ -1866,13 +1955,13 @@ int commander_thread_main(int argc, char *argv[])
 		/* publish states (armed, control mode, vehicle status) at least with 5 Hz */
 		if (counter % (200000 / COMMANDER_MONITORING_INTERVAL) == 0 || status_changed) {
 			set_control_mode();
-			control_mode.timestamp = t1;
+			control_mode.timestamp = now;
 			orb_publish(ORB_ID(vehicle_control_mode), control_mode_pub, &control_mode);
 
-			status.timestamp = t1;
+			status.timestamp = now;
 			orb_publish(ORB_ID(vehicle_status), status_pub, &status);
 
-			armed.timestamp = t1;
+			armed.timestamp = now;
 			orb_publish(ORB_ID(actuator_armed), armed_pub, &armed);
 		}
 
@@ -1897,6 +1986,12 @@ int commander_thread_main(int argc, char *argv[])
 
 		/* reset arm_tune_played when disarmed */
 		if (!armed.armed || (safety.safety_switch_available && !safety.safety_off)) {
+
+			//Notify the user that it is safe to approach the vehicle
+			if (arm_tune_played) {
+				tune_neutral(true);
+			}
+
 			arm_tune_played = false;
 		}
 
@@ -2183,8 +2278,7 @@ set_control_mode()
 {
 	/* set vehicle_control_mode according to set_navigation_state */
 	control_mode.flag_armed = armed.armed;
-	/* TODO: check this */
-	control_mode.flag_external_manual_override_ok = !status.is_rotary_wing;
+	control_mode.flag_external_manual_override_ok = (!status.is_rotary_wing && !status.is_vtol);
 	control_mode.flag_system_hil_enabled = status.hil_state == HIL_STATE_ON;
 	control_mode.flag_control_offboard_enabled = false;
 
@@ -2192,8 +2286,8 @@ set_control_mode()
 	case NAVIGATION_STATE_MANUAL:
 		control_mode.flag_control_manual_enabled = true;
 		control_mode.flag_control_auto_enabled = false;
-		control_mode.flag_control_rates_enabled = status.is_rotary_wing;
-		control_mode.flag_control_attitude_enabled = status.is_rotary_wing;
+		control_mode.flag_control_rates_enabled = (status.is_rotary_wing || status.vtol_fw_permanent_stab);
+		control_mode.flag_control_attitude_enabled = (status.is_rotary_wing || status.vtol_fw_permanent_stab);
 		control_mode.flag_control_altitude_enabled = false;
 		control_mode.flag_control_climb_rate_enabled = false;
 		control_mode.flag_control_position_enabled = false;
@@ -2361,6 +2455,7 @@ set_control_mode()
 			control_mode.flag_control_position_enabled = false;
 			control_mode.flag_control_velocity_enabled = false;
 		}
+
 		break;
 
 	default:
@@ -2399,7 +2494,7 @@ void answer_command(struct vehicle_command_s &cmd, enum VEHICLE_CMD_RESULT resul
 {
 	switch (result) {
 	case VEHICLE_CMD_RESULT_ACCEPTED:
-		tune_positive(true);
+			tune_positive(true);
 		break;
 
 	case VEHICLE_CMD_RESULT_DENIED:
diff --git a/src/modules/commander/commander_tests/state_machine_helper_test.cpp b/src/modules/commander/commander_tests/state_machine_helper_test.cpp
index 874090e93..3cfa8b4c6 100644
--- a/src/modules/commander/commander_tests/state_machine_helper_test.cpp
+++ b/src/modules/commander/commander_tests/state_machine_helper_test.cpp
@@ -280,6 +280,8 @@ bool StateMachineHelperTest::armingStateTransitionTest(void)
         status.arming_state = test->current_state.arming_state;
         status.condition_system_sensors_initialized = test->condition_system_sensors_initialized;
         status.hil_state = test->hil_state;
+        // The power status of the test unit is not relevant for the unit test
+        status.circuit_breaker_engaged_power_check = true;
         safety.safety_switch_available = test->safety_switch_available;
         safety.safety_off = test->safety_off;
         armed.armed = test->current_state.armed;
diff --git a/src/modules/commander/gyro_calibration.cpp b/src/modules/commander/gyro_calibration.cpp
index 8ab14dd52..8410297ef 100644
--- a/src/modules/commander/gyro_calibration.cpp
+++ b/src/modules/commander/gyro_calibration.cpp
@@ -62,6 +62,7 @@ static const char *sensor_name = "gyro";
 
 int do_gyro_calibration(int mavlink_fd)
 {
+	int32_t device_id;
 	mavlink_log_info(mavlink_fd, CAL_STARTED_MSG, sensor_name);
 	mavlink_log_info(mavlink_fd, "HOLD STILL");
 
@@ -81,6 +82,9 @@ int do_gyro_calibration(int mavlink_fd)
 
 	/* reset all offsets to zero and all scales to one */
 	int fd = open(GYRO_DEVICE_PATH, 0);
+
+	device_id = ioctl(fd, DEVIOCGDEVICEID, 0);
+
 	res = ioctl(fd, GYROIOCSSCALE, (long unsigned int)&gyro_scale);
 	close(fd);
 
@@ -95,7 +99,7 @@ int do_gyro_calibration(int mavlink_fd)
 		unsigned poll_errcount = 0;
 
 		/* subscribe to gyro sensor topic */
-		int sub_sensor_gyro = orb_subscribe(ORB_ID(sensor_gyro0));
+		int sub_sensor_gyro = orb_subscribe_multi(ORB_ID(sensor_gyro), 0);
 		struct gyro_report gyro_report;
 
 		while (calibration_counter < calibration_count) {
@@ -107,7 +111,7 @@ int do_gyro_calibration(int mavlink_fd)
 			int poll_ret = poll(fds, 1, 1000);
 
 			if (poll_ret > 0) {
-				orb_copy(ORB_ID(sensor_gyro0), sub_sensor_gyro, &gyro_report);
+				orb_copy(ORB_ID(sensor_gyro), sub_sensor_gyro, &gyro_report);
 				gyro_scale.x_offset += gyro_report.x;
 				gyro_scale.y_offset += gyro_report.y;
 				gyro_scale.z_offset += gyro_report.z;
@@ -277,6 +281,9 @@ int do_gyro_calibration(int mavlink_fd)
 			mavlink_log_critical(mavlink_fd, "ERROR: failed to set scale params");
 			res = ERROR;
 		}
+		if (param_set(param_find("SENS_GYRO_ID"), &(device_id))) {
+				res = ERROR;
+			}
 	}
 
 	if (res == OK) {
diff --git a/src/modules/commander/mag_calibration.cpp b/src/modules/commander/mag_calibration.cpp
index 7be8de9c6..2afb9a440 100644
--- a/src/modules/commander/mag_calibration.cpp
+++ b/src/modules/commander/mag_calibration.cpp
@@ -65,6 +65,7 @@ static const char *sensor_name = "mag";
 
 int do_mag_calibration(int mavlink_fd)
 {
+	int32_t device_id;
 	mavlink_log_info(mavlink_fd, CAL_STARTED_MSG, sensor_name);
 	mavlink_log_info(mavlink_fd, "don't move system");
 
@@ -88,6 +89,9 @@ int do_mag_calibration(int mavlink_fd)
 
 	/* erase old calibration */
 	int fd = open(MAG_DEVICE_PATH, O_RDONLY);
+
+	device_id = ioctl(fd, DEVIOCGDEVICEID, 0);
+
 	res = ioctl(fd, MAGIOCSSCALE, (long unsigned int)&mscale_null);
 
 	if (res != OK) {
@@ -145,55 +149,61 @@ int do_mag_calibration(int mavlink_fd)
 	}
 
 	if (res == OK) {
-		int sub_mag = orb_subscribe(ORB_ID(sensor_mag0));
-		struct mag_report mag;
+		int sub_mag = orb_subscribe_multi(ORB_ID(sensor_mag), 0);
+
+		if (sub_mag < 0) {
+			mavlink_log_critical(mavlink_fd, "No mag found, abort");
+			res = ERROR;
+		}  else {
+			struct mag_report mag;
+
+			/* limit update rate to get equally spaced measurements over time (in ms) */
+			orb_set_interval(sub_mag, (calibration_interval / 1000) / calibration_maxcount);
 
-		/* limit update rate to get equally spaced measurements over time (in ms) */
-		orb_set_interval(sub_mag, (calibration_interval / 1000) / calibration_maxcount);
+			/* calibrate offsets */
+			uint64_t calibration_deadline = hrt_absolute_time() + calibration_interval;
+			unsigned poll_errcount = 0;
 
-		/* calibrate offsets */
-		uint64_t calibration_deadline = hrt_absolute_time() + calibration_interval;
-		unsigned poll_errcount = 0;
+			mavlink_log_info(mavlink_fd, "Turn on all sides: front/back,left/right,up/down");
 
-		mavlink_log_info(mavlink_fd, "Turn on all sides: front/back,left/right,up/down");
+			calibration_counter = 0U;
 
-		calibration_counter = 0;
+			while (hrt_absolute_time() < calibration_deadline &&
+			       calibration_counter < calibration_maxcount) {
 
-		while (hrt_absolute_time() < calibration_deadline &&
-		       calibration_counter < calibration_maxcount) {
+				/* wait blocking for new data */
+				struct pollfd fds[1];
+				fds[0].fd = sub_mag;
+				fds[0].events = POLLIN;
 
-			/* wait blocking for new data */
-			struct pollfd fds[1];
-			fds[0].fd = sub_mag;
-			fds[0].events = POLLIN;
+				int poll_ret = poll(fds, 1, 1000);
 
-			int poll_ret = poll(fds, 1, 1000);
+				if (poll_ret > 0) {
+					orb_copy(ORB_ID(sensor_mag), sub_mag, &mag);
 
-			if (poll_ret > 0) {
-				orb_copy(ORB_ID(sensor_mag0), sub_mag, &mag);
+					x[calibration_counter] = mag.x;
+					y[calibration_counter] = mag.y;
+					z[calibration_counter] = mag.z;
 
-				x[calibration_counter] = mag.x;
-				y[calibration_counter] = mag.y;
-				z[calibration_counter] = mag.z;
+					calibration_counter++;
 
-				calibration_counter++;
+					if (calibration_counter % (calibration_maxcount / 20) == 0) {
+						mavlink_log_info(mavlink_fd, CAL_PROGRESS_MSG, sensor_name, 20 + (calibration_counter * 50) / calibration_maxcount);
+					}
 
-				if (calibration_counter % (calibration_maxcount / 20) == 0) {
-					mavlink_log_info(mavlink_fd, CAL_PROGRESS_MSG, sensor_name, 20 + (calibration_counter * 50) / calibration_maxcount);
+				} else {
+					poll_errcount++;
 				}
 
-			} else {
-				poll_errcount++;
+				if (poll_errcount > 1000) {
+					mavlink_log_critical(mavlink_fd, CAL_FAILED_SENSOR_MSG);
+					res = ERROR;
+					break;
+				}
 			}
 
-			if (poll_errcount > 1000) {
-				mavlink_log_critical(mavlink_fd, CAL_FAILED_SENSOR_MSG);
-				res = ERROR;
-				break;
-			}
+			close(sub_mag);
 		}
-
-		close(sub_mag);
 	}
 
 	float sphere_x;
@@ -201,7 +211,7 @@ int do_mag_calibration(int mavlink_fd)
 	float sphere_z;
 	float sphere_radius;
 
-	if (res == OK) {
+	if (res == OK && calibration_counter > (calibration_maxcount / 2)) {
 
 		/* sphere fit */
 		mavlink_log_info(mavlink_fd, CAL_PROGRESS_MSG, sensor_name, 70);
@@ -253,6 +263,9 @@ int do_mag_calibration(int mavlink_fd)
 
 		if (res == OK) {
 			/* set parameters */
+			if (param_set(param_find("SENS_MAG_ID"), &(device_id))) {
+				res = ERROR;
+			}
 			if (param_set(param_find("SENS_MAG_XOFF"), &(mscale.x_offset))) {
 				res = ERROR;
 			}
diff --git a/src/modules/commander/module.mk b/src/modules/commander/module.mk
index 27ca5c182..0e2a5356b 100644
--- a/src/modules/commander/module.mk
+++ b/src/modules/commander/module.mk
@@ -51,3 +51,6 @@ SRCS		 	= commander.cpp \
 MODULE_STACKSIZE = 1200
 
 MAXOPTIMIZATION	 = -Os
+
+EXTRACXXFLAGS = -Wframe-larger-than=2000
+
diff --git a/src/modules/commander/state_machine_helper.cpp b/src/modules/commander/state_machine_helper.cpp
index e37019d02..465f9cdc5 100644
--- a/src/modules/commander/state_machine_helper.cpp
+++ b/src/modules/commander/state_machine_helper.cpp
@@ -706,7 +706,7 @@ int prearm_check(const struct vehicle_status_s *status, const int mavlink_fd)
 		}
 
 		if (fabsf(airspeed.indicated_airspeed_m_s > 6.0f)) {
-			mavlink_log_critical(mavlink_fd, "AIRSPEED WARNING: WIND OR CALIBRATION MISSING");
+			mavlink_log_critical(mavlink_fd, "AIRSPEED WARNING: WIND OR CALIBRATION ISSUE");
 			// XXX do not make this fatal yet
 		}
 	}
diff --git a/src/modules/dataman/dataman.c b/src/modules/dataman/dataman.c
index b2355d4d8..68bf12024 100644
--- a/src/modules/dataman/dataman.c
+++ b/src/modules/dataman/dataman.c
@@ -629,9 +629,6 @@ task_main(int argc, char *argv[])
 {
 	work_q_item_t *work;
 
-	/* inform about start */
-	warnx("Initializing..");
-
 	/* Initialize global variables */
 	g_key_offsets[0] = 0;
 
@@ -694,16 +691,15 @@ task_main(int argc, char *argv[])
 		if (sys_restart_val == DM_INIT_REASON_POWER_ON) {
 			warnx("Power on restart");
 			_restart(DM_INIT_REASON_POWER_ON);
-		}
-		else if (sys_restart_val == DM_INIT_REASON_IN_FLIGHT) {
+		} else if (sys_restart_val == DM_INIT_REASON_IN_FLIGHT) {
 			warnx("In flight restart");
 			_restart(DM_INIT_REASON_IN_FLIGHT);
-		}
-		else
+		} else {
 			warnx("Unknown restart");
-	}
-	else
+		}
+	} else {
 		warnx("Unknown restart");
+	}
 
 	/* We use two file descriptors, one for the caller context and one for the worker thread */
 	/* They are actually the same but we need to some way to reject caller request while the */
@@ -797,7 +793,7 @@ start(void)
 	sem_init(&g_init_sema, 1, 0);
 
 	/* start the worker thread */
-	if ((task = task_spawn_cmd("dataman", SCHED_DEFAULT, SCHED_PRIORITY_DEFAULT, 2000, task_main, NULL)) <= 0) {
+	if ((task = task_spawn_cmd("dataman", SCHED_DEFAULT, SCHED_PRIORITY_DEFAULT, 1800, task_main, NULL)) <= 0) {
 		warn("task start failed");
 		return -1;
 	}
diff --git a/src/modules/ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp b/src/modules/ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp
index e7805daa9..d0f5fb6f8 100644
--- a/src/modules/ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp
+++ b/src/modules/ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp
@@ -1,6 +1,6 @@
 /****************************************************************************
  *
- *   Copyright (c) 2013, 2014 PX4 Development Team. All rights reserved.
+ *   Copyright (c) 2013-2015 PX4 Development Team. All rights reserved.
  *
  * Redistribution and use in source and binary forms, with or without
  * modification, are permitted provided that the following conditions
@@ -82,11 +82,10 @@
 #include <systemlib/perf_counter.h>
 #include <systemlib/systemlib.h>
 #include <mathlib/mathlib.h>
+#include <mathlib/math/filter/LowPassFilter2p.hpp>
 #include <mavlink/mavlink_log.h>
 
-#include "estimator_23states.h"
-
-
+#include "estimator_22states.h"
 
 /**
  * estimator app start / stop handling function
@@ -101,7 +100,7 @@ __EXPORT uint64_t getMicros();
 
 static uint64_t IMUmsec = 0;
 static uint64_t IMUusec = 0;
-static const uint64_t FILTER_INIT_DELAY = 1 * 1000 * 1000;
+static const uint64_t FILTER_INIT_DELAY = 1 * 1000 * 1000; // units: microseconds
 
 uint32_t millis()
 {
@@ -222,8 +221,10 @@ private:
 	float						_baro_ref;		/**< barometer reference altitude */
 	float						_baro_ref_offset;	/**< offset between initial baro reference and GPS init baro altitude */
 	float						_baro_gps_offset;	/**< offset between baro altitude (at GPS init time) and GPS altitude */
+	hrt_abstime					_last_debug_print = 0;
 
 	perf_counter_t	_loop_perf;			///< loop performance counter
+	perf_counter_t	_loop_intvl;		///< loop rate counter
 	perf_counter_t	_perf_gyro;			///<local performance counter for gyro updates
 	perf_counter_t	_perf_mag;			///<local performance counter for mag updates
 	perf_counter_t	_perf_gps;			///<local performance counter for gps updates
@@ -290,10 +291,6 @@ private:
 
 	AttPosEKF					*_ekf;
 
-	float						_velocity_xy_filtered;
-	float						_velocity_z_filtered;
-	float						_airspeed_filtered;
-
 	/**
 	 * Update our local parameter cache.
 	 */
@@ -399,6 +396,7 @@ FixedwingEstimator::FixedwingEstimator() :
 
 /* performance counters */
 	_loop_perf(perf_alloc(PC_ELAPSED, "ekf_att_pos_estimator")),
+	_loop_intvl(perf_alloc(PC_INTERVAL, "ekf_att_pos_est_interval")),
 	_perf_gyro(perf_alloc(PC_INTERVAL, "ekf_att_pos_gyro_upd")),
 	_perf_mag(perf_alloc(PC_INTERVAL, "ekf_att_pos_mag_upd")),
 	_perf_gps(perf_alloc(PC_INTERVAL, "ekf_att_pos_gps_upd")),
@@ -422,10 +420,7 @@ FixedwingEstimator::FixedwingEstimator() :
 	_mavlink_fd(-1),
 	_parameters{},
 	_parameter_handles{},
-	_ekf(nullptr),
-	_velocity_xy_filtered(0.0f),
-	_velocity_z_filtered(0.0f),
-	_airspeed_filtered(0.0f)
+	_ekf(nullptr)
 {
 
 	_last_run = hrt_absolute_time();
@@ -724,7 +719,7 @@ FixedwingEstimator::task_main()
 	 * do subscriptions
 	 */
 	_distance_sub = orb_subscribe(ORB_ID(sensor_range_finder));
-	_baro_sub = orb_subscribe(ORB_ID(sensor_baro0));
+	_baro_sub = orb_subscribe_multi(ORB_ID(sensor_baro), 0);
 	_airspeed_sub = orb_subscribe(ORB_ID(airspeed));
 	_gps_sub = orb_subscribe(ORB_ID(vehicle_gps_position));
 	_vstatus_sub = orb_subscribe(ORB_ID(vehicle_status));
@@ -782,6 +777,11 @@ FixedwingEstimator::task_main()
 	_gps.vel_e_m_s = 0.0f;
 	_gps.vel_d_m_s = 0.0f;
 
+	// init lowpass filters for baro and gps altitude
+	float _gps_alt_filt = 0, _baro_alt_filt = 0;
+	float rc = 10.0f;	// RC time constant of 1st order LPF in seconds
+	hrt_abstime baro_last = 0;
+
 	_task_running = true;
 
 	while (!_task_should_exit) {
@@ -800,6 +800,7 @@ FixedwingEstimator::task_main()
 		}
 
 		perf_begin(_loop_perf);
+		perf_count(_loop_intvl);
 
 		/* only update parameters if they changed */
 		if (fds[0].revents & POLLIN) {
@@ -1055,6 +1056,11 @@ FixedwingEstimator::task_main()
 					_ekf->gpsLon = math::radians(_gps.lon / (double)1e7) - M_PI;
 					_ekf->gpsHgt = _gps.alt / 1e3f;
 
+					// update LPF
+					_gps_alt_filt += (gps_elapsed / (rc + gps_elapsed)) * (_ekf->gpsHgt - _gps_alt_filt);
+
+					//warnx("gps alt: %6.1f, interval: %6.3f", (double)_ekf->gpsHgt, (double)gps_elapsed);
+
 					// if (_gps.s_variance_m_s > 0.25f && _gps.s_variance_m_s < 100.0f * 100.0f) {
 					// 	_ekf->vneSigma = sqrtf(_gps.s_variance_m_s);
 					// } else {
@@ -1070,7 +1076,6 @@ FixedwingEstimator::task_main()
 					// warnx("vel: %8.4f pos: %8.4f", _gps.s_variance_m_s, _gps.p_variance_m);
 
 					newDataGps = true;
-
 					last_gps = _gps.timestamp_position;
 
 				}
@@ -1082,15 +1087,15 @@ FixedwingEstimator::task_main()
 
 			if (baro_updated) {
 
-				hrt_abstime baro_last = _baro.timestamp;
-
-				orb_copy(ORB_ID(sensor_baro0), _baro_sub, &_baro);
+				orb_copy(ORB_ID(sensor_baro), _baro_sub, &_baro);
 
 				float baro_elapsed = (_baro.timestamp - baro_last) / 1e6f;
+				baro_last = _baro.timestamp;
 
 				_ekf->updateDtHgtFilt(math::constrain(baro_elapsed, 0.001f, 0.1f));
 
 				_ekf->baroHgt = _baro.altitude;
+				_baro_alt_filt += (baro_elapsed/(rc + baro_elapsed)) * (_baro.altitude - _baro_alt_filt);
 
 				if (!_baro_init) {
 					_baro_ref = _baro.altitude;
@@ -1180,6 +1185,24 @@ FixedwingEstimator::task_main()
 
 				float initVelNED[3];
 
+				// maintain filtered baro and gps altitudes to calculate weather offset
+				// baro sample rate is ~70Hz and measurement bandwidth is high
+				// gps sample rate is 5Hz and altitude is assumed accurate when averaged over 30 seconds
+				// maintain heavily filtered values for both baro and gps altitude
+				// Assume the filtered output should be identical for both sensors
+				_baro_gps_offset = _baro_alt_filt - _gps_alt_filt;
+//				if (hrt_elapsed_time(&_last_debug_print) >= 5e6) {
+//					_last_debug_print = hrt_absolute_time();
+//					perf_print_counter(_perf_baro);
+//					perf_reset(_perf_baro);
+//					warnx("gpsoff: %5.1f, baro_alt_filt: %6.1f, gps_alt_filt: %6.1f, gpos.alt: %5.1f, lpos.z: %6.1f",
+//							(double)_baro_gps_offset,
+//							(double)_baro_alt_filt,
+//							(double)_gps_alt_filt,
+//							(double)_global_pos.alt,
+//							(double)_local_pos.z);
+//				}
+
 				/* Initialize the filter first */
 				if (!_gps_initialized && _gps.fix_type > 2 && _gps.eph < _parameters.pos_stddev_threshold && _gps.epv < _parameters.pos_stddev_threshold) {
 
@@ -1193,9 +1216,13 @@ FixedwingEstimator::task_main()
 					initVelNED[2] = _gps.vel_d_m_s;
 
 					// Set up height correctly
-					orb_copy(ORB_ID(sensor_baro0), _baro_sub, &_baro);
+					orb_copy(ORB_ID(sensor_baro), _baro_sub, &_baro);
 					_baro_ref_offset = _ekf->states[9]; // this should become zero in the local frame
-					_baro_gps_offset = _baro.altitude - gps_alt;
+
+					// init filtered gps and baro altitudes
+					_gps_alt_filt = gps_alt;
+					_baro_alt_filt = _baro.altitude;
+
 					_ekf->baroHgt = _baro.altitude;
 					_ekf->hgtMea = 1.0f * (_ekf->baroHgt - (_baro_ref));
 
@@ -1371,10 +1398,15 @@ FixedwingEstimator::task_main()
 					}
 
 					if (newRangeData) {
-						_ekf->fuseRngData = true;
-						_ekf->useRangeFinder = true;
-						_ekf->RecallStates(_ekf->statesAtRngTime, (IMUmsec - 100.0f));
-						_ekf->GroundEKF();
+
+						if (_ekf->Tnb.z.z > 0.9f) {
+							// _ekf->rngMea is set in sensor readout already
+							_ekf->fuseRngData = true;
+							_ekf->fuseOptFlowData = false;
+							_ekf->RecallStates(_ekf->statesAtRngTime, (IMUmsec - 100.0f));
+							_ekf->OpticalFlowEKF();
+							_ekf->fuseRngData = false;
+						}
 					}
 
 
@@ -1434,22 +1466,6 @@ FixedwingEstimator::task_main()
 						_local_pos.v_z_valid = true;
 						_local_pos.xy_global = true;
 
-						_velocity_xy_filtered = 0.95f*_velocity_xy_filtered + 0.05f*sqrtf(_local_pos.vx*_local_pos.vx + _local_pos.vy*_local_pos.vy);
-						_velocity_z_filtered = 0.95f*_velocity_z_filtered + 0.05f*fabsf(_local_pos.vz);
-						_airspeed_filtered = 0.95f*_airspeed_filtered + 0.05f*_airspeed.true_airspeed_m_s;
-
-
-						/* crude land detector for fixedwing only,
-						* TODO: adapt so that it works for both, maybe move to another location
-						*/
-						if (_velocity_xy_filtered < 5
-							&& _velocity_z_filtered < 10
-							&& _airspeed_filtered < 10) {
-							_local_pos.landed = true;
-						} else {
-							_local_pos.landed = false;
-						}
-
 						_local_pos.z_global = false;
 						_local_pos.yaw = _att.yaw;
 
@@ -1470,7 +1486,7 @@ FixedwingEstimator::task_main()
 							map_projection_reproject(&_pos_ref, _local_pos.x, _local_pos.y, &est_lat, &est_lon);
 							_global_pos.lat = est_lat;
 							_global_pos.lon = est_lon;
-							_global_pos.time_gps_usec = _gps.time_gps_usec;
+							_global_pos.time_utc_usec = _gps.time_utc_usec;
 							_global_pos.eph = _gps.eph;
 							_global_pos.epv = _gps.epv;
 						}
@@ -1514,8 +1530,8 @@ FixedwingEstimator::task_main()
 
 						if (hrt_elapsed_time(&_wind.timestamp) > 99000) {
 							_wind.timestamp = _global_pos.timestamp;
-							_wind.windspeed_north = _ekf->windSpdFiltNorth;
-							_wind.windspeed_east = _ekf->windSpdFiltEast;
+							_wind.windspeed_north = _ekf->states[14];
+							_wind.windspeed_east = _ekf->states[15];
 							// XXX we need to do something smart about the covariance here
 							// but we default to the estimate covariance for now
 							_wind.covariance_north = _ekf->P[14][14];
diff --git a/src/modules/ekf_att_pos_estimator/estimator_21states.cpp b/src/modules/ekf_att_pos_estimator/estimator_21states.cpp
deleted file mode 100644
index 67bfec4ea..000000000
--- a/src/modules/ekf_att_pos_estimator/estimator_21states.cpp
+++ /dev/null
@@ -1,2142 +0,0 @@
-#include "estimator_21states.h"
-
-#include <string.h>
-
-AttPosEKF::AttPosEKF() :
-    fusionModeGPS(0),
-    covSkipCount(0),
-    EAS2TAS(1.0f),
-    statesInitialised(false),
-    fuseVelData(false),
-    fusePosData(false),
-    fuseHgtData(false),
-    fuseMagData(false),
-    fuseVtasData(false),
-    onGround(true),
-    staticMode(true),
-    useAirspeed(true),
-    useCompass(true),
-    numericalProtection(true),
-    storeIndex(0),
-    magDeclination(0.0f)
-{
-    InitialiseParameters();
-}
-
-AttPosEKF::~AttPosEKF()
-{
-}
-
-void AttPosEKF::UpdateStrapdownEquationsNED()
-{
-    Vector3f delVelNav;
-    float q00;
-    float q11;
-    float q22;
-    float q33;
-    float q01;
-    float q02;
-    float q03;
-    float q12;
-    float q13;
-    float q23;
-    Mat3f Tbn;
-    Mat3f Tnb;
-    float rotationMag;
-    float qUpdated[4];
-    float quatMag;
-    double deltaQuat[4];
-    const Vector3f gravityNED = {0.0,0.0,GRAVITY_MSS};
-
-// Remove sensor bias errors
-    correctedDelAng.x = dAngIMU.x - states[10];
-    correctedDelAng.y = dAngIMU.y - states[11];
-    correctedDelAng.z = dAngIMU.z - states[12];
-    dVelIMU.x = dVelIMU.x;
-    dVelIMU.y = dVelIMU.y;
-    dVelIMU.z = dVelIMU.z;
-
-// Save current measurements
-    Vector3f  prevDelAng = correctedDelAng;
-
-// Apply corrections for earths rotation rate and coning errors
-// * and + operators have been overloaded
-    correctedDelAng   = correctedDelAng - Tnb*earthRateNED*dtIMU + 8.333333333333333e-2f*(prevDelAng % correctedDelAng);
-// Convert the rotation vector to its equivalent quaternion
-    rotationMag = correctedDelAng.length();
-    if (rotationMag < 1e-12f)
-    {
-        deltaQuat[0] = 1.0;
-        deltaQuat[1] = 0.0;
-        deltaQuat[2] = 0.0;
-        deltaQuat[3] = 0.0;
-    }
-    else
-    {
-        deltaQuat[0] = cos(0.5f*rotationMag);
-        double rotScaler = (sin(0.5f*rotationMag))/rotationMag;
-        deltaQuat[1] = correctedDelAng.x*rotScaler;
-        deltaQuat[2] = correctedDelAng.y*rotScaler;
-        deltaQuat[3] = correctedDelAng.z*rotScaler;
-    }
-
-// Update the quaternions by rotating from the previous attitude through
-// the delta angle rotation quaternion
-    qUpdated[0] = states[0]*deltaQuat[0] - states[1]*deltaQuat[1] - states[2]*deltaQuat[2] - states[3]*deltaQuat[3];
-    qUpdated[1] = states[0]*deltaQuat[1] + states[1]*deltaQuat[0] + states[2]*deltaQuat[3] - states[3]*deltaQuat[2];
-    qUpdated[2] = states[0]*deltaQuat[2] + states[2]*deltaQuat[0] + states[3]*deltaQuat[1] - states[1]*deltaQuat[3];
-    qUpdated[3] = states[0]*deltaQuat[3] + states[3]*deltaQuat[0] + states[1]*deltaQuat[2] - states[2]*deltaQuat[1];
-
-// Normalise the quaternions and update the quaternion states
-    quatMag = sqrtf(sq(qUpdated[0]) + sq(qUpdated[1]) + sq(qUpdated[2]) + sq(qUpdated[3]));
-    if (quatMag > 1e-16f)
-    {
-        float quatMagInv = 1.0f/quatMag;
-        states[0] = quatMagInv*qUpdated[0];
-        states[1] = quatMagInv*qUpdated[1];
-        states[2] = quatMagInv*qUpdated[2];
-        states[3] = quatMagInv*qUpdated[3];
-    }
-
-// Calculate the body to nav cosine matrix
-    q00 = sq(states[0]);
-    q11 = sq(states[1]);
-    q22 = sq(states[2]);
-    q33 = sq(states[3]);
-    q01 =  states[0]*states[1];
-    q02 =  states[0]*states[2];
-    q03 =  states[0]*states[3];
-    q12 =  states[1]*states[2];
-    q13 =  states[1]*states[3];
-    q23 =  states[2]*states[3];
-
-    Tbn.x.x = q00 + q11 - q22 - q33;
-    Tbn.y.y = q00 - q11 + q22 - q33;
-    Tbn.z.z = q00 - q11 - q22 + q33;
-    Tbn.x.y = 2*(q12 - q03);
-    Tbn.x.z = 2*(q13 + q02);
-    Tbn.y.x = 2*(q12 + q03);
-    Tbn.y.z = 2*(q23 - q01);
-    Tbn.z.x = 2*(q13 - q02);
-    Tbn.z.y = 2*(q23 + q01);
-
-    Tnb = Tbn.transpose();
-
-// transform body delta velocities to delta velocities in the nav frame
-// * and + operators have been overloaded
-    //delVelNav = Tbn*dVelIMU + gravityNED*dtIMU;
-    delVelNav.x = Tbn.x.x*dVelIMU.x + Tbn.x.y*dVelIMU.y + Tbn.x.z*dVelIMU.z + gravityNED.x*dtIMU;
-    delVelNav.y = Tbn.y.x*dVelIMU.x + Tbn.y.y*dVelIMU.y + Tbn.y.z*dVelIMU.z + gravityNED.y*dtIMU;
-    delVelNav.z = Tbn.z.x*dVelIMU.x + Tbn.z.y*dVelIMU.y + Tbn.z.z*dVelIMU.z + gravityNED.z*dtIMU;
-
-// calculate the magnitude of the nav acceleration (required for GPS
-// variance estimation)
-    accNavMag = delVelNav.length()/dtIMU;
-
-// If calculating position save previous velocity
-    float lastVelocity[3];
-    lastVelocity[0] = states[4];
-    lastVelocity[1] = states[5];
-    lastVelocity[2] = states[6];
-
-// Sum delta velocities to get velocity
-    states[4] = states[4] + delVelNav.x;
-    states[5] = states[5] + delVelNav.y;
-    states[6] = states[6] + delVelNav.z;
-
-// If calculating postions, do a trapezoidal integration for position
-    states[7] = states[7] + 0.5f*(states[4] + lastVelocity[0])*dtIMU;
-    states[8] = states[8] + 0.5f*(states[5] + lastVelocity[1])*dtIMU;
-    states[9] = states[9] + 0.5f*(states[6] + lastVelocity[2])*dtIMU;
-
-    // Constrain states (to protect against filter divergence)
-    //ConstrainStates();
-}
-
-void AttPosEKF::CovariancePrediction(float dt)
-{
-    // scalars
-    float daxCov;
-    float dayCov;
-    float dazCov;
-    float dvxCov;
-    float dvyCov;
-    float dvzCov;
-    float dvx;
-    float dvy;
-    float dvz;
-    float dax;
-    float day;
-    float daz;
-    float q0;
-    float q1;
-    float q2;
-    float q3;
-    float dax_b;
-    float day_b;
-    float daz_b;
-
-    // arrays
-    float processNoise[21];
-    float SF[14];
-    float SG[8];
-    float SQ[11];
-    float SPP[13] = {0};
-    float nextP[21][21];
-
-    // calculate covariance prediction process noise
-    windVelSigma = dt*0.1f;
-    dAngBiasSigma = dt*5.0e-7f;
-    magEarthSigma = dt*3.0e-4f;
-    magBodySigma  = dt*3.0e-4f;
-    for (uint8_t i= 0; i<=9; i++) processNoise[i]  = 1.0e-9f;
-    for (uint8_t i=10; i<=12; i++) processNoise[i] = dAngBiasSigma;
-    if (onGround) processNoise[12] = dAngBiasSigma * yawVarScale;
-    for (uint8_t i=13; i<=14; i++) processNoise[i] = windVelSigma;
-    for (uint8_t i=15; i<=17; i++) processNoise[i] = magEarthSigma;
-    for (uint8_t i=18; i<=20; i++) processNoise[i] = magBodySigma;
-    for (uint8_t i= 0; i<=20; i++) processNoise[i] = sq(processNoise[i]);
-
-    // set variables used to calculate covariance growth
-    dvx = summedDelVel.x;
-    dvy = summedDelVel.y;
-    dvz = summedDelVel.z;
-    dax = summedDelAng.x;
-    day = summedDelAng.y;
-    daz = summedDelAng.z;
-    q0 = states[0];
-    q1 = states[1];
-    q2 = states[2];
-    q3 = states[3];
-    dax_b = states[10];
-    day_b = states[11];
-    daz_b = states[12];
-    daxCov = sq(dt*1.4544411e-2f);
-    dayCov = sq(dt*1.4544411e-2f);
-    dazCov = sq(dt*1.4544411e-2f);
-    if (onGround) dazCov = dazCov * sq(yawVarScale);
-    dvxCov = sq(dt*0.5f);
-    dvyCov = sq(dt*0.5f);
-    dvzCov = sq(dt*0.5f);
-
-    // Predicted covariance calculation
-    SF[0] = 2*dvx*q1 + 2*dvy*q2 + 2*dvz*q3;
-    SF[1] = 2*dvx*q3 + 2*dvy*q0 - 2*dvz*q1;
-    SF[2] = 2*dvx*q0 - 2*dvy*q3 + 2*dvz*q2;
-    SF[3] = day/2 - day_b/2;
-    SF[4] = daz/2 - daz_b/2;
-    SF[5] = dax/2 - dax_b/2;
-    SF[6] = dax_b/2 - dax/2;
-    SF[7] = daz_b/2 - daz/2;
-    SF[8] = day_b/2 - day/2;
-    SF[9] = q1/2;
-    SF[10] = q2/2;
-    SF[11] = q3/2;
-    SF[12] = 2*dvz*q0;
-    SF[13] = 2*dvy*q1;
-
-    SG[0] = q0/2;
-    SG[1] = sq(q3);
-    SG[2] = sq(q2);
-    SG[3] = sq(q1);
-    SG[4] = sq(q0);
-    SG[5] = 2*q2*q3;
-    SG[6] = 2*q1*q3;
-    SG[7] = 2*q1*q2;
-
-    SQ[0] = dvzCov*(SG[5] - 2*q0*q1)*(SG[1] - SG[2] - SG[3] + SG[4]) - dvyCov*(SG[5] + 2*q0*q1)*(SG[1] - SG[2] + SG[3] - SG[4]) + dvxCov*(SG[6] - 2*q0*q2)*(SG[7] + 2*q0*q3);
-    SQ[1] = dvzCov*(SG[6] + 2*q0*q2)*(SG[1] - SG[2] - SG[3] + SG[4]) - dvxCov*(SG[6] - 2*q0*q2)*(SG[1] + SG[2] - SG[3] - SG[4]) + dvyCov*(SG[5] + 2*q0*q1)*(SG[7] - 2*q0*q3);
-    SQ[2] = dvzCov*(SG[5] - 2*q0*q1)*(SG[6] + 2*q0*q2) - dvyCov*(SG[7] - 2*q0*q3)*(SG[1] - SG[2] + SG[3] - SG[4]) - dvxCov*(SG[7] + 2*q0*q3)*(SG[1] + SG[2] - SG[3] - SG[4]);
-    SQ[3] = (dayCov*q1*SG[0])/2 - (dazCov*q1*SG[0])/2 - (daxCov*q2*q3)/4;
-    SQ[4] = (dazCov*q2*SG[0])/2 - (daxCov*q2*SG[0])/2 - (dayCov*q1*q3)/4;
-    SQ[5] = (daxCov*q3*SG[0])/2 - (dayCov*q3*SG[0])/2 - (dazCov*q1*q2)/4;
-    SQ[6] = (daxCov*q1*q2)/4 - (dazCov*q3*SG[0])/2 - (dayCov*q1*q2)/4;
-    SQ[7] = (dazCov*q1*q3)/4 - (daxCov*q1*q3)/4 - (dayCov*q2*SG[0])/2;
-    SQ[8] = (dayCov*q2*q3)/4 - (daxCov*q1*SG[0])/2 - (dazCov*q2*q3)/4;
-    SQ[9] = sq(SG[0]);
-    SQ[10] = sq(q1);
-
-    SPP[0] = SF[12] + SF[13] - 2*dvx*q2;
-    SPP[1] = 2*dvx*q0 - 2*dvy*q3 + 2*dvz*q2;
-    SPP[2] = 2*dvx*q3 + 2*dvy*q0 - 2*dvz*q1;
-    SPP[3] = SF[11];
-    SPP[4] = SF[10];
-    SPP[5] = SF[9];
-    SPP[6] = SF[7];
-    SPP[7] = SF[8];
-
-    nextP[0][0] = P[0][0] + P[1][0]*SF[6] + P[2][0]*SPP[7] + P[3][0]*SPP[6] + P[10][0]*SPP[5] + P[11][0]*SPP[4] + P[12][0]*SPP[3] + (daxCov*SQ[10])/4 + SF[6]*(P[0][1] + P[1][1]*SF[6] + P[2][1]*SPP[7] + P[3][1]*SPP[6] + P[10][1]*SPP[5] + P[11][1]*SPP[4] + P[12][1]*SPP[3]) + SPP[7]*(P[0][2] + P[1][2]*SF[6] + P[2][2]*SPP[7] + P[3][2]*SPP[6] + P[10][2]*SPP[5] + P[11][2]*SPP[4] + P[12][2]*SPP[3]) + SPP[6]*(P[0][3] + P[1][3]*SF[6] + P[2][3]*SPP[7] + P[3][3]*SPP[6] + P[10][3]*SPP[5] + P[11][3]*SPP[4] + P[12][3]*SPP[3]) + SPP[5]*(P[0][10] + P[1][10]*SF[6] + P[2][10]*SPP[7] + P[3][10]*SPP[6] + P[10][10]*SPP[5] + P[11][10]*SPP[4] + P[12][10]*SPP[3]) + SPP[4]*(P[0][11] + P[1][11]*SF[6] + P[2][11]*SPP[7] + P[3][11]*SPP[6] + P[10][11]*SPP[5] + P[11][11]*SPP[4] + P[12][11]*SPP[3]) + SPP[3]*(P[0][12] + P[1][12]*SF[6] + P[2][12]*SPP[7] + P[3][12]*SPP[6] + P[10][12]*SPP[5] + P[11][12]*SPP[4] + P[12][12]*SPP[3]) + (dayCov*sq(q2))/4 + (dazCov*sq(q3))/4;
-    nextP[0][1] = P[0][1] + SQ[8] + P[1][1]*SF[6] + P[2][1]*SPP[7] + P[3][1]*SPP[6] + P[10][1]*SPP[5] + P[11][1]*SPP[4] + P[12][1]*SPP[3] + SF[5]*(P[0][0] + P[1][0]*SF[6] + P[2][0]*SPP[7] + P[3][0]*SPP[6] + P[10][0]*SPP[5] + P[11][0]*SPP[4] + P[12][0]*SPP[3]) + SF[4]*(P[0][2] + P[1][2]*SF[6] + P[2][2]*SPP[7] + P[3][2]*SPP[6] + P[10][2]*SPP[5] + P[11][2]*SPP[4] + P[12][2]*SPP[3]) + SPP[7]*(P[0][3] + P[1][3]*SF[6] + P[2][3]*SPP[7] + P[3][3]*SPP[6] + P[10][3]*SPP[5] + P[11][3]*SPP[4] + P[12][3]*SPP[3]) + SPP[3]*(P[0][11] + P[1][11]*SF[6] + P[2][11]*SPP[7] + P[3][11]*SPP[6] + P[10][11]*SPP[5] + P[11][11]*SPP[4] + P[12][11]*SPP[3]) - SPP[4]*(P[0][12] + P[1][12]*SF[6] + P[2][12]*SPP[7] + P[3][12]*SPP[6] + P[10][12]*SPP[5] + P[11][12]*SPP[4] + P[12][12]*SPP[3]) - (q0*(P[0][10] + P[1][10]*SF[6] + P[2][10]*SPP[7] + P[3][10]*SPP[6] + P[10][10]*SPP[5] + P[11][10]*SPP[4] + P[12][10]*SPP[3]))/2;
-    nextP[0][2] = P[0][2] + SQ[7] + P[1][2]*SF[6] + P[2][2]*SPP[7] + P[3][2]*SPP[6] + P[10][2]*SPP[5] + P[11][2]*SPP[4] + P[12][2]*SPP[3] + SF[3]*(P[0][0] + P[1][0]*SF[6] + P[2][0]*SPP[7] + P[3][0]*SPP[6] + P[10][0]*SPP[5] + P[11][0]*SPP[4] + P[12][0]*SPP[3]) + SF[5]*(P[0][3] + P[1][3]*SF[6] + P[2][3]*SPP[7] + P[3][3]*SPP[6] + P[10][3]*SPP[5] + P[11][3]*SPP[4] + P[12][3]*SPP[3]) + SPP[6]*(P[0][1] + P[1][1]*SF[6] + P[2][1]*SPP[7] + P[3][1]*SPP[6] + P[10][1]*SPP[5] + P[11][1]*SPP[4] + P[12][1]*SPP[3]) - SPP[3]*(P[0][10] + P[1][10]*SF[6] + P[2][10]*SPP[7] + P[3][10]*SPP[6] + P[10][10]*SPP[5] + P[11][10]*SPP[4] + P[12][10]*SPP[3]) + SPP[5]*(P[0][12] + P[1][12]*SF[6] + P[2][12]*SPP[7] + P[3][12]*SPP[6] + P[10][12]*SPP[5] + P[11][12]*SPP[4] + P[12][12]*SPP[3]) - (q0*(P[0][11] + P[1][11]*SF[6] + P[2][11]*SPP[7] + P[3][11]*SPP[6] + P[10][11]*SPP[5] + P[11][11]*SPP[4] + P[12][11]*SPP[3]))/2;
-    nextP[0][3] = P[0][3] + SQ[6] + P[1][3]*SF[6] + P[2][3]*SPP[7] + P[3][3]*SPP[6] + P[10][3]*SPP[5] + P[11][3]*SPP[4] + P[12][3]*SPP[3] + SF[4]*(P[0][0] + P[1][0]*SF[6] + P[2][0]*SPP[7] + P[3][0]*SPP[6] + P[10][0]*SPP[5] + P[11][0]*SPP[4] + P[12][0]*SPP[3]) + SF[3]*(P[0][1] + P[1][1]*SF[6] + P[2][1]*SPP[7] + P[3][1]*SPP[6] + P[10][1]*SPP[5] + P[11][1]*SPP[4] + P[12][1]*SPP[3]) + SF[6]*(P[0][2] + P[1][2]*SF[6] + P[2][2]*SPP[7] + P[3][2]*SPP[6] + P[10][2]*SPP[5] + P[11][2]*SPP[4] + P[12][2]*SPP[3]) + SPP[4]*(P[0][10] + P[1][10]*SF[6] + P[2][10]*SPP[7] + P[3][10]*SPP[6] + P[10][10]*SPP[5] + P[11][10]*SPP[4] + P[12][10]*SPP[3]) - SPP[5]*(P[0][11] + P[1][11]*SF[6] + P[2][11]*SPP[7] + P[3][11]*SPP[6] + P[10][11]*SPP[5] + P[11][11]*SPP[4] + P[12][11]*SPP[3]) - (q0*(P[0][12] + P[1][12]*SF[6] + P[2][12]*SPP[7] + P[3][12]*SPP[6] + P[10][12]*SPP[5] + P[11][12]*SPP[4] + P[12][12]*SPP[3]))/2;
-    nextP[0][4] = P[0][4] + P[1][4]*SF[6] + P[2][4]*SPP[7] + P[3][4]*SPP[6] + P[10][4]*SPP[5] + P[11][4]*SPP[4] + P[12][4]*SPP[3] + SF[2]*(P[0][0] + P[1][0]*SF[6] + P[2][0]*SPP[7] + P[3][0]*SPP[6] + P[10][0]*SPP[5] + P[11][0]*SPP[4] + P[12][0]*SPP[3]) + SF[0]*(P[0][1] + P[1][1]*SF[6] + P[2][1]*SPP[7] + P[3][1]*SPP[6] + P[10][1]*SPP[5] + P[11][1]*SPP[4] + P[12][1]*SPP[3]) + SPP[0]*(P[0][2] + P[1][2]*SF[6] + P[2][2]*SPP[7] + P[3][2]*SPP[6] + P[10][2]*SPP[5] + P[11][2]*SPP[4] + P[12][2]*SPP[3]) - SPP[2]*(P[0][3] + P[1][3]*SF[6] + P[2][3]*SPP[7] + P[3][3]*SPP[6] + P[10][3]*SPP[5] + P[11][3]*SPP[4] + P[12][3]*SPP[3]);
-    nextP[0][5] = P[0][5] + P[1][5]*SF[6] + P[2][5]*SPP[7] + P[3][5]*SPP[6] + P[10][5]*SPP[5] + P[11][5]*SPP[4] + P[12][5]*SPP[3] + SF[1]*(P[0][0] + P[1][0]*SF[6] + P[2][0]*SPP[7] + P[3][0]*SPP[6] + P[10][0]*SPP[5] + P[11][0]*SPP[4] + P[12][0]*SPP[3]) + SF[0]*(P[0][2] + P[1][2]*SF[6] + P[2][2]*SPP[7] + P[3][2]*SPP[6] + P[10][2]*SPP[5] + P[11][2]*SPP[4] + P[12][2]*SPP[3]) + SF[2]*(P[0][3] + P[1][3]*SF[6] + P[2][3]*SPP[7] + P[3][3]*SPP[6] + P[10][3]*SPP[5] + P[11][3]*SPP[4] + P[12][3]*SPP[3]) - SPP[0]*(P[0][1] + P[1][1]*SF[6] + P[2][1]*SPP[7] + P[3][1]*SPP[6] + P[10][1]*SPP[5] + P[11][1]*SPP[4] + P[12][1]*SPP[3]);
-    nextP[0][6] = P[0][6] + P[1][6]*SF[6] + P[2][6]*SPP[7] + P[3][6]*SPP[6] + P[10][6]*SPP[5] + P[11][6]*SPP[4] + P[12][6]*SPP[3] + SF[1]*(P[0][1] + P[1][1]*SF[6] + P[2][1]*SPP[7] + P[3][1]*SPP[6] + P[10][1]*SPP[5] + P[11][1]*SPP[4] + P[12][1]*SPP[3]) + SF[0]*(P[0][3] + P[1][3]*SF[6] + P[2][3]*SPP[7] + P[3][3]*SPP[6] + P[10][3]*SPP[5] + P[11][3]*SPP[4] + P[12][3]*SPP[3]) + SPP[0]*(P[0][0] + P[1][0]*SF[6] + P[2][0]*SPP[7] + P[3][0]*SPP[6] + P[10][0]*SPP[5] + P[11][0]*SPP[4] + P[12][0]*SPP[3]) - SPP[1]*(P[0][2] + P[1][2]*SF[6] + P[2][2]*SPP[7] + P[3][2]*SPP[6] + P[10][2]*SPP[5] + P[11][2]*SPP[4] + P[12][2]*SPP[3]);
-    nextP[0][7] = P[0][7] + P[1][7]*SF[6] + P[2][7]*SPP[7] + P[3][7]*SPP[6] + P[10][7]*SPP[5] + P[11][7]*SPP[4] + P[12][7]*SPP[3] + dt*(P[0][4] + P[1][4]*SF[6] + P[2][4]*SPP[7] + P[3][4]*SPP[6] + P[10][4]*SPP[5] + P[11][4]*SPP[4] + P[12][4]*SPP[3]);
-    nextP[0][8] = P[0][8] + P[1][8]*SF[6] + P[2][8]*SPP[7] + P[3][8]*SPP[6] + P[10][8]*SPP[5] + P[11][8]*SPP[4] + P[12][8]*SPP[3] + dt*(P[0][5] + P[1][5]*SF[6] + P[2][5]*SPP[7] + P[3][5]*SPP[6] + P[10][5]*SPP[5] + P[11][5]*SPP[4] + P[12][5]*SPP[3]);
-    nextP[0][9] = P[0][9] + P[1][9]*SF[6] + P[2][9]*SPP[7] + P[3][9]*SPP[6] + P[10][9]*SPP[5] + P[11][9]*SPP[4] + P[12][9]*SPP[3] + dt*(P[0][6] + P[1][6]*SF[6] + P[2][6]*SPP[7] + P[3][6]*SPP[6] + P[10][6]*SPP[5] + P[11][6]*SPP[4] + P[12][6]*SPP[3]);
-    nextP[0][10] = P[0][10] + P[1][10]*SF[6] + P[2][10]*SPP[7] + P[3][10]*SPP[6] + P[10][10]*SPP[5] + P[11][10]*SPP[4] + P[12][10]*SPP[3];
-    nextP[0][11] = P[0][11] + P[1][11]*SF[6] + P[2][11]*SPP[7] + P[3][11]*SPP[6] + P[10][11]*SPP[5] + P[11][11]*SPP[4] + P[12][11]*SPP[3];
-    nextP[0][12] = P[0][12] + P[1][12]*SF[6] + P[2][12]*SPP[7] + P[3][12]*SPP[6] + P[10][12]*SPP[5] + P[11][12]*SPP[4] + P[12][12]*SPP[3];
-    nextP[0][13] = P[0][13] + P[1][13]*SF[6] + P[2][13]*SPP[7] + P[3][13]*SPP[6] + P[10][13]*SPP[5] + P[11][13]*SPP[4] + P[12][13]*SPP[3];
-    nextP[0][14] = P[0][14] + P[1][14]*SF[6] + P[2][14]*SPP[7] + P[3][14]*SPP[6] + P[10][14]*SPP[5] + P[11][14]*SPP[4] + P[12][14]*SPP[3];
-    nextP[0][15] = P[0][15] + P[1][15]*SF[6] + P[2][15]*SPP[7] + P[3][15]*SPP[6] + P[10][15]*SPP[5] + P[11][15]*SPP[4] + P[12][15]*SPP[3];
-    nextP[0][16] = P[0][16] + P[1][16]*SF[6] + P[2][16]*SPP[7] + P[3][16]*SPP[6] + P[10][16]*SPP[5] + P[11][16]*SPP[4] + P[12][16]*SPP[3];
-    nextP[0][17] = P[0][17] + P[1][17]*SF[6] + P[2][17]*SPP[7] + P[3][17]*SPP[6] + P[10][17]*SPP[5] + P[11][17]*SPP[4] + P[12][17]*SPP[3];
-    nextP[0][18] = P[0][18] + P[1][18]*SF[6] + P[2][18]*SPP[7] + P[3][18]*SPP[6] + P[10][18]*SPP[5] + P[11][18]*SPP[4] + P[12][18]*SPP[3];
-    nextP[0][19] = P[0][19] + P[1][19]*SF[6] + P[2][19]*SPP[7] + P[3][19]*SPP[6] + P[10][19]*SPP[5] + P[11][19]*SPP[4] + P[12][19]*SPP[3];
-    nextP[0][20] = P[0][20] + P[1][20]*SF[6] + P[2][20]*SPP[7] + P[3][20]*SPP[6] + P[10][20]*SPP[5] + P[11][20]*SPP[4] + P[12][20]*SPP[3];
-    nextP[1][0] = P[1][0] + SQ[8] + P[0][0]*SF[5] + P[2][0]*SF[4] + P[3][0]*SPP[7] + P[11][0]*SPP[3] - P[12][0]*SPP[4] - (P[10][0]*q0)/2 + SF[6]*(P[1][1] + P[0][1]*SF[5] + P[2][1]*SF[4] + P[3][1]*SPP[7] + P[11][1]*SPP[3] - P[12][1]*SPP[4] - (P[10][1]*q0)/2) + SPP[7]*(P[1][2] + P[0][2]*SF[5] + P[2][2]*SF[4] + P[3][2]*SPP[7] + P[11][2]*SPP[3] - P[12][2]*SPP[4] - (P[10][2]*q0)/2) + SPP[6]*(P[1][3] + P[0][3]*SF[5] + P[2][3]*SF[4] + P[3][3]*SPP[7] + P[11][3]*SPP[3] - P[12][3]*SPP[4] - (P[10][3]*q0)/2) + SPP[5]*(P[1][10] + P[0][10]*SF[5] + P[2][10]*SF[4] + P[3][10]*SPP[7] + P[11][10]*SPP[3] - P[12][10]*SPP[4] - (P[10][10]*q0)/2) + SPP[4]*(P[1][11] + P[0][11]*SF[5] + P[2][11]*SF[4] + P[3][11]*SPP[7] + P[11][11]*SPP[3] - P[12][11]*SPP[4] - (P[10][11]*q0)/2) + SPP[3]*(P[1][12] + P[0][12]*SF[5] + P[2][12]*SF[4] + P[3][12]*SPP[7] + P[11][12]*SPP[3] - P[12][12]*SPP[4] - (P[10][12]*q0)/2);
-    nextP[1][1] = P[1][1] + P[0][1]*SF[5] + P[2][1]*SF[4] + P[3][1]*SPP[7] + P[11][1]*SPP[3] - P[12][1]*SPP[4] + daxCov*SQ[9] - (P[10][1]*q0)/2 + SF[5]*(P[1][0] + P[0][0]*SF[5] + P[2][0]*SF[4] + P[3][0]*SPP[7] + P[11][0]*SPP[3] - P[12][0]*SPP[4] - (P[10][0]*q0)/2) + SF[4]*(P[1][2] + P[0][2]*SF[5] + P[2][2]*SF[4] + P[3][2]*SPP[7] + P[11][2]*SPP[3] - P[12][2]*SPP[4] - (P[10][2]*q0)/2) + SPP[7]*(P[1][3] + P[0][3]*SF[5] + P[2][3]*SF[4] + P[3][3]*SPP[7] + P[11][3]*SPP[3] - P[12][3]*SPP[4] - (P[10][3]*q0)/2) + SPP[3]*(P[1][11] + P[0][11]*SF[5] + P[2][11]*SF[4] + P[3][11]*SPP[7] + P[11][11]*SPP[3] - P[12][11]*SPP[4] - (P[10][11]*q0)/2) - SPP[4]*(P[1][12] + P[0][12]*SF[5] + P[2][12]*SF[4] + P[3][12]*SPP[7] + P[11][12]*SPP[3] - P[12][12]*SPP[4] - (P[10][12]*q0)/2) + (dayCov*sq(q3))/4 + (dazCov*sq(q2))/4 - (q0*(P[1][10] + P[0][10]*SF[5] + P[2][10]*SF[4] + P[3][10]*SPP[7] + P[11][10]*SPP[3] - P[12][10]*SPP[4] - (P[10][10]*q0)/2))/2;
-    nextP[1][2] = P[1][2] + SQ[5] + P[0][2]*SF[5] + P[2][2]*SF[4] + P[3][2]*SPP[7] + P[11][2]*SPP[3] - P[12][2]*SPP[4] - (P[10][2]*q0)/2 + SF[3]*(P[1][0] + P[0][0]*SF[5] + P[2][0]*SF[4] + P[3][0]*SPP[7] + P[11][0]*SPP[3] - P[12][0]*SPP[4] - (P[10][0]*q0)/2) + SF[5]*(P[1][3] + P[0][3]*SF[5] + P[2][3]*SF[4] + P[3][3]*SPP[7] + P[11][3]*SPP[3] - P[12][3]*SPP[4] - (P[10][3]*q0)/2) + SPP[6]*(P[1][1] + P[0][1]*SF[5] + P[2][1]*SF[4] + P[3][1]*SPP[7] + P[11][1]*SPP[3] - P[12][1]*SPP[4] - (P[10][1]*q0)/2) - SPP[3]*(P[1][10] + P[0][10]*SF[5] + P[2][10]*SF[4] + P[3][10]*SPP[7] + P[11][10]*SPP[3] - P[12][10]*SPP[4] - (P[10][10]*q0)/2) + SPP[5]*(P[1][12] + P[0][12]*SF[5] + P[2][12]*SF[4] + P[3][12]*SPP[7] + P[11][12]*SPP[3] - P[12][12]*SPP[4] - (P[10][12]*q0)/2) - (q0*(P[1][11] + P[0][11]*SF[5] + P[2][11]*SF[4] + P[3][11]*SPP[7] + P[11][11]*SPP[3] - P[12][11]*SPP[4] - (P[10][11]*q0)/2))/2;
-    nextP[1][3] = P[1][3] + SQ[4] + P[0][3]*SF[5] + P[2][3]*SF[4] + P[3][3]*SPP[7] + P[11][3]*SPP[3] - P[12][3]*SPP[4] - (P[10][3]*q0)/2 + SF[4]*(P[1][0] + P[0][0]*SF[5] + P[2][0]*SF[4] + P[3][0]*SPP[7] + P[11][0]*SPP[3] - P[12][0]*SPP[4] - (P[10][0]*q0)/2) + SF[3]*(P[1][1] + P[0][1]*SF[5] + P[2][1]*SF[4] + P[3][1]*SPP[7] + P[11][1]*SPP[3] - P[12][1]*SPP[4] - (P[10][1]*q0)/2) + SF[6]*(P[1][2] + P[0][2]*SF[5] + P[2][2]*SF[4] + P[3][2]*SPP[7] + P[11][2]*SPP[3] - P[12][2]*SPP[4] - (P[10][2]*q0)/2) + SPP[4]*(P[1][10] + P[0][10]*SF[5] + P[2][10]*SF[4] + P[3][10]*SPP[7] + P[11][10]*SPP[3] - P[12][10]*SPP[4] - (P[10][10]*q0)/2) - SPP[5]*(P[1][11] + P[0][11]*SF[5] + P[2][11]*SF[4] + P[3][11]*SPP[7] + P[11][11]*SPP[3] - P[12][11]*SPP[4] - (P[10][11]*q0)/2) - (q0*(P[1][12] + P[0][12]*SF[5] + P[2][12]*SF[4] + P[3][12]*SPP[7] + P[11][12]*SPP[3] - P[12][12]*SPP[4] - (P[10][12]*q0)/2))/2;
-    nextP[1][4] = P[1][4] + P[0][4]*SF[5] + P[2][4]*SF[4] + P[3][4]*SPP[7] + P[11][4]*SPP[3] - P[12][4]*SPP[4] - (P[10][4]*q0)/2 + SF[2]*(P[1][0] + P[0][0]*SF[5] + P[2][0]*SF[4] + P[3][0]*SPP[7] + P[11][0]*SPP[3] - P[12][0]*SPP[4] - (P[10][0]*q0)/2) + SF[0]*(P[1][1] + P[0][1]*SF[5] + P[2][1]*SF[4] + P[3][1]*SPP[7] + P[11][1]*SPP[3] - P[12][1]*SPP[4] - (P[10][1]*q0)/2) + SPP[0]*(P[1][2] + P[0][2]*SF[5] + P[2][2]*SF[4] + P[3][2]*SPP[7] + P[11][2]*SPP[3] - P[12][2]*SPP[4] - (P[10][2]*q0)/2) - SPP[2]*(P[1][3] + P[0][3]*SF[5] + P[2][3]*SF[4] + P[3][3]*SPP[7] + P[11][3]*SPP[3] - P[12][3]*SPP[4] - (P[10][3]*q0)/2);
-    nextP[1][5] = P[1][5] + P[0][5]*SF[5] + P[2][5]*SF[4] + P[3][5]*SPP[7] + P[11][5]*SPP[3] - P[12][5]*SPP[4] - (P[10][5]*q0)/2 + SF[1]*(P[1][0] + P[0][0]*SF[5] + P[2][0]*SF[4] + P[3][0]*SPP[7] + P[11][0]*SPP[3] - P[12][0]*SPP[4] - (P[10][0]*q0)/2) + SF[0]*(P[1][2] + P[0][2]*SF[5] + P[2][2]*SF[4] + P[3][2]*SPP[7] + P[11][2]*SPP[3] - P[12][2]*SPP[4] - (P[10][2]*q0)/2) + SF[2]*(P[1][3] + P[0][3]*SF[5] + P[2][3]*SF[4] + P[3][3]*SPP[7] + P[11][3]*SPP[3] - P[12][3]*SPP[4] - (P[10][3]*q0)/2) - SPP[0]*(P[1][1] + P[0][1]*SF[5] + P[2][1]*SF[4] + P[3][1]*SPP[7] + P[11][1]*SPP[3] - P[12][1]*SPP[4] - (P[10][1]*q0)/2);
-    nextP[1][6] = P[1][6] + P[0][6]*SF[5] + P[2][6]*SF[4] + P[3][6]*SPP[7] + P[11][6]*SPP[3] - P[12][6]*SPP[4] - (P[10][6]*q0)/2 + SF[1]*(P[1][1] + P[0][1]*SF[5] + P[2][1]*SF[4] + P[3][1]*SPP[7] + P[11][1]*SPP[3] - P[12][1]*SPP[4] - (P[10][1]*q0)/2) + SF[0]*(P[1][3] + P[0][3]*SF[5] + P[2][3]*SF[4] + P[3][3]*SPP[7] + P[11][3]*SPP[3] - P[12][3]*SPP[4] - (P[10][3]*q0)/2) + SPP[0]*(P[1][0] + P[0][0]*SF[5] + P[2][0]*SF[4] + P[3][0]*SPP[7] + P[11][0]*SPP[3] - P[12][0]*SPP[4] - (P[10][0]*q0)/2) - SPP[1]*(P[1][2] + P[0][2]*SF[5] + P[2][2]*SF[4] + P[3][2]*SPP[7] + P[11][2]*SPP[3] - P[12][2]*SPP[4] - (P[10][2]*q0)/2);
-    nextP[1][7] = P[1][7] + P[0][7]*SF[5] + P[2][7]*SF[4] + P[3][7]*SPP[7] + P[11][7]*SPP[3] - P[12][7]*SPP[4] - (P[10][7]*q0)/2 + dt*(P[1][4] + P[0][4]*SF[5] + P[2][4]*SF[4] + P[3][4]*SPP[7] + P[11][4]*SPP[3] - P[12][4]*SPP[4] - (P[10][4]*q0)/2);
-    nextP[1][8] = P[1][8] + P[0][8]*SF[5] + P[2][8]*SF[4] + P[3][8]*SPP[7] + P[11][8]*SPP[3] - P[12][8]*SPP[4] - (P[10][8]*q0)/2 + dt*(P[1][5] + P[0][5]*SF[5] + P[2][5]*SF[4] + P[3][5]*SPP[7] + P[11][5]*SPP[3] - P[12][5]*SPP[4] - (P[10][5]*q0)/2);
-    nextP[1][9] = P[1][9] + P[0][9]*SF[5] + P[2][9]*SF[4] + P[3][9]*SPP[7] + P[11][9]*SPP[3] - P[12][9]*SPP[4] - (P[10][9]*q0)/2 + dt*(P[1][6] + P[0][6]*SF[5] + P[2][6]*SF[4] + P[3][6]*SPP[7] + P[11][6]*SPP[3] - P[12][6]*SPP[4] - (P[10][6]*q0)/2);
-    nextP[1][10] = P[1][10] + P[0][10]*SF[5] + P[2][10]*SF[4] + P[3][10]*SPP[7] + P[11][10]*SPP[3] - P[12][10]*SPP[4] - (P[10][10]*q0)/2;
-    nextP[1][11] = P[1][11] + P[0][11]*SF[5] + P[2][11]*SF[4] + P[3][11]*SPP[7] + P[11][11]*SPP[3] - P[12][11]*SPP[4] - (P[10][11]*q0)/2;
-    nextP[1][12] = P[1][12] + P[0][12]*SF[5] + P[2][12]*SF[4] + P[3][12]*SPP[7] + P[11][12]*SPP[3] - P[12][12]*SPP[4] - (P[10][12]*q0)/2;
-    nextP[1][13] = P[1][13] + P[0][13]*SF[5] + P[2][13]*SF[4] + P[3][13]*SPP[7] + P[11][13]*SPP[3] - P[12][13]*SPP[4] - (P[10][13]*q0)/2;
-    nextP[1][14] = P[1][14] + P[0][14]*SF[5] + P[2][14]*SF[4] + P[3][14]*SPP[7] + P[11][14]*SPP[3] - P[12][14]*SPP[4] - (P[10][14]*q0)/2;
-    nextP[1][15] = P[1][15] + P[0][15]*SF[5] + P[2][15]*SF[4] + P[3][15]*SPP[7] + P[11][15]*SPP[3] - P[12][15]*SPP[4] - (P[10][15]*q0)/2;
-    nextP[1][16] = P[1][16] + P[0][16]*SF[5] + P[2][16]*SF[4] + P[3][16]*SPP[7] + P[11][16]*SPP[3] - P[12][16]*SPP[4] - (P[10][16]*q0)/2;
-    nextP[1][17] = P[1][17] + P[0][17]*SF[5] + P[2][17]*SF[4] + P[3][17]*SPP[7] + P[11][17]*SPP[3] - P[12][17]*SPP[4] - (P[10][17]*q0)/2;
-    nextP[1][18] = P[1][18] + P[0][18]*SF[5] + P[2][18]*SF[4] + P[3][18]*SPP[7] + P[11][18]*SPP[3] - P[12][18]*SPP[4] - (P[10][18]*q0)/2;
-    nextP[1][19] = P[1][19] + P[0][19]*SF[5] + P[2][19]*SF[4] + P[3][19]*SPP[7] + P[11][19]*SPP[3] - P[12][19]*SPP[4] - (P[10][19]*q0)/2;
-    nextP[1][20] = P[1][20] + P[0][20]*SF[5] + P[2][20]*SF[4] + P[3][20]*SPP[7] + P[11][20]*SPP[3] - P[12][20]*SPP[4] - (P[10][20]*q0)/2;
-    nextP[2][0] = P[2][0] + SQ[7] + P[0][0]*SF[3] + P[3][0]*SF[5] + P[1][0]*SPP[6] - P[10][0]*SPP[3] + P[12][0]*SPP[5] - (P[11][0]*q0)/2 + SF[6]*(P[2][1] + P[0][1]*SF[3] + P[3][1]*SF[5] + P[1][1]*SPP[6] - P[10][1]*SPP[3] + P[12][1]*SPP[5] - (P[11][1]*q0)/2) + SPP[7]*(P[2][2] + P[0][2]*SF[3] + P[3][2]*SF[5] + P[1][2]*SPP[6] - P[10][2]*SPP[3] + P[12][2]*SPP[5] - (P[11][2]*q0)/2) + SPP[6]*(P[2][3] + P[0][3]*SF[3] + P[3][3]*SF[5] + P[1][3]*SPP[6] - P[10][3]*SPP[3] + P[12][3]*SPP[5] - (P[11][3]*q0)/2) + SPP[5]*(P[2][10] + P[0][10]*SF[3] + P[3][10]*SF[5] + P[1][10]*SPP[6] - P[10][10]*SPP[3] + P[12][10]*SPP[5] - (P[11][10]*q0)/2) + SPP[4]*(P[2][11] + P[0][11]*SF[3] + P[3][11]*SF[5] + P[1][11]*SPP[6] - P[10][11]*SPP[3] + P[12][11]*SPP[5] - (P[11][11]*q0)/2) + SPP[3]*(P[2][12] + P[0][12]*SF[3] + P[3][12]*SF[5] + P[1][12]*SPP[6] - P[10][12]*SPP[3] + P[12][12]*SPP[5] - (P[11][12]*q0)/2);
-    nextP[2][1] = P[2][1] + SQ[5] + P[0][1]*SF[3] + P[3][1]*SF[5] + P[1][1]*SPP[6] - P[10][1]*SPP[3] + P[12][1]*SPP[5] - (P[11][1]*q0)/2 + SF[5]*(P[2][0] + P[0][0]*SF[3] + P[3][0]*SF[5] + P[1][0]*SPP[6] - P[10][0]*SPP[3] + P[12][0]*SPP[5] - (P[11][0]*q0)/2) + SF[4]*(P[2][2] + P[0][2]*SF[3] + P[3][2]*SF[5] + P[1][2]*SPP[6] - P[10][2]*SPP[3] + P[12][2]*SPP[5] - (P[11][2]*q0)/2) + SPP[7]*(P[2][3] + P[0][3]*SF[3] + P[3][3]*SF[5] + P[1][3]*SPP[6] - P[10][3]*SPP[3] + P[12][3]*SPP[5] - (P[11][3]*q0)/2) + SPP[3]*(P[2][11] + P[0][11]*SF[3] + P[3][11]*SF[5] + P[1][11]*SPP[6] - P[10][11]*SPP[3] + P[12][11]*SPP[5] - (P[11][11]*q0)/2) - SPP[4]*(P[2][12] + P[0][12]*SF[3] + P[3][12]*SF[5] + P[1][12]*SPP[6] - P[10][12]*SPP[3] + P[12][12]*SPP[5] - (P[11][12]*q0)/2) - (q0*(P[2][10] + P[0][10]*SF[3] + P[3][10]*SF[5] + P[1][10]*SPP[6] - P[10][10]*SPP[3] + P[12][10]*SPP[5] - (P[11][10]*q0)/2))/2;
-    nextP[2][2] = P[2][2] + P[0][2]*SF[3] + P[3][2]*SF[5] + P[1][2]*SPP[6] - P[10][2]*SPP[3] + P[12][2]*SPP[5] + dayCov*SQ[9] + (dazCov*SQ[10])/4 - (P[11][2]*q0)/2 + SF[3]*(P[2][0] + P[0][0]*SF[3] + P[3][0]*SF[5] + P[1][0]*SPP[6] - P[10][0]*SPP[3] + P[12][0]*SPP[5] - (P[11][0]*q0)/2) + SF[5]*(P[2][3] + P[0][3]*SF[3] + P[3][3]*SF[5] + P[1][3]*SPP[6] - P[10][3]*SPP[3] + P[12][3]*SPP[5] - (P[11][3]*q0)/2) + SPP[6]*(P[2][1] + P[0][1]*SF[3] + P[3][1]*SF[5] + P[1][1]*SPP[6] - P[10][1]*SPP[3] + P[12][1]*SPP[5] - (P[11][1]*q0)/2) - SPP[3]*(P[2][10] + P[0][10]*SF[3] + P[3][10]*SF[5] + P[1][10]*SPP[6] - P[10][10]*SPP[3] + P[12][10]*SPP[5] - (P[11][10]*q0)/2) + SPP[5]*(P[2][12] + P[0][12]*SF[3] + P[3][12]*SF[5] + P[1][12]*SPP[6] - P[10][12]*SPP[3] + P[12][12]*SPP[5] - (P[11][12]*q0)/2) + (daxCov*sq(q3))/4 - (q0*(P[2][11] + P[0][11]*SF[3] + P[3][11]*SF[5] + P[1][11]*SPP[6] - P[10][11]*SPP[3] + P[12][11]*SPP[5] - (P[11][11]*q0)/2))/2;
-    nextP[2][3] = P[2][3] + SQ[3] + P[0][3]*SF[3] + P[3][3]*SF[5] + P[1][3]*SPP[6] - P[10][3]*SPP[3] + P[12][3]*SPP[5] - (P[11][3]*q0)/2 + SF[4]*(P[2][0] + P[0][0]*SF[3] + P[3][0]*SF[5] + P[1][0]*SPP[6] - P[10][0]*SPP[3] + P[12][0]*SPP[5] - (P[11][0]*q0)/2) + SF[3]*(P[2][1] + P[0][1]*SF[3] + P[3][1]*SF[5] + P[1][1]*SPP[6] - P[10][1]*SPP[3] + P[12][1]*SPP[5] - (P[11][1]*q0)/2) + SF[6]*(P[2][2] + P[0][2]*SF[3] + P[3][2]*SF[5] + P[1][2]*SPP[6] - P[10][2]*SPP[3] + P[12][2]*SPP[5] - (P[11][2]*q0)/2) + SPP[4]*(P[2][10] + P[0][10]*SF[3] + P[3][10]*SF[5] + P[1][10]*SPP[6] - P[10][10]*SPP[3] + P[12][10]*SPP[5] - (P[11][10]*q0)/2) - SPP[5]*(P[2][11] + P[0][11]*SF[3] + P[3][11]*SF[5] + P[1][11]*SPP[6] - P[10][11]*SPP[3] + P[12][11]*SPP[5] - (P[11][11]*q0)/2) - (q0*(P[2][12] + P[0][12]*SF[3] + P[3][12]*SF[5] + P[1][12]*SPP[6] - P[10][12]*SPP[3] + P[12][12]*SPP[5] - (P[11][12]*q0)/2))/2;
-    nextP[2][4] = P[2][4] + P[0][4]*SF[3] + P[3][4]*SF[5] + P[1][4]*SPP[6] - P[10][4]*SPP[3] + P[12][4]*SPP[5] - (P[11][4]*q0)/2 + SF[2]*(P[2][0] + P[0][0]*SF[3] + P[3][0]*SF[5] + P[1][0]*SPP[6] - P[10][0]*SPP[3] + P[12][0]*SPP[5] - (P[11][0]*q0)/2) + SF[0]*(P[2][1] + P[0][1]*SF[3] + P[3][1]*SF[5] + P[1][1]*SPP[6] - P[10][1]*SPP[3] + P[12][1]*SPP[5] - (P[11][1]*q0)/2) + SPP[0]*(P[2][2] + P[0][2]*SF[3] + P[3][2]*SF[5] + P[1][2]*SPP[6] - P[10][2]*SPP[3] + P[12][2]*SPP[5] - (P[11][2]*q0)/2) - SPP[2]*(P[2][3] + P[0][3]*SF[3] + P[3][3]*SF[5] + P[1][3]*SPP[6] - P[10][3]*SPP[3] + P[12][3]*SPP[5] - (P[11][3]*q0)/2);
-    nextP[2][5] = P[2][5] + P[0][5]*SF[3] + P[3][5]*SF[5] + P[1][5]*SPP[6] - P[10][5]*SPP[3] + P[12][5]*SPP[5] - (P[11][5]*q0)/2 + SF[1]*(P[2][0] + P[0][0]*SF[3] + P[3][0]*SF[5] + P[1][0]*SPP[6] - P[10][0]*SPP[3] + P[12][0]*SPP[5] - (P[11][0]*q0)/2) + SF[0]*(P[2][2] + P[0][2]*SF[3] + P[3][2]*SF[5] + P[1][2]*SPP[6] - P[10][2]*SPP[3] + P[12][2]*SPP[5] - (P[11][2]*q0)/2) + SF[2]*(P[2][3] + P[0][3]*SF[3] + P[3][3]*SF[5] + P[1][3]*SPP[6] - P[10][3]*SPP[3] + P[12][3]*SPP[5] - (P[11][3]*q0)/2) - SPP[0]*(P[2][1] + P[0][1]*SF[3] + P[3][1]*SF[5] + P[1][1]*SPP[6] - P[10][1]*SPP[3] + P[12][1]*SPP[5] - (P[11][1]*q0)/2);
-    nextP[2][6] = P[2][6] + P[0][6]*SF[3] + P[3][6]*SF[5] + P[1][6]*SPP[6] - P[10][6]*SPP[3] + P[12][6]*SPP[5] - (P[11][6]*q0)/2 + SF[1]*(P[2][1] + P[0][1]*SF[3] + P[3][1]*SF[5] + P[1][1]*SPP[6] - P[10][1]*SPP[3] + P[12][1]*SPP[5] - (P[11][1]*q0)/2) + SF[0]*(P[2][3] + P[0][3]*SF[3] + P[3][3]*SF[5] + P[1][3]*SPP[6] - P[10][3]*SPP[3] + P[12][3]*SPP[5] - (P[11][3]*q0)/2) + SPP[0]*(P[2][0] + P[0][0]*SF[3] + P[3][0]*SF[5] + P[1][0]*SPP[6] - P[10][0]*SPP[3] + P[12][0]*SPP[5] - (P[11][0]*q0)/2) - SPP[1]*(P[2][2] + P[0][2]*SF[3] + P[3][2]*SF[5] + P[1][2]*SPP[6] - P[10][2]*SPP[3] + P[12][2]*SPP[5] - (P[11][2]*q0)/2);
-    nextP[2][7] = P[2][7] + P[0][7]*SF[3] + P[3][7]*SF[5] + P[1][7]*SPP[6] - P[10][7]*SPP[3] + P[12][7]*SPP[5] - (P[11][7]*q0)/2 + dt*(P[2][4] + P[0][4]*SF[3] + P[3][4]*SF[5] + P[1][4]*SPP[6] - P[10][4]*SPP[3] + P[12][4]*SPP[5] - (P[11][4]*q0)/2);
-    nextP[2][8] = P[2][8] + P[0][8]*SF[3] + P[3][8]*SF[5] + P[1][8]*SPP[6] - P[10][8]*SPP[3] + P[12][8]*SPP[5] - (P[11][8]*q0)/2 + dt*(P[2][5] + P[0][5]*SF[3] + P[3][5]*SF[5] + P[1][5]*SPP[6] - P[10][5]*SPP[3] + P[12][5]*SPP[5] - (P[11][5]*q0)/2);
-    nextP[2][9] = P[2][9] + P[0][9]*SF[3] + P[3][9]*SF[5] + P[1][9]*SPP[6] - P[10][9]*SPP[3] + P[12][9]*SPP[5] - (P[11][9]*q0)/2 + dt*(P[2][6] + P[0][6]*SF[3] + P[3][6]*SF[5] + P[1][6]*SPP[6] - P[10][6]*SPP[3] + P[12][6]*SPP[5] - (P[11][6]*q0)/2);
-    nextP[2][10] = P[2][10] + P[0][10]*SF[3] + P[3][10]*SF[5] + P[1][10]*SPP[6] - P[10][10]*SPP[3] + P[12][10]*SPP[5] - (P[11][10]*q0)/2;
-    nextP[2][11] = P[2][11] + P[0][11]*SF[3] + P[3][11]*SF[5] + P[1][11]*SPP[6] - P[10][11]*SPP[3] + P[12][11]*SPP[5] - (P[11][11]*q0)/2;
-    nextP[2][12] = P[2][12] + P[0][12]*SF[3] + P[3][12]*SF[5] + P[1][12]*SPP[6] - P[10][12]*SPP[3] + P[12][12]*SPP[5] - (P[11][12]*q0)/2;
-    nextP[2][13] = P[2][13] + P[0][13]*SF[3] + P[3][13]*SF[5] + P[1][13]*SPP[6] - P[10][13]*SPP[3] + P[12][13]*SPP[5] - (P[11][13]*q0)/2;
-    nextP[2][14] = P[2][14] + P[0][14]*SF[3] + P[3][14]*SF[5] + P[1][14]*SPP[6] - P[10][14]*SPP[3] + P[12][14]*SPP[5] - (P[11][14]*q0)/2;
-    nextP[2][15] = P[2][15] + P[0][15]*SF[3] + P[3][15]*SF[5] + P[1][15]*SPP[6] - P[10][15]*SPP[3] + P[12][15]*SPP[5] - (P[11][15]*q0)/2;
-    nextP[2][16] = P[2][16] + P[0][16]*SF[3] + P[3][16]*SF[5] + P[1][16]*SPP[6] - P[10][16]*SPP[3] + P[12][16]*SPP[5] - (P[11][16]*q0)/2;
-    nextP[2][17] = P[2][17] + P[0][17]*SF[3] + P[3][17]*SF[5] + P[1][17]*SPP[6] - P[10][17]*SPP[3] + P[12][17]*SPP[5] - (P[11][17]*q0)/2;
-    nextP[2][18] = P[2][18] + P[0][18]*SF[3] + P[3][18]*SF[5] + P[1][18]*SPP[6] - P[10][18]*SPP[3] + P[12][18]*SPP[5] - (P[11][18]*q0)/2;
-    nextP[2][19] = P[2][19] + P[0][19]*SF[3] + P[3][19]*SF[5] + P[1][19]*SPP[6] - P[10][19]*SPP[3] + P[12][19]*SPP[5] - (P[11][19]*q0)/2;
-    nextP[2][20] = P[2][20] + P[0][20]*SF[3] + P[3][20]*SF[5] + P[1][20]*SPP[6] - P[10][20]*SPP[3] + P[12][20]*SPP[5] - (P[11][20]*q0)/2;
-    nextP[3][0] = P[3][0] + SQ[6] + P[0][0]*SF[4] + P[1][0]*SF[3] + P[2][0]*SF[6] + P[10][0]*SPP[4] - P[11][0]*SPP[5] - (P[12][0]*q0)/2 + SF[6]*(P[3][1] + P[0][1]*SF[4] + P[1][1]*SF[3] + P[2][1]*SF[6] + P[10][1]*SPP[4] - P[11][1]*SPP[5] - (P[12][1]*q0)/2) + SPP[7]*(P[3][2] + P[0][2]*SF[4] + P[1][2]*SF[3] + P[2][2]*SF[6] + P[10][2]*SPP[4] - P[11][2]*SPP[5] - (P[12][2]*q0)/2) + SPP[6]*(P[3][3] + P[0][3]*SF[4] + P[1][3]*SF[3] + P[2][3]*SF[6] + P[10][3]*SPP[4] - P[11][3]*SPP[5] - (P[12][3]*q0)/2) + SPP[5]*(P[3][10] + P[0][10]*SF[4] + P[1][10]*SF[3] + P[2][10]*SF[6] + P[10][10]*SPP[4] - P[11][10]*SPP[5] - (P[12][10]*q0)/2) + SPP[4]*(P[3][11] + P[0][11]*SF[4] + P[1][11]*SF[3] + P[2][11]*SF[6] + P[10][11]*SPP[4] - P[11][11]*SPP[5] - (P[12][11]*q0)/2) + SPP[3]*(P[3][12] + P[0][12]*SF[4] + P[1][12]*SF[3] + P[2][12]*SF[6] + P[10][12]*SPP[4] - P[11][12]*SPP[5] - (P[12][12]*q0)/2);
-    nextP[3][1] = P[3][1] + SQ[4] + P[0][1]*SF[4] + P[1][1]*SF[3] + P[2][1]*SF[6] + P[10][1]*SPP[4] - P[11][1]*SPP[5] - (P[12][1]*q0)/2 + SF[5]*(P[3][0] + P[0][0]*SF[4] + P[1][0]*SF[3] + P[2][0]*SF[6] + P[10][0]*SPP[4] - P[11][0]*SPP[5] - (P[12][0]*q0)/2) + SF[4]*(P[3][2] + P[0][2]*SF[4] + P[1][2]*SF[3] + P[2][2]*SF[6] + P[10][2]*SPP[4] - P[11][2]*SPP[5] - (P[12][2]*q0)/2) + SPP[7]*(P[3][3] + P[0][3]*SF[4] + P[1][3]*SF[3] + P[2][3]*SF[6] + P[10][3]*SPP[4] - P[11][3]*SPP[5] - (P[12][3]*q0)/2) + SPP[3]*(P[3][11] + P[0][11]*SF[4] + P[1][11]*SF[3] + P[2][11]*SF[6] + P[10][11]*SPP[4] - P[11][11]*SPP[5] - (P[12][11]*q0)/2) - SPP[4]*(P[3][12] + P[0][12]*SF[4] + P[1][12]*SF[3] + P[2][12]*SF[6] + P[10][12]*SPP[4] - P[11][12]*SPP[5] - (P[12][12]*q0)/2) - (q0*(P[3][10] + P[0][10]*SF[4] + P[1][10]*SF[3] + P[2][10]*SF[6] + P[10][10]*SPP[4] - P[11][10]*SPP[5] - (P[12][10]*q0)/2))/2;
-    nextP[3][2] = P[3][2] + SQ[3] + P[0][2]*SF[4] + P[1][2]*SF[3] + P[2][2]*SF[6] + P[10][2]*SPP[4] - P[11][2]*SPP[5] - (P[12][2]*q0)/2 + SF[3]*(P[3][0] + P[0][0]*SF[4] + P[1][0]*SF[3] + P[2][0]*SF[6] + P[10][0]*SPP[4] - P[11][0]*SPP[5] - (P[12][0]*q0)/2) + SF[5]*(P[3][3] + P[0][3]*SF[4] + P[1][3]*SF[3] + P[2][3]*SF[6] + P[10][3]*SPP[4] - P[11][3]*SPP[5] - (P[12][3]*q0)/2) + SPP[6]*(P[3][1] + P[0][1]*SF[4] + P[1][1]*SF[3] + P[2][1]*SF[6] + P[10][1]*SPP[4] - P[11][1]*SPP[5] - (P[12][1]*q0)/2) - SPP[3]*(P[3][10] + P[0][10]*SF[4] + P[1][10]*SF[3] + P[2][10]*SF[6] + P[10][10]*SPP[4] - P[11][10]*SPP[5] - (P[12][10]*q0)/2) + SPP[5]*(P[3][12] + P[0][12]*SF[4] + P[1][12]*SF[3] + P[2][12]*SF[6] + P[10][12]*SPP[4] - P[11][12]*SPP[5] - (P[12][12]*q0)/2) - (q0*(P[3][11] + P[0][11]*SF[4] + P[1][11]*SF[3] + P[2][11]*SF[6] + P[10][11]*SPP[4] - P[11][11]*SPP[5] - (P[12][11]*q0)/2))/2;
-    nextP[3][3] = P[3][3] + P[0][3]*SF[4] + P[1][3]*SF[3] + P[2][3]*SF[6] + P[10][3]*SPP[4] - P[11][3]*SPP[5] + (dayCov*SQ[10])/4 + dazCov*SQ[9] - (P[12][3]*q0)/2 + SF[4]*(P[3][0] + P[0][0]*SF[4] + P[1][0]*SF[3] + P[2][0]*SF[6] + P[10][0]*SPP[4] - P[11][0]*SPP[5] - (P[12][0]*q0)/2) + SF[3]*(P[3][1] + P[0][1]*SF[4] + P[1][1]*SF[3] + P[2][1]*SF[6] + P[10][1]*SPP[4] - P[11][1]*SPP[5] - (P[12][1]*q0)/2) + SF[6]*(P[3][2] + P[0][2]*SF[4] + P[1][2]*SF[3] + P[2][2]*SF[6] + P[10][2]*SPP[4] - P[11][2]*SPP[5] - (P[12][2]*q0)/2) + SPP[4]*(P[3][10] + P[0][10]*SF[4] + P[1][10]*SF[3] + P[2][10]*SF[6] + P[10][10]*SPP[4] - P[11][10]*SPP[5] - (P[12][10]*q0)/2) - SPP[5]*(P[3][11] + P[0][11]*SF[4] + P[1][11]*SF[3] + P[2][11]*SF[6] + P[10][11]*SPP[4] - P[11][11]*SPP[5] - (P[12][11]*q0)/2) + (daxCov*sq(q2))/4 - (q0*(P[3][12] + P[0][12]*SF[4] + P[1][12]*SF[3] + P[2][12]*SF[6] + P[10][12]*SPP[4] - P[11][12]*SPP[5] - (P[12][12]*q0)/2))/2;
-    nextP[3][4] = P[3][4] + P[0][4]*SF[4] + P[1][4]*SF[3] + P[2][4]*SF[6] + P[10][4]*SPP[4] - P[11][4]*SPP[5] - (P[12][4]*q0)/2 + SF[2]*(P[3][0] + P[0][0]*SF[4] + P[1][0]*SF[3] + P[2][0]*SF[6] + P[10][0]*SPP[4] - P[11][0]*SPP[5] - (P[12][0]*q0)/2) + SF[0]*(P[3][1] + P[0][1]*SF[4] + P[1][1]*SF[3] + P[2][1]*SF[6] + P[10][1]*SPP[4] - P[11][1]*SPP[5] - (P[12][1]*q0)/2) + SPP[0]*(P[3][2] + P[0][2]*SF[4] + P[1][2]*SF[3] + P[2][2]*SF[6] + P[10][2]*SPP[4] - P[11][2]*SPP[5] - (P[12][2]*q0)/2) - SPP[2]*(P[3][3] + P[0][3]*SF[4] + P[1][3]*SF[3] + P[2][3]*SF[6] + P[10][3]*SPP[4] - P[11][3]*SPP[5] - (P[12][3]*q0)/2);
-    nextP[3][5] = P[3][5] + P[0][5]*SF[4] + P[1][5]*SF[3] + P[2][5]*SF[6] + P[10][5]*SPP[4] - P[11][5]*SPP[5] - (P[12][5]*q0)/2 + SF[1]*(P[3][0] + P[0][0]*SF[4] + P[1][0]*SF[3] + P[2][0]*SF[6] + P[10][0]*SPP[4] - P[11][0]*SPP[5] - (P[12][0]*q0)/2) + SF[0]*(P[3][2] + P[0][2]*SF[4] + P[1][2]*SF[3] + P[2][2]*SF[6] + P[10][2]*SPP[4] - P[11][2]*SPP[5] - (P[12][2]*q0)/2) + SF[2]*(P[3][3] + P[0][3]*SF[4] + P[1][3]*SF[3] + P[2][3]*SF[6] + P[10][3]*SPP[4] - P[11][3]*SPP[5] - (P[12][3]*q0)/2) - SPP[0]*(P[3][1] + P[0][1]*SF[4] + P[1][1]*SF[3] + P[2][1]*SF[6] + P[10][1]*SPP[4] - P[11][1]*SPP[5] - (P[12][1]*q0)/2);
-    nextP[3][6] = P[3][6] + P[0][6]*SF[4] + P[1][6]*SF[3] + P[2][6]*SF[6] + P[10][6]*SPP[4] - P[11][6]*SPP[5] - (P[12][6]*q0)/2 + SF[1]*(P[3][1] + P[0][1]*SF[4] + P[1][1]*SF[3] + P[2][1]*SF[6] + P[10][1]*SPP[4] - P[11][1]*SPP[5] - (P[12][1]*q0)/2) + SF[0]*(P[3][3] + P[0][3]*SF[4] + P[1][3]*SF[3] + P[2][3]*SF[6] + P[10][3]*SPP[4] - P[11][3]*SPP[5] - (P[12][3]*q0)/2) + SPP[0]*(P[3][0] + P[0][0]*SF[4] + P[1][0]*SF[3] + P[2][0]*SF[6] + P[10][0]*SPP[4] - P[11][0]*SPP[5] - (P[12][0]*q0)/2) - SPP[1]*(P[3][2] + P[0][2]*SF[4] + P[1][2]*SF[3] + P[2][2]*SF[6] + P[10][2]*SPP[4] - P[11][2]*SPP[5] - (P[12][2]*q0)/2);
-    nextP[3][7] = P[3][7] + P[0][7]*SF[4] + P[1][7]*SF[3] + P[2][7]*SF[6] + P[10][7]*SPP[4] - P[11][7]*SPP[5] - (P[12][7]*q0)/2 + dt*(P[3][4] + P[0][4]*SF[4] + P[1][4]*SF[3] + P[2][4]*SF[6] + P[10][4]*SPP[4] - P[11][4]*SPP[5] - (P[12][4]*q0)/2);
-    nextP[3][8] = P[3][8] + P[0][8]*SF[4] + P[1][8]*SF[3] + P[2][8]*SF[6] + P[10][8]*SPP[4] - P[11][8]*SPP[5] - (P[12][8]*q0)/2 + dt*(P[3][5] + P[0][5]*SF[4] + P[1][5]*SF[3] + P[2][5]*SF[6] + P[10][5]*SPP[4] - P[11][5]*SPP[5] - (P[12][5]*q0)/2);
-    nextP[3][9] = P[3][9] + P[0][9]*SF[4] + P[1][9]*SF[3] + P[2][9]*SF[6] + P[10][9]*SPP[4] - P[11][9]*SPP[5] - (P[12][9]*q0)/2 + dt*(P[3][6] + P[0][6]*SF[4] + P[1][6]*SF[3] + P[2][6]*SF[6] + P[10][6]*SPP[4] - P[11][6]*SPP[5] - (P[12][6]*q0)/2);
-    nextP[3][10] = P[3][10] + P[0][10]*SF[4] + P[1][10]*SF[3] + P[2][10]*SF[6] + P[10][10]*SPP[4] - P[11][10]*SPP[5] - (P[12][10]*q0)/2;
-    nextP[3][11] = P[3][11] + P[0][11]*SF[4] + P[1][11]*SF[3] + P[2][11]*SF[6] + P[10][11]*SPP[4] - P[11][11]*SPP[5] - (P[12][11]*q0)/2;
-    nextP[3][12] = P[3][12] + P[0][12]*SF[4] + P[1][12]*SF[3] + P[2][12]*SF[6] + P[10][12]*SPP[4] - P[11][12]*SPP[5] - (P[12][12]*q0)/2;
-    nextP[3][13] = P[3][13] + P[0][13]*SF[4] + P[1][13]*SF[3] + P[2][13]*SF[6] + P[10][13]*SPP[4] - P[11][13]*SPP[5] - (P[12][13]*q0)/2;
-    nextP[3][14] = P[3][14] + P[0][14]*SF[4] + P[1][14]*SF[3] + P[2][14]*SF[6] + P[10][14]*SPP[4] - P[11][14]*SPP[5] - (P[12][14]*q0)/2;
-    nextP[3][15] = P[3][15] + P[0][15]*SF[4] + P[1][15]*SF[3] + P[2][15]*SF[6] + P[10][15]*SPP[4] - P[11][15]*SPP[5] - (P[12][15]*q0)/2;
-    nextP[3][16] = P[3][16] + P[0][16]*SF[4] + P[1][16]*SF[3] + P[2][16]*SF[6] + P[10][16]*SPP[4] - P[11][16]*SPP[5] - (P[12][16]*q0)/2;
-    nextP[3][17] = P[3][17] + P[0][17]*SF[4] + P[1][17]*SF[3] + P[2][17]*SF[6] + P[10][17]*SPP[4] - P[11][17]*SPP[5] - (P[12][17]*q0)/2;
-    nextP[3][18] = P[3][18] + P[0][18]*SF[4] + P[1][18]*SF[3] + P[2][18]*SF[6] + P[10][18]*SPP[4] - P[11][18]*SPP[5] - (P[12][18]*q0)/2;
-    nextP[3][19] = P[3][19] + P[0][19]*SF[4] + P[1][19]*SF[3] + P[2][19]*SF[6] + P[10][19]*SPP[4] - P[11][19]*SPP[5] - (P[12][19]*q0)/2;
-    nextP[3][20] = P[3][20] + P[0][20]*SF[4] + P[1][20]*SF[3] + P[2][20]*SF[6] + P[10][20]*SPP[4] - P[11][20]*SPP[5] - (P[12][20]*q0)/2;
-    nextP[4][0] = P[4][0] + P[0][0]*SF[2] + P[1][0]*SF[0] + P[2][0]*SPP[0] - P[3][0]*SPP[2] + SF[6]*(P[4][1] + P[0][1]*SF[2] + P[1][1]*SF[0] + P[2][1]*SPP[0] - P[3][1]*SPP[2]) + SPP[7]*(P[4][2] + P[0][2]*SF[2] + P[1][2]*SF[0] + P[2][2]*SPP[0] - P[3][2]*SPP[2]) + SPP[6]*(P[4][3] + P[0][3]*SF[2] + P[1][3]*SF[0] + P[2][3]*SPP[0] - P[3][3]*SPP[2]) + SPP[5]*(P[4][10] + P[0][10]*SF[2] + P[1][10]*SF[0] + P[2][10]*SPP[0] - P[3][10]*SPP[2]) + SPP[4]*(P[4][11] + P[0][11]*SF[2] + P[1][11]*SF[0] + P[2][11]*SPP[0] - P[3][11]*SPP[2]) + SPP[3]*(P[4][12] + P[0][12]*SF[2] + P[1][12]*SF[0] + P[2][12]*SPP[0] - P[3][12]*SPP[2]);
-    nextP[4][1] = P[4][1] + P[0][1]*SF[2] + P[1][1]*SF[0] + P[2][1]*SPP[0] - P[3][1]*SPP[2] + SF[5]*(P[4][0] + P[0][0]*SF[2] + P[1][0]*SF[0] + P[2][0]*SPP[0] - P[3][0]*SPP[2]) + SF[4]*(P[4][2] + P[0][2]*SF[2] + P[1][2]*SF[0] + P[2][2]*SPP[0] - P[3][2]*SPP[2]) + SPP[7]*(P[4][3] + P[0][3]*SF[2] + P[1][3]*SF[0] + P[2][3]*SPP[0] - P[3][3]*SPP[2]) + SPP[3]*(P[4][11] + P[0][11]*SF[2] + P[1][11]*SF[0] + P[2][11]*SPP[0] - P[3][11]*SPP[2]) - SPP[4]*(P[4][12] + P[0][12]*SF[2] + P[1][12]*SF[0] + P[2][12]*SPP[0] - P[3][12]*SPP[2]) - (q0*(P[4][10] + P[0][10]*SF[2] + P[1][10]*SF[0] + P[2][10]*SPP[0] - P[3][10]*SPP[2]))/2;
-    nextP[4][2] = P[4][2] + P[0][2]*SF[2] + P[1][2]*SF[0] + P[2][2]*SPP[0] - P[3][2]*SPP[2] + SF[3]*(P[4][0] + P[0][0]*SF[2] + P[1][0]*SF[0] + P[2][0]*SPP[0] - P[3][0]*SPP[2]) + SF[5]*(P[4][3] + P[0][3]*SF[2] + P[1][3]*SF[0] + P[2][3]*SPP[0] - P[3][3]*SPP[2]) + SPP[6]*(P[4][1] + P[0][1]*SF[2] + P[1][1]*SF[0] + P[2][1]*SPP[0] - P[3][1]*SPP[2]) - SPP[3]*(P[4][10] + P[0][10]*SF[2] + P[1][10]*SF[0] + P[2][10]*SPP[0] - P[3][10]*SPP[2]) + SPP[5]*(P[4][12] + P[0][12]*SF[2] + P[1][12]*SF[0] + P[2][12]*SPP[0] - P[3][12]*SPP[2]) - (q0*(P[4][11] + P[0][11]*SF[2] + P[1][11]*SF[0] + P[2][11]*SPP[0] - P[3][11]*SPP[2]))/2;
-    nextP[4][3] = P[4][3] + P[0][3]*SF[2] + P[1][3]*SF[0] + P[2][3]*SPP[0] - P[3][3]*SPP[2] + SF[4]*(P[4][0] + P[0][0]*SF[2] + P[1][0]*SF[0] + P[2][0]*SPP[0] - P[3][0]*SPP[2]) + SF[3]*(P[4][1] + P[0][1]*SF[2] + P[1][1]*SF[0] + P[2][1]*SPP[0] - P[3][1]*SPP[2]) + SF[6]*(P[4][2] + P[0][2]*SF[2] + P[1][2]*SF[0] + P[2][2]*SPP[0] - P[3][2]*SPP[2]) + SPP[4]*(P[4][10] + P[0][10]*SF[2] + P[1][10]*SF[0] + P[2][10]*SPP[0] - P[3][10]*SPP[2]) - SPP[5]*(P[4][11] + P[0][11]*SF[2] + P[1][11]*SF[0] + P[2][11]*SPP[0] - P[3][11]*SPP[2]) - (q0*(P[4][12] + P[0][12]*SF[2] + P[1][12]*SF[0] + P[2][12]*SPP[0] - P[3][12]*SPP[2]))/2;
-    nextP[4][4] = P[4][4] + P[0][4]*SF[2] + P[1][4]*SF[0] + P[2][4]*SPP[0] - P[3][4]*SPP[2] + dvyCov*sq(SG[7] - 2*q0*q3) + dvzCov*sq(SG[6] + 2*q0*q2) + SF[2]*(P[4][0] + P[0][0]*SF[2] + P[1][0]*SF[0] + P[2][0]*SPP[0] - P[3][0]*SPP[2]) + SF[0]*(P[4][1] + P[0][1]*SF[2] + P[1][1]*SF[0] + P[2][1]*SPP[0] - P[3][1]*SPP[2]) + SPP[0]*(P[4][2] + P[0][2]*SF[2] + P[1][2]*SF[0] + P[2][2]*SPP[0] - P[3][2]*SPP[2]) - SPP[2]*(P[4][3] + P[0][3]*SF[2] + P[1][3]*SF[0] + P[2][3]*SPP[0] - P[3][3]*SPP[2]) + dvxCov*sq(SG[1] + SG[2] - SG[3] - SG[4]);
-    nextP[4][5] = P[4][5] + SQ[2] + P[0][5]*SF[2] + P[1][5]*SF[0] + P[2][5]*SPP[0] - P[3][5]*SPP[2] + SF[1]*(P[4][0] + P[0][0]*SF[2] + P[1][0]*SF[0] + P[2][0]*SPP[0] - P[3][0]*SPP[2]) + SF[0]*(P[4][2] + P[0][2]*SF[2] + P[1][2]*SF[0] + P[2][2]*SPP[0] - P[3][2]*SPP[2]) + SF[2]*(P[4][3] + P[0][3]*SF[2] + P[1][3]*SF[0] + P[2][3]*SPP[0] - P[3][3]*SPP[2]) - SPP[0]*(P[4][1] + P[0][1]*SF[2] + P[1][1]*SF[0] + P[2][1]*SPP[0] - P[3][1]*SPP[2]);
-    nextP[4][6] = P[4][6] + SQ[1] + P[0][6]*SF[2] + P[1][6]*SF[0] + P[2][6]*SPP[0] - P[3][6]*SPP[2] + SF[1]*(P[4][1] + P[0][1]*SF[2] + P[1][1]*SF[0] + P[2][1]*SPP[0] - P[3][1]*SPP[2]) + SF[0]*(P[4][3] + P[0][3]*SF[2] + P[1][3]*SF[0] + P[2][3]*SPP[0] - P[3][3]*SPP[2]) + SPP[0]*(P[4][0] + P[0][0]*SF[2] + P[1][0]*SF[0] + P[2][0]*SPP[0] - P[3][0]*SPP[2]) - SPP[1]*(P[4][2] + P[0][2]*SF[2] + P[1][2]*SF[0] + P[2][2]*SPP[0] - P[3][2]*SPP[2]);
-    nextP[4][7] = P[4][7] + P[0][7]*SF[2] + P[1][7]*SF[0] + P[2][7]*SPP[0] - P[3][7]*SPP[2] + dt*(P[4][4] + P[0][4]*SF[2] + P[1][4]*SF[0] + P[2][4]*SPP[0] - P[3][4]*SPP[2]);
-    nextP[4][8] = P[4][8] + P[0][8]*SF[2] + P[1][8]*SF[0] + P[2][8]*SPP[0] - P[3][8]*SPP[2] + dt*(P[4][5] + P[0][5]*SF[2] + P[1][5]*SF[0] + P[2][5]*SPP[0] - P[3][5]*SPP[2]);
-    nextP[4][9] = P[4][9] + P[0][9]*SF[2] + P[1][9]*SF[0] + P[2][9]*SPP[0] - P[3][9]*SPP[2] + dt*(P[4][6] + P[0][6]*SF[2] + P[1][6]*SF[0] + P[2][6]*SPP[0] - P[3][6]*SPP[2]);
-    nextP[4][10] = P[4][10] + P[0][10]*SF[2] + P[1][10]*SF[0] + P[2][10]*SPP[0] - P[3][10]*SPP[2];
-    nextP[4][11] = P[4][11] + P[0][11]*SF[2] + P[1][11]*SF[0] + P[2][11]*SPP[0] - P[3][11]*SPP[2];
-    nextP[4][12] = P[4][12] + P[0][12]*SF[2] + P[1][12]*SF[0] + P[2][12]*SPP[0] - P[3][12]*SPP[2];
-    nextP[4][13] = P[4][13] + P[0][13]*SF[2] + P[1][13]*SF[0] + P[2][13]*SPP[0] - P[3][13]*SPP[2];
-    nextP[4][14] = P[4][14] + P[0][14]*SF[2] + P[1][14]*SF[0] + P[2][14]*SPP[0] - P[3][14]*SPP[2];
-    nextP[4][15] = P[4][15] + P[0][15]*SF[2] + P[1][15]*SF[0] + P[2][15]*SPP[0] - P[3][15]*SPP[2];
-    nextP[4][16] = P[4][16] + P[0][16]*SF[2] + P[1][16]*SF[0] + P[2][16]*SPP[0] - P[3][16]*SPP[2];
-    nextP[4][17] = P[4][17] + P[0][17]*SF[2] + P[1][17]*SF[0] + P[2][17]*SPP[0] - P[3][17]*SPP[2];
-    nextP[4][18] = P[4][18] + P[0][18]*SF[2] + P[1][18]*SF[0] + P[2][18]*SPP[0] - P[3][18]*SPP[2];
-    nextP[4][19] = P[4][19] + P[0][19]*SF[2] + P[1][19]*SF[0] + P[2][19]*SPP[0] - P[3][19]*SPP[2];
-    nextP[4][20] = P[4][20] + P[0][20]*SF[2] + P[1][20]*SF[0] + P[2][20]*SPP[0] - P[3][20]*SPP[2];
-    nextP[5][0] = P[5][0] + P[0][0]*SF[1] + P[2][0]*SF[0] + P[3][0]*SF[2] - P[1][0]*SPP[0] + SF[6]*(P[5][1] + P[0][1]*SF[1] + P[2][1]*SF[0] + P[3][1]*SF[2] - P[1][1]*SPP[0]) + SPP[7]*(P[5][2] + P[0][2]*SF[1] + P[2][2]*SF[0] + P[3][2]*SF[2] - P[1][2]*SPP[0]) + SPP[6]*(P[5][3] + P[0][3]*SF[1] + P[2][3]*SF[0] + P[3][3]*SF[2] - P[1][3]*SPP[0]) + SPP[5]*(P[5][10] + P[0][10]*SF[1] + P[2][10]*SF[0] + P[3][10]*SF[2] - P[1][10]*SPP[0]) + SPP[4]*(P[5][11] + P[0][11]*SF[1] + P[2][11]*SF[0] + P[3][11]*SF[2] - P[1][11]*SPP[0]) + SPP[3]*(P[5][12] + P[0][12]*SF[1] + P[2][12]*SF[0] + P[3][12]*SF[2] - P[1][12]*SPP[0]);
-    nextP[5][1] = P[5][1] + P[0][1]*SF[1] + P[2][1]*SF[0] + P[3][1]*SF[2] - P[1][1]*SPP[0] + SF[5]*(P[5][0] + P[0][0]*SF[1] + P[2][0]*SF[0] + P[3][0]*SF[2] - P[1][0]*SPP[0]) + SF[4]*(P[5][2] + P[0][2]*SF[1] + P[2][2]*SF[0] + P[3][2]*SF[2] - P[1][2]*SPP[0]) + SPP[7]*(P[5][3] + P[0][3]*SF[1] + P[2][3]*SF[0] + P[3][3]*SF[2] - P[1][3]*SPP[0]) + SPP[3]*(P[5][11] + P[0][11]*SF[1] + P[2][11]*SF[0] + P[3][11]*SF[2] - P[1][11]*SPP[0]) - SPP[4]*(P[5][12] + P[0][12]*SF[1] + P[2][12]*SF[0] + P[3][12]*SF[2] - P[1][12]*SPP[0]) - (q0*(P[5][10] + P[0][10]*SF[1] + P[2][10]*SF[0] + P[3][10]*SF[2] - P[1][10]*SPP[0]))/2;
-    nextP[5][2] = P[5][2] + P[0][2]*SF[1] + P[2][2]*SF[0] + P[3][2]*SF[2] - P[1][2]*SPP[0] + SF[3]*(P[5][0] + P[0][0]*SF[1] + P[2][0]*SF[0] + P[3][0]*SF[2] - P[1][0]*SPP[0]) + SF[5]*(P[5][3] + P[0][3]*SF[1] + P[2][3]*SF[0] + P[3][3]*SF[2] - P[1][3]*SPP[0]) + SPP[6]*(P[5][1] + P[0][1]*SF[1] + P[2][1]*SF[0] + P[3][1]*SF[2] - P[1][1]*SPP[0]) - SPP[3]*(P[5][10] + P[0][10]*SF[1] + P[2][10]*SF[0] + P[3][10]*SF[2] - P[1][10]*SPP[0]) + SPP[5]*(P[5][12] + P[0][12]*SF[1] + P[2][12]*SF[0] + P[3][12]*SF[2] - P[1][12]*SPP[0]) - (q0*(P[5][11] + P[0][11]*SF[1] + P[2][11]*SF[0] + P[3][11]*SF[2] - P[1][11]*SPP[0]))/2;
-    nextP[5][3] = P[5][3] + P[0][3]*SF[1] + P[2][3]*SF[0] + P[3][3]*SF[2] - P[1][3]*SPP[0] + SF[4]*(P[5][0] + P[0][0]*SF[1] + P[2][0]*SF[0] + P[3][0]*SF[2] - P[1][0]*SPP[0]) + SF[3]*(P[5][1] + P[0][1]*SF[1] + P[2][1]*SF[0] + P[3][1]*SF[2] - P[1][1]*SPP[0]) + SF[6]*(P[5][2] + P[0][2]*SF[1] + P[2][2]*SF[0] + P[3][2]*SF[2] - P[1][2]*SPP[0]) + SPP[4]*(P[5][10] + P[0][10]*SF[1] + P[2][10]*SF[0] + P[3][10]*SF[2] - P[1][10]*SPP[0]) - SPP[5]*(P[5][11] + P[0][11]*SF[1] + P[2][11]*SF[0] + P[3][11]*SF[2] - P[1][11]*SPP[0]) - (q0*(P[5][12] + P[0][12]*SF[1] + P[2][12]*SF[0] + P[3][12]*SF[2] - P[1][12]*SPP[0]))/2;
-    nextP[5][4] = P[5][4] + SQ[2] + P[0][4]*SF[1] + P[2][4]*SF[0] + P[3][4]*SF[2] - P[1][4]*SPP[0] + SF[2]*(P[5][0] + P[0][0]*SF[1] + P[2][0]*SF[0] + P[3][0]*SF[2] - P[1][0]*SPP[0]) + SF[0]*(P[5][1] + P[0][1]*SF[1] + P[2][1]*SF[0] + P[3][1]*SF[2] - P[1][1]*SPP[0]) + SPP[0]*(P[5][2] + P[0][2]*SF[1] + P[2][2]*SF[0] + P[3][2]*SF[2] - P[1][2]*SPP[0]) - SPP[2]*(P[5][3] + P[0][3]*SF[1] + P[2][3]*SF[0] + P[3][3]*SF[2] - P[1][3]*SPP[0]);
-    nextP[5][5] = P[5][5] + P[0][5]*SF[1] + P[2][5]*SF[0] + P[3][5]*SF[2] - P[1][5]*SPP[0] + dvxCov*sq(SG[7] + 2*q0*q3) + dvzCov*sq(SG[5] - 2*q0*q1) + SF[1]*(P[5][0] + P[0][0]*SF[1] + P[2][0]*SF[0] + P[3][0]*SF[2] - P[1][0]*SPP[0]) + SF[0]*(P[5][2] + P[0][2]*SF[1] + P[2][2]*SF[0] + P[3][2]*SF[2] - P[1][2]*SPP[0]) + SF[2]*(P[5][3] + P[0][3]*SF[1] + P[2][3]*SF[0] + P[3][3]*SF[2] - P[1][3]*SPP[0]) - SPP[0]*(P[5][1] + P[0][1]*SF[1] + P[2][1]*SF[0] + P[3][1]*SF[2] - P[1][1]*SPP[0]) + dvyCov*sq(SG[1] - SG[2] + SG[3] - SG[4]);
-    nextP[5][6] = P[5][6] + SQ[0] + P[0][6]*SF[1] + P[2][6]*SF[0] + P[3][6]*SF[2] - P[1][6]*SPP[0] + SF[1]*(P[5][1] + P[0][1]*SF[1] + P[2][1]*SF[0] + P[3][1]*SF[2] - P[1][1]*SPP[0]) + SF[0]*(P[5][3] + P[0][3]*SF[1] + P[2][3]*SF[0] + P[3][3]*SF[2] - P[1][3]*SPP[0]) + SPP[0]*(P[5][0] + P[0][0]*SF[1] + P[2][0]*SF[0] + P[3][0]*SF[2] - P[1][0]*SPP[0]) - SPP[1]*(P[5][2] + P[0][2]*SF[1] + P[2][2]*SF[0] + P[3][2]*SF[2] - P[1][2]*SPP[0]);
-    nextP[5][7] = P[5][7] + P[0][7]*SF[1] + P[2][7]*SF[0] + P[3][7]*SF[2] - P[1][7]*SPP[0] + dt*(P[5][4] + P[0][4]*SF[1] + P[2][4]*SF[0] + P[3][4]*SF[2] - P[1][4]*SPP[0]);
-    nextP[5][8] = P[5][8] + P[0][8]*SF[1] + P[2][8]*SF[0] + P[3][8]*SF[2] - P[1][8]*SPP[0] + dt*(P[5][5] + P[0][5]*SF[1] + P[2][5]*SF[0] + P[3][5]*SF[2] - P[1][5]*SPP[0]);
-    nextP[5][9] = P[5][9] + P[0][9]*SF[1] + P[2][9]*SF[0] + P[3][9]*SF[2] - P[1][9]*SPP[0] + dt*(P[5][6] + P[0][6]*SF[1] + P[2][6]*SF[0] + P[3][6]*SF[2] - P[1][6]*SPP[0]);
-    nextP[5][10] = P[5][10] + P[0][10]*SF[1] + P[2][10]*SF[0] + P[3][10]*SF[2] - P[1][10]*SPP[0];
-    nextP[5][11] = P[5][11] + P[0][11]*SF[1] + P[2][11]*SF[0] + P[3][11]*SF[2] - P[1][11]*SPP[0];
-    nextP[5][12] = P[5][12] + P[0][12]*SF[1] + P[2][12]*SF[0] + P[3][12]*SF[2] - P[1][12]*SPP[0];
-    nextP[5][13] = P[5][13] + P[0][13]*SF[1] + P[2][13]*SF[0] + P[3][13]*SF[2] - P[1][13]*SPP[0];
-    nextP[5][14] = P[5][14] + P[0][14]*SF[1] + P[2][14]*SF[0] + P[3][14]*SF[2] - P[1][14]*SPP[0];
-    nextP[5][15] = P[5][15] + P[0][15]*SF[1] + P[2][15]*SF[0] + P[3][15]*SF[2] - P[1][15]*SPP[0];
-    nextP[5][16] = P[5][16] + P[0][16]*SF[1] + P[2][16]*SF[0] + P[3][16]*SF[2] - P[1][16]*SPP[0];
-    nextP[5][17] = P[5][17] + P[0][17]*SF[1] + P[2][17]*SF[0] + P[3][17]*SF[2] - P[1][17]*SPP[0];
-    nextP[5][18] = P[5][18] + P[0][18]*SF[1] + P[2][18]*SF[0] + P[3][18]*SF[2] - P[1][18]*SPP[0];
-    nextP[5][19] = P[5][19] + P[0][19]*SF[1] + P[2][19]*SF[0] + P[3][19]*SF[2] - P[1][19]*SPP[0];
-    nextP[5][20] = P[5][20] + P[0][20]*SF[1] + P[2][20]*SF[0] + P[3][20]*SF[2] - P[1][20]*SPP[0];
-    nextP[6][0] = P[6][0] + P[1][0]*SF[1] + P[3][0]*SF[0] + P[0][0]*SPP[0] - P[2][0]*SPP[1] + SF[6]*(P[6][1] + P[1][1]*SF[1] + P[3][1]*SF[0] + P[0][1]*SPP[0] - P[2][1]*SPP[1]) + SPP[7]*(P[6][2] + P[1][2]*SF[1] + P[3][2]*SF[0] + P[0][2]*SPP[0] - P[2][2]*SPP[1]) + SPP[6]*(P[6][3] + P[1][3]*SF[1] + P[3][3]*SF[0] + P[0][3]*SPP[0] - P[2][3]*SPP[1]) + SPP[5]*(P[6][10] + P[1][10]*SF[1] + P[3][10]*SF[0] + P[0][10]*SPP[0] - P[2][10]*SPP[1]) + SPP[4]*(P[6][11] + P[1][11]*SF[1] + P[3][11]*SF[0] + P[0][11]*SPP[0] - P[2][11]*SPP[1]) + SPP[3]*(P[6][12] + P[1][12]*SF[1] + P[3][12]*SF[0] + P[0][12]*SPP[0] - P[2][12]*SPP[1]);
-    nextP[6][1] = P[6][1] + P[1][1]*SF[1] + P[3][1]*SF[0] + P[0][1]*SPP[0] - P[2][1]*SPP[1] + SF[5]*(P[6][0] + P[1][0]*SF[1] + P[3][0]*SF[0] + P[0][0]*SPP[0] - P[2][0]*SPP[1]) + SF[4]*(P[6][2] + P[1][2]*SF[1] + P[3][2]*SF[0] + P[0][2]*SPP[0] - P[2][2]*SPP[1]) + SPP[7]*(P[6][3] + P[1][3]*SF[1] + P[3][3]*SF[0] + P[0][3]*SPP[0] - P[2][3]*SPP[1]) + SPP[3]*(P[6][11] + P[1][11]*SF[1] + P[3][11]*SF[0] + P[0][11]*SPP[0] - P[2][11]*SPP[1]) - SPP[4]*(P[6][12] + P[1][12]*SF[1] + P[3][12]*SF[0] + P[0][12]*SPP[0] - P[2][12]*SPP[1]) - (q0*(P[6][10] + P[1][10]*SF[1] + P[3][10]*SF[0] + P[0][10]*SPP[0] - P[2][10]*SPP[1]))/2;
-    nextP[6][2] = P[6][2] + P[1][2]*SF[1] + P[3][2]*SF[0] + P[0][2]*SPP[0] - P[2][2]*SPP[1] + SF[3]*(P[6][0] + P[1][0]*SF[1] + P[3][0]*SF[0] + P[0][0]*SPP[0] - P[2][0]*SPP[1]) + SF[5]*(P[6][3] + P[1][3]*SF[1] + P[3][3]*SF[0] + P[0][3]*SPP[0] - P[2][3]*SPP[1]) + SPP[6]*(P[6][1] + P[1][1]*SF[1] + P[3][1]*SF[0] + P[0][1]*SPP[0] - P[2][1]*SPP[1]) - SPP[3]*(P[6][10] + P[1][10]*SF[1] + P[3][10]*SF[0] + P[0][10]*SPP[0] - P[2][10]*SPP[1]) + SPP[5]*(P[6][12] + P[1][12]*SF[1] + P[3][12]*SF[0] + P[0][12]*SPP[0] - P[2][12]*SPP[1]) - (q0*(P[6][11] + P[1][11]*SF[1] + P[3][11]*SF[0] + P[0][11]*SPP[0] - P[2][11]*SPP[1]))/2;
-    nextP[6][3] = P[6][3] + P[1][3]*SF[1] + P[3][3]*SF[0] + P[0][3]*SPP[0] - P[2][3]*SPP[1] + SF[4]*(P[6][0] + P[1][0]*SF[1] + P[3][0]*SF[0] + P[0][0]*SPP[0] - P[2][0]*SPP[1]) + SF[3]*(P[6][1] + P[1][1]*SF[1] + P[3][1]*SF[0] + P[0][1]*SPP[0] - P[2][1]*SPP[1]) + SF[6]*(P[6][2] + P[1][2]*SF[1] + P[3][2]*SF[0] + P[0][2]*SPP[0] - P[2][2]*SPP[1]) + SPP[4]*(P[6][10] + P[1][10]*SF[1] + P[3][10]*SF[0] + P[0][10]*SPP[0] - P[2][10]*SPP[1]) - SPP[5]*(P[6][11] + P[1][11]*SF[1] + P[3][11]*SF[0] + P[0][11]*SPP[0] - P[2][11]*SPP[1]) - (q0*(P[6][12] + P[1][12]*SF[1] + P[3][12]*SF[0] + P[0][12]*SPP[0] - P[2][12]*SPP[1]))/2;
-    nextP[6][4] = P[6][4] + SQ[1] + P[1][4]*SF[1] + P[3][4]*SF[0] + P[0][4]*SPP[0] - P[2][4]*SPP[1] + SF[2]*(P[6][0] + P[1][0]*SF[1] + P[3][0]*SF[0] + P[0][0]*SPP[0] - P[2][0]*SPP[1]) + SF[0]*(P[6][1] + P[1][1]*SF[1] + P[3][1]*SF[0] + P[0][1]*SPP[0] - P[2][1]*SPP[1]) + SPP[0]*(P[6][2] + P[1][2]*SF[1] + P[3][2]*SF[0] + P[0][2]*SPP[0] - P[2][2]*SPP[1]) - SPP[2]*(P[6][3] + P[1][3]*SF[1] + P[3][3]*SF[0] + P[0][3]*SPP[0] - P[2][3]*SPP[1]);
-    nextP[6][5] = P[6][5] + SQ[0] + P[1][5]*SF[1] + P[3][5]*SF[0] + P[0][5]*SPP[0] - P[2][5]*SPP[1] + SF[1]*(P[6][0] + P[1][0]*SF[1] + P[3][0]*SF[0] + P[0][0]*SPP[0] - P[2][0]*SPP[1]) + SF[0]*(P[6][2] + P[1][2]*SF[1] + P[3][2]*SF[0] + P[0][2]*SPP[0] - P[2][2]*SPP[1]) + SF[2]*(P[6][3] + P[1][3]*SF[1] + P[3][3]*SF[0] + P[0][3]*SPP[0] - P[2][3]*SPP[1]) - SPP[0]*(P[6][1] + P[1][1]*SF[1] + P[3][1]*SF[0] + P[0][1]*SPP[0] - P[2][1]*SPP[1]);
-    nextP[6][6] = P[6][6] + P[1][6]*SF[1] + P[3][6]*SF[0] + P[0][6]*SPP[0] - P[2][6]*SPP[1] + dvxCov*sq(SG[6] - 2*q0*q2) + dvyCov*sq(SG[5] + 2*q0*q1) + SF[1]*(P[6][1] + P[1][1]*SF[1] + P[3][1]*SF[0] + P[0][1]*SPP[0] - P[2][1]*SPP[1]) + SF[0]*(P[6][3] + P[1][3]*SF[1] + P[3][3]*SF[0] + P[0][3]*SPP[0] - P[2][3]*SPP[1]) + SPP[0]*(P[6][0] + P[1][0]*SF[1] + P[3][0]*SF[0] + P[0][0]*SPP[0] - P[2][0]*SPP[1]) - SPP[1]*(P[6][2] + P[1][2]*SF[1] + P[3][2]*SF[0] + P[0][2]*SPP[0] - P[2][2]*SPP[1]) + dvzCov*sq(SG[1] - SG[2] - SG[3] + SG[4]);
-    nextP[6][7] = P[6][7] + P[1][7]*SF[1] + P[3][7]*SF[0] + P[0][7]*SPP[0] - P[2][7]*SPP[1] + dt*(P[6][4] + P[1][4]*SF[1] + P[3][4]*SF[0] + P[0][4]*SPP[0] - P[2][4]*SPP[1]);
-    nextP[6][8] = P[6][8] + P[1][8]*SF[1] + P[3][8]*SF[0] + P[0][8]*SPP[0] - P[2][8]*SPP[1] + dt*(P[6][5] + P[1][5]*SF[1] + P[3][5]*SF[0] + P[0][5]*SPP[0] - P[2][5]*SPP[1]);
-    nextP[6][9] = P[6][9] + P[1][9]*SF[1] + P[3][9]*SF[0] + P[0][9]*SPP[0] - P[2][9]*SPP[1] + dt*(P[6][6] + P[1][6]*SF[1] + P[3][6]*SF[0] + P[0][6]*SPP[0] - P[2][6]*SPP[1]);
-    nextP[6][10] = P[6][10] + P[1][10]*SF[1] + P[3][10]*SF[0] + P[0][10]*SPP[0] - P[2][10]*SPP[1];
-    nextP[6][11] = P[6][11] + P[1][11]*SF[1] + P[3][11]*SF[0] + P[0][11]*SPP[0] - P[2][11]*SPP[1];
-    nextP[6][12] = P[6][12] + P[1][12]*SF[1] + P[3][12]*SF[0] + P[0][12]*SPP[0] - P[2][12]*SPP[1];
-    nextP[6][13] = P[6][13] + P[1][13]*SF[1] + P[3][13]*SF[0] + P[0][13]*SPP[0] - P[2][13]*SPP[1];
-    nextP[6][14] = P[6][14] + P[1][14]*SF[1] + P[3][14]*SF[0] + P[0][14]*SPP[0] - P[2][14]*SPP[1];
-    nextP[6][15] = P[6][15] + P[1][15]*SF[1] + P[3][15]*SF[0] + P[0][15]*SPP[0] - P[2][15]*SPP[1];
-    nextP[6][16] = P[6][16] + P[1][16]*SF[1] + P[3][16]*SF[0] + P[0][16]*SPP[0] - P[2][16]*SPP[1];
-    nextP[6][17] = P[6][17] + P[1][17]*SF[1] + P[3][17]*SF[0] + P[0][17]*SPP[0] - P[2][17]*SPP[1];
-    nextP[6][18] = P[6][18] + P[1][18]*SF[1] + P[3][18]*SF[0] + P[0][18]*SPP[0] - P[2][18]*SPP[1];
-    nextP[6][19] = P[6][19] + P[1][19]*SF[1] + P[3][19]*SF[0] + P[0][19]*SPP[0] - P[2][19]*SPP[1];
-    nextP[6][20] = P[6][20] + P[1][20]*SF[1] + P[3][20]*SF[0] + P[0][20]*SPP[0] - P[2][20]*SPP[1];
-    nextP[7][0] = P[7][0] + P[4][0]*dt + SF[6]*(P[7][1] + P[4][1]*dt) + SPP[7]*(P[7][2] + P[4][2]*dt) + SPP[6]*(P[7][3] + P[4][3]*dt) + SPP[5]*(P[7][10] + P[4][10]*dt) + SPP[4]*(P[7][11] + P[4][11]*dt) + SPP[3]*(P[7][12] + P[4][12]*dt);
-    nextP[7][1] = P[7][1] + P[4][1]*dt + SF[5]*(P[7][0] + P[4][0]*dt) + SF[4]*(P[7][2] + P[4][2]*dt) + SPP[7]*(P[7][3] + P[4][3]*dt) + SPP[3]*(P[7][11] + P[4][11]*dt) - SPP[4]*(P[7][12] + P[4][12]*dt) - (q0*(P[7][10] + P[4][10]*dt))/2;
-    nextP[7][2] = P[7][2] + P[4][2]*dt + SF[3]*(P[7][0] + P[4][0]*dt) + SF[5]*(P[7][3] + P[4][3]*dt) + SPP[6]*(P[7][1] + P[4][1]*dt) - SPP[3]*(P[7][10] + P[4][10]*dt) + SPP[5]*(P[7][12] + P[4][12]*dt) - (q0*(P[7][11] + P[4][11]*dt))/2;
-    nextP[7][3] = P[7][3] + P[4][3]*dt + SF[4]*(P[7][0] + P[4][0]*dt) + SF[3]*(P[7][1] + P[4][1]*dt) + SF[6]*(P[7][2] + P[4][2]*dt) + SPP[4]*(P[7][10] + P[4][10]*dt) - SPP[5]*(P[7][11] + P[4][11]*dt) - (q0*(P[7][12] + P[4][12]*dt))/2;
-    nextP[7][4] = P[7][4] + P[4][4]*dt + SF[0]*(P[7][1] + P[4][1]*dt) + SF[2]*(P[7][0] + P[4][0]*dt) + SPP[0]*(P[7][2] + P[4][2]*dt) - SPP[2]*(P[7][3] + P[4][3]*dt);
-    nextP[7][5] = P[7][5] + P[4][5]*dt + SF[1]*(P[7][0] + P[4][0]*dt) + SF[0]*(P[7][2] + P[4][2]*dt) + SF[2]*(P[7][3] + P[4][3]*dt) - SPP[0]*(P[7][1] + P[4][1]*dt);
-    nextP[7][6] = P[7][6] + P[4][6]*dt + SF[1]*(P[7][1] + P[4][1]*dt) + SF[0]*(P[7][3] + P[4][3]*dt) + SPP[0]*(P[7][0] + P[4][0]*dt) - SPP[1]*(P[7][2] + P[4][2]*dt);
-    nextP[7][7] = P[7][7] + P[4][7]*dt + dt*(P[7][4] + P[4][4]*dt);
-    nextP[7][8] = P[7][8] + P[4][8]*dt + dt*(P[7][5] + P[4][5]*dt);
-    nextP[7][9] = P[7][9] + P[4][9]*dt + dt*(P[7][6] + P[4][6]*dt);
-    nextP[7][10] = P[7][10] + P[4][10]*dt;
-    nextP[7][11] = P[7][11] + P[4][11]*dt;
-    nextP[7][12] = P[7][12] + P[4][12]*dt;
-    nextP[7][13] = P[7][13] + P[4][13]*dt;
-    nextP[7][14] = P[7][14] + P[4][14]*dt;
-    nextP[7][15] = P[7][15] + P[4][15]*dt;
-    nextP[7][16] = P[7][16] + P[4][16]*dt;
-    nextP[7][17] = P[7][17] + P[4][17]*dt;
-    nextP[7][18] = P[7][18] + P[4][18]*dt;
-    nextP[7][19] = P[7][19] + P[4][19]*dt;
-    nextP[7][20] = P[7][20] + P[4][20]*dt;
-    nextP[8][0] = P[8][0] + P[5][0]*dt + SF[6]*(P[8][1] + P[5][1]*dt) + SPP[7]*(P[8][2] + P[5][2]*dt) + SPP[6]*(P[8][3] + P[5][3]*dt) + SPP[5]*(P[8][10] + P[5][10]*dt) + SPP[4]*(P[8][11] + P[5][11]*dt) + SPP[3]*(P[8][12] + P[5][12]*dt);
-    nextP[8][1] = P[8][1] + P[5][1]*dt + SF[5]*(P[8][0] + P[5][0]*dt) + SF[4]*(P[8][2] + P[5][2]*dt) + SPP[7]*(P[8][3] + P[5][3]*dt) + SPP[3]*(P[8][11] + P[5][11]*dt) - SPP[4]*(P[8][12] + P[5][12]*dt) - (q0*(P[8][10] + P[5][10]*dt))/2;
-    nextP[8][2] = P[8][2] + P[5][2]*dt + SF[3]*(P[8][0] + P[5][0]*dt) + SF[5]*(P[8][3] + P[5][3]*dt) + SPP[6]*(P[8][1] + P[5][1]*dt) - SPP[3]*(P[8][10] + P[5][10]*dt) + SPP[5]*(P[8][12] + P[5][12]*dt) - (q0*(P[8][11] + P[5][11]*dt))/2;
-    nextP[8][3] = P[8][3] + P[5][3]*dt + SF[4]*(P[8][0] + P[5][0]*dt) + SF[3]*(P[8][1] + P[5][1]*dt) + SF[6]*(P[8][2] + P[5][2]*dt) + SPP[4]*(P[8][10] + P[5][10]*dt) - SPP[5]*(P[8][11] + P[5][11]*dt) - (q0*(P[8][12] + P[5][12]*dt))/2;
-    nextP[8][4] = P[8][4] + P[5][4]*dt + SF[0]*(P[8][1] + P[5][1]*dt) + SF[2]*(P[8][0] + P[5][0]*dt) + SPP[0]*(P[8][2] + P[5][2]*dt) - SPP[2]*(P[8][3] + P[5][3]*dt);
-    nextP[8][5] = P[8][5] + P[5][5]*dt + SF[1]*(P[8][0] + P[5][0]*dt) + SF[0]*(P[8][2] + P[5][2]*dt) + SF[2]*(P[8][3] + P[5][3]*dt) - SPP[0]*(P[8][1] + P[5][1]*dt);
-    nextP[8][6] = P[8][6] + P[5][6]*dt + SF[1]*(P[8][1] + P[5][1]*dt) + SF[0]*(P[8][3] + P[5][3]*dt) + SPP[0]*(P[8][0] + P[5][0]*dt) - SPP[1]*(P[8][2] + P[5][2]*dt);
-    nextP[8][7] = P[8][7] + P[5][7]*dt + dt*(P[8][4] + P[5][4]*dt);
-    nextP[8][8] = P[8][8] + P[5][8]*dt + dt*(P[8][5] + P[5][5]*dt);
-    nextP[8][9] = P[8][9] + P[5][9]*dt + dt*(P[8][6] + P[5][6]*dt);
-    nextP[8][10] = P[8][10] + P[5][10]*dt;
-    nextP[8][11] = P[8][11] + P[5][11]*dt;
-    nextP[8][12] = P[8][12] + P[5][12]*dt;
-    nextP[8][13] = P[8][13] + P[5][13]*dt;
-    nextP[8][14] = P[8][14] + P[5][14]*dt;
-    nextP[8][15] = P[8][15] + P[5][15]*dt;
-    nextP[8][16] = P[8][16] + P[5][16]*dt;
-    nextP[8][17] = P[8][17] + P[5][17]*dt;
-    nextP[8][18] = P[8][18] + P[5][18]*dt;
-    nextP[8][19] = P[8][19] + P[5][19]*dt;
-    nextP[8][20] = P[8][20] + P[5][20]*dt;
-    nextP[9][0] = P[9][0] + P[6][0]*dt + SF[6]*(P[9][1] + P[6][1]*dt) + SPP[7]*(P[9][2] + P[6][2]*dt) + SPP[6]*(P[9][3] + P[6][3]*dt) + SPP[5]*(P[9][10] + P[6][10]*dt) + SPP[4]*(P[9][11] + P[6][11]*dt) + SPP[3]*(P[9][12] + P[6][12]*dt);
-    nextP[9][1] = P[9][1] + P[6][1]*dt + SF[5]*(P[9][0] + P[6][0]*dt) + SF[4]*(P[9][2] + P[6][2]*dt) + SPP[7]*(P[9][3] + P[6][3]*dt) + SPP[3]*(P[9][11] + P[6][11]*dt) - SPP[4]*(P[9][12] + P[6][12]*dt) - (q0*(P[9][10] + P[6][10]*dt))/2;
-    nextP[9][2] = P[9][2] + P[6][2]*dt + SF[3]*(P[9][0] + P[6][0]*dt) + SF[5]*(P[9][3] + P[6][3]*dt) + SPP[6]*(P[9][1] + P[6][1]*dt) - SPP[3]*(P[9][10] + P[6][10]*dt) + SPP[5]*(P[9][12] + P[6][12]*dt) - (q0*(P[9][11] + P[6][11]*dt))/2;
-    nextP[9][3] = P[9][3] + P[6][3]*dt + SF[4]*(P[9][0] + P[6][0]*dt) + SF[3]*(P[9][1] + P[6][1]*dt) + SF[6]*(P[9][2] + P[6][2]*dt) + SPP[4]*(P[9][10] + P[6][10]*dt) - SPP[5]*(P[9][11] + P[6][11]*dt) - (q0*(P[9][12] + P[6][12]*dt))/2;
-    nextP[9][4] = P[9][4] + P[6][4]*dt + SF[0]*(P[9][1] + P[6][1]*dt) + SF[2]*(P[9][0] + P[6][0]*dt) + SPP[0]*(P[9][2] + P[6][2]*dt) - SPP[2]*(P[9][3] + P[6][3]*dt);
-    nextP[9][5] = P[9][5] + P[6][5]*dt + SF[1]*(P[9][0] + P[6][0]*dt) + SF[0]*(P[9][2] + P[6][2]*dt) + SF[2]*(P[9][3] + P[6][3]*dt) - SPP[0]*(P[9][1] + P[6][1]*dt);
-    nextP[9][6] = P[9][6] + P[6][6]*dt + SF[1]*(P[9][1] + P[6][1]*dt) + SF[0]*(P[9][3] + P[6][3]*dt) + SPP[0]*(P[9][0] + P[6][0]*dt) - SPP[1]*(P[9][2] + P[6][2]*dt);
-    nextP[9][7] = P[9][7] + P[6][7]*dt + dt*(P[9][4] + P[6][4]*dt);
-    nextP[9][8] = P[9][8] + P[6][8]*dt + dt*(P[9][5] + P[6][5]*dt);
-    nextP[9][9] = P[9][9] + P[6][9]*dt + dt*(P[9][6] + P[6][6]*dt);
-    nextP[9][10] = P[9][10] + P[6][10]*dt;
-    nextP[9][11] = P[9][11] + P[6][11]*dt;
-    nextP[9][12] = P[9][12] + P[6][12]*dt;
-    nextP[9][13] = P[9][13] + P[6][13]*dt;
-    nextP[9][14] = P[9][14] + P[6][14]*dt;
-    nextP[9][15] = P[9][15] + P[6][15]*dt;
-    nextP[9][16] = P[9][16] + P[6][16]*dt;
-    nextP[9][17] = P[9][17] + P[6][17]*dt;
-    nextP[9][18] = P[9][18] + P[6][18]*dt;
-    nextP[9][19] = P[9][19] + P[6][19]*dt;
-    nextP[9][20] = P[9][20] + P[6][20]*dt;
-    nextP[10][0] = P[10][0] + P[10][1]*SF[6] + P[10][2]*SPP[7] + P[10][3]*SPP[6] + P[10][10]*SPP[5] + P[10][11]*SPP[4] + P[10][12]*SPP[3];
-    nextP[10][1] = P[10][1] + P[10][0]*SF[5] + P[10][2]*SF[4] + P[10][3]*SPP[7] + P[10][11]*SPP[3] - P[10][12]*SPP[4] - (P[10][10]*q0)/2;
-    nextP[10][2] = P[10][2] + P[10][0]*SF[3] + P[10][3]*SF[5] + P[10][1]*SPP[6] - P[10][10]*SPP[3] + P[10][12]*SPP[5] - (P[10][11]*q0)/2;
-    nextP[10][3] = P[10][3] + P[10][0]*SF[4] + P[10][1]*SF[3] + P[10][2]*SF[6] + P[10][10]*SPP[4] - P[10][11]*SPP[5] - (P[10][12]*q0)/2;
-    nextP[10][4] = P[10][4] + P[10][1]*SF[0] + P[10][0]*SF[2] + P[10][2]*SPP[0] - P[10][3]*SPP[2];
-    nextP[10][5] = P[10][5] + P[10][0]*SF[1] + P[10][2]*SF[0] + P[10][3]*SF[2] - P[10][1]*SPP[0];
-    nextP[10][6] = P[10][6] + P[10][1]*SF[1] + P[10][3]*SF[0] + P[10][0]*SPP[0] - P[10][2]*SPP[1];
-    nextP[10][7] = P[10][7] + P[10][4]*dt;
-    nextP[10][8] = P[10][8] + P[10][5]*dt;
-    nextP[10][9] = P[10][9] + P[10][6]*dt;
-    nextP[10][10] = P[10][10];
-    nextP[10][11] = P[10][11];
-    nextP[10][12] = P[10][12];
-    nextP[10][13] = P[10][13];
-    nextP[10][14] = P[10][14];
-    nextP[10][15] = P[10][15];
-    nextP[10][16] = P[10][16];
-    nextP[10][17] = P[10][17];
-    nextP[10][18] = P[10][18];
-    nextP[10][19] = P[10][19];
-    nextP[10][20] = P[10][20];
-    nextP[11][0] = P[11][0] + P[11][1]*SF[6] + P[11][2]*SPP[7] + P[11][3]*SPP[6] + P[11][10]*SPP[5] + P[11][11]*SPP[4] + P[11][12]*SPP[3];
-    nextP[11][1] = P[11][1] + P[11][0]*SF[5] + P[11][2]*SF[4] + P[11][3]*SPP[7] + P[11][11]*SPP[3] - P[11][12]*SPP[4] - (P[11][10]*q0)/2;
-    nextP[11][2] = P[11][2] + P[11][0]*SF[3] + P[11][3]*SF[5] + P[11][1]*SPP[6] - P[11][10]*SPP[3] + P[11][12]*SPP[5] - (P[11][11]*q0)/2;
-    nextP[11][3] = P[11][3] + P[11][0]*SF[4] + P[11][1]*SF[3] + P[11][2]*SF[6] + P[11][10]*SPP[4] - P[11][11]*SPP[5] - (P[11][12]*q0)/2;
-    nextP[11][4] = P[11][4] + P[11][1]*SF[0] + P[11][0]*SF[2] + P[11][2]*SPP[0] - P[11][3]*SPP[2];
-    nextP[11][5] = P[11][5] + P[11][0]*SF[1] + P[11][2]*SF[0] + P[11][3]*SF[2] - P[11][1]*SPP[0];
-    nextP[11][6] = P[11][6] + P[11][1]*SF[1] + P[11][3]*SF[0] + P[11][0]*SPP[0] - P[11][2]*SPP[1];
-    nextP[11][7] = P[11][7] + P[11][4]*dt;
-    nextP[11][8] = P[11][8] + P[11][5]*dt;
-    nextP[11][9] = P[11][9] + P[11][6]*dt;
-    nextP[11][10] = P[11][10];
-    nextP[11][11] = P[11][11];
-    nextP[11][12] = P[11][12];
-    nextP[11][13] = P[11][13];
-    nextP[11][14] = P[11][14];
-    nextP[11][15] = P[11][15];
-    nextP[11][16] = P[11][16];
-    nextP[11][17] = P[11][17];
-    nextP[11][18] = P[11][18];
-    nextP[11][19] = P[11][19];
-    nextP[11][20] = P[11][20];
-    nextP[12][0] = P[12][0] + P[12][1]*SF[6] + P[12][2]*SPP[7] + P[12][3]*SPP[6] + P[12][10]*SPP[5] + P[12][11]*SPP[4] + P[12][12]*SPP[3];
-    nextP[12][1] = P[12][1] + P[12][0]*SF[5] + P[12][2]*SF[4] + P[12][3]*SPP[7] + P[12][11]*SPP[3] - P[12][12]*SPP[4] - (P[12][10]*q0)/2;
-    nextP[12][2] = P[12][2] + P[12][0]*SF[3] + P[12][3]*SF[5] + P[12][1]*SPP[6] - P[12][10]*SPP[3] + P[12][12]*SPP[5] - (P[12][11]*q0)/2;
-    nextP[12][3] = P[12][3] + P[12][0]*SF[4] + P[12][1]*SF[3] + P[12][2]*SF[6] + P[12][10]*SPP[4] - P[12][11]*SPP[5] - (P[12][12]*q0)/2;
-    nextP[12][4] = P[12][4] + P[12][1]*SF[0] + P[12][0]*SF[2] + P[12][2]*SPP[0] - P[12][3]*SPP[2];
-    nextP[12][5] = P[12][5] + P[12][0]*SF[1] + P[12][2]*SF[0] + P[12][3]*SF[2] - P[12][1]*SPP[0];
-    nextP[12][6] = P[12][6] + P[12][1]*SF[1] + P[12][3]*SF[0] + P[12][0]*SPP[0] - P[12][2]*SPP[1];
-    nextP[12][7] = P[12][7] + P[12][4]*dt;
-    nextP[12][8] = P[12][8] + P[12][5]*dt;
-    nextP[12][9] = P[12][9] + P[12][6]*dt;
-    nextP[12][10] = P[12][10];
-    nextP[12][11] = P[12][11];
-    nextP[12][12] = P[12][12];
-    nextP[12][13] = P[12][13];
-    nextP[12][14] = P[12][14];
-    nextP[12][15] = P[12][15];
-    nextP[12][16] = P[12][16];
-    nextP[12][17] = P[12][17];
-    nextP[12][18] = P[12][18];
-    nextP[12][19] = P[12][19];
-    nextP[12][20] = P[12][20];
-    nextP[13][0] = P[13][0] + P[13][1]*SF[6] + P[13][2]*SPP[7] + P[13][3]*SPP[6] + P[13][10]*SPP[5] + P[13][11]*SPP[4] + P[13][12]*SPP[3];
-    nextP[13][1] = P[13][1] + P[13][0]*SF[5] + P[13][2]*SF[4] + P[13][3]*SPP[7] + P[13][11]*SPP[3] - P[13][12]*SPP[4] - (P[13][10]*q0)/2;
-    nextP[13][2] = P[13][2] + P[13][0]*SF[3] + P[13][3]*SF[5] + P[13][1]*SPP[6] - P[13][10]*SPP[3] + P[13][12]*SPP[5] - (P[13][11]*q0)/2;
-    nextP[13][3] = P[13][3] + P[13][0]*SF[4] + P[13][1]*SF[3] + P[13][2]*SF[6] + P[13][10]*SPP[4] - P[13][11]*SPP[5] - (P[13][12]*q0)/2;
-    nextP[13][4] = P[13][4] + P[13][1]*SF[0] + P[13][0]*SF[2] + P[13][2]*SPP[0] - P[13][3]*SPP[2];
-    nextP[13][5] = P[13][5] + P[13][0]*SF[1] + P[13][2]*SF[0] + P[13][3]*SF[2] - P[13][1]*SPP[0];
-    nextP[13][6] = P[13][6] + P[13][1]*SF[1] + P[13][3]*SF[0] + P[13][0]*SPP[0] - P[13][2]*SPP[1];
-    nextP[13][7] = P[13][7] + P[13][4]*dt;
-    nextP[13][8] = P[13][8] + P[13][5]*dt;
-    nextP[13][9] = P[13][9] + P[13][6]*dt;
-    nextP[13][10] = P[13][10];
-    nextP[13][11] = P[13][11];
-    nextP[13][12] = P[13][12];
-    nextP[13][13] = P[13][13];
-    nextP[13][14] = P[13][14];
-    nextP[13][15] = P[13][15];
-    nextP[13][16] = P[13][16];
-    nextP[13][17] = P[13][17];
-    nextP[13][18] = P[13][18];
-    nextP[13][19] = P[13][19];
-    nextP[13][20] = P[13][20];
-    nextP[14][0] = P[14][0] + P[14][1]*SF[6] + P[14][2]*SPP[7] + P[14][3]*SPP[6] + P[14][10]*SPP[5] + P[14][11]*SPP[4] + P[14][12]*SPP[3];
-    nextP[14][1] = P[14][1] + P[14][0]*SF[5] + P[14][2]*SF[4] + P[14][3]*SPP[7] + P[14][11]*SPP[3] - P[14][12]*SPP[4] - (P[14][10]*q0)/2;
-    nextP[14][2] = P[14][2] + P[14][0]*SF[3] + P[14][3]*SF[5] + P[14][1]*SPP[6] - P[14][10]*SPP[3] + P[14][12]*SPP[5] - (P[14][11]*q0)/2;
-    nextP[14][3] = P[14][3] + P[14][0]*SF[4] + P[14][1]*SF[3] + P[14][2]*SF[6] + P[14][10]*SPP[4] - P[14][11]*SPP[5] - (P[14][12]*q0)/2;
-    nextP[14][4] = P[14][4] + P[14][1]*SF[0] + P[14][0]*SF[2] + P[14][2]*SPP[0] - P[14][3]*SPP[2];
-    nextP[14][5] = P[14][5] + P[14][0]*SF[1] + P[14][2]*SF[0] + P[14][3]*SF[2] - P[14][1]*SPP[0];
-    nextP[14][6] = P[14][6] + P[14][1]*SF[1] + P[14][3]*SF[0] + P[14][0]*SPP[0] - P[14][2]*SPP[1];
-    nextP[14][7] = P[14][7] + P[14][4]*dt;
-    nextP[14][8] = P[14][8] + P[14][5]*dt;
-    nextP[14][9] = P[14][9] + P[14][6]*dt;
-    nextP[14][10] = P[14][10];
-    nextP[14][11] = P[14][11];
-    nextP[14][12] = P[14][12];
-    nextP[14][13] = P[14][13];
-    nextP[14][14] = P[14][14];
-    nextP[14][15] = P[14][15];
-    nextP[14][16] = P[14][16];
-    nextP[14][17] = P[14][17];
-    nextP[14][18] = P[14][18];
-    nextP[14][19] = P[14][19];
-    nextP[14][20] = P[14][20];
-    nextP[15][0] = P[15][0] + P[15][1]*SF[6] + P[15][2]*SPP[7] + P[15][3]*SPP[6] + P[15][10]*SPP[5] + P[15][11]*SPP[4] + P[15][12]*SPP[3];
-    nextP[15][1] = P[15][1] + P[15][0]*SF[5] + P[15][2]*SF[4] + P[15][3]*SPP[7] + P[15][11]*SPP[3] - P[15][12]*SPP[4] - (P[15][10]*q0)/2;
-    nextP[15][2] = P[15][2] + P[15][0]*SF[3] + P[15][3]*SF[5] + P[15][1]*SPP[6] - P[15][10]*SPP[3] + P[15][12]*SPP[5] - (P[15][11]*q0)/2;
-    nextP[15][3] = P[15][3] + P[15][0]*SF[4] + P[15][1]*SF[3] + P[15][2]*SF[6] + P[15][10]*SPP[4] - P[15][11]*SPP[5] - (P[15][12]*q0)/2;
-    nextP[15][4] = P[15][4] + P[15][1]*SF[0] + P[15][0]*SF[2] + P[15][2]*SPP[0] - P[15][3]*SPP[2];
-    nextP[15][5] = P[15][5] + P[15][0]*SF[1] + P[15][2]*SF[0] + P[15][3]*SF[2] - P[15][1]*SPP[0];
-    nextP[15][6] = P[15][6] + P[15][1]*SF[1] + P[15][3]*SF[0] + P[15][0]*SPP[0] - P[15][2]*SPP[1];
-    nextP[15][7] = P[15][7] + P[15][4]*dt;
-    nextP[15][8] = P[15][8] + P[15][5]*dt;
-    nextP[15][9] = P[15][9] + P[15][6]*dt;
-    nextP[15][10] = P[15][10];
-    nextP[15][11] = P[15][11];
-    nextP[15][12] = P[15][12];
-    nextP[15][13] = P[15][13];
-    nextP[15][14] = P[15][14];
-    nextP[15][15] = P[15][15];
-    nextP[15][16] = P[15][16];
-    nextP[15][17] = P[15][17];
-    nextP[15][18] = P[15][18];
-    nextP[15][19] = P[15][19];
-    nextP[15][20] = P[15][20];
-    nextP[16][0] = P[16][0] + P[16][1]*SF[6] + P[16][2]*SPP[7] + P[16][3]*SPP[6] + P[16][10]*SPP[5] + P[16][11]*SPP[4] + P[16][12]*SPP[3];
-    nextP[16][1] = P[16][1] + P[16][0]*SF[5] + P[16][2]*SF[4] + P[16][3]*SPP[7] + P[16][11]*SPP[3] - P[16][12]*SPP[4] - (P[16][10]*q0)/2;
-    nextP[16][2] = P[16][2] + P[16][0]*SF[3] + P[16][3]*SF[5] + P[16][1]*SPP[6] - P[16][10]*SPP[3] + P[16][12]*SPP[5] - (P[16][11]*q0)/2;
-    nextP[16][3] = P[16][3] + P[16][0]*SF[4] + P[16][1]*SF[3] + P[16][2]*SF[6] + P[16][10]*SPP[4] - P[16][11]*SPP[5] - (P[16][12]*q0)/2;
-    nextP[16][4] = P[16][4] + P[16][1]*SF[0] + P[16][0]*SF[2] + P[16][2]*SPP[0] - P[16][3]*SPP[2];
-    nextP[16][5] = P[16][5] + P[16][0]*SF[1] + P[16][2]*SF[0] + P[16][3]*SF[2] - P[16][1]*SPP[0];
-    nextP[16][6] = P[16][6] + P[16][1]*SF[1] + P[16][3]*SF[0] + P[16][0]*SPP[0] - P[16][2]*SPP[1];
-    nextP[16][7] = P[16][7] + P[16][4]*dt;
-    nextP[16][8] = P[16][8] + P[16][5]*dt;
-    nextP[16][9] = P[16][9] + P[16][6]*dt;
-    nextP[16][10] = P[16][10];
-    nextP[16][11] = P[16][11];
-    nextP[16][12] = P[16][12];
-    nextP[16][13] = P[16][13];
-    nextP[16][14] = P[16][14];
-    nextP[16][15] = P[16][15];
-    nextP[16][16] = P[16][16];
-    nextP[16][17] = P[16][17];
-    nextP[16][18] = P[16][18];
-    nextP[16][19] = P[16][19];
-    nextP[16][20] = P[16][20];
-    nextP[17][0] = P[17][0] + P[17][1]*SF[6] + P[17][2]*SPP[7] + P[17][3]*SPP[6] + P[17][10]*SPP[5] + P[17][11]*SPP[4] + P[17][12]*SPP[3];
-    nextP[17][1] = P[17][1] + P[17][0]*SF[5] + P[17][2]*SF[4] + P[17][3]*SPP[7] + P[17][11]*SPP[3] - P[17][12]*SPP[4] - (P[17][10]*q0)/2;
-    nextP[17][2] = P[17][2] + P[17][0]*SF[3] + P[17][3]*SF[5] + P[17][1]*SPP[6] - P[17][10]*SPP[3] + P[17][12]*SPP[5] - (P[17][11]*q0)/2;
-    nextP[17][3] = P[17][3] + P[17][0]*SF[4] + P[17][1]*SF[3] + P[17][2]*SF[6] + P[17][10]*SPP[4] - P[17][11]*SPP[5] - (P[17][12]*q0)/2;
-    nextP[17][4] = P[17][4] + P[17][1]*SF[0] + P[17][0]*SF[2] + P[17][2]*SPP[0] - P[17][3]*SPP[2];
-    nextP[17][5] = P[17][5] + P[17][0]*SF[1] + P[17][2]*SF[0] + P[17][3]*SF[2] - P[17][1]*SPP[0];
-    nextP[17][6] = P[17][6] + P[17][1]*SF[1] + P[17][3]*SF[0] + P[17][0]*SPP[0] - P[17][2]*SPP[1];
-    nextP[17][7] = P[17][7] + P[17][4]*dt;
-    nextP[17][8] = P[17][8] + P[17][5]*dt;
-    nextP[17][9] = P[17][9] + P[17][6]*dt;
-    nextP[17][10] = P[17][10];
-    nextP[17][11] = P[17][11];
-    nextP[17][12] = P[17][12];
-    nextP[17][13] = P[17][13];
-    nextP[17][14] = P[17][14];
-    nextP[17][15] = P[17][15];
-    nextP[17][16] = P[17][16];
-    nextP[17][17] = P[17][17];
-    nextP[17][18] = P[17][18];
-    nextP[17][19] = P[17][19];
-    nextP[17][20] = P[17][20];
-    nextP[18][0] = P[18][0] + P[18][1]*SF[6] + P[18][2]*SPP[7] + P[18][3]*SPP[6] + P[18][10]*SPP[5] + P[18][11]*SPP[4] + P[18][12]*SPP[3];
-    nextP[18][1] = P[18][1] + P[18][0]*SF[5] + P[18][2]*SF[4] + P[18][3]*SPP[7] + P[18][11]*SPP[3] - P[18][12]*SPP[4] - (P[18][10]*q0)/2;
-    nextP[18][2] = P[18][2] + P[18][0]*SF[3] + P[18][3]*SF[5] + P[18][1]*SPP[6] - P[18][10]*SPP[3] + P[18][12]*SPP[5] - (P[18][11]*q0)/2;
-    nextP[18][3] = P[18][3] + P[18][0]*SF[4] + P[18][1]*SF[3] + P[18][2]*SF[6] + P[18][10]*SPP[4] - P[18][11]*SPP[5] - (P[18][12]*q0)/2;
-    nextP[18][4] = P[18][4] + P[18][1]*SF[0] + P[18][0]*SF[2] + P[18][2]*SPP[0] - P[18][3]*SPP[2];
-    nextP[18][5] = P[18][5] + P[18][0]*SF[1] + P[18][2]*SF[0] + P[18][3]*SF[2] - P[18][1]*SPP[0];
-    nextP[18][6] = P[18][6] + P[18][1]*SF[1] + P[18][3]*SF[0] + P[18][0]*SPP[0] - P[18][2]*SPP[1];
-    nextP[18][7] = P[18][7] + P[18][4]*dt;
-    nextP[18][8] = P[18][8] + P[18][5]*dt;
-    nextP[18][9] = P[18][9] + P[18][6]*dt;
-    nextP[18][10] = P[18][10];
-    nextP[18][11] = P[18][11];
-    nextP[18][12] = P[18][12];
-    nextP[18][13] = P[18][13];
-    nextP[18][14] = P[18][14];
-    nextP[18][15] = P[18][15];
-    nextP[18][16] = P[18][16];
-    nextP[18][17] = P[18][17];
-    nextP[18][18] = P[18][18];
-    nextP[18][19] = P[18][19];
-    nextP[18][20] = P[18][20];
-    nextP[19][0] = P[19][0] + P[19][1]*SF[6] + P[19][2]*SPP[7] + P[19][3]*SPP[6] + P[19][10]*SPP[5] + P[19][11]*SPP[4] + P[19][12]*SPP[3];
-    nextP[19][1] = P[19][1] + P[19][0]*SF[5] + P[19][2]*SF[4] + P[19][3]*SPP[7] + P[19][11]*SPP[3] - P[19][12]*SPP[4] - (P[19][10]*q0)/2;
-    nextP[19][2] = P[19][2] + P[19][0]*SF[3] + P[19][3]*SF[5] + P[19][1]*SPP[6] - P[19][10]*SPP[3] + P[19][12]*SPP[5] - (P[19][11]*q0)/2;
-    nextP[19][3] = P[19][3] + P[19][0]*SF[4] + P[19][1]*SF[3] + P[19][2]*SF[6] + P[19][10]*SPP[4] - P[19][11]*SPP[5] - (P[19][12]*q0)/2;
-    nextP[19][4] = P[19][4] + P[19][1]*SF[0] + P[19][0]*SF[2] + P[19][2]*SPP[0] - P[19][3]*SPP[2];
-    nextP[19][5] = P[19][5] + P[19][0]*SF[1] + P[19][2]*SF[0] + P[19][3]*SF[2] - P[19][1]*SPP[0];
-    nextP[19][6] = P[19][6] + P[19][1]*SF[1] + P[19][3]*SF[0] + P[19][0]*SPP[0] - P[19][2]*SPP[1];
-    nextP[19][7] = P[19][7] + P[19][4]*dt;
-    nextP[19][8] = P[19][8] + P[19][5]*dt;
-    nextP[19][9] = P[19][9] + P[19][6]*dt;
-    nextP[19][10] = P[19][10];
-    nextP[19][11] = P[19][11];
-    nextP[19][12] = P[19][12];
-    nextP[19][13] = P[19][13];
-    nextP[19][14] = P[19][14];
-    nextP[19][15] = P[19][15];
-    nextP[19][16] = P[19][16];
-    nextP[19][17] = P[19][17];
-    nextP[19][18] = P[19][18];
-    nextP[19][19] = P[19][19];
-    nextP[19][20] = P[19][20];
-    nextP[20][0] = P[20][0] + P[20][1]*SF[6] + P[20][2]*SPP[7] + P[20][3]*SPP[6] + P[20][10]*SPP[5] + P[20][11]*SPP[4] + P[20][12]*SPP[3];
-    nextP[20][1] = P[20][1] + P[20][0]*SF[5] + P[20][2]*SF[4] + P[20][3]*SPP[7] + P[20][11]*SPP[3] - P[20][12]*SPP[4] - (P[20][10]*q0)/2;
-    nextP[20][2] = P[20][2] + P[20][0]*SF[3] + P[20][3]*SF[5] + P[20][1]*SPP[6] - P[20][10]*SPP[3] + P[20][12]*SPP[5] - (P[20][11]*q0)/2;
-    nextP[20][3] = P[20][3] + P[20][0]*SF[4] + P[20][1]*SF[3] + P[20][2]*SF[6] + P[20][10]*SPP[4] - P[20][11]*SPP[5] - (P[20][12]*q0)/2;
-    nextP[20][4] = P[20][4] + P[20][1]*SF[0] + P[20][0]*SF[2] + P[20][2]*SPP[0] - P[20][3]*SPP[2];
-    nextP[20][5] = P[20][5] + P[20][0]*SF[1] + P[20][2]*SF[0] + P[20][3]*SF[2] - P[20][1]*SPP[0];
-    nextP[20][6] = P[20][6] + P[20][1]*SF[1] + P[20][3]*SF[0] + P[20][0]*SPP[0] - P[20][2]*SPP[1];
-    nextP[20][7] = P[20][7] + P[20][4]*dt;
-    nextP[20][8] = P[20][8] + P[20][5]*dt;
-    nextP[20][9] = P[20][9] + P[20][6]*dt;
-    nextP[20][10] = P[20][10];
-    nextP[20][11] = P[20][11];
-    nextP[20][12] = P[20][12];
-    nextP[20][13] = P[20][13];
-    nextP[20][14] = P[20][14];
-    nextP[20][15] = P[20][15];
-    nextP[20][16] = P[20][16];
-    nextP[20][17] = P[20][17];
-    nextP[20][18] = P[20][18];
-    nextP[20][19] = P[20][19];
-    nextP[20][20] = P[20][20];
-
-    for (unsigned i = 0; i < n_states; i++)
-    {
-        nextP[i][i] = nextP[i][i] + processNoise[i];
-    }
-
-    // If on ground or no magnetometer fitted, inhibit magnetometer bias updates by
-    // setting the coresponding covariance terms to zero.
-    if (onGround || !useCompass)
-    {
-        zeroRows(nextP,15,20);
-        zeroCols(nextP,15,20);
-    }
-
-    // If on ground or not using airspeed sensing, inhibit wind velocity
-    // covariance growth.
-    if (onGround || !useAirspeed)
-    {
-        zeroRows(nextP,13,14);
-        zeroCols(nextP,13,14);
-    }
-
-    // If the total position variance exceds 1E6 (1000m), then stop covariance
-    // growth by setting the predicted to the previous values
-    // This prevent an ill conditioned matrix from occurring for long periods
-    // without GPS
-    if ((P[7][7] + P[8][8]) > 1E6f)
-    {
-        for (uint8_t i=7; i<=8; i++)
-        {
-            for (unsigned j = 0; j < n_states; j++)
-            {
-                nextP[i][j] = P[i][j];
-                nextP[j][i] = P[j][i];
-            }
-        }
-    }
-
-    if (onGround || staticMode) {
-        // copy the portion of the variances we want to
-        // propagate
-        for (unsigned i = 0; i < 14; i++) {
-            P[i][i] = nextP[i][i];
-
-            // force symmetry for observable states
-            // force zero for non-observable states
-            for (unsigned i = 1; i < n_states; i++)
-            {
-                for (uint8_t j = 0; j < i; j++)
-                {
-                    if ((i > 12) || (j > 12)) {
-                        P[i][j] = 0.0f;
-                    } else {
-                        P[i][j] = 0.5f * (nextP[i][j] + nextP[j][i]);
-                    }
-                    P[j][i] = P[i][j];
-                }
-            }
-        }
-
-    } else {
-
-        // Copy covariance
-        for (unsigned i = 0; i < n_states; i++) {
-            P[i][i] = nextP[i][i];
-        }
-
-        // force symmetry for observable states
-        for (unsigned i = 1; i < n_states; i++)
-        {
-            for (uint8_t j = 0; j < i; j++)
-            {
-                P[i][j] = 0.5f * (nextP[i][j] + nextP[j][i]);
-                P[j][i] = P[i][j];
-            }
-        }
-
-    }
-
-    ConstrainVariances();
-}
-
-void AttPosEKF::FuseVelposNED()
-{
-
-// declare variables used by fault isolation logic
-    uint32_t gpsRetryTime = 30000; // time in msec before GPS fusion will be retried following innovation consistency failure
-    uint32_t gpsRetryTimeNoTAS = 5000; // retry time if no TAS measurement available
-    uint32_t hgtRetryTime = 5000; // height measurement retry time
-    uint32_t horizRetryTime;
-
-// declare variables used to check measurement errors
-    float velInnov[3] = {0.0f,0.0f,0.0f};
-    float posInnov[2] = {0.0f,0.0f};
-    float hgtInnov = 0.0f;
-
-// declare variables used to control access to arrays
-    bool fuseData[6] = {false,false,false,false,false,false};
-    uint8_t stateIndex;
-    uint8_t obsIndex;
-    uint8_t indexLimit;
-
-// declare variables used by state and covariance update calculations
-    float velErr;
-    float posErr;
-    float R_OBS[6];
-    float observation[6];
-    float SK;
-    float quatMag;
-
-// Perform sequential fusion of GPS measurements. This assumes that the
-// errors in the different velocity and position components are
-// uncorrelated which is not true, however in the absence of covariance
-// data from the GPS receiver it is the only assumption we can make
-// so we might as well take advantage of the computational efficiencies
-// associated with sequential fusion
-    if (fuseVelData || fusePosData || fuseHgtData)
-    {
-        // set the GPS data timeout depending on whether airspeed data is present
-        if (useAirspeed) horizRetryTime = gpsRetryTime;
-        else horizRetryTime = gpsRetryTimeNoTAS;
-
-        // Form the observation vector
-        for (uint8_t i=0; i<=2; i++) observation[i] = velNED[i];
-        for (uint8_t i=3; i<=4; i++) observation[i] = posNE[i-3];
-        observation[5] = -(hgtMea);
-
-        // Estimate the GPS Velocity, GPS horiz position and height measurement variances.
-        velErr = 0.2f*accNavMag; // additional error in GPS velocities caused by manoeuvring
-        posErr = 0.2f*accNavMag; // additional error in GPS position caused by manoeuvring
-        R_OBS[0] = 0.04f + sq(velErr);
-        R_OBS[1] = R_OBS[0];
-        R_OBS[2] = 0.08f + sq(velErr);
-        R_OBS[3] = R_OBS[2];
-        R_OBS[4] = 4.0f  + sq(posErr);
-        R_OBS[5] = 4.0f;
-
-        // Set innovation variances to zero default
-        for (uint8_t i = 0; i<=5; i++)
-        {
-            varInnovVelPos[i] = 0.0f;
-        }
-        // calculate innovations and check GPS data validity using an innovation consistency check
-        if (fuseVelData)
-        {
-            // test velocity measurements
-            uint8_t imax = 2;
-            if (fusionModeGPS == 1) imax = 1;
-            for (uint8_t i = 0; i<=imax; i++)
-            {
-                velInnov[i] = statesAtVelTime[i+4] - velNED[i];
-                stateIndex = 4 + i;
-                varInnovVelPos[i] = P[stateIndex][stateIndex] + R_OBS[i];
-            }
-            // apply a 5-sigma threshold
-            current_ekf_state.velHealth = (sq(velInnov[0]) + sq(velInnov[1]) + sq(velInnov[2])) < 25.0f * (varInnovVelPos[0] + varInnovVelPos[1] + varInnovVelPos[2]);
-            current_ekf_state.velTimeout = (millis() - current_ekf_state.velFailTime) > horizRetryTime;
-            if (current_ekf_state.velHealth || current_ekf_state.velTimeout)
-            {
-                current_ekf_state.velHealth = true;
-                current_ekf_state.velFailTime = millis();
-            }
-            else
-            {
-                current_ekf_state.velHealth = false;
-            }
-        }
-        if (fusePosData)
-        {
-            // test horizontal position measurements
-            posInnov[0] = statesAtPosTime[7] - posNE[0];
-            posInnov[1] = statesAtPosTime[8] - posNE[1];
-            varInnovVelPos[3] = P[7][7] + R_OBS[3];
-            varInnovVelPos[4] = P[8][8] + R_OBS[4];
-            // apply a 10-sigma threshold
-            current_ekf_state.posHealth = (sq(posInnov[0]) + sq(posInnov[1])) < 100.0f*(varInnovVelPos[3] + varInnovVelPos[4]);
-            current_ekf_state.posTimeout = (millis() - current_ekf_state.posFailTime) > horizRetryTime;
-            if (current_ekf_state.posHealth || current_ekf_state.posTimeout)
-            {
-                current_ekf_state.posHealth = true;
-                current_ekf_state.posFailTime = millis();
-            }
-            else
-            {
-                current_ekf_state.posHealth = false;
-            }
-        }
-        // test height measurements
-        if (fuseHgtData)
-        {
-            hgtInnov = statesAtHgtTime[9] + hgtMea;
-            varInnovVelPos[5] = P[9][9] + R_OBS[5];
-            // apply a 10-sigma threshold
-            current_ekf_state.hgtHealth = sq(hgtInnov) < 100.0f*varInnovVelPos[5];
-            current_ekf_state.hgtTimeout = (millis() - current_ekf_state.hgtFailTime) > hgtRetryTime;
-            if (current_ekf_state.hgtHealth || current_ekf_state.hgtTimeout)
-            {
-                current_ekf_state.hgtHealth = true;
-                current_ekf_state.hgtFailTime = millis();
-            }
-            else
-            {
-                current_ekf_state.hgtHealth = false;
-            }
-        }
-        // Set range for sequential fusion of velocity and position measurements depending
-        // on which data is available and its health
-        if (fuseVelData && fusionModeGPS == 0 && current_ekf_state.velHealth)
-        {
-            fuseData[0] = true;
-            fuseData[1] = true;
-            fuseData[2] = true;
-        }
-        if (fuseVelData && fusionModeGPS == 1 && current_ekf_state.velHealth)
-        {
-            fuseData[0] = true;
-            fuseData[1] = true;
-        }
-        if (fusePosData && fusionModeGPS <= 2 && current_ekf_state.posHealth)
-        {
-            fuseData[3] = true;
-            fuseData[4] = true;
-        }
-        if (fuseHgtData && current_ekf_state.hgtHealth)
-        {
-            fuseData[5] = true;
-        }
-        // Limit range of states modified when on ground
-        if(!onGround)
-        {
-            indexLimit = 20;
-        }
-        else
-        {
-            indexLimit = 12;
-        }
-        // Fuse measurements sequentially
-        for (obsIndex=0; obsIndex<=5; obsIndex++)
-        {
-            if (fuseData[obsIndex])
-            {
-                stateIndex = 4 + obsIndex;
-                // Calculate the measurement innovation, using states from a
-                // different time coordinate if fusing height data
-                if (obsIndex >= 0 && obsIndex <= 2)
-                {
-                    innovVelPos[obsIndex] = statesAtVelTime[stateIndex] - observation[obsIndex];
-                }
-                else if (obsIndex == 3 || obsIndex == 4)
-                {
-                    innovVelPos[obsIndex] = statesAtPosTime[stateIndex] - observation[obsIndex];
-                }
-                else if (obsIndex == 5)
-                {
-                    innovVelPos[obsIndex] = statesAtHgtTime[stateIndex] - observation[obsIndex];
-                }
-                // Calculate the Kalman Gain
-                // Calculate innovation variances - also used for data logging
-                varInnovVelPos[obsIndex] = P[stateIndex][stateIndex] + R_OBS[obsIndex];
-                SK = 1.0/varInnovVelPos[obsIndex];
-                for (uint8_t i= 0; i<=indexLimit; i++)
-                {
-                    Kfusion[i] = P[i][stateIndex]*SK;
-                }
-                // Calculate state corrections and re-normalise the quaternions
-                for (uint8_t i = 0; i<=indexLimit; i++)
-                {
-                    states[i] = states[i] - Kfusion[i] * innovVelPos[obsIndex];
-                }
-                quatMag = sqrt(states[0]*states[0] + states[1]*states[1] + states[2]*states[2] + states[3]*states[3]);
-                if (quatMag > 1e-12f) // divide by  0 protection
-                {
-                    for (uint8_t i = 0; i<=3; i++)
-                    {
-                        states[i] = states[i] / quatMag;
-                    }
-                }
-                // Update the covariance - take advantage of direct observation of a
-                // single state at index = stateIndex to reduce computations
-                // Optimised implementation of standard equation P = (I - K*H)*P;
-                for (uint8_t i= 0; i<=indexLimit; i++)
-                {
-                    for (uint8_t j= 0; j<=indexLimit; j++)
-                    {
-                        KHP[i][j] = Kfusion[i] * P[stateIndex][j];
-                    }
-                }
-                for (uint8_t i= 0; i<=indexLimit; i++)
-                {
-                    for (uint8_t j= 0; j<=indexLimit; j++)
-                    {
-                        P[i][j] = P[i][j] - KHP[i][j];
-                    }
-                }
-            }
-        }
-    }
-
-    ForceSymmetry();
-    ConstrainVariances();
-
-    //printf("velh: %s, posh: %s, hgth: %s\n", ((velHealth) ? "OK" : "FAIL"), ((posHealth) ? "OK" : "FAIL"), ((hgtHealth) ? "OK" : "FAIL"));
-}
-
-void AttPosEKF::FuseMagnetometer()
-{
-    uint8_t obsIndex;
-    uint8_t indexLimit;
-    float DCM[3][3] =
-    {
-        {1.0f,0.0f,0.0f} ,
-        {0.0f,1.0f,0.0f} ,
-        {0.0f,0.0f,1.0f}
-    };
-    float MagPred[3] = {0.0f,0.0f,0.0f};
-    float SK_MX[6];
-    float SK_MY[5];
-    float SK_MZ[6];
-    float SH_MAG[9] = {0.0f,0.0f,0.0f,0.0f,0.0f,0.0f,0.0f,0.0f,0.0f};
-
-// Perform sequential fusion of Magnetometer measurements.
-// This assumes that the errors in the different components are
-// uncorrelated which is not true, however in the absence of covariance
-// data fit is the only assumption we can make
-// so we might as well take advantage of the computational efficiencies
-// associated with sequential fusion
-    if (useCompass && (fuseMagData || obsIndex == 1 || obsIndex == 2))
-    {
-        // Limit range of states modified when on ground
-        if(!onGround)
-        {
-            indexLimit = 20;
-        }
-        else
-        {
-            indexLimit = 12;
-        }
-
-        static float q0 = 0.0f;
-        static float q1 = 0.0f;
-        static float q2 = 0.0f;
-        static float q3 = 1.0f;
-        static float magN = 0.4f;
-        static float magE = 0.0f;
-        static float magD = 0.3f;
-
-        static float R_MAG = 0.0025f;
-
-        float H_MAG[21] = {0.0f,0.0f,0.0f,0.0f,0.0f,0.0f,0.0f,0.0f,0.0f,0.0f,0.0f,0.0f,0.0f,0.0f,0.0f,0.0f,0.0f,0.0f,0.0f,0.0f,0.0f};
-
-        // Sequential fusion of XYZ components to spread processing load across
-        // three prediction time steps.
-
-        // Calculate observation jacobians and Kalman gains
-        if (fuseMagData)
-        {
-            static float magXbias = 0.0f;
-            static float magYbias = 0.0f;
-            static float magZbias = 0.0f;
-
-            // Copy required states to local variable names
-            q0       = statesAtMagMeasTime[0];
-            q1       = statesAtMagMeasTime[1];
-            q2       = statesAtMagMeasTime[2];
-            q3       = statesAtMagMeasTime[3];
-            magN     = statesAtMagMeasTime[15];
-            magE     = statesAtMagMeasTime[16];
-            magD     = statesAtMagMeasTime[17];
-            magXbias = statesAtMagMeasTime[18];
-            magYbias = statesAtMagMeasTime[19];
-            magZbias = statesAtMagMeasTime[20];
-
-            // rotate predicted earth components into body axes and calculate
-            // predicted measurments
-            DCM[0][0] = q0*q0 + q1*q1 - q2*q2 - q3*q3;
-            DCM[0][1] = 2*(q1*q2 + q0*q3);
-            DCM[0][2] = 2*(q1*q3-q0*q2);
-            DCM[1][0] = 2*(q1*q2 - q0*q3);
-            DCM[1][1] = q0*q0 - q1*q1 + q2*q2 - q3*q3;
-            DCM[1][2] = 2*(q2*q3 + q0*q1);
-            DCM[2][0] = 2*(q1*q3 + q0*q2);
-            DCM[2][1] = 2*(q2*q3 - q0*q1);
-            DCM[2][2] = q0*q0 - q1*q1 - q2*q2 + q3*q3;
-            MagPred[0] = DCM[0][0]*magN + DCM[0][1]*magE  + DCM[0][2]*magD + magXbias;
-            MagPred[1] = DCM[1][0]*magN + DCM[1][1]*magE  + DCM[1][2]*magD + magYbias;
-            MagPred[2] = DCM[2][0]*magN + DCM[2][1]*magE  + DCM[2][2]*magD + magZbias;
-
-            // scale magnetometer observation error with total angular rate
-            R_MAG = 0.0025f + sq(0.05f*dAngIMU.length()/dtIMU);
-
-            // Calculate observation jacobians
-            SH_MAG[0] = 2*magD*q3 + 2*magE*q2 + 2*magN*q1;
-            SH_MAG[1] = 2*magD*q0 - 2*magE*q1 + 2*magN*q2;
-            SH_MAG[2] = 2*magD*q1 + 2*magE*q0 - 2*magN*q3;
-            SH_MAG[3] = sq(q3);
-            SH_MAG[4] = sq(q2);
-            SH_MAG[5] = sq(q1);
-            SH_MAG[6] = sq(q0);
-            SH_MAG[7] = 2*magN*q0;
-            SH_MAG[8] = 2*magE*q3;
-
-            for (uint8_t i=0; i<=20; i++) H_MAG[i] = 0;
-            H_MAG[0] = SH_MAG[7] + SH_MAG[8] - 2*magD*q2;
-            H_MAG[1] = SH_MAG[0];
-            H_MAG[2] = 2*magE*q1 - 2*magD*q0 - 2*magN*q2;
-            H_MAG[3] = SH_MAG[2];
-            H_MAG[15] = SH_MAG[5] - SH_MAG[4] - SH_MAG[3] + SH_MAG[6];
-            H_MAG[16] = 2*q0*q3 + 2*q1*q2;
-            H_MAG[17] = 2*q1*q3 - 2*q0*q2;
-            H_MAG[18] = 1.0f;
-
-            // Calculate Kalman gain
-            SK_MX[0] = 1/(P[18][18] + R_MAG + P[1][18]*SH_MAG[0] + P[3][18]*SH_MAG[2] - P[15][18]*(SH_MAG[3] + SH_MAG[4] - SH_MAG[5] - SH_MAG[6]) - (2*magD*q0 - 2*magE*q1 + 2*magN*q2)*(P[18][2] + P[1][2]*SH_MAG[0] + P[3][2]*SH_MAG[2] - P[15][2]*(SH_MAG[3] + SH_MAG[4] - SH_MAG[5] - SH_MAG[6]) + P[16][2]*(2*q0*q3 + 2*q1*q2) - P[17][2]*(2*q0*q2 - 2*q1*q3) - P[2][2]*(2*magD*q0 - 2*magE*q1 + 2*magN*q2) + P[0][2]*(SH_MAG[7] + SH_MAG[8] - 2*magD*q2)) + (SH_MAG[7] + SH_MAG[8] - 2*magD*q2)*(P[18][0] + P[1][0]*SH_MAG[0] + P[3][0]*SH_MAG[2] - P[15][0]*(SH_MAG[3] + SH_MAG[4] - SH_MAG[5] - SH_MAG[6]) + P[16][0]*(2*q0*q3 + 2*q1*q2) - P[17][0]*(2*q0*q2 - 2*q1*q3) - P[2][0]*(2*magD*q0 - 2*magE*q1 + 2*magN*q2) + P[0][0]*(SH_MAG[7] + SH_MAG[8] - 2*magD*q2)) + SH_MAG[0]*(P[18][1] + P[1][1]*SH_MAG[0] + P[3][1]*SH_MAG[2] - P[15][1]*(SH_MAG[3] + SH_MAG[4] - SH_MAG[5] - SH_MAG[6]) + P[16][1]*(2*q0*q3 + 2*q1*q2) - P[17][1]*(2*q0*q2 - 2*q1*q3) - P[2][1]*(2*magD*q0 - 2*magE*q1 + 2*magN*q2) + P[0][1]*(SH_MAG[7] + SH_MAG[8] - 2*magD*q2)) + SH_MAG[2]*(P[18][3] + P[1][3]*SH_MAG[0] + P[3][3]*SH_MAG[2] - P[15][3]*(SH_MAG[3] + SH_MAG[4] - SH_MAG[5] - SH_MAG[6]) + P[16][3]*(2*q0*q3 + 2*q1*q2) - P[17][3]*(2*q0*q2 - 2*q1*q3) - P[2][3]*(2*magD*q0 - 2*magE*q1 + 2*magN*q2) + P[0][3]*(SH_MAG[7] + SH_MAG[8] - 2*magD*q2)) - (SH_MAG[3] + SH_MAG[4] - SH_MAG[5] - SH_MAG[6])*(P[18][15] + P[1][15]*SH_MAG[0] + P[3][15]*SH_MAG[2] - P[15][15]*(SH_MAG[3] + SH_MAG[4] - SH_MAG[5] - SH_MAG[6]) + P[16][15]*(2*q0*q3 + 2*q1*q2) - P[17][15]*(2*q0*q2 - 2*q1*q3) - P[2][15]*(2*magD*q0 - 2*magE*q1 + 2*magN*q2) + P[0][15]*(SH_MAG[7] + SH_MAG[8] - 2*magD*q2)) + P[16][18]*(2*q0*q3 + 2*q1*q2) - P[17][18]*(2*q0*q2 - 2*q1*q3) - P[2][18]*(2*magD*q0 - 2*magE*q1 + 2*magN*q2) + (2*q0*q3 + 2*q1*q2)*(P[18][16] + P[1][16]*SH_MAG[0] + P[3][16]*SH_MAG[2] - P[15][16]*(SH_MAG[3] + SH_MAG[4] - SH_MAG[5] - SH_MAG[6]) + P[16][16]*(2*q0*q3 + 2*q1*q2) - P[17][16]*(2*q0*q2 - 2*q1*q3) - P[2][16]*(2*magD*q0 - 2*magE*q1 + 2*magN*q2) + P[0][16]*(SH_MAG[7] + SH_MAG[8] - 2*magD*q2)) - (2*q0*q2 - 2*q1*q3)*(P[18][17] + P[1][17]*SH_MAG[0] + P[3][17]*SH_MAG[2] - P[15][17]*(SH_MAG[3] + SH_MAG[4] - SH_MAG[5] - SH_MAG[6]) + P[16][17]*(2*q0*q3 + 2*q1*q2) - P[17][17]*(2*q0*q2 - 2*q1*q3) - P[2][17]*(2*magD*q0 - 2*magE*q1 + 2*magN*q2) + P[0][17]*(SH_MAG[7] + SH_MAG[8] - 2*magD*q2)) + P[0][18]*(SH_MAG[7] + SH_MAG[8] - 2*magD*q2));
-            SK_MX[1] = SH_MAG[3] + SH_MAG[4] - SH_MAG[5] - SH_MAG[6];
-            SK_MX[2] = 2*magD*q0 - 2*magE*q1 + 2*magN*q2;
-            SK_MX[3] = SH_MAG[7] + SH_MAG[8] - 2*magD*q2;
-            SK_MX[4] = 2*q0*q2 - 2*q1*q3;
-            SK_MX[5] = 2*q0*q3 + 2*q1*q2;
-            Kfusion[0] = SK_MX[0]*(P[0][18] + P[0][1]*SH_MAG[0] + P[0][3]*SH_MAG[2] + P[0][0]*SK_MX[3] - P[0][2]*SK_MX[2] - P[0][15]*SK_MX[1] + P[0][16]*SK_MX[5] - P[0][17]*SK_MX[4]);
-            Kfusion[1] = SK_MX[0]*(P[1][18] + P[1][1]*SH_MAG[0] + P[1][3]*SH_MAG[2] + P[1][0]*SK_MX[3] - P[1][2]*SK_MX[2] - P[1][15]*SK_MX[1] + P[1][16]*SK_MX[5] - P[1][17]*SK_MX[4]);
-            Kfusion[2] = SK_MX[0]*(P[2][18] + P[2][1]*SH_MAG[0] + P[2][3]*SH_MAG[2] + P[2][0]*SK_MX[3] - P[2][2]*SK_MX[2] - P[2][15]*SK_MX[1] + P[2][16]*SK_MX[5] - P[2][17]*SK_MX[4]);
-            Kfusion[3] = SK_MX[0]*(P[3][18] + P[3][1]*SH_MAG[0] + P[3][3]*SH_MAG[2] + P[3][0]*SK_MX[3] - P[3][2]*SK_MX[2] - P[3][15]*SK_MX[1] + P[3][16]*SK_MX[5] - P[3][17]*SK_MX[4]);
-            Kfusion[4] = SK_MX[0]*(P[4][18] + P[4][1]*SH_MAG[0] + P[4][3]*SH_MAG[2] + P[4][0]*SK_MX[3] - P[4][2]*SK_MX[2] - P[4][15]*SK_MX[1] + P[4][16]*SK_MX[5] - P[4][17]*SK_MX[4]);
-            Kfusion[5] = SK_MX[0]*(P[5][18] + P[5][1]*SH_MAG[0] + P[5][3]*SH_MAG[2] + P[5][0]*SK_MX[3] - P[5][2]*SK_MX[2] - P[5][15]*SK_MX[1] + P[5][16]*SK_MX[5] - P[5][17]*SK_MX[4]);
-            Kfusion[6] = SK_MX[0]*(P[6][18] + P[6][1]*SH_MAG[0] + P[6][3]*SH_MAG[2] + P[6][0]*SK_MX[3] - P[6][2]*SK_MX[2] - P[6][15]*SK_MX[1] + P[6][16]*SK_MX[5] - P[6][17]*SK_MX[4]);
-            Kfusion[7] = SK_MX[0]*(P[7][18] + P[7][1]*SH_MAG[0] + P[7][3]*SH_MAG[2] + P[7][0]*SK_MX[3] - P[7][2]*SK_MX[2] - P[7][15]*SK_MX[1] + P[7][16]*SK_MX[5] - P[7][17]*SK_MX[4]);
-            Kfusion[8] = SK_MX[0]*(P[8][18] + P[8][1]*SH_MAG[0] + P[8][3]*SH_MAG[2] + P[8][0]*SK_MX[3] - P[8][2]*SK_MX[2] - P[8][15]*SK_MX[1] + P[8][16]*SK_MX[5] - P[8][17]*SK_MX[4]);
-            Kfusion[9] = SK_MX[0]*(P[9][18] + P[9][1]*SH_MAG[0] + P[9][3]*SH_MAG[2] + P[9][0]*SK_MX[3] - P[9][2]*SK_MX[2] - P[9][15]*SK_MX[1] + P[9][16]*SK_MX[5] - P[9][17]*SK_MX[4]);
-            Kfusion[10] = SK_MX[0]*(P[10][18] + P[10][1]*SH_MAG[0] + P[10][3]*SH_MAG[2] + P[10][0]*SK_MX[3] - P[10][2]*SK_MX[2] - P[10][15]*SK_MX[1] + P[10][16]*SK_MX[5] - P[10][17]*SK_MX[4]);
-            Kfusion[11] = SK_MX[0]*(P[11][18] + P[11][1]*SH_MAG[0] + P[11][3]*SH_MAG[2] + P[11][0]*SK_MX[3] - P[11][2]*SK_MX[2] - P[11][15]*SK_MX[1] + P[11][16]*SK_MX[5] - P[11][17]*SK_MX[4]);
-            Kfusion[12] = SK_MX[0]*(P[12][18] + P[12][1]*SH_MAG[0] + P[12][3]*SH_MAG[2] + P[12][0]*SK_MX[3] - P[12][2]*SK_MX[2] - P[12][15]*SK_MX[1] + P[12][16]*SK_MX[5] - P[12][17]*SK_MX[4]);
-            Kfusion[13] = SK_MX[0]*(P[13][18] + P[13][1]*SH_MAG[0] + P[13][3]*SH_MAG[2] + P[13][0]*SK_MX[3] - P[13][2]*SK_MX[2] - P[13][15]*SK_MX[1] + P[13][16]*SK_MX[5] - P[13][17]*SK_MX[4]);
-            Kfusion[14] = SK_MX[0]*(P[14][18] + P[14][1]*SH_MAG[0] + P[14][3]*SH_MAG[2] + P[14][0]*SK_MX[3] - P[14][2]*SK_MX[2] - P[14][15]*SK_MX[1] + P[14][16]*SK_MX[5] - P[14][17]*SK_MX[4]);
-            Kfusion[15] = SK_MX[0]*(P[15][18] + P[15][1]*SH_MAG[0] + P[15][3]*SH_MAG[2] + P[15][0]*SK_MX[3] - P[15][2]*SK_MX[2] - P[15][15]*SK_MX[1] + P[15][16]*SK_MX[5] - P[15][17]*SK_MX[4]);
-            Kfusion[16] = SK_MX[0]*(P[16][18] + P[16][1]*SH_MAG[0] + P[16][3]*SH_MAG[2] + P[16][0]*SK_MX[3] - P[16][2]*SK_MX[2] - P[16][15]*SK_MX[1] + P[16][16]*SK_MX[5] - P[16][17]*SK_MX[4]);
-            Kfusion[17] = SK_MX[0]*(P[17][18] + P[17][1]*SH_MAG[0] + P[17][3]*SH_MAG[2] + P[17][0]*SK_MX[3] - P[17][2]*SK_MX[2] - P[17][15]*SK_MX[1] + P[17][16]*SK_MX[5] - P[17][17]*SK_MX[4]);
-            Kfusion[18] = SK_MX[0]*(P[18][18] + P[18][1]*SH_MAG[0] + P[18][3]*SH_MAG[2] + P[18][0]*SK_MX[3] - P[18][2]*SK_MX[2] - P[18][15]*SK_MX[1] + P[18][16]*SK_MX[5] - P[18][17]*SK_MX[4]);
-            Kfusion[19] = SK_MX[0]*(P[19][18] + P[19][1]*SH_MAG[0] + P[19][3]*SH_MAG[2] + P[19][0]*SK_MX[3] - P[19][2]*SK_MX[2] - P[19][15]*SK_MX[1] + P[19][16]*SK_MX[5] - P[19][17]*SK_MX[4]);
-            Kfusion[20] = SK_MX[0]*(P[20][18] + P[20][1]*SH_MAG[0] + P[20][3]*SH_MAG[2] + P[20][0]*SK_MX[3] - P[20][2]*SK_MX[2] - P[20][15]*SK_MX[1] + P[20][16]*SK_MX[5] - P[20][17]*SK_MX[4]);
-            varInnovMag[0] = 1.0f/SK_MX[0];
-            innovMag[0] = MagPred[0] - magData.x;
-
-            // reset the observation index to 0 (we start by fusing the X
-            // measurement)
-            obsIndex = 0;
-        }
-        else if (obsIndex == 1) // we are now fusing the Y measurement
-        {
-            // Calculate observation jacobians
-            for (unsigned int i=0; i<n_states; i++) H_MAG[i] = 0;
-            H_MAG[0] = SH_MAG[2];
-            H_MAG[1] = SH_MAG[1];
-            H_MAG[2] = SH_MAG[0];
-            H_MAG[3] = 2*magD*q2 - SH_MAG[8] - SH_MAG[7];
-            H_MAG[15] = 2*q1*q2 - 2*q0*q3;
-            H_MAG[16] = SH_MAG[4] - SH_MAG[3] - SH_MAG[5] + SH_MAG[6];
-            H_MAG[17] = 2*q0*q1 + 2*q2*q3;
-            H_MAG[19] = 1;
-
-            // Calculate Kalman gain
-            SK_MY[0] = 1/(P[19][19] + R_MAG + P[0][19]*SH_MAG[2] + P[1][19]*SH_MAG[1] + P[2][19]*SH_MAG[0] - P[16][19]*(SH_MAG[3] - SH_MAG[4] + SH_MAG[5] - SH_MAG[6]) - (2*q0*q3 - 2*q1*q2)*(P[19][15] + P[0][15]*SH_MAG[2] + P[1][15]*SH_MAG[1] + P[2][15]*SH_MAG[0] - P[16][15]*(SH_MAG[3] - SH_MAG[4] + SH_MAG[5] - SH_MAG[6]) - P[15][15]*(2*q0*q3 - 2*q1*q2) + P[17][15]*(2*q0*q1 + 2*q2*q3) - P[3][15]*(SH_MAG[7] + SH_MAG[8] - 2*magD*q2)) + (2*q0*q1 + 2*q2*q3)*(P[19][17] + P[0][17]*SH_MAG[2] + P[1][17]*SH_MAG[1] + P[2][17]*SH_MAG[0] - P[16][17]*(SH_MAG[3] - SH_MAG[4] + SH_MAG[5] - SH_MAG[6]) - P[15][17]*(2*q0*q3 - 2*q1*q2) + P[17][17]*(2*q0*q1 + 2*q2*q3) - P[3][17]*(SH_MAG[7] + SH_MAG[8] - 2*magD*q2)) - (SH_MAG[7] + SH_MAG[8] - 2*magD*q2)*(P[19][3] + P[0][3]*SH_MAG[2] + P[1][3]*SH_MAG[1] + P[2][3]*SH_MAG[0] - P[16][3]*(SH_MAG[3] - SH_MAG[4] + SH_MAG[5] - SH_MAG[6]) - P[15][3]*(2*q0*q3 - 2*q1*q2) + P[17][3]*(2*q0*q1 + 2*q2*q3) - P[3][3]*(SH_MAG[7] + SH_MAG[8] - 2*magD*q2)) - P[15][19]*(2*q0*q3 - 2*q1*q2) + P[17][19]*(2*q0*q1 + 2*q2*q3) + SH_MAG[2]*(P[19][0] + P[0][0]*SH_MAG[2] + P[1][0]*SH_MAG[1] + P[2][0]*SH_MAG[0] - P[16][0]*(SH_MAG[3] - SH_MAG[4] + SH_MAG[5] - SH_MAG[6]) - P[15][0]*(2*q0*q3 - 2*q1*q2) + P[17][0]*(2*q0*q1 + 2*q2*q3) - P[3][0]*(SH_MAG[7] + SH_MAG[8] - 2*magD*q2)) + SH_MAG[1]*(P[19][1] + P[0][1]*SH_MAG[2] + P[1][1]*SH_MAG[1] + P[2][1]*SH_MAG[0] - P[16][1]*(SH_MAG[3] - SH_MAG[4] + SH_MAG[5] - SH_MAG[6]) - P[15][1]*(2*q0*q3 - 2*q1*q2) + P[17][1]*(2*q0*q1 + 2*q2*q3) - P[3][1]*(SH_MAG[7] + SH_MAG[8] - 2*magD*q2)) + SH_MAG[0]*(P[19][2] + P[0][2]*SH_MAG[2] + P[1][2]*SH_MAG[1] + P[2][2]*SH_MAG[0] - P[16][2]*(SH_MAG[3] - SH_MAG[4] + SH_MAG[5] - SH_MAG[6]) - P[15][2]*(2*q0*q3 - 2*q1*q2) + P[17][2]*(2*q0*q1 + 2*q2*q3) - P[3][2]*(SH_MAG[7] + SH_MAG[8] - 2*magD*q2)) - (SH_MAG[3] - SH_MAG[4] + SH_MAG[5] - SH_MAG[6])*(P[19][16] + P[0][16]*SH_MAG[2] + P[1][16]*SH_MAG[1] + P[2][16]*SH_MAG[0] - P[16][16]*(SH_MAG[3] - SH_MAG[4] + SH_MAG[5] - SH_MAG[6]) - P[15][16]*(2*q0*q3 - 2*q1*q2) + P[17][16]*(2*q0*q1 + 2*q2*q3) - P[3][16]*(SH_MAG[7] + SH_MAG[8] - 2*magD*q2)) - P[3][19]*(SH_MAG[7] + SH_MAG[8] - 2*magD*q2));
-            SK_MY[1] = SH_MAG[3] - SH_MAG[4] + SH_MAG[5] - SH_MAG[6];
-            SK_MY[2] = SH_MAG[7] + SH_MAG[8] - 2*magD*q2;
-            SK_MY[3] = 2*q0*q3 - 2*q1*q2;
-            SK_MY[4] = 2*q0*q1 + 2*q2*q3;
-            Kfusion[0] = SK_MY[0]*(P[0][19] + P[0][0]*SH_MAG[2] + P[0][1]*SH_MAG[1] + P[0][2]*SH_MAG[0] - P[0][3]*SK_MY[2] - P[0][16]*SK_MY[1] - P[0][15]*SK_MY[3] + P[0][17]*SK_MY[4]);
-            Kfusion[1] = SK_MY[0]*(P[1][19] + P[1][0]*SH_MAG[2] + P[1][1]*SH_MAG[1] + P[1][2]*SH_MAG[0] - P[1][3]*SK_MY[2] - P[1][16]*SK_MY[1] - P[1][15]*SK_MY[3] + P[1][17]*SK_MY[4]);
-            Kfusion[2] = SK_MY[0]*(P[2][19] + P[2][0]*SH_MAG[2] + P[2][1]*SH_MAG[1] + P[2][2]*SH_MAG[0] - P[2][3]*SK_MY[2] - P[2][16]*SK_MY[1] - P[2][15]*SK_MY[3] + P[2][17]*SK_MY[4]);
-            Kfusion[3] = SK_MY[0]*(P[3][19] + P[3][0]*SH_MAG[2] + P[3][1]*SH_MAG[1] + P[3][2]*SH_MAG[0] - P[3][3]*SK_MY[2] - P[3][16]*SK_MY[1] - P[3][15]*SK_MY[3] + P[3][17]*SK_MY[4]);
-            Kfusion[4] = SK_MY[0]*(P[4][19] + P[4][0]*SH_MAG[2] + P[4][1]*SH_MAG[1] + P[4][2]*SH_MAG[0] - P[4][3]*SK_MY[2] - P[4][16]*SK_MY[1] - P[4][15]*SK_MY[3] + P[4][17]*SK_MY[4]);
-            Kfusion[5] = SK_MY[0]*(P[5][19] + P[5][0]*SH_MAG[2] + P[5][1]*SH_MAG[1] + P[5][2]*SH_MAG[0] - P[5][3]*SK_MY[2] - P[5][16]*SK_MY[1] - P[5][15]*SK_MY[3] + P[5][17]*SK_MY[4]);
-            Kfusion[6] = SK_MY[0]*(P[6][19] + P[6][0]*SH_MAG[2] + P[6][1]*SH_MAG[1] + P[6][2]*SH_MAG[0] - P[6][3]*SK_MY[2] - P[6][16]*SK_MY[1] - P[6][15]*SK_MY[3] + P[6][17]*SK_MY[4]);
-            Kfusion[7] = SK_MY[0]*(P[7][19] + P[7][0]*SH_MAG[2] + P[7][1]*SH_MAG[1] + P[7][2]*SH_MAG[0] - P[7][3]*SK_MY[2] - P[7][16]*SK_MY[1] - P[7][15]*SK_MY[3] + P[7][17]*SK_MY[4]);
-            Kfusion[8] = SK_MY[0]*(P[8][19] + P[8][0]*SH_MAG[2] + P[8][1]*SH_MAG[1] + P[8][2]*SH_MAG[0] - P[8][3]*SK_MY[2] - P[8][16]*SK_MY[1] - P[8][15]*SK_MY[3] + P[8][17]*SK_MY[4]);
-            Kfusion[9] = SK_MY[0]*(P[9][19] + P[9][0]*SH_MAG[2] + P[9][1]*SH_MAG[1] + P[9][2]*SH_MAG[0] - P[9][3]*SK_MY[2] - P[9][16]*SK_MY[1] - P[9][15]*SK_MY[3] + P[9][17]*SK_MY[4]);
-            Kfusion[10] = SK_MY[0]*(P[10][19] + P[10][0]*SH_MAG[2] + P[10][1]*SH_MAG[1] + P[10][2]*SH_MAG[0] - P[10][3]*SK_MY[2] - P[10][16]*SK_MY[1] - P[10][15]*SK_MY[3] + P[10][17]*SK_MY[4]);
-            Kfusion[11] = SK_MY[0]*(P[11][19] + P[11][0]*SH_MAG[2] + P[11][1]*SH_MAG[1] + P[11][2]*SH_MAG[0] - P[11][3]*SK_MY[2] - P[11][16]*SK_MY[1] - P[11][15]*SK_MY[3] + P[11][17]*SK_MY[4]);
-            Kfusion[12] = SK_MY[0]*(P[12][19] + P[12][0]*SH_MAG[2] + P[12][1]*SH_MAG[1] + P[12][2]*SH_MAG[0] - P[12][3]*SK_MY[2] - P[12][16]*SK_MY[1] - P[12][15]*SK_MY[3] + P[12][17]*SK_MY[4]);
-            Kfusion[13] = SK_MY[0]*(P[13][19] + P[13][0]*SH_MAG[2] + P[13][1]*SH_MAG[1] + P[13][2]*SH_MAG[0] - P[13][3]*SK_MY[2] - P[13][16]*SK_MY[1] - P[13][15]*SK_MY[3] + P[13][17]*SK_MY[4]);
-            Kfusion[14] = SK_MY[0]*(P[14][19] + P[14][0]*SH_MAG[2] + P[14][1]*SH_MAG[1] + P[14][2]*SH_MAG[0] - P[14][3]*SK_MY[2] - P[14][16]*SK_MY[1] - P[14][15]*SK_MY[3] + P[14][17]*SK_MY[4]);
-            Kfusion[15] = SK_MY[0]*(P[15][19] + P[15][0]*SH_MAG[2] + P[15][1]*SH_MAG[1] + P[15][2]*SH_MAG[0] - P[15][3]*SK_MY[2] - P[15][16]*SK_MY[1] - P[15][15]*SK_MY[3] + P[15][17]*SK_MY[4]);
-            Kfusion[16] = SK_MY[0]*(P[16][19] + P[16][0]*SH_MAG[2] + P[16][1]*SH_MAG[1] + P[16][2]*SH_MAG[0] - P[16][3]*SK_MY[2] - P[16][16]*SK_MY[1] - P[16][15]*SK_MY[3] + P[16][17]*SK_MY[4]);
-            Kfusion[17] = SK_MY[0]*(P[17][19] + P[17][0]*SH_MAG[2] + P[17][1]*SH_MAG[1] + P[17][2]*SH_MAG[0] - P[17][3]*SK_MY[2] - P[17][16]*SK_MY[1] - P[17][15]*SK_MY[3] + P[17][17]*SK_MY[4]);
-            Kfusion[18] = SK_MY[0]*(P[18][19] + P[18][0]*SH_MAG[2] + P[18][1]*SH_MAG[1] + P[18][2]*SH_MAG[0] - P[18][3]*SK_MY[2] - P[18][16]*SK_MY[1] - P[18][15]*SK_MY[3] + P[18][17]*SK_MY[4]);
-            Kfusion[19] = SK_MY[0]*(P[19][19] + P[19][0]*SH_MAG[2] + P[19][1]*SH_MAG[1] + P[19][2]*SH_MAG[0] - P[19][3]*SK_MY[2] - P[19][16]*SK_MY[1] - P[19][15]*SK_MY[3] + P[19][17]*SK_MY[4]);
-            Kfusion[20] = SK_MY[0]*(P[20][19] + P[20][0]*SH_MAG[2] + P[20][1]*SH_MAG[1] + P[20][2]*SH_MAG[0] - P[20][3]*SK_MY[2] - P[20][16]*SK_MY[1] - P[20][15]*SK_MY[3] + P[20][17]*SK_MY[4]);
-            varInnovMag[1] = 1.0f/SK_MY[0];
-            innovMag[1] = MagPred[1] - magData.y;
-        }
-        else if (obsIndex == 2) // we are now fusing the Z measurement
-        {
-            // Calculate observation jacobians
-            for (uint8_t i=0; i<=20; i++) H_MAG[i] = 0;
-            H_MAG[0] = SH_MAG[1];
-            H_MAG[1] = 2*magN*q3 - 2*magE*q0 - 2*magD*q1;
-            H_MAG[2] = SH_MAG[7] + SH_MAG[8] - 2*magD*q2;
-            H_MAG[3] = SH_MAG[0];
-            H_MAG[15] = 2*q0*q2 + 2*q1*q3;
-            H_MAG[16] = 2*q2*q3 - 2*q0*q1;
-            H_MAG[17] = SH_MAG[3] - SH_MAG[4] - SH_MAG[5] + SH_MAG[6];
-            H_MAG[20] = 1;
-
-            // Calculate Kalman gain
-            SK_MZ[0] = 1/(P[20][20] + R_MAG + P[0][20]*SH_MAG[1] + P[3][20]*SH_MAG[0] + P[17][20]*(SH_MAG[3] - SH_MAG[4] - SH_MAG[5] + SH_MAG[6]) - (2*magD*q1 + 2*magE*q0 - 2*magN*q3)*(P[20][1] + P[0][1]*SH_MAG[1] + P[3][1]*SH_MAG[0] + P[17][1]*(SH_MAG[3] - SH_MAG[4] - SH_MAG[5] + SH_MAG[6]) + P[15][1]*(2*q0*q2 + 2*q1*q3) - P[16][1]*(2*q0*q1 - 2*q2*q3) - P[1][1]*(2*magD*q1 + 2*magE*q0 - 2*magN*q3) + P[2][1]*(SH_MAG[7] + SH_MAG[8] - 2*magD*q2)) + (SH_MAG[7] + SH_MAG[8] - 2*magD*q2)*(P[20][2] + P[0][2]*SH_MAG[1] + P[3][2]*SH_MAG[0] + P[17][2]*(SH_MAG[3] - SH_MAG[4] - SH_MAG[5] + SH_MAG[6]) + P[15][2]*(2*q0*q2 + 2*q1*q3) - P[16][2]*(2*q0*q1 - 2*q2*q3) - P[1][2]*(2*magD*q1 + 2*magE*q0 - 2*magN*q3) + P[2][2]*(SH_MAG[7] + SH_MAG[8] - 2*magD*q2)) + SH_MAG[1]*(P[20][0] + P[0][0]*SH_MAG[1] + P[3][0]*SH_MAG[0] + P[17][0]*(SH_MAG[3] - SH_MAG[4] - SH_MAG[5] + SH_MAG[6]) + P[15][0]*(2*q0*q2 + 2*q1*q3) - P[16][0]*(2*q0*q1 - 2*q2*q3) - P[1][0]*(2*magD*q1 + 2*magE*q0 - 2*magN*q3) + P[2][0]*(SH_MAG[7] + SH_MAG[8] - 2*magD*q2)) + SH_MAG[0]*(P[20][3] + P[0][3]*SH_MAG[1] + P[3][3]*SH_MAG[0] + P[17][3]*(SH_MAG[3] - SH_MAG[4] - SH_MAG[5] + SH_MAG[6]) + P[15][3]*(2*q0*q2 + 2*q1*q3) - P[16][3]*(2*q0*q1 - 2*q2*q3) - P[1][3]*(2*magD*q1 + 2*magE*q0 - 2*magN*q3) + P[2][3]*(SH_MAG[7] + SH_MAG[8] - 2*magD*q2)) + (SH_MAG[3] - SH_MAG[4] - SH_MAG[5] + SH_MAG[6])*(P[20][17] + P[0][17]*SH_MAG[1] + P[3][17]*SH_MAG[0] + P[17][17]*(SH_MAG[3] - SH_MAG[4] - SH_MAG[5] + SH_MAG[6]) + P[15][17]*(2*q0*q2 + 2*q1*q3) - P[16][17]*(2*q0*q1 - 2*q2*q3) - P[1][17]*(2*magD*q1 + 2*magE*q0 - 2*magN*q3) + P[2][17]*(SH_MAG[7] + SH_MAG[8] - 2*magD*q2)) + P[15][20]*(2*q0*q2 + 2*q1*q3) - P[16][20]*(2*q0*q1 - 2*q2*q3) - P[1][20]*(2*magD*q1 + 2*magE*q0 - 2*magN*q3) + (2*q0*q2 + 2*q1*q3)*(P[20][15] + P[0][15]*SH_MAG[1] + P[3][15]*SH_MAG[0] + P[17][15]*(SH_MAG[3] - SH_MAG[4] - SH_MAG[5] + SH_MAG[6]) + P[15][15]*(2*q0*q2 + 2*q1*q3) - P[16][15]*(2*q0*q1 - 2*q2*q3) - P[1][15]*(2*magD*q1 + 2*magE*q0 - 2*magN*q3) + P[2][15]*(SH_MAG[7] + SH_MAG[8] - 2*magD*q2)) - (2*q0*q1 - 2*q2*q3)*(P[20][16] + P[0][16]*SH_MAG[1] + P[3][16]*SH_MAG[0] + P[17][16]*(SH_MAG[3] - SH_MAG[4] - SH_MAG[5] + SH_MAG[6]) + P[15][16]*(2*q0*q2 + 2*q1*q3) - P[16][16]*(2*q0*q1 - 2*q2*q3) - P[1][16]*(2*magD*q1 + 2*magE*q0 - 2*magN*q3) + P[2][16]*(SH_MAG[7] + SH_MAG[8] - 2*magD*q2)) + P[2][20]*(SH_MAG[7] + SH_MAG[8] - 2*magD*q2));
-            SK_MZ[1] = SH_MAG[3] - SH_MAG[4] - SH_MAG[5] + SH_MAG[6];
-            SK_MZ[2] = 2*magD*q1 + 2*magE*q0 - 2*magN*q3;
-            SK_MZ[3] = SH_MAG[7] + SH_MAG[8] - 2*magD*q2;
-            SK_MZ[4] = 2*q0*q1 - 2*q2*q3;
-            SK_MZ[5] = 2*q0*q2 + 2*q1*q3;
-            Kfusion[0] = SK_MZ[0]*(P[0][20] + P[0][0]*SH_MAG[1] + P[0][3]*SH_MAG[0] - P[0][1]*SK_MZ[2] + P[0][2]*SK_MZ[3] + P[0][17]*SK_MZ[1] + P[0][15]*SK_MZ[5] - P[0][16]*SK_MZ[4]);
-            Kfusion[1] = SK_MZ[0]*(P[1][20] + P[1][0]*SH_MAG[1] + P[1][3]*SH_MAG[0] - P[1][1]*SK_MZ[2] + P[1][2]*SK_MZ[3] + P[1][17]*SK_MZ[1] + P[1][15]*SK_MZ[5] - P[1][16]*SK_MZ[4]);
-            Kfusion[2] = SK_MZ[0]*(P[2][20] + P[2][0]*SH_MAG[1] + P[2][3]*SH_MAG[0] - P[2][1]*SK_MZ[2] + P[2][2]*SK_MZ[3] + P[2][17]*SK_MZ[1] + P[2][15]*SK_MZ[5] - P[2][16]*SK_MZ[4]);
-            Kfusion[3] = SK_MZ[0]*(P[3][20] + P[3][0]*SH_MAG[1] + P[3][3]*SH_MAG[0] - P[3][1]*SK_MZ[2] + P[3][2]*SK_MZ[3] + P[3][17]*SK_MZ[1] + P[3][15]*SK_MZ[5] - P[3][16]*SK_MZ[4]);
-            Kfusion[4] = SK_MZ[0]*(P[4][20] + P[4][0]*SH_MAG[1] + P[4][3]*SH_MAG[0] - P[4][1]*SK_MZ[2] + P[4][2]*SK_MZ[3] + P[4][17]*SK_MZ[1] + P[4][15]*SK_MZ[5] - P[4][16]*SK_MZ[4]);
-            Kfusion[5] = SK_MZ[0]*(P[5][20] + P[5][0]*SH_MAG[1] + P[5][3]*SH_MAG[0] - P[5][1]*SK_MZ[2] + P[5][2]*SK_MZ[3] + P[5][17]*SK_MZ[1] + P[5][15]*SK_MZ[5] - P[5][16]*SK_MZ[4]);
-            Kfusion[6] = SK_MZ[0]*(P[6][20] + P[6][0]*SH_MAG[1] + P[6][3]*SH_MAG[0] - P[6][1]*SK_MZ[2] + P[6][2]*SK_MZ[3] + P[6][17]*SK_MZ[1] + P[6][15]*SK_MZ[5] - P[6][16]*SK_MZ[4]);
-            Kfusion[7] = SK_MZ[0]*(P[7][20] + P[7][0]*SH_MAG[1] + P[7][3]*SH_MAG[0] - P[7][1]*SK_MZ[2] + P[7][2]*SK_MZ[3] + P[7][17]*SK_MZ[1] + P[7][15]*SK_MZ[5] - P[7][16]*SK_MZ[4]);
-            Kfusion[8] = SK_MZ[0]*(P[8][20] + P[8][0]*SH_MAG[1] + P[8][3]*SH_MAG[0] - P[8][1]*SK_MZ[2] + P[8][2]*SK_MZ[3] + P[8][17]*SK_MZ[1] + P[8][15]*SK_MZ[5] - P[8][16]*SK_MZ[4]);
-            Kfusion[9] = SK_MZ[0]*(P[9][20] + P[9][0]*SH_MAG[1] + P[9][3]*SH_MAG[0] - P[9][1]*SK_MZ[2] + P[9][2]*SK_MZ[3] + P[9][17]*SK_MZ[1] + P[9][15]*SK_MZ[5] - P[9][16]*SK_MZ[4]);
-            Kfusion[10] = SK_MZ[0]*(P[10][20] + P[10][0]*SH_MAG[1] + P[10][3]*SH_MAG[0] - P[10][1]*SK_MZ[2] + P[10][2]*SK_MZ[3] + P[10][17]*SK_MZ[1] + P[10][15]*SK_MZ[5] - P[10][16]*SK_MZ[4]);
-            Kfusion[11] = SK_MZ[0]*(P[11][20] + P[11][0]*SH_MAG[1] + P[11][3]*SH_MAG[0] - P[11][1]*SK_MZ[2] + P[11][2]*SK_MZ[3] + P[11][17]*SK_MZ[1] + P[11][15]*SK_MZ[5] - P[11][16]*SK_MZ[4]);
-            Kfusion[12] = SK_MZ[0]*(P[12][20] + P[12][0]*SH_MAG[1] + P[12][3]*SH_MAG[0] - P[12][1]*SK_MZ[2] + P[12][2]*SK_MZ[3] + P[12][17]*SK_MZ[1] + P[12][15]*SK_MZ[5] - P[12][16]*SK_MZ[4]);
-            Kfusion[13] = SK_MZ[0]*(P[13][20] + P[13][0]*SH_MAG[1] + P[13][3]*SH_MAG[0] - P[13][1]*SK_MZ[2] + P[13][2]*SK_MZ[3] + P[13][17]*SK_MZ[1] + P[13][15]*SK_MZ[5] - P[13][16]*SK_MZ[4]);
-            Kfusion[14] = SK_MZ[0]*(P[14][20] + P[14][0]*SH_MAG[1] + P[14][3]*SH_MAG[0] - P[14][1]*SK_MZ[2] + P[14][2]*SK_MZ[3] + P[14][17]*SK_MZ[1] + P[14][15]*SK_MZ[5] - P[14][16]*SK_MZ[4]);
-            Kfusion[15] = SK_MZ[0]*(P[15][20] + P[15][0]*SH_MAG[1] + P[15][3]*SH_MAG[0] - P[15][1]*SK_MZ[2] + P[15][2]*SK_MZ[3] + P[15][17]*SK_MZ[1] + P[15][15]*SK_MZ[5] - P[15][16]*SK_MZ[4]);
-            Kfusion[16] = SK_MZ[0]*(P[16][20] + P[16][0]*SH_MAG[1] + P[16][3]*SH_MAG[0] - P[16][1]*SK_MZ[2] + P[16][2]*SK_MZ[3] + P[16][17]*SK_MZ[1] + P[16][15]*SK_MZ[5] - P[16][16]*SK_MZ[4]);
-            Kfusion[17] = SK_MZ[0]*(P[17][20] + P[17][0]*SH_MAG[1] + P[17][3]*SH_MAG[0] - P[17][1]*SK_MZ[2] + P[17][2]*SK_MZ[3] + P[17][17]*SK_MZ[1] + P[17][15]*SK_MZ[5] - P[17][16]*SK_MZ[4]);
-            Kfusion[18] = SK_MZ[0]*(P[18][20] + P[18][0]*SH_MAG[1] + P[18][3]*SH_MAG[0] - P[18][1]*SK_MZ[2] + P[18][2]*SK_MZ[3] + P[18][17]*SK_MZ[1] + P[18][15]*SK_MZ[5] - P[18][16]*SK_MZ[4]);
-            Kfusion[19] = SK_MZ[0]*(P[19][20] + P[19][0]*SH_MAG[1] + P[19][3]*SH_MAG[0] - P[19][1]*SK_MZ[2] + P[19][2]*SK_MZ[3] + P[19][17]*SK_MZ[1] + P[19][15]*SK_MZ[5] - P[19][16]*SK_MZ[4]);
-            Kfusion[20] = SK_MZ[0]*(P[20][20] + P[20][0]*SH_MAG[1] + P[20][3]*SH_MAG[0] - P[20][1]*SK_MZ[2] + P[20][2]*SK_MZ[3] + P[20][17]*SK_MZ[1] + P[20][15]*SK_MZ[5] - P[20][16]*SK_MZ[4]);
-            varInnovMag[2] = 1.0f/SK_MZ[0];
-            innovMag[2] = MagPred[2] - magData.z;
-
-        }
-
-        // Check the innovation for consistency and don't fuse if > 5Sigma
-        if ((innovMag[obsIndex]*innovMag[obsIndex]/varInnovMag[obsIndex]) < 25.0)
-        {
-            // correct the state vector
-            for (uint8_t j= 0; j<=indexLimit; j++)
-            {
-                states[j] = states[j] - Kfusion[j] * innovMag[obsIndex];
-            }
-            // normalise the quaternion states
-            float quatMag = sqrt(states[0]*states[0] + states[1]*states[1] + states[2]*states[2] + states[3]*states[3]);
-            if (quatMag > 1e-12)
-            {
-                for (uint8_t j= 0; j<=3; j++)
-                {
-                    float quatMagInv = 1.0f/quatMag;
-                    states[j] = states[j] * quatMagInv;
-                }
-            }
-            // correct the covariance P = (I - K*H)*P
-            // take advantage of the empty columns in KH to reduce the
-            // number of operations
-            for (uint8_t i = 0; i<=indexLimit; i++)
-            {
-                for (uint8_t j = 0; j<=3; j++)
-                {
-                    KH[i][j] = Kfusion[i] * H_MAG[j];
-                }
-                for (uint8_t j = 4; j<=17; j++) KH[i][j] = 0.0f;
-                if (!onGround)
-                {
-                    for (uint8_t j = 15; j<=20; j++)
-                    {
-                        KH[i][j] = Kfusion[i] * H_MAG[j];
-                    }
-                }
-                else
-                {
-                    for (uint8_t j = 15; j<=20; j++)
-                    {
-                        KH[i][j] = 0.0f;
-                    }
-                }
-            }
-            for (uint8_t i = 0; i<=indexLimit; i++)
-            {
-                for (uint8_t j = 0; j<=indexLimit; j++)
-                {
-                    KHP[i][j] = 0.0f;
-                    for (uint8_t k = 0; k<=3; k++)
-                    {
-                        KHP[i][j] = KHP[i][j] + KH[i][k] * P[k][j];
-                    }
-                    if (!onGround)
-                    {
-                        for (uint8_t k = 15; k<=20; k++)
-                        {
-                            KHP[i][j] = KHP[i][j] + KH[i][k] * P[k][j];
-                        }
-                    }
-                }
-            }
-        }
-        for (uint8_t i = 0; i<=indexLimit; i++)
-        {
-            for (uint8_t j = 0; j<=indexLimit; j++)
-            {
-                P[i][j] = P[i][j] - KHP[i][j];
-            }
-        }
-    }
-    obsIndex = obsIndex + 1;
-
-    ForceSymmetry();
-    ConstrainVariances();
-}
-
-void AttPosEKF::FuseAirspeed()
-{
-    float vn;
-    float ve;
-    float vd;
-    float vwn;
-    float vwe;
-    const float R_TAS = 2.0f;
-    float SH_TAS[3];
-    float Kfusion[21];
-    float VtasPred;
-
-    // Copy required states to local variable names
-    vn = statesAtVtasMeasTime[4];
-    ve = statesAtVtasMeasTime[5];
-    vd = statesAtVtasMeasTime[6];
-    vwn = statesAtVtasMeasTime[13];
-    vwe = statesAtVtasMeasTime[14];
-
-    // Need to check that it is flying before fusing airspeed data
-    // Calculate the predicted airspeed
-    VtasPred = sqrtf((ve - vwe)*(ve - vwe) + (vn - vwn)*(vn - vwn) + vd*vd);
-    // Perform fusion of True Airspeed measurement
-    if (useAirspeed && fuseVtasData && (VtasPred > 1.0f) && (VtasMeas > 8.0f))
-    {
-        // Calculate observation jacobians
-        SH_TAS[0] = 1/(sqrt(sq(ve - vwe) + sq(vn - vwn) + sq(vd)));
-        SH_TAS[1] = (SH_TAS[0]*(2.0f*ve - 2*vwe))/2.0f;
-        SH_TAS[2] = (SH_TAS[0]*(2.0f*vn - 2*vwn))/2.0f;
-        
-        float H_TAS[21];
-        for (uint8_t i=0; i<=20; i++) H_TAS[i] = 0.0f;
-        H_TAS[4] = SH_TAS[2];
-        H_TAS[5] = SH_TAS[1];
-        H_TAS[6] = vd*SH_TAS[0];
-        H_TAS[13] = -SH_TAS[2];
-        H_TAS[14] = -SH_TAS[1];
-
-        // Calculate Kalman gains
-        float SK_TAS = 1.0f/(R_TAS + SH_TAS[2]*(P[4][4]*SH_TAS[2] + P[5][4]*SH_TAS[1] - P[13][4]*SH_TAS[2] - P[14][4]*SH_TAS[1] + P[6][4]*vd*SH_TAS[0]) + SH_TAS[1]*(P[4][5]*SH_TAS[2] + P[5][5]*SH_TAS[1] - P[13][5]*SH_TAS[2] - P[14][5]*SH_TAS[1] + P[6][5]*vd*SH_TAS[0]) - SH_TAS[2]*(P[4][13]*SH_TAS[2] + P[5][13]*SH_TAS[1] - P[13][13]*SH_TAS[2] - P[14][13]*SH_TAS[1] + P[6][13]*vd*SH_TAS[0]) - SH_TAS[1]*(P[4][14]*SH_TAS[2] + P[5][14]*SH_TAS[1] - P[13][14]*SH_TAS[2] - P[14][14]*SH_TAS[1] + P[6][14]*vd*SH_TAS[0]) + vd*SH_TAS[0]*(P[4][6]*SH_TAS[2] + P[5][6]*SH_TAS[1] - P[13][6]*SH_TAS[2] - P[14][6]*SH_TAS[1] + P[6][6]*vd*SH_TAS[0]));
-        Kfusion[0] = SK_TAS*(P[0][4]*SH_TAS[2] - P[0][13]*SH_TAS[2] + P[0][5]*SH_TAS[1] - P[0][14]*SH_TAS[1] + P[0][6]*vd*SH_TAS[0]);
-        Kfusion[1] = SK_TAS*(P[1][4]*SH_TAS[2] - P[1][13]*SH_TAS[2] + P[1][5]*SH_TAS[1] - P[1][14]*SH_TAS[1] + P[1][6]*vd*SH_TAS[0]);
-        Kfusion[2] = SK_TAS*(P[2][4]*SH_TAS[2] - P[2][13]*SH_TAS[2] + P[2][5]*SH_TAS[1] - P[2][14]*SH_TAS[1] + P[2][6]*vd*SH_TAS[0]);
-        Kfusion[3] = SK_TAS*(P[3][4]*SH_TAS[2] - P[3][13]*SH_TAS[2] + P[3][5]*SH_TAS[1] - P[3][14]*SH_TAS[1] + P[3][6]*vd*SH_TAS[0]);
-        Kfusion[4] = SK_TAS*(P[4][4]*SH_TAS[2] - P[4][13]*SH_TAS[2] + P[4][5]*SH_TAS[1] - P[4][14]*SH_TAS[1] + P[4][6]*vd*SH_TAS[0]);
-        Kfusion[5] = SK_TAS*(P[5][4]*SH_TAS[2] - P[5][13]*SH_TAS[2] + P[5][5]*SH_TAS[1] - P[5][14]*SH_TAS[1] + P[5][6]*vd*SH_TAS[0]);
-        Kfusion[6] = SK_TAS*(P[6][4]*SH_TAS[2] - P[6][13]*SH_TAS[2] + P[6][5]*SH_TAS[1] - P[6][14]*SH_TAS[1] + P[6][6]*vd*SH_TAS[0]);
-        Kfusion[7] = SK_TAS*(P[7][4]*SH_TAS[2] - P[7][13]*SH_TAS[2] + P[7][5]*SH_TAS[1] - P[7][14]*SH_TAS[1] + P[7][6]*vd*SH_TAS[0]);
-        Kfusion[8] = SK_TAS*(P[8][4]*SH_TAS[2] - P[8][13]*SH_TAS[2] + P[8][5]*SH_TAS[1] - P[8][14]*SH_TAS[1] + P[8][6]*vd*SH_TAS[0]);
-        Kfusion[9] = SK_TAS*(P[9][4]*SH_TAS[2] - P[9][13]*SH_TAS[2] + P[9][5]*SH_TAS[1] - P[9][14]*SH_TAS[1] + P[9][6]*vd*SH_TAS[0]);
-        Kfusion[10] = SK_TAS*(P[10][4]*SH_TAS[2] - P[10][13]*SH_TAS[2] + P[10][5]*SH_TAS[1] - P[10][14]*SH_TAS[1] + P[10][6]*vd*SH_TAS[0]);
-        Kfusion[11] = SK_TAS*(P[11][4]*SH_TAS[2] - P[11][13]*SH_TAS[2] + P[11][5]*SH_TAS[1] - P[11][14]*SH_TAS[1] + P[11][6]*vd*SH_TAS[0]);
-        Kfusion[12] = SK_TAS*(P[12][4]*SH_TAS[2] - P[12][13]*SH_TAS[2] + P[12][5]*SH_TAS[1] - P[12][14]*SH_TAS[1] + P[12][6]*vd*SH_TAS[0]);
-        Kfusion[13] = SK_TAS*(P[13][4]*SH_TAS[2] - P[13][13]*SH_TAS[2] + P[13][5]*SH_TAS[1] - P[13][14]*SH_TAS[1] + P[13][6]*vd*SH_TAS[0]);
-        Kfusion[14] = SK_TAS*(P[14][4]*SH_TAS[2] - P[14][13]*SH_TAS[2] + P[14][5]*SH_TAS[1] - P[14][14]*SH_TAS[1] + P[14][6]*vd*SH_TAS[0]);
-        Kfusion[15] = SK_TAS*(P[15][4]*SH_TAS[2] - P[15][13]*SH_TAS[2] + P[15][5]*SH_TAS[1] - P[15][14]*SH_TAS[1] + P[15][6]*vd*SH_TAS[0]);
-        Kfusion[16] = SK_TAS*(P[16][4]*SH_TAS[2] - P[16][13]*SH_TAS[2] + P[16][5]*SH_TAS[1] - P[16][14]*SH_TAS[1] + P[16][6]*vd*SH_TAS[0]);
-        Kfusion[17] = SK_TAS*(P[17][4]*SH_TAS[2] - P[17][13]*SH_TAS[2] + P[17][5]*SH_TAS[1] - P[17][14]*SH_TAS[1] + P[17][6]*vd*SH_TAS[0]);
-        Kfusion[18] = SK_TAS*(P[18][4]*SH_TAS[2] - P[18][13]*SH_TAS[2] + P[18][5]*SH_TAS[1] - P[18][14]*SH_TAS[1] + P[18][6]*vd*SH_TAS[0]);
-        Kfusion[19] = SK_TAS*(P[19][4]*SH_TAS[2] - P[19][13]*SH_TAS[2] + P[19][5]*SH_TAS[1] - P[19][14]*SH_TAS[1] + P[19][6]*vd*SH_TAS[0]);
-        Kfusion[20] = SK_TAS*(P[20][4]*SH_TAS[2] - P[20][13]*SH_TAS[2] + P[20][5]*SH_TAS[1] - P[20][14]*SH_TAS[1] + P[20][6]*vd*SH_TAS[0]);
-        varInnovVtas = 1.0f/SK_TAS;
-
-        // Calculate the measurement innovation
-        innovVtas = VtasPred - VtasMeas;
-        // Check the innovation for consistency and don't fuse if > 5Sigma
-        if ((innovVtas*innovVtas*SK_TAS) < 25.0)
-        {
-            // correct the state vector
-            for (uint8_t j=0; j<=20; j++)
-            {
-                states[j] = states[j] - Kfusion[j] * innovVtas;
-            }
-            // normalise the quaternion states
-            float quatMag = sqrt(states[0]*states[0] + states[1]*states[1] + states[2]*states[2] + states[3]*states[3]);
-            if (quatMag > 1e-12f)
-            {
-                for (uint8_t j= 0; j<=3; j++)
-                {
-                    float quatMagInv = 1.0f/quatMag;
-                    states[j] = states[j] * quatMagInv;
-                }
-            }
-            // correct the covariance P = (I - K*H)*P
-            // take advantage of the empty columns in H to reduce the
-            // number of operations
-            for (uint8_t i = 0; i<=20; i++)
-            {
-                for (uint8_t j = 0; j<=3; j++) KH[i][j] = 0.0;
-                for (uint8_t j = 4; j<=6; j++)
-                {
-                    KH[i][j] = Kfusion[i] * H_TAS[j];
-                }
-                for (uint8_t j = 7; j<=12; j++) KH[i][j] = 0.0;
-                for (uint8_t j = 13; j<=14; j++)
-                {
-                    KH[i][j] = Kfusion[i] * H_TAS[j];
-                }
-                for (uint8_t j = 15; j<=20; j++) KH[i][j] = 0.0;
-            }
-            for (uint8_t i = 0; i<=20; i++)
-            {
-                for (uint8_t j = 0; j<=20; j++)
-                {
-                    KHP[i][j] = 0.0;
-                    for (uint8_t k = 4; k<=6; k++)
-                    {
-                        KHP[i][j] = KHP[i][j] + KH[i][k] * P[k][j];
-                    }
-                    for (uint8_t k = 13; k<=14; k++)
-                    {
-                        KHP[i][j] = KHP[i][j] + KH[i][k] * P[k][j];
-                    }
-                }
-            }
-            for (uint8_t i = 0; i<=20; i++)
-            {
-                for (uint8_t j = 0; j<=20; j++)
-                {
-                    P[i][j] = P[i][j] - KHP[i][j];
-                }
-            }
-        }
-    }
-
-    ForceSymmetry();
-    ConstrainVariances();
-}
-
-void AttPosEKF::zeroRows(float (&covMat)[n_states][n_states], uint8_t first, uint8_t last)
-{
-    uint8_t row;
-    uint8_t col;
-    for (row=first; row<=last; row++)
-    {
-        for (col=0; col<n_states; col++)
-        {
-            covMat[row][col] = 0.0;
-        }
-    }
-}
-
-void AttPosEKF::zeroCols(float (&covMat)[n_states][n_states], uint8_t first, uint8_t last)
-{
-    uint8_t row;
-    uint8_t col;
-    for (col=first; col<=last; col++)
-    {
-        for (row=0; row < n_states; row++)
-        {
-            covMat[row][col] = 0.0;
-        }
-    }
-}
-
-float AttPosEKF::sq(float valIn)
-{
-    return valIn*valIn;
-}
-
-// Store states in a history array along with time stamp
-void AttPosEKF::StoreStates(uint64_t timestamp_ms)
-{
-    for (unsigned i=0; i<n_states; i++)
-        storedStates[i][storeIndex] = states[i];
-    statetimeStamp[storeIndex] = timestamp_ms;
-    storeIndex++;
-    if (storeIndex == data_buffer_size)
-        storeIndex = 0;
-}
-
-void AttPosEKF::ResetStoredStates()
-{
-    // reset all stored states
-    memset(&storedStates[0][0], 0, sizeof(storedStates));
-    memset(&statetimeStamp[0], 0, sizeof(statetimeStamp));
-
-    // reset store index to first
-    storeIndex = 0;
-
-    // overwrite all existing states
-    for (unsigned i = 0; i < n_states; i++) {
-        storedStates[i][storeIndex] = states[i];
-    }
-
-    statetimeStamp[storeIndex] = millis();
-
-    // increment to next storage index
-    storeIndex++;
-}
-
-// Output the state vector stored at the time that best matches that specified by msec
-int AttPosEKF::RecallStates(float statesForFusion[n_states], uint64_t msec)
-{
-    int ret = 0;
-
-    // int64_t bestTimeDelta = 200;
-    // unsigned bestStoreIndex = 0;
-    // for (unsigned storeIndex = 0; storeIndex < data_buffer_size; storeIndex++)
-    // {
-    //     // The time delta can also end up as negative number,
-    //     // since we might compare future to past or past to future
-    //     // therefore cast to int64.
-    //     int64_t timeDelta = (int64_t)msec - (int64_t)statetimeStamp[storeIndex];
-    //     if (timeDelta < 0) {
-    //         timeDelta = -timeDelta;
-    //     }
-
-    //     if (timeDelta < bestTimeDelta)
-    //     {
-    //         bestStoreIndex = storeIndex;
-    //         bestTimeDelta = timeDelta;
-    //     }
-    // }
-    // if (bestTimeDelta < 200) // only output stored state if < 200 msec retrieval error
-    // {
-    //     for (uint8_t i=0; i < n_states; i++) {
-    //         if (isfinite(storedStates[i][bestStoreIndex])) {
-    //             statesForFusion[i] = storedStates[i][bestStoreIndex];
-    //         } else if (isfinite(states[i])) {
-    //             statesForFusion[i] = states[i];
-    //         } else {
-    //             // There is not much we can do here, except reporting the error we just
-    //             // found.
-    //             ret++;
-    //         }
-    // }
-    // else // otherwise output current state
-    // {
-        for (uint8_t i=0; i < n_states; i++) {
-            if (isfinite(states[i])) {
-                statesForFusion[i] = states[i];
-            } else {
-                ret++;
-            }
-        }
-    // }
-
-    return ret;
-}
-
-void AttPosEKF::quat2Tnb(Mat3f &Tnb, const float (&quat)[4])
-{
-    // Calculate the nav to body cosine matrix
-    float q00 = sq(quat[0]);
-    float q11 = sq(quat[1]);
-    float q22 = sq(quat[2]);
-    float q33 = sq(quat[3]);
-    float q01 =  quat[0]*quat[1];
-    float q02 =  quat[0]*quat[2];
-    float q03 =  quat[0]*quat[3];
-    float q12 =  quat[1]*quat[2];
-    float q13 =  quat[1]*quat[3];
-    float q23 =  quat[2]*quat[3];
-
-    Tnb.x.x = q00 + q11 - q22 - q33;
-    Tnb.y.y = q00 - q11 + q22 - q33;
-    Tnb.z.z = q00 - q11 - q22 + q33;
-    Tnb.y.x = 2*(q12 - q03);
-    Tnb.z.x = 2*(q13 + q02);
-    Tnb.x.y = 2*(q12 + q03);
-    Tnb.z.y = 2*(q23 - q01);
-    Tnb.x.z = 2*(q13 - q02);
-    Tnb.y.z = 2*(q23 + q01);
-}
-
-void AttPosEKF::quat2Tbn(Mat3f &Tbn, const float (&quat)[4])
-{
-    // Calculate the body to nav cosine matrix
-    float q00 = sq(quat[0]);
-    float q11 = sq(quat[1]);
-    float q22 = sq(quat[2]);
-    float q33 = sq(quat[3]);
-    float q01 =  quat[0]*quat[1];
-    float q02 =  quat[0]*quat[2];
-    float q03 =  quat[0]*quat[3];
-    float q12 =  quat[1]*quat[2];
-    float q13 =  quat[1]*quat[3];
-    float q23 =  quat[2]*quat[3];
-
-    Tbn.x.x = q00 + q11 - q22 - q33;
-    Tbn.y.y = q00 - q11 + q22 - q33;
-    Tbn.z.z = q00 - q11 - q22 + q33;
-    Tbn.x.y = 2*(q12 - q03);
-    Tbn.x.z = 2*(q13 + q02);
-    Tbn.y.x = 2*(q12 + q03);
-    Tbn.y.z = 2*(q23 - q01);
-    Tbn.z.x = 2*(q13 - q02);
-    Tbn.z.y = 2*(q23 + q01);
-}
-
-void AttPosEKF::eul2quat(float (&quat)[4], const float (&eul)[3])
-{
-    float u1 = cos(0.5f*eul[0]);
-    float u2 = cos(0.5f*eul[1]);
-    float u3 = cos(0.5f*eul[2]);
-    float u4 = sin(0.5f*eul[0]);
-    float u5 = sin(0.5f*eul[1]);
-    float u6 = sin(0.5f*eul[2]);
-    quat[0] = u1*u2*u3+u4*u5*u6;
-    quat[1] = u4*u2*u3-u1*u5*u6;
-    quat[2] = u1*u5*u3+u4*u2*u6;
-    quat[3] = u1*u2*u6-u4*u5*u3;
-}
-
-void AttPosEKF::quat2eul(float (&y)[3], const float (&u)[4])
-{
-    y[0] = atan2f((2.0f*(u[2]*u[3]+u[0]*u[1])) , (u[0]*u[0]-u[1]*u[1]-u[2]*u[2]+u[3]*u[3]));
-    y[1] = -asinf(2.0f*(u[1]*u[3]-u[0]*u[2]));
-    y[2] = atan2f((2.0f*(u[1]*u[2]+u[0]*u[3])) , (u[0]*u[0]+u[1]*u[1]-u[2]*u[2]-u[3]*u[3]));
-}
-
-void AttPosEKF::calcvelNED(float (&velNED)[3], float gpsCourse, float gpsGndSpd, float gpsVelD)
-{
-    velNED[0] = gpsGndSpd*cosf(gpsCourse);
-    velNED[1] = gpsGndSpd*sinf(gpsCourse);
-    velNED[2] = gpsVelD;
-}
-
-void AttPosEKF::calcposNED(float (&posNED)[3], float lat, float lon, float hgt, float latRef, float lonRef, float hgtRef)
-{
-    posNED[0] = earthRadius * (lat - latRef);
-    posNED[1] = earthRadius * cos(latRef) * (lon - lonRef);
-    posNED[2] = -(hgt - hgtRef);
-}
-
-void AttPosEKF::calcLLH(float (&posNED)[3], float lat, float lon, float hgt, float latRef, float lonRef, float hgtRef)
-{
-    lat = latRef + posNED[0] * earthRadiusInv;
-    lon = lonRef + posNED[1] * earthRadiusInv / cos(latRef);
-    hgt = hgtRef - posNED[2];
-}
-
-void AttPosEKF::OnGroundCheck()
-{
-    onGround = (((sq(velNED[0]) + sq(velNED[1]) + sq(velNED[2])) < 4.0f) && (VtasMeas < 8.0f));
-    if (staticMode) {
-        staticMode = !(GPSstatus > GPS_FIX_2D);
-    }
-}
-
-void AttPosEKF::calcEarthRateNED(Vector3f &omega, float latitude)
-{
-    //Define Earth rotation vector in the NED navigation frame
-    omega.x  = earthRate*cosf(latitude);
-    omega.y  = 0.0f;
-    omega.z  = -earthRate*sinf(latitude);
-}
-
-void AttPosEKF::CovarianceInit()
-{
-    // Calculate the initial covariance matrix P
-    P[0][0]   = 0.25f * sq(1.0f*deg2rad);
-    P[1][1]   = 0.25f * sq(1.0f*deg2rad);
-    P[2][2]   = 0.25f * sq(1.0f*deg2rad);
-    P[3][3]   = 0.25f * sq(10.0f*deg2rad);
-    P[4][4]   = sq(0.7);
-    P[5][5]   = P[4][4];
-    P[6][6]   = sq(0.7);
-    P[7][7]   = sq(15.0);
-    P[8][8]   = P[7][7];
-    P[9][9]   = sq(5.0);
-    P[10][10] = sq(0.1*deg2rad*dtIMU);
-    P[11][11] = P[10][10];
-    P[12][12] = P[10][10];
-    P[13][13] = sq(8.0f);
-    P[14][4]  = P[13][13];
-    P[15][15] = sq(0.02f);
-    P[16][16] = P[15][15];
-    P[17][17] = P[15][15];
-    P[18][18] = sq(0.02f);
-    P[19][19] = P[18][18];
-    P[20][20] = P[18][18];
-}
-
-float AttPosEKF::ConstrainFloat(float val, float min, float max)
-{
-    return (val > max) ? max : ((val < min) ? min : val);
-}
-
-void AttPosEKF::ConstrainVariances()
-{
-    if (!numericalProtection) {
-        return;
-    }
-
-    // State vector:
-    // 0-3: quaternions (q0, q1, q2, q3)
-    // 4-6: Velocity - m/sec (North, East, Down)
-    // 7-9: Position - m (North, East, Down)
-    // 10-12: Delta Angle bias - rad (X,Y,Z)
-    // 13-14: Wind Vector  - m/sec (North,East)
-    // 15-17: Earth Magnetic Field Vector - gauss (North, East, Down)
-    // 18-20: Body Magnetic Field Vector - gauss (X,Y,Z)
-
-    // Constrain quaternion variances
-    for (unsigned i = 0; i < 4; i++) {
-        P[i][i] = ConstrainFloat(P[i][i], 0.0f, 1.0f);
-    }
-
-    // Constrain velocitie variances
-    for (unsigned i = 4; i < 7; i++) {
-        P[i][i] = ConstrainFloat(P[i][i], 0.0f, 1.0e3f);
-    }
-
-    // Constrain position variances
-    for (unsigned i = 7; i < 10; i++) {
-        P[i][i] = ConstrainFloat(P[i][i], 0.0f, 1.0e6f);
-    }
-
-    // Angle bias variances
-    for (unsigned i = 10; i < 13; i++) {
-        P[i][i] = ConstrainFloat(P[i][i], 0.0f, sq(0.175f * dtIMU));
-    }
-
-    // Wind velocity variances
-    for (unsigned i = 13; i < 15; i++) {
-        P[i][i] = ConstrainFloat(P[i][i], 0.0f, 1.0e3f);
-    }
-
-    // Earth magnetic field variances
-    for (unsigned i = 15; i < 18; i++) {
-        P[i][i] = ConstrainFloat(P[i][i], 0.0f, 1.0f);
-    }
-
-    // Body magnetic field variances
-    for (unsigned i = 18; i < 21; i++) {
-        P[i][i] = ConstrainFloat(P[i][i], 0.0f, 1.0f);
-    }
-
-}
-
-void AttPosEKF::ConstrainStates()
-{
-    if (!numericalProtection) {
-        return;
-    }
-
-    // State vector:
-    // 0-3: quaternions (q0, q1, q2, q3)
-    // 4-6: Velocity - m/sec (North, East, Down)
-    // 7-9: Position - m (North, East, Down)
-    // 10-12: Delta Angle bias - rad (X,Y,Z)
-    // 13-14: Wind Vector  - m/sec (North,East)
-    // 15-17: Earth Magnetic Field Vector - gauss (North, East, Down)
-    // 18-20: Body Magnetic Field Vector - gauss (X,Y,Z)
-
-
-    // Constrain quaternion
-    for (unsigned i = 0; i < 4; i++) {
-        states[i] = ConstrainFloat(states[i], -1.0f, 1.0f);
-    }
-
-    // Constrain velocities to what GPS can do for us
-    for (unsigned i = 4; i < 7; i++) {
-        states[i] = ConstrainFloat(states[i], -5.0e2f, 5.0e2f);
-    }
-
-    // Constrain position to a reasonable vehicle range (in meters)
-    for (unsigned i = 7; i < 9; i++) {
-        states[i] = ConstrainFloat(states[i], -1.0e6f, 1.0e6f);
-    }
-
-    // Constrain altitude
-    states[9] = ConstrainFloat(states[9], -4.0e4f, 1.0e4f);
-
-    // Angle bias limit - set to 8 degrees / sec
-    for (unsigned i = 10; i < 13; i++) {
-        states[i] = ConstrainFloat(states[i], -0.12f * dtIMU, 0.12f * dtIMU);
-    }
-
-    // Wind velocity limits - assume 120 m/s max velocity
-    for (unsigned i = 13; i < 15; i++) {
-        states[i] = ConstrainFloat(states[i], -120.0f, 120.0f);
-    }
-
-    // Earth magnetic field limits (in Gauss)
-    for (unsigned i = 15; i < 18; i++) {
-        states[i] = ConstrainFloat(states[i], -1.0f, 1.0f);
-    }
-
-    // Body magnetic field variances (in Gauss).
-    // the max offset should be in this range.
-    for (unsigned i = 18; i < 21; i++) {
-        states[i] = ConstrainFloat(states[i], -0.5f, 0.5f);
-    }
-
-}
-
-void AttPosEKF::ForceSymmetry()
-{
-    if (!numericalProtection) {
-        return;
-    }
-
-    // Force symmetry on the covariance matrix to prevent ill-conditioning
-    // of the matrix which would cause the filter to blow-up
-    for (unsigned i = 1; i < n_states; i++)
-    {
-        for (uint8_t j = 0; j < i; j++)
-        {
-            P[i][j] = 0.5f * (P[i][j] + P[j][i]);
-            P[j][i] = P[i][j];
-        }
-    }
-}
-
-bool AttPosEKF::FilterHealthy()
-{
-    if (!statesInitialised) {
-        return false;
-    }
-
-    // XXX Check state vector for NaNs and ill-conditioning
-
-    // Check if any of the major inputs timed out
-    if (current_ekf_state.posTimeout || current_ekf_state.velTimeout || current_ekf_state.hgtTimeout) {
-        return false;
-    }
-
-    // Nothing fired, return ok.
-    return true;
-}
-
-/**
- * Reset the filter position states
- *
- * This resets the position to the last GPS measurement
- * or to zero in case of static position.
- */
-void AttPosEKF::ResetPosition(void)
-{
-    if (staticMode) {
-        states[7] = 0;
-        states[8] = 0;
-    } else if (GPSstatus >= GPS_FIX_3D) {
-
-        // reset the states from the GPS measurements
-        states[7] = posNE[0];
-        states[8] = posNE[1];
-    }
-}
-
-/**
- * Reset the height state.
- *
- * This resets the height state with the last altitude measurements
- */
-void AttPosEKF::ResetHeight(void)
-{
-    // write to the state vector
-    states[9]   = -hgtMea;
-}
-
-/**
- * Reset the velocity state.
- */
-void AttPosEKF::ResetVelocity(void)
-{
-    if (staticMode) {
-        states[4] = 0.0f;
-        states[5] = 0.0f;
-        states[6] = 0.0f;
-    } else if (GPSstatus >= GPS_FIX_3D) {
-
-        states[4]  = velNED[0]; // north velocity from last reading
-        states[5]  = velNED[1]; // east velocity from last reading
-        states[6]  = velNED[2]; // down velocity from last reading
-    }
-}
-
-
-void AttPosEKF::FillErrorReport(struct ekf_status_report *err)
-{
-    for (int i = 0; i < n_states; i++)
-    {
-        err->states[i] = states[i];
-    }
-
-    err->velHealth = current_ekf_state.velHealth;
-    err->posHealth = current_ekf_state.posHealth;
-    err->hgtHealth = current_ekf_state.hgtHealth;
-    err->velTimeout = current_ekf_state.velTimeout;
-    err->posTimeout = current_ekf_state.posTimeout;
-    err->hgtTimeout = current_ekf_state.hgtTimeout;
-}
-
-bool AttPosEKF::StatesNaN(struct ekf_status_report *err_report) {
-    bool err = false;
-
-    // check all states and covariance matrices
-    for (unsigned i = 0; i < n_states; i++) {
-        for (unsigned j = 0; j < n_states; j++) {
-            if (!isfinite(KH[i][j])) {
-
-                err_report->covarianceNaN = true;
-                err = true;
-            } //  intermediate result used for covariance updates
-            if (!isfinite(KHP[i][j])) {
-
-                err_report->covarianceNaN = true;
-                err = true;
-            } // intermediate result used for covariance updates
-            if (!isfinite(P[i][j])) {
-
-                err_report->covarianceNaN = true;
-                err = true;
-            } // covariance matrix
-        }
-
-        if (!isfinite(Kfusion[i])) {
-
-            err_report->kalmanGainsNaN = true;
-            err = true;
-        } // Kalman gains
-
-        if (!isfinite(states[i])) {
-
-            err_report->statesNaN = true;
-            err = true;
-        } // state matrix
-    }
-
-    if (err) {
-        FillErrorReport(err_report);
-    }
-
-    return err;
-
-}
-
-/**
- * Check the filter inputs and bound its operational state
- *
- * This check will reset the filter states if required
- * due to a failure of consistency or timeout checks.
- * it should be run after the measurement data has been
- * updated, but before any of the fusion steps are
- * executed.
- */
-int AttPosEKF::CheckAndBound()
-{
-
-    // Store the old filter state
-    bool currStaticMode = staticMode;
-
-    // Reset the filter if the states went NaN
-    if (StatesNaN(&last_ekf_error)) {
-
-        InitializeDynamic(velNED, magDeclination);
-
-        return 1;
-    }
-
-    // Reset the filter if the IMU data is too old
-    if (dtIMU > 0.2f) {
-
-        ResetVelocity();
-        ResetPosition();
-        ResetHeight();
-        ResetStoredStates();
-
-        // that's all we can do here, return
-        return 2;
-    }
-
-    // Check if we're on ground - this also sets static mode.
-    OnGroundCheck();
-
-    // Check if we switched between states
-    if (currStaticMode != staticMode) {
-        ResetVelocity();
-        ResetPosition();
-        ResetHeight();
-        ResetStoredStates();
-
-        return 3;
-    }
-
-    return 0;
-}
-
-void AttPosEKF::AttitudeInit(float ax, float ay, float az, float mx, float my, float mz, float declination, float *initQuat)
-{
-    float initialRoll, initialPitch;
-    float cosRoll, sinRoll, cosPitch, sinPitch;
-    float magX, magY;
-    float initialHdg, cosHeading, sinHeading;
-
-    initialRoll = atan2(-ay, -az);
-    initialPitch = atan2(ax, -az);
-
-    cosRoll = cosf(initialRoll);
-    sinRoll = sinf(initialRoll);
-    cosPitch = cosf(initialPitch);
-    sinPitch = sinf(initialPitch);
-
-    magX = mx * cosPitch + my * sinRoll * sinPitch + mz * cosRoll * sinPitch;
-
-    magY = my * cosRoll - mz * sinRoll;
-
-    initialHdg = atan2f(-magY, magX);
-    /* true heading is the mag heading minus declination */
-    initialHdg += declination;
-
-    cosRoll = cosf(initialRoll * 0.5f);
-    sinRoll = sinf(initialRoll * 0.5f);
-
-    cosPitch = cosf(initialPitch * 0.5f);
-    sinPitch = sinf(initialPitch * 0.5f);
-
-    cosHeading = cosf(initialHdg * 0.5f);
-    sinHeading = sinf(initialHdg * 0.5f);
-
-    initQuat[0] = cosRoll * cosPitch * cosHeading + sinRoll * sinPitch * sinHeading;
-    initQuat[1] = sinRoll * cosPitch * cosHeading - cosRoll * sinPitch * sinHeading;
-    initQuat[2] = cosRoll * sinPitch * cosHeading + sinRoll * cosPitch * sinHeading;
-    initQuat[3] = cosRoll * cosPitch * sinHeading - sinRoll * sinPitch * cosHeading;
-}
-
-void AttPosEKF::InitializeDynamic(float (&initvelNED)[3], float declination)
-{
-
-    // Clear the init flag
-    statesInitialised = false;
-
-    magDeclination = declination;
-
-    ZeroVariables();
-
-    // Calculate initial filter quaternion states from raw measurements
-    float initQuat[4];
-    Vector3f initMagXYZ;
-    initMagXYZ   = magData - magBias;
-    AttitudeInit(accel.x, accel.y, accel.z, initMagXYZ.x, initMagXYZ.y, initMagXYZ.z, magDeclination, initQuat);
-
-    // Calculate initial Tbn matrix and rotate Mag measurements into NED
-    // to set initial NED magnetic field states
-    Mat3f DCM;
-    quat2Tbn(DCM, initQuat);
-    Vector3f initMagNED;
-    initMagXYZ   = magData - magBias;
-    initMagNED.x = DCM.x.x*initMagXYZ.x + DCM.x.y*initMagXYZ.y + DCM.x.z*initMagXYZ.z;
-    initMagNED.y = DCM.y.x*initMagXYZ.x + DCM.y.y*initMagXYZ.y + DCM.y.z*initMagXYZ.z;
-    initMagNED.z = DCM.z.x*initMagXYZ.x + DCM.z.y*initMagXYZ.y + DCM.z.z*initMagXYZ.z;
-
-
-
-    // write to state vector
-    for (uint8_t j=0; j<=3; j++) states[j] = initQuat[j]; // quaternions
-    for (uint8_t j=0; j<=2; j++) states[j+4] = initvelNED[j]; // velocities
-    for (uint8_t j=0; j<=7; j++) states[j+7] = 0.0f; // positiions, dAngBias, windVel
-    states[15] = initMagNED.x; // Magnetic Field North
-    states[16] = initMagNED.y; // Magnetic Field East
-    states[17] = initMagNED.z; // Magnetic Field Down
-    states[18] = magBias.x; // Magnetic Field Bias X
-    states[19] = magBias.y; // Magnetic Field Bias Y
-    states[20] = magBias.z; // Magnetic Field Bias Z
-
-    statesInitialised = true;
-
-    // initialise the covariance matrix
-    CovarianceInit();
-
-    //Define Earth rotation vector in the NED navigation frame
-    calcEarthRateNED(earthRateNED, latRef);
-
-    //Initialise summed variables used by covariance prediction
-    summedDelAng.x = 0.0f;
-    summedDelAng.y = 0.0f;
-    summedDelAng.z = 0.0f;
-    summedDelVel.x = 0.0f;
-    summedDelVel.y = 0.0f;
-    summedDelVel.z = 0.0f;
-}
-
-void AttPosEKF::InitialiseFilter(float (&initvelNED)[3], double referenceLat, double referenceLon, float referenceHgt, float declination)
-{
-    //store initial lat,long and height
-    latRef = referenceLat;
-    lonRef = referenceLon;
-    hgtRef = referenceHgt;
-
-    memset(&last_ekf_error, 0, sizeof(last_ekf_error));
-
-    InitializeDynamic(initvelNED, declination);
-}
-
-void AttPosEKF::ZeroVariables()
-{
-    // Do the data structure init
-    for (unsigned i = 0; i < n_states; i++) {
-        for (unsigned j = 0; j < n_states; j++) {
-            KH[i][j] = 0.0f; //  intermediate result used for covariance updates
-            KHP[i][j] = 0.0f; // intermediate result used for covariance updates
-            P[i][j] = 0.0f; // covariance matrix
-        }
-
-        Kfusion[i] = 0.0f; // Kalman gains
-        states[i] = 0.0f; // state matrix
-    }
-
-    for (unsigned i = 0; i < data_buffer_size; i++) {
-
-        for (unsigned j = 0; j < n_states; j++) {
-            storedStates[j][i] = 0.0f;
-        }
-
-        statetimeStamp[i] = 0;
-    }
-
-    memset(&current_ekf_state, 0, sizeof(current_ekf_state));
-}
-
-void AttPosEKF::GetFilterState(struct ekf_status_report *state)
-{
-    memcpy(state, &current_ekf_state, sizeof(state));
-}
-
-void AttPosEKF::GetLastErrorState(struct ekf_status_report *last_error)
-{
-    memcpy(last_error, &last_ekf_error, sizeof(last_error));
-}
diff --git a/src/modules/ekf_att_pos_estimator/estimator_21states.h b/src/modules/ekf_att_pos_estimator/estimator_21states.h
deleted file mode 100644
index a19ff1995..000000000
--- a/src/modules/ekf_att_pos_estimator/estimator_21states.h
+++ /dev/null
@@ -1,247 +0,0 @@
-#pragma once
-
-#include "estimator_utilities.h"
-
-class AttPosEKF {
-
-public:
-
-    AttPosEKF();
-    ~AttPosEKF();
-
-    /* ##############################################
-     *
-     *   M A I N    F I L T E R    P A R A M E T E R S
-     *
-     * ########################################### */
-
-    /*
-     * parameters are defined here and initialised in
-     * the InitialiseParameters() (which is just 20 lines down)
-     */
-
-    float covTimeStepMax; // maximum time allowed between covariance predictions
-    float covDelAngMax; // maximum delta angle between covariance predictions
-    float rngFinderPitch; // pitch angle of laser range finder in radians. Zero is aligned with the Z body axis. Positive is RH rotation about Y body axis.
-
-    float yawVarScale;
-    float windVelSigma;
-    float dAngBiasSigma;
-    float dVelBiasSigma;
-    float magEarthSigma;
-    float magBodySigma;
-    float gndHgtSigma;
-
-    float vneSigma;
-    float vdSigma;
-    float posNeSigma;
-    float posDSigma;
-    float magMeasurementSigma;
-    float airspeedMeasurementSigma;
-
-    float gyroProcessNoise;
-    float accelProcessNoise;
-
-    float EAS2TAS; // ratio f true to equivalent airspeed
-
-    void InitialiseParameters()
-    {
-        covTimeStepMax = 0.07f; // maximum time allowed between covariance predictions
-        covDelAngMax = 0.02f; // maximum delta angle between covariance predictions
-        rngFinderPitch = 0.0f; // pitch angle of laser range finder in radians. Zero is aligned with the Z body axis. Positive is RH rotation about Y body axis.
-        EAS2TAS = 1.0f;
-
-        yawVarScale = 1.0f;
-        windVelSigma = 0.1f;
-        dAngBiasSigma = 5.0e-7f;
-        dVelBiasSigma = 1e-4f;
-        magEarthSigma = 3.0e-4f;
-        magBodySigma  = 3.0e-4f;
-        gndHgtSigma  = 0.02f; // assume 2% terrain gradient 1-sigma
-
-        vneSigma = 0.2f;
-        vdSigma = 0.3f;
-        posNeSigma = 2.0f;
-        posDSigma = 2.0f;
-
-        magMeasurementSigma = 0.05;
-        airspeedMeasurementSigma = 1.4f;
-        gyroProcessNoise = 1.4544411e-2f;
-        accelProcessNoise = 0.5f;
-    }
-
-    // Global variables
-    float KH[n_states][n_states]; //  intermediate result used for covariance updates
-    float KHP[n_states][n_states]; // intermediate result used for covariance updates
-    float P[n_states][n_states]; // covariance matrix
-    float Kfusion[n_states]; // Kalman gains
-    float states[n_states]; // state matrix
-    float storedStates[n_states][data_buffer_size]; // state vectors stored for the last 50 time steps
-    uint32_t statetimeStamp[data_buffer_size]; // time stamp for each state vector stored
-
-    float statesAtVelTime[n_states]; // States at the effective measurement time for posNE and velNED measurements
-    float statesAtPosTime[n_states]; // States at the effective measurement time for posNE and velNED measurements
-    float statesAtHgtTime[n_states]; // States at the effective measurement time for the hgtMea measurement
-    float statesAtMagMeasTime[n_states]; // filter satates at the effective measurement time
-    float statesAtVtasMeasTime[n_states]; // filter states at the effective measurement time
-
-    Vector3f correctedDelAng; // delta angles about the xyz body axes corrected for errors (rad)
-    Vector3f correctedDelVel; // delta velocities along the XYZ body axes corrected for errors (m/s)
-    Vector3f summedDelAng; // summed delta angles about the xyz body axes corrected for errors (rad)
-    Vector3f summedDelVel; // summed delta velocities along the XYZ body axes corrected for errors (m/s)
-    float accNavMag; // magnitude of navigation accel (- used to adjust GPS obs variance (m/s^2)
-    Vector3f earthRateNED; // earths angular rate vector in NED (rad/s)
-    Vector3f angRate; // angular rate vector in XYZ body axes measured by the IMU (rad/s)
-    Vector3f accel; // acceleration vector in XYZ body axes measured by the IMU (m/s^2)
-    Vector3f dVelIMU;
-    Vector3f dAngIMU;
-    float dtIMU; // time lapsed since the last IMU measurement or covariance update (sec)
-    uint8_t fusionModeGPS; // 0 = GPS outputs 3D velocity, 1 = GPS outputs 2D velocity, 2 = GPS outputs no velocity
-    float innovVelPos[6]; // innovation output
-    float varInnovVelPos[6]; // innovation variance output
-
-    float velNED[3]; // North, East, Down velocity obs (m/s)
-    float posNE[2]; // North, East position obs (m)
-    float hgtMea; //  measured height (m)
-    float posNED[3]; // North, East Down position (m)
-
-    float innovMag[3]; // innovation output
-    float varInnovMag[3]; // innovation variance output
-    Vector3f magData; // magnetometer flux radings in X,Y,Z body axes
-    float innovVtas; // innovation output
-    float varInnovVtas; // innovation variance output
-    float VtasMeas; // true airspeed measurement (m/s)
-    float magDeclination;
-    float latRef; // WGS-84 latitude of reference point (rad)
-    float lonRef; // WGS-84 longitude of reference point (rad)
-    float hgtRef; // WGS-84 height of reference point (m)
-    Vector3f magBias; // states representing magnetometer bias vector in XYZ body axes
-    uint8_t covSkipCount; // Number of state prediction frames (IMU daya updates to skip before doing the covariance prediction
-
-    // GPS input data variables
-    float gpsCourse;
-    float gpsVelD;
-    float gpsLat;
-    float gpsLon;
-    float gpsHgt;
-    uint8_t GPSstatus;
-
-    // Baro input
-    float baroHgt;
-
-    bool statesInitialised;
-
-    bool fuseVelData; // this boolean causes the posNE and velNED obs to be fused
-    bool fusePosData; // this boolean causes the posNE and velNED obs to be fused
-    bool fuseHgtData; // this boolean causes the hgtMea obs to be fused
-    bool fuseMagData; // boolean true when magnetometer data is to be fused
-    bool fuseVtasData; // boolean true when airspeed data is to be fused
-
-    bool onGround;    ///< boolean true when the flight vehicle is on the ground (not flying)
-    bool staticMode;    ///< boolean true if no position feedback is fused
-    bool useAirspeed;    ///< boolean true if airspeed data is being used
-    bool useCompass;    ///< boolean true if magnetometer data is being used
-
-    struct ekf_status_report current_ekf_state;
-    struct ekf_status_report last_ekf_error;
-
-    bool numericalProtection;
-
-    unsigned storeIndex;
-
-
-void  UpdateStrapdownEquationsNED();
-
-void CovariancePrediction(float dt);
-
-void FuseVelposNED();
-
-void FuseMagnetometer();
-
-void FuseAirspeed();
-
-void zeroRows(float (&covMat)[n_states][n_states], uint8_t first, uint8_t last);
-
-void zeroCols(float (&covMat)[n_states][n_states], uint8_t first, uint8_t last);
-
-void quatNorm(float (&quatOut)[4], const float quatIn[4]);
-
-// store staes along with system time stamp in msces
-void StoreStates(uint64_t timestamp_ms);
-
-/**
- * Recall the state vector.
- *
- * Recalls the vector stored at closest time to the one specified by msec
- *
- * @return zero on success, integer indicating the number of invalid states on failure.
- *         Does only copy valid states, if the statesForFusion vector was initialized
- *         correctly by the caller, the result can be safely used, but is a mixture
- *         time-wise where valid states were updated and invalid remained at the old
- *         value.
- */
-int RecallStates(float statesForFusion[n_states], uint64_t msec);
-
-void ResetStoredStates();
-
-void quat2Tbn(Mat3f &Tbn, const float (&quat)[4]);
-
-void calcEarthRateNED(Vector3f &omega, float latitude);
-
-static void eul2quat(float (&quat)[4], const float (&eul)[3]);
-
-static void quat2eul(float (&eul)[3], const float (&quat)[4]);
-
-static void calcvelNED(float (&velNED)[3], float gpsCourse, float gpsGndSpd, float gpsVelD);
-
-static void calcposNED(float (&posNED)[3], float lat, float lon, float hgt, float latRef, float lonRef, float hgtRef);
-
-static void calcLLH(float (&posNED)[3], float lat, float lon, float hgt, float latRef, float lonRef, float hgtRef);
-
-static void quat2Tnb(Mat3f &Tnb, const float (&quat)[4]);
-
-static float sq(float valIn);
-
-void OnGroundCheck();
-
-void CovarianceInit();
-
-void InitialiseFilter(float (&initvelNED)[3], double referenceLat, double referenceLon, float referenceHgt, float declination);
-
-float ConstrainFloat(float val, float min, float max);
-
-void ConstrainVariances();
-
-void ConstrainStates();
-
-void ForceSymmetry();
-
-int CheckAndBound();
-
-void ResetPosition();
-
-void ResetVelocity();
-
-void ZeroVariables();
-
-void GetFilterState(struct ekf_status_report *state);
-
-void GetLastErrorState(struct ekf_status_report *last_error);
-
-bool StatesNaN(struct ekf_status_report *err_report);
-void FillErrorReport(struct ekf_status_report *err);
-
-void InitializeDynamic(float (&initvelNED)[3], float declination);
-
-protected:
-
-bool FilterHealthy();
-
-void ResetHeight(void);
-
-void AttitudeInit(float ax, float ay, float az, float mx, float my, float mz, float declination, float *initQuat);
-
-};
-
-uint32_t millis();
-
diff --git a/src/modules/ekf_att_pos_estimator/estimator_23states.cpp b/src/modules/ekf_att_pos_estimator/estimator_22states.cpp
index c17e034ad..15d018ab6 100644
--- a/src/modules/ekf_att_pos_estimator/estimator_23states.cpp
+++ b/src/modules/ekf_att_pos_estimator/estimator_22states.cpp
@@ -1,4 +1,4 @@
-#include "estimator_23states.h"
+#include "estimator_22states.h"
 #include <string.h>
 #include <stdio.h>
 #include <stdarg.h>
@@ -14,9 +14,6 @@ AttPosEKF::AttPosEKF() :
     covTimeStepMax(0.0f),
     covDelAngMax(0.0f),
     rngFinderPitch(0.0f),
-    a1(0.0f),
-    a2(0.0f),
-    a3(0.0f),
     yawVarScale(0.0f),
     windVelSigma(0.0f),
     dAngBiasSigma(0.0f),
@@ -71,10 +68,6 @@ AttPosEKF::AttPosEKF() :
     dtVelPosFilt(0.01f),
     dtHgtFilt(0.01f),
     dtGpsFilt(0.1f),
-    windSpdFiltNorth(0.0f),
-    windSpdFiltEast(0.0f),
-    windSpdFiltAltitude(0.0f),
-    windSpdFiltClimb(0.0f),
     fusionModeGPS(0),
     innovVelPos{},
     varInnovVelPos{},
@@ -87,7 +80,6 @@ AttPosEKF::AttPosEKF() :
     innovMag{},
     varInnovMag{},
     magData{},
-    losData{},
     innovVtas(0.0f),
     innovRng(0.0f),
     innovOptFlow{},
@@ -102,6 +94,8 @@ AttPosEKF::AttPosEKF() :
     refSet(false),
     magBias(),
     covSkipCount(0),
+    lastFixTime_ms(0),
+    globalTimeStamp_ms(0),
     gpsLat(0.0),
     gpsLon(-M_PI),
     gpsHgt(0.0f),
@@ -123,6 +117,7 @@ AttPosEKF::AttPosEKF() :
 
     onGround(true),
     staticMode(true),
+    useGPS(false),
     useAirspeed(true),
     useCompass(true),
     useRangeFinder(true),
@@ -133,7 +128,7 @@ AttPosEKF::AttPosEKF() :
     current_ekf_state{},
     last_ekf_error{},
     numericalProtection(true),
-    storeIndex(0),
+    storeIndex(0),  
     storedOmega{},
     Popt{},
     flowStates{},
@@ -152,6 +147,7 @@ AttPosEKF::AttPosEKF() :
     minFlowRng(0.0f),
     moCompR_LOS(0.0f)
 {
+
     memset(&last_ekf_error, 0, sizeof(last_ekf_error));
     memset(&current_ekf_state, 0, sizeof(current_ekf_state));
     ZeroVariables();
@@ -185,9 +181,12 @@ void AttPosEKF::UpdateStrapdownEquationsNED()
     correctedDelAng.x = dAngIMU.x - states[10];
     correctedDelAng.y = dAngIMU.y - states[11];
     correctedDelAng.z = dAngIMU.z - states[12];
-    dVelIMU.x = dVelIMU.x;
-    dVelIMU.y = dVelIMU.y;
-    dVelIMU.z = dVelIMU.z - states[13];
+
+    Vector3f dVelIMURel;
+
+    dVelIMURel.x = dVelIMU.x;
+    dVelIMURel.y = dVelIMU.y;
+    dVelIMURel.z = dVelIMU.z - states[13];
 
     delAngTotal.x += correctedDelAng.x;
     delAngTotal.y += correctedDelAng.y;
@@ -267,9 +266,9 @@ void AttPosEKF::UpdateStrapdownEquationsNED()
 // transform body delta velocities to delta velocities in the nav frame
 // * and + operators have been overloaded
     //delVelNav = Tbn*dVelIMU + gravityNED*dtIMU;
-    delVelNav.x = Tbn.x.x*dVelIMU.x + Tbn.x.y*dVelIMU.y + Tbn.x.z*dVelIMU.z + gravityNED.x*dtIMU;
-    delVelNav.y = Tbn.y.x*dVelIMU.x + Tbn.y.y*dVelIMU.y + Tbn.y.z*dVelIMU.z + gravityNED.y*dtIMU;
-    delVelNav.z = Tbn.z.x*dVelIMU.x + Tbn.z.y*dVelIMU.y + Tbn.z.z*dVelIMU.z + gravityNED.z*dtIMU;
+    delVelNav.x = Tbn.x.x*dVelIMURel.x + Tbn.x.y*dVelIMURel.y + Tbn.x.z*dVelIMURel.z + gravityNED.x*dtIMU;
+    delVelNav.y = Tbn.y.x*dVelIMURel.x + Tbn.y.y*dVelIMURel.y + Tbn.y.z*dVelIMURel.z + gravityNED.y*dtIMU;
+    delVelNav.z = Tbn.z.x*dVelIMURel.x + Tbn.z.y*dVelIMURel.y + Tbn.z.z*dVelIMURel.z + gravityNED.z*dtIMU;
 
 // calculate the magnitude of the nav acceleration (required for GPS
 // variance estimation)
@@ -344,14 +343,9 @@ void AttPosEKF::CovariancePrediction(float dt)
     }
     if (!inhibitMagStates) {
         for (uint8_t i=16; i<=18; i++) processNoise[i] = dt * magEarthSigma;
-        for (uint8_t i=19; i<=21; i++) processNoise[i] = dt * magBodySigma;
-    } else {
-        for (uint8_t i=16; i<=21; i++) processNoise[i] = 0;
-    }
-    if (!inhibitGndState) {
-        processNoise[22] = dt * sqrtf(sq(states[4]) + sq(states[5])) * gndHgtSigma;
+        for (uint8_t i=19; i < n_states; i++) processNoise[i] = dt * magBodySigma;
     } else {
-        processNoise[22] = 0;
+        for (uint8_t i=16; i < n_states; i++) processNoise[i] = 0;
     }
 
     // square all sigmas
@@ -451,7 +445,6 @@ void AttPosEKF::CovariancePrediction(float dt)
     nextP[0][19] = P[0][19] + P[1][19]*SF[7] + P[2][19]*SF[9] + P[3][19]*SF[8] + P[10][19]*SF[11] + P[11][19]*SPP[7] + P[12][19]*SPP[6];
     nextP[0][20] = P[0][20] + P[1][20]*SF[7] + P[2][20]*SF[9] + P[3][20]*SF[8] + P[10][20]*SF[11] + P[11][20]*SPP[7] + P[12][20]*SPP[6];
     nextP[0][21] = P[0][21] + P[1][21]*SF[7] + P[2][21]*SF[9] + P[3][21]*SF[8] + P[10][21]*SF[11] + P[11][21]*SPP[7] + P[12][21]*SPP[6];
-    nextP[0][22] = P[0][22] + P[1][22]*SF[7] + P[2][22]*SF[9] + P[3][22]*SF[8] + P[10][22]*SF[11] + P[11][22]*SPP[7] + P[12][22]*SPP[6];
     nextP[1][0] = P[1][0] + SQ[8] + P[0][0]*SF[6] + P[2][0]*SF[5] + P[3][0]*SF[9] + P[11][0]*SPP[6] - P[12][0]*SPP[7] - (P[10][0]*q0)/2 + SF[7]*(P[1][1] + P[0][1]*SF[6] + P[2][1]*SF[5] + P[3][1]*SF[9] + P[11][1]*SPP[6] - P[12][1]*SPP[7] - (P[10][1]*q0)/2) + SF[9]*(P[1][2] + P[0][2]*SF[6] + P[2][2]*SF[5] + P[3][2]*SF[9] + P[11][2]*SPP[6] - P[12][2]*SPP[7] - (P[10][2]*q0)/2) + SF[8]*(P[1][3] + P[0][3]*SF[6] + P[2][3]*SF[5] + P[3][3]*SF[9] + P[11][3]*SPP[6] - P[12][3]*SPP[7] - (P[10][3]*q0)/2) + SF[11]*(P[1][10] + P[0][10]*SF[6] + P[2][10]*SF[5] + P[3][10]*SF[9] + P[11][10]*SPP[6] - P[12][10]*SPP[7] - (P[10][10]*q0)/2) + SPP[7]*(P[1][11] + P[0][11]*SF[6] + P[2][11]*SF[5] + P[3][11]*SF[9] + P[11][11]*SPP[6] - P[12][11]*SPP[7] - (P[10][11]*q0)/2) + SPP[6]*(P[1][12] + P[0][12]*SF[6] + P[2][12]*SF[5] + P[3][12]*SF[9] + P[11][12]*SPP[6] - P[12][12]*SPP[7] - (P[10][12]*q0)/2);
     nextP[1][1] = P[1][1] + P[0][1]*SF[6] + P[2][1]*SF[5] + P[3][1]*SF[9] + P[11][1]*SPP[6] - P[12][1]*SPP[7] + daxCov*SQ[9] - (P[10][1]*q0)/2 + SF[6]*(P[1][0] + P[0][0]*SF[6] + P[2][0]*SF[5] + P[3][0]*SF[9] + P[11][0]*SPP[6] - P[12][0]*SPP[7] - (P[10][0]*q0)/2) + SF[5]*(P[1][2] + P[0][2]*SF[6] + P[2][2]*SF[5] + P[3][2]*SF[9] + P[11][2]*SPP[6] - P[12][2]*SPP[7] - (P[10][2]*q0)/2) + SF[9]*(P[1][3] + P[0][3]*SF[6] + P[2][3]*SF[5] + P[3][3]*SF[9] + P[11][3]*SPP[6] - P[12][3]*SPP[7] - (P[10][3]*q0)/2) + SPP[6]*(P[1][11] + P[0][11]*SF[6] + P[2][11]*SF[5] + P[3][11]*SF[9] + P[11][11]*SPP[6] - P[12][11]*SPP[7] - (P[10][11]*q0)/2) - SPP[7]*(P[1][12] + P[0][12]*SF[6] + P[2][12]*SF[5] + P[3][12]*SF[9] + P[11][12]*SPP[6] - P[12][12]*SPP[7] - (P[10][12]*q0)/2) + (dayCov*sq(q3))/4 + (dazCov*sq(q2))/4 - (q0*(P[1][10] + P[0][10]*SF[6] + P[2][10]*SF[5] + P[3][10]*SF[9] + P[11][10]*SPP[6] - P[12][10]*SPP[7] - (P[10][10]*q0)/2))/2;
     nextP[1][2] = P[1][2] + SQ[5] + P[0][2]*SF[6] + P[2][2]*SF[5] + P[3][2]*SF[9] + P[11][2]*SPP[6] - P[12][2]*SPP[7] - (P[10][2]*q0)/2 + SF[4]*(P[1][0] + P[0][0]*SF[6] + P[2][0]*SF[5] + P[3][0]*SF[9] + P[11][0]*SPP[6] - P[12][0]*SPP[7] - (P[10][0]*q0)/2) + SF[8]*(P[1][1] + P[0][1]*SF[6] + P[2][1]*SF[5] + P[3][1]*SF[9] + P[11][1]*SPP[6] - P[12][1]*SPP[7] - (P[10][1]*q0)/2) + SF[6]*(P[1][3] + P[0][3]*SF[6] + P[2][3]*SF[5] + P[3][3]*SF[9] + P[11][3]*SPP[6] - P[12][3]*SPP[7] - (P[10][3]*q0)/2) + SF[11]*(P[1][12] + P[0][12]*SF[6] + P[2][12]*SF[5] + P[3][12]*SF[9] + P[11][12]*SPP[6] - P[12][12]*SPP[7] - (P[10][12]*q0)/2) - SPP[6]*(P[1][10] + P[0][10]*SF[6] + P[2][10]*SF[5] + P[3][10]*SF[9] + P[11][10]*SPP[6] - P[12][10]*SPP[7] - (P[10][10]*q0)/2) - (q0*(P[1][11] + P[0][11]*SF[6] + P[2][11]*SF[5] + P[3][11]*SF[9] + P[11][11]*SPP[6] - P[12][11]*SPP[7] - (P[10][11]*q0)/2))/2;
@@ -474,7 +467,6 @@ void AttPosEKF::CovariancePrediction(float dt)
     nextP[1][19] = P[1][19] + P[0][19]*SF[6] + P[2][19]*SF[5] + P[3][19]*SF[9] + P[11][19]*SPP[6] - P[12][19]*SPP[7] - (P[10][19]*q0)/2;
     nextP[1][20] = P[1][20] + P[0][20]*SF[6] + P[2][20]*SF[5] + P[3][20]*SF[9] + P[11][20]*SPP[6] - P[12][20]*SPP[7] - (P[10][20]*q0)/2;
     nextP[1][21] = P[1][21] + P[0][21]*SF[6] + P[2][21]*SF[5] + P[3][21]*SF[9] + P[11][21]*SPP[6] - P[12][21]*SPP[7] - (P[10][21]*q0)/2;
-    nextP[1][22] = P[1][22] + P[0][22]*SF[6] + P[2][22]*SF[5] + P[3][22]*SF[9] + P[11][22]*SPP[6] - P[12][22]*SPP[7] - (P[10][22]*q0)/2;
     nextP[2][0] = P[2][0] + SQ[7] + P[0][0]*SF[4] + P[1][0]*SF[8] + P[3][0]*SF[6] + P[12][0]*SF[11] - P[10][0]*SPP[6] - (P[11][0]*q0)/2 + SF[7]*(P[2][1] + P[0][1]*SF[4] + P[1][1]*SF[8] + P[3][1]*SF[6] + P[12][1]*SF[11] - P[10][1]*SPP[6] - (P[11][1]*q0)/2) + SF[9]*(P[2][2] + P[0][2]*SF[4] + P[1][2]*SF[8] + P[3][2]*SF[6] + P[12][2]*SF[11] - P[10][2]*SPP[6] - (P[11][2]*q0)/2) + SF[8]*(P[2][3] + P[0][3]*SF[4] + P[1][3]*SF[8] + P[3][3]*SF[6] + P[12][3]*SF[11] - P[10][3]*SPP[6] - (P[11][3]*q0)/2) + SF[11]*(P[2][10] + P[0][10]*SF[4] + P[1][10]*SF[8] + P[3][10]*SF[6] + P[12][10]*SF[11] - P[10][10]*SPP[6] - (P[11][10]*q0)/2) + SPP[7]*(P[2][11] + P[0][11]*SF[4] + P[1][11]*SF[8] + P[3][11]*SF[6] + P[12][11]*SF[11] - P[10][11]*SPP[6] - (P[11][11]*q0)/2) + SPP[6]*(P[2][12] + P[0][12]*SF[4] + P[1][12]*SF[8] + P[3][12]*SF[6] + P[12][12]*SF[11] - P[10][12]*SPP[6] - (P[11][12]*q0)/2);
     nextP[2][1] = P[2][1] + SQ[5] + P[0][1]*SF[4] + P[1][1]*SF[8] + P[3][1]*SF[6] + P[12][1]*SF[11] - P[10][1]*SPP[6] - (P[11][1]*q0)/2 + SF[6]*(P[2][0] + P[0][0]*SF[4] + P[1][0]*SF[8] + P[3][0]*SF[6] + P[12][0]*SF[11] - P[10][0]*SPP[6] - (P[11][0]*q0)/2) + SF[5]*(P[2][2] + P[0][2]*SF[4] + P[1][2]*SF[8] + P[3][2]*SF[6] + P[12][2]*SF[11] - P[10][2]*SPP[6] - (P[11][2]*q0)/2) + SF[9]*(P[2][3] + P[0][3]*SF[4] + P[1][3]*SF[8] + P[3][3]*SF[6] + P[12][3]*SF[11] - P[10][3]*SPP[6] - (P[11][3]*q0)/2) + SPP[6]*(P[2][11] + P[0][11]*SF[4] + P[1][11]*SF[8] + P[3][11]*SF[6] + P[12][11]*SF[11] - P[10][11]*SPP[6] - (P[11][11]*q0)/2) - SPP[7]*(P[2][12] + P[0][12]*SF[4] + P[1][12]*SF[8] + P[3][12]*SF[6] + P[12][12]*SF[11] - P[10][12]*SPP[6] - (P[11][12]*q0)/2) - (q0*(P[2][10] + P[0][10]*SF[4] + P[1][10]*SF[8] + P[3][10]*SF[6] + P[12][10]*SF[11] - P[10][10]*SPP[6] - (P[11][10]*q0)/2))/2;
     nextP[2][2] = P[2][2] + P[0][2]*SF[4] + P[1][2]*SF[8] + P[3][2]*SF[6] + P[12][2]*SF[11] - P[10][2]*SPP[6] + dayCov*SQ[9] + (dazCov*SQ[10])/4 - (P[11][2]*q0)/2 + SF[4]*(P[2][0] + P[0][0]*SF[4] + P[1][0]*SF[8] + P[3][0]*SF[6] + P[12][0]*SF[11] - P[10][0]*SPP[6] - (P[11][0]*q0)/2) + SF[8]*(P[2][1] + P[0][1]*SF[4] + P[1][1]*SF[8] + P[3][1]*SF[6] + P[12][1]*SF[11] - P[10][1]*SPP[6] - (P[11][1]*q0)/2) + SF[6]*(P[2][3] + P[0][3]*SF[4] + P[1][3]*SF[8] + P[3][3]*SF[6] + P[12][3]*SF[11] - P[10][3]*SPP[6] - (P[11][3]*q0)/2) + SF[11]*(P[2][12] + P[0][12]*SF[4] + P[1][12]*SF[8] + P[3][12]*SF[6] + P[12][12]*SF[11] - P[10][12]*SPP[6] - (P[11][12]*q0)/2) - SPP[6]*(P[2][10] + P[0][10]*SF[4] + P[1][10]*SF[8] + P[3][10]*SF[6] + P[12][10]*SF[11] - P[10][10]*SPP[6] - (P[11][10]*q0)/2) + (daxCov*sq(q3))/4 - (q0*(P[2][11] + P[0][11]*SF[4] + P[1][11]*SF[8] + P[3][11]*SF[6] + P[12][11]*SF[11] - P[10][11]*SPP[6] - (P[11][11]*q0)/2))/2;
@@ -497,7 +489,6 @@ void AttPosEKF::CovariancePrediction(float dt)
     nextP[2][19] = P[2][19] + P[0][19]*SF[4] + P[1][19]*SF[8] + P[3][19]*SF[6] + P[12][19]*SF[11] - P[10][19]*SPP[6] - (P[11][19]*q0)/2;
     nextP[2][20] = P[2][20] + P[0][20]*SF[4] + P[1][20]*SF[8] + P[3][20]*SF[6] + P[12][20]*SF[11] - P[10][20]*SPP[6] - (P[11][20]*q0)/2;
     nextP[2][21] = P[2][21] + P[0][21]*SF[4] + P[1][21]*SF[8] + P[3][21]*SF[6] + P[12][21]*SF[11] - P[10][21]*SPP[6] - (P[11][21]*q0)/2;
-    nextP[2][22] = P[2][22] + P[0][22]*SF[4] + P[1][22]*SF[8] + P[3][22]*SF[6] + P[12][22]*SF[11] - P[10][22]*SPP[6] - (P[11][22]*q0)/2;
     nextP[3][0] = P[3][0] + SQ[6] + P[0][0]*SF[5] + P[1][0]*SF[4] + P[2][0]*SF[7] - P[11][0]*SF[11] + P[10][0]*SPP[7] - (P[12][0]*q0)/2 + SF[7]*(P[3][1] + P[0][1]*SF[5] + P[1][1]*SF[4] + P[2][1]*SF[7] - P[11][1]*SF[11] + P[10][1]*SPP[7] - (P[12][1]*q0)/2) + SF[9]*(P[3][2] + P[0][2]*SF[5] + P[1][2]*SF[4] + P[2][2]*SF[7] - P[11][2]*SF[11] + P[10][2]*SPP[7] - (P[12][2]*q0)/2) + SF[8]*(P[3][3] + P[0][3]*SF[5] + P[1][3]*SF[4] + P[2][3]*SF[7] - P[11][3]*SF[11] + P[10][3]*SPP[7] - (P[12][3]*q0)/2) + SF[11]*(P[3][10] + P[0][10]*SF[5] + P[1][10]*SF[4] + P[2][10]*SF[7] - P[11][10]*SF[11] + P[10][10]*SPP[7] - (P[12][10]*q0)/2) + SPP[7]*(P[3][11] + P[0][11]*SF[5] + P[1][11]*SF[4] + P[2][11]*SF[7] - P[11][11]*SF[11] + P[10][11]*SPP[7] - (P[12][11]*q0)/2) + SPP[6]*(P[3][12] + P[0][12]*SF[5] + P[1][12]*SF[4] + P[2][12]*SF[7] - P[11][12]*SF[11] + P[10][12]*SPP[7] - (P[12][12]*q0)/2);
     nextP[3][1] = P[3][1] + SQ[4] + P[0][1]*SF[5] + P[1][1]*SF[4] + P[2][1]*SF[7] - P[11][1]*SF[11] + P[10][1]*SPP[7] - (P[12][1]*q0)/2 + SF[6]*(P[3][0] + P[0][0]*SF[5] + P[1][0]*SF[4] + P[2][0]*SF[7] - P[11][0]*SF[11] + P[10][0]*SPP[7] - (P[12][0]*q0)/2) + SF[5]*(P[3][2] + P[0][2]*SF[5] + P[1][2]*SF[4] + P[2][2]*SF[7] - P[11][2]*SF[11] + P[10][2]*SPP[7] - (P[12][2]*q0)/2) + SF[9]*(P[3][3] + P[0][3]*SF[5] + P[1][3]*SF[4] + P[2][3]*SF[7] - P[11][3]*SF[11] + P[10][3]*SPP[7] - (P[12][3]*q0)/2) + SPP[6]*(P[3][11] + P[0][11]*SF[5] + P[1][11]*SF[4] + P[2][11]*SF[7] - P[11][11]*SF[11] + P[10][11]*SPP[7] - (P[12][11]*q0)/2) - SPP[7]*(P[3][12] + P[0][12]*SF[5] + P[1][12]*SF[4] + P[2][12]*SF[7] - P[11][12]*SF[11] + P[10][12]*SPP[7] - (P[12][12]*q0)/2) - (q0*(P[3][10] + P[0][10]*SF[5] + P[1][10]*SF[4] + P[2][10]*SF[7] - P[11][10]*SF[11] + P[10][10]*SPP[7] - (P[12][10]*q0)/2))/2;
     nextP[3][2] = P[3][2] + SQ[3] + P[0][2]*SF[5] + P[1][2]*SF[4] + P[2][2]*SF[7] - P[11][2]*SF[11] + P[10][2]*SPP[7] - (P[12][2]*q0)/2 + SF[4]*(P[3][0] + P[0][0]*SF[5] + P[1][0]*SF[4] + P[2][0]*SF[7] - P[11][0]*SF[11] + P[10][0]*SPP[7] - (P[12][0]*q0)/2) + SF[8]*(P[3][1] + P[0][1]*SF[5] + P[1][1]*SF[4] + P[2][1]*SF[7] - P[11][1]*SF[11] + P[10][1]*SPP[7] - (P[12][1]*q0)/2) + SF[6]*(P[3][3] + P[0][3]*SF[5] + P[1][3]*SF[4] + P[2][3]*SF[7] - P[11][3]*SF[11] + P[10][3]*SPP[7] - (P[12][3]*q0)/2) + SF[11]*(P[3][12] + P[0][12]*SF[5] + P[1][12]*SF[4] + P[2][12]*SF[7] - P[11][12]*SF[11] + P[10][12]*SPP[7] - (P[12][12]*q0)/2) - SPP[6]*(P[3][10] + P[0][10]*SF[5] + P[1][10]*SF[4] + P[2][10]*SF[7] - P[11][10]*SF[11] + P[10][10]*SPP[7] - (P[12][10]*q0)/2) - (q0*(P[3][11] + P[0][11]*SF[5] + P[1][11]*SF[4] + P[2][11]*SF[7] - P[11][11]*SF[11] + P[10][11]*SPP[7] - (P[12][11]*q0)/2))/2;
@@ -520,7 +511,6 @@ void AttPosEKF::CovariancePrediction(float dt)
     nextP[3][19] = P[3][19] + P[0][19]*SF[5] + P[1][19]*SF[4] + P[2][19]*SF[7] - P[11][19]*SF[11] + P[10][19]*SPP[7] - (P[12][19]*q0)/2;
     nextP[3][20] = P[3][20] + P[0][20]*SF[5] + P[1][20]*SF[4] + P[2][20]*SF[7] - P[11][20]*SF[11] + P[10][20]*SPP[7] - (P[12][20]*q0)/2;
     nextP[3][21] = P[3][21] + P[0][21]*SF[5] + P[1][21]*SF[4] + P[2][21]*SF[7] - P[11][21]*SF[11] + P[10][21]*SPP[7] - (P[12][21]*q0)/2;
-    nextP[3][22] = P[3][22] + P[0][22]*SF[5] + P[1][22]*SF[4] + P[2][22]*SF[7] - P[11][22]*SF[11] + P[10][22]*SPP[7] - (P[12][22]*q0)/2;
     nextP[4][0] = P[4][0] + P[0][0]*SF[3] + P[1][0]*SF[1] + P[2][0]*SPP[0] - P[3][0]*SPP[2] - P[13][0]*SPP[4] + SF[7]*(P[4][1] + P[0][1]*SF[3] + P[1][1]*SF[1] + P[2][1]*SPP[0] - P[3][1]*SPP[2] - P[13][1]*SPP[4]) + SF[9]*(P[4][2] + P[0][2]*SF[3] + P[1][2]*SF[1] + P[2][2]*SPP[0] - P[3][2]*SPP[2] - P[13][2]*SPP[4]) + SF[8]*(P[4][3] + P[0][3]*SF[3] + P[1][3]*SF[1] + P[2][3]*SPP[0] - P[3][3]*SPP[2] - P[13][3]*SPP[4]) + SF[11]*(P[4][10] + P[0][10]*SF[3] + P[1][10]*SF[1] + P[2][10]*SPP[0] - P[3][10]*SPP[2] - P[13][10]*SPP[4]) + SPP[7]*(P[4][11] + P[0][11]*SF[3] + P[1][11]*SF[1] + P[2][11]*SPP[0] - P[3][11]*SPP[2] - P[13][11]*SPP[4]) + SPP[6]*(P[4][12] + P[0][12]*SF[3] + P[1][12]*SF[1] + P[2][12]*SPP[0] - P[3][12]*SPP[2] - P[13][12]*SPP[4]);
     nextP[4][1] = P[4][1] + P[0][1]*SF[3] + P[1][1]*SF[1] + P[2][1]*SPP[0] - P[3][1]*SPP[2] - P[13][1]*SPP[4] + SF[6]*(P[4][0] + P[0][0]*SF[3] + P[1][0]*SF[1] + P[2][0]*SPP[0] - P[3][0]*SPP[2] - P[13][0]*SPP[4]) + SF[5]*(P[4][2] + P[0][2]*SF[3] + P[1][2]*SF[1] + P[2][2]*SPP[0] - P[3][2]*SPP[2] - P[13][2]*SPP[4]) + SF[9]*(P[4][3] + P[0][3]*SF[3] + P[1][3]*SF[1] + P[2][3]*SPP[0] - P[3][3]*SPP[2] - P[13][3]*SPP[4]) + SPP[6]*(P[4][11] + P[0][11]*SF[3] + P[1][11]*SF[1] + P[2][11]*SPP[0] - P[3][11]*SPP[2] - P[13][11]*SPP[4]) - SPP[7]*(P[4][12] + P[0][12]*SF[3] + P[1][12]*SF[1] + P[2][12]*SPP[0] - P[3][12]*SPP[2] - P[13][12]*SPP[4]) - (q0*(P[4][10] + P[0][10]*SF[3] + P[1][10]*SF[1] + P[2][10]*SPP[0] - P[3][10]*SPP[2] - P[13][10]*SPP[4]))/2;
     nextP[4][2] = P[4][2] + P[0][2]*SF[3] + P[1][2]*SF[1] + P[2][2]*SPP[0] - P[3][2]*SPP[2] - P[13][2]*SPP[4] + SF[4]*(P[4][0] + P[0][0]*SF[3] + P[1][0]*SF[1] + P[2][0]*SPP[0] - P[3][0]*SPP[2] - P[13][0]*SPP[4]) + SF[8]*(P[4][1] + P[0][1]*SF[3] + P[1][1]*SF[1] + P[2][1]*SPP[0] - P[3][1]*SPP[2] - P[13][1]*SPP[4]) + SF[6]*(P[4][3] + P[0][3]*SF[3] + P[1][3]*SF[1] + P[2][3]*SPP[0] - P[3][3]*SPP[2] - P[13][3]*SPP[4]) + SF[11]*(P[4][12] + P[0][12]*SF[3] + P[1][12]*SF[1] + P[2][12]*SPP[0] - P[3][12]*SPP[2] - P[13][12]*SPP[4]) - SPP[6]*(P[4][10] + P[0][10]*SF[3] + P[1][10]*SF[1] + P[2][10]*SPP[0] - P[3][10]*SPP[2] - P[13][10]*SPP[4]) - (q0*(P[4][11] + P[0][11]*SF[3] + P[1][11]*SF[1] + P[2][11]*SPP[0] - P[3][11]*SPP[2] - P[13][11]*SPP[4]))/2;
@@ -543,7 +533,6 @@ void AttPosEKF::CovariancePrediction(float dt)
     nextP[4][19] = P[4][19] + P[0][19]*SF[3] + P[1][19]*SF[1] + P[2][19]*SPP[0] - P[3][19]*SPP[2] - P[13][19]*SPP[4];
     nextP[4][20] = P[4][20] + P[0][20]*SF[3] + P[1][20]*SF[1] + P[2][20]*SPP[0] - P[3][20]*SPP[2] - P[13][20]*SPP[4];
     nextP[4][21] = P[4][21] + P[0][21]*SF[3] + P[1][21]*SF[1] + P[2][21]*SPP[0] - P[3][21]*SPP[2] - P[13][21]*SPP[4];
-    nextP[4][22] = P[4][22] + P[0][22]*SF[3] + P[1][22]*SF[1] + P[2][22]*SPP[0] - P[3][22]*SPP[2] - P[13][22]*SPP[4];
     nextP[5][0] = P[5][0] + P[0][0]*SF[2] + P[2][0]*SF[1] + P[3][0]*SF[3] - P[1][0]*SPP[0] + P[13][0]*SPP[3] + SF[7]*(P[5][1] + P[0][1]*SF[2] + P[2][1]*SF[1] + P[3][1]*SF[3] - P[1][1]*SPP[0] + P[13][1]*SPP[3]) + SF[9]*(P[5][2] + P[0][2]*SF[2] + P[2][2]*SF[1] + P[3][2]*SF[3] - P[1][2]*SPP[0] + P[13][2]*SPP[3]) + SF[8]*(P[5][3] + P[0][3]*SF[2] + P[2][3]*SF[1] + P[3][3]*SF[3] - P[1][3]*SPP[0] + P[13][3]*SPP[3]) + SF[11]*(P[5][10] + P[0][10]*SF[2] + P[2][10]*SF[1] + P[3][10]*SF[3] - P[1][10]*SPP[0] + P[13][10]*SPP[3]) + SPP[7]*(P[5][11] + P[0][11]*SF[2] + P[2][11]*SF[1] + P[3][11]*SF[3] - P[1][11]*SPP[0] + P[13][11]*SPP[3]) + SPP[6]*(P[5][12] + P[0][12]*SF[2] + P[2][12]*SF[1] + P[3][12]*SF[3] - P[1][12]*SPP[0] + P[13][12]*SPP[3]);
     nextP[5][1] = P[5][1] + P[0][1]*SF[2] + P[2][1]*SF[1] + P[3][1]*SF[3] - P[1][1]*SPP[0] + P[13][1]*SPP[3] + SF[6]*(P[5][0] + P[0][0]*SF[2] + P[2][0]*SF[1] + P[3][0]*SF[3] - P[1][0]*SPP[0] + P[13][0]*SPP[3]) + SF[5]*(P[5][2] + P[0][2]*SF[2] + P[2][2]*SF[1] + P[3][2]*SF[3] - P[1][2]*SPP[0] + P[13][2]*SPP[3]) + SF[9]*(P[5][3] + P[0][3]*SF[2] + P[2][3]*SF[1] + P[3][3]*SF[3] - P[1][3]*SPP[0] + P[13][3]*SPP[3]) + SPP[6]*(P[5][11] + P[0][11]*SF[2] + P[2][11]*SF[1] + P[3][11]*SF[3] - P[1][11]*SPP[0] + P[13][11]*SPP[3]) - SPP[7]*(P[5][12] + P[0][12]*SF[2] + P[2][12]*SF[1] + P[3][12]*SF[3] - P[1][12]*SPP[0] + P[13][12]*SPP[3]) - (q0*(P[5][10] + P[0][10]*SF[2] + P[2][10]*SF[1] + P[3][10]*SF[3] - P[1][10]*SPP[0] + P[13][10]*SPP[3]))/2;
     nextP[5][2] = P[5][2] + P[0][2]*SF[2] + P[2][2]*SF[1] + P[3][2]*SF[3] - P[1][2]*SPP[0] + P[13][2]*SPP[3] + SF[4]*(P[5][0] + P[0][0]*SF[2] + P[2][0]*SF[1] + P[3][0]*SF[3] - P[1][0]*SPP[0] + P[13][0]*SPP[3]) + SF[8]*(P[5][1] + P[0][1]*SF[2] + P[2][1]*SF[1] + P[3][1]*SF[3] - P[1][1]*SPP[0] + P[13][1]*SPP[3]) + SF[6]*(P[5][3] + P[0][3]*SF[2] + P[2][3]*SF[1] + P[3][3]*SF[3] - P[1][3]*SPP[0] + P[13][3]*SPP[3]) + SF[11]*(P[5][12] + P[0][12]*SF[2] + P[2][12]*SF[1] + P[3][12]*SF[3] - P[1][12]*SPP[0] + P[13][12]*SPP[3]) - SPP[6]*(P[5][10] + P[0][10]*SF[2] + P[2][10]*SF[1] + P[3][10]*SF[3] - P[1][10]*SPP[0] + P[13][10]*SPP[3]) - (q0*(P[5][11] + P[0][11]*SF[2] + P[2][11]*SF[1] + P[3][11]*SF[3] - P[1][11]*SPP[0] + P[13][11]*SPP[3]))/2;
@@ -566,7 +555,6 @@ void AttPosEKF::CovariancePrediction(float dt)
     nextP[5][19] = P[5][19] + P[0][19]*SF[2] + P[2][19]*SF[1] + P[3][19]*SF[3] - P[1][19]*SPP[0] + P[13][19]*SPP[3];
     nextP[5][20] = P[5][20] + P[0][20]*SF[2] + P[2][20]*SF[1] + P[3][20]*SF[3] - P[1][20]*SPP[0] + P[13][20]*SPP[3];
     nextP[5][21] = P[5][21] + P[0][21]*SF[2] + P[2][21]*SF[1] + P[3][21]*SF[3] - P[1][21]*SPP[0] + P[13][21]*SPP[3];
-    nextP[5][22] = P[5][22] + P[0][22]*SF[2] + P[2][22]*SF[1] + P[3][22]*SF[3] - P[1][22]*SPP[0] + P[13][22]*SPP[3];
     nextP[6][0] = P[6][0] + P[1][0]*SF[2] + P[3][0]*SF[1] + P[0][0]*SPP[0] - P[2][0]*SPP[1] - P[13][0]*(sq(q0) - sq(q1) - sq(q2) + sq(q3)) + SF[7]*(P[6][1] + P[1][1]*SF[2] + P[3][1]*SF[1] + P[0][1]*SPP[0] - P[2][1]*SPP[1] - P[13][1]*(sq(q0) - sq(q1) - sq(q2) + sq(q3))) + SF[9]*(P[6][2] + P[1][2]*SF[2] + P[3][2]*SF[1] + P[0][2]*SPP[0] - P[2][2]*SPP[1] - P[13][2]*(sq(q0) - sq(q1) - sq(q2) + sq(q3))) + SF[8]*(P[6][3] + P[1][3]*SF[2] + P[3][3]*SF[1] + P[0][3]*SPP[0] - P[2][3]*SPP[1] - P[13][3]*(sq(q0) - sq(q1) - sq(q2) + sq(q3))) + SF[11]*(P[6][10] + P[1][10]*SF[2] + P[3][10]*SF[1] + P[0][10]*SPP[0] - P[2][10]*SPP[1] - P[13][10]*(sq(q0) - sq(q1) - sq(q2) + sq(q3))) + SPP[7]*(P[6][11] + P[1][11]*SF[2] + P[3][11]*SF[1] + P[0][11]*SPP[0] - P[2][11]*SPP[1] - P[13][11]*(sq(q0) - sq(q1) - sq(q2) + sq(q3))) + SPP[6]*(P[6][12] + P[1][12]*SF[2] + P[3][12]*SF[1] + P[0][12]*SPP[0] - P[2][12]*SPP[1] - P[13][12]*(sq(q0) - sq(q1) - sq(q2) + sq(q3)));
     nextP[6][1] = P[6][1] + P[1][1]*SF[2] + P[3][1]*SF[1] + P[0][1]*SPP[0] - P[2][1]*SPP[1] - P[13][1]*(sq(q0) - sq(q1) - sq(q2) + sq(q3)) + SF[6]*(P[6][0] + P[1][0]*SF[2] + P[3][0]*SF[1] + P[0][0]*SPP[0] - P[2][0]*SPP[1] - P[13][0]*(sq(q0) - sq(q1) - sq(q2) + sq(q3))) + SF[5]*(P[6][2] + P[1][2]*SF[2] + P[3][2]*SF[1] + P[0][2]*SPP[0] - P[2][2]*SPP[1] - P[13][2]*(sq(q0) - sq(q1) - sq(q2) + sq(q3))) + SF[9]*(P[6][3] + P[1][3]*SF[2] + P[3][3]*SF[1] + P[0][3]*SPP[0] - P[2][3]*SPP[1] - P[13][3]*(sq(q0) - sq(q1) - sq(q2) + sq(q3))) + SPP[6]*(P[6][11] + P[1][11]*SF[2] + P[3][11]*SF[1] + P[0][11]*SPP[0] - P[2][11]*SPP[1] - P[13][11]*(sq(q0) - sq(q1) - sq(q2) + sq(q3))) - SPP[7]*(P[6][12] + P[1][12]*SF[2] + P[3][12]*SF[1] + P[0][12]*SPP[0] - P[2][12]*SPP[1] - P[13][12]*(sq(q0) - sq(q1) - sq(q2) + sq(q3))) - (q0*(P[6][10] + P[1][10]*SF[2] + P[3][10]*SF[1] + P[0][10]*SPP[0] - P[2][10]*SPP[1] - P[13][10]*(sq(q0) - sq(q1) - sq(q2) + sq(q3))))/2;
     nextP[6][2] = P[6][2] + P[1][2]*SF[2] + P[3][2]*SF[1] + P[0][2]*SPP[0] - P[2][2]*SPP[1] - P[13][2]*(sq(q0) - sq(q1) - sq(q2) + sq(q3)) + SF[4]*(P[6][0] + P[1][0]*SF[2] + P[3][0]*SF[1] + P[0][0]*SPP[0] - P[2][0]*SPP[1] - P[13][0]*(sq(q0) - sq(q1) - sq(q2) + sq(q3))) + SF[8]*(P[6][1] + P[1][1]*SF[2] + P[3][1]*SF[1] + P[0][1]*SPP[0] - P[2][1]*SPP[1] - P[13][1]*(sq(q0) - sq(q1) - sq(q2) + sq(q3))) + SF[6]*(P[6][3] + P[1][3]*SF[2] + P[3][3]*SF[1] + P[0][3]*SPP[0] - P[2][3]*SPP[1] - P[13][3]*(sq(q0) - sq(q1) - sq(q2) + sq(q3))) + SF[11]*(P[6][12] + P[1][12]*SF[2] + P[3][12]*SF[1] + P[0][12]*SPP[0] - P[2][12]*SPP[1] - P[13][12]*(sq(q0) - sq(q1) - sq(q2) + sq(q3))) - SPP[6]*(P[6][10] + P[1][10]*SF[2] + P[3][10]*SF[1] + P[0][10]*SPP[0] - P[2][10]*SPP[1] - P[13][10]*(sq(q0) - sq(q1) - sq(q2) + sq(q3))) - (q0*(P[6][11] + P[1][11]*SF[2] + P[3][11]*SF[1] + P[0][11]*SPP[0] - P[2][11]*SPP[1] - P[13][11]*(sq(q0) - sq(q1) - sq(q2) + sq(q3))))/2;
@@ -589,7 +577,6 @@ void AttPosEKF::CovariancePrediction(float dt)
     nextP[6][19] = P[6][19] + P[1][19]*SF[2] + P[3][19]*SF[1] + P[0][19]*SPP[0] - P[2][19]*SPP[1] - P[13][19]*SPP[5];
     nextP[6][20] = P[6][20] + P[1][20]*SF[2] + P[3][20]*SF[1] + P[0][20]*SPP[0] - P[2][20]*SPP[1] - P[13][20]*SPP[5];
     nextP[6][21] = P[6][21] + P[1][21]*SF[2] + P[3][21]*SF[1] + P[0][21]*SPP[0] - P[2][21]*SPP[1] - P[13][21]*SPP[5];
-    nextP[6][22] = P[6][22] + P[1][22]*SF[2] + P[3][22]*SF[1] + P[0][22]*SPP[0] - P[2][22]*SPP[1] - P[13][22]*SPP[5];
     nextP[7][0] = P[7][0] + P[4][0]*dt + SF[7]*(P[7][1] + P[4][1]*dt) + SF[9]*(P[7][2] + P[4][2]*dt) + SF[8]*(P[7][3] + P[4][3]*dt) + SF[11]*(P[7][10] + P[4][10]*dt) + SPP[7]*(P[7][11] + P[4][11]*dt) + SPP[6]*(P[7][12] + P[4][12]*dt);
     nextP[7][1] = P[7][1] + P[4][1]*dt + SF[6]*(P[7][0] + P[4][0]*dt) + SF[5]*(P[7][2] + P[4][2]*dt) + SF[9]*(P[7][3] + P[4][3]*dt) + SPP[6]*(P[7][11] + P[4][11]*dt) - SPP[7]*(P[7][12] + P[4][12]*dt) - (q0*(P[7][10] + P[4][10]*dt))/2;
     nextP[7][2] = P[7][2] + P[4][2]*dt + SF[4]*(P[7][0] + P[4][0]*dt) + SF[8]*(P[7][1] + P[4][1]*dt) + SF[6]*(P[7][3] + P[4][3]*dt) + SF[11]*(P[7][12] + P[4][12]*dt) - SPP[6]*(P[7][10] + P[4][10]*dt) - (q0*(P[7][11] + P[4][11]*dt))/2;
@@ -612,7 +599,6 @@ void AttPosEKF::CovariancePrediction(float dt)
     nextP[7][19] = P[7][19] + P[4][19]*dt;
     nextP[7][20] = P[7][20] + P[4][20]*dt;
     nextP[7][21] = P[7][21] + P[4][21]*dt;
-    nextP[7][22] = P[7][22] + P[4][22]*dt;
     nextP[8][0] = P[8][0] + P[5][0]*dt + SF[7]*(P[8][1] + P[5][1]*dt) + SF[9]*(P[8][2] + P[5][2]*dt) + SF[8]*(P[8][3] + P[5][3]*dt) + SF[11]*(P[8][10] + P[5][10]*dt) + SPP[7]*(P[8][11] + P[5][11]*dt) + SPP[6]*(P[8][12] + P[5][12]*dt);
     nextP[8][1] = P[8][1] + P[5][1]*dt + SF[6]*(P[8][0] + P[5][0]*dt) + SF[5]*(P[8][2] + P[5][2]*dt) + SF[9]*(P[8][3] + P[5][3]*dt) + SPP[6]*(P[8][11] + P[5][11]*dt) - SPP[7]*(P[8][12] + P[5][12]*dt) - (q0*(P[8][10] + P[5][10]*dt))/2;
     nextP[8][2] = P[8][2] + P[5][2]*dt + SF[4]*(P[8][0] + P[5][0]*dt) + SF[8]*(P[8][1] + P[5][1]*dt) + SF[6]*(P[8][3] + P[5][3]*dt) + SF[11]*(P[8][12] + P[5][12]*dt) - SPP[6]*(P[8][10] + P[5][10]*dt) - (q0*(P[8][11] + P[5][11]*dt))/2;
@@ -635,7 +621,6 @@ void AttPosEKF::CovariancePrediction(float dt)
     nextP[8][19] = P[8][19] + P[5][19]*dt;
     nextP[8][20] = P[8][20] + P[5][20]*dt;
     nextP[8][21] = P[8][21] + P[5][21]*dt;
-    nextP[8][22] = P[8][22] + P[5][22]*dt;
     nextP[9][0] = P[9][0] + P[6][0]*dt + SF[7]*(P[9][1] + P[6][1]*dt) + SF[9]*(P[9][2] + P[6][2]*dt) + SF[8]*(P[9][3] + P[6][3]*dt) + SF[11]*(P[9][10] + P[6][10]*dt) + SPP[7]*(P[9][11] + P[6][11]*dt) + SPP[6]*(P[9][12] + P[6][12]*dt);
     nextP[9][1] = P[9][1] + P[6][1]*dt + SF[6]*(P[9][0] + P[6][0]*dt) + SF[5]*(P[9][2] + P[6][2]*dt) + SF[9]*(P[9][3] + P[6][3]*dt) + SPP[6]*(P[9][11] + P[6][11]*dt) - SPP[7]*(P[9][12] + P[6][12]*dt) - (q0*(P[9][10] + P[6][10]*dt))/2;
     nextP[9][2] = P[9][2] + P[6][2]*dt + SF[4]*(P[9][0] + P[6][0]*dt) + SF[8]*(P[9][1] + P[6][1]*dt) + SF[6]*(P[9][3] + P[6][3]*dt) + SF[11]*(P[9][12] + P[6][12]*dt) - SPP[6]*(P[9][10] + P[6][10]*dt) - (q0*(P[9][11] + P[6][11]*dt))/2;
@@ -658,7 +643,6 @@ void AttPosEKF::CovariancePrediction(float dt)
     nextP[9][19] = P[9][19] + P[6][19]*dt;
     nextP[9][20] = P[9][20] + P[6][20]*dt;
     nextP[9][21] = P[9][21] + P[6][21]*dt;
-    nextP[9][22] = P[9][22] + P[6][22]*dt;
     nextP[10][0] = P[10][0] + P[10][1]*SF[7] + P[10][2]*SF[9] + P[10][3]*SF[8] + P[10][10]*SF[11] + P[10][11]*SPP[7] + P[10][12]*SPP[6];
     nextP[10][1] = P[10][1] + P[10][0]*SF[6] + P[10][2]*SF[5] + P[10][3]*SF[9] + P[10][11]*SPP[6] - P[10][12]*SPP[7] - (P[10][10]*q0)/2;
     nextP[10][2] = P[10][2] + P[10][0]*SF[4] + P[10][1]*SF[8] + P[10][3]*SF[6] + P[10][12]*SF[11] - P[10][10]*SPP[6] - (P[10][11]*q0)/2;
@@ -681,7 +665,6 @@ void AttPosEKF::CovariancePrediction(float dt)
     nextP[10][19] = P[10][19];
     nextP[10][20] = P[10][20];
     nextP[10][21] = P[10][21];
-    nextP[10][22] = P[10][22];
     nextP[11][0] = P[11][0] + P[11][1]*SF[7] + P[11][2]*SF[9] + P[11][3]*SF[8] + P[11][10]*SF[11] + P[11][11]*SPP[7] + P[11][12]*SPP[6];
     nextP[11][1] = P[11][1] + P[11][0]*SF[6] + P[11][2]*SF[5] + P[11][3]*SF[9] + P[11][11]*SPP[6] - P[11][12]*SPP[7] - (P[11][10]*q0)/2;
     nextP[11][2] = P[11][2] + P[11][0]*SF[4] + P[11][1]*SF[8] + P[11][3]*SF[6] + P[11][12]*SF[11] - P[11][10]*SPP[6] - (P[11][11]*q0)/2;
@@ -704,7 +687,6 @@ void AttPosEKF::CovariancePrediction(float dt)
     nextP[11][19] = P[11][19];
     nextP[11][20] = P[11][20];
     nextP[11][21] = P[11][21];
-    nextP[11][22] = P[11][22];
     nextP[12][0] = P[12][0] + P[12][1]*SF[7] + P[12][2]*SF[9] + P[12][3]*SF[8] + P[12][10]*SF[11] + P[12][11]*SPP[7] + P[12][12]*SPP[6];
     nextP[12][1] = P[12][1] + P[12][0]*SF[6] + P[12][2]*SF[5] + P[12][3]*SF[9] + P[12][11]*SPP[6] - P[12][12]*SPP[7] - (P[12][10]*q0)/2;
     nextP[12][2] = P[12][2] + P[12][0]*SF[4] + P[12][1]*SF[8] + P[12][3]*SF[6] + P[12][12]*SF[11] - P[12][10]*SPP[6] - (P[12][11]*q0)/2;
@@ -727,7 +709,6 @@ void AttPosEKF::CovariancePrediction(float dt)
     nextP[12][19] = P[12][19];
     nextP[12][20] = P[12][20];
     nextP[12][21] = P[12][21];
-    nextP[12][22] = P[12][22];
     nextP[13][0] = P[13][0] + P[13][1]*SF[7] + P[13][2]*SF[9] + P[13][3]*SF[8] + P[13][10]*SF[11] + P[13][11]*SPP[7] + P[13][12]*SPP[6];
     nextP[13][1] = P[13][1] + P[13][0]*SF[6] + P[13][2]*SF[5] + P[13][3]*SF[9] + P[13][11]*SPP[6] - P[13][12]*SPP[7] - (P[13][10]*q0)/2;
     nextP[13][2] = P[13][2] + P[13][0]*SF[4] + P[13][1]*SF[8] + P[13][3]*SF[6] + P[13][12]*SF[11] - P[13][10]*SPP[6] - (P[13][11]*q0)/2;
@@ -750,7 +731,6 @@ void AttPosEKF::CovariancePrediction(float dt)
     nextP[13][19] = P[13][19];
     nextP[13][20] = P[13][20];
     nextP[13][21] = P[13][21];
-    nextP[13][22] = P[13][22];
     nextP[14][0] = P[14][0] + P[14][1]*SF[7] + P[14][2]*SF[9] + P[14][3]*SF[8] + P[14][10]*SF[11] + P[14][11]*SPP[7] + P[14][12]*SPP[6];
     nextP[14][1] = P[14][1] + P[14][0]*SF[6] + P[14][2]*SF[5] + P[14][3]*SF[9] + P[14][11]*SPP[6] - P[14][12]*SPP[7] - (P[14][10]*q0)/2;
     nextP[14][2] = P[14][2] + P[14][0]*SF[4] + P[14][1]*SF[8] + P[14][3]*SF[6] + P[14][12]*SF[11] - P[14][10]*SPP[6] - (P[14][11]*q0)/2;
@@ -773,7 +753,6 @@ void AttPosEKF::CovariancePrediction(float dt)
     nextP[14][19] = P[14][19];
     nextP[14][20] = P[14][20];
     nextP[14][21] = P[14][21];
-    nextP[14][22] = P[14][22];
     nextP[15][0] = P[15][0] + P[15][1]*SF[7] + P[15][2]*SF[9] + P[15][3]*SF[8] + P[15][10]*SF[11] + P[15][11]*SPP[7] + P[15][12]*SPP[6];
     nextP[15][1] = P[15][1] + P[15][0]*SF[6] + P[15][2]*SF[5] + P[15][3]*SF[9] + P[15][11]*SPP[6] - P[15][12]*SPP[7] - (P[15][10]*q0)/2;
     nextP[15][2] = P[15][2] + P[15][0]*SF[4] + P[15][1]*SF[8] + P[15][3]*SF[6] + P[15][12]*SF[11] - P[15][10]*SPP[6] - (P[15][11]*q0)/2;
@@ -796,7 +775,6 @@ void AttPosEKF::CovariancePrediction(float dt)
     nextP[15][19] = P[15][19];
     nextP[15][20] = P[15][20];
     nextP[15][21] = P[15][21];
-    nextP[15][22] = P[15][22];
     nextP[16][0] = P[16][0] + P[16][1]*SF[7] + P[16][2]*SF[9] + P[16][3]*SF[8] + P[16][10]*SF[11] + P[16][11]*SPP[7] + P[16][12]*SPP[6];
     nextP[16][1] = P[16][1] + P[16][0]*SF[6] + P[16][2]*SF[5] + P[16][3]*SF[9] + P[16][11]*SPP[6] - P[16][12]*SPP[7] - (P[16][10]*q0)/2;
     nextP[16][2] = P[16][2] + P[16][0]*SF[4] + P[16][1]*SF[8] + P[16][3]*SF[6] + P[16][12]*SF[11] - P[16][10]*SPP[6] - (P[16][11]*q0)/2;
@@ -819,7 +797,6 @@ void AttPosEKF::CovariancePrediction(float dt)
     nextP[16][19] = P[16][19];
     nextP[16][20] = P[16][20];
     nextP[16][21] = P[16][21];
-    nextP[16][22] = P[16][22];
     nextP[17][0] = P[17][0] + P[17][1]*SF[7] + P[17][2]*SF[9] + P[17][3]*SF[8] + P[17][10]*SF[11] + P[17][11]*SPP[7] + P[17][12]*SPP[6];
     nextP[17][1] = P[17][1] + P[17][0]*SF[6] + P[17][2]*SF[5] + P[17][3]*SF[9] + P[17][11]*SPP[6] - P[17][12]*SPP[7] - (P[17][10]*q0)/2;
     nextP[17][2] = P[17][2] + P[17][0]*SF[4] + P[17][1]*SF[8] + P[17][3]*SF[6] + P[17][12]*SF[11] - P[17][10]*SPP[6] - (P[17][11]*q0)/2;
@@ -842,7 +819,6 @@ void AttPosEKF::CovariancePrediction(float dt)
     nextP[17][19] = P[17][19];
     nextP[17][20] = P[17][20];
     nextP[17][21] = P[17][21];
-    nextP[17][22] = P[17][22];
     nextP[18][0] = P[18][0] + P[18][1]*SF[7] + P[18][2]*SF[9] + P[18][3]*SF[8] + P[18][10]*SF[11] + P[18][11]*SPP[7] + P[18][12]*SPP[6];
     nextP[18][1] = P[18][1] + P[18][0]*SF[6] + P[18][2]*SF[5] + P[18][3]*SF[9] + P[18][11]*SPP[6] - P[18][12]*SPP[7] - (P[18][10]*q0)/2;
     nextP[18][2] = P[18][2] + P[18][0]*SF[4] + P[18][1]*SF[8] + P[18][3]*SF[6] + P[18][12]*SF[11] - P[18][10]*SPP[6] - (P[18][11]*q0)/2;
@@ -865,7 +841,6 @@ void AttPosEKF::CovariancePrediction(float dt)
     nextP[18][19] = P[18][19];
     nextP[18][20] = P[18][20];
     nextP[18][21] = P[18][21];
-    nextP[18][22] = P[18][22];
     nextP[19][0] = P[19][0] + P[19][1]*SF[7] + P[19][2]*SF[9] + P[19][3]*SF[8] + P[19][10]*SF[11] + P[19][11]*SPP[7] + P[19][12]*SPP[6];
     nextP[19][1] = P[19][1] + P[19][0]*SF[6] + P[19][2]*SF[5] + P[19][3]*SF[9] + P[19][11]*SPP[6] - P[19][12]*SPP[7] - (P[19][10]*q0)/2;
     nextP[19][2] = P[19][2] + P[19][0]*SF[4] + P[19][1]*SF[8] + P[19][3]*SF[6] + P[19][12]*SF[11] - P[19][10]*SPP[6] - (P[19][11]*q0)/2;
@@ -888,7 +863,6 @@ void AttPosEKF::CovariancePrediction(float dt)
     nextP[19][19] = P[19][19];
     nextP[19][20] = P[19][20];
     nextP[19][21] = P[19][21];
-    nextP[19][22] = P[19][22];
     nextP[20][0] = P[20][0] + P[20][1]*SF[7] + P[20][2]*SF[9] + P[20][3]*SF[8] + P[20][10]*SF[11] + P[20][11]*SPP[7] + P[20][12]*SPP[6];
     nextP[20][1] = P[20][1] + P[20][0]*SF[6] + P[20][2]*SF[5] + P[20][3]*SF[9] + P[20][11]*SPP[6] - P[20][12]*SPP[7] - (P[20][10]*q0)/2;
     nextP[20][2] = P[20][2] + P[20][0]*SF[4] + P[20][1]*SF[8] + P[20][3]*SF[6] + P[20][12]*SF[11] - P[20][10]*SPP[6] - (P[20][11]*q0)/2;
@@ -911,7 +885,6 @@ void AttPosEKF::CovariancePrediction(float dt)
     nextP[20][19] = P[20][19];
     nextP[20][20] = P[20][20];
     nextP[20][21] = P[20][21];
-    nextP[20][22] = P[20][22];
     nextP[21][0] = P[21][0] + P[21][1]*SF[7] + P[21][2]*SF[9] + P[21][3]*SF[8] + P[21][10]*SF[11] + P[21][11]*SPP[7] + P[21][12]*SPP[6];
     nextP[21][1] = P[21][1] + P[21][0]*SF[6] + P[21][2]*SF[5] + P[21][3]*SF[9] + P[21][11]*SPP[6] - P[21][12]*SPP[7] - (P[21][10]*q0)/2;
     nextP[21][2] = P[21][2] + P[21][0]*SF[4] + P[21][1]*SF[8] + P[21][3]*SF[6] + P[21][12]*SF[11] - P[21][10]*SPP[6] - (P[21][11]*q0)/2;
@@ -934,30 +907,6 @@ void AttPosEKF::CovariancePrediction(float dt)
     nextP[21][19] = P[21][19];
     nextP[21][20] = P[21][20];
     nextP[21][21] = P[21][21];
-    nextP[21][22] = P[21][22];
-    nextP[22][0] = P[22][0] + P[22][1]*SF[7] + P[22][2]*SF[9] + P[22][3]*SF[8] + P[22][10]*SF[11] + P[22][11]*SPP[7] + P[22][12]*SPP[6];
-    nextP[22][1] = P[22][1] + P[22][0]*SF[6] + P[22][2]*SF[5] + P[22][3]*SF[9] + P[22][11]*SPP[6] - P[22][12]*SPP[7] - (P[22][10]*q0)/2;
-    nextP[22][2] = P[22][2] + P[22][0]*SF[4] + P[22][1]*SF[8] + P[22][3]*SF[6] + P[22][12]*SF[11] - P[22][10]*SPP[6] - (P[22][11]*q0)/2;
-    nextP[22][3] = P[22][3] + P[22][0]*SF[5] + P[22][1]*SF[4] + P[22][2]*SF[7] - P[22][11]*SF[11] + P[22][10]*SPP[7] - (P[22][12]*q0)/2;
-    nextP[22][4] = P[22][4] + P[22][1]*SF[1] + P[22][0]*SF[3] + P[22][2]*SPP[0] - P[22][3]*SPP[2] - P[22][13]*SPP[4];
-    nextP[22][5] = P[22][5] + P[22][0]*SF[2] + P[22][2]*SF[1] + P[22][3]*SF[3] - P[22][1]*SPP[0] + P[22][13]*SPP[3];
-    nextP[22][6] = P[22][6] + P[22][1]*SF[2] + P[22][3]*SF[1] + P[22][0]*SPP[0] - P[22][2]*SPP[1] - P[22][13]*SPP[5];
-    nextP[22][7] = P[22][7] + P[22][4]*dt;
-    nextP[22][8] = P[22][8] + P[22][5]*dt;
-    nextP[22][9] = P[22][9] + P[22][6]*dt;
-    nextP[22][10] = P[22][10];
-    nextP[22][11] = P[22][11];
-    nextP[22][12] = P[22][12];
-    nextP[22][13] = P[22][13];
-    nextP[22][14] = P[22][14];
-    nextP[22][15] = P[22][15];
-    nextP[22][16] = P[22][16];
-    nextP[22][17] = P[22][17];
-    nextP[22][18] = P[22][18];
-    nextP[22][19] = P[22][19];
-    nextP[22][20] = P[22][20];
-    nextP[22][21] = P[22][21];
-    nextP[22][22] = P[22][22];
 
     for (unsigned i = 0; i < n_states; i++)
     {
@@ -1031,7 +980,7 @@ void AttPosEKF::FuseVelposNED()
     bool fuseData[6] = {false,false,false,false,false,false};
     uint8_t stateIndex;
     uint8_t obsIndex;
-    uint8_t indexLimit = 22;
+    uint8_t indexLimit = 21;
 
 // declare variables used by state and covariance update calculations
     float velErr;
@@ -1230,15 +1179,11 @@ void AttPosEKF::FuseVelposNED()
                 }
                 // Don't update magnetic field states if inhibited
                 if (inhibitMagStates) {
-                    for (uint8_t i = 16; i<=21; i++)
+                    for (uint8_t i = 16; i < n_states; i++)
                     {
                         Kfusion[i] = 0;
                     }
                 }
-                // Don't update terrain state if inhibited
-                if (inhibitGndState) {
-                    Kfusion[22] = 0;
-               }
 
                 // Calculate state corrections and re-normalise the quaternions
                 for (uint8_t i = 0; i<=indexLimit; i++)
@@ -1415,15 +1360,10 @@ void AttPosEKF::FuseMagnetometer()
                 Kfusion[20] = SK_MX[0]*(P[20][19] + P[20][1]*SH_MAG[0] + P[20][3]*SH_MAG[2] + P[20][0]*SK_MX[3] - P[20][2]*SK_MX[2] - P[20][16]*SK_MX[1] + P[20][17]*SK_MX[5] - P[20][18]*SK_MX[4]);
                 Kfusion[21] = SK_MX[0]*(P[21][19] + P[21][1]*SH_MAG[0] + P[21][3]*SH_MAG[2] + P[21][0]*SK_MX[3] - P[21][2]*SK_MX[2] - P[21][16]*SK_MX[1] + P[21][17]*SK_MX[5] - P[21][18]*SK_MX[4]);
             } else {
-                for (uint8_t i=16; i <= 21; i++) {
+                for (uint8_t i=16; i < n_states; i++) {
                     Kfusion[i] = 0;
                 }
             }
-            if (!inhibitGndState) {
-                Kfusion[22] = SK_MX[0]*(P[22][19] + P[22][1]*SH_MAG[0] + P[22][3]*SH_MAG[2] + P[22][0]*SK_MX[3] - P[22][2]*SK_MX[2] - P[22][16]*SK_MX[1] + P[22][17]*SK_MX[5] - P[22][18]*SK_MX[4]);
-            } else {
-                Kfusion[22] = 0;
-            }
             varInnovMag[0] = 1.0f/SK_MX[0];
             innovMag[0] = MagPred[0] - magData.x;
         }
@@ -1598,14 +1538,14 @@ void AttPosEKF::FuseMagnetometer()
                 for (uint8_t j = 4; j <= 15; j++) KH[i][j] = 0.0f;
                 if (!onGround)
                 {
-                    for (uint8_t j = 16; j <= 21; j++)
+                    for (uint8_t j = 16; j < n_states; j++)
                     {
                         KH[i][j] = Kfusion[i] * H_MAG[j];
                     }
                 }
                 else
                 {
-                    for (uint8_t j = 16; j <= 21; j++)
+                    for (uint8_t j = 16; j < n_states; j++)
                     {
                         KH[i][j] = 0.0f;
                     }
@@ -1622,7 +1562,7 @@ void AttPosEKF::FuseMagnetometer()
                     }
                     if (!onGround)
                     {
-                        for (uint8_t k = 16; k<=21; k++)
+                        for (uint8_t k = 16; k < n_states; k++)
                         {
                             KHP[i][j] = KHP[i][j] + KH[i][k] * P[k][j];
                         }
@@ -1669,32 +1609,6 @@ void AttPosEKF::FuseAirspeed()
     // Perform fusion of True Airspeed measurement
     if (useAirspeed && fuseVtasData && (VtasPred > 1.0f) && (VtasMeas > 8.0f))
     {
-
-        float altDiff = fabsf(windSpdFiltAltitude - hgtMea);
-
-        if (isfinite(windSpdFiltClimb)) {
-            windSpdFiltClimb = ((1.0f - 0.0002f) * windSpdFiltClimb) + (0.0002f * states[6]);
-        } else {
-            windSpdFiltClimb = states[6];
-        }
-
-        if (altDiff < 20.0f) {
-            // Lowpass the output of the wind estimate - we want a long-term
-            // stable estimate, but not start to load into the overall dynamics
-            // of the system (which adjusting covariances would do)
-
-            // Change filter coefficient based on altitude change rate
-            float windFiltCoeff = ConstrainFloat(fabsf(windSpdFiltClimb) / 1000.0f, 0.00005f, 0.2f);
-
-            windSpdFiltNorth = ((1.0f - windFiltCoeff) * windSpdFiltNorth) + (windFiltCoeff * vwn);
-            windSpdFiltEast = ((1.0f - windFiltCoeff) * windSpdFiltEast) + (windFiltCoeff * vwe);
-        } else {
-            windSpdFiltNorth = vwn;
-            windSpdFiltEast = vwe;
-        }
-
-        windSpdFiltAltitude = hgtMea;
-
         // Calculate observation jacobians
         SH_TAS[0] = 1/(sqrt(sq(ve - vwe) + sq(vn - vwn) + sq(vd)));
         SH_TAS[1] = (SH_TAS[0]*(2.0f*ve - 2*vwe))/2.0f;
@@ -1750,7 +1664,7 @@ void AttPosEKF::FuseAirspeed()
             Kfusion[20] = SK_TAS*(P[20][4]*SH_TAS[2] - P[20][14]*SH_TAS[2] + P[20][5]*SH_TAS[1] - P[20][15]*SH_TAS[1] + P[20][6]*vd*SH_TAS[0]);
             Kfusion[21] = SK_TAS*(P[21][4]*SH_TAS[2] - P[21][14]*SH_TAS[2] + P[21][5]*SH_TAS[1] - P[21][15]*SH_TAS[1] + P[21][6]*vd*SH_TAS[0]);
         } else {
-            for (uint8_t i=16; i <= 21; i++) {
+            for (uint8_t i=16; i < n_states; i++) {
                 Kfusion[i] = 0;
             }
         }
@@ -1762,7 +1676,7 @@ void AttPosEKF::FuseAirspeed()
         if ((innovVtas*innovVtas*SK_TAS) < 25.0f)
         {
             // correct the state vector
-            for (uint8_t j=0; j <= 22; j++)
+            for (uint8_t j=0; j < n_states; j++)
             {
                 states[j] = states[j] - Kfusion[j] * innovVtas;
             }
@@ -1779,7 +1693,7 @@ void AttPosEKF::FuseAirspeed()
             // correct the covariance P = (I - K*H)*P
             // take advantage of the empty columns in H to reduce the
             // number of operations
-            for (uint8_t i = 0; i <= 22; i++)
+            for (uint8_t i = 0; i < n_states; i++)
             {
                 for (uint8_t j = 0; j <= 3; j++) KH[i][j] = 0.0;
                 for (uint8_t j = 4; j <= 6; j++)
@@ -1791,11 +1705,11 @@ void AttPosEKF::FuseAirspeed()
                 {
                     KH[i][j] = Kfusion[i] * H_TAS[j];
                 }
-                for (uint8_t j = 16; j <= 22; j++) KH[i][j] = 0.0;
+                for (uint8_t j = 16; j < n_states; j++) KH[i][j] = 0.0;
             }
-            for (uint8_t i = 0; i <= 22; i++)
+            for (uint8_t i = 0; i < n_states; i++)
             {
-                for (uint8_t j = 0; j <= 22; j++)
+                for (uint8_t j = 0; j < n_states; j++)
                 {
                     KHP[i][j] = 0.0;
                     for (uint8_t k = 4; k <= 6; k++)
@@ -1808,9 +1722,9 @@ void AttPosEKF::FuseAirspeed()
                     }
                 }
             }
-            for (uint8_t i = 0; i <= 22; i++)
+            for (uint8_t i = 0; i < n_states; i++)
             {
-                for (uint8_t j = 0; j <= 22; j++)
+                for (uint8_t j = 0; j < n_states; j++)
                 {
                     P[i][j] = P[i][j] - KHP[i][j];
                 }
@@ -1835,356 +1749,269 @@ void AttPosEKF::zeroRows(float (&covMat)[n_states][n_states], uint8_t first, uin
     }
 }
 
-void AttPosEKF::FuseRangeFinder()
+void AttPosEKF::FuseOptFlow()
 {
-
-    // Local variables
-    float rngPred;
-    float SH_RNG[5];
-    float H_RNG[23];
-    float SK_RNG[6];
-    float cosRngTilt;
-    const float R_RNG = 0.25f; // 0.5 m2 rangefinder measurement variance
-
-    // Copy required states to local variable names
-    float q0 = statesAtRngTime[0];
-    float q1 = statesAtRngTime[1];
-    float q2 = statesAtRngTime[2];
-    float q3 = statesAtRngTime[3];
-    float pd = statesAtRngTime[9];
-    float ptd = statesAtRngTime[22];
-
-    // Need to check that our range finder tilt angle is less than 30 degrees and we are using range finder data
-    SH_RNG[4] = sinf(rngFinderPitch);
-    cosRngTilt = - Tbn.z.x * SH_RNG[4] + Tbn.z.z * cosf(rngFinderPitch);
-    if (useRangeFinder && fuseRngData && cosRngTilt > 0.87f)
+    static float SH_LOS[13];
+    static float SK_LOS[9];
+    static float q0 = 0.0f;
+    static float q1 = 0.0f;
+    static float q2 = 0.0f;
+    static float q3 = 1.0f;
+    static float vn = 0.0f;
+    static float ve = 0.0f;
+    static float vd = 0.0f;
+    static float pd = 0.0f;
+    static float ptd = 0.0f;
+    static float losPred[2];
+
+    // Transformation matrix from nav to body axes
+    float H_LOS[2][n_states];
+    float K_LOS[2][n_states];
+    Vector3f velNED_local;
+    Vector3f relVelSensor;
+
+    // Perform sequential fusion of optical flow measurements only with valid tilt and height
+    flowStates[1] = maxf(flowStates[1], statesAtFlowTime[9] + minFlowRng);
+    float heightAboveGndEst = flowStates[1] - statesAtFlowTime[9];
+    bool validTilt = Tnb.z.z > 0.71f;
+    if (validTilt)
     {
-        // Calculate observation jacobian and Kalman gain ignoring all states other than the terrain offset
-        // This prevents the range finder measurement modifying any of the other filter states and significantly reduces computations
-        SH_RNG[0] = SH_RNG[4]*(2*q0*q2 - 2*q1*q3) - sq(q0) + sq(q1) + sq(q2) - sq(q3);
-        SH_RNG[1] = pd - ptd;
-        SH_RNG[2] = 1/sq(SH_RNG[0]);
-        SH_RNG[3] = 1/SH_RNG[0];
-        for (uint8_t i = 0; i < n_states; i++) {
-            H_RNG[i] = 0.0f;
-            Kfusion[i] = 0.0f;
-        }
-        H_RNG[22] = -SH_RNG[3];
-        SK_RNG[0] = 1/(R_RNG + SH_RNG[3]*(P[9][9]*SH_RNG[3] - P[22][9]*SH_RNG[3] + P[0][9]*SH_RNG[1]*SH_RNG[2]*(2*q0 - 2*q2*SH_RNG[4]) - P[1][9]*SH_RNG[1]*SH_RNG[2]*(2*q1 - 2*q3*SH_RNG[4]) - P[2][9]*SH_RNG[1]*SH_RNG[2]*(2*q2 + 2*q0*SH_RNG[4]) + P[3][9]*SH_RNG[1]*SH_RNG[2]*(2*q3 + 2*q1*SH_RNG[4])) - SH_RNG[3]*(P[9][22]*SH_RNG[3] - P[22][22]*SH_RNG[3] + P[0][22]*SH_RNG[1]*SH_RNG[2]*(2*q0 - 2*q2*SH_RNG[4]) - P[1][22]*SH_RNG[1]*SH_RNG[2]*(2*q1 - 2*q3*SH_RNG[4]) - P[2][22]*SH_RNG[1]*SH_RNG[2]*(2*q2 + 2*q0*SH_RNG[4]) + P[3][22]*SH_RNG[1]*SH_RNG[2]*(2*q3 + 2*q1*SH_RNG[4])) + SH_RNG[1]*SH_RNG[2]*(2*q0 - 2*q2*SH_RNG[4])*(P[9][0]*SH_RNG[3] - P[22][0]*SH_RNG[3] + P[0][0]*SH_RNG[1]*SH_RNG[2]*(2*q0 - 2*q2*SH_RNG[4]) - P[1][0]*SH_RNG[1]*SH_RNG[2]*(2*q1 - 2*q3*SH_RNG[4]) - P[2][0]*SH_RNG[1]*SH_RNG[2]*(2*q2 + 2*q0*SH_RNG[4]) + P[3][0]*SH_RNG[1]*SH_RNG[2]*(2*q3 + 2*q1*SH_RNG[4])) - SH_RNG[1]*SH_RNG[2]*(2*q1 - 2*q3*SH_RNG[4])*(P[9][1]*SH_RNG[3] - P[22][1]*SH_RNG[3] + P[0][1]*SH_RNG[1]*SH_RNG[2]*(2*q0 - 2*q2*SH_RNG[4]) - P[1][1]*SH_RNG[1]*SH_RNG[2]*(2*q1 - 2*q3*SH_RNG[4]) - P[2][1]*SH_RNG[1]*SH_RNG[2]*(2*q2 + 2*q0*SH_RNG[4]) + P[3][1]*SH_RNG[1]*SH_RNG[2]*(2*q3 + 2*q1*SH_RNG[4])) - SH_RNG[1]*SH_RNG[2]*(2*q2 + 2*q0*SH_RNG[4])*(P[9][2]*SH_RNG[3] - P[22][2]*SH_RNG[3] + P[0][2]*SH_RNG[1]*SH_RNG[2]*(2*q0 - 2*q2*SH_RNG[4]) - P[1][2]*SH_RNG[1]*SH_RNG[2]*(2*q1 - 2*q3*SH_RNG[4]) - P[2][2]*SH_RNG[1]*SH_RNG[2]*(2*q2 + 2*q0*SH_RNG[4]) + P[3][2]*SH_RNG[1]*SH_RNG[2]*(2*q3 + 2*q1*SH_RNG[4])) + SH_RNG[1]*SH_RNG[2]*(2*q3 + 2*q1*SH_RNG[4])*(P[9][3]*SH_RNG[3] - P[22][3]*SH_RNG[3] + P[0][3]*SH_RNG[1]*SH_RNG[2]*(2*q0 - 2*q2*SH_RNG[4]) - P[1][3]*SH_RNG[1]*SH_RNG[2]*(2*q1 - 2*q3*SH_RNG[4]) - P[2][3]*SH_RNG[1]*SH_RNG[2]*(2*q2 + 2*q0*SH_RNG[4]) + P[3][3]*SH_RNG[1]*SH_RNG[2]*(2*q3 + 2*q1*SH_RNG[4])));
-        SK_RNG[1] = 2*q1 - 2*q3*SH_RNG[4];
-        SK_RNG[2] = 2*q0 - 2*q2*SH_RNG[4];
-        SK_RNG[3] = 2*q3 + 2*q1*SH_RNG[4];
-        SK_RNG[4] = 2*q2 + 2*q0*SH_RNG[4];
-        SK_RNG[5] = SH_RNG[2];
-        Kfusion[22] = SK_RNG[0]*(P[22][9]*SH_RNG[3] - P[22][22]*SH_RNG[3] + P[22][0]*SH_RNG[1]*SK_RNG[2]*SK_RNG[5] - P[22][1]*SH_RNG[1]*SK_RNG[1]*SK_RNG[5] - P[22][2]*SH_RNG[1]*SK_RNG[4]*SK_RNG[5] + P[22][3]*SH_RNG[1]*SK_RNG[3]*SK_RNG[5]);
+        // Sequential fusion of XY components.
 
-        // Calculate the innovation variance for data logging
-        varInnovRng = 1.0f/SK_RNG[0];
+        // Calculate observation jacobians and Kalman gains
+        if (fuseOptFlowData)
+        {
+            // Copy required states to local variable names
+            q0       = statesAtFlowTime[0];
+            q1       = statesAtFlowTime[1];
+            q2       = statesAtFlowTime[2];
+            q3       = statesAtFlowTime[3];
+            vn       = statesAtFlowTime[4];
+            ve       = statesAtFlowTime[5];
+            vd       = statesAtFlowTime[6];
+            pd       = statesAtFlowTime[9];
+            ptd      = flowStates[1];
+            velNED_local.x = vn;
+            velNED_local.y = ve;
+            velNED_local.z = vd;
+
+            // calculate range from ground plain to centre of sensor fov assuming flat earth
+            float range = heightAboveGndEst/Tnb_flow.z.z;
+
+            // calculate relative velocity in sensor frame
+            relVelSensor = Tnb_flow*velNED_local;
+
+            // divide velocity by range  and include angular rate effects to get predicted angular LOS rates relative to X and Y axes
+            losPred[0] =  relVelSensor.y/range;
+            losPred[1] = -relVelSensor.x/range;
+
+            // Calculate common expressions for observation jacobians
+            SH_LOS[0] = sq(q0) - sq(q1) - sq(q2) + sq(q3);
+            SH_LOS[1] = vn*(sq(q0) + sq(q1) - sq(q2) - sq(q3)) - vd*(2*q0*q2 - 2*q1*q3) + ve*(2*q0*q3 + 2*q1*q2);
+            SH_LOS[2] = ve*(sq(q0) - sq(q1) + sq(q2) - sq(q3)) + vd*(2*q0*q1 + 2*q2*q3) - vn*(2*q0*q3 - 2*q1*q2);
+            SH_LOS[3] = 1/(pd - ptd);
+            SH_LOS[4] = 1/sq(pd - ptd);
+
+            // Calculate common expressions for Kalman gains
+            SK_LOS[0] = 1.0f/((R_LOS + sq(omegaAcrossFlowTime[0] * moCompR_LOS)) + (SH_LOS[0]*SH_LOS[3]*(2*q3*ve - 2*q2*vd + 2*q0*vn) + 2*q0*SH_LOS[1]*SH_LOS[3])*(P[0][0]*(SH_LOS[0]*SH_LOS[3]*(2*q3*ve - 2*q2*vd + 2*q0*vn) + 2*q0*SH_LOS[1]*SH_LOS[3]) + P[1][0]*(SH_LOS[0]*SH_LOS[3]*(2*q3*vd + 2*q2*ve + 2*q1*vn) - 2*q1*SH_LOS[1]*SH_LOS[3]) - P[2][0]*(SH_LOS[0]*SH_LOS[3]*(2*q0*vd - 2*q1*ve + 2*q2*vn) + 2*q2*SH_LOS[1]*SH_LOS[3]) + P[3][0]*(SH_LOS[0]*SH_LOS[3]*(2*q1*vd + 2*q0*ve - 2*q3*vn) + 2*q3*SH_LOS[1]*SH_LOS[3]) + P[5][0]*SH_LOS[0]*SH_LOS[3]*(2*q0*q3 + 2*q1*q2) - P[6][0]*SH_LOS[0]*SH_LOS[3]*(2*q0*q2 - 2*q1*q3) - P[9][0]*SH_LOS[0]*SH_LOS[1]*SH_LOS[4] + P[4][0]*SH_LOS[0]*SH_LOS[3]*(sq(q0) + sq(q1) - sq(q2) - sq(q3))) + (SH_LOS[0]*SH_LOS[3]*(2*q3*vd + 2*q2*ve + 2*q1*vn) - 2*q1*SH_LOS[1]*SH_LOS[3])*(P[0][1]*(SH_LOS[0]*SH_LOS[3]*(2*q3*ve - 2*q2*vd + 2*q0*vn) + 2*q0*SH_LOS[1]*SH_LOS[3]) + P[1][1]*(SH_LOS[0]*SH_LOS[3]*(2*q3*vd + 2*q2*ve + 2*q1*vn) - 2*q1*SH_LOS[1]*SH_LOS[3]) - P[2][1]*(SH_LOS[0]*SH_LOS[3]*(2*q0*vd - 2*q1*ve + 2*q2*vn) + 2*q2*SH_LOS[1]*SH_LOS[3]) + P[3][1]*(SH_LOS[0]*SH_LOS[3]*(2*q1*vd + 2*q0*ve - 2*q3*vn) + 2*q3*SH_LOS[1]*SH_LOS[3]) + P[5][1]*SH_LOS[0]*SH_LOS[3]*(2*q0*q3 + 2*q1*q2) - P[6][1]*SH_LOS[0]*SH_LOS[3]*(2*q0*q2 - 2*q1*q3) - P[9][1]*SH_LOS[0]*SH_LOS[1]*SH_LOS[4] + P[4][1]*SH_LOS[0]*SH_LOS[3]*(sq(q0) + sq(q1) - sq(q2) - sq(q3))) - (SH_LOS[0]*SH_LOS[3]*(2*q0*vd - 2*q1*ve + 2*q2*vn) + 2*q2*SH_LOS[1]*SH_LOS[3])*(P[0][2]*(SH_LOS[0]*SH_LOS[3]*(2*q3*ve - 2*q2*vd + 2*q0*vn) + 2*q0*SH_LOS[1]*SH_LOS[3]) + P[1][2]*(SH_LOS[0]*SH_LOS[3]*(2*q3*vd + 2*q2*ve + 2*q1*vn) - 2*q1*SH_LOS[1]*SH_LOS[3]) - P[2][2]*(SH_LOS[0]*SH_LOS[3]*(2*q0*vd - 2*q1*ve + 2*q2*vn) + 2*q2*SH_LOS[1]*SH_LOS[3]) + P[3][2]*(SH_LOS[0]*SH_LOS[3]*(2*q1*vd + 2*q0*ve - 2*q3*vn) + 2*q3*SH_LOS[1]*SH_LOS[3]) + P[5][2]*SH_LOS[0]*SH_LOS[3]*(2*q0*q3 + 2*q1*q2) - P[6][2]*SH_LOS[0]*SH_LOS[3]*(2*q0*q2 - 2*q1*q3) - P[9][2]*SH_LOS[0]*SH_LOS[1]*SH_LOS[4] + P[4][2]*SH_LOS[0]*SH_LOS[3]*(sq(q0) + sq(q1) - sq(q2) - sq(q3))) + (SH_LOS[0]*SH_LOS[3]*(2*q1*vd + 2*q0*ve - 2*q3*vn) + 2*q3*SH_LOS[1]*SH_LOS[3])*(P[0][3]*(SH_LOS[0]*SH_LOS[3]*(2*q3*ve - 2*q2*vd + 2*q0*vn) + 2*q0*SH_LOS[1]*SH_LOS[3]) + P[1][3]*(SH_LOS[0]*SH_LOS[3]*(2*q3*vd + 2*q2*ve + 2*q1*vn) - 2*q1*SH_LOS[1]*SH_LOS[3]) - P[2][3]*(SH_LOS[0]*SH_LOS[3]*(2*q0*vd - 2*q1*ve + 2*q2*vn) + 2*q2*SH_LOS[1]*SH_LOS[3]) + P[3][3]*(SH_LOS[0]*SH_LOS[3]*(2*q1*vd + 2*q0*ve - 2*q3*vn) + 2*q3*SH_LOS[1]*SH_LOS[3]) + P[5][3]*SH_LOS[0]*SH_LOS[3]*(2*q0*q3 + 2*q1*q2) - P[6][3]*SH_LOS[0]*SH_LOS[3]*(2*q0*q2 - 2*q1*q3) - P[9][3]*SH_LOS[0]*SH_LOS[1]*SH_LOS[4] + P[4][3]*SH_LOS[0]*SH_LOS[3]*(sq(q0) + sq(q1) - sq(q2) - sq(q3))) - SH_LOS[0]*SH_LOS[1]*SH_LOS[4]*(P[0][9]*(SH_LOS[0]*SH_LOS[3]*(2*q3*ve - 2*q2*vd + 2*q0*vn) + 2*q0*SH_LOS[1]*SH_LOS[3]) + P[1][9]*(SH_LOS[0]*SH_LOS[3]*(2*q3*vd + 2*q2*ve + 2*q1*vn) - 2*q1*SH_LOS[1]*SH_LOS[3]) - P[2][9]*(SH_LOS[0]*SH_LOS[3]*(2*q0*vd - 2*q1*ve + 2*q2*vn) + 2*q2*SH_LOS[1]*SH_LOS[3]) + P[3][9]*(SH_LOS[0]*SH_LOS[3]*(2*q1*vd + 2*q0*ve - 2*q3*vn) + 2*q3*SH_LOS[1]*SH_LOS[3]) + P[5][9]*SH_LOS[0]*SH_LOS[3]*(2*q0*q3 + 2*q1*q2) - P[6][9]*SH_LOS[0]*SH_LOS[3]*(2*q0*q2 - 2*q1*q3) - P[9][9]*SH_LOS[0]*SH_LOS[1]*SH_LOS[4] + P[4][9]*SH_LOS[0]*SH_LOS[3]*(sq(q0) + sq(q1) - sq(q2) - sq(q3))) + SH_LOS[0]*SH_LOS[3]*(sq(q0) + sq(q1) - sq(q2) - sq(q3))*(P[0][4]*(SH_LOS[0]*SH_LOS[3]*(2*q3*ve - 2*q2*vd + 2*q0*vn) + 2*q0*SH_LOS[1]*SH_LOS[3]) + P[1][4]*(SH_LOS[0]*SH_LOS[3]*(2*q3*vd + 2*q2*ve + 2*q1*vn) - 2*q1*SH_LOS[1]*SH_LOS[3]) - P[2][4]*(SH_LOS[0]*SH_LOS[3]*(2*q0*vd - 2*q1*ve + 2*q2*vn) + 2*q2*SH_LOS[1]*SH_LOS[3]) + P[3][4]*(SH_LOS[0]*SH_LOS[3]*(2*q1*vd + 2*q0*ve - 2*q3*vn) + 2*q3*SH_LOS[1]*SH_LOS[3]) + P[5][4]*SH_LOS[0]*SH_LOS[3]*(2*q0*q3 + 2*q1*q2) - P[6][4]*SH_LOS[0]*SH_LOS[3]*(2*q0*q2 - 2*q1*q3) - P[9][4]*SH_LOS[0]*SH_LOS[1]*SH_LOS[4] + P[4][4]*SH_LOS[0]*SH_LOS[3]*(sq(q0) + sq(q1) - sq(q2) - sq(q3))) + SH_LOS[0]*SH_LOS[3]*(2*q0*q3 + 2*q1*q2)*(P[0][5]*(SH_LOS[0]*SH_LOS[3]*(2*q3*ve - 2*q2*vd + 2*q0*vn) + 2*q0*SH_LOS[1]*SH_LOS[3]) + P[1][5]*(SH_LOS[0]*SH_LOS[3]*(2*q3*vd + 2*q2*ve + 2*q1*vn) - 2*q1*SH_LOS[1]*SH_LOS[3]) - P[2][5]*(SH_LOS[0]*SH_LOS[3]*(2*q0*vd - 2*q1*ve + 2*q2*vn) + 2*q2*SH_LOS[1]*SH_LOS[3]) + P[3][5]*(SH_LOS[0]*SH_LOS[3]*(2*q1*vd + 2*q0*ve - 2*q3*vn) + 2*q3*SH_LOS[1]*SH_LOS[3]) + P[5][5]*SH_LOS[0]*SH_LOS[3]*(2*q0*q3 + 2*q1*q2) - P[6][5]*SH_LOS[0]*SH_LOS[3]*(2*q0*q2 - 2*q1*q3) - P[9][5]*SH_LOS[0]*SH_LOS[1]*SH_LOS[4] + P[4][5]*SH_LOS[0]*SH_LOS[3]*(sq(q0) + sq(q1) - sq(q2) - sq(q3))) - SH_LOS[0]*SH_LOS[3]*(2*q0*q2 - 2*q1*q3)*(P[0][6]*(SH_LOS[0]*SH_LOS[3]*(2*q3*ve - 2*q2*vd + 2*q0*vn) + 2*q0*SH_LOS[1]*SH_LOS[3]) + P[1][6]*(SH_LOS[0]*SH_LOS[3]*(2*q3*vd + 2*q2*ve + 2*q1*vn) - 2*q1*SH_LOS[1]*SH_LOS[3]) - P[2][6]*(SH_LOS[0]*SH_LOS[3]*(2*q0*vd - 2*q1*ve + 2*q2*vn) + 2*q2*SH_LOS[1]*SH_LOS[3]) + P[3][6]*(SH_LOS[0]*SH_LOS[3]*(2*q1*vd + 2*q0*ve - 2*q3*vn) + 2*q3*SH_LOS[1]*SH_LOS[3]) + P[5][6]*SH_LOS[0]*SH_LOS[3]*(2*q0*q3 + 2*q1*q2) - P[6][6]*SH_LOS[0]*SH_LOS[3]*(2*q0*q2 - 2*q1*q3) - P[9][6]*SH_LOS[0]*SH_LOS[1]*SH_LOS[4] + P[4][6]*SH_LOS[0]*SH_LOS[3]*(sq(q0) + sq(q1) - sq(q2) - sq(q3))));
+            SK_LOS[1] = 1.0f/((R_LOS + sq(omegaAcrossFlowTime[1] * moCompR_LOS))+ (SH_LOS[0]*SH_LOS[3]*(2*q1*vd + 2*q0*ve - 2*q3*vn) + 2*q0*SH_LOS[2]*SH_LOS[3])*(P[0][0]*(SH_LOS[0]*SH_LOS[3]*(2*q1*vd + 2*q0*ve - 2*q3*vn) + 2*q0*SH_LOS[2]*SH_LOS[3]) + P[1][0]*(SH_LOS[0]*SH_LOS[3]*(2*q0*vd - 2*q1*ve + 2*q2*vn) - 2*q1*SH_LOS[2]*SH_LOS[3]) + P[2][0]*(SH_LOS[0]*SH_LOS[3]*(2*q3*vd + 2*q2*ve + 2*q1*vn) - 2*q2*SH_LOS[2]*SH_LOS[3]) - P[3][0]*(SH_LOS[0]*SH_LOS[3]*(2*q3*ve - 2*q2*vd + 2*q0*vn) - 2*q3*SH_LOS[2]*SH_LOS[3]) - P[4][0]*SH_LOS[0]*SH_LOS[3]*(2*q0*q3 - 2*q1*q2) + P[6][0]*SH_LOS[0]*SH_LOS[3]*(2*q0*q1 + 2*q2*q3) - P[9][0]*SH_LOS[0]*SH_LOS[2]*SH_LOS[4] + P[5][0]*SH_LOS[0]*SH_LOS[3]*(sq(q0) - sq(q1) + sq(q2) - sq(q3))) + (SH_LOS[0]*SH_LOS[3]*(2*q0*vd - 2*q1*ve + 2*q2*vn) - 2*q1*SH_LOS[2]*SH_LOS[3])*(P[0][1]*(SH_LOS[0]*SH_LOS[3]*(2*q1*vd + 2*q0*ve - 2*q3*vn) + 2*q0*SH_LOS[2]*SH_LOS[3]) + P[1][1]*(SH_LOS[0]*SH_LOS[3]*(2*q0*vd - 2*q1*ve + 2*q2*vn) - 2*q1*SH_LOS[2]*SH_LOS[3]) + P[2][1]*(SH_LOS[0]*SH_LOS[3]*(2*q3*vd + 2*q2*ve + 2*q1*vn) - 2*q2*SH_LOS[2]*SH_LOS[3]) - P[3][1]*(SH_LOS[0]*SH_LOS[3]*(2*q3*ve - 2*q2*vd + 2*q0*vn) - 2*q3*SH_LOS[2]*SH_LOS[3]) - P[4][1]*SH_LOS[0]*SH_LOS[3]*(2*q0*q3 - 2*q1*q2) + P[6][1]*SH_LOS[0]*SH_LOS[3]*(2*q0*q1 + 2*q2*q3) - P[9][1]*SH_LOS[0]*SH_LOS[2]*SH_LOS[4] + P[5][1]*SH_LOS[0]*SH_LOS[3]*(sq(q0) - sq(q1) + sq(q2) - sq(q3))) + (SH_LOS[0]*SH_LOS[3]*(2*q3*vd + 2*q2*ve + 2*q1*vn) - 2*q2*SH_LOS[2]*SH_LOS[3])*(P[0][2]*(SH_LOS[0]*SH_LOS[3]*(2*q1*vd + 2*q0*ve - 2*q3*vn) + 2*q0*SH_LOS[2]*SH_LOS[3]) + P[1][2]*(SH_LOS[0]*SH_LOS[3]*(2*q0*vd - 2*q1*ve + 2*q2*vn) - 2*q1*SH_LOS[2]*SH_LOS[3]) + P[2][2]*(SH_LOS[0]*SH_LOS[3]*(2*q3*vd + 2*q2*ve + 2*q1*vn) - 2*q2*SH_LOS[2]*SH_LOS[3]) - P[3][2]*(SH_LOS[0]*SH_LOS[3]*(2*q3*ve - 2*q2*vd + 2*q0*vn) - 2*q3*SH_LOS[2]*SH_LOS[3]) - P[4][2]*SH_LOS[0]*SH_LOS[3]*(2*q0*q3 - 2*q1*q2) + P[6][2]*SH_LOS[0]*SH_LOS[3]*(2*q0*q1 + 2*q2*q3) - P[9][2]*SH_LOS[0]*SH_LOS[2]*SH_LOS[4] + P[5][2]*SH_LOS[0]*SH_LOS[3]*(sq(q0) - sq(q1) + sq(q2) - sq(q3))) - (SH_LOS[0]*SH_LOS[3]*(2*q3*ve - 2*q2*vd + 2*q0*vn) - 2*q3*SH_LOS[2]*SH_LOS[3])*(P[0][3]*(SH_LOS[0]*SH_LOS[3]*(2*q1*vd + 2*q0*ve - 2*q3*vn) + 2*q0*SH_LOS[2]*SH_LOS[3]) + P[1][3]*(SH_LOS[0]*SH_LOS[3]*(2*q0*vd - 2*q1*ve + 2*q2*vn) - 2*q1*SH_LOS[2]*SH_LOS[3]) + P[2][3]*(SH_LOS[0]*SH_LOS[3]*(2*q3*vd + 2*q2*ve + 2*q1*vn) - 2*q2*SH_LOS[2]*SH_LOS[3]) - P[3][3]*(SH_LOS[0]*SH_LOS[3]*(2*q3*ve - 2*q2*vd + 2*q0*vn) - 2*q3*SH_LOS[2]*SH_LOS[3]) - P[4][3]*SH_LOS[0]*SH_LOS[3]*(2*q0*q3 - 2*q1*q2) + P[6][3]*SH_LOS[0]*SH_LOS[3]*(2*q0*q1 + 2*q2*q3) - P[9][3]*SH_LOS[0]*SH_LOS[2]*SH_LOS[4] + P[5][3]*SH_LOS[0]*SH_LOS[3]*(sq(q0) - sq(q1) + sq(q2) - sq(q3))) - SH_LOS[0]*SH_LOS[2]*SH_LOS[4]*(P[0][9]*(SH_LOS[0]*SH_LOS[3]*(2*q1*vd + 2*q0*ve - 2*q3*vn) + 2*q0*SH_LOS[2]*SH_LOS[3]) + P[1][9]*(SH_LOS[0]*SH_LOS[3]*(2*q0*vd - 2*q1*ve + 2*q2*vn) - 2*q1*SH_LOS[2]*SH_LOS[3]) + P[2][9]*(SH_LOS[0]*SH_LOS[3]*(2*q3*vd + 2*q2*ve + 2*q1*vn) - 2*q2*SH_LOS[2]*SH_LOS[3]) - P[3][9]*(SH_LOS[0]*SH_LOS[3]*(2*q3*ve - 2*q2*vd + 2*q0*vn) - 2*q3*SH_LOS[2]*SH_LOS[3]) - P[4][9]*SH_LOS[0]*SH_LOS[3]*(2*q0*q3 - 2*q1*q2) + P[6][9]*SH_LOS[0]*SH_LOS[3]*(2*q0*q1 + 2*q2*q3) - P[9][9]*SH_LOS[0]*SH_LOS[2]*SH_LOS[4] + P[5][9]*SH_LOS[0]*SH_LOS[3]*(sq(q0) - sq(q1) + sq(q2) - sq(q3))) + SH_LOS[0]*SH_LOS[3]*(sq(q0) - sq(q1) + sq(q2) - sq(q3))*(P[0][5]*(SH_LOS[0]*SH_LOS[3]*(2*q1*vd + 2*q0*ve - 2*q3*vn) + 2*q0*SH_LOS[2]*SH_LOS[3]) + P[1][5]*(SH_LOS[0]*SH_LOS[3]*(2*q0*vd - 2*q1*ve + 2*q2*vn) - 2*q1*SH_LOS[2]*SH_LOS[3]) + P[2][5]*(SH_LOS[0]*SH_LOS[3]*(2*q3*vd + 2*q2*ve + 2*q1*vn) - 2*q2*SH_LOS[2]*SH_LOS[3]) - P[3][5]*(SH_LOS[0]*SH_LOS[3]*(2*q3*ve - 2*q2*vd + 2*q0*vn) - 2*q3*SH_LOS[2]*SH_LOS[3]) - P[4][5]*SH_LOS[0]*SH_LOS[3]*(2*q0*q3 - 2*q1*q2) + P[6][5]*SH_LOS[0]*SH_LOS[3]*(2*q0*q1 + 2*q2*q3) - P[9][5]*SH_LOS[0]*SH_LOS[2]*SH_LOS[4] + P[5][5]*SH_LOS[0]*SH_LOS[3]*(sq(q0) - sq(q1) + sq(q2) - sq(q3))) - SH_LOS[0]*SH_LOS[3]*(2*q0*q3 - 2*q1*q2)*(P[0][4]*(SH_LOS[0]*SH_LOS[3]*(2*q1*vd + 2*q0*ve - 2*q3*vn) + 2*q0*SH_LOS[2]*SH_LOS[3]) + P[1][4]*(SH_LOS[0]*SH_LOS[3]*(2*q0*vd - 2*q1*ve + 2*q2*vn) - 2*q1*SH_LOS[2]*SH_LOS[3]) + P[2][4]*(SH_LOS[0]*SH_LOS[3]*(2*q3*vd + 2*q2*ve + 2*q1*vn) - 2*q2*SH_LOS[2]*SH_LOS[3]) - P[3][4]*(SH_LOS[0]*SH_LOS[3]*(2*q3*ve - 2*q2*vd + 2*q0*vn) - 2*q3*SH_LOS[2]*SH_LOS[3]) - P[4][4]*SH_LOS[0]*SH_LOS[3]*(2*q0*q3 - 2*q1*q2) + P[6][4]*SH_LOS[0]*SH_LOS[3]*(2*q0*q1 + 2*q2*q3) - P[9][4]*SH_LOS[0]*SH_LOS[2]*SH_LOS[4] + P[5][4]*SH_LOS[0]*SH_LOS[3]*(sq(q0) - sq(q1) + sq(q2) - sq(q3))) + SH_LOS[0]*SH_LOS[3]*(2*q0*q1 + 2*q2*q3)*(P[0][6]*(SH_LOS[0]*SH_LOS[3]*(2*q1*vd + 2*q0*ve - 2*q3*vn) + 2*q0*SH_LOS[2]*SH_LOS[3]) + P[1][6]*(SH_LOS[0]*SH_LOS[3]*(2*q0*vd - 2*q1*ve + 2*q2*vn) - 2*q1*SH_LOS[2]*SH_LOS[3]) + P[2][6]*(SH_LOS[0]*SH_LOS[3]*(2*q3*vd + 2*q2*ve + 2*q1*vn) - 2*q2*SH_LOS[2]*SH_LOS[3]) - P[3][6]*(SH_LOS[0]*SH_LOS[3]*(2*q3*ve - 2*q2*vd + 2*q0*vn) - 2*q3*SH_LOS[2]*SH_LOS[3]) - P[4][6]*SH_LOS[0]*SH_LOS[3]*(2*q0*q3 - 2*q1*q2) + P[6][6]*SH_LOS[0]*SH_LOS[3]*(2*q0*q1 + 2*q2*q3) - P[9][6]*SH_LOS[0]*SH_LOS[2]*SH_LOS[4] + P[5][6]*SH_LOS[0]*SH_LOS[3]*(sq(q0) - sq(q1) + sq(q2) - sq(q3))));
+            SK_LOS[2] = SH_LOS[0]*SH_LOS[3]*(2*q3*ve - 2*q2*vd + 2*q0*vn);
+            SK_LOS[3] = SH_LOS[0]*SH_LOS[3]*(2*q0*vd - 2*q1*ve + 2*q2*vn);
+            SK_LOS[4] = SH_LOS[0]*SH_LOS[3]*(2*q1*vd + 2*q0*ve - 2*q3*vn);
+            SK_LOS[5] = SH_LOS[0]*SH_LOS[3]*(2*q3*vd + 2*q2*ve + 2*q1*vn);
+            SK_LOS[6] = sq(q0) - sq(q1) + sq(q2) - sq(q3);
+            SK_LOS[7] = sq(q0) + sq(q1) - sq(q2) - sq(q3);
+            SK_LOS[8] = SH_LOS[3];
+
+            // Calculate common intermediate terms
+            float tempVar[9];
+            tempVar[0] = SH_LOS[0]*SK_LOS[6]*SK_LOS[8];
+            tempVar[1] = SH_LOS[0]*SH_LOS[2]*SH_LOS[4];
+            tempVar[2] = 2.0f*SH_LOS[2]*SK_LOS[8];
+            tempVar[3] = SH_LOS[0]*SK_LOS[8]*(2.0f*q0*q1 + 2.0f*q2*q3);
+            tempVar[4] = SH_LOS[0]*SK_LOS[8]*(2.0f*q0*q3 - 2.0f*q1*q2);
+            tempVar[5] = (SK_LOS[5] - q2*tempVar[2]);
+            tempVar[6] = (SK_LOS[2] - q3*tempVar[2]);
+            tempVar[7] = (SK_LOS[3] - q1*tempVar[2]);
+            tempVar[8] = (SK_LOS[4] + q0*tempVar[2]);
+
+            // calculate observation jacobians for X LOS rate
+            for (uint8_t i = 0; i < n_states; i++) H_LOS[0][i] = 0;
+            H_LOS[0][0] = - SH_LOS[0]*SH_LOS[3]*(2*q1*vd + 2*q0*ve - 2*q3*vn) - 2*q0*SH_LOS[2]*SH_LOS[3];
+            H_LOS[0][1] = 2*q1*SH_LOS[2]*SH_LOS[3] - SH_LOS[0]*SH_LOS[3]*(2*q0*vd - 2*q1*ve + 2*q2*vn);
+            H_LOS[0][2] = 2*q2*SH_LOS[2]*SH_LOS[3] - SH_LOS[0]*SH_LOS[3]*(2*q3*vd + 2*q2*ve + 2*q1*vn);
+            H_LOS[0][3] = SH_LOS[0]*SH_LOS[3]*(2*q3*ve - 2*q2*vd + 2*q0*vn) - 2*q3*SH_LOS[2]*SH_LOS[3];
+            H_LOS[0][4] = SH_LOS[0]*SH_LOS[3]*(2*q0*q3 - 2*q1*q2);
+            H_LOS[0][5] = -SH_LOS[0]*SH_LOS[3]*(sq(q0) - sq(q1) + sq(q2) - sq(q3));
+            H_LOS[0][6] = -SH_LOS[0]*SH_LOS[3]*(2*q0*q1 + 2*q2*q3);
+            H_LOS[0][9] = tempVar[1];
+
+            // calculate Kalman gains for X LOS rate
+            K_LOS[0][0] = -(P[0][0]*tempVar[8] + P[0][1]*tempVar[7] - P[0][3]*tempVar[6] + P[0][2]*tempVar[5] - P[0][4]*tempVar[4] + P[0][6]*tempVar[3] - P[0][9]*tempVar[1] + P[0][5]*tempVar[0])/(R_LOS + tempVar[8]*(P[0][0]*tempVar[8] + P[1][0]*tempVar[7] + P[2][0]*tempVar[5] - P[3][0]*tempVar[6] - P[4][0]*tempVar[4] + P[6][0]*tempVar[3] - P[9][0]*tempVar[1] + P[5][0]*tempVar[0]) + tempVar[7]*(P[0][1]*tempVar[8] + P[1][1]*tempVar[7] + P[2][1]*tempVar[5] - P[3][1]*tempVar[6] - P[4][1]*tempVar[4] + P[6][1]*tempVar[3] - P[9][1]*tempVar[1] + P[5][1]*tempVar[0]) + tempVar[5]*(P[0][2]*tempVar[8] + P[1][2]*tempVar[7] + P[2][2]*tempVar[5] - P[3][2]*tempVar[6] - P[4][2]*tempVar[4] + P[6][2]*tempVar[3] - P[9][2]*tempVar[1] + P[5][2]*tempVar[0]) - tempVar[6]*(P[0][3]*tempVar[8] + P[1][3]*tempVar[7] + P[2][3]*tempVar[5] - P[3][3]*tempVar[6] - P[4][3]*tempVar[4] + P[6][3]*tempVar[3] - P[9][3]*tempVar[1] + P[5][3]*tempVar[0]) - tempVar[4]*(P[0][4]*tempVar[8] + P[1][4]*tempVar[7] + P[2][4]*tempVar[5] - P[3][4]*tempVar[6] - P[4][4]*tempVar[4] + P[6][4]*tempVar[3] - P[9][4]*tempVar[1] + P[5][4]*tempVar[0]) + tempVar[3]*(P[0][6]*tempVar[8] + P[1][6]*tempVar[7] + P[2][6]*tempVar[5] - P[3][6]*tempVar[6] - P[4][6]*tempVar[4] + P[6][6]*tempVar[3] - P[9][6]*tempVar[1] + P[5][6]*tempVar[0]) - tempVar[1]*(P[0][9]*tempVar[8] + P[1][9]*tempVar[7] + P[2][9]*tempVar[5] - P[3][9]*tempVar[6] - P[4][9]*tempVar[4] + P[6][9]*tempVar[3] - P[9][9]*tempVar[1] + P[5][9]*tempVar[0]) + tempVar[0]*(P[0][5]*tempVar[8] + P[1][5]*tempVar[7] + P[2][5]*tempVar[5] - P[3][5]*tempVar[6] - P[4][5]*tempVar[4] + P[6][5]*tempVar[3] - P[9][5]*tempVar[1] + P[5][5]*tempVar[0]));
+            K_LOS[0][1] = -SK_LOS[1]*(P[1][0]*tempVar[8] + P[1][1]*tempVar[7] - P[1][3]*tempVar[6] + P[1][2]*tempVar[5] - P[1][4]*tempVar[4] + P[1][6]*tempVar[3] - P[1][9]*tempVar[1] + P[1][5]*tempVar[0]);
+            K_LOS[0][2] = -SK_LOS[1]*(P[2][0]*tempVar[8] + P[2][1]*tempVar[7] - P[2][3]*tempVar[6] + P[2][2]*tempVar[5] - P[2][4]*tempVar[4] + P[2][6]*tempVar[3] - P[2][9]*tempVar[1] + P[2][5]*tempVar[0]);
+            K_LOS[0][3] = -SK_LOS[1]*(P[3][0]*tempVar[8] + P[3][1]*tempVar[7] - P[3][3]*tempVar[6] + P[3][2]*tempVar[5] - P[3][4]*tempVar[4] + P[3][6]*tempVar[3] - P[3][9]*tempVar[1] + P[3][5]*tempVar[0]);
+            K_LOS[0][4] = -SK_LOS[1]*(P[4][0]*tempVar[8] + P[4][1]*tempVar[7] - P[4][3]*tempVar[6] + P[4][2]*tempVar[5] - P[4][4]*tempVar[4] + P[4][6]*tempVar[3] - P[4][9]*tempVar[1] + P[4][5]*tempVar[0]);
+            K_LOS[0][5] = -SK_LOS[1]*(P[5][0]*tempVar[8] + P[5][1]*tempVar[7] - P[5][3]*tempVar[6] + P[5][2]*tempVar[5] - P[5][4]*tempVar[4] + P[5][6]*tempVar[3] - P[5][9]*tempVar[1] + P[5][5]*tempVar[0]);
+            K_LOS[0][6] = -SK_LOS[1]*(P[6][0]*tempVar[8] + P[6][1]*tempVar[7] - P[6][3]*tempVar[6] + P[6][2]*tempVar[5] - P[6][4]*tempVar[4] + P[6][6]*tempVar[3] - P[6][9]*tempVar[1] + P[6][5]*tempVar[0]);
+            K_LOS[0][7] = -SK_LOS[1]*(P[7][0]*tempVar[8] + P[7][1]*tempVar[7] - P[7][3]*tempVar[6] + P[7][2]*tempVar[5] - P[7][4]*tempVar[4] + P[7][6]*tempVar[3] - P[7][9]*tempVar[1] + P[7][5]*tempVar[0]);
+            K_LOS[0][8] = -SK_LOS[1]*(P[8][0]*tempVar[8] + P[8][1]*tempVar[7] - P[8][3]*tempVar[6] + P[8][2]*tempVar[5] - P[8][4]*tempVar[4] + P[8][6]*tempVar[3] - P[8][9]*tempVar[1] + P[8][5]*tempVar[0]);
+            K_LOS[0][9] = -SK_LOS[1]*(P[9][0]*tempVar[8] + P[9][1]*tempVar[7] - P[9][3]*tempVar[6] + P[9][2]*tempVar[5] - P[9][4]*tempVar[4] + P[9][6]*tempVar[3] - P[9][9]*tempVar[1] + P[9][5]*tempVar[0]);
+            K_LOS[0][10] = -SK_LOS[1]*(P[10][0]*tempVar[8] + P[10][1]*tempVar[7] - P[10][3]*tempVar[6] + P[10][2]*tempVar[5] - P[10][4]*tempVar[4] + P[10][6]*tempVar[3] - P[10][9]*tempVar[1] + P[10][5]*tempVar[0]);
+            K_LOS[0][11] = -SK_LOS[1]*(P[11][0]*tempVar[8] + P[11][1]*tempVar[7] - P[11][3]*tempVar[6] + P[11][2]*tempVar[5] - P[11][4]*tempVar[4] + P[11][6]*tempVar[3] - P[11][9]*tempVar[1] + P[11][5]*tempVar[0]);
+            K_LOS[0][12] = -SK_LOS[1]*(P[12][0]*tempVar[8] + P[12][1]*tempVar[7] - P[12][3]*tempVar[6] + P[12][2]*tempVar[5] - P[12][4]*tempVar[4] + P[12][6]*tempVar[3] - P[12][9]*tempVar[1] + P[12][5]*tempVar[0]);
+            // only height measurements are allowed to modify the Z bias state to improve the stability of the estimate
+            K_LOS[0][13] = 0.0f;//-SK_LOS[1]*(P[13][0]*tempVar[8] + P[13][1]*tempVar[7] - P[13][3]*tempVar[6] + P[13][2]*tempVar[5] - P[13][4]*tempVar[4] + P[13][6]*tempVar[3] - P[13][9]*tempVar[1] + P[13][5]*tempVar[0]);
+            if (inhibitWindStates) {
+                K_LOS[0][14] = -SK_LOS[1]*(P[14][0]*tempVar[8] + P[14][1]*tempVar[7] - P[14][3]*tempVar[6] + P[14][2]*tempVar[5] - P[14][4]*tempVar[4] + P[14][6]*tempVar[3] - P[14][9]*tempVar[1] + P[14][5]*tempVar[0]);
+                K_LOS[0][15] = -SK_LOS[1]*(P[15][0]*tempVar[8] + P[15][1]*tempVar[7] - P[15][3]*tempVar[6] + P[15][2]*tempVar[5] - P[15][4]*tempVar[4] + P[15][6]*tempVar[3] - P[15][9]*tempVar[1] + P[15][5]*tempVar[0]);
+            } else {
+                K_LOS[0][14] = 0.0f;
+                K_LOS[0][15] = 0.0f;
+            }
+            if (inhibitMagStates) {
+                K_LOS[0][16] = -SK_LOS[1]*(P[16][0]*tempVar[8] + P[16][1]*tempVar[7] - P[16][3]*tempVar[6] + P[16][2]*tempVar[5] - P[16][4]*tempVar[4] + P[16][6]*tempVar[3] - P[16][9]*tempVar[1] + P[16][5]*tempVar[0]);
+                K_LOS[0][17] = -SK_LOS[1]*(P[17][0]*tempVar[8] + P[17][1]*tempVar[7] - P[17][3]*tempVar[6] + P[17][2]*tempVar[5] - P[17][4]*tempVar[4] + P[17][6]*tempVar[3] - P[17][9]*tempVar[1] + P[17][5]*tempVar[0]);
+                K_LOS[0][18] = -SK_LOS[1]*(P[18][0]*tempVar[8] + P[18][1]*tempVar[7] - P[18][3]*tempVar[6] + P[18][2]*tempVar[5] - P[18][4]*tempVar[4] + P[18][6]*tempVar[3] - P[18][9]*tempVar[1] + P[18][5]*tempVar[0]);
+                K_LOS[0][19] = -SK_LOS[1]*(P[19][0]*tempVar[8] + P[19][1]*tempVar[7] - P[19][3]*tempVar[6] + P[19][2]*tempVar[5] - P[19][4]*tempVar[4] + P[19][6]*tempVar[3] - P[19][9]*tempVar[1] + P[19][5]*tempVar[0]);
+                K_LOS[0][20] = -SK_LOS[1]*(P[20][0]*tempVar[8] + P[20][1]*tempVar[7] - P[20][3]*tempVar[6] + P[20][2]*tempVar[5] - P[20][4]*tempVar[4] + P[20][6]*tempVar[3] - P[20][9]*tempVar[1] + P[20][5]*tempVar[0]);
+                K_LOS[0][21] = -SK_LOS[1]*(P[21][0]*tempVar[8] + P[21][1]*tempVar[7] - P[21][3]*tempVar[6] + P[21][2]*tempVar[5] - P[21][4]*tempVar[4] + P[21][6]*tempVar[3] - P[21][9]*tempVar[1] + P[21][5]*tempVar[0]);
+            } else {
+                for (uint8_t i = 16; i < n_states; i++) {
+                    K_LOS[0][i] = 0.0f;
+                }
+            }
 
-        // Calculate the measurement innovation
-        rngPred = (ptd - pd)/cosRngTilt;
-        innovRng = rngPred - rngMea;
+            // calculate innovation variance and innovation for X axis observation
+            varInnovOptFlow[0] = 1.0f/SK_LOS[0];
+            innovOptFlow[0] = losPred[0] - flowRadXYcomp[0];
+
+            // calculate intermediate common variables
+            tempVar[0] = 2.0f*SH_LOS[1]*SK_LOS[8];
+            tempVar[1] = (SK_LOS[2] + q0*tempVar[0]);
+            tempVar[2] = (SK_LOS[5] - q1*tempVar[0]);
+            tempVar[3] = (SK_LOS[3] + q2*tempVar[0]);
+            tempVar[4] = (SK_LOS[4] + q3*tempVar[0]);
+            tempVar[5] = SH_LOS[0]*SK_LOS[8]*(2*q0*q3 + 2*q1*q2);
+            tempVar[6] = SH_LOS[0]*SK_LOS[8]*(2*q0*q2 - 2*q1*q3);
+            tempVar[7] = SH_LOS[0]*SH_LOS[1]*SH_LOS[4];
+            tempVar[8] = SH_LOS[0]*SK_LOS[7]*SK_LOS[8];
+
+            // Calculate observation jacobians for Y LOS rate
+            for (uint8_t i = 0; i < n_states; i++) H_LOS[1][i] = 0;
+            H_LOS[1][0] = SH_LOS[0]*SH_LOS[3]*(2*q3*ve - 2*q2*vd + 2*q0*vn) + 2*q0*SH_LOS[1]*SH_LOS[3];
+            H_LOS[1][1] = SH_LOS[0]*SH_LOS[3]*(2*q3*vd + 2*q2*ve + 2*q1*vn) - 2*q1*SH_LOS[1]*SH_LOS[3];
+            H_LOS[1][2] = - SH_LOS[0]*SH_LOS[3]*(2*q0*vd - 2*q1*ve + 2*q2*vn) - 2*q2*SH_LOS[1]*SH_LOS[3];
+            H_LOS[1][3] = SH_LOS[0]*SH_LOS[3]*(2*q1*vd + 2*q0*ve - 2*q3*vn) + 2*q3*SH_LOS[1]*SH_LOS[3];
+            H_LOS[1][4] = SH_LOS[0]*SH_LOS[3]*(sq(q0) + sq(q1) - sq(q2) - sq(q3));
+            H_LOS[1][5] = SH_LOS[0]*SH_LOS[3]*(2*q0*q3 + 2*q1*q2);
+            H_LOS[1][6] = -SH_LOS[0]*SH_LOS[3]*(2*q0*q2 - 2*q1*q3);
+            H_LOS[1][9] = -tempVar[7];
+
+            // Calculate Kalman gains for Y LOS rate
+            K_LOS[1][0] = SK_LOS[0]*(P[0][0]*tempVar[1] + P[0][1]*tempVar[2] - P[0][2]*tempVar[3] + P[0][3]*tempVar[4] + P[0][5]*tempVar[5] - P[0][6]*tempVar[6] - P[0][9]*tempVar[7] + P[0][4]*tempVar[8]);
+            K_LOS[1][1] = SK_LOS[0]*(P[1][0]*tempVar[1] + P[1][1]*tempVar[2] - P[1][2]*tempVar[3] + P[1][3]*tempVar[4] + P[1][5]*tempVar[5] - P[1][6]*tempVar[6] - P[1][9]*tempVar[7] + P[1][4]*tempVar[8]);
+            K_LOS[1][2] = SK_LOS[0]*(P[2][0]*tempVar[1] + P[2][1]*tempVar[2] - P[2][2]*tempVar[3] + P[2][3]*tempVar[4] + P[2][5]*tempVar[5] - P[2][6]*tempVar[6] - P[2][9]*tempVar[7] + P[2][4]*tempVar[8]);
+            K_LOS[1][3] = SK_LOS[0]*(P[3][0]*tempVar[1] + P[3][1]*tempVar[2] - P[3][2]*tempVar[3] + P[3][3]*tempVar[4] + P[3][5]*tempVar[5] - P[3][6]*tempVar[6] - P[3][9]*tempVar[7] + P[3][4]*tempVar[8]);
+            K_LOS[1][4] = SK_LOS[0]*(P[4][0]*tempVar[1] + P[4][1]*tempVar[2] - P[4][2]*tempVar[3] + P[4][3]*tempVar[4] + P[4][5]*tempVar[5] - P[4][6]*tempVar[6] - P[4][9]*tempVar[7] + P[4][4]*tempVar[8]);
+            K_LOS[1][5] = SK_LOS[0]*(P[5][0]*tempVar[1] + P[5][1]*tempVar[2] - P[5][2]*tempVar[3] + P[5][3]*tempVar[4] + P[5][5]*tempVar[5] - P[5][6]*tempVar[6] - P[5][9]*tempVar[7] + P[5][4]*tempVar[8]);
+            K_LOS[1][6] = SK_LOS[0]*(P[6][0]*tempVar[1] + P[6][1]*tempVar[2] - P[6][2]*tempVar[3] + P[6][3]*tempVar[4] + P[6][5]*tempVar[5] - P[6][6]*tempVar[6] - P[6][9]*tempVar[7] + P[6][4]*tempVar[8]);
+            K_LOS[1][7] = SK_LOS[0]*(P[7][0]*tempVar[1] + P[7][1]*tempVar[2] - P[7][2]*tempVar[3] + P[7][3]*tempVar[4] + P[7][5]*tempVar[5] - P[7][6]*tempVar[6] - P[7][9]*tempVar[7] + P[7][4]*tempVar[8]);
+            K_LOS[1][8] = SK_LOS[0]*(P[8][0]*tempVar[1] + P[8][1]*tempVar[2] - P[8][2]*tempVar[3] + P[8][3]*tempVar[4] + P[8][5]*tempVar[5] - P[8][6]*tempVar[6] - P[8][9]*tempVar[7] + P[8][4]*tempVar[8]);
+            K_LOS[1][9] = SK_LOS[0]*(P[9][0]*tempVar[1] + P[9][1]*tempVar[2] - P[9][2]*tempVar[3] + P[9][3]*tempVar[4] + P[9][5]*tempVar[5] - P[9][6]*tempVar[6] - P[9][9]*tempVar[7] + P[9][4]*tempVar[8]);
+            K_LOS[1][10] = SK_LOS[0]*(P[10][0]*tempVar[1] + P[10][1]*tempVar[2] - P[10][2]*tempVar[3] + P[10][3]*tempVar[4] + P[10][5]*tempVar[5] - P[10][6]*tempVar[6] - P[10][9]*tempVar[7] + P[10][4]*tempVar[8]);
+            K_LOS[1][11] = SK_LOS[0]*(P[11][0]*tempVar[1] + P[11][1]*tempVar[2] - P[11][2]*tempVar[3] + P[11][3]*tempVar[4] + P[11][5]*tempVar[5] - P[11][6]*tempVar[6] - P[11][9]*tempVar[7] + P[11][4]*tempVar[8]);
+            K_LOS[1][12] = SK_LOS[0]*(P[12][0]*tempVar[1] + P[12][1]*tempVar[2] - P[12][2]*tempVar[3] + P[12][3]*tempVar[4] + P[12][5]*tempVar[5] - P[12][6]*tempVar[6] - P[12][9]*tempVar[7] + P[12][4]*tempVar[8]);
+            // only height measurements are allowed to modify the Z bias state to improve the stability of the estimate
+            K_LOS[1][13] = 0.0f;//SK_LOS[0]*(P[13][0]*tempVar[1] + P[13][1]*tempVar[2] - P[13][2]*tempVar[3] + P[13][3]*tempVar[4] + P[13][5]*tempVar[5] - P[13][6]*tempVar[6] - P[13][9]*tempVar[7] + P[13][4]*tempVar[8]);
+            if (inhibitWindStates) {
+                K_LOS[1][14] = SK_LOS[0]*(P[14][0]*tempVar[1] + P[14][1]*tempVar[2] - P[14][2]*tempVar[3] + P[14][3]*tempVar[4] + P[14][5]*tempVar[5] - P[14][6]*tempVar[6] - P[14][9]*tempVar[7] + P[14][4]*tempVar[8]);
+                K_LOS[1][15] = SK_LOS[0]*(P[15][0]*tempVar[1] + P[15][1]*tempVar[2] - P[15][2]*tempVar[3] + P[15][3]*tempVar[4] + P[15][5]*tempVar[5] - P[15][6]*tempVar[6] - P[15][9]*tempVar[7] + P[15][4]*tempVar[8]);
+            } else {
+                K_LOS[1][14] = 0.0f;
+                K_LOS[1][15] = 0.0f;
+            }
+            if (inhibitMagStates) {
+            K_LOS[1][16] = SK_LOS[0]*(P[16][0]*tempVar[1] + P[16][1]*tempVar[2] - P[16][2]*tempVar[3] + P[16][3]*tempVar[4] + P[16][5]*tempVar[5] - P[16][6]*tempVar[6] - P[16][9]*tempVar[7] + P[16][4]*tempVar[8]);
+            K_LOS[1][17] = SK_LOS[0]*(P[17][0]*tempVar[1] + P[17][1]*tempVar[2] - P[17][2]*tempVar[3] + P[17][3]*tempVar[4] + P[17][5]*tempVar[5] - P[17][6]*tempVar[6] - P[17][9]*tempVar[7] + P[17][4]*tempVar[8]);
+            K_LOS[1][18] = SK_LOS[0]*(P[18][0]*tempVar[1] + P[18][1]*tempVar[2] - P[18][2]*tempVar[3] + P[18][3]*tempVar[4] + P[18][5]*tempVar[5] - P[18][6]*tempVar[6] - P[18][9]*tempVar[7] + P[18][4]*tempVar[8]);
+            K_LOS[1][19] = SK_LOS[0]*(P[19][0]*tempVar[1] + P[19][1]*tempVar[2] - P[19][2]*tempVar[3] + P[19][3]*tempVar[4] + P[19][5]*tempVar[5] - P[19][6]*tempVar[6] - P[19][9]*tempVar[7] + P[19][4]*tempVar[8]);
+            K_LOS[1][20] = SK_LOS[0]*(P[20][0]*tempVar[1] + P[20][1]*tempVar[2] - P[20][2]*tempVar[3] + P[20][3]*tempVar[4] + P[20][5]*tempVar[5] - P[20][6]*tempVar[6] - P[20][9]*tempVar[7] + P[20][4]*tempVar[8]);
+            K_LOS[1][21] = SK_LOS[0]*(P[21][0]*tempVar[1] + P[21][1]*tempVar[2] - P[21][2]*tempVar[3] + P[21][3]*tempVar[4] + P[21][5]*tempVar[5] - P[21][6]*tempVar[6] - P[21][9]*tempVar[7] + P[21][4]*tempVar[8]);
+            } else {
+                for (uint8_t i = 16; i < n_states; i++) {
+                    K_LOS[1][i] = 0.0f;
+                }
+            }
 
-        // calculate the innovation consistency test ratio
-        auxRngTestRatio = sq(innovRng) / (sq(rngInnovGate) * varInnovRng);
+            // calculate variance and innovation for Y observation
+            varInnovOptFlow[1] = 1.0f/SK_LOS[1];
+            innovOptFlow[1] = losPred[1] - flowRadXYcomp[1];
 
-        // Check the innovation for consistency and don't fuse if out of bounds
-        if (auxRngTestRatio < 1.0f)
-        {
-            // correct the state vector
-            states[22] = states[22] - Kfusion[22] * innovRng;
+        }
 
-            // correct the covariance P = (I - K*H)*P
-            P[22][22] = P[22][22] - Kfusion[22] * H_RNG[22] * P[22][22];
-            P[22][22] = ConstrainFloat(P[22][22], 0.0f, 10000.0f);
+        // loop through the X and Y observations and fuse them sequentially
+        for (uint8_t obsIndex = 0; obsIndex < 2; obsIndex++) {
+            // Check the innovation for consistency and don't fuse if > 5Sigma
+            if ((innovOptFlow[obsIndex]*innovOptFlow[obsIndex]/varInnovOptFlow[obsIndex]) < 25.0f) {
+                // correct the state vector
+                for (uint8_t j = 0; j < n_states; j++)
+                {
+                    states[j] = states[j] - K_LOS[obsIndex][j] * innovOptFlow[obsIndex];
+                }
+                // normalise the quaternion states
+                float quatMag = sqrt(states[0]*states[0] + states[1]*states[1] + states[2]*states[2] + states[3]*states[3]);
+                if (quatMag > 1e-12f)
+                {
+                    for (uint8_t j= 0; j<=3; j++)
+                    {
+                        float quatMagInv = 1.0f/quatMag;
+                        states[j] = states[j] * quatMagInv;
+                    }
+                }
+                // correct the covariance P = (I - K*H)*P
+                // take advantage of the empty columns in KH to reduce the
+                // number of operations
+                for (uint8_t i = 0; i < n_states; i++)
+                {
+                    for (uint8_t j = 0; j <= 6; j++)
+                    {
+                        KH[i][j] = K_LOS[obsIndex][i] * H_LOS[obsIndex][j];
+                    }
+                    for (uint8_t j = 7; j <= 8; j++)
+                    {
+                        KH[i][j] = 0.0f;
+                    }
+                    KH[i][9] = K_LOS[obsIndex][i] * H_LOS[obsIndex][9];
+                    for (uint8_t j = 10; j < n_states; j++)
+                    {
+                        KH[i][j] = 0.0f;
+                    }
+                }
+                for (uint8_t i = 0; i < n_states; i++)
+                {
+                    for (uint8_t j = 0; j < n_states; j++)
+                    {
+                        KHP[i][j] = 0.0f;
+                        for (uint8_t k = 0; k <= 6; k++)
+                        {
+                            KHP[i][j] = KHP[i][j] + KH[i][k] * P[k][j];
+                        }
+                        KHP[i][j] = KHP[i][j] + KH[i][9] * P[9][j];
+                    }
+                }
+                for (uint8_t i = 0; i <  n_states; i++)
+                {
+                    for (uint8_t j = 0; j <  n_states; j++)
+                    {
+                        P[i][j] = P[i][j] - KHP[i][j];
+                    }
+                }
+            }
         }
+        ForceSymmetry();
+        ConstrainVariances();
     }
-
-}
-
-void AttPosEKF::FuseOptFlow()
-{
-//     static uint8_t obsIndex;
-//     static float SH_LOS[13];
-//     static float SKK_LOS[15];
-//     static float SK_LOS[2];
-//     static float q0 = 0.0f;
-//     static float q1 = 0.0f;
-//     static float q2 = 0.0f;
-//     static float q3 = 1.0f;
-//     static float vn = 0.0f;
-//     static float ve = 0.0f;
-//     static float vd = 0.0f;
-//     static float pd = 0.0f;
-//     static float ptd = 0.0f;
-//     static float R_LOS = 0.01f;
-//     static float losPred[2];
-
-//     // Transformation matrix from nav to body axes
-//     Mat3f Tnb_local;
-//     // Transformation matrix from body to sensor axes
-//     // assume camera is aligned with Z body axis plus a misalignment
-//     // defined by 3 small angles about X, Y and Z body axis
-//     Mat3f Tbs;
-//     Tbs.x.y =  a3;
-//     Tbs.y.x = -a3;
-//     Tbs.x.z = -a2;
-//     Tbs.z.x =  a2;
-//     Tbs.y.z =  a1;
-//     Tbs.z.y = -a1;
-//     // Transformation matrix from navigation to sensor axes
-//     Mat3f Tns;
-//     float H_LOS[n_states];
-//     for (uint8_t i = 0; i < n_states; i++) {
-//         H_LOS[i] = 0.0f;
-//     }
-//     Vector3f velNED_local;
-//     Vector3f relVelSensor;
-
-// // Perform sequential fusion of optical flow measurements only when in the air and tilt is less than 30 deg.
-//     if (useOpticalFlow && (fuseOptFlowData || obsIndex == 1) && !onGround && Tbs.z.z > 0.866f && rngMea > 5.0f && rngMea < 39.0f)
-//     {
-//         // Sequential fusion of XY components to spread processing load across
-//         // two prediction time steps.
-
-//         // Calculate observation jacobians and Kalman gains
-//         if (fuseOptFlowData)
-//         {
-//             // Copy required states to local variable names
-//             q0       = statesAtOptFlowTime[0];
-//             q1       = statesAtOptFlowTime[1];
-//             q2       = statesAtOptFlowTime[2];
-//             q3       = statesAtOptFlowTime[3];
-//             vn       = statesAtOptFlowTime[4];
-//             ve       = statesAtOptFlowTime[5];
-//             vd       = statesAtOptFlowTime[6];
-//             pd       = statesAtOptFlowTime[9];
-//             ptd      = statesAtOptFlowTime[22];
-//             velNED_local.x = vn;
-//             velNED_local.y = ve;
-//             velNED_local.z = vd;
-
-//             // calculate rotation from NED to body axes
-//             float q00 = sq(q0);
-//             float q11 = sq(q1);
-//             float q22 = sq(q2);
-//             float q33 = sq(q3);
-//             float q01 = q0 * q1;
-//             float q02 = q0 * q2;
-//             float q03 = q0 * q3;
-//             float q12 = q1 * q2;
-//             float q13 = q1 * q3;
-//             float q23 = q2 * q3;
-//             Tnb_local.x.x = q00 + q11 - q22 - q33;
-//             Tnb_local.y.y = q00 - q11 + q22 - q33;
-//             Tnb_local.z.z = q00 - q11 - q22 + q33;
-//             Tnb_local.y.x = 2*(q12 - q03);
-//             Tnb_local.z.x = 2*(q13 + q02);
-//             Tnb_local.x.y = 2*(q12 + q03);
-//             Tnb_local.z.y = 2*(q23 - q01);
-//             Tnb_local.x.z = 2*(q13 - q02);
-//             Tnb_local.y.z = 2*(q23 + q01);
-
-//             // calculate transformation from NED to sensor axes
-//             Tns = Tbs*Tnb_local;
-
-//             // calculate range from ground plain to centre of sensor fov assuming flat earth
-//             float range = ConstrainFloat(((ptd - pd)/Tns.z.z),0.5f,100.0f);
-
-//             // calculate relative velocity in sensor frame
-//             relVelSensor = Tns*velNED_local;
-
-//             // divide velocity by range  and include angular rate effects to get predicted angular LOS rates relative to X and Y axes
-//             losPred[0] =  relVelSensor.y/range;
-//             losPred[1] = -relVelSensor.x/range;
-
-//             //printf("relVelSensor.x=%5.1f, relVelSensor.y=%5.1f\n", relVelSensor.x, relVelSensor.y);
-//             //printf("Xpred=%5.2f, Xmea=%5.2f, Ypred=%5.2f, Ymea=%5.2f, delAng.x=%4.4f, delAng.y=%4.4f\n", losPred[0], losData[0], losPred[1], losData[1], delAng.x, delAng.y);
-//             //printf("omegaX=%5.2f, omegaY=%5.2f, velY=%5.1f velX=%5.1f\n, range=%5.1f\n", delAngRel.x/dt, delAngRel.y/dt, relVelSensor.y, relVelSensor.x, range);
-
-//             // Calculate observation jacobians
-//             SH_LOS[0] = a1*(2*q0*q1 + 2*q2*q3) + a2*(2*q0*q2 - 2*q1*q3) - sq(q0) + sq(q1) + sq(q2) - sq(q3);
-//             SH_LOS[1] = vd*(a2*(sq(q0) - sq(q1) - sq(q2) + sq(q3)) + 2*q0*q2 - 2*q1*q3 - a3*(2*q0*q1 + 2*q2*q3)) - ve*(a3*(sq(q0) - sq(q1) + sq(q2) - sq(q3)) + 2*q0*q3 + 2*q1*q2 + a2*(2*q0*q1 - 2*q2*q3)) + vn*(a2*(2*q0*q2 + 2*q1*q3) + a3*(2*q0*q3 - 2*q1*q2) - sq(q0) - sq(q1) + sq(q2) + sq(q3));
-//             SH_LOS[2] = ve*(a1*(2*q0*q1 - 2*q2*q3) + a3*(2*q0*q3 + 2*q1*q2) - sq(q0) + sq(q1) - sq(q2) + sq(q3)) - vd*(a1*(sq(q0) - sq(q1) - sq(q2) + sq(q3)) + 2*q0*q1 + 2*q2*q3 + a3*(2*q0*q2 - 2*q1*q3)) + vn*(a3*(sq(q0) + sq(q1) - sq(q2) - sq(q3)) + 2*q0*q3 - 2*q1*q2 - a1*(2*q0*q2 + 2*q1*q3));
-//             SH_LOS[3] = 1/(pd - ptd);
-//             SH_LOS[4] = 2*q1 - 2*a2*q3 + 2*a3*q2;
-//             SH_LOS[5] = 2*a2*q2 - 2*q0 + 2*a3*q3;
-//             SH_LOS[6] = 2*q2 + 2*a2*q0 - 2*a3*q1;
-//             SH_LOS[7] = 1/sq(pd - ptd);
-//             SH_LOS[8] = 2*q2 + 2*a1*q3 - 2*a3*q1;
-//             SH_LOS[9] = 2*q3 - 2*a1*q2 + 2*a3*q0;
-//             SH_LOS[10] = 2*a1*q1 - 2*q0 + 2*a3*q3;
-//             SH_LOS[11] = 2*q3 + 2*a2*q1 + 2*a3*q0;
-//             SH_LOS[12] = 2*q1 + 2*a1*q0 + 2*a3*q2;
-
-//             for (uint8_t i = 0; i < n_states; i++) H_LOS[i] = 0;
-//             H_LOS[0] = - SH_LOS[2]*SH_LOS[3]*(2*a1*q1 - 2*q0 + 2*a2*q2) - SH_LOS[0]*SH_LOS[3]*(ve*SH_LOS[10] - vd*SH_LOS[12] + vn*SH_LOS[9]);
-//             H_LOS[1] = - SH_LOS[2]*SH_LOS[3]*(2*q1 + 2*a1*q0 - 2*a2*q3) - SH_LOS[0]*SH_LOS[3]*(vd*SH_LOS[10] + ve*SH_LOS[12] - vn*SH_LOS[8]);
-//             H_LOS[2] = SH_LOS[0]*SH_LOS[3]*(vd*SH_LOS[9] + ve*SH_LOS[8] + vn*SH_LOS[12]) - SH_LOS[2]*SH_LOS[3]*(2*q2 + 2*a2*q0 + 2*a1*q3);
-//             H_LOS[3] = SH_LOS[2]*SH_LOS[3]*(2*q3 - 2*a1*q2 + 2*a2*q1) + SH_LOS[0]*SH_LOS[3]*(vd*SH_LOS[8] - ve*SH_LOS[9] + vn*SH_LOS[10]);
-//             H_LOS[4] = -SH_LOS[0]*SH_LOS[3]*(a3*(sq(q0) + sq(q1) - sq(q2) - sq(q3)) + 2*q0*q3 - 2*q1*q2 - a1*(2*q0*q2 + 2*q1*q3));
-//             H_LOS[5] = -SH_LOS[0]*SH_LOS[3]*(a1*(2*q0*q1 - 2*q2*q3) + a3*(2*q0*q3 + 2*q1*q2) - sq(q0) + sq(q1) - sq(q2) + sq(q3));
-//             H_LOS[6] = SH_LOS[0]*SH_LOS[3]*(a1*(sq(q0) - sq(q1) - sq(q2) + sq(q3)) + 2*q0*q1 + 2*q2*q3 + a3*(2*q0*q2 - 2*q1*q3));
-//             H_LOS[9] = SH_LOS[0]*SH_LOS[2]*SH_LOS[7];
-//             H_LOS[22] = -SH_LOS[0]*SH_LOS[2]*SH_LOS[7];
-
-//             // Calculate Kalman gain
-//             SKK_LOS[0] = a2*(sq(q0) - sq(q1) - sq(q2) + sq(q3)) + 2*q0*q2 - 2*q1*q3 - a3*(2*q0*q1 + 2*q2*q3);
-//             SKK_LOS[1] = a3*(sq(q0) - sq(q1) + sq(q2) - sq(q3)) + 2*q0*q3 + 2*q1*q2 + a2*(2*q0*q1 - 2*q2*q3);
-//             SKK_LOS[2] = a2*(2*q0*q2 + 2*q1*q3) + a3*(2*q0*q3 - 2*q1*q2) - sq(q0) - sq(q1) + sq(q2) + sq(q3);
-//             SKK_LOS[3] = a1*(sq(q0) - sq(q1) - sq(q2) + sq(q3)) + 2*q0*q1 + 2*q2*q3 + a3*(2*q0*q2 - 2*q1*q3);
-//             SKK_LOS[4] = a1*(2*q0*q1 - 2*q2*q3) + a3*(2*q0*q3 + 2*q1*q2) - sq(q0) + sq(q1) - sq(q2) + sq(q3);
-//             SKK_LOS[5] = a3*(sq(q0) + sq(q1) - sq(q2) - sq(q3)) + 2*q0*q3 - 2*q1*q2 - a1*(2*q0*q2 + 2*q1*q3);
-//             SKK_LOS[6] = SH_LOS[2]*SH_LOS[3]*(2*q3 - 2*a1*q2 + 2*a2*q1) + SH_LOS[0]*SH_LOS[3]*(vd*SH_LOS[8] - ve*SH_LOS[9] + vn*SH_LOS[10]);
-//             SKK_LOS[7] = SH_LOS[2]*SH_LOS[3]*(2*q1 + 2*a1*q0 - 2*a2*q3) + SH_LOS[0]*SH_LOS[3]*(vd*SH_LOS[10] + ve*SH_LOS[12] - vn*SH_LOS[8]);
-//             SKK_LOS[8] = SH_LOS[2]*SH_LOS[3]*(2*a1*q1 - 2*q0 + 2*a2*q2) + SH_LOS[0]*SH_LOS[3]*(ve*SH_LOS[10] - vd*SH_LOS[12] + vn*SH_LOS[9]);
-//             SKK_LOS[9] = SH_LOS[2]*SH_LOS[3]*(2*q2 + 2*a2*q0 + 2*a1*q3) - SH_LOS[0]*SH_LOS[3]*(vd*SH_LOS[9] + ve*SH_LOS[8] + vn*SH_LOS[12]);
-//             SKK_LOS[10] = SH_LOS[1]*SH_LOS[3]*(2*a1*q1 - 2*q0 + 2*a2*q2) + SH_LOS[0]*SH_LOS[3]*(vd*SH_LOS[6] - ve*SH_LOS[11] + vn*SH_LOS[5]);
-//             SKK_LOS[11] = SH_LOS[1]*SH_LOS[3]*(2*q1 + 2*a1*q0 - 2*a2*q3) - SH_LOS[0]*SH_LOS[3]*(vd*SH_LOS[11] + ve*SH_LOS[6] + vn*SH_LOS[4]);
-//             SKK_LOS[12] = SH_LOS[1]*SH_LOS[3]*(2*q2 + 2*a2*q0 + 2*a1*q3) - SH_LOS[0]*SH_LOS[3]*(vd*SH_LOS[5] + ve*SH_LOS[4] - vn*SH_LOS[6]);
-//             SKK_LOS[13] = SH_LOS[1]*SH_LOS[3]*(2*q3 - 2*a1*q2 + 2*a2*q1) - SH_LOS[0]*SH_LOS[3]*(ve*SH_LOS[5] - vd*SH_LOS[4] + vn*SH_LOS[11]);
-//             SKK_LOS[14] = SH_LOS[0];
-
-//             SK_LOS[0] = 1/(R_LOS + SKK_LOS[8]*(P[0][0]*SKK_LOS[8] + P[1][0]*SKK_LOS[7] + P[2][0]*SKK_LOS[9] - P[3][0]*SKK_LOS[6] - P[9][0]*SH_LOS[2]*SH_LOS[7]*SKK_LOS[14] + P[22][0]*SH_LOS[2]*SH_LOS[7]*SKK_LOS[14] + P[4][0]*SH_LOS[3]*SKK_LOS[5]*SKK_LOS[14] + P[5][0]*SH_LOS[3]*SKK_LOS[4]*SKK_LOS[14] - P[6][0]*SH_LOS[3]*SKK_LOS[3]*SKK_LOS[14]) + SKK_LOS[7]*(P[0][1]*SKK_LOS[8] + P[1][1]*SKK_LOS[7] + P[2][1]*SKK_LOS[9] - P[3][1]*SKK_LOS[6] - P[9][1]*SH_LOS[2]*SH_LOS[7]*SKK_LOS[14] + P[22][1]*SH_LOS[2]*SH_LOS[7]*SKK_LOS[14] + P[4][1]*SH_LOS[3]*SKK_LOS[5]*SKK_LOS[14] + P[5][1]*SH_LOS[3]*SKK_LOS[4]*SKK_LOS[14] - P[6][1]*SH_LOS[3]*SKK_LOS[3]*SKK_LOS[14]) + SKK_LOS[9]*(P[0][2]*SKK_LOS[8] + P[1][2]*SKK_LOS[7] + P[2][2]*SKK_LOS[9] - P[3][2]*SKK_LOS[6] - P[9][2]*SH_LOS[2]*SH_LOS[7]*SKK_LOS[14] + P[22][2]*SH_LOS[2]*SH_LOS[7]*SKK_LOS[14] + P[4][2]*SH_LOS[3]*SKK_LOS[5]*SKK_LOS[14] + P[5][2]*SH_LOS[3]*SKK_LOS[4]*SKK_LOS[14] - P[6][2]*SH_LOS[3]*SKK_LOS[3]*SKK_LOS[14]) - SKK_LOS[6]*(P[0][3]*SKK_LOS[8] + P[1][3]*SKK_LOS[7] + P[2][3]*SKK_LOS[9] - P[3][3]*SKK_LOS[6] - P[9][3]*SH_LOS[2]*SH_LOS[7]*SKK_LOS[14] + P[22][3]*SH_LOS[2]*SH_LOS[7]*SKK_LOS[14] + P[4][3]*SH_LOS[3]*SKK_LOS[5]*SKK_LOS[14] + P[5][3]*SH_LOS[3]*SKK_LOS[4]*SKK_LOS[14] - P[6][3]*SH_LOS[3]*SKK_LOS[3]*SKK_LOS[14]) - SH_LOS[2]*SH_LOS[7]*SKK_LOS[14]*(P[0][9]*SKK_LOS[8] + P[1][9]*SKK_LOS[7] + P[2][9]*SKK_LOS[9] - P[3][9]*SKK_LOS[6] - P[9][9]*SH_LOS[2]*SH_LOS[7]*SKK_LOS[14] + P[22][9]*SH_LOS[2]*SH_LOS[7]*SKK_LOS[14] + P[4][9]*SH_LOS[3]*SKK_LOS[5]*SKK_LOS[14] + P[5][9]*SH_LOS[3]*SKK_LOS[4]*SKK_LOS[14] - P[6][9]*SH_LOS[3]*SKK_LOS[3]*SKK_LOS[14]) + SH_LOS[2]*SH_LOS[7]*SKK_LOS[14]*(P[0][22]*SKK_LOS[8] + P[1][22]*SKK_LOS[7] + P[2][22]*SKK_LOS[9] - P[3][22]*SKK_LOS[6] - P[9][22]*SH_LOS[2]*SH_LOS[7]*SKK_LOS[14] + P[22][22]*SH_LOS[2]*SH_LOS[7]*SKK_LOS[14] + P[4][22]*SH_LOS[3]*SKK_LOS[5]*SKK_LOS[14] + P[5][22]*SH_LOS[3]*SKK_LOS[4]*SKK_LOS[14] - P[6][22]*SH_LOS[3]*SKK_LOS[3]*SKK_LOS[14]) + SH_LOS[3]*SKK_LOS[5]*SKK_LOS[14]*(P[0][4]*SKK_LOS[8] + P[1][4]*SKK_LOS[7] + P[2][4]*SKK_LOS[9] - P[3][4]*SKK_LOS[6] - P[9][4]*SH_LOS[2]*SH_LOS[7]*SKK_LOS[14] + P[22][4]*SH_LOS[2]*SH_LOS[7]*SKK_LOS[14] + P[4][4]*SH_LOS[3]*SKK_LOS[5]*SKK_LOS[14] + P[5][4]*SH_LOS[3]*SKK_LOS[4]*SKK_LOS[14] - P[6][4]*SH_LOS[3]*SKK_LOS[3]*SKK_LOS[14]) + SH_LOS[3]*SKK_LOS[4]*SKK_LOS[14]*(P[0][5]*SKK_LOS[8] + P[1][5]*SKK_LOS[7] + P[2][5]*SKK_LOS[9] - P[3][5]*SKK_LOS[6] - P[9][5]*SH_LOS[2]*SH_LOS[7]*SKK_LOS[14] + P[22][5]*SH_LOS[2]*SH_LOS[7]*SKK_LOS[14] + P[4][5]*SH_LOS[3]*SKK_LOS[5]*SKK_LOS[14] + P[5][5]*SH_LOS[3]*SKK_LOS[4]*SKK_LOS[14] - P[6][5]*SH_LOS[3]*SKK_LOS[3]*SKK_LOS[14]) - SH_LOS[3]*SKK_LOS[3]*SKK_LOS[14]*(P[0][6]*SKK_LOS[8] + P[1][6]*SKK_LOS[7] + P[2][6]*SKK_LOS[9] - P[3][6]*SKK_LOS[6] - P[9][6]*SH_LOS[2]*SH_LOS[7]*SKK_LOS[14] + P[22][6]*SH_LOS[2]*SH_LOS[7]*SKK_LOS[14] + P[4][6]*SH_LOS[3]*SKK_LOS[5]*SKK_LOS[14] + P[5][6]*SH_LOS[3]*SKK_LOS[4]*SKK_LOS[14] - P[6][6]*SH_LOS[3]*SKK_LOS[3]*SKK_LOS[14]));
-//             Kfusion[0] = -SK_LOS[0]*(P[0][0]*SKK_LOS[8] + P[0][1]*SKK_LOS[7] - P[0][3]*SKK_LOS[6] + P[0][2]*SKK_LOS[9] - P[0][9]*SH_LOS[2]*SH_LOS[7]*SKK_LOS[14] + P[0][22]*SH_LOS[2]*SH_LOS[7]*SKK_LOS[14] + P[0][4]*SH_LOS[3]*SKK_LOS[5]*SKK_LOS[14] + P[0][5]*SH_LOS[3]*SKK_LOS[4]*SKK_LOS[14] - P[0][6]*SH_LOS[3]*SKK_LOS[3]*SKK_LOS[14]);
-//             Kfusion[1] = -SK_LOS[0]*(P[1][0]*SKK_LOS[8] + P[1][1]*SKK_LOS[7] - P[1][3]*SKK_LOS[6] + P[1][2]*SKK_LOS[9] - P[1][9]*SH_LOS[2]*SH_LOS[7]*SKK_LOS[14] + P[1][22]*SH_LOS[2]*SH_LOS[7]*SKK_LOS[14] + P[1][4]*SH_LOS[3]*SKK_LOS[5]*SKK_LOS[14] + P[1][5]*SH_LOS[3]*SKK_LOS[4]*SKK_LOS[14] - P[1][6]*SH_LOS[3]*SKK_LOS[3]*SKK_LOS[14]);
-//             Kfusion[2] = -SK_LOS[0]*(P[2][0]*SKK_LOS[8] + P[2][1]*SKK_LOS[7] - P[2][3]*SKK_LOS[6] + P[2][2]*SKK_LOS[9] - P[2][9]*SH_LOS[2]*SH_LOS[7]*SKK_LOS[14] + P[2][22]*SH_LOS[2]*SH_LOS[7]*SKK_LOS[14] + P[2][4]*SH_LOS[3]*SKK_LOS[5]*SKK_LOS[14] + P[2][5]*SH_LOS[3]*SKK_LOS[4]*SKK_LOS[14] - P[2][6]*SH_LOS[3]*SKK_LOS[3]*SKK_LOS[14]);
-//             Kfusion[3] = -SK_LOS[0]*(P[3][0]*SKK_LOS[8] + P[3][1]*SKK_LOS[7] - P[3][3]*SKK_LOS[6] + P[3][2]*SKK_LOS[9] - P[3][9]*SH_LOS[2]*SH_LOS[7]*SKK_LOS[14] + P[3][22]*SH_LOS[2]*SH_LOS[7]*SKK_LOS[14] + P[3][4]*SH_LOS[3]*SKK_LOS[5]*SKK_LOS[14] + P[3][5]*SH_LOS[3]*SKK_LOS[4]*SKK_LOS[14] - P[3][6]*SH_LOS[3]*SKK_LOS[3]*SKK_LOS[14]);
-//             Kfusion[4] = -SK_LOS[0]*(P[4][0]*SKK_LOS[8] + P[4][1]*SKK_LOS[7] - P[4][3]*SKK_LOS[6] + P[4][2]*SKK_LOS[9] - P[4][9]*SH_LOS[2]*SH_LOS[7]*SKK_LOS[14] + P[4][22]*SH_LOS[2]*SH_LOS[7]*SKK_LOS[14] + P[4][4]*SH_LOS[3]*SKK_LOS[5]*SKK_LOS[14] + P[4][5]*SH_LOS[3]*SKK_LOS[4]*SKK_LOS[14] - P[4][6]*SH_LOS[3]*SKK_LOS[3]*SKK_LOS[14]);
-//             Kfusion[5] = -SK_LOS[0]*(P[5][0]*SKK_LOS[8] + P[5][1]*SKK_LOS[7] - P[5][3]*SKK_LOS[6] + P[5][2]*SKK_LOS[9] - P[5][9]*SH_LOS[2]*SH_LOS[7]*SKK_LOS[14] + P[5][22]*SH_LOS[2]*SH_LOS[7]*SKK_LOS[14] + P[5][4]*SH_LOS[3]*SKK_LOS[5]*SKK_LOS[14] + P[5][5]*SH_LOS[3]*SKK_LOS[4]*SKK_LOS[14] - P[5][6]*SH_LOS[3]*SKK_LOS[3]*SKK_LOS[14]);
-//             Kfusion[6] = -SK_LOS[0]*(P[6][0]*SKK_LOS[8] + P[6][1]*SKK_LOS[7] - P[6][3]*SKK_LOS[6] + P[6][2]*SKK_LOS[9] - P[6][9]*SH_LOS[2]*SH_LOS[7]*SKK_LOS[14] + P[6][22]*SH_LOS[2]*SH_LOS[7]*SKK_LOS[14] + P[6][4]*SH_LOS[3]*SKK_LOS[5]*SKK_LOS[14] + P[6][5]*SH_LOS[3]*SKK_LOS[4]*SKK_LOS[14] - P[6][6]*SH_LOS[3]*SKK_LOS[3]*SKK_LOS[14]);
-//             Kfusion[7] = -SK_LOS[0]*(P[7][0]*SKK_LOS[8] + P[7][1]*SKK_LOS[7] - P[7][3]*SKK_LOS[6] + P[7][2]*SKK_LOS[9] - P[7][9]*SH_LOS[2]*SH_LOS[7]*SKK_LOS[14] + P[7][22]*SH_LOS[2]*SH_LOS[7]*SKK_LOS[14] + P[7][4]*SH_LOS[3]*SKK_LOS[5]*SKK_LOS[14] + P[7][5]*SH_LOS[3]*SKK_LOS[4]*SKK_LOS[14] - P[7][6]*SH_LOS[3]*SKK_LOS[3]*SKK_LOS[14]);
-//             Kfusion[8] = -SK_LOS[0]*(P[8][0]*SKK_LOS[8] + P[8][1]*SKK_LOS[7] - P[8][3]*SKK_LOS[6] + P[8][2]*SKK_LOS[9] - P[8][9]*SH_LOS[2]*SH_LOS[7]*SKK_LOS[14] + P[8][22]*SH_LOS[2]*SH_LOS[7]*SKK_LOS[14] + P[8][4]*SH_LOS[3]*SKK_LOS[5]*SKK_LOS[14] + P[8][5]*SH_LOS[3]*SKK_LOS[4]*SKK_LOS[14] - P[8][6]*SH_LOS[3]*SKK_LOS[3]*SKK_LOS[14]);
-//             Kfusion[9] = -SK_LOS[0]*(P[9][0]*SKK_LOS[8] + P[9][1]*SKK_LOS[7] - P[9][3]*SKK_LOS[6] + P[9][2]*SKK_LOS[9] - P[9][9]*SH_LOS[2]*SH_LOS[7]*SKK_LOS[14] + P[9][22]*SH_LOS[2]*SH_LOS[7]*SKK_LOS[14] + P[9][4]*SH_LOS[3]*SKK_LOS[5]*SKK_LOS[14] + P[9][5]*SH_LOS[3]*SKK_LOS[4]*SKK_LOS[14] - P[9][6]*SH_LOS[3]*SKK_LOS[3]*SKK_LOS[14]);
-//             Kfusion[10] = -SK_LOS[0]*(P[10][0]*SKK_LOS[8] + P[10][1]*SKK_LOS[7] - P[10][3]*SKK_LOS[6] + P[10][2]*SKK_LOS[9] - P[10][9]*SH_LOS[2]*SH_LOS[7]*SKK_LOS[14] + P[10][22]*SH_LOS[2]*SH_LOS[7]*SKK_LOS[14] + P[10][4]*SH_LOS[3]*SKK_LOS[5]*SKK_LOS[14] + P[10][5]*SH_LOS[3]*SKK_LOS[4]*SKK_LOS[14] - P[10][6]*SH_LOS[3]*SKK_LOS[3]*SKK_LOS[14]);
-//             Kfusion[11] = -SK_LOS[0]*(P[11][0]*SKK_LOS[8] + P[11][1]*SKK_LOS[7] - P[11][3]*SKK_LOS[6] + P[11][2]*SKK_LOS[9] - P[11][9]*SH_LOS[2]*SH_LOS[7]*SKK_LOS[14] + P[11][22]*SH_LOS[2]*SH_LOS[7]*SKK_LOS[14] + P[11][4]*SH_LOS[3]*SKK_LOS[5]*SKK_LOS[14] + P[11][5]*SH_LOS[3]*SKK_LOS[4]*SKK_LOS[14] - P[11][6]*SH_LOS[3]*SKK_LOS[3]*SKK_LOS[14]);
-//             Kfusion[12] = -SK_LOS[0]*(P[12][0]*SKK_LOS[8] + P[12][1]*SKK_LOS[7] - P[12][3]*SKK_LOS[6] + P[12][2]*SKK_LOS[9] - P[12][9]*SH_LOS[2]*SH_LOS[7]*SKK_LOS[14] + P[12][22]*SH_LOS[2]*SH_LOS[7]*SKK_LOS[14] + P[12][4]*SH_LOS[3]*SKK_LOS[5]*SKK_LOS[14] + P[12][5]*SH_LOS[3]*SKK_LOS[4]*SKK_LOS[14] - P[12][6]*SH_LOS[3]*SKK_LOS[3]*SKK_LOS[14]);
-//             Kfusion[13] = 0.0f;//-SK_LOS[0]*(P[13][0]*SKK_LOS[8] + P[13][1]*SKK_LOS[7] - P[13][3]*SKK_LOS[6] + P[13][2]*SKK_LOS[9] - P[13][9]*SH_LOS[2]*SH_LOS[7]*SKK_LOS[14] + P[13][22]*SH_LOS[2]*SH_LOS[7]*SKK_LOS[14] + P[13][4]*SH_LOS[3]*SKK_LOS[5]*SKK_LOS[14] + P[13][5]*SH_LOS[3]*SKK_LOS[4]*SKK_LOS[14] - P[13][6]*SH_LOS[3]*SKK_LOS[3]*SKK_LOS[14]);
-//             Kfusion[14] = -SK_LOS[0]*(P[14][0]*SKK_LOS[8] + P[14][1]*SKK_LOS[7] - P[14][3]*SKK_LOS[6] + P[14][2]*SKK_LOS[9] - P[14][9]*SH_LOS[2]*SH_LOS[7]*SKK_LOS[14] + P[14][22]*SH_LOS[2]*SH_LOS[7]*SKK_LOS[14] + P[14][4]*SH_LOS[3]*SKK_LOS[5]*SKK_LOS[14] + P[14][5]*SH_LOS[3]*SKK_LOS[4]*SKK_LOS[14] - P[14][6]*SH_LOS[3]*SKK_LOS[3]*SKK_LOS[14]);
-//             Kfusion[15] = -SK_LOS[0]*(P[15][0]*SKK_LOS[8] + P[15][1]*SKK_LOS[7] - P[15][3]*SKK_LOS[6] + P[15][2]*SKK_LOS[9] - P[15][9]*SH_LOS[2]*SH_LOS[7]*SKK_LOS[14] + P[15][22]*SH_LOS[2]*SH_LOS[7]*SKK_LOS[14] + P[15][4]*SH_LOS[3]*SKK_LOS[5]*SKK_LOS[14] + P[15][5]*SH_LOS[3]*SKK_LOS[4]*SKK_LOS[14] - P[15][6]*SH_LOS[3]*SKK_LOS[3]*SKK_LOS[14]);
-//             Kfusion[16] = -SK_LOS[0]*(P[16][0]*SKK_LOS[8] + P[16][1]*SKK_LOS[7] - P[16][3]*SKK_LOS[6] + P[16][2]*SKK_LOS[9] - P[16][9]*SH_LOS[2]*SH_LOS[7]*SKK_LOS[14] + P[16][22]*SH_LOS[2]*SH_LOS[7]*SKK_LOS[14] + P[16][4]*SH_LOS[3]*SKK_LOS[5]*SKK_LOS[14] + P[16][5]*SH_LOS[3]*SKK_LOS[4]*SKK_LOS[14] - P[16][6]*SH_LOS[3]*SKK_LOS[3]*SKK_LOS[14]);
-//             Kfusion[17] = -SK_LOS[0]*(P[17][0]*SKK_LOS[8] + P[17][1]*SKK_LOS[7] - P[17][3]*SKK_LOS[6] + P[17][2]*SKK_LOS[9] - P[17][9]*SH_LOS[2]*SH_LOS[7]*SKK_LOS[14] + P[17][22]*SH_LOS[2]*SH_LOS[7]*SKK_LOS[14] + P[17][4]*SH_LOS[3]*SKK_LOS[5]*SKK_LOS[14] + P[17][5]*SH_LOS[3]*SKK_LOS[4]*SKK_LOS[14] - P[17][6]*SH_LOS[3]*SKK_LOS[3]*SKK_LOS[14]);
-//             Kfusion[18] = -SK_LOS[0]*(P[18][0]*SKK_LOS[8] + P[18][1]*SKK_LOS[7] - P[18][3]*SKK_LOS[6] + P[18][2]*SKK_LOS[9] - P[18][9]*SH_LOS[2]*SH_LOS[7]*SKK_LOS[14] + P[18][22]*SH_LOS[2]*SH_LOS[7]*SKK_LOS[14] + P[18][4]*SH_LOS[3]*SKK_LOS[5]*SKK_LOS[14] + P[18][5]*SH_LOS[3]*SKK_LOS[4]*SKK_LOS[14] - P[18][6]*SH_LOS[3]*SKK_LOS[3]*SKK_LOS[14]);
-//             Kfusion[19] = -SK_LOS[0]*(P[19][0]*SKK_LOS[8] + P[19][1]*SKK_LOS[7] - P[19][3]*SKK_LOS[6] + P[19][2]*SKK_LOS[9] - P[19][9]*SH_LOS[2]*SH_LOS[7]*SKK_LOS[14] + P[19][22]*SH_LOS[2]*SH_LOS[7]*SKK_LOS[14] + P[19][4]*SH_LOS[3]*SKK_LOS[5]*SKK_LOS[14] + P[19][5]*SH_LOS[3]*SKK_LOS[4]*SKK_LOS[14] - P[19][6]*SH_LOS[3]*SKK_LOS[3]*SKK_LOS[14]);
-//             Kfusion[20] = -SK_LOS[0]*(P[20][0]*SKK_LOS[8] + P[20][1]*SKK_LOS[7] - P[20][3]*SKK_LOS[6] + P[20][2]*SKK_LOS[9] - P[20][9]*SH_LOS[2]*SH_LOS[7]*SKK_LOS[14] + P[20][22]*SH_LOS[2]*SH_LOS[7]*SKK_LOS[14] + P[20][4]*SH_LOS[3]*SKK_LOS[5]*SKK_LOS[14] + P[20][5]*SH_LOS[3]*SKK_LOS[4]*SKK_LOS[14] - P[20][6]*SH_LOS[3]*SKK_LOS[3]*SKK_LOS[14]);
-//             Kfusion[21] = -SK_LOS[0]*(P[21][0]*SKK_LOS[8] + P[21][1]*SKK_LOS[7] - P[21][3]*SKK_LOS[6] + P[21][2]*SKK_LOS[9] - P[21][9]*SH_LOS[2]*SH_LOS[7]*SKK_LOS[14] + P[21][22]*SH_LOS[2]*SH_LOS[7]*SKK_LOS[14] + P[21][4]*SH_LOS[3]*SKK_LOS[5]*SKK_LOS[14] + P[21][5]*SH_LOS[3]*SKK_LOS[4]*SKK_LOS[14] - P[21][6]*SH_LOS[3]*SKK_LOS[3]*SKK_LOS[14]);
-//             Kfusion[22] = -SK_LOS[0]*(P[22][0]*SKK_LOS[8] + P[22][1]*SKK_LOS[7] - P[22][3]*SKK_LOS[6] + P[22][2]*SKK_LOS[9] - P[22][9]*SH_LOS[2]*SH_LOS[7]*SKK_LOS[14] + P[22][22]*SH_LOS[2]*SH_LOS[7]*SKK_LOS[14] + P[22][4]*SH_LOS[3]*SKK_LOS[5]*SKK_LOS[14] + P[22][5]*SH_LOS[3]*SKK_LOS[4]*SKK_LOS[14] - P[22][6]*SH_LOS[3]*SKK_LOS[3]*SKK_LOS[14]);
-//             varInnovOptFlow[0] = 1.0f/SK_LOS[0];
-//             innovOptFlow[0] = losPred[0] - losData[0];
-
-//             // set the observation index to 1 to fuse the y component next time round and reset the commence fusion flag
-//             obsIndex = 1;
-//             fuseOptFlowData = false;
-//         }
-//         else if (obsIndex == 1) // we are now fusing the Y measurement
-//         {
-//             // Calculate observation jacobians
-//             for (uint8_t i = 0; i < n_states; i++) H_LOS[i] = 0;
-//             H_LOS[0] = SH_LOS[1]*SH_LOS[3]*(2*a1*q1 - 2*q0 + 2*a2*q2) + SH_LOS[0]*SH_LOS[3]*(vd*SH_LOS[6] - ve*SH_LOS[11] + vn*SH_LOS[5]);
-//             H_LOS[1] = SH_LOS[1]*SH_LOS[3]*(2*q1 + 2*a1*q0 - 2*a2*q3) - SH_LOS[0]*SH_LOS[3]*(vd*SH_LOS[11] + ve*SH_LOS[6] + vn*SH_LOS[4]);
-//             H_LOS[2] = SH_LOS[1]*SH_LOS[3]*(2*q2 + 2*a2*q0 + 2*a1*q3) - SH_LOS[0]*SH_LOS[3]*(vd*SH_LOS[5] + ve*SH_LOS[4] - vn*SH_LOS[6]);
-//             H_LOS[3] = SH_LOS[0]*SH_LOS[3]*(ve*SH_LOS[5] - vd*SH_LOS[4] + vn*SH_LOS[11]) - SH_LOS[1]*SH_LOS[3]*(2*q3 - 2*a1*q2 + 2*a2*q1);
-//             H_LOS[4] = SH_LOS[0]*SH_LOS[3]*(a2*(2*q0*q2 + 2*q1*q3) + a3*(2*q0*q3 - 2*q1*q2) - sq(q0) - sq(q1) + sq(q2) + sq(q3));
-//             H_LOS[5] = -SH_LOS[0]*SH_LOS[3]*(a3*(sq(q0) - sq(q1) + sq(q2) - sq(q3)) + 2*q0*q3 + 2*q1*q2 + a2*(2*q0*q1 - 2*q2*q3));
-//             H_LOS[6] = SH_LOS[0]*SH_LOS[3]*(a2*(sq(q0) - sq(q1) - sq(q2) + sq(q3)) + 2*q0*q2 - 2*q1*q3 - a3*(2*q0*q1 + 2*q2*q3));
-//             H_LOS[9] = -SH_LOS[0]*SH_LOS[1]*SH_LOS[7];
-//             H_LOS[22] = SH_LOS[0]*SH_LOS[1]*SH_LOS[7];
-
-//             // Calculate Kalman gains
-//             SK_LOS[1] = 1/(R_LOS + SKK_LOS[12]*(P[0][2]*SKK_LOS[10] + P[1][2]*SKK_LOS[11] + P[2][2]*SKK_LOS[12] - P[3][2]*SKK_LOS[13] - P[9][2]*SH_LOS[1]*SH_LOS[7]*SKK_LOS[14] + P[22][2]*SH_LOS[1]*SH_LOS[7]*SKK_LOS[14] + P[4][2]*SH_LOS[3]*SKK_LOS[2]*SKK_LOS[14] - P[5][2]*SH_LOS[3]*SKK_LOS[1]*SKK_LOS[14] + P[6][2]*SH_LOS[3]*SKK_LOS[0]*SKK_LOS[14]) - SKK_LOS[13]*(P[0][3]*SKK_LOS[10] + P[1][3]*SKK_LOS[11] + P[2][3]*SKK_LOS[12] - P[3][3]*SKK_LOS[13] - P[9][3]*SH_LOS[1]*SH_LOS[7]*SKK_LOS[14] + P[22][3]*SH_LOS[1]*SH_LOS[7]*SKK_LOS[14] + P[4][3]*SH_LOS[3]*SKK_LOS[2]*SKK_LOS[14] - P[5][3]*SH_LOS[3]*SKK_LOS[1]*SKK_LOS[14] + P[6][3]*SH_LOS[3]*SKK_LOS[0]*SKK_LOS[14]) + SKK_LOS[10]*(P[0][0]*SKK_LOS[10] + P[1][0]*SKK_LOS[11] + P[2][0]*(SH_LOS[1]*SH_LOS[3]*(2*q2 + 2*a2*q0 + 2*a1*q3) - SH_LOS[3]*SKK_LOS[14]*(vd*SH_LOS[5] + ve*SH_LOS[4] - vn*SH_LOS[6])) - P[3][0]*(SH_LOS[1]*SH_LOS[3]*(2*q3 - 2*a1*q2 + 2*a2*q1) - SH_LOS[3]*SKK_LOS[14]*(ve*SH_LOS[5] - vd*SH_LOS[4] + vn*SH_LOS[11])) - P[9][0]*SH_LOS[1]*SH_LOS[7]*SKK_LOS[14] + P[22][0]*SH_LOS[1]*SH_LOS[7]*SKK_LOS[14] + P[4][0]*SH_LOS[3]*SKK_LOS[2]*SKK_LOS[14] - P[5][0]*SH_LOS[3]*SKK_LOS[1]*SKK_LOS[14] + P[6][0]*SH_LOS[3]*SKK_LOS[0]*SKK_LOS[14]) + SKK_LOS[11]*(P[0][1]*SKK_LOS[10] + P[1][1]*SKK_LOS[11] + P[2][1]*(SH_LOS[1]*SH_LOS[3]*(2*q2 + 2*a2*q0 + 2*a1*q3) - SH_LOS[3]*SKK_LOS[14]*(vd*SH_LOS[5] + ve*SH_LOS[4] - vn*SH_LOS[6])) - P[3][1]*(SH_LOS[1]*SH_LOS[3]*(2*q3 - 2*a1*q2 + 2*a2*q1) - SH_LOS[3]*SKK_LOS[14]*(ve*SH_LOS[5] - vd*SH_LOS[4] + vn*SH_LOS[11])) - P[9][1]*SH_LOS[1]*SH_LOS[7]*SKK_LOS[14] + P[22][1]*SH_LOS[1]*SH_LOS[7]*SKK_LOS[14] + P[4][1]*SH_LOS[3]*SKK_LOS[2]*SKK_LOS[14] - P[5][1]*SH_LOS[3]*SKK_LOS[1]*SKK_LOS[14] + P[6][1]*SH_LOS[3]*SKK_LOS[0]*SKK_LOS[14]) - SH_LOS[1]*SH_LOS[7]*SKK_LOS[14]*(P[0][9]*SKK_LOS[10] + P[1][9]*SKK_LOS[11] + P[2][9]*SKK_LOS[12] - P[3][9]*SKK_LOS[13] - P[9][9]*SH_LOS[1]*SH_LOS[7]*SKK_LOS[14] + P[22][9]*SH_LOS[1]*SH_LOS[7]*SKK_LOS[14] + P[4][9]*SH_LOS[3]*SKK_LOS[2]*SKK_LOS[14] - P[5][9]*SH_LOS[3]*SKK_LOS[1]*SKK_LOS[14] + P[6][9]*SH_LOS[3]*SKK_LOS[0]*SKK_LOS[14]) + SH_LOS[1]*SH_LOS[7]*SKK_LOS[14]*(P[0][22]*SKK_LOS[10] + P[1][22]*SKK_LOS[11] + P[2][22]*SKK_LOS[12] - P[3][22]*SKK_LOS[13] - P[9][22]*SH_LOS[1]*SH_LOS[7]*SKK_LOS[14] + P[22][22]*SH_LOS[1]*SH_LOS[7]*SKK_LOS[14] + P[4][22]*SH_LOS[3]*SKK_LOS[2]*SKK_LOS[14] - P[5][22]*SH_LOS[3]*SKK_LOS[1]*SKK_LOS[14] + P[6][22]*SH_LOS[3]*SKK_LOS[0]*SKK_LOS[14]) + SH_LOS[3]*SKK_LOS[2]*SKK_LOS[14]*(P[0][4]*SKK_LOS[10] + P[1][4]*SKK_LOS[11] + P[2][4]*SKK_LOS[12] - P[3][4]*SKK_LOS[13] - P[9][4]*SH_LOS[1]*SH_LOS[7]*SKK_LOS[14] + P[22][4]*SH_LOS[1]*SH_LOS[7]*SKK_LOS[14] + P[4][4]*SH_LOS[3]*SKK_LOS[2]*SKK_LOS[14] - P[5][4]*SH_LOS[3]*SKK_LOS[1]*SKK_LOS[14] + P[6][4]*SH_LOS[3]*SKK_LOS[0]*SKK_LOS[14]) - SH_LOS[3]*SKK_LOS[1]*SKK_LOS[14]*(P[0][5]*SKK_LOS[10] + P[1][5]*SKK_LOS[11] + P[2][5]*SKK_LOS[12] - P[3][5]*SKK_LOS[13] - P[9][5]*SH_LOS[1]*SH_LOS[7]*SKK_LOS[14] + P[22][5]*SH_LOS[1]*SH_LOS[7]*SKK_LOS[14] + P[4][5]*SH_LOS[3]*SKK_LOS[2]*SKK_LOS[14] - P[5][5]*SH_LOS[3]*SKK_LOS[1]*SKK_LOS[14] + P[6][5]*SH_LOS[3]*SKK_LOS[0]*SKK_LOS[14]) + SH_LOS[3]*SKK_LOS[0]*SKK_LOS[14]*(P[0][6]*SKK_LOS[10] + P[1][6]*SKK_LOS[11] + P[2][6]*SKK_LOS[12] - P[3][6]*SKK_LOS[13] - P[9][6]*SH_LOS[1]*SH_LOS[7]*SKK_LOS[14] + P[22][6]*SH_LOS[1]*SH_LOS[7]*SKK_LOS[14] + P[4][6]*SH_LOS[3]*SKK_LOS[2]*SKK_LOS[14] - P[5][6]*SH_LOS[3]*SKK_LOS[1]*SKK_LOS[14] + P[6][6]*SH_LOS[3]*SKK_LOS[0]*SKK_LOS[14]));
-//             Kfusion[0] = SK_LOS[1]*(P[0][0]*SKK_LOS[10] - P[0][3]*SKK_LOS[13] + P[0][1]*SKK_LOS[11] + P[0][2]*SKK_LOS[12] - P[0][9]*SH_LOS[1]*SH_LOS[7]*SKK_LOS[14] + P[0][22]*SH_LOS[1]*SH_LOS[7]*SKK_LOS[14] + P[0][6]*SH_LOS[3]*SKK_LOS[0]*SKK_LOS[14] + P[0][4]*SH_LOS[3]*SKK_LOS[2]*SKK_LOS[14] - P[0][5]*SH_LOS[3]*SKK_LOS[1]*SKK_LOS[14]);
-//             Kfusion[1] = SK_LOS[1]*(P[1][0]*SKK_LOS[10] - P[1][3]*SKK_LOS[13] + P[1][1]*SKK_LOS[11] + P[1][2]*SKK_LOS[12] - P[1][9]*SH_LOS[1]*SH_LOS[7]*SKK_LOS[14] + P[1][22]*SH_LOS[1]*SH_LOS[7]*SKK_LOS[14] + P[1][6]*SH_LOS[3]*SKK_LOS[0]*SKK_LOS[14] + P[1][4]*SH_LOS[3]*SKK_LOS[2]*SKK_LOS[14] - P[1][5]*SH_LOS[3]*SKK_LOS[1]*SKK_LOS[14]);
-//             Kfusion[2] = SK_LOS[1]*(P[2][0]*SKK_LOS[10] - P[2][3]*SKK_LOS[13] + P[2][1]*SKK_LOS[11] + P[2][2]*SKK_LOS[12] - P[2][9]*SH_LOS[1]*SH_LOS[7]*SKK_LOS[14] + P[2][22]*SH_LOS[1]*SH_LOS[7]*SKK_LOS[14] + P[2][6]*SH_LOS[3]*SKK_LOS[0]*SKK_LOS[14] + P[2][4]*SH_LOS[3]*SKK_LOS[2]*SKK_LOS[14] - P[2][5]*SH_LOS[3]*SKK_LOS[1]*SKK_LOS[14]);
-//             Kfusion[3] = SK_LOS[1]*(P[3][0]*SKK_LOS[10] - P[3][3]*SKK_LOS[13] + P[3][1]*SKK_LOS[11] + P[3][2]*SKK_LOS[12] - P[3][9]*SH_LOS[1]*SH_LOS[7]*SKK_LOS[14] + P[3][22]*SH_LOS[1]*SH_LOS[7]*SKK_LOS[14] + P[3][6]*SH_LOS[3]*SKK_LOS[0]*SKK_LOS[14] + P[3][4]*SH_LOS[3]*SKK_LOS[2]*SKK_LOS[14] - P[3][5]*SH_LOS[3]*SKK_LOS[1]*SKK_LOS[14]);
-//             Kfusion[4] = SK_LOS[1]*(P[4][0]*SKK_LOS[10] - P[4][3]*SKK_LOS[13] + P[4][1]*SKK_LOS[11] + P[4][2]*SKK_LOS[12] - P[4][9]*SH_LOS[1]*SH_LOS[7]*SKK_LOS[14] + P[4][22]*SH_LOS[1]*SH_LOS[7]*SKK_LOS[14] + P[4][6]*SH_LOS[3]*SKK_LOS[0]*SKK_LOS[14] + P[4][4]*SH_LOS[3]*SKK_LOS[2]*SKK_LOS[14] - P[4][5]*SH_LOS[3]*SKK_LOS[1]*SKK_LOS[14]);
-//             Kfusion[5] = SK_LOS[1]*(P[5][0]*SKK_LOS[10] - P[5][3]*SKK_LOS[13] + P[5][1]*SKK_LOS[11] + P[5][2]*SKK_LOS[12] - P[5][9]*SH_LOS[1]*SH_LOS[7]*SKK_LOS[14] + P[5][22]*SH_LOS[1]*SH_LOS[7]*SKK_LOS[14] + P[5][6]*SH_LOS[3]*SKK_LOS[0]*SKK_LOS[14] + P[5][4]*SH_LOS[3]*SKK_LOS[2]*SKK_LOS[14] - P[5][5]*SH_LOS[3]*SKK_LOS[1]*SKK_LOS[14]);
-//             Kfusion[6] = SK_LOS[1]*(P[6][0]*SKK_LOS[10] - P[6][3]*SKK_LOS[13] + P[6][1]*SKK_LOS[11] + P[6][2]*SKK_LOS[12] - P[6][9]*SH_LOS[1]*SH_LOS[7]*SKK_LOS[14] + P[6][22]*SH_LOS[1]*SH_LOS[7]*SKK_LOS[14] + P[6][6]*SH_LOS[3]*SKK_LOS[0]*SKK_LOS[14] + P[6][4]*SH_LOS[3]*SKK_LOS[2]*SKK_LOS[14] - P[6][5]*SH_LOS[3]*SKK_LOS[1]*SKK_LOS[14]);
-//             Kfusion[7] = SK_LOS[1]*(P[7][0]*SKK_LOS[10] - P[7][3]*SKK_LOS[13] + P[7][1]*SKK_LOS[11] + P[7][2]*SKK_LOS[12] - P[7][9]*SH_LOS[1]*SH_LOS[7]*SKK_LOS[14] + P[7][22]*SH_LOS[1]*SH_LOS[7]*SKK_LOS[14] + P[7][6]*SH_LOS[3]*SKK_LOS[0]*SKK_LOS[14] + P[7][4]*SH_LOS[3]*SKK_LOS[2]*SKK_LOS[14] - P[7][5]*SH_LOS[3]*SKK_LOS[1]*SKK_LOS[14]);
-//             Kfusion[8] = SK_LOS[1]*(P[8][0]*SKK_LOS[10] - P[8][3]*SKK_LOS[13] + P[8][1]*SKK_LOS[11] + P[8][2]*SKK_LOS[12] - P[8][9]*SH_LOS[1]*SH_LOS[7]*SKK_LOS[14] + P[8][22]*SH_LOS[1]*SH_LOS[7]*SKK_LOS[14] + P[8][6]*SH_LOS[3]*SKK_LOS[0]*SKK_LOS[14] + P[8][4]*SH_LOS[3]*SKK_LOS[2]*SKK_LOS[14] - P[8][5]*SH_LOS[3]*SKK_LOS[1]*SKK_LOS[14]);
-//             Kfusion[9] = SK_LOS[1]*(P[9][0]*SKK_LOS[10] - P[9][3]*SKK_LOS[13] + P[9][1]*SKK_LOS[11] + P[9][2]*SKK_LOS[12] - P[9][9]*SH_LOS[1]*SH_LOS[7]*SKK_LOS[14] + P[9][22]*SH_LOS[1]*SH_LOS[7]*SKK_LOS[14] + P[9][6]*SH_LOS[3]*SKK_LOS[0]*SKK_LOS[14] + P[9][4]*SH_LOS[3]*SKK_LOS[2]*SKK_LOS[14] - P[9][5]*SH_LOS[3]*SKK_LOS[1]*SKK_LOS[14]);
-//             Kfusion[10] = SK_LOS[1]*(P[10][0]*SKK_LOS[10] - P[10][3]*SKK_LOS[13] + P[10][1]*SKK_LOS[11] + P[10][2]*SKK_LOS[12] - P[10][9]*SH_LOS[1]*SH_LOS[7]*SKK_LOS[14] + P[10][22]*SH_LOS[1]*SH_LOS[7]*SKK_LOS[14] + P[10][6]*SH_LOS[3]*SKK_LOS[0]*SKK_LOS[14] + P[10][4]*SH_LOS[3]*SKK_LOS[2]*SKK_LOS[14] - P[10][5]*SH_LOS[3]*SKK_LOS[1]*SKK_LOS[14]);
-//             Kfusion[11] = SK_LOS[1]*(P[11][0]*SKK_LOS[10] - P[11][3]*SKK_LOS[13] + P[11][1]*SKK_LOS[11] + P[11][2]*SKK_LOS[12] - P[11][9]*SH_LOS[1]*SH_LOS[7]*SKK_LOS[14] + P[11][22]*SH_LOS[1]*SH_LOS[7]*SKK_LOS[14] + P[11][6]*SH_LOS[3]*SKK_LOS[0]*SKK_LOS[14] + P[11][4]*SH_LOS[3]*SKK_LOS[2]*SKK_LOS[14] - P[11][5]*SH_LOS[3]*SKK_LOS[1]*SKK_LOS[14]);
-//             Kfusion[12] = SK_LOS[1]*(P[12][0]*SKK_LOS[10] - P[12][3]*SKK_LOS[13] + P[12][1]*SKK_LOS[11] + P[12][2]*SKK_LOS[12] - P[12][9]*SH_LOS[1]*SH_LOS[7]*SKK_LOS[14] + P[12][22]*SH_LOS[1]*SH_LOS[7]*SKK_LOS[14] + P[12][6]*SH_LOS[3]*SKK_LOS[0]*SKK_LOS[14] + P[12][4]*SH_LOS[3]*SKK_LOS[2]*SKK_LOS[14] - P[12][5]*SH_LOS[3]*SKK_LOS[1]*SKK_LOS[14]);
-//             Kfusion[13] = 0.0f;//SK_LOS[1]*(P[13][0]*SKK_LOS[10] - P[13][3]*SKK_LOS[13] + P[13][1]*SKK_LOS[11] + P[13][2]*SKK_LOS[12] - P[13][9]*SH_LOS[1]*SH_LOS[7]*SKK_LOS[14] + P[13][22]*SH_LOS[1]*SH_LOS[7]*SKK_LOS[14] + P[13][6]*SH_LOS[3]*SKK_LOS[0]*SKK_LOS[14] + P[13][4]*SH_LOS[3]*SKK_LOS[2]*SKK_LOS[14] - P[13][5]*SH_LOS[3]*SKK_LOS[1]*SKK_LOS[14]);
-//             Kfusion[14] = SK_LOS[1]*(P[14][0]*SKK_LOS[10] - P[14][3]*SKK_LOS[13] + P[14][1]*SKK_LOS[11] + P[14][2]*SKK_LOS[12] - P[14][9]*SH_LOS[1]*SH_LOS[7]*SKK_LOS[14] + P[14][22]*SH_LOS[1]*SH_LOS[7]*SKK_LOS[14] + P[14][6]*SH_LOS[3]*SKK_LOS[0]*SKK_LOS[14] + P[14][4]*SH_LOS[3]*SKK_LOS[2]*SKK_LOS[14] - P[14][5]*SH_LOS[3]*SKK_LOS[1]*SKK_LOS[14]);
-//             Kfusion[15] = SK_LOS[1]*(P[15][0]*SKK_LOS[10] - P[15][3]*SKK_LOS[13] + P[15][1]*SKK_LOS[11] + P[15][2]*SKK_LOS[12] - P[15][9]*SH_LOS[1]*SH_LOS[7]*SKK_LOS[14] + P[15][22]*SH_LOS[1]*SH_LOS[7]*SKK_LOS[14] + P[15][6]*SH_LOS[3]*SKK_LOS[0]*SKK_LOS[14] + P[15][4]*SH_LOS[3]*SKK_LOS[2]*SKK_LOS[14] - P[15][5]*SH_LOS[3]*SKK_LOS[1]*SKK_LOS[14]);
-//             Kfusion[16] = SK_LOS[1]*(P[16][0]*SKK_LOS[10] - P[16][3]*SKK_LOS[13] + P[16][1]*SKK_LOS[11] + P[16][2]*SKK_LOS[12] - P[16][9]*SH_LOS[1]*SH_LOS[7]*SKK_LOS[14] + P[16][22]*SH_LOS[1]*SH_LOS[7]*SKK_LOS[14] + P[16][6]*SH_LOS[3]*SKK_LOS[0]*SKK_LOS[14] + P[16][4]*SH_LOS[3]*SKK_LOS[2]*SKK_LOS[14] - P[16][5]*SH_LOS[3]*SKK_LOS[1]*SKK_LOS[14]);
-//             Kfusion[17] = SK_LOS[1]*(P[17][0]*SKK_LOS[10] - P[17][3]*SKK_LOS[13] + P[17][1]*SKK_LOS[11] + P[17][2]*SKK_LOS[12] - P[17][9]*SH_LOS[1]*SH_LOS[7]*SKK_LOS[14] + P[17][22]*SH_LOS[1]*SH_LOS[7]*SKK_LOS[14] + P[17][6]*SH_LOS[3]*SKK_LOS[0]*SKK_LOS[14] + P[17][4]*SH_LOS[3]*SKK_LOS[2]*SKK_LOS[14] - P[17][5]*SH_LOS[3]*SKK_LOS[1]*SKK_LOS[14]);
-//             Kfusion[18] = SK_LOS[1]*(P[18][0]*SKK_LOS[10] - P[18][3]*SKK_LOS[13] + P[18][1]*SKK_LOS[11] + P[18][2]*SKK_LOS[12] - P[18][9]*SH_LOS[1]*SH_LOS[7]*SKK_LOS[14] + P[18][22]*SH_LOS[1]*SH_LOS[7]*SKK_LOS[14] + P[18][6]*SH_LOS[3]*SKK_LOS[0]*SKK_LOS[14] + P[18][4]*SH_LOS[3]*SKK_LOS[2]*SKK_LOS[14] - P[18][5]*SH_LOS[3]*SKK_LOS[1]*SKK_LOS[14]);
-//             Kfusion[19] = SK_LOS[1]*(P[19][0]*SKK_LOS[10] - P[19][3]*SKK_LOS[13] + P[19][1]*SKK_LOS[11] + P[19][2]*SKK_LOS[12] - P[19][9]*SH_LOS[1]*SH_LOS[7]*SKK_LOS[14] + P[19][22]*SH_LOS[1]*SH_LOS[7]*SKK_LOS[14] + P[19][6]*SH_LOS[3]*SKK_LOS[0]*SKK_LOS[14] + P[19][4]*SH_LOS[3]*SKK_LOS[2]*SKK_LOS[14] - P[19][5]*SH_LOS[3]*SKK_LOS[1]*SKK_LOS[14]);
-//             Kfusion[20] = SK_LOS[1]*(P[20][0]*SKK_LOS[10] - P[20][3]*SKK_LOS[13] + P[20][1]*SKK_LOS[11] + P[20][2]*SKK_LOS[12] - P[20][9]*SH_LOS[1]*SH_LOS[7]*SKK_LOS[14] + P[20][22]*SH_LOS[1]*SH_LOS[7]*SKK_LOS[14] + P[20][6]*SH_LOS[3]*SKK_LOS[0]*SKK_LOS[14] + P[20][4]*SH_LOS[3]*SKK_LOS[2]*SKK_LOS[14] - P[20][5]*SH_LOS[3]*SKK_LOS[1]*SKK_LOS[14]);
-//             Kfusion[21] = SK_LOS[1]*(P[21][0]*SKK_LOS[10] - P[21][3]*SKK_LOS[13] + P[21][1]*SKK_LOS[11] + P[21][2]*SKK_LOS[12] - P[21][9]*SH_LOS[1]*SH_LOS[7]*SKK_LOS[14] + P[21][22]*SH_LOS[1]*SH_LOS[7]*SKK_LOS[14] + P[21][6]*SH_LOS[3]*SKK_LOS[0]*SKK_LOS[14] + P[21][4]*SH_LOS[3]*SKK_LOS[2]*SKK_LOS[14] - P[21][5]*SH_LOS[3]*SKK_LOS[1]*SKK_LOS[14]);
-//             Kfusion[22] = SK_LOS[1]*(P[22][0]*SKK_LOS[10] - P[22][3]*SKK_LOS[13] + P[22][1]*SKK_LOS[11] + P[22][2]*SKK_LOS[12] - P[22][9]*SH_LOS[1]*SH_LOS[7]*SKK_LOS[14] + P[22][22]*SH_LOS[1]*SH_LOS[7]*SKK_LOS[14] + P[22][6]*SH_LOS[3]*SKK_LOS[0]*SKK_LOS[14] + P[22][4]*SH_LOS[3]*SKK_LOS[2]*SKK_LOS[14] - P[22][5]*SH_LOS[3]*SKK_LOS[1]*SKK_LOS[14]);
-//             varInnovOptFlow[1] = 1.0f/SK_LOS[1];
-//             innovOptFlow[1] = losPred[1] - losData[1];
-
-//             // reset the observation index
-//             obsIndex = 0;
-//             fuseOptFlowData = false;
-//         }
-
-//         // Check the innovation for consistency and don't fuse if > 3Sigma
-//         if ((innovOptFlow[obsIndex]*innovOptFlow[obsIndex]/varInnovOptFlow[obsIndex]) < 9.0f)
-//         {
-//             // correct the state vector
-//             for (uint8_t j = 0; j < n_states; j++)
-//             {
-//                 states[j] = states[j] - Kfusion[j] * innovOptFlow[obsIndex];
-//             }
-//             // normalise the quaternion states
-//             float quatMag = sqrt(states[0]*states[0] + states[1]*states[1] + states[2]*states[2] + states[3]*states[3]);
-//             if (quatMag > 1e-12f)
-//             {
-//                 for (uint8_t j= 0; j<=3; j++)
-//                 {
-//                     float quatMagInv = 1.0f/quatMag;
-//                     states[j] = states[j] * quatMagInv;
-//                 }
-//             }
-//             // correct the covariance P = (I - K*H)*P
-//             // take advantage of the empty columns in KH to reduce the
-//             // number of operations
-//             for (uint8_t i = 0; i < n_states; i++)
-//             {
-//                 for (uint8_t j = 0; j <= 6; j++)
-//                 {
-//                     KH[i][j] = Kfusion[i] * H_LOS[j];
-//                 }
-//                 for (uint8_t j = 7; j <= 8; j++)
-//                 {
-//                     KH[i][j] = 0.0f;
-//                 }
-//                 KH[i][9] = Kfusion[i] * H_LOS[9];
-//                 for (uint8_t j = 10; j <= 21; j++)
-//                 {
-//                     KH[i][j] = 0.0f;
-//                 }
-//                 KH[i][22] = Kfusion[i] * H_LOS[22];
-//             }
-//             for (uint8_t i = 0; i < n_states; i++)
-//             {
-//                 for (uint8_t j = 0; j < n_states; j++)
-//                 {
-//                     KHP[i][j] = 0.0f;
-//                     for (uint8_t k = 0; k <= 6; k++)
-//                     {
-//                         KHP[i][j] = KHP[i][j] + KH[i][k] * P[k][j];
-//                     }
-//                     KHP[i][j] = KHP[i][j] + KH[i][9] * P[9][j];
-//                     KHP[i][j] = KHP[i][j] + KH[i][22] * P[2][j];
-//                 }
-//             }
-//         }
-//         for (uint8_t i = 0; i <  n_states; i++)
-//         {
-//             for (uint8_t j = 0; j <  n_states; j++)
-//             {
-//                 P[i][j] = P[i][j] - KHP[i][j];
-//             }
-//         }
-//         ForceSymmetry();
-//         ConstrainVariances();
-//     }
 }
 
 /*
@@ -2192,27 +2019,29 @@ Estimation of optical flow sensor focal length scale factor and terrain height u
 This fiter requires optical flow rates that are not motion compensated
 Range to ground measurement is assumed to be via a narrow beam type sensor - eg laser
 */
-void AttPosEKF::GroundEKF()
+void AttPosEKF::OpticalFlowEKF()
 {
     // propagate ground position state noise each time this is called using the difference in position since the last observations and an RMS gradient assumption
     // limit distance to prevent intialisation afer bad gps causing bad numerical conditioning
     if (!inhibitGndState) {
         float distanceTravelledSq;
-        distanceTravelledSq = sq(statesAtRngTime[7] - prevPosN) + sq(statesAtRngTime[8] - prevPosE);
-        prevPosN = statesAtRngTime[7];
-        prevPosE = statesAtRngTime[8];
+        if (fuseRngData) {
+            distanceTravelledSq = sq(statesAtRngTime[7] - prevPosN) + sq(statesAtRngTime[8] - prevPosE);
+            prevPosN = statesAtRngTime[7];
+            prevPosE = statesAtRngTime[8];
+        } else if (fuseOptFlowData) {
+            distanceTravelledSq = sq(statesAtFlowTime[7] - prevPosN) + sq(statesAtFlowTime[8] - prevPosE);
+            prevPosN = statesAtFlowTime[7];
+            prevPosE = statesAtFlowTime[8];
+        } else {
+            return;
+        }
         distanceTravelledSq = min(distanceTravelledSq, 100.0f);
         Popt[1][1] += (distanceTravelledSq * sq(gndHgtSigma));
     }
-    // we aren't using optical flow measurements in this hacked implementation so set the covariances for this state to zero to avoid numerical problems
-    Popt[0][0] = 0.0f;
-    Popt[0][1] = 0.0f;
-    Popt[1][0] = 0.0f;
 
-    // Fuse range finder data
-    // Need to check that our range finder tilt angle is less than 30 degrees
-    float cosRngTilt = - Tbn.z.x * sinf(rngFinderPitch) + Tbn.z.z * cosf(rngFinderPitch);
-    if (useRangeFinder && fuseRngData && cosRngTilt > 0.87f) {
+    // fuse range finder data
+    if (fuseRngData) {
         float range; // range from camera to centre of image
         float q0; // quaternion at optical flow measurement time
         float q1; // quaternion at optical flow measurement time
@@ -2266,10 +2095,7 @@ void AttPosEKF::GroundEKF()
                 flowStates[i] -= K_RNG[i] * innovRng;
             }
             // constrain the states
-
-            // constrain focal length to 0.1 to 10 mm
             flowStates[0] = ConstrainFloat(flowStates[0], 0.1f, 10.0f);
-            // constrain altitude
             flowStates[1] = maxf(flowStates[1], statesAtRngTime[9] + minFlowRng);
 
             // correct the covariance matrix
@@ -2285,6 +2111,132 @@ void AttPosEKF::GroundEKF()
             Popt[1][0] = Popt[0][1];
         }
     }
+
+    if (fuseOptFlowData) {
+        Vector3f vel; // velocity of sensor relative to ground in NED axes
+        Vector3f relVelSensor; // velocity of sensor relative to ground in sensor axes
+        float losPred[2]; // predicted optical flow angular rate measurements
+        float range; // range from camera to centre of image
+        float q0; // quaternion at optical flow measurement time
+        float q1; // quaternion at optical flow measurement time
+        float q2; // quaternion at optical flow measurement time
+        float q3; // quaternion at optical flow measurement time
+        float HP[2];
+        float SH_OPT[6];
+        float SK_OPT[3];
+        float K_OPT[2][2];
+        float H_OPT[2][2];
+        float nextPopt[2][2];
+
+        // propagate scale factor state noise
+        if (!inhibitScaleState) {
+            Popt[0][0] += 1e-8f;
+        } else {
+            Popt[0][0] = 0.0f;
+        }
+
+        // Copy required states to local variable names
+        q0             = statesAtFlowTime[0];
+        q1             = statesAtFlowTime[1];
+        q2             = statesAtFlowTime[2];
+        q3             = statesAtFlowTime[3];
+        vel.x          = statesAtFlowTime[4];
+        vel.y          = statesAtFlowTime[5];
+        vel.z          = statesAtFlowTime[6];
+
+        // constrain terrain height to be below the vehicle
+        flowStates[1] = maxf(flowStates[1], statesAtFlowTime[9] + minFlowRng);
+
+        // estimate range to centre of image
+        range = (flowStates[1] - statesAtFlowTime[9]) / Tnb_flow.z.z;
+
+        // calculate relative velocity in sensor frame
+        relVelSensor = Tnb_flow * vel;
+
+        // divide velocity by range, subtract body rates and apply scale factor to
+        // get predicted sensed angular optical rates relative to X and Y sensor axes
+        losPred[0] =  flowStates[0]*( relVelSensor.y / range) - omegaAcrossFlowTime[0];
+        losPred[1] =  flowStates[0]*(-relVelSensor.x / range) - omegaAcrossFlowTime[1];
+
+        // calculate innovations
+        auxFlowObsInnov[0] = losPred[0] - flowRadXY[0];
+        auxFlowObsInnov[1] = losPred[1] - flowRadXY[1];
+
+        // calculate Kalman gains
+        SH_OPT[0] = sq(q0) - sq(q1) - sq(q2) + sq(q3);
+        SH_OPT[1] = vel.x*(sq(q0) + sq(q1) - sq(q2) - sq(q3)) + vel.y*(2*q0*q3 + 2*q1*q2) - vel.z*(2*q0*q2 - 2*q1*q3);
+        SH_OPT[2] = vel.y*(sq(q0) - sq(q1) + sq(q2) - sq(q3)) - vel.x*(2*q0*q3 - 2*q1*q2) + vel.z*(2*q0*q1 + 2*q2*q3);
+        SH_OPT[3] = statesAtFlowTime[9] - flowStates[1];
+        SH_OPT[4] = 1.0f/sq(SH_OPT[3]);
+        SH_OPT[5] = 1.0f/SH_OPT[3];
+        float SH015 = SH_OPT[0]*SH_OPT[1]*SH_OPT[5];
+        float SH025 = SH_OPT[0]*SH_OPT[2]*SH_OPT[5];
+        float SH014 = SH_OPT[0]*SH_OPT[1]*SH_OPT[4];
+        float SH024 = SH_OPT[0]*SH_OPT[2]*SH_OPT[4];
+        SK_OPT[0] = 1.0f/(R_LOS + SH015*(Popt[0][0]*SH015 + Popt[1][0]*flowStates[0]*SH014) + flowStates[0]*SH014*(Popt[0][1]*SH015 + Popt[1][1]*flowStates[0]*SH014));
+        SK_OPT[1] = 1.0f/(R_LOS + SH025*(Popt[0][0]*SH025 + Popt[1][0]*flowStates[0]*SH024) + flowStates[0]*SH024*(Popt[0][1]*SH025 + Popt[1][1]*flowStates[0]*SH024));
+        SK_OPT[2] = SH_OPT[0];
+        if (!inhibitScaleState) {
+            K_OPT[0][0] = -SK_OPT[1]*(Popt[0][0]*SH_OPT[2]*SH_OPT[5]*SK_OPT[2] + Popt[0][1]*flowStates[0]*SH_OPT[2]*SH_OPT[4]*SK_OPT[2]);
+            K_OPT[0][1] =  SK_OPT[0]*(Popt[0][0]*SH_OPT[1]*SH_OPT[5]*SK_OPT[2] + Popt[0][1]*flowStates[0]*SH_OPT[1]*SH_OPT[4]*SK_OPT[2]);
+        } else {
+            K_OPT[0][0] = 0.0f;
+            K_OPT[0][1] = 0.0f;
+        }
+        if (!inhibitGndState) {
+            K_OPT[1][0] = -SK_OPT[1]*(Popt[1][0]*SH_OPT[2]*SH_OPT[5]*SK_OPT[2] + Popt[1][1]*flowStates[0]*SH_OPT[2]*SH_OPT[4]*SK_OPT[2]);
+            K_OPT[1][1] =  SK_OPT[0]*(Popt[1][0]*SH_OPT[1]*SH_OPT[5]*SK_OPT[2] + Popt[1][1]*flowStates[0]*SH_OPT[1]*SH_OPT[4]*SK_OPT[2]);
+        } else {
+            K_OPT[1][0] = 0.0f;
+            K_OPT[1][1] = 0.0f;
+        }
+
+        // calculate innovation variances
+        auxFlowObsInnovVar[0] = 1.0f/SK_OPT[1];
+        auxFlowObsInnovVar[1] = 1.0f/SK_OPT[0];
+
+        // calculate observations jacobians
+        H_OPT[0][0] = -SH025;
+        H_OPT[0][1] = -flowStates[0]*SH024;
+        H_OPT[1][0] = SH015;
+        H_OPT[1][1] = flowStates[0]*SH014;
+
+        // Check the innovation for consistency and don't fuse if > threshold
+        for (uint8_t obsIndex = 0; obsIndex < 2; obsIndex++) {
+
+            // calculate the innovation consistency test ratio
+            auxFlowTestRatio[obsIndex] = sq(auxFlowObsInnov[obsIndex]) / (sq(auxFlowInnovGate) * auxFlowObsInnovVar[obsIndex]);
+            if (auxFlowTestRatio[obsIndex] < 1.0f) {
+                // correct the state
+                for (uint8_t i = 0; i < 2 ; i++) {
+                    flowStates[i] -= K_OPT[i][obsIndex] * auxFlowObsInnov[obsIndex];
+                }
+                // constrain the states
+                flowStates[0] = ConstrainFloat(flowStates[0], 0.1f, 10.0f);
+                flowStates[1] = maxf(flowStates[1], statesAtFlowTime[9] + minFlowRng);
+
+                // correct the covariance matrix
+                for (uint8_t i = 0; i < 2 ; i++) {
+                    HP[i] = 0.0f;
+                    for (uint8_t j = 0; j < 2 ; j++) {
+                        HP[i] += H_OPT[obsIndex][j] * P[j][i];
+                    }
+                }
+                for (uint8_t i = 0; i < 2 ; i++) {
+                    for (uint8_t j = 0; j < 2 ; j++) {
+                        nextPopt[i][j] = P[i][j] - K_OPT[i][obsIndex] * HP[j];
+                    }
+                }
+
+                // prevent the state variances from becoming negative and maintain symmetry
+                Popt[0][0] = maxf(nextPopt[0][0],0.0f);
+                Popt[1][1] = maxf(nextPopt[1][1],0.0f);
+                Popt[0][1] = 0.5f * (nextPopt[0][1] + nextPopt[1][0]);
+                Popt[1][0] = Popt[0][1];
+            }
+        }
+    }
+
 }
 
 void AttPosEKF::zeroCols(float (&covMat)[n_states][n_states], uint8_t first, uint8_t last)
@@ -2328,6 +2280,9 @@ void AttPosEKF::StoreStates(uint64_t timestamp_ms)
 {
     for (unsigned i=0; i<n_states; i++)
         storedStates[i][storeIndex] = states[i];
+    storedOmega[0][storeIndex] = angRate.x;
+    storedOmega[1][storeIndex] = angRate.y;
+    storedOmega[2][storeIndex] = angRate.z;
     statetimeStamp[storeIndex] = timestamp_ms;
     storeIndex++;
     if (storeIndex == data_buffer_size)
@@ -2338,16 +2293,12 @@ void AttPosEKF::ResetStoredStates()
 {
     // reset all stored states
     memset(&storedStates[0][0], 0, sizeof(storedStates));
+    memset(&storedOmega[0][0], 0, sizeof(storedOmega));
     memset(&statetimeStamp[0], 0, sizeof(statetimeStamp));
 
     // reset store index to first
     storeIndex = 0;
 
-    // overwrite all existing states
-    for (unsigned i = 0; i < n_states; i++) {
-        storedStates[i][storeIndex] = states[i];
-    }
-
     statetimeStamp[storeIndex] = millis();
 
     // increment to next storage index
@@ -2407,6 +2358,37 @@ int AttPosEKF::RecallStates(float* statesForFusion, uint64_t msec)
     return ret;
 }
 
+void AttPosEKF::RecallOmega(float* omegaForFusion, uint64_t msec)
+{
+    // work back in time and calculate average angular rate over the time interval
+    for (unsigned i=0; i < 3; i++) {
+        omegaForFusion[i] = 0.0f;
+    }
+    uint8_t sumIndex = 0;
+    int64_t timeDelta;
+    for (unsigned storeIndexLocal = 0; storeIndexLocal < data_buffer_size; storeIndexLocal++)
+    {
+        // calculate the average of all samples younger than msec
+        timeDelta = statetimeStamp[storeIndexLocal] - msec;
+        if (timeDelta > 0)
+        {
+            for (unsigned i=0; i < 3; i++) {
+                omegaForFusion[i] += storedOmega[i][storeIndexLocal];
+            }
+            sumIndex += 1;
+        }
+    }
+    if (sumIndex >= 1) {
+        for (unsigned i=0; i < 3; i++) {
+            omegaForFusion[i] = omegaForFusion[i] / float(sumIndex);
+        }
+    } else {
+        omegaForFusion[0] = angRate.x;
+        omegaForFusion[1] = angRate.y;
+        omegaForFusion[2] = angRate.z;
+    }
+}
+
 void AttPosEKF::quat2Tnb(Mat3f &Tnb, const float (&quat)[4])
 {
     // Calculate the nav to body cosine matrix
@@ -2502,6 +2484,7 @@ void AttPosEKF::calcLLH(float posNED[3], double &lat, double &lon, float &hgt, d
 void AttPosEKF::OnGroundCheck()
 {
     onGround = (((sq(velNED[0]) + sq(velNED[1]) + sq(velNED[2])) < 4.0f) && (VtasMeas < 8.0f));
+
     if (staticMode) {
         staticMode = (!refSet || (GPSstatus < GPS_FIX_3D));
     }
@@ -2517,12 +2500,25 @@ void AttPosEKF::OnGroundCheck()
     } else {
         inhibitMagStates = false;
     }
-    // don't update terrain offset state if on ground
-    if (onGround) {
+    // don't update terrain offset state if there is no range finder and flying at low velocity or without GPS
+    if ((onGround || !useGPS) && !useRangeFinder) {
         inhibitGndState = true;
     } else {
         inhibitGndState = false;
     }
+    // don't update terrain offset state if there is no range finder and flying at low velocity, or without GPS, as it is poorly observable
+    if ((onGround || (globalTimeStamp_ms - lastFixTime_ms) > 1000) && !useRangeFinder) {
+        inhibitGndState = true;
+    } else {
+        inhibitGndState = false;
+    }
+    // Don't update focal length offset state if there is no range finder or optical flow sensor
+    // we need both sensors to do this estimation
+    if (!useRangeFinder || !useOpticalFlow) {
+        inhibitScaleState = true;
+    } else {
+        inhibitScaleState = false;
+    }
 }
 
 void AttPosEKF::calcEarthRateNED(Vector3f &omega, float latitude)
@@ -2558,7 +2554,9 @@ void AttPosEKF::CovarianceInit()
     P[19][19] = sq(0.02f);
     P[20][20] = P[19][19];
     P[21][21] = P[19][19];
-    P[22][22] = sq(0.5f);
+
+    fScaleFactorVar = 0.001f; // focal length scale factor variance
+    Popt[0][0] = 0.001f;
 }
 
 float AttPosEKF::ConstrainFloat(float val, float min_val, float max_val)
@@ -2596,7 +2594,6 @@ void AttPosEKF::ConstrainVariances()
     // 14-15: Wind Vector  - m/sec (North,East)
     // 16-18: Earth Magnetic Field Vector - gauss (North, East, Down)
     // 19-21: Body Magnetic Field Vector - gauss (X,Y,Z)
-    // 22: Terrain  offset - m
 
     // Constrain quaternion variances
     for (unsigned i = 0; i <= 3; i++) {
@@ -2636,8 +2633,6 @@ void AttPosEKF::ConstrainVariances()
         P[i][i] = ConstrainFloat(P[i][i], 0.0f, 1.0f);
     }
 
-    // Constrain terrain offset variance
-    P[22][22] = ConstrainFloat(P[22][22], 0.0f, 10000.0f);
 }
 
 void AttPosEKF::ConstrainStates()
@@ -2655,7 +2650,6 @@ void AttPosEKF::ConstrainStates()
     // 14-15: Wind Vector  - m/sec (North,East)
     // 16-18: Earth Magnetic Field Vector - gauss (North, East, Down)
     // 19-21: Body Magnetic Field Vector - gauss (X,Y,Z)
-    // 22: Terrain  offset - m
 
     // Constrain quaternion
     for (unsigned i = 0; i <= 3; i++) {
@@ -2673,7 +2667,8 @@ void AttPosEKF::ConstrainStates()
     }
 
     // Constrain altitude
-    states[9] = ConstrainFloat(states[9], -4.0e4f, 1.0e4f);
+    // NOT FOR FLIGHT : Upper value of 0.0 is a temporary fix to get around lack of range finder data during development testing
+    states[9] = ConstrainFloat(states[9], -4.0e4f, 4.0e4f);
 
     // Angle bias limit - set to 8 degrees / sec
     for (unsigned i = 10; i <= 12; i++) {
@@ -2699,9 +2694,6 @@ void AttPosEKF::ConstrainStates()
         states[i] = ConstrainFloat(states[i], -0.5f, 0.5f);
     }
 
-    // Constrain terrain offset
-    states[22] = ConstrainFloat(states[22], -1000.0f, 1000.0f);
-
 }
 
 void AttPosEKF::ForceSymmetry()
@@ -3162,12 +3154,14 @@ void AttPosEKF::InitializeDynamic(float (&initvelNED)[3], float declination)
     states[19] = magBias.x; // Magnetic Field Bias X
     states[20] = magBias.y; // Magnetic Field Bias Y
     states[21] = magBias.z; // Magnetic Field Bias Z
-    states[22] = 0.0f; // terrain height
 
     ResetVelocity();
     ResetPosition();
     ResetHeight();
 
+    // initialise focal length scale factor estimator states
+    flowStates[0] = 1.0f;
+
     statesInitialised = true;
 
     // initialise the covariance matrix
@@ -3223,6 +3217,9 @@ void AttPosEKF::ZeroVariables()
         states[i] = 0.0f; // state matrix
     }
 
+    // initialise the variables for the focal length scale factor to unity
+    flowStates[0] = 1.0f;
+
     correctedDelAng.zero();
     summedDelAng.zero();
     summedDelVel.zero();
@@ -3230,12 +3227,9 @@ void AttPosEKF::ZeroVariables()
     dVelIMU.zero();
     lastGyroOffset.zero();
 
-    windSpdFiltNorth = 0.0f;
-    windSpdFiltEast = 0.0f;
-    // setting the altitude to zero will give us a higher
-    // gain to adjust faster in the first step
-    windSpdFiltAltitude = 0.0f;
-    windSpdFiltClimb = 0.0f;
+    // initialise states used by OpticalFlowEKF
+    flowStates[0] = 1.0f;
+    flowStates[1] = 0.0f;
 
     for (unsigned i = 0; i < data_buffer_size; i++) {
 
diff --git a/src/modules/ekf_att_pos_estimator/estimator_23states.h b/src/modules/ekf_att_pos_estimator/estimator_22states.h
index a607955a8..b1d71bd74 100644
--- a/src/modules/ekf_att_pos_estimator/estimator_23states.h
+++ b/src/modules/ekf_att_pos_estimator/estimator_22states.h
@@ -2,7 +2,7 @@
 
 #include "estimator_utilities.h"
 
-const unsigned int n_states = 23;
+const unsigned int n_states = 22;
 const unsigned int data_buffer_size = 50;
 
 class AttPosEKF {
@@ -29,10 +29,6 @@ public:
     float covDelAngMax; // maximum delta angle between covariance predictions
     float rngFinderPitch; // pitch angle of laser range finder in radians. Zero is aligned with the Z body axis. Positive is RH rotation about Y body axis.
 
-    float a1; // optical flow sensor misalgnment angle about X axis (rad)
-    float a2; // optical flow sensor misalgnment angle about Y axis (rad)
-    float a3; // optical flow sensor misalgnment angle about Z axis (rad)
-
     float yawVarScale;
     float windVelSigma;
     float dAngBiasSigma;
@@ -59,9 +55,6 @@ public:
         covDelAngMax = 0.02f; // maximum delta angle between covariance predictions
         rngFinderPitch = 0.0f; // pitch angle of laser range finder in radians. Zero is aligned with the Z body axis. Positive is RH rotation about Y body axis.
         EAS2TAS = 1.0f;
-        a1 = 0.0f; // optical flow sensor misalgnment angle about X axis (rad)
-        a2 = 0.0f; // optical flow sensor misalgnment angle about Y axis (rad)
-        a3 = 0.0f; // optical flow sensor misalgnment angle about Z axis (rad)
 
         yawVarScale = 1.0f;
         windVelSigma = 0.1f;
@@ -69,7 +62,6 @@ public:
         dVelBiasSigma = 1e-4f;
         magEarthSigma = 3.0e-4f;
         magBodySigma  = 3.0e-4f;
-        gndHgtSigma  = 0.02f; // assume 2% terrain gradient 1-sigma
 
         vneSigma = 0.2f;
         vdSigma = 0.3f;
@@ -82,12 +74,13 @@ public:
         accelProcessNoise = 0.5f;
 
         gndHgtSigma  = 0.1f; // terrain gradient 1-sigma
-        R_LOS = 0.03f; // optical flow measurement noise variance (rad/sec)^2
+        R_LOS = 0.3f; // optical flow measurement noise variance (rad/sec)^2
         flowInnovGate = 3.0f; // number of standard deviations applied to the optical flow innovation consistency check
         auxFlowInnovGate = 10.0f; // number of standard deviations applied to the optical flow innovation consistency check used by the auxiliary filter
-        rngInnovGate = 10.0f; // number of standard deviations applied to the rnage finder innovation consistency check
-        minFlowRng = 0.01f; //minimum range between ground and flow sensor
-        moCompR_LOS = 0.2; // scaler from sensor gyro rate to uncertainty in LOS rate
+        rngInnovGate = 5.0f; // number of standard deviations applied to the range finder innovation consistency check
+        minFlowRng = 0.3f; //minimum range between ground and flow sensor
+        moCompR_LOS = 0.0; // scaler from sensor gyro rate to uncertainty in LOS rate
+
     }
 
     struct mag_state_struct {
@@ -134,6 +127,7 @@ public:
     float statesAtVtasMeasTime[n_states]; // filter states at the effective measurement time
     float statesAtRngTime[n_states]; // filter states at the effective measurement time
     float statesAtFlowTime[n_states]; // States at the effective optical flow measurement time
+    float omegaAcrossFlowTime[3]; // angular rates at the effective optical flow measurement time
 
     Vector3f correctedDelAng; // delta angles about the xyz body axes corrected for errors (rad)
     Vector3f correctedDelVel; // delta velocities along the XYZ body axes corrected for errors (m/s)
@@ -145,7 +139,7 @@ public:
     Vector3f lastGyroOffset;    // Last gyro offset
     Vector3f delAngTotal;
 
-    Mat3f Tbn; // transformation matrix from body to NED coordinates
+    Mat3f Tbn; // transformation matrix from body to NED coordinatesFuseOptFlow
     Mat3f Tnb; // transformation amtrix from NED to body coordinates
 
     Vector3f accel; // acceleration vector in XYZ body axes measured by the IMU (m/s^2)
@@ -157,10 +151,6 @@ public:
     float dtVelPosFilt; // average time between position / velocity fusion steps
     float dtHgtFilt; // average time between height measurement updates
     float dtGpsFilt; // average time between gps measurement updates
-    float windSpdFiltNorth; // average wind speed north component
-    float windSpdFiltEast; // average wind speed east component
-    float windSpdFiltAltitude; // the last altitude used to filter wind speed
-    float windSpdFiltClimb; // filtered climb rate
     uint8_t fusionModeGPS; // 0 = GPS outputs 3D velocity, 1 = GPS outputs 2D velocity, 2 = GPS outputs no velocity
     float innovVelPos[6]; // innovation output
     float varInnovVelPos[6]; // innovation variance output
@@ -175,7 +165,8 @@ public:
     float innovMag[3]; // innovation output
     float varInnovMag[3]; // innovation variance output
     Vector3f magData; // magnetometer flux radings in X,Y,Z body axes
-    float losData[2]; // optical flow LOS rate measurements (rad/sec)
+    float flowRadXYcomp[2]; // motion compensated optical flow angular rates(rad/sec)
+    float flowRadXY[2]; // raw (non motion compensated) optical flow angular rates (rad/sec)
     float innovVtas; // innovation output
     float innovRng; ///< Range finder innovation
     float innovOptFlow[2]; // optical flow LOS innovations (rad/sec)
@@ -190,6 +181,8 @@ public:
     bool refSet;                ///< flag to indicate if the reference position has been set
     Vector3f magBias; // states representing magnetometer bias vector in XYZ body axes
     unsigned covSkipCount; // Number of state prediction frames (IMU daya updates to skip before doing the covariance prediction
+    uint32_t lastFixTime_ms; // Number of msec since last GPS Fix that was used
+    uint32_t globalTimeStamp_ms; // time in msec of current prediction cycle
 
     // GPS input data variables
     double gpsLat;
@@ -217,6 +210,7 @@ public:
 
     bool onGround;    ///< boolean true when the flight vehicle is on the ground (not flying)
     bool staticMode;    ///< boolean true if no position feedback is fused
+    bool useGPS; // boolean true if GPS data is being used
     bool useAirspeed;    ///< boolean true if airspeed data is being used
     bool useCompass;    ///< boolean true if magnetometer data is being used
     bool useRangeFinder;     ///< true when rangefinder is being used
@@ -267,11 +261,9 @@ void FuseMagnetometer();
 
 void FuseAirspeed();
 
-void FuseRangeFinder();
-
 void FuseOptFlow();
 
-void GroundEKF();
+void OpticalFlowEKF();
 
 void zeroRows(float (&covMat)[n_states][n_states], uint8_t first, uint8_t last);
 
@@ -286,7 +278,7 @@ void StoreStates(uint64_t timestamp_ms);
  * Recall the state vector.
  *
  * Recalls the vector stored at closest time to the one specified by msec
- *
+ *FuseOptFlow
  * @return zero on success, integer indicating the number of invalid states on failure.
  *         Does only copy valid states, if the statesForFusion vector was initialized
  *         correctly by the caller, the result can be safely used, but is a mixture
@@ -295,6 +287,8 @@ void StoreStates(uint64_t timestamp_ms);
  */
 int RecallStates(float *statesForFusion, uint64_t msec);
 
+void RecallOmega(float *omegaForFusion, uint64_t msec);
+
 void ResetStoredStates();
 
 void quat2Tbn(Mat3f &TBodyNed, const float (&quat)[4]);
@@ -325,7 +319,7 @@ void CovarianceInit();
 
 void InitialiseFilter(float (&initvelNED)[3], double referenceLat, double referenceLon, float referenceHgt, float declination);
 
-float ConstrainFloat(float val, float min, float max);
+float ConstrainFloat(float val, float min, float maxf);
 
 void ConstrainVariances();
 
diff --git a/src/modules/ekf_att_pos_estimator/module.mk b/src/modules/ekf_att_pos_estimator/module.mk
index 36d854ddd..f51962113 100644
--- a/src/modules/ekf_att_pos_estimator/module.mk
+++ b/src/modules/ekf_att_pos_estimator/module.mk
@@ -39,7 +39,8 @@ MODULE_COMMAND	= ekf_att_pos_estimator
 
 SRCS		= ekf_att_pos_estimator_main.cpp \
 		  ekf_att_pos_estimator_params.c \
-		  estimator_23states.cpp \
+		  estimator_22states.cpp \
 		  estimator_utilities.cpp
 
-EXTRACXXFLAGS	= -Weffc++
+EXTRACXXFLAGS	= -Weffc++ -Wframe-larger-than=6100
+
diff --git a/src/modules/fixedwing_backside/fixedwing_backside_main.cpp b/src/modules/fixedwing_backside/fixedwing_backside_main.cpp
index f4ea05088..71b387b1e 100644
--- a/src/modules/fixedwing_backside/fixedwing_backside_main.cpp
+++ b/src/modules/fixedwing_backside/fixedwing_backside_main.cpp
@@ -115,7 +115,7 @@ int fixedwing_backside_main(int argc, char *argv[])
 					 SCHED_PRIORITY_MAX - 10,
 					 5120,
 					 control_demo_thread_main,
-					 (argv) ? (const char **)&argv[2] : (const char **)NULL);
+					 (argv) ? (char * const *)&argv[2] : (char * const *)NULL);
 		exit(0);
 	}
 
diff --git a/src/modules/fw_att_control/fw_att_control_main.cpp b/src/modules/fw_att_control/fw_att_control_main.cpp
index e770c11a2..4cb6c1aef 100644
--- a/src/modules/fw_att_control/fw_att_control_main.cpp
+++ b/src/modules/fw_att_control/fw_att_control_main.cpp
@@ -35,8 +35,9 @@
  * @file fw_att_control_main.c
  * Implementation of a generic attitude controller based on classic orthogonal PIDs.
  *
- * @author Lorenz Meier <lm@inf.ethz.ch>
- * @author Thomas Gubler <thomasgubler@gmail.com>
+ * @author Lorenz Meier 	<lm@inf.ethz.ch>
+ * @author Thomas Gubler 	<thomasgubler@gmail.com>
+ * @author Roman Bapst		<bapstr@ethz.ch>
  *
  */
 
@@ -92,12 +93,12 @@ public:
 	FixedwingAttitudeControl();
 
 	/**
-	 * Destructor, also kills the sensors task.
+	 * Destructor, also kills the main task.
 	 */
 	~FixedwingAttitudeControl();
 
 	/**
-	 * Start the sensors task.
+	 * Start the main task.
 	 *
 	 * @return	OK on success.
 	 */
@@ -112,9 +113,9 @@ public:
 
 private:
 
-	bool		_task_should_exit;		/**< if true, sensor task should exit */
+	bool		_task_should_exit;		/**< if true, attitude control task should exit */
 	bool		_task_running;			/**< if true, task is running in its mainloop */
-	int		_control_task;			/**< task handle for sensor task */
+	int		_control_task;			/**< task handle */
 
 	int		_att_sub;			/**< vehicle attitude subscription */
 	int		_accel_sub;			/**< accelerometer subscription */
@@ -130,11 +131,15 @@ private:
 	orb_advert_t	_rate_sp_pub;			/**< rate setpoint publication */
 	orb_advert_t	_attitude_sp_pub;		/**< attitude setpoint point */
 	orb_advert_t	_actuators_0_pub;		/**< actuator control group 0 setpoint */
-	orb_advert_t	_actuators_1_pub;		/**< actuator control group 1 setpoint (Airframe) */
+	orb_advert_t	_actuators_2_pub;		/**< actuator control group 1 setpoint (Airframe) */
+
+	orb_id_t _rates_sp_id;	// pointer to correct rates setpoint uORB metadata structure
+	orb_id_t _actuators_id;	// pointer to correct actuator controls0 uORB metadata structure
 
 	struct vehicle_attitude_s			_att;			/**< vehicle attitude */
 	struct accel_report				_accel;			/**< body frame accelerations */
 	struct vehicle_attitude_setpoint_s		_att_sp;		/**< vehicle attitude setpoint */
+	struct vehicle_rates_setpoint_s			_rates_sp;	/* attitude rates setpoint */
 	struct manual_control_setpoint_s		_manual;		/**< r/c channel data */
 	struct airspeed_s				_airspeed;		/**< airspeed */
 	struct vehicle_control_mode_s			_vcontrol_mode;		/**< vehicle control mode */
@@ -228,6 +233,7 @@ private:
 		param_t pitchsp_offset_deg;
 		param_t man_roll_max;
 		param_t man_pitch_max;
+
 	}		_parameter_handles;		/**< handles for interesting parameters */
 
 
@@ -289,7 +295,7 @@ private:
 	static void	task_main_trampoline(int argc, char *argv[]);
 
 	/**
-	 * Main sensor collection task.
+	 * Main attitude controller collection task.
 	 */
 	void		task_main();
 
@@ -327,7 +333,10 @@ FixedwingAttitudeControl::FixedwingAttitudeControl() :
 	_rate_sp_pub(-1),
 	_attitude_sp_pub(-1),
 	_actuators_0_pub(-1),
-	_actuators_1_pub(-1),
+	_actuators_2_pub(-1),
+
+	_rates_sp_id(0),
+	_actuators_id(0),
 
 /* performance counters */
 	_loop_perf(perf_alloc(PC_ELAPSED, "fw att control")),
@@ -341,6 +350,7 @@ FixedwingAttitudeControl::FixedwingAttitudeControl() :
 	_att = {};
 	_accel = {};
 	_att_sp = {};
+	_rates_sp = {};
 	_manual = {};
 	_airspeed = {};
 	_vcontrol_mode = {};
@@ -462,7 +472,6 @@ FixedwingAttitudeControl::parameters_update()
 	_parameters.man_roll_max = math::radians(_parameters.man_roll_max);
 	_parameters.man_pitch_max = math::radians(_parameters.man_pitch_max);
 
-
 	/* pitch control parameters */
 	_pitch_ctrl.set_time_constant(_parameters.tconst);
 	_pitch_ctrl.set_k_p(_parameters.p_p);
@@ -497,7 +506,7 @@ FixedwingAttitudeControl::vehicle_control_mode_poll()
 {
 	bool vcontrol_mode_updated;
 
-	/* Check HIL state if vehicle status has changed */
+	/* Check if vehicle control mode has changed */
 	orb_check(_vcontrol_mode_sub, &vcontrol_mode_updated);
 
 	if (vcontrol_mode_updated) {
@@ -529,7 +538,6 @@ FixedwingAttitudeControl::vehicle_airspeed_poll()
 
 	if (airspeed_updated) {
 		orb_copy(ORB_ID(airspeed), _airspeed_sub, &_airspeed);
-//		warnx("airspeed poll: ind: %.4f,  true: %.4f", _airspeed.indicated_airspeed_m_s, _airspeed.true_airspeed_m_s);
 	}
 }
 
@@ -541,7 +549,7 @@ FixedwingAttitudeControl::vehicle_accel_poll()
 	orb_check(_accel_sub, &accel_updated);
 
 	if (accel_updated) {
-		orb_copy(ORB_ID(sensor_accel0), _accel_sub, &_accel);
+		orb_copy(ORB_ID(sensor_accel), _accel_sub, &_accel);
 	}
 }
 
@@ -579,6 +587,16 @@ FixedwingAttitudeControl::vehicle_status_poll()
 
 	if (vehicle_status_updated) {
 		orb_copy(ORB_ID(vehicle_status), _vehicle_status_sub, &_vehicle_status);
+		/* set correct uORB ID, depending on if vehicle is VTOL or not */
+		if (!_rates_sp_id) {
+			if (_vehicle_status.is_vtol) {
+				_rates_sp_id = ORB_ID(fw_virtual_rates_setpoint);
+				_actuators_id = ORB_ID(actuator_controls_virtual_fw);
+			} else {
+				_rates_sp_id = ORB_ID(vehicle_rates_setpoint);
+				_actuators_id = ORB_ID(actuator_controls_0);
+			}
+		}
 	}
 }
 
@@ -601,7 +619,7 @@ FixedwingAttitudeControl::task_main()
 	 */
 	_att_sp_sub = orb_subscribe(ORB_ID(vehicle_attitude_setpoint));
 	_att_sub = orb_subscribe(ORB_ID(vehicle_attitude));
-	_accel_sub = orb_subscribe(ORB_ID(sensor_accel0));
+	_accel_sub = orb_subscribe_multi(ORB_ID(sensor_accel), 0);
 	_airspeed_sub = orb_subscribe(ORB_ID(airspeed));
 	_vcontrol_mode_sub = orb_subscribe(ORB_ID(vehicle_control_mode));
 	_params_sub = orb_subscribe(ORB_ID(parameter_update));
@@ -679,6 +697,67 @@ FixedwingAttitudeControl::task_main()
 			/* load local copies */
 			orb_copy(ORB_ID(vehicle_attitude), _att_sub, &_att);
 
+			if (_vehicle_status.is_vtol) {
+				/* vehicle type is VTOL, need to modify attitude!
+				 * The following modification to the attitude is vehicle specific and in this case applies
+				 *  to tail-sitter models !!!
+				 *
+				 * Since the VTOL airframe is initialized as a multicopter we need to
+				 * modify the estimated attitude for the fixed wing operation.
+				 * Since the neutral position of the vehicle in fixed wing mode is -90 degrees rotated around
+				 * the pitch axis compared to the neutral position of the vehicle in multicopter mode
+				 * we need to swap the roll and the yaw axis (1st and 3rd column) in the rotation matrix.
+				 * Additionally, in order to get the correct sign of the pitch, we need to multiply
+				 * the new x axis of the rotation matrix with -1
+				 *
+				 * original:			modified:
+				 *
+				 * Rxx  Ryx  Rzx		-Rzx  Ryx  Rxx
+				 * Rxy	Ryy  Rzy		-Rzy  Ryy  Rxy
+				 * Rxz	Ryz  Rzz		-Rzz  Ryz  Rxz
+				 * */
+				math::Matrix<3, 3> R;				//original rotation matrix
+				math::Matrix<3, 3> R_adapted;		//modified rotation matrix
+				R.set(_att.R);
+				R_adapted.set(_att.R);
+
+				/* move z to x */
+				R_adapted(0, 0) = R(0, 2);
+				R_adapted(1, 0) = R(1, 2);
+				R_adapted(2, 0) = R(2, 2);
+
+				/* move x to z */
+				R_adapted(0, 2) = R(0, 0);
+				R_adapted(1, 2) = R(1, 0);
+				R_adapted(2, 2) = R(2, 0);
+
+				/* change direction of pitch (convert to right handed system) */
+				R_adapted(0, 0) = -R_adapted(0, 0);
+				R_adapted(1, 0) = -R_adapted(1, 0);
+				R_adapted(2, 0) = -R_adapted(2, 0);
+				math::Vector<3> euler_angles;		//adapted euler angles for fixed wing operation
+				euler_angles = R_adapted.to_euler();
+
+				/* fill in new attitude data */
+				_att.roll    = euler_angles(0);
+				_att.pitch   = euler_angles(1);
+				_att.yaw     = euler_angles(2);
+				_att.R[0][0] = R_adapted(0, 0);
+				_att.R[0][1] = R_adapted(0, 1);
+				_att.R[0][2] = R_adapted(0, 2);
+				_att.R[1][0] = R_adapted(1, 0);
+				_att.R[1][1] = R_adapted(1, 1);
+				_att.R[1][2] = R_adapted(1, 2);
+				_att.R[2][0] = R_adapted(2, 0);
+				_att.R[2][1] = R_adapted(2, 1);
+				_att.R[2][2] = R_adapted(2, 2);
+
+				/* lastly, roll- and yawspeed have to be swaped */
+				float helper = _att.rollspeed;
+				_att.rollspeed = -_att.yawspeed;
+				_att.yawspeed = helper;
+			}
+
 			vehicle_airspeed_poll();
 
 			vehicle_setpoint_poll();
@@ -696,7 +775,7 @@ FixedwingAttitudeControl::task_main()
 			/* lock integrator until control is started */
 			bool lock_integrator;
 
-			if (_vcontrol_mode.flag_control_attitude_enabled) {
+			if (_vcontrol_mode.flag_control_attitude_enabled && !_vehicle_status.is_rotary_wing) {
 				lock_integrator = false;
 
 			} else {
@@ -705,10 +784,10 @@ FixedwingAttitudeControl::task_main()
 
 			/* Simple handling of failsafe: deploy parachute if failsafe is on */
 			if (_vcontrol_mode.flag_control_termination_enabled) {
-				_actuators_airframe.control[1] = 1.0f;
+				_actuators_airframe.control[7] = 1.0f;
 //				warnx("_actuators_airframe.control[1] = 1.0f;");
 			} else {
-				_actuators_airframe.control[1] = 0.0f;
+				_actuators_airframe.control[7] = 0.0f;
 //				warnx("_actuators_airframe.control[1] = -1.0f;");
 			}
 
@@ -744,6 +823,7 @@ FixedwingAttitudeControl::task_main()
 
 				float roll_sp = _parameters.rollsp_offset_rad;
 				float pitch_sp = _parameters.pitchsp_offset_rad;
+				float yaw_manual = 0.0f;
 				float throttle_sp = 0.0f;
 
 				/* Read attitude setpoint from uorb if
@@ -751,8 +831,7 @@ FixedwingAttitudeControl::task_main()
 				 * - manual control is disabled (another app may send the setpoint, but it should
 				 *   for sure not be set from the remote control values)
 				 */
-				if (_vcontrol_mode.flag_control_velocity_enabled ||
-						_vcontrol_mode.flag_control_position_enabled ||
+				if (_vcontrol_mode.flag_control_auto_enabled ||
 						!_vcontrol_mode.flag_control_manual_enabled) {
 					/* read in attitude setpoint from attitude setpoint uorb topic */
 					roll_sp = _att_sp.roll_body + _parameters.rollsp_offset_rad;
@@ -769,6 +848,25 @@ FixedwingAttitudeControl::task_main()
 					if (_att_sp.yaw_reset_integral) {
 						_yaw_ctrl.reset_integrator();
 					}
+				} else if (_vcontrol_mode.flag_control_velocity_enabled) {
+ 					/*
+					 * Velocity should be controlled and manual is enabled.
+					*/
+					roll_sp = (_manual.y * _parameters.man_roll_max - _parameters.trim_roll)
+											+ _parameters.rollsp_offset_rad;
+					pitch_sp = _att_sp.pitch_body + _parameters.pitchsp_offset_rad;
+					throttle_sp = _att_sp.thrust;
+
+					/* reset integrals where needed */
+					if (_att_sp.roll_reset_integral) {
+						_roll_ctrl.reset_integrator();
+					}
+					if (_att_sp.pitch_reset_integral) {
+						_pitch_ctrl.reset_integrator();
+					}
+					if (_att_sp.yaw_reset_integral) {
+						_yaw_ctrl.reset_integrator();
+					}
 				} else {
 					/*
 					 * Scale down roll and pitch as the setpoints are radians
@@ -786,6 +884,8 @@ FixedwingAttitudeControl::task_main()
 						+ _parameters.rollsp_offset_rad;
 					pitch_sp = -(_manual.x * _parameters.man_pitch_max - _parameters.trim_pitch)
 						+ _parameters.pitchsp_offset_rad;
+					/* allow manual control of rudder deflection */
+					yaw_manual = _manual.r;
 					throttle_sp = _manual.z;
 					_actuators.control[4] = _manual.flaps;
 
@@ -802,18 +902,18 @@ FixedwingAttitudeControl::task_main()
 					att_sp.thrust = throttle_sp;
 
 					/* lazily publish the setpoint only once available */
-					if (_attitude_sp_pub > 0) {
+					if (_attitude_sp_pub > 0 && !_vehicle_status.is_rotary_wing) {
 						/* publish the attitude setpoint */
 						orb_publish(ORB_ID(vehicle_attitude_setpoint), _attitude_sp_pub, &att_sp);
 
-					} else {
+					} else if (_attitude_sp_pub < 0 && !_vehicle_status.is_rotary_wing) {
 						/* advertise and publish */
 						_attitude_sp_pub = orb_advertise(ORB_ID(vehicle_attitude_setpoint), &att_sp);
 					}
 				}
 
 				/* If the aircraft is on ground reset the integrators */
-				if (_vehicle_status.condition_landed) {
+				if (_vehicle_status.condition_landed || _vehicle_status.is_rotary_wing) {
 					_roll_ctrl.reset_integrator();
 					_pitch_ctrl.reset_integrator();
 					_yaw_ctrl.reset_integrator();
@@ -833,19 +933,38 @@ FixedwingAttitudeControl::task_main()
 					}
 				}
 
+				/* Prepare data for attitude controllers */
+				struct ECL_ControlData control_input = {};
+				control_input.roll = _att.roll;
+				control_input.pitch = _att.pitch;
+				control_input.yaw = _att.yaw;
+				control_input.roll_rate = _att.rollspeed;
+				control_input.pitch_rate = _att.pitchspeed;
+				control_input.yaw_rate = _att.yawspeed;
+				control_input.speed_body_u = speed_body_u;
+				control_input.speed_body_v = speed_body_v;
+				control_input.speed_body_w = speed_body_w;
+				control_input.roll_setpoint = roll_sp;
+				control_input.pitch_setpoint = pitch_sp;
+				control_input.airspeed_min = _parameters.airspeed_min;
+				control_input.airspeed_max = _parameters.airspeed_max;
+				control_input.airspeed = airspeed;
+				control_input.scaler = airspeed_scaling;
+				control_input.lock_integrator = lock_integrator;
+
 				/* Run attitude controllers */
 				if (isfinite(roll_sp) && isfinite(pitch_sp)) {
-					_roll_ctrl.control_attitude(roll_sp, _att.roll);
-					_pitch_ctrl.control_attitude(pitch_sp, _att.roll, _att.pitch, airspeed);
-					_yaw_ctrl.control_attitude(_att.roll, _att.pitch,
-							speed_body_u, speed_body_v, speed_body_w,
-							_roll_ctrl.get_desired_rate(), _pitch_ctrl.get_desired_rate()); //runs last, because is depending on output of roll and pitch attitude
+					_roll_ctrl.control_attitude(control_input);
+					_pitch_ctrl.control_attitude(control_input);
+					_yaw_ctrl.control_attitude(control_input); //runs last, because is depending on output of roll and pitch attitude
+
+					/* Update input data for rate controllers */
+					control_input.roll_rate_setpoint = _roll_ctrl.get_desired_rate();
+					control_input.pitch_rate_setpoint = _pitch_ctrl.get_desired_rate();
+					control_input.yaw_rate_setpoint = _yaw_ctrl.get_desired_rate();
 
 					/* Run attitude RATE controllers which need the desired attitudes from above, add trim */
-					float roll_u = _roll_ctrl.control_bodyrate(_att.pitch,
-							_att.rollspeed, _att.yawspeed,
-							_yaw_ctrl.get_desired_rate(),
-							_parameters.airspeed_min, _parameters.airspeed_max, airspeed, airspeed_scaling, lock_integrator);
+					float roll_u = _roll_ctrl.control_bodyrate(control_input);
 					_actuators.control[0] = (isfinite(roll_u)) ? roll_u + _parameters.trim_roll : _parameters.trim_roll;
 					if (!isfinite(roll_u)) {
 						_roll_ctrl.reset_integrator();
@@ -856,10 +975,7 @@ FixedwingAttitudeControl::task_main()
 						}
 					}
 
-					float pitch_u = _pitch_ctrl.control_bodyrate(_att.roll, _att.pitch,
-							_att.pitchspeed, _att.yawspeed,
-							_yaw_ctrl.get_desired_rate(),
-							_parameters.airspeed_min, _parameters.airspeed_max, airspeed, airspeed_scaling, lock_integrator);
+					float pitch_u = _pitch_ctrl.control_bodyrate(control_input);
 					_actuators.control[1] = (isfinite(pitch_u)) ? pitch_u + _parameters.trim_pitch : _parameters.trim_pitch;
 					if (!isfinite(pitch_u)) {
 						_pitch_ctrl.reset_integrator();
@@ -880,11 +996,11 @@ FixedwingAttitudeControl::task_main()
 						}
 					}
 
-					float yaw_u = _yaw_ctrl.control_bodyrate(_att.roll, _att.pitch,
-							_att.pitchspeed, _att.yawspeed,
-							_pitch_ctrl.get_desired_rate(),
-							_parameters.airspeed_min, _parameters.airspeed_max, airspeed, airspeed_scaling, lock_integrator);
+					float yaw_u = _yaw_ctrl.control_bodyrate(control_input);
 					_actuators.control[2] = (isfinite(yaw_u)) ? yaw_u + _parameters.trim_yaw : _parameters.trim_yaw;
+
+					/* add in manual rudder control */
+					_actuators.control[2] += yaw_manual;
 					if (!isfinite(yaw_u)) {
 						_yaw_ctrl.reset_integrator();
 						perf_count(_nonfinite_output_perf);
@@ -915,20 +1031,18 @@ FixedwingAttitudeControl::task_main()
 				 * Lazily publish the rate setpoint (for analysis, the actuators are published below)
 				 * only once available
 				 */
-				vehicle_rates_setpoint_s rates_sp;
-				rates_sp.roll = _roll_ctrl.get_desired_rate();
-				rates_sp.pitch = _pitch_ctrl.get_desired_rate();
-				rates_sp.yaw = _yaw_ctrl.get_desired_rate();
+				_rates_sp.roll = _roll_ctrl.get_desired_rate();
+				_rates_sp.pitch = _pitch_ctrl.get_desired_rate();
+				_rates_sp.yaw = _yaw_ctrl.get_desired_rate();
 
-				rates_sp.timestamp = hrt_absolute_time();
+				_rates_sp.timestamp = hrt_absolute_time();
 
 				if (_rate_sp_pub > 0) {
-					/* publish the attitude setpoint */
-					orb_publish(ORB_ID(vehicle_rates_setpoint), _rate_sp_pub, &rates_sp);
-
-				} else {
-					/* advertise and publish */
-					_rate_sp_pub = orb_advertise(ORB_ID(vehicle_rates_setpoint), &rates_sp);
+					/* publish the attitude rates setpoint */
+					orb_publish(_rates_sp_id, _rate_sp_pub, &_rates_sp);
+				} else if (_rates_sp_id) {
+					/* advertise the attitude rates setpoint */
+					_rate_sp_pub = orb_advertise(_rates_sp_id, &_rates_sp);
 				}
 
 			} else {
@@ -946,30 +1060,25 @@ FixedwingAttitudeControl::task_main()
 
 			/* lazily publish the setpoint only once available */
 			_actuators.timestamp = hrt_absolute_time();
+			_actuators.timestamp_sample = _att.timestamp;
 			_actuators_airframe.timestamp = hrt_absolute_time();
+			_actuators_airframe.timestamp_sample = _att.timestamp;
 
+			/* publish the actuator controls */
 			if (_actuators_0_pub > 0) {
-				/* publish the attitude setpoint */
-				orb_publish(ORB_ID(actuator_controls_0), _actuators_0_pub, &_actuators);
-
-			} else {
-				/* advertise and publish */
-				_actuators_0_pub = orb_advertise(ORB_ID(actuator_controls_0), &_actuators);
+				orb_publish(_actuators_id, _actuators_0_pub, &_actuators);
+			} else if (_actuators_id) {
+				_actuators_0_pub= orb_advertise(_actuators_id, &_actuators);
 			}
 
-			if (_actuators_1_pub > 0) {
-				/* publish the attitude setpoint */
-				orb_publish(ORB_ID(actuator_controls_1), _actuators_1_pub, &_actuators_airframe);
-//				warnx("%.2f %.2f %.2f %.2f %.2f %.2f %.2f %.2f",
-//						(double)_actuators_airframe.control[0], (double)_actuators_airframe.control[1], (double)_actuators_airframe.control[2],
-//						(double)_actuators_airframe.control[3], (double)_actuators_airframe.control[4], (double)_actuators_airframe.control[5],
-//						(double)_actuators_airframe.control[6], (double)_actuators_airframe.control[7]);
+			if (_actuators_2_pub > 0) {
+				/* publish the actuator controls*/
+				orb_publish(ORB_ID(actuator_controls_2), _actuators_2_pub, &_actuators_airframe);
 
 			} else {
 				/* advertise and publish */
-				_actuators_1_pub = orb_advertise(ORB_ID(actuator_controls_1), &_actuators_airframe);
+				_actuators_2_pub = orb_advertise(ORB_ID(actuator_controls_2), &_actuators_airframe);
 			}
-
 		}
 
 		loop_counter++;
diff --git a/src/modules/fw_att_control/module.mk b/src/modules/fw_att_control/module.mk
index 89c6494c5..3661a171f 100644
--- a/src/modules/fw_att_control/module.mk
+++ b/src/modules/fw_att_control/module.mk
@@ -41,3 +41,5 @@ SRCS		= fw_att_control_main.cpp \
 		  fw_att_control_params.c
 
 MODULE_STACKSIZE = 1200
+
+MAXOPTIMIZATION = -Os
diff --git a/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp b/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp
index 6017369aa..dbf15d98a 100644
--- a/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp
+++ b/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp
@@ -163,6 +163,9 @@ private:
 
 	perf_counter_t	_loop_perf;			/**< loop performance counter */
 
+	float	_hold_alt;				/**< hold altitude for velocity mode */
+	hrt_abstime _control_position_last_called; /**<last call of control_position  */
+
 	/* land states */
 	bool land_noreturn_horizontal;
 	bool land_noreturn_vertical;
@@ -197,7 +200,11 @@ private:
 	ECL_L1_Pos_Controller				_l1_control;
 	TECS						_tecs;
 	fwPosctrl::mTecs				_mTecs;
-	bool						_was_pos_control_mode;
+	enum FW_POSCTRL_MODE {
+		FW_POSCTRL_MODE_AUTO,
+		FW_POSCTRL_MODE_POSITION,
+		FW_POSCTRL_MODE_OTHER
+	} _control_mode_current;			///< used to check the mode in the last control loop iteration. Use to check if the last iteration was in the same mode.
 
 	struct {
 		float l1_period;
@@ -317,6 +324,11 @@ private:
 	void		vehicle_status_poll();
 
 	/**
+	 * Check for manual setpoint updates.
+	 */
+	bool		vehicle_manual_control_setpoint_poll();
+
+	/**
 	 * Check for airspeed updates.
 	 */
 	bool		vehicle_airspeed_poll();
@@ -439,6 +451,9 @@ FixedwingPositionControl::FixedwingPositionControl() :
 /* performance counters */
 	_loop_perf(perf_alloc(PC_ELAPSED, "fw l1 control")),
 
+	_hold_alt(0.0f),
+	_control_position_last_called(0),
+
 	land_noreturn_horizontal(false),
 	land_noreturn_vertical(false),
 	land_stayonground(false),
@@ -458,7 +473,7 @@ FixedwingPositionControl::FixedwingPositionControl() :
 	_global_pos_valid(false),
 	_l1_control(),
 	_mTecs(),
-	_was_pos_control_mode(false)
+	_control_mode_current(FW_POSCTRL_MODE_OTHER)
 {
 	_nav_capabilities.turn_distance = 0.0f;
 
@@ -692,6 +707,22 @@ FixedwingPositionControl::vehicle_airspeed_poll()
 	return airspeed_updated;
 }
 
+bool
+FixedwingPositionControl::vehicle_manual_control_setpoint_poll()
+{
+	bool manual_updated;
+
+	/* Check if manual setpoint has changed */
+	orb_check(_manual_control_sub, &manual_updated);
+
+	if (manual_updated) {
+		orb_copy(ORB_ID(manual_control_setpoint), _manual_control_sub, &_manual);
+	}
+
+	return manual_updated;
+}
+
+
 void
 FixedwingPositionControl::vehicle_attitude_poll()
 {
@@ -852,6 +883,12 @@ bool
 FixedwingPositionControl::control_position(const math::Vector<2> &current_position, const math::Vector<3> &ground_speed,
 		const struct position_setpoint_triplet_s &pos_sp_triplet)
 {
+	float dt = FLT_MIN; // Using non zero value to a avoid division by zero
+	if (_control_position_last_called > 0) {
+		dt = (float)hrt_elapsed_time(&_control_position_last_called) * 1e-6f;
+	}
+	_control_position_last_called = hrt_absolute_time();
+
 	bool setpoint = true;
 
 	math::Vector<2> ground_speed_2d = {ground_speed(0), ground_speed(1)};
@@ -873,21 +910,22 @@ FixedwingPositionControl::control_position(const math::Vector<2> &current_positi
 	/* no throttle limit as default */
 	float throttle_max = 1.0f;
 
-	/* AUTONOMOUS FLIGHT */
-
-	// XXX this should only execute if auto AND safety off (actuators active),
-	// else integrators should be constantly reset.
-	if (pos_sp_triplet.current.valid) {
+	if (_control_mode.flag_control_auto_enabled &&
+			pos_sp_triplet.current.valid) {
+		/* AUTONOMOUS FLIGHT */
 
-		if (!_was_pos_control_mode) {
+		/* Reset integrators if switching to this mode from a other mode in which posctl was not active */
+		if (_control_mode_current == FW_POSCTRL_MODE_OTHER) {
 			/* reset integrators */
 			if (_mTecs.getEnabled()) {
 				_mTecs.resetIntegrators();
 				_mTecs.resetDerivatives(_airspeed.true_airspeed_m_s);
 			}
 		}
+		_control_mode_current = FW_POSCTRL_MODE_AUTO;
 
-		_was_pos_control_mode = true;
+		/* reset hold altitude */
+		_hold_alt = _global_pos.alt;
 
 		/* get circle mode */
 		bool was_circle_mode = _l1_control.circle_mode();
@@ -1169,15 +1207,15 @@ FixedwingPositionControl::control_position(const math::Vector<2> &current_positi
 					tecs_update_pitch_throttle(_pos_sp_triplet.current.alt,
 							calculate_target_airspeed(_parameters.airspeed_trim),
 							eas2tas,
-								math::radians(_parameters.pitch_limit_min),
-								math::radians(_parameters.pitch_limit_max),
-								_parameters.throttle_min,
-								takeoff_throttle,
-								_parameters.throttle_cruise,
-								false,
-								math::radians(_parameters.pitch_limit_min),
-								_global_pos.alt,
-								ground_speed);
+							math::radians(_parameters.pitch_limit_min),
+							math::radians(_parameters.pitch_limit_max),
+							_parameters.throttle_min,
+							takeoff_throttle,
+							_parameters.throttle_cruise,
+							false,
+							math::radians(_parameters.pitch_limit_min),
+							_global_pos.alt,
+							ground_speed);
 				}
 			} else {
 				/* Tell the attitude controller to stop integrating while we are waiting
@@ -1209,12 +1247,69 @@ FixedwingPositionControl::control_position(const math::Vector<2> &current_positi
 			_att_sp.roll_reset_integral = true;
 		}
 
-	} else {
+	} else if (_control_mode.flag_control_velocity_enabled &&
+			_control_mode.flag_control_altitude_enabled) {
+		/* POSITION CONTROL: pitch stick moves altitude setpoint, throttle stick sets airspeed */
 
-		_was_pos_control_mode = false;
+		const float deadBand = (60.0f/1000.0f);
+		const float factor = 1.0f - deadBand;
+		if (_control_mode_current != FW_POSCTRL_MODE_POSITION) {
+			/* Need to init because last loop iteration was in a different mode */
+			_hold_alt = _global_pos.alt;
+		}
+		/* Reset integrators if switching to this mode from a other mode in which posctl was not active */
+		if (_control_mode_current == FW_POSCTRL_MODE_OTHER) {
+			/* reset integrators */
+			if (_mTecs.getEnabled()) {
+				_mTecs.resetIntegrators();
+				_mTecs.resetDerivatives(_airspeed.true_airspeed_m_s);
+			}
+		}
+		_control_mode_current = FW_POSCTRL_MODE_POSITION;
+
+		/* Get demanded airspeed */
+		float altctrl_airspeed = _parameters.airspeed_min +
+					  (_parameters.airspeed_max - _parameters.airspeed_min) *
+					  _manual.z;
+
+		/* Get demanded vertical velocity from pitch control */
+		static bool was_in_deadband = false;
+		if (_manual.x > deadBand) {
+			float pitch = (_manual.x - deadBand) / factor;
+			_hold_alt -= (_parameters.max_climb_rate * dt) * pitch;
+			was_in_deadband = false;
+		} else if (_manual.x < - deadBand) {
+			float pitch = (_manual.x + deadBand) / factor;
+			_hold_alt -= (_parameters.max_sink_rate * dt) * pitch;
+			was_in_deadband = false;
+		} else if (!was_in_deadband) {
+			 /* store altitude at which manual.x was inside deadBand
+			  * The aircraft should immediately try to fly at this altitude
+			  * as this is what the pilot expects when he moves the stick to the center */
+			_hold_alt = _global_pos.alt;
+			was_in_deadband = true;
+		}
+		tecs_update_pitch_throttle(_hold_alt,
+				altctrl_airspeed,
+				eas2tas,
+				math::radians(_parameters.pitch_limit_min),
+				math::radians(_parameters.pitch_limit_max),
+				_parameters.throttle_min,
+				_parameters.throttle_max,
+				_parameters.throttle_cruise,
+				false,
+				math::radians(_parameters.pitch_limit_min),
+				_global_pos.alt,
+				ground_speed,
+				TECS_MODE_NORMAL);
+	} else {
+		_control_mode_current = FW_POSCTRL_MODE_OTHER;
 
 		/** MANUAL FLIGHT **/
 
+		/* reset hold altitude */
+		_hold_alt = _global_pos.alt;
+
 		/* no flight mode applies, do not publish an attitude setpoint */
 		setpoint = false;
 
@@ -1225,10 +1320,12 @@ FixedwingPositionControl::control_position(const math::Vector<2> &current_positi
 		}
 	}
 
+	/* Copy thrust output for publication */
 	if (_vehicle_status.engine_failure || _vehicle_status.engine_failure_cmd) {
 		/* Set thrust to 0 to minimize damage */
 		_att_sp.thrust = 0.0f;
-	} else if (pos_sp_triplet.current.type == SETPOINT_TYPE_TAKEOFF &&
+	} else if (_control_mode_current ==  FW_POSCTRL_MODE_AUTO && // launchdetector only available in auto
+			pos_sp_triplet.current.type == SETPOINT_TYPE_TAKEOFF &&
 			launch_detection_state != LAUNCHDETECTION_RES_DETECTED_ENABLEMOTORS) {
 		 /* making sure again that the correct thrust is used,
 		 * without depending on library calls for safety reasons */
@@ -1241,8 +1338,9 @@ FixedwingPositionControl::control_position(const math::Vector<2> &current_positi
 
 	/* During a takeoff waypoint while waiting for launch the pitch sp is set
 	 * already (not by tecs) */
-	if (!(pos_sp_triplet.current.type == SETPOINT_TYPE_TAKEOFF &&
-			launch_detection_state == LAUNCHDETECTION_RES_NONE)) {
+	if (!(_control_mode_current ==  FW_POSCTRL_MODE_AUTO &&
+				pos_sp_triplet.current.type == SETPOINT_TYPE_TAKEOFF &&
+				launch_detection_state == LAUNCHDETECTION_RES_NONE)) {
 		_att_sp.pitch_body = _mTecs.getEnabled() ? _mTecs.getPitchSetpoint() : _tecs.get_pitch_demand();
 	}
 
@@ -1313,8 +1411,6 @@ FixedwingPositionControl::task_main()
 			continue;
 		}
 
-		perf_begin(_loop_perf);
-
 		/* check vehicle control mode for changes to publication state */
 		vehicle_control_mode_poll();
 
@@ -1333,6 +1429,7 @@ FixedwingPositionControl::task_main()
 
 		/* only run controller if position changed */
 		if (fds[1].revents & POLLIN) {
+			perf_begin(_loop_perf);
 
 			/* XXX Hack to get mavlink output going */
 			if (_mavlink_fd < 0) {
@@ -1350,6 +1447,7 @@ FixedwingPositionControl::task_main()
 			vehicle_setpoint_poll();
 			vehicle_sensor_combined_poll();
 			vehicle_airspeed_poll();
+			vehicle_manual_control_setpoint_poll();
 			// vehicle_baro_poll();
 
 			math::Vector<3> ground_speed(_global_pos.vel_n, _global_pos.vel_e,  _global_pos.vel_d);
@@ -1386,10 +1484,9 @@ FixedwingPositionControl::task_main()
 				}
 
 			}
-
+			perf_end(_loop_perf);
 		}
 
-		perf_end(_loop_perf);
 	}
 
 	_task_running = false;
diff --git a/src/modules/fw_pos_control_l1/module.mk b/src/modules/fw_pos_control_l1/module.mk
index 15b575b50..98e5c0a1e 100644
--- a/src/modules/fw_pos_control_l1/module.mk
+++ b/src/modules/fw_pos_control_l1/module.mk
@@ -45,3 +45,7 @@ SRCS		= fw_pos_control_l1_main.cpp \
 		  mtecs/mTecs_params.c
 
 MODULE_STACKSIZE = 1200
+
+MAXOPTIMIZATION	 = -Os
+
+EXTRACXXFLAGS = -Wno-float-equal
diff --git a/src/modules/land_detector/FixedwingLandDetector.cpp b/src/modules/land_detector/FixedwingLandDetector.cpp
new file mode 100644
index 000000000..8e5bcf7ba
--- /dev/null
+++ b/src/modules/land_detector/FixedwingLandDetector.cpp
@@ -0,0 +1,125 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2013-2015 PX4 Development Team. All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in
+ *    the documentation and/or other materials provided with the
+ *    distribution.
+ * 3. Neither the name PX4 nor the names of its contributors may be
+ *    used to endorse or promote products derived from this software
+ *    without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ ****************************************************************************/
+
+/**
+ * @file FixedwingLandDetector.cpp
+ * Land detection algorithm for fixedwings
+ *
+ * @author Johan Jansen <jnsn.johan@gmail.com>
+ */
+
+#include "FixedwingLandDetector.h"
+
+#include <cmath>
+#include <drivers/drv_hrt.h>
+
+FixedwingLandDetector::FixedwingLandDetector() : LandDetector(),
+	_paramHandle(),
+	_params(),
+	_vehicleLocalPositionSub(-1),
+	_vehicleLocalPosition({}),
+	_airspeedSub(-1),
+	_airspeed({}),
+	_parameterSub(-1),
+	_velocity_xy_filtered(0.0f),
+	_velocity_z_filtered(0.0f),
+	_airspeed_filtered(0.0f),
+	_landDetectTrigger(0)
+{
+	_paramHandle.maxVelocity = param_find("LNDFW_VEL_XY_MAX");
+	_paramHandle.maxClimbRate = param_find("LNDFW_VEL_Z_MAX");
+	_paramHandle.maxAirSpeed = param_find("LNDFW_AIRSPD_MAX");
+}
+
+void FixedwingLandDetector::initialize()
+{
+	// Subscribe to local position and airspeed data
+	_vehicleLocalPositionSub = orb_subscribe(ORB_ID(vehicle_local_position));
+	_airspeedSub = orb_subscribe(ORB_ID(airspeed));
+}
+
+void FixedwingLandDetector::updateSubscriptions()
+{
+	orb_update(ORB_ID(vehicle_local_position), _vehicleLocalPositionSub, &_vehicleLocalPosition);
+	orb_update(ORB_ID(airspeed), _airspeedSub, &_airspeed);
+}
+
+bool FixedwingLandDetector::update()
+{
+	// First poll for new data from our subscriptions
+	updateSubscriptions();
+
+	const uint64_t now = hrt_absolute_time();
+	bool landDetected = false;
+
+	// TODO: reset filtered values on arming?
+	_velocity_xy_filtered = 0.95f * _velocity_xy_filtered + 0.05f * sqrtf(_vehicleLocalPosition.vx *
+				_vehicleLocalPosition.vx + _vehicleLocalPosition.vy * _vehicleLocalPosition.vy);
+	_velocity_z_filtered = 0.95f * _velocity_z_filtered + 0.05f * fabsf(_vehicleLocalPosition.vz);
+	_airspeed_filtered = 0.95f * _airspeed_filtered + 0.05f * _airspeed.true_airspeed_m_s;
+
+	// crude land detector for fixedwing
+	if (_velocity_xy_filtered < _params.maxVelocity
+	    && _velocity_z_filtered < _params.maxClimbRate
+	    && _airspeed_filtered < _params.maxAirSpeed) {
+
+		// these conditions need to be stable for a period of time before we trust them
+		if (now > _landDetectTrigger) {
+			landDetected = true;
+		}
+
+	} else {
+		// reset land detect trigger
+		_landDetectTrigger = now + LAND_DETECTOR_TRIGGER_TIME;
+	}
+
+	return landDetected;
+}
+
+void FixedwingLandDetector::updateParameterCache(const bool force)
+{
+	bool updated;
+	parameter_update_s paramUpdate;
+
+	orb_check(_parameterSub, &updated);
+
+	if (updated) {
+		orb_copy(ORB_ID(parameter_update), _parameterSub, &paramUpdate);
+	}
+
+	if (updated || force) {
+		param_get(_paramHandle.maxVelocity, &_params.maxVelocity);
+		param_get(_paramHandle.maxClimbRate, &_params.maxClimbRate);
+		param_get(_paramHandle.maxAirSpeed, &_params.maxAirSpeed);
+	}
+}
diff --git a/src/modules/land_detector/FixedwingLandDetector.h b/src/modules/land_detector/FixedwingLandDetector.h
new file mode 100644
index 000000000..0e9c092ee
--- /dev/null
+++ b/src/modules/land_detector/FixedwingLandDetector.h
@@ -0,0 +1,105 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2013-2015 PX4 Development Team. All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in
+ *    the documentation and/or other materials provided with the
+ *    distribution.
+ * 3. Neither the name PX4 nor the names of its contributors may be
+ *    used to endorse or promote products derived from this software
+ *    without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ ****************************************************************************/
+
+/**
+ * @file FixedwingLandDetector.h
+ * Land detection algorithm for fixedwing
+ *
+ * @author Johan Jansen <jnsn.johan@gmail.com>
+ */
+
+#ifndef __FIXED_WING_LAND_DETECTOR_H__
+#define __FIXED_WING_LAND_DETECTOR_H__
+
+#include "LandDetector.h"
+#include <uORB/topics/vehicle_local_position.h>
+#include <uORB/topics/airspeed.h>
+#include <uORB/topics/parameter_update.h>
+#include <systemlib/param/param.h>
+
+class FixedwingLandDetector : public LandDetector
+{
+public:
+	FixedwingLandDetector();
+
+protected:
+	/**
+	* @brief  blocking loop, should be run in a separate thread or task. Runs at 50Hz
+	**/
+	bool update() override;
+
+	/**
+	* @brief Initializes the land detection algorithm
+	**/
+	void initialize() override;
+
+	/**
+	* @brief  polls all subscriptions and pulls any data that has changed
+	**/
+	void updateSubscriptions();
+
+private:
+	/**
+	* @brief download and update local parameter cache
+	**/
+	void updateParameterCache(const bool force);
+
+	/**
+	* @brief Handles for interesting parameters
+	**/
+	struct {
+		param_t maxVelocity;
+		param_t maxClimbRate;
+		param_t maxAirSpeed;
+	}		_paramHandle;
+
+	struct {
+		float maxVelocity;
+		float maxClimbRate;
+		float maxAirSpeed;
+	} _params;
+
+private:
+	int                                     _vehicleLocalPositionSub;   /**< notification of local position */
+	struct vehicle_local_position_s         _vehicleLocalPosition;      /**< the result from local position subscription */
+	int                                     _airspeedSub;
+	struct airspeed_s                       _airspeed;
+	int 									_parameterSub;
+
+	float _velocity_xy_filtered;
+	float _velocity_z_filtered;
+	float _airspeed_filtered;
+	uint64_t _landDetectTrigger;
+};
+
+#endif //__FIXED_WING_LAND_DETECTOR_H__
diff --git a/src/modules/land_detector/LandDetector.cpp b/src/modules/land_detector/LandDetector.cpp
new file mode 100644
index 000000000..a4fbb9861
--- /dev/null
+++ b/src/modules/land_detector/LandDetector.cpp
@@ -0,0 +1,124 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2013-2015 PX4 Development Team. All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in
+ *    the documentation and/or other materials provided with the
+ *    distribution.
+ * 3. Neither the name PX4 nor the names of its contributors may be
+ *    used to endorse or promote products derived from this software
+ *    without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ ****************************************************************************/
+
+/**
+ * @file LandDetector.cpp
+ * Land detection algorithm
+ *
+ * @author Johan Jansen <jnsn.johan@gmail.com>
+ * @author Morten Lysgaard <morten@lysgaard.no>
+ */
+
+#include "LandDetector.h"
+#include <unistd.h>                 //usleep
+#include <drivers/drv_hrt.h>
+
+LandDetector::LandDetector() :
+	_landDetectedPub(-1),
+	_landDetected({0, false}),
+	_taskShouldExit(false),
+	_taskIsRunning(false)
+{
+	// ctor
+}
+
+LandDetector::~LandDetector()
+{
+	_taskShouldExit = true;
+	close(_landDetectedPub);
+}
+
+void LandDetector::shutdown()
+{
+	_taskShouldExit = true;
+}
+
+void LandDetector::start()
+{
+	// make sure this method has not already been called by another thread
+	if (isRunning()) {
+		return;
+	}
+
+	// advertise the first land detected uORB
+	_landDetected.timestamp = hrt_absolute_time();
+	_landDetected.landed = false;
+	_landDetectedPub = orb_advertise(ORB_ID(vehicle_land_detected), &_landDetected);
+
+	// initialize land detection algorithm
+	initialize();
+
+	// task is now running, keep doing so until shutdown() has been called
+	_taskIsRunning = true;
+	_taskShouldExit = false;
+
+	while (isRunning()) {
+
+		bool landDetected = update();
+
+		// publish if land detection state has changed
+		if (_landDetected.landed != landDetected) {
+			_landDetected.timestamp = hrt_absolute_time();
+			_landDetected.landed = landDetected;
+
+			// publish the land detected broadcast
+			orb_publish(ORB_ID(vehicle_land_detected), _landDetectedPub, &_landDetected);
+		}
+
+		// limit loop rate
+		usleep(1000000 / LAND_DETECTOR_UPDATE_RATE);
+	}
+
+	_taskIsRunning = false;
+	_exit(0);
+}
+
+bool LandDetector::orb_update(const struct orb_metadata *meta, int handle, void *buffer)
+{
+	bool newData = false;
+
+	// check if there is new data to grab
+	if (orb_check(handle, &newData) != OK) {
+		return false;
+	}
+
+	if (!newData) {
+		return false;
+	}
+
+	if (orb_copy(meta, handle, buffer) != OK) {
+		return false;
+	}
+
+	return true;
+}
diff --git a/src/modules/land_detector/LandDetector.h b/src/modules/land_detector/LandDetector.h
new file mode 100644
index 000000000..09db6e474
--- /dev/null
+++ b/src/modules/land_detector/LandDetector.h
@@ -0,0 +1,104 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2013-2015 PX4 Development Team. All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in
+ *    the documentation and/or other materials provided with the
+ *    distribution.
+ * 3. Neither the name PX4 nor the names of its contributors may be
+ *    used to endorse or promote products derived from this software
+ *    without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ ****************************************************************************/
+
+/**
+ * @file LandDetector.h
+ * Abstract interface for land detector algorithms
+ *
+ * @author Johan Jansen <jnsn.johan@gmail.com>
+ */
+
+#ifndef __LAND_DETECTOR_H__
+#define __LAND_DETECTOR_H__
+
+#include <uORB/uORB.h>
+#include <uORB/topics/vehicle_land_detected.h>
+
+class LandDetector
+{
+public:
+
+	LandDetector();
+	virtual ~LandDetector();
+
+	/**
+	* @return true if this task is currently running
+	**/
+	inline bool isRunning() const {return _taskIsRunning;}
+
+	/**
+	* @brief  Tells the Land Detector task that it should exit
+	**/
+	void shutdown();
+
+	/**
+	* @brief Blocking function that should be called from it's own task thread. This method will
+	*        run the underlying algorithm at the desired update rate and publish if the landing state changes.
+	**/
+	void start();
+
+protected:
+
+	/**
+	* @brief Pure abstract method that must be overriden by sub-classes. This actually runs the underlying algorithm
+	* @return true if a landing was detected and this should be broadcast to the rest of the system
+	**/
+	virtual bool update() = 0;
+
+	/**
+	* @brief Pure abstract method that is called by this class once for initializing the uderlying algorithm (memory allocation,
+	*        uORB topic subscription, creating file descriptors, etc.)
+	**/
+	virtual void initialize() = 0;
+
+	/**
+	* @brief Convinience function for polling uORB subscriptions
+	* @return true if there was new data and it was successfully copied
+	**/
+	bool orb_update(const struct orb_metadata *meta, int handle, void *buffer);
+
+	static constexpr uint32_t LAND_DETECTOR_UPDATE_RATE = 50;        /**< Run algorithm at 50Hz */
+
+	static constexpr uint64_t LAND_DETECTOR_TRIGGER_TIME = 2000000;  /**< usec that landing conditions have to hold
+                                                                          before triggering a land */
+
+protected:
+	orb_advert_t                            _landDetectedPub;           /**< publisher for position in local frame */
+	struct vehicle_land_detected_s          _landDetected;              /**< local vehicle position */
+
+private:
+	bool _taskShouldExit;                                               /**< true if it is requested that this task should exit */
+	bool _taskIsRunning;                                                /**< task has reached main loop and is currently running */
+};
+
+#endif //__LAND_DETECTOR_H__
diff --git a/src/modules/land_detector/MulticopterLandDetector.cpp b/src/modules/land_detector/MulticopterLandDetector.cpp
new file mode 100644
index 000000000..277cb9363
--- /dev/null
+++ b/src/modules/land_detector/MulticopterLandDetector.cpp
@@ -0,0 +1,145 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2013-2015 PX4 Development Team. All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in
+ *    the documentation and/or other materials provided with the
+ *    distribution.
+ * 3. Neither the name PX4 nor the names of its contributors may be
+ *    used to endorse or promote products derived from this software
+ *    without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ ****************************************************************************/
+
+/**
+ * @file MulticopterLandDetector.cpp
+ * Land detection algorithm for multicopters
+ *
+ * @author Johan Jansen <jnsn.johan@gmail.com>
+ * @author Morten Lysgaard <morten@lysgaard.no>
+ */
+
+#include "MulticopterLandDetector.h"
+
+#include <cmath>
+#include <drivers/drv_hrt.h>
+
+MulticopterLandDetector::MulticopterLandDetector() : LandDetector(),
+	_paramHandle(),
+	_params(),
+	_vehicleGlobalPositionSub(-1),
+	_sensorsCombinedSub(-1),
+	_waypointSub(-1),
+	_actuatorsSub(-1),
+	_armingSub(-1),
+	_parameterSub(-1),
+	_vehicleGlobalPosition({}),
+	_sensors({}),
+	_waypoint({}),
+	_actuators({}),
+	_arming({}),
+	_landTimer(0)
+{
+	_paramHandle.maxRotation = param_find("LNDMC_Z_VEL_MAX");
+	_paramHandle.maxVelocity = param_find("LNDMC_XY_VEL_MAX");
+	_paramHandle.maxClimbRate = param_find("LNDMC_ROT_MAX");
+	_paramHandle.maxThrottle = param_find("LNDMC_THR_MAX");
+}
+
+void MulticopterLandDetector::initialize()
+{
+	// subscribe to position, attitude, arming and velocity changes
+	_vehicleGlobalPositionSub = orb_subscribe(ORB_ID(vehicle_global_position));
+	_sensorsCombinedSub = orb_subscribe(ORB_ID(sensor_combined));
+	_waypointSub = orb_subscribe(ORB_ID(position_setpoint_triplet));
+	_actuatorsSub = orb_subscribe(ORB_ID_VEHICLE_ATTITUDE_CONTROLS);
+	_armingSub = orb_subscribe(ORB_ID(actuator_armed));
+	_parameterSub = orb_subscribe(ORB_ID(parameter_update));
+
+	// download parameters
+	updateParameterCache(true);
+}
+
+void MulticopterLandDetector::updateSubscriptions()
+{
+	orb_update(ORB_ID(vehicle_global_position), _vehicleGlobalPositionSub, &_vehicleGlobalPosition);
+	orb_update(ORB_ID(sensor_combined), _sensorsCombinedSub, &_sensors);
+	orb_update(ORB_ID(position_setpoint_triplet), _waypointSub, &_waypoint);
+	orb_update(ORB_ID_VEHICLE_ATTITUDE_CONTROLS, _actuatorsSub, &_actuators);
+	orb_update(ORB_ID(actuator_armed), _armingSub, &_arming);
+}
+
+bool MulticopterLandDetector::update()
+{
+	// first poll for new data from our subscriptions
+	updateSubscriptions();
+
+	// only trigger flight conditions if we are armed
+	if (!_arming.armed) {
+		return true;
+	}
+
+	const uint64_t now = hrt_absolute_time();
+
+	// check if we are moving vertically
+	bool verticalMovement = fabsf(_vehicleGlobalPosition.vel_d) > _params.maxClimbRate;
+
+	// check if we are moving horizontally
+	bool horizontalMovement = sqrtf(_vehicleGlobalPosition.vel_n * _vehicleGlobalPosition.vel_n
+					+ _vehicleGlobalPosition.vel_e * _vehicleGlobalPosition.vel_e) > _params.maxVelocity;
+
+	// next look if all rotation angles are not moving
+	bool rotating = sqrtf(_sensors.gyro_rad_s[0] * _sensors.gyro_rad_s[0] +
+			      _sensors.gyro_rad_s[1] * _sensors.gyro_rad_s[1] +
+			      _sensors.gyro_rad_s[2] * _sensors.gyro_rad_s[2]) > _params.maxRotation;
+
+	// check if thrust output is minimal (about half of default)
+	bool minimalThrust = _actuators.control[3] <= _params.maxThrottle;
+
+	if (verticalMovement || rotating || !minimalThrust || horizontalMovement) {
+		// sensed movement, so reset the land detector
+		_landTimer = now;
+		return false;
+	}
+
+	return now - _landTimer > LAND_DETECTOR_TRIGGER_TIME;
+}
+
+void MulticopterLandDetector::updateParameterCache(const bool force)
+{
+	bool updated;
+	parameter_update_s paramUpdate;
+
+	orb_check(_parameterSub, &updated);
+
+	if (updated) {
+		orb_copy(ORB_ID(parameter_update), _parameterSub, &paramUpdate);
+	}
+
+	if (updated || force) {
+		param_get(_paramHandle.maxClimbRate, &_params.maxClimbRate);
+		param_get(_paramHandle.maxVelocity, &_params.maxVelocity);
+		param_get(_paramHandle.maxRotation, &_params.maxRotation);
+		param_get(_paramHandle.maxThrottle, &_params.maxThrottle);
+	}
+}
diff --git a/src/modules/land_detector/MulticopterLandDetector.h b/src/modules/land_detector/MulticopterLandDetector.h
new file mode 100644
index 000000000..7bb7f1a90
--- /dev/null
+++ b/src/modules/land_detector/MulticopterLandDetector.h
@@ -0,0 +1,116 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2013-2015 PX4 Development Team. All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in
+ *    the documentation and/or other materials provided with the
+ *    distribution.
+ * 3. Neither the name PX4 nor the names of its contributors may be
+ *    used to endorse or promote products derived from this software
+ *    without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ ****************************************************************************/
+
+/**
+ * @file MulticopterLandDetector.h
+ * Land detection algorithm for multicopters
+ *
+ * @author Johan Jansen <jnsn.johan@gmail.com>
+ * @author Morten Lysgaard <morten@lysgaard.no>
+ */
+
+#ifndef __MULTICOPTER_LAND_DETECTOR_H__
+#define __MULTICOPTER_LAND_DETECTOR_H__
+
+#include "LandDetector.h"
+#include <uORB/topics/vehicle_global_position.h>
+#include <uORB/topics/sensor_combined.h>
+#include <uORB/topics/position_setpoint_triplet.h>
+#include <uORB/topics/actuator_controls.h>
+#include <uORB/topics/actuator_armed.h>
+#include <uORB/topics/parameter_update.h>
+#include <systemlib/param/param.h>
+
+class MulticopterLandDetector : public LandDetector
+{
+public:
+	MulticopterLandDetector();
+
+protected:
+	/**
+	* @brief  polls all subscriptions and pulls any data that has changed
+	**/
+	void updateSubscriptions();
+
+	/**
+	* @brief Runs one iteration of the land detection algorithm
+	**/
+	bool update() override;
+
+	/**
+	* @brief Initializes the land detection algorithm
+	**/
+	void initialize() override;
+
+
+private:
+	/**
+	* @brief download and update local parameter cache
+	**/
+	void updateParameterCache(const bool force);
+
+	/**
+	* @brief Handles for interesting parameters
+	**/
+	struct {
+		param_t maxClimbRate;
+		param_t maxVelocity;
+		param_t maxRotation;
+		param_t maxThrottle;
+	}		_paramHandle;
+
+	struct {
+		float maxClimbRate;
+		float maxVelocity;
+		float maxRotation;
+		float maxThrottle;
+	} _params;
+
+private:
+	int _vehicleGlobalPositionSub;                                      /**< notification of global position */
+	int _sensorsCombinedSub;
+	int _waypointSub;
+	int _actuatorsSub;
+	int _armingSub;
+	int _parameterSub;
+
+	struct vehicle_global_position_s          _vehicleGlobalPosition;   /**< the result from global position subscription */
+	struct sensor_combined_s                  _sensors;                 /**< subscribe to sensor readings */
+	struct position_setpoint_triplet_s        _waypoint;                /**< subscribe to autopilot navigation */
+	struct actuator_controls_s                _actuators;
+	struct actuator_armed_s                   _arming;
+
+	uint64_t _landTimer;                                                /**< timestamp in microseconds since a possible land was detected*/
+};
+
+#endif //__MULTICOPTER_LAND_DETECTOR_H__
diff --git a/src/modules/land_detector/land_detector_main.cpp b/src/modules/land_detector/land_detector_main.cpp
new file mode 100644
index 000000000..1e43e7ad5
--- /dev/null
+++ b/src/modules/land_detector/land_detector_main.cpp
@@ -0,0 +1,214 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2013-2015 PX4 Development Team. All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in
+ *    the documentation and/or other materials provided with the
+ *    distribution.
+ * 3. Neither the name PX4 nor the names of its contributors may be
+ *    used to endorse or promote products derived from this software
+ *    without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ ****************************************************************************/
+
+/**
+ * @file land_detector_main.cpp
+ * Land detection algorithm
+ *
+ * @author Johan Jansen <jnsn.johan@gmail.com>
+ */
+
+#include <unistd.h>					//usleep
+#include <stdio.h>
+#include <string.h>
+#include <stdlib.h>
+#include <errno.h>
+#include <drivers/drv_hrt.h>
+#include <systemlib/systemlib.h>	//Scheduler
+#include <systemlib/err.h>			//print to console
+
+#include "FixedwingLandDetector.h"
+#include "MulticopterLandDetector.h"
+
+//Function prototypes
+static int land_detector_start(const char *mode);
+static void land_detector_stop();
+
+/**
+ * land detector app start / stop handling function
+ * This makes the land detector module accessible from the nuttx shell
+ * @ingroup apps
+ */
+extern "C" __EXPORT int land_detector_main(int argc, char *argv[]);
+
+//Private variables
+static LandDetector *land_detector_task = nullptr;
+static int _landDetectorTaskID = -1;
+static char _currentMode[12];
+
+/**
+* Deamon thread function
+**/
+static void land_detector_deamon_thread(int argc, char *argv[])
+{
+	land_detector_task->start();
+}
+
+/**
+* Stop the task, force killing it if it doesn't stop by itself
+**/
+static void land_detector_stop()
+{
+	if (land_detector_task == nullptr || _landDetectorTaskID == -1) {
+		errx(1, "not running");
+		return;
+	}
+
+	land_detector_task->shutdown();
+
+	//Wait for task to die
+	int i = 0;
+
+	do {
+		/* wait 20ms */
+		usleep(20000);
+
+		/* if we have given up, kill it */
+		if (++i > 50) {
+			task_delete(_landDetectorTaskID);
+			break;
+		}
+	} while (land_detector_task->isRunning());
+
+
+	delete land_detector_task;
+	land_detector_task = nullptr;
+	_landDetectorTaskID = -1;
+	errx(0, "land_detector has been stopped");
+}
+
+/**
+* Start new task, fails if it is already running. Returns OK if successful
+**/
+static int land_detector_start(const char *mode)
+{
+	if (land_detector_task != nullptr || _landDetectorTaskID != -1) {
+		errx(1, "already running");
+		return -1;
+	}
+
+	//Allocate memory
+	if (!strcmp(mode, "fixedwing")) {
+		land_detector_task = new FixedwingLandDetector();
+
+	} else if (!strcmp(mode, "multicopter")) {
+		land_detector_task = new MulticopterLandDetector();
+
+	} else {
+		errx(1, "[mode] must be either 'fixedwing' or 'multicopter'");
+		return -1;
+	}
+
+	//Check if alloc worked
+	if (land_detector_task == nullptr) {
+		errx(1, "alloc failed");
+		return -1;
+	}
+
+	//Start new thread task
+	_landDetectorTaskID = task_spawn_cmd("land_detector",
+					     SCHED_DEFAULT,
+					     SCHED_PRIORITY_DEFAULT,
+					     1200,
+					     (main_t)&land_detector_deamon_thread,
+					     nullptr);
+
+	if (_landDetectorTaskID < 0) {
+		errx(1, "task start failed: %d", -errno);
+		return -1;
+	}
+
+	/* avoid memory fragmentation by not exiting start handler until the task has fully started */
+	const uint32_t timeout = hrt_absolute_time() + 5000000; //5 second timeout
+
+	while (!land_detector_task->isRunning()) {
+		usleep(50000);
+		printf(".");
+		fflush(stdout);
+
+		if (hrt_absolute_time() > timeout) {
+			err(1, "start failed - timeout");
+			land_detector_stop();
+			exit(1);
+		}
+	}
+
+	printf("\n");
+
+	//Remember current active mode
+	strncpy(_currentMode, mode, 12);
+
+	exit(0);
+	return 0;
+}
+
+/**
+* Main entry point for this module
+**/
+int land_detector_main(int argc, char *argv[])
+{
+
+	if (argc < 1) {
+		warnx("usage: land_detector {start|stop|status} [mode]\nmode can either be 'fixedwing' or 'multicopter'");
+		exit(0);
+	}
+
+	if (argc >= 2 && !strcmp(argv[1], "start")) {
+		land_detector_start(argv[2]);
+	}
+
+	if (!strcmp(argv[1], "stop")) {
+		land_detector_stop();
+		exit(0);
+	}
+
+	if (!strcmp(argv[1], "status")) {
+		if (land_detector_task) {
+
+			if (land_detector_task->isRunning()) {
+				warnx("running (%s)", _currentMode);
+
+			} else {
+				errx(1, "exists, but not running (%s)", _currentMode);
+			}
+
+			exit(0);
+
+		} else {
+			errx(1, "not running");
+		}
+	}
+
+	warn("usage: land_detector {start|stop|status} [mode]\nmode can either be 'fixedwing' or 'multicopter'");
+	return 1;
+}
diff --git a/src/modules/land_detector/land_detector_params.c b/src/modules/land_detector/land_detector_params.c
new file mode 100644
index 000000000..0302bc7c1
--- /dev/null
+++ b/src/modules/land_detector/land_detector_params.c
@@ -0,0 +1,104 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2014, 2015 PX4 Development Team. All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in
+ *    the documentation and/or other materials provided with the
+ *    distribution.
+ * 3. Neither the name PX4 nor the names of its contributors may be
+ *    used to endorse or promote products derived from this software
+ *    without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ ****************************************************************************/
+
+/**
+ * @file land_detector.c
+ * Land detector algorithm parameters.
+ *
+ * @author Johan Jansen <jnsn.johan@gmail.com>
+ */
+
+#include <systemlib/param/param.h>
+
+/**
+ * Multicopter max climb rate
+ *
+ * Maximum vertical velocity allowed to trigger a land (m/s up and down)
+ *
+ * @group Land Detector
+ */
+PARAM_DEFINE_FLOAT(LNDMC_Z_VEL_MAX, 0.30f);
+
+/**
+ * Multicopter max horizontal velocity
+ *
+ * Maximum horizontal velocity allowed to trigger a land (m/s)
+ *
+ * @group Land Detector
+ */
+PARAM_DEFINE_FLOAT(LNDMC_XY_VEL_MAX, 1.00f);
+
+/**
+ * Multicopter max rotation
+ *
+ * Maximum allowed around each axis to trigger a land (radians per second)
+ *
+ * @group Land Detector
+ */
+PARAM_DEFINE_FLOAT(LNDMC_ROT_MAX, 0.20f);
+
+/**
+ * Multicopter max throttle
+ *
+ * Maximum actuator output on throttle before triggering a land
+ *
+ * @group Land Detector
+ */
+PARAM_DEFINE_FLOAT(LNDMC_THR_MAX, 0.20f);
+
+/**
+ * Fixedwing max horizontal velocity
+ *
+ * Maximum horizontal velocity allowed to trigger a land (m/s)
+ *
+ * @group Land Detector
+ */
+PARAM_DEFINE_FLOAT(LNDFW_VEL_XY_MAX, 0.20f);
+
+/**
+ * Fixedwing max climb rate
+ *
+ * Maximum vertical velocity allowed to trigger a land (m/s up and down)
+ *
+ * @group Land Detector
+ */
+PARAM_DEFINE_FLOAT(LNDFW_VEL_Z_MAX, 10.00f);
+
+/**
+ * Airspeed max
+ *
+ * Maximum airspeed allowed to trigger a land (m/s)
+ *
+ * @group Land Detector
+ */
+PARAM_DEFINE_FLOAT(LNDFW_AIRSPD_MAX, 10.00f);
diff --git a/src/modules/land_detector/module.mk b/src/modules/land_detector/module.mk
new file mode 100644
index 000000000..e08a4b7a8
--- /dev/null
+++ b/src/modules/land_detector/module.mk
@@ -0,0 +1,13 @@
+#
+# Land detector
+#
+
+MODULE_COMMAND	= land_detector
+
+SRCS		= land_detector_main.cpp \
+              land_detector_params.c \
+              LandDetector.cpp \
+			  MulticopterLandDetector.cpp \
+              FixedwingLandDetector.cpp
+
+EXTRACXXFLAGS   = -Weffc++ -Os
diff --git a/src/modules/mavlink/mavlink_ftp.cpp b/src/modules/mavlink/mavlink_ftp.cpp
index f17497aa8..4ba595a87 100644
--- a/src/modules/mavlink/mavlink_ftp.cpp
+++ b/src/modules/mavlink/mavlink_ftp.cpp
@@ -801,7 +801,7 @@ MavlinkFTP::_return_request(Request *req)
 
 /// @brief Copy file (with limited space)
 int
-MavlinkFTP::_copy_file(const char *src_path, const char *dst_path, ssize_t length)
+MavlinkFTP::_copy_file(const char *src_path, const char *dst_path, size_t length)
 {
 	char buff[512];
 	int src_fd = -1, dst_fd = -1;
diff --git a/src/modules/mavlink/mavlink_ftp.h b/src/modules/mavlink/mavlink_ftp.h
index bef6775a9..9693a92a9 100644
--- a/src/modules/mavlink/mavlink_ftp.h
+++ b/src/modules/mavlink/mavlink_ftp.h
@@ -142,7 +142,7 @@ private:
 	static void	_worker_trampoline(void *arg);
 	void		_process_request(Request *req);
 	void		_reply(Request *req);
-	int		_copy_file(const char *src_path, const char *dst_path, ssize_t length);
+	int		_copy_file(const char *src_path, const char *dst_path, size_t length);
 
 	ErrorCode	_workList(PayloadHeader *payload);
 	ErrorCode	_workOpen(PayloadHeader *payload, int oflag);
diff --git a/src/modules/mavlink/mavlink_main.cpp b/src/modules/mavlink/mavlink_main.cpp
index b99719821..8eeeb5bd7 100644
--- a/src/modules/mavlink/mavlink_main.cpp
+++ b/src/modules/mavlink/mavlink_main.cpp
@@ -904,20 +904,20 @@ Mavlink::send_statustext(unsigned char severity, const char *string)
 	mavlink_logbuffer_write(&_logbuffer, &logmsg);
 }
 
-MavlinkOrbSubscription *Mavlink::add_orb_subscription(const orb_id_t topic)
+MavlinkOrbSubscription *Mavlink::add_orb_subscription(const orb_id_t topic, int instance)
 {
 	/* check if already subscribed to this topic */
 	MavlinkOrbSubscription *sub;
 
 	LL_FOREACH(_subscriptions, sub) {
-		if (sub->get_topic() == topic) {
+		if (sub->get_topic() == topic && sub->get_instance() == instance) {
 			/* already subscribed */
 			return sub;
 		}
 	}
 
 	/* add new subscription */
-	MavlinkOrbSubscription *sub_new = new MavlinkOrbSubscription(topic);
+	MavlinkOrbSubscription *sub_new = new MavlinkOrbSubscription(topic, instance);
 
 	LL_APPEND(_subscriptions, sub_new);
 
@@ -948,7 +948,6 @@ Mavlink::configure_stream(const char *stream_name, const float rate)
 				/* delete stream */
 				LL_DELETE(_streams, stream);
 				delete stream;
-				warnx("deleted stream %s", stream->get_name());
 			}
 
 			return OK;
@@ -1287,30 +1286,11 @@ Mavlink::task_main(int argc, char *argv[])
 	}
 
 	if (Mavlink::instance_exists(_device_name, this)) {
-		warnx("mavlink instance for %s already running", _device_name);
+		warnx("%s already running", _device_name);
 		return ERROR;
 	}
 
-	/* inform about mode */
-	switch (_mode) {
-	case MAVLINK_MODE_NORMAL:
-		warnx("mode: NORMAL");
-		break;
-
-	case MAVLINK_MODE_CUSTOM:
-		warnx("mode: CUSTOM");
-		break;
-
-	case MAVLINK_MODE_ONBOARD:
-		warnx("mode: ONBOARD");
-		break;
-
-	default:
-		warnx("ERROR: Unknown mode");
-		break;
-	}
-
-	warnx("data rate: %d Bytes/s, port: %s, baud: %d", _datarate, _device_name, _baudrate);
+	warnx("mode: %u, data rate: %d B/s on %s @ %dB", _mode, _datarate, _device_name, _baudrate);
 
 	/* flush stdout in case MAVLink is about to take it over */
 	fflush(stdout);
@@ -1338,7 +1318,7 @@ Mavlink::task_main(int argc, char *argv[])
 		 * marker ring buffer approach.
 		 */
 		if (OK != message_buffer_init(2 * sizeof(mavlink_message_t) + 1)) {
-			errx(1, "can't allocate message buffer, exiting");
+			errx(1, "msg buf:");
 		}
 
 		/* initialize message buffer mutex */
@@ -1405,17 +1385,23 @@ Mavlink::task_main(int argc, char *argv[])
 		configure_stream("POSITION_TARGET_GLOBAL_INT", 3.0f);
 		configure_stream("ATTITUDE_TARGET", 3.0f);
 		configure_stream("DISTANCE_SENSOR", 0.5f);
-		configure_stream("OPTICAL_FLOW", 5.0f);
+		configure_stream("OPTICAL_FLOW_RAD", 5.0f);
 		break;
 
 	case MAVLINK_MODE_ONBOARD:
 		configure_stream("SYS_STATUS", 1.0f);
 		configure_stream("ATTITUDE", 50.0f);
 		configure_stream("GLOBAL_POSITION_INT", 50.0f);
+		configure_stream("LOCAL_POSITION_NED", 30.0f);
 		configure_stream("CAMERA_CAPTURE", 2.0f);
 		configure_stream("ATTITUDE_TARGET", 10.0f);
 		configure_stream("POSITION_TARGET_GLOBAL_INT", 10.0f);
+		configure_stream("POSITION_TARGET_LOCAL_NED", 10.0f);
+		configure_stream("DISTANCE_SENSOR", 10.0f);
+		configure_stream("OPTICAL_FLOW_RAD", 10.0f);
 		configure_stream("VFR_HUD", 10.0f);
+		configure_stream("SYSTEM_TIME", 1.0f);
+		configure_stream("TIMESYNC", 10.0f);
 		break;
 
 	default:
@@ -1566,8 +1552,6 @@ Mavlink::task_main(int argc, char *argv[])
 
 	_subscriptions = nullptr;
 
-	warnx("waiting for UART receive thread");
-
 	/* wait for threads to complete */
 	pthread_join(_receive_thread, NULL);
 
@@ -1638,7 +1622,7 @@ Mavlink::start(int argc, char *argv[])
 		       SCHED_PRIORITY_DEFAULT,
 		       2800,
 		       (main_t)&Mavlink::start_helper,
-		       (const char **)argv);
+		       (char * const *)argv);
 
 	// Ensure that this shell command
 	// does not return before the instance
diff --git a/src/modules/mavlink/mavlink_main.h b/src/modules/mavlink/mavlink_main.h
index ad5e5001b..baaa7bc13 100644
--- a/src/modules/mavlink/mavlink_main.h
+++ b/src/modules/mavlink/mavlink_main.h
@@ -171,7 +171,7 @@ public:
 
 	void			handle_message(const mavlink_message_t *msg);
 
-	MavlinkOrbSubscription *add_orb_subscription(const orb_id_t topic);
+	MavlinkOrbSubscription *add_orb_subscription(const orb_id_t topic, int instance=0);
 
 	int			get_instance_id();
 
diff --git a/src/modules/mavlink/mavlink_messages.cpp b/src/modules/mavlink/mavlink_messages.cpp
index 978aee118..4a095a765 100644
--- a/src/modules/mavlink/mavlink_messages.cpp
+++ b/src/modules/mavlink/mavlink_messages.cpp
@@ -342,6 +342,8 @@ private:
 	MavlinkStreamStatustext(MavlinkStreamStatustext &);
 	MavlinkStreamStatustext& operator = (const MavlinkStreamStatustext &);
 	FILE *fp = nullptr;
+	unsigned write_err_count = 0;
+	static const unsigned write_err_threshold = 5;
 
 protected:
 	explicit MavlinkStreamStatustext(Mavlink *mavlink) : MavlinkStream(mavlink)
@@ -370,10 +372,21 @@ protected:
 				/* write log messages in first instance to disk */
 				if (_mavlink->get_instance_id() == 0) {
 					if (fp) {
-						fputs(msg.text, fp);
-						fputs("\n", fp);
-						fsync(fileno(fp));
-					} else {
+						if (EOF == fputs(msg.text, fp)) {
+							write_err_count++;
+						} else {
+							write_err_count = 0;
+						}
+
+						if (write_err_count >= write_err_threshold) {
+							(void)fclose(fp);
+							fp = nullptr;
+						} else {
+							(void)fputs("\n", fp);
+							(void)fsync(fileno(fp));
+						}
+
+					} else if (write_err_count < write_err_threshold) {
 						/* string to hold the path to the log */
 						char log_file_name[32] = "";
 						char log_file_path[64] = "";
@@ -389,6 +402,10 @@ protected:
 						strftime(log_file_name, sizeof(log_file_name), "msgs_%Y_%m_%d_%H_%M_%S.txt", &tt);
 						snprintf(log_file_path, sizeof(log_file_path), "/fs/microsd/%s", log_file_name);
 						fp = fopen(log_file_path, "ab");
+
+						/* write first message */
+						fputs(msg.text, fp);
+						fputs("\n", fp);
 					}
 				}
 			}
@@ -810,9 +827,6 @@ private:
 	MavlinkOrbSubscription *_airspeed_sub;
 	uint64_t _airspeed_time;
 
-	MavlinkOrbSubscription *_sensor_combined_sub;
-	uint64_t _sensor_combined_time;
-
 	/* do not allow top copying this class */
 	MavlinkStreamVFRHUD(MavlinkStreamVFRHUD &);
 	MavlinkStreamVFRHUD& operator = (const MavlinkStreamVFRHUD &);
@@ -828,9 +842,7 @@ protected:
 		_act_sub(_mavlink->add_orb_subscription(ORB_ID(actuator_controls_0))),
 		_act_time(0),
 		_airspeed_sub(_mavlink->add_orb_subscription(ORB_ID(airspeed))),
-		_airspeed_time(0),
-		_sensor_combined_sub(_mavlink->add_orb_subscription(ORB_ID(sensor_combined))),
-		_sensor_combined_time(0)
+		_airspeed_time(0)
 	{}
 
 	void send(const hrt_abstime t)
@@ -840,14 +852,12 @@ protected:
 		struct actuator_armed_s armed;
 		struct actuator_controls_s act;
 		struct airspeed_s airspeed;
-		struct sensor_combined_s sensor_combined;
 
 		bool updated = _att_sub->update(&_att_time, &att);
 		updated |= _pos_sub->update(&_pos_time, &pos);
 		updated |= _armed_sub->update(&_armed_time, &armed);
 		updated |= _act_sub->update(&_act_time, &act);
 		updated |= _airspeed_sub->update(&_airspeed_time, &airspeed);
-		updated |= _sensor_combined_sub->update(&_sensor_combined_time, &sensor_combined);
 
 		if (updated) {
 			mavlink_vfr_hud_t msg;
@@ -856,7 +866,7 @@ protected:
 			msg.groundspeed = sqrtf(pos.vel_n * pos.vel_n + pos.vel_e * pos.vel_e);
 			msg.heading = _wrap_2pi(att.yaw) * M_RAD_TO_DEG_F;
 			msg.throttle = armed.armed ? act.control[3] * 100.0f : 0.0f;
-			msg.alt = sensor_combined.baro_alt_meter;
+			msg.alt = pos.alt;
 			msg.climb = -pos.vel_d;
 
 			_mavlink->send_message(MAVLINK_MSG_ID_VFR_HUD, &msg);
@@ -930,6 +940,92 @@ protected:
 	}
 };
 
+class MavlinkStreamSystemTime : public MavlinkStream
+{
+public:
+	const char *get_name() const {
+		return MavlinkStreamSystemTime::get_name_static();
+	}
+
+	static const char *get_name_static() {
+		return "SYSTEM_TIME";
+	}
+
+	uint8_t get_id() {
+		return MAVLINK_MSG_ID_SYSTEM_TIME;
+	}
+
+	static MavlinkStream *new_instance(Mavlink *mavlink) {
+		return new MavlinkStreamSystemTime(mavlink);
+	}
+
+	unsigned get_size() {
+		return MAVLINK_MSG_ID_SYSTEM_TIME_LEN + MAVLINK_NUM_NON_PAYLOAD_BYTES;
+	}
+
+private:
+	/* do not allow top copying this class */
+	MavlinkStreamSystemTime(MavlinkStreamSystemTime &);
+	MavlinkStreamSystemTime &operator = (const MavlinkStreamSystemTime &);
+
+protected:
+	explicit MavlinkStreamSystemTime(Mavlink *mavlink) : MavlinkStream(mavlink)
+	{}
+
+	void send(const hrt_abstime t) {
+		mavlink_system_time_t msg;
+		timespec tv;
+
+		clock_gettime(CLOCK_REALTIME, &tv);
+
+		msg.time_boot_ms = hrt_absolute_time() / 1000;
+		msg.time_unix_usec = (uint64_t)tv.tv_sec * 1000000 + tv.tv_nsec / 1000;
+
+		_mavlink->send_message(MAVLINK_MSG_ID_SYSTEM_TIME, &msg);
+	}
+};
+
+class MavlinkStreamTimesync : public MavlinkStream
+{
+public:
+	const char *get_name() const {
+		return MavlinkStreamTimesync::get_name_static();
+	}
+
+	static const char *get_name_static() {
+		return "TIMESYNC";
+	}
+
+	uint8_t get_id() {
+		return MAVLINK_MSG_ID_TIMESYNC;
+	}
+
+	static MavlinkStream *new_instance(Mavlink *mavlink) {
+		return new MavlinkStreamTimesync(mavlink);
+	}
+
+	unsigned get_size() {
+		return MAVLINK_MSG_ID_TIMESYNC_LEN + MAVLINK_NUM_NON_PAYLOAD_BYTES;
+	}
+
+private:
+	/* do not allow top copying this class */
+	MavlinkStreamTimesync(MavlinkStreamTimesync &);
+	MavlinkStreamTimesync &operator = (const MavlinkStreamTimesync &);
+
+protected:
+	explicit MavlinkStreamTimesync(Mavlink *mavlink) : MavlinkStream(mavlink)
+	{}
+
+	void send(const hrt_abstime t) {
+		mavlink_timesync_t msg;
+
+		msg.tc1 = 0;
+		msg.ts1 = hrt_absolute_time() * 1000; // boot time in nanoseconds
+
+		_mavlink->send_message(MAVLINK_MSG_ID_TIMESYNC, &msg);
+	}
+};
 
 class MavlinkStreamGlobalPositionInt : public MavlinkStream
 {
@@ -1246,14 +1342,7 @@ protected:
 		_act_sub(nullptr),
 		_act_time(0)
 	{
-		orb_id_t act_topics[] = {
-			ORB_ID(actuator_outputs_0),
-			ORB_ID(actuator_outputs_1),
-			ORB_ID(actuator_outputs_2),
-			ORB_ID(actuator_outputs_3)
-		};
-
-		_act_sub = _mavlink->add_orb_subscription(act_topics[N]);
+		_act_sub = _mavlink->add_orb_subscription(ORB_ID(actuator_outputs), N);
 	}
 
 	void send(const hrt_abstime t)
@@ -1328,7 +1417,7 @@ protected:
 		_status_time(0),
 		_pos_sp_triplet_sub(_mavlink->add_orb_subscription(ORB_ID(position_setpoint_triplet))),
 		_pos_sp_triplet_time(0),
-		_act_sub(_mavlink->add_orb_subscription(ORB_ID(actuator_outputs_0))),
+		_act_sub(_mavlink->add_orb_subscription(ORB_ID(actuator_outputs))),
 		_act_time(0)
 	{}
 
@@ -1358,7 +1447,10 @@ protected:
 			/* scale outputs depending on system type */
 			if (system_type == MAV_TYPE_QUADROTOR ||
 				system_type == MAV_TYPE_HEXAROTOR ||
-				system_type == MAV_TYPE_OCTOROTOR) {
+				system_type == MAV_TYPE_OCTOROTOR ||
+				system_type == MAV_TYPE_VTOL_DUOROTOR ||
+				system_type == MAV_TYPE_VTOL_QUADROTOR) {
+
 				/* multirotors: set number of rotor outputs depending on type */
 
 				unsigned n;
@@ -1372,6 +1464,14 @@ protected:
 					n = 6;
 					break;
 
+				case MAV_TYPE_VTOL_DUOROTOR:
+					n = 2;
+					break;
+
+				case MAV_TYPE_VTOL_QUADROTOR:
+					n = 4;
+					break;
+
 				default:
 					n = 8;
 					break;
@@ -1482,6 +1582,7 @@ protected:
 		if (_pos_sp_triplet_sub->update(&pos_sp_triplet)) {
 			mavlink_position_target_global_int_t msg{};
 
+			msg.time_boot_ms = hrt_absolute_time()/1000;
 			msg.coordinate_frame = MAV_FRAME_GLOBAL;
 			msg.lat_int = pos_sp_triplet.current.lat * 1e7;
 			msg.lon_int = pos_sp_triplet.current.lon * 1e7;
@@ -1760,6 +1861,9 @@ protected:
 				case RC_INPUT_SOURCE_PX4IO_ST24:
 					msg.rssi |= (3 << 4);
 					break;
+				case RC_INPUT_SOURCE_UNKNOWN:
+					// do nothing
+					break;
 			}
 
 			if (rc.rc_lost) {
@@ -1834,33 +1938,32 @@ protected:
 	}
 };
 
-
-class MavlinkStreamOpticalFlow : public MavlinkStream
+class MavlinkStreamOpticalFlowRad : public MavlinkStream
 {
 public:
 	const char *get_name() const
 	{
-		return MavlinkStreamOpticalFlow::get_name_static();
+		return MavlinkStreamOpticalFlowRad::get_name_static();
 	}
 
 	static const char *get_name_static()
 	{
-		return "OPTICAL_FLOW";
+		return "OPTICAL_FLOW_RAD";
 	}
 
 	uint8_t get_id()
 	{
-		return MAVLINK_MSG_ID_OPTICAL_FLOW;
+		return MAVLINK_MSG_ID_OPTICAL_FLOW_RAD;
 	}
 
 	static MavlinkStream *new_instance(Mavlink *mavlink)
 	{
-		return new MavlinkStreamOpticalFlow(mavlink);
+		return new MavlinkStreamOpticalFlowRad(mavlink);
 	}
 
 	unsigned get_size()
 	{
-		return _flow_sub->is_published() ? (MAVLINK_MSG_ID_OPTICAL_FLOW_LEN + MAVLINK_NUM_NON_PAYLOAD_BYTES) : 0;
+		return _flow_sub->is_published() ? (MAVLINK_MSG_ID_OPTICAL_FLOW_RAD_LEN + MAVLINK_NUM_NON_PAYLOAD_BYTES) : 0;
 	}
 
 private:
@@ -1868,11 +1971,11 @@ private:
 	uint64_t _flow_time;
 
 	/* do not allow top copying this class */
-	MavlinkStreamOpticalFlow(MavlinkStreamOpticalFlow &);
-	MavlinkStreamOpticalFlow& operator = (const MavlinkStreamOpticalFlow &);
+	MavlinkStreamOpticalFlowRad(MavlinkStreamOpticalFlowRad &);
+	MavlinkStreamOpticalFlowRad& operator = (const MavlinkStreamOpticalFlowRad &);
 
 protected:
-	explicit MavlinkStreamOpticalFlow(Mavlink *mavlink) : MavlinkStream(mavlink),
+	explicit MavlinkStreamOpticalFlowRad(Mavlink *mavlink) : MavlinkStream(mavlink),
 		_flow_sub(_mavlink->add_orb_subscription(ORB_ID(optical_flow))),
 		_flow_time(0)
 	{}
@@ -1882,18 +1985,23 @@ protected:
 		struct optical_flow_s flow;
 
 		if (_flow_sub->update(&_flow_time, &flow)) {
-			mavlink_optical_flow_t msg;
+			mavlink_optical_flow_rad_t msg;
 
 			msg.time_usec = flow.timestamp;
 			msg.sensor_id = flow.sensor_id;
-			msg.flow_x = flow.flow_raw_x;
-			msg.flow_y = flow.flow_raw_y;
-			msg.flow_comp_m_x = flow.flow_comp_x_m;
-			msg.flow_comp_m_y = flow.flow_comp_y_m;
+			msg.integrated_x = flow.pixel_flow_x_integral;
+			msg.integrated_y = flow.pixel_flow_y_integral;
+			msg.integrated_xgyro = flow.gyro_x_rate_integral;
+			msg.integrated_ygyro = flow.gyro_y_rate_integral;
+			msg.integrated_zgyro = flow.gyro_z_rate_integral;
+			msg.distance = flow.ground_distance_m;
 			msg.quality = flow.quality;
-			msg.ground_distance = flow.ground_distance_m;
+			msg.integration_time_us = flow.integration_timespan;
+			msg.sensor_id = flow.sensor_id;
+			msg.time_delta_distance_us = flow.time_since_last_sonar_update;
+			msg.temperature = flow.gyro_temperature;
 
-			_mavlink->send_message(MAVLINK_MSG_ID_OPTICAL_FLOW, &msg);
+			_mavlink->send_message(MAVLINK_MSG_ID_OPTICAL_FLOW_RAD, &msg);
 		}
 	}
 };
@@ -2165,7 +2273,7 @@ protected:
 			msg.id = 0;
 			msg.orientation = 0;
 			msg.min_distance = range.minimum_distance * 100;
-			msg.max_distance = range.minimum_distance * 100;
+			msg.max_distance = range.maximum_distance * 100;
 			msg.current_distance = range.distance * 100;
 			msg.covariance = 20;
 
@@ -2185,6 +2293,8 @@ StreamListItem *streams_list[] = {
 	new StreamListItem(&MavlinkStreamAttitudeQuaternion::new_instance, &MavlinkStreamAttitudeQuaternion::get_name_static),
 	new StreamListItem(&MavlinkStreamVFRHUD::new_instance, &MavlinkStreamVFRHUD::get_name_static),
 	new StreamListItem(&MavlinkStreamGPSRawInt::new_instance, &MavlinkStreamGPSRawInt::get_name_static),
+	new StreamListItem(&MavlinkStreamSystemTime::new_instance, &MavlinkStreamSystemTime::get_name_static),
+	new StreamListItem(&MavlinkStreamTimesync::new_instance, &MavlinkStreamTimesync::get_name_static),
 	new StreamListItem(&MavlinkStreamGlobalPositionInt::new_instance, &MavlinkStreamGlobalPositionInt::get_name_static),
 	new StreamListItem(&MavlinkStreamLocalPositionNED::new_instance, &MavlinkStreamLocalPositionNED::get_name_static),
 	new StreamListItem(&MavlinkStreamViconPositionEstimate::new_instance, &MavlinkStreamViconPositionEstimate::get_name_static),
@@ -2199,7 +2309,7 @@ StreamListItem *streams_list[] = {
 	new StreamListItem(&MavlinkStreamAttitudeTarget::new_instance, &MavlinkStreamAttitudeTarget::get_name_static),
 	new StreamListItem(&MavlinkStreamRCChannelsRaw::new_instance, &MavlinkStreamRCChannelsRaw::get_name_static),
 	new StreamListItem(&MavlinkStreamManualControl::new_instance, &MavlinkStreamManualControl::get_name_static),
-	new StreamListItem(&MavlinkStreamOpticalFlow::new_instance, &MavlinkStreamOpticalFlow::get_name_static),
+	new StreamListItem(&MavlinkStreamOpticalFlowRad::new_instance, &MavlinkStreamOpticalFlowRad::get_name_static),
 	new StreamListItem(&MavlinkStreamAttitudeControls::new_instance, &MavlinkStreamAttitudeControls::get_name_static),
 	new StreamListItem(&MavlinkStreamNamedValueFloat::new_instance, &MavlinkStreamNamedValueFloat::get_name_static),
 	new StreamListItem(&MavlinkStreamCameraCapture::new_instance, &MavlinkStreamCameraCapture::get_name_static),
diff --git a/src/modules/mavlink/mavlink_mission.cpp b/src/modules/mavlink/mavlink_mission.cpp
index a3c127cdc..442d36dfb 100644
--- a/src/modules/mavlink/mavlink_mission.cpp
+++ b/src/modules/mavlink/mavlink_mission.cpp
@@ -120,13 +120,11 @@ MavlinkMissionManager::init_offboard_mission()
 		_count = mission_state.count;
 		_current_seq = mission_state.current_seq;
 
-		warnx("offboard mission init: dataman_id=%d, count=%u, current_seq=%d", _dataman_id, _count, _current_seq);
-
 	} else {
 		_dataman_id = 0;
 		_count = 0;
 		_current_seq = 0;
-		warnx("offboard mission init: ERROR, reading mission state failed");
+		warnx("offboard mission init: ERROR");
 	}
 }
 
@@ -292,9 +290,6 @@ MavlinkMissionManager::send_mission_item_reached(uint16_t seq)
 void
 MavlinkMissionManager::send(const hrt_abstime now)
 {
-	/* update interval for slow rate limiter */
-	_slow_rate_limiter.set_interval(_interval * 10 / _mavlink->get_rate_mult());
-
 	bool updated = false;
 	orb_check(_mission_result_sub, &updated);
 
@@ -312,6 +307,12 @@ MavlinkMissionManager::send(const hrt_abstime now)
 
 		send_mission_current(_current_seq);
 
+		if (mission_result.item_do_jump_changed) {
+			/* send a mission item again if the remaining DO_JUMPs has changed */
+			send_mission_item(_transfer_partner_sysid, _transfer_partner_compid,
+					  (uint16_t)mission_result.item_changed_index);
+		}
+
 	} else {
 		if (_slow_rate_limiter.check(now)) {
 			send_mission_current(_current_seq);
@@ -811,7 +812,7 @@ MavlinkMissionManager::format_mavlink_mission_item(const struct mission_item_s *
 
 	case NAV_CMD_DO_JUMP:
 		mavlink_mission_item->param1 = mission_item->do_jump_mission_index;
-		mavlink_mission_item->param2 = mission_item->do_jump_repeat_count;
+		mavlink_mission_item->param2 = mission_item->do_jump_repeat_count - mission_item->do_jump_current_count;
 		break;
 
 	default:
diff --git a/src/modules/mavlink/mavlink_orb_subscription.cpp b/src/modules/mavlink/mavlink_orb_subscription.cpp
index 734f0903a..315776e29 100644
--- a/src/modules/mavlink/mavlink_orb_subscription.cpp
+++ b/src/modules/mavlink/mavlink_orb_subscription.cpp
@@ -46,10 +46,11 @@
 
 #include "mavlink_orb_subscription.h"
 
-MavlinkOrbSubscription::MavlinkOrbSubscription(const orb_id_t topic) :
+MavlinkOrbSubscription::MavlinkOrbSubscription(const orb_id_t topic, int instance) :
 	next(nullptr),
 	_topic(topic),
-	_fd(orb_subscribe(_topic)),
+	_instance(instance),
+	_fd(orb_subscribe_multi(_topic, instance)),
 	_published(false)
 {
 }
@@ -65,6 +66,12 @@ MavlinkOrbSubscription::get_topic() const
 	return _topic;
 }
 
+int
+MavlinkOrbSubscription::get_instance() const
+{
+	return _instance;
+}
+
 bool
 MavlinkOrbSubscription::update(uint64_t *time, void* data)
 {
diff --git a/src/modules/mavlink/mavlink_orb_subscription.h b/src/modules/mavlink/mavlink_orb_subscription.h
index 7af454df6..5394e5097 100644
--- a/src/modules/mavlink/mavlink_orb_subscription.h
+++ b/src/modules/mavlink/mavlink_orb_subscription.h
@@ -50,7 +50,7 @@ class MavlinkOrbSubscription
 public:
 	MavlinkOrbSubscription *next;	///< pointer to next subscription in list
 
-	MavlinkOrbSubscription(const orb_id_t topic);
+	MavlinkOrbSubscription(const orb_id_t topic, int instance);
 	~MavlinkOrbSubscription();
 
 	/**
@@ -77,9 +77,11 @@ public:
 	 */
 	bool is_published();
 	orb_id_t get_topic() const;
+	int get_instance() const;
 
 private:
 	const orb_id_t _topic;		///< topic metadata
+	const int _instance;		///< get topic instance
 	int _fd;			///< subscription handle
 	bool _published;		///< topic was ever published
 
diff --git a/src/modules/mavlink/mavlink_parameters.cpp b/src/modules/mavlink/mavlink_parameters.cpp
index cd5f53d88..e9858b73c 100644
--- a/src/modules/mavlink/mavlink_parameters.cpp
+++ b/src/modules/mavlink/mavlink_parameters.cpp
@@ -44,7 +44,9 @@
 #include "mavlink_main.h"
 
 MavlinkParametersManager::MavlinkParametersManager(Mavlink *mavlink) : MavlinkStream(mavlink),
-	_send_all_index(-1)
+	_send_all_index(-1),
+	_rc_param_map_pub(-1),
+	_rc_param_map()
 {
 }
 
@@ -135,6 +137,43 @@ MavlinkParametersManager::handle_message(const mavlink_message_t *msg)
 			break;
 		}
 
+	case MAVLINK_MSG_ID_PARAM_MAP_RC: {
+			/* map a rc channel to a parameter */
+			mavlink_param_map_rc_t map_rc;
+			mavlink_msg_param_map_rc_decode(msg, &map_rc);
+
+			if (map_rc.target_system == mavlink_system.sysid &&
+			    (map_rc.target_component == mavlink_system.compid ||
+			     map_rc.target_component == MAV_COMP_ID_ALL)) {
+
+				/* Copy values from msg to uorb using the parameter_rc_channel_index as index */
+				size_t i = map_rc.parameter_rc_channel_index;
+				_rc_param_map.param_index[i] = map_rc.param_index;
+				strncpy(&(_rc_param_map.param_id[i][0]), map_rc.param_id, MAVLINK_MSG_PARAM_VALUE_FIELD_PARAM_ID_LEN);
+				/* enforce null termination */
+				_rc_param_map.param_id[i][MAVLINK_MSG_PARAM_MAP_RC_FIELD_PARAM_ID_LEN] = '\0';
+				_rc_param_map.scale[i] = map_rc.scale;
+				_rc_param_map.value0[i] = map_rc.param_value0;
+				_rc_param_map.value_min[i] = map_rc.param_value_min;
+				_rc_param_map.value_max[i] = map_rc.param_value_max;
+				if (map_rc.param_index == -2) { // -2 means unset map
+					_rc_param_map.valid[i] = false;
+				} else {
+					_rc_param_map.valid[i] = true;
+				}
+				_rc_param_map.timestamp = hrt_absolute_time();
+
+				if (_rc_param_map_pub < 0) {
+					_rc_param_map_pub = orb_advertise(ORB_ID(rc_parameter_map), &_rc_param_map);
+
+				} else {
+					orb_publish(ORB_ID(rc_parameter_map), _rc_param_map_pub, &_rc_param_map);
+				}
+
+			}
+			break;
+		}
+
 	default:
 		break;
 	}
diff --git a/src/modules/mavlink/mavlink_parameters.h b/src/modules/mavlink/mavlink_parameters.h
index 5576e6b84..b6736f212 100644
--- a/src/modules/mavlink/mavlink_parameters.h
+++ b/src/modules/mavlink/mavlink_parameters.h
@@ -44,6 +44,8 @@
 
 #include "mavlink_bridge_header.h"
 #include "mavlink_stream.h"
+#include <uORB/uORB.h>
+#include <uORB/topics/rc_parameter_map.h>
 
 class MavlinkParametersManager : public MavlinkStream
 {
@@ -112,4 +114,7 @@ protected:
 	void send(const hrt_abstime t);
 
 	void send_param(param_t param);
+
+	orb_advert_t _rc_param_map_pub;
+	struct rc_parameter_map_s _rc_param_map;
 };
diff --git a/src/modules/mavlink/mavlink_receiver.cpp b/src/modules/mavlink/mavlink_receiver.cpp
index 8b216d262..16d0422c7 100644
--- a/src/modules/mavlink/mavlink_receiver.cpp
+++ b/src/modules/mavlink/mavlink_receiver.cpp
@@ -1,6 +1,6 @@
 /****************************************************************************
  *
- *   Copyright (c) 2012-2014 PX4 Development Team. All rights reserved.
+ *   Copyright (c) 2012-2015 PX4 Development Team. All rights reserved.
  *
  * Redistribution and use in source and binary forms, with or without
  * modification, are permitted provided that the following conditions
@@ -67,6 +67,8 @@
 
 #include <mathlib/mathlib.h>
 
+#include <conversion/rotation.h>
+
 #include <systemlib/param/param.h>
 #include <systemlib/systemlib.h>
 #include <systemlib/err.h>
@@ -89,6 +91,7 @@ MavlinkReceiver::MavlinkReceiver(Mavlink *parent) :
 	_mavlink(parent),
 	status{},
 	hil_local_pos{},
+	hil_land_detector{},
 	_control_mode{},
 	_global_pos_pub(-1),
 	_local_pos_pub(-1),
@@ -115,12 +118,15 @@ MavlinkReceiver::MavlinkReceiver(Mavlink *parent) :
 	_telemetry_status_pub(-1),
 	_rc_pub(-1),
 	_manual_pub(-1),
+	_land_detector_pub(-1),
 	_control_mode_sub(orb_subscribe(ORB_ID(vehicle_control_mode))),
 	_hil_frames(0),
 	_old_timestamp(0),
 	_hil_local_proj_inited(0),
 	_hil_local_alt0(0.0f),
-	_hil_local_proj_ref{}
+	_hil_local_proj_ref{},
+	_time_offset_avg_alpha(0.6),
+	_time_offset(0)
 {
 
 	// make sure the FTP server is started
@@ -143,8 +149,8 @@ MavlinkReceiver::handle_message(mavlink_message_t *msg)
 		handle_message_command_int(msg);
 		break;
 
-	case MAVLINK_MSG_ID_OPTICAL_FLOW:
-		handle_message_optical_flow(msg);
+	case MAVLINK_MSG_ID_OPTICAL_FLOW_RAD:
+		handle_message_optical_flow_rad(msg);
 		break;
 
 	case MAVLINK_MSG_ID_SET_MODE:
@@ -187,6 +193,14 @@ MavlinkReceiver::handle_message(mavlink_message_t *msg)
 		MavlinkFTP::get_server()->handle_message(_mavlink, msg);
 		break;
 
+	case MAVLINK_MSG_ID_SYSTEM_TIME:
+		handle_message_system_time(msg);
+		break;
+
+	case MAVLINK_MSG_ID_TIMESYNC:
+		handle_message_timesync(msg);
+		break;
+
 	default:
 		break;
 	}
@@ -250,7 +264,7 @@ MavlinkReceiver::handle_message_command_long(mavlink_message_t *msg)
 		//check for MAVLINK terminate command
 		if (cmd_mavlink.command == MAV_CMD_PREFLIGHT_REBOOT_SHUTDOWN && ((int)cmd_mavlink.param1) == 3) {
 			/* This is the link shutdown command, terminate mavlink */
-			warnx("terminated by remote command");
+			warnx("terminated by remote");
 			fflush(stdout);
 			usleep(50000);
 
@@ -260,7 +274,7 @@ MavlinkReceiver::handle_message_command_long(mavlink_message_t *msg)
 		} else {
 
 			if (msg->sysid == mavlink_system.sysid && msg->compid == mavlink_system.compid) {
-				warnx("ignoring CMD spoofed with same SYS/COMP (%d/%d) ID",
+				warnx("ignoring CMD with same SYS/COMP (%d/%d) ID",
 				      mavlink_system.sysid, mavlink_system.compid);
 				return;
 			}
@@ -306,7 +320,7 @@ MavlinkReceiver::handle_message_command_int(mavlink_message_t *msg)
 		//check for MAVLINK terminate command
 		if (cmd_mavlink.command == MAV_CMD_PREFLIGHT_REBOOT_SHUTDOWN && ((int)cmd_mavlink.param1) == 3) {
 			/* This is the link shutdown command, terminate mavlink */
-			warnx("terminated by remote command");
+			warnx("terminated by remote");
 			fflush(stdout);
 			usleep(50000);
 
@@ -316,7 +330,7 @@ MavlinkReceiver::handle_message_command_int(mavlink_message_t *msg)
 		} else {
 
 			if (msg->sysid == mavlink_system.sysid && msg->compid == mavlink_system.compid) {
-				warnx("ignoring CMD spoofed with same SYS/COMP (%d/%d) ID",
+				warnx("ignoring CMD with same SYS/COMP (%d/%d) ID",
 				      mavlink_system.sysid, mavlink_system.compid);
 				return;
 			}
@@ -351,24 +365,34 @@ MavlinkReceiver::handle_message_command_int(mavlink_message_t *msg)
 }
 
 void
-MavlinkReceiver::handle_message_optical_flow(mavlink_message_t *msg)
+MavlinkReceiver::handle_message_optical_flow_rad(mavlink_message_t *msg)
 {
 	/* optical flow */
-	mavlink_optical_flow_t flow;
-	mavlink_msg_optical_flow_decode(msg, &flow);
+	mavlink_optical_flow_rad_t flow;
+	mavlink_msg_optical_flow_rad_decode(msg, &flow);
+
+	enum Rotation flow_rot;
+	param_get(param_find("SENS_FLOW_ROT"),&flow_rot);
 
 	struct optical_flow_s f;
 	memset(&f, 0, sizeof(f));
 
-	f.timestamp = hrt_absolute_time();
-	f.flow_timestamp = flow.time_usec;
-	f.flow_raw_x = flow.flow_x;
-	f.flow_raw_y = flow.flow_y;
-	f.flow_comp_x_m = flow.flow_comp_m_x;
-	f.flow_comp_y_m = flow.flow_comp_m_y;
-	f.ground_distance_m = flow.ground_distance;
+	f.timestamp = flow.time_usec;
+	f.integration_timespan = flow.integration_time_us;
+	f.pixel_flow_x_integral = flow.integrated_x;
+	f.pixel_flow_y_integral = flow.integrated_y;
+	f.gyro_x_rate_integral = flow.integrated_xgyro;
+	f.gyro_y_rate_integral = flow.integrated_ygyro;
+	f.gyro_z_rate_integral = flow.integrated_zgyro;
+	f.time_since_last_sonar_update = flow.time_delta_distance_us;
+	f.ground_distance_m = flow.distance;
 	f.quality = flow.quality;
 	f.sensor_id = flow.sensor_id;
+	f.gyro_temperature = flow.temperature;
+
+	/* rotate measurements according to parameter */
+	float zeroval = 0.0f;
+	rotate_3f(flow_rot, f.pixel_flow_x_integral, f.pixel_flow_y_integral, zeroval);
 
 	if (_flow_pub < 0) {
 		_flow_pub = orb_advertise(ORB_ID(optical_flow), &f);
@@ -388,13 +412,18 @@ MavlinkReceiver::handle_message_hil_optical_flow(mavlink_message_t *msg)
 	struct optical_flow_s f;
 	memset(&f, 0, sizeof(f));
 
-	f.timestamp = hrt_absolute_time();
-	f.flow_timestamp = flow.time_usec;
-	f.flow_raw_x = flow.integrated_x;
-	f.flow_raw_y = flow.integrated_y;
+	f.timestamp = hrt_absolute_time(); // XXX we rely on the system time for now and not flow.time_usec;
+	f.integration_timespan = flow.integration_time_us;
+	f.pixel_flow_x_integral = flow.integrated_x;
+	f.pixel_flow_y_integral = flow.integrated_y;
+	f.gyro_x_rate_integral = flow.integrated_xgyro;
+	f.gyro_y_rate_integral = flow.integrated_ygyro;
+	f.gyro_z_rate_integral = flow.integrated_zgyro;
+	f.time_since_last_sonar_update = flow.time_delta_distance_us;
 	f.ground_distance_m = flow.distance;
 	f.quality = flow.quality;
 	f.sensor_id = flow.sensor_id;
+	f.gyro_temperature = flow.temperature;
 
 	if (_flow_pub < 0) {
 		_flow_pub = orb_advertise(ORB_ID(optical_flow), &f);
@@ -405,7 +434,7 @@ MavlinkReceiver::handle_message_hil_optical_flow(mavlink_message_t *msg)
 
 	/* Use distance value for range finder report */
 	struct range_finder_report r;
-	memset(&r, 0, sizeof(f));
+	memset(&r, 0, sizeof(r));
 
 	r.timestamp = hrt_absolute_time();
 	r.error_count = 0;
@@ -537,12 +566,16 @@ MavlinkReceiver::handle_message_set_position_target_local_ned(mavlink_message_t
 			offboard_control_sp.ignore &=  ~(1 << i);
 			offboard_control_sp.ignore |=  (set_position_target_local_ned.type_mask & (1 << i));
 		}
+
 		offboard_control_sp.ignore &=  ~(1 << OFB_IGN_BIT_YAW);
-			offboard_control_sp.ignore |=  (set_position_target_local_ned.type_mask & (1 << 10)) <<
-					OFB_IGN_BIT_YAW;
+		if (set_position_target_local_ned.type_mask & (1 << 10)) {
+			offboard_control_sp.ignore |=  (1 << OFB_IGN_BIT_YAW);
+		}
+
 		offboard_control_sp.ignore &=  ~(1 << OFB_IGN_BIT_YAWRATE);
-			offboard_control_sp.ignore |=  (set_position_target_local_ned.type_mask & (1 << 11)) <<
-					OFB_IGN_BIT_YAWRATE;
+		if (set_position_target_local_ned.type_mask & (1 << 11)) {
+			offboard_control_sp.ignore |=  (1 << OFB_IGN_BIT_YAWRATE);
+		}
 
 		offboard_control_sp.timestamp = hrt_absolute_time();
 
@@ -676,7 +709,7 @@ MavlinkReceiver::handle_message_vision_position_estimate(mavlink_message_t *msg)
 	// Use the component ID to identify the vision sensor
 	vision_position.id = msg->compid;
 
-	vision_position.timestamp_boot = hrt_absolute_time();
+	vision_position.timestamp_boot = to_hrt(pos.usec); // Synced time
 	vision_position.timestamp_computer = pos.usec;
 	vision_position.x = pos.x;
 	vision_position.y = pos.y;
@@ -848,7 +881,7 @@ MavlinkReceiver::handle_message_manual_control(mavlink_message_t *msg)
 	manual.r = man.r / 1000.0f;
 	manual.z = man.z / 1000.0f;
 
-	warnx("pitch: %.2f, roll: %.2f, yaw: %.2f, throttle: %.2f", (double)manual.x, (double)manual.y, (double)manual.r, (double)manual.z);
+	// warnx("pitch: %.2f, roll: %.2f, yaw: %.2f, throttle: %.2f", (double)manual.x, (double)manual.y, (double)manual.r, (double)manual.z);
 
 	if (_manual_pub < 0) {
 		_manual_pub = orb_advertise(ORB_ID(manual_control_setpoint), &manual);
@@ -906,6 +939,66 @@ MavlinkReceiver::handle_message_request_data_stream(mavlink_message_t *msg)
 }
 
 void
+MavlinkReceiver::handle_message_system_time(mavlink_message_t *msg)
+{
+	mavlink_system_time_t time;
+	mavlink_msg_system_time_decode(msg, &time);
+
+	timespec tv;
+	clock_gettime(CLOCK_REALTIME, &tv);
+
+	// date -d @1234567890: Sat Feb 14 02:31:30 MSK 2009
+	bool onb_unix_valid = tv.tv_sec > PX4_EPOCH_SECS;
+	bool ofb_unix_valid = time.time_unix_usec > PX4_EPOCH_SECS * 1000ULL;
+
+	if (!onb_unix_valid && ofb_unix_valid) {
+		tv.tv_sec = time.time_unix_usec / 1000000ULL;
+		tv.tv_nsec = (time.time_unix_usec % 1000000ULL) * 1000ULL;
+		if(clock_settime(CLOCK_REALTIME, &tv)) {
+			warn("failed setting clock");
+		}
+		else {
+		warnx("[timesync] UTC time synced.");
+		}
+	}
+
+}
+
+void
+MavlinkReceiver::handle_message_timesync(mavlink_message_t *msg)
+{
+	mavlink_timesync_t tsync;
+	mavlink_msg_timesync_decode(msg, &tsync);
+
+	uint64_t now_ns = hrt_absolute_time() * 1000LL ;
+
+	if (tsync.tc1 == 0) {
+
+		mavlink_timesync_t rsync; // return timestamped sync message
+
+		rsync.tc1 = now_ns;
+		rsync.ts1 = tsync.ts1;
+
+		_mavlink->send_message(MAVLINK_MSG_ID_TIMESYNC, &rsync);
+
+		return;
+
+	} else if (tsync.tc1 > 0) {
+
+		int64_t offset_ns = (tsync.ts1 + now_ns - tsync.tc1*2)/2 ; 
+		int64_t dt = _time_offset - offset_ns;
+
+		if (dt > 10000000LL || dt < -10000000LL) { // 10 millisecond skew 
+			_time_offset = offset_ns;
+			warnx("[timesync] Hard setting offset.");
+		} else {
+			smooth_time_offset(offset_ns);
+		}
+	}
+
+}
+
+void
 MavlinkReceiver::handle_message_hil_sensor(mavlink_message_t *msg)
 {
 	mavlink_hil_sensor_t imu;
@@ -949,10 +1042,10 @@ MavlinkReceiver::handle_message_hil_sensor(mavlink_message_t *msg)
 		gyro.temperature = imu.temperature;
 
 		if (_gyro_pub < 0) {
-			_gyro_pub = orb_advertise(ORB_ID(sensor_gyro0), &gyro);
+			_gyro_pub = orb_advertise(ORB_ID(sensor_gyro), &gyro);
 
 		} else {
-			orb_publish(ORB_ID(sensor_gyro0), _gyro_pub, &gyro);
+			orb_publish(ORB_ID(sensor_gyro), _gyro_pub, &gyro);
 		}
 	}
 
@@ -971,10 +1064,10 @@ MavlinkReceiver::handle_message_hil_sensor(mavlink_message_t *msg)
 		accel.temperature = imu.temperature;
 
 		if (_accel_pub < 0) {
-			_accel_pub = orb_advertise(ORB_ID(sensor_accel0), &accel);
+			_accel_pub = orb_advertise(ORB_ID(sensor_accel), &accel);
 
 		} else {
-			orb_publish(ORB_ID(sensor_accel0), _accel_pub, &accel);
+			orb_publish(ORB_ID(sensor_accel), _accel_pub, &accel);
 		}
 	}
 
@@ -992,10 +1085,11 @@ MavlinkReceiver::handle_message_hil_sensor(mavlink_message_t *msg)
 		mag.z = imu.zmag;
 
 		if (_mag_pub < 0) {
-			_mag_pub = orb_advertise(ORB_ID(sensor_mag0), &mag);
+			/* publish to the first mag topic */
+			_mag_pub = orb_advertise(ORB_ID(sensor_mag), &mag);
 
 		} else {
-			orb_publish(ORB_ID(sensor_mag0), _mag_pub, &mag);
+			orb_publish(ORB_ID(sensor_mag), _mag_pub, &mag);
 		}
 	}
 
@@ -1010,10 +1104,10 @@ MavlinkReceiver::handle_message_hil_sensor(mavlink_message_t *msg)
 		baro.temperature = imu.temperature;
 
 		if (_baro_pub < 0) {
-			_baro_pub = orb_advertise(ORB_ID(sensor_baro0), &baro);
+			_baro_pub = orb_advertise(ORB_ID(sensor_baro), &baro);
 
 		} else {
-			orb_publish(ORB_ID(sensor_baro0), _baro_pub, &baro);
+			orb_publish(ORB_ID(sensor_baro), _baro_pub, &baro);
 		}
 	}
 
@@ -1097,7 +1191,7 @@ MavlinkReceiver::handle_message_hil_sensor(mavlink_message_t *msg)
 
 	/* print HIL sensors rate */
 	if ((timestamp - _old_timestamp) > 10000000) {
-		printf("receiving HIL sensors at %d hz\n", _hil_frames / 10);
+		// printf("receiving HIL sensors at %d hz\n", _hil_frames / 10);
 		_old_timestamp = timestamp;
 		_hil_frames = 0;
 	}
@@ -1115,7 +1209,7 @@ MavlinkReceiver::handle_message_hil_gps(mavlink_message_t *msg)
 	memset(&hil_gps, 0, sizeof(hil_gps));
 
 	hil_gps.timestamp_time = timestamp;
-	hil_gps.time_gps_usec = gps.time_usec;
+	hil_gps.time_utc_usec = gps.time_usec;
 
 	hil_gps.timestamp_position = timestamp;
 	hil_gps.lat = gps.lat;
@@ -1261,9 +1355,6 @@ MavlinkReceiver::handle_message_hil_state_quaternion(mavlink_message_t *msg)
 		hil_local_pos.xy_global = true;
 		hil_local_pos.z_global = true;
 
-		bool landed = (float)(hil_state.alt) / 1000.0f < (_hil_local_alt0 + 0.1f); // XXX improve?
-		hil_local_pos.landed = landed;
-
 		if (_local_pos_pub < 0) {
 			_local_pos_pub = orb_advertise(ORB_ID(vehicle_local_position), &hil_local_pos);
 
@@ -1272,6 +1363,22 @@ MavlinkReceiver::handle_message_hil_state_quaternion(mavlink_message_t *msg)
 		}
 	}
 
+	/* land detector */
+	{
+		bool landed = (float)(hil_state.alt) / 1000.0f < (_hil_local_alt0 + 0.1f); // XXX improve?
+		if(hil_land_detector.landed != landed) {
+			hil_land_detector.landed = landed;
+			hil_land_detector.timestamp = hrt_absolute_time();
+
+			if (_land_detector_pub < 0) {
+				_land_detector_pub = orb_advertise(ORB_ID(vehicle_land_detected), &hil_land_detector);
+
+			} else {
+				orb_publish(ORB_ID(vehicle_land_detected), _land_detector_pub, &hil_land_detector);
+			}
+		}
+	}
+
 	/* accelerometer */
 	{
 		struct accel_report accel;
@@ -1287,10 +1394,10 @@ MavlinkReceiver::handle_message_hil_state_quaternion(mavlink_message_t *msg)
 		accel.temperature = 25.0f;
 
 		if (_accel_pub < 0) {
-			_accel_pub = orb_advertise(ORB_ID(sensor_accel0), &accel);
+			_accel_pub = orb_advertise(ORB_ID(sensor_accel), &accel);
 
 		} else {
-			orb_publish(ORB_ID(sensor_accel0), _accel_pub, &accel);
+			orb_publish(ORB_ID(sensor_accel), _accel_pub, &accel);
 		}
 	}
 
@@ -1377,6 +1484,23 @@ void MavlinkReceiver::print_status()
 
 }
 
+uint64_t MavlinkReceiver::to_hrt(uint64_t usec)
+{
+	return usec - (_time_offset / 1000) ;
+}
+
+
+void MavlinkReceiver::smooth_time_offset(uint64_t offset_ns)
+{
+	/* alpha = 0.6 fixed for now. The closer alpha is to 1.0,
+         * the faster the moving average updates in response to
+	 * new offset samples.
+	 */
+
+ 	_time_offset = (_time_offset_avg_alpha * offset_ns) + (1.0 - _time_offset_avg_alpha) * _time_offset;
+}
+
+
 void *MavlinkReceiver::start_helper(void *context)
 {
 	MavlinkReceiver *rcv = new MavlinkReceiver((Mavlink *)context);
diff --git a/src/modules/mavlink/mavlink_receiver.h b/src/modules/mavlink/mavlink_receiver.h
index e5f2c6a73..699996860 100644
--- a/src/modules/mavlink/mavlink_receiver.h
+++ b/src/modules/mavlink/mavlink_receiver.h
@@ -50,6 +50,7 @@
 #include <uORB/topics/vehicle_gps_position.h>
 #include <uORB/topics/vehicle_global_position.h>
 #include <uORB/topics/vehicle_local_position.h>
+#include <uORB/topics/vehicle_land_detected.h>
 #include <uORB/topics/home_position.h>
 #include <uORB/topics/vehicle_status.h>
 #include <uORB/topics/offboard_control_setpoint.h>
@@ -75,6 +76,8 @@
 
 #include "mavlink_ftp.h"
 
+#define PX4_EPOCH_SECS 1234567890ULL
+
 class Mavlink;
 
 class MavlinkReceiver
@@ -112,7 +115,7 @@ private:
 	void handle_message(mavlink_message_t *msg);
 	void handle_message_command_long(mavlink_message_t *msg);
 	void handle_message_command_int(mavlink_message_t *msg);
-	void handle_message_optical_flow(mavlink_message_t *msg);
+	void handle_message_optical_flow_rad(mavlink_message_t *msg);
 	void handle_message_hil_optical_flow(mavlink_message_t *msg);
 	void handle_message_set_mode(mavlink_message_t *msg);
 	void handle_message_vicon_position_estimate(mavlink_message_t *msg);
@@ -124,14 +127,26 @@ private:
 	void handle_message_manual_control(mavlink_message_t *msg);
 	void handle_message_heartbeat(mavlink_message_t *msg);
 	void handle_message_request_data_stream(mavlink_message_t *msg);
+	void handle_message_system_time(mavlink_message_t *msg);
+	void handle_message_timesync(mavlink_message_t *msg);
 	void handle_message_hil_sensor(mavlink_message_t *msg);
 	void handle_message_hil_gps(mavlink_message_t *msg);
 	void handle_message_hil_state_quaternion(mavlink_message_t *msg);
 
 	void *receive_thread(void *arg);
 
+	/**
+	* Convert remote nsec timestamp to local hrt time (usec)
+	*/
+	uint64_t to_hrt(uint64_t nsec);
+	/**
+	* Exponential moving average filter to smooth time offset
+	*/
+	void smooth_time_offset(uint64_t offset_ns);	
+
 	mavlink_status_t status;
 	struct vehicle_local_position_s hil_local_pos;
+	struct vehicle_land_detected_s hil_land_detector;
 	struct vehicle_control_mode_s _control_mode;
 	orb_advert_t _global_pos_pub;
 	orb_advert_t _local_pos_pub;
@@ -158,12 +173,15 @@ private:
 	orb_advert_t _telemetry_status_pub;
 	orb_advert_t _rc_pub;
 	orb_advert_t _manual_pub;
+	orb_advert_t _land_detector_pub;
 	int _control_mode_sub;
 	int _hil_frames;
 	uint64_t _old_timestamp;
 	bool _hil_local_proj_inited;
 	float _hil_local_alt0;
 	struct map_projection_reference_s _hil_local_proj_ref;
+	double _time_offset_avg_alpha;
+	uint64_t _time_offset;
 
 	/* do not allow copying this class */
 	MavlinkReceiver(const MavlinkReceiver&);
diff --git a/src/modules/mavlink/mavlink_tests/module.mk b/src/modules/mavlink/mavlink_tests/module.mk
index 1cc28cce1..b46d2bd35 100644
--- a/src/modules/mavlink/mavlink_tests/module.mk
+++ b/src/modules/mavlink/mavlink_tests/module.mk
@@ -47,4 +47,6 @@ MODULE_STACKSIZE = 5000
 
 MAXOPTIMIZATION	 = -Os
 
-EXTRACXXFLAGS	= -Weffc++ -DMAVLINK_FTP_UNIT_TEST
+EXTRACXXFLAGS	= -Weffc++ -DMAVLINK_FTP_UNIT_TEST -Wno-attributes -Wno-packed
+
+EXTRACFLAGS	= -Wno-packed
diff --git a/src/modules/mavlink/module.mk b/src/modules/mavlink/module.mk
index 91fdd6154..f9d30dcbe 100644
--- a/src/modules/mavlink/module.mk
+++ b/src/modules/mavlink/module.mk
@@ -53,4 +53,6 @@ MAXOPTIMIZATION	 = -Os
 
 MODULE_STACKSIZE = 1024
 
-EXTRACXXFLAGS	= -Weffc++
+EXTRACXXFLAGS	= -Weffc++ -Wno-attributes -Wno-packed
+
+EXTRACFLAGS	= -Wno-packed
diff --git a/src/modules/mc_att_control/mc_att_control_main.cpp b/src/modules/mc_att_control/mc_att_control_main.cpp
index 19c10198c..c828f1f94 100644
--- a/src/modules/mc_att_control/mc_att_control_main.cpp
+++ b/src/modules/mc_att_control/mc_att_control_main.cpp
@@ -35,6 +35,10 @@
  * @file mc_att_control_main.cpp
  * Multicopter attitude controller.
  *
+ * Publication for the desired attitude tracking:
+ * Daniel Mellinger and Vijay Kumar. Minimum Snap Trajectory Generation and Control for Quadrotors.
+ * Int. Conf. on Robotics and Automation, Shanghai, China, May 2011.
+ *
  * @author Tobias Naegeli <naegelit@student.ethz.ch>
  * @author Lorenz Meier <lm@inf.ethz.ch>
  * @author Anton Babushkin <anton.babushkin@me.com>
@@ -66,6 +70,7 @@
 #include <uORB/topics/vehicle_rates_setpoint.h>
 #include <uORB/topics/vehicle_attitude.h>
 #include <uORB/topics/vehicle_control_mode.h>
+#include <uORB/topics/vehicle_status.h>
 #include <uORB/topics/actuator_armed.h>
 #include <uORB/topics/parameter_update.h>
 #include <systemlib/param/param.h>
@@ -96,12 +101,12 @@ public:
 	MulticopterAttitudeControl();
 
 	/**
-	 * Destructor, also kills the sensors task.
+	 * Destructor, also kills the main task
 	 */
 	~MulticopterAttitudeControl();
 
 	/**
-	 * Start the sensors task.
+	 * Start the multicopter attitude control task.
 	 *
 	 * @return		OK on success.
 	 */
@@ -109,8 +114,8 @@ public:
 
 private:
 
-	bool	_task_should_exit;		/**< if true, sensor task should exit */
-	int		_control_task;			/**< task handle for sensor task */
+	bool	_task_should_exit;		/**< if true, task_main() should exit */
+	int		_control_task;			/**< task handle */
 
 	int		_v_att_sub;				/**< vehicle attitude subscription */
 	int		_v_att_sp_sub;			/**< vehicle attitude setpoint subscription */
@@ -119,11 +124,15 @@ private:
 	int		_params_sub;			/**< parameter updates subscription */
 	int		_manual_control_sp_sub;	/**< manual control setpoint subscription */
 	int		_armed_sub;				/**< arming status subscription */
+	int		_vehicle_status_sub;	/**< vehicle status subscription */
 
 	orb_advert_t	_att_sp_pub;			/**< attitude setpoint publication */
 	orb_advert_t	_v_rates_sp_pub;		/**< rate setpoint publication */
 	orb_advert_t	_actuators_0_pub;		/**< attitude actuator controls publication */
 
+	orb_id_t _rates_sp_id;	/**< pointer to correct rates setpoint uORB metadata structure */
+	orb_id_t _actuators_id;	/**< pointer to correct actuator controls0 uORB metadata structure */
+
 	bool		_actuators_0_circuit_breaker_enabled;	/**< circuit breaker to suppress output */
 
 	struct vehicle_attitude_s			_v_att;				/**< vehicle attitude */
@@ -133,8 +142,10 @@ private:
 	struct vehicle_control_mode_s		_v_control_mode;	/**< vehicle control mode */
 	struct actuator_controls_s			_actuators;			/**< actuator controls */
 	struct actuator_armed_s				_armed;				/**< actuator arming status */
+	struct vehicle_status_s				_vehicle_status;	/**< vehicle status */
 
 	perf_counter_t	_loop_perf;			/**< loop performance counter */
+	perf_counter_t	_controller_latency_perf;
 
 	math::Vector<3>		_rates_prev;	/**< angular rates on previous step */
 	math::Vector<3>		_rates_sp;		/**< angular rates setpoint */
@@ -168,6 +179,7 @@ private:
 		param_t acro_roll_max;
 		param_t acro_pitch_max;
 		param_t acro_yaw_max;
+
 	}		_params_handles;		/**< handles for interesting parameters */
 
 	struct {
@@ -182,6 +194,7 @@ private:
 		float man_pitch_max;
 		float man_yaw_max;
 		math::Vector<3> acro_rate_max;		/**< max attitude rates in acro mode */
+
 	}		_params;
 
 	/**
@@ -230,6 +243,11 @@ private:
 	void		control_attitude_rates(float dt);
 
 	/**
+	 * Check for vehicle status updates.
+	 */
+	void		vehicle_status_poll();
+
+	/**
 	 * Shim for calling task_main from task_create.
 	 */
 	static void	task_main_trampoline(int argc, char *argv[]);
@@ -264,16 +282,20 @@ MulticopterAttitudeControl::MulticopterAttitudeControl() :
 	_params_sub(-1),
 	_manual_control_sp_sub(-1),
 	_armed_sub(-1),
+	_vehicle_status_sub(-1),
 
 /* publications */
 	_att_sp_pub(-1),
 	_v_rates_sp_pub(-1),
 	_actuators_0_pub(-1),
+	_rates_sp_id(0),
+	_actuators_id(0),
 
 	_actuators_0_circuit_breaker_enabled(false),
 
 /* performance counters */
-	_loop_perf(perf_alloc(PC_ELAPSED, "mc_att_control"))
+	_loop_perf(perf_alloc(PC_ELAPSED, "mc_att_control")),
+	_controller_latency_perf(perf_alloc_once(PC_ELAPSED, "ctrl_latency"))
 
 {
 	memset(&_v_att, 0, sizeof(_v_att));
@@ -283,6 +305,8 @@ MulticopterAttitudeControl::MulticopterAttitudeControl() :
 	memset(&_v_control_mode, 0, sizeof(_v_control_mode));
 	memset(&_actuators, 0, sizeof(_actuators));
 	memset(&_armed, 0, sizeof(_armed));
+	memset(&_vehicle_status, 0, sizeof(_vehicle_status));
+	_vehicle_status.is_rotary_wing = true;
 
 	_params.att_p.zero();
 	_params.rate_p.zero();
@@ -417,7 +441,7 @@ MulticopterAttitudeControl::parameter_update_poll()
 {
 	bool updated;
 
-	/* Check HIL state if vehicle status has changed */
+	/* Check if parameters have changed */
 	orb_check(_params_sub, &updated);
 
 	if (updated) {
@@ -432,7 +456,7 @@ MulticopterAttitudeControl::vehicle_control_mode_poll()
 {
 	bool updated;
 
-	/* Check HIL state if vehicle status has changed */
+	/* Check if vehicle control mode has changed */
 	orb_check(_v_control_mode_sub, &updated);
 
 	if (updated) {
@@ -489,6 +513,28 @@ MulticopterAttitudeControl::arming_status_poll()
 	}
 }
 
+void
+MulticopterAttitudeControl::vehicle_status_poll()
+{
+	/* check if there is new status information */
+	bool vehicle_status_updated;
+	orb_check(_vehicle_status_sub, &vehicle_status_updated);
+
+	if (vehicle_status_updated) {
+		orb_copy(ORB_ID(vehicle_status), _vehicle_status_sub, &_vehicle_status);
+		/* set correct uORB ID, depending on if vehicle is VTOL or not */
+		if (!_rates_sp_id) {
+			if (_vehicle_status.is_vtol) {
+				_rates_sp_id = ORB_ID(mc_virtual_rates_setpoint);
+				_actuators_id = ORB_ID(actuator_controls_virtual_mc);
+			} else {
+				_rates_sp_id = ORB_ID(vehicle_rates_setpoint);
+				_actuators_id = ORB_ID(actuator_controls_0);
+			}
+		}
+	}
+}
+
 /*
  * Attitude controller.
  * Input: 'manual_control_setpoint' and 'vehicle_attitude_setpoint' topics (depending on mode)
@@ -585,7 +631,7 @@ MulticopterAttitudeControl::control_attitude(float dt)
 	}
 
 	/* publish the attitude setpoint if needed */
-	if (publish_att_sp) {
+	if (publish_att_sp && _vehicle_status.is_rotary_wing) {
 		_v_att_sp.timestamp = hrt_absolute_time();
 
 		if (_att_sp_pub > 0) {
@@ -682,7 +728,7 @@ void
 MulticopterAttitudeControl::control_attitude_rates(float dt)
 {
 	/* reset integral if disarmed */
-	if (!_armed.armed) {
+	if (!_armed.armed || !_vehicle_status.is_rotary_wing) {
 		_rates_int.zero();
 	}
 
@@ -721,8 +767,6 @@ MulticopterAttitudeControl::task_main_trampoline(int argc, char *argv[])
 void
 MulticopterAttitudeControl::task_main()
 {
-	warnx("started");
-	fflush(stdout);
 
 	/*
 	 * do subscriptions
@@ -734,6 +778,7 @@ MulticopterAttitudeControl::task_main()
 	_params_sub = orb_subscribe(ORB_ID(parameter_update));
 	_manual_control_sp_sub = orb_subscribe(ORB_ID(manual_control_setpoint));
 	_armed_sub = orb_subscribe(ORB_ID(actuator_armed));
+	_vehicle_status_sub = orb_subscribe(ORB_ID(vehicle_status));
 
 	/* initialize parameters cache */
 	parameters_update();
@@ -785,6 +830,7 @@ MulticopterAttitudeControl::task_main()
 			vehicle_control_mode_poll();
 			arming_status_poll();
 			vehicle_manual_poll();
+			vehicle_status_poll();
 
 			if (_v_control_mode.flag_control_attitude_enabled) {
 				control_attitude(dt);
@@ -797,10 +843,10 @@ MulticopterAttitudeControl::task_main()
 				_v_rates_sp.timestamp = hrt_absolute_time();
 
 				if (_v_rates_sp_pub > 0) {
-					orb_publish(ORB_ID(vehicle_rates_setpoint), _v_rates_sp_pub, &_v_rates_sp);
+					orb_publish(_rates_sp_id, _v_rates_sp_pub, &_v_rates_sp);
 
-				} else {
-					_v_rates_sp_pub = orb_advertise(ORB_ID(vehicle_rates_setpoint), &_v_rates_sp);
+				} else if (_rates_sp_id) {
+					_v_rates_sp_pub = orb_advertise(_rates_sp_id, &_v_rates_sp);
 				}
 
 			} else {
@@ -821,10 +867,10 @@ MulticopterAttitudeControl::task_main()
 					_v_rates_sp.timestamp = hrt_absolute_time();
 
 					if (_v_rates_sp_pub > 0) {
-						orb_publish(ORB_ID(vehicle_rates_setpoint), _v_rates_sp_pub, &_v_rates_sp);
+						orb_publish(_rates_sp_id, _v_rates_sp_pub, &_v_rates_sp);
 
-					} else {
-						_v_rates_sp_pub = orb_advertise(ORB_ID(vehicle_rates_setpoint), &_v_rates_sp);
+					} else if (_rates_sp_id) {
+						_v_rates_sp_pub = orb_advertise(_rates_sp_id, &_v_rates_sp);
 					}
 
 				} else {
@@ -846,14 +892,17 @@ MulticopterAttitudeControl::task_main()
 				_actuators.control[2] = (isfinite(_att_control(2))) ? _att_control(2) : 0.0f;
 				_actuators.control[3] = (isfinite(_thrust_sp)) ? _thrust_sp : 0.0f;
 				_actuators.timestamp = hrt_absolute_time();
+				_actuators.timestamp_sample = _v_att.timestamp;
 
 				if (!_actuators_0_circuit_breaker_enabled) {
 					if (_actuators_0_pub > 0) {
-						orb_publish(ORB_ID(actuator_controls_0), _actuators_0_pub, &_actuators);
+						orb_publish(_actuators_id, _actuators_0_pub, &_actuators);
+						perf_end(_controller_latency_perf);
 
-					} else {
-						_actuators_0_pub = orb_advertise(ORB_ID(actuator_controls_0), &_actuators);
+					} else if (_actuators_id) {
+						_actuators_0_pub = orb_advertise(_actuators_id, &_actuators);
 					}
+
 				}
 			}
 		}
diff --git a/src/modules/mc_pos_control/mc_pos_control_main.cpp b/src/modules/mc_pos_control/mc_pos_control_main.cpp
index d52138522..cad6cf3ae 100644
--- a/src/modules/mc_pos_control/mc_pos_control_main.cpp
+++ b/src/modules/mc_pos_control/mc_pos_control_main.cpp
@@ -35,6 +35,12 @@
  * @file mc_pos_control_main.cpp
  * Multicopter position controller.
  *
+ * Original publication for the desired attitude generation:
+ * Daniel Mellinger and Vijay Kumar. Minimum Snap Trajectory Generation and Control for Quadrotors.
+ * Int. Conf. on Robotics and Automation, Shanghai, China, May 2011
+ *
+ * Also inspired by https://pixhawk.org/firmware/apps/fw_pos_control_l1
+ *
  * The controller has two loops: P loop for position error and PID loop for velocity error.
  * Output of velocity controller is thrust vector that splitted to thrust direction
  * (i.e. rotation matrix for multicopter orientation) and thrust module (i.e. multicopter thrust itself).
@@ -535,7 +541,7 @@ MulticopterPositionControl::reset_pos_sp()
 				- _params.vel_ff(0) * _sp_move_rate(0)) / _params.pos_p(0);
 		_pos_sp(1) = _pos(1) + (_vel(1) - _att_sp.R_body[1][2] * _att_sp.thrust / _params.vel_p(1)
 				- _params.vel_ff(1) * _sp_move_rate(1)) / _params.pos_p(1);
-		mavlink_log_info(_mavlink_fd, "[mpc] reset pos sp: %.2f, %.2f", (double)_pos_sp(0), (double)_pos_sp(1));
+		mavlink_log_info(_mavlink_fd, "[mpc] reset pos sp: %d, %d", (int)_pos_sp(0), (int)_pos_sp(1));
 	}
 }
 
@@ -545,7 +551,7 @@ MulticopterPositionControl::reset_alt_sp()
 	if (_reset_alt_sp) {
 		_reset_alt_sp = false;
 		_pos_sp(2) = _pos(2) + (_vel(2) - _params.vel_ff(2) * _sp_move_rate(2)) / _params.pos_p(2);
-		mavlink_log_info(_mavlink_fd, "[mpc] reset alt sp: %.2f", -(double)_pos_sp(2));
+		mavlink_log_info(_mavlink_fd, "[mpc] reset alt sp: %d", -(int)_pos_sp(2));
 	}
 }
 
@@ -652,8 +658,6 @@ MulticopterPositionControl::control_offboard(float dt)
 			/* control position */
 			_pos_sp(0) = _pos_sp_triplet.current.x;
 			_pos_sp(1) = _pos_sp_triplet.current.y;
-			_pos_sp(2) = _pos_sp_triplet.current.z;
-
 		} else if (_control_mode.flag_control_velocity_enabled && _pos_sp_triplet.current.velocity_valid) {
 			/* control velocity */
 			/* reset position setpoint to current position if needed */
@@ -670,7 +674,10 @@ MulticopterPositionControl::control_offboard(float dt)
 			_att_sp.yaw_body = _att_sp.yaw_body + _pos_sp_triplet.current.yawspeed * dt;
 		}
 
-		if (_control_mode.flag_control_altitude_enabled) {
+		if (_control_mode.flag_control_altitude_enabled && _pos_sp_triplet.current.position_valid) {
+			/* Control altitude */
+			_pos_sp(2) = _pos_sp_triplet.current.z;
+		} else if (_control_mode.flag_control_climb_rate_enabled && _pos_sp_triplet.current.velocity_valid) {
 			/* reset alt setpoint to current altitude if needed */
 			reset_alt_sp();
 
@@ -725,11 +732,18 @@ MulticopterPositionControl::control_auto(float dt)
 		reset_alt_sp();
 	}
 
+	//Poll position setpoint
 	bool updated;
 	orb_check(_pos_sp_triplet_sub, &updated);
-
 	if (updated) {
 		orb_copy(ORB_ID(position_setpoint_triplet), _pos_sp_triplet_sub, &_pos_sp_triplet);
+
+		//Make sure that the position setpoint is valid
+		if (!isfinite(_pos_sp_triplet.current.lat) || 
+			!isfinite(_pos_sp_triplet.current.lon) || 
+			!isfinite(_pos_sp_triplet.current.alt)) {
+			_pos_sp_triplet.current.valid = false;
+		}
 	}
 
 	if (_pos_sp_triplet.current.valid) {
@@ -857,10 +871,8 @@ MulticopterPositionControl::control_auto(float dt)
 void
 MulticopterPositionControl::task_main()
 {
-	warnx("started");
 
 	_mavlink_fd = open(MAVLINK_LOG_DEVICE, 0);
-	mavlink_log_info(_mavlink_fd, "[mpc] started");
 
 	/*
 	 * do subscriptions
diff --git a/src/modules/navigator/datalinkloss.cpp b/src/modules/navigator/datalinkloss.cpp
index e789fd10d..87a6e023a 100644
--- a/src/modules/navigator/datalinkloss.cpp
+++ b/src/modules/navigator/datalinkloss.cpp
@@ -155,7 +155,7 @@ DataLinkLoss::set_dll_item()
 	case DLL_STATE_TERMINATE: {
 		/* Request flight termination from the commander */
 		_navigator->get_mission_result()->flight_termination = true;
-		_navigator->publish_mission_result();
+		_navigator->set_mission_result_updated();
 		reset_mission_item_reached();
 		warnx("not switched to manual: request flight termination");
 		pos_sp_triplet->previous.valid = false;
@@ -188,7 +188,7 @@ DataLinkLoss::advance_dll()
 					_navigator->get_vstatus()->data_link_lost_counter, _param_numberdatalinklosses.get());
 			mavlink_log_info(_navigator->get_mavlink_fd(), "#audio: too many DL losses, fly to airfield home");
 			_navigator->get_mission_result()->stay_in_failsafe = true;
-			_navigator->publish_mission_result();
+			_navigator->set_mission_result_updated();
 			reset_mission_item_reached();
 			_dll_state = DLL_STATE_FLYTOAIRFIELDHOMEWP;
 		} else {
@@ -209,7 +209,7 @@ DataLinkLoss::advance_dll()
 			mavlink_log_info(_navigator->get_mavlink_fd(), "#audio: fly to airfield home");
 		_dll_state = DLL_STATE_FLYTOAIRFIELDHOMEWP;
 		_navigator->get_mission_result()->stay_in_failsafe = true;
-		_navigator->publish_mission_result();
+		_navigator->set_mission_result_updated();
 		reset_mission_item_reached();
 		break;
 	case DLL_STATE_FLYTOAIRFIELDHOMEWP:
@@ -217,7 +217,7 @@ DataLinkLoss::advance_dll()
 		warnx("time is up, state should have been changed manually by now");
 		mavlink_log_info(_navigator->get_mavlink_fd(), "#audio: no manual control, terminating");
 		_navigator->get_mission_result()->stay_in_failsafe = true;
-		_navigator->publish_mission_result();
+		_navigator->set_mission_result_updated();
 		reset_mission_item_reached();
 		break;
 	case DLL_STATE_TERMINATE:
diff --git a/src/modules/navigator/geofence.cpp b/src/modules/navigator/geofence.cpp
index 0f431ded2..9bc9be245 100644
--- a/src/modules/navigator/geofence.cpp
+++ b/src/modules/navigator/geofence.cpp
@@ -58,17 +58,17 @@
 static const int ERROR = -1;
 
 Geofence::Geofence() :
-		SuperBlock(NULL, "GF"),
-		_fence_pub(-1),
-		_altitude_min(0),
-		_altitude_max(0),
-		_verticesCount(0),
-		_param_geofence_on(this, "ON"),
-		_param_altitude_mode(this, "ALTMODE"),
-		_param_source(this, "SOURCE"),
-		_param_counter_threshold(this, "COUNT"),
-		_outside_counter(0),
-		_mavlinkFd(-1)
+	SuperBlock(NULL, "GF"),
+	_fence_pub(-1),
+	_altitude_min(0),
+	_altitude_max(0),
+	_verticesCount(0),
+	_param_geofence_on(this, "ON"),
+	_param_altitude_mode(this, "ALTMODE"),
+	_param_source(this, "SOURCE"),
+	_param_counter_threshold(this, "COUNT"),
+	_outside_counter(0),
+	_mavlinkFd(-1)
 {
 	/* Load initial params */
 	updateParams();
@@ -92,22 +92,26 @@ bool Geofence::inside(const struct vehicle_global_position_s &global_position, f
 
 
 bool Geofence::inside(const struct vehicle_global_position_s &global_position,
-			const struct vehicle_gps_position_s &gps_position,float baro_altitude_amsl) {
+		      const struct vehicle_gps_position_s &gps_position, float baro_altitude_amsl)
+{
 	updateParams();
 
 	if (getAltitudeMode() == Geofence::GF_ALT_MODE_WGS84) {
 		if (getSource() == Geofence::GF_SOURCE_GLOBALPOS) {
 			return inside(global_position);
+
 		} else {
 			return inside((double)gps_position.lat * 1.0e-7, (double)gps_position.lon * 1.0e-7,
-					(double)gps_position.alt * 1.0e-3);
+				      (double)gps_position.alt * 1.0e-3);
 		}
+
 	} else {
 		if (getSource() == Geofence::GF_SOURCE_GLOBALPOS) {
 			return inside(global_position, baro_altitude_amsl);
+
 		} else {
 			return inside((double)gps_position.lat * 1.0e-7, (double)gps_position.lon * 1.0e-7,
-					baro_altitude_amsl);
+				      baro_altitude_amsl);
 		}
 	}
 }
@@ -120,9 +124,12 @@ bool Geofence::inside(double lat, double lon, float altitude)
 		_outside_counter = 0;
 		return inside_fence;
 	} {
+
 		_outside_counter++;
-		if(_outside_counter > _param_counter_threshold.get()) {
+
+		if (_outside_counter > _param_counter_threshold.get()) {
 			return inside_fence;
+
 		} else {
 			return true;
 		}
@@ -133,8 +140,9 @@ bool Geofence::inside(double lat, double lon, float altitude)
 bool Geofence::inside_polygon(double lat, double lon, float altitude)
 {
 	/* Return true if geofence is disabled */
-	if (_param_geofence_on.get() != 1)
+	if (_param_geofence_on.get() != 1) {
 		return true;
+	}
 
 	if (valid()) {
 
@@ -159,20 +167,22 @@ bool Geofence::inside_polygon(double lat, double lon, float altitude)
 				if (dm_read(DM_KEY_FENCE_POINTS, i, &temp_vertex_i, sizeof(struct fence_vertex_s)) != sizeof(struct fence_vertex_s)) {
 					break;
 				}
+
 				if (dm_read(DM_KEY_FENCE_POINTS, j, &temp_vertex_j, sizeof(struct fence_vertex_s)) != sizeof(struct fence_vertex_s)) {
 					break;
 				}
 
 				// skip vertex 0 (return point)
 				if (((double)temp_vertex_i.lon >= lon) != ((double)temp_vertex_j.lon >= lon) &&
-							(lat <= (double)(temp_vertex_j.lat - temp_vertex_i.lat) * (lon - (double)temp_vertex_i.lon) /
-							 (double)(temp_vertex_j.lon - temp_vertex_i.lon) + (double)temp_vertex_i.lat)) {
-							c = !c;
+				    (lat <= (double)(temp_vertex_j.lat - temp_vertex_i.lat) * (lon - (double)temp_vertex_i.lon) /
+				     (double)(temp_vertex_j.lon - temp_vertex_i.lon) + (double)temp_vertex_i.lat)) {
+					c = !c;
 				}
 
 			}
 
 			return c;
+
 		} else {
 			/* Empty fence --> accept all points */
 			return true;
@@ -188,8 +198,9 @@ bool
 Geofence::valid()
 {
 	// NULL fence is valid
-	if (isEmpty())
+	if (isEmpty()) {
 		return true;
+	}
 
 	// Otherwise
 	if ((_verticesCount < 4) || (_verticesCount > GEOFENCE_MAX_VERTICES)) {
@@ -214,26 +225,33 @@ Geofence::addPoint(int argc, char *argv[])
 		return;
 	}
 
-	if (argc < 3)
+	if (argc < 3) {
 		errx(1, "Specify: -clear | sequence latitude longitude [-publish]");
+	}
 
 	ix = atoi(argv[0]);
-	if (ix >= DM_KEY_FENCE_POINTS_MAX)
+
+	if (ix >= DM_KEY_FENCE_POINTS_MAX) {
 		errx(1, "Sequence must be less than %d", DM_KEY_FENCE_POINTS_MAX);
+	}
 
 	lat = strtod(argv[1], &end);
 	lon = strtod(argv[2], &end);
 
 	last = 0;
-	if ((argc > 3) && (strcmp(argv[3], "-publish") == 0))
+
+	if ((argc > 3) && (strcmp(argv[3], "-publish") == 0)) {
 		last = 1;
+	}
 
 	vertex.lat = (float)lat;
 	vertex.lon = (float)lon;
 
 	if (dm_write(DM_KEY_FENCE_POINTS, ix, DM_PERSIST_POWER_ON_RESET, &vertex, sizeof(vertex)) == sizeof(vertex)) {
-		if (last)
+		if (last) {
 			publishFence((unsigned)ix + 1);
+		}
+
 		return;
 	}
 
@@ -243,10 +261,12 @@ Geofence::addPoint(int argc, char *argv[])
 void
 Geofence::publishFence(unsigned vertices)
 {
-	if (_fence_pub == -1)
+	if (_fence_pub == -1) {
 		_fence_pub = orb_advertise(ORB_ID(fence), &vertices);
-	else
+
+	} else {
 		orb_publish(ORB_ID(fence), _fence_pub, &vertices);
+	}
 }
 
 int
@@ -257,30 +277,39 @@ Geofence::loadFromFile(const char *filename)
 	int			pointCounter = 0;
 	bool		gotVertical = false;
 	const char commentChar = '#';
+	int rc = ERROR;
 
 	/* Make sure no data is left in the datamanager */
 	clearDm();
 
 	/* open the mixer definition file */
 	fp = fopen(GEOFENCE_FILENAME, "r");
+
 	if (fp == NULL) {
 		return ERROR;
 	}
 
 	/* create geofence points from valid lines and store in DM */
 	for (;;) {
-
 		/* get a line, bail on error/EOF */
-		if (fgets(line, sizeof(line), fp) == NULL)
+		if (fgets(line, sizeof(line), fp) == NULL) {
 			break;
+		}
 
 		/* Trim leading whitespace */
 		size_t textStart = 0;
-		while((textStart < sizeof(line)/sizeof(char)) && isspace(line[textStart])) textStart++;
+
+		while ((textStart < sizeof(line) / sizeof(char)) && isspace(line[textStart])) { textStart++; }
 
 		/* if the line starts with #, skip */
-		if (line[textStart] == commentChar)
+		if (line[textStart] == commentChar) {
 			continue;
+		}
+
+		/* if there is only a linefeed, skip it */
+		if (line[0] == '\n') {
+			continue;
+		}
 
 		if (gotVertical) {
 			/* Parse the line as a geofence point */
@@ -291,54 +320,58 @@ Geofence::loadFromFile(const char *filename)
 				/* Handle degree minute second format */
 				float lat_d, lat_m, lat_s, lon_d, lon_m, lon_s;
 
-				if (sscanf(line, "DMS %f %f %f %f %f %f", &lat_d, &lat_m, &lat_s, &lon_d, &lon_m, &lon_s) != 6)
-					return ERROR;
+				if (sscanf(line, "DMS %f %f %f %f %f %f", &lat_d, &lat_m, &lat_s, &lon_d, &lon_m, &lon_s) != 6) {
+					warnx("Scanf to parse DMS geofence vertex failed.");
+					goto error;
+				}
 
 //				warnx("Geofence DMS: %.5f %.5f %.5f ; %.5f %.5f %.5f", (double)lat_d, (double)lat_m, (double)lat_s, (double)lon_d, (double)lon_m, (double)lon_s);
 
-				vertex.lat = lat_d + lat_m/60.0f + lat_s/3600.0f;
-				vertex.lon = lon_d + lon_m/60.0f + lon_s/3600.0f;
+				vertex.lat = lat_d + lat_m / 60.0f + lat_s / 3600.0f;
+				vertex.lon = lon_d + lon_m / 60.0f + lon_s / 3600.0f;
 
 			} else {
 				/* Handle decimal degree format */
-
-				if (sscanf(line, "%f %f", &(vertex.lat), &(vertex.lon)) != 2)
-					return ERROR;
+				if (sscanf(line, "%f %f", &(vertex.lat), &(vertex.lon)) != 2) {
+					warnx("Scanf to parse geofence vertex failed.");
+					goto error;
+				}
 			}
 
-			if (dm_write(DM_KEY_FENCE_POINTS, pointCounter, DM_PERSIST_POWER_ON_RESET, &vertex, sizeof(vertex)) != sizeof(vertex))
-								return ERROR;
+			if (dm_write(DM_KEY_FENCE_POINTS, pointCounter, DM_PERSIST_POWER_ON_RESET, &vertex, sizeof(vertex)) != sizeof(vertex)) {
+				goto error;
+			}
 
-			warnx("Geofence: point: %d, lat %.5f: lon: %.5f", pointCounter,  (double)vertex.lat, (double)vertex.lon);
+			warnx("Geofence: point: %d, lat %.5f: lon: %.5f", pointCounter, (double)vertex.lat, (double)vertex.lon);
 
 			pointCounter++;
+
 		} else {
 			/* Parse the line as the vertical limits */
-			if (sscanf(line, "%f %f", &_altitude_min, &_altitude_max) != 2)
-				return ERROR;
-
+			if (sscanf(line, "%f %f", &_altitude_min, &_altitude_max) != 2) {
+				goto error;
+			}
 
 			warnx("Geofence: alt min: %.4f, alt_max: %.4f", (double)_altitude_min, (double)_altitude_max);
 			gotVertical = true;
 		}
-
-
 	}
 
-	fclose(fp);
-
 	/* Check if import was successful */
-	if(gotVertical && pointCounter > 0)
-	{
+	if (gotVertical && pointCounter > 0) {
 		_verticesCount = pointCounter;
 		warnx("Geofence: imported successfully");
 		mavlink_log_info(_mavlinkFd, "Geofence imported");
+		rc = OK;
+
 	} else {
 		warnx("Geofence: import error");
 		mavlink_log_critical(_mavlinkFd, "#audio: Geofence import error");
 	}
 
-	return ERROR;
+error:
+	fclose(fp);
+	return rc;
 }
 
 int Geofence::clearDm()
diff --git a/src/modules/navigator/gpsfailure.cpp b/src/modules/navigator/gpsfailure.cpp
index cd55f60b0..e370796c0 100644
--- a/src/modules/navigator/gpsfailure.cpp
+++ b/src/modules/navigator/gpsfailure.cpp
@@ -141,7 +141,7 @@ GpsFailure::set_gpsf_item()
 	case GPSF_STATE_TERMINATE: {
 		/* Request flight termination from the commander */
 		_navigator->get_mission_result()->flight_termination = true;
-		_navigator->publish_mission_result();
+		_navigator->set_mission_result_updated();
 		warnx("gps fail: request flight termination");
 	}
 	default:
diff --git a/src/modules/navigator/mission.cpp b/src/modules/navigator/mission.cpp
index 0765e9b7c..b52b12ce7 100644
--- a/src/modules/navigator/mission.cpp
+++ b/src/modules/navigator/mission.cpp
@@ -38,6 +38,8 @@
  * @author Julian Oes <julian@oes.ch>
  * @author Thomas Gubler <thomasgubler@gmail.com>
  * @author Anton Babushkin <anton.babushkin@me.com>
+ * @author Ban Siesta <bansiesta@gmail.com>
+ * @author Simon Wilks <simon@uaventure.com>
  */
 
 #include <sys/types.h>
@@ -67,6 +69,7 @@ Mission::Mission(Navigator *navigator, const char *name) :
 	_param_takeoff_alt(this, "MIS_TAKEOFF_ALT", false),
 	_param_dist_1wp(this, "MIS_DIST_1WP", false),
 	_param_altmode(this, "MIS_ALTMODE", false),
+	_param_yawmode(this, "MIS_YAWMODE", false),
 	_onboard_mission({0}),
 	_offboard_mission({0}),
 	_current_onboard_mission_index(-1),
@@ -79,6 +82,7 @@ Mission::Mission(Navigator *navigator, const char *name) :
 	_missionFeasiblityChecker(),
 	_min_current_sp_distance_xy(FLT_MAX),
 	_mission_item_previous_alt(NAN),
+  	_on_arrival_yaw(NAN),
 	_distance_current_previous(0.0f)
 {
 	/* load initial params */
@@ -149,18 +153,12 @@ Mission::on_active()
 
 	/* lets check if we reached the current mission item */
 	if (_mission_type != MISSION_TYPE_NONE && is_mission_item_reached()) {
+		set_mission_item_reached();
 		if (_mission_item.autocontinue) {
 			/* switch to next waypoint if 'autocontinue' flag set */
 			advance_mission();
 			set_mission_items();
 
-		} else {
-			/* else just report that item reached */
-			if (_mission_type == MISSION_TYPE_OFFBOARD) {
-				if (!(_navigator->get_mission_result()->seq_reached == _current_offboard_mission_index && _navigator->get_mission_result()->reached)) {
-					set_mission_item_reached();
-				}
-			}
 		}
 
 	} else if (_mission_type != MISSION_TYPE_NONE &&_param_altmode.get() == MISSION_ALTMODE_FOH) {
@@ -171,6 +169,13 @@ Mission::on_active()
 			_navigator->set_can_loiter_at_sp(true);
 		}
 	}
+
+	/* see if we need to update the current yaw heading for rotary wing types */
+	if (_navigator->get_vstatus()->is_rotary_wing 
+			&& _param_yawmode.get() != MISSION_YAWMODE_NONE
+			&& _mission_type != MISSION_TYPE_NONE) {
+		heading_sp_update();
+	}
 }
 
 void
@@ -280,7 +285,7 @@ Mission::check_dist_1wp()
 						&mission_item, sizeof(mission_item_s)) == sizeof(mission_item_s)) {
 
 					/* check only items with valid lat/lon */
-					if (	mission_item.nav_cmd == NAV_CMD_WAYPOINT ||
+					if ( mission_item.nav_cmd == NAV_CMD_WAYPOINT ||
 							mission_item.nav_cmd == NAV_CMD_LOITER_TIME_LIMIT ||
 							mission_item.nav_cmd == NAV_CMD_LOITER_TURN_COUNT ||
 							mission_item.nav_cmd == NAV_CMD_LOITER_UNLIMITED ||
@@ -367,7 +372,6 @@ Mission::set_mission_items()
 			mavlink_log_critical(_navigator->get_mavlink_fd(), "offboard mission now running");
 		}
 		_mission_type = MISSION_TYPE_OFFBOARD;
-
 	} else {
 		/* no mission available or mission finished, switch to loiter */
 		if (_mission_type != MISSION_TYPE_NONE) {
@@ -395,13 +399,16 @@ Mission::set_mission_items()
 		/* reuse setpoint for LOITER only if it's not IDLE */
 		_navigator->set_can_loiter_at_sp(pos_sp_triplet->current.type == SETPOINT_TYPE_LOITER);
 
-		reset_mission_item_reached();
 		set_mission_finished();
 
 		_navigator->set_position_setpoint_triplet_updated();
 		return;
 	}
 
+	if (pos_sp_triplet->current.valid) {
+		_on_arrival_yaw = _mission_item.yaw;
+	}
+
 	/* do takeoff on first waypoint for rotary wing vehicles */
 	if (_navigator->get_vstatus()->is_rotary_wing) {
 		/* force takeoff if landed (additional protection) */
@@ -448,6 +455,7 @@ Mission::set_mission_items()
 			_mission_item.nav_cmd = NAV_CMD_TAKEOFF;
 			_mission_item.lat = _navigator->get_global_position()->lat;
 			_mission_item.lon = _navigator->get_global_position()->lon;
+			_mission_item.yaw = NAN;
 			_mission_item.altitude = takeoff_alt;
 			_mission_item.altitude_is_relative = false;
 			_mission_item.autocontinue = true;
@@ -487,7 +495,6 @@ Mission::set_mission_items()
 		if (read_mission_item(_mission_type == MISSION_TYPE_ONBOARD, false, &mission_item_next)) {
 			/* got next mission item, update setpoint triplet */
 			mission_item_to_position_setpoint(&mission_item_next, &pos_sp_triplet->next);
-
 		} else {
 			/* next mission item is not available */
 			pos_sp_triplet->next.valid = false;
@@ -510,6 +517,59 @@ Mission::set_mission_items()
 }
 
 void
+Mission::heading_sp_update()
+{
+	if (_takeoff) {
+		/* we don't want to be yawing during takeoff */
+		return;
+	}
+
+	struct position_setpoint_triplet_s *pos_sp_triplet = _navigator->get_position_setpoint_triplet();
+
+	/* Don't change setpoint if last and current waypoint are not valid */
+	if (!pos_sp_triplet->previous.valid || !pos_sp_triplet->current.valid ||
+			!isfinite(_on_arrival_yaw)) {
+		return;
+	}
+
+	/* Don't do FOH for landing and takeoff waypoints, the ground may be near
+	 * and the FW controller has a custom landing logic */
+	if (_mission_item.nav_cmd == NAV_CMD_LAND || _mission_item.nav_cmd == NAV_CMD_TAKEOFF) {
+		return;
+	}
+
+	/* set yaw angle for the waypoint iff a loiter time has been specified */
+	if (_waypoint_position_reached && _mission_item.time_inside > 0.0f) {
+		_mission_item.yaw = _on_arrival_yaw;
+	/* always keep the front of the rotary wing pointing to the next waypoint */
+	} else if (_param_yawmode.get() == MISSION_YAWMODE_FRONT_TO_WAYPOINT) {
+		_mission_item.yaw = get_bearing_to_next_waypoint(
+		        _navigator->get_global_position()->lat,
+		        _navigator->get_global_position()->lon,
+		        _mission_item.lat,
+		        _mission_item.lon);
+	/* always keep the back of the rotary wing pointing towards home */
+	} else if (_param_yawmode.get() == MISSION_YAWMODE_FRONT_TO_HOME) {
+		_mission_item.yaw = get_bearing_to_next_waypoint(
+		        _navigator->get_global_position()->lat,
+		        _navigator->get_global_position()->lon,
+		        _navigator->get_home_position()->lat,
+		        _navigator->get_home_position()->lon);
+	/* always keep the back of the rotary wing pointing towards home */
+	} else if (_param_yawmode.get() == MISSION_YAWMODE_BACK_TO_HOME) {
+		_mission_item.yaw = _wrap_pi(get_bearing_to_next_waypoint(
+		        _navigator->get_global_position()->lat,
+		        _navigator->get_global_position()->lon,
+		        _navigator->get_home_position()->lat,
+		        _navigator->get_home_position()->lon) + M_PI_F);
+	}
+
+	mission_item_to_position_setpoint(&_mission_item, &pos_sp_triplet->current);
+	_navigator->set_position_setpoint_triplet_updated();
+}
+
+
+void
 Mission::altitude_sp_foh_update()
 {
 	struct position_setpoint_triplet_s *pos_sp_triplet = _navigator->get_position_setpoint_triplet();
@@ -636,6 +696,8 @@ Mission::read_mission_item(bool onboard, bool is_current, struct mission_item_s
 								     "ERROR DO JUMP waypoint could not be written");
 						return false;
 					}
+					report_do_jump_mission_changed(*mission_index_ptr,
+								       mission_item_tmp.do_jump_repeat_count);
 				}
 				/* set new mission item index and repeat
 				* we don't have to validate here, if it's invalid, we should realize this later .*/
@@ -707,22 +769,31 @@ Mission::save_offboard_mission_state()
 }
 
 void
+Mission::report_do_jump_mission_changed(int index, int do_jumps_remaining)
+{
+	/* inform about the change */
+	_navigator->get_mission_result()->item_do_jump_changed = true;
+	_navigator->get_mission_result()->item_changed_index = index;
+	_navigator->get_mission_result()->item_do_jump_remaining = do_jumps_remaining;
+	_navigator->set_mission_result_updated();
+}
+
+void
 Mission::set_mission_item_reached()
 {
 	_navigator->get_mission_result()->reached = true;
 	_navigator->get_mission_result()->seq_reached = _current_offboard_mission_index;
-	_navigator->publish_mission_result();
+	_navigator->set_mission_result_updated();
 	reset_mission_item_reached();
 }
 
 void
 Mission::set_current_offboard_mission_item()
 {
-	warnx("current offboard mission index: %d", _current_offboard_mission_index);
 	_navigator->get_mission_result()->reached = false;
 	_navigator->get_mission_result()->finished = false;
 	_navigator->get_mission_result()->seq_current = _current_offboard_mission_index;
-	_navigator->publish_mission_result();
+	_navigator->set_mission_result_updated();
 
 	save_offboard_mission_state();
 }
@@ -731,5 +802,5 @@ void
 Mission::set_mission_finished()
 {
 	_navigator->get_mission_result()->finished = true;
-	_navigator->publish_mission_result();
+	_navigator->set_mission_result_updated();
 }
diff --git a/src/modules/navigator/mission.h b/src/modules/navigator/mission.h
index ea7cc0927..e9f78e8fd 100644
--- a/src/modules/navigator/mission.h
+++ b/src/modules/navigator/mission.h
@@ -38,6 +38,7 @@
  * @author Julian Oes <julian@oes.ch>
  * @author Thomas Gubler <thomasgubler@gmail.com>
  * @author Anton Babushkin <anton.babushkin@me.com>
+ * @author Ban Siesta <bansiesta@gmail.com>
  */
 
 #ifndef NAVIGATOR_MISSION_H
@@ -82,6 +83,13 @@ public:
 		MISSION_ALTMODE_FOH = 1
 	};
 
+	enum mission_yaw_mode {
+		MISSION_YAWMODE_NONE = 0,
+		MISSION_YAWMODE_FRONT_TO_WAYPOINT = 1,
+		MISSION_YAWMODE_FRONT_TO_HOME = 2,
+		MISSION_YAWMODE_BACK_TO_HOME = 3
+	};
+
 private:
 	/**
 	 * Update onboard mission topic
@@ -110,6 +118,11 @@ private:
 	void set_mission_items();
 
 	/**
+	 * Updates the heading of the vehicle. Rotary wings only.
+	 */
+	void heading_sp_update();
+
+	/**
 	 * Updates the altitude sp to follow a foh
 	 */
 	void altitude_sp_foh_update();
@@ -131,6 +144,11 @@ private:
 	void save_offboard_mission_state();
 
 	/**
+	 * Inform about a changed mission item after a DO_JUMP
+	 */
+	void report_do_jump_mission_changed(int index, int do_jumps_remaining);
+
+	/**
 	 * Set a mission item as reached
 	 */
 	void set_mission_item_reached();
@@ -149,6 +167,7 @@ private:
 	control::BlockParamFloat _param_takeoff_alt;
 	control::BlockParamFloat _param_dist_1wp;
 	control::BlockParamInt _param_altmode;
+	control::BlockParamInt _param_yawmode;
 
 	struct mission_s _onboard_mission;
 	struct mission_s _offboard_mission;
@@ -171,7 +190,8 @@ private:
 
 	float _min_current_sp_distance_xy; /**< minimum distance which was achieved to the current waypoint  */
 	float _mission_item_previous_alt; /**< holds the altitude of the previous mission item,
-					    can be replaced by a full copy of the previous mission item if needed*/
+					    can be replaced by a full copy of the previous mission item if needed */
+	float _on_arrival_yaw; /**< holds the yaw value that should be applied when the current waypoint is reached */
 	float _distance_current_previous; /**< distance from previous to current sp in pos_sp_triplet,
 					    only use if current and previous are valid */
 };
diff --git a/src/modules/navigator/mission_block.cpp b/src/modules/navigator/mission_block.cpp
index 723caec7c..e39fb3216 100644
--- a/src/modules/navigator/mission_block.cpp
+++ b/src/modules/navigator/mission_block.cpp
@@ -134,6 +134,7 @@ MissionBlock::is_mission_item_reached()
 		}
 	}
 
+	/* Check if the waypoint and the requested yaw setpoint. */
 	if (_waypoint_position_reached && !_waypoint_yaw_reached) {
 
 		/* TODO: removed takeoff, why? */
@@ -151,7 +152,7 @@ MissionBlock::is_mission_item_reached()
 		}
 	}
 
-	/* check if the current waypoint was reached */
+	/* Once the waypoint and yaw setpoint have been reached we can start the loiter time countdown */
 	if (_waypoint_position_reached && _waypoint_yaw_reached) {
 
 		if (_time_first_inside_orbit == 0) {
diff --git a/src/modules/navigator/mission_params.c b/src/modules/navigator/mission_params.c
index 04c01fe51..6310cf6de 100644
--- a/src/modules/navigator/mission_params.c
+++ b/src/modules/navigator/mission_params.c
@@ -95,3 +95,19 @@ PARAM_DEFINE_FLOAT(MIS_DIST_1WP, 500);
  * @group Mission
  */
 PARAM_DEFINE_INT32(MIS_ALTMODE, 0);
+
+/**
+ * Multirotor only. Yaw setpoint mode.
+ *
+ * 0: Set the yaw heading to the yaw value specified for the destination waypoint.
+ * 1: Maintain a yaw heading pointing towards the next waypoint.
+ * 2: Maintain a yaw heading that always points to the home location.
+ * 3: Maintain a yaw heading that always points away from the home location (ie: back always faces home).
+ *
+ * The values are defined in the enum mission_altitude_mode
+ *
+ * @min 0
+ * @max 3
+ * @group Mission
+ */
+PARAM_DEFINE_INT32(MIS_YAWMODE, 0);
diff --git a/src/modules/navigator/module.mk b/src/modules/navigator/module.mk
index c44d4c35e..0d7d6b9ef 100644
--- a/src/modules/navigator/module.mk
+++ b/src/modules/navigator/module.mk
@@ -62,3 +62,5 @@ INCLUDE_DIRS	 += $(MAVLINK_SRC)/include/mavlink
 MODULE_STACKSIZE = 1200
 
 MAXOPTIMIZATION = -Os
+
+EXTRACXXFLAGS	= -Wno-sign-compare
diff --git a/src/modules/navigator/navigator.h b/src/modules/navigator/navigator.h
index d550dcc4c..d9d911d9c 100644
--- a/src/modules/navigator/navigator.h
+++ b/src/modules/navigator/navigator.h
@@ -54,6 +54,7 @@
 #include <uORB/topics/vehicle_gps_position.h>
 #include <uORB/topics/parameter_update.h>
 #include <uORB/topics/mission_result.h>
+#include <uORB/topics/geofence_result.h>
 #include <uORB/topics/vehicle_attitude_setpoint.h>
 
 #include "navigator_mode.h"
@@ -107,9 +108,9 @@ public:
 	void		load_fence_from_file(const char *filename);
 
 	/**
-	 * Publish the mission result so commander and mavlink know what is going on
+	 * Publish the geofence result
 	 */
-	void publish_mission_result();
+	void publish_geofence_result();
 
 	/**
 	 * Publish the attitude sp, only to be used in very special modes when position control is deactivated
@@ -122,6 +123,7 @@ public:
 	 */
 	void		set_can_loiter_at_sp(bool can_loiter) { _can_loiter_at_sp = can_loiter; }
 	void		set_position_setpoint_triplet_updated() { _pos_sp_triplet_updated = true; }
+	void		set_mission_result_updated() { _mission_result_updated = true; }
 
 	/**
 	 * Getters
@@ -134,6 +136,7 @@ public:
 	struct home_position_s*		    get_home_position() { return &_home_pos; }
 	struct position_setpoint_triplet_s* get_position_setpoint_triplet() { return &_pos_sp_triplet; }
 	struct mission_result_s*	    get_mission_result() { return &_mission_result; }
+	struct geofence_result_s*		    get_geofence_result() { return &_geofence_result; }
 	struct vehicle_attitude_setpoint_s* get_att_sp() { return &_att_sp; }
 
 	int		get_onboard_mission_sub() { return _onboard_mission_sub; }
@@ -164,6 +167,7 @@ private:
 
 	orb_advert_t	_pos_sp_triplet_pub;		/**< publish position setpoint triplet */
 	orb_advert_t	_mission_result_pub;
+	orb_advert_t	_geofence_result_pub;
 	orb_advert_t	_att_sp_pub;			/**< publish att sp
 							  used only in very special failsafe modes
 							  when pos control is deactivated */
@@ -179,7 +183,8 @@ private:
 	position_setpoint_triplet_s			_pos_sp_triplet;	/**< triplet of position setpoints */
 
 	mission_result_s				_mission_result;
-	vehicle_attitude_setpoint_s					_att_sp;
+	geofence_result_s				_geofence_result;
+	vehicle_attitude_setpoint_s			_att_sp;
 
 	bool 		_mission_item_valid;		/**< flags if the current mission item is valid */
 
@@ -205,6 +210,8 @@ private:
 
 	bool		_can_loiter_at_sp;			/**< flags if current position SP can be used to loiter */
 	bool		_pos_sp_triplet_updated;		/**< flags if position SP triplet needs to be published */
+	bool 		_pos_sp_triplet_published_invalid_once;	/**< flags if position SP triplet has been published once to UORB */
+	bool		_mission_result_updated;		/**< flags if mission result has seen an update */
 
 	control::BlockParamFloat _param_loiter_radius;	/**< loiter radius for fixedwing */
 	control::BlockParamFloat _param_acceptance_radius;	/**< acceptance for takeoff */
@@ -270,6 +277,12 @@ private:
 	 */
 	void		publish_position_setpoint_triplet();
 
+
+	/**
+	 * Publish the mission result so commander and mavlink know what is going on
+	 */
+	void		publish_mission_result();
+
 	/* this class has ptr data members, so it should not be copied,
 	 * consequently the copy constructors are private.
 	 */
diff --git a/src/modules/navigator/navigator_main.cpp b/src/modules/navigator/navigator_main.cpp
index 10a4ee88f..3f7670ec4 100644
--- a/src/modules/navigator/navigator_main.cpp
+++ b/src/modules/navigator/navigator_main.cpp
@@ -110,6 +110,7 @@ Navigator::Navigator() :
 	_param_update_sub(-1),
 	_pos_sp_triplet_pub(-1),
 	_mission_result_pub(-1),
+	_geofence_result_pub(-1),
 	_att_sp_pub(-1),
 	_vstatus{},
 	_control_mode{},
@@ -137,6 +138,8 @@ Navigator::Navigator() :
 	_gpsFailure(this, "GPSF"),
 	_can_loiter_at_sp(false),
 	_pos_sp_triplet_updated(false),
+	_pos_sp_triplet_published_invalid_once(false),
+	_mission_result_updated(false),
 	_param_loiter_radius(this, "LOITER_RAD"),
 	_param_acceptance_radius(this, "ACC_RAD"),
 	_param_datalinkloss_obc(this, "DLL_OBC"),
@@ -397,8 +400,8 @@ Navigator::task_main()
 			have_geofence_position_data = false;
 			if (!inside) {
 				/* inform other apps via the mission result */
-				_mission_result.geofence_violated = true;
-				publish_mission_result();
+				_geofence_result.geofence_violated = true;
+				publish_geofence_result();
 
 				/* Issue a warning about the geofence violation once */
 				if (!_geofence_violation_warning_sent) {
@@ -407,8 +410,8 @@ Navigator::task_main()
 				}
 			} else {
 				/* inform other apps via the mission result */
-				_mission_result.geofence_violated = false;
-				publish_mission_result();
+				_geofence_result.geofence_violated = false;
+				publish_geofence_result();
 				/* Reset the _geofence_violation_warning_sent field */
 				_geofence_violation_warning_sent = false;
 			}
@@ -427,12 +430,15 @@ Navigator::task_main()
 				_can_loiter_at_sp = false;
 				break;
 			case NAVIGATION_STATE_AUTO_MISSION:
+				_pos_sp_triplet_published_invalid_once = false;
 				_navigation_mode = &_mission;
 				break;
 			case NAVIGATION_STATE_AUTO_LOITER:
+				_pos_sp_triplet_published_invalid_once = false;
 				_navigation_mode = &_loiter;
 				break;
 			case NAVIGATION_STATE_AUTO_RCRECOVER:
+				_pos_sp_triplet_published_invalid_once = false;
 				if (_param_rcloss_obc.get() != 0) {
 					_navigation_mode = &_rcLoss;
 				} else {
@@ -440,11 +446,13 @@ Navigator::task_main()
 				}
 				break;
 			case NAVIGATION_STATE_AUTO_RTL:
-					_navigation_mode = &_rtl;
+				_pos_sp_triplet_published_invalid_once = false;
+				_navigation_mode = &_rtl;
 				break;
 			case NAVIGATION_STATE_AUTO_RTGS:
 				/* Use complex data link loss mode only when enabled via param
 				* otherwise use rtl */
+				_pos_sp_triplet_published_invalid_once = false;
 				if (_param_datalinkloss_obc.get() != 0) {
 					_navigation_mode = &_dataLinkLoss;
 				} else {
@@ -452,9 +460,11 @@ Navigator::task_main()
 				}
 				break;
 			case NAVIGATION_STATE_AUTO_LANDENGFAIL:
+				_pos_sp_triplet_published_invalid_once = false;
 				_navigation_mode = &_engineFailure;
 				break;
 			case NAVIGATION_STATE_AUTO_LANDGPSFAIL:
+				_pos_sp_triplet_published_invalid_once = false;
 				_navigation_mode = &_gpsFailure;
 				break;
 			default:
@@ -468,9 +478,9 @@ Navigator::task_main()
 			_navigation_mode_array[i]->run(_navigation_mode == _navigation_mode_array[i]);
 		}
 
-		/* if nothing is running, set position setpoint triplet invalid */
-		if (_navigation_mode == nullptr) {
-			// TODO publish empty sp only once
+		/* if nothing is running, set position setpoint triplet invalid once */
+		if (_navigation_mode == nullptr && !_pos_sp_triplet_published_invalid_once) {
+			_pos_sp_triplet_published_invalid_once = true;
 			_pos_sp_triplet.previous.valid = false;
 			_pos_sp_triplet.current.valid = false;
 			_pos_sp_triplet.next.valid = false;
@@ -482,6 +492,11 @@ Navigator::task_main()
 			_pos_sp_triplet_updated = false;
 		}
 
+		if (_mission_result_updated) {
+			publish_mission_result();
+			_mission_result_updated = false;
+		}
+
 		perf_end(_loop_perf);
 	}
 	warnx("exiting.");
@@ -631,6 +646,28 @@ Navigator::publish_mission_result()
 		/* advertise and publish */
 		_mission_result_pub = orb_advertise(ORB_ID(mission_result), &_mission_result);
 	}
+
+	/* reset some of the flags */
+	_mission_result.seq_reached = false;
+	_mission_result.seq_current = 0;
+	_mission_result.item_do_jump_changed = false;
+	_mission_result.item_changed_index = 0;
+	_mission_result.item_do_jump_remaining = 0;
+}
+
+void
+Navigator::publish_geofence_result()
+{
+
+	/* lazily publish the geofence result only once available */
+	if (_geofence_result_pub > 0) {
+		/* publish mission result */
+		orb_publish(ORB_ID(geofence_result), _geofence_result_pub, &_geofence_result);
+
+	} else {
+		/* advertise and publish */
+		_geofence_result_pub = orb_advertise(ORB_ID(geofence_result), &_geofence_result);
+	}
 }
 
 void
diff --git a/src/modules/navigator/navigator_mode.cpp b/src/modules/navigator/navigator_mode.cpp
index 3807c5ea8..2f322031c 100644
--- a/src/modules/navigator/navigator_mode.cpp
+++ b/src/modules/navigator/navigator_mode.cpp
@@ -65,7 +65,7 @@ NavigatorMode::run(bool active) {
 			_first_run = false;
 			/* Reset stay in failsafe flag */
 			_navigator->get_mission_result()->stay_in_failsafe = false;
-			_navigator->publish_mission_result();
+			_navigator->set_mission_result_updated();
 			on_activation();
 
 		} else {
diff --git a/src/modules/navigator/navigator_params.c b/src/modules/navigator/navigator_params.c
index 1f40e634e..5f8f8d02f 100644
--- a/src/modules/navigator/navigator_params.c
+++ b/src/modules/navigator/navigator_params.c
@@ -50,7 +50,8 @@
  * Default value of loiter radius for missions, loiter, RTL, etc. (fixedwing only).
  *
  * @unit meters
- * @min 0.0
+ * @min 20
+ * @max 200
  * @group Mission
  */
 PARAM_DEFINE_FLOAT(NAV_LOITER_RAD, 50.0f);
@@ -61,10 +62,11 @@ PARAM_DEFINE_FLOAT(NAV_LOITER_RAD, 50.0f);
  * Default acceptance radius, overridden by acceptance radius of waypoint if set.
  *
  * @unit meters
- * @min 1.0
+ * @min 0.05
+ * @max 200
  * @group Mission
  */
-PARAM_DEFINE_FLOAT(NAV_ACC_RAD, 25.0f);
+PARAM_DEFINE_FLOAT(NAV_ACC_RAD, 10.0f);
 
 /**
  * Set OBC mode for data link loss
diff --git a/src/modules/navigator/rcloss.cpp b/src/modules/navigator/rcloss.cpp
index 42392e739..a7cde6325 100644
--- a/src/modules/navigator/rcloss.cpp
+++ b/src/modules/navigator/rcloss.cpp
@@ -128,7 +128,7 @@ RCLoss::set_rcl_item()
 	case RCL_STATE_TERMINATE: {
 		/* Request flight termination from the commander */
 		_navigator->get_mission_result()->flight_termination = true;
-		_navigator->publish_mission_result();
+		_navigator->set_mission_result_updated();
 		warnx("rc not recovered: request flight termination");
 		pos_sp_triplet->previous.valid = false;
 		pos_sp_triplet->current.valid = false;
@@ -162,7 +162,7 @@ RCLoss::advance_rcl()
 			mavlink_log_info(_navigator->get_mavlink_fd(), "#audio: rc loss, terminating");
 			_rcl_state = RCL_STATE_TERMINATE;
 			_navigator->get_mission_result()->stay_in_failsafe = true;
-			_navigator->publish_mission_result();
+			_navigator->set_mission_result_updated();
 			reset_mission_item_reached();
 		}
 		break;
@@ -171,7 +171,7 @@ RCLoss::advance_rcl()
 		warnx("time is up, no RC regain, terminating");
 		mavlink_log_info(_navigator->get_mavlink_fd(), "#audio: RC not regained, terminating");
 		_navigator->get_mission_result()->stay_in_failsafe = true;
-		_navigator->publish_mission_result();
+		_navigator->set_mission_result_updated();
 		reset_mission_item_reached();
 		break;
 	case RCL_STATE_TERMINATE:
diff --git a/src/modules/navigator/rtl_params.c b/src/modules/navigator/rtl_params.c
index bfe6ce7e1..1568233b0 100644
--- a/src/modules/navigator/rtl_params.c
+++ b/src/modules/navigator/rtl_params.c
@@ -53,7 +53,8 @@
  * Default value of loiter radius after RTL (fixedwing only).
  *
  * @unit meters
- * @min 0.0
+ * @min 20
+ * @max 200
  * @group RTL
  */
 PARAM_DEFINE_FLOAT(RTL_LOITER_RAD, 50.0f);
@@ -65,10 +66,10 @@ PARAM_DEFINE_FLOAT(RTL_LOITER_RAD, 50.0f);
  *
  * @unit meters
  * @min 0
- * @max 1
+ * @max 150
  * @group RTL
  */
-PARAM_DEFINE_FLOAT(RTL_RETURN_ALT, 100);
+PARAM_DEFINE_FLOAT(RTL_RETURN_ALT, 60);
 
 
 /**
@@ -78,7 +79,7 @@ PARAM_DEFINE_FLOAT(RTL_RETURN_ALT, 100);
  * Land (i.e. slowly descend) from this altitude if autolanding allowed.
  *
  * @unit meters
- * @min 0
+ * @min 2
  * @max 100
  * @group RTL
  */
@@ -92,7 +93,7 @@ PARAM_DEFINE_FLOAT(RTL_DESCEND_ALT, 20);
  *
  * @unit seconds
  * @min -1
- * @max
+ * @max 300
  * @group RTL
  */
 PARAM_DEFINE_FLOAT(RTL_LAND_DELAY, -1.0f);
diff --git a/src/modules/position_estimator_inav/module.mk b/src/modules/position_estimator_inav/module.mk
index 0658d3f09..45c876299 100644
--- a/src/modules/position_estimator_inav/module.mk
+++ b/src/modules/position_estimator_inav/module.mk
@@ -41,3 +41,6 @@ SRCS		 	= position_estimator_inav_main.c \
 			inertial_filter.c
 
 MODULE_STACKSIZE = 1200
+
+EXTRACFLAGS = -Wframe-larger-than=3500
+
diff --git a/src/modules/position_estimator_inav/position_estimator_inav_main.c b/src/modules/position_estimator_inav/position_estimator_inav_main.c
index 81bbaa658..86764d620 100644
--- a/src/modules/position_estimator_inav/position_estimator_inav_main.c
+++ b/src/modules/position_estimator_inav/position_estimator_inav_main.c
@@ -151,7 +151,7 @@ int position_estimator_inav_main(int argc, char *argv[])
 		position_estimator_inav_task = task_spawn_cmd("position_estimator_inav",
 					       SCHED_DEFAULT, SCHED_PRIORITY_MAX - 5, 5000,
 					       position_estimator_inav_thread_main,
-					       (argv) ? (const char **) &argv[2] : (const char **) NULL);
+					       (argv) ? (char * const *) &argv[2] : (char * const *) NULL);
 		exit(0);
 	}
 
@@ -161,7 +161,7 @@ int position_estimator_inav_main(int argc, char *argv[])
 			thread_should_exit = true;
 
 		} else {
-			warnx("app not started");
+			warnx("not started");
 		}
 
 		exit(0);
@@ -169,10 +169,10 @@ int position_estimator_inav_main(int argc, char *argv[])
 
 	if (!strcmp(argv[1], "status")) {
 		if (thread_running) {
-			warnx("app is running");
+			warnx("is running");
 
 		} else {
-			warnx("app not started");
+			warnx("not started");
 		}
 
 		exit(0);
@@ -210,10 +210,8 @@ static void write_debug_log(const char *msg, float dt, float x_est[2], float y_e
  ****************************************************************************/
 int position_estimator_inav_thread_main(int argc, char *argv[])
 {
-	warnx("started");
 	int mavlink_fd;
 	mavlink_fd = open(MAVLINK_LOG_DEVICE, 0);
-	mavlink_log_info(mavlink_fd, "[inav] started");
 
 	float x_est[2] = { 0.0f, 0.0f };	// pos, vel
 	float y_est[2] = { 0.0f, 0.0f };	// pos, vel
@@ -235,8 +233,8 @@ int position_estimator_inav_thread_main(int argc, char *argv[])
 
 	float eph_flow = 1.0f;
 
-	float eph_vision = 0.5f;
-	float epv_vision = 0.5f;
+	float eph_vision = 0.2f;
+	float epv_vision = 0.2f;
 
 	float x_est_prev[2], y_est_prev[2], z_est_prev[2];
 	memset(x_est_prev, 0, sizeof(x_est_prev));
@@ -248,9 +246,6 @@ int position_estimator_inav_thread_main(int argc, char *argv[])
 	float baro_offset = 0.0f;		// baro offset for reference altitude, initialized on start, then adjusted
 	float surface_offset = 0.0f;	// ground level offset from reference altitude
 	float surface_offset_rate = 0.0f;	// surface offset change rate
-	float alt_avg = 0.0f;
-	bool landed = true;
-	hrt_abstime landed_time = 0;
 
 	hrt_abstime accel_timestamp = 0;
 	hrt_abstime baro_timestamp = 0;
@@ -284,13 +279,13 @@ int position_estimator_inav_thread_main(int argc, char *argv[])
 	};
 	float w_gps_xy = 1.0f;
 	float w_gps_z = 1.0f;
-	
+
 	float corr_vision[3][2] = {
 		{ 0.0f, 0.0f },		// N (pos, vel)
 		{ 0.0f, 0.0f },		// E (pos, vel)
 		{ 0.0f, 0.0f },		// D (pos, vel)
 	};
-	
+
 	float corr_sonar = 0.0f;
 	float corr_sonar_filtered = 0.0f;
 
@@ -298,7 +293,7 @@ int position_estimator_inav_thread_main(int argc, char *argv[])
 	float w_flow = 0.0f;
 
 	float sonar_prev = 0.0f;
-	hrt_abstime flow_prev = 0;			// time of last flow measurement
+	//hrt_abstime flow_prev = 0;			// time of last flow measurement
 	hrt_abstime sonar_time = 0;			// time of last sonar measurement (not filtered)
 	hrt_abstime sonar_valid_time = 0;	// time of last sonar measurement used for correction (filtered)
 
@@ -389,8 +384,8 @@ int position_estimator_inav_thread_main(int argc, char *argv[])
 					} else {
 						wait_baro = false;
 						baro_offset /= (float) baro_init_cnt;
-						warnx("baro offs: %.2f", (double)baro_offset);
-						mavlink_log_info(mavlink_fd, "[inav] baro offs: %.2f", (double)baro_offset);
+						warnx("baro offs: %d", (int)baro_offset);
+						mavlink_log_info(mavlink_fd, "[inav] baro offs: %d", (int)baro_offset);
 						local_pos.z_valid = true;
 						local_pos.v_z_valid = true;
 					}
@@ -491,8 +486,8 @@ int position_estimator_inav_thread_main(int argc, char *argv[])
 				orb_copy(ORB_ID(optical_flow), optical_flow_sub, &flow);
 
 				/* calculate time from previous update */
-				float flow_dt = flow_prev > 0 ? (flow.flow_timestamp - flow_prev) * 1e-6f : 0.1f;
-				flow_prev = flow.flow_timestamp;
+//				float flow_dt = flow_prev > 0 ? (flow.flow_timestamp - flow_prev) * 1e-6f : 0.1f;
+//				flow_prev = flow.flow_timestamp;
 
 				if ((flow.ground_distance_m > 0.31f) &&
 					(flow.ground_distance_m < 4.0f) &&
@@ -520,7 +515,7 @@ int position_estimator_inav_thread_main(int argc, char *argv[])
 							sonar_valid_time = t;
 							sonar_valid = true;
 							local_pos.surface_bottom_timestamp = t;
-							mavlink_log_info(mavlink_fd, "[inav] new surface level: %.2f", (double)surface_offset);
+							mavlink_log_info(mavlink_fd, "[inav] new surface level: %d", (int)surface_offset);
 						}
 
 					} else {
@@ -550,8 +545,9 @@ int position_estimator_inav_thread_main(int argc, char *argv[])
 
 					/* convert raw flow to angular flow (rad/s) */
 					float flow_ang[2];
-					flow_ang[0] = flow.flow_raw_x * params.flow_k / 1000.0f / flow_dt;
-					flow_ang[1] = flow.flow_raw_y * params.flow_k / 1000.0f / flow_dt;
+					//todo check direction of x und y axis
+					flow_ang[0] = flow.pixel_flow_x_integral/(float)flow.integration_timespan*1000000.0f;//flow.flow_raw_x * params.flow_k / 1000.0f / flow_dt;
+					flow_ang[1] = flow.pixel_flow_y_integral/(float)flow.integration_timespan*1000000.0f;//flow.flow_raw_y * params.flow_k / 1000.0f / flow_dt;
 					/* flow measurements vector */
 					float flow_m[3];
 					flow_m[0] = -flow_ang[0] * flow_dist;
@@ -641,20 +637,29 @@ int position_estimator_inav_thread_main(int argc, char *argv[])
 				if (updated) {
 					orb_copy(ORB_ID(vision_position_estimate), vision_position_estimate_sub, &vision);
 
+					static float last_vision_x = 0.0f;
+					static float last_vision_y = 0.0f;
+					static float last_vision_z = 0.0f;
+
 					/* reset position estimate on first vision update */
 					if (!vision_valid) {
 						x_est[0] = vision.x;
 						x_est[1] = vision.vx;
 						y_est[0] = vision.y;
 						y_est[1] = vision.vy;
-						/* only reset the z estimate if the z weight parameter is not zero */ 
+						/* only reset the z estimate if the z weight parameter is not zero */
 						if (params.w_z_vision_p > MIN_VALID_W)
 						{
 							z_est[0] = vision.z;
 							z_est[1] = vision.vz;
 						}
-						
+
 						vision_valid = true;
+
+						last_vision_x = vision.x;
+						last_vision_y = vision.y;
+						last_vision_z = vision.z;
+
 						warnx("VISION estimate valid");
 						mavlink_log_info(mavlink_fd, "[inav] VISION estimate valid");
 					}
@@ -664,10 +669,30 @@ int position_estimator_inav_thread_main(int argc, char *argv[])
 					corr_vision[1][0] = vision.y - y_est[0];
 					corr_vision[2][0] = vision.z - z_est[0];
 
-					/* calculate correction for velocity */
-					corr_vision[0][1] = vision.vx - x_est[1];
-					corr_vision[1][1] = vision.vy - y_est[1];
-					corr_vision[2][1] = vision.vz - z_est[1];
+					static hrt_abstime last_vision_time = 0;
+
+					float vision_dt = (vision.timestamp_boot - last_vision_time) / 1e6f;
+					last_vision_time = vision.timestamp_boot;
+
+					if (vision_dt > 0.000001f && vision_dt < 0.2f) {
+						vision.vx = (vision.x - last_vision_x) / vision_dt;
+						vision.vy = (vision.y - last_vision_y) / vision_dt;
+						vision.vz = (vision.z - last_vision_z) / vision_dt;
+
+						last_vision_x = vision.x;
+						last_vision_y = vision.y;
+						last_vision_z = vision.z;
+
+						/* calculate correction for velocity */
+						corr_vision[0][1] = vision.vx - x_est[1];
+						corr_vision[1][1] = vision.vy - y_est[1];
+						corr_vision[2][1] = vision.vz - z_est[1];
+					} else {
+						/* assume zero motion */
+						corr_vision[0][1] = 0.0f - x_est[1];
+						corr_vision[1][1] = 0.0f - y_est[1];
+						corr_vision[2][1] = 0.0f - z_est[1];
+					}
 
 				}
 			}
@@ -721,8 +746,9 @@ int position_estimator_inav_thread_main(int argc, char *argv[])
 
 							/* initialize projection */
 							map_projection_init(&ref, lat, lon);
-							warnx("init ref: lat=%.7f, lon=%.7f, alt=%.2f", (double)lat, (double)lon, (double)alt);
-							mavlink_log_info(mavlink_fd, "[inav] init ref: %.7f, %.7f, %.2f", (double)lat, (double)lon, (double)alt);
+							// XXX replace this print
+							warnx("init ref: lat=%.7f, lon=%.7f, alt=%8.4f", (double)lat, (double)lon, (double)alt);
+							mavlink_log_info(mavlink_fd, "[inav] init ref: %.7f, %.7f, %8.4f", (double)lat, (double)lon, (double)alt);
 						}
 					}
 
@@ -846,6 +872,7 @@ int position_estimator_inav_thread_main(int argc, char *argv[])
 		float w_xy_gps_p = params.w_xy_gps_p * w_gps_xy;
 		float w_xy_gps_v = params.w_xy_gps_v * w_gps_xy;
 		float w_z_gps_p = params.w_z_gps_p * w_gps_z;
+		float w_z_gps_v = params.w_z_gps_v * w_gps_z;
 
 		float w_xy_vision_p = params.w_xy_vision_p;
 		float w_xy_vision_v = params.w_xy_vision_v;
@@ -876,6 +903,7 @@ int position_estimator_inav_thread_main(int argc, char *argv[])
 
 		if (use_gps_z) {
 			accel_bias_corr[2] -= corr_gps[2][0] * w_z_gps_p * w_z_gps_p;
+			accel_bias_corr[2] -= corr_gps[2][1] * w_z_gps_v;
 		}
 
 		/* transform error vector from NED frame to body frame */
@@ -960,6 +988,7 @@ int position_estimator_inav_thread_main(int argc, char *argv[])
 			epv = fminf(epv, gps.epv);
 
 			inertial_filter_correct(corr_gps[2][0], dt, z_est, 0, w_z_gps_p);
+			inertial_filter_correct(corr_gps[2][1], dt, z_est, 1, w_z_gps_v);
 		}
 
 		if (use_vision_z) {
@@ -1039,40 +1068,6 @@ int position_estimator_inav_thread_main(int argc, char *argv[])
 			inertial_filter_correct(-y_est[1], dt, y_est, 1, params.w_xy_res_v);
 		}
 
-		/* detect land */
-		alt_avg += (- z_est[0] - alt_avg) * dt / params.land_t;
-		float alt_disp2 = - z_est[0] - alt_avg;
-		alt_disp2 = alt_disp2 * alt_disp2;
-		float land_disp2 = params.land_disp * params.land_disp;
-		/* get actual thrust output */
-		float thrust = armed.armed ? actuator.control[3] : 0.0f;
-
-		if (landed) {
-			if (alt_disp2 > land_disp2 || thrust > params.land_thr) {
-				landed = false;
-				landed_time = 0;
-			}
-			/* reset xy velocity estimates when landed */
-			x_est[1] = 0.0f;
-			y_est[1] = 0.0f;
-
-		} else {
-			if (alt_disp2 < land_disp2 && thrust < params.land_thr) {
-				if (landed_time == 0) {
-					landed_time = t;    // land detected first time
-
-				} else {
-					if (t > landed_time + params.land_t * 1000000.0f) {
-						landed = true;
-						landed_time = 0;
-					}
-				}
-
-			} else {
-				landed_time = 0;
-			}
-		}
-
 		if (verbose_mode) {
 			/* print updates rate */
 			if (t > updates_counter_start + updates_counter_len) {
@@ -1123,7 +1118,6 @@ int position_estimator_inav_thread_main(int argc, char *argv[])
 			local_pos.vy = y_est[1];
 			local_pos.z = z_est[0];
 			local_pos.vz = z_est[1];
-			local_pos.landed = landed;
 			local_pos.yaw = att.yaw;
 			local_pos.dist_bottom_valid = dist_bottom_valid;
 			local_pos.eph = eph;
@@ -1141,7 +1135,7 @@ int position_estimator_inav_thread_main(int argc, char *argv[])
 			if (local_pos.xy_global && local_pos.z_global) {
 				/* publish global position */
 				global_pos.timestamp = t;
-				global_pos.time_gps_usec = gps.time_gps_usec;
+				global_pos.time_utc_usec = gps.time_utc_usec;
 
 				double est_lat, est_lon;
 				map_projection_reproject(&ref, local_pos.x, local_pos.y, &est_lat, &est_lon);
diff --git a/src/modules/position_estimator_inav/position_estimator_inav_params.c b/src/modules/position_estimator_inav/position_estimator_inav_params.c
index cc0fb4155..5387b7e87 100644
--- a/src/modules/position_estimator_inav/position_estimator_inav_params.c
+++ b/src/modules/position_estimator_inav/position_estimator_inav_params.c
@@ -64,6 +64,17 @@ PARAM_DEFINE_FLOAT(INAV_W_Z_BARO, 0.5f);
 PARAM_DEFINE_FLOAT(INAV_W_Z_GPS_P, 0.005f);
 
 /**
+ * Z velocity weight for GPS
+ *
+ * Weight (cutoff frequency) for GPS altitude velocity measurements.
+ *
+ * @min 0.0
+ * @max 10.0
+ * @group Position Estimator INAV
+ */
+PARAM_DEFINE_FLOAT(INAV_W_Z_GPS_V, 0.0f);
+
+/**
  * Z axis weight for vision
  *
  * Weight (cutoff frequency) for vision altitude measurements. vision altitude data is very noisy and should be used only as slow correction for baro offset.
@@ -116,7 +127,7 @@ PARAM_DEFINE_FLOAT(INAV_W_XY_GPS_V, 2.0f);
  * @max 10.0
  * @group Position Estimator INAV
  */
-PARAM_DEFINE_FLOAT(INAV_W_XY_VIS_P, 5.0f);
+PARAM_DEFINE_FLOAT(INAV_W_XY_VIS_P, 7.0f);
 
 /**
  * XY axis weight for vision velocity
@@ -281,6 +292,7 @@ int parameters_init(struct position_estimator_inav_param_handles *h)
 {
 	h->w_z_baro = param_find("INAV_W_Z_BARO");
 	h->w_z_gps_p = param_find("INAV_W_Z_GPS_P");
+	h->w_z_gps_v = param_find("INAV_W_Z_GPS_V");
 	h->w_z_vision_p = param_find("INAV_W_Z_VIS_P");
 	h->w_z_sonar = param_find("INAV_W_Z_SONAR");
 	h->w_xy_gps_p = param_find("INAV_W_XY_GPS_P");
diff --git a/src/modules/position_estimator_inav/position_estimator_inav_params.h b/src/modules/position_estimator_inav/position_estimator_inav_params.h
index d0a65e42e..51bbda412 100644
--- a/src/modules/position_estimator_inav/position_estimator_inav_params.h
+++ b/src/modules/position_estimator_inav/position_estimator_inav_params.h
@@ -44,6 +44,7 @@
 struct position_estimator_inav_params {
 	float w_z_baro;
 	float w_z_gps_p;
+	float w_z_gps_v;
 	float w_z_vision_p;
 	float w_z_sonar;
 	float w_xy_gps_p;
@@ -68,6 +69,7 @@ struct position_estimator_inav_params {
 struct position_estimator_inav_param_handles {
 	param_t w_z_baro;
 	param_t w_z_gps_p;
+	param_t w_z_gps_v;
 	param_t w_z_vision_p;
 	param_t w_z_sonar;
 	param_t w_xy_gps_p;
diff --git a/src/modules/px4iofirmware/controls.c b/src/modules/px4iofirmware/controls.c
index 58c9429b6..d20f776d6 100644
--- a/src/modules/px4iofirmware/controls.c
+++ b/src/modules/px4iofirmware/controls.c
@@ -454,6 +454,10 @@ ppm_input(uint16_t *values, uint16_t *num_values, uint16_t *frame_len)
 {
 	bool result = false;
 
+	if (!(num_values) || !(values) || !(frame_len)) {
+		return result;
+	}
+
 	/* avoid racing with PPM updates */
 	irqstate_t state = irqsave();
 
@@ -468,7 +472,7 @@ ppm_input(uint16_t *values, uint16_t *num_values, uint16_t *frame_len)
 		if (*num_values > PX4IO_RC_INPUT_CHANNELS)
 			*num_values = PX4IO_RC_INPUT_CHANNELS;
 
-		for (unsigned i = 0; i < *num_values; i++) {
+		for (unsigned i = 0; ((i < *num_values) && (i < PPM_MAX_CHANNELS)); i++) {
 			values[i] = ppm_buffer[i];
 		}
 
@@ -476,8 +480,7 @@ ppm_input(uint16_t *values, uint16_t *num_values, uint16_t *frame_len)
 		ppm_last_valid_decode = 0;
 
 		/* store PPM frame length */
-		if (num_values)
-			*frame_len = ppm_frame_length;
+		*frame_len = ppm_frame_length;
 
 		/* good if we got any channels */
 		result = (*num_values > 0);
diff --git a/src/modules/px4iofirmware/mixer.cpp b/src/modules/px4iofirmware/mixer.cpp
index c0b8ac358..66f0969de 100644
--- a/src/modules/px4iofirmware/mixer.cpp
+++ b/src/modules/px4iofirmware/mixer.cpp
@@ -353,12 +353,16 @@ static unsigned mixer_text_length = 0;
 int
 mixer_handle_text(const void *buffer, size_t length)
 {
-	/* do not allow a mixer change while safety off */
-	if ((r_status_flags & PX4IO_P_STATUS_FLAGS_SAFETY_OFF)) {
+	/* do not allow a mixer change while safety off and FMU armed */
+	if ((r_status_flags & PX4IO_P_STATUS_FLAGS_SAFETY_OFF) &&
+		(r_setup_arming & PX4IO_P_SETUP_ARMING_FMU_ARMED)) {
 		return 1;
 	}
 
-	/* abort if we're in the mixer */
+	/* disable mixing, will be enabled once load is complete */
+	r_status_flags &= ~(PX4IO_P_STATUS_FLAGS_MIXER_OK);
+
+	/* abort if we're in the mixer - the caller is expected to retry */
 	if (in_mixer) {
 		return 1;
 	}
@@ -367,17 +371,16 @@ mixer_handle_text(const void *buffer, size_t length)
 
 	isr_debug(2, "mix txt %u", length);
 
-	if (length < sizeof(px4io_mixdata))
+	if (length < sizeof(px4io_mixdata)) {
 		return 0;
+	}
 
-	unsigned	text_length = length - sizeof(px4io_mixdata);
+	unsigned text_length = length - sizeof(px4io_mixdata);
 
 	switch (msg->action) {
 	case F2I_MIXER_ACTION_RESET:
 		isr_debug(2, "reset");
 
-		/* FIRST mark the mixer as invalid */
-		r_status_flags &= ~PX4IO_P_STATUS_FLAGS_MIXER_OK;
 		/* THEN actually delete it */
 		mixer_group.reset();
 		mixer_text_length = 0;
@@ -386,9 +389,6 @@ mixer_handle_text(const void *buffer, size_t length)
 	case F2I_MIXER_ACTION_APPEND:
 		isr_debug(2, "append %d", length);
 
-		/* disable mixing during the update */
-		r_status_flags &= ~PX4IO_P_STATUS_FLAGS_MIXER_OK;
-
 		/* check for overflow - this would be really fatal */
 		if ((mixer_text_length + text_length + 1) > sizeof(mixer_text)) {
 			r_status_flags &= ~PX4IO_P_STATUS_FLAGS_MIXER_OK;
diff --git a/src/modules/px4iofirmware/module.mk b/src/modules/px4iofirmware/module.mk
index eb99e8a96..844e493cd 100644
--- a/src/modules/px4iofirmware/module.mk
+++ b/src/modules/px4iofirmware/module.mk
@@ -1,5 +1,4 @@
 
-
 SRCS		= adc.c \
 		  controls.c \
 		  dsm.c \
@@ -24,3 +23,7 @@ ifeq ($(BOARD),px4io-v2)
 SRCS		+= serial.c \
 		   ../systemlib/hx_stream.c
 endif
+
+SELF_DIR := $(dir $(lastword $(MAKEFILE_LIST)))
+include $(SELF_DIR)../systemlib/mixer/multi_tables.mk
+	
\ No newline at end of file
diff --git a/src/modules/px4iofirmware/protocol.h b/src/modules/px4iofirmware/protocol.h
index c7e9ae3eb..bd777428f 100644
--- a/src/modules/px4iofirmware/protocol.h
+++ b/src/modules/px4iofirmware/protocol.h
@@ -33,6 +33,8 @@
 
 #pragma once
 
+#include <inttypes.h>
+
 /**
  * @file protocol.h
  *
diff --git a/src/modules/px4iofirmware/registers.c b/src/modules/px4iofirmware/registers.c
index fbfdd35db..f0c2cfd26 100644
--- a/src/modules/px4iofirmware/registers.c
+++ b/src/modules/px4iofirmware/registers.c
@@ -407,11 +407,11 @@ registers_set(uint8_t page, uint8_t offset, const uint16_t *values, unsigned num
 
 		/* handle text going to the mixer parser */
 	case PX4IO_PAGE_MIXERLOAD:
-		if (!(r_status_flags & PX4IO_P_STATUS_FLAGS_SAFETY_OFF) ||
-				    (r_status_flags & PX4IO_P_STATUS_FLAGS_OUTPUTS_ARMED)) {
-			return mixer_handle_text(values, num_values * sizeof(*values));
-		}
-		break;
+		/* do not change the mixer if FMU is armed and IO's safety is off
+		 * this state defines an active system. This check is done in the
+		 * text handling function.
+		 */
+		return mixer_handle_text(values, num_values * sizeof(*values));
 
 	default:
 		/* avoid offset wrap */
@@ -583,8 +583,7 @@ registers_set_one(uint8_t page, uint8_t offset, uint16_t value)
 			break;
 
 		case PX4IO_P_SETUP_REBOOT_BL:
-			if ((r_status_flags & PX4IO_P_STATUS_FLAGS_SAFETY_OFF) ||
-			    (r_status_flags & PX4IO_P_STATUS_FLAGS_OUTPUTS_ARMED)) {
+			if (r_status_flags & PX4IO_P_STATUS_FLAGS_SAFETY_OFF) {
 				// don't allow reboot while armed
 				break;
 			}
@@ -630,10 +629,9 @@ registers_set_one(uint8_t page, uint8_t offset, uint16_t value)
 	case PX4IO_PAGE_RC_CONFIG: {
 
 		/**
-		 * do not allow a RC config change while outputs armed
+		 * do not allow a RC config change while safety is off
 		 */
-		if ((r_status_flags & PX4IO_P_STATUS_FLAGS_SAFETY_OFF) ||
-			    (r_status_flags & PX4IO_P_STATUS_FLAGS_OUTPUTS_ARMED)) {
+		if (r_status_flags & PX4IO_P_STATUS_FLAGS_SAFETY_OFF) {
 			break;
 		}
 
diff --git a/src/modules/sdlog2/module.mk b/src/modules/sdlog2/module.mk
index d4a00af39..91a9611d4 100644
--- a/src/modules/sdlog2/module.mk
+++ b/src/modules/sdlog2/module.mk
@@ -45,3 +45,6 @@ SRCS = sdlog2.c \
 MODULE_STACKSIZE = 1200
 
 MAXOPTIMIZATION	 = -Os
+
+EXTRACFLAGS = -Wframe-larger-than=1300
+
diff --git a/src/modules/sdlog2/sdlog2.c b/src/modules/sdlog2/sdlog2.c
index af580f1f7..99632ef0b 100644
--- a/src/modules/sdlog2/sdlog2.c
+++ b/src/modules/sdlog2/sdlog2.c
@@ -1,6 +1,6 @@
 /****************************************************************************
  *
- *   Copyright (c) 2012-2014 PX4 Development Team. All rights reserved.
+ *   Copyright (c) 2012-2015 PX4 Development Team. All rights reserved.
  *
  * Redistribution and use in source and binary forms, with or without
  * modification, are permitted provided that the following conditions
@@ -39,12 +39,14 @@
  *
  * @author Lorenz Meier <lm@inf.ethz.ch>
  * @author Anton Babushkin <anton.babushkin@me.com>
+ * @author Ban Siesta <bansiesta@gmail.com>
  */
 
 #include <nuttx/config.h>
 #include <sys/types.h>
 #include <sys/stat.h>
 #include <sys/prctl.h>
+#include <sys/statfs.h>
 #include <fcntl.h>
 #include <errno.h>
 #include <unistd.h>
@@ -57,6 +59,7 @@
 #include <unistd.h>
 #include <drivers/drv_hrt.h>
 #include <math.h>
+#include <time.h>
 
 #include <drivers/drv_range_finder.h>
 
@@ -91,6 +94,7 @@
 #include <uORB/topics/servorail_status.h>
 #include <uORB/topics/wind_estimate.h>
 #include <uORB/topics/encoders.h>
+#include <uORB/topics/vtol_vehicle_status.h>
 
 #include <systemlib/systemlib.h>
 #include <systemlib/param/param.h>
@@ -103,6 +107,8 @@
 #include "sdlog2_format.h"
 #include "sdlog2_messages.h"
 
+#define PX4_EPOCH_SECS 1234567890ULL
+
 /**
  * Logging rate.
  *
@@ -158,7 +164,9 @@ static const int MIN_BYTES_TO_WRITE = 512;
 
 static bool _extended_logging = false;
 
-static const char *log_root = "/fs/microsd/log";
+#define MOUNTPOINT "/fs/microsd"
+static const char *mountpoint = MOUNTPOINT;
+static const char *log_root = MOUNTPOINT "/log";
 static int mavlink_fd = -1;
 struct logbuffer_s lb;
 
@@ -171,6 +179,7 @@ static char log_dir[32];
 /* statistics counters */
 static uint64_t start_time = 0;
 static unsigned long log_bytes_written = 0;
+static unsigned long last_checked_bytes_written = 0;
 static unsigned long log_msgs_written = 0;
 static unsigned long log_msgs_skipped = 0;
 
@@ -185,9 +194,14 @@ static bool log_name_timestamp = false;
 /* helper flag to track system state changes */
 static bool flag_system_armed = false;
 
+/* flag if warning about MicroSD card being almost full has already been sent */
+static bool space_warning_sent = false;
+
 static pthread_t logwriter_pthread = 0;
 static pthread_attr_t logwriter_attr;
 
+static perf_counter_t perf_write;
+
 /**
  * Log buffer writing thread. Open and close file here.
  */
@@ -244,6 +258,11 @@ static bool file_exist(const char *filename);
 
 static int file_copy(const char *file_old, const char *file_new);
 
+/**
+ * Check if there is still free space available
+ */
+static int check_free_space(void);
+
 static void handle_command(struct vehicle_command_s *cmd);
 
 static void handle_status(struct vehicle_status_s *cmd);
@@ -304,7 +323,7 @@ int sdlog2_main(int argc, char *argv[])
 						 SCHED_PRIORITY_DEFAULT - 30,
 						 3000,
 						 sdlog2_thread_main,
-						 (const char **)argv);
+						 (char * const *)argv);
 		exit(0);
 	}
 
@@ -338,13 +357,16 @@ int create_log_dir()
 	uint16_t dir_number = 1; // start with dir sess001
 	int mkdir_ret;
 
-	if (log_name_timestamp && gps_time != 0) {
-		/* use GPS date for log dir naming: e.g. /fs/microsd/2014-01-19 */
-		time_t gps_time_sec = gps_time / 1000000;
-		struct tm t;
-		gmtime_r(&gps_time_sec, &t);
+	struct timespec ts;
+	clock_gettime(CLOCK_REALTIME, &ts);
+	/* use RTC time for log file naming, e.g. /fs/microsd/2014-01-19/19_37_52.px4log */
+	time_t utc_time_sec = ts.tv_sec + (ts.tv_nsec / 1e9);
+	struct tm tt;
+	struct tm *ttp = gmtime_r(&utc_time_sec, &tt);
+
+	if (log_name_timestamp && ttp && (utc_time_sec > PX4_EPOCH_SECS)) {
 		int n = snprintf(log_dir, sizeof(log_dir), "%s/", log_root);
-		strftime(log_dir + n, sizeof(log_dir) - n, "%Y-%m-%d", &t);
+		strftime(log_dir + n, sizeof(log_dir) - n, "%Y-%m-%d", &tt);
 		mkdir_ret = mkdir(log_dir, S_IRWXU | S_IRWXG | S_IRWXO);
 
 		if (mkdir_ret == OK) {
@@ -384,8 +406,7 @@ int create_log_dir()
 	}
 
 	/* print logging path, important to find log file later */
-	warnx("log dir: %s", log_dir);
-	mavlink_log_info(mavlink_fd, "[sdlog2] log dir: %s", log_dir);
+	mavlink_and_console_log_info(mavlink_fd, "[sdlog2] log dir: %s", log_dir);
 	return 0;
 }
 
@@ -395,12 +416,16 @@ int open_log_file()
 	char log_file_name[32] = "";
 	char log_file_path[64] = "";
 
-	if (log_name_timestamp && gps_time != 0) {
-		/* use GPS time for log file naming, e.g. /fs/microsd/2014-01-19/19_37_52.bin */
-		time_t gps_time_sec = gps_time / 1000000;
-		struct tm t;
-		gmtime_r(&gps_time_sec, &t);
-		strftime(log_file_name, sizeof(log_file_name), "%H_%M_%S.bin", &t);
+	struct timespec ts;
+	clock_gettime(CLOCK_REALTIME, &ts);
+	/* use RTC time for log file naming, e.g. /fs/microsd/2014-01-19/19_37_52.px4log */
+	time_t utc_time_sec = ts.tv_sec + (ts.tv_nsec / 1e9);
+	struct tm tt;
+	struct tm *ttp = gmtime_r(&utc_time_sec, &tt);
+
+	/* start logging if we have a valid time and the time is not in the past */
+	if (log_name_timestamp && ttp && (utc_time_sec > PX4_EPOCH_SECS)) {
+		strftime(log_file_name, sizeof(log_file_name), "%H_%M_%S.px4log", &tt);
 		snprintf(log_file_path, sizeof(log_file_path), "%s/%s", log_dir, log_file_name);
 
 	} else {
@@ -408,8 +433,8 @@ int open_log_file()
 
 		/* look for the next file that does not exist */
 		while (file_number <= MAX_NO_LOGFILE) {
-			/* format log file path: e.g. /fs/microsd/sess001/log001.bin */
-			snprintf(log_file_name, sizeof(log_file_name), "log%03u.bin", file_number);
+			/* format log file path: e.g. /fs/microsd/sess001/log001.px4log */
+			snprintf(log_file_name, sizeof(log_file_name), "log%03u.px4log", file_number);
 			snprintf(log_file_path, sizeof(log_file_path), "%s/%s", log_dir, log_file_name);
 
 			if (!file_exist(log_file_path)) {
@@ -421,7 +446,7 @@ int open_log_file()
 
 		if (file_number > MAX_NO_LOGFILE) {
 			/* we should not end up here, either we have more than MAX_NO_LOGFILE on the SD card, or another problem */
-			mavlink_log_critical(mavlink_fd, "[sdlog2] ERR: max files %d", MAX_NO_LOGFILE);
+			mavlink_and_console_log_critical(mavlink_fd, "[sdlog2] ERR: max files %d", MAX_NO_LOGFILE);
 			return -1;
 		}
 	}
@@ -429,12 +454,10 @@ int open_log_file()
 	int fd = open(log_file_path, O_CREAT | O_WRONLY | O_DSYNC);
 
 	if (fd < 0) {
-		warn("failed opening log: %s", log_file_name);
-		mavlink_log_critical(mavlink_fd, "[sdlog2] failed opening log: %s", log_file_name);
+		mavlink_and_console_log_critical(mavlink_fd, "[sdlog2] failed opening: %s", log_file_name);
 
 	} else {
-		warnx("log file: %s", log_file_name);
-		mavlink_log_info(mavlink_fd, "[sdlog2] log file: %s", log_file_name);
+		mavlink_and_console_log_info(mavlink_fd, "[sdlog2] starting: %s", log_file_name);
 	}
 
 	return fd;
@@ -446,12 +469,15 @@ int open_perf_file(const char* str)
 	char log_file_name[32] = "";
 	char log_file_path[64] = "";
 
-	if (log_name_timestamp && gps_time != 0) {
-		/* use GPS time for log file naming, e.g. /fs/microsd/2014-01-19/19_37_52.bin */
-		time_t gps_time_sec = gps_time / 1000000;
-		struct tm t;
-		gmtime_r(&gps_time_sec, &t);
-		strftime(log_file_name, sizeof(log_file_name), "perf%H_%M_%S.txt", &t);
+	struct timespec ts;
+	clock_gettime(CLOCK_REALTIME, &ts);
+	/* use RTC time for log file naming, e.g. /fs/microsd/2014-01-19/19_37_52.txt */
+	time_t utc_time_sec = ts.tv_sec + (ts.tv_nsec / 1e9);
+	struct tm tt;
+	struct tm *ttp = gmtime_r(&utc_time_sec, &tt);
+
+	if (log_name_timestamp && ttp && (utc_time_sec > PX4_EPOCH_SECS)) {
+		strftime(log_file_name, sizeof(log_file_name), "perf%H_%M_%S.txt", &tt);
 		snprintf(log_file_path, sizeof(log_file_path), "%s/%s_%s", log_dir, str, log_file_name);
 
 	} else {
@@ -459,7 +485,7 @@ int open_perf_file(const char* str)
 
 		/* look for the next file that does not exist */
 		while (file_number <= MAX_NO_LOGFILE) {
-			/* format log file path: e.g. /fs/microsd/sess001/log001.bin */
+			/* format log file path: e.g. /fs/microsd/sess001/log001.txt */
 			snprintf(log_file_name, sizeof(log_file_name), "perf%03u.txt", file_number);
 			snprintf(log_file_path, sizeof(log_file_path), "%s/%s_%s", log_dir, str, log_file_name);
 
@@ -472,7 +498,7 @@ int open_perf_file(const char* str)
 
 		if (file_number > MAX_NO_LOGFILE) {
 			/* we should not end up here, either we have more than MAX_NO_LOGFILE on the SD card, or another problem */
-			mavlink_log_critical(mavlink_fd, "[sdlog2] ERR: max files %d", MAX_NO_LOGFILE);
+			mavlink_and_console_log_critical(mavlink_fd, "[sdlog2] ERR: max files %d", MAX_NO_LOGFILE);
 			return -1;
 		}
 	}
@@ -480,12 +506,8 @@ int open_perf_file(const char* str)
 	int fd = open(log_file_path, O_CREAT | O_WRONLY | O_DSYNC);
 
 	if (fd < 0) {
-		warn("failed opening log: %s", log_file_name);
-		mavlink_log_critical(mavlink_fd, "[sdlog2] failed opening log: %s", log_file_name);
+		mavlink_and_console_log_critical(mavlink_fd, "[sdlog2] failed opening: %s", log_file_name);
 
-	} else {
-		warnx("log file: %s", log_file_name);
-		mavlink_log_info(mavlink_fd, "[sdlog2] log file: %s", log_file_name);
 	}
 
 	return fd;
@@ -545,6 +567,7 @@ static void *logwriter_thread(void *arg)
 		pthread_mutex_unlock(&logbuffer_mutex);
 
 		if (available > 0) {
+
 			/* do heavy IO here */
 			if (available > MAX_WRITE_CHUNK) {
 				n = MAX_WRITE_CHUNK;
@@ -553,7 +576,9 @@ static void *logwriter_thread(void *arg)
 				n = available;
 			}
 
+			perf_begin(perf_write);
 			n = write(log_fd, read_ptr, n);
+			perf_end(perf_write);
 
 			should_wait = (n == available) && !is_part;
 
@@ -580,6 +605,16 @@ static void *logwriter_thread(void *arg)
 		if (++poll_count == 10) {
 			fsync(log_fd);
 			poll_count = 0;
+
+		}
+
+		if (log_bytes_written - last_checked_bytes_written > 20*1024*1024) {
+			/* check if space is available, if not stop everything */
+			if (check_free_space() != OK) {
+				logwriter_should_exit = true;
+				main_thread_should_exit = true;
+			}
+			last_checked_bytes_written = log_bytes_written;
 		}
 	}
 
@@ -591,15 +626,13 @@ static void *logwriter_thread(void *arg)
 
 void sdlog2_start_log()
 {
-	warnx("start logging");
-	mavlink_log_info(mavlink_fd, "[sdlog2] start logging");
-
 	/* create log dir if needed */
 	if (create_log_dir() != 0) {
-		mavlink_log_critical(mavlink_fd, "[sdlog2] error creating log dir");
-		errx(1, "error creating log dir");
+		mavlink_and_console_log_critical(mavlink_fd, "[sdlog2] error creating log dir");
+		exit(1);
 	}
 
+
 	/* initialize statistics counter */
 	log_bytes_written = 0;
 	start_time = hrt_absolute_time();
@@ -618,6 +651,9 @@ void sdlog2_start_log()
 
 	logwriter_should_exit = false;
 
+	/* allocate write performance counter */
+	perf_write = perf_alloc(PC_ELAPSED, "sd write");
+
 	/* start log buffer emptying thread */
 	if (0 != pthread_create(&logwriter_pthread, &logwriter_attr, logwriter_thread, &lb)) {
 		errx(1, "error creating logwriter thread");
@@ -637,9 +673,6 @@ void sdlog2_start_log()
 
 void sdlog2_stop_log()
 {
-	warnx("stop logging");
-	mavlink_log_info(mavlink_fd, "[sdlog2] stop logging");
-
 	logging_enabled = false;
 
 	/* wake up write thread one last time */
@@ -665,6 +698,11 @@ void sdlog2_stop_log()
 	perf_print_all(perf_fd);
 	close(perf_fd);
 
+	/* free log writer performance counter */
+	perf_free(perf_write);
+
+	mavlink_and_console_log_info(mavlink_fd, "[sdlog2] logging stopped");
+
 	sdlog2_status();
 }
 
@@ -764,7 +802,7 @@ int sdlog2_thread_main(int argc, char *argv[])
 	mavlink_fd = open(MAVLINK_LOG_DEVICE, 0);
 
 	if (mavlink_fd < 0) {
-		warnx("failed to open MAVLink log stream, start mavlink app first");
+		warnx("ERR: log stream, start mavlink app first");
 	}
 
 	/* delay = 1 / rate (rate defined by -r option), default log rate: 50 Hz */
@@ -892,11 +930,17 @@ int sdlog2_thread_main(int argc, char *argv[])
 
 	}
 
+
+	if (check_free_space() != OK) {
+		errx(1, "ERR: MicroSD almost full");
+	}
+
+
 	/* create log root dir */
 	int mkdir_ret = mkdir(log_root, S_IRWXU | S_IRWXG | S_IRWXO);
 
 	if (mkdir_ret != 0 && errno != EEXIST) {
-		err(1, "failed creating log root dir: %s", log_root);
+		err(1, "ERR: failed creating log dir: %s", log_root);
 	}
 
 	/* copy conversion scripts */
@@ -934,6 +978,7 @@ int sdlog2_thread_main(int argc, char *argv[])
 		struct vehicle_rates_setpoint_s rates_sp;
 		struct actuator_outputs_s act_outputs;
 		struct actuator_controls_s act_controls;
+		struct actuator_controls_s act_controls1;
 		struct vehicle_local_position_s local_pos;
 		struct vehicle_local_position_setpoint_s local_pos_sp;
 		struct vehicle_global_position_s global_pos;
@@ -956,6 +1001,7 @@ int sdlog2_thread_main(int argc, char *argv[])
 		struct satellite_info_s sat_info;
 		struct wind_estimate_s wind_estimate;
 		struct encoders_s encoders;
+		struct vtol_vehicle_status_s vtol_status;
 	} buf;
 
 	memset(&buf, 0, sizeof(buf));
@@ -975,6 +1021,7 @@ int sdlog2_thread_main(int argc, char *argv[])
 			struct log_GPS_s log_GPS;
 			struct log_ATTC_s log_ATTC;
 			struct log_STAT_s log_STAT;
+			struct log_VTOL_s log_VTOL;
 			struct log_RC_s log_RC;
 			struct log_OUT0_s log_OUT0;
 			struct log_AIRS_s log_AIRS;
@@ -1009,12 +1056,14 @@ int sdlog2_thread_main(int argc, char *argv[])
 	struct {
 		int cmd_sub;
 		int status_sub;
+		int vtol_status_sub;
 		int sensor_sub;
 		int att_sub;
 		int att_sp_sub;
 		int rates_sp_sub;
 		int act_outputs_sub;
 		int act_controls_sub;
+		int act_controls_1_sub;
 		int local_pos_sub;
 		int local_pos_sp_sub;
 		int global_pos_sub;
@@ -1041,13 +1090,15 @@ int sdlog2_thread_main(int argc, char *argv[])
 
 	subs.cmd_sub = orb_subscribe(ORB_ID(vehicle_command));
 	subs.status_sub = orb_subscribe(ORB_ID(vehicle_status));
+	subs.vtol_status_sub = orb_subscribe(ORB_ID(vtol_vehicle_status));
 	subs.gps_pos_sub = orb_subscribe(ORB_ID(vehicle_gps_position));
 	subs.sensor_sub = orb_subscribe(ORB_ID(sensor_combined));
 	subs.att_sub = orb_subscribe(ORB_ID(vehicle_attitude));
 	subs.att_sp_sub = orb_subscribe(ORB_ID(vehicle_attitude_setpoint));
 	subs.rates_sp_sub = orb_subscribe(ORB_ID(vehicle_rates_setpoint));
-	subs.act_outputs_sub = orb_subscribe(ORB_ID_VEHICLE_CONTROLS);
+	subs.act_outputs_sub = orb_subscribe(ORB_ID(actuator_outputs));
 	subs.act_controls_sub = orb_subscribe(ORB_ID_VEHICLE_ATTITUDE_CONTROLS);
+	subs.act_controls_1_sub = orb_subscribe(ORB_ID(actuator_controls_1));
 	subs.local_pos_sub = orb_subscribe(ORB_ID(vehicle_local_position));
 	subs.local_pos_sp_sub = orb_subscribe(ORB_ID(vehicle_local_position_setpoint));
 	subs.global_pos_sub = orb_subscribe(ORB_ID(vehicle_global_position));
@@ -1066,6 +1117,7 @@ int sdlog2_thread_main(int argc, char *argv[])
 	subs.system_power_sub = orb_subscribe(ORB_ID(system_power));
 	subs.servorail_status_sub = orb_subscribe(ORB_ID(servorail_status));
 	subs.wind_sub = orb_subscribe(ORB_ID(wind_estimate));
+	
 	/* we need to rate-limit wind, as we do not need the full update rate */
 	orb_set_interval(subs.wind_sub, 90);
 	subs.encoders_sub = orb_subscribe(ORB_ID(encoders));
@@ -1079,6 +1131,8 @@ int sdlog2_thread_main(int argc, char *argv[])
 
 	if (_extended_logging) {
 		subs.sat_info_sub = orb_subscribe(ORB_ID(satellite_info));
+	} else {
+		subs.sat_info_sub = 0;
 	}
 
 	/* close non-needed fd's */
@@ -1100,6 +1154,7 @@ int sdlog2_thread_main(int argc, char *argv[])
 	hrt_abstime magnetometer_timestamp = 0;
 	hrt_abstime barometer_timestamp = 0;
 	hrt_abstime differential_pressure_timestamp = 0;
+	hrt_abstime barometer1_timestamp = 0;
 	hrt_abstime gyro1_timestamp = 0;
 	hrt_abstime accelerometer1_timestamp = 0;
 	hrt_abstime magnetometer1_timestamp = 0;
@@ -1115,7 +1170,7 @@ int sdlog2_thread_main(int argc, char *argv[])
 		/* check GPS topic to get GPS time */
 		if (log_name_timestamp) {
 			if (!orb_copy(ORB_ID(vehicle_gps_position), subs.gps_pos_sub, &buf_gps_pos)) {
-				gps_time = buf_gps_pos.time_gps_usec;
+				gps_time = buf_gps_pos.time_utc_usec;
 			}
 		}
 
@@ -1143,7 +1198,7 @@ int sdlog2_thread_main(int argc, char *argv[])
 		bool gps_pos_updated = copy_if_updated(ORB_ID(vehicle_gps_position), subs.gps_pos_sub, &buf_gps_pos);
 
 		if (gps_pos_updated && log_name_timestamp) {
-			gps_time = buf_gps_pos.time_gps_usec;
+			gps_time = buf_gps_pos.time_utc_usec;
 		}
 
 		if (!logging_enabled) {
@@ -1169,11 +1224,18 @@ int sdlog2_thread_main(int argc, char *argv[])
 			LOGBUFFER_WRITE_AND_COUNT(STAT);
 		}
 
+		/* --- VTOL VEHICLE STATUS --- */
+		if(copy_if_updated(ORB_ID(vtol_vehicle_status), subs.vtol_status_sub, &buf.vtol_status)) {
+			log_msg.msg_type = LOG_VTOL_MSG;
+			log_msg.body.log_VTOL.airspeed_tot = buf.vtol_status.airspeed_tot;
+			LOGBUFFER_WRITE_AND_COUNT(VTOL);
+		}
+
 		/* --- GPS POSITION - UNIT #1 --- */
 		if (gps_pos_updated) {
 
 			log_msg.msg_type = LOG_GPS_MSG;
-			log_msg.body.log_GPS.gps_time = buf_gps_pos.time_gps_usec;
+			log_msg.body.log_GPS.gps_time = buf_gps_pos.time_utc_usec;
 			log_msg.body.log_GPS.fix_type = buf_gps_pos.fix_type;
 			log_msg.body.log_GPS.eph = buf_gps_pos.eph;
 			log_msg.body.log_GPS.epv = buf_gps_pos.epv;
@@ -1245,6 +1307,7 @@ int sdlog2_thread_main(int argc, char *argv[])
 			bool write_IMU1 = false;
 			bool write_IMU2 = false;
 			bool write_SENS = false;
+			bool write_SENS1 = false;
 
 			if (buf.sensor.timestamp != gyro_timestamp) {
 				gyro_timestamp = buf.sensor.timestamp;
@@ -1295,6 +1358,22 @@ int sdlog2_thread_main(int argc, char *argv[])
 				LOGBUFFER_WRITE_AND_COUNT(SENS);
 			}
 
+			if (buf.sensor.baro1_timestamp != barometer1_timestamp) {
+				barometer1_timestamp = buf.sensor.baro1_timestamp;
+				write_SENS1 = true;
+			}
+
+			if (write_SENS1) {
+				log_msg.msg_type = LOG_AIR1_MSG;
+				log_msg.body.log_SENS.baro_pres = buf.sensor.baro1_pres_mbar;
+				log_msg.body.log_SENS.baro_alt = buf.sensor.baro1_alt_meter;
+				log_msg.body.log_SENS.baro_temp = buf.sensor.baro1_temp_celcius;
+				log_msg.body.log_SENS.diff_pres = buf.sensor.differential_pressure1_pa;
+				log_msg.body.log_SENS.diff_pres_filtered = buf.sensor.differential_pressure1_filtered_pa;
+				// XXX moving to AIR0-AIR2 instead of SENS
+				LOGBUFFER_WRITE_AND_COUNT(SENS);
+			}
+
 			if (buf.sensor.accelerometer1_timestamp != accelerometer1_timestamp) {
 				accelerometer1_timestamp = buf.sensor.accelerometer1_timestamp;
 				write_IMU1 = true;
@@ -1358,6 +1437,10 @@ int sdlog2_thread_main(int argc, char *argv[])
 		/* --- ATTITUDE --- */
 		if (copy_if_updated(ORB_ID(vehicle_attitude), subs.att_sub, &buf.att)) {
 			log_msg.msg_type = LOG_ATT_MSG;
+			log_msg.body.log_ATT.q_w = buf.att.q[0];
+			log_msg.body.log_ATT.q_x = buf.att.q[1];
+			log_msg.body.log_ATT.q_y = buf.att.q[2];
+			log_msg.body.log_ATT.q_z = buf.att.q[3];
 			log_msg.body.log_ATT.roll = buf.att.roll;
 			log_msg.body.log_ATT.pitch = buf.att.pitch;
 			log_msg.body.log_ATT.yaw = buf.att.yaw;
@@ -1390,7 +1473,7 @@ int sdlog2_thread_main(int argc, char *argv[])
 		}
 
 		/* --- ACTUATOR OUTPUTS --- */
-		if (copy_if_updated(ORB_ID(actuator_outputs_0), subs.act_outputs_sub, &buf.act_outputs)) {
+		if (copy_if_updated(ORB_ID(actuator_outputs), subs.act_outputs_sub, &buf.act_outputs)) {
 			log_msg.msg_type = LOG_OUT0_MSG;
 			memcpy(log_msg.body.log_OUT0.output, buf.act_outputs.output, sizeof(log_msg.body.log_OUT0.output));
 			LOGBUFFER_WRITE_AND_COUNT(OUT0);
@@ -1406,6 +1489,16 @@ int sdlog2_thread_main(int argc, char *argv[])
 			LOGBUFFER_WRITE_AND_COUNT(ATTC);
 		}
 
+		/* --- ACTUATOR CONTROL FW VTOL --- */
+		if(copy_if_updated(ORB_ID(actuator_controls_1),subs.act_controls_1_sub,&buf.act_controls)) {
+			log_msg.msg_type = LOG_ATC1_MSG;
+			log_msg.body.log_ATTC.roll = buf.act_controls.control[0];
+			log_msg.body.log_ATTC.pitch = buf.act_controls.control[1];
+			log_msg.body.log_ATTC.yaw = buf.act_controls.control[2];
+			log_msg.body.log_ATTC.thrust = buf.act_controls.control[3];
+			LOGBUFFER_WRITE_AND_COUNT(ATTC);
+		}
+
 		/* --- LOCAL POSITION --- */
 		if (copy_if_updated(ORB_ID(vehicle_local_position), subs.local_pos_sub, &buf.local_pos)) {
 			log_msg.msg_type = LOG_LPOS_MSG;
@@ -1426,7 +1519,6 @@ int sdlog2_thread_main(int argc, char *argv[])
 											  (buf.local_pos.v_z_valid ? 8 : 0) |
 											  (buf.local_pos.xy_global ? 16 : 0) |
 											  (buf.local_pos.z_global ? 32 : 0);
-			log_msg.body.log_LPOS.landed = buf.local_pos.landed;
 			log_msg.body.log_LPOS.ground_dist_flags = (buf.local_pos.dist_bottom_valid ? 1 : 0);
 			log_msg.body.log_LPOS.eph = buf.local_pos.eph;
 			log_msg.body.log_LPOS.epv = buf.local_pos.epv;
@@ -1511,11 +1603,14 @@ int sdlog2_thread_main(int argc, char *argv[])
 		/* --- FLOW --- */
 		if (copy_if_updated(ORB_ID(optical_flow), subs.flow_sub, &buf.flow)) {
 			log_msg.msg_type = LOG_FLOW_MSG;
-			log_msg.body.log_FLOW.flow_raw_x = buf.flow.flow_raw_x;
-			log_msg.body.log_FLOW.flow_raw_y = buf.flow.flow_raw_y;
-			log_msg.body.log_FLOW.flow_comp_x = buf.flow.flow_comp_x_m;
-			log_msg.body.log_FLOW.flow_comp_y = buf.flow.flow_comp_y_m;
-			log_msg.body.log_FLOW.distance = buf.flow.ground_distance_m;
+			log_msg.body.log_FLOW.ground_distance_m = buf.flow.ground_distance_m;
+			log_msg.body.log_FLOW.gyro_temperature = buf.flow.gyro_temperature;
+			log_msg.body.log_FLOW.gyro_x_rate_integral = buf.flow.gyro_x_rate_integral;
+			log_msg.body.log_FLOW.gyro_y_rate_integral = buf.flow.gyro_y_rate_integral;
+			log_msg.body.log_FLOW.gyro_z_rate_integral = buf.flow.gyro_z_rate_integral;
+			log_msg.body.log_FLOW.integration_timespan = buf.flow.integration_timespan;
+			log_msg.body.log_FLOW.pixel_flow_x_integral = buf.flow.pixel_flow_x_integral;
+			log_msg.body.log_FLOW.pixel_flow_y_integral = buf.flow.pixel_flow_y_integral;
 			log_msg.body.log_FLOW.quality = buf.flow.quality;
 			log_msg.body.log_FLOW.sensor_id = buf.flow.sensor_id;
 			LOGBUFFER_WRITE_AND_COUNT(FLOW);
@@ -1700,8 +1795,6 @@ int sdlog2_thread_main(int argc, char *argv[])
 
 	free(lb.data);
 
-	warnx("exiting");
-
 	thread_running = false;
 
 	return 0;
@@ -1734,7 +1827,7 @@ int file_copy(const char *file_old, const char *file_new)
 	int ret = 0;
 
 	if (source == NULL) {
-		warnx("failed opening input file to copy");
+		warnx("ERR: open in");
 		return 1;
 	}
 
@@ -1742,7 +1835,7 @@ int file_copy(const char *file_old, const char *file_new)
 
 	if (target == NULL) {
 		fclose(source);
-		warnx("failed to open output file to copy");
+		warnx("ERR: open out");
 		return 1;
 	}
 
@@ -1767,6 +1860,34 @@ int file_copy(const char *file_old, const char *file_new)
 	return OK;
 }
 
+int check_free_space()
+{
+	/* use statfs to determine the number of blocks left */
+	FAR struct statfs statfs_buf;
+	if (statfs(mountpoint, &statfs_buf) != OK) {
+		errx(ERROR, "ERR: statfs");
+	}
+
+	/* use a threshold of 50 MiB */
+	if (statfs_buf.f_bavail < (int)(50 * 1024 * 1024 / statfs_buf.f_bsize)) {
+		mavlink_and_console_log_critical(mavlink_fd,
+			"[sdlog2] no space on MicroSD: %u MiB",
+			(unsigned int)(statfs_buf.f_bavail * statfs_buf.f_bsize) / (1024U * 1024U));
+		/* we do not need a flag to remember that we sent this warning because we will exit anyway */
+		return ERROR;
+
+	/* use a threshold of 100 MiB to send a warning */
+	} else if (!space_warning_sent && statfs_buf.f_bavail < (int)(100 * 1024 * 1024 / statfs_buf.f_bsize)) {
+		mavlink_and_console_log_critical(mavlink_fd,
+			"[sdlog2] space on MicroSD low: %u MiB",
+			(unsigned int)(statfs_buf.f_bavail * statfs_buf.f_bsize) / (1024U * 1024U));
+		/* we don't want to flood the user with warnings */
+		space_warning_sent = true;
+	}
+
+	return OK;
+}
+
 void handle_command(struct vehicle_command_s *cmd)
 {
 	int param;
diff --git a/src/modules/sdlog2/sdlog2_messages.h b/src/modules/sdlog2/sdlog2_messages.h
index fa9bdacb8..7080d9c31 100644
--- a/src/modules/sdlog2/sdlog2_messages.h
+++ b/src/modules/sdlog2/sdlog2_messages.h
@@ -51,6 +51,10 @@
 /* --- ATT - ATTITUDE --- */
 #define LOG_ATT_MSG 2
 struct log_ATT_s {
+	float q_w;
+	float q_x;
+	float q_y;
+	float q_z;
 	float roll;
 	float pitch;
 	float yaw;
@@ -112,7 +116,6 @@ struct log_LPOS_s {
 	int32_t ref_lon;
 	float ref_alt;
 	uint8_t pos_flags;
-	uint8_t landed;
 	uint8_t ground_dist_flags;
 	float eph;
 	float epv;
@@ -149,6 +152,7 @@ struct log_GPS_s {
 
 /* --- ATTC - ATTITUDE CONTROLS (ACTUATOR_0 CONTROLS)--- */
 #define LOG_ATTC_MSG 9
+#define LOG_ATC1_MSG 40
 struct log_ATTC_s {
 	float roll;
 	float pitch;
@@ -200,13 +204,19 @@ struct log_ARSP_s {
 /* --- FLOW - OPTICAL FLOW --- */
 #define LOG_FLOW_MSG 15
 struct log_FLOW_s {
-	int16_t flow_raw_x;
-	int16_t flow_raw_y;
-	float flow_comp_x;
-	float flow_comp_y;
-	float distance;
-	uint8_t	quality;
+	uint64_t timestamp;
 	uint8_t sensor_id;
+	float pixel_flow_x_integral;
+	float pixel_flow_y_integral;
+	float gyro_x_rate_integral;
+	float gyro_y_rate_integral;
+	float gyro_z_rate_integral;
+	float ground_distance_m;
+	uint32_t integration_timespan;
+	uint32_t time_since_last_sonar_update;
+	uint16_t frame_count_since_last_readout;
+	int16_t gyro_temperature;
+	uint8_t	quality;
 };
 
 /* --- GPOS - GLOBAL POSITION ESTIMATE --- */
@@ -416,6 +426,14 @@ struct log_ENCD_s {
 	float vel1;
 };
 
+/* --- AIR SPEED SENSORS - DIFF. PRESSURE --- */
+#define LOG_AIR1_MSG 40
+
+/* --- VTOL - VTOL VEHICLE STATUS */
+#define LOG_VTOL_MSG 42
+struct log_VTOL_s {
+	float airspeed_tot;
+};
 
 /********** SYSTEM MESSAGES, ID > 0x80 **********/
 
@@ -444,22 +462,26 @@ struct log_PARM_s {
 /* construct list of all message formats */
 static const struct log_format_s log_formats[] = {
 	/* business-level messages, ID < 0x80 */
-	LOG_FORMAT(ATT, "fffffffff",		"Roll,Pitch,Yaw,RollRate,PitchRate,YawRate,GX,GY,GZ"),
+	LOG_FORMAT_S(ATT, ATT, "fffffffff",		"Roll,Pitch,Yaw,RollRate,PitchRate,YawRate,GX,GY,GZ"),
+	LOG_FORMAT(ATT, "fffffffffffff",	"qw,qx,qy,qz,Roll,Pitch,Yaw,RollRate,PitchRate,YawRate,GX,GY,GZ"),
 	LOG_FORMAT(ATSP, "ffff",		"RollSP,PitchSP,YawSP,ThrustSP"),
 	LOG_FORMAT_S(IMU, IMU, "fffffffff",		"AccX,AccY,AccZ,GyroX,GyroY,GyroZ,MagX,MagY,MagZ"),
 	LOG_FORMAT_S(IMU1, IMU, "fffffffff",		"AccX,AccY,AccZ,GyroX,GyroY,GyroZ,MagX,MagY,MagZ"),
 	LOG_FORMAT_S(IMU2, IMU, "fffffffff",		"AccX,AccY,AccZ,GyroX,GyroY,GyroZ,MagX,MagY,MagZ"),
-	LOG_FORMAT(SENS, "fffff",		"BaroPres,BaroAlt,BaroTemp,DiffPres,DiffPresFilt"),
-	LOG_FORMAT(LPOS, "ffffffffLLfBBBff",	"X,Y,Z,Dist,DistR,VX,VY,VZ,RLat,RLon,RAlt,PFlg,LFlg,GFlg,EPH,EPV"),
+	LOG_FORMAT_S(SENS, SENS, "fffff",		"BaroPres,BaroAlt,BaroTemp,DiffPres,DiffPresFilt"),
+	LOG_FORMAT_S(AIR1, SENS, "fffff",	"BaroPa,BaroAlt,BaroTmp,DiffPres,DiffPresF"),
+	LOG_FORMAT(LPOS, "ffffffffLLfBBff",	"X,Y,Z,Dist,DistR,VX,VY,VZ,RLat,RLon,RAlt,PFlg,GFlg,EPH,EPV"),
 	LOG_FORMAT(LPSP, "ffff",		"X,Y,Z,Yaw"),
 	LOG_FORMAT(GPS, "QBffLLfffffBHHH",	"GPSTime,Fix,EPH,EPV,Lat,Lon,Alt,VelN,VelE,VelD,Cog,nSat,SNR,N,J"),
-	LOG_FORMAT(ATTC, "ffff",		"Roll,Pitch,Yaw,Thrust"),
+	LOG_FORMAT_S(ATTC, ATTC, "ffff",		"Roll,Pitch,Yaw,Thrust"),
+	LOG_FORMAT_S(ATC1, ATTC, "ffff",		"Roll,Pitch,Yaw,Thrust"),
 	LOG_FORMAT(STAT, "BBBfBB",		"MainState,ArmState,FailsafeState,BatRem,BatWarn,Landed"),
+	LOG_FORMAT(VTOL, "f",		"Arsp"),
 	LOG_FORMAT(RC, "ffffffffBB",		"Ch0,Ch1,Ch2,Ch3,Ch4,Ch5,Ch6,Ch7,Count,SignalLost"),
 	LOG_FORMAT(OUT0, "ffffffff",		"Out0,Out1,Out2,Out3,Out4,Out5,Out6,Out7"),
 	LOG_FORMAT(AIRS, "fff",			"IndSpeed,TrueSpeed,AirTemp"),
 	LOG_FORMAT(ARSP, "fff",			"RollRateSP,PitchRateSP,YawRateSP"),
-	LOG_FORMAT(FLOW, "hhfffBB",		"RawX,RawY,CompX,CompY,Dist,Q,SensID"),
+	LOG_FORMAT(FLOW, "QBffffffLLHhB",	"IntT,ID,RawX,RawY,RX,RY,RZ,Dist,TSpan,DtSonar,FrmCnt,GT,Qlty"),
 	LOG_FORMAT(GPOS, "LLfffffff",		"Lat,Lon,Alt,VelN,VelE,VelD,EPH,EPV,TALT"),
 	LOG_FORMAT(GPSP, "BLLffBfbf",		"NavState,Lat,Lon,Alt,Yaw,Type,LoitR,LoitDir,PitMin"),
 	LOG_FORMAT(ESC, "HBBBHHffiffH",		"count,nESC,Conn,N,Ver,Adr,Volt,Amp,RPM,Temp,SetP,SetPRAW"),
diff --git a/src/modules/segway/segway_main.cpp b/src/modules/segway/segway_main.cpp
index 061fbf9b9..ee492f85a 100644
--- a/src/modules/segway/segway_main.cpp
+++ b/src/modules/segway/segway_main.cpp
@@ -110,7 +110,7 @@ int segway_main(int argc, char *argv[])
 					 SCHED_PRIORITY_MAX - 10,
 					 5120,
 					 segway_thread_main,
-					 (argv) ? (const char **)&argv[2] : (const char **)NULL);
+					 (argv) ? (char * const *)&argv[2] : (char * const *)NULL);
 		exit(0);
 	}
 
diff --git a/src/modules/sensors/module.mk b/src/modules/sensors/module.mk
index dfbba92d9..f37bc9327 100644
--- a/src/modules/sensors/module.mk
+++ b/src/modules/sensors/module.mk
@@ -44,3 +44,5 @@ SRCS		= sensors.cpp \
 MODULE_STACKSIZE = 1200
 
 MAXOPTIMIZATION	 = -Os
+
+EXTRACXXFLAGS	= -Wno-type-limits
diff --git a/src/modules/sensors/sensor_params.c b/src/modules/sensors/sensor_params.c
index 229bfe3ce..67b7feef7 100644
--- a/src/modules/sensors/sensor_params.c
+++ b/src/modules/sensors/sensor_params.c
@@ -45,6 +45,13 @@
 #include <systemlib/param/param.h>
 
 /**
+ * ID of the Gyro that the calibration is for.
+ *
+ * @group Sensor Calibration
+ */
+PARAM_DEFINE_INT32(SENS_GYRO_ID, 0);
+
+/**
  * Gyro X-axis offset
  *
  * @min -10.0
@@ -98,6 +105,12 @@ PARAM_DEFINE_FLOAT(SENS_GYRO_YSCALE, 1.0f);
  */
 PARAM_DEFINE_FLOAT(SENS_GYRO_ZSCALE, 1.0f);
 
+/**
+ * ID of Magnetometer the calibration is for.
+ *
+ * @group Sensor Calibration
+ */
+PARAM_DEFINE_INT32(SENS_MAG_ID, 0);
 
 /**
  * Magnetometer X-axis offset
@@ -147,6 +160,12 @@ PARAM_DEFINE_FLOAT(SENS_MAG_YSCALE, 1.0f);
  */
 PARAM_DEFINE_FLOAT(SENS_MAG_ZSCALE, 1.0f);
 
+/**
+ * ID of the Accelerometer that the calibration is for.
+ *
+ * @group Sensor Calibration
+ */
+PARAM_DEFINE_INT32(SENS_ACC_ID, 0);
 
 /**
  * Accelerometer X-axis offset
@@ -262,6 +281,25 @@ PARAM_DEFINE_FLOAT(SENS_BARO_QNH, 1013.25f);
 PARAM_DEFINE_INT32(SENS_BOARD_ROT, 0);
 
 /**
+ * PX4Flow board rotation
+ *
+ * This parameter defines the rotation of the PX4FLOW board relative to the platform.
+ * Zero rotation is defined as Y on flow board pointing towards front of vehicle
+ * Possible values are:
+ *    0 = No rotation
+ *    1 = Yaw 45°
+ *    2 = Yaw 90°
+ *    3 = Yaw 135°
+ *    4 = Yaw 180°
+ *    5 = Yaw 225°
+ *    6 = Yaw 270°
+ *    7 = Yaw 315°
+ *
+ * @group Sensor Calibration
+ */
+PARAM_DEFINE_INT32(SENS_FLOW_ROT, 0);
+
+/**
  * Board rotation Y (Pitch) offset
  *
  * This parameter defines a rotational offset in degrees around the Y (Pitch) axis. It allows the user
@@ -747,6 +785,41 @@ PARAM_DEFINE_INT32(RC_MAP_AUX2, 0);	/**< default function: camera roll */
  */
 PARAM_DEFINE_INT32(RC_MAP_AUX3, 0);
 
+/**
+ * Channel which changes a parameter
+ *
+ * Can be used for parameter tuning with the RC. This one is further referenced as the 1st parameter channel.
+ * Set to 0 to deactivate *
+ *
+ * @min 0
+ * @max 18
+ * @group Radio Calibration
+ */
+PARAM_DEFINE_INT32(RC_MAP_PARAM1, 0);
+
+/**
+ * Channel which changes a parameter
+ *
+ * Can be used for parameter tuning with the RC. This one is further referenced as the 2nd parameter channel.
+ * Set to 0 to deactivate *
+ *
+ * @min 0
+ * @max 18
+ * @group Radio Calibration
+ */
+PARAM_DEFINE_INT32(RC_MAP_PARAM2, 0);
+
+/**
+ * Channel which changes a parameter
+ *
+ * Can be used for parameter tuning with the RC. This one is further referenced as the 3th parameter channel.
+ * Set to 0 to deactivate *
+ *
+ * @min 0
+ * @max 18
+ * @group Radio Calibration
+ */
+PARAM_DEFINE_INT32(RC_MAP_PARAM3, 0);
 
 /**
  * Failsafe channel PWM threshold.
diff --git a/src/modules/sensors/sensors.cpp b/src/modules/sensors/sensors.cpp
index cdcb428dd..630c54335 100644
--- a/src/modules/sensors/sensors.cpp
+++ b/src/modules/sensors/sensors.cpp
@@ -1,6 +1,6 @@
 /****************************************************************************
  *
- *   Copyright (c) 2012-2014 PX4 Development Team. All rights reserved.
+ *   Copyright (c) 2012-2015 PX4 Development Team. All rights reserved.
  *
  * Redistribution and use in source and binary forms, with or without
  * modification, are permitted provided that the following conditions
@@ -37,7 +37,7 @@
  *
  * @author Lorenz Meier <lm@inf.ethz.ch>
  * @author Julian Oes <joes@student.ethz.ch>
- * @author Thomas Gubler <thomasgubler@student.ethz.ch>
+ * @author Thomas Gubler <thomasgubler@gmail.com>
  */
 
 #include <nuttx/config.h>
@@ -63,6 +63,7 @@
 #include <drivers/drv_rc_input.h>
 #include <drivers/drv_adc.h>
 #include <drivers/drv_airspeed.h>
+#include <drivers/drv_px4flow.h>
 
 #include <systemlib/systemlib.h>
 #include <systemlib/param/param.h>
@@ -82,6 +83,7 @@
 #include <uORB/topics/battery_status.h>
 #include <uORB/topics/differential_pressure.h>
 #include <uORB/topics/airspeed.h>
+#include <uORB/topics/rc_parameter_map.h>
 
 #define GYRO_HEALTH_COUNTER_LIMIT_ERROR 20   /* 40 ms downtime at 500 Hz update rate   */
 #define ACC_HEALTH_COUNTER_LIMIT_ERROR  20   /* 40 ms downtime at 500 Hz update rate   */
@@ -191,6 +193,14 @@ private:
 	switch_pos_t	get_rc_sw2pos_position(enum RC_CHANNELS_FUNCTION func, float on_th, bool on_inv);
 
 	/**
+	 * Update paramters from RC channels if the functionality is activated and the
+	 * input has changed since the last update
+	 *
+	 * @param
+	 */
+	void set_params_from_rc();
+
+	/**
 	 * Gather and publish RC input data.
 	 */
 	void		rc_poll();
@@ -216,10 +226,12 @@ private:
 	int		_mag2_sub;			/**< raw mag2 data subscription */
 	int 		_rc_sub;			/**< raw rc channels data subscription */
 	int		_baro_sub;			/**< raw baro data subscription */
+	int		_baro1_sub;			/**< raw baro data subscription */
 	int		_airspeed_sub;			/**< airspeed subscription */
 	int		_diff_pres_sub;			/**< raw differential pressure subscription */
 	int		_vcontrol_mode_sub;			/**< vehicle control mode subscription */
 	int 		_params_sub;			/**< notification of parameter updates */
+	int		_rc_parameter_map_sub;			/**< rc parameter map subscription */
 	int 		_manual_control_sub;			/**< notification of manual control updates */
 
 	orb_advert_t	_sensor_pub;			/**< combined sensor data topic */
@@ -237,6 +249,7 @@ private:
 	struct baro_report _barometer;			/**< barometer data */
 	struct differential_pressure_s _diff_pres;
 	struct airspeed_s _airspeed;
+	struct rc_parameter_map_s _rc_parameter_map;
 
 	math::Matrix<3, 3>	_board_rotation;		/**< rotation matrix for the orientation that the board is mounted */
 	math::Matrix<3, 3>	_external_mag_rotation;		/**< rotation matrix for the orientation that an external mag is mounted */
@@ -263,6 +276,7 @@ private:
 		float diff_pres_analog_scale;
 
 		int board_rotation;
+		int flow_rotation;
 		int external_mag_rotation;
 
 		float board_offset[3];
@@ -288,6 +302,8 @@ private:
 		int rc_map_aux4;
 		int rc_map_aux5;
 
+		int rc_map_param[RC_PARAM_MAP_NCHAN];
+
 		int32_t rc_fails_thr;
 		float rc_assist_th;
 		float rc_auto_th;
@@ -348,6 +364,12 @@ private:
 		param_t rc_map_aux4;
 		param_t rc_map_aux5;
 
+		param_t rc_map_param[RC_PARAM_MAP_NCHAN];
+		param_t rc_param[RC_PARAM_MAP_NCHAN];	/**< param handles for the paramters which are bound
+							  to a RC channel, equivalent float values in the
+							  _parameters struct are not existing
+							  because these parameters are never read. */
+
 		param_t rc_fails_thr;
 		param_t rc_assist_th;
 		param_t rc_auto_th;
@@ -361,6 +383,7 @@ private:
 		param_t battery_current_scaling;
 
 		param_t board_rotation;
+		param_t flow_rotation;
 		param_t external_mag_rotation;
 
 		param_t board_offset[3];
@@ -378,27 +401,27 @@ private:
 	/**
 	 * Do accel-related initialisation.
 	 */
-	void		accel_init();
+	int		accel_init();
 
 	/**
 	 * Do gyro-related initialisation.
 	 */
-	void		gyro_init();
+	int		gyro_init();
 
 	/**
 	 * Do mag-related initialisation.
 	 */
-	void		mag_init();
+	int		mag_init();
 
 	/**
 	 * Do baro-related initialisation.
 	 */
-	void		baro_init();
+	int		baro_init();
 
 	/**
 	 * Do adc-related initialisation.
 	 */
-	void		adc_init();
+	int		adc_init();
 
 	/**
 	 * Poll the accelerometer for updated data.
@@ -451,6 +474,11 @@ private:
 	void 		parameter_update_poll(bool forced = false);
 
 	/**
+	 * Check for changes in rc_parameter_map
+	 */
+	void 		rc_parameter_map_poll(bool forced = false);
+
+	/**
 	 * Poll the ADC and update readings to suit.
 	 *
 	 * @param raw			Combined sensor data structure into which
@@ -479,7 +507,7 @@ Sensors::Sensors() :
 	_fd_adc(-1),
 	_last_adc(0),
 
-	_task_should_exit(false),
+	_task_should_exit(true),
 	_sensors_task(-1),
 	_hil_enabled(false),
 	_publishing(true),
@@ -496,8 +524,10 @@ Sensors::Sensors() :
 	_mag2_sub(-1),
 	_rc_sub(-1),
 	_baro_sub(-1),
+	_baro1_sub(-1),
 	_vcontrol_mode_sub(-1),
 	_params_sub(-1),
+	_rc_parameter_map_sub(-1),
 	_manual_control_sub(-1),
 
 /* publications */
@@ -518,6 +548,7 @@ Sensors::Sensors() :
 {
 	memset(&_rc, 0, sizeof(_rc));
 	memset(&_diff_pres, 0, sizeof(_diff_pres));
+	memset(&_rc_parameter_map, 0, sizeof(_rc_parameter_map));
 
 	/* basic r/c parameters */
 	for (unsigned i = 0; i < _rc_max_chan_count; i++) {
@@ -570,6 +601,13 @@ Sensors::Sensors() :
 	_parameter_handles.rc_map_aux4 = param_find("RC_MAP_AUX4");
 	_parameter_handles.rc_map_aux5 = param_find("RC_MAP_AUX5");
 
+	/* RC to parameter mapping for changing parameters with RC */
+	for (int i = 0; i < RC_PARAM_MAP_NCHAN; i++) {
+		char name[PARAM_ID_LEN];
+		snprintf(name, PARAM_ID_LEN, "RC_MAP_PARAM%d", i + 1); // shifted by 1 because param name starts at 1
+		_parameter_handles.rc_map_param[i] = param_find(name);
+	}
+
 	/* RC thresholds */
 	_parameter_handles.rc_fails_thr = param_find("RC_FAILS_THR");
 	_parameter_handles.rc_assist_th = param_find("RC_ASSIST_TH");
@@ -614,6 +652,7 @@ Sensors::Sensors() :
 
 	/* rotations */
 	_parameter_handles.board_rotation = param_find("SENS_BOARD_ROT");
+	_parameter_handles.flow_rotation = param_find("SENS_FLOW_ROT");
 	_parameter_handles.external_mag_rotation = param_find("SENS_EXT_MAG_ROT");
 
 	/* rotation offsets */
@@ -747,6 +786,9 @@ Sensors::parameters_update()
 	param_get(_parameter_handles.rc_map_aux3, &(_parameters.rc_map_aux3));
 	param_get(_parameter_handles.rc_map_aux4, &(_parameters.rc_map_aux4));
 	param_get(_parameter_handles.rc_map_aux5, &(_parameters.rc_map_aux5));
+	for (int i = 0; i < RC_PARAM_MAP_NCHAN; i++) {
+		param_get(_parameter_handles.rc_map_param[i], &(_parameters.rc_map_param[i]));
+	}
 	param_get(_parameter_handles.rc_fails_thr, &(_parameters.rc_fails_thr));
 	param_get(_parameter_handles.rc_assist_th, &(_parameters.rc_assist_th));
 	_parameters.rc_assist_inv = (_parameters.rc_assist_th < 0);
@@ -791,6 +833,10 @@ Sensors::parameters_update()
 	_rc.function[AUX_4] = _parameters.rc_map_aux4 - 1;
 	_rc.function[AUX_5] = _parameters.rc_map_aux5 - 1;
 
+	for (int i = 0; i < RC_PARAM_MAP_NCHAN; i++) {
+		_rc.function[PARAM_1 + i] = _parameters.rc_map_param[i] - 1;
+	}
+
 	/* gyro offsets */
 	param_get(_parameter_handles.gyro_offset[0], &(_parameters.gyro_offset[0]));
 	param_get(_parameter_handles.gyro_offset[1], &(_parameters.gyro_offset[1]));
@@ -831,8 +877,22 @@ Sensors::parameters_update()
 	}
 
 	param_get(_parameter_handles.board_rotation, &(_parameters.board_rotation));
+	param_get(_parameter_handles.flow_rotation, &(_parameters.flow_rotation));
 	param_get(_parameter_handles.external_mag_rotation, &(_parameters.external_mag_rotation));
 
+	/* set px4flow rotation */
+	int	flowfd;
+	flowfd = open(PX4FLOW_DEVICE_PATH, 0);
+	if (flowfd >= 0) {
+ 		int flowret = ioctl(flowfd, SENSORIOCSROTATION, _parameters.flow_rotation);
+ 		if (flowret) {
+ 			warnx("flow rotation could not be set");
+			close(flowfd);
+			return ERROR;
+ 		}
+		close(flowfd);
+ 	}	
+	
 	get_rot_matrix((enum Rotation)_parameters.board_rotation, &_board_rotation);
 	get_rot_matrix((enum Rotation)_parameters.external_mag_rotation, &_external_mag_rotation);
 
@@ -850,26 +910,26 @@ Sensors::parameters_update()
 
 	/* update barometer qnh setting */
 	param_get(_parameter_handles.baro_qnh, &(_parameters.baro_qnh));
-	int	fd;
-	fd = open(BARO_DEVICE_PATH, 0);
-	if (fd < 0) {
-		warn("%s", BARO_DEVICE_PATH);
-		errx(1, "FATAL: no barometer found");
+	int	barofd;
+	barofd = open(BARO_DEVICE_PATH, 0);
+	if (barofd < 0) {
+		warnx("ERROR: no barometer foundon %s", BARO_DEVICE_PATH);
+		return ERROR;
 
 	} else {
-		int ret = ioctl(fd, BAROIOCSMSLPRESSURE, (unsigned long)(_parameters.baro_qnh * 100));
-		if (ret) {
+		int baroret = ioctl(barofd, BAROIOCSMSLPRESSURE, (unsigned long)(_parameters.baro_qnh * 100));
+		if (baroret) {
 			warnx("qnh could not be set");
-			close(fd);
+			close(barofd);
 			return ERROR;
 		}
-		close(fd);
+		close(barofd);
 	}
 
 	return OK;
 }
 
-void
+int
 Sensors::accel_init()
 {
 	int	fd;
@@ -877,8 +937,8 @@ Sensors::accel_init()
 	fd = open(ACCEL_DEVICE_PATH, 0);
 
 	if (fd < 0) {
-		warn("%s", ACCEL_DEVICE_PATH);
-		errx(1, "FATAL: no accelerometer found");
+		warnx("FATAL: no accelerometer found: %s", ACCEL_DEVICE_PATH);
+		return ERROR;
 
 	} else {
 
@@ -907,9 +967,11 @@ Sensors::accel_init()
 
 		close(fd);
 	}
+
+	return OK;
 }
 
-void
+int
 Sensors::gyro_init()
 {
 	int	fd;
@@ -917,8 +979,8 @@ Sensors::gyro_init()
 	fd = open(GYRO_DEVICE_PATH, 0);
 
 	if (fd < 0) {
-		warn("%s", GYRO_DEVICE_PATH);
-		errx(1, "FATAL: no gyro found");
+		warnx("FATAL: no gyro found: %s", GYRO_DEVICE_PATH);
+		return ERROR;
 
 	} else {
 
@@ -948,9 +1010,11 @@ Sensors::gyro_init()
 
 		close(fd);
 	}
+
+	return OK;
 }
 
-void
+int
 Sensors::mag_init()
 {
 	int	fd;
@@ -959,8 +1023,8 @@ Sensors::mag_init()
 	fd = open(MAG_DEVICE_PATH, 0);
 
 	if (fd < 0) {
-		warn("%s", MAG_DEVICE_PATH);
-		errx(1, "FATAL: no magnetometer found");
+		warnx("FATAL: no magnetometer found: %s", MAG_DEVICE_PATH);
+		return ERROR;
 	}
 
 	/* try different mag sampling rates */
@@ -981,7 +1045,8 @@ Sensors::mag_init()
 			ioctl(fd, SENSORIOCSPOLLRATE, 100);
 
 		} else {
-			errx(1, "FATAL: mag sampling rate could not be set");
+			warnx("FATAL: mag sampling rate could not be set");
+			return ERROR;
 		}
 	}
 
@@ -990,7 +1055,8 @@ Sensors::mag_init()
 	ret = ioctl(fd, MAGIOCGEXTERNAL, 0);
 
 	if (ret < 0) {
-		errx(1, "FATAL: unknown if magnetometer is external or onboard");
+		warnx("FATAL: unknown if magnetometer is external or onboard");
+		return ERROR;
 
 	} else if (ret == 1) {
 		_mag_is_external = true;
@@ -1000,9 +1066,11 @@ Sensors::mag_init()
 	}
 
 	close(fd);
+
+	return OK;
 }
 
-void
+int
 Sensors::baro_init()
 {
 	int	fd;
@@ -1010,26 +1078,30 @@ Sensors::baro_init()
 	fd = open(BARO_DEVICE_PATH, 0);
 
 	if (fd < 0) {
-		warn("%s", BARO_DEVICE_PATH);
-		errx(1, "FATAL: No barometer found");
+		warnx("FATAL: No barometer found: %s", BARO_DEVICE_PATH);
+		return ERROR;
 	}
 
 	/* set the driver to poll at 150Hz */
 	ioctl(fd, SENSORIOCSPOLLRATE, 150);
 
 	close(fd);
+
+	return OK;
 }
 
-void
+int
 Sensors::adc_init()
 {
 
 	_fd_adc = open(ADC_DEVICE_PATH, O_RDONLY | O_NONBLOCK);
 
 	if (_fd_adc < 0) {
-		warn(ADC_DEVICE_PATH);
-		warnx("FATAL: no ADC found");
+		warnx("FATAL: no ADC found: %s", ADC_DEVICE_PATH);
+		return ERROR;
 	}
+
+	return OK;
 }
 
 void
@@ -1041,7 +1113,7 @@ Sensors::accel_poll(struct sensor_combined_s &raw)
 	if (accel_updated) {
 		struct accel_report	accel_report;
 
-		orb_copy(ORB_ID(sensor_accel0), _accel_sub, &accel_report);
+		orb_copy(ORB_ID(sensor_accel), _accel_sub, &accel_report);
 
 		math::Vector<3> vect(accel_report.x, accel_report.y, accel_report.z);
 		vect = _board_rotation * vect;
@@ -1062,7 +1134,7 @@ Sensors::accel_poll(struct sensor_combined_s &raw)
 	if (accel_updated) {
 		struct accel_report	accel_report;
 
-		orb_copy(ORB_ID(sensor_accel1), _accel_sub, &accel_report);
+		orb_copy(ORB_ID(sensor_accel), _accel1_sub, &accel_report);
 
 		math::Vector<3> vect(accel_report.x, accel_report.y, accel_report.z);
 		vect = _board_rotation * vect;
@@ -1083,7 +1155,7 @@ Sensors::accel_poll(struct sensor_combined_s &raw)
 	if (accel_updated) {
 		struct accel_report	accel_report;
 
-		orb_copy(ORB_ID(sensor_accel2), _accel_sub, &accel_report);
+		orb_copy(ORB_ID(sensor_accel), _accel2_sub, &accel_report);
 
 		math::Vector<3> vect(accel_report.x, accel_report.y, accel_report.z);
 		vect = _board_rotation * vect;
@@ -1109,7 +1181,7 @@ Sensors::gyro_poll(struct sensor_combined_s &raw)
 	if (gyro_updated) {
 		struct gyro_report	gyro_report;
 
-		orb_copy(ORB_ID(sensor_gyro0), _gyro_sub, &gyro_report);
+		orb_copy(ORB_ID(sensor_gyro), _gyro_sub, &gyro_report);
 
 		math::Vector<3> vect(gyro_report.x, gyro_report.y, gyro_report.z);
 		vect = _board_rotation * vect;
@@ -1130,7 +1202,7 @@ Sensors::gyro_poll(struct sensor_combined_s &raw)
 	if (gyro_updated) {
 		struct gyro_report	gyro_report;
 
-		orb_copy(ORB_ID(sensor_gyro1), _gyro1_sub, &gyro_report);
+		orb_copy(ORB_ID(sensor_gyro), _gyro1_sub, &gyro_report);
 
 		math::Vector<3> vect(gyro_report.x, gyro_report.y, gyro_report.z);
 		vect = _board_rotation * vect;
@@ -1151,7 +1223,7 @@ Sensors::gyro_poll(struct sensor_combined_s &raw)
 	if (gyro_updated) {
 		struct gyro_report	gyro_report;
 
-		orb_copy(ORB_ID(sensor_gyro2), _gyro_sub, &gyro_report);
+		orb_copy(ORB_ID(sensor_gyro), _gyro2_sub, &gyro_report);
 
 		math::Vector<3> vect(gyro_report.x, gyro_report.y, gyro_report.z);
 		vect = _board_rotation * vect;
@@ -1177,7 +1249,7 @@ Sensors::mag_poll(struct sensor_combined_s &raw)
 	if (mag_updated) {
 		struct mag_report	mag_report;
 
-		orb_copy(ORB_ID(sensor_mag0), _mag_sub, &mag_report);
+		orb_copy(ORB_ID(sensor_mag), _mag_sub, &mag_report);
 
 		math::Vector<3> vect(mag_report.x, mag_report.y, mag_report.z);
 
@@ -1200,6 +1272,34 @@ Sensors::mag_poll(struct sensor_combined_s &raw)
 
 		raw.magnetometer_timestamp = mag_report.timestamp;
 	}
+
+	orb_check(_mag1_sub, &mag_updated);
+
+	if (mag_updated) {
+		struct mag_report	mag_report;
+
+		orb_copy(ORB_ID(sensor_mag), _mag1_sub, &mag_report);
+
+		raw.magnetometer1_raw[0] = mag_report.x_raw;
+		raw.magnetometer1_raw[1] = mag_report.y_raw;
+		raw.magnetometer1_raw[2] = mag_report.z_raw;
+
+		raw.magnetometer1_timestamp = mag_report.timestamp;
+	}
+
+	orb_check(_mag2_sub, &mag_updated);
+
+	if (mag_updated) {
+		struct mag_report	mag_report;
+
+		orb_copy(ORB_ID(sensor_mag), _mag2_sub, &mag_report);
+
+		raw.magnetometer2_raw[0] = mag_report.x_raw;
+		raw.magnetometer2_raw[1] = mag_report.y_raw;
+		raw.magnetometer2_raw[2] = mag_report.z_raw;
+
+		raw.magnetometer2_timestamp = mag_report.timestamp;
+	}
 }
 
 void
@@ -1210,7 +1310,7 @@ Sensors::baro_poll(struct sensor_combined_s &raw)
 
 	if (baro_updated) {
 
-		orb_copy(ORB_ID(sensor_baro0), _baro_sub, &_barometer);
+		orb_copy(ORB_ID(sensor_baro), _baro_sub, &_barometer);
 
 		raw.baro_pres_mbar = _barometer.pressure; // Pressure in mbar
 		raw.baro_alt_meter = _barometer.altitude; // Altitude in meters
@@ -1218,6 +1318,21 @@ Sensors::baro_poll(struct sensor_combined_s &raw)
 
 		raw.baro_timestamp = _barometer.timestamp;
 	}
+
+	orb_check(_baro1_sub, &baro_updated);
+
+	if (baro_updated) {
+
+		struct baro_report	baro_report;
+
+		orb_copy(ORB_ID(sensor_baro), _baro1_sub, &baro_report);
+
+		raw.baro1_pres_mbar = baro_report.pressure; // Pressure in mbar
+		raw.baro1_alt_meter = baro_report.altitude; // Altitude in meters
+		raw.baro1_temp_celcius = baro_report.temperature; // Temperature in degrees celcius
+
+		raw.baro1_timestamp = baro_report.timestamp;
+	}
 }
 
 void
@@ -1350,7 +1465,7 @@ Sensors::parameter_update_poll(bool forced)
 
 		/* this sensor is optional, abort without error */
 
-		if (fd > 0) {
+		if (fd >= 0) {
 			struct airspeed_scale airscale = {
 				_parameters.diff_pres_offset_pa,
 				1.0f,
@@ -1374,6 +1489,45 @@ Sensors::parameter_update_poll(bool forced)
 }
 
 void
+Sensors::rc_parameter_map_poll(bool forced)
+{
+	bool map_updated;
+	orb_check(_rc_parameter_map_sub, &map_updated);
+
+	if (map_updated) {
+		orb_copy(ORB_ID(rc_parameter_map), _rc_parameter_map_sub, &_rc_parameter_map);
+		/* update paramter handles to which the RC channels are mapped */
+		for (int i = 0; i < RC_PARAM_MAP_NCHAN; i++) {
+			if (_rc.function[PARAM_1 + i] < 0 || !_rc_parameter_map.valid[i]) {
+				/* This RC channel is not mapped to a RC-Parameter Channel (e.g. RC_MAP_PARAM1 == 0)
+				 * or no request to map this channel to a param has been sent via mavlink
+				 */
+				continue;
+			}
+
+			/* Set the handle by index if the index is set, otherwise use the id */
+			if (_rc_parameter_map.param_index[i] >= 0) {
+				_parameter_handles.rc_param[i] = param_for_index((unsigned)_rc_parameter_map.param_index[i]);
+			} else {
+				_parameter_handles.rc_param[i] = param_find(_rc_parameter_map.param_id[i]);
+			}
+
+		}
+		warnx("rc to parameter map updated");
+		for (int i = 0; i < RC_PARAM_MAP_NCHAN; i++) {
+			warnx("\ti %d param_id %s scale %.3f value0 %.3f, min %.3f, max %.3f",
+					i,
+					_rc_parameter_map.param_id[i],
+					(double)_rc_parameter_map.scale[i],
+					(double)_rc_parameter_map.value0[i],
+					(double)_rc_parameter_map.value_min[i],
+					(double)_rc_parameter_map.value_max[i]
+					);
+		}
+	}
+}
+
+void
 Sensors::adc_poll(struct sensor_combined_s &raw)
 {
 	/* only read if publishing */
@@ -1552,6 +1706,31 @@ Sensors::get_rc_sw2pos_position(enum RC_CHANNELS_FUNCTION func, float on_th, boo
 }
 
 void
+Sensors::set_params_from_rc()
+{
+	static float param_rc_values[RC_PARAM_MAP_NCHAN] = {};
+	for (int i = 0; i < RC_PARAM_MAP_NCHAN; i++) {
+		if (_rc.function[PARAM_1 + i] < 0 || !_rc_parameter_map.valid[i]) {
+			/* This RC channel is not mapped to a RC-Parameter Channel (e.g. RC_MAP_PARAM1 == 0)
+			 * or no request to map this channel to a param has been sent via mavlink
+			 */
+			continue;
+		}
+
+		float rc_val = get_rc_value((enum RC_CHANNELS_FUNCTION)(PARAM_1 + i), -1.0, 1.0);
+		/* Check if the value has changed,
+		 * maybe we need to introduce a more aggressive limit here */
+		if (rc_val > param_rc_values[i] + FLT_EPSILON || rc_val < param_rc_values[i] - FLT_EPSILON) {
+			param_rc_values[i] = rc_val;
+			float param_val = math::constrain(
+					_rc_parameter_map.value0[i] + _rc_parameter_map.scale[i] * rc_val,
+					_rc_parameter_map.value_min[i], _rc_parameter_map.value_max[i]);
+			param_set(_parameter_handles.rc_param[i], &param_val);
+		}
+	}
+}
+
+void
 Sensors::rc_poll()
 {
 	bool rc_updated;
@@ -1717,6 +1896,13 @@ Sensors::rc_poll()
 			} else {
 				_actuator_group_3_pub = orb_advertise(ORB_ID(actuator_controls_3), &actuator_group_3);
 			}
+
+			/* Update parameters from RC Channels (tuning with RC) if activated */
+			static hrt_abstime last_rc_to_param_map_time = 0;
+			if (hrt_elapsed_time(&last_rc_to_param_map_time) > 1e6) {
+				set_params_from_rc();
+				last_rc_to_param_map_time = hrt_absolute_time();
+			}
 		}
 	}
 }
@@ -1732,29 +1918,47 @@ Sensors::task_main()
 {
 
 	/* start individual sensors */
-	accel_init();
-	gyro_init();
-	mag_init();
-	baro_init();
-	adc_init();
+	int ret;
+	ret = accel_init();
+	if (ret) {
+		goto exit_immediate;
+	}
+	ret = gyro_init();
+	if (ret) {
+		goto exit_immediate;
+	}
+	ret = mag_init();
+	if (ret) {
+		goto exit_immediate;
+	}
+	ret = baro_init();
+	if (ret) {
+		goto exit_immediate;
+	}
+	ret = adc_init();
+	if (ret) {
+		goto exit_immediate;
+	}
 
 	/*
 	 * do subscriptions
 	 */
-	_gyro_sub = orb_subscribe(ORB_ID(sensor_gyro0));
-	_accel_sub = orb_subscribe(ORB_ID(sensor_accel0));
-	_mag_sub = orb_subscribe(ORB_ID(sensor_mag0));
-	_gyro1_sub = orb_subscribe(ORB_ID(sensor_gyro1));
-	_accel1_sub = orb_subscribe(ORB_ID(sensor_accel1));
-	_mag1_sub = orb_subscribe(ORB_ID(sensor_mag1));
-	_gyro2_sub = orb_subscribe(ORB_ID(sensor_gyro2));
-	_accel2_sub = orb_subscribe(ORB_ID(sensor_accel2));
-	_mag2_sub = orb_subscribe(ORB_ID(sensor_mag2));
+	_gyro_sub = orb_subscribe_multi(ORB_ID(sensor_gyro), 0);
+	_accel_sub = orb_subscribe_multi(ORB_ID(sensor_accel), 0);
+	_mag_sub = orb_subscribe_multi(ORB_ID(sensor_mag), 0);
+	_gyro1_sub = orb_subscribe_multi(ORB_ID(sensor_gyro), 1);
+	_accel1_sub = orb_subscribe_multi(ORB_ID(sensor_accel), 1);
+	_mag1_sub = orb_subscribe_multi(ORB_ID(sensor_mag), 1);
+	_gyro2_sub = orb_subscribe_multi(ORB_ID(sensor_gyro), 2);
+	_accel2_sub = orb_subscribe_multi(ORB_ID(sensor_accel), 2);
+	_mag2_sub = orb_subscribe_multi(ORB_ID(sensor_mag), 2);
 	_rc_sub = orb_subscribe(ORB_ID(input_rc));
-	_baro_sub = orb_subscribe(ORB_ID(sensor_baro0));
+	_baro_sub = orb_subscribe_multi(ORB_ID(sensor_baro), 0);
+	_baro1_sub = orb_subscribe_multi(ORB_ID(sensor_baro), 1);
 	_diff_pres_sub = orb_subscribe(ORB_ID(differential_pressure));
 	_vcontrol_mode_sub = orb_subscribe(ORB_ID(vehicle_control_mode));
 	_params_sub = orb_subscribe(ORB_ID(parameter_update));
+	_rc_parameter_map_sub = orb_subscribe(ORB_ID(rc_parameter_map));
 	_manual_control_sub = orb_subscribe(ORB_ID(manual_control_setpoint));
 
 	/* rate limit vehicle status updates to 5Hz */
@@ -1788,6 +1992,7 @@ Sensors::task_main()
 	diff_pres_poll(raw);
 
 	parameter_update_poll(true /* forced */);
+	rc_parameter_map_poll(true /* forced */);
 
 	/* advertise the sensor_combined topic and make the initial publication */
 	_sensor_pub = orb_advertise(ORB_ID(sensor_combined), &raw);
@@ -1799,6 +2004,8 @@ Sensors::task_main()
 	fds[0].fd = _gyro_sub;
 	fds[0].events = POLLIN;
 
+	_task_should_exit = false;
+
 	while (!_task_should_exit) {
 
 		/* wait for up to 50ms for data */
@@ -1820,6 +2027,9 @@ Sensors::task_main()
 		/* check parameters for updates */
 		parameter_update_poll();
 
+		/* check rc parameter map for updates */
+		rc_parameter_map_poll();
+
 		/* the timestamp of the raw struct is updated by the gyro_poll() method */
 
 		/* copy most recent sensor data */
@@ -1846,8 +2056,9 @@ Sensors::task_main()
 
 	warnx("[sensors] exiting.");
 
+exit_immediate:
 	_sensors_task = -1;
-	_exit(0);
+	_exit(ret);
 }
 
 int
@@ -1863,9 +2074,13 @@ Sensors::start()
 				       (main_t)&Sensors::task_main_trampoline,
 				       nullptr);
 
+	/* wait until the task is up and running or has failed */
+	while(_sensors_task > 0 && _task_should_exit) {
+		usleep(100);
+	}
+
 	if (_sensors_task < 0) {
-		warn("task start failed");
-		return -errno;
+		return -ERROR;
 	}
 
 	return OK;
diff --git a/src/modules/systemlib/err.c b/src/modules/systemlib/err.c
index 998b5ac7d..a1a8e7ea8 100644
--- a/src/modules/systemlib/err.c
+++ b/src/modules/systemlib/err.c
@@ -154,6 +154,7 @@ warn(const char *fmt, ...)
 
 	va_start(args, fmt);
 	vwarn(fmt, args);
+	va_end(args);
 }
 
 void
@@ -169,6 +170,7 @@ warnc(int errcode, const char *fmt, ...)
 
 	va_start(args, fmt);
 	vwarnc(errcode, fmt, args);
+	va_end(args);
 }
 
 void
@@ -184,6 +186,7 @@ warnx(const char *fmt, ...)
 
 	va_start(args, fmt);
 	vwarnx(fmt, args);
+	va_end(args);
 }
 
 void
diff --git a/src/modules/systemlib/mcu_version.c b/src/modules/systemlib/mcu_version.c
index 4bcf95784..24f4e4207 100644
--- a/src/modules/systemlib/mcu_version.c
+++ b/src/modules/systemlib/mcu_version.c
@@ -47,7 +47,8 @@
 #ifdef CONFIG_ARCH_CHIP_STM32
 #include <up_arch.h>
 
-#define DBGMCU_IDCODE	0xE0042000
+#define DBGMCU_IDCODE	0xE0042000  //STM DocID018909 Rev 8 Sect 38.18 (MCU device ID code)
+#define UNIQUE_ID    	0x1FFF7A10  //STM DocID018909 Rev 8 Sect 39.1 (Unique device ID Register)
 
 #define STM32F40x_41x	0x413
 #define STM32F42x_43x	0x419
@@ -57,7 +58,13 @@
 
 #endif
 
-
+/** Copy the 96bit MCU Unique ID into the provided pointer */
+void mcu_unique_id(uint32_t *uid_96_bit)
+{
+	uid_96_bit[0] = getreg32(UNIQUE_ID);
+	uid_96_bit[1] = getreg32(UNIQUE_ID+4);
+	uid_96_bit[2] = getreg32(UNIQUE_ID+8);
+}
 
 int mcu_version(char* rev, char** revstr)
 {
diff --git a/src/modules/systemlib/mcu_version.h b/src/modules/systemlib/mcu_version.h
index 1b3d0aba9..c8a0bf5cd 100644
--- a/src/modules/systemlib/mcu_version.h
+++ b/src/modules/systemlib/mcu_version.h
@@ -33,6 +33,8 @@
 
 #pragma once
 
+#include <stdint.h>
+
 /* magic numbers from reference manual */
 enum MCU_REV {
 	MCU_REV_STM32F4_REV_A = 0x1000,
@@ -42,6 +44,15 @@ enum MCU_REV {
 	MCU_REV_STM32F4_REV_3 = 0x2001
 };
 
+
+/**
+ * Reports the microcontroller unique id.
+ *
+ * This ID is guaranteed to be unique for every mcu. 
+ * @param uid_96_bit A uint32_t[3] array to copy the data to.
+ */
+__EXPORT void mcu_unique_id(uint32_t *uid_96_bit);
+
 /**
  * Reports the microcontroller version of the main CPU.
  *
diff --git a/src/modules/systemlib/mixer/mixer.h b/src/modules/systemlib/mixer/mixer.h
index 17989558e..67ef521b4 100644
--- a/src/modules/systemlib/mixer/mixer.h
+++ b/src/modules/systemlib/mixer/mixer.h
@@ -442,6 +442,14 @@ private:
 };
 
 /**
+ * Supported multirotor geometries.
+ *
+ * Values are generated by the multi_tables script and placed to mixer_multirotor.generated.h
+ */
+typedef unsigned int MultirotorGeometryUnderlyingType;
+enum class MultirotorGeometry : MultirotorGeometryUnderlyingType;
+
+/**
  * Multi-rotor mixer for pre-defined vehicle geometries.
  *
  * Collects four inputs (roll, pitch, yaw, thrust) and mixes them to
@@ -451,32 +459,14 @@ class __EXPORT MultirotorMixer : public Mixer
 {
 public:
 	/**
-	 * Supported multirotor geometries.
-	 *
-	 * XXX add more
-	 */
-	enum Geometry {
-		QUAD_X = 0,	/**< quad in X configuration */
-		QUAD_PLUS,	/**< quad in + configuration */
-		QUAD_V,		/**< quad in V configuration */
-		QUAD_WIDE,	/**< quad in wide configuration */
-		HEX_X,		/**< hex in X configuration */
-		HEX_PLUS,	/**< hex in + configuration */
-		HEX_COX,
-		OCTA_X,
-		OCTA_PLUS,
-		OCTA_COX,
-
-		MAX_GEOMETRY
-	};
 
-	/**
 	 * Precalculated rotor mix.
 	 */
 	struct Rotor {
 		float	roll_scale;	/**< scales roll for this rotor */
 		float	pitch_scale;	/**< scales pitch for this rotor */
 		float	yaw_scale;	/**< scales yaw for this rotor */
+		float	out_scale;	/**< scales total out for this rotor */
 	};
 
 	/**
@@ -497,7 +487,7 @@ public:
 	 */
 	MultirotorMixer(ControlCallback control_cb,
 			uintptr_t cb_handle,
-			Geometry geometry,
+			MultirotorGeometry geometry,
 			float roll_scale,
 			float pitch_scale,
 			float yaw_scale,
diff --git a/src/modules/systemlib/mixer/mixer_multirotor.cpp b/src/modules/systemlib/mixer/mixer_multirotor.cpp
index 57e17b67d..2ab5b5e8e 100644
--- a/src/modules/systemlib/mixer/mixer_multirotor.cpp
+++ b/src/modules/systemlib/mixer/mixer_multirotor.cpp
@@ -55,6 +55,9 @@
 
 #include "mixer.h"
 
+// This file is generated by the multi_tables script which is invoked during the build process
+#include "mixer_multirotor.generated.h"
+
 #define debug(fmt, args...)	do { } while(0)
 //#define debug(fmt, args...)	do { printf("[mixer] " fmt "\n", ##args); } while(0)
 //#include <debug.h>
@@ -73,117 +76,11 @@ float constrain(float val, float min, float max)
 	return (val < min) ? min : ((val > max) ? max : val);
 }
 
-/*
- * These tables automatically generated by multi_tables - do not edit.
- */
-const MultirotorMixer::Rotor _config_quad_x[] = {
-	{ -0.707107,  0.707107,  1.00 },
-	{  0.707107, -0.707107,  1.00 },
-	{  0.707107,  0.707107, -1.00 },
-	{ -0.707107, -0.707107, -1.00 },
-};
-const MultirotorMixer::Rotor _config_quad_plus[] = {
-	{ -1.000000,  0.000000,  1.00 },
-	{  1.000000,  0.000000,  1.00 },
-	{  0.000000,  1.000000, -1.00 },
-	{ -0.000000, -1.000000, -1.00 },
-};
-const MultirotorMixer::Rotor _config_quad_v[] = {
-	{ -0.927184,  0.374607,  1.00 },
-	{  0.694658, -0.719340,  1.00 },
-	{  0.927184,  0.374607, -1.00 },
-	{ -0.694658, -0.719340, -1.00 },
-};
-const MultirotorMixer::Rotor _config_quad_wide[] = {
-	{ -0.927184,  0.374607,  1.00 },
-	{  0.777146, -0.629320,  1.00 },
-	{  0.927184,  0.374607, -1.00 },
-	{ -0.777146, -0.629320, -1.00 },
-};
-const MultirotorMixer::Rotor _config_hex_x[] = {
-	{ -1.000000,  0.000000, -1.00 },
-	{  1.000000,  0.000000,  1.00 },
-	{  0.500000,  0.866025, -1.00 },
-	{ -0.500000, -0.866025,  1.00 },
-	{ -0.500000,  0.866025,  1.00 },
-	{  0.500000, -0.866025, -1.00 },
-};
-const MultirotorMixer::Rotor _config_hex_plus[] = {
-	{  0.000000,  1.000000, -1.00 },
-	{ -0.000000, -1.000000,  1.00 },
-	{  0.866025, -0.500000, -1.00 },
-	{ -0.866025,  0.500000,  1.00 },
-	{  0.866025,  0.500000,  1.00 },
-	{ -0.866025, -0.500000, -1.00 },
-};
-const MultirotorMixer::Rotor _config_hex_cox[] = {
-	{ -0.866025,  0.500000, -1.00 },
-	{ -0.866025,  0.500000,  1.00 },
-	{ -0.000000, -1.000000, -1.00 },
-	{ -0.000000, -1.000000,  1.00 },
-	{  0.866025,  0.500000, -1.00 },
-	{  0.866025,  0.500000,  1.00 },
-};
-const MultirotorMixer::Rotor _config_octa_x[] = {
-	{ -0.382683,  0.923880, -1.00 },
-	{  0.382683, -0.923880, -1.00 },
-	{ -0.923880,  0.382683,  1.00 },
-	{ -0.382683, -0.923880,  1.00 },
-	{  0.382683,  0.923880,  1.00 },
-	{  0.923880, -0.382683,  1.00 },
-	{  0.923880,  0.382683, -1.00 },
-	{ -0.923880, -0.382683, -1.00 },
-};
-const MultirotorMixer::Rotor _config_octa_plus[] = {
-	{  0.000000,  1.000000, -1.00 },
-	{ -0.000000, -1.000000, -1.00 },
-	{ -0.707107,  0.707107,  1.00 },
-	{ -0.707107, -0.707107,  1.00 },
-	{  0.707107,  0.707107,  1.00 },
-	{  0.707107, -0.707107,  1.00 },
-	{  1.000000,  0.000000, -1.00 },
-	{ -1.000000,  0.000000, -1.00 },
-};
-const MultirotorMixer::Rotor _config_octa_cox[] = {
-	{ -0.707107,  0.707107,  1.00 },
-	{  0.707107,  0.707107, -1.00 },
-	{  0.707107, -0.707107,  1.00 },
-	{ -0.707107, -0.707107, -1.00 },
-	{  0.707107,  0.707107,  1.00 },
-	{ -0.707107,  0.707107, -1.00 },
-	{ -0.707107, -0.707107,  1.00 },
-	{  0.707107, -0.707107, -1.00 },
-};
-const MultirotorMixer::Rotor *_config_index[MultirotorMixer::MAX_GEOMETRY] = {
-	&_config_quad_x[0],
-	&_config_quad_plus[0],
-	&_config_quad_v[0],
-	&_config_quad_wide[0],
-	&_config_hex_x[0],
-	&_config_hex_plus[0],
-	&_config_hex_cox[0],
-	&_config_octa_x[0],
-	&_config_octa_plus[0],
-	&_config_octa_cox[0],
-};
-const unsigned _config_rotor_count[MultirotorMixer::MAX_GEOMETRY] = {
-	4, /* quad_x */
-	4, /* quad_plus */
-	4, /* quad_v */
-	4, /* quad_wide */
-	6, /* hex_x */
-	6, /* hex_plus */
-	6, /* hex_cox */
-	8, /* octa_x */
-	8, /* octa_plus */
-	8, /* octa_cox */
-};
-
-}
+} // anonymous namespace
 
 MultirotorMixer::MultirotorMixer(ControlCallback control_cb,
 				 uintptr_t cb_handle,
-				 Geometry geometry,
+				 MultirotorGeometry geometry,
 				 float roll_scale,
 				 float pitch_scale,
 				 float yaw_scale,
@@ -193,8 +90,9 @@ MultirotorMixer::MultirotorMixer(ControlCallback control_cb,
 	_pitch_scale(pitch_scale),
 	_yaw_scale(yaw_scale),
 	_idle_speed(-1.0f + idle_speed * 2.0f),	/* shift to output range here to avoid runtime calculation */
-	_rotor_count(_config_rotor_count[geometry]),
-	_rotors(_config_index[geometry])
+	_limits_pub(),
+	_rotor_count(_config_rotor_count[(MultirotorGeometryUnderlyingType)geometry]),
+	_rotors(_config_index[(MultirotorGeometryUnderlyingType)geometry])
 {
 }
 
@@ -205,7 +103,7 @@ MultirotorMixer::~MultirotorMixer()
 MultirotorMixer *
 MultirotorMixer::from_text(Mixer::ControlCallback control_cb, uintptr_t cb_handle, const char *buf, unsigned &buflen)
 {
-	MultirotorMixer::Geometry geometry;
+	MultirotorGeometry geometry;
 	char geomname[8];
 	int s[4];
 	int used;
@@ -245,35 +143,40 @@ MultirotorMixer::from_text(Mixer::ControlCallback control_cb, uintptr_t cb_handl
 	debug("remaining in buf: %d, first char: %c", buflen, buf[0]);
 
 	if (!strcmp(geomname, "4+")) {
-		geometry = MultirotorMixer::QUAD_PLUS;
+		geometry = MultirotorGeometry::QUAD_PLUS;
 
 	} else if (!strcmp(geomname, "4x")) {
-		geometry = MultirotorMixer::QUAD_X;
+		geometry = MultirotorGeometry::QUAD_X;
 
 	} else if (!strcmp(geomname, "4v")) {
-		geometry = MultirotorMixer::QUAD_V;
+		geometry = MultirotorGeometry::QUAD_V;
 
 	} else if (!strcmp(geomname, "4w")) {
-		geometry = MultirotorMixer::QUAD_WIDE;
+		geometry = MultirotorGeometry::QUAD_WIDE;
+
+	} else if (!strcmp(geomname, "4dc")) {
+		geometry = MultirotorGeometry::QUAD_DEADCAT;
 
 	} else if (!strcmp(geomname, "6+")) {
-		geometry = MultirotorMixer::HEX_PLUS;
+		geometry = MultirotorGeometry::HEX_PLUS;
 
 	} else if (!strcmp(geomname, "6x")) {
-		geometry = MultirotorMixer::HEX_X;
+		geometry = MultirotorGeometry::HEX_X;
 
 	} else if (!strcmp(geomname, "6c")) {
-		geometry = MultirotorMixer::HEX_COX;
+		geometry = MultirotorGeometry::HEX_COX;
 
 	} else if (!strcmp(geomname, "8+")) {
-		geometry = MultirotorMixer::OCTA_PLUS;
+		geometry = MultirotorGeometry::OCTA_PLUS;
 
 	} else if (!strcmp(geomname, "8x")) {
-		geometry = MultirotorMixer::OCTA_X;
+		geometry = MultirotorGeometry::OCTA_X;
 		
 	} else if (!strcmp(geomname, "8c")) {
-		geometry = MultirotorMixer::OCTA_COX;
+		geometry = MultirotorGeometry::OCTA_COX;
 
+	} else if (!strcmp(geomname, "2-")) {
+		geometry = MultirotorGeometry::TWIN_ENGINE;
 	} else {
 		debug("unrecognised geometry '%s'", geomname);
 		return nullptr;
@@ -314,6 +217,8 @@ MultirotorMixer::mix(float *outputs, unsigned space)
 			    pitch * _rotors[i].pitch_scale +
 			    thrust;
 
+		out *= _rotors[i].out_scale;
+
 		/* limit yaw if it causes outputs clipping */
 		if (out >= 0.0f && out < -yaw * _rotors[i].yaw_scale) {
 			yaw = -out / _rotors[i].yaw_scale;
diff --git a/src/modules/systemlib/mixer/module.mk b/src/modules/systemlib/mixer/module.mk
index fc7485e20..3fd07f5ba 100644
--- a/src/modules/systemlib/mixer/module.mk
+++ b/src/modules/systemlib/mixer/module.mk
@@ -31,13 +31,17 @@
 #
 ############################################################################
 
+
 #
 # mixer library
 #
 LIBNAME	= mixerlib
-
+	
 SRCS		= mixer.cpp \
 		  mixer_group.cpp \
 		  mixer_multirotor.cpp \
 		  mixer_simple.cpp \
 		  mixer_load.c
+
+SELF_DIR := $(dir $(lastword $(MAKEFILE_LIST)))
+include $(SELF_DIR)multi_tables.mk
diff --git a/src/modules/systemlib/mixer/multi_tables b/src/modules/systemlib/mixer/multi_tables
deleted file mode 100755
index b5698036e..000000000
--- a/src/modules/systemlib/mixer/multi_tables
+++ /dev/null
@@ -1,127 +0,0 @@
-#!/usr/bin/tclsh
-#
-# Generate multirotor mixer scale tables compatible with the ArduCopter layout
-#
-
-proc rad {a} { expr ($a / 360.0) * 2 * acos(-1) }
-proc rcos {a} { expr cos([rad $a])}
-
-set quad_x {
-	  45 CCW
-	-135 CCW
-	 -45 CW
-	 135 CW
-}
-
-set quad_plus {
-	  90 CCW
-	 -90 CCW
-	   0 CW
-	 180 CW
-}
-
-set quad_v {
-	  68 CCW
-	-136 CCW
-	 -68 CW
-	 136 CW
-}
-
-set quad_wide {
-	  68 CCW
-	-129 CCW
-	 -68 CW
-	 129 CW
-}
-
-set hex_x {
-	  90 CW
-	 -90 CCW
-	 -30 CW
-	 150 CCW
-	  30 CCW
-	-150 CW
-}
-
-set hex_plus {
-	   0 CW
-	 180 CCW
-	-120 CW
-	  60 CCW
-	 -60 CCW
-	 120 CW
-}
-
-set hex_cox {
-	  60 CW
-	  60 CCW
-	 180 CW
-	 180 CCW
-	 -60 CW
-	 -60 CCW
-}
-
-set octa_x {
-	  22.5 CW
-	-157.5 CW
-	  67.5 CCW
-	 157.5 CCW
-	 -22.5 CCW
-	-112.5 CCW
-	 -67.5 CW
-	 112.5 CW
-}
-
-set octa_plus {
-	   0 CW
-	 180 CW
-	  45 CCW
-	 135 CCW
-	 -45 CCW
-	-135 CCW
-	 -90 CW
-	  90 CW
-}
-
-set octa_cox {
-	  45 CCW
-	 -45 CW
-	-135 CCW
-	 135 CW
-	 -45 CCW
-	  45 CW
-	 135 CCW
-	-135 CW
-}
-
-set tables {quad_x quad_plus quad_v quad_wide hex_x hex_plus hex_cox octa_x octa_plus octa_cox}
-
-proc factors {a d} { puts [format "\t{ %9.6f, %9.6f, %5.2f }," [rcos [expr $a + 90]] [rcos $a] [expr -$d]]}
-
-foreach table $tables {
-	puts [format "const MultirotorMixer::Rotor _config_%s\[\] = {" $table]
-
-	upvar #0 $table angles
-	foreach {angle dir} $angles {
-		if {$dir == "CW"} {
-			set dd 1.0
-		} else {
-			set dd -1.0
-		}
-		factors $angle $dd
-	}
-	puts "};"
-}
-
-puts "const MultirotorMixer::Rotor *_config_index\[MultirotorMixer::MAX_GEOMETRY\] = {"
-foreach table $tables {
-	puts [format "\t&_config_%s\[0\]," $table]
-}
-puts "};"
-
-puts "const unsigned _config_rotor_count\[MultirotorMixer::MAX_GEOMETRY\] = {"
-foreach table $tables {
-	upvar #0 $table angles
-	puts [format "\t%u, /* %s */" [expr [llength $angles] / 2] $table]
-}
-puts "};"
diff --git a/src/modules/systemlib/mixer/multi_tables.mk b/src/modules/systemlib/mixer/multi_tables.mk
new file mode 100644
index 000000000..c537c83a4
--- /dev/null
+++ b/src/modules/systemlib/mixer/multi_tables.mk
@@ -0,0 +1,42 @@
+############################################################################
+#
+#   Copyright (c) 2014 Anton Matosov <anton.matosov@gmail.com>. All rights reserved.
+#
+# Redistribution and use in source and binary forms, with or without
+# modification, are permitted provided that the following conditions
+# are met:
+#
+# 1. Redistributions of source code must retain the above copyright
+#    notice, this list of conditions and the following disclaimer.
+# 2. Redistributions in binary form must reproduce the above copyright
+#    notice, this list of conditions and the following disclaimer in
+#    the documentation and/or other materials provided with the
+#    distribution.
+# 3. Neither the name PX4 nor the names of its contributors may be
+#    used to endorse or promote products derived from this software
+#    without specific prior written permission.
+#
+# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+# OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+# AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+# POSSIBILITY OF SUCH DAMAGE.
+#
+############################################################################
+
+
+SELF_DIR := $(dir $(lastword $(MAKEFILE_LIST)))
+MULTI_TABLES := $(SELF_DIR)multi_tables.py
+
+# Add explicit dependency, as implicit one doesn't work often.
+$(SELF_DIR)mixer_multirotor.cpp : $(SELF_DIR)mixer_multirotor.generated.h
+
+$(SELF_DIR)mixer_multirotor.generated.h : $(MULTI_TABLES)
+	$(Q) $(PYTHON) $(MULTI_TABLES) > $(SELF_DIR)mixer_multirotor.generated.h
diff --git a/src/modules/systemlib/mixer/multi_tables.py b/src/modules/systemlib/mixer/multi_tables.py
new file mode 100755
index 000000000..ba59e0536
--- /dev/null
+++ b/src/modules/systemlib/mixer/multi_tables.py
@@ -0,0 +1,213 @@
+#!/usr/bin/env python
+############################################################################
+#
+#   Copyright (c) 2013, 2014 PX4 Development Team. All rights reserved.
+#
+# Redistribution and use in source and binary forms, with or without
+# modification, are permitted provided that the following conditions
+# are met:
+#
+# 1. Redistributions of source code must retain the above copyright
+#    notice, this list of conditions and the following disclaimer.
+# 2. Redistributions in binary form must reproduce the above copyright
+#    notice, this list of conditions and the following disclaimer in
+#    the documentation and/or other materials provided with the
+#    distribution.
+# 3. Neither the name PX4 nor the names of its contributors may be
+#    used to endorse or promote products derived from this software
+#    without specific prior written permission.
+#
+# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+# OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+# AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+# POSSIBILITY OF SUCH DAMAGE.
+#
+############################################################################
+
+#
+# Generate multirotor mixer scale tables compatible with the ArduCopter layout
+#
+
+# for python2.7 compatibility
+from __future__ import  print_function
+
+import math
+
+print("/*")
+print("* This file is automatically generated by multi_tables - do not edit.")
+print("*/")
+print("")
+print("#ifndef _MIXER_MULTI_TABLES")
+print("#define _MIXER_MULTI_TABLES")
+print("")
+
+def rcos(angleInRadians):
+  return math.cos(math.radians(angleInRadians))
+
+CCW = 1.0
+CW = -CCW
+
+quad_x = [
+    [  45, CCW],
+    [-135, CCW],
+    [-45,  CW],
+    [135,  CW],
+]
+
+quad_plus = [
+    [  90, CCW],
+    [ -90, CCW],
+    [   0, CW],
+    [ 180, CW],
+]
+
+quad_deadcat = [
+    [  63, CCW, 1.0],
+    [-135, CCW, 0.964],
+    [ -63, CW, 1.0],
+    [ 135, CW, 0.964],
+]
+
+quad_v = [
+    [  18.8, 0.4242],
+    [ -18.8, 1.0],
+    [ -18.8, -0.4242],
+    [  18.8, -1.0],
+]
+
+quad_wide = [
+    [   68, CCW],
+    [ -129, CCW],
+    [  -68, CW],
+    [  129, CW],
+]
+
+hex_x = [
+    [  90, CW],
+    [ -90, CCW],
+    [ -30, CW],
+    [ 150, CCW],
+    [  30, CCW],
+    [-150, CW],
+]
+
+hex_plus = [
+    [   0, CW],
+    [ 180, CCW],
+    [-120, CW],
+    [  60, CCW],
+    [ -60, CCW],
+    [ 120, CW],
+]
+
+hex_cox = [
+    [  60, CW],
+    [  60, CCW],
+    [ 180, CW],
+    [ 180, CCW],
+    [ -60, CW],
+    [ -60, CCW],
+]
+
+octa_x = [
+    [  22.5, CW],
+    [-157.5, CW],
+    [  67.5, CCW],
+    [ 157.5, CCW],
+    [ -22.5, CCW],
+    [-112.5, CCW],
+    [ -67.5, CW],
+    [ 112.5, CW],
+]
+
+octa_plus = [
+    [   0, CW],
+    [ 180, CW],
+    [  45, CCW],
+    [ 135, CCW],
+    [ -45, CCW],
+    [-135, CCW],
+    [ -90, CW],
+    [  90, CW],
+]
+
+octa_cox = [
+    [  45, CCW],
+    [ -45, CW],
+    [-135, CCW],
+    [ 135, CW],
+    [ -45, CCW],
+    [  45, CW],
+    [ 135, CCW],
+    [-135, CW],
+]
+
+twin_engine = [
+    [ 90, 0.0],
+    [-90, 0.0],
+]
+
+
+tables = [quad_x, quad_plus, quad_v, quad_wide, quad_deadcat, hex_x, hex_plus, hex_cox, octa_x, octa_plus, octa_cox, twin_engine]
+
+def variableName(variable):
+    for variableName, value in list(globals().items()):
+        if value is variable:
+            return variableName
+
+def unpackScales(scalesList):
+    if len(scalesList) == 2:
+        scalesList += [1.0] #Add thrust scale
+    return scalesList
+
+def printEnum():
+    print("enum class MultirotorGeometry : MultirotorGeometryUnderlyingType {")
+    for table in tables:
+        print("\t{},".format(variableName(table).upper()))
+
+    print("\n\tMAX_GEOMETRY")
+    print("}; // enum class MultirotorGeometry\n")
+
+def printScaleTables():
+    for table in tables:
+        print("const MultirotorMixer::Rotor _config_{}[] = {{".format(variableName(table)))
+        for row in table:
+            angle, yawScale, thrustScale = unpackScales(row)
+            rollScale = rcos(angle + 90)
+            pitchScale = rcos(angle)
+            print("\t{{ {:9f}, {:9f}, {:9f}, {:9f} }},".format(rollScale, pitchScale, yawScale, thrustScale))
+        print("};\n")
+
+def printScaleTablesIndex():
+    print("const MultirotorMixer::Rotor *_config_index[] = {")
+    for table in tables:
+        print("\t&_config_{}[0],".format(variableName(table)))
+    print("};\n")
+
+
+def printScaleTablesCounts():
+    print("const unsigned _config_rotor_count[] = {")
+    for table in tables:
+        print("\t{}, /* {} */".format(len(table), variableName(table)))
+    print("};\n")
+
+
+
+printEnum()
+
+print("namespace {")
+printScaleTables()
+printScaleTablesIndex()
+printScaleTablesCounts()
+
+print("} // anonymous namespace\n")
+print("#endif /* _MIXER_MULTI_TABLES */")
+print("")
diff --git a/src/modules/systemlib/module.mk b/src/modules/systemlib/module.mk
index fe8b7e75a..f4dff2838 100644
--- a/src/modules/systemlib/module.mk
+++ b/src/modules/systemlib/module.mk
@@ -57,3 +57,5 @@ SRCS		 = err.c \
 		   mcu_version.c
 
 MAXOPTIMIZATION	 = -Os
+
+EXTRACFLAGS	= -Wno-sign-compare
diff --git a/src/modules/systemlib/perf_counter.c b/src/modules/systemlib/perf_counter.c
index d6d8284d2..950577f00 100644
--- a/src/modules/systemlib/perf_counter.c
+++ b/src/modules/systemlib/perf_counter.c
@@ -39,9 +39,10 @@
 
 #include <stdlib.h>
 #include <stdio.h>
+#include <string.h>
 #include <sys/queue.h>
 #include <drivers/drv_hrt.h>
-
+#include <math.h>
 #include "perf_counter.h"
 
 /**
@@ -67,10 +68,13 @@ struct perf_ctr_count {
 struct perf_ctr_elapsed {
 	struct perf_ctr_header	hdr;
 	uint64_t		event_count;
+	uint64_t		event_overruns;
 	uint64_t		time_start;
 	uint64_t		time_total;
 	uint64_t		time_least;
 	uint64_t		time_most;
+	float			mean;
+	float			M2;
 };
 
 /**
@@ -84,7 +88,8 @@ struct perf_ctr_interval {
 	uint64_t		time_last;
 	uint64_t		time_least;
 	uint64_t		time_most;
-
+	float			mean;
+	float			M2;
 };
 
 /**
@@ -109,6 +114,7 @@ perf_alloc(enum perf_counter_type type, const char *name)
 
 	case PC_INTERVAL:
 		ctr = (perf_counter_t)calloc(sizeof(struct perf_ctr_interval), 1);
+
 		break;
 
 	default:
@@ -124,6 +130,28 @@ perf_alloc(enum perf_counter_type type, const char *name)
 	return ctr;
 }
 
+perf_counter_t
+perf_alloc_once(enum perf_counter_type type, const char *name)
+{
+	perf_counter_t handle = (perf_counter_t)sq_peek(&perf_counters);
+
+	while (handle != NULL) {
+		if (!strcmp(handle->name, name)) {
+			if (type == handle->type) {
+				/* they are the same counter */
+				return handle;
+			} else {
+				/* same name but different type, assuming this is an error and not intended */
+				return NULL;
+			}
+		}
+		handle = (perf_counter_t)sq_next(&handle->link);
+	}
+
+	/* if the execution reaches here, no existing counter of that name was found */
+	return perf_alloc(type, name);
+}
+
 void
 perf_free(perf_counter_t handle)
 {
@@ -156,15 +184,23 @@ perf_count(perf_counter_t handle)
 		case 1:
 			pci->time_least = now - pci->time_last;
 			pci->time_most = now - pci->time_last;
+			pci->mean = pci->time_least / 1e6f;
+			pci->M2 = 0;
 			break;
 		default: {
-			hrt_abstime interval = now - pci->time_last;
-			if (interval < pci->time_least)
-				pci->time_least = interval;
-			if (interval > pci->time_most)
-				pci->time_most = interval;
-			break;
-		}
+				hrt_abstime interval = now - pci->time_last;
+				if (interval < pci->time_least)
+					pci->time_least = interval;
+				if (interval > pci->time_most)
+					pci->time_most = interval;
+				// maintain mean and variance of interval in seconds
+				// Knuth/Welford recursive mean and variance of update intervals (via Wikipedia)
+				float dt = interval / 1e6f;
+				float delta_intvl = dt - pci->mean;
+				pci->mean += delta_intvl / pci->event_count;
+				pci->M2 += delta_intvl * (dt - pci->mean);
+				break;
+			}
 		}
 		pci->time_last = now;
 		pci->event_count++;
@@ -203,17 +239,72 @@ perf_end(perf_counter_t handle)
 			struct perf_ctr_elapsed *pce = (struct perf_ctr_elapsed *)handle;
 
 			if (pce->time_start != 0) {
-				hrt_abstime elapsed = hrt_absolute_time() - pce->time_start;
+				int64_t elapsed = hrt_absolute_time() - pce->time_start;
+
+				if (elapsed < 0) {
+					pce->event_overruns++;
+				} else {
+
+					pce->event_count++;
+					pce->time_total += elapsed;
+
+					if ((pce->time_least > (uint64_t)elapsed) || (pce->time_least == 0))
+						pce->time_least = elapsed;
+
+					if (pce->time_most < (uint64_t)elapsed)
+						pce->time_most = elapsed;
+
+					// maintain mean and variance of the elapsed time in seconds
+					// Knuth/Welford recursive mean and variance of update intervals (via Wikipedia)
+					float dt = elapsed / 1e6f;
+					float delta_intvl = dt - pce->mean;
+					pce->mean += delta_intvl / pce->event_count;
+					pce->M2 += delta_intvl * (dt - pce->mean);
+
+					pce->time_start = 0;
+				}
+			}
+		}
+		break;
+
+	default:
+		break;
+	}
+}
+
+#include <systemlib/err.h>
+
+void
+perf_set(perf_counter_t handle, int64_t elapsed)
+{
+	if (handle == NULL) {
+		return;
+	}
+
+	switch (handle->type) {
+	case PC_ELAPSED: {
+			struct perf_ctr_elapsed *pce = (struct perf_ctr_elapsed *)handle;
+
+			if (elapsed < 0) {
+				pce->event_overruns++;
+			} else {
 
 				pce->event_count++;
 				pce->time_total += elapsed;
 
-				if ((pce->time_least > elapsed) || (pce->time_least == 0))
+				if ((pce->time_least > (uint64_t)elapsed) || (pce->time_least == 0))
 					pce->time_least = elapsed;
 
-				if (pce->time_most < elapsed)
+				if (pce->time_most < (uint64_t)elapsed)
 					pce->time_most = elapsed;
 
+				// maintain mean and variance of the elapsed time in seconds
+				// Knuth/Welford recursive mean and variance of update intervals (via Wikipedia)
+				float dt = elapsed / 1e6f;
+				float delta_intvl = dt - pce->mean;
+				pce->mean += delta_intvl / pce->event_count;
+				pce->M2 += delta_intvl * (dt - pce->mean);
+
 				pce->time_start = 0;
 			}
 		}
@@ -300,26 +391,31 @@ perf_print_counter_fd(int fd, perf_counter_t handle)
 
 	case PC_ELAPSED: {
 		struct perf_ctr_elapsed *pce = (struct perf_ctr_elapsed *)handle;
-
-		dprintf(fd, "%s: %llu events, %lluus elapsed, %lluus avg, min %lluus max %lluus\n",
-		       handle->name,
-		       pce->event_count,
-		       pce->time_total,
-		       pce->time_total / pce->event_count,
-		       pce->time_least,
-		       pce->time_most);
+		float rms = sqrtf(pce->M2 / (pce->event_count-1));
+
+		dprintf(fd, "%s: %llu events, %llu overruns, %lluus elapsed, %lluus avg, min %lluus max %lluus %5.3fus rms\n",
+			handle->name,
+			pce->event_count,
+			pce->event_overruns,
+			pce->time_total,
+			pce->time_total / pce->event_count,
+			pce->time_least,
+			pce->time_most,
+			(double)(1e6f * rms));
 		break;
 	}
 
 	case PC_INTERVAL: {
 		struct perf_ctr_interval *pci = (struct perf_ctr_interval *)handle;
-
-		dprintf(fd, "%s: %llu events, %lluus avg, min %lluus max %lluus\n",
-		       handle->name,
-		       pci->event_count,
-		       (pci->time_last - pci->time_first) / pci->event_count,
-		       pci->time_least,
-		       pci->time_most);
+		float rms = sqrtf(pci->M2 / (pci->event_count-1));
+
+		dprintf(fd, "%s: %llu events, %lluus avg, min %lluus max %lluus %5.3fus rms\n",
+			handle->name,
+			pci->event_count,
+			(pci->time_last - pci->time_first) / pci->event_count,
+			pci->time_least,
+			pci->time_most,
+			(double)(1e6f * rms));
 		break;
 	}
 
@@ -365,6 +461,21 @@ perf_print_all(int fd)
 	}
 }
 
+extern const uint16_t latency_bucket_count;
+extern uint32_t latency_counters[];
+extern const uint16_t latency_buckets[];
+
+void
+perf_print_latency(int fd)
+{
+	dprintf(fd, "bucket : events\n");
+	for (int i = 0; i < latency_bucket_count; i++) {
+		printf("  %4i : %i\n", latency_buckets[i], latency_counters[i]);
+	}
+	// print the overflow bucket value
+	dprintf(fd, " >%4i : %i\n", latency_buckets[latency_bucket_count-1], latency_counters[latency_bucket_count]);
+}
+
 void
 perf_reset_all(void)
 {
@@ -374,4 +485,7 @@ perf_reset_all(void)
 		perf_reset(handle);
 		handle = (perf_counter_t)sq_next(&handle->link);
 	}
+	for (int i = 0; i <= latency_bucket_count; i++) {
+		latency_counters[i] = 0;
+	}
 }
diff --git a/src/modules/systemlib/perf_counter.h b/src/modules/systemlib/perf_counter.h
index 668d9dfdf..0c1243de3 100644
--- a/src/modules/systemlib/perf_counter.h
+++ b/src/modules/systemlib/perf_counter.h
@@ -56,7 +56,7 @@ typedef struct perf_ctr_header	*perf_counter_t;
 __BEGIN_DECLS
 
 /**
- * Create a new counter.
+ * Create a new local counter.
  *
  * @param type			The type of the new counter.
  * @param name			The counter name.
@@ -66,6 +66,16 @@ __BEGIN_DECLS
 __EXPORT extern perf_counter_t	perf_alloc(enum perf_counter_type type, const char *name);
 
 /**
+ * Get the reference to an existing counter or create a new one if it does not exist.
+ *
+ * @param type			The type of the counter.
+ * @param name			The counter name.
+ * @return			Handle for the counter, or NULL if a counter
+ *				could not be allocated.
+ */
+__EXPORT extern perf_counter_t	perf_alloc_once(enum perf_counter_type type, const char *name);
+
+/**
  * Free a counter.
  *
  * @param handle		The performance counter's handle.
@@ -94,7 +104,7 @@ __EXPORT extern void		perf_begin(perf_counter_t handle);
  * End a performance event.
  *
  * This call applies to counters that operate over ranges of time; PC_ELAPSED etc.
- * If a call is made without a corresopnding perf_begin call, or if perf_cancel
+ * If a call is made without a corresponding perf_begin call, or if perf_cancel
  * has been called subsequently, no change is made to the counter.
  *
  * @param handle		The handle returned from perf_alloc.
@@ -102,6 +112,18 @@ __EXPORT extern void		perf_begin(perf_counter_t handle);
 __EXPORT extern void		perf_end(perf_counter_t handle);
 
 /**
+ * Register a measurement
+ *
+ * This call applies to counters that operate over ranges of time; PC_ELAPSED etc.
+ * If a call is made without a corresponding perf_begin call. It sets the
+ * value provided as argument as a new measurement.
+ *
+ * @param handle		The handle returned from perf_alloc.
+ * @param elapsed		The time elapsed. Negative values lead to incrementing the overrun counter.
+ */
+__EXPORT extern void		perf_set(perf_counter_t handle, int64_t elapsed);
+
+/**
  * Cancel a performance event.
  *
  * This call applies to counters that operate over ranges of time; PC_ELAPSED etc.
@@ -143,6 +165,13 @@ __EXPORT extern void		perf_print_counter_fd(int fd, perf_counter_t handle);
 __EXPORT extern void		perf_print_all(int fd);
 
 /**
+ * Print hrt latency counters.
+ *
+ * @param fd			File descriptor to print to - e.g. 0 for stdout
+ */
+__EXPORT extern void		perf_print_latency(int fd);
+
+/**
  * Reset all of the performance counters.
  */
 __EXPORT extern void		perf_reset_all(void);
diff --git a/src/modules/systemlib/system_params.c b/src/modules/systemlib/system_params.c
index 702e435ac..a0988035c 100644
--- a/src/modules/systemlib/system_params.c
+++ b/src/modules/systemlib/system_params.c
@@ -82,3 +82,17 @@ PARAM_DEFINE_INT32(SYS_USE_IO, 1);
 * @group System
 */
 PARAM_DEFINE_INT32(SYS_RESTART_TYPE, 2);
+
+/**
+* Companion computer interface
+*
+* Configures the baud rate of the companion computer interface.
+* Set to zero to disable, set to 921600 to enable.
+* CURRENTLY ONLY SUPPORTS 921600 BAUD! Use extras.txt for
+* other baud rates.
+*
+* @min 0
+* @max 921600
+* @group System
+*/
+PARAM_DEFINE_INT32(SYS_COMPANION, 0);
diff --git a/src/modules/systemlib/systemlib.c b/src/modules/systemlib/systemlib.c
index 90d8dd77c..82183b0d7 100644
--- a/src/modules/systemlib/systemlib.c
+++ b/src/modules/systemlib/systemlib.c
@@ -87,7 +87,7 @@ static void kill_task(FAR struct tcb_s *tcb, FAR void *arg)
 	kill(tcb->pid, SIGUSR1);
 }
 
-int task_spawn_cmd(const char *name, int scheduler, int priority, int stack_size, main_t entry, const char *argv[])
+int task_spawn_cmd(const char *name, int scheduler, int priority, int stack_size, main_t entry, char * const argv[])
 {
 	int pid;
 
diff --git a/src/modules/systemlib/systemlib.h b/src/modules/systemlib/systemlib.h
index 3728f2067..2f24215a9 100644
--- a/src/modules/systemlib/systemlib.h
+++ b/src/modules/systemlib/systemlib.h
@@ -41,6 +41,7 @@
 #define SYSTEMLIB_H_
 #include <float.h>
 #include <stdint.h>
+#include <sched.h>
 
 __BEGIN_DECLS
 
@@ -63,7 +64,7 @@ __EXPORT int task_spawn_cmd(const char *name,
 			int scheduler,
 			int stack_size,
 			main_t entry,
-			const char *argv[]);
+			char * const argv[]);
 
 enum MULT_PORTS {
 	MULT_0_US2_RXTX = 0,
diff --git a/src/modules/uORB/Publication.cpp b/src/modules/uORB/Publication.cpp
index cd0b30dd6..d33f93060 100644
--- a/src/modules/uORB/Publication.cpp
+++ b/src/modules/uORB/Publication.cpp
@@ -1,6 +1,6 @@
 /****************************************************************************
  *
- *   Copyright (C) 2012 PX4 Development Team. All rights reserved.
+ *   Copyright (c) 2012-2015 PX4 Development Team. All rights reserved.
  *
  * Redistribution and use in source and binary forms, with or without
  * modification, are permitted provided that the following conditions
@@ -46,6 +46,7 @@
 #include "topics/vehicle_attitude_setpoint.h"
 #include "topics/vehicle_rates_setpoint.h"
 #include "topics/actuator_outputs.h"
+#include "topics/actuator_direct.h"
 #include "topics/encoders.h"
 #include "topics/tecs_status.h"
 
@@ -76,6 +77,7 @@ template class __EXPORT Publication<vehicle_global_velocity_setpoint_s>;
 template class __EXPORT Publication<vehicle_attitude_setpoint_s>;
 template class __EXPORT Publication<vehicle_rates_setpoint_s>;
 template class __EXPORT Publication<actuator_outputs_s>;
+template class __EXPORT Publication<actuator_direct_s>;
 template class __EXPORT Publication<encoders_s>;
 template class __EXPORT Publication<tecs_status_s>;
 
diff --git a/src/modules/uORB/Publication.hpp b/src/modules/uORB/Publication.hpp
index 8650b3df8..b64559734 100644
--- a/src/modules/uORB/Publication.hpp
+++ b/src/modules/uORB/Publication.hpp
@@ -1,6 +1,6 @@
 /****************************************************************************
  *
- *   Copyright (C) 2012 PX4 Development Team. All rights reserved.
+ *   Copyright (c) 2012-2015 PX4 Development Team. All rights reserved.
  *
  * Redistribution and use in source and binary forms, with or without
  * modification, are permitted provided that the following conditions
diff --git a/src/modules/uORB/Subscription.cpp b/src/modules/uORB/Subscription.cpp
index 44b6debc7..8884e5a3a 100644
--- a/src/modules/uORB/Subscription.cpp
+++ b/src/modules/uORB/Subscription.cpp
@@ -1,6 +1,6 @@
 /****************************************************************************
  *
- *   Copyright (C) 2012 PX4 Development Team. All rights reserved.
+ *   Copyright (c) 2012-2015 PX4 Development Team. All rights reserved.
  *
  * Redistribution and use in source and binary forms, with or without
  * modification, are permitted provided that the following conditions
diff --git a/src/modules/uORB/Subscription.hpp b/src/modules/uORB/Subscription.hpp
index 34e9a83e0..75cf36254 100644
--- a/src/modules/uORB/Subscription.hpp
+++ b/src/modules/uORB/Subscription.hpp
@@ -1,6 +1,6 @@
 /****************************************************************************
  *
- *   Copyright (C) 2012 PX4 Development Team. All rights reserved.
+ *   Copyright (c) 2012-2015 PX4 Development Team. All rights reserved.
  *
  * Redistribution and use in source and binary forms, with or without
  * modification, are permitted provided that the following conditions
diff --git a/src/modules/uORB/module.mk b/src/modules/uORB/module.mk
index 9385ce253..71ad09130 100644
--- a/src/modules/uORB/module.mk
+++ b/src/modules/uORB/module.mk
@@ -1,6 +1,6 @@
 ############################################################################
 #
-#   Copyright (c) 2012, 2013 PX4 Development Team. All rights reserved.
+#   Copyright (c) 2012-2015 PX4 Development Team. All rights reserved.
 #
 # Redistribution and use in source and binary forms, with or without
 # modification, are permitted provided that the following conditions
@@ -37,8 +37,7 @@
 
 MODULE_COMMAND		= uorb
 
-# XXX probably excessive, 2048 should be sufficient
-MODULE_STACKSIZE	= 4096
+MODULE_STACKSIZE	= 2048
 
 SRCS			= uORB.cpp \
 			  objects_common.cpp \
diff --git a/src/modules/uORB/objects_common.cpp b/src/modules/uORB/objects_common.cpp
index b91a00c1e..ba1ac0350 100644
--- a/src/modules/uORB/objects_common.cpp
+++ b/src/modules/uORB/objects_common.cpp
@@ -1,6 +1,6 @@
 /****************************************************************************
  *
- *   Copyright (C) 2012, 2013 PX4 Development Team. All rights reserved.
+ *   Copyright (c) 2012-2015 PX4 Development Team. All rights reserved.
  *
  * Redistribution and use in source and binary forms, with or without
  * modification, are permitted provided that the following conditions
@@ -46,23 +46,16 @@
 #include <drivers/drv_orb_dev.h>
 
 #include <drivers/drv_mag.h>
-ORB_DEFINE(sensor_mag0, struct mag_report);
-ORB_DEFINE(sensor_mag1, struct mag_report);
-ORB_DEFINE(sensor_mag2, struct mag_report);
+ORB_DEFINE(sensor_mag, struct mag_report);
 
 #include <drivers/drv_accel.h>
-ORB_DEFINE(sensor_accel0, struct accel_report);
-ORB_DEFINE(sensor_accel1, struct accel_report);
-ORB_DEFINE(sensor_accel2, struct accel_report);
+ORB_DEFINE(sensor_accel, struct accel_report);
 
 #include <drivers/drv_gyro.h>
-ORB_DEFINE(sensor_gyro0, struct gyro_report);
-ORB_DEFINE(sensor_gyro1, struct gyro_report);
-ORB_DEFINE(sensor_gyro2, struct gyro_report);
+ORB_DEFINE(sensor_gyro, struct gyro_report);
 
 #include <drivers/drv_baro.h>
-ORB_DEFINE(sensor_baro0, struct baro_report);
-ORB_DEFINE(sensor_baro1, struct baro_report);
+ORB_DEFINE(sensor_baro, struct baro_report);
 
 #include <drivers/drv_range_finder.h>
 ORB_DEFINE(sensor_range_finder, struct range_finder_report);
@@ -82,6 +75,9 @@ ORB_DEFINE(sensor_combined, struct sensor_combined_s);
 #include "topics/vehicle_gps_position.h"
 ORB_DEFINE(vehicle_gps_position, struct vehicle_gps_position_s);
 
+#include "topics/vehicle_land_detected.h"
+ORB_DEFINE(vehicle_land_detected, struct vehicle_land_detected_s);
+
 #include "topics/satellite_info.h"
 ORB_DEFINE(satellite_info, struct satellite_info_s);
 
@@ -91,6 +87,9 @@ ORB_DEFINE(home_position, struct home_position_s);
 #include "topics/vehicle_status.h"
 ORB_DEFINE(vehicle_status, struct vehicle_status_s);
 
+#include "topics/vtol_vehicle_status.h"
+ORB_DEFINE(vtol_vehicle_status, struct vtol_vehicle_status_s);
+
 #include "topics/safety.h"
 ORB_DEFINE(safety, struct safety_s);
 
@@ -114,6 +113,8 @@ ORB_DEFINE(vehicle_vicon_position, struct vehicle_vicon_position_s);
 
 #include "topics/vehicle_rates_setpoint.h"
 ORB_DEFINE(vehicle_rates_setpoint, struct vehicle_rates_setpoint_s);
+ORB_DEFINE(mc_virtual_rates_setpoint, struct vehicle_rates_setpoint_s);
+ORB_DEFINE(fw_virtual_rates_setpoint, struct vehicle_rates_setpoint_s);
 
 #include "topics/rc_channels.h"
 ORB_DEFINE(rc_channels, struct rc_channels_s);
@@ -143,11 +144,16 @@ ORB_DEFINE(onboard_mission, struct mission_s);
 #include "topics/mission_result.h"
 ORB_DEFINE(mission_result, struct mission_result_s);
 
+#include "topics/geofence_result.h"
+ORB_DEFINE(geofence_result, struct geofence_result_s);
+
 #include "topics/fence.h"
 ORB_DEFINE(fence, unsigned);
 
 #include "topics/vehicle_attitude_setpoint.h"
 ORB_DEFINE(vehicle_attitude_setpoint, struct vehicle_attitude_setpoint_s);
+ORB_DEFINE(mc_virtual_attitude_setpoint, struct vehicle_attitude_setpoint_s);
+ORB_DEFINE(fw_virtual_attitude_setpoint, struct vehicle_attitude_setpoint_s);
 
 #include "topics/manual_control_setpoint.h"
 ORB_DEFINE(manual_control_setpoint, struct manual_control_setpoint_s);
@@ -182,15 +188,18 @@ ORB_DEFINE(actuator_controls_0, struct actuator_controls_s);
 ORB_DEFINE(actuator_controls_1, struct actuator_controls_s);
 ORB_DEFINE(actuator_controls_2, struct actuator_controls_s);
 ORB_DEFINE(actuator_controls_3, struct actuator_controls_s);
+//Virtual control groups, used for VTOL operation
+ORB_DEFINE(actuator_controls_virtual_mc, struct actuator_controls_s);
+ORB_DEFINE(actuator_controls_virtual_fw, struct actuator_controls_s);
 
 #include "topics/actuator_armed.h"
 ORB_DEFINE(actuator_armed, struct actuator_armed_s);
 
 #include "topics/actuator_outputs.h"
-ORB_DEFINE(actuator_outputs_0, struct actuator_outputs_s);
-ORB_DEFINE(actuator_outputs_1, struct actuator_outputs_s);
-ORB_DEFINE(actuator_outputs_2, struct actuator_outputs_s);
-ORB_DEFINE(actuator_outputs_3, struct actuator_outputs_s);
+ORB_DEFINE(actuator_outputs, struct actuator_outputs_s);
+
+#include "topics/actuator_direct.h"
+ORB_DEFINE(actuator_direct, struct actuator_direct_s);
 
 #include "topics/multirotor_motor_limits.h"
 ORB_DEFINE(multirotor_motor_limits, struct multirotor_motor_limits_s);
@@ -230,3 +239,6 @@ ORB_DEFINE(tecs_status, struct tecs_status_s);
 
 #include "topics/wind_estimate.h"
 ORB_DEFINE(wind_estimate, struct wind_estimate_s);
+
+#include "topics/rc_parameter_map.h"
+ORB_DEFINE(rc_parameter_map, struct rc_parameter_map_s);
diff --git a/src/modules/uORB/topics/actuator_controls.h b/src/modules/uORB/topics/actuator_controls.h
index e768ab2f6..668f8f164 100644
--- a/src/modules/uORB/topics/actuator_controls.h
+++ b/src/modules/uORB/topics/actuator_controls.h
@@ -62,6 +62,7 @@
 
 struct actuator_controls_s {
 	uint64_t timestamp;
+	uint64_t timestamp_sample;			/**< the timestamp the data this control response is based on was sampled */
 	float	control[NUM_ACTUATOR_CONTROLS];
 };
 
@@ -74,5 +75,8 @@ ORB_DECLARE(actuator_controls_0);
 ORB_DECLARE(actuator_controls_1);
 ORB_DECLARE(actuator_controls_2);
 ORB_DECLARE(actuator_controls_3);
+ORB_DECLARE(actuator_controls_virtual_mc);
+ORB_DECLARE(actuator_controls_virtual_fw);
+
 
 #endif
diff --git a/src/modules/uORB/topics/actuator_direct.h b/src/modules/uORB/topics/actuator_direct.h
new file mode 100644
index 000000000..5f9d0f56d
--- /dev/null
+++ b/src/modules/uORB/topics/actuator_direct.h
@@ -0,0 +1,69 @@
+/****************************************************************************
+ *
+ *   Copyright (C) 2012 PX4 Development Team. All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in
+ *    the documentation and/or other materials provided with the
+ *    distribution.
+ * 3. Neither the name PX4 nor the names of its contributors may be
+ *    used to endorse or promote products derived from this software
+ *    without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ ****************************************************************************/
+
+/**
+ * @file actuator_direct.h
+ *
+ * Actuator direct values.
+ *
+ * Values published to this topic are the direct actuator values which
+ * should be passed to actuators, bypassing mixing
+ */
+
+#ifndef TOPIC_ACTUATOR_DIRECT_H
+#define TOPIC_ACTUATOR_DIRECT_H
+
+#include <stdint.h>
+#include "../uORB.h"
+
+#define NUM_ACTUATORS_DIRECT		16
+
+/**
+ * @addtogroup topics
+ * @{
+ */
+
+struct actuator_direct_s {
+	uint64_t timestamp;				/**< timestamp in us since system boot */
+	float values[NUM_ACTUATORS_DIRECT];		/**< actuator values, from -1 to 1 */
+	unsigned nvalues;				/**< number of valid values */
+};
+
+/**
+ * @}
+ */
+
+/* actuator direct ORB */
+ORB_DECLARE(actuator_direct);
+
+#endif // TOPIC_ACTUATOR_DIRECT_H
diff --git a/src/modules/uORB/topics/actuator_outputs.h b/src/modules/uORB/topics/actuator_outputs.h
index 446140423..c6fbaaed5 100644
--- a/src/modules/uORB/topics/actuator_outputs.h
+++ b/src/modules/uORB/topics/actuator_outputs.h
@@ -68,12 +68,6 @@ struct actuator_outputs_s {
  */
 
 /* actuator output sets; this list can be expanded as more drivers emerge */
-ORB_DECLARE(actuator_outputs_0);
-ORB_DECLARE(actuator_outputs_1);
-ORB_DECLARE(actuator_outputs_2);
-ORB_DECLARE(actuator_outputs_3);
+ORB_DECLARE(actuator_outputs);
 
-/* output sets with pre-defined applications */
-#define ORB_ID_VEHICLE_CONTROLS		ORB_ID(actuator_outputs_0)
-
-#endif
\ No newline at end of file
+#endif
diff --git a/src/modules/uORB/topics/geofence_result.h b/src/modules/uORB/topics/geofence_result.h
new file mode 100644
index 000000000..b07e04499
--- /dev/null
+++ b/src/modules/uORB/topics/geofence_result.h
@@ -0,0 +1,65 @@
+/****************************************************************************
+ *
+ *   Copyright (C) 2014 PX4 Development Team. All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in
+ *    the documentation and/or other materials provided with the
+ *    distribution.
+ * 3. Neither the name PX4 nor the names of its contributors may be
+ *    used to endorse or promote products derived from this software
+ *    without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ ****************************************************************************/
+
+/**
+ * @file geofence_result.h
+ * Status of the plance concerning the geofence
+ *
+ * @author Ban Siesta <bansiesta@gmail.com>
+ */
+
+#ifndef TOPIC_GEOFENCE_RESULT_H
+#define TOPIC_GEOFENCE_RESULT_H
+
+#include <stdint.h>
+#include <stdbool.h>
+#include "../uORB.h"
+
+/**
+ * @addtogroup topics
+ * @{
+ */
+
+struct geofence_result_s
+{
+	bool geofence_violated;		/**< true if the geofence is violated */
+};
+
+/**
+ * @}
+ */
+
+/* register this as object request broker structure */
+ORB_DECLARE(geofence_result);
+
+#endif
diff --git a/src/modules/uORB/topics/home_position.h b/src/modules/uORB/topics/home_position.h
index 70071130d..d747b5f43 100644
--- a/src/modules/uORB/topics/home_position.h
+++ b/src/modules/uORB/topics/home_position.h
@@ -61,7 +61,7 @@ struct home_position_s
 
 	double lat;				/**< Latitude in degrees 			*/
 	double lon;				/**< Longitude in degrees			*/
-	float alt;				/**< Altitude in meters				*/
+	float alt;				/**< Altitude in meters	(AMSL)		*/
 
 	float x;				/**< X coordinate in meters			*/
 	float y;				/**< Y coordinate in meters			*/
diff --git a/src/modules/uORB/topics/mission.h b/src/modules/uORB/topics/mission.h
index e4b72f87c..22a8f3ecb 100644
--- a/src/modules/uORB/topics/mission.h
+++ b/src/modules/uORB/topics/mission.h
@@ -83,7 +83,7 @@ struct mission_item_s {
 	bool altitude_is_relative;	/**< true if altitude is relative from start point	*/
 	double lat;			/**< latitude in degrees				*/
 	double lon;			/**< longitude in degrees				*/
-	float altitude;			/**< altitude in meters					*/
+	float altitude;			/**< altitude in meters	(AMSL)			*/
 	float yaw;			/**< in radians NED -PI..+PI, NAN means don't change yaw		*/
 	float loiter_radius;		/**< loiter radius in meters, 0 for a VTOL to hover     */
 	int8_t loiter_direction;	/**< 1: positive / clockwise, -1, negative.		*/
diff --git a/src/modules/uORB/topics/mission_result.h b/src/modules/uORB/topics/mission_result.h
index c7d25d1f0..2ddc529a3 100644
--- a/src/modules/uORB/topics/mission_result.h
+++ b/src/modules/uORB/topics/mission_result.h
@@ -1,9 +1,6 @@
 /****************************************************************************
  *
- *   Copyright (C) 2012 PX4 Development Team. All rights reserved.
- *   Author: @author Thomas Gubler <thomasgubler@student.ethz.ch>
- *           @author Julian Oes <joes@student.ethz.ch>
- *           @author Lorenz Meier <lm@inf.ethz.ch>
+ *   Copyright (C) 2012-2014 PX4 Development Team. All rights reserved.
  *
  * Redistribution and use in source and binary forms, with or without
  * modification, are permitted provided that the following conditions
@@ -37,6 +34,11 @@
 /**
  * @file mission_result.h
  * Mission results that navigator needs to pass on to commander and mavlink.
+ *
+ * @author Thomas Gubler <thomasgubler@student.ethz.ch>
+ * @author Julian Oes <joes@student.ethz.ch>
+ * @author Lorenz Meier <lm@inf.ethz.ch>
+ * @author Ban Siesta <bansiesta@gmail.com>
  */
 
 #ifndef TOPIC_MISSION_RESULT_H
@@ -58,8 +60,10 @@ struct mission_result_s
 	bool reached;			/**< true if mission has been reached					 */
 	bool finished;			/**< true if mission has been completed					 */
 	bool stay_in_failsafe;		/**< true if the commander should not switch out of the failsafe mode*/
-	bool geofence_violated;		/**< true if the geofence is violated */
 	bool flight_termination;	/**< true if the navigator demands a flight termination from the commander app */
+	bool item_do_jump_changed;	/**< true if the number of do jumps remaining has changed */
+	unsigned item_changed_index;	/**< indicate which item has changed */
+	unsigned item_do_jump_remaining;/**< set to the number of do jumps remaining for that item */
 };
 
 /**
diff --git a/src/modules/uORB/topics/optical_flow.h b/src/modules/uORB/topics/optical_flow.h
index 0196ae86b..d3dc46ee0 100644
--- a/src/modules/uORB/topics/optical_flow.h
+++ b/src/modules/uORB/topics/optical_flow.h
@@ -55,16 +55,22 @@
  */
 struct optical_flow_s {
 
-	uint64_t timestamp;		/**< in microseconds since system start          */
-
-	uint64_t flow_timestamp;		/**< timestamp from flow sensor */
-	int16_t flow_raw_x;		/**< flow in pixels in X direction, not rotation-compensated */
-	int16_t flow_raw_y;		/**< flow in pixels in Y direction, not rotation-compensated */
-	float flow_comp_x_m;		/**< speed over ground in meters, rotation-compensated */
-	float flow_comp_y_m;		/**< speed over ground in meters, rotation-compensated */
-	float ground_distance_m;	/**< Altitude / distance to ground in meters */
-	uint8_t	quality;		/**< Quality of the measurement, 0: bad quality, 255: maximum quality */
+	uint64_t timestamp;		/**< in microseconds since system start  */
 	uint8_t sensor_id;		/**< id of the sensor emitting the flow value */
+	float pixel_flow_x_integral; /**< accumulated optical flow in radians around x axis */
+	float pixel_flow_y_integral; /**< accumulated optical flow in radians around y axis */
+	float gyro_x_rate_integral;	 /**< accumulated gyro value in radians around x axis */
+	float gyro_y_rate_integral;  /**< accumulated gyro value in radians around y axis */
+	float gyro_z_rate_integral;   /**< accumulated gyro value in radians around z axis */
+	float ground_distance_m;	 /**< Altitude / distance to ground in meters */
+	uint32_t integration_timespan; /**<accumulation timespan in microseconds     */
+	uint32_t time_since_last_sonar_update;/**< time since last sonar update in microseconds */
+	uint16_t frame_count_since_last_readout;/**< number of accumulated frames in timespan */
+	int16_t gyro_temperature;/**< 	Temperature * 100 in centi-degrees Celsius */
+	uint8_t	quality;		/**< Average of quality of accumulated frames, 0: bad quality, 255: maximum quality */
+
+
+
 
 };
 
diff --git a/src/modules/uORB/topics/rc_channels.h b/src/modules/uORB/topics/rc_channels.h
index 16916cc4d..2fde5d424 100644
--- a/src/modules/uORB/topics/rc_channels.h
+++ b/src/modules/uORB/topics/rc_channels.h
@@ -65,12 +65,15 @@ enum RC_CHANNELS_FUNCTION {
 	AUX_2,
 	AUX_3,
 	AUX_4,
-	AUX_5
+	AUX_5,
+	PARAM_1,
+	PARAM_2,
+	PARAM_3
 };
 
 // MAXIMUM FUNCTIONS IS != MAXIMUM RC INPUT CHANNELS
 
-#define RC_CHANNELS_FUNCTION_MAX 18
+#define RC_CHANNELS_FUNCTION_MAX 19
 
 /**
  * @addtogroup topics
diff --git a/src/modules/uORB/topics/rc_parameter_map.h b/src/modules/uORB/topics/rc_parameter_map.h
new file mode 100644
index 000000000..6e68dc4b6
--- /dev/null
+++ b/src/modules/uORB/topics/rc_parameter_map.h
@@ -0,0 +1,76 @@
+/****************************************************************************
+ *
+ *   Copyright (C) 2014 PX4 Development Team. All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in
+ *    the documentation and/or other materials provided with the
+ *    distribution.
+ * 3. Neither the name PX4 nor the names of its contributors may be
+ *    used to endorse or promote products derived from this software
+ *    without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT ,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ ****************************************************************************/
+
+/**
+ * @file rc_parameter_map.h
+ * Maps RC channels to parameters
+ *
+ * @author Thomas Gubler <thomasgubler@gmail.com>
+ */
+
+#ifndef TOPIC_RC_PARAMETER_MAP_H
+#define TOPIC_RC_PARAMETER_MAP_H
+
+#include <stdint.h>
+#include "../uORB.h"
+
+#define RC_PARAM_MAP_NCHAN 3 // This limit is also hardcoded in the enum RC_CHANNELS_FUNCTION in rc_channels.h
+#define PARAM_ID_LEN 16 // corresponds to MAVLINK_MSG_PARAM_VALUE_FIELD_PARAM_ID_LEN
+
+/**
+ * @addtogroup topics
+ * @{
+ */
+
+struct rc_parameter_map_s {
+	uint64_t timestamp;			/**< time at which the map was updated */
+
+	bool valid[RC_PARAM_MAP_NCHAN];		/**< true for RC-Param channels which are mapped to a param */
+
+	int param_index[RC_PARAM_MAP_NCHAN];	/**< corresponding param index, this
+						  this field is ignored if set to -1, in this case param_id will
+						  be used*/
+	char param_id[RC_PARAM_MAP_NCHAN][PARAM_ID_LEN + 1];	/**< corresponding param id, null terminated */
+	float scale[RC_PARAM_MAP_NCHAN];	/** scale to map the RC input [-1, 1] to a parameter value */
+	float value0[RC_PARAM_MAP_NCHAN];	/** inital value around which the parameter value is changed */
+	float value_min[RC_PARAM_MAP_NCHAN];	/** minimal parameter value */
+	float value_max[RC_PARAM_MAP_NCHAN];	/** minimal parameter value */
+};
+
+/**
+ * @}
+ */
+
+ORB_DECLARE(rc_parameter_map);
+
+#endif
diff --git a/src/modules/uORB/topics/sensor_combined.h b/src/modules/uORB/topics/sensor_combined.h
index 06dfcdab3..583a39ded 100644
--- a/src/modules/uORB/topics/sensor_combined.h
+++ b/src/modules/uORB/topics/sensor_combined.h
@@ -122,15 +122,25 @@ struct sensor_combined_s {
 	float baro_pres_mbar;			/**< Barometric pressure, already temp. comp.     */
 	float baro_alt_meter;			/**< Altitude, already temp. comp.                */
 	float baro_temp_celcius;		/**< Temperature in degrees celsius               */
-	float adc_voltage_v[10];		/**< ADC voltages of ADC Chan 10/11/12/13 or -1      */
+	uint64_t baro_timestamp;		/**< Barometer timestamp        */
+
+	float baro1_pres_mbar;			/**< Barometric pressure, already temp. comp.     */
+	float baro1_alt_meter;			/**< Altitude, already temp. comp.                */
+	float baro1_temp_celcius;		/**< Temperature in degrees celsius               */
+	uint64_t baro1_timestamp;		/**< Barometer timestamp        */
+
+	float adc_voltage_v[10];		/**< ADC voltages of ADC Chan 10/11/12/13 or -1   */
 	unsigned adc_mapping[10];		/**< Channel indices of each of these values */
 	float mcu_temp_celcius;			/**< Internal temperature measurement of MCU */
-	uint64_t baro_timestamp;		/**< Barometer timestamp        */
 
-	float differential_pressure_pa;				/**< Airspeed sensor differential pressure                  */
-	uint64_t differential_pressure_timestamp;		/**< Last measurement timestamp */
+	float differential_pressure_pa;			/**< Airspeed sensor differential pressure */
+	uint64_t differential_pressure_timestamp;	/**< Last measurement timestamp */
 	float differential_pressure_filtered_pa;	/**< Low pass filtered airspeed sensor differential pressure reading */
 
+	float differential_pressure1_pa;			/**< Airspeed sensor differential pressure */
+	uint64_t differential_pressure1_timestamp;	/**< Last measurement timestamp */
+	float differential_pressure1_filtered_pa;	/**< Low pass filtered airspeed sensor differential pressure reading */
+
 };
 
 /**
diff --git a/src/modules/uORB/topics/vehicle_attitude.h b/src/modules/uORB/topics/vehicle_attitude.h
index 40328af14..019944dc0 100755
--- a/src/modules/uORB/topics/vehicle_attitude.h
+++ b/src/modules/uORB/topics/vehicle_attitude.h
@@ -79,7 +79,6 @@ struct vehicle_attitude_s {
 	float g_comp[3];	/**< Compensated gravity vector					*/
 	bool R_valid;		/**< Rotation matrix valid					*/
 	bool q_valid;		/**< Quaternion valid						*/
-
 };
 
 /**
diff --git a/src/modules/uORB/topics/vehicle_attitude_setpoint.h b/src/modules/uORB/topics/vehicle_attitude_setpoint.h
index 8446e9c6e..1cfc37cc6 100644
--- a/src/modules/uORB/topics/vehicle_attitude_setpoint.h
+++ b/src/modules/uORB/topics/vehicle_attitude_setpoint.h
@@ -83,5 +83,7 @@ struct vehicle_attitude_setpoint_s {
 
 /* register this as object request broker structure */
 ORB_DECLARE(vehicle_attitude_setpoint);
+ORB_DECLARE(mc_virtual_attitude_setpoint);
+ORB_DECLARE(fw_virtual_attitude_setpoint);
 
 #endif /* TOPIC_ARDRONE_CONTROL_H_ */
diff --git a/src/modules/uORB/topics/vehicle_command.h b/src/modules/uORB/topics/vehicle_command.h
index f264accbb..6b4cb483b 100644
--- a/src/modules/uORB/topics/vehicle_command.h
+++ b/src/modules/uORB/topics/vehicle_command.h
@@ -65,7 +65,7 @@ enum VEHICLE_CMD {
 	VEHICLE_CMD_NAV_TAKEOFF = 22, /* Takeoff from ground / hand |Minimum pitch (if airspeed sensor present), desired pitch without sensor| Empty| Empty| Yaw angle (if magnetometer present), ignored without magnetometer| Latitude| Longitude| Altitude|  */
 	VEHICLE_CMD_NAV_ROI = 80, /* Sets the region of interest (ROI) for a sensor set or the vehicle itself. This can then be used by the vehicles control system to control the vehicle attitude and the attitude of various sensors such as cameras. |Region of intereset mode. (see MAV_ROI enum)| MISSION index/ target ID. (see MAV_ROI enum)| ROI index (allows a vehicle to manage multiple ROI's)| Empty| x the location of the fixed ROI (see MAV_FRAME)| y| z|  */
 	VEHICLE_CMD_NAV_PATHPLANNING = 81, /* Control autonomous path planning on the MAV. |0: Disable local obstacle avoidance / local path planning (without resetting map), 1: Enable local path planning, 2: Enable and reset local path planning| 0: Disable full path planning (without resetting map), 1: Enable, 2: Enable and reset map/occupancy grid, 3: Enable and reset planned route, but not occupancy grid| Empty| Yaw angle at goal, in compass degrees, [0..360]| Latitude/X of goal| Longitude/Y of goal| Altitude/Z of goal|  */
-	VEHICLE_CMD_NAV_GUIDED_LIMITS=90, /* set limits for external control |timeout - maximum time (in seconds) that external controller will be allowed to control vehicle. 0 means no timeout| absolute altitude min (in meters, WGS84) - if vehicle moves below this alt, the command will be aborted and the mission will continue.  0 means no lower altitude limit| absolute altitude max (in meters)- if vehicle moves above this alt, the command will be aborted and the mission will continue.  0 means no upper altitude limit| horizontal move limit (in meters, WGS84) - if vehicle moves more than this distance from it's location at the moment the command was executed, the command will be aborted and the mission will continue. 0 means no horizontal altitude limit| Empty| Empty| Empty|  */
+	VEHICLE_CMD_NAV_GUIDED_LIMITS=90, /* set limits for external control |timeout - maximum time (in seconds) that external controller will be allowed to control vehicle. 0 means no timeout| absolute altitude min (in meters, AMSL) - if vehicle moves below this alt, the command will be aborted and the mission will continue.  0 means no lower altitude limit| absolute altitude max (in meters)- if vehicle moves above this alt, the command will be aborted and the mission will continue.  0 means no upper altitude limit| horizontal move limit (in meters, AMSL) - if vehicle moves more than this distance from it's location at the moment the command was executed, the command will be aborted and the mission will continue. 0 means no horizontal altitude limit| Empty| Empty| Empty|  */
 	VEHICLE_CMD_NAV_GUIDED_MASTER=91, /* set id of master controller |System ID| Component ID| Empty| Empty| Empty| Empty| Empty|  */
 	VEHICLE_CMD_NAV_GUIDED_ENABLE=92, /* hand control over to an external controller |On / Off (> 0.5f on)| Empty| Empty| Empty| Empty| Empty| Empty|  */
 	VEHICLE_CMD_NAV_LAST = 95, /* NOP - This command is only used to mark the upper limit of the NAV/ACTION commands in the enumeration |Empty| Empty| Empty| Empty| Empty| Empty| Empty|  */
diff --git a/src/modules/uORB/topics/vehicle_global_position.h b/src/modules/uORB/topics/vehicle_global_position.h
index c3bb3b893..bc7046690 100644
--- a/src/modules/uORB/topics/vehicle_global_position.h
+++ b/src/modules/uORB/topics/vehicle_global_position.h
@@ -62,7 +62,7 @@
  */
 struct vehicle_global_position_s {
 	uint64_t timestamp;		/**< Time of this estimate, in microseconds since system start		*/
-	uint64_t time_gps_usec;		/**< GPS timestamp in microseconds					   */
+	uint64_t time_utc_usec;		/**< GPS UTC timestamp in microseconds					   */
 	double lat;			/**< Latitude in degrees							 	   */
 	double lon;			/**< Longitude in degrees							 	   */
 	float alt;			/**< Altitude AMSL in meters						 	   */
diff --git a/src/modules/uORB/topics/vehicle_gps_position.h b/src/modules/uORB/topics/vehicle_gps_position.h
index 31e616f4f..102914bbb 100644
--- a/src/modules/uORB/topics/vehicle_gps_position.h
+++ b/src/modules/uORB/topics/vehicle_gps_position.h
@@ -79,7 +79,7 @@ struct vehicle_gps_position_s {
 	bool vel_ned_valid;				/**< Flag to indicate if NED speed is valid */
 
 	uint64_t timestamp_time;			/**< Timestamp for time information */
-	uint64_t time_gps_usec;				/**< Timestamp (microseconds in GPS format), this is the timestamp which comes from the gps module   */
+	uint64_t time_utc_usec;				/**< Timestamp (microseconds, UTC), this is the timestamp which comes from the gps module. It might be unavailable right after cold start, indicated by a value of 0 */
 
 	uint8_t satellites_used;			/**< Number of satellites used */
 };
diff --git a/src/modules/uORB/topics/vehicle_land_detected.h b/src/modules/uORB/topics/vehicle_land_detected.h
new file mode 100644
index 000000000..51b3568e8
--- /dev/null
+++ b/src/modules/uORB/topics/vehicle_land_detected.h
@@ -0,0 +1,63 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2013-2015 PX4 Development Team. All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in
+ *    the documentation and/or other materials provided with the
+ *    distribution.
+ * 3. Neither the name PX4 nor the names of its contributors may be
+ *    used to endorse or promote products derived from this software
+ *    without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ ****************************************************************************/
+
+/**
+ * @file vehicle_land_detected.h
+ * Land detected uORB topic
+ *
+ * @author Johan Jansen <jnsn.johan@gmail.com>
+ */
+
+#ifndef __TOPIC_VEHICLE_LANDED_H__
+#define __TOPIC_VEHICLE_LANDED_H__
+
+#include "../uORB.h"
+
+/**
+ * @addtogroup topics
+ * @{
+ */
+
+struct vehicle_land_detected_s {
+	uint64_t timestamp;	   /**< timestamp of the setpoint */
+	bool landed;           /**< true if vehicle is currently landed on the ground*/
+};
+
+/**
+ * @}
+ */
+
+/* register this as object request broker structure */
+ORB_DECLARE(vehicle_land_detected);
+
+#endif //__TOPIC_VEHICLE_LANDED_H__
diff --git a/src/modules/uORB/topics/vehicle_local_position.h b/src/modules/uORB/topics/vehicle_local_position.h
index 5d39c897d..8b46c5a3f 100644
--- a/src/modules/uORB/topics/vehicle_local_position.h
+++ b/src/modules/uORB/topics/vehicle_local_position.h
@@ -77,7 +77,6 @@ struct vehicle_local_position_s {
 	double ref_lat;		/**< Reference point latitude in degrees */
 	double ref_lon;		/**< Reference point longitude in degrees */
 	float ref_alt;			/**< Reference altitude AMSL in meters, MUST be set to current (not at reference point!) ground level */
-	bool landed;			/**< true if vehicle is landed */
 	/* Distance to surface */
 	float dist_bottom;		/**< Distance to bottom surface (ground) */
 	float dist_bottom_rate;		/**< Distance to bottom surface (ground) change rate */
diff --git a/src/modules/uORB/topics/vehicle_rates_setpoint.h b/src/modules/uORB/topics/vehicle_rates_setpoint.h
index 9f8b412a7..47d51f199 100644
--- a/src/modules/uORB/topics/vehicle_rates_setpoint.h
+++ b/src/modules/uORB/topics/vehicle_rates_setpoint.h
@@ -63,5 +63,6 @@ struct vehicle_rates_setpoint_s {
 
 /* register this as object request broker structure */
 ORB_DECLARE(vehicle_rates_setpoint);
-
+ORB_DECLARE(mc_virtual_rates_setpoint);
+ORB_DECLARE(fw_virtual_rates_setpoint);
 #endif
diff --git a/src/modules/uORB/topics/vehicle_status.h b/src/modules/uORB/topics/vehicle_status.h
index 91491c148..b56e81e04 100644
--- a/src/modules/uORB/topics/vehicle_status.h
+++ b/src/modules/uORB/topics/vehicle_status.h
@@ -147,7 +147,10 @@ enum VEHICLE_TYPE {
 	VEHICLE_TYPE_TRICOPTER = 15, /* Octorotor | */
 	VEHICLE_TYPE_FLAPPING_WING = 16, /* Flapping wing | */
 	VEHICLE_TYPE_KITE = 17, /* Kite | */
-	VEHICLE_TYPE_ENUM_END = 18, /*  | */
+	VEHICLE_TYPE_ONBOARD_CONTROLLER=18, /* Onboard companion controller | */
+	VEHICLE_TYPE_VTOL_DUOROTOR = 19, /* Vtol with two engines */
+	VEHICLE_TYPE_VTOL_QUADROTOR = 20, /* Vtol with four engines*/
+	VEHICLE_TYPE_ENUM_END = 21 /*  | */
 };
 
 enum VEHICLE_BATTERY_WARNING {
@@ -182,7 +185,11 @@ struct vehicle_status_s {
 	int32_t	system_id;				/**< system id, inspired by MAVLink's system ID field */
 	int32_t component_id;				/**< subsystem / component id, inspired by MAVLink's component ID field */
 
-	bool is_rotary_wing;
+	bool is_rotary_wing;				/**< True if system is in rotary wing configuration, so for a VTOL
+							  this is only true while flying as a multicopter */
+	bool is_vtol;					/**< True if the system is VTOL capable */
+
+	bool vtol_fw_permanent_stab;	/**< True if vtol should stabilize attitude for fw in manual mode */
 
 	bool condition_battery_voltage_valid;
 	bool condition_system_in_air_restore;	/**< true if we can restore in mid air */
@@ -201,6 +208,7 @@ struct vehicle_status_s {
 
 	bool rc_signal_found_once;
 	bool rc_signal_lost;				/**< true if RC reception lost */
+	uint64_t rc_signal_lost_timestamp;		/**< Time at which the RC reception was lost */
 	bool rc_signal_lost_cmd;				/**< true if RC lost mode is commanded */
 	bool rc_input_blocked;				/**< set if RC input should be ignored */
 
diff --git a/src/modules/uORB/topics/vtol_vehicle_status.h b/src/modules/uORB/topics/vtol_vehicle_status.h
new file mode 100644
index 000000000..968c2b6bd
--- /dev/null
+++ b/src/modules/uORB/topics/vtol_vehicle_status.h
@@ -0,0 +1,68 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2013 PX4 Development Team. All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in
+ *    the documentation and/or other materials provided with the
+ *    distribution.
+ * 3. Neither the name PX4 nor the names of its contributors may be
+ *    used to endorse or promote products derived from this software
+ *    without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ ****************************************************************************/
+
+/**
+ * @file vtol_status.h
+ *
+ * Vtol status topic
+ *
+ */
+
+#ifndef TOPIC_VTOL_STATUS_H
+#define TOPIC_VTOL_STATUS_H
+
+#include <stdint.h>
+#include "../uORB.h"
+
+/**
+ * @addtogroup topics
+ * @{
+ */
+
+/* Indicates in which mode the vtol aircraft is in */
+struct vtol_vehicle_status_s {
+
+	uint64_t	timestamp;	/**< Microseconds since system boot */
+	bool vtol_in_rw_mode;	/*true: vtol vehicle is in rotating wing mode */
+	bool fw_permanent_stab;	/**< In fw mode stabilize attitude even if in manual mode*/
+	float airspeed_tot;		/*< Estimated airspeed over control surfaces */
+};
+
+/**
+ * @}
+ */
+
+/* register this as object request broker structure */
+ORB_DECLARE(vtol_vehicle_status);
+
+#endif
diff --git a/src/modules/uORB/uORB.cpp b/src/modules/uORB/uORB.cpp
index 149b8f6d2..6f021459c 100644
--- a/src/modules/uORB/uORB.cpp
+++ b/src/modules/uORB/uORB.cpp
@@ -1,6 +1,6 @@
 /****************************************************************************
  *
- *   Copyright (C) 2012 PX4 Development Team. All rights reserved.
+ *   Copyright (c) 2012-2015 PX4 Development Team. All rights reserved.
  *
  * Redistribution and use in source and binary forms, with or without
  * modification, are permitted provided that the following conditions
@@ -51,6 +51,7 @@
 #include <stdio.h>
 #include <math.h>
 #include <unistd.h>
+#include <systemlib/err.h>
 
 #include <nuttx/arch.h>
 #include <nuttx/wqueue.h>
@@ -68,6 +69,7 @@
 namespace
 {
 
+/* internal use only */
 static const unsigned orb_maxpath = 64;
 
 /* oddly, ERROR is not defined for c++ */
@@ -81,17 +83,30 @@ enum Flavor {
 	PARAM
 };
 
+struct orb_advertdata {
+	const struct orb_metadata *meta;
+	int *instance;
+	int priority;
+};
+
 int
-node_mkpath(char *buf, Flavor f, const struct orb_metadata *meta)
+node_mkpath(char *buf, Flavor f, const struct orb_metadata *meta, int *instance = nullptr)
 {
 	unsigned len;
 
-	len = snprintf(buf, orb_maxpath, "/%s/%s",
-		       (f == PUBSUB) ? "obj" : "param",
-		       meta->o_name);
+	unsigned index = 0;
+
+	if (instance != nullptr) {
+		index = *instance;
+	}
+
+	len = snprintf(buf, orb_maxpath, "/%s/%s%d",
+			(f == PUBSUB) ? "obj" : "param",
+			meta->o_name, index);
 
-	if (len >= orb_maxpath)
+	if (len >= orb_maxpath) {
 		return -ENAMETOOLONG;
+	}
 
 	return OK;
 }
@@ -104,7 +119,7 @@ node_mkpath(char *buf, Flavor f, const struct orb_metadata *meta)
 class ORBDevNode : public device::CDev
 {
 public:
-	ORBDevNode(const struct orb_metadata *meta, const char *name, const char *path);
+	ORBDevNode(const struct orb_metadata *meta, const char *name, const char *path, int priority);
 	~ORBDevNode();
 
 	virtual int		open(struct file *filp);
@@ -126,6 +141,7 @@ private:
 		struct hrt_call	update_call;	/**< deferred wakeup call if update_period is nonzero */
 		void		*poll_priv;	/**< saved copy of fds->f_priv while poll is active */
 		bool		update_reported; /**< true if we have reported the update via poll/check */
+		int		priority;	/**< priority of publisher */
 	};
 
 	const struct orb_metadata *_meta;	/**< object metadata information */
@@ -133,6 +149,7 @@ private:
 	hrt_abstime		_last_update;	/**< time the object was last updated */
 	volatile unsigned 	_generation;	/**< object generation count */
 	pid_t			_publisher;	/**< if nonzero, current publisher */
+	const int		_priority;	/**< priority of topic */
 
 	SubscriberData		*filp_to_sd(struct file *filp) {
 		SubscriberData *sd = (SubscriberData *)(filp->f_priv);
@@ -160,13 +177,14 @@ private:
 	bool			appears_updated(SubscriberData *sd);
 };
 
-ORBDevNode::ORBDevNode(const struct orb_metadata *meta, const char *name, const char *path) :
+ORBDevNode::ORBDevNode(const struct orb_metadata *meta, const char *name, const char *path, int priority) :
 	CDev(name, path),
 	_meta(meta),
 	_data(nullptr),
 	_last_update(0),
 	_generation(0),
-	_publisher(0)
+	_publisher(0),
+	_priority(priority)
 {
 	// enable debug() calls
 	_debug_enabled = true;
@@ -176,6 +194,7 @@ ORBDevNode::~ORBDevNode()
 {
 	if (_data != nullptr)
 		delete[] _data;
+
 }
 
 int
@@ -225,12 +244,15 @@ ORBDevNode::open(struct file *filp)
 		/* default to no pending update */
 		sd->generation = _generation;
 
+		/* set priority */
+		sd->priority = _priority;
+
 		filp->f_priv = (void *)sd;
 
 		ret = CDev::open(filp);
 
 		if (ret != OK)
-			free(sd);
+			delete sd;
 
 		return ret;
 	}
@@ -283,6 +305,9 @@ ORBDevNode::read(struct file *filp, char *buffer, size_t buflen)
 	/* track the last generation that the file has seen */
 	sd->generation = _generation;
 
+	/* set priority */
+	sd->priority = _priority;
+
 	/*
 	 * Clear the flag that indicates that an update has been reported, as
 	 * we have just collected it.
@@ -364,6 +389,10 @@ ORBDevNode::ioctl(struct file *filp, int cmd, unsigned long arg)
 		*(uintptr_t *)arg = (uintptr_t)this;
 		return OK;
 
+	case ORBIOCGPRIORITY:
+		*(int *)arg = sd->priority;
+		return OK;
+
 	default:
 		/* give it to the superclass */
 		return CDev::ioctl(filp, cmd, arg);
@@ -556,40 +585,85 @@ ORBDevMaster::ioctl(struct file *filp, int cmd, unsigned long arg)
 
 	switch (cmd) {
 	case ORBIOCADVERTISE: {
-			const struct orb_metadata *meta = (const struct orb_metadata *)arg;
+			const struct orb_advertdata *adv = (const struct orb_advertdata *)arg;
+			const struct orb_metadata *meta = adv->meta;
 			const char *objname;
+			const char *devpath;
 			char nodepath[orb_maxpath];
 			ORBDevNode *node;
 
+			/* set instance to zero - we could allow selective multi-pubs later based on value */
+			if (adv->instance != nullptr) {
+				*(adv->instance) = 0;
+			}
+
 			/* construct a path to the node - this also checks the node name */
-			ret = node_mkpath(nodepath, _flavor, meta);
+			ret = node_mkpath(nodepath, _flavor, meta, adv->instance);
 
-			if (ret != OK)
+			if (ret != OK) {
 				return ret;
+			}
+
+			/* ensure that only one advertiser runs through this critical section */
+			lock();
 
-			/* driver wants a permanent copy of the node name, so make one here */
-			objname = strdup(meta->o_name);
+			ret = ERROR;
 
-			if (objname == nullptr)
-				return -ENOMEM;
+			/* try for topic groups */
+			const unsigned max_group_tries = (adv->instance != nullptr) ? ORB_MULTI_MAX_INSTANCES : 1;
+			unsigned group_tries = 0;
+			do {
+				/* if path is modifyable change try index */
+				if (adv->instance != nullptr) {
+					/* replace the number at the end of the string */
+					nodepath[strlen(nodepath) - 1] = '0' + group_tries;
+					*(adv->instance) = group_tries;
+				}
 
-			/* construct the new node */
-			node = new ORBDevNode(meta, objname, nodepath);
+				/* driver wants a permanent copy of the node name, so make one here */
+				objname = strdup(meta->o_name);
 
-			/* initialise the node - this may fail if e.g. a node with this name already exists */
-			if (node != nullptr)
+				if (objname == nullptr) {
+					return -ENOMEM;
+				}
+
+				/* driver wants a permanent copy of the path, so make one here */
+				devpath = strdup(nodepath);
+
+				if (devpath == nullptr) {
+					return -ENOMEM;
+				}
+
+				/* construct the new node */
+				node = new ORBDevNode(meta, objname, devpath, adv->priority);
+
+				/* if we didn't get a device, that's bad */
+				if (node == nullptr) {
+					unlock();
+					return -ENOMEM;
+				}
+
+				/* initialise the node - this may fail if e.g. a node with this name already exists */
 				ret = node->init();
+				
+				/* if init failed, discard the node and its name */
+				if (ret != OK) {
+					delete node;
+					free((void *)objname);
+					free((void *)devpath);
+				}
 
-			/* if we didn't get a device, that's bad */
-			if (node == nullptr)
-				return -ENOMEM;
+				group_tries++;
 
-			/* if init failed, discard the node and its name */
-			if (ret != OK) {
-				delete node;
-				free((void *)objname);
+			} while (ret != OK && (group_tries < max_group_tries));
+
+			if (group_tries > max_group_tries) {
+				ret = -ENOMEM;
 			}
 
+			/* the file handle for the driver has been created, unlock */
+			unlock();
+
 			return ret;
 		}
 
@@ -614,6 +688,7 @@ struct orb_test {
 };
 
 ORB_DEFINE(orb_test, struct orb_test);
+ORB_DEFINE(orb_multitest, struct orb_test);
 
 int
 test_fail(const char *fmt, ...)
@@ -643,8 +718,6 @@ test_note(const char *fmt, ...)
 	return OK;
 }
 
-ORB_DECLARE(sensor_combined);
-
 int
 test()
 {
@@ -700,48 +773,65 @@ test()
 	orb_unsubscribe(sfd);
 	close(pfd);
 
-#if 0
-	/* this is a hacky test that exploits the sensors app to test rate-limiting */
+	/* this routine tests the multi-topic support */
+	test_note("try multi-topic support");
 
-	sfd = orb_subscribe(ORB_ID(sensor_combined));
+	int instance0;
+	int pfd0 = orb_advertise_multi(ORB_ID(orb_multitest), &t, &instance0, ORB_PRIO_MAX);
 
-	hrt_abstime start, end;
-	unsigned count;
+		test_note("advertised");
+	usleep(300000);
 
-	start = hrt_absolute_time();
-	count = 0;
+	int instance1;
+	int pfd1 = orb_advertise_multi(ORB_ID(orb_multitest), &t, &instance1, ORB_PRIO_MIN);
 
-	do {
-		orb_check(sfd, &updated);
+	if (instance0 != 0)
+		return test_fail("mult. id0: %d", instance0);
 
-		if (updated) {
-			orb_copy(ORB_ID(sensor_combined), sfd, nullptr);
-			count++;
-		}
-	} while (count < 100);
+	if (instance1 != 1)
+		return test_fail("mult. id1: %d", instance1);
 
-	end = hrt_absolute_time();
-	test_note("full-speed, 100 updates in %llu", end - start);
+	t.val = 103;
+	if (OK != orb_publish(ORB_ID(orb_multitest), pfd0, &t))
+		return test_fail("mult. pub0 fail");
 
-	orb_set_interval(sfd, 10);
+		test_note("published");
+	usleep(300000);
 
-	start = hrt_absolute_time();
-	count = 0;
+	t.val = 203;
+	if (OK != orb_publish(ORB_ID(orb_multitest), pfd1, &t))
+		return test_fail("mult. pub1 fail");
 
-	do {
-		orb_check(sfd, &updated);
+	/* subscribe to both topics and ensure valid data is received */
+	int sfd0 = orb_subscribe_multi(ORB_ID(orb_multitest), 0);
 
-		if (updated) {
-			orb_copy(ORB_ID(sensor_combined), sfd, nullptr);
-			count++;
-		}
-	} while (count < 100);
+	if (OK != orb_copy(ORB_ID(orb_multitest), sfd0, &t))
+		return test_fail("sub #0 copy failed: %d", errno);
 
-	end = hrt_absolute_time();
-	test_note("100Hz, 100 updates in %llu", end - start);
+	if (t.val != 103)
+		return test_fail("sub #0 val. mismatch: %d", t.val);
 
-	orb_unsubscribe(sfd);
-#endif
+	int sfd1 = orb_subscribe_multi(ORB_ID(orb_multitest), 1);
+
+	if (OK != orb_copy(ORB_ID(orb_multitest), sfd1, &t))
+		return test_fail("sub #1 copy failed: %d", errno);
+
+	if (t.val != 203)
+		return test_fail("sub #1 val. mismatch: %d", t.val);
+
+	/* test priorities */
+	int prio;
+	if (OK != orb_priority(sfd0, &prio))
+		return test_fail("prio #0");
+	
+	if (prio != ORB_PRIO_MAX)
+		return test_fail("prio: %d", prio);
+
+	if (OK != orb_priority(sfd1, &prio))
+		return test_fail("prio #1");
+
+	if (prio != ORB_PRIO_MIN)
+		return test_fail("prio: %d", prio);
 
 	return test_note("PASS");
 }
@@ -771,7 +861,7 @@ uorb_main(int argc, char *argv[])
 	if (!strcmp(argv[1], "start")) {
 
 		if (g_dev != nullptr) {
-			fprintf(stderr, "[uorb] already loaded\n");
+			warnx("already loaded");
 			/* user wanted to start uorb, its already running, no error */
 			return 0;
 		}
@@ -780,18 +870,17 @@ uorb_main(int argc, char *argv[])
 		g_dev = new ORBDevMaster(PUBSUB);
 
 		if (g_dev == nullptr) {
-			fprintf(stderr, "[uorb] driver alloc failed\n");
+			warnx("driver alloc failed");
 			return -ENOMEM;
 		}
 
 		if (OK != g_dev->init()) {
-			fprintf(stderr, "[uorb] driver init failed\n");
+			warnx("driver init failed");
 			delete g_dev;
 			g_dev = nullptr;
 			return -EIO;
 		}
 
-		printf("[uorb] ready\n");
 		return OK;
 	}
 
@@ -807,8 +896,7 @@ uorb_main(int argc, char *argv[])
 	if (!strcmp(argv[1], "status"))
 		return info();
 
-	fprintf(stderr, "unrecognised command, try 'start', 'test' or 'status'\n");
-	return -EINVAL;
+	errx(-EINVAL, "unrecognized command, try 'start', 'test' or 'status'");
 }
 
 /*
@@ -817,18 +905,6 @@ uorb_main(int argc, char *argv[])
 namespace
 {
 
-void debug(const char *fmt, ...)
-{
-	va_list ap;
-
-	va_start(ap, fmt);
-	vfprintf(stderr, fmt, ap);
-	va_end(ap);
-	fprintf(stderr, "\n");
-	fflush(stderr);
-	usleep(100000);
-}
-
 /**
  * Advertise a node; don't consider it an error if the node has
  * already been advertised.
@@ -837,11 +913,14 @@ void debug(const char *fmt, ...)
  *       we tried to advertise.
  */
 int
-node_advertise(const struct orb_metadata *meta)
+node_advertise(const struct orb_metadata *meta, int *instance = nullptr, int priority = ORB_PRIO_DEFAULT)
 {
 	int fd = -1;
 	int ret = ERROR;
 
+	/* fill advertiser data */
+	const struct orb_advertdata adv = { meta, instance, priority };
+
 	/* open the control device */
 	fd = open(TOPIC_MASTER_DEVICE_PATH, 0);
 
@@ -849,11 +928,12 @@ node_advertise(const struct orb_metadata *meta)
 		goto out;
 
 	/* advertise the object */
-	ret = ioctl(fd, ORBIOCADVERTISE, (unsigned long)(uintptr_t)meta);
+	ret = ioctl(fd, ORBIOCADVERTISE, (unsigned long)(uintptr_t)&adv);
 
 	/* it's OK if it already exists */
-	if ((OK != ret) && (EEXIST == errno))
+	if ((OK != ret) && (EEXIST == errno)) {
 		ret = OK;
+	}
 
 out:
 
@@ -870,7 +950,7 @@ out:
  * advertisers.
  */
 int
-node_open(Flavor f, const struct orb_metadata *meta, const void *data, bool advertiser)
+node_open(Flavor f, const struct orb_metadata *meta, const void *data, bool advertiser, int *instance = nullptr, int priority = ORB_PRIO_DEFAULT)
 {
 	char path[orb_maxpath];
 	int fd, ret;
@@ -895,7 +975,7 @@ node_open(Flavor f, const struct orb_metadata *meta, const void *data, bool adve
 	/*
 	 * Generate the path to the node and try to open it.
 	 */
-	ret = node_mkpath(path, f, meta);
+	ret = node_mkpath(path, f, meta, instance);
 
 	if (ret != OK) {
 		errno = -ret;
@@ -905,15 +985,34 @@ node_open(Flavor f, const struct orb_metadata *meta, const void *data, bool adve
 	/* open the path as either the advertiser or the subscriber */
 	fd = open(path, (advertiser) ? O_WRONLY : O_RDONLY);
 
+	/* if we want to advertise and the node existed, we have to re-try again */
+	if ((fd >= 0) && (instance != nullptr) && (advertiser)) {
+		/* close the fd, we want a new one */
+		close(fd);
+		/* the node_advertise call will automatically go for the next free entry */
+		fd = -1;
+	}
+
 	/* we may need to advertise the node... */
 	if (fd < 0) {
 
 		/* try to create the node */
-		ret = node_advertise(meta);
+		ret = node_advertise(meta, instance, priority);
+
+		if (ret == OK) {
+			/* update the path, as it might have been updated during the node_advertise call */
+			ret = node_mkpath(path, f, meta, instance);
+
+			if (ret != OK) {
+				errno = -ret;
+				return ERROR;
+			}
+		}
 
 		/* on success, try the open again */
-		if (ret == OK)
+		if (ret == OK) {
 			fd = open(path, (advertiser) ? O_WRONLY : O_RDONLY);
+		}
 	}
 
 	if (fd < 0) {
@@ -930,11 +1029,17 @@ node_open(Flavor f, const struct orb_metadata *meta, const void *data, bool adve
 orb_advert_t
 orb_advertise(const struct orb_metadata *meta, const void *data)
 {
+	return orb_advertise_multi(meta, data, nullptr, ORB_PRIO_DEFAULT);
+}
+
+orb_advert_t
+orb_advertise_multi(const struct orb_metadata *meta, const void *data, int *instance, int priority)
+{
 	int result, fd;
 	orb_advert_t advertiser;
 
 	/* open the node as an advertiser */
-	fd = node_open(PUBSUB, meta, data, true);
+	fd = node_open(PUBSUB, meta, data, true, instance, priority);
 	if (fd == ERROR)
 		return ERROR;
 
@@ -945,7 +1050,7 @@ orb_advertise(const struct orb_metadata *meta, const void *data)
 		return ERROR;
 
 	/* the advertiser must perform an initial publish to initialise the object */
-	result= orb_publish(meta, advertiser, data);
+	result = orb_publish(meta, advertiser, data);
 	if (result == ERROR)
 		return ERROR;
 
@@ -959,6 +1064,13 @@ orb_subscribe(const struct orb_metadata *meta)
 }
 
 int
+orb_subscribe_multi(const struct orb_metadata *meta, unsigned instance)
+{
+	int inst = instance;
+	return node_open(PUBSUB, meta, nullptr, false, &inst);
+}
+
+int
 orb_unsubscribe(int handle)
 {
 	return close(handle);
@@ -1001,6 +1113,12 @@ orb_stat(int handle, uint64_t *time)
 }
 
 int
+orb_priority(int handle, int *priority)
+{
+	return ioctl(handle, ORBIOCGPRIORITY, (unsigned long)(uintptr_t)priority);
+}
+
+int
 orb_set_interval(int handle, unsigned interval)
 {
 	return ioctl(handle, ORBIOCSETINTERVAL, interval * 1000);
diff --git a/src/modules/uORB/uORB.h b/src/modules/uORB/uORB.h
index 82ff46ad2..9c33c8a3e 100644
--- a/src/modules/uORB/uORB.h
+++ b/src/modules/uORB/uORB.h
@@ -1,6 +1,6 @@
 /****************************************************************************
  *
- *   Copyright (C) 2012 PX4 Development Team. All rights reserved.
+ *   Copyright (c) 2012-2015 PX4 Development Team. All rights reserved.
  *
  * Redistribution and use in source and binary forms, with or without
  * modification, are permitted provided that the following conditions
@@ -57,6 +57,25 @@ struct orb_metadata {
 typedef const struct orb_metadata *orb_id_t;
 
 /**
+ * Maximum number of multi topic instances
+ */
+#define ORB_MULTI_MAX_INSTANCES	3
+
+/**
+ * Topic priority.
+ * Relevant for multi-topics / topic groups
+ */
+enum ORB_PRIO {
+	ORB_PRIO_MIN = 0,
+	ORB_PRIO_VERY_LOW = 25,
+	ORB_PRIO_LOW = 50,
+	ORB_PRIO_DEFAULT = 75,
+	ORB_PRIO_HIGH = 100,
+	ORB_PRIO_VERY_HIGH = 125,
+	ORB_PRIO_MAX = 255
+};
+
+/**
  * Generates a pointer to the uORB metadata structure for
  * a given topic.
  *
@@ -141,6 +160,34 @@ typedef intptr_t	orb_advert_t;
 extern orb_advert_t orb_advertise(const struct orb_metadata *meta, const void *data) __EXPORT;
 
 /**
+ * Advertise as the publisher of a topic.
+ *
+ * This performs the initial advertisement of a topic; it creates the topic
+ * node in /obj if required and publishes the initial data.
+ *
+ * Any number of advertisers may publish to a topic; publications are atomic
+ * but co-ordination between publishers is not provided by the ORB. 
+ *
+ * @param meta		The uORB metadata (usually from the ORB_ID() macro)
+ *			for the topic.
+ * @param data		A pointer to the initial data to be published.
+ *			For topics updated by interrupt handlers, the advertisement
+ *			must be performed from non-interrupt context.
+ * @param instance	Pointer to an integer which will yield the instance ID (0-based)
+ *			of the publication.
+ * @param priority	The priority of the instance. If a subscriber subscribes multiple
+ *			instances, the priority allows the subscriber to prioritize the best
+ *			data source as long as its available.
+ * @return		ERROR on error, otherwise returns a handle
+ *			that can be used to publish to the topic.
+ *			If the topic in question is not known (due to an
+ *			ORB_DEFINE with no corresponding ORB_DECLARE)
+ *			this function will return -1 and set errno to ENOENT.
+ */
+extern orb_advert_t orb_advertise_multi(const struct orb_metadata *meta, const void *data, int *instance, int priority) __EXPORT;
+
+
+/**
  * Publish new data to a topic.
  *
  * The data is atomically published to the topic and any waiting subscribers
@@ -184,6 +231,37 @@ extern int	orb_publish(const struct orb_metadata *meta, orb_advert_t handle, con
 extern int	orb_subscribe(const struct orb_metadata *meta) __EXPORT;
 
 /**
+ * Subscribe to a multi-instance of a topic.
+ *
+ * The returned value is a file descriptor that can be passed to poll()
+ * in order to wait for updates to a topic, as well as topic_read,
+ * orb_check and orb_stat.
+ *
+ * Subscription will succeed even if the topic has not been advertised;
+ * in this case the topic will have a timestamp of zero, it will never
+ * signal a poll() event, checking will always return false and it cannot
+ * be copied. When the topic is subsequently advertised, poll, check,
+ * stat and copy calls will react to the initial publication that is
+ * performed as part of the advertisement.
+ *
+ * Subscription will fail if the topic is not known to the system, i.e.
+ * there is nothing in the system that has declared the topic and thus it
+ * can never be published.
+ *
+ * @param meta		The uORB metadata (usually from the ORB_ID() macro)
+ *			for the topic.
+ * @param instance	The instance of the topic. Instance 0 matches the
+ *			topic of the orb_subscribe() call, higher indices
+ *			are for topics created with orb_publish_multi().
+ * @return		ERROR on error, otherwise returns a handle
+ *			that can be used to read and update the topic.
+ *			If the topic in question is not known (due to an
+ *			ORB_DEFINE_OPTIONAL with no corresponding ORB_DECLARE)
+ *			this function will return -1 and set errno to ENOENT.
+ */
+extern int	orb_subscribe_multi(const struct orb_metadata *meta, unsigned instance) __EXPORT;
+
+/**
  * Unsubscribe from a topic.
  *
  * @param handle	A handle returned from orb_subscribe.
@@ -240,6 +318,18 @@ extern int	orb_check(int handle, bool *updated) __EXPORT;
 extern int	orb_stat(int handle, uint64_t *time) __EXPORT;
 
 /**
+ * Return the priority of the topic
+ *
+ * @param handle	A handle returned from orb_subscribe.
+ * @param priority	Returns the priority of this topic. This is only relevant for
+ *			topics which are published by multiple publishers (e.g. mag0, mag1, etc.)
+ *			and allows a subscriber to automatically pick the topic with the highest
+ *			priority, independent of the startup order of the associated publishers.
+ * @return		OK on success, ERROR otherwise with errno set accordingly.
+ */
+extern int	orb_priority(int handle, int *priority) __EXPORT;
+
+/**
  * Set the minimum interval between which updates are seen for a subscription.
  *
  * If this interval is set, the subscriber will not see more than one update
diff --git a/src/modules/uavcan/actuators/esc.cpp b/src/modules/uavcan/actuators/esc.cpp
index 1d23099f3..51589e43e 100644
--- a/src/modules/uavcan/actuators/esc.cpp
+++ b/src/modules/uavcan/actuators/esc.cpp
@@ -49,6 +49,13 @@ UavcanEscController::UavcanEscController(uavcan::INode &node) :
 	_uavcan_sub_status(node),
 	_orb_timer(node)
 {
+	if (_perfcnt_invalid_input == nullptr) {
+		errx(1, "uavcan: couldn't allocate _perfcnt_invalid_input");
+	}
+
+	if (_perfcnt_scaling_error == nullptr) {
+		errx(1, "uavcan: couldn't allocate _perfcnt_scaling_error");
+	}
 }
 
 UavcanEscController::~UavcanEscController()
@@ -76,7 +83,9 @@ int UavcanEscController::init()
 
 void UavcanEscController::update_outputs(float *outputs, unsigned num_outputs)
 {
-	if ((outputs == nullptr) || (num_outputs > uavcan::equipment::esc::RawCommand::FieldTypes::cmd::MaxSize)) {
+	if ((outputs == nullptr) || 
+            (num_outputs > uavcan::equipment::esc::RawCommand::FieldTypes::cmd::MaxSize) ||
+            (num_outputs > CONNECTED_ESC_MAX)) {
 		perf_count(_perfcnt_invalid_input);
 		return;
 	}
@@ -101,10 +110,15 @@ void UavcanEscController::update_outputs(float *outputs, unsigned num_outputs)
 	for (unsigned i = 0; i < num_outputs; i++) {
 		if (_armed_mask & MOTOR_BIT(i)) {
 			float scaled = (outputs[i] + 1.0F) * 0.5F * cmd_max;
-			if (scaled < 1.0F) {
-				scaled = 1.0F;  // Since we're armed, we don't want to stop it completely
-			}
-
+                        // trim negative values back to 0. Previously
+                        // we set this to 0.1, which meant motors kept
+                        // spinning when armed, but that should be a
+                        // policy decision for a specific vehicle
+                        // type, as it is not appropriate for all
+                        // types of vehicles (eg. fixed wing).
+			if (scaled < 0.0F) {
+				scaled = 0.0F;
+                        }
 			if (scaled > cmd_max) {
 				scaled = cmd_max;
 				perf_count(_perfcnt_scaling_error);
diff --git a/src/modules/uavcan/module.mk b/src/modules/uavcan/module.mk
index f92bc754f..4f63629a0 100644
--- a/src/modules/uavcan/module.mk
+++ b/src/modules/uavcan/module.mk
@@ -1,6 +1,6 @@
 ############################################################################
 #
-#   Copyright (C) 2013 PX4 Development Team. All rights reserved.
+#   Copyright (c) 2013-2015 PX4 Development Team. All rights reserved.
 #   Author: Pavel Kirienko <pavel.kirienko@gmail.com>
 #
 # Redistribution and use in source and binary forms, with or without
@@ -38,7 +38,7 @@
 
 MODULE_COMMAND = uavcan
 
-MAXOPTIMIZATION = -Os
+MAXOPTIMIZATION = -O3
 
 # Main
 SRCS += uavcan_main.cpp              \
@@ -57,7 +57,7 @@ SRCS += sensors/sensor_bridge.cpp   \
 #
 # libuavcan
 #
-include $(UAVCAN_DIR)/libuavcan/include.mk
+include $(PX4_LIB_DIR)uavcan/libuavcan/include.mk
 SRCS += $(LIBUAVCAN_SRC)
 INCLUDE_DIRS += $(LIBUAVCAN_INC)
 # Since actual compiler mode is C++11, the library will default to UAVCAN_CPP11, but it will fail to compile
@@ -67,7 +67,7 @@ override EXTRADEFINES := $(EXTRADEFINES) -DUAVCAN_CPP_VERSION=UAVCAN_CPP03 -DUAV
 #
 # libuavcan drivers for STM32
 #
-include $(UAVCAN_DIR)/libuavcan_drivers/stm32/driver/include.mk
+include $(PX4_LIB_DIR)uavcan/libuavcan_drivers/stm32/driver/include.mk
 SRCS += $(LIBUAVCAN_STM32_SRC)
 INCLUDE_DIRS += $(LIBUAVCAN_STM32_INC)
 override EXTRADEFINES := $(EXTRADEFINES) -DUAVCAN_STM32_NUTTX -DUAVCAN_STM32_NUM_IFACES=2
diff --git a/src/modules/uavcan/sensors/baro.cpp b/src/modules/uavcan/sensors/baro.cpp
index 80c5e3828..ad09dfcac 100644
--- a/src/modules/uavcan/sensors/baro.cpp
+++ b/src/modules/uavcan/sensors/baro.cpp
@@ -1,6 +1,6 @@
 /****************************************************************************
  *
- *   Copyright (C) 2014 PX4 Development Team. All rights reserved.
+ *   Copyright (c) 2014, 2015 PX4 Development Team. All rights reserved.
  *
  * Redistribution and use in source and binary forms, with or without
  * modification, are permitted provided that the following conditions
@@ -38,15 +38,10 @@
 #include "baro.hpp"
 #include <cmath>
 
-static const orb_id_t BARO_TOPICS[2] = {
-	ORB_ID(sensor_baro0),
-	ORB_ID(sensor_baro1)
-};
-
 const char *const UavcanBarometerBridge::NAME = "baro";
 
 UavcanBarometerBridge::UavcanBarometerBridge(uavcan::INode& node) :
-UavcanCDevSensorBridgeBase("uavcan_baro", "/dev/uavcan/baro", BARO_DEVICE_PATH, BARO_TOPICS),
+UavcanCDevSensorBridgeBase("uavcan_baro", "/dev/uavcan/baro", BARO_DEVICE_PATH, ORB_ID(sensor_baro)),
 _sub_air_data(node)
 {
 }
@@ -91,11 +86,7 @@ void UavcanBarometerBridge::air_data_sub_cb(const uavcan::ReceivedDataStructure<
 {
 	auto report = ::baro_report();
 
-	report.timestamp = msg.getUtcTimestamp().toUSec();
-	if (report.timestamp == 0) {
-		report.timestamp = msg.getMonotonicTimestamp().toUSec();
-	}
-
+	report.timestamp   = msg.getMonotonicTimestamp().toUSec();
 	report.temperature = msg.static_temperature;
 	report.pressure    = msg.static_pressure / 100.0F;  // Convert to millibar
 
diff --git a/src/modules/uavcan/sensors/baro.hpp b/src/modules/uavcan/sensors/baro.hpp
index 9d470219e..c7bbc5af8 100644
--- a/src/modules/uavcan/sensors/baro.hpp
+++ b/src/modules/uavcan/sensors/baro.hpp
@@ -1,6 +1,6 @@
 /****************************************************************************
  *
- *   Copyright (C) 2014 PX4 Development Team. All rights reserved.
+ *   Copyright (c) 2014, 2015 PX4 Development Team. All rights reserved.
  *
  * Redistribution and use in source and binary forms, with or without
  * modification, are permitted provided that the following conditions
diff --git a/src/modules/uavcan/sensors/gnss.cpp b/src/modules/uavcan/sensors/gnss.cpp
index 24afe6aaf..3ae07367f 100644
--- a/src/modules/uavcan/sensors/gnss.cpp
+++ b/src/modules/uavcan/sensors/gnss.cpp
@@ -1,6 +1,6 @@
 /****************************************************************************
  *
- *   Copyright (C) 2014 PX4 Development Team. All rights reserved.
+ *   Copyright (c) 2014, 2015 PX4 Development Team. All rights reserved.
  *
  * Redistribution and use in source and binary forms, with or without
  * modification, are permitted provided that the following conditions
@@ -92,7 +92,7 @@ void UavcanGnssBridge::gnss_fix_sub_cb(const uavcan::ReceivedDataStructure<uavca
 
 	auto report = ::vehicle_gps_position_s();
 
-	report.timestamp_position = hrt_absolute_time();
+	report.timestamp_position = msg.getMonotonicTimestamp().toUSec();
 	report.lat = msg.latitude_deg_1e8 / 10;
 	report.lon = msg.longitude_deg_1e8 / 10;
 	report.alt = msg.height_msl_mm;
@@ -159,7 +159,7 @@ void UavcanGnssBridge::gnss_fix_sub_cb(const uavcan::ReceivedDataStructure<uavca
 	report.vel_ned_valid = true;
 
 	report.timestamp_time = report.timestamp_position;
-	report.time_gps_usec = uavcan::UtcTime(msg.gnss_timestamp).toUSec();	// Convert to microseconds
+	report.time_utc_usec = uavcan::UtcTime(msg.gnss_timestamp).toUSec();	// Convert to microseconds
 
 	report.satellites_used = msg.sats_used;
 
diff --git a/src/modules/uavcan/sensors/gnss.hpp b/src/modules/uavcan/sensors/gnss.hpp
index e8466b401..96ff9404f 100644
--- a/src/modules/uavcan/sensors/gnss.hpp
+++ b/src/modules/uavcan/sensors/gnss.hpp
@@ -1,6 +1,6 @@
 /****************************************************************************
  *
- *   Copyright (C) 2014 PX4 Development Team. All rights reserved.
+ *   Copyright (c) 2014, 2015 PX4 Development Team. All rights reserved.
  *
  * Redistribution and use in source and binary forms, with or without
  * modification, are permitted provided that the following conditions
@@ -43,8 +43,6 @@
 
 #pragma once
 
-#include <drivers/drv_hrt.h>
-
 #include <uORB/uORB.h>
 #include <uORB/topics/vehicle_gps_position.h>
 
diff --git a/src/modules/uavcan/sensors/mag.cpp b/src/modules/uavcan/sensors/mag.cpp
index 0d9ea08c5..ee278aaf5 100644
--- a/src/modules/uavcan/sensors/mag.cpp
+++ b/src/modules/uavcan/sensors/mag.cpp
@@ -1,6 +1,6 @@
 /****************************************************************************
  *
- *   Copyright (C) 2014 PX4 Development Team. All rights reserved.
+ *   Copyright (c) 2014, 2015 PX4 Development Team. All rights reserved.
  *
  * Redistribution and use in source and binary forms, with or without
  * modification, are permitted provided that the following conditions
@@ -37,16 +37,12 @@
 
 #include "mag.hpp"
 
-static const orb_id_t MAG_TOPICS[3] = {
-	ORB_ID(sensor_mag0),
-	ORB_ID(sensor_mag1),
-	ORB_ID(sensor_mag2)
-};
+#include <systemlib/err.h>
 
 const char *const UavcanMagnetometerBridge::NAME = "mag";
 
 UavcanMagnetometerBridge::UavcanMagnetometerBridge(uavcan::INode& node) :
-UavcanCDevSensorBridgeBase("uavcan_mag", "/dev/uavcan/mag", MAG_DEVICE_PATH, MAG_TOPICS),
+UavcanCDevSensorBridgeBase("uavcan_mag", "/dev/uavcan/mag", MAG_DEVICE_PATH, ORB_ID(sensor_mag)),
 _sub_mag(node)
 {
 	_device_id.devid_s.devtype = DRV_MAG_DEVTYPE_HMC5883;
@@ -71,9 +67,36 @@ int UavcanMagnetometerBridge::init()
 	return 0;
 }
 
+ssize_t UavcanMagnetometerBridge::read(struct file *filp, char *buffer, size_t buflen)
+{
+	static uint64_t last_read = 0;
+	struct mag_report *mag_buf = reinterpret_cast<struct mag_report *>(buffer);
+
+	/* buffer must be large enough */
+	unsigned count = buflen / sizeof(struct mag_report);
+	if (count < 1) {
+		return -ENOSPC;
+	}
+
+	if (last_read < _report.timestamp) {
+		/* copy report */
+		lock();
+		*mag_buf = _report;
+		last_read = _report.timestamp;
+		unlock();
+		return sizeof(struct mag_report);
+	} else {
+		/* no new data available, warn caller */
+		return -EAGAIN;
+	}
+}
+
 int UavcanMagnetometerBridge::ioctl(struct file *filp, int cmd, unsigned long arg)
 {
 	switch (cmd) {
+	case SENSORIOCSQUEUEDEPTH: {
+		return OK;			// Pretend that this stuff is supported to keep APM happy
+	}
 	case MAGIOCSSCALE: {
 		std::memcpy(&_scale, reinterpret_cast<const void*>(arg), sizeof(_scale));
 		return 0;
@@ -86,7 +109,7 @@ int UavcanMagnetometerBridge::ioctl(struct file *filp, int cmd, unsigned long ar
 		return 0;           // Nothing to do
 	}
 	case MAGIOCGEXTERNAL: {
-		return 0;           // We don't want anyone to transform the coordinate frame, so we declare it onboard
+		return 1;           // declare it external rise it's priority and to allow for correct orientation compensation
 	}
 	case MAGIOCSSAMPLERATE: {
 		return 0;           // Pretend that this stuff is supported to keep the sensor app happy
@@ -108,18 +131,14 @@ int UavcanMagnetometerBridge::ioctl(struct file *filp, int cmd, unsigned long ar
 
 void UavcanMagnetometerBridge::mag_sub_cb(const uavcan::ReceivedDataStructure<uavcan::equipment::ahrs::Magnetometer> &msg)
 {
-	auto report = ::mag_report();
-
-	report.range_ga = 1.3F;   // Arbitrary number, doesn't really mean anything
-
-	report.timestamp = msg.getUtcTimestamp().toUSec();
-	if (report.timestamp == 0) {
-		report.timestamp = msg.getMonotonicTimestamp().toUSec();
-	}
+	lock();
+	_report.range_ga = 1.3F;   // Arbitrary number, doesn't really mean anything
+	_report.timestamp = msg.getMonotonicTimestamp().toUSec();
 
-	report.x = (msg.magnetic_field[0] - _scale.x_offset) * _scale.x_scale;
-	report.y = (msg.magnetic_field[1] - _scale.y_offset) * _scale.y_scale;
-	report.z = (msg.magnetic_field[2] - _scale.z_offset) * _scale.z_scale;
+	_report.x = (msg.magnetic_field[0] - _scale.x_offset) * _scale.x_scale;
+	_report.y = (msg.magnetic_field[1] - _scale.y_offset) * _scale.y_scale;
+	_report.z = (msg.magnetic_field[2] - _scale.z_offset) * _scale.z_scale;
+	unlock();
 
-	publish(msg.getSrcNodeID().get(), &report);
+	publish(msg.getSrcNodeID().get(), &_report);
 }
diff --git a/src/modules/uavcan/sensors/mag.hpp b/src/modules/uavcan/sensors/mag.hpp
index 6d413a8f7..db38aee1d 100644
--- a/src/modules/uavcan/sensors/mag.hpp
+++ b/src/modules/uavcan/sensors/mag.hpp
@@ -1,6 +1,6 @@
 /****************************************************************************
  *
- *   Copyright (C) 2014 PX4 Development Team. All rights reserved.
+ *   Copyright (c) 2014, 2015 PX4 Development Team. All rights reserved.
  *
  * Redistribution and use in source and binary forms, with or without
  * modification, are permitted provided that the following conditions
@@ -54,6 +54,7 @@ public:
 	int init() override;
 
 private:
+	ssize_t	read(struct file *filp, char *buffer, size_t buflen);
 	int ioctl(struct file *filp, int cmd, unsigned long arg) override;
 
 	void mag_sub_cb(const uavcan::ReceivedDataStructure<uavcan::equipment::ahrs::Magnetometer> &msg);
@@ -65,4 +66,5 @@ private:
 
 	uavcan::Subscriber<uavcan::equipment::ahrs::Magnetometer, MagCbBinder> _sub_mag;
 	mag_scale _scale = {};
+	mag_report _report =  {};
 };
diff --git a/src/modules/uavcan/sensors/sensor_bridge.cpp b/src/modules/uavcan/sensors/sensor_bridge.cpp
index 0999938fc..b37076444 100644
--- a/src/modules/uavcan/sensors/sensor_bridge.cpp
+++ b/src/modules/uavcan/sensors/sensor_bridge.cpp
@@ -62,7 +62,6 @@ UavcanCDevSensorBridgeBase::~UavcanCDevSensorBridgeBase()
 			(void)unregister_class_devname(_class_devname, _channels[i].class_instance);
 		}
 	}
-	delete [] _orb_topics;
 	delete [] _channels;
 }
 
@@ -116,11 +115,10 @@ void UavcanCDevSensorBridgeBase::publish(const int node_id, const void *report)
 		}
 
 		// Publish to the appropriate topic, abort on failure
-		channel->orb_id         = _orb_topics[class_instance];
 		channel->node_id        = node_id;
 		channel->class_instance = class_instance;
 
-		channel->orb_advert = orb_advertise(channel->orb_id, report);
+		channel->orb_advert = orb_advertise_multi(_orb_topic, report, &channel->orb_instance, ORB_PRIO_HIGH);
 		if (channel->orb_advert < 0) {
 			log("ADVERTISE FAILED");
 			(void)unregister_class_devname(_class_devname, class_instance);
@@ -132,7 +130,7 @@ void UavcanCDevSensorBridgeBase::publish(const int node_id, const void *report)
 	}
 	assert(channel != nullptr);
 
-	(void)orb_publish(channel->orb_id, channel->orb_advert, report);
+	(void)orb_publish(_orb_topic, channel->orb_advert, report);
 }
 
 unsigned UavcanCDevSensorBridgeBase::get_num_redundant_channels() const
diff --git a/src/modules/uavcan/sensors/sensor_bridge.hpp b/src/modules/uavcan/sensors/sensor_bridge.hpp
index e31960537..de130b078 100644
--- a/src/modules/uavcan/sensors/sensor_bridge.hpp
+++ b/src/modules/uavcan/sensors/sensor_bridge.hpp
@@ -1,6 +1,6 @@
 /****************************************************************************
  *
- *   Copyright (C) 2014 PX4 Development Team. All rights reserved.
+ *   Copyright (c) 2014, 2015 PX4 Development Team. All rights reserved.
  *
  * Redistribution and use in source and binary forms, with or without
  * modification, are permitted provided that the following conditions
@@ -75,8 +75,7 @@ public:
 
 	/**
 	 * Sensor bridge factory.
-	 * Creates a bridge object by its ASCII name, e.g. "gnss", "mag".
-	 * @return nullptr if such bridge can't be created.
+	 * Creates all known sensor bridges and puts them in the linked list.
 	 */
 	static void make_all(uavcan::INode &node, List<IUavcanSensorBridge*> &list);
 };
@@ -90,28 +89,29 @@ class UavcanCDevSensorBridgeBase : public IUavcanSensorBridge, public device::CD
 	struct Channel
 	{
 		int node_id              = -1;
-		orb_id_t orb_id          = nullptr;
 		orb_advert_t orb_advert  = -1;
 		int class_instance       = -1;
+		int orb_instance	 = -1;
 	};
 
 	const unsigned _max_channels;
 	const char *const _class_devname;
-	orb_id_t *const _orb_topics;
+	const orb_id_t _orb_topic;
 	Channel *const _channels;
 	bool _out_of_channels = false;
 
 protected:
-	template <unsigned MaxChannels>
+	static constexpr unsigned DEFAULT_MAX_CHANNELS = 5; // 640 KB ought to be enough for anybody
+
 	UavcanCDevSensorBridgeBase(const char *name, const char *devname, const char *class_devname,
-	                           const orb_id_t (&orb_topics)[MaxChannels]) :
+	                           const orb_id_t orb_topic_sensor,
+	                           const unsigned max_channels = DEFAULT_MAX_CHANNELS) :
 	device::CDev(name, devname),
-	_max_channels(MaxChannels),
+	_max_channels(max_channels),
 	_class_devname(class_devname),
-	_orb_topics(new orb_id_t[MaxChannels]),
-	_channels(new Channel[MaxChannels])
+	_orb_topic(orb_topic_sensor),
+	_channels(new Channel[max_channels])
 	{
-		memcpy(_orb_topics, orb_topics, sizeof(orb_id_t) * MaxChannels);
 		_device_id.devid_s.bus_type = DeviceBusType_UAVCAN;
 		_device_id.devid_s.bus = 0;
 	}
diff --git a/src/modules/uavcan/uavcan_main.cpp b/src/modules/uavcan/uavcan_main.cpp
index 2c543462e..4dc03b61b 100644
--- a/src/modules/uavcan/uavcan_main.cpp
+++ b/src/modules/uavcan/uavcan_main.cpp
@@ -46,6 +46,8 @@
 #include <arch/board/board.h>
 #include <arch/chip/chip.h>
 
+#include <uORB/topics/esc_status.h>
+
 #include <drivers/drv_hrt.h>
 #include <drivers/drv_pwm_output.h>
 
@@ -79,6 +81,18 @@ UavcanNode::UavcanNode(uavcan::ICanDriver &can_driver, uavcan::ISystemClock &sys
 	if (res < 0) {
 		std::abort();
 	}
+
+	if (_perfcnt_node_spin_elapsed == nullptr) {
+		errx(1, "uavcan: couldn't allocate _perfcnt_node_spin_elapsed");
+	}
+
+	if (_perfcnt_esc_mixer_output_elapsed == nullptr) {
+		errx(1, "uavcan: couldn't allocate _perfcnt_esc_mixer_output_elapsed");
+	}
+
+	if (_perfcnt_esc_mixer_total_elapsed == nullptr) {
+		errx(1, "uavcan: couldn't allocate _perfcnt_esc_mixer_total_elapsed");
+	}
 }
 
 UavcanNode::~UavcanNode()
@@ -116,6 +130,10 @@ UavcanNode::~UavcanNode()
 	}
 
 	_instance = nullptr;
+
+	perf_free(_perfcnt_node_spin_elapsed);
+	perf_free(_perfcnt_esc_mixer_output_elapsed);
+	perf_free(_perfcnt_esc_mixer_total_elapsed);
 }
 
 int UavcanNode::start(uavcan::NodeID node_id, uint32_t bitrate)
@@ -263,12 +281,32 @@ int UavcanNode::init(uavcan::NodeID node_id)
 
 void UavcanNode::node_spin_once()
 {
+	perf_begin(_perfcnt_node_spin_elapsed);
 	const int spin_res = _node.spin(uavcan::MonotonicTime());
 	if (spin_res < 0) {
 		warnx("node spin error %i", spin_res);
 	}
+	perf_end(_perfcnt_node_spin_elapsed);
+}
+
+/*
+  add a fd to the list of polled events. This assumes you want
+  POLLIN for now.
+ */
+int UavcanNode::add_poll_fd(int fd)
+{
+	int ret = _poll_fds_num;
+	if (_poll_fds_num >= UAVCAN_NUM_POLL_FDS) {
+		errx(1, "uavcan: too many poll fds, exiting");
+	}
+	_poll_fds[_poll_fds_num] = ::pollfd();
+	_poll_fds[_poll_fds_num].fd = fd;
+	_poll_fds[_poll_fds_num].events = POLLIN;
+	_poll_fds_num += 1;
+	return ret;
 }
 
+
 int UavcanNode::run()
 {
 	(void)pthread_mutex_lock(&_node_mutex);
@@ -280,9 +318,9 @@ int UavcanNode::run()
 
 	_armed_sub = orb_subscribe(ORB_ID(actuator_armed));
 	_test_motor_sub = orb_subscribe(ORB_ID(test_motor));
+	_actuator_direct_sub = orb_subscribe(ORB_ID(actuator_direct));
 
-	actuator_outputs_s outputs;
-	memset(&outputs, 0, sizeof(outputs));
+	memset(&_outputs, 0, sizeof(_outputs));
 
 	const int busevent_fd = ::open(uavcan_stm32::BusEvent::DevName, 0);
 	if (busevent_fd < 0)
@@ -304,11 +342,15 @@ int UavcanNode::run()
 	 * the value returned from poll() to detect whether actuator control has timed out or not.
 	 * Instead, all ORB events need to be checked individually (see below).
 	 */
-	_poll_fds_num = 0;
-	_poll_fds[_poll_fds_num] = ::pollfd();
-	_poll_fds[_poll_fds_num].fd = busevent_fd;
-	_poll_fds[_poll_fds_num].events = POLLIN;
-	_poll_fds_num += 1;
+	add_poll_fd(busevent_fd);
+
+	/*
+	 * setup poll to look for actuator direct input if we are
+	 * subscribed to the topic
+	 */
+	if (_actuator_direct_sub != -1) {
+		_actuator_direct_poll_fd_num = add_poll_fd(_actuator_direct_sub);
+	}
 
 	while (!_task_should_exit) {
 		// update actuator controls subscriptions if needed
@@ -320,12 +362,18 @@ int UavcanNode::run()
 		// Mutex is unlocked while the thread is blocked on IO multiplexing
 		(void)pthread_mutex_unlock(&_node_mutex);
 
+		perf_end(_perfcnt_esc_mixer_total_elapsed); // end goes first, it's not a mistake
+
 		const int poll_ret = ::poll(_poll_fds, _poll_fds_num, PollTimeoutMs);
 
+		perf_begin(_perfcnt_esc_mixer_total_elapsed);
+
 		(void)pthread_mutex_lock(&_node_mutex);
 
 		node_spin_once();  // Non-blocking
 
+		bool new_output = false;
+
 		// this would be bad...
 		if (poll_ret < 0) {
 			log("poll error %d", errno);
@@ -333,24 +381,39 @@ int UavcanNode::run()
 		} else {
 			// get controls for required topics
 			bool controls_updated = false;
-			unsigned poll_id = 1;
 			for (unsigned i = 0; i < NUM_ACTUATOR_CONTROL_GROUPS; i++) {
 				if (_control_subs[i] > 0) {
-					if (_poll_fds[poll_id].revents & POLLIN) {
+					if (_poll_fds[_poll_ids[i]].revents & POLLIN) {
 						controls_updated = true;
 						orb_copy(_control_topics[i], _control_subs[i], &_controls[i]);
 					}
-					poll_id++;
 				}
 			}
 
+			/*
+			  see if we have any direct actuator updates
+			 */
+			if (_actuator_direct_sub != -1 && 
+			    (_poll_fds[_actuator_direct_poll_fd_num].revents & POLLIN) &&
+			    orb_copy(ORB_ID(actuator_direct), _actuator_direct_sub, &_actuator_direct) == OK &&
+			    !_test_in_progress) {
+				if (_actuator_direct.nvalues > NUM_ACTUATOR_OUTPUTS) {
+					_actuator_direct.nvalues = NUM_ACTUATOR_OUTPUTS;
+				}
+				memcpy(&_outputs.output[0], &_actuator_direct.values[0], 
+				       _actuator_direct.nvalues*sizeof(float));
+				_outputs.noutputs = _actuator_direct.nvalues;
+				new_output = true;
+			}
+
 			// can we mix?
 			if (_test_in_progress) {
-				float test_outputs[NUM_ACTUATOR_OUTPUTS] = {};
-				test_outputs[_test_motor.motor_number] = _test_motor.value*2.0f-1.0f;
-
-				// Output to the bus
-				_esc_controller.update_outputs(test_outputs, NUM_ACTUATOR_OUTPUTS);
+				memset(&_outputs, 0, sizeof(_outputs));
+				if (_test_motor.motor_number < NUM_ACTUATOR_OUTPUTS) {
+					_outputs.output[_test_motor.motor_number] = _test_motor.value*2.0f-1.0f;
+					_outputs.noutputs = _test_motor.motor_number+1;
+				}
+				new_output = true;
 			} else if (controls_updated && (_mixers != nullptr)) {
 
 				// XXX one output group has 8 outputs max,
@@ -358,39 +421,43 @@ int UavcanNode::run()
 				unsigned num_outputs_max = 8;
 
 				// Do mixing
-				outputs.noutputs = _mixers->mix(&outputs.output[0], num_outputs_max);
-				outputs.timestamp = hrt_absolute_time();
-
-				// iterate actuators
-				for (unsigned i = 0; i < outputs.noutputs; i++) {
-					// last resort: catch NaN, INF and out-of-band errors
-					if (!isfinite(outputs.output[i])) {
-						/*
-						 * Value is NaN, INF or out of band - set to the minimum value.
-						 * This will be clearly visible on the servo status and will limit the risk of accidentally
-						 * spinning motors. It would be deadly in flight.
-						 */
-						outputs.output[i] = -1.0f;
-					}
+				_outputs.noutputs = _mixers->mix(&_outputs.output[0], num_outputs_max);
 
-					// limit outputs to valid range
+				new_output = true;
+			}
+		}
 
-					// never go below min
-					if (outputs.output[i] < -1.0f) {
-						outputs.output[i] = -1.0f;
-					}
+		if (new_output) {
+			// iterate actuators, checking for valid values
+			for (uint8_t i = 0; i < _outputs.noutputs; i++) {
+				// last resort: catch NaN, INF and out-of-band errors
+				if (!isfinite(_outputs.output[i])) {
+					/*
+					 * Value is NaN, INF or out of band - set to the minimum value.
+					 * This will be clearly visible on the servo status and will limit the risk of accidentally
+					 * spinning motors. It would be deadly in flight.
+					 */
+					_outputs.output[i] = -1.0f;
+				}
 
-					// never go below max
-					if (outputs.output[i] > 1.0f) {
-						outputs.output[i] = 1.0f;
-					}
+				// never go below min
+				if (_outputs.output[i] < -1.0f) {
+					_outputs.output[i] = -1.0f;
 				}
 
-				// Output to the bus
-				_esc_controller.update_outputs(outputs.output, outputs.noutputs);
+				// never go above max
+				if (_outputs.output[i] > 1.0f) {
+					_outputs.output[i] = 1.0f;
+				}
 			}
+			// Output to the bus
+			_outputs.timestamp = hrt_absolute_time();
+			perf_begin(_perfcnt_esc_mixer_output_elapsed);
+			_esc_controller.update_outputs(_outputs.output, _outputs.noutputs);
+			perf_end(_perfcnt_esc_mixer_output_elapsed);
 		}
 
+
 		// Check motor test state
 		bool updated = false;
 		orb_check(_test_motor_sub, &updated);
@@ -441,7 +508,7 @@ UavcanNode::teardown()
 			_control_subs[i] = -1;
 		}
 	}
-	return ::close(_armed_sub);
+	return (_armed_sub >= 0) ? ::close(_armed_sub) : 0;
 }
 
 int
@@ -459,7 +526,6 @@ UavcanNode::subscribe()
 	uint32_t sub_groups = _groups_required & ~_groups_subscribed;
 	uint32_t unsub_groups = _groups_subscribed & ~_groups_required;
 	// the first fd used by CAN
-	_poll_fds_num = 1;
 	for (unsigned i = 0; i < NUM_ACTUATOR_CONTROL_GROUPS; i++) {
 		if (sub_groups & (1 << i)) {
 			warnx("subscribe to actuator_controls_%d", i);
@@ -472,9 +538,7 @@ UavcanNode::subscribe()
 		}
 
 		if (_control_subs[i] > 0) {
-			_poll_fds[_poll_fds_num].fd = _control_subs[i];
-			_poll_fds[_poll_fds_num].events = POLLIN;
-			_poll_fds_num++;
+			_poll_ids[i] = add_poll_fd(_control_subs[i]);
 		}
 	}
 }
@@ -572,6 +636,36 @@ UavcanNode::print_info()
 	       (unsigned)_groups_subscribed, (unsigned)_groups_required, _poll_fds_num);
 	printf("ESC mixer: %s\n", (_mixers == nullptr) ? "NONE" : "OK");
 
+	if (_outputs.noutputs != 0) {
+		printf("ESC output: ");
+
+		for (uint8_t i=0; i<_outputs.noutputs; i++) {
+			printf("%d ", (int)(_outputs.output[i]*1000));
+		}
+		printf("\n");
+
+		// ESC status
+		int esc_sub = orb_subscribe(ORB_ID(esc_status));
+		struct esc_status_s esc;
+		memset(&esc, 0, sizeof(esc));
+		orb_copy(ORB_ID(esc_status), esc_sub, &esc);
+
+		printf("ESC Status:\n");
+		printf("Addr\tV\tA\tTemp\tSetpt\tRPM\tErr\n");
+		for (uint8_t i=0; i<_outputs.noutputs; i++) {
+			printf("%d\t",    esc.esc[i].esc_address);
+			printf("%3.2f\t", (double)esc.esc[i].esc_voltage);
+			printf("%3.2f\t", (double)esc.esc[i].esc_current);
+			printf("%3.2f\t", (double)esc.esc[i].esc_temperature);
+			printf("%3.2f\t", (double)esc.esc[i].esc_setpoint);
+			printf("%d\t",    esc.esc[i].esc_rpm);
+			printf("%d",      esc.esc[i].esc_errorcount);
+			printf("\n");
+		}
+
+		orb_unsubscribe(esc_sub);
+	}
+
 	// Sensor bridges
 	auto br = _sensor_bridges.getHead();
 	while (br != nullptr) {
@@ -590,7 +684,7 @@ UavcanNode::print_info()
 static void print_usage()
 {
 	warnx("usage: \n"
-	      "\tuavcan {start|status|stop}");
+	      "\tuavcan {start|status|stop|arm|disarm}");
 }
 
 extern "C" __EXPORT int uavcan_main(int argc, char *argv[]);
@@ -637,6 +731,16 @@ int uavcan_main(int argc, char *argv[])
 		::exit(0);
 	}
 
+	if (!std::strcmp(argv[1], "arm")) {
+		inst->arm_actuators(true);
+		::exit(0);
+	}
+
+	if (!std::strcmp(argv[1], "disarm")) {
+		inst->arm_actuators(false);
+		::exit(0);
+	}
+
 	if (!std::strcmp(argv[1], "stop")) {
 		delete inst;
 		::exit(0);
diff --git a/src/modules/uavcan/uavcan_main.hpp b/src/modules/uavcan/uavcan_main.hpp
index 274321f0d..19b9b4b48 100644
--- a/src/modules/uavcan/uavcan_main.hpp
+++ b/src/modules/uavcan/uavcan_main.hpp
@@ -34,14 +34,15 @@
 #pragma once
 
 #include <nuttx/config.h>
-
 #include <uavcan_stm32/uavcan_stm32.hpp>
 #include <drivers/device/device.h>
+#include <systemlib/perf_counter.h>
 
 #include <uORB/topics/actuator_controls.h>
 #include <uORB/topics/actuator_outputs.h>
 #include <uORB/topics/actuator_armed.h>
 #include <uORB/topics/test_motor.h>
+#include <uORB/topics/actuator_direct.h>
 
 #include "actuators/esc.hpp"
 #include "sensors/sensor_bridge.hpp"
@@ -57,12 +58,15 @@
 #define NUM_ACTUATOR_CONTROL_GROUPS_UAVCAN	4
 #define UAVCAN_DEVICE_PATH	"/dev/uavcan/esc"
 
+// we add two to allow for actuator_direct and busevent
+#define UAVCAN_NUM_POLL_FDS (NUM_ACTUATOR_CONTROL_GROUPS_UAVCAN+2)
+
 /**
  * A UAVCAN node.
  */
 class UavcanNode : public device::CDev
 {
-	static constexpr unsigned MemPoolSize        = 10752;
+	static constexpr unsigned MemPoolSize        = 10752; ///< Refer to the libuavcan manual to learn why
 	static constexpr unsigned RxQueueLenPerIface = 64;
 	static constexpr unsigned StackSize          = 3000;
 
@@ -97,15 +101,17 @@ private:
 	int		init(uavcan::NodeID node_id);
 	void		node_spin_once();
 	int		run();
+	int		add_poll_fd(int fd);			///< add a fd to poll list, returning index into _poll_fds[]
+
 
 	int			_task = -1;			///< handle to the OS task
 	bool			_task_should_exit = false;	///< flag to indicate to tear down the CAN driver
 	int			_armed_sub = -1;		///< uORB subscription of the arming status
-	actuator_armed_s	_armed;				///< the arming request of the system
+	actuator_armed_s	_armed = {};			///< the arming request of the system
 	bool			_is_armed = false;		///< the arming status of the actuators on the bus
 
 	int			_test_motor_sub = -1;   ///< uORB subscription of the test_motor status
-	test_motor_s		_test_motor;
+	test_motor_s		_test_motor = {};
 	bool			_test_in_progress = false;
 
 	unsigned		_output_count = 0;		///< number of actuators currently available
@@ -125,6 +131,19 @@ private:
 	int			_control_subs[NUM_ACTUATOR_CONTROL_GROUPS_UAVCAN] = {};
 	actuator_controls_s 	_controls[NUM_ACTUATOR_CONTROL_GROUPS_UAVCAN] = {};
 	orb_id_t		_control_topics[NUM_ACTUATOR_CONTROL_GROUPS_UAVCAN] = {};
-	pollfd			_poll_fds[NUM_ACTUATOR_CONTROL_GROUPS_UAVCAN + 1] = {};	///< +1 for /dev/uavcan/busevent
+	pollfd			_poll_fds[UAVCAN_NUM_POLL_FDS] = {};
 	unsigned		_poll_fds_num = 0;
+
+	int			_actuator_direct_sub = -1;   ///< uORB subscription of the actuator_direct topic
+	uint8_t			_actuator_direct_poll_fd_num = 0;
+	actuator_direct_s	_actuator_direct = {};
+
+	actuator_outputs_s	_outputs = {};
+
+	// index into _poll_fds for each _control_subs handle
+	uint8_t			_poll_ids[NUM_ACTUATOR_CONTROL_GROUPS_UAVCAN];
+
+	perf_counter_t _perfcnt_node_spin_elapsed        = perf_alloc(PC_ELAPSED, "uavcan_node_spin_elapsed");
+	perf_counter_t _perfcnt_esc_mixer_output_elapsed = perf_alloc(PC_ELAPSED, "uavcan_esc_mixer_output_elapsed");
+	perf_counter_t _perfcnt_esc_mixer_total_elapsed  = perf_alloc(PC_ELAPSED, "uavcan_esc_mixer_total_elapsed");
 };
diff --git a/src/modules/vtol_att_control/module.mk b/src/modules/vtol_att_control/module.mk
new file mode 100644
index 000000000..0cf3072c8
--- /dev/null
+++ b/src/modules/vtol_att_control/module.mk
@@ -0,0 +1,44 @@
+############################################################################
+#
+#   Copyright (c) 2013, 2014 PX4 Development Team. All rights reserved.
+#
+# Redistribution and use in source and binary forms, with or without
+# modification, are permitted provided that the following conditions
+# are met:
+#
+# 1. Redistributions of source code must retain the above copyright
+#    notice, this list of conditions and the following disclaimer.
+# 2. Redistributions in binary form must reproduce the above copyright
+#    notice, this list of conditions and the following disclaimer in
+#    the documentation and/or other materials provided with the
+#    distribution.
+# 3. Neither the name PX4 nor the names of its contributors may be
+#    used to endorse or promote products derived from this software
+#    without specific prior written permission.
+#
+# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+# OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+# AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+# POSSIBILITY OF SUCH DAMAGE.
+#
+############################################################################
+
+#
+# VTOL attitude controller
+#
+
+MODULE_COMMAND	= vtol_att_control
+
+SRCS		= vtol_att_control_main.cpp \
+			  vtol_att_control_params.c
+
+EXTRACXXFLAGS = -Wno-write-strings
+
diff --git a/src/modules/vtol_att_control/vtol_att_control_main.cpp b/src/modules/vtol_att_control/vtol_att_control_main.cpp
new file mode 100644
index 000000000..8e68730b8
--- /dev/null
+++ b/src/modules/vtol_att_control/vtol_att_control_main.cpp
@@ -0,0 +1,912 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2013, 2014 PX4 Development Team. All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in
+ *    the documentation and/or other materials provided with the
+ *    distribution.
+ * 3. Neither the name PX4 nor the names of its contributors may be
+ *    used to endorse or promote products derived from this software
+ *    without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ ****************************************************************************/
+
+/**
+ * @file VTOL_att_control_main.cpp
+ * Implementation of an attitude controller for VTOL airframes. This module receives data
+ * from both the fixed wing- and the multicopter attitude controllers and processes it.
+ * It computes the correct actuator controls depending on which mode the vehicle is in (hover,forward-
+ * flight or transition). It also publishes the resulting controls on the actuator controls topics.
+ *
+ * @author Roman Bapst 		<bapstr@ethz.ch>
+ * @author Lorenz Meier 	<lm@inf.ethz.ch>
+ * @author Thomas Gubler	<thomasgubler@gmail.com>
+ *
+ */
+
+#include <nuttx/config.h>
+#include <stdio.h>
+#include <stdlib.h>
+#include <string.h>
+#include <unistd.h>
+#include <errno.h>
+#include <math.h>
+#include <poll.h>
+#include <drivers/drv_hrt.h>
+#include <arch/board/board.h>
+#include <uORB/uORB.h>
+#include <uORB/topics/vehicle_attitude_setpoint.h>
+#include <uORB/topics/manual_control_setpoint.h>
+#include <uORB/topics/actuator_controls.h>
+#include <uORB/topics/vehicle_rates_setpoint.h>
+#include <uORB/topics/vehicle_attitude.h>
+#include <uORB/topics/vehicle_control_mode.h>
+#include <uORB/topics/vtol_vehicle_status.h>
+#include <uORB/topics/actuator_armed.h>
+#include <uORB/topics/airspeed.h>
+#include <uORB/topics/parameter_update.h>
+#include <uORB/topics/vehicle_local_position.h>
+#include <uORB/topics/battery_status.h>
+#include <systemlib/param/param.h>
+#include <systemlib/err.h>
+#include <systemlib/perf_counter.h>
+#include <systemlib/systemlib.h>
+#include <systemlib/circuit_breaker.h>
+#include <lib/mathlib/mathlib.h>
+#include <lib/geo/geo.h>
+
+#include "drivers/drv_pwm_output.h"
+#include <nuttx/fs/ioctl.h>
+
+#include <fcntl.h>
+
+
+extern "C" __EXPORT int vtol_att_control_main(int argc, char *argv[]);
+
+class VtolAttitudeControl
+{
+public:
+
+	VtolAttitudeControl();
+	~VtolAttitudeControl();
+
+	int start();	/* start the task and return OK on success */
+
+
+private:
+//******************flags & handlers******************************************************
+	bool _task_should_exit;
+	int _control_task;		//task handle for VTOL attitude controller
+
+	/* handlers for subscriptions */
+	int		_v_att_sub;				//vehicle attitude subscription
+	int		_v_att_sp_sub;			//vehicle attitude setpoint subscription
+	int		_mc_virtual_v_rates_sp_sub;		//vehicle rates setpoint subscription
+	int		_fw_virtual_v_rates_sp_sub;		//vehicle rates setpoint subscription
+	int		_v_control_mode_sub;	//vehicle control mode subscription
+	int		_params_sub;			//parameter updates subscription
+	int		_manual_control_sp_sub;	//manual control setpoint subscription
+	int		_armed_sub;				//arming status subscription
+	int 	_local_pos_sub;			// sensor subscription
+	int 	_airspeed_sub;			// airspeed subscription
+	int 	_battery_status_sub;	// battery status subscription
+
+	int 	_actuator_inputs_mc;	//topic on which the mc_att_controller publishes actuator inputs
+	int 	_actuator_inputs_fw;	//topic on which the fw_att_controller publishes actuator inputs
+
+	//handlers for publishers
+	orb_advert_t	_actuators_0_pub;		//input for the mixer (roll,pitch,yaw,thrust)
+	orb_advert_t 	_actuators_1_pub;
+	orb_advert_t	_vtol_vehicle_status_pub;
+	orb_advert_t	_v_rates_sp_pub;
+//*******************data containers***********************************************************
+	struct vehicle_attitude_s			_v_att;				//vehicle attitude
+	struct vehicle_attitude_setpoint_s	_v_att_sp;			//vehicle attitude setpoint
+	struct vehicle_rates_setpoint_s		_v_rates_sp;		//vehicle rates setpoint
+	struct vehicle_rates_setpoint_s		_mc_virtual_v_rates_sp;		// virtual mc vehicle rates setpoint
+	struct vehicle_rates_setpoint_s		_fw_virtual_v_rates_sp;		// virtual fw vehicle rates setpoint
+	struct manual_control_setpoint_s	_manual_control_sp; //manual control setpoint
+	struct vehicle_control_mode_s		_v_control_mode;	//vehicle control mode
+	struct vtol_vehicle_status_s 		_vtol_vehicle_status;
+	struct actuator_controls_s			_actuators_out_0;	//actuator controls going to the mc mixer
+	struct actuator_controls_s			_actuators_out_1;	//actuator controls going to the fw mixer (used for elevons)
+	struct actuator_controls_s			_actuators_mc_in;	//actuator controls from mc_att_control
+	struct actuator_controls_s			_actuators_fw_in;	//actuator controls from fw_att_control
+	struct actuator_armed_s				_armed;				//actuator arming status
+	struct vehicle_local_position_s		_local_pos;
+	struct airspeed_s 					_airspeed;			// airspeed
+	struct battery_status_s 			_batt_status; 		// battery status
+
+	struct {
+		param_t idle_pwm_mc;	//pwm value for idle in mc mode
+		param_t vtol_motor_count;
+		param_t vtol_fw_permanent_stab;	// in fw mode stabilize attitude also in manual mode
+		float mc_airspeed_min;		// min airspeed in multicoper mode (including prop-wash)
+		float mc_airspeed_trim;		// trim airspeed in multicopter mode
+		float mc_airspeed_max;		// max airpseed in multicopter mode
+		float fw_pitch_trim;		// trim for neutral elevon position in fw mode
+		float power_max;			// maximum power of one engine
+		float prop_eff;				// factor to calculate prop efficiency
+		float arsp_lp_gain;			// total airspeed estimate low pass gain
+	} _params;
+
+	struct {
+		param_t idle_pwm_mc;
+		param_t vtol_motor_count;
+		param_t vtol_fw_permanent_stab;
+		param_t mc_airspeed_min;
+		param_t mc_airspeed_trim;
+		param_t mc_airspeed_max;
+		param_t fw_pitch_trim;
+		param_t power_max;
+		param_t prop_eff;
+		param_t arsp_lp_gain;
+	} _params_handles;
+
+	perf_counter_t	_loop_perf;			/**< loop performance counter */
+	perf_counter_t	_nonfinite_input_perf;		/**< performance counter for non finite input */
+
+	/* for multicopters it is usual to have a non-zero idle speed of the engines
+	 * for fixed wings we want to have an idle speed of zero since we do not want
+	 * to waste energy when gliding. */
+	bool flag_idle_mc;		//false = "idle is set for fixed wing mode"; true = "idle is set for multicopter mode"
+	unsigned _motor_count;	// number of motors
+	float _airspeed_tot;
+
+//*****************Member functions***********************************************************************
+
+	void 		task_main();	//main task
+	static void	task_main_trampoline(int argc, char *argv[]);	//Shim for calling task_main from task_create.
+
+	void		vehicle_control_mode_poll();	//Check for changes in vehicle control mode.
+	void		vehicle_manual_poll();			//Check for changes in manual inputs.
+	void		arming_status_poll();			//Check for arming status updates.
+	void 		actuator_controls_mc_poll();	//Check for changes in mc_attitude_control output
+	void 		actuator_controls_fw_poll();	//Check for changes in fw_attitude_control output
+	void 		vehicle_rates_sp_mc_poll();
+	void 		vehicle_rates_sp_fw_poll();
+	void 		vehicle_local_pos_poll();		// Check for changes in sensor values
+	void 		vehicle_airspeed_poll();		// Check for changes in airspeed
+	void 		vehicle_battery_poll();			// Check for battery updates
+	void 		parameters_update_poll();		//Check if parameters have changed
+	int 		parameters_update();			//Update local paraemter cache
+	void  		fill_mc_att_control_output();	//write mc_att_control results to actuator message
+	void		fill_fw_att_control_output();	//write fw_att_control results to actuator message
+	void 		fill_mc_att_rates_sp();
+	void 		fill_fw_att_rates_sp();
+	void 		set_idle_fw();
+	void 		set_idle_mc();
+	void 		scale_mc_output();
+	void 		calc_tot_airspeed();			// estimated airspeed seen by elevons
+};
+
+namespace VTOL_att_control
+{
+VtolAttitudeControl *g_control;
+}
+
+/**
+* Constructor
+*/
+VtolAttitudeControl::VtolAttitudeControl() :
+	_task_should_exit(false),
+	_control_task(-1),
+
+	//init subscription handlers
+	_v_att_sub(-1),
+	_v_att_sp_sub(-1),
+	_mc_virtual_v_rates_sp_sub(-1),
+	_fw_virtual_v_rates_sp_sub(-1),
+	_v_control_mode_sub(-1),
+	_params_sub(-1),
+	_manual_control_sp_sub(-1),
+	_armed_sub(-1),
+	_local_pos_sub(-1),
+	_airspeed_sub(-1),
+	_battery_status_sub(-1),
+
+	//init publication handlers
+	_actuators_0_pub(-1),
+	_actuators_1_pub(-1),
+	_vtol_vehicle_status_pub(-1),
+	_v_rates_sp_pub(-1),
+
+	_loop_perf(perf_alloc(PC_ELAPSED, "vtol_att_control")),
+	_nonfinite_input_perf(perf_alloc(PC_COUNT, "vtol att control nonfinite input"))
+{
+
+	flag_idle_mc = true;
+	_airspeed_tot = 0.0f;
+
+	memset(& _vtol_vehicle_status, 0, sizeof(_vtol_vehicle_status));
+	_vtol_vehicle_status.vtol_in_rw_mode = true;	/* start vtol in rotary wing mode*/
+	memset(&_v_att, 0, sizeof(_v_att));
+	memset(&_v_att_sp, 0, sizeof(_v_att_sp));
+	memset(&_v_rates_sp, 0, sizeof(_v_rates_sp));
+	memset(&_mc_virtual_v_rates_sp, 0, sizeof(_mc_virtual_v_rates_sp));
+	memset(&_fw_virtual_v_rates_sp, 0, sizeof(_fw_virtual_v_rates_sp));
+	memset(&_manual_control_sp, 0, sizeof(_manual_control_sp));
+	memset(&_v_control_mode, 0, sizeof(_v_control_mode));
+	memset(&_vtol_vehicle_status, 0, sizeof(_vtol_vehicle_status));
+	memset(&_actuators_out_0, 0, sizeof(_actuators_out_0));
+	memset(&_actuators_out_1, 0, sizeof(_actuators_out_1));
+	memset(&_actuators_mc_in, 0, sizeof(_actuators_mc_in));
+	memset(&_actuators_fw_in, 0, sizeof(_actuators_fw_in));
+	memset(&_armed, 0, sizeof(_armed));
+	memset(&_local_pos,0,sizeof(_local_pos));
+	memset(&_airspeed,0,sizeof(_airspeed));
+	memset(&_batt_status,0,sizeof(_batt_status));
+
+	_params.idle_pwm_mc = PWM_LOWEST_MIN;
+	_params.vtol_motor_count = 0;
+	_params.vtol_fw_permanent_stab = 0;
+
+	_params_handles.idle_pwm_mc = param_find("VT_IDLE_PWM_MC");
+	_params_handles.vtol_motor_count = param_find("VT_MOT_COUNT");
+	_params_handles.vtol_fw_permanent_stab = param_find("VT_FW_PERM_STAB");
+	_params_handles.mc_airspeed_min = param_find("VT_MC_ARSPD_MIN");
+	_params_handles.mc_airspeed_max = param_find("VT_MC_ARSPD_MAX");
+	_params_handles.mc_airspeed_trim = param_find("VT_MC_ARSPD_TRIM");
+	_params_handles.fw_pitch_trim = param_find("VT_FW_PITCH_TRIM");
+	_params_handles.power_max = param_find("VT_POWER_MAX");
+	_params_handles.prop_eff = param_find("VT_PROP_EFF");
+	_params_handles.arsp_lp_gain = param_find("VT_ARSP_LP_GAIN");
+
+	/* fetch initial parameter values */
+	parameters_update();
+}
+
+/**
+* Destructor
+*/
+VtolAttitudeControl::~VtolAttitudeControl()
+{
+	if (_control_task != -1) {
+		/* task wakes up every 100ms or so at the longest */
+		_task_should_exit = true;
+
+		/* wait for a second for the task to quit at our request */
+		unsigned i = 0;
+
+		do {
+			/* wait 20ms */
+			usleep(20000);
+
+			/* if we have given up, kill it */
+			if (++i > 50) {
+				task_delete(_control_task);
+				break;
+			}
+		} while (_control_task != -1);
+	}
+
+	VTOL_att_control::g_control = nullptr;
+}
+
+/**
+* Check for changes in vehicle control mode.
+*/
+void VtolAttitudeControl::vehicle_control_mode_poll()
+{
+	bool updated;
+
+	/* Check if vehicle control mode has changed */
+	orb_check(_v_control_mode_sub, &updated);
+
+	if (updated) {
+		orb_copy(ORB_ID(vehicle_control_mode), _v_control_mode_sub, &_v_control_mode);
+	}
+}
+
+/**
+* Check for changes in manual inputs.
+*/
+void VtolAttitudeControl::vehicle_manual_poll()
+{
+	bool updated;
+
+	/* get pilots inputs */
+	orb_check(_manual_control_sp_sub, &updated);
+
+	if (updated) {
+		orb_copy(ORB_ID(manual_control_setpoint), _manual_control_sp_sub, &_manual_control_sp);
+	}
+}
+/**
+* Check for arming status updates.
+*/
+void VtolAttitudeControl::arming_status_poll()
+{
+	/* check if there is a new setpoint */
+	bool updated;
+	orb_check(_armed_sub, &updated);
+
+	if (updated) {
+		orb_copy(ORB_ID(actuator_armed), _armed_sub, &_armed);
+	}
+}
+
+/**
+* Check for inputs from mc attitude controller.
+*/
+void VtolAttitudeControl::actuator_controls_mc_poll()
+{
+	bool updated;
+	orb_check(_actuator_inputs_mc, &updated);
+
+	if (updated) {
+		orb_copy(ORB_ID(actuator_controls_virtual_mc), _actuator_inputs_mc , &_actuators_mc_in);
+	}
+}
+
+/**
+* Check for inputs from fw attitude controller.
+*/
+void VtolAttitudeControl::actuator_controls_fw_poll()
+{
+	bool updated;
+	orb_check(_actuator_inputs_fw, &updated);
+
+	if (updated) {
+		orb_copy(ORB_ID(actuator_controls_virtual_fw), _actuator_inputs_fw , &_actuators_fw_in);
+	}
+}
+
+/**
+* Check for attitude rates setpoint from mc attitude controller
+*/
+void VtolAttitudeControl::vehicle_rates_sp_mc_poll()
+{
+	bool updated;
+	orb_check(_mc_virtual_v_rates_sp_sub, &updated);
+
+	if (updated) {
+		orb_copy(ORB_ID(mc_virtual_rates_setpoint), _mc_virtual_v_rates_sp_sub , &_mc_virtual_v_rates_sp);
+	}
+}
+
+/**
+* Check for attitude rates setpoint from fw attitude controller
+*/
+void VtolAttitudeControl::vehicle_rates_sp_fw_poll()
+{
+	bool updated;
+	orb_check(_fw_virtual_v_rates_sp_sub, &updated);
+
+	if (updated) {
+		orb_copy(ORB_ID(fw_virtual_rates_setpoint), _fw_virtual_v_rates_sp_sub , &_fw_virtual_v_rates_sp);
+	}
+}
+
+/**
+* Check for airspeed updates.
+*/
+void
+VtolAttitudeControl::vehicle_airspeed_poll() {
+	bool updated;
+	orb_check(_airspeed_sub, &updated);
+
+	if (updated) {
+		orb_copy(ORB_ID(airspeed), _airspeed_sub , &_airspeed);
+	}
+}
+
+/**
+* Check for battery updates.
+*/
+void
+VtolAttitudeControl::vehicle_battery_poll() {
+	bool updated;
+	orb_check(_battery_status_sub, &updated);
+
+	if (updated) {
+		orb_copy(ORB_ID(battery_status), _battery_status_sub , &_batt_status);
+	}
+}
+
+/**
+* Check for parameter updates.
+*/
+void
+VtolAttitudeControl::parameters_update_poll()
+{
+	bool updated;
+
+	/* Check if parameters have changed */
+	orb_check(_params_sub, &updated);
+
+	if (updated) {
+		struct parameter_update_s param_update;
+		orb_copy(ORB_ID(parameter_update), _params_sub, &param_update);
+		parameters_update();
+	}
+}
+
+/**
+* Check for sensor updates.
+*/
+void
+VtolAttitudeControl::vehicle_local_pos_poll()
+{
+	bool updated;
+	/* Check if parameters have changed */
+	orb_check(_local_pos_sub, &updated);
+
+	if (updated) {
+		orb_copy(ORB_ID(vehicle_local_position), _local_pos_sub , &_local_pos);
+	}
+
+}
+
+/**
+* Update parameters.
+*/
+int
+VtolAttitudeControl::parameters_update()
+{
+	float v;
+	/* idle pwm for mc mode */
+	param_get(_params_handles.idle_pwm_mc, &_params.idle_pwm_mc);
+
+	/* vtol motor count */
+	param_get(_params_handles.vtol_motor_count, &_params.vtol_motor_count);
+
+	/* vtol fw permanent stabilization */
+	param_get(_params_handles.vtol_fw_permanent_stab, &_params.vtol_fw_permanent_stab);
+
+	/* vtol mc mode min airspeed */
+	param_get(_params_handles.mc_airspeed_min, &v);
+	_params.mc_airspeed_min = v;
+
+	/* vtol mc mode max airspeed */
+	param_get(_params_handles.mc_airspeed_max, &v);
+	_params.mc_airspeed_max = v;
+
+	/* vtol mc mode trim airspeed */
+	param_get(_params_handles.mc_airspeed_trim, &v);
+	_params.mc_airspeed_trim = v;
+
+	/* vtol pitch trim for fw mode */
+	param_get(_params_handles.fw_pitch_trim, &v);
+	_params.fw_pitch_trim = v;
+
+	/* vtol maximum power engine can produce */
+	param_get(_params_handles.power_max, &v);
+	_params.power_max = v;
+
+	/* vtol propeller efficiency factor */
+	param_get(_params_handles.prop_eff, &v);
+	_params.prop_eff = v;
+
+	/* vtol total airspeed estimate low pass gain */
+	param_get(_params_handles.arsp_lp_gain, &v);
+	_params.arsp_lp_gain = v;
+
+	return OK;
+}
+
+/**
+* Prepare message to acutators with data from mc attitude controller.
+*/
+void VtolAttitudeControl::fill_mc_att_control_output()
+{
+	_actuators_out_0.control[0] = _actuators_mc_in.control[0];
+	_actuators_out_0.control[1] = _actuators_mc_in.control[1];
+	_actuators_out_0.control[2] = _actuators_mc_in.control[2];
+	_actuators_out_0.control[3] = _actuators_mc_in.control[3];
+	//set neutral position for elevons
+	_actuators_out_1.control[0] = _actuators_mc_in.control[2];	//roll elevon
+	_actuators_out_1.control[1] = _actuators_mc_in.control[1];;	//pitch elevon
+}
+
+/**
+* Prepare message to acutators with data from fw attitude controller.
+*/
+void VtolAttitudeControl::fill_fw_att_control_output()
+{
+	/*For the first test in fw mode, only use engines for thrust!!!*/
+	_actuators_out_0.control[0] = 0;
+	_actuators_out_0.control[1] = 0;
+	_actuators_out_0.control[2] = 0;
+	_actuators_out_0.control[3] = _actuators_fw_in.control[3];
+	/*controls for the elevons */
+	_actuators_out_1.control[0] = -_actuators_fw_in.control[0];	// roll elevon
+	_actuators_out_1.control[1] = _actuators_fw_in.control[1] + _params.fw_pitch_trim;	// pitch elevon
+	// unused now but still logged
+	_actuators_out_1.control[2] = _actuators_fw_in.control[2];	// yaw
+	_actuators_out_1.control[3] = _actuators_fw_in.control[3];	// throttle
+}
+
+/**
+* Prepare message for mc attitude rates setpoint topic
+*/
+void VtolAttitudeControl::fill_mc_att_rates_sp()
+{
+	_v_rates_sp.roll 	= _mc_virtual_v_rates_sp.roll;
+	_v_rates_sp.pitch 	= _mc_virtual_v_rates_sp.pitch;
+	_v_rates_sp.yaw 	= _mc_virtual_v_rates_sp.yaw;
+	_v_rates_sp.thrust 	= _mc_virtual_v_rates_sp.thrust;
+}
+
+/**
+* Prepare message for fw attitude rates setpoint topic
+*/
+void VtolAttitudeControl::fill_fw_att_rates_sp()
+{
+	_v_rates_sp.roll 	= _fw_virtual_v_rates_sp.roll;
+	_v_rates_sp.pitch 	= _fw_virtual_v_rates_sp.pitch;
+	_v_rates_sp.yaw 	= _fw_virtual_v_rates_sp.yaw;
+	_v_rates_sp.thrust 	= _fw_virtual_v_rates_sp.thrust;
+}
+
+/**
+* Adjust idle speed for fw mode.
+*/
+void VtolAttitudeControl::set_idle_fw()
+{
+	int ret;
+	char *dev = PWM_OUTPUT_DEVICE_PATH;
+	int fd = open(dev, 0);
+
+	if (fd < 0) {err(1, "can't open %s", dev);}
+
+	unsigned pwm_value = PWM_LOWEST_MIN;
+	struct pwm_output_values pwm_values;
+	memset(&pwm_values, 0, sizeof(pwm_values));
+
+	for (unsigned i = 0; i < _params.vtol_motor_count; i++) {
+
+		pwm_values.values[i] = pwm_value;
+		pwm_values.channel_count++;
+	}
+
+	ret = ioctl(fd, PWM_SERVO_SET_MIN_PWM, (long unsigned int)&pwm_values);
+
+	if (ret != OK) {errx(ret, "failed setting min values");}
+
+	close(fd);
+}
+
+/**
+* Adjust idle speed for mc mode.
+*/
+void VtolAttitudeControl::set_idle_mc()
+{
+	int ret;
+	unsigned servo_count;
+	char *dev = PWM_OUTPUT_DEVICE_PATH;
+	int fd = open(dev, 0);
+
+	if (fd < 0) {err(1, "can't open %s", dev);}
+
+	ret = ioctl(fd, PWM_SERVO_GET_COUNT, (unsigned long)&servo_count);
+	unsigned pwm_value = _params.idle_pwm_mc;
+	struct pwm_output_values pwm_values;
+	memset(&pwm_values, 0, sizeof(pwm_values));
+
+	for (unsigned i = 0; i < _params.vtol_motor_count; i++) {
+		pwm_values.values[i] = pwm_value;
+		pwm_values.channel_count++;
+	}
+
+	ret = ioctl(fd, PWM_SERVO_SET_MIN_PWM, (long unsigned int)&pwm_values);
+
+	if (ret != OK) {errx(ret, "failed setting min values");}
+
+	close(fd);
+}
+
+void
+VtolAttitudeControl::scale_mc_output() {
+	// scale around tuning airspeed
+	float airspeed;
+	calc_tot_airspeed();	// estimate air velocity seen by elevons
+	// if airspeed is not updating, we assume the normal average speed
+	if (bool nonfinite = !isfinite(_airspeed.true_airspeed_m_s) ||
+	    hrt_elapsed_time(&_airspeed.timestamp) > 1e6) {
+		airspeed = _params.mc_airspeed_trim;
+		if (nonfinite) {
+			perf_count(_nonfinite_input_perf);
+		}
+	} else {
+		airspeed = _airspeed_tot;
+		airspeed = math::constrain(airspeed,_params.mc_airspeed_min, _params.mc_airspeed_max);
+	}
+
+	_vtol_vehicle_status.airspeed_tot = airspeed;	// save value for logging
+	/*
+	 * For scaling our actuators using anything less than the min (close to stall)
+	 * speed doesn't make any sense - its the strongest reasonable deflection we
+	 * want to do in flight and its the baseline a human pilot would choose.
+	 *
+	 * Forcing the scaling to this value allows reasonable handheld tests.
+	 */
+	float airspeed_scaling = _params.mc_airspeed_trim / ((airspeed < _params.mc_airspeed_min) ? _params.mc_airspeed_min : airspeed);
+	_actuators_mc_in.control[1] = math::constrain(_actuators_mc_in.control[1]*airspeed_scaling*airspeed_scaling,-1.0f,1.0f);
+}
+
+void VtolAttitudeControl::calc_tot_airspeed() {
+	float airspeed = math::max(1.0f, _airspeed.true_airspeed_m_s);	// prevent numerical drama
+	// calculate momentary power of one engine
+	float P = _batt_status.voltage_filtered_v * _batt_status.current_a / _params.vtol_motor_count;
+	P = math::constrain(P,1.0f,_params.power_max);
+	// calculate prop efficiency
+	float power_factor = 1.0f - P*_params.prop_eff/_params.power_max;
+	float eta = (1.0f/(1 + expf(-0.4f * power_factor * airspeed)) - 0.5f)*2.0f;
+	eta = math::constrain(eta,0.001f,1.0f);	// live on the safe side
+	// calculate induced airspeed by propeller
+	float v_ind = (airspeed/eta - airspeed)*2.0f;
+	// calculate total airspeed
+	float airspeed_raw = airspeed + v_ind;
+	// apply low-pass filter
+	_airspeed_tot = _params.arsp_lp_gain * (_airspeed_tot - airspeed_raw) + airspeed_raw;
+}
+
+void
+VtolAttitudeControl::task_main_trampoline(int argc, char *argv[])
+{
+	VTOL_att_control::g_control->task_main();
+}
+
+void VtolAttitudeControl::task_main()
+{
+	warnx("started");
+	fflush(stdout);
+
+	/* do subscriptions */
+	_v_att_sp_sub          = orb_subscribe(ORB_ID(vehicle_attitude_setpoint));
+	_mc_virtual_v_rates_sp_sub = orb_subscribe(ORB_ID(mc_virtual_rates_setpoint));
+	_fw_virtual_v_rates_sp_sub = orb_subscribe(ORB_ID(fw_virtual_rates_setpoint));
+	_v_att_sub             = orb_subscribe(ORB_ID(vehicle_attitude));
+	_v_control_mode_sub    = orb_subscribe(ORB_ID(vehicle_control_mode));
+	_params_sub            = orb_subscribe(ORB_ID(parameter_update));
+	_manual_control_sp_sub = orb_subscribe(ORB_ID(manual_control_setpoint));
+	_armed_sub             = orb_subscribe(ORB_ID(actuator_armed));
+	_local_pos_sub         = orb_subscribe(ORB_ID(vehicle_local_position));
+	_airspeed_sub          = orb_subscribe(ORB_ID(airspeed));
+	_battery_status_sub	   = orb_subscribe(ORB_ID(battery_status));
+
+	_actuator_inputs_mc    = orb_subscribe(ORB_ID(actuator_controls_virtual_mc));
+	_actuator_inputs_fw    = orb_subscribe(ORB_ID(actuator_controls_virtual_fw));
+
+	parameters_update();  // initialize parameter cache
+
+	/* update vtol vehicle status*/
+	_vtol_vehicle_status.fw_permanent_stab = _params.vtol_fw_permanent_stab == 1 ? true : false;
+
+	// make sure we start with idle in mc mode
+	set_idle_mc();
+	flag_idle_mc = true;
+
+	/* wakeup source*/
+	struct pollfd fds[3];	/*input_mc, input_fw, parameters*/
+
+	fds[0].fd     = _actuator_inputs_mc;
+	fds[0].events = POLLIN;
+	fds[1].fd     = _actuator_inputs_fw;
+	fds[1].events = POLLIN;
+	fds[2].fd     = _params_sub;
+	fds[2].events = POLLIN;
+
+	while (!_task_should_exit) {
+		/*Advertise/Publish vtol vehicle status*/
+		if (_vtol_vehicle_status_pub > 0) {
+			orb_publish(ORB_ID(vtol_vehicle_status), _vtol_vehicle_status_pub, &_vtol_vehicle_status);
+
+		} else {
+			_vtol_vehicle_status.timestamp = hrt_absolute_time();
+			_vtol_vehicle_status_pub = orb_advertise(ORB_ID(vtol_vehicle_status), &_vtol_vehicle_status);
+		}
+
+		/* wait for up to 100ms for data */
+		int pret = poll(&fds[0], (sizeof(fds) / sizeof(fds[0])), 100);
+
+
+		/* timed out - periodic check for _task_should_exit */
+		if (pret == 0) {
+			continue;
+		}
+
+		/* this is undesirable but not much we can do - might want to flag unhappy status */
+		if (pret < 0) {
+			warn("poll error %d, %d", pret, errno);
+			/* sleep a bit before next try */
+			usleep(100000);
+			continue;
+		}
+
+		if (fds[2].revents & POLLIN) {	//parameters were updated, read them now
+			/* read from param to clear updated flag */
+			struct parameter_update_s update;
+			orb_copy(ORB_ID(parameter_update), _params_sub, &update);
+
+			/* update parameters from storage */
+			parameters_update();
+		}
+
+		_vtol_vehicle_status.fw_permanent_stab = _params.vtol_fw_permanent_stab == 1 ? true : false;
+
+		vehicle_control_mode_poll();	//Check for changes in vehicle control mode.
+		vehicle_manual_poll();			//Check for changes in manual inputs.
+		arming_status_poll();			//Check for arming status updates.
+		actuator_controls_mc_poll();	//Check for changes in mc_attitude_control output
+		actuator_controls_fw_poll();	//Check for changes in fw_attitude_control output
+		vehicle_rates_sp_mc_poll();
+		vehicle_rates_sp_fw_poll();
+		parameters_update_poll();
+		vehicle_local_pos_poll();			// Check for new sensor values
+		vehicle_airspeed_poll();
+		vehicle_battery_poll();
+
+
+		if (_manual_control_sp.aux1 <= 0.0f) {		/* vehicle is in mc mode */
+			_vtol_vehicle_status.vtol_in_rw_mode = true;
+
+			if (!flag_idle_mc) {	/* we want to adjust idle speed for mc mode */
+				set_idle_mc();
+				flag_idle_mc = true;
+			}
+
+			/* got data from mc_att_controller */
+			if (fds[0].revents & POLLIN) {
+				vehicle_manual_poll();	/* update remote input */
+				orb_copy(ORB_ID(actuator_controls_virtual_mc), _actuator_inputs_mc, &_actuators_mc_in);
+
+				// scale pitch control with total airspeed
+				scale_mc_output();
+
+				fill_mc_att_control_output();
+				fill_mc_att_rates_sp();
+
+				if (_actuators_0_pub > 0) {
+					orb_publish(ORB_ID(actuator_controls_0), _actuators_0_pub, &_actuators_out_0);
+
+				} else {
+					_actuators_0_pub = orb_advertise(ORB_ID(actuator_controls_0), &_actuators_out_0);
+				}
+
+				if (_actuators_1_pub > 0) {
+					orb_publish(ORB_ID(actuator_controls_1), _actuators_1_pub, &_actuators_out_1);
+
+				} else {
+					_actuators_1_pub = orb_advertise(ORB_ID(actuator_controls_1), &_actuators_out_1);
+				}
+			}
+		}
+
+		if (_manual_control_sp.aux1 >= 0.0f) {			/* vehicle is in fw mode */
+			_vtol_vehicle_status.vtol_in_rw_mode = false;
+
+			if (flag_idle_mc) {	/* we want to adjust idle speed for fixed wing mode */
+				set_idle_fw();
+				flag_idle_mc = false;
+			}
+
+			if (fds[1].revents & POLLIN) {		/* got data from fw_att_controller */
+				orb_copy(ORB_ID(actuator_controls_virtual_fw), _actuator_inputs_fw, &_actuators_fw_in);
+				vehicle_manual_poll();	//update remote input
+
+				fill_fw_att_control_output();
+				fill_fw_att_rates_sp();
+
+				if (_actuators_0_pub > 0) {
+					orb_publish(ORB_ID(actuator_controls_0), _actuators_0_pub, &_actuators_out_0);
+
+				} else {
+					_actuators_0_pub = orb_advertise(ORB_ID(actuator_controls_0), &_actuators_out_0);
+				}
+
+				if (_actuators_1_pub > 0) {
+					orb_publish(ORB_ID(actuator_controls_1), _actuators_1_pub, &_actuators_out_1);
+
+				} else {
+					_actuators_1_pub = orb_advertise(ORB_ID(actuator_controls_1), &_actuators_out_1);
+				}
+			}
+		}
+
+		// publish the attitude rates setpoint
+		if(_v_rates_sp_pub > 0) {
+			orb_publish(ORB_ID(vehicle_rates_setpoint),_v_rates_sp_pub,&_v_rates_sp);
+		}
+		else {
+			_v_rates_sp_pub = orb_advertise(ORB_ID(vehicle_rates_setpoint),&_v_rates_sp);
+		}
+	}
+
+	warnx("exit");
+	_control_task = -1;
+	_exit(0);
+}
+
+int
+VtolAttitudeControl::start()
+{
+	ASSERT(_control_task == -1);
+
+	/* start the task */
+	_control_task = task_spawn_cmd("vtol_att_control",
+				       SCHED_DEFAULT,
+				       SCHED_PRIORITY_MAX - 10,
+				       2048,
+				       (main_t)&VtolAttitudeControl::task_main_trampoline,
+				       nullptr);
+
+	if (_control_task < 0) {
+		warn("task start failed");
+		return -errno;
+	}
+
+	return OK;
+}
+
+
+int vtol_att_control_main(int argc, char *argv[])
+{
+	if (argc < 1) {
+		errx(1, "usage: vtol_att_control {start|stop|status}");
+	}
+
+	if (!strcmp(argv[1], "start")) {
+
+		if (VTOL_att_control::g_control != nullptr) {
+			errx(1, "already running");
+		}
+
+		VTOL_att_control::g_control = new VtolAttitudeControl;
+
+		if (VTOL_att_control::g_control == nullptr) {
+			errx(1, "alloc failed");
+		}
+
+		if (OK != VTOL_att_control::g_control->start()) {
+			delete VTOL_att_control::g_control;
+			VTOL_att_control::g_control = nullptr;
+			err(1, "start failed");
+		}
+
+		exit(0);
+	}
+
+	if (!strcmp(argv[1], "stop")) {
+		if (VTOL_att_control::g_control == nullptr) {
+			errx(1, "not running");
+		}
+
+		delete VTOL_att_control::g_control;
+		VTOL_att_control::g_control = nullptr;
+		exit(0);
+	}
+
+	if (!strcmp(argv[1], "status")) {
+		if (VTOL_att_control::g_control) {
+			errx(0, "running");
+
+		} else {
+			errx(1, "not running");
+		}
+	}
+
+	warnx("unrecognized command");
+	return 1;
+}
diff --git a/src/modules/vtol_att_control/vtol_att_control_params.c b/src/modules/vtol_att_control/vtol_att_control_params.c
new file mode 100644
index 000000000..33752b2c4
--- /dev/null
+++ b/src/modules/vtol_att_control/vtol_att_control_params.c
@@ -0,0 +1,144 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2014 PX4 Development Team. All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in
+ *    the documentation and/or other materials provided with the
+ *    distribution.
+ * 3. Neither the name PX4 nor the names of its contributors may be
+ *    used to endorse or promote products derived from this software
+ *    without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ ****************************************************************************/
+
+/**
+ * @file vtol_att_control_params.c
+ * Parameters for vtol attitude controller.
+ *
+ * @author Roman Bapst <bapstr@ethz.ch>
+ */
+
+#include <systemlib/param/param.h>
+
+/**
+ * VTOL number of engines
+ *
+ * @min 1.0
+ * @group VTOL Attitude Control
+ */
+PARAM_DEFINE_INT32(VT_MOT_COUNT,0);
+
+/**
+ * Idle speed of VTOL when in multicopter mode
+ *
+ * @min 900
+ * @group VTOL Attitude Control
+ */
+PARAM_DEFINE_INT32(VT_IDLE_PWM_MC,900);
+
+/**
+ * Minimum airspeed in multicopter mode
+ *
+ * This is the minimum speed of the air flowing over the control surfaces.
+ *
+ * @min 0.0
+ * @group VTOL Attitude Control
+ */
+PARAM_DEFINE_FLOAT(VT_MC_ARSPD_MIN,10.0f);
+
+/**
+ * Maximum airspeed in multicopter mode
+ *
+ * This is the maximum speed of the air flowing over the control surfaces.
+ *
+ * @min 0.0
+ * @group VTOL Attitude Control
+ */
+PARAM_DEFINE_FLOAT(VT_MC_ARSPD_MAX,30.0f);
+
+/**
+ * Trim airspeed when in multicopter mode
+ *
+ * This is the airflow over the control surfaces for which no airspeed scaling is applied in multicopter mode.
+ *
+ * @min 0.0
+ * @group VTOL Attitude Control
+ */
+PARAM_DEFINE_FLOAT(VT_MC_ARSPD_TRIM,10.0f);
+
+/**
+ * Permanent stabilization in fw mode
+ *
+ * If set to one this parameter will cause permanent attitude stabilization in fw mode.
+ * This parameter has been introduced for pure convenience sake.
+ *
+ * @min 0.0
+ * @max 1.0
+ * @group VTOL Attitude Control
+ */
+PARAM_DEFINE_INT32(VT_FW_PERM_STAB,0);
+
+/**
+ * Fixed wing pitch trim
+ *
+ * This parameter allows to adjust the neutral elevon position in fixed wing mode.
+ *
+ * @min -1
+ * @max 1
+ * @group VTOL Attitude Control
+ */
+PARAM_DEFINE_FLOAT(VT_FW_PITCH_TRIM,0.0f);
+
+/**
+ * Motor max power
+ *
+ * Indicates the maximum power the motor is able to produce. Used to calculate
+ * propeller efficiency map.
+ *
+ * @min 1
+ * @group VTOL Attitude Control
+ */
+PARAM_DEFINE_FLOAT(VT_POWER_MAX,120.0f);
+
+/**
+ * Propeller efficiency parameter
+ *
+ * Influences propeller efficiency at different power settings. Should be tuned beforehand.
+ *
+ * @min 0.5
+ * @max 0.9
+ * @group VTOL Attitude Control
+ */
+PARAM_DEFINE_FLOAT(VT_PROP_EFF,0.0f);
+
+/**
+ * Total airspeed estimate low-pass filter gain
+ *
+ * Gain for tuning the low-pass filter for the total airspeed estimate
+ *
+ * @min 0.0
+ * @max 0.99
+ * @group VTOL Attitude Control
+ */
+PARAM_DEFINE_FLOAT(VT_ARSP_LP_GAIN,0.3f);
+