本文主要翻译自rlabbe/Kalman-and-Bayesian-Filters-in-Python的第10章节10-Unscented-Kalman-Filter（无迹卡尔曼滤波）。

%matplotlib inline

# format the book
import book_format
book_format.set_style()

前文

在上一章中，我们讨论了非线性系统带来的困难。这种非线性可以出现在两个地方。它可能出现在我们的观测值里，也可能发生在过程模型中。对于这些问题，标准卡尔曼滤波器的性能可能会很差。

在上一章中，我向你们展示了这样的一个现象（为了强调非线性的影响，我对方程做了一些修改）：

from kf_book.book_plots import set_figsize, figsize
import matplotlib.pyplot as plt
from kf_book.nonlinear_plots import plot_nonlinear_func
from numpy.random import normal
import numpy as np

# create 500,000 samples with mean 0, std 1
gaussian = (0., 1.)
data = normal(loc=gaussian[0], scale=gaussian[1], size=500000)

def f(x):
    return (np.cos(4*(x/2 + 0.7))) - 1.3*x

plot_nonlinear_func(data, f)

我通过从输入中采样500000个样本，将其通过非线性变换，并构建输出的直方图来显示结果。我们称这些点为sigma点。根据输出直方图，我们可以计算平均值和标准偏差，这将为我们提供一个更新后的，尽管是近似的高斯分布。

让我给你们看数据通过f(x)前后的散点图。

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

输入数据本身似乎是高斯分布的，事实也就是如此。我的意思是，输入数据看起来像是散布在平均零点周围的白噪声。相反，g(data)就明显不是这样了。

你可能已经想到，这个采样过程构成了我们问题的解决方案。假设每次更新我们生成500000个点，通过函数传递它们，然后计算结果的平均值和方差。这被称为蒙特卡罗方法，它被一些卡尔曼滤波设计所使用，例如集合卡尔曼滤波和粒子滤波。采样不需要专业知识，也不需要封闭形式的解决方案。无论函数是多么非线性或表现得多么糟糕，只要我们用足够多的sigma点进行采样，我们就会建立一个精确的输出分布。

足够多的sigma点是关键。上面的图表是用500000个sigma点创建的，输出仍然不平滑。更糟糕的是，这只适用于一维，并且所需点数随维数的增长指数级别的增长。如果一维只需要500点，二维需要500平方，三维需要500立方，依此类推。因此，虽然这种方法确实有效，但计算成本非常高。集合卡尔曼滤波和粒子滤波使用巧妙的技术来显著降低这种维数，但计算负担仍然很大。无迹卡尔曼滤波使用sigma点，但通过使用确定性方法选择点，大大减少了计算量。

sigma点-从分布中取样

让我们从二维协方差椭圆的角度来看这个问题。我选择二维仅仅是因为它易于绘制，这可以扩展到任意数量的维度。假设一些任意的非线性函数，我们将从协方差椭圆中选取随机点，通过非线性函数，并绘制它们的新位置。然后我们可以计算变换点的平均值和协方差，并将其用作平均值和概率分布的估计。

import kf_book.ukf_internal as ukf_internal

ukf_internal.show_2d_transform()

在左边，我们展示了一个椭圆，描绘了两个状态变量的$1\sigma$分布。箭头显示了几个随机采样点如何被任意非线性函数转换为新分布。右边的椭圆表示在新分布中，对应点集合的均值和方差的估计。

让我们写一个函数，它通过从下面的高斯分布中随机抽取的10000个点：

$$\mu =\left[\begin{array}{c}0\\ 0\end{array}\right]$$

$$\Sigma =\left[\begin{array}{cc}32& 15\\ 15& 40\end{array}\right]$$

并通过非线性系统：

$$\bar{x}=x+y$$

$$\bar{y}=0.1x^2+y^2$$

import numpy as np
from numpy.random import multivariate_normal
from kf_book.nonlinear_plots import plot_monte_carlo_mean

def f_nonlinear_xy(x, y):
    return np.array([x + y, .1*x**2 + y*y])

mean = (0., 0.)
p = np.array([[32., 15.], [15., 40.]])
# Compute linearized mean
mean_fx = f_nonlinear_xy(*mean)

#generate random points
xs, ys = multivariate_normal(mean=mean, cov=p, size=10000).T
plot_monte_carlo_mean(xs, ys, f_nonlinear_xy, mean_fx, 'Linearized Mean')

Difference in mean x=0.254, y=42.846

该图显示了该函数所产生的强非线性，以及如果我们采用扩展卡尔曼滤波的方式进行线性化所产生的大误差（我们将在下一章中学习）。

一个简单的例子

我将很快介绍无迹卡尔曼滤波（UKF）的数学原理。但是让我们从一个简单的例子开始，先来看看FilterPy中已经实现好的滤波器：

我们将了解到UKF可以使用许多不同的算法来生成sigma点，FilterPy提供了几种算法。这里介绍其中的一种：

from filterpy.kalman import JulierSigmaPoints

sigmas = JulierSigmaPoints(n=2, kappa=1)

这个对象将为任何给定的平均值和协方差生成加权sigma点，其内部实现之后会介绍。让我们先看一个示例，其中点的大小表示其权重：

from kf_book.ukf_internal import plot_sigmas

plot_sigmas(sigmas, x=[3, 17], cov=[[1, .5], [.5, 3]])

你可以看到，我们有5个点，以平均值(3, 17)为中心。这5个点的效果，会和500000个随机生成的点的效果一样好，甚至更好。这可能看起来很荒谬，但它确实会！

好的，现在让我们实现UKF滤波器。我们先处理一个标准的线性问题，这比较简单（UKF可以处理非线性问题，当然线性问题也是可以处理的）。UKF滤波器的设计流程与我们到目前为止所学的没有太大区别，只是有一个区别。KalmanFilter类使用矩阵$\mathbf{F}$来描述状态转移函数。然而矩阵表示线性代数，它适用于线性问题，但不适用于非线性问题。因此，我们提供了一个函数，而不是一个矩阵，就像我们上面所做的那样。KalmanFilter类使用另一个矩阵$\mathbf{H}$来描述观测函数，该函数将状态转换为等效观测。同样，矩阵意味着线性，因此我们提供的不是矩阵而是函数。

以下是一维跟踪问题的状态转移函数和观测函数，其中状态为$\mathbf{x}=[x\dot{x}{]}^T$：

def fx(x, dt):
    xout = np.empty_like(x)
    xout[0] = x[1] * dt + x[0]
    xout[1] = x[1]
    return xout

def hx(x):
    return x[:1] # return position [x] 

让我们明确一点，这是一个线性的例子。对于线性问题，不需要使用UKF，但是我从最简单的例子开始介绍。请注意我是如何编写fx()函数来计算$\bar{x}$，而不是使用矩阵乘法。由此可以看出，我可以在这里实现任意线性或非线性函数。

设计的其余部分是相同的。设计$\mathbf{P}$、$\mathbf{Q}$、$\mathbf{R}$，让我们完成滤波器并运行它。

from numpy.random import randn
from filterpy.kalman import UnscentedKalmanFilter
from filterpy.common import Q_discrete_white_noise

ukf = UnscentedKalmanFilter(dim_x=2, dim_z=1, dt=1., hx=hx, fx=fx, points=sigmas)
ukf.P *= 10
ukf.R *= .5
ukf.Q = Q_discrete_white_noise(2, dt=1., var=0.03)

zs, xs = [], []
for i in range(50):
    z = i + randn()*.5
    ukf.predict()
    ukf.update(z)
    xs.append(ukf.x[0])
    zs.append(z)
    
plt.plot(xs)
plt.plot(zs, marker='x', ls='')

这里真的没有什么新鲜事。你必须创建一个为你创建sigma点的对象，并为$\mathbf{F}$和$\mathbf{H}$提供函数而不是矩阵，但其余的与之前相同。这应该会给你足够的信心，让你能够通过一点数学和算法，从而了解UKF在做什么。

选择sigma点

在本章开始时，我使用500000个随机生成的sigma点，来计算高斯函数通过非线性函数的概率分布。虽然计算出的平均值相当准确，但每次更新计算500000个点会导致我们的滤波器非常慢。那么，我们可以使用的最少采样点数量是多少，这些点之间又被添加了什么样的约束？我们假设我们没有关于这些非线性函数的任何信息，因为我们想找到一个适用于任何函数的广义算法。

让我们考虑最简单的情况，看看它是否提供了一些思路。最简单的可能系统是恒等函数：$f(x)=x$。

我们可以使用的点数最少的方法是每个维度一个，这是线性卡尔曼滤波器使用的数字。卡尔曼滤波器对高斯分布$N(\mu ,{\sigma }^2)$的输入是$\mu$本身。因此，虽然这适用于线性情况，但对于非线性情况，这不是一个很好的答案。

或许我们仍可以在每个维度上使用一个点，但该点的位置要有一些改变。然而，如果我们将某个非$\mu$值（$\mu +△$）传递给恒等函数$f(x)=x$，它将不会收敛，那么这将不起作用。如果我们不改变$\mu$，那么这就是标准的卡尔曼滤波器。我们得出结论，每个维度一个样本不起作用。

我们可以选择的下一个最低数字是多少？2。考虑高斯分布是对称的事实，并且我们可能总是希望我们的采样点中的一个是恒等函数输入的平均值。两点需要我们选择平均值，然后选择另一点。另外一点会在我们的输入中引入一种我们可能不想要的不对称。

下一个最低的数字是3。3个点允许我们选择平均值，然后在平均值的每一侧选择一个点，如下图所示。

ukf_internal.show_3_sigma_points()

我们可以将这些点通过非线性函数$f(x)$进行转换，并计算它们的均值和方差。平均值可以计算为3个点的平均值，但这不是很一般性。例如，对于一个非常非线性的问题，我们可能希望中心点的权重远高于外部点，或者我们可能希望外部点的权重更高。

更一般性的方法是计算加权平均值$\mu ={\sum }_i{w}_{i}f(x_i)$，其中$x_i$就是sigma点。我们要求所有sigma点的权重之和等于1。鉴于这一要求，我们的任务是选择$x$及其相应的权重，以便计算转换后的sigma点的均值和方差。

如果我们对均值进行加权，那么对协方差进行加权也是有意义的。同样，也可以对平均值和协方差使用不同的权重$w^m$（均值权重）和$w^c$（协方差权重）。我们可以得到：

$$1=\sum _i{w}_i^m$$

$$1=\sum _i{w}_i^c$$

$$\mu =\sum _i{w}_i^{m}f(x_i)$$

$$\Sigma =\sum _i{w}_i^c(f(x_i-\mu ))(f(x_i-\mu {)}^T)$$

前两个等式是权重之和必须为1的约束条件。第三个等式是如何计算带权重平均值。第四个等式可能不太熟悉，但请记住，两个随机变量的协方差方程为：

$$COV(x,y)=\frac{\sum (x-\bar{x})(y-\bar{y})}{n}$$

这些约束不会形成唯一的解决方案。例如，如果你将$w_0^m$变小，还可以通过将$w_1^m$和$w_2^m$变大来补偿。你可以对均值和协方差使用不同的权重，也可以使用相同的权重。事实上，这些方程也根本不要求其中某个点是输入的平均值，尽管这样做似乎感觉更好。

我们需要一个满足约束条件的算法，最好每个维度只有3个点。在我们继续之前，我想确定这个思想你是清楚的。下面是同一协方差椭圆的三个不同示例，它们具有不同的sigma点。sigma点的大小与每个点的权重成正比。

ukf_internal.show_sigma_selections()

这些点不位于椭圆的长轴和短轴上，但约束条件中也没有要求我必须那样做。这些点和平均值之间等间距，但约束条件中也没有要求这样。

sigma点的排列和权重会影响我们如何对分布进行抽样。密集的点将对局部效应进行采样，因此可能对非常非线性的问题更有效。稀疏的点将采样非局部效果和非高斯行为。但是，通过改变每个点使用的权重，我们可以缓解这种情况。如果sigma点远离平均值，但加权很小，我们可以结合一些关于分布的知识，从而不允许问题的非线性产生错误的估计。

请理解选择sigma点有无限的方法，同一约束下的sigma点可能就有很多种，甚至我选择的约束只是其中一种。例如，并非所有创建sigma点的算法都要求权重总和为1。

无迹变换

目前，假设已经存在了用于选择sigma点和权重的算法。如何使用sigma点来实现滤波器？

无迹变换（UT变换）是算法的核心，但它非常简单。它通过一个非线性函数传递sigma点$x$，得到一组经过变换的点。

$$y=f(x)$$

然后计算变换后的点的平均值和协方差，该平均值和协方差成为新的估计值。下图描述了无迹变换的操作。右边的绿色椭圆表示计算出的转换后的sigma点的平均值和协方差。

ukf_internal.show_sigma_transform(with_text=True)

sigma点的平均值和协方差计算如下：

$$\mu =\sum _{i=0}{w}_i^m{y}_i$$

$$\Sigma =\sum _{i=0}{w}_i^c(y_i-\mu )(y_i-\mu {)}^T$$

这些方程应该很熟悉——它们是我们上面提到的约束方程。

简而言之，无迹变换从任意概率分布中采样点，通过任意非线性函数，并为每个变换点生成高斯分布。我希望你能想象我们如何使用它来实现一个非线性卡尔曼滤波器。一旦我们有了高斯函数，我们已经研究得到的所有数学公式都将发挥作用！

无迹变换的精度

之前我们写了一个函数，通过一个非线性函数传递50000个点来求分布的平均值。现在让我们通过相同的函数传递5个sigma点，并用无迹变换计算它们的平均值。我们将使用FilterPy内的MerweScaledSigmaPoints()函数创建sigma点，并使用unscented_transform()执行变换（稍后我们将了解这些函数）。在本章的第一个例子中，我使用了JulierSigmaPoints()，他们都生成sigma点，只是生成的方式不同，我将在后面解释。

from filterpy.kalman import unscented_transform, MerweScaledSigmaPoints
import scipy.stats as stats

#initial mean and covariance
mean = (0., 0.)
p = np.array([[32., 15], [15., 40.]])

# create sigma points and weights
points = MerweScaledSigmaPoints(n=2, alpha=.3, beta=2., kappa=.1)
sigmas = points.sigma_points(mean, p)

### pass through nonlinear function
sigmas_f = np.empty((5, 2))
for i in range(5):
    sigmas_f[i] = f_nonlinear_xy(sigmas[i, 0], sigmas[i ,1])

### use unscented transform to get new mean and covariance
ukf_mean, ukf_cov = unscented_transform(sigmas_f, points.Wm, points.Wc)

#generate random points
np.random.seed(100)
xs, ys = multivariate_normal(mean=mean, cov=p, size=5000).T

plot_monte_carlo_mean(xs, ys, f_nonlinear_xy, ukf_mean, 'Unscented Mean')
ax = plt.gcf().axes[0]
ax.scatter(sigmas[:,0], sigmas[:,1], c='r', s=30)

我觉得这个结果很了不起。仅使用5个点，我们就能够以惊人的精度计算出平均值。$x$的误差仅为-0.097，$y$中的误差为0.549。相比之下，线性化方法（由EKF使用，我们将在下一章中学习）给出的$y$误差超过43。如果你查看生成sigma点的代码，你会发现它不知道非线性函数，只知道初始分布的平均值和协方差。

我承认选择了一个使得UKF相比较于EKF拥有更加优秀性能的非线性函数。但物理世界充满了非线性的行为，UKF都可以较好的解决。我并没有很努力就找到了一个UKF工作得很好的函数。在下一章中，你将看到更多传统技术如何与强非线性进行斗争。

无迹卡尔曼滤波

现在我们可以介绍UKF算法。

预测步

UKF的预测步骤使用过程模型f()计算先验值。假设f()是非线性的，所以我们生成sigma点$x$及其相应的权重$W^m$（均值权重），$W^c$（协方差权重），可以根据下列函数得到：

$$x=sigma\_function(\mathbf{x},\mathbf{P})$$

$$W^m,W^c=weight\_function(n,parameters)$$

我们将每个sigma点通过过程模型$f(\mathbf{x},△t)$转换，形成新的先验，即一组新的sigma点：

$$y=f(x,△t)$$

我们使用变换后的sigma点，通过无迹变换计算先验的均值和协方差。

$$\bar{\mathbf{x}},\bar{\mathbf{P}}=UT(y,w^m,w^c,\mathbf{Q})$$

以下是无迹变换的方程式：

$$\bar{\mathbf{x}}=\sum _{i=0}{w}_i^m{y}_i$$

$$\bar{\mathbf{P}}=\sum _{i=0}{w}_i^c(y_i-\bar{\mathbf{x}})(y_i-\bar{\mathbf{x}}{)}^T+\mathbf{Q}$$

更新步

卡尔曼滤波在观测空间中执行更新。因此，我们必须使用观测函数h(x)将先前的sigma点转换为观测值。

$$z=h(y)$$

我们使用无迹变换计算这些点的平均值和协方差，带有z下标的表示这些是观测sigma点的平均值和协方差。

$${\mu }_z,{\mathbf{P}}_z=UT(z,w^m,w^c,\mathbf{R})$$

$${\mu }_z=\sum _{i=0}{w}_i^m{z}_i$$

$${\mathbf{P}}_z=\sum _{i=0}{w}_i^c(z_i-{\mu }_z)(z_i-{\mu }_z{)}^T+\mathbf{R}$$

接下来，我们计算残差和卡尔曼增益。观测值$\mathbf{z}$的残差计算起来很简单：

$$\mathbf{y}=\mathbf{z}-{\mu }_z$$

为了计算卡尔曼增益，我们首先计算状态和观测值的互协方差，其定义为：

$${\mathbf{P}}_{xz}=\sum _{i=0}^{2n}{w}_i^c(y_i-\bar{\mathbf{x}})(z_i-{\mu }_z{)}^T$$

然后将卡尔曼增益定义为：

$$\mathbf{K}={\mathbf{P}}_{xz}{\mathbf{P}}_z^{-1}$$

如果你将逆矩阵是一种矩阵倒数，你可以看到Kalman增益是一个简单的比率，它计算：

$$\mathbf{K}\approx \frac{{\mathbf{P}}_{xz}}{{\mathbf{P}}_z}\approx \frac{beliefinstate}{beliefinmeasurement}$$

最后，我们使用残差和卡尔曼增益计算新的状态估计：

$$\mathbf{x}=\bar{\mathbf{x}}+\mathbf{Ky}$$

新的协方差计算如下：

$$\mathbf{P}=\bar{\mathbf{P}}-\mathbf{K}{\mathbf{P}}_z{\mathbf{K}}^T$$

下表比较了线性卡尔曼滤波和无迹卡尔曼滤波方程。为了便于阅读，我去掉了下标$i$。

$$\begin{array}{ll}\text{Kalman Filter}& \text{Unscented Kalman Filter}\\ & y=f(x)\\ \bar{\mathbf{x}}=\mathbf{Fx}& \bar{\mathbf{x}}=\sum w^{m}y\\ \bar{\mathbf{P}}=\mathbf{FP}{\mathbf{F}}^T+\mathbf{Q}& \bar{\mathbf{P}}=\sum w^c(y-\bar{\mathbf{x}})(y-\bar{\mathbf{x}}{)}^T+\mathbf{Q}\\ & z=h(y)\\ & {\mu }_z=\sum w^{m}z\\ \mathbf{y}=\mathbf{z}-\mathbf{Hx}& \mathbf{y}=\mathbf{z}-{\mu }_z\\ \mathbf{S}=\mathbf{H}\bar{\mathbf{P}}{\mathbf{H}}^T+\mathbf{R}& {\mathbf{P}}_z=\sum w^c(z-{\mu }_z)(z-{\mu }_z{)}^T+\mathbf{R}\\ \mathbf{K}=\bar{\mathbf{P}}{\mathbf{H}}^T{\mathbf{S}}^{-1}& \mathbf{K}=[\sum w^c(y-\bar{\mathbf{x}})(z-{\mu }_z{)}^T]{\mathbf{P}}_z^{-1}\\ \mathbf{x}=\bar{\mathbf{x}}+\mathbf{Ky}& \mathbf{x}=\bar{\mathbf{x}}+\mathbf{Ky}\\ \mathbf{P}=(\mathbf{I}-\mathbf{KH})\bar{\mathbf{P}}& \mathbf{P}=\bar{\mathbf{P}}-\mathbf{K}{\mathbf{P}}_z{\mathbf{K}}^T\end{array}$$

Van der Merwe的sigma点算法

有许多选择sigma点的算法。自2005年左右以来，学术界和工业界大多对Rudolph Van der Merwe在其2004年博士论文中发表的算法表示满意。它可以很好地处理各种问题，并且在性能和准确性之间有很好的折衷。

该公式使用3个超调量来控制sigma点的分布和权重：$\alpha$、$\beta$和$\kappa$。在我们研究这些方程之前，让我们先来看看这种sigma点生成算法的结果。我将在显示一个标准偏差和两个标准偏差的协方差椭圆上绘制sigma点，并根据均值权重$W_{}^c$缩放点的大小。

ukf_internal.plot_sigma_points()

我们可以看到sigma点位于两个标准偏差之间，$\alpha$越大，点越分散。此外，较大$\alpha$时中心点的均值权重比较小$\alpha$时中心点的均值权重高，其余非中心点的均值权重较小。这应该符合我们的直觉——离平均值（中心点）越远，我们就越不应该重视它。我们尽管还不知道这些均值权重和sigma点是如何选择的，但这些选择看起来是合理的。

sigma点计算

第一个sigma点是输入的平均值，也就是上图中椭圆中心显示的sigma点。我们称之为$x_0$。

$$x_0=\mu$$

为了便于记录，我们定义了$\lambda ={\alpha }^2(n+\kappa )-n$，其中$n$是$\mathbf{x}$的维度。剩余的sigma点计算如下：

$$x_i=\{\begin{array}{ll}\mu +[\sqrt{(n+\lambda )\Sigma }{]}_i& i=\mathrm{1...}n\\ \mu -[\sqrt{(n+\lambda )\Sigma }{]}_i& i=(n+1)...(2n)\end{array}$$

其中，$i$下标表示取矩阵的第$i$列的向量。

换句话说，我们用一个常数来缩放协方差矩阵，取其平方根，并通过从平均值中加减来确保对称性。稍后我们将讨论如何求矩阵的平方根。

权重计算

该算法使用一组权重来计算平均值，另一组权重来计算协方差。$x_0$的均值权重为：

$$W_0^m=\frac{\lambda }{n+\lambda }$$

协方差权重为：

$$W_0^c=\frac{\lambda }{n+\lambda }+1-{\alpha }^2+\beta$$

其余sigma点（$x_1...x_{2n}$）的均值权重和协方差权重相同。它们是：

$$W_i^m=W_i^c=\frac{1}{2(n+\lambda )},i=\mathrm{1...2}n$$

这可能不太能解释地清楚，为什么这样选择sigma是“正确的”，事实上，无法证明这是所有非线性问题的理想选择。但是你可以看到，我们选择的sigma点与协方差矩阵的平方根成正比。也就是说，sigma点大致按照$\pm 1\sigma$乘以某个比例因子展开。sigma点的权重的分母中有一个$n$。因此随着维度的增加，点会分散开来，权重也会减小。

重要提示：通常这些权重总和不等于一。我有很多关于这个的问题，得到的权重总和大于1，甚至是负值。我将在下面更详细地介绍这一点。

参数的合理选择

对于高斯问题，$\beta =2$，$\kappa =3-n$是一个很好的选择，其中$n$是$\mathbf{x}$的维数，$0\le \alpha \le 1$是$\alpha$的适当选择，其中$\alpha$变大使sigma点远离平均值。

使用UKF

让我们解决一些问题，这样你就可以对UKF的易用性充满信心。我们将从一个线性问题开始，尽管你已经知道如何用线性卡尔曼滤波器解决这个问题。虽然UKF是为非线性问题设计的，但它可以找到与线性问题的线性卡尔曼滤波器相同的优化结果。我们将编写一个滤波器，使用恒定速度模型在二维中跟踪对象。这将使我们能够专注于和UKF哪些是相同的，哪些是不同的。

设计卡尔曼滤波器需要指定$\mathbf{x}$、$\mathbf{F}$、$\mathbf{H}$、$\mathbf{R}$和$\mathbf{Q}$。我们已经做了很多次了，所以我会直接给你矩阵，不用太多讨论。我们需要一个等速模型，所以我们定义$\mathbf{x}$为：

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

状态转移矩阵是：

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

它实现了牛顿方程：

$$x_k=x_{k-1}+{\dot{x}}_{k-1}△t$$

$$y_k=y_{k-1}+{\dot{y}}_{k-1}△t$$

我们的传感器提供位置，但不提供速度，因此观测矩阵：

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

传感器读数以$m$为单位，$x$和$y$的误差$\sigma =0.3$。这为我们提供了观测噪声矩阵：

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

最后，让我们假设过程噪声可以用离散白噪声模型来表示——也就是说，在每个时间段内，加速度是恒定的。我们可以使用FilterPy的Q_discrete_white_noise()为我们创建此矩阵，但为了便于查看，该矩阵是：

$$\mathbf{Q}=\left[\begin{array}{cc}\frac{1}{4}△t^4& \frac{1}{2}△t^3\\ \frac{1}{2}△t^3& △t^2\end{array}\right]{\sigma }^2$$

该滤波器的实现是：

from filterpy.kalman import KalmanFilter
from filterpy.common import Q_discrete_white_noise
from numpy.random import randn

std_x, std_y = .3, .3
dt = 1.0

np.random.seed(1234)
kf = KalmanFilter(4, 2)
kf.x = np.array([0., 0., 0., 0.])
kf.R = np.diag([std_x**2, std_y**2])
kf.F = np.array([[1, dt, 0, 0], 
                 [0, 1, 0, 0],
                 [0, 0, 1, dt],
                 [0, 0, 0, 1]])
kf.H = np.array([[1, 0, 0, 0],
                 [0, 0, 1, 0]])

kf.Q[0:2, 0:2] = Q_discrete_white_noise(2, dt=1, var=0.02)
kf.Q[2:4, 2:4] = Q_discrete_white_noise(2, dt=1, var=0.02)

zs = [np.array([i + randn()*std_x, 
                i + randn()*std_y]) for i in range(100)]               
xs, _, _, _ = kf.batch_filter(zs)
plt.plot(xs[:, 0], xs[:, 2])

这应该不会让你感到意外。现在让我们实现一个UKF。这纯粹是为了教学目的，对线性问题使用UKF不会带来任何好处。FilterPy使用UnscentedKalmanFilter类实现UKF。

首先要做的是实现函数$f(x,dt)$和$h(x)$。$f(x,dt)$实现状态转移函数，$h(x)$实现观测函数。这些对应于线性滤波器中的矩阵$\mathbf{F}$和$\mathbf{H}$。下面是这两个功能的合理实现。每个都将返回一个一维NumPy数组或包含结果的列表。

def f_cv(x, dt):
    """ state transition function for a 
    constant velocity aircraft"""
    F = np.array([[1, dt, 0,  0],
                  [0,  1, 0,  0],
                  [0,  0, 1, dt],
                  [0,  0, 0,  1]])
    return F @ x

def h_cv(x):
    return x[[0, 2]]

接下来指定如何计算sigma点和权重。我们在上面给出了Van der Merwe的版本，但是有很多不同的选择。FilterPy使用SigmaPoints类，该类必须实现一个方法：

def sigma_points(self, x, P)

还需要包含属性$W_m$和$W_c$，它们分别表示用于计算平均值和协方差的权重。

FilterPy从SigmaPoints类派生出子类MerweScaledSigmaPoints类，并实现上述方法。

创建UKF时，你想要传入$f()$函数、$h()$函数以及sigma点对象，如本例所示：

from filterpy.kalman import MerweScaledSigmaPoints
from filterpy.kalman import UnscentedKalmanFilter as UKF

points = MerweScaledSigmaPoints(n=4, alpha=.1, beta=2., kappa=-1)
ukf = UKF(dim_x=4, dim_z=2, fx=f_cv, hx=h_cv, dt=dt, points=points)

其余代码与线性卡尔曼滤波类似，我将使用相同的观测值，并计算两个解之间差值的标准偏差。

from filterpy.kalman import UnscentedKalmanFilter as UKF
import numpy as np

sigmas = MerweScaledSigmaPoints(4, alpha=.1, beta=2., kappa=1.)
ukf = UKF(dim_x=4, dim_z=2, fx=f_cv,
          hx=h_cv, dt=dt, points=sigmas)
ukf.x = np.array([0., 0., 0., 0.])
ukf.R = np.diag([0.09, 0.09]) 
ukf.Q[0:2, 0:2] = Q_discrete_white_noise(2, dt=1, var=0.02)
ukf.Q[2:4, 2:4] = Q_discrete_white_noise(2, dt=1, var=0.02)

uxs = []
for z in zs:
    ukf.predict()
    ukf.update(z)
    uxs.append(ukf.x.copy())
uxs = np.array(uxs)

plt.plot(uxs[:, 0], uxs[:, 2])
print(f'UKF standard deviation {np.std(uxs - xs):.3f} meters')

UKF standard deviation 0.013 meters

结果是0.013米的标准偏差，这是相当小的。

UKF的实现与线性卡尔曼滤波没有太多的不同。不过是状态转换和测量函数并不使用矩阵$\mathbf{F}$和$\mathbf{H}$，而是提供非线性函数$f()$和$h()$，其余的理论和实现基本保持不变。实现predict()和update()的代码不同，但从设计师的角度来看，问题公式和滤波器设计非常相似。

跟踪飞机

让我们来解决第一个非线性问题。我们将编写一个滤波器，用雷达作为传感器跟踪飞机。为了使问题尽可能与前一个问题相似，我们将在两个维度上进行跟踪：地面上的一维坐标和飞机的高度。每个维度都是独立的，因此我们可以在不损失通用性的情况下做到这一点。

雷达通过发射无线电波或微波来工作，传输路径中的任何东西都会将部分信号反射回雷达。通过计时反射信号返回所需的时间，可以计算到目标的直线距离。我们可以根据直线距离和仰角计算飞机的$(x,y)$位置，如下图所示：

import kf_book.ekf_internal as ekf_internal

ekf_internal.show_radar_chart()

其中，$ϵ$表示仰角。

我们假设飞机在恒定高度飞行。因此，我们有一个三变量的状态量：

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

状态转移函数是线性的：

$$\bar{\mathbf{x}}=\left[\begin{array}{ccc}1& △t& 0\\ 0& 1& 0\\ 0& 0& 1\end{array}\right]\left[\begin{array}{c}x\\ \dot{x}\\ y\end{array}\right]$$

可使用一下代码计算：

def f_radar(x, dt):
    """ state transition function for a constant velocity 
    aircraft with state vector [x, velocity, altitude]'"""
    F = np.array([[1, dt, 0],
                  [0,  1, 0],
                  [0,  0, 1]], dtype=float)
    return F @ x

接下来我们设计下观测函数。与线性卡尔曼滤波器一样，观测功能将滤波器的先验值转换为观测值。我们需要将飞机的位置和速度转换成雷达站的仰角和直线距离。

使用Pythagorean定理计算直线距离：

$$range=\sqrt{(x_{ac}-x_{radar}{)}^2+(y_{ac}-y_{radar}{)}^2}$$

仰角$ϵ$是$y/x$反正切：

$$ϵ=ta{n}^{-1}\frac{y_{ac}-y_{radar}}{x_{ac}-x_{radar}}$$

我们需要定义一个Python函数来实现：

def h_radar(x):
    dx = x[0] - h_radar.radar_pos[0]
    dy = x[2] - h_radar.radar_pos[1]
    slant_range = math.sqrt(dx**2 + dy**2)
    elevation_angle = math.atan2(dy, dx)
    return [slant_range, elevation_angle]

h_radar.radar_pos = (0, 0)

还有一个我们没有考虑的非线性，即角度是循环的。残差是观测值与投影到观测空间的先验值之间的差值，$359^{\circ }$和$1^{\circ }$之间的角度差为$2^{\circ }$，但$359^{\circ }-1^{\circ }=358^{\circ }$。UKF在无迹变换中需要计算加权和，这将加剧了这种情况。目前，我们将把传感器和目标放置在避开这些非线性区域的区域，稍后我们将讨论如何处理这个问题。

我们需要模拟雷达和飞机。到目前为止，这应该比较简单，因此我提供代码而不进行讨论。

from numpy.linalg import norm
from math import atan2

class RadarStation:

    def __init__(self, pos, range_std, elev_angle_std):
        self.pos = np.asarray(pos)       
        self.range_std = range_std
        self.elev_angle_std = elev_angle_std

    def reading_of(self, ac_pos):
        """ Returns (range, elevation angle) to aircraft. 
        Elevation angle is in radians.
        """
        diff = np.subtract(ac_pos, self.pos)
        rng = norm(diff)
        brg = atan2(diff[1], diff[0])
        return rng, brg

    def noisy_reading(self, ac_pos):
        """ Compute range and elevation angle to aircraft with 
        simulated noise"""
        rng, brg = self.reading_of(ac_pos)      
        rng += randn() * self.range_std
        brg += randn() * self.elev_angle_std 
        return rng, brg       

class ACSim:   
    def __init__(self, pos, vel, vel_std):
        self.pos = np.asarray(pos, dtype=float)
        self.vel = np.asarray(vel, dtype=float)
        self.vel_std = vel_std        
        
    def update(self, dt):
        """ Compute and returns next position. Incorporates 
        random variation in velocity. """
        dx = self.vel*dt + (randn() * self.vel_std) * dt      
        self.pos += dx     
        return self.pos

军用级雷达的距离精度达到$1m$ RMS，仰角达到$1mrad$ RMS。我们将假设$5m$ RMS的距离精度和$0.5^{\circ }$ RMS的角度精度较为适中，因为这为滤波器提供了更具挑战性的数据集。

$\mathbf{Q}$的设计需要一些讨论，状态量是${\left[\begin{array}{ccc}x& \dot{x}& y\end{array}\right]}^T$。前两个值是下地面距离和地面距离上变化产生的速度，因此我们可以使用Q_discrete_white_noise计算$\mathbf{Q}$左上角的值。第三个值是高度，我们假设它与$x$无关。因此$\mathbf{Q}$可以设计为：

$$\mathbf{Q}=\left[\begin{array}{cc}{\mathbf{Q}}_{\mathbf{x}}& 0\\ 0& Q_y\end{array}\right]$$

飞机从雷达站正上方时开始，以$100m/s$的速度飞行，雷达数据每$3s$更新一次。

import math
from kf_book.ukf_internal import plot_radar

dt = 3. # 12 seconds between readings
range_std = 5 # meters
elevation_angle_std = math.radians(0.5)
ac_pos = (0., 1000.)
ac_vel = (100., 0.)
radar_pos = (0., 0.)
h_radar.radar_pos = radar_pos

points = MerweScaledSigmaPoints(n=3, alpha=.1, beta=2., kappa=0.)
kf = UKF(3, 2, dt, fx=f_radar, hx=h_radar, points=points)

kf.Q[0:2, 0:2] = Q_discrete_white_noise(2, dt=dt, var=0.1)
kf.Q[2,2] = 0.1

kf.R = np.diag([range_std**2, elevation_angle_std**2])
kf.x = np.array([0., 90., 1100.])
kf.P = np.diag([300**2, 30**2, 150**2])

np.random.seed(200)
pos = (0, 0)
radar = RadarStation(pos, range_std, elevation_angle_std)
ac = ACSim(ac_pos, (100, 0), 0.02)

time = np.arange(0, 360 + dt, dt)
xs = []
for _ in time:
    ac.update(dt)
    r = radar.noisy_reading(ac.pos)
    kf.predict()
    kf.update([r[0], r[1]]) 
    xs.append(kf.x)    
plot_radar(xs, time)

这可能会给你留下深刻的印象，也可能不会，但它会给我留下深刻的印象！在扩展卡尔曼滤波一章中，我们将解决同样的问题，但需要大量的数学知识。

跟踪正在被操纵的飞机

前面的例子产生了良好的结果，但它假设飞机没有改变高度。如果飞机在一分钟后开始爬升，以下是滤波结果。

from kf_book.ukf_internal import plot_altitude

# reset aircraft position
kf.x = np.array([0., 90., 1100.])
kf.P = np.diag([300**2, 30**2, 150**2])
ac = ACSim(ac_pos, (100, 0), 0.02)

np.random.seed(200)
time = np.arange(0, 360 + dt, dt)
xs, ys = [], []
for t in time:
    if t >= 60:
        ac.vel[1] = 300/60 # 300 meters/minute climb
    ac.update(dt)
    r = radar.noisy_reading(ac.pos)
    ys.append(ac.pos[1])
    kf.predict()
    kf.update([r[0], r[1]]) 
    xs.append(kf.x)

plot_altitude(xs, time, ys)
print(f'Actual altitude: {ac.pos[1]:.1f}')
print(f'UKF altitude   : {xs[-1][2]:.1f}')

Actual altitude: 2515.6
UKF altitude   : 1042.1

滤波器无法跟踪不断变化的高度，那么在我们的设计中，需要改变什么？

我希望你的回答是将高度变化产生的速度添加到状态量，如下所示：

$$\mathbf{x}=\left[\begin{array}{c}distance\\ velcsity\\ altitude\\ climbrate\end{array}\right]=\left[\begin{array}{c}x\\ \dot{x}\\ y\\ \dot{y}\end{array}\right]$$

这需要对状态转换函数进行以下更改，该函数仍然是线性的：

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

观测函数保持不变，但我们必须改变$\mathbf{Q}$以考虑$\mathbf{x}$的维数变化。

def f_cv_radar(x, dt):
    """ state transition function for a constant velocity 
    aircraft"""
    F = np.array([[1, dt, 0, 0],
                  [0,  1, 0, 0],
                  [0,  0, 1, dt],
                  [0,  0, 0, 1]], dtype=float)
    return F @ x

def cv_UKF(fx, hx, R_std):
    points = MerweScaledSigmaPoints(n=4, alpha=.1, beta=2., kappa=-1.)
    kf = UKF(4, len(R_std), dt, fx=fx, hx=hx, points=points)

    kf.Q[0:2, 0:2] = Q_discrete_white_noise(2, dt=dt, var=0.1)
    kf.Q[2:4, 2:4] = Q_discrete_white_noise(2, dt=dt, var=0.1)
    kf.R = np.diag(R_std)
    kf.R = kf.R @ kf.R  # square to get variance
    kf.x = np.array([0., 90., 1100., 0.])
    kf.P = np.diag([300**2, 3**2, 150**2, 3**2])
    return kf

np.random.seed(200)
ac = ACSim(ac_pos, (100, 0), 0.02)

kf_cv = cv_UKF(f_cv_radar, h_radar, R_std=[range_std, elevation_angle_std])
time = np.arange(0, 360 + dt, dt)
xs, ys = [], []
for t in time:
    if t >= 60:
        ac.vel[1] = 300/60 # 300 meters/minute climb
    ac.update(dt)
    r = radar.noisy_reading(ac.pos)
    ys.append(ac.pos[1])
    kf_cv.predict()
    kf_cv.update([r[0], r[1]]) 
    xs.append(kf_cv.x)

plot_altitude(xs, time, ys)
print(f'Actual altitude: {ac.pos[1]:.1f}')
print(f'UKF altitude   : {xs[-1][2]:.1f}'

Actual altitude: 2515.6
UKF altitude   : 2500.1

高度估计中引入了大量噪声，但我们现在正在精确跟踪高度。

传感器融合

现在让我们考虑一个传感器融合的例子。假如我们有某种类型的多普勒系统传感器，可以直接产生$2m/s$ RMS精度的速度估计。

在上一个例子中，雷达的精度使我们能够将速度估计在$1m/s$左右，我将降低该精度，以说明传感器融合的效果。让我们将距离精度改为$\sigma =500m$，然后计算估计速度的标准偏差。

不使用多普勒系统的标准偏差为：

range_std = 500.
elevation_angle_std = math.degrees(0.5)
np.random.seed(200)
pos = (0, 0)
radar = RadarStation(pos, range_std, elevation_angle_std)
ac = ACSim(ac_pos, (100, 0), 0.02)

kf_sf = cv_UKF(f_cv_radar, h_radar, R_std=[range_std, elevation_angle_std])
time = np.arange(0, 360 + dt, dt)
xs = []
for _ in time:
    ac.update(dt)
    r = radar.noisy_reading(ac.pos)
    kf_sf.predict()
    kf_sf.update([r[0], r[1]]) 
    xs.append(kf_sf.x)

xs = np.asarray(xs)
plot_radar(xs, time, plot_x=False, plot_vel=True, plot_alt=False)
print(f'Velocity std {np.std(xs[10:, 1]):.1f} m/s')

Velocity std 3.4 m/s

对于多普勒系统，我们需要在观测中添加$x$和$y$方向的速度。ACSim类将速度存储在数据成员vel中。要执行卡尔曼滤波更新，我们只需要调用update，其中包含直线距离、仰角和$x$和$y$方向的速度：

$$z=[slantRange,elevationAngle,\dot{x},\dot{y}]$$

观测包含四个值，因此观测函数还需要返回四个值。直线距离和仰角将像以前一样计算，我们不需要计算$x$和$y$方向的速度，因为它们是由状态量提供的。

def h_vel(x):
    dx = x[0] - h_vel.radar_pos[0]
    dz = x[2] - h_vel.radar_pos[1]
    slant_range = math.sqrt(dx**2 + dz**2)
    elevation_angle = math.atan2(dz, dx)
    return slant_range, elevation_angle, x[1], x[3]

现在我们可以实现我们的滤波器了。

h_radar.radar_pos = (0, 0)
h_vel.radar_pos = (0, 0)

range_std = 500.
elevation_angle_std = math.degrees(0.5)
vel_std = 2.

np.random.seed(200)
ac = ACSim(ac_pos, (100, 0), 0.02)
radar = RadarStation((0, 0), range_std, elevation_angle_std)

kf_sf2 = cv_UKF(f_cv_radar, h_vel, 
            R_std=[range_std, elevation_angle_std, vel_std, vel_std])

time = np.arange(0, 360 + dt, dt)
xs = []
for t in time:
    ac.update(dt)
    r = radar.noisy_reading(ac.pos)
    # simulate the doppler velocity reading
    vx = ac.vel[0] + randn()*vel_std
    vz = ac.vel[1] + randn()*vel_std
    kf_sf2.predict()
    kf_sf2.update([r[0], r[1], vx, vz]) 
    xs.append(kf_sf2.x)
xs = np.asarray(xs)
plot_radar(xs, time, plot_x=False, plot_vel=True, plot_alt=False)
print(f'Velocity std {np.std(xs[10:,1]):.1f} m/s')

Velocity std 0.9 m/s

通过加入速度传感器，我们能够将标准差从$3.4m/s$降低到$0.9m/s$。

传感器融合是一个很大的主题，而这只是一个相当简单的实现。在一个经典的导航问题中，我们有提供补充信息的传感器。例如，GPS每秒能够提供一次较为准确的位置更新，但速度估计较差，而惯性传感器能够以50Hz的频率提供非常准确的速度刷新，但位置估计较差。每个传感器的优缺点相互交错，这导致了互补滤波器，该滤波器将高刷新率的惯性传感器观测值与GPS的精确但缓慢刷新的位置观测值融合，以产生高刷新率且精确的位置和速度估计值。在GPS刷新过程中，集成高刷新率的速度估计，以生成准确的高刷新率的位置估计。

多位置传感器

上一个传感器融合问题太简单了，让我们来解决一个不那么简单的问题。在GPS之前，船舶和飞机通过各种方位系统导航，如VOR、LORAN、TACAN、DME等。这些系统以无线电波的形式发射信号，传感器从反射信号中提取到物体的方向。例如，一架飞机可能有两个VOR接收机，飞行员将每个接收机连接到不同的VOR发射机。每个VOR接收机确定一个方向——从地面VOR发射机与飞机的方向。飞行员使用图找到同时满足两个VOR发射机方向放射线的交点，从而确定飞机的位置。

这是一种低精度的手动方法，卡尔曼滤波器将产生更精确的位置估计。假设我们有两个传感器，每个传感器对目标仅提供方向观测，如下图所示。圆周的宽度与传感器噪声$3\sigma$成正比。飞机很高概率地处于两个圆周交叉点内的某个位置。

ukf_internal.show_two_sensor_bearing()

我们计算传感器和目标之间的方位角为：

def bearing(sensor, target_pos):
    return math.atan2(target_pos[1] - sensor[1], 
                      target_pos[0] - sensor[0])

滤波器以矢量形式接收来自两个传感器的观测值。代码将接受任何可迭代的容器，因此为了提高效率，我使用Python列表。我们可以这样做：

def measurement(A_pos, B_pos, pos):
    angle_a = bearing(A_pos, pos)
    angle_b = bearing(B_pos, pos)
    return [angle_a, angle_b]

假设飞机为等速模型。对于速度的变化，我显式地计算新位置，而不是使用矩阵向量乘法：

def fx_VOR(x, dt):
    x[0] += x[1] * dt
    x[2] += x[3] * dt
    return x

接下来我们实现观测函数。它将状态量的先验值转换为到两个传感器的观测值。我不喜欢全局变量，但我将两个传感器的位置放在全局变量sa_pos和sb_pos中，以演示与$h()$共享数据的方法：

sa_pos = [-400, 0]
sb_pos = [400, 0]

def hx_VOR(x):
    # measurement to A
    pos = (x[0], x[2])
    return measurement(sa_pos, sb_pos, pos)

现在，我们编写构建滤波器，运行滤波器并绘制结果：

def moving_target_filter(pos, std_noise, Q, dt=0.1, kappa=0.0):
    points = MerweScaledSigmaPoints(n=4, alpha=.1, beta=2., kappa=kappa)
    f = UKF(dim_x=4, dim_z=2, dt=dt, 
            hx=hx_VOR, fx=fx_VOR, points=points)
    f.x = np.array([pos[0], 1., pos[1], 1.])

    q = Q_discrete_white_noise(2, dt, Q)
    f.Q[0:2, 0:2] = q
    f.Q[2:4, 2:4] = q
    f.R *= std_noise**2
    f.P *= 1000    
    return f

def plot_straight_line_target(f, std_noise):
    xs, txs = [], []
    for i in range(300):
        target_pos[0] += 1 + randn()*0.0001
        target_pos[1] += 1 + randn()*0.0001
        txs.append((target_pos[0], target_pos[1]))

        z = measurement(sa_pos, sb_pos, target_pos)
        z[0] += randn() * std_noise
        z[1] += randn() * std_noise

        f.predict()
        f.update(z)
        xs.append(f.x)

    xs = np.asarray(xs)
    txs = np.asarray(txs)

    plt.plot(xs[:, 0], xs[:, 2])
    plt.plot(txs[:, 0], txs[:, 1], ls='--', lw=2, c='k')
    plt.show()

np.random.seed(123)
target_pos = [100, 200]

std_noise = math.radians(0.5)
f = moving_target_filter(target_pos, std_noise, Q=1.0)
plot_straight_line_target(f, std_noise)

我觉得这个结果还算不错。轨迹的开始部分显示出较大的误差，但滤波器很快稳定下来并产生良好的估计。

让我们重新讨论一下角度的非线性。上面的例子中，目标的初始位置为$(100,200)$，速度为$(1,1)$。而现在，我将目标定位在两个传感器之间，坐标为$(0,0)$。这将导致残差计算中的非线性，因为平均角度将接近零。当角度低于0时，观测函数将计算一个接近$2\pi$的大正角度。因此，预测和观测之间的残差将非常大，接近$2\pi$，而不是接近0。这使得滤波器无法准确执行，如下面的示例所示。

target_pos = [0, 0]
f = moving_target_filter(target_pos, std_noise, Q=1.0)
plot_straight_line_target(f, std_noise)

这种表现令人无法接受。FilterPy的UKF代码允许你指定一个函数，该函数用于出现这样的非线性行为时计算残差。本章的最后一个示例演示了它的用法。

UKF的实现

FilterPy已经实现了UKF，但学习如何将方程转换为代码是很有意义的。实现UKF非常简单。首先，让我们编写代码来计算sigma点的平均值和协方差。

我们将sigma点和权重存储在矩阵中，如下所示：

$$weights=\left[\begin{array}{cccc}{w}_0& w_1& ...& w_{2n}\end{array}\right]$$

$$sigmas=\left[\begin{array}{cccc}{x}_{0,0}& x_{0,1}& ...& x_{0,n-1}\\ x_{1,0}& x_{1,1}& ...& x_{1,n-1}\\ ⋮& ⋮& \ddots & ⋮\\ x_{2n,0}& x_{2n,1}& ...& x_{2n,n-1}\end{array}\right]$$

这里用很多下标来描述一些非常简单的东西，所以用一个二维问题的例子（$n=2$）：

points = MerweScaledSigmaPoints(n=2, alpha=.1, beta=2., kappa=1.)
points.sigma_points(x=[0.,0], P=[[1.,.1],[.1, 1]])

array([[ 0.   ,  0.   ],
       [ 0.173,  0.017],
       [ 0.   ,  0.172],
       [-0.173, -0.017],
       [ 0.   , -0.172]])

表示平均值的sigma点位于第一行，其位置为$(0,0)$，等于平均值$(0,0)$。第二个sigma点位于位置$(0.173,0.017)$，依此类推。总共$2n+1=5$行，每行为一个sigma点。如果$n=3$，则将有3列7行。

以行-列或者是列-行格式存储sigma都是没问题的，可以按照个人习惯存储。

权重

使用NumPy计算权重很容易。回想一下，Van der Merwe的权重表示为：

$$\lambda ={\alpha }^2(n+\kappa )-n$$

$$W_0^m=\frac{\lambda }{n+\lambda }$$

$$W_0^c=\frac{\lambda }{n+\lambda }+1-{\alpha }^2+\beta$$

$$W_i^m=W_i^c=\frac{1}{2(n+\lambda )},i=\mathrm{1...2}n$$

代码为：

lambda_ = alpha**2 * (n + kappa) - n
Wc = np.full(2*n + 1,  1. / (2*(n + lambda_))
Wm = np.full(2*n + 1,  1. / (2*(n + lambda_))
Wc[0] = lambda_ / (n + lambda_) + (1. - alpha**2 + beta)
Wm[0] = lambda_ / (n + lambda_)

我在lambda_中使用下划线，因为lambda在Python中是一个保留字。尾随下划线是Python的变通方法。

sigma点

sigma点的表达式为：

$$x_0=\mu$$

$$x_i=\mu +[\sqrt{(n+\lambda )\Sigma }{]}_i,i=\mathrm{1...}n$$

$$x_i=\mu -[\sqrt{(n+\lambda )\Sigma }{]}_{i-n},i=(n+1)...2n$$

一旦我们理解了$[\sqrt{(n+\lambda )\Sigma }{]}_i$项的含义，是有Python来实现它就并不困难了。

项$\sqrt{(n+\lambda )\Sigma }$是一个矩阵，因为$\Sigma$是一个矩阵。$[\sqrt{(n+\lambda )\Sigma }{]}_i$的下标为$i$，表示选择矩阵的第i行向量。矩阵的平方根是多少？没有唯一的定义。一种定义是，当矩阵$S$与自身相乘时，产生$\Sigma$，则矩阵$\Sigma$的平方根是矩阵$S$：如果$\Sigma =SS$，则$S=\sqrt{\Sigma }$。

我们将选择一个具有数值特性的替代定义，使其更易于计算。我们可以将矩阵$\Sigma$的平方根定义为，当矩阵$S$乘以其转置与矩阵$\Sigma$相等：

$$\Sigma ={\mathbf{SS}}^T$$

这种定义是可取的，因为$\mathbf{S}$是使用Cholesky分解计算得到的。它将Hermitian矩阵（自共轭矩阵）、正定矩阵分解为三角矩阵及其共轭转置。三角矩阵可以是上三角或下三角，如下所示：

$$A=L{L}^{\ast }A=U^{\ast }U$$

*表示共轭转置，如果只有实数，则可以写成：

$$A=L{L}^{T}A=U^{T}U$$

$\mathbf{P}$具有这些性质，因此我们可以将$\mathbf{S}=cholesky(\mathbf{P})$视为$\mathbf{P}$的平方根。

SciPy中提供cholesky()方法，如果你选择的语言是Fortran、C或C++，如库LAPACK提供这个方法，Matlab提供chol()。

scipy.linalg.cholesky()默认返回一个上三角矩阵，因此我编写代码来获得它。

import scipy
a = np.array([[2., .1], [.1, 3]])
s = scipy.linalg.cholesky(a)
print("cholesky:")
print(s)
print("\nsquare of cholesky:")
print(s @ s.T)

cholesky:
[[1.414 0.071]
 [0.    1.731]]

square of cholesky:
[[2.005 0.122]
 [0.122 2.995]]

因此，我们可以使用以下代码实现sigma点：

sigmas = np.zeros((2*n+1, n))
U = scipy.linalg.cholesky((n+lambda_)*P) # sqrt

sigmas[0] = X
for k in range (n):
    sigmas[k+1]   = X + U[k]
    sigmas[n+k+1] = X - U[k]

现在，让我们实现无迹变换。回想一下方程式：

$$\mu =\sum _i{w}_i^m{x}_i$$

$$\Sigma =\sum _i{w}_i^c(x_i-\mu )(x_i-\mu {)}^T$$

我们计算平均值：

x = np.dot(Wm, sigmas)

如果你对NumPy不熟练，这可能会让你觉得陌生。NumPy不仅仅是一个使线性代数成为可能的库，它的底层是用C和Fortran编写的，这使得它获得了比Python快得多的速度。为了获得这种加速，我们必须避免使用for循环，而是使用NumPy的内置函数来执行计算。因此，我们不编写for循环来计算乘积之和，而是调用内置的numpy.dot(x, y)方法：两个向量的点积是每个元素的元素乘法之和。如果传递一个一维数组和一个二维数组，它将计算内积之和：

a = np.array([10, 100])
b = np.array([[1, 2, 3],
              [4, 5, 6]])
np.dot(a, b)

array([410, 520, 630])

剩下的就是计算$\mathbf{P}={\sum }_i{w}_i^c(x_i-\mu )(x_i-\mu {)}^T+\mathbf{Q}$：

kmax, n = sigmas.shape
P = zeros((n, n))
for k in range(kmax):
    y = sigmas[k] - x
    P += Wc[k] * np.outer(y, y) 
P += Q

这引入了NumPy的另一个特性。状态变量x是一维的，正如sigmas[k]一样，因此残差sigmas[k]-x也是一维的。NumPy不会计算一维向量的转置，它认为[1, 2, 3]的转置为[1, 2, 3]。所以我们可以调用np.outer(y, y)函数，来计算一维向量$\mathbf{y}$的${\mathbf{yy}}^T$。一个可替代的实现是：

y = (sigmas[k] - x).reshape(kmax, 1) # convert into 2D array
P += Wc[K] * np.dot(y, y.T)

这段代码速度较慢，而且不通用，因此我们将不使用它。

预测步

对于预测步骤，我们将按照上面的步骤生成权重和sigma点。我们通过函数f()对每个sigma点进行状态转移。

$$y=f(x)$$

然后，我们使用无迹变换计算状态量预测值的均值和协方差。在下面的代码中，我假设预测步是一个类中的方法，该类存储滤波器所需的各种矩阵和向量。

def predict(self, sigma_points_fn):
    """ Performs the predict step of the UKF. On return, 
    self.xp and self.Pp contain the predicted state (xp) 
    and covariance (Pp). 'p' stands for prediction.
    """
    # calculate sigma points for given mean and covariance
    sigmas = sigma_points_fn(self.x, self.Pp)

    for i in range(self._num_sigmas):
        self.sigmas_f[i] = self.fx(sigmas[i], self._dt)

    self.xp, self.Pp = unscented_transform(
                       self.sigmas_f, self.Wm, self.Wc, self.Q)

更新步

更新步通过函数h()将sigma点转换到观测空间。

$$z=h(y)$$

这些sigma点的平均值和协方差是通过无迹变换计算的，紧接着计算残差和卡尔曼增益。交叉方差计算如下：

$${\mathbf{P}}_{xz}=\sum _{i=0}^{2n}{w}_i^c(y_i-\mu )(z_i-{\mu }_z{)}^T$$

最后，我们使用残差和卡尔曼增益计算新的状态估计：

$$\mathbf{K}={\mathbf{P}}_{xz}{\mathbf{P}}_z^{-1}$$

$$\mathbf{x}=\bar{\mathbf{x}}+\mathbf{Ky}$$

新的协方差计算如下：

$$\mathbf{P}=\bar{\mathbf{P}}-\mathbf{K}{\mathbf{P}}_z{\mathbf{K}}^T$$

假设该函数是已存储必要矩阵和数据的类的方法，则可以按如下方式实现该函数。

def update(self, z):
    # rename for readability
    sigmas_f = self.sigmas_f
    sigmas_h = self.sigmas_h

    # transform sigma points into measurement space
    for i in range(self._num_sigmas):
        sigmas_h[i] = self.hx(sigmas_f[i])

    # mean and covariance of prediction passed through UT
    zp, Pz = unscented_transform(sigmas_h, self.Wm, self.Wc, self.R)

    # compute cross variance of the state and the measurements
    Pxz = np.zeros((self._dim_x, self._dim_z))
    for i in range(self._num_sigmas):
        Pxz += self.Wc[i] * np.outer(sigmas_f[i] - self.xp,
                                    sigmas_h[i] - zp)

    K = np.dot(Pxz, inv(Pz)) # Kalman gain

    self.x = self.xp + np.dot(K, z - zp)
    self.P = self.Pp - np.dot(K, Pz).dot(K.T)

FilterPy的实现

FilterPy在某种程度上对代码流程进行了封装。你可以指定不同的sigma点算法，指定如何计算状态变量的残差，指定矩阵平方根函数等等。更多相关的详细信息，请参阅帮助。

https://filterpy.readthedocs.org/#unscented-kalman-filter

批处理

卡尔曼滤波器是递归的——基于先验估计和当前观测。但是，有一组已经收集的想要滤波的的数据是很常见的。在这种情况下，滤波器可以在批处理模式下运行，在该模式下，所有观测值都将被一次滤波。

将观测值收集到一个数组或列表中。

zs = read_altitude_from_csv()

然后调用batch_filter()方法。

Xs, Ps = ukf.batch_filter(zs)

该函数获取观测的列表/数组，对其进行滤波，并返回整个数据集的状态估计（Xs）和协方差矩阵（Ps）数组。

这是一个完整的例子，来自上面的雷达跟踪问题。

dt = 12. # 12 seconds between readings
range_std = 5 # meters
bearing_std = math.radians(0.5)

ac_pos = (0., 1000.)
ac_vel = (100., 0.)
radar_pos = (0., 0.)
h_radar.radar_pos = radar_pos

points = MerweScaledSigmaPoints(n=3, alpha=.1, beta=2., kappa=0.)
kf = UKF(3, 2, dt, fx=f_radar, hx=h_radar, points=points)

kf.Q[0:2 ,0:2] = Q_discrete_white_noise(2, dt=dt, var=0.1)
kf.Q[2, 2] = 0.1

kf.R = np.diag([range_std**2, bearing_std**2])
kf.x = np.array([0., 90., 1100.])
kf.P = np.diag([300**2, 30**2, 150**2])

radar = RadarStation((0, 0), range_std, bearing_std)
ac = ACSim(ac_pos, (100, 0), 0.02)

np.random.seed(200)

t = np.arange(0, 360 + dt, dt)
n = len(t)

zs = []
for i in range(len(t)):
    ac.update(dt)
    r = radar.noisy_reading(ac.pos)
    zs.append([r[0], r[1]])

xs, covs = kf.batch_filter(zs)
ukf_internal.plot_radar(xs, t)

平滑结果

假设我们正在跟踪一辆汽车。假设我们得到一个有噪声的观测值：汽车开始向左转弯，但状态函数预测汽车正在直线行驶。卡尔曼滤波器别无选择，只能将状态估计值稍微移向噪声测量值，因为它无法判断这是一个噪声特别大的测量值还是转弯的真正开始。

如果我们正在收集数据并对其进行后处理，我们会在有问题的数据出现之后再进行测量，以通知我们是否真正转弯。假设随后的观测都继续左转，那么我们可以确定观测值不是特别嘈杂，而是启动了一个转弯。

我们不会在这里研究数学或算法，我将演示如何在FilterPy中调用算法。我们实现的算法被称为RTS平滑器，其命名是以三个发明者的首字母：Rauch、Tung和Striebel。

使用的函数为UnscentedKalmanFilter.rts_smoother()，我们把batch_filter()步骤中计算出的平均值和协方差传入，然后接收平滑后的平均值、协方差和卡尔曼增益。

Ms, P, K = kf.rts_smoother(xs, covs)
ukf_internal.plot_rts_output(xs, Ms, t)

Difference in position in meters:
	 [-1.4166 -0.2815  1.2679 -1.2405 -2.1863]

从这些图表中，我们可以看到位置的改善很小，但速度的改善很好。位置上的差异非常小，所以我打印了UKF和平滑结果之间最后5个点的差异。如果可以对数据进行后期处理，我建议始终使用RTS平滑器。

sigma点参数的选择

我发现关于选择$\alpha$、$\beta$和$\kappa$值的文献相当有限。Van der Merwe的论文包含的信息最多，但并不详尽。那么，让我们来探讨一下这些参数的作用。

Van der Merwe建议对高斯问题使用$\beta =2$，而$\kappa =3-n$。所以让我们从这里开始，改变$\alpha$。我将让$n=1$来最小化我们需要查看的数组的大小，并避免计算矩阵的平方根。

from kf_book.ukf_internal import print_sigmas

print_sigmas(mean=0, cov=3, alpha=1)

sigmas:  [ 0.  3. -3.]
mean weights: [0.6667 0.1667 0.1667]
cov weights: [2.6667 0.1667 0.1667]
lambda: 2
sum cov 2.9999999999999996

这是怎么回事？我们可以看到，对于0的平均值，算法选择0、3和-3的sigma点，但是为什么呢？回想计算sigma点的方程式：

$$x_0=\mu$$

$$x_i=\mu \pm \sqrt{(n+\lambda )\Sigma }$$

我选择的$n=1$将所有内容简化为标量，从而避免计算矩阵的平方根。因此，对于我们的值，方程是：

$$x_0=\mu =0$$

$$x_i=\mu \pm \sqrt{(n+\lambda )\Sigma }=0\pm \sqrt{(1+2)\times 3}=\pm 3$$

让我们把$\alpha$设为一个荒谬的值。

print_sigmas(mean=0, cov=3, alpha=200)

sigmas:  [   0.  600. -600.]
mean weights: [1. 0. 0.]
cov weights: [-39996.      0.      0.]
lambda: 119999
sum cov -39996.00000000001

我们可以看到sigma点为0、600、-600。如果我们的数据是高斯的，我们将合并与平均值相差许多标准差的数据；对于非线性问题，这不太可能产生好的结果。但假设我们的分布不是高斯分布，而是有很长的尾巴？我们可能需要从这些尾部取样以获得一个好的估计值，因此将$\kappa$变大是有意义的（而不是200，这对于sigma点的变化来说是荒谬的大，一般不会设置$\alpha$这么大，那么就只能调整$\kappa$了）。

可以看出，$\alpha$、$\kappa$越大，sigma点越分散。

现在让我们看看权重的变化。当$\kappa +n=3$时，平均值的权重为0.6667，两个离群的sigma点的权重为0.1667。另一方面，当$\alpha =200$时，平均权重迅速上升至0.99999，离群值权重设置为0.000004。回想权重的方程式：

$$W_0=\frac{\lambda }{\lambda +n}$$

$$W_i=\frac{1}{2(\lambda +n)}$$

我们可以看到，随着$\lambda$变大，平均值的权重分数$\lambda /(\lambda +n)$接近1，其余sigma点的权重分数接近0。因此，当我们的样本越来越远离平均值时，我们最终给这些样本的权重会越来越小；如果我们的样本非常接近平均值，我们会给所有样本赋予非常相似的权重。

然而，Van der Merwe给出的建议是将$\alpha$限制在$0\le \alpha \le 1$的范围内。他建议$10^{-3}$作为一个良好的值。让我们试试看。

print_sigmas(mean=0, cov=13, alpha=.001, kappa=0)

sigmas:  [ 0.      0.0036 -0.0036]
mean weights: [-999999.  500000.  500000.]
cov weights: [-999996.  500000.  500000.]
lambda: -0.999999
sum cov 3.9999989999923855

机器人定位——一个完整的工作示例

现在是解决一个重大问题的时候了。大多数书都选择简单的教科书式问题和简单的答案，而你却在想如何解决现实世界中的问题。这个例子不会教你如何处理任何问题，而是说明在设计和实现滤波器时你必须考虑的事情。

我们将考虑机器人定位的问题。在这个场景中，我们有一个机器人在一个环境中移动，它使用传感器来检测地标。这可能是一辆自动驾驶汽车，使用计算机视觉技术来识别树木、建筑物和其他地标；可能是一个小型机器人，可以为你的房子吸尘；也可能是仓库里的机器人。

该机器人有4个轮子，形状与汽车相近。它通过转动前轮来操纵，这会使机器人在向前移动并转向时，绕后轴转动。这是我们必须建模的非线性行为。

该机器人有一个传感器，它可以提供与地面已知目标的大致距离和方向。这是非线性的，因为从距离和方向计算位置需要平方根和三角函数。

过程模型和观测模型都是非线性的。UKF兼顾了两者，因此我们暂时得出结论，UKF是解决这一问题的可行选择。

机器人运动模型

首先假设，汽车在向前行驶过程中通过转动前轮胎来转向。汽车的前部沿着车轮指向的方向移动，同时绕后轮胎旋转。由于摩擦导致的滑移，轮胎在不同速度下的不同行为，以及外侧轮胎与内侧轮胎的行驶半径不同等问题，这一简单描述将会变得复杂。对转向进行精确建模需要一组复杂的微分方程。

对于卡尔曼滤波，特别是对于低速机器人应用，一个简单的自行车模型就可以表现良好。这是对模型的描述：

ekf_internal.plot_bicycle()

在这里，我们看到前轮胎的指向相对于轴距的方向为$\alpha$。在很短的一段时间内，汽车向前行驶，后轮进一步向前，并稍微向内转向，如蓝色阴影轮胎所示。在如此短的时间范围内，我们可以将其近似为曲率为$R$的转弯。我们可以使用转角$\beta$表示为：

$$\beta =\frac{d}{w}tan(\alpha )$$

转弯曲率$R$表示为：

$$R=\frac{d}{\beta }$$

其中，后轮在给定前进速度$v$下行驶的距离为$d=v△t$。

转弯开始前，机器人的方向为$\theta$，那么它的位置$C$表示为：

$$C_x=x-Rsin(\theta )$$

$$C_y=y+Rcos(\theta )$$

向前移动时间$△t$后，机器人的新位置和方向为：

$$\bar{x}=C_x+Rsin(\theta +\beta )$$

$$\bar{y}=C_y-Rcos(\theta +\beta )$$

$$\bar{\theta }=\theta +\beta$$

一旦我们用$C$代替，我们得到：

$$\bar{x}=x-Rsin(\theta )+Rsin(\theta +\beta )$$

$$\bar{y}=y+Rcos(\theta )-Rcos(\theta +\beta )$$

$$\bar{\theta }=\theta +\beta$$

如果你对转向模型没有兴趣，你就没有必要了解这个数学模型的细节。你需要认识到的一点是，我们的运动模型是非线性的，需要用卡尔曼滤波器来处理这一点。

设计状态量

对于我们的机器人，我们将跟踪位置和方向：

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

我可以在这个模型中加入速度，但正如你们看到的，数学公式就已经很有挑战性了。

控制输入$\mathbf{u}$是指令速度和转向角：

$$\mathbf{u}={\left[\begin{array}{cc}v& \alpha \end{array}\right]}^T$$

设计系统模型

我们将我们的系统建模为一个非线性运动模型加上白噪声。

$$\bar{x}=x+f(x,u)+\mathcal{N}(0,Q)$$

使用上面创建的机器人运动模型，我们可以编写：

from math import tan, sin, cos, sqrt

def move(x, dt, u, wheelbase):
    hdg = x[2]
    vel = u[0]
    steering_angle = u[1]
    dist = vel * dt

    if abs(steering_angle) > 0.001: # is robot turning?
        beta = (dist / wheelbase) * tan(steering_angle)
        r = wheelbase / tan(steering_angle) # radius

        sinh, sinhb = sin(hdg), sin(hdg + beta)
        cosh, coshb = cos(hdg), cos(hdg + beta)
        return x + np.array([-r*sinh + r*sinhb, 
                              r*cosh - r*coshb, beta])
    else: # moving in straight line
        return x + np.array([dist*cos(hdg), dist*sin(hdg), 0])

我们将使用这个函数来实现状态转移函数f(x)。

之前设计运动模型，假设$△t$很小。如果机器人移动缓慢，则该功能应给出合理准确的预测。如果$△t$较大或系统的动力学非常非线性，此方法将失败。在这些情况下，你需要使用更复杂的数值积分技术（如Runge-Kutta）来实现它。

设计观测模型

传感器提供了相对于已知地标的、有噪声的方向和距离观测结果。观测模型必须转换状态量${\left[\begin{array}{ccc}x& y& \theta \end{array}\right]}^T$，到传感器观测量。如果$p$是地标的位置，则距离$r$为：

$$r=\sqrt{(p_x-x{)}^2+(p_y-y{)}^2}$$

我们假设传感器提供相对于机器人方向的方向，因此我们必须从方向中减去机器人的方向，以获得传感器读数，如下所示：

$$\varphi =ta{n}^{-1}(\frac{p_y-y}{p_x-x})-\theta$$

因此，我们的观测方程是：

$$\mathbf{z}=h(\mathbf{x},\mathbf{P})+\mathcal{N}(0,R)$$

$$\mathbf{z}=\left[\begin{array}{c}\sqrt{(p_x-x{)}^2+(p_y-y{)}^2}\\ ta{n}^{-1}(\frac{p_y-y}{p_x-x})-\theta \end{array}\right]+\mathcal{N}(0,R)$$

我现在还不打算使用代码来实现它，因为有一个困难将在下面的内容讨论。

设计观测噪声

假设距离和方向的测量噪声是独立的，因此：

$$\mathbf{R}=\left[\begin{array}{cc}{\sigma }_{range}^2& 0\\ 0& {\sigma }_{bearing}^2\end{array}\right]$$

实现

在开始使用代码实现之前，我们还有另一个问题要处理。残差为$y=z-h(x)$。假设$z$的方向为$1^{\circ }$， $h(x)$是$359^{\circ }$，残差计算得到$-358^{\circ }$。这将使卡尔曼增益的计算中断，因为正确的角度差是$2^{\circ }$。因此，我们必须编写代码来正确计算方向残差。

def normalize_angle(x):
    x = x % (2 * np.pi)    # force in range [0, 2 pi)
    if x > np.pi:          # move to [-pi, pi)
        x -= 2 * np.pi
    return x

print(np.degrees(normalize_angle(np.radians(1-359))))

1.9999999999999774

残差的计算有状态量之间的，也有观测量之间的。而需要额外处理的方向，在状态量中处于索引2，在观测量中处于索引1，因此我们需要分别编写函数来处理。我们面临的另一个问题是，当地标数量超过1个时，当机器人在移动过程中，可见的地标数量将会发生变化，因此我们需要处理不同数量的观测。残差函数将通过多个观测的数组传递，每个地标一个。

def residual_h(a, b):
    y = a - b
    # data in format [dist_1, bearing_1, dist_2, bearing_2,...]
    for i in range(0, len(y), 2):
        y[i + 1] = normalize_angle(y[i + 1])
    return y

def residual_x(a, b):
    y = a - b
    y[2] = normalize_angle(y[2])
    return y

我们现在可以实现观测模型了：

$$h(\mathbf{x},\mathbf{P})=\left[\begin{array}{c}\sqrt{(p_x-x{)}^2+(p_y-y{)}^2}\\ ta{n}^{-1}(\frac{p_y-y}{p_x-x})-\theta \end{array}\right]$$

表达式$ta{n}^{-1}(\frac{p_y-y}{p_x-x})-\theta$，可能会产生超出$[-\pi ,\pi )$范围的结果，因此我们应该将角度标准化到那个范围。

函数将被传递一组地标，并需要生成一组观测值，其形式为[dist_to_1, bearing_to_1, dist_to_2, bearing_to_2_, ...]。

def Hx(x, landmarks):
    """ takes a state variable and returns the measurement
    that would correspond to that state. """
    hx = []
    for lmark in landmarks:
        px, py = lmark
        dist = sqrt((px - x[0])**2 + (py - x[1])**2)
        angle = atan2(py - x[1], px - x[0])
        hx.extend([dist, normalize_angle(angle - x[2])])
    return np.array(hx)

我们的困难还没有结束。无迹变换计算状态量和观测量的平均值，但每个向量都包含一个方向角。没有唯一的方法来计算一组角度的平均值。例如，$359^{\circ }$和$3^{\circ }$的平均值是多少？直觉告诉我们答案应该是$1^{\circ }$，但是$\frac{1}{n}\sum x$的结果是$181^{\circ }$。

一种常见的方法是取$sin$和$cos$之和的$arctan$：

$$\bar{\theta }=atan2(\frac{{\sum }_{i=1}^{n}sin{\theta }_i}{n},\frac{{\sum }_{i=1}^{n}cos{\theta }_i}{n})$$

UnscentedKalmanFilter.__init__()函数，有一个参数x_mean_fn来指定状态量的平均值的计算函数，有一个参数z_mean_fn来指定观测量的平均值的计算函数。我们将这些函数定义为：

def state_mean(sigmas, Wm):
    x = np.zeros(3)

    sum_sin = np.sum(np.dot(np.sin(sigmas[:, 2]), Wm))
    sum_cos = np.sum(np.dot(np.cos(sigmas[:, 2]), Wm))
    x[0] = np.sum(np.dot(sigmas[:, 0], Wm))
    x[1] = np.sum(np.dot(sigmas[:, 1], Wm))
    x[2] = atan2(sum_sin, sum_cos)
    return x

def z_mean(sigmas, Wm):
    z_count = sigmas.shape[1]
    x = np.zeros(z_count)

    for z in range(0, z_count, 2):
        sum_sin = np.sum(np.dot(np.sin(sigmas[:, z+1]), Wm))
        sum_cos = np.sum(np.dot(np.cos(sigmas[:, z+1]), Wm))

        x[z] = np.sum(np.dot(sigmas[:,z], Wm))
        x[z+1] = atan2(sum_sin, sum_cos)
    return x

这些函数利用了NumPy对数组进行三角运算，并且执行点乘操作。NumPy是用C和Fortran实现的，因此sum(dot(sin(x), w))比用Python编写循环高效得多。

完成后，我们现在准备实现UKF。我想指出的是，当我设计这个滤波器时，我并不会一次就设计了上面所有的函数，而是从易到难。我先把一个基础的UKF和一个已知的地标放在一起，验证它的有效性，然后再开始扩充。如果我看到不同的地标怎么办？这使我改变了观测函数，以接受一系列地标。如何计算角度的残差？这使我编写了角度归一化的代码。一组角度的平均值怎么计算？我在维基百科上找到一篇文章，并实现了这个算法。设计你能做的，然后提出问题并逐一解决。

当我们构造sigma点和滤波器时，我们必须为它提供我们编写的函数来计算残差和均值。

points = SigmaPoints(n=3, alpha=.00001, beta=2, kappa=0, 
                     subtract=residual_x)

ukf = UKF(dim_x=3, dim_z=2, fx=fx, hx=Hx, dt=dt, points=points,
         x_mean_fn=state_mean, z_mean_fn=z_mean,
         residual_x=residual_x, residual_z=residual_h)

接下来，我们需要将额外的数据传递到$f(x,dt)$和$h(x)$函数中。我们想用$move(x,dt,u,wheelbase)$表示$f(x,dt)$，用$Hx(x,landmarks)$表示$h(x)$。我们可以这样做，只需将额外的参数作为关键字参数传递到predict()和update()，如下所示：

ukf.predict(u=u, wheelbase=wheelbase)        
ukf.update(z, landmarks=landmarks)

其余的代码就是运行模拟并绘制结果。我创建了一个变量landmarks 来包含地标的坐标。我每秒更新机器人的位置10次，但每秒只运行一次UKF。这有两个原因。首先，我们没有使用Runge Kutta积分运动微分方程，因此较窄的时间步长使我们的模拟更精确。其次，在嵌入式系统中，处理速度有限是很正常的。这迫使你仅在绝对需要时运行卡尔曼滤波器。

from filterpy.stats import plot_covariance_ellipse

dt = 1.0
wheelbase = 0.5

def run_localization(
    cmds, landmarks, sigma_vel, sigma_steer, sigma_range, 
    sigma_bearing, ellipse_step=1, step=10):

    plt.figure()
    points = MerweScaledSigmaPoints(n=3, alpha=.00001, beta=2, kappa=0, 
                                    subtract=residual_x)
    ukf = UKF(dim_x=3, dim_z=2*len(landmarks), fx=move, hx=Hx,
              dt=dt, points=points, x_mean_fn=state_mean, 
              z_mean_fn=z_mean, residual_x=residual_x, 
              residual_z=residual_h)

    ukf.x = np.array([2, 6, .3])
    ukf.P = np.diag([.1, .1, .05])
    ukf.R = np.diag([sigma_range**2, 
                     sigma_bearing**2]*len(landmarks))
    ukf.Q = np.eye(3)*0.0001
    
    sim_pos = ukf.x.copy()
    
    # plot landmarks
    if len(landmarks) > 0:
        plt.scatter(landmarks[:, 0], landmarks[:, 1], 
                    marker='s', s=60)
    
    track = []
    for i, u in enumerate(cmds):     
        sim_pos = move(sim_pos, dt/step, u, wheelbase)
        track.append(sim_pos)

        if i % step == 0:
            ukf.predict(u=u, wheelbase=wheelbase)

            if i % ellipse_step == 0:
                plot_covariance_ellipse(
                    (ukf.x[0], ukf.x[1]), ukf.P[0:2, 0:2], std=6,
                     facecolor='k', alpha=0.3)

            x, y = sim_pos[0], sim_pos[1]
            z = []
            for lmark in landmarks:
                dx, dy = lmark[0] - x, lmark[1] - y
                d = sqrt(dx**2 + dy**2) + randn()*sigma_range
                bearing = atan2(lmark[1] - y, lmark[0] - x)
                a = (normalize_angle(bearing - sim_pos[2] + 
                     randn()*sigma_bearing))
                z.extend([d, a])            
            ukf.update(z, landmarks=landmarks)

            if i % ellipse_step == 0:
                plot_covariance_ellipse(
                    (ukf.x[0], ukf.x[1]), ukf.P[0:2, 0:2], std=6,
                     facecolor='g', alpha=0.8)
    track = np.array(track)
    plt.plot(track[:, 0], track[:,1], color='k', lw=2)
    plt.axis('equal')
    plt.title("UKF Robot localization")
    plt.show()
    return ukf

landmarks = np.array([[5, 10], [10, 5], [15, 15]])
cmds = [np.array([1.1, .01])] * 200
ukf = run_localization(
    cmds, landmarks, sigma_vel=0.1, sigma_steer=np.radians(1),
    sigma_range=0.3, sigma_bearing=0.1)
print('Final P:', ukf.P.diagonal())

Final P: [0.0092 0.0187 0.0007]

操纵机器人

上述运行中的转向模拟不现实，因为速度和转向角从未改变。我们可以实现一个稍有复杂的机器人运动过程，我将使用NumPy的linspace()方法生成不同的转向命令。我还将添加更多的地标，因为机器人将比第一个示例中走得更远。

landmarks = np.array([[5, 10], [10, 5], [15, 15], [20, 5],
                      [0, 30], [50, 30], [40, 10]])
dt = 0.1
wheelbase = 0.5
sigma_range=0.3
sigma_bearing=0.1

def turn(v, t0, t1, steps):
  return [[v, a] for a in np.linspace(
                 np.radians(t0), np.radians(t1), steps)]  
    
# accelerate from a stop
cmds = [[v, .0] for v in np.linspace(0.001, 1.1, 30)]
cmds.extend([cmds[-1]]*50)

# turn left
v = cmds[-1][0]
cmds.extend(turn(v, 0, 2, 15))
cmds.extend([cmds[-1]]*100)

#turn right
cmds.extend(turn(v, 2, -2, 15))
cmds.extend([cmds[-1]]*200)

cmds.extend(turn(v, -2, 0, 15))
cmds.extend([cmds[-1]]*150)

cmds.extend(turn(v, 0, 1, 25))
cmds.extend([cmds[-1]]*100)

ukf = run_localization(
    cmds, landmarks, sigma_vel=0.1, sigma_steer=np.radians(1),
    sigma_range=0.3, sigma_bearing=0.1, step=1,
    ellipse_step=20)
print('final covariance', ukf.P.diagonal())

final covariance [0.0013 0.0043 0.0004]

不确定性很快变得很小。协方差椭圆显示$6\sigma$协方差，但椭圆太小，很难看到。

但如果我们只在起点附近提供两个地标，最终结果中就会加入更多的错误。当我们运行这个滤波器时，误差会随着机器人离这两个地标越远而增加。

ukf = run_localization(
    cmds, landmarks[0:2], sigma_vel=0.1, sigma_steer=np.radians(1),
    sigma_range=0.3, sigma_bearing=0.1, step=1,
    ellipse_step=20)
print('final covariance', ukf.P.diagonal())

final covariance [0.0026 0.0657 0.0008]

总结

你对本文的理解可能取决于你在过去实现了多少非线性卡尔曼滤波器。如果这是你第一次接触，也许$2n+1$个sigma点的计算以及随后$f(x)$和$h(x)$的函数的编写可能让你头疼。为UKF编写函数有一点繁琐，但概念非常基础。解决同样的问题，EKF需要一些相当困难的数学基础。

与EKF相比，UKF的优势不仅在于相对容易实现。讨论这一点有些为时过早，因为你还没有学习EKF，但EKF在某一点将问题线性化，并通过线性卡尔曼滤波器来解决。相比之下，UKF采集$2n+1$个sigma点。因此，UKF通常比EKF更精确，特别是当问题高度非线性时。虽然UKF不能保证总是优于EKF，但在实践中，它至少表现得同样好，通常比EKF好得多。

因此，我始终建议从实现UKF开始。如果你的滤波器出现了发散，那么你当然需要进行复杂的分析和实验来选择最佳的滤波器。

最后，UKF看似是执行sigma点滤波的方法，但事实并非如此。我选择的具体版本是Julier的无迹卡尔曼滤波器，由Van der Merwe在其2004年的论文中参数化。如果你搜索Julier、Van der Merwe、Uhlmann和Wan，你会发现他们开发的一系列类似的sigma点滤波器。每种技术都使用不同的方法来选择和加权sigma点。但选择不止于此。例如，SVD卡尔曼滤波器使用奇异值分解（SVD）来找到概率分布的近似平均值和协方差。

相关阅读

• Kalman-and-Bayesian-Filters-in-Python/blob/master/10-Unscented-Kalman-Filter