组合导航的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

或查看专栏文章:

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

相关推荐
@LetsTGBot搜索引擎机器人1 天前
Telegram 被封是什么原因?如何解决?(附 @letstgbot 搜索引擎重连技巧)
开发语言·python·搜索引擎·机器人·.net
沐欣工作室_lvyiyi1 天前
基于Matlab的简易振动信号分析系统(论文+仿真)
开发语言·matlab·毕业设计·振动信号分析
liu****1 天前
1.模拟算法
开发语言·c++·算法·1024程序员节
数据村的古老师1 天前
Python数据分析实战:基于25年黄金价格数据的特征提取与算法应用【数据集可下载】
开发语言·python·数据分析
孔明兴汉1 天前
第一章-第三节-Java开发环境配置
java·开发语言
小王不爱笑1321 天前
Java 核心知识点查漏补缺(一)
java·开发语言·python
空空kkk1 天前
Java——类和对象
java·开发语言
沐知全栈开发1 天前
Python3 集合
开发语言
Jonathan Star1 天前
用Python轻松提取视频音频并去除静音片段
开发语言·python·音视频
Evand J1 天前
【自适应粒子滤波MATLAB例程】Sage Husa自适应粒子滤波,用于克服初始Q和R不准确的问题,一维非线性滤波。附下载链接
开发语言·matlab·卡尔曼滤波·自适应滤波·非线性