mirror of
https://gitee.com/mirrors_PX4/PX4-Autopilot.git
synced 2026-07-12 14:40:35 +08:00
Matlab: Add basic implementation of EKF to support development testing
This commit is contained in:
Paul Riseborough
@@ -0,0 +1,20 @@
|
||||
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
|
||||
@@ -0,0 +1,220 @@
|
||||
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;
|
||||
@@ -0,0 +1,46 @@
|
||||
function ConvertToM(nStates)
|
||||
%% Initialize variables
|
||||
fileName = strcat('SymbolicOutput',int2str(nStates),'.txt');
|
||||
delimiter = '';
|
||||
|
||||
%% Format string for each line of text:
|
||||
% column1: text (%s)
|
||||
% For more information, see the TEXTSCAN documentation.
|
||||
formatSpec = '%s%[^\n\r]';
|
||||
|
||||
%% Open the text file.
|
||||
fileID = fopen(fileName,'r');
|
||||
|
||||
%% Read columns of data according to format string.
|
||||
% This call is based on the structure of the file used to generate this
|
||||
% code. If an error occurs for a different file, try regenerating the code
|
||||
% from the Import Tool.
|
||||
dataArray = textscan(fileID, formatSpec, 'Delimiter', delimiter, 'ReturnOnError', false,'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
|
||||
@@ -0,0 +1,23 @@
|
||||
function quaterion = EulToQuat(Euler)
|
||||
|
||||
% Convert from a 321 Euler rotation sequence specified in radians to a
|
||||
% Quaternion
|
||||
|
||||
quaterion = zeros(4,1);
|
||||
|
||||
Euler = Euler * 0.5;
|
||||
cosPhi = cos(Euler(1));
|
||||
sinPhi = sin(Euler(1));
|
||||
cosTheta = cos(Euler(2));
|
||||
sinTheta = sin(Euler(2));
|
||||
cosPsi = cos(Euler(3));
|
||||
sinPsi = sin(Euler(3));
|
||||
|
||||
quaterion(1,1) = (cosPhi*cosTheta*cosPsi + sinPhi*sinTheta*sinPsi);
|
||||
quaterion(2,1) = (sinPhi*cosTheta*cosPsi - cosPhi*sinTheta*sinPsi);
|
||||
quaterion(3,1) = (cosPhi*sinTheta*cosPsi + sinPhi*cosTheta*sinPsi);
|
||||
quaterion(4,1) = (cosPhi*cosTheta*sinPsi - sinPhi*sinTheta*cosPsi);
|
||||
|
||||
return;
|
||||
|
||||
|
||||
@@ -0,0 +1,12 @@
|
||||
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
|
||||
@@ -0,0 +1,5 @@
|
||||
% normalise the quaternion
|
||||
function quaternion = normQuat(quaternion)
|
||||
|
||||
quatMag = sqrt(quaternion(1)^2 + quaternion(2)^2 + quaternion(3)^2 + quaternion(4)^2);
|
||||
quaternion(1:4) = quaternion / quatMag;
|
||||
@@ -0,0 +1,29 @@
|
||||
function [SymExpOut,SubExpArray] = OptimiseAlgebra(SymExpIn,SubExpName)
|
||||
|
||||
% Loop through symbolic expression, identifying repeated expressions and
|
||||
% bringing them out as shared expression or sub expressions
|
||||
% do this until no further repeated expressions found
|
||||
% This can significantly reduce computations
|
||||
|
||||
syms SubExpIn SubExpArray ;
|
||||
|
||||
SubExpArray(1,1) = 'invalid';
|
||||
index = 0;
|
||||
f_complete = 0;
|
||||
while f_complete==0
|
||||
index = index + 1;
|
||||
SubExpIn = [SubExpName,'(',num2str(index),')'];
|
||||
SubExpInStore{index} = SubExpIn;
|
||||
[SymExpOut,SubExpOut]=subexpr(SymExpIn,SubExpIn);
|
||||
for k = 1:index
|
||||
if SubExpOut == SubExpInStore{k}
|
||||
f_complete = 1;
|
||||
end
|
||||
end
|
||||
if f_complete || index > 100
|
||||
SymExpOut = SymExpIn;
|
||||
else
|
||||
SubExpArray(index,1) = SubExpOut;
|
||||
SymExpIn = SymExpOut;
|
||||
end
|
||||
end
|
||||
@@ -0,0 +1,14 @@
|
||||
function Tbn = Quat2Tbn(quat)
|
||||
|
||||
% Convert from quaternions defining the flight vehicles rotation to
|
||||
% the direction cosine matrix defining the rotation from body to navigation
|
||||
% coordinates
|
||||
|
||||
q0 = quat(1);
|
||||
q1 = quat(2);
|
||||
q2 = quat(3);
|
||||
q3 = quat(4);
|
||||
|
||||
Tbn = [q0^2 + q1^2 - q2^2 - q3^2, 2*(q1*q2 - q0*q3), 2*(q1*q3 + q0*q2); ...
|
||||
2*(q1*q2 + q0*q3), q0^2 - q1^2 + q2^2 - q3^2, 2*(q2*q3 - q0*q1); ...
|
||||
2*(q1*q3-q0*q2), 2*(q2*q3 + q0*q1), q0^2 - q1^2 - q2^2 + q3^2];
|
||||
@@ -0,0 +1,16 @@
|
||||
function q_out = QuatDivide(qin1,qin2)
|
||||
|
||||
q0 = qin1(1);
|
||||
q1 = qin1(2);
|
||||
q2 = qin1(3);
|
||||
q3 = qin1(4);
|
||||
|
||||
r0 = qin2(1);
|
||||
r1 = qin2(2);
|
||||
r2 = qin2(3);
|
||||
r3 = qin2(4);
|
||||
|
||||
q_out(1,1) = (qin2(1)*qin1(1) + qin2(2)*qin1(2) + qin2(3)*qin1(3) + qin2(4)*qin1(4));
|
||||
q_out(2,1) = (r0*q1 - r1*q0 - r2*q3 + r3*q2);
|
||||
q_out(3,1) = (r0*q2 + r1*q3 - r2*q0 - r3*q1);
|
||||
q_out(4,1) = (r0*q3 - r1*q2 + r2*q1 - r3*q0);
|
||||
@@ -0,0 +1,5 @@
|
||||
function quatOut = QuatMult(quatA,quatB)
|
||||
% Calculate the following quaternion product quatA * quatB using the
|
||||
% standard identity
|
||||
|
||||
quatOut = [quatA(1)*quatB(1)-quatA(2:4)'*quatB(2:4); quatA(1)*quatB(2:4) + quatB(1)*quatA(2:4) + cross(quatA(2:4),quatB(2:4))];
|
||||
@@ -0,0 +1,9 @@
|
||||
% Convert from a quaternion to a 321 Euler rotation sequence in radians
|
||||
|
||||
function Euler = QuatToEul(quat)
|
||||
|
||||
Euler = zeros(3,1);
|
||||
|
||||
Euler(1) = atan2(2*(quat(3)*quat(4)+quat(1)*quat(2)), quat(1)*quat(1) - quat(2)*quat(2) - quat(3)*quat(3) + quat(4)*quat(4));
|
||||
Euler(2) = -asin(2*(quat(2)*quat(4)-quat(1)*quat(3)));
|
||||
Euler(3) = atan2(2*(quat(2)*quat(3)+quat(1)*quat(4)), quat(1)*quat(1) + quat(2)*quat(2) - quat(3)*quat(3) - quat(4)*quat(4));
|
||||
@@ -0,0 +1,10 @@
|
||||
% convert froma rotation vector in radians to a quaternion
|
||||
function quaternion = RotToQuat(rotVec)
|
||||
|
||||
vecLength = sqrt(rotVec(1)^2 + rotVec(2)^2 + rotVec(3)^2);
|
||||
|
||||
if vecLength < 1e-6
|
||||
quaternion = [1;0;0;0];
|
||||
else
|
||||
quaternion = [cos(0.5*vecLength); rotVec/vecLength*sin(0.5*vecLength)];
|
||||
end
|
||||
@@ -0,0 +1,94 @@
|
||||
%% 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
|
||||
|
||||
%% convert IMU data to delta angles and velocities using trapezoidal integration
|
||||
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);
|
||||
|
||||
%% convert magnetomer 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 GPS daa
|
||||
clear gps_data;
|
||||
gps_data.time_us = GPS(:,2);
|
||||
gps_data.pos_error = GPA(:,4);
|
||||
gps_data.spd_error = GPA(:,6);
|
||||
gps_data.hgt_error = GPA(:,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:length(GPS)
|
||||
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 range finder data
|
||||
clear rng_data;
|
||||
rng_data.time_us = RFND(:,2);
|
||||
rng_data.dist = RFND(:,3);
|
||||
|
||||
%% save optical flow data
|
||||
clear flow_data;
|
||||
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); % time period the measurement was sampled across (sec)
|
||||
|
||||
%% save visual odometry data
|
||||
clear viso_data;
|
||||
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
|
||||
viso_data.dAngY = VISO(:,5); % delta angle about the Y body axis
|
||||
viso_data.dAngZ = VISO(:,6); % delta angle about the Z body axis
|
||||
viso_data.dVelX = VISO(:,7); % delta velocity along the X body axis
|
||||
viso_data.dVelY = VISO(:,8); % delta velocity along the Y body axis
|
||||
viso_data.dVelZ = VISO(:,9); % delta velocity along the Z body axis
|
||||
viso_data.qual = VISO(:,10)/100; % quality from 0 - 1
|
||||
|
||||
%% save data and clear workspace
|
||||
clearvars -except baro_data imu_data mag_data gps_data rng_data flow_data viso_data;
|
||||
|
||||
save baro_data.mat baro_data;
|
||||
save imu_data.mat imu_data;
|
||||
save mag_data.mat mag_data;
|
||||
save gps_data.mat gps_data;
|
||||
save rng_data.mat rng_data;
|
||||
save flow_data.mat flow_data;
|
||||
save viso_data.mat viso_data;
|
||||
@@ -0,0 +1,54 @@
|
||||
%% convert baro data
|
||||
clear baro_data;
|
||||
last_time = 0;
|
||||
output_index = 1;
|
||||
for source_index = 1:length(timestamp)
|
||||
baro_timestamp = timestamp(source_index) + baro_timestamp_relative(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 using trapezoidal integration
|
||||
clear imu_data;
|
||||
n_samples = length(timestamp);
|
||||
imu_data.time_us = timestamp(2:n_samples) + accelerometer_timestamp_relative(2:n_samples);
|
||||
imu_data.gyro_dt = gyro_integral_dt(2:n_samples);
|
||||
imu_data.del_ang = 0.5 * ([gyro_rad0(1:n_samples-1).*imu_data.gyro_dt, ...
|
||||
gyro_rad1(1:n_samples-1).*imu_data.gyro_dt, ...
|
||||
gyro_rad2(1:n_samples-1).*imu_data.gyro_dt] + ...
|
||||
[gyro_rad0(2:n_samples).*imu_data.gyro_dt, ...
|
||||
gyro_rad1(2:n_samples).*imu_data.gyro_dt, ...
|
||||
gyro_rad2(2:n_samples).*imu_data.gyro_dt]);
|
||||
|
||||
imu_data.accel_dt = accelerometer_integral_dt(2:n_samples);
|
||||
imu_data.del_vel = 0.5 * ([accelerometer_m_s20(1:n_samples-1).*imu_data.accel_dt, ...
|
||||
accelerometer_m_s21(1:n_samples-1).*imu_data.accel_dt, ...
|
||||
accelerometer_m_s22(1:n_samples-1).*imu_data.accel_dt] + ...
|
||||
[accelerometer_m_s20(2:n_samples).*imu_data.accel_dt, ...
|
||||
accelerometer_m_s21(2:n_samples).*imu_data.accel_dt, ...
|
||||
accelerometer_m_s22(2:n_samples).*imu_data.accel_dt]);
|
||||
|
||||
%% convert magnetomer data
|
||||
clear mag_data;
|
||||
last_time = 0;
|
||||
output_index = 1;
|
||||
for source_index = 1:length(timestamp)
|
||||
mag_timestamp = timestamp(source_index) + magnetometer_timestamp_relative(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 and clear workspace
|
||||
clearvars -except baro_data imu_data mag_data gps_data;
|
||||
|
||||
save baro_data.mat baro_data;
|
||||
save imu_data.mat imu_data;
|
||||
save mag_data.mat mag_data;
|
||||
@@ -0,0 +1,24 @@
|
||||
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)));
|
||||
gps_data.refLLH = [1e-7*lat(gps_data.start_index);1e-7*lon(gps_data.start_index);0.001*alt(gps_data.start_index)];
|
||||
|
||||
% 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
|
||||
|
||||
clearvars -except baro_data imu_data mag_data gps_data;
|
||||
|
||||
save gps_data.mat;
|
||||
@@ -0,0 +1,47 @@
|
||||
function quat = AlignHeading( ...
|
||||
quat, ... % quaternion state vector
|
||||
magMea, ... % body frame magnetic flux measurements
|
||||
declination) % Estimated magnetic field delination at current location
|
||||
|
||||
% Calculate the predicted magnetic declination
|
||||
Tbn = Quat2Tbn(quat);
|
||||
magMeasNED = Tbn*magMea;
|
||||
predDec = atan2(magMeasNED(2),magMeasNED(1));
|
||||
|
||||
% Calculate the measurement innovation
|
||||
innovation = predDec - declination;
|
||||
|
||||
if (innovation > pi)
|
||||
innovation = innovation - 2*pi;
|
||||
elseif (innovation < -pi)
|
||||
innovation = innovation + 2*pi;
|
||||
end
|
||||
|
||||
% form the NED rotation vector
|
||||
deltaRotNED = -[0;0;innovation];
|
||||
|
||||
% rotate into body axes
|
||||
% Calculate the body to nav cosine matrix
|
||||
Tbn = Quat2Tbn(quat);
|
||||
deltaRotBody = transpose(Tbn)*deltaRotNED;
|
||||
|
||||
% Convert the error rotation vector to its equivalent quaternion
|
||||
% error = truth - estimate
|
||||
rotationMag = abs(innovation);
|
||||
if rotationMag<1e-6
|
||||
deltaQuat = single([1;0;0;0]);
|
||||
else
|
||||
deltaQuat = [cos(0.5*rotationMag); [deltaRotBody(1);deltaRotBody(2);deltaRotBody(3)]/rotationMag*sin(0.5*rotationMag)];
|
||||
end
|
||||
|
||||
% Update the quaternion states by rotating from the previous attitude through
|
||||
% the delta angle rotation quaternion
|
||||
quat = [quat(1)*deltaQuat(1)-transpose(quat(2:4))*deltaQuat(2:4); quat(1)*deltaQuat(2:4) + deltaQuat(1)*quat(2:4) + cross(quat(2:4),deltaQuat(2:4))];
|
||||
|
||||
% normalise the updated quaternion states
|
||||
quatMag = sqrt(quat(1)^2 + quat(2)^2 + quat(3)^2 + quat(4)^2);
|
||||
if (quatMag > 1e-12)
|
||||
quat = quat / quatMag;
|
||||
end
|
||||
|
||||
end
|
||||
@@ -0,0 +1,23 @@
|
||||
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
|
||||
@@ -0,0 +1,56 @@
|
||||
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
|
||||
@@ -0,0 +1,82 @@
|
||||
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
|
||||
@@ -0,0 +1,47 @@
|
||||
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
|
||||
@@ -0,0 +1,71 @@
|
||||
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
|
||||
@@ -0,0 +1,87 @@
|
||||
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) % magnetoemter 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
|
||||
@@ -0,0 +1,88 @@
|
||||
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
|
||||
@@ -0,0 +1,68 @@
|
||||
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
|
||||
@@ -0,0 +1,68 @@
|
||||
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
|
||||
@@ -0,0 +1,203 @@
|
||||
%% 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 covariance prediction equation
|
||||
% This reduces the number of floating point operations by a factor of 6 or
|
||||
% more compared to using the standard matrix operations in code
|
||||
|
||||
% 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 becasue 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');
|
||||
@@ -0,0 +1,36 @@
|
||||
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
|
||||
|
||||
@@ -0,0 +1,51 @@
|
||||
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];
|
||||
|
||||
% 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);
|
||||
|
||||
% 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));
|
||||
initAccel(2) = mean(imu_data.del_vel(imu_start_index:imu_start_index+99,2));
|
||||
initAccel(3) = mean(imu_data.del_vel(imu_start_index:imu_start_index+99,3));
|
||||
|
||||
% align tilt using gravity vector (If the velocity is changing this will
|
||||
% induce errors)
|
||||
quat = AlignTilt(quat,initAccel);
|
||||
states(1:4) = quat;
|
||||
|
||||
% find magnetometer start index that coresponds to first valid GPS data
|
||||
mag_start_index = (find(mag_data.time_us > gps_data.time_us(gps_data.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);
|
||||
|
||||
% initialise the NED magnetic field states
|
||||
Tbn = Quat2Tbn(quat);
|
||||
states(17:19) = Tbn*magBody;
|
||||
|
||||
% 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);
|
||||
|
||||
% find baro start index that coresponds to first valid GPS data
|
||||
baro_start_index = (find(baro_data.time_us > gps_data.time_us(gps_data.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
|
||||
@@ -0,0 +1,504 @@
|
||||
function PlotData(output)
|
||||
rad2deg = 180/pi;
|
||||
runIdentifier = ' : EKF replay ';
|
||||
folder = strcat('../OutputPlots');
|
||||
|
||||
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);
|
||||
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)');
|
||||
|
||||
subplot(3,1,2);
|
||||
plot(output.time_lapsed,output.euler_angles(:,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)');
|
||||
|
||||
subplot(3,1,3);
|
||||
plot(output.time_lapsed,output.euler_angles(:,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)');
|
||||
|
||||
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
|
||||
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);
|
||||
|
||||
%% plot position innovations
|
||||
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);
|
||||
|
||||
%% 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))']);
|
||||
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))']);
|
||||
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
|
||||
@@ -0,0 +1,66 @@
|
||||
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 predition 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
|
||||
@@ -0,0 +1,74 @@
|
||||
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);
|
||||
|
||||
% Apply rotational and skulling corrections
|
||||
correctedDelVel= delVel + ...
|
||||
0.5*cross(prevDelAng + delAng , prevDelVel + delVel) + 1/6*cross(prevDelAng + delAng , cross(prevDelAng + delAng , prevDelVel + delVel)) + ... % rotational correction
|
||||
1/12*(cross(prevDelAng , delVel) + cross(prevDelVel , delAng)); % sculling correction
|
||||
|
||||
% 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
|
||||
correctedDelAng = delAng - 1/12*cross(prevDelAng , 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
|
||||
@@ -0,0 +1,274 @@
|
||||
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
|
||||
useOpticalFlow = ~isempty(varargin{1}) && ~isempty(varargin{2});
|
||||
rng_data = varargin{1};
|
||||
flow_data = varargin{2};
|
||||
if useOpticalFlow
|
||||
fprintf('Using optical Flow Data\n',nVarargs);
|
||||
end
|
||||
else
|
||||
useOpticalFlow = 0;
|
||||
end
|
||||
|
||||
if nVarargs >= 3
|
||||
useVisualOdometry = ~isempty(varargin{3});
|
||||
viso_data = varargin{3};
|
||||
if useVisualOdometry
|
||||
fprintf('Using ZED camera odometry data\n',nVarargs);
|
||||
end
|
||||
else
|
||||
useVisualOdometry = 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 at the first position where there is OK GPS
|
||||
[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_variance',[],...
|
||||
'innovations',[],...
|
||||
'magFuseMethod',[]);
|
||||
|
||||
% initialise the state covariance matrix
|
||||
covariance = InitCovariance(param,dt_imu_avg,1,gps_data);
|
||||
|
||||
%% Main Loop
|
||||
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;
|
||||
delAngCov = [0;0;0];
|
||||
delVelCov = [0;0;0];
|
||||
dtCov = 0;
|
||||
dtCovInt = 0;
|
||||
covIndex = 0;
|
||||
output.magFuseMethod = param.fusion.magFuseMethod;
|
||||
range = 0.1;
|
||||
latest_range_index = 1;
|
||||
for index = indexStart:indexStop
|
||||
|
||||
% read IMU measurements
|
||||
localTime=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) = localTime;
|
||||
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
|
||||
|
||||
% Fuse new GPS data that has fallen beind the fusion time horizon
|
||||
latest_gps_index = find((gps_data.time_us - 1e6 * param.fusion.gpsTimeDelay) < imu_data.time_us(imuIndex), 1, 'last' );
|
||||
if (latest_gps_index > last_gps_index)
|
||||
last_gps_index = latest_gps_index;
|
||||
gps_fuse_index = gps_fuse_index + 1;
|
||||
|
||||
% 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) = localTime;
|
||||
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) = localTime;
|
||||
output.innovations.posInnov(gps_fuse_index,:) = posInnov';
|
||||
output.innovations.posInnovVar(gps_fuse_index,:) = posInnovVar';
|
||||
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) = localTime;
|
||||
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) = localTime;
|
||||
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 magnetomer 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) = localTime;
|
||||
output.innovations.hdgInnov(mag_fuse_index) = hdgInnov;
|
||||
output.innovations.hdgInnovVar(mag_fuse_index) = hdgInnovVar;
|
||||
|
||||
end
|
||||
|
||||
end
|
||||
|
||||
% Attempt to use optical flow and range finder data if available
|
||||
if (useOpticalFlow)
|
||||
|
||||
% Get latest range finder data and gate to remove dropouts
|
||||
latest_range_index = find((rng_data.time_us - 1e6 * param.fusion.rangeTimeDelay) < imu_data.time_us(imuIndex), 1, 'last' );
|
||||
if (rng_data.dist(latest_range_index) < 5.0 && rng_data.dist(latest_range_index) > 0.05)
|
||||
range = rng_data.dist(latest_range_index);
|
||||
end
|
||||
|
||||
% Fuse optical flow data that has fallen behind the fusion time horizon
|
||||
latest_flow_index = find((flow_data.time_us - 1e6 * param.fusion.flowTimeDelay) < imu_data.time_us(imuIndex), 1, 'last' );
|
||||
if (latest_flow_index > last_flow_index)
|
||||
last_flow_index = latest_flow_index;
|
||||
flow_fuse_index = flow_fuse_index + 1;
|
||||
|
||||
% 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) = localTime;
|
||||
output.innovations.flowInnov(flow_fuse_index,:) = flowInnov;
|
||||
output.innovations.flowInnovVar(flow_fuse_index,:) = flowInnovVar;
|
||||
|
||||
end
|
||||
|
||||
end
|
||||
|
||||
% attempt to use ZED camera visual odmetry data if available
|
||||
if (useVisualOdometry)
|
||||
|
||||
% 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;
|
||||
|
||||
% convert delta positon measurements to velocity
|
||||
relVelBodyMea = [viso_data.dVelX(latest_viso_index);viso_data.dVelY(latest_viso_index);viso_data.dVelZ(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) = localTime;
|
||||
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
|
||||
@@ -0,0 +1,55 @@
|
||||
|
||||
%% GPS fusion
|
||||
param.fusion.gpsTimeDelay = 0.1; % GPS measurement delay relative to IMU (sec)
|
||||
param.fusion.gpsCheckTimeout = 5.0; % Length of time that GPS measurements will be rejected by the filter before states are reset to the GPS velocity. (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.baroCheckTimeout = 5.0; % Length of time that baro measurements will be rejected by the filter before states are reset to the baro height. (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; % Magnetomer 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.5; % 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
|
||||
|
||||
%% Body frame velocity measurement fusion
|
||||
param.fusion.bodyVelTimeDelay = 0.01; % Optical flow sensor time delay relative to IMU (sec)
|
||||
param.fusion.bodyVelErrorMin = 0.5; % Observation noise 1SD for the odometry sensor at the highest quality value (m/sec)
|
||||
param.fusion.bodyVelErrorMax = 5.0; % 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.003; % 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)
|
||||
param.prediction.windPnoiseNE = 0.1; % wind state process 1SD rate of change (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)
|
||||
@@ -0,0 +1,45 @@
|
||||
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]);
|
||||
@@ -0,0 +1,51 @@
|
||||
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];
|
||||
@@ -0,0 +1,9 @@
|
||||
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];
|
||||
@@ -0,0 +1,9 @@
|
||||
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];
|
||||
@@ -0,0 +1,12 @@
|
||||
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];
|
||||
@@ -0,0 +1,8 @@
|
||||
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];
|
||||
@@ -0,0 +1,8 @@
|
||||
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];
|
||||
@@ -0,0 +1,8 @@
|
||||
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];
|
||||
@@ -0,0 +1,8 @@
|
||||
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];
|
||||
@@ -0,0 +1,8 @@
|
||||
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];
|
||||
@@ -0,0 +1,8 @@
|
||||
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];
|
||||
@@ -0,0 +1,45 @@
|
||||
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]);
|
||||
@@ -0,0 +1,28 @@
|
||||
clear all;
|
||||
close all;
|
||||
|
||||
% 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 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
|
||||
|
||||
% oad data required for ZED camera replay
|
||||
if exist('../TestData/APM/viso_data.mat','file')
|
||||
load '../TestData/APM/viso_data.mat';
|
||||
else
|
||||
viso_data = [];
|
||||
end
|
||||
|
||||
run('SetParameterDefaults.m');
|
||||
output = RunFilter(param,imu_data,mag_data,baro_data,gps_data,rng_data,flow_data,viso_data);
|
||||
PlotData(output);
|
||||
@@ -0,0 +1,9 @@
|
||||
clear all;
|
||||
close all;
|
||||
load '../TestData/PX4/baro_data.mat';
|
||||
load '../TestData/PX4/gps_data.mat';
|
||||
load '../TestData/PX4/imu_data.mat';
|
||||
load '../TestData/PX4/mag_data.mat';
|
||||
run('SetParameterDefaults.m');
|
||||
output = RunFilter(param,imu_data,mag_data,baro_data,gps_data);
|
||||
PlotData(output);
|
||||
@@ -0,0 +1,13 @@
|
||||
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];
|
||||
@@ -0,0 +1,19 @@
|
||||
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)
|
||||
@@ -0,0 +1,196 @@
|
||||
% 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'];
|
||||
|
||||
% direct loading of .px4log files
|
||||
ecl_path = '/Users/paul/src/pacflyer_PX4/PX4/src/lib/ecl/matlab/analysis';
|
||||
addpath ecl_path;
|
||||
data1 = importPX4log('/Users/paul/Downloads/20170507_1013_sess006.px4log','IMU');
|
||||
data2 = importPX4log('/Users/paul/Downloads/20170507_1013_sess007.px4log','IMU');
|
||||
data3 = importPX4log('/Users/paul/Downloads/20170507_1013_sess008.px4log','IMU');
|
||||
|
||||
% 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)');
|
||||
@@ -0,0 +1,61 @@
|
||||
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;
|
||||
@@ -0,0 +1,50 @@
|
||||
Instructions for running the EKF replay
|
||||
|
||||
1) Ensure this ‘EKF_replay’ directory is in a location you have full read and write access and add it and all its subdirectories to your path.
|
||||
|
||||
2) Create a ‘TestData’ sub-directory inside ‘EKF_replay’ directory
|
||||
|
||||
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
|
||||
|
||||
Copy the generated .mat files into the /EKF_replay/TestData/APM directory.
|
||||
|
||||
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.
|
||||
|
||||
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
|
||||
Import the .csv file containing the sensor_combined_0 data into the matlab workspace and process it using …/EKF_replay/Common/convert_px4_sensor_combined_csv_data.m. This will generate the following data files:
|
||||
|
||||
imu_data.mat
|
||||
baro_data.mat
|
||||
mag_data.mat
|
||||
|
||||
Import the .csv file containing the vehicle_gps_position_0 data into the matlab workspace and process it using …/EKF_replay/Common/convert_px4_vehicle_gps_position_csv. This will generate the gps_data.mat file.
|
||||
|
||||
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 or replay_px4_data.m script files.
|
||||
|
||||
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.
|
||||
Reference in New Issue
Block a user