北-东-地坐标系下的低精度的SINS/GPS组合导航系统中初始航向对准技术

 

推导了一篇严恭敏老师的论文《低精度的SINS/GPS组合导航系统中初始航向对准技术》

主要思想：为了降低航向的非线性，直接估计 sin(航向) 和 cos(航向)

文中仿真了两种场景：

　　场景1：初始速度：0m/s，加速度：1m/s^2，GPS水平定位精度：10m；

　　场景2：初始速度：10m/s，拐弯半径为100m，GPS水平定位精度：10m。

仿真结果：20s航向可以收敛到3°。

注：从公式中状态系数阵看，必须保证水平加速度不为零，才能估计出航向

 

因为我习惯用北-东-地坐标系，所以本博文在北-东-地坐标系下推导，下面是推导细节：

 

 

 下面推导欧拉角微分方程，并得出航向微分方程：

 

参考：

[1]严恭敏,秦永元.低精度的SINS/GPS组合导航系统中初始航向对准技术(英文)[J].传感技术学报,2007,(01):237-241.

[2]CSDN: 大失准角下的滤波方程.