%matplotlib inline

#format the book
import book_format
book_format.set_style()

简介

当你考虑未来的数据时，卡尔曼滤波器的性能并不是最优的。例如，假设我们在跟踪飞行器，最新的观测值突然偏离的很离谱，就像这样（我只考虑简单的1维）：

import matplotlib.pyplot as plt

data = [10.1, 10.2, 9.8, 10.1, 10.2, 10.3, 
        10.1, 9.9, 10.2, 10.0, 9.9, 11.4]
plt.plot(data)
plt.xlabel('time')
plt.ylabel('position')

在接近稳定状态的一段时间后，观测值突然有一个很大的变化。假设该变化已经超过了飞机飞行的极限。尽管如此，卡尔曼滤波器还是会根据当前的卡尔曼增益，将新的观测值合并到滤波器中。它不能拒绝噪声，因为观测的结果可能反映转弯的开始。当然，飞机也不太可能如此突然地转向，但不可否认，以下的情况都是可能的：

• 飞机之前就开始转弯，但之前的观测值结果很嘈杂，没有显示变化；
• 飞机正在转弯，观测值比较嘈杂；
• 飞机没有转弯，观测值非常嘈杂；
• 飞机正在相反方向转弯，观测值比较嘈杂；

现在，假设接下去的观测值为：

data2 = [11.3, 12.1, 13.3, 13.9, 14.5, 15.2]
plt.plot(data + data2)

根据这些未来的观测结果，我们可以推断出：飞机开始转弯。

另一方面，假设接下去的观测值为：

data3 = [9.8, 10.2, 9.9, 10.1, 10.0, 10.3, 9.9, 10.1]
plt.plot(data + data3)

在这种情况下，我们得出结论：飞机没有转弯，只是因为观测值非常嘈杂。

平滑器工作原理概述

卡尔曼滤波器是一种具有马尔可夫特性的递归滤波器——它在步骤$k$的估计仅基于步骤$k-1$的估计和步骤$k$的观测。但这意味着步骤$k-1$的估计基于步骤$k-2$，依此类推回到初始值。因此，第$k$步的估计取决于之前的所有观测，尽管程度不同。$k-1$的影响力最大，$k-2$次之，以此类推。

平滑器将未来的观测值也纳入步骤$k$的估计中。来自$k+1$的观测值的影响最大，$k+2$的影响较小，$k+3$的影响更小，依此类推。

当然，我可以通过一个低通滤波器平滑上面的数据。最终结果将是平滑的，但不一定准确，因为低通滤波器将真实的变化也消除了，就像它消除噪声一样。相比之下，卡尔曼平滑器是最优的——它们结合了所有可用信息，以做出数学上可以实现的最佳估计。

平滑器的类型

在这些情况下，有三种卡尔曼平滑器可以产生更好的效果：

固定区间平滑

这是一个基于批处理的平滑器。此平滑器在进行任何估计之前，会等待收集所有数据。例如，你可能是一名为实验收集数据的科学家，在实验完成之前不需要知道结果。固定区间平滑器将收集所有数据，然后使用所有可用的历史的和未来的观测值来估计每次观测的状态。如果你可以在批处理模式下运行卡尔曼平滑器，则始终建议使用这类平滑器，它将提供比之前章节中递归形式的滤波器更好的结果。

固定延迟平滑

固定延迟平滑器将延迟引入到输出。假设我们选择4步的延迟，平滑器将接收前3个观测值，但不输出滤波结果。然后，当第4个观测值进入时，滤波器将会考虑第1到4个的观测值，同时产生第1个观测值的输出。当第5个观测值进入时，将会考虑第2到5个的观测值，同时产生第2个观测值的输出。但是，如果你需要最新的数据，可能就需要滞后了。

固定点平滑

固定点平滑器，与普通的卡尔曼滤波器一样工作，但只会在某个固定时间点$j$处产生状态估计。在时间$k$达到$j$之前，平滑器和常规滤波器一样工作。一旦$k>j$，平滑器仍会继续估计$x_k$，同时也会使用$j...k$之间的观测对$x_j$进行更新。这有助于估计系统的初始参数，或对特定时间发生的事件产生最佳估计。例如，你可能有一个机器人在时间$j$拍摄了一张照片。当机器人继续移动时，你可以使用固定点平滑器为时间$j$获取尽可能最佳的机器人姿势信息。

平滑器的选择

这些平滑器的选择取决于你的需求，以及你可以提供多少内存和计算能力。

固定区间平滑以批处理为代价产生最平滑的输出。大多数算法都使用某种向前/向后的算法，其速度只比递归卡尔曼滤波器慢一倍。

固定延迟平滑只需要存储一个数据窗口，并且处理能力要求不高，因为每次新观测只处理该窗口。缺点是平滑器的输出总是滞后于输入，并且平滑效果不如固定区间平滑效果明显。

固定区间平滑

文献中有许多固定区间平滑器可用。我选择实现Rauch、Striebel和Tung发明的平滑器，因为它易于实现，计算效率高。它也是我见过的在实际应用中最常用的平滑方法，这种平滑器通常被称为RTS平滑器。

RTS平滑器的推导过程涉及数页密集的数学。我不会把它强加给你的，取而代之的是，我将简要介绍算法、方程，然后直接介绍平滑器的实现和演示。

RTS平滑器首先以批处理模式运行卡尔曼滤波器，计算每个步骤的滤波器输出。给定了每个观测的滤波器输出以及对应于每个输出的协方差矩阵之后，RTS向后运行数据，将对未来的信息融入到过去的观测中。当它到达第一个观测值时，整个过程就完成了，最终的输出以最大的最佳形式包含了所有的信息。

RTS平滑器的方程非常简单，易于实现。此推导适用于线性卡尔曼滤波器，EKF和UKF也存在类似的推导。这些步骤是在批处理的输出上执行的，从最近的时间倒退到第一次估计。每次迭代都将未来的信息纳入状态估计中。由于状态估计已经包含了所有过去的观测值，因此每个估计值将包含过去和未来所有观测值的知识。区分过去、现在和未来非常重要，因此我使用了下标来表示数据的时刻。后向运行数据的步骤为：

预测步：

$${\mathbf{P}=\mathbf{FP}}_k{\mathbf{F}}^T+\mathbf{Q}$$

更新步：

$${\mathbf{K}}_k={\mathbf{P}}_k{\mathbf{F}}^T{\mathbf{P}}^{-1}$$

$${\mathbf{x}}_k={\mathbf{x}}_k+{\mathbf{K}}_k({\mathbf{x}}_{k+1}-\mathbf{F}{\mathbf{x}}_k)$$

$${\mathbf{P}}_k={\mathbf{P}}_k+{\mathbf{K}}_k({\mathbf{P}}_{k+1}-\mathbf{P}){\mathbf{K}}_k^T$$

和往常一样，实现中最困难的部分是正确地计算下标。一个基本实现是：

def rts_smoother(Xs, Ps, F, Q):
    n, dim_x, _ = Xs.shape

    # smoother gain
    K = zeros((n,dim_x, dim_x))
    x, P, Pp = Xs.copy(), Ps.copy(), Ps.copy

    for k in range(n-2,-1,-1):
        Pp[k] = F @ P[k] @ F.T + Q # predicted covariance

        K[k]  = P[k] @ F.T @inv(Pp[k])
        x[k] += K[k] @ (x[k+1] - (F @ x[k]))     
        P[k] += K[k] @ (P[k+1] - Pp[k]) @ K[k].T
    return (x, P, K, Pp)

此实现和FilterPy中提供的实现很类似。它假设卡尔曼滤波器以批处理模式在外部运行，状态和协方差的结果通过Xs和Ps变量传入。

下面是一个例子：

import numpy as np
from numpy import random
from numpy.random import randn
import matplotlib.pyplot as plt
from filterpy.kalman import KalmanFilter
from filterpy.common import Q_discrete_white_noise
import kf_book.book_plots as bp

def plot_rts(noise, Q=0.001, show_velocity=False):
    random.seed(123)
    fk = KalmanFilter(dim_x=2, dim_z=1)

    fk.x = np.array([0., 1.])      # state (x and dx)

    fk.F = np.array([[1., 1.],
                     [0., 1.]])    # state transition matrix

    fk.H = np.array([[1., 0.]])    # Measurement function
    fk.P*= 10.                     # covariance matrix
    fk.R = noise                   # state uncertainty
    fk.Q = Q_discrete_white_noise(dim=2, dt=1., var=Q)  # process uncertainty

    # create noisy data
    zs = np.asarray([t + randn()*noise for t in range (40)])

    # filter data with Kalman filter, than run smoother on it
    mu, cov, _, _ = fk.batch_filter(zs)
    M, P, C, _ = fk.rts_smoother(mu, cov)

    # plot data
    if show_velocity:
        index = 1
        print('gu')
    else:
        index = 0
    if not show_velocity:
        bp.plot_measurements(zs, lw=1)
    plt.plot(M[:, index], c='b', label='RTS')
    plt.plot(mu[:, index], c='g', ls='--', label='KF output')
    if not show_velocity:
        N = len(zs)
        plt.plot([0, N], [0, N], 'k', lw=2, label='track') 
    plt.legend(loc=4)
    plt.show()
    
plot_rts(7.)

我已经在信号中注入了大量的噪声，让你可以从视觉上区分RTS输出和KF滤波器输出。在上图中，我们可以看到，与输入相比，绘制为绿色虚线的卡尔曼滤波器比较平滑。但是，当连续几次观测值都发生偏离时，滤波器也发生了偏离。相比之下，RTS输出非常平滑，非常接近理想值。

通过一个可能更合理的噪声量，我们可以看到RTS输出几乎位于理想值上。相比较于观测值，卡尔曼滤波器的输出虽然好得多，但变化幅度仍然较大。

plot_rts(noise=1.)

然而，我们必须理解，这种平滑是基于系统模型的。我们告诉滤波器，我们跟踪的是一个恒速模型，过程误差非常小。当滤波器发现未来的行为与恒定的速度非常匹配时，它就能够抑制信号中的大部分噪声。相反，假设我们的系统有很多过程噪声。例如，如果我们在狂风中跟踪轻型飞机，其速度将经常变化，并且滤波器将无法区分噪声和风引起的不稳定运动。我们可以在下一张图表中看到这一点。

plot_rts(noise=7., Q=.1)

平滑器根据之前的观测结果、未来的观测结果，以及你告诉它的关于系统行为、系统和观测中噪声的信息，做出最佳估计。

让我们通过观察卡尔曼滤波器和RTS平滑器的速度估计来总结一下：

plot_rts(7.,show_velocity=True)

作为一个隐藏量，速度的改善更为显著。

固定延迟平滑

如果可以在批处理模式下运行，则上述RTS平滑器应始终是你的首选，因为它将所有的可用数据合并到每个估计中。然而，并非所有的问题都允许你这样做，但你仍然想要获取先前估计的平滑值。下面的数字行说明了这个概念。

from kf_book.book_plots import figsize
from kf_book.smoothing_internal import *

with figsize(y=2):
    show_fixed_lag_numberline()

在步骤$k$中，我们可以使用标准卡尔曼滤波方程估计$x_k$。然而，我们可以使用$x_k$收到的观测值，更好地估计$x_{k-1}$。同样，我们可以使用$x_{k-1}$和$x_k$收到的观测值，更好地估计$x_{k-2}$。我们可以将这种计算向后扩展到任意N步。

数学推导超出了本文的范围。如果你感兴趣的话，Dan Simon的《最优状态估计》中有一个非常好的解释。这个想法的本质是，我们不需要状态向量$\mathbf{x}$，而是建立一个增强状态量：

$$\mathbf{x}=\left[\begin{array}{c}{\mathbf{x}}_k\\ {\mathbf{x}}_{k-1}\\ ...\\ {\mathbf{x}}_{k-N+1}\end{array}\right]$$

这将产生一个非常大的协方差矩阵，其中包含不同步的状态量的协方差。FilterPy的类FixedLagSmoother为你处理所有的这些计算，包括增广矩阵的创建。你需要做的就是像使用KalmanFilter类一样编写它，然后调用smooth()，它实现了算法的预测和更新步。

每次调用smooth()都会计算当前观测的估计值，同时它也会返回并调整之前的$N-1$个点。平滑后的值包含在列表FixedLagSmoother.xSmooth。如果使用FixedLagSmoother.x，你将获得最新的估计值，但它不是平滑的，与标准卡尔曼滤波器输出没有区别。

from filterpy.kalman import FixedLagSmoother, KalmanFilter
import numpy.random as random

fls = FixedLagSmoother(dim_x=2, dim_z=1, N=8)

fls.x = np.array([0., .5])
fls.F = np.array([[1.,1.],
                  [0.,1.]])

fls.H = np.array([[1.,0.]])
fls.P *= 200
fls.R *= 5.
fls.Q *= 0.001

kf = KalmanFilter(dim_x=2, dim_z=1)
kf.x = np.array([0., .5])
kf.F = np.array([[1.,1.],
                 [0.,1.]])
kf.H = np.array([[1.,0.]])
kf.P *= 200
kf.R *= 5.
kf.Q = Q_discrete_white_noise(dim=2, dt=1., var=0.001)

N = 4 # size of lag

nom =  np.array([t/2. for t in range (0, 40)])
zs = np.array([t + random.randn()*5.1 for t in nom])

for z in zs:
    fls.smooth(z)
    
kf_x, _, _, _ = kf.batch_filter(zs)
x_smooth = np.array(fls.xSmooth)[:, 0]


fls_res = abs(x_smooth - nom)
kf_res = abs(kf_x[:, 0] - nom)

plt.plot(zs,'o', alpha=0.5, marker='o', label='zs')
plt.plot(x_smooth, label='FLS')
plt.plot(kf_x[:, 0], label='KF', ls='--')
plt.legend(loc=4)

print(f'standard deviation fixed-lag: {np.mean(fls_res):.3f}')
print(f'standard deviation kalman: {np.mean(kf_res):.3f}')

standard deviation fixed-lag: 2.616
standard deviation kalman: 3.564

这里我设置了$N=8$，这意味着我们将把8个未来的观测值纳入我们的估计。一旦滤波器收敛，这为我们提供了一个非常平滑的估计，代价大约是标准卡尔曼滤波器计算量的8倍。请随意尝试$N$的大小值。我随机选择了8，不是出于任何理论上的考虑。

相关阅读

• Kalman-and-Bayesian-Filters-in-Python/blob/master/13-Smoothing
• 卡尔曼滤波系列——（六）卡尔曼平滑