三维状态量的EKF例程(严格的组合导航推导)。基于15维误差状态模型:位置(3)、速度(3)、姿态(3)、陀螺偏差(3)、加速度计偏差(3)。观测量为三维位置(另有三维位置+速度为观测的程序) 订阅专栏后,可直接查看源代码,粘贴到MATLAB空脚本中即可直接运行、得到结果 文章目录 运行结果 MATLAB源代码 程序详解 系统概述 状态空间模型 方向余弦矩阵 运行结果 三维轨迹对比图像: 速度、位置、姿态对比图: 误差对比图像: 命令行输出的结果: MATLAB源代码 完整代码如下: