卡尔曼滤波器家族与观测器

  1. 1. 卡尔曼滤波KF
  2. 2. 扩展卡尔曼滤波EKF
  3. 3. 无迹卡尔曼滤波UKF
  4. 4. 再升级该怎么办?
  5. 5. 总结一下

在之前的文章(现代控制理论总结1——Linear部分)中曾经提到过观测器。
观测器可以这样理解:
使用软件构建一个虚拟的系统,使这个系统与实际系统同步更新,并根据给予的输入与传感器测得的输出不断更新、纠正这个虚拟系统,使得系统的状态变量趋近于实际系统,这样我们就能够获得一些测不到的状态变量,来进行状态反馈。
而卡尔曼滤波器则通过对系统噪声(高斯分布噪声)建立模型,在更新系统状态的同时也在更新系统的方差,也可以看作是同时更新系统的均值与方差,同时考虑状态更新方差与测量方差,得到一个卡尔曼增益K,这个卡尔曼增益则对应着观测器中的观测增益L。使用每次后验的均值作为结果,来不断更新系统。

我们来进一步,仔细地看一看Kalman Filter是如何工作的。

卡尔曼滤波KF

首先要注意,卡尔曼滤波器只适用于线性系统,并且噪声应为高斯分布的系统。
既然是一个线性系统,那么我们就可以使用状态方程与测量方程(对于离散系统,使用差分方程)来表示这个系统:
Snipaste_2020-04-16_21-19-41.png
通过以下方程更新一个虚拟系统:
Snipaste_2020-04-16_20-05-44.png
其中参数的含义:
Snipaste_2020-04-16_20-06-03.png
我们先使用上一时刻估计得到的状态变量,通过一个完全理想化的系统(不一定完全准确),得到本时刻先验的状态变量;
使用同样的系统更新本时刻协方差矩阵的先验,注意由于每次更新都使用了不准确的更新方程,我们要将这个不确定性考虑进去,所以每次都要引入系统更新带来的噪声的协方差;
由于使用的是理想的系统,没有考虑系统误差、扰动带来的影响,这样的开环观测器一定是无法准确观测的。
接着使用测量方程,通过先验的状态变量得到先验的测量量;
同理,使用相同的测量方程得到测量量的协方差,这里同样也要引入一个由于每次测量不准确噪声的协方差;
使用状态量协方差与测量量协方差计算卡尔曼增益;
使用卡尔曼增益作为权重,权衡测量量与先验状态量,来更新本时刻的后验状态量。
至此,一次迭代就完成了,让这个虚拟系统运行下去,就能使估计值趋向于真实值。

卡尔曼滤波器主要需要调整的参数使Q、R两个协方差矩阵的值,而其他的值都会在迭代中收敛,如何整定Q、R参数呢?
我们知道Q、R代表着系统更新引入噪声的协方差与测量引入噪声的协方差,我们很容易想到,Q越大,我们就会越“相信”(“相信”体现在卡尔曼增益上)测量,滤波器收敛变快,但由于测量带来的噪声也随之增大;而R越大我们则越“相信”系统更新,结果更加平滑但收敛变慢。通常我们忽略各个状态变量之间、测量量之间的影响,认为它们使无关的,这样得到的Q、R阵都是对角阵,更方便我们去调参。

何时该使用卡尔曼滤波器?
当系统方程比较清晰,并且单独使用传感器误差难以接受的情况下,使用系统方程辅助获得数据也是一种弥补的方法。
与此同时,卡尔曼滤波不仅能够对传感器数据进行平滑,还能够得到完整的状态向量,这意味着我们能够通过一个单独的位置传感器得到速度,加速度等等,这样我们就能对目标进行预测,Robomaster比赛中的视觉目标跟踪,射击预瞄提前量利用的就是这样的原理。

扩展卡尔曼滤波EKF

上面提到的最基本的卡尔曼滤波仅适用于线性系统,假设有非线性系统如下,我们该如何处理呢?
Snipaste_2020-04-16_21-32-59.png
最简单粗暴的方法就是线性化。
如何线性化一个非线性系统,在扩展卡尔曼滤波中,我们使用非线性函数的一阶偏导矩阵来代替线性的系数矩阵,这样来更新那一组方程就可以啦!
具体怎么做呢?请看下面的式子:
Snipaste_2020-04-16_21-31-36.png
这组方程与上面的有什么区别呢?细心的小伙伴可能发现了,我们合情合理地将先验更新的方程替换为了非线性形式,其他好像都没有改变。但是对于一个非线性系统,我们如何确定这个A矩阵与H矩阵呢?答案上文已经提到了,就是使用(该点处的)一阶偏导矩阵对非线性系统进行一阶线性近似(相当于只取f、h泰勒展开一次项的系数),这个一阶偏导矩阵也就是大名鼎鼎的雅可比矩阵:
Snipaste_2020-04-16_21-40-48.png
注意到A一定还是一个nxn的方阵,H的阶数也没有改变(仍取决于状态变量个数与测量量个数)
其他步骤均与上面提到的一致。

无迹卡尔曼滤波UKF

在实际应用中,对于非线性程度比较大的系统,一阶近似往往效果不是很理想,聪明的人们又想到了一种方法来解决非线性系统的卡尔曼滤波,无迹卡尔曼滤波就这样诞生了。
无迹卡尔曼滤波的本质同它的两个兄弟一样,也是通过协方差从概率角度计算卡尔曼增益K进行迭代的,不同的是,不是像线性系统那样仅根据迭代就能计算出协方差,而是在均值周围取sigma点,让这些点真正的去参与系统运算,再使用这几个sigma点的协方差参与运算,进行迭代。
Snipaste_2020-04-16_21-56-12.png
这些sigma点的个数与位置的选择也是有要求滴!

再升级该怎么办?

再升级?再升级就是粒子滤波啦!相当于有更多的sigma点来还原系统,效果当然也更好,就是计算量更大了。
粒子滤波是什么?看这儿:particle filtering—粒子滤波(讲的很通俗易懂)

总结一下

这篇文章带大家十分粗略的了解了三种卡尔曼滤波的形式,其中少了许多数学推导部分,尤其是概率相关的数学推导,这些部分我不是很熟悉,仅能通过定性的方式去说,如果想要了解本质,大家可以看这篇文章:Kalman_Intro
最后用一张图总结:
KF_EKF_UKF.jpg