【matlab例程】GNSS与UWB紧耦合滤波，二维滤波，可根据需要自行修改为一维和三维的情况，附下载链接

这是一段MATLAB代码，用于模拟GNSS（全球导航卫星系统）和UWB（超宽带）技术的融合定位系统。下面是对代码的分析和讲解：

程序介绍

参数设置

• dt: 时间步长（单位：秒）
• total_time: 总时长（单位：秒）
• gnss_noise: GNSS定位噪声（单位：米）
• uwb_noise: UWB测距噪声（单位：米）
• gnss_anchor: GNSS基站坐标（二维）
• uwb_anchors: UWB基站坐标（二维）
• true_pos: 目标初始位置（二维）
• true_vel: 目标速度（二维）

模拟传感器观测

代码模拟GNSS和UWB传感器观测：

• gnss_obs: GNSS观测位置序列（添加了噪声）
• uwb_dist: UWB测距序列（添加了噪声）

EKF初始化

代码初始化了扩展卡尔曼滤波器（EKF）：

• x: 状态向量（包含位置和速度）
• P: 状态协方差矩阵
• Q: 过程噪声矩阵
• pos_ekf: 存储滤波结果的位置序列

主循环

主循环执行以下步骤：

1. 预测步骤：使用状态转移矩阵预测状态向量和协方差矩阵。
2. GNSS更新：使用GNSS观测更新状态向量和协方差矩阵。
3. UWB更新：使用UWB测距更新状态向量和协方差矩阵（对于每个基站）。

可视化

代码最后可视化了真实轨迹、GNSS观测和融合结果：

• 使用plot函数绘制了真实轨迹、GNSS观测和融合结果。
• 使用legend函数显示了图例。

误差分析

代码最后计算了GNSS和融合结果的平均误差：

• 使用vecnorm函数计算了位置误差。
• 使用fprintf函数输出了平均误差。

总的来说，这段代码模拟了GNSS和UWB技术的融合定位系统，并使用扩展卡尔曼滤波器估计了目标位置和速度。结果显示了真实轨迹、GNSS观测和融合结果，并计算了平均误差。

运行结果

matlab代码

部分代码如下：

%% GNSS-UWB融合定位仿真系统，紧耦合
clc; clear; close all;

%% 参数设置
dt = 1;           % 时间步长(s)
total_time = 100; % 总时长(秒)
gnss_noise = 1.5;% GNSS定位噪声(m)
uwb_noise = 0.3;  % UWB测距噪声(m)

% 基站坐标(二维)
gnss_anchor = [0, 0];       % GNSS基站(参考点)
uwb_anchors = [100,0; 0,100; 100,100]; % UWB基站坐标

% 目标运动参数(匀速模型)
true_pos = [20, 50];        % 初始位置
true_vel = [1.2, -0.5];     % 速度(m/s)

%% 生成真实轨迹
time = 0:dt:total_time;
pos_true = zeros(length(time),2);
for t = 1:length(time)
    pos_true(t,:) = true_pos + true_vel*time(t); 
end

%% 模拟传感器观测
% GNSS观测(直接位置测量)

完整代码：https://download.youkuaiyun.com/download/callmeup/90541601

如需帮助，或有导航、定位滤波相关的代码定制需求，请点击下方卡片联系作者