mpu6050姿态解算与卡尔曼滤波（1）数学

定义地理坐标系n系：x轴指向东，y轴指向北，z轴指向天。在mpu6050芯片上定义载体坐标系b系。那么b系的姿态就是指n系与b系相对的旋转关系，即如何由n系旋转到b系。

描述这种旋转关系通常使用欧拉角$(\psi, \theta,\gamma)^T$，姿态矩阵T(3x3)，四元数$Q=(q_0，q_1，q_2，q_3)^T$。

欧拉角指的是将n系按照z轴->x’轴->y”轴的顺序依次转动 $\psi, \theta,\gamma$ 后就是b系，事实上欧拉角并没有规定转动顺序，以上转动顺序z轴->x’轴->y”轴为惯性导航中常使用的规定，$(\psi, \theta,\gamma)^T$依次命名为偏航、俯仰、滚转。

姿态矩阵T描述的是三维空间中的两个标准正交基之间的向量转换关系，T是一个3x3的正交阵。若一个向量在n系下的坐标为三维列向量$x_n$，则该向量在b系下的坐标为$x_b=T_n^b\cdot x_n$，反之$x_n=T_b^n\cdot x_b$。T阵是一个正交矩阵，故$T_n^b$与$T_b^n$互为转置矩阵，互为逆矩阵。

四元数Q的本质涉及到更高等的数学，若仅考虑用四元数描述姿态，只需记住：描述姿态的四元数是归一化的，即:

$$|Q|=\sqrt{q_0^2+q_1^2+q_2^2+q_3^2}=1$$
每个归一化的四元数都描述了一个姿态，或者说描述了一个可由n系旋转得到的b坐标系。

以上三种描述方式是相互等价的，只要确定了两个坐标系的旋转关系，那么就唯一地确定了一组欧拉角，也唯一地确定了一个姿态矩阵和归一化四元数。三种描述方式可以互相推导，由欧拉角可以推导出姿态矩阵，再由姿态矩阵可以推导出四元数。同样，由四元数可以得到姿态矩阵，再得到欧拉角。
改正：以上三种描述方式是等价的，但是要注意对于两个确定的旋转关系的坐标系，表示两者旋转关系的三种方式中，姿态矩阵一定是唯一的，但是四元数却可以有两个，而欧拉角可以有很多。在做彼此的转换，尤其是涉及到四元数表示的时候，一定要注意。

转换关系如下：

由欧拉角$\psi, \theta,\gamma$得到姿态矩阵$T_n^b$：
绕z轴旋转矩阵$T_z$：

$$T_z=\begin{bmatrix} \cos \psi & \sin\psi & 0 \\ -\sin\psi & \cos\psi & 0 \\ 0 & 0 & 1 \\ \end{bmatrix}$$
绕x轴旋转矩阵 $T_x$:
$$T_x=\begin{bmatrix} 1 & 0& 0 \\ 0 & \cos\theta & \sin\theta \\ 0 & -\sin\theta&\cos\theta \\ \end{bmatrix}$$
绕y轴旋转矩阵 $T_y$:
$$T_y=\begin{bmatrix} \cos \gamma &0& -\sin\gamma \\ 0&1 & 0 \\ \sin\gamma & 0 & \cos\gamma \\ \end{bmatrix}$$
则姿态矩阵 $T_n^b=T_y\cdot T_x\cdot T_z$
所以：

Tbn=⎡⎣⎢cosγcosψ+sinγsinθsinψ−cosθsinψsinγcosψ+cosγsinθsinψcosγsinψ+sinγsinθcosψcosθcosψsinγsinψ−cosγsinθcosψ−sinγcosθsinθcosγcosθ⎤⎦⎥