首先是上一个视频的问题，有不少朋友问我，为什么视频中推导出来的旋转矩阵跟英文维基百科上的几个旋转矩阵都对不上，是不是推错了：https://en.wikipedia.org/wiki/Euler_angles

(资料图片仅供参考)

当然不是因为推错了，如果你仔细观察一下这几个矩阵和我视频中的矩阵，会发现这个矩阵跟我的矩阵好像有点像，但是差了个转置的关系。

如果再仔细观察一下维基百科中的前提条件：

Each matrix is meant to represent an active rotation(the composing and composed matrices are supposed to act on the coordinates of vectors defined in the initial fixed reference frame and give as a result the coordinates of a rotated vector defined in the same reference frame).

简单来说，就是他的矩阵中描述的是“主动旋转”，也就是旋转向量，而我视频中的推导是旋转坐标系。从旋转角度上来看，向量旋转，可以看作是坐标系旋转了，在最终的矩阵上，两个矩阵互为转置。

具体到每个轴的旋转矩阵： ，我所使用的都是描述坐标系旋转的，因此有：

于是：

四元数基础知识

其中， 为旋转轴

注意，四元数不遵循交换律，的关系为：

四元数旋转

四元数 其实描述了原坐标系1 绕着旋转轴旋转到新坐标系2的过程，对于某个向量 以及其旋转后的新向量有：

（必须为被归一化为单位四元数）

或者：

上面这两个式子也被称作坐标变换的四元数表示法。

对上式展开:

整理一下，有：

上式的矩阵记为：，也就是说，通过这个矩阵，我们可以求出新坐标系2下的向量，在坐标系1中的坐标。换句话说，就是把向量从新坐标系2下旋转回去坐标系1。

那如果我要反过来呢？

同样的，按照上个视频的方法，我们可以分别写出用四元数描述绕XYZ三轴的旋转：

(这三个欧拉角不再用希腊字母来表示了，太麻烦了，用abc也方便和xyz对应。）

同样的，有：

再把实部虚部整理一下，有：

把得到的带到上面的矩阵中，即可得到我们上个视频推导的方向余弦矩阵。

姿态融合

有了上面这些基础知识，我们可以看一下到底怎么用四元数融合姿态了。

基本原理其实如上图所示处于简单起见，这里使用最简单的互补滤波，用文字描述流程如下：

将重力加速度利用四元数的旋转矩阵旋转至机体坐标系，得到

读取飞机上安装的加速度计的值

计算与之间的误差，加速度计能读到的加速度其实也是重力加速度在机体坐标系下的分量，因此如果我们计算出来的旋转矩阵是准确的，经过旋转后，这两个向量应该是重合的。由于这里的误差主要指的是旋转误差，因此不能够把两个向量直接相减，可以通过计算两个向量之间的向量积：，再把两个向量归一化之后，向量积就只剩下一个，在小角度下近似于与角度成正比，因此可以用它来描述两个向量的旋转误差。

得到误差之后，可以将误差补偿到陀螺仪的角速度上，同时通过调整误差的增益系数/积分系数来调整误差和陀螺仪的比值，如果误差的系数高一点，相当于更信任加速度计一点。从融合效果来说，误差的系数越高，融合后的姿态响应越快，但是更易受噪声影响。

补偿完误差之后，可以使用角速度来更新四元数

角速度和四元数有啥关系？

在上一个视频中，我们定义了两个坐标系，分别是东北天坐标系（ENU，记为E）和机体坐标系(记为B），假设有某个四元数，绕着其旋转轴旋转，即可由东北天坐标系旋转到机体坐标系。

对时间微分，有：

其中

再整理一下：

这个表示E系到B系的旋转角速度在E系下的表示

我们从陀螺仪上读到的角速度其实是这个角速度在B系下的表示，也就是，用这个来表示会更方便一点。利用上面提到的坐标变换的四元数法，有：

其中，Q为E系到B系的旋转四元数，与上面的定义一致，于是：

将Q用表示并展开：

展开整理可得：

这个,其实也就是四元数下一时刻的增量，那么我们可以用一阶龙格库塔法来更新四元数：

当然了，还有其他的方法，比如更高阶的龙格库塔、毕卡法等等，这里就不再介绍了。

代码演示

具体代码实现，可以参考我在安卓上实现的一个demo：https://github.com/Ncerzzk/IMUUpdate_Demo

该demo除了显示用上文算法计算出来的姿态角，还显示了用安卓自己的TYPE_GAME_ROTATION_VECTOR传感器算出来的姿态（不过由于欧拉角旋转顺序定义不一致，角度以及正负号有点偏差）

好了，以上就是这篇教程的内容了，断断续续写了好几天，中间也略过了很多推导和证明，如果想看很多结论更详细的证明，请阅读以下参考文献。

参考文献：

惯性导航 (秦永元 第三版）

https://krasjet.github.io/quaternion/quaternion.pdf

https://zhuanlan.zhihu.com/p/103623879