【滤波跟踪】基于扩展卡尔曼滤波器从IMU和GPS数据中计算无人机的姿态附Matlab代码
✅作者简介：热爱科研的Matlab仿真开发者，擅长毕业设计辅导、数学建模、数据处理、程序设计科研仿真。
🍎完整代码获取 定制创新 论文复现私信
🍊个人信条：做科研，博学之、审问之、慎思之、明辨之、笃行之，是为：博学慎思，明辨笃行。
🔥 内容介绍
微型飞行器（MAVs）已无处不在，在复杂城市环境中，其在检测、监控和配送等场景中的应用将愈发重要。这类环境下的导航对定位精度的要求远高于传统 GNSS 系统所能提供的水平。虽然MAVs通常配备惯性测量单元（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;
%% Function
function []= plotPose(R, t, v)
v_scale = 0.1;
v = v.*v_scale;
x = t(1);
y = t(2);
z = t(3);
x_vec = R * [1; 0; 0];
y_vec = R * [0; 1; 0];
z_vec = R * [0; 0; 1];
vx = v(1);
vy = v(2);
vz = v(3);
quiver3(x, y, z, x_vec(1), x_vec(2), x_vec(3), 'r');
quiver3(x, y, z, y_vec(1), y_vec(2), y_vec(3), 'g');
quiver3(x, y, z, z_vec(1), z_vec(2), z_vec(3), 'b');
% quiver3(x, y, z, vx, vy, vz, 'k');
end
