✅作者简介热爱科研的Matlab仿真开发者擅长毕业设计辅导、数学建模、数据处理、程序设计科研仿真。完整代码获取 定制创新 论文复现点击Matlab科研工作室 关注我领取海量matlab电子书和数学建模资料个人信条做科研博学之、审问之、慎思之、明辨之、笃行之是为博学慎思明辨笃行。 内容介绍一、引言在现代监测与协同作业场景中无人机UAV对地面无人车UGV的精准跟踪至关重要。然而实际环境的复杂性如噪声干扰、UGV 运动模式的突然变化等给跟踪带来挑战。自适应卡尔曼滤波器AKF能依据系统实时信息自动调整滤波器参数有效应对这些复杂情况实现对 UGV 的可靠跟踪。二、系统模型建立一地面无人车运动模型⛳️ 运行结果 部分代码clear; clc;% Load dataload(ugv_ekf.mat);load(uav_ekf.mat);% Sampling parametersN length(vx_ugv);dt 0.025;% Initialize EKF state: [xr; yr; zr; wxr; wyr; wzr]x zeros(6, N);x(1:3, 1:2) repmat([99; 100; 50], 1, 2);P eye(6) * 5;% Q diag([0.006, 0.006, 0.006, 0.0005, 0.0005, 0.0005]);% R diag([0.9, 0.9, 0.9, 0.05, 0.05, 0.05]);% Adjusted Noise Matrices for NIS within boundsQ diag([0.01, 0.01, 0.01, 0.001, 0.001, 0.001]); % Process noiseR diag([2.0, 2.0, 2.0, 0.2, 0.2, 0.2]); % Measurement noiseQ diag([0.01, 0.01, 0.01, 0.001, 0.001, 0.001]); % Increased process noiseR diag([2.2, 2.2, 2.2, 0.08, 0.08, 0.08]); % Increased measurement noiseexecution_time zeros(1, N);nees zeros(1, N);nis zeros(1, N);% Measured data arraysrel_pose_meas zeros(3, N);rel_w_meas zeros(3, N);est_rel_pos zeros(3, N);est_rel_w zeros(3, N);total_timer tic;for k 2:Nstep_timer tic;% Measurementsrel_pose_meas(:,k) [gpsx_uav(k) - gpsx_ugv(k);gpsy_uav(k) - gpsy_ugv(k);gpsz_uav(k) - gpsz_ugv(k)];rel_w_meas(:,k) [wx_uav(k) - wx_ugv(k);wy_uav(k) - wy_ugv(k);wz_uav(k) - wz_ugv(k)];z [rel_pose_meas(:,k); rel_w_meas(:,k)];% EKF PredictionF eye(6);x_pred x(:,k-1);P_pred F * P * F Q;% EKF UpdateH eye(6);y z - H * x_pred;S H * P_pred * H R;K P_pred * H / S;x(:,k) x_pred K * y;P (eye(6) - K * H) * P_pred;% Store estimatesest_rel_pos(:,k) x(1:3,k);est_rel_w(:,k) x(4:6,k);% NEESe x(:,k) - z;nees(k) e * (P \ e);% NISnis(k) y * (S \ y); % Same as y * inv(S) * y% Execution time per iterationexecution_time(k) toc(step_timer);endtotal_exec_time toc(total_timer);fprintf(✅ Total EKF Execution Time: %.4f s\n, total_exec_time);% --- RMSE ---rmse_pos sqrt(mean((est_rel_pos - rel_pose_meas).^2, 2));rmse_w sqrt(mean((est_rel_w - rel_w_meas).^2, 2));fprintf( RMSE Position [m]: x%.3f, y%.3f, z%.3f\n, rmse_pos);fprintf( RMSE Angular Vel [rad/s]: wx%.3f, wy%.3f, wz%.3f\n, rmse_w);% --- Time Vector ---time (0:N-1) * dt;% --- Plot: Relative Pose ---figure;for i 1:3subplot(3,1,i);plot(time, rel_pose_meas(i,:), r--, LineWidth, 1.3); hold on;plot(time, est_rel_pos(i,:), b, LineWidth, 1.3);ylabel([Axis , xyz, (m)]);if i 1, title(Estimated vs Measured Relative Position); endlegend(Measured, Estimated); grid on;endxlabel(Time (s));% --- Plot: Angular Velocity ---figure;for i 1:3subplot(3,1,i);plot(time, rel_w_meas(i,:), r--, LineWidth, 1.3); hold on;plot(time, est_rel_w(i,:), b, LineWidth, 1.3);ylabel([w, xyz, (rad/s)]);if i 1, title(Estimated vs Measured Angular Velocity); endlegend(Measured, Estimated); grid on;endxlabel(Time (s));% --- NEES Plot ---figure;plot(time, nees, LineWidth, 1.2); hold on;dim 6; alpha 0.5;nees_upper chi2inv(1 - alpha/2, dim);nees_lower chi2inv(alpha/2, dim);yline(nees_upper, r--, LineWidth, 1);yline(nees_lower, r--, LineWidth, 1);xlabel(Time (s)); ylabel(NEES);title(Normalized Estimation Error Squared (NEES));legend(NEES, 95% Bounds); grid on;% --- NIS Plot ---figure;alpha 0.05plot(nis,b, LineWidth, 1.2); hold on;nis_upper chi2inv(1 - alpha/2, dim); % 95% CI for 6 DOFnis_lower chi2inv(alpha/2, dim);yline(nis_upper, r--, LineWidth, 1);yline(nis_lower, r--, LineWidth, 1);xlabel(Time (s)); ylabel(NIS);title(Normalized Innovation Squared (NIS));legend(NIS, 95% Bounds); grid on;% --- Bar Chart: Execution Time (First 5 Steps) ---figure;bar(1:5, execution_time(1:5), FaceColor, [0.2 0.4 0.6]);xlabel(Iteration);ylabel(Time (s));title(Execution Time for First 5 EKF Steps);grid on;% --- Execution Time Stats ---mean_time mean(execution_time);min_time min(execution_time);max_time max(execution_time);fprintf(⏱️ Mean Time: %.6f s | Min: %.6f s | Max: %.6f s\n, mean_time, min_time, max_time); 参考文献更多免费数学建模和仿真教程关注领取
