GNSS（GPS、北斗等）与UWB的融合定位例程，matlab，二维平面，使用卡尔曼滤波

基于GNSS（GPS/北斗）与UWB融合定位的探索方案及Matlab实现框架，结合二维平面场景设计。该方案综合了多传感器数据融合算法和工程实践经验

概述

系统架构

% 系统初始化参数
gnss_noise = 1.5;   % GNSS定位噪声标准差（米）
uwb_noise = 0.3;    % UWB测距噪声标准差（米）
sensor_pos = [0 0; 100 0];  % GNSS基站与UWB基站坐标（二维）

• 室外场景：优先使用GNSS定位，UWB作为辅助修正（通过协方差权重调整）
• 室内场景：依赖UWB与IMU组合，GNSS信号失效时启动补偿算法
• 过渡区域：采用动态权重融合策略（如卡尔曼滤波+协方差交集）

核心算法流程

% 数据预处理函数
function [position, covariance] = preprocess_data(gnss_data, uwb_data)
    % GNSS坐标转换（WGS84转平面坐标系）
    gnss_xy = wgs2utm(gnss_data.lat, gnss_data.lon);
    
    % UWB测距解算（三边定位）
    uwb_dist = uwb_data.distance;
    uwb_xy = trilateration(uwb_dist, sensor_pos);
    
    % 数据同步与对齐
    [synced_gnss, synced_uwb] = time_alignment(gnss_xy, uwb_xy);
end

• 坐标统一化：将GNSS的经纬度转换为平面坐标（UTM投影）
• 时间对齐：采用插值法解决传感器数据时间不同步问题
• 异常值剔除：基于卡方检验的残差分析法

滤波融合算法

% 扩展卡尔曼滤波（EKF）实现
function [fused_pos] = ekf_fusion(gnss, uwb)
    % 状态向量：x = [px, py, vx, vy]^T
    F = [1 0 dt 0; 0 1 0 dt; 0 0 1 0; 0 0 0 1]; % 状态转移矩阵
    H_gnss = [1 0 0 0; 0 1 0 0];               % GNSS观测矩阵
    H_uwb = @(x) compute_uwb_jacobian(x);       % UWB观测雅可比
    
    % 预测步骤
    x_pred = F * x_prev;
    P_pred = F * P_prev * F' + Q;
    
    % 更新步骤（GNSS）
    y_gnss = z_gnss - H_gnss * x_pred;
    S_gnss = H_gnss * P_pred * H_gnss' + R_gnss;
    K_gnss = P_pred * H_gnss' / S_gnss;
    
    % 更新步骤（UWB）
    y_uwb = z_uwb - h_uwb(x_pred);
    S_uwb = H_uwb * P_pred * H_uwb' + R_uwb;
    K_uwb = P_pred * H_uwb' / S_uwb;
    
    % 融合结果
    x_fused = x_pred + K_gnss*y_gnss + K_uwb*y_uwb;
end

• 误差状态卡尔曼滤波（ESKF）：更适合处理GNSS/UWB的非线性误差
• 协方差交集法：在传感器置信度未知时保证融合结果的保守性

Matlab例程

仿真数据生成

% 生成运动轨迹（匀速直线模型）
t = 0:0.1:60;       % 60秒仿真时长
true_x = 0.5*t;      % X方向速度0.5m/s
true_y = 2*sin(0.1*t); % Y方向正弦运动

% 添加噪声
gnss_x = true_x + gnss_noise*randn(size(t));
gnss_y = true_y + gnss_noise*randn(size(t));

% UWB测距模拟
uwb_dist = sqrt((true_x-sensor_pos(2,1)).^2 + (true_y-sensor_pos(2,2)).^2);
uwb_dist = uwb_dist + uwb_noise*randn(size(t));

可视化代码

figure;
plot(true_x, true_y, 'k-', 'LineWidth', 2); hold on;
plot(gnss_x, gnss_y, 'ro', 'MarkerSize', 4);
plot(fused_x, fused_y, 'b-', 'LineWidth', 1.5);
legend('真实轨迹', 'GNSS观测', '融合结果');
xlabel('X坐标 (m)'); ylabel('Y坐标 (m)');
title('GNSS/UWB融合定位效果');
grid on; axis equal;

性能评估指标

% 计算均方根误差
rmse_gnss = sqrt(mean((true_x - gnss_x).^2 + (true_y - gnss_y).^2));
rmse_fused = sqrt(mean((true_x - fused_x).^2 + (true_y - fused_y).^2));
fprintf('GNSS RMSE: %.2fm\nFusion RMSE: %.2fm\n', rmse_gnss, rmse_fused);

应用场景优化

1. 动态权重调整：根据卫星数（GNSS）和信噪比（UWB）实时调整融合权重
2. 多目标扩展：通过数据关联算法处理多个移动目标的跟踪问题
3. 抗多径优化：在UWB信号处理中引入RANSAC算法剔除异常测距值

完整代码可根据需要进行个性化定制。如需帮助，或有导航、定位滤波相关的代码定制需求，请点击下方卡片联系作者