forked from Archive/PX4-Autopilot
matlab: improve EKF simulation fusion control
Add ability to start before GPS checks pass. Add ability to turn GPS fusion off and on during replay. Add ability to turn Optical Flow fusion off and on during replay. Add ability to turn Visual Odometry fusion off and on during replay. Convert miscellaneous constants to parameters
This commit is contained in:
Paul Riseborough
parent
dabf129daf
commit
3a7a66d360
|
|
@ -7,9 +7,13 @@ deg2rad = pi/180;
|
|||
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);
|
||||
if (param.control.waitForGps == 1)
|
||||
% find IMU start index that coresponds to first valid GPS data
|
||||
imu_start_index = (find(imu_data.time_us > gps_data.time_us(gps_data.start_index), 1, 'first' ) - 50);
|
||||
imu_start_index = max(imu_start_index,1);
|
||||
else
|
||||
imu_start_index = 1;
|
||||
end
|
||||
|
||||
% average first 100 accel readings to reduce effect of vibration
|
||||
initAccel(1) = mean(imu_data.del_vel(imu_start_index:imu_start_index+99,1));
|
||||
|
|
@ -21,8 +25,8 @@ initAccel(3) = mean(imu_data.del_vel(imu_start_index:imu_start_index+99,3));
|
|||
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);
|
||||
% find magnetometer start index
|
||||
mag_start_index = (find(mag_data.time_us > imu_data.time_us(imu_start_index), 1, 'first' ) - 5);
|
||||
mag_start_index = max(mag_start_index,1);
|
||||
|
||||
% mean to reduce effect of noise in data
|
||||
|
|
@ -37,12 +41,18 @@ quat = AlignHeading(quat,magBody,param.fusion.magDeclDeg*deg2rad);
|
|||
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);
|
||||
if (param.control.waitForGps == 1)
|
||||
% initialise velocity and position using gps
|
||||
states(5:7) = gps_data.vel_ned(gps_data.start_index,:);
|
||||
states(8:9) = gps_data.pos_ned(gps_data.start_index,1:2);
|
||||
else
|
||||
% initialise to be stationary at the origin
|
||||
states(5:7) = zeros(1,3);
|
||||
states(8:9) = zeros(1,2);
|
||||
end
|
||||
|
||||
% find baro start index 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);
|
||||
% find baro start index
|
||||
baro_start_index = (find(baro_data.time_us > imu_data.time_us(imu_start_index), 1, 'first' ) - 10);
|
||||
baro_start_index = max(baro_start_index,1);
|
||||
|
||||
% average baro data and initialise the vertical position
|
||||
|
|
|
|||
|
|
@ -16,24 +16,24 @@ function output = RunFilter(param,imu_data,mag_data,baro_data,gps_data,varargin)
|
|||
|
||||
nVarargs = length(varargin);
|
||||
if nVarargs >= 2
|
||||
useOpticalFlow = ~isempty(varargin{1}) && ~isempty(varargin{2});
|
||||
flowDataPresent = ~isempty(varargin{1}) && ~isempty(varargin{2});
|
||||
rng_data = varargin{1};
|
||||
flow_data = varargin{2};
|
||||
if useOpticalFlow
|
||||
if flowDataPresent
|
||||
fprintf('Using optical Flow Data\n',nVarargs);
|
||||
end
|
||||
else
|
||||
useOpticalFlow = 0;
|
||||
flowDataPresent = 0;
|
||||
end
|
||||
|
||||
if nVarargs >= 3
|
||||
useVisualOdometry = ~isempty(varargin{3});
|
||||
visoDataPresent = ~isempty(varargin{3});
|
||||
viso_data = varargin{3};
|
||||
if useVisualOdometry
|
||||
if visoDataPresent
|
||||
fprintf('Using ZED camera odometry data\n',nVarargs);
|
||||
end
|
||||
else
|
||||
useVisualOdometry = 0;
|
||||
visoDataPresent = 0;
|
||||
end
|
||||
|
||||
|
||||
|
|
@ -43,7 +43,7 @@ end
|
|||
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
|
||||
% initialise the state vector
|
||||
[states, imu_start_index] = InitStates(param,imu_data,gps_data,mag_data,baro_data);
|
||||
|
||||
dt_imu_avg = 0.5 * (median(imu_data.gyro_dt) + median(imu_data.accel_dt));
|
||||
|
|
@ -69,6 +69,14 @@ output = struct('time_lapsed',[]',...
|
|||
covariance = InitCovariance(param,dt_imu_avg,1,gps_data);
|
||||
|
||||
%% Main Loop
|
||||
|
||||
% control flags
|
||||
gps_use_started = boolean(false);
|
||||
gps_use_blocked = boolean(false);
|
||||
viso_use_blocked = boolean(false);
|
||||
flow_use_blocked = boolean(false);
|
||||
|
||||
% array access index variables
|
||||
imuIndex = imu_start_index;
|
||||
last_gps_index = 0;
|
||||
gps_fuse_index = 0;
|
||||
|
|
@ -80,18 +88,27 @@ 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;
|
||||
last_range_index = 0;
|
||||
|
||||
% covariance prediction variables
|
||||
delAngCov = [0;0;0]; % delta angle vector used by the covariance prediction (rad)
|
||||
delVelCov = [0;0;0]; % delta velocity vector used by the covariance prediction (m/sec)
|
||||
dtCov = 0; % time step used by the covariance prediction (sec)
|
||||
dtCovInt = 0; % accumulated time step of covariance predictions (sec)
|
||||
covIndex = 0; % covariance prediction frame counter
|
||||
|
||||
output.magFuseMethod = param.fusion.magFuseMethod;
|
||||
range = 0.1;
|
||||
latest_range_index = 1;
|
||||
|
||||
% variables used to control dead-reckoning timeout
|
||||
last_drift_constrain_time = - param.control.velDriftTimeLim;
|
||||
last_synthetic_velocity_fusion_time = 0;
|
||||
last_valid_range_time = - param.fusion.rngTimeout;
|
||||
|
||||
for index = indexStart:indexStop
|
||||
|
||||
% read IMU measurements
|
||||
localTime=imu_data.time_us(imuIndex)*1e-6;
|
||||
local_time=imu_data.time_us(imuIndex)*1e-6;
|
||||
delta_angle(:,1) = imu_data.del_ang(imuIndex,:);
|
||||
delta_velocity(:,1) = imu_data.del_vel(imuIndex,:);
|
||||
dt_imu = 0.5 * (imu_data.accel_dt(imuIndex) + imu_data.gyro_dt(imuIndex));
|
||||
|
|
@ -116,7 +133,7 @@ for index = indexStart:indexStop
|
|||
covIndex = covIndex + 1;
|
||||
|
||||
% output state data
|
||||
output.time_lapsed(covIndex) = localTime;
|
||||
output.time_lapsed(covIndex) = local_time;
|
||||
output.euler_angles(covIndex,:) = QuatToEul(states(1:4)')';
|
||||
output.velocity_NED(covIndex,:) = states(5:7)';
|
||||
output.position_NED(covIndex,:) = states(8:10)';
|
||||
|
|
@ -131,17 +148,38 @@ for index = indexStart:indexStop
|
|||
output.state_variances(covIndex,i) = covariance(i,i);
|
||||
end
|
||||
|
||||
% Fuse new GPS data that has fallen beind the fusion time horizon
|
||||
% Get most recent GPS data that had fallen behind the fusion time horizon
|
||||
latest_gps_index = find((gps_data.time_us - 1e6 * param.fusion.gpsTimeDelay) < imu_data.time_us(imuIndex), 1, 'last' );
|
||||
if (latest_gps_index > last_gps_index)
|
||||
|
||||
% Check if GPS use is being blocked by the user
|
||||
if ((local_time < param.control.gpsOnTime) && (local_time > param.control.gpsOffTime))
|
||||
gps_use_started = false;
|
||||
gps_use_blocked = true;
|
||||
else
|
||||
gps_use_blocked = false;
|
||||
end
|
||||
|
||||
% If we haven't started using GPS, check that the quality is sufficient before aligning the position and velocity states to GPS
|
||||
if (~gps_use_started && ~gps_use_blocked)
|
||||
if ((gps_data.spd_error(latest_gps_index) < param.control.gpsSpdErrLim) && (gps_data.pos_error(latest_gps_index) < param.control.gpsPosErrLim))
|
||||
states(5:7) = gps_data.vel_ned(latest_gps_index,:);
|
||||
states(8:9) = gps_data.pos_ned(latest_gps_index,1:2);
|
||||
gps_use_started = true;
|
||||
last_drift_constrain_time = local_time;
|
||||
end
|
||||
end
|
||||
|
||||
% Fuse GPS data when available if GPS use has started
|
||||
if (gps_use_started && ~gps_use_blocked && (latest_gps_index > last_gps_index))
|
||||
last_gps_index = latest_gps_index;
|
||||
gps_fuse_index = gps_fuse_index + 1;
|
||||
last_drift_constrain_time = local_time;
|
||||
|
||||
% fuse NED GPS velocity
|
||||
[states,covariance,velInnov,velInnovVar] = FuseVelocity(states,covariance,gps_data.vel_ned(latest_gps_index,:),param.fusion.gpsVelGate,gps_data.spd_error(latest_gps_index));
|
||||
|
||||
% data logging
|
||||
output.innovations.vel_time_lapsed(gps_fuse_index) = localTime;
|
||||
output.innovations.vel_time_lapsed(gps_fuse_index) = local_time;
|
||||
output.innovations.vel_innov(gps_fuse_index,:) = velInnov';
|
||||
output.innovations.vel_innov_var(gps_fuse_index,:) = velInnovVar';
|
||||
|
||||
|
|
@ -149,9 +187,17 @@ for index = indexStart:indexStop
|
|||
[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.pos_time_lapsed(gps_fuse_index) = local_time;
|
||||
output.innovations.posInnov(gps_fuse_index,:) = posInnov';
|
||||
output.innovations.posInnovVar(gps_fuse_index,:) = posInnovVar';
|
||||
else
|
||||
% Check if drift is being corrected by some form of aiding and if not, fuse in a zero position measurement at 5Hz to prevent states diverging
|
||||
if ((local_time - last_drift_constrain_time) > param.control.velDriftTimeLim)
|
||||
if ((local_time - last_synthetic_velocity_fusion_time) > 0.2)
|
||||
[states,covariance,~,~] = FusePosition(states,covariance,zeros(1,2),100.0,param.control.gpsPosErrLim);
|
||||
last_synthetic_velocity_fusion_time = local_time;
|
||||
end
|
||||
end
|
||||
end
|
||||
|
||||
% Fuse new Baro data that has fallen beind the fusion time horizon
|
||||
|
|
@ -164,7 +210,7 @@ for index = indexStart:indexStop
|
|||
[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.hgt_time_lapsed(baro_fuse_index) = local_time;
|
||||
output.innovations.hgtInnov(baro_fuse_index) = hgtInnov;
|
||||
output.innovations.hgtInnovVar(baro_fuse_index) = hgtInnovVar;
|
||||
end
|
||||
|
|
@ -183,7 +229,7 @@ for index = indexStart:indexStop
|
|||
[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.mag_time_lapsed(mag_fuse_index) = local_time;
|
||||
output.innovations.magInnov(mag_fuse_index,:) = magInnov;
|
||||
output.innovations.magInnovVar(mag_fuse_index,:) = magInnovVar;
|
||||
|
||||
|
|
@ -198,7 +244,7 @@ for index = indexStart:indexStop
|
|||
[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.mag_time_lapsed(mag_fuse_index) = local_time;
|
||||
output.innovations.hdgInnov(mag_fuse_index) = hdgInnov;
|
||||
output.innovations.hdgInnovVar(mag_fuse_index) = hdgInnovVar;
|
||||
|
||||
|
|
@ -206,20 +252,29 @@ for index = indexStart:indexStop
|
|||
|
||||
end
|
||||
|
||||
% Attempt to use optical flow and range finder data if available
|
||||
if (useOpticalFlow)
|
||||
% Check if optical flow use is being blocked by the user
|
||||
if ((local_time < param.control.flowOnTime) && (local_time > param.control.flowOffTime))
|
||||
flow_use_blocked = true;
|
||||
else
|
||||
flow_use_blocked = false;
|
||||
end
|
||||
|
||||
% Attempt to use optical flow and range finder data if available and not blocked
|
||||
if (flowDataPresent && ~flow_use_blocked)
|
||||
|
||||
% Get latest range finder data and gate to remove dropouts
|
||||
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);
|
||||
last_range_index = find((rng_data.time_us - 1e6 * param.fusion.rangeTimeDelay) < imu_data.time_us(imuIndex), 1, 'last' );
|
||||
if (rng_data.dist(last_range_index) < param.fusion.rngValidMin && rng_data.dist(last_range_index) > param.fusion.rngValidMin)
|
||||
range = rng_data.dist(last_range_index);
|
||||
last_valid_range_time = local_time;
|
||||
end
|
||||
|
||||
% Fuse optical flow data that has fallen behind the fusion time horizon
|
||||
% Fuse optical flow data that has fallen behind the fusion time horizon if we have a valid range measurement
|
||||
latest_flow_index = find((flow_data.time_us - 1e6 * param.fusion.flowTimeDelay) < imu_data.time_us(imuIndex), 1, 'last' );
|
||||
if (latest_flow_index > last_flow_index)
|
||||
if ((latest_flow_index > last_flow_index) && ((local_time - last_valid_range_time) < param.fusion.rngTimeout))
|
||||
last_flow_index = latest_flow_index;
|
||||
flow_fuse_index = flow_fuse_index + 1;
|
||||
last_drift_constrain_time = local_time;
|
||||
|
||||
% fuse flow data
|
||||
flowRate = [flow_data.flowX(latest_flow_index);flow_data.flowY(latest_flow_index)];
|
||||
|
|
@ -227,7 +282,7 @@ for index = indexStart:indexStop
|
|||
[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.flow_time_lapsed(flow_fuse_index) = local_time;
|
||||
output.innovations.flowInnov(flow_fuse_index,:) = flowInnov;
|
||||
output.innovations.flowInnovVar(flow_fuse_index,:) = flowInnovVar;
|
||||
|
||||
|
|
@ -235,14 +290,22 @@ for index = indexStart:indexStop
|
|||
|
||||
end
|
||||
|
||||
% attempt to use ZED camera visual odmetry data if available
|
||||
if (useVisualOdometry)
|
||||
% Check if optical flow use is being blocked by the user
|
||||
if ((local_time < param.control.visoOnTime) && (local_time > param.control.visoOffTime))
|
||||
viso_use_blocked = true;
|
||||
else
|
||||
viso_use_blocked = false;
|
||||
end
|
||||
|
||||
% attempt to use ZED camera visual odmetry data if available and not blocked
|
||||
if (visoDataPresent && ~viso_use_blocked)
|
||||
|
||||
% Fuse ZED camera body frame odmometry data that has fallen behind the fusion time horizon
|
||||
latest_viso_index = find((viso_data.time_us - 1e6 * param.fusion.bodyVelTimeDelay) < imu_data.time_us(imuIndex), 1, 'last' );
|
||||
if (latest_viso_index > last_viso_index)
|
||||
last_viso_index = latest_viso_index;
|
||||
viso_fuse_index = viso_fuse_index + 1;
|
||||
last_drift_constrain_time = local_time;
|
||||
|
||||
% convert delta 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);
|
||||
|
|
@ -256,7 +319,7 @@ for index = indexStart:indexStop
|
|||
[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.bodyVel_time_lapsed(viso_fuse_index) = local_time;
|
||||
output.innovations.bodyVelInnov(viso_fuse_index,:) = bodyVelInnov;
|
||||
output.innovations.bodyVelInnovVar(viso_fuse_index,:) = bodyVelInnovVar;
|
||||
|
||||
|
|
|
|||
|
|
@ -1,3 +1,14 @@
|
|||
%% Filter Control
|
||||
param.control.waitForGps = 0; % set to 1 if the filter start should be delayed until GPS checks to pass
|
||||
param.control.gpsSpdErrLim = 1.0; % GPS use will not start if reported GPS speed error is greater than this (m/s)
|
||||
param.control.gpsPosErrLim = 5.0; % GPS use will not start if reported GPS position error is greater than this (m)
|
||||
param.control.velDriftTimeLim = 5.0; % The maximum time without observations to constrain velocity drift before a zero velocity is fused to prevent the filter diverging (sec)
|
||||
param.control.gpsOffTime = 0; % GPS aiding will be turned off at this time (sec)
|
||||
param.control.gpsOnTime = 0; % GPS aiding will be turned back on at this time (sec)
|
||||
param.control.flowOffTime = 0; % optical flow aiding will be turned off at this time (sec)
|
||||
param.control.flowOnTime = 0; % optical flow aiding will be turned back on on at this time (sec)
|
||||
param.control.visoOffTime = 0; % visual odometry aiding will be turned off at this time (sec)
|
||||
param.control.visoOnTime = 0; % visual odometry aiding will be turned back on at this time (sec)
|
||||
|
||||
%% GPS fusion
|
||||
param.fusion.gpsTimeDelay = 0.1; % GPS measurement delay relative to IMU (sec)
|
||||
|
|
@ -26,11 +37,14 @@ param.fusion.rangeTimeDelay = 0.05; % range fidner sensor delay relative to IMU
|
|||
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
|
||||
param.fusion.rngValidMin = 0.05; % ignore range measurements smaller than this (m)
|
||||
param.fusion.rngValidMin = 5.0; % ignore range measurements larger than this (m)
|
||||
param.fusion.rngTimeout = 2.0; % optical flow measurements will not be used if more than this time since valid range finder data was received (sec)
|
||||
|
||||
%% Body frame velocity measurement fusion
|
||||
%% Visual odometry body frame velocity measurement fusion
|
||||
param.fusion.bodyVelTimeDelay = 0.01; % Optical flow sensor time delay relative to IMU (sec)
|
||||
param.fusion.bodyVelErrorMin = 0.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.bodyVelErrorMin = 0.2; % Observation noise 1SD for the odometry sensor at the highest quality value (m/sec)
|
||||
param.fusion.bodyVelErrorMax = 1.8; % 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
|
||||
|
|
|
|||
|
|
@ -1,13 +1,16 @@
|
|||
clear all;
|
||||
close all;
|
||||
|
||||
% add required paths
|
||||
addpath('../Common');
|
||||
|
||||
% load compulsory data
|
||||
load '../TestData/APM/baro_data.mat';
|
||||
load '../TestData/APM/gps_data.mat';
|
||||
load '../TestData/APM/imu_data.mat';
|
||||
load '../TestData/APM/mag_data.mat';
|
||||
|
||||
% load data required for optical flow replay
|
||||
% load optional data required for optical flow replay
|
||||
if exist('../TestData/APM/rng_data.mat','file') && exist('../TestData/APM/flow_data.mat','file')
|
||||
load '../TestData/APM/rng_data.mat';
|
||||
load '../TestData/APM/flow_data.mat';
|
||||
|
|
@ -16,19 +19,23 @@ else
|
|||
flow_data = [];
|
||||
end
|
||||
|
||||
% load data required for ZED camera replay
|
||||
% load optional data required for ZED camera replay
|
||||
if exist('../TestData/APM/viso_data.mat','file')
|
||||
load '../TestData/APM/viso_data.mat';
|
||||
else
|
||||
viso_data = [];
|
||||
end
|
||||
|
||||
% load default parameters
|
||||
run('SetParameterDefaults.m');
|
||||
|
||||
% run the filter replay
|
||||
output = RunFilter(param,imu_data,mag_data,baro_data,gps_data,rng_data,flow_data,viso_data);
|
||||
|
||||
% genrate and save output plots
|
||||
PlotData(output);
|
||||
|
||||
% save output data
|
||||
folder = '../OutputData';
|
||||
fileName = '../OutputData/ekf_replay_output.mat';
|
||||
if ~exist(folder,'dir')
|
||||
|
|
|
|||
|
|
@ -1,16 +1,25 @@
|
|||
clear all;
|
||||
close all;
|
||||
|
||||
% add required paths
|
||||
addpath('../Common');
|
||||
|
||||
% load test data
|
||||
load '../TestData/PX4/baro_data.mat';
|
||||
load '../TestData/PX4/gps_data.mat';
|
||||
load '../TestData/PX4/imu_data.mat';
|
||||
load '../TestData/PX4/mag_data.mat';
|
||||
|
||||
% set parameters to default values
|
||||
run('SetParameterDefaults.m');
|
||||
|
||||
% run the filter replay
|
||||
output = RunFilter(param,imu_data,mag_data,baro_data,gps_data);
|
||||
|
||||
% generate and save output plots
|
||||
PlotData(output);
|
||||
|
||||
% save output data
|
||||
folder = '../OutputData';
|
||||
fileName = '../OutputData/ekf_replay_output.mat';
|
||||
if ~exist(folder,'dir')
|
||||
|
|
|
|||
|
|
@ -1,7 +1,6 @@
|
|||
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.
|
||||
|
||||
1) Ensure the ‘EKF_replay’ directory is in a location you have full read and write access
|
||||
2) Create a ‘TestData’ sub-directory under the ‘EKF_replay’ directory
|
||||
|
||||
A sample dataset can be downloaded here: https://drive.google.com/file/d/0By4v2BuLAaCfSW9fWl9aSWNGbGs/view?usp=sharing
|
||||
|
|
|
|||
Loading…
Reference in New Issue