✅作者简介热爱科研的Matlab仿真开发者擅长毕业设计辅导、数学建模、数据处理、程序设计科研仿真。完整代码获取 定制创新 论文复现私信个人信条做科研博学之、审问之、慎思之、明辨之、笃行之是为博学慎思明辨笃行。 内容介绍微型飞行器MAVs已无处不在在复杂城市环境中其在检测、监控和配送等场景中的应用将愈发重要。这类环境下的导航对定位精度的要求远高于传统 GNSS 系统所能提供的水平。虽然MAVs通常配备惯性测量单元IMU但基于积分法的状态估计值易随时间产生漂移。我们研究了传感器融合技术以整合这些互补传感器。本项目中我们采用不变扩展卡尔曼滤波器InEKF来估算 MAV 在复杂城市环境中的位置并通过将估算结果与真实数据集进行比对来验证效果。⛳️ 运行结果 部分代码%for testingclcclearclose allpauseLen 0;%%Initializations%TODO: load data heredata 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 doesnt cause floating point problemsnumSteps 500000;%TODO largest timestamp in GPS file, divided by deltaT, cast to intresults 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)^2filter filter_initialization(initialStateMean, initialStateCov, Q);%IMU noise? do in filter initializationIMUIdx 1;GPSIdx 1;nextIMU IMUData(IMUIdx, :); %first IMU measurementnextGPS GPSData(GPSIdx, :); %first GPS measurement%plot ground truth, raw GPS data% plot ground truth positionsplot3(gt(:,2), gt(:,3), gt(:,4), .g)grid onhold on% plot gps positions% plot3(GPSData(:,2), GPSData(:,3), GPSData(:,4), .b)axis equalaxis vis3dcounter 0;MAXIGPS 2708;MAXIIMU 27050;isStart false;%% Functionfunction [] 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 参考文献更多免费数学建模和仿真教程关注领取