mirror of
https://gitee.com/mirrors_PX4/PX4-Autopilot.git
synced 2026-05-15 12:27:34 +08:00
Add derivation script files for inertial nav EKF
This commit is contained in:
Paul Riseborough
@@ -0,0 +1,213 @@
|
||||
function ConvertToC(nStates)
|
||||
%% Define file to read in
|
||||
fileName = strcat('M_code',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;
|
||||
|
||||
%% 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 ^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('C_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;
|
||||
@@ -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
|
||||
@@ -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;
|
||||
|
||||
|
||||
@@ -0,0 +1,349 @@
|
||||
% IMPORTANT - This script requires the Matlab symbolic toolbox and takes ~3 hours to run
|
||||
|
||||
% Derivation of Navigation EKF using a local NED earth Tangent Frame and
|
||||
% XYZ body fixed frame
|
||||
% Sequential fusion of velocity and position measurements
|
||||
% Fusion of true airspeed
|
||||
% Sequential fusion of magnetic flux measurements
|
||||
% 24 state architecture.
|
||||
% IMU data is assumed to arrive at a constant rate with a time step of dt
|
||||
% IMU delta angle and velocity data are used as time varying parameters,
|
||||
% not observations
|
||||
|
||||
% Author: Paul Riseborough
|
||||
|
||||
% 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.
|
||||
|
||||
% State vector:
|
||||
% error rotation vector in body frame (X,Y,Z)
|
||||
% Velocity - m/sec (North, East, Down)
|
||||
% Position - m (North, East, Down)
|
||||
% Delta Angle bias - rad (X,Y,Z)
|
||||
% Delta Angle scale factor (X,Y,Z)
|
||||
% Delta Velocity bias - m/s (Z)
|
||||
% Earth Magnetic Field Vector - (North, East, Down)
|
||||
% Body Magnetic Field Vector - (X,Y,Z)
|
||||
% Wind Vector - m/sec (North,East)
|
||||
|
||||
% Observations:
|
||||
% NED velocity - m/s
|
||||
% NED position - m
|
||||
% True airspeed - m/s
|
||||
% angle of sideslip - rad
|
||||
% XYZ magnetic flux
|
||||
|
||||
% Time varying parameters:
|
||||
% XYZ delta angle measurements in body axes - rad
|
||||
% XYZ delta velocity measurements in body axes - m/sec
|
||||
|
||||
|
||||
%% define symbolic variables and constants
|
||||
clear all;
|
||||
reset(symengine);
|
||||
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 pn pe pd real % NED position - m
|
||||
syms dax_b day_b daz_b real % delta angle bias - rad
|
||||
syms dax_s day_s daz_s real % delta angle scale factor
|
||||
syms 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_PN R_PE R_PD real % variances for NED position measurements - m^2
|
||||
syms R_TAS real % variance for true airspeed measurement - (m/sec)^2
|
||||
syms R_MAG real % variance for magnetic flux measurements - milligauss^2
|
||||
syms R_BETA real % variance of sidelsip measurements rad^2
|
||||
syms R_LOS real % variance of LOS angular rate mesurements (rad/sec)^2
|
||||
syms ptd real % location of terrain in D axis
|
||||
syms rotErrX rotErrY rotErrZ real; % error rotation vector in body frame
|
||||
syms decl real; % earth magnetic field declination from true north
|
||||
syms R_MAGS real; % variance for magnetic deviation measurement
|
||||
syms R_DECL real; % variance of supplied declination
|
||||
syms BCXinv BCYinv real % inverse of ballistic coefficient for wind relative movement along the x and y body axes
|
||||
syms rho real % air density (kg/m^3)
|
||||
syms R_ACC real % variance of accelerometer measurements (m/s^2)^2
|
||||
syms Kacc real % ratio of horizontal acceleration to top speed for a multirotor
|
||||
|
||||
%% define the process equations
|
||||
|
||||
% define the measured Delta angle and delta velocity vectors
|
||||
dAngMeas = [dax; day; daz];
|
||||
dVelMeas = [dvx; dvy; dvz];
|
||||
|
||||
% define the IMU bias errors and scale factor
|
||||
dAngBias = [dax_b; day_b; daz_b];
|
||||
dAngScale = [dax_s; day_s; daz_s];
|
||||
dVelBias = [0;0;dvz_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 = [rotErrX;rotErrY;rotErrZ];
|
||||
|
||||
% 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 compensation as these effects are negligible in terms of
|
||||
% covariance growth for our application and grade of sensor
|
||||
dAngTruth = dAngMeas.*dAngScale - dAngBias - [daxNoise;dayNoise;dazNoise];
|
||||
|
||||
% 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 truth delta velocity -ignore sculling and transport rate
|
||||
% corrections as these negligible are in terms of covariance growth for our
|
||||
% application and grade of sensor
|
||||
dVelTruth = dVelMeas - dVelBias - [dvxNoise;dvyNoise;dvzNoise];
|
||||
|
||||
% define the velocity update equations
|
||||
% ignore coriolis terms for linearisation purposes
|
||||
vNew = [vn;ve;vd] + [0;0;gravity]*dt + Tbn*dVelTruth;
|
||||
|
||||
% define the position update equations
|
||||
pNew = [pn;pe;pd] + [vn;ve;vd]*dt;
|
||||
|
||||
% define the IMU error update equations
|
||||
dabNew = [dax_b; day_b; daz_b];
|
||||
dasNew = [dax_s; day_s; daz_s];
|
||||
dvbNew = dvz_b;
|
||||
|
||||
% define the wind velocity update equations
|
||||
vwnNew = vwn;
|
||||
vweNew = vwe;
|
||||
|
||||
% define the earth magnetic field update equations
|
||||
magNnew = magN;
|
||||
magEnew = magE;
|
||||
magDnew = magD;
|
||||
|
||||
% define the body magnetic field update equations
|
||||
magXnew = magX;
|
||||
magYnew = magY;
|
||||
magZnew = magZ;
|
||||
|
||||
% Define the state vector & number of states
|
||||
stateVector = [errRotVec;vn;ve;vd;pn;pe;pd;dax_b;day_b;daz_b;dax_s;day_s;daz_s;dvz_b;magN;magE;magD;magX;magY;magZ;vwn;vwe];
|
||||
nStates=numel(stateVector);
|
||||
|
||||
% Define vector of process equations
|
||||
newStateVector = [errRotNew;vNew;pNew;dabNew;dasNew;dvbNew;magNnew;magEnew;magDnew;magXnew;magYnew;magZnew;vwnNew;vweNew];
|
||||
|
||||
%% 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(newStateVector, distVector);
|
||||
G = subs(G, {'rotErrX', 'rotErrY', 'rotErrZ'}, {0,0,0});
|
||||
[G,SG]=OptimiseAlgebra(G,'SG');
|
||||
|
||||
% derive the state error matrix
|
||||
distMatrix = diag(distVector);
|
||||
Q = G*distMatrix*transpose(G);
|
||||
[Q,SQ]=OptimiseAlgebra(Q,'SQ');
|
||||
|
||||
% remove the disturbance noise from the process equations as it is only
|
||||
% needed when calculating the disturbance influence matrix
|
||||
vNew = subs(vNew,{'daxNoise','dayNoise','dazNoise','dvxNoise','dvyNoise','dvzNoise'}, {0,0,0,0,0,0},0);
|
||||
errRotNew = subs(errRotNew,{'daxNoise','dayNoise','dazNoise','dvxNoise','dvyNoise','dvzNoise'}, {0,0,0,0,0,0},0);
|
||||
|
||||
% derive the state transition matrix
|
||||
F = jacobian(newStateVector, stateVector);
|
||||
% set the rotation error states to zero
|
||||
F = subs(F, {'rotErrX', 'rotErrY', 'rotErrZ'}, {0,0,0});
|
||||
[F,SF]=OptimiseAlgebra(F,'SF');
|
||||
|
||||
% define a symbolic covariance matrix using strings to represent
|
||||
% '_l_' to represent '( '
|
||||
% '_c_' to represent ,
|
||||
% '_r_' to represent ')'
|
||||
% these can be substituted later to create executable code
|
||||
for rowIndex = 1:nStates
|
||||
for colIndex = 1:nStates
|
||||
eval(['syms OP_l_',num2str(rowIndex),'_c_',num2str(colIndex), '_r_ real']);
|
||||
eval(['P(',num2str(rowIndex),',',num2str(colIndex), ') = OP_l_',num2str(rowIndex),'_c_',num2str(colIndex),'_r_;']);
|
||||
end
|
||||
end
|
||||
|
||||
% Derive the predicted covariance matrix using the standard equation
|
||||
PP = F*P*transpose(F) + Q;
|
||||
|
||||
% Collect common expressions to optimise processing
|
||||
[PP,SPP]=OptimiseAlgebra(PP,'SPP');
|
||||
|
||||
%% derive equations for fusion of true airspeed measurements
|
||||
VtasPred = sqrt((vn-vwn)^2 + (ve-vwe)^2 + vd^2); % predicted measurement
|
||||
H_TAS = jacobian(VtasPred,stateVector); % measurement Jacobian
|
||||
H_TAS = subs(H_TAS, {'rotErrX', 'rotErrY', 'rotErrZ'}, {0,0,0});
|
||||
[H_TAS,SH_TAS]=OptimiseAlgebra(H_TAS,'SH_TAS'); % optimise processing
|
||||
K_TAS = (P*transpose(H_TAS))/(H_TAS*P*transpose(H_TAS) + R_TAS);[K_TAS,SK_TAS]=OptimiseAlgebra(K_TAS,'SK_TAS'); % Kalman gain vector
|
||||
|
||||
%% derive equations for fusion of angle of sideslip measurements
|
||||
% calculate wind relative velocities in nav frame and rotate into body frame
|
||||
Vbw = Tbn'*[(vn-vwn);(ve-vwe);vd];
|
||||
% calculate predicted angle of sideslip using small angle assumption
|
||||
BetaPred = Vbw(2)/Vbw(1);
|
||||
H_BETA = jacobian(BetaPred,stateVector); % measurement Jacobian
|
||||
H_BETA = subs(H_BETA, {'rotErrX', 'rotErrY', 'rotErrZ'}, {0,0,0});
|
||||
[H_BETA,SH_BETA]=OptimiseAlgebra(H_BETA,'SH_BETA'); % optimise processing
|
||||
K_BETA = (P*transpose(H_BETA))/(H_BETA*P*transpose(H_BETA) + R_BETA);[K_BETA,SK_BETA]=OptimiseAlgebra(K_BETA,'SK_BETA'); % Kalman gain vector
|
||||
|
||||
%% derive equations for fusion of magnetic field measurement
|
||||
magMeas = transpose(Tbn)*[magN;magE;magD] + [magX;magY;magZ]; % predicted measurement
|
||||
H_MAG = jacobian(magMeas,stateVector); % measurement Jacobian
|
||||
H_MAG = subs(H_MAG, {'rotErrX', 'rotErrY', 'rotErrZ'}, {0,0,0});
|
||||
[H_MAG,SH_MAG]=OptimiseAlgebra(H_MAG,'SH_MAG');
|
||||
|
||||
K_MX = (P*transpose(H_MAG(1,:)))/(H_MAG(1,:)*P*transpose(H_MAG(1,:)) + R_MAG); % Kalman gain vector
|
||||
[K_MX,SK_MX]=OptimiseAlgebra(K_MX,'SK_MX');
|
||||
K_MY = (P*transpose(H_MAG(2,:)))/(H_MAG(2,:)*P*transpose(H_MAG(2,:)) + R_MAG); % Kalman gain vector
|
||||
[K_MY,SK_MY]=OptimiseAlgebra(K_MY,'SK_MY');
|
||||
K_MZ = (P*transpose(H_MAG(3,:)))/(H_MAG(3,:)*P*transpose(H_MAG(3,:)) + R_MAG); % Kalman gain vector
|
||||
[K_MZ,SK_MZ]=OptimiseAlgebra(K_MZ,'SK_MZ');
|
||||
|
||||
%% derive equations for sequential fusion of optical flow measurements
|
||||
|
||||
% calculate range from plane to centre of sensor fov assuming flat earth
|
||||
% and camera axes aligned with body axes
|
||||
range = ((ptd - pd)/Tbn(3,3));
|
||||
% calculate relative velocity in body frame
|
||||
relVelBody = transpose(Tbn)*[vn;ve;vd];
|
||||
% divide by range to get predicted angular LOS rates relative to X and Y
|
||||
% axes. Note these are body angular rate motion compensated optical flow rates
|
||||
losRateX = +relVelBody(2)/range;
|
||||
losRateY = -relVelBody(1)/range;
|
||||
|
||||
H_LOS = jacobian([losRateX;losRateY],stateVector); % measurement Jacobian
|
||||
H_LOS = subs(H_LOS, {'rotErrX', 'rotErrY', 'rotErrZ'}, {0,0,0});
|
||||
H_LOS = simplify(H_LOS);
|
||||
[H_LOS,SH_LOS] = OptimiseAlgebra(H_LOS,'SH_LOS');
|
||||
|
||||
% combine into a single K matrix to enable common expressions to be found
|
||||
% note this matrix cannot be used in a single step fusion
|
||||
K_LOSX = (P*transpose(H_LOS(1,:)))/(H_LOS(1,:)*P*transpose(H_LOS(1,:)) + R_LOS); % Kalman gain vector
|
||||
K_LOSX = subs(K_LOSX, {'rotErrX', 'rotErrY', 'rotErrZ'}, {0,0,0});
|
||||
K_LOSY = (P*transpose(H_LOS(2,:)))/(H_LOS(2,:)*P*transpose(H_LOS(2,:)) + R_LOS); % Kalman gain vector
|
||||
K_LOSY = subs(K_LOSY, {'rotErrX', 'rotErrY', 'rotErrZ'}, {0,0,0});
|
||||
K_LOS = [K_LOSX,K_LOSY];
|
||||
simplify(K_LOS);
|
||||
[K_LOS,SK_LOS]=OptimiseAlgebra(K_LOS,'SK_LOS');
|
||||
|
||||
% Use matlab c code converter for an alternate method of
|
||||
ccode(H_LOS,'file','H_LOS.txt');
|
||||
ccode(K_LOSX,'file','K_LOSX.txt');
|
||||
ccode(K_LOSY,'file','K_LOSY.txt');
|
||||
|
||||
%% derive equations for fusion of magnetic heading measurement
|
||||
|
||||
% rotate magnetic field into earth axes
|
||||
magMeasNED = Tbn*[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_MAGS = jacobian(angMeas,errRotVec); % measurement Jacobian
|
||||
%H_MAGS = H_MAGS(1:3);
|
||||
H_MAGS = subs(H_MAGS, {'rotErrX', 'rotErrY', 'rotErrZ'}, {0,0,0});
|
||||
H_MAGS = simplify(H_MAGS);
|
||||
%[H_MAGS,SH_MAGS]=OptimiseAlgebra(H_MAGS,'SH_MAGS');
|
||||
ccode(H_MAGS,'file','calcH_MAGS.c');
|
||||
|
||||
%% derive equations for fusion of synthetic deviation measurement
|
||||
% used to keep correct heading when operating without absolute position or
|
||||
% velocity measurements - eg when using optical flow
|
||||
% rotate magnetic field into earth axes
|
||||
magMeasNED = [magN;magE;magD];
|
||||
% the predicted measurement is the angle wrt magnetic north of the horizontal
|
||||
% component of the measured field
|
||||
angMeas = tan(magMeasNED(2)/magMeasNED(1));
|
||||
H_MAGD = jacobian(angMeas,stateVector); % measurement Jacobian
|
||||
H_MAGD = subs(H_MAGD, {'rotErrX', 'rotErrY', 'rotErrZ'}, {0,0,0});
|
||||
H_MAGD = simplify(H_MAGD);
|
||||
%[H_MAGD,SH_MAGD]=OptimiseAlgebra(H_MAGD,'SH_MAGD');
|
||||
%ccode(H_MAGD,'file','calcH_MAGD.c');
|
||||
% Calculate Kalman gain vector
|
||||
K_MAGD = (P*transpose(H_MAGD))/(H_MAGD*P*transpose(H_MAGD) + R_DECL);
|
||||
ccode([H_MAGD',K_MAGD],'file','calcMAGD.c');
|
||||
|
||||
%% derive equations for fusion of lateral body acceleration (multirotors only)
|
||||
|
||||
% use relationship between airspeed along the X and Y body axis and the
|
||||
% drag to predict the lateral acceleration for a multirotor vehicle type
|
||||
% where propulsion forces are generated primarily along the Z body axis
|
||||
|
||||
vrel = transpose(Tbn)*[(vn-vwn);(ve-vwe);vd]; % predicted wind relative velocity
|
||||
|
||||
% calculate drag assuming flight along axis in positive direction
|
||||
% sign change will be looked after in implementation rather than by adding
|
||||
% sign functions to symbolic derivation which genererates output with dirac
|
||||
% functions
|
||||
% accXpred = -0.5*rho*vrel(1)*vrel(1)*BCXinv; % predicted acceleration measured along X body axis
|
||||
% accYpred = -0.5*rho*vrel(2)*vrel(2)*BCYinv; % predicted acceleration measured along Y body axis
|
||||
|
||||
% Use a simple viscous drag model for the linear estimator equations
|
||||
% Use the the derivative from speed to acceleration averaged across the
|
||||
% speed range
|
||||
% The nonlinear equation will be used to calculate the predicted
|
||||
% measurement in implementation
|
||||
accXpred = -Kacc*vrel(1); % predicted acceleration measured along X body axis
|
||||
accYpred = -Kacc*vrel(2); % predicted acceleration measured along Y body axis
|
||||
|
||||
% Derive observation Jacobian and Kalman gain matrix for X accel fusion
|
||||
H_ACCX = jacobian(accXpred,stateVector); % measurement Jacobian
|
||||
H_ACCX = subs(H_ACCX, {'rotErrX', 'rotErrY', 'rotErrZ'}, {0,0,0});
|
||||
[H_ACCX,SH_ACCX]=OptimiseAlgebra(H_ACCX,'SH_ACCX'); % optimise processing
|
||||
K_ACCX = (P*transpose(H_ACCX))/(H_ACCX*P*transpose(H_ACCX) + R_ACC);
|
||||
ccode([H_ACCX',K_ACCX],'file','calcACCX.c');
|
||||
[K_ACCX,SK_ACCX]=OptimiseAlgebra(K_ACCX,'SK_ACCX'); % Kalman gain vector
|
||||
|
||||
% Derive observation Jacobian and Kalman gain matrix for Y accel fusion
|
||||
H_ACCY = jacobian(accYpred,stateVector); % measurement Jacobian
|
||||
H_ACCY = subs(H_ACCY, {'rotErrX', 'rotErrY', 'rotErrZ'}, {0,0,0});
|
||||
[H_ACCY,SH_ACCY]=OptimiseAlgebra(H_ACCY,'SH_ACCY'); % optimise processing
|
||||
K_ACCY = (P*transpose(H_ACCY))/(H_ACCY*P*transpose(H_ACCY) + R_ACC);
|
||||
[K_ACCY,SK_ACCY]=OptimiseAlgebra(K_ACCY,'SK_ACCY'); % Kalman gain vector
|
||||
|
||||
% generate c code for jacobians using the matlab code generator
|
||||
ccode([H_ACCX;H_ACCY],'file','calcH_ACCXY.c');
|
||||
|
||||
%% Save output and convert to m and c code fragments
|
||||
fileName = strcat('SymbolicOutput',int2str(nStates),'.mat');
|
||||
save(fileName);
|
||||
SaveScriptCode(nStates);
|
||||
ConvertToM(nStates);
|
||||
ConvertToC(nStates);
|
||||
@@ -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;
|
||||
@@ -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
|
||||
@@ -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];
|
||||
@@ -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);
|
||||
@@ -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))];
|
||||
@@ -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));
|
||||
@@ -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
|
||||
@@ -0,0 +1,647 @@
|
||||
function SaveScriptCode(nStates)
|
||||
%% Load Data
|
||||
fileName = strcat('SymbolicOutput',int2str(nStates),'.mat');
|
||||
load(fileName);
|
||||
|
||||
%% Open output file
|
||||
fileName = strcat('SymbolicOutput',int2str(nStates),'.txt');
|
||||
fid = fopen(fileName,'wt');
|
||||
|
||||
%% Write equation for state transition matrix
|
||||
if exist('SF','var')
|
||||
|
||||
fprintf(fid,'SF = zeros(%d,1);\n',numel(SF));
|
||||
for rowIndex = 1:numel(SF)
|
||||
string = char(SF(rowIndex,1));
|
||||
fprintf(fid,'SF(%d) = %s;\n',rowIndex,string);
|
||||
end
|
||||
|
||||
% fprintf(fid,'\n');
|
||||
% fprintf(fid,'F = zeros(%d,%d);\n',nStates,nStates);
|
||||
% for rowIndex = 1:nStates
|
||||
% for colIndex = 1:nStates
|
||||
% string = char(F(rowIndex,colIndex));
|
||||
% % don't write out a zero-assignment
|
||||
% if ~strcmpi(string,'0')
|
||||
% fprintf(fid,'F(%d,%d) = %s;\n',rowIndex,colIndex,string);
|
||||
% end
|
||||
% end
|
||||
% end
|
||||
% fprintf(fid,'\n');
|
||||
|
||||
end
|
||||
%% Write equations for control influence (disturbance) matrix
|
||||
if exist('SG','var')
|
||||
|
||||
fprintf(fid,'\n');
|
||||
fprintf(fid,'SG = zeros(%d,1);\n',numel(SG));
|
||||
for rowIndex = 1:numel(SG)
|
||||
string = char(SG(rowIndex,1));
|
||||
fprintf(fid,'SG(%d) = %s;\n',rowIndex,string);
|
||||
end
|
||||
fprintf(fid,'\n');
|
||||
|
||||
% fprintf(fid,'\n');
|
||||
% fprintf(fid,'G = zeros(%d,%d);\n',nStates,numel([da;dv]));
|
||||
% for rowIndex = 1:nStates
|
||||
% for colIndex = 1:numel([da;dv])
|
||||
% string = char(G(rowIndex,colIndex));
|
||||
% % don't write out a zero-assignment
|
||||
% if ~strcmpi(string,'0')
|
||||
% fprintf(fid,'G(%d,%d) = %s;\n',rowIndex,colIndex,string);
|
||||
% end
|
||||
% end
|
||||
% end
|
||||
% fprintf(fid,'\n');
|
||||
|
||||
end
|
||||
%% Write equations for state error matrix
|
||||
if exist('SQ','var')
|
||||
|
||||
fprintf(fid,'\n');
|
||||
fprintf(fid,'SQ = zeros(%d,1);\n',numel(SQ));
|
||||
for rowIndex = 1:numel(SQ)
|
||||
string = char(SQ(rowIndex,1));
|
||||
fprintf(fid,'SQ(%d) = %s;\n',rowIndex,string);
|
||||
end
|
||||
fprintf(fid,'\n');
|
||||
|
||||
% fprintf(fid,'\n');
|
||||
% fprintf(fid,'Q = zeros(%d,%d);\n',nStates,nStates);
|
||||
% for rowIndex = 1:nStates
|
||||
% for colIndex = 1:nStates
|
||||
% string = char(Q(rowIndex,colIndex));
|
||||
% % don't write out a zero-assignment
|
||||
% if ~strcmpi(string,'0')
|
||||
% fprintf(fid,'Q(%d,%d) = %s;\n',rowIndex,colIndex,string);
|
||||
% end
|
||||
% end
|
||||
% end
|
||||
% fprintf(fid,'\n');
|
||||
|
||||
end
|
||||
%% Write equations for covariance prediction
|
||||
% Only write out upper diagonal (matrix is symmetric)
|
||||
if exist('SPP','var')
|
||||
|
||||
fprintf(fid,'\n');
|
||||
fprintf(fid,'SPP = zeros(%d,1);\n',numel(SPP));
|
||||
for rowIndex = 1:numel(SPP)
|
||||
string = char(SPP(rowIndex,1));
|
||||
fprintf(fid,'SPP(%d) = %s;\n',rowIndex,string);
|
||||
end
|
||||
fprintf(fid,'\n');
|
||||
|
||||
end
|
||||
|
||||
if exist('PP','var')
|
||||
|
||||
fprintf(fid,'\n');
|
||||
fprintf(fid,'nextP = zeros(%d,%d);\n',nStates,nStates);
|
||||
for colIndex = 1:nStates
|
||||
for rowIndex = 1:colIndex
|
||||
string = char(PP(rowIndex,colIndex));
|
||||
% don't write out a zero-assignment
|
||||
if ~strcmpi(string,'0')
|
||||
fprintf(fid,'nextP(%d,%d) = %s;\n',rowIndex,colIndex,string);
|
||||
end
|
||||
end
|
||||
end
|
||||
fprintf(fid,'\n');
|
||||
|
||||
end
|
||||
|
||||
%% Write equations for velocity and position data fusion
|
||||
if exist('H_VP','var')
|
||||
|
||||
[nRow,nCol] = size(H_VP);
|
||||
fprintf(fid,'\n');
|
||||
fprintf(fid,'H_VP = zeros(%d,%d);\n',nRow,nCol);
|
||||
for rowIndex = 1:nRow
|
||||
for colIndex = 1:nCol
|
||||
string = char(H_VP(rowIndex,colIndex));
|
||||
% don't write out a zero-assignment
|
||||
if ~strcmpi(string,'0')
|
||||
fprintf(fid,'H_VP(%d,%d) = %s;\n',rowIndex,colIndex,string);
|
||||
end
|
||||
end
|
||||
end
|
||||
fprintf(fid,'\n');
|
||||
|
||||
[nRow,nCol] = size(SK_VP);
|
||||
fprintf(fid,'\n');
|
||||
fprintf(fid,'SK_VP = zeros(%d,%d);\n',nRow,nCol);
|
||||
for rowIndex = 1:nRow
|
||||
for colIndex = 1:nCol
|
||||
string = char(SK_VP(rowIndex,colIndex));
|
||||
% don't write out a zero-assignment
|
||||
if ~strcmpi(string,'0')
|
||||
fprintf(fid,'SK_VP(%d,%d) = %s;\n',rowIndex,colIndex,string);
|
||||
end
|
||||
end
|
||||
end
|
||||
fprintf(fid,'\n');
|
||||
|
||||
[nRow,nCol] = size(K_VP);
|
||||
fprintf(fid,'\n');
|
||||
fprintf(fid,'Kfusion = zeros(%d,%d);\n',nRow,nCol);
|
||||
for rowIndex = 1:nRow
|
||||
for colIndex = 1:nCol
|
||||
string = char(K_VP(rowIndex,colIndex));
|
||||
% don't write out a zero-assignment
|
||||
if ~strcmpi(string,'0')
|
||||
fprintf(fid,'Kfusion(%d,%d) = %s;\n',rowIndex,colIndex,string);
|
||||
end
|
||||
end
|
||||
end
|
||||
fprintf(fid,'\n');
|
||||
|
||||
end
|
||||
%% Write equations for true airspeed data fusion
|
||||
if exist('SH_TAS','var')
|
||||
|
||||
fprintf(fid,'\n');
|
||||
fprintf(fid,'SH_TAS = zeros(%d,1);\n',numel(SH_TAS));
|
||||
for rowIndex = 1:numel(SH_TAS)
|
||||
string = char(SH_TAS(rowIndex,1));
|
||||
fprintf(fid,'SH_TAS(%d) = %s;\n',rowIndex,string);
|
||||
end
|
||||
|
||||
[nRow,nCol] = size(H_TAS);
|
||||
fprintf(fid,'\n');
|
||||
fprintf(fid,'H_TAS = zeros(1,%d);\n',nCol);
|
||||
for rowIndex = 1:nRow
|
||||
for colIndex = 1:nCol
|
||||
string = char(H_TAS(rowIndex,colIndex));
|
||||
% don't write out a zero-assignment
|
||||
if ~strcmpi(string,'0')
|
||||
fprintf(fid,'H_TAS(1,%d) = %s;\n',colIndex,string);
|
||||
end
|
||||
end
|
||||
end
|
||||
fprintf(fid,'\n');
|
||||
|
||||
fprintf(fid,'\n');
|
||||
fprintf(fid,'SK_TAS = zeros(%d,1);\n',numel(SK_TAS));
|
||||
for rowIndex = 1:numel(SK_TAS)
|
||||
string = char(SK_TAS(rowIndex,1));
|
||||
fprintf(fid,'SK_TAS(%d) = %s;\n',rowIndex,string);
|
||||
end
|
||||
fprintf(fid,'\n');
|
||||
|
||||
[nRow,nCol] = size(K_TAS);
|
||||
fprintf(fid,'\n');
|
||||
fprintf(fid,'Kfusion = zeros(%d,1);\n',nRow,nCol);
|
||||
for rowIndex = 1:nRow
|
||||
string = char(K_TAS(rowIndex,1));
|
||||
% don't write out a zero-assignment
|
||||
if ~strcmpi(string,'0')
|
||||
fprintf(fid,'Kfusion(%d) = %s;\n',rowIndex,string);
|
||||
end
|
||||
end
|
||||
fprintf(fid,'\n');
|
||||
|
||||
end
|
||||
%% Write equations for sideslip data fusion
|
||||
if exist('SH_BETA','var')
|
||||
|
||||
fprintf(fid,'\n');
|
||||
fprintf(fid,'SH_BETA = zeros(%d,1);\n',numel(SH_BETA));
|
||||
for rowIndex = 1:numel(SH_BETA)
|
||||
string = char(SH_BETA(rowIndex,1));
|
||||
fprintf(fid,'SH_BETA(%d) = %s;\n',rowIndex,string);
|
||||
end
|
||||
|
||||
[nRow,nCol] = size(H_BETA);
|
||||
fprintf(fid,'\n');
|
||||
fprintf(fid,'H_BETA = zeros(1,%d);\n',nCol);
|
||||
for rowIndex = 1:nRow
|
||||
for colIndex = 1:nCol
|
||||
string = char(H_BETA(rowIndex,colIndex));
|
||||
% don't write out a zero-assignment
|
||||
if ~strcmpi(string,'0')
|
||||
fprintf(fid,'H_BETA(1,%d) = %s;\n',colIndex,string);
|
||||
end
|
||||
end
|
||||
end
|
||||
fprintf(fid,'\n');
|
||||
|
||||
fprintf(fid,'\n');
|
||||
fprintf(fid,'SK_BETA = zeros(%d,1);\n',numel(SK_BETA));
|
||||
for rowIndex = 1:numel(SK_BETA)
|
||||
string = char(SK_BETA(rowIndex,1));
|
||||
fprintf(fid,'SK_BETA(%d) = %s;\n',rowIndex,string);
|
||||
end
|
||||
fprintf(fid,'\n');
|
||||
|
||||
[nRow,nCol] = size(K_BETA);
|
||||
fprintf(fid,'\n');
|
||||
fprintf(fid,'Kfusion = zeros(%d,1);\n',nRow,nCol);
|
||||
for rowIndex = 1:nRow
|
||||
string = char(K_BETA(rowIndex,1));
|
||||
% don't write out a zero-assignment
|
||||
if ~strcmpi(string,'0')
|
||||
fprintf(fid,'Kfusion(%d) = %s;\n',rowIndex,string);
|
||||
end
|
||||
end
|
||||
fprintf(fid,'\n');
|
||||
|
||||
end
|
||||
%% Write equations for magnetometer data fusion
|
||||
if exist('SH_MAG','var')
|
||||
|
||||
fprintf(fid,'\n');
|
||||
fprintf(fid,'SH_MAG = zeros(%d,1);\n',numel(SH_MAG));
|
||||
for rowIndex = 1:numel(SH_MAG)
|
||||
string = char(SH_MAG(rowIndex,1));
|
||||
fprintf(fid,'SH_MAG(%d) = %s;\n',rowIndex,string);
|
||||
end
|
||||
fprintf(fid,'\n');
|
||||
|
||||
[nRow,nCol] = size(H_MAG);
|
||||
fprintf(fid,'\n');
|
||||
fprintf(fid,'H_MAG = zeros(1,%d);\n',nCol);
|
||||
for colIndex = 1:nCol
|
||||
string = char(H_MAG(1,colIndex));
|
||||
% don't write out a zero-assignment
|
||||
if ~strcmpi(string,'0')
|
||||
fprintf(fid,'H_MAG(%d) = %s;\n',colIndex,string);
|
||||
end
|
||||
end
|
||||
fprintf(fid,'\n');
|
||||
|
||||
fprintf(fid,'\n');
|
||||
fprintf(fid,'SK_MX = zeros(%d,1);\n',numel(SK_MX));
|
||||
for rowIndex = 1:numel(SK_MX)
|
||||
string = char(SK_MX(rowIndex,1));
|
||||
fprintf(fid,'SK_MX(%d) = %s;\n',rowIndex,string);
|
||||
end
|
||||
fprintf(fid,'\n');
|
||||
|
||||
[nRow,nCol] = size(K_MX);
|
||||
fprintf(fid,'\n');
|
||||
fprintf(fid,'Kfusion = zeros(%d,1);\n',nRow,nCol);
|
||||
for rowIndex = 1:nRow
|
||||
string = char(K_MX(rowIndex,1));
|
||||
% don't write out a zero-assignment
|
||||
if ~strcmpi(string,'0')
|
||||
fprintf(fid,'Kfusion(%d) = %s;\n',rowIndex,string);
|
||||
end
|
||||
end
|
||||
fprintf(fid,'\n');
|
||||
|
||||
[nRow,nCol] = size(H_MAG);
|
||||
fprintf(fid,'\n');
|
||||
fprintf(fid,'H_MAG = zeros(1,%d);\n',nCol);
|
||||
for colIndex = 1:nCol
|
||||
string = char(H_MAG(2,colIndex));
|
||||
% don't write out a zero-assignment
|
||||
if ~strcmpi(string,'0')
|
||||
fprintf(fid,'H_MAG(%d) = %s;\n',colIndex,string);
|
||||
end
|
||||
end
|
||||
fprintf(fid,'\n');
|
||||
|
||||
fprintf(fid,'\n');
|
||||
fprintf(fid,'SK_MY = zeros(%d,1);\n',numel(SK_MY));
|
||||
for rowIndex = 1:numel(SK_MY)
|
||||
string = char(SK_MY(rowIndex,1));
|
||||
fprintf(fid,'SK_MY(%d) = %s;\n',rowIndex,string);
|
||||
end
|
||||
fprintf(fid,'\n');
|
||||
|
||||
[nRow,nCol] = size(K_MY);
|
||||
fprintf(fid,'\n');
|
||||
fprintf(fid,'Kfusion = zeros(%d,1);\n',nRow,nCol);
|
||||
for rowIndex = 1:nRow
|
||||
string = char(K_MY(rowIndex,1));
|
||||
% don't write out a zero-assignment
|
||||
if ~strcmpi(string,'0')
|
||||
fprintf(fid,'Kfusion(%d) = %s;\n',rowIndex,string);
|
||||
end
|
||||
end
|
||||
fprintf(fid,'\n');
|
||||
|
||||
[nRow,nCol] = size(H_MAG);
|
||||
fprintf(fid,'\n');
|
||||
fprintf(fid,'H_MAG = zeros(1,%d);\n',nCol);
|
||||
for colIndex = 1:nCol
|
||||
string = char(H_MAG(3,colIndex));
|
||||
% don't write out a zero-assignment
|
||||
if ~strcmpi(string,'0')
|
||||
fprintf(fid,'H_MAG(%d) = %s;\n',colIndex,string);
|
||||
end
|
||||
end
|
||||
fprintf(fid,'\n');
|
||||
|
||||
fprintf(fid,'\n');
|
||||
fprintf(fid,'SK_MZ = zeros(%d,1);\n',numel(SK_MZ));
|
||||
for rowIndex = 1:numel(SK_MZ)
|
||||
string = char(SK_MZ(rowIndex,1));
|
||||
fprintf(fid,'SK_MZ(%d) = %s;\n',rowIndex,string);
|
||||
end
|
||||
fprintf(fid,'\n');
|
||||
|
||||
[nRow,nCol] = size(K_MZ);
|
||||
fprintf(fid,'\n');
|
||||
fprintf(fid,'Kfusion = zeros(%d,1);\n',nRow,nCol);
|
||||
for rowIndex = 1:nRow
|
||||
string = char(K_MZ(rowIndex,1));
|
||||
% don't write out a zero-assignment
|
||||
if ~strcmpi(string,'0')
|
||||
fprintf(fid,'Kfusion(%d) = %s;\n',rowIndex,string);
|
||||
end
|
||||
end
|
||||
fprintf(fid,'\n');
|
||||
|
||||
end
|
||||
%% Write equations for optical flow sensor angular LOS data fusion
|
||||
if exist('SH_LOS','var')
|
||||
|
||||
fprintf(fid,'\n');
|
||||
fprintf(fid,'SH_LOS = zeros(%d,1);\n',numel(SH_LOS));
|
||||
for rowIndex = 1:numel(SH_LOS)
|
||||
string = char(SH_LOS(rowIndex,1));
|
||||
fprintf(fid,'SH_LOS(%d) = %s;\n',rowIndex,string);
|
||||
end
|
||||
fprintf(fid,'\n');
|
||||
|
||||
fprintf(fid,'\n');
|
||||
[nRow,nCol] = size(H_LOS);
|
||||
fprintf(fid,'\n');
|
||||
fprintf(fid,'H_LOS = zeros(1,%d);\n',nCol);
|
||||
for colIndex = 1:nCol
|
||||
string = char(H_LOS(1,colIndex));
|
||||
% don't write out a zero-assignment
|
||||
if ~strcmpi(string,'0')
|
||||
fprintf(fid,'H_LOS(%d) = %s;\n',colIndex,string);
|
||||
end
|
||||
end
|
||||
fprintf(fid,'\n');
|
||||
|
||||
fprintf(fid,'\n');
|
||||
[nRow,nCol] = size(H_LOS);
|
||||
fprintf(fid,'\n');
|
||||
fprintf(fid,'H_LOS = zeros(1,%d);\n',nCol);
|
||||
for colIndex = 1:nCol
|
||||
string = char(H_LOS(2,colIndex));
|
||||
% don't write out a zero-assignment
|
||||
if ~strcmpi(string,'0')
|
||||
fprintf(fid,'H_LOS(%d) = %s;\n',colIndex,string);
|
||||
end
|
||||
end
|
||||
|
||||
% fprintf(fid,'\n');
|
||||
% fprintf(fid,'SKK_LOS = zeros(%d,1);\n',numel(SKK_LOS));
|
||||
% for rowIndex = 1:numel(SKK_LOS)
|
||||
% string = char(SKK_LOS(rowIndex,1));
|
||||
% fprintf(fid,'SKK_LOS(%d) = %s;\n',rowIndex,string);
|
||||
% end
|
||||
|
||||
fprintf(fid,'\n');
|
||||
fprintf(fid,'SK_LOS = zeros(%d,1);\n',numel(SK_LOS));
|
||||
for rowIndex = 1:numel(SK_LOS)
|
||||
string = char(SK_LOS(rowIndex,1));
|
||||
fprintf(fid,'SK_LOS(%d) = %s;\n',rowIndex,string);
|
||||
end
|
||||
|
||||
[nRow,nCol] = size(K_LOSX);
|
||||
fprintf(fid,'\n');
|
||||
fprintf(fid,'Kfusion = zeros(%d,1);\n',nRow,nCol);
|
||||
for rowIndex = 1:nRow
|
||||
string = char(K_LOS(rowIndex,1));
|
||||
% don't write out a zero-assignment
|
||||
if ~strcmpi(string,'0')
|
||||
fprintf(fid,'Kfusion(%d) = %s;\n',rowIndex,string);
|
||||
end
|
||||
end
|
||||
fprintf(fid,'\n');
|
||||
|
||||
[nRow,nCol] = size(K_LOSY);
|
||||
fprintf(fid,'\n');
|
||||
fprintf(fid,'Kfusion = zeros(%d,1);\n',nRow,nCol);
|
||||
for rowIndex = 1:nRow
|
||||
string = char(K_LOS(rowIndex,2));
|
||||
% don't write out a zero-assignment
|
||||
if ~strcmpi(string,'0')
|
||||
fprintf(fid,'Kfusion(%d) = %s;\n',rowIndex,string);
|
||||
end
|
||||
end
|
||||
|
||||
end
|
||||
%% Write observation fusion equations for optical flow sensor scale factor error estimation
|
||||
if exist('SH_OPT','var')
|
||||
|
||||
fprintf(fid,'\n');
|
||||
for rowIndex = 1:numel(SH_OPT)
|
||||
string = char(SH_OPT(rowIndex,1));
|
||||
fprintf(fid,'SH_OPT(%d) = %s;\n',rowIndex,string);
|
||||
end
|
||||
fprintf(fid,'\n');
|
||||
|
||||
fprintf(fid,'\n');
|
||||
string = char(H_OPT(1));
|
||||
% don't write out a zero-assignment
|
||||
if ~strcmpi(string,'0')
|
||||
fprintf(fid,'H_OPT(1) = %s;\n',1,string);
|
||||
end
|
||||
fprintf(fid,'\n');
|
||||
|
||||
fprintf(fid,'\n');
|
||||
string = char(H_OPT(2));
|
||||
% don't write out a zero-assignment
|
||||
if ~strcmpi(string,'0')
|
||||
fprintf(fid,'H_OPT(2) = %s;\n',1,string);
|
||||
end
|
||||
fprintf(fid,'\n');
|
||||
|
||||
fprintf(fid,'\n');
|
||||
for rowIndex = 1:numel(SK_OPT)
|
||||
string = char(SK_OPT(rowIndex,1));
|
||||
fprintf(fid,'SK_OPT(%d) = %s;\n',rowIndex,string);
|
||||
end
|
||||
|
||||
fprintf(fid,'\n');
|
||||
string = char(K_OPT(1));
|
||||
% don't write out a zero-assignment
|
||||
if ~strcmpi(string,'0')
|
||||
fprintf(fid,'K_OPT(1) = %s;\n',1,string);
|
||||
end
|
||||
fprintf(fid,'\n');
|
||||
|
||||
fprintf(fid,'\n');
|
||||
string = char(K_OPT(2));
|
||||
% don't write out a zero-assignment
|
||||
if ~strcmpi(string,'0')
|
||||
fprintf(fid,'K_OPT(2) = %s;\n',1,string);
|
||||
end
|
||||
fprintf(fid,'\n');
|
||||
|
||||
end
|
||||
%% Write equations for laser range finder data fusion
|
||||
if exist('SH_RNG','var')
|
||||
|
||||
fprintf(fid,'\n');
|
||||
fprintf(fid,'SH_RNG = zeros(%d,1);\n',numel(SH_RNG));
|
||||
for rowIndex = 1:numel(SH_RNG)
|
||||
string = char(SH_RNG(rowIndex,1));
|
||||
fprintf(fid,'SH_RNG(%d) = %s;\n',rowIndex,string);
|
||||
end
|
||||
|
||||
[nRow,nCol] = size(H_RNG);
|
||||
fprintf(fid,'\n');
|
||||
fprintf(fid,'H_RNG = zeros(1,%d);\n',nCol);
|
||||
for rowIndex = 1:nRow
|
||||
for colIndex = 1:nCol
|
||||
string = char(H_RNG(rowIndex,colIndex));
|
||||
% don't write out a zero-assignment
|
||||
if ~strcmpi(string,'0')
|
||||
fprintf(fid,'H_RNG(1,%d) = %s;\n',colIndex,string);
|
||||
end
|
||||
end
|
||||
end
|
||||
fprintf(fid,'\n');
|
||||
|
||||
fprintf(fid,'\n');
|
||||
fprintf(fid,'SK_RNG = zeros(%d,1);\n',numel(SK_RNG));
|
||||
for rowIndex = 1:numel(SK_RNG)
|
||||
string = char(SK_RNG(rowIndex,1));
|
||||
fprintf(fid,'SK_RNG(%d) = %s;\n',rowIndex,string);
|
||||
end
|
||||
fprintf(fid,'\n');
|
||||
|
||||
[nRow,nCol] = size(K_RNG);
|
||||
fprintf(fid,'\n');
|
||||
fprintf(fid,'Kfusion = zeros(%d,1);\n',nRow,nCol);
|
||||
for rowIndex = 1:nRow
|
||||
string = char(K_RNG(rowIndex,1));
|
||||
% don't write out a zero-assignment
|
||||
if ~strcmpi(string,'0')
|
||||
fprintf(fid,'Kfusion(%d) = %s;\n',rowIndex,string);
|
||||
end
|
||||
end
|
||||
fprintf(fid,'\n');
|
||||
|
||||
end
|
||||
|
||||
%% Write equations for simple magnetomter data fusion
|
||||
if exist('SH_MAGS','var')
|
||||
|
||||
fprintf(fid,'\n');
|
||||
fprintf(fid,'SH_MAGS = zeros(%d,1);\n',numel(SH_MAGS));
|
||||
for rowIndex = 1:numel(SH_MAGS)
|
||||
string = char(SH_MAGS(rowIndex,1));
|
||||
fprintf(fid,'SH_MAGS(%d) = %s;\n',rowIndex,string);
|
||||
end
|
||||
fprintf(fid,'\n');
|
||||
|
||||
[nRow,nCol] = size(H_MAGS);
|
||||
fprintf(fid,'\n');
|
||||
fprintf(fid,'H_MAGS = zeros(1,%d);\n',nCol);
|
||||
for colIndex = 1:nCol
|
||||
string = char(H_MAGS(1,colIndex));
|
||||
% don't write out a zero-assignment
|
||||
if ~strcmpi(string,'0')
|
||||
fprintf(fid,'H_MAGS(%d) = %s;\n',colIndex,string);
|
||||
end
|
||||
end
|
||||
fprintf(fid,'\n');
|
||||
|
||||
end
|
||||
|
||||
%% Write equations for X accel fusion
|
||||
if exist('SH_ACCX','var')
|
||||
|
||||
fprintf(fid,'\n');
|
||||
fprintf(fid,'SH_ACCX = zeros(%d,1);\n',numel(SH_ACCX));
|
||||
for rowIndex = 1:numel(SH_ACCX)
|
||||
string = char(SH_ACCX(rowIndex,1));
|
||||
fprintf(fid,'SH_ACCX(%d) = %s;\n',rowIndex,string);
|
||||
end
|
||||
|
||||
[nRow,nCol] = size(H_ACCX);
|
||||
fprintf(fid,'\n');
|
||||
fprintf(fid,'H_ACCX = zeros(1,%d);\n',nCol);
|
||||
for rowIndex = 1:nRow
|
||||
for colIndex = 1:nCol
|
||||
string = char(H_ACCX(rowIndex,colIndex));
|
||||
% don't write out a zero-assignment
|
||||
if ~strcmpi(string,'0')
|
||||
fprintf(fid,'H_ACCX(1,%d) = %s;\n',colIndex,string);
|
||||
end
|
||||
end
|
||||
end
|
||||
fprintf(fid,'\n');
|
||||
|
||||
fprintf(fid,'\n');
|
||||
fprintf(fid,'SK_ACCX = zeros(%d,1);\n',numel(SK_ACCX));
|
||||
for rowIndex = 1:numel(SK_ACCX)
|
||||
string = char(SK_ACCX(rowIndex,1));
|
||||
fprintf(fid,'SK_ACCX(%d) = %s;\n',rowIndex,string);
|
||||
end
|
||||
fprintf(fid,'\n');
|
||||
|
||||
[nRow,nCol] = size(K_ACCX);
|
||||
fprintf(fid,'\n');
|
||||
fprintf(fid,'Kfusion = zeros(%d,1);\n',nRow,nCol);
|
||||
for rowIndex = 1:nRow
|
||||
string = char(K_ACCX(rowIndex,1));
|
||||
% don't write out a zero-assignment
|
||||
if ~strcmpi(string,'0')
|
||||
fprintf(fid,'Kfusion(%d) = %s;\n',rowIndex,string);
|
||||
end
|
||||
end
|
||||
fprintf(fid,'\n');
|
||||
|
||||
end
|
||||
|
||||
%% Write equations for Y accel fusion
|
||||
if exist('SH_ACCY','var')
|
||||
|
||||
fprintf(fid,'\n');
|
||||
fprintf(fid,'SH_ACCY = zeros(%d,1);\n',numel(SH_ACCY));
|
||||
for rowIndex = 1:numel(SH_ACCY)
|
||||
string = char(SH_ACCY(rowIndex,1));
|
||||
fprintf(fid,'SH_ACCY(%d) = %s;\n',rowIndex,string);
|
||||
end
|
||||
|
||||
[nRow,nCol] = size(H_ACCY);
|
||||
fprintf(fid,'\n');
|
||||
fprintf(fid,'H_ACCY = zeros(1,%d);\n',nCol);
|
||||
for rowIndex = 1:nRow
|
||||
for colIndex = 1:nCol
|
||||
string = char(H_ACCY(rowIndex,colIndex));
|
||||
% don't write out a zero-assignment
|
||||
if ~strcmpi(string,'0')
|
||||
fprintf(fid,'H_ACCY(1,%d) = %s;\n',colIndex,string);
|
||||
end
|
||||
end
|
||||
end
|
||||
fprintf(fid,'\n');
|
||||
|
||||
fprintf(fid,'\n');
|
||||
fprintf(fid,'SK_ACCY = zeros(%d,1);\n',numel(SK_ACCY));
|
||||
for rowIndex = 1:numel(SK_ACCY)
|
||||
string = char(SK_ACCY(rowIndex,1));
|
||||
fprintf(fid,'SK_ACCY(%d) = %s;\n',rowIndex,string);
|
||||
end
|
||||
fprintf(fid,'\n');
|
||||
|
||||
[nRow,nCol] = size(K_ACCY);
|
||||
fprintf(fid,'\n');
|
||||
fprintf(fid,'Kfusion = zeros(%d,1);\n',nRow,nCol);
|
||||
for rowIndex = 1:nRow
|
||||
string = char(K_ACCY(rowIndex,1));
|
||||
% don't write out a zero-assignment
|
||||
if ~strcmpi(string,'0')
|
||||
fprintf(fid,'Kfusion(%d) = %s;\n',rowIndex,string);
|
||||
end
|
||||
end
|
||||
fprintf(fid,'\n');
|
||||
|
||||
end
|
||||
|
||||
%% Close output file
|
||||
fclose(fid);
|
||||
|
||||
end
|
||||
Reference in New Issue
Block a user