Merge branch 'ROS_shared_lib_base_class' into dev_ros_rossharedlib

36 files changed, 2606 insertions, 2676 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 8d537d676..9784ea1a1 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>
@@ -74,8 +75,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,7 +132,7 @@ 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,
+					      7200,
 					      attitude_estimator_ekf_thread_main,
 					      (argv) ? (const char **)&argv[2] : (const char **)NULL);
 		exit(0);
@@ -206,10 +206,12 @@ const unsigned int loop_interval_alarm = 6500;	// loop interval in microseconds
 			      0,  0,  1.f
 			     };		/**< init: identity matrix */
 
+	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();
@@ -273,9 +275,9 @@ const unsigned int loop_interval_alarm = 6500;	// loop interval in microseconds
 	/* keep track of sensor updates */
 	uint64_t sensor_last_timestamp[3] = {0, 0, 0};
 
-	struct attitude_estimator_ekf_params ekf_params;
+	struct attitude_estimator_ekf_params ekf_params = { 0 };
 
-	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);
@@ -504,8 +506,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..749b0a91c 100644
--- a/src/modules/attitude_estimator_ekf/module.mk
+++ b/src/modules/attitude_estimator_ekf/module.mk
@@ -39,16 +39,6 @@ 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
diff --git a/src/modules/uORB/topics/vehicle_gps_position.h b/src/modules/uORB/topics/vehicle_gps_position.h
index 31e616f4f..7888a50f4 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_gps_usec;				/**< Timestamp (microseconds in GPS format), 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/uavcan/uavcan_main.cpp b/src/modules/uavcan/uavcan_main.cpp
index 8147a8b89..60901e0c7 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>
 
@@ -612,10 +614,32 @@ UavcanNode::print_info()
 
 	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