基础知识回顾，在这篇文章中我提到了可以采用四元数作为EKF的状态量实现扩展Kalman滤波。借此机会动手编写完整的姿态解算，验证上述理论的实际效果
原理
在如何理解Kalman滤波中已经指出过典型的Kalman滤波的过程如下
预测过程
更新过程
要利用EKF估计姿态，首先要确定状态量x与观测量z。这里我们定义状态量为四元数，即当前姿态，而观测量则可以定义为加速度计测量出的重力加速度的方向，这个在一定程度上可以代表姿态值。
为了融合加速度计和角速度计的值，我们可以将预测状态量的过程表达为角速度为当前状态上积分出的值
第二步就是构造各个矩阵，在预测一步可以将角速度积分为四元数计算作为状态量的预测
恰好这一过程可以写为一个矩阵乘以四元数状态量，于是可以得到矩阵A
另一个是过程噪声Q矩阵变换到四元数状态量上的矩阵W，由于此时预测的噪声即为三轴角速度的噪声，而获得先验预测方差一步需要计算四元数状态量的噪声，W矩阵即完成了角速度噪声转换到“四元数的噪声”的变换。
W矩阵可对式1中的各个角速度积分得到
下一步是求解H矩阵，也即四元数转换为当前姿态的重力加速度的方向的过程。在上一篇文章中提到过四元数转换为方向余弦矩阵的方法，如果仔细观察方向余弦矩阵的末行三个元素，就会发现这其实是重力加速方向矢量的三个维度的量。
但是这一转换过程是非线性的，需要进行线性化。写出其对应的非线性转换函数h，并求该函数的Jacobian矩阵作为Kalman滤波器的H矩阵
最后将求得的矩阵代入上述Kalman的过程中即可
算法实现过程
总而言之，上述设计的EKF的整个过程描述如下
- 构造EKF的预测方程并计算先验状态和先验方差
- 构造对应的非线性四元数转换到重力加速度方向矢量的函数h以及求偏导后的H矩阵，并执行更新过程（更新过程应采用未简化的公式，原因在文末指出）
- 最后归一化四元数并修正方差。由于归一化导致Kalman滤波器输出的值发生非线性改变，根据论文指出了这种带约束的Kalman方差修正方法
考虑到归一化处理的函数为
可计算出对应的Jacobian矩阵，并计算修正后的方差
- 如果需要，可能还要将四元数转换为欧拉角
参数设置
由于加速度计与角速度计各轴间的噪声理论上相互独立，故轴与轴之间的协方差为零。因此在上述EKF算法过程中，过程噪声方差矩阵Q与测量噪声方差矩阵R可定为对角阵，即
其中Q矩阵和R矩阵的σ值虽然也可以通过后续试凑获得，但是我们也可以根据IMU数据手册上的噪声密度以及带宽计算出对应的σ
同时为了减小外部冲击对加速度计的影响，通常会根据加速度计偏离一个g的程度大小动态地调整在R矩阵的σ值的大小，相当于减小加速度计的置信度
嵌入式平台实现
对于多维的Kalman滤波器的在嵌入式平台上的实现，基础的操作就是矩阵运算。需要至少实现矩阵加减，乘法，求逆以及转置等操作。在Arm平台上的实现可借助CMSIS库中的标准函数。当然也有四元数的操作，不过仅仅是需要将四元归一化。
经过测试可用的代码在Github的项目内，采用QMI8658C+STM32F302的硬件进行实时解算，理论上支持移植至任意带DSP库的Arm处理器或微控制器内运行
更新：修复了方差更新的错误，旧版的方差更新变量位置有误
FAQ
这个Kalman滤波器似乎输出不稳定，有时候会不收敛
实际上在最后一步更新方差的过程中，我们采用了简化后的公式
有资料指出，在实际计算中这条公式会由于数值计算误差而出现明显的不稳定，解决办法当然是老老实实用未简化的公式计算Kalman算法运行的速度欠佳
Kalman滤波融合IMU的缺点也在于此。Mahony算法虽然简单其优势在于运算速度较快，在大多数嵌入式平台上都可以完成。而Kalman滤波即使在带浮点运算单元的情况下也需要大量的乘法和加法运算。或许后续可以尝试定点运算的优化？这样Arm里的SIMD可以进一步加速运算。该算法的缺点或者可以改进的地方？
除了上面提到的运行速度外，在Kalman滤波器中采用四元数也有先天性的缺陷。
由于Kalman滤波器本身是处理线性问题的，而姿态到四元数之间的转换关系总是非线性的，因此在执行更新过程中将四元数通过一个线性的增益进行融合似乎不太合适。另外四元数本身代表的姿态包含了航向角(Yaw)，但是对于航向角的计算仅仅是通过预测过程也就是角速度计数据完成，而加速度计的数据又不能参与到更新过程中航向角的计算。也就是相当于Kalman滤波器中对于航向角的反馈消失了，这样航向角就不是严格稳定收敛的。实际测试上面算法的时候在大部分情况下都能正确，但是在一些情况下航向角并不能很好地收敛。