【滤波】多元卡尔曼滤波器

%matplotlib inline

#format the book
import book_format
book_format.set_style()

简介

我们现在准备研究和实现完整的、多元形式的卡尔曼滤波器。在上一章中，我们学习了多元高斯如何表达多个随机变量之间的相关性，例如飞机的位置和速度，我们还了解了变量之间的相关性如何显著提高后验概率。如果我们只粗略地知道位置和速度，但它们是相关的，那么我们的新估计可以非常准确。

我更希望你通过几个示例，来培养这些滤波器工作原理的直觉。本文中，我将要掩盖许多问题和细节。我向你们展示的一些东西只适用于特殊情况，可能是神奇的——你可能不清楚我是如何得出某个结果的。但如果我从严格的、广义的方程开始，你可能会对所有这些术语的含义，以及你如何将它们应用到你的问题上感到困惑。在后面的章节中，我将提供一个更严格的数学基础，在那个时候，我将必须纠正我在本章中所做的近似，或者提供我在这里没有涵盖的额外信息。

为了使这成为可能，我们将把自己限制在一个子集的问题里，我们研究可以用牛顿的运动方程来描述的问题。这些滤波器称为离散连续时间运动的滤波器。在后续的卡尔曼滤波数学一章中，我们将延申到非牛顿系统的数学。

牛顿运动方程

牛顿的运动方程告诉我们，给定一个系统的恒定速度 v v v，我们可以计算出它在时间 t t t后的位置 x x x，公式为：

x = v t + x 0 x = vt + x_{0} x=vt+x0

例如，如果我们从位置13开始，我们的速度是10米/秒，行驶12秒，我们的最终位置是133（ 10 × 12 + 13 10\times 12+13 10×12+13）。

我们可以把恒加速度 a a a和这个方程结合起来：

x = 1 2 a t 2 + v 0 t + x 0 x= \frac{1}{2}at^{2} + v_{0}t + x_{0} x=21at2+v0t+x0

如果我们假设持续的加加速度 j j j：

x = 1 6 j t 3 + 1 2 a 0 t 2 + v 0 t + x 0 x = \frac{1}{6}jt^{3} + \frac{1}{2}a_{0}t^{2} + v_{0}t + x_{0} x=61jt3+21a0t2+v0t+x0

这些方程是通过积分微分方程得到的。例如：给定一个恒定的速度 v v v，我们可以用这个方程计算随时间推移的距离：

x = v t + x 0 x = vt + x_{0} x=vt+x0

我们可以通过以下的积分微分得到：

v = d x d t v = \frac{dx}{dt} v=dtdx

d x = v d t dx = vdt dx=vdt

∫ x 0 x d x = ∫ 0 t v d t \int_{x_{0}}^{x} dx = \int_{0}^{t} vdt ∫x0xdx=∫0tvdt

x − x 0 = v t − 0 x - x_{0} = vt - 0 x−x0=vt−0

x = v t + x 0 x = vt + x_{0} x=vt+x0

当你设计一个卡尔曼滤波器时，你需要从一个描述系统变化的微分方程组开始。大多数微分方程组不容易用这种方法积分。我们从牛顿方程开始，因为我们可以积分得到一个封闭形式的解，这使得卡尔曼滤波器更容易设计。另一个好处是牛顿方程是跟踪运动物体的正确方程，这是卡尔曼滤波器的主要用途之一。

卡尔曼滤波算法

该算法与我们在之前使用的贝叶斯滤波算法基本相同，只是更新步骤稍微复杂一点，但我会在后面解释原因。

初始化

初始化滤波器的状态量
初始化状态量的协方差矩阵

预测

使用过程模型来预测下一时间步的状态量
调整协方差矩阵来解释预测中的不确定性

更新

得到一个观测值和它的准确度
计算估计状态量和观测值之间的残差
根据先验值和观测值谁更准确来计算比例因子
基于比例因子在先验和观测值之间设置状态量
根据我们在观测中的确定程度更新对状态量的协方差矩阵

作为提醒，以下是算法的图示：

import kf_book.book_plots as book_plotsbook_plots.show_residual_chart()

一元卡尔曼滤波器用一元高斯表示状态量。当然，多元卡尔曼滤波器将使用多元高斯状态量。我们在上一章中了解到，多元高斯函数使用向量作为均值，使用矩阵作为协方差。这意味着卡尔曼滤波器需要使用线性代数的知识来进行估计。

我不想让你记住这些方程，但我在下面列出了一元和多元的对比。它们很相似。

预测

Univariate Univariate Multivariate (Kalman mode) μ ˉ = μ + μ f x x ˉ = x + d x X ˉ = F x + B u σ ˉ 2 = σ 2 + σ f x 2 P ˉ = P + Q P ˉ = F P F T + Q \begin{array}{|l|l|l|} \hline \text{Univariate} & \text{Univariate} & \text{Multivariate} \\ & \text{(Kalman mode)} \\ \hline \bar{\mu} = \mu + \mu_{f_{x}} & \bar {x} = x + dx & \bar{\mathbf{X}} = \mathbf{F}\mathbf{x} + \mathbf{B}\mathbf{u} \\ \bar{\sigma}^{2} = \sigma^{2} + \sigma_{f_{x}}^{2} & \bar {P} = P + Q & \bar{\mathbf{P}} = \mathbf{F}\mathbf{P}\mathbf{F}^{T} + \mathbf{Q}\\ \hline \end{array} Univariateμˉ=μ+μfxσˉ2=σ2+σfx2Univariate(Kalman mode)xˉ=x+dxPˉ=P+QMultivariateXˉ=Fx+BuPˉ=FPFT+Q

不用担心线性代数的细节，我们可以看到：

x \mathbf{x} x、 P \mathbf{P} P是状态量均值和协方差，它们对应于 x x x和 σ 2 \sigma^{2} σ2；
F \mathbf{F} F是状态转移函数，当乘以 x \mathbf{x} x时，用来计算先验；
Q \mathbf{Q} Q是过程协方差，它对应于 σ f x 2 \sigma_{f_{x}}^{2} σfx2；
B \mathbf{B} B、 u \mathbf{u} u对我们来说是新接触的，它们让我们对系统的输入进行建模控制。

更新

Univariate Univariate Multivariate (Kalman Form) y = z − x ˉ y = z − H x ˉ K = P ˉ P ˉ + R K = P ˉ H T ( H P ˉ H T + R ) − 1 μ = σ ˉ 2 μ z + σ z 2 μ ˉ σ ˉ 2 + σ z 2 x = x ˉ + K y x = x ˉ + K y σ 2 = σ ˉ 2 σ z 2 σ ˉ 2 + σ z 2 P = ( 1 − K ) P ˉ P = ( I − K H ) P ˉ \begin{array}{|l|l|l|} \hline \text{Univariate} & \text{Univariate}& \text{Multivariate} \\ & \text{(Kalman Form)} \\ \hline & y = z - \bar{x} & \mathbf{y} = \mathbf{z} - \mathbf{H}\bar {\mathbf{x}} \\ & K = \frac {\bar {P}}{\bar {P}+R} & \mathbf{K} = \bar{\mathbf{P}} \mathbf{H} ^{T}(\mathbf{H}\bar{\mathbf{P}}\mathbf{H}^{T} + \mathbf{R})^{-1}\\ \mu = \frac{\bar{\sigma }^{2}\mu_{z} + \sigma _{z}^{2}\bar{\mu}}{\bar{\sigma }^{2} + \sigma _{z}^{2}} & x = \bar {x} + Ky & \mathbf{x} = \bar {\mathbf{x}} + \mathbf{K}\mathbf{y}\\ \sigma^{2} = \frac {\bar{\sigma}^{2} \sigma_{z}^{2}} {\bar{\sigma}^{2} + \sigma_{z}^{2}} & P = (1-K)\bar {P} & \mathbf{P} = (\mathbf{I}-\mathbf{K}\mathbf{H})\bar {\mathbf{P}}\\ \hline \end{array} Univariateμ=σˉ2+σz2σˉ2μz+σz2μˉσ2=σˉ2+σz2σˉ2σz2Univariate(Kalman Form)y=z−xˉK=Pˉ+RPˉx=xˉ+KyP=(1−K)PˉMultivariatey=z−HxˉK=PˉHT(HPˉHT+R)−1x=xˉ+KyP=(I−KH)Pˉ

H \mathbf{H} H是观测函数，我稍后会解释。如果你把 H \mathbf{H} H从方程中去掉，你会发现这些方程也很相似；
z \mathbf{z} z、 R \mathbf{R} R是观测均值和噪声协方差。它们对应于单变量滤波器中的 z z z和 σ z 2 \sigma_{z}^{2} σz2（我用 x x x代替了一元方程的 μ \mu μ，以使符号尽可能相似）；
y \mathbf{y} y、 K \mathbf{K} K是残差和卡尔曼增益。

一些细节不同于一元滤波器，因为它们是向量和矩阵，但概念完全相同：

用高斯函数来表示我们对状态量和误差的估计
用高斯函数表示观测值及其误差
用高斯函数表示过程模型
使用过程模型预测下一个状态量（先验）
在观测值和先验值之间形成一个估计值

作为设计者，你的工作将是设计状态量 ( x , P ) (\mathbf{x}, \mathbf{P}) (x,P)、过程 ( F , Q ) (\mathbf{F}, \mathbf{Q}) (F,Q)、观测 ( z , R ) (\mathbf{z}, \mathbf{R}) (z,R)和观测函数 H \mathbf{H} H。如果系统有控制输入，比如机器人，你也会设计 ( u , B ) (\mathbf{u}, \mathbf{B}) (u,B)。

卡尔曼滤波方程的predict()和update()函数，都可以在FilterPy中找到，你可以通过以下方式导入它们：

from filterpy.kalman import predict, update

跟踪狗

让我们回到追踪狗这个久经考验的问题上来。这次我们将囊括上一章的基本观点，并使用隐藏变量来改善我们的估计。我可以从数学开始讲解，但是先让我们实现一个滤波器，边做边学习。从表面上看，数学是不同的，也许比前几章更复杂，但思想都是一样的——我们只是高斯乘法和高斯加法。

我们从为狗写一个模拟开始。模拟将运行 c o u n t count count步，每一步狗向前移动大约 1 m 1m 1m。在每一步中，速度将根据过程方差 p r o c e s s _ v a r process\_var process_var而变化。更新位置后，我们假定传感器方差为 z _ v a r z\_var z_var，模拟观测值。该函数返回位置的NumPy数组和另一个观测值。

import math
import numpy as np
from numpy.random import randndef compute_dog_data(z_var, process_var, count=1, dt=1.):"returns track, measurements 1D ndarrays"x, vel = 0., 1.z_std = math.sqrt(z_var)p_std = math.sqrt(process_var)xs, zs = [], []for _ in range(count):v = vel + (randn() * p_std)x += v*dtxs.append(x)zs.append(x + randn() * z_std)return np.array(xs), np.array(zs)

预测步

对于预测，我们需要设计状态量和协方差、过程模型和过程噪声，以及可选的控制输入。我们把它们整理好。

设计状态量

我们以前用高斯函数在一维上跟踪狗。均值（ μ \mu μ）表示最可能的位置，方差（ σ 2 \sigma^{2} σ2）表示位置的概率分布。位置是系统的状态量，即 μ \mu μ为状态量。

在这个问题中，我们将跟踪狗的位置和速度。这要求我们使用状态向量 x \mathbf{x} x，及其相应协方差矩阵 P \mathbf{P} P表示多元高斯分布。

状态量可以是观测量（由传感器直接观测得到），也可以是隐藏量（由观测量推断得到）。对于我们的狗跟踪问题，传感器只读取位置，所以位置被观测，速度被隐藏。我们将很快学习如何跟踪隐藏量。

重要的是，跟踪位置和速度是一个充满暗示和假设的设计选择。当然，我们还可以跟踪加速度。回想一下在上一章中，我们展示了在协方差矩阵中包含速度，会导致更小的位置方差。我们将在本章后面学习，卡尔曼滤波器如何计算隐藏量的估计。

在一元的章节中，我们用标量值表示狗的位置（例如 μ = 3.27 \mu=3.27 μ=3.27）。在上一章中，我们学习了多元高斯。例如，如果我们想指定 10.0 m 10.0m 10.0m的位置和 4.5 m / s 4.5 m/s 4.5m/s的速度，我们会写：

μ = [ 10.0 4.5 ] \mu = \begin{bmatrix} 10.0 \\ 4.5 \end{bmatrix} μ=[10.04.5]

利用线性代数，实现卡尔曼滤波。我们使用一个 n × 1 n\times 1 n×1的矩阵（向量）来存储 n n n个状态量。对于狗的跟踪问题，我们用 x x x表示位置，用 x x x的一阶导数 x ˙ \dot{x} x˙表示速度。这里，我使用牛顿的点表示法来表示导数； x ˙ \dot{x} x˙表示 x x x对 t t t的一阶导数： x ˙ = d x d t \dot{x} = \frac{dx}{dt} x˙=dtdx。卡尔曼滤波方程使用 x \mathbf{x} x表示状态量，因此我们将 x \mathbf{x} x定义为：

x = [ x x ˙ ] \mathbf{x} = \begin{bmatrix} x \\ \dot{x} \end{bmatrix} x=[xx˙]

我们用 x \mathbf{x} x代替 μ \mu μ，但要知道这是多元高斯函数的均值。

另一种写法是 x = [ x x ˙ ] T \mathbf{x} = \begin{bmatrix} x & \dot{x} \end{bmatrix} ^{T} x=[xx˙]T，因为行向量的转置是列向量。这种符号更容易在文本中使用，因为它占用较少的垂直空间。

x \mathbf{x} x和位置 x x x只是恰好具有相同的名称：如果我们在 y y y轴上跟踪狗，我们会写 x = [ y y ˙ ] T \mathbf{x} = \begin{bmatrix} y & \dot{y} \end{bmatrix} ^{T} x=[yy˙]T，而不是 y = [ y y ˙ ] T \mathbf{y} = \begin{bmatrix} y & \dot{y} \end{bmatrix} ^{T} y=[yy˙]T。 x \mathbf{x} x是卡尔曼滤波器文献中使用的状态量的标准名称，我们不会对其进行更改。这种命名的一致性使我们能够与同行交流。

让我们把它进行编程， x \mathbf{x} x的初始化非常简单：

x = np.array([[10.0],[4.5]])
x

array([[10. ],[ 4.5]])

我经常在代码中使用转置将行矩阵转换为列向量，因为我发现这样更容易键入和读取：

x = np.array([[10., 4.5]]).T
x

array([[10. ],[ 4.5]])
x

但是，NumPy将一维数组识别为向量，因此我可以将这一行简化为使用一维数组：

x = np.array([10.0, 4.5])
x

array([10. ,  4.5])

数组内所有元素的类型都相同，通常为float或int。如果列表包含的所有值都是int，则创建的数组的数据类型也将为int，否则将为float。我通常会利用这一点，只指定其中一个数字作为浮点数：

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

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

这里有一些例子。

A = np.array([[1, 2], [3, 4]])
x = np.array([[10.0], [4.5]])# matrix multiply
print(np.dot(A, x))
print()

[[19.][48.]]

在Python3.5+中，我们有矩阵乘法器 @ @ @，其中 n p . d o t ( A , B ) = = A @ B np.dot(A, B) == A @ B np.dot(A,B)==A@B。因为它要求A和B都是数组，所以你可能意识到这个运算符的用处相对较少。在这本书的数学中，有些变量是标量，因此 @ @ @的实用性经常丢失。

# alternative matrix multiply
print(A @ x)
print()x = np.array([[10.0, 4.5]]).T
print(A @ x)
print()x = np.array([10.0, 4.5])
print(A @ x)

[[19.][48.]][[19.][48.]][19. 48.]

最后一个返回一维数组，但是我已经编写了Kalman filter类来处理这个问题。回想起来，这可能会导致混乱，但它确实有效。

设计状态量协方差矩阵

用高斯表示状态量，另一半是协方差矩阵 P \mathbf{P} P。在一元卡尔曼滤波器中，我们为 σ 2 \sigma^{2} σ2指定了一个初始值，然后当观测值添加到滤波器中时，滤波器负责更新其值。在多元卡尔曼滤波器中也会发生同样的情况。我们为 P \mathbf{P} P指定一个初始值，并且滤波器在每个滤波循环更新它。

我们需要将方差设置为合理的值。例如，如果我们对初始位置非常不确定，我们可以设置 σ p o s 2 = 500 m 2 \sigma_{pos}^{2} = 500 m^{2} σpos2=500m2。狗的最高速度约为 21 m / s 21 m/s 21m/s，因此在没有任何其他速度信息的情况下，我们可以设置 3 σ v e l = 21 3\sigma_{vel} = 21 3σvel=21或者 σ v e l 2 = 49 \sigma_{vel}^{2} = 49 σvel2=49。

在上一章中，我们证明了位置和速度是相关的。但是对于一只狗来说，它们有多大的关联呢？我不知道。如果我们想让滤波器为我们计算这个，我就将协方差初始化为零。当然，如果你知道协方差，你最好直接使用它们。

回想一下，协方差矩阵的对角线表示每个变量的方差，非对角线元素表示两两之间的协方差。因此我们有：

P = [ 500 0 0 49 ] \mathbf{P} = \begin{bmatrix} 500 & 0 \\ 0 & 49 \end{bmatrix} P=[5000049]

我们可以使用numpy.diag，它从对角线的值创建对角线矩阵。回想一下线性代数，对角矩阵表示非对角的元素都是零。

P = np.diag([500., 49.])
P

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

我可以写：

P = np.array([[500., 0.],[0., 49.]])
P

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

我们将滤波器的状态量表示为多元高斯函数，并在代码中实现了它。

设计过程模型

下一步是设计过程模型，它是描述系统行为的数学模型。滤波器使用它来预测离散时间步后的状态量，我们用一组方程来描述系统的动力学。

在一元卡尔曼的章节中，我们用：

x = v Δ t + x 0 x=v\Delta t + x_{0} x=vΔt+x0

我们代码如下编写：

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

我们将在本章中做同样的事情，不过使用多元高斯而不是一元高斯。你可以想象这种实现：

x = [ 5.4 4.2 ] , x ˙ = [ 1.1 0. ] \mathbf{x} = \begin{bmatrix} 5.4 \\ 4.2 \end{bmatrix} , \dot{\mathbf{x}} = \begin{bmatrix} 1.1 \\ 0. \end{bmatrix} x=[5.44.2],x˙=[1.10.]

x = x ˙ t + x \mathbf{x}=\dot{\mathbf{x}} t + \mathbf{x} x=x˙t+x

但我们需要概括抽象一下。卡尔曼滤波方程适用于任何线性系统，而不仅仅是牛顿系统。可能你正在滤波的系统是化工厂的管道系统，而给定管道中的流量是由不同阀门设置的线性组合决定的。例如:

p i p e 1 = 0.134 ( v a l u e 1 ) + 0.41 ( v a l u e 2 − v a l u e 3 ) + 1.34 pipe_{1}=0.134(value_{1})+0.41(value_{2}−value_{3})+1.34 pipe1=0.134(value1)+0.41(value2−value3)+1.34

p i p e 2 = 0.210 ( v a l u e 2 ) − 0.62 ( v a l u e 1 − v a l u e 5 ) + 1.86 pipe_{2}=0.210(value_{2})−0.62(value_{1}−value_{5})+1.86 pipe2=0.210(value2)−0.62(value1−value5)+1.86

线性代数有一种强有力的方法来表达方程组。以这个系统为例：

{ 2 x + 3 y = 8 4 x − y = 2 \left\{\begin{matrix} 2x + 3y = 8 \\ 4x - y = 2 \end{matrix}\right. {2x+3y=84x−y=2

我们可以把它写成矩阵形式：

[ 2 3 4 − 1 ] [ x y ] = [ 8 2 ] \begin{bmatrix} 2 & 3\\ 4 & -1 \end{bmatrix}\begin{bmatrix} x \\y\end{bmatrix}= \begin{bmatrix} 8 \\2\end{bmatrix} [243−1][xy]=[82]

如果在这个等式中执行矩阵乘法，结果将是上面的两个等式。在线性代数中，我们把它写成 A x = B \mathbf{A}\mathbf{x}=\mathbf{B} Ax=B，其中：

A = [ 2 3 4 − 1 ] , x = [ x y ] , B = [ 8 2 ] \mathbf{A} = \begin{bmatrix} 2 & 3\\ 4 & -1 \end{bmatrix}, \mathbf{x} = \begin{bmatrix} x \\y\end{bmatrix}, \mathbf{B}= \begin{bmatrix} 8 \\2\end{bmatrix} A=[243−1],x=[xy],B=[82]

然后我们可以用SciPy的linalg包来解 x \mathbf{x} x：

from scipy.linalg import solveA = np.array([[2, 3],[4, -1]])
b = np.array([[8], [2]])
x = solve(A, b)
x

[[1.][2.]]

我们使用过程模型来进行预测，因为方程告诉我们在当前状态的下一个状态将是什么。卡尔曼滤波器使用这个线性方程来实现这一点，其中 x ˉ \bar{\mathbf{x}} xˉ是先验或预测的状态：

x ˉ = F x \bar{\mathbf{x}} = \mathbf{F}\mathbf{x} xˉ=Fx

在本例中，我们可以明确表示为：

[ x ˉ x ˙ ˉ ] = [ ? ? ? ? ] [ x x ˙ ] \begin{bmatrix} \bar{x} \\ \bar{\dot{x}} \end{bmatrix} = \begin{bmatrix} ? & ? \\ ? & ? \end{bmatrix}\begin{bmatrix} x \\ \dot{x} \end{bmatrix} [xˉx˙ˉ]=[????][xx˙]

作为卡尔曼滤波器的设计者，我们的工作是指定 F \mathbf{F} F，以便执行系统的预测 x ˉ = F x \bar{\mathbf{x}} = \mathbf{F}\mathbf{x} xˉ=Fx。要做到这一点，我们需要一个包含每个状态量的方程。在我们的问题中， [ x x ˙ ] T \begin{bmatrix} x & \dot{x} \end{bmatrix} ^{T} [xx˙]T，所以我们需要一个方程来计算位置 x x x，另一个方程来计算速度 x ˙ \dot{x} x˙。我们已经知道了位置更新的方程式：

x ˉ = x + x ˙ Δ t \bar{x}=x+\dot{x}\Delta t xˉ=x+x˙Δt

我们的速度方程是什么？对于狗的速度如何随时间变化，我们没有预测模型。在这种情况下，我们假设狗的速度保持不变。当然这并不完全正确，但是只要狗的速度在每一次滤波中确实没有太大的变化，你就会看到滤波器的性能非常好。所以我们说:

x ˙ ˉ = x ˙ \bar{\dot{x}} = \dot{x} x˙ˉ=x˙

这为我们的系统提供了过程模型：

{ x ˉ = x + x ˙ Δ t x ˙ ˉ = x ˙ \left\{\begin{matrix} \bar{x}=x+\dot{x}\Delta t \\ \bar{\dot{x}} = \dot{x} \end{matrix}\right. {xˉ=x+x˙Δtx˙ˉ=x˙

对于状态中的每一个变量，都有一个正确的方程，并且都在左侧隔离。我们需要用 x ˉ = F x \bar{\mathbf{x}} = \mathbf{F}\mathbf{x} xˉ=Fx的形式来表示这组方程，就会看得更加清楚：

[ x ˉ x ˙ ˉ ] = [ 1 Δ t 0 1 ] [ x x ˙ ] \begin{bmatrix} \bar{x} \\ \bar{\dot{x}} \end{bmatrix} = \begin{bmatrix} 1 & \Delta t \\ 0 & 1 \end{bmatrix}\begin{bmatrix} x \\ \dot{x} \end{bmatrix} [xˉx˙ˉ]=[10Δt1][xx˙]
x ˉ = F x \bar{\mathbf{x}} = \mathbf{F}\mathbf{x} xˉ=Fx

F \mathbf{F} F称为状态转移函数或状态转移矩阵。在后面的章节中，它将是一个真正的函数，而不是一个矩阵，因此将其称为函数更为一般。

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

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

我们来测试一下！FilterPy有一种predict()方法，通过计算 x ˉ = F x \bar{\mathbf{x}} = \mathbf{F}\mathbf{x} xˉ=Fx来执行预测。让我们调用一下，看看会发生什么。我们把位置设为 10.0 m 10.0m 10.0m，速度设为 4.5 m / s 4.5m/s 4.5m/s。我们已经定义了 d t = 0.1 s dt=0.1s dt=0.1s，这意味着时间步长是0.1秒，因此我们预计在更新后的新位置为10.45米，速度不变。

from filterpy.kalman import predictx = np.array([10.0, 4.5])
P = np.diag([500, 49])
F = np.array([[1, dt], [0, 1]])# Q is the process noise
x, P = predict(x=x, P=P, F=F, Q=0)
print('x =', x)

x = [10.45  4.5 ]

这起作用了。如果我们连续多次调用predict()，则每次都会更新值。

for _ in range(4):x, P = predict(x=x, P=P, F=F, Q=0)print('x =', x)

x = [10.9  4.5]
x = [11.35  4.5 ]
x = [11.8  4.5]
x = [12.25  4.5 ]

predict()可以同时计算预测后的均值和协方差。这是五次预测后的 P \mathbf{P} P值，我们在卡尔曼滤波方程中表示 P ˉ \bar{\mathbf{P}} Pˉ。

print(P)

[[512.25  24.5 ][ 24.5   49.  ]]

通过观察对角线，我们发现位置方差变大了。我们已经完成了五次没有观测量更新的预测步骤，我们的不确定性增加了。非对角元素变为非零——卡尔曼滤波器检测到位置和速度之间的相关性！速度的方差没有改变。

这里我绘制了预测前后的协方差。初始值用红色表示，先验值（预测）用黑色虚线表示。我修改了协方差和时间步长，以便更好地说明这种变化。

from filterpy.stats import plot_covariance_ellipsedt = 0.3
F = np.array([[1, dt], [0, 1]])
x = np.array([10.0, 4.5])
P = np.diag([500, 500])
plot_covariance_ellipse(x, P, edgecolor='r')
x, P = predict(x, P, F, Q=0)
plot_covariance_ellipse(x, P, edgecolor='k', ls='dashed')

你可以看到椭圆的中心移动了一小部分（从 10 10 10到 11.35 11.35 11.35），这是因为位置改变了。椭圆也拉长了，显示了位置和速度之间的相关性。滤波器如何计算 P ˉ \bar{\mathbf{P}} Pˉ的新值，其依据是什么？请注意，我每次都将过程噪声 Q \mathbf{Q} Q设置为零，因此这不是因为我添加了噪声。现在讨论这个问题还为时过早，但请记住，到目前为止，在每个滤波器中，预测步都会导致信息丢失。这里也是如此。我们再谈一谈细节，我就告诉你。

设计过程噪声

快速回顾一下过程噪声。一辆汽车开着巡航控制在路上行驶，它应该以恒定的速度行驶。我们用 x ˉ k = x ˙ Δ t + x k − 1 \bar{x}_{k} = \dot{x}\Delta t + x_{k-1} xˉk=x˙Δt+xk−1对此进行建模。然而，它受到许多未知因素的影响，这导致巡航控制不能完全保持恒定的速度。例如：风会影响汽车，山丘和坑洼也会影响汽车；乘客们摇下车窗，改变汽车的阻力曲线等等。

我们可以用微分方程来模拟这个系统：

x ˙ = f ( x ) + w \dot{\mathbf{x}} = f(\mathbf{x}) + w x˙=f(x)+w

其中： f ( x ) f(\mathbf{x}) f(x)表示状态转移， w w w表示白过程噪声。

在后续的卡尔曼滤波数学一章中，我们将学习如何从一组微分方程转换到卡尔曼滤波矩阵。在本章中，我们利用牛顿公式已经为我们导出的运动方程。现在你只需要知道，我们通过将过程噪声协方差矩阵 Q \mathbf{Q} Q加到协方差 P \mathbf{P} P中，来解释系统中的噪声。我们不给 x \mathbf{x} x加任何东西，因为噪声是白色的，这意味着噪声的均值是0。如果均值是0， x \mathbf{x} x就不会改变。

一元卡尔曼滤波器使用 v a r i a n c e = v a r i a n c e + p r o c e s s _ n o i s e variance = variance + process\_noise variance=variance+process_noise来计算预测步后的方差；多元卡尔曼滤波器也是这样做的，基本上 P = P + Q P=P+Q P=P+Q。我说基本上，是因为协方差方程中还有其他与噪声无关的项，我们将在后面看到。

推导预测过程中的过程噪声矩阵可能要求很高，我们将推迟到卡尔曼数学一章。现在只需要知道 Q \mathbf{Q} Q等于白噪声 w w w的期望值，计算为 Q = E [ w w T ] \mathbf{Q} = \mathbb{E}[\mathbf{w}\mathbf{w}^{T}] Q=E[wwT]。在本章中，我们将集中于学习修改该矩阵后，滤波器的行为如何改变。

FilterPy为本章的运动学问题提供了计算 Q \mathbf{Q} Q的函数。离散白噪声（Q_discrete_white_noise）有3个参数： d i m dim dim指定了矩阵的维数， d t dt dt是以秒为单位的时间步， v a r var var是噪声的方差。简单地说，它将会对给定时间段内的噪声进行离散化。下面代码计算方差为 2.35 2.35 2.35，且时间步为 1 1 1秒的白噪声的Q：

from filterpy.common import Q_discrete_white_noiseQ = Q_discrete_white_noise(dim=2, dt=1., var=2.35)
print(Q)

[[0.5875 1.175 ][1.175  2.35  ]]

设计控制函数

卡尔曼滤波器还可以允许我们合并机器人和飞机等系统的控制输入。假设我们在控制一个机器人，在每一个时间步，我们都会根据机器人的当前位置和期望到达的位置，来向机器人发送转向和速度信号。卡尔曼滤波方程可以将这些信息结合到滤波方程中，根据当前速度和驱动电机的控制输入创建预测位置。记住，我们从不丢弃信息。

对于线性系统，控制输入的影响可以描述为一组线性方程组，我们可以用线性代数表示为：

Δ x = B u \Delta \mathbf{x} = \mathbf{B}\mathbf{u} Δx=Bu

这里 u \mathbf{u} u是控制输入， B \mathbf{B} B是控制输入模型或控制函数。例如， u \mathbf{u} u可能是一个控制车轮电机转动速度的电压，乘以 B \mathbf{B} B得到 Δ x \Delta \mathbf{x} Δx。换句话说，它必须计算状态量 x \mathbf{x} x因控制输入而发生的变化量。

因此，先验值的完整卡尔曼滤波方程是：

x ˉ = F x + B u \bar{\mathbf{x}} = \mathbf{F}\mathbf{x} + \mathbf{B}\mathbf{u} xˉ=Fx+Bu

这是当你调用KalmanFilter.predict()后的方程。

假设你的狗已经被训练了，可以响应语音命令，比如你说过来，它就会往你的位置移动（这就是控制输入）。现实是我的狗并没有任何控制输入，所以我把 B \mathbf{B} B设置为零。

B = 0.  # my dog doesn't listen to me!
u = 0
x, P = predict(x, P, F, Q, B, u)
print('x =', x)
print('P =', P)

x = [12.7  4.5]
P = [[680.587 301.175][301.175 502.35 ]]

代码上没有必要将 B \mathbf{B} B和 u \mathbf{u} u设置为0，因为predict()使用0作为其默认值：

predict(x, P, F, Q)[0] == predict(x, P, F, Q, B, u)[0]

array([ True,  True])

predict(x, P, F, Q)[1] == predict(x, P, F, Q, B, u)[1]

array([[ True,  True],[ True,  True]])

预测：总结

作为一名设计师，你的工作就是设计：

x \mathbf{x} x， P \mathbf{P} P：状态量和协方差矩阵
F \mathbf{F} F， Q \mathbf{Q} Q：过程模型和噪声协方差
B \mathbf{B} B， u \mathbf{u} u：可选的控制输入和功能

更新步

现在我们可以实现滤波器的更新步骤。你只需要再提供两个矩阵，它们很容易理解。

设计观测函数

卡尔曼滤波器在我们获取观测值后，需要在观测空间中，计算更新步。我们在一元卡尔曼一章中大多忽略了这个问题，因为它增加了复杂性。例如：我们用一个传感器记录狗的位置，计算残差很容易——从观测值中减去滤波器的预测位置：

r e s i d u a l = m e a s u r e d _ p o s i t i o n − p r e d i c t e d _ p o s i t i o n residual = measured\_position - predicted\_position residual=measured_position−predicted_position

我们需要计算残差，因为我们利用卡尔曼增益对残差进行缩放，以得到新的估计。

而如果我们试图用一个输出电压量与温度读数对应的的温度计来跟踪温度，会发生什么？残差计算的方程式是没有意义的：你不能从电压中减去温度。

r e s i d u a l = v o l t a g e − t e m p e r a t u r e ( ! ! ! N O N S E N S E ! ! ! ) residual = voltage - temperature (!!! NONSENSE !!!) residual=voltage−temperature(!!!NONSENSE!!!)

我们需要把温度转换成电压，这样才能进行减法运算。对于温度计，我们可以这样写：

CELSIUS_TO_VOLTS = 0.21475
residual = voltage - (CELSIUS_TO_VOLTS * predicted_temperature)

卡尔曼滤波器通过提供将状态转换为度量的观测函数，来解决这个问题。

我们为什么要在观测空间工作？为什么不在状态空间中工作，把电压转换成温度，让残余的温度变成温差？

我们不能这样做，因为大多数观测是不可逆的。跟踪问题的状态包含隐藏量 x ˙ \dot{x} x˙，我们无法将位置观测转换为包含速度的状态量。而另一方面，如果将包含位置和速度的状态，转换为仅包含位置的等效观测很简单。为了使残差的计算成为可能，我们必须在观测空间中工作。

观测量 z \mathbf{z} z和状态量 x \mathbf{x} x都是向量，所以我们需要使用矩阵来执行转换。执行此步骤的卡尔曼滤波方程为：

y = z − H x ˉ \mathbf{y} = \mathbf{z} - \mathbf{H}\bar{\mathbf{x}} y=z−Hxˉ

其中： y \mathbf{y} y是残差， x ˉ \bar{\mathbf{x}} xˉ是先验值， z \mathbf{z} z是观测值， H \mathbf{H} H是观测函数。所以我们取先验值，把它和 H \mathbf{H} H相乘，然后从观测值中减去。这表示：在观测空间中的预测值和观测值之间的差异！

我们需要设计 H \mathbf{H} H，以便 H x ˉ \mathbf{H}\bar{\mathbf{x}} Hxˉ产生一个观测值。对于上面跟踪狗的问题，假如我们有一个观测位置的传感器，所以 z \mathbf{z} z是一个一元向量：

z = [ z ] \mathbf{z} = \begin{bmatrix} z \end{bmatrix} z=[z]

残差方程的形式：

y = z − H x ˉ \mathbf{y} = \mathbf{z} - \mathbf{H}\bar{\mathbf{x}} y=z−Hxˉ

[ y ] = [ z ] − [ ? ? ] [ x x ˙ ] \begin{bmatrix} y \end{bmatrix} = \begin{bmatrix} z \end{bmatrix} - \begin{bmatrix} ? & ? \end{bmatrix} \begin{bmatrix} x \\ \dot{x} \end{bmatrix} [y]=[z]−[??][xx˙]

H \mathbf{H} H必须是 1 × 2 1\times 2 1×2矩阵， H x ˉ \mathbf{H}\bar{\mathbf{x}} Hxˉ才是 1 × 1 1\times 1 1×1。回想一下，将矩阵 m × n m\times n m×n乘以 n × p n\times p n×p得到一个 m × p m\times p m×p矩阵。

我们将把位置 x x x乘以1，得到相应的位置观测值。我们不需要使用速度来找到相应的观测值，所以我们将 x ˙ \dot{x} x˙乘以0。

y = z − [ 1 0 ] [ x x ˙ ] \mathbf{y} = \mathbf{z} - \begin{bmatrix} 1 & 0 \end{bmatrix} \begin{bmatrix} x \\ \dot{x} \end{bmatrix} y=z−[10][xx˙]

因此，对于我们的卡尔曼滤波器，我们设置：

H = [ 1 0 ] \mathbf{H} = \begin{bmatrix} 1 & 0 \end{bmatrix} H=[10]

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

我们已经设计了大部分的卡尔曼滤波器，剩下的就是对传感器中的噪声进行建模。

设计观测量

观测量用观测均值 z \mathbf{z} z和观测噪声协方差 R \mathbf{R} R来实现。

z \mathbf{z} z很简单，它将观测值表示成向量的形式即可。由于我们只有一个观测值，所以我们有：

z = [ z ] \mathbf{z} = \begin{bmatrix} z \end{bmatrix} z=[z]

如果我们有两个传感器的观测值，我们会有：

z = [ z 1 z 2 ] \mathbf{z} = \begin{bmatrix} z_{1} \\ z_{2} \end{bmatrix} z=[z1z2]

观测噪声矩阵，将传感器中的噪声建模为协方差矩阵。实际上，这可能很困难。一个复杂的系统可能有许多传感器，它们之间的相关性可能不清楚，通常它们的噪声不是纯高斯噪声。例如，如果温度很高，传感器可能会偏向于读取高值，因此噪声在均值的两侧分布不均匀。我们稍后将学习处理这些问题。

卡尔曼滤波方程使用观测噪声的协方差矩阵 R \mathbf{R} R。矩阵的尺寸为 m × m m\times m m×m，其中 m m m是传感器的数量。它是一个协方差矩阵，用于说明传感器之间的相关性。我们只有一个传感器，所以 R \mathbf{R} R是：

R = [ σ z 2 ] \mathbf{R} = \begin{bmatrix} \sigma_{z}^{2} \end{bmatrix} R=[σz2]

如果 σ z 2 \sigma_{z}^{2} σz2是5，我们得到 R = [ 5 ] \mathbf{R} = \begin{bmatrix} 5 \end{bmatrix} R=[5]。

如果我们有两个位置传感器，第一个方差为5，第二个方差为3，我们会这样写：

R = [ 5 0 0 3 ] \mathbf{R} = \begin{bmatrix} 5 & 0 \\ 0 & 3 \end{bmatrix} R=[5003]

因为这是一个协方差矩阵，我们把方差放在对角线上，如果有协方差的话，放在非对角线元素上。这里我们假设两个传感器之间的噪声没有相关性，所以协方差为0。

对于我们的问题，我们只有一个传感器，所以我们可以实现这一点：

R = np.array([[5.]])

我们通过调用update()来执行更新。

from filterpy.kalman import updatez = 1.
x, P = update(x, P, z, R, H)
print('x =', x)

[[1. 0.]] [[680.587 301.175][301.175 502.35 ]] [[5.]]
x = [ 1.085 -0.64 ]

这么多代码写起来还是很麻烦的，因此FilterPy提供了KalmanFilter类来实现滤波器。我之后将直接使用这个类，但是我想让你们看看这些函数的过程形式，因为我知道你们中的一些人不喜欢面向对象编程。

卡尔曼滤波的实现

我已经给了你滤波器的所有代码，但是现在让我们在一个地方总结整理它。首先我们构造一个KalmanFilter对象。我们必须用 d i m x dim_x dimx参数指定状态量的维度，用 d i m z dim_z dimz指定观测量的维度。我们有两个状态量和一个观测量：

from filterpy.kalman import KalmanFilterdog_filter = KalmanFilter(dim_x=2, dim_z=1)

这将为卡尔曼滤波器矩阵创建一个具有默认值的对象：

from filterpy.kalman import KalmanFilterdog_filter = KalmanFilter(dim_x=2, dim_z=1)
print('x = ', dog_filter.x.T)
print('R = ', dog_filter.R)
print('Q = \n', dog_filter.Q)
# etc...

x =  [[0. 0.]]
R =  [[1.]]
Q = [[1. 0.][0. 1.]]

现在我们初始化滤波器的矩阵和向量。我把它放在一个函数中，让你为 R \mathbf{R} R、 P \mathbf{P} P、 Q \mathbf{Q} Q指定不同的初始值。我们可能会创建和运行许多这样的滤波器，这为我们节省了很多麻烦。

from filterpy.kalman import KalmanFilter
from filterpy.common import Q_discrete_white_noisedef pos_vel_filter(x, P, R, Q=0., dt=1.0):""" Returns a KalmanFilter which implements aconstant velocity model for a state [x dx].T"""kf = KalmanFilter(dim_x=2, dim_z=1)kf.x = np.array([x[0], x[1]]) # location and velocitykf.F = np.array([[1., dt],[0.,  1.]])  # state transition matrixkf.H = np.array([[1., 0]])    # Measurement functionkf.R *= R                     # measurement uncertaintyif np.isscalar(P):kf.P *= P                 # covariance matrix else:kf.P[:] = P               # [:] makes deep copyif np.isscalar(Q):kf.Q = Q_discrete_white_noise(dim=2, dt=dt, var=Q)else:kf.Q[:] = Qreturn kf

KalmanFilter将 R \mathbf{R} R、 P \mathbf{P} P、 Q \mathbf{Q} Q初始化为单位矩阵，因此kf.P *= P是一种快速将所有对角元素赋给相同标量值的方法。现在我们创建滤波器：

dt = .1
x = np.array([0., 0.])
kf = pos_vel_filter(x, P=500, R=5, Q=0.1, dt=dt)

通过在命令行中输入变量，可以检查滤波器所有属性的当前值。

kf

KalmanFilter object
dim_x = 2
dim_z = 1
dim_u = 0
x = [0. 0.]
P = [[500.   0.][  0. 500.]]
x_prior = [[0. 0.]].T
P_prior = [[1. 0.][0. 1.]]
x_post = [[0. 0.]].T
P_post = [[1. 0.][0. 1.]]
F = [[1.  0.1][0.  1. ]]
Q = [[0.    0.   ][0.    0.001]]
R = [[5.]]
H = [[1. 0.]]
K = [[0. 0.]].T
y = [[0.]]
S = [[0.]]
SI = [[0.]]
M = [[0.]]
B = None
z = [[None]]
log-likelihood = -708.3964185322641
likelihood = 2.2250738585072014e-308
mahalanobis = 0.0
alpha = 1.0
inv = <function inv at 0x00000296F726A948>

剩下的就是编写运行卡尔曼滤波器的代码。

from kf_book.mkf_internal import plot_trackdef run(x0=(0.,0.), P=500, R=0, Q=0, dt=1.0, track=None, zs=None,count=0, do_plot=True, **kwargs):"""track is the actual position of the dog, zs are the corresponding measurements. """# Simulate dog if no data provided. if zs is None:track, zs = compute_dog_data(R, Q, count)# create the Kalman filterkf = pos_vel_filter(x0, R=R, P=P, Q=Q, dt=dt)  # run the kalman filter and store the resultsxs, cov = [], []for z in zs:kf.predict()kf.update(z)xs.append(kf.x)cov.append(kf.P)xs, cov = np.array(xs), np.array(cov)if do_plot:plot_track(xs[:, 0], track, zs, cov, **kwargs)return xs, cov

以上就是滤波器的完整代码，主要集中在run()函数，其他大部分基本上都是模板代码。让我们一行一行地看一遍。

第一行检查data中是否提供了观测量。如果没有，它将使用前面编写的compute_dog_data()函数创建数据。

下一行我们使用pos_vel_filter()函数，创建一个卡尔曼滤波器。

# create the Kalman filter
kf = pos_vel_filter(x0, R=R, P=P, Q=Q, dt=dt)

我们所需要做的就是不断地执行卡尔曼滤波器的预测和更新步骤。KalmanFilter类为此提供了两个方法predict()和update()。update()执行卡尔曼滤波器的观测更新步骤，因此它接受包含传感器的观测值。

在不需要存储结果的情况下，就只是循环进行以下操作：

for z in zs:kf.predict()kf.update(z)

每次调用predict()和update()都会修改状态量 x \mathbf{x} x和对应的协方差矩阵 P \mathbf{P} P。因此，在调用predict()之后，kf.x表示的是先验值；在调用update()之后，kf.x表示的是后验值。data包含狗的实际位置和观测值，因此我们可以使用 [ : , 1 ] [:, 1] [:,1]来获得一个观测值数组。

这真的再简单不过了。当我们处理更复杂的问题时，这段代码也基本上可以保持不变。所有的工作都是建立KalmanFilter类对象，然后再执行滤波器。

代码的其余部分就是存储数据、打印结果，然后返回保存的状态量和协方差矩阵。

我们开始运行吧。假如我们有 50 50 50个观测值，噪声方差为 10 10 10，过程方差为 0.01 0.01 0.01。

P = np.diag([500., 49.])
Ms, Ps = run(count=50, R=10, Q=0.01, P=P)

尽管还有很多东西要学，但我们已经可以使用理论和公式实现了一个卡尔曼滤波器！你的GPS、飞机、机器人等等里面很多都运行这样的类似代码。

第一个图绘制了狗的实际位置（标记轨迹）、观测值和卡尔曼滤波器的输出。在初始稳定期后，滤波器应非常紧密地跟踪狗的位置。黑色虚线之间的黄色阴影部分显示了滤波器方差的标准偏差，我将在下一段中解释。

接下来的两个图显示了 x x x和 x ˙ \dot{x} x˙的方差，也就是 P \mathbf{P} P的对角线。回想一下，协方差矩阵的对角线包含每个状态量的方差。所以 P [ 0 , 0 ] \mathbf{P}[0,0] P[0,0]是 x x x的方差， P [ 1 , 1 ] \mathbf{P}[1,1] P[1,1]是 x ˙ \dot{x} x˙的方差。你可以看到，很快收敛到很小。

协方差矩阵 P \mathbf{P} P告诉我们滤波器的理论性能，假设我们所说的一切都是真的。回想一下，标准差是方差的平方根，高斯分布大约 68 % 68\% 68%都出现在一个标准差内。如果至少 68 % 68\% 68%的滤波器输出在一个标准偏差内，则滤波器可能表现良好。在顶部图表中，我将一个标准偏差显示为两条虚线之间的黄色阴影区域。在我看来，可能滤波器稍微超出了这个界限，所以滤波器可能需要一些调整。也就是说， P \mathbf{P} P的值并不能真实反映当前滤波器的实际效果。

在一元卡尔曼一章中，我们使用比上述代码简单得多的代码滤波了非常嘈杂的信号。但是，请注意，当时我们针对的是一个非常简单的示例——一个在一维空间中移动的对象和一个传感器。这就是我们可以用上一章的代码计算的极限。相反，我们可以用本章的代码实现非常复杂的多元滤波器，只需改变对滤波器变量的赋值。也许我们想追踪金融模型中的100个维度，或者我们有一架带有GPS、INS、TACAN、雷达高度计、气压高度计和空速指示器的飞机，我们想把所有这些传感器整合到一个模型中，在3D空间中预测位置、速度和加速度。

为了让你更好地了解状态量（高斯表示）是如何随时间变化的，所以这里有一个3D图，显示了每7个时间步长的状态量的变化。每七个时间步长是为了将它们足够分开，可以独立地看到每一个。第一个状态量 t = 0 t=0 t=0在3D图的最左边。

from kf_book.book_plots import set_figsize, figsize
from kf_book.nonlinear_plots import plot_gaussiansP = np.diag([3., 1.])
np.random.seed(3)
Ms, Ps = run(count=25, R=10, Q=0.01, P=P, do_plot=False)
with figsize(x=9, y=5):plot_gaussians(Ms[::7], Ps[::7], (-5,25), (-5, 5), 75)

Saver类

在run()方法中，我手动编写了代码来保存滤波器的结果：

xs, cov = [], []
for z in zs:kf.predict()kf.update(z)xs.append(kf.x)cov.append(kf.P)xs, cov = np.array(xs), np.array(cov)

有一个更简单的方法可以实现。filtery.common提供了Saver类来保存KalmanFilter类中所有属性，通过调用Saver类Saver.save()函数。让我们先实际看看它的用法：

from filterpy.common import Saverkf = pos_vel_filter([0, .1], R=R, P=P, Q=Q, dt=1.)
s = Saver(kf)
for i in range(1, 6):kf.predict()kf.update([i])s.save()  # save the current state

Saver对象现在包含KalmanFilter对象的所有属性的列表。kf.x是滤波器的当前状态量的估计。因此，s.x包含在循环内计算的已保存状态估计：

s.x

[array([0.531, 0.304]),array([1.555, 0.763]),array([2.784, 1.036]),array([3.944, 1.105]),array([5.015, 1.086])]

你也可以使用keys属性查看所有可用属性：

s.keys

['alpha','likelihood','log_likelihood','mahalanobis','dim_x','dim_z','dim_u','x','P','Q','B','F','H','R','_alpha_sq','M','z','K','y','S','SI','_I','x_prior','P_prior','x_post','P_post','_log_likelihood','_likelihood','_mahalanobis','inv']

有许多属性尽管我们没有讨论，但绝大多数都应该可以通过名称来知道其含义的。

此时，你可以编写代码来绘制这些变量中的任何一个。但是，使用np.array比list通常更有用。调用Saver.to_array()将list转换为np.array。

如果你再看一遍keys，你会发现观测值 z z z也被保存了。让我们把它与估计值作比较：

import matplotlib.pyplot as plts.to_array()
book_plots.plot_measurements(s.z)
plt.plot(s.x[:, 0])

使用Saver会减慢代码的速度，但是对于学习和调试来说，这种方便是无与伦比的。

卡尔曼滤波方程

现在我们可以学习predict()和update()如何执行计算了。

预测方程

卡尔曼滤波器利用这些方程来计算先验值——对系统预测的下一个状态，它用先验值（ x ˉ \bar{\mathbf{x}} xˉ）和协方差（ P ˉ \bar{\mathbf{P}} Pˉ）来表示：

x ˉ = F x + B u \bar{\mathbf{x}} = \mathbf{F}\mathbf{x} + \mathbf{B}\mathbf{u} xˉ=Fx+Bu

P ˉ = F P F T + Q \bar{\mathbf{P}} = \mathbf{F}\mathbf{P}\mathbf{F}^{T} + \mathbf{Q} Pˉ=FPFT+Q

先验值

x ˉ = F x + B u \bar{\mathbf{x}} = \mathbf{F}\mathbf{x} + \mathbf{B}\mathbf{u} xˉ=Fx+Bu

作为提醒，线性方程 A x = b \mathbf{A}\mathbf{x} = \mathbf{b} Ax=b表示一个方程组，其中 A \mathbf{A} A表示方程组的系数， x \mathbf{x} x表示变量向量。执行乘法 A x \mathbf{A}\mathbf{x} Ax计算得到的值，用 b \mathbf{b} b表示。

如果 F \mathbf{F} F包含给定时间步长的状态转换，那么乘积 F x \mathbf{F}\mathbf{x} Fx计算该转换之后的状态。容易的！同样， B \mathbf{B} B是控制函数， u \mathbf{u} u是控制输入，因此 B u \mathbf{B}\mathbf{u} Bu计算控制输出对转换后状态的贡献。因此，前面的 x ˉ \bar{\mathbf{x}} xˉ计算为 F x \mathbf{F}\mathbf{x} Fx和 B u \mathbf{B}\mathbf{u} Bu之和。

等价的一元方程是：

μ ˉ = μ + μ m o v e \bar{\mu} = \mu + \mu_{move} μˉ=μ+μmove

如果你执行矩阵乘法 F x \mathbf{F}\mathbf{x} Fx，它会为 x x x生成类似的方程。

让我们把这个说清楚。回想一下上一章中 F \mathbf{F} F的值：

F = [ 1 Δ t 0 1 ] \mathbf{F} = \begin{bmatrix} 1 & \Delta t \\ 0 & 1 \end{bmatrix} F=[10Δt1]

因此 x ˉ = F x \bar{\mathbf{x}} = \mathbf{F}\mathbf{x} xˉ=Fx对应于线性方程组：

{ x ˉ = 1 x + Δ t x ˙ x ˙ = 0 x + 1 x ˙ \left\{\begin{matrix} \bar{x} = 1x + \Delta t\dot{x} \\ \dot{x} = 0x + 1\dot{x} \end{matrix}\right. {xˉ=1x+Δtx˙x˙=0x+1x˙

协方差

P ˉ = F P F T + Q \bar{\mathbf{P}} = \mathbf{F}\mathbf{P}\mathbf{F}^{T} + \mathbf{Q} Pˉ=FPFT+Q

这个等式不太容易理解，所以我们会花更多的时间在上面。

这个方程的一元形式是：

σ ˉ 2 = σ 2 + σ m o v e 2 \bar{\sigma}^{2} = \sigma^{2} + \sigma_{move}^{2} σˉ2=σ2+σmove2

我们在估计值的方差中加上变动的方差，以反映信息的损失。我们需要做同样的事情，但是多元高斯函数不是那么容易。

我们不能简单地写 P ˉ = P + Q \bar{\mathbf{P}} = \mathbf{P} + \mathbf{Q} Pˉ=P+Q。在多元高斯模型中，状态量是相关的。这意味着什么？我们对速度的了解是不完善的，但我们正在用它来增加位置：

x ˉ = x + x ˙ Δ t \bar{x} = x + \dot{x}\Delta t xˉ=x+x˙Δt

因为我们对 x ˙ \dot{x} x˙的值没有完全的了解，所以 x ˉ = x + x ˙ Δ t \bar{x} = x + \dot{x}\Delta t xˉ=x+x˙Δt的和就获得了不确定性。因为位置和速度是相关的，我们不能简单地加上协方差矩阵。例如，如果 P \mathbf{P} P和 Q \mathbf{Q} Q是对角矩阵，那么和也是对角的。但是我们知道位置和速度相关，所以非对角元素应该是非零的。

正确的公式是：

P ˉ = F P F T + Q \bar{\mathbf{P}} = \mathbf{F}\mathbf{P}\mathbf{F}^{T} + \mathbf{Q} Pˉ=FPFT+Q

A B A T \mathbf{A}\mathbf{B}\mathbf{A}^{T} ABAT形式的表达式在线性代数中很常见。你可以把它看作是把中间项和外部项联系起来，我承认这对你来说可能是个神奇的等式，让我们来探索一下。

当我们初始化 P \mathbf{P} P：

P = [ σ x 2 0 0 σ v 2 ] \mathbf{P} = \begin{bmatrix} \sigma_{x}^{2} & 0 \\ 0 & \sigma_{v}^{2} \end{bmatrix} P=[σx200σv2]

F P F T \mathbf{F}\mathbf{P}\mathbf{F}^{T} FPFT的值为：

F P F T = [ 1 Δ t 0 1 ] [ σ x 2 0 0 σ v 2 ] [ 1 0 Δ t 1 ] \mathbf{F}\mathbf{P}\mathbf{F}^{T} = \begin{bmatrix} 1 & \Delta t \\ 0 & 1 \end{bmatrix} \begin{bmatrix} \sigma_{x}^{2} & 0 \\ 0 & \sigma_{v}^{2} \end{bmatrix} \begin{bmatrix} 1 & 0 \\ \Delta t & 1 \end{bmatrix} FPFT=[10Δt1][σx200σv2][1Δt01]

= [ σ x 2 σ v 2 Δ t 0 σ v 2 ] [ 1 0 Δ t 1 ] = \begin{bmatrix} \sigma_{x}^{2} & \sigma_{v}^{2}\Delta t \\ 0 &\sigma_{v}^{2} \end{bmatrix}\begin{bmatrix} 1 & 0 \\ \Delta t & 1 \end{bmatrix} =[σx20σv2Δtσv2][1Δt01]

= [ σ x 2 + σ v 2 Δ t 2 σ v 2 Δ t σ v 2 Δ t σ v 2 ] = \begin{bmatrix} \sigma_{x}^{2} + \sigma_{v}^{2}\Delta t^{2} & \sigma_{v}^{2}\Delta t \\ \sigma_{v}^{2}\Delta t & \sigma_{v}^{2} \end{bmatrix} =[σx2+σv2Δt2σv2Δtσv2Δtσv2]

P \mathbf{P} P的初始值在位置和速度之间没有协方差。由于位置预测计算公式为 x + x ˙ Δ t x + \dot{x}\Delta t x+x˙Δt，因此位置和速度之间存在一定的相关性。乘法 F P F T \mathbf{F}\mathbf{P}\mathbf{F}^{T} FPFT计算了协方差 σ v 2 Δ t \sigma_{v}^{2}\Delta t σv2Δt。精确的值并不重要，你只需要认识到 F P F T \mathbf{F}\mathbf{P}\mathbf{F}^{T} FPFT使用过程模型来自动计算位置和速度之间的协方差！

思考这个问题的另一种方式是对比 F x \mathbf{F}\mathbf{x} Fx的乘法操作， F P \mathbf{F}\mathbf{P} FP似乎是等价的运算。但 P \mathbf{P} P是矩阵， x \mathbf{x} x是向量。后面的 F T \mathbf{F}^{T} FT项确保我们同时乘以 F \mathbf{F} F的行和列。在上面计算 F P F T \mathbf{F}\mathbf{P}\mathbf{F}^{T} FPFT过程的第二行中，我们得到了 F P \mathbf{F}\mathbf{P} FP的值。你可以看到它是一个上三角矩阵，因为我们还没有把 F \mathbf{F} F完全合并到乘法中。

如果你有一些线性代数和统计学的经验，下面的计算过程可能会有所帮助。由预测产生的协方差可以建模为预测步中误差的期望值，由该方程给出:

P ˉ = E [ ( F x − μ ) ( F x − μ ) T ] \bar{\mathbf{P}} = \mathbb{E}[(\mathbf{F}\mathbf{x} - \mu)(\mathbf{F}\mathbf{x} - \mu)^{T}] Pˉ=E[(Fx−μ)(Fx−μ)T]

= F E [ ( x − μ ) ( x − μ ) T ] F T = \mathbf{F}\mathbb{E}[(\mathbf{x} - \mu)(\mathbf{x} - \mu)^{T}]\mathbf{F}^{T} =FE[(x−μ)(x−μ)T]FT

当然， E [ ( x − μ ) ( x − μ ) T ] \mathbb{E}[(\mathbf{x} - \mu)(\mathbf{x} - \mu)^{T}] E[(x−μ)(x−μ)T]就是 P \mathbf{P} P，给我们：

P ˉ = F P F T \bar{\mathbf{P}} = \mathbf{F}\mathbf{P}\mathbf{F}^{T} Pˉ=FPFT

让我们看看它的效果。在这里，我使用滤波器中的 F \mathbf{F} F，将时间间隔设置成 0.6 0.6 0.6。我做了五次，这样你就可以看到 P \mathbf{P} P是如何继续变化的。

dt = 0.6
x = np.array([0., 5.])
F = np.array([[1., dt], [0, 1.]])
P = np.array([[1.5, 0], [0, 3.]])
plot_covariance_ellipse(x, P, edgecolor='r')for _ in range(5):x = F @ xP = F @ P @ F.Tplot_covariance_ellipse(x, P, edgecolor='k', ls='dashed')
book_plots.set_labels(x='position', y='velocity')

你可以看到，当速度为5时，位置在每 0.6 s 0.6s 0.6s的间隔中正确地移动了3个单位。在每一步后，椭圆的宽度都逐渐较大，这表明由于在每一步中向 x x x加上 x ˙ Δ t \dot{x}\Delta t x˙Δt，我们丢失了某些关于位置的信息。高度没有改变——我们的系统模型说速度没有改变。随着时间的推移，你可以看到椭圆变得越来越倾斜。回想一下，倾斜表示相关性。 F \mathbf{F} F将 x x x与 x ˙ \dot{x} x˙线性相关，表达式为 x ˉ = x + x ˙ Δ t \bar{x} = x + \dot{x}\Delta t xˉ=x+x˙Δt。 A B A T \mathbf{A}\mathbf{B}\mathbf{A}^{T} ABAT计算，将这种相关性正确地合并到协方差矩阵中。

下面是这个等式的交互动画，可以让你改变 F \mathbf{F} F的值，看看它是如何影响 P \mathbf{P} P的形状的。F00影响 F [ 0 , 0 ] F[0, 0] F[0,0]的值，covar设置位置和速度之间的初始协方差（ σ x x ˙ \sigma_{x\dot{x}} σxx˙）。我建议至少回答这些问题：

如果 x x x与 x ˙ \dot{x} x˙不相关怎么办？（将F01设置为0，其余为默认值）
如果 x = 2 x ˙ Δ t + x 0 x = 2\dot{x}\Delta t + x_{0} x=2x˙Δt+x0怎么办？（将F01设置为2，其余为默认值）
如果 x = x ˙ Δ t + 2 x 0 x = \dot{x}\Delta t + 2x_{0} x=x˙Δt+2x0怎么办？（将F00设置为2，其余为默认值）
如果 x = x ˙ Δ t x = \dot{x}\Delta t x=x˙Δt怎么办？（将F00设置为0，其余为默认值）

from ipywidgets import interact
from kf_book.book_plots import IntSlider, FloatSliderdef plot_FPFT(F00, F01, F10, F11, covar):   plt.figure()dt = 1.x = np.array((0, 0.))P = np.array(((1, covar), (covar, 2)))F = np.array(((F00, F01), (F10, F11)))plot_covariance_ellipse(x, P)plot_covariance_ellipse(x, F @ P @ F.T, ec='r')plt.gca().set_aspect('equal')plt.xlim(-4, 4)plt.ylim(-4, 4)#plt.title(str(F))plt.xlabel('position')plt.ylabel('velocity')interact(plot_FPFT, F00=IntSlider(value=1, min=0, max=2), F01=FloatSlider(value=1, min=0, max=2, description='F01(dt)'),F10=FloatSlider(value=0, min=0, max=2),F11=FloatSlider(value=1, min=0, max=2),covar=FloatSlider(value=0, min=0, max=1))

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

更新方程

更新方程看起来比预测方程更混乱，但这主要是由于卡尔曼滤波器在观测空间计算更新，这是因为观测值是不可逆的。例如，考虑一个传感器，它提供目标的距离。把一个距离转换成一个位置是不可能的——一个圆中的无限多个位置将产生到圆心相同的距离。另一方面，我们总是可以计算给定位置的距离（观测）。

在我继续之前，回想一下我们正在尝试做一件非常简单的事情：在观测值和预测值之间选择一个新的估计值，如下图所示：

方程会很复杂，因为状态有多个维度，但这个操作就是我们要做的。不要让这些方程式分散你对这个简单想法的注意力。

系统不确定性

S = H P ˉ H T + R \mathbf{S} = \mathbf{H}\bar{\mathbf{P}}\mathbf{H}^{T} + \mathbf{R} S=HPˉHT+R

为了在观测空间工作，卡尔曼滤波器必须将协方差矩阵投影到观测空间。其数学表达式为 H P ˉ H T \mathbf{H}\bar{\mathbf{P}}\mathbf{H}^{T} HPˉHT，其中 P ˉ \mathbf{\bar{P}} Pˉ是先验协方差， H \mathbf{H} H是观测函数。

你应该认识到这个 A B A T \mathbf{A}\mathbf{B}\mathbf{A}^{T} ABAT类型——在预测步使用状态转换函数更新 P \mathbf{P} P： F P F T \mathbf{F}\mathbf{P}\mathbf{F}^{T} FPFT。在这里，我们使用观测函数的相同形式来更新它。线性代数的方法正在为我们改变坐标系。

一旦协方差在观测空间中，我们就需要考虑传感器噪声，结果被称为系统不确定性。计算起来很简单——我们只需要直接加上一个矩阵。

如果忽略 H \mathbf{H} H项，则该方程相当于卡尔曼增益的一元卡尔曼方程中的分母：

K = σ ˉ 2 σ ˉ 2 + σ z 2 K = \frac{\bar{\sigma}^{2}}{\bar{\sigma}^{2} + \sigma_{z}^{2}} K=σˉ2+σz2σˉ2

比较系统不确定性和协方差的方程：

S = H P ˉ H T + R \mathbf{S} = \mathbf{H}\bar{\mathbf{P}}\mathbf{H}^{T} + \mathbf{R} S=HPˉHT+R

P ˉ = F P F T + Q \bar{\mathbf{P}} = \mathbf{F}\mathbf{P}\mathbf{F}^{T} + \mathbf{Q} Pˉ=FPFT+Q

在每一个方程中， P \mathbf{P} P用函数 H \mathbf{H} H或 F \mathbf{F} F放入不同的空间。然后我们添加与该空间相关联的噪声矩阵。

卡尔曼增益

K = P ˉ H T S − 1 \mathbf{K} = \bar{\mathbf{P}}\mathbf{H}^{T}\mathbf{S}^{-1} K=PˉHTS−1

回顾一下残差图。一旦我们有了预测值和观测值，我们就需要在两者之间选择一个估计值。如果我们对观测值更有把握，估计值就会更接近它。相反，如果我们对预测值有更多的确定性，那么估计值就会更接近它。

在一元卡尔曼一章中，我们用这个方程来计算均值：

μ = σ ˉ 2 μ z + σ z 2 μ ˉ σ ˉ 2 + σ z 2 \mu = \frac{\bar{\sigma}^{2}\mu_{z} + \sigma_{z}^{2}\bar{\mu}}{\bar{\sigma}^{2} + \sigma_{z}^{2}} μ=σˉ2+σz2σˉ2μz+σz2μˉ

我们简化为：

μ = ( 1 − K ) μ ˉ + K μ z \mu = (1 - K)\bar{\mu} + K\mu_{z} μ=(1−K)μˉ+Kμz

我们定义：

K = σ ˉ 2 σ ˉ 2 + σ z 2 K = \frac{\bar{\sigma}^{2}}{\bar{\sigma}^{2} + \sigma_{z}^{2}} K=σˉ2+σz2σˉ2

K K K是卡尔曼增益，它是一个介于0和1之间的实数。确保你了解它是如何在预测值和观测值之间选择均值的。卡尔曼增益是一个百分比或比率——如果 K K K是0.9，则需要 90 % 90\% 90%的观测值和 10 % 10\% 10%的预测值。

对于多元卡尔曼滤波器， K \mathbf{K} K是一个向量，而不是一个标量。下面是公式： K = P ˉ H T S − 1 \mathbf{K} = \bar{\mathbf{P}}\mathbf{H}^{T}\mathbf{S}^{-1} K=PˉHTS−1。这是一个比率吗？我们可以把矩阵的逆看作线性代数求倒数的方法。矩阵没有定义除法操作，但是用这种方式来类比思考它是有用的。所以我们可以把K的方程理解为一下意义：

K ≈ P ˉ H T S \mathbf{K} \approx \frac{\bar{\mathbf{P}}\mathbf{H}^{T}}{\mathbf{S}} K≈SPˉHT

K ≈ u n c e r t a i n t y p r e d i c t i o n u n c e r t a i n t y p r e d i c t i o n + u n c e r t a i n t y m e a s u r e m e n t H T \mathbf{K} \approx \frac{uncertainty_{prediction}}{uncertainty_{prediction} + uncertainty_{measurement}}\mathbf{H}^{T} K≈uncertaintyprediction+uncertaintymeasurementuncertaintypredictionHT

卡尔曼增益方程根据我们对预测值和观测值的信任程度来计算一个比率，我们在前面的每一章都做了同样的事情。这个方程很复杂，因为我们是通过矩阵在多个维度上进行的，但是概念很简单。 H T \mathbf{H}^{T} HT术语不太清楚，我很快会解释的。如果忽略该项，则卡尔曼增益的公式与一元的情况相同：将先验不确定度除以先验不确定度和观测不确定度之和。

残差

y = z − H x ˉ \mathbf{y} = \mathbf{z} - \mathbf{H}\bar{\mathbf{x}} y=z−Hxˉ

这是一个简单的问题，因为我们在设计观测函数 H \mathbf{H} H时已经讨论了这个方程。回想一下，观测函数将状态量转换为观测量，所以 H x \mathbf{H}\mathbf{x} Hx把 x \mathbf{x} x转换成一个等效的观测值。一旦完成了，我们就可以从观测值 z \mathbf{z} z中减去它，得到残差，即观测值和预测值之间的差值。

一元方程是：

y = z − x ˉ y = z - \bar{x} y=z−xˉ

显然是在计算同一个东西，但只在一个维度上。

状态更新

x = x ˉ + K y \mathbf{x} = \bar{\mathbf{x}} + \mathbf{K}\mathbf{y} x=xˉ+Ky

我们选择我们的新状态量：沿着残差，由卡尔曼增益缩放。缩放的大小由 K y \mathbf{K}\mathbf{y} Ky决定，它既对残差进行标度，又利用 K \mathbf{K} K的 H T \mathbf{H}^{T} HT将残差转换回状态空间。这加上先验，得到公式： x = x ˉ + K y \mathbf{x} = \bar{\mathbf{x}} + \mathbf{K}\mathbf{y} x=xˉ+Ky。让我把 K \mathbf{K} K展开，这样我们可以看到整个计算过程：

x = x ˉ + K y \mathbf{x} = \bar{\mathbf{x}} + \mathbf{K}\mathbf{y} x=xˉ+Ky

= x ˉ + P ˉ H T S − 1 y = \bar{\mathbf{x}} + \bar{\mathbf{P}}\mathbf{H}^{T}\mathbf{S}^{-1}\mathbf{y} =xˉ+PˉHTS−1y

≈ x ˉ + u n c e r t a i n t y p r e d i c t i o n u n c e r t a i n t y p r e d i c t i o n + u n c e r t a i n t y m e a s u r e m e n t H T y \approx \bar{\mathbf{x}} + \frac{uncertainty_{prediction}}{uncertainty_{prediction} + uncertainty_{measurement}}\mathbf{H}^{T}\mathbf{y} ≈xˉ+uncertaintyprediction+uncertaintymeasurementuncertaintypredictionHTy

或许更好的方法是重写估算公式：

x = x ˉ + K y \mathbf{x} = \bar{\mathbf{x}} + \mathbf{K}\mathbf{y} x=xˉ+Ky

= x ˉ + K ( z − H x ˉ ) = \bar{\mathbf{x}} + \mathbf{K}(\mathbf{z} - \mathbf{H}\bar{\mathbf{x}}) =xˉ+K(z−Hxˉ)

= ( I − K H ) x ˉ + K z = (\mathbf{I} - \mathbf{K}\mathbf{H})\bar{\mathbf{x}} + \mathbf{K}\mathbf{z} =(I−KH)xˉ+Kz

这和一元卡尔曼形式之间的相似性应该很明显：

μ = ( 1 − K ) μ ˉ + K μ z \mu = (1 - K)\bar{\mu} + K\mu_{z} μ=(1−K)μˉ+Kμz

协方差更新

P = ( I − K H ) P ˉ \mathbf{P} = (\mathbf{I} - \mathbf{K}\mathbf{H})\bar{\mathbf{P}} P=(I−KH)Pˉ

I \mathbf{I} I是单位矩阵，是我们在多维中表示1的方式。 H \mathbf{H} H是我们的观测函数，是一个常数。我们可以把这个方程看作 P = ( 1 − c K ) P ˉ \mathbf{P} = (1 - c\mathbf{K})\bar{\mathbf{P}} P=(1−cK)Pˉ。 K \mathbf{K} K是我们使用的预测与观测的比率。如果 K \mathbf{K} K很大，那么 1 − c K 1 - c\mathbf{K} 1−cK就很小， P \mathbf{P} P就会比原来小；如果 K \mathbf{K} K很小，那么 1 − c K 1 - c\mathbf{K} 1−cK很大，而 P \mathbf{P} P相对较大。这意味着我们通过卡尔曼增益来调整不确定性的大小。

这个方程在数值上是不稳定的，我不在FilterPy中使用它。减法会破坏对称性，并随着时间的推移导致浮点错误。在之后的卡尔曼数学一章中，我将分享这个方程更复杂但数值稳定的形式。

不使用FilterPy的示例

FilterPy对我们隐藏了实现的细节。通常你会喜欢这个，但是让我们实现没有FilterPy的滤波器。为此，我们需要将矩阵定义为变量，然后显式地实现卡尔曼滤波方程。

这里我们初始化矩阵：

dt = 1.
R_var = 10
Q_var = 0.01
x = np.array([[10.0, 4.5]]).T
P = np.diag([500, 49])
F = np.array([[1, dt],[0,  1]])
H = np.array([[1., 0.]])
R = np.array([[R_var]])
Q = Q_discrete_white_noise(dim=2, dt=dt, var=Q_var)

from scipy.linalg import invcount = 50
track, zs = compute_dog_data(R_var, Q_var, count)
xs, cov = [], []
for z in zs:# predictx = F @ xP = F @ P @ F.T + Q#updateS = H @ P @ H.T + RK = P @ H.T @ inv(S)y = z - H @ xx += K @ yP = P - K @ H @ Pxs.append(x)cov.append(P)xs, cov = np.array(xs), np.array(cov)
plot_track(xs[:, 0], track, zs, cov, plot_P=False)

结果与FilterPy版本基本相同。你喜欢哪一种取决于你自己。我不喜欢用 x x x、 P P P等变量污染我的命名空间，dog_filter.x对我来说更具可读性。

更重要的是，这个例子需要你记住卡尔曼滤波方程。然而，你迟早会犯错误的，FilterPy的版本确保你的代码是正确的。另一方面，如果你在代码中犯了一个错误，例如使 H \mathbf{H} H成为列向量而不是行向量，FilterPy的错误消息将比这个显式代码更难调试。

FilterPy的KalmanFilter类提供了额外的功能，如平滑、批处理、内存过滤、最大似然函数的计算等。你无需显式编程即可获得所有这些功能。

总结

我们学习了卡尔曼滤波方程，下面内容都是供你复习的。尽管还有很多东西要学，但我希望当你检查了每一个方程时，会发现它和一元滤波器中的亲缘关系。在卡尔曼数学的一章中，我将向你们展示，如果我们把 x \mathbf{x} x的维数设为1，这些方程将恢复为一元滤波器的方程。

预测步：

x ˉ = F x + B u \bar{\mathbf{x}} = \mathbf{F}\mathbf{x} + \mathbf{B}\mathbf{u} xˉ=Fx+Bu

P ˉ = F P F T + Q \bar{\mathbf{P}} = \mathbf{F}\mathbf{P}\mathbf{F}^{T} + \mathbf{Q} Pˉ=FPFT+Q

更新步：

S = H P ˉ H T + R \mathbf{S} = \mathbf{H}\bar{\mathbf{P}}\mathbf{H}^{T} + \mathbf{R} S=HPˉHT+R

K = P ˉ H T S − 1 \mathbf{K} = \bar{\mathbf{P}}\mathbf{H}^{T}\mathbf{S}^{-1} K=PˉHTS−1

y = z − H x ˉ \mathbf{y} = \mathbf{z} - \mathbf{H}\bar{\mathbf{x}} y=z−Hxˉ

x = x ˉ + K y \mathbf{x} = \bar{\mathbf{x}} + \mathbf{K}\mathbf{y} x=xˉ+Ky

P = ( I − K H ) P ˉ \mathbf{P} = (\mathbf{I} - \mathbf{K}\mathbf{H})\bar{\mathbf{P}} P=(I−KH)Pˉ

我想和大家分享一组你们将在文献中可能看到的方程式。它使用不同的符号系统来表示，但你也要明白这是一个什么样的想法。

x ^ k ∣ k − 1 = F k x ^ k − 1 ∣ k − 1 + B k u k \hat{\mathbf{x}}_{k|k-1} = \mathbf{F}_{k}\hat{\mathbf{x}}_{k-1|k-1} + \mathbf{B}_{k}\mathbf{u}_{k} x^k∣k−1=Fkx^k−1∣k−1+Bkuk

P k ∣ k − 1 = F k P k − 1 ∣ k − 1 F k T + Q k \mathbf{P}_{k|k-1} = \mathbf{F}_{k}\mathbf{P}_{k-1|k-1}\mathbf{F}_{k}^{T} + \mathbf{Q}_{k} Pk∣k−1=FkPk−1∣k−1FkT+Qk

y ~ k = z k − H k x ^ k ∣ k − 1 \tilde{\mathbf{y}}_{k} = \mathbf{z}_{k} - \mathbf{H}_{k}\hat{\mathbf{x}}_{k|k-1} y~k=zk−Hkx^k∣k−1

S k = H k P k ∣ k − 1 H k T + R k \mathbf{S}_{k} = \mathbf{H}_{k}\mathbf{P}_{k|k-1}\mathbf{H}_{k}^{T} + \mathbf{R}_{k} Sk=HkPk∣k−1HkT+Rk

K k = P k ∣ k − 1 H k T S k − 1 \mathbf{K}_{k} = \mathbf{P}_{k|k-1}\mathbf{H}_{k}^{T}\mathbf{S}_{k}^{-1} Kk=Pk∣k−1HkTSk−1

x ^ k ∣ k = x ^ k ∣ k − 1 + K k y ~ k \hat{\mathbf{x}}_{k|k} = \hat{\mathbf{x}}_{k|k-1} + \mathbf{K}_{k}\tilde{\mathbf{y}}_{k} x^k∣k=x^k∣k−1+Kky~k

P k ∣ k = ( I − K k H k ) P k ∣ k − 1 \mathbf{P}_{k|k} = (\mathbf{I} - \mathbf{K}_{k}\mathbf{H}_{k})\mathbf{P}_{k|k-1} Pk∣k=(I−KkHk)Pk∣k−1

这种表示法使用贝叶斯 a ∣ b a|b a∣b表示法，这意味着给定了b条件下的a。 ^ \hat{} ^表示估计。 x ^ k ∣ k \hat{\mathbf{x}}_{k|k} x^k∣k表示给定第 k k k步（第二个 k k k）的条件下，第 k k k步（第一个 k k k）的 x \mathbf{x} x状态的估计，也就是后验。 x ^ k ∣ k − 1 \hat{\mathbf{x}}_{k|k-1} x^k∣k−1表示给定第 k − 1 k-1 k−1步的条件下，第 k k k步的 x \mathbf{x} x状态的估计，也就是先验。

这个符号允许数学家精确地表达自己的想法，在正式出版物中，这种精确性是必要的。但是作为一个程序员，我觉得这很难理解。我习惯于在程序运行时考虑变量状态的变化，并且不会对每个新的计算使用不同的变量名。在文献中并没有约定的格式，因此每个作者做出不同的选择。

符号附录列出了不同作者使用的符号，这带来了另一个困难。不同的作者使用不同的变量名： x \mathbf{x} x是相当普遍的，但也有很多并不是很普遍。例如，通常使用 A \mathbf{A} A表示我所称的 F \mathbf{F} F。你必须仔细阅读，并希望作者定义他们的变量（他们往往没有）。

如果你是一个试图理解一篇论文方程式的程序员，我建议你先去掉所有的上标、下标和其他特殊符号，单独看字母。如果你每天都用这样的方程，这是多余的建议，但当我阅读时，我通常试图理解计算的流程。对我来说，记住这一步中的 P \mathbf{P} P代表上一步计算的 P \mathbf{P} P的更新值要容易理解得多，而不是试图记住 P k − 1 ( + ) \mathbf{P}_{k-1}(+) Pk−1(+)表示什么，以及它与 P k − 1 ( − ) \mathbf{P}_{k-1}(-) Pk−1(−)的关系（如果有的话）。

练习：显示隐变量的效果

在我们的滤波器中，速度是一个隐变量。如果在状态量中不使用速度，那么滤波器工作的效果又如何呢？

编写一个使用状态量为 x = [ x ] \mathbf{x} = \begin{bmatrix} x \end{bmatrix} x=[x]的卡尔曼滤波器，并将其与使用 x = [ x x ˙ ] T \mathbf{x} = \begin{bmatrix} x & \dot{x} \end{bmatrix}^{T} x=[xx˙]T的滤波器进行比较。

解决办法

我们之前已经为位置和速度实现了一个卡尔曼滤波器，所以下面的代码我将不会做太多的注释：

from math import sqrt
from numpy.random import randndef univariate_filter(x0, P, R, Q):f = KalmanFilter(dim_x=1, dim_z=1, dim_u=1)f.x = np.array([[x0]])f.P *= Pf.H = np.array([[1.]])f.F = np.array([[1.]])f.B = np.array([[1.]])f.Q *= Qf.R *= Rreturn fdef plot_1d_2d(xs, xs1d, xs2d):plt.plot(xs1d, label='1D Filter')plt.scatter(range(len(xs2d)), xs2d, c='r', alpha=0.7, label='2D Filter')plt.plot(xs, ls='--', color='k', lw=1, label='track')plt.title('State')plt.legend(loc=4)plt.show()def compare_1D_2D(x0, P, R, Q, vel, u=None):# storage for filter outputxs, xs1, xs2 = [], [], []# 1d KalmanFilterf1D = univariate_filter(x0, P, R, Q)#2D Kalman filterf2D = pos_vel_filter(x=(x0, vel), P=P, R=R, Q=0)if np.isscalar(u):u = [u]pos = 0 # true positionfor i in range(100):pos += velxs.append(pos)# control input u - discussed belowf1D.predict(u=u)f2D.predict()z = pos + randn()*sqrt(R) # measurementf1D.update(z)f2D.update(z)xs1.append(f1D.x[0])xs2.append(f2D.x[0])plt.figure()plot_1d_2d(xs, xs1, xs2)compare_1D_2D(x0=0, P=50., R=5., Q=.02, vel=1.)

讨论

与只跟踪位置的滤波器相比，将速度融入状态量的滤波器产生更好的估计。一元滤波器无法估计速度（位置的变化），因此会滞后于被跟踪对象。

如果在一元卡尔曼滤波中，我们对预测方程有一个控制输入 u \mathbf{u} u（表示位置的变化）：

def predict(self, u=0.0):self.x += uself.P += self.Q

让我们尝试指定与实际速度相同的控制输入：

compare_1D_2D(x0=0, P=50., R=5., Q=.02, vel=1., u=1.)

此时，两个滤波器的性能是相似的，也许一元滤波器的跟踪更接近。但让我们看看当实际速度与控制输入 u \mathbf{u} u不同时会发生什么：

compare_1D_2D(x0=0, P=50., R=5., Q=.02, vel=-2., u=1.)

如果我们跟踪一个机器人，我们利用一元滤波器也可以做得很好，可能是因为控制输入允许滤波器作出准确的预测。但是，如果我们是被动跟踪，控制输入就没有多大的帮助了，除非我们可以作出准确的先验猜测的速度。这几乎是不可能的。

如何计算速度

我还没有解释滤波器是如何计算出速度的，或者任何的隐藏量。如果我们仔细查看每个滤波器矩阵计算的值，我们可以看到发生了什么。

我们就以上面的场景为例，首先需要计算系统的不确定性。

S = H P ˉ H T + R \mathbf{S} = \mathbf{H}\bar{\mathbf{P}}\mathbf{H}^{T} + \mathbf{R} S=HPˉHT+R

= [ 1 0 ] [ σ x 2 σ x v σ x v σ v 2 ] [ 1 0 ] + [ σ z 2 ] = \begin{bmatrix} 1 & 0 \end{bmatrix}\begin{bmatrix} \sigma_{x}^{2} & \sigma_{xv} \\ \sigma_{xv} & \sigma_{v}^{2} \end{bmatrix}\begin{bmatrix} 1 \\ 0 \end{bmatrix} + \begin{bmatrix} \sigma_{z}^{2} \end{bmatrix} =[10][σx2σxvσxvσv2][10]+[σz2]

= [ σ x 2 σ x v ] [ 1 0 ] + [ σ z 2 ] = \begin{bmatrix} \sigma_{x}^{2} & \sigma_{xv} \end{bmatrix}\begin{bmatrix} 1 \\ 0 \end{bmatrix} + \begin{bmatrix} \sigma_{z}^{2} \end{bmatrix} =[σx2σxv][10]+[σz2]

= [ σ x 2 + σ z 2 ] = \begin{bmatrix} \sigma_{x}^{2} + \sigma_{z}^{2} \end{bmatrix} =[σx2+σz2]

现在我们有了 S \mathbf{S} S，我们可以计算出卡尔曼增益的值：

K = P ˉ H T S − 1 \mathbf{K} = \bar{\mathbf{P}}\mathbf{H}^{T}\mathbf{S}^{-1} K=PˉHTS−1

= [ σ x 2 σ x v σ x v σ v 2 ] [ 1 0 ] [ 1 σ x 2 + σ z 2 ] = \begin{bmatrix} \sigma_{x}^{2} & \sigma_{xv} \\ \sigma_{xv} & \sigma_{v}^{2} \end{bmatrix}\begin{bmatrix} 1 \\ 0 \end{bmatrix}\begin{bmatrix} \frac{1}{\sigma_{x}^{2} + \sigma_{z}^{2}} \end{bmatrix} =[σx2σxvσxvσv2][10][σx2+σz21]

= [ σ x 2 σ x v ] [ 1 σ x 2 + σ z 2 ] = \begin{bmatrix} \sigma_{x}^{2} \\ \sigma_{xv} \end{bmatrix}\begin{bmatrix} \frac{1}{\sigma_{x}^{2} + \sigma_{z}^{2}} \end{bmatrix} =[σx2σxv][σx2+σz21]

= [ σ x 2 / ( σ x 2 + σ z 2 ) σ x v / ( σ x 2 + σ z 2 ) ] = \begin{bmatrix} \sigma_{x}^{2}/(\sigma_{x}^{2} + \sigma_{z}^{2}) \\ \sigma_{xv}/(\sigma_{x}^{2} + \sigma_{z}^{2})\end{bmatrix} =[σx2/(σx2+σz2)σxv/(σx2+σz2)]

换句话说，距离 x x x的卡尔曼增益是：

K x = V A R ( x ) V A R ( x ) + V A R ( z ) K_{x} = \frac{VAR(x)}{VAR(x) + VAR(z)} Kx=VAR(x)+VAR(z)VAR(x)

你应该很熟悉，这和一元的情况类似。

速度 x ˙ \dot{x} x˙的卡尔曼增益为：

K x ˙ = C O V ( x , x ˙ ) V A R ( x ) + V A R ( z ) K_{\dot{x}} = \frac{COV(x, \dot{x})}{VAR(x) + VAR(z)} Kx˙=VAR(x)+VAR(z)COV(x,x˙)

这有什么影响？回想一下，我们将状态量计算为：

x = x ˉ + K ( z − H x ) \mathbf{x} = \bar{\mathbf{x}} + \mathbf{K}(z - \mathbf{H}\mathbf{x}) x=xˉ+K(z−Hx)

= x ˉ + K y = \bar{\mathbf{x}} + \mathbf{K}y =xˉ+Ky

这里的残差 y y y是一个标量。因此它被乘以到 K \mathbf{K} K的每个元素。因此我们有：

[ x x ˙ ] = [ x ˉ x ˙ ˉ ] + [ K x K x ˙ ] y \begin{bmatrix} x \\ \dot{x} \end{bmatrix} = \begin{bmatrix} \bar{x} \\ \bar{\dot{x}} \end{bmatrix} + \begin{bmatrix} K_{x} \\ K_{\dot{x}} \end{bmatrix}y [xx˙]=[xˉx˙ˉ]+[KxKx˙]y

那么，在更新过程中，给出了这个方程组：

x = x ˉ + y K x x = \bar{x} + yK_{x} x=xˉ+yKx

x ˙ = x ˙ ˉ + y K x ˙ \dot{x} = \bar{\dot{x}} + yK_{\dot{x}} x˙=x˙ˉ+yKx˙

再考虑预测过程，预测 x ˉ \bar{x} xˉ的计算公式为 x + x ˙ Δ t x + \dot{x}\Delta t x+x˙Δt。

如果预测是完美的，那么残差将 y = 0 y=0 y=0（忽略观测中的噪声），速度估计将保持不变。另一方面，如果速度估计非常差，那么预测将非常差，并且残差将很大： y > > 0 y>>0 y>>0。在这种情况下，我们用 y K x ˙ yK_{\dot{x}} yKx˙更新速度估计。 K x ˙ K_{\dot{x}} Kx˙与 C O V ( x , x ˙ ) COV(x, \dot{x}) COV(x,x˙)成正比。因此，速度由位置误差乘以与位置和速度之间的协方差成比例的值来更新。相关性越高，校正越大。

C O V ( x , x ˙ ) COV(x, \dot{x}) COV(x,x˙)是 P \mathbf{P} P的非对角元素。回想一下，这些值是用 F P F T \mathbf{F}\mathbf{P}\mathbf{F}^{T} FPFT计算的。因此，在预测步骤中计算位置和速度的协方差。速度的卡尔曼增益与这个协方差成正比，我们根据上一个残差乘以一个与这个协方差成正比的值来调整速度估计。

调整滤波器的噪声

让我们开始改变参数，看看各种变化的效果。这是学习卡尔曼滤波器要做的一件非常正常的事情。精确地模拟我们的传感器是困难的，而且常常是不可能的。不完美的模型意味着我们的滤波器输出常常不完美。工程师们花了大量的时间来调整卡尔曼滤波器，使其在实际传感器中表现良好。我们现在将花时间来了解这些变化的影响。当你了解每一个变化的影响后，你将会养成如何设计一个卡尔曼滤波器的直觉。设计一个卡尔曼滤波器既是科学也是艺术。

让我们看看观测噪声 R \mathbf{R} R和过程噪声 Q \mathbf{Q} Q的影响，我们想看看不同设置对 R \mathbf{R} R和 Q \mathbf{Q} Q的影响。我们的第一个实验在改变 Q \mathbf{Q} Q的同时保持 R \mathbf{R} R不变，所以固定了观测值 225 225 225的方差。

from numpy.random import seedseed(2)
trk, zs = compute_dog_data(z_var=225, process_var=.02, count=50)run(track=trk, zs=zs, R=225, Q=200, P=P, plot_P=False, title='R_var = 225 $m^2$, Q_var = 20 $m^2$')
run(track=trk, zs=zs, R=225, Q=.02, P=P, plot_P=False, title='R_var = 225 $m^2$, Q_var = 0.02 $m^2$')

第一个图中的滤波器几乎紧跟着有噪声的观测值。而在第二个图中，滤波器与观测值相差较大，并且比第一个图更接近于一条直线。为什么改变 Q \mathbf{Q} Q会这样影响结果呢？

让我们理解一下过程不确定性一词的含义。在考虑跟踪球的问题中，我们可以用数学精确地模拟它在真空中的行为，但是在不同大小的风、不同的空气密度、不同的温度、不完美表面的旋转球等不同的情况下，我们的模型就会偏离现实。

在第一种情况下，我们设置 Q _ v a r = 200 Q\_var=200 Q_var=200，这相当大。在物理方面，这告诉滤波器我不相信我的运动预测步，因为我们说速度的方差是 200 200 200。也就是说，我们告诉滤波器有很多我们没有用 F \mathbf{F} F建模的外部噪声，这就导致结果不信任运动预测步。滤波器也会计算速度（ x ˙ \dot{x} x˙），但主要忽略它，因为我们告诉滤波器计算非常可疑。因此，滤波器除了观测值之外没有什么可信任的，因此它密切跟踪观测值。

在第二种情况下，我们设置 Q _ v a r = 0.02 Q\_var=0.02 Q_var=0.02，这是非常小的。在物理方面，我们告诉滤波器相信运动预测步，它真的很好。也就是说，仅仅有非常少量的过程噪声（方差 0.02 0.02 0.02），所以过程模型是非常准确的。因此，滤波器在上下跳跃时忽略了一些观测值，因为观测值的变化与我们可靠的速度预测不匹配。

现在，让我们将 Q _ v a r Q\_var Q_var设置为0.2，并将 R _ v a r R\_var R_var增加到10000。这告诉滤波器观测噪声非常大。

run(track=trk, zs=zs, R=10000, Q=.2, P=P, plot_P=False, title='R=$10,000\, m^2$, Q=$.2\, m^2$')

结果可能很微妙。我们创建了一个次优滤波器，因为实际观测噪声方差为225，而不是10000。通过设置滤波器的噪声方差如此之高，我们迫使滤波器有利于预测而不是观测。这可以导致明显比较顺利和好看的结果。在上面的图表中，轨迹看起来还是比较好的，因为它比较接近理想路径。但是，我们可以看到 P \mathbf{P} P没有收敛，因为整个图表用黄色背景表示 P \mathbf{P} P的大小。

让我们通过猜测初始位置为50，初始速度为1来查看初始值错误下的结果：

run(track=trk, zs=zs, R=10000, Q=.2, P=P, plot_P=False,x0=np.array([50., 1.]), title='R=$10,000\, m^2$, Q=$.2\, m^2$')

在这里我们可以看到，滤波器无法得到正确的轨迹。这是因为即使滤波器得到了相当好的观测值，它也假设观测值是坏的，并最终在每一步从一个坏的位置向前预测。如果你认为，对于较小的观测噪声，错误的初始位置可能会给出类似的结果，那么让我们将其设置回正确的值225。

run(track=trk, zs=zs, R=225, Q=.2, P=P, plot_P=False, x0=np.array([20., 1.]),title='R=$225\, m^2$, Q=$.2\, m^2$')

在这里，我们看到，滤波器最初挣扎了几次迭代，但后来它准确地跟踪上了。事实上，这几乎是最优的——我们没有优化设计 Q \mathbf{Q} Q，但 R \mathbf{R} R是最优的。 Q \mathbf{Q} Q的经验法则是将其设置在 1 2 Δ a \frac{1}{2}\Delta a 21Δa到 Δ a \Delta a Δa之间，其中 Δ a \Delta a Δa是采样周期之间加速度变化的最大值。这只适用于我们在本章中所做的假设——加速度是恒定的，并且在每个时间段之间不相关。在卡尔曼数学一章中，我们将讨论几种设计 Q \mathbf{Q} Q的不同方法。

在某种程度上，你可以通过改变 R \mathbf{R} R或 Q \mathbf{Q} Q来获得类似的输出，但我劝解你不要盲目地改变这些。要始终考虑这些值的物理含义，并根据你对所滤波系统的了解来改变 R \mathbf{R} R或 Q \mathbf{Q} Q。用大量的模拟或真实数据的试运行可以得到这些值。

协方差矩阵的详细分析

让我们看看协方差矩阵对滤波器的影响。我将从 P = 500 P=500 P=500开始。

import kf_book.mkf_internal as mkf_internalvar = 27.5
data = mkf_internal.zs_var_275()
run(track=trk, zs=zs, R=var, Q=.02, P=500., plot_P=True, title='$P=500\, m^2$')

从输出来看，我们看到在滤波器输出的最开始有一个非常大的尖峰。我们设置 P = 500 I 2 \mathbf{P}=500\mathbf{I}_{2} P=500I2（这是 2 × 2 2\times 2 2×2对角线矩阵的简写符号，对角线为500）。我们现在有足够的信息来理解这意味着什么，以及卡尔曼滤波器如何处理它。左上角的500对应于 σ x 2 \sigma_{x}^{2} σx2；因此，我们说 x x x的标准偏差为 500 \sqrt{500} 500 ，或大约22.36。大约 99 % 99\% 99%的样本出现在 3 σ 3\sigma 3σ内，因此 σ x 2 = 500 \sigma_{x}^{2} = 500 σx2=500告诉滤波器，预测值（先验）可能会偏离67米。这是一个很大的误差，所以当有观测时，滤波器不相信它自己的估计，并且疯狂地跳来尝试拟合观测值。然后，随着滤波器的发展， P \mathbf{P} P迅速收敛到一个更真实的值。

让我们看看这背后的数学。卡尔曼增益的方程是：

K = P ˉ H T S − 1 ≈ P ˉ H T S ≈ u n c e r t a i n t y p r e d i c t i o n u n c e r t a i n t y p r e d i c t i o n + u n c e r t a i n t y m e a s u r e m e n t H T \mathbf{K} = \bar{\mathbf{P}}\mathbf{H}^{T}\mathbf{S}^{-1} \approx \frac{\bar{\mathbf{P}}\mathbf{H}^{T}}{\mathbf{S}} \approx \frac{uncertainty_{prediction}}{uncertainty_{prediction} + uncertainty_{measurement}}{\mathbf{H}^{T}} K=PˉHTS−1≈SPˉHT≈uncertaintyprediction+uncertaintymeasurementuncertaintypredictionHT

它是预测值和观测值的不确定度的组合。在这里，预测值的不确定性很大，因此 K \mathbf{K} K很大（如果这是一个标量，则接近1）。 K \mathbf{K} K乘以残差 y = z − H x ˉ \mathbf{y} = \mathbf{z} - \mathbf{H}\bar{\mathbf{x}} y=z−Hxˉ，即观测值减去预测值，因此较大的 K \mathbf{K} K有利于观测值。因此，如果 P \mathbf{P} P相对于传感器不确定度 R \mathbf{R} R较大，则滤波器将形成观测主导的估计值。

现在让我们看看较小的初始值 P = 1.0 I 2 \mathbf{P}=1.0\mathbf{I}_{2} P=1.0I2的效果：

run(track=trk, zs=zs, R=var, Q=.02, P=1., plot_P=True, title='$P=1\, m^2$')

乍一看这个不错。图中没有前一个图中开始的尖峰；滤波器开始跟踪观测值，不需要任何时间来适应信号。然而，如果我们看 P \mathbf{P} P的曲线图，你会发现位置上的方差有一个初始峰值，而且它永远不会收敛。糟糕的设计会导致较长的收敛时间和次优的结果。

因此，尽管滤波器非常接近实际信号，但我们不能得出结论，原因是使用一个小 P \mathbf{P} P。是的，这将避免卡尔曼滤波器需要时间来才能精确地跟踪信号。但如果我们确实不确定初始值，这可能会导致滤波器产生非常糟糕的结果。比如：如果我们在跟踪一个有生命的物体，在我们开始跟踪它之前，我们可能就不确定它在哪里。为了使卡尔曼滤波器性能良好，你必须将 P \mathbf{P} P设置为一个真正反映你对数据的了解的值。

让我们看看一个糟糕的初始估计加上一个很小的 P \mathbf{P} P的结果。我们将初始估计值设为 x = 100 x=100 x=100（而实际上是从0开始的），但设 P = 1 \mathbf{P}=1 P=1。这对于 P \mathbf{P} P来说显然是一个不正确的值，因为估计值偏离了100，但是我们告诉滤波器，它的 3 σ 3\sigma 3σ误差是3。

x = np.array([100., 0.])
run(track=trk, zs=zs, R=var, Q=.02, P=1., x0=x,plot_P=False, title='$P=1\, m^2$')

我们可以看到，最初的估计是可怕的，它需要很长时间滤波器才开始比较稳定。这是因为我们告诉卡尔曼滤波器，我们非常相信我们最初估计的100，但这种想法是不正确的。

现在，让我们为 P \mathbf{P} P提供一个更合理的值，并查看其差异。

x = np.array([100., 0.])
run(track=trk, zs=zs, R=var, Q=.02, P=500., x0=x,plot_P=False, title='$P=500\, m^2$')

在这种情况下，卡尔曼滤波器对初始状态非常不确定，因此收敛速度更快。它只经过5到6个循环就产生了良好的值。

根据我们目前所学习的理论，这已经是我们所能做到的了。但是，这种情况有点人为：如果我们在开始跟踪时不知道目标在哪里，我们不会将滤波器初始化为某个任意值，例如0或100。我在下面的滤波器初始化部分对此进行了说明。

让我们为我们的狗做另一个卡尔曼滤波器，这次把协方差椭圆绘制在与位置相同的图上。

from kf_book.mkf_internal import plot_track_ellipsesdef plot_covariances(count, R, Q=0, P=20., title=''):    track, zs = compute_dog_data(R, Q, count)f = pos_vel_filter(x=(0., 0.), R=R, Q=Q, P=P)xs, cov = [], []for z in zs:f.predict()f.update(z)xs.append(f.x[0])cov.append(f.P)plot_track_ellipses(count, zs, xs, cov, title)plt.figure(figsize=(10,6))
plt.subplot(121)
plot_covariances(R=5, Q=.02, count=20, title='$R = 5\, m^2$')
plt.subplot(122)
plot_covariances(R=.1, Q=.02, count=20, title='$R = 0.5\, m^2$')

下面是滤波器滤波数据的动画。我已经调整了滤波器的参数，这样就可以很容易地看到 P \mathbf{P} P随着滤波器的进展而变化。

上面的输出有点混乱，但是你应该能够看到发生了什么。在这两个图中，我们为每个点绘制协方差矩阵。我们从协方差 P = [ 20 0 0 20 ] \mathbf{P} = \begin{bmatrix} 20 & 0 \\ 0 & 20 \end{bmatrix} P=[200020]开始，这意味着我们的初始值有很多不确定性。在我们接收到第一个观测值后，卡尔曼滤波器会更新协方差矩阵，因此方差不再那么大。在上图中，第一个椭圆（最左边的那个）应该是一个稍微压扁的椭圆。当滤波器继续处理观测值时，协方差椭圆迅速改变形状，直到它稳定下来成为一个在运动方向上倾斜的长而窄的椭圆。

想想这在物理意义上意味着什么。椭圆的 x x x轴表示位置的不确定性， y y y轴表示速度的不确定性。所以，一个窄而高的椭圆意味着速度比位置更不确定；相反，一个宽而窄的椭圆意味着位置比速度更不确定。最后，倾斜量显示了两个变量之间的相关性。

第一个图 R = 5 \mathbf{R} = 5 R=5，最后的图像是一个宽度比高度大的椭圆。如果还不清楚的话，我已经打印出右下角最后一个椭圆的方差。

相比之下，第二个图 R = 0.5 \mathbf{R} = 0.5 R=0.5，最后的图像是一个高度比宽度大的椭圆。而且，第二个图中的椭圆都比第一个图中的椭圆小得多。这是有道理的，因为在我们的观测值中，一个较小的 R \mathbf{R} R意味着少量的噪声，意味着观测值更准确。

问题：解释椭圆的区别

为什么 R = 5 \mathbf{R} = 5 R=5的椭圆比 R = 0.5 \mathbf{R} = 0.5 R=0.5的椭圆更倾向于水平方向。提示：请结合这些椭圆的物理意义来考虑，而不是数学意义。如果你不确定答案，请将 R \mathbf{R} R改为真正的大数字和小数字，如100和0.1，观察这些变化，并思考这意味着什么。

解决办法

x x x轴表示位置， y y y轴表示速度。垂直的椭圆表示位置和速度之间没有相关性，而对角线的椭圆表示有很多相关性。这样说，结果听起来不太可能。椭圆的倾斜度会改变，但相关性不应该随时间而改变。但这是对滤波器输出的描述，而不是对实际物理世界的描述。当 R \mathbf{R} R非常大时，我们告诉滤波器在观测值中有很多噪声。在这种情况下，卡尔曼增益 K \mathbf{K} K被设置为有利于预测值而不是观测值，并且预测来自速度状态量。因此， x x x和 x ˙ \dot{x} x˙之间有很大的相关性。相反的，如果 R \mathbf{R} R很小，我们告诉滤波器观测值是非常可信的， K \mathbf{K} K被设置为有利于观测值而不是预测值。如果观测值接近完美，为什么滤波器要使用预测？如果滤波器没有从预测中使用太多的信息，那么两者的相关性将非常小。

这是理解的关键点！卡尔曼滤波是一种针对实际世界的数学模型。相关性很小的结果并不意味着物理系统中没有相关性，只是建立的数学模型中没有线性相关性。

让我们用一个非常大的观测噪声来说明这一点。我们将设置 R = 200 \mathbf{R} = 200 R=200。在看结果之前先想想会是什么样子。

plot_covariances(R=200., Q=.2, count=5, title='$R = 200\, m^2$')

我希望结果是你所期望的。椭圆很快变得很宽，但不是很高。这是因为卡尔曼滤波器主要使用预测值和观测值来产生滤波结果。我们还可以看到滤波器的输出是如何缓慢地获取轨迹的。卡尔曼滤波器假设观测值是非常有噪声的，因此更新 x ˙ \dot{x} x˙的估计值非常慢。

继续看这些图，直到你掌握了如何解释协方差矩阵 P \mathbf{P} P。当你使用一个 9 × 9 9\times 9 9×9的矩阵时，它可能会显得势不可挡——有81个数字需要解释。分解一下——对角线包含每个状态量的方差，所有非对角线元素都是两个方差和一个比例因子 p p p的乘积。你不能在屏幕上绘制一个 9 × 9 9\times 9 9×9的矩阵，所以你必须在这个简单的二维案例中培养你的直觉和理解力。

滤波器初始化

初始化滤波器有许多方案，以下方法在大多数情况下都表现良好。在这个方案中，在得到第一个观测值 z 0 \mathbf{z}_{0} z0之前，不初始化滤波器。由此可以计算出 x \mathbf{x} x的初始值， x 0 = z 0 \mathbf{x}_{0} = \mathbf{z}_{0} x0=z0。如果 z \mathbf{z} z的大小、类型和单位与 x \mathbf{x} x不同（通常是这样），我们可以使用下面的观测函数。

我们知道

z = H x \mathbf{z} = \mathbf{H}\mathbf{x} z=Hx

因此

H − 1 H x = H − 1 z \mathbf{H}^{-1}\mathbf{H}\mathbf{x} = \mathbf{H}^{-1}\mathbf{z} H−1Hx=H−1z

x = H − 1 z \mathbf{x} = \mathbf{H}^{-1}\mathbf{z} x=H−1z

矩阵求逆需要一个方阵，但 H \mathbf{H} H很少是方阵的。SciPy将通过scipy.linalg.pinv计算矩阵伪逆矩阵，因此你的代码可能看起来像：

from scipy.linalg import pinvH = np.array([[1, 0.]])
z0 = 3.2
x = np.dot(pinv(H), z0)
print(x)

[[3.2][0. ]]

你的问题所在领域的专业知识可能会引导你进行不同的初值设定，但这是一种方法。例如，如果状态量包括速度，则可以对位置进行前两次观测，再进行差分，并将其用作初始速度。

现在我们需要设置 P \mathbf{P} P的初始值。这将因问题而异，但一般情况下，对于状态量和观测量完全相同的项，将使用观测噪声 R \mathbf{R} R，对于其他的项，可以设置为其最大值。例如：我们用位置和速度作为状态量来跟踪目标，并且观测的是位置。在这种情况下，我们可以用：

P = [ R 0 0 0 v e l m a x 2 ] \mathbf{P} = \begin{bmatrix} \mathbf{R}_{0} & 0 \\ 0 & vel_{max}^{2} \end{bmatrix} P=[R000velmax2]

P \mathbf{P} P的对角线包含每个状态量的方差，因此我们用合理的值填充它。 R \mathbf{R} R是位置的合理方差，最大速度的平方是速度的合理方差。它是平方的，因为方差是平方的： σ 2 \sigma^{2} σ2。

你真的需要了解你工作的领域，并根据最佳的可用信息来初始化你的滤波器。例如，假设我们试图在赛马中追踪马。最初的观测值可能非常糟糕，是一个远离起始栅栏的位置。我们知道马必须从起始栅栏开始，将滤波器初始化为初始观测值将导致次优结果。在这种情况下，我们希望总是用马的起始栅栏位置初始化卡尔曼滤波器。

批处理

卡尔曼滤波器被设计成一种递归算法——当新的观测值出现时，我们立即计算出一个新的估计值。但有时候我们想要滤波一批已经观测得到的数据，这也是很常见的。卡尔曼滤波器可以在批处理模式下运行，在批处理模式下，所有的观测值都被一次性滤波掉。我们已经在KalmanFilter.batch_filter()中实现了。在内部实现上，该函数所做的只是在观测值上循环，并收集得到的状态和协方差估计。它简化了逻辑并方便地将所有输出收集到数组中。我经常使用这个函数。

首先将观测值收集到一个数组或列表中。可能在CSV文件中：

zs = read_altitude_from_csv('altitude_data.csv')

或者你可以使用生成器生成它：

zs = [some_func(i) for i in range(1000)]

然后调用batch_filter()方法：

Xs, Ps, Xs_prior, Ps_prior = kfilter.batch_filter(zs)

该函数获取观测值列表，对其进行滤波，并返回状态估计（ X s Xs Xs）、协方差矩阵（ P s Ps Ps）和它们的先验（ X s _ p r i o r Xs\_prior Xs_prior， P s _ p r i o r Ps\_prior Ps_prior）的NumPy数组。

下面是一个完整的例子：

count = 50
track, zs = compute_dog_data(10, .2, count)
P = np.diag([500., 49.])
f = pos_vel_filter(x=(0., 0.), R=3., Q=.02, P=P)
xs, _, _, _ = f.batch_filter(zs)book_plots.plot_measurements(range(1, count + 1), zs)
book_plots.plot_filter(range(1, count + 1), xs[:, 0])
plt.legend(loc='best')

批处理滤波器也是可选filterpy.common.Saver对象的。如果提供，滤波器的所有属性也将被保存。如果要检查除状态量和协方差矩阵以外的值，这非常有用。这里我绘制残差图，看它是否像以0为中心的噪声。这是一个快速的目视检查，看看滤波器是否设计良好。如果从零开始漂移，或者看起来不像噪声，则滤波器设计不当或者过程不是高斯的。我们将在后面的章节中详细讨论这一点。现在，请将此视为Saver类的演示。

track, zs = compute_dog_data(10, .2, 200)
P = np.diag([500., 49.])
f = pos_vel_filter(x=(0., 0.), R=3., Q=.02, P=P)
s = Saver(f)
xs, _, _, _ = f.batch_filter(zs, saver=s)
s.to_array()
plt.plot(s.y)

平滑结果

假设我们正在追踪一辆直线行驶的汽车。我们得到的观测结果表明汽车开始左转。卡尔曼滤波器会将状态估计稍微移向观测值，但是它不能判断这是一个特别有噪声的观测值还是一个转弯的真正开始。

然而，如果我们有未来的观测值，我们可以决定是否转弯。假设随后的观测值都继续左转。我们就可以确定一个左转已经开始了。另一方面，如果随后的观测值继续在一条直线上进行，我们就会知道该观测很可能是噪声，应该忽略不计。状态量估计要么完全包含观测值，要么完全忽略观测值，这取决于未来观测值对物体运动的暗示，而不是在观测值和预测值之间做一个估计。

卡尔曼实现了这种算法的一种形式，称为RTS平滑器，使用方法是rts_smoother()。输入从batch_filter步骤计算出的均值和协方差，就可以接收到平滑后的均值、协方差和卡尔曼增益。

from numpy.random import seed
count = 50
seed(8923)P = np.diag([500., 49.])
f = pos_vel_filter(x=(0., 0.), R=3., Q=.02, P=P)
track, zs = compute_dog_data(3., .02, count)
Xs, Covs, _, _ = f.batch_filter(zs)
Ms, Ps, _, _ = f.rts_smoother(Xs, Covs)book_plots.plot_measurements(zs)
plt.plot(Xs[:, 0], ls='--', label='Kalman Position')
plt.plot(Ms[:, 0], label='RTS Position')
plt.legend(loc=4)

这个输出太棒了！在这张图表中有两件事我看得很清楚。首先，RTS平滑器的输出比KF输出平滑得多。其次，它几乎总是比KF输出更精确（我们将在之后的平滑一章中详细研究）。作为一个隐藏量，速度的提高更为显著：

plt.plot(Xs[:, 1], ls='--', label='Kalman Velocity')
plt.plot(Ms[:, 1], label='RTS Velocity')
plt.legend(loc=4)
plt.gca().axhline(1, lw=1, c='k')

练习：比较速度

因为我们正在绘制速度，让我们看看原始速度是什么，我们可以通过后续观测值差分来计算原始速度。例如：时间 1 1 1的速度可以用 x s [ 1 ] − x s [ 0 ] xs[1]-xs[0] xs[1]−xs[0]来近似。同时，绘制上卡尔曼滤波和RTS滤波估计的值：

dx = np.diff(Xs[:, 0], axis=0)
plt.scatter(range(1, len(dx) + 1), dx, facecolor='none', edgecolor='k', lw=2, label='Raw velocity')
plt.plot(Xs[:, 1], ls='--', label='Filter')
plt.plot(Ms[:, 1], label='RTS')
plt.legend(loc=4)

我们看到噪声淹没了信号，导致原始速度基本上毫无价值。滤波器对速度的估计相对来说好上不少。卡尔曼增益 K \mathbf{K} K是多维的。例如，它的值可能为 K = [ 0.1274 0.843 ] T \mathbf{K}=\begin{bmatrix} 0.1274 & 0.843 \end{bmatrix}^{T} K=[0.12740.843]T。第一个值用于缩放位置的残差，第二个值将缩放速度的残差。协方差矩阵告诉滤波器位置和速度的相关程度，并且每个位置和速度都将得到最佳滤波。

我展示这一点是为了重申使用卡尔曼滤波器来计算速度、加速度甚至更高阶值的重要性。我使用卡尔曼滤波器，因为它允许我准确估计速度和加速度。

讨论与总结

多元高斯函数允许我们同时处理多个维度，包括空间和其他维度（速度等）。我们了解了一个关键的信息：隐藏量有能力显著提高滤波器的精度。这是可能的，因为隐藏量与观测量是相关的。

关于隐藏量有一个重要的警告：构造一个能产生隐藏量的滤波器是很容易的，我可以写一个滤波器来估计一辆有轨电车的颜色。但是没有办法根据位置来计算汽车的颜色，所以颜色的估计将是无稽之谈。因此，设计者必须验证这些隐藏量能否被正确估计。如果你没有速度传感器，但正在估计速度，则需要测试速度估计是否正确。例如，假设速度是一个周期分量，它看起来像一个正弦波。如果采样时间小于频率的2倍，则无法准确估计速度。假设采样周期等于速度的频率，滤波器将报告速度是恒定的，因为它在正弦波的同一点对系统进行采样。

初始化对于隐藏量来说是一个特别困难的问题。如果一开始的初始化不好，滤波器可能就会因为与隐藏量发生冲突并失败。估计隐藏量是一个强大的工具，但也是一个危险的工具。

R \mathbf{R} R和 Q \mathbf{Q} Q的设计通常是相当有挑战性的。如果你的传感器具有 N ( 0 , σ 2 ) N(0, \sigma^{2}) N(0,σ2)的高斯噪声，因此可以设置 R = σ 2 \mathbf{R} = \sigma^{2} R=σ2。这很容易的，但显示中的传感器并不是完美高斯的。同时，假设 σ = 1 k g \sigma = 1kg σ=1kg，你试着称一个重 0.5 k g 0.5kg 0.5kg的东西。理论告诉我们，我们可能得到负的观测值。但实际上，体重秤永远不会报告重量小于零的结果。现实世界中的传感器通常具有峰度和偏度，这都要注意。

Q \mathbf{Q} Q的情况更可怕。当我兴高采烈地给狗运动的预测分配了一个噪声矩阵时，但你可能是怀疑的。谁能说出狗下一步会做什么？我GPS里的卡尔曼滤波器不知道山丘、河流，也不知道我糟糕的驾驶技术。

卡尔曼滤波是一种对世界的数学模型，最终输出只能和那个模型一样精确。为了使数学易于理解，我们必须做一些假设。我们假设传感器和运动模型具有高斯噪声。我们假设一切都是线性的。如果这是真的，卡尔曼滤波器在最小二乘意义上是最优的。这意味着没有办法比滤波器给我们的估计更好。然而，这些假设几乎从来都不是真的，因此模型必然是有限的，并且工作滤波器很少是最优的。

在后面的章节中，我们将讨论非线性问题。现在我想让你们明白，设计线性滤波器的矩阵是一个实验过程，而不是一个数学过程。如果世界上有很多无法解释的噪音，你可能不得不把 Q \mathbf{Q} Q调大。如果将其设置得太大，则滤波器无法快速响应更改。在自适应滤波器一章中，你将学习一些技术允许你根据输入和性能实时更改滤波器设计，但目前你需要找到一组适用于滤波器将遇到的条件的值。