【MATLAB代码】三维状态量的组合导航仿真（UKF用来滤波，带推导）基于15维误差状态模型：位置(3)、速度(3)、姿态(3)、陀螺偏差(3)、加速度计偏差(3)，观测量6维

三维状态量的UKF例程（严格的组合导航推导）使用基于15维误差状态模型：位置(3)、速度(3)、姿态(3)、陀螺偏差(3)、加速度计偏差(3)；观测量为6维：位置(3)、速度(3)
订阅专栏后，可直接查看源代码，粘贴到MATLAB空脚本中即可直接运行、得到结果

运行结果

三轴轨迹对比：

各轴位置和速度变化曲线：

三维空间中的位置误差、速度误差变化曲线：

MATLAB源代码

完整代码如下：

% 三维状态量的UKF例程（严格的组合导航推导）
% 基于15维误差状态模型：位置(3)、速度(3)、姿态(3)、陀螺偏差(3)、加速度计偏差(3)
% 作者：matlabfilter