组合导航的MATLAB例程,二维平面上的CKF滤波,融合IMU和GNSS数据,仿真,观测为X和Y轴的坐标,附代码下载链接

  • 使用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

或查看专栏文章:

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

相关推荐
蔗理苦2 小时前
2025-10-07 Python不基础 20——全局变量与自由变量
开发语言·python
-森屿安年-3 小时前
C++ 类与对象
开发语言·c++
小蒜学长3 小时前
springboot基于javaweb的小零食销售系统的设计与实现(代码+数据库+LW)
java·开发语言·数据库·spring boot·后端
会开花的二叉树4 小时前
c语言贪吃蛇游戏开发
c语言·开发语言
韩立学长4 小时前
【开题答辩实录分享】以《C#大型超市商品上架调配管理系统的设计与实现》为例进行答辩实录分享
开发语言·c#
tao3556674 小时前
【Python刷力扣hot100】49. Group Anagrams
开发语言·python·leetcode
龙腾AI白云4 小时前
大模型-扩散模型(Diffusion Model)原理讲解(4)
开发语言
爱吃小胖橘5 小时前
Lua语法(2)
开发语言·unity·lua
_Power_Y5 小时前
SSM面试题学习
java·开发语言·学习