Pixhawk学习6.2——姿态解算

在PX4中，采用三轴陀螺，三轴加计，三轴磁力计采用EKF进行姿态解算。
（现在在examples/attitude_estimator_ekf里面，在cmake里面并没有打开，nuttx_px4fmu-v2_default里面用的是ekf2）
该算法中估计的是机体系下的量。

定义机体坐标系（前右下），导航坐标系（北东地）。

一、 概念定义

卡尔曼滤波，可以简单理解为先通过运动状态方程求得先验状态，再根据观测方程求得后验状态。与贝叶斯概率中的先验概率和后验概率有些相像。
贝叶斯概率更新有关的两个概念。假如某一不确定事件发生的主观概率 因为某个新情况的出现 而发生了改变，那么改变前的那个概率就被叫做先验概率，改变后的概率就叫后验概率。举个简单的更新概率的例子。
想象有 A、B、C 三个不透明的碗倒扣在桌面上，已知其中有（且仅有）一个瓷碗下面盖住一个鸡蛋。此时请问，鸡蛋在 A 碗下面的概率是多少？答曰 1/3。现在发生一件事：有人揭开了 C 碗，发现 C 碗下面没有蛋。此时再问：鸡蛋在 A 碗下面的概率是多少？答曰 1/2。注意，由于有“揭开C碗发现鸡蛋不在C碗下面”这个新情况，对于“鸡蛋在 A 碗下面”这件事的主观概率由原来的 1/3 上升到了1/2。这里的先验概率就是 1/3，后验概率是 1/2。
也就是说“先”和“后”是相对于引起主观概率变化的那个新情况而言的。

二、 算法解析

1 状态矩阵：

其中： w为机体角速度； c为机体角加速度（取acceleration的第二个字母）； a为机体加速度； m为机体磁力计；

2 量测矩阵：

其中： 为三轴陀螺仪输出； 为三轴加计输出； 为三轴磁力计输出；

3 状态方程：

该算法中，主要解决状态方程的问题。先写出状态方程如下：

其中：

上式中比较难理解的是加速度和磁力两项的状态方程是怎么得到的。以加计为例进行分析。
根据方向余弦矩阵的微分方程，加速度的微分方程可以表示为：

因此，在当对 求导时就会存在 和 矩阵。对 求导时会存在 和 矩阵。
（微分方程的理解：机体系下加计和磁力计的变化有两部分，即方向变化和大小变化。这也对应了 中的内容，即对角速度的偏导和对自身的偏导两部分。）
则一步转移矩阵为：

4 量测方程：

5 姿态计算：

根据上述EKF计算得到状态矩阵之后。
在提取状态信息这里需要说明的是：
1、 加速度矢量（7:9）反应的是姿态转移矩阵的Z列是没有问题的；
2、 按理说，如果定义导航坐标系为北东地，那么只有Xn、Zn向是有磁力信息的，Yn向为零。那么由姿态转移矩阵的Z列与机体磁力计叉乘出来的应该是姿态转移矩阵的X列，但下面公式里却是Y列，没想明白。不知道是不是定义的导航坐标系是东北天。但由后面欧拉角的计算来看，坐标系应该是北东地才对。
(犯懒截图)