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

EKF2: purge outdated matlab scripts

Browse Source 
Those files are no longer maintained and will just confuse new users
This commit is contained in:
bresch 2022-12-30 09:29:42 +01:00 committed by Daniel Agar
parent cc1b043e18
commit 28d664aed5
68 changed files with 0 additions and 4435 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

20
src/modules/ekf2/EKF/matlab/EKF_replay/Common/AlignTilt.m
View File

@ -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

220
src/modules/ekf2/EKF/matlab/EKF_replay/Common/ConvertToC.m
View File

@ -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;

46
src/modules/ekf2/EKF/matlab/EKF_replay/Common/ConvertToM.m
View File

@ -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

21
src/modules/ekf2/EKF/matlab/EKF_replay/Common/EulToQuat.m
View File

@ -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;

12
src/modules/ekf2/EKF/matlab/EKF_replay/Common/LLH2NED.m
View File

@ -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

5
src/modules/ekf2/EKF/matlab/EKF_replay/Common/NormQuat.m
View File

@ -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;

29
src/modules/ekf2/EKF/matlab/EKF_replay/Common/OptimiseAlgebra.m
View File

@ -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

14
src/modules/ekf2/EKF/matlab/EKF_replay/Common/Quat2Tbn.m
View File

@ -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];

16
src/modules/ekf2/EKF/matlab/EKF_replay/Common/QuatDivide.m
View File

@ -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);

5
src/modules/ekf2/EKF/matlab/EKF_replay/Common/QuatMult.m
View File

@ -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))];

9
src/modules/ekf2/EKF/matlab/EKF_replay/Common/QuatToEul.m
View File

@ -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));

10
src/modules/ekf2/EKF/matlab/EKF_replay/Common/RotToQuat.m
View File

@ -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

103
src/modules/ekf2/EKF/matlab/EKF_replay/Common/convert_apm_data.m
View File

@ -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;

34
src/modules/ekf2/EKF/matlab/EKF_replay/Common/convert_px4_actuators_csv_data.m
View File

@ -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;

18
src/modules/ekf2/EKF/matlab/EKF_replay/Common/convert_px4_distance_sensor_csv_data.m
View File

@ -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;

53
src/modules/ekf2/EKF/matlab/EKF_replay/Common/convert_px4_groundtruth_csv_data.m
View File

@ -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;

23
src/modules/ekf2/EKF/matlab/EKF_replay/Common/convert_px4_optical_flow_csv_data.m
View File

@ -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;

47
src/modules/ekf2/EKF/matlab/EKF_replay/Common/convert_px4_sensor_combined_csv_data.m
View File

@ -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;

32
src/modules/ekf2/EKF/matlab/EKF_replay/Common/convert_px4_vehicle_gps_position_csv.m
View File

@ -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;

23
src/modules/ekf2/EKF/matlab/EKF_replay/Filter/AlignHeading.m
View File

@ -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

23
src/modules/ekf2/EKF/matlab/EKF_replay/Filter/ConstrainStates.m
View File

@ -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

56
src/modules/ekf2/EKF/matlab/EKF_replay/Filter/FuseBaroHeight.m
View File

@ -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

82
src/modules/ekf2/EKF/matlab/EKF_replay/Filter/FuseBodyVel.m
View File

@ -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

47
src/modules/ekf2/EKF/matlab/EKF_replay/Filter/FuseMagDeclination.m
View File

@ -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

71
src/modules/ekf2/EKF/matlab/EKF_replay/Filter/FuseMagHeading.m
View File

@ -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

87
src/modules/ekf2/EKF/matlab/EKF_replay/Filter/FuseMagnetometer.m
View File

@ -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

88
src/modules/ekf2/EKF/matlab/EKF_replay/Filter/FuseOpticalFlow.m
View File

@ -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

68
src/modules/ekf2/EKF/matlab/EKF_replay/Filter/FusePosition.m
View File

@ -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

68
src/modules/ekf2/EKF/matlab/EKF_replay/Filter/FuseVelocity.m
View File

@ -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

211
src/modules/ekf2/EKF/matlab/EKF_replay/Filter/GenerateEquations24.m
View File

@ -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');

35
src/modules/ekf2/EKF/matlab/EKF_replay/Filter/InitCovariance.m
View File

@ -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

66
src/modules/ekf2/EKF/matlab/EKF_replay/Filter/InitStates.m
View File

@ -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

512
src/modules/ekf2/EKF/matlab/EKF_replay/Filter/PlotData.m
View File

@ -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

66
src/modules/ekf2/EKF/matlab/EKF_replay/Filter/PredictCovariance.m
View File

@ -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

85
src/modules/ekf2/EKF/matlab/EKF_replay/Filter/PredictStates.m
View File

@ -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

347
src/modules/ekf2/EKF/matlab/EKF_replay/Filter/RunFilter.m
View File

@ -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

69
src/modules/ekf2/EKF/matlab/EKF_replay/Filter/SetParameterDefaults.m
View File

@ -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)

69
src/modules/ekf2/EKF/matlab/EKF_replay/Filter/SetParameters.m
View File

@ -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)

45
src/modules/ekf2/EKF/matlab/EKF_replay/Filter/calcF24.m
View File

@ -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]);

51
src/modules/ekf2/EKF/matlab/EKF_replay/Filter/calcH_HDG.m
View File

@ -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];

9
src/modules/ekf2/EKF/matlab/EKF_replay/Filter/calcH_LOSX.m
View File

@ -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];

9
src/modules/ekf2/EKF/matlab/EKF_replay/Filter/calcH_LOSY.m
View File

@ -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];

12
src/modules/ekf2/EKF/matlab/EKF_replay/Filter/calcH_MAGD.m
View File

@ -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];

8
src/modules/ekf2/EKF/matlab/EKF_replay/Filter/calcH_MAGX.m
View File

@ -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];

8
src/modules/ekf2/EKF/matlab/EKF_replay/Filter/calcH_MAGY.m
View File

@ -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];

8
src/modules/ekf2/EKF/matlab/EKF_replay/Filter/calcH_MAGZ.m
View File

@ -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];

8
src/modules/ekf2/EKF/matlab/EKF_replay/Filter/calcH_VELX.m
View File

@ -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];

8
src/modules/ekf2/EKF/matlab/EKF_replay/Filter/calcH_VELY.m
View File

@ -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];

8
src/modules/ekf2/EKF/matlab/EKF_replay/Filter/calcH_VELZ.m
View File

@ -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];

45
src/modules/ekf2/EKF/matlab/EKF_replay/Filter/calcQ24.m
View File

@ -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]);

50
src/modules/ekf2/EKF/matlab/EKF_replay/Filter/find_best_gps_delay.m
View File

@ -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');

38
src/modules/ekf2/EKF/matlab/EKF_replay/Filter/quat_to_euler_error_transfer_matrix.m
View File

@ -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]);

46
src/modules/ekf2/EKF/matlab/EKF_replay/Filter/replay_apm_data.m
View File

@ -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');

30
src/modules/ekf2/EKF/matlab/EKF_replay/Filter/replay_px4_data.m
View File

@ -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');

32
src/modules/ekf2/EKF/matlab/EKF_replay/Filter/replay_px4_optflow_data.m
View File

@ -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');

13
src/modules/ekf2/EKF/matlab/EKF_replay/Filter/transfer_matrix.m
View File

@ -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];

19
src/modules/ekf2/EKF/matlab/EKF_replay/SensorCalibration/allan.m
View File

@ -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)

189
src/modules/ekf2/EKF/matlab/EKF_replay/SensorCalibration/compare_mag_calibration.m
View File

@ -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)');

61
src/modules/ekf2/EKF/matlab/EKF_replay/SensorCalibration/ellipsoid_fit.m
View File

@ -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;

387
src/modules/ekf2/EKF/matlab/EKF_replay/px4_replay_import.m
View File

@ -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

108
src/modules/ekf2/EKF/matlab/EKF_replay/readme.txt
View File

@ -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.

3
src/modules/ekf2/EKF/matlab/README.md
View File

@ -1,3 +0,0 @@
# MATLAB Derivations
This folder contains the derivations and test suites in Matlab for ECL components.

133
src/modules/ekf2/EKF/matlab/analysis/estimatorLogViewerPX4.m
View File

@ -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

199
src/modules/ekf2/EKF/matlab/analysis/importPX4log.m
View File

@ -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

16
src/modules/ekf2/EKF/matlab/analysis/usageSamples.m
View File

@ -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');

91
src/modules/ekf2/EKF/matlab/scripts/Terrain Estimator/GenerateEquationsTerrainEstimator.m
View File

@ -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');

10
src/modules/ekf2/EKF/matlab/scripts/Terrain Estimator/H_OPT.c
View File

@ -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));

1
src/modules/ekf2/EKF/matlab/scripts/Terrain Estimator/H_RNG.c
View File

@ -1 +0,0 @@
A0[0][0] = 1.0/(q0*q0-q1*q1-q2*q2+q3*q3);