- 使用8维状态模型，全面描述了平面运动中的位置、速度、姿态和传感器偏差，采用容积卡尔曼滤波器处理非线性问题，相比传统EKF具有更好的数值稳定性和精度，实现了IMU与GNSS的有效融合，充分发挥两种传感器的互补优势
文章目录
程序简介
核心创新：
- 使用8维状态模型，全面描述了平面运动中的位置、速度、姿态和传感器偏差
- 采用容积卡尔曼滤波器处理非线性问题，相比传统EKF具有更好的数值稳定性和精度
- 实现了IMU与GNSS的有效融合，充分发挥两种传感器的互补优势
状态模型
8维误差状态模型：
位置：2维（X, Y坐标）
速度：2维（X, Y方向速度）
航向角：1维（偏航角yaw）
陀螺仪偏差：1维（角速度偏差）
加速度计偏差：2维（X, Y方向加速度偏差）
观测模型
2维观测：GNSS提供的XY位置信息
观测频率：GNSS每1秒更新一次（10Hz IMU，1Hz GNSS）
核心算法
容积卡尔曼滤波器：采用球面-径向积分规则，通过2n个容积点来近似状态分布
非线性系统处理：能够有效处理姿态更新中的非线性问题
仿真场景
轨迹类型：圆形运动
运动参数：
半径：50米
角速度：0.1 rad/s
仿真时间：100秒
采样频率：10Hz
噪声模型
IMU噪声：
陀螺仪噪声：0.1°/s
加速度计噪声：0.01 m/s²
陀螺仪偏差：0.01°/s
加速度计偏差：0.001 m/s²
GNSS噪声：
位置测量噪声：3米标准差
运行结果
轨迹对比：
两轴的位置对比和速度对比：
误差对比曲线：
命令行窗口的截图：
MATLAB源代码
部分代码如下：
matlab
% 平面CKF例程（严格的组合导航推导）,82
% 基于8维误差状态模型：位置(2)、速度(2)、航向角、航向偏差(1)、加速度计偏差(2)
% 基于4维观测：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)
% 观测量：GNSS位置(2)
R = eye(2) * gnss_pos_noise_std^2;完整代码：
https://download.csdn.net/download/callmeup/92007983
或查看专栏文章：
- 组合导航的MATLAB代码，二维平面上的CKF滤波，融合IMU和GNSS数据，仿真，观测为X和Y轴的坐标：https://blog.csdn.net/callmeup/article/details/152035022?spm=1011.2415.3001.5331
如需帮助，或有导航、定位滤波相关的代码定制需求，请点击下方卡片联系作者