Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-02-19 22:33:57 -04:00
Code Issues Packages Projects Releases Wiki Activity

AP_NavEKF: Add Matlab derivations and simulations behind small EKF

Browse Source
This commit is contained in:
Paul Riseborough 2015-04-02 07:54:15 +11:00 committed by Randy Mackay
parent fea7632eac
commit 586e4a7d2b
71 changed files with 5949 additions and 0 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

47
libraries/AP_NavEKF/Models/AttErrVecMathExample/AlignHeading.m Normal file
View File

@ -0,0 +1,47 @@
function quat = AlignHeading( ...
quat, ... % quaternion state vector
magMea, ... % body frame magnetic flux measurements
declination) % Estimated magnetic field delination at current location
% Calculate the predicted magnetic declination
Tbn = Quat2Tbn(quat);
magMeasNED = Tbn*magMea;
predDec = atan2(magMeasNED(2),magMeasNED(1));
% Calculate the measurement innovation
innovation = predDec - declination;
if (innovation > pi)
innovation = innovation - 2*pi;
elseif (innovation < -pi)
innovation = innovation + 2*pi;
end
% form the NED rotation vector
deltaRotNED = -[0;0;innovation];
% rotate into body axes
% Calculate the body to nav cosine matrix
Tbn = Quat2Tbn(quat);
deltaRotBody = transpose(Tbn)*deltaRotNED;
% Convert the error rotation vector to its equivalent quaternion
% error = truth - estimate
rotationMag = abs(innovation);
if rotationMag<1e-6
deltaQuat = single([1;0;0;0]);
else
deltaQuat = [cos(0.5*rotationMag); [deltaRotBody(1);deltaRotBody(2);deltaRotBody(3)]/rotationMag*sin(0.5*rotationMag)];
end
% Update the quaternion states by rotating from the previous attitude through
% the delta angle rotation quaternion
quat = [quat(1)*deltaQuat(1)-transpose(quat(2:4))*deltaQuat(2:4); quat(1)*deltaQuat(2:4) + deltaQuat(1)*quat(2:4) + cross(quat(2:4),deltaQuat(2:4))];
% normalise the updated quaternion states
quatMag = sqrt(quat(1)^2 + quat(2)^2 + quat(3)^2 + quat(4)^2);
if (quatMag > 1e-12)
quat = quat / quatMag;
end
end

20
libraries/AP_NavEKF/Models/AttErrVecMathExample/AlignTilt.m Normal file
View File

@ -0,0 +1,20 @@
function quat = AlignTilt( ...
quat, ... % quaternion state vector
initAccel) % initial accelerometer vector
% check length
lengthAccel = sqrt(dot([initAccel(1);initAccel(2);initAccel(3)],[initAccel(1);initAccel(2);initAccel(3)]));
% if length is > 0.7g and < 1.4g initialise tilt using gravity vector
if (lengthAccel > 5 && lengthAccel < 14)
% calculate length of the tilt vector
tiltMagnitude = atan2(sqrt(dot([initAccel(1);initAccel(2)],[initAccel(1);initAccel(2)])),-initAccel(3));
% take the unit cross product of the accel vector and the -Z vector to
% give the tilt unit vector
if (tiltMagnitude > 1e-3)
tiltUnitVec = cross([initAccel(1);initAccel(2);initAccel(3)],[0;0;-1]);
tiltUnitVec = tiltUnitVec/sqrt(dot(tiltUnitVec,tiltUnitVec));
tiltVec = tiltMagnitude*tiltUnitVec;
quat = [cos(0.5*tiltMagnitude); tiltVec/tiltMagnitude*sin(0.5*tiltMagnitude)];
end
end
end

91
libraries/AP_NavEKF/Models/AttErrVecMathExample/FuseMagnetometer.m Normal file
View File

@ -0,0 +1,91 @@
function [...
nextQuat, ... % quaternion state vector after fusion of measurements
nextStates, ... % state vector after fusion of measurements
nextP, ... % state covariance matrix after fusion of corrections
innovation, ... % Declination innovation - rad
varInnov] ... %
= FuseMagnetometer( ...
quat, ... % predicted quaternion states
states, ... % predicted states
P, ... % predicted covariance
magData, ... % body frame magnetic flux measurements
measDec, ... % magnetic field declination - azimuth angle measured from true north (rad)
Tbn) % Estimated coordinate transformation matrix from body to NED frame
q0 = quat(1);
q1 = quat(2);
q2 = quat(3);
q3 = quat(4);
magX = magData(1);
magY = magData(2);
magZ = magData(3);
R_MAG = 0.1745^2;
H = calcH_MAG(magX,magY,magZ,q0,q1,q2,q3);
varInnov = (H*P*transpose(H) + R_MAG);
Kfusion = (P*transpose(H))/varInnov;
% Calculate the predicted magnetic declination
magMeasNED = Tbn*[magX;magY;magZ];
predDec = atan2(magMeasNED(2),magMeasNED(1));
% Calculate the measurement innovation
innovation = predDec - measDec;
if (innovation > pi)
innovation = innovation - 2*pi;
elseif (innovation < -pi)
innovation = innovation + 2*pi;
end
if (innovation > 0.5)
innovation = 0.5;
elseif (innovation < -0.5)
innovation = -0.5;
end
% correct the state vector
states(1:3) = 0;
states = states - Kfusion * innovation;
% the first 3 states represent the angular misalignment vector. This is
% is used to correct the estimate quaternion
% Convert the error rotation vector to its equivalent quaternion
% error = truth - estimate
rotationMag = sqrt(states(1)^2 + states(2)^2 + states(3)^2);
if rotationMag<1e-6
deltaQuat = single([1;0;0;0]);
else
deltaQuat = [cos(0.5*rotationMag); [states(1);states(2);states(3)]/rotationMag*sin(0.5*rotationMag)];
end
% Update the quaternion states by rotating from the previous attitude through
% the delta angle rotation quaternion
nextQuat = [quat(1)*deltaQuat(1)-transpose(quat(2:4))*deltaQuat(2:4); quat(1)*deltaQuat(2:4) + deltaQuat(1)*quat(2:4) + cross(quat(2:4),deltaQuat(2:4))];
% normalise the updated quaternion states
quatMag = sqrt(nextQuat(1)^2 + nextQuat(2)^2 + nextQuat(3)^2 + nextQuat(4)^2);
if (quatMag > 1e-6)
nextQuat = nextQuat / quatMag;
end
% correct the covariance P = P - K*H*P
P = P - Kfusion*H*P;
% Force symmetry on the covariance matrix to prevent ill-conditioning
% of the matrix which would cause the filter to blow-up
P = 0.5*(P + transpose(P));
% ensure diagonals are positive
for i=1:9
if P(i,i) < 0
P(i,i) = 0;
end
end
% Set default output for states and covariance
nextP = P;
nextStates = states;
end

72
libraries/AP_NavEKF/Models/AttErrVecMathExample/FuseVelocity.m Normal file
View File

@ -0,0 +1,72 @@
function [...
quat, ... % quaternion state vector after fusion of measurements
states, ... % state vector after fusion of measurements
angErr, ... % angle error
P, ... % state covariance matrix after fusion of corrections
innovation,... % NED velocity innovations (m/s)
varInnov] ... % NED velocity innovation variance ((m/s)^2)
= FuseVelocity( ...
quat, ... % predicted quaternion states from the INS
states, ... % predicted states from the INS
P, ... % predicted covariance
measVel) % NED velocity measurements (m/s)
R_OBS = 0.5^2;
innovation = zeros(1,3);
varInnov = zeros(1,3);
% Fuse measurements sequentially
angErrVec = [0;0;0];
for obsIndex = 1:3
stateIndex = 3 + obsIndex;
% Calculate the velocity measurement innovation
innovation(obsIndex) = states(stateIndex) - measVel(obsIndex);
% Calculate the Kalman Gain
H = zeros(1,9);
H(1,stateIndex) = 1;
varInnov(obsIndex) = (H*P*transpose(H) + R_OBS);
K = (P*transpose(H))/varInnov(obsIndex);
% Calculate state corrections
xk = K * innovation(obsIndex);
% Apply the state corrections
states(1:3) = 0;
states = states - xk;
% Store tilt error estimate for external monitoring
angErrVec = angErrVec + states(1:3);
% the first 3 states represent the angular misalignment vector. This is
% is used to correct the estimated quaternion
% Convert the error rotation vector to its equivalent quaternion
% truth = estimate + error
rotationMag = sqrt(states(1)^2 + states(2)^2 + states(3)^2);
if rotationMag > 1e-12
deltaQuat = [cos(0.5*rotationMag); [states(1);states(2);states(3)]/rotationMag*sin(0.5*rotationMag)];
% Update the quaternion states by rotating from the previous attitude through
% the error quaternion
quat = QuatMult(quat,deltaQuat);
% re-normalise the quaternion
quatMag = sqrt(quat(1)^2 + quat(2)^2 + quat(3)^2 + quat(4)^2);
quat = quat / quatMag;
end
% Update the covariance
P = P - K*H*P;
% Force symmetry on the covariance matrix to prevent ill-conditioning
P = 0.5*(P + transpose(P));
% ensure diagonals are positive
for i=1:9
if P(i,i) < 0
P(i,i) = 0;
end
end
end
angErr = sqrt(dot(angErrVec,angErrVec));
end

161
libraries/AP_NavEKF/Models/AttErrVecMathExample/GenerateEquations.m Normal file
View File

@ -0,0 +1,161 @@
% IMPORTANT - This script requires the Matlab symbolic toolbox
% Author: Paul Riseborough
% Last Modified: 16 Feb 2015
% Derivation of a navigation EKF using a local NED earth Tangent Frame for
% the initial alignment and gyro bias estimation from a moving platform
% 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 benefits for use of rotation error vector over use of a four parameter
% quaternion representation of the estiamted orientation are:
% a) Reduced computational load
% b) Improved stability
% c) The ability to recover faster from large orientation errors. This
% makes this filter particularly suitable where the initial alignment is
% uncertain
% State vector:
% error rotation vector
% Velocity - North, East, Down (m/s)
% Delta Angle bias - X,Y,Z (rad)
% Observations:
% NED velocity - N,E,D (m/s)
% body fixed magnetic field vector - X,Y,Z
% Time varying parameters:
% XYZ delta angle measurements in body axes - rad
% XYZ delta velocity measurements in body axes - m/sec
% magnetic declination
clear all;
%% define symbolic variables and constants
syms dax day daz real % IMU delta angle measurements in body axes - rad
syms dvx dvy dvz real % IMU delta velocity measurements in body axes - m/sec
syms q0 q1 q2 q3 real % quaternions defining attitude of body 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 body fixed 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
%% 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 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 body to nav direction cosine matrix
Tbn = 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 - [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 + Tbn*dVelTruth;
% define the IMU bias error update equations
dabNew = [dax_b; day_b; daz_b];
% Define the state vector & number of states
stateVector = [errRotVec;vn;ve;vd;dAngBias];
nStates=numel(stateVector);
%% derive the covariance prediction equation
% This reduces the number of floating point operations by a factor of 6 or
% more compared to using the standard matrix operations in code
% 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 becasue we
% have sensor bias accounted for in the state equations.
distVector = [daxNoise;dayNoise;dazNoise;dvxNoise;dvyNoise;dvzNoise];
% derive the control(disturbance) influence matrix
G = jacobian([errRotNew;vNew;dabNew], distVector);
G = subs(G, {'rotErr1', 'rotErr2', 'rotErr3'}, {0,0,0});
% derive the state error matrix
distMatrix = diag(distVector);
Q = G*distMatrix*transpose(G);
f = 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});
F = jacobian([errRotNew;vNew;dabNew], stateVector);
F = subs(F, {'rotErr1', 'rotErr2', 'rotErr3'}, {0,0,0});
f = matlabFunction(F,'file','calcF.m');
% 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
% nextP = F*P*transpose(F) + Q;
% f = matlabFunction(nextP,'file','calcP.m');
%% derive equations for fusion of magnetic deviation measurement
% rotate body measured 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 = tan(magMeasNED(2)/magMeasNED(1));
H_MAG = jacobian(angMeas,stateVector); % measurement Jacobian
H_MAG = subs(H_MAG, {'rotErr1', 'rotErr2', 'rotErr3'}, {0,0,0});
f = matlabFunction(H_MAG,'file','calcH_MAG.m');

28
libraries/AP_NavEKF/Models/AttErrVecMathExample/PlotData.m Normal file
View File

@ -0,0 +1,28 @@
%% calculate and plot tilt correction magnitude
figure;
plot(angErrLog(1,:)*0.001,angErrLog(2,:));
grid on;
ylabel('Tilt correction magnitude (deg)');
xlabel('time (sec)');
%% plot gyro bias estimates
figure;
plot(statesLog(1,:)*0.001,statesLog(8:10,:)/dt*180/pi);
grid on;
ylabel('Gyro Bias Estimate (deg/sec)');
xlabel('time (sec)');
%% plot Euler angle estimates
figure;
eulLog(4,:) = eulLog(4,:) + pi;
plot(eulLog(1,:)*0.001,eulLog(2:4,:)*180/pi);
grid on;
ylabel('Euler Angle Estimates (deg)');
xlabel('time (sec)');
%% plot velocity innovations
figure;
plot(statesLog(1,:)*0.001,statesLog(5:7,:));
grid on;
ylabel('EKF velocity Innovations (m/s)');
xlabel('time (sec)');

61
libraries/AP_NavEKF/Models/AttErrVecMathExample/PredictCovariance.m Normal file
View File

@ -0,0 +1,61 @@
function P = PredictCovariance(deltaAngle, ...
deltaVelocity, ...
quat, ...
states,...
P, ... % Previous state covariance matrix
dt) ... % IMU and prediction time step
% Set filter state process noise other than IMU errors, which are already
% built into the derived covariance predition equations.
% This process noise determines the rate of estimation of IMU bias errors
dAngBiasSigma = dt*5E-6;
processNoise = [0*ones(1,6), dAngBiasSigma*[1 1 1]];
% Specify the estimated errors on the IMU delta angles and delta velocities
% Allow for 0.5 deg/sec of gyro error
daxNoise = (dt*0.5*pi/180)^2;
dayNoise = (dt*0.5*pi/180)^2;
dazNoise = (dt*0.5*pi/180)^2;
% Allow for 0.5 m/s/s of accelerometer error
dvxNoise = (dt*0.5)^2;
dvyNoise = (dt*0.5)^2;
dvzNoise = (dt*0.5)^2;
dvx = deltaVelocity(1);
dvy = deltaVelocity(2);
dvz = deltaVelocity(3);
dax = deltaAngle(1);
day = deltaAngle(2);
daz = deltaAngle(3);
q0 = quat(1);
q1 = quat(2);
q2 = quat(3);
q3 = quat(4);
dax_b = states(7);
day_b = states(8);
daz_b = states(9);
% Predicted covariance
F = calcF(dax,dax_b,day,day_b,daz,daz_b,dvx,dvy,dvz,q0,q1,q2,q3);
Q = calcQ(daxNoise,dayNoise,dazNoise,dvxNoise,dvyNoise,dvzNoise,q0,q1,q2,q3);
P = F*P*transpose(F) + Q;
% Add the general process noise
for i = 1:9
P(i,i) = P(i,i) + processNoise(i)^2;
end
% Force symmetry on the covariance matrix to prevent ill-conditioning
% of the matrix which would cause the filter to blow-up
P = 0.5*(P + transpose(P));
% ensure diagonals are positive
for i=1:9
if P(i,i) < 0
P(i,i) = 0;
end
end
end

70
libraries/AP_NavEKF/Models/AttErrVecMathExample/PredictStates.m Normal file
View File

@ -0,0 +1,70 @@
function [quat, states, Tbn, correctedDelAng, correctedDelVel] = PredictStates( ...
quat, ... % previous quaternion states 4x1
states, ... % previous states (3x1 rotation error, 3x1 velocity, 3x1 gyro bias)
angRate, ... % IMU rate gyro measurements, 3x1 (rad/sec)
accel, ... % IMU accelerometer measurements 3x1 (m/s/s)
dt) % time since last IMU measurement (sec)
% Define parameters used for previous angular rates and acceleration shwich
% are used for trapezoidal integration
persistent prevAngRate;
if isempty(prevAngRate)
prevAngRate = angRate;
end
persistent prevAccel;
if isempty(prevAccel)
prevAccel = accel;
end
% define persistent variables for previous delta angle and velocity which
% are required for sculling and coning error corrections
persistent prevDelAng;
if isempty(prevDelAng)
prevDelAng = prevAngRate*dt;
end
persistent prevDelVel;
if isempty(prevDelVel)
prevDelVel = prevAccel*dt;
end
% Convert IMU data to delta angles and velocities using trapezoidal
% integration
dAng = 0.5*(angRate + prevAngRate)*dt;
dVel = 0.5*(accel + prevAccel )*dt;
prevAngRate = angRate;
prevAccel = accel;
% Remove sensor bias errors
dAng = dAng - states(7:9);
% Apply rotational and skulling corrections
correctedDelVel= dVel + ...
0.5*cross(prevDelAng + dAng , prevDelVel + dVel) + 1/6*cross(prevDelAng + dAng , cross(prevDelAng + dAng , prevDelVel + dVel)) + ... % rotational correction
1/12*(cross(prevDelAng , dVel) + cross(prevDelVel , dAng)); % sculling correction
% Apply corrections for coning errors
correctedDelAng = dAng - 1/12*cross(prevDelAng , dAng);
% Save current measurements
prevDelAng = dAng;
prevDelVel = dVel;
% Convert the rotation vector to its equivalent quaternion
deltaQuat = RotToQuat(correctedDelAng);
% Update the quaternions by rotating from the previous attitude through
% the delta angle rotation quaternion
quat = QuatMult(quat,deltaQuat);
% Normalise the quaternions
quat = NormQuat(quat);
% Calculate the body to nav cosine matrix
Tbn = Quat2Tbn(quat);
% transform body delta velocities to delta velocities in the nav frame
delVelNav = Tbn * correctedDelVel + [0;0;9.807]*dt;
% Sum delta velocities to get velocity
states(4:6) = states(4:6) + delVelNav(1:3);
end

86
libraries/AP_NavEKF/Models/AttErrVecMathExample/RunRealData.m Normal file
View File

@ -0,0 +1,86 @@
%% Set initial conditions
clear all;
load('fltTest.mat');
startDelayTime = 100; % number of seconds to delay filter start (used to simulate in-flight restart)
dt = 1/50;
indexLimit = length(IMU);
magIndexlimit = length(MAG);
statesLog = zeros(10,indexLimit);
eulLog = zeros(4,indexLimit);
velInnovLog = zeros(4,indexLimit);
angErrLog = zeros(2,indexLimit);
decInnovLog = zeros(2,magIndexlimit);
velInnovVarLog = velInnovLog;
decInnovVarLog = decInnovLog;
% initialise the state vector and quaternion
states = zeros(9,1);
quat = [1;0;0;0];
Tbn = Quat2Tbn(quat);
% average last 10 accel readings to reduce effect of noise
initAccel(1) = mean(IMU(1:10,6));
initAccel(2) = mean(IMU(1:10,7));
initAccel(3) = mean(IMU(1:10,8));
% Use averaged accel readings to align tilt
quat = AlignTilt(quat,initAccel);
% Set the expected declination
measDec = 0.18;
% define the state covariances
Sigma_velNED = 0.5; % 1 sigma uncertainty in horizontal velocity components
Sigma_dAngBias = 5*pi/180*dt; % 1 Sigma uncertainty in delta angle bias
Sigma_angErr = 1; % 1 Sigma uncertainty in angular misalignment (rad)
covariance = single(diag([Sigma_angErr*[1;1;1];Sigma_velNED*[1;1;1];Sigma_dAngBias*[1;1;1]].^2));
%% Main Loop
magIndex = 1;
time = 0;
angErr = 0;
headingAligned = 0;
% delay start by a minimum of 10 IMU samples to allow for initial tilt
% alignment delay
startIndex = max(11,ceil(startDelayTime/dt));
for index = startIndex:indexLimit
time=time+dt + startIndex*dt;
% read IMU measurements
angRate = IMU(index,3:5)';
% switch in a bias offset to test the filter
if (time > +inf)
angRate = angRate + [1;-1;1]*pi/180;
end
accel = IMU(index,6:8)';
% predict states
[quat, states, Tbn, delAng, delVel] = PredictStates(quat,states,angRate,accel,dt);
statesLog(1,index) = time;
statesLog(2:10,index) = states;
eulLog(1,index) = time;
eulLog(2:4,index) = QuatToEul(quat);
% predict covariance matrix
covariance = PredictCovariance(delAng,delVel,quat,states,covariance,dt);
% fuse velocity measurements - use synthetic measurements
measVel = [0;0;0];
[quat,states,angErr,covariance,velInnov,velInnovVar] = FuseVelocity(quat,states,covariance,measVel);
velInnovLog(1,index) = time;
velInnovLog(2:4,index) = velInnov;
velInnovVarLog(1,index) = time;
velInnovVarLog(2:4,index) = velInnovVar;
angErrLog(1,index) = time;
angErrLog(2,index) = angErr;
% read magnetometer measurements
while ((MAG(magIndex,1) < IMU(index,1)) && (magIndex < magIndexlimit))
magIndex = magIndex + 1;
magBody = 0.001*MAG(magIndex,3:5)';
if (time >= 1.0 && headingAligned==0 && angErr < 1e-3)
quat = AlignHeading(quat,magBody,measDec);
headingAligned = 1;
end
% fuse magnetometer measurements if new data available and when tilt has settled
if (headingAligned == 1)
[quat,states,covariance,decInnov,decInnovVar] = FuseMagnetometer(quat,states,covariance,magBody,measDec,Tbn);
decInnovLog(1,magIndex) = time;
decInnovLog(2,magIndex) = decInnov;
decInnovVarLog(1,magIndex) = time;
decInnovVarLog(2,magIndex) = decInnovVar;
end
end
end
%% Generate plots
PlotData;

78
libraries/AP_NavEKF/Models/AttErrVecMathExample/RunSyntheticData.m Normal file
View File

@ -0,0 +1,78 @@
%% Set initial conditions
clear all;
dt = 1/100;
duration = 10;
indexLimit = round(duration/dt);
statesLog = zeros(10,indexLimit);
eulLog = zeros(4,indexLimit);
velInnovLog = zeros(4,indexLimit);
decInnovLog = zeros(2,indexLimit);
velInnovVarLog = velInnovLog;
decInnovVarLog = decInnovLog;
angErrLog = zeros(2,indexLimit);
% Use a random initial orientation
quatTruth = [rand;randn;randn;randn];
quatLength = sqrt(quatTruth(1)^2 + quatTruth(2)^2 + quatTruth(3)^2 + quatTruth(4)^2);
quatTruth = quatTruth / quatLength;
TbnTruth = Quat2Tbn(quatTruth);
% initialise the filter to level
quat = [1;0;0;0];
states = zeros(9,1);
Tbn = Quat2Tbn(quat);
% define the earths truth magnetic field
magEarthTruth = [0.3;0.1;-0.5];
% define the assumed declination using th etruth field plus location
% variation
measDec = atan2(magEarthTruth(2),magEarthTruth(1)) + 2*pi/180*randn;
% define the magnetometer bias errors
magMeasBias = 0.02*[randn;randn;randn];
% define the state covariances with the exception of the quaternion covariances
Sigma_velNED = 0.5; % 1 sigma uncertainty in horizontal velocity components
Sigma_dAngBias = 1*pi/180*dt; % 1 Sigma uncertainty in delta angle bias
Sigma_angErr = 1; % 1 Sigma uncertainty in angular misalignment (rad)
covariance = single(diag([Sigma_angErr*[1;1;1];Sigma_velNED*[1;1;1];Sigma_dAngBias*[1;1;1]].^2));
%% Main Loop
headingAligned=0;
time = 0;
for index = 1:indexLimit
time=time+dt;
% synthesise IMU measurements
angRate = 0*[randn;randn;randn];
accel = 0*[randn;randn;randn] + transpose(TbnTruth)*[0;0;-9.81];
% predict states
[quat, states, Tbn, delAng, delVel] = PredictStates(quat,states,angRate,accel,dt);
statesLog(1,index) = time;
statesLog(2:10,index) = states;
eulLog(1,index) = time;
eulLog(2:4,index) = QuatToEul(quat);
% predict covariance matrix
covariance = PredictCovariance(delAng,delVel,quat,states,covariance,dt);
% synthesise velocity measurements
measVel = [0;0;0];
% fuse velocity measurements
[quat,states,angErr,covariance,velInnov,velInnovVar] = FuseVelocity(quat,states,covariance,measVel);
velInnovLog(1,index) = time;
velInnovLog(2:4,index) = velInnov;
velInnovVarLog(1,index) = time;
velInnovVarLog(2:4,index) = velInnovVar;
angErrLog(1,index) = time;
angErrLog(2,index) = angErr;
% synthesise magnetometer measurements adding sensor bias
magBody = transpose(TbnTruth)*magEarthTruth + magMeasBias;
% fuse magnetometer measurements
if (index > 500 && headingAligned==0 && angErr < 1e-4)
quat = AlignHeading(quat,magBody,measDec);
headingAligned = 1;
end
if (headingAligned == 1)
[quat,states,covariance,decInnov,decInnovVar] = FuseMagnetometer(quat,states,covariance,magBody,measDec,Tbn);
decInnovLog(1,index) = time;
decInnovLog(2,index) = decInnov;
decInnovVarLog(1,index) = time;
decInnovVarLog(2,index) = decInnovVar;
end
end
%% Generate Plots
PlotData;

65
libraries/AP_NavEKF/Models/AttErrVecMathExample/calcF.m Normal file
View File

@ -0,0 +1,65 @@
function F = calcF(dax,dax_b,day,day_b,daz,daz_b,dvx,dvy,dvz,q0,q1,q2,q3)
%CALCF
% F = CALCF(DAX,DAX_B,DAY,DAY_B,DAZ,DAZ_B,DVX,DVY,DVZ,Q0,Q1,Q2,Q3)
% This function was generated by the Symbolic Math Toolbox version 5.8.
% 27-Dec-2014 13:59:07
t2 = dax.*(1.0./2.0);
t3 = dax_b.*(1.0./2.0);
t4 = t2-t3;
t5 = day.*(1.0./2.0);
t6 = day_b.*(1.0./2.0);
t7 = t5-t6;
t8 = daz.*(1.0./2.0);
t9 = daz_b.*(1.0./2.0);
t10 = t8-t9;
t11 = q2.*t4.*(1.0./2.0);
t12 = q1.*t7.*(1.0./2.0);
t13 = q0.*t10.*(1.0./2.0);
t14 = q2.*(1.0./2.0);
t15 = q3.*t4.*(1.0./2.0);
t16 = q1.*t10.*(1.0./2.0);
t17 = q1.*(1.0./2.0);
t18 = q0.*t4.*(1.0./2.0);
t19 = q3.*t7.*(1.0./2.0);
t20 = q0.*(1.0./2.0);
t21 = q2.*t7.*(1.0./2.0);
t22 = q3.*t10.*(1.0./2.0);
t23 = q0.*t7.*(1.0./2.0);
t24 = q3.*(1.0./2.0);
t25 = q1.*t4.*(1.0./2.0);
t26 = q2.*t10.*(1.0./2.0);
t27 = t20-t21+t22+t25;
t28 = -t17+t18+t19+t26;
t29 = t14-t15+t16+t23;
t30 = t11+t12-t13+t24;
t31 = t17-t18+t19+t26;
t32 = t20+t21-t22+t25;
t33 = t11-t12+t13+t24;
t34 = -t14+t15+t16+t23;
t35 = q0.^2;
t36 = q1.^2;
t37 = q2.^2;
t38 = q3.^2;
t39 = -t35-t36-t37-t38;
t40 = t14+t15+t16-t23;
t41 = t11+t12+t13-t24;
t42 = t20+t21+t22-t25;
t43 = t17+t18+t19-t26;
t44 = q0.*q2.*2.0;
t45 = q1.*q3.*2.0;
t46 = t44+t45;
t47 = t35+t36-t37-t38;
t48 = q0.*q3.*2.0;
t52 = q1.*q2.*2.0;
t49 = t48-t52;
t50 = q0.*q1.*2.0;
t55 = q2.*q3.*2.0;
t51 = t50-t55;
t53 = t35-t36+t37-t38;
t54 = t48+t52;
t56 = t35-t36-t37+t38;
t57 = t50+t55;
t58 = t44-t45;
F = reshape([q3.*(q3.*(-1.0./2.0)+t11+t12+t13).*-2.0+q2.*(t14+t15+t16-q0.*t7.*(1.0./2.0)).*2.0+q1.*(t17+t18+t19-q2.*t10.*(1.0./2.0)).*2.0+q0.*(t20+t21+t22-q1.*t4.*(1.0./2.0)).*2.0,q0.*t41.*-2.0-q1.*t40.*2.0+q2.*t43.*2.0-q3.*t42.*2.0,q0.*t40.*-2.0+q1.*t41.*2.0+q2.*t42.*2.0+q3.*t43.*2.0,dvy.*t46+dvz.*t49,-dvy.*t51-dvz.*t53,dvy.*t56-dvz.*t57,0.0,0.0,0.0,q0.*t30.*-2.0+q1.*t29.*2.0+q2.*t28.*2.0+q3.*t27.*2.0,q0.*t27.*2.0-q1.*t28.*2.0+q2.*t29.*2.0+q3.*t30.*2.0,q0.*t28.*-2.0-q1.*t27.*2.0-q2.*t30.*2.0+q3.*t29.*2.0,-dvx.*t46+dvz.*t47,dvx.*t51+dvz.*t54,-dvx.*t56-dvz.*t58,0.0,0.0,0.0,q0.*t34.*-2.0+q1.*t33.*2.0-q2.*t32.*2.0-q3.*t31.*2.0,q0.*t31.*-2.0+q1.*t32.*2.0+q2.*t33.*2.0+q3.*t34.*2.0,q0.*t32.*2.0+q1.*t31.*2.0-q2.*t34.*2.0+q3.*t33.*2.0,-dvx.*t49-dvy.*t47,dvx.*t53-dvy.*t54,dvx.*t57+dvy.*t58,0.0,0.0,0.0,0.0,0.0,0.0,1.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,1.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,1.0,0.0,0.0,0.0,t39,0.0,0.0,0.0,0.0,0.0,1.0,0.0,0.0,0.0,t39,0.0,0.0,0.0,0.0,0.0,1.0,0.0,0.0,0.0,t39,0.0,0.0,0.0,0.0,0.0,1.0],[9, 9]);

38
libraries/AP_NavEKF/Models/AttErrVecMathExample/calcH_MAG.m Normal file
View File

@ -0,0 +1,38 @@
function H_MAG = calcH_MAG(magX,magY,magZ,q0,q1,q2,q3)
%CALCH_MAG
% H_MAG = CALCH_MAG(MAGX,MAGY,MAGZ,Q0,Q1,Q2,Q3)
% This function was generated by the Symbolic Math Toolbox version 5.8.
% 27-Dec-2014 13:59:09
t2 = q0.^2;
t3 = q1.^2;
t4 = q2.^2;
t5 = q3.^2;
t6 = q0.*q2.*2.0;
t7 = q1.*q3.*2.0;
t8 = t6+t7;
t9 = q0.*q3.*2.0;
t13 = q1.*q2.*2.0;
t10 = t9-t13;
t11 = t2+t3-t4-t5;
t12 = magX.*t11;
t14 = magZ.*t8;
t19 = magY.*t10;
t15 = t12+t14-t19;
t16 = t2-t3+t4-t5;
t17 = q0.*q1.*2.0;
t24 = q2.*q3.*2.0;
t18 = t17-t24;
t20 = 1.0./t15;
t21 = magY.*t16;
t22 = t9+t13;
t23 = magX.*t22;
t28 = magZ.*t18;
t25 = t21+t23-t28;
t29 = t20.*t25;
t26 = tan(t29);
t27 = 1.0./t15.^2;
t30 = t26.^2;
t31 = t30+1.0;
H_MAG = [-t31.*(t20.*(magZ.*t16+magY.*t18)+t25.*t27.*(magY.*t8+magZ.*t10)),t31.*(t20.*(magX.*t18+magZ.*t22)+t25.*t27.*(magX.*t8-magZ.*t11)),t31.*(t20.*(magX.*t16-magY.*t22)+t25.*t27.*(magX.*t10+magY.*t11)),0.0,0.0,0.0,0.0,0.0,0.0];

34
libraries/AP_NavEKF/Models/AttErrVecMathExample/calcQ.m Normal file
View File

@ -0,0 +1,34 @@
function Q = calcQ(daxNoise,dayNoise,dazNoise,dvxNoise,dvyNoise,dvzNoise,q0,q1,q2,q3)
%CALCQ
% Q = CALCQ(DAXNOISE,DAYNOISE,DAZNOISE,DVXNOISE,DVYNOISE,DVZNOISE,Q0,Q1,Q2,Q3)
% This function was generated by the Symbolic Math Toolbox version 5.8.
% 27-Dec-2014 13:59:04
t3 = q0.^2;
t4 = q1.^2;
t5 = q2.^2;
t6 = q3.^2;
t2 = t3+t4+t5+t6;
t7 = t2.^2;
t11 = q0.*q3.*2.0;
t12 = q1.*q2.*2.0;
t8 = t11-t12;
t13 = q0.*q2.*2.0;
t14 = q1.*q3.*2.0;
t9 = t13+t14;
t10 = t3+t4-t5-t6;
t15 = q0.*q1.*2.0;
t16 = t11+t12;
t17 = dvxNoise.*t10.*t16;
t18 = t3-t4+t5-t6;
t19 = q2.*q3.*2.0;
t20 = t15-t19;
t21 = t15+t19;
t22 = t3-t4-t5+t6;
t23 = t13-t14;
t24 = dvzNoise.*t9.*t22;
t25 = t24-dvxNoise.*t10.*t23-dvyNoise.*t8.*t21;
t26 = dvyNoise.*t18.*t21;
t27 = t26-dvxNoise.*t16.*t23-dvzNoise.*t20.*t22;
Q = reshape([daxNoise.*t7,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,dayNoise.*t7,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,dazNoise.*t7,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,dvxNoise.*t10.^2+dvyNoise.*t8.^2+dvzNoise.*t9.^2,t17-dvyNoise.*t8.*t18-dvzNoise.*t9.*t20,t25,0.0,0.0,0.0,0.0,0.0,0.0,t17-dvzNoise.*t9.*(t15-q2.*q3.*2.0)-dvyNoise.*t8.*t18,dvxNoise.*t16.^2+dvyNoise.*t18.^2+dvzNoise.*t20.^2,t27,0.0,0.0,0.0,0.0,0.0,0.0,t25,t27,dvxNoise.*t23.^2+dvyNoise.*t21.^2+dvzNoise.*t22.^2,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0],[9, 9]);

BIN
libraries/AP_NavEKF/Models/AttErrVecMathExample/with initial alignment.fig Normal file
View File

Binary file not shown.

220
libraries/AP_NavEKF/Models/Common/ConvertToC.m Normal file
View File

@ -0,0 +1,220 @@
function ConvertToC(fileName)
delimiter = '';
%% Format string for each line of text:
% column1: text (%s)
% For more information, see the TEXTSCAN documentation.
formatSpec = '%s%[^\n\r]';
%% Open the text file.
fileID = fopen(strcat(fileName,'.m'),'r');
%% Read columns of data according to format string.
% This call is based on the structure of the file used to generate this
% code. If an error occurs for a different file, try regenerating the code
% from the Import Tool.
dataArray = textscan(fileID, formatSpec, 'Delimiter', delimiter, 'ReturnOnError', false, 'Bufsize', 65535);
%% Close the text file.
fclose(fileID);
%% Create output variable
SymbolicOutput = [dataArray{1:end-1}];
%% Clear temporary variables
clearvars filename delimiter formatSpec fileID dataArray ans;
%% Convert indexing and replace brackets
% replace 1-D indexes
for arrayIndex = 1:99
strIndex = int2str(arrayIndex);
strRep = sprintf('[%d]',(arrayIndex-1));
strPat = strcat('\(',strIndex,'\)');
for lineIndex = 1:length(SymbolicOutput)
str = char(SymbolicOutput(lineIndex));
SymbolicOutput(lineIndex) = {regexprep(str, strPat, strRep)};
end
end
% replace 2-D left indexes
for arrayIndex = 1:99
strIndex = int2str(arrayIndex);
strRep = sprintf('[%d,',(arrayIndex-1));
strPat = strcat('\(',strIndex,'\,');
for lineIndex = 1:length(SymbolicOutput)
str = char(SymbolicOutput(lineIndex));
SymbolicOutput(lineIndex) = {regexprep(str, strPat, strRep)};
end
end
% replace 2-D right indexes
for arrayIndex = 1:99
strIndex = int2str(arrayIndex);
strRep = sprintf(',%d]',(arrayIndex-1));
strPat = strcat('\,',strIndex,'\)');
for lineIndex = 1:length(SymbolicOutput)
str = char(SymbolicOutput(lineIndex));
SymbolicOutput(lineIndex) = {regexprep(str, strPat, strRep)};
end
end
% replace commas
for lineIndex = 1:length(SymbolicOutput)
str = char(SymbolicOutput(lineIndex));
SymbolicOutput(lineIndex) = {regexprep(str, '\,', '][')};
end
%% replace . operators
for lineIndex = 1:length(SymbolicOutput)
strIn = char(SymbolicOutput(lineIndex));
strIn = regexprep(strIn,'\.\*','\*');
strIn = regexprep(strIn,'\.\/','\/');
strIn = regexprep(strIn,'\.\^','\^');
SymbolicOutput(lineIndex) = cellstr(strIn);
end
%% Replace ^2
% replace where adjacent to ) parenthesis
for lineIndex = 1:length(SymbolicOutput)
idxsq = regexp(SymbolicOutput(lineIndex),'\)\^2');
if ~isempty(idxsq{1})
strIn = SymbolicOutput(lineIndex);
idxlp = regexp(strIn,'\(');
idxrp = regexp(strIn,'\)');
for pwrIndex = 1:length(idxsq{1})
counter = 1;
index = idxsq{1}(pwrIndex);
endIndex(pwrIndex) = index;
while (counter > 0 && index > 0)
index = index - 1;
counter = counter + ~isempty(find(idxrp{1} == index, 1));
counter = counter - ~isempty(find(idxlp{1} == index, 1));
end
startIndex(pwrIndex) = index;
% strPat = strcat(strIn{1}(startIndex(pwrIndex):endIndex(pwrIndex)),'^2');
strRep = strcat('sq',strIn{1}(startIndex(pwrIndex):endIndex(pwrIndex)));
% cellStrPat(pwrIndex) = cellstr(strPat);
cellStrRep(pwrIndex) = cellstr(strRep);
end
for pwrIndex = 1:length(idxsq{1})
strRep = char(cellStrRep(pwrIndex));
strIn{1}(startIndex(pwrIndex):endIndex(pwrIndex)+2) = strRep;
end
SymbolicOutput(lineIndex) = strIn;
end
end
% replace where adjacent to ] parenthesis
for lineIndex = 1:length(SymbolicOutput)
strIn = char(SymbolicOutput(lineIndex));
[match,idxsq1,idxsq2] = regexp(strIn,'\w*\[\w*\]\^2','match','start','end');
[idxsq3] = regexp(strIn,'\[\w*\]\^2','start');
if ~isempty(match)
for pwrIndex = 1:length(match)
strVar = strIn(idxsq1(pwrIndex):idxsq3(pwrIndex)-1);
strIndex = strIn(idxsq3(pwrIndex)+1:idxsq2(pwrIndex)-3);
strPat = strcat(strVar,'\[',strIndex,'\]\^2');
strRep = strcat('sq(',strVar,'[',strIndex,']',')');
strIn = regexprep(strIn,strPat,strRep);
end
SymbolicOutput(lineIndex) = cellstr(strIn);
end
end
% replace where adjacent to alpanumeric characters
for lineIndex = 1:length(SymbolicOutput)
strIn = char(SymbolicOutput(lineIndex));
[match,idxsq1,idxsq2] = regexp(strIn,'\w*\^2','match','start','end');
[idxsq3] = regexp(strIn,'\^2','start');
if ~isempty(match)
for pwrIndex = 1:length(match)
strVar = strIn(idxsq1(pwrIndex)+2*(pwrIndex-1):idxsq2(pwrIndex)-2+2*(pwrIndex-1));
strPat = strcat(strVar,'\^2');
strRep = strcat('sq(',strVar,')');
strIn = regexprep(strIn,strPat,strRep);
end
SymbolicOutput(lineIndex) = cellstr(strIn);
end
end
%% Replace ^(1/2)
% replace where adjacent to ) parenthesis
for lineIndex = 1:length(SymbolicOutput)
idxsq = regexp(SymbolicOutput(lineIndex),'\)\^\(1\/2\)');
if ~isempty(idxsq{1})
strIn = SymbolicOutput(lineIndex);
idxlp = regexp(strIn,'\(');
idxrp = regexp(strIn,'\)');
for pwrIndex = 1:length(idxsq{1})
counter = 1;
index = idxsq{1}(pwrIndex);
endIndex(pwrIndex) = index;
while (counter > 0 && index > 0)
index = index - 1;
counter = counter + ~isempty(find(idxrp{1} == index, 1));
counter = counter - ~isempty(find(idxlp{1} == index, 1));
end
startIndex(pwrIndex) = index;
% strPat = strcat(strIn{1}(startIndex(pwrIndex):endIndex(pwrIndex)),'^2');
strRep = strcat('(sqrt',strIn{1}(startIndex(pwrIndex):endIndex(pwrIndex)),')');
% cellStrPat(pwrIndex) = cellstr(strPat);
cellStrRep(pwrIndex) = cellstr(strRep);
end
for pwrIndex = 1:length(idxsq{1})
strRep = char(cellStrRep(pwrIndex));
strIn{1}(startIndex(pwrIndex):endIndex(pwrIndex)+6) = strRep;
end
SymbolicOutput(lineIndex) = strIn;
end
end
%% Replace Divisions
% Compiler looks after this type of optimisation for us
% for lineIndex = 1:length(SymbolicOutput)
% strIn = char(SymbolicOutput(lineIndex));
% strIn = regexprep(strIn,'\/2','\*0\.5');
% strIn = regexprep(strIn,'\/4','\*0\.25');
% SymbolicOutput(lineIndex) = cellstr(strIn);
% end
%% Convert declarations
for lineIndex = 1:length(SymbolicOutput)
str = char(SymbolicOutput(lineIndex));
if ~isempty(regexp(str,'zeros', 'once'))
index1 = regexp(str,' = zeros[','once')-1;
index2 = regexp(str,' = zeros[','end','once')+1;
index3 = regexp(str,'\]\[','once')-1;
index4 = index3 + 3;
index5 = max(regexp(str,'\]'))-1;
str1 = {'float '};
str2 = str(1:index1);
str3 = '[';
str4 = str(index2:index3);
str4 = num2str(str2num(str4)+1);
str5 = '][';
str6 = str(index4:index5);
str6 = num2str(str2num(str6)+1);
str7 = '];';
SymbolicOutput(lineIndex) = strcat(str1,str2,str3,str4,str5,str6,str7);
end
end
%% Change covariance matrix variable name to P
for lineIndex = 1:length(SymbolicOutput)
strIn = char(SymbolicOutput(lineIndex));
strIn = regexprep(strIn,'OP\[','P[');
SymbolicOutput(lineIndex) = cellstr(strIn);
end
%% Write to file
fileName = strcat(fileName,'.cpp');
fid = fopen(fileName,'wt');
for lineIndex = 1:length(SymbolicOutput)
fprintf(fid,char(SymbolicOutput(lineIndex)));
fprintf(fid,'\n');
end
fclose(fid);
clear all;

46
libraries/AP_NavEKF/Models/Common/ConvertToM.m Normal file
View File

@ -0,0 +1,46 @@
function ConvertToM(nStates)
%% Initialize variables
fileName = strcat('SymbolicOutput',int2str(nStates),'.txt');
delimiter = '';
%% Format string for each line of text:
% column1: text (%s)
% For more information, see the TEXTSCAN documentation.
formatSpec = '%s%[^\n\r]';
%% Open the text file.
fileID = fopen(fileName,'r');
%% Read columns of data according to format string.
% This call is based on the structure of the file used to generate this
% code. If an error occurs for a different file, try regenerating the code
% from the Import Tool.
dataArray = textscan(fileID, formatSpec, 'Delimiter', delimiter, 'ReturnOnError', false,'Bufsize',65535);
%% Close the text file.
fclose(fileID);
%% Create output variable
SymbolicOutput = [dataArray{1:end-1}];
%% Clear temporary variables
clearvars filename delimiter formatSpec fileID dataArray ans;
%% replace brackets and commas
for lineIndex = 1:length(SymbolicOutput)
SymbolicOutput(lineIndex) = regexprep(SymbolicOutput(lineIndex), '_l_', '(');
SymbolicOutput(lineIndex) = regexprep(SymbolicOutput(lineIndex), '_c_', ',');
SymbolicOutput(lineIndex) = regexprep(SymbolicOutput(lineIndex), '_r_', ')');
end
%% Write to file
fileName = strcat('M_code',int2str(nStates),'.txt');
fid = fopen(fileName,'wt');
for lineIndex = 1:length(SymbolicOutput)
fprintf(fid,char(SymbolicOutput(lineIndex)));
fprintf(fid,'\n');
end
fclose(fid);
clear all;
end

23
libraries/AP_NavEKF/Models/Common/EulToQuat.m Normal file
View File

@ -0,0 +1,23 @@
function quaterion = EulToQuat(Euler)
% Convert from a 321 Euler rotation sequence specified in radians to a
% Quaternion
quaterion = zeros(4,1);
Euler = Euler * 0.5;
cosPhi = cos(Euler(1));
sinPhi = sin(Euler(1));
cosTheta = cos(Euler(2));
sinTheta = sin(Euler(2));
cosPsi = cos(Euler(3));
sinPsi = sin(Euler(3));
quaterion(1,1) = (cosPhi*cosTheta*cosPsi + sinPhi*sinTheta*sinPsi);
quaterion(2,1) = (sinPhi*cosTheta*cosPsi - cosPhi*sinTheta*sinPsi);
quaterion(3,1) = (cosPhi*sinTheta*cosPsi + sinPhi*cosTheta*sinPsi);
quaterion(4,1) = (cosPhi*cosTheta*sinPsi - sinPhi*sinTheta*cosPsi);
return;

5
libraries/AP_NavEKF/Models/Common/NormQuat.m Normal file
View File

@ -0,0 +1,5 @@
% normalise the quaternion
function quaternion = normQuat(quaternion)
quatMag = sqrt(quaternion(1)^2 + quaternion(2)^2 + quaternion(3)^2 + quaternion(4)^2);
quaternion(1:4) = quaternion / quatMag;

29
libraries/AP_NavEKF/Models/Common/OptimiseAlgebra.m Normal file
View File

@ -0,0 +1,29 @@
function [SymExpOut,SubExpArray] = OptimiseAlgebra(SymExpIn,SubExpName)
% Loop through symbolic expression, identifying repeated expressions and
% bringing them out as shared expression or sub expressions
% do this until no further repeated expressions found
% This can significantly reduce computations
syms SubExpIn SubExpArray ;
SubExpArray(1,1) = 'invalid';
index = 0;
f_complete = 0;
while f_complete==0
index = index + 1;
SubExpIn = [SubExpName,'(',num2str(index),')'];
SubExpInStore{index} = SubExpIn;
[SymExpOut,SubExpOut]=subexpr(SymExpIn,SubExpIn);
for k = 1:index
if SubExpOut == SubExpInStore{k}
f_complete = 1;
end
end
if f_complete || index > 100
SymExpOut = SymExpIn;
else
SubExpArray(index,1) = SubExpOut;
SymExpIn = SymExpOut;
end
end

14
libraries/AP_NavEKF/Models/Common/Quat2Tbn.m Normal file
View File

@ -0,0 +1,14 @@
function Tbn = Quat2Tbn(quat)
% Convert from quaternions defining the flight vehicles rotation to
% the direction cosine matrix defining the rotation from body to navigation
% coordinates
q0 = quat(1);
q1 = quat(2);
q2 = quat(3);
q3 = quat(4);
Tbn = [q0^2 + q1^2 - q2^2 - q3^2, 2*(q1*q2 - q0*q3), 2*(q1*q3 + q0*q2); ...
2*(q1*q2 + q0*q3), q0^2 - q1^2 + q2^2 - q3^2, 2*(q2*q3 - q0*q1); ...
2*(q1*q3-q0*q2), 2*(q2*q3 + q0*q1), q0^2 - q1^2 - q2^2 + q3^2];

16
libraries/AP_NavEKF/Models/Common/QuatDivide.m Normal file
View File

@ -0,0 +1,16 @@
function q_out = QuatDivide(qin1,qin2)
q0 = qin1(1);
q1 = qin1(2);
q2 = qin1(3);
q3 = qin1(4);
r0 = qin2(1);
r1 = qin2(2);
r2 = qin2(3);
r3 = qin2(4);
q_out(1,1) = (qin2(1)*qin1(1) + qin2(2)*qin1(2) + qin2(3)*qin1(3) + qin2(4)*qin1(4));
q_out(2,1) = (r0*q1 - r1*q0 - r2*q3 + r3*q2);
q_out(3,1) = (r0*q2 + r1*q3 - r2*q0 - r3*q1);
q_out(4,1) = (r0*q3 - r1*q2 + r2*q1 - r3*q0);

5
libraries/AP_NavEKF/Models/Common/QuatMult.m Normal file
View File

@ -0,0 +1,5 @@
function quatOut = QuatMult(quatA,quatB)
% Calculate the following quaternion product quatA * quatB using the
% standard identity
quatOut = [quatA(1)*quatB(1)-quatA(2:4)'*quatB(2:4); quatA(1)*quatB(2:4) + quatB(1)*quatA(2:4) + cross(quatA(2:4),quatB(2:4))];

9
libraries/AP_NavEKF/Models/Common/QuatToEul.m Normal file
View File

@ -0,0 +1,9 @@
% Convert from a quaternion to a 321 Euler rotation sequence in radians
function Euler = QuatToEul(quat)
Euler = zeros(3,1);
Euler(1) = atan2(2*(quat(3)*quat(4)+quat(1)*quat(2)), quat(1)*quat(1) - quat(2)*quat(2) - quat(3)*quat(3) + quat(4)*quat(4));
Euler(2) = -asin(2*(quat(2)*quat(4)-quat(1)*quat(3)));
Euler(3) = atan2(2*(quat(2)*quat(3)+quat(1)*quat(4)), quat(1)*quat(1) + quat(2)*quat(2) - quat(3)*quat(3) - quat(4)*quat(4));

10
libraries/AP_NavEKF/Models/Common/RotToQuat.m Normal file
View File

@ -0,0 +1,10 @@
% convert froma rotation vector in radians to a quaternion
function quaternion = RotToQuat(rotVec)
vecLength = sqrt(rotVec(1)^2 + rotVec(2)^2 + rotVec(3)^2);
if vecLength < 1e-6
quaternion = [1;0;0;0];
else
quaternion = [cos(0.5*vecLength); rotVec/vecLength*sin(0.5*vecLength)];
end

51
libraries/AP_NavEKF/Models/GimbalEstimatorExample/AlignHeading.m Normal file
View File

@ -0,0 +1,51 @@
function quat = AlignHeading( ...
gPhi, ... % gimbal roll angle
gPsi, ... % gimbal yaw angle
gTheta, ... % gimbal pitch angle
Tsn, ... % sensor to NED rotation matrix
magMea, ... % body frame magnetic flux measurements
quat, ... % quaternion defining rotation from sensor to NED axes
declination) % Estimated magnetic field delination at current location
% calculate rotation from magnetometer to NED axes
Tmn = calcTmn(gPhi,gPsi,gTheta,quat(1),quat(2),quat(3),quat(4));
% Calculate the predicted magnetic declination
magMeasNED = Tmn*magMea;
predDec = atan2(magMeasNED(2),magMeasNED(1));
% Calculate the measurement innovation
innovation = predDec - declination;
if (innovation > pi)
innovation = innovation - 2*pi;
elseif (innovation < -pi)
innovation = innovation + 2*pi;
end
% form the NED rotation vector
deltaRotNED = -[0;0;innovation];
% rotate into sensor axes
deltaRotBody = transpose(Tsn)*deltaRotNED;
% Convert the error rotation vector to its equivalent quaternion
% error = truth - estimate
rotationMag = abs(innovation);
if rotationMag<1e-6
deltaQuat = single([1;0;0;0]);
else
deltaQuat = [cos(0.5*rotationMag); [deltaRotBody(1);deltaRotBody(2);deltaRotBody(3)]/rotationMag*sin(0.5*rotationMag)];
end
% Update the quaternion states by rotating from the previous attitude through
% the delta angle rotation quaternion
quat = [quat(1)*deltaQuat(1)-transpose(quat(2:4))*deltaQuat(2:4); quat(1)*deltaQuat(2:4) + deltaQuat(1)*quat(2:4) + cross(quat(2:4),deltaQuat(2:4))];
% normalise the updated quaternion states
quatMag = sqrt(quat(1)^2 + quat(2)^2 + quat(3)^2 + quat(4)^2);
if (quatMag > 1e-12)
quat = quat / quatMag;
end
end

20
libraries/AP_NavEKF/Models/GimbalEstimatorExample/AlignTilt.m Normal file
View File

@ -0,0 +1,20 @@
function quat = AlignTilt( ...
quat, ... % quaternion state vector
initAccel) % initial accelerometer vector
% check length
lengthAccel = sqrt(dot([initAccel(1);initAccel(2);initAccel(3)],[initAccel(1);initAccel(2);initAccel(3)]));
% if length is > 0.7g and < 1.4g initialise tilt using gravity vector
if (lengthAccel > 5 && lengthAccel < 14)
% calculate length of the tilt vector
tiltMagnitude = atan2(sqrt(dot([initAccel(1);initAccel(2)],[initAccel(1);initAccel(2)])),-initAccel(3));
% take the unit cross product of the accel vector and the -Z vector to
% give the tilt unit vector
if (tiltMagnitude > 1e-3)
tiltUnitVec = cross([initAccel(1);initAccel(2);initAccel(3)],[0;0;-1]);
tiltUnitVec = tiltUnitVec/sqrt(dot(tiltUnitVec,tiltUnitVec));
tiltVec = tiltMagnitude*tiltUnitVec;
quat = [cos(0.5*tiltMagnitude); tiltVec/tiltMagnitude*sin(0.5*tiltMagnitude)];
end
end
end

347
libraries/AP_NavEKF/Models/GimbalEstimatorExample/C_code.txt Normal file
View File

@ -0,0 +1,347 @@
t2 = dax*(1.0/2.0);
t3 = dax_b*(1.0/2.0);
t4 = t2-t3;
t5 = day*(1.0/2.0);
t6 = day_b*(1.0/2.0);
t7 = t5-t6;
t8 = daz*(1.0/2.0);
t9 = daz_b*(1.0/2.0);
t10 = t8-t9;
t11 = q2*t4*(1.0/2.0);
t12 = q1*t7*(1.0/2.0);
t13 = q0*t10*(1.0/2.0);
t14 = q2*(1.0/2.0);
t15 = q3*t4*(1.0/2.0);
t16 = q1*t10*(1.0/2.0);
t17 = q1*(1.0/2.0);
t18 = q0*t4*(1.0/2.0);
t19 = q3*t7*(1.0/2.0);
t20 = q0*(1.0/2.0);
t21 = q2*t7*(1.0/2.0);
t22 = q3*t10*(1.0/2.0);
t23 = q0*t7*(1.0/2.0);
t24 = q3*(1.0/2.0);
t25 = q1*t4*(1.0/2.0);
t26 = q2*t10*(1.0/2.0);
t27 = t11+t12+t13-t24;
t28 = t14+t15+t16-t23;
t29 = q2*t28*2.0;
t30 = t17+t18+t19-t26;
t31 = q1*t30*2.0;
t32 = t20+t21+t22-t25;
t33 = q0*t32*2.0;
t40 = q3*t27*2.0;
t34 = t29+t31+t33-t40;
t35 = q0*q0;
t36 = q1*q1;
t37 = q2*q2;
t38 = q3*q3;
t39 = t35+t36+t37+t38;
t41 = t11+t12-t13+t24;
t42 = t14-t15+t16+t23;
t43 = q1*t42*2.0;
t44 = -t17+t18+t19+t26;
t45 = q2*t44*2.0;
t46 = t20-t21+t22+t25;
t47 = q3*t46*2.0;
t57 = q0*t41*2.0;
t48 = t43+t45+t47-t57;
t49 = -t14+t15+t16+t23;
t50 = q0*t49*2.0;
t51 = t11-t12+t13+t24;
t52 = t20+t21-t22+t25;
t53 = q2*t52*2.0;
t54 = t17-t18+t19+t26;
t55 = q3*t54*2.0;
t58 = q1*t51*2.0;
t56 = t50+t53+t55-t58;
t59 = OP[0][0]*t34;
t60 = OP[1][0]*t48;
t66 = OP[6][0]*t39;
t67 = OP[2][0]*t56;
t61 = t59+t60-t66-t67;
t62 = OP[0][1]*t34;
t63 = OP[1][1]*t48;
t64 = OP[0][2]*t34;
t65 = OP[1][2]*t48;
t71 = OP[6][1]*t39;
t72 = OP[2][1]*t56;
t68 = t62+t63-t71-t72;
t79 = OP[6][2]*t39;
t80 = OP[2][2]*t56;
t69 = t64+t65-t79-t80;
t70 = t35+t36-t37-t38;
t73 = q0*q2*2.0;
t74 = q1*q3*2.0;
t75 = t73+t74;
t76 = q0*q3*2.0;
t78 = q1*q2*2.0;
t77 = t76-t78;
t81 = t35-t36+t37-t38;
t82 = q0*q1*2.0;
t86 = q2*q3*2.0;
t83 = t82-t86;
t84 = t76+t78;
t85 = t35-t36-t37+t38;
t87 = t82+t86;
t88 = t73-t74;
t89 = OP[0][6]*t34;
t90 = OP[1][6]*t48;
t265 = OP[6][6]*t39;
t91 = t89+t90-t265-OP[2][6]*t56;
t92 = OP[0][7]*t34;
t93 = OP[1][7]*t48;
t266 = OP[6][7]*t39;
t94 = t92+t93-t266-OP[2][7]*t56;
t95 = OP[0][8]*t34;
t96 = OP[1][8]*t48;
t267 = OP[6][8]*t39;
t97 = t95+t96-t267-OP[2][8]*t56;
t98 = q0*t27*2.0;
t99 = q1*t28*2.0;
t100 = q3*t32*2.0;
t110 = q2*t30*2.0;
t101 = t98+t99+t100-t110;
t102 = q0*t46*2.0;
t103 = q2*t42*2.0;
t104 = q3*t41*2.0;
t111 = q1*t44*2.0;
t105 = t102+t103+t104-t111;
t106 = q1*t52*2.0;
t107 = q2*t51*2.0;
t108 = q3*t49*2.0;
t112 = q0*t54*2.0;
t109 = t106+t107+t108-t112;
t113 = OP[7][0]*t39;
t114 = OP[0][0]*t101;
t123 = OP[1][0]*t105;
t124 = OP[2][0]*t109;
t115 = t113+t114-t123-t124;
t116 = OP[7][1]*t39;
t117 = OP[0][1]*t101;
t129 = OP[1][1]*t105;
t130 = OP[2][1]*t109;
t118 = t116+t117-t129-t130;
t119 = OP[7][2]*t39;
t120 = OP[0][2]*t101;
t135 = OP[1][2]*t105;
t136 = OP[2][2]*t109;
t121 = t119+t120-t135-t136;
t122 = t39*t39;
t125 = q1*t27*2.0;
t126 = q2*t32*2.0;
t127 = q3*t30*2.0;
t170 = q0*t28*2.0;
t128 = t125+t126+t127-t170;
t131 = q0*t44*2.0;
t132 = q1*t46*2.0;
t133 = q2*t41*2.0;
t171 = q3*t42*2.0;
t134 = t131+t132+t133-t171;
t137 = q0*t52*2.0;
t138 = q1*t54*2.0;
t139 = q3*t51*2.0;
t172 = q2*t49*2.0;
t140 = t137+t138+t139-t172;
t141 = dvy*t70;
t142 = dvx*t77;
t143 = t141+t142;
t144 = dvx*t75;
t145 = dvy*t75;
t146 = dvz*t77;
t147 = t145+t146;
t148 = dvx*t81;
t188 = dvy*t84;
t149 = t148-t188;
t150 = dvz*t81;
t151 = dvy*t83;
t152 = t150+t151;
t153 = dvx*t83;
t154 = dvz*t84;
t155 = t153+t154;
t156 = dvx*t85;
t157 = dvz*t88;
t158 = t156+t157;
t159 = dvy*t85;
t189 = dvz*t87;
t160 = t159-t189;
t161 = dvx*t87;
t162 = dvy*t88;
t163 = t161+t162;
t164 = OP[7][6]*t39;
t165 = OP[0][6]*t101;
t166 = OP[7][7]*t39;
t167 = OP[0][7]*t101;
t168 = OP[7][8]*t39;
t169 = OP[0][8]*t101;
t173 = OP[8][0]*t39;
t174 = OP[1][0]*t134;
t182 = OP[0][0]*t128;
t183 = OP[2][0]*t140;
t175 = t173+t174-t182-t183;
t176 = OP[8][1]*t39;
t177 = OP[1][1]*t134;
t184 = OP[0][1]*t128;
t185 = OP[2][1]*t140;
t178 = t176+t177-t184-t185;
t179 = OP[8][2]*t39;
t180 = OP[1][2]*t134;
t186 = OP[0][2]*t128;
t187 = OP[2][2]*t140;
t181 = t179+t180-t186-t187;
t190 = OP[8][6]*t39;
t191 = OP[1][6]*t134;
t192 = OP[8][7]*t39;
t193 = OP[1][7]*t134;
t194 = OP[8][8]*t39;
t195 = OP[1][8]*t134;
t197 = dvz*t70;
t196 = t144-t197;
t198 = OP[0][0]*t147;
t204 = OP[2][0]*t143;
t205 = OP[1][0]*t196;
t199 = OP[3][0]+t198-t204-t205;
t200 = OP[0][1]*t147;
t206 = OP[2][1]*t143;
t207 = OP[1][1]*t196;
t201 = OP[3][1]+t200-t206-t207;
t202 = OP[0][2]*t147;
t208 = OP[2][2]*t143;
t209 = OP[1][2]*t196;
t203 = OP[3][2]+t202-t208-t209;
t210 = -t144+t197;
t211 = OP[1][0]*t210;
t212 = OP[3][0]+t198-t204+t211;
t213 = OP[1][1]*t210;
t214 = OP[3][1]+t200-t206+t213;
t215 = OP[1][2]*t210;
t216 = OP[3][2]+t202-t208+t215;
t217 = OP[0][6]*t147;
t218 = OP[0][7]*t147;
t219 = OP[0][8]*t147;
t220 = OP[1][0]*t155;
t221 = OP[2][0]*t149;
t229 = OP[0][0]*t152;
t222 = OP[4][0]+t220+t221-t229;
t223 = OP[1][1]*t155;
t224 = OP[2][1]*t149;
t230 = OP[0][1]*t152;
t225 = OP[4][1]+t223+t224-t230;
t226 = OP[1][2]*t155;
t227 = OP[2][2]*t149;
t231 = OP[0][2]*t152;
t228 = OP[4][2]+t226+t227-t231;
t232 = dvxNoise*t70*t84;
t233 = OP[1][6]*t155;
t234 = OP[2][6]*t149;
t235 = OP[4][6]+t233+t234-OP[0][6]*t152;
t236 = OP[1][7]*t155;
t237 = OP[2][7]*t149;
t238 = OP[4][7]+t236+t237-OP[0][7]*t152;
t239 = OP[1][8]*t155;
t240 = OP[2][8]*t149;
t241 = OP[4][8]+t239+t240-OP[0][8]*t152;
t242 = OP[2][0]*t163;
t243 = OP[0][0]*t160;
t251 = OP[1][0]*t158;
t244 = OP[5][0]+t242+t243-t251;
t245 = OP[2][1]*t163;
t246 = OP[0][1]*t160;
t252 = OP[1][1]*t158;
t247 = OP[5][1]+t245+t246-t252;
t248 = OP[2][2]*t163;
t249 = OP[0][2]*t160;
t253 = OP[1][2]*t158;
t250 = OP[5][2]+t248+t249-t253;
t254 = dvzNoise*t75*t85;
t255 = dvyNoise*t81*t87;
t256 = OP[2][6]*t163;
t257 = OP[0][6]*t160;
t258 = OP[5][6]+t256+t257-OP[1][6]*t158;
t259 = OP[2][7]*t163;
t260 = OP[0][7]*t160;
t261 = OP[5][7]+t259+t260-OP[1][7]*t158;
t262 = OP[2][8]*t163;
t263 = OP[0][8]*t160;
t264 = OP[5][8]+t262+t263-OP[1][8]*t158;
A0[0][0] = daxNoise*t122+t34*t61+t48*t68-t56*t69-t39*t91;
A0[0][0] = -t39*t94-t61*t101+t68*t105+t69*t109;
A0[0][1] = -t39*t97-t68*t134+t69*t140+t128*(t59+t60-t66-t67);
A0[0][2] = OP[0][3]*t34+OP[1][3]*t48-OP[2][3]*t56-OP[6][3]*t39-t69*t143+t147*(t59+t60-t66-t67)-t196*(t62+t63-t71-t72);
A0[0][3] = OP[0][4]*t34+OP[1][4]*t48-OP[2][4]*t56-OP[6][4]*t39-t61*t152+t69*t149+t155*(t62+t63-t71-t72);
A0[0][4] = OP[0][5]*t34+OP[1][5]*t48-OP[2][5]*t56-OP[6][5]*t39+t61*t160-t68*t158+t163*(t64+t65-t79-t80);
A0[0][5] = t91;
A0[0][6] = t94;
A0[0][7] = t97;
A0[0][0] = -t34*t115-t48*t118+t56*t121+t39*(t164+t165-OP[1][6]*t105-OP[2][6]*t109);
A0[0][0] = dayNoise*t122+t101*t115-t105*t118-t109*t121+t39*(t166+t167-OP[1][7]*t105-OP[2][7]*t109);
A0[0][1] = -t115*t128+t118*t134-t121*t140+t39*(t168+t169-OP[1][8]*t105-OP[2][8]*t109);
A0[0][2] = -OP[0][3]*t101-OP[7][3]*t39+OP[1][3]*t105+OP[2][3]*t109-t115*t147+t121*t143+t118*t196;
A0[0][3] = -OP[0][4]*t101-OP[7][4]*t39+OP[1][4]*t105+OP[2][4]*t109+t115*t152-t121*t149-t118*t155;
A0[0][4] = -OP[0][5]*t101-OP[7][5]*t39+OP[1][5]*t105+OP[2][5]*t109-t115*t160+t118*t158-t121*t163;
A0[0][5] = -t164-t165+OP[1][6]*t105+OP[2][6]*t109;
A0[0][6] = -t166-t167+OP[1][7]*t105+OP[2][7]*t109;
A0[0][7] = -t168-t169+OP[1][8]*t105+OP[2][8]*t109;
A0[1][0] = -t34*t175-t48*t178+t56*t181+t39*(t190+t191-OP[0][6]*t128-OP[2][6]*t140);
A0[1][0] = t101*t175-t105*t178-t109*t181+t39*(t192+t193-OP[0][7]*t128-OP[2][7]*t140);
A0[1][1] = dazNoise*t122-t128*t175+t134*t178-t140*t181+t39*(t194+t195-OP[0][8]*t128-OP[2][8]*t140);
A0[1][2] = -OP[8][3]*t39+OP[0][3]*t128-OP[1][3]*t134+OP[2][3]*t140-t147*t175+t143*t181+t178*t196;
A0[1][3] = -OP[8][4]*t39+OP[0][4]*t128-OP[1][4]*t134+OP[2][4]*t140+t152*t175-t149*t181-t155*t178;
A0[1][4] = -OP[8][5]*t39+OP[0][5]*t128-OP[1][5]*t134+OP[2][5]*t140-t160*t175+t158*t178-t163*t181;
A0[1][5] = -t190-t191+OP[0][6]*t128+OP[2][6]*t140;
A0[1][6] = -t192-t193+OP[0][7]*t128+OP[2][7]*t140;
A0[1][7] = -t194-t195+OP[0][8]*t128+OP[2][8]*t140;
A0[2][0] = -t39*(OP[3][6]+t217-OP[2][6]*t143-OP[1][6]*t196)+t34*t199+t48*t201-t56*t203;
A0[2][0] = -t39*(OP[3][7]+t218-OP[2][7]*t143-OP[1][7]*t196)-t101*t199+t105*t201+t109*t203;
A0[2][1] = -t39*(OP[3][8]+t219-OP[2][8]*t143-OP[1][8]*t196)+t128*t199-t134*t201+t140*t203;
A0[2][2] = OP[3][3]+OP[0][3]*t147-OP[2][3]*t143+OP[1][3]*t210-t143*t203+t147*t212+t210*t214+dvxNoise*(t70*t70)+dvyNoise*(t77*t77)+dvzNoise*(t75*t75);
A0[2][3] = OP[3][4]+t232+OP[0][4]*t147-OP[2][4]*t143+OP[1][4]*t210-t152*t212+t149*t216+t155*t214-dvyNoise*t77*t81-dvzNoise*t75*t83;
A0[2][4] = OP[3][5]+t254+OP[0][5]*t147-OP[2][5]*t143+OP[1][5]*t210-t158*t214+t160*t212+t163*t216-dvxNoise*t70*t88-dvyNoise*t77*t87;
A0[2][5] = OP[3][6]+t217-OP[2][6]*t143+OP[1][6]*t210;
A0[2][6] = OP[3][7]+t218-OP[2][7]*t143+OP[1][7]*t210;
A0[2][7] = OP[3][8]+t219-OP[2][8]*t143+OP[1][8]*t210;
A0[3][0] = t34*t222+t48*t225-t39*t235-t56*t228;
A0[3][0] = -t39*t238-t101*t222+t105*t225+t109*t228;
A0[3][1] = -t39*t241+t128*t222-t134*t225+t140*t228;
A0[3][2] = OP[4][3]+t232-OP[0][3]*t152+OP[1][3]*t155+OP[2][3]*t149+t147*t222-t143*t228+t210*t225-dvyNoise*t77*t81-dvzNoise*t75*t83;
A0[3][3] = OP[4][4]-OP[0][4]*t152+OP[1][4]*t155+OP[2][4]*t149-t152*t222+t149*t228+t155*t225+dvxNoise*(t84*t84)+dvyNoise*(t81*t81)+dvzNoise*(t83*t83);
A0[3][4] = OP[4][5]+t255-OP[0][5]*t152+OP[1][5]*t155+OP[2][5]*t149+t160*t222-t158*t225+t163*t228-dvxNoise*t84*t88-dvzNoise*t83*t85;
A0[3][5] = t235;
A0[3][6] = t238;
A0[3][7] = t241;
A0[4][0] = t34*t244+t48*t247-t39*t258-t56*t250;
A0[4][0] = -t39*t261-t101*t244+t105*t247+t109*t250;
A0[4][1] = -t39*t264+t128*t244-t134*t247+t140*t250;
A0[4][2] = OP[5][3]+t254+OP[0][3]*t160-OP[1][3]*t158+OP[2][3]*t163+t147*t244-t143*t250+t210*t247-dvxNoise*t70*t88-dvyNoise*t77*t87;
A0[4][3] = OP[5][4]+t255+OP[0][4]*t160-OP[1][4]*t158+OP[2][4]*t163-t152*t244+t149*t250+t155*t247-dvxNoise*t84*t88-dvzNoise*t83*t85;
A0[4][4] = OP[5][5]+OP[0][5]*t160-OP[1][5]*t158+OP[2][5]*t163+t160*t244-t158*t247+t163*t250+dvxNoise*(t88*t88)+dvyNoise*(t87*t87)+dvzNoise*(t85*t85);
A0[4][5] = t258;
A0[4][6] = t261;
A0[4][7] = t264;
A0[5][0] = -t265+OP[6][0]*t34+OP[6][1]*t48-OP[6][2]*t56;
A0[5][0] = -t266-OP[6][0]*t101+OP[6][1]*t105+OP[6][2]*t109;
A0[5][1] = -t267+OP[6][0]*t128-OP[6][1]*t134+OP[6][2]*t140;
A0[5][2] = OP[6][3]-OP[6][2]*t143+OP[6][0]*t147+OP[6][1]*t210;
A0[5][3] = OP[6][4]+OP[6][2]*t149-OP[6][0]*t152+OP[6][1]*t155;
A0[5][4] = OP[6][5]-OP[6][1]*t158+OP[6][0]*t160+OP[6][2]*t163;
A0[5][5] = OP[6][6];
A0[5][6] = OP[6][7];
A0[5][7] = OP[6][8];
A0[6][0] = -t164+OP[7][0]*t34+OP[7][1]*t48-OP[7][2]*t56;
A0[6][0] = -t166-OP[7][0]*t101+OP[7][1]*t105+OP[7][2]*t109;
A0[6][1] = -t168+OP[7][0]*t128-OP[7][1]*t134+OP[7][2]*t140;
A0[6][2] = OP[7][3]-OP[7][2]*t143+OP[7][0]*t147+OP[7][1]*t210;
A0[6][3] = OP[7][4]+OP[7][2]*t149-OP[7][0]*t152+OP[7][1]*t155;
A0[6][4] = OP[7][5]-OP[7][1]*t158+OP[7][0]*t160+OP[7][2]*t163;
A0[6][5] = OP[7][6];
A0[6][6] = OP[7][7];
A0[6][7] = OP[7][8];
A0[7][0] = -t190+OP[8][0]*t34+OP[8][1]*t48-OP[8][2]*t56;
A0[7][0] = -t192-OP[8][0]*t101+OP[8][1]*t105+OP[8][2]*t109;
A0[7][1] = -t194+OP[8][0]*t128-OP[8][1]*t134+OP[8][2]*t140;
A0[7][2] = OP[8][3]-OP[8][2]*t143+OP[8][0]*t147+OP[8][1]*t210;
A0[7][3] = OP[8][4]+OP[8][2]*t149-OP[8][0]*t152+OP[8][1]*t155;
A0[7][4] = OP[8][5]-OP[8][1]*t158+OP[8][0]*t160+OP[8][2]*t163;
A0[7][5] = OP[8][6];
A0[7][6] = OP[8][7];
A0[7][7] = OP[8][8];

94
libraries/AP_NavEKF/Models/GimbalEstimatorExample/FixAutoGenCCode.m Normal file
View File

@ -0,0 +1,94 @@
function FixAutoGenCCode(fileName)
%% Initialize variables
delimiter = '';
%% Format string for each line of text:
% column1: text (%s)
% For more information, see the TEXTSCAN documentation.
formatSpec = '%s%[^\n\r]';
%% Open the text file.
fileID = fopen(strcat(fileName,'.c'),'r');
%% Read columns of data according to format string.
% This call is based on the structure of the file used to generate this
% code. If an error occurs for a different file, try regenerating the code
% from the Import Tool.
dataArray = textscan(fileID, formatSpec, 'Delimiter', delimiter, 'ReturnOnError', false);
%% Close the text file.
fclose(fileID);
%% Create output variable
SymbolicOutput = [dataArray{1:end-1}];
%% Clear temporary variables
clearvars filename delimiter formatSpec fileID dataArray ans;
%% Convert 1 based indexes in symbolic array variables
for lineIndex = 1:length(SymbolicOutput)
SymbolicOutput(lineIndex) = regexprep(SymbolicOutput(lineIndex), '_l_', '(');
SymbolicOutput(lineIndex) = regexprep(SymbolicOutput(lineIndex), '_c_', ',');
SymbolicOutput(lineIndex) = regexprep(SymbolicOutput(lineIndex), '_r_', ')');
end
% replace 2-D left indexes
for arrayIndex = 1:99
strIndex = int2str(arrayIndex);
strRep = sprintf('[%d,',(arrayIndex-1));
strPat = strcat('\(',strIndex,'\,');
for lineIndex = 1:length(SymbolicOutput)
str = char(SymbolicOutput(lineIndex));
SymbolicOutput(lineIndex) = {regexprep(str, strPat, strRep)};
end
end
% replace 2-D right indexes
for arrayIndex = 1:99
strIndex = int2str(arrayIndex);
strRep = sprintf(',%d]',(arrayIndex-1));
strPat = strcat('\,',strIndex,'\)');
for lineIndex = 1:length(SymbolicOutput)
str = char(SymbolicOutput(lineIndex));
SymbolicOutput(lineIndex) = {regexprep(str, strPat, strRep)};
end
end
% replace commas
for lineIndex = 1:length(SymbolicOutput)
str = char(SymbolicOutput(lineIndex));
SymbolicOutput(lineIndex) = {regexprep(str, '\,', '][')};
end
%% add float declarations in front of temporary variables
expression = 't(\w*) =';
replace = 'float t$1 =';
for lineIndex = 1:length(SymbolicOutput)
SymbolicOutput(lineIndex) = regexprep(SymbolicOutput(lineIndex),expression,replace);
end
%% replace (1.0/2.0) with 0.5f
expression = '\(1.0/2.0\)';
replace = '0.5f';
for lineIndex = 1:length(SymbolicOutput)
SymbolicOutput(lineIndex) = regexprep(SymbolicOutput(lineIndex),expression,replace);
end
%% replace 2.0
expression = '2\.0';
replace = '2.0f';
for lineIndex = 1:length(SymbolicOutput)
SymbolicOutput(lineIndex) = regexprep(SymbolicOutput(lineIndex),expression,replace);
end
%% Write to file
fileName = strcat(fileName,'.cpp');
fid = fopen(fileName,'wt');
for lineIndex = 1:length(SymbolicOutput)
fprintf(fid,char(SymbolicOutput(lineIndex)));
fprintf(fid,'\n');
end
fclose(fid);
clear all;
end

91
libraries/AP_NavEKF/Models/GimbalEstimatorExample/FuseMagnetometer.m Normal file
View File

@ -0,0 +1,91 @@
function [...
nextQuat, ... % quaternion state vector after fusion of measurements
nextStates, ... % state vector after fusion of measurements
nextP, ... % state covariance matrix after fusion of corrections
innovation, ... % Declination innovation - rad
varInnov] ... %
= FuseMagnetometer( ...
quat, ... % predicted quaternion states
states, ... % predicted states
P, ... % predicted covariance
magData, ... % body frame magnetic flux measurements
decl, ... % magnetic field declination from true north
gPhi, gPsi, gTheta) % gimbal yaw, roll, pitch angles
q0 = quat(1);
q1 = quat(2);
q2 = quat(3);
q3 = quat(4);
magX = magData(1);
magY = magData(2);
magZ = magData(3);
R_MAG = 0.1745^2;
% Calculate observation Jacobian
H = calcH_MAG(gPhi,gPsi,gTheta,magX,magY,magZ,q0,q1,q2,q3);
% Calculate innovation variance and Kalman gains
% Take advantage of the fact that only the first 3 elements in H are non zero
varInnov = (H(1,1:3)*P(1:3,1:3)*transpose(H(1,1:3)) + R_MAG);
Kfusion = (P(:,1:3)*transpose(H(1,1:3)))/varInnov;
% Calculate the innovation
innovation = calcMagAng(decl,gPhi,gPsi,gTheta,magX,magY,magZ,q0,q1,q2,q3);
if (innovation > pi)
innovation = innovation - 2*pi;
elseif (innovation < -pi)
innovation = innovation + 2*pi;
end
if (innovation > 0.5)
innovation = 0.5;
elseif (innovation < -0.5)
innovation = -0.5;
end
% correct the state vector
states(1:3) = 0;
states = states - Kfusion * innovation;
% the first 3 states represent the angular misalignment vector. This is
% is used to correct the estimate quaternion
% Convert the error rotation vector to its equivalent quaternion
% error = truth - estimate
rotationMag = sqrt(states(1)^2 + states(2)^2 + states(3)^2);
if rotationMag<1e-6
deltaQuat = single([1;0;0;0]);
else
deltaQuat = [cos(0.5*rotationMag); [states(1);states(2);states(3)]/rotationMag*sin(0.5*rotationMag)];
end
% Update the quaternion states by rotating from the previous attitude through
% the delta angle rotation quaternion
nextQuat = [quat(1)*deltaQuat(1)-transpose(quat(2:4))*deltaQuat(2:4); quat(1)*deltaQuat(2:4) + deltaQuat(1)*quat(2:4) + cross(quat(2:4),deltaQuat(2:4))];
% normalise the updated quaternion states
quatMag = sqrt(nextQuat(1)^2 + nextQuat(2)^2 + nextQuat(3)^2 + nextQuat(4)^2);
if (quatMag > 1e-6)
nextQuat = nextQuat / quatMag;
end
% correct the covariance P = P - K*H*P
% Take advantage of the fact that only the first 3 elements in H are non zero
P = P - Kfusion*H(1,1:3)*P(1:3,:);
% Force symmetry on the covariance matrix to prevent ill-conditioning
% of the matrix which would cause the filter to blow-up
P = 0.5*(P + transpose(P));
% ensure diagonals are positive
for i=1:9
if P(i,i) < 0
P(i,i) = 0;
end
end
% Set default output for states and covariance
nextP = P;
nextStates = states;
end

72
libraries/AP_NavEKF/Models/GimbalEstimatorExample/FuseVelocity.m Normal file
View File

@ -0,0 +1,72 @@
function [...
quat, ... % quaternion state vector after fusion of measurements
states, ... % state vector after fusion of measurements
tiltErr, ... % angle error
P, ... % state covariance matrix after fusion of corrections
innovation,... % NED velocity innovations (m/s)
varInnov] ... % NED velocity innovation variance ((m/s)^2)
= FuseVelocity( ...
quat, ... % predicted quaternion states from the INS
states, ... % predicted states from the INS
P, ... % predicted covariance
measVel) % NED velocity measurements (m/s)
R_OBS = 0.5^2;
innovation = zeros(1,3);
varInnov = zeros(1,3);
% Fuse measurements sequentially
angErrVec = [0;0;0];
for obsIndex = 1:3
stateIndex = 3 + obsIndex;
% Calculate the velocity measurement innovation
innovation(obsIndex) = states(stateIndex) - measVel(obsIndex);
% Calculate the Kalman Gain taking advantage of direct state observation
H = zeros(1,9);
H(1,stateIndex) = 1;
varInnov(obsIndex) = P(stateIndex,stateIndex) + R_OBS;
K = P(:,stateIndex)/varInnov(obsIndex);
% Calculate state corrections
xk = K * innovation(obsIndex);
% Apply the state corrections
states(1:3) = 0;
states = states - xk;
% Store tilt error estimate for external monitoring
angErrVec = angErrVec + states(1:3);
% the first 3 states represent the angular misalignment vector. This is
% is used to correct the estimated quaternion
% Convert the error rotation vector to its equivalent quaternion
% truth = estimate + error
rotationMag = sqrt(states(1)^2 + states(2)^2 + states(3)^2);
if rotationMag > 1e-12
deltaQuat = [cos(0.5*rotationMag); [states(1);states(2);states(3)]/rotationMag*sin(0.5*rotationMag)];
% Update the quaternion states by rotating from the previous attitude through
% the error quaternion
quat = QuatMult(quat,deltaQuat);
% re-normalise the quaternion
quatMag = sqrt(quat(1)^2 + quat(2)^2 + quat(3)^2 + quat(4)^2);
quat = quat / quatMag;
end
% Update the covariance
P = P - K*P(stateIndex,:);
% Force symmetry on the covariance matrix to prevent ill-conditioning
P = 0.5*(P + transpose(P));
% ensure diagonals are positive
for i=1:9
if P(i,i) < 0
P(i,i) = 0;
end
end
end
tiltErr = sqrt(dot(angErrVec(1:2),angErrVec(1:2)));
end

241
libraries/AP_NavEKF/Models/GimbalEstimatorExample/GenerateEquations.m Normal file
View File

@ -0,0 +1,241 @@
% 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 becasue 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 magentic 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');

65
libraries/AP_NavEKF/Models/GimbalEstimatorExample/PlotData.m Normal file
View File

@ -0,0 +1,65 @@
%% plot gyro bias estimates
figure;
plot(EKFlogs.time,EKFlogs.states(7:9,:)/dtSlow*180/pi);
grid on;
ylabel('Gyro Bias Estimate (deg/sec)');
xlabel('time (sec)');
hold on;
plot([min(time),max(time)],[gyroBias,gyroBias]*180/pi);
hold off;
%% plot velocity
figure;
plot(EKFlogs.time,EKFlogs.states(4:6,:));
grid on;
ylabel('Velocity (m/sec)');
xlabel('time (sec)');
%% calculate and plot tilt correction magnitude
figure;
plot(EKFlogs.time,EKFlogs.tiltCorr);
grid on;
ylabel('Tilt Correction Magnitude (rad)');
xlabel('time (sec)');
%% plot velocity innovations
figure;
subplot(3,1,1);plot(EKFlogs.time,[EKFlogs.velInnov(1,:);sqrt(EKFlogs.velInnovVar(1,:));-sqrt(EKFlogs.velInnovVar(1,:))]');
grid on;
ylabel('VelN Innovations (m)');
subplot(3,1,2);plot(EKFlogs.time,[EKFlogs.velInnov(2,:);sqrt(EKFlogs.velInnovVar(2,:));-sqrt(EKFlogs.velInnovVar(2,:))]');
grid on;
ylabel('VelE Innovations (m)');
subplot(3,1,3);plot(EKFlogs.time,[EKFlogs.velInnov(3,:);sqrt(EKFlogs.velInnovVar(3,:));-sqrt(EKFlogs.velInnovVar(3,:))]');
grid on;
ylabel('VelD Innovations (m)');
xlabel('time (sec)');
%% plot compass innovations
figure;
plot(EKFlogs.time,[EKFlogs.decInnov;sqrt(EKFlogs.decInnovVar);-sqrt(EKFlogs.decInnovVar)]');
grid on;
ylabel('Compass Innovations (rad)');
xlabel('time (sec)');
%% plot Euler angle estimates
figure;
subplot(3,1,1);plot(gimbal.time,gimbal.euler(1,:)*180/pi);
ylabel('Roll (deg)');grid on;
subplot(3,1,2);plot(gimbal.time,gimbal.euler(2,:)*180/pi);
ylabel('Pitch (deg)');grid on;
subplot(3,1,3);plot(gimbal.time,gimbal.euler(3,:)*180/pi);
ylabel('Yaw (deg)');
grid on;
xlabel('time (sec)');
%% plot Euler angle error estimates
figure;
subplot(3,1,1);plot(gimbal.time,gimbal.eulerError(1,:)*180/pi);
ylabel('Roll Error (deg)');grid on;
subplot(3,1,2);plot(gimbal.time,gimbal.eulerError(2,:)*180/pi);
ylabel('Pitch Error (deg)');grid on;
subplot(3,1,3);plot(gimbal.time,gimbal.eulerError(3,:)*180/pi);
ylabel('Yaw Error (deg)');
grid on;
xlabel('time (sec)');

61
libraries/AP_NavEKF/Models/GimbalEstimatorExample/PredictCovariance.m Normal file
View File

@ -0,0 +1,61 @@
function P = PredictCovariance(deltaAngle, ...
deltaVelocity, ...
quat, ...
states,...
P, ... % Previous state covariance matrix
dt) ... % IMU and prediction time step
% Set filter state process noise other than IMU errors, which are already
% built into the derived covariance predition equations.
% This process noise determines the rate of estimation of IMU bias errors
dAngBiasSigma = dt*dt*5E-4; % delta angle bias process noise (rad)
processNoise = [0*ones(1,6), dAngBiasSigma*[1 1 1]];
% Specify the estimated errors on the IMU delta angles and delta velocities
% Allow for 0.5 deg/sec of gyro error
daxNoise = (dt*0.5*pi/180)^2;
dayNoise = (dt*0.5*pi/180)^2;
dazNoise = (dt*0.5*pi/180)^2;
% Allow for 0.5 m/s/s of accelerometer error
dvxNoise = (dt*0.5)^2;
dvyNoise = (dt*0.5)^2;
dvzNoise = (dt*0.5)^2;
dvx = deltaVelocity(1);
dvy = deltaVelocity(2);
dvz = deltaVelocity(3);
dax = deltaAngle(1);
day = deltaAngle(2);
daz = deltaAngle(3);
q0 = quat(1);
q1 = quat(2);
q2 = quat(3);
q3 = quat(4);
dax_b = states(7);
day_b = states(8);
daz_b = states(9);
% Predicted covariance
F = calcF(dax,dax_b,day,day_b,daz,daz_b,dvx,dvy,dvz,q0,q1,q2,q3);
Q = calcQ(daxNoise,dayNoise,dazNoise,dvxNoise,dvyNoise,dvzNoise,q0,q1,q2,q3);
P = F*P*transpose(F) + Q;
% Add the general process noise
for i = 1:9
P(i,i) = P(i,i) + processNoise(i)^2;
end
% Force symmetry on the covariance matrix to prevent ill-conditioning
% of the matrix which would cause the filter to blow-up
P = 0.5*(P + transpose(P));
% ensure diagonals are positive
for i=1:9
if P(i,i) < 0
P(i,i) = 0;
end
end
end

404
libraries/AP_NavEKF/Models/GimbalEstimatorExample/PredictCovarianceOptimised.m Normal file
View File

@ -0,0 +1,404 @@
function nextP = PredictCovarianceOptimised(deltaAngle, ...
deltaVelocity, ...
quat, ...
states,...
P, ... % Previous state covariance matrix
dt) ... % IMU and prediction time step
% Set filter state process noise other than IMU errors, which are already
% built into the derived covariance predition equations.
% This process noise determines the rate of estimation of IMU bias errors
dAngBiasSigma = dt*dt*5E-4; % delta angle bias process noise (rad)
processNoise = [0*ones(1,6), dAngBiasSigma*[1 1 1]];
% Specify the estimated errors on the IMU delta angles and delta velocities
% Allow for 0.5 deg/sec of gyro error
daxNoise = (dt*0.5*pi/180)^2;
dayNoise = (dt*0.5*pi/180)^2;
dazNoise = (dt*0.5*pi/180)^2;
% Allow for 0.5 m/s/s of accelerometer error
dvxNoise = (dt*0.5)^2;
dvyNoise = (dt*0.5)^2;
dvzNoise = (dt*0.5)^2;
dvx = deltaVelocity(1);
dvy = deltaVelocity(2);
dvz = deltaVelocity(3);
dax = deltaAngle(1);
day = deltaAngle(2);
daz = deltaAngle(3);
q0 = quat(1);
q1 = quat(2);
q2 = quat(3);
q3 = quat(4);
dax_b = states(7);
day_b = states(8);
daz_b = states(9);
t1365 = dax*0.5;
t1366 = dax_b*0.5;
t1367 = t1365-t1366;
t1368 = day*0.5;
t1369 = day_b*0.5;
t1370 = t1368-t1369;
t1371 = daz*0.5;
t1372 = daz_b*0.5;
t1373 = t1371-t1372;
t1374 = q2*t1367*0.5;
t1375 = q1*t1370*0.5;
t1376 = q0*t1373*0.5;
t1377 = q2*0.5;
t1378 = q3*t1367*0.5;
t1379 = q1*t1373*0.5;
t1380 = q1*0.5;
t1381 = q0*t1367*0.5;
t1382 = q3*t1370*0.5;
t1383 = q0*0.5;
t1384 = q2*t1370*0.5;
t1385 = q3*t1373*0.5;
t1386 = q0*t1370*0.5;
t1387 = q3*0.5;
t1388 = q1*t1367*0.5;
t1389 = q2*t1373*0.5;
t1390 = t1374+t1375+t1376-t1387;
t1391 = t1377+t1378+t1379-t1386;
t1392 = q2*t1391*2.0;
t1393 = t1380+t1381+t1382-t1389;
t1394 = q1*t1393*2.0;
t1395 = t1383+t1384+t1385-t1388;
t1396 = q0*t1395*2.0;
t1403 = q3*t1390*2.0;
t1397 = t1392+t1394+t1396-t1403;
t1398 = q0^2;
t1399 = q1^2;
t1400 = q2^2;
t1401 = q3^2;
t1402 = t1398+t1399+t1400+t1401;
t1404 = t1374+t1375-t1376+t1387;
t1405 = t1377-t1378+t1379+t1386;
t1406 = q1*t1405*2.0;
t1407 = -t1380+t1381+t1382+t1389;
t1408 = q2*t1407*2.0;
t1409 = t1383-t1384+t1385+t1388;
t1410 = q3*t1409*2.0;
t1420 = q0*t1404*2.0;
t1411 = t1406+t1408+t1410-t1420;
t1412 = -t1377+t1378+t1379+t1386;
t1413 = q0*t1412*2.0;
t1414 = t1374-t1375+t1376+t1387;
t1415 = t1383+t1384-t1385+t1388;
t1416 = q2*t1415*2.0;
t1417 = t1380-t1381+t1382+t1389;
t1418 = q3*t1417*2.0;
t1421 = q1*t1414*2.0;
t1419 = t1413+t1416+t1418-t1421;
t1422 = P(1,1)*t1397;
t1423 = P(2,1)*t1411;
t1429 = P(7,1)*t1402;
t1430 = P(3,1)*t1419;
t1424 = t1422+t1423-t1429-t1430;
t1425 = P(1,2)*t1397;
t1426 = P(2,2)*t1411;
t1427 = P(1,3)*t1397;
t1428 = P(2,3)*t1411;
t1434 = P(7,2)*t1402;
t1435 = P(3,2)*t1419;
t1431 = t1425+t1426-t1434-t1435;
t1442 = P(7,3)*t1402;
t1443 = P(3,3)*t1419;
t1432 = t1427+t1428-t1442-t1443;
t1433 = t1398+t1399-t1400-t1401;
t1436 = q0*q2*2.0;
t1437 = q1*q3*2.0;
t1438 = t1436+t1437;
t1439 = q0*q3*2.0;
t1441 = q1*q2*2.0;
t1440 = t1439-t1441;
t1444 = t1398-t1399+t1400-t1401;
t1445 = q0*q1*2.0;
t1449 = q2*q3*2.0;
t1446 = t1445-t1449;
t1447 = t1439+t1441;
t1448 = t1398-t1399-t1400+t1401;
t1450 = t1445+t1449;
t1451 = t1436-t1437;
t1452 = P(1,7)*t1397;
t1453 = P(2,7)*t1411;
t1628 = P(7,7)*t1402;
t1454 = t1452+t1453-t1628-P(3,7)*t1419;
t1455 = P(1,8)*t1397;
t1456 = P(2,8)*t1411;
t1629 = P(7,8)*t1402;
t1457 = t1455+t1456-t1629-P(3,8)*t1419;
t1458 = P(1,9)*t1397;
t1459 = P(2,9)*t1411;
t1630 = P(7,9)*t1402;
t1460 = t1458+t1459-t1630-P(3,9)*t1419;
t1461 = q0*t1390*2.0;
t1462 = q1*t1391*2.0;
t1463 = q3*t1395*2.0;
t1473 = q2*t1393*2.0;
t1464 = t1461+t1462+t1463-t1473;
t1465 = q0*t1409*2.0;
t1466 = q2*t1405*2.0;
t1467 = q3*t1404*2.0;
t1474 = q1*t1407*2.0;
t1468 = t1465+t1466+t1467-t1474;
t1469 = q1*t1415*2.0;
t1470 = q2*t1414*2.0;
t1471 = q3*t1412*2.0;
t1475 = q0*t1417*2.0;
t1472 = t1469+t1470+t1471-t1475;
t1476 = P(8,1)*t1402;
t1477 = P(1,1)*t1464;
t1486 = P(2,1)*t1468;
t1487 = P(3,1)*t1472;
t1478 = t1476+t1477-t1486-t1487;
t1479 = P(8,2)*t1402;
t1480 = P(1,2)*t1464;
t1492 = P(2,2)*t1468;
t1493 = P(3,2)*t1472;
t1481 = t1479+t1480-t1492-t1493;
t1482 = P(8,3)*t1402;
t1483 = P(1,3)*t1464;
t1498 = P(2,3)*t1468;
t1499 = P(3,3)*t1472;
t1484 = t1482+t1483-t1498-t1499;
t1485 = t1402^2;
t1488 = q1*t1390*2.0;
t1489 = q2*t1395*2.0;
t1490 = q3*t1393*2.0;
t1533 = q0*t1391*2.0;
t1491 = t1488+t1489+t1490-t1533;
t1494 = q0*t1407*2.0;
t1495 = q1*t1409*2.0;
t1496 = q2*t1404*2.0;
t1534 = q3*t1405*2.0;
t1497 = t1494+t1495+t1496-t1534;
t1500 = q0*t1415*2.0;
t1501 = q1*t1417*2.0;
t1502 = q3*t1414*2.0;
t1535 = q2*t1412*2.0;
t1503 = t1500+t1501+t1502-t1535;
t1504 = dvy*t1433;
t1505 = dvx*t1440;
t1506 = t1504+t1505;
t1507 = dvx*t1438;
t1508 = dvy*t1438;
t1509 = dvz*t1440;
t1510 = t1508+t1509;
t1511 = dvx*t1444;
t1551 = dvy*t1447;
t1512 = t1511-t1551;
t1513 = dvz*t1444;
t1514 = dvy*t1446;
t1515 = t1513+t1514;
t1516 = dvx*t1446;
t1517 = dvz*t1447;
t1518 = t1516+t1517;
t1519 = dvx*t1448;
t1520 = dvz*t1451;
t1521 = t1519+t1520;
t1522 = dvy*t1448;
t1552 = dvz*t1450;
t1523 = t1522-t1552;
t1524 = dvx*t1450;
t1525 = dvy*t1451;
t1526 = t1524+t1525;
t1527 = P(8,7)*t1402;
t1528 = P(1,7)*t1464;
t1529 = P(8,8)*t1402;
t1530 = P(1,8)*t1464;
t1531 = P(8,9)*t1402;
t1532 = P(1,9)*t1464;
t1536 = P(9,1)*t1402;
t1537 = P(2,1)*t1497;
t1545 = P(1,1)*t1491;
t1546 = P(3,1)*t1503;
t1538 = t1536+t1537-t1545-t1546;
t1539 = P(9,2)*t1402;
t1540 = P(2,2)*t1497;
t1547 = P(1,2)*t1491;
t1548 = P(3,2)*t1503;
t1541 = t1539+t1540-t1547-t1548;
t1542 = P(9,3)*t1402;
t1543 = P(2,3)*t1497;
t1549 = P(1,3)*t1491;
t1550 = P(3,3)*t1503;
t1544 = t1542+t1543-t1549-t1550;
t1553 = P(9,7)*t1402;
t1554 = P(2,7)*t1497;
t1555 = P(9,8)*t1402;
t1556 = P(2,8)*t1497;
t1557 = P(9,9)*t1402;
t1558 = P(2,9)*t1497;
t1560 = dvz*t1433;
t1559 = t1507-t1560;
t1561 = P(1,1)*t1510;
t1567 = P(3,1)*t1506;
t1568 = P(2,1)*t1559;
t1562 = P(4,1)+t1561-t1567-t1568;
t1563 = P(1,2)*t1510;
t1569 = P(3,2)*t1506;
t1570 = P(2,2)*t1559;
t1564 = P(4,2)+t1563-t1569-t1570;
t1565 = P(1,3)*t1510;
t1571 = P(3,3)*t1506;
t1572 = P(2,3)*t1559;
t1566 = P(4,3)+t1565-t1571-t1572;
t1573 = -t1507+t1560;
t1574 = P(2,1)*t1573;
t1575 = P(4,1)+t1561-t1567+t1574;
t1576 = P(2,2)*t1573;
t1577 = P(4,2)+t1563-t1569+t1576;
t1578 = P(2,3)*t1573;
t1579 = P(4,3)+t1565-t1571+t1578;
t1580 = P(1,7)*t1510;
t1581 = P(1,8)*t1510;
t1582 = P(1,9)*t1510;
t1583 = P(2,1)*t1518;
t1584 = P(3,1)*t1512;
t1592 = P(1,1)*t1515;
t1585 = P(5,1)+t1583+t1584-t1592;
t1586 = P(2,2)*t1518;
t1587 = P(3,2)*t1512;
t1593 = P(1,2)*t1515;
t1588 = P(5,2)+t1586+t1587-t1593;
t1589 = P(2,3)*t1518;
t1590 = P(3,3)*t1512;
t1594 = P(1,3)*t1515;
t1591 = P(5,3)+t1589+t1590-t1594;
t1595 = dvxNoise*t1433*t1447;
t1596 = P(2,7)*t1518;
t1597 = P(3,7)*t1512;
t1598 = P(5,7)+t1596+t1597-P(1,7)*t1515;
t1599 = P(2,8)*t1518;
t1600 = P(3,8)*t1512;
t1601 = P(5,8)+t1599+t1600-P(1,8)*t1515;
t1602 = P(2,9)*t1518;
t1603 = P(3,9)*t1512;
t1604 = P(5,9)+t1602+t1603-P(1,9)*t1515;
t1605 = P(3,1)*t1526;
t1606 = P(1,1)*t1523;
t1614 = P(2,1)*t1521;
t1607 = P(6,1)+t1605+t1606-t1614;
t1608 = P(3,2)*t1526;
t1609 = P(1,2)*t1523;
t1615 = P(2,2)*t1521;
t1610 = P(6,2)+t1608+t1609-t1615;
t1611 = P(3,3)*t1526;
t1612 = P(1,3)*t1523;
t1616 = P(2,3)*t1521;
t1613 = P(6,3)+t1611+t1612-t1616;
t1617 = dvzNoise*t1438*t1448;
t1618 = dvyNoise*t1444*t1450;
t1619 = P(3,7)*t1526;
t1620 = P(1,7)*t1523;
t1621 = P(6,7)+t1619+t1620-P(2,7)*t1521;
t1622 = P(3,8)*t1526;
t1623 = P(1,8)*t1523;
t1624 = P(6,8)+t1622+t1623-P(2,8)*t1521;
t1625 = P(3,9)*t1526;
t1626 = P(1,9)*t1523;
t1627 = P(6,9)+t1625+t1626-P(2,9)*t1521;
nextP(1,1) = daxNoise*t1485+t1397*t1424+t1411*t1431-t1419*t1432-t1402*t1454;
nextP(2,1) = -t1397*t1478-t1411*t1481+t1419*t1484+t1402*(t1527+t1528-P(2,7)*t1468-P(3,7)*t1472);
nextP(3,1) = -t1397*t1538-t1411*t1541+t1419*t1544+t1402*(t1553+t1554-P(1,7)*t1491-P(3,7)*t1503);
nextP(4,1) = -t1402*(P(4,7)+t1580-P(3,7)*t1506-P(2,7)*t1559)+t1397*t1562+t1411*t1564-t1419*t1566;
nextP(5,1) = t1397*t1585+t1411*t1588-t1402*t1598-t1419*t1591;
nextP(6,1) = t1397*t1607+t1411*t1610-t1402*t1621-t1419*t1613;
nextP(7,1) = -t1628+P(7,1)*t1397+P(7,2)*t1411-P(7,3)*t1419;
nextP(8,1) = -t1527+P(8,1)*t1397+P(8,2)*t1411-P(8,3)*t1419;
nextP(9,1) = -t1553+P(9,1)*t1397+P(9,2)*t1411-P(9,3)*t1419;
nextP(1,2) = -t1402*t1457-t1424*t1464+t1431*t1468+t1432*t1472;
nextP(2,2) = dayNoise*t1485+t1464*t1478-t1468*t1481-t1472*t1484+t1402*(t1529+t1530-P(2,8)*t1468-P(3,8)*t1472);
nextP(3,2) = t1464*t1538-t1468*t1541-t1472*t1544+t1402*(t1555+t1556-P(1,8)*t1491-P(3,8)*t1503);
nextP(4,2) = -t1402*(P(4,8)+t1581-P(3,8)*t1506-P(2,8)*t1559)-t1464*t1562+t1468*t1564+t1472*t1566;
nextP(5,2) = -t1402*t1601-t1464*t1585+t1468*t1588+t1472*t1591;
nextP(6,2) = -t1402*t1624-t1464*t1607+t1468*t1610+t1472*t1613;
nextP(7,2) = -t1629-P(7,1)*t1464+P(7,2)*t1468+P(7,3)*t1472;
nextP(8,2) = -t1529-P(8,1)*t1464+P(8,2)*t1468+P(8,3)*t1472;
nextP(9,2) = -t1555-P(9,1)*t1464+P(9,2)*t1468+P(9,3)*t1472;
nextP(1,3) = -t1402*t1460-t1431*t1497+t1432*t1503+t1491*(t1422+t1423-t1429-t1430);
nextP(2,3) = -t1478*t1491+t1481*t1497-t1484*t1503+t1402*(t1531+t1532-P(2,9)*t1468-P(3,9)*t1472);
nextP(3,3) = dazNoise*t1485-t1491*t1538+t1497*t1541-t1503*t1544+t1402*(t1557+t1558-P(1,9)*t1491-P(3,9)*t1503);
nextP(4,3) = -t1402*(P(4,9)+t1582-P(3,9)*t1506-P(2,9)*t1559)+t1491*t1562-t1497*t1564+t1503*t1566;
nextP(5,3) = -t1402*t1604+t1491*t1585-t1497*t1588+t1503*t1591;
nextP(6,3) = -t1402*t1627+t1491*t1607-t1497*t1610+t1503*t1613;
nextP(7,3) = -t1630+P(7,1)*t1491-P(7,2)*t1497+P(7,3)*t1503;
nextP(8,3) = -t1531+P(8,1)*t1491-P(8,2)*t1497+P(8,3)*t1503;
nextP(9,3) = -t1557+P(9,1)*t1491-P(9,2)*t1497+P(9,3)*t1503;
nextP(1,4) = P(1,4)*t1397+P(2,4)*t1411-P(3,4)*t1419-P(7,4)*t1402-t1432*t1506+t1510*(t1422+t1423-t1429-t1430)-t1559*(t1425+t1426-t1434-t1435);
nextP(2,4) = -P(1,4)*t1464-P(8,4)*t1402+P(2,4)*t1468+P(3,4)*t1472-t1478*t1510+t1484*t1506+t1481*t1559;
nextP(3,4) = -P(9,4)*t1402+P(1,4)*t1491-P(2,4)*t1497+P(3,4)*t1503-t1510*t1538+t1506*t1544+t1541*t1559;
nextP(4,4) = P(4,4)+P(1,4)*t1510-P(3,4)*t1506+P(2,4)*t1573-t1506*t1566+t1510*t1575+t1573*t1577+dvxNoise*t1433^2+dvyNoise*t1440^2+dvzNoise*t1438^2;
nextP(5,4) = P(5,4)+t1595-P(1,4)*t1515+P(2,4)*t1518+P(3,4)*t1512+t1510*t1585-t1506*t1591+t1573*t1588-dvyNoise*t1440*t1444-dvzNoise*t1438*t1446;
nextP(6,4) = P(6,4)+t1617+P(1,4)*t1523-P(2,4)*t1521+P(3,4)*t1526+t1510*t1607-t1506*t1613+t1573*t1610-dvxNoise*t1433*t1451-dvyNoise*t1440*t1450;
nextP(7,4) = P(7,4)-P(7,3)*t1506+P(7,1)*t1510+P(7,2)*t1573;
nextP(8,4) = P(8,4)-P(8,3)*t1506+P(8,1)*t1510+P(8,2)*t1573;
nextP(9,4) = P(9,4)-P(9,3)*t1506+P(9,1)*t1510+P(9,2)*t1573;
nextP(1,5) = P(1,5)*t1397+P(2,5)*t1411-P(3,5)*t1419-P(7,5)*t1402-t1424*t1515+t1432*t1512+t1518*(t1425+t1426-t1434-t1435);
nextP(2,5) = -P(1,5)*t1464-P(8,5)*t1402+P(2,5)*t1468+P(3,5)*t1472+t1478*t1515-t1484*t1512-t1481*t1518;
nextP(3,5) = -P(9,5)*t1402+P(1,5)*t1491-P(2,5)*t1497+P(3,5)*t1503+t1515*t1538-t1512*t1544-t1518*t1541;
nextP(4,5) = P(4,5)+t1595+P(1,5)*t1510-P(3,5)*t1506+P(2,5)*t1573-t1515*t1575+t1512*t1579+t1518*t1577-dvyNoise*t1440*t1444-dvzNoise*t1438*t1446;
nextP(5,5) = P(5,5)-P(1,5)*t1515+P(2,5)*t1518+P(3,5)*t1512-t1515*t1585+t1512*t1591+t1518*t1588+dvxNoise*t1447^2+dvyNoise*t1444^2+dvzNoise*t1446^2;
nextP(6,5) = P(6,5)+t1618+P(1,5)*t1523-P(2,5)*t1521+P(3,5)*t1526-t1515*t1607+t1512*t1613+t1518*t1610-dvxNoise*t1447*t1451-dvzNoise*t1446*t1448;
nextP(7,5) = P(7,5)+P(7,3)*t1512-P(7,1)*t1515+P(7,2)*t1518;
nextP(8,5) = P(8,5)+P(8,3)*t1512-P(8,1)*t1515+P(8,2)*t1518;
nextP(9,5) = P(9,5)+P(9,3)*t1512-P(9,1)*t1515+P(9,2)*t1518;
nextP(1,6) = P(1,6)*t1397+P(2,6)*t1411-P(3,6)*t1419-P(7,6)*t1402+t1424*t1523-t1431*t1521+t1526*(t1427+t1428-t1442-t1443);
nextP(2,6) = -P(1,6)*t1464-P(8,6)*t1402+P(2,6)*t1468+P(3,6)*t1472-t1478*t1523+t1481*t1521-t1484*t1526;
nextP(3,6) = -P(9,6)*t1402+P(1,6)*t1491-P(2,6)*t1497+P(3,6)*t1503-t1523*t1538+t1521*t1541-t1526*t1544;
nextP(4,6) = P(4,6)+t1617+P(1,6)*t1510-P(3,6)*t1506+P(2,6)*t1573-t1521*t1577+t1523*t1575+t1526*t1579-dvxNoise*t1433*t1451-dvyNoise*t1440*t1450;
nextP(5,6) = P(5,6)+t1618-P(1,6)*t1515+P(2,6)*t1518+P(3,6)*t1512+t1523*t1585-t1521*t1588+t1526*t1591-dvxNoise*t1447*t1451-dvzNoise*t1446*t1448;
nextP(6,6) = P(6,6)+P(1,6)*t1523-P(2,6)*t1521+P(3,6)*t1526+t1523*t1607-t1521*t1610+t1526*t1613+dvxNoise*t1451^2+dvyNoise*t1450^2+dvzNoise*t1448^2;
nextP(7,6) = P(7,6)-P(7,2)*t1521+P(7,1)*t1523+P(7,3)*t1526;
nextP(8,6) = P(8,6)-P(8,2)*t1521+P(8,1)*t1523+P(8,3)*t1526;
nextP(9,6) = P(9,6)-P(9,2)*t1521+P(9,1)*t1523+P(9,3)*t1526;
nextP(1,7) = t1454;
nextP(2,7) = -t1527-t1528+P(2,7)*t1468+P(3,7)*t1472;
nextP(3,7) = -t1553-t1554+P(1,7)*t1491+P(3,7)*t1503;
nextP(4,7) = P(4,7)+t1580-P(3,7)*t1506+P(2,7)*t1573;
nextP(5,7) = t1598;
nextP(6,7) = t1621;
nextP(7,7) = P(7,7);
nextP(8,7) = P(8,7);
nextP(9,7) = P(9,7);
nextP(1,8) = t1457;
nextP(2,8) = -t1529-t1530+P(2,8)*t1468+P(3,8)*t1472;
nextP(3,8) = -t1555-t1556+P(1,8)*t1491+P(3,8)*t1503;
nextP(4,8) = P(4,8)+t1581-P(3,8)*t1506+P(2,8)*t1573;
nextP(5,8) = t1601;
nextP(6,8) = t1624;
nextP(7,8) = P(7,8);
nextP(8,8) = P(8,8);
nextP(9,8) = P(9,8);
nextP(1,9) = t1460;
nextP(2,9) = -t1531-t1532+P(2,9)*t1468+P(3,9)*t1472;
nextP(3,9) = -t1557-t1558+P(1,9)*t1491+P(3,9)*t1503;
nextP(4,9) = P(4,9)+t1582-P(3,9)*t1506+P(2,9)*t1573;
nextP(5,9) = t1604;
nextP(6,9) = t1627;
nextP(7,9) = P(7,9);
nextP(8,9) = P(8,9);
nextP(9,9) = P(9,9);
% Add the general process noise
for i = 1:9
nextP(i,i) = nextP(i,i) + processNoise(i)^2;
end
% Force symmetry on the covariance matrix to prevent ill-conditioning
% of the matrix which would cause the filter to blow-up
P = 0.5*(P + transpose(P));
% ensure diagonals are positive
for i=1:9
if P(i,i) < 0
P(i,i) = 0;
end
end
end

55
libraries/AP_NavEKF/Models/GimbalEstimatorExample/PredictStates.m Normal file
View File

@ -0,0 +1,55 @@
function [quat, states, Tbn, correctedDelAng, correctedDelVel] = PredictStates( ...
quat, ... % previous quaternion states 4x1
states, ... % previous states (3x1 rotation error, 3x1 velocity, 3x1 gyro bias)
dAng, ... % IMU delta angles, 3x1 (rad)
dVel, ... % IMU delta velocities 3x1 (m/s)
dt) % time since last IMU measurement (sec)
% Define parameters used for previous angular rates and acceleration shwich
% are used for trapezoidal integration
% define persistent variables for previous delta angle and velocity which
% are required for sculling and coning error corrections
persistent prevDelAng;
if isempty(prevDelAng)
prevDelAng = dAng;
end
persistent prevDelVel;
if isempty(prevDelVel)
prevDelVel = dVel;
end
% Remove sensor bias errors
dAng = dAng - states(7:9);
% Apply rotational and skulling corrections
correctedDelVel= dVel + ...
0.5*cross(prevDelAng + dAng , prevDelVel + dVel) + 1/6*cross(prevDelAng + dAng , cross(prevDelAng + dAng , prevDelVel + dVel)) + ... % rotational correction
1/12*(cross(prevDelAng , dVel) + cross(prevDelVel , dAng)); % sculling correction
% Apply corrections for coning errors
correctedDelAng = dAng - 1/12*cross(prevDelAng , dAng);
% Save current measurements
prevDelAng = dAng;
prevDelVel = dVel;
% Convert the rotation vector to its equivalent quaternion
deltaQuat = RotToQuat(correctedDelAng);
% Update the quaternions by rotating from the previous attitude through
% the delta angle rotation quaternion
quat = QuatMult(quat,deltaQuat);
% Normalise the quaternions
quat = NormQuat(quat);
% Calculate the body to nav cosine matrix
Tbn = Quat2Tbn(quat);
% transform body delta velocities to delta velocities in the nav frame
delVelNav = Tbn * correctedDelVel + [0;0;9.807]*dt;
% Sum delta velocities to get velocity
states(4:6) = states(4:6) + delVelNav(1:3);
end

203
libraries/AP_NavEKF/Models/GimbalEstimatorExample/RunSimulation.m Normal file
View File

@ -0,0 +1,203 @@
%% Set initial conditions
clear all;
dtSlow = 1/50;
dtFast = 1/1000;
rateMult = round(dtSlow/dtFast);
duration = 60;
indexLimitSlow = round(duration/dtSlow);
indexLimitFast = indexLimitSlow*rateMult;
% create data logging variables
gimbal.time = zeros(1,indexLimitFast);
gimbal.euler = zeros(3,indexLimitFast);
gimbal.eulerTruth = zeros(3,indexLimitFast);
gimbal.eulerError = zeros(3,indexLimitFast);
% Use a random initial truth orientation
phiInit = 0.1*randn;
thetaInit = 0.1*randn;
psiInit = 2*pi*rand - pi;
quatTruth = EulToQuat([phiInit,thetaInit,psiInit]);% [1;0.05*randn;0.05*randn;2*(rand-0.5)];
quatLength = sqrt(quatTruth(1)^2 + quatTruth(2)^2 + quatTruth(3)^2 + quatTruth(4)^2);
quatTruth = quatTruth / quatLength;
TsnTruth = Quat2Tbn(quatTruth);
% define the earths truth magnetic field
declTruth = 10*pi/180;
magEarthTruth = [0.25*cos(declTruth);0.25*sin(declTruth);-0.5];
% define the declination parameter assuming 2deg RMS error - this would be
% obtained from the main EKF to take advantage of in-flight learning
declParam = declTruth + 2*pi/180*randn;
% define the magnetometer bias errors
magMeasBias = 0.02*[randn;randn;randn];
% Define IMU bias errors and noise
gyroBias = 1*pi/180*[randn;randn;randn];
accBias = 0.05*[randn;randn;randn];
gyroNoise = 0.01;
accNoise = 0.05;
% define the state covariances with the exception of the quaternion covariances
Sigma_velNED = 0.5; % 1 sigma uncertainty in horizontal velocity components
Sigma_dAngBias = 1*pi/180*dtSlow; % 1 Sigma uncertainty in delta angle bias
Sigma_angErr = 1; % 1 Sigma uncertainty in angular misalignment (rad)
covariance = single(diag([Sigma_angErr*[1;1;1];Sigma_velNED*[1;1;1];Sigma_dAngBias*[1;1;1]]).^2);
% Initialise truth trajectory variables
% fly a CCW circle with constant gimbal angles
gPsiInit = 20*pi/180; % gimbal yaw
gThetaInit = 0; % gimbal pitch
gPhiInit = 0; % gimbal roll
psiTruth = psiInit;
radius = 20;
gndSpd = 5;
trackAngTruth = -pi;
centripAccelMag = gndSpd/radius*gndSpd;
gravAccel = [0;0;-9.81];
%% Main Loop
hdgAlignedEKF=0;
hdgAlignedGimbal=0;
slowIndex = 0;
delAngFast = [0;0;0];
delVelFast = [0;0;0];
delAngSlow = [0;0;0];
delVelSlow = [0;0;0];
prevAngRateMeas = [0;0;0];
prevAccelMeas = [0;0;0];
quatFast = [1;0;0;0];
quatFastSaved = quatFast;
angRateBiasEKF = [0;0;0];
quatEKF = [1;0;0;0];
for fastIndex = 1:indexLimitFast % 1000 Hz gimbal prediction loop
time = dtFast*fastIndex;
% Calculate Truth Data
% Need to replace this with a full kinematic model or test data
% calculate truth angular rates - we don't start maneouvring until
% heading alignment is complete
psiRateTruth = gndSpd/radius*hdgAlignedEKF;
angRateTruth = [0;0;psiRateTruth]; % constant yaw rate
% calculate yaw and track angles
psiTruth = psiTruth + psiRateTruth*dtFast;
trackAngTruth = trackAngTruth + psiRateTruth*dtFast;
% Cacluate truth quternion
quatTruth = EulToQuat([phiInit,thetaInit,psiTruth]);
% Calculate truth rotaton from sensor to NED
TsnTruth = Quat2Tbn(quatTruth);
% calculate truth accel vector
centripAccel = centripAccelMag*[-sin(trackAngTruth);cos(trackAngTruth);0];
accelTruth = transpose(TsnTruth)*(gravAccel + centripAccel);
% calculate truth velocity vector
truthVel = gndSpd*[cos(trackAngTruth);sin(trackAngTruth);0];
% synthesise sensor measurements
% Synthesise IMU measurements, adding bias and noise
angRateMeas = angRateTruth + gyroBias + gyroNoise*[randn;randn;randn];
accelMeas = accelTruth + accBias + accNoise*[randn;randn;randn];
% synthesise velocity measurements
measVel = truthVel;
% synthesise gimbal angles
gPhi = 0;
gTheta = 0;
gPsi = gPsiInit;
% Define rotation from magnetometer to sensor using a 312 rotation sequence
TmsTruth = calcTms(gPhi,gPsi,gTheta);
% calculate rotation from NED to magnetometer axes Tnm = Tsm * Tns
TnmTruth = transpose(TmsTruth) * transpose(TsnTruth);
% synthesise magnetometer measurements adding sensor bias
magMeas = TnmTruth*magEarthTruth + magMeasBias;
% integrate the IMU measurements to produce delta angles and velocities
% using a trapezoidal integrator
if isempty(prevAngRateMeas)
prevAngRateMeas = angRateMeas;
end
if isempty(prevAccelMeas)
prevAccelMeas = accelMeas;
end
delAngFast = delAngFast + 0.5*(angRateMeas + prevAngRateMeas)*dtFast;
delVelFast = delVelFast + 0.5*(accelMeas + prevAccelMeas)*dtFast;
prevAngRateMeas = angRateMeas;
prevAccelMeas = accelMeas;
% Run an attitude prediction calculation at 1000Hz
% Convert the rotation vector to its equivalent quaternion
% using a first order approximation after applying the correction for
% gyro bias using bias estimates from the EKF
deltaQuat = [1;0.5*(angRateMeas - angRateBiasEKF)*dtFast];
% Update the quaternions by rotating from the previous attitude through
% the delta angle rotation quaternion
quatFast = QuatMult(quatFast,deltaQuat);
% Normalise the quaternions
quatFast = NormQuat(quatFast);
% log the high rate data
eulLogFast(:,fastIndex) = QuatToEul(quatFast);
% every 20msec we send them to the EKF computer and reset
% the total
% we also save a copy of the quaternion from our high rate prediction
if (rem(fastIndex,rateMult) == 0)
delAngSlow = delAngFast;
delVelSlow = delVelFast;
delAngFast = [0;0;0];
delVelFast = [0;0;0];
quatFastSaved = quatFast;
end
% run the 50Hz EKF loop but do so 5 msec behind the
% data transmission to simulate the effect of transmission and
% computational delays
if (rem(fastIndex,rateMult) == 5)
slowIndex = slowIndex + 1;
[quatEKF,angRateBiasEKF,EKFlogs,hdgAlignedEKF] = calcEKF(delAngSlow,delVelSlow,measVel,gPhi,gPsi,gTheta,magMeas,declParam,time,dtSlow,slowIndex,indexLimitSlow);
end
% Correct Gimbal attitude usng EKF data
% Assume the gimbal controller receive the EKF solution 10 msec after
% it sent the sensor data
if (rem(fastIndex,rateMult) == 10)
% calculate the quaternion from the EKF corrected attitude to the
% attitude calculated using the local fast prediction algorithm
deltaQuatFast = QuatDivide(quatEKF,quatFastSaved);
% apply this correction to the fast solution at the current time
% step (this can be applied across several steps to smooth the
% output if required)
quatFast = QuatMult(quatFast,deltaQuatFast);
% normalise the resultant quaternion
quatFast = NormQuat(quatFast);
% flag when the gimbals own heading is aligned
hdgAlignedGimbal = hdgAlignedEKF;
end
% Log gimbal data
gimbal.time(fastIndex) = time;
gimbal.euler(:,fastIndex) = QuatToEul(quatFast);
gimbal.eulerTruth(:,fastIndex) = QuatToEul(quatTruth);
if (hdgAlignedGimbal)
gimbal.eulerError(:,fastIndex) = gimbal.euler(:,fastIndex) - gimbal.eulerTruth(:,fastIndex);
if (gimbal.eulerError(3,fastIndex) > pi)
gimbal.eulerError(3,fastIndex) = gimbal.eulerError(3,fastIndex) - 2*pi;
elseif (gimbal.eulerError(3,fastIndex) < -pi)
gimbal.eulerError(3,fastIndex) = gimbal.eulerError(3,fastIndex) + 2*pi;
end
else
gimbal.eulerError(:,fastIndex) = [NaN;NaN;NaN];
end
end
%% Generate Plots
close all;
PlotData;

96
libraries/AP_NavEKF/Models/GimbalEstimatorExample/calcEKF.m Normal file
View File

@ -0,0 +1,96 @@
function [quatOut,angRateBiasOut,logOut,headingAlignedOut] = calcEKF(delAngSlow,delVelSlow,measVel,gPhi,gPsi,gTheta,magMeas,declParam,time,dtSlow,frameIndex,maxFrameIndex)
persistent velocityAligned;
if isempty(velocityAligned)
velocityAligned = 0;
end
persistent quat;
if isempty(quat)
quat = [1;0;0;0];
end
persistent states;
if isempty(states)
states = zeros(9,1);
end
persistent covariance;
if isempty(covariance)
% define the state covariances with the exception of the quaternion covariances
Sigma_velNED = 0.5; % 1 sigma uncertainty in horizontal velocity components
Sigma_dAngBias = 1*pi/180*dtSlow; % 1 Sigma uncertainty in delta angle bias
Sigma_angErr = 1; % 1 Sigma uncertainty in angular misalignment (rad)
covariance = single(diag([Sigma_angErr*[1;1;1];Sigma_velNED*[1;1;1];Sigma_dAngBias*[1;1;1]]).^2);
end
persistent headingAligned
if isempty(headingAligned)
headingAligned = 0;
end
persistent log;
if isempty(log)
% create data logging variables
log.time = zeros(1,maxFrameIndex);
log.states = zeros(9,maxFrameIndex);
log.quat = zeros(4,maxFrameIndex);
log.euler = zeros(3,maxFrameIndex);
log.tiltCorr = zeros(1,maxFrameIndex);
log.velInnov = zeros(3,maxFrameIndex);
log.decInnov = zeros(1,maxFrameIndex);
log.velInnovVar = log.velInnov;
log.decInnovVar = log.decInnov;
end
% predict states
[quat, states, Tsn, delAngCorrected, delVelCorrected] = PredictStates(quat,states,delAngSlow,delVelSlow,dtSlow);
% log state prediction data
log.states(:,frameIndex) = states;
log.quat(:,frameIndex) = quat;
log.euler(:,frameIndex) = QuatToEul(quat);
% predict covariance matrix
covariance = PredictCovarianceOptimised(delAngCorrected,delVelCorrected,quat,states,covariance,dtSlow);
if (velocityAligned == 0)
% initialise velocity states
states(4:6) = measVel;
velocityAligned = 1;
else
% fuse velocity measurements
[quat,states,tiltCorrection,covariance,velInnov,velInnovVar] = FuseVelocity(quat,states,covariance,measVel);
% log velocity fusion data
log.velInnov(:,frameIndex) = velInnov;
log.velInnovVar(:,frameIndex) = velInnovVar;
log.tiltCorr(1,frameIndex) = tiltCorrection;
end
% Align the heading once there has been enough time for the filter to
% settle and the tilt corrections have dropped below a threshold
if (((time > 5.0 && tiltCorrection < 1e-4) || (time > 30.0)) && headingAligned==0)
% calculate the initial heading using magnetometer, gimbal,
% estimated tilt and declination
quat = AlignHeading(gPhi,gPsi,gTheta,Tsn,magMeas,quat,declParam);
headingAligned = 1;
end
% fuse magnetometer measurements and log fusion data
if (headingAligned == 1)
[quat,states,covariance,decInnov,decInnovVar] = FuseMagnetometer(quat,states,covariance,magMeas,declParam,gPhi,gPsi,gTheta);
log.decInnov(:,frameIndex) = decInnov;
log.decInnovVar(:,frameIndex) = decInnovVar;
end
% time stamp the log data
log.time(frameIndex) = time;
% write to the output data
quatOut = quat;
angRateBiasOut = states(7:9)/dtSlow;
logOut = log;
headingAlignedOut = headingAligned;
end

0
libraries/AP_NavEKF/Models/GimbalEstimatorExample/calcF.c Normal file
View File

0
libraries/AP_NavEKF/Models/GimbalEstimatorExample/calcF.cpp Normal file
View File

68
libraries/AP_NavEKF/Models/GimbalEstimatorExample/calcF.m Normal file
View File

@ -0,0 +1,68 @@
function F = calcF(dax,dax_b,day,day_b,daz,daz_b,dvx,dvx_b,dvy,dvy_b,dvz,dvz_b,q0,q1,q2,q3)
%CALCF
% F = CALCF(DAX,DAX_B,DAY,DAY_B,DAZ,DAZ_B,DVX,DVX_B,DVY,DVY_B,DVZ,DVZ_B,Q0,Q1,Q2,Q3)
% This function was generated by the Symbolic Math Toolbox version 6.1.
% 15-Feb-2015 15:45:52
t2 = dax.*(1.0./2.0);
t3 = dax_b.*(1.0./2.0);
t4 = t2-t3;
t5 = day.*(1.0./2.0);
t6 = day_b.*(1.0./2.0);
t7 = t5-t6;
t8 = daz.*(1.0./2.0);
t9 = daz_b.*(1.0./2.0);
t10 = t8-t9;
t11 = q2.*t4.*(1.0./2.0);
t12 = q1.*t7.*(1.0./2.0);
t13 = q0.*t10.*(1.0./2.0);
t14 = q2.*(1.0./2.0);
t15 = q3.*t4.*(1.0./2.0);
t16 = q1.*t10.*(1.0./2.0);
t17 = q1.*(1.0./2.0);
t18 = q0.*t4.*(1.0./2.0);
t19 = q3.*t7.*(1.0./2.0);
t20 = q0.*(1.0./2.0);
t21 = q2.*t7.*(1.0./2.0);
t22 = q3.*t10.*(1.0./2.0);
t23 = q0.*t7.*(1.0./2.0);
t24 = q3.*(1.0./2.0);
t25 = q1.*t4.*(1.0./2.0);
t26 = q2.*t10.*(1.0./2.0);
t27 = t20-t21+t22+t25;
t28 = -t17+t18+t19+t26;
t29 = t14-t15+t16+t23;
t30 = t11+t12-t13+t24;
t31 = t17-t18+t19+t26;
t32 = t20+t21-t22+t25;
t33 = t11-t12+t13+t24;
t34 = -t14+t15+t16+t23;
t35 = q0.^2;
t36 = q1.^2;
t37 = q2.^2;
t38 = q3.^2;
t39 = -t35-t36-t37-t38;
t40 = t14+t15+t16-t23;
t41 = t11+t12+t13-t24;
t42 = t20+t21+t22-t25;
t43 = t17+t18+t19-t26;
t44 = dvz-dvz_b;
t45 = q0.*q2.*2.0;
t46 = q1.*q3.*2.0;
t47 = t45+t46;
t48 = dvy-dvy_b;
t49 = t35+t36-t37-t38;
t50 = dvx-dvx_b;
t51 = q0.*q3.*2.0;
t53 = q1.*q2.*2.0;
t52 = t51-t53;
t54 = q0.*q1.*2.0;
t58 = q2.*q3.*2.0;
t55 = t54-t58;
t56 = t35-t36+t37-t38;
t57 = t51+t53;
t59 = t35-t36-t37+t38;
t60 = t54+t58;
t61 = t45-t46;
F = reshape([q3.*(q3.*(-1.0./2.0)+t11+t12+t13).*-2.0+q2.*(t14+t15+t16-q0.*t7.*(1.0./2.0)).*2.0+q1.*(t17+t18+t19-q2.*t10.*(1.0./2.0)).*2.0+q0.*(t20+t21+t22-q1.*t4.*(1.0./2.0)).*2.0,q0.*t41.*-2.0-q1.*t40.*2.0+q2.*t43.*2.0-q3.*t42.*2.0,q0.*t40.*-2.0+q1.*t41.*2.0+q2.*t42.*2.0+q3.*t43.*2.0,t47.*t48+t44.*t52,-t44.*t56-t48.*t55,-t44.*t60+t48.*t59,0.0,0.0,0.0,0.0,0.0,0.0,q0.*t30.*-2.0+q1.*t29.*2.0+q2.*t28.*2.0+q3.*t27.*2.0,q0.*t27.*2.0-q1.*t28.*2.0+q2.*t29.*2.0+q3.*t30.*2.0,q0.*t28.*-2.0-q1.*t27.*2.0-q2.*t30.*2.0+q3.*t29.*2.0,t44.*t49-t47.*t50,t44.*t57+t50.*t55,-t44.*t61-t50.*t59,0.0,0.0,0.0,0.0,0.0,0.0,q0.*t34.*-2.0+q1.*t33.*2.0-q2.*t32.*2.0-q3.*t31.*2.0,q0.*t31.*-2.0+q1.*t32.*2.0+q2.*t33.*2.0+q3.*t34.*2.0,q0.*t32.*2.0+q1.*t31.*2.0-q2.*t34.*2.0+q3.*t33.*2.0,-t48.*t49-t50.*t52,-t48.*t57+t50.*t56,t48.*t61+t50.*t60,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,1.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,1.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,1.0,0.0,0.0,0.0,0.0,0.0,0.0,t39,0.0,0.0,0.0,0.0,0.0,1.0,0.0,0.0,0.0,0.0,0.0,0.0,t39,0.0,0.0,0.0,0.0,0.0,1.0,0.0,0.0,0.0,0.0,0.0,0.0,t39,0.0,0.0,0.0,0.0,0.0,1.0,0.0,0.0,0.0,0.0,0.0,0.0,-t35-t36+t37+t38,-t51-t53,t61,0.0,0.0,0.0,1.0,0.0,0.0,0.0,0.0,0.0,t52,-t35+t36-t37+t38,-t54-t58,0.0,0.0,0.0,0.0,1.0,0.0,0.0,0.0,0.0,-t45-t46,t55,-t35+t36+t37-t38,0.0,0.0,0.0,0.0,0.0,1.0],[12, 12]);

75
libraries/AP_NavEKF/Models/GimbalEstimatorExample/calcH_MAG.c Normal file
View File

@ -0,0 +1,75 @@
t2 = cos(gPhi);
t3 = cos(gTheta);
t4 = sin(gPhi);
t5 = sin(gTheta);
t6 = q0*q0;
t7 = q1*q1;
t8 = q2*q2;
t9 = q3*q3;
t10 = t6+t7-t8-t9;
t11 = sin(gPsi);
t12 = cos(gPsi);
t13 = q0*q2*2.0;
t14 = q1*q3*2.0;
t15 = t13+t14;
t16 = q0*q3*2.0;
t18 = q1*q2*2.0;
t17 = t16-t18;
t19 = t3*t11;
t20 = t4*t5*t12;
t21 = t19+t20;
t22 = t16+t18;
t23 = t5*t11;
t41 = t3*t4*t12;
t24 = t23-t41;
t25 = q0*q1*2.0;
t31 = q2*q3*2.0;
t26 = t25-t31;
t27 = t6-t7+t8-t9;
t28 = t5*t12;
t29 = t3*t4*t11;
t30 = t28+t29;
t32 = t3*t12;
t46 = t4*t5*t11;
t33 = t32-t46;
t35 = t4*t17;
t36 = t2*t5*t10;
t37 = t2*t3*t15;
t38 = t35+t36-t37;
t39 = magZ*t38;
t40 = t10*t21;
t42 = t15*t24;
t43 = t2*t12*t17;
t44 = t40+t42-t43;
t45 = magY*t44;
t47 = t10*t33;
t48 = t15*t30;
t49 = t2*t11*t17;
t50 = t47+t48+t49;
t51 = magX*t50;
t52 = -t39+t45+t51;
t53 = 1.0/t52;
t54 = t4*t27;
t55 = t2*t3*t26;
t56 = t2*t5*t22;
t57 = -t54+t55+t56;
t58 = magZ*t57;
t59 = t21*t22;
t60 = t24*t26;
t61 = t2*t12*t27;
t62 = t59-t60+t61;
t63 = magY*t62;
t64 = t26*t30;
t65 = t22*t33;
t66 = t2*t11*t27;
t67 = t64-t65+t66;
t68 = magX*t67;
t69 = t58-t63+t68;
t70 = t53*t69;
t34 = tan(t70);
t71 = t34*t34;
t72 = t71+1.0;
t73 = 1.0/(t52*t52);
A0[0][0] = -t72*(t53*(magZ*(t4*t26+t2*t3*t27)+magY*(t24*t27+t2*t12*t26)+magX*(t27*t30-t2*t11*t26))-t69*t73*(magZ*(t4*t15+t2*t3*t17)+magY*(t17*t24+t2*t12*t15)+magX*(t17*t30-t2*t11*t15)));
A0[0][1] = t72*(t53*(magZ*(t2*t3*t22-t2*t5*t26)+magY*(t22*t24+t21*t26)+magX*(t22*t30+t26*t33))+t69*t73*(magZ*(t2*t3*t10+t2*t5*t15)+magY*(t10*t24-t15*t21)+magX*(t10*t30-t15*t33)));
A0[0][2] = t72*(t53*(-magZ*(t4*t22+t2*t5*t27)+magY*(t21*t27-t2*t12*t22)+magX*(t27*t33+t2*t11*t22))-t69*t73*(magZ*(t4*t10-t2*t5*t17)+magY*(t17*t21+t2*t10*t12)+magX*(t17*t33-t2*t10*t11)));

75
libraries/AP_NavEKF/Models/GimbalEstimatorExample/calcH_MAG.cpp Normal file
View File

@ -0,0 +1,75 @@
float t2 = cos(gPhi);
float t3 = cos(gTheta);
float t4 = sin(gPhi);
float t5 = sin(gTheta);
float t6 = q0*q0;
float t7 = q1*q1;
float t8 = q2*q2;
float t9 = q3*q3;
float t10 = t6+t7-t8-t9;
float t11 = sin(gPsi);
float t12 = cos(gPsi);
float t13 = q0*q2*2.0f;
float t14 = q1*q3*2.0f;
float t15 = t13+t14;
float t16 = q0*q3*2.0f;
float t18 = q1*q2*2.0f;
float t17 = t16-t18;
float t19 = t3*t11;
float t20 = t4*t5*t12;
float t21 = t19+t20;
float t22 = t16+t18;
float t23 = t5*t11;
float t41 = t3*t4*t12;
float t24 = t23-t41;
float t25 = q0*q1*2.0f;
float t31 = q2*q3*2.0f;
float t26 = t25-t31;
float t27 = t6-t7+t8-t9;
float t28 = t5*t12;
float t29 = t3*t4*t11;
float t30 = t28+t29;
float t32 = t3*t12;
float t46 = t4*t5*t11;
float t33 = t32-t46;
float t35 = t4*t17;
float t36 = t2*t5*t10;
float t37 = t2*t3*t15;
float t38 = t35+t36-t37;
float t39 = magZ*t38;
float t40 = t10*t21;
float t42 = t15*t24;
float t43 = t2*t12*t17;
float t44 = t40+t42-t43;
float t45 = magY*t44;
float t47 = t10*t33;
float t48 = t15*t30;
float t49 = t2*t11*t17;
float t50 = t47+t48+t49;
float t51 = magX*t50;
float t52 = -t39+t45+t51;
float t53 = 1.0/t52;
float t54 = t4*t27;
float t55 = t2*t3*t26;
float t56 = t2*t5*t22;
float t57 = -t54+t55+t56;
float t58 = magZ*t57;
float t59 = t21*t22;
float t60 = t24*t26;
float t61 = t2*t12*t27;
float t62 = t59-t60+t61;
float t63 = magY*t62;
float t64 = t26*t30;
float t65 = t22*t33;
float t66 = t2*t11*t27;
float t67 = t64-t65+t66;
float t68 = magX*t67;
float t69 = t58-t63+t68;
float t70 = t53*t69;
float t34 = tan(t70);
float t71 = t34*t34;
float t72 = t71+1.0;
float t73 = 1.0/(t52*t52);
A0[0][0] = -t72*(t53*(magZ*(t4*t26+t2*t3*t27)+magY*(t24*t27+t2*t12*t26)+magX*(t27*t30-t2*t11*t26))-t69*t73*(magZ*(t4*t15+t2*t3*t17)+magY*(t17*t24+t2*t12*t15)+magX*(t17*t30-t2*t11*t15)));
A0[0][1] = t72*(t53*(magZ*(t2*t3*t22-t2*t5*t26)+magY*(t22*t24+t21*t26)+magX*(t22*t30+t26*t33))+t69*t73*(magZ*(t2*t3*t10+t2*t5*t15)+magY*(t10*t24-t15*t21)+magX*(t10*t30-t15*t33)));
A0[0][2] = t72*(t53*(-magZ*(t4*t22+t2*t5*t27)+magY*(t21*t27-t2*t12*t22)+magX*(t27*t33+t2*t11*t22))-t69*t73*(magZ*(t4*t10-t2*t5*t17)+magY*(t17*t21+t2*t10*t12)+magX*(t17*t33-t2*t10*t11)));

80
libraries/AP_NavEKF/Models/GimbalEstimatorExample/calcH_MAG.m Normal file
View File

@ -0,0 +1,80 @@
function H_MAG = calcH_MAG(gPhi,gPsi,gTheta,magX,magY,magZ,q0,q1,q2,q3)
%CALCH_MAG
% H_MAG = CALCH_MAG(GPHI,GPSI,GTHETA,MAGX,MAGY,MAGZ,Q0,Q1,Q2,Q3)
% This function was generated by the Symbolic Math Toolbox version 6.1.
% 15-Feb-2015 16:00:09
t2 = cos(gPhi);
t3 = cos(gTheta);
t4 = sin(gPhi);
t5 = sin(gTheta);
t6 = q0.^2;
t7 = q1.^2;
t8 = q2.^2;
t9 = q3.^2;
t10 = t6+t7-t8-t9;
t11 = sin(gPsi);
t12 = cos(gPsi);
t13 = q0.*q2.*2.0;
t14 = q1.*q3.*2.0;
t15 = t13+t14;
t16 = q0.*q3.*2.0;
t18 = q1.*q2.*2.0;
t17 = t16-t18;
t19 = t3.*t11;
t20 = t4.*t5.*t12;
t21 = t19+t20;
t22 = t16+t18;
t23 = t5.*t11;
t41 = t3.*t4.*t12;
t24 = t23-t41;
t25 = q0.*q1.*2.0;
t31 = q2.*q3.*2.0;
t26 = t25-t31;
t27 = t6-t7+t8-t9;
t28 = t5.*t12;
t29 = t3.*t4.*t11;
t30 = t28+t29;
t32 = t3.*t12;
t46 = t4.*t5.*t11;
t33 = t32-t46;
t35 = t4.*t17;
t36 = t2.*t5.*t10;
t37 = t2.*t3.*t15;
t38 = t35+t36-t37;
t39 = magZ.*t38;
t40 = t10.*t21;
t42 = t15.*t24;
t43 = t2.*t12.*t17;
t44 = t40+t42-t43;
t45 = magY.*t44;
t47 = t10.*t33;
t48 = t15.*t30;
t49 = t2.*t11.*t17;
t50 = t47+t48+t49;
t51 = magX.*t50;
t52 = -t39+t45+t51;
t53 = 1.0./t52;
t54 = t4.*t27;
t55 = t2.*t3.*t26;
t56 = t2.*t5.*t22;
t57 = -t54+t55+t56;
t58 = magZ.*t57;
t59 = t21.*t22;
t60 = t24.*t26;
t61 = t2.*t12.*t27;
t62 = t59-t60+t61;
t63 = magY.*t62;
t64 = t26.*t30;
t65 = t22.*t33;
t66 = t2.*t11.*t27;
t67 = t64-t65+t66;
t68 = magX.*t67;
t69 = t58-t63+t68;
t70 = t53.*t69;
t34 = tan(t70);
t71 = t34.^2;
t72 = t71+1.0;
t73 = 1.0./t52.^2;
H_MAG = [-t72.*(t53.*(magZ.*(t4.*t26+t2.*t3.*t27)+magY.*(t24.*t27+t2.*t12.*t26)+magX.*(t27.*t30-t2.*t11.*t26))-t69.*t73.*(magZ.*(t4.*t15+t2.*t3.*t17)+magY.*(t17.*t24+t2.*t12.*t15)+magX.*(t17.*t30-t2.*t11.*t15))),t72.*(t53.*(magZ.*(t2.*t3.*t22-t2.*t5.*t26)+magY.*(t22.*t24+t21.*t26)+magX.*(t22.*t30+t26.*t33))+t69.*t73.*(magZ.*(t2.*t3.*t10+t2.*t5.*t15)+magY.*(t10.*t24-t15.*t21)+magX.*(t10.*t30-t15.*t33))),t72.*(t53.*(-magZ.*(t4.*t22+t2.*t5.*t27)+magY.*(t21.*t27-t2.*t12.*t22)+magX.*(t27.*t33+t2.*t11.*t22))-t69.*t73.*(magZ.*(t4.*t10-t2.*t5.*t17)+magY.*(t17.*t21+t2.*t10.*t12)+magX.*(t17.*t33-t2.*t10.*t11))),0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0];

18
libraries/AP_NavEKF/Models/GimbalEstimatorExample/calcMagAng.m Normal file
View File

@ -0,0 +1,18 @@
function angMeas = calcMagAng(decl,gPhi,gPsi,gTheta,magX,magY,magZ,q0,q1,q2,q3)
%CALCMAGANG
% ANGMEAS = CALCMAGANG(DECL,GPHI,GPSI,GTHETA,MAGX,MAGY,MAGZ,Q0,Q1,Q2,Q3)
% This function was generated by the Symbolic Math Toolbox version 5.8.
% 14-Jan-2015 16:51:18
% Define rotation from magnetometer to sensor using a 312 rotation sequence
Tms = calcTms(gPhi,gPsi,gTheta);
% Define rotation from sensor to NED
Tsn = Quat2Tbn([q0;q1;q2;q3]);
% Define rotation from magnetometer to NED axes
Tmn = Tsn*Tms;
% rotate magentic 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 = atan2(magMeasNED(2),magMeasNED(1)) - decl;

556
libraries/AP_NavEKF/Models/GimbalEstimatorExample/calcP.c Normal file
View File

@ -0,0 +1,556 @@
t2 = dax*(1.0/2.0);
t3 = dax_b*(1.0/2.0);
t4 = t2-t3;
t5 = day*(1.0/2.0);
t6 = day_b*(1.0/2.0);
t7 = t5-t6;
t8 = daz*(1.0/2.0);
t9 = daz_b*(1.0/2.0);
t10 = t8-t9;
t11 = q2*t4*(1.0/2.0);
t12 = q1*t7*(1.0/2.0);
t13 = q0*t10*(1.0/2.0);
t14 = q2*(1.0/2.0);
t15 = q3*t4*(1.0/2.0);
t16 = q1*t10*(1.0/2.0);
t17 = q1*(1.0/2.0);
t18 = q0*t4*(1.0/2.0);
t19 = q3*t7*(1.0/2.0);
t20 = q0*(1.0/2.0);
t21 = q2*t7*(1.0/2.0);
t22 = q3*t10*(1.0/2.0);
t23 = q0*t7*(1.0/2.0);
t24 = q3*(1.0/2.0);
t25 = q1*t4*(1.0/2.0);
t26 = q2*t10*(1.0/2.0);
t27 = t11+t12+t13-t24;
t28 = t14+t15+t16-t23;
t29 = q2*t28*2.0;
t30 = t17+t18+t19-t26;
t31 = q1*t30*2.0;
t32 = t20+t21+t22-t25;
t33 = q0*t32*2.0;
t40 = q3*t27*2.0;
t34 = t29+t31+t33-t40;
t35 = q0*q0;
t36 = q1*q1;
t37 = q2*q2;
t38 = q3*q3;
t39 = t35+t36+t37+t38;
t41 = t11+t12-t13+t24;
t42 = t14-t15+t16+t23;
t43 = q1*t42*2.0;
t44 = -t17+t18+t19+t26;
t45 = q2*t44*2.0;
t46 = t20-t21+t22+t25;
t47 = q3*t46*2.0;
t57 = q0*t41*2.0;
t48 = t43+t45+t47-t57;
t49 = -t14+t15+t16+t23;
t50 = q0*t49*2.0;
t51 = t11-t12+t13+t24;
t52 = t20+t21-t22+t25;
t53 = q2*t52*2.0;
t54 = t17-t18+t19+t26;
t55 = q3*t54*2.0;
t58 = q1*t51*2.0;
t56 = t50+t53+t55-t58;
t59 = OP_l_1_c_1_r_*t34;
t60 = OP_l_2_c_1_r_*t48;
t66 = OP_l_7_c_1_r_*t39;
t67 = OP_l_3_c_1_r_*t56;
t61 = t59+t60-t66-t67;
t62 = OP_l_1_c_2_r_*t34;
t63 = OP_l_2_c_2_r_*t48;
t64 = OP_l_1_c_3_r_*t34;
t65 = OP_l_2_c_3_r_*t48;
t72 = OP_l_7_c_2_r_*t39;
t73 = OP_l_3_c_2_r_*t56;
t68 = t62+t63-t72-t73;
t69 = t35+t36-t37-t38;
t86 = OP_l_7_c_3_r_*t39;
t87 = OP_l_3_c_3_r_*t56;
t70 = t64+t65-t86-t87;
t71 = dvx-dvx_b;
t74 = q0*q3*2.0;
t81 = q1*q2*2.0;
t75 = t74-t81;
t76 = q0*q2*2.0;
t77 = q1*q3*2.0;
t78 = t76+t77;
t79 = dvy-dvy_b;
t80 = dvz-dvz_b;
t82 = OP_l_1_c_11_r_*t34;
t83 = OP_l_2_c_11_r_*t48;
t103 = OP_l_7_c_11_r_*t39;
t104 = OP_l_3_c_11_r_*t56;
t84 = t82+t83-t103-t104;
t85 = t35-t36+t37-t38;
t88 = t74+t81;
t89 = OP_l_1_c_10_r_*t34;
t90 = OP_l_2_c_10_r_*t48;
t100 = OP_l_7_c_10_r_*t39;
t101 = OP_l_3_c_10_r_*t56;
t91 = t89+t90-t100-t101;
t92 = q0*q1*2.0;
t96 = q2*q3*2.0;
t93 = t92-t96;
t94 = OP_l_1_c_12_r_*t34;
t95 = OP_l_2_c_12_r_*t48;
t114 = OP_l_7_c_12_r_*t39;
t115 = OP_l_3_c_12_r_*t56;
t97 = t94+t95-t114-t115;
t98 = t35-t36-t37+t38;
t99 = t76-t77;
t102 = t92+t96;
t105 = OP_l_1_c_7_r_*t34;
t106 = OP_l_2_c_7_r_*t48;
t411 = OP_l_7_c_7_r_*t39;
t107 = t105+t106-t411-OP_l_3_c_7_r_*t56;
t108 = OP_l_1_c_8_r_*t34;
t109 = OP_l_2_c_8_r_*t48;
t412 = OP_l_7_c_8_r_*t39;
t110 = t108+t109-t412-OP_l_3_c_8_r_*t56;
t111 = OP_l_1_c_9_r_*t34;
t112 = OP_l_2_c_9_r_*t48;
t413 = OP_l_7_c_9_r_*t39;
t113 = t111+t112-t413-OP_l_3_c_9_r_*t56;
t116 = q0*t27*2.0;
t117 = q1*t28*2.0;
t118 = q3*t32*2.0;
t128 = q2*t30*2.0;
t119 = t116+t117+t118-t128;
t120 = q0*t46*2.0;
t121 = q2*t42*2.0;
t122 = q3*t41*2.0;
t129 = q1*t44*2.0;
t123 = t120+t121+t122-t129;
t124 = q1*t52*2.0;
t125 = q2*t51*2.0;
t126 = q3*t49*2.0;
t130 = q0*t54*2.0;
t127 = t124+t125+t126-t130;
t131 = OP_l_8_c_1_r_*t39;
t132 = OP_l_1_c_1_r_*t119;
t141 = OP_l_2_c_1_r_*t123;
t142 = OP_l_3_c_1_r_*t127;
t133 = t131+t132-t141-t142;
t134 = OP_l_8_c_2_r_*t39;
t135 = OP_l_1_c_2_r_*t119;
t147 = OP_l_2_c_2_r_*t123;
t148 = OP_l_3_c_2_r_*t127;
t136 = t134+t135-t147-t148;
t137 = OP_l_8_c_3_r_*t39;
t138 = OP_l_1_c_3_r_*t119;
t153 = OP_l_2_c_3_r_*t123;
t154 = OP_l_3_c_3_r_*t127;
t139 = t137+t138-t153-t154;
t140 = t39*t39;
t143 = q1*t27*2.0;
t144 = q2*t32*2.0;
t145 = q3*t30*2.0;
t204 = q0*t28*2.0;
t146 = t143+t144+t145-t204;
t149 = q0*t44*2.0;
t150 = q1*t46*2.0;
t151 = q2*t41*2.0;
t205 = q3*t42*2.0;
t152 = t149+t150+t151-t205;
t155 = q0*t52*2.0;
t156 = q1*t54*2.0;
t157 = q3*t51*2.0;
t206 = q2*t49*2.0;
t158 = t155+t156+t157-t206;
t159 = t69*t79;
t160 = t71*t75;
t161 = t159+t160;
t162 = t69*t80;
t222 = t71*t78;
t163 = t162-t222;
t164 = t78*t79;
t165 = t75*t80;
t166 = t164+t165;
t167 = OP_l_8_c_11_r_*t39;
t168 = OP_l_1_c_11_r_*t119;
t193 = OP_l_2_c_11_r_*t123;
t194 = OP_l_3_c_11_r_*t127;
t169 = t167+t168-t193-t194;
t170 = t71*t85;
t226 = t79*t88;
t171 = t170-t226;
t172 = t80*t85;
t173 = t79*t93;
t174 = t172+t173;
t175 = OP_l_8_c_10_r_*t39;
t176 = OP_l_1_c_10_r_*t119;
t191 = OP_l_2_c_10_r_*t123;
t192 = OP_l_3_c_10_r_*t127;
t177 = t175+t176-t191-t192;
t178 = OP_l_8_c_12_r_*t39;
t179 = OP_l_1_c_12_r_*t119;
t184 = OP_l_2_c_12_r_*t123;
t185 = OP_l_3_c_12_r_*t127;
t180 = t178+t179-t184-t185;
t181 = t71*t93;
t182 = t80*t88;
t183 = t181+t182;
t186 = t71*t98;
t187 = t80*t99;
t188 = t186+t187;
t189 = t79*t98;
t233 = t80*t102;
t190 = t189-t233;
t195 = t71*t102;
t196 = t79*t99;
t197 = t195+t196;
t198 = OP_l_8_c_7_r_*t39;
t199 = OP_l_1_c_7_r_*t119;
t200 = OP_l_8_c_8_r_*t39;
t201 = OP_l_1_c_8_r_*t119;
t202 = OP_l_8_c_9_r_*t39;
t203 = OP_l_1_c_9_r_*t119;
t207 = OP_l_9_c_1_r_*t39;
t208 = OP_l_2_c_1_r_*t152;
t216 = OP_l_1_c_1_r_*t146;
t217 = OP_l_3_c_1_r_*t158;
t209 = t207+t208-t216-t217;
t210 = OP_l_9_c_2_r_*t39;
t211 = OP_l_2_c_2_r_*t152;
t218 = OP_l_1_c_2_r_*t146;
t219 = OP_l_3_c_2_r_*t158;
t212 = t210+t211-t218-t219;
t213 = OP_l_9_c_3_r_*t39;
t214 = OP_l_2_c_3_r_*t152;
t220 = OP_l_1_c_3_r_*t146;
t221 = OP_l_3_c_3_r_*t158;
t215 = t213+t214-t220-t221;
t223 = OP_l_1_c_11_r_*t146;
t224 = OP_l_3_c_11_r_*t158;
t236 = OP_l_9_c_11_r_*t39;
t237 = OP_l_2_c_11_r_*t152;
t225 = t223+t224-t236-t237;
t227 = OP_l_1_c_10_r_*t146;
t228 = OP_l_3_c_10_r_*t158;
t234 = OP_l_9_c_10_r_*t39;
t235 = OP_l_2_c_10_r_*t152;
t229 = t227+t228-t234-t235;
t230 = OP_l_1_c_12_r_*t146;
t231 = OP_l_3_c_12_r_*t158;
t244 = OP_l_9_c_12_r_*t39;
t245 = OP_l_2_c_12_r_*t152;
t232 = t230+t231-t244-t245;
t238 = OP_l_9_c_7_r_*t39;
t239 = OP_l_2_c_7_r_*t152;
t240 = OP_l_9_c_8_r_*t39;
t241 = OP_l_2_c_8_r_*t152;
t242 = OP_l_9_c_9_r_*t39;
t243 = OP_l_2_c_9_r_*t152;
t246 = OP_l_2_c_1_r_*t163;
t247 = OP_l_11_c_1_r_*t75;
t248 = OP_l_1_c_1_r_*t166;
t258 = OP_l_10_c_1_r_*t69;
t259 = OP_l_3_c_1_r_*t161;
t260 = OP_l_12_c_1_r_*t78;
t249 = OP_l_4_c_1_r_+t246+t247+t248-t258-t259-t260;
t250 = OP_l_2_c_2_r_*t163;
t251 = OP_l_11_c_2_r_*t75;
t252 = OP_l_1_c_2_r_*t166;
t261 = OP_l_10_c_2_r_*t69;
t262 = OP_l_3_c_2_r_*t161;
t263 = OP_l_12_c_2_r_*t78;
t253 = OP_l_4_c_2_r_+t250+t251+t252-t261-t262-t263;
t254 = OP_l_2_c_3_r_*t163;
t255 = OP_l_11_c_3_r_*t75;
t256 = OP_l_1_c_3_r_*t166;
t264 = OP_l_10_c_3_r_*t69;
t265 = OP_l_3_c_3_r_*t161;
t266 = OP_l_12_c_3_r_*t78;
t257 = OP_l_4_c_3_r_+t254+t255+t256-t264-t265-t266;
t267 = OP_l_2_c_10_r_*t163;
t268 = OP_l_11_c_10_r_*t75;
t269 = OP_l_1_c_10_r_*t166;
t279 = OP_l_10_c_10_r_*t69;
t280 = OP_l_3_c_10_r_*t161;
t281 = OP_l_12_c_10_r_*t78;
t270 = OP_l_4_c_10_r_+t267+t268+t269-t279-t280-t281;
t271 = OP_l_2_c_12_r_*t163;
t272 = OP_l_11_c_12_r_*t75;
t273 = OP_l_1_c_12_r_*t166;
t285 = OP_l_10_c_12_r_*t69;
t286 = OP_l_3_c_12_r_*t161;
t287 = OP_l_12_c_12_r_*t78;
t274 = OP_l_4_c_12_r_+t271+t272+t273-t285-t286-t287;
t275 = OP_l_2_c_11_r_*t163;
t276 = OP_l_11_c_11_r_*t75;
t277 = OP_l_1_c_11_r_*t166;
t282 = OP_l_10_c_11_r_*t69;
t283 = OP_l_3_c_11_r_*t161;
t284 = OP_l_12_c_11_r_*t78;
t278 = OP_l_4_c_11_r_+t275+t276+t277-t282-t283-t284;
t288 = OP_l_2_c_7_r_*t163;
t289 = OP_l_11_c_7_r_*t75;
t290 = OP_l_1_c_7_r_*t166;
t291 = OP_l_4_c_7_r_+t288+t289+t290-OP_l_10_c_7_r_*t69-OP_l_3_c_7_r_*t161-OP_l_12_c_7_r_*t78;
t292 = OP_l_2_c_8_r_*t163;
t293 = OP_l_11_c_8_r_*t75;
t294 = OP_l_1_c_8_r_*t166;
t295 = OP_l_4_c_8_r_+t292+t293+t294-OP_l_10_c_8_r_*t69-OP_l_3_c_8_r_*t161-OP_l_12_c_8_r_*t78;
t296 = OP_l_2_c_9_r_*t163;
t297 = OP_l_11_c_9_r_*t75;
t298 = OP_l_1_c_9_r_*t166;
t299 = OP_l_4_c_9_r_+t296+t297+t298-OP_l_10_c_9_r_*t69-OP_l_3_c_9_r_*t161-OP_l_12_c_9_r_*t78;
t300 = OP_l_3_c_1_r_*t171;
t301 = OP_l_12_c_1_r_*t93;
t302 = OP_l_2_c_1_r_*t183;
t312 = OP_l_11_c_1_r_*t85;
t313 = OP_l_1_c_1_r_*t174;
t314 = OP_l_10_c_1_r_*t88;
t303 = OP_l_5_c_1_r_+t300+t301+t302-t312-t313-t314;
t304 = OP_l_3_c_2_r_*t171;
t305 = OP_l_12_c_2_r_*t93;
t306 = OP_l_2_c_2_r_*t183;
t315 = OP_l_11_c_2_r_*t85;
t316 = OP_l_1_c_2_r_*t174;
t317 = OP_l_10_c_2_r_*t88;
t307 = OP_l_5_c_2_r_+t304+t305+t306-t315-t316-t317;
t308 = OP_l_3_c_3_r_*t171;
t309 = OP_l_12_c_3_r_*t93;
t310 = OP_l_2_c_3_r_*t183;
t318 = OP_l_11_c_3_r_*t85;
t319 = OP_l_1_c_3_r_*t174;
t320 = OP_l_10_c_3_r_*t88;
t311 = OP_l_5_c_3_r_+t308+t309+t310-t318-t319-t320;
t321 = dvxNoise*t69*t88;
t322 = OP_l_3_c_10_r_*t171;
t323 = OP_l_12_c_10_r_*t93;
t324 = OP_l_2_c_10_r_*t183;
t334 = OP_l_11_c_10_r_*t85;
t335 = OP_l_1_c_10_r_*t174;
t336 = OP_l_10_c_10_r_*t88;
t325 = OP_l_5_c_10_r_+t322+t323+t324-t334-t335-t336;
t326 = OP_l_3_c_12_r_*t171;
t327 = OP_l_12_c_12_r_*t93;
t328 = OP_l_2_c_12_r_*t183;
t340 = OP_l_11_c_12_r_*t85;
t341 = OP_l_1_c_12_r_*t174;
t342 = OP_l_10_c_12_r_*t88;
t329 = OP_l_5_c_12_r_+t326+t327+t328-t340-t341-t342;
t330 = OP_l_3_c_11_r_*t171;
t331 = OP_l_12_c_11_r_*t93;
t332 = OP_l_2_c_11_r_*t183;
t337 = OP_l_11_c_11_r_*t85;
t338 = OP_l_1_c_11_r_*t174;
t339 = OP_l_10_c_11_r_*t88;
t333 = OP_l_5_c_11_r_+t330+t331+t332-t337-t338-t339;
t343 = OP_l_3_c_7_r_*t171;
t344 = OP_l_12_c_7_r_*t93;
t345 = OP_l_2_c_7_r_*t183;
t346 = OP_l_5_c_7_r_+t343+t344+t345-OP_l_1_c_7_r_*t174-OP_l_10_c_7_r_*t88-OP_l_11_c_7_r_*t85;
t347 = OP_l_3_c_8_r_*t171;
t348 = OP_l_12_c_8_r_*t93;
t349 = OP_l_2_c_8_r_*t183;
t350 = OP_l_5_c_8_r_+t347+t348+t349-OP_l_1_c_8_r_*t174-OP_l_10_c_8_r_*t88-OP_l_11_c_8_r_*t85;
t351 = OP_l_3_c_9_r_*t171;
t352 = OP_l_12_c_9_r_*t93;
t353 = OP_l_2_c_9_r_*t183;
t354 = OP_l_5_c_9_r_+t351+t352+t353-OP_l_1_c_9_r_*t174-OP_l_10_c_9_r_*t88-OP_l_11_c_9_r_*t85;
t355 = OP_l_1_c_1_r_*t190;
t356 = OP_l_10_c_1_r_*t99;
t357 = OP_l_3_c_1_r_*t197;
t367 = OP_l_12_c_1_r_*t98;
t368 = OP_l_2_c_1_r_*t188;
t369 = OP_l_11_c_1_r_*t102;
t358 = OP_l_6_c_1_r_+t355+t356+t357-t367-t368-t369;
t359 = OP_l_1_c_2_r_*t190;
t360 = OP_l_10_c_2_r_*t99;
t361 = OP_l_3_c_2_r_*t197;
t370 = OP_l_12_c_2_r_*t98;
t371 = OP_l_2_c_2_r_*t188;
t372 = OP_l_11_c_2_r_*t102;
t362 = OP_l_6_c_2_r_+t359+t360+t361-t370-t371-t372;
t363 = OP_l_1_c_3_r_*t190;
t364 = OP_l_10_c_3_r_*t99;
t365 = OP_l_3_c_3_r_*t197;
t373 = OP_l_12_c_3_r_*t98;
t374 = OP_l_2_c_3_r_*t188;
t375 = OP_l_11_c_3_r_*t102;
t366 = OP_l_6_c_3_r_+t363+t364+t365-t373-t374-t375;
t376 = dvzNoise*t78*t98;
t377 = OP_l_1_c_10_r_*t190;
t378 = OP_l_10_c_10_r_*t99;
t379 = OP_l_3_c_10_r_*t197;
t390 = OP_l_12_c_10_r_*t98;
t391 = OP_l_2_c_10_r_*t188;
t392 = OP_l_11_c_10_r_*t102;
t380 = OP_l_6_c_10_r_+t377+t378+t379-t390-t391-t392;
t381 = OP_l_1_c_12_r_*t190;
t382 = OP_l_10_c_12_r_*t99;
t383 = OP_l_3_c_12_r_*t197;
t396 = OP_l_12_c_12_r_*t98;
t397 = OP_l_2_c_12_r_*t188;
t398 = OP_l_11_c_12_r_*t102;
t384 = OP_l_6_c_12_r_+t381+t382+t383-t396-t397-t398;
t385 = OP_l_1_c_11_r_*t190;
t386 = OP_l_10_c_11_r_*t99;
t387 = OP_l_3_c_11_r_*t197;
t393 = OP_l_12_c_11_r_*t98;
t394 = OP_l_2_c_11_r_*t188;
t395 = OP_l_11_c_11_r_*t102;
t388 = OP_l_6_c_11_r_+t385+t386+t387-t393-t394-t395;
t389 = dvyNoise*t85*t102;
t399 = OP_l_1_c_7_r_*t190;
t400 = OP_l_10_c_7_r_*t99;
t401 = OP_l_3_c_7_r_*t197;
t402 = OP_l_6_c_7_r_+t399+t400+t401-OP_l_2_c_7_r_*t188-OP_l_11_c_7_r_*t102-OP_l_12_c_7_r_*t98;
t403 = OP_l_1_c_8_r_*t190;
t404 = OP_l_10_c_8_r_*t99;
t405 = OP_l_3_c_8_r_*t197;
t406 = OP_l_6_c_8_r_+t403+t404+t405-OP_l_2_c_8_r_*t188-OP_l_11_c_8_r_*t102-OP_l_12_c_8_r_*t98;
t407 = OP_l_1_c_9_r_*t190;
t408 = OP_l_10_c_9_r_*t99;
t409 = OP_l_3_c_9_r_*t197;
t410 = OP_l_6_c_9_r_+t407+t408+t409-OP_l_2_c_9_r_*t188-OP_l_11_c_9_r_*t102-OP_l_12_c_9_r_*t98;
A0[0][0] = daxNoise*t140+t34*t61+t48*t68-t56*t70-t39*t107;
A0[0][1] = -t39*t110-t61*t119+t68*t123+t70*t127;
A0[0][2] = -t39*t113-t68*t152+t70*t158+t146*(t59+t60-t66-t67);
A0[0][3] = OP_l_1_c_4_r_*t34+OP_l_2_c_4_r_*t48-OP_l_3_c_4_r_*t56-OP_l_7_c_4_r_*t39+t75*t84-t69*t91-t78*t97-t70*t161+t166*(t59+t60-t66-t67)+t163*(t62+t63-t72-t73);
A0[0][4] = OP_l_1_c_5_r_*t34+OP_l_2_c_5_r_*t48-OP_l_3_c_5_r_*t56-OP_l_7_c_5_r_*t39-t84*t85-t88*t91+t93*t97-t61*t174+t70*t171+t183*(t62+t63-t72-t73);
A0[0][5] = OP_l_1_c_6_r_*t34+OP_l_2_c_6_r_*t48-OP_l_3_c_6_r_*t56-OP_l_7_c_6_r_*t39-t84*t102-t97*t98+t61*t190-t68*t188+t99*(t89+t90-t100-t101)+t197*(t64+t65-t86-t87);
A0[0][6] = t107;
A0[0][7] = t110;
A0[0][8] = t113;
A0[0][9] = t91;
A0[0][10] = t84;
A0[0][11] = t97;
A0[1][0] = -t34*t133-t48*t136+t56*t139+t39*(t198+t199-OP_l_2_c_7_r_*t123-OP_l_3_c_7_r_*t127);
A0[1][1] = dayNoise*t140+t119*t133-t123*t136-t127*t139+t39*(t200+t201-OP_l_2_c_8_r_*t123-OP_l_3_c_8_r_*t127);
A0[1][2] = -t133*t146+t136*t152-t139*t158+t39*(t202+t203-OP_l_2_c_9_r_*t123-OP_l_3_c_9_r_*t127);
A0[1][3] = -OP_l_8_c_4_r_*t39-OP_l_1_c_4_r_*t119+OP_l_2_c_4_r_*t123+OP_l_3_c_4_r_*t127-t75*t169+t69*t177+t78*t180-t133*t166-t136*t163+t139*t161;
A0[1][4] = -OP_l_8_c_5_r_*t39-OP_l_1_c_5_r_*t119+OP_l_2_c_5_r_*t123+OP_l_3_c_5_r_*t127+t85*t169+t88*t177-t93*t180+t133*t174-t139*t171-t136*t183;
A0[1][5] = -OP_l_8_c_6_r_*t39-OP_l_1_c_6_r_*t119+OP_l_2_c_6_r_*t123+OP_l_3_c_6_r_*t127+t102*t169-t99*t177+t98*t180-t133*t190+t136*t188-t139*t197;
A0[1][6] = -t198-t199+OP_l_2_c_7_r_*t123+OP_l_3_c_7_r_*t127;
A0[1][7] = -t200-t201+OP_l_2_c_8_r_*t123+OP_l_3_c_8_r_*t127;
A0[1][8] = -t202-t203+OP_l_2_c_9_r_*t123+OP_l_3_c_9_r_*t127;
A0[1][9] = -t175-t176+t191+t192;
A0[1][10] = -t167-t168+t193+t194;
A0[1][11] = -t178-t179+t184+t185;
A0[2][0] = -t34*t209-t48*t212+t56*t215+t39*(t238+t239-OP_l_1_c_7_r_*t146-OP_l_3_c_7_r_*t158);
A0[2][1] = t119*t209-t123*t212-t127*t215+t39*(t240+t241-OP_l_1_c_8_r_*t146-OP_l_3_c_8_r_*t158);
A0[2][2] = dazNoise*t140-t146*t209+t152*t212-t158*t215+t39*(t242+t243-OP_l_1_c_9_r_*t146-OP_l_3_c_9_r_*t158);
A0[2][3] = -OP_l_9_c_4_r_*t39+OP_l_1_c_4_r_*t146-OP_l_2_c_4_r_*t152+OP_l_3_c_4_r_*t158-t69*t229+t75*t225-t78*t232-t163*t212-t166*t209+t161*t215;
A0[2][4] = -OP_l_9_c_5_r_*t39+OP_l_1_c_5_r_*t146-OP_l_2_c_5_r_*t152+OP_l_3_c_5_r_*t158-t85*t225-t88*t229+t93*t232+t174*t209-t171*t215-t183*t212;
A0[2][5] = -OP_l_9_c_6_r_*t39+OP_l_1_c_6_r_*t146-OP_l_2_c_6_r_*t152+OP_l_3_c_6_r_*t158-t102*t225-t98*t232-t190*t209+t188*t212-t197*t215+t99*(t227+t228-t234-t235);
A0[2][6] = -t238-t239+OP_l_1_c_7_r_*t146+OP_l_3_c_7_r_*t158;
A0[2][7] = -t240-t241+OP_l_1_c_8_r_*t146+OP_l_3_c_8_r_*t158;
A0[2][8] = -t242-t243+OP_l_1_c_9_r_*t146+OP_l_3_c_9_r_*t158;
A0[2][9] = t229;
A0[2][10] = t225;
A0[2][11] = t232;
A0[3][0] = t34*t249+t48*t253-t56*t257-t39*t291;
A0[3][1] = -t39*t295-t119*t249+t123*t253+t127*t257;
A0[3][2] = -t39*t299+t146*t249-t152*t253+t158*t257;
A0[3][3] = OP_l_4_c_4_r_-OP_l_10_c_4_r_*t69+OP_l_1_c_4_r_*t166+OP_l_2_c_4_r_*t163+OP_l_11_c_4_r_*t75-OP_l_3_c_4_r_*t161-OP_l_12_c_4_r_*t78-t69*t270-t78*t274+t75*t278+t166*t249+t163*t253-t161*t257+dvxNoise*(t69*t69)+dvyNoise*(t75*t75)+dvzNoise*(t78*t78);
A0[3][4] = OP_l_4_c_5_r_+t321-OP_l_10_c_5_r_*t69+OP_l_1_c_5_r_*t166+OP_l_2_c_5_r_*t163+OP_l_11_c_5_r_*t75-OP_l_3_c_5_r_*t161-OP_l_12_c_5_r_*t78-t88*t270-t85*t278+t93*t274-t174*t249+t171*t257+t183*t253-dvyNoise*t75*t85-dvzNoise*t78*t93;
A0[3][5] = OP_l_4_c_6_r_+t376-OP_l_10_c_6_r_*t69+OP_l_1_c_6_r_*t166+OP_l_2_c_6_r_*t163+OP_l_11_c_6_r_*t75-OP_l_3_c_6_r_*t161-OP_l_12_c_6_r_*t78+t99*t270-t98*t274-t102*t278+t190*t249-t188*t253+t197*t257-dvxNoise*t69*t99-dvyNoise*t75*t102;
A0[3][6] = t291;
A0[3][7] = t295;
A0[3][8] = t299;
A0[3][9] = t270;
A0[3][10] = t278;
A0[3][11] = t274;
A0[4][0] = t34*t303+t48*t307-t56*t311-t39*t346;
A0[4][1] = -t39*t350-t119*t303+t123*t307+t127*t311;
A0[4][2] = -t39*t354+t146*t303-t152*t307+t158*t311;
A0[4][3] = OP_l_5_c_4_r_+t321-OP_l_1_c_4_r_*t174-OP_l_10_c_4_r_*t88-OP_l_11_c_4_r_*t85+OP_l_3_c_4_r_*t171+OP_l_2_c_4_r_*t183+OP_l_12_c_4_r_*t93-t69*t325-t78*t329+t75*t333+t166*t303+t163*t307-t161*t311-dvyNoise*t75*t85-dvzNoise*t78*t93;
A0[4][4] = OP_l_5_c_5_r_-OP_l_1_c_5_r_*t174-OP_l_10_c_5_r_*t88-OP_l_11_c_5_r_*t85+OP_l_3_c_5_r_*t171+OP_l_2_c_5_r_*t183+OP_l_12_c_5_r_*t93-t88*t325-t85*t333+t93*t329-t174*t303+t171*t311+t183*t307+dvxNoise*(t88*t88)+dvyNoise*(t85*t85)+dvzNoise*(t93*t93);
A0[4][5] = OP_l_5_c_6_r_+t389-OP_l_1_c_6_r_*t174-OP_l_10_c_6_r_*t88-OP_l_11_c_6_r_*t85+OP_l_3_c_6_r_*t171+OP_l_2_c_6_r_*t183+OP_l_12_c_6_r_*t93+t99*t325-t98*t329-t102*t333+t190*t303-t188*t307+t197*t311-dvxNoise*t88*t99-dvzNoise*t93*t98;
A0[4][6] = t346;
A0[4][7] = t350;
A0[4][8] = t354;
A0[4][9] = t325;
A0[4][10] = t333;
A0[4][11] = t329;
A0[5][0] = t34*t358+t48*t362-t56*t366-t39*t402;
A0[5][1] = -t39*t406-t119*t358+t123*t362+t127*t366;
A0[5][2] = -t39*t410+t146*t358-t152*t362+t158*t366;
A0[5][3] = OP_l_6_c_4_r_+t376+OP_l_10_c_4_r_*t99+OP_l_1_c_4_r_*t190-OP_l_2_c_4_r_*t188-OP_l_11_c_4_r_*t102-OP_l_12_c_4_r_*t98+OP_l_3_c_4_r_*t197-t69*t380-t78*t384+t75*t388+t166*t358+t163*t362-t161*t366-dvxNoise*t69*t99-dvyNoise*t75*t102;
A0[5][4] = OP_l_6_c_5_r_+t389+OP_l_10_c_5_r_*t99+OP_l_1_c_5_r_*t190-OP_l_2_c_5_r_*t188-OP_l_11_c_5_r_*t102-OP_l_12_c_5_r_*t98+OP_l_3_c_5_r_*t197-t88*t380-t85*t388+t93*t384-t174*t358+t171*t366+t183*t362-dvxNoise*t88*t99-dvzNoise*t93*t98;
A0[5][5] = OP_l_6_c_6_r_+OP_l_10_c_6_r_*t99+OP_l_1_c_6_r_*t190-OP_l_2_c_6_r_*t188-OP_l_11_c_6_r_*t102-OP_l_12_c_6_r_*t98+OP_l_3_c_6_r_*t197+t99*t380-t98*t384-t102*t388+t190*t358-t188*t362+t197*t366+dvxNoise*(t99*t99)+dvyNoise*(t102*t102)+dvzNoise*(t98*t98);
A0[5][6] = t402;
A0[5][7] = t406;
A0[5][8] = t410;
A0[5][9] = t380;
A0[5][10] = t388;
A0[5][11] = t384;
A0[6][0] = -t411+OP_l_7_c_1_r_*t34+OP_l_7_c_2_r_*t48-OP_l_7_c_3_r_*t56;
A0[6][1] = -t412-OP_l_7_c_1_r_*t119+OP_l_7_c_2_r_*t123+OP_l_7_c_3_r_*t127;
A0[6][2] = -t413+OP_l_7_c_1_r_*t146-OP_l_7_c_2_r_*t152+OP_l_7_c_3_r_*t158;
A0[6][3] = OP_l_7_c_4_r_-OP_l_7_c_3_r_*t161+OP_l_7_c_2_r_*t163+OP_l_7_c_1_r_*t166-OP_l_7_c_10_r_*t69+OP_l_7_c_11_r_*t75-OP_l_7_c_12_r_*t78;
A0[6][4] = OP_l_7_c_5_r_+OP_l_7_c_3_r_*t171-OP_l_7_c_1_r_*t174+OP_l_7_c_2_r_*t183-OP_l_7_c_11_r_*t85-OP_l_7_c_10_r_*t88+OP_l_7_c_12_r_*t93;
A0[6][5] = OP_l_7_c_6_r_-OP_l_7_c_2_r_*t188+OP_l_7_c_1_r_*t190+OP_l_7_c_3_r_*t197+OP_l_7_c_10_r_*t99-OP_l_7_c_12_r_*t98-OP_l_7_c_11_r_*t102;
A0[6][6] = OP_l_7_c_7_r_;
A0[6][7] = OP_l_7_c_8_r_;
A0[6][8] = OP_l_7_c_9_r_;
A0[6][9] = OP_l_7_c_10_r_;
A0[6][10] = OP_l_7_c_11_r_;
A0[6][11] = OP_l_7_c_12_r_;
A0[7][0] = -t198+OP_l_8_c_1_r_*t34+OP_l_8_c_2_r_*t48-OP_l_8_c_3_r_*t56;
A0[7][1] = -t200-OP_l_8_c_1_r_*t119+OP_l_8_c_2_r_*t123+OP_l_8_c_3_r_*t127;
A0[7][2] = -t202+OP_l_8_c_1_r_*t146-OP_l_8_c_2_r_*t152+OP_l_8_c_3_r_*t158;
A0[7][3] = OP_l_8_c_4_r_-OP_l_8_c_3_r_*t161+OP_l_8_c_2_r_*t163+OP_l_8_c_1_r_*t166-OP_l_8_c_10_r_*t69+OP_l_8_c_11_r_*t75-OP_l_8_c_12_r_*t78;
A0[7][4] = OP_l_8_c_5_r_+OP_l_8_c_3_r_*t171-OP_l_8_c_1_r_*t174+OP_l_8_c_2_r_*t183-OP_l_8_c_11_r_*t85-OP_l_8_c_10_r_*t88+OP_l_8_c_12_r_*t93;
A0[7][5] = OP_l_8_c_6_r_-OP_l_8_c_2_r_*t188+OP_l_8_c_1_r_*t190+OP_l_8_c_3_r_*t197+OP_l_8_c_10_r_*t99-OP_l_8_c_12_r_*t98-OP_l_8_c_11_r_*t102;
A0[7][6] = OP_l_8_c_7_r_;
A0[7][7] = OP_l_8_c_8_r_;
A0[7][8] = OP_l_8_c_9_r_;
A0[7][9] = OP_l_8_c_10_r_;
A0[7][10] = OP_l_8_c_11_r_;
A0[7][11] = OP_l_8_c_12_r_;
A0[8][0] = -t238+OP_l_9_c_1_r_*t34+OP_l_9_c_2_r_*t48-OP_l_9_c_3_r_*t56;
A0[8][1] = -t240-OP_l_9_c_1_r_*t119+OP_l_9_c_2_r_*t123+OP_l_9_c_3_r_*t127;
A0[8][2] = -t242+OP_l_9_c_1_r_*t146-OP_l_9_c_2_r_*t152+OP_l_9_c_3_r_*t158;
A0[8][3] = OP_l_9_c_4_r_-OP_l_9_c_3_r_*t161+OP_l_9_c_2_r_*t163+OP_l_9_c_1_r_*t166-OP_l_9_c_10_r_*t69+OP_l_9_c_11_r_*t75-OP_l_9_c_12_r_*t78;
A0[8][4] = OP_l_9_c_5_r_+OP_l_9_c_3_r_*t171-OP_l_9_c_1_r_*t174+OP_l_9_c_2_r_*t183-OP_l_9_c_11_r_*t85-OP_l_9_c_10_r_*t88+OP_l_9_c_12_r_*t93;
A0[8][5] = OP_l_9_c_6_r_-OP_l_9_c_2_r_*t188+OP_l_9_c_1_r_*t190+OP_l_9_c_3_r_*t197+OP_l_9_c_10_r_*t99-OP_l_9_c_12_r_*t98-OP_l_9_c_11_r_*t102;
A0[8][6] = OP_l_9_c_7_r_;
A0[8][7] = OP_l_9_c_8_r_;
A0[8][8] = OP_l_9_c_9_r_;
A0[8][9] = OP_l_9_c_10_r_;
A0[8][10] = OP_l_9_c_11_r_;
A0[8][11] = OP_l_9_c_12_r_;
A0[9][0] = OP_l_10_c_1_r_*t34-OP_l_10_c_7_r_*t39+OP_l_10_c_2_r_*t48-OP_l_10_c_3_r_*t56;
A0[9][1] = -OP_l_10_c_8_r_*t39-OP_l_10_c_1_r_*t119+OP_l_10_c_2_r_*t123+OP_l_10_c_3_r_*t127;
A0[9][2] = -OP_l_10_c_9_r_*t39+OP_l_10_c_1_r_*t146-OP_l_10_c_2_r_*t152+OP_l_10_c_3_r_*t158;
A0[9][3] = OP_l_10_c_4_r_-t279-OP_l_10_c_3_r_*t161+OP_l_10_c_2_r_*t163+OP_l_10_c_1_r_*t166+OP_l_10_c_11_r_*t75-OP_l_10_c_12_r_*t78;
A0[9][4] = OP_l_10_c_5_r_-t336+OP_l_10_c_3_r_*t171-OP_l_10_c_1_r_*t174+OP_l_10_c_2_r_*t183-OP_l_10_c_11_r_*t85+OP_l_10_c_12_r_*t93;
A0[9][5] = OP_l_10_c_6_r_+t378-OP_l_10_c_2_r_*t188+OP_l_10_c_1_r_*t190+OP_l_10_c_3_r_*t197-OP_l_10_c_12_r_*t98-OP_l_10_c_11_r_*t102;
A0[9][6] = OP_l_10_c_7_r_;
A0[9][7] = OP_l_10_c_8_r_;
A0[9][8] = OP_l_10_c_9_r_;
A0[9][9] = OP_l_10_c_10_r_;
A0[9][10] = OP_l_10_c_11_r_;
A0[9][11] = OP_l_10_c_12_r_;
A0[10][0] = OP_l_11_c_1_r_*t34-OP_l_11_c_7_r_*t39+OP_l_11_c_2_r_*t48-OP_l_11_c_3_r_*t56;
A0[10][1] = -OP_l_11_c_8_r_*t39-OP_l_11_c_1_r_*t119+OP_l_11_c_2_r_*t123+OP_l_11_c_3_r_*t127;
A0[10][2] = -OP_l_11_c_9_r_*t39+OP_l_11_c_1_r_*t146-OP_l_11_c_2_r_*t152+OP_l_11_c_3_r_*t158;
A0[10][3] = OP_l_11_c_4_r_+t276-OP_l_11_c_3_r_*t161+OP_l_11_c_2_r_*t163+OP_l_11_c_1_r_*t166-OP_l_11_c_10_r_*t69-OP_l_11_c_12_r_*t78;
A0[10][4] = OP_l_11_c_5_r_-t337+OP_l_11_c_3_r_*t171-OP_l_11_c_1_r_*t174+OP_l_11_c_2_r_*t183-OP_l_11_c_10_r_*t88+OP_l_11_c_12_r_*t93;
A0[10][5] = OP_l_11_c_6_r_-t395-OP_l_11_c_2_r_*t188+OP_l_11_c_1_r_*t190+OP_l_11_c_3_r_*t197+OP_l_11_c_10_r_*t99-OP_l_11_c_12_r_*t98;
A0[10][6] = OP_l_11_c_7_r_;
A0[10][7] = OP_l_11_c_8_r_;
A0[10][8] = OP_l_11_c_9_r_;
A0[10][9] = OP_l_11_c_10_r_;
A0[10][10] = OP_l_11_c_11_r_;
A0[10][11] = OP_l_11_c_12_r_;
A0[11][0] = OP_l_12_c_1_r_*t34-OP_l_12_c_7_r_*t39+OP_l_12_c_2_r_*t48-OP_l_12_c_3_r_*t56;
A0[11][1] = -OP_l_12_c_8_r_*t39-OP_l_12_c_1_r_*t119+OP_l_12_c_2_r_*t123+OP_l_12_c_3_r_*t127;
A0[11][2] = -OP_l_12_c_9_r_*t39+OP_l_12_c_1_r_*t146-OP_l_12_c_2_r_*t152+OP_l_12_c_3_r_*t158;
A0[11][3] = OP_l_12_c_4_r_-t287-OP_l_12_c_3_r_*t161+OP_l_12_c_2_r_*t163+OP_l_12_c_1_r_*t166-OP_l_12_c_10_r_*t69+OP_l_12_c_11_r_*t75;
A0[11][4] = OP_l_12_c_5_r_+t327+OP_l_12_c_3_r_*t171-OP_l_12_c_1_r_*t174+OP_l_12_c_2_r_*t183-OP_l_12_c_11_r_*t85-OP_l_12_c_10_r_*t88;
A0[11][5] = OP_l_12_c_6_r_-t396-OP_l_12_c_2_r_*t188+OP_l_12_c_1_r_*t190+OP_l_12_c_3_r_*t197+OP_l_12_c_10_r_*t99-OP_l_12_c_11_r_*t102;
A0[11][6] = OP_l_12_c_7_r_;
A0[11][7] = OP_l_12_c_8_r_;
A0[11][8] = OP_l_12_c_9_r_;
A0[11][9] = OP_l_12_c_10_r_;
A0[11][10] = OP_l_12_c_11_r_;
A0[11][11] = OP_l_12_c_12_r_;

556
libraries/AP_NavEKF/Models/GimbalEstimatorExample/calcP.cpp Normal file
View File

@ -0,0 +1,556 @@
float t2 = dax*0.5f;
float t3 = dax_b*0.5f;
float t4 = t2-t3;
float t5 = day*0.5f;
float t6 = day_b*0.5f;
float t7 = t5-t6;
float t8 = daz*0.5f;
float t9 = daz_b*0.5f;
float t10 = t8-t9;
float t11 = q2*t4*0.5f;
float t12 = q1*t7*0.5f;
float t13 = q0*t10*0.5f;
float t14 = q2*0.5f;
float t15 = q3*t4*0.5f;
float t16 = q1*t10*0.5f;
float t17 = q1*0.5f;
float t18 = q0*t4*0.5f;
float t19 = q3*t7*0.5f;
float t20 = q0*0.5f;
float t21 = q2*t7*0.5f;
float t22 = q3*t10*0.5f;
float t23 = q0*t7*0.5f;
float t24 = q3*0.5f;
float t25 = q1*t4*0.5f;
float t26 = q2*t10*0.5f;
float t27 = t11+t12+t13-t24;
float t28 = t14+t15+t16-t23;
float t29 = q2*t28*2.0f;
float t30 = t17+t18+t19-t26;
float t31 = q1*t30*2.0f;
float t32 = t20+t21+t22-t25;
float t33 = q0*t32*2.0f;
float t40 = q3*t27*2.0f;
float t34 = t29+t31+t33-t40;
float t35 = q0*q0;
float t36 = q1*q1;
float t37 = q2*q2;
float t38 = q3*q3;
float t39 = t35+t36+t37+t38;
float t41 = t11+t12-t13+t24;
float t42 = t14-t15+t16+t23;
float t43 = q1*t42*2.0f;
float t44 = -t17+t18+t19+t26;
float t45 = q2*t44*2.0f;
float t46 = t20-t21+t22+t25;
float t47 = q3*t46*2.0f;
float t57 = q0*t41*2.0f;
float t48 = t43+t45+t47-t57;
float t49 = -t14+t15+t16+t23;
float t50 = q0*t49*2.0f;
float t51 = t11-t12+t13+t24;
float t52 = t20+t21-t22+t25;
float t53 = q2*t52*2.0f;
float t54 = t17-t18+t19+t26;
float t55 = q3*t54*2.0f;
float t58 = q1*t51*2.0f;
float t56 = t50+t53+t55-t58;
float t59 = OP[0][0]*t34;
float t60 = OP[1][0]*t48;
float t66 = OP[6][0]*t39;
float t67 = OP[2][0]*t56;
float t61 = t59+t60-t66-t67;
float t62 = OP[0][1]*t34;
float t63 = OP[1][1]*t48;
float t64 = OP[0][2]*t34;
float t65 = OP[1][2]*t48;
float t72 = OP[6][1]*t39;
float t73 = OP[2][1]*t56;
float t68 = t62+t63-t72-t73;
float t69 = t35+t36-t37-t38;
float t86 = OP[6][2]*t39;
float t87 = OP[2][2]*t56;
float t70 = t64+t65-t86-t87;
float t71 = dvx-dvx_b;
float t74 = q0*q3*2.0f;
float t81 = q1*q2*2.0f;
float t75 = t74-t81;
float t76 = q0*q2*2.0f;
float t77 = q1*q3*2.0f;
float t78 = t76+t77;
float t79 = dvy-dvy_b;
float t80 = dvz-dvz_b;
float t82 = OP[0][10]*t34;
float t83 = OP[1][10]*t48;
float t103 = OP[6][10]*t39;
float t104 = OP[2][10]*t56;
float t84 = t82+t83-t103-t104;
float t85 = t35-t36+t37-t38;
float t88 = t74+t81;
float t89 = OP[0][9]*t34;
float t90 = OP[1][9]*t48;
float t100 = OP[6][9]*t39;
float t101 = OP[2][9]*t56;
float t91 = t89+t90-t100-t101;
float t92 = q0*q1*2.0f;
float t96 = q2*q3*2.0f;
float t93 = t92-t96;
float t94 = OP[0][11]*t34;
float t95 = OP[1][11]*t48;
float t114 = OP[6][11]*t39;
float t115 = OP[2][11]*t56;
float t97 = t94+t95-t114-t115;
float t98 = t35-t36-t37+t38;
float t99 = t76-t77;
float t102 = t92+t96;
float t105 = OP[0][6]*t34;
float t106 = OP[1][6]*t48;
float t411 = OP[6][6]*t39;
float t107 = t105+t106-t411-OP[2][6]*t56;
float t108 = OP[0][7]*t34;
float t109 = OP[1][7]*t48;
float t412 = OP[6][7]*t39;
float t110 = t108+t109-t412-OP[2][7]*t56;
float t111 = OP[0][8]*t34;
float t112 = OP[1][8]*t48;
float t413 = OP[6][8]*t39;
float t113 = t111+t112-t413-OP[2][8]*t56;
float t116 = q0*t27*2.0f;
float t117 = q1*t28*2.0f;
float t118 = q3*t32*2.0f;
float t128 = q2*t30*2.0f;
float t119 = t116+t117+t118-t128;
float t120 = q0*t46*2.0f;
float t121 = q2*t42*2.0f;
float t122 = q3*t41*2.0f;
float t129 = q1*t44*2.0f;
float t123 = t120+t121+t122-t129;
float t124 = q1*t52*2.0f;
float t125 = q2*t51*2.0f;
float t126 = q3*t49*2.0f;
float t130 = q0*t54*2.0f;
float t127 = t124+t125+t126-t130;
float t131 = OP[7][0]*t39;
float t132 = OP[0][0]*t119;
float t141 = OP[1][0]*t123;
float t142 = OP[2][0]*t127;
float t133 = t131+t132-t141-t142;
float t134 = OP[7][1]*t39;
float t135 = OP[0][1]*t119;
float t147 = OP[1][1]*t123;
float t148 = OP[2][1]*t127;
float t136 = t134+t135-t147-t148;
float t137 = OP[7][2]*t39;
float t138 = OP[0][2]*t119;
float t153 = OP[1][2]*t123;
float t154 = OP[2][2]*t127;
float t139 = t137+t138-t153-t154;
float t140 = t39*t39;
float t143 = q1*t27*2.0f;
float t144 = q2*t32*2.0f;
float t145 = q3*t30*2.0f;
float t204 = q0*t28*2.0f;
float t146 = t143+t144+t145-t204;
float t149 = q0*t44*2.0f;
float t150 = q1*t46*2.0f;
float t151 = q2*t41*2.0f;
float t205 = q3*t42*2.0f;
float t152 = t149+t150+t151-t205;
float t155 = q0*t52*2.0f;
float t156 = q1*t54*2.0f;
float t157 = q3*t51*2.0f;
float t206 = q2*t49*2.0f;
float t158 = t155+t156+t157-t206;
float t159 = t69*t79;
float t160 = t71*t75;
float t161 = t159+t160;
float t162 = t69*t80;
float t222 = t71*t78;
float t163 = t162-t222;
float t164 = t78*t79;
float t165 = t75*t80;
float t166 = t164+t165;
float t167 = OP[7][10]*t39;
float t168 = OP[0][10]*t119;
float t193 = OP[1][10]*t123;
float t194 = OP[2][10]*t127;
float t169 = t167+t168-t193-t194;
float t170 = t71*t85;
float t226 = t79*t88;
float t171 = t170-t226;
float t172 = t80*t85;
float t173 = t79*t93;
float t174 = t172+t173;
float t175 = OP[7][9]*t39;
float t176 = OP[0][9]*t119;
float t191 = OP[1][9]*t123;
float t192 = OP[2][9]*t127;
float t177 = t175+t176-t191-t192;
float t178 = OP[7][11]*t39;
float t179 = OP[0][11]*t119;
float t184 = OP[1][11]*t123;
float t185 = OP[2][11]*t127;
float t180 = t178+t179-t184-t185;
float t181 = t71*t93;
float t182 = t80*t88;
float t183 = t181+t182;
float t186 = t71*t98;
float t187 = t80*t99;
float t188 = t186+t187;
float t189 = t79*t98;
float t233 = t80*t102;
float t190 = t189-t233;
float t195 = t71*t102;
float t196 = t79*t99;
float t197 = t195+t196;
float t198 = OP[7][6]*t39;
float t199 = OP[0][6]*t119;
float t200 = OP[7][7]*t39;
float t201 = OP[0][7]*t119;
float t202 = OP[7][8]*t39;
float t203 = OP[0][8]*t119;
float t207 = OP[8][0]*t39;
float t208 = OP[1][0]*t152;
float t216 = OP[0][0]*t146;
float t217 = OP[2][0]*t158;
float t209 = t207+t208-t216-t217;
float t210 = OP[8][1]*t39;
float t211 = OP[1][1]*t152;
float t218 = OP[0][1]*t146;
float t219 = OP[2][1]*t158;
float t212 = t210+t211-t218-t219;
float t213 = OP[8][2]*t39;
float t214 = OP[1][2]*t152;
float t220 = OP[0][2]*t146;
float t221 = OP[2][2]*t158;
float t215 = t213+t214-t220-t221;
float t223 = OP[0][10]*t146;
float t224 = OP[2][10]*t158;
float t236 = OP[8][10]*t39;
float t237 = OP[1][10]*t152;
float t225 = t223+t224-t236-t237;
float t227 = OP[0][9]*t146;
float t228 = OP[2][9]*t158;
float t234 = OP[8][9]*t39;
float t235 = OP[1][9]*t152;
float t229 = t227+t228-t234-t235;
float t230 = OP[0][11]*t146;
float t231 = OP[2][11]*t158;
float t244 = OP[8][11]*t39;
float t245 = OP[1][11]*t152;
float t232 = t230+t231-t244-t245;
float t238 = OP[8][6]*t39;
float t239 = OP[1][6]*t152;
float t240 = OP[8][7]*t39;
float t241 = OP[1][7]*t152;
float t242 = OP[8][8]*t39;
float t243 = OP[1][8]*t152;
float t246 = OP[1][0]*t163;
float t247 = OP[10][0]*t75;
float t248 = OP[0][0]*t166;
float t258 = OP[9][0]*t69;
float t259 = OP[2][0]*t161;
float t260 = OP[11][0]*t78;
float t249 = OP[3][0]+t246+t247+t248-t258-t259-t260;
float t250 = OP[1][1]*t163;
float t251 = OP[10][1]*t75;
float t252 = OP[0][1]*t166;
float t261 = OP[9][1]*t69;
float t262 = OP[2][1]*t161;
float t263 = OP[11][1]*t78;
float t253 = OP[3][1]+t250+t251+t252-t261-t262-t263;
float t254 = OP[1][2]*t163;
float t255 = OP[10][2]*t75;
float t256 = OP[0][2]*t166;
float t264 = OP[9][2]*t69;
float t265 = OP[2][2]*t161;
float t266 = OP[11][2]*t78;
float t257 = OP[3][2]+t254+t255+t256-t264-t265-t266;
float t267 = OP[1][9]*t163;
float t268 = OP[10][9]*t75;
float t269 = OP[0][9]*t166;
float t279 = OP[9][9]*t69;
float t280 = OP[2][9]*t161;
float t281 = OP[11][9]*t78;
float t270 = OP[3][9]+t267+t268+t269-t279-t280-t281;
float t271 = OP[1][11]*t163;
float t272 = OP[10][11]*t75;
float t273 = OP[0][11]*t166;
float t285 = OP[9][11]*t69;
float t286 = OP[2][11]*t161;
float t287 = OP[11][11]*t78;
float t274 = OP[3][11]+t271+t272+t273-t285-t286-t287;
float t275 = OP[1][10]*t163;
float t276 = OP[10][10]*t75;
float t277 = OP[0][10]*t166;
float t282 = OP[9][10]*t69;
float t283 = OP[2][10]*t161;
float t284 = OP[11][10]*t78;
float t278 = OP[3][10]+t275+t276+t277-t282-t283-t284;
float t288 = OP[1][6]*t163;
float t289 = OP[10][6]*t75;
float t290 = OP[0][6]*t166;
float t291 = OP[3][6]+t288+t289+t290-OP[9][6]*t69-OP[2][6]*t161-OP[11][6]*t78;
float t292 = OP[1][7]*t163;
float t293 = OP[10][7]*t75;
float t294 = OP[0][7]*t166;
float t295 = OP[3][7]+t292+t293+t294-OP[9][7]*t69-OP[2][7]*t161-OP[11][7]*t78;
float t296 = OP[1][8]*t163;
float t297 = OP[10][8]*t75;
float t298 = OP[0][8]*t166;
float t299 = OP[3][8]+t296+t297+t298-OP[9][8]*t69-OP[2][8]*t161-OP[11][8]*t78;
float t300 = OP[2][0]*t171;
float t301 = OP[11][0]*t93;
float t302 = OP[1][0]*t183;
float t312 = OP[10][0]*t85;
float t313 = OP[0][0]*t174;
float t314 = OP[9][0]*t88;
float t303 = OP[4][0]+t300+t301+t302-t312-t313-t314;
float t304 = OP[2][1]*t171;
float t305 = OP[11][1]*t93;
float t306 = OP[1][1]*t183;
float t315 = OP[10][1]*t85;
float t316 = OP[0][1]*t174;
float t317 = OP[9][1]*t88;
float t307 = OP[4][1]+t304+t305+t306-t315-t316-t317;
float t308 = OP[2][2]*t171;
float t309 = OP[11][2]*t93;
float t310 = OP[1][2]*t183;
float t318 = OP[10][2]*t85;
float t319 = OP[0][2]*t174;
float t320 = OP[9][2]*t88;
float t311 = OP[4][2]+t308+t309+t310-t318-t319-t320;
float t321 = dvxNoise*t69*t88;
float t322 = OP[2][9]*t171;
float t323 = OP[11][9]*t93;
float t324 = OP[1][9]*t183;
float t334 = OP[10][9]*t85;
float t335 = OP[0][9]*t174;
float t336 = OP[9][9]*t88;
float t325 = OP[4][9]+t322+t323+t324-t334-t335-t336;
float t326 = OP[2][11]*t171;
float t327 = OP[11][11]*t93;
float t328 = OP[1][11]*t183;
float t340 = OP[10][11]*t85;
float t341 = OP[0][11]*t174;
float t342 = OP[9][11]*t88;
float t329 = OP[4][11]+t326+t327+t328-t340-t341-t342;
float t330 = OP[2][10]*t171;
float t331 = OP[11][10]*t93;
float t332 = OP[1][10]*t183;
float t337 = OP[10][10]*t85;
float t338 = OP[0][10]*t174;
float t339 = OP[9][10]*t88;
float t333 = OP[4][10]+t330+t331+t332-t337-t338-t339;
float t343 = OP[2][6]*t171;
float t344 = OP[11][6]*t93;
float t345 = OP[1][6]*t183;
float t346 = OP[4][6]+t343+t344+t345-OP[0][6]*t174-OP[9][6]*t88-OP[10][6]*t85;
float t347 = OP[2][7]*t171;
float t348 = OP[11][7]*t93;
float t349 = OP[1][7]*t183;
float t350 = OP[4][7]+t347+t348+t349-OP[0][7]*t174-OP[9][7]*t88-OP[10][7]*t85;
float t351 = OP[2][8]*t171;
float t352 = OP[11][8]*t93;
float t353 = OP[1][8]*t183;
float t354 = OP[4][8]+t351+t352+t353-OP[0][8]*t174-OP[9][8]*t88-OP[10][8]*t85;
float t355 = OP[0][0]*t190;
float t356 = OP[9][0]*t99;
float t357 = OP[2][0]*t197;
float t367 = OP[11][0]*t98;
float t368 = OP[1][0]*t188;
float t369 = OP[10][0]*t102;
float t358 = OP[5][0]+t355+t356+t357-t367-t368-t369;
float t359 = OP[0][1]*t190;
float t360 = OP[9][1]*t99;
float t361 = OP[2][1]*t197;
float t370 = OP[11][1]*t98;
float t371 = OP[1][1]*t188;
float t372 = OP[10][1]*t102;
float t362 = OP[5][1]+t359+t360+t361-t370-t371-t372;
float t363 = OP[0][2]*t190;
float t364 = OP[9][2]*t99;
float t365 = OP[2][2]*t197;
float t373 = OP[11][2]*t98;
float t374 = OP[1][2]*t188;
float t375 = OP[10][2]*t102;
float t366 = OP[5][2]+t363+t364+t365-t373-t374-t375;
float t376 = dvzNoise*t78*t98;
float t377 = OP[0][9]*t190;
float t378 = OP[9][9]*t99;
float t379 = OP[2][9]*t197;
float t390 = OP[11][9]*t98;
float t391 = OP[1][9]*t188;
float t392 = OP[10][9]*t102;
float t380 = OP[5][9]+t377+t378+t379-t390-t391-t392;
float t381 = OP[0][11]*t190;
float t382 = OP[9][11]*t99;
float t383 = OP[2][11]*t197;
float t396 = OP[11][11]*t98;
float t397 = OP[1][11]*t188;
float t398 = OP[10][11]*t102;
float t384 = OP[5][11]+t381+t382+t383-t396-t397-t398;
float t385 = OP[0][10]*t190;
float t386 = OP[9][10]*t99;
float t387 = OP[2][10]*t197;
float t393 = OP[11][10]*t98;
float t394 = OP[1][10]*t188;
float t395 = OP[10][10]*t102;
float t388 = OP[5][10]+t385+t386+t387-t393-t394-t395;
float t389 = dvyNoise*t85*t102;
float t399 = OP[0][6]*t190;
float t400 = OP[9][6]*t99;
float t401 = OP[2][6]*t197;
float t402 = OP[5][6]+t399+t400+t401-OP[1][6]*t188-OP[10][6]*t102-OP[11][6]*t98;
float t403 = OP[0][7]*t190;
float t404 = OP[9][7]*t99;
float t405 = OP[2][7]*t197;
float t406 = OP[5][7]+t403+t404+t405-OP[1][7]*t188-OP[10][7]*t102-OP[11][7]*t98;
float t407 = OP[0][8]*t190;
float t408 = OP[9][8]*t99;
float t409 = OP[2][8]*t197;
float t410 = OP[5][8]+t407+t408+t409-OP[1][8]*t188-OP[10][8]*t102-OP[11][8]*t98;
A0[0][0] = daxNoise*t140+t34*t61+t48*t68-t56*t70-t39*t107;
A0[0][1] = -t39*t110-t61*t119+t68*t123+t70*t127;
A0[0][2] = -t39*t113-t68*t152+t70*t158+t146*(t59+t60-t66-t67);
A0[0][3] = OP[0][3]*t34+OP[1][3]*t48-OP[2][3]*t56-OP[6][3]*t39+t75*t84-t69*t91-t78*t97-t70*t161+t166*(t59+t60-t66-t67)+t163*(t62+t63-t72-t73);
A0[0][4] = OP[0][4]*t34+OP[1][4]*t48-OP[2][4]*t56-OP[6][4]*t39-t84*t85-t88*t91+t93*t97-t61*t174+t70*t171+t183*(t62+t63-t72-t73);
A0[0][5] = OP[0][5]*t34+OP[1][5]*t48-OP[2][5]*t56-OP[6][5]*t39-t84*t102-t97*t98+t61*t190-t68*t188+t99*(t89+t90-t100-t101)+t197*(t64+t65-t86-t87);
A0[0][6] = t107;
A0[0][7] = t110;
A0[0][8] = t113;
A0[0][9] = t91;
A0[0][10] = t84;
A0[0][11] = t97;
A0[1][0] = -t34*t133-t48*t136+t56*t139+t39*(t198+t199-OP[1][6]*t123-OP[2][6]*t127);
A0[1][1] = dayNoise*t140+t119*t133-t123*t136-t127*t139+t39*(t200+t201-OP[1][7]*t123-OP[2][7]*t127);
A0[1][2] = -t133*t146+t136*t152-t139*t158+t39*(t202+t203-OP[1][8]*t123-OP[2][8]*t127);
A0[1][3] = -OP[7][3]*t39-OP[0][3]*t119+OP[1][3]*t123+OP[2][3]*t127-t75*t169+t69*t177+t78*t180-t133*t166-t136*t163+t139*t161;
A0[1][4] = -OP[7][4]*t39-OP[0][4]*t119+OP[1][4]*t123+OP[2][4]*t127+t85*t169+t88*t177-t93*t180+t133*t174-t139*t171-t136*t183;
A0[1][5] = -OP[7][5]*t39-OP[0][5]*t119+OP[1][5]*t123+OP[2][5]*t127+t102*t169-t99*t177+t98*t180-t133*t190+t136*t188-t139*t197;
A0[1][6] = -t198-t199+OP[1][6]*t123+OP[2][6]*t127;
A0[1][7] = -t200-t201+OP[1][7]*t123+OP[2][7]*t127;
A0[1][8] = -t202-t203+OP[1][8]*t123+OP[2][8]*t127;
A0[1][9] = -t175-t176+t191+t192;
A0[1][10] = -t167-t168+t193+t194;
A0[1][11] = -t178-t179+t184+t185;
A0[2][0] = -t34*t209-t48*t212+t56*t215+t39*(t238+t239-OP[0][6]*t146-OP[2][6]*t158);
A0[2][1] = t119*t209-t123*t212-t127*t215+t39*(t240+t241-OP[0][7]*t146-OP[2][7]*t158);
A0[2][2] = dazNoise*t140-t146*t209+t152*t212-t158*t215+t39*(t242+t243-OP[0][8]*t146-OP[2][8]*t158);
A0[2][3] = -OP[8][3]*t39+OP[0][3]*t146-OP[1][3]*t152+OP[2][3]*t158-t69*t229+t75*t225-t78*t232-t163*t212-t166*t209+t161*t215;
A0[2][4] = -OP[8][4]*t39+OP[0][4]*t146-OP[1][4]*t152+OP[2][4]*t158-t85*t225-t88*t229+t93*t232+t174*t209-t171*t215-t183*t212;
A0[2][5] = -OP[8][5]*t39+OP[0][5]*t146-OP[1][5]*t152+OP[2][5]*t158-t102*t225-t98*t232-t190*t209+t188*t212-t197*t215+t99*(t227+t228-t234-t235);
A0[2][6] = -t238-t239+OP[0][6]*t146+OP[2][6]*t158;
A0[2][7] = -t240-t241+OP[0][7]*t146+OP[2][7]*t158;
A0[2][8] = -t242-t243+OP[0][8]*t146+OP[2][8]*t158;
A0[2][9] = t229;
A0[2][10] = t225;
A0[2][11] = t232;
A0[3][0] = t34*t249+t48*t253-t56*t257-t39*t291;
A0[3][1] = -t39*t295-t119*t249+t123*t253+t127*t257;
A0[3][2] = -t39*t299+t146*t249-t152*t253+t158*t257;
A0[3][3] = OP[3][3]-OP[9][3]*t69+OP[0][3]*t166+OP[1][3]*t163+OP[10][3]*t75-OP[2][3]*t161-OP[11][3]*t78-t69*t270-t78*t274+t75*t278+t166*t249+t163*t253-t161*t257+dvxNoise*(t69*t69)+dvyNoise*(t75*t75)+dvzNoise*(t78*t78);
A0[3][4] = OP[3][4]+t321-OP[9][4]*t69+OP[0][4]*t166+OP[1][4]*t163+OP[10][4]*t75-OP[2][4]*t161-OP[11][4]*t78-t88*t270-t85*t278+t93*t274-t174*t249+t171*t257+t183*t253-dvyNoise*t75*t85-dvzNoise*t78*t93;
A0[3][5] = OP[3][5]+t376-OP[9][5]*t69+OP[0][5]*t166+OP[1][5]*t163+OP[10][5]*t75-OP[2][5]*t161-OP[11][5]*t78+t99*t270-t98*t274-t102*t278+t190*t249-t188*t253+t197*t257-dvxNoise*t69*t99-dvyNoise*t75*t102;
A0[3][6] = t291;
A0[3][7] = t295;
A0[3][8] = t299;
A0[3][9] = t270;
A0[3][10] = t278;
A0[3][11] = t274;
A0[4][0] = t34*t303+t48*t307-t56*t311-t39*t346;
A0[4][1] = -t39*t350-t119*t303+t123*t307+t127*t311;
A0[4][2] = -t39*t354+t146*t303-t152*t307+t158*t311;
A0[4][3] = OP[4][3]+t321-OP[0][3]*t174-OP[9][3]*t88-OP[10][3]*t85+OP[2][3]*t171+OP[1][3]*t183+OP[11][3]*t93-t69*t325-t78*t329+t75*t333+t166*t303+t163*t307-t161*t311-dvyNoise*t75*t85-dvzNoise*t78*t93;
A0[4][4] = OP[4][4]-OP[0][4]*t174-OP[9][4]*t88-OP[10][4]*t85+OP[2][4]*t171+OP[1][4]*t183+OP[11][4]*t93-t88*t325-t85*t333+t93*t329-t174*t303+t171*t311+t183*t307+dvxNoise*(t88*t88)+dvyNoise*(t85*t85)+dvzNoise*(t93*t93);
A0[4][5] = OP[4][5]+t389-OP[0][5]*t174-OP[9][5]*t88-OP[10][5]*t85+OP[2][5]*t171+OP[1][5]*t183+OP[11][5]*t93+t99*t325-t98*t329-t102*t333+t190*t303-t188*t307+t197*t311-dvxNoise*t88*t99-dvzNoise*t93*t98;
A0[4][6] = t346;
A0[4][7] = t350;
A0[4][8] = t354;
A0[4][9] = t325;
A0[4][10] = t333;
A0[4][11] = t329;
A0[5][0] = t34*t358+t48*t362-t56*t366-t39*t402;
A0[5][1] = -t39*t406-t119*t358+t123*t362+t127*t366;
A0[5][2] = -t39*t410+t146*t358-t152*t362+t158*t366;
A0[5][3] = OP[5][3]+t376+OP[9][3]*t99+OP[0][3]*t190-OP[1][3]*t188-OP[10][3]*t102-OP[11][3]*t98+OP[2][3]*t197-t69*t380-t78*t384+t75*t388+t166*t358+t163*t362-t161*t366-dvxNoise*t69*t99-dvyNoise*t75*t102;
A0[5][4] = OP[5][4]+t389+OP[9][4]*t99+OP[0][4]*t190-OP[1][4]*t188-OP[10][4]*t102-OP[11][4]*t98+OP[2][4]*t197-t88*t380-t85*t388+t93*t384-t174*t358+t171*t366+t183*t362-dvxNoise*t88*t99-dvzNoise*t93*t98;
A0[5][5] = OP[5][5]+OP[9][5]*t99+OP[0][5]*t190-OP[1][5]*t188-OP[10][5]*t102-OP[11][5]*t98+OP[2][5]*t197+t99*t380-t98*t384-t102*t388+t190*t358-t188*t362+t197*t366+dvxNoise*(t99*t99)+dvyNoise*(t102*t102)+dvzNoise*(t98*t98);
A0[5][6] = t402;
A0[5][7] = t406;
A0[5][8] = t410;
A0[5][9] = t380;
A0[5][10] = t388;
A0[5][11] = t384;
A0[6][0] = -t411+OP[6][0]*t34+OP[6][1]*t48-OP[6][2]*t56;
A0[6][1] = -t412-OP[6][0]*t119+OP[6][1]*t123+OP[6][2]*t127;
A0[6][2] = -t413+OP[6][0]*t146-OP[6][1]*t152+OP[6][2]*t158;
A0[6][3] = OP[6][3]-OP[6][2]*t161+OP[6][1]*t163+OP[6][0]*t166-OP[6][9]*t69+OP[6][10]*t75-OP[6][11]*t78;
A0[6][4] = OP[6][4]+OP[6][2]*t171-OP[6][0]*t174+OP[6][1]*t183-OP[6][10]*t85-OP[6][9]*t88+OP[6][11]*t93;
A0[6][5] = OP[6][5]-OP[6][1]*t188+OP[6][0]*t190+OP[6][2]*t197+OP[6][9]*t99-OP[6][11]*t98-OP[6][10]*t102;
A0[6][6] = OP[6][6];
A0[6][7] = OP[6][7];
A0[6][8] = OP[6][8];
A0[6][9] = OP[6][9];
A0[6][10] = OP[6][10];
A0[6][11] = OP[6][11];
A0[7][0] = -t198+OP[7][0]*t34+OP[7][1]*t48-OP[7][2]*t56;
A0[7][1] = -t200-OP[7][0]*t119+OP[7][1]*t123+OP[7][2]*t127;
A0[7][2] = -t202+OP[7][0]*t146-OP[7][1]*t152+OP[7][2]*t158;
A0[7][3] = OP[7][3]-OP[7][2]*t161+OP[7][1]*t163+OP[7][0]*t166-OP[7][9]*t69+OP[7][10]*t75-OP[7][11]*t78;
A0[7][4] = OP[7][4]+OP[7][2]*t171-OP[7][0]*t174+OP[7][1]*t183-OP[7][10]*t85-OP[7][9]*t88+OP[7][11]*t93;
A0[7][5] = OP[7][5]-OP[7][1]*t188+OP[7][0]*t190+OP[7][2]*t197+OP[7][9]*t99-OP[7][11]*t98-OP[7][10]*t102;
A0[7][6] = OP[7][6];
A0[7][7] = OP[7][7];
A0[7][8] = OP[7][8];
A0[7][9] = OP[7][9];
A0[7][10] = OP[7][10];
A0[7][11] = OP[7][11];
A0[8][0] = -t238+OP[8][0]*t34+OP[8][1]*t48-OP[8][2]*t56;
A0[8][1] = -t240-OP[8][0]*t119+OP[8][1]*t123+OP[8][2]*t127;
A0[8][2] = -t242+OP[8][0]*t146-OP[8][1]*t152+OP[8][2]*t158;
A0[8][3] = OP[8][3]-OP[8][2]*t161+OP[8][1]*t163+OP[8][0]*t166-OP[8][9]*t69+OP[8][10]*t75-OP[8][11]*t78;
A0[8][4] = OP[8][4]+OP[8][2]*t171-OP[8][0]*t174+OP[8][1]*t183-OP[8][10]*t85-OP[8][9]*t88+OP[8][11]*t93;
A0[8][5] = OP[8][5]-OP[8][1]*t188+OP[8][0]*t190+OP[8][2]*t197+OP[8][9]*t99-OP[8][11]*t98-OP[8][10]*t102;
A0[8][6] = OP[8][6];
A0[8][7] = OP[8][7];
A0[8][8] = OP[8][8];
A0[8][9] = OP[8][9];
A0[8][10] = OP[8][10];
A0[8][11] = OP[8][11];
A0[9][0] = OP[9][0]*t34-OP[9][6]*t39+OP[9][1]*t48-OP[9][2]*t56;
A0[9][1] = -OP[9][7]*t39-OP[9][0]*t119+OP[9][1]*t123+OP[9][2]*t127;
A0[9][2] = -OP[9][8]*t39+OP[9][0]*t146-OP[9][1]*t152+OP[9][2]*t158;
A0[9][3] = OP[9][3]-t279-OP[9][2]*t161+OP[9][1]*t163+OP[9][0]*t166+OP[9][10]*t75-OP[9][11]*t78;
A0[9][4] = OP[9][4]-t336+OP[9][2]*t171-OP[9][0]*t174+OP[9][1]*t183-OP[9][10]*t85+OP[9][11]*t93;
A0[9][5] = OP[9][5]+t378-OP[9][1]*t188+OP[9][0]*t190+OP[9][2]*t197-OP[9][11]*t98-OP[9][10]*t102;
A0[9][6] = OP[9][6];
A0[9][7] = OP[9][7];
A0[9][8] = OP[9][8];
A0[9][9] = OP[9][9];
A0[9][10] = OP[9][10];
A0[9][11] = OP[9][11];
A0[10][0] = OP[10][0]*t34-OP[10][6]*t39+OP[10][1]*t48-OP[10][2]*t56;
A0[10][1] = -OP[10][7]*t39-OP[10][0]*t119+OP[10][1]*t123+OP[10][2]*t127;
A0[10][2] = -OP[10][8]*t39+OP[10][0]*t146-OP[10][1]*t152+OP[10][2]*t158;
A0[10][3] = OP[10][3]+t276-OP[10][2]*t161+OP[10][1]*t163+OP[10][0]*t166-OP[10][9]*t69-OP[10][11]*t78;
A0[10][4] = OP[10][4]-t337+OP[10][2]*t171-OP[10][0]*t174+OP[10][1]*t183-OP[10][9]*t88+OP[10][11]*t93;
A0[10][5] = OP[10][5]-t395-OP[10][1]*t188+OP[10][0]*t190+OP[10][2]*t197+OP[10][9]*t99-OP[10][11]*t98;
A0[10][6] = OP[10][6];
A0[10][7] = OP[10][7];
A0[10][8] = OP[10][8];
A0[10][9] = OP[10][9];
A0[10][10] = OP[10][10];
A0[10][11] = OP[10][11];
A0[11][0] = OP[11][0]*t34-OP[11][6]*t39+OP[11][1]*t48-OP[11][2]*t56;
A0[11][1] = -OP[11][7]*t39-OP[11][0]*t119+OP[11][1]*t123+OP[11][2]*t127;
A0[11][2] = -OP[11][8]*t39+OP[11][0]*t146-OP[11][1]*t152+OP[11][2]*t158;
A0[11][3] = OP[11][3]-t287-OP[11][2]*t161+OP[11][1]*t163+OP[11][0]*t166-OP[11][9]*t69+OP[11][10]*t75;
A0[11][4] = OP[11][4]+t327+OP[11][2]*t171-OP[11][0]*t174+OP[11][1]*t183-OP[11][10]*t85-OP[11][9]*t88;
A0[11][5] = OP[11][5]-t396-OP[11][1]*t188+OP[11][0]*t190+OP[11][2]*t197+OP[11][9]*t99-OP[11][10]*t102;
A0[11][6] = OP[11][6];
A0[11][7] = OP[11][7];
A0[11][8] = OP[11][8];
A0[11][9] = OP[11][9];
A0[11][10] = OP[11][10];
A0[11][11] = OP[11][11];

420
libraries/AP_NavEKF/Models/GimbalEstimatorExample/calcP.m Normal file
View File

File diff suppressed because one or more lines are too long

152
libraries/AP_NavEKF/Models/GimbalEstimatorExample/calcP.txt Normal file
View File

@ -0,0 +1,152 @@
SF = zeros(25,1);
SF(1) = daz/2 - daz_b/2;
SF(2) = day/2 - day_b/2;
SF(3) = dax/2 - dax_b/2;
SF(4) = (q0*SF(2))/2 - q2/2 + (q1*SF(1))/2 + (q3*SF(3))/2;
SF(5) = q3/2 + (q0*SF(1))/2 - (q1*SF(2))/2 + (q2*SF(3))/2;
SF(6) = q0/2 + (q1*SF(3))/2 + (q2*SF(2))/2 - (q3*SF(1))/2;
SF(7) = q1/2 - (q0*SF(3))/2 + (q2*SF(1))/2 + (q3*SF(2))/2;
SF(8) = q3/2 - (q0*SF(1))/2 + (q1*SF(2))/2 + (q2*SF(3))/2;
SF(9) = q2/2 + (q0*SF(2))/2 + (q1*SF(1))/2 - (q3*SF(3))/2;
SF(10) = (q0*SF(3))/2 - q1/2 + (q2*SF(1))/2 + (q3*SF(2))/2;
SF(11) = q0/2 + (q1*SF(3))/2 - (q2*SF(2))/2 + (q3*SF(1))/2;
SF(12) = q0/2 - (q1*SF(3))/2 + (q2*SF(2))/2 + (q3*SF(1))/2;
SF(13) = q1/2 + (q0*SF(3))/2 - (q2*SF(1))/2 + (q3*SF(2))/2;
SF(14) = q2/2 - (q0*SF(2))/2 + (q1*SF(1))/2 + (q3*SF(3))/2;
SF(15) = (q0*SF(1))/2 - q3/2 + (q1*SF(2))/2 + (q2*SF(3))/2;
SF(16) = - q0^2 - q1^2 - q2^2 - q3^2;
SF(17) = q0^2 - q1^2 - q2^2 + q3^2;
SF(18) = q0^2 - q1^2 + q2^2 - q3^2;
SF(19) = q0^2 + q1^2 - q2^2 - q3^2;
SF(20) = 2*q0*q2 - 2*q1*q3;
SF(21) = 2*q0*q1 - 2*q2*q3;
SF(22) = 2*q0*q3 - 2*q1*q2;
SF(23) = 2*q0*q1 + 2*q2*q3;
SF(24) = 2*q0*q3 + 2*q1*q2;
SF(25) = 2*q0*q2 + 2*q1*q3;
SG = zeros(5,1);
SG(1) = - q0^2 - q1^2 - q2^2 - q3^2;
SG(2) = q3^2;
SG(3) = q2^2;
SG(4) = q1^2;
SG(5) = q0^2;
SQ = zeros(8,1);
SQ(1) = dvyNoise*(2*q0*q1 + 2*q2*q3)*(q0^2 - q1^2 + q2^2 - q3^2) - dvzNoise*(2*q0*q1 - 2*q2*q3)*(q0^2 - q1^2 - q2^2 + q3^2) - dvxNoise*(2*q0*q2 - 2*q1*q3)*(2*q0*q3 + 2*q1*q2);
SQ(2) = dvzNoise*(2*q0*q2 + 2*q1*q3)*(q0^2 - q1^2 - q2^2 + q3^2) - dvxNoise*(2*q0*q2 - 2*q1*q3)*(q0^2 + q1^2 - q2^2 - q3^2) - dvyNoise*(2*q0*q1 + 2*q2*q3)*(2*q0*q3 - 2*q1*q2);
SQ(3) = dvxNoise*(2*q0*q3 + 2*q1*q2)*(q0^2 + q1^2 - q2^2 - q3^2) - dvyNoise*(2*q0*q3 - 2*q1*q2)*(q0^2 - q1^2 + q2^2 - q3^2) - dvzNoise*(2*q0*q1 - 2*q2*q3)*(2*q0*q2 + 2*q1*q3);
SQ(4) = (q0^2 + q1^2 + q2^2 + q3^2)^2;
SQ(5) = q3^2;
SQ(6) = q2^2;
SQ(7) = q1^2;
SQ(8) = q0^2;
SPP = zeros(19,1);
SPP(1) = 2*q2*SF(12) - 2*q0*SF(14) + 2*q1*SF(15) + 2*q3*SF(13);
SPP(2) = 2*q0*SF(10) + 2*q2*SF(8) + 2*q1*SF(11) - 2*q3*SF(9);
SPP(3) = 2*q0*SF(15) + 2*q1*SF(14) - 2*q2*SF(13) + 2*q3*SF(12);
SPP(4) = 2*q0*SF(11) - 2*q1*SF(10) + 2*q2*SF(9) + 2*q3*SF(8);
SPP(5) = 2*q0*SF(12) + 2*q1*SF(13) + 2*q2*SF(14) - 2*q3*SF(15);
SPP(6) = 2*q1*SF(9) - 2*q0*SF(8) + 2*q2*SF(10) + 2*q3*SF(11);
SPP(7) = 2*q0*SF(6) - 2*q2*SF(4) + 2*q1*SF(7) + 2*q3*SF(5);
SPP(8) = 2*q1*SF(6) - 2*q0*SF(7) + 2*q2*SF(5) + 2*q3*SF(4);
SPP(9) = 2*q0*SF(4) - 2*q1*SF(5) + 2*q2*SF(6) + 2*q3*SF(7);
SPP(10) = dvy*SF(17) - dvz*SF(23);
SPP(11) = dvx*SF(18) - dvy*SF(24);
SPP(12) = dvx*SF(25) - dvz*SF(19);
SPP(13) = dvx*SF(17) + dvz*SF(20);
SPP(14) = dvx*SF(23) + dvy*SF(20);
SPP(15) = dvy*SF(21) + dvz*SF(18);
SPP(16) = dvx*SF(21) + dvz*SF(24);
SPP(17) = dvy*SF(25) + dvz*SF(22);
SPP(18) = dvx*SF(22) + dvy*SF(19);
SPP(19) = SF(16);
nextP = zeros(9,9);
nextP(1,1) = daxNoise*SQ(4) + SPP(5)*(OP_l_1_c_1_r_*SPP(5) + OP_l_2_c_1_r_*SPP(6) - OP_l_3_c_1_r_*SPP(9) + OP_l_7_c_1_r_*SPP(19)) + SPP(6)*(OP_l_1_c_2_r_*SPP(5) + OP_l_2_c_2_r_*SPP(6) - OP_l_3_c_2_r_*SPP(9) + OP_l_7_c_2_r_*SPP(19)) - SPP(9)*(OP_l_1_c_3_r_*SPP(5) + OP_l_2_c_3_r_*SPP(6) - OP_l_3_c_3_r_*SPP(9) + OP_l_7_c_3_r_*SPP(19)) + SPP(19)*(OP_l_1_c_7_r_*SPP(5) + OP_l_2_c_7_r_*SPP(6) - OP_l_3_c_7_r_*SPP(9) + OP_l_7_c_7_r_*SPP(19));
nextP(1,2) = SPP(4)*(OP_l_1_c_2_r_*SPP(5) + OP_l_2_c_2_r_*SPP(6) - OP_l_3_c_2_r_*SPP(9) + OP_l_7_c_2_r_*SPP(19)) - SPP(3)*(OP_l_1_c_1_r_*SPP(5) + OP_l_2_c_1_r_*SPP(6) - OP_l_3_c_1_r_*SPP(9) + OP_l_7_c_1_r_*SPP(19)) + SPP(8)*(OP_l_1_c_3_r_*SPP(5) + OP_l_2_c_3_r_*SPP(6) - OP_l_3_c_3_r_*SPP(9) + OP_l_7_c_3_r_*SPP(19)) + SPP(19)*(OP_l_1_c_8_r_*SPP(5) + OP_l_2_c_8_r_*SPP(6) - OP_l_3_c_8_r_*SPP(9) + OP_l_7_c_8_r_*SPP(19));
nextP(1,3) = SPP(1)*(OP_l_1_c_1_r_*SPP(5) + OP_l_2_c_1_r_*SPP(6) - OP_l_3_c_1_r_*SPP(9) + OP_l_7_c_1_r_*SPP(19)) - SPP(2)*(OP_l_1_c_2_r_*SPP(5) + OP_l_2_c_2_r_*SPP(6) - OP_l_3_c_2_r_*SPP(9) + OP_l_7_c_2_r_*SPP(19)) + SPP(7)*(OP_l_1_c_3_r_*SPP(5) + OP_l_2_c_3_r_*SPP(6) - OP_l_3_c_3_r_*SPP(9) + OP_l_7_c_3_r_*SPP(19)) + SPP(19)*(OP_l_1_c_9_r_*SPP(5) + OP_l_2_c_9_r_*SPP(6) - OP_l_3_c_9_r_*SPP(9) + OP_l_7_c_9_r_*SPP(19));
nextP(1,4) = OP_l_1_c_4_r_*SPP(5) + OP_l_2_c_4_r_*SPP(6) - OP_l_3_c_4_r_*SPP(9) + OP_l_7_c_4_r_*SPP(19) - SPP(12)*(OP_l_1_c_2_r_*SPP(5) + OP_l_2_c_2_r_*SPP(6) - OP_l_3_c_2_r_*SPP(9) + OP_l_7_c_2_r_*SPP(19)) + SPP(17)*(OP_l_1_c_1_r_*SPP(5) + OP_l_2_c_1_r_*SPP(6) - OP_l_3_c_1_r_*SPP(9) + OP_l_7_c_1_r_*SPP(19)) - SPP(18)*(OP_l_1_c_3_r_*SPP(5) + OP_l_2_c_3_r_*SPP(6) - OP_l_3_c_3_r_*SPP(9) + OP_l_7_c_3_r_*SPP(19));
nextP(1,5) = OP_l_1_c_5_r_*SPP(5) + OP_l_2_c_5_r_*SPP(6) - OP_l_3_c_5_r_*SPP(9) + OP_l_7_c_5_r_*SPP(19) - SPP(15)*(OP_l_1_c_1_r_*SPP(5) + OP_l_2_c_1_r_*SPP(6) - OP_l_3_c_1_r_*SPP(9) + OP_l_7_c_1_r_*SPP(19)) + SPP(11)*(OP_l_1_c_3_r_*SPP(5) + OP_l_2_c_3_r_*SPP(6) - OP_l_3_c_3_r_*SPP(9) + OP_l_7_c_3_r_*SPP(19)) + SPP(16)*(OP_l_1_c_2_r_*SPP(5) + OP_l_2_c_2_r_*SPP(6) - OP_l_3_c_2_r_*SPP(9) + OP_l_7_c_2_r_*SPP(19));
nextP(1,6) = OP_l_1_c_6_r_*SPP(5) + OP_l_2_c_6_r_*SPP(6) - OP_l_3_c_6_r_*SPP(9) + OP_l_7_c_6_r_*SPP(19) + SPP(10)*(OP_l_1_c_1_r_*SPP(5) + OP_l_2_c_1_r_*SPP(6) - OP_l_3_c_1_r_*SPP(9) + OP_l_7_c_1_r_*SPP(19)) - SPP(13)*(OP_l_1_c_2_r_*SPP(5) + OP_l_2_c_2_r_*SPP(6) - OP_l_3_c_2_r_*SPP(9) + OP_l_7_c_2_r_*SPP(19)) + SPP(14)*(OP_l_1_c_3_r_*SPP(5) + OP_l_2_c_3_r_*SPP(6) - OP_l_3_c_3_r_*SPP(9) + OP_l_7_c_3_r_*SPP(19));
nextP(1,7) = OP_l_1_c_7_r_*SPP(5) + OP_l_2_c_7_r_*SPP(6) - OP_l_3_c_7_r_*SPP(9) + OP_l_7_c_7_r_*SPP(19);
nextP(1,8) = OP_l_1_c_8_r_*SPP(5) + OP_l_2_c_8_r_*SPP(6) - OP_l_3_c_8_r_*SPP(9) + OP_l_7_c_8_r_*SPP(19);
nextP(1,9) = OP_l_1_c_9_r_*SPP(5) + OP_l_2_c_9_r_*SPP(6) - OP_l_3_c_9_r_*SPP(9) + OP_l_7_c_9_r_*SPP(19);
nextP(2,1) = SPP(5)*(OP_l_2_c_1_r_*SPP(4) - OP_l_1_c_1_r_*SPP(3) + OP_l_3_c_1_r_*SPP(8) + OP_l_8_c_1_r_*SPP(19)) + SPP(6)*(OP_l_2_c_2_r_*SPP(4) - OP_l_1_c_2_r_*SPP(3) + OP_l_3_c_2_r_*SPP(8) + OP_l_8_c_2_r_*SPP(19)) - SPP(9)*(OP_l_2_c_3_r_*SPP(4) - OP_l_1_c_3_r_*SPP(3) + OP_l_3_c_3_r_*SPP(8) + OP_l_8_c_3_r_*SPP(19)) + SPP(19)*(OP_l_2_c_7_r_*SPP(4) - OP_l_1_c_7_r_*SPP(3) + OP_l_3_c_7_r_*SPP(8) + OP_l_8_c_7_r_*SPP(19));
nextP(2,2) = dayNoise*SQ(4) - SPP(3)*(OP_l_2_c_1_r_*SPP(4) - OP_l_1_c_1_r_*SPP(3) + OP_l_3_c_1_r_*SPP(8) + OP_l_8_c_1_r_*SPP(19)) + SPP(4)*(OP_l_2_c_2_r_*SPP(4) - OP_l_1_c_2_r_*SPP(3) + OP_l_3_c_2_r_*SPP(8) + OP_l_8_c_2_r_*SPP(19)) + SPP(8)*(OP_l_2_c_3_r_*SPP(4) - OP_l_1_c_3_r_*SPP(3) + OP_l_3_c_3_r_*SPP(8) + OP_l_8_c_3_r_*SPP(19)) + SPP(19)*(OP_l_2_c_8_r_*SPP(4) - OP_l_1_c_8_r_*SPP(3) + OP_l_3_c_8_r_*SPP(8) + OP_l_8_c_8_r_*SPP(19));
nextP(2,3) = SPP(1)*(OP_l_2_c_1_r_*SPP(4) - OP_l_1_c_1_r_*SPP(3) + OP_l_3_c_1_r_*SPP(8) + OP_l_8_c_1_r_*SPP(19)) - SPP(2)*(OP_l_2_c_2_r_*SPP(4) - OP_l_1_c_2_r_*SPP(3) + OP_l_3_c_2_r_*SPP(8) + OP_l_8_c_2_r_*SPP(19)) + SPP(7)*(OP_l_2_c_3_r_*SPP(4) - OP_l_1_c_3_r_*SPP(3) + OP_l_3_c_3_r_*SPP(8) + OP_l_8_c_3_r_*SPP(19)) + SPP(19)*(OP_l_2_c_9_r_*SPP(4) - OP_l_1_c_9_r_*SPP(3) + OP_l_3_c_9_r_*SPP(8) + OP_l_8_c_9_r_*SPP(19));
nextP(2,4) = OP_l_2_c_4_r_*SPP(4) - OP_l_1_c_4_r_*SPP(3) + OP_l_3_c_4_r_*SPP(8) + OP_l_8_c_4_r_*SPP(19) - SPP(12)*(OP_l_2_c_2_r_*SPP(4) - OP_l_1_c_2_r_*SPP(3) + OP_l_3_c_2_r_*SPP(8) + OP_l_8_c_2_r_*SPP(19)) + SPP(17)*(OP_l_2_c_1_r_*SPP(4) - OP_l_1_c_1_r_*SPP(3) + OP_l_3_c_1_r_*SPP(8) + OP_l_8_c_1_r_*SPP(19)) - SPP(18)*(OP_l_2_c_3_r_*SPP(4) - OP_l_1_c_3_r_*SPP(3) + OP_l_3_c_3_r_*SPP(8) + OP_l_8_c_3_r_*SPP(19));
nextP(2,5) = OP_l_2_c_5_r_*SPP(4) - OP_l_1_c_5_r_*SPP(3) + OP_l_3_c_5_r_*SPP(8) + OP_l_8_c_5_r_*SPP(19) - SPP(15)*(OP_l_2_c_1_r_*SPP(4) - OP_l_1_c_1_r_*SPP(3) + OP_l_3_c_1_r_*SPP(8) + OP_l_8_c_1_r_*SPP(19)) + SPP(11)*(OP_l_2_c_3_r_*SPP(4) - OP_l_1_c_3_r_*SPP(3) + OP_l_3_c_3_r_*SPP(8) + OP_l_8_c_3_r_*SPP(19)) + SPP(16)*(OP_l_2_c_2_r_*SPP(4) - OP_l_1_c_2_r_*SPP(3) + OP_l_3_c_2_r_*SPP(8) + OP_l_8_c_2_r_*SPP(19));
nextP(2,6) = OP_l_2_c_6_r_*SPP(4) - OP_l_1_c_6_r_*SPP(3) + OP_l_3_c_6_r_*SPP(8) + OP_l_8_c_6_r_*SPP(19) + SPP(10)*(OP_l_2_c_1_r_*SPP(4) - OP_l_1_c_1_r_*SPP(3) + OP_l_3_c_1_r_*SPP(8) + OP_l_8_c_1_r_*SPP(19)) - SPP(13)*(OP_l_2_c_2_r_*SPP(4) - OP_l_1_c_2_r_*SPP(3) + OP_l_3_c_2_r_*SPP(8) + OP_l_8_c_2_r_*SPP(19)) + SPP(14)*(OP_l_2_c_3_r_*SPP(4) - OP_l_1_c_3_r_*SPP(3) + OP_l_3_c_3_r_*SPP(8) + OP_l_8_c_3_r_*SPP(19));
nextP(2,7) = OP_l_2_c_7_r_*SPP(4) - OP_l_1_c_7_r_*SPP(3) + OP_l_3_c_7_r_*SPP(8) + OP_l_8_c_7_r_*SPP(19);
nextP(2,8) = OP_l_2_c_8_r_*SPP(4) - OP_l_1_c_8_r_*SPP(3) + OP_l_3_c_8_r_*SPP(8) + OP_l_8_c_8_r_*SPP(19);
nextP(2,9) = OP_l_2_c_9_r_*SPP(4) - OP_l_1_c_9_r_*SPP(3) + OP_l_3_c_9_r_*SPP(8) + OP_l_8_c_9_r_*SPP(19);
nextP(3,1) = SPP(5)*(OP_l_1_c_1_r_*SPP(1) - OP_l_2_c_1_r_*SPP(2) + OP_l_3_c_1_r_*SPP(7) + OP_l_9_c_1_r_*SPP(19)) + SPP(6)*(OP_l_1_c_2_r_*SPP(1) - OP_l_2_c_2_r_*SPP(2) + OP_l_3_c_2_r_*SPP(7) + OP_l_9_c_2_r_*SPP(19)) - SPP(9)*(OP_l_1_c_3_r_*SPP(1) - OP_l_2_c_3_r_*SPP(2) + OP_l_3_c_3_r_*SPP(7) + OP_l_9_c_3_r_*SPP(19)) + SPP(19)*(OP_l_1_c_7_r_*SPP(1) - OP_l_2_c_7_r_*SPP(2) + OP_l_3_c_7_r_*SPP(7) + OP_l_9_c_7_r_*SPP(19));
nextP(3,2) = SPP(4)*(OP_l_1_c_2_r_*SPP(1) - OP_l_2_c_2_r_*SPP(2) + OP_l_3_c_2_r_*SPP(7) + OP_l_9_c_2_r_*SPP(19)) - SPP(3)*(OP_l_1_c_1_r_*SPP(1) - OP_l_2_c_1_r_*SPP(2) + OP_l_3_c_1_r_*SPP(7) + OP_l_9_c_1_r_*SPP(19)) + SPP(8)*(OP_l_1_c_3_r_*SPP(1) - OP_l_2_c_3_r_*SPP(2) + OP_l_3_c_3_r_*SPP(7) + OP_l_9_c_3_r_*SPP(19)) + SPP(19)*(OP_l_1_c_8_r_*SPP(1) - OP_l_2_c_8_r_*SPP(2) + OP_l_3_c_8_r_*SPP(7) + OP_l_9_c_8_r_*SPP(19));
nextP(3,3) = dazNoise*SQ(4) + SPP(1)*(OP_l_1_c_1_r_*SPP(1) - OP_l_2_c_1_r_*SPP(2) + OP_l_3_c_1_r_*SPP(7) + OP_l_9_c_1_r_*SPP(19)) - SPP(2)*(OP_l_1_c_2_r_*SPP(1) - OP_l_2_c_2_r_*SPP(2) + OP_l_3_c_2_r_*SPP(7) + OP_l_9_c_2_r_*SPP(19)) + SPP(7)*(OP_l_1_c_3_r_*SPP(1) - OP_l_2_c_3_r_*SPP(2) + OP_l_3_c_3_r_*SPP(7) + OP_l_9_c_3_r_*SPP(19)) + SPP(19)*(OP_l_1_c_9_r_*SPP(1) - OP_l_2_c_9_r_*SPP(2) + OP_l_3_c_9_r_*SPP(7) + OP_l_9_c_9_r_*SPP(19));
nextP(3,4) = OP_l_1_c_4_r_*SPP(1) - OP_l_2_c_4_r_*SPP(2) + OP_l_3_c_4_r_*SPP(7) + OP_l_9_c_4_r_*SPP(19) - SPP(12)*(OP_l_1_c_2_r_*SPP(1) - OP_l_2_c_2_r_*SPP(2) + OP_l_3_c_2_r_*SPP(7) + OP_l_9_c_2_r_*SPP(19)) + SPP(17)*(OP_l_1_c_1_r_*SPP(1) - OP_l_2_c_1_r_*SPP(2) + OP_l_3_c_1_r_*SPP(7) + OP_l_9_c_1_r_*SPP(19)) - SPP(18)*(OP_l_1_c_3_r_*SPP(1) - OP_l_2_c_3_r_*SPP(2) + OP_l_3_c_3_r_*SPP(7) + OP_l_9_c_3_r_*SPP(19));
nextP(3,5) = OP_l_1_c_5_r_*SPP(1) - OP_l_2_c_5_r_*SPP(2) + OP_l_3_c_5_r_*SPP(7) + OP_l_9_c_5_r_*SPP(19) - SPP(15)*(OP_l_1_c_1_r_*SPP(1) - OP_l_2_c_1_r_*SPP(2) + OP_l_3_c_1_r_*SPP(7) + OP_l_9_c_1_r_*SPP(19)) + SPP(11)*(OP_l_1_c_3_r_*SPP(1) - OP_l_2_c_3_r_*SPP(2) + OP_l_3_c_3_r_*SPP(7) + OP_l_9_c_3_r_*SPP(19)) + SPP(16)*(OP_l_1_c_2_r_*SPP(1) - OP_l_2_c_2_r_*SPP(2) + OP_l_3_c_2_r_*SPP(7) + OP_l_9_c_2_r_*SPP(19));
nextP(3,6) = OP_l_1_c_6_r_*SPP(1) - OP_l_2_c_6_r_*SPP(2) + OP_l_3_c_6_r_*SPP(7) + OP_l_9_c_6_r_*SPP(19) + SPP(10)*(OP_l_1_c_1_r_*SPP(1) - OP_l_2_c_1_r_*SPP(2) + OP_l_3_c_1_r_*SPP(7) + OP_l_9_c_1_r_*SPP(19)) - SPP(13)*(OP_l_1_c_2_r_*SPP(1) - OP_l_2_c_2_r_*SPP(2) + OP_l_3_c_2_r_*SPP(7) + OP_l_9_c_2_r_*SPP(19)) + SPP(14)*(OP_l_1_c_3_r_*SPP(1) - OP_l_2_c_3_r_*SPP(2) + OP_l_3_c_3_r_*SPP(7) + OP_l_9_c_3_r_*SPP(19));
nextP(3,7) = OP_l_1_c_7_r_*SPP(1) - OP_l_2_c_7_r_*SPP(2) + OP_l_3_c_7_r_*SPP(7) + OP_l_9_c_7_r_*SPP(19);
nextP(3,8) = OP_l_1_c_8_r_*SPP(1) - OP_l_2_c_8_r_*SPP(2) + OP_l_3_c_8_r_*SPP(7) + OP_l_9_c_8_r_*SPP(19);
nextP(3,9) = OP_l_1_c_9_r_*SPP(1) - OP_l_2_c_9_r_*SPP(2) + OP_l_3_c_9_r_*SPP(7) + OP_l_9_c_9_r_*SPP(19);
nextP(4,1) = SPP(5)*(OP_l_4_c_1_r_ + OP_l_1_c_1_r_*SPP(17) - OP_l_2_c_1_r_*SPP(12) - OP_l_3_c_1_r_*SPP(18)) + SPP(6)*(OP_l_4_c_2_r_ + OP_l_1_c_2_r_*SPP(17) - OP_l_2_c_2_r_*SPP(12) - OP_l_3_c_2_r_*SPP(18)) - SPP(9)*(OP_l_4_c_3_r_ + OP_l_1_c_3_r_*SPP(17) - OP_l_2_c_3_r_*SPP(12) - OP_l_3_c_3_r_*SPP(18)) + SPP(19)*(OP_l_4_c_7_r_ + OP_l_1_c_7_r_*SPP(17) - OP_l_2_c_7_r_*SPP(12) - OP_l_3_c_7_r_*SPP(18));
nextP(4,2) = SPP(4)*(OP_l_4_c_2_r_ + OP_l_1_c_2_r_*SPP(17) - OP_l_2_c_2_r_*SPP(12) - OP_l_3_c_2_r_*SPP(18)) - SPP(3)*(OP_l_4_c_1_r_ + OP_l_1_c_1_r_*SPP(17) - OP_l_2_c_1_r_*SPP(12) - OP_l_3_c_1_r_*SPP(18)) + SPP(8)*(OP_l_4_c_3_r_ + OP_l_1_c_3_r_*SPP(17) - OP_l_2_c_3_r_*SPP(12) - OP_l_3_c_3_r_*SPP(18)) + SPP(19)*(OP_l_4_c_8_r_ + OP_l_1_c_8_r_*SPP(17) - OP_l_2_c_8_r_*SPP(12) - OP_l_3_c_8_r_*SPP(18));
nextP(4,3) = SPP(1)*(OP_l_4_c_1_r_ + OP_l_1_c_1_r_*SPP(17) - OP_l_2_c_1_r_*SPP(12) - OP_l_3_c_1_r_*SPP(18)) - SPP(2)*(OP_l_4_c_2_r_ + OP_l_1_c_2_r_*SPP(17) - OP_l_2_c_2_r_*SPP(12) - OP_l_3_c_2_r_*SPP(18)) + SPP(7)*(OP_l_4_c_3_r_ + OP_l_1_c_3_r_*SPP(17) - OP_l_2_c_3_r_*SPP(12) - OP_l_3_c_3_r_*SPP(18)) + SPP(19)*(OP_l_4_c_9_r_ + OP_l_1_c_9_r_*SPP(17) - OP_l_2_c_9_r_*SPP(12) - OP_l_3_c_9_r_*SPP(18));
nextP(4,4) = OP_l_4_c_4_r_ + OP_l_1_c_4_r_*SPP(17) - OP_l_2_c_4_r_*SPP(12) - OP_l_3_c_4_r_*SPP(18) + dvyNoise*(2*q0*q3 - 2*q1*q2)^2 + dvzNoise*(2*q0*q2 + 2*q1*q3)^2 - SPP(12)*(OP_l_4_c_2_r_ + OP_l_1_c_2_r_*SPP(17) - OP_l_2_c_2_r_*SPP(12) - OP_l_3_c_2_r_*SPP(18)) + SPP(17)*(OP_l_4_c_1_r_ + OP_l_1_c_1_r_*SPP(17) - OP_l_2_c_1_r_*SPP(12) - OP_l_3_c_1_r_*SPP(18)) - SPP(18)*(OP_l_4_c_3_r_ + OP_l_1_c_3_r_*SPP(17) - OP_l_2_c_3_r_*SPP(12) - OP_l_3_c_3_r_*SPP(18)) + dvxNoise*(SQ(5) + SQ(6) - SQ(7) - SQ(8))^2;
nextP(4,5) = OP_l_4_c_5_r_ + SQ(3) + OP_l_1_c_5_r_*SPP(17) - OP_l_2_c_5_r_*SPP(12) - OP_l_3_c_5_r_*SPP(18) - SPP(15)*(OP_l_4_c_1_r_ + OP_l_1_c_1_r_*SPP(17) - OP_l_2_c_1_r_*SPP(12) - OP_l_3_c_1_r_*SPP(18)) + SPP(11)*(OP_l_4_c_3_r_ + OP_l_1_c_3_r_*SPP(17) - OP_l_2_c_3_r_*SPP(12) - OP_l_3_c_3_r_*SPP(18)) + SPP(16)*(OP_l_4_c_2_r_ + OP_l_1_c_2_r_*SPP(17) - OP_l_2_c_2_r_*SPP(12) - OP_l_3_c_2_r_*SPP(18));
nextP(4,6) = OP_l_4_c_6_r_ + SQ(2) + OP_l_1_c_6_r_*SPP(17) - OP_l_2_c_6_r_*SPP(12) - OP_l_3_c_6_r_*SPP(18) + SPP(10)*(OP_l_4_c_1_r_ + OP_l_1_c_1_r_*SPP(17) - OP_l_2_c_1_r_*SPP(12) - OP_l_3_c_1_r_*SPP(18)) - SPP(13)*(OP_l_4_c_2_r_ + OP_l_1_c_2_r_*SPP(17) - OP_l_2_c_2_r_*SPP(12) - OP_l_3_c_2_r_*SPP(18)) + SPP(14)*(OP_l_4_c_3_r_ + OP_l_1_c_3_r_*SPP(17) - OP_l_2_c_3_r_*SPP(12) - OP_l_3_c_3_r_*SPP(18));
nextP(4,7) = OP_l_4_c_7_r_ + OP_l_1_c_7_r_*SPP(17) - OP_l_2_c_7_r_*SPP(12) - OP_l_3_c_7_r_*SPP(18);
nextP(4,8) = OP_l_4_c_8_r_ + OP_l_1_c_8_r_*SPP(17) - OP_l_2_c_8_r_*SPP(12) - OP_l_3_c_8_r_*SPP(18);
nextP(4,9) = OP_l_4_c_9_r_ + OP_l_1_c_9_r_*SPP(17) - OP_l_2_c_9_r_*SPP(12) - OP_l_3_c_9_r_*SPP(18);
nextP(5,1) = SPP(5)*(OP_l_5_c_1_r_ - OP_l_1_c_1_r_*SPP(15) + OP_l_2_c_1_r_*SPP(16) + OP_l_3_c_1_r_*SPP(11)) + SPP(6)*(OP_l_5_c_2_r_ - OP_l_1_c_2_r_*SPP(15) + OP_l_2_c_2_r_*SPP(16) + OP_l_3_c_2_r_*SPP(11)) - SPP(9)*(OP_l_5_c_3_r_ - OP_l_1_c_3_r_*SPP(15) + OP_l_2_c_3_r_*SPP(16) + OP_l_3_c_3_r_*SPP(11)) + SPP(19)*(OP_l_5_c_7_r_ - OP_l_1_c_7_r_*SPP(15) + OP_l_2_c_7_r_*SPP(16) + OP_l_3_c_7_r_*SPP(11));
nextP(5,2) = SPP(4)*(OP_l_5_c_2_r_ - OP_l_1_c_2_r_*SPP(15) + OP_l_2_c_2_r_*SPP(16) + OP_l_3_c_2_r_*SPP(11)) - SPP(3)*(OP_l_5_c_1_r_ - OP_l_1_c_1_r_*SPP(15) + OP_l_2_c_1_r_*SPP(16) + OP_l_3_c_1_r_*SPP(11)) + SPP(8)*(OP_l_5_c_3_r_ - OP_l_1_c_3_r_*SPP(15) + OP_l_2_c_3_r_*SPP(16) + OP_l_3_c_3_r_*SPP(11)) + SPP(19)*(OP_l_5_c_8_r_ - OP_l_1_c_8_r_*SPP(15) + OP_l_2_c_8_r_*SPP(16) + OP_l_3_c_8_r_*SPP(11));
nextP(5,3) = SPP(1)*(OP_l_5_c_1_r_ - OP_l_1_c_1_r_*SPP(15) + OP_l_2_c_1_r_*SPP(16) + OP_l_3_c_1_r_*SPP(11)) - SPP(2)*(OP_l_5_c_2_r_ - OP_l_1_c_2_r_*SPP(15) + OP_l_2_c_2_r_*SPP(16) + OP_l_3_c_2_r_*SPP(11)) + SPP(7)*(OP_l_5_c_3_r_ - OP_l_1_c_3_r_*SPP(15) + OP_l_2_c_3_r_*SPP(16) + OP_l_3_c_3_r_*SPP(11)) + SPP(19)*(OP_l_5_c_9_r_ - OP_l_1_c_9_r_*SPP(15) + OP_l_2_c_9_r_*SPP(16) + OP_l_3_c_9_r_*SPP(11));
nextP(5,4) = OP_l_5_c_4_r_ + SQ(3) - OP_l_1_c_4_r_*SPP(15) + OP_l_2_c_4_r_*SPP(16) + OP_l_3_c_4_r_*SPP(11) - SPP(12)*(OP_l_5_c_2_r_ - OP_l_1_c_2_r_*SPP(15) + OP_l_2_c_2_r_*SPP(16) + OP_l_3_c_2_r_*SPP(11)) + SPP(17)*(OP_l_5_c_1_r_ - OP_l_1_c_1_r_*SPP(15) + OP_l_2_c_1_r_*SPP(16) + OP_l_3_c_1_r_*SPP(11)) - SPP(18)*(OP_l_5_c_3_r_ - OP_l_1_c_3_r_*SPP(15) + OP_l_2_c_3_r_*SPP(16) + OP_l_3_c_3_r_*SPP(11));
nextP(5,5) = OP_l_5_c_5_r_ - OP_l_1_c_5_r_*SPP(15) + OP_l_2_c_5_r_*SPP(16) + OP_l_3_c_5_r_*SPP(11) + dvxNoise*(2*q0*q3 + 2*q1*q2)^2 + dvzNoise*(2*q0*q1 - 2*q2*q3)^2 - SPP(15)*(OP_l_5_c_1_r_ - OP_l_1_c_1_r_*SPP(15) + OP_l_2_c_1_r_*SPP(16) + OP_l_3_c_1_r_*SPP(11)) + SPP(11)*(OP_l_5_c_3_r_ - OP_l_1_c_3_r_*SPP(15) + OP_l_2_c_3_r_*SPP(16) + OP_l_3_c_3_r_*SPP(11)) + SPP(16)*(OP_l_5_c_2_r_ - OP_l_1_c_2_r_*SPP(15) + OP_l_2_c_2_r_*SPP(16) + OP_l_3_c_2_r_*SPP(11)) + dvyNoise*(SQ(5) - SQ(6) + SQ(7) - SQ(8))^2;
nextP(5,6) = OP_l_5_c_6_r_ + SQ(1) - OP_l_1_c_6_r_*SPP(15) + OP_l_2_c_6_r_*SPP(16) + OP_l_3_c_6_r_*SPP(11) + SPP(10)*(OP_l_5_c_1_r_ - OP_l_1_c_1_r_*SPP(15) + OP_l_2_c_1_r_*SPP(16) + OP_l_3_c_1_r_*SPP(11)) - SPP(13)*(OP_l_5_c_2_r_ - OP_l_1_c_2_r_*SPP(15) + OP_l_2_c_2_r_*SPP(16) + OP_l_3_c_2_r_*SPP(11)) + SPP(14)*(OP_l_5_c_3_r_ - OP_l_1_c_3_r_*SPP(15) + OP_l_2_c_3_r_*SPP(16) + OP_l_3_c_3_r_*SPP(11));
nextP(5,7) = OP_l_5_c_7_r_ - OP_l_1_c_7_r_*SPP(15) + OP_l_2_c_7_r_*SPP(16) + OP_l_3_c_7_r_*SPP(11);
nextP(5,8) = OP_l_5_c_8_r_ - OP_l_1_c_8_r_*SPP(15) + OP_l_2_c_8_r_*SPP(16) + OP_l_3_c_8_r_*SPP(11);
nextP(5,9) = OP_l_5_c_9_r_ - OP_l_1_c_9_r_*SPP(15) + OP_l_2_c_9_r_*SPP(16) + OP_l_3_c_9_r_*SPP(11);
nextP(6,1) = SPP(5)*(OP_l_6_c_1_r_ + OP_l_1_c_1_r_*SPP(10) - OP_l_2_c_1_r_*SPP(13) + OP_l_3_c_1_r_*SPP(14)) + SPP(6)*(OP_l_6_c_2_r_ + OP_l_1_c_2_r_*SPP(10) - OP_l_2_c_2_r_*SPP(13) + OP_l_3_c_2_r_*SPP(14)) - SPP(9)*(OP_l_6_c_3_r_ + OP_l_1_c_3_r_*SPP(10) - OP_l_2_c_3_r_*SPP(13) + OP_l_3_c_3_r_*SPP(14)) + SPP(19)*(OP_l_6_c_7_r_ + OP_l_1_c_7_r_*SPP(10) - OP_l_2_c_7_r_*SPP(13) + OP_l_3_c_7_r_*SPP(14));
nextP(6,2) = SPP(4)*(OP_l_6_c_2_r_ + OP_l_1_c_2_r_*SPP(10) - OP_l_2_c_2_r_*SPP(13) + OP_l_3_c_2_r_*SPP(14)) - SPP(3)*(OP_l_6_c_1_r_ + OP_l_1_c_1_r_*SPP(10) - OP_l_2_c_1_r_*SPP(13) + OP_l_3_c_1_r_*SPP(14)) + SPP(8)*(OP_l_6_c_3_r_ + OP_l_1_c_3_r_*SPP(10) - OP_l_2_c_3_r_*SPP(13) + OP_l_3_c_3_r_*SPP(14)) + SPP(19)*(OP_l_6_c_8_r_ + OP_l_1_c_8_r_*SPP(10) - OP_l_2_c_8_r_*SPP(13) + OP_l_3_c_8_r_*SPP(14));
nextP(6,3) = SPP(1)*(OP_l_6_c_1_r_ + OP_l_1_c_1_r_*SPP(10) - OP_l_2_c_1_r_*SPP(13) + OP_l_3_c_1_r_*SPP(14)) - SPP(2)*(OP_l_6_c_2_r_ + OP_l_1_c_2_r_*SPP(10) - OP_l_2_c_2_r_*SPP(13) + OP_l_3_c_2_r_*SPP(14)) + SPP(7)*(OP_l_6_c_3_r_ + OP_l_1_c_3_r_*SPP(10) - OP_l_2_c_3_r_*SPP(13) + OP_l_3_c_3_r_*SPP(14)) + SPP(19)*(OP_l_6_c_9_r_ + OP_l_1_c_9_r_*SPP(10) - OP_l_2_c_9_r_*SPP(13) + OP_l_3_c_9_r_*SPP(14));
nextP(6,4) = OP_l_6_c_4_r_ + SQ(2) + OP_l_1_c_4_r_*SPP(10) - OP_l_2_c_4_r_*SPP(13) + OP_l_3_c_4_r_*SPP(14) - SPP(12)*(OP_l_6_c_2_r_ + OP_l_1_c_2_r_*SPP(10) - OP_l_2_c_2_r_*SPP(13) + OP_l_3_c_2_r_*SPP(14)) + SPP(17)*(OP_l_6_c_1_r_ + OP_l_1_c_1_r_*SPP(10) - OP_l_2_c_1_r_*SPP(13) + OP_l_3_c_1_r_*SPP(14)) - SPP(18)*(OP_l_6_c_3_r_ + OP_l_1_c_3_r_*SPP(10) - OP_l_2_c_3_r_*SPP(13) + OP_l_3_c_3_r_*SPP(14));
nextP(6,5) = OP_l_6_c_5_r_ + SQ(1) + OP_l_1_c_5_r_*SPP(10) - OP_l_2_c_5_r_*SPP(13) + OP_l_3_c_5_r_*SPP(14) - SPP(15)*(OP_l_6_c_1_r_ + OP_l_1_c_1_r_*SPP(10) - OP_l_2_c_1_r_*SPP(13) + OP_l_3_c_1_r_*SPP(14)) + SPP(11)*(OP_l_6_c_3_r_ + OP_l_1_c_3_r_*SPP(10) - OP_l_2_c_3_r_*SPP(13) + OP_l_3_c_3_r_*SPP(14)) + SPP(16)*(OP_l_6_c_2_r_ + OP_l_1_c_2_r_*SPP(10) - OP_l_2_c_2_r_*SPP(13) + OP_l_3_c_2_r_*SPP(14));
nextP(6,6) = OP_l_6_c_6_r_ + OP_l_1_c_6_r_*SPP(10) - OP_l_2_c_6_r_*SPP(13) + OP_l_3_c_6_r_*SPP(14) + dvxNoise*(2*q0*q2 - 2*q1*q3)^2 + dvyNoise*(2*q0*q1 + 2*q2*q3)^2 + SPP(10)*(OP_l_6_c_1_r_ + OP_l_1_c_1_r_*SPP(10) - OP_l_2_c_1_r_*SPP(13) + OP_l_3_c_1_r_*SPP(14)) - SPP(13)*(OP_l_6_c_2_r_ + OP_l_1_c_2_r_*SPP(10) - OP_l_2_c_2_r_*SPP(13) + OP_l_3_c_2_r_*SPP(14)) + SPP(14)*(OP_l_6_c_3_r_ + OP_l_1_c_3_r_*SPP(10) - OP_l_2_c_3_r_*SPP(13) + OP_l_3_c_3_r_*SPP(14)) + dvzNoise*(SQ(5) - SQ(6) - SQ(7) + SQ(8))^2;
nextP(6,7) = OP_l_6_c_7_r_ + OP_l_1_c_7_r_*SPP(10) - OP_l_2_c_7_r_*SPP(13) + OP_l_3_c_7_r_*SPP(14);
nextP(6,8) = OP_l_6_c_8_r_ + OP_l_1_c_8_r_*SPP(10) - OP_l_2_c_8_r_*SPP(13) + OP_l_3_c_8_r_*SPP(14);
nextP(6,9) = OP_l_6_c_9_r_ + OP_l_1_c_9_r_*SPP(10) - OP_l_2_c_9_r_*SPP(13) + OP_l_3_c_9_r_*SPP(14);
nextP(7,1) = OP_l_7_c_1_r_*SPP(5) + OP_l_7_c_2_r_*SPP(6) - OP_l_7_c_3_r_*SPP(9) + OP_l_7_c_7_r_*SPP(19);
nextP(7,2) = OP_l_7_c_2_r_*SPP(4) - OP_l_7_c_1_r_*SPP(3) + OP_l_7_c_3_r_*SPP(8) + OP_l_7_c_8_r_*SPP(19);
nextP(7,3) = OP_l_7_c_1_r_*SPP(1) - OP_l_7_c_2_r_*SPP(2) + OP_l_7_c_3_r_*SPP(7) + OP_l_7_c_9_r_*SPP(19);
nextP(7,4) = OP_l_7_c_4_r_ - OP_l_7_c_2_r_*SPP(12) + OP_l_7_c_1_r_*SPP(17) - OP_l_7_c_3_r_*SPP(18);
nextP(7,5) = OP_l_7_c_5_r_ + OP_l_7_c_3_r_*SPP(11) - OP_l_7_c_1_r_*SPP(15) + OP_l_7_c_2_r_*SPP(16);
nextP(7,6) = OP_l_7_c_6_r_ + OP_l_7_c_1_r_*SPP(10) - OP_l_7_c_2_r_*SPP(13) + OP_l_7_c_3_r_*SPP(14);
nextP(7,7) = OP_l_7_c_7_r_;
nextP(7,8) = OP_l_7_c_8_r_;
nextP(7,9) = OP_l_7_c_9_r_;
nextP(8,1) = OP_l_8_c_1_r_*SPP(5) + OP_l_8_c_2_r_*SPP(6) - OP_l_8_c_3_r_*SPP(9) + OP_l_8_c_7_r_*SPP(19);
nextP(8,2) = OP_l_8_c_2_r_*SPP(4) - OP_l_8_c_1_r_*SPP(3) + OP_l_8_c_3_r_*SPP(8) + OP_l_8_c_8_r_*SPP(19);
nextP(8,3) = OP_l_8_c_1_r_*SPP(1) - OP_l_8_c_2_r_*SPP(2) + OP_l_8_c_3_r_*SPP(7) + OP_l_8_c_9_r_*SPP(19);
nextP(8,4) = OP_l_8_c_4_r_ - OP_l_8_c_2_r_*SPP(12) + OP_l_8_c_1_r_*SPP(17) - OP_l_8_c_3_r_*SPP(18);
nextP(8,5) = OP_l_8_c_5_r_ + OP_l_8_c_3_r_*SPP(11) - OP_l_8_c_1_r_*SPP(15) + OP_l_8_c_2_r_*SPP(16);
nextP(8,6) = OP_l_8_c_6_r_ + OP_l_8_c_1_r_*SPP(10) - OP_l_8_c_2_r_*SPP(13) + OP_l_8_c_3_r_*SPP(14);
nextP(8,7) = OP_l_8_c_7_r_;
nextP(8,8) = OP_l_8_c_8_r_;
nextP(8,9) = OP_l_8_c_9_r_;
nextP(9,1) = OP_l_9_c_1_r_*SPP(5) + OP_l_9_c_2_r_*SPP(6) - OP_l_9_c_3_r_*SPP(9) + OP_l_9_c_7_r_*SPP(19);
nextP(9,2) = OP_l_9_c_2_r_*SPP(4) - OP_l_9_c_1_r_*SPP(3) + OP_l_9_c_3_r_*SPP(8) + OP_l_9_c_8_r_*SPP(19);
nextP(9,3) = OP_l_9_c_1_r_*SPP(1) - OP_l_9_c_2_r_*SPP(2) + OP_l_9_c_3_r_*SPP(7) + OP_l_9_c_9_r_*SPP(19);
nextP(9,4) = OP_l_9_c_4_r_ - OP_l_9_c_2_r_*SPP(12) + OP_l_9_c_1_r_*SPP(17) - OP_l_9_c_3_r_*SPP(18);
nextP(9,5) = OP_l_9_c_5_r_ + OP_l_9_c_3_r_*SPP(11) - OP_l_9_c_1_r_*SPP(15) + OP_l_9_c_2_r_*SPP(16);
nextP(9,6) = OP_l_9_c_6_r_ + OP_l_9_c_1_r_*SPP(10) - OP_l_9_c_2_r_*SPP(13) + OP_l_9_c_3_r_*SPP(14);
nextP(9,7) = OP_l_9_c_7_r_;
nextP(9,8) = OP_l_9_c_8_r_;
nextP(9,9) = OP_l_9_c_9_r_;

38
libraries/AP_NavEKF/Models/GimbalEstimatorExample/calcQ.c Normal file
View File

@ -0,0 +1,38 @@
t3 = q0*q0;
t4 = q1*q1;
t5 = q2*q2;
t6 = q3*q3;
t2 = t3+t4+t5+t6;
t7 = t2*t2;
t11 = q0*q3*2.0;
t12 = q1*q2*2.0;
t8 = t11-t12;
t13 = q0*q2*2.0;
t14 = q1*q3*2.0;
t9 = t13+t14;
t10 = t3+t4-t5-t6;
t15 = q0*q1*2.0;
t16 = t11+t12;
t17 = dvxNoise*t10*t16;
t18 = t3-t4+t5-t6;
t19 = q2*q3*2.0;
t20 = t15-t19;
t21 = t15+t19;
t22 = t3-t4-t5+t6;
t23 = t13-t14;
t24 = dvzNoise*t9*t22;
t25 = t24-dvxNoise*t10*t23-dvyNoise*t8*t21;
t26 = dvyNoise*t18*t21;
t27 = t26-dvxNoise*t16*t23-dvzNoise*t20*t22;
A0[0][0] = daxNoise*t7;
A0[1][1] = dayNoise*t7;
A0[2][2] = dazNoise*t7;
A0[3][3] = dvxNoise*(t10*t10)+dvyNoise*(t8*t8)+dvzNoise*(t9*t9);
A0[3][4] = t17-dvzNoise*t9*(t15-q2*q3*2.0)-dvyNoise*t8*t18;
A0[3][5] = t25;
A0[4][3] = t17-dvyNoise*t8*t18-dvzNoise*t9*t20;
A0[4][4] = dvxNoise*(t16*t16)+dvyNoise*(t18*t18)+dvzNoise*(t20*t20);
A0[4][5] = t27;
A0[5][3] = t25;
A0[5][4] = t27;
A0[5][5] = dvxNoise*(t23*t23)+dvyNoise*(t21*t21)+dvzNoise*(t22*t22);

38
libraries/AP_NavEKF/Models/GimbalEstimatorExample/calcQ.cpp Normal file
View File

@ -0,0 +1,38 @@
float t3 = q0*q0;
float t4 = q1*q1;
float t5 = q2*q2;
float t6 = q3*q3;
float t2 = t3+t4+t5+t6;
float t7 = t2*t2;
float t11 = q0*q3*2.0f;
float t12 = q1*q2*2.0f;
float t8 = t11-t12;
float t13 = q0*q2*2.0f;
float t14 = q1*q3*2.0f;
float t9 = t13+t14;
float t10 = t3+t4-t5-t6;
float t15 = q0*q1*2.0f;
float t16 = t11+t12;
float t17 = dvxNoise*t10*t16;
float t18 = t3-t4+t5-t6;
float t19 = q2*q3*2.0f;
float t20 = t15-t19;
float t21 = t15+t19;
float t22 = t3-t4-t5+t6;
float t23 = t13-t14;
float t24 = dvzNoise*t9*t22;
float t25 = t24-dvxNoise*t10*t23-dvyNoise*t8*t21;
float t26 = dvyNoise*t18*t21;
float t27 = t26-dvxNoise*t16*t23-dvzNoise*t20*t22;
A0[0][0] = daxNoise*t7;
A0[1][1] = dayNoise*t7;
A0[2][2] = dazNoise*t7;
A0[3][3] = dvxNoise*(t10*t10)+dvyNoise*(t8*t8)+dvzNoise*(t9*t9);
A0[3][4] = t17-dvzNoise*t9*(t15-q2*q3*2.0f)-dvyNoise*t8*t18;
A0[3][5] = t25;
A0[4][3] = t17-dvyNoise*t8*t18-dvzNoise*t9*t20;
A0[4][4] = dvxNoise*(t16*t16)+dvyNoise*(t18*t18)+dvzNoise*(t20*t20);
A0[4][5] = t27;
A0[5][3] = t25;
A0[5][4] = t27;
A0[5][5] = dvxNoise*(t23*t23)+dvyNoise*(t21*t21)+dvzNoise*(t22*t22);

34
libraries/AP_NavEKF/Models/GimbalEstimatorExample/calcQ.m Normal file
View File

@ -0,0 +1,34 @@
function Q = calcQ(daxNoise,dayNoise,dazNoise,dvxNoise,dvyNoise,dvzNoise,q0,q1,q2,q3)
%CALCQ
% Q = CALCQ(DAXNOISE,DAYNOISE,DAZNOISE,DVXNOISE,DVYNOISE,DVZNOISE,Q0,Q1,Q2,Q3)
% This function was generated by the Symbolic Math Toolbox version 6.1.
% 15-Feb-2015 15:45:50
t3 = q0.^2;
t4 = q1.^2;
t5 = q2.^2;
t6 = q3.^2;
t2 = t3+t4+t5+t6;
t7 = t2.^2;
t11 = q0.*q3.*2.0;
t12 = q1.*q2.*2.0;
t8 = t11-t12;
t13 = q0.*q2.*2.0;
t14 = q1.*q3.*2.0;
t9 = t13+t14;
t10 = t3+t4-t5-t6;
t15 = q0.*q1.*2.0;
t16 = t11+t12;
t17 = dvxNoise.*t10.*t16;
t18 = t3-t4+t5-t6;
t19 = q2.*q3.*2.0;
t20 = t15-t19;
t21 = t15+t19;
t22 = t3-t4-t5+t6;
t23 = t13-t14;
t24 = dvzNoise.*t9.*t22;
t25 = t24-dvxNoise.*t10.*t23-dvyNoise.*t8.*t21;
t26 = dvyNoise.*t18.*t21;
t27 = t26-dvxNoise.*t16.*t23-dvzNoise.*t20.*t22;
Q = reshape([daxNoise.*t7,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,dayNoise.*t7,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,dazNoise.*t7,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,dvxNoise.*t10.^2+dvyNoise.*t8.^2+dvzNoise.*t9.^2,t17-dvyNoise.*t8.*t18-dvzNoise.*t9.*t20,t25,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,t17-dvzNoise.*t9.*(t15-q2.*q3.*2.0)-dvyNoise.*t8.*t18,dvxNoise.*t16.^2+dvyNoise.*t18.^2+dvzNoise.*t20.^2,t27,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,t25,t27,dvxNoise.*t23.^2+dvyNoise.*t21.^2+dvzNoise.*t22.^2,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0],[12, 12]);

51
libraries/AP_NavEKF/Models/GimbalEstimatorExample/calcSF.c Normal file
View File

@ -0,0 +1,51 @@
t2 = SF(1.0);
t3 = SF(2.0);
t4 = SF(3.0);
t5 = q3*(1.0/2.0);
t6 = q0*t2*(1.0/2.0);
t7 = q2*t4*(1.0/2.0);
t8 = q0*t3*(1.0/2.0);
t9 = q1*t2*(1.0/2.0);
t10 = q3*t4*(1.0/2.0);
t11 = q1*(1.0/2.0);
t12 = q2*t2*(1.0/2.0);
t13 = q3*t3*(1.0/2.0);
t14 = q0*(1.0/2.0);
t15 = q1*t4*(1.0/2.0);
t16 = q2*t3*(1.0/2.0);
t17 = q3*t2*(1.0/2.0);
t18 = q0*t4*(1.0/2.0);
t19 = q2*(1.0/2.0);
t20 = q1*t3*(1.0/2.0);
t21 = q0*q0;
t22 = q1*q1;
t23 = q2*q2;
t24 = q3*q3;
t25 = q0*q1*2.0;
t26 = q0*q3*2.0;
t27 = q0*q2*2.0;
A0[0][0] = daz*(1.0/2.0)-daz_b*(1.0/2.0);
A0[1][0] = day*(1.0/2.0)-day_b*(1.0/2.0);
A0[2][0] = dax*(1.0/2.0)-dax_b*(1.0/2.0);
A0[3][0] = q2*(-1.0/2.0)+t8+t9+t10;
A0[4][0] = t5+t6+t7-q1*t3*(1.0/2.0);
A0[5][0] = t14+t15+t16-q3*t2*(1.0/2.0);
A0[6][0] = t11+t12+t13-q0*t4*(1.0/2.0);
A0[7][0] = t5-t6+t7+t20;
A0[8][0] = t8+t9-t10+t19;
A0[9][0] = -t11+t12+t13+t18;
A0[10][0] = t14+t15-t16+t17;
A0[11][0] = t14-t15+t16+t17;
A0[12][0] = t11-t12+t13+t18;
A0[13][0] = -t8+t9+t10+t19;
A0[14][0] = -t5+t6+t7+t20;
A0[15][0] = -t21-t22-t23-t24;
A0[16][0] = t21-t22-t23+t24;
A0[17][0] = t21-t22+t23-t24;
A0[18][0] = t21+t22-t23-t24;
A0[19][0] = t27-q1*q3*2.0;
A0[20][0] = t25-q2*q3*2.0;
A0[21][0] = t26-q1*q2*2.0;
A0[22][0] = t25+q2*q3*2.0;
A0[23][0] = t26+q1*q2*2.0;
A0[24][0] = t27+q1*q3*2.0;

40
libraries/AP_NavEKF/Models/GimbalEstimatorExample/calcSP.c Normal file
View File

@ -0,0 +1,40 @@
t2 = SF(1.5E1);
t3 = SF(1.4E1);
t4 = SF(1.3E1);
t5 = SF(1.2E1);
t6 = SF(1.1E1);
t7 = SF(1.0E1);
t8 = SF(9.0);
t9 = SF(8.0);
t10 = SF(7.0);
t11 = SF(6.0);
t12 = SF(5.0);
t13 = SF(4.0);
t14 = SF(1.7E1);
t15 = SF(2.3E1);
t16 = SF(2.0E1);
t17 = SF(1.8E1);
t18 = SF(2.1E1);
t19 = SF(2.4E1);
t20 = SF(2.5E1);
t21 = SF(2.2E1);
t22 = SF(1.9E1);
A0[0][0] = q0*t3*-2.0+q1*t2*2.0+q2*t5*2.0+q3*t4*2.0;
A0[1][0] = q0*t7*2.0+q1*t6*2.0+q2*t9*2.0-q3*t8*2.0;
A0[2][0] = q0*t2*2.0+q1*t3*2.0-q2*t4*2.0+q3*t5*2.0;
A0[3][0] = q0*t6*2.0-q1*t7*2.0+q2*t8*2.0+q3*t9*2.0;
A0[4][0] = q0*t5*2.0+q1*t4*2.0+q2*t3*2.0-q3*t2*2.0;
A0[5][0] = q0*t9*-2.0+q1*t8*2.0+q2*t7*2.0+q3*t6*2.0;
A0[6][0] = q0*t11*2.0+q1*t10*2.0-q2*t13*2.0+q3*t12*2.0;
A0[7][0] = q0*t10*-2.0+q1*t11*2.0+q2*t12*2.0+q3*t13*2.0;
A0[8][0] = q0*t13*2.0-q1*t12*2.0+q2*t11*2.0+q3*t10*2.0;
A0[9][0] = dvy*t14-dvz*t15;
A0[10][0] = dvx*t17-dvy*t19;
A0[11][0] = dvx*t20-dvz*t22;
A0[12][0] = dvx*t14+dvz*t16;
A0[13][0] = dvx*t15+dvy*t16;
A0[14][0] = dvy*t18+dvz*t17;
A0[15][0] = dvx*t18+dvz*t19;
A0[16][0] = dvy*t20+dvz*t21;
A0[17][0] = dvx*t21+dvy*t22;
A0[18][0] = SF(1.6E1);

46
libraries/AP_NavEKF/Models/GimbalEstimatorExample/calcTmn.c Normal file
View File

@ -0,0 +1,46 @@
t2 = cos(gPsi);
t3 = sin(gTheta);
t4 = cos(gTheta);
t5 = sin(gPhi);
t6 = sin(gPsi);
t7 = q0*q0;
t8 = q1*q1;
t9 = q2*q2;
t10 = q3*q3;
t11 = t7+t8-t9-t10;
t12 = q0*q2*2.0;
t13 = q1*q3*2.0;
t14 = t12+t13;
t15 = cos(gPhi);
t16 = q0*q3*2.0;
t18 = q1*q2*2.0;
t17 = t16-t18;
t19 = t2*t3;
t20 = t4*t5*t6;
t21 = t19+t20;
t22 = t2*t4;
t34 = t3*t5*t6;
t23 = t22-t34;
t24 = t4*t6;
t25 = t2*t3*t5;
t26 = t24+t25;
t27 = t16+t18;
t28 = t3*t6;
t35 = t2*t4*t5;
t29 = t28-t35;
t30 = q0*q1*2.0;
t33 = q2*q3*2.0;
t31 = t30-t33;
t32 = t7-t8+t9-t10;
t36 = t7-t8-t9+t10;
t37 = t12-t13;
t38 = t30+t33;
A0[0][0] = t11*t23+t14*t21+t6*t15*t17;
A0[0][1] = t11*t26+t14*t29-t2*t15*t17;
A0[0][2] = -t5*t17-t3*t11*t15+t4*t14*t15;
A0[1][0] = t23*t27-t21*t31-t6*t15*t32;
A0[1][1] = t26*t27-t29*t31+t2*t15*t32;
A0[1][2] = t5*t32-t3*t15*t27-t4*t15*t31;
A0[2][0] = t21*t36-t23*t37-t6*t15*t38;
A0[2][1] = -t26*t37+t29*t36+t2*t15*t38;
A0[2][2] = t5*t38+t3*t15*t37+t4*t15*t36;

45
libraries/AP_NavEKF/Models/GimbalEstimatorExample/calcTmn.m Normal file
View File

@ -0,0 +1,45 @@
function Tmn = calcTmn(gPhi,gPsi,gTheta,q0,q1,q2,q3)
%CALCTMN
% TMN = CALCTMN(GPHI,GPSI,GTHETA,Q0,Q1,Q2,Q3)
% This function was generated by the Symbolic Math Toolbox version 6.1.
% 15-Feb-2015 16:02:10
t2 = cos(gPsi);
t3 = sin(gTheta);
t4 = cos(gTheta);
t5 = sin(gPhi);
t6 = sin(gPsi);
t7 = q0.^2;
t8 = q1.^2;
t9 = q2.^2;
t10 = q3.^2;
t11 = t7+t8-t9-t10;
t12 = q0.*q2.*2.0;
t13 = q1.*q3.*2.0;
t14 = t12+t13;
t15 = cos(gPhi);
t16 = q0.*q3.*2.0;
t18 = q1.*q2.*2.0;
t17 = t16-t18;
t19 = t2.*t3;
t20 = t4.*t5.*t6;
t21 = t19+t20;
t22 = t2.*t4;
t34 = t3.*t5.*t6;
t23 = t22-t34;
t24 = t4.*t6;
t25 = t2.*t3.*t5;
t26 = t24+t25;
t27 = t16+t18;
t28 = t3.*t6;
t35 = t2.*t4.*t5;
t29 = t28-t35;
t30 = q0.*q1.*2.0;
t33 = q2.*q3.*2.0;
t31 = t30-t33;
t32 = t7-t8+t9-t10;
t36 = t7-t8-t9+t10;
t37 = t12-t13;
t38 = t30+t33;
Tmn = reshape([t11.*t23+t14.*t21+t6.*t15.*t17,t23.*t27-t21.*t31-t6.*t15.*t32,t21.*t36-t23.*t37-t6.*t15.*t38,t11.*t26+t14.*t29-t2.*t15.*t17,t26.*t27-t29.*t31+t2.*t15.*t32,-t26.*t37+t29.*t36+t2.*t15.*t38,-t5.*t17-t3.*t11.*t15+t4.*t14.*t15,t5.*t32-t3.*t15.*t27-t4.*t15.*t31,t5.*t38+t3.*t15.*t37+t4.*t15.*t36],[3, 3]);

1
libraries/AP_NavEKF/Models/GimbalEstimatorExample/calcTms.c Normal file
View File

@ -0,0 +1 @@
t0 = _symans_32_297;

14
libraries/AP_NavEKF/Models/GimbalEstimatorExample/calcTms.m Normal file
View File

@ -0,0 +1,14 @@
function Tms = calcTms(gPhi,gPsi,gTheta)
%CALCTMS
% TMS = CALCTMS(GPHI,GPSI,GTHETA)
% This function was generated by the Symbolic Math Toolbox version 6.1.
% 15-Feb-2015 16:02:09
t2 = cos(gTheta);
t3 = sin(gPsi);
t4 = cos(gPsi);
t5 = sin(gPhi);
t6 = sin(gTheta);
t7 = cos(gPhi);
Tms = reshape([t2.*t4-t3.*t5.*t6,-t3.*t7,t4.*t6+t2.*t3.*t5,t2.*t3+t4.*t5.*t6,t4.*t7,t3.*t6-t2.*t4.*t5,-t6.*t7,t5,t2.*t7],[3, 3]);

56
libraries/AP_NavEKF/Models/QuaternionMathExample/FuseMagnetometer.m Normal file
View File

@ -0,0 +1,56 @@
function [...
states, ... % state vector after fusion of measurements
P, ... % state covariance matrix after fusion of corrections
innovation, ... % Declination innovation - rad
varInnov] ... %
= FuseMagnetometer( ...
states, ... % predicted states
P, ... % predicted covariance
magData, ... % body frame magnetic flux measurements
measDec, ... % magnetic field declination - azimuth angle measured from true north (rad)
Tbn) % Estimated coordinate transformation matrix from body to NED frame
q0 = states(1);
q1 = states(2);
q2 = states(3);
q3 = states(4);
magX = magData(1);
magY = magData(2);
magZ = magData(3);
R_MAG = 0.05^2;
H = calcH_MAG(magX,magY,magZ,q0,q1,q2,q3);
varInnov = (H*P*transpose(H) + R_MAG);
Kfusion = (P*transpose(H))/varInnov;
% Calculate the predicted magnetic declination
magMeasNED = Tbn*[magX;magY;magZ];
predDec = atan2(magMeasNED(2),magMeasNED(1));
% Calculate the measurement innovation
innovation = predDec - measDec;
% correct the state vector
states = states - Kfusion * innovation;
% re-normalise the quaternion
quatMag = sqrt(states(1)^2 + states(2)^2 + states(3)^2 + states(4)^2);
states(1:4) = states(1:4) / quatMag;
% correct the covariance P = P - K*H*P
P = P - Kfusion*H*P;
% Force symmetry on the covariance matrix to prevent ill-conditioning
% of the matrix which would cause the filter to blow-up
P = 0.5*(P + transpose(P));
% ensure diagonals are positive
for i=1:10
if P(i,i) < 0
P(i,i) = 0;
end
end
end

51
libraries/AP_NavEKF/Models/QuaternionMathExample/FuseVelocity.m Normal file
View File

@ -0,0 +1,51 @@
function [...
states, ... % state vector after fusion of measurements
P, ... % state covariance matrix after fusion of corrections
innovation,... % NED velocity innovations (m/s)
varInnov] ... % NED velocity innovation variance ((m/s)^2)
= FuseVelocity( ...
states, ... % predicted states from the INS
P, ... % predicted covariance
measVel) % NED velocity measurements (m/s)
R_OBS = 0.5^2;
innovation = zeros(1,3);
varInnov = zeros(1,3);
% Fuse measurements sequentially
for obsIndex = 1:3
stateIndex = 4 + obsIndex;
% Calculate the velocity measurement innovation
innovation(obsIndex) = states(stateIndex) - measVel(obsIndex);
% Calculate the Kalman Gain
H = zeros(1,10);
H(1,stateIndex) = 1;
varInnov(obsIndex) = (H*P*transpose(H) + R_OBS);
K = (P*transpose(H))/varInnov(obsIndex);
% Calculate state corrections
xk = K * innovation(obsIndex);
% Apply the state corrections
states = states - xk;
% re-normalise the quaternion
quatMag = sqrt(states(1)^2 + states(2)^2 + states(3)^2 + states(4)^2);
states(1:4) = states(1:4) / quatMag;
% Update the covariance
P = P - K*H*P;
% Force symmetry on the covariance matrix to prevent ill-conditioning
P = 0.5*(P + transpose(P));
% ensure diagonals are positive
for i=1:10
if P(i,i) < 0
P(i,i) = 0;
end
end
end
end

113
libraries/AP_NavEKF/Models/QuaternionMathExample/GenerateEquations.m Normal file
View File

@ -0,0 +1,113 @@
% IMPORTANT - This script requires the Matlab symbolic toolbox
% Author: Paul Riseborough
% Last Modified: 16 Feb 2014
% Derivation of a navigation EKF using a local NED earth Tangent Frame for
% the initial alignment and gyro bias estimation from a moving platform
% Uses quaternions for attitude propagation
% State vector:
% quaternions
% Velocity - North, East, Down (m/s)
% Delta Angle bias - X,Y,Z (rad)
% Observations:
% NED velocity - N,E,D (m/s)
% body fixed magnetic field vector - X,Y,Z
% Time varying parameters:
% XYZ delta angle measurements in body axes - rad
% XYZ delta velocity measurements in body axes - m/sec
% magnetic declination
clear all;
%% define symbolic variables and constants
syms dax day daz real % IMU delta angle measurements in body axes - rad
syms dvx dvy dvz real % IMU delta velocity measurements in body axes - m/sec
syms q0 q1 q2 q3 real % quaternions defining attitude of body 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 body fixed 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
%% 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 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 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;
% Define the truth delta velocity
dVelTruth = dVelMeas;
% 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);
];
quatNew = QuatMult(quat,deltaQuat);
% define the velocity update equations
% ignore coriolis terms for linearisation purposes
vNew = [vn;ve;vd] + [0;0;gravity]*dt + Tbn*dVelTruth;
% define the IMU bias error update equations
dabNew = [dax_b; day_b; daz_b];
% Define the state vector & number of states
stateVector = [quat;vn;ve;vd;dAngBias];
nStates=numel(stateVector);
%% derive the covariance prediction equation
% This reduces the number of floating point operations by a factor of 6 or
% more compared to using the standard matrix operations in code
% Define the control (disturbance) vector. Use the measured delta angles
% and velocities (not truth) to simplify the derivation
distVector = [dAngMeas;dVelMeas];
% derive the control(disturbance) influence matrix
G = jacobian([quatNew;vNew;dabNew], distVector);
f = matlabFunction(G,'file','calcG.m');
% derive the state error matrix
imuNoise = diag([daxNoise dayNoise dazNoise dvxNoise dvyNoise dvzNoise]);
Q = G*imuNoise*transpose(G);
f = matlabFunction(Q,'file','calcQ.m');
% derive the state transition matrix
F = jacobian([quatNew;vNew;dabNew], stateVector);
f = matlabFunction(F,'file','calcF.m');
%% derive equations for fusion of magnetic deviation measurement
% rotate body measured 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 = tan(magMeasNED(2)/magMeasNED(1));
H_MAG = jacobian(angMeas,stateVector); % measurement Jacobian
f = matlabFunction(H_MAG,'file','calcH_MAG.m');

21
libraries/AP_NavEKF/Models/QuaternionMathExample/PlotData.m Normal file
View File

@ -0,0 +1,21 @@
%% plot gyro bias estimates
figure;
plot(statesLog(1,:)*0.001,statesLog(9:11,:)/dt*180/pi);
grid on;
ylabel('Gyro Bias Estimate (deg/sec)');
xlabel('time (sec)');
%% plot Euler angle estimates
figure;
eulLog(4,:) = eulLog(4,:) + pi;
plot(eulLog(1,:)*0.001,eulLog(2:4,:)*180/pi);
grid on;
ylabel('Euler Angle Estimates (deg)');
xlabel('time (sec)');
%% plot velocity innovations
figure;
plot(statesLog(1,:)*0.001,statesLog(6:8,:));
grid on;
ylabel('EKF velocity Innovations (m/s)');
xlabel('time (sec)');

60
libraries/AP_NavEKF/Models/QuaternionMathExample/PredictCovariance.m Normal file
View File

@ -0,0 +1,60 @@
function P = PredictCovariance(deltaAngle, ...
deltaVelocity, ...
quat, ...
states,...
P, ... % Previous state covariance matrix
dt) ... % IMU and prediction time step
% Set the filter state process noise (IMU errors are already built into the
% equations). It includes the process noise required for evolution of the
% IMU bias errors
dAngBiasSigma = dt*1E-6;
processNoise = [0*ones(1,7), dAngBiasSigma*[1 1 1]];
% Specify the estimated errors on the delta angles and delta velocities
daxNoise = (dt*25.0/60*pi/180)^2;
dayNoise = (dt*25.0/60*pi/180)^2;
dazNoise = (dt*25.0/60*pi/180)^2;
dvxNoise = (dt*0.5)^2;
dvyNoise = (dt*0.5)^2;
dvzNoise = (dt*0.5)^2;
dvx = deltaVelocity(1);
dvy = deltaVelocity(2);
dvz = deltaVelocity(3);
dax = deltaAngle(1);
day = deltaAngle(2);
daz = deltaAngle(3);
q0 = quat(1);
q1 = quat(2);
q2 = quat(3);
q3 = quat(4);
dax_b = states(8);
day_b = states(9);
daz_b = states(10);
% Predicted covariance
F = calcF(dax,dax_b,day,day_b,daz,daz_b,dvx,dvy,dvz,q0,q1,q2,q3);
Q = calcQ(daxNoise,dayNoise,dazNoise,dvxNoise,dvyNoise,dvzNoise,q0,q1,q2,q3);
P = F*P*transpose(F) + Q;
% Add the general process noise
for i = 1:10
P(i,i) = P(i,i) + processNoise(i)^2;
end
% Force symmetry on the covariance matrix to prevent ill-conditioning
% of the matrix which would cause the filter to blow-up
P = 0.5*(P + transpose(P));
% ensure diagonals are positive
for i=1:9
if P(i,i) < 0
P(i,i) = 0;
end
end
end

84
libraries/AP_NavEKF/Models/QuaternionMathExample/PredictStates.m Normal file
View File

@ -0,0 +1,84 @@
function [nextQuat, nextStates, Tbn, correctedDelAng, correctedDelVel] = PredictStates( ...
quat, ... % previous quaternion states 4x1
states, ... % previous states (3x1 rotation error, 3x1 velocity, 3x1 gyro bias)
angRate, ... % IMU rate gyro measurements, 3x1 (rad/sec)
accel, ... % IMU accelerometer measurements 3x1 (m/s/s)
dt) % time since last IMU measurement (sec)
% Define parameters used for previous angular rates and acceleration shwich
% are used for trapezoidal integration
persistent prevAngRate;
if isempty(prevAngRate)
prevAngRate = angRate;
end
persistent prevAccel;
if isempty(prevAccel)
prevAccel = accel;
end
% define persistent variables for previous delta angle and velocity which
% are required for sculling and coning error corrections
persistent prevDelAng;
if isempty(prevDelAng)
prevDelAng = single([0;0;0]);
end
persistent prevDelVel;
if isempty(prevDelVel)
prevDelVel = single([0;0;0]);
end
% Convert IMU data to delta angles and velocities using trapezoidal
% integration
dAng = 0.5*(angRate + prevAngRate)*dt;
dVel = 0.5*(accel + prevAccel )*dt;
prevAngRate = angRate;
prevAccel = accel;
% Remove sensor bias errors
dAng = dAng - states(7:9);
% Apply rotational and skulling corrections
correctedDelVel= dVel + ...
0.5*cross(prevDelAng + dAng , prevDelVel + dVel) + 1/6*cross(prevDelAng + dAng , cross(prevDelAng + dAng , prevDelVel + dVel)) + ... % rotational correction
1/12*(cross(prevDelAng , dVel) + cross(prevDelVel , dAng)); % sculling correction
% Apply corrections for coning errors
correctedDelAng = dAng - 1/12*cross(prevDelAng , dAng);
% Save current measurements
prevDelAng = dAng;
prevDelVel = dVel;
% Initialise the updated state vector
nextQuat = quat;
nextStates = states;
% Convert the rotation vector to its equivalent quaternion
rotationMag = sqrt(correctedDelAng(1)^2 + correctedDelAng(2)^2 + correctedDelAng(3)^2);
if rotationMag<1e-12
deltaQuat = single([1;0;0;0]);
else
deltaQuat = [cos(0.5*rotationMag); correctedDelAng/rotationMag*sin(0.5*rotationMag)];
end
% Update the quaternions by rotating from the previous attitude through
% the delta angle rotation quaternion
qUpdated = [quat(1)*deltaQuat(1)-transpose(quat(2:4))*deltaQuat(2:4); quat(1)*deltaQuat(2:4) + deltaQuat(1)*quat(2:4) + cross(quat(2:4),deltaQuat(2:4))];
% Normalise the quaternions and update the quaternion states
quatMag = sqrt(qUpdated(1)^2 + qUpdated(2)^2 + qUpdated(3)^2 + qUpdated(4)^2);
if (quatMag < 1e-16)
nextQuat(1:4) = qUpdated;
else
nextQuat(1:4) = qUpdated / quatMag;
end
% Calculate the body to nav cosine matrix
Tbn = Quat2Tbn(nextQuat);
% transform body delta velocities to delta velocities in the nav frame
delVelNav = Tbn * correctedDelVel + [0;0;9.807]*dt;
% Sum delta velocities to get velocity
nextStates(4:6) = states(4:6) + delVelNav(1:3);
end

71
libraries/AP_NavEKF/Models/QuaternionMathExample/RunRealData.m Normal file
View File

@ -0,0 +1,71 @@
%% Set initial conditions
clear all;
load('fltTest.mat');
startDelayTime = 100; % number of seconds to delay filter start (used to simulate in-flight restart)
dt = 1/50;
startTime = 0.001*(IMU(1,2));
stopTime = 0.001*(IMU(length(IMU),2));
indexLimit = length(IMU);
magIndexlimit = length(MAG);
statesLog = zeros(11,indexLimit);
eulLog = zeros(4,indexLimit);
velInnovLog = zeros(4,indexLimit);
angErrLog = velInnovLog;
decInnovLog = zeros(2,magIndexlimit);
velInnovVarLog = velInnovLog;
decInnovVarLog = decInnovLog;
% initialise the filter to level
quat = [1;0;0;0];
states = zeros(10,1);
Tbn = Quat2Tbn(quat);
% Set the expected declination
measDec = 0.18;
% define the state covariances with the exception of the quaternion covariances
Sigma_velNED = 0.5; % 1 sigma uncertainty in horizontal velocity components
Sigma_dAngBias = 5*pi/180*dt; % 1 Sigma uncertainty in delta angle bias
Sigma_quatErr = 1; % 1 Sigma uncertainty in angular misalignment (rad)
covariance = single(diag([Sigma_quatErr*[1;1;1;1];Sigma_velNED*[1;1;1];Sigma_dAngBias*[1;1;1]].^2));
%% Main Loop
magIndex = 1;
time = 0;
tiltError = 0;
headingAligned = 0;
angErrVec = [0;0;0];
startIndex = max(11,ceil(startDelayTime/dt));
for index = startIndex:indexLimit
time=time+dt + startIndex*dt;
% read IMU measurements and correct rates using estimated bias
angRate = IMU(index,3:5)' - states(7:9)./dt;
accel = IMU(index,6:8)';
% predict states
[quat, states, Tbn, delAng, delVel] = PredictStates(quat,states,angRate,accel,dt);
statesLog(1,index) = time;
statesLog(2:11,index) = states;
eulLog(1,index) = time;
eulLog(2:4,index) = QuatToEul(quat);
% predict covariance matrix
covariance = PredictCovariance(delAng,delVel,quat,states,covariance,dt);
% read magnetometer measurements
while ((MAG(magIndex,1) < IMU(index,1)) && (magIndex < magIndexlimit))
magIndex = magIndex + 1;
% fuse magnetometer measurements if new data available and when tilt has settled
if ((MAG(magIndex,1) >= IMU(index,1)) && ((angErrVec(1)^2 + angErrVec(2)^2) < 0.05^2) && (index > 50))
magBody = 0.001*MAG(magIndex,3:5)';
[states,covariance,decInnov,decInnovVar] = FuseMagnetometer(states,covariance,magBody,measDec,Tbn);
decInnovLog(1,magIndex) = time;
decInnovLog(2,magIndex) = decInnov;
decInnovVarLog(1,magIndex) = time;
decInnovVarLog(2,magIndex) = decInnovVar;
end
end
% fuse velocity measurements - use synthetic measurements
measVel = [0;0;0];
[states,covariance,velInnov,velInnovVar] = FuseVelocity(states,covariance,measVel);
velInnovLog(1,index) = time;
velInnovLog(2:4,index) = velInnov;
velInnovVarLog(1,index) = time;
velInnovVarLog(2:4,index) = velInnovVar;
end
%% Generate Plots
PlotData;

36
libraries/AP_NavEKF/Models/QuaternionMathExample/calcF.m Normal file
View File

@ -0,0 +1,36 @@
function F = calcF(dax,dax_b,day,day_b,daz,daz_b,dvx,dvy,dvz,q0,q1,q2,q3)
%CALCF
% F = CALCF(DAX,DAX_B,DAY,DAY_B,DAZ,DAZ_B,DVX,DVY,DVZ,Q0,Q1,Q2,Q3)
% This function was generated by the Symbolic Math Toolbox version 5.8.
% 14-Jan-2015 11:09:08
t2 = dax_b.*(1.0./2.0);
t3 = daz_b.*(1.0./2.0);
t4 = day_b.*(1.0./2.0);
t8 = day.*(1.0./2.0);
t5 = t4-t8;
t6 = q3.*(1.0./2.0);
t7 = q2.*(1.0./2.0);
t9 = daz.*(1.0./2.0);
t10 = dax.*(1.0./2.0);
t11 = -t2+t10;
t12 = q1.*(1.0./2.0);
t13 = -t3+t9;
t14 = -t4+t8;
t15 = dvz.*q1.*2.0;
t16 = dvy.*q1.*2.0;
t17 = dvz.*q0.*2.0;
t18 = dvx.*q1.*2.0;
t19 = dvy.*q2.*2.0;
t20 = dvz.*q3.*2.0;
t21 = t18+t19+t20;
t22 = dvx.*q0.*2.0;
t23 = dvz.*q2.*2.0;
t29 = dvy.*q3.*2.0;
t24 = t22+t23-t29;
t25 = dvx.*q2.*2.0;
t26 = dvx.*q3.*2.0;
t27 = dvy.*q0.*2.0;
t28 = -t15+t26+t27;
F = reshape([1.0,t11,t14,t13,t24,t28,t16+t17-t25,0.0,0.0,0.0,dax.*(-1.0./2.0)+t2,1.0,t3-t9,t14,t21,-t16-t17+t25,t28,0.0,0.0,0.0,t5,t13,1.0,t2-t10,t16+t17-dvx.*q2.*2.0,t21,-t22-t23+t29,0.0,0.0,0.0,daz.*(-1.0./2.0)+t3,t5,t11,1.0,t15-dvx.*q3.*2.0-dvy.*q0.*2.0,t24,t21,0.0,0.0,0.0,0.0,0.0,0.0,0.0,1.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,1.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,1.0,0.0,0.0,0.0,t12,q0.*(-1.0./2.0),-t6,t7,0.0,0.0,0.0,1.0,0.0,0.0,t7,t6,q0.*(-1.0./2.0),-t12,0.0,0.0,0.0,0.0,1.0,0.0,t6,-t7,t12,q0.*(-1.0./2.0),0.0,0.0,0.0,0.0,0.0,1.0],[10, 10]);

18
libraries/AP_NavEKF/Models/QuaternionMathExample/calcG.m Normal file
View File

@ -0,0 +1,18 @@
function G = calcG(q0,q1,q2,q3)
%CALCG
% G = CALCG(Q0,Q1,Q2,Q3)
% This function was generated by the Symbolic Math Toolbox version 5.8.
% 14-Jan-2015 11:09:06
t2 = q0.*(1.0./2.0);
t3 = q2.*(1.0./2.0);
t4 = q1.*q2.*2.0;
t5 = q0.^2;
t6 = q1.^2;
t7 = q2.^2;
t8 = q3.^2;
t9 = q0.*q2.*2.0;
t10 = q1.*q3.*2.0;
t11 = q2.*q3.*2.0;
G = reshape([q1.*(-1.0./2.0),t2,q3.*(1.0./2.0),-t3,0.0,0.0,0.0,0.0,0.0,0.0,q2.*(-1.0./2.0),q3.*(-1.0./2.0),t2,q1.*(1.0./2.0),0.0,0.0,0.0,0.0,0.0,0.0,q3.*(-1.0./2.0),t3,q1.*(-1.0./2.0),t2,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,t5+t6-t7-t8,t4+q0.*q3.*2.0,-t9+t10,0.0,0.0,0.0,0.0,0.0,0.0,0.0,t4-q0.*q3.*2.0,t5-t6+t7-t8,t11+q0.*q1.*2.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,t9+t10,t11-q0.*q1.*2.0,t5-t6-t7+t8,0.0,0.0,0.0],[10, 6]);

52
libraries/AP_NavEKF/Models/QuaternionMathExample/calcH_MAG.m Normal file
View File

@ -0,0 +1,52 @@
function H_MAG = calcH_MAG(magX,magY,magZ,q0,q1,q2,q3)
%CALCH_MAG
% H_MAG = CALCH_MAG(MAGX,MAGY,MAGZ,Q0,Q1,Q2,Q3)
% This function was generated by the Symbolic Math Toolbox version 5.8.
% 14-Jan-2015 11:09:09
t2 = q0.^2;
t3 = q1.^2;
t4 = q2.^2;
t5 = q3.^2;
t6 = q0.*q3.*2.0;
t8 = t2+t3-t4-t5;
t9 = magX.*t8;
t10 = q1.*q2.*2.0;
t11 = q0.*q2.*2.0;
t12 = q1.*q3.*2.0;
t13 = t11+t12;
t14 = magZ.*t13;
t17 = t2-t3+t4-t5;
t18 = magY.*t17;
t19 = t6+t10;
t20 = magX.*t19;
t21 = q0.*q1.*2.0;
t22 = q2.*q3.*2.0;
t23 = t21-t22;
t24 = magZ.*t23;
t25 = t18+t20-t24;
t7 = tan(t25./(t9+t14-magY.*(t6-q1.*q2.*2.0)));
t15 = t6-t10;
t26 = magY.*t15;
t16 = t9+t14-t26;
t27 = 1.0./t16;
t30 = t25.*t27;
t28 = tan(t30);
t29 = 1.0./t16.^2;
t31 = t28.^2;
t32 = t31+1.0;
t33 = magX.*q1.*2.0;
t34 = magY.*q2.*2.0;
t35 = magZ.*q3.*2.0;
t36 = t33+t34+t35;
t37 = magY.*q1.*2.0;
t38 = magZ.*q0.*2.0;
t39 = t37+t38-magX.*q2.*2.0;
t40 = magX.*q0.*2.0;
t41 = magZ.*q2.*2.0;
t42 = t40+t41-magY.*q3.*2.0;
t43 = magY.*q0.*2.0;
t44 = magX.*q3.*2.0;
t45 = t43+t44-magZ.*q1.*2.0;
H_MAG = [(t7.^2+1.0).*(t27.*t45-t25.*t29.*t42),-t32.*(t27.*t39+t25.*t29.*t36),t32.*(t27.*t36-t25.*t29.*t39),t32.*(t27.*t42+t25.*t29.*t45),0.0,0.0,0.0,0.0,0.0,0.0];

45
libraries/AP_NavEKF/Models/QuaternionMathExample/calcQ.m Normal file
View File

@ -0,0 +1,45 @@
function Q = calcQ(daxNoise,dayNoise,dazNoise,dvxNoise,dvyNoise,dvzNoise,q0,q1,q2,q3)
%CALCQ
% Q = CALCQ(DAXNOISE,DAYNOISE,DAZNOISE,DVXNOISE,DVYNOISE,DVZNOISE,Q0,Q1,Q2,Q3)
% This function was generated by the Symbolic Math Toolbox version 5.8.
% 14-Jan-2015 11:09:07
t2 = dayNoise.*q2.*q3.*(1.0./4.0);
t3 = t2-daxNoise.*q0.*q1.*(1.0./4.0)-dazNoise.*q2.*q3.*(1.0./4.0);
t4 = q3.^2;
t5 = q2.^2;
t6 = dazNoise.*q1.*q3.*(1.0./4.0);
t7 = t6-daxNoise.*q1.*q3.*(1.0./4.0)-dayNoise.*q0.*q2.*(1.0./4.0);
t8 = daxNoise.*q0.*q3.*(1.0./4.0);
t9 = t8-dayNoise.*q0.*q3.*(1.0./4.0)-dazNoise.*q1.*q2.*(1.0./4.0);
t10 = q0.^2;
t11 = q1.^2;
t12 = daxNoise.*q1.*q2.*(1.0./4.0);
t13 = t12-dayNoise.*q1.*q2.*(1.0./4.0)-dazNoise.*q0.*q3.*(1.0./4.0);
t14 = dazNoise.*q0.*q2.*(1.0./4.0);
t15 = t14-daxNoise.*q0.*q2.*(1.0./4.0)-dayNoise.*q1.*q3.*(1.0./4.0);
t16 = dayNoise.*q0.*q1.*(1.0./4.0);
t17 = t16-daxNoise.*q2.*q3.*(1.0./4.0)-dazNoise.*q0.*q1.*(1.0./4.0);
t21 = q0.*q3.*2.0;
t22 = q1.*q2.*2.0;
t18 = t21-t22;
t23 = q0.*q2.*2.0;
t24 = q1.*q3.*2.0;
t19 = t23+t24;
t20 = t4+t5-t10-t11;
t25 = q0.*q1.*2.0;
t26 = t21+t22;
t32 = t4-t5-t10+t11;
t27 = dvyNoise.*t18.*t32;
t28 = q2.*q3.*2.0;
t29 = t25-t28;
t30 = t4-t5-t10+t11;
t31 = t25+t28;
t33 = t4-t5+t10-t11;
t34 = t23-t24;
t35 = dvxNoise.*t34.*(t4+t5-t10-t11);
t36 = dvzNoise.*t19.*t33;
t37 = t35+t36-dvyNoise.*t18.*t31;
t38 = -dvxNoise.*t26.*t34-dvyNoise.*t31.*t32-dvzNoise.*t29.*t33;
Q = reshape([daxNoise.*t11.*(1.0./4.0)+dayNoise.*t5.*(1.0./4.0)+dazNoise.*t4.*(1.0./4.0),t3,t7,t13,0.0,0.0,0.0,0.0,0.0,0.0,t3,daxNoise.*t10.*(1.0./4.0)+dayNoise.*t4.*(1.0./4.0)+dazNoise.*t5.*(1.0./4.0),t9,t15,0.0,0.0,0.0,0.0,0.0,0.0,t7,t9,daxNoise.*t4.*(1.0./4.0)+dayNoise.*t10.*(1.0./4.0)+dazNoise.*t11.*(1.0./4.0),t17,0.0,0.0,0.0,0.0,0.0,0.0,t13,t15,t17,daxNoise.*t5.*(1.0./4.0)+dayNoise.*t11.*(1.0./4.0)+dazNoise.*t10.*(1.0./4.0),0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,dvxNoise.*t20.^2+dvyNoise.*t18.^2+dvzNoise.*t19.^2,t27-dvxNoise.*t20.*t26-dvzNoise.*t19.*t29,t37,0.0,0.0,0.0,0.0,0.0,0.0,0.0,t27-dvzNoise.*t19.*(t25-q2.*q3.*2.0)-dvxNoise.*t20.*t26,dvxNoise.*t26.^2+dvyNoise.*t30.^2+dvzNoise.*t29.^2,t38,0.0,0.0,0.0,0.0,0.0,0.0,0.0,t37,t38,dvxNoise.*t34.^2+dvyNoise.*t31.^2+dvzNoise.*t33.^2,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0],[10, 10]);

7
libraries/AP_NavEKF/Models/ReadMe.txt Normal file
View File

@ -0,0 +1,7 @@
Matlab derivations and simulations relating to development of the small EKF containing the following material:
testData: bench and flight test data used by the various estimator test harnesses
QuaternionMathExample: Small EKF using the same quaternion attitude representation as the main EKF and with a in-flight alignment test using real data. The inability to perform and in-flight alignment demonstrated by this example shows why we have change to a different attitude representation for the samll EKF.
AttErrVecExample: Small EKF using a new error vector representation for the vehicle attitude and subjected to in-flight alignment tests using both simulated and real data. The robust alignment achieved shows why this new math has been selected for use in the small EKF.
GimbalEstimatorExample: A simulation of 3 axis stabilised gimbal estimator showing application of the small EKF combined with a high rate predictor running on the gimbal that is corrected using the EKF attitude estimates.
Common: Common Matlab functions

BIN
libraries/AP_NavEKF/Models/testData/fltTest.mat Normal file
View File

Binary file not shown.

BIN
libraries/AP_NavEKF/Models/testData/gndTest.mat Normal file
View File

Binary file not shown.