【组合导航例程】三维空间，CKF（容积卡尔曼滤波），融合IMU+GNSS数据，观测为XYZ三轴的坐标|MATLAB仿真|附下载链接

三维状态量的CKF例程（严格的组合导航推导）。% 基于15维误差状态模型：位置(3)、速度(3)、姿态(3)、陀螺偏差(3)、加速度计偏差(3)，% 基于3维观测：XYZ位置（3）
容积卡尔曼滤波器（Cubature Kalman Filter）实现

@

目录

• 运行结果
• 程序详解
  • 算法模型
  • 程序流程
• MATLAB源代码

运行结果

三维轨迹图：

程序详解

实现为 三维容积卡尔曼滤波器（Cubature Kalman Filter, CKF） 的组合导航仿真例程，核心思想是利用 15维误差状态模型 对惯性测量单元（IMU）的累积误差进行建模，并结合 GNSS位置观测 实现高精度定位与姿态估计。

算法模型

1. 状态向量
   采用 15 维误差状态：

   $$
   \delta \mathbf{x} =
   \begin{bmatrix}
   \delta \mathbf{p} &
   \delta \mathbf{v} &
   \delta \boldsymbol{\theta} &
   \delta \mathbf{b}_g &
   \delta \mathbf{b}_a
   \end{bmatrix}^T
   $$

   • 位置误差 $\delta \mathbf{p} \in \mathbb{R}^3$
   • 速度误差$\delta \mathbf{v} \in \mathbb{R}^3$
   • 姿态误差 $\delta \boldsymbol{\theta} \in \mathbb{R}^3$
   • 陀螺偏差 $\delta \mathbf{b}_g \in \mathbb{R}^3$
   • 加速度计偏差 $\delta \mathbf{b}_a \in \mathbb{R}^3$

2. 观测向量
   仅采用 GNSS 提供的三维位置观测：

   $$
   \mathbf{z}_k =
   \begin{bmatrix}
   x_k \ y_k \ z_k
   \end{bmatrix} + \mathbf{v}_k
   , \quad \mathbf{v}_k \sim \mathcal{N}(0, R)
   $$

3. 滤波核心公式
   CKF 通过生成 $2n$ 个容积点来逼近高斯分布，避免了 UKF 中权重不均衡的问题：

   • 容积点生成：

   $$
   \chi_i = \hat{\mathbf{x}} \pm \sqrt{n} , L_i \quad (i = 1,\dots,n)
   $$

   其中 $L_i$ 为协方差矩阵 $P$ 的 Cholesky 分解列向量。

   • 预测与更新均基于容积点传播的均值与协方差计算。

程序流程

1. 系统初始化

   • 设置 IMU 噪声（陀螺、加速度计、偏差）
   • 设置 GNSS 观测噪声
   • 构造过程噪声协方差 $Q$ 和观测噪声协方差 $R$
   • 初始化状态与协方差矩阵 $P$

2. 生成真实轨迹与传感器数据

   • 模拟螺旋上升运动轨迹
   • 根据真实运动计算 IMU 比力和角速度，再加入噪声和偏差
   • 模拟 GNSS 观测（每秒更新一次）

3. 纯 IMU 积分

   • 仅依靠 IMU 进行导航解算，用于对比基准

4. CKF 主循环

   • 时间更新：生成容积点 → 状态传播 → 计算预测均值与协方差
   • 量测更新：传播观测模型 → 计算新息协方差 $P_{zz}$ 与交叉协方差 $P_{xz}$ → 更新状态与协方差

5. 结果分析与绘图

   • 绘制三维轨迹（真值、纯IMU、CKF、GNSS）
   • 分别对比位置、速度、姿态随时间的变化
   • 统计 RMSE 误差，包括位置、速度、姿态三类指标

MATLAB源代码

部分代码如下：

% 三维状态量的CKF（容积卡尔曼滤波器）例程（严格的组合导航推导）,153
% 基于15维误差状态模型：位置(3)、速度(3)、姿态(3)、陀螺偏差(3)、加速度计偏差(3)
% 基于3维观测：XYZ位置（3）
% 作者：matlabfilter（微信同号），接定位与导航、滤波相关的matlab代码定制
% 2025-09-24/Ver1

clear; clc; close all;
rng(0); % 固定随机种子

%% 系统参数设置
dt = 0.1;           % 采样时间间隔 (s)
total_time = 100;   % 总仿真时间 (s)
N = total_time / dt; % 采样点数

%% 噪声参数设置
% IMU噪声参数
gyro_noise_std = 0.001 * pi/180;      % 陀螺噪声标准差 (rad/s)
accel_noise_std = 0.001;             % 加速度计噪声标准差 (m/s^2)
gyro_bias_std = 0.001 * pi/180;      % 陀螺偏差标准差 (rad/s)
accel_bias_std = 0.0001;             % 加速度计偏差标准差 (m/s^2)

% GNSS观测噪声
gnss_pos_noise_std = 0.3;             % GNSS位置噪声标准差 (m)

%% 过程噪声协方差矩阵Q (15×15)
% 状态顺序：[位置(3), 速度(3), 姿态(3), 陀螺偏差(3), 加速度计偏差(3)]
Q = zeros(15, 15);
% 位置噪声（通过速度积分产生）
Q(1:3, 1:3) = eye(3) * (accel_noise_std * dt^2 / 2)^2;
% 速度噪声
Q(4:6, 4:6) = eye(3) * (accel_noise_std * dt)^2;
% 姿态噪声
Q(7:9, 7:9) = eye(3) * (gyro_noise_std * dt)^2;
% 陀螺偏差噪声
Q(10:12, 10:12) = eye(3) * (gyro_bias_std * dt)^2;
% 加速度计偏差噪声
Q(13:15, 13:15) = eye(3) * (accel_bias_std * dt)^2;

%% 观测噪声协方差矩阵R (3*3)
% 观测量：GNSS位置(3) 
R = eye(3) * gnss_pos_noise_std^2;

%% 初始化
% 真实轨迹参数（螺旋上升运动）

完整代码：
https://download.csdn.net/download/callmeup/92007979

或查看专栏文章：【组合导航代码】三维空间，CKF（容积卡尔曼滤波），融合IMU+GNSS数据，观测为XYZ三轴的坐标|MATLAB仿真|附完整的代码，可粘贴后直接运行

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