本文主要翻译自rlabbe/Kalman-and-Bayesian-Filters-in-Python的第9章节09-Nonlinear-Filtering（非线性滤波）。

%matplotlib inline

#format the book
import book_format
book_format.set_style()

介绍

我们开发的卡尔曼滤波器使用线性方程组，因此该滤波器只能处理线性问题。但是这个世界是非线性的，所以我们一直在研究的经典滤波器的使用范围非常有限。

过程模型可能存在非线性。假设我们想要追踪一个从大气坠落的物体，物体的加速度取决于它遇到的阻力。阻力取决于空气密度，空气密度随高度降低。在一维空间中，这可以用非线性微分方程来建模：

$$\ddot{x}=\frac{0.0034g{e}^{(-x/22000)}{\dot{x}}^2}{2\beta }-g$$

非线性的第二个来源是观测模型。例如，雷达测量与物体之间的直线距离，而我们通常对飞机在地面上的投影位置的距离感兴趣。我们调用Pythagoras公式，得到非线性方程：

$$x=\sqrt{slan{t}^2-altitud{e}^2}$$

在Kalman博士发表论文后不久，人们开始研究如何将卡尔曼滤波器扩展到非线性问题。

几乎可以肯定的是，唯一任何人都知道如何求解的方程就是$\mathbf{Ax}=\mathbf{b}$了。我可以给你任何线性方程组，你可以解它，也可以证明它没有解。

任何受过数学或物理正规教育的人都花了数年时间学习各种解析方法来解积分、微分方程等。然而，任何学科和现实问题都可能会产生无法解析求解的方程。我可以随意取一个方程，你可以通过积分，插入一个对数项等操作，使它不可解。这导致了物理学家们的笑话，他们说假设一个处于真空中的球，在无摩擦的表面上……如果不进行极端简化，大多数物理问题就没有解析解。

我们如何在计算机中模拟飞机上方的气流，或者预测天气，或者使用卡尔曼滤波器跟踪导弹？我们重新回到我们所知道的：$\mathbf{Ax}=\mathbf{b}$。我们可以找到一些方法来线性化问题，将其转化为一组线性方程组，然后使用线性代数软件包来计算近似解。

将非线性问题线性化会给我们带来不精确的答案，而在像卡尔曼滤波器或天气预测系统这样的递归算法中，这些小错误有时会在每一次递归中相互强化，很快导致最终结果相差很远。

我们即将着手的是一个难题：再也没有一个明显的、正确的、数学上最优的解决方案了。我们将使用近似法，我们将在计算中引入误差，我们将永远与发散的滤波器作斗争。所谓发散，也就是说，数值误差淹没了正确结果的滤波器。

接下来，我将说明非线性卡尔曼滤波器面临的具体问题。只有在了解问题中非线性导致的特定问题后，才能设计好滤波器。

非线性带来的问题

卡尔曼滤波器的数学部分的巧妙，是因为高斯方程的特殊性。如果滤波器是非线性的，例如一个高斯分布经过相加和相乘操作后，得到另一个高斯分布，这是非常罕见的。

我所说的线性可能是显而易见的，但也有一些微妙之处。数学要求有两方面：

• 可加性：$f(x+y)=f(x)+f(y)$
• 同质性：$f(ax)=af(x)$

这使我们可以说，线性系统定义为其输出与其所有输入之和成线性比例的系统。这样做的结果是，要想是线性系统，如果输入为零，那么输出也必须为零。考虑音频放大器——如果我对着麦克风讲话，你也开始说话，那么输出应该是我们的声音之和（输入），再乘以放大器的增益。但是，如果放大器零输入，但输出非零信号，则加法关系不再成立。这是因为线性要求$amp(voice)=amp(voice+0)$。这很显然，如果要得到相同的输出，而$amp(0)$非零，则：

$$amp(voice)=amp(voice+0)=amp(voice)+amp(0)=amp(voice)+none\_zero\_value$$

这显然是胡说八道。因此，对于一个线性方程$f(x)$，而方程$L()$若满足：

$$L(f(t))=f(t)+1$$

那么，方程$L()$不是线性的，因为$L(0)=1$。小心！

直觉地看待问题

考虑一个跟踪问题，我们需要跟踪目标的位置，而可以观测得到的是与目标之间的距离和方向。现有一个目标，与观测点之间的准确距离为$50km$，准确方向为$90^{\circ }$。假设距离和方向的误差均以高斯方式分布。给定无限多个观测值，位置的预期值是多少？

我一直建议使用直觉来获得洞察力，所以让我们看看它如何解决这个问题。我们可以推断：因为距离的平均值是$50km$，方向的平均值是$90^{\circ }$，因而猜测位置的预期值是$x=0km$，$y=50km$。

让我们试验验证一下。假设有5000个点，它们的距离服从$0.4km$的正态分布，角度服从$0.35rad$的正态分布。我们计算所有位置的平均值，并将其用星形绘制出来，我们的直觉值用圆圈绘制出来。

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

N = 5000
a = np.pi/2. + (randn(N) * 0.35)
r = 50.0     + (randn(N) * 0.4)
xs = r * np.cos(a)
ys = r * np.sin(a)

plt.scatter(xs, ys, label='Sensor', color='k', 
            alpha=0.4, marker='.', s=1)
xmean, ymean = sum(xs) / N, sum(ys) / N
plt.scatter(0, 50, c='k', marker='o', s=200, label='Intuition')
plt.scatter(xmean, ymean, c='r', marker='*', s=200, label='Mean')
plt.axis('equal')
plt.legend()

我们可以看到，我们的直觉失败了，因为问题的非线性迫使所有的误差都偏向一个方向。在多次迭代中，这种偏差会导致卡尔曼滤波器发散。即使没有发散，解决方案也不是最优的。应用于非线性问题的线性近似会产生不准确的结果。

非线性函数对高斯分布的影响

高斯分布在任意非线性函数下是不闭合的。回想一下卡尔曼滤波器的方程——在每次迭代中，我们通过状态转移方程传递代表状态量的高斯分布，得到时间$k+1$时刻的高斯分布。我们的状态转移方程始终是线性的，因此输出总是另一个高斯分布。让我们从图表上看一下。我将取一个任意的高斯分布，通过函数$f(x)=2x+1$并绘制结果。我们使用采样的方式来体现结果：我将以正态分布生成500000个点，通过$f(x)$，并绘制结果。

from numpy.random import normal

data = normal(loc=0., scale=1., size=500000)
plt.hist(2*data + 1, 1000)

这是一个意料之中的结果。通过$f(x)=2x+1$传递高斯分布的结果是以1为中心的另一个高斯分布。让我们同时看看输入、状态转移方程和输出。

from kf_book.book_plots import set_figsize, figsize
from kf_book.nonlinear_plots import plot_nonlinear_func

def g1(x):
    return 2*x+1

plot_nonlinear_func(data, g1)

标有Input的图是原始数据的曲线。它通过函数$f(x)=2x+1$传递，该曲线显示在左下角的图表中。红线显示了其中一个值（$x=0$）是如何通过函数传递的。输入的每个值都以相同的方式传递到上侧的输出函数。对于输出，我通过取所有点的平均值来计算平均值，并用蓝色虚线绘制结果。可以看出，输出看起来很像高斯分布，实际上就是高斯分布。我们可以看到，输出中的方差大于输入中的方差，并且平均值已从0移动到1，这是我们在给定传递函数$f(x)=2x+1$的情况下所期望的，$2x$影响方差，$+1$影响平均值。蓝色虚线所表示的计算平均值几乎等于实际平均值。

现在我们来看一个非线性函数，看看它是如何影响概率分布的。

def g2(x):
    return (np.cos(3*(x/2 + 0.7))) * np.sin(0.3*x) - 1.6*x

plot_nonlinear_func(data, g2)

这个结果可能会让你有些惊讶。函数看起来相当线性，但输出的概率分布却与高斯分布完全不同。回想一下两个单变量高斯分布相乘的方程式：

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

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

这些方程不适用于非高斯分布，当然也不适用于上面Output图表中所示的概率分布。

这里有另一种方法来查看与散点图相同的数据。

N = 30000
plt.subplot(121)
plt.scatter(data[:N], range(N), alpha=.1, s=1.5)
plt.title('Input')
plt.subplot(122)
plt.title('Output')
plt.scatter(g2(data[:N]), range(N), alpha=.1, s=1.5)

原始数据明显是高斯分布，但通过$g2()$函数的数据不再是正态分布：条带两侧的点分布不均匀，正方向的明显密于负方向的。如果你将其与上一张图表中标记为Output的pdf（概率密度函数）进行比较，你应该能够看到pdf形状是如何匹配$g()$的分布。

想想这卡尔曼滤波算法意味着什么。所有的方程都假设一个高斯分布通过状态转移方程会产生另一个高斯分布。如果这不是真的，那么卡尔曼滤波器的所有假设和保证都不成立。让我们看看当我们再次通过函数传递输出时会发生什么，模拟卡尔曼滤波器的下一个时间步。

y = g2(data)
plot_nonlinear_func(y, g2)

正如你所看到的，概率密度函数比高斯分布进一步失真了。然而，这个图在$x=0$附近仍然可以近似地认为是对称的，让我们看看这是什么意思。

print('input  mean, variance: %.4f, %.4f' % 
      (np.mean(data), np.var(data)))
print('output mean, variance: %.4f, %.4f' % 
      (np.mean(y), np.var(y)))

input  mean, variance: -0.0006, 1.0033
output mean, variance: -0.1243, 2.4105

让我们将高斯分布通过以$(-2,3)$和$(2,-3)$确定的线性函数，会发现最后的输出与我们之前绘制的非线性函数有相似之处。用我们得到的直线方程：

$$m=\frac{-3-3}{2-(-2)}=-1.5$$

def g3(x): 
    return -1.5 * x

plot_nonlinear_func(data, g3)
out = g3(data)
print('output mean, variance: %.4f, %.4f' % 
      (np.mean(out), np.var(out)))

output mean, variance: 0.0009, 2.2575

虽然输出的形状非常不同，但输出的均值和方差比较接近。这可能导致我们推断，如果非线性方程接近线性，我们也许可以忽略这个问题。为了测试这一点，我们可以迭代几次，然后比较结果。

out = g3(data)
out2 = g2(data)

for i in range(10):
    out = g3(out)
    out2 = g2(out2)
print('linear    output mean, variance: %.4f, %.4f' % 
      (np.average(out), np.std(out)**2))
print('nonlinear output mean, variance: %.4f, %.4f' % 
      (np.average(out2), np.std(out2)**2))

linear    output mean, variance: 0.0511, 7506.6256
nonlinear output mean, variance: -9.0540, 30529.4381

不幸的是，非线性版本是不稳定的。它明显偏离了0的平均值，方差也相差好几个数量级。

我使用了一个非常接近直线的函数来最小化这个问题。如果函数是$y(x)=-x^2$？

def g3(x): 
    return -x*x

data = normal(loc=1, scale=1, size=500000)
plot_nonlinear_func(data, g3)

尽管曲线在$x=1$时平滑且相当笔直，但输出的概率分布看起来并不像高斯分布，并且输出的计算平均值与直接计算的值相差很大。这不是一个很罕见的函数——弹道物体以抛物线运动，这就是你的滤波器需要处理的非线性类型。此图应能让你深入了解滤波器性能如此差的原因。

二维例子

让我们考虑一下用雷达跟踪飞机，假设当前估计值的协方差如下所示：

import kf_book.nonlinear_internal as nonlinear_internal

nonlinear_internal.plot1()

当我们试图线性化这个问题时会发生什么？

首先，雷达给了我们飞机的直线距离。假设在开始时刻，雷达在飞机正下方$(x=10)$，下一次观测表明飞机与雷达的直线距离为3。可以匹配该观测值的位置形成一个半径为3的圆，如下所示：

nonlinear_internal.plot2()

通过检查，我们可以看到飞机的可能位置在$x=11.4$，$y=2.7$附近，因为这是协方差椭圆和距离观测重叠的地方。但由于距离观测是非线性的，所以我们必须将其线性化。在$x=10$时，距离观测值为$y=3$，因此我们在该点处线性化。

nonlinear_internal.plot3()

现在我们有了该问题的线性表示，我们可以解决它。不幸的是，你可以看到直线和协方差椭圆的交点，距离飞机的实际位置很远。

nonlinear_internal.plot4()

这种错误常常导致灾难性的后果，这个估计的误差很大。但在滤波器的下一次迭代中，非常糟糕的估计将用于线性化下一次雷达测量，因此下一次的估计可能比这次还要差。在仅仅几次迭代之后，卡尔曼滤波器就会发散，并开始产生与现实完全不符的结果。

这个协方差的椭圆跨越很大，我夸大了其规模，来说明非线性系统的困难。在实际的雷达跟踪问题中，非线性通常不是很严重，但误差仍然会累积。你使用的其他系统可能会有这么多的非线性——这不是一个夸张的说法。只是为了说明一点：在处理非线性系统时，你将始终与发散作斗争。

滤波器算法

你可能迫不及待地想解决一个特定的问题，并想知道使用哪种滤波器。接下来的章节就是讲述解决非线性问题的滤波器方案，并且章节之间基本相互独立，你可以跳着阅读。如果你真的想掌握所有的内容，我建议你线性阅读。

解决非线性问题的滤波器，最常见的就是无迹卡尔曼滤波器（UKF）和扩展卡尔曼滤波器（EKF）。这两种技术是在Kalman发表论文后不久发明的，从那时起，这两种技术一直是使用的主要技术。飞机上的飞行软件、汽车或手机上的GPS几乎肯定会使用这些技术之一。

然而，这些技术对你的知识积累提出了额外的要求。EKF在某一点将非线性问题线性化，这要求你能够找到雅可比矩阵。如果不能找到解析解，你只能使用数值技术来找到雅可比矩阵，但这在会带来很大的计算消耗的，并且会给系统带来更多的误差。如果问题是非常非线性的，线性化会导致在每个步骤中引入大量误差，从而导致滤波器经常发散。你没法把一个非线性方程扔进解算器，一下子就得到好的结果。我注意到，尽管EKF是现实世界应用中最常用的技术，但大多数Kalman滤波教科书都掩盖了它。

后来这个领域发生了一些令人兴奋的变化。首先，计算能力已经发展到我们可以使用曾经超出计算机能力的技术。这些都使用蒙特卡罗技术——计算机生成数千到数万个随机点，并根据观测结果对所有点进行测试。然后，它根据随机点与观测值的匹配程度，概率地删除或复制点。远离测量的点不太可能被保留，而非常接近的点很可能被保留。经过几次迭代后，会有一个粒子团紧密跟踪对象，在没有对象的地方只会有一个稀疏的点云。

这有两个好处。首先，该算法对于那些非常非线性的问题也具有鲁棒性。其次，该算法可以一次跟踪任意多个对象——一些粒子将匹配一个对象，而其他粒子将匹配其他对象。所以这项技术经常被用于跟踪汽车交通、人群等。

但是成本应该也是明确的：对于滤波器中的每一步，测试数万个点的计算成本很高。另一个代价是答案不是数学的。通过卡尔曼滤波器，协方差矩阵为我提供了有关估计误差量的重要信息。粒子滤波器并没有给出一个严格的计算方法。最后，滤波器的输出是点，我们必须弄清楚如何解释它。通常你会做一些事情，比如取点的平均值和标准差，但这是一个困难的问题。仍然有许多点不属于被跟踪对象，因此首先必须运行某种聚类算法，以首先找到似乎正在跟踪对象的点，然后需要另一种算法从这些点生成状态估计。所有这些都不难解决，但在计算上都相当昂贵。

最后，我们有一个新的算法称为无迹卡尔曼滤波（UKF）。它不需要你找到非线性方程的解析解，但几乎总是比EKF性能更好。而且该滤波器的设计也非常简单。我建议UKF应该是任何实现的起点，特别是如果你不是控制理论研究生学位的卡尔曼滤波器专业人员。主要的缺点是UKF可能比EKF慢一些，但这实际上取决于EKF是通过解析法还是通过数值方法求解雅可比矩阵。UKF总是比EKF产生更准确的结果，这一点尚未得到证明（可能也无法证明）。在实践中，它几乎总是这样，而且往往意义重大。它很容易理解和实现，我强烈建议你从这个滤波器开始。

总结

世界是非线性的，但我们只知道如何解决线性问题。这给卡尔曼滤波器带来了很大的困难。我们已经研究了非线性如何影响滤波的，我给了你们解决方法的简要总结：扩展卡尔曼滤波、无迹卡尔曼滤波和粒子滤波。

直到最近，EKF一直是解决这些问题的标准方法。它很难理解和使用，而且可能非常不稳定。

最近的技术发展为我提供了更优越的方法。UKF不需要寻找雅可比矩阵，但它通常也比EKF更精确。它易于使用和理解。我可以通过使用FilterPy在几分钟内得到一个基本的UKF。粒子滤波器完全不需要数学建模，而是采用蒙特卡罗技术生成数千个点。它运行缓慢，但可以相对轻松地解决其他棘手的问题。

我收到的关于EKF的电子邮件比其他的都多：我怀疑这是因为书籍、报纸和互联网上的大多数资料都使用EKF。如果你对掌握这个领域感兴趣，你当然会想了解EKF。但是如果你只是想得到好的结果，我会先推荐你UKF和粒子滤波器。它们更易于实现、理解和使用，并且通常比EKF稳定得多。

我对这一点感到厌烦是因为在大多数教科书中，EKF被放在了中心位置，UKF要么根本没有被提及，要么只是给了一个很短的介绍，让你完全没有准备好使用该滤波器。UKF仍然相对较新，编写新版本的书籍需要时间。在很多书被写的时候，UKF要么还没有被发现，要么只是一个未经证实但有希望的滤波器。但在我写这篇文章的时候，UKF已经取得了巨大的成功，它需要放在你的工具包中。这就是我将花费我大部分的努力来教你的。

相关阅读

• Kalman-and-Bayesian-Filters-in-Python/blob/master/09-Nonlinear-Filtering