From 28d664aed5fdc6fc4d5bced9cf6e22e5cce96e7c Mon Sep 17 00:00:00 2001 From: bresch Date: Fri, 30 Dec 2022 09:29:42 +0100 Subject: [PATCH] EKF2: purge outdated matlab scripts Those files are no longer maintained and will just confuse new users --- .../EKF/matlab/EKF_replay/Common/AlignTilt.m | 20 - .../EKF/matlab/EKF_replay/Common/ConvertToC.m | 220 -------- .../EKF/matlab/EKF_replay/Common/ConvertToM.m | 46 -- .../EKF/matlab/EKF_replay/Common/EulToQuat.m | 21 - .../EKF/matlab/EKF_replay/Common/LLH2NED.m | 12 - .../EKF/matlab/EKF_replay/Common/NormQuat.m | 5 - .../EKF_replay/Common/OptimiseAlgebra.m | 29 - .../EKF/matlab/EKF_replay/Common/Quat2Tbn.m | 14 - .../EKF/matlab/EKF_replay/Common/QuatDivide.m | 16 - .../EKF/matlab/EKF_replay/Common/QuatMult.m | 5 - .../EKF/matlab/EKF_replay/Common/QuatToEul.m | 9 - .../EKF/matlab/EKF_replay/Common/RotToQuat.m | 10 - .../EKF_replay/Common/convert_apm_data.m | 103 ---- .../Common/convert_px4_actuators_csv_data.m | 34 -- .../convert_px4_distance_sensor_csv_data.m | 18 - .../Common/convert_px4_groundtruth_csv_data.m | 53 -- .../convert_px4_optical_flow_csv_data.m | 23 - .../convert_px4_sensor_combined_csv_data.m | 47 -- .../convert_px4_vehicle_gps_position_csv.m | 32 -- .../matlab/EKF_replay/Filter/AlignHeading.m | 23 - .../EKF_replay/Filter/ConstrainStates.m | 23 - .../matlab/EKF_replay/Filter/FuseBaroHeight.m | 56 -- .../matlab/EKF_replay/Filter/FuseBodyVel.m | 82 --- .../EKF_replay/Filter/FuseMagDeclination.m | 47 -- .../matlab/EKF_replay/Filter/FuseMagHeading.m | 71 --- .../EKF_replay/Filter/FuseMagnetometer.m | 87 --- .../EKF_replay/Filter/FuseOpticalFlow.m | 88 --- .../matlab/EKF_replay/Filter/FusePosition.m | 68 --- .../matlab/EKF_replay/Filter/FuseVelocity.m | 68 --- .../EKF_replay/Filter/GenerateEquations24.m | 211 -------- .../matlab/EKF_replay/Filter/InitCovariance.m | 35 -- .../EKF/matlab/EKF_replay/Filter/InitStates.m | 66 --- .../EKF/matlab/EKF_replay/Filter/PlotData.m | 512 ------------------ .../EKF_replay/Filter/PredictCovariance.m | 66 --- .../matlab/EKF_replay/Filter/PredictStates.m | 85 --- .../EKF/matlab/EKF_replay/Filter/RunFilter.m | 347 ------------ .../EKF_replay/Filter/SetParameterDefaults.m | 69 --- .../matlab/EKF_replay/Filter/SetParameters.m | 69 --- .../EKF/matlab/EKF_replay/Filter/calcF24.m | 45 -- .../EKF/matlab/EKF_replay/Filter/calcH_HDG.m | 51 -- .../EKF/matlab/EKF_replay/Filter/calcH_LOSX.m | 9 - .../EKF/matlab/EKF_replay/Filter/calcH_LOSY.m | 9 - .../EKF/matlab/EKF_replay/Filter/calcH_MAGD.m | 12 - .../EKF/matlab/EKF_replay/Filter/calcH_MAGX.m | 8 - .../EKF/matlab/EKF_replay/Filter/calcH_MAGY.m | 8 - .../EKF/matlab/EKF_replay/Filter/calcH_MAGZ.m | 8 - .../EKF/matlab/EKF_replay/Filter/calcH_VELX.m | 8 - .../EKF/matlab/EKF_replay/Filter/calcH_VELY.m | 8 - .../EKF/matlab/EKF_replay/Filter/calcH_VELZ.m | 8 - .../EKF/matlab/EKF_replay/Filter/calcQ24.m | 45 -- .../EKF_replay/Filter/find_best_gps_delay.m | 50 -- .../quat_to_euler_error_transfer_matrix.m | 38 -- .../EKF_replay/Filter/replay_apm_data.m | 46 -- .../EKF_replay/Filter/replay_px4_data.m | 30 - .../Filter/replay_px4_optflow_data.m | 32 -- .../EKF_replay/Filter/transfer_matrix.m | 13 - .../EKF_replay/SensorCalibration/allan.m | 19 - .../compare_mag_calibration.m | 189 ------- .../SensorCalibration/ellipsoid_fit.m | 61 --- .../EKF/matlab/EKF_replay/px4_replay_import.m | 387 ------------- .../ekf2/EKF/matlab/EKF_replay/readme.txt | 108 ---- src/modules/ekf2/EKF/matlab/README.md | 3 - .../matlab/analysis/estimatorLogViewerPX4.m | 133 ----- .../ekf2/EKF/matlab/analysis/importPX4log.m | 199 ------- .../ekf2/EKF/matlab/analysis/usageSamples.m | 16 - .../GenerateEquationsTerrainEstimator.m | 91 ---- .../matlab/scripts/Terrain Estimator/H_OPT.c | 10 - .../matlab/scripts/Terrain Estimator/H_RNG.c | 1 - 68 files changed, 4435 deletions(-) delete mode 100644 src/modules/ekf2/EKF/matlab/EKF_replay/Common/AlignTilt.m delete mode 100644 src/modules/ekf2/EKF/matlab/EKF_replay/Common/ConvertToC.m delete mode 100644 src/modules/ekf2/EKF/matlab/EKF_replay/Common/ConvertToM.m delete mode 100644 src/modules/ekf2/EKF/matlab/EKF_replay/Common/EulToQuat.m delete mode 100644 src/modules/ekf2/EKF/matlab/EKF_replay/Common/LLH2NED.m delete mode 100644 src/modules/ekf2/EKF/matlab/EKF_replay/Common/NormQuat.m delete mode 100644 src/modules/ekf2/EKF/matlab/EKF_replay/Common/OptimiseAlgebra.m delete mode 100644 src/modules/ekf2/EKF/matlab/EKF_replay/Common/Quat2Tbn.m delete mode 100644 src/modules/ekf2/EKF/matlab/EKF_replay/Common/QuatDivide.m delete mode 100644 src/modules/ekf2/EKF/matlab/EKF_replay/Common/QuatMult.m delete mode 100644 src/modules/ekf2/EKF/matlab/EKF_replay/Common/QuatToEul.m delete mode 100644 src/modules/ekf2/EKF/matlab/EKF_replay/Common/RotToQuat.m delete mode 100755 src/modules/ekf2/EKF/matlab/EKF_replay/Common/convert_apm_data.m delete mode 100644 src/modules/ekf2/EKF/matlab/EKF_replay/Common/convert_px4_actuators_csv_data.m delete mode 100755 src/modules/ekf2/EKF/matlab/EKF_replay/Common/convert_px4_distance_sensor_csv_data.m delete mode 100644 src/modules/ekf2/EKF/matlab/EKF_replay/Common/convert_px4_groundtruth_csv_data.m delete mode 100755 src/modules/ekf2/EKF/matlab/EKF_replay/Common/convert_px4_optical_flow_csv_data.m delete mode 100755 src/modules/ekf2/EKF/matlab/EKF_replay/Common/convert_px4_sensor_combined_csv_data.m delete mode 100755 src/modules/ekf2/EKF/matlab/EKF_replay/Common/convert_px4_vehicle_gps_position_csv.m delete mode 100644 src/modules/ekf2/EKF/matlab/EKF_replay/Filter/AlignHeading.m delete mode 100644 src/modules/ekf2/EKF/matlab/EKF_replay/Filter/ConstrainStates.m delete mode 100644 src/modules/ekf2/EKF/matlab/EKF_replay/Filter/FuseBaroHeight.m delete mode 100644 src/modules/ekf2/EKF/matlab/EKF_replay/Filter/FuseBodyVel.m delete mode 100644 src/modules/ekf2/EKF/matlab/EKF_replay/Filter/FuseMagDeclination.m delete mode 100644 src/modules/ekf2/EKF/matlab/EKF_replay/Filter/FuseMagHeading.m delete mode 100644 src/modules/ekf2/EKF/matlab/EKF_replay/Filter/FuseMagnetometer.m delete mode 100644 src/modules/ekf2/EKF/matlab/EKF_replay/Filter/FuseOpticalFlow.m delete mode 100644 src/modules/ekf2/EKF/matlab/EKF_replay/Filter/FusePosition.m delete mode 100644 src/modules/ekf2/EKF/matlab/EKF_replay/Filter/FuseVelocity.m delete mode 100755 src/modules/ekf2/EKF/matlab/EKF_replay/Filter/GenerateEquations24.m delete mode 100644 src/modules/ekf2/EKF/matlab/EKF_replay/Filter/InitCovariance.m delete mode 100644 src/modules/ekf2/EKF/matlab/EKF_replay/Filter/InitStates.m delete mode 100755 src/modules/ekf2/EKF/matlab/EKF_replay/Filter/PlotData.m delete mode 100644 src/modules/ekf2/EKF/matlab/EKF_replay/Filter/PredictCovariance.m delete mode 100755 src/modules/ekf2/EKF/matlab/EKF_replay/Filter/PredictStates.m delete mode 100755 src/modules/ekf2/EKF/matlab/EKF_replay/Filter/RunFilter.m delete mode 100755 src/modules/ekf2/EKF/matlab/EKF_replay/Filter/SetParameterDefaults.m delete mode 100755 src/modules/ekf2/EKF/matlab/EKF_replay/Filter/SetParameters.m delete mode 100644 src/modules/ekf2/EKF/matlab/EKF_replay/Filter/calcF24.m delete mode 100644 src/modules/ekf2/EKF/matlab/EKF_replay/Filter/calcH_HDG.m delete mode 100644 src/modules/ekf2/EKF/matlab/EKF_replay/Filter/calcH_LOSX.m delete mode 100644 src/modules/ekf2/EKF/matlab/EKF_replay/Filter/calcH_LOSY.m delete mode 100644 src/modules/ekf2/EKF/matlab/EKF_replay/Filter/calcH_MAGD.m delete mode 100644 src/modules/ekf2/EKF/matlab/EKF_replay/Filter/calcH_MAGX.m delete mode 100644 src/modules/ekf2/EKF/matlab/EKF_replay/Filter/calcH_MAGY.m delete mode 100644 src/modules/ekf2/EKF/matlab/EKF_replay/Filter/calcH_MAGZ.m delete mode 100644 src/modules/ekf2/EKF/matlab/EKF_replay/Filter/calcH_VELX.m delete mode 100644 src/modules/ekf2/EKF/matlab/EKF_replay/Filter/calcH_VELY.m delete mode 100644 src/modules/ekf2/EKF/matlab/EKF_replay/Filter/calcH_VELZ.m delete mode 100644 src/modules/ekf2/EKF/matlab/EKF_replay/Filter/calcQ24.m delete mode 100755 src/modules/ekf2/EKF/matlab/EKF_replay/Filter/find_best_gps_delay.m delete mode 100644 src/modules/ekf2/EKF/matlab/EKF_replay/Filter/quat_to_euler_error_transfer_matrix.m delete mode 100755 src/modules/ekf2/EKF/matlab/EKF_replay/Filter/replay_apm_data.m delete mode 100755 src/modules/ekf2/EKF/matlab/EKF_replay/Filter/replay_px4_data.m delete mode 100644 src/modules/ekf2/EKF/matlab/EKF_replay/Filter/replay_px4_optflow_data.m delete mode 100644 src/modules/ekf2/EKF/matlab/EKF_replay/Filter/transfer_matrix.m delete mode 100644 src/modules/ekf2/EKF/matlab/EKF_replay/SensorCalibration/allan.m delete mode 100644 src/modules/ekf2/EKF/matlab/EKF_replay/SensorCalibration/compare_mag_calibration.m delete mode 100644 src/modules/ekf2/EKF/matlab/EKF_replay/SensorCalibration/ellipsoid_fit.m delete mode 100644 src/modules/ekf2/EKF/matlab/EKF_replay/px4_replay_import.m delete mode 100644 src/modules/ekf2/EKF/matlab/EKF_replay/readme.txt delete mode 100644 src/modules/ekf2/EKF/matlab/README.md delete mode 100644 src/modules/ekf2/EKF/matlab/analysis/estimatorLogViewerPX4.m delete mode 100644 src/modules/ekf2/EKF/matlab/analysis/importPX4log.m delete mode 100644 src/modules/ekf2/EKF/matlab/analysis/usageSamples.m delete mode 100644 src/modules/ekf2/EKF/matlab/scripts/Terrain Estimator/GenerateEquationsTerrainEstimator.m delete mode 100644 src/modules/ekf2/EKF/matlab/scripts/Terrain Estimator/H_OPT.c delete mode 100644 src/modules/ekf2/EKF/matlab/scripts/Terrain Estimator/H_RNG.c diff --git a/src/modules/ekf2/EKF/matlab/EKF_replay/Common/AlignTilt.m b/src/modules/ekf2/EKF/matlab/EKF_replay/Common/AlignTilt.m deleted file mode 100644 index fed5afc060..0000000000 --- a/src/modules/ekf2/EKF/matlab/EKF_replay/Common/AlignTilt.m +++ /dev/null @@ -1,20 +0,0 @@ -function quat = AlignTilt( ... - quat, ... % quaternion state vector - initAccel) % initial accelerometer vector -% check length -lengthAccel = sqrt(dot([initAccel(1);initAccel(2);initAccel(3)],[initAccel(1);initAccel(2);initAccel(3)])); -% if length is > 0.7g and < 1.4g initialise tilt using gravity vector -if (lengthAccel > 5 && lengthAccel < 14) - % calculate length of the tilt vector - tiltMagnitude = atan2(sqrt(dot([initAccel(1);initAccel(2)],[initAccel(1);initAccel(2)])),-initAccel(3)); - % take the unit cross product of the accel vector and the -Z vector to - % give the tilt unit vector - if (tiltMagnitude > 1e-3) - tiltUnitVec = cross([initAccel(1);initAccel(2);initAccel(3)],[0;0;-1]); - tiltUnitVec = tiltUnitVec/sqrt(dot(tiltUnitVec,tiltUnitVec)); - tiltVec = tiltMagnitude*tiltUnitVec; - quat = [cos(0.5*tiltMagnitude); tiltVec/tiltMagnitude*sin(0.5*tiltMagnitude)]; - end -end - -end \ No newline at end of file diff --git a/src/modules/ekf2/EKF/matlab/EKF_replay/Common/ConvertToC.m b/src/modules/ekf2/EKF/matlab/EKF_replay/Common/ConvertToC.m deleted file mode 100644 index 762ed278a5..0000000000 --- a/src/modules/ekf2/EKF/matlab/EKF_replay/Common/ConvertToC.m +++ /dev/null @@ -1,220 +0,0 @@ -function ConvertToC(fileName) -delimiter = ''; - -%% Format string for each line of text: -% column1: text (%s) -% For more information, see the TEXTSCAN documentation. -formatSpec = '%s%[^\n\r]'; - -%% Open the text file. -fileID = fopen(strcat(fileName,'.m'),'r'); - -%% Read columns of data according to format string. -% This call is based on the structure of the file used to generate this -% code. If an error occurs for a different file, try regenerating the code -% from the Import Tool. -dataArray = textscan(fileID, formatSpec, 'Delimiter', delimiter, 'ReturnOnError', false, 'Bufsize', 65535); - -%% Close the text file. -fclose(fileID); - -%% Create output variable -SymbolicOutput = [dataArray{1:end-1}]; - -%% Clear temporary variables -clearvars filename delimiter formatSpec fileID dataArray ans; - -%% Convert indexing and replace brackets - -% replace 1-D indexes -for arrayIndex = 1:99 - strIndex = int2str(arrayIndex); - strRep = sprintf('[%d]',(arrayIndex-1)); - strPat = strcat('\(',strIndex,'\)'); - for lineIndex = 1:length(SymbolicOutput) - str = char(SymbolicOutput(lineIndex)); - SymbolicOutput(lineIndex) = {regexprep(str, strPat, strRep)}; - end -end - -% replace 2-D left indexes -for arrayIndex = 1:99 - strIndex = int2str(arrayIndex); - strRep = sprintf('[%d,',(arrayIndex-1)); - strPat = strcat('\(',strIndex,'\,'); - for lineIndex = 1:length(SymbolicOutput) - str = char(SymbolicOutput(lineIndex)); - SymbolicOutput(lineIndex) = {regexprep(str, strPat, strRep)}; - end -end - -% replace 2-D right indexes -for arrayIndex = 1:99 - strIndex = int2str(arrayIndex); - strRep = sprintf(',%d]',(arrayIndex-1)); - strPat = strcat('\,',strIndex,'\)'); - for lineIndex = 1:length(SymbolicOutput) - str = char(SymbolicOutput(lineIndex)); - SymbolicOutput(lineIndex) = {regexprep(str, strPat, strRep)}; - end -end - -% replace commas -for lineIndex = 1:length(SymbolicOutput) - str = char(SymbolicOutput(lineIndex)); - SymbolicOutput(lineIndex) = {regexprep(str, '\,', '][')}; -end - -%% replace . operators -for lineIndex = 1:length(SymbolicOutput) - strIn = char(SymbolicOutput(lineIndex)); - strIn = regexprep(strIn,'\.\*','\*'); - strIn = regexprep(strIn,'\.\/','\/'); - strIn = regexprep(strIn,'\.\^','\^'); - SymbolicOutput(lineIndex) = cellstr(strIn); -end - -%% Replace ^2 - -% replace where adjacent to ) parenthesis -for lineIndex = 1:length(SymbolicOutput) - idxsq = regexp(SymbolicOutput(lineIndex),'\)\^2'); - if ~isempty(idxsq{1}) - strIn = SymbolicOutput(lineIndex); - idxlp = regexp(strIn,'\('); - idxrp = regexp(strIn,'\)'); - for pwrIndex = 1:length(idxsq{1}) - counter = 1; - index = idxsq{1}(pwrIndex); - endIndex(pwrIndex) = index; - while (counter > 0 && index > 0) - index = index - 1; - counter = counter + ~isempty(find(idxrp{1} == index, 1)); - counter = counter - ~isempty(find(idxlp{1} == index, 1)); - end - startIndex(pwrIndex) = index; - % strPat = strcat(strIn{1}(startIndex(pwrIndex):endIndex(pwrIndex)),'^2'); - strRep = strcat('sq',strIn{1}(startIndex(pwrIndex):endIndex(pwrIndex))); - % cellStrPat(pwrIndex) = cellstr(strPat); - cellStrRep(pwrIndex) = cellstr(strRep); - end - for pwrIndex = 1:length(idxsq{1}) - strRep = char(cellStrRep(pwrIndex)); - strIn{1}(startIndex(pwrIndex):endIndex(pwrIndex)+2) = strRep; - end - SymbolicOutput(lineIndex) = strIn; - end -end - -% replace where adjacent to ] parenthesis -for lineIndex = 1:length(SymbolicOutput) - strIn = char(SymbolicOutput(lineIndex)); - [match,idxsq1,idxsq2] = regexp(strIn,'\w*\[\w*\]\^2','match','start','end'); - [idxsq3] = regexp(strIn,'\[\w*\]\^2','start'); - if ~isempty(match) - for pwrIndex = 1:length(match) - strVar = strIn(idxsq1(pwrIndex):idxsq3(pwrIndex)-1); - strIndex = strIn(idxsq3(pwrIndex)+1:idxsq2(pwrIndex)-3); - strPat = strcat(strVar,'\[',strIndex,'\]\^2'); - strRep = strcat('sq(',strVar,'[',strIndex,']',')'); - strIn = regexprep(strIn,strPat,strRep); - end - SymbolicOutput(lineIndex) = cellstr(strIn); - end -end - -% replace where adjacent to alpanumeric characters -for lineIndex = 1:length(SymbolicOutput) - strIn = char(SymbolicOutput(lineIndex)); - [match,idxsq1,idxsq2] = regexp(strIn,'\w*\^2','match','start','end'); - [idxsq3] = regexp(strIn,'\^2','start'); - if ~isempty(match) - for pwrIndex = 1:length(match) - strVar = strIn(idxsq1(pwrIndex)+2*(pwrIndex-1):idxsq2(pwrIndex)-2+2*(pwrIndex-1)); - strPat = strcat(strVar,'\^2'); - strRep = strcat('sq(',strVar,')'); - strIn = regexprep(strIn,strPat,strRep); - end - SymbolicOutput(lineIndex) = cellstr(strIn); - end -end - -%% Replace ^(1/2) - -% replace where adjacent to ) parenthesis -for lineIndex = 1:length(SymbolicOutput) - idxsq = regexp(SymbolicOutput(lineIndex),'\)\^\(1\/2\)'); - if ~isempty(idxsq{1}) - strIn = SymbolicOutput(lineIndex); - idxlp = regexp(strIn,'\('); - idxrp = regexp(strIn,'\)'); - for pwrIndex = 1:length(idxsq{1}) - counter = 1; - index = idxsq{1}(pwrIndex); - endIndex(pwrIndex) = index; - while (counter > 0 && index > 0) - index = index - 1; - counter = counter + ~isempty(find(idxrp{1} == index, 1)); - counter = counter - ~isempty(find(idxlp{1} == index, 1)); - end - startIndex(pwrIndex) = index; - % strPat = strcat(strIn{1}(startIndex(pwrIndex):endIndex(pwrIndex)),'^2'); - strRep = strcat('(sqrt',strIn{1}(startIndex(pwrIndex):endIndex(pwrIndex)),')'); - % cellStrPat(pwrIndex) = cellstr(strPat); - cellStrRep(pwrIndex) = cellstr(strRep); - end - for pwrIndex = 1:length(idxsq{1}) - strRep = char(cellStrRep(pwrIndex)); - strIn{1}(startIndex(pwrIndex):endIndex(pwrIndex)+6) = strRep; - end - SymbolicOutput(lineIndex) = strIn; - end -end - -%% Replace Divisions -% Compiler looks after this type of optimisation for us -% for lineIndex = 1:length(SymbolicOutput) -% strIn = char(SymbolicOutput(lineIndex)); -% strIn = regexprep(strIn,'\/2','\*0\.5'); -% strIn = regexprep(strIn,'\/4','\*0\.25'); -% SymbolicOutput(lineIndex) = cellstr(strIn); -% end - -%% Convert declarations -for lineIndex = 1:length(SymbolicOutput) - str = char(SymbolicOutput(lineIndex)); - if ~isempty(regexp(str,'zeros', 'once')) - index1 = regexp(str,' = zeros[','once')-1; - index2 = regexp(str,' = zeros[','end','once')+1; - index3 = regexp(str,'\]\[','once')-1; - index4 = index3 + 3; - index5 = max(regexp(str,'\]'))-1; - str1 = {'float '}; - str2 = str(1:index1); - str3 = '['; - str4 = str(index2:index3); - str4 = num2str(str2num(str4)+1); - str5 = ']['; - str6 = str(index4:index5); - str6 = num2str(str2num(str6)+1); - str7 = '];'; - SymbolicOutput(lineIndex) = strcat(str1,str2,str3,str4,str5,str6,str7); - end -end - -%% Change covariance matrix variable name to P -for lineIndex = 1:length(SymbolicOutput) - strIn = char(SymbolicOutput(lineIndex)); - strIn = regexprep(strIn,'OP\[','P['); - SymbolicOutput(lineIndex) = cellstr(strIn); -end - -%% Write to file -fileName = strcat(fileName,'.cpp'); -fid = fopen(fileName,'wt'); -for lineIndex = 1:length(SymbolicOutput) - fprintf(fid,char(SymbolicOutput(lineIndex))); - fprintf(fid,'\n'); -end -fclose(fid); -clear all; \ No newline at end of file diff --git a/src/modules/ekf2/EKF/matlab/EKF_replay/Common/ConvertToM.m b/src/modules/ekf2/EKF/matlab/EKF_replay/Common/ConvertToM.m deleted file mode 100644 index 1e5f7f4388..0000000000 --- a/src/modules/ekf2/EKF/matlab/EKF_replay/Common/ConvertToM.m +++ /dev/null @@ -1,46 +0,0 @@ -function ConvertToM(nStates) -%% Initialize variables -fileName = strcat('SymbolicOutput',int2str(nStates),'.txt'); -delimiter = ''; - -%% Format string for each line of text: -% column1: text (%s) -% For more information, see the TEXTSCAN documentation. -formatSpec = '%s%[^\n\r]'; - -%% Open the text file. -fileID = fopen(fileName,'r'); - -%% Read columns of data according to format string. -% This call is based on the structure of the file used to generate this -% code. If an error occurs for a different file, try regenerating the code -% from the Import Tool. -dataArray = textscan(fileID, formatSpec, 'Delimiter', delimiter, 'ReturnOnError', false,'Bufsize',65535); - -%% Close the text file. -fclose(fileID); - -%% Create output variable -SymbolicOutput = [dataArray{1:end-1}]; - -%% Clear temporary variables -clearvars filename delimiter formatSpec fileID dataArray ans; - -%% replace brackets and commas -for lineIndex = 1:length(SymbolicOutput) - SymbolicOutput(lineIndex) = regexprep(SymbolicOutput(lineIndex), '_l_', '('); - SymbolicOutput(lineIndex) = regexprep(SymbolicOutput(lineIndex), '_c_', ','); - SymbolicOutput(lineIndex) = regexprep(SymbolicOutput(lineIndex), '_r_', ')'); -end - -%% Write to file -fileName = strcat('M_code',int2str(nStates),'.txt'); -fid = fopen(fileName,'wt'); -for lineIndex = 1:length(SymbolicOutput) - fprintf(fid,char(SymbolicOutput(lineIndex))); - fprintf(fid,'\n'); -end -fclose(fid); -clear all; - -end \ No newline at end of file diff --git a/src/modules/ekf2/EKF/matlab/EKF_replay/Common/EulToQuat.m b/src/modules/ekf2/EKF/matlab/EKF_replay/Common/EulToQuat.m deleted file mode 100644 index 89d1ca5092..0000000000 --- a/src/modules/ekf2/EKF/matlab/EKF_replay/Common/EulToQuat.m +++ /dev/null @@ -1,21 +0,0 @@ -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; diff --git a/src/modules/ekf2/EKF/matlab/EKF_replay/Common/LLH2NED.m b/src/modules/ekf2/EKF/matlab/EKF_replay/Common/LLH2NED.m deleted file mode 100644 index 72ed981215..0000000000 --- a/src/modules/ekf2/EKF/matlab/EKF_replay/Common/LLH2NED.m +++ /dev/null @@ -1,12 +0,0 @@ -function posNED = LLH2NED(LLH,refLLH) - -radius = 6378137; -flattening = 1/298.257223563; -e = sqrt(flattening*(2-flattening)); -Rm = radius*(1-e^2)/(1-e^2*sin(refLLH(1)*pi/180)^2)^(3/2); -Rn = radius/(1-e^2*sin(refLLH(1)*pi/180)^2)^(1/2); -posN = (LLH(1,:)-refLLH(1))*pi/180.*(Rm+LLH(3,:)); -posE = (LLH(2,:)-refLLH(2))*pi/180.*(Rn+LLH(3,:))*cos(refLLH(1)*pi/180); -posD = -(LLH(3,:)-refLLH(3)); -posNED = [posN;posE;posD]; -end \ No newline at end of file diff --git a/src/modules/ekf2/EKF/matlab/EKF_replay/Common/NormQuat.m b/src/modules/ekf2/EKF/matlab/EKF_replay/Common/NormQuat.m deleted file mode 100644 index 7d1913efda..0000000000 --- a/src/modules/ekf2/EKF/matlab/EKF_replay/Common/NormQuat.m +++ /dev/null @@ -1,5 +0,0 @@ -% 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; diff --git a/src/modules/ekf2/EKF/matlab/EKF_replay/Common/OptimiseAlgebra.m b/src/modules/ekf2/EKF/matlab/EKF_replay/Common/OptimiseAlgebra.m deleted file mode 100644 index 16d4c353ca..0000000000 --- a/src/modules/ekf2/EKF/matlab/EKF_replay/Common/OptimiseAlgebra.m +++ /dev/null @@ -1,29 +0,0 @@ -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 \ No newline at end of file diff --git a/src/modules/ekf2/EKF/matlab/EKF_replay/Common/Quat2Tbn.m b/src/modules/ekf2/EKF/matlab/EKF_replay/Common/Quat2Tbn.m deleted file mode 100644 index 3748f965a6..0000000000 --- a/src/modules/ekf2/EKF/matlab/EKF_replay/Common/Quat2Tbn.m +++ /dev/null @@ -1,14 +0,0 @@ -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]; diff --git a/src/modules/ekf2/EKF/matlab/EKF_replay/Common/QuatDivide.m b/src/modules/ekf2/EKF/matlab/EKF_replay/Common/QuatDivide.m deleted file mode 100644 index 20e789dcfb..0000000000 --- a/src/modules/ekf2/EKF/matlab/EKF_replay/Common/QuatDivide.m +++ /dev/null @@ -1,16 +0,0 @@ -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); diff --git a/src/modules/ekf2/EKF/matlab/EKF_replay/Common/QuatMult.m b/src/modules/ekf2/EKF/matlab/EKF_replay/Common/QuatMult.m deleted file mode 100644 index 8572529091..0000000000 --- a/src/modules/ekf2/EKF/matlab/EKF_replay/Common/QuatMult.m +++ /dev/null @@ -1,5 +0,0 @@ -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))]; \ No newline at end of file diff --git a/src/modules/ekf2/EKF/matlab/EKF_replay/Common/QuatToEul.m b/src/modules/ekf2/EKF/matlab/EKF_replay/Common/QuatToEul.m deleted file mode 100644 index 09b1505b6f..0000000000 --- a/src/modules/ekf2/EKF/matlab/EKF_replay/Common/QuatToEul.m +++ /dev/null @@ -1,9 +0,0 @@ -% 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)); \ No newline at end of file diff --git a/src/modules/ekf2/EKF/matlab/EKF_replay/Common/RotToQuat.m b/src/modules/ekf2/EKF/matlab/EKF_replay/Common/RotToQuat.m deleted file mode 100644 index 3c9777acd5..0000000000 --- a/src/modules/ekf2/EKF/matlab/EKF_replay/Common/RotToQuat.m +++ /dev/null @@ -1,10 +0,0 @@ -% 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 \ No newline at end of file diff --git a/src/modules/ekf2/EKF/matlab/EKF_replay/Common/convert_apm_data.m b/src/modules/ekf2/EKF/matlab/EKF_replay/Common/convert_apm_data.m deleted file mode 100755 index f17dc96667..0000000000 --- a/src/modules/ekf2/EKF/matlab/EKF_replay/Common/convert_apm_data.m +++ /dev/null @@ -1,103 +0,0 @@ -%% convert baro data -clear baro_data; -last_time = 0; -output_index = 1; -for source_index = 1:length(BARO) - if (BARO(source_index,2) ~= last_time) - baro_data.time_us(output_index,1) = BARO(source_index,2); - baro_data.height(output_index) = BARO(source_index,3); - last_time = BARO(source_index,2); - output_index = output_index + 1; - end -end -save baro_data.mat baro_data; - -%% extract IMU delta angles and velocity data -clear imu_data; -imu_data.time_us = IMT(:,2); -imu_data.gyro_dt = IMT(:,5); -imu_data.del_ang = IMT(:,6:8); -imu_data.accel_dt = IMT(:,4); -imu_data.del_vel = IMT(:,9:11); -save imu_data.mat imu_data; - -%% convert magnetometer data -clear mag_data; -last_time = 0; -output_index = 1; -for source_index = 1:length(MAG) - mag_timestamp = MAG(source_index,2); - if (mag_timestamp ~= last_time) - mag_data.time_us(output_index,1) = mag_timestamp; - mag_data.field_ga(output_index,:) = 0.001*[MAG(source_index,3),MAG(source_index,4),MAG(source_index,5)]; - last_time = mag_timestamp; - output_index = output_index + 1; - end -end -save mag_data.mat mag_data; - -%% save GPS daa -clear gps_data; - -maxindex = min(length(GPS),length(GPA)); - -gps_data.time_us = GPS(1:maxindex,2); -gps_data.pos_error = GPA(1:maxindex,4); -gps_data.spd_error = GPA(1:maxindex,6); -gps_data.hgt_error = GPA(1:maxindex,5); - -% set reference point used to set NED origin when GPS accuracy is sufficient -gps_data.start_index = max(find(gps_data.pos_error < 5.0, 1 ),find(gps_data.spd_error < 1.0, 1 )); -gps_data.refLLH = [GPS(gps_data.start_index,8);GPS(gps_data.start_index,9);GPS(gps_data.start_index,10)]; - -% convert GPS data to NED -deg2rad = pi/180; -for index = 1:1:maxindex - if (GPS(index,3) >= 3) - gps_data.pos_ned(index,:) = LLH2NED([GPS(index,8);GPS(index,9);GPS(index,10)],gps_data.refLLH); - gps_data.vel_ned(index,:) = [GPS(index,11).*cos(deg2rad*GPS(index,12)),GPS(index,11).*sin(deg2rad*GPS(index,12)),GPS(index,13)]; - else - gps_data.pos_ned(index,:) = [0,0,0]; - gps_data.vel_ned(index,:) = [0,0,0]; - end -end - -save gps_data.mat gps_data; - -%% save range finder data -clear rng_data; -if (exist('RFND','var')) - rng_data.time_us = RFND(:,2); - rng_data.dist = RFND(:,3); - save rng_data.mat rng_data; -end - -%% save optical flow data -clear flow_data; -if (exist('OF','var')) - flow_data.time_us = OF(:,2); - flow_data.qual = OF(:,3)/255; % scale quality from 0 to 1 - flow_data.flowX = OF(:,4); % optical flow rate about the X body axis (rad/sec) - flow_data.flowY = OF(:,5); % optical flow rate about the Y body axis (rad/sec) - flow_data.bodyX = OF(:,6); % angular rate about the X body axis (rad/sec) - flow_data.bodyY = OF(:,7); % angular rate about the Y body axis (rad/sec) - save flow_data.mat flow_data; -end - -%% save visual odometry data -clear viso_data; -if (exist('VISO','var')) - viso_data.time_us = VISO(:,2); - viso_data.dt = VISO(:,3); % time period the measurement was sampled across (sec) - viso_data.dAngX = VISO(:,4); % delta angle about the X body axis (rad) - viso_data.dAngY = VISO(:,5); % delta angle about the Y body axis (rad) - viso_data.dAngZ = VISO(:,6); % delta angle about the Z body axis (rad) - viso_data.dPosX = VISO(:,7); % delta position along the X body axis (m) - viso_data.dPosY = VISO(:,8); % delta position along the Y body axis (m) - viso_data.dPosZ = VISO(:,9); % delta position along the Z body axis (m) - viso_data.qual = VISO(:,10)/100; % quality from 0 - 1 - save viso_data.mat viso_data; -end - -%% save data and clear workspace -clearvars -except baro_data imu_data mag_data gps_data rng_data flow_data viso_data; diff --git a/src/modules/ekf2/EKF/matlab/EKF_replay/Common/convert_px4_actuators_csv_data.m b/src/modules/ekf2/EKF/matlab/EKF_replay/Common/convert_px4_actuators_csv_data.m deleted file mode 100644 index 084514a677..0000000000 --- a/src/modules/ekf2/EKF/matlab/EKF_replay/Common/convert_px4_actuators_csv_data.m +++ /dev/null @@ -1,34 +0,0 @@ -%% convert actuator control data -clear actctrl_data; -last_time = 0; -output_index = 1; -for source_index = 1:length(timestamp_actctrl) - actctrl_timestamp = timestamp_actctrl(source_index); - if (actctrl_timestamp ~= last_time) - actctrl_data.time_us(output_index,1) = actctrl_timestamp; - actctrl_data.M_XYZ(output_index,:) = [control0(source_index),control1(source_index),control2(source_index)]; - actctrl_data.F_t(output_index,:) = control3(source_index); - last_time = actctrl_timestamp; - output_index = output_index + 1; - end -end - -%% convert actuator output data -clear actout_data; -last_time = 0; -output_index = 1; -for source_index = 1:length(timestamp_actout) - actout_timestamp = timestamp_actout(source_index); - if (actout_timestamp ~= last_time) - actout_data.time_us(output_index,1) = actout_timestamp; - actout_data.pwm(output_index,:) = [output0(source_index),output1(source_index),output2(source_index),output3(source_index)]; - last_time = actout_timestamp; - output_index = output_index + 1; - end -end - -%% save data -% DO NOT clear the workspace (yet) - -save actctrl_data.mat actctrl_data; -save actout_data.mat actout_data; diff --git a/src/modules/ekf2/EKF/matlab/EKF_replay/Common/convert_px4_distance_sensor_csv_data.m b/src/modules/ekf2/EKF/matlab/EKF_replay/Common/convert_px4_distance_sensor_csv_data.m deleted file mode 100755 index e5de236f30..0000000000 --- a/src/modules/ekf2/EKF/matlab/EKF_replay/Common/convert_px4_distance_sensor_csv_data.m +++ /dev/null @@ -1,18 +0,0 @@ -%% convert baro data -clear rng_data; -last_time = 0; -output_index = 1; -for source_index = 1:length(timestamp) - rng_timestamp = timestamp(source_index); - if (rng_timestamp ~= last_time) - last_time = rng_timestamp; - rng_data.time_us(output_index,1) = rng_timestamp; - rng_data.dist(output_index,1) = current_distance(source_index); - output_index = output_index + 1; - end -end - -%% save data and clear workspace -clearvars -except rng_data; - -save rng_data.mat rng_data; \ No newline at end of file diff --git a/src/modules/ekf2/EKF/matlab/EKF_replay/Common/convert_px4_groundtruth_csv_data.m b/src/modules/ekf2/EKF/matlab/EKF_replay/Common/convert_px4_groundtruth_csv_data.m deleted file mode 100644 index 8de4c45509..0000000000 --- a/src/modules/ekf2/EKF/matlab/EKF_replay/Common/convert_px4_groundtruth_csv_data.m +++ /dev/null @@ -1,53 +0,0 @@ -%% convert attitude data -clear attitude_data; -last_time = 0; -output_index = 1; -for source_index = 1:length(timestamp_att) - att_timestamp = timestamp_att(source_index); - if (att_timestamp ~= last_time) - attitude_data.time_us(output_index,1) = att_timestamp; - attitude_data.quat(output_index,:) = [q0(source_index),q1(source_index),q2(source_index),q3(source_index)]; - attitude_data.del_ang(output_index,:) = [rollspeed(source_index),pitchspeed(source_index),yawspeed(source_index)]; - last_time = att_timestamp; - output_index = output_index + 1; - end -end - -%% convert global position data -clear globalpos_data; -last_time = 0; -output_index = 1; -for source_index = 1:length(timestamp_gpos) - gpos_timestamp = timestamp_gpos(source_index); - if (gpos_timestamp ~= last_time) - globalpos_data.time_us(output_index,1) = gpos_timestamp; - globalpos_data.position_NED(output_index,:) = [gpos_lat(source_index),gpos_lon(source_index),gpos_alt(source_index)]; - globalpos_data.velocity_NED(output_index,:) = [gpos_vel_n(source_index),gpos_vel_e(source_index),gpos_vel_d(source_index)]; - last_time = gpos_timestamp; - output_index = output_index + 1; - end -end - -%% convert local position data -clear localpos_data; -last_time = 0; -output_index = 1; -for source_index = 1:length(timestamp_lpos) - lpos_timestamp = timestamp_lpos(source_index); - if (lpos_timestamp ~= last_time) - localpos_data.time_us(output_index,1) = lpos_timestamp; - localpos_data.ref_pos(output_index,:) = [lpos_ref_lat(source_index),lpos_ref_lon(source_index)]; - localpos_data.position_XYZ(output_index,:) = [lpos_x(source_index),lpos_y(source_index),lpos_z(source_index)]; - localpos_data.velocity_XYZ(output_index,:) = [lpos_vz(source_index),lpos_vy(source_index),lpos_vz(source_index)]; - localpos_data.yaw(output_index,1) = lpos_yaw(source_index); - last_time = lpos_timestamp; - output_index = output_index + 1; - end -end - -%% save data -% DO NOT clear the workspace (yet) - -save attitude_data.mat attitude_data; -save localpos_data.mat localpos_data; -save globalpos_data.mat globalpos_data; diff --git a/src/modules/ekf2/EKF/matlab/EKF_replay/Common/convert_px4_optical_flow_csv_data.m b/src/modules/ekf2/EKF/matlab/EKF_replay/Common/convert_px4_optical_flow_csv_data.m deleted file mode 100755 index e04b949469..0000000000 --- a/src/modules/ekf2/EKF/matlab/EKF_replay/Common/convert_px4_optical_flow_csv_data.m +++ /dev/null @@ -1,23 +0,0 @@ -%% convert baro data -clear flow_data; -last_time = 0; -output_index = 1; -for source_index = 1:length(timestamp) - flow_timestamp = timestamp(source_index); - if ((flow_timestamp ~= last_time) && (integration_timespan(source_index) > 0)) - last_time = flow_timestamp; - flow_data.time_us(output_index,1) = flow_timestamp; - flow_data.qual(output_index,1) = quality(source_index)/255; % scale quality from 0 to 1 - dt_inv = 1e6 / integration_timespan(source_index); - flow_data.flowX(output_index,1) = dt_inv * pixel_flow_x_integral(source_index); % optical flow rate about the X body axis (rad/sec) - flow_data.flowY(output_index,1) = dt_inv * pixel_flow_y_integral(source_index); % optical flow rate about the Y body axis (rad/sec) - flow_data.bodyX(output_index,1) = dt_inv * gyro_x_rate_integral(source_index); % angular rate about the X body axis (rad/sec) - flow_data.bodyY(output_index,1) = dt_inv * gyro_y_rate_integral(source_index); % angular rate about the Y body axis (rad/sec) - output_index = output_index + 1; - end -end - -%% save data and clear workspace -clearvars -except flow_data; - -save flow_data.mat flow_data; \ No newline at end of file diff --git a/src/modules/ekf2/EKF/matlab/EKF_replay/Common/convert_px4_sensor_combined_csv_data.m b/src/modules/ekf2/EKF/matlab/EKF_replay/Common/convert_px4_sensor_combined_csv_data.m deleted file mode 100755 index 6b5d1b6879..0000000000 --- a/src/modules/ekf2/EKF/matlab/EKF_replay/Common/convert_px4_sensor_combined_csv_data.m +++ /dev/null @@ -1,47 +0,0 @@ -%% convert baro data -clear baro_data; -last_time = 0; -output_index = 1; -for source_index = 1:length(timestamp_baro) - baro_timestamp = timestamp_baro(source_index); - if (baro_timestamp ~= last_time) - baro_data.time_us(output_index,1) = baro_timestamp; - baro_data.height(output_index) = baro_alt_meter(source_index); - last_time = baro_timestamp; - output_index = output_index + 1; - end -end - -%% convert IMU data to delta angles and velocities -% Note: these quatntis were converted from delta angles and velocites using -% the integral_dt values in the PX4 sensor module so we only need to -% multiply by integral_dt to convert back -clear imu_data; -n_samples = length(timestamp); -imu_data.time_us = timestamp + accelerometer_timestamp_relative; -imu_data.gyro_dt = gyro_integral_dt ./ 1e6; -imu_data.del_ang = [gyro_rad0.*imu_data.gyro_dt, gyro_rad1.*imu_data.gyro_dt, gyro_rad2.*imu_data.gyro_dt]; - -imu_data.accel_dt = accelerometer_integral_dt ./ 1e6; -imu_data.del_vel = [accelerometer_m_s20.*imu_data.accel_dt, accelerometer_m_s21.*imu_data.accel_dt, accelerometer_m_s22.*imu_data.accel_dt]; - -%% convert magnetometer data -clear mag_data; -last_time = 0; -output_index = 1; -for source_index = 1:length(timestamp_mag) - mag_timestamp = timestamp_mag(source_index); - if (mag_timestamp ~= last_time) - mag_data.time_us(output_index,1) = mag_timestamp; - mag_data.field_ga(output_index,:) = [magnetometer_ga0(source_index),magnetometer_ga1(source_index),magnetometer_ga2(source_index)]; - last_time = mag_timestamp; - output_index = output_index + 1; - end -end - -%% save data -% DO NOT clear the workspace (yet) - -save baro_data.mat baro_data; -save imu_data.mat imu_data; -save mag_data.mat mag_data; diff --git a/src/modules/ekf2/EKF/matlab/EKF_replay/Common/convert_px4_vehicle_gps_position_csv.m b/src/modules/ekf2/EKF/matlab/EKF_replay/Common/convert_px4_vehicle_gps_position_csv.m deleted file mode 100755 index 74a92782b1..0000000000 --- a/src/modules/ekf2/EKF/matlab/EKF_replay/Common/convert_px4_vehicle_gps_position_csv.m +++ /dev/null @@ -1,32 +0,0 @@ -%% convert GPS data -clear gps_data; -gps_data.time_us = timestamp + timestamp_time_relative; -gps_data.pos_error = eph; -gps_data.spd_error = s_variance_m_s; -gps_data.hgt_error = epv; - -% set reference point used to set NED origin when GPS accuracy is sufficient -gps_data.start_index = max(min(find(gps_data.pos_error < 5.0)),min(find(gps_data.spd_error < 1.0))); -if isempty(gps_data.start_index) - gps_data.start_index = 1; - gps_data.refLLH = [1e-7*median(lat);1e-7*median(lon);0.001*median(alt)]; -else - gps_data.refLLH = [1e-7*lat(gps_data.start_index);1e-7*lon(gps_data.start_index);0.001*alt(gps_data.start_index)]; -end - -% convert GPS data to NED -for index = 1:length(timestamp) - if (fix_type(index) >= 3) - gps_data.pos_ned(index,:) = LLH2NED([1e-7*lat(index);1e-7*lon(index);0.001*alt(index)],gps_data.refLLH); - gps_data.vel_ned(index,:) = [vel_n_m_s(index),vel_e_m_s(index),vel_d_m_s(index)]; - else - gps_data.pos_ned(index,:) = [0,0,0]; - gps_data.vel_ned(index,:) = [0,0,0]; - end -end - - -%% save data -% DO NOT clear the workspace (yet) - -save gps_data.mat gps_data; diff --git a/src/modules/ekf2/EKF/matlab/EKF_replay/Filter/AlignHeading.m b/src/modules/ekf2/EKF/matlab/EKF_replay/Filter/AlignHeading.m deleted file mode 100644 index 1e817e1fcc..0000000000 --- a/src/modules/ekf2/EKF/matlab/EKF_replay/Filter/AlignHeading.m +++ /dev/null @@ -1,23 +0,0 @@ -function quat = AlignHeading( ... - quat, ... % quaternion state vector - magMea, ... % body frame magnetic flux measurements - declination) % Estimated magnetic field delination at current location (rad) - -% Get the Euler angles and set yaw to zero -euler = QuatToEul(quat); -euler(3) = 0.0; - -% calculate the rotation matrix from body to earth frame -quat_zero_yaw = EulToQuat(euler); -Tbn_zero_yaw = Quat2Tbn(quat_zero_yaw); - -% Rotate the magnetic field measurement into earth frame -magMeasNED = Tbn_zero_yaw*magMea; - -% Use the projection onto the horizontal to calculate the yaw angle -euler(3) = declination - atan2(magMeasNED(2),magMeasNED(1)); - -% convert to a quaternion -quat = EulToQuat(euler); - -end diff --git a/src/modules/ekf2/EKF/matlab/EKF_replay/Filter/ConstrainStates.m b/src/modules/ekf2/EKF/matlab/EKF_replay/Filter/ConstrainStates.m deleted file mode 100644 index 6accf3ee61..0000000000 --- a/src/modules/ekf2/EKF/matlab/EKF_replay/Filter/ConstrainStates.m +++ /dev/null @@ -1,23 +0,0 @@ -function [states] = ConstrainStates(states,dt_imu_avg) - -% constrain gyro bias states -limit = 5.0*pi/180*dt_imu_avg; -for i=11:13 - if (states(i) > limit) - states(i) = limit; - elseif (states(i) < -limit) - states(i) = -limit; - end -end - -% constrain accel bias states -limit = 0.5*dt_imu_avg; -for i=14:16 - if (states(i) > limit) - states(i) = limit; - elseif (states(i) < -limit) - states(i) = -limit; - end -end - -end \ No newline at end of file diff --git a/src/modules/ekf2/EKF/matlab/EKF_replay/Filter/FuseBaroHeight.m b/src/modules/ekf2/EKF/matlab/EKF_replay/Filter/FuseBaroHeight.m deleted file mode 100644 index 1032f33fe7..0000000000 --- a/src/modules/ekf2/EKF/matlab/EKF_replay/Filter/FuseBaroHeight.m +++ /dev/null @@ -1,56 +0,0 @@ -function [... - states, ... % state vector after fusion of measurements - P, ... % state covariance matrix after fusion of corrections - innovation,... % NE position innovations (m) - varInnov] ... % NE position innovation variance (m^2) - = FuseBaroHeight( ... - states, ... % predicted states from the INS - P, ... % predicted covariance - measHgt, ... % NE position measurements (m) - gateSize, ... % Size of the innovation consistency check gate (std-dev) - R_OBS) % position observation variance (m)^2 - -H = zeros(1,24); - -% position states start at index 8 -stateIndex = 10; - -% Calculate the vertical position height innovation (posD is opposite -% sign to height) -innovation = states(stateIndex) + measHgt; - -% Calculate the observation Jacobian -H(stateIndex) = 1; - -varInnov = (H*P*transpose(H) + R_OBS); - -% Apply an innovation consistency check -if (innovation^2 / (gateSize^2 * varInnov)) > 1.0 - return; -end - -% Calculate Kalman gains and update states and covariances - -% Calculate the Kalman gains -K = (P*transpose(H))/varInnov; - -% Calculate state corrections -xk = K * innovation; - -% Apply the state corrections -states = states - xk; - -% Update the covariance -P = P - K*H*P; - -% Force symmetry on the covariance matrix to prevent ill-conditioning -P = 0.5*(P + transpose(P)); - -% ensure diagonals are positive -for i=1:24 - if P(i,i) < 0 - P(i,i) = 0; - end -end - -end \ No newline at end of file diff --git a/src/modules/ekf2/EKF/matlab/EKF_replay/Filter/FuseBodyVel.m b/src/modules/ekf2/EKF/matlab/EKF_replay/Filter/FuseBodyVel.m deleted file mode 100644 index 798b89a499..0000000000 --- a/src/modules/ekf2/EKF/matlab/EKF_replay/Filter/FuseBodyVel.m +++ /dev/null @@ -1,82 +0,0 @@ -function [... - states, ... % state vector after fusion of measurements - P, ... % state covariance matrix after fusion of corrections - innovation, ... % XY optical flow innovations - rad/sec - varInnov] ... % XY optical flow innovation variances (rad/sec)^2 - = FuseBodyVel( ... - states, ... % predicted states - P, ... % predicted covariance - relVelBodyMea, ... % XYZ velocity measured by the camera (m/sec) - obsVar, ... % velocity variances - (m/sec)^2 - gateSize) % innovation gate size (SD) - -q0 = states(1); -q1 = states(2); -q2 = states(3); -q3 = states(4); -vn = states(5); -ve = states(6); -vd = states(7); - -innovation = zeros(1,2); -varInnov = zeros(1,2); -H = zeros(2,24); - -% Calculate predicted velocity measured in body frame axes -Tbn = Quat2Tbn(states(1:4)); -relVelBodyPred = transpose(Tbn)*[vn;ve;vd]; - -% calculate the observation jacobian, innovation variance and innovation -for obsIndex = 1:3 - - % Calculate corrections using X component - if (obsIndex == 1) - H(1,:) = calcH_VELX(q0,q1,q2,q3,vd,ve,vn); - elseif (obsIndex == 2) - H(2,:) = calcH_VELY(q0,q1,q2,q3,vd,ve,vn); - elseif (obsIndex == 3) - H(3,:) = calcH_VELZ(q0,q1,q2,q3,vd,ve,vn); - end - varInnov(obsIndex) = (H(obsIndex,:)*P*transpose(H(obsIndex,:)) + obsVar); - innovation(obsIndex) = relVelBodyPred(obsIndex) - relVelBodyMea(obsIndex); -end - -% check innovations for consistency and exit if they fail the test -for obsIndex = 1:3 - if (innovation(obsIndex)^2 / (varInnov(obsIndex) * gateSize^2) > 1.0); - return; - end -end - -% calculate the kalman gains and perform the state and covariance update -% using sequential fusion -for obsIndex = 1:3 - - Kfusion = (P*transpose(H(obsIndex,:)))/varInnov(obsIndex); - - % correct the state vector - states = states - Kfusion * innovation(obsIndex); - - % normalise the updated quaternion states - quatMag = sqrt(states(1)^2 + states(2)^2 + states(3)^2 + states(4)^2); - if (quatMag > 1e-12) - states(1:4) = states(1:4) / quatMag; - end - - % correct the covariance P = P - K*H*P - P = P - Kfusion*H(obsIndex,:)*P; - - % Force symmetry on the covariance matrix to prevent ill-conditioning - % of the matrix which would cause the filter to blow-up - P = 0.5*(P + transpose(P)); - - % ensure diagonals are positive - for i=1:24 - if P(i,i) < 0 - P(i,i) = 0; - end - end - -end - -end diff --git a/src/modules/ekf2/EKF/matlab/EKF_replay/Filter/FuseMagDeclination.m b/src/modules/ekf2/EKF/matlab/EKF_replay/Filter/FuseMagDeclination.m deleted file mode 100644 index 7b28ac40eb..0000000000 --- a/src/modules/ekf2/EKF/matlab/EKF_replay/Filter/FuseMagDeclination.m +++ /dev/null @@ -1,47 +0,0 @@ -function [... - states, ... % state vector after fusion of measurements - P] ... % - = FuseMagDeclination( ... - states, ... % predicted states - P, ... % predicted covariance - measDec) % magnetic field declination - azimuth angle measured from true north (rad) - -magN = states(17); -magE = states(18); - -R_MAG = 0.5^2; - -H = calcH_MAGD(magE,magN); -varInnov = (H*P*transpose(H) + R_MAG); -Kfusion = (P*transpose(H))/varInnov; - -% Calculate the predicted magnetic declination -predDec = atan2(magE,magN); - -% Calculate the measurement innovation -innovation = predDec - measDec; - -if (innovation > pi) - innovation = innovation - 2*pi; -elseif (innovation < -pi) - innovation = innovation + 2*pi; -end - -% correct the state vector -states = states - Kfusion * innovation; - -% correct the covariance P = P - K*H*P -P = P - Kfusion*H*P; - -% Force symmetry on the covariance matrix to prevent ill-conditioning -% of the matrix which would cause the filter to blow-up -P = 0.5*(P + transpose(P)); - -% ensure diagonals are positive -for i=1:24 - if P(i,i) < 0 - P(i,i) = 0; - end -end - -end \ No newline at end of file diff --git a/src/modules/ekf2/EKF/matlab/EKF_replay/Filter/FuseMagHeading.m b/src/modules/ekf2/EKF/matlab/EKF_replay/Filter/FuseMagHeading.m deleted file mode 100644 index 36d27ecf58..0000000000 --- a/src/modules/ekf2/EKF/matlab/EKF_replay/Filter/FuseMagHeading.m +++ /dev/null @@ -1,71 +0,0 @@ -function [... - states, ... % state vector after fusion of measurements - P, ... % state covariance matrix after fusion of corrections - innovation, ... % Declination innovation - rad - varInnov] ... % - = FuseMagHeading( ... - states, ... % predicted states - P, ... % predicted covariance - magData, ... % XYZ body frame magnetic flux measurements - gauss - measDec, ... % magnetic field declination - azimuth angle measured from true north (rad) - innovGate, ... % innovation gate size (SD) - R_MAG) % magnetic heading measurement variance - rad^2 - -q0 = states(1); -q1 = states(2); -q2 = states(3); -q3 = states(4); - -magX = magData(1); -magY = magData(2); -magZ = magData(3); - -H = calcH_HDG(magX,magY,magZ,q0,q1,q2,q3); -varInnov = (H*P*transpose(H) + R_MAG); -Kfusion = (P*transpose(H))/varInnov; - -% Calculate the predicted magnetic declination -Tbn = Quat2Tbn(states(1:4)); -magMeasNED = Tbn*[magX;magY;magZ]; -predDec = atan2(magMeasNED(2),magMeasNED(1)); - -% Calculate the measurement innovation -innovation = predDec - measDec; - -if (innovation > pi) - innovation = innovation - 2*pi; -elseif (innovation < -pi) - innovation = innovation + 2*pi; -end - -% Apply a innovation consistency check -if (innovation^2 / (innovGate^2 * varInnov)) > 1.0 - innovation = NaN; - varInnov = NaN; - return; -end - -% correct the state vector -states = states - Kfusion * innovation; - -% normalise the updated quaternion states -quatMag = sqrt(states(1)^2 + states(2)^2 + states(3)^2 + states(4)^2); -if (quatMag > 1e-12) - states(1:4) = states(1:4) / quatMag; -end - -% correct the covariance P = P - K*H*P -P = P - Kfusion*H*P; - -% Force symmetry on the covariance matrix to prevent ill-conditioning -% of the matrix which would cause the filter to blow-up -P = 0.5*(P + transpose(P)); - -% ensure diagonals are positive -for i=1:24 - if P(i,i) < 0 - P(i,i) = 0; - end -end - -end \ No newline at end of file diff --git a/src/modules/ekf2/EKF/matlab/EKF_replay/Filter/FuseMagnetometer.m b/src/modules/ekf2/EKF/matlab/EKF_replay/Filter/FuseMagnetometer.m deleted file mode 100644 index 6e66bc5962..0000000000 --- a/src/modules/ekf2/EKF/matlab/EKF_replay/Filter/FuseMagnetometer.m +++ /dev/null @@ -1,87 +0,0 @@ -function [... - states, ... % state vector after fusion of measurements - P, ... % state covariance matrix after fusion of corrections - innovation, ... % Declination innovation - rad - varInnov] ... % - = FuseMagnetometer( ... - states, ... % predicted states - P, ... % predicted covariance - magMea, ... % body frame magnetic flux measurements - testRatio, ... % Size of magnetometer innovation in standard deviations before measurements are rejected - R_MAG) % magnetometer measurement variance - gauss^2 - -q0 = states(1); -q1 = states(2); -q2 = states(3); -q3 = states(4); - -magXbias = states(20); -magYbias = states(21); -magZbias = states(22); - -magN = states(17); -magE = states(18); -magD = states(19); - -innovation = zeros(1,3); -varInnov = zeros(1,3); -H = zeros(3,24); - -% Calculate the predicted magnetometer measurement -Tbn = Quat2Tbn(states(1:4)); -magPred = transpose(Tbn)*[magN;magE;magD] + [magXbias;magYbias;magZbias]; - -% calculate the observation jacobian, innovation variance and innovation -for obsIndex = 1:3 - - % Calculate corrections using X component - if (obsIndex == 1) - H(1,:) = calcH_MAGX(magD,magE,magN,q0,q1,q2,q3); - elseif (obsIndex == 2) - H(2,:) = calcH_MAGY(magD,magE,magN,q0,q1,q2,q3); - elseif (obsIndex == 3) - H(3,:) = calcH_MAGZ(magD,magE,magN,q0,q1,q2,q3); - end - varInnov(obsIndex) = (H(obsIndex,:)*P*transpose(H(obsIndex,:)) + R_MAG); - innovation(obsIndex) = magPred(obsIndex) - magMea(obsIndex); -end - -% check innovations for consistency and exit if they fail the test -for obsIndex = 1:3 - if (innovation(obsIndex)^2 / (varInnov(obsIndex) * testRatio^2) > 1.0); - return; - end -end - -% calculate the kalman gains and perform the state and covariance update -% using sequential fusion -for obsIndex = 1:3 - - Kfusion = (P*transpose(H(obsIndex,:)))/varInnov(obsIndex); - - % correct the state vector - states = states - Kfusion * innovation(obsIndex); - - % normalise the updated quaternion states - quatMag = sqrt(states(1)^2 + states(2)^2 + states(3)^2 + states(4)^2); - if (quatMag > 1e-12) - states(1:4) = states(1:4) / quatMag; - end - - % correct the covariance P = P - K*H*P - P = P - Kfusion*H(obsIndex,:)*P; - - % Force symmetry on the covariance matrix to prevent ill-conditioning - % of the matrix which would cause the filter to blow-up - P = 0.5*(P + transpose(P)); - - % ensure diagonals are positive - for i=1:24 - if P(i,i) < 0 - P(i,i) = 0; - end - end - -end - -end diff --git a/src/modules/ekf2/EKF/matlab/EKF_replay/Filter/FuseOpticalFlow.m b/src/modules/ekf2/EKF/matlab/EKF_replay/Filter/FuseOpticalFlow.m deleted file mode 100644 index 0c9e0e7f14..0000000000 --- a/src/modules/ekf2/EKF/matlab/EKF_replay/Filter/FuseOpticalFlow.m +++ /dev/null @@ -1,88 +0,0 @@ -function [... - states, ... % state vector after fusion of measurements - P, ... % state covariance matrix after fusion of corrections - innovation, ... % XY optical flow innovations - rad/sec - varInnov] ... % XY optical flow innovation variances (rad/sec)^2 - = FuseOpticalFlow( ... - states, ... % predicted states - P, ... % predicted covariance - flowRate, ... % XY axis optical flow rate (rad/sec) - bodyRate, ... % XY axis body rate (rad/sec) - range, ... % range from lens to ground measured along the centre of the optical flow sensor field of view - flowObsVar, ... % flow observation variance - (rad/sec)^2 - gateSize) % innovation gate size (SD) - -q0 = states(1); -q1 = states(2); -q2 = states(3); -q3 = states(4); -vn = states(5); -ve = states(6); -vd = states(7); - -innovation = zeros(1,2); -varInnov = zeros(1,2); -H = zeros(2,24); - -% Calculate predicted angular LOS rates about body frame axes -Tbn = Quat2Tbn(states(1:4)); -relVelBody = transpose(Tbn)*[vn;ve;vd]; -losRatePred(1) = +relVelBody(2)/range; -losRatePred(2) = -relVelBody(1)/range; - -% Calculate measured LOS angular rates using body motion corrected flow -% measurements -losRateMea = - flowRate + bodyRate; - -% calculate the observation jacobian, innovation variance and innovation -for obsIndex = 1:2 - - % Calculate corrections using X component - if (obsIndex == 1) - H(1,:) = calcH_LOSX(q0,q1,q2,q3,range,vd,ve,vn); - elseif (obsIndex == 2) - H(2,:) = calcH_LOSY(q0,q1,q2,q3,range,vd,ve,vn); - end - varInnov(obsIndex) = (H(obsIndex,:)*P*transpose(H(obsIndex,:)) + flowObsVar); - innovation(obsIndex) = losRatePred(obsIndex) - losRateMea(obsIndex); -end - -% check innovations for consistency and exit if they fail the test -for obsIndex = 1:2 - if (innovation(obsIndex)^2 / (varInnov(obsIndex) * gateSize^2) > 1.0); - return; - end -end - -% calculate the kalman gains and perform the state and covariance update -% using sequential fusion -for obsIndex = 1:2 - - Kfusion = (P*transpose(H(obsIndex,:)))/varInnov(obsIndex); - - % correct the state vector - states = states - Kfusion * innovation(obsIndex); - - % normalise the updated quaternion states - quatMag = sqrt(states(1)^2 + states(2)^2 + states(3)^2 + states(4)^2); - if (quatMag > 1e-12) - states(1:4) = states(1:4) / quatMag; - end - - % correct the covariance P = P - K*H*P - P = P - Kfusion*H(obsIndex,:)*P; - - % Force symmetry on the covariance matrix to prevent ill-conditioning - % of the matrix which would cause the filter to blow-up - P = 0.5*(P + transpose(P)); - - % ensure diagonals are positive - for i=1:24 - if P(i,i) < 0 - P(i,i) = 0; - end - end - -end - -end diff --git a/src/modules/ekf2/EKF/matlab/EKF_replay/Filter/FusePosition.m b/src/modules/ekf2/EKF/matlab/EKF_replay/Filter/FusePosition.m deleted file mode 100644 index c25f6dd770..0000000000 --- a/src/modules/ekf2/EKF/matlab/EKF_replay/Filter/FusePosition.m +++ /dev/null @@ -1,68 +0,0 @@ -function [... - states, ... % state vector after fusion of measurements - P, ... % state covariance matrix after fusion of corrections - innovation,... % NE position innovations (m) - varInnov] ... % NE position innovation variance (m^2) - = FusePosition( ... - states, ... % predicted states from the INS - P, ... % predicted covariance - measPos, ... % NE position measurements (m) - gateSize, ... % Size of the innovation consistency check gate (std-dev) - R_OBS) % position observation variance (m)^2 - -innovation = zeros(1,2); -varInnov = zeros(1,2); -H = zeros(2,24); - -for obsIndex = 1:2 - - % velocity states start at index 8 - stateIndex = 7 + obsIndex; - - % Calculate the velocity measurement innovation - innovation(obsIndex) = states(stateIndex) - measPos(obsIndex); - - % Calculate the observation Jacobian - H(obsIndex,stateIndex) = 1; - - varInnov(obsIndex) = (H(obsIndex,:)*P*transpose(H(obsIndex,:)) + R_OBS); - -end - -% Apply an innovation consistency check -for obsIndex = 1:2 - - if (innovation(obsIndex)^2 / (gateSize^2 * varInnov(obsIndex))) > 1.0 - return; - end - -end - -% Calculate Kalman gains and update states and covariances -for obsIndex = 1:2 - - % Calculate the Kalman gains - K = (P*transpose(H(obsIndex,:)))/varInnov(obsIndex); - - % Calculate state corrections - xk = K * innovation(obsIndex); - - % Apply the state corrections - states = states - xk; - - % Update the covariance - P = P - K*H(obsIndex,:)*P; - - % Force symmetry on the covariance matrix to prevent ill-conditioning - P = 0.5*(P + transpose(P)); - - % ensure diagonals are positive - for i=1:24 - if P(i,i) < 0 - P(i,i) = 0; - end - end - -end - -end diff --git a/src/modules/ekf2/EKF/matlab/EKF_replay/Filter/FuseVelocity.m b/src/modules/ekf2/EKF/matlab/EKF_replay/Filter/FuseVelocity.m deleted file mode 100644 index bc533beb44..0000000000 --- a/src/modules/ekf2/EKF/matlab/EKF_replay/Filter/FuseVelocity.m +++ /dev/null @@ -1,68 +0,0 @@ -function [... - states, ... % state vector after fusion of measurements - P, ... % state covariance matrix after fusion of corrections - innovation,... % NED velocity innovations (m/s) - varInnov] ... % NED velocity innovation variance ((m/s)^2) - = FuseVelocity( ... - states, ... % predicted states from the INS - P, ... % predicted covariance - measVel, ... % NED velocity measurements (m/s) - gateSize, ... % Size of the innovation consistency check gate (std-dev) - R_OBS) % velocity observation variance (m/s)^2 - -innovation = zeros(1,3); -varInnov = zeros(1,3); -H = zeros(3,24); - -for obsIndex = 1:3 - - % velocity states start at index 5 - stateIndex = 4 + obsIndex; - - % Calculate the velocity measurement innovation - innovation(obsIndex) = states(stateIndex) - measVel(obsIndex); - - % Calculate the observation Jacobian - H(obsIndex,stateIndex) = 1; - - varInnov(obsIndex) = (H(obsIndex,:)*P*transpose(H(obsIndex,:)) + R_OBS); - -end - -% Apply an innovation consistency check -for obsIndex = 1:3 - - if (innovation(obsIndex)^2 / (gateSize^2 * varInnov(obsIndex))) > 1.0 - return; - end - -end - -% Calculate Kalman gains and update states and covariances -for obsIndex = 1:3 - - % Calculate the Kalman gains - K = (P*transpose(H(obsIndex,:)))/varInnov(obsIndex); - - % Calculate state corrections - xk = K * innovation(obsIndex); - - % Apply the state corrections - states = states - xk; - - % Update the covariance - P = P - K*H(obsIndex,:)*P; - - % Force symmetry on the covariance matrix to prevent ill-conditioning - P = 0.5*(P + transpose(P)); - - % ensure diagonals are positive - for i=1:24 - if P(i,i) < 0 - P(i,i) = 0; - end - end - -end - -end diff --git a/src/modules/ekf2/EKF/matlab/EKF_replay/Filter/GenerateEquations24.m b/src/modules/ekf2/EKF/matlab/EKF_replay/Filter/GenerateEquations24.m deleted file mode 100755 index 90f9259893..0000000000 --- a/src/modules/ekf2/EKF/matlab/EKF_replay/Filter/GenerateEquations24.m +++ /dev/null @@ -1,211 +0,0 @@ -%% 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_MAG real % variance for magnetic flux measurements - milligauss^2 - -%% 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 -stateVectorNew = [quatNew;vNew;pNew;dAngBiasNew;dVelBiasNew;magNnew;magEnew;magDnew;magXnew;magYnew;magZnew;vwnNew;vweNew]; - -%% derive the state transition and state error matrix - -% Define the control (disturbance) vector. Error growth in the inertial -% solution is assumed to be driven by 'noise' in the delta angles and -% velocities, after bias effects have been removed. This is OK because we -% have sensor bias accounted for in the state equations. -distVector = [daxVar;dayVar;dazVar;dvxVar;dvyVar;dvzVar]; - -% derive the control(disturbance) influence matrix -G = jacobian(stateVectorNew, [dAngMeas;dVelMeas]); - -% derive the state error matrix -distMatrix = diag(distVector); -Q = G*distMatrix*transpose(G); -f = matlabFunction(Q,'file','calcQ24.m'); - -% derive the state transition matrix -F = jacobian(stateVectorNew, stateVector); -f = matlabFunction(F,'file','calcF24.m'); - -%% derive equations for fusion of magnetometer measurements -% rotate earth field into body axes -magMeas = transpose(Tbn)*[magN;magE;magD] + [magX;magY;magZ]; - -magMeasX = magMeas(1); -H_MAGX = jacobian(magMeasX,stateVector); % measurement Jacobian -f = matlabFunction(H_MAGX,'file','calcH_MAGX.m'); - -magMeasY = magMeas(2); -H_MAGY = jacobian(magMeasY,stateVector); % measurement Jacobian -f = matlabFunction(H_MAGY,'file','calcH_MAGY.m'); - -magMeasZ = magMeas(3); -H_MAGZ = jacobian(magMeasZ,stateVector); % measurement Jacobian -f = matlabFunction(H_MAGZ,'file','calcH_MAGZ.m'); - -%% derive equations for fusion of synthetic deviation measurement -% used to keep correct heading when operating without absolute position or -% velocity measurements - eg when using optical flow - -% rotate magnetic field into earth axes -magMeasNED = [magN;magE;magD]; - -% the predicted measurement is the angle wrt magnetic north of the horizontal -% component of the measured field -angMeas = atan(magMeasNED(2)/magMeasNED(1)); -H_MAGD = jacobian(angMeas,stateVector); % measurement Jacobian -H_MAGD = simplify(H_MAGD); - -f = matlabFunction(H_MAGD,'file','calcH_MAGD.m'); - -%% derive equations for fusion of a single magneic compass heading measurement - -% rotate body measured field into earth axes -magMeasNED = Tbn*[magX;magY;magZ]; - -% the predicted measurement is the angle wrt true north of the horizontal -% component of the measured field -angMeas = atan(magMeasNED(2)/magMeasNED(1)); -H_MAG = jacobian(angMeas,stateVector); % measurement Jacobian -f = matlabFunction(H_MAG,'file','calcH_HDG.m'); - -%% derive equations for sequential fusion of optical flow measurements - -% range is defined as distance from camera focal point to centre of sensor fov -syms range real; - -% calculate relative velocity in body frame -relVelBody = transpose(Tbn)*[vn;ve;vd]; - -% divide by range to get predicted angular LOS rates relative to X and Y -% axes. Note these are body angular rate motion compensated optical flow rates -losRateX = +relVelBody(2)/range; -losRateY = -relVelBody(1)/range; - -% calculate the observation Jacobian for the X axis -H_LOSX = jacobian(losRateX,stateVector); % measurement Jacobian -H_LOSX = simplify(H_LOSX); -f = matlabFunction(H_LOSX,'file','calcH_LOSX.m'); - -% calculate the observation Jacobian for the Y axis -H_LOSY = jacobian(losRateY,stateVector); % measurement Jacobian -H_LOSY = simplify(H_LOSY); -f = matlabFunction(H_LOSY,'file','calcH_LOSY.m'); - -%% derive equations for sequential fusion of body frame velocity measurements - -% 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]; - -% calculate the observation Jacobian for the X axis -H_VELX = jacobian(relVelBody(1),stateVector); % measurement Jacobian -H_VELX = simplify(H_VELX); -f = matlabFunction(H_VELX,'file','calcH_VELX.m'); - -% calculate the observation Jacobian for the Y axis -H_VELY = jacobian(relVelBody(2),stateVector); % measurement Jacobian -H_VELY = simplify(H_VELY); -f = matlabFunction(H_VELY,'file','calcH_VELY.m'); - -% calculate the observation Jacobian for the Z axis -H_VELZ = jacobian(relVelBody(3),stateVector); % measurement Jacobian -H_VELZ = simplify(H_VELZ); -f = matlabFunction(H_VELZ,'file','calcH_VELZ.m'); - -%% calculate error transfer matrix for declination error estimate -declination = atan(magE/magN); -T_MAG = jacobian(declination,[magN,magE]); - -f = matlabFunction(T_MAG,'file','transfer_matrix.m'); - -%% calculate Quaternion to Euler angle error transfer matrix -% quat = [q0;q1;q2;q3]; -% syms roll pitch yaw 'real'; -roll = 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))); -pitch = -asin(2*(quat(2)*quat(4)-quat(1)*quat(3))); -yaw = 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))); -euler = [roll;pitch;yaw]; -error_transfer_matrix = jacobian(euler,quat); -matlabFunction(error_transfer_matrix,'file','quat_to_euler_error_transfer_matrix.m'); diff --git a/src/modules/ekf2/EKF/matlab/EKF_replay/Filter/InitCovariance.m b/src/modules/ekf2/EKF/matlab/EKF_replay/Filter/InitCovariance.m deleted file mode 100644 index dccefb852c..0000000000 --- a/src/modules/ekf2/EKF/matlab/EKF_replay/Filter/InitCovariance.m +++ /dev/null @@ -1,35 +0,0 @@ -function covariance = InitCovariance(param,dt,gps_alignment,gps_data) - -% Define quaternion state errors -Sigma_quat = param.alignment.quatErr * [1;1;1;1]; - -% Define velocity state errors -if (gps_alignment == 1) - Sigma_velocity = gps_data.spd_error(gps_data.start_index) * [1;1;1]; -else - Sigma_velocity = [param.alignment.velErrNE;param.alignment.velErrNE;param.alignment.velErrD]; -end - -% Define position state errors -if (gps_alignment == 1) - Sigma_position = gps_data.pos_error(gps_data.start_index) * [1;1;0] + [0;0;param.alignment.hgtErr]; -else - Sigma_position = [param.alignment.posErrNE;param.alignment.posErrNE;param.alignment.hgtErr]; -end - -% Define delta angle bias state errors -Sigma_dAngBias = param.alignment.delAngBiasErr*dt*[1;1;1]; - -% Define delta velocity bias state errors -Sigma_dVelBias = param.alignment.delVelBiasErr*dt*[1;1;1]; - -% Define magnetic field state errors -Sigma_magNED = [param.alignment.magErrNED;param.alignment.magErrNED;param.alignment.magErrNED]; % 1 Sigma uncertainty in initial NED mag field -Sigma_magXYZ = [param.alignment.magErrXYZ;param.alignment.magErrXYZ;param.alignment.magErrXYZ]; % 1 Sigma uncertainty in initial XYZ mag sensor offset - -% Define wind velocity state errors -Sigma_wind = param.alignment.windErrNE * [1;1]; - -% Convert to variances and write to covariance matrix diagonals -covariance = diag([Sigma_quat;Sigma_velocity;Sigma_position;Sigma_dAngBias;Sigma_dVelBias;Sigma_magNED;Sigma_magXYZ;Sigma_wind].^2); -end diff --git a/src/modules/ekf2/EKF/matlab/EKF_replay/Filter/InitStates.m b/src/modules/ekf2/EKF/matlab/EKF_replay/Filter/InitStates.m deleted file mode 100644 index 361bec1063..0000000000 --- a/src/modules/ekf2/EKF/matlab/EKF_replay/Filter/InitStates.m +++ /dev/null @@ -1,66 +0,0 @@ -function [states, imu_start_index] = InitStates(param,imu_data,gps_data,mag_data,baro_data) - -% constants -deg2rad = pi/180; - -% initialise the state vector and quaternion -states = zeros(24,1); -quat = [1;0;0;0]; - -if (param.control.waitForGps == 1) - % find IMU start index that coresponds to first valid GPS data - imu_start_index = (find(imu_data.time_us > gps_data.time_us(gps_data.start_index), 1, 'first' ) - 50); - imu_start_index = max(imu_start_index,1); -else - imu_start_index = 1; -end - -% average first 100 accel readings to reduce effect of vibration -initAccel(1) = mean(imu_data.del_vel(imu_start_index:imu_start_index+99,1))./mean(imu_data.accel_dt(imu_start_index:imu_start_index+99,1)); -initAccel(2) = mean(imu_data.del_vel(imu_start_index:imu_start_index+99,2))./mean(imu_data.accel_dt(imu_start_index:imu_start_index+99,1)); -initAccel(3) = mean(imu_data.del_vel(imu_start_index:imu_start_index+99,3))./mean(imu_data.accel_dt(imu_start_index:imu_start_index+99,1)); - -% align tilt using gravity vector (If the velocity is changing this will -% induce errors) -quat = AlignTilt(quat,initAccel); -states(1:4) = quat; - -% add a roll, pitch, yaw mislignment -quat_align_err = EulToQuat([param.control.rollAlignErr,param.control.pitchAlignErr,param.control.yawAlignErr]); -quat = QuatMult(quat,quat_align_err); - -% find magnetometer start index -mag_start_index = (find(mag_data.time_us > imu_data.time_us(imu_start_index), 1, 'first' ) - 5); -mag_start_index = max(mag_start_index,1); - -% mean to reduce effect of noise in data -magBody(1,1) = mean(mag_data.field_ga(mag_start_index:mag_start_index+9,1)); -magBody(2,1) = mean(mag_data.field_ga(mag_start_index:mag_start_index+9,2)); -magBody(3,1) = mean(mag_data.field_ga(mag_start_index:mag_start_index+9,3)); - -% align heading and initialise the NED magnetic field states -quat = AlignHeading(quat,magBody,param.fusion.magDeclDeg*deg2rad); -states(1:4) = quat; - -% initialise the NED magnetic field states -Tbn = Quat2Tbn(quat); -states(17:19) = Tbn*magBody; - -if (param.control.waitForGps == 1) - % initialise velocity and position using gps - states(5:7) = gps_data.vel_ned(gps_data.start_index,:); - states(8:9) = gps_data.pos_ned(gps_data.start_index,1:2); -else - % initialise to be stationary at the origin - states(5:7) = zeros(1,3); - states(8:9) = zeros(1,2); -end - -% find baro start index -baro_start_index = (find(baro_data.time_us > imu_data.time_us(imu_start_index), 1, 'first' ) - 10); -baro_start_index = max(baro_start_index,1); - -% average baro data and initialise the vertical position -states(10) = -mean(baro_data.height(baro_start_index:baro_start_index+20)); - -end \ No newline at end of file diff --git a/src/modules/ekf2/EKF/matlab/EKF_replay/Filter/PlotData.m b/src/modules/ekf2/EKF/matlab/EKF_replay/Filter/PlotData.m deleted file mode 100755 index 8bc9bc357b..0000000000 --- a/src/modules/ekf2/EKF/matlab/EKF_replay/Filter/PlotData.m +++ /dev/null @@ -1,512 +0,0 @@ -function PlotData(output,folder,runIdentifier) -rad2deg = 180/pi; -if ~exist(folder,'dir') - mkdir(folder); -end -plotDimensions = [0 0 210*3 297*3]; - -%% plot Euler angle estimates -figure('Units','Pixels','Position',plotDimensions,'PaperOrientation','portrait'); -h=gcf; -set(h,'PaperOrientation','portrait'); -set(h,'PaperUnits','normalized'); -set(h,'PaperPosition', [0 0 1 1]); - -margin = 5; - -subplot(3,1,1); -plot(output.time_lapsed,[output.euler_angles(:,1)*rad2deg,output.euler_angles(:,1)*rad2deg-2*sqrt(output.euler_variances(:,1)*rad2deg),output.euler_angles(:,1)*rad2deg+2*sqrt(output.euler_variances(:,1)*rad2deg)]); -minVal = rad2deg*min(output.euler_angles(:,1))-margin; -maxVal = rad2deg*max(output.euler_angles(:,1))+margin; -ylim([minVal maxVal]); -grid on; -titleText=strcat({'Euler Angle Estimates'},runIdentifier); -title(titleText); -ylabel('Roll (deg)'); -xlabel('time (sec)'); -legend('estimate','upper 95% bound','lower 95% bound'); - -subplot(3,1,2); -plot(output.time_lapsed,[output.euler_angles(:,2)*rad2deg,output.euler_angles(:,2)*rad2deg-2*sqrt(output.euler_variances(:,2)*rad2deg),output.euler_angles(:,2)*rad2deg+2*sqrt(output.euler_variances(:,2)*rad2deg)]); -minVal = rad2deg*min(output.euler_angles(:,2))-margin; -maxVal = rad2deg*max(output.euler_angles(:,2))+margin; -ylim([minVal maxVal]); -grid on; -ylabel('Pitch (deg)'); -xlabel('time (sec)'); -legend('estimate','upper 95% bound','lower 95% bound'); - -subplot(3,1,3); -plot(output.time_lapsed,[output.euler_angles(:,3)*rad2deg,output.euler_angles(:,3)*rad2deg-2*sqrt(output.euler_variances(:,3)*rad2deg),output.euler_angles(:,3)*rad2deg+2*sqrt(output.euler_variances(:,3)*rad2deg)]); -minVal = rad2deg*min(output.euler_angles(:,3))-margin; -maxVal = rad2deg*max(output.euler_angles(:,3))+margin; -ylim([minVal maxVal]); -grid on; -ylabel('Yaw (deg)'); -xlabel('time (sec)'); -legend('estimate','upper 95% bound','lower 95% bound'); - -fileName='euler_angle_estimates.png'; -fullFileName = fullfile(folder, fileName); -saveas(h,fullFileName); - -%% plot NED velocity estimates -figure('Units','Pixels','Position',plotDimensions,'PaperOrientation','portrait'); -h=gcf; -set(h,'PaperOrientation','portrait'); -set(h,'PaperUnits','normalized'); -set(h,'PaperPosition', [0 0 1 1]); - -subplot(3,1,1); -plot(output.time_lapsed,[output.velocity_NED(:,1),output.velocity_NED(:,1)+2*sqrt(output.state_variances(:,5)),output.velocity_NED(:,1)-2*sqrt(output.state_variances(:,5))]); -grid on; -titleText=strcat({'NED Velocity Estimates'},runIdentifier); -title(titleText); -ylabel('North (m/s)'); -xlabel('time (sec)'); -legend('estimate','upper 95% bound','lower 95% bound'); - -subplot(3,1,2); -plot(output.time_lapsed,[output.velocity_NED(:,2),output.velocity_NED(:,2)+2*sqrt(output.state_variances(:,6)),output.velocity_NED(:,2)-2*sqrt(output.state_variances(:,6))]); -grid on; -ylabel('East (m/s)'); -xlabel('time (sec)'); -legend('estimate','upper 95% bound','lower 95% bound'); - -subplot(3,1,3); -plot(output.time_lapsed,[output.velocity_NED(:,3),output.velocity_NED(:,3)+2*sqrt(output.state_variances(:,7)),output.velocity_NED(:,3)-2*sqrt(output.state_variances(:,7))]); -grid on; -ylabel('Down (m/s)'); -xlabel('time (sec)'); -legend('estimate','upper 95% bound','lower 95% bound'); - -fileName='velocity_estimates.png'; -fullFileName = fullfile(folder, fileName); -saveas(h,fullFileName); - -%% plot NED position estimates -figure('Units','Pixels','Position',plotDimensions,'PaperOrientation','portrait'); -h=gcf; -set(h,'PaperOrientation','portrait'); -set(h,'PaperUnits','normalized'); -set(h,'PaperPosition', [0 0 1 1]); - -subplot(3,1,1); -plot(output.time_lapsed,[output.position_NED(:,1),output.position_NED(:,1)+2*sqrt(output.state_variances(:,8)),output.position_NED(:,1)-2*sqrt(output.state_variances(:,8))]); -grid on; -titleText=strcat({'NED Position Estimates'},runIdentifier); -title(titleText); -ylabel('North (m)'); -xlabel('time (sec)'); -legend('estimate','upper 95% bound','lower 95% bound'); - -subplot(3,1,2); -plot(output.time_lapsed,[output.position_NED(:,2),output.position_NED(:,2)+2*sqrt(output.state_variances(:,9)),output.position_NED(:,2)-2*sqrt(output.state_variances(:,9))]); -grid on; -ylabel('East (m)'); -xlabel('time (sec)'); -legend('estimate','upper 95% bound','lower 95% bound'); - -subplot(3,1,3); -plot(output.time_lapsed,[output.position_NED(:,3),output.position_NED(:,3)+2*sqrt(output.state_variances(:,10)),output.position_NED(:,3)-2*sqrt(output.state_variances(:,10))]); -grid on; -ylabel('Down (m)'); -xlabel('time (sec)'); -legend('estimate','upper 95% bound','lower 95% bound'); - -fileName='position_estimates.png'; -fullFileName = fullfile(folder, fileName); -saveas(h,fullFileName); - -%% plot IMU gyro bias estimates -figure('Units','Pixels','Position',plotDimensions,'PaperOrientation','portrait'); -h=gcf; -set(h,'PaperOrientation','portrait'); -set(h,'PaperUnits','normalized'); -set(h,'PaperPosition', [0 0 1 1]); - -margin = 0.1; - -subplot(3,1,1); -plot(output.time_lapsed,(1/output.dt)*[output.gyro_bias(:,1),output.gyro_bias(:,1)+2*sqrt(output.state_variances(:,11)),output.gyro_bias(:,1)-2*sqrt(output.state_variances(:,11))]*rad2deg);%%output.gyro_bias(:,1)*rad2deg); -minVal = (1/output.dt)*rad2deg*min(output.gyro_bias(:,1))-margin; -maxVal = (1/output.dt)*rad2deg*max(output.gyro_bias(:,1))+margin; -ylim([minVal maxVal]); -grid on; -titleText=strcat({'IMU Gyro Bias Estimates'},runIdentifier); -title(titleText); -ylabel('X gyro (deg/s)'); -xlabel('time (sec)'); -legend('estimate','upper 95% bound','lower 95% bound'); - -subplot(3,1,2); -plot(output.time_lapsed,(1/output.dt)*[output.gyro_bias(:,2),output.gyro_bias(:,2)+2*sqrt(output.state_variances(:,12)),output.gyro_bias(:,2)-2*sqrt(output.state_variances(:,12))]*rad2deg); -minVal = (1/output.dt)*rad2deg*min(output.gyro_bias(:,2))-margin; -maxVal = (1/output.dt)*rad2deg*max(output.gyro_bias(:,2))+margin; -ylim([minVal maxVal]); -grid on; -ylabel('Y gyro (deg/s)'); -xlabel('time (sec)'); -legend('estimate','upper 95% bound','lower 95% bound'); - -subplot(3,1,3); -plot(output.time_lapsed,(1/output.dt)*[output.gyro_bias(:,3),output.gyro_bias(:,3)+2*sqrt(output.state_variances(:,13)),output.gyro_bias(:,3)-2*sqrt(output.state_variances(:,13))]*rad2deg); -minVal = (1/output.dt)*rad2deg*min(output.gyro_bias(:,3))-margin; -maxVal = (1/output.dt)*rad2deg*max(output.gyro_bias(:,3))+margin; -ylim([minVal maxVal]); -grid on; -ylabel('Z gyro (deg/s)'); -xlabel('time (sec)'); -legend('estimate','upper 95% bound','lower 95% bound'); - -fileName='imu_gyro_bias_estimates.png'; -fullFileName = fullfile(folder, fileName); -saveas(h,fullFileName); - -%% plot IMU accel bias estimates -figure('Units','Pixels','Position',plotDimensions,'PaperOrientation','portrait'); -h=gcf; -set(h,'PaperOrientation','portrait'); -set(h,'PaperUnits','normalized'); -set(h,'PaperPosition', [0 0 1 1]); - -margin = 0.1; - -subplot(3,1,1); -plot(output.time_lapsed,(1/output.dt)*[output.accel_bias(:,1),output.accel_bias(:,1)+2*sqrt(output.state_variances(:,14)),output.accel_bias(:,1)-2*sqrt(output.state_variances(:,14))]); -titleText=strcat({'IMU Accel Bias Estimates'},runIdentifier); -title(titleText); -minVal = (1/output.dt)*min(output.accel_bias(:,1))-margin; -maxVal = (1/output.dt)*max(output.accel_bias(:,1))+margin; -ylim([minVal maxVal]); -grid on; -ylabel('X accel (m/s/s)'); -xlabel('time (sec)'); -legend('estimate','upper 95% bound','lower 95% bound'); - -subplot(3,1,2); -plot(output.time_lapsed,(1/output.dt)*[output.accel_bias(:,2),output.accel_bias(:,2)+2*sqrt(output.state_variances(:,15)),output.accel_bias(:,2)-2*sqrt(output.state_variances(:,15))]); -minVal = (1/output.dt)*min(output.accel_bias(:,1))-margin; -maxVal = (1/output.dt)*max(output.accel_bias(:,1))+margin; -ylim([minVal maxVal]); -grid on; -ylabel('Y accel (m/s/s)'); -xlabel('time (sec)'); -legend('estimate','upper 95% bound','lower 95% bound'); - -subplot(3,1,3); -plot(output.time_lapsed,(1/output.dt)*[output.accel_bias(:,3),output.accel_bias(:,3)+2*sqrt(output.state_variances(:,16)),output.accel_bias(:,3)-2*sqrt(output.state_variances(:,16))]); -minVal = (1/output.dt)*min(output.accel_bias(:,1))-margin; -maxVal = (1/output.dt)*max(output.accel_bias(:,1))+margin; -ylim([minVal maxVal]); -grid on; -ylabel('Z accel (m/s/s)'); -xlabel('time (sec)'); -legend('estimate','upper 95% bound','lower 95% bound'); - -fileName='imu_accel_bias_estimates.png'; -fullFileName = fullfile(folder, fileName); -saveas(h,fullFileName); - -%% plot magnetometer bias estimates -if (output.magFuseMethod <= 1) - figure('Units','Pixels','Position',plotDimensions,'PaperOrientation','portrait'); - h=gcf; - set(h,'PaperOrientation','portrait'); - set(h,'PaperUnits','normalized'); - set(h,'PaperPosition', [0 0 1 1]); - - subplot(3,1,1); - plot(output.time_lapsed',[output.mag_XYZ(:,1),output.mag_XYZ(:,1)+2*sqrt(output.state_variances(:,20)),output.mag_XYZ(:,1)-2*sqrt(output.state_variances(:,20))]); - grid on; - titleText=strcat({'Magnetometer Bias Estimates'},runIdentifier); - title(titleText); - ylabel('X bias (gauss)'); - xlabel('time (sec)'); - legend('estimate','upper 95% bound','lower 95% bound'); - - subplot(3,1,2); - plot(output.time_lapsed',[output.mag_XYZ(:,2),output.mag_XYZ(:,2)+2*sqrt(output.state_variances(:,21)),output.mag_XYZ(:,2)-2*sqrt(output.state_variances(:,21))]); - grid on; - ylabel('Y bias (gauss)'); - xlabel('time (sec)'); - legend('estimate','upper 95% bound','lower 95% bound'); - - subplot(3,1,3); - plot(output.time_lapsed',[output.mag_XYZ(:,3),output.mag_XYZ(:,3)+2*sqrt(output.state_variances(:,22)),output.mag_XYZ(:,3)-2*sqrt(output.state_variances(:,22))]); - grid on; - ylabel('Z bias (gauss)'); - xlabel('time (sec)'); - legend('estimate','upper 95% bound','lower 95% bound'); - - fileName='body_field_estimates.png'; - fullFileName = fullfile(folder, fileName); - saveas(h,fullFileName); -end - -%% plot earth field estimates -if (output.magFuseMethod <= 1) - figure('Units','Pixels','Position',plotDimensions,'PaperOrientation','portrait'); - h=gcf; - set(h,'PaperOrientation','portrait'); - set(h,'PaperUnits','normalized'); - set(h,'PaperPosition', [0 0 1 1]); - - margin = 0.1; - - subplot(4,1,1); - plot(output.time_lapsed',[output.mag_NED(:,1),output.mag_NED(:,1)+2*sqrt(output.state_variances(:,17)),output.mag_NED(:,1)-2*sqrt(output.state_variances(:,17))]); - minVal = min(output.mag_NED(:,1))-margin; - maxVal = max(output.mag_NED(:,1))+margin; - ylim([minVal maxVal]); - grid on; - titleText=strcat({'Earth Magnetic Field Estimates'},runIdentifier); - title(titleText); - ylabel('North (gauss)'); - xlabel('time (sec)'); - legend('estimate','upper 95% bound','lower 95% bound'); - - subplot(4,1,2); - plot(output.time_lapsed',[output.mag_NED(:,2),output.mag_NED(:,2)+2*sqrt(output.state_variances(:,18)),output.mag_NED(:,2)-2*sqrt(output.state_variances(:,18))]); - minVal = min(output.mag_NED(:,2))-margin; - maxVal = max(output.mag_NED(:,2))+margin; - ylim([minVal maxVal]); - grid on; - ylabel('East (gauss)'); - xlabel('time (sec)'); - legend('estimate','upper 95% bound','lower 95% bound'); - - subplot(4,1,3); - plot(output.time_lapsed',[output.mag_NED(:,3),output.mag_NED(:,3)+2*sqrt(output.state_variances(:,19)),output.mag_NED(:,3)-2*sqrt(output.state_variances(:,19))]); - grid on; - minVal = min(output.mag_NED(:,3))-margin; - maxVal = max(output.mag_NED(:,3))+margin; - ylim([minVal maxVal]); - ylabel('Down (gauss)'); - xlabel('time (sec)'); - legend('estimate','upper 95% bound','lower 95% bound'); - - subplot(4,1,4); - plot(output.time_lapsed',rad2deg*atan2(output.mag_NED(:,2),output.mag_NED(:,1))); - grid on; - titleText=strcat({'Magnetic Declination Estimate'},runIdentifier); - title(titleText); - ylabel('declination (deg)'); - xlabel('time (sec)'); - - fileName='earth_field_estimates.png'; - fullFileName = fullfile(folder, fileName); - saveas(h,fullFileName); -end - -%% plot velocity innovations -if isfield(output.innovations,'vel_innov') - - figure('Units','Pixels','Position',plotDimensions,'PaperOrientation','portrait'); - h=gcf; - set(h,'PaperOrientation','portrait'); - set(h,'PaperUnits','normalized'); - set(h,'PaperPosition', [0 0 1 1]); - - subplot(3,1,1); - plot(output.innovations.vel_time_lapsed',[output.innovations.vel_innov(:,1),sqrt(output.innovations.vel_innov_var(:,1)),-sqrt(output.innovations.vel_innov_var(:,1))]); - grid on; - titleText=strcat({'Velocity Innovations and Variances'},runIdentifier); - title(titleText); - ylabel('North (m/s)'); - xlabel('time (sec)'); - legend('innovation','variance sqrt','variance sqrt'); - - subplot(3,1,2); - plot(output.innovations.vel_time_lapsed',[output.innovations.vel_innov(:,2),sqrt(output.innovations.vel_innov_var(:,2)),-sqrt(output.innovations.vel_innov_var(:,2))]); - grid on; - ylabel('East (m/s)'); - xlabel('time (sec)'); - legend('innovation','variance sqrt','variance sqrt'); - - subplot(3,1,3); - plot(output.innovations.vel_time_lapsed',[output.innovations.vel_innov(:,3),sqrt(output.innovations.vel_innov_var(:,3)),-sqrt(output.innovations.vel_innov_var(:,3))]); - grid on; - ylabel('Down (m/s)'); - xlabel('time (sec)'); - legend('innovation','variance sqrt','variance sqrt'); - - fileName='velocity_fusion.png'; - fullFileName = fullfile(folder, fileName); - saveas(h,fullFileName); -end - -%% plot position innovations -if isfield(output.innovations,'posInnov') - figure('Units','Pixels','Position',plotDimensions,'PaperOrientation','portrait'); - h=gcf; - set(h,'PaperOrientation','portrait'); - set(h,'PaperUnits','normalized'); - set(h,'PaperPosition', [0 0 1 1]); - - subplot(3,1,1); - plot(output.innovations.vel_time_lapsed',[output.innovations.posInnov(:,1),sqrt(output.innovations.posInnovVar(:,1)),-sqrt(output.innovations.posInnovVar(:,1))]); - grid on; - titleText=strcat({'Position Innovations and Variances'},runIdentifier); - title(titleText); - ylabel('North (m)'); - xlabel('time (sec)'); - legend('innovation','variance sqrt','variance sqrt'); - - subplot(3,1,2); - plot(output.innovations.vel_time_lapsed',[output.innovations.posInnov(:,2),sqrt(output.innovations.posInnovVar(:,2)),-sqrt(output.innovations.posInnovVar(:,2))]); - grid on; - ylabel('East (m)'); - xlabel('time (sec)'); - legend('innovation','variance sqrt','variance sqrt'); - - subplot(3,1,3); - plot(output.innovations.hgt_time_lapsed',[output.innovations.hgtInnov(:),sqrt(output.innovations.hgtInnovVar(:)),-sqrt(output.innovations.hgtInnovVar(:))]); - grid on; - ylabel('Up (m)'); - xlabel('time (sec)'); - legend('innovation','variance sqrt','variance sqrt'); - - fileName='position_fusion.png'; - fullFileName = fullfile(folder, fileName); - saveas(h,fullFileName); -end - -%% plot magnetometer innovations -if isfield(output.innovations,'magInnov') - - figure('Units','Pixels','Position',plotDimensions,'PaperOrientation','portrait'); - h=gcf; - set(h,'PaperOrientation','portrait'); - set(h,'PaperUnits','normalized'); - set(h,'PaperPosition', [0 0 1 1]); - subplot(4,1,1); - plot(output.innovations.mag_time_lapsed,[output.innovations.magInnov(:,1)';sqrt(output.innovations.magInnovVar(:,1))';-sqrt(output.innovations.magInnovVar(:,1))']); - ylim([-0.15 0.15]); - grid on; - title(strcat({'Magnetometer Innovations and Variances'},runIdentifier)); - ylabel('X (gauss)'); - xlabel('time (sec)'); - legend('innovation','innovation variance sqrt','innovation variance sqrt'); - subplot(4,1,2); - plot(output.innovations.mag_time_lapsed,[output.innovations.magInnov(:,2)';sqrt(output.innovations.magInnovVar(:,2))';-sqrt(output.innovations.magInnovVar(:,2))']); - ylim([-0.15 0.15]); - grid on; - ylabel('Y (gauss)'); - xlabel('time (sec)'); - legend('innovation','innovation variance sqrt','innovation variance sqrt'); - subplot(4,1,3); - plot(output.innovations.mag_time_lapsed,[output.innovations.magInnov(:,3)';sqrt(output.innovations.magInnovVar(:,3))';-sqrt(output.innovations.magInnovVar(:,3))']); - ylim([-0.15 0.15]); - grid on; - ylabel('Z (gauss)'); - xlabel('time (sec)'); - legend('innovation','innovation variance sqrt','innovation variance sqrt'); - subplot(4,1,4); - plot(output.innovations.mag_time_lapsed,output.innovations.magLength); - ylim([0 0.7]); - grid on; - title(strcat({'Magnetic Flux'},runIdentifier)); - ylabel('Flux (Gauss)'); - xlabel('time (sec)'); - fileName='magnetometer_fusion.png'; - fullFileName = fullfile(folder, fileName); - saveas(h,fullFileName); - -end - -%% plot magnetic yaw innovations -if isfield(output.innovations,'hdgInnov') - - figure('Units','Pixels','Position',plotDimensions,'PaperOrientation','portrait'); - h=gcf; - set(h,'PaperOrientation','portrait'); - set(h,'PaperUnits','normalized'); - set(h,'PaperPosition', [0 0 1 1]); - - subplot(2,1,1); - plot(output.innovations.mag_time_lapsed,[output.innovations.hdgInnov*rad2deg;sqrt(output.innovations.hdgInnovVar)*rad2deg;-sqrt(output.innovations.hdgInnovVar)*rad2deg]); - ylim([-30 30]); - grid on; - title(strcat({'Magnetic Heading Innovations and Variances'},runIdentifier)); - ylabel('yaw innovation (deg)'); - xlabel('time (sec)'); - legend('innovation','innovation variance sqrt','innovation variance sqrt'); - subplot(2,1,2); - plot(output.innovations.mag_time_lapsed,output.innovations.magLength); - ylim([0 0.7]); - grid on; - title(strcat({'Magnetic Flux'},runIdentifier)); - ylabel('Flux (Gauss)'); - xlabel('time (sec)'); - fileName='magnetometer_fusion.png'; - fullFileName = fullfile(folder, fileName); - saveas(h,fullFileName); - -end - -%% plot optical flow innovations -if isfield(output.innovations,'flowInnov') - - figure('Units','Pixels','Position',plotDimensions,'PaperOrientation','portrait'); - h=gcf; - set(h,'PaperOrientation','portrait'); - set(h,'PaperUnits','normalized'); - set(h,'PaperPosition', [0 0 1 1]); - subplot(2,1,1); - plot(output.innovations.flow_time_lapsed,[output.innovations.flowInnov(:,1)';sqrt(output.innovations.flowInnovVar(:,1))';-sqrt(output.innovations.flowInnovVar(:,1))']); - ylim([-1.0 1.0]); - grid on; - title(strcat({'Optical Flow Innovations and Variances'},runIdentifier)); - ylabel('X (rad/sec)'); - xlabel('time (sec)'); - legend('innovation','innovation variance sqrt','innovation variance sqrt'); - subplot(2,1,2); - plot(output.innovations.flow_time_lapsed,[output.innovations.flowInnov(:,2)';sqrt(output.innovations.flowInnovVar(:,2))';-sqrt(output.innovations.flowInnovVar(:,2))']); - ylim([-1.0 1.0]); - grid on; - ylabel('Y (rad/sec)'); - xlabel('time (sec)'); - legend('innovation','innovation variance sqrt','innovation variance sqrt'); - fileName='optical_flow_fusion.png'; - fullFileName = fullfile(folder, fileName); - saveas(h,fullFileName); - -end - -%% plot ZED camera innovations -if isfield(output.innovations,'bodyVelInnov') - - figure('Units','Pixels','Position',plotDimensions,'PaperOrientation','portrait'); - h=gcf; - set(h,'PaperOrientation','portrait'); - set(h,'PaperUnits','normalized'); - set(h,'PaperPosition', [0 0 1 1]); - - subplot(3,1,1); - plot(output.innovations.bodyVel_time_lapsed,[output.innovations.bodyVelInnov(:,1)';sqrt(output.innovations.bodyVelInnovVar(:,1))';-sqrt(output.innovations.bodyVelInnovVar(:,1))']); - grid on; - title(strcat({'ZED Camera Innovations and Variances'},runIdentifier)); - ylabel('X (m/sec)'); - xlabel('time (sec)'); - legend('innovation','innovation variance sqrt','innovation variance sqrt'); - - subplot(3,1,2); - plot(output.innovations.bodyVel_time_lapsed,[output.innovations.bodyVelInnov(:,2)';sqrt(output.innovations.bodyVelInnovVar(:,2))';-sqrt(output.innovations.bodyVelInnovVar(:,2))']); - grid on; - ylabel('Y (m/sec)'); - xlabel('time (sec)'); - legend('innovation','innovation variance sqrt','innovation variance sqrt'); - - subplot(3,1,3); - plot(output.innovations.bodyVel_time_lapsed,[output.innovations.bodyVelInnov(:,3)';sqrt(output.innovations.bodyVelInnovVar(:,3))';-sqrt(output.innovations.bodyVelInnovVar(:,3))']); - grid on; - ylabel('Z (m/sec)'); - xlabel('time (sec)'); - legend('innovation','innovation variance sqrt','innovation variance sqrt'); - - fileName='zed_camera_fusion.png'; - fullFileName = fullfile(folder, fileName); - saveas(h,fullFileName); - -end diff --git a/src/modules/ekf2/EKF/matlab/EKF_replay/Filter/PredictCovariance.m b/src/modules/ekf2/EKF/matlab/EKF_replay/Filter/PredictCovariance.m deleted file mode 100644 index 6f6a57a853..0000000000 --- a/src/modules/ekf2/EKF/matlab/EKF_replay/Filter/PredictCovariance.m +++ /dev/null @@ -1,66 +0,0 @@ -function P = PredictCovariance(deltaAngle, ... - deltaVelocity, ... - states,... - P, ... % Previous state covariance matrix - dt, ... % IMU and prediction time step - param) % tuning parameters - -% Set filter state process noise other than IMU errors, which are already -% built into the derived covariance prediction equations. -% This process noise determines the rate of estimation of IMU bias errors -dAngBiasSigma = dt*dt*param.prediction.dAngBiasPnoise; -dVelBiasSigma = dt*dt*param.prediction.dVelBiasPnoise; -magSigmaNED = dt*param.prediction.magPnoiseNED; -magSigmaXYZ = dt*param.prediction.magPnoiseXYZ; -processNoiseVariance = [zeros(1,10), dAngBiasSigma*[1 1 1], dVelBiasSigma*[1 1 1], magSigmaNED*[1 1 1], magSigmaXYZ*[1 1 1], [0 0]].^2; - -% Specify the noise variance on the IMU delta angles and delta velocities -daxVar = (dt*param.prediction.angRateNoise)^2; -dayVar = daxVar; -dazVar = daxVar; -dvxVar = (dt*param.prediction.accelNoise)^2; -dvyVar = dvxVar; -dvzVar = dvxVar; - -dvx = deltaVelocity(1); -dvy = deltaVelocity(2); -dvz = deltaVelocity(3); -dax = deltaAngle(1); -day = deltaAngle(2); -daz = deltaAngle(3); - -q0 = states(1); -q1 = states(2); -q2 = states(3); -q3 = states(4); - -dax_b = states(11); -day_b = states(12); -daz_b = states(13); - -dvx_b = states(14); -dvy_b = states(15); -dvz_b = states(16); - -% Predicted covariance -F = calcF24(dax,dax_b,day,day_b,daz,daz_b,dt,dvx,dvx_b,dvy,dvy_b,dvz,dvz_b,q0,q1,q2,q3); -Q = calcQ24(daxVar,dayVar,dazVar,dvxVar,dvyVar,dvzVar,q0,q1,q2,q3); -P = F*P*transpose(F) + Q; - -% Add the general process noise variance -for i = 1:24 - P(i,i) = P(i,i) + processNoiseVariance(i); -end - -% Force symmetry on the covariance matrix to prevent ill-conditioning -% of the matrix which would cause the filter to blow-up -P = 0.5*(P + transpose(P)); - -% ensure diagonals are positive -for i=1:24 - if P(i,i) < 0 - P(i,i) = 0; - end -end - -end diff --git a/src/modules/ekf2/EKF/matlab/EKF_replay/Filter/PredictStates.m b/src/modules/ekf2/EKF/matlab/EKF_replay/Filter/PredictStates.m deleted file mode 100755 index bf6096008a..0000000000 --- a/src/modules/ekf2/EKF/matlab/EKF_replay/Filter/PredictStates.m +++ /dev/null @@ -1,85 +0,0 @@ -function [states, correctedDelAng, correctedDelVel] = PredictStates( ... - states, ... % previous state vector (4x1 quaternion, 3x1 velocity, 3x1 position, 3x1 delAng bias, 3x1 delVel bias) - delAng, ... % IMU delta angle measurements, 3x1 (rad) - delVel, ... % IMU delta velocity measurements 3x1 (m/s) - dt, ... % accumulation time of the IMU measurement (sec) - gravity, ... % acceleration due to gravity (m/s/s) - latitude) % WGS-84 latitude (rad) - -% define persistent variables for previous delta angle and velocity which -% are required for sculling and coning error corrections -persistent prevDelAng; -if isempty(prevDelAng) - prevDelAng = delAng; -end - -persistent prevDelVel; -if isempty(prevDelVel) - prevDelVel = delVel; -end - -persistent Tbn_prev; -if isempty(Tbn_prev) - Tbn_prev = Quat2Tbn(states(1:4)); -end - -% Remove sensor bias errors -delAng = delAng - states(11:13); -delVel = delVel - states(14:16); - -% Correct delta velocity for rotation and skulling -% Derived from Eqn 25 of: -% "Computational Elements For Strapdown Systems" -% Savage, P.G. -% Strapdown Associates -% 2015, WBN-14010 -% correctedDelVel= delVel + ... -% 0.5*cross(prevDelAng + delAng , prevDelVel + delVel) + 1/6*cross(prevDelAng + delAng , cross(prevDelAng + delAng , prevDelVel + delVel)) + 1/12*(cross(prevDelAng , delVel) + cross(prevDelVel , delAng)); -correctedDelVel= delVel; - -% Calculate earth delta angle spin vector -delAngEarth_NED(1,1) = 0.000072921 * cos(latitude) * dt; -delAngEarth_NED(2,1) = 0.0; -delAngEarth_NED(3,1) = -0.000072921 * sin(latitude) * dt; - -% Apply corrections for coning errors and earth spin rate -% Coning correction from : -% "A new strapdown attitude algorithm", -% R. B. MILLER, -% Journal of Guidance, Control, and Dynamics -% July, Vol. 6, No. 4, pp. 287-291, Eqn 11 -% correctedDelAng = delAng - 1/12*cross(prevDelAng , delAng) - transpose(Tbn_prev)*delAngEarth_NED; -correctedDelAng = delAng - transpose(Tbn_prev)*delAngEarth_NED; - -% Save current measurements -prevDelAng = delAng; -prevDelVel = delVel; - -% Convert the rotation vector to its equivalent quaternion -deltaQuat = RotToQuat(correctedDelAng); - -% Update the quaternions by rotating from the previous attitude through -% the delta angle rotation quaternion -states(1:4) = QuatMult(states(1:4),deltaQuat); - -% Normalise the quaternions -states(1:4) = NormQuat(states(1:4)); - -% Calculate the body to nav cosine matrix -Tbn = Quat2Tbn(states(1:4)); -Tbn_prev = Tbn; - -% transform body delta velocities to delta velocities in the nav frame -delVelNav = Tbn * correctedDelVel + [0;0;gravity]*dt; - -% take a copy of the previous velocity -prevVel = states(5:7); - -% Sum delta velocities to get the velocity -states(5:7) = states(5:7) + delVelNav(1:3); - -% integrate the velocity vrctor to get the position using trapezoidal -% integration -states(8:10) = states(8:10) + 0.5 * dt * (prevVel + states(5:7)); - -end \ No newline at end of file diff --git a/src/modules/ekf2/EKF/matlab/EKF_replay/Filter/RunFilter.m b/src/modules/ekf2/EKF/matlab/EKF_replay/Filter/RunFilter.m deleted file mode 100755 index 33259f94c7..0000000000 --- a/src/modules/ekf2/EKF/matlab/EKF_replay/Filter/RunFilter.m +++ /dev/null @@ -1,347 +0,0 @@ -function output = RunFilter(param,imu_data,mag_data,baro_data,gps_data,varargin) - -% compulsory inputs - -% param : parameters defined by SetParameterDefaults.m -% imu_data : IMU delta angle and velocity data in body frame -% mag_data : corrected magnetometer field measurements in body frame -% baro_data : barometric height measurements -% gps_data : GPS NED pos vel measurements in local earth frame - -% optional inputs - -% rng _data : measurements for a Z body axis aligned range finder -% flow_data : XY axis optical flow angular rate measurements in body frame -% viso_data : ZED camera visula odometry measurements - -nVarargs = length(varargin); -if nVarargs >= 2 - flowDataPresent = ~isempty(varargin{1}) && ~isempty(varargin{2}); - rng_data = varargin{1}; - flow_data = varargin{2}; - if flowDataPresent - fprintf('Using optical Flow Data\n',nVarargs); - end -else - flowDataPresent = 0; -end - -if nVarargs >= 3 - visoDataPresent = ~isempty(varargin{3}); - viso_data = varargin{3}; - if visoDataPresent - fprintf('Using ZED camera odometry data\n',nVarargs); - end -else - visoDataPresent = 0; -end - - -%% Set initial conditions - -% constants -deg2rad = pi/180; -gravity = 9.80665; % initial value of gravity - will be updated when WGS-84 position is known - -% initialise the state vector -[states, imu_start_index] = InitStates(param,imu_data,gps_data,mag_data,baro_data); - -dt_imu_avg = 0.5 * (median(imu_data.gyro_dt) + median(imu_data.accel_dt)); -indexStop = length(imu_data.time_us) - imu_start_index; -indexStart = 1; - -% create static structs for output data -output = struct('time_lapsed',[]',... - 'euler_angles',[],... - 'velocity_NED',[],... - 'position_NED',[],... - 'gyro_bias',[],... - 'accel_bias',[],... - 'mag_NED',[],... - 'mag_XYZ',[],... - 'wind_NE',[],... - 'dt',0,... - 'state_variances',[],... - 'innovations',[],... - 'magFuseMethod',[]); - -% initialise the state covariance matrix -covariance = InitCovariance(param,dt_imu_avg,1,gps_data); - -%% Main Loop - -% control flags -gps_use_started = boolean(false); -gps_use_blocked = boolean(false); -viso_use_blocked = boolean(false); -flow_use_blocked = boolean(false); - -% array access index variables -imuIndex = imu_start_index; -last_gps_index = 0; -gps_fuse_index = 0; -last_baro_index = 0; -baro_fuse_index = 0; -last_mag_index = 0; -mag_fuse_index = 0; -last_flow_index = 0; -flow_fuse_index = 0; -last_viso_index = 0; -viso_fuse_index = 0; -last_range_index = 0; - -% covariance prediction variables -delAngCov = [0;0;0]; % delta angle vector used by the covariance prediction (rad) -delVelCov = [0;0;0]; % delta velocity vector used by the covariance prediction (m/sec) -dtCov = 0; % time step used by the covariance prediction (sec) -dtCovInt = 0; % accumulated time step of covariance predictions (sec) -covIndex = 0; % covariance prediction frame counter - -output.magFuseMethod = param.fusion.magFuseMethod; -range = 0.1; - -% variables used to control dead-reckoning timeout -last_drift_constrain_time = - param.control.velDriftTimeLim; -last_synthetic_velocity_fusion_time = 0; -last_valid_range_time = - param.fusion.rngTimeout; - -for index = indexStart:indexStop - - % read IMU measurements - local_time=imu_data.time_us(imuIndex)*1e-6; - delta_angle(:,1) = imu_data.del_ang(imuIndex,:); - delta_velocity(:,1) = imu_data.del_vel(imuIndex,:); - dt_imu = 0.5 * (imu_data.accel_dt(imuIndex) + imu_data.gyro_dt(imuIndex)); - imuIndex = imuIndex+1; - - % predict states - [states, delAngCorrected, delVelCorrected] = PredictStates(states,delta_angle,delta_velocity,imu_data.accel_dt(imuIndex),gravity,gps_data.refLLH(1,1)*deg2rad); - - % constrain states - [states] = ConstrainStates(states,dt_imu_avg); - - dtCov = dtCov + dt_imu; - delAngCov = delAngCov + delAngCorrected; - delVelCov = delVelCov + delVelCorrected; - if (dtCov > 0.01) - % predict covariance - covariance = PredictCovariance(delAngCov,delVelCov,states,covariance,dtCov,param); - delAngCov = [0;0;0]; - delVelCov = [0;0;0]; - dtCovInt = dtCovInt + dtCov; - dtCov = 0; - covIndex = covIndex + 1; - - % output state data - output.time_lapsed(covIndex) = local_time; - output.euler_angles(covIndex,:) = QuatToEul(states(1:4)')'; - output.velocity_NED(covIndex,:) = states(5:7)'; - output.position_NED(covIndex,:) = states(8:10)'; - output.gyro_bias(covIndex,:) = states(11:13)'; - output.accel_bias(covIndex,:) = states(14:16)'; - output.mag_NED(covIndex,:) = states(17:19); - output.mag_XYZ(covIndex,:) = states(20:22); - output.wind_NE(covIndex,:) = states(23:24); - - % output covariance data - for i=1:24 - output.state_variances(covIndex,i) = covariance(i,i); - end - - % output equivalent euler angle variances - error_transfer_matrix = quat_to_euler_error_transfer_matrix(states(1),states(2),states(3),states(4)); - euler_covariance_matrix = error_transfer_matrix * covariance(1:4,1:4) * transpose(error_transfer_matrix); - for i=1:3 - output.euler_variances(covIndex,i) = euler_covariance_matrix(i,i); - end - - % Get most recent GPS data that had fallen behind the fusion time horizon - latest_gps_index = find((gps_data.time_us - 1e6 * param.fusion.gpsTimeDelay) < imu_data.time_us(imuIndex), 1, 'last' ); - - if ~isempty(latest_gps_index) - % Check if GPS use is being blocked by the user - if ((local_time < param.control.gpsOnTime) && (local_time > param.control.gpsOffTime)) - gps_use_started = false; - gps_use_blocked = true; - else - gps_use_blocked = false; - end - - % If we haven't started using GPS, check that the quality is sufficient before aligning the position and velocity states to GPS - if (~gps_use_started && ~gps_use_blocked) - if ((gps_data.spd_error(latest_gps_index) < param.control.gpsSpdErrLim) && (gps_data.pos_error(latest_gps_index) < param.control.gpsPosErrLim)) - states(5:7) = gps_data.vel_ned(latest_gps_index,:); - states(8:9) = gps_data.pos_ned(latest_gps_index,1:2); - gps_use_started = true; - last_drift_constrain_time = local_time; - end - end - - % Fuse GPS data when available if GPS use has started - if (gps_use_started && ~gps_use_blocked && (latest_gps_index > last_gps_index)) - last_gps_index = latest_gps_index; - gps_fuse_index = gps_fuse_index + 1; - last_drift_constrain_time = local_time; - - % fuse NED GPS velocity - [states,covariance,velInnov,velInnovVar] = FuseVelocity(states,covariance,gps_data.vel_ned(latest_gps_index,:),param.fusion.gpsVelGate,gps_data.spd_error(latest_gps_index)); - - % data logging - output.innovations.vel_time_lapsed(gps_fuse_index) = local_time; - output.innovations.vel_innov(gps_fuse_index,:) = velInnov'; - output.innovations.vel_innov_var(gps_fuse_index,:) = velInnovVar'; - - % fuse NE GPS position - [states,covariance,posInnov,posInnovVar] = FusePosition(states,covariance,gps_data.pos_ned(latest_gps_index,:),param.fusion.gpsPosGate,gps_data.pos_error(latest_gps_index)); - - % data logging - output.innovations.pos_time_lapsed(gps_fuse_index) = local_time; - output.innovations.posInnov(gps_fuse_index,:) = posInnov'; - output.innovations.posInnovVar(gps_fuse_index,:) = posInnovVar'; - else - % Check if drift is being corrected by some form of aiding and if not, fuse in a zero position measurement at 5Hz to prevent states diverging - if ((local_time - last_drift_constrain_time) > param.control.velDriftTimeLim) - if ((local_time - last_synthetic_velocity_fusion_time) > 0.2) - [states,covariance,~,~] = FusePosition(states,covariance,zeros(1,2),100.0,param.control.gpsPosErrLim); - last_synthetic_velocity_fusion_time = local_time; - end - end - end - end - - % Fuse new Baro data that has fallen beind the fusion time horizon - latest_baro_index = find((baro_data.time_us - 1e6 * param.fusion.baroTimeDelay) < imu_data.time_us(imuIndex), 1, 'last' ); - if (latest_baro_index > last_baro_index) - last_baro_index = latest_baro_index; - baro_fuse_index = baro_fuse_index + 1; - - % fuse baro height - [states,covariance,hgtInnov,hgtInnovVar] = FuseBaroHeight(states,covariance,baro_data.height(latest_baro_index),param.fusion.baroHgtGate,param.fusion.baroHgtNoise); - - % data logging - output.innovations.hgt_time_lapsed(baro_fuse_index) = local_time; - output.innovations.hgtInnov(baro_fuse_index) = hgtInnov; - output.innovations.hgtInnovVar(baro_fuse_index) = hgtInnovVar; - end - - % Fuse new mag data that has fallen behind the fusion time horizon - latest_mag_index = find((mag_data.time_us - 1e6 * param.fusion.magTimeDelay) < imu_data.time_us(imuIndex), 1, 'last' ); - if (latest_mag_index > last_mag_index) - last_mag_index = latest_mag_index; - mag_fuse_index = mag_fuse_index + 1; - - % output magnetic field length to help with diagnostics - output.innovations.magLength(mag_fuse_index) = sqrt(dot(mag_data.field_ga(latest_mag_index,:),mag_data.field_ga(latest_mag_index,:))); - - % fuse magnetometer data - if (param.fusion.magFuseMethod == 0 || param.fusion.magFuseMethod == 1) - [states,covariance,magInnov,magInnovVar] = FuseMagnetometer(states,covariance,mag_data.field_ga(latest_mag_index,:),param.fusion.magFieldGate, param.fusion.magFieldError^2); - - % data logging - output.innovations.mag_time_lapsed(mag_fuse_index) = local_time; - output.innovations.magInnov(mag_fuse_index,:) = magInnov; - output.innovations.magInnovVar(mag_fuse_index,:) = magInnovVar; - - if (param.fusion.magFuseMethod == 1) - % fuse in the local declination value - [states, covariance] = FuseMagDeclination(states, covariance, param.fusion.magDeclDeg*deg2rad); - - end - - elseif (param.fusion.magFuseMethod == 2) - % fuse magnetometer data as a single magnetic heading measurement - [states, covariance, hdgInnov, hdgInnovVar] = FuseMagHeading(states, covariance, mag_data.field_ga(latest_mag_index,:), param.fusion.magDeclDeg*deg2rad, param.fusion.magHdgGate, param.fusion.magHdgError^2); - - % log data - output.innovations.mag_time_lapsed(mag_fuse_index) = local_time; - output.innovations.hdgInnov(mag_fuse_index) = hdgInnov; - output.innovations.hdgInnovVar(mag_fuse_index) = hdgInnovVar; - - end - - end - - % Check if optical flow use is being blocked by the user - if ((local_time < param.control.flowOnTime) && (local_time > param.control.flowOffTime)) - flow_use_blocked = true; - else - flow_use_blocked = false; - end - - % Attempt to use optical flow and range finder data if available and not blocked - if (flowDataPresent && ~flow_use_blocked) - - % Get latest range finder data and gate to remove dropouts - last_range_index = find((rng_data.time_us - 1e6 * param.fusion.rangeTimeDelay) < imu_data.time_us(imuIndex), 1, 'last' ); - if (rng_data.dist(last_range_index) < param.fusion.rngValidMax) - range = max(rng_data.dist(last_range_index) , param.fusion.rngValidMin); - last_valid_range_time = local_time; - end - - % Fuse optical flow data that has fallen behind the fusion time horizon if we have a valid range measurement - latest_flow_index = find((flow_data.time_us - 1e6 * param.fusion.flowTimeDelay) < imu_data.time_us(imuIndex), 1, 'last' ); - - if (~isempty(latest_flow_index) && (latest_flow_index > last_flow_index) && ((local_time - last_valid_range_time) < param.fusion.rngTimeout)) - last_flow_index = latest_flow_index; - flow_fuse_index = flow_fuse_index + 1; - last_drift_constrain_time = local_time; - - % fuse flow data - flowRate = [flow_data.flowX(latest_flow_index);flow_data.flowY(latest_flow_index)]; - bodyRate = [flow_data.bodyX(latest_flow_index);flow_data.bodyY(latest_flow_index)]; - [states,covariance,flowInnov,flowInnovVar] = FuseOpticalFlow(states, covariance, flowRate, bodyRate, range, param.fusion.flowRateError^2, param.fusion.flowGate); - - % data logging - output.innovations.flow_time_lapsed(flow_fuse_index) = local_time; - output.innovations.flowInnov(flow_fuse_index,:) = flowInnov; - output.innovations.flowInnovVar(flow_fuse_index,:) = flowInnovVar; - - end - - end - - % Check if optical flow use is being blocked by the user - if ((local_time < param.control.visoOnTime) && (local_time > param.control.visoOffTime)) - viso_use_blocked = true; - else - viso_use_blocked = false; - end - - % attempt to use ZED camera visual odmetry data if available and not blocked - if (visoDataPresent && ~viso_use_blocked) - - % Fuse ZED camera body frame odmometry data that has fallen behind the fusion time horizon - latest_viso_index = find((viso_data.time_us - 1e6 * param.fusion.bodyVelTimeDelay) < imu_data.time_us(imuIndex), 1, 'last' ); - if (latest_viso_index > last_viso_index) - last_viso_index = latest_viso_index; - viso_fuse_index = viso_fuse_index + 1; - last_drift_constrain_time = local_time; - - % convert delta position measurements to velocity - relVelBodyMea = [viso_data.dPosX(latest_viso_index);viso_data.dPosY(latest_viso_index);viso_data.dPosZ(latest_viso_index)]./viso_data.dt(latest_viso_index); - - % convert quality metric to equivalent observation error - % (this is a guess) - quality = viso_data.qual(latest_viso_index); - bodyVelError = param.fusion.bodyVelErrorMin * quality + param.fusion.bodyVelErrorMax * (1 - quality); - - % fuse measurements - [states,covariance,bodyVelInnov,bodyVelInnovVar] = FuseBodyVel(states, covariance, relVelBodyMea, bodyVelError^2, param.fusion.bodyVelGate); - - % data logging - output.innovations.bodyVel_time_lapsed(viso_fuse_index) = local_time; - output.innovations.bodyVelInnov(viso_fuse_index,:) = bodyVelInnov; - output.innovations.bodyVelInnovVar(viso_fuse_index,:) = bodyVelInnovVar; - - end - - end - - end - - % update average delta time estimate - output.dt = dtCovInt / covIndex; - -end - -end diff --git a/src/modules/ekf2/EKF/matlab/EKF_replay/Filter/SetParameterDefaults.m b/src/modules/ekf2/EKF/matlab/EKF_replay/Filter/SetParameterDefaults.m deleted file mode 100755 index e9d80139e4..0000000000 --- a/src/modules/ekf2/EKF/matlab/EKF_replay/Filter/SetParameterDefaults.m +++ /dev/null @@ -1,69 +0,0 @@ -%% Filter Control -param.control.waitForGps = 0; % set to 1 if the filter start should be delayed until GPS checks to pass -param.control.gpsSpdErrLim = 1.0; % GPS use will not start if reported GPS speed error is greater than this (m/s) -param.control.gpsPosErrLim = 5.0; % GPS use will not start if reported GPS position error is greater than this (m) -param.control.velDriftTimeLim = 5.0; % The maximum time without observations to constrain velocity drift before a zero velocity is fused to prevent the filter diverging (sec) -param.control.gpsOffTime = 0; % GPS aiding will be turned off at this time (sec) -param.control.gpsOnTime = 0; % GPS aiding will be turned back on at this time (sec) -param.control.flowOffTime = 0; % optical flow aiding will be turned off at this time (sec) -param.control.flowOnTime = 0; % optical flow aiding will be turned back on on at this time (sec) -param.control.visoOffTime = 0; % visual odometry aiding will be turned off at this time (sec) -param.control.visoOnTime = 0; % visual odometry aiding will be turned back on at this time (sec) -param.control.rollAlignErr = 0.0; % initial roll misalignment (rad) -param.control.pitchAlignErr = 0.0; % initial pitch misalignment (rad) -param.control.yawAlignErr = 0.0; % initial yaw misalignment (rad) - -%% GPS fusion -param.fusion.gpsTimeDelay = 0.1; % GPS measurement delay relative to IMU (sec) -param.fusion.gpsVelGate = 5.0; % Size of the IMU velocity innovation consistency check gate in SD -param.fusion.gpsPosGate = 5.0; % Size of the IMU velocity innovation consistency check gate in SD -param.fusion.gpsCheckTimeout = 10.0; % Length of time that GPS measurements will be rejected by the filter before states are reset to the GPS velocity. (sec) - -%% Baro fusion -param.fusion.baroTimeDelay = 0.05; % Baro measurement delay relative to IMU (sec) -param.fusion.baroHgtGate = 5.0; % Size of the IMU velocity innovation consistency check gate in SD -param.fusion.baroHgtNoise = 2.0; % 1SD observation noise of the baro measurements (m) - -%% Magnetometer measurement fusion -param.fusion.magTimeDelay = 0.0; % magnetometer time delay relative to IMU (sec) -param.fusion.magFuseMethod = 1; % 0: 3-Axis field fusion with free declination, 1: 3-Axis field fusion with constrained declination, 2: magnetic heading fusion. (None) -param.fusion.magFieldError = 0.05; % Magnetic field measurement 1SD error including hard and soft iron interference. Used when magFuseMethod = 0 or 1. (gauss) -param.fusion.magHdgError = 0.1745; % Magnetic heading measurement 1SD error including hard and soft iron interference. Used when magFuseMethod = 2. (rad) -param.fusion.magFieldGate = 5.0; % Size of the magnetic field innovation consistency check gate in SD -param.fusion.magHdgGate = 5.0; % Size of the magnetic heading innovation consistency check gate in SD -param.fusion.magDeclDeg = 10.5; % Magnetic declination in deg - -%% Optical flow measurement fusion -param.fusion.rangeTimeDelay = 0.05; % range fidner sensor delay relative to IMU (sec) -param.fusion.flowTimeDelay = 0.05; % Optical flow sensor time delay relative to IMU (sec) -param.fusion.flowRateError = 0.15; % Observation noise 1SD for the flow sensor (rad/sec) -param.fusion.flowGate = 5.0; % Size of the optical flow rate innovation consistency check gate in SD -param.fusion.rngValidMin = 0.05; % range measurements wil be constrained to be no less than this (m) -param.fusion.rngValidMax = 5.0; % ignore range measurements larger than this (m) -param.fusion.rngTimeout = 2.0; % optical flow measurements will not be used if more than this time since valid range finder data was received (sec) - -%% Visual odometry body frame velocity measurement fusion -param.fusion.bodyVelTimeDelay = 0.01; % Optical flow sensor time delay relative to IMU (sec) -param.fusion.bodyVelErrorMin = 0.1; % Observation noise 1SD for the odometry sensor at the highest quality value (m/sec) -param.fusion.bodyVelErrorMax = 0.9; % Observation noise 1SD for the odometry sensor at the lowest quality value (m/sec) -param.fusion.bodyVelGate = 5.0; % Size of the optical flow rate innovation consistency check gate in SD - -%% State prediction error growth -param.prediction.magPnoiseNED = 1e-3; % Earth magnetic field 1SD rate of change. (gauss/sec) -param.prediction.magPnoiseXYZ = 1e-3; % Body magnetic field 1SD rate of change. (gauss/sec) -param.prediction.dAngBiasPnoise = 0.001; % IMU gyro bias 1SD rate of change (rad/sec^2) -param.prediction.dVelBiasPnoise = 0.03; % IMU accel bias 1SD rate of change (m/sec^3) -param.prediction.angRateNoise = 0.015; % IMU gyro 1SD rate process noise (rad/sec) -param.prediction.accelNoise = 0.35; % IMU accelerometer 1SD error noise including switch on bias uncertainty. (m/sec^2) - -%% Initial Uncertainty -param.alignment.posErrNE = 10.0; % Initial 1SD position error when aligning without GPS. (m/sec) -param.alignment.velErrNE = 5.0; % Initial 1SD velocity error when aligning without GPS. (m/sec) -param.alignment.velErrD = 1.0; % Initial 1SD vertical velocity error when aligning without GPS. (m/sec) -param.alignment.delAngBiasErr = 0.05*pi/180; % Initial 1SD rate gyro bias uncertainty. (rad/sec) -param.alignment.delVelBiasErr = 0.07; % Initial 1SD accelerometer bias uncertainty. (m/sec^2) -param.alignment.quatErr = 0.1; % Initial 1SD uncertainty in quaternion. -param.alignment.magErrXYZ = 0.01; % Initial 1SD uncertainty in body frame XYZ magnetic field states. (gauss) -param.alignment.magErrNED = 0.5; % Initial 1SD uncertainty in earth frame NED magnetic field states. (gauss) -param.alignment.hgtErr = 0.5; % Initial 1SD uncertainty in height. (m) -param.alignment.windErrNE = 5.0; % Initial 1SD error in wind states. (m/sec) diff --git a/src/modules/ekf2/EKF/matlab/EKF_replay/Filter/SetParameters.m b/src/modules/ekf2/EKF/matlab/EKF_replay/Filter/SetParameters.m deleted file mode 100755 index e9d80139e4..0000000000 --- a/src/modules/ekf2/EKF/matlab/EKF_replay/Filter/SetParameters.m +++ /dev/null @@ -1,69 +0,0 @@ -%% Filter Control -param.control.waitForGps = 0; % set to 1 if the filter start should be delayed until GPS checks to pass -param.control.gpsSpdErrLim = 1.0; % GPS use will not start if reported GPS speed error is greater than this (m/s) -param.control.gpsPosErrLim = 5.0; % GPS use will not start if reported GPS position error is greater than this (m) -param.control.velDriftTimeLim = 5.0; % The maximum time without observations to constrain velocity drift before a zero velocity is fused to prevent the filter diverging (sec) -param.control.gpsOffTime = 0; % GPS aiding will be turned off at this time (sec) -param.control.gpsOnTime = 0; % GPS aiding will be turned back on at this time (sec) -param.control.flowOffTime = 0; % optical flow aiding will be turned off at this time (sec) -param.control.flowOnTime = 0; % optical flow aiding will be turned back on on at this time (sec) -param.control.visoOffTime = 0; % visual odometry aiding will be turned off at this time (sec) -param.control.visoOnTime = 0; % visual odometry aiding will be turned back on at this time (sec) -param.control.rollAlignErr = 0.0; % initial roll misalignment (rad) -param.control.pitchAlignErr = 0.0; % initial pitch misalignment (rad) -param.control.yawAlignErr = 0.0; % initial yaw misalignment (rad) - -%% GPS fusion -param.fusion.gpsTimeDelay = 0.1; % GPS measurement delay relative to IMU (sec) -param.fusion.gpsVelGate = 5.0; % Size of the IMU velocity innovation consistency check gate in SD -param.fusion.gpsPosGate = 5.0; % Size of the IMU velocity innovation consistency check gate in SD -param.fusion.gpsCheckTimeout = 10.0; % Length of time that GPS measurements will be rejected by the filter before states are reset to the GPS velocity. (sec) - -%% Baro fusion -param.fusion.baroTimeDelay = 0.05; % Baro measurement delay relative to IMU (sec) -param.fusion.baroHgtGate = 5.0; % Size of the IMU velocity innovation consistency check gate in SD -param.fusion.baroHgtNoise = 2.0; % 1SD observation noise of the baro measurements (m) - -%% Magnetometer measurement fusion -param.fusion.magTimeDelay = 0.0; % magnetometer time delay relative to IMU (sec) -param.fusion.magFuseMethod = 1; % 0: 3-Axis field fusion with free declination, 1: 3-Axis field fusion with constrained declination, 2: magnetic heading fusion. (None) -param.fusion.magFieldError = 0.05; % Magnetic field measurement 1SD error including hard and soft iron interference. Used when magFuseMethod = 0 or 1. (gauss) -param.fusion.magHdgError = 0.1745; % Magnetic heading measurement 1SD error including hard and soft iron interference. Used when magFuseMethod = 2. (rad) -param.fusion.magFieldGate = 5.0; % Size of the magnetic field innovation consistency check gate in SD -param.fusion.magHdgGate = 5.0; % Size of the magnetic heading innovation consistency check gate in SD -param.fusion.magDeclDeg = 10.5; % Magnetic declination in deg - -%% Optical flow measurement fusion -param.fusion.rangeTimeDelay = 0.05; % range fidner sensor delay relative to IMU (sec) -param.fusion.flowTimeDelay = 0.05; % Optical flow sensor time delay relative to IMU (sec) -param.fusion.flowRateError = 0.15; % Observation noise 1SD for the flow sensor (rad/sec) -param.fusion.flowGate = 5.0; % Size of the optical flow rate innovation consistency check gate in SD -param.fusion.rngValidMin = 0.05; % range measurements wil be constrained to be no less than this (m) -param.fusion.rngValidMax = 5.0; % ignore range measurements larger than this (m) -param.fusion.rngTimeout = 2.0; % optical flow measurements will not be used if more than this time since valid range finder data was received (sec) - -%% Visual odometry body frame velocity measurement fusion -param.fusion.bodyVelTimeDelay = 0.01; % Optical flow sensor time delay relative to IMU (sec) -param.fusion.bodyVelErrorMin = 0.1; % Observation noise 1SD for the odometry sensor at the highest quality value (m/sec) -param.fusion.bodyVelErrorMax = 0.9; % Observation noise 1SD for the odometry sensor at the lowest quality value (m/sec) -param.fusion.bodyVelGate = 5.0; % Size of the optical flow rate innovation consistency check gate in SD - -%% State prediction error growth -param.prediction.magPnoiseNED = 1e-3; % Earth magnetic field 1SD rate of change. (gauss/sec) -param.prediction.magPnoiseXYZ = 1e-3; % Body magnetic field 1SD rate of change. (gauss/sec) -param.prediction.dAngBiasPnoise = 0.001; % IMU gyro bias 1SD rate of change (rad/sec^2) -param.prediction.dVelBiasPnoise = 0.03; % IMU accel bias 1SD rate of change (m/sec^3) -param.prediction.angRateNoise = 0.015; % IMU gyro 1SD rate process noise (rad/sec) -param.prediction.accelNoise = 0.35; % IMU accelerometer 1SD error noise including switch on bias uncertainty. (m/sec^2) - -%% Initial Uncertainty -param.alignment.posErrNE = 10.0; % Initial 1SD position error when aligning without GPS. (m/sec) -param.alignment.velErrNE = 5.0; % Initial 1SD velocity error when aligning without GPS. (m/sec) -param.alignment.velErrD = 1.0; % Initial 1SD vertical velocity error when aligning without GPS. (m/sec) -param.alignment.delAngBiasErr = 0.05*pi/180; % Initial 1SD rate gyro bias uncertainty. (rad/sec) -param.alignment.delVelBiasErr = 0.07; % Initial 1SD accelerometer bias uncertainty. (m/sec^2) -param.alignment.quatErr = 0.1; % Initial 1SD uncertainty in quaternion. -param.alignment.magErrXYZ = 0.01; % Initial 1SD uncertainty in body frame XYZ magnetic field states. (gauss) -param.alignment.magErrNED = 0.5; % Initial 1SD uncertainty in earth frame NED magnetic field states. (gauss) -param.alignment.hgtErr = 0.5; % Initial 1SD uncertainty in height. (m) -param.alignment.windErrNE = 5.0; % Initial 1SD error in wind states. (m/sec) diff --git a/src/modules/ekf2/EKF/matlab/EKF_replay/Filter/calcF24.m b/src/modules/ekf2/EKF/matlab/EKF_replay/Filter/calcF24.m deleted file mode 100644 index b79e329291..0000000000 --- a/src/modules/ekf2/EKF/matlab/EKF_replay/Filter/calcF24.m +++ /dev/null @@ -1,45 +0,0 @@ -function F = calcF24(dax,dax_b,day,day_b,daz,daz_b,dt,dvx,dvx_b,dvy,dvy_b,dvz,dvz_b,q0,q1,q2,q3) -%CALCF24 -% F = CALCF24(DAX,DAX_B,DAY,DAY_B,DAZ,DAZ_B,DT,DVX,DVX_B,DVY,DVY_B,DVZ,DVZ_B,Q0,Q1,Q2,Q3) - -% This function was generated by the Symbolic Math Toolbox version 6.2. -% 29-May-2017 00:16:12 - -t2 = dax_b.*(1.0./2.0); -t3 = daz_b.*(1.0./2.0); -t4 = day_b.*(1.0./2.0); -t8 = day.*(1.0./2.0); -t5 = t4-t8; -t6 = q3.*(1.0./2.0); -t7 = q2.*(1.0./2.0); -t9 = daz.*(1.0./2.0); -t10 = dax.*(1.0./2.0); -t11 = -t2+t10; -t12 = q1.*(1.0./2.0); -t13 = -t3+t9; -t14 = -t4+t8; -t15 = dvx-dvx_b; -t16 = dvy-dvy_b; -t17 = dvz-dvz_b; -t18 = q1.*t17.*2.0; -t19 = q1.*t16.*2.0; -t20 = q0.*t17.*2.0; -t21 = q1.*t15.*2.0; -t22 = q2.*t16.*2.0; -t23 = q3.*t17.*2.0; -t24 = t21+t22+t23; -t25 = q0.*t15.*2.0; -t26 = q2.*t17.*2.0; -t37 = q3.*t16.*2.0; -t27 = t25+t26-t37; -t28 = q0.*q3.*2.0; -t29 = q0.^2; -t30 = q1.^2; -t31 = q2.^2; -t32 = q3.^2; -t33 = q2.*t15.*2.0; -t34 = q3.*t15.*2.0; -t35 = q0.*t16.*2.0; -t36 = -t18+t34+t35; -t38 = q0.*q1.*2.0; -F = reshape([1.0,t11,t14,t13,t27,t36,t19+t20-t33,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,dax.*(-1.0./2.0)+t2,1.0,t3-t9,t14,t24,-t19-t20+t33,t36,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,t5,t13,1.0,t2-t10,t19+t20-q2.*t15.*2.0,t24,-t25-t26+t37,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,daz.*(-1.0./2.0)+t3,t5,t11,1.0,t18-q0.*t16.*2.0-q3.*t15.*2.0,t27,t24,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,1.0,0.0,0.0,dt,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,1.0,0.0,0.0,dt,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,1.0,0.0,0.0,dt,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,1.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,1.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,1.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,t12,q0.*(-1.0./2.0),-t6,t7,0.0,0.0,0.0,0.0,0.0,0.0,1.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,t7,t6,q0.*(-1.0./2.0),-t12,0.0,0.0,0.0,0.0,0.0,0.0,0.0,1.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,t6,-t7,t12,q0.*(-1.0./2.0),0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,1.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,-t29-t30+t31+t32,-t28-q1.*q2.*2.0,q0.*q2.*2.0-q1.*q3.*2.0,0.0,0.0,0.0,0.0,0.0,0.0,1.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,t28-q1.*q2.*2.0,-t29+t30-t31+t32,-t38-q2.*q3.*2.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,1.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,q0.*q2.*-2.0-q1.*q3.*2.0,t38-q2.*q3.*2.0,-t29+t30+t31-t32,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,1.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,1.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,1.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,1.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,1.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,1.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,1.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,1.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,1.0],[24, 24]); diff --git a/src/modules/ekf2/EKF/matlab/EKF_replay/Filter/calcH_HDG.m b/src/modules/ekf2/EKF/matlab/EKF_replay/Filter/calcH_HDG.m deleted file mode 100644 index dc93b29dfa..0000000000 --- a/src/modules/ekf2/EKF/matlab/EKF_replay/Filter/calcH_HDG.m +++ /dev/null @@ -1,51 +0,0 @@ -function H_MAG = calcH_HDG(magX,magY,magZ,q0,q1,q2,q3) -%CALCH_HDG -% H_MAG = CALCH_HDG(MAGX,MAGY,MAGZ,Q0,Q1,Q2,Q3) - -% This function was generated by the Symbolic Math Toolbox version 6.2. -% 29-May-2017 00:16:14 - -t2 = q0.^2; -t3 = q1.^2; -t4 = q2.^2; -t5 = q3.^2; -t6 = q0.*q3.*2.0; -t10 = q1.*q2.*2.0; -t17 = t2-t3+t4-t5; -t18 = magY.*t17; -t19 = t6+t10; -t20 = magX.*t19; -t21 = q0.*q1.*2.0; -t22 = q2.*q3.*2.0; -t23 = t21-t22; -t24 = magZ.*t23; -t7 = t18+t20-t24; -t8 = t2+t3-t4-t5; -t9 = magX.*t8; -t11 = q0.*q2.*2.0; -t12 = q1.*q3.*2.0; -t13 = t11+t12; -t14 = magZ.*t13; -t15 = t6-t10; -t25 = magY.*t15; -t16 = t9+t14-t25; -t26 = 1.0./t16.^2; -t27 = t7.^2; -t28 = 1.0./t16; -t29 = t26.*t27; -t30 = t29+1.0; -t31 = 1.0./t30; -t32 = magX.*q1.*2.0; -t33 = magY.*q2.*2.0; -t34 = magZ.*q3.*2.0; -t35 = t32+t33+t34; -t36 = magY.*q1.*2.0; -t37 = magZ.*q0.*2.0; -t38 = t36+t37-magX.*q2.*2.0; -t39 = magX.*q0.*2.0; -t40 = magZ.*q2.*2.0; -t41 = t39+t40-magY.*q3.*2.0; -t42 = magY.*q0.*2.0; -t43 = magX.*q3.*2.0; -t44 = t42+t43-magZ.*q1.*2.0; -H_MAG = [(t28.*t44-t7.*t26.*t41)./(t27.*1.0./(t9+t14-magY.*(t6-q1.*q2.*2.0)).^2+1.0),-t31.*(t28.*t38+t7.*t26.*t35),t31.*(t28.*t35-t7.*t26.*t38),t31.*(t28.*t41+t7.*t26.*t44),0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,t31.*(t19.*t28-t7.*t8.*t26),t31.*(t17.*t28+t7.*t15.*t26),-t31.*(t23.*t28+t7.*t13.*t26),0.0,0.0]; diff --git a/src/modules/ekf2/EKF/matlab/EKF_replay/Filter/calcH_LOSX.m b/src/modules/ekf2/EKF/matlab/EKF_replay/Filter/calcH_LOSX.m deleted file mode 100644 index c91e24fa28..0000000000 --- a/src/modules/ekf2/EKF/matlab/EKF_replay/Filter/calcH_LOSX.m +++ /dev/null @@ -1,9 +0,0 @@ -function H_LOSX = calcH_LOSX(q0,q1,q2,q3,range,vd,ve,vn) -%CALCH_LOSX -% H_LOSX = CALCH_LOSX(Q0,Q1,Q2,Q3,RANGE,VD,VE,VN) - -% This function was generated by the Symbolic Math Toolbox version 6.2. -% 29-May-2017 00:16:14 - -t2 = 1.0./range; -H_LOSX = [t2.*(q1.*vd.*2.0+q0.*ve.*2.0-q3.*vn.*2.0),t2.*(q0.*vd.*2.0-q1.*ve.*2.0+q2.*vn.*2.0),t2.*(q3.*vd.*2.0+q2.*ve.*2.0+q1.*vn.*2.0),-t2.*(q2.*vd.*-2.0+q3.*ve.*2.0+q0.*vn.*2.0),-t2.*(q0.*q3.*2.0-q1.*q2.*2.0),t2.*(q0.^2-q1.^2+q2.^2-q3.^2),t2.*(q0.*q1.*2.0+q2.*q3.*2.0),0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0]; diff --git a/src/modules/ekf2/EKF/matlab/EKF_replay/Filter/calcH_LOSY.m b/src/modules/ekf2/EKF/matlab/EKF_replay/Filter/calcH_LOSY.m deleted file mode 100644 index 50f4e9fea6..0000000000 --- a/src/modules/ekf2/EKF/matlab/EKF_replay/Filter/calcH_LOSY.m +++ /dev/null @@ -1,9 +0,0 @@ -function H_LOSY = calcH_LOSY(q0,q1,q2,q3,range,vd,ve,vn) -%CALCH_LOSY -% H_LOSY = CALCH_LOSY(Q0,Q1,Q2,Q3,RANGE,VD,VE,VN) - -% This function was generated by the Symbolic Math Toolbox version 6.2. -% 29-May-2017 00:16:15 - -t2 = 1.0./range; -H_LOSY = [-t2.*(q2.*vd.*-2.0+q3.*ve.*2.0+q0.*vn.*2.0),-t2.*(q3.*vd.*2.0+q2.*ve.*2.0+q1.*vn.*2.0),t2.*(q0.*vd.*2.0-q1.*ve.*2.0+q2.*vn.*2.0),-t2.*(q1.*vd.*2.0+q0.*ve.*2.0-q3.*vn.*2.0),-t2.*(q0.^2+q1.^2-q2.^2-q3.^2),-t2.*(q0.*q3.*2.0+q1.*q2.*2.0),t2.*(q0.*q2.*2.0-q1.*q3.*2.0),0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0]; diff --git a/src/modules/ekf2/EKF/matlab/EKF_replay/Filter/calcH_MAGD.m b/src/modules/ekf2/EKF/matlab/EKF_replay/Filter/calcH_MAGD.m deleted file mode 100644 index 6c0ef0f62c..0000000000 --- a/src/modules/ekf2/EKF/matlab/EKF_replay/Filter/calcH_MAGD.m +++ /dev/null @@ -1,12 +0,0 @@ -function H_MAGD = calcH_MAGD(magE,magN) -%CALCH_MAGD -% H_MAGD = CALCH_MAGD(MAGE,MAGN) - -% This function was generated by the Symbolic Math Toolbox version 6.2. -% 29-May-2017 00:16:13 - -t2 = magE.^2; -t3 = magN.^2; -t4 = t2+t3; -t5 = 1.0./t4; -H_MAGD = [0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,-magE.*t5,magN.*t5,0.0,0.0,0.0,0.0,0.0,0.0]; diff --git a/src/modules/ekf2/EKF/matlab/EKF_replay/Filter/calcH_MAGX.m b/src/modules/ekf2/EKF/matlab/EKF_replay/Filter/calcH_MAGX.m deleted file mode 100644 index 8f951c92f4..0000000000 --- a/src/modules/ekf2/EKF/matlab/EKF_replay/Filter/calcH_MAGX.m +++ /dev/null @@ -1,8 +0,0 @@ -function H_MAGX = calcH_MAGX(magD,magE,magN,q0,q1,q2,q3) -%CALCH_MAGX -% H_MAGX = CALCH_MAGX(MAGD,MAGE,MAGN,Q0,Q1,Q2,Q3) - -% This function was generated by the Symbolic Math Toolbox version 6.2. -% 29-May-2017 00:16:12 - -H_MAGX = [magD.*q2.*-2.0+magE.*q3.*2.0+magN.*q0.*2.0,magD.*q3.*2.0+magE.*q2.*2.0+magN.*q1.*2.0,magD.*q0.*-2.0+magE.*q1.*2.0-magN.*q2.*2.0,magD.*q1.*2.0+magE.*q0.*2.0-magN.*q3.*2.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,q0.^2+q1.^2-q2.^2-q3.^2,q0.*q3.*2.0+q1.*q2.*2.0,q0.*q2.*-2.0+q1.*q3.*2.0,1.0,0.0,0.0,0.0,0.0]; diff --git a/src/modules/ekf2/EKF/matlab/EKF_replay/Filter/calcH_MAGY.m b/src/modules/ekf2/EKF/matlab/EKF_replay/Filter/calcH_MAGY.m deleted file mode 100644 index b8e7481be9..0000000000 --- a/src/modules/ekf2/EKF/matlab/EKF_replay/Filter/calcH_MAGY.m +++ /dev/null @@ -1,8 +0,0 @@ -function H_MAGY = calcH_MAGY(magD,magE,magN,q0,q1,q2,q3) -%CALCH_MAGY -% H_MAGY = CALCH_MAGY(MAGD,MAGE,MAGN,Q0,Q1,Q2,Q3) - -% This function was generated by the Symbolic Math Toolbox version 6.2. -% 29-May-2017 00:16:13 - -H_MAGY = [magD.*q1.*2.0+magE.*q0.*2.0-magN.*q3.*2.0,magD.*q0.*2.0-magE.*q1.*2.0+magN.*q2.*2.0,magD.*q3.*2.0+magE.*q2.*2.0+magN.*q1.*2.0,magD.*q2.*2.0-magE.*q3.*2.0-magN.*q0.*2.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,q0.*q3.*-2.0+q1.*q2.*2.0,q0.^2-q1.^2+q2.^2-q3.^2,q0.*q1.*2.0+q2.*q3.*2.0,0.0,1.0,0.0,0.0,0.0]; diff --git a/src/modules/ekf2/EKF/matlab/EKF_replay/Filter/calcH_MAGZ.m b/src/modules/ekf2/EKF/matlab/EKF_replay/Filter/calcH_MAGZ.m deleted file mode 100644 index 710a40a639..0000000000 --- a/src/modules/ekf2/EKF/matlab/EKF_replay/Filter/calcH_MAGZ.m +++ /dev/null @@ -1,8 +0,0 @@ -function H_MAGZ = calcH_MAGZ(magD,magE,magN,q0,q1,q2,q3) -%CALCH_MAGZ -% H_MAGZ = CALCH_MAGZ(MAGD,MAGE,MAGN,Q0,Q1,Q2,Q3) - -% This function was generated by the Symbolic Math Toolbox version 6.2. -% 29-May-2017 00:16:13 - -H_MAGZ = [magD.*q0.*2.0-magE.*q1.*2.0+magN.*q2.*2.0,magD.*q1.*-2.0-magE.*q0.*2.0+magN.*q3.*2.0,magD.*q2.*-2.0+magE.*q3.*2.0+magN.*q0.*2.0,magD.*q3.*2.0+magE.*q2.*2.0+magN.*q1.*2.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,q0.*q2.*2.0+q1.*q3.*2.0,q0.*q1.*-2.0+q2.*q3.*2.0,q0.^2-q1.^2-q2.^2+q3.^2,0.0,0.0,1.0,0.0,0.0]; diff --git a/src/modules/ekf2/EKF/matlab/EKF_replay/Filter/calcH_VELX.m b/src/modules/ekf2/EKF/matlab/EKF_replay/Filter/calcH_VELX.m deleted file mode 100644 index 1a636cf51d..0000000000 --- a/src/modules/ekf2/EKF/matlab/EKF_replay/Filter/calcH_VELX.m +++ /dev/null @@ -1,8 +0,0 @@ -function H_VELX = calcH_VELX(q0,q1,q2,q3,vd,ve,vn) -%CALCH_VELX -% H_VELX = CALCH_VELX(Q0,Q1,Q2,Q3,VD,VE,VN) - -% This function was generated by the Symbolic Math Toolbox version 6.2. -% 29-May-2017 00:16:15 - -H_VELX = [q2.*vd.*-2.0+q3.*ve.*2.0+q0.*vn.*2.0,q3.*vd.*2.0+q2.*ve.*2.0+q1.*vn.*2.0,q0.*vd.*-2.0+q1.*ve.*2.0-q2.*vn.*2.0,q1.*vd.*2.0+q0.*ve.*2.0-q3.*vn.*2.0,q0.^2+q1.^2-q2.^2-q3.^2,q0.*q3.*2.0+q1.*q2.*2.0,q0.*q2.*-2.0+q1.*q3.*2.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0]; diff --git a/src/modules/ekf2/EKF/matlab/EKF_replay/Filter/calcH_VELY.m b/src/modules/ekf2/EKF/matlab/EKF_replay/Filter/calcH_VELY.m deleted file mode 100644 index 79112ce715..0000000000 --- a/src/modules/ekf2/EKF/matlab/EKF_replay/Filter/calcH_VELY.m +++ /dev/null @@ -1,8 +0,0 @@ -function H_VELY = calcH_VELY(q0,q1,q2,q3,vd,ve,vn) -%CALCH_VELY -% H_VELY = CALCH_VELY(Q0,Q1,Q2,Q3,VD,VE,VN) - -% This function was generated by the Symbolic Math Toolbox version 6.2. -% 29-May-2017 00:16:15 - -H_VELY = [q1.*vd.*2.0+q0.*ve.*2.0-q3.*vn.*2.0,q0.*vd.*2.0-q1.*ve.*2.0+q2.*vn.*2.0,q3.*vd.*2.0+q2.*ve.*2.0+q1.*vn.*2.0,q2.*vd.*2.0-q3.*ve.*2.0-q0.*vn.*2.0,q0.*q3.*-2.0+q1.*q2.*2.0,q0.^2-q1.^2+q2.^2-q3.^2,q0.*q1.*2.0+q2.*q3.*2.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0]; diff --git a/src/modules/ekf2/EKF/matlab/EKF_replay/Filter/calcH_VELZ.m b/src/modules/ekf2/EKF/matlab/EKF_replay/Filter/calcH_VELZ.m deleted file mode 100644 index fb54af4b10..0000000000 --- a/src/modules/ekf2/EKF/matlab/EKF_replay/Filter/calcH_VELZ.m +++ /dev/null @@ -1,8 +0,0 @@ -function H_VELZ = calcH_VELZ(q0,q1,q2,q3,vd,ve,vn) -%CALCH_VELZ -% H_VELZ = CALCH_VELZ(Q0,Q1,Q2,Q3,VD,VE,VN) - -% This function was generated by the Symbolic Math Toolbox version 6.2. -% 29-May-2017 00:16:16 - -H_VELZ = [q0.*vd.*2.0-q1.*ve.*2.0+q2.*vn.*2.0,q1.*vd.*-2.0-q0.*ve.*2.0+q3.*vn.*2.0,q2.*vd.*-2.0+q3.*ve.*2.0+q0.*vn.*2.0,q3.*vd.*2.0+q2.*ve.*2.0+q1.*vn.*2.0,q0.*q2.*2.0+q1.*q3.*2.0,q0.*q1.*-2.0+q2.*q3.*2.0,q0.^2-q1.^2-q2.^2+q3.^2,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0]; diff --git a/src/modules/ekf2/EKF/matlab/EKF_replay/Filter/calcQ24.m b/src/modules/ekf2/EKF/matlab/EKF_replay/Filter/calcQ24.m deleted file mode 100644 index 87c913a50a..0000000000 --- a/src/modules/ekf2/EKF/matlab/EKF_replay/Filter/calcQ24.m +++ /dev/null @@ -1,45 +0,0 @@ -function Q = calcQ24(daxVar,dayVar,dazVar,dvxVar,dvyVar,dvzVar,q0,q1,q2,q3) -%CALCQ24 -% Q = CALCQ24(DAXVAR,DAYVAR,DAZVAR,DVXVAR,DVYVAR,DVZVAR,Q0,Q1,Q2,Q3) - -% This function was generated by the Symbolic Math Toolbox version 6.2. -% 29-May-2017 00:16:11 - -t2 = dayVar.*q2.*q3.*(1.0./4.0); -t3 = t2-daxVar.*q0.*q1.*(1.0./4.0)-dazVar.*q2.*q3.*(1.0./4.0); -t4 = q3.^2; -t5 = q2.^2; -t6 = dazVar.*q1.*q3.*(1.0./4.0); -t7 = t6-daxVar.*q1.*q3.*(1.0./4.0)-dayVar.*q0.*q2.*(1.0./4.0); -t8 = daxVar.*q0.*q3.*(1.0./4.0); -t9 = t8-dayVar.*q0.*q3.*(1.0./4.0)-dazVar.*q1.*q2.*(1.0./4.0); -t10 = q0.^2; -t11 = q1.^2; -t12 = daxVar.*q1.*q2.*(1.0./4.0); -t13 = t12-dayVar.*q1.*q2.*(1.0./4.0)-dazVar.*q0.*q3.*(1.0./4.0); -t14 = dazVar.*q0.*q2.*(1.0./4.0); -t15 = t14-daxVar.*q0.*q2.*(1.0./4.0)-dayVar.*q1.*q3.*(1.0./4.0); -t16 = dayVar.*q0.*q1.*(1.0./4.0); -t17 = t16-daxVar.*q2.*q3.*(1.0./4.0)-dazVar.*q0.*q1.*(1.0./4.0); -t21 = q0.*q3.*2.0; -t22 = q1.*q2.*2.0; -t18 = t21-t22; -t23 = q0.*q2.*2.0; -t24 = q1.*q3.*2.0; -t19 = t23+t24; -t20 = t4+t5-t10-t11; -t25 = q0.*q1.*2.0; -t26 = t21+t22; -t32 = t4-t5-t10+t11; -t27 = dvyVar.*t18.*t32; -t28 = q2.*q3.*2.0; -t29 = t25-t28; -t30 = t4-t5-t10+t11; -t31 = t25+t28; -t33 = t4-t5+t10-t11; -t34 = t23-t24; -t35 = dvxVar.*t34.*(t4+t5-t10-t11); -t36 = dvzVar.*t19.*t33; -t37 = t35+t36-dvyVar.*t18.*t31; -t38 = -dvxVar.*t26.*t34-dvyVar.*t31.*t32-dvzVar.*t29.*t33; -Q = reshape([daxVar.*t11.*(1.0./4.0)+dayVar.*t5.*(1.0./4.0)+dazVar.*t4.*(1.0./4.0),t3,t7,t13,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,t3,daxVar.*t10.*(1.0./4.0)+dayVar.*t4.*(1.0./4.0)+dazVar.*t5.*(1.0./4.0),t9,t15,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,t7,t9,daxVar.*t4.*(1.0./4.0)+dayVar.*t10.*(1.0./4.0)+dazVar.*t11.*(1.0./4.0),t17,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,t13,t15,t17,daxVar.*t5.*(1.0./4.0)+dayVar.*t11.*(1.0./4.0)+dazVar.*t10.*(1.0./4.0),0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,dvxVar.*t20.^2+dvyVar.*t18.^2+dvzVar.*t19.^2,t27-dvxVar.*t20.*t26-dvzVar.*t19.*t29,t37,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,t27-dvzVar.*t19.*(t25-q2.*q3.*2.0)-dvxVar.*t20.*t26,dvxVar.*t26.^2+dvyVar.*t30.^2+dvzVar.*t29.^2,t38,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,t37,t38,dvxVar.*t34.^2+dvyVar.*t31.^2+dvzVar.*t33.^2,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0],[24, 24]); diff --git a/src/modules/ekf2/EKF/matlab/EKF_replay/Filter/find_best_gps_delay.m b/src/modules/ekf2/EKF/matlab/EKF_replay/Filter/find_best_gps_delay.m deleted file mode 100755 index a320153591..0000000000 --- a/src/modules/ekf2/EKF/matlab/EKF_replay/Filter/find_best_gps_delay.m +++ /dev/null @@ -1,50 +0,0 @@ -clear all; -close all; - -% add required paths -addpath('../Common'); - -% load test data -load '../TestData/PX4/baro_data.mat'; -load '../TestData/PX4/gps_data.mat'; -load '../TestData/PX4/imu_data.mat'; -load '../TestData/PX4/mag_data.mat'; - -% set parameters to default values -run('SetParameterDefaults.m'); - -% Loop through range of GPS time delays and capture the RMS velocity -% innovation and corresponding delay each time -for index = 1:1:21 - index - param.fusion.gpsTimeDelay = 0.01*(index - 1); - output = RunFilter(param,imu_data,mag_data,baro_data,gps_data); - gps_delay(index) = param.fusion.gpsTimeDelay; - gps_vel_innov_rms(index) = sqrt(sum(output.innovations.vel_innov(:,1).^2) + sum(output.innovations.vel_innov(:,2).^2))/sqrt(length(output.innovations.vel_innov)); -end - -% plot the innvation level vs time delay -figure; -plot(gps_delay,gps_vel_innov_rms); -grid on; -xlabel('GPS delay (sec)'); -ylabel('RMS velocity innovation (m/s)'); - -% find the smallest value and re-run the replay -param.fusion.gpsTimeDelay = gps_delay(gps_delay == min(gps_delay)) - -% run the filter replay -output = RunFilter(param,imu_data,mag_data,baro_data,gps_data); - -% generate and save output plots -runIdentifier = ' : PX4 data replay '; -folder = strcat('../OutputPlots/PX4'); -PlotData(output,folder,runIdentifier); - -% save output data -folder = '../OutputData/PX4'; -fileName = '../OutputData/PX4/ekf_replay_output.mat'; -if ~exist(folder,'dir') - mkdir(folder); -end -save(fileName,'output'); \ No newline at end of file diff --git a/src/modules/ekf2/EKF/matlab/EKF_replay/Filter/quat_to_euler_error_transfer_matrix.m b/src/modules/ekf2/EKF/matlab/EKF_replay/Filter/quat_to_euler_error_transfer_matrix.m deleted file mode 100644 index ffcb87d8dd..0000000000 --- a/src/modules/ekf2/EKF/matlab/EKF_replay/Filter/quat_to_euler_error_transfer_matrix.m +++ /dev/null @@ -1,38 +0,0 @@ -function error_transfer_matrix = quat_to_euler_error_transfer_matrix(q0,q1,q2,q3) -%QUAT_TO_EULER_ERROR_TRANSFER_MATRIX -% ERROR_TRANSFER_MATRIX = QUAT_TO_EULER_ERROR_TRANSFER_MATRIX(Q0,Q1,Q2,Q3) - -% This function was generated by the Symbolic Math Toolbox version 6.2. -% 13-Jul-2017 08:41:41 - -t8 = q0.*q1.*2.0; -t9 = q2.*q3.*2.0; -t2 = t8+t9; -t4 = q0.^2; -t5 = q1.^2; -t6 = q2.^2; -t7 = q3.^2; -t3 = t4-t5-t6+t7; -t10 = t3.^2; -t11 = t2.^2; -t12 = t10+t11; -t13 = 1.0./t12; -t14 = 1.0./t3; -t15 = 1.0./t3.^2; -t17 = q0.*q2.*2.0; -t18 = q1.*q3.*2.0; -t16 = t17-t18; -t19 = t16.^2; -t20 = -t19+1.0; -t21 = 1.0./sqrt(t20); -t24 = q0.*q3.*2.0; -t25 = q1.*q2.*2.0; -t22 = t24+t25; -t23 = t4+t5-t6-t7; -t26 = t23.^2; -t27 = t22.^2; -t28 = t26+t27; -t29 = 1.0./t28; -t30 = 1.0./t23; -t31 = 1.0./t23.^2; -error_transfer_matrix = reshape([t10.*t13.*(q1.*t14.*2.0-q0.*t2.*t15.*2.0),q2.*t21.*2.0,t26.*t29.*(q3.*t30.*2.0-q0.*t22.*t31.*2.0),t10.*t13.*(q0.*t14.*2.0+q1.*t2.*t15.*2.0),q3.*t21.*-2.0,t26.*t29.*(q2.*t30.*2.0-q1.*t22.*t31.*2.0),t10.*t13.*(q3.*t14.*2.0+q2.*t2.*t15.*2.0),q0.*t21.*2.0,t26.*t29.*(q1.*t30.*2.0+q2.*t22.*t31.*2.0),t10.*t13.*(q2.*t14.*2.0-q3.*t2.*t15.*2.0),q1.*t21.*-2.0,t26.*t29.*(q0.*t30.*2.0+q3.*t22.*t31.*2.0)],[3, 4]); diff --git a/src/modules/ekf2/EKF/matlab/EKF_replay/Filter/replay_apm_data.m b/src/modules/ekf2/EKF/matlab/EKF_replay/Filter/replay_apm_data.m deleted file mode 100755 index 2fe352c5cd..0000000000 --- a/src/modules/ekf2/EKF/matlab/EKF_replay/Filter/replay_apm_data.m +++ /dev/null @@ -1,46 +0,0 @@ -clear all; -close all; - -% add required paths -addpath('../Common'); - -% load compulsory data -load '../TestData/APM/baro_data.mat'; -load '../TestData/APM/gps_data.mat'; -load '../TestData/APM/imu_data.mat'; -load '../TestData/APM/mag_data.mat'; - -% load optional data required for optical flow replay -if exist('../TestData/APM/rng_data.mat','file') && exist('../TestData/APM/flow_data.mat','file') - load '../TestData/APM/rng_data.mat'; - load '../TestData/APM/flow_data.mat'; -else - rng_data = []; - flow_data = []; -end - -% load optional data required for ZED camera replay -if exist('../TestData/APM/viso_data.mat','file') - load '../TestData/APM/viso_data.mat'; -else - viso_data = []; -end - -% load default parameters -run('SetParameters.m'); - -% run the filter replay -output = RunFilter(param,imu_data,mag_data,baro_data,gps_data,rng_data,flow_data,viso_data); - -% generate and save output plots -runIdentifier = ' : APM data replay '; -folder = strcat('../OutputPlots/APM'); -PlotData(output,folder,runIdentifier); - -% save output data -folder = '../OutputData/APM'; -fileName = '../OutputData/APM/ekf_replay_output.mat'; -if ~exist(folder,'dir') - mkdir(folder); -end -save(fileName,'output'); diff --git a/src/modules/ekf2/EKF/matlab/EKF_replay/Filter/replay_px4_data.m b/src/modules/ekf2/EKF/matlab/EKF_replay/Filter/replay_px4_data.m deleted file mode 100755 index ce5a15c31c..0000000000 --- a/src/modules/ekf2/EKF/matlab/EKF_replay/Filter/replay_px4_data.m +++ /dev/null @@ -1,30 +0,0 @@ -clear all; -close all; - -% add required paths -addpath('../Common'); - -% load test data -load '../TestData/PX4/baro_data.mat'; -load '../TestData/PX4/gps_data.mat'; -load '../TestData/PX4/imu_data.mat'; -load '../TestData/PX4/mag_data.mat'; - -% set parameters to default values -run('SetParameters.m'); - -% run the filter replay -output = RunFilter(param,imu_data,mag_data,baro_data,gps_data); - -% generate and save output plots -runIdentifier = ' : PX4 data replay '; -folder = strcat('../OutputPlots/PX4'); -PlotData(output,folder,runIdentifier); - -% save output data -folder = '../OutputData/PX4'; -fileName = '../OutputData/PX4/ekf_replay_output.mat'; -if ~exist(folder,'dir') - mkdir(folder); -end -save(fileName,'output'); \ No newline at end of file diff --git a/src/modules/ekf2/EKF/matlab/EKF_replay/Filter/replay_px4_optflow_data.m b/src/modules/ekf2/EKF/matlab/EKF_replay/Filter/replay_px4_optflow_data.m deleted file mode 100644 index a054210128..0000000000 --- a/src/modules/ekf2/EKF/matlab/EKF_replay/Filter/replay_px4_optflow_data.m +++ /dev/null @@ -1,32 +0,0 @@ -clear all; -close all; - -% add required paths -addpath('../Common'); - -% load test data -load '../TestData/PX4_optical_flow/baro_data.mat'; -load '../TestData/PX4_optical_flow/gps_data.mat'; -load '../TestData/PX4_optical_flow/imu_data.mat'; -load '../TestData/PX4_optical_flow/mag_data.mat'; -load '../TestData/PX4_optical_flow/rng_data.mat'; -load '../TestData/PX4_optical_flow/flow_data.mat'; - -% set parameters to default values -run('SetParameterDefaults.m'); - -% run the filter replay -output = RunFilter(param,imu_data,mag_data,baro_data,gps_data,rng_data,flow_data); - -% generate and save output plots -runIdentifier = ' : PX4 optical flow replay '; -folder = strcat('../OutputPlots/PX4_optical_flow'); -PlotData(output,folder,runIdentifier); - -% save output data -folder = '../OutputData/PX4_optical_flow'; -fileName = '../OutputData/PX4_optical_flow/ekf_replay_output.mat'; -if ~exist(folder,'dir') - mkdir(folder); -end -save(fileName,'output'); \ No newline at end of file diff --git a/src/modules/ekf2/EKF/matlab/EKF_replay/Filter/transfer_matrix.m b/src/modules/ekf2/EKF/matlab/EKF_replay/Filter/transfer_matrix.m deleted file mode 100644 index 355102f4e9..0000000000 --- a/src/modules/ekf2/EKF/matlab/EKF_replay/Filter/transfer_matrix.m +++ /dev/null @@ -1,13 +0,0 @@ -function T_MAG = transfer_matrix(magE,magN) -%TRANSFER_MATRIX -% T_MAG = TRANSFER_MATRIX(MAGE,MAGN) - -% This function was generated by the Symbolic Math Toolbox version 6.2. -% 29-May-2017 00:16:16 - -t2 = 1.0./magN.^2; -t3 = magE.^2; -t4 = t2.*t3; -t5 = t4+1.0; -t6 = 1.0./t5; -T_MAG = [-magE.*t2.*t6,t6./magN]; diff --git a/src/modules/ekf2/EKF/matlab/EKF_replay/SensorCalibration/allan.m b/src/modules/ekf2/EKF/matlab/EKF_replay/SensorCalibration/allan.m deleted file mode 100644 index c8c84da801..0000000000 --- a/src/modules/ekf2/EKF/matlab/EKF_replay/SensorCalibration/allan.m +++ /dev/null @@ -1,19 +0,0 @@ -function [T,sigma] = allan(omega,fs,pts) -[N,M] = size(omega); % figure out how big the output data set is -n = 2.^(0:floor(log2(N/2)))'; % determine largest bin size -maxN = n(end); -endLogInc = log10(maxN); -m = unique(ceil(logspace(0,endLogInc,pts)))'; % create log spaced vector average factor -t0 = 1/fs; -T = m*t0; -theta = cumsum(omega)/fs; -sigma2 = zeros(length(T),M); -for i=1:length(m) - % t0 = sample interval -% T = length of time for each cluster -% integration of samples over time to obtain output angle ? - % array of dimensions (cluster periods) X (#variables) - % loop over the various cluster sizes - % implements the summation in the AV equation -sigma2 = sigma2./repmat((2*T.^2.*(N-2*m)),1,M); -sigma = sqrt(sigma2) \ No newline at end of file diff --git a/src/modules/ekf2/EKF/matlab/EKF_replay/SensorCalibration/compare_mag_calibration.m b/src/modules/ekf2/EKF/matlab/EKF_replay/SensorCalibration/compare_mag_calibration.m deleted file mode 100644 index 3eba488511..0000000000 --- a/src/modules/ekf2/EKF/matlab/EKF_replay/SensorCalibration/compare_mag_calibration.m +++ /dev/null @@ -1,189 +0,0 @@ -% test ellipsoid sphere fitting algorithms -% -% http://www.st.com/content/ccc/resource/technical/document/design_tip/group0/a2/98/f5/d4/9c/48/4a/d1/DM00286302/files/DM00286302.pdf/jcr:content/translations/en.DM00286302.pdf -% - -%% load log data -clear all; -close all; - -% uncomment these lines if using legacy .px4log format -%load sysvector.mat; -%mag_meas = [sysvector.IMU_MagX';sysvector.IMU_MagY';sysvector.IMU_MagZ']; - -% uncomment these lines if using data imported from ulog format -%load sysdata.mat; -%mag_meas = [magnetometer_ga0';magnetometer_ga1';magnetometer_ga2']; - -% thin data points to use data every 5 deg -delta_angle_lim = 5* pi/180; -counter = 1; - -angle = 0; -last_angle = 0; -for i = 2:length(data1.IMU.Tsec) - ang_rate = 0.5 * (sqrt(data1.IMU.GyroX(i)^2 + data1.IMU.GyroY(i)^2 + data1.IMU.GyroZ(i)^2) + ... - sqrt(data1.IMU.GyroX(i-1)^2 + data1.IMU.GyroY(i-1)^2 + data1.IMU.GyroZ(i-1)^2)); - dt = data1.IMU.Tsec(i) - data1.IMU.Tsec(i-1); - angle = angle + ang_rate * dt; - if ((angle - last_angle) > delta_angle_lim) - mag_meas(:,counter) = [data1.IMU.MagX(i);data1.IMU.MagY(i);data1.IMU.MagZ(i)]; - counter = counter + 1; - last_angle = angle; - end -end - -angle = 0; -last_angle = 0; -for i = 2:length(data2.IMU.Tsec) - ang_rate = 0.5 * (sqrt(data2.IMU.GyroX(i)^2 + data2.IMU.GyroY(i)^2 + data2.IMU.GyroZ(i)^2) + ... - sqrt(data2.IMU.GyroX(i-1)^2 + data2.IMU.GyroY(i-1)^2 + data2.IMU.GyroZ(i-1)^2)); - dt = data2.IMU.Tsec(i) - data2.IMU.Tsec(i-1); - angle = angle + ang_rate * dt; - if ((angle - last_angle) > delta_angle_lim) - mag_meas(:,counter) = [data2.IMU.MagX(i);data2.IMU.MagY(i);data2.IMU.MagZ(i)]; - counter = counter + 1; - last_angle = angle; - end - -end - -angle = 0; -last_angle = 0; -for i = 2:length(data3.IMU.Tsec) - ang_rate = 0.5 * (sqrt(data3.IMU.GyroX(i)^2 + data3.IMU.GyroY(i)^2 + data3.IMU.GyroZ(i)^2) + ... - sqrt(data3.IMU.GyroX(i-1)^2 + data3.IMU.GyroY(i-1)^2 + data3.IMU.GyroZ(i-1)^2)); - dt = data3.IMU.Tsec(i) - data3.IMU.Tsec(i-1); - angle = angle + ang_rate * dt; - if ((angle - last_angle) > delta_angle_lim) - mag_meas(:,counter) = [data3.IMU.MagX(i);data3.IMU.MagY(i);data3.IMU.MagZ(i)]; - counter = counter + 1; - last_angle = angle; - end - -end - - -%% fit a sphere and determine the fit quality -[offset,gain,rotation]=ellipsoid_fit(mag_meas',5); - -% correct the data -mag_corrected_5 = zeros(size(mag_meas)); -rotation_correction = inv(rotation); % we apply the inverse of the original rotation -scale_correction = 1./gain; -scale_correction = scale_correction ./ mean(scale_correction); -mag_strength = zeros(length(mag_meas),1); -for i = 1:length(mag_meas) - % subtract the offsets - mag_corrected_5(:,i) = mag_meas(:,i) - offset; - - % correct the rotation - mag_corrected_5(:,i) = rotation_correction * mag_corrected_5(:,i); - - % correct the scale factor - mag_corrected_5(:,i) = mag_corrected_5(:,i) .* scale_correction; - - % calculate the residual - mag_strength(i) = sqrt(dot(mag_corrected_5(:,i),mag_corrected_5(:,i))); - -end - -% calculate the fit residual for fit option 5 -fit_residual_5 = mag_strength - mean(mag_strength); - -%% fit a un-rotated ellipsoid and determine the fit quality -[offset,gain,rotation]=ellipsoid_fit(mag_meas',1); - -% correct the data -mag_corrected_1 = zeros(size(mag_meas)); -rotation_correction = inv(rotation); % we apply the inverse of the original rotation -scale_correction = 1./gain; -scale_correction = scale_correction ./ mean(scale_correction); -mag_strength = zeros(length(mag_meas),1); -angle_change_1 = zeros(length(mag_meas),1); -for i = 1:length(mag_meas) - % subtract the offsets - mag_corrected_1(:,i) = mag_meas(:,i) - offset; - - % correct the rotation - mag_corrected_1(:,i) = rotation_correction * mag_corrected_1(:,i); - - % correct the scale factor - mag_corrected_1(:,i) = mag_corrected_1(:,i) .* scale_correction; - - % calculate the residual - mag_strength(i) = sqrt(dot(mag_corrected_1(:,i),mag_corrected_1(:,i))); - - % calculate the angular change due to the fit - angle_change_1(i) = atan2(norm(cross(mag_corrected_1(:,i),mag_meas(:,i))),dot(mag_corrected_1(:,i),mag_meas(:,i))); - -end - -% calculate the fit residual for fit option 1 -fit_residual_1 = mag_strength - mean(mag_strength); - -%% fit a rotated ellipsoid and check the fit quality -[offset,gain,rotation]=ellipsoid_fit(mag_meas',0); - -% correct the data -mag_corrected_0 = zeros(size(mag_meas)); -rotation_correction = inv(rotation); % we apply the inverse of the original rotation -scale_correction = 1./gain; -scale_correction = scale_correction ./ mean(scale_correction); -mag_strength = zeros(length(mag_meas),1); -angle_change_0 = zeros(length(mag_meas),1); -for i = 1:length(mag_meas) - % subtract the offsets - mag_corrected_0(:,i) = mag_meas(:,i) - offset; - - % correct the rotation - mag_corrected_0(:,i) = rotation_correction * mag_corrected_0(:,i); - - % correct the scale factor - mag_corrected_0(:,i) = mag_corrected_0(:,i) .* scale_correction; - - % calculate the residual - mag_strength(i) = sqrt(dot(mag_corrected_0(:,i),mag_corrected_0(:,i))); - - % calculate the angular change due to the fit - angle_change_0(i) = atan2(norm(cross(mag_corrected_0(:,i),mag_meas(:,i))),dot(mag_corrected_0(:,i),mag_meas(:,i))); - -end - -% calculate the fit residual for fit option 0 -fit_residual_0 = mag_strength - mean(mag_strength); - -%% calculate the residual for uncorrected data -for i = 1:length(mag_meas) - mag_strength(i) = sqrt(dot(mag_meas(:,i),mag_meas(:,i))); -end -uncorrected_residual = mag_strength - mean(mag_strength); - -%% plot the fit residuals -plot(uncorrected_residual,'k+'); -hold on; -plot(fit_residual_5,'r+'); -plot(fit_residual_1,'b+'); -plot(fit_residual_0,'g+'); -hold off; -grid on; -title('mag fit comparison'); -xlabel('measurement index'); -ylabel('fit residual (Gauss)'); -legend('uncorrected','sphere','non-rotated ellipse','rotated ellipse'); - -%% plot the data points in 3D -figure; -plot3(mag_meas(1,:),mag_meas(2,:),mag_meas(3,:),' .');hold on;plot3(mag_corrected_1(1,:),mag_corrected_1(2,:),mag_corrected_1(3,:),'r.'); -hold off;grid on;axis equal; -xlabel('x (Gauss)'); -xlabel('y (Gauss)'); -xlabel('z (Gauss)'); -legend('uncorrected','unrotated ellipse'); - -%% calculate and plot the angular error -figure; -plot(angle_change_1*(180/pi),'b+'); -title('angle change after un-rotated ellipse fit'); -xlabel('measurement index'); -ylabel('angle change magnitude (deg)'); diff --git a/src/modules/ekf2/EKF/matlab/EKF_replay/SensorCalibration/ellipsoid_fit.m b/src/modules/ekf2/EKF/matlab/EKF_replay/SensorCalibration/ellipsoid_fit.m deleted file mode 100644 index 73819b21e2..0000000000 --- a/src/modules/ekf2/EKF/matlab/EKF_replay/SensorCalibration/ellipsoid_fit.m +++ /dev/null @@ -1,61 +0,0 @@ -function [ofs,gain,rotM]=ellipsoid_fit(XYZ,varargin) - -% Fit an (non)rotated ellipsoid or sphere to a set of xyz data points - -% XYZ: N(rows) x 3(cols), matrix of N data points (x,y,z) - -% optional flag f, default to 0 (fitting of rotated ellipsoid) - -x=XYZ(:,1); y=XYZ(:,2); z=XYZ(:,3); if nargin>1, f=varargin{1}; else f=0; end; - -if f==0, D=[x.*x, y.*y, z.*z, 2*x.*y,2*x.*z,2*y.*z, 2*x,2*y,2*z]; % any axes (rotated ellipsoid) - -elseif f==1, D=[x.*x, y.*y, z.*z, 2*x,2*y,2*z]; % XYZ axes (non-rotated ellipsoid) - -elseif f==2, D=[x.*x+y.*y, z.*z, 2*x,2*y,2*z]; % and radius x=y - -elseif f==3, D=[x.*x+z.*z, y.*y, 2*x,2*y,2*z]; % and radius x=z - -elseif f==4, D=[y.*y+z.*z, x.*x, 2*x,2*y,2*z]; % and radius y=z - -elseif f==5, D=[x.*x+y.*y+z.*z, 2*x,2*y,2*z]; % and radius x=y=z (sphere) - -end; - -v = (D'*D)\(D'*ones(length(x),1)); % least square fitting - -if f==0, % rotated ellipsoid - - A = [ v(1) v(4) v(5) v(7); v(4) v(2) v(6) v(8); v(5) v(6) v(3) v(9); v(7) v(8) v(9) -1 ]; - - ofs=-A(1:3,1:3)\[v(7);v(8);v(9)]; % offset is center of ellipsoid - - Tmtx=eye(4); Tmtx(4,1:3)=ofs'; AT=Tmtx*A*Tmtx'; % ellipsoid translated to (0,0,0) - - [rotM ev]=eig(AT(1:3,1:3)/-AT(4,4)); % eigenvectors (rotation) and eigenvalues (gain) - - gain=sqrt(1./diag(ev)); % gain is radius of the ellipsoid - -else % non-rotated ellipsoid - - if f==1, v = [ v(1) v(2) v(3) 0 0 0 v(4) v(5) v(6) ]; - - elseif f==2, v = [ v(1) v(1) v(2) 0 0 0 v(3) v(4) v(5) ]; - - elseif f==3, v = [ v(1) v(2) v(1) 0 0 0 v(3) v(4) v(5) ]; - - elseif f==4, v = [ v(2) v(1) v(1) 0 0 0 v(3) v(4) v(5) ]; - - elseif f==5, v = [ v(1) v(1) v(1) 0 0 0 v(2) v(3) v(4) ]; % sphere - - end; - - ofs=-(v(1:3).\v(7:9))'; % offset is center of ellipsoid - - rotM=eye(3); % eigenvectors (rotation), identity = no rotation - - g=1+(v(7)^2/v(1)+v(8)^2/v(2)+v(9)^2/v(3)); - - gain=(sqrt(g./v(1:3)))'; % find radii of the ellipsoid (scale) - -end; diff --git a/src/modules/ekf2/EKF/matlab/EKF_replay/px4_replay_import.m b/src/modules/ekf2/EKF/matlab/EKF_replay/px4_replay_import.m deleted file mode 100644 index f5e1c3c2a9..0000000000 --- a/src/modules/ekf2/EKF/matlab/EKF_replay/px4_replay_import.m +++ /dev/null @@ -1,387 +0,0 @@ -%% PX4 replay: import sensors CSV -% the following variables must be set beforehand! -if ~exist('sensors_file','var') - error('sensors_file missing'); -end -if ~exist('air_data_file','var') - error('air_data_file missing'); -end -if ~exist('magnetometer_file','var') - error('magnetometer_file missing'); -end -if ~exist('gps_file','var') - error('gps_file missing'); -end - -if ~exist('attitude_file','var') - disp('INFO no attitude_file set, all ground truth data will be skipped'); -end -if ~exist('localpos_file','var') - disp('INFO no localpos_file set, all ground truth data will be skipped'); -end -if ~exist('globalpos_file','var') - disp('INFO no globalpos_file set, all ground truth data will be skipped'); -end - -if ~exist('actctrl_file','var') - disp('INFO no actctrl_file set, all actuator data will be skipped'); -end -if ~exist('actout_file','var') - disp('INFO no actout_file set, all actuator data will be skipped'); -end - -%% ------ SECTION 1: IMU, Baro, Mag ------ - -%% Import IMU data from text file -opts = delimitedTextImportOptions("NumVariables", 10); -opts.DataLines = [2, Inf]; -opts.Delimiter = ","; - -% Specify column names and types -opts.VariableNames = ["timestamp", "gyro_rad0", "gyro_rad1", "gyro_rad2", "gyro_integral_dt", "accelerometer_timestamp_relative", "accelerometer_m_s20", "accelerometer_m_s21", "accelerometer_m_s22", "accelerometer_integral_dt"]; -opts.VariableTypes = ["double", "double", "double", "double", "double", "double", "double", "double", "double", "double"]; -opts.ExtraColumnsRule = "ignore"; -opts.EmptyLineRule = "read"; - -% Import the data -tbl = readtable(sensors_file, opts); - -% Convert to output type -timestamp = tbl.timestamp; -gyro_rad0 = tbl.gyro_rad0; -gyro_rad1 = tbl.gyro_rad1; -gyro_rad2 = tbl.gyro_rad2; -gyro_integral_dt = tbl.gyro_integral_dt; -accelerometer_timestamp_relative = tbl.accelerometer_timestamp_relative; -accelerometer_m_s20 = tbl.accelerometer_m_s20; -accelerometer_m_s21 = tbl.accelerometer_m_s21; -accelerometer_m_s22 = tbl.accelerometer_m_s22; -accelerometer_integral_dt = tbl.accelerometer_integral_dt; - -clear opts tbl - -%% Import Baro data from text file -opts = delimitedTextImportOptions("NumVariables", 5); -opts.DataLines = [2, Inf]; -opts.Delimiter = ","; - -% Specify column names and types -opts.VariableNames = ["timestamp_baro", "baro_alt_meter", "baro_temp_celcius", "baro_pressure_pa", "rho"]; -opts.VariableTypes = ["double", "double", "double", "double", "double"]; -opts.ExtraColumnsRule = "ignore"; -opts.EmptyLineRule = "read"; - -% Import the data -tbl = readtable(air_data_file, opts); - -% Convert to output type -timestamp_baro = tbl.timestamp_baro; -baro_alt_meter = tbl.baro_alt_meter; -baro_temp_celcius = tbl.baro_temp_celcius; -baro_pressure_pa = tbl.baro_pressure_pa; -rho = tbl.rho; - -clear opts tbl - -%% Import Mag data from text file -opts = delimitedTextImportOptions("NumVariables", 4); -opts.DataLines = [2, Inf]; -opts.Delimiter = ","; - -% Specify column names and types -opts.VariableNames = ["timestamp_mag", "magnetometer_ga0", "magnetometer_ga1", "magnetometer_ga2"]; -opts.VariableTypes = ["double", "double", "double", "double"]; -opts.ExtraColumnsRule = "ignore"; -opts.EmptyLineRule = "read"; - -% Import the data -tbl = readtable(magnetometer_file, opts); - -% Convert to output type -timestamp_mag = tbl.timestamp_mag; -magnetometer_ga0 = tbl.magnetometer_ga0; -magnetometer_ga1 = tbl.magnetometer_ga1; -magnetometer_ga2 = tbl.magnetometer_ga2; - -clear opts tbl - -%% Run conversion script for IMU, Baro and Mag -cd Common/; -convert_px4_sensor_combined_csv_data; -cd ../; - - -%% ------ SECTION 2: GPS ------ - -%% Import data from text file -opts = delimitedTextImportOptions("NumVariables", 25); -opts.DataLines = [2, Inf]; -opts.Delimiter = ","; - -% Specify column names and types -opts.VariableNames = ["timestamp", "time_utc_usec", "lat", "lon", "alt", "alt_ellipsoid", "s_variance_m_s", "c_variance_rad", "eph", "epv", "hdop", "vdop", "noise_per_ms", "jamming_indicator", "vel_m_s", "vel_n_m_s", "vel_e_m_s", "vel_d_m_s", "cog_rad", "timestamp_time_relative", "heading", "heading_offset", "fix_type", "vel_ned_valid", "satellites_used"]; -opts.VariableTypes = ["double", "double", "double", "double", "double", "double", "double", "double", "double", "double", "double", "double", "double", "double", "double", "double", "double", "double", "double", "double", "double", "double", "double", "double", "double"]; -opts.ExtraColumnsRule = "ignore"; -opts.EmptyLineRule = "read"; - -% Import the data -tbl = readtable(gps_file, opts); - -% Convert to output type -timestamp = tbl.timestamp; -time_utc_usec = tbl.time_utc_usec; -lat = tbl.lat; -lon = tbl.lon; -alt = tbl.alt; -alt_ellipsoid = tbl.alt_ellipsoid; -s_variance_m_s = tbl.s_variance_m_s; -c_variance_rad = tbl.c_variance_rad; -eph = tbl.eph; -epv = tbl.epv; -hdop = tbl.hdop; -vdop = tbl.vdop; -noise_per_ms = tbl.noise_per_ms; -jamming_indicator = tbl.jamming_indicator; -vel_m_s = tbl.vel_m_s; -vel_n_m_s = tbl.vel_n_m_s; -vel_e_m_s = tbl.vel_e_m_s; -vel_d_m_s = tbl.vel_d_m_s; -cog_rad = tbl.cog_rad; -timestamp_time_relative = tbl.timestamp_time_relative; -heading = tbl.heading; -heading_offset = tbl.heading_offset; -fix_type = tbl.fix_type; -vel_ned_valid = tbl.vel_ned_valid; -satellites_used = tbl.satellites_used; - -clear opts tbl - - -%% Run conversion script for GPS -cd Common/; -convert_px4_vehicle_gps_position_csv; -cd ../; - - -%% ------ SECTION 3: Ground Truth Data (STIL only, optional) ------ - -if exist('attitude_file','var') && exist('localpos_file','var') && exist('globalpos_file','var') - -%- Import Attitude data from text file -opts = delimitedTextImportOptions("NumVariables", 13); -opts.DataLines = [2, Inf]; -opts.Delimiter = ","; - -% Specify column names and types -opts.VariableNames = ["timestamp", "rollspeed", "pitchspeed", "yawspeed", "q0", "q1", "q2", "q3", "delta_q_reset0", "delta_q_reset1", "delta_q_reset2", "delta_q_reset3", "quat_reset_counter"]; -opts.VariableTypes = ["double", "double", "double", "double", "double", "double", "double", "double", "double", "double", "double", "double", "double"]; -opts.ExtraColumnsRule = "ignore"; -opts.EmptyLineRule = "read"; - -% Import the data -tbl = readtable(attitude_file, opts); - -% Convert to output type -timestamp_att = tbl.timestamp; -rollspeed = tbl.rollspeed; -pitchspeed = tbl.pitchspeed; -yawspeed = tbl.yawspeed; -q0 = tbl.q0; -q1 = tbl.q1; -q2 = tbl.q2; -q3 = tbl.q3; -% delta_q_reset0 = tbl.delta_q_reset0; -% delta_q_reset1 = tbl.delta_q_reset1; -% delta_q_reset2 = tbl.delta_q_reset2; -% delta_q_reset3 = tbl.delta_q_reset3; -% quat_reset_counter = tbl.quat_reset_counter; - -clear opts tbl - - -%- Import Global Position data from text file -opts = delimitedTextImportOptions("NumVariables", 17); -opts.DataLines = [2, Inf]; -opts.Delimiter = ","; - -% Specify column names and types -opts.VariableNames = ["timestamp", "lat", "lon", "alt", "alt_ellipsoid", "delta_alt", "vel_n", "vel_e", "vel_d", "yaw", "eph", "epv", "terrain_alt", "lat_lon_reset_counter", "alt_reset_counter", "terrain_alt_valid", "dead_reckoning"]; -opts.VariableTypes = ["double", "double", "double", "double", "double", "double", "double", "double", "double", "double", "double", "double", "double", "double", "double", "double", "double"]; -opts.ExtraColumnsRule = "ignore"; -opts.EmptyLineRule = "read"; - -% Import the data -tbl = readtable(globalpos_file, opts); - -% Convert to output type -timestamp_gpos = tbl.timestamp; -gpos_lat = tbl.lat; -gpos_lon = tbl.lon; -gpos_alt = tbl.alt; -% gpos_alt_ellipsoid = tbl.alt_ellipsoid; -% gpos_delta_alt = tbl.delta_alt; -gpos_vel_n = tbl.vel_n; -gpos_vel_e = tbl.vel_e; -gpos_vel_d = tbl.vel_d; -% gpos_yaw = tbl.yaw; -% gpos_eph = tbl.eph; -% gpos_epv = tbl.epv; -% gpos_terrain_alt = tbl.terrain_alt; -% gpos_lat_lon_reset_counter = tbl.lat_lon_reset_counter; -% gpos_alt_reset_counter = tbl.alt_reset_counter; -% gpos_terrain_alt_valid = tbl.terrain_alt_valid; -% gpos_dead_reckoning = tbl.dead_reckoning; - -% Clear temporary variables -clear opts tbl - - -%- Import Local Position data from text file -opts = delimitedTextImportOptions("NumVariables", 43); -opts.DataLines = [2, Inf]; -opts.Delimiter = ","; - -% Specify column names and types -opts.VariableNames = ["timestamp", "ref_timestamp", "ref_lat", "ref_lon", "x", "y", "z", "delta_xy0", "delta_xy1", "delta_z", "vx", "vy", "vz", "z_deriv", "delta_vxy0", "delta_vxy1", "delta_vz", "ax", "ay", "az", "yaw", "ref_alt", "dist_bottom", "dist_bottom_rate", "eph", "epv", "evh", "evv", "vxy_max", "vz_max", "hagl_min", "hagl_max", "xy_valid", "z_valid", "v_xy_valid", "v_z_valid", "xy_reset_counter", "z_reset_counter", "vxy_reset_counter", "vz_reset_counter", "xy_global", "z_global", "dist_bottom_valid"]; -opts.VariableTypes = ["double", "double", "double", "double", "double", "double", "double", "double", "double", "double", "double", "double", "double", "double", "double", "double", "double", "double", "double", "double", "double", "double", "double", "double", "double", "double", "double", "double", "double", "double", "double", "double", "double", "double", "double", "double", "double", "double", "double", "double", "double", "double", "double"]; -opts.ExtraColumnsRule = "ignore"; -opts.EmptyLineRule = "read"; - -% Import the data -tbl = readtable(localpos_file, opts); - -% Convert to output type -timestamp_lpos = tbl.timestamp; -% lpos_ref_timestamp = tbl.ref_timestamp; -lpos_ref_lat = tbl.ref_lat; -lpos_ref_lon = tbl.ref_lon; -lpos_x = tbl.x; -lpos_y = tbl.y; -lpos_z = tbl.z; -% lpos_delta_xy0 = tbl.delta_xy0; -% lpos_delta_xy1 = tbl.delta_xy1; -% lpos_delta_z = tbl.delta_z; -lpos_vx = tbl.vx; -lpos_vy = tbl.vy; -lpos_vz = tbl.vz; -% lpos_z_deriv = tbl.z_deriv; -% lpos_delta_vxy0 = tbl.delta_vxy0; -% lpos_delta_vxy1 = tbl.delta_vxy1; -% lpos_delta_vz = tbl.delta_vz; -% lpos_ax = tbl.ax; -% lpos_ay = tbl.ay; -% lpos_az = tbl.az; -lpos_yaw = tbl.yaw; -% ref_alt = tbl.ref_alt; -% dist_bottom = tbl.dist_bottom; -% dist_bottom_rate = tbl.dist_bottom_rate; -% eph = tbl.eph; -% epv = tbl.epv; -% evh = tbl.evh; -% evv = tbl.evv; -% vxy_max = tbl.vxy_max; -% vz_max = tbl.vz_max; -% hagl_min = tbl.hagl_min; -% hagl_max = tbl.hagl_max; -% xy_valid = tbl.xy_valid; -% z_valid = tbl.z_valid; -% v_xy_valid = tbl.v_xy_valid; -% v_z_valid = tbl.v_z_valid; -% xy_reset_counter = tbl.xy_reset_counter; -% z_reset_counter = tbl.z_reset_counter; -% vxy_reset_counter = tbl.vxy_reset_counter; -% vz_reset_counter = tbl.vz_reset_counter; -% xy_global = tbl.xy_global; -% z_global = tbl.z_global; -% dist_bottom_valid = tbl.dist_bottom_valid; - -% Clear temporary variables -clear opts tbl - - -%- Run conversion script for GPS -cd Common/; -convert_px4_groundtruth_csv_data; -cd ../; - -end - - -%% ------ SECTION 4: Actuator Controls (optional) ------ - -if exist('actctrl_file','var') && exist('actout_file','var') - -%- Import Actuator Control data from text file -opts = delimitedTextImportOptions("NumVariables", 10); -opts.DataLines = [2, Inf]; -opts.Delimiter = ","; - -% Specify column names and types -opts.VariableNames = ["timestamp", "timestamp_sample", "control0", "control1", "control2", "control3", "control4", "control5", "control6", "control7"]; -opts.VariableTypes = ["double", "double", "double", "double", "double", "double", "double", "double", "double", "double"]; -opts.ExtraColumnsRule = "ignore"; -opts.EmptyLineRule = "read"; - -% Import the data -tbl = readtable(actctrl_file, opts); - -% Convert to output type -timestamp_actctrl = tbl.timestamp; -%timestamp_sample = tbl.timestamp_sample; -control0 = tbl.control0; -control1 = tbl.control1; -control2 = tbl.control2; -control3 = tbl.control3; -% control4 = tbl.control4; -% control5 = tbl.control5; -% control6 = tbl.control6; -% control7 = tbl.control7; - -% Clear temporary variables -clear opts tbl - - -%- Import Actuator Output data from text file -opts = delimitedTextImportOptions("NumVariables", 18); -opts.DataLines = [2, Inf]; -opts.Delimiter = ","; - -% Specify column names and types -opts.VariableNames = ["timestamp", "noutputs", "output0", "output1", "output2", "output3", "output4", "output5", "output6", "output7", "output8", "output9", "output10", "output11", "output12", "output13", "output14", "output15"]; -opts.VariableTypes = ["double", "double", "double", "double", "double", "double", "double", "double", "double", "double", "double", "double", "double", "double", "double", "double", "double", "double"]; -opts.ExtraColumnsRule = "ignore"; -opts.EmptyLineRule = "read"; - -% Import the data -tbl = readtable(actout_file, opts); - -% Convert to output type -timestamp_actout = tbl.timestamp; -% noutputs = tbl.noutputs; -output0 = tbl.output0; -output1 = tbl.output1; -output2 = tbl.output2; -output3 = tbl.output3; -% output4 = tbl.output4; -% output5 = tbl.output5; -% output6 = tbl.output6; -% output7 = tbl.output7; -% output8 = tbl.output8; -% output9 = tbl.output9; -% output10 = tbl.output10; -% output11 = tbl.output11; -% output12 = tbl.output12; -% output13 = tbl.output13; -% output14 = tbl.output14; -% output15 = tbl.output15; - -% Clear temporary variables -clear opts tbl - - -%- Run conversion script for GPS -cd Common/; -convert_px4_actuators_csv_data; -cd ../; - -end diff --git a/src/modules/ekf2/EKF/matlab/EKF_replay/readme.txt b/src/modules/ekf2/EKF/matlab/EKF_replay/readme.txt deleted file mode 100644 index 0a2bd490b2..0000000000 --- a/src/modules/ekf2/EKF/matlab/EKF_replay/readme.txt +++ /dev/null @@ -1,108 +0,0 @@ -Instructions for running the EKF replay - -1) Ensure the ‘EKF_replay’ directory is in a location you have full read and write access - - -2) Create a ‘TestData’ sub-directory under the ‘EKF_replay’ directory - -A sample dataset can be downloaded here: https://drive.google.com/file/d/0By4v2BuLAaCfSW9fWl9aSWNGbGs/view?usp=sharing - - -3a) If replaying APM data: - -Collect data with LOG_REPLAY = 1 and LOG_DISARMED = 1. -Convert data to a .mat file using the MissionPlanner ‘Create Matlab File’ option under the DataFlash Logs tab. -Convert .mat file to the required data format using the convert_apm_data.m script file. This will generate the following data files: - -imu_data.mat -baro_data.mat -gps_data.mat -mag_data.mat - -and optionally - -rng_data.mat -flow_data.mat -viso_data.mat - -Note: If the rangefinder, optical flow or ZED camera odometer data are not present in the log, then the corresponding sections in the convert_apm_data.m script file will need to be commented out. - -Copy the generated .mat files into the /EKF_replay/TestData/APM directory. - - -3b) If replaying PX4 data: - -Collect data with EK2_REC_RPL = 1 -Convert the .ulg log file to .csv files using the PX4/pyulog python script https://github.com/PX4/pyulog/blob/master/pyulog/ulog2csv.py - -Make this directory your current MARLAB working directory and fill these variables with the paths to the CSV files: -- sensors_file: path to *_sensor_combined_0.csv -- air_data_file: path to *vehicle_air_data_0.csv -- magnetometer_file: path to *_vehicle_magetometer_0.csv -- gps_file: path to *_vehicle_gps_position_0.csv - -If a simulation was used, ground truth data can be converted as well: -- attitude_file: path to *_vehicle_attitude_groundtruth_0.csv -- localpos_file: path to *_vehicle_local_position_groundtruth_0.csv -- globalpos_file: path to *_vehicle_global_position_groundtruth_0.csv - -The actuator controls can also be converted: -- actctrl_file: path to *_actuator_controls_0_0.csv -- actout_file: path to *_actuator_outputs_0.csv - -When executing .../EKF_replay/px4_replay_import.m, the CSV files will be automatically loaded and converted using these scripts: -- .../EKF_replay/Common/convert_px4_sensor_combined_csv_data.m - * uses imported data from: - + sensors_file - + air_data_file - + magnetometer_file - * generates: - + imu_data.mat - + baro_data.mat - + mag_data.mat -- .../EKF_replay/Common/convert_px4_vehicle_gps_position_csv - * uses imported data from: - + gps_file - * generates: - + gps_data.mat -- .../EKF_replay/Common/convert_px4_groundtruth_csv_data - * uses imported data from: - + attitude_file - + localpos_file - + globalpos_file - * generates: - + attitude_data.mat - + localpos_data.mat - + globalpos_data.mat -- .../EKF_replay/Common/convert_px4_actuators_csv_data - * uses imported data from: - + actctrl_file - + actout_file - * generates: - + actctrl_data.mat - + actout_data.mat - -If you have an optical flow and range finder sensor fitted, they must be imported and converted manually: - -Import the .csv file containing the optical_flow_0 data into the matlab workspace and process it using …/EKF_replay/Common/convert_px4_optical_flow_csv_data.m. -Import the .csv file containing the distance_sensor_0 data into the matlab workspace and process it using …/EKF_replay/Common/convert_px4_distance_sensor_csv_data.m. -This will generate the following data files: - -flow_data.mat -rng_data.mat - - -Finally copy the generated .mat files into the /EKF_replay/TestData/PX4 directory. - - -4) Make ‘…/EKF_replay/Filter’ the working directory. - - -5) Execute ‘SetParameterDefaults’ at the command prompt to load the default filter tuning parameter struct ‘param’ into the workspace. The defaults have been set to provide robust estimation across the entire data set, not optimised for accuracy. - - -6) Replay the data by running either the replay_apm_data.m, replay_px4_data.m or if you have px4 optical flow data, the replay_px4_optflow_data.m script file. - -Output plots are saved as .png files in the ‘…/EKF_replay/OutputPlots/‘ directory. - -Output data is written to the Matlab workspace in the ‘output’ struct. diff --git a/src/modules/ekf2/EKF/matlab/README.md b/src/modules/ekf2/EKF/matlab/README.md deleted file mode 100644 index c2461c7548..0000000000 --- a/src/modules/ekf2/EKF/matlab/README.md +++ /dev/null @@ -1,3 +0,0 @@ -# MATLAB Derivations - -This folder contains the derivations and test suites in Matlab for ECL components. diff --git a/src/modules/ekf2/EKF/matlab/analysis/estimatorLogViewerPX4.m b/src/modules/ekf2/EKF/matlab/analysis/estimatorLogViewerPX4.m deleted file mode 100644 index efc3e8adde..0000000000 --- a/src/modules/ekf2/EKF/matlab/analysis/estimatorLogViewerPX4.m +++ /dev/null @@ -1,133 +0,0 @@ -function estimatorLogViewer(fname) -close all; -load(fname); - -stateFieldNames = [fieldnames(estimatorData.EST0);fieldnames(estimatorData.EST1)]; -stateFieldNames(strcmp('T',stateFieldNames) | strcmp('index',stateFieldNames) | ... - strcmp('nStat',stateFieldNames) | strcmp('fNaN',stateFieldNames) | ... - strcmp('fHealth',stateFieldNames) | strcmp('fTOut',stateFieldNames)) = []; - -innovFieldNamesT = [fieldnames(estimatorData.EST4);fieldnames(estimatorData.EST5)]; -innovFieldNamesT(strcmp('T',innovFieldNamesT) | strcmp('index',innovFieldNamesT)) = []; -innovFieldInds = cellfun(@(x)x(end)=='V',innovFieldNamesT); -innovFieldNames = innovFieldNamesT(~innovFieldInds); - - -% lay out static objects -f = figure('Units','normalized'); -set(f,'Position',[.25,.25,.5,.5]); - -tabs = uitabgroup(f); -statesTab = uitab(tabs,'Title','States'); -innovsTab = uitab(tabs,'Title','Innovations'); - -axesConfig = {'Units','Normalized','Position',[.05,.1,.65,.85],'NextPlot','add'}; -BGconfig = {'Units','Normalized','Position',[.75,.1,.2,.85]}; -TBconfig = {'Style','Checkbox','Units','Normalized','Position',[.75,0,.2,.1],'String','Show variance'}; - -statesAxes = axes('Parent',statesTab,axesConfig{:},'XLabel',text('string','Time (sec)')); -innovsAxes = axes('Parent',innovsTab,axesConfig{:},'XLabel',text('string','Time (sec)')); - -statesBG = uibuttongroup(statesTab,BGconfig{:}); -innovsBG = uibuttongroup(innovsTab,BGconfig{:}); - -statesVarToggle = uicontrol(statesTab,TBconfig{:}); -innovsVarToggle = uicontrol(innovsTab,TBconfig{:}); - -% lay out dynamic objects -stateButtons = zeros(numel(stateFieldNames),1); -for k = 1:numel(stateFieldNames) - stateButtons(k) = uicontrol(statesBG,'Style','Checkbox','Units','normalized',... - 'String',stateFieldNames{k},'Position',[0,1 - k/numel(stateFieldNames),1, 1/numel(stateFieldNames)],... - 'Callback',@(es,ed)toggleLine(es,ed,statesAxes,statesVarToggle,estimatorData.EST0,estimatorData.EST1,estimatorData.EST2,estimatorData.EST3)); -end -innovButtons = zeros(numel(innovFieldNames),1); -for k = 1:numel(innovFieldNames) - innovButtons(k) = uicontrol(innovsBG,'Style','Checkbox','Units','normalized',... - 'String',innovFieldNames{k},'Position',[0,1 - k/numel(innovFieldNames),1, 1/numel(innovFieldNames)],... - 'Callback',@(es,ed)toggleLine(es,ed,innovsAxes,innovsVarToggle,estimatorData.EST4,estimatorData.EST5,[],[])); -end - -set(statesVarToggle,'Callback',@(es,ed)toggleVariance(es,ed,statesAxes,estimatorData.EST2,estimatorData.EST3)); -set(innovsVarToggle,'Callback',@(es,ed)toggleVariance(es,ed,innovsAxes,estimatorData.EST4,estimatorData.EST5)); -end - -function toggleLine(es,~,axes,varToggle,struct1,struct2,vstruct1,vstruct2) -if es.Value == 0 - delete(findobj(axes,'UserData',es.String)); -else - if any(strcmp(es.String,fieldnames(struct1))) - dataSrc = struct1; - else - dataSrc = struct2; - end - p = plot(dataSrc.Tsec,dataSrc.(es.String)); - set(p,'Parent',axes,'UserData',es.String); -end -if varToggle.Value == 1 - if ~isempty(vstruct1) && ~isempty(vstruct2) - toggleVariance(varToggle,[],axes,vstruct1,vstruct2); - else - toggleVariance(varToggle,[],axes,struct1,struct2); - end -end -updateLegend(axes); -end - -function toggleVariance(es,~,axes,struct1,struct2) -if es.Value == 0 - delete(findobj(axes,'Type','Patch')); -else - lines = findobj(axes,'Type','Line'); - if isempty(lines) - return; - end - stateNames = {lines.UserData}; - if any(strncmp('s',stateNames,1)) - varNames = strrep(stateNames,'s','P'); - else - varNames = strrep(stateNames,'I','IV'); - end - for k = 1:numel(varNames) - if any(strcmp(varNames{k},fieldnames(struct1))) - dataSrc = struct1; - else - dataSrc = struct2; - end - centerLineTimeFull = get(lines(k),'XData'); - centerLineDataFull = get(lines(k),'YData'); - - startTime = max(centerLineTimeFull(1),dataSrc.Tsec(1)); - endTime = min(centerLineTimeFull(end),dataSrc.Tsec(end)); - plotTimeFull = dataSrc.Tsec(dataSrc.Tsec >= startTime & dataSrc.Tsec <= endTime); - plotDataFull = dataSrc.(varNames{k})(dataSrc.Tsec >= startTime & dataSrc.Tsec <= endTime); - - centerLineTime = centerLineTimeFull(centerLineTimeFull >= startTime & centerLineTimeFull <= endTime); - centerLineData = centerLineDataFull(centerLineTimeFull >= startTime & centerLineTimeFull <= endTime); - - plotTime = linspace(startTime,endTime,250); - plotData = sqrt(interp1(plotTimeFull,plotDataFull,plotTime)); - centerLineData = interp1(centerLineTime,centerLineData,plotTime); - - % plotTime = downsample(centerLineTime,round(numel(plotDataT)/350),0); - if strcmp('IV',varNames{k}(end-1:end)) - plotDataL = -plotData; - plotDataU = plotData; - else - plotDataL = centerLineData-plotData; - plotDataU = centerLineData+plotData; - end - p = patch([plotTime,fliplr(plotTime)],[plotDataL,fliplr(plotDataU)],lines(k).Color); - set(p,'Parent',axes,'EdgeColor','none','FaceAlpha',.3,'UserData',stateNames{k}); - end -end -end - -function updateLegend(axes) -lines = findobj(axes,'Type','Line'); -if isempty(lines) - legend(axes,'off'); -else - legend(axes,lines,{lines.UserData}); -end -end \ No newline at end of file diff --git a/src/modules/ekf2/EKF/matlab/analysis/importPX4log.m b/src/modules/ekf2/EKF/matlab/analysis/importPX4log.m deleted file mode 100644 index 42b17658a8..0000000000 --- a/src/modules/ekf2/EKF/matlab/analysis/importPX4log.m +++ /dev/null @@ -1,199 +0,0 @@ -function allData = importPX4log(fname,keep_msgs) - -% import a .px4log file -% INPUTS -% fname: path to a valid .px4log file -% keep_msgs: cell array of message names to keep -% OUTPUT -% allData: a Matlab struct with a field names for each message name in the log -% file. The content of each field is itself a struct with field names for -% each label in the corresponding message. The content of each of *these* -% fields will be an array of message data that is nonempty if the message -% name appears in keep_msgs - -% import method essentially translated from sdlog2_dump.py -% George Hines, 3D Robotics, Berkeley, CA -% 7 April 2016 - -BLOCK_SIZE = 8192; -MSG_HEADER_LEN = 3; -MSG_HEAD1 = uint8(hex2dec('A3')); -MSG_HEAD2 = uint8(hex2dec('95')); -MSG_FORMAT_PACKET_LEN = 89; -MSG_TYPE_FORMAT = uint8(hex2dec('80')); -% MSG_FORMAT_STRUCT = {'uint8','uint8','char4','char16','char64'}; -FORMAT_TO_STRUCT = { - 'b', {'int8', 1}; - 'B', {'uint8', 1}; - 'h', {'int16', 1}; - 'H', {'uint16', 1}; - 'i', {'int32', 1}; - 'I', {'uint32', 1}; - 'f', {'single', 1}; - 'd', {'double', 1}; - 'n', {'char4', 1}; - 'N', {'char16', 1}; - 'Z', {'char64', 1}; - 'c', {'int16', 0.01}; - 'C', {'uint16', 0.01}; - 'e', {'int32', 0.01}; - 'E', {'uint32', 0.01}; - 'L', {'int32', 0.0000001}; - 'M', {'uint8', 1}; - 'q', {'int64', 1}; - 'Q', {'uint64', 1}}; - -fid = fopen(fname,'r','b'); -fInfo = dir(fname); -totalBytes = fInfo.bytes; -bytes_read = 0; -msg_descrs = {}; -buffer = []; -ptr = 1; -allData = []; -nextPrint = 0; -disp('Reading file'); -while 1 - percentDone = bytes_read / totalBytes * 100; - if percentDone >= nextPrint - fprintf('%.0f%%\n',percentDone); - nextPrint = nextPrint + 5; - end - - chunk = fread(fid,BLOCK_SIZE,'uint8'); - if numel(chunk) == 0; - break - end - buffer = [buffer(ptr:end), chunk']; - ptr = 1; - while numel(buffer) - ptr > MSG_HEADER_LEN - head1 = buffer(ptr); - head2 = buffer(ptr+1); - if head1 ~= MSG_HEAD1 || head2 ~= MSG_HEAD2 - ptr = ptr + 1; - continue; - end - msg_type = buffer(ptr+2); - - if msg_type == MSG_TYPE_FORMAT - if numel(buffer) - ptr <= MSG_FORMAT_PACKET_LEN - break; - end - % return new pointer, and all message descriptor info - [ptr, msg_descr] = LOCAL_parse_message_descriptors(buffer, ptr, MSG_TYPE_FORMAT, MSG_FORMAT_PACKET_LEN, FORMAT_TO_STRUCT); - msg_descrs(msg_descr{1},:) = msg_descr; - cells = repmat({inf(1,500000)},1,numel(msg_descr{5})); - cells(msg_descr{4}=='n' | msg_descr{4} == 'N' | msg_descr{4} == 'Z') = {[]}; - seed = [{'index'},{'Tsec'},msg_descr{5};[{1},{inf(1,500000)},cells]]; - allData.(msg_descr{3}) = struct(seed{:}); - else - msg_descr = msg_descrs(msg_type,:); - msg_length = msg_descr{2}; - if numel(buffer) - ptr <= msg_length - break; - end - % return new pointer, and all message info - if strcmp(msg_descr{3},'TIME') || any(strcmp(msg_descr{3}, keep_msgs)) || isempty(keep_msgs) - [ptr,msg_data] = LOCAL_parse_message(buffer, ptr, MSG_HEADER_LEN, msg_descr); - ind = allData.(msg_descr{3}).index; - for k = 1:numel(msg_data) - if isnumeric(msg_data{k}) - allData.(msg_descr{3}).(msg_descr{5}{k})(ind) = msg_data{k}; - try - allData.(msg_descr{3}).Tsec(ind) = double(allData.TIME.StartTime(max(1,allData.TIME.index-1)))*1e-6; - end - noInc = false; - else - allData.(msg_descr{3}).(msg_descr{5}{k}) = [allData.(msg_descr{3}).(msg_descr{5}{k}), msg_data(k)]; - noInc = true; - end - end - if ~noInc - allData.(msg_descr{3}).index = ind + 1; - end - else - ptr = ptr + msg_descr{2}; - end - end - end - bytes_read = bytes_read + ptr; -end - -disp('Releasing excess preallocated memory'); -% clean out inf values -fields1 = fieldnames(allData); -for k = 1:numel(fields1) - fields2 = fieldnames(allData.(fields1{k})); - for m = 1:numel(fields2) - if isnumeric(allData.(fields1{k}).(fields2{m})) - allData.(fields1{k}).(fields2{m})(isinf(allData.(fields1{k}).(fields2{m}))) = []; - end - end -end -disp('Done'); -end - -function [ptr, msg_descr] = LOCAL_parse_message_descriptors(buffer, ptr, MSG_TYPE_FORMAT, MSG_FORMAT_PACKET_LEN, FORMAT_TO_STRUCT) -thisBlock = buffer((ptr+3):(ptr+MSG_FORMAT_PACKET_LEN+1)); -msg_descr = cell(1,7); -msg_type = thisBlock(1); -%if msg_type ~= MSG_TYPE_FORMAT - msg_length = thisBlock(2); - msg_name = LOCAL_parse_string(thisBlock(3:6)); - msg_format = LOCAL_parse_string(thisBlock(7:22)); - msg_labels = strsplit(LOCAL_parse_string(thisBlock(23:end)),','); - msg_struct = cell(1,numel(msg_format)); - msg_mults = zeros(1,numel(msg_format)); - - for k = 1:numel(msg_format) - info = FORMAT_TO_STRUCT{strcmp(msg_format(k),FORMAT_TO_STRUCT(:,1)),2}; - msg_struct{k} = info{1}; - msg_mults(k) = info{2}; - end - if isempty([msg_labels{:}]) - msg_labels = {'none'}; - end - msg_descr = {msg_type, msg_length, msg_name, msg_format, msg_labels, msg_struct, msg_mults}; -%end -ptr = ptr + MSG_FORMAT_PACKET_LEN; -end - -function [ptr, data] = LOCAL_parse_message(buffer, ptr, MSG_HEADER_LEN, msg_descr) -[~, msg_length, ~, ~, ~, msg_struct, msg_mults] = msg_descr{:}; - -% unpack message per the format specifiers in msg_struct -data = cell(1,numel(msg_struct)); -thisBytes = buffer((ptr+MSG_HEADER_LEN):(ptr+msg_length+1)); -thisPtr = 1; -for k = 1:numel(msg_struct) - % parse the data chunk per msg_struct{k} - [dataLen,data{k}] = LOCAL_unpack(thisPtr, thisBytes, msg_struct{k}, msg_mults(k)); - thisPtr = thisPtr + dataLen; -end -ptr = ptr + msg_length; -end - -function [dataLen, data] = LOCAL_unpack(thisPtr, byte_array, format_type, mult) -if strncmp('char',format_type,4) - dataLen = str2double(format_type(5:end)); - data = LOCAL_parse_string(byte_array(thisPtr:(thisPtr+dataLen))); -else - if strncmp('int',format_type,3) - dataLen = str2double(format_type(4:end))/8; - elseif strncmp('uint',format_type,4) - dataLen = str2double(format_type(5:end))/8; - elseif strcmp('single',format_type) - dataLen = 4; - end - data = double(typecast(uint8(byte_array(thisPtr:(thisPtr+dataLen-1))),format_type))*mult; -end -end - -function str = LOCAL_parse_string(byteArray) -firstZero = find(byteArray==0); -if ~isempty(firstZero) - str = char(byteArray(1:firstZero-1)); -else - str = char(byteArray); -end -end \ No newline at end of file diff --git a/src/modules/ekf2/EKF/matlab/analysis/usageSamples.m b/src/modules/ekf2/EKF/matlab/analysis/usageSamples.m deleted file mode 100644 index e5e76177c8..0000000000 --- a/src/modules/ekf2/EKF/matlab/analysis/usageSamples.m +++ /dev/null @@ -1,16 +0,0 @@ -fname = 'indoor_flight_test.px4log'; - -%% entire log -wholeLog = importPX4log(fname,{}); - -%% attitude message -attitudeData = importPX4log(fname,{'ATT'}); - -%% estimator messages -estimatorData = importPX4log(fname,{'EST0','EST1','EST2','EST3','EST4','EST5','EST6'}); - -%% to save -save ift estimatorData; - -%% to run viewer -estimatorLogViewer('ift.mat'); \ No newline at end of file diff --git a/src/modules/ekf2/EKF/matlab/scripts/Terrain Estimator/GenerateEquationsTerrainEstimator.m b/src/modules/ekf2/EKF/matlab/scripts/Terrain Estimator/GenerateEquationsTerrainEstimator.m deleted file mode 100644 index 47c78ae335..0000000000 --- a/src/modules/ekf2/EKF/matlab/scripts/Terrain Estimator/GenerateEquationsTerrainEstimator.m +++ /dev/null @@ -1,91 +0,0 @@ -% 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'); diff --git a/src/modules/ekf2/EKF/matlab/scripts/Terrain Estimator/H_OPT.c b/src/modules/ekf2/EKF/matlab/scripts/Terrain Estimator/H_OPT.c deleted file mode 100644 index c16eaf2010..0000000000 --- a/src/modules/ekf2/EKF/matlab/scripts/Terrain Estimator/H_OPT.c +++ /dev/null @@ -1,10 +0,0 @@ -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)); diff --git a/src/modules/ekf2/EKF/matlab/scripts/Terrain Estimator/H_RNG.c b/src/modules/ekf2/EKF/matlab/scripts/Terrain Estimator/H_RNG.c deleted file mode 100644 index 9a195c0901..0000000000 --- a/src/modules/ekf2/EKF/matlab/scripts/Terrain Estimator/H_RNG.c +++ /dev/null @@ -1 +0,0 @@ -A0[0][0] = 1.0/(q0*q0-q1*q1-q2*q2+q3*q3);