随着微型飞行器(MA Vs)变得越来越便宜和普遍存在,它们在复杂的城市环境中的使用将变得更加需要诸如检查,监视和交付之类的用途。与传统GNSS系统相比,这些环境的导航要求更高的定位精度。虽然MA V通常包含惯性测量单位(IMU),但其基于积分的状态估计值容易随时间漂移。我们探索了传感器融合的用法,以结合这些互补传感器。在这个项目中,我们使用不变扩展卡尔曼滤波器(InEKF)来估计MAV在充满挑战的城市环境中的位置。我们通过将估计位置与地面真实数据集进行比较来评估结果。
%for testing
clc
clear
close all
pauseLen = 0;
%%Initializations
%TODO: load data here
data = load('lib/IMU_GPS_GT_data.mat');
IMUData = data.imu;
GPSData = data.gpsAGL;
gt = data.gt;
addpath([cd, filesep, 'lib'])
initialStateMean = eye(5);
initialStateCov = eye(9);
deltaT = 1 / 30; %hope this doesn't cause floating point problems
numSteps = 500000;%TODO largest timestamp in GPS file, divided by deltaT, cast to int
results = zeros(7, numSteps);
% time x y z Rx Ry Rz
% sys = system_initialization(deltaT);
Q = blkdiag(eye(3)*(0.35)^2, eye(3)*(0.015)^2, zeros(3));
%IMU noise characteristics
%Using default values from pixhawk px4 controller
%https://dev.px4.io/v1.9.0/en/advanced/parameter_reference.html
%accel: first three values, (m/s^2)^2
%gyro: next three values, (rad/s)^2
filter = filter_initialization(initialStateMean, initialStateCov, Q);
%IMU noise? do in filter initialization
IMUIdx = 1;
GPSIdx = 1;
nextIMU = IMUData(IMUIdx, :); %first IMU measurement
nextGPS = GPSData(GPSIdx, :); %first GPS measurement
%plot ground truth, raw GPS data
% plot ground truth positions
plot3(gt(:,2), gt(:,3), gt(:,4), '.g')
grid on
hold on
% plot gps positions
% plot3(GPSData(:,2), GPSData(:,3), GPSData(:,4), '.b')
axis equal
axis vis3d
counter = 0;
MAXIGPS = 2708;
MAXIIMU = 27050;
isStart = false;
for t = 1:numSteps
currT = t * deltaT;
if(currT >= nextIMU(1)) %if the next IMU measurement has happened
% disp('prediction')
filter.prediction(nextIMU(2:7));
isStart = true;
IMUIdx = IMUIdx + 1;
nextIMU = IMUData(IMUIdx, :);
% plot3(filter.mu(1, 5), filter.mu(2, 5), filter.mu(3, 5), 'or');
end
if(currT >= nextGPS(1) & isStart) %if the next GPS measurement has happened
% disp('correction')
counter = counter + 1;
filter.correction(nextGPS(2:4));
GPSIdx = GPSIdx + 1;
nextGPS = GPSData(GPSIdx, :);
plot3(nextGPS(2), nextGPS(3), nextGPS(4), '.r');
% plot3(filter.mu(1, 5), filter.mu(2, 5), filter.mu(3, 5), 'ok');
% plotPose(filter.mu(1:3, 1:3), filter.mu(1:3, 5), filter.mu(1:3,4));
end
results(1, t) = currT;
results(2:4, t) = filter.mu(1:3, 5); %just position so far
% plot3(results(2, t), results(3, t), results(4, t), 'or');
% disp(filter.mu(1:3, 1:3));
if pauseLen == inf
pause;
elseif pauseLen > 0
pause(pauseLen);
end
if IMUIdx >= MAXIIMU || GPSIdx >= MAXIGPS
break
end
end
plot3(results(2,:), results(3,:), results(4,:), '.b');
% xlim([-10 10]);
% ylim([-10 10]);
xlabel('x, m');
ylabel('y, m');
zlabel('z, m');
%% Evaluation
gps_score = evaluation(gt, GPSData)
results_eval = results.';
score = 0;
estimation_idx = 1;
count = 0;
for i = 2:length(gt)
score = score + norm(gt(i, 2:4) - results_eval(30 * (i-1), 2:4)) ^ 2;
count = count + 1;
end
count
score = sqrt(score / count)
%% Function
完整代码或者代写添加QQ1575304183