ardupilot/libraries/AP_NavEKF/Models/GimbalEstimatorExample/GenerateEquations.m

242 lines
8.5 KiB
Matlab

% IMPORTANT - This script requires the Matlab symbolic toolbox
% Author: Paul Riseborough
% Last Modified: 16 Feb 2015
% Derivation of a 3-axis gimbal attitude estimator using a local NED earth Tangent
% Frame. 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),
% pp. 855-860.
% The gimbal is assumed to have the following characteristics:
% A three axis gimbal having a fixed top plate mounted to the vehicle body with a magnetometer
% Yaw, roll and pitch degrees of freedom (yaw, roll, pitch Euler sequence)
% with angle measurements on each gimbal axis
% IMU measuring delta angles and delta velocites mounted on the
% camera/sensor assembly
% When the gimbal joints are all at zero degrees, the sensor assembly X,Y,Z
% axis is aligned with the top plate X,Y,Z axis
% State vector:
% error rotation vector - X,Y,Z (rad)
% Velocity - North, East, Down (m/s)
% Delta Angle bias - X,Y,Z (rad)
% Delta Velocity Bias - X,Y,Z (m/s)
% Observations:
% NED velocity - N,E,D (m/s)
% sensor fixed magnetic field vector of base - X,Y,Z
% Time varying parameters:
% XYZ delta angle measurements in sensor axes - rad
% XYZ delta velocity measurements in sensor axes - m/sec
% yaw, roll, pitch gimbal rotation angles
%% define symbolic variables and constants
clear all;
% specify if we want to incorporate accerometer bias estimation into the
% filter, 0 = no, 1 = yes
f_accBiasEst = 0;
syms dax day daz real % IMU delta angle measurements in sensor axes - rad
syms dvx dvy dvz real % IMU delta velocity measurements in sensor axes - m/sec
syms q0 q1 q2 q3 real % quaternions defining attitude of sensor axes relative to local NED
syms vn ve vd real % NED velocity - m/sec
syms dax_b day_b daz_b real % delta angle bias - rad
syms dvx_b dvy_b dvz_b real % delta velocity bias - m/sec
syms dt real % IMU time step - sec
syms gravity real % gravity - m/sec^2
syms daxNoise dayNoise dazNoise dvxNoise dvyNoise dvzNoise real; % IMU delta angle and delta velocity measurement noise
syms vwn vwe real; % NE wind velocity - m/sec
syms magX magY magZ real; % XYZ top plate magnetic field measurements - milligauss
syms magN magE magD real; % NED earth fixed magnetic field components - milligauss
syms R_VN R_VE R_VD real % variances for NED velocity measurements - (m/sec)^2
syms R_MAG real % variance for magnetic flux measurements - milligauss^2
syms rotErr1 rotErr2 rotErr3 real; % error rotation vector
syms decl real; % earth magnetic field declination from true north
syms gPsi gPhi gTheta real; % gimbal joint angles yaw, roll, pitch (rad)
%% define the process equations
% define the measured Delta angle and delta velocity vectors
dAngMeas = [dax; day; daz];
dVelMeas = [dvx; dvy; dvz];
% define the delta angle bias errors
dAngBias = [dax_b; day_b; daz_b];
% define the delta velocity bias errors
if (f_accBiasEst > 0)
dVelBias = [dvx_b; dvy_b; dvz_b];
else
dVelBias = [0; 0; 0];
end
% define the quaternion rotation vector for the state estimate
estQuat = [q0;q1;q2;q3];
% define the attitude error rotation vector, where error = truth - estimate
errRotVec = [rotErr1;rotErr2;rotErr3];
% define the attitude error quaternion using a first order linearisation
errQuat = [1;0.5*errRotVec];
% Define the truth quaternion as the estimate + error
truthQuat = QuatMult(estQuat, errQuat);
% derive the truth sensor to nav direction cosine matrix
Tsn = Quat2Tbn(truthQuat);
% define the truth delta angle
% ignore coning acompensation as these effects are negligible in terms of
% covariance growth for our application and grade of sensor
dAngTruth = dAngMeas - dAngBias - [daxNoise;dayNoise;dazNoise];
% Define the truth delta velocity
dVelTruth = dVelMeas - dVelBias - [dvxNoise;dvyNoise;dvzNoise];
% 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*dAngTruth(1);
0.5*dAngTruth(2);
0.5*dAngTruth(3);
];
truthQuatNew = QuatMult(truthQuat,deltaQuat);
% calculate the updated attitude error quaternion with respect to the previous estimate
errQuatNew = QuatDivide(truthQuatNew,estQuat);
% change to a rotaton vector - this is the error rotation vector updated state
errRotNew = 2 * [errQuatNew(2);errQuatNew(3);errQuatNew(4)];
% define the velocity update equations
% ignore coriolis terms for linearisation purposes
vNew = [vn;ve;vd] + [0;0;gravity]*dt + Tsn*dVelTruth;
% define the IMU bias error update equations
dabNew = dAngBias;
dvbNew = dVelBias;
% Define the state vector & number of states
if (f_accBiasEst > 0)
stateVector = [errRotVec;vn;ve;vd;dAngBias;dVelBias];
else
stateVector = [errRotVec;vn;ve;vd;dAngBias];
end
nStates=numel(stateVector);
save 'symeqns.mat';
%% derive the filter Jacobians
% Define the control (disturbance) vector. 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. This is OK because we
% have sensor bias accounted for in the state equations.
distVector = [daxNoise;dayNoise;dazNoise;dvxNoise;dvyNoise;dvzNoise];
% derive the control(disturbance) influence matrix
if (f_accBiasEst > 0)
predictedState = [errRotNew;vNew;dabNew;dvbNew];
else
predictedState = [errRotNew;vNew;dabNew];
end
G = jacobian(predictedState, distVector);
G = subs(G, {'rotErr1', 'rotErr2', 'rotErr3'}, {0,0,0});
% derive the state error matrix
distMatrix = diag(distVector);
Q = G*distMatrix*transpose(G);
%matlabFunction(Q,'file','calcQ.m');
% derive the state transition matrix
vNew = subs(vNew,{'daxNoise','dayNoise','dazNoise','dvxNoise','dvyNoise','dvzNoise'}, {0,0,0,0,0,0});
errRotNew = subs(errRotNew,{'daxNoise','dayNoise','dazNoise','dvxNoise','dvyNoise','dvzNoise'}, {0,0,0,0,0,0});
% Define the state vector & number of states
if (f_accBiasEst)
predictedState = [errRotNew;vNew;dabNew;dvbNew];
else
predictedState = [errRotNew;vNew;dabNew];
end
F = jacobian(predictedState, stateVector);
F = subs(F, {'rotErr1', 'rotErr2', 'rotErr3'}, {0,0,0});
%matlabFunction(F,'file','calcF.m');
%% Derive the predicted covariance
% This reduces the number of floating point operations by a factor of 4 or
% more compared to using the standard matrix operations in code
% 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
% Derive the predicted covariance matrix using the standard equation
PP = F*P*transpose(F) + Q;
%matlabFunction(PP,'file','calcP.m');
ccode(PP,'file','calcP.c');
FixCode('calcP');
% free up memory
clear all;
reset(symengine);
%% derive equations for fusion of magnetic deviation measurement
load('symeqns.mat');
% Define rotation from magnetometer to yaw gimbal
T3 = [ cos(gPsi) sin(gPsi) 0; ...
-sin(gPsi) cos(gPsi) 0; ...
0 0 1];
% Define rotation from yaw gimbal to roll gimbal
T1 = [ 1 0 0; ...
0 cos(gPhi) sin(gPhi); ...
0 -sin(gPhi) cos(gPhi)];
% Define rotation from roll gimbal to pitch gimbal
T2 = [ cos(gTheta) 0 -sin(gTheta); ...
0 1 0; ...
sin(gTheta) 0 cos(gTheta)];
% Define rotation from magnetometer to sensor using a 312 rotation sequence
Tms = T2*T1*T3;
% Define rotation from magnetometer to nav axes
Tmn = Tsn*Tms;
save 'symeqns.mat';
% rotate magnetic field measured at top plate into nav axes
magMeasNED = Tmn*[magX;magY;magZ];
% the predicted measurement is the angle wrt magnetic north of the horizontal
% component of the measured field
angMeas = tan(magMeasNED(2)/magMeasNED(1)) - decl;
H_MAG = jacobian(angMeas,stateVector); % measurement Jacobian
H_MAG = subs(H_MAG, {'rotErr1', 'rotErr2', 'rotErr3'}, {0,0,0});
H_MAG = H_MAG(1:3);
H_MAG = simplify(H_MAG);
% matlabFunction(H_MAG,'file','calcH_MAG.m');
ccode(H_MAG,'file','calcH_MAG.c');
FixCode('calcH_MAG');
% free up memory
clear all;
reset(symengine);
%% generate helper functions
load 'symeqns.mat';
matlabFunction(Tms,'file','calcTms.m');
Tmn = subs(Tmn, {'rotErr1', 'rotErr2', 'rotErr3'}, {0,0,0});
matlabFunction(Tmn,'file','calcTmn.m');