forked from Archive/PX4-Autopilot
EKF: Derive equations enabling yaw variance to be increased
This commit is contained in:
Paul Riseborough
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]);
|
||||
Loading…
Reference in New Issue