深入理解卡尔曼滤波器（2）： 一维卡尔曼滤波器

一天到晚潜水的鱼
939 阅读8分钟

本文已参与「新人创作礼」活动，一起开启掘金创作之路。

声明： 本文图文均来源于www.kalmanfilter.net/，如有侵权请联系删除。

本文由微信公众号【DeepDriving】整理，由于全文内容较多所以分成3部分发出来。关注公众号【DeepDriving】，后台回复关键字【卡尔曼滤波器】可获取全文PDF。

一个简单的例子

估计金条的重量

我们先从一个简单的例子开始，在这个例子中我们将会去估计一个静态系统，所谓静态系统就是在一定的时间内其状态不会改变的系统。

在这个例子中，我们将估计一个金条的重量。首先假设我们使用的是一个无偏差的秤，也就是没有系统误差，但是测量值是包含随机误差的。这里的系统就是金条，系统的状态就是金条的重量。系统的动态模型是恒定的，因为我们假设金条的重量在短时间内不会发生变化。为了估计系统的状态，我们可以进行多次测量，然后对测量值求平均。

经过NN次测量，估计值xN,Nx_{N,N}是之前所有测量值的平均值：

x^N,N=1N(z1+z2+…+zN−1+zN)=1N∑n=1N(zn)\hat{x}_{N,N}= \frac{1}{N} \left( z_{1}+ z_{2}+ \ldots + z_{N-1}+ z_{N} \right) = \frac{1}{N} \sum _{n=1}^{N} \left( z_{n} \right)

在上式中，

  • xx表示金条重量的真实值；
  • znz_{n}表示第nn次的测量值；
  • x^n,n\hat{x}_{n,n}表示第nn次对xx的估计值，估计是在采用了第nn次的测量值znz_{n}后做出的；
  • x^n,n−1\hat{x}_{n,n-1}表示之前对xx的估计值，是在第n−1n-1次时采用了测量值zn−1z_{n-1}后做出的；
  • x^n+1,n\hat{x}_{n+1,n}表示对xx未来状态的估计值，这个估计是在第nn次时得到测量值znz_{n}后做出的，也就是说x^n+1,n\hat{x}_{n+1,n}是一个预测状态值。由于在本例中动态模型是恒定的，所以有xn+1,n=xn,nx_{n+1,n}=x_{n,n}。

对上式进行一些数学变换：

x^N,N=1N∑n=1N(zn)=1N(∑n=1N−1(zn)+zN)=1N∑n=1N−1(zn)+1NzN=1NN−1N−1∑n=1N−1(zn)+1NzN=N−1N1N−1∑n=1N−1(zn)+1NzN=N−1Nx^N−1,N−1+1NzN=x^N−1,N−1−1Nx^N−1,N−1+1NzN=x^N−1,N−1+1N(zN−x^N−1,N−1)\begin{align*} \hat{x}_{N,N} &= \frac{1}{N} \sum _{n=1}^{N} \left( z_{n} \right) \\ &= \frac{1}{N} \left( \sum _{n=1}^{N-1} \left( z_{n} \right) + z_{N} \right) \\ &= \frac{1}{N} \sum _{n=1}^{N-1} \left( z_{n} \right) + \frac{1}{N} z_{N} \\ &= \frac{1}{N}\frac{N-1}{N-1} \sum _{n=1}^{N-1} \left( z_{n} \right) + \frac{1}{N} z_{N} \\ &= \frac{N-1}{N}{\frac{1}{N-1} \sum _{n=1}^{N-1} \left( z_{n} \right)} + \frac{1}{N} z_{N} \\ &= \frac{N-1}{N}{\hat{x}_{N-1,N-1}} + \frac{1}{N} z_{N} \\ &= \hat{x}_{N-1,N-1}- \frac{1}{N}\hat{x}_{N-1,N-1}+ \frac{1}{N} z_{N} \\ &= \hat{x}_{N-1,N-1}+ \frac{1}{N} \left( z_{N}- \hat{x}_{N-1,N-1} \right) \end{align*}

通过前面的知识，我们知道x^N−1,N−1\hat{x}_{N-1,N-1}是第N−1N-1次时对xx的估计值，现在我们需要基于x^N−1,N−1\hat{x}_{N-1,N-1}在第NN次时对状态xx进行预测从而得到x^N,N−1\hat{x}_{N,N-1}。由于是一个静态系统，所以有x^N,N−1=x^N−1,N−1\hat{x}_{N,N-1}=\hat{x}_{N-1,N-1}，那么上式就可以写为

x^N,N=x^N,N−1+1N(zn−x^N,N−1)\hat{x}_{N,N} = \hat{x}_{N,N-1} + \frac{1}{N} \left( z_{n} - \hat{x}_{N,N-1} \right)

上面这个公式是卡尔曼滤波器的5个方程之一，被称为状态更新方程，它的含义如下：

在本例中，系数1N\frac{1}{N}是一个特定的值。在卡尔曼滤波器中，这个系数被称为卡尔曼增益（Kalman Gain），记作KnK_{n}，其下标nn表示卡尔曼增益会随着迭代而变化。在深入卡尔曼滤波器之前，我们先用αn\alpha_{n}代替KnK_{n}，那么状态更新方程可以写为

x^n,n=x^n,n−1+αn(zn−x^n,n−1)\hat{x}_{n,n}= \hat{x}_{n,n-1}+ \alpha _{n} \left( z_{n}-\hat{x}_{n,n-1} \right)

这里的(zn−x^n,n−1)\left( z_{n}-\hat{x}_{n,n-1} \right)被称为测量残差，也叫更新项。更新意味着包含了新的信息，也就是新的测量值。

在本例中，系数1N\frac{1}{N}会随着NN的变大而变小。那么怎么理解这个状态更新方程呢？在开始的时候，我们没有足够的信息来估计金条的重量，只能基于测量值对状态做出估计，也就是更多地采用测量值。随着迭代次数增多，系数1N\frac{1}{N}越来越小，每次的测量值的权重也就越来越小，当迭代次数足够多的时候，新的测量值对估计值的影响已经可以忽略不计了。

在状态更新方程中，我们还需要有一个初始值。在本例中，在进行第一次测量前我们可以通过猜测来粗略估计一下金条的重量，这个初始猜测（Initial Guess）值也就是第一个估计值。在这个特定的例子中，初始猜测值可以是任何值，因为α1=1\alpha_{1}=1，初始猜测值在第一次迭代时就被消掉了。

根据状态更新方程，估计算法的流程如下图所示：

看到这个图的时候，我好像有点悟了

我们对金条进行10次称重，然后根据上面的估计算法对金条的重量做出估计，得到的结果如下表所示：

nn12345678910
αn\alpha_{n}112\frac{1}{2}13\frac{1}{3}14\frac{1}{4}15\frac{1}{5}16\frac{1}{6}17\frac{1}{7}18\frac{1}{8}19\frac{1}{9}110\frac{1}{10}
znz_{n}10309891017100910139791008104210121011
xn,n^\hat{x_{n,n}}10301009.510121011.251011.61006.171006.131010.8710111011
xn+1,n^\hat{x_{n+1,n}}10301009.510121011.251011.61006.171006.131010.8710111011

将这些数据可视化：

可以看到，我们的估计算法可以很好地对测量数据进行平滑滤波，并且朝着真值方向逐渐收敛。

一维卡尔曼滤波器

在前面称金条重量的例子中，我们通过多次测量然后求平均的方法去估计金条的重量。通过对数据的可视化可以方便地看到，测量值存在随机测量误差。通常我们用方差σ2\sigma^{2}来描述这种随机误差，测量误差的方差实际上就是测量的不确定度，一般由测量设备的生产厂商提供。在一些文献中，测量的不确定度也称为测量误差，我们用rr来表示。估计值与真实值之间的差别被称为估计误差，可以看到，随着测量次数的增加，估计误差越来越小最后收敛为零。实际中我们并不知道估计误差到底是多少，但是我们可以知道估计值的不确定度，这个不确定度我们用pp来表示。

前面介绍了状态更新方程，在卡尔曼滤波器中，系数αn\alpha_{n}是在每一次迭代过程中动态计算的，被称为卡尔曼增益，用KnK_{n}来表示。卡尔曼增益方程的表达式如下：

Kn=UncertaintyinEstimateUncertaintyinEstimate+UncertaintyinMeasurement=pn,n−1pn,n−1+rn\begin{align*} K_{n} &= \frac{Uncertainty \quad in \quad Estimate}{Uncertainty \quad in \quad Estimate \quad + \quad Uncertainty \quad in \quad Measurement} \\ &= \frac{p_{n,n-1}}{p_{n,n-1}+r_{n}} \end{align*}

其中，pn,n−1p_{n,n-1}是外推的估计不确定度 ,rnr_{n}是当前的测量不确定度。可以知道，卡尔曼增益的值域为0≤Kn≤10 \leq K_{n} \leq 1。卡尔曼增益这个公式是怎么来的呢？接下来我们详细推导一下。

给定当前的测量值znz_{n}和先前的估计值x^n,n−1\hat{x}_{n,n-1}，我们的目的是要找到一组参数将znz_{n}和x^n,n−1\hat{x}_{n,n-1}组合到一起得到当前最优的估计值x^n,n\hat{x}_{n,n}，也就是需要给它们赋予最优的权重:

x^n,n=kzn+(1−k)x^n,n−1\hat{x}_{n,n} = kz_{n} + (1-k)\hat{x}_{n,n-1}

这个最优状态估计的方差为

pn,n=k2rn+(1−k)2pn,n−1p_{n,n} = k^{2}r_{n} + (1 - k)^{2}p_{n,n-1}

因为我们是要找一个最优估计，那么就希望pn,np_{n,n}最小，也就是要找到一个kk值，使得pn,np_{n,n}最小。为了找到这个kk值，我们对pn,np_{n,n}求关于kk的导数，并且设导数为零:

dpn,ndk=2krn−2(1−k)pn,n−1=0\frac{dp_{n,n}}{dk} = 2kr_{n} - 2(1 - k)p_{n,n-1} = 0

可得

krn=pn,n−1−kpn,n−1kr_{n} = p_{n,n-1} - kp_{n,n-1} \\
kpn,n−1+krn=pn,n−1kp_{n,n-1} + kr_{n} = p_{n,n-1}
k=pn,n−1pn,n−1+rnk = \frac{p_{n,n-1}}{p_{n,n-1} + r_{n}}

到这里，一维卡尔曼增益的公式就推导出来了！

让我们重写一下状态更新方程

x^n,n=x^n,n−1+Kn(zn−x^n,n−1)=(1−Kn)x^n,n−1+Knzn\begin{align*} \hat{x}_{n,n} &= \hat{x}_{n,n-1}+ K_{n} \left( z_{n}- \hat{x}_{n,n-1} \right) \\ &= \left( 1-K_{n} \right) \hat{x}_{n,n-1}+ K_{n}z_{n} \end{align*}

可以看到，卡尔曼增益KnK_{n}就是一个赋给测量值的权重，而(1−Kn)\left( 1-K_{n} \right)则是赋给估计值的权重。当测量的不确定度很小而估计的不确定度很大的时候，卡尔曼增益接近于1，此时测量值的权重很大，相当于更相信测量值；反之，当测量的不确定度很大而估计的不确定度很小的时候，卡尔曼增益接近于0，此时估计值的权重很大，相当于更相信估计值。

下面的公式是估计不确定度的更新方程：

pn,n= (1−Kn)pn,n−1p_{n,n}=~ \left( 1-K_{n} \right) p_{n,n-1}

其中，KnK_{n}是卡尔曼增益；pn,n−1p_{n,n-1}是在先前滤波器估计期间计算的估计不确定度；pn,np_{n,n}是当前状态估计的不确定度。这个方程用于更新当前状态估计的不确定度，也叫做协方差更新方程。从这个公式可以知道，由于(1−Kn)≤1\left( 1-K_{n} \right) \leq 1，估计值的不确定度将会随着迭代次数的增加而变得越来越小。

在这个例子中，我们采用的系统动态模型是恒定模型，所以状态外推方程为

xn+1,n=xn,nx_{n+1,n}=x_{n,n}

同样的，估计不确定度外推方程（也叫作协方差外推方程）为

pn+1,n=pn,np_{n+1,n}= p_{n,n}

下面的表格对一维卡尔曼滤波器的5个方程进行了总结（针对恒定模型）：

方程表达式方程名
x^n,n= x^n,n−1+Kn(zn−x^n,n−1)\hat{x}_{n,n}=~ \hat{x}_{n,n-1}+ K_{n} \left( z_{n}- \hat{x}_{n,n-1} \right)状态更新方程
xn+1,n=xn,nx_{n+1,n}=x_{n,n}状态外推方程
Kn=pn,n−1pn,n−1+rnK_{n}= \frac{p_{n,n-1}}{p_{n,n-1}+r_{n}}卡尔曼增益方程
pn,n= (1−Kn)pn,n−1p_{n,n}=~ \left( 1-K_{n} \right) p_{n,n-1}协方差更新方程
pn+1,n=pn,np_{n+1,n}= p_{n,n}协方差外推方程

卡尔曼滤波器算法的框图如下所示：

我们来梳理一下算法的流程：

  • 步骤0: 初始化

    卡尔曼滤波器需要初始化两个参数：系统状态x^1,0\hat{x}_{1,0}和状态的不确定度p1,0p_{1,0}。初始化完成后，滤波器将会基于这个初始状态去做预测。

  • 步骤1: 测量

    测量会给系统提供两个参数：系统状态的测量值znz_{n}和测量的不确定度rnr_{n}。

  • 步骤2: 状态更新

    状态更新过程的输入为：

    • 测量值znz_{n}
    • 测量的不确定度rnr_{n}
    • 先前的系统状态估计值x^n,n−1\hat{x}_{n,n-1}
    • 先前估计的不确定度pn,n−1p_{n,n-1}

    基于这些输入数据，状态更新过程将会计算卡尔曼增益KnK_{n}，然后提供两个输出：

    • 当前的系统状态估计x^n,n\hat{x}_{n,n}
    • 当前状态估计的不确定度pn,np_{n,n}

  • 步骤3: 预测

    预测过程基于系统的动态模型将当前系统状态和当前系统状态估计的不确定度外推到下一个系统状态。本次迭代预测过程的输出，在滤波器的下一次迭代过程中就变成了先前的系统状态估计和估计的不确定度了。

avatar
文章
阅读
粉丝