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

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

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

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

一个简略的比如

估量金条的分量

咱们先从一个简略的比如开端，在这个比如中咱们将会去估量一个静态体系，所谓静态体系便是在一定的时间内其状况不会改变的体系。

在这个比如中，咱们将估量一个金条的分量。首先假定咱们运用的是一个无差错的秤，也便是没有体系差错，但是丈量值是包括随机差错的。这儿的体系便是金条，体系的状况便是金条的分量。体系的动态模型是稳定的，由于咱们假定金条的分量在短时间内不会发生变化。为了估量体系的状况，咱们能够进行多次丈量，然后对丈量值求均匀。

经过 $N$ 次丈量，估量值 $x_{N,N}$ 是之前一切丈量值的均匀值：

xN,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)

在上式中，

$x$ 表明金条分量的真实值；
$z_{n}$ 表明第 $n$ 次的丈量值；
$xn,n\hat{x}_{n,n}$ 表明第 $n$ 次对 $x$ 的估量值，估量是在采用了第 $n$ 次的丈量值 $z_{n}$ 后做出的；
$xn,n−1\hat{x}_{n,n-1}$ 表明之前对 $x$ 的估量值，是在第 $n - 1$ 次时采用了丈量值 $z_{n-1}$ 后做出的；
$xn+1,n\hat{x}_{n+1,n}$ 表明对 $x$ 未来状况的估量值，这个估量是在第 $n$ 次时得到丈量值 $z_{n}$ 后做出的，也便是说 $xn+1,n\hat{x}_{n+1,n}$ 是一个猜想状况值。由于在本例中动态模型是稳定的，所以有 $x_{n+1,n}=x_{n,n}$ 。

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

xN,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−1NxN−1,N−1+1NzN=xN−1,N−1−1NxN−1,N−1+1NzN=xN−1,N−1+1N(zN−xN−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*}

经过前面的常识，咱们知道 $xN−1,N−1\hat{x}_{N-1,N-1}$ 是第 $N - 1$ 次时对 $x$ 的估量值，现在咱们需求根据 $xN−1,N−1\hat{x}_{N-1,N-1}$ 在第 $N$ 次时对状况 $x$ 进行猜想然后得到 $xN,N−1\hat{x}_{N,N-1}$ 。由于是一个静态体系，所以有 $xN,N−1=xN−1,N−1\hat{x}_{N,N-1}=\hat{x}_{N-1,N-1}$ ，那么上式就能够写为

xN,N=xN,N−1+1N(zn−xN,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），记作 $K_{n}$ ，其下标 $n$ 表明卡尔曼增益会跟着迭代而变化。在深入卡尔曼滤波器之前，咱们先用 $n\alpha_{n}$ 替代 $K_{n}$ ，那么状况更新方程能够写为

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

这儿的 $(zn−xn,n−1)\left( z_{n}-\hat{x}_{n,n-1} \right)$ 被称为丈量残差，也叫更新项。更新意味着包括了新的信息，也便是新的丈量值。

在本例中，系数 $1N\frac{1}{N}$ 会跟着 $N$ 的变大而变小。那么怎样理解这个状况更新方程呢？在开端的时分，咱们没有满足的信息来估量金条的分量，只能根据丈量值对状况做出估量，也便是更多地采用丈量值。跟着迭代次数增多，系数 $1N\frac{1}{N}$ 越来越小，每次的丈量值的权重也就越来越小，当迭代次数满足多的时分，新的丈量值对估量值的影响已经能够忽略不计了。

在状况更新方程中，咱们还需求有一个初始值。在本例中，在进行第一次丈量前咱们能够经过猜想来大略估量一下金条的分量，这个初始猜想（Initial Guess）值也便是第一个估量值。在这个特定的比如中，初始猜想值能够是任何值，由于 $1=1\alpha_{1}=1$ ，初始猜想值在第一次迭代时就被消掉了。

依据状况更新方程，估量算法的流程如下图所示：

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

$n$	1	2	3	4	5	6	7	8	9	10
$n\alpha_{n}$	1	$12\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}$
$z_{n}$	1030	989	1017	1009	1013	979	1008	1042	1012	1011
$xn,n\hat{x_{n,n}}$	1030	1009.5	1012	1011.25	1011.6	1006.17	1006.13	1010.87	1011	1011
$xn+1,n\hat{x_{n+1,n}}$	1030	1009.5	1012	1011.25	1011.6	1006.17	1006.13	1010.87	1011	1011

将这些数据可视化：

能够看到，咱们的估量算法能够很好地对丈量数据进行平滑滤波，而且朝着真值方向逐渐收敛。

一维卡尔曼滤波器

在前面称金条分量的比如中，咱们经过多次丈量然后求均匀的方法去估量金条的分量。经过对数据的可视化能够方便地看到，丈量值存在随机丈量差错。一般咱们用方差 $2\sigma^{2}$ 来描绘这种随机差错，丈量差错的方差实践上便是丈量的不确定度，一般由丈量设备的生产厂商供给。在一些文献中，丈量的不确定度也称为丈量差错，咱们用 $r$ 来表明。估量值与真实值之间的不同被称为估量差错，能够看到，跟着丈量次数的添加，估量差错越来越小最终收敛为零。实践中咱们并不知道估量差错究竟是多少，但是咱们能够知道估量值的不确定度，这个不确定度咱们用 $p$ 来表明。

前面介绍了状况更新方程，在卡尔曼滤波器中，系数 $n\alpha_{n}$ 是在每一次迭代进程中动态计算的，被称为卡尔曼增益，用 $K_{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*}

其间， $p_{n,n-1}$ 是外推的估量不确定度 , $r_{n}$ 是当时的丈量不确定度。能够知道，卡尔曼增益的值域为 $\leq K_{n} \leq 1$ 。卡尔曼增益这个公式是怎样来的呢？接下来咱们具体推导一下。

给定当时的丈量值 $z_{n}$ 和从前的估量值 $xn,n−1\hat{x}_{n,n-1}$ ，咱们的目的是要找到一组参数将 $z_{n}$ 和 $xn,n−1\hat{x}_{n,n-1}$ 组合到一起得到当时最优的估量值 $xn,n\hat{x}_{n,n}$ ，也便是需求给它们赋予最优的权重:

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

这个最优状况估量的方差为

p_{n,n} = k^{2}r_{n} + (1 – k)^{2}p_{n,n-1}

由于咱们是要找一个最优估量，那么就希望 $p_{n,n}$ 最小，也便是要找到一个 $k$ 值，使得 $p_{n,n}$ 最小。为了找到这个 $k$ 值，咱们对 $p_{n,n}$ 求关于 $k$ 的导数，而且设导数为零:

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

可得

kr_{n} = p_{n,n-1} – kp_{n,n-1} \\

kp_{n,n-1} + kr_{n} = p_{n,n-1}

\frac{p_{n,n-1}}{p_{n,n-1} + r_{n}}

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

让咱们重写一下状况更新方程

xn,n=xn,n−1+Kn(zn−xn,n−1)=(1−Kn)xn,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*}

能够看到，卡尔曼增益 $K_{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}

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

在这个比如中，咱们采用的体系动态模型是稳定模型，所以状况外推方程为

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

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

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

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

方程表达式	方程名
$xn,n=xn,n−1+Kn(zn−xn,n−1)\hat{x}_{n,n}=~ \hat{x}_{n,n-1}+ K_{n} \left( z_{n}- \hat{x}_{n,n-1} \right)$	状况更新方程
$x_{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}$	协方差更新方程
$p_{n+1,n}= p_{n,n}$	协方差外推方程

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

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

进程0: 初始化

卡尔曼滤波器需求初始化两个参数：体系状况 $x1,0\hat{x}_{1,0}$ 和状况的不确定度 $p_{1,0}$ 。初始化完成后，滤波器将会根据这个初始状况去做猜想。
进程1: 丈量

丈量会给体系供给两个参数：体系状况的丈量值 $z_{n}$ 和丈量的不确定度 $r_{n}$ 。
进程2: 状况更新

状况更新进程的输入为：
- 丈量值 $z_{n}$
- 丈量的不确定度 $r_{n}$
- 从前的体系状况估量值 $xn,n−1\hat{x}_{n,n-1}$
- 从前估量的不确定度 $p_{n,n-1}$
根据这些输入数据，状况更新进程将会计算卡尔曼增益 $K_{n}$ ，然后供给两个输出：
- 当时的体系状况估量 $xn,n\hat{x}_{n,n}$
- 当时状况估量的不确定度 $p_{n,n}$
进程3: 猜想

猜想进程根据体系的动态模型将当时体系状况和当时体系状况估量的不确定度外推到下一个体系状况。本次迭代猜想进程的输出，在滤波器的下一次迭代进程中就变成了从前的体系状况估量和估量的不确定度了。

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

一个简略的比如

估量金条的分量

一维卡尔曼滤波器

相关文章

Neumorphism 组件 ーー Dart extension 扩展

实时渲染常用纹理技术总结：视差映射

一文拿下Web端基于AudioWorkletNode录制音频

2023 跟我一起学设计模式: 外观模式(facade)

作者信息

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

Neumorphism 组件ーー Dart extension 扩展