文章目录
程序介绍
程序为UAV 辅助 UGV 的二维协同定位仿真框架，用于验证在低精度GNSS条件下空地协同的定位效果，引入空中平台（UAV）测距测角信息，对地面移动载体（UGV）定位精度的提升效果。通过 UAV--UGV 的异构平台协同，利用空间几何优势弥补单一地面传感器观测能力不足的问题。
在仿真场景中，UAV 以固定高度进行圆周飞行，其轨迹和运动参数预先设定，用于持续对 UGV 提供距离与方位角观测。UGV 在地面进行连续运动，其真实轨迹由速度和随时间缓慢变化的航向角共同决定，以模拟实际行驶过程中存在的小幅转向行为。该设置保证了运动模型的连续性和一定非理想性，从而更贴近工程应用场景。
在滤波结构上，程序采用标准卡尔曼滤波框架进行信息融合。基于运动模型完成状态预测，按照"UAV 测量优先、GNSS 测量补充"的顺序进行两次观测更新
仿真过程记录了 UAV 轨迹、UGV 真实轨迹、滤波估计轨迹以及 GNSS 测量轨迹，并从空间轨迹、分轴位移和定位误差三个层面对结果进行可视化分析。通过对 EKF 融合结果与 GNSS 单独定位结果的误差统计对比，可以直观评估 UAV 辅助协同定位在平均误差、稳定性和极值误差方面的性能提升效果。
运行结果
轨迹图：
位移曲线：
位置误差曲线：
柱状图输出结果对比：
命令行输出结果对比：
MATLAB源代码
部分代码如下：
matlab
%% UAV辅助的UGV协同定位仿真
% UAV通过测距测角辅助UGV进行精确定位
% 作者：matlabfilter（V同号，可接导航、定位相关的代码定制）
% 2026-01-07/Ver1
clear; close all; clc;
rng(0);
%% 仿真参数设置
dt = 0.1; % 时间步长 (s)
T = 10; % 总仿真时间 (s)
steps = T/dt; % 仿真步数
% UAV参数
uav_altitude = 50; % 飞行高度 (m)
uav_orbit_radius = 30; % 轨道半径 (m)
uav_angular_vel = 0.2; % 角速度 (rad/s)
uav_pos = [50; 50]; % 初始位置 [x; y] (m)
% UGV参数
ugv_real_pos = [20; 20]; % 真实初始位置 (m)
ugv_est_pos = [20.5; 20.5]; % 估计初始位置 (m)
ugv_velocity = 5; % 速度 (m/s)
ugv_heading = pi/4; % 航向角 (rad)
% 噪声参数
noise_process = 0.1; % 过程噪声标准差
noise_measure_dist = 1; % 距离测量噪声标准差 (m)
noise_measure_ang = 0.1; % 角度测量噪声标准差 (rad)
noise_GNSS = 5; % GNSS噪声标准差 (m)完整代码下载链接：https://download.csdn.net/download/callmeup/92543195
如需帮助，或有导航、定位滤波相关的代码定制需求，请点击下方卡片联系作者