导航过程各坐标系之间转换

做导航最烦人的就是坐标系之间的转换，每次都要去推相应的公式，所以特地写点干货，解决你坐标系转换问题。

1.经纬高转地心

                                                                              （1）

式1中，为纬度，为经度，e为地球的椭圆率。

2.地心转经纬高

                                                           （2）

式2中变量同式1。

3.东北天坐标系转地心坐标系

由此可得转换矩阵为：

注意这里东北天坐标系和地心坐标系原点不同，所以在计算坐标时，还有一个原点的偏移量。

4.地心坐标系转东北天坐标系

                                                       

通过3可以求相应的转换矩阵

5.载体坐标系转东北天坐标系

不知道你是否有这样的疑惑，就是网上查资料，可能会出现各种各样的转移矩阵，把人弄得很迷糊，其实可能这些状态转移矩阵都是正确的，只不过他们定义导航坐标系中翻滚角、俯仰角、偏航角的方式各种各样罢了，下面我给定一种常用的导航坐标系各角度的定义方式，大家以后可以按照自己定义的坐标系进行更改。

上面是书上的定义，懒得敲，就直接贴图了。

总结一下，对于偏航角，顺时针旋转为正，俯仰角逆时针方向为正，翻滚角顺时针方向为正。

所以正常情况下的转动过程如下：

由此可得状态转移矩阵为：

看到这个状态转移矩阵你可能觉得不对劲，平时用的不是这个，我想说的是你平时用的状态转移矩阵定义偏航角的时候进行了更改，将北偏东为正改成了北偏西为正，就导致偏航角逆时针为正，其旋转方式如下：

状态转移矩阵如下：

平时常见的状态转换矩阵如上所示，同理将东北天坐标系转载体坐标系只需要将上面的矩阵转置即可。