Daniel Agar
7 years ago
10 changed files with 0 additions and 3348 deletions
@ -1,298 +0,0 @@
@@ -1,298 +0,0 @@
|
||||
function [xa_apo,Pa_apo,Rot_matrix,eulerAngles,debugOutput]... |
||||
= AttitudeEKF(approx_prediction,use_inertia_matrix,zFlag,dt,z,q_rotSpeed,q_rotAcc,q_acc,q_mag,r_gyro,r_accel,r_mag,J) |
||||
|
||||
|
||||
%LQG Position Estimator and Controller |
||||
% Observer: |
||||
% x[n|n] = x[n|n-1] + M(y[n] - Cx[n|n-1] - Du[n]) |
||||
% x[n+1|n] = Ax[n|n] + Bu[n] |
||||
% |
||||
% $Author: Tobias Naegeli $ $Date: 2014 $ $Revision: 3 $ |
||||
% |
||||
% |
||||
% Arguments: |
||||
% approx_prediction: if 1 then the exponential map is approximated with a |
||||
% first order taylor approximation. has at the moment not a big influence |
||||
% (just 1st or 2nd order approximation) we should change it to rodriquez |
||||
% approximation. |
||||
% use_inertia_matrix: set to true if you have the inertia matrix J for your |
||||
% quadrotor |
||||
% xa_apo_k: old state vectotr |
||||
% zFlag: if sensor measurement is available [gyro, acc, mag] |
||||
% dt: dt in s |
||||
% z: measurements [gyro, acc, mag] |
||||
% q_rotSpeed: process noise gyro |
||||
% q_rotAcc: process noise gyro acceleration |
||||
% q_acc: process noise acceleration |
||||
% q_mag: process noise magnetometer |
||||
% r_gyro: measurement noise gyro |
||||
% r_accel: measurement noise accel |
||||
% r_mag: measurement noise mag |
||||
% J: moment of inertia matrix |
||||
|
||||
|
||||
% Output: |
||||
% xa_apo: updated state vectotr |
||||
% Pa_apo: updated state covariance matrix |
||||
% Rot_matrix: rotation matrix |
||||
% eulerAngles: euler angles |
||||
% debugOutput: not used |
||||
|
||||
|
||||
%% model specific parameters |
||||
|
||||
|
||||
|
||||
% compute once the inverse of the Inertia |
||||
persistent Ji; |
||||
if isempty(Ji) |
||||
Ji=single(inv(J)); |
||||
end |
||||
|
||||
%% init |
||||
persistent x_apo |
||||
if(isempty(x_apo)) |
||||
gyro_init=single([0;0;0]); |
||||
gyro_acc_init=single([0;0;0]); |
||||
acc_init=single([0;0;-9.81]); |
||||
mag_init=single([1;0;0]); |
||||
x_apo=single([gyro_init;gyro_acc_init;acc_init;mag_init]); |
||||
|
||||
end |
||||
|
||||
persistent P_apo |
||||
if(isempty(P_apo)) |
||||
% P_apo = single(eye(NSTATES) * 1000); |
||||
P_apo = single(200*ones(12)); |
||||
end |
||||
|
||||
debugOutput = single(zeros(4,1)); |
||||
|
||||
%% copy the states |
||||
wx= x_apo(1); % x body angular rate |
||||
wy= x_apo(2); % y body angular rate |
||||
wz= x_apo(3); % z body angular rate |
||||
|
||||
wax= x_apo(4); % x body angular acceleration |
||||
way= x_apo(5); % y body angular acceleration |
||||
waz= x_apo(6); % z body angular acceleration |
||||
|
||||
zex= x_apo(7); % x component gravity vector |
||||
zey= x_apo(8); % y component gravity vector |
||||
zez= x_apo(9); % z component gravity vector |
||||
|
||||
mux= x_apo(10); % x component magnetic field vector |
||||
muy= x_apo(11); % y component magnetic field vector |
||||
muz= x_apo(12); % z component magnetic field vector |
||||
|
||||
|
||||
|
||||
|
||||
%% prediction section |
||||
% compute the apriori state estimate from the previous aposteriori estimate |
||||
%body angular accelerations |
||||
if (use_inertia_matrix==1) |
||||
wak =[wax;way;waz]+Ji*(-cross([wax;way;waz],J*[wax;way;waz]))*dt; |
||||
else |
||||
wak =[wax;way;waz]; |
||||
end |
||||
|
||||
%body angular rates |
||||
wk =[wx; wy; wz] + dt*wak; |
||||
|
||||
%derivative of the prediction rotation matrix |
||||
O=[0,-wz,wy;wz,0,-wx;-wy,wx,0]'; |
||||
|
||||
%prediction of the earth z vector |
||||
if (approx_prediction==1) |
||||
%e^(Odt)=I+dt*O+dt^2/2!O^2 |
||||
% so we do a first order approximation of the exponential map |
||||
zek =(O*dt+single(eye(3)))*[zex;zey;zez]; |
||||
|
||||
else |
||||
zek =(single(eye(3))+O*dt+dt^2/2*O^2)*[zex;zey;zez]; |
||||
%zek =expm2(O*dt)*[zex;zey;zez]; not working because use double |
||||
%precision |
||||
end |
||||
|
||||
|
||||
|
||||
%prediction of the magnetic vector |
||||
if (approx_prediction==1) |
||||
%e^(Odt)=I+dt*O+dt^2/2!O^2 |
||||
% so we do a first order approximation of the exponential map |
||||
muk =(O*dt+single(eye(3)))*[mux;muy;muz]; |
||||
else |
||||
muk =(single(eye(3))+O*dt+dt^2/2*O^2)*[mux;muy;muz]; |
||||
%muk =expm2(O*dt)*[mux;muy;muz]; not working because use double |
||||
%precision |
||||
end |
||||
|
||||
x_apr=[wk;wak;zek;muk]; |
||||
|
||||
% compute the apriori error covariance estimate from the previous |
||||
%aposteriori estimate |
||||
|
||||
EZ=[0,zez,-zey; |
||||
-zez,0,zex; |
||||
zey,-zex,0]'; |
||||
MA=[0,muz,-muy; |
||||
-muz,0,mux; |
||||
muy,-mux,0]'; |
||||
|
||||
E=single(eye(3)); |
||||
Z=single(zeros(3)); |
||||
|
||||
A_lin=[ Z, E, Z, Z |
||||
Z, Z, Z, Z |
||||
EZ, Z, O, Z |
||||
MA, Z, Z, O]; |
||||
|
||||
A_lin=eye(12)+A_lin*dt; |
||||
|
||||
%process covariance matrix |
||||
|
||||
persistent Q |
||||
if (isempty(Q)) |
||||
Q=diag([ q_rotSpeed,q_rotSpeed,q_rotSpeed,... |
||||
q_rotAcc,q_rotAcc,q_rotAcc,... |
||||
q_acc,q_acc,q_acc,... |
||||
q_mag,q_mag,q_mag]); |
||||
end |
||||
|
||||
P_apr=A_lin*P_apo*A_lin'+Q; |
||||
|
||||
|
||||
%% update |
||||
if zFlag(1)==1&&zFlag(2)==1&&zFlag(3)==1 |
||||
|
||||
% R=[r_gyro,0,0,0,0,0,0,0,0; |
||||
% 0,r_gyro,0,0,0,0,0,0,0; |
||||
% 0,0,r_gyro,0,0,0,0,0,0; |
||||
% 0,0,0,r_accel,0,0,0,0,0; |
||||
% 0,0,0,0,r_accel,0,0,0,0; |
||||
% 0,0,0,0,0,r_accel,0,0,0; |
||||
% 0,0,0,0,0,0,r_mag,0,0; |
||||
% 0,0,0,0,0,0,0,r_mag,0; |
||||
% 0,0,0,0,0,0,0,0,r_mag]; |
||||
R_v=[r_gyro,r_gyro,r_gyro,r_accel,r_accel,r_accel,r_mag,r_mag,r_mag]; |
||||
%observation matrix |
||||
%[zw;ze;zmk]; |
||||
H_k=[ E, Z, Z, Z; |
||||
Z, Z, E, Z; |
||||
Z, Z, Z, E]; |
||||
|
||||
y_k=z(1:9)-H_k*x_apr; |
||||
|
||||
|
||||
%S_k=H_k*P_apr*H_k'+R; |
||||
S_k=H_k*P_apr*H_k'; |
||||
S_k(1:9+1:end) = S_k(1:9+1:end) + R_v; |
||||
K_k=(P_apr*H_k'/(S_k)); |
||||
|
||||
|
||||
x_apo=x_apr+K_k*y_k; |
||||
P_apo=(eye(12)-K_k*H_k)*P_apr; |
||||
else |
||||
if zFlag(1)==1&&zFlag(2)==0&&zFlag(3)==0 |
||||
|
||||
R=[r_gyro,0,0; |
||||
0,r_gyro,0; |
||||
0,0,r_gyro]; |
||||
R_v=[r_gyro,r_gyro,r_gyro]; |
||||
%observation matrix |
||||
|
||||
H_k=[ E, Z, Z, Z]; |
||||
|
||||
y_k=z(1:3)-H_k(1:3,1:12)*x_apr; |
||||
|
||||
% S_k=H_k(1:3,1:12)*P_apr*H_k(1:3,1:12)'+R(1:3,1:3); |
||||
S_k=H_k(1:3,1:12)*P_apr*H_k(1:3,1:12)'; |
||||
S_k(1:3+1:end) = S_k(1:3+1:end) + R_v; |
||||
K_k=(P_apr*H_k(1:3,1:12)'/(S_k)); |
||||
|
||||
|
||||
x_apo=x_apr+K_k*y_k; |
||||
P_apo=(eye(12)-K_k*H_k(1:3,1:12))*P_apr; |
||||
else |
||||
if zFlag(1)==1&&zFlag(2)==1&&zFlag(3)==0 |
||||
|
||||
% R=[r_gyro,0,0,0,0,0; |
||||
% 0,r_gyro,0,0,0,0; |
||||
% 0,0,r_gyro,0,0,0; |
||||
% 0,0,0,r_accel,0,0; |
||||
% 0,0,0,0,r_accel,0; |
||||
% 0,0,0,0,0,r_accel]; |
||||
|
||||
R_v=[r_gyro,r_gyro,r_gyro,r_accel,r_accel,r_accel]; |
||||
%observation matrix |
||||
|
||||
H_k=[ E, Z, Z, Z; |
||||
Z, Z, E, Z]; |
||||
|
||||
y_k=z(1:6)-H_k(1:6,1:12)*x_apr; |
||||
|
||||
% S_k=H_k(1:6,1:12)*P_apr*H_k(1:6,1:12)'+R(1:6,1:6); |
||||
S_k=H_k(1:6,1:12)*P_apr*H_k(1:6,1:12)'; |
||||
S_k(1:6+1:end) = S_k(1:6+1:end) + R_v; |
||||
K_k=(P_apr*H_k(1:6,1:12)'/(S_k)); |
||||
|
||||
|
||||
x_apo=x_apr+K_k*y_k; |
||||
P_apo=(eye(12)-K_k*H_k(1:6,1:12))*P_apr; |
||||
else |
||||
if zFlag(1)==1&&zFlag(2)==0&&zFlag(3)==1 |
||||
% R=[r_gyro,0,0,0,0,0; |
||||
% 0,r_gyro,0,0,0,0; |
||||
% 0,0,r_gyro,0,0,0; |
||||
% 0,0,0,r_mag,0,0; |
||||
% 0,0,0,0,r_mag,0; |
||||
% 0,0,0,0,0,r_mag]; |
||||
R_v=[r_gyro,r_gyro,r_gyro,r_mag,r_mag,r_mag]; |
||||
%observation matrix |
||||
|
||||
H_k=[ E, Z, Z, Z; |
||||
Z, Z, Z, E]; |
||||
|
||||
y_k=[z(1:3);z(7:9)]-H_k(1:6,1:12)*x_apr; |
||||
|
||||
%S_k=H_k(1:6,1:12)*P_apr*H_k(1:6,1:12)'+R(1:6,1:6); |
||||
S_k=H_k(1:6,1:12)*P_apr*H_k(1:6,1:12)'; |
||||
S_k(1:6+1:end) = S_k(1:6+1:end) + R_v; |
||||
K_k=(P_apr*H_k(1:6,1:12)'/(S_k)); |
||||
|
||||
|
||||
x_apo=x_apr+K_k*y_k; |
||||
P_apo=(eye(12)-K_k*H_k(1:6,1:12))*P_apr; |
||||
else |
||||
x_apo=x_apr; |
||||
P_apo=P_apr; |
||||
end |
||||
end |
||||
end |
||||
end |
||||
|
||||
|
||||
|
||||
%% euler anglels extraction |
||||
z_n_b = -x_apo(7:9)./norm(x_apo(7:9)); |
||||
m_n_b = x_apo(10:12)./norm(x_apo(10:12)); |
||||
|
||||
y_n_b=cross(z_n_b,m_n_b); |
||||
y_n_b=y_n_b./norm(y_n_b); |
||||
|
||||
x_n_b=(cross(y_n_b,z_n_b)); |
||||
x_n_b=x_n_b./norm(x_n_b); |
||||
|
||||
|
||||
xa_apo=x_apo; |
||||
Pa_apo=P_apo; |
||||
% rotation matrix from earth to body system |
||||
Rot_matrix=[x_n_b,y_n_b,z_n_b]; |
||||
|
||||
|
||||
phi=atan2(Rot_matrix(2,3),Rot_matrix(3,3)); |
||||
theta=-asin(Rot_matrix(1,3)); |
||||
psi=atan2(Rot_matrix(1,2),Rot_matrix(1,1)); |
||||
eulerAngles=[phi;theta;psi]; |
||||
|
||||
@ -1,45 +0,0 @@
@@ -1,45 +0,0 @@
|
||||
############################################################################ |
||||
# |
||||
# Copyright (c) 2015 PX4 Development Team. All rights reserved. |
||||
# |
||||
# Redistribution and use in source and binary forms, with or without |
||||
# modification, are permitted provided that the following conditions |
||||
# are met: |
||||
# |
||||
# 1. Redistributions of source code must retain the above copyright |
||||
# notice, this list of conditions and the following disclaimer. |
||||
# 2. Redistributions in binary form must reproduce the above copyright |
||||
# notice, this list of conditions and the following disclaimer in |
||||
# the documentation and/or other materials provided with the |
||||
# distribution. |
||||
# 3. Neither the name PX4 nor the names of its contributors may be |
||||
# used to endorse or promote products derived from this software |
||||
# without specific prior written permission. |
||||
# |
||||
# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS |
||||
# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT |
||||
# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS |
||||
# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE |
||||
# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, |
||||
# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, |
||||
# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS |
||||
# OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED |
||||
# AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT |
||||
# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN |
||||
# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE |
||||
# POSSIBILITY OF SUCH DAMAGE. |
||||
# |
||||
############################################################################ |
||||
px4_add_module( |
||||
MODULE examples__attitude_estimator_ekf |
||||
MAIN attitude_estimator_ekf |
||||
STACK_MAIN 1200 |
||||
COMPILE_FLAGS |
||||
-Wno-float-equal |
||||
SRCS |
||||
attitude_estimator_ekf_main.cpp |
||||
codegen/AttitudeEKF.c |
||||
DEPENDS |
||||
platforms__common |
||||
) |
||||
# vim: set noet ft=cmake fenc=utf-8 ff=unix : |
||||
@ -1,502 +0,0 @@
@@ -1,502 +0,0 @@
|
||||
<?xml version="1.0" encoding="UTF-8"?> |
||||
<deployment-project plugin="plugin.matlabcoder" plugin-version="R2013a"> |
||||
<configuration target="target.matlab.coder" target-name="MEX, C, and C++ Code Generation" name="attitudeKalmanfilter" location="/home/thomasgubler/src/Firmware/src/modules/attitude_estimator_ekf" file="/home/thomasgubler/src/Firmware/src/modules/attitude_estimator_ekf/attitudeKalmanfilter.prj" build-checksum="1213478164"> |
||||
<profile key="profile.mex"> |
||||
<param.MergeInstrumentationResults>false</param.MergeInstrumentationResults> |
||||
<param.BuiltInstrumentedMex>false</param.BuiltInstrumentedMex> |
||||
<param.RanInstrumentedMex>false</param.RanInstrumentedMex> |
||||
<param.WorkingFolder>option.WorkingFolder.Project</param.WorkingFolder> |
||||
<param.SpecifiedWorkingFolder /> |
||||
<param.BuildFolder>option.BuildFolder.Project</param.BuildFolder> |
||||
<param.SpecifiedBuildFolder /> |
||||
<param.SearchPaths /> |
||||
<param.ResponsivenessChecks>true</param.ResponsivenessChecks> |
||||
<param.ExtrinsicCalls>true</param.ExtrinsicCalls> |
||||
<param.IntegrityChecks>true</param.IntegrityChecks> |
||||
<param.SaturateOnIntegerOverflow>true</param.SaturateOnIntegerOverflow> |
||||
<param.GlobalDataSyncMethod>option.GlobalDataSyncMethod.SyncAlways</param.GlobalDataSyncMethod> |
||||
<param.EnableVariableSizing>true</param.EnableVariableSizing> |
||||
<param.DynamicMemoryAllocation>option.DynamicMemoryAllocation.Threshold</param.DynamicMemoryAllocation> |
||||
<param.DynamicMemoryAllocationThreshold>65536</param.DynamicMemoryAllocationThreshold> |
||||
<param.StackUsageMax>200000</param.StackUsageMax> |
||||
<param.FilePartitionMethod>option.FilePartitionMethod.MapMFileToCFile</param.FilePartitionMethod> |
||||
<param.GenerateComments>true</param.GenerateComments> |
||||
<param.MATLABSourceComments>false</param.MATLABSourceComments> |
||||
<param.ReservedNameArray /> |
||||
<param.EnableScreener>true</param.EnableScreener> |
||||
<param.EnableDebugging>false</param.EnableDebugging> |
||||
<param.GenerateReport>true</param.GenerateReport> |
||||
<param.LaunchReport>false</param.LaunchReport> |
||||
<param.CustomSourceCode /> |
||||
<param.CustomHeaderCode /> |
||||
<param.CustomInitializer /> |
||||
<param.CustomTerminator /> |
||||
<param.CustomInclude /> |
||||
<param.CustomSource /> |
||||
<param.CustomLibrary /> |
||||
<param.PostCodeGenCommand /> |
||||
<param.ProposeFixedPointDataTypes>true</param.ProposeFixedPointDataTypes> |
||||
<param.mex.GenCodeOnly>false</param.mex.GenCodeOnly> |
||||
<param.ConstantFoldingTimeout>40000</param.ConstantFoldingTimeout> |
||||
<param.RecursionLimit>100</param.RecursionLimit> |
||||
<param.TargetLang>option.TargetLang.C</param.TargetLang> |
||||
<param.EchoExpressions>true</param.EchoExpressions> |
||||
<param.InlineThreshold>10</param.InlineThreshold> |
||||
<param.InlineThresholdMax>200</param.InlineThresholdMax> |
||||
<param.InlineStackLimit>4000</param.InlineStackLimit> |
||||
<param.EnableMemcpy>true</param.EnableMemcpy> |
||||
<param.MemcpyThreshold>64</param.MemcpyThreshold> |
||||
<param.EnableOpenMP>true</param.EnableOpenMP> |
||||
<param.InitFltsAndDblsToZero>true</param.InitFltsAndDblsToZero> |
||||
<param.ConstantInputs>option.ConstantInputs.CheckValues</param.ConstantInputs> |
||||
<unset> |
||||
<param.MergeInstrumentationResults /> |
||||
<param.BuiltInstrumentedMex /> |
||||
<param.RanInstrumentedMex /> |
||||
<param.WorkingFolder /> |
||||
<param.SpecifiedWorkingFolder /> |
||||
<param.BuildFolder /> |
||||
<param.SpecifiedBuildFolder /> |
||||
<param.SearchPaths /> |
||||
<param.ResponsivenessChecks /> |
||||
<param.ExtrinsicCalls /> |
||||
<param.IntegrityChecks /> |
||||
<param.SaturateOnIntegerOverflow /> |
||||
<param.GlobalDataSyncMethod /> |
||||
<param.EnableVariableSizing /> |
||||
<param.DynamicMemoryAllocation /> |
||||
<param.DynamicMemoryAllocationThreshold /> |
||||
<param.StackUsageMax /> |
||||
<param.FilePartitionMethod /> |
||||
<param.GenerateComments /> |
||||
<param.MATLABSourceComments /> |
||||
<param.ReservedNameArray /> |
||||
<param.EnableScreener /> |
||||
<param.EnableDebugging /> |
||||
<param.GenerateReport /> |
||||
<param.LaunchReport /> |
||||
<param.CustomSourceCode /> |
||||
<param.CustomHeaderCode /> |
||||
<param.CustomInitializer /> |
||||
<param.CustomTerminator /> |
||||
<param.CustomInclude /> |
||||
<param.CustomSource /> |
||||
<param.CustomLibrary /> |
||||
<param.PostCodeGenCommand /> |
||||
<param.ProposeFixedPointDataTypes /> |
||||
<param.mex.GenCodeOnly /> |
||||
<param.ConstantFoldingTimeout /> |
||||
<param.RecursionLimit /> |
||||
<param.TargetLang /> |
||||
<param.EchoExpressions /> |
||||
<param.InlineThreshold /> |
||||
<param.InlineThresholdMax /> |
||||
<param.InlineStackLimit /> |
||||
<param.EnableMemcpy /> |
||||
<param.MemcpyThreshold /> |
||||
<param.EnableOpenMP /> |
||||
<param.InitFltsAndDblsToZero /> |
||||
<param.ConstantInputs /> |
||||
</unset> |
||||
</profile> |
||||
<profile key="profile.c"> |
||||
<param.grt.GenCodeOnly>true</param.grt.GenCodeOnly> |
||||
<param.WorkingFolder>option.WorkingFolder.Project</param.WorkingFolder> |
||||
<param.SpecifiedWorkingFolder /> |
||||
<param.BuildFolder>option.BuildFolder.Specified</param.BuildFolder> |
||||
<param.SpecifiedBuildFolder>codegen</param.SpecifiedBuildFolder> |
||||
<param.SearchPaths /> |
||||
<param.SaturateOnIntegerOverflow>true</param.SaturateOnIntegerOverflow> |
||||
<param.PurelyIntegerCode>false</param.PurelyIntegerCode> |
||||
<param.SupportNonFinite>false</param.SupportNonFinite> |
||||
<param.EnableVariableSizing>false</param.EnableVariableSizing> |
||||
<param.DynamicMemoryAllocation>option.DynamicMemoryAllocation.Threshold</param.DynamicMemoryAllocation> |
||||
<param.DynamicMemoryAllocationThreshold>65536</param.DynamicMemoryAllocationThreshold> |
||||
<param.StackUsageMax>4000</param.StackUsageMax> |
||||
<param.MultiInstanceCode>false</param.MultiInstanceCode> |
||||
<param.FilePartitionMethod>option.FilePartitionMethod.SingleFile</param.FilePartitionMethod> |
||||
<param.GenerateComments>true</param.GenerateComments> |
||||
<param.MATLABSourceComments>true</param.MATLABSourceComments> |
||||
<param.MATLABFcnDesc>false</param.MATLABFcnDesc> |
||||
<param.DataTypeReplacement>option.DataTypeReplacement.CBuiltIn</param.DataTypeReplacement> |
||||
<param.ConvertIfToSwitch>false</param.ConvertIfToSwitch> |
||||
<param.PreserveExternInFcnDecls>true</param.PreserveExternInFcnDecls> |
||||
<param.ParenthesesLevel>option.ParenthesesLevel.Nominal</param.ParenthesesLevel> |
||||
<param.MaxIdLength>31</param.MaxIdLength> |
||||
<param.CustomSymbolStrGlobalVar>$M$N</param.CustomSymbolStrGlobalVar> |
||||
<param.CustomSymbolStrType>$M$N</param.CustomSymbolStrType> |
||||
<param.CustomSymbolStrField>$M$N</param.CustomSymbolStrField> |
||||
<param.CustomSymbolStrFcn>$M$N</param.CustomSymbolStrFcn> |
||||
<param.CustomSymbolStrTmpVar>$M$N</param.CustomSymbolStrTmpVar> |
||||
<param.CustomSymbolStrMacro>$M$N</param.CustomSymbolStrMacro> |
||||
<param.CustomSymbolStrEMXArray>emxArray_$M$N</param.CustomSymbolStrEMXArray> |
||||
<param.CustomSymbolStrEMXArrayFcn>emx$M$N</param.CustomSymbolStrEMXArrayFcn> |
||||
<param.ReservedNameArray /> |
||||
<param.EnableScreener>true</param.EnableScreener> |
||||
<param.Verbose>false</param.Verbose> |
||||
<param.GenerateReport>true</param.GenerateReport> |
||||
<param.GenerateCodeMetricsReport>true</param.GenerateCodeMetricsReport> |
||||
<param.GenerateCodeReplacementReport>false</param.GenerateCodeReplacementReport> |
||||
<param.LaunchReport>true</param.LaunchReport> |
||||
<param.CustomSourceCode /> |
||||
<param.CustomHeaderCode /> |
||||
<param.CustomInitializer /> |
||||
<param.CustomTerminator /> |
||||
<param.CustomInclude /> |
||||
<param.CustomSource /> |
||||
<param.CustomLibrary /> |
||||
<param.PostCodeGenCommand /> |
||||
<param.CodeReplacementLibrary>C89/C90 (ANSI)</param.CodeReplacementLibrary> |
||||
<param.SameHardware>true</param.SameHardware> |
||||
<param.HardwareVendor.Production>ARM Compatible</param.HardwareVendor.Production> |
||||
<param.HardwareType.Production>ARM Cortex</param.HardwareType.Production> |
||||
<var.instance.enabled.Production>true</var.instance.enabled.Production> |
||||
<param.HardwareSizeChar.Production>8</param.HardwareSizeChar.Production> |
||||
<param.HardwareSizeShort.Production>16</param.HardwareSizeShort.Production> |
||||
<param.HardwareSizeInt.Production>32</param.HardwareSizeInt.Production> |
||||
<param.HardwareSizeLong.Production>32</param.HardwareSizeLong.Production> |
||||
<param.HardwareSizeLongLong.Production>64</param.HardwareSizeLongLong.Production> |
||||
<param.HardwareSizeFloat.Production>32</param.HardwareSizeFloat.Production> |
||||
<param.HardwareSizeDouble.Production>64</param.HardwareSizeDouble.Production> |
||||
<param.HardwareSizeWord.Production>32</param.HardwareSizeWord.Production> |
||||
<param.HardwareSizePointer.Production>32</param.HardwareSizePointer.Production> |
||||
<param.HardwareEndianness.Production>option.HardwareEndianness.Little</param.HardwareEndianness.Production> |
||||
<param.HardwareArithmeticRightShift.Production>true</param.HardwareArithmeticRightShift.Production> |
||||
<param.HardwareLongLongMode.Production>false</param.HardwareLongLongMode.Production> |
||||
<param.HardwareAtomicIntegerSize.Production>option.HardwareAtomicIntegerSize.Long</param.HardwareAtomicIntegerSize.Production> |
||||
<param.HardwareAtomicFloatSize.Production>option.HardwareAtomicFloatSize.Double</param.HardwareAtomicFloatSize.Production> |
||||
<param.HardwareDivisionRounding.Production>option.HardwareDivisionRounding.Undefined</param.HardwareDivisionRounding.Production> |
||||
<param.HardwareVendor.Target>Generic</param.HardwareVendor.Target> |
||||
<param.HardwareType.Target>MATLAB Host Computer</param.HardwareType.Target> |
||||
<var.instance.enabled.Target>false</var.instance.enabled.Target> |
||||
<param.HardwareSizeChar.Target>8</param.HardwareSizeChar.Target> |
||||
<param.HardwareSizeShort.Target>16</param.HardwareSizeShort.Target> |
||||
<param.HardwareSizeInt.Target>32</param.HardwareSizeInt.Target> |
||||
<param.HardwareSizeLong.Target>64</param.HardwareSizeLong.Target> |
||||
<param.HardwareSizeLongLong.Target>64</param.HardwareSizeLongLong.Target> |
||||
<param.HardwareSizeFloat.Target>32</param.HardwareSizeFloat.Target> |
||||
<param.HardwareSizeDouble.Target>64</param.HardwareSizeDouble.Target> |
||||
<param.HardwareSizeWord.Target>64</param.HardwareSizeWord.Target> |
||||
<param.HardwareSizePointer.Target>64</param.HardwareSizePointer.Target> |
||||
<param.HardwareEndianness.Target>option.HardwareEndianness.Little</param.HardwareEndianness.Target> |
||||
<param.HardwareArithmeticRightShift.Target>true</param.HardwareArithmeticRightShift.Target> |
||||
<param.HardwareLongLongMode.Target>true</param.HardwareLongLongMode.Target> |
||||
<param.HardwareAtomicIntegerSize.Target>option.HardwareAtomicIntegerSize.Char</param.HardwareAtomicIntegerSize.Target> |
||||
<param.HardwareAtomicFloatSize.Target>option.HardwareAtomicFloatSize.None</param.HardwareAtomicFloatSize.Target> |
||||
<param.HardwareDivisionRounding.Target>option.HardwareDivisionRounding.Zero</param.HardwareDivisionRounding.Target> |
||||
<param.Toolchain>Automatically locate an installed toolchain</param.Toolchain> |
||||
<param.BuildConfiguration>Faster Builds</param.BuildConfiguration> |
||||
<param.CustomToolchainOptions /> |
||||
<param.ConstantFoldingTimeout>40000</param.ConstantFoldingTimeout> |
||||
<param.RecursionLimit>100</param.RecursionLimit> |
||||
<param.IncludeTerminateFcn>true</param.IncludeTerminateFcn> |
||||
<param.TargetLang>option.TargetLang.C</param.TargetLang> |
||||
<param.CCompilerOptimization>option.CCompilerOptimization.On</param.CCompilerOptimization> |
||||
<param.CCompilerCustomOptimizations /> |
||||
<param.GenerateMakefile>true</param.GenerateMakefile> |
||||
<param.BuildToolEnable>false</param.BuildToolEnable> |
||||
<param.MakeCommand>make_rtw</param.MakeCommand> |
||||
<param.TemplateMakefile>default_tmf</param.TemplateMakefile> |
||||
<param.BuildToolConfiguration /> |
||||
<param.InlineThreshold>10</param.InlineThreshold> |
||||
<param.InlineThresholdMax>200</param.InlineThresholdMax> |
||||
<param.InlineStackLimit>4000</param.InlineStackLimit> |
||||
<param.EnableMemcpy>true</param.EnableMemcpy> |
||||
<param.MemcpyThreshold>64</param.MemcpyThreshold> |
||||
<param.EnableOpenMP>true</param.EnableOpenMP> |
||||
<param.InitFltsAndDblsToZero>true</param.InitFltsAndDblsToZero> |
||||
<param.PassStructByReference>false</param.PassStructByReference> |
||||
<param.UseECoderFeatures>true</param.UseECoderFeatures> |
||||
<unset> |
||||
<param.WorkingFolder /> |
||||
<param.SpecifiedWorkingFolder /> |
||||
<param.SearchPaths /> |
||||
<param.SaturateOnIntegerOverflow /> |
||||
<param.PurelyIntegerCode /> |
||||
<param.DynamicMemoryAllocation /> |
||||
<param.DynamicMemoryAllocationThreshold /> |
||||
<param.MultiInstanceCode /> |
||||
<param.GenerateComments /> |
||||
<param.MATLABFcnDesc /> |
||||
<param.DataTypeReplacement /> |
||||
<param.ConvertIfToSwitch /> |
||||
<param.PreserveExternInFcnDecls /> |
||||
<param.ParenthesesLevel /> |
||||
<param.MaxIdLength /> |
||||
<param.CustomSymbolStrGlobalVar /> |
||||
<param.CustomSymbolStrType /> |
||||
<param.CustomSymbolStrField /> |
||||
<param.CustomSymbolStrFcn /> |
||||
<param.CustomSymbolStrTmpVar /> |
||||
<param.CustomSymbolStrMacro /> |
||||
<param.CustomSymbolStrEMXArray /> |
||||
<param.CustomSymbolStrEMXArrayFcn /> |
||||
<param.ReservedNameArray /> |
||||
<param.EnableScreener /> |
||||
<param.Verbose /> |
||||
<param.GenerateReport /> |
||||
<param.GenerateCodeMetricsReport /> |
||||
<param.GenerateCodeReplacementReport /> |
||||
<param.CustomInclude /> |
||||
<param.CustomSource /> |
||||
<param.CustomLibrary /> |
||||
<param.SameHardware /> |
||||
<var.instance.enabled.Production /> |
||||
<param.HardwareSizeChar.Production /> |
||||
<param.HardwareSizeShort.Production /> |
||||
<param.HardwareSizeInt.Production /> |
||||
<param.HardwareSizeLong.Production /> |
||||
<param.HardwareSizeLongLong.Production /> |
||||
<param.HardwareSizeFloat.Production /> |
||||
<param.HardwareSizeDouble.Production /> |
||||
<param.HardwareSizeWord.Production /> |
||||
<param.HardwareSizePointer.Production /> |
||||
<param.HardwareEndianness.Production /> |
||||
<param.HardwareLongLongMode.Production /> |
||||
<param.HardwareDivisionRounding.Production /> |
||||
<var.instance.enabled.Target /> |
||||
<param.HardwareSizeChar.Target /> |
||||
<param.HardwareSizeShort.Target /> |
||||
<param.HardwareSizeInt.Target /> |
||||
<param.HardwareSizeLongLong.Target /> |
||||
<param.HardwareSizeFloat.Target /> |
||||
<param.HardwareSizeDouble.Target /> |
||||
<param.HardwareEndianness.Target /> |
||||
<param.HardwareAtomicFloatSize.Target /> |
||||
<param.CustomToolchainOptions /> |
||||
<param.ConstantFoldingTimeout /> |
||||
<param.RecursionLimit /> |
||||
<param.IncludeTerminateFcn /> |
||||
<param.TargetLang /> |
||||
<param.CCompilerCustomOptimizations /> |
||||
<param.GenerateMakefile /> |
||||
<param.BuildToolEnable /> |
||||
<param.MakeCommand /> |
||||
<param.TemplateMakefile /> |
||||
<param.BuildToolConfiguration /> |
||||
<param.InlineThreshold /> |
||||
<param.InlineThresholdMax /> |
||||
<param.InlineStackLimit /> |
||||
<param.EnableMemcpy /> |
||||
<param.MemcpyThreshold /> |
||||
<param.EnableOpenMP /> |
||||
<param.InitFltsAndDblsToZero /> |
||||
<param.UseECoderFeatures /> |
||||
</unset> |
||||
</profile> |
||||
<param.outputfile>/opt/matlab/r2013b/bin/codegen/codegen/lib/AttitudeEKF/AttitudeEKF.a</param.outputfile> |
||||
<param.version>R2012a</param.version> |
||||
<param.HasECoderFeatures>true</param.HasECoderFeatures> |
||||
<param.mex.mainhtml>t:\private\desktop-dinfk-xp\Attitude_Kalmanfilter\codegen\mex\attitudeKalmanfilter\html\index.html</param.mex.mainhtml> |
||||
<param.grt.mainhtml>/home/thomasgubler/src/Firmware/src/modules/attitude_estimator_ekf/codegen/html/index.html</param.grt.mainhtml> |
||||
<param.CallGeneratedCodeFromTest>true</param.CallGeneratedCodeFromTest> |
||||
<param.VerificationMode>option.VerificationMode.None</param.VerificationMode> |
||||
<param.SILDebugging>false</param.SILDebugging> |
||||
<param.DefaultTestFile>${PROJECT_ROOT}/AttitudeEKF_Test.m</param.DefaultTestFile> |
||||
<param.AutoInferDefaultFile>${PROJECT_ROOT}/AttitudeEKF_Test.m</param.AutoInferDefaultFile> |
||||
<param.AutoInferUseVariableSize>false</param.AutoInferUseVariableSize> |
||||
<param.AutoInferUseUnboundedSize>false</param.AutoInferUseUnboundedSize> |
||||
<param.AutoInferVariableSizeThreshold>1024</param.AutoInferVariableSizeThreshold> |
||||
<param.AutoInferUnboundedSizeThreshold>2048</param.AutoInferUnboundedSizeThreshold> |
||||
<param.mex.outputfile>AttitudeEKF_mex</param.mex.outputfile> |
||||
<param.grt.outputfile>AttitudeEKF</param.grt.outputfile> |
||||
<param.artifact>option.target.artifact.lib</param.artifact> |
||||
<param.FixedPointTypeProposalMode>option.FixedPointTypeProposalMode.ProposeFractionLengths</param.FixedPointTypeProposalMode> |
||||
<param.DefaultProposedFixedPointType>numerictype([],16,12)</param.DefaultProposedFixedPointType> |
||||
<param.MinMaxSafetyMargin>0</param.MinMaxSafetyMargin> |
||||
<param.OptimizeWholeNumbers>true</param.OptimizeWholeNumbers> |
||||
<param.LaunchInstrumentationReport>false</param.LaunchInstrumentationReport> |
||||
<param.OpenInstrumentationReportInBrowser>false</param.OpenInstrumentationReportInBrowser> |
||||
<param.CreatePrintableInstrumentationReport>false</param.CreatePrintableInstrumentationReport> |
||||
<param.EnableAutoExtrinsicCalls>true</param.EnableAutoExtrinsicCalls> |
||||
<param.UsePreconditions>false</param.UsePreconditions> |
||||
<param.FeatureFlags /> |
||||
<param.FixedPointMode>option.FixedPointMode.None</param.FixedPointMode> |
||||
<param.AutoScaleLoopIndexVariables>false</param.AutoScaleLoopIndexVariables> |
||||
<param.ComputedFixedPointData /> |
||||
<param.UserFixedPointData /> |
||||
<param.DefaultWordLength>16</param.DefaultWordLength> |
||||
<param.DefaultFractionLength>4</param.DefaultFractionLength> |
||||
<param.FixedPointSafetyMargin>0</param.FixedPointSafetyMargin> |
||||
<param.FixedPointFimath>fimath('RoundingMethod', 'Floor', 'OverflowAction', 'Wrap', 'ProductMode', 'FullPrecision', 'MaxProductWordLength', 128, 'SumMode', 'FullPrecision', 'MaxSumWordLength', 128)</param.FixedPointFimath> |
||||
<param.FixedPointTypeSource>option.FixedPointTypeSource.SimAndDerived</param.FixedPointTypeSource> |
||||
<param.StaticAnalysisTimeout /> |
||||
<param.StaticAnalysisGlobalRangesOnly>false</param.StaticAnalysisGlobalRangesOnly> |
||||
<param.LogAllIOValues>false</param.LogAllIOValues> |
||||
<param.LogHistogram>false</param.LogHistogram> |
||||
<param.ShowCoverage>true</param.ShowCoverage> |
||||
<param.ExcludedFixedPointVerificationFiles /> |
||||
<param.ExcludedFixedPointSimulationFiles /> |
||||
<param.InstrumentedBuildChecksum /> |
||||
<param.FixedPointStaticAnalysisChecksum /> |
||||
<param.InstrumentedMexFile /> |
||||
<param.FixedPointValidationChecksum /> |
||||
<param.FixedPointSourceCodeChecksum /> |
||||
<param.FixedPointFunctionReplacements /> |
||||
<param.OptimizeWholeNumbers>true</param.OptimizeWholeNumbers> |
||||
<param.GeneratedFixedPointFileSuffix>_fixpt</param.GeneratedFixedPointFileSuffix> |
||||
<param.DefaultFixedPointSignedness>option.DefaultFixedPointSignedness.Automatic</param.DefaultFixedPointSignedness> |
||||
<unset> |
||||
<param.outputfile /> |
||||
<param.version /> |
||||
<param.HasECoderFeatures /> |
||||
<param.CallGeneratedCodeFromTest /> |
||||
<param.VerificationMode /> |
||||
<param.SILDebugging /> |
||||
<param.AutoInferUseVariableSize /> |
||||
<param.AutoInferUseUnboundedSize /> |
||||
<param.AutoInferVariableSizeThreshold /> |
||||
<param.AutoInferUnboundedSizeThreshold /> |
||||
<param.mex.outputfile /> |
||||
<param.grt.outputfile /> |
||||
<param.FixedPointTypeProposalMode /> |
||||
<param.DefaultProposedFixedPointType /> |
||||
<param.MinMaxSafetyMargin /> |
||||
<param.OptimizeWholeNumbers /> |
||||
<param.LaunchInstrumentationReport /> |
||||
<param.OpenInstrumentationReportInBrowser /> |
||||
<param.CreatePrintableInstrumentationReport /> |
||||
<param.EnableAutoExtrinsicCalls /> |
||||
<param.UsePreconditions /> |
||||
<param.FeatureFlags /> |
||||
<param.FixedPointMode /> |
||||
<param.AutoScaleLoopIndexVariables /> |
||||
<param.ComputedFixedPointData /> |
||||
<param.UserFixedPointData /> |
||||
<param.DefaultWordLength /> |
||||
<param.DefaultFractionLength /> |
||||
<param.FixedPointSafetyMargin /> |
||||
<param.FixedPointFimath /> |
||||
<param.FixedPointTypeSource /> |
||||
<param.StaticAnalysisTimeout /> |
||||
<param.StaticAnalysisGlobalRangesOnly /> |
||||
<param.LogAllIOValues /> |
||||
<param.LogHistogram /> |
||||
<param.ShowCoverage /> |
||||
<param.ExcludedFixedPointVerificationFiles /> |
||||
<param.ExcludedFixedPointSimulationFiles /> |
||||
<param.InstrumentedBuildChecksum /> |
||||
<param.FixedPointStaticAnalysisChecksum /> |
||||
<param.InstrumentedMexFile /> |
||||
<param.FixedPointValidationChecksum /> |
||||
<param.FixedPointSourceCodeChecksum /> |
||||
<param.FixedPointFunctionReplacements /> |
||||
<param.GeneratedFixedPointFileSuffix /> |
||||
<param.DefaultFixedPointSignedness /> |
||||
</unset> |
||||
<fileset.entrypoints> |
||||
<file value="${PROJECT_ROOT}/AttitudeEKF.m" custom-data-expanded="true"> |
||||
<Inputs fileName="AttitudeEKF.m" functionName="AttitudeEKF"> |
||||
<Input Name="approx_prediction"> |
||||
<Class>uint8</Class> |
||||
<UserDefined>false</UserDefined> |
||||
<Size>1 x 1</Size> |
||||
<Complex>false</Complex> |
||||
</Input> |
||||
<Input Name="use_inertia_matrix"> |
||||
<Class>uint8</Class> |
||||
<UserDefined>false</UserDefined> |
||||
<Size>1 x 1</Size> |
||||
<Complex>false</Complex> |
||||
</Input> |
||||
<Input Name="zFlag"> |
||||
<Class>uint8</Class> |
||||
<UserDefined>false</UserDefined> |
||||
<Size>3 x 1</Size> |
||||
<Complex>false</Complex> |
||||
</Input> |
||||
<Input Name="dt"> |
||||
<Class>single</Class> |
||||
<UserDefined>false</UserDefined> |
||||
<Size>1 x 1</Size> |
||||
<Complex>false</Complex> |
||||
</Input> |
||||
<Input Name="z"> |
||||
<Class>single</Class> |
||||
<UserDefined>false</UserDefined> |
||||
<Size>9 x 1</Size> |
||||
<Complex>false</Complex> |
||||
</Input> |
||||
<Input Name="q_rotSpeed"> |
||||
<Class>single</Class> |
||||
<UserDefined>false</UserDefined> |
||||
<Size>1 x 1</Size> |
||||
<Complex>false</Complex> |
||||
</Input> |
||||
<Input Name="q_rotAcc"> |
||||
<Class>single</Class> |
||||
<UserDefined>false</UserDefined> |
||||
<Size>1 x 1</Size> |
||||
<Complex>false</Complex> |
||||
</Input> |
||||
<Input Name="q_acc"> |
||||
<Class>single</Class> |
||||
<UserDefined>false</UserDefined> |
||||
<Size>1 x 1</Size> |
||||
<Complex>false</Complex> |
||||
</Input> |
||||
<Input Name="q_mag"> |
||||
<Class>single</Class> |
||||
<UserDefined>false</UserDefined> |
||||
<Size>1 x 1</Size> |
||||
<Complex>false</Complex> |
||||
</Input> |
||||
<Input Name="r_gyro"> |
||||
<Class>single</Class> |
||||
<UserDefined>false</UserDefined> |
||||
<Size>1 x 1</Size> |
||||
<Complex>false</Complex> |
||||
</Input> |
||||
<Input Name="r_accel"> |
||||
<Class>single</Class> |
||||
<UserDefined>false</UserDefined> |
||||
<Size>1 x 1</Size> |
||||
<Complex>false</Complex> |
||||
</Input> |
||||
<Input Name="r_mag"> |
||||
<Class>single</Class> |
||||
<UserDefined>false</UserDefined> |
||||
<Size>1 x 1</Size> |
||||
<Complex>false</Complex> |
||||
</Input> |
||||
<Input Name="J"> |
||||
<Class>single</Class> |
||||
<UserDefined>false</UserDefined> |
||||
<Size>3 x 3</Size> |
||||
<Complex>false</Complex> |
||||
</Input> |
||||
</Inputs> |
||||
</file> |
||||
</fileset.entrypoints> |
||||
<fileset.testbench> |
||||
<file>${PROJECT_ROOT}/AttitudeEKF_Test.m</file> |
||||
</fileset.testbench> |
||||
<fileset.package /> |
||||
<build-deliverables> |
||||
<file name="AttitudeEKF.a" location="${MATLAB_ROOT}/bin/codegen/codegen/lib/AttitudeEKF" optional="false">/opt/matlab/r2013b/bin/codegen/codegen/lib/AttitudeEKF/AttitudeEKF.a</file> |
||||
</build-deliverables> |
||||
<workflow /> |
||||
<matlab> |
||||
<root>/opt/matlab/r2013b</root> |
||||
<toolboxes> |
||||
<toolbox name="fixedpoint" /> |
||||
</toolboxes> |
||||
</matlab> |
||||
<platform> |
||||
<unix>true</unix> |
||||
<mac>false</mac> |
||||
<windows>false</windows> |
||||
<win2k>false</win2k> |
||||
<winxp>false</winxp> |
||||
<vista>false</vista> |
||||
<linux>true</linux> |
||||
<solaris>false</solaris> |
||||
<osver>3.16.1-1-ARCH</osver> |
||||
<os32>false</os32> |
||||
<os64>true</os64> |
||||
<arch>glnxa64</arch> |
||||
<matlab>true</matlab> |
||||
</platform> |
||||
</configuration> |
||||
</deployment-project> |
||||
|
||||
@ -1,639 +0,0 @@
@@ -1,639 +0,0 @@
|
||||
/****************************************************************************
|
||||
* |
||||
* Copyright (c) 2012-2014 PX4 Development Team. All rights reserved. |
||||
* |
||||
* Redistribution and use in source and binary forms, with or without |
||||
* modification, are permitted provided that the following conditions |
||||
* are met: |
||||
* |
||||
* 1. Redistributions of source code must retain the above copyright |
||||
* notice, this list of conditions and the following disclaimer. |
||||
* 2. Redistributions in binary form must reproduce the above copyright |
||||
* notice, this list of conditions and the following disclaimer in |
||||
* the documentation and/or other materials provided with the |
||||
* distribution. |
||||
* 3. Neither the name PX4 nor the names of its contributors may be |
||||
* used to endorse or promote products derived from this software |
||||
* without specific prior written permission. |
||||
* |
||||
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS |
||||
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT |
||||
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS |
||||
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE |
||||
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, |
||||
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, |
||||
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS |
||||
* OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED |
||||
* AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT |
||||
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN |
||||
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE |
||||
* POSSIBILITY OF SUCH DAMAGE. |
||||
* |
||||
****************************************************************************/ |
||||
|
||||
/*
|
||||
* @file attitude_estimator_ekf_main.cpp |
||||
* |
||||
* Extended Kalman Filter for Attitude Estimation. |
||||
* |
||||
* @author Tobias Naegeli <naegelit@student.ethz.ch> |
||||
* @author Lorenz Meier <lm@inf.ethz.ch> |
||||
* @author Thomas Gubler <thomasgubler@gmail.com> |
||||
*/ |
||||
|
||||
#include <px4_config.h> |
||||
#include <px4_defines.h> |
||||
#include <px4_posix.h> |
||||
#include <px4_tasks.h> |
||||
#include <unistd.h> |
||||
#include <stdlib.h> |
||||
#include <stdio.h> |
||||
#include <stdbool.h> |
||||
#include <poll.h> |
||||
#include <fcntl.h> |
||||
#include <float.h> |
||||
#include <errno.h> |
||||
#include <limits.h> |
||||
#include <math.h> |
||||
#include <uORB/uORB.h> |
||||
#include <uORB/topics/debug_key_value.h> |
||||
#include <uORB/topics/sensor_combined.h> |
||||
#include <uORB/topics/vehicle_attitude.h> |
||||
#include <uORB/topics/vehicle_control_mode.h> |
||||
#include <uORB/topics/vehicle_gps_position.h> |
||||
#include <uORB/topics/vehicle_global_position.h> |
||||
#include <uORB/topics/parameter_update.h> |
||||
#include <uORB/topics/att_pos_mocap.h> |
||||
#include <drivers/drv_hrt.h> |
||||
|
||||
#include <lib/mathlib/mathlib.h> |
||||
#include <lib/geo/geo.h> |
||||
|
||||
#include <systemlib/perf_counter.h> |
||||
#include <systemlib/err.h> |
||||
|
||||
#ifdef __cplusplus |
||||
extern "C" { |
||||
#endif |
||||
#include "codegen/AttitudeEKF.h" |
||||
#include "attitude_estimator_ekf_params.h" |
||||
#ifdef __cplusplus |
||||
} |
||||
#endif |
||||
|
||||
extern "C" __EXPORT int attitude_estimator_ekf_main(int argc, char *argv[]); |
||||
|
||||
static bool thread_should_exit = false; /**< Deamon exit flag */ |
||||
static bool thread_running = false; /**< Deamon status flag */ |
||||
static int attitude_estimator_ekf_task; /**< Handle of deamon task / thread */ |
||||
|
||||
/**
|
||||
* Mainloop of attitude_estimator_ekf. |
||||
*/ |
||||
int attitude_estimator_ekf_thread_main(int argc, char *argv[]); |
||||
|
||||
/**
|
||||
* Print the correct usage. |
||||
*/ |
||||
static void usage(const char *reason); |
||||
|
||||
static void |
||||
usage(const char *reason) |
||||
{ |
||||
if (reason) { |
||||
fprintf(stderr, "%s\n", reason); |
||||
} |
||||
|
||||
fprintf(stderr, "usage: attitude_estimator_ekf {start|stop|status} [-p <additional params>]\n\n"); |
||||
} |
||||
|
||||
int parameters_init(struct attitude_estimator_ekf_param_handles *h) |
||||
{ |
||||
/* PID parameters */ |
||||
h->q0 = param_find("EKF_ATT_V3_Q0"); |
||||
h->q1 = param_find("EKF_ATT_V3_Q1"); |
||||
h->q2 = param_find("EKF_ATT_V3_Q2"); |
||||
h->q3 = param_find("EKF_ATT_V3_Q3"); |
||||
|
||||
h->r0 = param_find("EKF_ATT_V4_R0"); |
||||
h->r1 = param_find("EKF_ATT_V4_R1"); |
||||
h->r2 = param_find("EKF_ATT_V4_R2"); |
||||
|
||||
h->moment_inertia_J[0] = param_find("ATT_J11"); |
||||
h->moment_inertia_J[1] = param_find("ATT_J22"); |
||||
h->moment_inertia_J[2] = param_find("ATT_J33"); |
||||
h->use_moment_inertia = param_find("ATT_J_EN"); |
||||
|
||||
return OK; |
||||
} |
||||
|
||||
int parameters_update(const struct attitude_estimator_ekf_param_handles *h, struct attitude_estimator_ekf_params *p) |
||||
{ |
||||
param_get(h->q0, &(p->q[0])); |
||||
param_get(h->q1, &(p->q[1])); |
||||
param_get(h->q2, &(p->q[2])); |
||||
param_get(h->q3, &(p->q[3])); |
||||
|
||||
param_get(h->r0, &(p->r[0])); |
||||
param_get(h->r1, &(p->r[1])); |
||||
param_get(h->r2, &(p->r[2])); |
||||
|
||||
for (int i = 0; i < 3; i++) { |
||||
param_get(h->moment_inertia_J[i], &(p->moment_inertia_J[3 * i + i])); |
||||
} |
||||
|
||||
param_get(h->use_moment_inertia, &(p->use_moment_inertia)); |
||||
|
||||
return OK; |
||||
} |
||||
|
||||
/**
|
||||
* The attitude_estimator_ekf app only briefly exists to start |
||||
* the background job. The stack size assigned in the |
||||
* Makefile does only apply to this management task. |
||||
* |
||||
* The actual stack size should be set in the call |
||||
* to px4_task_spawn_cmd(). |
||||
*/ |
||||
int attitude_estimator_ekf_main(int argc, char *argv[]) |
||||
{ |
||||
if (argc < 2) { |
||||
usage("missing command"); |
||||
return 1; |
||||
} |
||||
|
||||
if (!strcmp(argv[1], "start")) { |
||||
|
||||
if (thread_running) { |
||||
warnx("already running\n"); |
||||
/* this is not an error */ |
||||
return 0; |
||||
} |
||||
|
||||
thread_should_exit = false; |
||||
attitude_estimator_ekf_task = px4_task_spawn_cmd("attitude_estimator_ekf", |
||||
SCHED_DEFAULT, |
||||
SCHED_PRIORITY_MAX - 5, |
||||
7000, |
||||
attitude_estimator_ekf_thread_main, |
||||
(argv) ? (char * const *)&argv[2] : (char * const *)nullptr); |
||||
return 0; |
||||
} |
||||
|
||||
if (!strcmp(argv[1], "stop")) { |
||||
thread_should_exit = true; |
||||
return 0; |
||||
} |
||||
|
||||
if (!strcmp(argv[1], "status")) { |
||||
if (thread_running) { |
||||
warnx("running"); |
||||
return 0; |
||||
|
||||
} else { |
||||
warnx("not started"); |
||||
return 1; |
||||
} |
||||
|
||||
return 0; |
||||
} |
||||
|
||||
usage("unrecognized command"); |
||||
return 1; |
||||
} |
||||
|
||||
/*
|
||||
* [Rot_matrix,x_aposteriori,P_aposteriori] = attitudeKalmanfilter(dt,z_k,x_aposteriori_k,P_aposteriori_k,knownConst) |
||||
*/ |
||||
|
||||
/*
|
||||
* EKF Attitude Estimator main function. |
||||
* |
||||
* Estimates the attitude recursively once started. |
||||
* |
||||
* @param argc number of commandline arguments (plus command name) |
||||
* @param argv strings containing the arguments |
||||
*/ |
||||
int attitude_estimator_ekf_thread_main(int argc, char *argv[]) |
||||
{ |
||||
|
||||
float dt = 0.005f; |
||||
/* state vector x has the following entries [ax,ay,az||mx,my,mz||wox,woy,woz||wx,wy,wz]' */ |
||||
float z_k[9] = {0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 9.81f, 0.2f, -0.2f, 0.2f}; /**< Measurement vector */ |
||||
float x_aposteriori_k[12]; /**< states */ |
||||
float P_aposteriori_k[144] = {100.f, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, |
||||
0, 100.f, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, |
||||
0, 0, 100.f, 0, 0, 0, 0, 0, 0, 0, 0, 0, |
||||
0, 0, 0, 100.f, 0, 0, 0, 0, 0, 0, 0, 0, |
||||
0, 0, 0, 0, 100.f, 0, 0, 0, 0, 0, 0, 0, |
||||
0, 0, 0, 0, 0, 100.f, 0, 0, 0, 0, 0, 0, |
||||
0, 0, 0, 0, 0, 0, 100.f, 0, 0, 0, 0, 0, |
||||
0, 0, 0, 0, 0, 0, 0, 100.f, 0, 0, 0, 0, |
||||
0, 0, 0, 0, 0, 0, 0, 0, 100.f, 0, 0, 0, |
||||
0, 0, 0, 0, 0, 0, 0, 0, 0.0f, 100.0f, 0, 0, |
||||
0, 0, 0, 0, 0, 0, 0, 0, 0.0f, 0, 100.0f, 0, |
||||
0, 0, 0, 0, 0, 0, 0, 0, 0.0f, 0, 0, 100.0f, |
||||
}; /**< init: diagonal matrix with big values */ |
||||
|
||||
float x_aposteriori[12]; |
||||
float P_aposteriori[144]; |
||||
|
||||
/* output euler angles */ |
||||
float euler[3] = {0.0f, 0.0f, 0.0f}; |
||||
|
||||
float Rot_matrix[9] = {1.f, 0, 0, |
||||
0, 1.f, 0, |
||||
0, 0, 1.f |
||||
}; /**< init: identity matrix */ |
||||
|
||||
float debugOutput[4] = { 0.0f }; |
||||
|
||||
/* Initialize filter */ |
||||
AttitudeEKF_initialize(); |
||||
|
||||
struct sensor_combined_s raw; |
||||
memset(&raw, 0, sizeof(raw)); |
||||
struct vehicle_gps_position_s gps; |
||||
memset(&gps, 0, sizeof(gps)); |
||||
gps.eph = 100000; |
||||
gps.epv = 100000; |
||||
struct vehicle_global_position_s global_pos; |
||||
memset(&global_pos, 0, sizeof(global_pos)); |
||||
struct vehicle_attitude_s att; |
||||
memset(&att, 0, sizeof(att)); |
||||
struct vehicle_control_mode_s control_mode; |
||||
memset(&control_mode, 0, sizeof(control_mode)); |
||||
|
||||
uint64_t last_data = 0; |
||||
uint64_t last_measurement = 0; |
||||
uint64_t last_vel_t = 0; |
||||
|
||||
/* current velocity */ |
||||
math::Vector<3> vel; |
||||
vel.zero(); |
||||
/* previous velocity */ |
||||
math::Vector<3> vel_prev; |
||||
vel_prev.zero(); |
||||
/* actual acceleration (by GPS velocity) in body frame */ |
||||
math::Vector<3> acc; |
||||
acc.zero(); |
||||
/* rotation matrix */ |
||||
math::Matrix<3, 3> R; |
||||
R.identity(); |
||||
|
||||
/* subscribe to raw data */ |
||||
int sub_raw = orb_subscribe(ORB_ID(sensor_combined)); |
||||
|
||||
/* subscribe to GPS */ |
||||
int sub_gps = orb_subscribe(ORB_ID(vehicle_gps_position)); |
||||
|
||||
/* subscribe to GPS */ |
||||
int sub_global_pos = orb_subscribe(ORB_ID(vehicle_global_position)); |
||||
|
||||
/* subscribe to param changes */ |
||||
int sub_params = orb_subscribe(ORB_ID(parameter_update)); |
||||
|
||||
/* subscribe to control mode */ |
||||
int sub_control_mode = orb_subscribe(ORB_ID(vehicle_control_mode)); |
||||
|
||||
/* subscribe to vision estimate */ |
||||
int vision_sub = orb_subscribe(ORB_ID(vehicle_vision_attitude)); |
||||
|
||||
/* subscribe to mocap data */ |
||||
int mocap_sub = orb_subscribe(ORB_ID(att_pos_mocap)); |
||||
|
||||
/* advertise attitude */ |
||||
orb_advert_t pub_att = orb_advertise(ORB_ID(vehicle_attitude), &att); |
||||
|
||||
int loopcounter = 0; |
||||
|
||||
thread_running = true; |
||||
|
||||
/* keep track of sensor updates */ |
||||
uint64_t sensor_last_timestamp[3] = {0, 0, 0}; |
||||
|
||||
struct attitude_estimator_ekf_params ekf_params; |
||||
memset(&ekf_params, 0, sizeof(ekf_params)); |
||||
|
||||
struct attitude_estimator_ekf_param_handles ekf_param_handles = { 0 }; |
||||
|
||||
/* initialize parameter handles */ |
||||
parameters_init(&ekf_param_handles); |
||||
|
||||
bool initialized = false; |
||||
|
||||
float gyro_offsets[3] = { 0.0f, 0.0f, 0.0f }; |
||||
|
||||
/* magnetic declination, in radians */ |
||||
float mag_decl = 0.0f; |
||||
|
||||
/* rotation matrix for magnetic declination */ |
||||
math::Matrix<3, 3> R_decl; |
||||
R_decl.identity(); |
||||
|
||||
struct vehicle_attitude_s vision {}; |
||||
struct att_pos_mocap_s mocap {}; |
||||
|
||||
/* register the perf counter */ |
||||
perf_counter_t ekf_loop_perf = perf_alloc(PC_ELAPSED, "attitude_estimator_ekf"); |
||||
|
||||
/* Main loop*/ |
||||
while (!thread_should_exit) { |
||||
|
||||
px4_pollfd_struct_t fds[2]; |
||||
fds[0].fd = sub_raw; |
||||
fds[0].events = POLLIN; |
||||
fds[1].fd = sub_params; |
||||
fds[1].events = POLLIN; |
||||
int ret = px4_poll(fds, 2, 1000); |
||||
|
||||
if (ret < 0) { |
||||
/* XXX this is seriously bad - should be an emergency */ |
||||
} else if (ret == 0) { |
||||
/* check if we're in HIL - not getting sensor data is fine then */ |
||||
orb_copy(ORB_ID(vehicle_control_mode), sub_control_mode, &control_mode); |
||||
|
||||
if (!control_mode.flag_system_hil_enabled) { |
||||
warnx("WARNING: Not getting sensor data - sensor app running?"); |
||||
} |
||||
|
||||
} else { |
||||
|
||||
/* only update parameters if they changed */ |
||||
if (fds[1].revents & POLLIN) { |
||||
/* read from param to clear updated flag */ |
||||
struct parameter_update_s update; |
||||
orb_copy(ORB_ID(parameter_update), sub_params, &update); |
||||
|
||||
/* update parameters */ |
||||
parameters_update(&ekf_param_handles, &ekf_params); |
||||
} |
||||
|
||||
/* only run filter if sensor values changed */ |
||||
if (fds[0].revents & POLLIN) { |
||||
|
||||
/* get latest measurements */ |
||||
orb_copy(ORB_ID(sensor_combined), sub_raw, &raw); |
||||
|
||||
bool gps_updated; |
||||
orb_check(sub_gps, &gps_updated); |
||||
if (gps_updated) { |
||||
orb_copy(ORB_ID(vehicle_gps_position), sub_gps, &gps); |
||||
|
||||
if (gps.eph < 20.0f && hrt_elapsed_time(&gps.timestamp) < 1000000) { |
||||
mag_decl = math::radians(get_mag_declination(gps.lat / 1e7f, gps.lon / 1e7f)); |
||||
|
||||
/* update mag declination rotation matrix */ |
||||
R_decl.from_euler(0.0f, 0.0f, mag_decl); |
||||
} |
||||
} |
||||
|
||||
bool global_pos_updated; |
||||
orb_check(sub_global_pos, &global_pos_updated); |
||||
if (global_pos_updated) { |
||||
orb_copy(ORB_ID(vehicle_global_position), sub_global_pos, &global_pos); |
||||
} |
||||
|
||||
if (!initialized) { |
||||
// XXX disabling init for now
|
||||
initialized = true; |
||||
|
||||
// gyro_offsets[0] += raw.gyro_rad[0];
|
||||
// gyro_offsets[1] += raw.gyro_rad[1];
|
||||
// gyro_offsets[2] += raw.gyro_rad[2];
|
||||
// offset_count++;
|
||||
|
||||
// if (hrt_absolute_time() - start_time > 3000000LL) {
|
||||
// initialized = true;
|
||||
// gyro_offsets[0] /= offset_count;
|
||||
// gyro_offsets[1] /= offset_count;
|
||||
// gyro_offsets[2] /= offset_count;
|
||||
// }
|
||||
|
||||
} else { |
||||
|
||||
perf_begin(ekf_loop_perf); |
||||
|
||||
/* Calculate data time difference in seconds */ |
||||
dt = (raw.timestamp - last_measurement) / 1000000.0f; |
||||
last_measurement = raw.timestamp; |
||||
uint8_t update_vect[3] = {0, 0, 0}; |
||||
|
||||
/* Fill in gyro measurements */ |
||||
if (sensor_last_timestamp[0] != raw.timestamp) { |
||||
update_vect[0] = 1; |
||||
// sensor_update_hz[0] = 1e6f / (raw.timestamp - sensor_last_timestamp[0]);
|
||||
sensor_last_timestamp[0] = raw.timestamp; |
||||
} |
||||
|
||||
z_k[0] = raw.gyro_rad[0] - gyro_offsets[0]; |
||||
z_k[1] = raw.gyro_rad[1] - gyro_offsets[1]; |
||||
z_k[2] = raw.gyro_rad[2] - gyro_offsets[2]; |
||||
|
||||
/* update accelerometer measurements */ |
||||
if (sensor_last_timestamp[1] != raw.timestamp + raw.accelerometer_timestamp_relative) { |
||||
update_vect[1] = 1; |
||||
// sensor_update_hz[1] = 1e6f / (raw.timestamp - sensor_last_timestamp[1]);
|
||||
sensor_last_timestamp[1] = raw.timestamp + raw.accelerometer_timestamp_relative; |
||||
} |
||||
|
||||
hrt_abstime vel_t = 0; |
||||
bool vel_valid = false; |
||||
if (gps.eph < 5.0f && global_pos.timestamp != 0 && hrt_absolute_time() < global_pos.timestamp + 20000) { |
||||
vel_valid = true; |
||||
if (global_pos_updated) { |
||||
vel_t = global_pos.timestamp; |
||||
vel(0) = global_pos.vel_n; |
||||
vel(1) = global_pos.vel_e; |
||||
vel(2) = global_pos.vel_d; |
||||
} |
||||
} |
||||
|
||||
if (vel_valid) { |
||||
/* velocity is valid */ |
||||
if (vel_t != 0) { |
||||
/* velocity updated */ |
||||
if (last_vel_t != 0 && vel_t != last_vel_t) { |
||||
float vel_dt = (vel_t - last_vel_t) / 1000000.0f; |
||||
/* calculate acceleration in body frame */ |
||||
acc = R.transposed() * ((vel - vel_prev) / vel_dt); |
||||
} |
||||
last_vel_t = vel_t; |
||||
vel_prev = vel; |
||||
} |
||||
|
||||
} else { |
||||
/* velocity is valid, reset acceleration */ |
||||
acc.zero(); |
||||
vel_prev.zero(); |
||||
last_vel_t = 0; |
||||
} |
||||
|
||||
z_k[3] = raw.accelerometer_m_s2[0] - acc(0); |
||||
z_k[4] = raw.accelerometer_m_s2[1] - acc(1); |
||||
z_k[5] = raw.accelerometer_m_s2[2] - acc(2); |
||||
|
||||
/* update magnetometer measurements */ |
||||
if (sensor_last_timestamp[2] != raw.timestamp + raw.magnetometer_timestamp_relative && |
||||
/* check that the mag vector is > 0 */ |
||||
fabsf(sqrtf(raw.magnetometer_ga[0] * raw.magnetometer_ga[0] + |
||||
raw.magnetometer_ga[1] * raw.magnetometer_ga[1] + |
||||
raw.magnetometer_ga[2] * raw.magnetometer_ga[2])) > 0.1f) { |
||||
update_vect[2] = 1; |
||||
// sensor_update_hz[2] = 1e6f / (raw.timestamp - sensor_last_timestamp[2]);
|
||||
sensor_last_timestamp[2] = raw.timestamp + raw.magnetometer_timestamp_relative; |
||||
} |
||||
|
||||
bool vision_updated = false; |
||||
orb_check(vision_sub, &vision_updated); |
||||
|
||||
bool mocap_updated = false; |
||||
orb_check(mocap_sub, &mocap_updated); |
||||
|
||||
if (vision_updated) { |
||||
orb_copy(ORB_ID(vehicle_vision_attitude), vision_sub, &vision); |
||||
} |
||||
|
||||
if (mocap_updated) { |
||||
orb_copy(ORB_ID(att_pos_mocap), mocap_sub, &mocap); |
||||
} |
||||
|
||||
if (mocap.timestamp > 0 && (hrt_elapsed_time(&mocap.timestamp) < 500000)) { |
||||
|
||||
math::Quaternion q(mocap.q); |
||||
math::Matrix<3, 3> Rmoc = q.to_dcm(); |
||||
|
||||
math::Vector<3> v(1.0f, 0.0f, 0.4f); |
||||
|
||||
math::Vector<3> vn = Rmoc.transposed() * v; //Rmoc is Rwr (robot respect to world) while v is respect to world. Hence Rmoc must be transposed having (Rwr)' * Vw
|
||||
// Rrw * Vw = vn. This way we have consistency
|
||||
z_k[6] = vn(0); |
||||
z_k[7] = vn(1); |
||||
z_k[8] = vn(2); |
||||
} else if (vision.timestamp > 0 && (hrt_elapsed_time(&vision.timestamp) < 500000)) { |
||||
|
||||
math::Quaternion q(vision.q); |
||||
math::Matrix<3, 3> Rvis = q.to_dcm(); |
||||
|
||||
math::Vector<3> v(1.0f, 0.0f, 0.4f); |
||||
|
||||
math::Vector<3> vn = Rvis.transposed() * v; //Rvis is Rwr (robot respect to world) while v is respect to world. Hence Rvis must be transposed having (Rwr)' * Vw
|
||||
// Rrw * Vw = vn. This way we have consistency
|
||||
z_k[6] = vn(0); |
||||
z_k[7] = vn(1); |
||||
z_k[8] = vn(2); |
||||
} else { |
||||
z_k[6] = raw.magnetometer_ga[0]; |
||||
z_k[7] = raw.magnetometer_ga[1]; |
||||
z_k[8] = raw.magnetometer_ga[2]; |
||||
} |
||||
|
||||
static bool const_initialized = false; |
||||
|
||||
/* initialize with good values once we have a reasonable dt estimate */ |
||||
if (!const_initialized && dt < 0.05f && dt > 0.001f) { |
||||
dt = 0.005f; |
||||
parameters_update(&ekf_param_handles, &ekf_params); |
||||
|
||||
/* update mag declination rotation matrix */ |
||||
if (gps.eph < 20.0f && hrt_elapsed_time(&gps.timestamp) < 1000000) { |
||||
mag_decl = math::radians(get_mag_declination(gps.lat / 1e7f, gps.lon / 1e7f)); |
||||
|
||||
} |
||||
|
||||
/* update mag declination rotation matrix */ |
||||
R_decl.from_euler(0.0f, 0.0f, mag_decl); |
||||
|
||||
x_aposteriori_k[0] = z_k[0]; |
||||
x_aposteriori_k[1] = z_k[1]; |
||||
x_aposteriori_k[2] = z_k[2]; |
||||
x_aposteriori_k[3] = 0.0f; |
||||
x_aposteriori_k[4] = 0.0f; |
||||
x_aposteriori_k[5] = 0.0f; |
||||
x_aposteriori_k[6] = z_k[3]; |
||||
x_aposteriori_k[7] = z_k[4]; |
||||
x_aposteriori_k[8] = z_k[5]; |
||||
x_aposteriori_k[9] = z_k[6]; |
||||
x_aposteriori_k[10] = z_k[7]; |
||||
x_aposteriori_k[11] = z_k[8]; |
||||
|
||||
const_initialized = true; |
||||
} |
||||
|
||||
/* do not execute the filter if not initialized */ |
||||
if (!const_initialized) { |
||||
continue; |
||||
} |
||||
|
||||
/* Call the estimator */ |
||||
AttitudeEKF(false, // approx_prediction
|
||||
(unsigned char)ekf_params.use_moment_inertia, |
||||
update_vect, |
||||
dt, |
||||
z_k, |
||||
ekf_params.q[0], // q_rotSpeed,
|
||||
ekf_params.q[1], // q_rotAcc
|
||||
ekf_params.q[2], // q_acc
|
||||
ekf_params.q[3], // q_mag
|
||||
ekf_params.r[0], // r_gyro
|
||||
ekf_params.r[1], // r_accel
|
||||
ekf_params.r[2], // r_mag
|
||||
ekf_params.moment_inertia_J, |
||||
x_aposteriori, |
||||
P_aposteriori, |
||||
Rot_matrix, |
||||
euler, |
||||
debugOutput); |
||||
|
||||
/* swap values for next iteration, check for fatal inputs */ |
||||
if (PX4_ISFINITE(euler[0]) && PX4_ISFINITE(euler[1]) && PX4_ISFINITE(euler[2])) { |
||||
memcpy(P_aposteriori_k, P_aposteriori, sizeof(P_aposteriori_k)); |
||||
memcpy(x_aposteriori_k, x_aposteriori, sizeof(x_aposteriori_k)); |
||||
|
||||
} else { |
||||
/* due to inputs or numerical failure the output is invalid, skip it */ |
||||
continue; |
||||
} |
||||
|
||||
if (last_data > 0 && raw.timestamp - last_data > 30000) { |
||||
warnx("sensor data missed! (%llu)\n", static_cast<unsigned long long>(raw.timestamp - last_data)); |
||||
} |
||||
|
||||
last_data = raw.timestamp; |
||||
|
||||
/* send out */ |
||||
att.timestamp = raw.timestamp; |
||||
|
||||
|
||||
att.rollspeed = x_aposteriori[0]; |
||||
att.pitchspeed = x_aposteriori[1]; |
||||
att.yawspeed = x_aposteriori[2]; |
||||
|
||||
/* magnetic declination */ |
||||
matrix::Dcmf Ro(&Rot_matrix[0]); |
||||
matrix::Dcmf R_declination(&R_decl.data[0][0]); |
||||
matrix::Quatf q = matrix::Quatf(R_declination * Ro); |
||||
|
||||
memcpy(&att.q[0],&q._data[0],sizeof(att.q)); |
||||
|
||||
if (PX4_ISFINITE(att.q[0]) && PX4_ISFINITE(att.q[1]) |
||||
&& PX4_ISFINITE(att.q[2]) && PX4_ISFINITE(att.q[3])) { |
||||
// Broadcast
|
||||
orb_publish(ORB_ID(vehicle_attitude), pub_att, &att); |
||||
|
||||
} else { |
||||
PX4_ERR("ERR: NaN estimate!"); |
||||
} |
||||
|
||||
perf_end(ekf_loop_perf); |
||||
} |
||||
} |
||||
} |
||||
|
||||
loopcounter++; |
||||
} |
||||
|
||||
thread_running = false; |
||||
|
||||
return 0; |
||||
} |
||||
@ -1,130 +0,0 @@
@@ -1,130 +0,0 @@
|
||||
/****************************************************************************
|
||||
* |
||||
* Copyright (C) 2012 PX4 Development Team. All rights reserved. |
||||
* Author: Tobias Naegeli <naegelit@student.ethz.ch> |
||||
* Lorenz Meier <lm@inf.ethz.ch> |
||||
* |
||||
* Redistribution and use in source and binary forms, with or without |
||||
* modification, are permitted provided that the following conditions |
||||
* are met: |
||||
* |
||||
* 1. Redistributions of source code must retain the above copyright |
||||
* notice, this list of conditions and the following disclaimer. |
||||
* 2. Redistributions in binary form must reproduce the above copyright |
||||
* notice, this list of conditions and the following disclaimer in |
||||
* the documentation and/or other materials provided with the |
||||
* distribution. |
||||
* 3. Neither the name PX4 nor the names of its contributors may be |
||||
* used to endorse or promote products derived from this software |
||||
* without specific prior written permission. |
||||
* |
||||
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS |
||||
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT |
||||
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS |
||||
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE |
||||
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, |
||||
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, |
||||
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS |
||||
* OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED |
||||
* AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT |
||||
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN |
||||
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE |
||||
* POSSIBILITY OF SUCH DAMAGE. |
||||
* |
||||
****************************************************************************/ |
||||
|
||||
/*
|
||||
* @file attitude_estimator_ekf_params.c |
||||
* |
||||
* Parameters for EKF filter |
||||
*/ |
||||
|
||||
#include "attitude_estimator_ekf_params.h" |
||||
#include <math.h> |
||||
#include <px4_defines.h> |
||||
|
||||
/* Extended Kalman Filter covariances */ |
||||
|
||||
|
||||
/**
|
||||
* Body angular rate process noise |
||||
* |
||||
* @group Attitude EKF estimator |
||||
*/ |
||||
PARAM_DEFINE_FLOAT(EKF_ATT_V3_Q0, 1e-4f); |
||||
|
||||
/**
|
||||
* Body angular acceleration process noise |
||||
* |
||||
* @group Attitude EKF estimator |
||||
*/ |
||||
PARAM_DEFINE_FLOAT(EKF_ATT_V3_Q1, 0.08f); |
||||
|
||||
/**
|
||||
* Acceleration process noise |
||||
* |
||||
* @group Attitude EKF estimator |
||||
*/ |
||||
PARAM_DEFINE_FLOAT(EKF_ATT_V3_Q2, 0.009f); |
||||
|
||||
/**
|
||||
* Magnet field vector process noise |
||||
* |
||||
* @group Attitude EKF estimator |
||||
*/ |
||||
PARAM_DEFINE_FLOAT(EKF_ATT_V3_Q3, 0.005f); |
||||
|
||||
/**
|
||||
* Gyro measurement noise |
||||
* |
||||
* @group Attitude EKF estimator |
||||
*/ |
||||
PARAM_DEFINE_FLOAT(EKF_ATT_V4_R0, 0.0008f); |
||||
|
||||
/**
|
||||
* Accel measurement noise |
||||
* |
||||
* @group Attitude EKF estimator |
||||
*/ |
||||
PARAM_DEFINE_FLOAT(EKF_ATT_V4_R1, 10000.0f); |
||||
|
||||
/**
|
||||
* Mag measurement noise |
||||
* |
||||
* @group Attitude EKF estimator |
||||
*/ |
||||
PARAM_DEFINE_FLOAT(EKF_ATT_V4_R2, 100.0f); |
||||
|
||||
/**
|
||||
* Moment of inertia matrix diagonal entry (1, 1) |
||||
* |
||||
* @group Attitude EKF estimator |
||||
* @unit kg*m^2 |
||||
*/ |
||||
PARAM_DEFINE_FLOAT(ATT_J11, 0.0018); |
||||
|
||||
/**
|
||||
* Moment of inertia matrix diagonal entry (2, 2) |
||||
* |
||||
* @group Attitude EKF estimator |
||||
* @unit kg*m^2 |
||||
*/ |
||||
PARAM_DEFINE_FLOAT(ATT_J22, 0.0018); |
||||
|
||||
/**
|
||||
* Moment of inertia matrix diagonal entry (3, 3) |
||||
* |
||||
* @group Attitude EKF estimator |
||||
* @unit kg*m^2 |
||||
*/ |
||||
PARAM_DEFINE_FLOAT(ATT_J33, 0.0037); |
||||
|
||||
/**
|
||||
* Moment of inertia enabled in estimator |
||||
* |
||||
* If set to != 0 the moment of inertia will be used in the estimator |
||||
* |
||||
* @group Attitude EKF estimator |
||||
* @boolean |
||||
*/ |
||||
PARAM_DEFINE_INT32(ATT_J_EN, 0); |
||||
@ -1,75 +0,0 @@
@@ -1,75 +0,0 @@
|
||||
/****************************************************************************
|
||||
* |
||||
* Copyright (C) 2012 PX4 Development Team. All rights reserved. |
||||
* Author: Tobias Naegeli <naegelit@student.ethz.ch> |
||||
* Lorenz Meier <lm@inf.ethz.ch> |
||||
* |
||||
* Redistribution and use in source and binary forms, with or without |
||||
* modification, are permitted provided that the following conditions |
||||
* are met: |
||||
* |
||||
* 1. Redistributions of source code must retain the above copyright |
||||
* notice, this list of conditions and the following disclaimer. |
||||
* 2. Redistributions in binary form must reproduce the above copyright |
||||
* notice, this list of conditions and the following disclaimer in |
||||
* the documentation and/or other materials provided with the |
||||
* distribution. |
||||
* 3. Neither the name PX4 nor the names of its contributors may be |
||||
* used to endorse or promote products derived from this software |
||||
* without specific prior written permission. |
||||
* |
||||
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS |
||||
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT |
||||
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS |
||||
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE |
||||
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, |
||||
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, |
||||
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS |
||||
* OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED |
||||
* AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT |
||||
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN |
||||
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE |
||||
* POSSIBILITY OF SUCH DAMAGE. |
||||
* |
||||
****************************************************************************/ |
||||
|
||||
/*
|
||||
* @file attitude_estimator_ekf_params.h |
||||
* |
||||
* Parameters for EKF filter |
||||
*/ |
||||
|
||||
#include <systemlib/param/param.h> |
||||
|
||||
struct attitude_estimator_ekf_params { |
||||
float r[3]; |
||||
float q[4]; |
||||
float moment_inertia_J[9]; |
||||
int32_t use_moment_inertia; |
||||
float roll_off; |
||||
float pitch_off; |
||||
float yaw_off; |
||||
float mag_decl; |
||||
int acc_comp; |
||||
}; |
||||
|
||||
struct attitude_estimator_ekf_param_handles { |
||||
param_t r0, r1, r2; |
||||
param_t q0, q1, q2, q3; |
||||
param_t moment_inertia_J[3]; /**< diagonal entries of the matrix */ |
||||
param_t use_moment_inertia; |
||||
param_t mag_decl; |
||||
param_t acc_comp; |
||||
}; |
||||
|
||||
/**
|
||||
* Initialize all parameter handles and values |
||||
* |
||||
*/ |
||||
int parameters_init(struct attitude_estimator_ekf_param_handles *h); |
||||
|
||||
/**
|
||||
* Update all parameters |
||||
* |
||||
*/ |
||||
int parameters_update(const struct attitude_estimator_ekf_param_handles *h, struct attitude_estimator_ekf_params *p); |
||||
File diff suppressed because it is too large
Load Diff
@ -1,26 +0,0 @@
@@ -1,26 +0,0 @@
|
||||
/*
|
||||
* AttitudeEKF.h |
||||
* |
||||
* Code generation for function 'AttitudeEKF' |
||||
* |
||||
* C source code generated on: Thu Aug 21 11:17:28 2014 |
||||
* |
||||
*/ |
||||
|
||||
#ifndef __ATTITUDEEKF_H__ |
||||
#define __ATTITUDEEKF_H__ |
||||
/* Include files */ |
||||
#include <math.h> |
||||
#include <stddef.h> |
||||
#include <stdlib.h> |
||||
#include <string.h> |
||||
|
||||
#include "rtwtypes.h" |
||||
#include "AttitudeEKF_types.h" |
||||
|
||||
/* Function Declarations */ |
||||
extern void AttitudeEKF(unsigned char approx_prediction, unsigned char use_inertia_matrix, const unsigned char zFlag[3], float dt, const float z[9], float q_rotSpeed, float q_rotAcc, float q_acc, float q_mag, float r_gyro, float r_accel, float r_mag, const float J[9], float xa_apo[12], float Pa_apo[144], float Rot_matrix[9], float eulerAngles[3], float debugOutput[4]); |
||||
extern void AttitudeEKF_initialize(void); |
||||
extern void AttitudeEKF_terminate(void); |
||||
#endif |
||||
/* End of code generation (AttitudeEKF.h) */ |
||||
@ -1,17 +0,0 @@
@@ -1,17 +0,0 @@
|
||||
/*
|
||||
* AttitudeEKF_types.h |
||||
* |
||||
* Code generation for function 'AttitudeEKF' |
||||
* |
||||
* C source code generated on: Thu Aug 21 11:17:28 2014 |
||||
* |
||||
*/ |
||||
|
||||
#ifndef __ATTITUDEEKF_TYPES_H__ |
||||
#define __ATTITUDEEKF_TYPES_H__ |
||||
|
||||
/* Include files */ |
||||
#include "rtwtypes.h" |
||||
|
||||
#endif |
||||
/* End of code generation (AttitudeEKF_types.h) */ |
||||
@ -1,160 +0,0 @@
@@ -1,160 +0,0 @@
|
||||
/*
|
||||
* rtwtypes.h |
||||
* |
||||
* Code generation for function 'AttitudeEKF' |
||||
* |
||||
* C source code generated on: Thu Aug 21 11:17:28 2014 |
||||
* |
||||
*/ |
||||
|
||||
#ifndef __RTWTYPES_H__ |
||||
#define __RTWTYPES_H__ |
||||
#ifndef TRUE |
||||
# define TRUE (1U) |
||||
#endif |
||||
#ifndef FALSE |
||||
# define FALSE (0U) |
||||
#endif |
||||
#ifndef __TMWTYPES__ |
||||
#define __TMWTYPES__ |
||||
|
||||
#include <limits.h> |
||||
|
||||
/*=======================================================================*
|
||||
* Target hardware information |
||||
* Device type: ARM Compatible->ARM Cortex |
||||
* Number of bits: char: 8 short: 16 int: 32 |
||||
* long: 32 |
||||
* native word size: 32 |
||||
* Byte ordering: LittleEndian |
||||
* Signed integer division rounds to: Undefined |
||||
* Shift right on a signed integer as arithmetic shift: on |
||||
*=======================================================================*/ |
||||
|
||||
/*=======================================================================*
|
||||
* Fixed width word size data types: *
|
||||
* int8_T, int16_T, int32_T - signed 8, 16, or 32 bit integers *
|
||||
* uint8_T, uint16_T, uint32_T - unsigned 8, 16, or 32 bit integers *
|
||||
* real32_T, real64_T - 32 and 64 bit floating point numbers *
|
||||
*=======================================================================*/ |
||||
|
||||
typedef signed char int8_T; |
||||
typedef unsigned char uint8_T; |
||||
typedef short int16_T; |
||||
typedef unsigned short uint16_T; |
||||
typedef int int32_T; |
||||
typedef unsigned int uint32_T; |
||||
typedef float real32_T; |
||||
typedef double real64_T; |
||||
|
||||
/*===========================================================================*
|
||||
* Generic type definitions: real_T, time_T, boolean_T, int_T, uint_T, *
|
||||
* ulong_T, char_T and byte_T. *
|
||||
*===========================================================================*/ |
||||
|
||||
typedef double real_T; |
||||
typedef double time_T; |
||||
typedef unsigned char boolean_T; |
||||
typedef int int_T; |
||||
typedef unsigned int uint_T; |
||||
typedef unsigned long ulong_T; |
||||
typedef char char_T; |
||||
typedef char_T byte_T; |
||||
|
||||
/*===========================================================================*
|
||||
* Complex number type definitions *
|
||||
*===========================================================================*/ |
||||
#define CREAL_T |
||||
typedef struct {
|
||||
real32_T re;
|
||||
real32_T im;
|
||||
} creal32_T;
|
||||
|
||||
typedef struct {
|
||||
real64_T re;
|
||||
real64_T im;
|
||||
} creal64_T;
|
||||
|
||||
typedef struct {
|
||||
real_T re;
|
||||
real_T im;
|
||||
} creal_T;
|
||||
|
||||
typedef struct {
|
||||
int8_T re;
|
||||
int8_T im;
|
||||
} cint8_T;
|
||||
|
||||
typedef struct {
|
||||
uint8_T re;
|
||||
uint8_T im;
|
||||
} cuint8_T;
|
||||
|
||||
typedef struct {
|
||||
int16_T re;
|
||||
int16_T im;
|
||||
} cint16_T;
|
||||
|
||||
typedef struct {
|
||||
uint16_T re;
|
||||
uint16_T im;
|
||||
} cuint16_T;
|
||||
|
||||
typedef struct {
|
||||
int32_T re;
|
||||
int32_T im;
|
||||
} cint32_T;
|
||||
|
||||
typedef struct {
|
||||
uint32_T re;
|
||||
uint32_T im;
|
||||
} cuint32_T;
|
||||
|
||||
|
||||
/*=======================================================================*
|
||||
* Min and Max: *
|
||||
* int8_T, int16_T, int32_T - signed 8, 16, or 32 bit integers *
|
||||
* uint8_T, uint16_T, uint32_T - unsigned 8, 16, or 32 bit integers *
|
||||
*=======================================================================*/ |
||||
|
||||
#define MAX_int8_T ((int8_T)(127)) |
||||
#define MIN_int8_T ((int8_T)(-128)) |
||||
#define MAX_uint8_T ((uint8_T)(255)) |
||||
#define MIN_uint8_T ((uint8_T)(0)) |
||||
#define MAX_int16_T ((int16_T)(32767)) |
||||
#define MIN_int16_T ((int16_T)(-32768)) |
||||
#define MAX_uint16_T ((uint16_T)(65535)) |
||||
#define MIN_uint16_T ((uint16_T)(0)) |
||||
#define MAX_int32_T ((int32_T)(2147483647)) |
||||
#define MIN_int32_T ((int32_T)(-2147483647-1)) |
||||
#define MAX_uint32_T ((uint32_T)(0xFFFFFFFFU)) |
||||
#define MIN_uint32_T ((uint32_T)(0)) |
||||
|
||||
/* Logical type definitions */ |
||||
#if !defined(__cplusplus) && !defined(__true_false_are_keywords) |
||||
# ifndef false |
||||
# define false (0U) |
||||
# endif |
||||
# ifndef true |
||||
# define true (1U) |
||||
# endif |
||||
#endif |
||||
|
||||
/*
|
||||
* MATLAB for code generation assumes the code is compiled on a target using a 2's compliment representation |
||||
* for signed integer values. |
||||
*/ |
||||
#if ((SCHAR_MIN + 1) != -SCHAR_MAX) |
||||
#error "This code must be compiled using a 2's complement representation for signed integer values" |
||||
#endif |
||||
|
||||
/*
|
||||
* Maximum length of a MATLAB identifier (function/variable) |
||||
* including the null-termination character. Referenced by |
||||
* rt_logging.c and rt_matrx.c. |
||||
*/ |
||||
#define TMW_NAME_LENGTH_MAX 64 |
||||
|
||||
#endif |
||||
#endif |
||||
/* End of code generation (rtwtypes.h) */ |
||||
Loading…
Reference in new issue