fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-10-13 20:03:33 +08:00
Code Issues Packages Projects Releases Wiki Activity

matlab: Update replay scripts

Browse Source 
Miscellaneous fixes and tuning changes.
Add plotting of Euler angle uncertainties.
This commit is contained in:
Paul Riseborough
2017-07-16 12:56:02 +10:00
parent 667517a9b6
commit e2a5e57e14
15 changed files with 211 additions and 39 deletions
Download Patch File Download Diff File Expand all files Collapse all files
matlab/EKF_replay/Common/convert_apm_data.m
Regular → Executable
+6 -6
View File
@@ -74,12 +74,12 @@ flow_data.bodyY = OF(:,7); % angular rate about the Y body axis (rad/sec)
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.dAngX = VISO(:,4); % delta angle about the X body axis (rad)
viso_data.dAngY = VISO(:,5); % delta angle about the Y body axis (rad)
viso_data.dAngZ = VISO(:,6); % delta angle about the Z body axis (rad)
viso_data.dPosX = VISO(:,7); % delta position along the X body axis (m)
viso_data.dPosY = VISO(:,8); % delta position along the Y body axis (m)
viso_data.dPosZ = VISO(:,9); % delta position along the Z body axis (m)
viso_data.qual = VISO(:,10)/100; % quality from 0 - 1
%% save data and clear workspace
matlab/EKF_replay/Common/convert_px4_distance_sensor_csv_data.m
Regular → Executable
View File
matlab/EKF_replay/Common/convert_px4_optical_flow_csv_data.m
Regular → Executable
View File
matlab/EKF_replay/Common/convert_px4_sensor_combined_csv_data.m
Regular → Executable
+9 -16
View File
@@ -12,25 +12,18 @@ for source_index = 1:length(timestamp)
end
end
%% convert IMU data to delta angles and velocities using trapezoidal integration
%% convert IMU data to delta angles and velocities
% Note: these quatntis were converted from delta angles and velocites using
% the integral_dt values in the PX4 sensor module so we only need to
% multiply by integral_dt to convert back
clear imu_data;
n_samples = length(timestamp);
imu_data.time_us = timestamp(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.time_us = timestamp + accelerometer_timestamp_relative;
imu_data.gyro_dt = gyro_integral_dt;
imu_data.del_ang = [gyro_rad0.*imu_data.gyro_dt, gyro_rad1.*imu_data.gyro_dt, gyro_rad2.*imu_data.gyro_dt];
imu_data.accel_dt = accelerometer_integral_dt(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]);
imu_data.accel_dt = accelerometer_integral_dt;
imu_data.del_vel = [accelerometer_m_s20.*imu_data.accel_dt, accelerometer_m_s21.*imu_data.accel_dt, accelerometer_m_s22.*imu_data.accel_dt];
%% convert magnetomer data
clear mag_data;
matlab/EKF_replay/Common/convert_px4_vehicle_gps_position_csv.m
Regular → Executable
+4 -2
View File
@@ -7,9 +7,11 @@ gps_data.hgt_error = epv;
% set reference point used to set NED origin when GPS accuracy is sufficient
gps_data.start_index = max(min(find(gps_data.pos_error < 5.0)),min(find(gps_data.spd_error < 1.0)));
if isempty(gps_data.start_index)
gps_data.start_index = find(gps_data.pos_error == min(gps_data.pos_error));
gps_data.start_index = 1;
gps_data.refLLH = [1e-7*median(lat);1e-7*median(lon);0.001*median(alt)];
else
gps_data.refLLH = [1e-7*lat(gps_data.start_index);1e-7*lon(gps_data.start_index);0.001*alt(gps_data.start_index)];
end
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)
matlab/EKF_replay/Filter/GenerateEquations24.m
Regular → Executable
+11 -3
View File
@@ -83,9 +83,7 @@ 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
%% derive the state transition and state error matrix
% Define the control (disturbance) vector. Error growth in the inertial
% solution is assumed to be driven by 'noise' in the delta angles and
@@ -201,3 +199,13 @@ declination = atan(magE/magN);
T_MAG = jacobian(declination,[magN,magE]);
f = matlabFunction(T_MAG,'file','transfer_matrix.m');
%% calculate Quaternion to Euler angle error transfer matrix
% quat = [q0;q1;q2;q3];
% syms roll pitch yaw 'real';
roll = atan2(2*(quat(3)*quat(4)+quat(1)*quat(2)) , (quat(1)*quat(1) - quat(2)*quat(2) - quat(3)*quat(3) + quat(4)*quat(4)));
pitch = -asin(2*(quat(2)*quat(4)-quat(1)*quat(3)));
yaw = atan2(2*(quat(2)*quat(3)+quat(1)*quat(4)) , (quat(1)*quat(1) + quat(2)*quat(2) - quat(3)*quat(3) - quat(4)*quat(4)));
euler = [roll;pitch;yaw];
error_transfer_matrix = jacobian(euler,quat);
matlabFunction(error_transfer_matrix,'file','quat_to_euler_error_transfer_matrix.m');
matlab/EKF_replay/Filter/PlotData.m
Regular → Executable
+6 -3
View File
@@ -15,7 +15,7 @@ set(h,'PaperPosition', [0 0 1 1]);
margin = 5;
subplot(3,1,1);
plot(output.time_lapsed,output.euler_angles(:,1)*rad2deg);
plot(output.time_lapsed,[output.euler_angles(:,1)*rad2deg,output.euler_angles(:,1)*rad2deg-2*sqrt(output.euler_variances(:,1)*rad2deg),output.euler_angles(:,1)*rad2deg+2*sqrt(output.euler_variances(:,1)*rad2deg)]);
minVal = rad2deg*min(output.euler_angles(:,1))-margin;
maxVal = rad2deg*max(output.euler_angles(:,1))+margin;
ylim([minVal maxVal]);
@@ -24,24 +24,27 @@ titleText=strcat({'Euler Angle Estimates'},runIdentifier);
title(titleText);
ylabel('Roll (deg)');
xlabel('time (sec)');
legend('estimate','upper 95% bound','lower 95% bound');
subplot(3,1,2);
plot(output.time_lapsed,output.euler_angles(:,2)*rad2deg);
plot(output.time_lapsed,[output.euler_angles(:,2)*rad2deg,output.euler_angles(:,2)*rad2deg-2*sqrt(output.euler_variances(:,2)*rad2deg),output.euler_angles(:,2)*rad2deg+2*sqrt(output.euler_variances(:,2)*rad2deg)]);
minVal = rad2deg*min(output.euler_angles(:,2))-margin;
maxVal = rad2deg*max(output.euler_angles(:,2))+margin;
ylim([minVal maxVal]);
grid on;
ylabel('Pitch (deg)');
xlabel('time (sec)');
legend('estimate','upper 95% bound','lower 95% bound');
subplot(3,1,3);
plot(output.time_lapsed,output.euler_angles(:,3)*rad2deg);
plot(output.time_lapsed,[output.euler_angles(:,3)*rad2deg,output.euler_angles(:,3)*rad2deg-2*sqrt(output.euler_variances(:,3)*rad2deg),output.euler_angles(:,3)*rad2deg+2*sqrt(output.euler_variances(:,3)*rad2deg)]);
minVal = rad2deg*min(output.euler_angles(:,3))-margin;
maxVal = rad2deg*max(output.euler_angles(:,3))+margin;
ylim([minVal maxVal]);
grid on;
ylabel('Yaw (deg)');
xlabel('time (sec)');
legend('estimate','upper 95% bound','lower 95% bound');
fileName='euler_angle_estimates.png';
fullFileName = fullfile(folder, fileName);
matlab/EKF_replay/Filter/PredictStates.m
Regular → Executable
+5 -3
View File
@@ -33,8 +33,9 @@ delVel = delVel - states(14:16);
% Savage, P.G.
% Strapdown Associates
% 2015, WBN-14010
correctedDelVel= delVel + ...
0.5*cross(prevDelAng + delAng , prevDelVel + delVel) + 1/6*cross(prevDelAng + delAng , cross(prevDelAng + delAng , prevDelVel + delVel)) + 1/12*(cross(prevDelAng , delVel) + cross(prevDelVel , delAng));
% correctedDelVel= delVel + ...
% 0.5*cross(prevDelAng + delAng , prevDelVel + delVel) + 1/6*cross(prevDelAng + delAng , cross(prevDelAng + delAng , prevDelVel + delVel)) + 1/12*(cross(prevDelAng , delVel) + cross(prevDelVel , delAng));
correctedDelVel= delVel;
% Calculate earth delta angle spin vector
delAngEarth_NED(1,1) = 0.000072921 * cos(latitude) * dt;
@@ -47,7 +48,8 @@ delAngEarth_NED(3,1) = -0.000072921 * sin(latitude) * dt;
% R. B. MILLER,
% Journal of Guidance, Control, and Dynamics
% July, Vol. 6, No. 4, pp. 287-291, Eqn 11
correctedDelAng = delAng - 1/12*cross(prevDelAng , delAng) - transpose(Tbn_prev)*delAngEarth_NED;
% correctedDelAng = delAng - 1/12*cross(prevDelAng , delAng) - transpose(Tbn_prev)*delAngEarth_NED;
correctedDelAng = delAng - transpose(Tbn_prev)*delAngEarth_NED;
% Save current measurements
prevDelAng = delAng;
matlab/EKF_replay/Filter/RunFilter.m
Regular → Executable
+8 -1
View File
@@ -148,6 +148,13 @@ for index = indexStart:indexStop
output.state_variances(covIndex,i) = covariance(i,i);
end
% output equivalent euler angle variances
error_transfer_matrix = quat_to_euler_error_transfer_matrix(states(1),states(2),states(3),states(4));
euler_covariance_matrix = error_transfer_matrix * covariance(1:4,1:4) * transpose(error_transfer_matrix);
for i=1:3
output.euler_variances(covIndex,i) = euler_covariance_matrix(i,i);
end
% Get most recent GPS data that had fallen behind the fusion time horizon
latest_gps_index = find((gps_data.time_us - 1e6 * param.fusion.gpsTimeDelay) < imu_data.time_us(imuIndex), 1, 'last' );
@@ -309,7 +316,7 @@ for index = indexStart:indexStop
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);
relVelBodyMea = [viso_data.dPosX(latest_viso_index);viso_data.dPosY(latest_viso_index);viso_data.dPosZ(latest_viso_index)]./viso_data.dt(latest_viso_index);
% convert quality metric to equivalent observation error
% (this is a guess)
matlab/EKF_replay/Filter/SetParameterDefaults.m
Regular → Executable
+3 -3
View File
@@ -36,7 +36,7 @@ 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.1; % Observation noise 1SD for the flow sensor (rad/sec)
param.fusion.flowRateError = 0.15; % Observation noise 1SD for the flow sensor (rad/sec)
param.fusion.flowGate = 5.0; % Size of the optical flow rate innovation consistency check gate in SD
param.fusion.rngValidMin = 0.05; % range measurements wil be constrained to be no less than this (m)
param.fusion.rngValidMax = 5.0; % ignore range measurements larger than this (m)
@@ -44,8 +44,8 @@ param.fusion.rngTimeout = 2.0; % optical flow measurements will not be used if m
%% 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.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.bodyVelErrorMin = 0.1; % Observation noise 1SD for the odometry sensor at the highest quality value (m/sec)
param.fusion.bodyVelErrorMax = 0.9; % Observation noise 1SD for the odometry sensor at the lowest quality value (m/sec)
param.fusion.bodyVelGate = 5.0; % Size of the optical flow rate innovation consistency check gate in SD
%% State prediction error growth
matlab/EKF_replay/Filter/SetParameters.m
Executable
+69
View File
@@ -0,0 +1,69 @@
%% Filter Control
param.control.waitForGps = 0; % set to 1 if the filter start should be delayed until GPS checks to pass
param.control.gpsSpdErrLim = 1.0; % GPS use will not start if reported GPS speed error is greater than this (m/s)
param.control.gpsPosErrLim = 5.0; % GPS use will not start if reported GPS position error is greater than this (m)
param.control.velDriftTimeLim = 5.0; % The maximum time without observations to constrain velocity drift before a zero velocity is fused to prevent the filter diverging (sec)
param.control.gpsOffTime = 0; % GPS aiding will be turned off at this time (sec)
param.control.gpsOnTime = 0; % GPS aiding will be turned back on at this time (sec)
param.control.flowOffTime = 0; % optical flow aiding will be turned off at this time (sec)
param.control.flowOnTime = 0; % optical flow aiding will be turned back on on at this time (sec)
param.control.visoOffTime = 0; % visual odometry aiding will be turned off at this time (sec)
param.control.visoOnTime = 0; % visual odometry aiding will be turned back on at this time (sec)
param.control.rollAlignErr = 0.0; % initial roll misalignment (rad)
param.control.pitchAlignErr = 0.0; % initial pitch misalignment (rad)
param.control.yawAlignErr = 0.0; % initial yaw misalignment (rad)
%% GPS fusion
param.fusion.gpsTimeDelay = 0.1; % GPS measurement delay relative to IMU (sec)
param.fusion.gpsVelGate = 5.0; % Size of the IMU velocity innovation consistency check gate in SD
param.fusion.gpsPosGate = 5.0; % Size of the IMU velocity innovation consistency check gate in SD
param.fusion.gpsCheckTimeout = 10.0; % Length of time that GPS measurements will be rejected by the filter before states are reset to the GPS velocity. (sec)
%% Baro fusion
param.fusion.baroTimeDelay = 0.05; % Baro measurement delay relative to IMU (sec)
param.fusion.baroHgtGate = 5.0; % Size of the IMU velocity innovation consistency check gate in SD
param.fusion.baroHgtNoise = 2.0; % 1SD observation noise of the baro measurements (m)
%% Magnetometer measurement fusion
param.fusion.magTimeDelay = 0.0; % 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.15; % Observation noise 1SD for the flow sensor (rad/sec)
param.fusion.flowGate = 5.0; % Size of the optical flow rate innovation consistency check gate in SD
param.fusion.rngValidMin = 0.05; % range measurements wil be constrained to be no less than this (m)
param.fusion.rngValidMax = 5.0; % ignore range measurements larger than this (m)
param.fusion.rngTimeout = 2.0; % optical flow measurements will not be used if more than this time since valid range finder data was received (sec)
%% Visual odometry body frame velocity measurement fusion
param.fusion.bodyVelTimeDelay = 0.01; % Optical flow sensor time delay relative to IMU (sec)
param.fusion.bodyVelErrorMin = 0.1; % Observation noise 1SD for the odometry sensor at the highest quality value (m/sec)
param.fusion.bodyVelErrorMax = 0.9; % Observation noise 1SD for the odometry sensor at the lowest quality value (m/sec)
param.fusion.bodyVelGate = 5.0; % Size of the optical flow rate innovation consistency check gate in SD
%% State prediction error growth
param.prediction.magPnoiseNED = 1e-3; % Earth magnetic field 1SD rate of change. (gauss/sec)
param.prediction.magPnoiseXYZ = 1e-3; % Body magnetic field 1SD rate of change. (gauss/sec)
param.prediction.dAngBiasPnoise = 0.001; % IMU gyro bias 1SD rate of change (rad/sec^2)
param.prediction.dVelBiasPnoise = 0.03; % IMU accel bias 1SD rate of change (m/sec^3)
param.prediction.angRateNoise = 0.015; % IMU gyro 1SD rate process noise (rad/sec)
param.prediction.accelNoise = 0.35; % IMU accelerometer 1SD error noise including switch on bias uncertainty. (m/sec^2)
%% Initial Uncertainty
param.alignment.posErrNE = 10.0; % Initial 1SD position error when aligning without GPS. (m/sec)
param.alignment.velErrNE = 5.0; % Initial 1SD velocity error when aligning without GPS. (m/sec)
param.alignment.velErrD = 1.0; % Initial 1SD vertical velocity error when aligning without GPS. (m/sec)
param.alignment.delAngBiasErr = 0.05*pi/180; % Initial 1SD rate gyro bias uncertainty. (rad/sec)
param.alignment.delVelBiasErr = 0.07; % Initial 1SD accelerometer bias uncertainty. (m/sec^2)
param.alignment.quatErr = 0.1; % Initial 1SD uncertainty in quaternion.
param.alignment.magErrXYZ = 0.01; % Initial 1SD uncertainty in body frame XYZ magnetic field states. (gauss)
param.alignment.magErrNED = 0.5; % Initial 1SD uncertainty in earth frame NED magnetic field states. (gauss)
param.alignment.hgtErr = 0.5; % Initial 1SD uncertainty in height. (m)
param.alignment.windErrNE = 5.0; % Initial 1SD error in wind states. (m/sec)
matlab/EKF_replay/Filter/find_best_gps_delay.m
Executable
+50
View File
@@ -0,0 +1,50 @@
clear all;
close all;
% add required paths
addpath('../Common');
% load test data
load '../TestData/PX4/baro_data.mat';
load '../TestData/PX4/gps_data.mat';
load '../TestData/PX4/imu_data.mat';
load '../TestData/PX4/mag_data.mat';
% set parameters to default values
run('SetParameterDefaults.m');
% Loop through range of GPS time delays and capture the RMS velocity
% innovation and corresponding delay each time
for index = 1:1:21
index
param.fusion.gpsTimeDelay = 0.01*(index - 1);
output = RunFilter(param,imu_data,mag_data,baro_data,gps_data);
gps_delay(index) = param.fusion.gpsTimeDelay;
gps_vel_innov_rms(index) = sqrt(sum(output.innovations.vel_innov(:,1).^2) + sum(output.innovations.vel_innov(:,2).^2))/sqrt(length(output.innovations.vel_innov));
end
% plot the innvation level vs time delay
figure;
plot(gps_delay,gps_vel_innov_rms);
grid on;
xlabel('GPS delay (sec)');
ylabel('RMS velocity innovation (m/s)');
% find the smallest value and re-run the replay
param.fusion.gpsTimeDelay = gps_delay(gps_delay == min(gps_delay))
% run the filter replay
output = RunFilter(param,imu_data,mag_data,baro_data,gps_data);
% generate and save output plots
runIdentifier = ' : PX4 data replay ';
folder = strcat('../OutputPlots/PX4');
PlotData(output,folder,runIdentifier);
% save output data
folder = '../OutputData/PX4';
fileName = '../OutputData/PX4/ekf_replay_output.mat';
if ~exist(folder,'dir')
mkdir(folder);
end
save(fileName,'output');
matlab/EKF_replay/Filter/quat_to_euler_error_transfer_matrix.m
+38
View File
@@ -0,0 +1,38 @@
function error_transfer_matrix = quat_to_euler_error_transfer_matrix(q0,q1,q2,q3)
%QUAT_TO_EULER_ERROR_TRANSFER_MATRIX
% ERROR_TRANSFER_MATRIX = QUAT_TO_EULER_ERROR_TRANSFER_MATRIX(Q0,Q1,Q2,Q3)
% This function was generated by the Symbolic Math Toolbox version 6.2.
% 13-Jul-2017 08:41:41
t8 = q0.*q1.*2.0;
t9 = q2.*q3.*2.0;
t2 = t8+t9;
t4 = q0.^2;
t5 = q1.^2;
t6 = q2.^2;
t7 = q3.^2;
t3 = t4-t5-t6+t7;
t10 = t3.^2;
t11 = t2.^2;
t12 = t10+t11;
t13 = 1.0./t12;
t14 = 1.0./t3;
t15 = 1.0./t3.^2;
t17 = q0.*q2.*2.0;
t18 = q1.*q3.*2.0;
t16 = t17-t18;
t19 = t16.^2;
t20 = -t19+1.0;
t21 = 1.0./sqrt(t20);
t24 = q0.*q3.*2.0;
t25 = q1.*q2.*2.0;
t22 = t24+t25;
t23 = t4+t5-t6-t7;
t26 = t23.^2;
t27 = t22.^2;
t28 = t26+t27;
t29 = 1.0./t28;
t30 = 1.0./t23;
t31 = 1.0./t23.^2;
error_transfer_matrix = reshape([t10.*t13.*(q1.*t14.*2.0-q0.*t2.*t15.*2.0),q2.*t21.*2.0,t26.*t29.*(q3.*t30.*2.0-q0.*t22.*t31.*2.0),t10.*t13.*(q0.*t14.*2.0+q1.*t2.*t15.*2.0),q3.*t21.*-2.0,t26.*t29.*(q2.*t30.*2.0-q1.*t22.*t31.*2.0),t10.*t13.*(q3.*t14.*2.0+q2.*t2.*t15.*2.0),q0.*t21.*2.0,t26.*t29.*(q1.*t30.*2.0+q2.*t22.*t31.*2.0),t10.*t13.*(q2.*t14.*2.0-q3.*t2.*t15.*2.0),q1.*t21.*-2.0,t26.*t29.*(q0.*t30.*2.0+q3.*t22.*t31.*2.0)],[3, 4]);
matlab/EKF_replay/Filter/replay_apm_data.m
Regular → Executable
+1 -1
View File
@@ -27,7 +27,7 @@ else
end
% load default parameters
run('SetParameterDefaults.m');
run('SetParameters.m');
% run the filter replay
output = RunFilter(param,imu_data,mag_data,baro_data,gps_data,rng_data,flow_data,viso_data);
matlab/EKF_replay/Filter/replay_px4_data.m
Regular → Executable
+1 -1
View File
@@ -11,7 +11,7 @@ load '../TestData/PX4/imu_data.mat';
load '../TestData/PX4/mag_data.mat';
% set parameters to default values
run('SetParameterDefaults.m');
run('SetParameters.m');
% run the filter replay
output = RunFilter(param,imu_data,mag_data,baro_data,gps_data);