%matplotlib inline

#format the book
import book_format
book_format.set_style()

简介

到目前为止，我们已经考虑了跟踪与我们的过程模型相关的表现良好的对象的问题。例如，我们可以使用恒速滤波器来跟踪直线运动的物体。只要物体以合理的恒定速度沿直线移动，或者非常缓慢地改变其轨迹或速度，这个滤波器都会表现得比较好。相反，假设我们正试图跟踪机动目标，例如草原上的马、天空中飞行的鸟等。在这些情况下，滤波器的性能相当差。或者，考虑一种情况，假设在海上跟踪帆船。即使我们可以对控制输入进行建模，却无法对风或洋流进行建模。

解决这个问题的一种方法是使过程噪声$\mathbf{Q}$更大，以说明系统动力学的不可预测性。虽然这可能对滤波器不发散起作用，但结果通常远远不是最佳的。$\mathbf{Q}$值越大，滤波器越重视观测中的噪声。我们很快就会看到一个例子。

在本文中，我们将讨论自适应滤波器的概念。当滤波器检测到过程模型无法解释的动态时，它将自行调整。我将从这个问题的一个例子开始，然后讨论并实现各种自适应滤波器。

机动目标

我们需要对一个机动目标使用低阶模型进行模拟。我将实现一个带有转向输入的简单二维模型。你只提供了一个新的速度或方向，它将修改其状态量以匹配。

from math import sin, cos, radians

def angle_between(x, y):
  return min(y-x, y-x+360, y-x-360, key=abs)

class ManeuveringTarget(object): 
    def __init__(self, x0, y0, v0, heading):
        self.x = x0
        self.y = y0
        self.vel = v0
        self.hdg = heading
        
        self.cmd_vel = v0
        self.cmd_hdg = heading
        self.vel_step = 0
        self.hdg_step = 0
        self.vel_delta = 0
        self.hdg_delta = 0

    def update(self):
        vx = self.vel * cos(radians(90-self.hdg))
        vy = self.vel * sin(radians(90-self.hdg))
        self.x += vx
        self.y += vy
        
        if self.hdg_step > 0:
            self.hdg_step -= 1
            self.hdg += self.hdg_delta

        if self.vel_step > 0:
            self.vel_step -= 1
            self.vel += self.vel_delta
        return (self.x, self.y)

    def set_commanded_heading(self, hdg_degrees, steps):
        self.cmd_hdg = hdg_degrees
        self.hdg_delta = angle_between(self.cmd_hdg, 
                                       self.hdg) / steps
        if abs(self.hdg_delta) > 0:
            self.hdg_step = steps
        else:
            self.hdg_step = 0

    def set_commanded_speed(self, speed, steps):
        self.cmd_vel = speed
        self.vel_delta = (self.cmd_vel - self.vel) / steps
        if abs(self.vel_delta) > 0:
            self.vel_step = steps
        else:
            self.vel_step = 0

同时，让我们实现一个带有噪声的模拟传感器。

from numpy.random import randn

class NoisySensor(object):
    def __init__(self, std_noise=1.):
        self.std = std_noise

    def sense(self, pos):
        """Pass in actual position as tuple (x, y).       
        Returns position with noise added (x,y)"""
        return (pos[0] + (randn() * self.std), 
                pos[1] + (randn() * self.std))

现在，让我们生成一条轨迹并绘制它，以测试一切是否正常。我将把数据生成放在一个函数中，这样我们就可以创建不同长度的路径（原因很快就会清楚）。

import kf_book.book_plots as bp
import numpy as np
import matplotlib.pyplot as plt

def generate_data(steady_count, std):
    t = ManeuveringTarget(x0=0, y0=0, v0=0.3, heading=0)
    xs, ys = [], []

    for i in range(30):
        x, y = t.update()
        xs.append(x)
        ys.append(y)

    t.set_commanded_heading(310, 25)
    t.set_commanded_speed(1, 15)

    for i in range(steady_count):
        x, y = t.update()
        xs.append(x)
        ys.append(y)

    ns = NoisySensor(std)
    pos = np.array(list(zip(xs, ys)))
    zs = np.array([ns.sense(p) for p in pos])
    return pos, zs

sensor_std = 2.
track, zs = generate_data(50, sensor_std)
plt.figure()
bp.plot_measurements(*zip(*zs), alpha=0.5)
plt.plot(*zip(*track), color='b', label='track')
plt.axis('equal')
plt.legend(loc=4)
bp.set_labels(title='Track vs Measurements', x='X', y='Y')

这种巨大的噪音让我们更容易看到各种滤波器设计选择的效果。

现在我们实现一个卡尔曼滤波器来跟踪这个物体。$x$和$y$坐标是独立的，所以我们可以独立地跟踪它们。在本文的剩余部分中，我们将只跟踪$x$坐标，以简化问题。

我们从一个恒速滤波器开始。

from filterpy.kalman import KalmanFilter
from filterpy.common import Q_discrete_white_noise

def make_cv_filter(dt, std):
    cvfilter = KalmanFilter(dim_x = 2, dim_z=1)
    cvfilter.x = np.array([0., 0.])
    cvfilter.P *= 3
    cvfilter.R *= std**2
    cvfilter.F = np.array([[1, dt],
                           [0,  1]], dtype=float)
    cvfilter.H = np.array([[1, 0]], dtype=float)
    cvfilter.Q = Q_discrete_white_noise(dim=2, dt=dt, var=0.02)
    return cvfilter

def initialize_filter(kf, std_R=None):
    """ helper function - we will be reinitialing the filter
    many times.
    """
    kf.x.fill(0.)
    kf.P = np.eye(kf.dim_x) * .1
    if std_R is not None:
        kf.R = np.eye(kf.dim_z) * std_R

现在我们运行它：

sensor_std = 2.
dt = 0.1

# initialize filter
cvfilter = make_cv_filter(dt, sensor_std)
initialize_filter(cvfilter)

track, zs = generate_data(50, sensor_std)

# run it
z_xs = zs[:, 0]
kxs, _, _, _ = cvfilter.batch_filter(z_xs)

# plot results
bp.plot_track(track[:, 0], dt=dt)
bp.plot_filter(kxs[:, 0], dt=dt, label='KF')
bp.set_labels(title='Track vs KF', x='time (sec)', y='X')
plt.legend(loc=4)

从图中我们可以看出，卡尔曼滤波器无法跟踪航向的变化。**这是因为滤波器没有建模加速度，因此它总是滞后于输入。**如果信号进入稳定状态，滤波器最终将赶上信号。让我们看看这个：

# reinitialize filter
dt = 0.1
initialize_filter(cvfilter)

track2, zs2 = generate_data(150, sensor_std)
xs2 = track2[:, 0]
z_xs2 = zs2[:, 0]

kxs2, _, _, _ = cvfilter.batch_filter(z_xs2)

bp.plot_track(xs2, dt=dt)
bp.plot_filter(kxs2[:, 0], dt=dt, label='KF')
plt.legend(loc=4)
bp.set_labels(title='Effects of Acceleration', 
              x='time (sec)', y='X')

潜在的问题是，我们的过程模型对于稳态部分是正确的，但对于机动运动时是不正确的。我们可以通过增加$\mathbf{Q}$的大小来解释这一点，就像这样。

# reinitialize filter
dt = 0.1
initialize_filter(cvfilter)
cvfilter.Q = Q_discrete_white_noise(dim=2, dt=dt, var=2.0)
track, zs = generate_data(50, sensor_std)

# recompute track
kxs2, _, _, _ = cvfilter.batch_filter(z_xs2)
bp.plot_track(xs2, dt=dt)
bp.plot_filter(kxs2[:, 0], dt=dt, label='KF')
plt.legend(loc=4)
bp.set_labels(title='Large Q (var=2.0)', x='time (sec)', y='X')

我们可以看到，滤波器更快地重新获得轨迹，但代价是输出中存在较大的噪声。此外，许多跟踪情况不能容忍$4s$到$8s$之间的延迟量。我们可以进一步降低延迟，但代价是输出噪音更大。比如：

# reinitialize filter
dt = 0.1
initialize_filter(cvfilter)
cvfilter.Q = Q_discrete_white_noise(dim=2, dt=dt, var=50.0)
track, zs = generate_data(50, sensor_std)

# recompute track
cvfilter.x.fill(0.)
kxs2, _, _, _ = cvfilter.batch_filter(z_xs2)

bp.plot_track(xs2, dt=dt)
bp.plot_filter(kxs2[:, 0], dt=dt, label='KF')
plt.legend(loc=4)
bp.set_labels(title='Huge Q (var=50.0)', x='time (sec)', y='X')

机动意味着加速，所以让我们实现一个恒加速度卡尔曼滤波器，看看它如何处理相同的数据。

def make_ca_filter(dt, std):
    cafilter = KalmanFilter(dim_x=3, dim_z=1)
    cafilter.x = np.array([0., 0., 0.])
    cafilter.P *= 3
    cafilter.R *= std
    cafilter.Q = Q_discrete_white_noise(dim=3, dt=dt, var=0.02)
    cafilter.F = np.array([[1, dt, 0.5*dt*dt],
                           [0, 1,         dt], 
                           [0, 0,          1]])
    cafilter.H = np.array([[1., 0, 0]])
    return cafilter

def initialize_const_accel(f):
    f.x = np.array([0., 0., 0.])
    f.P = np.eye(3) * 3

dt = 0.1
cafilter = make_ca_filter(dt, sensor_std)
initialize_const_accel(cafilter)

kxs2, _, _, _ = cafilter.batch_filter(z_xs2)

bp.plot_track(xs2, dt=dt)
bp.plot_filter(kxs2[:, 0], dt=dt, label='KF')
plt.legend(loc=4)
bp.set_labels(title='Constant Acceleration Kalman Filter',
              x='time (sec)', y='X')

恒加速度模型能够无滞后地跟踪机动，但在稳态行为期间会产生较大的噪声输出。噪声输出是由于滤波器无法区分机动的开始和信号中的噪声。信号中的噪声意味着加速度，因此滤波器的加速度项跟踪它。

看起来我们似乎赢不了。当目标加速时，恒速度滤波器无法快速反应，但恒加速度滤波器会将零加速度状态下的噪声误解为加速度而不是噪声。

然而，这里有一个重要的见解，将引导我们找到一个解决方案。当目标没有机动（加速度为零）时，恒速度滤波器的性能最佳。当目标机动时，恒加速度滤波器性能良好，与人工加大恒速度滤波器的过程噪声$\mathbf{Q}$一样。如果我们制作一个滤波器，使其自身适应被跟踪对象的行为，那我们就可以同时获得这两个方面的最佳效果。

检测机动

在讨论如何创建自适应滤波器之前，我们必须解决的是：我们如何检测机动？如果我们都不知道机动何时发生，我们就无法合理地调整滤波器以响应机动。

一般来说，如果该对象的行为不同于卡尔曼滤波器中使用的过程模型，我们可以说该对象相对于卡尔曼滤波器是机动的。

机动物体对滤波器的数学结果产生什么影响？对象的行为将与滤波器的预测不同，因此残差将很大。回想一下，残差是滤波器当前预测值与观测值之间的差值。

为了确认这一点，让我们绘制机动过程中滤波器的残差。我将减少数据中的噪声量，以便更容易看到残差。

from kf_book.adaptive_internal import plot_track_and_residuals

def show_residual_chart():
    dt = 0.1
    sensor_std = 0.2
    
    # initialize filter
    cvfilter = make_cv_filter(dt, sensor_std)
    initialize_filter(cvfilter)
    pos2, zs2 = generate_data(150, sensor_std)
    xs2 = pos2[:, 0]
    z_xs2 = zs2[:, 0]

    cvfilter.Q = Q_discrete_white_noise(dim=2, dt=dt, var=0.02)
    xs, res = [], []
    for z in z_xs2:
        cvfilter.predict()
        cvfilter.update([z])
        xs.append(cvfilter.x[0])
        res.append(cvfilter.y[0])
        
    xs = np.asarray(xs)
    plot_track_and_residuals(dt, xs, z_xs2, res)

show_residual_chart()

左图绘制了有噪声的观测值与卡尔曼滤波器输出的对比图。右图显示了滤波器计算的残差——观测值和卡尔曼滤波器预测值之间的差异。让我再强调这一点：右图不是左图中两条线之间的差异。左图显示观测值和最终卡尔曼滤波器输出之间的差异，而右图显示观测值和过程模型预测之间的差异。

这似乎是一个微妙的区别，但从你图上来看并非如此。当机动开始时，左图中的偏差量很小，但右图中的残差却说明了不同的情况。如果被跟踪对象根据过程模型移动，则残差图应在$0.0$左右波动。这是因为观测将遵循以下等式：

$$\mathtt{measurement}=process\_model(t)+\mathtt{noise}(t)$$

一旦目标的行为与预测不匹配，则目标的行为将与预测不匹配：

$$\mathtt{measurement}=process\_model(t)+maneuver\_delta(t)+\mathtt{noise}(t)$$

所以，如果残差偏离均值$0.0$，我们知道机动已经开始。

我们可以在残差图中清楚地看到机动的结果，但信号中的噪声量掩盖了机动的开始。这是我们从噪声中提取信号的老问题。

可调的过程噪声

我们考虑的第一种方法使用低阶模型，通过判断机动是否发生来调整过程噪声。当残差变得大（对于大的合理定义）时，我们将增加过程噪声。这将使滤波器更倾向于观测而不是过程预测，并且滤波器将密切跟踪信号。当残差很小时，我们将缩小过程噪声。

在文献中有很多方法来做这件事，我会考虑几个选择。

连续调整

第一种方法（来自Bar Shalom），使用以下等式对残差的平方进行归一化：

$$ϵ={{\mathbf{y}}^{\mathsf{T}}\mathbf{S}}^{-1}\mathbf{y}$$

其中，$\mathbf{y}$为残差，$\mathbf{S}$为系统不确定性（协方差），其定义如下：

$$\mathbf{S}=\mathbf{HP}{\mathbf{H}}^{\mathsf{T}}+\mathbf{R}$$

如果上述表达的线性代数让你感到困惑，回想一下，我们可以用除法来考虑矩阵逆，所以$ϵ={{\mathbf{y}}^{\mathsf{T}}\mathbf{S}}^{-1}\mathbf{y}$可以被认为是计算：

$$ϵ\approx \frac{{\mathbf{y}}^2}{\mathbf{S}}$$

其中，$\mathbf{y}$和$\mathbf{S}$都是filterpy.KalmanFilter的属性。因此实现将非常简单。

让我们看看$ϵ$与时间的关系图。

from numpy.linalg import inv
dt = 0.1
sensor_std = 0.2
cvfilter= make_cv_filter(dt, sensor_std)
_, zs2 = generate_data(150, sensor_std)

epss = []
for z in zs2[:, 0]:
    cvfilter.predict()
    cvfilter.update([z])
    y, S = cvfilter.y, cvfilter.S
    eps = y.T @ inv(S) @ y
    epss.append(eps)

t = np.arange(0, len(epss) * dt, dt)
plt.plot(t, epss)
bp.set_labels(title='Epsilon vs time', 
              x='time (sec)', y='$\epsilon$')

该图应该能清楚地说明标准化残差的效果。对残差进行平方处理可确保始终大于零，并通过观测协方差进行归一化，以便我们能够区分残差何时相对于观测噪声发生显著变化。机动开始于$t=3s$，我们可以看到，在那之后不久，$ϵ$开始迅速增加。

一旦$ϵ$超过某个阈值，我们就要开始扩大$\mathbf{Q}$，一旦它再次低于该阈值，我们就要缩小$\mathbf{Q}$。我们可以用一个比例因子乘以$\mathbf{Q}$。也许有关于如何得到这个比例因子的文献，但我是通过实验推导出来的。对于$ϵ$（或者${ϵ}_{max}$）：一般来说，一旦残差大于3个标准差，我们可以假设差异是由于实际变化而不是噪声造成的。然而，传感器很少是真正的高斯分布，因此在实践中使用了更大的数值，例如5-6个标准差。

我使用合理的${ϵ}_{max}$和$\mathbf{Q}$的比例因子，实现该算法。

# reinitialize filter
dt = 0.1
sensor_std = 0.2
cvfilter = make_cv_filter(dt, sensor_std)
_, zs2 = generate_data(180, sensor_std)

Q_scale_factor = 1000.
eps_max = 4.

xs, epss = [], []

count = 0
for i, z in zip(t, zs2[:, 0]):
    cvfilter.predict()
    cvfilter.update([z])
    y, S = cvfilter.y, cvfilter.S
    eps = y.T @ inv(S) @ y
    epss.append(eps)
    xs.append(cvfilter.x[0])

    if eps > eps_max:
        cvfilter.Q *= Q_scale_factor
        count += 1
    elif count > 0:
        cvfilter.Q /= Q_scale_factor
        count -= 1

bp.plot_measurements(zs2[:,0], dt=dt, label='z', alpha=0.5)
bp.plot_filter(t, xs, label='filter')
plt.legend(loc=4)
bp.set_labels(title='epsilon=4', x='time (sec)', y='$\epsilon$')

这种滤波器的性能明显优于恒速度滤波器。在机动开始后，恒速度滤波器大约需要$10s$才能重新获取信号，但自适应滤波器需要不到$1s$的时间来完成了同样的操作。

连续调整-标准偏差版本

Zarchan的另一种非常类似的方法是：基于观测误差协方差的标准偏差设定限值。这里的表达式是：

$$\begin{array}{rl}std& =\sqrt{{\mathbf{HPH}}^{\mathsf{T}}+\mathbf{R}}\\ & =\sqrt{\mathbf{S}}\end{array}$$

如果残差的绝对值大于上面计算的标准偏差的一定倍数，我们将过程噪声增加固定量，重新计算$\mathbf{Q}$，然后继续。

from math import sqrt

def zarchan_adaptive_filter(Q_scale_factor, std_scale, 
                            std_title=False,
                            Q_title=False):
    cvfilter = make_cv_filter(dt, std=0.2)
    pos2, zs2 = generate_data(180-30, std=0.2)
    xs2 = pos2[:,0]
    z_xs2 = zs2[:,0]

    # reinitialize filter
    initialize_filter(cvfilter)
    cvfilter.R = np.eye(1)*0.2

    phi = 0.02
    cvfilter.Q = Q_discrete_white_noise(dim=2, dt=dt, var=phi)
    xs, ys = [], []
    count = 0
    for z in z_xs2:
        cvfilter.predict()
        cvfilter.update([z])
        y = cvfilter.y
        S = cvfilter.S
        std = sqrt(S)

        xs.append(cvfilter.x)
        ys.append(y)

        if abs(y[0]) > std_scale*std:
            phi += Q_scale_factor
            cvfilter.Q = Q_discrete_white_noise(2, dt, phi)
            count += 1
        elif count > 0:
            phi -= Q_scale_factor
            cvfilter.Q = Q_discrete_white_noise(2, dt, phi)
            count -= 1

    xs = np.asarray(xs)
    plt.subplot(121)
    bp.plot_measurements(z_xs2, dt=dt, label='z')
    bp.plot_filter(xs[:, 0], dt=dt, lw=1.5)
    bp.set_labels(x='time (sec)', y='$\epsilon$')
    plt.legend(loc=2)
    if std_title:
        plt.title(f'position(std={std_scale})')
    elif Q_title:
        plt.title(f'position(Q scale={Q_scale_factor})')
    else:
        plt.title('position')
        
    plt.subplot(122)
    plt.plot(np.arange(0, len(xs)*dt, dt), xs[:, 1], lw=1.5)
    plt.xlabel('time (sec)')
    if std_title:
        plt.title(f'velocity(std={std_scale})')
    elif Q_title:
        plt.title(f'velocity(Q scale={Q_scale_factor})')
    else:
        plt.title('velocity')        
    plt.show()
 
zarchan_adaptive_filter(1000, 2, std_title=True)

我选择$1000$作为过程噪声的固定增加量，$2$作为标准偏差的一定倍数。为什么是这些数字？首先，让我们看看2和3个标准差之间的差异。

zarchan_adaptive_filter(1000, 3, std_title=True)

我们可以从图中看到，无论我们使用2个标准差还是3个标准差，位置滤波器的输出都非常相似。但速度的计算却相差较大。让我们进一步探讨这个问题。首先，让我们把标准差比例设得很小。

zarchan_adaptive_filter(1000, .1, std_title=True)
zarchan_adaptive_filter(1000, 1, std_title=True)

随着标准差比例的变小，速度的计算变得更糟。想想为什么会这样。如果我们开始改变滤波器，使其更喜欢观测而不是预测，只要残差稍微偏离预测，我们很快就会给予观测几乎所有的权重。由于预测没有权重，我们没有足够的信息来优化隐藏量。所以，当比例为$0.1std$时，你可以看到速度被观测中的噪声淹没了。另一方面，因为我们非常倾向于观测，所以位置几乎完美地遵循了机动。

现在让我们来看看固定增加量对过程噪声的影响。在这里，我将标准差比例设置为2，并将增加量从1递增到10000。

zarchan_adaptive_filter(1, 2, Q_title=True)
zarchan_adaptive_filter(10, 2, Q_title=True)
zarchan_adaptive_filter(100, 2, Q_title=True)
zarchan_adaptive_filter(1000, 2, Q_title=True)
zarchan_adaptive_filter(10000, 2, Q_title=True)

在这里，我们可以看到，随着固定增加量的增加，位置估计稍微好一些，但速度估计开始产生较大的超调。

我不可能告诉你哪一个是正确的。你需要根据真实和模拟数据测试滤波器的性能，并选择与每个状态量所需性能最匹配的设计。

衰减记忆滤波器

衰减记忆滤波器通常不被归类为自适应滤波器，因为它们不适应输入，但它们在机动目标上的性能表现良好。它们还具有一阶、二阶和三阶运动学滤波器的非常简单的计算形式的优点。这种简单的形式不需要Ricatti方程来计算卡尔曼滤波器的增益，这大大减少了计算量。然而，它也有一种形式可以与标准的卡尔曼滤波器配合使用。在本文中，我将重点介绍后者，因为我们更关注自适应滤波器。这两种形式的衰减记忆滤波器在FilterPy中都有实现。

卡尔曼滤波器是递归的，但它将以前的所有观测值合并到当前的滤波器增益计算中。如果目标行为与过程模型一致，则这允许卡尔曼滤波器找到每个观测的最佳估计。考虑一个飞行中的球——如果我们考虑所有以前的观测结果，我们可以清楚地估计球在时间上的位置。如果我们只使用一些观测值，我们对当前位置的确定性就会降低，因此会受到观测中噪声的影响更大。如果观测值有噪声，则估计值也会有噪声。每次初始化卡尔曼滤波器时，我们都会看到这种效果。早期的估计是有噪声的，但随着获得更多的观测数据，它们就稳定下来了。

然而，如果目标是机动的，它的行为并不总是像过程模型预测的那样。在这种情况下，记住过去所有的观测和估计是一种负担。我们可以在上面所有的图中看到这一点。目标开始转弯，卡尔曼滤波器继续直线运动。这是因为滤波器已经建立了目标移动的历史记录，并且错误地以为：目标仍以给定的航向和速度沿直线移动。

衰减记忆过滤器通过减少旧观测的权重，增加新观测的权重来解决这个问题。

衰减记忆滤波器有很多公式。我使用了Dan Simon在《最优状态估计》中提供的方法。我将不讨论它的推导，只提供结果。

给出了协方差矩阵的卡尔曼滤波方程：

$$\bar{\mathbf{P}}={\mathbf{FPF}}^{\mathsf{T}}+\mathbf{Q}$$

我们可以通过乘以一项$\alpha$来迫使滤波器忘记过去的观测值：

$$\tilde{\mathbf{P}}={\alpha }^2{\mathbf{FPF}}^{\mathsf{T}}+\mathbf{Q}$$

其中：$\alpha >1.0$。如果$\alpha =1$，则得到正常的卡尔曼滤波性能。$\alpha$是KalmanFilter类的一个属性，其值默认为1，因此该滤波器的作用类似于卡尔曼滤波器，除非为$\alpha$指定了除1以外的值。选择$\alpha$没有硬性规定，但它通常非常接近1，比如1.01。你需要使用模拟或真实数据进行多次试验，以确定响应机动的值。从而避免由于对有噪声的观测值进行过度加权，导致估计值变得太过嘈杂。

为什么这样做会生效呢？如果我们增加协方差矩阵，滤波器的先验就变得更加不确定，因此它给观测赋予了更多的权重。

有一个警告——如果我们使用$\alpha$，那么我们计算的是$\tilde{\mathbf{P}}$，不是$\bar{\mathbf{P}}$。换句话说，KalmanFilter.P不等于先验的协方差，所以不要把它当作先验的协方差。

让我们使用衰减记忆滤波器对数据进行滤波，并查看结果。

pos2, zs2 = generate_data(70, std=1.2)
xs2 = pos2[:, 0]
z_xs2 = zs2[:, 0]

cvfilter = make_cv_filter(dt, std=1.2)
cvfilter.x.fill(0.)
cvfilter.Q = Q_discrete_white_noise(dim=2, dt=dt, var=0.02)
cvfilter.alpha = 1.00

xs, res = [], []
for z in z_xs2:
    cvfilter.predict()
    cvfilter.update([z])
    xs.append(cvfilter.x[0])
    res.append(cvfilter.y[0])
xs = np.asarray(xs)
plt.subplot(221)
bp.plot_measurements(z_xs2, dt=dt, label='z')
plt.plot(t[0:100], xs, label='filter')
plt.legend(loc=2)
plt.title('Standard Kalman Filter')

cvfilter.x.fill(0.)
cvfilter.Q = Q_discrete_white_noise(dim=2, dt=dt, var=20.)
cvfilter.alpha = 1.00

xs, res = [], []
for z in z_xs2:
    cvfilter.predict()
    cvfilter.update([z])
    xs.append(cvfilter.x[0])
    res.append(cvfilter.y[0])
    
xs = np.asarray(xs)

plt.subplot(222)
bp.plot_measurements(z_xs2, dt=dt, label='z')
plt.plot(t[0:100], xs, label='filter')
plt.legend(loc=2)
plt.title('$\mathbf{Q}=20$')

cvfilter.x.fill(0.)
cvfilter.Q = Q_discrete_white_noise(dim=2, dt=dt, var=0.02)
cvfilter.alpha = 1.02

xs, res = [], []
for z in z_xs2:
    cvfilter.predict()
    cvfilter.update([z])
    xs.append(cvfilter.x[0])
    res.append(cvfilter.y[0])
xs = np.asarray(xs)
plt.subplot(223)
bp.plot_measurements(z_xs2, dt=dt, label='z')
plt.plot(t[0:100], xs, label='filter')
plt.legend(loc=2)
plt.title('Fading Memory ($\\alpha$ = 1.02)')

cvfilter.x.fill(0.)
cvfilter.Q = Q_discrete_white_noise(dim=2, dt=dt, var=0.02)
cvfilter.alpha = 1.05

xs, res = [], []
for z in z_xs2:
    cvfilter.predict()
    cvfilter.update([z])
    xs.append(cvfilter.x[0])
    res.append(cvfilter.y[0])
xs = np.asarray(xs)
plt.subplot(224)
bp.plot_measurements(z_xs2, dt=dt, label='z')
plt.plot(t[0:100], xs, label='filter')
plt.legend(loc=2)
plt.title('Fading Memory ($\\alpha$ = 1.05)')

图一显示了卡尔曼滤波器的性能。当机动开始时，滤波器出现了明显的滞后现象。然后，我通过使过程噪声变大，使滤波器非常快速地跟踪机动，但由于对噪声观测值进行了不适当的加权，这会使滤波器估计值变得非常噪声。然后我实现了一个$\alpha =1.02$的衰减记忆滤波器。滤波后的估计非常平滑，但当目标恢复稳态行为时，确实需要几秒钟才能收敛。然而，这样做的时间比卡尔曼滤波器小得多，滞后量也小得多——对衰减记忆的估计比卡尔曼滤波器的轨迹更接近实际轨迹。最后，我把$\alpha$提高到1.05。在这里，我们可以看到，滤波器几乎立即对机动做出响应，但在稳态期间，由于滤波器忘记了过去的观测值，因此估计值不那么平缓。

代码上只需要做出如此小的变化，却能获得相当好的性能！请注意，这里没有正确的选择。你需要根据你的需求和观测噪声、过程噪声和目标机动行为的特点来设计滤波器。

多模型估计

我在本文中使用的例子可以描述为目标在稳定状态下移动，目标机动，然后返回稳定状态。我们一直认为这是两个模型——恒速度模型和恒加速度模型。只要你能把系统描述为服从一组有限模型中的一个，你就可以使用多模型（MM）估计。我们使用一组多个滤波器，每个滤波器使用不同的过程来描述系统，并根据跟踪对象的动力学在它们之间切换或混合。

正如你所想象的，这是一个广泛的话题，设计和实现MM估计器有很多方法。考虑本文中使用的例子，一个想法是同时运行一个恒速度和一个恒加速度滤波器，我们通过检查残差来检测机动，并在它们的输出之间切换。考虑汽车转弯的例子，这是一个非线性过程，因此为了获得最佳结果，我们希望使用某种类型的非线性滤波器（EKF、UKF等）来模拟转弯。另一方面，对于行程的稳态部分，线性恒速滤波器的性能良好。所以我们的滤波器组可能由线性KF和EKF滤波器组成。因此，一个高性能的MM估计器可能包含一组多个滤波器，每个滤波器都被设计为对被跟踪对象的某种特定性能表现最佳。

例如，在上面的部分中，我展示了许多描述参数变化对速度和位置估计的影响的图。也许一种设置对位置更有效，而另一种设置对速度更有效。那么，就把它们都放进你的滤波器里。然后，可以从一个滤波器中获取位置的最佳估计，并从另一个滤波器中获取速度的最佳估计。

双滤波器自适应滤波器

我相信在滤波器之间切换以获得最佳性能的想法是明确的，但我们应该使用什么数学基础来实现它呢？我们面临的问题是，试图通过有噪声的观测来检测，什么时候观测的变化要导致模型的变化。卡尔曼滤波器的哪个方面体现了观测值与预测值之间的偏差？是的，残差。

假设我们有一个一阶（恒速度）卡尔曼滤波器。只要目标没有移动，滤波器就会密切跟踪其行为，大约$68\%$的观测值应在$1\sigma$范围内。此外，残差应该在0左右波动，因为如果传感器是高斯的，那么正误差和负误差应该有相同数量的观测。如果残差增长并保持在预测范围之外，则目标就不得按照过程模型的预测执行。我们之前在这张图中看到了这一点，当被跟踪的物体开始机动时，残差从0左右波动，突然跳跃并保持在零以上。

show_residual_chart()

对于这个问题，我们发现当物体处于稳定状态时，恒速度滤波器比恒加速度滤波器性能更好。而当物体处于机动状态时，情况正好相反。在上面的图中，转换发生在$4s$。

所以算法很简单。初始化恒速度和恒加速度滤波器，并在预测/更新循环中一起运行它们。每次更新后，检查恒速度滤波器的残差部分。如果在理论范围内，则使用恒速度滤波器的估计值作为最终估计值，否则使用恒加速度滤波器的估计值。

def run_filter_bank(threshold, show_zs=True):
    dt = 0.1
    cvfilter= make_cv_filter(dt, std=0.8)
    cafilter = make_ca_filter(dt, std=0.8)
    pos, zs = generate_data(120, std=0.8)
    z_xs = zs[:, 0]
    xs, res = [], []

    for z in z_xs:
        cvfilter.predict()
        cafilter.predict()
        cvfilter.update([z])
        cafilter.update([z])
        
        std = np.sqrt(cvfilter.R[0,0])
        if abs(cvfilter.y[0]) < 2 * std:
            xs.append(cvfilter.x[0])
        else:
            xs.append(cafilter.x[0])
        res.append(cvfilter.y[0])
    xs = np.asarray(xs)
    if show_zs:
        plot_track_and_residuals(dt, xs, z_xs, res)
    else:
        plot_track_and_residuals(dt, xs, None, res)

run_filter_bank(threshold=1.4)

在这里，滤波器密切跟踪目标的整个稳定-机动-稳定的过程。当目标没有机动时，我们的估计几乎没有噪声，一旦目标机动，我们会迅速检测到，并切换到恒加速度滤波器。然而，这并不理想。以下是单独绘制的滤波器输出：

run_filter_bank(threshold=1.4, show_zs=False)

可以看到，当滤波器组从一个滤波器切换到另一个滤波器时，估计值会发生跳变。下面将介绍一个滤波器组的最新实现，它可以消除这个问题。

MMAE

使用多个滤波器来检测机动的核心思想是合理的，但当我们在滤波器之间突然切换时，会导致最终的估计是参差不齐的。多模型自适应估计器（MMAE）使用概率来确定观测和各模型的相似可能性，而摒弃直接选择其中的一个滤波器的估计值的做法。就像是在卡尔曼滤波中：我们既不选择观测值也不选择预测值，根据两者的可能性大小最终确定两者的混合。

在设计卡尔曼滤波器的章节中，我们学习了似然函数：

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

这告诉我们，在给定输入的情况下，一个滤波器达到最佳性能的可能性有多大。其中，$\mathbf{y}$是残差，$\mathbf{S}$是系统不确定性（观测空间下的协方差）。$\mathcal{L}$表述的是：残差在系统不确定性的高斯分布下的似然函数。较大的残差将产生较大的不确定性，因此观测值与滤波器当前状态匹配的可能性较低。我们可以用它来计算每个滤波器的似然，如果我们有$N$个滤波器，我们可以计算滤波器$i$相对于其余滤波器正确的概率：

$$p_k^i=\frac{{\mathcal{L}}_k^i{p}_{k-1}^i}{\sum _{j=1}^N{\mathcal{L}}_k^j{p}_{k-1}^j}$$

这看起来很混乱，但其实很简单。分子就是该滤波器在当前时间步与观测值的似然，乘以上个时间步正确的概率。同时，我们需要各个滤波器的所有概率总和为一，所以我们通过分母进行归一化。

这是一个递归定义，所以我们需要为每个滤波器分配一些初始概率。如果没有更好的信息，就每个滤波器都是$\frac{1}{N}$。同时，我们可以将每个滤波器的状态量乘以该滤波器正确的概率，然后求和，得到最终估计的状态。

下面是一个完整的实现：

def run_filter_bank():
    dt = 0.1
    cvfilter = make_cv_filter(dt, std=0.2)
    cafilter = make_ca_filter(dt, std=0.2)

    _, zs = generate_data(120, std=0.2)
    z_xs = zs[:, 0]
    xs, probs = [], []

    pv, pa = 0.8, 0.2
    pvsum, pasum = 0., 0.
    
    for z in z_xs:
        cvfilter.predict()
        cafilter.predict()
        cvfilter.update([z])
        cafilter.update([z])
        
        cv_likelihood = cvfilter.likelihood * pv
        ca_likelihood = cafilter.likelihood * pa
        
        pv = (cv_likelihood) / (cv_likelihood + ca_likelihood)
        pa = (ca_likelihood) / (cv_likelihood + ca_likelihood)
        
        x = (pv * cvfilter.x[0]) + (pa*cafilter.x[0])
        xs.append(x)
        probs.append(pv / pa)

    xs = np.asarray(xs)
    t = np.arange(0, len(xs) * dt, dt)
    plt.subplot(121)
    plt.plot(t, xs)
    plt.subplot(122)
    plt.plot(t, xs)
    plt.plot(t, z_xs)
    return xs, probs

xs, probs = run_filter_bank()

左图绘制了该滤波器最终的估计值，可以看到结果很平滑。而右图，我同时绘制上了观测值，以证明该滤波器可以迅速跟踪上机动。

我想再次强调，尽管MMAE的过程中有一些额外的操作，但本质上都是我们之前一直使用的贝叶斯算法。只不过我们有两个（或多个）估计值，且每个估计都有一个相关的正确率。我们将这些估计值进行一个加权组合作为最终的估计值，其中权重与对应的正确率成正比。每一步的概率计算如下：

$$\frac{\text{Prob(meas | state)}\times \text{prior}}{\text{normalization}}$$

MMAE滤波器的局限

MMAE存在一个重大问题。例如，下图描绘的是恒速度与恒加速度滤波器的概率比：

plt.plot(t[0:len(probs)], probs)
plt.title('probability ratio p(cv)/p(ca)')
plt.xlabel('time (sec)')

在前$3s$，当被跟踪目标静止时，等速度滤波器比等加速度滤波器更有可能。一旦机动开始，概率迅速变为有利于恒加速度模型。然而，机动的操作在$6s$时已经完成。你可能会期望等速度滤波器的概率会再次变大，但它仍然为零。（在前$3s$，比值是一个正数，不等于0。但相较于机动开始，比值太小。而$6s$后，比值就是0）

这是因为概率的递归计算：

$$p_k=\frac{\mathcal{L}{p}_{k-1}}{\sum \text{probabilities}}$$

一旦概率变得非常小，它就永远无法恢复。这样的结果是滤波器组会快速收敛于最可能的滤波器。一个稳健的方案是：需要监控每个滤波器的概率，同时废弃概率非常低的滤波器，并用表现良好的可能性更大的滤波器来替换它们。在最坏的情况下，如果滤波器发散，你可以重新初始化滤波器的状态，使其更接近当前观测值。

交互式多模型（IMM）

让我们以另一种方式考虑多个模型。场景和以前一样——我们希望跟踪一个机动目标。我们可以设计一组不同建模假设的卡尔曼滤波器。它们可以在滤波器阶数或过程模型中的噪声量等方面有所不同。因为当新观测传递到每个滤波器中，都有成为正确模型的概率。

这种方法可能会导致组合爆炸。在第1个循环，我们生成$N$个假设。在第2个循环，我们还有$N$个假设，然后需要将其与之前的$N$个假设相结合，从而产生$N^2$个假设。尽管可以尝试多种剪枝的方案，例如剔除不太可能的假设、合并相似的假设等，但仍然存在计算成本或性能不佳的问题。我不会在本文中介绍这些。

交互式多模型（IMM）算法是由Blom发明的，它用于解决多模型的组合爆炸问题。它的想法是：为系统的每种可能的行为模式设置一个滤波器。在每个循环中，我们让滤波器相互交互。更有可能的滤波器会修改不太可能的滤波器的估计，使它们更接近于系统的当前状态。当然，这种混合是概率性地完成，因此不太可能的滤波器也会修改可能的滤波器，但幅度要小得多。

也就是说，MMAE的各个滤波器只会根据各自的似然和历史的概率进行递归的改变（只是最终结果把各个滤波器的结果进行比例融合），而IMM会将各个滤波器之间进行概率的交互。

例如，假设我们有两种行为模式：直行或转弯。每个模式都由卡尔曼滤波器表示，可能是一阶和二阶滤波器。如果说它是转动的目标，那么二阶滤波器将产生良好的估计，而一阶滤波器将滞后于信号。每个滤波器的似然函数告诉我们哪个滤波器最有可能。一阶滤波器的似然较低，因此我们使用二阶滤波器大幅调整其估计。二阶滤波器的似然较高，所以它的估计只会被一阶卡尔曼滤波器稍微改变。

现在假设目标停止转动。因为我们一直在用二阶滤波器修正一阶滤波器的估计，所以它不会非常滞后于信号。在接着的几个循环内，它仍然会产生较好的（高可能性）估计，并将成为最可能的滤波器。然后它将开始对二阶滤波器的估计做出修正。回想一下，二阶滤波器会将观测噪声误认为是加速度。这种调整将大大降低了这种影响。

模式概率

我们为系统$m$定义一组模式，并假设目标始终处于其中的一种。例如，我们有直行和转弯模式，所以 m = { m=\{ m={直行$,$转弯$\}$。

我们再分配目标处于各个模式的概率，可以用概率向量来描述，即每个模式都有一个概率。$m$有两种模式，所以我们将有一个包含两个概率的向量。如果我们认为目标直线前进的可能性为$70\%$，我们可以说：

$$\mu =\left[\begin{array}{cc}0.7& 0.3\end{array}\right]$$

转弯模式的概率就是0.3，因为概率总和必须为1。$\mu$通常但不普遍用作表示模式概率的符号，所以我仍使用它。不要将它与均值混淆。

在Python中，我们可以将其实现为：

mu = np.array([0.7, 0.3])
mu

array([0.7, 0.3])

我们可以将它描述为机动目标正确地处于模式$m_i$的概率，假设给定的观测值是$Z$：

$${\mu }_i=P(m_i|Z)$$

模式转换

接下来我们必须考虑这是一个机动目标。它会直走，然后转弯，然后再直走。我们可以将这些模式之间的转换建模为马尔可夫链，如下图所示：

import kf_book.adaptive_internal as adaptive_internal

adaptive_internal.plot_markov_chain()

这显示了目标两种模式直行和转弯的示例。如果目标的当前模式是直线，那么我们预测目标有$97\%$的机会继续直线，并且有$3\%$的机会开始转弯。一旦目标转弯，我们就预测有$95\%$的机会继续转弯，有$5\%$的机会返回直线运动。

该算法对确切的数字并不敏感，你通常可以使用模拟或试验来选择合适的值。但是，这些值必须具有相当的代表性。

我们用转移概率矩阵表示马尔可夫链，我们将其称为$\mathbf{M}$。对于上图中的马尔可夫链，我们可以描述为：

$$\mathbf{M}=\left[\begin{array}{cc}.97& .03\\ .05& .95\end{array}\right]$$

换句话说，$\mathbf{M}[i,j]$是在上一个模式是$i$的情况下，切换到模式$j$的概率。在此示例中，假设上一个模式是转弯（$i=1$），当前模式为直线（$j=0$）的概率是$\mathbf{M}[1,0]=0.05$。在Python中，我们可以这样写：

M = np.array([[.97, .03], [.05, .95]])
print(M)
print('From turn to straight probablility is', M[1, 0], 'percent')

[[0.97 0.03]
 [0.05 0.95]]
From turn to straight probablility is 0.05 percent

这使我们能够根据转换的概率计算新的模式的概率。让我们计算模式为直线的概率：我们有两种方法可以获得直线前进。我们本来直行，然后继续直行，或者我们本来转弯，然后切换到直行。前者的概率用（$0.7\times 0.97$）计算，后者用（$0.3\times 0.05$）计算。我们将模式的概率与来自马尔可夫链的转换相关概率相乘，而总概率是两者之和，即$(0.7\times 0.97)+(0.3\times 0.05)=0.694$。

回忆全概率定理，它指出几个不同事件的概率是：

$$P(A)=\sum P(A|B)P(B)$$

其中，$P(A|B)$是转移矩阵$\mathbf{M}$，$P(B)$是$\mu$。我们可以利用了向量乘以矩阵的计算规律来简化编程：

$$\left[\begin{array}{cc}{\mu }_1& {\mu }_2\end{array}\right]\left[\begin{array}{cc}{m}_{11}& m_{12}\\ m_{21}& m_{22}\end{array}\right]=\left[\begin{array}{cc}{\mu }_1{m}_{11}+{\mu }_2{m}_{21}& {\mu }_1{m}_{12}+{\mu }_2{m}_{22}\end{array}\right]$$

也可表示为：

$${\bar{c}}_j=\sum _{i=1}^N{\mu }_i{M}_{ij}$$

我们可以使用NumPy的dot()函数为我们计算这个值。我们也可以使用矩阵乘法运算符@，但我发现使用dot()作为求和符号，即点积，更直观：

cbar = np.dot(mu, M)
cbar

array([0.694, 0.306])

计算模式的概率

我们将使用贝叶斯定理计算模式的概率。回想一下贝叶斯定理：

$$\text{posterior}=\frac{\text{prior}\cdot \text{likelihood}}{\text{normalization factor}}$$

这里的先验是我们在上文计算的总概率计算。卡尔曼滤波器计算似然度，这是给定滤波器当前状态的观测值的似然度。回顾一下，方程式是：

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

用数学符号描述的话，更新的模式概率为：

$${\mu }_i=\Vert {\mathcal{L}}_i{\bar{c}}_i\Vert$$

换句话说，对于每个卡尔曼滤波器（模式）：我们将考虑了模式转换后的模式概率，乘以该模式正确的可能性（似然）。然后将所有概率归一化，使它们总和为1。

这在Python中计算起来很简单。我将引入变量L来存储似然。似然是由KalmanFilter.update()步骤计算的，在下面的代码片段中，我只是硬编码了L的值，因为我们还没有创建卡尔曼滤波器：

# L = [kf0.L, kf1.L]  # get likelihoods from Kalman filters
L = [0.000134, 0.0000748] 
mu = cbar * L
mu /= sum(mu) # normalize
mu

array([0.802, 0.198])

在这里你可以看到，相对更高似然的滤波器的可能性从$70\%$提高到了$80.2\%$。

混合概率

此时，我们可以使用模式转换来计算所有可能选择的概率。如果$\mu =\left[\begin{array}{cc}0.63& 0.27\end{array}\right]$，那么我们可以使用转移概率矩阵来计算所有可能的结果。换句话说，如果当前模式是直线（$\mu =0.63$），我们可以根据目标是保持直线移动还是切换到转弯来计算两个新的概率。我们对转动模式做同样的事情（$\mu =0.27$）。我们将从2种模式概率变为4种，在下一步中，4种将变为8种，依此类推。它在计算上是精确的，但在实践中是不可行的。仅需要30个循环之后，你就需要$8GB$的​​内存来以双精度存储模式概率。

我们需要一个更好的，哪怕是近似的方法。IMM通过计算混合概率来解决这个问题。

我们需要做的非常简单：对每个卡尔曼滤波器的状态进行混合，计算新的均值和协方差。即我们根据混合概率$w$计算每个滤波器的新的均值和协方差。最终产生的效果：比较可能的滤波器会被不太可能的滤波器轻微调整，而不太可能的滤波器将被比较可能的滤波器强烈调整。这些调整后的均值和协方差，我分别使用符号${\mathbf{x}}_j^m$和${\mathbf{P}}_j^m$表示。表达式为：

$$\begin{array}{rl}{\mathbf{x}}_j^m& =\sum _{i=1}^N{\omega }_{ij}{\mathbf{x}}_i\\ {\mathbf{P}}_j^m& =\sum _{i=1}^N{\omega }_{ij}\left[({\mathbf{x}}^i-{\mathbf{x}}_i^m)({\mathbf{x}}^i-{\mathbf{x}}_i^m{)}^{\mathsf{T}}+{\mathbf{P}}_i\right]\end{array}$$
只需将下标视为数组的索引。把它放在Python中，我们可以这样写：

for j in N:
    x0[j] = sum_over_i(w[i,j] * x[i])
    P0[j] = sum_over_i(w[i, j] * (P[i] + np.outer(x[i] - x0[j])))

不要让符号混淆一个简单的想法：将比较可能的滤波器的估计值合并到不太可能的滤波器的估计值中，确保所有滤波器都有一个好的估计值。

我们如何计算混合概率？我们有模式概率来描述每个模式的当前概率，然后是转移概率来描述我们改变模式的可能性。我们如何计算新的概率？

贝叶斯定理，当然！先验乘以似然，归一化。先验是模式概率，似然来自马尔可夫链，我们存储在矩阵$\mathbf{M}$中。

$${\mathbf{\omega }}_{ij}=\Vert {\mu }_i\cdot {\mathbf{M}}_{ij}\Vert$$

我们可以如下计算：

cbar = np.dot(mu, M) #compute total probability that target is in mode j

omega = np.zeros((2, 2))
for i in range(2):
    for j in range(2):
        omega[i, j] = (M[i, j] * mu[i]) / cbar[j]
omega

array([[0.987, 0.114],
       [0.013, 0.886]])

卡尔曼滤波器需要执行预测步来计算新的先验。他们使用混合估计：

$$\begin{array}{rl}{\bar{\mathbf{x}}}_j& ={\mathbf{F}}_j{\mathbf{x}}_j^m\\ {\bar{\mathbf{P}}}_j& ={\mathbf{F}}_j{\mathbf{P}}_j^m{\mathbf{F}}_j^{\mathsf{T}}+{\mathbf{Q}}_j\end{array}$$

IMM估计

现在我们需要计算来自滤波器组的最终状态估计。我们如何做到这一点？只需对每个卡尔曼滤波器的混合估计进行加权：

$$\begin{array}{rl}\mathbf{x}& =\sum _{j=1}^N{\mu }_j{\bar{\mathbf{x}}}_j\\ \mathbf{P}& =\sum _{j=1}^N{\mu }_j\left[({\bar{\mathbf{x}}}_j-\bar{\mathbf{x}})({\bar{\mathbf{x}}}_j-\bar{\mathbf{x}}{)}^{\mathsf{T}}+\overline{{\mathbf{P}}_j}\right]\end{array}$$

使用IMM跟踪机动目标

让我们举个例子。假设跟踪一个移动的目标$600s$，目标开始直线移动，然后从$400s$开始有控制输入，使得目标旋转$90^{\circ }$。假设使用两个恒定加速度卡尔曼滤波器。一个滤波器假设没有过程噪声，另一个假设有一定的的过程噪声。假设滤波器的初始化非常好，两个滤波器的初始$\mathbf{P}=10^{-12}$。代码实现如下：

import copy
from scipy.linalg import block_diag
from filterpy.kalman import IMMEstimator
from filterpy.common import Saver

N = 600
dt = 1.
imm_track = adaptive_internal.turning_target(N)

# create noisy measurements
zs = np.zeros((N, 2))
r = 1
for i in range(N):
    zs[i, 0] = imm_track[i, 0] + randn()*r
    zs[i, 1] = imm_track[i, 2] + randn()*r

ca = KalmanFilter(6, 2)
dt2 = (dt**2)/2
F = np.array([[1, dt, dt2],
              [0,  1,  dt],
              [0,  0,   1]])
            
ca.F = block_diag(F, F)
ca.x = np.array([[2000., 0, 0, 10000, -15, 0]]).T
ca.P *= 1.e-12
ca.R *= r**2
q = np.array([[.05, .125, 1/6],
              [.125, 1/3, .5],
              [1/6, .5, 1]])*1.e-3
ca.Q = block_diag(q, q)
ca.H = np.array([[1, 0, 0, 0, 0, 0],
                 [0, 0, 0, 1, 0, 0]])

# create identical filter, but with no process error
cano = copy.deepcopy(ca)
cano.Q *= 0

filters = [ca, cano]

M = np.array([[0.97, 0.03],
              [0.03, 0.97]])
mu = np.array([0.5, 0.5])
bank = IMMEstimator(filters, mu, M)

xs, probs = [], []
for i, z in enumerate(zs):
    z = np.array([z]).T
    bank.predict()
    bank.update(z)

    xs.append(bank.x.copy())
    probs.append(bank.mu.copy())

xs = np.array(xs)
probs = np.array(probs)
plt.subplot(121)
plt.plot(xs[:, 0], xs[:, 3], 'k')
plt.scatter(zs[:, 0], zs[:, 1], marker='+')

plt.subplot(122)
plt.plot(probs[:, 0])
plt.plot(probs[:, 1])
plt.ylim(-1.5, 1.5)
plt.title('probability ratio p(cv)/p(ca)')

滤波器的性能总体上看的话，比较难看出差别，所以让我们从转弯后开始比较。我交换了$x$轴和$y$轴，并放大图像，让我们可以更清楚地看到。在下图中，转弯从$Y=4000$开始。如果你仔细观察，会发现在转弯开始后估计值略有波动，但滤波器跟踪观测值并没有滞后，很快就可以顺利跟踪。

plt.plot(xs[390:450, 3], xs[390:450, 0], 'k')
plt.scatter(zs[390:450, 1], zs[390:450, 0], marker='+', s=100)
plt.xlabel('Y'); plt.ylabel('X')
plt.gca().invert_xaxis()
plt.axis('equal')

IMM的局限性

我没有很广泛地使用过IMM，因此我不能给予太多这方面的经验和教训。然而，IMM的发明是为了跟踪机动飞机进行空中交通管制，据所有的报道，它在这方面表现出色。

这个用例假设了一些事情。其中最重要的是要求所有滤波器具有相同的尺寸设计。回顾一下数学公式，就应该很能说明原因。最终创建混合估计，IMM将执行以下计算：

$$\mathbf{x}=\sum _{j=1}^N{\mu }_j{\bar{\mathbf{x}}}_j$$

当且仅当每个滤波器中的状态$\mathbf{x}$都具有相同维度时，这才是可计算的。此外，每个滤波器对$\mathbf{x}$的解释必须相同。

例如，假设我们尝试使用恒定速度模型和恒定加速度模型进行滤波。这将不起作用，因为$\mathbf{x}$的维度不同。如果尝试使用不同维度的滤波器，FilterPy将引发ValueError。

ca = KalmanFilter(3, 1)
cv = KalmanFilter(2, 1)

trans = np.array([[0.97, 0.03],
                  [0.03, 0.97]])

imm = IMMEstimator([ca, cv], (0.5, 0.5), trans)

---------------------------------------------------------------------------
ValueError                                Traceback (most recent call last)
<ipython-input-35-e96cb02ea05b> in <module>
      5                   [0.03, 0.97]])
      6 
----> 7 imm = IMMEstimator([ca, cv], (0.5, 0.5), trans)

C:\dev\filterpy\filterpy\kalman\IMM.py in __init__(self, filters, mu, M)
    140             if x_shape != f.x.shape:
    141                 raise ValueError(
--> 142                     'All filters must have the same state dimension')
    143 
    144         self.x = zeros(filters[0].x.shape)

ValueError: All filters must have the same state dimension

我偶尔会收到关于这个异常的电子邮件或bug报告。在以前，我的建议是将恒速度滤波器也设计为3维，然后使用$\mathbf{F}$来忽略加速度：

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

现在回想起来，我不确定这是否是一个合理的建议。这样做允许IMM工作，但加速度的估计显然是不正确的，因为一个滤波器将准确估计加速度，而另一个滤波器的估计值为0。这种不准确的加速度也会用于执行下一个循环。

考虑一个更极端的情况。假设其中一个滤波器将x[2]解释为加速度，另一个滤波器将其解释为角旋转速率。很明显，x[2]的混合估计是没有意义的，因为你不能将（线性）加速度和旋转速率相加。

正如我所说，我对IMM不是特别精通。也许一些文献中解释了如何处理这些情况。我只能说FilterPy实现的IMM不能适用于这些用例。

为空中交通管制设计的IMM使用具有不同过程模型假设的滤波器。飞机可以水平飞行，可以下降/上升，可以执行协调转弯，也可以执行不协调转弯。你可以用不同的$\mathbf{F}$和$\mathbf{Q}$矩阵为每种情况设计一个滤波器，但状态估计$\mathbf{x}$对所有情况都是相同的。

IMM流程补充（译者补）

在其他的一些文献中，我们会发现IMM的顺序和上文介绍的顺序不同。现在就对这些内容进行补充。

输入模型交互

根据前一时刻系统状态估计和协方差，对模型进行重新初始化计算，其中新的初始值是通过不同模型之间的马尔科夫运算矩阵获得的。这一步对应上文的模式转换、混合概率。

$$\begin{array}{rl}{\mathbf{x}}_j^m& =\sum _{i=1}^N{\omega }_{ij}{\mathbf{x}}_i\\ {\mathbf{P}}_j^m& =\sum _{i=1}^N{\omega }_{ij}\left[({\mathbf{x}}^i-{\mathbf{x}}_i^m)({\mathbf{x}}^i-{\mathbf{x}}_i^m{)}^{\mathsf{T}}+{\mathbf{P}}_i\right]\end{array}$$

其中，${\mathbf{\omega }}_{ij}=\Vert {\mu }_i\cdot {\mathbf{M}}_{ij}\Vert$。

滤波器的滤波

滤波器组内的各个滤波器执行各自的预测-更新循环。

模型概率的更新

一般采用极大似然函数法实现模型概率的更新：通过计算当前模型和当前运动目标状态的相似度来给出当前最适合跟踪模型所占权重。这一步对应上文的计算模式的概率。

$${\mu }_i=\Vert {\mathcal{L}}_i{\bar{c}}_i\Vert$$

其中，$\mathcal{L}=\frac{1}{\sqrt{2\pi \mathbf{S}}}\exp [-\frac{1}{2}{\mathbf{y}}^{\mathsf{T}}{\mathbf{S}}^{-1}\mathbf{y}]$，${\bar{c}}_j=\sum _{i=1}^N{\mu }_i{M}_{ij}$。

新的数据融合

根据每个模型单独计算出的估计结果和模型匹配的权重，给出$k$时刻交互数据的最终输出结果。这一步对应上文的IMM估计。

$$\begin{array}{rl}\mathbf{x}& =\sum _{j=1}^N{\mu }_j{\bar{\mathbf{x}}}_j\\ \mathbf{P}& =\sum _{j=1}^N{\mu }_j\left[({\bar{\mathbf{x}}}_j-\bar{\mathbf{x}})({\bar{\mathbf{x}}}_j-\bar{\mathbf{x}}{)}^{\mathsf{T}}+\overline{{\mathbf{P}}_j}\right]\end{array}$$

总结

本文包含了一些更具挑战性的滤波情况。假设我们控制一个机器人，我们知道它的过程模型，并且可以很容易为它构造一个卡尔曼滤波器。但更常见的情况是，过程模型对我们来说是未知的。我们可以使用本文中的技术，来学习（从机器学习的角度）如何参数化我们的模型。随着目标机动，模型会随时间变化，因此我们的滤波器必须是自适应的。

寻找最优答案可能会导致组合爆炸，在实际情况下是不切实际的。IMM算法因其良好的性能和计算的可处理性而成为标准算法。

一个真正的滤波器组通常包含两个以上的滤波器。随着时间的变化，一些滤波器变得极有可能。大多数自适应滤波器都会实现一种算法，可以消除极不可能出现的滤波器，并用与当前状态更接近的滤波器来取代它们。你需要设计销毁和创建滤波器的方式方法，并采用模拟数据或真实数据对其进行测试。

尽管算法看上去很复杂，但我希望你能认识到其基本思想非常简单。我们使用了两个工具：贝叶斯定理和全概率定理。我们使用贝叶斯定理合并新信息，并使用全概率定理计算过程模型的影响。

补充阅读

• Kalman-and-Bayesian-Filters-in-Python/blob/master/14-Adaptive-Filtering
• 交互式多模型 IMM的原理及代码实现（matlab）