imu姿态估计(MEMS器件的四元数EKF滤波)

MEMS器件的四元数EKF滤波

1，关于Kalman滤波

本文应用的是MEMS器件的IMU,姿态的表示为四元数形式（不是网上的单轴卡尔曼滤波！不过原理都是一样的）
首先是卡尔曼的五条公式

预测部分

卡尔曼增益

更新部分

其中

式中变量的含义：
状态向量
状态向量的协方差矩阵，表示状态的不确定度（或精度）
在时刻的最优估计（后验）
用时刻估计的状态量（先验）
后验协方差矩阵
预测协方差矩阵
控制矩阵
控制向量
预测过程的噪声
时刻的卡尔曼增益
时刻状态转移矩阵
时刻观测向量
观测噪声

2，关于EKF

卡尔曼滤波是由线性系统推导来的，为了应用于非线性系统，我们可以把非线性系统在平衡点附近线性化，这样就有了EKF（扩展卡尔曼滤波）

在预测部分

由于是非线性的函数，我们把它在一点展开求它的矩阵

有时转移矩阵也需要线性化

这样可以写成：

3，实例化

现在我们要以MEMS的6DOF的imu器件为例看看怎么把EKF应用到以四元数为基础的姿态解算上，首先考虑下我们的器件特性，现认为陀螺仪在静差校正后的输出数学模型为：

即认为陀螺仪的输出值符合正态分布（当然真实模型会复杂的多，不止包含白噪声）

先把上面这个式子放一下，现在我们选取四元数作为机体的姿态状态量

然后我们要知道四元数的微分方程，以便更新四元数
$\dot{Q} = \begin{bmatrix} \dot{q0} \\ \dot{q1} \\ \dot{q2} \\ \dot{q3} \end{bmatrix} = \frac{1}{2} \begin{bmatrix} 0&-\omega_x&-\omega_y&-\omega_z\\ -\omega_x&0&-\omega_z&-\omega_y\\ \omega_y&-\omega_z&0&\omega_x\\ -\omega_z&\omega_y&-\omega_z&0 \end{bmatrix} \begin{bmatrix} q0\\q1\\q2\\q3 \end{bmatrix} \tag{1-1}$
我们可以用一阶龙格-库卡法来做数值积分，当然可以用更高阶的如二阶，四阶龙格-库卡法，这里用最简单的一阶（也就是矩形积分的形式）

现在我们得到了状态转移矩阵

但注意到（1-2）中的真值是不知道的，而我们把陀螺仪的数据带进去是有误差的，下面我们需要定义协方差矩阵来描述状态向量的误差。

很明显协方差矩阵是一个4x4的方阵，的四个元素一定不是相互独立的，在也是为什么用协方差矩阵来表示状态向量的分布情况。我们先把（2）式初始化为零，回到（1-2）讨论一下预测过程的噪声该如何表示
由前面的讨论可以知道：
$\begin{bmatrix}q0_{k|k-1} \\ q1_{k|k-1} \\q2_{k|k-1} \\q3_{k|k-1}\end{bmatrix}=\frac{T}{2}\begin{bmatrix} 2&-\omega_{x|k}&-\omega_{y|k}&-\omega_{z|k}\\ -\omega_{x|k}&2&-\omega_{z|k}&-\omega_{y|k}\\ \omega_{y|k}&-\omega_{z|k}&2&\omega_{x|k}\\ -\omega_{z|k}&\omega_{y|k}&-\omega_{x|k} &2 \end{bmatrix} \begin{bmatrix} q0_{k-1|k-1} \\ q1_{k-1|k-1} \\q2_{k-1|k-1} \\q3_{k-1|k-1} \end{bmatrix}$
现在我们把这个矩阵乘出来，以第一行为例：

现在我们考虑现实情况把陀螺仪的输出值代替
前面说明了，于是上式即为正太分布概率密度函数的四则运算问题，我们认为是互相独立的，这一点是非常重要的。
假设是符合正态分布的概率密度函数是一个常数。那么：

与相互独立时：

回到式（2-2）我们可以把它变成：
$\begin{cases} q0_{k|k-1} = N(\frac{T}{2}(2q0_{k-1|k-1}-q1_{k-1|k-1}g_{x|k}-q2_{k-1|k-1}g_{y|k}-q3_{k-1|k-1}g_{z|k}),\sigma_0^2) \\ \sigma_0^2 = (\frac{T}{2})^2(q1_{k-1|k-1}^2\sigma_x^2 + q2_{k-1|k-1}^2\sigma_y^2 + q3_{k-1|k-1}^2\sigma_z^2 ) \end{cases}$
式中
综上我们可以写出预测过程的噪声

其中
$\begin{cases}Q_1=\sigma_0^2 = (\frac{T}{2})^2(q1_{k-1|k-1}^2\sigma_x^2 + q2_{k-1|k-1}^2\sigma_y^2 + q3_{k-1|k-1}^2\sigma_z^2 ) \\Q_2=\sigma_1^2 = (\frac{T}{2})^2(q0_{k-1|k-1}^2\sigma_x^2 + q2_{k-1|k-1}^2\sigma_z^2 + q3_{k-1|k-1}^2\sigma_y^2 ) \\Q_3=\sigma_2^2 = (\frac{T}{2})^2(q0_{k-1|k-1}^2\sigma_y^2 + q1_{k-1|k-1}^2\sigma_z^2 + q3_{k-1|k-1}^2\sigma_x^2 ) \\Q_4=\sigma_3^2 = (\frac{T}{2})^2(q0_{k-1|k-1}^2\sigma_z^2 + q1_{k-1|k-1}^2\sigma_y^2 + q2_{k-1|k-1}^2\sigma_x^2 ) \end{cases}$
ok现在我们的预测过程结束了！下面我们要用传感器的观测来修正预测，在此之前我们先来讨论下转换矩阵
我们的观测量往往与我们选择的状态量不是同一个单位甚至不是同一个维度，因此需要引入转换矩阵

是我们的观测向量

表示机体坐标系下重力加速度的分量
是观测噪声，我们同样认为是白噪声（事实上忽略了机体运动的加速度)
根据惯性导航的知识（参考秦永元《惯性导航》）

我们在点处将线性化
$H_k = \frac{\partial h}{\partial x}\bigg{|}_{X_{k|k-1}=0}=2\begin{bmatrix} -q2_{k|k-1}&q3_{k|k-1} & -q0_{k|k-1} & q1_{k|k-1}\\q1_{k|k-1}&q0_{k|k-1}&q3_{k|k-1}&q2_{k|k-1}\\q0_{k|k-1}& -q1_{k|k-1}& -q2_{k|k-1}&q3_{k|k-1}\\ \end{bmatrix}\tag{3-1}$
现在我们看下整个状态转换的过程：

$\begin{bmatrix} a_x^b \\ a_y^b \\a_z^b \end{bmatrix}= 2 \begin{bmatrix} -q2_{k|k-1}&q3_{k|k-1} & -q0_{k|k-1} & q1_{k|k-1}\\q1_{k|k-1}&q0_{k|k-1}&q3_{k|k-1}&q2_{k|k-1}\\q0_{k|k-1}& -q1_{k|k-1}& -q2_{k|k-1}&q3_{k|k-1}\\ \end{bmatrix} \begin{bmatrix} q0_{k|k-1}\\q1_{k|k-1} \\q2_{k|k-1}\\q3_{k|k-1} \end{bmatrix}$
与相同我们可以写出观测噪声矩阵也就是加速度计噪声

好了，到此所有卡尔曼需要的变量都推出来了，接下来只要按照卡尔曼的后三条公式依次计算卡尔曼增益，然后更新状态向量，再更新协方差矩阵就完成了一次迭代。

4，一些tips

1.由于机体振动的影响，加速度计的可取变量，在加速度变化剧烈时取大些
2.对于资源有限的MCU来说，矩阵运算非常耗时，可以把矩阵乘开，含零的项就可以不写到代码中了(不过这样牺牲了可读性)

5，结语

后续的笔记再慢慢补充吧！
如有错误欢迎指出