diff --git a/src/examples/attitude_estimator_ekf/AttitudeEKF.m b/src/examples/attitude_estimator_ekf/AttitudeEKF.m
deleted file mode 100644
index cf559919ae291bada1e88304895d97bd9f23dc99..0000000000000000000000000000000000000000
--- a/src/examples/attitude_estimator_ekf/AttitudeEKF.m
+++ /dev/null
@@ -1,298 +0,0 @@
-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 Position 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/examples/attitude_estimator_ekf/CMakeLists.txt b/src/examples/attitude_estimator_ekf/CMakeLists.txt
deleted file mode 100644
index b3869bf290b3dbc1b1cfdc23cb60c8598c1c0d3c..0000000000000000000000000000000000000000
--- a/src/examples/attitude_estimator_ekf/CMakeLists.txt
+++ /dev/null
@@ -1,45 +0,0 @@
-############################################################################
-#
-# Copyright (c) 2015 PX4 Development Team. All rights reserved.
-#
-# Redistribution and use in source and binary forms, with or without
-# modification, are permitted provided that the following conditions
-# are met:
-#
-# 1. Redistributions of source code must retain the above copyright
-# notice, this list of conditions and the following disclaimer.
-# 2. Redistributions in binary form must reproduce the above copyright
-# notice, this list of conditions and the following disclaimer in
-# the documentation and/or other materials provided with the
-# distribution.
-# 3. Neither the name PX4 nor the names of its contributors may be
-# used to endorse or promote products derived from this software
-# without specific prior written permission.
-#
-# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
-# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
-# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
-# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
-# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
-# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
-# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
-# OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
-# AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
-# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
-# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
-# POSSIBILITY OF SUCH DAMAGE.
-#
-############################################################################
-px4_add_module(
- MODULE examples__attitude_estimator_ekf
- MAIN attitude_estimator_ekf
- STACK_MAIN 1200
- COMPILE_FLAGS
- -Wno-float-equal
- SRCS
- attitude_estimator_ekf_main.cpp
- codegen/AttitudeEKF.c
- DEPENDS
- platforms__common
- )
-# vim: set noet ft=cmake fenc=utf-8 ff=unix :
diff --git a/src/examples/attitude_estimator_ekf/attitudeKalmanfilter.prj b/src/examples/attitude_estimator_ekf/attitudeKalmanfilter.prj
deleted file mode 100644
index 9ea52034639c11613849bb57007fd2cf20acda24..0000000000000000000000000000000000000000
--- a/src/examples/attitude_estimator_ekf/attitudeKalmanfilter.prj
+++ /dev/null
@@ -1,502 +0,0 @@
-<?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/examples/attitude_estimator_ekf/attitude_estimator_ekf_main.cpp b/src/examples/attitude_estimator_ekf/attitude_estimator_ekf_main.cpp
deleted file mode 100755
index 1ff585412e19da326ae7e9d5de383c583ebf65ba..0000000000000000000000000000000000000000
--- a/src/examples/attitude_estimator_ekf/attitude_estimator_ekf_main.cpp
+++ /dev/null
@@ -1,639 +0,0 @@
-/****************************************************************************
- *
- * Copyright (c) 2012-2014 PX4 Development Team. All rights reserved.
- *
- * Redistribution and use in source and binary forms, with or without
- * modification, are permitted provided that the following conditions
- * are met:
- *
- * 1. Redistributions of source code must retain the above copyright
- * notice, this list of conditions and the following disclaimer.
- * 2. Redistributions in binary form must reproduce the above copyright
- * notice, this list of conditions and the following disclaimer in
- * the documentation and/or other materials provided with the
- * distribution.
- * 3. Neither the name PX4 nor the names of its contributors may be
- * used to endorse or promote products derived from this software
- * without specific prior written permission.
- *
- * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
- * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
- * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
- * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
- * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
- * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
- * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
- * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
- * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
- * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
- * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
- * POSSIBILITY OF SUCH DAMAGE.
- *
- ****************************************************************************/
-
-/*
- * @file attitude_estimator_ekf_main.cpp
- *
- * Extended Kalman Filter for Attitude Estimation.
- *
- * @author Tobias Naegeli <naegelit@student.ethz.ch>
- * @author Lorenz Meier <lm@inf.ethz.ch>
- * @author Thomas Gubler <thomasgubler@gmail.com>
- */
-
-#include <px4_config.h>
-#include <px4_defines.h>
-#include <px4_posix.h>
-#include <px4_tasks.h>
-#include <unistd.h>
-#include <stdlib.h>
-#include <stdio.h>
-#include <stdbool.h>
-#include <poll.h>
-#include <fcntl.h>
-#include <float.h>
-#include <errno.h>
-#include <limits.h>
-#include <math.h>
-#include <uORB/uORB.h>
-#include <uORB/topics/debug_key_value.h>
-#include <uORB/topics/sensor_combined.h>
-#include <uORB/topics/vehicle_attitude.h>
-#include <uORB/topics/vehicle_control_mode.h>
-#include <uORB/topics/vehicle_gps_position.h>
-#include <uORB/topics/vehicle_global_position.h>
-#include <uORB/topics/parameter_update.h>
-#include <uORB/topics/att_pos_mocap.h>
-#include <drivers/drv_hrt.h>
-
-#include <lib/mathlib/mathlib.h>
-#include <lib/geo/geo.h>
-
-#include <systemlib/perf_counter.h>
-#include <systemlib/err.h>
-
-#ifdef __cplusplus
-extern "C" {
-#endif
-#include "codegen/AttitudeEKF.h"
-#include "attitude_estimator_ekf_params.h"
-#ifdef __cplusplus
-}
-#endif
-
-extern "C" __EXPORT int attitude_estimator_ekf_main(int argc, char *argv[]);
-
-static bool thread_should_exit = false; /**< Deamon exit flag */
-static bool thread_running = false; /**< Deamon status flag */
-static int attitude_estimator_ekf_task; /**< Handle of deamon task / thread */
-
-/**
- * Mainloop of attitude_estimator_ekf.
- */
-int attitude_estimator_ekf_thread_main(int argc, char *argv[]);
-
-/**
- * Print the correct usage.
- */
-static void usage(const char *reason);
-
-static void
-usage(const char *reason)
-{
- if (reason) {
- fprintf(stderr, "%s\n", reason);
- }
-
- fprintf(stderr, "usage: attitude_estimator_ekf {start|stop|status} [-p <additional params>]\n\n");
-}
-
-int parameters_init(struct attitude_estimator_ekf_param_handles *h)
-{
- /* PID parameters */
- h->q0 = param_find("EKF_ATT_V3_Q0");
- 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->r0 = param_find("EKF_ATT_V4_R0");
- h->r1 = param_find("EKF_ATT_V4_R1");
- h->r2 = param_find("EKF_ATT_V4_R2");
-
- 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;
-}
-
-int parameters_update(const struct attitude_estimator_ekf_param_handles *h, struct attitude_estimator_ekf_params *p)
-{
- param_get(h->q0, &(p->q[0]));
- param_get(h->q1, &(p->q[1]));
- param_get(h->q2, &(p->q[2]));
- param_get(h->q3, &(p->q[3]));
-
- param_get(h->r0, &(p->r[0]));
- param_get(h->r1, &(p->r[1]));
- param_get(h->r2, &(p->r[2]));
-
- 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;
-}
-
-/**
- * The attitude_estimator_ekf app only briefly exists to start
- * the background job. The stack size assigned in the
- * Makefile does only apply to this management task.
- *
- * The actual stack size should be set in the call
- * to px4_task_spawn_cmd().
- */
-int attitude_estimator_ekf_main(int argc, char *argv[])
-{
- if (argc < 2) {
- usage("missing command");
- return 1;
- }
-
- if (!strcmp(argv[1], "start")) {
-
- if (thread_running) {
- warnx("already running\n");
- /* this is not an error */
- return 0;
- }
-
- thread_should_exit = false;
- attitude_estimator_ekf_task = px4_task_spawn_cmd("attitude_estimator_ekf",
- SCHED_DEFAULT,
- SCHED_PRIORITY_MAX - 5,
- 7000,
- attitude_estimator_ekf_thread_main,
- (argv) ? (char * const *)&argv[2] : (char * const *)nullptr);
- return 0;
- }
-
- if (!strcmp(argv[1], "stop")) {
- thread_should_exit = true;
- return 0;
- }
-
- if (!strcmp(argv[1], "status")) {
- if (thread_running) {
- warnx("running");
- return 0;
-
- } else {
- warnx("not started");
- return 1;
- }
-
- return 0;
- }
-
- usage("unrecognized command");
- return 1;
-}
-
-/*
- * [Rot_matrix,x_aposteriori,P_aposteriori] = attitudeKalmanfilter(dt,z_k,x_aposteriori_k,P_aposteriori_k,knownConst)
- */
-
-/*
- * EKF Attitude Estimator main function.
- *
- * Estimates the attitude recursively once started.
- *
- * @param argc number of commandline arguments (plus command name)
- * @param argv strings containing the arguments
- */
-int attitude_estimator_ekf_thread_main(int argc, char *argv[])
-{
-
- float dt = 0.005f;
-/* state vector x has the following entries [ax,ay,az||mx,my,mz||wox,woy,woz||wx,wy,wz]' */
- float z_k[9] = {0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 9.81f, 0.2f, -0.2f, 0.2f}; /**< Measurement vector */
- float x_aposteriori_k[12]; /**< states */
- float P_aposteriori_k[144] = {100.f, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0,
- 0, 100.f, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0,
- 0, 0, 100.f, 0, 0, 0, 0, 0, 0, 0, 0, 0,
- 0, 0, 0, 100.f, 0, 0, 0, 0, 0, 0, 0, 0,
- 0, 0, 0, 0, 100.f, 0, 0, 0, 0, 0, 0, 0,
- 0, 0, 0, 0, 0, 100.f, 0, 0, 0, 0, 0, 0,
- 0, 0, 0, 0, 0, 0, 100.f, 0, 0, 0, 0, 0,
- 0, 0, 0, 0, 0, 0, 0, 100.f, 0, 0, 0, 0,
- 0, 0, 0, 0, 0, 0, 0, 0, 100.f, 0, 0, 0,
- 0, 0, 0, 0, 0, 0, 0, 0, 0.0f, 100.0f, 0, 0,
- 0, 0, 0, 0, 0, 0, 0, 0, 0.0f, 0, 100.0f, 0,
- 0, 0, 0, 0, 0, 0, 0, 0, 0.0f, 0, 0, 100.0f,
- }; /**< init: diagonal matrix with big values */
-
- float x_aposteriori[12];
- float P_aposteriori[144];
-
- /* output euler angles */
- float euler[3] = {0.0f, 0.0f, 0.0f};
-
- float Rot_matrix[9] = {1.f, 0, 0,
- 0, 1.f, 0,
- 0, 0, 1.f
- }; /**< init: identity matrix */
-
- float debugOutput[4] = { 0.0f };
-
- /* Initialize filter */
- AttitudeEKF_initialize();
-
- struct sensor_combined_s raw;
- memset(&raw, 0, sizeof(raw));
- struct vehicle_gps_position_s gps;
- memset(&gps, 0, sizeof(gps));
- gps.eph = 100000;
- gps.epv = 100000;
- struct vehicle_global_position_s global_pos;
- memset(&global_pos, 0, sizeof(global_pos));
- struct vehicle_attitude_s att;
- memset(&att, 0, sizeof(att));
- struct vehicle_control_mode_s control_mode;
- memset(&control_mode, 0, sizeof(control_mode));
-
- uint64_t last_data = 0;
- uint64_t last_measurement = 0;
- uint64_t last_vel_t = 0;
-
- /* current velocity */
- math::Vector<3> vel;
- vel.zero();
- /* previous velocity */
- math::Vector<3> vel_prev;
- vel_prev.zero();
- /* actual acceleration (by GPS velocity) in body frame */
- math::Vector<3> acc;
- acc.zero();
- /* rotation matrix */
- math::Matrix<3, 3> R;
- R.identity();
-
- /* subscribe to raw data */
- int sub_raw = orb_subscribe(ORB_ID(sensor_combined));
-
- /* subscribe to GPS */
- int sub_gps = orb_subscribe(ORB_ID(vehicle_gps_position));
-
- /* subscribe to GPS */
- int sub_global_pos = orb_subscribe(ORB_ID(vehicle_global_position));
-
- /* subscribe to param changes */
- int sub_params = orb_subscribe(ORB_ID(parameter_update));
-
- /* subscribe to control mode */
- int sub_control_mode = orb_subscribe(ORB_ID(vehicle_control_mode));
-
- /* subscribe to vision estimate */
- int vision_sub = orb_subscribe(ORB_ID(vehicle_vision_attitude));
-
- /* subscribe to mocap data */
- int mocap_sub = orb_subscribe(ORB_ID(att_pos_mocap));
-
- /* advertise attitude */
- orb_advert_t pub_att = orb_advertise(ORB_ID(vehicle_attitude), &att);
-
- int loopcounter = 0;
-
- thread_running = true;
-
- /* keep track of sensor updates */
- uint64_t sensor_last_timestamp[3] = {0, 0, 0};
-
- struct attitude_estimator_ekf_params ekf_params;
- memset(&ekf_params, 0, sizeof(ekf_params));
-
- struct attitude_estimator_ekf_param_handles ekf_param_handles = { 0 };
-
- /* initialize parameter handles */
- parameters_init(&ekf_param_handles);
-
- bool initialized = false;
-
- float gyro_offsets[3] = { 0.0f, 0.0f, 0.0f };
-
- /* magnetic declination, in radians */
- float mag_decl = 0.0f;
-
- /* rotation matrix for magnetic declination */
- math::Matrix<3, 3> R_decl;
- R_decl.identity();
-
- struct vehicle_attitude_s vision {};
- struct att_pos_mocap_s mocap {};
-
- /* register the perf counter */
- perf_counter_t ekf_loop_perf = perf_alloc(PC_ELAPSED, "attitude_estimator_ekf");
-
- /* Main loop*/
- while (!thread_should_exit) {
-
- px4_pollfd_struct_t fds[2];
- fds[0].fd = sub_raw;
- fds[0].events = POLLIN;
- fds[1].fd = sub_params;
- fds[1].events = POLLIN;
- int ret = px4_poll(fds, 2, 1000);
-
- if (ret < 0) {
- /* XXX this is seriously bad - should be an emergency */
- } else if (ret == 0) {
- /* check if we're in HIL - not getting sensor data is fine then */
- orb_copy(ORB_ID(vehicle_control_mode), sub_control_mode, &control_mode);
-
- if (!control_mode.flag_system_hil_enabled) {
- warnx("WARNING: Not getting sensor data - sensor app running?");
- }
-
- } else {
-
- /* only update parameters if they changed */
- if (fds[1].revents & POLLIN) {
- /* read from param to clear updated flag */
- struct parameter_update_s update;
- orb_copy(ORB_ID(parameter_update), sub_params, &update);
-
- /* update parameters */
- parameters_update(&ekf_param_handles, &ekf_params);
- }
-
- /* only run filter if sensor values changed */
- if (fds[0].revents & POLLIN) {
-
- /* get latest measurements */
- orb_copy(ORB_ID(sensor_combined), sub_raw, &raw);
-
- bool gps_updated;
- orb_check(sub_gps, &gps_updated);
- if (gps_updated) {
- orb_copy(ORB_ID(vehicle_gps_position), sub_gps, &gps);
-
- if (gps.eph < 20.0f && hrt_elapsed_time(&gps.timestamp) < 1000000) {
- mag_decl = math::radians(get_mag_declination(gps.lat / 1e7f, gps.lon / 1e7f));
-
- /* update mag declination rotation matrix */
- R_decl.from_euler(0.0f, 0.0f, mag_decl);
- }
- }
-
- bool global_pos_updated;
- orb_check(sub_global_pos, &global_pos_updated);
- if (global_pos_updated) {
- orb_copy(ORB_ID(vehicle_global_position), sub_global_pos, &global_pos);
- }
-
- if (!initialized) {
- // XXX disabling init for now
- initialized = true;
-
- // gyro_offsets[0] += raw.gyro_rad[0];
- // gyro_offsets[1] += raw.gyro_rad[1];
- // gyro_offsets[2] += raw.gyro_rad[2];
- // offset_count++;
-
- // if (hrt_absolute_time() - start_time > 3000000LL) {
- // initialized = true;
- // gyro_offsets[0] /= offset_count;
- // gyro_offsets[1] /= offset_count;
- // gyro_offsets[2] /= offset_count;
- // }
-
- } else {
-
- perf_begin(ekf_loop_perf);
-
- /* Calculate data time difference in seconds */
- dt = (raw.timestamp - last_measurement) / 1000000.0f;
- last_measurement = raw.timestamp;
- uint8_t update_vect[3] = {0, 0, 0};
-
- /* Fill in gyro measurements */
- if (sensor_last_timestamp[0] != raw.timestamp) {
- update_vect[0] = 1;
- // sensor_update_hz[0] = 1e6f / (raw.timestamp - sensor_last_timestamp[0]);
- sensor_last_timestamp[0] = raw.timestamp;
- }
-
- z_k[0] = raw.gyro_rad[0] - gyro_offsets[0];
- z_k[1] = raw.gyro_rad[1] - gyro_offsets[1];
- z_k[2] = raw.gyro_rad[2] - gyro_offsets[2];
-
- /* update accelerometer measurements */
- if (sensor_last_timestamp[1] != raw.timestamp + raw.accelerometer_timestamp_relative) {
- update_vect[1] = 1;
- // sensor_update_hz[1] = 1e6f / (raw.timestamp - sensor_last_timestamp[1]);
- sensor_last_timestamp[1] = raw.timestamp + raw.accelerometer_timestamp_relative;
- }
-
- hrt_abstime vel_t = 0;
- bool vel_valid = false;
- if (gps.eph < 5.0f && global_pos.timestamp != 0 && hrt_absolute_time() < global_pos.timestamp + 20000) {
- vel_valid = true;
- if (global_pos_updated) {
- vel_t = global_pos.timestamp;
- vel(0) = global_pos.vel_n;
- vel(1) = global_pos.vel_e;
- vel(2) = global_pos.vel_d;
- }
- }
-
- if (vel_valid) {
- /* velocity is valid */
- if (vel_t != 0) {
- /* velocity updated */
- if (last_vel_t != 0 && vel_t != last_vel_t) {
- float vel_dt = (vel_t - last_vel_t) / 1000000.0f;
- /* calculate acceleration in body frame */
- acc = R.transposed() * ((vel - vel_prev) / vel_dt);
- }
- last_vel_t = vel_t;
- vel_prev = vel;
- }
-
- } else {
- /* velocity is valid, reset acceleration */
- acc.zero();
- vel_prev.zero();
- last_vel_t = 0;
- }
-
- z_k[3] = raw.accelerometer_m_s2[0] - acc(0);
- z_k[4] = raw.accelerometer_m_s2[1] - acc(1);
- z_k[5] = raw.accelerometer_m_s2[2] - acc(2);
-
- /* update magnetometer measurements */
- if (sensor_last_timestamp[2] != raw.timestamp + raw.magnetometer_timestamp_relative &&
- /* check that the mag vector is > 0 */
- fabsf(sqrtf(raw.magnetometer_ga[0] * raw.magnetometer_ga[0] +
- raw.magnetometer_ga[1] * raw.magnetometer_ga[1] +
- raw.magnetometer_ga[2] * raw.magnetometer_ga[2])) > 0.1f) {
- update_vect[2] = 1;
- // sensor_update_hz[2] = 1e6f / (raw.timestamp - sensor_last_timestamp[2]);
- sensor_last_timestamp[2] = raw.timestamp + raw.magnetometer_timestamp_relative;
- }
-
- bool vision_updated = false;
- orb_check(vision_sub, &vision_updated);
-
- bool mocap_updated = false;
- orb_check(mocap_sub, &mocap_updated);
-
- if (vision_updated) {
- orb_copy(ORB_ID(vehicle_vision_attitude), vision_sub, &vision);
- }
-
- if (mocap_updated) {
- orb_copy(ORB_ID(att_pos_mocap), mocap_sub, &mocap);
- }
-
- if (mocap.timestamp > 0 && (hrt_elapsed_time(&mocap.timestamp) < 500000)) {
-
- math::Quaternion q(mocap.q);
- math::Matrix<3, 3> Rmoc = q.to_dcm();
-
- math::Vector<3> v(1.0f, 0.0f, 0.4f);
-
- math::Vector<3> vn = Rmoc.transposed() * v; //Rmoc is Rwr (robot respect to world) while v is respect to world. Hence Rmoc must be transposed having (Rwr)' * Vw
- // Rrw * Vw = vn. This way we have consistency
- z_k[6] = vn(0);
- z_k[7] = vn(1);
- z_k[8] = vn(2);
- } else if (vision.timestamp > 0 && (hrt_elapsed_time(&vision.timestamp) < 500000)) {
-
- math::Quaternion q(vision.q);
- math::Matrix<3, 3> Rvis = q.to_dcm();
-
- math::Vector<3> v(1.0f, 0.0f, 0.4f);
-
- math::Vector<3> vn = Rvis.transposed() * v; //Rvis is Rwr (robot respect to world) while v is respect to world. Hence Rvis must be transposed having (Rwr)' * Vw
- // Rrw * Vw = vn. This way we have consistency
- z_k[6] = vn(0);
- z_k[7] = vn(1);
- z_k[8] = vn(2);
- } else {
- z_k[6] = raw.magnetometer_ga[0];
- z_k[7] = raw.magnetometer_ga[1];
- z_k[8] = raw.magnetometer_ga[2];
- }
-
- static bool const_initialized = false;
-
- /* initialize with good values once we have a reasonable dt estimate */
- if (!const_initialized && dt < 0.05f && dt > 0.001f) {
- dt = 0.005f;
- parameters_update(&ekf_param_handles, &ekf_params);
-
- /* update mag declination rotation matrix */
- if (gps.eph < 20.0f && hrt_elapsed_time(&gps.timestamp) < 1000000) {
- mag_decl = math::radians(get_mag_declination(gps.lat / 1e7f, gps.lon / 1e7f));
-
- }
-
- /* update mag declination rotation matrix */
- R_decl.from_euler(0.0f, 0.0f, mag_decl);
-
- x_aposteriori_k[0] = z_k[0];
- x_aposteriori_k[1] = z_k[1];
- x_aposteriori_k[2] = z_k[2];
- x_aposteriori_k[3] = 0.0f;
- x_aposteriori_k[4] = 0.0f;
- x_aposteriori_k[5] = 0.0f;
- x_aposteriori_k[6] = z_k[3];
- x_aposteriori_k[7] = z_k[4];
- x_aposteriori_k[8] = z_k[5];
- x_aposteriori_k[9] = z_k[6];
- x_aposteriori_k[10] = z_k[7];
- x_aposteriori_k[11] = z_k[8];
-
- const_initialized = true;
- }
-
- /* do not execute the filter if not initialized */
- if (!const_initialized) {
- continue;
- }
-
- /* 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 (PX4_ISFINITE(euler[0]) && PX4_ISFINITE(euler[1]) && PX4_ISFINITE(euler[2])) {
- memcpy(P_aposteriori_k, P_aposteriori, sizeof(P_aposteriori_k));
- memcpy(x_aposteriori_k, x_aposteriori, sizeof(x_aposteriori_k));
-
- } else {
- /* due to inputs or numerical failure the output is invalid, skip it */
- continue;
- }
-
- if (last_data > 0 && raw.timestamp - last_data > 30000) {
- warnx("sensor data missed! (%llu)\n", static_cast<unsigned long long>(raw.timestamp - last_data));
- }
-
- last_data = raw.timestamp;
-
- /* send out */
- att.timestamp = raw.timestamp;
-
-
- att.rollspeed = x_aposteriori[0];
- att.pitchspeed = x_aposteriori[1];
- att.yawspeed = x_aposteriori[2];
-
- /* magnetic declination */
- matrix::Dcmf Ro(&Rot_matrix[0]);
- matrix::Dcmf R_declination(&R_decl.data[0][0]);
- matrix::Quatf q = matrix::Quatf(R_declination * Ro);
-
- memcpy(&att.q[0],&q._data[0],sizeof(att.q));
-
- if (PX4_ISFINITE(att.q[0]) && PX4_ISFINITE(att.q[1])
- && PX4_ISFINITE(att.q[2]) && PX4_ISFINITE(att.q[3])) {
- // Broadcast
- orb_publish(ORB_ID(vehicle_attitude), pub_att, &att);
-
- } else {
- PX4_ERR("ERR: NaN estimate!");
- }
-
- perf_end(ekf_loop_perf);
- }
- }
- }
-
- loopcounter++;
- }
-
- thread_running = false;
-
- return 0;
-}
diff --git a/src/examples/attitude_estimator_ekf/attitude_estimator_ekf_params.c b/src/examples/attitude_estimator_ekf/attitude_estimator_ekf_params.c
deleted file mode 100755
index c10f871022dc72c16b00b1ab1e36249f7015b7fd..0000000000000000000000000000000000000000
--- a/src/examples/attitude_estimator_ekf/attitude_estimator_ekf_params.c
+++ /dev/null
@@ -1,130 +0,0 @@
-/****************************************************************************
- *
- * Copyright (C) 2012 PX4 Development Team. All rights reserved.
- * Author: Tobias Naegeli <naegelit@student.ethz.ch>
- * Lorenz Meier <lm@inf.ethz.ch>
- *
- * Redistribution and use in source and binary forms, with or without
- * modification, are permitted provided that the following conditions
- * are met:
- *
- * 1. Redistributions of source code must retain the above copyright
- * notice, this list of conditions and the following disclaimer.
- * 2. Redistributions in binary form must reproduce the above copyright
- * notice, this list of conditions and the following disclaimer in
- * the documentation and/or other materials provided with the
- * distribution.
- * 3. Neither the name PX4 nor the names of its contributors may be
- * used to endorse or promote products derived from this software
- * without specific prior written permission.
- *
- * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
- * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
- * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
- * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
- * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
- * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
- * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
- * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
- * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
- * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
- * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
- * POSSIBILITY OF SUCH DAMAGE.
- *
- ****************************************************************************/
-
-/*
- * @file attitude_estimator_ekf_params.c
- *
- * Parameters for EKF filter
- */
-
-#include "attitude_estimator_ekf_params.h"
-#include <math.h>
-#include <px4_defines.h>
-
-/* Extended Kalman Filter covariances */
-
-
-/**
- * Body angular rate process noise
- *
- * @group Attitude EKF estimator
- */
-PARAM_DEFINE_FLOAT(EKF_ATT_V3_Q0, 1e-4f);
-
-/**
- * Body angular acceleration process noise
- *
- * @group Attitude EKF estimator
- */
-PARAM_DEFINE_FLOAT(EKF_ATT_V3_Q1, 0.08f);
-
-/**
- * Acceleration process noise
- *
- * @group Attitude EKF estimator
- */
-PARAM_DEFINE_FLOAT(EKF_ATT_V3_Q2, 0.009f);
-
-/**
- * Magnet field vector process noise
- *
- * @group Attitude EKF estimator
- */
-PARAM_DEFINE_FLOAT(EKF_ATT_V3_Q3, 0.005f);
-
-/**
- * Gyro measurement noise
- *
- * @group Attitude EKF estimator
- */
-PARAM_DEFINE_FLOAT(EKF_ATT_V4_R0, 0.0008f);
-
-/**
- * Accel measurement noise
- *
- * @group Attitude EKF estimator
- */
-PARAM_DEFINE_FLOAT(EKF_ATT_V4_R1, 10000.0f);
-
-/**
- * Mag measurement noise
- *
- * @group Attitude EKF estimator
- */
-PARAM_DEFINE_FLOAT(EKF_ATT_V4_R2, 100.0f);
-
-/**
- * Moment of inertia matrix diagonal entry (1, 1)
- *
- * @group Attitude EKF estimator
- * @unit kg*m^2
- */
-PARAM_DEFINE_FLOAT(ATT_J11, 0.0018);
-
-/**
- * Moment of inertia matrix diagonal entry (2, 2)
- *
- * @group Attitude EKF estimator
- * @unit kg*m^2
- */
-PARAM_DEFINE_FLOAT(ATT_J22, 0.0018);
-
-/**
- * Moment of inertia matrix diagonal entry (3, 3)
- *
- * @group Attitude EKF estimator
- * @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 estimator
- * @boolean
- */
-PARAM_DEFINE_INT32(ATT_J_EN, 0);
diff --git a/src/examples/attitude_estimator_ekf/attitude_estimator_ekf_params.h b/src/examples/attitude_estimator_ekf/attitude_estimator_ekf_params.h
deleted file mode 100755
index 5d3b6b244057fcae0d795a03ee59d9d5498cfd4b..0000000000000000000000000000000000000000
--- a/src/examples/attitude_estimator_ekf/attitude_estimator_ekf_params.h
+++ /dev/null
@@ -1,75 +0,0 @@
-/****************************************************************************
- *
- * Copyright (C) 2012 PX4 Development Team. All rights reserved.
- * Author: Tobias Naegeli <naegelit@student.ethz.ch>
- * Lorenz Meier <lm@inf.ethz.ch>
- *
- * Redistribution and use in source and binary forms, with or without
- * modification, are permitted provided that the following conditions
- * are met:
- *
- * 1. Redistributions of source code must retain the above copyright
- * notice, this list of conditions and the following disclaimer.
- * 2. Redistributions in binary form must reproduce the above copyright
- * notice, this list of conditions and the following disclaimer in
- * the documentation and/or other materials provided with the
- * distribution.
- * 3. Neither the name PX4 nor the names of its contributors may be
- * used to endorse or promote products derived from this software
- * without specific prior written permission.
- *
- * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
- * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
- * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
- * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
- * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
- * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
- * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
- * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
- * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
- * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
- * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
- * POSSIBILITY OF SUCH DAMAGE.
- *
- ****************************************************************************/
-
-/*
- * @file attitude_estimator_ekf_params.h
- *
- * Parameters for EKF filter
- */
-
-#include <systemlib/param/param.h>
-
-struct attitude_estimator_ekf_params {
- 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;
- float mag_decl;
- int acc_comp;
-};
-
-struct attitude_estimator_ekf_param_handles {
- 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;
-};
-
-/**
- * Initialize all parameter handles and values
- *
- */
-int parameters_init(struct attitude_estimator_ekf_param_handles *h);
-
-/**
- * Update all parameters
- *
- */
-int parameters_update(const struct attitude_estimator_ekf_param_handles *h, struct attitude_estimator_ekf_params *p);
diff --git a/src/examples/attitude_estimator_ekf/codegen/AttitudeEKF.c b/src/examples/attitude_estimator_ekf/codegen/AttitudeEKF.c
deleted file mode 100644
index 25bdde5cf158e4bfc98f08bbe8c9a7622702a043..0000000000000000000000000000000000000000
--- a/src/examples/attitude_estimator_ekf/codegen/AttitudeEKF.c
+++ /dev/null
@@ -1,1456 +0,0 @@
-/*
- * 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 Position 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/examples/attitude_estimator_ekf/codegen/AttitudeEKF.h b/src/examples/attitude_estimator_ekf/codegen/AttitudeEKF.h
deleted file mode 100644
index 7094da1da55f2991a7d33706b019b20d6dca6755..0000000000000000000000000000000000000000
--- a/src/examples/attitude_estimator_ekf/codegen/AttitudeEKF.h
+++ /dev/null
@@ -1,26 +0,0 @@
-/*
- * 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/examples/attitude_estimator_ekf/codegen/AttitudeEKF_types.h b/src/examples/attitude_estimator_ekf/codegen/AttitudeEKF_types.h
deleted file mode 100644
index 3f7522ffa0ed26e1dd3a45f591f16a5296b99535..0000000000000000000000000000000000000000
--- a/src/examples/attitude_estimator_ekf/codegen/AttitudeEKF_types.h
+++ /dev/null
@@ -1,17 +0,0 @@
-/*
- * 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/examples/attitude_estimator_ekf/codegen/rtwtypes.h b/src/examples/attitude_estimator_ekf/codegen/rtwtypes.h
deleted file mode 100644
index b5a02a7a6d8c0703817f23db160c21e29704749b..0000000000000000000000000000000000000000
--- a/src/examples/attitude_estimator_ekf/codegen/rtwtypes.h
+++ /dev/null
@@ -1,160 +0,0 @@
-/*
- * 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) */