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

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

描述这种旋转关系通常使用欧拉角 (ψ,θ,γ)T ，姿态矩阵T(3x3)，四元数 Q=(q0，q1，q2，q3)T 。

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

姿态矩阵T描述的是三维空间中的两个标准正交基之间的向量转换关系，T是一个3x3的正交阵。若一个向量在n系下的坐标为三维列向量 xn ，则该向量在b系下的坐标为 xb=Tbn⋅xn ，反之 xn=Tnb⋅xb 。T阵是一个正交矩阵，故 Tbn 与 Tnb 互为转置矩阵，互为逆矩阵。

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

|Q|=q20+q21+q22+q23−−−−−−−−−−−−−−√=1

每个归一化的四元数都描述了一个姿态，或者说描述了一个可由n系旋转得到的b坐标系。

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

转换关系如下：

由欧拉角 ψ,θ,γ 得到姿态矩阵 Tbn ：
绕z轴旋转矩阵 Tz ：

Tz=⎡⎣⎢cosψ−sinψ0sinψcosψ0001⎤⎦⎥

绕x轴旋转矩阵 Tx :

Tx=⎡⎣⎢1000cosθ−sinθ0sinθcosθ⎤⎦⎥

绕y轴旋转矩阵 Ty :

Ty=⎡⎣⎢cosγ0sinγ010−sinγ0cosγ⎤⎦⎥

则姿态矩阵 Tbn=Ty⋅Tx⋅Tz
所以：

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θ⎤⎦⎥

由姿态矩阵得到欧拉角 ψ,θ,γ

由四元数 Q 转换到姿态矩阵T：

Tbn=⎡⎣⎢⎢q20+q21−q22−q232(q1q2−q0q3)2(q1q3+q0q2)2(q1q2+q0q3)q20−q21+q22−q232(q2q3−q0q1)2(q1q3−q0q2)2(q2q3+q0q1)q20−q21−q22+q23⎤⎦⎥⎥

由姿态矩阵转换到四元数：
由矩阵 T 的对角线元素可知:

q20+q21−q22−q23=T11q20−q21+q22−q23=T22q20−q21−q22+q23=T33q20+q21+q22+q23=1

故而有

|q1|=121+T11−T22−T33−−−−−−−−−−−−−−−√|q2|=121−T11+T22−T33−−−−−−−−−−−−−−−√|q3|=121−T11−T22+T33−−−−−−−−−−−−−−−√|q0|=1−q21−q22−q23−−−−−−−−−−−−−√

其中，正负号如下：

sign(q0)=+sign(q1)=sign(T32−T23)sign(q2)=sign(T13−T31)sign(q3)=sign(T21−T12)

——————————————————————————————————————————————————————————
考虑这样一个问题：若b系相对n系的欧拉角为 (ψ,θ,γ) ，以角速度 ωnb 转动了 Δt时间 ，注意角速度 ωnb 是一个三维向量，那么欧拉角的变化量 (Δψ,Δθ,Δγ) 应该是多少呢？这个问题旨在找出欧拉角的变化速率与转动角速度的关系，以下给出这个关系。
欧拉角微分方程

⎡⎣⎢ψ˙θ˙γ˙⎤⎦⎥=1cosθ⎡⎣⎢−sinγcosγcosθsinθsinγ00cosθcosγsinγcosθ−sinθcosγ⎤⎦⎥⎡⎣⎢⎢ωbnbxωbnbyωbnbz⎤⎦⎥⎥

上式中 ωbnbx 表示转动角速度 ωnb 在b坐标系x轴上的投影，也就是固连在转动物体上的三轴陀螺仪测量的角速度。若已知 t0 时刻的欧拉角 (ψ,θ,γ) ，以及从 t0 到 t 时刻的角速度曲线，那么 t 时刻的欧拉角自然是可以计算出的，这是一个求解一阶微分方程组的问题。
然而欧拉角微分方程是一个非线性微分方程组，它的解析解并不能以简单函数的形式表达出来。若把目光投向姿态矩阵和四元数，亦有微分方程如下：

姿态矩阵微分方程

⎡⎣⎢⎢T˙11T˙21T˙31T˙12T˙22T˙32T˙13T˙23T˙33⎤⎦⎥⎥=⎡⎣⎢T11T21T31T12T22T32T13T23T33⎤⎦⎥⎡⎣⎢⎢0ωbnbz−ωbnby−ωbnbz0ωbnbxωbnby−ωbnbx0⎤⎦⎥⎥

四元微分方程：

⎡⎣⎢⎢⎢⎢q˙0q˙1q˙2q˙3⎤⎦⎥⎥⎥⎥=12⎡⎣⎢⎢⎢⎢⎢⎢0ωbnbxωbnbyωbnbz−ωbnbx0−ωbnbzωbnby−ωbnbyωbnbz0−ωbnbx−ωbnbz−ωbnbyωbnbx0⎤⎦⎥⎥⎥⎥⎥⎥⎡⎣⎢⎢⎢⎢q0q1q2q3⎤⎦⎥⎥⎥⎥

到这里，对于载体的姿态问题就有了一个解决办法：在载体上安装相互正交的三个陀螺仪，测量载体相对导航坐标系的转动，选取欧拉角/姿态矩阵/四元数描述姿态，按照微分方程积分，就可以得到姿态了。
若以四元数描述姿态，则一个典型公式为：

⎡⎣⎢⎢⎢⎢q0q1q2q3⎤⎦⎥⎥⎥⎥k+1=⎡⎣⎢⎢⎢⎢q0q1q2q3⎤⎦⎥⎥⎥⎥k+⎡⎣⎢⎢⎢⎢q˙0q˙1q˙2q˙3⎤⎦⎥⎥⎥⎥Δt

若记

[ω×]=⎡⎣⎢⎢⎢⎢⎢⎢0ωbnbxωbnbyωbnbz−ωbnbx0−ωbnbzωbnby−ωbnbyωbnbz0−ωbnbx−ωbnbz−ωbnbyωbnbx0⎤⎦⎥⎥⎥⎥⎥⎥

则

Qk+1={I4×4+12[ω×]}Qk

由于这是一个积分的做法，在实际应用中不可避免的会出现误差发散，长时间之后这个办法给出的姿态就不具有太高的可信度了。此外实际应用中还需注意一个问题，由于角速度测量的误差，四元数/姿态矩阵在一段时间后可能会不满足归一性/正交性，需要对四元数/姿态矩阵做归一化/正交化处理。
——————————————————————————————————————————————————————————
关于mpu6050加速度计，很多人可能无法理解为什么在相对n系静止的情况下加速度计会测量到一个与重力加速度相反的加速度，建议阅读这篇知乎专栏文章中关于加速度计的部分： https://zhuanlan.zhihu.com/p/20082486这里对加速度计的测量解释的较为形象。
理想的载体相对n系无加速度状态下，加速度计的测量为：

⎡⎣⎢axayaz⎤⎦⎥=Tbn⎡⎣⎢00g⎤⎦⎥=⎡⎣⎢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θ⎤⎦⎥⎡⎣⎢00g⎤⎦⎥=⎡⎣⎢−gsinγcosθgsinθgcosγcosθ⎤⎦⎥

可以看到，在载体本身无加速度的情况下，加速度计的测量输出是与 ψ 无关的，也就是说加速度计只能反映姿态的一部分信息，仅凭加速度计的测量并不能唯一确定一个姿态，这是显而易见的。
要测量姿态，我们至少需要在b系下测量两个n系下已知的向量，很容易想到，引入磁场传感器测量地磁向量，这样就能唯一地计算出姿态矩阵，从而确定载体的姿态。
若考虑载体本身的加速度，如果不依靠其他手段对载体本身的加速度做出估计，那么加速度计的测量就既与载体姿态变化有关，亦与载体加速度有关。