fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-05-16 16:17:36 +08:00
Code Issues Packages Projects Releases Wiki Activity

Move old estimators to examples

Browse Source
This commit is contained in:
Lorenz Meier
2016-08-06 11:56:58 +02:00
parent cff9e90bec
commit ee5cdab963
18 changed files with 0 additions and 0 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/examples/attitude_estimator_ekf/AttitudeEKF.m
+298
View File
@@ -0,0 +1,298 @@
function [xa_apo,Pa_apo,Rot_matrix,eulerAngles,debugOutput]...
= AttitudeEKF(approx_prediction,use_inertia_matrix,zFlag,dt,z,q_rotSpeed,q_rotAcc,q_acc,q_mag,r_gyro,r_accel,r_mag,J)
%LQG 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];
src/examples/attitude_estimator_ekf/CMakeLists.txt
+45
View File
@@ -0,0 +1,45 @@
############################################################################
#
# 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 modules__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 :
src/examples/attitude_estimator_ekf/attitudeKalmanfilter.prj
+502
View File
@@ -0,0 +1,502 @@
<?xml version="1.0" encoding="UTF-8"?>
<deployment-project plugin="plugin.matlabcoder" plugin-version="R2013a">
<configuration target="target.matlab.coder" target-name="MEX, C, and C++ Code Generation" name="attitudeKalmanfilter" location="/home/thomasgubler/src/Firmware/src/modules/attitude_estimator_ekf" file="/home/thomasgubler/src/Firmware/src/modules/attitude_estimator_ekf/attitudeKalmanfilter.prj" build-checksum="1213478164">
<profile key="profile.mex">
<param.MergeInstrumentationResults>false</param.MergeInstrumentationResults>
<param.BuiltInstrumentedMex>false</param.BuiltInstrumentedMex>
<param.RanInstrumentedMex>false</param.RanInstrumentedMex>
<param.WorkingFolder>option.WorkingFolder.Project</param.WorkingFolder>
<param.SpecifiedWorkingFolder />
<param.BuildFolder>option.BuildFolder.Project</param.BuildFolder>
<param.SpecifiedBuildFolder />
<param.SearchPaths />
<param.ResponsivenessChecks>true</param.ResponsivenessChecks>
<param.ExtrinsicCalls>true</param.ExtrinsicCalls>
<param.IntegrityChecks>true</param.IntegrityChecks>
<param.SaturateOnIntegerOverflow>true</param.SaturateOnIntegerOverflow>
<param.GlobalDataSyncMethod>option.GlobalDataSyncMethod.SyncAlways</param.GlobalDataSyncMethod>
<param.EnableVariableSizing>true</param.EnableVariableSizing>
<param.DynamicMemoryAllocation>option.DynamicMemoryAllocation.Threshold</param.DynamicMemoryAllocation>
<param.DynamicMemoryAllocationThreshold>65536</param.DynamicMemoryAllocationThreshold>
<param.StackUsageMax>200000</param.StackUsageMax>
<param.FilePartitionMethod>option.FilePartitionMethod.MapMFileToCFile</param.FilePartitionMethod>
<param.GenerateComments>true</param.GenerateComments>
<param.MATLABSourceComments>false</param.MATLABSourceComments>
<param.ReservedNameArray />
<param.EnableScreener>true</param.EnableScreener>
<param.EnableDebugging>false</param.EnableDebugging>
<param.GenerateReport>true</param.GenerateReport>
<param.LaunchReport>false</param.LaunchReport>
<param.CustomSourceCode />
<param.CustomHeaderCode />
<param.CustomInitializer />
<param.CustomTerminator />
<param.CustomInclude />
<param.CustomSource />
<param.CustomLibrary />
<param.PostCodeGenCommand />
<param.ProposeFixedPointDataTypes>true</param.ProposeFixedPointDataTypes>
<param.mex.GenCodeOnly>false</param.mex.GenCodeOnly>
<param.ConstantFoldingTimeout>40000</param.ConstantFoldingTimeout>
<param.RecursionLimit>100</param.RecursionLimit>
<param.TargetLang>option.TargetLang.C</param.TargetLang>
<param.EchoExpressions>true</param.EchoExpressions>
<param.InlineThreshold>10</param.InlineThreshold>
<param.InlineThresholdMax>200</param.InlineThresholdMax>
<param.InlineStackLimit>4000</param.InlineStackLimit>
<param.EnableMemcpy>true</param.EnableMemcpy>
<param.MemcpyThreshold>64</param.MemcpyThreshold>
<param.EnableOpenMP>true</param.EnableOpenMP>
<param.InitFltsAndDblsToZero>true</param.InitFltsAndDblsToZero>
<param.ConstantInputs>option.ConstantInputs.CheckValues</param.ConstantInputs>
<unset>
<param.MergeInstrumentationResults />
<param.BuiltInstrumentedMex />
<param.RanInstrumentedMex />
<param.WorkingFolder />
<param.SpecifiedWorkingFolder />
<param.BuildFolder />
<param.SpecifiedBuildFolder />
<param.SearchPaths />
<param.ResponsivenessChecks />
<param.ExtrinsicCalls />
<param.IntegrityChecks />
<param.SaturateOnIntegerOverflow />
<param.GlobalDataSyncMethod />
<param.EnableVariableSizing />
<param.DynamicMemoryAllocation />
<param.DynamicMemoryAllocationThreshold />
<param.StackUsageMax />
<param.FilePartitionMethod />
<param.GenerateComments />
<param.MATLABSourceComments />
<param.ReservedNameArray />
<param.EnableScreener />
<param.EnableDebugging />
<param.GenerateReport />
<param.LaunchReport />
<param.CustomSourceCode />
<param.CustomHeaderCode />
<param.CustomInitializer />
<param.CustomTerminator />
<param.CustomInclude />
<param.CustomSource />
<param.CustomLibrary />
<param.PostCodeGenCommand />
<param.ProposeFixedPointDataTypes />
<param.mex.GenCodeOnly />
<param.ConstantFoldingTimeout />
<param.RecursionLimit />
<param.TargetLang />
<param.EchoExpressions />
<param.InlineThreshold />
<param.InlineThresholdMax />
<param.InlineStackLimit />
<param.EnableMemcpy />
<param.MemcpyThreshold />
<param.EnableOpenMP />
<param.InitFltsAndDblsToZero />
<param.ConstantInputs />
</unset>
</profile>
<profile key="profile.c">
<param.grt.GenCodeOnly>true</param.grt.GenCodeOnly>
<param.WorkingFolder>option.WorkingFolder.Project</param.WorkingFolder>
<param.SpecifiedWorkingFolder />
<param.BuildFolder>option.BuildFolder.Specified</param.BuildFolder>
<param.SpecifiedBuildFolder>codegen</param.SpecifiedBuildFolder>
<param.SearchPaths />
<param.SaturateOnIntegerOverflow>true</param.SaturateOnIntegerOverflow>
<param.PurelyIntegerCode>false</param.PurelyIntegerCode>
<param.SupportNonFinite>false</param.SupportNonFinite>
<param.EnableVariableSizing>false</param.EnableVariableSizing>
<param.DynamicMemoryAllocation>option.DynamicMemoryAllocation.Threshold</param.DynamicMemoryAllocation>
<param.DynamicMemoryAllocationThreshold>65536</param.DynamicMemoryAllocationThreshold>
<param.StackUsageMax>4000</param.StackUsageMax>
<param.MultiInstanceCode>false</param.MultiInstanceCode>
<param.FilePartitionMethod>option.FilePartitionMethod.SingleFile</param.FilePartitionMethod>
<param.GenerateComments>true</param.GenerateComments>
<param.MATLABSourceComments>true</param.MATLABSourceComments>
<param.MATLABFcnDesc>false</param.MATLABFcnDesc>
<param.DataTypeReplacement>option.DataTypeReplacement.CBuiltIn</param.DataTypeReplacement>
<param.ConvertIfToSwitch>false</param.ConvertIfToSwitch>
<param.PreserveExternInFcnDecls>true</param.PreserveExternInFcnDecls>
<param.ParenthesesLevel>option.ParenthesesLevel.Nominal</param.ParenthesesLevel>
<param.MaxIdLength>31</param.MaxIdLength>
<param.CustomSymbolStrGlobalVar>$M$N</param.CustomSymbolStrGlobalVar>
<param.CustomSymbolStrType>$M$N</param.CustomSymbolStrType>
<param.CustomSymbolStrField>$M$N</param.CustomSymbolStrField>
<param.CustomSymbolStrFcn>$M$N</param.CustomSymbolStrFcn>
<param.CustomSymbolStrTmpVar>$M$N</param.CustomSymbolStrTmpVar>
<param.CustomSymbolStrMacro>$M$N</param.CustomSymbolStrMacro>
<param.CustomSymbolStrEMXArray>emxArray_$M$N</param.CustomSymbolStrEMXArray>
<param.CustomSymbolStrEMXArrayFcn>emx$M$N</param.CustomSymbolStrEMXArrayFcn>
<param.ReservedNameArray />
<param.EnableScreener>true</param.EnableScreener>
<param.Verbose>false</param.Verbose>
<param.GenerateReport>true</param.GenerateReport>
<param.GenerateCodeMetricsReport>true</param.GenerateCodeMetricsReport>
<param.GenerateCodeReplacementReport>false</param.GenerateCodeReplacementReport>
<param.LaunchReport>true</param.LaunchReport>
<param.CustomSourceCode />
<param.CustomHeaderCode />
<param.CustomInitializer />
<param.CustomTerminator />
<param.CustomInclude />
<param.CustomSource />
<param.CustomLibrary />
<param.PostCodeGenCommand />
<param.CodeReplacementLibrary>C89/C90 (ANSI)</param.CodeReplacementLibrary>
<param.SameHardware>true</param.SameHardware>
<param.HardwareVendor.Production>ARM Compatible</param.HardwareVendor.Production>
<param.HardwareType.Production>ARM Cortex</param.HardwareType.Production>
<var.instance.enabled.Production>true</var.instance.enabled.Production>
<param.HardwareSizeChar.Production>8</param.HardwareSizeChar.Production>
<param.HardwareSizeShort.Production>16</param.HardwareSizeShort.Production>
<param.HardwareSizeInt.Production>32</param.HardwareSizeInt.Production>
<param.HardwareSizeLong.Production>32</param.HardwareSizeLong.Production>
<param.HardwareSizeLongLong.Production>64</param.HardwareSizeLongLong.Production>
<param.HardwareSizeFloat.Production>32</param.HardwareSizeFloat.Production>
<param.HardwareSizeDouble.Production>64</param.HardwareSizeDouble.Production>
<param.HardwareSizeWord.Production>32</param.HardwareSizeWord.Production>
<param.HardwareSizePointer.Production>32</param.HardwareSizePointer.Production>
<param.HardwareEndianness.Production>option.HardwareEndianness.Little</param.HardwareEndianness.Production>
<param.HardwareArithmeticRightShift.Production>true</param.HardwareArithmeticRightShift.Production>
<param.HardwareLongLongMode.Production>false</param.HardwareLongLongMode.Production>
<param.HardwareAtomicIntegerSize.Production>option.HardwareAtomicIntegerSize.Long</param.HardwareAtomicIntegerSize.Production>
<param.HardwareAtomicFloatSize.Production>option.HardwareAtomicFloatSize.Double</param.HardwareAtomicFloatSize.Production>
<param.HardwareDivisionRounding.Production>option.HardwareDivisionRounding.Undefined</param.HardwareDivisionRounding.Production>
<param.HardwareVendor.Target>Generic</param.HardwareVendor.Target>
<param.HardwareType.Target>MATLAB Host Computer</param.HardwareType.Target>
<var.instance.enabled.Target>false</var.instance.enabled.Target>
<param.HardwareSizeChar.Target>8</param.HardwareSizeChar.Target>
<param.HardwareSizeShort.Target>16</param.HardwareSizeShort.Target>
<param.HardwareSizeInt.Target>32</param.HardwareSizeInt.Target>
<param.HardwareSizeLong.Target>64</param.HardwareSizeLong.Target>
<param.HardwareSizeLongLong.Target>64</param.HardwareSizeLongLong.Target>
<param.HardwareSizeFloat.Target>32</param.HardwareSizeFloat.Target>
<param.HardwareSizeDouble.Target>64</param.HardwareSizeDouble.Target>
<param.HardwareSizeWord.Target>64</param.HardwareSizeWord.Target>
<param.HardwareSizePointer.Target>64</param.HardwareSizePointer.Target>
<param.HardwareEndianness.Target>option.HardwareEndianness.Little</param.HardwareEndianness.Target>
<param.HardwareArithmeticRightShift.Target>true</param.HardwareArithmeticRightShift.Target>
<param.HardwareLongLongMode.Target>true</param.HardwareLongLongMode.Target>
<param.HardwareAtomicIntegerSize.Target>option.HardwareAtomicIntegerSize.Char</param.HardwareAtomicIntegerSize.Target>
<param.HardwareAtomicFloatSize.Target>option.HardwareAtomicFloatSize.None</param.HardwareAtomicFloatSize.Target>
<param.HardwareDivisionRounding.Target>option.HardwareDivisionRounding.Zero</param.HardwareDivisionRounding.Target>
<param.Toolchain>Automatically locate an installed toolchain</param.Toolchain>
<param.BuildConfiguration>Faster Builds</param.BuildConfiguration>
<param.CustomToolchainOptions />
<param.ConstantFoldingTimeout>40000</param.ConstantFoldingTimeout>
<param.RecursionLimit>100</param.RecursionLimit>
<param.IncludeTerminateFcn>true</param.IncludeTerminateFcn>
<param.TargetLang>option.TargetLang.C</param.TargetLang>
<param.CCompilerOptimization>option.CCompilerOptimization.On</param.CCompilerOptimization>
<param.CCompilerCustomOptimizations />
<param.GenerateMakefile>true</param.GenerateMakefile>
<param.BuildToolEnable>false</param.BuildToolEnable>
<param.MakeCommand>make_rtw</param.MakeCommand>
<param.TemplateMakefile>default_tmf</param.TemplateMakefile>
<param.BuildToolConfiguration />
<param.InlineThreshold>10</param.InlineThreshold>
<param.InlineThresholdMax>200</param.InlineThresholdMax>
<param.InlineStackLimit>4000</param.InlineStackLimit>
<param.EnableMemcpy>true</param.EnableMemcpy>
<param.MemcpyThreshold>64</param.MemcpyThreshold>
<param.EnableOpenMP>true</param.EnableOpenMP>
<param.InitFltsAndDblsToZero>true</param.InitFltsAndDblsToZero>
<param.PassStructByReference>false</param.PassStructByReference>
<param.UseECoderFeatures>true</param.UseECoderFeatures>
<unset>
<param.WorkingFolder />
<param.SpecifiedWorkingFolder />
<param.SearchPaths />
<param.SaturateOnIntegerOverflow />
<param.PurelyIntegerCode />
<param.DynamicMemoryAllocation />
<param.DynamicMemoryAllocationThreshold />
<param.MultiInstanceCode />
<param.GenerateComments />
<param.MATLABFcnDesc />
<param.DataTypeReplacement />
<param.ConvertIfToSwitch />
<param.PreserveExternInFcnDecls />
<param.ParenthesesLevel />
<param.MaxIdLength />
<param.CustomSymbolStrGlobalVar />
<param.CustomSymbolStrType />
<param.CustomSymbolStrField />
<param.CustomSymbolStrFcn />
<param.CustomSymbolStrTmpVar />
<param.CustomSymbolStrMacro />
<param.CustomSymbolStrEMXArray />
<param.CustomSymbolStrEMXArrayFcn />
<param.ReservedNameArray />
<param.EnableScreener />
<param.Verbose />
<param.GenerateReport />
<param.GenerateCodeMetricsReport />
<param.GenerateCodeReplacementReport />
<param.CustomInclude />
<param.CustomSource />
<param.CustomLibrary />
<param.SameHardware />
<var.instance.enabled.Production />
<param.HardwareSizeChar.Production />
<param.HardwareSizeShort.Production />
<param.HardwareSizeInt.Production />
<param.HardwareSizeLong.Production />
<param.HardwareSizeLongLong.Production />
<param.HardwareSizeFloat.Production />
<param.HardwareSizeDouble.Production />
<param.HardwareSizeWord.Production />
<param.HardwareSizePointer.Production />
<param.HardwareEndianness.Production />
<param.HardwareLongLongMode.Production />
<param.HardwareDivisionRounding.Production />
<var.instance.enabled.Target />
<param.HardwareSizeChar.Target />
<param.HardwareSizeShort.Target />
<param.HardwareSizeInt.Target />
<param.HardwareSizeLongLong.Target />
<param.HardwareSizeFloat.Target />
<param.HardwareSizeDouble.Target />
<param.HardwareEndianness.Target />
<param.HardwareAtomicFloatSize.Target />
<param.CustomToolchainOptions />
<param.ConstantFoldingTimeout />
<param.RecursionLimit />
<param.IncludeTerminateFcn />
<param.TargetLang />
<param.CCompilerCustomOptimizations />
<param.GenerateMakefile />
<param.BuildToolEnable />
<param.MakeCommand />
<param.TemplateMakefile />
<param.BuildToolConfiguration />
<param.InlineThreshold />
<param.InlineThresholdMax />
<param.InlineStackLimit />
<param.EnableMemcpy />
<param.MemcpyThreshold />
<param.EnableOpenMP />
<param.InitFltsAndDblsToZero />
<param.UseECoderFeatures />
</unset>
</profile>
<param.outputfile>/opt/matlab/r2013b/bin/codegen/codegen/lib/AttitudeEKF/AttitudeEKF.a</param.outputfile>
<param.version>R2012a</param.version>
<param.HasECoderFeatures>true</param.HasECoderFeatures>
<param.mex.mainhtml>t:\private\desktop-dinfk-xp\Attitude_Kalmanfilter\codegen\mex\attitudeKalmanfilter\html\index.html</param.mex.mainhtml>
<param.grt.mainhtml>/home/thomasgubler/src/Firmware/src/modules/attitude_estimator_ekf/codegen/html/index.html</param.grt.mainhtml>
<param.CallGeneratedCodeFromTest>true</param.CallGeneratedCodeFromTest>
<param.VerificationMode>option.VerificationMode.None</param.VerificationMode>
<param.SILDebugging>false</param.SILDebugging>
<param.DefaultTestFile>${PROJECT_ROOT}/AttitudeEKF_Test.m</param.DefaultTestFile>
<param.AutoInferDefaultFile>${PROJECT_ROOT}/AttitudeEKF_Test.m</param.AutoInferDefaultFile>
<param.AutoInferUseVariableSize>false</param.AutoInferUseVariableSize>
<param.AutoInferUseUnboundedSize>false</param.AutoInferUseUnboundedSize>
<param.AutoInferVariableSizeThreshold>1024</param.AutoInferVariableSizeThreshold>
<param.AutoInferUnboundedSizeThreshold>2048</param.AutoInferUnboundedSizeThreshold>
<param.mex.outputfile>AttitudeEKF_mex</param.mex.outputfile>
<param.grt.outputfile>AttitudeEKF</param.grt.outputfile>
<param.artifact>option.target.artifact.lib</param.artifact>
<param.FixedPointTypeProposalMode>option.FixedPointTypeProposalMode.ProposeFractionLengths</param.FixedPointTypeProposalMode>
<param.DefaultProposedFixedPointType>numerictype([],16,12)</param.DefaultProposedFixedPointType>
<param.MinMaxSafetyMargin>0</param.MinMaxSafetyMargin>
<param.OptimizeWholeNumbers>true</param.OptimizeWholeNumbers>
<param.LaunchInstrumentationReport>false</param.LaunchInstrumentationReport>
<param.OpenInstrumentationReportInBrowser>false</param.OpenInstrumentationReportInBrowser>
<param.CreatePrintableInstrumentationReport>false</param.CreatePrintableInstrumentationReport>
<param.EnableAutoExtrinsicCalls>true</param.EnableAutoExtrinsicCalls>
<param.UsePreconditions>false</param.UsePreconditions>
<param.FeatureFlags />
<param.FixedPointMode>option.FixedPointMode.None</param.FixedPointMode>
<param.AutoScaleLoopIndexVariables>false</param.AutoScaleLoopIndexVariables>
<param.ComputedFixedPointData />
<param.UserFixedPointData />
<param.DefaultWordLength>16</param.DefaultWordLength>
<param.DefaultFractionLength>4</param.DefaultFractionLength>
<param.FixedPointSafetyMargin>0</param.FixedPointSafetyMargin>
<param.FixedPointFimath>fimath('RoundingMethod', 'Floor', 'OverflowAction', 'Wrap', 'ProductMode', 'FullPrecision', 'MaxProductWordLength', 128, 'SumMode', 'FullPrecision', 'MaxSumWordLength', 128)</param.FixedPointFimath>
<param.FixedPointTypeSource>option.FixedPointTypeSource.SimAndDerived</param.FixedPointTypeSource>
<param.StaticAnalysisTimeout />
<param.StaticAnalysisGlobalRangesOnly>false</param.StaticAnalysisGlobalRangesOnly>
<param.LogAllIOValues>false</param.LogAllIOValues>
<param.LogHistogram>false</param.LogHistogram>
<param.ShowCoverage>true</param.ShowCoverage>
<param.ExcludedFixedPointVerificationFiles />
<param.ExcludedFixedPointSimulationFiles />
<param.InstrumentedBuildChecksum />
<param.FixedPointStaticAnalysisChecksum />
<param.InstrumentedMexFile />
<param.FixedPointValidationChecksum />
<param.FixedPointSourceCodeChecksum />
<param.FixedPointFunctionReplacements />
<param.OptimizeWholeNumbers>true</param.OptimizeWholeNumbers>
<param.GeneratedFixedPointFileSuffix>_fixpt</param.GeneratedFixedPointFileSuffix>
<param.DefaultFixedPointSignedness>option.DefaultFixedPointSignedness.Automatic</param.DefaultFixedPointSignedness>
<unset>
<param.outputfile />
<param.version />
<param.HasECoderFeatures />
<param.CallGeneratedCodeFromTest />
<param.VerificationMode />
<param.SILDebugging />
<param.AutoInferUseVariableSize />
<param.AutoInferUseUnboundedSize />
<param.AutoInferVariableSizeThreshold />
<param.AutoInferUnboundedSizeThreshold />
<param.mex.outputfile />
<param.grt.outputfile />
<param.FixedPointTypeProposalMode />
<param.DefaultProposedFixedPointType />
<param.MinMaxSafetyMargin />
<param.OptimizeWholeNumbers />
<param.LaunchInstrumentationReport />
<param.OpenInstrumentationReportInBrowser />
<param.CreatePrintableInstrumentationReport />
<param.EnableAutoExtrinsicCalls />
<param.UsePreconditions />
<param.FeatureFlags />
<param.FixedPointMode />
<param.AutoScaleLoopIndexVariables />
<param.ComputedFixedPointData />
<param.UserFixedPointData />
<param.DefaultWordLength />
<param.DefaultFractionLength />
<param.FixedPointSafetyMargin />
<param.FixedPointFimath />
<param.FixedPointTypeSource />
<param.StaticAnalysisTimeout />
<param.StaticAnalysisGlobalRangesOnly />
<param.LogAllIOValues />
<param.LogHistogram />
<param.ShowCoverage />
<param.ExcludedFixedPointVerificationFiles />
<param.ExcludedFixedPointSimulationFiles />
<param.InstrumentedBuildChecksum />
<param.FixedPointStaticAnalysisChecksum />
<param.InstrumentedMexFile />
<param.FixedPointValidationChecksum />
<param.FixedPointSourceCodeChecksum />
<param.FixedPointFunctionReplacements />
<param.GeneratedFixedPointFileSuffix />
<param.DefaultFixedPointSignedness />
</unset>
<fileset.entrypoints>
<file value="${PROJECT_ROOT}/AttitudeEKF.m" custom-data-expanded="true">
<Inputs fileName="AttitudeEKF.m" functionName="AttitudeEKF">
<Input Name="approx_prediction">
<Class>uint8</Class>
<UserDefined>false</UserDefined>
<Size>1 x 1</Size>
<Complex>false</Complex>
</Input>
<Input Name="use_inertia_matrix">
<Class>uint8</Class>
<UserDefined>false</UserDefined>
<Size>1 x 1</Size>
<Complex>false</Complex>
</Input>
<Input Name="zFlag">
<Class>uint8</Class>
<UserDefined>false</UserDefined>
<Size>3 x 1</Size>
<Complex>false</Complex>
</Input>
<Input Name="dt">
<Class>single</Class>
<UserDefined>false</UserDefined>
<Size>1 x 1</Size>
<Complex>false</Complex>
</Input>
<Input Name="z">
<Class>single</Class>
<UserDefined>false</UserDefined>
<Size>9 x 1</Size>
<Complex>false</Complex>
</Input>
<Input Name="q_rotSpeed">
<Class>single</Class>
<UserDefined>false</UserDefined>
<Size>1 x 1</Size>
<Complex>false</Complex>
</Input>
<Input Name="q_rotAcc">
<Class>single</Class>
<UserDefined>false</UserDefined>
<Size>1 x 1</Size>
<Complex>false</Complex>
</Input>
<Input Name="q_acc">
<Class>single</Class>
<UserDefined>false</UserDefined>
<Size>1 x 1</Size>
<Complex>false</Complex>
</Input>
<Input Name="q_mag">
<Class>single</Class>
<UserDefined>false</UserDefined>
<Size>1 x 1</Size>
<Complex>false</Complex>
</Input>
<Input Name="r_gyro">
<Class>single</Class>
<UserDefined>false</UserDefined>
<Size>1 x 1</Size>
<Complex>false</Complex>
</Input>
<Input Name="r_accel">
<Class>single</Class>
<UserDefined>false</UserDefined>
<Size>1 x 1</Size>
<Complex>false</Complex>
</Input>
<Input Name="r_mag">
<Class>single</Class>
<UserDefined>false</UserDefined>
<Size>1 x 1</Size>
<Complex>false</Complex>
</Input>
<Input Name="J">
<Class>single</Class>
<UserDefined>false</UserDefined>
<Size>3 x 3</Size>
<Complex>false</Complex>
</Input>
</Inputs>
</file>
</fileset.entrypoints>
<fileset.testbench>
<file>${PROJECT_ROOT}/AttitudeEKF_Test.m</file>
</fileset.testbench>
<fileset.package />
<build-deliverables>
<file name="AttitudeEKF.a" location="${MATLAB_ROOT}/bin/codegen/codegen/lib/AttitudeEKF" optional="false">/opt/matlab/r2013b/bin/codegen/codegen/lib/AttitudeEKF/AttitudeEKF.a</file>
</build-deliverables>
<workflow />
<matlab>
<root>/opt/matlab/r2013b</root>
<toolboxes>
<toolbox name="fixedpoint" />
</toolboxes>
</matlab>
<platform>
<unix>true</unix>
<mac>false</mac>
<windows>false</windows>
<win2k>false</win2k>
<winxp>false</winxp>
<vista>false</vista>
<linux>true</linux>
<solaris>false</solaris>
<osver>3.16.1-1-ARCH</osver>
<os32>false</os32>
<os64>true</os64>
<arch>glnxa64</arch>
<matlab>true</matlab>
</platform>
</configuration>
</deployment-project>
src/examples/attitude_estimator_ekf/attitude_estimator_ekf_main.cpp
Executable
+657
View File
@@ -0,0 +1,657 @@
/****************************************************************************
*
* 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 <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/vision_position_estimate.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/systemlib.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 *)NULL);
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(vision_position_estimate));
/* 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 vision_position_estimate_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(vision_position_estimate), 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.roll = euler[0];
att.pitch = euler[1];
att.yaw = euler[2] + mag_decl;
att.rollspeed = x_aposteriori[0];
att.pitchspeed = x_aposteriori[1];
att.yawspeed = x_aposteriori[2];
att.rollacc = x_aposteriori[3];
att.pitchacc = x_aposteriori[4];
att.yawacc = x_aposteriori[5];
att.g_comp[0] = raw.accelerometer_m_s2[0] - acc(0);
att.g_comp[1] = raw.accelerometer_m_s2[1] - acc(1);
att.g_comp[2] = raw.accelerometer_m_s2[2] - acc(2);
/* copy offsets */
memcpy(&att.rate_offsets, &(x_aposteriori[3]), sizeof(att.rate_offsets));
/* magnetic declination */
math::Matrix<3, 3> R_body = (&Rot_matrix[0]);
R = R_decl * R_body;
math::Quaternion q;
q.from_dcm(R);
/* copy rotation matrix */
memcpy(&att.R[0], &R.data[0][0], sizeof(att.R));
memcpy(&att.q[0],&q.data[0],sizeof(att.q));
att.R_valid = true;
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 {
warnx("ERR: NaN estimate!");
}
perf_end(ekf_loop_perf);
}
}
}
loopcounter++;
}
thread_running = false;
return 0;
}
src/examples/attitude_estimator_ekf/attitude_estimator_ekf_params.c
Executable
+130
View File
@@ -0,0 +1,130 @@
/****************************************************************************
*
* 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);
src/examples/attitude_estimator_ekf/attitude_estimator_ekf_params.h
Executable
+75
View File
@@ -0,0 +1,75 @@
/****************************************************************************
*
* 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);
src/examples/attitude_estimator_ekf/codegen/AttitudeEKF.c
+1456
View File
File diff suppressed because it is too large Load Diff
src/examples/attitude_estimator_ekf/codegen/AttitudeEKF.h
+26
View File
@@ -0,0 +1,26 @@
/*
* AttitudeEKF.h
*
* Code generation for function 'AttitudeEKF'
*
* C source code generated on: Thu Aug 21 11:17:28 2014
*
*/
#ifndef __ATTITUDEEKF_H__
#define __ATTITUDEEKF_H__
/* Include files */
#include <math.h>
#include <stddef.h>
#include <stdlib.h>
#include <string.h>
#include "rtwtypes.h"
#include "AttitudeEKF_types.h"
/* Function Declarations */
extern void AttitudeEKF(unsigned char approx_prediction, unsigned char use_inertia_matrix, const unsigned char zFlag[3], float dt, const float z[9], float q_rotSpeed, float q_rotAcc, float q_acc, float q_mag, float r_gyro, float r_accel, float r_mag, const float J[9], float xa_apo[12], float Pa_apo[144], float Rot_matrix[9], float eulerAngles[3], float debugOutput[4]);
extern void AttitudeEKF_initialize(void);
extern void AttitudeEKF_terminate(void);
#endif
/* End of code generation (AttitudeEKF.h) */
src/examples/attitude_estimator_ekf/codegen/AttitudeEKF_types.h
+17
View File
@@ -0,0 +1,17 @@
/*
* AttitudeEKF_types.h
*
* Code generation for function 'AttitudeEKF'
*
* C source code generated on: Thu Aug 21 11:17:28 2014
*
*/
#ifndef __ATTITUDEEKF_TYPES_H__
#define __ATTITUDEEKF_TYPES_H__
/* Include files */
#include "rtwtypes.h"
#endif
/* End of code generation (AttitudeEKF_types.h) */
src/examples/attitude_estimator_ekf/codegen/rtwtypes.h
+160
View File
@@ -0,0 +1,160 @@
/*
* 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) */
src/examples/ekf_att_pos_estimator/AttitudePositionEstimatorEKF.h
+379
View File
@@ -0,0 +1,379 @@
/****************************************************************************
*
* Copyright (c) 2013-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.
*
****************************************************************************/
/**
* @file AttitudePositionEstimatorEKF.h
* Implementation of the attitude and position estimator. This is a PX4 wrapper around
* the EKF IntertialNav filter of Paul Riseborough (@see estimator_22states.cpp)
*
* @author Paul Riseborough <p_riseborough@live.com.au>
* @author Lorenz Meier <lm@inf.ethz.ch>
* @author Johan Jansen <jnsn.johan@gmail.com>
*/
#pragma once
#include <uORB/uORB.h>
#include <uORB/topics/airspeed.h>
#include <uORB/topics/vehicle_global_position.h>
#include <uORB/topics/vehicle_local_position.h>
#include <uORB/topics/vehicle_gps_position.h>
#include <uORB/topics/vehicle_attitude.h>
#include <uORB/topics/control_state.h>
#include <uORB/topics/vehicle_status.h>
#include <uORB/topics/vehicle_land_detected.h>
#include <uORB/topics/actuator_controls.h>
#include <uORB/topics/estimator_status.h>
#include <uORB/topics/actuator_armed.h>
#include <uORB/topics/home_position.h>
#include <uORB/topics/wind_estimate.h>
#include <uORB/topics/sensor_combined.h>
#include <uORB/topics/distance_sensor.h>
#include <drivers/drv_hrt.h>
#include <drivers/drv_gyro.h>
#include <drivers/drv_accel.h>
#include <drivers/drv_mag.h>
#include <drivers/drv_baro.h>
#include <mathlib/math/filter/LowPassFilter2p.hpp>
#include <geo/geo.h>
#include <terrain_estimation/terrain_estimator.h>
#include <systemlib/perf_counter.h>
#include "estimator_22states.h"
#include <controllib/blocks.hpp>
#include <controllib/block/BlockParam.hpp>
//Forward declaration
class AttPosEKF;
class AttitudePositionEstimatorEKF : public control::SuperBlock
{
public:
/**
* Constructor
*/
AttitudePositionEstimatorEKF();
/* we do not want people ever copying this class */
AttitudePositionEstimatorEKF(const AttitudePositionEstimatorEKF& that) = delete;
AttitudePositionEstimatorEKF operator=(const AttitudePositionEstimatorEKF&) = delete;
/**
* Destructor, also kills the sensors task.
*/
~AttitudePositionEstimatorEKF();
/**
* Start the sensors task.
*
* @return OK on success.
*/
int start();
/**
* Task status
*
* @return true if the mainloop is running
*/
bool task_running() { return _task_running; }
/**
* Print the current status.
*/
void print_status();
/**
* Trip the filter by feeding it NaN values.
*/
int trip_nan();
/**
* Enable logging.
*
* @param enable Set to true to enable logging, false to disable
*/
int enable_logging(bool enable);
/**
* Set debug level.
*
* @param debug Desired debug level - 0 to disable.
*/
int set_debuglevel(unsigned debug) { _debug = debug; return 0; }
static constexpr unsigned MAX_PREDICITION_STEPS = 3; /**< maximum number of prediction steps between updates */
private:
bool _task_should_exit; /**< if true, sensor task should exit */
bool _task_running; /**< if true, task is running in its mainloop */
int _estimator_task; /**< task handle for sensor task */
int _sensor_combined_sub;
int _distance_sub; /**< distance measurement */
int _airspeed_sub; /**< airspeed subscription */
int _baro_sub; /**< barometer subscription */
int _gps_sub; /**< GPS subscription */
int _vehicle_status_sub;
int _vehicle_land_detected_sub;
int _params_sub; /**< notification of parameter updates */
int _manual_control_sub; /**< notification of manual control updates */
int _mission_sub;
int _home_sub; /**< home position as defined by commander / user */
int _landDetectorSub;
int _armedSub;
orb_advert_t _att_pub; /**< vehicle attitude */
orb_advert_t _ctrl_state_pub; /**< control state */
orb_advert_t _global_pos_pub; /**< global position */
orb_advert_t _local_pos_pub; /**< position in local frame */
orb_advert_t _estimator_status_pub; /**< status of the estimator */
orb_advert_t _wind_pub; /**< wind estimate */
struct vehicle_attitude_s _att; /**< vehicle attitude */
struct control_state_s _ctrl_state; /**< control state */
struct gyro_report _gyro;
struct accel_report _accel;
struct mag_report _mag;
struct airspeed_s _airspeed; /**< airspeed */
struct baro_report _baro; /**< baro readings */
struct vehicle_status_s _vehicle_status;
struct vehicle_land_detected_s _vehicle_land_detected;
struct vehicle_global_position_s _global_pos; /**< global vehicle position */
struct vehicle_local_position_s _local_pos; /**< local vehicle position */
struct vehicle_gps_position_s _gps; /**< GPS position */
struct wind_estimate_s _wind; /**< wind estimate */
struct distance_sensor_s _distance; /**< distance estimate */
struct actuator_armed_s _armed;
hrt_abstime _last_accel;
hrt_abstime _last_mag;
unsigned _prediction_steps;
uint64_t _prediction_last;
struct sensor_combined_s _sensor_combined;
struct map_projection_reference_s _pos_ref;
float _filter_ref_offset; /**< offset between initial baro reference and GPS init baro altitude */
float _baro_gps_offset; /**< offset between baro altitude (at GPS init time) and GPS altitude */
hrt_abstime _last_debug_print = 0;
perf_counter_t _loop_perf; ///< loop performance counter
perf_counter_t _loop_intvl; ///< loop rate counter
perf_counter_t _perf_gyro; ///<local performance counter for gyro updates
perf_counter_t _perf_mag; ///<local performance counter for mag updates
perf_counter_t _perf_gps; ///<local performance counter for gps updates
perf_counter_t _perf_baro; ///<local performance counter for baro updates
perf_counter_t _perf_airspeed; ///<local performance counter for airspeed updates
perf_counter_t _perf_reset; ///<local performance counter for filter resets
float _gps_alt_filt;
float _baro_alt_filt;
float _covariancePredictionDt; ///< time lapsed since last covariance prediction
bool _gpsIsGood; ///< True if the current GPS fix is good enough for us to use
uint64_t _previousGPSTimestamp; ///< Timestamp of last good GPS fix we have received
bool _baro_init;
bool _gps_initialized;
hrt_abstime _filter_start_time;
hrt_abstime _last_sensor_timestamp;
hrt_abstime _distance_last_valid;
bool _data_good; ///< all required filter data is ok
bool _ekf_logging; ///< log EKF state
unsigned _debug; ///< debug level - default 0
bool _was_landed; ///< indicates if was landed in previous iteration
bool _newHgtData;
bool _newAdsData;
bool _newDataMag;
bool _newRangeData;
orb_advert_t _mavlink_log_pub;
control::BlockParamFloat _mag_offset_x;
control::BlockParamFloat _mag_offset_y;
control::BlockParamFloat _mag_offset_z;
struct {
int32_t vel_delay_ms;
int32_t pos_delay_ms;
int32_t height_delay_ms;
int32_t mag_delay_ms;
int32_t tas_delay_ms;
float velne_noise;
float veld_noise;
float posne_noise;
float posd_noise;
float mag_noise;
float gyro_pnoise;
float acc_pnoise;
float gbias_pnoise;
float abias_pnoise;
float mage_pnoise;
float magb_pnoise;
float eas_noise;
float pos_stddev_threshold;
int32_t airspeed_mode;
} _parameters; /**< local copies of interesting parameters */
struct {
param_t vel_delay_ms;
param_t pos_delay_ms;
param_t height_delay_ms;
param_t mag_delay_ms;
param_t tas_delay_ms;
param_t velne_noise;
param_t veld_noise;
param_t posne_noise;
param_t posd_noise;
param_t mag_noise;
param_t gyro_pnoise;
param_t acc_pnoise;
param_t gbias_pnoise;
param_t abias_pnoise;
param_t mage_pnoise;
param_t magb_pnoise;
param_t eas_noise;
param_t pos_stddev_threshold;
param_t airspeed_mode;
} _parameter_handles; /**< handles for interesting parameters */
AttPosEKF *_ekf;
TerrainEstimator *_terrain_estimator;
/* Low pass filter for attitude rates */
math::LowPassFilter2p _LP_att_P;
math::LowPassFilter2p _LP_att_Q;
math::LowPassFilter2p _LP_att_R;
private:
/**
* Update our local parameter cache.
*/
int parameters_update();
/**
* Update control outputs
*
*/
void control_update();
/**
* Check for changes in land detected.
*/
void vehicle_status_poll();
/**
* Check for changes in land detected.
*/
void vehicle_land_detected_poll();
/**
* Shim for calling task_main from task_create.
*/
static void task_main_trampoline(int argc, char *argv[]);
/**
* Main filter task.
*/
void task_main();
/**
* Check filter sanity state
*
* @return zero if ok, non-zero for a filter error condition.
*/
int check_filter_state();
/**
* @brief
* Publish the euler and quaternions for attitude estimation
**/
void publishAttitude();
/**
* @brief
* Publish the system state for control modules
**/
void publishControlState();
/**
* @brief
* Publish local position relative to reference point where filter was initialized
**/
void publishLocalPosition();
/**
* @brief
* Publish global position estimation (WSG84 and AMSL).
* A global position estimate is only valid if we have a good GPS fix
**/
void publishGlobalPosition();
/**
* @brief
* Publish wind estimates for north and east in m/s
**/
void publishWindEstimate();
/**
* @brief
* Runs the sensor fusion step of the filter. The parameters determine which of the sensors
* are fused with each other
**/
void updateSensorFusion(const bool fuseGPS, const bool fuseMag, const bool fuseRangeSensor,
const bool fuseBaro, const bool fuseAirSpeed);
/**
* @brief
* Initialize first time good GPS fix so we can get a reference point to calculate local position from
* Should only be required to call once
**/
void initializeGPS();
/**
* Initialize the reference position for the local coordinate frame
*/
void initReferencePosition(hrt_abstime timestamp, bool gps_valid,
double lat, double lon, float gps_alt, float baro_alt);
/**
* @brief
* Polls all uORB subscriptions if any new sensor data has been publish and sets the appropriate
* flags to true (e.g newDataGps)
**/
void pollData();
};
src/examples/ekf_att_pos_estimator/CMakeLists.txt
+47
View File
@@ -0,0 +1,47 @@
############################################################################
#
# 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 modules__ekf_att_pos_estimator
MAIN ekf_att_pos_estimator
COMPILE_FLAGS -Os
STACK_MAIN 3000
STACK_MAX 3400
SRCS
ekf_att_pos_estimator_main.cpp
estimator_22states.cpp
estimator_utilities.cpp
DEPENDS
platforms__common
)
# vim: set noet ft=cmake fenc=utf-8 ff=unix :
src/examples/ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp
+1747
View File
File diff suppressed because it is too large Load Diff
src/examples/ekf_att_pos_estimator/ekf_att_pos_estimator_params.c
+307
View File
@@ -0,0 +1,307 @@
/****************************************************************************
*
* Copyright (c) 2013, 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 ekf_att_pos_estimator_params.c
*
* Parameters defined by the attitude and position estimator task
*
* @author Lorenz Meier <lorenz@px4.io>
*/
/*
* Estimator parameters, accessible via MAVLink
*
*/
/**
* Velocity estimate delay
*
* The delay in milliseconds of the velocity estimate from GPS.
*
* @unit ms
* @min 0
* @max 1000
* @group Position Estimator
*/
PARAM_DEFINE_INT32(PE_VEL_DELAY_MS, 230);
/**
* Position estimate delay
*
* The delay in milliseconds of the position estimate from GPS.
*
* @unit ms
* @min 0
* @max 1000
* @group Position Estimator
*/
PARAM_DEFINE_INT32(PE_POS_DELAY_MS, 210);
/**
* Height estimate delay
*
* The delay in milliseconds of the height estimate from the barometer.
*
* @unit ms
* @min 0
* @max 1000
* @group Position Estimator
*/
PARAM_DEFINE_INT32(PE_HGT_DELAY_MS, 350);
/**
* Mag estimate delay
*
* The delay in milliseconds of the magnetic field estimate from
* the magnetometer.
*
* @unit ms
* @min 0
* @max 1000
* @group Position Estimator
*/
PARAM_DEFINE_INT32(PE_MAG_DELAY_MS, 30);
/**
* True airspeeed estimate delay
*
* The delay in milliseconds of the airspeed estimate.
*
* @unit ms
* @min 0
* @max 1000
* @group Position Estimator
*/
PARAM_DEFINE_INT32(PE_TAS_DELAY_MS, 210);
/**
* GPS vs. barometric altitude update weight
*
* RE-CHECK this.
* @min 0.0
* @max 1.0
* @group Position Estimator
*/
PARAM_DEFINE_FLOAT(PE_GPS_ALT_WGT, 0.9f);
/**
* Airspeed measurement noise.
*
* Increasing this value will make the filter trust this sensor
* less and trust other sensors more.
*
* @min 0.5
* @max 5.0
* @group Position Estimator
*/
PARAM_DEFINE_FLOAT(PE_EAS_NOISE, 1.4f);
/**
* Velocity measurement noise in north-east (horizontal) direction.
*
* Generic default: 0.3, multicopters: 0.5, ground vehicles: 0.5
*
* @min 0.05
* @max 5.0
* @group Position Estimator
*/
PARAM_DEFINE_FLOAT(PE_VELNE_NOISE, 0.3f);
/**
* Velocity noise in down (vertical) direction
*
* Generic default: 0.3, multicopters: 0.4, ground vehicles: 0.7
*
* @min 0.2
* @max 3.0
* @group Position Estimator
*/
PARAM_DEFINE_FLOAT(PE_VELD_NOISE, 0.3f);
/**
* Position noise in north-east (horizontal) direction
*
* Generic defaults: 0.5, multicopters: 0.5, ground vehicles: 0.5
*
* @min 0.1
* @max 10.0
* @group Position Estimator
*/
PARAM_DEFINE_FLOAT(PE_POSNE_NOISE, 0.5f);
/**
* Position noise in down (vertical) direction
*
* Generic defaults: 1.25, multicopters: 1.0, ground vehicles: 1.0
*
* @min 0.5
* @max 3.0
* @group Position Estimator
*/
PARAM_DEFINE_FLOAT(PE_POSD_NOISE, 1.25f);
/**
* Magnetometer measurement noise
*
* Generic defaults: 0.05, multicopters: 0.05, ground vehicles: 0.05
*
* @min 0.01
* @max 1.0
* @group Position Estimator
*/
PARAM_DEFINE_FLOAT(PE_MAG_NOISE, 0.05f);
/**
* Gyro process noise
*
* Generic defaults: 0.015, multicopters: 0.015, ground vehicles: 0.015.
* This noise controls how much the filter trusts the gyro measurements.
* Increasing it makes the filter trust the gyro less and other sensors more.
*
* @min 0.001
* @max 0.05
* @group Position Estimator
*/
PARAM_DEFINE_FLOAT(PE_GYRO_PNOISE, 0.015f);
/**
* Accelerometer process noise
*
* Generic defaults: 0.25, multicopters: 0.25, ground vehicles: 0.25.
* Increasing this value makes the filter trust the accelerometer less
* and other sensors more.
*
* @min 0.05
* @max 1.0
* @group Position Estimator
*/
PARAM_DEFINE_FLOAT(PE_ACC_PNOISE, 0.125f);
/**
* Gyro bias estimate process noise
*
* Generic defaults: 1e-07f, multicopters: 1e-07f, ground vehicles: 1e-07f.
* Increasing this value will make the gyro bias converge faster but noisier.
*
* @min 0.00000005
* @max 0.00001
* @group Position Estimator
*/
PARAM_DEFINE_FLOAT(PE_GBIAS_PNOISE, 1e-07f);
/**
* Accelerometer bias estimate process noise
*
* Generic defaults: 0.00001f, multicopters: 0.00001f, ground vehicles: 0.00001f.
* Increasing this value makes the bias estimation faster and noisier.
*
* @min 0.00001
* @max 0.001
* @group Position Estimator
*/
PARAM_DEFINE_FLOAT(PE_ABIAS_PNOISE, 1e-05f);
/**
* Magnetometer earth frame offsets process noise
*
* Generic defaults: 0.0001, multicopters: 0.0001, ground vehicles: 0.0001.
* Increasing this value makes the magnetometer earth bias estimate converge
* faster but also noisier.
*
* @min 0.0001
* @max 0.01
* @group Position Estimator
*/
PARAM_DEFINE_FLOAT(PE_MAGE_PNOISE, 0.0003f);
/**
* Magnetometer body frame offsets process noise
*
* Generic defaults: 0.0003, multicopters: 0.0003, ground vehicles: 0.0003.
* Increasing this value makes the magnetometer body bias estimate converge faster
* but also noisier.
*
* @min 0.0001
* @max 0.01
* @group Position Estimator
*/
PARAM_DEFINE_FLOAT(PE_MAGB_PNOISE, 0.0003f);
/**
* Magnetometer X bias
*
* The magnetometer bias. This bias is learnt by the filter over time and
* persists between boots.
*
* @min -0.6
* @max 0.6
* @group Position Estimator
*/
PARAM_DEFINE_FLOAT(PE_MAGB_X, 0.0f);
/**
* Magnetometer Y bias
*
* The magnetometer bias. This bias is learnt by the filter over time and
* persists between boots.
*
* @min -0.6
* @max 0.6
* @group Position Estimator
*/
PARAM_DEFINE_FLOAT(PE_MAGB_Y, 0.0f);
/**
* Magnetometer Z bias
*
* The magnetometer bias. This bias is learnt by the filter over time and
* persists between boots.
*
* @min -0.6
* @max 0.6
* @group Position Estimator
*/
PARAM_DEFINE_FLOAT(PE_MAGB_Z, 0.0f);
/**
* Threshold for filter initialization.
*
* If the standard deviation of the GPS position estimate is below this threshold
* in meters, the filter will initialize.
*
* @min 0.3
* @max 10.0
* @group Position Estimator
*/
PARAM_DEFINE_FLOAT(PE_POSDEV_INIT, 5.0f);
src/examples/ekf_att_pos_estimator/estimator_22states.cpp
+3346
View File
File diff suppressed because one or more lines are too long
src/examples/ekf_att_pos_estimator/estimator_22states.h
+424
View File
@@ -0,0 +1,424 @@
/****************************************************************************
* Copyright (c) 2014, Paul Riseborough All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions are met:
*
* Redistributions of source code must retain the above copyright notice, this
* list of conditions and the following disclaimer.
*
* 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.
*
* Neither the name of the {organization} 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 HOLDER 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 estimator_22states.h
*
* Definition of the attitude and position estimator.
*
* @author Paul Riseborough <p_riseborough@live.com.au>
* @author Lorenz Meier <lorenz@px4.io>
*/
#pragma once
#include "estimator_utilities.h"
#include <cstddef>
constexpr size_t EKF_STATE_ESTIMATES = 22;
constexpr size_t EKF_DATA_BUFFER_SIZE = 50;
class AttPosEKF {
public:
AttPosEKF();
~AttPosEKF();
/* ##############################################
*
* M A I N F I L T E R P A R A M E T E R S
*
* ########################################### */
/*
* parameters are defined here and initialised in
* the InitialiseParameters()
*/
float covTimeStepMax; // maximum time allowed between covariance predictions
float covDelAngMax; // maximum delta angle between covariance predictions
float rngFinderPitch; // pitch angle of laser range finder in radians. Zero is aligned with the Z body axis. Positive is RH rotation about Y body axis.
float yawVarScale;
float windVelSigma;
float dAngBiasSigma;
float dVelBiasSigma;
float magEarthSigma;
float magBodySigma;
float gndHgtSigma;
float vneSigma;
float vdSigma;
float posNeSigma;
float posDSigma;
float magMeasurementSigma;
float airspeedMeasurementSigma;
float gyroProcessNoise;
float accelProcessNoise;
float EAS2TAS; // ratio f true to equivalent airspeed
struct mag_state_struct {
unsigned obsIndex;
float MagPred[3];
float SH_MAG[9];
float q0;
float q1;
float q2;
float q3;
float magN;
float magE;
float magD;
float magXbias;
float magYbias;
float magZbias;
float R_MAG;
Mat3f DCM;
};
struct mag_state_struct magstate;
struct mag_state_struct resetMagState;
// Global variables
float KH[EKF_STATE_ESTIMATES][EKF_STATE_ESTIMATES]; // intermediate result used for covariance updates
float KHP[EKF_STATE_ESTIMATES][EKF_STATE_ESTIMATES]; // intermediate result used for covariance updates
float P[EKF_STATE_ESTIMATES][EKF_STATE_ESTIMATES]; // covariance matrix
float Kfusion[EKF_STATE_ESTIMATES]; // Kalman gains
float states[EKF_STATE_ESTIMATES]; // state matrix
float resetStates[EKF_STATE_ESTIMATES];
float storedStates[EKF_STATE_ESTIMATES][EKF_DATA_BUFFER_SIZE]; // state vectors stored for the last 50 time steps
uint32_t statetimeStamp[EKF_DATA_BUFFER_SIZE]; // time stamp for each state vector stored
// Times
uint64_t lastVelPosFusion; // the time of the last velocity fusion, in the standard time unit of the filter
float statesAtVelTime[EKF_STATE_ESTIMATES]; // States at the effective measurement time for posNE and velNED measurements
float statesAtPosTime[EKF_STATE_ESTIMATES]; // States at the effective measurement time for posNE and velNED measurements
float statesAtHgtTime[EKF_STATE_ESTIMATES]; // States at the effective measurement time for the hgtMea measurement
float statesAtMagMeasTime[EKF_STATE_ESTIMATES]; // filter satates at the effective measurement time
float statesAtVtasMeasTime[EKF_STATE_ESTIMATES]; // filter states at the effective measurement time
float statesAtRngTime[EKF_STATE_ESTIMATES]; // filter states at the effective measurement time
float statesAtFlowTime[EKF_STATE_ESTIMATES]; // States at the effective optical flow measurement time
float omegaAcrossFlowTime[3]; // angular rates at the effective optical flow measurement time
Vector3f correctedDelAng; // delta angles about the xyz body axes corrected for errors (rad)
Vector3f correctedDelVel; // delta velocities along the XYZ body axes corrected for errors (m/s)
Vector3f summedDelAng; // summed delta angles about the xyz body axes corrected for errors (rad)
Vector3f summedDelVel; // summed delta velocities along the XYZ body axes corrected for errors (m/s)
Vector3f prevDelAng; ///< previous delta angle, used for coning correction
float accNavMag; // magnitude of navigation accel (- used to adjust GPS obs variance (m/s^2)
Vector3f earthRateNED; // earths angular rate vector in NED (rad/s)
Vector3f angRate; // angular rate vector in XYZ body axes measured by the IMU (rad/s)
Vector3f lastGyroOffset; // Last gyro offset
Vector3f delAngTotal;
Mat3f Tbn; // transformation matrix from body to NED coordinatesFuseOptFlow
Mat3f Tnb; // transformation amtrix from NED to body coordinates
Vector3f accel; // acceleration vector in XYZ body axes measured by the IMU (m/s^2)
Vector3f dVelIMU;
Vector3f dAngIMU;
float dtIMU; // time lapsed since the last IMU measurement or covariance update (sec), this may have significant jitter
float dtIMUfilt; // average time between IMU measurements (sec)
float dtVelPos; // time lapsed since the last position / velocity fusion (seconds), this may have significant jitter
float dtVelPosFilt; // average time between position / velocity fusion steps
float dtHgtFilt; // average time between height measurement updates
float dtGpsFilt; // average time between gps measurement updates
uint8_t fusionModeGPS; // 0 = GPS outputs 3D velocity, 1 = GPS outputs 2D velocity, 2 = GPS outputs no velocity
float innovVelPos[6]; // innovation output
float varInnovVelPos[6]; // innovation variance output
float velNED[3]; // North, East, Down velocity obs (m/s)
float posNE[2]; // North, East position obs (m)
float hgtMea; // measured height (m)
float baroHgtOffset; ///< the baro (weather) offset from normalized altitude
float rngMea; // Ground distance
float innovMag[3]; // innovation output
float varInnovMag[3]; // innovation variance output
Vector3f magData; // magnetometer flux radings in X,Y,Z body axes
float flowRadXYcomp[2]; // motion compensated optical flow angular rates(rad/sec)
float flowRadXY[2]; // raw (non motion compensated) optical flow angular rates (rad/sec)
float innovVtas; // innovation output
float innovRng; ///< Range finder innovation
float innovOptFlow[2]; // optical flow LOS innovations (rad/sec)
float varInnovOptFlow[2]; // optical flow innovations variances (rad/sec)^2
float varInnovVtas; // innovation variance output
float varInnovRng; // range finder innovation variance
float VtasMeas; // true airspeed measurement (m/s)
float magDeclination; ///< magnetic declination
double latRef; // WGS-84 latitude of reference point (rad)
double lonRef; // WGS-84 longitude of reference point (rad)
float hgtRef; // WGS-84 height of reference point (m)
bool refSet; ///< flag to indicate if the reference position has been set
Vector3f magBias; // states representing magnetometer bias vector in XYZ body axes
unsigned covSkipCount; // Number of state prediction frames (IMU daya updates to skip before doing the covariance prediction
uint32_t lastFixTime_ms; // Number of msec since last GPS Fix that was used
uint32_t globalTimeStamp_ms; // time in msec of current prediction cycle
// GPS input data variables
double gpsLat;
double gpsLon;
float gpsHgt;
uint8_t GPSstatus;
// Baro input
float baroHgt;
bool statesInitialised;
bool fuseVelData; // this boolean causes the posNE and velNED obs to be fused
bool fusePosData; // this boolean causes the posNE and velNED obs to be fused
bool fuseHgtData; // this boolean causes the hgtMea obs to be fused
bool fuseMagData; // boolean true when magnetometer data is to be fused
bool fuseVtasData; // boolean true when airspeed data is to be fused
bool fuseRngData; ///< true when range data is fused
bool fuseOptFlowData; // true when optical flow data is fused
bool inhibitWindStates; // true when wind states and covariances are to remain constant
bool inhibitMagStates; // true when magnetic field states and covariances are to remain constant
bool inhibitGndState; // true when the terrain ground height offset state and covariances are to remain constant
bool inhibitScaleState; // true when the focal length scale factor state and covariances are to remain constant
bool staticMode; ///< boolean true if no position feedback is fused
bool useGPS; // boolean true if GPS data is being used
bool useAirspeed; ///< boolean true if airspeed data is being used
bool useCompass; ///< boolean true if magnetometer data is being used
bool useRangeFinder; ///< true when rangefinder is being used
bool useOpticalFlow; // true when optical flow data is being used
bool ekfDiverged;
uint64_t lastReset;
struct ekf_status_report current_ekf_state;
struct ekf_status_report last_ekf_error;
bool numericalProtection;
unsigned storeIndex;
// Optical Flow error estimation
float storedOmega[3][EKF_DATA_BUFFER_SIZE]; // angular rate vector stored for the last 50 time steps used by optical flow eror estimators
// Two state EKF used to estimate focal length scale factor and terrain position
float Popt[2][2]; // state covariance matrix
float flowStates[2]; // flow states [scale factor, terrain position]
float prevPosN; // north position at last measurement
float prevPosE; // east position at last measurement
float auxFlowObsInnov[2]; // optical flow observation innovations from focal length scale factor estimator
float auxFlowObsInnovVar[2]; // innovation variance for optical flow observations from focal length scale factor estimator
float fScaleFactorVar; // optical flow sensor focal length scale factor variance
Mat3f Tnb_flow; // Transformation matrix from nav to body at the time fo the optical flow measurement
float R_LOS; // Optical flow observation noise variance (rad/sec)^2
float auxFlowTestRatio[2]; // ratio of X and Y flow observation innovations to fault threshold
float auxRngTestRatio; // ratio of range observation innovations to fault threshold
float flowInnovGate; // number of standard deviations used for the innovation consistency check
float auxFlowInnovGate; // number of standard deviations applied to the optical flow innovation consistency check
float rngInnovGate; // number of standard deviations used for the innovation consistency check
float minFlowRng; // minimum range over which to fuse optical flow measurements
float moCompR_LOS; // scaler from sensor gyro rate to uncertainty in LOS rate
void updateDtGpsFilt(float dt);
void updateDtHgtFilt(float dt);
void UpdateStrapdownEquationsNED();
void CovariancePrediction(float dt);
void FuseVelposNED();
void FuseMagnetometer();
void FuseAirspeed();
void FuseOptFlow();
/**
* @brief
* Estimation of optical flow sensor focal length scale factor and terrain height using a two state EKF
* This fiter requires optical flow rates that are not motion compensated
* Range to ground measurement is assumed to be via a narrow beam type sensor - eg laser
**/
void OpticalFlowEKF();
void zeroRows(float (&covMat)[EKF_STATE_ESTIMATES][EKF_STATE_ESTIMATES], uint8_t first, uint8_t last);
void zeroCols(float (&covMat)[EKF_STATE_ESTIMATES][EKF_STATE_ESTIMATES], uint8_t first, uint8_t last);
void quatNorm(float (&quatOut)[4], const float quatIn[4]);
// store staes along with system time stamp in msces
void StoreStates(uint64_t timestamp_ms);
/**
* Recall the state vector.
*
* Recalls the vector stored at closest time to the one specified by msec
* @return zero on success, integer indicating the number of invalid states on failure.
* Does only copy valid states, if the statesForFusion vector was initialized
* correctly by the caller, the result can be safely used, but is a mixture
* time-wise where valid states were updated and invalid remained at the old
* value.
*/
int RecallStates(float *statesForFusion, uint64_t msec);
void RecallOmega(float *omegaForFusion, uint64_t msec);
void quat2Tbn(Mat3f &TBodyNed, const float (&quat)[4]);
void calcEarthRateNED(Vector3f &omega, float latitude);
static void eul2quat(float (&quat)[4], const float (&eul)[3]);
static void quat2eul(float (&eul)[3], const float (&quat)[4]);
//static void quat2Tnb(Mat3f &Tnb, const float (&quat)[4]);
static inline float sq(float valIn) {return valIn * valIn;}
/**
* @brief
* Tell the EKF if the vehicle has landed
**/
void setOnGround(const bool isLanded);
void CovarianceInit();
void InitialiseFilter(float (&initvelNED)[3], double referenceLat, double referenceLon, float referenceHgt, float declination);
float ConstrainFloat(float val, float min, float maxf);
void ConstrainVariances();
void ConstrainStates();
void ForceSymmetry();
/**
* @brief
* Check the filter inputs and bound its operational state
*
* This check will reset the filter states if required
* due to a failure of consistency or timeout checks.
* it should be run after the measurement data has been
* updated, but before any of the fusion steps are
* executed.
*/
int CheckAndBound(struct ekf_status_report *last_error);
/**
* @brief
* Reset the filter position states
*
* This resets the position to the last GPS measurement
* or to zero in case of static position.
*/
void ResetPosition();
/**
* @brief
* Reset the velocity state.
*/
void ResetVelocity();
void GetFilterState(struct ekf_status_report *state);
void GetLastErrorState(struct ekf_status_report *last_error);
bool StatesNaN();
void InitializeDynamic(float (&initvelNED)[3], float declination);
/**
* @brief
* Tells the EKF wheter the vehicle is a fixed wing frame or not.
* This changes the way the EKF fuses position or attitude calulations
* by making some assumptions on movement.
* @param fixedWing
* true if the vehicle moves like a Fixed Wing, false otherwise
**/
void setIsFixedWing(const bool fixedWing);
/**
* @brief
* Reset internal filter states and clear all variables to zero value
*/
void ZeroVariables();
void get_covariance(float c[28]);
float getAccNavMagHorizontal() { return _accNavMagHorizontal; }
protected:
/**
* @brief
* Initializes algorithm parameters to safe default values
**/
void InitialiseParameters();
void updateDtVelPosFilt(float dt);
bool FilterHealthy();
bool GyroOffsetsDiverged();
bool VelNEDDiverged();
/**
* @brief
* Reset the height state.
*
* This resets the height state with the last altitude measurements
*/
void ResetHeight();
void AttitudeInit(float ax, float ay, float az, float mx, float my, float mz, float declination, float *initQuat);
void ResetStoredStates();
private:
bool _isFixedWing; ///< True if the vehicle is a fixed-wing frame type
bool _onGround; ///< boolean true when the flight vehicle is on the ground (not flying)
float _accNavMagHorizontal; ///< First-order low-pass filtered rate of change maneuver velocity
};
uint32_t millis();
uint64_t getMicros();
src/examples/ekf_att_pos_estimator/estimator_utilities.cpp
+234
View File
@@ -0,0 +1,234 @@
/****************************************************************************
* Copyright (c) 2014, Paul Riseborough All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions are met:
*
* Redistributions of source code must retain the above copyright notice, this
* list of conditions and the following disclaimer.
*
* 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.
*
* Neither the name of the {organization} 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 HOLDER 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 estimator_utilities.cpp
*
* Estimator support utilities.
*
* @author Paul Riseborough <p_riseborough@live.com.au>
* @author Lorenz Meier <lorenz@px4.io>
*/
#include "estimator_utilities.h"
// Define EKF_DEBUG here to enable the debug print calls
// if the macro is not set, these will be completely
// optimized out by the compiler.
//#define EKF_DEBUG
#ifdef EKF_DEBUG
#include <stdio.h>
#include <stdarg.h>
static void
ekf_debug_print(const char *fmt, va_list args)
{
fprintf(stderr, "%s: ", "[ekf]");
vfprintf(stderr, fmt, args);
fprintf(stderr, "\n");
}
void
ekf_debug(const char *fmt, ...)
{
va_list args;
va_start(args, fmt);
ekf_debug_print(fmt, args);
}
#else
void ekf_debug(const char *fmt, ...) { while(0){} }
#endif
/* we don't want to pull in the standard lib just to swap two floats */
void swap_var(float &d1, float &d2);
float Vector3f::length(void) const
{
return sqrt(x*x + y*y + z*z);
}
void Vector3f::zero(void)
{
x = 0.0f;
y = 0.0f;
z = 0.0f;
}
Mat3f::Mat3f() :
x{1.0f, 0.0f, 0.0f},
y{0.0f, 1.0f, 0.0f},
z{0.0f, 0.0f, 1.0f}
{
}
void Mat3f::identity() {
x.x = 1.0f;
x.y = 0.0f;
x.z = 0.0f;
y.x = 0.0f;
y.y = 1.0f;
y.z = 0.0f;
z.x = 0.0f;
z.y = 0.0f;
z.z = 1.0f;
}
Mat3f Mat3f::transpose() const
{
Mat3f ret = *this;
swap_var(ret.x.y, ret.y.x);
swap_var(ret.x.z, ret.z.x);
swap_var(ret.y.z, ret.z.y);
return ret;
}
void calcvelNED(float (&velNEDr)[3], float gpsCourse, float gpsGndSpd, float gpsVelD)
{
velNEDr[0] = gpsGndSpd*cosf(gpsCourse);
velNEDr[1] = gpsGndSpd*sinf(gpsCourse);
velNEDr[2] = gpsVelD;
}
void calcposNED(float (&posNEDr)[3], double lat, double lon, float hgt, double latReference, double lonReference, float hgtReference)
{
posNEDr[0] = earthRadius * (lat - latReference);
posNEDr[1] = earthRadius * cos(latReference) * (lon - lonReference);
posNEDr[2] = -(hgt - hgtReference);
}
void calcLLH(float posNEDi[3], double &lat, double &lon, float &hgt, double latRef, double lonRef, float hgtRef)
{
lat = latRef + (double)posNEDi[0] * earthRadiusInv;
lon = lonRef + (double)posNEDi[1] * earthRadiusInv / cos(latRef);
hgt = hgtRef - posNEDi[2];
}
// overload + operator to provide a vector addition
Vector3f operator+(const Vector3f &vecIn1, const Vector3f &vecIn2)
{
Vector3f vecOut;
vecOut.x = vecIn1.x + vecIn2.x;
vecOut.y = vecIn1.y + vecIn2.y;
vecOut.z = vecIn1.z + vecIn2.z;
return vecOut;
}
// overload - operator to provide a vector subtraction
Vector3f operator-(const Vector3f &vecIn1, const Vector3f &vecIn2)
{
Vector3f vecOut;
vecOut.x = vecIn1.x - vecIn2.x;
vecOut.y = vecIn1.y - vecIn2.y;
vecOut.z = vecIn1.z - vecIn2.z;
return vecOut;
}
// overload * operator to provide a matrix vector product
Vector3f operator*(const Mat3f &matIn, const Vector3f &vecIn)
{
Vector3f vecOut;
vecOut.x = matIn.x.x*vecIn.x + matIn.x.y*vecIn.y + matIn.x.z*vecIn.z;
vecOut.y = matIn.y.x*vecIn.x + matIn.y.y*vecIn.y + matIn.y.z*vecIn.z;
vecOut.z = matIn.z.x*vecIn.x + matIn.z.y*vecIn.y + matIn.z.z*vecIn.z;
return vecOut;
}
// overload * operator to provide a matrix product
Mat3f operator*(const Mat3f &matIn1, const Mat3f &matIn2)
{
Mat3f matOut;
matOut.x.x = matIn1.x.x*matIn2.x.x + matIn1.x.y*matIn2.y.x + matIn1.x.z*matIn2.z.x;
matOut.x.y = matIn1.x.x*matIn2.x.y + matIn1.x.y*matIn2.y.y + matIn1.x.z*matIn2.z.y;
matOut.x.z = matIn1.x.x*matIn2.x.z + matIn1.x.y*matIn2.y.z + matIn1.x.z*matIn2.z.z;
matOut.y.x = matIn1.y.x*matIn2.x.x + matIn1.y.y*matIn2.y.x + matIn1.y.z*matIn2.z.x;
matOut.y.y = matIn1.y.x*matIn2.x.y + matIn1.y.y*matIn2.y.y + matIn1.y.z*matIn2.z.y;
matOut.y.z = matIn1.y.x*matIn2.x.z + matIn1.y.y*matIn2.y.z + matIn1.y.z*matIn2.z.z;
matOut.z.x = matIn1.z.x*matIn2.x.x + matIn1.z.y*matIn2.y.x + matIn1.z.z*matIn2.z.x;
matOut.z.y = matIn1.z.x*matIn2.x.y + matIn1.z.y*matIn2.y.y + matIn1.z.z*matIn2.z.y;
matOut.z.z = matIn1.z.x*matIn2.x.z + matIn1.z.y*matIn2.y.z + matIn1.z.z*matIn2.z.z;
return matOut;
}
// overload % operator to provide a vector cross product
Vector3f operator%(const Vector3f &vecIn1, const Vector3f &vecIn2)
{
Vector3f vecOut;
vecOut.x = vecIn1.y*vecIn2.z - vecIn1.z*vecIn2.y;
vecOut.y = vecIn1.z*vecIn2.x - vecIn1.x*vecIn2.z;
vecOut.z = vecIn1.x*vecIn2.y - vecIn1.y*vecIn2.x;
return vecOut;
}
// overload * operator to provide a vector scaler product
Vector3f operator*(const Vector3f &vecIn1, const float sclIn1)
{
Vector3f vecOut;
vecOut.x = vecIn1.x * sclIn1;
vecOut.y = vecIn1.y * sclIn1;
vecOut.z = vecIn1.z * sclIn1;
return vecOut;
}
// overload * operator to provide a vector scaler product
Vector3f operator*(float sclIn1, const Vector3f &vecIn1)
{
Vector3f vecOut;
vecOut.x = vecIn1.x * sclIn1;
vecOut.y = vecIn1.y * sclIn1;
vecOut.z = vecIn1.z * sclIn1;
return vecOut;
}
// overload / operator to provide a vector scalar division
Vector3f operator/(const Vector3f &vec, const float scalar)
{
Vector3f vecOut;
vecOut.x = vec.x / scalar;
vecOut.y = vec.y / scalar;
vecOut.z = vec.z / scalar;
return vecOut;
}
void swap_var(float &d1, float &d2)
{
float tmp = d1;
d1 = d2;
d2 = tmp;
}
src/examples/ekf_att_pos_estimator/estimator_utilities.h
+135
View File
@@ -0,0 +1,135 @@
/****************************************************************************
* Copyright (c) 2014, Paul Riseborough All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions are met:
*
* Redistributions of source code must retain the above copyright notice, this
* list of conditions and the following disclaimer.
*
* 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.
*
* Neither the name of the {organization} 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 HOLDER 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 estimator_utilities.h
*
* Estimator support utilities.
*
* @author Paul Riseborough <p_riseborough@live.com.au>
* @author Lorenz Meier <lorenz@px4.io>
*/
#pragma once
#include <math.h>
#include <stdint.h>
#define GRAVITY_MSS 9.80665f
#define deg2rad 0.017453292f
#define rad2deg 57.295780f
#define earthRate 0.000072921f
#define earthRadius 6378145.0
#define earthRadiusInv 1.5678540e-7
class Vector3f
{
public:
float x;
float y;
float z;
Vector3f(float a=0.0f, float b=0.0f, float c=0.0f) :
x(a),
y(b),
z(c)
{}
float length() const;
void zero();
};
class Mat3f
{
private:
public:
Vector3f x;
Vector3f y;
Vector3f z;
Mat3f();
void identity();
Mat3f transpose() const;
};
Vector3f operator*(const float sclIn1, const Vector3f &vecIn1);
Vector3f operator+(const Vector3f &vecIn1, const Vector3f &vecIn2);
Vector3f operator-(const Vector3f &vecIn1, const Vector3f &vecIn2);
Vector3f operator*(const Mat3f &matIn, const Vector3f &vecIn);
Mat3f operator*(const Mat3f &matIn1, const Mat3f &matIn2);
Vector3f operator%(const Vector3f &vecIn1, const Vector3f &vecIn2);
Vector3f operator*(const Vector3f &vecIn1, const float sclIn1);
Vector3f operator/(const Vector3f &vec, const float scalar);
enum GPS_FIX {
GPS_FIX_NOFIX = 0,
GPS_FIX_2D = 2,
GPS_FIX_3D = 3
};
struct ekf_status_report {
bool error;
bool velHealth;
bool posHealth;
bool hgtHealth;
bool velTimeout;
bool posTimeout;
bool hgtTimeout;
bool imuTimeout;
bool onGround;
bool staticMode;
bool useCompass;
bool useAirspeed;
uint32_t velFailTime;
uint32_t posFailTime;
uint32_t hgtFailTime;
float states[32];
unsigned n_states;
bool angNaN;
bool summedDelVelNaN;
bool KHNaN;
bool KHPNaN;
bool PNaN;
bool covarianceNaN;
bool kalmanGainsNaN;
bool statesNaN;
bool gyroOffsetsExcessive;
bool covariancesExcessive;
bool velOffsetExcessive;
};
void ekf_debug(const char *fmt, ...);
void calcvelNED(float (&velNEDr)[3], float gpsCourse, float gpsGndSpd, float gpsVelD);
void calcposNED(float (&posNEDr)[3], double lat, double lon, float hgt, double latReference, double lonReference, float hgtReference);
void calcLLH(float posNEDi[3], double &lat, double &lon, float &hgt, double latRef, double lonRef, float hgtRef);