【三维，UKF滤波】速度观测与IMU融合，适用于三维环境、使用无迹卡尔曼滤波作为融合方法，附MATLAB代码下载链接

基于 无迹卡尔曼滤波器（Unscented Kalman Filter, UKF） 的 三维惯性导航系统（INS） 与 多普勒计程仪（DVL） 融合仿真的 MATLAB 实现。代码以三维速度作为状态量，通过 UKF 算法处理非线性运动模型与观测模型，实现对载体三维速度的精确估计与位置解算。

代码简介

程序概述

本程序是一个基于 无迹卡尔曼滤波器（Unscented Kalman Filter, UKF） 的 三维惯性导航系统（INS） 与 多普勒计程仪（DVL） 融合仿真的 MATLAB 实现。代码以三维速度作为状态量，通过 UKF 算法处理非线性运动模型与观测模型，实现对载体三维速度的精确估计与位置解算。

核心功能与技术特点

1. 非线性系统建模：

   • 状态模型：采用非线性递推方程模拟三维速度动态变化，包含周期性扰动（8*cos(1.2*t)）。
   • 观测模型：模拟 DVL 的非线性观测，其中 Y、Z 轴速度为平方关系（如 X(2,i)^2），贴近实际 DVL 工作原理。

2. 完整 UKF 实现：

   • Sigma 点生成：基于 Cholesky 分解的对称采样策略。
   • 预测与更新步骤：包含状态预测、观测预测、协方差计算与卡尔曼增益更新。
   • 权重自适应：根据参数 alpha、beta、kappa 调整 Sigma 点权重。

3. 误差分析与可视化：

   • 提供 速度真值、UKF 估计值、未滤波值 的对比曲线。
   • 绘制 三维轨迹图，直观展示融合效果。
   • 输出 多项误差指标（最大值、均值、标准差、均方误差），量化评估滤波性能。

代码结构说明

关键参数说明

适用场景

1. 水下导航系统开发：INS/DVL 组合导航算法的原型验证。
2. 非线性滤波教学：作为 UKF 算法的三维仿真案例。
3. 状态估计研究：适用于具有非线性观测模型的状态估计问题。

运行结果

轨迹变化：

速度曲线：

速度误差曲线：

程序结构：

MATLAB源代码

部分代码如下：

% UKF融合INS与DVL的核心程序，三维
% 作者:matlabfilter（V同号，可接代码定制、讲解与调试）
% 2026-01-11/Ver1

clear;clc;close all;
rng(0); %注释此行可以在每次运行时使用不同的随机数
%% 滤波模型初始化
t = 1:1:100;
Q = 0.01*diag([1,1,1]);w=sqrt(Q)*randn(size(Q,1),length(t));
R = 1^2*diag([1,1,1]);v=sqrt(R)*randn(size(R,1),length(t));
P0 = 1*eye(3);
X=zeros(3,length(t)); %构建滤波状态量(三维速度)
X_ukf=zeros(3,length(t)); %构建滤波后的输出状态
X_ukf(1,1)=X(1,1);
Z=zeros(3,length(t)); %定义观测值形式
Z(:,1)=[X(1,1);X(2,1).^2;X(3,1).^2]+v(:,1); %观测量——对三维速度进行观测
%% 运动模型
X_=zeros(3,length(t));
X_(:,1)=X(:,1);

完整代码下载链接：

https://download.csdn.net/download/callmeup/92552869

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