ekf2 MATLAB离线版本详解（二）

最新推荐文章于 2025-06-14 15:54:25 发布

原创

最新推荐文章于 2025-06-14 15:54:25 发布 · 1.1k 阅读

CC 4.0 BY-SA版权

本文详细介绍了ekf2在MATLAB中的离线实现过程，主要包括主循环Main Loop中的步骤，如IMU数据读取、状态预测、角度和速度修正、四元数计算、加速度转换、速度和位置更新、协方差矩阵处理等，以及各种噪声和漂移的处理，最终输出包括欧拉角、速度、位置、漂移和零偏等多种估计值。

摘要生成于 C知道，由 DeepSeek-R1 满血版支持，前往体验 >

初始化设置融合标志位，设为非
gps_use_started = boolean(false); gps_use_blocked = boolean(false); viso_use_blocked = boolean(false); flow_use_blocked = boolean(false);
涉及的传感器索引的初始化
imuIndex = imu_start_index; last_gps_index = 0; gps_fuse_index = 0; last_baro_index = 0; baro_fuse_index = 0; last_mag_index = 0; mag_fuse_index = 0; last_flow_index = 0; flow_fuse_index = 0; last_viso_index = 0; viso_fuse_index = 0; last_range_index = 0;
一步预测协方差变量的初始化
delAngCov = [0;0;0];一步预测协方差时的角度方差
delVelCov = [0;0;0];一步预测协方差时的速度方差
dtCov = 0;一步预测协方差时的时间间隔
dtCovInt = 0;一步预测协方差时累计的时间间隔
covIndex = 0; 一步预测协方差时帧的统计
选择磁力计融合的方式
output.magFuseMethod = param.fusion.magFuseMethod;
range = 0.1;
控制位姿解算错误的时间的初始化
last_drift_constrain_time