本程序演示了利用 BP（Back Propagation）神经网络 对 USBL（超短基线定位系统） 的观测误差进行非线性补偿。附带与DVL（多普勒计程仪）的建模

程序介绍

在水下导航中，USBL提供绝对位置测量，而DVL提供速度信息，两者的观测值都受到姿态变化、声速分布和传感器安装误差等影响，导致输出存在非线性、时变误差。传统滤波算法难以显式建模这些复杂偏差，因此引入BP神经网络学习输入特征（如姿态、深度）与误差之间的隐含关系，实现误差估计与补偿，从而提高整体定位精度。

数据生成与初始化：

N = 20000;
t = linspace(0, 2000, N)';
pos_true = [50*sin(0.03*t), 30*cos(0.03*t)];

程序首先生成一段二维正弦轨迹作为真实运动路径，并计算对应速度。
同时构造姿态与环境特征（深度、横滚角、俯仰角、航向角），作为网络输入特征。

这些变量模拟了水下航行器的状态信息：

• depth：深度变化代表水层影响；
• roll / pitch / yaw：姿态角变化引起声学测量方向偏差；
• 这些特征共同构成误差的主要影响因子。

USBL与DVL测量误差建模：

usbl_err = [2*sin(0.05*t) + 0.5*randn(N,1), ...
            1.5*cos(0.04*t) + 0.5*randn(N,1)];
dvl_err = [0.2*sin(roll*pi/180) + 0.05*randn(N,1), ...
           0.3*cos(pitch*pi/180) + 0.05*randn(N,1)];

• USBL误差：包含非线性正弦分量与随机噪声，模拟声线折射及设备偏差；
• DVL误差：与姿态角耦合，反映传感器安装角误差和底质反射偏差。

生成的带噪观测：

pos_usbl = pos_true + usbl_err;
vel_dvl = vel_true + dvl_err;

构造训练数据集：

X = features';        % 输入特征
Yx = usbl_err(:,1)';  % USBL x方向误差
Yy = usbl_err(:,2)';  % USBL y方向误差

BP网络以 姿态和环境参数为输入，以 USBL观测误差为输出。
数据集按照 80% 训练、20% 测试比例划分。

BP神经网络训练：

hiddenLayerSize = 10;
net_x = fitnet(hiddenLayerSize, 'trainlm');
net_y = fitnet(hiddenLayerSize, 'trainlm');

采用MATLAB自带的 fitnet 函数构建BP网络，激活函数默认tansig，训练算法为 Levenberg–Marquardt（trainlm），迭代500次。

误差预测与补偿：

Yx_pred = net_x(X_test);
Yy_pred = net_y(X_test);
usbl_err_pred = [Yx_pred', Yy_pred'];
pos_usbl_corr = pos_usbl(idx_test,:) - usbl_err_pred;

网络预测出每个测试样本对应的USBL误差，并将其从原始测量中扣除，实现误差补偿。
得到的 pos_usbl_corr 即为补偿后的改进位置估计。

运行结果

误差补偿前后的轨迹对比：

定位误差对比：

误差输出：

MATLAB源代码

部分代码如下：

%%  基于BP神经网络的USBL误差补偿+DVL示例
% 非线性误差补偿
% 作者:matlabfilter
% 2025-10-12/Ver1

clear; clc; close all;% 清除变量、命令行和图形窗口
rng(0); % 设置随机数种子
%% 生成仿真数据 初始化
N = 20000;                  % 样本数量
t = linspace(0, 2000, N)';  % 时间序列

% ---- 真实轨迹（二维）
pos_true = [50*sin(0.03*t), 30*cos(0.03*t)];   % [x, y]
vel_true = gradient(pos_true) ./ gradient(t);   % 真实速度

订阅专栏后也可查看完整代码：
https://blog.csdn.net/callmeup/article/details/153119334?spm=1011.2415.3001.5331

单独代码的下载链接：
https://download.csdn.net/download/callmeup/92114151

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