坐标系

导航坐标系常用的导航坐标系有北东地(NED)和东北天(ENU)两种。他们的指向定义分别如下：

北东地(ENU)坐标系                        东北天(NED)坐标系            

    X→北                                                X→东

    Y→东                                                Y→北

    Z→地                                                Z→天

载体坐标系与导航坐标系类似，常用的也有如下两种：

前右下坐标系-北东地坐标系            右前上坐标系-东北天坐标系 

    X→前                                                X→右

    Y→右                                                Y→前

    Z→下                                                Z→上

二维旋转

首先要明确旋转在二维中是绕着某一个点进行旋转，三维中是绕着某一个轴进行旋转。二维旋转中最简单的场景是绕着坐标原点进行的旋转，如下图所示：

如图所示点v 绕 原点旋转θθ 角，得到点v’，假设 v点的坐标是(x, y) ，那么可以推导得到 v’点的坐标（x’, y’)(设原点到v的距离是r，原点到v点的向量与x轴的夹角是ϕϕ ) 

通过三角函数展开：

代入x、y表达式：

写成矩阵的形式：

方向余弦矩阵

方向余弦是指在解析几何里，一个向量的三个方向余弦分别是这向量与三个坐标轴之间的角度的余弦。两个向量之间的方向余弦指的是这两个向量之间的角度的余弦。

“方向余弦矩阵”是由两组不同的标准正交基的基底向量之间的方向余弦所形成的矩阵。方向余弦矩阵可以用来表达一组标准正交基与另一组标准正交基之间的关系，也可以用来表达一个向量对于另一组标准正交基的方向余弦。

本文采用右手坐标系，同时旋转角度的正负也遵循右手坐标系的约定。

旋转顺序依次为 Z - Y - X。

导航坐标系：    北东地NED - 前右下

绕 Z 轴的旋转矩阵：

绕 Y 轴的旋转矩阵：

绕 X 轴的旋转矩阵：

将以上矩阵按照Z-Y-X的转动顺序连乘，可以求得一个可以表示这个欧拉转动的旋转矩阵，也被称之为方向余弦矩阵。

导航坐标系：    东北天ENU-右前上

绕 Z 轴的旋转矩阵：

绕 Y 轴的旋转矩阵：

绕 X 轴的旋转矩阵：

则：

DCM转欧拉角

记

则

    roll：            

    pitch:           

    yaw: