【滤波】一维卡尔曼滤波器

%matplotlib inline

#format the book
import book_format
book_format.set_style()

现在，我们了解了离散贝叶斯滤波器和高斯，我们准备实现一个卡尔曼滤波器。我们将和离散贝叶斯滤波器章节一样完成这项工作，根据对问题的推理一步一步地编写代码。

一维意味着滤波器只跟踪一个状态量，例如$x$轴上的位置。因此，在本文中，我们将学习如何使用高斯函数来实现贝叶斯滤波器。这就是卡尔曼滤波的全部：一种使用高斯的贝叶斯滤波。

问题描述

和离散贝叶斯滤波器章节一样，我们跟踪正在走廊中移动的狗。假设有人发明了一个RFID跟踪器，它提供了一个相当准确的狗的位置。传感器以$m$为单位，返回狗距离走廊最左端的距离。所以，23.4意味着狗距离走廊左端$23.4m$。

当然，传感器不完美。读数为23.4，可能狗的真实位置是23.7或23.0。然而，它不太可能对应于47.6的位置。传感器是合理准确的，虽然有误差，但误差很小。并且，有误差的观测值均匀分布在真实位置的两侧。也许我们可以用高斯函数来模拟。

我们对狗的位置进行预测，当然这个预测也不完美。有时我们的预测会超出狗的实际位置，有时会不及狗的实际位置。我们预测虽然也有偏差，但也不会很大。也许我们也可以用高斯函数来模拟。

高斯概率

我们可以用高斯曲线，来表示我们对狗位置的概率。假设我们觉得狗在$10m$处，且方差是$1m^2$，那么可以简写成$N(10,1)$。概率密度函数pdf为：

import filterpy.stats as stats

stats.plot_gaussian_pdf(mean=10., variance=1., 
                        xlim=(4, 16), ylim=(0, .5))

这个图描述了我们对狗位置的不确定性。它表示狗的位置可能不太准确，虽然我们认为狗最有可能在$10m$处，但从$9m$到$11m$也很有可能。假设狗静止不动，我们再次用传感器进行观测。这一次传感器的数值是$10.2m$。我们能用这个额外的信息来改进我们的估计吗？

直觉告诉我们可以。考虑一下：如果我们读取传感器500次，每次它返回的值都在8到12之间，且都以10为中心，那么我们应该非常确信狗的位置接近10。当然，可能有不同的解释。也许我们的狗是随机游荡来回的方式，完全随机模拟高斯分布。但这似乎是极不可能的——我从来没有见过狗这样做。让我们看一下$N(10,1)$的500次的结果：

import numpy as np
from numpy.random import randn
import matplotlib.pyplot as plt

xs = range(500)
ys = randn(500)*1. + 10.
plt.plot(xs, ys)
print(f'Mean of readings is {np.mean(ys):.3f}')

Mean of readings is 10.021

然而，有噪声的传感器数据看起来确实是这样的，读数均值的计算结果几乎正好是10。假设狗站着不动，我们可以说狗在位置10，方差为1。

高斯概率跟踪

离散贝叶斯滤波器使用概率直方图来跟踪狗。直方图中的每个直方列代表一个位置，该列对应的值是狗处于该位置的概率。

跟踪是通过不断地预测和更新的循环来执行的。我们用了下列方程式计算新的概率分布：

$$\{\begin{array}{cc}\bar{\mathbf{x}}=\mathbf{x}\ast f_{\mathbf{x}}(\bullet )& Predict\\ \mathbf{x}=\mathcal{L}\cdot \bar{\mathbf{x}}& Correct\end{array}$$

回想一下，$\bar{\mathbf{x}}$是先验值，$\mathcal{L}$是观测值和先验值$\bar{\mathbf{x}}$的似然，$f_{\mathbf{x}}(\bullet )$是过程模型，$\ast$表示卷积。$\mathbf{x}$是粗体的，表示它是一个向量，是直方图的所有可能性的值，而非是单独的值。

这种方法是可行的，但最终结果可能会表现出狗可以同一时间在多个地方（多峰）。而且，对于大型问题，计算速度非常慢。

那我们能用高斯$N(\mu ,{\sigma }^2)$代替直方图$\mathbf{x}$吗？当然可以！我们已经学会了如何用高斯函数来表达概率。高斯分布是一个单独的数对$N(\mu ,{\sigma }^2)$，可以代替整个概率直方图：

import kf_book.kf_internal as kf_internal

kf_internal.gaussian_vs_histogram()

我们可以用一对数字替换成百上千个数字：$x=N(\mu ,{\sigma }^2)$。

高斯分布的尾部在两侧延伸到无穷远，因此它在直方图中包含无数列。如果这代表了我们对狗在走廊中位置的概率，那么这个高斯分布覆盖了整个走廊（甚至这个走廊延申出的整个宇宙）。我们可能认为狗的位置在10，也可能在8、14，或者以无穷小的概率，在$10^{80}$。

在本文中，我们将直方图替换为高斯：

$$\begin{array}{llc}\text{discrete Bayes}& \text{Gaussian}& \text{Step}\\ \bar{\mathbf{x}}=\mathbf{x}\ast f(\mathbf{x})& {\bar{x}}_{\mathcal{N}}=x_{\mathcal{N}}\oplus f_{x_{\mathcal{N}}}(\bullet )& \text{Predict}\\ \mathbf{x}=|\mathcal{L}\cdot \bar{\mathbf{x}}|& x_{\mathcal{N}}=L\otimes {\bar{x}}_{\mathcal{N}}& \text{Update}\end{array}$$

其中，$\oplus$和$\otimes$表示高斯上的某个未知算子。

如果进行类比：离散贝叶斯滤波器采用卷积进行预测，它使用全概率公式，最终计算出一个和，所以也许我们可以进行高斯相加；它使用乘法将观测值合并到先验值中，所以也许我们可以进行高斯相乘。能这么简单吗：

$$\begin{array}{rl}\bar{x}& \stackrel{?}{=}x+f_x(\bullet )\\ x& \stackrel{?}{=}\mathcal{L}\cdot \bar{x}\end{array}$$

只有当两个高斯的和、积都是另一个高斯时，这才有效。否则在第一个循环之后，$x$就不是高斯的，这个方案就无法继续下去了。

高斯预测

我们使用牛顿运动方程，根据当前速度和先前位置计算当前位置：

$${\bar{x}}_k=x_{k-1}+v_k\Delta t=x_{k-1}+f_x$$

我放弃了$f_x(\bullet )$的符号，转而使用$f_x$来保持方程的整洁。

如果当前狗在$10m$处，它的速度是$15m/s$，时间步长是$2s$，我们有：

$${\bar{x}}_k=x_{k-1}+f_x=10+(15\times 2)=40$$

其实，它当前的位置和速度都不是很精确的值，所以单纯这样计算可不行。我们需要用高斯函数来表示不确定性。

位置很容易，我们把$x$定义为高斯函数。如果我们认为狗在$10m$处，距离不确定度的标准差是$0.2m$，我们得到$x=N(10,0.2^2)$。

我们对它行动的不确定性怎么描述？我们将$f_x$定义为高斯函数。如果狗的速度是$15m/s$，时间是$1s$，速度不确定度的标准偏差是$0.7m/s$，我们得到$f_x=N(15,0.7^2)$。

先验值的计算公式是：

$$\bar{x}=x+f_x$$

两个高斯的和是多少？在上一章我证明了：

$$\mu ={\mu }_1+{\mu }_2$$

$${\sigma }^2={\sigma }_1^2+{\sigma }_2^2$$

这是个好消息：两个高斯的和就是另一个高斯！

数学上是可行的，但这有直觉意义吗？想想这个抽象方程的物理表示。我们有：

$$x=N(10,0.2^2)$$

$$f_x=N(15,0.7^2)$$

利用之前的公式，我们得到：

$$\bar{x}={\mu }_x+{\mu }_{f_x}=25$$

$${\bar{\sigma }}^2={\sigma }_x^2+{\sigma }_{f_x}^2=0.53$$

预测的位置是前一个位置加上移动距离，这是有意义的。方差呢？很难对此形成直觉。但是，回想一下离散贝叶斯滤波器的predict()函数，如果预测过程存在噪声，我们总是会丢失信息。我们更不知道狗移动到哪里，所以概率应该变小（方差变大）。${\sigma }_{f_x}^2$是由于对运动的不完全预测而增加了系统中的不确定性，因此我们将把它加到现有的不确定性中。

让我们利用Python的collection模块中的namedtuple类来实现一个Gaussian对象。当然，我们也可以使用一个tuple实现一个Gaussian，其中$N(10,0.04)$在Python中可以表示为$g=(10,0.04)$。我们用g[0]取均值，用g[1]取方差。

namedtuple的工作原理与tuple相同，除了一点，namedtuple可以添加类型名和字段名。理解这一点并不重要，但我修改了__repr__方法，使用本文中的符号来显示它的值。

from collections import namedtuple

gaussian = namedtuple('Gaussian', ['mean', 'var'])
gaussian.__repr__ = lambda s: '𝒩(μ={:.3f}, 𝜎²={:.3f})'.format(s[0], s[1])

现在我们就可以创建高斯：

g1 = gaussian(3.4, 10.1)
g2 = gaussian(mean=4.5, var=0.2**2)
print(g1)
print(g2)

N(μ=3.400, 𝜎²=10.100)
N(μ=4.500, 𝜎²=0.040)

而且我们可以使用下标或字段名直接访问均值和方差：

g1.mean, g1[0], g1[1], g1.var

(3.4, 3.4, 10.1, 10.1)

下面是我们对预测函数的实现，其中pos和movement是高斯，形式为$(\mu ,{\sigma }^2)$：

def predict(pos, movement):
    return gaussian(pos.mean + movement.mean, pos.var + movement.var)

我们来测试一下。如果初始位置是高斯$N(10,0.2^2)$，运动导致的移动是高斯$N(15,0.7^2)$，那么先验是什么？

pos = gaussian(10., .2**2)
move = gaussian(15., .7**2)
predict(pos, move)

N(μ=25.000, 𝜎²=0.530)

于是，我们计算出来，先验值是狗在$25m$处，方差为$0.53m^2$。

高斯更新

离散贝叶斯滤波器将狗在概率直方图中位置的概率，进行编码。最终分布可能是离散且多峰的：它能表达一种含义，即狗同时处于两个位置，而且位置是离散的。

我们建议用高斯函数代替直方图。离散贝叶斯滤波器使用此代码计算后验概率：

def update(likelihood, prior):
    posterior = likelihood * prior
    return normalize(posterior)

这是如下等式的一个实现：

$$x=|\mathcal{L}\cdot \bar{x}|$$

我们已经证明了我们可以用高斯函数来表示先验值。那么似然呢？似然是给定当前状态的观测概率。我们已经学会了如何用高斯函数来表示观测值。例如，也许我们的传感器显示狗在$23m$处，标准差为$0.4m$。我们的观测值，用似然表示，是$z=N(23,0.16)$。

似然和先验都是用高斯函数建模的。我们能进行高斯相乘吗？两个高斯的乘积是另一个高斯吗？

在上一章我证明了两个高斯函数的乘积与另一个高斯函数成正比：

$$\mu =\frac{{\sigma }_1^2{\mu }_2+{\sigma }_2^2{\mu }_1}{{\sigma }_1^2+{\sigma }_2^2}$$

$${\sigma }^2=\frac{{\sigma }_1^2{\sigma }_2^2}{{\sigma }_1^2+{\sigma }_2^2}$$

我们可以立即推断出几件事：如果我们对结果进行标准化，结果就是另一个高斯分布。如果一个高斯是似然，第二个高斯是先验，那么均值是先验值和观测值平均值的比例和，方差是先验值和观测值方差的组合。最后，方差完全不受均值的影响！

我们用贝叶斯的术语来表示：

$$N(\mu ,{\sigma }^2)=|prior\times likelihood|$$

$$=|N(\bar{\mu },{\bar{\sigma }}^2)\cdot N({\mu }_z,{\sigma }_z^2)|$$

$$=N(\frac{{\bar{\sigma }}^2{\mu }_z+{\sigma }_z^2\bar{\mu }}{{\sigma }_z^2+{\bar{\sigma }}^2},\frac{{\bar{\sigma }}^2{\sigma }_z^2}{{\sigma }_z^2+{\bar{\sigma }}^2})$$

如果我们在gaussian_multiply()函数中实现滤波器的更新步骤：

def gaussian_multiply(g1, g2):
    mean = (g1.var * g2.mean + g2.var * g1.mean) / (g1.var + g2.var)
    variance = (g1.var * g2.var) / (g1.var + g2.var)
    return gaussian(mean, variance)

def update(prior, likelihood):
    posterior = gaussian_multiply(likelihood, prior)
    return posterior

# test the update function
predicted_pos = gaussian(10., .2**2)
measured_pos = gaussian(11., .1**2)
estimated_pos = update(predicted_pos, measured_pos)
estimated_pos

N(μ=10.800, 𝜎²=0.008)

如果我们使用更具体的名称，也许这会更清楚：

def update_dog(dog_pos, measurement):
    estimated_pos = gaussian_multiply(measurement, dog_pos)
    return estimated_pos

这样写代码有助于理解，但不太抽象，是糟糕的编码实践。我们正在编写一个卡尔曼滤波器，它应该可以解决任何问题，而不仅仅是跟踪走廊里的狗，所以我们不应该使用带有dog的变量名。而且，这种形式掩盖了这样一个事实，即我们将似然乘以先验。

我们已经实现了滤波器的大部分，但我担心这一步你仍然有点混乱。让我们再花点时间理解一下高斯的乘法。

理解高斯乘法

让我们绘制$N(10,1)\times N(10,1)$的pdf。你能不看结果就确定它的形状吗？曲线是更宽、更窄，还是与$N(10,1)$相同？

z = gaussian(10., 1.)  # Gaussian N(10, 1)

product = gaussian_multiply(z, z)

xs = np.arange(5, 15, 0.1)
ys = [stats.gaussian(x, z.mean, z.var) for x in xs]
plt.plot(xs, ys, label='$\mathcal{N}(10,1)$')

ys = [stats.gaussian(x, product.mean, product.var) for x in xs]
plt.plot(xs, ys, label='$\mathcal{N}(10,1) \\times \mathcal{N}(10,1)$', ls='--')
plt.legend()
print(product)

N(μ=10.000, 𝜎²=0.500)

乘法的结果比原来的高斯更高且更窄，但均值不变。这符合你的直觉吗？

把高斯函数看作两次观测。如果我观测两次，每次都是$10m$，我会得出结论，位置接近$10m$。因此均值应该是10。另外，我的两次观测，导致我对一次观测的结果更有信心，所以结果的方差应该更小。

我不太可能连续两次得到相同的尺寸。现在我们来绘制$N(10.2,1)\times N(9.7,1)$的pdf。你认为结果会怎样？想一想，然后看图表。

def plot_products(g1, g2): 
    plt.figure()
    product = gaussian_multiply(g1, g2)

    xs = np.arange(5, 15, 0.1)
    ys = [stats.gaussian(x, g1.mean, g1.var) for x in xs]
    plt.plot(xs, ys, label='$\mathcal{N}$'+'$({},{})$'.format(g1.mean, g1.var))

    ys = [stats.gaussian(x, g2.mean, g2.var) for x in xs]
    plt.plot(xs, ys, label='$\mathcal{N}$'+'$({},{})$'.format(g2.mean, g2.var))

    ys = [stats.gaussian(x, product.mean, product.var) for x in xs]
    plt.plot(xs, ys, label='product', ls='--')
    plt.legend()
    
z1 = gaussian(10.2, 1)
z2 = gaussian(9.7, 1)
 
plot_products(z1, z2)

如果你让两个人进行观测，一个人得到$10.2m$，另一个人得到$9.7m$，你最好的猜测一定是均值，如果你平等地相信两个人，那就是$9.95m$。

尽管也许其中的一个人犯了错误，真实距离不是$10.2m$就是$9.7m$，且肯定不是$9.95m$。这当然是可能的。但是我们知道我们的观测是有噪声的，所以我们没有理由认为其中一个观测没有噪声，或者一个观测的噪声异常大，从而导致我们放弃它们的观测值。考虑到所有可用信息，最佳估计值必须为$9.95m$。

在卡尔曼滤波器的更新步骤中，我们不是合并两个观测值，而是合并一个观测值和之前的一个估计值。但是从数学的角度上说，不管我们是把两次观测的信息，还是一次观测和一次预测的信息结合起来，本质上是一样的。

我们来看看。我将创建一个相当不精确的先验值$N(8.5,1.5)$和一个更精确的观测值$N(10.2,0.5)$。所谓精确，我的意思是传感器的方差小于先验值的方差，并不是指狗相比较于$8.5m$，更接近$10.2m$。接下来，我将同时绘制反向的关系：精确的先验值$N(8.5,0.5)$和不精确的观测值$N(10.2,1.5)$。

prior, z = gaussian(8.5, 1.5), gaussian(10.2, 0.5)
plot_products(prior, z)

prior, z = gaussian(8.5, 0.5), gaussian(10.2, 1.5)
plot_products(prior, z)

结果是一个比任何一个输入都高的高斯分布。这是有道理的————我们已经加入了新的信息，所以我们的方差应该减少。但注意一下结果是更接近哪一种输入的。

这似乎管用，但真的正确吗？我先让你可以体验一下它的作用。之后，我们将继续讨论高斯乘法，并确定为什么它是正确的。

交互例子

下面的交互代码提供了滑块来改变两个高斯数相乘的均值和方差。移动滑块时，将重新绘制：

from ipywidgets import interact

def interactive_gaussian(m1, m2, v1, v2):
    g1 = gaussian(m1, v1)
    g2 = gaussian(m2, v2)
    plot_products(g1, g2)
    
interact(interactive_gaussian,
         m1=(5, 10., .5), m2=(10, 15, .5), 
         v1=(.1, 2, .1), v2=(.1, 2, .1))

如果你没有本地运行环境，可以点击链接在线运行调试（加载的时间可能比较久）：http://mybinder.org/repo/rlabbe/Kalman-and-Bayesian-Filters-in-Python

第一个卡尔曼滤波器

让我们回到具体的例子，并实现一个卡尔曼滤波器。我们已经实现了predict()和update()函数，只需要编写一些代码来模拟狗并创建观测值。我创建了一个DogSimulation类，通过定义均值、方差来模拟狗的运动：

import kf_book.kf_internal as kf_internal
from kf_book.kf_internal import DogSimulation

np.random.seed(13)

process_var = 1. # variance in the dog's movement
sensor_var = 2. # variance in the sensor

x = gaussian(0., 20.**2)  # dog's position, N(0, 20**2)
velocity = 1
dt = 1. # time step in seconds
process_model = gaussian(velocity*dt, process_var) # displacement to add to x
  
# simulate dog and get measurements
dog = DogSimulation(
    x0=x.mean, 
    velocity=process_model.mean, 
    measurement_var=sensor_var, 
    process_var=process_model.var)

# create list of measurements
zs = [dog.move_and_sense() for _ in range(10)]

接下来是卡尔曼滤波器：

print('PREDICT\t\t\tUPDATE')
print('     x      var\t\t  z\t    x      var')

# perform Kalman filter on measurement z
for z in zs:    
    prior = predict(x, process_model)
    likelihood = gaussian(z, sensor_var)
    x = update(prior, likelihood)

    kf_internal.print_gh(prior, x, z)

print()
print('final estimate:        {:10.3f}'.format(x.mean))
print('actual final position: {:10.3f}'.format(dog.x))

PREDICT			UPDATE
     x      var		  z	    x      var
  1.000  401.000	1.354	  1.352   1.990
  2.352    2.990	1.882	  2.070   1.198
  3.070    2.198	4.341	  3.736   1.047
  4.736    2.047	7.156	  5.960   1.012
  6.960    2.012	6.939	  6.949   1.003
  7.949    2.003	6.844	  7.396   1.001
  8.396    2.001	9.847	  9.122   1.000
 10.122    2.000	12.553	 11.338   1.000
 12.338    2.000	16.273	 14.305   1.000
 15.305    2.000	14.800	 15.053   1.000

final estimate:            15.053
actual final position:     14.838

下面是该滤波器的动图。预测用红色三角形绘制。在预测之后，滤波器接收下一个观测值，绘制成黑色圆圈。然后，滤波器在两者之间形成一个估计部分。

from kf_book import book_plots as book_plots
from ipywidgets.widgets import IntSlider

# save output in these lists for plotting
xs, predictions = [], []

process_model = gaussian(velocity, process_var) 

# perform Kalman filter
x = gaussian(0., 20.**2)
for z in zs:    
    prior = predict(x, process_model)
    likelihood = gaussian(z, sensor_var)
    x = update(prior, likelihood)

    # save results
    predictions.append(prior.mean)
    xs.append(x.mean)

def plot_filter(step):
    plt.cla()
    step -= 1
    i = step // 3 + 1
 
    book_plots.plot_predictions(predictions[:i])    
    if step % 3 == 0:
        book_plots.plot_measurements(zs[:i-1])
        book_plots.plot_filter(xs[:i-1])
    elif step % 3 == 1:
        book_plots.plot_measurements(zs[:i])
        book_plots.plot_filter(xs[:i-1])
    else:
        book_plots.plot_measurements(zs[:i])
        book_plots.plot_filter(xs[:i])

    plt.xlim(-1, 10)
    plt.ylim(0, 20)
    plt.legend(loc=2)
interact(plot_filter, step=IntSlider(value=1, min=1, max=len(predictions)*3))

我已经绘制了先验值（预测值）、观测值和滤波器输出。对于每一次循环迭代，我们形成一个先验值，再取一个观测值，从观测值中形成一个似然，最后将似然合并到先验值中。

如果你看这个图，你会发现滤波器的估计值总是介于观测值和预测值之间。在这两个值之外选择一个值是没有意义的。如果我预测我在10岁，但观测我在9岁，那么决定我必须在8岁或11岁是愚蠢的。

代码解读

现在让我们浏览一下代码：

process_var = 1.
sensor_var = 2.

这些是过程模型和传感器的方差。传感器方差的含义应该很清楚——它是指在每次观测中有多少误差；过程模型方差是指过程模型中有多少误差。我们预测狗每走一步就向前走一米，但是狗很少做我们期望的事情，像障碍物或食物的气味都会改变它的运动。如果这是一个机器人对数字指令做出的响应，那么性能会更好，也许过程模型方差会是${\sigma }^2=.05$。这些不是能随意编写的数字：方差的平方根是距离标准差，单位为$m$。只需对数字赋值就能很容易地使卡尔曼滤波器工作，但如果数字并不能反映实际情况，则滤波器的性能会很差。

x = gaussian(0., 20.**2)

这是狗的初始位置，用高斯来表示。初始位置为$0m$，方差为$400m^2$，标准差为$20m$。你可以这样想：$99.7\%$的概率下，狗的初始位置在$0\pm 60m$之间。这是因为在高斯条件下，$99.7\%$的值落在均值的$\pm 3\sigma$范围内。

process_model = gaussian(velocity, process_var)

这是一个过程模型——描述我们认为狗是如何移动的。我怎么知道速度？本例中是直接赋值的。当然，你可以把它当作一个预测，或者我们有一个二级速度传感器。如果这是一个机器人，那么这将是机器人的控制输入。在后面的文章中，我们将学习如何处理没有速度传感器或控制输入的情况，所以现在请接受这种简化。

接下来，我们初始化模拟并创建10个观测值：

dog = DogSimulation(
    x0=x.mean, 
    velocity=process_model.mean, 
    measurement_var=sensor_var, 
    process_var=process_model.var)

zs = [dog.move_and_sense() for _ in range(10)]

现在我们进入我们的predict()...update()循环。

for z in zs:
    prior = predict(x, process_model)
    likelihood = gaussian(z, sensor_var)
    x = update(prior, likelihood)

可以看到：第一次通过循环，先验值是$(1.0,401.0)$。也就是说，在预测之后，我们相信狗的位置在1.0，方差是401，方差很是糟糕。这是预测步骤中经常发生的情况，因为它涉及到信息的丢失。

然后我们使用先验值作为当前位置，调用update()函数。

我得到的结果是：$pos=(1.352,1.990)$，$z=1.354$。

发生了什么事？狗的预测位置在1.0，但观测的位置是1.354。由于先验的方差是401，传感器的方差是2.0，较大的方差意味着概率非常低，因此滤波器估计位置非常接近观测值：1.352。

现在看看方差：1.99。它从401大幅下降。为什么呢？RFID传感器有一个相当小的方差2.0，所以我们比起先验值更相信观测值。但是，先验值里面确实也包含了一些有用的信息，因此我们的方差现在略小于2.0。

接着，依次调用predict()和update()。到最后，最终估计位置为15.053，而实际位置为14.838，且方差也已收敛到1.0。

现在看图形。噪声观测用黑色圆圈绘制，滤波结果用蓝色实线绘制。两者都很吵，但请注意观测值有多吵。我用红色三角形绘制了预测图。估计值总是介于先验值和观测值之间。这是你的第一个卡尔曼滤波器，它似乎工作！

滤波只在几行代码中实现。其他的大部分代码要么是初始化，要么是存储数据，要么是模拟狗的运动，要么是打印结果。执行滤波的代码非常简洁：

prior = predict(x, process_model)
likelihood = gaussian(z, sensor_var)
x = update(prior, likelihood)

如果不使用predict()和update()函数，代码可能是：

for z in zs:
    # predict
    dx = velocity*dt
    pos = pos + dx
    var = var + process_var

    # update
    pos  = (var*z + sensor_var*pos) / (var + sensor_var)
    var = (var * sensor_var) / (var + sensor_var)

只有5行非常简单的数学代码，实现了整个滤波器！

在这个例子中，我只画了10个数据点，这样print()语句的输出就不会让我们看起来很累。现在让我们看看滤波器更多数据下的性能。方差在虚线之间绘制成淡黄色阴影区域。我已经增加了过程和传感器方差的大小，所以它们更容易在图表上看到——对于一个真正的卡尔曼滤波器，当然你不会随机改变这些值。

process_var = 2.
sensor_var = 4.5
x = gaussian(0., 400.)
process_model = gaussian(1., process_var)
N = 25

dog = DogSimulation(x.mean, process_model.mean, sensor_var, process_var)
zs = [dog.move_and_sense() for _ in range(N)]

xs, priors = np.zeros((N, 2)), np.zeros((N, 2))
for i, z in enumerate(zs):
    prior = predict(x, process_model)    
    x = update(prior, gaussian(z, sensor_var))
    priors[i] = prior
    
    xs[i] = x

book_plots.plot_measurements(zs)
book_plots.plot_filter(xs[:, 0], var=priors[:, 1])
book_plots.plot_predictions(priors[:, 0])
book_plots.show_legend()
kf_internal.print_variance(xs)

4.4502 2.6507 2.2871 2.1955 2.1712
2.1647 2.1629 2.1625 2.1623 2.1623
2.1623 2.1623 2.1623 2.1623 2.1623
2.1623 2.1623 2.1623 2.1623 2.1623
2.1623 2.1623 2.1623 2.1623 2.1623

在这里我们可以看到，方差第9次循环就已经收敛到2.1623。此时$\sigma =1.47$，相比较于传感器的$\sigma =2.12$，这意味着我们对我们的位置估计变得非常有信心。由于初始位置的不确定性，最初的几个观测值是不确定的，但是滤波器很快收敛到比传感器方差更低的估计值！

这段代码完全实现了卡尔曼滤波。如果你试过阅读其他文献，你可能会感到惊讶，因为这一点都不像那些书中没完没了的数学篇章。我们用高斯表示概率，随着时间的推移，它们会变得更好，因为更多的观测意味着我们有更多的数据要处理。

练习：修改方差值

修改process_var和sensor_var的值，并注意对滤波器的影响。哪个因素对方差收敛的影响较大？例如，哪个会导致较小的方差：

process_var = 40
sensor_var = 2

还是

process_var = 2
sensor_var = 40

结果显而易见，前者会导致更小的方差。

简单思考一下就能得到结论，后验的均值会介于先验值和观测值之间，后验的方差会小于先验值和观测值。由于过程模型是有噪声的，导致信息丢失方差变大，也就是说，先验的方差肯定是会大于process_var的。那么，前者的方差肯定小于2，后者的方差会小于先验的方差和40的较小值，但先验的方差值肯定大于2了。

KF动画

我们看一下狗的跟踪滤波器的输出结果：

上图以绿色显示滤波器的输出，以红色虚线显示观测值。下图则显示了每一步的高斯分布。

当第一次循环时，你可以看到观测值与最初的预测值相差很大。此时高斯概率很小（曲线很低且很宽），因此滤波器不信任其预测值。因此，滤波器会大量调整其估计值。随着滤波器循环次数的增加，你可以看到，随着高斯变得更高，表明估计的确定性更强，滤波器的输出变得非常接近于一条直线。当$x=15$或更大时，你可以看到观测中存在大量噪声，但与第一次观测时的噪声引起的变化量相比，滤波器对噪声的反应不大。

卡尔曼增益

我们看到卡尔曼滤波器工作正常。现在让我们回到数学上，来理解发生了什么。后验$x$的计算方法是似然乘以先验（$\mathcal{L}\cdot \bar{x}$），两者都是高斯分布。

因此，后验平均值由下式给出：

$$\mu =\frac{{\bar{\sigma }}^2{\mu }_z+{\sigma }_z^2\bar{\mu }}{{\bar{\sigma }}^2+{\sigma }_z^2}$$

我用下标$z$来表示观测，我们可以改写为：

$$\mu =\frac{{\bar{\sigma }}^2}{{\bar{\sigma }}^2+{\sigma }_z^2}{\mu }_z+\frac{{\sigma }_z^2}{{\bar{\sigma }}^2+{\sigma }_z^2}\bar{\mu }$$

在这种形式中，很容易看出，我们正在按权重缩放观测值和先验值：

$$\mu =W_1{\mu }_z+W_2\bar{\mu }$$

因为分母是一个标准化项，所以权重总和为1。我们引入一个新术语，$K=W_1$，因此有：

$$\mu =K{\mu }_z+(1-K)\bar{\mu }=\bar{\mu }+K({\mu }_z-\bar{\mu })$$

其中，

$$K=\frac{{\bar{\sigma }}^2}{{\bar{\sigma }}^2+{\sigma }_z^2}$$

$K$是卡尔曼增益，它是卡尔曼滤波的关键。它是一个用于选择介于${\mu }_z$和$\bar{\mu }$之间的一个值的标度项。

让我们举几个例子。如果观测的精确度是先验值的九倍，则${\bar{\sigma }}^2=9{\sigma }_z^2$，那么有：

$$\mu =\frac{9{\sigma }_z^2{\mu }_z+{\sigma }_z^2\bar{\mu }}{9{\sigma }_z^2+{\sigma }_z^2}=(\frac{9}{10}){\mu }_z+(\frac{1}{10})\bar{\mu }$$

因此，$K=\frac{9}{10}$。后验的组成部分就是，取观测值的十分之九和先验值的十分之一。

如果观测值和先验值同样准确，则${\bar{\sigma }}^2={\sigma }_z^2$，那么有：

$$\mu =\frac{{\sigma }_z^2{\mu }_z+{\sigma }_z^2\bar{\mu }}{{\sigma }_z^2+{\sigma }_z^2}=(\frac{1}{2}){\mu }_z+(\frac{1}{2})\bar{\mu }$$

后验就是，观测值和先验值的平均值。取两个同样精确值的均值是很直观的。

我们也可以用卡尔曼增益表示方差：

$${\sigma }^2=\frac{{\bar{\sigma }}^2{\sigma }_z^2}{{\bar{\sigma }}^2+{\sigma }_z^2}=K{\sigma }_z^2=(1-K){\bar{\sigma }}^2$$

我们可以通过查看以下图来理解这一点：

import kf_book.book_plots as book_plots

book_plots.show_residual_chart()

卡尔曼增益$K$是一个比例因子，用来沿残差的方向选择一个值作为后验。这将导致update()和predict()的另一个等效实现：

def update(prior, measurement):
    x, P = prior        # mean and variance of prior
    z, R = measurement  # mean and variance of measurement
    
    y = z - x        # residual
    K = P / (P + R)  # Kalman gain

    x = x + K*y      # posterior
    P = (1 - K) * P  # posterior variance
    return gaussian(x, P)

def predict(posterior, movement):
    x, P = posterior # mean and variance of posterior
    dx, Q = movement # mean and variance of movement
    x = x + dx
    P = P + Q
    return gaussian(x, P)

为什么我要用这种形式写它，为什么我要选择这些可怕的变量名？有一些相关的原因：大多数书籍和论文都以这种形式介绍了卡尔曼滤波器。此外，多元卡尔曼滤波器的方程看起来几乎完全像这些方程。所以，你需要学习和理解它们。

$z$、$P$、$Q$和$R$的名字从哪里来？你在文献中，$R$几乎普遍用于观测噪声，$Q$用于过程噪声，$P$用于状态协方差。使用$z$表示观测值是很常见的，尽管不是通用的。几乎你读过的每一本书和每一篇论文都会使用这些变量名。

这也是一种思考滤波的有效方法：它强调取残差$y={\mu }_z-\bar{\mu }$，将先验的不确定性与观测的不确定性的某种比值：$K=P/(P+R)$作为卡尔曼增益，并通过将它来计算后验值。

贝叶斯在这种推导形式下是模糊的，你可能找不到什么具体的解释含义，正如我们用先验乘以似然这一事实一样。但其实两种观点是等价的，因为数学上是相同的。我之所以选择贝叶斯方法，是因为我认为它对概率推理有更直观、更深刻的理解。然而，这种使用$K$的替代形式对正交投影法有了深刻的理解。卡尔曼博士在发明这种滤波器时使用的就是这种推导，而不是贝叶斯推理。

算法的完整描述

卡尔曼滤波器进行预测，进行观测，然后在两者之间形成一个新的估计。

理解这一点非常重要：本系列中的每个滤波器都实现相同的算法，只是数学细节不同。在后面的文章中，数学可能会变得很有挑战性，但这个想法很容易理解。

重要的是要看具体滤波器方程的细节，并了解方程式计算的内容和原因。滤波器的种类很多，它们都使用不同的数学方法来实现相同的算法。数学方法的选择影响结果的质量和问题的表现，但不影响基本思想。

以下是通用算法：

初始化

1. 初始化滤波器的状态量
2. 初始化状态量的概率

预测

1. 使用过程模型来预测下一时间步的状态量
2. 调整概率来解释预测中的不确定性

更新

1. 得到一个观测值和它的准确度
2. 根据先验值和观测值谁更准确来计算比例因子
3. 基于比例因子在先验值和观测值之间设置状态量
4. 基于比例因子更新对状态量的概率

你将很难找到一个贝叶斯滤波算法，不适合这种形式。一维卡尔曼滤波器的方程为：

预测

$$\begin{array}{lll}\text{Equation}& \text{Implementation}& \text{Kalman Form}\\ \bar{x}=x+f_x& \bar{\mu }=\mu +{\mu }_{f_x}& \bar{x}=x+dx\\ & {\bar{\sigma }}^2={\sigma }^2+{\sigma }_{f_x}^2& \bar{P}=P+Q\end{array}$$

更新

$$\begin{array}{lll}\text{Equation}& \text{Implementation}& \text{Kalman Form}\\ x=|\mathcal{L}\cdot \bar{x}|& y=z-\bar{\mu }& y=z-\bar{x}\\ & K=\frac{{\bar{\sigma }}^2}{{\bar{\sigma }}^2+{\sigma }_z^2}& K=\frac{\bar{P}}{\bar{P}+R}\\ & \mu =\bar{\mu }+Ky& x=\bar{x}+Ky\\ & {\sigma }^2=\frac{{\bar{\sigma }}^2{\sigma }_z^2}{{\bar{\sigma }}^2+{\sigma }_z^2}& P=(1-K)\bar{P}\end{array}$$

和离散贝叶斯滤波器的比较

让我们使用均匀分布、高斯分布和离散分布来绘制一个传感器的数据。

from random import random

xs = np.arange(145, 190, 0.1)
ys = [stats.gaussian(x, 160, 3**2) for x in xs]
belief = np.array([random() for _ in range(40)])
belief = belief / sum(belief)

x = np.linspace(155, 165, len(belief))
plt.gca().bar(x, belief, width=0.2)
plt.plot(xs, ys, label='A', color='g')
plt.errorbar(160, [0.04], xerr=[3], fmt='o', color='k', capthick=2, capsize=10)    
plt.xlim(150, 170)

使用均匀分布或高斯分布只是一种建模选择，两者都不能准确描述现实。但在大多数情况下，高斯分布更为真实。大多数传感器更可能返回接近被测值的读数，不太可能返回远离该值的读数。高斯模型模拟了这种趋势。相反，均匀分布假设在一个范围内的任何观测都是同样可能的。

离散贝叶斯滤波器中使用离散分布，该模型将可能值的范围划分为离散范围，并为每个离散值分配一个概率。只要概率和为1，这个分配就可以是没有问题的。

我用随机数形成离散分布来说明它可以模拟任意概率分布，这为它提供了巨大的力量。只要有了足够的离散值，我们可以建模任何传感器的误差特性，无论多么复杂。但伴随着这种力量，数学上的难题也随之而来。将高斯数相乘或相加需要两行数学运算，结果是另一个高斯数。这种规律性允许我们对滤波器的性能和行为进行强有力的分析。而乘以或添加一个离散分布需要在数据上循环，我们没有简单的方法来描述结果。分析基于离散分布的滤波器的性能特性是非常困难和不可能的。

这里没有正确的选择。在后面的文章中，我们将介绍使用离散分布的粒子滤波器。这是一个非常强大的技术，因为它可以处理任意复杂的情况。这是以缓慢的性能和对分析的阻力为代价的。

一些示例

示例：温度计

到目前为止，我们已经为位置传感器编写了滤波器。我们现在已经习惯了这个问题，可能会觉得没有能力实现一个不同问题的卡尔曼滤波器。现在让我们通过设计和实现一个温度计的卡尔曼滤波器来感受一下。温度计传感器输出一个与被测温度相对应的电压。假设传感器的标准偏差为$0.13V$。

我们可以用这个函数来模拟温度传感器的观测：

def volt(voltage, std):
    return voltage + (randn() * std)

现在我们需要编写卡尔曼滤波的处理过程了。与前面的问题一样，我们需要执行预测和更新的循环。更新步骤可能看起来很清楚——调用volt()获取观测值，然后将结果传递到update()方法。但是预测步骤呢？我们没有一个传感器来检测电压中的变化，这该怎么处理？

我们没有已知的电压变化，所以我们将把它设为零。然而，这意味着我们预测温度永远不会改变。如果这是真的，那么随着时间的推移，我们应该对我们的结果充满信心。一旦滤波器有足够的观测值，它将变得非常有信心，它可以预测随后的温度，这将导致它会忽略由于实际温度变化而产生的观测值。这是你想要避免的。

因此，我们将在预测步骤中添加一点噪声，以告诉滤波器不要忽略电压随时间的变化。在下面的代码中，我设置了$process\_var=.05\ast \ast 2$。这是每个时间步上电压变化的预期方差。我选择这个值仅仅是为了能够通过预测和更新步骤显示方差是如何变化的。对于一个真正的传感器，你可以为你期望的实际变化量设置这个值。例如，如果这是一个用于观测室内环境空气温度的温度计，那么这将是一个非常小的数字；如果这是一个化学反应室中的热电偶，那么这将是一个很高的数字。

让我们看看会发生什么：

temp_change = 0
voltage_std = .13
process_var = .05**2
actual_voltage = 16.3

x = gaussian(25., 1000.) # initial state
process_model = gaussian(0., process_var)

N = 50
zs = [volt(actual_voltage, voltage_std) for i in range(N)]
ps = []
estimates = []

for z in zs:
    prior = predict(x, process_model)
    x = update(prior, gaussian(z, voltage_std**2))

    # save for latter plotting
    estimates.append(x.mean)
    ps.append(x.var)

# plot the filter output and the variance
book_plots.plot_measurements(zs)
book_plots.plot_filter(estimates, var=np.array(ps))
book_plots.show_legend()
plt.ylim(16, 17)
book_plots.set_labels(x='step', y='volts')
plt.show()
    
plt.plot(ps)
plt.title('Variance')
print('Variance converges to {:.3f}'.format(ps[-1]))

Variance converges to 0.005

第一个图显示传感器观测值与滤波器输出值的对比。尽管传感器中有很多噪声，我们还是很快发现了传感器的近似电压。滤波器的最后一个电压输出是16.213，这与volt()函数使用的16.4非常接近。

尽管基于正常且相同的制造过程，但传感器都会表现出不同的性能。如果你买了一件昂贵的设备，它通常会附带一张纸，显示他在特定项目的测试结果；这通常是非常值得信赖的。另一方面，如果这是一个便宜的传感器，它很可能只接受到很少、甚至没有进行测试。因此如果你有一个关键的项目需要用到传感器，你需要仔细阅读说明书，弄清楚它们的范围到底意味着什么。

例如，我正在研究一个气流传感器的可重复性，说明书上其值为$\pm 0.50$。这是高斯分布吗？例如，在低温下，可重复性可能接近0.0；在高温下，可重复性总是接近$+0.50$。电气元件的说明书通常包含典型性能特征，它用于描述用表格无法轻松传递的信息。例如，我正在看LM555定时器的输出电压与电流的图表。有三条曲线显示了在不同温度下的性能。理想情况下电压和电流是线性的，但这三条线都是曲线。这说明电压输出中的误差可能不是高斯分布的——可能在这种芯片中，较高的温度导致较低的电压输出，如果输入电流非常高，电压输出是非常非线性的。

正如你可能猜到的，建立传感器性能模型是创建性能良好的卡尔曼滤波器的困难部分之一。

动图

这里有一个显示滤波器工作的动图：

上图将预测的电压绘制一条绿线，实际观测值绘制成一个红色的+，同时绘制一条浅红色竖线表示残差，然后为滤波器的输出绘制一条蓝色线。你可以看到，当滤波器刚开始启动时，所做的校正非常大，但是仅进行了几次循环后，即使观测值距离滤波器较远，滤波器也只调整了少量的输出。

下图显示了该滤波器的pdf。当滤波器开始时，高斯曲线的中心在25以上，这是我们对电压的初始猜测。由于我们对初始状态很不确定，它非常宽且低。但随着滤波器的更新，高斯函数很快就移到16.0左右，并变得更高，这反映出滤波器对电压估计的信心不断增强。你也会注意到高斯的高度有点上下起伏。如果你仔细观察，你会发现在预测步骤中，高斯分布变得更低，更分散，并且随着滤波器加入另一个观测值，高斯分布变得更高更窄。

从残差角度来考虑这个动图。在每个循环中，滤波器进行预测，进行观测，计算残差（预测值和观测值之间的差），然后基于比例因子$g$在残差线上选择一个点。只是比例因子$g$随时间而变化，当滤波器对其状态更加自信时，比例因子有利于滤波器的预测而不是观测。

示例：极端噪音

在狗的跟踪滤波器中，我并没有在信号中加入太多噪音。我将在RFID传感器中注入更多噪声（噪声淹没了实际观测），同时将过程方差保持在2。如果传感器的标准差为300，你直觉这会对滤波器的性能造成什么影响？换言之，$1.0m$的实际位置可能报告为$287.9m$或$-589.6m$，或是在该范围内的任何其他数字。在阅读下面内容之前先考虑一下。

sensor_var = 300.**2
process_var = 2.
process_model = gaussian(1., process_var)
pos = gaussian(0., 500.)
N = 1000
dog = DogSimulation(pos.mean, 1., sensor_var, process_var)
zs = [dog.move_and_sense() for _ in range(N)]
ps = []

for i in range(N):
    prior = predict(pos, process_model)    
    pos = update(prior, gaussian(zs[i], sensor_var))
    ps.append(pos.mean)

book_plots.plot_measurements(zs, lw=1)
book_plots.plot_filter(ps)
plt.legend(loc=4)

在这个例子中，噪声是极端的，但滤波器仍然输出一条近乎直线！这是一个惊人的结果！你认为产生这个结果的原因是什么？

我们得到一条近似直线，因为我们的过程误差很小。一个小的过程误差告诉滤波器，预测是非常可信的，并且预测是一条直线，因此滤波器输出一条近似直线。

示例：不正确的过程方差

上一个滤波器看起来棒极了！那为什么我们不把过程方差设得很低，因为这样可以保证结果的平滑呢？

过程方差告诉滤波器系统随时间变化的程度。如果你对滤波器撒谎，人为地把这个数字设置得很低，滤波器将无法对正在发生的变化做出反应。让狗在每一个时间步增加一点速度，看看滤波器在0.001的过程方差下的性能。

sensor_var = 20.
process_var = .001
process_model = gaussian(1., process_var)
pos = gaussian(0., 500.)
N = 100
dog = DogSimulation(pos.mean, 1, sensor_var, process_var*10000)
zs, ps = [], []
for _ in range(N):
    dog.velocity += 0.04
    zs.append(dog.move_and_sense())

for z in zs:
    prior = predict(pos, process_model)    
    pos = update(prior, gaussian(z, sensor_var))
    ps.append(pos.mean)

book_plots.plot_measurements(zs, lw=1)
book_plots.plot_filter(ps)
plt.legend(loc=4)

很容易看出滤波器没有正确响应观测值。观测结果清楚地表明狗正在改变速度，但是滤波器却被告知它的预测几乎是完美的，所以它几乎完全忽略了它们。关键的一点是要认识到，数学上要求方差正确地描述你的系统。滤波器不会注意到它偏离了观测值并自行校正，它从先验值和观测值的方差中计算出卡尔曼增益，并根据哪个更精确来形成估计。

示例：错误的初始估计

现在让我们看看，当我们对初始位置做出错误的初始估计时的结果。我将传感器方差设置为25，但将初始位置设置为$1000m$。滤波器能从$1000m$的误差中恢复吗？

sensor_var = 5.**2
process_var = 2.
pos = gaussian(1000., 500.)
process_model = gaussian(1., process_var)
N = 100
dog = DogSimulation(0, 1, sensor_var, process_var)
zs = [dog.move_and_sense() for _ in range(N)]
ps = []

for z in zs:
    prior = predict(pos, process_model)    
    pos = update(prior, gaussian(z, sensor_var))
    ps.append(pos.mean)

book_plots.plot_measurements(zs, lw=1)
book_plots.plot_filter(ps)
plt.legend(loc=4)

答案是肯定的！因为我们相对确定我们对传感器的观测值（${\sigma }^2=5^2$），仅在第一步之后，我们就将位置从1000更改为大约50。在另外5-10次观测之后，我们已经收敛到正确的值。在实践中，我们可能会将传感器的第一个观测值指定为初始值。但你可以看到，如果我们疯狂地猜测初始条件，这并不重要——只要选择的滤波器方差与实际过程和观测方差相匹配，卡尔曼滤波器仍然收敛。

示例：噪声大，初始估计差

如果两种糟糕的情况都碰到了一起，大噪音和糟糕的初步估计，情况又会怎样呢？

sensor_var = 30000.
process_var = 2.
pos = gaussian(1000., 500.)
process_model = gaussian(1., process_var)

N = 1000
dog = DogSimulation(0, 1, sensor_var, process_var)
zs = [dog.move_and_sense() for _ in range(N)]
ps = []

for z in zs:
    prior = predict(pos, process_model) 
    pos = update(prior, gaussian(z, sensor_var))
    ps.append(pos.mean)

book_plots.plot_measurements(zs, lw=1)
book_plots.plot_filter(ps)
plt.legend(loc=4)

这一次滤波器在挣扎。注意，前面的示例只计算了100个更新，而这个示例使用1000个更新。在我看来，滤波器需要400次左右的迭代才能变得合理准确，但在结果良好之前，可能需要600次以上的迭代。卡尔曼滤波器是好的，但我们不能期待奇迹。如果我们有非常嘈杂的数据和非常糟糕的初始条件，这也许就是比较好的结果了。

最后，让我们实施以第一次观测作为初始位置的建议：

sensor_var = 30000.
process_var = 2.
process_model = gaussian(1., process_var)
N = 1000
dog = DogSimulation(0, 1, sensor_var, process_var)
zs = [dog.move_and_sense() for _ in range(N)]

pos = gaussian(zs[0], 500.)
ps = []
for z in zs:
    prior = predict(pos, process_model) 
    pos = update(prior, gaussian(z, sensor_var))
    ps.append(pos.mean)

book_plots.plot_measurements(zs, lw=1)
book_plots.plot_filter(ps)
plt.legend(loc='best')

这个简单的改变显著地改善了结果。在某些运行中，需要200次迭代才能找到一个好的估计；但在其他运行中，收敛速度非常快。这完全取决于第一次观测中的噪声量。大量的噪音导致最初的估计可能远离狗的位置。

200次迭代看起来可能很多，但我们注入的噪声量确实很大。在现实世界中，我们使用诸如温度计、激光测距仪、GPS卫星、计算机视觉等传感器。在这些例子中，绝不会有如此大的错误。便宜的温度计的合理方差可能是0.2，而我们的代码使用的是30000。

示例：交互例子

使用Jupyter笔记本的动画功能实现卡尔曼过滤器，允许你使用滑块实时修改各种参数。你将使用interact()函数调用计算和打印函数，传入interact()的每个参数都会自动为其创建一个滑块。我已经为此写了样板，你只需要填写所需的代码即可。

from ipywidgets import interact
from kf_book.book_plots import FloatSlider

def plot_kalman_filter(start_pos, 
                       sensor_noise, 
                       velocity, 
                       process_noise):
    plt.figure()
    # your code goes here

interact(plot_kalman_filter,
         start_pos=(-10, 10), 
         sensor_noise=FloatSlider(value=5, min=0, max=100), 
         velocity=FloatSlider(value=1, min=-2., max=2.), 
         process_noise=FloatSlider(value=5, min=0, max=100.))

如果你没有本地运行环境，可以点击链接在线运行调试（加载的时间可能比较久）：http://mybinder.org/repo/rlabbe/Kalman-and-Bayesian-Filters-in-Python

交互例子的一种填法

一个可能的填法如下。我们可调整的参数有初始位置，传感器中的噪声量，每个时间步中移动的量，以及有多大的移动误差。过程噪声也许是最不清晰的——它模拟了狗在每一个时间步偏离路线的程度，所以我们在每一步都把它加到狗的位置上。

from numpy.random import seed 
from ipywidgets import interact

def plot_kalman_filter(start_pos, 
                       sensor_noise, 
                       velocity,
                       process_noise):
    N = 20
    zs, ps = [], []   
    seed(303)
    dog = DogSimulation(start_pos, velocity, sensor_noise, process_noise)
    zs = [dog.move_and_sense() for _ in range(N)]
    pos = gaussian(0., 1000.) # mean and variance
    process_model = gaussian(velocity, process_noise)
    
    for z in zs:    
        pos = predict(pos, process_model)
        pos = update(pos, gaussian(z, sensor_noise))
        ps.append(pos.mean)

    plt.figure()
    plt.plot(zs, c='k', marker='o', linestyle='', label='measurement')
    plt.plot(ps, c='#004080', alpha=0.7, label='filter')
    plt.legend(loc=4)

interact(plot_kalman_filter,
         start_pos=(-10, 10), 
         sensor_noise=FloatSlider(value=5, min=0., max=100), 
         velocity=FloatSlider(value=1, min=-2., max=2.), 
         process_noise=FloatSlider(value=.1, min=0, max=40))

如果你没有本地运行环境，可以点击链接在线运行调试（加载的时间可能比较久）：http://mybinder.org/repo/rlabbe/Kalman-and-Bayesian-Filters-in-Python

示例：非线性系统

我们的卡尔曼滤波方程是线性的：

$$N(\bar{\mu },{\bar{\sigma }}^2)=N(\mu ,{\sigma }^2)+N({\mu }_{move},{\sigma }_{move}^2)$$

$$N(\mu ,{\sigma }^2)=N(\bar{\mu },{\bar{\sigma }}^2)\times N({\mu }_z,{\sigma }_z^2)$$

你认为这个滤波器在非线性系统中工作得好还是不好？

实现一个卡尔曼滤波器，该滤波器使用以下等式来生成观测值：

for i in range(100):
    z = math.sin(i/3.) * 2

调整方差和初始位置以查看效果。例如，一个非常糟糕的初始猜测的结果是什么？

#enter your code here.

解决方法

import math

sensor_var = 30.
process_var = 2.
pos = gaussian(100., 500.)
process_model = gaussian(1., process_var)

zs, ps = [], []

for i in range(100):
    pos = predict(pos, process_model)

    z = math.sin(i/3.)*2 + randn()*1.2
    zs.append(z)
    
    pos = update(pos, gaussian(z, sensor_var))
    ps.append(pos.mean)

plt.plot(zs, c='r', linestyle='dashed', label='measurement')
plt.plot(ps, c='#004080', label='filter')
plt.legend(loc='best')

讨论

太可怕了！输出完全不像一个正弦波。对于线性系统，我们可以在信号中加入大量的噪声，仍然可以提取出非常精确的结果，但在这里，即使是适度的噪声也会产生非常糟糕的结果。

卡尔曼滤波器的结构要求滤波器输出在预测和观测之间选择一个值。像这样的变化信号总是在加速，而我们的过程模型假设速度恒定，所以从数学上导致滤波器总是滞后于输入信号。

固定增益滤波器

嵌入式设备的处理器的计算能力通常非常有限，许多没有浮点计算单元，这些简单的方程会给芯片带来沉重的负担。

在上面的例子中，滤波器的方差收敛到一个固定值。如果观测和过程的方差是一个常数，则总是会发生这种情况。你可以通过运行模拟来确定方差收敛到什么程度，从而可以利用这一事实。然后你可以把这个值硬编码到你的滤波器里。只要你将滤波器初始化为一个良好的值（我建议使用第一个观测值作为初始值），滤波器将运行得非常好。例如，狗跟踪滤波器可以简化为：

def update(x, z):
    K = .13232  # experimentally derived Kalman gain
    y = z - x   # residual
    x = x + K*y # posterior
    return x

def predict(x):
    return x + vel*dt

我用卡尔曼增益形式的更新函数来强调我们根本不需要考虑方差。如果方差收敛到一个值，那么卡尔曼增益也会收敛。

FilterPy的实现

FilterPy实现predict()和update()，它们不仅适用于本文中提出的一元情况，而且适用于我们在后续文章中学习的更一般的多元情况。因此，它们的接口略有不同。它们不将高斯数的$\mu$和$\sigma$作为元组，而是作为两个单独命名的变量。

predict()接受好几个参数，但我们只需要使用以下四个参数：

predict(x, P, u, Q)

$x$是系统的状态，$P$是系统的方差；$u$是过程引起的运动，$Q$是过程中的噪声。调用predict()时需要使用命名参数，因为大多数参数都是可选的。

你可能会觉得这些变量名很糟糕，确实是！正如我已经提到的，它们来自控制理论的悠久历史，你读的每一篇论文或书都会用到这些变量名。所以，我们必须习惯它。

让我们试试状态$N(10,3)$和运动$N(1,4)$。我们预计最终位置为11，方差为7。

import filterpy.kalman as kf

kf.predict(x=10., P=3., u=1., Q=4.)

(11.0, 7.0)

update()也包含几个参数，但目前你只需要对以下四个参数感兴趣：

update(x, P, z, R)

和前面一样，$x$和$P$是系统的状态和方差；$z$是观测值，$R$是观测方差。让我们执行上一个predict()语句以获取先验，然后执行更新：

x, P = kf.predict(x=10., P=3., u=1., Q=2.**2)
print('%.3f' % x)

x, P = kf.update(x=x, P=P, z=12., R=3.5**2)
print('%.3f' % x, '%.3f' % P)

11.000
11.364 4.455

我给它一个有噪声的观测和一个大的方差，但是估计仍然接近先验值11。

最后一点：我没有将predict()的输出中使用变量名prior。卡尔曼滤波方程只用$\mathbf{x}$，无论是先验和后验都是系统的状态，前者是观测合并前的状态，后者是观测合并后的状态。

总结

本文中描述的卡尔曼滤波器，是我们将在下一文章中学习到的更一般的滤波器的一个特殊的、受限的情况。大多数文章并不讨论这种一元形式。然而，我认为这是一个至关重要的垫脚石。我们从离散贝叶斯滤波器开始，到现在实现了一元卡尔曼滤波器。我试着向你们展示，这些滤波器中的每一个都使用相同的算法。我们不久将学习的卡尔曼滤波器的数学是相当复杂的。这种复杂性带来了显著的好处：性能将明显优于本文中的滤波器。

本文需要时间来理解。要真正理解这一点，你可能要把本文多读几遍。我鼓励你更改代码中的各种变量并观察结果。最重要的是，花足够的时间对算法部分进行完整的描述，以确保你了解算法以及它与离散贝叶斯滤波器的关系。这里只有一个窍门——在预测值和观测值之间选择一个值。每一种算法都用不同的数学运算来实现这一点，但都使用相同的逻辑。

相关阅读

• Kalman-and-Bayesian-Filters-in-Python/04-One-Dimensional-Kalman-Filters