本本文所述程序实现了 水下机器人（AUV）长基线（LBL）+ IMU在三维空间中的定位融合，采用扩展卡尔曼滤波（EKF）方法来融合较高频率的IMU数据和较低频率的声学基站测距数据，从而获得更准确、稳定的轨迹估计结果。
文章目录
程序简介
核心特点
- 多传感器融合：LBL声学定位 + IMU惯性导航
- 实时滤波：扩展卡尔曼滤波器处理非线性观测
- 四面体基站配置：提供三维空间完整覆盖
- NED坐标系：符合海洋工程标准的北东地坐标系
核心数学模型
- 系统状态模型
系统状态向量定义为12维：
x = [p_n, p_e, p_d, v_n, v_e, v_d, b_ax, b_ay, b_az, b_gx, b_gy, b_gz]^T其中：
[p_n, p_e, p_d]：北东地坐标系下的位置 (m)[v_n, v_e, v_d]：速度分量 (m/s)[b_ax, b_ay, b_az]：加速度计偏差 (m/s²)[b_gx, b_gy, b_gz]：陀螺仪偏差 (rad/s)
- 状态转移方程
预测模型 (恒速度模型)：
x_{k+1|k} = F_k · x_k + w_k状态转移矩阵：
F = [I_3 Δt·I_3 0_3 0_3]
[0_3 I_3 0_3 0_3]
[0_3 0_3 I_3 0_3]
[0_3 0_3 0_3 I_3]其中 I_3 为3×3单位矩阵，0_3 为3×3零矩阵，Δt 为采样间隔。
- LBL声学观测模型
距离观测方程：
r_i = ||p - p_i|| + v_r,i其中：
r_i：到第i个基站的距离测量值p = [p_n, p_e, p_d]^T：AUV当前位置p_i：第i个基站位置v_r,i：距离测量噪声
雅可比矩阵：
H_acoustic[i, 1:3] = (p - p_i)^T / ||p - p_i||- IMU观测模型
加速度计模型：
a_meas = R_b^n(a_true + g) + b_a + v_a陀螺仪模型：
ω_meas = ω_true + b_g + v_g其中：
R_b^n：体坐标系到导航坐标系的旋转矩阵g = [0, 0, 9.81]^T：重力加速度向量v_a, v_g：传感器噪声
- 扩展卡尔曼滤波递推公式
预测步骤：
x_{k|k-1} = f(x_{k-1|k-1})
P_{k|k-1} = F_k P_{k-1|k-1} F_k^T + Q_k更新步骤：
K_k = P_{k|k-1} H_k^T (H_k P_{k|k-1} H_k^T + R_k)^{-1}
x_{k|k} = x_{k|k-1} + K_k(z_k - h(x_{k|k-1}))
P_{k|k} = (I - K_k H_k) P_{k|k-1}- 长基线三维定位解算
采用最小二乘法求解AUV位置：
线性化方程组：
H · X = a其中：
H = [x_2-x_1, y_2-y_1, z_2-z_1]
[x_3-x_1, y_3-y_1, z_3-z_1]
[x_4-x_1, y_4-y_1, z_4-z_1]
a = [0.5(x_2²+y_2²+z_2²-r_2²-x_1²-y_1²-z_1²+r_1²)]
[0.5(x_3²+y_3²+z_3²-r_3²-x_1²-y_1²-z_1²+r_1²)]
[0.5(x_4²+y_4²+z_4²-r_4²-x_1²-y_1²-z_1²+r_1²)]解算公式：
X = (H^T H)^{-1} H^T a (伪逆解)系统参数配置
传感器规格
- IMU频率：100 Hz
- 声学定位频率：1 Hz
- 声速：1500 m/s
- 距离精度：20 cm (1σ)
- 加速度计精度：0.02 m/s² (1σ)
- 陀螺仪精度：0.01 rad/s (1σ)
噪声模型示例：
σ_range = 0.2/1500 s (距离测量噪声)
σ_acc = 0.002 m/s² (加速度计噪声)
σ_gyro = 0.001 rad/s (陀螺仪噪声)运行结果
轨迹图与锚点（基站）位置在三维空间中的绘图显示如下：
误差曲线：
XoY平面上的二维轨迹图：
命令行窗口输出的结果：
=== EKF融合定位性能评估 ===
3D RMSE: 7.392 m
平均误差: 5.406 m
最大误差: 33.617 m
误差标准差: 5.043 m
North RMSE: 3.300 m
East RMSE: 3.298 m
Down RMSE: 5.734 m
=== LBL纯声学定位性能 ===
3D RMSE: 29.133 m
平均误差: 22.820 m
最大误差: 76.001 m
=== 性能改进 ===
融合相对改进: 74.6%
EKF融合定位优于纯LBL定位
=== 系统配置信息 ===
仿真时长: 100.0 秒
IMU频率: 100 Hz
声学定位频率: 1.0 Hz
总计算点数: 10001
声学更新次数: 101
=== 滤波器一致性分析 ===
位置估计偏差 [N,E,D]: [-0.084, 0.058, -0.068] m
速度估计偏差 [N,E,D]: [-0.149, 0.248, -0.044] m/s
估计加速度偏差: [-7.0385, -3.3170, 19.4977] m/s²
估计陀螺偏差: [0.0000, 0.0000, 0.0000] rad/s
=== 收敛性分析 ===
初始位置误差: 1.826 m
最终位置误差: 0.387 m
误差收敛比: 0.21
滤波器成功收敛
>> MATLAB源代码
程序结构：
部分代码如下：
matlab
%% 水下机器人长基线(LBL)，三维轨迹定位，使用EKF滤波，融合LBL和IMU，4个锚点
% 适用于AUV自主导航与定位
% 作者: matlabfilter
% 2025-08-19/Ver1
clear; clc; close all;
rng(0);
% 水下长基线定位与IMU融合系统
% 使用扩展卡尔曼滤波进行多传感器融合
% 坐标系：NED (North-East-Down)
%% 系统参数设置
% 基站配置（四面体配置，单位：米）
beacon_positions = [
-500, -500, -50; % 基站1 (西北)
500, -500, -50; % 基站2 (东北)
500, 500, -50; % 基站3 (东南)
-500, 500, -60 % 基站4 (西南，深度不同)
];
% 仿真参数
dt_imu = 0.01; % IMU采样间隔 (100Hz)
dt_acoustic = 1; % 声学定位间隔
total_time = 100; % 总仿真时间 (秒)
sound_speed = 1500; % 声速 m/s
% 噪声参数（符合实际水下设备精度）
range_noise_std = 0.2/sound_speed; % 距离测量噪声标准差(20cm)
imu_acc_noise_std = 0.002; % 加速度计噪声 (0.02 m/s²)
imu_gyro_noise_std = 0.001; % 陀螺仪噪声 (0.01 rad/s)
imu_bias_acc = 0.01*[0.01, -0.02, 0.015]; % 加速度计偏差
%% 生成真实轨迹（复杂的水下探测轨迹）
t = 0:dt_imu:total_time;
n_steps = length(t);
% 轨迹参数
R = 150; % 基础螺旋半径
omega = 2*pi/40; % 螺旋角频率
descent_rate = -1.5; % 基础下沉速度 m/s
% 生成真实状态（位置、速度、加速度）
true_pos = zeros(n_steps, 3);
true_vel = zeros(n_steps, 3);
true_acc = zeros(n_steps, 3);
true_euler = zeros(n_steps, 3); % 欧拉角 (roll, pitch, yaw)
for i = 1:n_steps完整代码：
https://download.csdn.net/download/callmeup/91710148
或：
如需帮助，或有导航、定位滤波相关的代码定制需求，请点击下方卡片联系作者