javascript卡尔曼滤波 详解卡尔曼滤波

一、什么是卡尔曼滤波

简单来说，卡尔曼滤波器是一个“optimal recursive data processing algorithm（最优递归数据处理算法）”。

在自然界中往往存在各种不确定性，不管是传感器测量的数据还是系统模型计算得到的数据，往往不是物体真实的值，存在各种各样的干扰，卡尔曼滤波就是从有干扰的数据中获取最优（最接近真实）的数据。

二、卡尔曼滤波基础

先来看一个简单的例子，我们用一把尺子来测一个硬币的直径，如图1：

我们测量多组硬币的直径，用$z1,z2...zk$表示，$zk$表示第n次测量的结果，此时我们怎么获得硬币比较接近真实值的直径，在现实中我们测量是有误差的，这个误差可能来自测量器具本身或者其他干扰，并且这个误差一般是服从正态分布，很显然我们可以通过求平均来减小误差，从而得到更加逼近真实值的结果,即$\hat{x_{k}}=\frac{1}{k}\left ( z_{1}+z_{2}+z_{3}+...+z_{k}\right )$

将$zk$提出来，即$\begin{aligned} \hat{x_{k}}&=\frac{1}{k}\left ( z_{1}+z_{2}+...+z_{k-1}\right )+\frac{1}{k}z_{k}\\ &=\frac{1}{k}\cdot \frac{k-1}{k-1}\left ( z_{1}+z_{2}+...+z_{k-1}\right )+\frac{1}{k}z_{k}\\ &=\frac{k-1}{k}\cdot \frac{1}{k-1}\left ( z_{1}+z_{2}+...+z_{k-1}\right )+\frac{1}{k}z_{k}\\ &=\frac{k-1}{k}\cdot \hat{x_{k-1}}+\frac{1}{k}z_{k}\\ &=\hat{x_{k-1}}+\frac{1}{k}(z_{k}-\hat{x_{k-1}}) \end{aligned}$

其中，$\hat{x_{k}}$是我们计算的平均值，我们用此值估计硬币的直径，也可以将此值称为估计值，所以公式也可以表达如下：

当前的估计值=上一次的估计值+系数*（当前测量值-上次估计值）

我们分析一下公式，当k变大时，即测量次数变多时，$\hat{x_{k}}$会随着k的增大而越发逼近$\hat{x_{k-1}}$，也就是说，随着测量次数增加，测量结果将不再重要；当k很小时（k是测量次数，所以k>1），测量值对$\hat{x_{k}}$的影响很大。我们令$K_{k}=\frac{1}{k}$

则$\hat{x_{k}}=\hat{x_{k-1}}+K_{k}(z_{k}-\hat{x_{k-1}})$

我们往往把这个$K_{k}$称为卡尔曼增益，这时大家可能有疑惑，这不就是一个求平均的递归形式嘛，算什么卡尔曼滤波，你这就是标题党。大家稍安勿躁，我们接着往下看

还是刚刚求硬币直径的问题，在上一个例子中，我们使用一把尺子来测硬币直径，我觉得一把尺子测得不准，这回我拿两把尺子，第一把尺子测得硬币直径z1=30mm,第二把尺子测得硬币直径z2=32mm，假定这两把尺子测得都有误差，且误差服从正态分布，第一把尺子的标准差为$\sigma _{1}=2mm$,第二把尺子标准差为$\sigma _{2}=4mm$,此时我们怎么得到比较准确的硬币直径？两尺子测量值的正态分布图如下

我们此时为了得到更准确的硬币直径，参考上一个例子求平均的公式,可以得到$\hat{z}=z_{1}+K_{k}(z_{2}-z_{1}),(K_{k}\in \left [ 0,1 \right ])$当$K_{k}=0$时，$\hat{z}=z_{1}$,当$K_{k}=1$时，$\hat{z}=z_{2}$，选择合适的$K_{k}$可以得到更逼近真实值的值，这个问题由此转变为求优的问题。为了使估计值$\hat{z}$更接近真实值，那么$\hat{z}$的标准差$\sigma _{\hat{z}}$必须最小，才能得到更准确的值。$\hat{z}$的方差可以表示为$\begin{aligned} \sigma _{\hat{z}}^{2}&=var(z_{1}+K_{k}(z_{2}-z_{1}))\\ &=var((1-K_{k})z_{1}+K_{k}z_{2}) \end{aligned}$又因为$z_{1}$与$z_{2}$相互独立，所以上式$\begin{aligned} &=var((1-K_{k})z_{1})+var(K_{k}z_{2})\\ &=(1-K_{k})^{2}\cdot var(z_{1})+K_{k}^{2}\cdot var(z_{2})\\ &=(1-K_{k})^{2}\sigma _{1}^{2}+K_{k}^{2}\sigma _{2}^{2} \end{aligned}$相当于求二次函数最小值，我们可以解出此时$K_{k}=\frac{\sigma _{1}^{2}}{\sigma_{1}^{2}+\sigma_{2}^{2} }=0.2$$\hat{z}=30+0.2\times (32-30)=30.4mm$$\sigma _{\hat{z}}^{2}=3.2$,此时我们可以看到标准差只有$\sqrt{3.2}$,小于$\sigma _{1}$和$\sigma _{2}$,说明此时估计值的可信度是高于两个传感器的测量值，即估计值$\hat{z}$更接近真实值，通过这种算法，我们从两个具有不同误差的传感器中获得了更加准确的值，这就是所谓的数据融合（Data Fusion）,将估计值曲线用matlab画出(中间黄色的曲线)

记住这个思想，接下来要开始我们的重头戏了，卡尔曼滤波五大公式的推导。