三维状态量的CKF例程（严格的组合导航推导）基于8维误差状态模型：位置(2)、速度(2)、航向角、航向偏差(1)、加速度计偏差(2)
，基于,4维观测：XY位置（2）+XY速度（2）
文章目录
程序详解
算法概述
本程序实现**容积卡尔曼滤波（CKF）**的二维组合导航：利用 IMU（比力、角速度）进行预测，用 GNSS（位置、速度）进行校正，抑制纯 IMU 积分的漂移，提升位置与速度精度。轨迹为匀速圆周运动，用于对比 CKF、纯 IMU、仅 GNSS 的性能。
状态与观测
- 状态（8维） ：位置X、位置Y、速度X、速度Y、航向角、陀螺零偏、加速度计零偏X、加速度计零偏Y。
- 观测（4维） ：GNSS 的位置X、位置Y、速度X、速度Y（1 Hz 提供，其余时刻缺测）。
关键流程（CKF）
- 生成容积点：围绕先验均值和协方差，构造对称的 2n 个容积点。
- 状态传播：对每个容积点用 IMU 数据做一次预测更新（姿态由航向角积分，速度用 DCM 将机体系比力旋到导航系后积分，位置由速度积分；零偏按随机游走保持）。
- 统计量计算：由传播后的容积点集求预测均值与协方差。
- 量测更新：当有 GNSS 时，将容积点映射到观测空间，计算观测均值、新息协方差与交叉协方差，完成卡尔曼更新；无量测则仅用预测值。
噪声与模型
- 过程噪声 Q：考虑加速度噪声（影响速度与位置）、陀螺噪声（影响航向角）、陀螺零偏与加计零偏的随机游走。
- 观测噪声 R：GNSS 位置与速度的高斯噪声。
- 姿态/坐标：仅使用航向角，方向余弦矩阵由航向角生成（二维平面导航）。
输出与评估
- 绘制：二维轨迹、X/Y 位置曲线、X/Y 速度曲线、位置/速度误差曲线。
- 统计：对比纯 IMU、仅 GNSS、CKF 融合的 RMSE。
适配与扩展建议
- GNSS 失锁/非等时观测：已用 NaN 占位，CKF 自动“只预测”。
- 模型扩展：可加入重力、外部力或一阶马尔可夫零偏（非纯随机游走）。
- 参数整定：先调 R（贴合 GNSS 噪声），再按漂移量级调 Q（尤其零偏项）。
运行结果
轨迹对比图像：
位移曲线对比：
速度曲线对比：
误差曲线对比：
命令行输出的结果：
MATLAB源代码
部分代码如下：
% 三维状态量的CKF例程（严格的组合导航推导）,84
% 基于8维误差状态模型：位置(2)、速度(2)、航向角、航向偏差(1)、加速度计偏差(2)
% 基于4维观测：XY位置（2）+XY速度（2）
% 容积卡尔曼滤波器（Cubature Kalman Filter）实现
% 作者：matlabfilter
% 2025-08-30/Ver1
clear; clc; close all;
rng(0); % 固定随机种子
%% 系统参数设置
dt = 0.1; % 采样时间间隔 (s)
total_time = 100; % 总仿真时间 (s)
N = total_time / dt; % 采样点数
%% 噪声参数设置
% IMU噪声参数
gyro_noise_std = 0.1 * pi/180; % 陀螺噪声标准差 (rad/s)
accel_noise_std = 0.01; % 加速度计噪声标准差 (m/s^2)
gyro_bias_std = 0.01 * pi/180; % 陀螺偏差标准差 (rad/s)
accel_bias_std = 0.001; % 加速度计偏差标准差 (m/s^2)
% GNSS观测噪声
gnss_pos_noise_std = 3; % GNSS位置噪声标准差 (m)
gnss_vel_noise_std = 0.1; % GNSS速度噪声标准差 (m/s)
%% 过程噪声协方差矩阵Q (8×8)
% 状态顺序：[位置(2), 速度(2), 姿态(1), 陀螺偏差(1), 加速度计偏差(2)]
Q = zeros(8, 8);
% 位置噪声（通过速度积分产生）
Q(1:2, 1:2) = eye(2) * (accel_noise_std * dt^2)^2;
% 速度噪声
Q(3:4, 3:4) = eye(2) * (accel_noise_std * dt)^2;
% 姿态噪声
Q(5, 5) = eye(1) * (gyro_noise_std * dt)^2;
% 陀螺偏差噪声
Q(6, 6) = eye(1) * (gyro_bias_std * dt)^2;
% 加速度计偏差噪声
Q(7:8, 7:8) = eye(2) * (accel_bias_std * dt)^2;
%% 观测噪声协方差矩阵R (4×4)
