递推定位的姿态旋转和位置平移

递推定位的平移和旋转

很多定位应用场景中，往往需要在载体上已知某一点的位姿(全局坐标系)条件下，获取另一明确的目标点的全局坐标系。其中从初始位置到目标位置的变换向量在载体坐标系下是已知的，这样就使用到了定位点的平移和旋转。

定位点的平移

姿态及其变化表述

姿态是描述对象的方向向量相对于坐标系的横、纵和竖轴旋转变化。

姿态描述

因为建立右手坐标系的方式不同，矩阵不同，此处以机体纵轴为x轴，且正向指向机头，机体竖轴为z轴，且正向指向天花板方向；因为满足右手定义，y轴也随之确定。
1、姿态角:
航向(yaw or heading):沿竖轴(上下穿过体体)方向的旋转，如上假设为z轴角；
俯仰(pitch):沿横轴(左右穿过机体)方向的旋转，如上假设为y轴角；
横滚(roll or bank):沿纵轴(前后穿过机体)方向的旋转，如上假设为z轴角；

2、姿态矩阵:
姿态矩阵C为3行3列。

3、四元数:q为三个实部，一个虚部；

角、矩阵和四元数间的转换

// 姿态矩阵(3x3)
[C1,C2,C3]`;
C1 = [sin*SIN]