dark0707
/
px4-firmware
forked from Archive/PX4-Autopilot
1
0
Fork 0
Code Pull Requests Activity
px4-firmware/matlab/scripts/Inertial Nav EKF/SaveScriptCode.m

667 lines
19 KiB
Matlab
Raw Blame History

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
View Git Blame Copy Permalink