mirror of
https://gitee.com/mirrors_PX4/PX4-Autopilot.git
synced 2026-10-03 14:08:52 +08:00
EKF: Derive equations enabling yaw variance to be increased
This commit is contained in:
Paul Riseborough
committed by
Daniel Agar
Daniel Agar
parent
81eabc1903
commit
bf1f3a224e
@@ -0,0 +1,35 @@
|
|||||||
|
float SF[9][1];
|
||||||
|
SF[0] = 0;
|
||||||
|
SF[1] = 0;
|
||||||
|
SF[2] = 0;
|
||||||
|
SF[3] = -SF[2];
|
||||||
|
SF[4] = SF[2];
|
||||||
|
SF[5] = 0;
|
||||||
|
SF[6] = 0;
|
||||||
|
SF[7] = sq(q3);
|
||||||
|
SF[8] = sq(q2);
|
||||||
|
float SG[3][1];
|
||||||
|
SG[0] = sq(q0) - sq(q1) - sq(q2) + sq(q3);
|
||||||
|
SG[1] = 2*q0*q2 - 2*q1*q3;
|
||||||
|
SG[2] = 2*q0*q1 + 2*q2*q3;
|
||||||
|
float SQ[4][1];
|
||||||
|
SQ[0] = (q1*SG[0])/2 - (q0*SG[2])/2 + (q3*SG[1])/2;
|
||||||
|
SQ[1] = (q0*SG[1])/2 - (q2*SG[0])/2 + (q3*SG[2])/2;
|
||||||
|
SQ[2] = (q3*SG[0])/2 - (q1*SG[1])/2 + (q2*SG[2])/2;
|
||||||
|
SQ[3] = (q0*SG[0])/2 + (q1*SG[2])/2 + (q2*SG[1])/2;
|
||||||
|
float SPP[4][1];
|
||||||
|
SPP[0] = SF[5] - SF[0] + SF[6];
|
||||||
|
SPP[1] = SF[0] - SF[5];
|
||||||
|
SPP[2] = SF[0] + SF[5] + SF[6];
|
||||||
|
SPP[3] = SF[1];
|
||||||
|
float nextP[4][4];
|
||||||
|
nextP[0][0] = P[0][0] + P[2][0]*SF[3] + P[1][0]*SPP[3] + P[3][0]*SPP[1] + SF[3]*(P[0][2] + P[2][2]*SF[3] + P[1][2]*SPP[3] + P[3][2]*SPP[1]) + SPP[3]*(P[0][1] + P[2][1]*SF[3] + P[1][1]*SPP[3] + P[3][1]*SPP[1]) + SPP[1]*(P[0][3] + P[2][3]*SF[3] + P[1][3]*SPP[3] + P[3][3]*SPP[1]) + daYawVar*sq(SQ[2]);
|
||||||
|
nextP[0][1] = P[0][1] + P[2][1]*SF[3] + P[1][1]*SPP[3] + P[3][1]*SPP[1] - (SF[0] + SF[6])*(P[0][2] + P[2][2]*SF[3] + P[1][2]*SPP[3] + P[3][2]*SPP[1]) + SF[3]*(P[0][3] + P[2][3]*SF[3] + P[1][3]*SPP[3] + P[3][3]*SPP[1]) - SPP[3]*(P[0][0] + P[2][0]*SF[3] + P[1][0]*SPP[3] + P[3][0]*SPP[1]) + daYawVar*SQ[1]*SQ[2];
|
||||||
|
nextP[1][1] = P[1][1] + P[3][1]*SF[3] - P[0][1]*SPP[3] + SF[3]*(P[1][3] + P[3][3]*SF[3] - P[0][3]*SPP[3] - P[2][3]*(SF[0] + SF[6])) - SPP[3]*(P[1][0] + P[3][0]*SF[3] - P[0][0]*SPP[3] - P[2][0]*(SF[0] + SF[6])) + daYawVar*sq(SQ[1]) - P[2][1]*(SF[0] + SF[6]) - (SF[0] + SF[6])*(P[1][2] + P[3][2]*SF[3] - P[0][2]*SPP[3] - P[2][2]*(SF[0] + SF[6]));
|
||||||
|
nextP[0][2] = P[0][2] + P[2][2]*SF[3] + P[1][2]*SPP[3] + P[3][2]*SPP[1] + SF[4]*(P[0][0] + P[2][0]*SF[3] + P[1][0]*SPP[3] + P[3][0]*SPP[1]) + SPP[2]*(P[0][1] + P[2][1]*SF[3] + P[1][1]*SPP[3] + P[3][1]*SPP[1]) - SPP[3]*(P[0][3] + P[2][3]*SF[3] + P[1][3]*SPP[3] + P[3][3]*SPP[1]) + daYawVar*SQ[0]*SQ[2];
|
||||||
|
nextP[1][2] = P[1][2] + P[3][2]*SF[3] - P[0][2]*SPP[3] + SF[4]*(P[1][0] + P[3][0]*SF[3] - P[0][0]*SPP[3] - P[2][0]*(SF[0] + SF[6])) + SPP[2]*(P[1][1] + P[3][1]*SF[3] - P[0][1]*SPP[3] - P[2][1]*(SF[0] + SF[6])) - SPP[3]*(P[1][3] + P[3][3]*SF[3] - P[0][3]*SPP[3] - P[2][3]*(SF[0] + SF[6])) - P[2][2]*(SF[0] + SF[6]) + daYawVar*SQ[0]*SQ[1];
|
||||||
|
nextP[2][2] = P[2][2] + P[0][2]*SF[4] + P[1][2]*SPP[2] - P[3][2]*SPP[3] + SF[4]*(P[2][0] + P[0][0]*SF[4] + P[1][0]*SPP[2] - P[3][0]*SPP[3]) + SPP[2]*(P[2][1] + P[0][1]*SF[4] + P[1][1]*SPP[2] - P[3][1]*SPP[3]) - SPP[3]*(P[2][3] + P[0][3]*SF[4] + P[1][3]*SPP[2] - P[3][3]*SPP[3]) + daYawVar*sq(SQ[0]);
|
||||||
|
nextP[0][3] = P[0][3] + P[2][3]*SF[3] + P[1][3]*SPP[3] + P[3][3]*SPP[1] + SF[4]*(P[0][1] + P[2][1]*SF[3] + P[1][1]*SPP[3] + P[3][1]*SPP[1]) + SPP[0]*(P[0][0] + P[2][0]*SF[3] + P[1][0]*SPP[3] + P[3][0]*SPP[1]) + SPP[3]*(P[0][2] + P[2][2]*SF[3] + P[1][2]*SPP[3] + P[3][2]*SPP[1]) - daYawVar*SQ[2]*SQ[3];
|
||||||
|
nextP[1][3] = P[1][3] + P[3][3]*SF[3] - P[0][3]*SPP[3] + SF[4]*(P[1][1] + P[3][1]*SF[3] - P[0][1]*SPP[3] - P[2][1]*(SF[0] + SF[6])) + SPP[0]*(P[1][0] + P[3][0]*SF[3] - P[0][0]*SPP[3] - P[2][0]*(SF[0] + SF[6])) + SPP[3]*(P[1][2] + P[3][2]*SF[3] - P[0][2]*SPP[3] - P[2][2]*(SF[0] + SF[6])) - P[2][3]*(SF[0] + SF[6]) - daYawVar*SQ[1]*SQ[3];
|
||||||
|
nextP[2][3] = P[2][3] + P[0][3]*SF[4] + P[1][3]*SPP[2] - P[3][3]*SPP[3] + SF[4]*(P[2][1] + P[0][1]*SF[4] + P[1][1]*SPP[2] - P[3][1]*SPP[3]) + SPP[0]*(P[2][0] + P[0][0]*SF[4] + P[1][0]*SPP[2] - P[3][0]*SPP[3]) + SPP[3]*(P[2][2] + P[0][2]*SF[4] + P[1][2]*SPP[2] - P[3][2]*SPP[3]) - daYawVar*SQ[0]*SQ[3];
|
||||||
|
nextP[3][3] = P[3][3] + P[1][3]*SF[4] + P[0][3]*SPP[0] + P[2][3]*SPP[3] + SF[4]*(P[3][1] + P[1][1]*SF[4] + P[0][1]*SPP[0] + P[2][1]*SPP[3]) + SPP[0]*(P[3][0] + P[1][0]*SF[4] + P[0][0]*SPP[0] + P[2][0]*SPP[3]) + SPP[3]*(P[3][2] + P[1][2]*SF[4] + P[0][2]*SPP[0] + P[2][2]*SPP[3]) + daYawVar*sq(SQ[3]);
|
||||||
@@ -0,0 +1,106 @@
|
|||||||
|
% IMPORTANT - This script requires the Matlab symbolic toolbox
|
||||||
|
|
||||||
|
% Derivation of quaterion covariance prediction for a rotation about the
|
||||||
|
% earth frame Z axis and starting at an arbitary orientation. This 4x4
|
||||||
|
% matrix can be used to add an additional
|
||||||
|
|
||||||
|
% Author: Paul Riseborough
|
||||||
|
|
||||||
|
%% define symbolic variables and constants
|
||||||
|
clear all;
|
||||||
|
reset(symengine);
|
||||||
|
syms q0 q1 q2 q3 real % quaternions defining attitude of body axes relative to local NED
|
||||||
|
syms daYaw real % earth frame yaw delta angle - rad
|
||||||
|
syms daYawVar real; % earth frame yaw delta angle variance - rad^2
|
||||||
|
|
||||||
|
%% define the state prediction equations
|
||||||
|
|
||||||
|
% define the quaternion rotation vector for the state estimate
|
||||||
|
quat = [q0;q1;q2;q3];
|
||||||
|
|
||||||
|
% derive the truth body to nav direction cosine matrix
|
||||||
|
Tbn = Quat2Tbn(quat);
|
||||||
|
|
||||||
|
% define the yaw rotation delta angle in body frame
|
||||||
|
dAngMeas = transpose(Tbn) * [0; 0; daYaw];
|
||||||
|
|
||||||
|
% define the attitude update equations
|
||||||
|
% use a first order expansion of rotation to calculate the quaternion increment
|
||||||
|
% acceptable for propagation of covariances
|
||||||
|
deltaQuat = [1;
|
||||||
|
0.5*dAngMeas(1);
|
||||||
|
0.5*dAngMeas(2);
|
||||||
|
0.5*dAngMeas(3);
|
||||||
|
];
|
||||||
|
quatNew = QuatMult(quat,deltaQuat);
|
||||||
|
|
||||||
|
% Define the state vector & number of states
|
||||||
|
stateVector = quat;
|
||||||
|
nStates=numel(stateVector);
|
||||||
|
|
||||||
|
% Define vector of process equations
|
||||||
|
newStateVector = quatNew;
|
||||||
|
|
||||||
|
% derive the state transition matrix
|
||||||
|
F = jacobian(newStateVector, stateVector);
|
||||||
|
% set the rotation error states to zero
|
||||||
|
[F,SF]=OptimiseAlgebra(F,'SF');
|
||||||
|
|
||||||
|
% define a symbolic covariance matrix using strings to represent
|
||||||
|
% '_l_' to represent '( '
|
||||||
|
% '_c_' to represent ,
|
||||||
|
% '_r_' to represent ')'
|
||||||
|
% these can be substituted later to create executable code
|
||||||
|
for rowIndex = 1:nStates
|
||||||
|
for colIndex = 1:nStates
|
||||||
|
eval(['syms OP_l_',num2str(rowIndex),'_c_',num2str(colIndex), '_r_ real']);
|
||||||
|
eval(['P(',num2str(rowIndex),',',num2str(colIndex), ') = OP_l_',num2str(rowIndex),'_c_',num2str(colIndex),'_r_;']);
|
||||||
|
end
|
||||||
|
end
|
||||||
|
|
||||||
|
save 'StatePrediction.mat';
|
||||||
|
|
||||||
|
%% derive the covariance prediction equations
|
||||||
|
% This reduces the number of floating point operations by a factor of 6 or
|
||||||
|
% more compared to using the standard matrix operations in code
|
||||||
|
|
||||||
|
% Error growth in the inertial solution is assumed to be driven by 'noise' in the delta angles and
|
||||||
|
% velocities, after bias effects have been removed.
|
||||||
|
|
||||||
|
% derive the control(disturbance) influence matrix from IMU noise to state
|
||||||
|
% noise
|
||||||
|
G = jacobian(newStateVector, daYaw);
|
||||||
|
[G,SG]=OptimiseAlgebra(G,'SG');
|
||||||
|
|
||||||
|
% derive the state error matrix
|
||||||
|
distMatrix = diag(daYawVar);
|
||||||
|
Q = G*distMatrix*transpose(G);
|
||||||
|
[Q,SQ]=OptimiseAlgebra(Q,'SQ');
|
||||||
|
|
||||||
|
% set the yaw delta angle to zero - we only needed it to determine the error
|
||||||
|
% propagation
|
||||||
|
SF = subs(SF, daYaw, 0);
|
||||||
|
SG = subs(SG, daYaw, 0);
|
||||||
|
SQ = subs(SQ, daYaw, 0);
|
||||||
|
|
||||||
|
% Derive the predicted covariance matrix using the standard equation
|
||||||
|
PP = F*P*transpose(F) + Q;
|
||||||
|
PP = subs(PP, daYaw, 0);
|
||||||
|
|
||||||
|
% Collect common expressions to optimise processing
|
||||||
|
[PP,SPP]=OptimiseAlgebra(PP,'SPP');
|
||||||
|
|
||||||
|
save('StateAndCovariancePrediction.mat');
|
||||||
|
clear all;
|
||||||
|
reset(symengine);
|
||||||
|
|
||||||
|
%% Save output and convert to m and c code fragments
|
||||||
|
|
||||||
|
% load equations for predictions and updates
|
||||||
|
load('StateAndCovariancePrediction.mat');
|
||||||
|
|
||||||
|
fileName = strcat('SymbolicOutput',int2str(nStates),'.mat');
|
||||||
|
save(fileName);
|
||||||
|
SaveScriptCode(nStates);
|
||||||
|
ConvertToM(nStates);
|
||||||
|
ConvertToC(nStates);
|
||||||
@@ -0,0 +1,34 @@
|
|||||||
|
/*
|
||||||
|
C code fragment for function that enables the yaw uncertainty to be increased following a yaw reset.
|
||||||
|
The variables q0 -> q3 are the attitude quaternions
|
||||||
|
The variable daYawVar is the variance of the yaw angle uncertainty in rad**2
|
||||||
|
See DeriveYawResetEquations.m for the derivation
|
||||||
|
*/
|
||||||
|
|
||||||
|
// Intermediate variables
|
||||||
|
float SG[3];
|
||||||
|
SG[0] = sq(q0) - sq(q1) - sq(q2) + sq(q3);
|
||||||
|
SG[1] = 2*q0*q2 - 2*q1*q3;
|
||||||
|
SG[2] = 2*q0*q1 + 2*q2*q3;
|
||||||
|
|
||||||
|
float SQ[4];
|
||||||
|
SQ[0] = 0.5f * ((q1*SG[0]) - (q0*SG[2]) + (q3*SG[1]));
|
||||||
|
SQ[1] = 0.5f * ((q0*SG[1]) - (q2*SG[0]) + (q3*SG[2]));
|
||||||
|
SQ[2] = 0.5f * ((q3*SG[0]) - (q1*SG[1]) + (q2*SG[2]));
|
||||||
|
SQ[3] = 0.5f * ((q0*SG[0]) + (q1*SG[2]) + (q2*SG[1]));
|
||||||
|
|
||||||
|
// Variance of yaw angle uncertainty (rad**2)
|
||||||
|
const float daYawVar = TBD;
|
||||||
|
|
||||||
|
// Add covariances for additonal yaw uncertainty to exisiting covariances.
|
||||||
|
// This assumes that the additional yaw error is uncorrrelated
|
||||||
|
P[0][0] += daYawVar*sq(SQ[2]);
|
||||||
|
P[0][1] += daYawVar*SQ[1]*SQ[2];
|
||||||
|
P[1][1] += daYawVar*sq(SQ[1]);
|
||||||
|
P[0][2] += daYawVar*SQ[0]*SQ[2];
|
||||||
|
P[1][2] += daYawVar*SQ[0]*SQ[1];
|
||||||
|
P[2][2] += daYawVar*sq(SQ[0]);
|
||||||
|
P[0][3] += daYawVar*SQ[2]*SQ[3];
|
||||||
|
P[1][3] += daYawVar*SQ[1]*SQ[3];
|
||||||
|
P[2][3] += daYawVar*SQ[0]*SQ[3];
|
||||||
|
P[3][3] += daYawVar*sq(SQ[3]);
|
||||||
Reference in New Issue
Block a user