mirror of
https://gitee.com/mirrors_PX4/PX4-Autopilot.git
synced 2026-04-28 12:14:06 +08:00
delete exampales attitude_estimator_ekf
This commit is contained in:
Daniel Agar
parent
dd7e82389c
commit
d5ea688f00
@ -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 @@
|
||||
############################################################################
|
||||
#
|
||||
# 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 @@
|
||||
<?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 @@
|
||||
/****************************************************************************
|
||||
*
|
||||
* 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 @@
|
||||
/****************************************************************************
|
||||
*
|
||||
* 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 @@
|
||||
/****************************************************************************
|
||||
*
|
||||
* 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 @@
|
||||
/*
|
||||
* 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 @@
|
||||
/*
|
||||
* 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 @@
|
||||
/*
|
||||
* 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…
x
Reference in New Issue
Block a user