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

EKF: Provide auto-code output that uses (row, col) syntax for covariance matrix

Browse Source
This commit is contained in:
Paul Riseborough 2019-12-12 08:30:53 +11:00 committed by Paul Riseborough
parent 92ba618f57
commit 166d260058
3 changed files with 747 additions and 2 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

686
EKF/matlab/scripts/Inertial Nav EKF/C_code_use_matrix_lib24.txt Normal file
View File

File diff suppressed because one or more lines are too long

58
EKF/matlab/scripts/Inertial Nav EKF/ConvertCToC.m Normal file
View File

@ -0,0 +1,58 @@
function ConvertCToC(nStates)
% This function converts the nextP and P covaraince matrix expressions from
% P[row index][col index] to P(row index,col index) syntax to enable use
% of the matrix library type used by PX4
%% Define file to read in
fileName = strcat('C_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;
%% Replace [row][col] brackets for nextP and P arrays with (row,col)
% replace 2-D left indexes
for rowIndex = 1:nStates
for colIndex = 1:nStates
strRowIndex = int2str(rowIndex-1);
strColIndex = int2str(colIndex-1);
strRep = sprintf('P(%d,%d)',(rowIndex-1),(colIndex-1));
strPat = strcat('P\[',strRowIndex,'\]\[',strColIndex,'\]');
for lineIndex = 1:length(SymbolicOutput)
str = char(SymbolicOutput(lineIndex));
SymbolicOutput(lineIndex) = {regexprep(str, strPat, strRep)};
end
end
end
%% Write to file
fileName = strcat('C_code_use_matrix_lib',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;

5
EKF/matlab/scripts/Inertial Nav EKF/GenerateNavFilterEquations.m
View File

@ -510,5 +510,6 @@ load('Drag.mat');
fileName = strcat('SymbolicOutput',int2str(nStates),'.mat');
save(fileName);
SaveScriptCode(nStates);
ConvertToM(nStates);
ConvertToC(nStates);
ConvertToM(nStates); % convert symbolic expressions to Matlab expressions
ConvertToC(nStates); % convert Matlab expressions to C code expressions
ConvertCtoC(nStates); % convert covariance matrix expressions from array to matrix syntax