mirror of
https://gitee.com/mirrors_PX4/PX4-Autopilot.git
synced 2026-06-30 02:00:35 +08:00
update attitude_estimator_ekf, includes matlab
This adds the latest c implementation (matlab coder) of attitude_estimator_ekf, the .m matlab script and the .prj file with the settings to export the matlab code to c
This commit is contained in:
Thomas Gubler
@@ -0,0 +1,286 @@
|
||||
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];
|
||||
%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;
|
||||
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];
|
||||
%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);
|
||||
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];
|
||||
|
||||
%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);
|
||||
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];
|
||||
%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);
|
||||
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];
|
||||
|
||||
@@ -0,0 +1,503 @@
|
||||
<?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="2197173368">
|
||||
<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>200000</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.StackUsageMax />
|
||||
<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.15.5-1-ARCH</osver>
|
||||
<os32>false</os32>
|
||||
<os64>true</os64>
|
||||
<arch>glnxa64</arch>
|
||||
<matlab>true</matlab>
|
||||
</platform>
|
||||
</configuration>
|
||||
</deployment-project>
|
||||
|
||||
@@ -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
|
||||
}
|
||||
@@ -206,6 +206,8 @@ const unsigned int loop_interval_alarm = 6500; // loop interval in microseconds
|
||||
0, 0, 1.f
|
||||
}; /**< init: identity matrix */
|
||||
|
||||
float debugOutput[4] = { 0.0f };
|
||||
|
||||
// print text
|
||||
printf("Extended Kalman Filter Attitude Estimator initialized..\n\n");
|
||||
fflush(stdout);
|
||||
@@ -213,7 +215,7 @@ const unsigned int loop_interval_alarm = 6500; // loop interval in microseconds
|
||||
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();
|
||||
@@ -277,9 +279,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);
|
||||
@@ -508,8 +510,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
|
||||
false, // use_inertia_matrix
|
||||
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])) {
|
||||
|
||||
@@ -50,7 +50,6 @@ PARAM_DEFINE_FLOAT(EKF_ATT_V3_Q1, 0.08f);
|
||||
PARAM_DEFINE_FLOAT(EKF_ATT_V3_Q2, 0.009f);
|
||||
/* gyro offsets process noise */
|
||||
PARAM_DEFINE_FLOAT(EKF_ATT_V3_Q3, 0.005f);
|
||||
PARAM_DEFINE_FLOAT(EKF_ATT_V3_Q4, 0.0f);
|
||||
|
||||
/* gyro measurement noise */
|
||||
PARAM_DEFINE_FLOAT(EKF_ATT_V4_R0, 0.0008f);
|
||||
@@ -58,14 +57,38 @@ PARAM_DEFINE_FLOAT(EKF_ATT_V4_R0, 0.0008f);
|
||||
PARAM_DEFINE_FLOAT(EKF_ATT_V4_R1, 10000.0f);
|
||||
/* mag measurement noise */
|
||||
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);
|
||||
|
||||
|
||||
int parameters_init(struct attitude_estimator_ekf_param_handles *h)
|
||||
{
|
||||
/* PID parameters */
|
||||
@@ -73,17 +96,19 @@ 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");
|
||||
|
||||
return OK;
|
||||
}
|
||||
|
||||
@@ -93,17 +118,19 @@ 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]));
|
||||
}
|
||||
|
||||
return OK;
|
||||
}
|
||||
|
||||
@@ -42,8 +42,9 @@
|
||||
#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];
|
||||
float roll_off;
|
||||
float pitch_off;
|
||||
float yaw_off;
|
||||
@@ -52,8 +53,9 @@ 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 mag_decl;
|
||||
param_t acc_comp;
|
||||
};
|
||||
|
||||
File diff suppressed because it is too large
Load Diff
@@ -0,0 +1,26 @@
|
||||
/*
|
||||
* AttitudeEKF.h
|
||||
*
|
||||
* Code generation for function 'AttitudeEKF'
|
||||
*
|
||||
* C source code generated on: Fri Jul 25 14:06:41 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) */
|
||||
@@ -0,0 +1,17 @@
|
||||
/*
|
||||
* AttitudeEKF_types.h
|
||||
*
|
||||
* Code generation for function 'AttitudeEKF'
|
||||
*
|
||||
* C source code generated on: Fri Jul 25 14:06:41 2014
|
||||
*
|
||||
*/
|
||||
|
||||
#ifndef __ATTITUDEEKF_TYPES_H__
|
||||
#define __ATTITUDEEKF_TYPES_H__
|
||||
|
||||
/* Include files */
|
||||
#include "rtwtypes.h"
|
||||
|
||||
#endif
|
||||
/* End of code generation (AttitudeEKF_types.h) */
|
||||
File diff suppressed because it is too large
Load Diff
@@ -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) */
|
||||
@@ -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) */
|
||||
@@ -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) */
|
||||
@@ -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) */
|
||||
@@ -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) */
|
||||
@@ -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) */
|
||||
@@ -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) */
|
||||
@@ -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) */
|
||||
@@ -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) */
|
||||
@@ -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) */
|
||||
@@ -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) */
|
||||
@@ -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) */
|
||||
@@ -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) */
|
||||
@@ -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) */
|
||||
@@ -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) */
|
||||
@@ -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) */
|
||||
@@ -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) */
|
||||
@@ -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) */
|
||||
@@ -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) */
|
||||
@@ -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) */
|
||||
@@ -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) */
|
||||
@@ -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) */
|
||||
@@ -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) */
|
||||
Executable → Regular
+160
-159
@@ -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: Fri Jul 25 14:06:41 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) */
|
||||
|
||||
@@ -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
|
||||
|
||||
Reference in New Issue
Block a user