zrzk
/
px4_autopilot
1
0
Fork 0
Code Issues Pull Requests Projects Releases Wiki Activity
Browse Source

EKF: Derive equations enabling yaw variance to be increased

master
Paul Riseborough 6 years ago committed by Daniel Agar
parent
81eabc1903
commit
bf1f3a224e
3 changed files with 175 additions and 0 deletions
Whitespace
Show all changes Ignore whitespace when comparing lines Ignore changes in amount of whitespace Ignore changes in whitespace at EOL
Split View
Diff Options
Show Stats Download Patch File Download Diff File
  1. 35
      EKF/matlab/scripts/Inertial Nav EKF/C_code4.txt
  2. 106
      EKF/matlab/scripts/Inertial Nav EKF/DeriveYawResetEquations.m
  3. 34
      EKF/matlab/scripts/Inertial Nav EKF/YawCovariance.c

35
EKF/matlab/scripts/Inertial Nav EKF/C_code4.txt
Unescape View File

@ -0,0 +1,35 @@ @@ -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]);

106
EKF/matlab/scripts/Inertial Nav EKF/DeriveYawResetEquations.m
Unescape View File

@ -0,0 +1,106 @@ @@ -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);

34
EKF/matlab/scripts/Inertial Nav EKF/YawCovariance.c
Unescape View File

@ -0,0 +1,34 @@ @@ -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]);
Write Preview
Loading…
Cancel
Save