diff options
author | Thomas Gubler <thomasgubler@gmail.com> | 2014-12-05 09:43:40 +0100 |
---|---|---|
committer | Thomas Gubler <thomasgubler@gmail.com> | 2014-12-05 09:43:40 +0100 |
commit | 041aea7532b9bf6725c73d47686ba808a48fcc56 (patch) | |
tree | 709d8f563c5c98ed47b60c2dd0b0c979ad48ff0a /src/modules/attitude_estimator_ekf | |
parent | c64c184948bed07f354904c0549b93225c85ad58 (diff) | |
parent | 92135d155d36096fad5c215f119cca166f6ac611 (diff) | |
download | px4-firmware-041aea7532b9bf6725c73d47686ba808a48fcc56.tar.gz px4-firmware-041aea7532b9bf6725c73d47686ba808a48fcc56.tar.bz2 px4-firmware-041aea7532b9bf6725c73d47686ba808a48fcc56.zip |
Merge branch 'ROS_shared_lib_base_class' into dev_ros_rossharedlib
Diffstat (limited to 'src/modules/attitude_estimator_ekf')
34 files changed, 2581 insertions, 2675 deletions
diff --git a/src/modules/attitude_estimator_ekf/AttitudeEKF.m b/src/modules/attitude_estimator_ekf/AttitudeEKF.m new file mode 100644 index 000000000..fea1a773e --- /dev/null +++ b/src/modules/attitude_estimator_ekf/AttitudeEKF.m @@ -0,0 +1,298 @@ +function [xa_apo,Pa_apo,Rot_matrix,eulerAngles,debugOutput]... + = AttitudeEKF(approx_prediction,use_inertia_matrix,zFlag,dt,z,q_rotSpeed,q_rotAcc,q_acc,q_mag,r_gyro,r_accel,r_mag,J) + + +%LQG Postion Estimator and Controller +% Observer: +% x[n|n] = x[n|n-1] + M(y[n] - Cx[n|n-1] - Du[n]) +% x[n+1|n] = Ax[n|n] + Bu[n] +% +% $Author: Tobias Naegeli $ $Date: 2014 $ $Revision: 3 $ +% +% +% Arguments: +% approx_prediction: if 1 then the exponential map is approximated with a +% first order taylor approximation. has at the moment not a big influence +% (just 1st or 2nd order approximation) we should change it to rodriquez +% approximation. +% use_inertia_matrix: set to true if you have the inertia matrix J for your +% quadrotor +% xa_apo_k: old state vectotr +% zFlag: if sensor measurement is available [gyro, acc, mag] +% dt: dt in s +% z: measurements [gyro, acc, mag] +% q_rotSpeed: process noise gyro +% q_rotAcc: process noise gyro acceleration +% q_acc: process noise acceleration +% q_mag: process noise magnetometer +% r_gyro: measurement noise gyro +% r_accel: measurement noise accel +% r_mag: measurement noise mag +% J: moment of inertia matrix + + +% Output: +% xa_apo: updated state vectotr +% Pa_apo: updated state covariance matrix +% Rot_matrix: rotation matrix +% eulerAngles: euler angles +% debugOutput: not used + + +%% model specific parameters + + + +% compute once the inverse of the Inertia +persistent Ji; +if isempty(Ji) + Ji=single(inv(J)); +end + +%% init +persistent x_apo +if(isempty(x_apo)) + gyro_init=single([0;0;0]); + gyro_acc_init=single([0;0;0]); + acc_init=single([0;0;-9.81]); + mag_init=single([1;0;0]); + x_apo=single([gyro_init;gyro_acc_init;acc_init;mag_init]); + +end + +persistent P_apo +if(isempty(P_apo)) + % P_apo = single(eye(NSTATES) * 1000); + P_apo = single(200*ones(12)); +end + +debugOutput = single(zeros(4,1)); + +%% copy the states +wx= x_apo(1); % x body angular rate +wy= x_apo(2); % y body angular rate +wz= x_apo(3); % z body angular rate + +wax= x_apo(4); % x body angular acceleration +way= x_apo(5); % y body angular acceleration +waz= x_apo(6); % z body angular acceleration + +zex= x_apo(7); % x component gravity vector +zey= x_apo(8); % y component gravity vector +zez= x_apo(9); % z component gravity vector + +mux= x_apo(10); % x component magnetic field vector +muy= x_apo(11); % y component magnetic field vector +muz= x_apo(12); % z component magnetic field vector + + + + +%% prediction section +% compute the apriori state estimate from the previous aposteriori estimate +%body angular accelerations +if (use_inertia_matrix==1) + wak =[wax;way;waz]+Ji*(-cross([wax;way;waz],J*[wax;way;waz]))*dt; +else + wak =[wax;way;waz]; +end + +%body angular rates +wk =[wx; wy; wz] + dt*wak; + +%derivative of the prediction rotation matrix +O=[0,-wz,wy;wz,0,-wx;-wy,wx,0]'; + +%prediction of the earth z vector +if (approx_prediction==1) + %e^(Odt)=I+dt*O+dt^2/2!O^2 + % so we do a first order approximation of the exponential map + zek =(O*dt+single(eye(3)))*[zex;zey;zez]; + +else + zek =(single(eye(3))+O*dt+dt^2/2*O^2)*[zex;zey;zez]; + %zek =expm2(O*dt)*[zex;zey;zez]; not working because use double + %precision +end + + + +%prediction of the magnetic vector +if (approx_prediction==1) + %e^(Odt)=I+dt*O+dt^2/2!O^2 + % so we do a first order approximation of the exponential map + muk =(O*dt+single(eye(3)))*[mux;muy;muz]; +else + muk =(single(eye(3))+O*dt+dt^2/2*O^2)*[mux;muy;muz]; + %muk =expm2(O*dt)*[mux;muy;muz]; not working because use double + %precision +end + +x_apr=[wk;wak;zek;muk]; + +% compute the apriori error covariance estimate from the previous +%aposteriori estimate + +EZ=[0,zez,-zey; + -zez,0,zex; + zey,-zex,0]'; +MA=[0,muz,-muy; + -muz,0,mux; + muy,-mux,0]'; + +E=single(eye(3)); +Z=single(zeros(3)); + +A_lin=[ Z, E, Z, Z + Z, Z, Z, Z + EZ, Z, O, Z + MA, Z, Z, O]; + +A_lin=eye(12)+A_lin*dt; + +%process covariance matrix + +persistent Q +if (isempty(Q)) + Q=diag([ q_rotSpeed,q_rotSpeed,q_rotSpeed,... + q_rotAcc,q_rotAcc,q_rotAcc,... + q_acc,q_acc,q_acc,... + q_mag,q_mag,q_mag]); +end + +P_apr=A_lin*P_apo*A_lin'+Q; + + +%% update +if zFlag(1)==1&&zFlag(2)==1&&zFlag(3)==1 + +% R=[r_gyro,0,0,0,0,0,0,0,0; +% 0,r_gyro,0,0,0,0,0,0,0; +% 0,0,r_gyro,0,0,0,0,0,0; +% 0,0,0,r_accel,0,0,0,0,0; +% 0,0,0,0,r_accel,0,0,0,0; +% 0,0,0,0,0,r_accel,0,0,0; +% 0,0,0,0,0,0,r_mag,0,0; +% 0,0,0,0,0,0,0,r_mag,0; +% 0,0,0,0,0,0,0,0,r_mag]; + R_v=[r_gyro,r_gyro,r_gyro,r_accel,r_accel,r_accel,r_mag,r_mag,r_mag]; + %observation matrix + %[zw;ze;zmk]; + H_k=[ E, Z, Z, Z; + Z, Z, E, Z; + Z, Z, Z, E]; + + y_k=z(1:9)-H_k*x_apr; + + + %S_k=H_k*P_apr*H_k'+R; + S_k=H_k*P_apr*H_k'; + S_k(1:9+1:end) = S_k(1:9+1:end) + R_v; + K_k=(P_apr*H_k'/(S_k)); + + + x_apo=x_apr+K_k*y_k; + P_apo=(eye(12)-K_k*H_k)*P_apr; +else + if zFlag(1)==1&&zFlag(2)==0&&zFlag(3)==0 + + R=[r_gyro,0,0; + 0,r_gyro,0; + 0,0,r_gyro]; + R_v=[r_gyro,r_gyro,r_gyro]; + %observation matrix + + H_k=[ E, Z, Z, Z]; + + y_k=z(1:3)-H_k(1:3,1:12)*x_apr; + + % S_k=H_k(1:3,1:12)*P_apr*H_k(1:3,1:12)'+R(1:3,1:3); + S_k=H_k(1:3,1:12)*P_apr*H_k(1:3,1:12)'; + S_k(1:3+1:end) = S_k(1:3+1:end) + R_v; + K_k=(P_apr*H_k(1:3,1:12)'/(S_k)); + + + x_apo=x_apr+K_k*y_k; + P_apo=(eye(12)-K_k*H_k(1:3,1:12))*P_apr; + else + if zFlag(1)==1&&zFlag(2)==1&&zFlag(3)==0 + +% R=[r_gyro,0,0,0,0,0; +% 0,r_gyro,0,0,0,0; +% 0,0,r_gyro,0,0,0; +% 0,0,0,r_accel,0,0; +% 0,0,0,0,r_accel,0; +% 0,0,0,0,0,r_accel]; + + R_v=[r_gyro,r_gyro,r_gyro,r_accel,r_accel,r_accel]; + %observation matrix + + H_k=[ E, Z, Z, Z; + Z, Z, E, Z]; + + y_k=z(1:6)-H_k(1:6,1:12)*x_apr; + + % S_k=H_k(1:6,1:12)*P_apr*H_k(1:6,1:12)'+R(1:6,1:6); + S_k=H_k(1:6,1:12)*P_apr*H_k(1:6,1:12)'; + S_k(1:6+1:end) = S_k(1:6+1:end) + R_v; + K_k=(P_apr*H_k(1:6,1:12)'/(S_k)); + + + x_apo=x_apr+K_k*y_k; + P_apo=(eye(12)-K_k*H_k(1:6,1:12))*P_apr; + else + if zFlag(1)==1&&zFlag(2)==0&&zFlag(3)==1 +% R=[r_gyro,0,0,0,0,0; +% 0,r_gyro,0,0,0,0; +% 0,0,r_gyro,0,0,0; +% 0,0,0,r_mag,0,0; +% 0,0,0,0,r_mag,0; +% 0,0,0,0,0,r_mag]; + R_v=[r_gyro,r_gyro,r_gyro,r_mag,r_mag,r_mag]; + %observation matrix + + H_k=[ E, Z, Z, Z; + Z, Z, Z, E]; + + y_k=[z(1:3);z(7:9)]-H_k(1:6,1:12)*x_apr; + + %S_k=H_k(1:6,1:12)*P_apr*H_k(1:6,1:12)'+R(1:6,1:6); + S_k=H_k(1:6,1:12)*P_apr*H_k(1:6,1:12)'; + S_k(1:6+1:end) = S_k(1:6+1:end) + R_v; + K_k=(P_apr*H_k(1:6,1:12)'/(S_k)); + + + x_apo=x_apr+K_k*y_k; + P_apo=(eye(12)-K_k*H_k(1:6,1:12))*P_apr; + else + x_apo=x_apr; + P_apo=P_apr; + end + end + end +end + + + +%% euler anglels extraction +z_n_b = -x_apo(7:9)./norm(x_apo(7:9)); +m_n_b = x_apo(10:12)./norm(x_apo(10:12)); + +y_n_b=cross(z_n_b,m_n_b); +y_n_b=y_n_b./norm(y_n_b); + +x_n_b=(cross(y_n_b,z_n_b)); +x_n_b=x_n_b./norm(x_n_b); + + +xa_apo=x_apo; +Pa_apo=P_apo; +% rotation matrix from earth to body system +Rot_matrix=[x_n_b,y_n_b,z_n_b]; + + +phi=atan2(Rot_matrix(2,3),Rot_matrix(3,3)); +theta=-asin(Rot_matrix(1,3)); +psi=atan2(Rot_matrix(1,2),Rot_matrix(1,1)); +eulerAngles=[phi;theta;psi]; + diff --git a/src/modules/attitude_estimator_ekf/attitudeKalmanfilter.prj b/src/modules/attitude_estimator_ekf/attitudeKalmanfilter.prj new file mode 100644 index 000000000..9ea520346 --- /dev/null +++ b/src/modules/attitude_estimator_ekf/attitudeKalmanfilter.prj @@ -0,0 +1,502 @@ +<?xml version="1.0" encoding="UTF-8"?>
+<deployment-project plugin="plugin.matlabcoder" plugin-version="R2013a">
+ <configuration target="target.matlab.coder" target-name="MEX, C, and C++ Code Generation" name="attitudeKalmanfilter" location="/home/thomasgubler/src/Firmware/src/modules/attitude_estimator_ekf" file="/home/thomasgubler/src/Firmware/src/modules/attitude_estimator_ekf/attitudeKalmanfilter.prj" build-checksum="1213478164">
+ <profile key="profile.mex">
+ <param.MergeInstrumentationResults>false</param.MergeInstrumentationResults>
+ <param.BuiltInstrumentedMex>false</param.BuiltInstrumentedMex>
+ <param.RanInstrumentedMex>false</param.RanInstrumentedMex>
+ <param.WorkingFolder>option.WorkingFolder.Project</param.WorkingFolder>
+ <param.SpecifiedWorkingFolder />
+ <param.BuildFolder>option.BuildFolder.Project</param.BuildFolder>
+ <param.SpecifiedBuildFolder />
+ <param.SearchPaths />
+ <param.ResponsivenessChecks>true</param.ResponsivenessChecks>
+ <param.ExtrinsicCalls>true</param.ExtrinsicCalls>
+ <param.IntegrityChecks>true</param.IntegrityChecks>
+ <param.SaturateOnIntegerOverflow>true</param.SaturateOnIntegerOverflow>
+ <param.GlobalDataSyncMethod>option.GlobalDataSyncMethod.SyncAlways</param.GlobalDataSyncMethod>
+ <param.EnableVariableSizing>true</param.EnableVariableSizing>
+ <param.DynamicMemoryAllocation>option.DynamicMemoryAllocation.Threshold</param.DynamicMemoryAllocation>
+ <param.DynamicMemoryAllocationThreshold>65536</param.DynamicMemoryAllocationThreshold>
+ <param.StackUsageMax>200000</param.StackUsageMax>
+ <param.FilePartitionMethod>option.FilePartitionMethod.MapMFileToCFile</param.FilePartitionMethod>
+ <param.GenerateComments>true</param.GenerateComments>
+ <param.MATLABSourceComments>false</param.MATLABSourceComments>
+ <param.ReservedNameArray />
+ <param.EnableScreener>true</param.EnableScreener>
+ <param.EnableDebugging>false</param.EnableDebugging>
+ <param.GenerateReport>true</param.GenerateReport>
+ <param.LaunchReport>false</param.LaunchReport>
+ <param.CustomSourceCode />
+ <param.CustomHeaderCode />
+ <param.CustomInitializer />
+ <param.CustomTerminator />
+ <param.CustomInclude />
+ <param.CustomSource />
+ <param.CustomLibrary />
+ <param.PostCodeGenCommand />
+ <param.ProposeFixedPointDataTypes>true</param.ProposeFixedPointDataTypes>
+ <param.mex.GenCodeOnly>false</param.mex.GenCodeOnly>
+ <param.ConstantFoldingTimeout>40000</param.ConstantFoldingTimeout>
+ <param.RecursionLimit>100</param.RecursionLimit>
+ <param.TargetLang>option.TargetLang.C</param.TargetLang>
+ <param.EchoExpressions>true</param.EchoExpressions>
+ <param.InlineThreshold>10</param.InlineThreshold>
+ <param.InlineThresholdMax>200</param.InlineThresholdMax>
+ <param.InlineStackLimit>4000</param.InlineStackLimit>
+ <param.EnableMemcpy>true</param.EnableMemcpy>
+ <param.MemcpyThreshold>64</param.MemcpyThreshold>
+ <param.EnableOpenMP>true</param.EnableOpenMP>
+ <param.InitFltsAndDblsToZero>true</param.InitFltsAndDblsToZero>
+ <param.ConstantInputs>option.ConstantInputs.CheckValues</param.ConstantInputs>
+ <unset>
+ <param.MergeInstrumentationResults />
+ <param.BuiltInstrumentedMex />
+ <param.RanInstrumentedMex />
+ <param.WorkingFolder />
+ <param.SpecifiedWorkingFolder />
+ <param.BuildFolder />
+ <param.SpecifiedBuildFolder />
+ <param.SearchPaths />
+ <param.ResponsivenessChecks />
+ <param.ExtrinsicCalls />
+ <param.IntegrityChecks />
+ <param.SaturateOnIntegerOverflow />
+ <param.GlobalDataSyncMethod />
+ <param.EnableVariableSizing />
+ <param.DynamicMemoryAllocation />
+ <param.DynamicMemoryAllocationThreshold />
+ <param.StackUsageMax />
+ <param.FilePartitionMethod />
+ <param.GenerateComments />
+ <param.MATLABSourceComments />
+ <param.ReservedNameArray />
+ <param.EnableScreener />
+ <param.EnableDebugging />
+ <param.GenerateReport />
+ <param.LaunchReport />
+ <param.CustomSourceCode />
+ <param.CustomHeaderCode />
+ <param.CustomInitializer />
+ <param.CustomTerminator />
+ <param.CustomInclude />
+ <param.CustomSource />
+ <param.CustomLibrary />
+ <param.PostCodeGenCommand />
+ <param.ProposeFixedPointDataTypes />
+ <param.mex.GenCodeOnly />
+ <param.ConstantFoldingTimeout />
+ <param.RecursionLimit />
+ <param.TargetLang />
+ <param.EchoExpressions />
+ <param.InlineThreshold />
+ <param.InlineThresholdMax />
+ <param.InlineStackLimit />
+ <param.EnableMemcpy />
+ <param.MemcpyThreshold />
+ <param.EnableOpenMP />
+ <param.InitFltsAndDblsToZero />
+ <param.ConstantInputs />
+ </unset>
+ </profile>
+ <profile key="profile.c">
+ <param.grt.GenCodeOnly>true</param.grt.GenCodeOnly>
+ <param.WorkingFolder>option.WorkingFolder.Project</param.WorkingFolder>
+ <param.SpecifiedWorkingFolder />
+ <param.BuildFolder>option.BuildFolder.Specified</param.BuildFolder>
+ <param.SpecifiedBuildFolder>codegen</param.SpecifiedBuildFolder>
+ <param.SearchPaths />
+ <param.SaturateOnIntegerOverflow>true</param.SaturateOnIntegerOverflow>
+ <param.PurelyIntegerCode>false</param.PurelyIntegerCode>
+ <param.SupportNonFinite>false</param.SupportNonFinite>
+ <param.EnableVariableSizing>false</param.EnableVariableSizing>
+ <param.DynamicMemoryAllocation>option.DynamicMemoryAllocation.Threshold</param.DynamicMemoryAllocation>
+ <param.DynamicMemoryAllocationThreshold>65536</param.DynamicMemoryAllocationThreshold>
+ <param.StackUsageMax>4000</param.StackUsageMax>
+ <param.MultiInstanceCode>false</param.MultiInstanceCode>
+ <param.FilePartitionMethod>option.FilePartitionMethod.SingleFile</param.FilePartitionMethod>
+ <param.GenerateComments>true</param.GenerateComments>
+ <param.MATLABSourceComments>true</param.MATLABSourceComments>
+ <param.MATLABFcnDesc>false</param.MATLABFcnDesc>
+ <param.DataTypeReplacement>option.DataTypeReplacement.CBuiltIn</param.DataTypeReplacement>
+ <param.ConvertIfToSwitch>false</param.ConvertIfToSwitch>
+ <param.PreserveExternInFcnDecls>true</param.PreserveExternInFcnDecls>
+ <param.ParenthesesLevel>option.ParenthesesLevel.Nominal</param.ParenthesesLevel>
+ <param.MaxIdLength>31</param.MaxIdLength>
+ <param.CustomSymbolStrGlobalVar>$M$N</param.CustomSymbolStrGlobalVar>
+ <param.CustomSymbolStrType>$M$N</param.CustomSymbolStrType>
+ <param.CustomSymbolStrField>$M$N</param.CustomSymbolStrField>
+ <param.CustomSymbolStrFcn>$M$N</param.CustomSymbolStrFcn>
+ <param.CustomSymbolStrTmpVar>$M$N</param.CustomSymbolStrTmpVar>
+ <param.CustomSymbolStrMacro>$M$N</param.CustomSymbolStrMacro>
+ <param.CustomSymbolStrEMXArray>emxArray_$M$N</param.CustomSymbolStrEMXArray>
+ <param.CustomSymbolStrEMXArrayFcn>emx$M$N</param.CustomSymbolStrEMXArrayFcn>
+ <param.ReservedNameArray />
+ <param.EnableScreener>true</param.EnableScreener>
+ <param.Verbose>false</param.Verbose>
+ <param.GenerateReport>true</param.GenerateReport>
+ <param.GenerateCodeMetricsReport>true</param.GenerateCodeMetricsReport>
+ <param.GenerateCodeReplacementReport>false</param.GenerateCodeReplacementReport>
+ <param.LaunchReport>true</param.LaunchReport>
+ <param.CustomSourceCode />
+ <param.CustomHeaderCode />
+ <param.CustomInitializer />
+ <param.CustomTerminator />
+ <param.CustomInclude />
+ <param.CustomSource />
+ <param.CustomLibrary />
+ <param.PostCodeGenCommand />
+ <param.CodeReplacementLibrary>C89/C90 (ANSI)</param.CodeReplacementLibrary>
+ <param.SameHardware>true</param.SameHardware>
+ <param.HardwareVendor.Production>ARM Compatible</param.HardwareVendor.Production>
+ <param.HardwareType.Production>ARM Cortex</param.HardwareType.Production>
+ <var.instance.enabled.Production>true</var.instance.enabled.Production>
+ <param.HardwareSizeChar.Production>8</param.HardwareSizeChar.Production>
+ <param.HardwareSizeShort.Production>16</param.HardwareSizeShort.Production>
+ <param.HardwareSizeInt.Production>32</param.HardwareSizeInt.Production>
+ <param.HardwareSizeLong.Production>32</param.HardwareSizeLong.Production>
+ <param.HardwareSizeLongLong.Production>64</param.HardwareSizeLongLong.Production>
+ <param.HardwareSizeFloat.Production>32</param.HardwareSizeFloat.Production>
+ <param.HardwareSizeDouble.Production>64</param.HardwareSizeDouble.Production>
+ <param.HardwareSizeWord.Production>32</param.HardwareSizeWord.Production>
+ <param.HardwareSizePointer.Production>32</param.HardwareSizePointer.Production>
+ <param.HardwareEndianness.Production>option.HardwareEndianness.Little</param.HardwareEndianness.Production>
+ <param.HardwareArithmeticRightShift.Production>true</param.HardwareArithmeticRightShift.Production>
+ <param.HardwareLongLongMode.Production>false</param.HardwareLongLongMode.Production>
+ <param.HardwareAtomicIntegerSize.Production>option.HardwareAtomicIntegerSize.Long</param.HardwareAtomicIntegerSize.Production>
+ <param.HardwareAtomicFloatSize.Production>option.HardwareAtomicFloatSize.Double</param.HardwareAtomicFloatSize.Production>
+ <param.HardwareDivisionRounding.Production>option.HardwareDivisionRounding.Undefined</param.HardwareDivisionRounding.Production>
+ <param.HardwareVendor.Target>Generic</param.HardwareVendor.Target>
+ <param.HardwareType.Target>MATLAB Host Computer</param.HardwareType.Target>
+ <var.instance.enabled.Target>false</var.instance.enabled.Target>
+ <param.HardwareSizeChar.Target>8</param.HardwareSizeChar.Target>
+ <param.HardwareSizeShort.Target>16</param.HardwareSizeShort.Target>
+ <param.HardwareSizeInt.Target>32</param.HardwareSizeInt.Target>
+ <param.HardwareSizeLong.Target>64</param.HardwareSizeLong.Target>
+ <param.HardwareSizeLongLong.Target>64</param.HardwareSizeLongLong.Target>
+ <param.HardwareSizeFloat.Target>32</param.HardwareSizeFloat.Target>
+ <param.HardwareSizeDouble.Target>64</param.HardwareSizeDouble.Target>
+ <param.HardwareSizeWord.Target>64</param.HardwareSizeWord.Target>
+ <param.HardwareSizePointer.Target>64</param.HardwareSizePointer.Target>
+ <param.HardwareEndianness.Target>option.HardwareEndianness.Little</param.HardwareEndianness.Target>
+ <param.HardwareArithmeticRightShift.Target>true</param.HardwareArithmeticRightShift.Target>
+ <param.HardwareLongLongMode.Target>true</param.HardwareLongLongMode.Target>
+ <param.HardwareAtomicIntegerSize.Target>option.HardwareAtomicIntegerSize.Char</param.HardwareAtomicIntegerSize.Target>
+ <param.HardwareAtomicFloatSize.Target>option.HardwareAtomicFloatSize.None</param.HardwareAtomicFloatSize.Target>
+ <param.HardwareDivisionRounding.Target>option.HardwareDivisionRounding.Zero</param.HardwareDivisionRounding.Target>
+ <param.Toolchain>Automatically locate an installed toolchain</param.Toolchain>
+ <param.BuildConfiguration>Faster Builds</param.BuildConfiguration>
+ <param.CustomToolchainOptions />
+ <param.ConstantFoldingTimeout>40000</param.ConstantFoldingTimeout>
+ <param.RecursionLimit>100</param.RecursionLimit>
+ <param.IncludeTerminateFcn>true</param.IncludeTerminateFcn>
+ <param.TargetLang>option.TargetLang.C</param.TargetLang>
+ <param.CCompilerOptimization>option.CCompilerOptimization.On</param.CCompilerOptimization>
+ <param.CCompilerCustomOptimizations />
+ <param.GenerateMakefile>true</param.GenerateMakefile>
+ <param.BuildToolEnable>false</param.BuildToolEnable>
+ <param.MakeCommand>make_rtw</param.MakeCommand>
+ <param.TemplateMakefile>default_tmf</param.TemplateMakefile>
+ <param.BuildToolConfiguration />
+ <param.InlineThreshold>10</param.InlineThreshold>
+ <param.InlineThresholdMax>200</param.InlineThresholdMax>
+ <param.InlineStackLimit>4000</param.InlineStackLimit>
+ <param.EnableMemcpy>true</param.EnableMemcpy>
+ <param.MemcpyThreshold>64</param.MemcpyThreshold>
+ <param.EnableOpenMP>true</param.EnableOpenMP>
+ <param.InitFltsAndDblsToZero>true</param.InitFltsAndDblsToZero>
+ <param.PassStructByReference>false</param.PassStructByReference>
+ <param.UseECoderFeatures>true</param.UseECoderFeatures>
+ <unset>
+ <param.WorkingFolder />
+ <param.SpecifiedWorkingFolder />
+ <param.SearchPaths />
+ <param.SaturateOnIntegerOverflow />
+ <param.PurelyIntegerCode />
+ <param.DynamicMemoryAllocation />
+ <param.DynamicMemoryAllocationThreshold />
+ <param.MultiInstanceCode />
+ <param.GenerateComments />
+ <param.MATLABFcnDesc />
+ <param.DataTypeReplacement />
+ <param.ConvertIfToSwitch />
+ <param.PreserveExternInFcnDecls />
+ <param.ParenthesesLevel />
+ <param.MaxIdLength />
+ <param.CustomSymbolStrGlobalVar />
+ <param.CustomSymbolStrType />
+ <param.CustomSymbolStrField />
+ <param.CustomSymbolStrFcn />
+ <param.CustomSymbolStrTmpVar />
+ <param.CustomSymbolStrMacro />
+ <param.CustomSymbolStrEMXArray />
+ <param.CustomSymbolStrEMXArrayFcn />
+ <param.ReservedNameArray />
+ <param.EnableScreener />
+ <param.Verbose />
+ <param.GenerateReport />
+ <param.GenerateCodeMetricsReport />
+ <param.GenerateCodeReplacementReport />
+ <param.CustomInclude />
+ <param.CustomSource />
+ <param.CustomLibrary />
+ <param.SameHardware />
+ <var.instance.enabled.Production />
+ <param.HardwareSizeChar.Production />
+ <param.HardwareSizeShort.Production />
+ <param.HardwareSizeInt.Production />
+ <param.HardwareSizeLong.Production />
+ <param.HardwareSizeLongLong.Production />
+ <param.HardwareSizeFloat.Production />
+ <param.HardwareSizeDouble.Production />
+ <param.HardwareSizeWord.Production />
+ <param.HardwareSizePointer.Production />
+ <param.HardwareEndianness.Production />
+ <param.HardwareLongLongMode.Production />
+ <param.HardwareDivisionRounding.Production />
+ <var.instance.enabled.Target />
+ <param.HardwareSizeChar.Target />
+ <param.HardwareSizeShort.Target />
+ <param.HardwareSizeInt.Target />
+ <param.HardwareSizeLongLong.Target />
+ <param.HardwareSizeFloat.Target />
+ <param.HardwareSizeDouble.Target />
+ <param.HardwareEndianness.Target />
+ <param.HardwareAtomicFloatSize.Target />
+ <param.CustomToolchainOptions />
+ <param.ConstantFoldingTimeout />
+ <param.RecursionLimit />
+ <param.IncludeTerminateFcn />
+ <param.TargetLang />
+ <param.CCompilerCustomOptimizations />
+ <param.GenerateMakefile />
+ <param.BuildToolEnable />
+ <param.MakeCommand />
+ <param.TemplateMakefile />
+ <param.BuildToolConfiguration />
+ <param.InlineThreshold />
+ <param.InlineThresholdMax />
+ <param.InlineStackLimit />
+ <param.EnableMemcpy />
+ <param.MemcpyThreshold />
+ <param.EnableOpenMP />
+ <param.InitFltsAndDblsToZero />
+ <param.UseECoderFeatures />
+ </unset>
+ </profile>
+ <param.outputfile>/opt/matlab/r2013b/bin/codegen/codegen/lib/AttitudeEKF/AttitudeEKF.a</param.outputfile>
+ <param.version>R2012a</param.version>
+ <param.HasECoderFeatures>true</param.HasECoderFeatures>
+ <param.mex.mainhtml>t:\private\desktop-dinfk-xp\Attitude_Kalmanfilter\codegen\mex\attitudeKalmanfilter\html\index.html</param.mex.mainhtml>
+ <param.grt.mainhtml>/home/thomasgubler/src/Firmware/src/modules/attitude_estimator_ekf/codegen/html/index.html</param.grt.mainhtml>
+ <param.CallGeneratedCodeFromTest>true</param.CallGeneratedCodeFromTest>
+ <param.VerificationMode>option.VerificationMode.None</param.VerificationMode>
+ <param.SILDebugging>false</param.SILDebugging>
+ <param.DefaultTestFile>${PROJECT_ROOT}/AttitudeEKF_Test.m</param.DefaultTestFile>
+ <param.AutoInferDefaultFile>${PROJECT_ROOT}/AttitudeEKF_Test.m</param.AutoInferDefaultFile>
+ <param.AutoInferUseVariableSize>false</param.AutoInferUseVariableSize>
+ <param.AutoInferUseUnboundedSize>false</param.AutoInferUseUnboundedSize>
+ <param.AutoInferVariableSizeThreshold>1024</param.AutoInferVariableSizeThreshold>
+ <param.AutoInferUnboundedSizeThreshold>2048</param.AutoInferUnboundedSizeThreshold>
+ <param.mex.outputfile>AttitudeEKF_mex</param.mex.outputfile>
+ <param.grt.outputfile>AttitudeEKF</param.grt.outputfile>
+ <param.artifact>option.target.artifact.lib</param.artifact>
+ <param.FixedPointTypeProposalMode>option.FixedPointTypeProposalMode.ProposeFractionLengths</param.FixedPointTypeProposalMode>
+ <param.DefaultProposedFixedPointType>numerictype([],16,12)</param.DefaultProposedFixedPointType>
+ <param.MinMaxSafetyMargin>0</param.MinMaxSafetyMargin>
+ <param.OptimizeWholeNumbers>true</param.OptimizeWholeNumbers>
+ <param.LaunchInstrumentationReport>false</param.LaunchInstrumentationReport>
+ <param.OpenInstrumentationReportInBrowser>false</param.OpenInstrumentationReportInBrowser>
+ <param.CreatePrintableInstrumentationReport>false</param.CreatePrintableInstrumentationReport>
+ <param.EnableAutoExtrinsicCalls>true</param.EnableAutoExtrinsicCalls>
+ <param.UsePreconditions>false</param.UsePreconditions>
+ <param.FeatureFlags />
+ <param.FixedPointMode>option.FixedPointMode.None</param.FixedPointMode>
+ <param.AutoScaleLoopIndexVariables>false</param.AutoScaleLoopIndexVariables>
+ <param.ComputedFixedPointData />
+ <param.UserFixedPointData />
+ <param.DefaultWordLength>16</param.DefaultWordLength>
+ <param.DefaultFractionLength>4</param.DefaultFractionLength>
+ <param.FixedPointSafetyMargin>0</param.FixedPointSafetyMargin>
+ <param.FixedPointFimath>fimath('RoundingMethod', 'Floor', 'OverflowAction', 'Wrap', 'ProductMode', 'FullPrecision', 'MaxProductWordLength', 128, 'SumMode', 'FullPrecision', 'MaxSumWordLength', 128)</param.FixedPointFimath>
+ <param.FixedPointTypeSource>option.FixedPointTypeSource.SimAndDerived</param.FixedPointTypeSource>
+ <param.StaticAnalysisTimeout />
+ <param.StaticAnalysisGlobalRangesOnly>false</param.StaticAnalysisGlobalRangesOnly>
+ <param.LogAllIOValues>false</param.LogAllIOValues>
+ <param.LogHistogram>false</param.LogHistogram>
+ <param.ShowCoverage>true</param.ShowCoverage>
+ <param.ExcludedFixedPointVerificationFiles />
+ <param.ExcludedFixedPointSimulationFiles />
+ <param.InstrumentedBuildChecksum />
+ <param.FixedPointStaticAnalysisChecksum />
+ <param.InstrumentedMexFile />
+ <param.FixedPointValidationChecksum />
+ <param.FixedPointSourceCodeChecksum />
+ <param.FixedPointFunctionReplacements />
+ <param.OptimizeWholeNumbers>true</param.OptimizeWholeNumbers>
+ <param.GeneratedFixedPointFileSuffix>_fixpt</param.GeneratedFixedPointFileSuffix>
+ <param.DefaultFixedPointSignedness>option.DefaultFixedPointSignedness.Automatic</param.DefaultFixedPointSignedness>
+ <unset>
+ <param.outputfile />
+ <param.version />
+ <param.HasECoderFeatures />
+ <param.CallGeneratedCodeFromTest />
+ <param.VerificationMode />
+ <param.SILDebugging />
+ <param.AutoInferUseVariableSize />
+ <param.AutoInferUseUnboundedSize />
+ <param.AutoInferVariableSizeThreshold />
+ <param.AutoInferUnboundedSizeThreshold />
+ <param.mex.outputfile />
+ <param.grt.outputfile />
+ <param.FixedPointTypeProposalMode />
+ <param.DefaultProposedFixedPointType />
+ <param.MinMaxSafetyMargin />
+ <param.OptimizeWholeNumbers />
+ <param.LaunchInstrumentationReport />
+ <param.OpenInstrumentationReportInBrowser />
+ <param.CreatePrintableInstrumentationReport />
+ <param.EnableAutoExtrinsicCalls />
+ <param.UsePreconditions />
+ <param.FeatureFlags />
+ <param.FixedPointMode />
+ <param.AutoScaleLoopIndexVariables />
+ <param.ComputedFixedPointData />
+ <param.UserFixedPointData />
+ <param.DefaultWordLength />
+ <param.DefaultFractionLength />
+ <param.FixedPointSafetyMargin />
+ <param.FixedPointFimath />
+ <param.FixedPointTypeSource />
+ <param.StaticAnalysisTimeout />
+ <param.StaticAnalysisGlobalRangesOnly />
+ <param.LogAllIOValues />
+ <param.LogHistogram />
+ <param.ShowCoverage />
+ <param.ExcludedFixedPointVerificationFiles />
+ <param.ExcludedFixedPointSimulationFiles />
+ <param.InstrumentedBuildChecksum />
+ <param.FixedPointStaticAnalysisChecksum />
+ <param.InstrumentedMexFile />
+ <param.FixedPointValidationChecksum />
+ <param.FixedPointSourceCodeChecksum />
+ <param.FixedPointFunctionReplacements />
+ <param.GeneratedFixedPointFileSuffix />
+ <param.DefaultFixedPointSignedness />
+ </unset>
+ <fileset.entrypoints>
+ <file value="${PROJECT_ROOT}/AttitudeEKF.m" custom-data-expanded="true">
+ <Inputs fileName="AttitudeEKF.m" functionName="AttitudeEKF">
+ <Input Name="approx_prediction">
+ <Class>uint8</Class>
+ <UserDefined>false</UserDefined>
+ <Size>1 x 1</Size>
+ <Complex>false</Complex>
+ </Input>
+ <Input Name="use_inertia_matrix">
+ <Class>uint8</Class>
+ <UserDefined>false</UserDefined>
+ <Size>1 x 1</Size>
+ <Complex>false</Complex>
+ </Input>
+ <Input Name="zFlag">
+ <Class>uint8</Class>
+ <UserDefined>false</UserDefined>
+ <Size>3 x 1</Size>
+ <Complex>false</Complex>
+ </Input>
+ <Input Name="dt">
+ <Class>single</Class>
+ <UserDefined>false</UserDefined>
+ <Size>1 x 1</Size>
+ <Complex>false</Complex>
+ </Input>
+ <Input Name="z">
+ <Class>single</Class>
+ <UserDefined>false</UserDefined>
+ <Size>9 x 1</Size>
+ <Complex>false</Complex>
+ </Input>
+ <Input Name="q_rotSpeed">
+ <Class>single</Class>
+ <UserDefined>false</UserDefined>
+ <Size>1 x 1</Size>
+ <Complex>false</Complex>
+ </Input>
+ <Input Name="q_rotAcc">
+ <Class>single</Class>
+ <UserDefined>false</UserDefined>
+ <Size>1 x 1</Size>
+ <Complex>false</Complex>
+ </Input>
+ <Input Name="q_acc">
+ <Class>single</Class>
+ <UserDefined>false</UserDefined>
+ <Size>1 x 1</Size>
+ <Complex>false</Complex>
+ </Input>
+ <Input Name="q_mag">
+ <Class>single</Class>
+ <UserDefined>false</UserDefined>
+ <Size>1 x 1</Size>
+ <Complex>false</Complex>
+ </Input>
+ <Input Name="r_gyro">
+ <Class>single</Class>
+ <UserDefined>false</UserDefined>
+ <Size>1 x 1</Size>
+ <Complex>false</Complex>
+ </Input>
+ <Input Name="r_accel">
+ <Class>single</Class>
+ <UserDefined>false</UserDefined>
+ <Size>1 x 1</Size>
+ <Complex>false</Complex>
+ </Input>
+ <Input Name="r_mag">
+ <Class>single</Class>
+ <UserDefined>false</UserDefined>
+ <Size>1 x 1</Size>
+ <Complex>false</Complex>
+ </Input>
+ <Input Name="J">
+ <Class>single</Class>
+ <UserDefined>false</UserDefined>
+ <Size>3 x 3</Size>
+ <Complex>false</Complex>
+ </Input>
+ </Inputs>
+ </file>
+ </fileset.entrypoints>
+ <fileset.testbench>
+ <file>${PROJECT_ROOT}/AttitudeEKF_Test.m</file>
+ </fileset.testbench>
+ <fileset.package />
+ <build-deliverables>
+ <file name="AttitudeEKF.a" location="${MATLAB_ROOT}/bin/codegen/codegen/lib/AttitudeEKF" optional="false">/opt/matlab/r2013b/bin/codegen/codegen/lib/AttitudeEKF/AttitudeEKF.a</file>
+ </build-deliverables>
+ <workflow />
+ <matlab>
+ <root>/opt/matlab/r2013b</root>
+ <toolboxes>
+ <toolbox name="fixedpoint" />
+ </toolboxes>
+ </matlab>
+ <platform>
+ <unix>true</unix>
+ <mac>false</mac>
+ <windows>false</windows>
+ <win2k>false</win2k>
+ <winxp>false</winxp>
+ <vista>false</vista>
+ <linux>true</linux>
+ <solaris>false</solaris>
+ <osver>3.16.1-1-ARCH</osver>
+ <os32>false</os32>
+ <os64>true</os64>
+ <arch>glnxa64</arch>
+ <matlab>true</matlab>
+ </platform>
+ </configuration>
+</deployment-project>
+
diff --git a/src/modules/attitude_estimator_ekf/attitude_estimator_ekf_main.cpp b/src/modules/attitude_estimator_ekf/attitude_estimator_ekf_main.cpp index 8d537d676..9784ea1a1 100755 --- a/src/modules/attitude_estimator_ekf/attitude_estimator_ekf_main.cpp +++ b/src/modules/attitude_estimator_ekf/attitude_estimator_ekf_main.cpp @@ -38,6 +38,7 @@ * * @author Tobias Naegeli <naegelit@student.ethz.ch> * @author Lorenz Meier <lm@inf.ethz.ch> + * @author Thomas Gubler <thomasgubler@gmail.com> */ #include <nuttx/config.h> @@ -74,8 +75,7 @@ #ifdef __cplusplus extern "C" { #endif -#include "codegen/attitudeKalmanfilter_initialize.h" -#include "codegen/attitudeKalmanfilter.h" +#include "codegen/AttitudeEKF.h" #include "attitude_estimator_ekf_params.h" #ifdef __cplusplus } @@ -132,7 +132,7 @@ int attitude_estimator_ekf_main(int argc, char *argv[]) attitude_estimator_ekf_task = task_spawn_cmd("attitude_estimator_ekf", SCHED_DEFAULT, SCHED_PRIORITY_MAX - 5, - 14000, + 7200, attitude_estimator_ekf_thread_main, (argv) ? (const char **)&argv[2] : (const char **)NULL); exit(0); @@ -206,10 +206,12 @@ const unsigned int loop_interval_alarm = 6500; // loop interval in microseconds 0, 0, 1.f }; /**< init: identity matrix */ + float debugOutput[4] = { 0.0f }; + int overloadcounter = 19; /* Initialize filter */ - attitudeKalmanfilter_initialize(); + AttitudeEKF_initialize(); /* store start time to guard against too slow update rates */ uint64_t last_run = hrt_absolute_time(); @@ -273,9 +275,9 @@ const unsigned int loop_interval_alarm = 6500; // loop interval in microseconds /* keep track of sensor updates */ uint64_t sensor_last_timestamp[3] = {0, 0, 0}; - struct attitude_estimator_ekf_params ekf_params; + struct attitude_estimator_ekf_params ekf_params = { 0 }; - struct attitude_estimator_ekf_param_handles ekf_param_handles; + struct attitude_estimator_ekf_param_handles ekf_param_handles = { 0 }; /* initialize parameter handles */ parameters_init(&ekf_param_handles); @@ -504,8 +506,25 @@ const unsigned int loop_interval_alarm = 6500; // loop interval in microseconds continue; } - attitudeKalmanfilter(update_vect, dt, z_k, x_aposteriori_k, P_aposteriori_k, ekf_params.q, ekf_params.r, - euler, Rot_matrix, x_aposteriori, P_aposteriori); + /* Call the estimator */ + AttitudeEKF(false, // approx_prediction + (unsigned char)ekf_params.use_moment_inertia, + update_vect, + dt, + z_k, + ekf_params.q[0], // q_rotSpeed, + ekf_params.q[1], // q_rotAcc + ekf_params.q[2], // q_acc + ekf_params.q[3], // q_mag + ekf_params.r[0], // r_gyro + ekf_params.r[1], // r_accel + ekf_params.r[2], // r_mag + ekf_params.moment_inertia_J, + x_aposteriori, + P_aposteriori, + Rot_matrix, + euler, + debugOutput); /* swap values for next iteration, check for fatal inputs */ if (isfinite(euler[0]) && isfinite(euler[1]) && isfinite(euler[2])) { diff --git a/src/modules/attitude_estimator_ekf/attitude_estimator_ekf_params.c b/src/modules/attitude_estimator_ekf/attitude_estimator_ekf_params.c index bc0e3b93a..5637ec102 100755 --- a/src/modules/attitude_estimator_ekf/attitude_estimator_ekf_params.c +++ b/src/modules/attitude_estimator_ekf/attitude_estimator_ekf_params.c @@ -44,28 +44,96 @@ /* Extended Kalman Filter covariances */ -/* gyro process noise */ + +/** + * Body angular rate process noise + * + * @group attitude_ekf + */ PARAM_DEFINE_FLOAT(EKF_ATT_V3_Q0, 1e-4f); + +/** + * Body angular acceleration process noise + * + * @group attitude_ekf + */ PARAM_DEFINE_FLOAT(EKF_ATT_V3_Q1, 0.08f); + +/** + * Acceleration process noise + * + * @group attitude_ekf + */ PARAM_DEFINE_FLOAT(EKF_ATT_V3_Q2, 0.009f); -/* gyro offsets process noise */ + +/** + * Magnet field vector process noise + * + * @group attitude_ekf + */ PARAM_DEFINE_FLOAT(EKF_ATT_V3_Q3, 0.005f); -PARAM_DEFINE_FLOAT(EKF_ATT_V3_Q4, 0.0f); -/* gyro measurement noise */ +/** + * Gyro measurement noise + * + * @group attitude_ekf + */ PARAM_DEFINE_FLOAT(EKF_ATT_V4_R0, 0.0008f); -/* accel measurement noise */ + +/** + * Accel measurement noise + * + * @group attitude_ekf + */ PARAM_DEFINE_FLOAT(EKF_ATT_V4_R1, 10000.0f); -/* mag measurement noise */ + +/** + * Mag measurement noise + * + * @group attitude_ekf + */ PARAM_DEFINE_FLOAT(EKF_ATT_V4_R2, 100.0f); -/* offset estimation - UNUSED */ -PARAM_DEFINE_FLOAT(EKF_ATT_V4_R3, 0.0f); /* magnetic declination, in degrees */ PARAM_DEFINE_FLOAT(ATT_MAG_DECL, 0.0f); PARAM_DEFINE_INT32(ATT_ACC_COMP, 2); +/** + * Moment of inertia matrix diagonal entry (1, 1) + * + * @group attitude_ekf + * @unit kg*m^2 + */ +PARAM_DEFINE_FLOAT(ATT_J11, 0.0018); + +/** + * Moment of inertia matrix diagonal entry (2, 2) + * + * @group attitude_ekf + * @unit kg*m^2 + */ +PARAM_DEFINE_FLOAT(ATT_J22, 0.0018); + +/** + * Moment of inertia matrix diagonal entry (3, 3) + * + * @group attitude_ekf + * @unit kg*m^2 + */ +PARAM_DEFINE_FLOAT(ATT_J33, 0.0037); + +/** + * Moment of inertia enabled in estimator + * + * If set to != 0 the moment of inertia will be used in the estimator + * + * @group attitude_ekf + * @min 0 + * @max 1 + */ +PARAM_DEFINE_INT32(ATT_J_EN, 0); + int parameters_init(struct attitude_estimator_ekf_param_handles *h) { /* PID parameters */ @@ -73,17 +141,20 @@ int parameters_init(struct attitude_estimator_ekf_param_handles *h) h->q1 = param_find("EKF_ATT_V3_Q1"); h->q2 = param_find("EKF_ATT_V3_Q2"); h->q3 = param_find("EKF_ATT_V3_Q3"); - h->q4 = param_find("EKF_ATT_V3_Q4"); h->r0 = param_find("EKF_ATT_V4_R0"); h->r1 = param_find("EKF_ATT_V4_R1"); h->r2 = param_find("EKF_ATT_V4_R2"); - h->r3 = param_find("EKF_ATT_V4_R3"); h->mag_decl = param_find("ATT_MAG_DECL"); h->acc_comp = param_find("ATT_ACC_COMP"); + h->moment_inertia_J[0] = param_find("ATT_J11"); + h->moment_inertia_J[1] = param_find("ATT_J22"); + h->moment_inertia_J[2] = param_find("ATT_J33"); + h->use_moment_inertia = param_find("ATT_J_EN"); + return OK; } @@ -93,17 +164,20 @@ int parameters_update(const struct attitude_estimator_ekf_param_handles *h, stru param_get(h->q1, &(p->q[1])); param_get(h->q2, &(p->q[2])); param_get(h->q3, &(p->q[3])); - param_get(h->q4, &(p->q[4])); param_get(h->r0, &(p->r[0])); param_get(h->r1, &(p->r[1])); param_get(h->r2, &(p->r[2])); - param_get(h->r3, &(p->r[3])); param_get(h->mag_decl, &(p->mag_decl)); p->mag_decl *= M_PI_F / 180.0f; param_get(h->acc_comp, &(p->acc_comp)); + for (int i = 0; i < 3; i++) { + param_get(h->moment_inertia_J[i], &(p->moment_inertia_J[3 * i + i])); + } + param_get(h->use_moment_inertia, &(p->use_moment_inertia)); + return OK; } diff --git a/src/modules/attitude_estimator_ekf/attitude_estimator_ekf_params.h b/src/modules/attitude_estimator_ekf/attitude_estimator_ekf_params.h index 5985541ca..5d3b6b244 100755 --- a/src/modules/attitude_estimator_ekf/attitude_estimator_ekf_params.h +++ b/src/modules/attitude_estimator_ekf/attitude_estimator_ekf_params.h @@ -42,8 +42,10 @@ #include <systemlib/param/param.h> struct attitude_estimator_ekf_params { - float r[9]; - float q[12]; + float r[3]; + float q[4]; + float moment_inertia_J[9]; + int32_t use_moment_inertia; float roll_off; float pitch_off; float yaw_off; @@ -52,8 +54,10 @@ struct attitude_estimator_ekf_params { }; struct attitude_estimator_ekf_param_handles { - param_t r0, r1, r2, r3; - param_t q0, q1, q2, q3, q4; + param_t r0, r1, r2; + param_t q0, q1, q2, q3; + param_t moment_inertia_J[3]; /**< diagonal entries of the matrix */ + param_t use_moment_inertia; param_t mag_decl; param_t acc_comp; }; diff --git a/src/modules/attitude_estimator_ekf/codegen/AttitudeEKF.c b/src/modules/attitude_estimator_ekf/codegen/AttitudeEKF.c new file mode 100644 index 000000000..68db382cf --- /dev/null +++ b/src/modules/attitude_estimator_ekf/codegen/AttitudeEKF.c @@ -0,0 +1,1456 @@ +/* + * AttitudeEKF.c + * + * Code generation for function 'AttitudeEKF' + * + * C source code generated on: Thu Aug 21 11:17:28 2014 + * + */ + +/* Include files */ +#include "AttitudeEKF.h" + +/* Variable Definitions */ +static float Ji[9]; +static boolean_T Ji_not_empty; +static float x_apo[12]; +static float P_apo[144]; +static float Q[144]; +static boolean_T Q_not_empty; + +/* Function Declarations */ +static void AttitudeEKF_init(void); +static void b_mrdivide(const float A[72], const float B[36], float y[72]); +static void inv(const float x[9], float y[9]); +static void mrdivide(const float A[108], const float B[81], float y[108]); +static float norm(const float x[3]); + +/* Function Definitions */ +static void AttitudeEKF_init(void) +{ + int i; + static const float fv5[12] = { 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, + -9.81F, 1.0F, 0.0F, 0.0F }; + + for (i = 0; i < 12; i++) { + x_apo[i] = fv5[i]; + } + + for (i = 0; i < 144; i++) { + P_apo[i] = 200.0F; + } +} + +/* + * + */ +static void b_mrdivide(const float A[72], const float B[36], float y[72]) +{ + float b_A[36]; + signed char ipiv[6]; + int i1; + int iy; + int j; + int c; + int ix; + float temp; + int k; + float s; + int jy; + int ijA; + float Y[72]; + for (i1 = 0; i1 < 6; i1++) { + for (iy = 0; iy < 6; iy++) { + b_A[iy + 6 * i1] = B[i1 + 6 * iy]; + } + + ipiv[i1] = (signed char)(1 + i1); + } + + for (j = 0; j < 5; j++) { + c = j * 7; + iy = 0; + ix = c; + temp = (real32_T)fabs(b_A[c]); + for (k = 2; k <= 6 - j; k++) { + ix++; + s = (real32_T)fabs(b_A[ix]); + if (s > temp) { + iy = k - 1; + temp = s; + } + } + + if (b_A[c + iy] != 0.0F) { + if (iy != 0) { + ipiv[j] = (signed char)((j + iy) + 1); + ix = j; + iy += j; + for (k = 0; k < 6; k++) { + temp = b_A[ix]; + b_A[ix] = b_A[iy]; + b_A[iy] = temp; + ix += 6; + iy += 6; + } + } + + i1 = (c - j) + 6; + for (jy = c + 1; jy + 1 <= i1; jy++) { + b_A[jy] /= b_A[c]; + } + } + + iy = c; + jy = c + 6; + for (k = 1; k <= 5 - j; k++) { + temp = b_A[jy]; + if (b_A[jy] != 0.0F) { + ix = c + 1; + i1 = (iy - j) + 12; + for (ijA = 7 + iy; ijA + 1 <= i1; ijA++) { + b_A[ijA] += b_A[ix] * -temp; + ix++; + } + } + + jy += 6; + iy += 6; + } + } + + for (i1 = 0; i1 < 12; i1++) { + for (iy = 0; iy < 6; iy++) { + Y[iy + 6 * i1] = A[i1 + 12 * iy]; + } + } + + for (jy = 0; jy < 6; jy++) { + if (ipiv[jy] != jy + 1) { + for (j = 0; j < 12; j++) { + temp = Y[jy + 6 * j]; + Y[jy + 6 * j] = Y[(ipiv[jy] + 6 * j) - 1]; + Y[(ipiv[jy] + 6 * j) - 1] = temp; + } + } + } + + for (j = 0; j < 12; j++) { + c = 6 * j; + for (k = 0; k < 6; k++) { + iy = 6 * k; + if (Y[k + c] != 0.0F) { + for (jy = k + 2; jy < 7; jy++) { + Y[(jy + c) - 1] -= Y[k + c] * b_A[(jy + iy) - 1]; + } + } + } + } + + for (j = 0; j < 12; j++) { + c = 6 * j; + for (k = 5; k > -1; k += -1) { + iy = 6 * k; + if (Y[k + c] != 0.0F) { + Y[k + c] /= b_A[k + iy]; + for (jy = 0; jy + 1 <= k; jy++) { + Y[jy + c] -= Y[k + c] * b_A[jy + iy]; + } + } + } + } + + for (i1 = 0; i1 < 6; i1++) { + for (iy = 0; iy < 12; iy++) { + y[iy + 12 * i1] = Y[i1 + 6 * iy]; + } + } +} + +/* + * + */ +static void inv(const float x[9], float y[9]) +{ + float b_x[9]; + int p1; + int p2; + int p3; + float absx11; + float absx21; + float absx31; + int itmp; + for (p1 = 0; p1 < 9; p1++) { + b_x[p1] = x[p1]; + } + + p1 = 0; + p2 = 3; + p3 = 6; + absx11 = (real32_T)fabs(x[0]); + absx21 = (real32_T)fabs(x[1]); + absx31 = (real32_T)fabs(x[2]); + if ((absx21 > absx11) && (absx21 > absx31)) { + p1 = 3; + p2 = 0; + b_x[0] = x[1]; + b_x[1] = x[0]; + b_x[3] = x[4]; + b_x[4] = x[3]; + b_x[6] = x[7]; + b_x[7] = x[6]; + } else { + if (absx31 > absx11) { + p1 = 6; + p3 = 0; + b_x[0] = x[2]; + b_x[2] = x[0]; + b_x[3] = x[5]; + b_x[5] = x[3]; + b_x[6] = x[8]; + b_x[8] = x[6]; + } + } + + absx11 = b_x[1] / b_x[0]; + b_x[1] /= b_x[0]; + absx21 = b_x[2] / b_x[0]; + b_x[2] /= b_x[0]; + b_x[4] -= absx11 * b_x[3]; + b_x[5] -= absx21 * b_x[3]; + b_x[7] -= absx11 * b_x[6]; + b_x[8] -= absx21 * b_x[6]; + if ((real32_T)fabs(b_x[5]) > (real32_T)fabs(b_x[4])) { + itmp = p2; + p2 = p3; + p3 = itmp; + b_x[1] = absx21; + b_x[2] = absx11; + absx11 = b_x[4]; + b_x[4] = b_x[5]; + b_x[5] = absx11; + absx11 = b_x[7]; + b_x[7] = b_x[8]; + b_x[8] = absx11; + } + + absx11 = b_x[5] / b_x[4]; + b_x[5] /= b_x[4]; + b_x[8] -= absx11 * b_x[7]; + absx11 = (b_x[5] * b_x[1] - b_x[2]) / b_x[8]; + absx21 = -(b_x[1] + b_x[7] * absx11) / b_x[4]; + y[p1] = ((1.0F - b_x[3] * absx21) - b_x[6] * absx11) / b_x[0]; + y[p1 + 1] = absx21; + y[p1 + 2] = absx11; + absx11 = -b_x[5] / b_x[8]; + absx21 = (1.0F - b_x[7] * absx11) / b_x[4]; + y[p2] = -(b_x[3] * absx21 + b_x[6] * absx11) / b_x[0]; + y[p2 + 1] = absx21; + y[p2 + 2] = absx11; + absx11 = 1.0F / b_x[8]; + absx21 = -b_x[7] * absx11 / b_x[4]; + y[p3] = -(b_x[3] * absx21 + b_x[6] * absx11) / b_x[0]; + y[p3 + 1] = absx21; + y[p3 + 2] = absx11; +} + +/* + * + */ +static void mrdivide(const float A[108], const float B[81], float y[108]) +{ + float b_A[81]; + signed char ipiv[9]; + int i0; + int iy; + int j; + int c; + int ix; + float temp; + int k; + float s; + int jy; + int ijA; + float Y[108]; + for (i0 = 0; i0 < 9; i0++) { + for (iy = 0; iy < 9; iy++) { + b_A[iy + 9 * i0] = B[i0 + 9 * iy]; + } + + ipiv[i0] = (signed char)(1 + i0); + } + + for (j = 0; j < 8; j++) { + c = j * 10; + iy = 0; + ix = c; + temp = (real32_T)fabs(b_A[c]); + for (k = 2; k <= 9 - j; k++) { + ix++; + s = (real32_T)fabs(b_A[ix]); + if (s > temp) { + iy = k - 1; + temp = s; + } + } + + if (b_A[c + iy] != 0.0F) { + if (iy != 0) { + ipiv[j] = (signed char)((j + iy) + 1); + ix = j; + iy += j; + for (k = 0; k < 9; k++) { + temp = b_A[ix]; + b_A[ix] = b_A[iy]; + b_A[iy] = temp; + ix += 9; + iy += 9; + } + } + + i0 = (c - j) + 9; + for (jy = c + 1; jy + 1 <= i0; jy++) { + b_A[jy] /= b_A[c]; + } + } + + iy = c; + jy = c + 9; + for (k = 1; k <= 8 - j; k++) { + temp = b_A[jy]; + if (b_A[jy] != 0.0F) { + ix = c + 1; + i0 = (iy - j) + 18; + for (ijA = 10 + iy; ijA + 1 <= i0; ijA++) { + b_A[ijA] += b_A[ix] * -temp; + ix++; + } + } + + jy += 9; + iy += 9; + } + } + + for (i0 = 0; i0 < 12; i0++) { + for (iy = 0; iy < 9; iy++) { + Y[iy + 9 * i0] = A[i0 + 12 * iy]; + } + } + + for (jy = 0; jy < 9; jy++) { + if (ipiv[jy] != jy + 1) { + for (j = 0; j < 12; j++) { + temp = Y[jy + 9 * j]; + Y[jy + 9 * j] = Y[(ipiv[jy] + 9 * j) - 1]; + Y[(ipiv[jy] + 9 * j) - 1] = temp; + } + } + } + + for (j = 0; j < 12; j++) { + c = 9 * j; + for (k = 0; k < 9; k++) { + iy = 9 * k; + if (Y[k + c] != 0.0F) { + for (jy = k + 2; jy < 10; jy++) { + Y[(jy + c) - 1] -= Y[k + c] * b_A[(jy + iy) - 1]; + } + } + } + } + + for (j = 0; j < 12; j++) { + c = 9 * j; + for (k = 8; k > -1; k += -1) { + iy = 9 * k; + if (Y[k + c] != 0.0F) { + Y[k + c] /= b_A[k + iy]; + for (jy = 0; jy + 1 <= k; jy++) { + Y[jy + c] -= Y[k + c] * b_A[jy + iy]; + } + } + } + } + + for (i0 = 0; i0 < 9; i0++) { + for (iy = 0; iy < 12; iy++) { + y[iy + 12 * i0] = Y[i0 + 9 * iy]; + } + } +} + +/* + * + */ +static float norm(const float x[3]) +{ + float y; + float scale; + int k; + float absxk; + float t; + y = 0.0F; + scale = 1.17549435E-38F; + for (k = 0; k < 3; k++) { + absxk = (real32_T)fabs(x[k]); + if (absxk > scale) { + t = scale / absxk; + y = 1.0F + y * t * t; + scale = absxk; + } else { + t = absxk / scale; + y += t * t; + } + } + + return scale * (real32_T)sqrt(y); +} + +/* + * function [xa_apo,Pa_apo,Rot_matrix,eulerAngles,debugOutput]... + * = AttitudeEKF(approx_prediction,use_inertia_matrix,zFlag,dt,z,q_rotSpeed,q_rotAcc,q_acc,q_mag,r_gyro,r_accel,r_mag,J) + */ +void AttitudeEKF(unsigned char approx_prediction, unsigned char + use_inertia_matrix, const unsigned char zFlag[3], float dt, + const float z[9], float q_rotSpeed, float q_rotAcc, float q_acc, + float q_mag, float r_gyro, float r_accel, float r_mag, const + float J[9], float xa_apo[12], float Pa_apo[144], float + Rot_matrix[9], float eulerAngles[3], float debugOutput[4]) +{ + int i; + float fv0[3]; + int r2; + float zek[3]; + float muk[3]; + float b_muk[3]; + float c_muk[3]; + float fv1[3]; + float wak[3]; + float O[9]; + float b_O[9]; + static const signed char iv0[9] = { 1, 0, 0, 0, 1, 0, 0, 0, 1 }; + + float fv2[3]; + float maxval; + int r1; + float fv3[9]; + float fv4[3]; + float x_apr[12]; + signed char I[144]; + static float A_lin[144]; + static const signed char iv1[36] = { 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 1, + 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0 }; + + static float b_A_lin[144]; + float v[12]; + static float P_apr[144]; + float a[108]; + static const signed char b_a[108] = { 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, + 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, + 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0, + 0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, + 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1 }; + + float S_k[81]; + static const signed char b[108] = { 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, + 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, + 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, + 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, + 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1 }; + + float b_r_gyro[9]; + float K_k[108]; + float b_S_k[36]; + static const signed char c_a[36] = { 1, 0, 0, 0, 1, 0, 0, 0, 1, 0, 0, 0, 0, 0, + 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0 }; + + static const signed char b_b[36] = { 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, + 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0 }; + + float c_r_gyro[3]; + float B[36]; + int r3; + float a21; + float Y[36]; + float d_a[72]; + static const signed char e_a[72] = { 1, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, + 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, + 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, + 0, 0, 0, 0, 0, 0 }; + + static const signed char c_b[72] = { 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, + 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, + 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, + 0, 0, 1, 0, 0, 0 }; + + float d_r_gyro[6]; + float c_S_k[6]; + float b_K_k[72]; + static const signed char f_a[72] = { 1, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, + 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, + 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 1, 0, + 0, 0, 0, 0, 0, 1 }; + + static const signed char d_b[72] = { 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, + 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, + 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, + 0, 0, 0, 0, 0, 1 }; + + float b_z[6]; + + /* LQG Postion Estimator and Controller */ + /* Observer: */ + /* x[n|n] = x[n|n-1] + M(y[n] - Cx[n|n-1] - Du[n]) */ + /* x[n+1|n] = Ax[n|n] + Bu[n] */ + /* */ + /* $Author: Tobias Naegeli $ $Date: 2014 $ $Revision: 3 $ */ + /* */ + /* */ + /* Arguments: */ + /* approx_prediction: if 1 then the exponential map is approximated with a */ + /* first order taylor approximation. has at the moment not a big influence */ + /* (just 1st or 2nd order approximation) we should change it to rodriquez */ + /* approximation. */ + /* use_inertia_matrix: set to true if you have the inertia matrix J for your */ + /* quadrotor */ + /* xa_apo_k: old state vectotr */ + /* zFlag: if sensor measurement is available [gyro, acc, mag] */ + /* dt: dt in s */ + /* z: measurements [gyro, acc, mag] */ + /* q_rotSpeed: process noise gyro */ + /* q_rotAcc: process noise gyro acceleration */ + /* q_acc: process noise acceleration */ + /* q_mag: process noise magnetometer */ + /* r_gyro: measurement noise gyro */ + /* r_accel: measurement noise accel */ + /* r_mag: measurement noise mag */ + /* J: moment of inertia matrix */ + /* Output: */ + /* xa_apo: updated state vectotr */ + /* Pa_apo: updated state covariance matrix */ + /* Rot_matrix: rotation matrix */ + /* eulerAngles: euler angles */ + /* debugOutput: not used */ + /* % model specific parameters */ + /* compute once the inverse of the Inertia */ + /* 'AttitudeEKF:48' if isempty(Ji) */ + if (!Ji_not_empty) { + /* 'AttitudeEKF:49' Ji=single(inv(J)); */ + inv(J, Ji); + Ji_not_empty = TRUE; + } + + /* % init */ + /* 'AttitudeEKF:54' if(isempty(x_apo)) */ + /* 'AttitudeEKF:64' if(isempty(P_apo)) */ + /* 'AttitudeEKF:69' debugOutput = single(zeros(4,1)); */ + for (i = 0; i < 4; i++) { + debugOutput[i] = 0.0F; + } + + /* % copy the states */ + /* 'AttitudeEKF:72' wx= x_apo(1); */ + /* x body angular rate */ + /* 'AttitudeEKF:73' wy= x_apo(2); */ + /* y body angular rate */ + /* 'AttitudeEKF:74' wz= x_apo(3); */ + /* z body angular rate */ + /* 'AttitudeEKF:76' wax= x_apo(4); */ + /* x body angular acceleration */ + /* 'AttitudeEKF:77' way= x_apo(5); */ + /* y body angular acceleration */ + /* 'AttitudeEKF:78' waz= x_apo(6); */ + /* z body angular acceleration */ + /* 'AttitudeEKF:80' zex= x_apo(7); */ + /* x component gravity vector */ + /* 'AttitudeEKF:81' zey= x_apo(8); */ + /* y component gravity vector */ + /* 'AttitudeEKF:82' zez= x_apo(9); */ + /* z component gravity vector */ + /* 'AttitudeEKF:84' mux= x_apo(10); */ + /* x component magnetic field vector */ + /* 'AttitudeEKF:85' muy= x_apo(11); */ + /* y component magnetic field vector */ + /* 'AttitudeEKF:86' muz= x_apo(12); */ + /* z component magnetic field vector */ + /* % prediction section */ + /* compute the apriori state estimate from the previous aposteriori estimate */ + /* body angular accelerations */ + /* 'AttitudeEKF:94' if (use_inertia_matrix==1) */ + if (use_inertia_matrix == 1) { + /* 'AttitudeEKF:95' wak =[wax;way;waz]+Ji*(-cross([wax;way;waz],J*[wax;way;waz]))*dt; */ + fv0[0] = x_apo[3]; + fv0[1] = x_apo[4]; + fv0[2] = x_apo[5]; + for (r2 = 0; r2 < 3; r2++) { + zek[r2] = 0.0F; + for (i = 0; i < 3; i++) { + zek[r2] += J[r2 + 3 * i] * fv0[i]; + } + } + + muk[0] = x_apo[3]; + muk[1] = x_apo[4]; + muk[2] = x_apo[5]; + b_muk[0] = x_apo[4] * zek[2] - x_apo[5] * zek[1]; + b_muk[1] = x_apo[5] * zek[0] - x_apo[3] * zek[2]; + b_muk[2] = x_apo[3] * zek[1] - x_apo[4] * zek[0]; + for (r2 = 0; r2 < 3; r2++) { + c_muk[r2] = -b_muk[r2]; + } + + fv1[0] = x_apo[3]; + fv1[1] = x_apo[4]; + fv1[2] = x_apo[5]; + for (r2 = 0; r2 < 3; r2++) { + fv0[r2] = 0.0F; + for (i = 0; i < 3; i++) { + fv0[r2] += Ji[r2 + 3 * i] * c_muk[i]; + } + + wak[r2] = fv1[r2] + fv0[r2] * dt; + } + } else { + /* 'AttitudeEKF:96' else */ + /* 'AttitudeEKF:97' wak =[wax;way;waz]; */ + wak[0] = x_apo[3]; + wak[1] = x_apo[4]; + wak[2] = x_apo[5]; + } + + /* body angular rates */ + /* 'AttitudeEKF:101' wk =[wx; wy; wz] + dt*wak; */ + /* derivative of the prediction rotation matrix */ + /* 'AttitudeEKF:104' O=[0,-wz,wy;wz,0,-wx;-wy,wx,0]'; */ + O[0] = 0.0F; + O[1] = -x_apo[2]; + O[2] = x_apo[1]; + O[3] = x_apo[2]; + O[4] = 0.0F; + O[5] = -x_apo[0]; + O[6] = -x_apo[1]; + O[7] = x_apo[0]; + O[8] = 0.0F; + + /* prediction of the earth z vector */ + /* 'AttitudeEKF:107' if (approx_prediction==1) */ + if (approx_prediction == 1) { + /* e^(Odt)=I+dt*O+dt^2/2!O^2 */ + /* so we do a first order approximation of the exponential map */ + /* 'AttitudeEKF:110' zek =(O*dt+single(eye(3)))*[zex;zey;zez]; */ + for (r2 = 0; r2 < 3; r2++) { + for (i = 0; i < 3; i++) { + b_O[i + 3 * r2] = O[i + 3 * r2] * dt + (float)iv0[i + 3 * r2]; + } + } + + fv2[0] = x_apo[6]; + fv2[1] = x_apo[7]; + fv2[2] = x_apo[8]; + for (r2 = 0; r2 < 3; r2++) { + zek[r2] = 0.0F; + for (i = 0; i < 3; i++) { + zek[r2] += b_O[r2 + 3 * i] * fv2[i]; + } + } + } else { + /* 'AttitudeEKF:112' else */ + /* 'AttitudeEKF:113' zek =(single(eye(3))+O*dt+dt^2/2*O^2)*[zex;zey;zez]; */ + maxval = dt * dt / 2.0F; + for (r2 = 0; r2 < 3; r2++) { + for (i = 0; i < 3; i++) { + b_O[r2 + 3 * i] = 0.0F; + for (r1 = 0; r1 < 3; r1++) { + b_O[r2 + 3 * i] += O[r2 + 3 * r1] * O[r1 + 3 * i]; + } + } + } + + for (r2 = 0; r2 < 3; r2++) { + for (i = 0; i < 3; i++) { + fv3[i + 3 * r2] = ((float)iv0[i + 3 * r2] + O[i + 3 * r2] * dt) + maxval + * b_O[i + 3 * r2]; + } + } + + fv2[0] = x_apo[6]; + fv2[1] = x_apo[7]; + fv2[2] = x_apo[8]; + for (r2 = 0; r2 < 3; r2++) { + zek[r2] = 0.0F; + for (i = 0; i < 3; i++) { + zek[r2] += fv3[r2 + 3 * i] * fv2[i]; + } + } + + /* zek =expm2(O*dt)*[zex;zey;zez]; not working because use double */ + /* precision */ + } + + /* prediction of the magnetic vector */ + /* 'AttitudeEKF:121' if (approx_prediction==1) */ + if (approx_prediction == 1) { + /* e^(Odt)=I+dt*O+dt^2/2!O^2 */ + /* so we do a first order approximation of the exponential map */ + /* 'AttitudeEKF:124' muk =(O*dt+single(eye(3)))*[mux;muy;muz]; */ + for (r2 = 0; r2 < 3; r2++) { + for (i = 0; i < 3; i++) { + b_O[i + 3 * r2] = O[i + 3 * r2] * dt + (float)iv0[i + 3 * r2]; + } + } + + fv4[0] = x_apo[9]; + fv4[1] = x_apo[10]; + fv4[2] = x_apo[11]; + for (r2 = 0; r2 < 3; r2++) { + muk[r2] = 0.0F; + for (i = 0; i < 3; i++) { + muk[r2] += b_O[r2 + 3 * i] * fv4[i]; + } + } + } else { + /* 'AttitudeEKF:125' else */ + /* 'AttitudeEKF:126' muk =(single(eye(3))+O*dt+dt^2/2*O^2)*[mux;muy;muz]; */ + maxval = dt * dt / 2.0F; + for (r2 = 0; r2 < 3; r2++) { + for (i = 0; i < 3; i++) { + b_O[r2 + 3 * i] = 0.0F; + for (r1 = 0; r1 < 3; r1++) { + b_O[r2 + 3 * i] += O[r2 + 3 * r1] * O[r1 + 3 * i]; + } + } + } + + for (r2 = 0; r2 < 3; r2++) { + for (i = 0; i < 3; i++) { + fv3[i + 3 * r2] = ((float)iv0[i + 3 * r2] + O[i + 3 * r2] * dt) + maxval + * b_O[i + 3 * r2]; + } + } + + fv4[0] = x_apo[9]; + fv4[1] = x_apo[10]; + fv4[2] = x_apo[11]; + for (r2 = 0; r2 < 3; r2++) { + muk[r2] = 0.0F; + for (i = 0; i < 3; i++) { + muk[r2] += fv3[r2 + 3 * i] * fv4[i]; + } + } + + /* muk =expm2(O*dt)*[mux;muy;muz]; not working because use double */ + /* precision */ + } + + /* 'AttitudeEKF:131' x_apr=[wk;wak;zek;muk]; */ + x_apr[0] = x_apo[0] + dt * wak[0]; + x_apr[1] = x_apo[1] + dt * wak[1]; + x_apr[2] = x_apo[2] + dt * wak[2]; + for (i = 0; i < 3; i++) { + x_apr[i + 3] = wak[i]; + } + + for (i = 0; i < 3; i++) { + x_apr[i + 6] = zek[i]; + } + + for (i = 0; i < 3; i++) { + x_apr[i + 9] = muk[i]; + } + + /* compute the apriori error covariance estimate from the previous */ + /* aposteriori estimate */ + /* 'AttitudeEKF:136' EZ=[0,zez,-zey; */ + /* 'AttitudeEKF:137' -zez,0,zex; */ + /* 'AttitudeEKF:138' zey,-zex,0]'; */ + /* 'AttitudeEKF:139' MA=[0,muz,-muy; */ + /* 'AttitudeEKF:140' -muz,0,mux; */ + /* 'AttitudeEKF:141' muy,-mux,0]'; */ + /* 'AttitudeEKF:143' E=single(eye(3)); */ + /* 'AttitudeEKF:144' Z=single(zeros(3)); */ + /* 'AttitudeEKF:146' A_lin=[ Z, E, Z, Z */ + /* 'AttitudeEKF:147' Z, Z, Z, Z */ + /* 'AttitudeEKF:148' EZ, Z, O, Z */ + /* 'AttitudeEKF:149' MA, Z, Z, O]; */ + /* 'AttitudeEKF:151' A_lin=eye(12)+A_lin*dt; */ + memset(&I[0], 0, 144U * sizeof(signed char)); + for (i = 0; i < 12; i++) { + I[i + 12 * i] = 1; + for (r2 = 0; r2 < 3; r2++) { + A_lin[r2 + 12 * i] = iv1[r2 + 3 * i]; + } + + for (r2 = 0; r2 < 3; r2++) { + A_lin[(r2 + 12 * i) + 3] = 0.0F; + } + } + + A_lin[6] = 0.0F; + A_lin[7] = x_apo[8]; + A_lin[8] = -x_apo[7]; + A_lin[18] = -x_apo[8]; + A_lin[19] = 0.0F; + A_lin[20] = x_apo[6]; + A_lin[30] = x_apo[7]; + A_lin[31] = -x_apo[6]; + A_lin[32] = 0.0F; + for (r2 = 0; r2 < 3; r2++) { + for (i = 0; i < 3; i++) { + A_lin[(i + 12 * (r2 + 3)) + 6] = 0.0F; + } + } + + for (r2 = 0; r2 < 3; r2++) { + for (i = 0; i < 3; i++) { + A_lin[(i + 12 * (r2 + 6)) + 6] = O[i + 3 * r2]; + } + } + + for (r2 = 0; r2 < 3; r2++) { + for (i = 0; i < 3; i++) { + A_lin[(i + 12 * (r2 + 9)) + 6] = 0.0F; + } + } + + A_lin[9] = 0.0F; + A_lin[10] = x_apo[11]; + A_lin[11] = -x_apo[10]; + A_lin[21] = -x_apo[11]; + A_lin[22] = 0.0F; + A_lin[23] = x_apo[9]; + A_lin[33] = x_apo[10]; + A_lin[34] = -x_apo[9]; + A_lin[35] = 0.0F; + for (r2 = 0; r2 < 3; r2++) { + for (i = 0; i < 3; i++) { + A_lin[(i + 12 * (r2 + 3)) + 9] = 0.0F; + } + } + + for (r2 = 0; r2 < 3; r2++) { + for (i = 0; i < 3; i++) { + A_lin[(i + 12 * (r2 + 6)) + 9] = 0.0F; + } + } + + for (r2 = 0; r2 < 3; r2++) { + for (i = 0; i < 3; i++) { + A_lin[(i + 12 * (r2 + 9)) + 9] = O[i + 3 * r2]; + } + } + + for (r2 = 0; r2 < 12; r2++) { + for (i = 0; i < 12; i++) { + b_A_lin[i + 12 * r2] = (float)I[i + 12 * r2] + A_lin[i + 12 * r2] * dt; + } + } + + /* process covariance matrix */ + /* 'AttitudeEKF:156' if (isempty(Q)) */ + if (!Q_not_empty) { + /* 'AttitudeEKF:157' Q=diag([ q_rotSpeed,q_rotSpeed,q_rotSpeed,... */ + /* 'AttitudeEKF:158' q_rotAcc,q_rotAcc,q_rotAcc,... */ + /* 'AttitudeEKF:159' q_acc,q_acc,q_acc,... */ + /* 'AttitudeEKF:160' q_mag,q_mag,q_mag]); */ + v[0] = q_rotSpeed; + v[1] = q_rotSpeed; + v[2] = q_rotSpeed; + v[3] = q_rotAcc; + v[4] = q_rotAcc; + v[5] = q_rotAcc; + v[6] = q_acc; + v[7] = q_acc; + v[8] = q_acc; + v[9] = q_mag; + v[10] = q_mag; + v[11] = q_mag; + memset(&Q[0], 0, 144U * sizeof(float)); + for (i = 0; i < 12; i++) { + Q[i + 12 * i] = v[i]; + } + + Q_not_empty = TRUE; + } + + /* 'AttitudeEKF:163' P_apr=A_lin*P_apo*A_lin'+Q; */ + for (r2 = 0; r2 < 12; r2++) { + for (i = 0; i < 12; i++) { + A_lin[r2 + 12 * i] = 0.0F; + for (r1 = 0; r1 < 12; r1++) { + A_lin[r2 + 12 * i] += b_A_lin[r2 + 12 * r1] * P_apo[r1 + 12 * i]; + } + } + } + + for (r2 = 0; r2 < 12; r2++) { + for (i = 0; i < 12; i++) { + maxval = 0.0F; + for (r1 = 0; r1 < 12; r1++) { + maxval += A_lin[r2 + 12 * r1] * b_A_lin[i + 12 * r1]; + } + + P_apr[r2 + 12 * i] = maxval + Q[r2 + 12 * i]; + } + } + + /* % update */ + /* 'AttitudeEKF:167' if zFlag(1)==1&&zFlag(2)==1&&zFlag(3)==1 */ + if ((zFlag[0] == 1) && (zFlag[1] == 1) && (zFlag[2] == 1)) { + /* R=[r_gyro,0,0,0,0,0,0,0,0; */ + /* 0,r_gyro,0,0,0,0,0,0,0; */ + /* 0,0,r_gyro,0,0,0,0,0,0; */ + /* 0,0,0,r_accel,0,0,0,0,0; */ + /* 0,0,0,0,r_accel,0,0,0,0; */ + /* 0,0,0,0,0,r_accel,0,0,0; */ + /* 0,0,0,0,0,0,r_mag,0,0; */ + /* 0,0,0,0,0,0,0,r_mag,0; */ + /* 0,0,0,0,0,0,0,0,r_mag]; */ + /* 'AttitudeEKF:178' R_v=[r_gyro,r_gyro,r_gyro,r_accel,r_accel,r_accel,r_mag,r_mag,r_mag]; */ + /* observation matrix */ + /* [zw;ze;zmk]; */ + /* 'AttitudeEKF:181' H_k=[ E, Z, Z, Z; */ + /* 'AttitudeEKF:182' Z, Z, E, Z; */ + /* 'AttitudeEKF:183' Z, Z, Z, E]; */ + /* 'AttitudeEKF:185' y_k=z(1:9)-H_k*x_apr; */ + /* S_k=H_k*P_apr*H_k'+R; */ + /* 'AttitudeEKF:189' S_k=H_k*P_apr*H_k'; */ + for (r2 = 0; r2 < 9; r2++) { + for (i = 0; i < 12; i++) { + a[r2 + 9 * i] = 0.0F; + for (r1 = 0; r1 < 12; r1++) { + a[r2 + 9 * i] += (float)b_a[r2 + 9 * r1] * P_apr[r1 + 12 * i]; + } + } + + for (i = 0; i < 9; i++) { + S_k[r2 + 9 * i] = 0.0F; + for (r1 = 0; r1 < 12; r1++) { + S_k[r2 + 9 * i] += a[r2 + 9 * r1] * (float)b[r1 + 12 * i]; + } + } + } + + /* 'AttitudeEKF:190' S_k(1:9+1:end) = S_k(1:9+1:end) + R_v; */ + b_r_gyro[0] = r_gyro; + b_r_gyro[1] = r_gyro; + b_r_gyro[2] = r_gyro; + b_r_gyro[3] = r_accel; + b_r_gyro[4] = r_accel; + b_r_gyro[5] = r_accel; + b_r_gyro[6] = r_mag; + b_r_gyro[7] = r_mag; + b_r_gyro[8] = r_mag; + for (r2 = 0; r2 < 9; r2++) { + b_O[r2] = S_k[10 * r2] + b_r_gyro[r2]; + } + + for (r2 = 0; r2 < 9; r2++) { + S_k[10 * r2] = b_O[r2]; + } + + /* 'AttitudeEKF:191' K_k=(P_apr*H_k'/(S_k)); */ + for (r2 = 0; r2 < 12; r2++) { + for (i = 0; i < 9; i++) { + a[r2 + 12 * i] = 0.0F; + for (r1 = 0; r1 < 12; r1++) { + a[r2 + 12 * i] += P_apr[r2 + 12 * r1] * (float)b[r1 + 12 * i]; + } + } + } + + mrdivide(a, S_k, K_k); + + /* 'AttitudeEKF:194' x_apo=x_apr+K_k*y_k; */ + for (r2 = 0; r2 < 9; r2++) { + maxval = 0.0F; + for (i = 0; i < 12; i++) { + maxval += (float)b_a[r2 + 9 * i] * x_apr[i]; + } + + b_O[r2] = z[r2] - maxval; + } + + for (r2 = 0; r2 < 12; r2++) { + maxval = 0.0F; + for (i = 0; i < 9; i++) { + maxval += K_k[r2 + 12 * i] * b_O[i]; + } + + x_apo[r2] = x_apr[r2] + maxval; + } + + /* 'AttitudeEKF:195' P_apo=(eye(12)-K_k*H_k)*P_apr; */ + memset(&I[0], 0, 144U * sizeof(signed char)); + for (i = 0; i < 12; i++) { + I[i + 12 * i] = 1; + } + + for (r2 = 0; r2 < 12; r2++) { + for (i = 0; i < 12; i++) { + maxval = 0.0F; + for (r1 = 0; r1 < 9; r1++) { + maxval += K_k[r2 + 12 * r1] * (float)b_a[r1 + 9 * i]; + } + + A_lin[r2 + 12 * i] = (float)I[r2 + 12 * i] - maxval; + } + } + + for (r2 = 0; r2 < 12; r2++) { + for (i = 0; i < 12; i++) { + P_apo[r2 + 12 * i] = 0.0F; + for (r1 = 0; r1 < 12; r1++) { + P_apo[r2 + 12 * i] += A_lin[r2 + 12 * r1] * P_apr[r1 + 12 * i]; + } + } + } + } else { + /* 'AttitudeEKF:196' else */ + /* 'AttitudeEKF:197' if zFlag(1)==1&&zFlag(2)==0&&zFlag(3)==0 */ + if ((zFlag[0] == 1) && (zFlag[1] == 0) && (zFlag[2] == 0)) { + /* 'AttitudeEKF:199' R=[r_gyro,0,0; */ + /* 'AttitudeEKF:200' 0,r_gyro,0; */ + /* 'AttitudeEKF:201' 0,0,r_gyro]; */ + /* 'AttitudeEKF:202' R_v=[r_gyro,r_gyro,r_gyro]; */ + /* observation matrix */ + /* 'AttitudeEKF:205' H_k=[ E, Z, Z, Z]; */ + /* 'AttitudeEKF:207' y_k=z(1:3)-H_k(1:3,1:12)*x_apr; */ + /* S_k=H_k(1:3,1:12)*P_apr*H_k(1:3,1:12)'+R(1:3,1:3); */ + /* 'AttitudeEKF:210' S_k=H_k(1:3,1:12)*P_apr*H_k(1:3,1:12)'; */ + for (r2 = 0; r2 < 3; r2++) { + for (i = 0; i < 12; i++) { + b_S_k[r2 + 3 * i] = 0.0F; + for (r1 = 0; r1 < 12; r1++) { + b_S_k[r2 + 3 * i] += (float)c_a[r2 + 3 * r1] * P_apr[r1 + 12 * i]; + } + } + + for (i = 0; i < 3; i++) { + O[r2 + 3 * i] = 0.0F; + for (r1 = 0; r1 < 12; r1++) { + O[r2 + 3 * i] += b_S_k[r2 + 3 * r1] * (float)b_b[r1 + 12 * i]; + } + } + } + + /* 'AttitudeEKF:211' S_k(1:3+1:end) = S_k(1:3+1:end) + R_v; */ + c_r_gyro[0] = r_gyro; + c_r_gyro[1] = r_gyro; + c_r_gyro[2] = r_gyro; + for (r2 = 0; r2 < 3; r2++) { + b_muk[r2] = O[r2 << 2] + c_r_gyro[r2]; + } + + for (r2 = 0; r2 < 3; r2++) { + O[r2 << 2] = b_muk[r2]; + } + + /* 'AttitudeEKF:212' K_k=(P_apr*H_k(1:3,1:12)'/(S_k)); */ + for (r2 = 0; r2 < 3; r2++) { + for (i = 0; i < 3; i++) { + b_O[i + 3 * r2] = O[r2 + 3 * i]; + } + + for (i = 0; i < 12; i++) { + B[r2 + 3 * i] = 0.0F; + for (r1 = 0; r1 < 12; r1++) { + B[r2 + 3 * i] += P_apr[i + 12 * r1] * (float)b_b[r1 + 12 * r2]; + } + } + } + + r1 = 0; + r2 = 1; + r3 = 2; + maxval = (real32_T)fabs(b_O[0]); + a21 = (real32_T)fabs(b_O[1]); + if (a21 > maxval) { + maxval = a21; + r1 = 1; + r2 = 0; + } + + if ((real32_T)fabs(b_O[2]) > maxval) { + r1 = 2; + r2 = 1; + r3 = 0; + } + + b_O[r2] /= b_O[r1]; + b_O[r3] /= b_O[r1]; + b_O[3 + r2] -= b_O[r2] * b_O[3 + r1]; + b_O[3 + r3] -= b_O[r3] * b_O[3 + r1]; + b_O[6 + r2] -= b_O[r2] * b_O[6 + r1]; + b_O[6 + r3] -= b_O[r3] * b_O[6 + r1]; + if ((real32_T)fabs(b_O[3 + r3]) > (real32_T)fabs(b_O[3 + r2])) { + i = r2; + r2 = r3; + r3 = i; + } + + b_O[3 + r3] /= b_O[3 + r2]; + b_O[6 + r3] -= b_O[3 + r3] * b_O[6 + r2]; + for (i = 0; i < 12; i++) { + Y[3 * i] = B[r1 + 3 * i]; + Y[1 + 3 * i] = B[r2 + 3 * i] - Y[3 * i] * b_O[r2]; + Y[2 + 3 * i] = (B[r3 + 3 * i] - Y[3 * i] * b_O[r3]) - Y[1 + 3 * i] * + b_O[3 + r3]; + Y[2 + 3 * i] /= b_O[6 + r3]; + Y[3 * i] -= Y[2 + 3 * i] * b_O[6 + r1]; + Y[1 + 3 * i] -= Y[2 + 3 * i] * b_O[6 + r2]; + Y[1 + 3 * i] /= b_O[3 + r2]; + Y[3 * i] -= Y[1 + 3 * i] * b_O[3 + r1]; + Y[3 * i] /= b_O[r1]; + } + + for (r2 = 0; r2 < 3; r2++) { + for (i = 0; i < 12; i++) { + b_S_k[i + 12 * r2] = Y[r2 + 3 * i]; + } + } + + /* 'AttitudeEKF:215' x_apo=x_apr+K_k*y_k; */ + for (r2 = 0; r2 < 3; r2++) { + maxval = 0.0F; + for (i = 0; i < 12; i++) { + maxval += (float)c_a[r2 + 3 * i] * x_apr[i]; + } + + b_muk[r2] = z[r2] - maxval; + } + + for (r2 = 0; r2 < 12; r2++) { + maxval = 0.0F; + for (i = 0; i < 3; i++) { + maxval += b_S_k[r2 + 12 * i] * b_muk[i]; + } + + x_apo[r2] = x_apr[r2] + maxval; + } + + /* 'AttitudeEKF:216' P_apo=(eye(12)-K_k*H_k(1:3,1:12))*P_apr; */ + memset(&I[0], 0, 144U * sizeof(signed char)); + for (i = 0; i < 12; i++) { + I[i + 12 * i] = 1; + } + + for (r2 = 0; r2 < 12; r2++) { + for (i = 0; i < 12; i++) { + maxval = 0.0F; + for (r1 = 0; r1 < 3; r1++) { + maxval += b_S_k[r2 + 12 * r1] * (float)c_a[r1 + 3 * i]; + } + + A_lin[r2 + 12 * i] = (float)I[r2 + 12 * i] - maxval; + } + } + + for (r2 = 0; r2 < 12; r2++) { + for (i = 0; i < 12; i++) { + P_apo[r2 + 12 * i] = 0.0F; + for (r1 = 0; r1 < 12; r1++) { + P_apo[r2 + 12 * i] += A_lin[r2 + 12 * r1] * P_apr[r1 + 12 * i]; + } + } + } + } else { + /* 'AttitudeEKF:217' else */ + /* 'AttitudeEKF:218' if zFlag(1)==1&&zFlag(2)==1&&zFlag(3)==0 */ + if ((zFlag[0] == 1) && (zFlag[1] == 1) && (zFlag[2] == 0)) { + /* R=[r_gyro,0,0,0,0,0; */ + /* 0,r_gyro,0,0,0,0; */ + /* 0,0,r_gyro,0,0,0; */ + /* 0,0,0,r_accel,0,0; */ + /* 0,0,0,0,r_accel,0; */ + /* 0,0,0,0,0,r_accel]; */ + /* 'AttitudeEKF:227' R_v=[r_gyro,r_gyro,r_gyro,r_accel,r_accel,r_accel]; */ + /* observation matrix */ + /* 'AttitudeEKF:230' H_k=[ E, Z, Z, Z; */ + /* 'AttitudeEKF:231' Z, Z, E, Z]; */ + /* 'AttitudeEKF:233' y_k=z(1:6)-H_k(1:6,1:12)*x_apr; */ + /* S_k=H_k(1:6,1:12)*P_apr*H_k(1:6,1:12)'+R(1:6,1:6); */ + /* 'AttitudeEKF:236' S_k=H_k(1:6,1:12)*P_apr*H_k(1:6,1:12)'; */ + for (r2 = 0; r2 < 6; r2++) { + for (i = 0; i < 12; i++) { + d_a[r2 + 6 * i] = 0.0F; + for (r1 = 0; r1 < 12; r1++) { + d_a[r2 + 6 * i] += (float)e_a[r2 + 6 * r1] * P_apr[r1 + 12 * i]; + } + } + + for (i = 0; i < 6; i++) { + b_S_k[r2 + 6 * i] = 0.0F; + for (r1 = 0; r1 < 12; r1++) { + b_S_k[r2 + 6 * i] += d_a[r2 + 6 * r1] * (float)c_b[r1 + 12 * i]; + } + } + } + + /* 'AttitudeEKF:237' S_k(1:6+1:end) = S_k(1:6+1:end) + R_v; */ + d_r_gyro[0] = r_gyro; + d_r_gyro[1] = r_gyro; + d_r_gyro[2] = r_gyro; + d_r_gyro[3] = r_accel; + d_r_gyro[4] = r_accel; + d_r_gyro[5] = r_accel; + for (r2 = 0; r2 < 6; r2++) { + c_S_k[r2] = b_S_k[7 * r2] + d_r_gyro[r2]; + } + + for (r2 = 0; r2 < 6; r2++) { + b_S_k[7 * r2] = c_S_k[r2]; + } + + /* 'AttitudeEKF:238' K_k=(P_apr*H_k(1:6,1:12)'/(S_k)); */ + for (r2 = 0; r2 < 12; r2++) { + for (i = 0; i < 6; i++) { + d_a[r2 + 12 * i] = 0.0F; + for (r1 = 0; r1 < 12; r1++) { + d_a[r2 + 12 * i] += P_apr[r2 + 12 * r1] * (float)c_b[r1 + 12 * i]; + } + } + } + + b_mrdivide(d_a, b_S_k, b_K_k); + + /* 'AttitudeEKF:241' x_apo=x_apr+K_k*y_k; */ + for (r2 = 0; r2 < 6; r2++) { + maxval = 0.0F; + for (i = 0; i < 12; i++) { + maxval += (float)e_a[r2 + 6 * i] * x_apr[i]; + } + + d_r_gyro[r2] = z[r2] - maxval; + } + + for (r2 = 0; r2 < 12; r2++) { + maxval = 0.0F; + for (i = 0; i < 6; i++) { + maxval += b_K_k[r2 + 12 * i] * d_r_gyro[i]; + } + + x_apo[r2] = x_apr[r2] + maxval; + } + + /* 'AttitudeEKF:242' P_apo=(eye(12)-K_k*H_k(1:6,1:12))*P_apr; */ + memset(&I[0], 0, 144U * sizeof(signed char)); + for (i = 0; i < 12; i++) { + I[i + 12 * i] = 1; + } + + for (r2 = 0; r2 < 12; r2++) { + for (i = 0; i < 12; i++) { + maxval = 0.0F; + for (r1 = 0; r1 < 6; r1++) { + maxval += b_K_k[r2 + 12 * r1] * (float)e_a[r1 + 6 * i]; + } + + A_lin[r2 + 12 * i] = (float)I[r2 + 12 * i] - maxval; + } + } + + for (r2 = 0; r2 < 12; r2++) { + for (i = 0; i < 12; i++) { + P_apo[r2 + 12 * i] = 0.0F; + for (r1 = 0; r1 < 12; r1++) { + P_apo[r2 + 12 * i] += A_lin[r2 + 12 * r1] * P_apr[r1 + 12 * i]; + } + } + } + } else { + /* 'AttitudeEKF:243' else */ + /* 'AttitudeEKF:244' if zFlag(1)==1&&zFlag(2)==0&&zFlag(3)==1 */ + if ((zFlag[0] == 1) && (zFlag[1] == 0) && (zFlag[2] == 1)) { + /* R=[r_gyro,0,0,0,0,0; */ + /* 0,r_gyro,0,0,0,0; */ + /* 0,0,r_gyro,0,0,0; */ + /* 0,0,0,r_mag,0,0; */ + /* 0,0,0,0,r_mag,0; */ + /* 0,0,0,0,0,r_mag]; */ + /* 'AttitudeEKF:251' R_v=[r_gyro,r_gyro,r_gyro,r_mag,r_mag,r_mag]; */ + /* observation matrix */ + /* 'AttitudeEKF:254' H_k=[ E, Z, Z, Z; */ + /* 'AttitudeEKF:255' Z, Z, Z, E]; */ + /* 'AttitudeEKF:257' y_k=[z(1:3);z(7:9)]-H_k(1:6,1:12)*x_apr; */ + /* S_k=H_k(1:6,1:12)*P_apr*H_k(1:6,1:12)'+R(1:6,1:6); */ + /* 'AttitudeEKF:260' S_k=H_k(1:6,1:12)*P_apr*H_k(1:6,1:12)'; */ + for (r2 = 0; r2 < 6; r2++) { + for (i = 0; i < 12; i++) { + d_a[r2 + 6 * i] = 0.0F; + for (r1 = 0; r1 < 12; r1++) { + d_a[r2 + 6 * i] += (float)f_a[r2 + 6 * r1] * P_apr[r1 + 12 * i]; + } + } + + for (i = 0; i < 6; i++) { + b_S_k[r2 + 6 * i] = 0.0F; + for (r1 = 0; r1 < 12; r1++) { + b_S_k[r2 + 6 * i] += d_a[r2 + 6 * r1] * (float)d_b[r1 + 12 * i]; + } + } + } + + /* 'AttitudeEKF:261' S_k(1:6+1:end) = S_k(1:6+1:end) + R_v; */ + d_r_gyro[0] = r_gyro; + d_r_gyro[1] = r_gyro; + d_r_gyro[2] = r_gyro; + d_r_gyro[3] = r_mag; + d_r_gyro[4] = r_mag; + d_r_gyro[5] = r_mag; + for (r2 = 0; r2 < 6; r2++) { + c_S_k[r2] = b_S_k[7 * r2] + d_r_gyro[r2]; + } + + for (r2 = 0; r2 < 6; r2++) { + b_S_k[7 * r2] = c_S_k[r2]; + } + + /* 'AttitudeEKF:262' K_k=(P_apr*H_k(1:6,1:12)'/(S_k)); */ + for (r2 = 0; r2 < 12; r2++) { + for (i = 0; i < 6; i++) { + d_a[r2 + 12 * i] = 0.0F; + for (r1 = 0; r1 < 12; r1++) { + d_a[r2 + 12 * i] += P_apr[r2 + 12 * r1] * (float)d_b[r1 + 12 * i]; + } + } + } + + b_mrdivide(d_a, b_S_k, b_K_k); + + /* 'AttitudeEKF:265' x_apo=x_apr+K_k*y_k; */ + for (r2 = 0; r2 < 3; r2++) { + d_r_gyro[r2] = z[r2]; + } + + for (r2 = 0; r2 < 3; r2++) { + d_r_gyro[r2 + 3] = z[6 + r2]; + } + + for (r2 = 0; r2 < 6; r2++) { + c_S_k[r2] = 0.0F; + for (i = 0; i < 12; i++) { + c_S_k[r2] += (float)f_a[r2 + 6 * i] * x_apr[i]; + } + + b_z[r2] = d_r_gyro[r2] - c_S_k[r2]; + } + + for (r2 = 0; r2 < 12; r2++) { + maxval = 0.0F; + for (i = 0; i < 6; i++) { + maxval += b_K_k[r2 + 12 * i] * b_z[i]; + } + + x_apo[r2] = x_apr[r2] + maxval; + } + + /* 'AttitudeEKF:266' P_apo=(eye(12)-K_k*H_k(1:6,1:12))*P_apr; */ + memset(&I[0], 0, 144U * sizeof(signed char)); + for (i = 0; i < 12; i++) { + I[i + 12 * i] = 1; + } + + for (r2 = 0; r2 < 12; r2++) { + for (i = 0; i < 12; i++) { + maxval = 0.0F; + for (r1 = 0; r1 < 6; r1++) { + maxval += b_K_k[r2 + 12 * r1] * (float)f_a[r1 + 6 * i]; + } + + A_lin[r2 + 12 * i] = (float)I[r2 + 12 * i] - maxval; + } + } + + for (r2 = 0; r2 < 12; r2++) { + for (i = 0; i < 12; i++) { + P_apo[r2 + 12 * i] = 0.0F; + for (r1 = 0; r1 < 12; r1++) { + P_apo[r2 + 12 * i] += A_lin[r2 + 12 * r1] * P_apr[r1 + 12 * i]; + } + } + } + } else { + /* 'AttitudeEKF:267' else */ + /* 'AttitudeEKF:268' x_apo=x_apr; */ + for (i = 0; i < 12; i++) { + x_apo[i] = x_apr[i]; + } + + /* 'AttitudeEKF:269' P_apo=P_apr; */ + memcpy(&P_apo[0], &P_apr[0], 144U * sizeof(float)); + } + } + } + } + + /* % euler anglels extraction */ + /* 'AttitudeEKF:278' z_n_b = -x_apo(7:9)./norm(x_apo(7:9)); */ + maxval = norm(*(float (*)[3])&x_apo[6]); + a21 = norm(*(float (*)[3])&x_apo[9]); + for (i = 0; i < 3; i++) { + /* 'AttitudeEKF:279' m_n_b = x_apo(10:12)./norm(x_apo(10:12)); */ + muk[i] = -x_apo[i + 6] / maxval; + zek[i] = x_apo[i + 9] / a21; + } + + /* 'AttitudeEKF:281' y_n_b=cross(z_n_b,m_n_b); */ + wak[0] = muk[1] * zek[2] - muk[2] * zek[1]; + wak[1] = muk[2] * zek[0] - muk[0] * zek[2]; + wak[2] = muk[0] * zek[1] - muk[1] * zek[0]; + + /* 'AttitudeEKF:282' y_n_b=y_n_b./norm(y_n_b); */ + maxval = norm(wak); + for (r2 = 0; r2 < 3; r2++) { + wak[r2] /= maxval; + } + + /* 'AttitudeEKF:284' x_n_b=(cross(y_n_b,z_n_b)); */ + zek[0] = wak[1] * muk[2] - wak[2] * muk[1]; + zek[1] = wak[2] * muk[0] - wak[0] * muk[2]; + zek[2] = wak[0] * muk[1] - wak[1] * muk[0]; + + /* 'AttitudeEKF:285' x_n_b=x_n_b./norm(x_n_b); */ + maxval = norm(zek); + for (r2 = 0; r2 < 3; r2++) { + zek[r2] /= maxval; + } + + /* 'AttitudeEKF:288' xa_apo=x_apo; */ + for (i = 0; i < 12; i++) { + xa_apo[i] = x_apo[i]; + } + + /* 'AttitudeEKF:289' Pa_apo=P_apo; */ + memcpy(&Pa_apo[0], &P_apo[0], 144U * sizeof(float)); + + /* rotation matrix from earth to body system */ + /* 'AttitudeEKF:291' Rot_matrix=[x_n_b,y_n_b,z_n_b]; */ + for (r2 = 0; r2 < 3; r2++) { + Rot_matrix[r2] = zek[r2]; + Rot_matrix[3 + r2] = wak[r2]; + Rot_matrix[6 + r2] = muk[r2]; + } + + /* 'AttitudeEKF:294' phi=atan2(Rot_matrix(2,3),Rot_matrix(3,3)); */ + /* 'AttitudeEKF:295' theta=-asin(Rot_matrix(1,3)); */ + /* 'AttitudeEKF:296' psi=atan2(Rot_matrix(1,2),Rot_matrix(1,1)); */ + /* 'AttitudeEKF:297' eulerAngles=[phi;theta;psi]; */ + eulerAngles[0] = (real32_T)atan2(Rot_matrix[7], Rot_matrix[8]); + eulerAngles[1] = -(real32_T)asin(Rot_matrix[6]); + eulerAngles[2] = (real32_T)atan2(Rot_matrix[3], Rot_matrix[0]); +} + +void AttitudeEKF_initialize(void) +{ + Q_not_empty = FALSE; + Ji_not_empty = FALSE; + AttitudeEKF_init(); +} + +void AttitudeEKF_terminate(void) +{ + /* (no terminate code required) */ +} + +/* End of code generation (AttitudeEKF.c) */ diff --git a/src/modules/attitude_estimator_ekf/codegen/AttitudeEKF.h b/src/modules/attitude_estimator_ekf/codegen/AttitudeEKF.h new file mode 100644 index 000000000..7094da1da --- /dev/null +++ b/src/modules/attitude_estimator_ekf/codegen/AttitudeEKF.h @@ -0,0 +1,26 @@ +/* + * AttitudeEKF.h + * + * Code generation for function 'AttitudeEKF' + * + * C source code generated on: Thu Aug 21 11:17:28 2014 + * + */ + +#ifndef __ATTITUDEEKF_H__ +#define __ATTITUDEEKF_H__ +/* Include files */ +#include <math.h> +#include <stddef.h> +#include <stdlib.h> +#include <string.h> + +#include "rtwtypes.h" +#include "AttitudeEKF_types.h" + +/* Function Declarations */ +extern void AttitudeEKF(unsigned char approx_prediction, unsigned char use_inertia_matrix, const unsigned char zFlag[3], float dt, const float z[9], float q_rotSpeed, float q_rotAcc, float q_acc, float q_mag, float r_gyro, float r_accel, float r_mag, const float J[9], float xa_apo[12], float Pa_apo[144], float Rot_matrix[9], float eulerAngles[3], float debugOutput[4]); +extern void AttitudeEKF_initialize(void); +extern void AttitudeEKF_terminate(void); +#endif +/* End of code generation (AttitudeEKF.h) */ diff --git a/src/modules/attitude_estimator_ekf/codegen/AttitudeEKF_types.h b/src/modules/attitude_estimator_ekf/codegen/AttitudeEKF_types.h new file mode 100644 index 000000000..3f7522ffa --- /dev/null +++ b/src/modules/attitude_estimator_ekf/codegen/AttitudeEKF_types.h @@ -0,0 +1,17 @@ +/* + * AttitudeEKF_types.h + * + * Code generation for function 'AttitudeEKF' + * + * C source code generated on: Thu Aug 21 11:17:28 2014 + * + */ + +#ifndef __ATTITUDEEKF_TYPES_H__ +#define __ATTITUDEEKF_TYPES_H__ + +/* Include files */ +#include "rtwtypes.h" + +#endif +/* End of code generation (AttitudeEKF_types.h) */ diff --git a/src/modules/attitude_estimator_ekf/codegen/attitudeKalmanfilter.c b/src/modules/attitude_estimator_ekf/codegen/attitudeKalmanfilter.c deleted file mode 100755 index 9e97ad11a..000000000 --- a/src/modules/attitude_estimator_ekf/codegen/attitudeKalmanfilter.c +++ /dev/null @@ -1,1148 +0,0 @@ -/*
- * attitudeKalmanfilter.c
- *
- * Code generation for function 'attitudeKalmanfilter'
- *
- * C source code generated on: Sat Jan 19 15:25:29 2013
- *
- */
-
-/* Include files */
-#include "rt_nonfinite.h"
-#include "attitudeKalmanfilter.h"
-#include "rdivide.h"
-#include "norm.h"
-#include "cross.h"
-#include "eye.h"
-#include "mrdivide.h"
-
-/* Type Definitions */
-
-/* Named Constants */
-
-/* Variable Declarations */
-
-/* Variable Definitions */
-
-/* Function Declarations */
-static real32_T rt_atan2f_snf(real32_T u0, real32_T u1);
-
-/* Function Definitions */
-static real32_T rt_atan2f_snf(real32_T u0, real32_T u1)
-{
- real32_T y;
- int32_T b_u0;
- int32_T b_u1;
- if (rtIsNaNF(u0) || rtIsNaNF(u1)) {
- y = ((real32_T)rtNaN);
- } else if (rtIsInfF(u0) && rtIsInfF(u1)) {
- if (u0 > 0.0F) {
- b_u0 = 1;
- } else {
- b_u0 = -1;
- }
-
- if (u1 > 0.0F) {
- b_u1 = 1;
- } else {
- b_u1 = -1;
- }
-
- y = (real32_T)atan2((real32_T)b_u0, (real32_T)b_u1);
- } else if (u1 == 0.0F) {
- if (u0 > 0.0F) {
- y = RT_PIF / 2.0F;
- } else if (u0 < 0.0F) {
- y = -(RT_PIF / 2.0F);
- } else {
- y = 0.0F;
- }
- } else {
- y = (real32_T)atan2(u0, u1);
- }
-
- return y;
-}
-
-/*
- * function [eulerAngles,Rot_matrix,x_aposteriori,P_aposteriori] = attitudeKalmanfilter_wo(updateVect,dt,z,x_aposteriori_k,P_aposteriori_k,q,r)
- */
-void attitudeKalmanfilter(const uint8_T updateVect[3], real32_T dt, const
- real32_T z[9], const real32_T x_aposteriori_k[12], const real32_T
- P_aposteriori_k[144], const real32_T q[12], real32_T r[9], real32_T
- eulerAngles[3], real32_T Rot_matrix[9], real32_T x_aposteriori[12], real32_T
- P_aposteriori[144])
-{
- real32_T wak[3];
- real32_T O[9];
- real_T dv0[9];
- real32_T a[9];
- int32_T i;
- real32_T b_a[9];
- real32_T x_n_b[3];
- real32_T b_x_aposteriori_k[3];
- real32_T z_n_b[3];
- real32_T c_a[3];
- real32_T d_a[3];
- int32_T i0;
- real32_T x_apriori[12];
- real_T dv1[144];
- real32_T A_lin[144];
- static const int8_T iv0[36] = { 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 1, 0, 0,
- 0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0 };
-
- real32_T b_A_lin[144];
- real32_T b_q[144];
- real32_T c_A_lin[144];
- real32_T d_A_lin[144];
- real32_T e_A_lin[144];
- int32_T i1;
- real32_T P_apriori[144];
- real32_T b_P_apriori[108];
- static const int8_T iv1[108] = { 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 0,
- 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0,
- 0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0,
- 0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0,
- 0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1 };
-
- real32_T K_k[108];
- real32_T fv0[81];
- static const int8_T iv2[108] = { 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0,
- 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0,
- 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0,
- 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0,
- 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1 };
-
- real32_T b_r[81];
- real32_T fv1[81];
- real32_T f0;
- real32_T c_P_apriori[36];
- static const int8_T iv3[36] = { 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0,
- 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0 };
-
- real32_T fv2[36];
- static const int8_T iv4[36] = { 1, 0, 0, 0, 1, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0,
- 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0 };
-
- real32_T c_r[9];
- real32_T b_K_k[36];
- real32_T d_P_apriori[72];
- static const int8_T iv5[72] = { 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0,
- 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0,
- 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0,
- 1, 0, 0, 0 };
-
- real32_T c_K_k[72];
- static const int8_T iv6[72] = { 1, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 1, 0,
- 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0,
- 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0,
- 0, 0, 0, 0 };
-
- real32_T b_z[6];
- static const int8_T iv7[72] = { 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0,
- 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0,
- 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0,
- 0, 0, 0, 1 };
-
- static const int8_T iv8[72] = { 1, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 1, 0,
- 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0,
- 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0,
- 0, 0, 0, 1 };
-
- real32_T fv3[6];
- real32_T c_z[6];
-
- /* Extended Attitude Kalmanfilter */
- /* */
- /* state vector x has the following entries [ax,ay,az||mx,my,mz||wox,woy,woz||wx,wy,wz]' */
- /* measurement vector z has the following entries [ax,ay,az||mx,my,mz||wmx,wmy,wmz]' */
- /* knownConst has the following entries [PrvaA,PrvarM,PrvarWO,PrvarW||MsvarA,MsvarM,MsvarW] */
- /* */
- /* [x_aposteriori,P_aposteriori] = AttKalman(dt,z_k,x_aposteriori_k,P_aposteriori_k,knownConst) */
- /* */
- /* Example.... */
- /* */
- /* $Author: Tobias Naegeli $ $Date: 2012 $ $Revision: 1 $ */
- /* coder.varsize('udpIndVect', [9,1], [1,0]) */
- /* udpIndVect=find(updVect); */
- /* process and measurement noise covariance matrix */
- /* Q = diag(q.^2*dt); */
- /* observation matrix */
- /* 'attitudeKalmanfilter:33' wx= x_aposteriori_k(1); */
- /* 'attitudeKalmanfilter:34' wy= x_aposteriori_k(2); */
- /* 'attitudeKalmanfilter:35' wz= x_aposteriori_k(3); */
- /* 'attitudeKalmanfilter:37' wax= x_aposteriori_k(4); */
- /* 'attitudeKalmanfilter:38' way= x_aposteriori_k(5); */
- /* 'attitudeKalmanfilter:39' waz= x_aposteriori_k(6); */
- /* 'attitudeKalmanfilter:41' zex= x_aposteriori_k(7); */
- /* 'attitudeKalmanfilter:42' zey= x_aposteriori_k(8); */
- /* 'attitudeKalmanfilter:43' zez= x_aposteriori_k(9); */
- /* 'attitudeKalmanfilter:45' mux= x_aposteriori_k(10); */
- /* 'attitudeKalmanfilter:46' muy= x_aposteriori_k(11); */
- /* 'attitudeKalmanfilter:47' muz= x_aposteriori_k(12); */
- /* % prediction section */
- /* body angular accelerations */
- /* 'attitudeKalmanfilter:51' wak =[wax;way;waz]; */
- wak[0] = x_aposteriori_k[3];
- wak[1] = x_aposteriori_k[4];
- wak[2] = x_aposteriori_k[5];
-
- /* body angular rates */
- /* 'attitudeKalmanfilter:54' wk =[wx; wy; wz] + dt*wak; */
- /* derivative of the prediction rotation matrix */
- /* 'attitudeKalmanfilter:57' O=[0,-wz,wy;wz,0,-wx;-wy,wx,0]'; */
- O[0] = 0.0F;
- O[1] = -x_aposteriori_k[2];
- O[2] = x_aposteriori_k[1];
- O[3] = x_aposteriori_k[2];
- O[4] = 0.0F;
- O[5] = -x_aposteriori_k[0];
- O[6] = -x_aposteriori_k[1];
- O[7] = x_aposteriori_k[0];
- O[8] = 0.0F;
-
- /* prediction of the earth z vector */
- /* 'attitudeKalmanfilter:60' zek =(eye(3)+O*dt)*[zex;zey;zez]; */
- eye(dv0);
- for (i = 0; i < 9; i++) {
- a[i] = (real32_T)dv0[i] + O[i] * dt;
- }
-
- /* prediction of the magnetic vector */
- /* 'attitudeKalmanfilter:63' muk =(eye(3)+O*dt)*[mux;muy;muz]; */
- eye(dv0);
- for (i = 0; i < 9; i++) {
- b_a[i] = (real32_T)dv0[i] + O[i] * dt;
- }
-
- /* 'attitudeKalmanfilter:65' EZ=[0,zez,-zey; */
- /* 'attitudeKalmanfilter:66' -zez,0,zex; */
- /* 'attitudeKalmanfilter:67' zey,-zex,0]'; */
- /* 'attitudeKalmanfilter:68' MA=[0,muz,-muy; */
- /* 'attitudeKalmanfilter:69' -muz,0,mux; */
- /* 'attitudeKalmanfilter:70' zey,-mux,0]'; */
- /* 'attitudeKalmanfilter:74' E=eye(3); */
- /* 'attitudeKalmanfilter:76' Z=zeros(3); */
- /* 'attitudeKalmanfilter:77' x_apriori=[wk;wak;zek;muk]; */
- x_n_b[0] = x_aposteriori_k[0];
- x_n_b[1] = x_aposteriori_k[1];
- x_n_b[2] = x_aposteriori_k[2];
- b_x_aposteriori_k[0] = x_aposteriori_k[6];
- b_x_aposteriori_k[1] = x_aposteriori_k[7];
- b_x_aposteriori_k[2] = x_aposteriori_k[8];
- z_n_b[0] = x_aposteriori_k[9];
- z_n_b[1] = x_aposteriori_k[10];
- z_n_b[2] = x_aposteriori_k[11];
- for (i = 0; i < 3; i++) {
- c_a[i] = 0.0F;
- for (i0 = 0; i0 < 3; i0++) {
- c_a[i] += a[i + 3 * i0] * b_x_aposteriori_k[i0];
- }
-
- d_a[i] = 0.0F;
- for (i0 = 0; i0 < 3; i0++) {
- d_a[i] += b_a[i + 3 * i0] * z_n_b[i0];
- }
-
- x_apriori[i] = x_n_b[i] + dt * wak[i];
- }
-
- for (i = 0; i < 3; i++) {
- x_apriori[i + 3] = wak[i];
- }
-
- for (i = 0; i < 3; i++) {
- x_apriori[i + 6] = c_a[i];
- }
-
- for (i = 0; i < 3; i++) {
- x_apriori[i + 9] = d_a[i];
- }
-
- /* 'attitudeKalmanfilter:81' A_lin=[ Z, E, Z, Z */
- /* 'attitudeKalmanfilter:82' Z, Z, Z, Z */
- /* 'attitudeKalmanfilter:83' EZ, Z, O, Z */
- /* 'attitudeKalmanfilter:84' MA, Z, Z, O]; */
- /* 'attitudeKalmanfilter:86' A_lin=eye(12)+A_lin*dt; */
- b_eye(dv1);
- for (i = 0; i < 12; i++) {
- for (i0 = 0; i0 < 3; i0++) {
- A_lin[i0 + 12 * i] = (real32_T)iv0[i0 + 3 * i];
- }
-
- for (i0 = 0; i0 < 3; i0++) {
- A_lin[(i0 + 12 * i) + 3] = 0.0F;
- }
- }
-
- A_lin[6] = 0.0F;
- A_lin[7] = x_aposteriori_k[8];
- A_lin[8] = -x_aposteriori_k[7];
- A_lin[18] = -x_aposteriori_k[8];
- A_lin[19] = 0.0F;
- A_lin[20] = x_aposteriori_k[6];
- A_lin[30] = x_aposteriori_k[7];
- A_lin[31] = -x_aposteriori_k[6];
- A_lin[32] = 0.0F;
- for (i = 0; i < 3; i++) {
- for (i0 = 0; i0 < 3; i0++) {
- A_lin[(i0 + 12 * (i + 3)) + 6] = 0.0F;
- }
- }
-
- for (i = 0; i < 3; i++) {
- for (i0 = 0; i0 < 3; i0++) {
- A_lin[(i0 + 12 * (i + 6)) + 6] = O[i0 + 3 * i];
- }
- }
-
- for (i = 0; i < 3; i++) {
- for (i0 = 0; i0 < 3; i0++) {
- A_lin[(i0 + 12 * (i + 9)) + 6] = 0.0F;
- }
- }
-
- A_lin[9] = 0.0F;
- A_lin[10] = x_aposteriori_k[11];
- A_lin[11] = -x_aposteriori_k[10];
- A_lin[21] = -x_aposteriori_k[11];
- A_lin[22] = 0.0F;
- A_lin[23] = x_aposteriori_k[9];
- A_lin[33] = x_aposteriori_k[7];
- A_lin[34] = -x_aposteriori_k[9];
- A_lin[35] = 0.0F;
- for (i = 0; i < 3; i++) {
- for (i0 = 0; i0 < 3; i0++) {
- A_lin[(i0 + 12 * (i + 3)) + 9] = 0.0F;
- }
- }
-
- for (i = 0; i < 3; i++) {
- for (i0 = 0; i0 < 3; i0++) {
- A_lin[(i0 + 12 * (i + 6)) + 9] = 0.0F;
- }
- }
-
- for (i = 0; i < 3; i++) {
- for (i0 = 0; i0 < 3; i0++) {
- A_lin[(i0 + 12 * (i + 9)) + 9] = O[i0 + 3 * i];
- }
- }
-
- for (i = 0; i < 12; i++) {
- for (i0 = 0; i0 < 12; i0++) {
- b_A_lin[i0 + 12 * i] = (real32_T)dv1[i0 + 12 * i] + A_lin[i0 + 12 * i] *
- dt;
- }
- }
-
- /* 'attitudeKalmanfilter:88' Qtemp=[ q(1), 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0; */
- /* 'attitudeKalmanfilter:89' 0, q(1), 0, 0, 0, 0, 0, 0, 0, 0, 0, 0; */
- /* 'attitudeKalmanfilter:90' 0, 0, q(1), 0, 0, 0, 0, 0, 0, 0, 0, 0; */
- /* 'attitudeKalmanfilter:91' 0, 0, 0, q(2), 0, 0, 0, 0, 0, 0, 0, 0; */
- /* 'attitudeKalmanfilter:92' 0, 0, 0, 0, q(2), 0, 0, 0, 0, 0, 0, 0; */
- /* 'attitudeKalmanfilter:93' 0, 0, 0, 0, 0, q(2), 0, 0, 0, 0, 0, 0; */
- /* 'attitudeKalmanfilter:94' 0, 0, 0, 0, 0, 0, q(3), 0, 0, 0, 0, 0; */
- /* 'attitudeKalmanfilter:95' 0, 0, 0, 0, 0, 0, 0, q(3), 0, 0, 0, 0; */
- /* 'attitudeKalmanfilter:96' 0, 0, 0, 0, 0, 0, 0, 0, q(3), 0, 0, 0; */
- /* 'attitudeKalmanfilter:97' 0, 0, 0, 0, 0, 0, 0, 0, 0, q(4), 0, 0; */
- /* 'attitudeKalmanfilter:98' 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, q(4), 0; */
- /* 'attitudeKalmanfilter:99' 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, q(4)]; */
- /* 'attitudeKalmanfilter:103' Q=A_lin*Qtemp*A_lin'; */
- /* 'attitudeKalmanfilter:106' P_apriori=A_lin*P_aposteriori_k*A_lin'+Q; */
- b_q[0] = q[0];
- b_q[12] = 0.0F;
- b_q[24] = 0.0F;
- b_q[36] = 0.0F;
- b_q[48] = 0.0F;
- b_q[60] = 0.0F;
- b_q[72] = 0.0F;
- b_q[84] = 0.0F;
- b_q[96] = 0.0F;
- b_q[108] = 0.0F;
- b_q[120] = 0.0F;
- b_q[132] = 0.0F;
- b_q[1] = 0.0F;
- b_q[13] = q[0];
- b_q[25] = 0.0F;
- b_q[37] = 0.0F;
- b_q[49] = 0.0F;
- b_q[61] = 0.0F;
- b_q[73] = 0.0F;
- b_q[85] = 0.0F;
- b_q[97] = 0.0F;
- b_q[109] = 0.0F;
- b_q[121] = 0.0F;
- b_q[133] = 0.0F;
- b_q[2] = 0.0F;
- b_q[14] = 0.0F;
- b_q[26] = q[0];
- b_q[38] = 0.0F;
- b_q[50] = 0.0F;
- b_q[62] = 0.0F;
- b_q[74] = 0.0F;
- b_q[86] = 0.0F;
- b_q[98] = 0.0F;
- b_q[110] = 0.0F;
- b_q[122] = 0.0F;
- b_q[134] = 0.0F;
- b_q[3] = 0.0F;
- b_q[15] = 0.0F;
- b_q[27] = 0.0F;
- b_q[39] = q[1];
- b_q[51] = 0.0F;
- b_q[63] = 0.0F;
- b_q[75] = 0.0F;
- b_q[87] = 0.0F;
- b_q[99] = 0.0F;
- b_q[111] = 0.0F;
- b_q[123] = 0.0F;
- b_q[135] = 0.0F;
- b_q[4] = 0.0F;
- b_q[16] = 0.0F;
- b_q[28] = 0.0F;
- b_q[40] = 0.0F;
- b_q[52] = q[1];
- b_q[64] = 0.0F;
- b_q[76] = 0.0F;
- b_q[88] = 0.0F;
- b_q[100] = 0.0F;
- b_q[112] = 0.0F;
- b_q[124] = 0.0F;
- b_q[136] = 0.0F;
- b_q[5] = 0.0F;
- b_q[17] = 0.0F;
- b_q[29] = 0.0F;
- b_q[41] = 0.0F;
- b_q[53] = 0.0F;
- b_q[65] = q[1];
- b_q[77] = 0.0F;
- b_q[89] = 0.0F;
- b_q[101] = 0.0F;
- b_q[113] = 0.0F;
- b_q[125] = 0.0F;
- b_q[137] = 0.0F;
- b_q[6] = 0.0F;
- b_q[18] = 0.0F;
- b_q[30] = 0.0F;
- b_q[42] = 0.0F;
- b_q[54] = 0.0F;
- b_q[66] = 0.0F;
- b_q[78] = q[2];
- b_q[90] = 0.0F;
- b_q[102] = 0.0F;
- b_q[114] = 0.0F;
- b_q[126] = 0.0F;
- b_q[138] = 0.0F;
- b_q[7] = 0.0F;
- b_q[19] = 0.0F;
- b_q[31] = 0.0F;
- b_q[43] = 0.0F;
- b_q[55] = 0.0F;
- b_q[67] = 0.0F;
- b_q[79] = 0.0F;
- b_q[91] = q[2];
- b_q[103] = 0.0F;
- b_q[115] = 0.0F;
- b_q[127] = 0.0F;
- b_q[139] = 0.0F;
- b_q[8] = 0.0F;
- b_q[20] = 0.0F;
- b_q[32] = 0.0F;
- b_q[44] = 0.0F;
- b_q[56] = 0.0F;
- b_q[68] = 0.0F;
- b_q[80] = 0.0F;
- b_q[92] = 0.0F;
- b_q[104] = q[2];
- b_q[116] = 0.0F;
- b_q[128] = 0.0F;
- b_q[140] = 0.0F;
- b_q[9] = 0.0F;
- b_q[21] = 0.0F;
- b_q[33] = 0.0F;
- b_q[45] = 0.0F;
- b_q[57] = 0.0F;
- b_q[69] = 0.0F;
- b_q[81] = 0.0F;
- b_q[93] = 0.0F;
- b_q[105] = 0.0F;
- b_q[117] = q[3];
- b_q[129] = 0.0F;
- b_q[141] = 0.0F;
- b_q[10] = 0.0F;
- b_q[22] = 0.0F;
- b_q[34] = 0.0F;
- b_q[46] = 0.0F;
- b_q[58] = 0.0F;
- b_q[70] = 0.0F;
- b_q[82] = 0.0F;
- b_q[94] = 0.0F;
- b_q[106] = 0.0F;
- b_q[118] = 0.0F;
- b_q[130] = q[3];
- b_q[142] = 0.0F;
- b_q[11] = 0.0F;
- b_q[23] = 0.0F;
- b_q[35] = 0.0F;
- b_q[47] = 0.0F;
- b_q[59] = 0.0F;
- b_q[71] = 0.0F;
- b_q[83] = 0.0F;
- b_q[95] = 0.0F;
- b_q[107] = 0.0F;
- b_q[119] = 0.0F;
- b_q[131] = 0.0F;
- b_q[143] = q[3];
- for (i = 0; i < 12; i++) {
- for (i0 = 0; i0 < 12; i0++) {
- A_lin[i + 12 * i0] = 0.0F;
- for (i1 = 0; i1 < 12; i1++) {
- A_lin[i + 12 * i0] += b_A_lin[i + 12 * i1] * P_aposteriori_k[i1 + 12 *
- i0];
- }
-
- c_A_lin[i + 12 * i0] = 0.0F;
- for (i1 = 0; i1 < 12; i1++) {
- c_A_lin[i + 12 * i0] += b_A_lin[i + 12 * i1] * b_q[i1 + 12 * i0];
- }
- }
-
- for (i0 = 0; i0 < 12; i0++) {
- d_A_lin[i + 12 * i0] = 0.0F;
- for (i1 = 0; i1 < 12; i1++) {
- d_A_lin[i + 12 * i0] += A_lin[i + 12 * i1] * b_A_lin[i0 + 12 * i1];
- }
-
- e_A_lin[i + 12 * i0] = 0.0F;
- for (i1 = 0; i1 < 12; i1++) {
- e_A_lin[i + 12 * i0] += c_A_lin[i + 12 * i1] * b_A_lin[i0 + 12 * i1];
- }
- }
- }
-
- for (i = 0; i < 12; i++) {
- for (i0 = 0; i0 < 12; i0++) {
- P_apriori[i0 + 12 * i] = d_A_lin[i0 + 12 * i] + e_A_lin[i0 + 12 * i];
- }
- }
-
- /* % update */
- /* 'attitudeKalmanfilter:110' if updateVect(1)==1&&updateVect(2)==1&&updateVect(3)==1 */
- if ((updateVect[0] == 1) && (updateVect[1] == 1) && (updateVect[2] == 1)) {
- /* 'attitudeKalmanfilter:111' if z(6)<4 || z(5)>15 */
- if ((z[5] < 4.0F) || (z[4] > 15.0F)) {
- /* 'attitudeKalmanfilter:112' r(2)=10000; */
- r[1] = 10000.0F;
- }
-
- /* 'attitudeKalmanfilter:114' R=[r(1),0,0,0,0,0,0,0,0; */
- /* 'attitudeKalmanfilter:115' 0,r(1),0,0,0,0,0,0,0; */
- /* 'attitudeKalmanfilter:116' 0,0,r(1),0,0,0,0,0,0; */
- /* 'attitudeKalmanfilter:117' 0,0,0,r(2),0,0,0,0,0; */
- /* 'attitudeKalmanfilter:118' 0,0,0,0,r(2),0,0,0,0; */
- /* 'attitudeKalmanfilter:119' 0,0,0,0,0,r(2),0,0,0; */
- /* 'attitudeKalmanfilter:120' 0,0,0,0,0,0,r(3),0,0; */
- /* 'attitudeKalmanfilter:121' 0,0,0,0,0,0,0,r(3),0; */
- /* 'attitudeKalmanfilter:122' 0,0,0,0,0,0,0,0,r(3)]; */
- /* observation matrix */
- /* [zw;ze;zmk]; */
- /* 'attitudeKalmanfilter:125' H_k=[ E, Z, Z, Z; */
- /* 'attitudeKalmanfilter:126' Z, Z, E, Z; */
- /* 'attitudeKalmanfilter:127' Z, Z, Z, E]; */
- /* 'attitudeKalmanfilter:129' y_k=z(1:9)-H_k*x_apriori; */
- /* 'attitudeKalmanfilter:132' S_k=H_k*P_apriori*H_k'+R; */
- /* 'attitudeKalmanfilter:133' K_k=(P_apriori*H_k'/(S_k)); */
- for (i = 0; i < 12; i++) {
- for (i0 = 0; i0 < 9; i0++) {
- b_P_apriori[i + 12 * i0] = 0.0F;
- for (i1 = 0; i1 < 12; i1++) {
- b_P_apriori[i + 12 * i0] += P_apriori[i + 12 * i1] * (real32_T)iv1[i1
- + 12 * i0];
- }
- }
- }
-
- for (i = 0; i < 9; i++) {
- for (i0 = 0; i0 < 12; i0++) {
- K_k[i + 9 * i0] = 0.0F;
- for (i1 = 0; i1 < 12; i1++) {
- K_k[i + 9 * i0] += (real32_T)iv2[i + 9 * i1] * P_apriori[i1 + 12 * i0];
- }
- }
-
- for (i0 = 0; i0 < 9; i0++) {
- fv0[i + 9 * i0] = 0.0F;
- for (i1 = 0; i1 < 12; i1++) {
- fv0[i + 9 * i0] += K_k[i + 9 * i1] * (real32_T)iv1[i1 + 12 * i0];
- }
- }
- }
-
- b_r[0] = r[0];
- b_r[9] = 0.0F;
- b_r[18] = 0.0F;
- b_r[27] = 0.0F;
- b_r[36] = 0.0F;
- b_r[45] = 0.0F;
- b_r[54] = 0.0F;
- b_r[63] = 0.0F;
- b_r[72] = 0.0F;
- b_r[1] = 0.0F;
- b_r[10] = r[0];
- b_r[19] = 0.0F;
- b_r[28] = 0.0F;
- b_r[37] = 0.0F;
- b_r[46] = 0.0F;
- b_r[55] = 0.0F;
- b_r[64] = 0.0F;
- b_r[73] = 0.0F;
- b_r[2] = 0.0F;
- b_r[11] = 0.0F;
- b_r[20] = r[0];
- b_r[29] = 0.0F;
- b_r[38] = 0.0F;
- b_r[47] = 0.0F;
- b_r[56] = 0.0F;
- b_r[65] = 0.0F;
- b_r[74] = 0.0F;
- b_r[3] = 0.0F;
- b_r[12] = 0.0F;
- b_r[21] = 0.0F;
- b_r[30] = r[1];
- b_r[39] = 0.0F;
- b_r[48] = 0.0F;
- b_r[57] = 0.0F;
- b_r[66] = 0.0F;
- b_r[75] = 0.0F;
- b_r[4] = 0.0F;
- b_r[13] = 0.0F;
- b_r[22] = 0.0F;
- b_r[31] = 0.0F;
- b_r[40] = r[1];
- b_r[49] = 0.0F;
- b_r[58] = 0.0F;
- b_r[67] = 0.0F;
- b_r[76] = 0.0F;
- b_r[5] = 0.0F;
- b_r[14] = 0.0F;
- b_r[23] = 0.0F;
- b_r[32] = 0.0F;
- b_r[41] = 0.0F;
- b_r[50] = r[1];
- b_r[59] = 0.0F;
- b_r[68] = 0.0F;
- b_r[77] = 0.0F;
- b_r[6] = 0.0F;
- b_r[15] = 0.0F;
- b_r[24] = 0.0F;
- b_r[33] = 0.0F;
- b_r[42] = 0.0F;
- b_r[51] = 0.0F;
- b_r[60] = r[2];
- b_r[69] = 0.0F;
- b_r[78] = 0.0F;
- b_r[7] = 0.0F;
- b_r[16] = 0.0F;
- b_r[25] = 0.0F;
- b_r[34] = 0.0F;
- b_r[43] = 0.0F;
- b_r[52] = 0.0F;
- b_r[61] = 0.0F;
- b_r[70] = r[2];
- b_r[79] = 0.0F;
- b_r[8] = 0.0F;
- b_r[17] = 0.0F;
- b_r[26] = 0.0F;
- b_r[35] = 0.0F;
- b_r[44] = 0.0F;
- b_r[53] = 0.0F;
- b_r[62] = 0.0F;
- b_r[71] = 0.0F;
- b_r[80] = r[2];
- for (i = 0; i < 9; i++) {
- for (i0 = 0; i0 < 9; i0++) {
- fv1[i0 + 9 * i] = fv0[i0 + 9 * i] + b_r[i0 + 9 * i];
- }
- }
-
- mrdivide(b_P_apriori, fv1, K_k);
-
- /* 'attitudeKalmanfilter:136' x_aposteriori=x_apriori+K_k*y_k; */
- for (i = 0; i < 9; i++) {
- f0 = 0.0F;
- for (i0 = 0; i0 < 12; i0++) {
- f0 += (real32_T)iv2[i + 9 * i0] * x_apriori[i0];
- }
-
- O[i] = z[i] - f0;
- }
-
- for (i = 0; i < 12; i++) {
- f0 = 0.0F;
- for (i0 = 0; i0 < 9; i0++) {
- f0 += K_k[i + 12 * i0] * O[i0];
- }
-
- x_aposteriori[i] = x_apriori[i] + f0;
- }
-
- /* 'attitudeKalmanfilter:137' P_aposteriori=(eye(12)-K_k*H_k)*P_apriori; */
- b_eye(dv1);
- for (i = 0; i < 12; i++) {
- for (i0 = 0; i0 < 12; i0++) {
- f0 = 0.0F;
- for (i1 = 0; i1 < 9; i1++) {
- f0 += K_k[i + 12 * i1] * (real32_T)iv2[i1 + 9 * i0];
- }
-
- b_A_lin[i + 12 * i0] = (real32_T)dv1[i + 12 * i0] - f0;
- }
- }
-
- for (i = 0; i < 12; i++) {
- for (i0 = 0; i0 < 12; i0++) {
- P_aposteriori[i + 12 * i0] = 0.0F;
- for (i1 = 0; i1 < 12; i1++) {
- P_aposteriori[i + 12 * i0] += b_A_lin[i + 12 * i1] * P_apriori[i1 + 12
- * i0];
- }
- }
- }
- } else {
- /* 'attitudeKalmanfilter:138' else */
- /* 'attitudeKalmanfilter:139' if updateVect(1)==1&&updateVect(2)==0&&updateVect(3)==0 */
- if ((updateVect[0] == 1) && (updateVect[1] == 0) && (updateVect[2] == 0)) {
- /* 'attitudeKalmanfilter:141' R=[r(1),0,0; */
- /* 'attitudeKalmanfilter:142' 0,r(1),0; */
- /* 'attitudeKalmanfilter:143' 0,0,r(1)]; */
- /* observation matrix */
- /* 'attitudeKalmanfilter:146' H_k=[ E, Z, Z, Z]; */
- /* 'attitudeKalmanfilter:148' y_k=z(1:3)-H_k(1:3,1:12)*x_apriori; */
- /* 'attitudeKalmanfilter:150' S_k=H_k(1:3,1:12)*P_apriori*H_k(1:3,1:12)'+R(1:3,1:3); */
- /* 'attitudeKalmanfilter:151' K_k=(P_apriori*H_k(1:3,1:12)'/(S_k)); */
- for (i = 0; i < 12; i++) {
- for (i0 = 0; i0 < 3; i0++) {
- c_P_apriori[i + 12 * i0] = 0.0F;
- for (i1 = 0; i1 < 12; i1++) {
- c_P_apriori[i + 12 * i0] += P_apriori[i + 12 * i1] * (real32_T)
- iv3[i1 + 12 * i0];
- }
- }
- }
-
- for (i = 0; i < 3; i++) {
- for (i0 = 0; i0 < 12; i0++) {
- fv2[i + 3 * i0] = 0.0F;
- for (i1 = 0; i1 < 12; i1++) {
- fv2[i + 3 * i0] += (real32_T)iv4[i + 3 * i1] * P_apriori[i1 + 12 *
- i0];
- }
- }
-
- for (i0 = 0; i0 < 3; i0++) {
- O[i + 3 * i0] = 0.0F;
- for (i1 = 0; i1 < 12; i1++) {
- O[i + 3 * i0] += fv2[i + 3 * i1] * (real32_T)iv3[i1 + 12 * i0];
- }
- }
- }
-
- c_r[0] = r[0];
- c_r[3] = 0.0F;
- c_r[6] = 0.0F;
- c_r[1] = 0.0F;
- c_r[4] = r[0];
- c_r[7] = 0.0F;
- c_r[2] = 0.0F;
- c_r[5] = 0.0F;
- c_r[8] = r[0];
- for (i = 0; i < 3; i++) {
- for (i0 = 0; i0 < 3; i0++) {
- a[i0 + 3 * i] = O[i0 + 3 * i] + c_r[i0 + 3 * i];
- }
- }
-
- b_mrdivide(c_P_apriori, a, b_K_k);
-
- /* 'attitudeKalmanfilter:154' x_aposteriori=x_apriori+K_k*y_k; */
- for (i = 0; i < 3; i++) {
- f0 = 0.0F;
- for (i0 = 0; i0 < 12; i0++) {
- f0 += (real32_T)iv4[i + 3 * i0] * x_apriori[i0];
- }
-
- x_n_b[i] = z[i] - f0;
- }
-
- for (i = 0; i < 12; i++) {
- f0 = 0.0F;
- for (i0 = 0; i0 < 3; i0++) {
- f0 += b_K_k[i + 12 * i0] * x_n_b[i0];
- }
-
- x_aposteriori[i] = x_apriori[i] + f0;
- }
-
- /* 'attitudeKalmanfilter:155' P_aposteriori=(eye(12)-K_k*H_k(1:3,1:12))*P_apriori; */
- b_eye(dv1);
- for (i = 0; i < 12; i++) {
- for (i0 = 0; i0 < 12; i0++) {
- f0 = 0.0F;
- for (i1 = 0; i1 < 3; i1++) {
- f0 += b_K_k[i + 12 * i1] * (real32_T)iv4[i1 + 3 * i0];
- }
-
- b_A_lin[i + 12 * i0] = (real32_T)dv1[i + 12 * i0] - f0;
- }
- }
-
- for (i = 0; i < 12; i++) {
- for (i0 = 0; i0 < 12; i0++) {
- P_aposteriori[i + 12 * i0] = 0.0F;
- for (i1 = 0; i1 < 12; i1++) {
- P_aposteriori[i + 12 * i0] += b_A_lin[i + 12 * i1] * P_apriori[i1 +
- 12 * i0];
- }
- }
- }
- } else {
- /* 'attitudeKalmanfilter:156' else */
- /* 'attitudeKalmanfilter:157' if updateVect(1)==1&&updateVect(2)==1&&updateVect(3)==0 */
- if ((updateVect[0] == 1) && (updateVect[1] == 1) && (updateVect[2] == 0))
- {
- /* 'attitudeKalmanfilter:158' if z(6)<4 || z(5)>15 */
- if ((z[5] < 4.0F) || (z[4] > 15.0F)) {
- /* 'attitudeKalmanfilter:159' r(2)=10000; */
- r[1] = 10000.0F;
- }
-
- /* 'attitudeKalmanfilter:162' R=[r(1),0,0,0,0,0; */
- /* 'attitudeKalmanfilter:163' 0,r(1),0,0,0,0; */
- /* 'attitudeKalmanfilter:164' 0,0,r(1),0,0,0; */
- /* 'attitudeKalmanfilter:165' 0,0,0,r(2),0,0; */
- /* 'attitudeKalmanfilter:166' 0,0,0,0,r(2),0; */
- /* 'attitudeKalmanfilter:167' 0,0,0,0,0,r(2)]; */
- /* observation matrix */
- /* 'attitudeKalmanfilter:170' H_k=[ E, Z, Z, Z; */
- /* 'attitudeKalmanfilter:171' Z, Z, E, Z]; */
- /* 'attitudeKalmanfilter:173' y_k=z(1:6)-H_k(1:6,1:12)*x_apriori; */
- /* 'attitudeKalmanfilter:175' S_k=H_k(1:6,1:12)*P_apriori*H_k(1:6,1:12)'+R(1:6,1:6); */
- /* 'attitudeKalmanfilter:176' K_k=(P_apriori*H_k(1:6,1:12)'/(S_k)); */
- for (i = 0; i < 12; i++) {
- for (i0 = 0; i0 < 6; i0++) {
- d_P_apriori[i + 12 * i0] = 0.0F;
- for (i1 = 0; i1 < 12; i1++) {
- d_P_apriori[i + 12 * i0] += P_apriori[i + 12 * i1] * (real32_T)
- iv5[i1 + 12 * i0];
- }
- }
- }
-
- for (i = 0; i < 6; i++) {
- for (i0 = 0; i0 < 12; i0++) {
- c_K_k[i + 6 * i0] = 0.0F;
- for (i1 = 0; i1 < 12; i1++) {
- c_K_k[i + 6 * i0] += (real32_T)iv6[i + 6 * i1] * P_apriori[i1 + 12
- * i0];
- }
- }
-
- for (i0 = 0; i0 < 6; i0++) {
- fv2[i + 6 * i0] = 0.0F;
- for (i1 = 0; i1 < 12; i1++) {
- fv2[i + 6 * i0] += c_K_k[i + 6 * i1] * (real32_T)iv5[i1 + 12 * i0];
- }
- }
- }
-
- b_K_k[0] = r[0];
- b_K_k[6] = 0.0F;
- b_K_k[12] = 0.0F;
- b_K_k[18] = 0.0F;
- b_K_k[24] = 0.0F;
- b_K_k[30] = 0.0F;
- b_K_k[1] = 0.0F;
- b_K_k[7] = r[0];
- b_K_k[13] = 0.0F;
- b_K_k[19] = 0.0F;
- b_K_k[25] = 0.0F;
- b_K_k[31] = 0.0F;
- b_K_k[2] = 0.0F;
- b_K_k[8] = 0.0F;
- b_K_k[14] = r[0];
- b_K_k[20] = 0.0F;
- b_K_k[26] = 0.0F;
- b_K_k[32] = 0.0F;
- b_K_k[3] = 0.0F;
- b_K_k[9] = 0.0F;
- b_K_k[15] = 0.0F;
- b_K_k[21] = r[1];
- b_K_k[27] = 0.0F;
- b_K_k[33] = 0.0F;
- b_K_k[4] = 0.0F;
- b_K_k[10] = 0.0F;
- b_K_k[16] = 0.0F;
- b_K_k[22] = 0.0F;
- b_K_k[28] = r[1];
- b_K_k[34] = 0.0F;
- b_K_k[5] = 0.0F;
- b_K_k[11] = 0.0F;
- b_K_k[17] = 0.0F;
- b_K_k[23] = 0.0F;
- b_K_k[29] = 0.0F;
- b_K_k[35] = r[1];
- for (i = 0; i < 6; i++) {
- for (i0 = 0; i0 < 6; i0++) {
- c_P_apriori[i0 + 6 * i] = fv2[i0 + 6 * i] + b_K_k[i0 + 6 * i];
- }
- }
-
- c_mrdivide(d_P_apriori, c_P_apriori, c_K_k);
-
- /* 'attitudeKalmanfilter:179' x_aposteriori=x_apriori+K_k*y_k; */
- for (i = 0; i < 6; i++) {
- f0 = 0.0F;
- for (i0 = 0; i0 < 12; i0++) {
- f0 += (real32_T)iv6[i + 6 * i0] * x_apriori[i0];
- }
-
- b_z[i] = z[i] - f0;
- }
-
- for (i = 0; i < 12; i++) {
- f0 = 0.0F;
- for (i0 = 0; i0 < 6; i0++) {
- f0 += c_K_k[i + 12 * i0] * b_z[i0];
- }
-
- x_aposteriori[i] = x_apriori[i] + f0;
- }
-
- /* 'attitudeKalmanfilter:180' P_aposteriori=(eye(12)-K_k*H_k(1:6,1:12))*P_apriori; */
- b_eye(dv1);
- for (i = 0; i < 12; i++) {
- for (i0 = 0; i0 < 12; i0++) {
- f0 = 0.0F;
- for (i1 = 0; i1 < 6; i1++) {
- f0 += c_K_k[i + 12 * i1] * (real32_T)iv6[i1 + 6 * i0];
- }
-
- b_A_lin[i + 12 * i0] = (real32_T)dv1[i + 12 * i0] - f0;
- }
- }
-
- for (i = 0; i < 12; i++) {
- for (i0 = 0; i0 < 12; i0++) {
- P_aposteriori[i + 12 * i0] = 0.0F;
- for (i1 = 0; i1 < 12; i1++) {
- P_aposteriori[i + 12 * i0] += b_A_lin[i + 12 * i1] * P_apriori[i1
- + 12 * i0];
- }
- }
- }
- } else {
- /* 'attitudeKalmanfilter:181' else */
- /* 'attitudeKalmanfilter:182' if updateVect(1)==1&&updateVect(2)==0&&updateVect(3)==1 */
- if ((updateVect[0] == 1) && (updateVect[1] == 0) && (updateVect[2] == 1))
- {
- /* 'attitudeKalmanfilter:183' R=[r(1),0,0,0,0,0; */
- /* 'attitudeKalmanfilter:184' 0,r(1),0,0,0,0; */
- /* 'attitudeKalmanfilter:185' 0,0,r(1),0,0,0; */
- /* 'attitudeKalmanfilter:186' 0,0,0,r(3),0,0; */
- /* 'attitudeKalmanfilter:187' 0,0,0,0,r(3),0; */
- /* 'attitudeKalmanfilter:188' 0,0,0,0,0,r(3)]; */
- /* observation matrix */
- /* 'attitudeKalmanfilter:191' H_k=[ E, Z, Z, Z; */
- /* 'attitudeKalmanfilter:192' Z, Z, Z, E]; */
- /* 'attitudeKalmanfilter:194' y_k=[z(1:3);z(7:9)]-H_k(1:6,1:12)*x_apriori; */
- /* 'attitudeKalmanfilter:196' S_k=H_k(1:6,1:12)*P_apriori*H_k(1:6,1:12)'+R(1:6,1:6); */
- /* 'attitudeKalmanfilter:197' K_k=(P_apriori*H_k(1:6,1:12)'/(S_k)); */
- for (i = 0; i < 12; i++) {
- for (i0 = 0; i0 < 6; i0++) {
- d_P_apriori[i + 12 * i0] = 0.0F;
- for (i1 = 0; i1 < 12; i1++) {
- d_P_apriori[i + 12 * i0] += P_apriori[i + 12 * i1] * (real32_T)
- iv7[i1 + 12 * i0];
- }
- }
- }
-
- for (i = 0; i < 6; i++) {
- for (i0 = 0; i0 < 12; i0++) {
- c_K_k[i + 6 * i0] = 0.0F;
- for (i1 = 0; i1 < 12; i1++) {
- c_K_k[i + 6 * i0] += (real32_T)iv8[i + 6 * i1] * P_apriori[i1 +
- 12 * i0];
- }
- }
-
- for (i0 = 0; i0 < 6; i0++) {
- fv2[i + 6 * i0] = 0.0F;
- for (i1 = 0; i1 < 12; i1++) {
- fv2[i + 6 * i0] += c_K_k[i + 6 * i1] * (real32_T)iv7[i1 + 12 *
- i0];
- }
- }
- }
-
- b_K_k[0] = r[0];
- b_K_k[6] = 0.0F;
- b_K_k[12] = 0.0F;
- b_K_k[18] = 0.0F;
- b_K_k[24] = 0.0F;
- b_K_k[30] = 0.0F;
- b_K_k[1] = 0.0F;
- b_K_k[7] = r[0];
- b_K_k[13] = 0.0F;
- b_K_k[19] = 0.0F;
- b_K_k[25] = 0.0F;
- b_K_k[31] = 0.0F;
- b_K_k[2] = 0.0F;
- b_K_k[8] = 0.0F;
- b_K_k[14] = r[0];
- b_K_k[20] = 0.0F;
- b_K_k[26] = 0.0F;
- b_K_k[32] = 0.0F;
- b_K_k[3] = 0.0F;
- b_K_k[9] = 0.0F;
- b_K_k[15] = 0.0F;
- b_K_k[21] = r[2];
- b_K_k[27] = 0.0F;
- b_K_k[33] = 0.0F;
- b_K_k[4] = 0.0F;
- b_K_k[10] = 0.0F;
- b_K_k[16] = 0.0F;
- b_K_k[22] = 0.0F;
- b_K_k[28] = r[2];
- b_K_k[34] = 0.0F;
- b_K_k[5] = 0.0F;
- b_K_k[11] = 0.0F;
- b_K_k[17] = 0.0F;
- b_K_k[23] = 0.0F;
- b_K_k[29] = 0.0F;
- b_K_k[35] = r[2];
- for (i = 0; i < 6; i++) {
- for (i0 = 0; i0 < 6; i0++) {
- c_P_apriori[i0 + 6 * i] = fv2[i0 + 6 * i] + b_K_k[i0 + 6 * i];
- }
- }
-
- c_mrdivide(d_P_apriori, c_P_apriori, c_K_k);
-
- /* 'attitudeKalmanfilter:200' x_aposteriori=x_apriori+K_k*y_k; */
- for (i = 0; i < 3; i++) {
- b_z[i] = z[i];
- }
-
- for (i = 0; i < 3; i++) {
- b_z[i + 3] = z[i + 6];
- }
-
- for (i = 0; i < 6; i++) {
- fv3[i] = 0.0F;
- for (i0 = 0; i0 < 12; i0++) {
- fv3[i] += (real32_T)iv8[i + 6 * i0] * x_apriori[i0];
- }
-
- c_z[i] = b_z[i] - fv3[i];
- }
-
- for (i = 0; i < 12; i++) {
- f0 = 0.0F;
- for (i0 = 0; i0 < 6; i0++) {
- f0 += c_K_k[i + 12 * i0] * c_z[i0];
- }
-
- x_aposteriori[i] = x_apriori[i] + f0;
- }
-
- /* 'attitudeKalmanfilter:201' P_aposteriori=(eye(12)-K_k*H_k(1:6,1:12))*P_apriori; */
- b_eye(dv1);
- for (i = 0; i < 12; i++) {
- for (i0 = 0; i0 < 12; i0++) {
- f0 = 0.0F;
- for (i1 = 0; i1 < 6; i1++) {
- f0 += c_K_k[i + 12 * i1] * (real32_T)iv8[i1 + 6 * i0];
- }
-
- b_A_lin[i + 12 * i0] = (real32_T)dv1[i + 12 * i0] - f0;
- }
- }
-
- for (i = 0; i < 12; i++) {
- for (i0 = 0; i0 < 12; i0++) {
- P_aposteriori[i + 12 * i0] = 0.0F;
- for (i1 = 0; i1 < 12; i1++) {
- P_aposteriori[i + 12 * i0] += b_A_lin[i + 12 * i1] *
- P_apriori[i1 + 12 * i0];
- }
- }
- }
- } else {
- /* 'attitudeKalmanfilter:202' else */
- /* 'attitudeKalmanfilter:203' x_aposteriori=x_apriori; */
- for (i = 0; i < 12; i++) {
- x_aposteriori[i] = x_apriori[i];
- }
-
- /* 'attitudeKalmanfilter:204' P_aposteriori=P_apriori; */
- memcpy(&P_aposteriori[0], &P_apriori[0], 144U * sizeof(real32_T));
- }
- }
- }
- }
-
- /* % euler anglels extraction */
- /* 'attitudeKalmanfilter:213' z_n_b = -x_aposteriori(7:9)./norm(x_aposteriori(7:9)); */
- for (i = 0; i < 3; i++) {
- x_n_b[i] = -x_aposteriori[i + 6];
- }
-
- rdivide(x_n_b, norm(*(real32_T (*)[3])&x_aposteriori[6]), z_n_b);
-
- /* 'attitudeKalmanfilter:214' m_n_b = x_aposteriori(10:12)./norm(x_aposteriori(10:12)); */
- rdivide(*(real32_T (*)[3])&x_aposteriori[9], norm(*(real32_T (*)[3])&
- x_aposteriori[9]), wak);
-
- /* 'attitudeKalmanfilter:216' y_n_b=cross(z_n_b,m_n_b); */
- for (i = 0; i < 3; i++) {
- x_n_b[i] = wak[i];
- }
-
- cross(z_n_b, x_n_b, wak);
-
- /* 'attitudeKalmanfilter:217' y_n_b=y_n_b./norm(y_n_b); */
- for (i = 0; i < 3; i++) {
- x_n_b[i] = wak[i];
- }
-
- rdivide(x_n_b, norm(wak), wak);
-
- /* 'attitudeKalmanfilter:219' x_n_b=(cross(y_n_b,z_n_b)); */
- cross(wak, z_n_b, x_n_b);
-
- /* 'attitudeKalmanfilter:220' x_n_b=x_n_b./norm(x_n_b); */
- for (i = 0; i < 3; i++) {
- b_x_aposteriori_k[i] = x_n_b[i];
- }
-
- rdivide(b_x_aposteriori_k, norm(x_n_b), x_n_b);
-
- /* 'attitudeKalmanfilter:226' Rot_matrix=[x_n_b,y_n_b,z_n_b]; */
- for (i = 0; i < 3; i++) {
- Rot_matrix[i] = x_n_b[i];
- Rot_matrix[3 + i] = wak[i];
- Rot_matrix[6 + i] = z_n_b[i];
- }
-
- /* 'attitudeKalmanfilter:230' phi=atan2(Rot_matrix(2,3),Rot_matrix(3,3)); */
- /* 'attitudeKalmanfilter:231' theta=-asin(Rot_matrix(1,3)); */
- /* 'attitudeKalmanfilter:232' psi=atan2(Rot_matrix(1,2),Rot_matrix(1,1)); */
- /* 'attitudeKalmanfilter:233' eulerAngles=[phi;theta;psi]; */
- eulerAngles[0] = rt_atan2f_snf(Rot_matrix[7], Rot_matrix[8]);
- eulerAngles[1] = -(real32_T)asin(Rot_matrix[6]);
- eulerAngles[2] = rt_atan2f_snf(Rot_matrix[3], Rot_matrix[0]);
-}
-
-/* End of code generation (attitudeKalmanfilter.c) */
diff --git a/src/modules/attitude_estimator_ekf/codegen/attitudeKalmanfilter.h b/src/modules/attitude_estimator_ekf/codegen/attitudeKalmanfilter.h deleted file mode 100755 index afa63c1a9..000000000 --- a/src/modules/attitude_estimator_ekf/codegen/attitudeKalmanfilter.h +++ /dev/null @@ -1,34 +0,0 @@ -/*
- * attitudeKalmanfilter.h
- *
- * Code generation for function 'attitudeKalmanfilter'
- *
- * C source code generated on: Sat Jan 19 15:25:29 2013
- *
- */
-
-#ifndef __ATTITUDEKALMANFILTER_H__
-#define __ATTITUDEKALMANFILTER_H__
-/* Include files */
-#include <math.h>
-#include <stddef.h>
-#include <stdlib.h>
-#include <string.h>
-#include "rt_defines.h"
-#include "rt_nonfinite.h"
-
-#include "rtwtypes.h"
-#include "attitudeKalmanfilter_types.h"
-
-/* Type Definitions */
-
-/* Named Constants */
-
-/* Variable Declarations */
-
-/* Variable Definitions */
-
-/* Function Declarations */
-extern void attitudeKalmanfilter(const uint8_T updateVect[3], real32_T dt, const real32_T z[9], const real32_T x_aposteriori_k[12], const real32_T P_aposteriori_k[144], const real32_T q[12], real32_T r[9], real32_T eulerAngles[3], real32_T Rot_matrix[9], real32_T x_aposteriori[12], real32_T P_aposteriori[144]);
-#endif
-/* End of code generation (attitudeKalmanfilter.h) */
diff --git a/src/modules/attitude_estimator_ekf/codegen/attitudeKalmanfilter_initialize.c b/src/modules/attitude_estimator_ekf/codegen/attitudeKalmanfilter_initialize.c deleted file mode 100755 index 7d620d7fa..000000000 --- a/src/modules/attitude_estimator_ekf/codegen/attitudeKalmanfilter_initialize.c +++ /dev/null @@ -1,31 +0,0 @@ -/*
- * attitudeKalmanfilter_initialize.c
- *
- * Code generation for function 'attitudeKalmanfilter_initialize'
- *
- * C source code generated on: Sat Jan 19 15:25:29 2013
- *
- */
-
-/* Include files */
-#include "rt_nonfinite.h"
-#include "attitudeKalmanfilter.h"
-#include "attitudeKalmanfilter_initialize.h"
-
-/* Type Definitions */
-
-/* Named Constants */
-
-/* Variable Declarations */
-
-/* Variable Definitions */
-
-/* Function Declarations */
-
-/* Function Definitions */
-void attitudeKalmanfilter_initialize(void)
-{
- rt_InitInfAndNaN(8U);
-}
-
-/* End of code generation (attitudeKalmanfilter_initialize.c) */
diff --git a/src/modules/attitude_estimator_ekf/codegen/attitudeKalmanfilter_initialize.h b/src/modules/attitude_estimator_ekf/codegen/attitudeKalmanfilter_initialize.h deleted file mode 100755 index 8b599be66..000000000 --- a/src/modules/attitude_estimator_ekf/codegen/attitudeKalmanfilter_initialize.h +++ /dev/null @@ -1,34 +0,0 @@ -/*
- * attitudeKalmanfilter_initialize.h
- *
- * Code generation for function 'attitudeKalmanfilter_initialize'
- *
- * C source code generated on: Sat Jan 19 15:25:29 2013
- *
- */
-
-#ifndef __ATTITUDEKALMANFILTER_INITIALIZE_H__
-#define __ATTITUDEKALMANFILTER_INITIALIZE_H__
-/* Include files */
-#include <math.h>
-#include <stddef.h>
-#include <stdlib.h>
-#include <string.h>
-#include "rt_defines.h"
-#include "rt_nonfinite.h"
-
-#include "rtwtypes.h"
-#include "attitudeKalmanfilter_types.h"
-
-/* Type Definitions */
-
-/* Named Constants */
-
-/* Variable Declarations */
-
-/* Variable Definitions */
-
-/* Function Declarations */
-extern void attitudeKalmanfilter_initialize(void);
-#endif
-/* End of code generation (attitudeKalmanfilter_initialize.h) */
diff --git a/src/modules/attitude_estimator_ekf/codegen/attitudeKalmanfilter_terminate.c b/src/modules/attitude_estimator_ekf/codegen/attitudeKalmanfilter_terminate.c deleted file mode 100755 index 7f9727419..000000000 --- a/src/modules/attitude_estimator_ekf/codegen/attitudeKalmanfilter_terminate.c +++ /dev/null @@ -1,31 +0,0 @@ -/*
- * attitudeKalmanfilter_terminate.c
- *
- * Code generation for function 'attitudeKalmanfilter_terminate'
- *
- * C source code generated on: Sat Jan 19 15:25:29 2013
- *
- */
-
-/* Include files */
-#include "rt_nonfinite.h"
-#include "attitudeKalmanfilter.h"
-#include "attitudeKalmanfilter_terminate.h"
-
-/* Type Definitions */
-
-/* Named Constants */
-
-/* Variable Declarations */
-
-/* Variable Definitions */
-
-/* Function Declarations */
-
-/* Function Definitions */
-void attitudeKalmanfilter_terminate(void)
-{
- /* (no terminate code required) */
-}
-
-/* End of code generation (attitudeKalmanfilter_terminate.c) */
diff --git a/src/modules/attitude_estimator_ekf/codegen/attitudeKalmanfilter_terminate.h b/src/modules/attitude_estimator_ekf/codegen/attitudeKalmanfilter_terminate.h deleted file mode 100755 index da84a5024..000000000 --- a/src/modules/attitude_estimator_ekf/codegen/attitudeKalmanfilter_terminate.h +++ /dev/null @@ -1,34 +0,0 @@ -/*
- * attitudeKalmanfilter_terminate.h
- *
- * Code generation for function 'attitudeKalmanfilter_terminate'
- *
- * C source code generated on: Sat Jan 19 15:25:29 2013
- *
- */
-
-#ifndef __ATTITUDEKALMANFILTER_TERMINATE_H__
-#define __ATTITUDEKALMANFILTER_TERMINATE_H__
-/* Include files */
-#include <math.h>
-#include <stddef.h>
-#include <stdlib.h>
-#include <string.h>
-#include "rt_defines.h"
-#include "rt_nonfinite.h"
-
-#include "rtwtypes.h"
-#include "attitudeKalmanfilter_types.h"
-
-/* Type Definitions */
-
-/* Named Constants */
-
-/* Variable Declarations */
-
-/* Variable Definitions */
-
-/* Function Declarations */
-extern void attitudeKalmanfilter_terminate(void);
-#endif
-/* End of code generation (attitudeKalmanfilter_terminate.h) */
diff --git a/src/modules/attitude_estimator_ekf/codegen/attitudeKalmanfilter_types.h b/src/modules/attitude_estimator_ekf/codegen/attitudeKalmanfilter_types.h deleted file mode 100755 index 30fd1e75e..000000000 --- a/src/modules/attitude_estimator_ekf/codegen/attitudeKalmanfilter_types.h +++ /dev/null @@ -1,16 +0,0 @@ -/*
- * attitudeKalmanfilter_types.h
- *
- * Code generation for function 'attitudeKalmanfilter'
- *
- * C source code generated on: Sat Jan 19 15:25:29 2013
- *
- */
-
-#ifndef __ATTITUDEKALMANFILTER_TYPES_H__
-#define __ATTITUDEKALMANFILTER_TYPES_H__
-
-/* Type Definitions */
-
-#endif
-/* End of code generation (attitudeKalmanfilter_types.h) */
diff --git a/src/modules/attitude_estimator_ekf/codegen/cross.c b/src/modules/attitude_estimator_ekf/codegen/cross.c deleted file mode 100755 index 84ada9002..000000000 --- a/src/modules/attitude_estimator_ekf/codegen/cross.c +++ /dev/null @@ -1,37 +0,0 @@ -/*
- * cross.c
- *
- * Code generation for function 'cross'
- *
- * C source code generated on: Sat Jan 19 15:25:29 2013
- *
- */
-
-/* Include files */
-#include "rt_nonfinite.h"
-#include "attitudeKalmanfilter.h"
-#include "cross.h"
-
-/* Type Definitions */
-
-/* Named Constants */
-
-/* Variable Declarations */
-
-/* Variable Definitions */
-
-/* Function Declarations */
-
-/* Function Definitions */
-
-/*
- *
- */
-void cross(const real32_T a[3], const real32_T b[3], real32_T c[3])
-{
- c[0] = a[1] * b[2] - a[2] * b[1];
- c[1] = a[2] * b[0] - a[0] * b[2];
- c[2] = a[0] * b[1] - a[1] * b[0];
-}
-
-/* End of code generation (cross.c) */
diff --git a/src/modules/attitude_estimator_ekf/codegen/cross.h b/src/modules/attitude_estimator_ekf/codegen/cross.h deleted file mode 100755 index e727f0684..000000000 --- a/src/modules/attitude_estimator_ekf/codegen/cross.h +++ /dev/null @@ -1,34 +0,0 @@ -/*
- * cross.h
- *
- * Code generation for function 'cross'
- *
- * C source code generated on: Sat Jan 19 15:25:29 2013
- *
- */
-
-#ifndef __CROSS_H__
-#define __CROSS_H__
-/* Include files */
-#include <math.h>
-#include <stddef.h>
-#include <stdlib.h>
-#include <string.h>
-#include "rt_defines.h"
-#include "rt_nonfinite.h"
-
-#include "rtwtypes.h"
-#include "attitudeKalmanfilter_types.h"
-
-/* Type Definitions */
-
-/* Named Constants */
-
-/* Variable Declarations */
-
-/* Variable Definitions */
-
-/* Function Declarations */
-extern void cross(const real32_T a[3], const real32_T b[3], real32_T c[3]);
-#endif
-/* End of code generation (cross.h) */
diff --git a/src/modules/attitude_estimator_ekf/codegen/eye.c b/src/modules/attitude_estimator_ekf/codegen/eye.c deleted file mode 100755 index b89ab58ef..000000000 --- a/src/modules/attitude_estimator_ekf/codegen/eye.c +++ /dev/null @@ -1,51 +0,0 @@ -/*
- * eye.c
- *
- * Code generation for function 'eye'
- *
- * C source code generated on: Sat Jan 19 15:25:29 2013
- *
- */
-
-/* Include files */
-#include "rt_nonfinite.h"
-#include "attitudeKalmanfilter.h"
-#include "eye.h"
-
-/* Type Definitions */
-
-/* Named Constants */
-
-/* Variable Declarations */
-
-/* Variable Definitions */
-
-/* Function Declarations */
-
-/* Function Definitions */
-
-/*
- *
- */
-void b_eye(real_T I[144])
-{
- int32_T i;
- memset(&I[0], 0, 144U * sizeof(real_T));
- for (i = 0; i < 12; i++) {
- I[i + 12 * i] = 1.0;
- }
-}
-
-/*
- *
- */
-void eye(real_T I[9])
-{
- int32_T i;
- memset(&I[0], 0, 9U * sizeof(real_T));
- for (i = 0; i < 3; i++) {
- I[i + 3 * i] = 1.0;
- }
-}
-
-/* End of code generation (eye.c) */
diff --git a/src/modules/attitude_estimator_ekf/codegen/eye.h b/src/modules/attitude_estimator_ekf/codegen/eye.h deleted file mode 100755 index 94fbe7671..000000000 --- a/src/modules/attitude_estimator_ekf/codegen/eye.h +++ /dev/null @@ -1,35 +0,0 @@ -/*
- * eye.h
- *
- * Code generation for function 'eye'
- *
- * C source code generated on: Sat Jan 19 15:25:29 2013
- *
- */
-
-#ifndef __EYE_H__
-#define __EYE_H__
-/* Include files */
-#include <math.h>
-#include <stddef.h>
-#include <stdlib.h>
-#include <string.h>
-#include "rt_defines.h"
-#include "rt_nonfinite.h"
-
-#include "rtwtypes.h"
-#include "attitudeKalmanfilter_types.h"
-
-/* Type Definitions */
-
-/* Named Constants */
-
-/* Variable Declarations */
-
-/* Variable Definitions */
-
-/* Function Declarations */
-extern void b_eye(real_T I[144]);
-extern void eye(real_T I[9]);
-#endif
-/* End of code generation (eye.h) */
diff --git a/src/modules/attitude_estimator_ekf/codegen/mrdivide.c b/src/modules/attitude_estimator_ekf/codegen/mrdivide.c deleted file mode 100755 index a810f22e4..000000000 --- a/src/modules/attitude_estimator_ekf/codegen/mrdivide.c +++ /dev/null @@ -1,357 +0,0 @@ -/*
- * mrdivide.c
- *
- * Code generation for function 'mrdivide'
- *
- * C source code generated on: Sat Jan 19 15:25:29 2013
- *
- */
-
-/* Include files */
-#include "rt_nonfinite.h"
-#include "attitudeKalmanfilter.h"
-#include "mrdivide.h"
-
-/* Type Definitions */
-
-/* Named Constants */
-
-/* Variable Declarations */
-
-/* Variable Definitions */
-
-/* Function Declarations */
-
-/* Function Definitions */
-
-/*
- *
- */
-void b_mrdivide(const real32_T A[36], const real32_T B[9], real32_T y[36])
-{
- real32_T b_A[9];
- int32_T rtemp;
- int32_T k;
- real32_T b_B[36];
- int32_T r1;
- int32_T r2;
- int32_T r3;
- real32_T maxval;
- real32_T a21;
- real32_T Y[36];
- for (rtemp = 0; rtemp < 3; rtemp++) {
- for (k = 0; k < 3; k++) {
- b_A[k + 3 * rtemp] = B[rtemp + 3 * k];
- }
- }
-
- for (rtemp = 0; rtemp < 12; rtemp++) {
- for (k = 0; k < 3; k++) {
- b_B[k + 3 * rtemp] = A[rtemp + 12 * k];
- }
- }
-
- r1 = 0;
- r2 = 1;
- r3 = 2;
- maxval = (real32_T)fabs(b_A[0]);
- a21 = (real32_T)fabs(b_A[1]);
- if (a21 > maxval) {
- maxval = a21;
- r1 = 1;
- r2 = 0;
- }
-
- if ((real32_T)fabs(b_A[2]) > maxval) {
- r1 = 2;
- r2 = 1;
- r3 = 0;
- }
-
- b_A[r2] /= b_A[r1];
- b_A[r3] /= b_A[r1];
- b_A[3 + r2] -= b_A[r2] * b_A[3 + r1];
- b_A[3 + r3] -= b_A[r3] * b_A[3 + r1];
- b_A[6 + r2] -= b_A[r2] * b_A[6 + r1];
- b_A[6 + r3] -= b_A[r3] * b_A[6 + r1];
- if ((real32_T)fabs(b_A[3 + r3]) > (real32_T)fabs(b_A[3 + r2])) {
- rtemp = r2;
- r2 = r3;
- r3 = rtemp;
- }
-
- b_A[3 + r3] /= b_A[3 + r2];
- b_A[6 + r3] -= b_A[3 + r3] * b_A[6 + r2];
- for (k = 0; k < 12; k++) {
- Y[3 * k] = b_B[r1 + 3 * k];
- Y[1 + 3 * k] = b_B[r2 + 3 * k] - Y[3 * k] * b_A[r2];
- Y[2 + 3 * k] = (b_B[r3 + 3 * k] - Y[3 * k] * b_A[r3]) - Y[1 + 3 * k] * b_A[3
- + r3];
- Y[2 + 3 * k] /= b_A[6 + r3];
- Y[3 * k] -= Y[2 + 3 * k] * b_A[6 + r1];
- Y[1 + 3 * k] -= Y[2 + 3 * k] * b_A[6 + r2];
- Y[1 + 3 * k] /= b_A[3 + r2];
- Y[3 * k] -= Y[1 + 3 * k] * b_A[3 + r1];
- Y[3 * k] /= b_A[r1];
- }
-
- for (rtemp = 0; rtemp < 3; rtemp++) {
- for (k = 0; k < 12; k++) {
- y[k + 12 * rtemp] = Y[rtemp + 3 * k];
- }
- }
-}
-
-/*
- *
- */
-void c_mrdivide(const real32_T A[72], const real32_T B[36], real32_T y[72])
-{
- real32_T b_A[36];
- int8_T ipiv[6];
- int32_T i3;
- int32_T iy;
- int32_T j;
- int32_T c;
- int32_T ix;
- real32_T temp;
- int32_T k;
- real32_T s;
- int32_T jy;
- int32_T ijA;
- real32_T Y[72];
- for (i3 = 0; i3 < 6; i3++) {
- for (iy = 0; iy < 6; iy++) {
- b_A[iy + 6 * i3] = B[i3 + 6 * iy];
- }
-
- ipiv[i3] = (int8_T)(1 + i3);
- }
-
- for (j = 0; j < 5; j++) {
- c = j * 7;
- iy = 0;
- ix = c;
- temp = (real32_T)fabs(b_A[c]);
- for (k = 2; k <= 6 - j; k++) {
- ix++;
- s = (real32_T)fabs(b_A[ix]);
- if (s > temp) {
- iy = k - 1;
- temp = s;
- }
- }
-
- if (b_A[c + iy] != 0.0F) {
- if (iy != 0) {
- ipiv[j] = (int8_T)((j + iy) + 1);
- ix = j;
- iy += j;
- for (k = 0; k < 6; k++) {
- temp = b_A[ix];
- b_A[ix] = b_A[iy];
- b_A[iy] = temp;
- ix += 6;
- iy += 6;
- }
- }
-
- i3 = (c - j) + 6;
- for (jy = c + 1; jy + 1 <= i3; jy++) {
- b_A[jy] /= b_A[c];
- }
- }
-
- iy = c;
- jy = c + 6;
- for (k = 1; k <= 5 - j; k++) {
- temp = b_A[jy];
- if (b_A[jy] != 0.0F) {
- ix = c + 1;
- i3 = (iy - j) + 12;
- for (ijA = 7 + iy; ijA + 1 <= i3; ijA++) {
- b_A[ijA] += b_A[ix] * -temp;
- ix++;
- }
- }
-
- jy += 6;
- iy += 6;
- }
- }
-
- for (i3 = 0; i3 < 12; i3++) {
- for (iy = 0; iy < 6; iy++) {
- Y[iy + 6 * i3] = A[i3 + 12 * iy];
- }
- }
-
- for (jy = 0; jy < 6; jy++) {
- if (ipiv[jy] != jy + 1) {
- for (j = 0; j < 12; j++) {
- temp = Y[jy + 6 * j];
- Y[jy + 6 * j] = Y[(ipiv[jy] + 6 * j) - 1];
- Y[(ipiv[jy] + 6 * j) - 1] = temp;
- }
- }
- }
-
- for (j = 0; j < 12; j++) {
- c = 6 * j;
- for (k = 0; k < 6; k++) {
- iy = 6 * k;
- if (Y[k + c] != 0.0F) {
- for (jy = k + 2; jy < 7; jy++) {
- Y[(jy + c) - 1] -= Y[k + c] * b_A[(jy + iy) - 1];
- }
- }
- }
- }
-
- for (j = 0; j < 12; j++) {
- c = 6 * j;
- for (k = 5; k > -1; k += -1) {
- iy = 6 * k;
- if (Y[k + c] != 0.0F) {
- Y[k + c] /= b_A[k + iy];
- for (jy = 0; jy + 1 <= k; jy++) {
- Y[jy + c] -= Y[k + c] * b_A[jy + iy];
- }
- }
- }
- }
-
- for (i3 = 0; i3 < 6; i3++) {
- for (iy = 0; iy < 12; iy++) {
- y[iy + 12 * i3] = Y[i3 + 6 * iy];
- }
- }
-}
-
-/*
- *
- */
-void mrdivide(const real32_T A[108], const real32_T B[81], real32_T y[108])
-{
- real32_T b_A[81];
- int8_T ipiv[9];
- int32_T i2;
- int32_T iy;
- int32_T j;
- int32_T c;
- int32_T ix;
- real32_T temp;
- int32_T k;
- real32_T s;
- int32_T jy;
- int32_T ijA;
- real32_T Y[108];
- for (i2 = 0; i2 < 9; i2++) {
- for (iy = 0; iy < 9; iy++) {
- b_A[iy + 9 * i2] = B[i2 + 9 * iy];
- }
-
- ipiv[i2] = (int8_T)(1 + i2);
- }
-
- for (j = 0; j < 8; j++) {
- c = j * 10;
- iy = 0;
- ix = c;
- temp = (real32_T)fabs(b_A[c]);
- for (k = 2; k <= 9 - j; k++) {
- ix++;
- s = (real32_T)fabs(b_A[ix]);
- if (s > temp) {
- iy = k - 1;
- temp = s;
- }
- }
-
- if (b_A[c + iy] != 0.0F) {
- if (iy != 0) {
- ipiv[j] = (int8_T)((j + iy) + 1);
- ix = j;
- iy += j;
- for (k = 0; k < 9; k++) {
- temp = b_A[ix];
- b_A[ix] = b_A[iy];
- b_A[iy] = temp;
- ix += 9;
- iy += 9;
- }
- }
-
- i2 = (c - j) + 9;
- for (jy = c + 1; jy + 1 <= i2; jy++) {
- b_A[jy] /= b_A[c];
- }
- }
-
- iy = c;
- jy = c + 9;
- for (k = 1; k <= 8 - j; k++) {
- temp = b_A[jy];
- if (b_A[jy] != 0.0F) {
- ix = c + 1;
- i2 = (iy - j) + 18;
- for (ijA = 10 + iy; ijA + 1 <= i2; ijA++) {
- b_A[ijA] += b_A[ix] * -temp;
- ix++;
- }
- }
-
- jy += 9;
- iy += 9;
- }
- }
-
- for (i2 = 0; i2 < 12; i2++) {
- for (iy = 0; iy < 9; iy++) {
- Y[iy + 9 * i2] = A[i2 + 12 * iy];
- }
- }
-
- for (jy = 0; jy < 9; jy++) {
- if (ipiv[jy] != jy + 1) {
- for (j = 0; j < 12; j++) {
- temp = Y[jy + 9 * j];
- Y[jy + 9 * j] = Y[(ipiv[jy] + 9 * j) - 1];
- Y[(ipiv[jy] + 9 * j) - 1] = temp;
- }
- }
- }
-
- for (j = 0; j < 12; j++) {
- c = 9 * j;
- for (k = 0; k < 9; k++) {
- iy = 9 * k;
- if (Y[k + c] != 0.0F) {
- for (jy = k + 2; jy < 10; jy++) {
- Y[(jy + c) - 1] -= Y[k + c] * b_A[(jy + iy) - 1];
- }
- }
- }
- }
-
- for (j = 0; j < 12; j++) {
- c = 9 * j;
- for (k = 8; k > -1; k += -1) {
- iy = 9 * k;
- if (Y[k + c] != 0.0F) {
- Y[k + c] /= b_A[k + iy];
- for (jy = 0; jy + 1 <= k; jy++) {
- Y[jy + c] -= Y[k + c] * b_A[jy + iy];
- }
- }
- }
- }
-
- for (i2 = 0; i2 < 9; i2++) {
- for (iy = 0; iy < 12; iy++) {
- y[iy + 12 * i2] = Y[i2 + 9 * iy];
- }
- }
-}
-
-/* End of code generation (mrdivide.c) */
diff --git a/src/modules/attitude_estimator_ekf/codegen/mrdivide.h b/src/modules/attitude_estimator_ekf/codegen/mrdivide.h deleted file mode 100755 index 2d3b0d51f..000000000 --- a/src/modules/attitude_estimator_ekf/codegen/mrdivide.h +++ /dev/null @@ -1,36 +0,0 @@ -/*
- * mrdivide.h
- *
- * Code generation for function 'mrdivide'
- *
- * C source code generated on: Sat Jan 19 15:25:29 2013
- *
- */
-
-#ifndef __MRDIVIDE_H__
-#define __MRDIVIDE_H__
-/* Include files */
-#include <math.h>
-#include <stddef.h>
-#include <stdlib.h>
-#include <string.h>
-#include "rt_defines.h"
-#include "rt_nonfinite.h"
-
-#include "rtwtypes.h"
-#include "attitudeKalmanfilter_types.h"
-
-/* Type Definitions */
-
-/* Named Constants */
-
-/* Variable Declarations */
-
-/* Variable Definitions */
-
-/* Function Declarations */
-extern void b_mrdivide(const real32_T A[36], const real32_T B[9], real32_T y[36]);
-extern void c_mrdivide(const real32_T A[72], const real32_T B[36], real32_T y[72]);
-extern void mrdivide(const real32_T A[108], const real32_T B[81], real32_T y[108]);
-#endif
-/* End of code generation (mrdivide.h) */
diff --git a/src/modules/attitude_estimator_ekf/codegen/norm.c b/src/modules/attitude_estimator_ekf/codegen/norm.c deleted file mode 100755 index 0c418cc7b..000000000 --- a/src/modules/attitude_estimator_ekf/codegen/norm.c +++ /dev/null @@ -1,54 +0,0 @@ -/*
- * norm.c
- *
- * Code generation for function 'norm'
- *
- * C source code generated on: Sat Jan 19 15:25:29 2013
- *
- */
-
-/* Include files */
-#include "rt_nonfinite.h"
-#include "attitudeKalmanfilter.h"
-#include "norm.h"
-
-/* Type Definitions */
-
-/* Named Constants */
-
-/* Variable Declarations */
-
-/* Variable Definitions */
-
-/* Function Declarations */
-
-/* Function Definitions */
-
-/*
- *
- */
-real32_T norm(const real32_T x[3])
-{
- real32_T y;
- real32_T scale;
- int32_T k;
- real32_T absxk;
- real32_T t;
- y = 0.0F;
- scale = 1.17549435E-38F;
- for (k = 0; k < 3; k++) {
- absxk = (real32_T)fabs(x[k]);
- if (absxk > scale) {
- t = scale / absxk;
- y = 1.0F + y * t * t;
- scale = absxk;
- } else {
- t = absxk / scale;
- y += t * t;
- }
- }
-
- return scale * (real32_T)sqrt(y);
-}
-
-/* End of code generation (norm.c) */
diff --git a/src/modules/attitude_estimator_ekf/codegen/norm.h b/src/modules/attitude_estimator_ekf/codegen/norm.h deleted file mode 100755 index 60cf77b57..000000000 --- a/src/modules/attitude_estimator_ekf/codegen/norm.h +++ /dev/null @@ -1,34 +0,0 @@ -/*
- * norm.h
- *
- * Code generation for function 'norm'
- *
- * C source code generated on: Sat Jan 19 15:25:29 2013
- *
- */
-
-#ifndef __NORM_H__
-#define __NORM_H__
-/* Include files */
-#include <math.h>
-#include <stddef.h>
-#include <stdlib.h>
-#include <string.h>
-#include "rt_defines.h"
-#include "rt_nonfinite.h"
-
-#include "rtwtypes.h"
-#include "attitudeKalmanfilter_types.h"
-
-/* Type Definitions */
-
-/* Named Constants */
-
-/* Variable Declarations */
-
-/* Variable Definitions */
-
-/* Function Declarations */
-extern real32_T norm(const real32_T x[3]);
-#endif
-/* End of code generation (norm.h) */
diff --git a/src/modules/attitude_estimator_ekf/codegen/rdivide.c b/src/modules/attitude_estimator_ekf/codegen/rdivide.c deleted file mode 100755 index d035dae5e..000000000 --- a/src/modules/attitude_estimator_ekf/codegen/rdivide.c +++ /dev/null @@ -1,38 +0,0 @@ -/*
- * rdivide.c
- *
- * Code generation for function 'rdivide'
- *
- * C source code generated on: Sat Jan 19 15:25:29 2013
- *
- */
-
-/* Include files */
-#include "rt_nonfinite.h"
-#include "attitudeKalmanfilter.h"
-#include "rdivide.h"
-
-/* Type Definitions */
-
-/* Named Constants */
-
-/* Variable Declarations */
-
-/* Variable Definitions */
-
-/* Function Declarations */
-
-/* Function Definitions */
-
-/*
- *
- */
-void rdivide(const real32_T x[3], real32_T y, real32_T z[3])
-{
- int32_T i;
- for (i = 0; i < 3; i++) {
- z[i] = x[i] / y;
- }
-}
-
-/* End of code generation (rdivide.c) */
diff --git a/src/modules/attitude_estimator_ekf/codegen/rdivide.h b/src/modules/attitude_estimator_ekf/codegen/rdivide.h deleted file mode 100755 index 4bbebebe2..000000000 --- a/src/modules/attitude_estimator_ekf/codegen/rdivide.h +++ /dev/null @@ -1,34 +0,0 @@ -/*
- * rdivide.h
- *
- * Code generation for function 'rdivide'
- *
- * C source code generated on: Sat Jan 19 15:25:29 2013
- *
- */
-
-#ifndef __RDIVIDE_H__
-#define __RDIVIDE_H__
-/* Include files */
-#include <math.h>
-#include <stddef.h>
-#include <stdlib.h>
-#include <string.h>
-#include "rt_defines.h"
-#include "rt_nonfinite.h"
-
-#include "rtwtypes.h"
-#include "attitudeKalmanfilter_types.h"
-
-/* Type Definitions */
-
-/* Named Constants */
-
-/* Variable Declarations */
-
-/* Variable Definitions */
-
-/* Function Declarations */
-extern void rdivide(const real32_T x[3], real32_T y, real32_T z[3]);
-#endif
-/* End of code generation (rdivide.h) */
diff --git a/src/modules/attitude_estimator_ekf/codegen/rtGetInf.c b/src/modules/attitude_estimator_ekf/codegen/rtGetInf.c deleted file mode 100755 index 34164d104..000000000 --- a/src/modules/attitude_estimator_ekf/codegen/rtGetInf.c +++ /dev/null @@ -1,139 +0,0 @@ -/*
- * rtGetInf.c
- *
- * Code generation for function 'attitudeKalmanfilter'
- *
- * C source code generated on: Sat Jan 19 15:25:29 2013
- *
- */
-
-/*
- * Abstract:
- * MATLAB for code generation function to initialize non-finite, Inf and MinusInf
- */
-#include "rtGetInf.h"
-#define NumBitsPerChar 8U
-
-/* Function: rtGetInf ==================================================
- * Abstract:
- * Initialize rtInf needed by the generated code.
- * Inf is initialized as non-signaling. Assumes IEEE.
- */
-real_T rtGetInf(void)
-{
- size_t bitsPerReal = sizeof(real_T) * (NumBitsPerChar);
- real_T inf = 0.0;
- if (bitsPerReal == 32U) {
- inf = rtGetInfF();
- } else {
- uint16_T one = 1U;
- enum {
- LittleEndian,
- BigEndian
- } machByteOrder = (*((uint8_T *) &one) == 1U) ? LittleEndian : BigEndian;
- switch (machByteOrder) {
- case LittleEndian:
- {
- union {
- LittleEndianIEEEDouble bitVal;
- real_T fltVal;
- } tmpVal;
-
- tmpVal.bitVal.words.wordH = 0x7FF00000U;
- tmpVal.bitVal.words.wordL = 0x00000000U;
- inf = tmpVal.fltVal;
- break;
- }
-
- case BigEndian:
- {
- union {
- BigEndianIEEEDouble bitVal;
- real_T fltVal;
- } tmpVal;
-
- tmpVal.bitVal.words.wordH = 0x7FF00000U;
- tmpVal.bitVal.words.wordL = 0x00000000U;
- inf = tmpVal.fltVal;
- break;
- }
- }
- }
-
- return inf;
-}
-
-/* Function: rtGetInfF ==================================================
- * Abstract:
- * Initialize rtInfF needed by the generated code.
- * Inf is initialized as non-signaling. Assumes IEEE.
- */
-real32_T rtGetInfF(void)
-{
- IEEESingle infF;
- infF.wordL.wordLuint = 0x7F800000U;
- return infF.wordL.wordLreal;
-}
-
-/* Function: rtGetMinusInf ==================================================
- * Abstract:
- * Initialize rtMinusInf needed by the generated code.
- * Inf is initialized as non-signaling. Assumes IEEE.
- */
-real_T rtGetMinusInf(void)
-{
- size_t bitsPerReal = sizeof(real_T) * (NumBitsPerChar);
- real_T minf = 0.0;
- if (bitsPerReal == 32U) {
- minf = rtGetMinusInfF();
- } else {
- uint16_T one = 1U;
- enum {
- LittleEndian,
- BigEndian
- } machByteOrder = (*((uint8_T *) &one) == 1U) ? LittleEndian : BigEndian;
- switch (machByteOrder) {
- case LittleEndian:
- {
- union {
- LittleEndianIEEEDouble bitVal;
- real_T fltVal;
- } tmpVal;
-
- tmpVal.bitVal.words.wordH = 0xFFF00000U;
- tmpVal.bitVal.words.wordL = 0x00000000U;
- minf = tmpVal.fltVal;
- break;
- }
-
- case BigEndian:
- {
- union {
- BigEndianIEEEDouble bitVal;
- real_T fltVal;
- } tmpVal;
-
- tmpVal.bitVal.words.wordH = 0xFFF00000U;
- tmpVal.bitVal.words.wordL = 0x00000000U;
- minf = tmpVal.fltVal;
- break;
- }
- }
- }
-
- return minf;
-}
-
-/* Function: rtGetMinusInfF ==================================================
- * Abstract:
- * Initialize rtMinusInfF needed by the generated code.
- * Inf is initialized as non-signaling. Assumes IEEE.
- */
-real32_T rtGetMinusInfF(void)
-{
- IEEESingle minfF;
- minfF.wordL.wordLuint = 0xFF800000U;
- return minfF.wordL.wordLreal;
-}
-
-/* End of code generation (rtGetInf.c) */
diff --git a/src/modules/attitude_estimator_ekf/codegen/rtGetInf.h b/src/modules/attitude_estimator_ekf/codegen/rtGetInf.h deleted file mode 100755 index 145373cd0..000000000 --- a/src/modules/attitude_estimator_ekf/codegen/rtGetInf.h +++ /dev/null @@ -1,23 +0,0 @@ -/*
- * rtGetInf.h
- *
- * Code generation for function 'attitudeKalmanfilter'
- *
- * C source code generated on: Sat Jan 19 15:25:29 2013
- *
- */
-
-#ifndef __RTGETINF_H__
-#define __RTGETINF_H__
-
-#include <stddef.h>
-#include "rtwtypes.h"
-#include "rt_nonfinite.h"
-
-extern real_T rtGetInf(void);
-extern real32_T rtGetInfF(void);
-extern real_T rtGetMinusInf(void);
-extern real32_T rtGetMinusInfF(void);
-
-#endif
-/* End of code generation (rtGetInf.h) */
diff --git a/src/modules/attitude_estimator_ekf/codegen/rtGetNaN.c b/src/modules/attitude_estimator_ekf/codegen/rtGetNaN.c deleted file mode 100755 index d84ca9573..000000000 --- a/src/modules/attitude_estimator_ekf/codegen/rtGetNaN.c +++ /dev/null @@ -1,96 +0,0 @@ -/*
- * rtGetNaN.c
- *
- * Code generation for function 'attitudeKalmanfilter'
- *
- * C source code generated on: Sat Jan 19 15:25:29 2013
- *
- */
-
-/*
- * Abstract:
- * MATLAB for code generation function to initialize non-finite, NaN
- */
-#include "rtGetNaN.h"
-#define NumBitsPerChar 8U
-
-/* Function: rtGetNaN ==================================================
- * Abstract:
- * Initialize rtNaN needed by the generated code.
- * NaN is initialized as non-signaling. Assumes IEEE.
- */
-real_T rtGetNaN(void)
-{
- size_t bitsPerReal = sizeof(real_T) * (NumBitsPerChar);
- real_T nan = 0.0;
- if (bitsPerReal == 32U) {
- nan = rtGetNaNF();
- } else {
- uint16_T one = 1U;
- enum {
- LittleEndian,
- BigEndian
- } machByteOrder = (*((uint8_T *) &one) == 1U) ? LittleEndian : BigEndian;
- switch (machByteOrder) {
- case LittleEndian:
- {
- union {
- LittleEndianIEEEDouble bitVal;
- real_T fltVal;
- } tmpVal;
-
- tmpVal.bitVal.words.wordH = 0xFFF80000U;
- tmpVal.bitVal.words.wordL = 0x00000000U;
- nan = tmpVal.fltVal;
- break;
- }
-
- case BigEndian:
- {
- union {
- BigEndianIEEEDouble bitVal;
- real_T fltVal;
- } tmpVal;
-
- tmpVal.bitVal.words.wordH = 0x7FFFFFFFU;
- tmpVal.bitVal.words.wordL = 0xFFFFFFFFU;
- nan = tmpVal.fltVal;
- break;
- }
- }
- }
-
- return nan;
-}
-
-/* Function: rtGetNaNF ==================================================
- * Abstract:
- * Initialize rtNaNF needed by the generated code.
- * NaN is initialized as non-signaling. Assumes IEEE.
- */
-real32_T rtGetNaNF(void)
-{
- IEEESingle nanF = { { 0 } };
- uint16_T one = 1U;
- enum {
- LittleEndian,
- BigEndian
- } machByteOrder = (*((uint8_T *) &one) == 1U) ? LittleEndian : BigEndian;
- switch (machByteOrder) {
- case LittleEndian:
- {
- nanF.wordL.wordLuint = 0xFFC00000U;
- break;
- }
-
- case BigEndian:
- {
- nanF.wordL.wordLuint = 0x7FFFFFFFU;
- break;
- }
- }
-
- return nanF.wordL.wordLreal;
-}
-
-/* End of code generation (rtGetNaN.c) */
diff --git a/src/modules/attitude_estimator_ekf/codegen/rtGetNaN.h b/src/modules/attitude_estimator_ekf/codegen/rtGetNaN.h deleted file mode 100755 index 65fdaa96f..000000000 --- a/src/modules/attitude_estimator_ekf/codegen/rtGetNaN.h +++ /dev/null @@ -1,21 +0,0 @@ -/*
- * rtGetNaN.h
- *
- * Code generation for function 'attitudeKalmanfilter'
- *
- * C source code generated on: Sat Jan 19 15:25:29 2013
- *
- */
-
-#ifndef __RTGETNAN_H__
-#define __RTGETNAN_H__
-
-#include <stddef.h>
-#include "rtwtypes.h"
-#include "rt_nonfinite.h"
-
-extern real_T rtGetNaN(void);
-extern real32_T rtGetNaNF(void);
-
-#endif
-/* End of code generation (rtGetNaN.h) */
diff --git a/src/modules/attitude_estimator_ekf/codegen/rt_defines.h b/src/modules/attitude_estimator_ekf/codegen/rt_defines.h deleted file mode 100755 index 356498363..000000000 --- a/src/modules/attitude_estimator_ekf/codegen/rt_defines.h +++ /dev/null @@ -1,24 +0,0 @@ -/*
- * rt_defines.h
- *
- * Code generation for function 'attitudeKalmanfilter'
- *
- * C source code generated on: Sat Jan 19 15:25:29 2013
- *
- */
-
-#ifndef __RT_DEFINES_H__
-#define __RT_DEFINES_H__
-
-#include <stdlib.h>
-
-#define RT_PI 3.14159265358979323846
-#define RT_PIF 3.1415927F
-#define RT_LN_10 2.30258509299404568402
-#define RT_LN_10F 2.3025851F
-#define RT_LOG10E 0.43429448190325182765
-#define RT_LOG10EF 0.43429449F
-#define RT_E 2.7182818284590452354
-#define RT_EF 2.7182817F
-#endif
-/* End of code generation (rt_defines.h) */
diff --git a/src/modules/attitude_estimator_ekf/codegen/rt_nonfinite.c b/src/modules/attitude_estimator_ekf/codegen/rt_nonfinite.c deleted file mode 100755 index 303d1d9d2..000000000 --- a/src/modules/attitude_estimator_ekf/codegen/rt_nonfinite.c +++ /dev/null @@ -1,87 +0,0 @@ -/*
- * rt_nonfinite.c
- *
- * Code generation for function 'attitudeKalmanfilter'
- *
- * C source code generated on: Sat Jan 19 15:25:29 2013
- *
- */
-
-/*
- * Abstract:
- * MATLAB for code generation function to initialize non-finites,
- * (Inf, NaN and -Inf).
- */
-#include "rt_nonfinite.h"
-#include "rtGetNaN.h"
-#include "rtGetInf.h"
-
-real_T rtInf;
-real_T rtMinusInf;
-real_T rtNaN;
-real32_T rtInfF;
-real32_T rtMinusInfF;
-real32_T rtNaNF;
-
-/* Function: rt_InitInfAndNaN ==================================================
- * Abstract:
- * Initialize the rtInf, rtMinusInf, and rtNaN needed by the
- * generated code. NaN is initialized as non-signaling. Assumes IEEE.
- */
-void rt_InitInfAndNaN(size_t realSize)
-{
- (void) (realSize);
- rtNaN = rtGetNaN();
- rtNaNF = rtGetNaNF();
- rtInf = rtGetInf();
- rtInfF = rtGetInfF();
- rtMinusInf = rtGetMinusInf();
- rtMinusInfF = rtGetMinusInfF();
-}
-
-/* Function: rtIsInf ==================================================
- * Abstract:
- * Test if value is infinite
- */
-boolean_T rtIsInf(real_T value)
-{
- return ((value==rtInf || value==rtMinusInf) ? 1U : 0U);
-}
-
-/* Function: rtIsInfF =================================================
- * Abstract:
- * Test if single-precision value is infinite
- */
-boolean_T rtIsInfF(real32_T value)
-{
- return(((value)==rtInfF || (value)==rtMinusInfF) ? 1U : 0U);
-}
-
-/* Function: rtIsNaN ==================================================
- * Abstract:
- * Test if value is not a number
- */
-boolean_T rtIsNaN(real_T value)
-{
-#if defined(_MSC_VER) && (_MSC_VER <= 1200)
- return _isnan(value)? TRUE:FALSE;
-#else
- return (value!=value)? 1U:0U;
-#endif
-}
-
-/* Function: rtIsNaNF =================================================
- * Abstract:
- * Test if single-precision value is not a number
- */
-boolean_T rtIsNaNF(real32_T value)
-{
-#if defined(_MSC_VER) && (_MSC_VER <= 1200)
- return _isnan((real_T)value)? true:false;
-#else
- return (value!=value)? 1U:0U;
-#endif
-}
-
-
-/* End of code generation (rt_nonfinite.c) */
diff --git a/src/modules/attitude_estimator_ekf/codegen/rt_nonfinite.h b/src/modules/attitude_estimator_ekf/codegen/rt_nonfinite.h deleted file mode 100755 index bd56b30d9..000000000 --- a/src/modules/attitude_estimator_ekf/codegen/rt_nonfinite.h +++ /dev/null @@ -1,53 +0,0 @@ -/*
- * rt_nonfinite.h
- *
- * Code generation for function 'attitudeKalmanfilter'
- *
- * C source code generated on: Sat Jan 19 15:25:29 2013
- *
- */
-
-#ifndef __RT_NONFINITE_H__
-#define __RT_NONFINITE_H__
-
-#if defined(_MSC_VER) && (_MSC_VER <= 1200)
-#include <float.h>
-#endif
-#include <stddef.h>
-#include "rtwtypes.h"
-
-extern real_T rtInf;
-extern real_T rtMinusInf;
-extern real_T rtNaN;
-extern real32_T rtInfF;
-extern real32_T rtMinusInfF;
-extern real32_T rtNaNF;
-extern void rt_InitInfAndNaN(size_t realSize);
-extern boolean_T rtIsInf(real_T value);
-extern boolean_T rtIsInfF(real32_T value);
-extern boolean_T rtIsNaN(real_T value);
-extern boolean_T rtIsNaNF(real32_T value);
-
-typedef struct {
- struct {
- uint32_T wordH;
- uint32_T wordL;
- } words;
-} BigEndianIEEEDouble;
-
-typedef struct {
- struct {
- uint32_T wordL;
- uint32_T wordH;
- } words;
-} LittleEndianIEEEDouble;
-
-typedef struct {
- union {
- real32_T wordLreal;
- uint32_T wordLuint;
- } wordL;
-} IEEESingle;
-
-#endif
-/* End of code generation (rt_nonfinite.h) */
diff --git a/src/modules/attitude_estimator_ekf/codegen/rtwtypes.h b/src/modules/attitude_estimator_ekf/codegen/rtwtypes.h index 9a5c96267..b5a02a7a6 100755..100644 --- a/src/modules/attitude_estimator_ekf/codegen/rtwtypes.h +++ b/src/modules/attitude_estimator_ekf/codegen/rtwtypes.h @@ -1,159 +1,160 @@ -/*
- * rtwtypes.h
- *
- * Code generation for function 'attitudeKalmanfilter'
- *
- * C source code generated on: Sat Jan 19 15:25:29 2013
- *
- */
-
-#ifndef __RTWTYPES_H__
-#define __RTWTYPES_H__
-#ifndef TRUE
-# define TRUE (1U)
-#endif
-#ifndef FALSE
-# define FALSE (0U)
-#endif
-#ifndef __TMWTYPES__
-#define __TMWTYPES__
-
-#include <limits.h>
-
-/*=======================================================================*
- * Target hardware information
- * Device type: Generic->MATLAB Host Computer
- * Number of bits: char: 8 short: 16 int: 32
- * long: 32 native word size: 32
- * Byte ordering: LittleEndian
- * Signed integer division rounds to: Zero
- * Shift right on a signed integer as arithmetic shift: on
- *=======================================================================*/
-
-/*=======================================================================*
- * Fixed width word size data types: *
- * int8_T, int16_T, int32_T - signed 8, 16, or 32 bit integers *
- * uint8_T, uint16_T, uint32_T - unsigned 8, 16, or 32 bit integers *
- * real32_T, real64_T - 32 and 64 bit floating point numbers *
- *=======================================================================*/
-
-typedef signed char int8_T;
-typedef unsigned char uint8_T;
-typedef short int16_T;
-typedef unsigned short uint16_T;
-typedef int int32_T;
-typedef unsigned int uint32_T;
-typedef float real32_T;
-typedef double real64_T;
-
-/*===========================================================================*
- * Generic type definitions: real_T, time_T, boolean_T, int_T, uint_T, *
- * ulong_T, char_T and byte_T. *
- *===========================================================================*/
-
-typedef double real_T;
-typedef double time_T;
-typedef unsigned char boolean_T;
-typedef int int_T;
-typedef unsigned int uint_T;
-typedef unsigned long ulong_T;
-typedef char char_T;
-typedef char_T byte_T;
-
-/*===========================================================================*
- * Complex number type definitions *
- *===========================================================================*/
-#define CREAL_T
- typedef struct {
- real32_T re;
- real32_T im;
- } creal32_T;
-
- typedef struct {
- real64_T re;
- real64_T im;
- } creal64_T;
-
- typedef struct {
- real_T re;
- real_T im;
- } creal_T;
-
- typedef struct {
- int8_T re;
- int8_T im;
- } cint8_T;
-
- typedef struct {
- uint8_T re;
- uint8_T im;
- } cuint8_T;
-
- typedef struct {
- int16_T re;
- int16_T im;
- } cint16_T;
-
- typedef struct {
- uint16_T re;
- uint16_T im;
- } cuint16_T;
-
- typedef struct {
- int32_T re;
- int32_T im;
- } cint32_T;
-
- typedef struct {
- uint32_T re;
- uint32_T im;
- } cuint32_T;
-
-
-/*=======================================================================*
- * Min and Max: *
- * int8_T, int16_T, int32_T - signed 8, 16, or 32 bit integers *
- * uint8_T, uint16_T, uint32_T - unsigned 8, 16, or 32 bit integers *
- *=======================================================================*/
-
-#define MAX_int8_T ((int8_T)(127))
-#define MIN_int8_T ((int8_T)(-128))
-#define MAX_uint8_T ((uint8_T)(255))
-#define MIN_uint8_T ((uint8_T)(0))
-#define MAX_int16_T ((int16_T)(32767))
-#define MIN_int16_T ((int16_T)(-32768))
-#define MAX_uint16_T ((uint16_T)(65535))
-#define MIN_uint16_T ((uint16_T)(0))
-#define MAX_int32_T ((int32_T)(2147483647))
-#define MIN_int32_T ((int32_T)(-2147483647-1))
-#define MAX_uint32_T ((uint32_T)(0xFFFFFFFFU))
-#define MIN_uint32_T ((uint32_T)(0))
-
-/* Logical type definitions */
-#if !defined(__cplusplus) && !defined(__true_false_are_keywords)
-# ifndef false
-# define false (0U)
-# endif
-# ifndef true
-# define true (1U)
-# endif
-#endif
-
-/*
- * MATLAB for code generation assumes the code is compiled on a target using a 2's compliment representation
- * for signed integer values.
- */
-#if ((SCHAR_MIN + 1) != -SCHAR_MAX)
-#error "This code must be compiled using a 2's complement representation for signed integer values"
-#endif
-
-/*
- * Maximum length of a MATLAB identifier (function/variable)
- * including the null-termination character. Referenced by
- * rt_logging.c and rt_matrx.c.
- */
-#define TMW_NAME_LENGTH_MAX 64
-
-#endif
-#endif
-/* End of code generation (rtwtypes.h) */
+/* + * rtwtypes.h + * + * Code generation for function 'AttitudeEKF' + * + * C source code generated on: Thu Aug 21 11:17:28 2014 + * + */ + +#ifndef __RTWTYPES_H__ +#define __RTWTYPES_H__ +#ifndef TRUE +# define TRUE (1U) +#endif +#ifndef FALSE +# define FALSE (0U) +#endif +#ifndef __TMWTYPES__ +#define __TMWTYPES__ + +#include <limits.h> + +/*=======================================================================* + * Target hardware information + * Device type: ARM Compatible->ARM Cortex + * Number of bits: char: 8 short: 16 int: 32 + * long: 32 + * native word size: 32 + * Byte ordering: LittleEndian + * Signed integer division rounds to: Undefined + * Shift right on a signed integer as arithmetic shift: on + *=======================================================================*/ + +/*=======================================================================* + * Fixed width word size data types: * + * int8_T, int16_T, int32_T - signed 8, 16, or 32 bit integers * + * uint8_T, uint16_T, uint32_T - unsigned 8, 16, or 32 bit integers * + * real32_T, real64_T - 32 and 64 bit floating point numbers * + *=======================================================================*/ + +typedef signed char int8_T; +typedef unsigned char uint8_T; +typedef short int16_T; +typedef unsigned short uint16_T; +typedef int int32_T; +typedef unsigned int uint32_T; +typedef float real32_T; +typedef double real64_T; + +/*===========================================================================* + * Generic type definitions: real_T, time_T, boolean_T, int_T, uint_T, * + * ulong_T, char_T and byte_T. * + *===========================================================================*/ + +typedef double real_T; +typedef double time_T; +typedef unsigned char boolean_T; +typedef int int_T; +typedef unsigned int uint_T; +typedef unsigned long ulong_T; +typedef char char_T; +typedef char_T byte_T; + +/*===========================================================================* + * Complex number type definitions * + *===========================================================================*/ +#define CREAL_T + typedef struct { + real32_T re; + real32_T im; + } creal32_T; + + typedef struct { + real64_T re; + real64_T im; + } creal64_T; + + typedef struct { + real_T re; + real_T im; + } creal_T; + + typedef struct { + int8_T re; + int8_T im; + } cint8_T; + + typedef struct { + uint8_T re; + uint8_T im; + } cuint8_T; + + typedef struct { + int16_T re; + int16_T im; + } cint16_T; + + typedef struct { + uint16_T re; + uint16_T im; + } cuint16_T; + + typedef struct { + int32_T re; + int32_T im; + } cint32_T; + + typedef struct { + uint32_T re; + uint32_T im; + } cuint32_T; + + +/*=======================================================================* + * Min and Max: * + * int8_T, int16_T, int32_T - signed 8, 16, or 32 bit integers * + * uint8_T, uint16_T, uint32_T - unsigned 8, 16, or 32 bit integers * + *=======================================================================*/ + +#define MAX_int8_T ((int8_T)(127)) +#define MIN_int8_T ((int8_T)(-128)) +#define MAX_uint8_T ((uint8_T)(255)) +#define MIN_uint8_T ((uint8_T)(0)) +#define MAX_int16_T ((int16_T)(32767)) +#define MIN_int16_T ((int16_T)(-32768)) +#define MAX_uint16_T ((uint16_T)(65535)) +#define MIN_uint16_T ((uint16_T)(0)) +#define MAX_int32_T ((int32_T)(2147483647)) +#define MIN_int32_T ((int32_T)(-2147483647-1)) +#define MAX_uint32_T ((uint32_T)(0xFFFFFFFFU)) +#define MIN_uint32_T ((uint32_T)(0)) + +/* Logical type definitions */ +#if !defined(__cplusplus) && !defined(__true_false_are_keywords) +# ifndef false +# define false (0U) +# endif +# ifndef true +# define true (1U) +# endif +#endif + +/* + * MATLAB for code generation assumes the code is compiled on a target using a 2's compliment representation + * for signed integer values. + */ +#if ((SCHAR_MIN + 1) != -SCHAR_MAX) +#error "This code must be compiled using a 2's complement representation for signed integer values" +#endif + +/* + * Maximum length of a MATLAB identifier (function/variable) + * including the null-termination character. Referenced by + * rt_logging.c and rt_matrx.c. + */ +#define TMW_NAME_LENGTH_MAX 64 + +#endif +#endif +/* End of code generation (rtwtypes.h) */ diff --git a/src/modules/attitude_estimator_ekf/module.mk b/src/modules/attitude_estimator_ekf/module.mk index 99d0c5bf2..749b0a91c 100644 --- a/src/modules/attitude_estimator_ekf/module.mk +++ b/src/modules/attitude_estimator_ekf/module.mk @@ -39,16 +39,6 @@ MODULE_COMMAND = attitude_estimator_ekf SRCS = attitude_estimator_ekf_main.cpp \ attitude_estimator_ekf_params.c \ - codegen/eye.c \ - codegen/attitudeKalmanfilter.c \ - codegen/mrdivide.c \ - codegen/rdivide.c \ - codegen/attitudeKalmanfilter_initialize.c \ - codegen/attitudeKalmanfilter_terminate.c \ - codegen/rt_nonfinite.c \ - codegen/rtGetInf.c \ - codegen/rtGetNaN.c \ - codegen/norm.c \ - codegen/cross.c + codegen/AttitudeEKF.c MODULE_STACKSIZE = 1200 |