fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-10-19 08:03:33 +08:00
Code Issues Packages Projects Releases Wiki Activity

cmake standalone build

Browse Source
This commit is contained in:
Daniel Agar
2018-05-03 22:25:36 -04:00
parent eec71d1a10
commit e5952fadaf
173 changed files with 588 additions and 324 deletions
Download Patch File Download Diff File Expand all files Collapse all files
EKF/matlab/scripts/Inertial Nav EKF/Airspeed.mat
BIN
View File
Binary file not shown.
EKF/matlab/scripts/Inertial Nav EKF/C_code24.txt
+686
View File
File diff suppressed because one or more lines are too long
EKF/matlab/scripts/Inertial Nav EKF/ConvertToC.m
+213
View File
@@ -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);
%% 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;
EKF/matlab/scripts/Inertial Nav EKF/ConvertToM.m
+46
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);
%% 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
EKF/matlab/scripts/Inertial Nav EKF/Drag.mat
BIN
View File
Binary file not shown.
EKF/matlab/scripts/Inertial Nav EKF/EulToQuat.m
+23
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;
EKF/matlab/scripts/Inertial Nav EKF/GenerateNavFilterEquations.m
+491
View File
@@ -0,0 +1,491 @@
% 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 control inputs,
% not observations
% Author: Paul Riseborough
% State vector:
% attitude quaternion
% Velocity - m/sec (North, East, Down)
% Position - m (North, East, Down)
% Delta Angle bias - rad (X,Y,Z)
% Delta Velocity bias - m/s (X,Y,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 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 daxVar dayVar dazVar dvxVar dvyVar dvzVar real; % IMU delta angle and delta velocity measurement variances
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 decl real; % earth magnetic field declination from true north
syms R_DECL R_YAW real; % variance of declination or yaw angle observation
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 Kaccx Kaccy real % derivative of X and Y body specific forces wrt componenent of true airspeed along each axis (1/s)
%% define the state prediction 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];
dVelBias = [dvx_b; dvy_b; dvz_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 compensation 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 -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;
% 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 position update equations
pNew = [pn;pe;pd] + [vn;ve;vd]*dt;
% define the IMU error update equations
dAngBiasNew = dAngBias;
dVelBiasNew = dVelBias;
% 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 = [quat;vn;ve;vd;pn;pe;pd;dAngBias;dVelBias;magN;magE;magD;magX;magY;magZ;vwn;vwe];
nStates=numel(stateVector);
% Define vector of process equations
newStateVector = [quatNew;vNew;pNew;dAngBiasNew;dVelBiasNew;magNnew;magEnew;magDnew;magXnew;magYnew;magZnew;vwnNew;vweNew];
% derive the state transition matrix
F = jacobian(newStateVector, stateVector);
% set the rotation error states to zero
[F,SF]=OptimiseAlgebra(F,'SF');
% define a symbolic covariance matrix using strings to represent
% '_l_' to represent '( '
% '_c_' to represent ,
% '_r_' to represent ')'
% these can be substituted later to create executable code
for rowIndex = 1:nStates
for colIndex = 1:nStates
eval(['syms OP_l_',num2str(rowIndex),'_c_',num2str(colIndex), '_r_ real']);
eval(['P(',num2str(rowIndex),',',num2str(colIndex), ') = OP_l_',num2str(rowIndex),'_c_',num2str(colIndex),'_r_;']);
end
end
save 'StatePrediction.mat';
%% derive the covariance prediction equations
% This reduces the number of floating point operations by a factor of 6 or
% more compared to using the standard matrix operations in code
% Error growth in the inertial solution is assumed to be driven by 'noise' in the delta angles and
% velocities, after bias effects have been removed.
% derive the control(disturbance) influence matrix from IMu noise to state
% noise
G = jacobian(newStateVector, [dAngMeas;dVelMeas]);
[G,SG]=OptimiseAlgebra(G,'SG');
% derive the state error matrix
distMatrix = diag([daxVar dayVar dazVar dvxVar dvyVar dvzVar]);
Q = G*distMatrix*transpose(G);
[Q,SQ]=OptimiseAlgebra(Q,'SQ');
% 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');
save('StateAndCovariancePrediction.mat');
clear all;
reset(symengine);
%% derive equations for fusion of true airspeed measurements
load('StatePrediction.mat');
VtasPred = sqrt((vn-vwn)^2 + (ve-vwe)^2 + vd^2); % predicted measurement
H_TAS = jacobian(VtasPred,stateVector); % measurement Jacobian
[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
% save equations and reset workspace
save('Airspeed.mat','SH_TAS','H_TAS','SK_TAS','K_TAS');
clear all;
reset(symengine);
%% derive equations for fusion of angle of sideslip measurements
load('StatePrediction.mat');
% calculate wind relative velocities in nav frame and rotate into body frame
Vbw = transpose(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,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
% save equations and reset workspace
save('Sideslip.mat','SH_BETA','H_BETA','SK_BETA','K_BETA');
clear all;
reset(symengine);
%% derive equations for fusion of magnetic field measurement
load('StatePrediction.mat');
magMeas = transpose(Tbn)*[magN;magE;magD] + [magX;magY;magZ]; % predicted measurement
H_MAG = jacobian(magMeas,stateVector); % measurement Jacobian
[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');
% save equations and reset workspace
save('Magnetometer.mat','SH_MAG','H_MAG','SK_MX','K_MX','SK_MY','K_MY','SK_MZ','K_MZ');
clear all;
reset(symengine);
%% derive equations for sequential fusion of optical flow measurements
load('StatePrediction.mat');
% Range is defined as distance from camera focal point to object measured
% along sensor Z axis
syms range real;
% Define rotation matrix from body to sensor frame
syms Tbs_a_x Tbs_a_y Tbs_a_z real;
syms Tbs_b_x Tbs_b_y Tbs_b_z real;
syms Tbs_c_x Tbs_c_y Tbs_c_z real;
Tbs = [ ...
Tbs_a_x Tbs_a_y Tbs_a_z ; ...
Tbs_b_x Tbs_b_y Tbs_b_z ; ...
Tbs_c_x Tbs_c_y Tbs_c_z ...
];
% Calculate earth relative velocity in a non-rotating sensor frame
relVelSensor = Tbs * transpose(Tbn) * [vn;ve;vd];
% Divide by range to get predicted angular LOS rates relative to X and Y
% axes. Note these are rates in a non-rotating sensor frame
losRateSensorX = +relVelSensor(2)/range;
losRateSensorY = -relVelSensor(1)/range;
save('temp1.mat','losRateSensorX','losRateSensorY');
clear all;
reset(symengine);
load('StatePrediction.mat');
load('temp1.mat');
% calculate the observation Jacobian and Kalman gain for the X axis
H_LOSX = jacobian(losRateSensorX,stateVector); % measurement Jacobian
H_LOSX = simplify(H_LOSX);
K_LOSX = (P*transpose(H_LOSX))/(H_LOSX*P*transpose(H_LOSX) + R_LOS); % Kalman gain vector
K_LOSX = simplify(K_LOSX);
save('temp2.mat','H_LOSX','K_LOSX');
ccode([H_LOSX;transpose(K_LOSX)],'file','LOSX.c');
fix_c_code('LOSX.c');
clear all;
reset(symengine);
load('StatePrediction.mat');
load('temp1.mat');
% calculate the observation Jacobian for the Y axis
H_LOSY = jacobian(losRateSensorY,stateVector); % measurement Jacobian
H_LOSY = simplify(H_LOSY);
K_LOSY = (P*transpose(H_LOSY))/(H_LOSY*P*transpose(H_LOSY) + R_LOS); % Kalman gain vector
K_LOSY = simplify(K_LOSY);
save('temp3.mat','H_LOSY','K_LOSY');
ccode([H_LOSY;transpose(K_LOSY)],'file','LOSY.c');
fix_c_code('LOSY.c');
% reset workspace
clear all;
reset(symengine);
%% derive equations for sequential fusion of body frame velocity measurements
load('StatePrediction.mat');
% body frame velocity observations
syms velX velY velZ real;
% velocity observation variance
syms R_VEL real;
% calculate relative velocity in body frame
relVelBody = transpose(Tbn)*[vn;ve;vd];
save('temp1.mat','relVelBody','R_VEL');
% calculate the observation Jacobian for the X axis
H_VELX = jacobian(relVelBody(1),stateVector); % measurement Jacobian
H_VELX = simplify(H_VELX);
save('temp2.mat','H_VELX');
ccode(H_VELX,'file','H_VELX.c');
fix_c_code('H_VELX.c');
clear all;
reset(symengine);
load('StatePrediction.mat');
load('temp1.mat');
% calculate the observation Jacobian for the Y axis
H_VELY = jacobian(relVelBody(2),stateVector); % measurement Jacobian
H_VELY = simplify(H_VELY);
save('temp3.mat','H_VELY');
ccode(H_VELY,'file','H_VELY.c');
fix_c_code('H_VELY.c');
clear all;
reset(symengine);
load('StatePrediction.mat');
load('temp1.mat');
% calculate the observation Jacobian for the Z axis
H_VELZ = jacobian(relVelBody(3),stateVector); % measurement Jacobian
H_VELZ = simplify(H_VELZ);
save('temp4.mat','H_VELZ');
ccode(H_VELZ,'file','H_VELZ.c');
fix_c_code('H_VELZ.c');
clear all;
reset(symengine);
% calculate Kalman gain vector for the X axis
load('StatePrediction.mat');
load('temp1.mat');
load('temp2.mat');
K_VELX = (P*transpose(H_VELX))/(H_VELX*P*transpose(H_VELX) + R_VEL); % Kalman gain vector
K_VELX = simplify(K_VELX);
ccode(K_VELX,'file','K_VELX.c');
fix_c_code('K_VELX.c');
clear all;
reset(symengine);
% calculate Kalman gain vector for the Y axis
load('StatePrediction.mat');
load('temp1.mat');
load('temp3.mat');
K_VELY = (P*transpose(H_VELY))/(H_VELY*P*transpose(H_VELY) + R_VEL); % Kalman gain vector
K_VELY = simplify(K_VELY);
ccode(K_VELY,'file','K_VELY.c');
fix_c_code('K_VELY.c');
clear all;
reset(symengine);
% calculate Kalman gain vector for the Z axis
load('StatePrediction.mat');
load('temp1.mat');
load('temp4.mat');
K_VELZ = (P*transpose(H_VELZ))/(H_VELZ*P*transpose(H_VELZ) + R_VEL); % Kalman gain vector
K_VELZ = simplify(K_VELZ);
ccode(K_VELZ,'file','K_VELZ.c');
fix_c_code('K_VELZ.c');
% reset workspace
clear all;
reset(symengine);
% calculate Kalman gains vectors for X,Y,Z to take advantage of common
% terms
load('StatePrediction.mat');
load('temp1.mat');
load('temp2.mat');
load('temp3.mat');
load('temp4.mat');
K_VELX = (P*transpose(H_VELX))/(H_VELX*P*transpose(H_VELX) + R_VEL); % Kalman gain vector
K_VELY = (P*transpose(H_VELY))/(H_VELY*P*transpose(H_VELY) + R_VEL); % Kalman gain vector
K_VELZ = (P*transpose(H_VELZ))/(H_VELZ*P*transpose(H_VELZ) + R_VEL); % Kalman gain vector
K_VEL = simplify([K_VELX,K_VELY,K_VELZ]);
ccode(K_VEL,'file','K_VEL.c');
fix_c_code('K_VEL.c');
%% derive equations for fusion of 321 sequence yaw measurement
load('StatePrediction.mat');
% Calculate the yaw (first rotation) angle from the 321 rotation sequence
angMeas = atan(Tbn(2,1)/Tbn(1,1));
H_YAW321 = jacobian(angMeas,stateVector); % measurement Jacobian
H_YAW321 = simplify(H_YAW321);
ccode(H_YAW321,'file','calcH_YAW321.c');
fix_c_code('calcH_YAW321.c');
% reset workspace
clear all;
reset(symengine);
%% derive equations for fusion of 312 sequence yaw measurement
load('StatePrediction.mat');
% Calculate the yaw (first rotation) angle from an Euler 312 sequence
angMeas = atan(-Tbn(1,2)/Tbn(2,2));
H_YAW312 = jacobian(angMeas,stateVector); % measurement Jacobianclea
H_YAW312 = simplify(H_YAW312);
ccode(H_YAW312,'file','calcH_YAW312.c');
fix_c_code('calcH_YAW312.c');
% reset workspace
clear all;
reset(symengine);
%% derive equations for fusion of declination
load('StatePrediction.mat');
% the predicted measurement is the angle wrt magnetic north of the horizontal
% component of the measured field
angMeas = atan(magE/magN);
H_MAGD = jacobian(angMeas,stateVector); % measurement Jacobian
H_MAGD = simplify(H_MAGD);
K_MAGD = (P*transpose(H_MAGD))/(H_MAGD*P*transpose(H_MAGD) + R_DECL);
K_MAGD = simplify(K_MAGD);
ccode([K_MAGD,H_MAGD'],'file','calcMAGD.c');
fix_c_code('calcMAGD.c');
% reset workspace
clear all;
reset(symengine);
%% derive equations for fusion of lateral body acceleration (multirotors only)
load('StatePrediction.mat');
% 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 = -Kaccx*vrel(1); % predicted acceleration measured along X body axis
accYpred = -Kaccy*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 = simplify(H_ACCX);
[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);
[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 = simplify(H_ACCY);
[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
% save equations and reset workspace
save('Drag.mat','SH_ACCX','H_ACCX','SK_ACCX','K_ACCX','SH_ACCY','H_ACCY','SK_ACCY','K_ACCY');
clear all;
reset(symengine);
%% Save output and convert to m and c code fragments
% load equations for predictions and updates
load('StateAndCovariancePrediction.mat');
load('Airspeed.mat');
load('Sideslip.mat');
load('Magnetometer.mat');
load('Drag.mat');
fileName = strcat('SymbolicOutput',int2str(nStates),'.mat');
save(fileName);
SaveScriptCode(nStates);
ConvertToM(nStates);
ConvertToC(nStates);
EKF/matlab/scripts/Inertial Nav EKF/H_LOSX.c
+8
View File
@@ -0,0 +1,8 @@
t2 = 1.0/range;
A0[0][0] = t2*(q1*vd*2.0+q0*ve*2.0-q3*vn*2.0);
A0[0][1] = t2*(q0*vd*2.0-q1*ve*2.0+q2*vn*2.0);
A0[0][2] = t2*(q3*vd*2.0+q2*ve*2.0+q1*vn*2.0);
A0[0][3] = -t2*(q2*vd*-2.0+q3*ve*2.0+q0*vn*2.0);
A0[0][4] = -t2*(q0*q3*2.0-q1*q2*2.0);
A0[0][5] = t2*(q0*q0-q1*q1+q2*q2-q3*q3);
A0[0][6] = t2*(q0*q1*2.0+q2*q3*2.0);
EKF/matlab/scripts/Inertial Nav EKF/H_LOSY.c
+8
View File
@@ -0,0 +1,8 @@
t2 = 1.0/range;
A0[0][0] = -t2*(q2*vd*-2.0+q3*ve*2.0+q0*vn*2.0);
A0[0][1] = -t2*(q3*vd*2.0+q2*ve*2.0+q1*vn*2.0);
A0[0][2] = t2*(q0*vd*2.0-q1*ve*2.0+q2*vn*2.0);
A0[0][3] = -t2*(q1*vd*2.0+q0*ve*2.0-q3*vn*2.0);
A0[0][4] = -t2*(q0*q0+q1*q1-q2*q2-q3*q3);
A0[0][5] = -t2*(q0*q3*2.0+q1*q2*2.0);
A0[0][6] = t2*(q0*q2*2.0-q1*q3*2.0);
EKF/matlab/scripts/Inertial Nav EKF/H_VELX.c
+7
View File
@@ -0,0 +1,7 @@
H_VEL[0] = q2*vd*-2.0f+q3*ve*2.0f+q0*vn*2.0f;
H_VEL[1] = q3*vd*2.0f+q2*ve*2.0f+q1*vn*2.0f;
H_VEL[2] = q0*vd*-2.0f+q1*ve*2.0f-q2*vn*2.0f;
H_VEL[3] = q1*vd*2.0f+q0*ve*2.0f-q3*vn*2.0f;
H_VEL[4] = q0*q0+q1*q1-q2*q2-q3*q3;
H_VEL[5] = q0*q3*2.0f+q1*q2*2.0f;
H_VEL[6] = q0*q2*-2.0f+q1*q3*2.0f;
EKF/matlab/scripts/Inertial Nav EKF/H_VELY.c
+7
View File
@@ -0,0 +1,7 @@
H_VEL[0] = q1*vd*2.0f+q0*ve*2.0f-q3*vn*2.0f;
H_VEL[1] = q0*vd*2.0f-q1*ve*2.0f+q2*vn*2.0f;
H_VEL[2] = q3*vd*2.0f+q2*ve*2.0f+q1*vn*2.0f;
H_VEL[3] = q2*vd*2.0f-q3*ve*2.0f-q0*vn*2.0f;
H_VEL[4] = q0*q3*-2.0f+q1*q2*2.0f;
H_VEL[5] = q0*q0-q1*q1+q2*q2-q3*q3;
H_VEL[6] = q0*q1*2.0f+q2*q3*2.0f;
EKF/matlab/scripts/Inertial Nav EKF/H_VELZ.c
+7
View File
@@ -0,0 +1,7 @@
H_VEL[0] = q0*vd*2.0f-q1*ve*2.0f+q2*vn*2.0f;
H_VEL[1] = q1*vd*-2.0f-q0*ve*2.0f+q3*vn*2.0f;
H_VEL[2] = q2*vd*-2.0f+q3*ve*2.0f+q0*vn*2.0f;
H_VEL[3] = q3*vd*2.0f+q2*ve*2.0f+q1*vn*2.0f;
H_VEL[4] = q0*q2*2.0f+q1*q3*2.0f;
H_VEL[5] = q0*q1*-2.0f+q2*q3*2.0f;
H_VEL[6] = q0*q0-q1*q1-q2*q2+q3*q3;
EKF/matlab/scripts/Inertial Nav EKF/K_LOSX.c
+117
View File
@@ -0,0 +1,117 @@
t2 = 1.0/range;
t3 = q1*vd*2.0;
t4 = q0*ve*2.0;
t11 = q3*vn*2.0;
t5 = t3+t4-t11;
t6 = q0*q3*2.0;
t29 = q1*q2*2.0;
t7 = t6-t29;
t8 = q0*q1*2.0;
t9 = q2*q3*2.0;
t10 = t8+t9;
t12 = P[0][0]*t2*t5;
t13 = q0*vd*2.0;
t14 = q2*vn*2.0;
t28 = q1*ve*2.0;
t15 = t13+t14-t28;
t16 = q3*vd*2.0;
t17 = q2*ve*2.0;
t18 = q1*vn*2.0;
t19 = t16+t17+t18;
t20 = q3*ve*2.0;
t21 = q0*vn*2.0;
t30 = q2*vd*2.0;
t22 = t20+t21-t30;
t23 = q0*q0;
t24 = q1*q1;
t25 = q2*q2;
t26 = q3*q3;
t27 = t23-t24+t25-t26;
t31 = P[1][1]*t2*t15;
t32 = P[6][0]*t2*t10;
t33 = P[1][0]*t2*t15;
t34 = P[2][0]*t2*t19;
t35 = P[5][0]*t2*t27;
t79 = P[4][0]*t2*t7;
t80 = P[3][0]*t2*t22;
t36 = t12+t32+t33+t34+t35-t79-t80;
t37 = t2*t5*t36;
t38 = P[6][1]*t2*t10;
t39 = P[0][1]*t2*t5;
t40 = P[2][1]*t2*t19;
t41 = P[5][1]*t2*t27;
t81 = P[4][1]*t2*t7;
t82 = P[3][1]*t2*t22;
t42 = t31+t38+t39+t40+t41-t81-t82;
t43 = t2*t15*t42;
t44 = P[6][2]*t2*t10;
t45 = P[0][2]*t2*t5;
t46 = P[1][2]*t2*t15;
t47 = P[2][2]*t2*t19;
t48 = P[5][2]*t2*t27;
t83 = P[4][2]*t2*t7;
t84 = P[3][2]*t2*t22;
t49 = t44+t45+t46+t47+t48-t83-t84;
t50 = t2*t19*t49;
t51 = P[6][3]*t2*t10;
t52 = P[0][3]*t2*t5;
t53 = P[1][3]*t2*t15;
t54 = P[2][3]*t2*t19;
t55 = P[5][3]*t2*t27;
t85 = P[4][3]*t2*t7;
t86 = P[3][3]*t2*t22;
t56 = t51+t52+t53+t54+t55-t85-t86;
t57 = P[6][5]*t2*t10;
t58 = P[0][5]*t2*t5;
t59 = P[1][5]*t2*t15;
t60 = P[2][5]*t2*t19;
t61 = P[5][5]*t2*t27;
t88 = P[4][5]*t2*t7;
t89 = P[3][5]*t2*t22;
t62 = t57+t58+t59+t60+t61-t88-t89;
t63 = t2*t27*t62;
t64 = P[6][4]*t2*t10;
t65 = P[0][4]*t2*t5;
t66 = P[1][4]*t2*t15;
t67 = P[2][4]*t2*t19;
t68 = P[5][4]*t2*t27;
t90 = P[4][4]*t2*t7;
t91 = P[3][4]*t2*t22;
t69 = t64+t65+t66+t67+t68-t90-t91;
t70 = P[6][6]*t2*t10;
t71 = P[0][6]*t2*t5;
t72 = P[1][6]*t2*t15;
t73 = P[2][6]*t2*t19;
t74 = P[5][6]*t2*t27;
t93 = P[4][6]*t2*t7;
t94 = P[3][6]*t2*t22;
t75 = t70+t71+t72+t73+t74-t93-t94;
t76 = t2*t10*t75;
t87 = t2*t22*t56;
t92 = t2*t7*t69;
t77 = R_LOS+t37+t43+t50+t63+t76-t87-t92;
t78 = 1.0/t77;
A0[0][0] = t78*(t12-P[0][4]*t2*t7+P[0][1]*t2*t15+P[0][6]*t2*t10+P[0][2]*t2*t19-P[0][3]*t2*t22+P[0][5]*t2*t27);
A0[1][0] = t78*(t31+P[1][0]*t2*t5-P[1][4]*t2*t7+P[1][6]*t2*t10+P[1][2]*t2*t19-P[1][3]*t2*t22+P[1][5]*t2*t27);
A0[2][0] = t78*(t47+P[2][0]*t2*t5-P[2][4]*t2*t7+P[2][1]*t2*t15+P[2][6]*t2*t10-P[2][3]*t2*t22+P[2][5]*t2*t27);
A0[3][0] = t78*(-t86+P[3][0]*t2*t5-P[3][4]*t2*t7+P[3][1]*t2*t15+P[3][6]*t2*t10+P[3][2]*t2*t19+P[3][5]*t2*t27);
A0[4][0] = t78*(-t90+P[4][0]*t2*t5+P[4][1]*t2*t15+P[4][6]*t2*t10+P[4][2]*t2*t19-P[4][3]*t2*t22+P[4][5]*t2*t27);
A0[5][0] = t78*(t61+P[5][0]*t2*t5-P[5][4]*t2*t7+P[5][1]*t2*t15+P[5][6]*t2*t10+P[5][2]*t2*t19-P[5][3]*t2*t22);
A0[6][0] = t78*(t70+P[6][0]*t2*t5-P[6][4]*t2*t7+P[6][1]*t2*t15+P[6][2]*t2*t19-P[6][3]*t2*t22+P[6][5]*t2*t27);
A0[7][0] = t78*(P[7][0]*t2*t5-P[7][4]*t2*t7+P[7][1]*t2*t15+P[7][6]*t2*t10+P[7][2]*t2*t19-P[7][3]*t2*t22+P[7][5]*t2*t27);
A0[8][0] = t78*(P[8][0]*t2*t5-P[8][4]*t2*t7+P[8][1]*t2*t15+P[8][6]*t2*t10+P[8][2]*t2*t19-P[8][3]*t2*t22+P[8][5]*t2*t27);
A0[9][0] = t78*(P[9][0]*t2*t5-P[9][4]*t2*t7+P[9][1]*t2*t15+P[9][6]*t2*t10+P[9][2]*t2*t19-P[9][3]*t2*t22+P[9][5]*t2*t27);
A0[10][0] = t78*(P[10][0]*t2*t5-P[10][4]*t2*t7+P[10][1]*t2*t15+P[10][6]*t2*t10+P[10][2]*t2*t19-P[10][3]*t2*t22+P[10][5]*t2*t27);
A0[11][0] = t78*(P[11][0]*t2*t5-P[11][4]*t2*t7+P[11][1]*t2*t15+P[11][6]*t2*t10+P[11][2]*t2*t19-P[11][3]*t2*t22+P[11][5]*t2*t27);
A0[12][0] = t78*(P[12][0]*t2*t5-P[12][4]*t2*t7+P[12][1]*t2*t15+P[12][6]*t2*t10+P[12][2]*t2*t19-P[12][3]*t2*t22+P[12][5]*t2*t27);
A0[13][0] = t78*(P[13][0]*t2*t5-P[13][4]*t2*t7+P[13][1]*t2*t15+P[13][6]*t2*t10+P[13][2]*t2*t19-P[13][3]*t2*t22+P[13][5]*t2*t27);
A0[14][0] = t78*(P[14][0]*t2*t5-P[14][4]*t2*t7+P[14][1]*t2*t15+P[14][6]*t2*t10+P[14][2]*t2*t19-P[14][3]*t2*t22+P[14][5]*t2*t27);
A0[15][0] = t78*(P[15][0]*t2*t5-P[15][4]*t2*t7+P[15][1]*t2*t15+P[15][6]*t2*t10+P[15][2]*t2*t19-P[15][3]*t2*t22+P[15][5]*t2*t27);
A0[16][0] = t78*(P[16][0]*t2*t5-P[16][4]*t2*t7+P[16][1]*t2*t15+P[16][6]*t2*t10+P[16][2]*t2*t19-P[16][3]*t2*t22+P[16][5]*t2*t27);
A0[17][0] = t78*(P[17][0]*t2*t5-P[17][4]*t2*t7+P[17][1]*t2*t15+P[17][6]*t2*t10+P[17][2]*t2*t19-P[17][3]*t2*t22+P[17][5]*t2*t27);
A0[18][0] = t78*(P[18][0]*t2*t5-P[18][4]*t2*t7+P[18][1]*t2*t15+P[18][6]*t2*t10+P[18][2]*t2*t19-P[18][3]*t2*t22+P[18][5]*t2*t27);
A0[19][0] = t78*(P[19][0]*t2*t5-P[19][4]*t2*t7+P[19][1]*t2*t15+P[19][6]*t2*t10+P[19][2]*t2*t19-P[19][3]*t2*t22+P[19][5]*t2*t27);
A0[20][0] = t78*(P[20][0]*t2*t5-P[20][4]*t2*t7+P[20][1]*t2*t15+P[20][6]*t2*t10+P[20][2]*t2*t19-P[20][3]*t2*t22+P[20][5]*t2*t27);
A0[21][0] = t78*(P[21][0]*t2*t5-P[21][4]*t2*t7+P[21][1]*t2*t15+P[21][6]*t2*t10+P[21][2]*t2*t19-P[21][3]*t2*t22+P[21][5]*t2*t27);
A0[22][0] = t78*(P[22][0]*t2*t5-P[22][4]*t2*t7+P[22][1]*t2*t15+P[22][6]*t2*t10+P[22][2]*t2*t19-P[22][3]*t2*t22+P[22][5]*t2*t27);
A0[23][0] = t78*(P[23][0]*t2*t5-P[23][4]*t2*t7+P[23][1]*t2*t15+P[23][6]*t2*t10+P[23][2]*t2*t19-P[23][3]*t2*t22+P[23][5]*t2*t27);
EKF/matlab/scripts/Inertial Nav EKF/K_LOSY.c
+117
View File
@@ -0,0 +1,117 @@
t2 = 1.0/range;
t3 = q3*ve*2.0;
t4 = q0*vn*2.0;
t11 = q2*vd*2.0;
t5 = t3+t4-t11;
t6 = q0*q3*2.0;
t7 = q1*q2*2.0;
t8 = t6+t7;
t9 = q0*q2*2.0;
t28 = q1*q3*2.0;
t10 = t9-t28;
t12 = P[0][0]*t2*t5;
t13 = q3*vd*2.0;
t14 = q2*ve*2.0;
t15 = q1*vn*2.0;
t16 = t13+t14+t15;
t17 = q0*vd*2.0;
t18 = q2*vn*2.0;
t29 = q1*ve*2.0;
t19 = t17+t18-t29;
t20 = q1*vd*2.0;
t21 = q0*ve*2.0;
t30 = q3*vn*2.0;
t22 = t20+t21-t30;
t23 = q0*q0;
t24 = q1*q1;
t25 = q2*q2;
t26 = q3*q3;
t27 = t23+t24-t25-t26;
t31 = P[1][1]*t2*t16;
t32 = P[5][0]*t2*t8;
t33 = P[1][0]*t2*t16;
t34 = P[3][0]*t2*t22;
t35 = P[4][0]*t2*t27;
t80 = P[6][0]*t2*t10;
t81 = P[2][0]*t2*t19;
t36 = t12+t32+t33+t34+t35-t80-t81;
t37 = t2*t5*t36;
t38 = P[5][1]*t2*t8;
t39 = P[0][1]*t2*t5;
t40 = P[3][1]*t2*t22;
t41 = P[4][1]*t2*t27;
t82 = P[6][1]*t2*t10;
t83 = P[2][1]*t2*t19;
t42 = t31+t38+t39+t40+t41-t82-t83;
t43 = t2*t16*t42;
t44 = P[5][2]*t2*t8;
t45 = P[0][2]*t2*t5;
t46 = P[1][2]*t2*t16;
t47 = P[3][2]*t2*t22;
t48 = P[4][2]*t2*t27;
t79 = P[2][2]*t2*t19;
t84 = P[6][2]*t2*t10;
t49 = t44+t45+t46+t47+t48-t79-t84;
t50 = P[5][3]*t2*t8;
t51 = P[0][3]*t2*t5;
t52 = P[1][3]*t2*t16;
t53 = P[3][3]*t2*t22;
t54 = P[4][3]*t2*t27;
t86 = P[6][3]*t2*t10;
t87 = P[2][3]*t2*t19;
t55 = t50+t51+t52+t53+t54-t86-t87;
t56 = t2*t22*t55;
t57 = P[5][4]*t2*t8;
t58 = P[0][4]*t2*t5;
t59 = P[1][4]*t2*t16;
t60 = P[3][4]*t2*t22;
t61 = P[4][4]*t2*t27;
t88 = P[6][4]*t2*t10;
t89 = P[2][4]*t2*t19;
t62 = t57+t58+t59+t60+t61-t88-t89;
t63 = t2*t27*t62;
t64 = P[5][5]*t2*t8;
t65 = P[0][5]*t2*t5;
t66 = P[1][5]*t2*t16;
t67 = P[3][5]*t2*t22;
t68 = P[4][5]*t2*t27;
t90 = P[6][5]*t2*t10;
t91 = P[2][5]*t2*t19;
t69 = t64+t65+t66+t67+t68-t90-t91;
t70 = t2*t8*t69;
t71 = P[5][6]*t2*t8;
t72 = P[0][6]*t2*t5;
t73 = P[1][6]*t2*t16;
t74 = P[3][6]*t2*t22;
t75 = P[4][6]*t2*t27;
t92 = P[6][6]*t2*t10;
t93 = P[2][6]*t2*t19;
t76 = t71+t72+t73+t74+t75-t92-t93;
t85 = t2*t19*t49;
t94 = t2*t10*t76;
t77 = R_LOS+t37+t43+t56+t63+t70-t85-t94;
t78 = 1.0/t77;
A0[0][0] = -t78*(t12+P[0][5]*t2*t8-P[0][6]*t2*t10+P[0][1]*t2*t16-P[0][2]*t2*t19+P[0][3]*t2*t22+P[0][4]*t2*t27);
A0[1][0] = -t78*(t31+P[1][0]*t2*t5+P[1][5]*t2*t8-P[1][6]*t2*t10-P[1][2]*t2*t19+P[1][3]*t2*t22+P[1][4]*t2*t27);
A0[2][0] = -t78*(-t79+P[2][0]*t2*t5+P[2][5]*t2*t8-P[2][6]*t2*t10+P[2][1]*t2*t16+P[2][3]*t2*t22+P[2][4]*t2*t27);
A0[3][0] = -t78*(t53+P[3][0]*t2*t5+P[3][5]*t2*t8-P[3][6]*t2*t10+P[3][1]*t2*t16-P[3][2]*t2*t19+P[3][4]*t2*t27);
A0[4][0] = -t78*(t61+P[4][0]*t2*t5+P[4][5]*t2*t8-P[4][6]*t2*t10+P[4][1]*t2*t16-P[4][2]*t2*t19+P[4][3]*t2*t22);
A0[5][0] = -t78*(t64+P[5][0]*t2*t5-P[5][6]*t2*t10+P[5][1]*t2*t16-P[5][2]*t2*t19+P[5][3]*t2*t22+P[5][4]*t2*t27);
A0[6][0] = -t78*(-t92+P[6][0]*t2*t5+P[6][5]*t2*t8+P[6][1]*t2*t16-P[6][2]*t2*t19+P[6][3]*t2*t22+P[6][4]*t2*t27);
A0[7][0] = -t78*(P[7][0]*t2*t5+P[7][5]*t2*t8-P[7][6]*t2*t10+P[7][1]*t2*t16-P[7][2]*t2*t19+P[7][3]*t2*t22+P[7][4]*t2*t27);
A0[8][0] = -t78*(P[8][0]*t2*t5+P[8][5]*t2*t8-P[8][6]*t2*t10+P[8][1]*t2*t16-P[8][2]*t2*t19+P[8][3]*t2*t22+P[8][4]*t2*t27);
A0[9][0] = -t78*(P[9][0]*t2*t5+P[9][5]*t2*t8-P[9][6]*t2*t10+P[9][1]*t2*t16-P[9][2]*t2*t19+P[9][3]*t2*t22+P[9][4]*t2*t27);
A0[10][0] = -t78*(P[10][0]*t2*t5+P[10][5]*t2*t8-P[10][6]*t2*t10+P[10][1]*t2*t16-P[10][2]*t2*t19+P[10][3]*t2*t22+P[10][4]*t2*t27);
A0[11][0] = -t78*(P[11][0]*t2*t5+P[11][5]*t2*t8-P[11][6]*t2*t10+P[11][1]*t2*t16-P[11][2]*t2*t19+P[11][3]*t2*t22+P[11][4]*t2*t27);
A0[12][0] = -t78*(P[12][0]*t2*t5+P[12][5]*t2*t8-P[12][6]*t2*t10+P[12][1]*t2*t16-P[12][2]*t2*t19+P[12][3]*t2*t22+P[12][4]*t2*t27);
A0[13][0] = -t78*(P[13][0]*t2*t5+P[13][5]*t2*t8-P[13][6]*t2*t10+P[13][1]*t2*t16-P[13][2]*t2*t19+P[13][3]*t2*t22+P[13][4]*t2*t27);
A0[14][0] = -t78*(P[14][0]*t2*t5+P[14][5]*t2*t8-P[14][6]*t2*t10+P[14][1]*t2*t16-P[14][2]*t2*t19+P[14][3]*t2*t22+P[14][4]*t2*t27);
A0[15][0] = -t78*(P[15][0]*t2*t5+P[15][5]*t2*t8-P[15][6]*t2*t10+P[15][1]*t2*t16-P[15][2]*t2*t19+P[15][3]*t2*t22+P[15][4]*t2*t27);
A0[16][0] = -t78*(P[16][0]*t2*t5+P[16][5]*t2*t8-P[16][6]*t2*t10+P[16][1]*t2*t16-P[16][2]*t2*t19+P[16][3]*t2*t22+P[16][4]*t2*t27);
A0[17][0] = -t78*(P[17][0]*t2*t5+P[17][5]*t2*t8-P[17][6]*t2*t10+P[17][1]*t2*t16-P[17][2]*t2*t19+P[17][3]*t2*t22+P[17][4]*t2*t27);
A0[18][0] = -t78*(P[18][0]*t2*t5+P[18][5]*t2*t8-P[18][6]*t2*t10+P[18][1]*t2*t16-P[18][2]*t2*t19+P[18][3]*t2*t22+P[18][4]*t2*t27);
A0[19][0] = -t78*(P[19][0]*t2*t5+P[19][5]*t2*t8-P[19][6]*t2*t10+P[19][1]*t2*t16-P[19][2]*t2*t19+P[19][3]*t2*t22+P[19][4]*t2*t27);
A0[20][0] = -t78*(P[20][0]*t2*t5+P[20][5]*t2*t8-P[20][6]*t2*t10+P[20][1]*t2*t16-P[20][2]*t2*t19+P[20][3]*t2*t22+P[20][4]*t2*t27);
A0[21][0] = -t78*(P[21][0]*t2*t5+P[21][5]*t2*t8-P[21][6]*t2*t10+P[21][1]*t2*t16-P[21][2]*t2*t19+P[21][3]*t2*t22+P[21][4]*t2*t27);
A0[22][0] = -t78*(P[22][0]*t2*t5+P[22][5]*t2*t8-P[22][6]*t2*t10+P[22][1]*t2*t16-P[22][2]*t2*t19+P[22][3]*t2*t22+P[22][4]*t2*t27);
A0[23][0] = -t78*(P[23][0]*t2*t5+P[23][5]*t2*t8-P[23][6]*t2*t10+P[23][1]*t2*t16-P[23][2]*t2*t19+P[23][3]*t2*t22+P[23][4]*t2*t27);
EKF/matlab/scripts/Inertial Nav EKF/K_VELX.c
+116
View File
@@ -0,0 +1,116 @@
float t2 = q0*q3*2.0f;
float t3 = q1*q2*2.0f;
float t4 = t2+t3;
float t5 = q0*q0;
float t6 = q1*q1;
float t7 = q2*q2;
float t8 = q3*q3;
float t9 = t5+t6-t7-t8;
float t10 = q0*q2*2.0f;
float t25 = q1*q3*2.0f;
float t11 = t10-t25;
float t12 = q3*ve*2.0f;
float t13 = q0*vn*2.0f;
float t26 = q2*vd*2.0f;
float t14 = t12+t13-t26;
float t15 = q3*vd*2.0f;
float t16 = q2*ve*2.0f;
float t17 = q1*vn*2.0f;
float t18 = t15+t16+t17;
float t19 = q0*vd*2.0f;
float t20 = q2*vn*2.0f;
float t27 = q1*ve*2.0f;
float t21 = t19+t20-t27;
float t22 = q1*vd*2.0f;
float t23 = q0*ve*2.0f;
float t28 = q3*vn*2.0f;
float t24 = t22+t23-t28;
float t29 = P[0][0]*t14;
float t30 = P[1][1]*t18;
float t31 = P[4][5]*t9;
float t32 = P[5][5]*t4;
float t33 = P[0][5]*t14;
float t34 = P[1][5]*t18;
float t35 = P[3][5]*t24;
float t79 = P[6][5]*t11;
float t80 = P[2][5]*t21;
float t36 = t31+t32+t33+t34+t35-t79-t80;
float t37 = t4*t36;
float t38 = P[4][6]*t9;
float t39 = P[5][6]*t4;
float t40 = P[0][6]*t14;
float t41 = P[1][6]*t18;
float t42 = P[3][6]*t24;
float t81 = P[6][6]*t11;
float t82 = P[2][6]*t21;
float t43 = t38+t39+t40+t41+t42-t81-t82;
float t44 = P[4][0]*t9;
float t45 = P[5][0]*t4;
float t46 = P[1][0]*t18;
float t47 = P[3][0]*t24;
float t84 = P[6][0]*t11;
float t85 = P[2][0]*t21;
float t48 = t29+t44+t45+t46+t47-t84-t85;
float t49 = t14*t48;
float t50 = P[4][1]*t9;
float t51 = P[5][1]*t4;
float t52 = P[0][1]*t14;
float t53 = P[3][1]*t24;
float t86 = P[6][1]*t11;
float t87 = P[2][1]*t21;
float t54 = t30+t50+t51+t52+t53-t86-t87;
float t55 = t18*t54;
float t56 = P[4][2]*t9;
float t57 = P[5][2]*t4;
float t58 = P[0][2]*t14;
float t59 = P[1][2]*t18;
float t60 = P[3][2]*t24;
float t78 = P[2][2]*t21;
float t88 = P[6][2]*t11;
float t61 = t56+t57+t58+t59+t60-t78-t88;
float t62 = P[4][3]*t9;
float t63 = P[5][3]*t4;
float t64 = P[0][3]*t14;
float t65 = P[1][3]*t18;
float t66 = P[3][3]*t24;
float t90 = P[6][3]*t11;
float t91 = P[2][3]*t21;
float t67 = t62+t63+t64+t65+t66-t90-t91;
float t68 = t24*t67;
float t69 = P[4][4]*t9;
float t70 = P[5][4]*t4;
float t71 = P[0][4]*t14;
float t72 = P[1][4]*t18;
float t73 = P[3][4]*t24;
float t92 = P[6][4]*t11;
float t93 = P[2][4]*t21;
float t74 = t69+t70+t71+t72+t73-t92-t93;
float t75 = t9*t74;
float t83 = t11*t43;
float t89 = t21*t61;
float t76 = R_VEL+t37+t49+t55+t68+t75-t83-t89;
float t77 = 1.0f/t76;
Kfusion[0] = t77*(t29+P[0][5]*t4+P[0][4]*t9-P[0][6]*t11+P[0][1]*t18-P[0][2]*t21+P[0][3]*t24);
Kfusion[1] = t77*(t30+P[1][5]*t4+P[1][4]*t9+P[1][0]*t14-P[1][6]*t11-P[1][2]*t21+P[1][3]*t24);
Kfusion[2] = t77*(-t78+P[2][5]*t4+P[2][4]*t9+P[2][0]*t14-P[2][6]*t11+P[2][1]*t18+P[2][3]*t24);
Kfusion[3] = t77*(t66+P[3][5]*t4+P[3][4]*t9+P[3][0]*t14-P[3][6]*t11+P[3][1]*t18-P[3][2]*t21);
Kfusion[4] = t77*(t69+P[4][5]*t4+P[4][0]*t14-P[4][6]*t11+P[4][1]*t18-P[4][2]*t21+P[4][3]*t24);
Kfusion[5] = t77*(t32+P[5][4]*t9+P[5][0]*t14-P[5][6]*t11+P[5][1]*t18-P[5][2]*t21+P[5][3]*t24);
Kfusion[6] = t77*(-t81+P[6][5]*t4+P[6][4]*t9+P[6][0]*t14+P[6][1]*t18-P[6][2]*t21+P[6][3]*t24);
Kfusion[7] = t77*(P[7][5]*t4+P[7][4]*t9+P[7][0]*t14-P[7][6]*t11+P[7][1]*t18-P[7][2]*t21+P[7][3]*t24);
Kfusion[8] = t77*(P[8][5]*t4+P[8][4]*t9+P[8][0]*t14-P[8][6]*t11+P[8][1]*t18-P[8][2]*t21+P[8][3]*t24);
Kfusion[9] = t77*(P[9][5]*t4+P[9][4]*t9+P[9][0]*t14-P[9][6]*t11+P[9][1]*t18-P[9][2]*t21+P[9][3]*t24);
Kfusion[10] = t77*(P[10][5]*t4+P[10][4]*t9+P[10][0]*t14-P[10][6]*t11+P[10][1]*t18-P[10][2]*t21+P[10][3]*t24);
Kfusion[11] = t77*(P[11][5]*t4+P[11][4]*t9+P[11][0]*t14-P[11][6]*t11+P[11][1]*t18-P[11][2]*t21+P[11][3]*t24);
Kfusion[12] = t77*(P[12][5]*t4+P[12][4]*t9+P[12][0]*t14-P[12][6]*t11+P[12][1]*t18-P[12][2]*t21+P[12][3]*t24);
Kfusion[13] = t77*(P[13][5]*t4+P[13][4]*t9+P[13][0]*t14-P[13][6]*t11+P[13][1]*t18-P[13][2]*t21+P[13][3]*t24);
Kfusion[14] = t77*(P[14][5]*t4+P[14][4]*t9+P[14][0]*t14-P[14][6]*t11+P[14][1]*t18-P[14][2]*t21+P[14][3]*t24);
Kfusion[15] = t77*(P[15][5]*t4+P[15][4]*t9+P[15][0]*t14-P[15][6]*t11+P[15][1]*t18-P[15][2]*t21+P[15][3]*t24);
Kfusion[16] = t77*(P[16][5]*t4+P[16][4]*t9+P[16][0]*t14-P[16][6]*t11+P[16][1]*t18-P[16][2]*t21+P[16][3]*t24);
Kfusion[17] = t77*(P[17][5]*t4+P[17][4]*t9+P[17][0]*t14-P[17][6]*t11+P[17][1]*t18-P[17][2]*t21+P[17][3]*t24);
Kfusion[18] = t77*(P[18][5]*t4+P[18][4]*t9+P[18][0]*t14-P[18][6]*t11+P[18][1]*t18-P[18][2]*t21+P[18][3]*t24);
Kfusion[19] = t77*(P[19][5]*t4+P[19][4]*t9+P[19][0]*t14-P[19][6]*t11+P[19][1]*t18-P[19][2]*t21+P[19][3]*t24);
Kfusion[20] = t77*(P[20][5]*t4+P[20][4]*t9+P[20][0]*t14-P[20][6]*t11+P[20][1]*t18-P[20][2]*t21+P[20][3]*t24);
Kfusion[21] = t77*(P[21][5]*t4+P[21][4]*t9+P[21][0]*t14-P[21][6]*t11+P[21][1]*t18-P[21][2]*t21+P[21][3]*t24);
Kfusion[22] = t77*(P[22][5]*t4+P[22][4]*t9+P[22][0]*t14-P[22][6]*t11+P[22][1]*t18-P[22][2]*t21+P[22][3]*t24);
Kfusion[23] = t77*(P[23][5]*t4+P[23][4]*t9+P[23][0]*t14-P[23][6]*t11+P[23][1]*t18-P[23][2]*t21+P[23][3]*t24);
EKF/matlab/scripts/Inertial Nav EKF/K_VELY.c
+116
View File
@@ -0,0 +1,116 @@
float t2 = q0*q3*2.0f;
float t9 = q1*q2*2.0f;
float t3 = t2-t9;
float t4 = q0*q0;
float t5 = q1*q1;
float t6 = q2*q2;
float t7 = q3*q3;
float t8 = t4-t5+t6-t7;
float t10 = q0*q1*2.0f;
float t11 = q2*q3*2.0f;
float t12 = t10+t11;
float t13 = q1*vd*2.0f;
float t14 = q0*ve*2.0f;
float t26 = q3*vn*2.0f;
float t15 = t13+t14-t26;
float t16 = q0*vd*2.0f;
float t17 = q2*vn*2.0f;
float t27 = q1*ve*2.0f;
float t18 = t16+t17-t27;
float t19 = q3*vd*2.0f;
float t20 = q2*ve*2.0f;
float t21 = q1*vn*2.0f;
float t22 = t19+t20+t21;
float t23 = q3*ve*2.0f;
float t24 = q0*vn*2.0f;
float t28 = q2*vd*2.0f;
float t25 = t23+t24-t28;
float t29 = P[0][0]*t15;
float t30 = P[1][1]*t18;
float t31 = P[5][4]*t8;
float t32 = P[6][4]*t12;
float t33 = P[0][4]*t15;
float t34 = P[1][4]*t18;
float t35 = P[2][4]*t22;
float t78 = P[4][4]*t3;
float t79 = P[3][4]*t25;
float t36 = t31+t32+t33+t34+t35-t78-t79;
float t37 = P[5][6]*t8;
float t38 = P[6][6]*t12;
float t39 = P[0][6]*t15;
float t40 = P[1][6]*t18;
float t41 = P[2][6]*t22;
float t81 = P[4][6]*t3;
float t82 = P[3][6]*t25;
float t42 = t37+t38+t39+t40+t41-t81-t82;
float t43 = t12*t42;
float t44 = P[5][0]*t8;
float t45 = P[6][0]*t12;
float t46 = P[1][0]*t18;
float t47 = P[2][0]*t22;
float t83 = P[4][0]*t3;
float t84 = P[3][0]*t25;
float t48 = t29+t44+t45+t46+t47-t83-t84;
float t49 = t15*t48;
float t50 = P[5][1]*t8;
float t51 = P[6][1]*t12;
float t52 = P[0][1]*t15;
float t53 = P[2][1]*t22;
float t85 = P[4][1]*t3;
float t86 = P[3][1]*t25;
float t54 = t30+t50+t51+t52+t53-t85-t86;
float t55 = t18*t54;
float t56 = P[5][2]*t8;
float t57 = P[6][2]*t12;
float t58 = P[0][2]*t15;
float t59 = P[1][2]*t18;
float t60 = P[2][2]*t22;
float t87 = P[4][2]*t3;
float t88 = P[3][2]*t25;
float t61 = t56+t57+t58+t59+t60-t87-t88;
float t62 = t22*t61;
float t63 = P[5][3]*t8;
float t64 = P[6][3]*t12;
float t65 = P[0][3]*t15;
float t66 = P[1][3]*t18;
float t67 = P[2][3]*t22;
float t89 = P[4][3]*t3;
float t90 = P[3][3]*t25;
float t68 = t63+t64+t65+t66+t67-t89-t90;
float t69 = P[5][5]*t8;
float t70 = P[6][5]*t12;
float t71 = P[0][5]*t15;
float t72 = P[1][5]*t18;
float t73 = P[2][5]*t22;
float t92 = P[4][5]*t3;
float t93 = P[3][5]*t25;
float t74 = t69+t70+t71+t72+t73-t92-t93;
float t75 = t8*t74;
float t80 = t3*t36;
float t91 = t25*t68;
float t76 = R_VEL+t43+t49+t55+t62+t75-t80-t91;
float t77 = 1.0f/t76;
Kfusion[0] = t77*(t29-P[0][4]*t3+P[0][5]*t8+P[0][6]*t12+P[0][1]*t18+P[0][2]*t22-P[0][3]*t25);
Kfusion[1] = t77*(t30-P[1][4]*t3+P[1][5]*t8+P[1][0]*t15+P[1][6]*t12+P[1][2]*t22-P[1][3]*t25);
Kfusion[2] = t77*(t60-P[2][4]*t3+P[2][5]*t8+P[2][0]*t15+P[2][6]*t12+P[2][1]*t18-P[2][3]*t25);
Kfusion[3] = t77*(-t90-P[3][4]*t3+P[3][5]*t8+P[3][0]*t15+P[3][6]*t12+P[3][1]*t18+P[3][2]*t22);
Kfusion[4] = t77*(-t78+P[4][5]*t8+P[4][0]*t15+P[4][6]*t12+P[4][1]*t18+P[4][2]*t22-P[4][3]*t25);
Kfusion[5] = t77*(t69-P[5][4]*t3+P[5][0]*t15+P[5][6]*t12+P[5][1]*t18+P[5][2]*t22-P[5][3]*t25);
Kfusion[6] = t77*(t38-P[6][4]*t3+P[6][5]*t8+P[6][0]*t15+P[6][1]*t18+P[6][2]*t22-P[6][3]*t25);
Kfusion[7] = t77*(-P[7][4]*t3+P[7][5]*t8+P[7][0]*t15+P[7][6]*t12+P[7][1]*t18+P[7][2]*t22-P[7][3]*t25);
Kfusion[8] = t77*(-P[8][4]*t3+P[8][5]*t8+P[8][0]*t15+P[8][6]*t12+P[8][1]*t18+P[8][2]*t22-P[8][3]*t25);
Kfusion[9] = t77*(-P[9][4]*t3+P[9][5]*t8+P[9][0]*t15+P[9][6]*t12+P[9][1]*t18+P[9][2]*t22-P[9][3]*t25);
Kfusion[10] = t77*(-P[10][4]*t3+P[10][5]*t8+P[10][0]*t15+P[10][6]*t12+P[10][1]*t18+P[10][2]*t22-P[10][3]*t25);
Kfusion[11] = t77*(-P[11][4]*t3+P[11][5]*t8+P[11][0]*t15+P[11][6]*t12+P[11][1]*t18+P[11][2]*t22-P[11][3]*t25);
Kfusion[12] = t77*(-P[12][4]*t3+P[12][5]*t8+P[12][0]*t15+P[12][6]*t12+P[12][1]*t18+P[12][2]*t22-P[12][3]*t25);
Kfusion[13] = t77*(-P[13][4]*t3+P[13][5]*t8+P[13][0]*t15+P[13][6]*t12+P[13][1]*t18+P[13][2]*t22-P[13][3]*t25);
Kfusion[14] = t77*(-P[14][4]*t3+P[14][5]*t8+P[14][0]*t15+P[14][6]*t12+P[14][1]*t18+P[14][2]*t22-P[14][3]*t25);
Kfusion[15] = t77*(-P[15][4]*t3+P[15][5]*t8+P[15][0]*t15+P[15][6]*t12+P[15][1]*t18+P[15][2]*t22-P[15][3]*t25);
Kfusion[16] = t77*(-P[16][4]*t3+P[16][5]*t8+P[16][0]*t15+P[16][6]*t12+P[16][1]*t18+P[16][2]*t22-P[16][3]*t25);
Kfusion[17] = t77*(-P[17][4]*t3+P[17][5]*t8+P[17][0]*t15+P[17][6]*t12+P[17][1]*t18+P[17][2]*t22-P[17][3]*t25);
Kfusion[18] = t77*(-P[18][4]*t3+P[18][5]*t8+P[18][0]*t15+P[18][6]*t12+P[18][1]*t18+P[18][2]*t22-P[18][3]*t25);
Kfusion[19] = t77*(-P[19][4]*t3+P[19][5]*t8+P[19][0]*t15+P[19][6]*t12+P[19][1]*t18+P[19][2]*t22-P[19][3]*t25);
Kfusion[20] = t77*(-P[20][4]*t3+P[20][5]*t8+P[20][0]*t15+P[20][6]*t12+P[20][1]*t18+P[20][2]*t22-P[20][3]*t25);
Kfusion[21] = t77*(-P[21][4]*t3+P[21][5]*t8+P[21][0]*t15+P[21][6]*t12+P[21][1]*t18+P[21][2]*t22-P[21][3]*t25);
Kfusion[22] = t77*(-P[22][4]*t3+P[22][5]*t8+P[22][0]*t15+P[22][6]*t12+P[22][1]*t18+P[22][2]*t22-P[22][3]*t25);
Kfusion[23] = t77*(-P[23][4]*t3+P[23][5]*t8+P[23][0]*t15+P[23][6]*t12+P[23][1]*t18+P[23][2]*t22-P[23][3]*t25);
EKF/matlab/scripts/Inertial Nav EKF/K_VELZ.c
+116
View File
@@ -0,0 +1,116 @@
float t2 = q0*q2*2.0;
float t3 = q1*q3*2.0;
float t4 = t2+t3;
float t5 = q0*q0;
float t6 = q1*q1;
float t7 = q2*q2;
float t8 = q3*q3;
float t9 = t5-t6-t7+t8;
float t10 = q0*q1*2.0;
float t25 = q2*q3*2.0;
float t11 = t10-t25;
float t12 = q0*vd*2.0;
float t13 = q2*vn*2.0;
float t26 = q1*ve*2.0;
float t14 = t12+t13-t26;
float t15 = q1*vd*2.0;
float t16 = q0*ve*2.0;
float t27 = q3*vn*2.0;
float t17 = t15+t16-t27;
float t18 = q3*ve*2.0;
float t19 = q0*vn*2.0;
float t28 = q2*vd*2.0;
float t20 = t18+t19-t28;
float t21 = q3*vd*2.0;
float t22 = q2*ve*2.0;
float t23 = q1*vn*2.0;
float t24 = t21+t22+t23;
float t29 = P[0][0]*t14;
float t30 = P[6][4]*t9;
float t31 = P[4][4]*t4;
float t32 = P[0][4]*t14;
float t33 = P[2][4]*t20;
float t34 = P[3][4]*t24;
float t78 = P[5][4]*t11;
float t79 = P[1][4]*t17;
float t35 = t30+t31+t32+t33+t34-t78-t79;
float t36 = t4*t35;
float t37 = P[6][5]*t9;
float t38 = P[4][5]*t4;
float t39 = P[0][5]*t14;
float t40 = P[2][5]*t20;
float t41 = P[3][5]*t24;
float t80 = P[5][5]*t11;
float t81 = P[1][5]*t17;
float t42 = t37+t38+t39+t40+t41-t80-t81;
float t43 = P[6][0]*t9;
float t44 = P[4][0]*t4;
float t45 = P[2][0]*t20;
float t46 = P[3][0]*t24;
float t83 = P[5][0]*t11;
float t84 = P[1][0]*t17;
float t47 = t29+t43+t44+t45+t46-t83-t84;
float t48 = t14*t47;
float t49 = P[6][1]*t9;
float t50 = P[4][1]*t4;
float t51 = P[0][1]*t14;
float t52 = P[2][1]*t20;
float t53 = P[3][1]*t24;
float t85 = P[5][1]*t11;
float t86 = P[1][1]*t17;
float t54 = t49+t50+t51+t52+t53-t85-t86;
float t55 = P[6][2]*t9;
float t56 = P[4][2]*t4;
float t57 = P[0][2]*t14;
float t58 = P[2][2]*t20;
float t59 = P[3][2]*t24;
float t88 = P[5][2]*t11;
float t89 = P[1][2]*t17;
float t60 = t55+t56+t57+t58+t59-t88-t89;
float t61 = t20*t60;
float t62 = P[6][3]*t9;
float t63 = P[4][3]*t4;
float t64 = P[0][3]*t14;
float t65 = P[2][3]*t20;
float t66 = P[3][3]*t24;
float t90 = P[5][3]*t11;
float t91 = P[1][3]*t17;
float t67 = t62+t63+t64+t65+t66-t90-t91;
float t68 = t24*t67;
float t69 = P[6][6]*t9;
float t70 = P[4][6]*t4;
float t71 = P[0][6]*t14;
float t72 = P[2][6]*t20;
float t73 = P[3][6]*t24;
float t92 = P[5][6]*t11;
float t93 = P[1][6]*t17;
float t74 = t69+t70+t71+t72+t73-t92-t93;
float t75 = t9*t74;
float t82 = t11*t42;
float t87 = t17*t54;
float t76 = R_VEL+t36+t48+t61+t68+t75-t82-t87;
float t77 = 1.0f/t76;
Kfusion[0] = t77*(t29+P[0][4]*t4+P[0][6]*t9-P[0][5]*t11-P[0][1]*t17+P[0][2]*t20+P[0][3]*t24);
Kfusion[1] = t77*(P[1][4]*t4+P[1][0]*t14+P[1][6]*t9-P[1][5]*t11-P[1][1]*t17+P[1][2]*t20+P[1][3]*t24);
Kfusion[2] = t77*(t58+P[2][4]*t4+P[2][0]*t14+P[2][6]*t9-P[2][5]*t11-P[2][1]*t17+P[2][3]*t24);
Kfusion[3] = t77*(t66+P[3][4]*t4+P[3][0]*t14+P[3][6]*t9-P[3][5]*t11-P[3][1]*t17+P[3][2]*t20);
Kfusion[4] = t77*(t31+P[4][0]*t14+P[4][6]*t9-P[4][5]*t11-P[4][1]*t17+P[4][2]*t20+P[4][3]*t24);
Kfusion[5] = t77*(-t80+P[5][4]*t4+P[5][0]*t14+P[5][6]*t9-P[5][1]*t17+P[5][2]*t20+P[5][3]*t24);
Kfusion[6] = t77*(t69+P[6][4]*t4+P[6][0]*t14-P[6][5]*t11-P[6][1]*t17+P[6][2]*t20+P[6][3]*t24);
Kfusion[7] = t77*(P[7][4]*t4+P[7][0]*t14+P[7][6]*t9-P[7][5]*t11-P[7][1]*t17+P[7][2]*t20+P[7][3]*t24);
Kfusion[8] = t77*(P[8][4]*t4+P[8][0]*t14+P[8][6]*t9-P[8][5]*t11-P[8][1]*t17+P[8][2]*t20+P[8][3]*t24);
Kfusion[9] = t77*(P[9][4]*t4+P[9][0]*t14+P[9][6]*t9-P[9][5]*t11-P[9][1]*t17+P[9][2]*t20+P[9][3]*t24);
Kfusion[10] = t77*(P[10][4]*t4+P[10][0]*t14+P[10][6]*t9-P[10][5]*t11-P[10][1]*t17+P[10][2]*t20+P[10][3]*t24);
Kfusion[11] = t77*(P[11][4]*t4+P[11][0]*t14+P[11][6]*t9-P[11][5]*t11-P[11][1]*t17+P[11][2]*t20+P[11][3]*t24);
Kfusion[12] = t77*(P[12][4]*t4+P[12][0]*t14+P[12][6]*t9-P[12][5]*t11-P[12][1]*t17+P[12][2]*t20+P[12][3]*t24);
Kfusion[13] = t77*(P[13][4]*t4+P[13][0]*t14+P[13][6]*t9-P[13][5]*t11-P[13][1]*t17+P[13][2]*t20+P[13][3]*t24);
Kfusion[14] = t77*(P[14][4]*t4+P[14][0]*t14+P[14][6]*t9-P[14][5]*t11-P[14][1]*t17+P[14][2]*t20+P[14][3]*t24);
Kfusion[15] = t77*(P[15][4]*t4+P[15][0]*t14+P[15][6]*t9-P[15][5]*t11-P[15][1]*t17+P[15][2]*t20+P[15][3]*t24);
Kfusion[16] = t77*(P[16][4]*t4+P[16][0]*t14+P[16][6]*t9-P[16][5]*t11-P[16][1]*t17+P[16][2]*t20+P[16][3]*t24);
Kfusion[17] = t77*(P[17][4]*t4+P[17][0]*t14+P[17][6]*t9-P[17][5]*t11-P[17][1]*t17+P[17][2]*t20+P[17][3]*t24);
Kfusion[18] = t77*(P[18][4]*t4+P[18][0]*t14+P[18][6]*t9-P[18][5]*t11-P[18][1]*t17+P[18][2]*t20+P[18][3]*t24);
Kfusion[19] = t77*(P[19][4]*t4+P[19][0]*t14+P[19][6]*t9-P[19][5]*t11-P[19][1]*t17+P[19][2]*t20+P[19][3]*t24);
Kfusion[20] = t77*(P[20][4]*t4+P[20][0]*t14+P[20][6]*t9-P[20][5]*t11-P[20][1]*t17+P[20][2]*t20+P[20][3]*t24);
Kfusion[21] = t77*(P[21][4]*t4+P[21][0]*t14+P[21][6]*t9-P[21][5]*t11-P[21][1]*t17+P[21][2]*t20+P[21][3]*t24);
Kfusion[22] = t77*(P[22][4]*t4+P[22][0]*t14+P[22][6]*t9-P[22][5]*t11-P[22][1]*t17+P[22][2]*t20+P[22][3]*t24);
Kfusion[23] = t77*(P[23][4]*t4+P[23][0]*t14+P[23][6]*t9-P[23][5]*t11-P[23][1]*t17+P[23][2]*t20+P[23][3]*t24);
EKF/matlab/scripts/Inertial Nav EKF/LOSX.c
+160
View File
@@ -0,0 +1,160 @@
float t2 = 1.0f/range;
float t3 = Tbs.b.y*q0*2.0f;
float t4 = Tbs.b.x*q3*2.0f;
float t18 = Tbs.b.z*q1*2.0f;
float t5 = t3+t4-t18;
float t6 = Tbs.b.y*q1*2.0f;
float t7 = Tbs.b.z*q0*2.0f;
float t16 = Tbs.b.x*q2*2.0f;
float t8 = t6+t7-t16;
float t9 = Tbs.b.x*q0*2.0f;
float t10 = Tbs.b.z*q2*2.0f;
float t17 = Tbs.b.y*q3*2.0f;
float t11 = t9+t10-t17;
float t12 = Tbs.b.x*q1*2.0f;
float t13 = Tbs.b.y*q2*2.0f;
float t14 = Tbs.b.z*q3*2.0f;
float t15 = t12+t13+t14;
float t19 = q0*q0;
float t20 = q1*q1;
float t21 = q2*q2;
float t22 = q3*q3;
float t23 = q0*q3*2.0f;
float t24 = q0*q2*2.0f;
float t25 = q1*q3*2.0f;
float t26 = q0*q1*2.0f;
float t27 = t19+t20-t21-t22;
float t28 = Tbs.b.x*t27;
float t29 = q1*q2*2.0f;
float t30 = t24+t25;
float t31 = Tbs.b.z*t30;
float t32 = t19-t20+t21-t22;
float t33 = Tbs.b.y*t32;
float t34 = t23+t29;
float t35 = Tbs.b.x*t34;
float t36 = q2*q3*2.0f;
float t37 = t19-t20-t21+t22;
float t38 = Tbs.b.z*t37;
float t39 = t24-t25;
float t40 = t26+t36;
float t41 = Tbs.b.y*t40;
float t60 = Tbs.b.x*t39;
float t42 = t38+t41-t60;
float t43 = t8*vd;
float t44 = t5*ve;
float t45 = t11*vn;
float t46 = t43+t44+t45;
float t47 = t5*vd;
float t48 = t15*vn;
float t62 = t8*ve;
float t49 = t47+t48-t62;
float t50 = t15*ve;
float t51 = t8*vn;
float t63 = t11*vd;
float t52 = t50+t51-t63;
float t53 = t15*vd;
float t54 = t11*ve;
float t64 = t5*vn;
float t55 = t53+t54-t64;
float t56 = t23-t29;
float t65 = Tbs.b.y*t56;
float t57 = t28+t31-t65;
float t58 = t26-t36;
float t66 = Tbs.b.z*t58;
float t59 = t33+t35-t66;
float t61 = P[0][0]*t2*t46;
float t67 = P[1][1]*t2*t49;
float t68 = P[4][0]*t2*t57;
float t69 = P[5][0]*t2*t59;
float t70 = P[6][0]*t2*t42;
float t71 = P[1][0]*t2*t49;
float t72 = P[2][0]*t2*t52;
float t73 = P[3][0]*t2*t55;
float t74 = t61+t68+t69+t70+t71+t72+t73;
float t75 = t2*t46*t74;
float t76 = P[4][1]*t2*t57;
float t77 = P[5][1]*t2*t59;
float t78 = P[6][1]*t2*t42;
float t79 = P[0][1]*t2*t46;
float t80 = P[2][1]*t2*t52;
float t81 = P[3][1]*t2*t55;
float t82 = t67+t76+t77+t78+t79+t80+t81;
float t83 = t2*t49*t82;
float t84 = P[4][2]*t2*t57;
float t85 = P[5][2]*t2*t59;
float t86 = P[6][2]*t2*t42;
float t87 = P[0][2]*t2*t46;
float t88 = P[1][2]*t2*t49;
float t89 = P[2][2]*t2*t52;
float t90 = P[3][2]*t2*t55;
float t91 = t84+t85+t86+t87+t88+t89+t90;
float t92 = t2*t52*t91;
float t93 = P[4][3]*t2*t57;
float t94 = P[5][3]*t2*t59;
float t95 = P[6][3]*t2*t42;
float t96 = P[0][3]*t2*t46;
float t97 = P[1][3]*t2*t49;
float t98 = P[2][3]*t2*t52;
float t99 = P[3][3]*t2*t55;
float t100 = t93+t94+t95+t96+t97+t98+t99;
float t101 = t2*t55*t100;
float t102 = P[4][4]*t2*t57;
float t103 = P[5][4]*t2*t59;
float t104 = P[6][4]*t2*t42;
float t105 = P[0][4]*t2*t46;
float t106 = P[1][4]*t2*t49;
float t107 = P[2][4]*t2*t52;
float t108 = P[3][4]*t2*t55;
float t109 = t102+t103+t104+t105+t106+t107+t108;
float t110 = t2*t57*t109;
float t111 = P[4][5]*t2*t57;
float t112 = P[5][5]*t2*t59;
float t113 = P[6][5]*t2*t42;
float t114 = P[0][5]*t2*t46;
float t115 = P[1][5]*t2*t49;
float t116 = P[2][5]*t2*t52;
float t117 = P[3][5]*t2*t55;
float t118 = t111+t112+t113+t114+t115+t116+t117;
float t119 = t2*t59*t118;
float t120 = P[4][6]*t2*t57;
float t121 = P[5][6]*t2*t59;
float t122 = P[6][6]*t2*t42;
float t123 = P[0][6]*t2*t46;
float t124 = P[1][6]*t2*t49;
float t125 = P[2][6]*t2*t52;
float t126 = P[3][6]*t2*t55;
float t127 = t120+t121+t122+t123+t124+t125+t126;
float t128 = t2*t42*t127;
float t129 = R_LOS+t75+t83+t92+t101+t110+t119+t128;
float t130 = 1.0f/t129;
H_LOS[0] = t2*t46;
H_LOS[1] = t2*t49;
H_LOS[2] = t2*t52;
H_LOS[3] = t2*t55;
H_LOS[4] = t2*(t28+t31-Tbs.b.y*(t23-q1*q2*2.0));
H_LOS[5] = t2*(t33+t35-Tbs.b.z*(t26-q2*q3*2.0));
H_LOS[6] = t2*t42;
Kfusion[0] = t130*(t61+P[0][6]*t2*t42+P[0][1]*t2*t49+P[0][2]*t2*t52+P[0][3]*t2*t55+P[0][4]*t2*t57+P[0][5]*t2*t59);
Kfusion[1] = t130*(t67+P[1][0]*t2*t46+P[1][6]*t2*t42+P[1][2]*t2*t52+P[1][3]*t2*t55+P[1][4]*t2*t57+P[1][5]*t2*t59);
Kfusion[2] = t130*(t89+P[2][0]*t2*t46+P[2][6]*t2*t42+P[2][1]*t2*t49+P[2][3]*t2*t55+P[2][4]*t2*t57+P[2][5]*t2*t59);
Kfusion[3] = t130*(t99+P[3][0]*t2*t46+P[3][6]*t2*t42+P[3][1]*t2*t49+P[3][2]*t2*t52+P[3][4]*t2*t57+P[3][5]*t2*t59);
Kfusion[4] = t130*(t102+P[4][0]*t2*t46+P[4][6]*t2*t42+P[4][1]*t2*t49+P[4][2]*t2*t52+P[4][3]*t2*t55+P[4][5]*t2*t59);
Kfusion[5] = t130*(t112+P[5][0]*t2*t46+P[5][6]*t2*t42+P[5][1]*t2*t49+P[5][2]*t2*t52+P[5][3]*t2*t55+P[5][4]*t2*t57);
Kfusion[6] = t130*(t122+P[6][0]*t2*t46+P[6][1]*t2*t49+P[6][2]*t2*t52+P[6][3]*t2*t55+P[6][4]*t2*t57+P[6][5]*t2*t59);
Kfusion[7] = t130*(P[7][0]*t2*t46+P[7][6]*t2*t42+P[7][1]*t2*t49+P[7][2]*t2*t52+P[7][3]*t2*t55+P[7][4]*t2*t57+P[7][5]*t2*t59);
Kfusion[8] = t130*(P[8][0]*t2*t46+P[8][6]*t2*t42+P[8][1]*t2*t49+P[8][2]*t2*t52+P[8][3]*t2*t55+P[8][4]*t2*t57+P[8][5]*t2*t59);
Kfusion[9] = t130*(P[9][0]*t2*t46+P[9][6]*t2*t42+P[9][1]*t2*t49+P[9][2]*t2*t52+P[9][3]*t2*t55+P[9][4]*t2*t57+P[9][5]*t2*t59);
Kfusion[10] = t130*(P[10][0]*t2*t46+P[10][6]*t2*t42+P[10][1]*t2*t49+P[10][2]*t2*t52+P[10][3]*t2*t55+P[10][4]*t2*t57+P[10][5]*t2*t59);
Kfusion[11] = t130*(P[11][0]*t2*t46+P[11][6]*t2*t42+P[11][1]*t2*t49+P[11][2]*t2*t52+P[11][3]*t2*t55+P[11][4]*t2*t57+P[11][5]*t2*t59);
Kfusion[12] = t130*(P[12][0]*t2*t46+P[12][6]*t2*t42+P[12][1]*t2*t49+P[12][2]*t2*t52+P[12][3]*t2*t55+P[12][4]*t2*t57+P[12][5]*t2*t59);
Kfusion[13] = t130*(P[13][0]*t2*t46+P[13][6]*t2*t42+P[13][1]*t2*t49+P[13][2]*t2*t52+P[13][3]*t2*t55+P[13][4]*t2*t57+P[13][5]*t2*t59);
Kfusion[14] = t130*(P[14][0]*t2*t46+P[14][6]*t2*t42+P[14][1]*t2*t49+P[14][2]*t2*t52+P[14][3]*t2*t55+P[14][4]*t2*t57+P[14][5]*t2*t59);
Kfusion[15] = t130*(P[15][0]*t2*t46+P[15][6]*t2*t42+P[15][1]*t2*t49+P[15][2]*t2*t52+P[15][3]*t2*t55+P[15][4]*t2*t57+P[15][5]*t2*t59);
Kfusion[16] = t130*(P[16][0]*t2*t46+P[16][6]*t2*t42+P[16][1]*t2*t49+P[16][2]*t2*t52+P[16][3]*t2*t55+P[16][4]*t2*t57+P[16][5]*t2*t59);
Kfusion[17] = t130*(P[17][0]*t2*t46+P[17][6]*t2*t42+P[17][1]*t2*t49+P[17][2]*t2*t52+P[17][3]*t2*t55+P[17][4]*t2*t57+P[17][5]*t2*t59);
Kfusion[18] = t130*(P[18][0]*t2*t46+P[18][6]*t2*t42+P[18][1]*t2*t49+P[18][2]*t2*t52+P[18][3]*t2*t55+P[18][4]*t2*t57+P[18][5]*t2*t59);
Kfusion[19] = t130*(P[19][0]*t2*t46+P[19][6]*t2*t42+P[19][1]*t2*t49+P[19][2]*t2*t52+P[19][3]*t2*t55+P[19][4]*t2*t57+P[19][5]*t2*t59);
Kfusion[20] = t130*(P[20][0]*t2*t46+P[20][6]*t2*t42+P[20][1]*t2*t49+P[20][2]*t2*t52+P[20][3]*t2*t55+P[20][4]*t2*t57+P[20][5]*t2*t59);
Kfusion[21] = t130*(P[21][0]*t2*t46+P[21][6]*t2*t42+P[21][1]*t2*t49+P[21][2]*t2*t52+P[21][3]*t2*t55+P[21][4]*t2*t57+P[21][5]*t2*t59);
Kfusion[22] = t130*(P[22][0]*t2*t46+P[22][6]*t2*t42+P[22][1]*t2*t49+P[22][2]*t2*t52+P[22][3]*t2*t55+P[22][4]*t2*t57+P[22][5]*t2*t59);
Kfusion[23] = t130*(P[23][0]*t2*t46+P[23][6]*t2*t42+P[23][1]*t2*t49+P[23][2]*t2*t52+P[23][3]*t2*t55+P[23][4]*t2*t57+P[23][5]*t2*t59);
EKF/matlab/scripts/Inertial Nav EKF/LOSY.c
+162
View File
@@ -0,0 +1,162 @@
float t2 = 1.0f/range;
float t3 = Tbs.a.y*q0*2.0f;
float t4 = Tbs.a.x*q3*2.0f;
float t18 = Tbs.a.z*q1*2.0f;
float t5 = t3+t4-t18;
float t6 = Tbs.a.y*q1*2.0f;
float t7 = Tbs.a.z*q0*2.0f;
float t16 = Tbs.a.x*q2*2.0f;
float t8 = t6+t7-t16;
float t9 = Tbs.a.x*q0*2.0f;
float t10 = Tbs.a.z*q2*2.0f;
float t17 = Tbs.a.y*q3*2.0f;
float t11 = t9+t10-t17;
float t12 = Tbs.a.x*q1*2.0f;
float t13 = Tbs.a.y*q2*2.0f;
float t14 = Tbs.a.z*q3*2.0f;
float t15 = t12+t13+t14;
float t19 = q0*q0;
float t20 = q1*q1;
float t21 = q2*q2;
float t22 = q3*q3;
float t23 = q0*q3*2.0f;
float t24 = q0*q2*2.0f;
float t25 = q1*q3*2.0f;
float t26 = q0*q1*2.0f;
float t27 = t19+t20-t21-t22;
float t28 = Tbs.a.x*t27;
float t29 = q1*q2*2.0f;
float t30 = t24+t25;
float t31 = Tbs.a.z*t30;
float t32 = t19-t20+t21-t22;
float t33 = Tbs.a.y*t32;
float t34 = t23+t29;
float t35 = Tbs.a.x*t34;
float t36 = q2*q3*2.0f;
float t37 = t19-t20-t21+t22;
float t38 = Tbs.a.z*t37;
float t39 = t24-t25;
float t40 = t26+t36;
float t41 = Tbs.a.y*t40;
float t60 = Tbs.a.x*t39;
float t42 = t38+t41-t60;
float t43 = t8*vd;
float t44 = t5*ve;
float t45 = t11*vn;
float t46 = t43+t44+t45;
float t47 = t5*vd;
float t48 = t15*vn;
float t62 = t8*ve;
float t49 = t47+t48-t62;
float t50 = t15*ve;
float t51 = t8*vn;
float t63 = t11*vd;
float t52 = t50+t51-t63;
float t53 = t15*vd;
float t54 = t11*ve;
float t64 = t5*vn;
float t55 = t53+t54-t64;
float t56 = t23-t29;
float t65 = Tbs.a.y*t56;
float t57 = t28+t31-t65;
float t58 = t26-t36;
float t66 = Tbs.a.z*t58;
float t59 = t33+t35-t66;
float t61 = P[0][0]*t2*t46;
float t67 = P[1][1]*t2*t49;
float t68 = P[4][0]*t2*t57;
float t69 = P[5][0]*t2*t59;
float t70 = P[6][0]*t2*t42;
float t71 = P[1][0]*t2*t49;
float t72 = P[2][0]*t2*t52;
float t73 = P[3][0]*t2*t55;
float t74 = t61+t68+t69+t70+t71+t72+t73;
float t75 = t2*t46*t74;
float t76 = P[4][1]*t2*t57;
float t77 = P[5][1]*t2*t59;
float t78 = P[6][1]*t2*t42;
float t79 = P[0][1]*t2*t46;
float t80 = P[2][1]*t2*t52;
float t81 = P[3][1]*t2*t55;
float t82 = t67+t76+t77+t78+t79+t80+t81;
float t83 = t2*t49*t82;
float t84 = P[4][2]*t2*t57;
float t85 = P[5][2]*t2*t59;
float t86 = P[6][2]*t2*t42;
float t87 = P[0][2]*t2*t46;
float t88 = P[1][2]*t2*t49;
float t89 = P[2][2]*t2*t52;
float t90 = P[3][2]*t2*t55;
float t91 = t84+t85+t86+t87+t88+t89+t90;
float t92 = t2*t52*t91;
float t93 = P[4][3]*t2*t57;
float t94 = P[5][3]*t2*t59;
float t95 = P[6][3]*t2*t42;
float t96 = P[0][3]*t2*t46;
float t97 = P[1][3]*t2*t49;
float t98 = P[2][3]*t2*t52;
float t99 = P[3][3]*t2*t55;
float t100 = t93+t94+t95+t96+t97+t98+t99;
float t101 = t2*t55*t100;
float t102 = P[4][4]*t2*t57;
float t103 = P[5][4]*t2*t59;
float t104 = P[6][4]*t2*t42;
float t105 = P[0][4]*t2*t46;
float t106 = P[1][4]*t2*t49;
float t107 = P[2][4]*t2*t52;
float t108 = P[3][4]*t2*t55;
float t109 = t102+t103+t104+t105+t106+t107+t108;
float t110 = t2*t57*t109;
float t111 = P[4][5]*t2*t57;
float t112 = P[5][5]*t2*t59;
float t113 = P[6][5]*t2*t42;
float t114 = P[0][5]*t2*t46;
float t115 = P[1][5]*t2*t49;
float t116 = P[2][5]*t2*t52;
float t117 = P[3][5]*t2*t55;
float t118 = t111+t112+t113+t114+t115+t116+t117;
float t119 = t2*t59*t118;
float t120 = P[4][6]*t2*t57;
float t121 = P[5][6]*t2*t59;
float t122 = P[6][6]*t2*t42;
float t123 = P[0][6]*t2*t46;
float t124 = P[1][6]*t2*t49;
float t125 = P[2][6]*t2*t52;
float t126 = P[3][6]*t2*t55;
float t127 = t120+t121+t122+t123+t124+t125+t126;
float t128 = t2*t42*t127;
float t129 = R_LOS+t75+t83+t92+t101+t110+t119+t128;
float t130 = 1.0f/t129;
H_LOS[0] = -t2*t46;
H_LOS[1] = -t2*t49;
H_LOS[2] = -t2*t52;
H_LOS[3] = -t2*t55;
H_LOS[4] = -t2*(t28+t31-Tbs.a.y*(t23-q1*q2*2.0f));
H_LOS[5] = -t2*(t33+t35-Tbs.a.z*(t26-q2*q3*2.0f));
H_LOS[6] = -t2*t42;
Kfusion[0] = -t130*(t61+P[0][6]*t2*t42+P[0][1]*t2*t49+P[0][2]*t2*t52+P[0][3]*t2*t55+P[0][4]*t2*t57+P[0][5]*t2*t59);
Kfusion[1] = -t130*(t67+P[1][0]*t2*t46+P[1][6]*t2*t42+P[1][2]*t2*t52+P[1][3]*t2*t55+P[1][4]*t2*t57+P[1][5]*t2*t59);
Kfusion[2] = -t130*(t89+P[2][0]*t2*t46+P[2][6]*t2*t42+P[2][1]*t2*t49+P[2][3]*t2*t55+P[2][4]*t2*t57+P[2][5]*t2*t59);
Kfusion[3] = -t130*(t99+P[3][0]*t2*t46+P[3][6]*t2*t42+P[3][1]*t2*t49+P[3][2]*t2*t52+P[3][4]*t2*t57+P[3][5]*t2*t59);
Kfusion[4] = -t130*(t102+P[4][0]*t2*t46+P[4][6]*t2*t42+P[4][1]*t2*t49+P[4][2]*t2*t52+P[4][3]*t2*t55+P[4][5]*t2*t59);
Kfusion[5] = -t130*(t112+P[5][0]*t2*t46+P[5][6]*t2*t42+P[5][1]*t2*t49+P[5][2]*t2*t52+P[5][3]*t2*t55+P[5][4]*t2*t57);
Kfusion[6] = -t130*(t122+P[6][0]*t2*t46+P[6][1]*t2*t49+P[6][2]*t2*t52+P[6][3]*t2*t55+P[6][4]*t2*t57+P[6][5]*t2*t59);
Kfusion[7] = -t130*(P[7][0]*t2*t46+P[7][6]*t2*t42+P[7][1]*t2*t49+P[7][2]*t2*t52+P[7][3]*t2*t55+P[7][4]*t2*t57+P[7][5]*t2*t59);
Kfusion[8] = -t130*(P[8][0]*t2*t46+P[8][6]*t2*t42+P[8][1]*t2*t49+P[8][2]*t2*t52+P[8][3]*t2*t55+P[8][4]*t2*t57+P[8][5]*t2*t59);
Kfusion[9] = -t130*(P[9][0]*t2*t46+P[9][6]*t2*t42+P[9][1]*t2*t49+P[9][2]*t2*t52+P[9][3]*t2*t55+P[9][4]*t2*t57+P[9][5]*t2*t59);
Kfusion[10] = -t130*(P[10][0]*t2*t46+P[10][6]*t2*t42+P[10][1]*t2*t49+P[10][2]*t2*t52+P[10][3]*t2*t55+P[10][4]*t2*t57+P[10][5]*t2*t59);
Kfusion[11] = -t130*(P[11][0]*t2*t46+P[11][6]*t2*t42+P[11][1]*t2*t49+P[11][2]*t2*t52+P[11][3]*t2*t55+P[11][4]*t2*t57+P[11][5]*t2*t59);
Kfusion[12] = -t130*(P[12][0]*t2*t46+P[12][6]*t2*t42+P[12][1]*t2*t49+P[12][2]*t2*t52+P[12][3]*t2*t55+P[12][4]*t2*t57+P[12][5]*t2*t59);
Kfusion[13] = -t130*(P[13][0]*t2*t46+P[13][6]*t2*t42+P[13][1]*t2*t49+P[13][2]*t2*t52+P[13][3]*t2*t55+P[13][4]*t2*t57+P[13][5]*t2*t59);
Kfusion[14] = -t130*(P[14][0]*t2*t46+P[14][6]*t2*t42+P[14][1]*t2*t49+P[14][2]*t2*t52+P[14][3]*t2*t55+P[14][4]*t2*t57+P[14][5]*t2*t59);
Kfusion[15] = -t130*(P[15][0]*t2*t46+P[15][6]*t2*t42+P[15][1]*t2*t49+P[15][2]*t2*t52+P[15][3]*t2*t55+P[15][4]*t2*t57+P[15][5]*t2*t59);
Kfusion[16] = -t130*(P[16][0]*t2*t46+P[16][6]*t2*t42+P[16][1]*t2*t49+P[16][2]*t2*t52+P[16][3]*t2*t55+P[16][4]*t2*t57+P[16][5]*t2*t59);
Kfusion[17] = -t130*(P[17][0]*t2*t46+P[17][6]*t2*t42+P[17][1]*t2*t49+P[17][2]*t2*t52+P[17][3]*t2*t55+P[17][4]*t2*t57+P[17][5]*t2*t59);
Kfusion[18] = -t130*(P[18][0]*t2*t46+P[18][6]*t2*t42+P[18][1]*t2*t49+P[18][2]*t2*t52+P[18][3]*t2*t55+P[18][4]*t2*t57+P[18][5]*t2*t59);
Kfusion[19] = -t130*(P[19][0]*t2*t46+P[19][6]*t2*t42+P[19][1]*t2*t49+P[19][2]*t2*t52+P[19][3]*t2*t55+P[19][4]*t2*t57+P[19][5]*t2*t59);
Kfusion[20] = -t130*(P[20][0]*t2*t46+P[20][6]*t2*t42+P[20][1]*t2*t49+P[20][2]*t2*t52+P[20][3]*t2*t55+P[20][4]*t2*t57+P[20][5]*t2*t59);
Kfusion[21] = -t130*(P[21][0]*t2*t46+P[21][6]*t2*t42+P[21][1]*t2*t49+P[21][2]*t2*t52+P[21][3]*t2*t55+P[21][4]*t2*t57+P[21][5]*t2*t59);
Kfusion[22] = -t130*(P[22][0]*t2*t46+P[22][6]*t2*t42+P[22][1]*t2*t49+P[22][2]*t2*t52+P[22][3]*t2*t55+P[22][4]*t2*t57+P[22][5]*t2*t59);
Kfusion[23] = -t130*(P[23][0]*t2*t46+P[23][6]*t2*t42+P[23][1]*t2*t49+P[23][2]*t2*t52+P[23][3]*t2*t55+P[23][4]*t2*t57+P[23][5]*t2*t59);
EKF/matlab/scripts/Inertial Nav EKF/M_code24.txt
+686
View File
File diff suppressed because one or more lines are too long
EKF/matlab/scripts/Inertial Nav EKF/Magnetometer.mat
BIN
View File
Binary file not shown.
EKF/matlab/scripts/Inertial Nav EKF/NormQuat.m
+5
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;
EKF/matlab/scripts/Inertial Nav EKF/OptimiseAlgebra.m
+29
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
EKF/matlab/scripts/Inertial Nav EKF/Q_airdata.c
+89
View File
@@ -0,0 +1,89 @@
float t3 = ve-vwe;
float t4 = q0*q0;
float t5 = q1*q1;
float t6 = q2*q2;
float t7 = q3*q3;
float t8 = vd-vwd;
float t9 = q0*q1*2.0f;
float t10 = q2*q3*2.0f;
float t11 = vn-vwn;
float t13 = q0*q2*2.0f;
float t14 = q1*q3*2.0f;
float t18 = t4-t5-t6+t7;
float t19 = t8*t18;
float t20 = t9-t10;
float t21 = t3*t20;
float t22 = t13+t14;
float t23 = t11*t22;
float t2 = t19-t21+t23;
float t15 = q0*q3*2.0f;
float t16 = q1*q2*2.0f;
float t24 = t9+t10;
float t25 = t4-t5+t6-t7;
float t26 = t3*t25;
float t27 = t8*t24;
float t28 = t15-t16;
float t29 = t11*t28;
float t12 = t26+t27-t29;
float t30 = t13-t14;
float t31 = t4+t5-t6-t7;
float t32 = t11*t31;
float t33 = t8*t30;
float t34 = t15+t16;
float t35 = t3*t34;
float t17 = t32-t33+t35;
float t44 = t2*t18*2.0f;
float t45 = t12*t24*2.0f;
float t46 = t17*t30*2.0f;
float t36 = t44+t45-t46;
float t37 = t2*t2;
float t38 = t12*t12;
float t39 = t17*t17;
float t40 = t37+t38+t39;
float t41 = 1.0f/t40;
float t48 = t12*t25*2.0f;
float t49 = t2*t20*2.0f;
float t50 = t17*t34*2.0f;
float t42 = t48-t49+t50;
float t52 = t17*t31*2.0f;
float t53 = t2*t22*2.0f;
float t54 = t12*t28*2.0f;
float t43 = t52+t53-t54;
float t47 = t36*t36;
float t51 = t42*t42;
float t55 = t43*t43;
float t57 = 1.0f/(t17*t17);
float t58 = 1.0f/t17;
float t63 = t18*t58;
float t64 = t2*t30*t57;
float t56 = t63+t64;
float t66 = t22*t58;
float t67 = t2*t31*t57;
float t59 = t66-t67;
float t60 = t37*t57;
float t61 = t60+1.0f;
float t62 = 1.0f/(t61*t61);
float t65 = t56*t56;
float t68 = t59*t59;
float t70 = t20*t58;
float t71 = t2*t34*t57;
float t69 = t70+t71;
float t72 = t69*t69;
float t78 = t25*t58;
float t79 = t12*t34*t57;
float t73 = t78-t79;
float t81 = t28*t58;
float t82 = t12*t31*t57;
float t74 = t81+t82;
float t75 = t38*t57;
float t76 = t75+1.0f;
float t77 = 1.0f/(t76*t76);
float t80 = t73*t73;
float t83 = t74*t74;
float t85 = t24*t58;
float t86 = t12*t30*t57;
float t84 = t85+t86;
float t87 = t84*t84;
float tas_var = t41*t47*vd_var*0.25f+t41*t51*ve_var*0.25f+t41*t55*vn_var*0.25f+t41*t47*vwd_var*0.25f+t41*t51*vwe_var*0.25f+t41*t55*vwn_var*0.25f;
float aoa_var = t62*t65*vd_var+t62*t72*ve_var+t62*t68*vn_var+t62*t65*vwd_var+t62*t72*vwe_var+t62*t68*vwn_var;
float aos_var = t77*t87*vd_var+t77*t80*ve_var+t77*t83*vn_var+t77*t87*vwd_var+t77*t80*vwe_var+t77*t83*vwn_var;
EKF/matlab/scripts/Inertial Nav EKF/Quat2Tbn.m
+14
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];
EKF/matlab/scripts/Inertial Nav EKF/QuatDivide.m
+16
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);
EKF/matlab/scripts/Inertial Nav EKF/QuatErrTransferEquations.m
+62
View File
@@ -0,0 +1,62 @@
%% calculate the rotation vector variances from an equivalent quaternion
% inputs are the quaternion orientation and the 4x4 covariance matrix for the quaternions
% output is a vector of variances for the rotation vector that is equivalent to the quaternion
clear all;
reset(symengine);
syms q0 q1 q2 q3 real % quaternions defining attitude of body axes relative to local NED
% define quaternion rotation
quat = [q0;q1;q2;q3];
% convert to a rotation vector
delta = 2*acos(q0);
rotVec = (delta/sin(delta/2))*[q1;q2;q3];
% calculate transfer matrix from quaternion to rotation vector
G = jacobian(rotVec, quat);
% 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:4
for colIndex = 1:4
eval(['syms P_l_',num2str(rowIndex-1),'_c_',num2str(colIndex-1), '_r_ real']);
eval(['quatCovMat(',num2str(rowIndex),',',num2str(colIndex), ') = P_l_',num2str(rowIndex-1),'_c_',num2str(colIndex-1),'_r_;']);
end
end
% rotate the covariance from quaternion to rotation vector
rotCovMat = G*quatCovMat*transpose(G);
% take the variances
rotVarVec = [rotCovMat(1,1);rotCovMat(2,2);rotCovMat(3,3)];
% convert to c-code
ccode(rotVarVec,'file','rotVarVec.c');
%% calculate the quaternion variances from an equivalent rotation vector
% define a rotation vector
syms rotX rotY rotZ real;
rotVec = [rotX;rotY;rotZ];
% convert to a quaternion
vecLength = sqrt(rotVec(1)^2 + rotVec(2)^2 + rotVec(3)^2);
quat = [cos(0.5*vecLength); rotVec/vecLength*sin(0.5*vecLength)];
% calculate transfer matrix from rotation vector to quaternion
G = jacobian(quat, rotVec);
% define the rotation vector variances
syms rotVarX rotVarY rotVarZ real;
% define the rotation vector covariance matrix
rotCovMat = diag([rotVarX;rotVarY;rotVarZ]);
% rotate the covariance matrix into quaternion coordinates
quatCovMat = G*rotCovMat*transpose(G);
% convert to c-code
ccode(quatCovMat,'file','quatCovMat.c');
EKF/matlab/scripts/Inertial Nav EKF/QuatMult.m
+5
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))];
EKF/matlab/scripts/Inertial Nav EKF/QuatToEul.m
+9
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));
EKF/matlab/scripts/Inertial Nav EKF/RotToQuat.m
+10
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
EKF/matlab/scripts/Inertial Nav EKF/SaveScriptCode.m
+666
View File
@@ -0,0 +1,666 @@
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_LOSX(rowIndex));
% 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_LOSY(rowIndex));
% 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');
fprintf(fid,'\n');
fprintf(fid,'SK_MAGS = zeros(%d,1);\n',numel(SK_MAGS));
for rowIndex = 1:numel(SK_MAGS)
string = char(SK_MAGS(rowIndex,1));
fprintf(fid,'SK_MAGS(%d) = %s;\n',rowIndex,string);
end
fprintf(fid,'\n');
[nRow,nCol] = size(K_MAGS);
fprintf(fid,'\n');
fprintf(fid,'Kfusion = zeros(%d,1);\n',nRow,nCol);
for rowIndex = 1:nRow
string = char(K_MAGS(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 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
EKF/matlab/scripts/Inertial Nav EKF/Sideslip.mat
BIN
View File
Binary file not shown.
EKF/matlab/scripts/Inertial Nav EKF/StateAndCovariancePrediction.mat
BIN
View File
Binary file not shown.
EKF/matlab/scripts/Inertial Nav EKF/StatePrediction.mat
BIN
View File
Binary file not shown.
EKF/matlab/scripts/Inertial Nav EKF/SymbolicOutput24.mat
BIN
View File
Binary file not shown.
EKF/matlab/scripts/Inertial Nav EKF/SymbolicOutput24.txt
+742
View File
File diff suppressed because one or more lines are too long
EKF/matlab/scripts/Inertial Nav EKF/Tbn_312.c
+15
View File
@@ -0,0 +1,15 @@
t2 = sin(yaw);
t3 = cos(yaw);
t4 = sin(pitch);
t5 = cos(pitch);
t6 = sin(roll);
t7 = cos(roll);
A0[0][0] = t3*t5-t2*t4*t6;
A0[0][1] = -t2*t7;
A0[0][2] = t3*t4+t2*t5*t6;
A0[1][0] = t2*t5+t3*t4*t6;
A0[1][1] = t3*t7;
A0[1][2] = t2*t4-t3*t5*t6;
A0[2][0] = -t4*t7;
A0[2][1] = t6;
A0[2][2] = t5*t7;
EKF/matlab/scripts/Inertial Nav EKF/Tbn_321.c
+15
View File
@@ -0,0 +1,15 @@
t2 = cos(yaw);
t3 = sin(roll);
t4 = sin(yaw);
t5 = cos(roll);
t6 = sin(pitch);
t7 = cos(pitch);
A0[0][0] = t2*t7;
A0[0][1] = -t4*t5+t2*t3*t6;
A0[0][2] = t3*t4+t2*t5*t6;
A0[1][0] = t4*t7;
A0[1][1] = t2*t5+t3*t4*t6;
A0[1][2] = -t2*t3+t4*t5*t6;
A0[2][0] = -t6;
A0[2][1] = t3*t7;
A0[2][2] = t5*t7;
EKF/matlab/scripts/Inertial Nav EKF/Tbn_quat.c
+17
View File
@@ -0,0 +1,17 @@
float t2 = q1*q2*2.0;
float t3 = q0*q0;
float t4 = q1*q1;
float t5 = q2*q2;
float t6 = q3*q3;
float t7 = q0*q2*2.0;
float t8 = q1*q3*2.0;
float t9 = q2*q3*2.0;
A0[0][0] = t3+t4-t5-t6;
A0[0][1] = t2-q0*q3*2.0;
A0[0][2] = t7+t8;
A0[1][0] = t2+q0*q3*2.0;
A0[1][1] = t3-t4+t5-t6;
A0[1][2] = t9-q0*q1*2.0;
A0[2][0] = -t7+t8;
A0[2][1] = t9+q0*q1*2.0;
A0[2][2] = t3-t4-t5+t6;
EKF/matlab/scripts/Inertial Nav EKF/calcH_YAW312.c
+17
View File
@@ -0,0 +1,17 @@
t9 = q0*q3;
t10 = q1*q2;
t2 = t9-t10;
t3 = q0*q0;
t4 = q1*q1;
t5 = q2*q2;
t6 = q3*q3;
t7 = t3-t4+t5-t6;
t8 = 1.0/(t7*t7);
t11 = t2*t2;
t12 = t8*t11*4.0;
t13 = t12+1.0;
t14 = 1.0/t13;
A0[0][0] = t8*t14*(q3*t3+q3*t4-q3*t5+q3*t6-q0*q1*q2*2.0)*-2.0;
A0[0][1] = t8*t14*(q2*t3+q2*t4+q2*t5-q2*t6-q0*q1*q3*2.0)*-2.0;
A0[0][2] = t8*t14*(-q1*t3+q1*t4+q1*t5+q1*t6-q0*q2*q3*2.0)*2.0;
A0[0][3] = t8*t14*(q0*t3-q0*t4+q0*t5+q0*t6-q1*q2*q3*2.0)*2.0;
EKF/matlab/scripts/Inertial Nav EKF/calcH_YAW321.c
+17
View File
@@ -0,0 +1,17 @@
t9 = q0*q3;
t10 = q1*q2;
t2 = t9+t10;
t3 = q0*q0;
t4 = q1*q1;
t5 = q2*q2;
t6 = q3*q3;
t7 = t3+t4-t5-t6;
t8 = 1.0/(t7*t7);
t11 = t2*t2;
t12 = t8*t11*4.0;
t13 = t12+1.0;
t14 = 1.0/t13;
A0[0][0] = t8*t14*(q3*t3-q3*t4+q3*t5+q3*t6+q0*q1*q2*2.0)*-2.0;
A0[0][1] = t8*t14*(-q2*t3+q2*t4+q2*t5+q2*t6+q0*q1*q3*2.0)*-2.0;
A0[0][2] = t8*t14*(q1*t3+q1*t4+q1*t5-q1*t6+q0*q2*q3*2.0)*2.0;
A0[0][3] = t8*t14*(q0*t3+q0*t4-q0*t5+q0*t6+q1*q2*q3*2.0)*2.0;
EKF/matlab/scripts/Inertial Nav EKF/calcMAGD.c
+46
View File
@@ -0,0 +1,46 @@
t2 = magE*magE;
t3 = magN*magN;
t4 = t2+t3;
t5 = P[16][16]*t2;
t6 = P[17][17]*t3;
t7 = t2*t2;
t8 = R_DECL*t7;
t9 = t3*t3;
t10 = R_DECL*t9;
t11 = R_DECL*t2*t3*2.0;
t14 = P[16][17]*magE*magN;
t15 = P[17][16]*magE*magN;
t12 = t5+t6+t8+t10+t11-t14-t15;
t13 = 1.0/t12;
t16 = conjugate(magE);
t17 = conjugate(magN);
t18 = t16*t16;
t19 = t17*t17;
t20 = t18+t19;
t21 = 1.0/t20;
A0[0][0] = -t4*t13*(P[0][16]*magE-P[0][17]*magN);
A0[1][0] = -t4*t13*(P[1][16]*magE-P[1][17]*magN);
A0[2][0] = -t4*t13*(P[2][16]*magE-P[2][17]*magN);
A0[3][0] = -t4*t13*(P[3][16]*magE-P[3][17]*magN);
A0[4][0] = -t4*t13*(P[4][16]*magE-P[4][17]*magN);
A0[5][0] = -t4*t13*(P[5][16]*magE-P[5][17]*magN);
A0[6][0] = -t4*t13*(P[6][16]*magE-P[6][17]*magN);
A0[7][0] = -t4*t13*(P[7][16]*magE-P[7][17]*magN);
A0[8][0] = -t4*t13*(P[8][16]*magE-P[8][17]*magN);
A0[9][0] = -t4*t13*(P[9][16]*magE-P[9][17]*magN);
A0[10][0] = -t4*t13*(P[10][16]*magE-P[10][17]*magN);
A0[11][0] = -t4*t13*(P[11][16]*magE-P[11][17]*magN);
A0[12][0] = -t4*t13*(P[12][16]*magE-P[12][17]*magN);
A0[13][0] = -t4*t13*(P[13][16]*magE-P[13][17]*magN);
A0[14][0] = -t4*t13*(P[14][16]*magE-P[14][17]*magN);
A0[15][0] = -t4*t13*(P[15][16]*magE-P[15][17]*magN);
A0[16][0] = -t4*t13*(P[16][16]*magE-P[16][17]*magN);
A0[16][1] = -t16*t21;
A0[17][0] = -t4*t13*(P[17][16]*magE-P[17][17]*magN);
A0[17][1] = t17*t21;
A0[18][0] = -t4*t13*(P[18][16]*magE-P[18][17]*magN);
A0[19][0] = -t4*t13*(P[19][16]*magE-P[19][17]*magN);
A0[20][0] = -t4*t13*(P[20][16]*magE-P[20][17]*magN);
A0[21][0] = -t4*t13*(P[21][16]*magE-P[21][17]*magN);
A0[22][0] = -t4*t13*(P[22][16]*magE-P[22][17]*magN);
A0[23][0] = -t4*t13*(P[23][16]*magE-P[23][17]*magN);
EKF/matlab/scripts/Inertial Nav EKF/derive_air_data_errors.m
+97
View File
@@ -0,0 +1,97 @@
% This script generates c code required to calculate the variance of the
% TAS, AoA and AoS estimates calculated from the vehicle quaternions, NED
% velocity and NED wind velocity. Uncertainty in the quaternions is
% ignored. Variance in vehicle velocity and wind velocity is accounted for.
%% calculate TAS error terms
clear all;
reset(symengine);
syms vn ve vd 'real' % navigation frame NED velocity (m/s)
syms vwn vwe vwd 'real' % navigation frame NED wind velocity (m/s)
syms vn_var ve_var vd_var 'real' % navigation frame NED velocity variances (m/s)^2
syms vwn_var vwe_var vwd_var 'real' % navigation frame NED wind velocity variances (m/s)^2
syms q0 q1 q2 q3 'real' % quaternions defining rotation from navigation NED frame to body XYZ frame
quat = [q0;q1;q2;q3];
% rotation matrix from navigation to body frame
Tnb = transpose(Quat2Tbn(quat));
% crete velocity vectors
ground_velocity_truth = [vn;ve;vd];
wind_velocity_truth = [vwn;vwe;vwd];
% calcuate wind relative velocity
rel_vel_ef = ground_velocity_truth - wind_velocity_truth;
% rotate into body frame
rel_vel_bf = Tnb * rel_vel_ef;
% calculate the true airspeed
TAS = sqrt(rel_vel_bf(1)^2 + rel_vel_bf(2)^2 + rel_vel_bf(3)^2);
% derive the control(disturbance) influence matrix from velocity error to
% TAS error
G_TAS = jacobian(TAS, [ground_velocity_truth;wind_velocity_truth]);
% derive the error matrix
TAS_dist_matrix = diag([vn_var ve_var vd_var vwn_var vwe_var vwd_var]);
Q_TAS = G_TAS*TAS_dist_matrix*transpose(G_TAS);
% save as C code
ccode(Q_TAS,'file','Q_TAS.c');
save temp.mat;
%% calculate AoA error equations
clear all;
reset(symengine);
load temp.mat;
AoA = atan(rel_vel_bf(3) / rel_vel_bf(1));
% derive the control(disturbance) influence matrix from velocity error to
% AoA error
G_AoA = jacobian(AoA, [ground_velocity_truth;wind_velocity_truth]);
% derive the error matrix
AoA_dist_matrix = diag([vn_var ve_var vd_var vwn_var vwe_var vwd_var]);
Q_AoA = G_AoA*AoA_dist_matrix*transpose(G_AoA);
% save as C code
ccode(Q_AoA,'file','Q_AoA.c');
save temp.mat;
%% Calculate AoS error equations
clear all;
reset(symengine);
load temp.mat;
AoS = atan(rel_vel_bf(2) / rel_vel_bf(1));
% derive the control(disturbance) influence matrix from velocity error to
% AoS error
G_AoS = jacobian(AoS, [ground_velocity_truth;wind_velocity_truth]);
% derive the error matrix
AoS_dist_matrix = diag([vn_var ve_var vd_var vwn_var vwe_var vwd_var]);
Q_AoS = G_AoS*AoS_dist_matrix*transpose(G_AoS);
% save as C code
ccode(Q_AoS,'file','Q_AoS.c');
save temp.mat;
%% convert them combined to take advantage of shared terms in the optimiser
clear all;
reset(symengine);
load temp.mat;
% save as C code
ccode([Q_TAS;Q_AoA;Q_AoS],'file','Q_airdata.c');
EKF/matlab/scripts/Inertial Nav EKF/fix_c_code.m
+92
View File
@@ -0,0 +1,92 @@
function fix_c_code(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(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);
%% 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
%% 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
%% 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
fid = fopen(fileName,'wt');
for lineIndex = 1:length(SymbolicOutput)
fprintf(fid,char(SymbolicOutput(lineIndex)));
fprintf(fid,'\n');
end
fclose(fid);
clear all;
end
EKF/matlab/scripts/Inertial Nav EKF/polar2cart_cov.m
+8
View File
@@ -0,0 +1,8 @@
clear all;
syms spd yaw real;
syms R_spd R_yaw real;
vx = spd*cos(yaw);
vy = spd*sin(yaw);
Tpc = jacobian([vx;vy],[spd;yaw]);
R_polar = [R_spd 0;0 R_yaw];
R_cartesian = Tpc*R_polar*Tpc';
EKF/matlab/scripts/Inertial Nav EKF/quat2yaw312.m
+29
View File
@@ -0,0 +1,29 @@
% define roll, pitch and yaw variables
syms roll pitch yaw 'real'
% Define yransformaton matrices for rotations about the X,Y and Z body axes
Xrot = [1 0 0 ; 0 cos(roll) sin(roll) ; 0 -sin(roll) cos(roll)];
Yrot = [cos(pitch) 0 -sin(pitch) ; 0 1 0 ; sin(pitch) 0 cos(pitch)];
Zrot = [cos(yaw) sin(yaw) 0 ; -sin(yaw) cos(yaw) 0 ; 0 0 1];
% calculate the tranformation matrix from body to navigation frame resulting from
% a rotation in yaw, roll, pitch order
Tbn_312 = transpose(Yrot*Xrot*Zrot)
% convert to c code and save
ccode(Tbn_312,'file','Tbn_312.c');
fix_c_code('Tbn_312.c');
% define the quaternion elements
syms q0 q1 q2 q3 'real'
% calculate the tranformation matrix from body to navigation frame as a
% function of the quaternions
Tbn_quat = Quat2Tbn([q0;q1;q2;q3]);
% collect the y,x terms required for calculation of the yaw angle
yaw_input_312 = [-Tbn_quat(1,2);Tbn_quat(2,2)];
% convert to c code and save
ccode(yaw_input_312,'file','yaw_input_312.c');
fix_c_code('yaw_input_312.c');
EKF/matlab/scripts/Inertial Nav EKF/quat2yaw321.m
+29
View File
@@ -0,0 +1,29 @@
% define roll, pitch and yaw variables
syms roll pitch yaw 'real'
% Define yransformaton matrices for rotations about the X,Y and Z body axes
Xrot = [1 0 0 ; 0 cos(roll) sin(roll) ; 0 -sin(roll) cos(roll)];
Yrot = [cos(pitch) 0 -sin(pitch) ; 0 1 0 ; sin(pitch) 0 cos(pitch)];
Zrot = [cos(yaw) sin(yaw) 0 ; -sin(yaw) cos(yaw) 0 ; 0 0 1];
% calculate the tranformation matrix from body to navigation frame resulting from
% a rotation in yaw, pitch, roll order
Tbn_321 = transpose(Xrot*Yrot*Zrot)
% convert to c code and save
ccode(Tbn_321,'file','Tbn_321.c');
fix_c_code('Tbn_321.c');
% define the quaternion elements
syms q0 q1 q2 q3 'real'
% calculate the tranformation matrix from body to navigation frame as a
% function of the quaternions
Tbn_quat = Quat2Tbn([q0;q1;q2;q3]);
% calculate the y,x terms required for calculation fo the yaw angle
yaw_input_321 = [Tbn_quat(2,1);Tbn_quat(1,1)];
% convert to c code and save
ccode(yaw_input_321,'file','yaw_input_321.c');
fix_c_code('yaw_input_321.c');
EKF/matlab/scripts/Inertial Nav EKF/quatCovMat.c
+59
View File
@@ -0,0 +1,59 @@
float t2 = rotX*rotX;
float t4 = rotY*rotY;
float t5 = rotZ*rotZ;
float t6 = t2+t4+t5;
float t7 = sqrtf(t6);
float t8 = t7*0.5f;
float t3 = sinf(t8);
float t9 = t3*t3;
float t10 = 1.0f/t6;
float t11 = 1.0f/sqrtf(t6);
float t12 = cosf(t8);
float t13 = 1.0f/powf(t6,1.5f);
float t14 = t3*t11;
float t15 = rotX*rotY*t3*t13;
float t16 = rotX*rotZ*t3*t13;
float t17 = rotY*rotZ*t3*t13;
float t18 = t2*t10*t12*0.5f;
float t27 = t2*t3*t13;
float t19 = t14+t18-t27;
float t23 = rotX*rotY*t10*t12*0.5f;
float t28 = t15-t23;
float t20 = rotY*rotVarY*t3*t11*t28*0.5f;
float t25 = rotX*rotZ*t10*t12*0.5f;
float t31 = t16-t25;
float t21 = rotZ*rotVarZ*t3*t11*t31*0.5f;
float t22 = t20+t21-rotX*rotVarX*t3*t11*t19*0.5f;
float t24 = t15-t23;
float t26 = t16-t25;
float t29 = t4*t10*t12*0.5f;
float t34 = t3*t4*t13;
float t30 = t14+t29-t34;
float t32 = t5*t10*t12*0.5f;
float t40 = t3*t5*t13;
float t33 = t14+t32-t40;
float t36 = rotY*rotZ*t10*t12*0.5f;
float t39 = t17-t36;
float t35 = rotZ*rotVarZ*t3*t11*t39*0.5f;
float t37 = t15-t23;
float t38 = t17-t36;
float t41 = rotVarX*(t15-t23)*(t16-t25);
float t42 = t41-rotVarY*t30*t39-rotVarZ*t33*t39;
float t43 = t16-t25;
float t44 = t17-t36;
P[0][0] = rotVarX*t2*t9*t10*0.25f+rotVarY*t4*t9*t10*0.25f+rotVarZ*t5*t9*t10*0.25f;
P[0][1] = t22;
P[0][2] = t35+rotX*rotVarX*t3*t11*(t15-rotX*rotY*t10*t12*0.5f)*0.5f-rotY*rotVarY*t3*t11*t30*0.5f;
P[0][3] = rotX*rotVarX*t3*t11*(t16-rotX*rotZ*t10*t12*0.5f)*0.5f+rotY*rotVarY*t3*t11*(t17-rotY*rotZ*t10*t12*0.5f)*0.5f-rotZ*rotVarZ*t3*t11*t33*0.5f;
P[1][0] = t22;
P[1][1] = rotVarX*(t19*t19)+rotVarY*(t24*t24)+rotVarZ*(t26*t26);
P[1][2] = rotVarZ*(t16-t25)*(t17-rotY*rotZ*t10*t12*0.5f)-rotVarX*t19*t28-rotVarY*t28*t30;
P[1][3] = rotVarY*(t15-t23)*(t17-rotY*rotZ*t10*t12*0.5f)-rotVarX*t19*t31-rotVarZ*t31*t33;
P[2][0] = t35-rotY*rotVarY*t3*t11*t30*0.5f+rotX*rotVarX*t3*t11*(t15-t23)*0.5f;
P[2][1] = rotVarZ*(t16-t25)*(t17-t36)-rotVarX*t19*t28-rotVarY*t28*t30;
P[2][2] = rotVarY*(t30*t30)+rotVarX*(t37*t37)+rotVarZ*(t38*t38);
P[2][3] = t42;
P[3][0] = rotZ*rotVarZ*t3*t11*t33*(-1.0f/2.0f)+rotX*rotVarX*t3*t11*(t16-t25)*0.5f+rotY*rotVarY*t3*t11*(t17-t36)*0.5f;
P[3][1] = rotVarY*(t15-t23)*(t17-t36)-rotVarX*t19*t31-rotVarZ*t31*t33;
P[3][2] = t42;
P[3][3] = rotVarZ*(t33*t33)+rotVarX*(t43*t43)+rotVarY*(t44*t44);
EKF/matlab/scripts/Inertial Nav EKF/rotVarVec.c
+19
View File
@@ -0,0 +1,19 @@
t2 = q0*q0;
t3 = acos(q0);
t4 = -t2+1.0;
t5 = t2-1.0;
t6 = 1.0/t5;
t7 = q1*t6*2.0;
t8 = 1.0/pow(t4,3.0/2.0);
t9 = q0*q1*t3*t8*2.0;
t10 = t7+t9;
t11 = 1.0/sqrt(t4);
t12 = q2*t6*2.0;
t13 = q0*q2*t3*t8*2.0;
t14 = t12+t13;
t15 = q3*t6*2.0;
t16 = q0*q3*t3*t8*2.0;
t17 = t15+t16;
A0[0][0] = t10*(P_l_0_c_0_r_*t10+P_l_1_c_0_r_*t3*t11*2.0)+t3*t11*(P_l_0_c_1_r_*t10+P_l_1_c_1_r_*t3*t11*2.0)*2.0;
A0[1][0] = t14*(P_l_0_c_0_r_*t14+P_l_2_c_0_r_*t3*t11*2.0)+t3*t11*(P_l_0_c_2_r_*t14+P_l_2_c_2_r_*t3*t11*2.0)*2.0;
A0[2][0] = t17*(P_l_0_c_0_r_*t17+P_l_3_c_0_r_*t3*t11*2.0)+t3*t11*(P_l_0_c_3_r_*t17+P_l_3_c_3_r_*t3*t11*2.0)*2.0;
EKF/matlab/scripts/Inertial Nav EKF/temp1.mat
BIN
View File
Binary file not shown.
EKF/matlab/scripts/Inertial Nav EKF/temp2.mat
BIN
View File
Binary file not shown.
EKF/matlab/scripts/Inertial Nav EKF/temp3.mat
BIN
View File
Binary file not shown.
EKF/matlab/scripts/Inertial Nav EKF/test_output_filter_gains.m
+35
View File
@@ -0,0 +1,35 @@
% script to test complementary output filter gain structure
vel_state = 0.0;
pos_state = 1.0;
vel_demand = 0.0;
pos_demand = 0.0;
dt = 0.01;
buffer_delay = 30;
vel_state_history = zeros(1,10000);
pos_state_history = zeros(1,10000);
time = [0:dt:9999*dt];
vel_correction = 0;
pos_correction = 0;
omega = 1.0/(dt*buffer_delay);
prev_vel = 1.0;
for i=1:10000
prev_vel = vel_state;
vel_state = vel_state + vel_correction*dt;
vel_state_history(i) = vel_state;
pos_state = pos_state + 0.5*dt*(vel_state + prev_vel) + pos_correction*dt;
pos_state_history(i) = pos_state;
if i > buffer_delay
vel_correction = (vel_demand - vel_state_history(i-buffer_delay))*omega*0.5;
pos_correction = (pos_demand - pos_state_history(i-buffer_delay))*omega*0.6;
else
vel_correction = 0;
pos_correction = 0;
end
end
min(vel_state_history)
close all;
figure;
subplot(2,1,1)
plot(time,vel_state_history);
subplot(2,1,2);
plot(time,pos_state_history);
EKF/matlab/scripts/Inertial Nav EKF/yaw_input_312.c
+2
View File
@@ -0,0 +1,2 @@
A0[0][0] = q0*q3*2.0-q1*q2*2.0;
A0[1][0] = q0*q0-q1*q1+q2*q2-q3*q3;
EKF/matlab/scripts/Inertial Nav EKF/yaw_input_321.c
+2
View File
@@ -0,0 +1,2 @@
A0[0][0] = q0*q3*2.0+q1*q2*2.0;
A0[1][0] = q0*q0+q1*q1-q2*q2-q3*q3;
EKF/matlab/scripts/Terrain Estimator/GenerateEquationsTerrainEstimator.m
+91
View File
@@ -0,0 +1,91 @@
% IMPORTANT - This script requires the Matlab symbolic toolbox
% Derivation of EKF equations for estimation of terrain height offset
% Author: Paul Riseborough
% Last Modified: 20 June 2017
% State vector:
% terrain vertical position (ptd)
% Observations:
% line of sight (LOS) angular rate measurements (rel to sensor frame)
% from a downwards looking optical flow sensor measured in rad/sec about
% the X and Y sensor axes. These rates are motion compensated.
% A positive LOS X rate is a RH rotation of the image about the X sensor
% axis, and is produced by either a positive ground relative velocity in
% the direction of the Y axis.
% A positive LOS Y rate is a RH rotation of the image about the Y sensor
% axis, and is produced by either a negative ground relative velocity in
% the direction of the X axis.
% Range measurement aligned with the Z body axis (flat earth model assumed)
% Time varying parameters:
% quaternion parameters defining the rotation from navigation to body axes
% NED flight vehicle velocities
% vehicle vertical position
clear all;
%% define symbolic variables and constants
syms vel_x vel_y vel_z real % NED velocity : m/sec
syms R_OPT real % variance of LOS angular rate mesurements : (rad/sec)^2
syms R_RNG real % variance of range finder measurement : m^2
syms stateNoiseVar real % state process noise variance
syms pd real % position of vehicle in down axis : (m)
syms ptd real % position of terrain in down axis : (m)
syms q0 q1 q2 q3 real % quaternions defining attitude of body axes relative to local NED
syms Popt real % state variance
nStates = 1;
%% derive Jacobians for fusion of optical flow measurements
syms vel_x vel_y vel_z real % NED velocity : m/sec
syms pd real % position of vehicle in down axis : (m)
syms ptd real % position of terrain in down axis : (m)
syms q0 q1 q2 q3 real % quaternions defining attitude of body axes relative to local NED
% derive the body to nav direction cosine matrix
Tbn = Quat2Tbn([q0,q1,q2,q3]);
% calculate relative velocity in sensor frame
relVelSensor = transpose(Tbn)*[vel_x;vel_y;vel_z];
% calculate range to centre of flow sensor fov assuming flat earth
range = ((ptd - pd)/Tbn(3,3));
% divide velocity by range to get predicted motion compensated flow rates
optRateX = relVelSensor(2)/range;
optRateY = -relVelSensor(1)/range;
% calculate the observation jacobians
H_OPTX = jacobian(optRateX,ptd);
H_OPTY = jacobian(optRateY,ptd);
H_OPT = [H_OPTX;H_OPTY];
ccode(H_OPT,'file','H_OPT.c');
fix_c_code('H_OPT.c');
clear all;
reset(symengine)
%% derive Jacobian for fusion of range finder measurements
syms vel_x vel_y vel_z real % NED velocity : m/sec
syms R_RNG real % variance of range finder measurement : m^2
syms pd real % position of vehicle in down axis : (m)
syms ptd real % position of terrain in down axis : (m)
syms q0 q1 q2 q3 real % quaternions defining attitude of body axes relative to local NED
% derive the body to nav direction cosine matrix
Tbn = Quat2Tbn([q0,q1,q2,q3]);
% calculate range assuming flat earth
range = ((ptd - pd)/Tbn(3,3));
% calculate range observation Jacobian
H_RNG = jacobian(range,ptd); % measurement Jacobian
ccode(H_RNG,'file','H_RNG.c');
fix_c_code('H_RNG.c');
EKF/matlab/scripts/Terrain Estimator/H_OPT.c
+10
View File
@@ -0,0 +1,10 @@
t2 = q0*q0;
t3 = q1*q1;
t4 = q2*q2;
t5 = q3*q3;
t6 = pd-ptd;
t7 = 1.0/(t6*t6);
t8 = q0*q3*2.0;
t9 = t2-t3-t4+t5;
A0[0][0] = -t7*t9*(vel_z*(q0*q1*2.0+q2*q3*2.0)+vel_y*(t2-t3+t4-t5)-vel_x*(t8-q1*q2*2.0));
A0[1][0] = t7*t9*(-vel_z*(q0*q2*2.0-q1*q3*2.0)+vel_x*(t2+t3-t4-t5)+vel_y*(t8+q1*q2*2.0));
EKF/matlab/scripts/Terrain Estimator/H_RNG.c
+1
View File
@@ -0,0 +1 @@
A0[0][0] = 1.0/(q0*q0-q1*q1-q2*q2+q3*q3);