卡尔曼滤波主要分两个步骤，预测加校正。预测是基于上一时刻的状态对当前状态进行估计，校正是根据当前状态的观测与上一时刻的估计进行综合分析，估计出系统的最优状态值，然后下一时刻接着重复这个过程；卡尔曼不断的进行迭代，它不需要大量的粒子状态输入，只需要过程量，因此它的速度很快，非常适合线性系统的状态估计。

众所周知卡尔曼滤波在处理IMU传感器数据融合中作用巨大，但在实际实现起来并非那么容易；本文从MPU6050入手，分析卡尔曼滤波的使用。

本篇文章需要你在夜深人静的时候、先去冲一杯咖啡、准备一张纸、一支笔……

卡尔曼滤波

从来没有坐下来认真的计算卡尔曼滤波的公式由来以及它背后更深层次的原理，为什么在处理加速度以及陀螺仪的数据融合中卡尔曼滤波就那么的有效。但是对于大多数人来说，可能更感兴趣的是如何正确的去使用它，卡尔曼滤波的那五个公式到底怎么使用。

开始之前需要你具备一定的矩阵乘法、矩阵变换等知识，大家都知道矩阵乘法的重要性，不夸张的说，不懂矩阵乘法根本做不了复杂的模型。当然本篇涉及到的矩阵乘法没那么复杂，如果忘记了请翻大学时的课本脑补，或参考以下网站:

• http://en.wikipedia.org/wiki/Matrix_multiplication#Matrix_product_.28two_matrices.29

• http://www.mathwarehouse.com/algebra/matrix/multiply-matrix.php

• http://en.wikipedia.org/wiki/Transpose

• http://en.wikipedia.org/wiki/Covariance_matrix

卡尔曼滤波器简单讲就是一个算法，它使用一系列随着时间推移的包含噪声的观察、测量，基于系统当前状态以及系统的前一个状态进行估计运算，不断的迭代，最终估计出系统的最优状态，类似加权平均。

对于mpu6050这个IMU惯性测量模块来讲，相对于加速度计，陀螺仪的精度会高一些，但是时间长了它会漂移；加速度计虽然精度不高，但是没有明显的随着时间增加的漂移。

有人会说那么短时间内我们相信陀螺仪，长时间内我们可以相信加速度计。可以使用互补滤波来处理这个问题；加速度计可以用一个数字低通滤波器来处理，陀螺仪可以用数字高通滤波器。但是它并不如卡尔曼滤波的效果好，当然也有很多人使用一阶互补滤波成功的应用到自己的产品中，比如平衡车以及四旋翼飞行器中。

关于互补滤波与卡尔曼的比较请移步这里：

• http://robottini.altervista.org/kalman-filter-vs-complementary-filter

卡尔曼滤波器根据系统的观察和测量对系统做出最优的状态估计，它需要知道系统的测量噪声，以及系统本身的噪声，我们将系统本身的噪声称之为过程噪声。并且假定这些噪声符合高斯分布，令人庆幸的是现实中大部分随机噪声都是符合高斯分布的。如果忘记了什么是高斯分布，请回去翻高等数学脑补。

关于卡尔曼滤波的原理推导请参考：

• http://en.wikipedia.org/wiki/Kalman_filter

• http://www.cs.unc.edu/~welch/media/pdf/kalman_intro.pdf

• http://academic.csuohio.edu/simond/courses/eec644/kalman.pdf

系统状态 xk

接下来的部分可能比较费解，但是只要你坐下来准备好纸和笔，一步一步跟着来，也没那么难理解。

关于矩阵的乘法计算 可以使用这个在线计算器

• http://www.wolframalpha.com

关于本篇文章的几点声明：

a.如果矩阵是个常量，不依赖于当前时间，那么就不用写下标 kk。

b.前一个系统状态估计值，英文称为prveious，即 k−1k−1 时刻的系统状态估计值：

c.先验状态估计值，英文称为 priori，简单讲就是对系统状态的估计值：

d.后验状态 英文称为posteriori，简单讲就是对系统的测量值即观测值：

对于我们的惯性测量模块，问题是系统状态是隐藏的，我们不能直接获取系统的角度，只能通过观测方程 zkzk 来观测。这也称为隐马尔科夫模型。不懂什么叫隐马尔可夫模型，请移步这里：

• https://en.wikipedia.org/wiki/Hidden_Markov_model

这就意味着系统状态是与当前时刻 kk 的状态以及之前的状态有直接关系的，如果没有使用卡尔曼滤波是无法更真实的得到系统的状态的。

在 xx 上面加一个帽子 x^x^ 意思是表示状态的估计。xx 则表示系统的真实状态，我们试图努力去估计的值。
因此用 xkxk 表示 kk 时刻系统的真实状态。

参考卡尔曼滤波的五个方程，系统状态方程可以这样给出：

xkxk 为系统状态矩阵，大小为 n∗1n∗1 列，对于MPU6050，n为2，因此可以这样表示系统状态矩阵：

θθ 表示角度，θ`bθ`b 表示系统角速度偏差，基于加速度计和陀螺仪测量的角速度偏差即陀螺仪的漂移量。理论上，如果从陀螺仪的测量值减去这个偏差就可以得到陀螺仪的真实角速度值。

FF 为状态转移模型矩阵，大小为 n∗nn∗n ，对于本系统此处为:

先不要管为什么是 −Δt−Δt ，接着往下看。

ukuk 是系统控制输入，大小为 k∗1k∗1，对应我们的系统那就是陀螺仪在当前时刻 kk 的测量值即角速度，单位是(°/s),我们使用 θ`θ` 表示角速度，因此系统状态方程可以写成：

BB 表示控制输入模型矩阵，大小为 n∗kn∗k，对于我们的系统应该定义如下：

当用控制输入模型矩阵 BB 去和 θ`θ` 相乘时，第一行的 ΔtΔt 与 θ`θ` 相乘可以直接得到角度 θθ，因为我们不能直接通过角速度的计算得到偏差，因此，我们将矩阵 BB 的底部设为0。

wkwk 是 kk 时刻的过程噪声，符合均值为0协方差为 QQ 的高斯分布：

QkQk 是过程噪声协方差矩阵，对于我们的系统那就是加速度计的状态估计协方差以及陀螺仪的偏差协方差。我们假定系统的加速度计以及陀螺仪偏差估计都是独立的，即是相互解耦的，因此可以用对角矩阵表示：

可以看到，过程噪声协方差矩阵 QkQk 是跟时间相关的，它依赖于当前时刻k。

这将造成过程噪声随着时间的增加越来越大，比如陀螺仪的漂移。这些常数是卡尔曼滤波器工作的前提，必须知道这些参数才能正确的使用卡尔曼滤波。

如果 QkQk 设置了一个较大的值，那么系统的噪声也会随之增大。

如果我们的角度估计值开始漂移，那么我们就必须增加 Qθ`bQθ`b；

如果对系统的估计趋势变慢，意味着对角度的信任度过高，那么我们应该降低 QθQθ 的值，以便于提高系统的动态响应能力。

测量值 zk

测量值或称为观测值，有以下方程给出：

zkzk 为系统测量值，大小为 m*1，vkvk 为测量噪声，HH 为称为观测模型矩阵，大小为 m∗nm∗n ，作用是将系统真实状态空间映射到观测空间。系统的真实状态是无法观测的，因为测量仅仅对加速度计的测量。对于本系统：

测量噪声也是假定符合均值为0协方差为R的高斯分布：

因为R不是一个矩阵，测量噪声和测量的方差相等，因为同一个变量的协方差和方差相等。
因此我们将R定义为：

我们假定测量噪声是不随时间改变的，即我们假定测量噪声是个固定值，那么可以写成：

如果将测量噪声方差 var(v)var(v) 设定的太大，卡尔曼滤波器的响应会变慢，因此他会对新测量值的信任度降低。但是如果测量噪声方差 var(v)var(v) 设置的太小，那么预测值的权重会增大，因为我们对加速度计的测量值信任度更高。

因此我们必须想办法找出过程噪声方差以及测量噪声方差，有很多种方法找到他们，但是具体怎么去找不在本篇的讨论范围内。

卡尔曼滤波方程

下面我们将使用卡尔曼滤波方程估计出系统在k时刻的真实状态 x^kx^k。

预测阶段

卡尔曼滤波的五个方程中预测阶段由两个方程组成。在这两个方程中，我们将试图预测系统的当前状态即k时刻的状态，以及在k时刻的误差协方差矩阵。

卡尔曼滤波器首先基于系统之前的状态和陀螺仪的测量值估计系统的当前状态，即k时刻的系统估计先验状态估计值：

接下来我们将基于前一个误差协方差矩阵 Pk−1|k−1Pk−1|k−1

试图估计这个先验误差协方差矩阵 Pk|k−1Pk|k−1，,它的定义如下：

这个矩阵表示我们对当前预测状态的信任度，它越小说明我们越相信当前预测状态，上面方程的原理很容易理解，它表明误差协方差会随着上一次的系统预测状态增加，因此我们将误差协方差矩阵与状态转移模型 FF 以及状态转移模型矩阵的转置矩阵 FTFT相乘，最后和当前时刻k的过程噪声 QkQk 相加 。

对于我们的系统，误差协方差矩阵 PP 是一个2x2的矩阵

更新阶段

首先我们计算出实际测量值 zkzk

和先验状态即预测值 xk|k−1xk|k−1 的差别，这也叫做残差：

它表示测量值和预测值的偏离程度，如果残差为0那么意味着测量值和预测值完全吻合。

观测模型 HH 用于将先验状态 xk|k−1xk|k−1 映射到观测空间，所谓的观测也就是对于加速度的测量，因此残差 y~ky~k 不是一个矩阵：

接下来我们将要计算残差协方差：

它的计算基于先验误差协方差矩阵 Pk|k−1Pk|k−1

和测量协方差矩阵 RR，观测模型 HH 用于将先验误差协方差矩阵 Pk|k−1Pk|k−1 映射到观测空间。

测量噪声越大 SS 的值越大，也就意味着我们不能过于信任测量值；对于我们的系统，SS 不是一个矩阵:

接下来计算卡尔曼增益，卡尔曼增益用于表示我们对于残差 y~ky~k 的信任度，它以下式定义：

从卡尔曼的增益公式可以看出，我们对残差 y~ky~k 的信任度越小，y~ky~k 的协方差将会越大；如果对于估计状态的信任度越高，误差协方差矩阵将会越小，因此卡尔曼增益将会随之变小；反之，如果我们对于 y~ky~k 的信任度越高，那么我们对于当前状态的预测的信任度就会降低。

观测模型 HH 的转置用于将误差协方差矩阵 PP 映射到观测空间，如果不知道误差协方差矩阵 PP 的起始值，我们可以将 PP 设置为：

LL 初始以一个较大的值代替。对于我们的系统，一开始的起始角度是确定的，而且一开始的陀螺仪偏差也是可以校正的，因此我们可以认为系统的初始状态是确定的，因此我们可以初始化误差协方差矩阵为：

卡尔曼增益是一个2x1的矩阵：

现在我们可以更新当前状态的后验估计：

卡尔曼增益 KkKk

与 残差 y~ky~k 的乘积

再加上先验状态 x^k|k−1x^k|k−1，这就是卡尔曼滤波的本质，卡尔曼增益 KkKk 作为一个加权值，决定我们对于系统观测值与估计值的信任度。

yk~yk~

是测量值 zkzk

与先验状态估计值 x^k|k−1x^k|k−1 的差。

最后我们需要更新后验协方差矩阵：

公式中的 II 是单位矩阵：

卡尔曼滤波器可以基于我们对于系统状态估计值的修正度来自动修正误差协方差矩阵。修正系统的状态既要基于误差协方差矩阵Pk|k−1Pk|k−1 也要基于残差 y~ky~k 协方差 SkSk。

代码实现

下面关键的部分来了，你一定迫切的想知道如何用代码实现上面的卡尔曼公式，计算出系统的角度、偏差等。代码参考请移步：

https://github.com/TKJElectronics/KalmanFilter

Step 1:

从以上计算可以看出角度的先验状态估计 θ^k|k−1θ^k|k−1

等于前一个状态估计 θ^k−1|k−1θ^k−1|k−1

加上角速度与时间 −Δt−Δt 的乘积。

因为偏差不能通过直接测量得到，可以认为先验状态偏差等于它前一个状态的偏差，因此可以用C语言这样计算角速度rate和角度angle：

1
2

rate = newRate -bias;
angle += dt*rate

Step 2:

误差协方差矩阵写成C语言：

1
2
3
4

P[0][0] += dt * (dt*P[1][1] - P[0][1] - P[1][0] + Q_angle);
P[0][1] -= dt * P[1][1];
P[1][0] -= dt * P[1][1];
P[1][1] += Q_gyroBias * dt;

Step 3:

角度的残差写成C语言：

1

y = newAngle - angle;

Step 4:

C代码实现：

1

S = P[0][0] + R_measure;

Step 5:

值得注意的是在其他系统下 SS 可能是一个矩阵，所以在某些系统下不能简单的用 PP 去除以 SS ,而是需要求矩阵的逆。

C语言实现为：

1
2

K[0] = P[0][0] / S;
K[1] = P[1][0] / S;

Step 6:

C代码实现：

1
2

angle += K[0] * y;
bias += K[1] * y;

Step 7:

C代码实现：

1
2
3
4
5
6
7

float P00_temp = P[0][0];
float P01_temp = P[0][1];

P[0][0] -= K[0] * P00_temp;
P[0][1] -= K[0] * P01_temp;
P[1][0] -= K[1] * P00_temp;
P[1][1] -= K[1] * P01_temp;

经过验证，初始参数设置为以下值时适用于大多数的IMU，并且这些初始参数将会使mpu6050工作在最佳状态；

1
2
3

float Q_angle = 0.001;
float Q_gyroBias = 0.003;
float R_measure = 0.03;

参考：

• http://blog.tkjelectronics.dk/2012/09/a-practical-approach-to-kalman-filter-and-how-to-implement-it/

• https://blog.csdn.net/heyijia0327/article/details/17487467

• https://github.com/xiahouzuoxin/kalman_filter

• http://en.wikipedia.org/wiki/Kalman_filter

• http://www.cs.unc.edu/~welch/media/pdf/kalman_intro.pdf

• http://academic.csuohio.edu/simond/courses/eec644/kalman.pdf

• 本文作者： Steven Shi

• 本文链接： http://stevenshi.me/2018/05/03/kalman-filter-implement/

• 版权声明： 本博客所有文章除特别声明外，均采用 CC BY-NC-SA 3.0 许可协议。转载请注明出处！