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

MEMS器件的四元数EKF滤波

1，关于Kalman滤波

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

预测部分
$X_{k|k-1}=F_kX_{k-1|k-1}+B_ku_k$
$P_{k|k-1}=F_kP_{k-1|k-1}F_k^T+Q_k$
卡尔曼增益
$K_k=P_{k|k-1}H_k^T(H_kP_{k|k-1}H_k^T+R_k)^{-1}$
更新部分
$X_{k|k}=X_{k|k-1}+K_k(Z_k-H_kX_{k|k-1})$
$P_{k|k}=(I-K_kH_k)P_{k|k-1}$
其中
$Z=HX+V$
式中变量的含义：
$X--$ 状态向量
$P--$ 状态向量的协方差矩阵，表示状态的不确定度（或精度）
$X_{k|k}--$ $X$ 在 $k$ 时刻的最优估计（后验）
$X_{k|k-1}--$ 用 $k-1$ 时刻估计的状态量（先验）
$P_{k|k}--$ 后验协方差矩阵
$P_{k|k-1}--$ 预测协方差矩阵
$B_k--$ 控制矩阵
$u_k--$ 控制向量
$Q_k--$ 预测过程的噪声
$K_k--$ $k$ 时刻的卡尔曼增益
$H_k--$ $k$ 时刻状态转移矩阵
$Z_k--$ $k$ 时刻观测向量
$R_k--$ 观测噪声

2，关于EKF

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

在预测部分
$X_{k|k-1}=f(X_{k-1|k-1},u_k)$
由于 $f$ 是非线性的函数，我们把它在一点展开求它的 $Jacobians$ 矩阵
$F_k=\frac{\partial f}{\partial x}\bigg{|}_{X_{k-1|k-1},u_k}$
有时转移矩阵也需要线性化
$\begin{align} Z=h(X)+V \end{align}$
$H_k=\frac{\partial h}{\partial x}\bigg{|}_{X_{k|k-1}}$
这样可以写成：
$Z=HX+V$

3，实例化

现在我们要以MEMS的6DOF的imu器件为例看看怎么把EKF应用到以四元数为基础的姿态解算上，首先考虑下我们的器件特性，现认为陀螺仪在静差校正后的输出数学模型为：
$\begin{bmatrix} g_x\\ g_y \\ g_z\\ \end{bmatrix} = \begin{bmatrix} \omega_x+\delta_x \\ \omega_y+\delta_y \\\omega_z+ \delta_z \end{bmatrix}$
即认为陀螺仪的输出值符合正态分布（当然真实模型会复杂的多，不止包含白噪声）
$g = N(\omega,\delta)$
先把上面这个式子放一下，现在我们选取四元数作为机体的姿态状态量

$Q=\begin{bmatrix} q0 \\ q1 \\ q2 \\q3\end{bmatrix}$
然后我们要知道四元数的微分方程，以便更新四元数
$\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}$
我们可以用一阶龙格-库卡法来做数值积分，当然可以用更高阶的如二阶，四阶龙格-库卡法，这里用最简单的一阶（也就是矩形积分的形式）
$Q(T+t)=Q(t)+T\dot{Q}(t)$
现在我们得到了状态转移矩阵
$F_K = \frac{T}{2} \begin{bmatrix} 2&-\omega_x&-\omega_y&-\omega_z\\ -\omega_x&2&-\omega_z&-\omega_y\\ \omega_y&-\omega_z&2&\omega_x\\ -\omega_z&\omega_y&-\omega_x&2 \end{bmatrix} \tag{1-2}$
但注意到（1-2）中 $\omega$ 的真值是不知道的，而我们把陀螺仪的数据带进去是有误差的，下面我们需要定义协方差矩阵 $P$ 来描述状态向量的误差。
$P= E \left[ (X-E[X])(X-E[X])^T \right]$
$P_{i,j}=cov(x_i,x_j) \tag{2-1}$
很明显协方差矩阵 $P$ 是一个4x4的方阵， $Q$ 的四个元素一定不是相互独立的，在也是为什么用协方差矩阵来表示状态向量的分布情况。我们先把（2）式初始化为零，回到（1-2）讨论一下预测过程的噪声 $Q_k$ 该如何表示
由前面的讨论可以知道：
$\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}$
现在我们把这个矩阵乘出来，以第一行为例：
$q0_{k|k-1}=\frac{T}{2}(2q0_{k-1|k-1}-q1_{k-1|k-1}\omega_{x|k}-q2_{k-1|k-1}\omega_{y|k}-q3_{k-1|k-1}\omega_{z|k}) \tag{2-2}$
现在我们考虑现实情况把陀螺仪的输出值 $g$ 代替 $\omega$
前面说明了 $g=N(\omega,\delta)$ ，于是上式即为正太分布概率密度函数的四则运算问题，我们认为 $gyro_x,gyro_y,gyro_z$ 是互相独立的，这一点是非常重要的。
假设 $X_{1},X_{2}$ 是符合正态分布的概率密度函数 $X_{1}=N(\mu_1,\sigma_1^2) ,X_{2}=N(\mu_2,\sigma_2^2)$ $t$ 是一个常数。那么：
$tX_{1}=N(t\mu_1,t^2\sigma_1^2)$
$X_{1}$ 与 $X_{2}$ 相互独立时：
$X_{1}+X_{2}=N(\mu_1+\mu_2,\sigma_1^2+\sigma_2^2)$
回到式（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}$
式中 $gyro_x=N(g_x,\sigma_x^2)\\gyro_y=N(g_y,\sigma_y^2)\\gyro_z=N(g_z,\sigma_z^2)$
综上我们可以写出预测过程的噪声
$Q_k=\begin{bmatrix} Q_1&0&0&0 \\0&Q_2&0&0 \\0&0&Q_3&0 \\0&0&0&Q_4 \end{bmatrix} \tag{2-3}$
其中
$\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$
我们的观测量往往与我们选择的状态量不是同一个单位甚至不是同一个维度，因此需要引入转换矩阵
$Z=HX+V$
$Z$ 是我们的观测向量
$Z=\begin{bmatrix} a_x^b \\ a_y^b \\a_z^b\end{bmatrix}$
表示机体坐标系下重力加速度的分量
$V$ 是观测噪声，我们同样认为是白噪声（事实上忽略了机体运动的加速度)
根据惯性导航的知识（参考秦永元《惯性导航》）
$Z=\begin{bmatrix} a_x^b \\ a_y^b \\a_z^b\end{bmatrix} = \begin{bmatrix} 2g(q1q3 - q0q2)\\2g(q0q1 + q2q3)\\g(q0^2 + q3^2 - q1^2 - q2^2) \end{bmatrix}+V=h(X)+V$
我们在 $X = 0$ 点处将 $h(X)$ 线性化
$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}$
与 $Q_k$ 相同我们可以写出观测噪声矩阵也就是加速度计噪声 $R_k$
$R_k=\begin{bmatrix}\sigma_{ax}^2 & 0 & 0\\ 0 & \sigma_{ay}^2 & 0\\0 & 0 & \sigma_{az}^2\end{bmatrix}$
好了，到此所有卡尔曼需要的变量都推出来了，接下来只要按照卡尔曼的后三条公式依次计算卡尔曼增益，然后更新状态向量，再更新协方差矩阵就完成了一次迭代。

4，一些tips

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

5，结语

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

最后编辑于：2019.03.31 15:32:42

人面猴
序言：七十年代末，一起剥皮案震惊了整个滨河市，随后出现的几起案子，更是在滨河造成了极大的恐慌，老刑警刘岩，带你破解...
沈念sama阅读 205,132评论 6赞 478
死咒
序言：滨河连续发生了三起死亡事件，死亡现场离奇诡异，居然都是意外死亡，警方通过查阅死者的电脑和手机，发现死者居然都...
沈念sama阅读 87,802评论 2赞 381
救了他两次的神仙让他今天三更去死
文/潘晓璐我一进店门，熙熙楼的掌柜王于贵愁眉苦脸地迎上来，“玉大人，你说我怎么就摊上这事。” “怎么了？”我有些...
开封第一讲书人阅读 151,566评论 0赞 338
道士缉凶录：失踪的卖姜人
文/不坏的土叔我叫张陵，是天一观的道长。经常有香客问我，道长，这世上最难降的妖魔是什么？我笑而不...
开封第一讲书人阅读 54,858评论 1赞 277
港岛之恋（遗憾婚礼）
正文为了忘掉前任，我火速办了婚礼，结果婚礼上，老公的妹妹穿的比我还像新娘。我一直安慰自己，他们只是感情好，可当我...
茶点故事阅读 63,867评论 5赞 368
恶毒庶女顶嫁案：这布局不是一般人想出来的
文/花漫我一把揭开白布。她就那样静静地躺着，像睡着了一般。火红的嫁衣衬着肌肤如雪。梳的纹丝不乱的头发上，一...
开封第一讲书人阅读 48,695评论 1赞 282
城市分裂传说
那天，我揣着相机与录音，去河边找鬼。笑死，一个胖子当着我的面吹牛，可吹牛的内容都是我干的。我是一名探鬼主播，决...
沈念sama阅读 38,064评论 3赞 399
双鸳鸯连环套：你想象不到人心有多黑
文/苍兰香墨我猛地睁开眼，长吁一口气：“原来是场噩梦啊……” “哼！你这毒妇竟也来了？” 一声冷哼从身侧响起，我...
开封第一讲书人阅读 36,705评论 0赞 258
万荣杀人案实录
序言：老挝万荣一对情侣失踪，失踪者是张志新（化名）和其女友刘颖，没想到半个月后，有当地人在树林里发现了一具尸体，经...
沈念sama阅读 42,915评论 1赞 300
护林员之死
正文独居荒郊野岭守林人离奇死亡，尸身上长有42处带血的脓包…… 初始之章·张勋以下内容为张勋视角年9月15日...
茶点故事阅读 35,677评论 2赞 323
白月光启示录
正文我和宋清朗相恋三年，在试婚纱的时候发现自己被绿了。大学时的朋友给我发了我未婚夫和他白月光在一起吃饭的照片。...
茶点故事阅读 37,796评论 1赞 333
活死人
序言：一个原本活蹦乱跳的男人离奇死亡，死状恐怖，灵堂内的尸体忽然破棺而出，到底是诈尸还是另有隐情，我是刑警宁泽，带...
沈念sama阅读 33,432评论 4赞 322
日本核电站爆炸内幕
正文年R本政府宣布，位于F岛的核电站，受9级特大地震影响，放射性物质发生泄漏。R本人自食恶果不足惜，却给世界环境...
茶点故事阅读 39,041评论 3赞 307
男人毒药：我在死后第九天来索命
文/蒙蒙一、第九天我趴在偏房一处隐蔽的房顶上张望。院中可真热闹，春花似锦、人声如沸。这庄子的主人今日做“春日...
开封第一讲书人阅读 29,992评论 0赞 19
一桩弑父案，背后竟有这般阴谋
文/苍兰香墨我抬头看了看天上的太阳。三九已至，却和暖如春，着一层夹袄步出监牢的瞬间，已是汗流浃背。一阵脚步声响...
开封第一讲书人阅读 31,223评论 1赞 260
情欲美人皮
我被黑心中介骗来泰国打工，没想到刚下飞机就差点儿被人妖公主榨干…… 1. 我叫王不留，地道东北人。一个月前我还...
沈念sama阅读 45,185评论 2赞 352
代替公主和亲
正文我出身青楼，却偏偏与公主长得像，于是被迫代替她去往敌国和亲。传闻我的和亲对象是个残疾皇子，可洞房花烛夜当晚...
茶点故事阅读 42,535评论 2赞 343