基础知识回顾，在这篇文章中提到了Heikki Hyyti, Arto Visala的论文A DCM Based Attitude Estimation Algorithm for Low-Cost MEMS IMUs所采用的算法。这篇文章将详细补充评析论文中原理与实现的各个细节.
原理
基础知识
在如何理解Kalman滤波中列举过典型的Kalman滤波的过程如下
预测过程
更新过程
各种算法的概述里提到了方向余弦矩阵(DCM)。这也是论文中表示姿态的最主要的方式，后续Kalman滤波器的状态也与矩阵有关。
DCM矩阵的明显特征是，三个欧拉角均在第一列和末行以较为简洁的形式出现，并且末行元素只含Pitch角和Roll角，第一列的元素只含Pitch角和Yaw角，并且左下角元素只与Pitch角有关。只要计算出这几个元素即可较为简单地求解出三个欧拉角的大小，四元数表示的姿态转换到欧拉角也是采用这种方式。
另外末行还有实际的物理意义。假设变换的原坐标系为世界坐标系，末行三个元素组成的向量恰好所表示的就是向量[0,0,1]在旋转后的坐标系下的表示，而世界坐标系下的[0,0,1]可认为是重力加速度的方向，因此末行三个元素可以代表在旋转变换后的坐标系下的重力加速方向。这个结论可以直接将加速度计测量值与方向余弦矩阵联系起来。
方向余弦矩阵的微分可以用如下方式表示
主要过程
类似上一篇文章的方法，但是状态变量设置为DCM末行与角速度计的偏置，测量变量也同样设置为重力加速度矢量的方向，也即DCM矩阵末行，观测过程由角速度计给出的数据完成。即设置状态变量x为
将方向余弦矩阵的微分形式提取其中的末行展开后将其重写为矩阵与角速度三维矢量相乘的结果
实际上这一过程的目的为了通过角速度对方向余弦矩阵末行积分，也相当于是对方向余弦矩阵求角速度矢量的Jacobian矩阵，即[C3×]矩阵
根据上述方程，Kalman的观测过程可以写为
更新过程的H矩阵则是将DCM末行矢量换算为实际加速度测量值，也即乘以一个重力加速度的值，测量值中不含角速度偏置故还需要在对角阵下增加增加0
过程噪声矩阵Q和测量噪声矩阵R的构造上，均假设各个状态变量和测量值之间相互独立两者均设置为对角阵。但需要注意的一点是为了增加剧烈运动下的非重力加速度对滤波器的影响，对于R矩阵其参数根据非重力加速度调节。越大的非重力加速度就意味着该过程加速度测量越不可信
按照原论文的解释，由于角速度偏置随时间变化较小，角速度偏置的过程噪声方差σ_b应给到一个足够大的值，确保其尽可能接近预测过程的结果（即不发生改变）。而非重力加速的计算则是将Kalman滤波输出的状态量DCM末行矢量与加速度计测量得到的值相减得到
其他实现细节
上面主要提及了Kalman滤波器的几个关键矩阵的获得与设置，这里还需要一些额外的篇幅讨论其中所需要解决一些其他的问题
Yaw轴角度计算
这个Kalman滤波器仅仅融合得到的结果是DCM的末行，显然末行中无Yaw角度，因此仅根据Kalman滤波的结果是没有办法计算出Yaw轴角度，需要在滤波器外进行Yaw角度的积分。要完成的工作简单来讲就是把整个DCM矩阵重新算一次，即除了末行外还需要重算第二第一行。
具体在代码里主要有两种思路，两种均围绕获得DCM矩阵的第一列（与欧拉角的关系简单且只与Yaw和Pitch角度有关）求解。
第一种是老老实从欧拉角出发计算其正弦和余弦值，当然可以利用已有的上一次计算出的DCM末行来替换某些项，最终只需要Yaw的正弦&余弦值即可。最后用上面提到的方向余弦矩阵的微分更新DCM首列的其余两个值，即可求解Yaw角度。第二种办法则是增加额外的DCM首行作为记录，角速度积分得到旋转后的第二行整行的元素，归一化后再重新求出第一行，最终同样用第一列求出Yaw角度。
在Github上的代码内Matlab和C++代码内均可见这两种方法的实现
状态约束
上述滤波过程的在最终输出前增加了归一化的动作，实际上这一约束对实际的Kalman滤波过程的方差是有影响的。在S.J.Julier and J.J.La Viola Jr.,”On Kalman filtering with nonlinear equality constraints”的论文中提到了考虑状态约束的Kalman这一问题的解决方法。由于状态归一化的过程可以表示为
对于最终方差乘以上述归一化变换过程的Jacobian矩阵即可
求解的具体矩阵在论文的Appendix内也有提到
零偏估计
这个问题的解决方案是我对这一存在一定疑问的地方。因为从给出的测量模型来看零偏并不完全可观，当然一种可行的解释是利用了加速计的测量值反过来估计出三轴角速度的偏置。然而这样做的问题在于，对于六轴的IMU而言加速度计的测量值不能估计出等效Yaw轴的角速度偏置，也就是说存在角速度偏置不能由测量过程验证的情况。
实际上在六轴融合算法上总是存在一个问题是，角速度计能够测量到的维度总是高于加速度计，融合过程的状态应该采用哪个。采用DCM末行作为状态量则是采用较低测量维度即加速度计的测量值作为待估计的状态。而上一篇文章中则是采用较高的测量维度。较低的测量维度能够使得状态完全可观并且带Kalman滤波器的反馈回路，然而对于六轴IMU而言Yaw轴角度的获得需要额外的操作，而高的测量维度则恰好相反，存在不可测量的部分但是将Yaw的状态也纳入了滤波器内。
或许这个算法本身的零偏估计应该在该Kalman滤波器外完成？或者人为增加一些虚假的测量值（实际上是由预测过程得到的值）来确保Yaw的状态可观？