%matplotlib inline

#format the book
import book_format
book_format.set_style()

简介

在上一章节中，我们讨论了教科书式的问题。这些问题很容易陈述，只需几行代码就可以编程，也很容易教学。但现实世界的问题很少这么简单。在本章中，我们将使用更实际的示例，并学习如何评估滤波器性能。

我们将首先在二维空间中跟踪一个机器人，比如一个场地或仓库。我们将从一个简单的传感器开始，该传感器输出有噪声的$(x,y)$坐标，我们需要对其进行滤波以生成二维轨迹。一旦我们掌握了这个本领，我们将通过更多的传感器，然后添加控制输入来扩展这个问题。

然后我们将讨论一个非线性问题。世界是非线性的，而卡尔曼滤波是线性的。有时你可以用它来解决轻微的非线性问题，有时你不能。我会给你两个例子，这将后面的章节奠定基础。

跟踪机器人

跟踪机器人问题，类似于之前利用在走廊里输出位置的传感器跟踪一维空间下的狗的问题。我们现在有一个在二维空间中，提供有噪声的位置观测的传感器。在每个时间$t$，它将提供一个有噪声的$(x,y)$坐标。

与真实传感器交互的代码实现不是我们要研究的内容，因此我们将像以前一样编写传感器的简单模拟程序。我们将编写几个不同情况下的传感器，每一个都有更多的复杂性，所以当我对它们进行编程时，我只会在函数名后面加上一个数字。

让我们从一个非常简单的情况开始，一个模拟跟踪直线运动物体的传感器。它用初始位置、速度和噪声标准差来进行初始化。每次调用read()都会更新一个时间步后的位置，并返回新的观测值。

from numpy.random import randn

class PosSensor(object):
    def __init__(self, pos=(0, 0), vel=(0, 0), noise_std=1.):
        self.vel = vel
        self.noise_std = noise_std
        self.pos = [pos[0], pos[1]]
        
    def read(self):
        self.pos[0] += self.vel[0]
        self.pos[1] += self.vel[1]
        
        return [self.pos[0] + randn() * self.noise_std,
                self.pos[1] + randn() * self.noise_std]

一个快速的测试来验证它是否如我们所期望的那样工作。

import matplotlib.pyplot as plt
import numpy as np
from kf_book.book_plots import plot_measurements

pos, vel = (4, 3), (2, 1)
sensor = PosSensor(pos, vel, noise_std=1)
ps = np.array([sensor.read() for _ in range(50)])
plot_measurements(ps[:, 0], ps[:, 1])

看起来不错。斜率是$1/2$，正如我们预期的，即速度是$(2,1)$。但其实，这仍然是一个教科书式的问题。在本文接下来的内容中，我们会增加一些复杂因素，从而模拟真实世界的行为。

选择状态量

一如既往，第一步是选择我们的状态量。我们在两个维度上进行跟踪，并且有一个传感器可以在这两个维度上给我们一个观测，所以我们知道有两个观测量$x$和$y$。如果我们只使用这两个状态量来创建卡尔曼滤波器，性能将不会很好，因为我们将忽略速度可以提供给我们的信息。因此，我们也要把速度纳入我们的方程中，即：

$$\mathbf{x}={\left[\begin{array}{cccc}x& \dot{x}& y& \dot{y}\end{array}\right]}^T$$

当然，我也可以使用诸如$\mathbf{x}={\left[\begin{array}{cccc}x& y& \dot{x}& \dot{y}\end{array}\right]}^T$的类似形式。我喜欢保持位置和速度相邻，因为它保持位置和速度之间的协方差，在协方差矩阵的同一个子块中。

让我们暂停一下，讨论如何识别隐藏量。这个例子有点明显，因为我们已经研究了一维的情况，但是其他问题可能不会那么容易地被识别出来。首先要问自己的是，传感器数据的一阶导数和二阶导数是什么含义。我们这样做是因为，如果你用一个固定的时间步从传感器中读取数据，那么获得一阶和二阶导数在数学意义上是很自然的。因为，一阶导数就是两个连续读数之间的差值。在我们的跟踪案例中，一阶导数有一个明显的物理解释：两个连续位置之间的差异就是速度。

除此之外，你还可以研究如何通过将两个或更多传感器的数据组合，以产生更多信息。这将打开了传感器融合的领域，我们将在后面的内容中介绍这方面的例子。现在，请认识到选择适当的状态量对于从滤波器中获得最佳性能至关重要。一旦选择了隐藏量，就必须进行许多测试，以确保它们生成真实的结果。无论你给出什么样的模型，卡尔曼滤波器都会运行；如果你的模型不能为隐藏量生成好的信息，卡尔曼滤波器的输出将是毫无意义的。

设计状态转移函数

下一步是设计状态转移函数。回想一下，状态转移函数被表示为一个矩阵$\mathbf{F}$，我们将它与系统的前一个状态相乘，得到下一个状态，就像这样。

$$\bar{\mathbf{x}}=\mathbf{Fx}$$

我不会像我们在之前所做的一维案例那样，再重复唠叨一遍。状态转换方程为：

$$x=1x+\Delta t\dot{x}+0y+0\dot{y}$$
$$\dot{x}=0x+1\dot{x}+0y+0\dot{y}$$
$$x=0x+0\dot{x}+1y+\Delta t\dot{y}$$
$$\dot{y}=0x+0\dot{x}+0y+1\dot{y}$$

我们将其转换为矩阵向量形式：

$$\left[\begin{array}{c}x\\ \dot{x}\\ y\\ \dot{y}\end{array}\right]=\left[\begin{array}{cccc}1& \Delta t& 0& 0\\ 0& 1& 0& 0\\ 0& 0& 1& \Delta t\\ 0& 0& 0& 1\end{array}\right]\left[\begin{array}{c}x\\ \dot{x}\\ y\\ \dot{y}\end{array}\right]$$

让我们用代码实现：

from filterpy.kalman import KalmanFilter

tracker = KalmanFilter(dim_x=4, dim_z=2)
dt = 1.   # time step 1 second

tracker.F = np.array([[1, dt, 0,  0],
                      [0,  1, 0,  0],
                      [0,  0, 1, dt],
                      [0,  0, 0,  1]])

设计过程噪声矩阵

FilterPy可以为我们计算$\mathbf{Q}$矩阵。为了简单起见，假设噪声在离散时间上符合分段白噪声模型——它每个时间段都是常数。这个假设允许我使用一个方差来表示，模型在不同步骤之间的变化程度。如果不清楚，请重新阅读卡尔曼滤波数学一章。

from scipy.linalg import block_diag
from filterpy.common import Q_discrete_white_noise

q = Q_discrete_white_noise(dim=2, dt=dt, var=0.001)
tracker.Q = block_diag(q, q)
print(tracker.Q)

[[0.    0.001 0.    0.   ]
 [0.001 0.001 0.    0.   ]
 [0.    0.    0.    0.001]
 [0.    0.    0.001 0.001]]

这里我假设$x$和$y$中的噪声是独立的，所以任何$x$和$y$变量之间的协方差应该都是零。这允许我计算一个维度的$\mathbf{Q}$，然后使用block_diag()复制$x$和$y$。

设计控制功能

由于现在还没有为我们的机器人添加控制输入，所以这一步没有什么可做的。KalmanFilter类在假设没有控制输入的情况下，将$\mathbf{B}$初始化为零，因此没有必要编写代码。如果你愿意，可以显式地将tracker.B设置为0。

设计观测函数

观测函数$\mathbf{H}$用于定义如何将状态量使用转换方程$\mathbf{z}=\mathbf{Hx}$得到观测。在这种情况下，我们有$(x,y)$的观测值，所以我们将$\mathbf{z}$设计为${\left[\begin{array}{cc}x& y\end{array}\right]}^T$，即尺寸为$2\times 1$。我们的状态量的尺寸为$4\times 1$。我们可以思考：将大小为$M\times N$的矩阵乘以$N\times P$得到大小为$M\times P$的矩阵，从而推断出$\mathbf{H}$所需的大小。即：

$$(2\times 1)=(a\times b)(4\times 1)=(2\times 4)(4\times 1)$$

所以，$\mathbf{H}$的尺寸是$2\times 4$。

填充$\mathbf{H}$值很容易，因为观测值是机器人的位置，即状态$\mathbf{x}$的$x$和$y$。让我们改变单位，让这个转换过程稍微有趣一点。观测的结果以英尺为单位，而状态量以米为单位。$\mathbf{H}$将状态量转换到观测空间，由于$feet=meters/0.3048$。这就产生了：

$$\mathbf{H}=\left[\begin{array}{cccc}\frac{1}{0.3048}& 0& 0& 0\\ 0& 0& \frac{1}{0.3048}& 0\end{array}\right]$$

对应于这些线性方程：

$$z_x=(\frac{x}{0.3048})+(0\ast v_x)+(0\ast y)+(0\ast v_y)=\frac{x}{0.3048}$$
$$z_y=(0\ast x)+(0\ast v_x)+(\frac{y}{0.3048})+(0\ast v_y)=\frac{y}{0.3048}$$

卡尔曼滤波器的方程意味着所有矩阵都有一个特定的维数，当你开始迷茫于如何设计一些东西时，看看矩阵维数是很有用的。

以下是我的实现：

tracker.H = np.array([[1/0.3048, 0, 0,        0],
                      [0,        0, 1/0.3048, 0]])

设计观测噪声矩阵

我们假设$x$和$y$是独立的白高斯过程。也就是说，$x$中的噪声与$y$中的噪声没有任何关系，并且噪声的平均值在0附近。现在让我们把$x$和$y$的方差设为5。它们是独立的，所以没有协方差，我们的对角线将是0。即：

$$\mathbf{R}=\left[\begin{array}{cc}{\sigma }_x^2& {\sigma }_{yx}\\ {\sigma }_{xy}& {\sigma }_y^2\end{array}\right]=\left[\begin{array}{cc}5& 0\\ 0& 5\end{array}\right]$$

它是一个$2\times 2$矩阵，因为我们有2个传感器输入，对于$n$个变量，协方差矩阵的大小总是$n\times n$。在Python中，可以表示为：

tracker.R = np.array([[5., 0],
                      [0, 5]])
tracker.R

array([[5., 0.],
       [0., 5.]])

初始条件

对于我们的问题，我们将初始位置设置为$(0,0)$，速度为$(0,0)$。因为这是纯粹的猜测，所以我们将初始协方差矩阵$\mathbf{P}$设置为一个大值。

$$\mathbf{x}=\left[\begin{array}{c}0\\ 0\\ 0\\ 0\end{array}\right],\mathbf{P}=\left[\begin{array}{cccc}500& 0& 0& 0\\ 0& 500& 0& 0\\ 0& 0& 500& 0\\ 0& 0& 0& 500\end{array}\right]$$

Python实现是：

tracker.x = np.array([[0, 0, 0, 0]]).T
tracker.P = np.eye(4) * 500.

滤波器实现

滤波器的设计完成了，现在我们只需要编写代码来运行即可。我们将代码运行30次迭代。

from filterpy.stats import plot_covariance_ellipse
from kf_book.book_plots import plot_filter

R_std = 0.35
Q_std = 0.04

def tracker1():
    tracker = KalmanFilter(dim_x=4, dim_z=2)
    dt = 1.0   # time step

    tracker.F = np.array([[1, dt, 0,  0],
                          [0,  1, 0,  0],
                          [0,  0, 1, dt],
                          [0,  0, 0,  1]])
    tracker.u = 0.
    tracker.H = np.array([[1/0.3048, 0, 0, 0],
                          [0, 0, 1/0.3048, 0]])

    tracker.R = np.eye(2) * R_std**2
    q = Q_discrete_white_noise(dim=2, dt=dt, var=Q_std**2)
    tracker.Q = block_diag(q, q)
    tracker.x = np.array([[0, 0, 0, 0]]).T
    tracker.P = np.eye(4) * 500.
    return tracker

# simulate robot movement
N = 30
sensor = PosSensor((0, 0), (2, .2), noise_std=R_std)

zs = np.array([sensor.read() for _ in range(N)])

# run filter
robot_tracker = tracker1()
mu, cov, _, _ = robot_tracker.batch_filter(zs)

for x, P in zip(mu, cov):
    # covariance of x and y
    cov = np.array([[P[0, 0], P[2, 0]], 
                    [P[0, 2], P[2, 2]]])
    mean = (x[0, 0], x[2, 0])
    plot_covariance_ellipse(mean, cov=cov, fc='g', std=3, alpha=0.5)
    
#plot results
zs *= .3048 # convert to meters
plot_filter(mu[:, 0], mu[:, 2])
plot_measurements(zs[:, 0], zs[:, 1])
plt.legend(loc=2)
plt.xlim(0, 20)

我鼓励你多尝试：把$\mathbf{Q}$和$\mathbf{R}$设置成不同的值。然而，我们在前几章中做了大量类似的事情，所以我将讨论其他更复杂的案例。在这些案例中，我们也将有机会体验这些值改变后的影响。

我用绿色绘制了$x$和$y$的$3\sigma$协方差椭圆。你能解释一下它们的形状吗？也许你在期待一个倾斜的椭圆，就像之前章节一样。但是回想一下，在那些章节中，我们不是在绘制$x$和$y$，而是在绘制$x$和$\dot{x}$。$x$和$\dot{x}$相关，但$x$与$y$无关。因此我们的椭圆是不倾斜的。

此外，$x$和$y$的噪声被建模为具有相同的噪声标准差。例如，如果我们把R设为：

$$\mathbf{R}=\left[\begin{array}{cc}1& 0\\ 0& 0.5\end{array}\right]$$

我们会告诉卡尔曼滤波器，$x$中的噪声比$y$中的要多，我们的椭圆的宽度会比它的高度长。

最终的$\mathbf{P}$值告诉我们，关于状态量之间的相关性信息。如果我们单独看对角线，我们会看到每个变量的方差。换句话说，${\mathbf{P}}_{0,0}$是$x$的方差，${\mathbf{P}}_{1,1}$的方差是$\dot{x}$的方差，${\mathbf{P}}_{2,2}$是$y$的方差，${\mathbf{P}}_{3,3}$是$\dot{y}$的方差。我们可以使用numpy.diag()提取矩阵的对角线：

print(np.diag(robot_tracker.P))

[0.007 0.003 0.007 0.003]

协方差矩阵包含四个$2\times 2$矩阵，你应该能够很容易地挑选出来。这是因为$x$与$\dot{x}$、$y$与$\dot{y}$的相关性。左上侧显示$x$与$\dot{x}$的协方差：

c = robot_tracker.P[0:2, 0:2]
print(c)
plot_covariance_ellipse((0, 0), cov=c, fc='g', alpha=0.2)

[[0.007 0.003]
 [0.003 0.003]]

最后，让我们看看$\mathbf{P}$的左下角，都是0。为什么是0？回想一下，${\mathbf{P}}_{i,j}$和${\mathbf{P}}_{j,i}$包含${\sigma }_{ij}$。考虑${\mathbf{P}}_{3,0}$表示${\sigma }_{ij}$，它是$\dot{y}$和$x$之间的协方差。它们是独立的，所以这个项是0。

robot_tracker.P[2:4, 0:2]

array([[0., 0.],
      [0., 0.]])

滤波器阶数

我们研究了跟踪位置和速度，它可以工作得很好。现在你已经对卡尔曼滤波有了足够的经验，可以用更一般的情形来考虑它。

我说的阶数指的什么？阶数就是精确地建模系统所需的最高阶导数项。如果考虑一个不变的系统，例如建筑物的高度。没有变化，所以不需要导数，系统的阶数为零。我们可以用公式表示为$x=312.5$。

一阶系统有一阶导数。例如，位置的变化就是速度，我们可以这样写：

$$v=\frac{dx}{dt}$$

我们把它积分到牛顿方程中：

$$x=vt+x_0$$

这也被称为等速模型，因为假设为等速。

二阶系统有一个二阶导数。位置的二阶导数是加速度：

$$a=\frac{d^{2}x}{d{t}^2}$$

我们可以得到：

$$x=\frac{1}{2}a{t}^2+v_{0}t+x_0$$

这也被称为恒加速度模型。

另一种方法，要想知道滤波器的阶数，直接看系统模型的多项式的阶数。恒加速度模型有一个二阶导数，所以它是二阶的。同样，多项式$x=\frac{1}{2}a{t}^2+v_{0}t+x_0$也是二阶。

在设计状态量和过程模型时，我们必须首先正确地选择要建模的系统的阶数。假设我们在以恒定的速度跟踪某个物体，现实中没有一个过程是真正完美的，因此在短时间内速度一定会有微小的变化。你可能会认为最好的方法是使用二阶滤波器，允许加速度项处理速度的微小变化。

实际上，这不太好。为了彻底了解这个问题，让我们看看使用与滤波器的阶数与系统模型不匹配的效果。

首先我们需要一个系统来滤波。我将编写一个类来模拟一个恒定速度的对象。但基本上没有物理系统具有真正恒定的速度，所以每次都会改变一小部分速度，于是我模拟了传感器中的高斯噪声。代码如下，我还绘制了一个运行示例，以验证它是否正常工作。

from kf_book.book_plots import plot_track

class ConstantVelocityObject(object):
    def __init__(self, x0=0, vel=1., noise_scale=0.06):
        self.x = x0
        self.vel = vel
        self.noise_scale = noise_scale

    def update(self):
        self.vel += randn() * self.noise_scale
        self.x += self.vel
        return (self.x, self.vel)

def sense(x, noise_scale=1.):
    return x[0] + randn()*noise_scale

np.random.seed(124)
obj = ConstantVelocityObject()

xs, zs = [], []
for i in range(50):
    x = obj.update()
    z = sense(x)
    xs.append(x)
    zs.append(z)

xs = np.asarray(xs)

plot_track(xs[:, 0])
plot_measurements(range(len(zs)), zs)
plt.legend(loc='best')

我对这个图的效果很满意。由于我们添加到系统中的噪音，轨迹并不完全笔直——这可能是一个人在街上行走的轨迹，也可能是一架飞机受到可变风的冲击的轨迹。这里没有有意的设置加速度，所以我们还称之为等速系统。你可能会问，既然速度发生了变化，事实上肯定会产生加速度，为什么我们不使用二阶卡尔曼滤波器呢？让我们来看看。

如何设计零阶、一阶或二阶卡尔曼滤波器？我们一直在这么做，只是不常使用这些术语，因为这可能有点乏味，但我将详细阐述每一种阶数的滤波器——如果概念清楚，你可以自由地略读一下。

零阶卡尔曼滤波

零阶卡尔曼滤波器就是一种不带导数项的滤波器。我们跟踪的是位置，所以我们只有一个位置的状态量（没有速度或加速度），状态转换函数也只有位置。状态量用矩阵形式表示：

$$\mathbf{x}=\left[\begin{array}{c}x\end{array}\right]$$

状态转移函数非常简单。位置没有变化，所以我们需要建模$x=x$；换句话说，时间$t+1$的$x$与时间$t$的$x$相同。状态转移函数的矩阵形式：

$$\mathbf{F}=\left[\begin{array}{c}1\end{array}\right]$$

观测方程也非常简单。回想一下，我们需要定义如何将状态量$\mathbf{x}$转换为观测值。假设我们的观测值是位置，状态量也只包含一个位置，所以我们得到：

$$\mathbf{H}=\left[\begin{array}{c}1\end{array}\right]$$

让我们编写一个函数，构造并返回一个零阶卡尔曼滤波器。

def ZeroOrderKF(R, Q, P=20):
    """ Create zero order Kalman filter.
    Specify R and Q as floats."""
    kf = KalmanFilter(dim_x=1, dim_z=1)
    kf.x = np.array([0.])
    kf.R *= R
    kf.Q *= Q
    kf.P *= P
    kf.F = np.eye(1)
    kf.H = np.eye(1)
    return kf

一阶卡尔曼滤波

一阶卡尔曼滤波器跟踪一阶系统，如位置和速度。一阶系统有位置和速度，因此状态量需要这两个。矩阵表示为：

$$\mathbf{x}=\left[\begin{array}{c}x\\ \dot{x}\end{array}\right]$$

所以现在我们必须设计我们的状态转移函数。步长时间下的牛顿方程为：

$$x_t=x_{t-1}+v\Delta t$$

$$v_t=v_{t-1}$$

回想一下，我们需要把它转换成矩阵表达：

$$\left[\begin{array}{c}x\\ \dot{x}\end{array}\right]=\mathbf{F}\left[\begin{array}{c}x\\ \dot{x}\end{array}\right]$$

其中，

$$\mathbf{F}=\left[\begin{array}{cc}1& \Delta t\\ 0& 1\end{array}\right]$$

最后，设计观测方程。观测方程需要实现：

$$\mathbf{z}=\mathbf{Hx}$$

我们的传感器仍然只读取位置，所以它应该从状态中获取位置，速度和加速度都是0，如下所示：

$$\mathbf{H}=\left[\begin{array}{cc}1& 0\end{array}\right]$$

让我们编写一个函数，构造并返回一个一阶卡尔曼滤波器。

def FirstOrderKF(R, Q, dt):
    """ Create first order Kalman filter. 
    Specify R and Q as floats."""
    kf = KalmanFilter(dim_x=2, dim_z=1)
    kf.x = np.zeros(2)
    kf.P *= np.array([[100, 0], [0, 1]])
    kf.R *= R
    kf.Q = Q_discrete_white_noise(2, dt, Q)
    kf.F = np.array([[1., dt],
                     [0., 1]])
    kf.H = np.array([[1., 0]])
    return kf

二阶卡尔曼滤波

二阶卡尔曼滤波器跟踪二阶系统，如位置、速度和加速度。状态量为：

$$\mathbf{x}=\left[\begin{array}{c}x\\ \dot{x}\\ \ddot{x}\end{array}\right]$$

所以现在我们必须设计我们的状态转移函数。步长时间的牛顿方程为：

$$x_t=x_{t-1}+v_{t-1}\Delta t+0.5a_{t-1}\Delta t^2$$
$$v_t=v_{t-1}+a_{t-1}\Delta t$$
$$a_t=a_{t-1}$$

回想一下，我们需要把它转换成矩阵形式：

$$\left[\begin{array}{c}x\\ \dot{x}\\ \ddot{x}\end{array}\right]=\mathbf{F}\left[\begin{array}{c}x\\ \dot{x}\\ \ddot{x}\end{array}\right]$$

其中，

$$\mathbf{F}=\left[\begin{array}{ccc}1& \Delta t& 0.5\Delta t^2\\ 0& 1& \Delta t\\ 0& 0& 1\end{array}\right]$$

最后，设计了观测方程。观测方程需要实现：

$$z=\mathbf{Hx}$$

我们的传感器仍然只读取位置，所以它应该从状态中获取位置，0表示速度，如下所示：

$$\mathbf{H}=\left[\begin{array}{ccc}1& 0& 0\end{array}\right]$$

让我们编写一个函数，构造并返回一个二阶卡尔曼滤波器。

def SecondOrderKF(R_std, Q, dt, P=100):
    """ Create second order Kalman filter. 
    Specify R and Q as floats."""
    kf = KalmanFilter(dim_x=3, dim_z=1)
    kf.x = np.zeros(3)
    kf.P[0, 0] = P
    kf.P[1, 1] = 1
    kf.P[2, 2] = 1
    kf.R *= R_std**2
    kf.Q = Q_discrete_white_noise(3, dt, Q)
    kf.F = np.array([[1., dt, .5*dt*dt],
                     [0., 1.,       dt],
                     [0., 0.,       1.]])
    kf.H = np.array([[1., 0., 0.]])
    return kf

评估滤波器的阶数

现在我们可以运行每个卡尔曼滤波器来模拟和评估结果。

我们如何评估结果？通过绘制轨迹和卡尔曼滤波输出，并对结果进行目视观察，可以定性地完成这一点。然而，如果需要严谨的结果，就应使用数学。请记住，系统协方差矩阵$\mathbf{P}$包含每个状态量的方差和相互间的协方差。如果噪声是高斯的，大约$99\%$的测量值都在$3\sigma$范围内。

因此，我们可以通过观察估计状态和实际状态之间的残差，并将它们与我们从$\mathbf{P}$得到的标准差进行比较，来评估滤波器。如果滤波器正确运行，99%的残差将在$3\sigma$范围内。这对于所有状态量都是正确的，而不仅仅是位置。

我必须提醒，这只适用于模拟系统。真实的传感器并不是完全高斯的，你可能需要扩展你的标准，比如说，$5\sigma$的真实传感器数据。

让我们对一阶系统运行一阶卡尔曼滤波器，看看它的性能。你可能猜到它会做得很好，但是让我们用标准差来看看。

首先，让我们编写一个例程来生成噪声观测值。

def simulate_system(Q, count):
    obj = ConstantVelocityObject(x0=.0, vel=0.5, noise_scale=Q)
    xs, zs = [], []
    for i in range(count):
        x = obj.update()
        z = sense(x)
        xs.append(x)
        zs.append(z)
    return np.array(xs), np.array(zs)

现在编写执行滤波的例子，并将输出保存在Saver对象中。

from filterpy.common import Saver

def filter_data(kf, zs):
    s = Saver(kf)
    kf.batch_filter(zs, saver=s)
    s.to_array()
    return s

现在我们准备运行滤波器并查看结果。

from kf_book.book_plots import plot_kf_output

R, Q = 1, 0.03
xs, zs = simulate_system(Q=Q, count=50)

kf = FirstOrderKF(R, Q, dt=1)
data1 = filter_data(kf, zs)

plot_kf_output(xs, data1.x, data1.z)

看起来滤波器的性能不错，但也很难说到底有多好。让我们看看残差，看看它们是否有用。我们会经常这样做，所以我会写一个函数来绘制它们。

from kf_book.book_plots import plot_residual_limits, set_labels

def plot_residuals(xs, data, col, title, y_label, stds=1):
    res = xs - data.x[:, col]
    plt.plot(res)
    plot_residual_limits(data.P[:, col, col], stds)
    set_labels(title, 'time (sec)', y_label)

plot_residuals(xs[:, 0], data1, 0, 
               title='First Order Position Residuals(1$\sigma$)',
               y_label='meters')  

我们如何解释这个结果？残差被画成锯齿线——观测和预测位置之间的差值。如果没有观测噪声，并且卡尔曼滤波的预测总是完美的，则残差总是零。所以理想的输出应该是0处的水平线。我们可以看到残差是以0为中心的，所以这让我们确信噪声是高斯噪声（因为误差在0上下相等）。虚线之间的黄色区域表示滤波器在1个标准偏差下的理论性能。换句话说，大约68%的误差应该落在虚线内。残差主要都在这个范围内，所以我们断定滤波器的表现良好，没有发散。

让我们再看看速度的残差。

plot_residuals(xs[:, 1], data1, 1, 
               title='First Order Velocity Residuals(1$\sigma$)',
               y_label='meters/sec')  

同样，正如预期的那样，速度的残差也在滤波器的理论性能范围内，因此我们确信滤波器是为这个系统设计的。

现在让我们用零阶卡尔曼滤波器做同样的事情。所有的代码在很大程度上是相同的，所以让我们只看结果，而不讨论太多的实现。

kf0 = ZeroOrderKF(R, Q)
data0 = filter_data(kf0, zs)
plot_kf_output(xs, data0.x, data0.z)

正如我们所料，滤波器有问题。在每个predict()步骤中，卡尔曼滤波器假设位置没有变化——如果当前位置为4.3，它将预测下一时间段的位置为4.3。当然，实际位置更接近5.3。有噪声的观测值可能是5.4，因此滤波器在4.3和5.4之间选择了一个估计部分，导致它显著滞后于5.3的实际值。同样的事情会发生在下一步、下一步等等。滤波器永远也赶不上。

现在让我们看看残差。我们不跟踪速度，所以我们只能看位置的残差。

plot_residuals(xs[:, 0], data0, 0, 
               title='Zero Order Position Residuals(3$\sigma$)',
               y_label='meters',
               stds=3)

我们可以看到滤波器几乎瞬间发散。几秒钟后，残差超过了三个标准差的界限。重要的是要理解：假设所有输入都正确，协方差矩阵$\mathbf{P}$仅仅能体现滤波器的理论性能。换句话说，这个卡尔曼滤波器是发散的，但是$\mathbf{P}$暗示着卡尔曼滤波器的估计随着时间的推移越来越好，因为方差越来越小。滤波器没有办法知道，关于系统模型，你在对它撒谎。

在这个系统中，发散是直接且明显的。在许多其他系统中，它只会是渐进的或轻微的。因此，对于你的系统来说，查看这样的图表非常重要，以确保滤波器的性能在其理论性能的范围内。

现在让我们试试二阶系统。你可能觉得这是件好事。毕竟，我们知道在模拟物体的运动中有一些噪音，这意味着有一些加速度。为什么不用二阶模型来模拟加速度呢？如果没有加速度，加速度应该估计为0。但事情就应该是这样吗？想一想再继续往下看。

kf2 = SecondOrderKF(R, Q, dt=1)
data2 = filter_data(kf2, zs)
plot_kf_output(xs, data2.x, data2.z)

这一切是否如你所料？

我们可以看到二阶滤波器的性能比一阶滤波器差。为什么？这个滤波器模拟加速度，因此观测中的巨大变化被解释为加速度而不是噪声。因此，滤波器密切跟踪噪声。不仅如此，如果噪声始终高于或低于轨迹，它还会在某些地方超过噪声，因为滤波器错误地假设了一个不存在的加速度，因此它的预测在每次观测中都离轨迹越来越远。这种情况不太好。

不过，轨迹并没有直接发散。让我们看看残差。二阶系统的残差看起来也还好，因为它们没有偏离或超过三个标准差。然而，将一阶滤波器和二阶滤波器的残差比较起来看，显然是更有说服力的。

res2 = xs[:, 0] - data2.x[:, 0]
res1 = xs[:, 0] - data1.x[:, 0]

plt.plot(res1, ls="--", label='order 1')
plt.plot(res2, label='order 2')
plot_residual_limits(data2.P[:, 0, 0])
set_labels('Second Order Position Residuals',
           'meters', 'time (sec)')
plt.legend()

二阶滤波器的位置残差比一阶滤波器的残差稍差，但仍在滤波器的理论极限之内。这里没有什么特别令人担忧的。

现在让我们来看看速度的残差。

res2 = xs[:, 1] - data2.x[:, 1]
res1 = xs[:, 1] - data1.x[:, 1]

plt.plot(res2, label='order 2')
plt.plot(res1, ls='--', label='order 1')
plot_residual_limits(data2.P[:, 1, 1])
set_labels('Second Order Velocity Residuals', 
                      'meters/sec', 'time (sec)')
plt.legend()

这里的情况完全不同。尽管二阶系统的残差也在滤波器性能的理论范围内，但是我们可以看到它远比一阶滤波器差。二阶滤波器假设了不存在的加速度，它将观测中的噪声误认为是加速度，并将其添加到每个预测周期的速度估计中。当然，加速度实际上并不存在，所以速度的残差远大于它的最佳值。

有一个trick的做法。假设我们有一个一阶系统，即速度或多或少是恒定的。现实世界中的系统从来都不是完美的，所以速度在不同的时间段之间也不完全相同。当我们使用一阶滤波器时，我们使用过程噪声$\mathbf{Q}$来表示速度的微小变化。如果我们使用一个二阶滤波器，我们再考虑速度的变化。此时我们就没有必要设置过程噪声了，让加速度去表示速度的微小变化，即把$\mathbf{Q}$设为零！

kf2 = SecondOrderKF(R, 0, dt=1)
data2 = filter_data(kf2, zs)
plot_kf_output(xs, data2.x, data2.z)

看起来滤波器迅速收敛到实际轨迹。成功！

也许并不成功。将过程噪声设置为0，即告诉滤波器过程模型是完美的。让我们看看滤波器在较长时间内的性能。

np.random.seed(25944)
xs500, zs500 = simulate_system(Q=Q, count=500)

kf2 = SecondOrderKF(R, 0, dt=1)
data500 = filter_data(kf2, zs500)

plot_kf_output(xs500, data500.x, data500.z)
plot_residuals(xs500[:, 0], data500, 0, 
               'Second Order Position Residuals',
               'meters') 

我们可以看出，滤波器的性能非常差。在轨迹图中，滤波器发散了很长一段时间。在残差图上问题更加明显，滤波器的性能与理论值相差很大。但是在整个过程中，滤波器输出的协方差却越来越小。因此不要相信滤波器的协方差矩阵，来告诉你滤波器是否运行良好！

为什么会这样？回想一下，如果我们将过程噪声设置为零，我们就告诉滤波器只使用过程模型。观测结果最终被忽略了。这样的系统模型并不完美，因此滤波器无法适应一些突变的行为。

也许只需要设置一个非常低的过程噪音？我们试试看：

np.random.seed(32594)
xs2000, zs2000 = simulate_system(Q=0.0001, count=2000)

kf2 = SecondOrderKF(R, 0, dt=1)
data2000 = filter_data(kf2, zs2000)

plot_kf_output(xs2000, data2000.x, data2000.z)
plot_residuals(xs2000[:, 0], data2000, 0, 
               'Second Order Position Residuals',
               'meters') 

残差图再次佐证了这个结论：轨迹看起来很好，但残差图显示滤波器在很长一段时间内发散。

你可能会争辩说，最后一个图对你的应用程序来说已经足够好了，也许是这样。然而，我警告你，发散的滤波器并不总能得到一个看起来还可以的结果。使用不同的数据集，或者使用性能不同的物理系统，你可能会得到一个越来越远离观测的滤波器。

另外，让我们从数据拟合的角度来考虑这个问题。假设我给你两点，告诉你在两点上画一条直线。

plt.scatter([1, 2], [1, 1], s=100, c='r')
plt.plot([0, 3], [1, 1])

直线是唯一可能且最优的答案。如果我给你更多的点，你可以用最小二乘拟合来找到最好的线，答案仍然是最小二乘意义上的最优。

但是假设我让你用一个高阶多项式来拟合这两点。这个问题有无数个答案。例如，无穷多的二阶抛物线通过这些点。当卡尔曼滤波器的阶数高于系统时，它也有无穷多的解可供选择。你的答案不仅仅是非最优的，而且它经常会出现发散。

为了获得最佳性能，你需要一个阶数与系统阶数匹配的滤波器。在许多情况下，这将是很容易做到的——如果你是设计一个卡尔曼滤波器读取一个冰柜温度计，零阶滤波器是正确的选择。但是如果我们追踪一辆车，我们应该使用几阶呢？当汽车以匀速直线行驶时，一阶滤波器工作得很好，但汽车会转向、加速和减速，在这种情况下，二阶滤波器的性能会更好。这就是之后的自适应滤波器一章中要解决的问题。在那里，我们将学习如何设计一个滤波器，以适应跟踪对象的阶不断变化的行为。

也就是说，低阶滤波器可以跟踪高阶系统，只要添加足够的过程噪声，并且保持离散化周期足够小（每秒100个样本通常是局部线性的）。结果可能不会是最佳的，但仍然是比较好的。在尝试自适应滤波器之前，我总是使用这个方法。让我们看一个加速的例子。首先是模拟。

class ConstantAccelerationObject(object):
    def __init__(self, x0=0, vel=1., acc=0.1, acc_noise=.1):
        self.x = x0
        self.vel = vel
        self.acc = acc
        self.acc_noise_scale = acc_noise
    
    def update(self):
        self.acc += randn() * self.acc_noise_scale       
        self.vel += self.acc
        self.x += self.vel
        return (self.x, self.vel, self.acc)
  
R, Q = 6., 0.02
def simulate_acc_system(R, Q, count):
    obj = ConstantAccelerationObject(acc_noise=Q)
    zs = []
    xs = []
    for i in range(count):
        x = obj.update()
        z = sense(x, R)
        xs.append(x)
        zs.append(z)
    return np.asarray(xs), zs

np.random.seed(124)
xs, zs = simulate_acc_system(R=R, Q=Q, count=80)
plt.plot(xs[:, 0])

现在我们将使用二阶滤波器来滤波数据：

np.random.seed(124)
xs, zs = simulate_acc_system(R=R, Q=Q, count=80)

kf2 = SecondOrderKF(R, Q, dt=1)
data2 = filter_data(kf2, zs)

plot_kf_output(xs, data2.x, data2.z, aspect_equal=False)
plot_residuals(xs[:, 0], data2, 0, 
               'Second Order Position Residuals',
               'meters') 

我们可以看到，滤波器的性能在滤波器的理论极限范围内。

现在让我们使用一个低阶过滤器。低阶滤波器将会产生滞后，因为它并不模拟加速度。然而，我们可以通过增加过程噪声的大小（在一定程度上）来解决这一点。该滤波器将加速度视为过程模型中的噪声。结果将是次优的，但如果设计良好，也不会出现发散。增加额外的过程噪声，不是一门精确的科学。你必须用有代表性的数据进行实验。这里，我把它乘以10，得到了不错的结果。

kf3 = FirstOrderKF(R, Q * 10, dt=1)
data3= filter_data(kf3, zs)

plot_kf_output(xs, data3.x, data3.z, aspect_equal=False)
plot_residuals(xs[:, 0], data3, 0, 
               'First Order Position Residuals',
               'meters') 

想一想如果你使过程噪音比需要的大很多倍会发生什么。较大的过程噪声告诉滤波器更相信观测，因此我们希望滤波器能够在观测中紧密地模拟噪声。让我们看看：

kf4 = FirstOrderKF(R, Q * 10000, dt=1)
data4 = filter_data(kf4, zs)

plot_kf_output(xs, data4.x, data4.z, aspect_equal=False)
plot_residuals(xs[:, 0], data4, 0, 
               'First Order Position Residuals',
               'meters') 

检测和拒绝不良观测

卡尔曼滤波器本身无法检测和拒绝不良观测。假设你正在跟踪飞机，并且在距离飞机当前位置100公里处收到观测结果。如果使用该值调用update，则新的估计值将在估计值和观测值中按比例产生。

我将运行一个例子以表示异常的观测对滤波器的影响：在100个时间步之后，我将使用等于当前位置两倍的观测值进行更新。

from filterpy.common import kinematic_kf

kf = kinematic_kf(dim=2, order=1, dt=1.0, order_by_dim=False)
kf.Q = np.diag([0, 0, .003, .003])
kf.x = np.array([[1., 1., 0., 0.]]).T
kf.R = np.diag([0.03, 0.21]) # use different errors

for i in range(101):
    kf.predict()
    kf.update(np.array([[i*.05, i*.05]])) # around 200 kph

p0 = kf.x[0:2]

kf.predict()
prior = kf.x
z = kf.x[0:2]*2
kf.update(z)
p1 = kf.x[0:2]

# compute error of measurement from prior
y = np.abs(z - kf.H @ prior)
dist = np.linalg.norm(y)

np.set_printoptions(precision=2, suppress=True)

print(f'bad measurement       : {
      z.T} km')
print(f'before bad measurement: {
      p0.T} km')
print(f'after bad measurement : {
      p1.T} km')
print(f'estimate shift        : {
      np.linalg.norm(p1 - prior[:2]):.1f} km')
print(f'distance from prior   : {
      dist:.1f} km')

bad measurement       : [[10.1 10.1]] km
before bad measurement: [[5. 5.]] km
after bad measurement : [[7.84 7.01]] km
estimate shift        : 3.4 km
distance from prior   : 7.1 km

kinematic_kf？那是什么？filterpy.common提供了kinematic_kf ，可创建任意阶数的线性运动学滤波器。我将在这里使用它来保持代码整洁，我不会在其它部分使用它，因为我希望你能够大量练习滤波器的编写。

回到话题上来。如你所见，估计值跳了$3.4km$，预测值（先验值）和观测值之间的误差超过7公里。

我们能做些什么来避免这种情况？我们的第一个想法可能是添加一个检查，检查先验值与观测值是否相差很远。为什么是先验值而不是当前的估计值？因为在更新之后，估计值现在可能非常接近坏的观测值，尽管在本例中不是这样。

请注意，虽然我可以通过prior[0:2] - z来计算残差，但我使用的是$\mathbf{z}-\mathbf{Hx}$。这只是为了举例说明：Kalman filter类存储了KalmanFilter.y的更新结果。我使用它而不是上面计算的值来说明这一点：

print(f'error = {
      np.linalg.norm(kf.y):.1f} km, at a speed of {
      dist*3600:.0f} kph')

error = 7.1 km, at a speed of 25710 kph

在本例中，观测值与预测值之间相差近$7km$。听起来比较远。如果单位为公里，更新频率为1秒，那么确实比较远；这一误差意味着任何飞机的飞行速度都不能超过25000公里/小时。如果单位是厘米，更新频率是1分钟，那么误差就小得离谱。

我们可以添加一项检查，将飞机的性能限制考虑在内：

vel = y / dt
if vel >= MIN_AC_VELOCITY and vel <= MAX_AC_VELOCITY:
    kf.update()

你认为这是一个合理且可靠的解决方案吗？在进一步阅读之前，提出尽可能多的意见。

这对我来说不是很满意。假设我们刚刚用猜测的位置初始化了滤波器，并使用这项检查，可能会导致我们直接丢弃好的观测值，永远不会开始滤波。其次，这忽略了我们对传感器和过程误差的认知。卡尔曼滤波器定义了其当前精度为$\mathbf{P}$。如果$\mathbf{P}$表示了${\sigma }_x=10m$，而观测距离为$1km$，显然观测不好，因为它与之前的观测值有100个标准偏差。

我们来画$\mathbf{P}$，我来画第一、第二和第三个标准差：

x, P = kf.x[0:2], kf.P[0:2, 0:2]
plot_covariance_ellipse(x, P, std=[1,2,3])

可以看出，$\mathbf{P}$是椭圆。这是因为代码中，我设置了$\mathbf{R}=\left[\begin{array}{cc}0.003& 0\\ 0& 0.15\end{array}\right]$，它表示$y$观测值的误差是$x$的5倍。

想想这意味着什么。统计数据告诉我们，$99\%$的观测值都在3个标准差之内。这意味着$99\%$的观测值应该在这个椭圆内。让我们用椭圆绘制观测值：

plot_covariance_ellipse(x, P, std=[1,2,3])
plt.scatter(z[0], z[1], marker='x')

显然，观测远远超出协方差椭圆。我们可能会认为这是一个坏的观测，而不是使用它。我们应该怎么做？

第一个想法是提取$x$和$y$的标准偏差，并编写一个简单的if语句。这里我将使用KalmanFilter类的另一个特性。residual_of方法计算与先验值之间的残差。我不需要在这种情况下使用它，因为kf.y在调用update()时已经被赋值了。但是如果我们放弃观测值，则不会调用update()，kf.y只会包含前一个循环的值。

首先，让我们介绍一个术语——门禁。门禁是用于确定观测是好是坏的公式或算法，只有好的观测才能通过门禁。

在实践中，观测值不是纯粹的高斯分布，因此3个标准偏差的门禁可能会丢弃一些好的观测值。我将很快详细说明，因此现在我们将使用4个标准差。

GATE_LIMIT = 4.
std_x = np.sqrt(P[0,0])
std_y = np.sqrt(P[1,1])
y = kf.residual_of(z)[:,0]

if y[0] > GATE_LIMIT * std_x or y[1] > GATE_LIMIT * std_y:
    print(f'discarding measurement, error is {
      y[0]/std_x:.0f} std, {
      y[1]/std_y:.0f} std')

print('y   is', y)
print(f'std is {
      std_x:.2f} {
      std_y:.2f}')

discarding measurement, error is 39 std, 18 std
y   is [5.05 5.05]
std is 0.13 0.29

我们看到误差大约是39和18个标准差。这够好吗？

但是，请注意，if语句在椭圆周围形成一个矩形区域。在下面的图中，我画了一个明显在$3\sigma$椭圆之外的观测值，但却依然会被门禁接受，另一个观测值正好位于$3\sigma$椭圆边界上。

plot_covariance_ellipse(x, P, std=[1,2,3])
plt.scatter(8.08, 7.7, marker='x')
plt.scatter(8.2, 7.65, marker='x')

还有其他方法可以定义此门禁。马氏距离是点到分布的距离的统计度量。在我们开始定义和数学之前，让我们计算一些点的马氏距离。filterpy.stats实现了mahalanobis()。

from filterpy.stats import mahalanobis

m = mahalanobis(x=z, mean=x, cov=P)
print(f'mahalanobis distance = {
      m:.1f}')

mahalanobis distance = 20.6

在不知道单位的情况下，我们可以将其与$x$和$y$的标准偏差误差（39和18）进行比较，并确定其相当接近。让我们看看我上面画的点得到了什么。

print(f'mahalanobis distance = {
      mahalanobis(x=[8.08, 7.7], mean=x, cov=P):.1f}')
print(f'mahalanobis distance = {
      mahalanobis(x=[8.2, 7.65], mean=x, cov=P):.1f}')

mahalanobis distance = 3.0
mahalanobis distance = 3.6

正如我们将看到的，马氏距离计算点到分布的标量标准偏差距离，就像欧几里得距离计算点到另一点的标量距离一样。

上面的过程证明了这一点。位于$3\sigma$椭圆边界上的点的马氏距离为3.0，$3\sigma$椭圆外的点的值为3.6。

我们如何计算马氏距离？它被定义为：

$$D_m=\sqrt{(\mathbf{x}-\mu {)}^T{\mathbf{S}}^{-1}(\mathbf{x}-\mu )}$$

请注意，这与欧几里德距离非常相似，我们可以将其写成：

$$D_e=\sqrt{(\mathbf{x}-\mathbf{y}{)}^T(\mathbf{x}-\mathbf{y})}$$

事实上，如果协方差矩阵$\mathbf{S}$是单位矩阵，则马氏距离与欧几里德距离相同。凭直觉思考：如果每个维度的标准偏差为1，则半径为1的圆上围绕平均值的任意点，将位于$1\sigma$圆上，并且在欧几里德度量中距离为1单位。

这意味着另一种解释。如果协方差矩阵是对角的，那么我们可以将马氏距离视为缩放的欧几里德距离，其中每个项都由对角的协方差缩放：

$$D_m=\sqrt{\sum _{i-1}^N\frac{(x_i-{\mu }_i{)}^2}{{\sigma }_i}}$$

在二维空间就是：

$$D_m=\sqrt{\frac{1}{{\sigma }_x^2}(x_0-x_1{)}^2+\frac{1}{{\sigma }_y^2}(y_0-y_1{)}^2}$$

这将使你更深入了解马氏距离的方程式。你不能被矩阵除法，但用逆矩阵相乘实际上是一样的。乘以每条边的残差$\mathbf{y}=\mathbf{x}-\mu$，给出了协方差标度的平方范数：${\mathbf{y}}^T{\mathbf{S}}^{-1}{\mathbf{y}}^T$。协方差项都是平方的，所以在末尾取平方根，我们得到了一个标量距离，它是由协方差缩放的欧几里德距离。

由于其形状，上述两个门禁在一些文献中称为矩形门和椭圆形门。还有很多其他种类的门禁，这里我不做探讨。例如，机动门用于定义目标可能机动的区域，同时考虑目标的当前速度和机动能力。

你应该使用哪种门禁？没有统一的答案。这在很大程度上取决于问题的维度和计算能力。

矩形门的计算成本很低，机动门的计算成本也不高，但椭圆形门在高维度上的计算成本可能会较昂贵。但是，随着维度的增加，椭圆形门禁和矩形门禁之间的相对面积差会显著增加。

这可能比你意识到的更重要。由于每次观测都有噪音，一个虚假的观测值可能会通过我们的门禁，导致我们接受它。通过椭圆形的面积越大，门禁通过错误观测的概率越大。我不会在这里做数学计算，但在五维中，矩形门接受错误观测的可能性是椭球形的两倍。

如果计算时间是一个问题，并且你有许多的虚假观测，你可以采取双门禁的方法。第一道门禁是大且矩形的，以丢弃明显不好的观测值。通过该门的少数观测值随后要进行更加耗时的马氏距离计算。如果你运行在现代桌面处理器上，这些矩阵乘法的时间并不重要；但如果你运行在具有适度浮点性能的嵌入式芯片上，这可能很重要。

回到我们简单的问题本身——跟踪某个对象时偶尔会出现错误观测。应该如何规避？这相当简单：如果观测结果不正确，就放弃它，不要调用update()。这将导致你连续调用predict()多次，这很好。你的不确定性会增加，但一些遗漏的update()通常不会导致问题。

你的门禁应该使用什么门禁值？我不知道。理论上说是$3\sigma$，但实践上却并不是这样的。你需要进行实验：收集数据，运行滤波器并在其上使用各种门禁，最后查看哪个值可以提供最佳的结果。在下面中，我将为你介绍一些评估滤波器性能的数学方法。也许你会发现你需要接受所有小于$4.5\sigma$的观测值。我看过一段NASA的视频，他们说他们使用了$5\sim 6\sigma$的门禁。这取决于你的问题和数据。

评价滤波器性能

为模拟的数据设计卡尔曼滤波器很容易。你知道在过程模型中添加了多少噪声，因此你指定具有相同值的$\mathbf{Q}$。你还知道模拟的观测中添加了多少噪声，因此观测噪声矩阵$\mathbf{R}$的定义同样简单。

但是，真正的传感器很少按照某个规范执行，也很少以高斯方式执行。他们也很容易被环境噪音所干扰，例如：电路噪声会导致电压波动，从而影响传感器的输出。这导致设计过程模型和噪声更加困难。对汽车进行建模也是很困难，例如：车辆转向导致的非线性行为，刹车和加速的力度可能会导致轮胎打滑，大风导致汽车偏离路线。最终的结果就是卡尔曼滤波器中系统的不精确模型。这种不精确性会导致次优行为，在最坏的情况下会导致滤波器完全发散。

由于未知因素的存在，你将无法得到滤波器状态量的正确值，因此需要针对各种模拟和真实数据测试你的滤波器。你对滤波器效果的评估将，指导你对矩阵进行的更改。我们已经做了一些，例如，我向你们展示了$\mathbf{Q}$太大或太小的影响。

现在让我们考虑更多分析性能的方法。如果卡尔曼滤波器以最佳方式执行，其估计误差（真实状态和估计状态之间的差异）将具有以下特性：

1. 估计误差的平均值为零；
2. 估计误差的协方差由卡尔曼滤波器的协方差矩阵描述；

归一化估计误差平方（NEES）

第一种方法是最强大的，但只能在模拟中使用。如果你在模拟一个系统，你知道它的真实状态。然后，计算系统在每一步的估计误差（真值$\mathbf{x}$和滤波器的估计状态$\hat{\mathbf{x}}$）是很简单的：

$$\tilde{\mathbf{x}}=\mathbf{x}-\hat{\mathbf{x}}$$

然后，我们可以将归一化估计误差平方（NEES）定义为：

$$ϵ={\tilde{\mathbf{x}}}^T{\mathbf{P}}^{-1}\tilde{\mathbf{x}}$$

为了理解这个公式，让我们稍微分析一下它。如果状态量的维数是1，在这种情况下，$\mathbf{x}$和$\mathbf{P}$都是标量，所以：

$$ϵ=\frac{x^2}{P}$$

如果看不懂得话，请回想一下，如果$a$是标量，$a^T=a$，$a^{-1}=\frac{1}{a}$。

因此，当协方差矩阵变小时，对于相同的误差，NEES变大。协方差矩阵是滤波器对其误差的估计。因此，如果它相对于估计误差较小，则其性能比它相对于相同估计误差较大时更差。

这个计算给了我们一个标量结果：如果$\mathbf{x}$是维数$n\times 1$，那么计算的维数是$(1\times n)(n\times n)(n\times 1)=(1\times 1)$。我们拿到这个值有什么用呢？

数学证明不在本书的范围之内，但形式上是${\tilde{\mathbf{x}}}^T{\mathbf{P}}^{-1}\tilde{\mathbf{x}}$的一个随机变量，服从$n$个自由度的卡方分布，因此其期望值应为$n$。

简单地说，取所有NEES值的平均值，它们应该小于$\mathbf{x}$的维数。让我们使用本章前面的示例：

from scipy.linalg import inv

def NEES(xs, est_xs, Ps):
    est_err = xs - est_xs
    err = []
    for x, p in zip(est_err, Ps):
        err.append(x.T @ inv(p) @ x)
    return err

R, Q = 6., 0.02
xs, zs = simulate_acc_system(R=R, Q=Q, count=80)
kf2 = SecondOrderKF(R, Q, dt=1)
est_xs, ps, _, _ = kf2.batch_filter(zs)

nees = NEES (xs, est_xs, ps)
eps = np.mean(nees)

print(f'mean NEES is: {
      eps:.4f}')
if eps < kf2.dim_x:
    print('passed')
else:
    print('failed')

mean NEES is: 0.8893
passed

NEES在FilterPy中已经有了实现，可以通过：

from filterpy.stats import NEES

这是一个对滤波器性能很好的度量，应该尽可能地使用，特别是当你需要评估一个正在运行的滤波器。在设计滤波器时，我仍然喜欢绘制残差，因为它让我更直观地了解正在发生的事情。

但是，如果你的模拟逼真度有限，则需要使用另一种方法。

似然函数

在统计学中，似然与概率非常相似，但有一个对我们很重要的细微的差别。概率是指某件事情发生的概率，比如一个公平的骰子投掷六次出现三次五点的概率是多少？似然提出了相反的问题——假设一个骰子投掷六次出现三次五点的概率，那么骰子公平的可能性是多少？

我们在离散贝叶斯一章中首先讨论了似然函数。在这些滤波器中，似然是对给定当前状态的观测可能性的度量。

这对我们来说很重要，因为我们有滤波器输出，我们想知道在假设高斯噪声和线性行为的情况下，滤波器性能最佳的可能性。如果可能性很低，我们知道我们的假设是错误的。在自适应滤波章节中，我们将学习如何利用这些信息改进滤波器；在这里，我们将只学习如何进行对观测处理。

滤波器的残差和系统不确定度定义为：

$$\mathbf{y}=\mathbf{x}-\mathbf{H}\bar{\mathbf{x}}$$

$${\mathbf{S}=\mathbf{H}\bar{\mathbf{P}}\mathbf{H}}^T+\mathbf{R}$$

考虑到这些，我们可以这样计算似然：

$$\mathcal{L}=\frac{1}{\sqrt{2\pi S}}exp[-\frac{1}{2}{\mathbf{y}}^T{\mathbf{S}}^{-1}\mathbf{y}]$$

这看起来可能很复杂，但请注意，指数是高斯分布的方程。这意味着可以这样实现：

from scipy.stats import multivariate_normal

hx = (H @ x).flatten()
S = H @ P @ H.T  + R
likelihood = multivariate_normal.pdf(z.flatten(), mean=hx, cov=S)

实际情况下，情况稍有点不同。似然很难用数学方法处理，通常计算并使用对数似然，也就是似然的自然对数。这有几个好处：首先，$log$严格递增，并在应用它的函数的同一点达到其最大值；其次，如果你想求一个函数的最大值，你通常要求它的导数。找一些任意函数的导数可能很困难，但是$\frac{d}{dx}log(f(x))$是简单不少的。在对滤波器执行分析时，它是必不可少的。

调用update()时，将为你计算似然和对数似然，并可通过log_likelihood和likelihood数据属性进行访问。让我们看一下：我将使用多个在预期范围内的观测值运行滤波器，然后再输入超出预期范围的观测值：

R, Q = .05, 0.02
xs, zs = simulate_acc_system(R=R, Q=Q, count=50)
zs[-5:-1] = [100, 200, 200, 200] # bad measurements, bad!

kf = SecondOrderKF(R, Q, dt=1, P=1)
s = Saver(kf)
kf.batch_filter(zs, saver=s)
plt.plot(s.likelihood)

随着滤波器在前几次循环中的收敛，似然变得较大。在这之后，滤波器达到坏的观测值，此时似然变为零，这表明如果观测值有效，则滤波器不太可能是最优的。

看看对数似然性是如何清楚地说明滤波器坏的地方：

plt.plot(s.log_likelihood)

为什么它最终会回到零？通过将状态移到接近观测值的位置，滤波器开始适应新的观测值。残差变小，因此状态和残差一致。

传感器融合

在滤波的过程中，我们不应该丢弃任何信息。因此，当我们有两个传感器观测系统。我们应该如何将其纳入我们的卡尔曼滤波器？

假设我们在铁路上有一辆火车，它在车轮上安装了一个传感器，用于计算转数，我们可将转数转换为沿轨道行驶的距离。然后，假设我们有一个类似GPS的传感器，我称之为位置传感器，它安装在列车上，用于确定位置。因此，我们有两个观测值，都可以得到火车在轨道上的位置。进一步假设车轮传感器的精度为$1m$，位置传感器的精度为$10m$。我们如何将这两个观测值组合成一个滤波器？这看起来很麻烦，但飞机就是使用传感器融合的技术来融合来自GPS、INS、多普勒雷达、VOR、空速指示器等传感器的观测值。

假设我们的卡尔曼滤波器状态量应包含列车的位置和速度，即：

$$\mathbf{x}=\left[\begin{array}{c}x\\ \dot{x}\end{array}\right]$$

我们有两个位置的观测，因此我们将观测向量定义为车轮和位置传感器的观测向量：

$$\mathbf{z}=\left[\begin{array}{c}{z}_{wheel}\\ z_{ps}\end{array}\right]$$

所以我们必须设计矩阵$\mathbf{H}$来将$\mathbf{x}$转换成$\mathbf{z}$。它们都是位置，因此转换矩阵比较简单：

$$\left[\begin{array}{c}{z}_{wheel}\\ z_{ps}\end{array}\right]=\left[\begin{array}{cc}1& 0\\ 1& 0\end{array}\right]\left[\begin{array}{c}x\\ \dot{x}\end{array}\right]$$

为了更清楚，假设车轮传感器得到的的不是位置，而是车轮的旋转次数，其中1转产生2米的行程。如果是那样的话，我们就写成：

$$\left[\begin{array}{c}{z}_{rot}\\ z_{ps}\end{array}\right]=\left[\begin{array}{cc}0.5& 0\\ 1& 0\end{array}\right]\left[\begin{array}{c}x\\ \dot{x}\end{array}\right]$$

现在我们必须设计观测噪声矩阵$\mathbf{R}$。假设位置的观测方差是车轮方差的两倍，车轮的标准偏差为1.5米。于是有：

$${\sigma }_{wheel}=1.5$$

$${\sigma }_{wheel}^2=2.25$$

$${\sigma }_{ps}=1.5\times 2=3$$

$${\sigma }_{ps}^2=9$$

这就是我们的卡尔曼滤波器设计。我们需要为$\mathbf{Q}$设计，但这对我们是否进行传感器融合是没有什么影响的，所以我将选择一些任意值。

让我们来模拟一下这个滤波器设计。我将假设速度为$10m/s$，更新频率为$0.1s$。

from numpy import array, asarray
import numpy.random as random

def fusion_test(wheel_sigma, ps_sigma, do_plot=True):
    dt = 0.1
    kf = KalmanFilter(dim_x=2, dim_z=2)

    kf.F = array([[1., dt], [0., 1.]])
    kf.H = array([[1., 0.], [1., 0.]])
    kf.x = array([[0.], [1.]])
    kf.Q *= array([[(dt**3)/3, (dt**2)/2],
                   [(dt**2)/2,  dt      ]]) * 0.02
    kf.P *= 100
    kf.R[0, 0] = wheel_sigma**2
    kf.R[1, 1] = ps_sigma**2 
    s = Saver(kf)

    random.seed(1123)
    for i in range(1, 100):
        m0 = i + randn()*wheel_sigma
        m1 = i + randn()*ps_sigma
        kf.predict()
        kf.update(array([[m0], [m1]]))
        s.save()
    s.to_array()
    print(f'fusion std: {
      np.std(s.y[:, 0]):.3f}')
    if do_plot:
        ts = np.arange(0.1, 10, .1)
        plot_measurements(ts, s.z[:, 0], label='Wheel')
        plt.plot(ts, s.z[:, 1], ls='--', label='Pos Sensor')
        plot_filter(ts, s.x[:, 0], label='Kalman filter')
        plt.legend(loc=4)
        plt.ylim(0, 100)
        set_labels(x='time (sec)', y='meters')

fusion_test(1.5, 3.0)

fusion std: 1.647

我们可以看到蓝色的卡尔曼滤波结果。

从直观的角度理解前面的示例可能有点困难。让我们看看另一个问题。假设我们在二维空间中跟踪一个物体，在不同的位置有两个雷达系统。每个雷达系统都给我们提供了目标的距离和方位。每个数据的读数如何影响结果？

这是一个非线性问题，因为我们需要使用三角函数来从距离和方向中计算出坐标，我们还没有学会如何用卡尔曼滤波器解决非线性问题。因此，对于这个问题，忽略我使用的代码，只关注代码输出的图表。我们将在后续章节中重新讨论这个问题，并学习如何编写此代码。

我将目标定位在$(100,100)$。第一台雷达的位置为$(50,50)$，第二台雷达的位置为$(150,50)$。这将导致第一台雷达观测得到$45^{\circ }$的方位角，第二台雷达观测得到$135^{\circ }$的方位角。

我将首先创建卡尔曼滤波器，然后绘制其初始协方差矩阵。我使用的是一个无迹卡尔曼滤波器，这将在后面的章节中介绍。

from kf_book.kf_design_internal import sensor_fusion_kf

kf = sensor_fusion_kf()
x0, p0 = kf.x.copy(), kf.P.copy()
plot_covariance_ellipse(x0, p0, fc='y', ec=None, alpha=0.6)

我们同样不确定$x$和$y$的位置，所以协方差是圆形的。

现在我们将用第一台雷达的读数来更新卡尔曼滤波器。我将方向误差的标准偏差设置为$0.5^{\circ }$, 距离误差的标准偏差为$3$。

from math import radians
from kf_book.kf_design_internal import sensor_fusion_kf, set_radar_pos

# set the error of the radar's bearing and distance
kf.R[0, 0] = radians (.5)**2
kf.R[1, 1] = 3.**2

# compute position and covariance from first radar station
set_radar_pos((50, 50))
dist = (50**2 + 50**2) ** 0.5
kf.predict()
kf.update([radians(45), dist])

# plot the results
x1, p1 = kf.x.copy(), kf.P.copy()

plot_covariance_ellipse(x0, p0, fc='y', ec=None, alpha=0.6)
plot_covariance_ellipse(x1, p1, fc='g', ec='k', alpha=0.6)

plt.scatter([100], [100], c='y', label='Initial')
plt.scatter([100], [100], c='g', label='1st station')
plt.legend(scatterpoints=1, markerscale=3)
plt.plot([92, 100], [92, 100], c='g', lw=2, ls='--')

我们可以看到误差对协方差椭圆形状的影响。第一个雷达的位置在目标的左下角，方向观测$\sigma =0.5^{\circ }$非常精确，但距离观测$\sigma =3$不太精确。我已经用绿色虚线显示了雷达的读数。我们可以很容易地看到协方差椭圆形状中精确方位和不精确距离的影响。

现在我们可以融合第二个雷达的观测。第二个雷达位于$(150,50)$，位于目标右下角。在你继续之前，想一想当我们融合这个新的观测时，协方差会发生怎样的变化。

# compute position and covariance from second radar station
set_radar_pos((150, 50))
kf.predict()
kf.update([radians(135), dist])

plot_covariance_ellipse(x0, p0, fc='y', ec='k', alpha=0.6)
plot_covariance_ellipse(x1, p1, fc='g', ec='k', alpha=0.6)
plot_covariance_ellipse(kf.x, kf.P, fc='b', ec='k', alpha=0.6)

plt.scatter([100], [100], c='y', label='Initial')
plt.scatter([100], [100], c='g', label='1st station')
plt.scatter([100], [100], c='b', label='2nd station')
plt.legend(scatterpoints=1, markerscale=3)
plt.plot([92, 100], [92, 100], c='g', lw=2, ls='--')
plt.plot([108, 100], [92, 100], c='b', lw=2, ls='--')

我们可以看到第二个雷达的观测是如何改变协方差的。第二个雷达测得的方向与第一个雷达正交，因此方向和距离误差的影响被交换。因此，协方差矩阵的角度将切换到，与第二个雷达的方向相匹配。值得注意的是，方向不仅仅是改变，协方差矩阵的大小也变得更小。

协方差将始终包含所有可用信息，这个方法使得观察正在发生的事情变得特别容易。需要注意：如果一个传感器给你位置，另一个传感器给你速度，或者如果两个传感器提供位置观测，都会发生同样的事情。

在我们继续之前，最后一件事是：传感器融合是一个广泛的话题，我的介绍过于简单，以至于有误导性。例如，GPS使用迭代最小二乘法从卫星的一组伪距读数中确定位置，而不使用卡尔曼滤波器。当然，这只是GPS确定位置最常见的，但并非唯一的方式。

练习：你能滤波GPS的输出吗？

上文中，我们将卡尔曼滤波器应用于类似GPS的传感器，这可能就是某个商用GPS的方案。那能将卡尔曼滤波器应用于商业GPS的输出吗？换句话说，这个新的滤波器的输出会比商用GPS的输出更好、更差或相等吗？

解决方案

商用GPS内置了一个卡尔曼滤波器，其输出是该滤波器产生的滤波估计值。因此，假设该商用GPS有稳定的输出流，包括位置和位置误差。你能不能将这两个数据流传递到新的的卡尔曼滤波器中吗？

那么，这两个数据流的特征是什么，更重要的是，卡尔曼滤波器输入的基本要求是什么？

卡尔曼滤波器的输入必须是高斯的且与时间无关的。这是因为我们加入了马尔可夫属性的要求：当前状态仅依赖于先前状态和当前输入。这使得滤波器的递归形式成为可能。商用GPS的输出取决于时间，因为滤波器将其当前估计，基于所有先前观测的递归估计。因此，它不是与时间无关的，如果将数据传递到卡尔曼滤波器，则违反了滤波器的数学要求。因此，答案是否定的，通过对商业GPS的输出流运行KF无法获得更好的估计。

另一种看法是，卡尔曼滤波器在最小二乘意义上是最优的。没有办法将一个最优的结果，通过一个卡尔曼滤波器，得到一个更最优的答案，因为这在逻辑上是不可能的。充其量，结果将保持不变，在这种情况下，它仍然是最优的，或者它将被改变，因此不再是最优的。

这是业余爱好者在尝试集成GPS、IMU和其他现成传感器时面临的一个难题。

让我们看看效果。商用GPS观测得到位置和位置误差，位置误差仅来自卡尔曼滤波器的$\mathbf{P}$矩阵。因此，让我们将已经滤波了的输出作为新的有噪声输入传输给滤波器，然后看看结果是什么。换句话说，原本的$\mathbf{x}$将作为$\mathbf{z}$输入，原本的$\mathbf{P}$将作为$\mathbf{R}$。为了方便描述，我们暂把一个这样的过程叫做一次迭代。为了稍微夸大效果使其更明显，我将进行一次迭代，然后再进行一次迭代。第二次迭代没有任何意义（没有人会尝试），它只是帮助我说明。首先，编写代码和绘图。

np.random.seed(124)
R = 5.
xs, zs = simulate_acc_system(R=R, Q=Q, count=30)

kf0 = SecondOrderKF(R, Q, dt=1)
kf1 = SecondOrderKF(R, Q, dt=1)
kf2 = SecondOrderKF(R, Q, dt=1)

# Filter measurements
fxs0, ps0, _, _ = kf0.batch_filter(zs)

# filter twice more, using the state as the input
fxs1, ps1, _, _ = kf1.batch_filter(fxs0[:, 0])
fxs2, _, _, _ = kf2.batch_filter(fxs1[:, 0])

plot_kf_output(xs, fxs0, zs, 'KF', False)
plot_kf_output(xs, fxs1, zs, '1 iteration', False)
plot_kf_output(xs, fxs2, zs, '2 iterations', False)
R,Q

(5.0, 0.02)

我们看到，经过多次滤波后的输出更平滑，但它也偏离了轨迹。发生了什么事？回想一下，卡尔曼滤波器要求输入不具有时间相关性。然而，卡尔曼滤波器的输出是时间相关的，因为它将所有以前的观测值合并到其对输出当前时刻的估计中。看最后一张图，运行了2次迭代。观测从几个大于轨迹的峰值开始，这将会被滤波器记住（这是一个模糊的术语，我试图避免数学运算），它已经开始确信对象在轨迹上方。之后，在大约13秒的时间里，我们有一个周期，在这个周期里，所有的观测值都恰好在轨迹下面。这也会被合并到滤波器的记忆中，迭代输出会在轨迹下方发散很远。

现在让我们用另一种方式来看待这个问题。我们绘制的量不是$\mathbf{z}$，而是各自卡尔曼滤波器的输入（前一个滤波器的输出）：

plot_kf_output(xs, fxs0, zs, title='KF', aspect_equal=False)
plot_kf_output(xs, fxs1, fxs0[:, 0], '1 iteration', False)
plot_kf_output(xs, fxs2, fxs1[:, 0], '2 iterations', False)

这种滤波另一个滤波器的输出的方法的问题，已经很明显了。从图中，我们可以看到KF正在跟踪先前滤波器的不完美估计，并且由于先前观测的记忆被合并到结果中，因此也将延迟合并到结果中。

非平稳过程

到目前为止，我们假设卡尔曼滤波器中的各种矩阵都是平稳的，不随时间变化。例如，在跟踪机器人部分，我们假设$\Delta t=1.0s$，并将状态转移矩阵设计为：

$$\mathbf{F}=\left[\begin{array}{cccc}1& \Delta t& 0& 0\\ 0& 1& 0& 0\\ 0& 0& 1& \Delta t\\ 0& 0& 0& 1\end{array}\right]=\left[\begin{array}{cccc}1& 1& 0& 0\\ 0& 1& 0& 0\\ 0& 0& 1& 1\\ 0& 0& 0& 1\end{array}\right]$$

但是，如果我们数据的频率以某种不可预测的方式发生变化呢？如果我们有两个不同频率的传感器呢？如果观测误差发生变化怎么办？

处理这件事很容易，你只需更改卡尔曼滤波器矩阵以反映当前情况。让我们回到我们的狗跟踪问题，并假设数据输入有点零星。针对这个问题，我们设计了：

$$\bar{\mathbf{x}}=\left[\begin{array}{c}x\\ \dot{x}\end{array}\right]$$

$$\mathbf{F}=\left[\begin{array}{cc}1& \Delta t\\ 0& 1\end{array}\right]$$

在初始化过程中设置卡尔曼滤波器状态转移矩阵F，如下所示：

dt = 0.1
kf.F = np.array([[1, dt],
                 [0, 1]])

我们如何处理每次观测的$\Delta t$变化？这很简单——只需修改相关的矩阵。在这种情况下，$\mathbf{F}$是变化的，因此我们需要在update()/predict()循环中去更新它。$\mathbf{Q}$也依赖于时间，所以它也必须在每个循环期间分配。下面是我们如何编写代码的示例：

kf = KalmanFilter(dim_x=2, dim_z=1)
kf.x = array([0., 1.])
kf.H = array([[1, 0]])
kf.P = np.eye(2) * 50
kf.R = np.eye(1)
q_var = 0.02

# measurement tuple: (value, time)
zs = [(1., 1.),  (2., 1.1), (3., 0.9), (4.1, 1.23), (5.01, 0.97)]
for z, dt in zs:
    kf.F = array([[1, dt],
                  [0, 1]])
    kf.Q = Q_discrete_white_noise(dim=2, dt=dt, var=q_var)
    kf.predict()
    kf.update(z)
    print(kf.x)

[1. 1.]
[2.   0.92]
[2.96 1.  ]
[4.12 0.97]
[5.03 0.96]

传感器融合：不同的数据频率

很少有两种不同的传感器类别以相同的频率输出数据。假设位置传感器以$3Hz$的频率更新，车轮传感器以$7Hz$的频率更新。进一步假设传感器输出时间并不精确——存在一点抖动，使得观测可能在预测时间之前或之后发生。让我通过让轮子传感器提供速度估计，而不是位置估计来进一步使情况复杂化。

我们可以通过等待来自任一传感器的数据包来实现这一点。当我们得到它时，我们得到自上次update以来经过的时间量。然后我们需要修改受影响的矩阵，$\mathbf{F}$和$\mathbf{Q}$都包含一个时间项$\Delta t$，因此我们需要在每次predict/update中调整它们。

观测值每次都会改变，因此我们必须修改$\mathbf{H}$和$\mathbf{R}$。位置传感器会改变$\mathbf{x}$的位置元素，因此我们指定：

$$\mathbf{H}=\left[\begin{array}{cc}1& 0\end{array}\right]$$

$$\mathbf{R}={\sigma }_{ps}^2$$

车轮传感器改变$\mathbf{x}$的速度元素，因此我们指定：

$$\mathbf{H}=\left[\begin{array}{cc}0& 1\end{array}\right]$$

$$\mathbf{R}={\sigma }_{wheel}^2$$

def gen_sensor_data(t, ps_std, wheel_std):
    # generate simulated sensor data
    pos_data, vel_data = [], []
    dt = 0.
    for i in range(t*3):
        dt += 1/3.
        t_i = dt + randn() * .01 # time jitter
        pos_data.append([t_i, t_i + randn()*ps_std])

    dt = 0.    
    for i in range(t*7):
        dt += 1/7.
        t_i = dt + randn() * .006 # time jitter
        vel_data.append([t_i, 1. + randn()*wheel_std])
    return pos_data, vel_data

def plot_fusion(xs, ts, zs_ps, zs_wheel):
    xs = np.array(xs)
    plt.subplot(211)
    plt.plot(zs_ps[:, 0], zs_ps[:, 1], ls='--', label='Pos Sensor')
    plot_filter(xs=ts, ys=xs[:, 0], label='Kalman filter')
    set_labels(title='Position', y='meters',)

    plt.subplot(212)
    plot_measurements(zs_wheel[:, 0], zs_wheel[:, 1],  label='Wheel')
    plot_filter(xs=ts, ys=xs[:, 1], label='Kalman filter')
    set_labels('Velocity', 'time (sec)', 'meters/sec')

def fusion_test(pos_data, vel_data, wheel_std, ps_std):
    kf = KalmanFilter(dim_x=2, dim_z=1)
    kf.F = array([[1., 1.], [0., 1.]])
    kf.H = array([[1., 0.], [1., 0.]])
    kf.x = array([[0.], [1.]])
    kf.P *= 100

    xs, ts = [],  []

    # copy data for plotting
    zs_wheel = np.array(vel_data)
    zs_ps = np.array(pos_data)
                     
    last_t = 0
    while len(pos_data) > 0 and len(vel_data) > 0:
        if pos_data[0][0] < vel_data[0][0]:
            t, z = pos_data.pop(0)
            dt = t - last_t
            last_t = t
            
            kf.H = np.array([[1., 0.]])
            kf.R[0,0] = ps_std**2
        else:
            t, z = vel_data.pop(0)
            dt = t - last_t
            last_t = t
            
            kf.H = np.array([[0., 1.]])
            kf.R[0,0] = wheel_std**2

        kf.F[0,1] = dt
        kf.Q = Q_discrete_white_noise(2, dt=dt, var=.02)
        kf.predict()
        kf.update(np.array([z]))

        xs.append(kf.x.T[0])
        ts.append(t)
    plot_fusion(xs, ts, zs_ps, zs_wheel)

random.seed(1123)
pos_data, vel_data = gen_sensor_data(25, 1.5, 3.0)
fusion_test(pos_data, vel_data, 1.5, 3.0)

跟踪球

现在让我们把注意力转向一种情况：在真空中投掷的球必须遵守牛顿定律。即，在恒定的引力场中，它将以抛物线运动。我假设你熟悉这些公式的推导：

$$y=\frac{g}{2}{t}^2+v_{y_0}t+y_0$$

$$x=v_{x_0}t+x_0$$

其中，$g$是引力常数，$t$是时间，$v_{x_0}$和$v_{y_0}$是$x$和$y$平面上的初始速度。如果球以$v$的初始速度在地平线以上$\theta$角抛掷，我们可以计算$v_{x_0}$和$v_{y_0}$为：

$$v_{x_0}=vcos(\theta )$$

$$v_{y_0}=vsin(\theta )$$

因为我们没有真实的数据，我们将从编写一个球的模拟器开始。和往常一样，我们添加了一个独立于时间的噪声项，以便我们可以模拟有噪声的传感器。

from math import radians, sin, cos
import math

def rk4(y, x, dx, f):
    """computes 4th order Runge-Kutta for dy/dx.
    y is the initial value for y
    x is the initial value for x
    dx is the difference in x (e.g. the time step)
    f is a callable function (y, x) that you supply to 
      compute dy/dx for the specified values.
    """
    k1 = dx * f(y, x)
    k2 = dx * f(y + 0.5*k1, x + 0.5*dx)
    k3 = dx * f(y + 0.5*k2, x + 0.5*dx)
    k4 = dx * f(y + k3, x + dx)

    return y + (k1 + 2*k2 + 2*k3 + k4) / 6.

def fx(x,t):
    return fx.vel

def fy(y,t):
    return fy.vel - 9.8*t


class BallTrajectory2D(object):
    def __init__(self, x0, y0, velocity, 
                 theta_deg=0., 
                 g=9.8, 
                 noise=[0.0, 0.0]):
        self.x = x0
        self.y = y0
        self.t = 0        
        theta = math.radians(theta_deg)
        fx.vel = math.cos(theta) * velocity
        fy.vel = math.sin(theta) * velocity        
        self.g = g
        self.noise = noise

    def step(self, dt):
        self.x = rk4(self.x, self.t, dt, fx)
        self.y = rk4(self.y, self.t, dt, fy)
        self.t += dt 
        return (self.x + randn()*self.noise[0], 
                self.y + randn()*self.noise[1])

因此，要创建起始位置为$(0,15)$，速度为$100m/s$，角度为$60^{\circ }$的轨迹，我们可以写成：

traj = BallTrajectory2D(x0=0, y0=15, velocity=100, theta_deg=60)

然后每个时间步调用traj.step(t)。让我们测试一下：

def test_ball_vacuum(noise):
    y = 15
    x = 0
    ball = BallTrajectory2D(x0=x, y0=y, 
                            theta_deg=60., velocity=100., 
                            noise=noise)
    t = 0
    dt = 0.25
    while y >= 0:
        x, y = ball.step(dt)
        t += dt
        if y >= 0:
            plt.scatter(x, y, color='r', marker='.', s=75, alpha=0.5)
         
    plt.axis('equal')

#test_ball_vacuum([0, 0]) # plot ideal ball position
test_ball_vacuum([1, 1]) # plot with noise 

这看起来很合理，所以让我们继续。

选择状态量

我们可能会考虑使用与跟踪狗相同的状态量。然而，这是行不通的。回想一下，卡尔曼滤波器状态转换必须写为$\bar{\mathbf{x}}=\mathbf{Fx}+\mathbf{Bu}$，这意味着我们必须根据上一个状态计算当前状态。我们假设球在真空中运动，因此$x$方向的速度是一个常数，$y$方向的加速度完全是由重力常数$g$引起的。我们可以使用众所周知的欧拉方法，将牛顿方程离散化可以得到：

$$x_t=x_{t-1}+v_{x_{t-1}}\Delta t$$

$$v_{x_t}=v_{x_{t-1}}$$

$$y_t=y_{t-1}+v_{y_{t-1}}\Delta t$$

$$v_{y_t}=-g\Delta t+v_{y_{t-1}}$$

然而，加速度是由重力引起的，重力是一个常数。我们不需要卡尔曼滤波器跟踪一个常数，因此可以将重力视为一个控制输入。换句话说，重力是以已知的方式改变系统行为，在球的整个飞行过程中都会施加重力。

状态转移方程为$\bar{\mathbf{x}}=\mathbf{Fx}+\mathbf{Bu}$。$\mathbf{Fx}$是我们熟悉的状态转换矩阵，我们将使用它来模拟球的位置和速度，向量$\mathbf{u}$用于指定滤波器的控制输入。对于汽车来说，控制输入将是诸如踩下油门和制动器的值、方向盘的位置等；对于球来说，控制输入将是重力。矩阵$\mathbf{B}$模拟了控制输入如何影响系统的行为。对于汽车，$\mathbf{B}$将制动器和加速器的输入转换为速度的变化，并将方向盘的输入转换为不同的位置和方向；对于球，它将重力转换为速度的变化。我们将很快详细讨论此事。现在，我们将状态量设计为：

$$\mathbf{x}={\left[\begin{array}{cccc}x& \dot{x}& y& \dot{y}\end{array}\right]}^T$$

设计状态转移函数

我们的下一步是设计状态转移函数。回想一下，状态转移函数被实现为矩阵$\mathbf{F}$，我们将其与系统的前一个状态相乘，得到下一个状态$\bar{\mathbf{x}}=\mathbf{Fx}$。

我不会详细说明这一点，因为它与我们之前的情况非常相似。我们的位置和速度状态方程为：

$$\bar{x}=(1\times x)+(\Delta t\times v_x)+(0\times y)+(0\times v_y)$$

$${\bar{v}}_x=(0\times x)+(1\times v_x)+(0\times y)+(0\times v_y)$$

$$\bar{y}=(0\times x)+(0\times v_x)+(1\times y)+(\Delta t\times v_y)$$

$${\bar{v}}_y=(0\times x)+(0\times v_x)+(0\times y)+(1\times v_y)$$

请注意，这些项都不包括重力常数$g$。正如我在前面的所解释的，我们将使用卡尔曼滤波器的控制输入来解释重力。我们将其用矩阵形式写成：

$$\mathbf{F}=\left[\begin{array}{cccc}1& \Delta t& 0& 0\\ 0& 1& 0& 0\\ 0& 0& 1& \Delta t\\ 0& 0& 0& 1\end{array}\right]$$

设计控制输入函数

我们将使用控制输入来解释重力。将$\mathbf{Bu}$添加到$\bar{\mathbf{x}}$要说明重力引起的$\bar{\mathbf{x}}$的变化。我们可以说$\mathbf{Bu}$包含${\left[\begin{array}{cccc}\Delta x_g& \Delta \dot{x_g}& \Delta y_g& \Delta \dot{y_g}\end{array}\right]}^T$。

如果我们看状态转移方程，我们会发现重力只影响$y$方向的速度。

因此，我们希望$\mathbf{Bu}$等于${\left[\begin{array}{cccc}0& 0& 0& -g\Delta t\end{array}\right]}^T$。在某种意义上，我们定义$\mathbf{B}$和$\mathbf{u}$的方式是任意的，只要将它们相乘得到这个结果就行了。例如，我们可以定义$\mathbf{B}=1$、$\mathbf{u}={\left[\begin{array}{cccc}0& 0& 0& -g\Delta t\end{array}\right]}^T$。但这与我们对$\mathbf{B}$和$\mathbf{u}$的定义不符，其中$\mathbf{u}$是控制输入，$\mathbf{B}$是控制函数。我们也可以这么定义：

$$\mathbf{B}=\left[\begin{array}{cccc}0& 0& 0& 0\\ 0& 0& 0& 0\\ 0& 0& 0& 0\\ 0& 0& 0& \Delta t\end{array}\right],\mathbf{u}=\left[\begin{array}{c}0\\ 0\\ 0\\ -g\end{array}\right]$$

对我来说，这似乎有点过分。我建议我们的$\mathbf{u}$包含$x$和$y$两个维度的控制输入，这表明：

$$\mathbf{B}=\left[\begin{array}{cc}0& 0\\ 0& 0\\ 0& 0\\ 0& \Delta t\end{array}\right],\mathbf{u}=\left[\begin{array}{c}0\\ -g\end{array}\right]$$

你可能更愿意只提供实际存在的控制输入，而$x$方向没有控制输入，因此我们得出：

$$\mathbf{B}=\left[\begin{array}{c}0\\ 0\\ 0\\ \Delta t\end{array}\right],\mathbf{u}=\left[\begin{array}{c}-g\end{array}\right]$$

我见过有人用：

$$\mathbf{B}=\left[\begin{array}{cccc}0& 0& 0& 0\\ 0& 0& 0& 0\\ 0& 0& 0& 0\\ 0& 0& 0& 1\end{array}\right],\mathbf{u}=\left[\begin{array}{c}0\\ 0\\ 0\\ -g\Delta t\end{array}\right]$$

虽然这确实也能得到正确的结果，但我不愿意将时间放入$\mathbf{u}$，因为时间不是控制输入，而是我们用来将控制输入转换为状态变化的，这是$\mathbf{B}$的工作。

设计观测函数

观测函数定义了如何使用方程$\mathbf{z}=\mathbf{Hx}$，将状态量转换到观测空间的。我们假设我们有一个传感器，可以提供球在$(x,y)$中的位置，但不能观测得到速度或加速度。因此：

$$\left[\begin{array}{c}{x}_x\\ z_y\end{array}\right]=\left[\begin{array}{cccc}1& 0& 0& 0\\ 0& 0& 1& 0\end{array}\right]\left[\begin{array}{c}x\\ \dot{x}\\ y\\ \dot{y}\end{array}\right]$$

其中，

$$\mathbf{H}=\left[\begin{array}{cccc}1& 0& 0& 0\\ 0& 0& 1& 0\end{array}\right]$$

设计观测噪声

我们将假设误差在$x$和$y$方向上是独立的。在这种情况下，我们将首先假设$x$和$y$方向上的观测误差为$0.5m^2$。因此：

$$\mathbf{R}=\left[\begin{array}{cc}0.5& 0\\ 0& 0.5\end{array}\right]$$

设计过程噪声

我们假设一个球在真空中运动，所以应该没有过程噪声。我们有4个状态量，所以我们需要一个$4\times 4$的协方差矩阵：

$$\mathbf{Q}=\left[\begin{array}{cccc}0& 0& 0& 0\\ 0& 0& 0& 0\\ 0& 0& 0& 0\\ 0& 0& 0& 0\end{array}\right]$$

设计初始条件

在提供初始位置、速度、角度的情况下，我们使用三角函数计算$x$和$y$的初始速度，并使用以下公式设置了$\mathbf{x}$的值：

omega = radians(omega)
vx = cos(omega) * v0
vy = sin(omega) * v0

f1.x = np.array([[x, vx, y, vy]]).T

完成所有步骤后，我们就可以实现滤波器并对其进行测试了。

from math import sin, cos, radians

def ball_kf(x, y, omega, v0, dt, r=0.5, q=0.):
    kf = KalmanFilter(dim_x=4, dim_z=2, dim_u=1)

    kf.F = np.array([[1., dt, 0., 0.],   # x   = x0 + dx*dt
                     [0., 1., 0., 0.],   # dx  = dx0
                     [0., 0., 1., dt],   # y   = y0 + dy*dt
                     [0., 0., 0., 1.]])  # dy  = dy0

    kf.H = np.array([[1., 0., 0., 0.],
                     [0., 0., 1., 0.]])
    
    kf.B = np.array([[0., 0., 0., dt]]).T
    kf.R *= r
    kf.Q *= q

    omega = radians(omega)
    vx = cos(omega) * v0
    vy = sin(omega) * v0
    kf.x = np.array([[x, vx, y, vy]]).T
    return kf

现在，我们使用球的模拟器生成观测值来测试滤波器：

def track_ball_vacuum(dt):
    global kf
    x, y = 0., 1.
    theta = 35.  # launch angle
    v0 = 80.
    g = np.array([[-9.8]])  # gravitational constant
    ball = BallTrajectory2D(x0=x, y0=y, theta_deg=theta, velocity=v0, 
                            noise=[.2, .2])
    kf = ball_kf(x, y, theta, v0, dt)

    t = 0
    xs, ys = [], []
    while kf.x[2] > 0:
        t += dt
        x, y = ball.step(dt)
        z = np.array([[x, y]]).T

        kf.update(z)
        xs.append(kf.x[0])
        ys.append(kf.x[2])    
        kf.predict(u=g)     
        p1 = plt.scatter(x, y, color='r', marker='.', s=75, alpha=0.5)
    p2, = plt.plot(xs, ys, lw=2)
    plt.legend([p2, p1], ['Kalman filter', 'Measurements'],
               scatterpoints=1)

track_ball_vacuum(dt=1./10)

我们看到卡尔曼滤波器合理地跟踪球。然而，正如已经解释过的，这是一个微不足道的简单的例子，因为我们没有过程噪声。我们可以以任意精度预测真空中的轨迹。在本例中使用卡尔曼滤波器是一个不必要的复杂化，最小二乘的曲线拟合将给出相同的结果。

在空气中追踪球

对于这个问题，我们假设我们正在跟踪一个穿过地球大气层的球，球的轨迹受风、阻力和球的旋转影响。假设我们的传感器是一个摄像头，我们将执行某种图像处理的算法来检测球的位置——这通常称为计算机视觉中的斑点检测。然而，图像处理的算法并不完美。在任何给定的帧中，都可能检测不到斑点或检测与球不对应的伪斑点。最后，我们不会假设我们知道球的起始位置、速度、角度等信息，代码中必须根据提供的观测值来启动跟踪。我们在这里考虑的问题简化到二维世界，我们假设球总是垂直于相机传感器的平面运动。我们必须在这一点上进行简化，因为我们没有讨论如何从只提供二维数据的相机中提取三维信息。

实现空气阻力

我们的第一步是实现球在空气中运动的数学模型。稳健的解决方案需要考虑以下问题：球的粗糙度（其对阻力的影响与速度成非线性关系）、马格纳斯效应（旋转导致球的一侧相对于空气的速度高于另一侧，因此阻力系数在相对侧不同）、升力、湿度、空气密度等等。我假设读者对这些物理细节并不感兴趣，因此将把模型限制在空气阻力对非旋转球的影响上。我将使用Nicholas Giordano和Hisao Nakanishi在《Computational Physics》中研究出的数学模型。这种处理方法没有考虑所有的因素。

重要提示：在我继续之前，请允许我指出，你不必理解下面的物理部分，也可以继续使用卡尔曼滤波器。我的目标是在现实世界中创建一个相当精确的球的行为，这样我们就可以测试我们的卡尔曼滤波器如何处理现实世界的行为。但通常不可能对真实世界系统的物理进行完全建模，我们只需要一个包含大多数行为的过程模型。然后，我们调整观测噪声和过程噪声，直到滤波器与我们的数据配合良好。这是一个真正的风险：微调卡尔曼滤波器很容易，因此它可以完美地与你的测试数据贴合，但当使用稍微不同的数据时，性能可能会变得很差。这也许是设计卡尔曼滤波器最困难的部分，也是为什么它会被称为黑色艺术。

我不喜欢那些不加解释直接给出结果的书，所以我现在将稍微解释一下球在空气中的运动物理。如果你不感兴趣，可以跳过模拟的内容。

球在空中移动时会遇到风的阻力。这会在球面上施加一种阻力，从而改变球的飞行。Giordano将这表示为：

$$F_{drag}=-B_2{v}^2$$

其中，$B_2$是通过实验得出的系数，$v$是球的运动速度。$F_{drag}$可以分解为$x$和$y$分量：

$$F_{drag,x}=-B_{2}v{v}_x$$

$$F_{drag,y}=-B_{2}v{v}_y$$

如果$m$是球的质量，我们可以用$F=ma$来计算加速度：

$$a_x=-\frac{B_2}{m}v{v}_x$$

$$a_y=-\frac{B_2}{m}v{v}_y$$

考虑空气密度、棒球横截面及其粗糙度， Giordano为$\frac{B_2}{m}$提供了一下函数。请理解，这是基于风洞试验和几个简化假设的近似值。它以国际单位制为单位：速度以$m/s$为单位，时间以$s$为单位。

$$\frac{B_2}{m}=0.0039+\frac{0.0058}{1+exp[(v-35)/5{]}^2}$$

从真空中球轨迹的欧拉离散化开始：

$$x=v_x\Delta t$$

$$y=v_y\Delta t$$

$$v_x=v_x$$

$$v_y=v_y-9.8\Delta t$$

接下来，需要将$accel\times \Delta t$加入到速度更新方程。我们应该减去这个分量，因为阻力会降低速度。这样做的代码非常简单，我们只需要将力分解为$x$和$y$分量。

因为物理的推导超出了本文的范围，所以我不打算进一步讨论这个问题。如果想要更高精确度的模拟需要结合海拔、温度、球旋转和其他几个因素。我在这里的目的是将一些真实世界的行为输入到我们的模拟中，以测试卡尔曼滤波器使用更简单的过程模型对这种行为的反应。你的过程模型永远不会精确地模拟世界上发生的事情，设计一个好的卡尔曼滤波器的一个重要因素是：仔细地测试它对真实世界数据的性能。

下面的代码计算球在无风以及有风的情况下的行为，在两种风速情况下，使用相同的初始初始值绘制。

from math import sqrt, exp

def mph_to_mps(x):
    return x * .447

def drag_force(velocity):
    """ Returns the force on a baseball due to air drag at
    the specified velocity. Units are SI"""
    return velocity * (0.0039 + 0.0058 / 
            (1. + exp((velocity-35.)/5.)))

v = mph_to_mps(110.)
x, y = 0., 1.
dt = .1
theta = radians(35)

def solve(x, y, vel, v_wind, launch_angle):
    xs = []
    ys = []
    v_x = vel*cos(launch_angle)
    v_y = vel*sin(launch_angle)
    while y >= 0:
        # Euler equations for x and y
        x += v_x*dt
        y += v_y*dt

        # force due to air drag    
        velocity = sqrt((v_x-v_wind)**2 + v_y**2)    
        F = drag_force(velocity)

        # euler's equations for vx and vy
        v_x = v_x - F*(v_x-v_wind)*dt
        v_y = v_y - 9.8*dt - F*v_y*dt
        
        xs.append(x)
        ys.append(y)

    return xs, ys

x, y = solve(x=0, y=1, vel=v, v_wind=0, launch_angle=theta)
p1 = plt.scatter(x, y, color='blue', label='no wind')

wind = mph_to_mps(10)
x, y = solve(x=0, y=1, vel=v, v_wind=wind, launch_angle=theta)
p2 = plt.scatter(x, y, color='green', marker="v", 
                 label='10mph wind')
plt.legend(scatterpoints=1)

我们可以很容易地看到球在真空中的轨迹和在空气中的轨迹之间的差别。我在上面的代码中使用了相同的初始速度和发射角度。

无需进一步讨论，我们将使用上述数学模型创建一个球的运动模拟。我会注意到阻力的非线性行为意味着在任何时间点都没有球位置的解析解，所以我们需要逐步计算位置。我使用欧拉方法来求解，使用更精确的技术，如Runge-Kutta，留给读者作为练习。对于我们正在做的事情来说，这种程度的复杂性是不必要的，因为对于我们将要使用的时间步长，这两种技术之间的精度差异将很小。

class BaseballPath:
    def __init__(self, x0, y0, launch_angle_deg, velocity_ms, 
                 noise=(1.0, 1.0)): 
        """ Create 2D baseball path object  
           (x = distance from start point in ground plane, 
            y=height above ground)
        
        x0,y0            initial position
        launch_angle_deg angle ball is travelling respective to 
                         ground plane
        velocity_ms      speeed of ball in meters/second
        noise            amount of noise to add to each position
                         in (x, y)
        """
        omega = radians(launch_angle_deg)
        self.v_x = velocity_ms * cos(omega)
        self.v_y = velocity_ms * sin(omega)

        self.x = x0
        self.y = y0
        self.noise = noise

    def drag_force(self, velocity):
        """ Returns the force on a baseball due to air drag at
        the specified velocity. Units are SI
        """
        B_m = 0.0039 + 0.0058 / (1. + exp((velocity-35.)/5.))
        return B_m * velocity

    def update(self, dt, vel_wind=0.):
        """ compute the ball position based on the specified time 
        step and wind velocity. Returns (x, y) position tuple.
        """
        # Euler equations for x and y
        self.x += self.v_x*dt
        self.y += self.v_y*dt

        # force due to air drag
        v_x_wind = self.v_x - vel_wind
        v = sqrt(v_x_wind**2 + self.v_y**2)
        F = self.drag_force(v)

        # Euler's equations for velocity
        self.v_x = self.v_x - F*v_x_wind*dt
        self.v_y = self.v_y - 9.81*dt - F*self.v_y*dt

        return (self.x + randn()*self.noise[0], 
                self.y + randn()*self.noise[1])

现在，我们可以根据该模型创建的观测值测试卡尔曼滤波器。

x, y = 0, 1.

theta = 35. # launch angle
v0 = 50.
dt = 1/10.   # time step
g = np.array([[-9.8]])

plt.figure()
ball = BaseballPath(x0=x, y0=y, launch_angle_deg=theta,
                    velocity_ms=v0, noise=[.3,.3])
f1 = ball_kf(x, y, theta, v0, dt, r=1.)
f2 = ball_kf(x, y, theta, v0, dt, r=10.)
t = 0
xs, ys = [], []
xs2, ys2 = [], []

while f1.x[2] > 0:
    t += dt
    x, y = ball.update(dt)
    z = np.array([[x, y]]).T

    f1.update(z)
    f2.update(z)
    xs.append(f1.x[0])
    ys.append(f1.x[2])
    xs2.append(f2.x[0])
    ys2.append(f2.x[2])    
    f1.predict(u=g) 
    f2.predict(u=g)
    
    p1 = plt.scatter(x, y, color='r', marker='.', s=75, alpha=0.5)

p2, = plt.plot(xs, ys, lw=2)
p3, = plt.plot(xs2, ys2, lw=4)
plt.legend([p1, p2, p3], 
           ['Measurements', 'Filter(R=0.5)', 'Filter(R=10)'],
           loc='best', scatterpoints=1)

我绘制了卡尔曼滤波器两种不同配置的结果输出。观测值表示为圆圈点，$R=0.5$的卡尔曼滤波器表示为细线，$R=10$的卡尔曼滤波器表示为粗线。选择这些$R$值只是为了显示观测噪声对输出的影响，并不意味着设计正确。

我们可以看到，这两个滤波器都做得较好。起初，两者都能很好地跟踪观测结果，但随着时间的推移，它们都出现了分歧。这是因为空气阻力的状态模型是非线性的，而卡尔曼滤波器假设它是线性的。由于加速度为负，因此卡尔曼滤波器始终会超出球的位置。只要加速度继续，滤波器就无法赶上，因此滤波器将继续发散。

我们可以做些什么来改善这一点？最好的方法是使用非线性卡尔曼滤波器进行滤波，我们将在后续章节中这样做。然而，对于这个问题，还有一种我称之为工程的解决方案。我们的卡尔曼滤波器假设球在真空中，因此没有过程噪声。然而，由于球在空气中，大气会对球施加力，我们可以把这个力看作过程噪声。这不是一个特别严谨的想法。首先，这个力不是高斯的。其次，我们可以计算这个力，所以说这是随机的不会得到最优解。但让我们看看如果我们遵循这条思路会发生什么。

以下代码实现了与之前相同的卡尔曼滤波器，但过程噪声非零。我画了两个例子，一个是$Q=0.1$，另一个是$Q=0.01$。

def plot_ball_with_q(q, r=1., noise=0.3):
    x, y = 0., 1.
    theta = 35. # launch angle
    v0 = 50.
    dt = 1/10.   # time step
    g = np.array([[-9.8]])

    ball = BaseballPath(x0=x, 
                        y0=y, 
                        launch_angle_deg=theta, 
                        velocity_ms=v0, 
                        noise=[noise,noise])
    f1 = ball_kf(x, y, theta, v0, dt, r=r, q=q)
    t = 0
    xs, ys = [], []

    while f1.x[2] > 0:
        t += dt
        x, y = ball.update(dt)
        z = np.array([[x, y]]).T

        f1.update(z)
        xs.append(f1.x[0])
        ys.append(f1.x[2]) 
        f1.predict(u=g) 

        p1 = plt.scatter(x, y, c='r', marker='.', s=75, alpha=0.5)

    p2, = plt.plot(xs, ys, lw=2, color='b')
    plt.legend([p1, p2], ['Measurements', 'Kalman filter'])
    plt.show()

plot_ball_with_q(0.01)
plot_ball_with_q(0.1)

第二个滤波器可以很好地跟踪观测结果。看起来似乎有点滞后，但滞后量很少。

这是一种好的方法吗？通常情况下不是，但也要视情况而定。在这里，作用在球上的力是相当恒定的。假设我们正试图跟踪一辆汽车——加速度会随着汽车的速度和转弯程度的变化而变化。当我们使过程噪声高于系统中的实际噪声时，滤波器将选择更偏向于观测值。如果你的观测中没有太多的噪音，这可能适合你。然而，考虑下一个情况，我增加了观测中的噪声。

plot_ball_with_q(0.01, r=3, noise=3.)
plot_ball_with_q(0.1, r=3, noise=3.)

这个输出很糟糕。滤波器别无选择，只能给观测赋予比过程（预测步）更多的权重。但当观测值有噪声时，滤波器输出将只跟踪噪声。线性卡尔曼滤波器的这种固有局限性导致了滤波器的非线性版本的发展。

话虽如此，使用过程噪声处理系统中的微小的非线性肯定是可以的，这是卡尔曼滤波器黑色艺术的一部分。我们的传感器和系统模型从来都不是完美的。传感器是非高斯的，我们的过程模型从来都不是完美的。你可以通过将观测误差和过程误差设置得高于其理论正确值，来掩盖其中一些问题，但这种方法是非最佳解决方案。当然，非最优总比卡尔曼滤波器发散要好。然而，正如我们在上面的图表中所看到的，滤波器的输出很容易比较糟糕。通过运行许多模拟和测试数据，最终在这些类似条件下获得良好性能的滤波器是非常常见的。然而，当你在实际其他条件下的数据上使用滤波器时，滤波器的性能可能会变得非常差。

现在我们将把这个问题放在一边，因为在这个例子中我们显然误用了卡尔曼滤波器。我们将在后续章节中重新讨论这个问题，以了解使用各种非线性技术的效果。在某些领域中，你可以使用线性卡尔曼滤波器来解决非线性问题，但通常你必须使用一种或多种你将在本书其余部分学习的技术。

相关阅读

• Kalman-and-Bayesian-Filters-in-Python/blob/master/08-Designing-Kalman-Filters