企业官网建设流程全解析

✅作者简介：热爱科研的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);​

🔗 参考文献

🍅更多免费数学建模和仿真教程关注领取