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

EKF: Update magnetometer fusion derivations

Browse Source
This commit is contained in:
Paul Riseborough
2016-02-20 20:09:15 +11:00
parent a711632017
commit 70a3b91217
3 changed files with 107 additions and 37 deletions
Download Patch File Download Diff File Expand all files Collapse all files
matlab/generated/Inertial Nav EKF/Simple Magnetometer Fusion.txt
+46 -21
View File
@@ -1,32 +1,57 @@
/*
Autocode for fusion of a yaw error measurement where the innovation is given by:
Observation jacobian for fusion of the horizontal components of magnetometer measurements.
innovation = atan2f(magMeasEarthFrameEast,magMeasEarthFrameNorth) - declinationAngle;
innovation = atan2(magE/magN) - declination, where magN and magE are the North and East components obtained
by rotating the measured magnetometer readings from body into earth axes.
magMeasEarthFrameEast and magMeasEarthFrameNorth are obtained by rotating the magnetometer measurements from body frame to eath frame.
declinationAngle is the estimated declination as that location
This method of fusion reduces roll and pitch errors due to external field disturbances and is suitable for initial alignment and ground / indoor use
Protection against /0 and covariance matrix errors will need to be added.
Divide by zero protection hs been added
*/
// intermediate variables
// calculate intermediate variables for observation jacobian
float t2 = q0*q0;
float t3 = q1*q1;
float t4 = q2*q2;
float t5 = q3*q3;
float t6 = t2+t3-t4-t5;
float t7 = q0*q3*2.0f;
float t8 = q1*q2*2.0f;
float t9 = t7+t8;
float t10 = 1.0f/(t6*t6);
float t11 = t9*t9;
float t12 = t10*t11;
float t13 = t12+1.0f;
float t14 = 1.0f/t13;
float t15 = 1.0f/t6;
float t6 = q0*q3*2.0f;
float t8 = t2-t3+t4-t5;
float t9 = q0*q1*2.0f;
float t10 = q2*q3*2.0f;
float t11 = t9-t10;
float t14 = q1*q2*2.0f;
float t21 = magY*t8;
float t22 = t6+t14;
float t23 = magX*t22;
float t24 = magZ*t11;
float t7 = t21+t23-t24;
float t12 = t2+t3-t4-t5;
float t13 = magX*t12;
float t15 = q0*q2*2.0f;
float t16 = q1*q3*2.0f;
float t17 = t15+t16;
float t18 = magZ*t17;
float t19 = t6-t14;
float t25 = magY*t19;
float t20 = t13+t18-t25;
if (fabsf(t20) < 1e-6f) {
return;
}
float t26 = 1.0f/(t20*t20);
float t27 = t7*t7;
float t28 = t26*t27;
float t29 = t28+1.0;
if (fabsf(t29) < 1e-12f) {
return;
}
float t30 = 1.0f/t29;
if (fabsf(t20) < 1e-12f) {
return;
}
float t31 = 1.0f/t20;
// Calculate the observation jacobian for the innovation derivative wrt the attitude error states only
// Use the reduced order to optimise the calculation of the Kalman gain matrix and covariance update
float H_MAG[3];
H_YAW[1] = t14*(t15*(q0*q1*2.0f-q2*q3*2.0f)+t9*t10*(q0*q2*2.0f+q1*q3*2.0f));
H_YAW[2] = t14*(t15*(t2-t3+t4-t5)+t9*t10*(t7-t8));
// calculate observation jacobian
float H_DECL[3] = {};
H_DECL[0] = -t30*(t31*(magZ*t8+magY*t11)+t7*t26*(magY*t17+magZ*t19));
H_DECL[1] = t30*(t31*(magX*t11+magZ*t22)-t7*t26*(magZ*t12-magX*t17));
H_DECL[2] = t30*(t31*(magX*t8-magY*t22)+t7*t26*(magY*t12+magX*t19));
matlab/generated/Inertial Nav EKF/Yaw Angle Fusion.txt
+27
View File
@@ -0,0 +1,27 @@
/*
Autocode for fusion of an Euler yaw measurement.
Protection against /0 and covariance matrix errors will need to be added.
*/
// intermediate variables
float t2 = q0*q0;
float t3 = q1*q1;
float t4 = q2*q2;
float t5 = q3*q3;
float t6 = t2+t3-t4-t5;
float t7 = q0*q3*2.0f;
float t8 = q1*q2*2.0f;
float t9 = t7+t8;
float t10 = 1.0f/(t6*t6);
float t11 = t9*t9;
float t12 = t10*t11;
float t13 = t12+1.0f;
float t14 = 1.0f/t13;
float t15 = 1.0f/t6;
// Calculate the observation jacobian for the innovation derivative wrt the attitude error states only
// Use the reduced order to optimise the calculation of the Kalman gain matrix and covariance update
float H_MAG[3];
H_YAW[1] = t14*(t15*(q0*q1*2.0f-q2*q3*2.0f)+t9*t10*(q0*q2*2.0f+q1*q3*2.0f));
H_YAW[2] = t14*(t15*(t2-t3+t4-t5)+t9*t10*(t7-t8));
matlab/scripts/Inertial Nav EKF/GenerateNavFilterEquations.m
+34 -16
View File
@@ -1,6 +1,6 @@
% IMPORTANT - This script requires the Matlab symbolic toolbox and takes ~3 hours to run
% Derivation of Navigation EKF using a local NED earth Tangent Frame and
% Derivation of Navigation EKF using a local NED earth Tangent Frame and
% XYZ body fixed frame
% Sequential fusion of velocity and position measurements
% Fusion of true airspeed
@@ -15,8 +15,8 @@
% Based on use of a rotation vector for attitude estimation as described
% here:
% Mark E. Pittelkau. "Rotation Vector in Attitude Estimation",
% Journal of Guidance, Control, and Dynamics, Vol. 26, No. 6 (2003),
% Mark E. Pittelkau. "Rotation Vector in Attitude Estimation",
% Journal of Guidance, Control, and Dynamics, Vol. 26, No. 6 (2003),
% pp. 855-860.
% State vector:
@@ -68,8 +68,7 @@ syms R_LOS real % variance of LOS angular rate mesurements (rad/sec)^2
syms ptd real % location of terrain in D axis
syms rotErrX rotErrY rotErrZ real; % error rotation vector in body frame
syms decl real; % earth magnetic field declination from true north
syms R_YAW real; % variance for magnetic deviation measurement
syms R_DECL real; % variance of supplied declination
syms R_DECL R_YAW real; % variance of declination or yaw angle observation
syms BCXinv BCYinv real % inverse of ballistic coefficient for wind relative movement along the x and y body axes
syms rho real % air density (kg/m^3)
syms R_ACC real % variance of accelerometer measurements (m/s^2)^2
@@ -102,7 +101,7 @@ truthQuat = QuatMult(estQuat, errQuat);
Tbn = Quat2Tbn(truthQuat);
% define the truth delta angle
% ignore coning compensation as these effects are negligible in terms of
% ignore coning compensation as these effects are negligible in terms of
% covariance growth for our application and grade of sensor
dAngTruth = dAngMeas.*dAngScale - dAngBias - [daxNoise;dayNoise;dazNoise];
@@ -189,10 +188,10 @@ F = jacobian(newStateVector, stateVector);
F = subs(F, {'rotErrX', 'rotErrY', 'rotErrZ'}, {0,0,0});
[F,SF]=OptimiseAlgebra(F,'SF');
% define a symbolic covariance matrix using strings to represent
% define a symbolic covariance matrix using strings to represent
% '_l_' to represent '( '
% '_c_' to represent ,
% '_r_' to represent ')'
% '_r_' to represent ')'
% these can be substituted later to create executable code
for rowIndex = 1:nStates
for colIndex = 1:nStates
@@ -264,12 +263,33 @@ K_LOS = [K_LOSX,K_LOSY];
simplify(K_LOS);
[K_LOS,SK_LOS]=OptimiseAlgebra(K_LOS,'SK_LOS');
% Use matlab c code converter for an alternate method of
% Use matlab c code converter for an alternate method of
ccode(H_LOS,'file','H_LOS.txt');
ccode(K_LOSX,'file','K_LOSX.txt');
ccode(K_LOSY,'file','K_LOSY.txt');
%% derive equations for fusion of direct yaw measurement
%% derive equations for simple fusion of 2-D magnetic heading measurements
% rotate magnetic field into earth axes
magMeasNED = Tbn*[magX;magY;magZ];
% the predicted measurement is the angle wrt true north of the horizontal
% component of the measured field
angMeas = atan(magMeasNED(2)/magMeasNED(1));
simpleStateVector = [errRotVec;vn;ve;vd;pn;pe;pd;dax_b;day_b;daz_b;dax_s;day_s;daz_s;dvz_b];
Psimple = P(1:16,1:16);
H_MAGS = jacobian(angMeas,simpleStateVector); % measurement Jacobian
%H_MAGS = H_MAGS(1:3);
H_MAGS = subs(H_MAGS, {'rotErrX', 'rotErrY', 'rotErrZ'}, {0,0,0});
%H_MAGS = simplify(H_MAGS);
%[H_MAGS,SH_MAGS]=OptimiseAlgebra(H_MAGS,'SH_MAGS');
ccode(H_MAGS,'file','calcH_MAGS.c');
% Calculate Kalman gain vector
K_MAGS = (Psimple*transpose(H_MAGS))/(H_MAGS*Psimple*transpose(H_MAGS) + R_DECL);
%K_MAGS = simplify(K_MAGS);
%[K_MAGS,SK_MAGS]=OptimiseAlgebra(K_MAGS,'SK_MAGS');
ccode(K_MAGS,'file','calcK_MAGS.c');
%% derive equations for fusion of yaw measurements
% rotate X body axis into earth axes
yawVec = Tbn*[1;0;0];
@@ -298,7 +318,7 @@ H_MAGD = jacobian(angMeas,stateVector); % measurement Jacobian
H_MAGD = subs(H_MAGD, {'rotErrX', 'rotErrY', 'rotErrZ'}, {0,0,0});
H_MAGD = simplify(H_MAGD);
%[H_MAGD,SH_MAGD]=OptimiseAlgebra(H_MAGD,'SH_MAGD');
%ccode(H_MAGD,'file','calcH_MAGD.c');
ccode(H_MAGD,'file','calcH_MAGD.c');
% Calculate Kalman gain vector
K_MAGD = (P*transpose(H_MAGD))/(H_MAGD*P*transpose(H_MAGD) + R_DECL);
ccode([H_MAGD',K_MAGD],'file','calcMAGD.c');
@@ -319,7 +339,7 @@ vrel = transpose(Tbn)*[(vn-vwn);(ve-vwe);vd]; % predicted wind relative velocity
% accYpred = -0.5*rho*vrel(2)*vrel(2)*BCYinv; % predicted acceleration measured along Y body axis
% Use a simple viscous drag model for the linear estimator equations
% Use the the derivative from speed to acceleration averaged across the
% Use the the derivative from speed to acceleration averaged across the
% speed range
% The nonlinear equation will be used to calculate the predicted
% measurement in implementation
@@ -339,14 +359,12 @@ H_ACCY = jacobian(accYpred,stateVector); % measurement Jacobian
H_ACCY = subs(H_ACCY, {'rotErrX', 'rotErrY', 'rotErrZ'}, {0,0,0});
[H_ACCY,SH_ACCY]=OptimiseAlgebra(H_ACCY,'SH_ACCY'); % optimise processing
K_ACCY = (P*transpose(H_ACCY))/(H_ACCY*P*transpose(H_ACCY) + R_ACC);
ccode([H_ACCY',K_ACCY],'file','calcACCY.c');
[K_ACCY,SK_ACCY]=OptimiseAlgebra(K_ACCY,'SK_ACCY'); % Kalman gain vector
% generate c code for jacobians using the matlab code generator
ccode([H_ACCX;H_ACCY],'file','calcH_ACCXY.c');
%% Save output and convert to m and c code fragments
fileName = strcat('SymbolicOutput',int2str(nStates),'.mat');
save(fileName);
SaveScriptCode(nStates);
ConvertToM(nStates);
ConvertToC(nStates);
ConvertToC(nStates);