本文主要翻译自rlabbe/Kalman-and-Bayesian-Filters-in-Python的第11章节11-Extended-Kalman-Filters（扩展卡尔曼滤波）。

%matplotlib inline

#format the book
import book_format
book_format.set_style()

我们发展了线性卡尔曼滤波器的理论。然后在上两章中，我们提出了使用卡尔曼滤波器解决非线性问题的主题。在本章中，我们将学习扩展卡尔曼滤波器（EKF）。EKF通过在当前估计点对系统进行线性化来处理非线性，然后使用线性卡尔曼滤波器对该线性化系统进行滤波。这是最早用于非线性问题的技术之一，也是最常用的技术。

EKF为滤波器的设计者设置了较大的数学挑战，因此我尽我所能避免EKF，而采用其他技术来解决非线性滤波问题。然而，这是不可避免的，有很多经典论文和大多数当前论文都使用EKF。即使你没有使用过EKF，你也需要熟悉它才能阅读文献。

卡尔曼滤波器的线性化

卡尔曼滤波器需要使用线性方程组，因此它不适用于非线性问题。非线性可以体现在两个方面。首先，过程模型可能是非线性的。例如：物体在大气中下落时会遇到阻力，从而降低其加速度。阻力系数随对象的速度而变化。由此产生的行为是非线性的——它不能用线性方程来建模。其次，观测模型可能是非线性的。例如：雷达提供目标的距离和方向。我们需要使用非线性的三角函数来计算目标的坐标。

对于线性滤波器，我们有以下过程和观测模型方程：

$$\dot{\mathbf{x}}=\mathbf{Ax}+w_x$$

$$\mathbf{z}=\mathbf{Hx}+w_z$$

其中，$\mathbf{A}$是系统动态矩阵。使用卡尔曼滤波数学一章中介绍的状态空间方法，可以将这些方程转换为：

$$\dot{\mathbf{x}}=\mathbf{Fx}$$

$$\mathbf{z}=\mathbf{Hx}$$

其中，$\mathbf{F}$是状态转移矩阵。噪音$w_x$和$w_z$项被合并到矩阵$\mathbf{R}$和$\mathbf{Q}$中。这种形式的方程允许我们在给定步骤$k$的观测值和步骤$k-1$的状态估计值，计算步骤$k$的状态。在前面的章节中，我们通过使用牛顿方程描述的问题来使数学模型最小化。我们知道如何根据高中物理设计$\mathbf{F}$。

对于非线性模型，线性表达式$\mathbf{Fx}+\mathbf{Bu}$替换为非线性函数$f(\mathbf{x},\mathbf{u})$，线性表达式$\mathbf{Hx}$替换为非线性函数$h(\mathbf{x})$：

$$\dot{\mathbf{x}}=f(\mathbf{x},\mathbf{u})+w_x$$

$$\mathbf{z}=h(\mathbf{x})+w_z$$

如果你还记得非线性滤波章节中的图表，通过一个非线性函数传递一个高斯分布，会得到一个不再是高斯分布的概率分布。所以这是行不通的。

EKF不会改变卡尔曼滤波器的线性方程组。相反，它将当前估计点处的非线性方程线性化，并在线性卡尔曼滤波器中使用此线性化方程组。

线性化正如字面的意思，将非线性问题变成线性问题。我们在定义的点上找到一条与曲线最接近的直线，下图将抛物线$f(x)=x^2-2x$在$x=1.5$处线性化。

import kf_book.ekf_internal as ekf_internal

ekf_internal.show_linearization()

如果上面的曲线是过程模型，则虚线表示该曲线在$x=1.5$的线性化。

我们通过求导数使系统线性化，该导数求出曲线的斜率：

$$f(x)=x^2-2x$$

$$\frac{df}{dx}=2x-2$$

然后在$x$处对其进行估计：

$$m=f^{\prime }(x=1.5)=1$$

一组微分方程的线性化也是类似的。我们通过对${\mathbf{x}}_t$点处求偏导来估计$\mathbf{F}$和$\mathbf{H}$，从而将$f(\mathbf{x},\mathbf{u})$和$h(\mathbf{x})$线性化。我们称矩阵的偏导数为雅可比矩阵。这为我们提供了状态转移矩阵和观测模型矩阵：

$$\mathbf{F}=\frac{\partial f({\mathbf{x}}_t,{\mathbf{u}}_t)}{\partial \mathbf{x}}{\mid }_{{\mathbf{x}}_t,{\mathbf{u}}_t}$$

$$\mathbf{H}=\frac{\partial h({\bar{\mathbf{x}}}_t)}{\partial \bar{\mathbf{x}}}{\mid }_{{\bar{\mathbf{x}}}_t}$$

这导致EKF流程是以下的方程，和线性卡尔曼滤波器之间存在一些不同：

$$\begin{array}{ll}\text{KF}& \text{EKF}\\ & \mathbf{F}=\frac{\partial f({\mathbf{x}}_t,{\mathbf{u}}_t)}{\partial \mathbf{x}}{\mid }_{{\mathbf{x}}_t,{\mathbf{u}}_t}\\ \bar{\mathbf{x}}=\mathbf{Fx}& \bar{\mathbf{x}}=f(\mathbf{x},\mathbf{u})\\ \bar{\mathbf{P}}=\mathbf{FP}{\mathbf{F}}^T+\mathbf{Q}& \bar{\mathbf{P}}=\mathbf{FP}{\mathbf{F}}^T+\mathbf{Q}\\ & \mathbf{H}=\frac{\partial h({\bar{\mathbf{x}}}_t)}{\partial \bar{\mathbf{x}}}{\mid }_{{\bar{\mathbf{x}}}_t}\\ \mathbf{y}=\mathbf{z}-\mathbf{Hx}& \mathbf{y}=\mathbf{z}-h(\bar{\mathbf{x}})\\ \mathbf{S}=\mathbf{H}\bar{\mathbf{P}}{\mathbf{H}}^T+\mathbf{R}& \mathbf{S}=\mathbf{H}\bar{\mathbf{P}}{\mathbf{H}}^T+\mathbf{R}\\ \mathbf{K}=\bar{\mathbf{P}}{\mathbf{H}}^T{\mathbf{S}}^{-1}& \mathbf{K}=\bar{\mathbf{P}}{\mathbf{H}}^T{\mathbf{S}}^{-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}=(\mathbf{I}-\mathbf{KH})\bar{\mathbf{P}}\end{array}$$

对于EKF，我们通常不使用$\mathbf{Fx}$进行状态转移，因为线性化会导致不准确。使用合适的数值积分技术来计算$\bar{\mathbf{x}}$是很典型的做法，如Euler或Runge Kutta。因此，我将状态转移表示为$\bar{\mathbf{x}}=f(\mathbf{x},\mathbf{u})$。出于同样的原因，我们不使用$\mathbf{Hx}$计算残差，选择更精确的$h(\bar{\mathbf{x}})$。

我认为理解EKF最简单的方法是从一个例子开始。

示例：跟踪飞机

此示例使用地基雷达跟踪飞机。在上一章中，我们针对这个问题使用UKF来实现。现在，我们将针对同一问题使用EKF来实现，以便比较滤波器性能和滤波器实现所需的工作量。

雷达的工作原理是发射一束无线电波并扫描回波。光束路径中的任何东西都会将部分信号反射回雷达。通过计算反射信号返回雷达所需的时间，系统可以计算出从雷达装置到目标的直线距离。

雷达的直线距离$\gamma$和仰角$\epsilon$，与飞机水平位置$x$和高度$y$的关系如下图所示：

ekf_internal.show_radar_chart()

这给了我们等式：

$$\epsilon =ta{n}^{-1}\frac{y}{x}$$

$${\gamma }^2=x^2+y^2$$

设计状态量

假设我们在跟踪一架速度恒定、高度恒定的飞机的位置，雷达能够观测到距飞机的直线距离。这意味着我们需要3个状态量——水平距离、水平速度和高度：

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

设计过程模型

我们假设飞机的运动符合牛顿运动学。我们在前面的章节中已经使用了这个模型了：

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

我将矩阵分成了两块，左上的$2\times 2$矩阵表示$x$的等速模型，右下的$1\times 1$矩阵表示$y$的等速模型。

但是让我们练习寻找这些矩阵，我们用一组微分方程对系统建模。我们需要的一个形式为：

$$\bar{\mathbf{x}}=\mathbf{Ax}+\mathbf{w}$$

其中，$\mathbf{w}$为系统噪声。

变量$x$和$y$是独立的，所以我们可以分别计算它们。一维运动的微分方程为：

$$v=\dot{x}$$

$$a=\ddot{x}=0$$

现在我们把微分方程转化为状态空间形式。如果这是一个二阶或更高阶的微分系统，我们必须首先将它们简化为一个等价的一阶方程组。这些方程是一阶的，所以我们把它们放在状态空间矩阵形式中：

$$\left[\begin{array}{c}\dot{x}\\ \ddot{x}\end{array}\right]=\left[\begin{array}{cc}0& 1\\ 0& 0\end{array}\right]\left[\begin{array}{c}x\\ \dot{x}\end{array}\right]$$

$$\bar{x}=\mathbf{Ax}$$

其中，$\mathbf{A}=\left[\begin{array}{cc}0& 1\\ 0& 0\end{array}\right]$。

回想一下，$\mathbf{A}$是系统动态矩阵。它描述了一组线性微分方程，我们必须从中计算状态转移矩阵$\mathbf{F}$。$\mathbf{F}$描述了一组离散线性方程组，该方程组计算离散时间步长$△t$的$\mathbf{x}$。

计算$\mathbf{F}$的常用方法是使用矩阵指数的幂级数展开式：

$$\mathbf{F}(△t)=e^{\mathbf{A}△t}=\mathbf{I}+\mathbf{A}△t+\frac{(\mathbf{A}△t{)}^2}{2!}+\frac{(\mathbf{A}△t{)}^3}{3!}+...$$

其中，${\mathbf{A}}^2=\left[\begin{array}{cc}0& 0\\ 0& 0\end{array}\right]$，因此$\mathbf{A}$的更高阶都是$0$。因此，幂级数展开为：

$$\mathbf{F}=\mathbf{I}+\mathbf{A}t+0=\left[\begin{array}{cc}1& △t\\ 0& 1\end{array}\right]$$

这与运动学方程使用的结果相同！

设计观测模型

观测函数采用状态量的先验$\bar{\mathbf{x}}$，并将其转换为表示直线距离的观测值。我们使用毕达哥拉斯定理推导：

$$h(\bar{\mathbf{x}})=\sqrt{x^2+y^2}$$

由于平方根的存在，直线距离和地面位置之间的关系是非线性的。我们通过计算它在${\mathbf{x}}_t$处的偏导数来线性化它:

$$\mathbf{H}=\frac{\partial h(\bar{\mathbf{x}})}{\partial \bar{\mathbf{x}}}{\mid }_{{\bar{\mathbf{x}}}_t}$$

矩阵的偏导数称为雅可比矩阵，其形式为：

$$\frac{\partial \mathbf{H}}{\partial \bar{\mathbf{x}}}=\left[\begin{array}{ccc}\frac{\partial h_1}{\partial x_1}& \frac{\partial h_1}{\partial x_2}& ...\\ \frac{\partial h_2}{\partial x_1}& \frac{\partial h_2}{\partial x_2}& ...\\ ⋮& ⋮& \end{array}\right]$$

换句话说，矩阵中的每个元素都是函数$h$对$x$变量的偏导数。对于我们的问题，我们有：

$$\mathbf{H}=\left[\begin{array}{ccc}\partial h/\partial x& \partial h/\partial \dot{x}& \partial h/\partial y\end{array}\right]$$

依次解决每个问题：

$$\frac{\partial h}{\partial x}=\frac{\partial }{\partial x}\sqrt{x^2+y^2}=\frac{x}{\sqrt{x^2+y^2}}$$

$$\frac{\partial h}{\partial \dot{x}}=\frac{\partial }{\partial \dot{x}}\sqrt{x^2+y^2}=0$$

$$\frac{\partial h}{\partial x}=\frac{\partial }{\partial y}\sqrt{x^2+y^2}=\frac{y}{\sqrt{x^2+y^2}}$$

因此：

$$\mathbf{H}=\left[\begin{array}{ccc}\frac{x}{\sqrt{x^2+y^2}}& 0& \frac{y}{\sqrt{x^2+y^2}}\end{array}\right]$$

这可能看起来令人望而生畏，所以你要认识到所有这些数学都在做一些非常简单的事情。我们有一个关于飞机直线距离的方程，它是非线性的。卡尔曼滤波器只适用于线性方程组，因此我们需要找到一个近似于$\mathbf{H}$的线性方程。正如我们前面所讨论的，在给定点找到非线性方程的斜率是一个很好的近似值。对于卡尔曼滤波器，给定点就是状态量$\mathbf{x}$，因此我们需要取直线距离相对于$\mathbf{x}$的导数。对于线性卡尔曼滤波器，$\mathbf{H}$是我们在运行滤波器之前计算的恒量。对于EKF，$\mathbf{H}$在每一步都要进行更新，因为状态量$\mathbf{x}$点在每个循环后都会发生变化。

为了使这更具体，现在让我们编写一个Python函数来计算这个问题中$h$的雅可比矩阵。

from math import sqrt

def HJacobian_at(x):
    """ compute Jacobian of H matrix at x """
    horiz_dist = x[0]
    altitude   = x[2]
    denom = sqrt(horiz_dist**2 + altitude**2)
    return array ([[horiz_dist/denom, 0., altitude/denom]])

最后，让我们提供$h(\bar{\mathbf{x}})$代码：

def hx(x):
    """ compute measurement for slant range that
    would correspond to state x.
    """
    return (x[0]**2 + x[2]**2) ** 0.5

现在让我们为我们的雷达写一个模拟。

from numpy.random import randn
import math

class RadarSim:
    """ Simulates the radar signal returns from an object
    flying at a constant altityude and velocity in 1D. 
    """
    def __init__(self, dt, pos, vel, alt):
        self.pos = pos
        self.vel = vel
        self.alt = alt
        self.dt = dt

    def get_range(self):
        """ Returns slant range to the object. Call once 
        for each new measurement at dt time from last call.
        """
        # add some process noise to the system
        self.vel = self.vel  + .1*randn()
        self.alt = self.alt + .1*randn()
        self.pos = self.pos + self.vel*self.dt
    
        # add measurement noise
        err = self.pos * 0.05*randn()
        slant_dist = math.sqrt(self.pos**2 + self.alt**2)
        
        return slant_dist + err

设计过程噪声与观测噪声

雷达可以观测到与目标之间的直线距离，假如定义${\sigma }_{range}=5m$表示观测噪声：

$$\mathbf{R}=\left[\begin{array}{c}{\sigma }_{range}^2\end{array}\right]=\left[\begin{array}{c}25\end{array}\right]$$

$\mathbf{Q}$的设计需要一些讨论。状态量为$\mathbf{x}={\left[\begin{array}{ccc}x& \dot{x}& y\end{array}\right]}^T$。前两个元素是地面投影距离和它的变化速度，因此我们可以使用Q_discrete_white_noise()噪声，来计算$\mathbf{Q}$左上角的值。第三个元素是高度，我们假设它与地面投影距离无关。这就导致了$\mathbf{Q}$的块设计：

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

实现

FilterPy提供ExtendedKalmanFilter类。它的工作原理与我们使用的KalmanFilter类类似，只是它允许你提供一个计算$\mathbf{H}$和$h(\mathbf{x})$函数的雅可比矩阵。

我们首先导入滤波器并创建它。$\mathbf{x}$的尺寸为3，$\mathbf{z}$的尺寸为1。

from filterpy.kalman import ExtendedKalmanFilter

rk = ExtendedKalmanFilter(dim_x=3, dim_z=1)

我们创建了雷达模拟器：

radar = RadarSim(dt, pos=0., vel=100., alt=1000.)

我们将在飞机实际位置附近初始化滤波器：

rk.x = array([radar.pos, radar.vel-10, radar.alt+100])

我们使用上面计算的泰勒级数展开式的第一项分配系统矩阵：

dt = 0.05
rk.F = eye(3) + array([[0, 1, 0],
                       [0, 0, 0],
                       [0, 0, 0]])*dt

在为$\mathbf{R}$、$\mathbf{Q}$和$\mathbf{P}$分配了合理的值之后，我们可以用简单地运行滤波器。我们将计算$\mathbf{H}$和$h(\mathbf{x})$的雅可比矩阵的函数传递到update方法中。

for i in range(int(20/dt)):
    z = radar.get_range()
    rk.update(array([z]), HJacobian_at, hx)
    rk.predict()

添加一些代码以保存和绘制我们得到的结果：

from filterpy.common import Q_discrete_white_noise
from filterpy.kalman import ExtendedKalmanFilter
from numpy import eye, array, asarray
import numpy as np

dt = 0.05
rk = ExtendedKalmanFilter(dim_x=3, dim_z=1)
radar = RadarSim(dt, pos=0., vel=100., alt=1000.)

# make an imperfect starting guess
rk.x = array([radar.pos-100, radar.vel+100, radar.alt+1000])

rk.F = eye(3) + array([[0, 1, 0],
                       [0, 0, 0],
                       [0, 0, 0]]) * dt

range_std = 5. # meters
rk.R = np.diag([range_std**2])
rk.Q[0:2, 0:2] = Q_discrete_white_noise(2, dt=dt, var=0.1)
rk.Q[2,2] = 0.1
rk.P *= 50

xs, track = [], []
for i in range(int(20/dt)):
    z = radar.get_range()
    track.append((radar.pos, radar.vel, radar.alt))
    
    rk.update(array([z]), HJacobian_at, hx)
    xs.append(rk.x)
    rk.predict()

xs = asarray(xs)
track = asarray(track)
time = np.arange(0, len(xs)*dt, dt)
ekf_internal.plot_radar(xs, track, time)

用SymPy计算雅可比矩阵

根据你对导数的经验，你可能会发现雅可比矩阵的计算很困难。可能你会很容易地发现它，但也可能稍微难一点的问题就会导致非常困难的计算。

你可以使用SymPy软件包为我们计算雅可比矩阵：

import sympy
from IPython.display import display
sympy.init_printing(use_latex='mathjax')

x, x_vel, y = sympy.symbols('x, x_vel y')
H = sympy.Matrix([sympy.sqrt(x**2 + y**2)])

state = sympy.Matrix([x, x_vel, y])
J = H.jacobian(state)

display(state)
display(J)

$\left[\begin{array}{c}x\\ x_{vel}\\ y\end{array}\right]$

$\left[\begin{array}{ccc}\frac{x}{\sqrt{x^2+y^2}}& 0& \frac{y}{\sqrt{x^2+y^2}}\end{array}\right]$

这个结果与我们上面计算的结果相同，而且我们不必花太大的力气！

机器人定位

是时候尝试解决一个真正的问题了，我提醒一下你，这一部分很难。然而，大多数书籍都选择了简单的教科书式的问题和解决办法，而你想要解决的却是现实世界中的问题。

我们将考虑机器人定位的问题。我们已经在无迹卡尔曼滤波一章中实现了这一点，如果你还没有阅读到，我建议你现在就去阅读它。在这个场景中，我们有一个机器人在一个环境中移动，它使用传感器来检测地标。这可能是一辆自动驾驶汽车，使用计算机视觉技术来识别树木、建筑物和其他地标；可能是一个小型机器人，可以为你的房子吸尘；也可能是仓库里的机器人。

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

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

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

机器人运动模型

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

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

ekf_internal.plot_bicycle()

在无迹卡尔曼滤波一章中，我们推导了以下方程式：

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

$$x=x-Rsin(\theta )+Rsin(\theta +\beta )$$

$$y=y+Rcos(\theta )-Rcos(\theta +\beta )$$

$$\theta =\theta +\beta$$

其中，$\theta$是机器人的航向角。

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

设计状态量

对于我们的滤波器，我们将追踪机器人的位置$x$、$y$和方向$\theta$：

$$\mathbf{x}=\left[\begin{array}{c}x\\ y\\ \theta \end{array}\right]$$

我们的控制输入$\mathbf{u}$是速度$v$和转向角$\alpha$：

$$\mathbf{u}=\left[\begin{array}{c}v\\ \alpha \end{array}\right]$$

设计系统模型

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

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

使用上面研究的机器人运动模型，我们可以将其扩展到：

$$\left[\begin{array}{c}\bar{x}\\ y\\ \theta \end{array}\right]=\left[\begin{array}{c}x\\ y\\ \theta \end{array}\right]+\left[\begin{array}{c}-Rsin(\theta )+Rsin(\theta +\beta )\\ Rcos(\theta )-Rcos(\theta +\beta )\\ \beta \end{array}\right]$$

我们通过取$f(x,u)$的雅可比矩阵来求$\mathbf{F}$：

$$\mathbf{F}=\frac{\partial f(x,u)}{\partial x}=\left[\begin{array}{ccc}\frac{\partial f_1}{\partial x}& \frac{\partial f_1}{\partial y}& \frac{\partial f_1}{\partial \theta }\\ \frac{\partial f_2}{\partial x}& \frac{\partial f_2}{\partial y}& \frac{\partial f_2}{\partial \theta }\\ \frac{\partial f_3}{\partial x}& \frac{\partial f_3}{\partial y}& \frac{\partial f_3}{\partial \theta }\end{array}\right]$$

通过计算，我们得到：

$$\mathbf{F}=\left[\begin{array}{ccc}1& 0& -Rcos(\theta )+Rcos(\theta +\beta )\\ 0& 1& -Rsin(\theta )+Rsin(\theta +\beta )\\ 0& 0& 1\end{array}\right]$$

我们可以根据SymPy再核对一下：

import sympy
from sympy.abc import alpha, x, y, v, w, R, theta
from sympy import symbols, Matrix

sympy.init_printing(use_latex="mathjax", fontsize='16pt')
time = symbols('t')
d = v*time
beta = (d/w)*sympy.tan(alpha)
r = w/sympy.tan(alpha)

fxu = Matrix([[x-r*sympy.sin(theta) + r*sympy.sin(theta+beta)],
              [y+r*sympy.cos(theta)- r*sympy.cos(theta+beta)],
              [theta+beta]])
F = fxu.jacobian(Matrix([x, y, theta]))
F

$\left[\begin{array}{ccc}1& 0& -\frac{wcos(\theta )}{tan(\alpha )}+\frac{wcos(\frac{tvtan(\alpha )}{w}+\theta )}{tan(\alpha )}\\ 0& 1& -\frac{wsin(\theta )}{tan(\alpha )}+\frac{wsin(\frac{tvtan(\alpha )}{w}+\theta )}{tan(\alpha )}\\ 0& 0& 1\end{array}\right]$

这看起来有点复杂。我们可以用SymPy来得到更简洁的形式：

# reduce common expressions
B, R = symbols('beta, R')
F = F.subs((d/w)*sympy.tan(alpha), B)
F.subs(w/sympy.tan(alpha), R)

$\left[\begin{array}{ccc}1& 0& -Rcos(\theta )+Rcos(\theta +\beta )\\ 0& 1& -Rsin(\theta )+Rsin(\theta +\beta )\\ 0& 0& 1\end{array}\right]$

该形式验证了之前雅可比矩阵的计算是正确的。

现在我们可以把注意力转移到噪音上。这里，噪声在我们的控制输入中，所以它在控制空间中。换句话说，我们需要一个特定的速度和转向角，但我们需要把它转换成$x$，$y$，$\theta$的误差。在实际系统中，这可能因速度而异，因此每次预测都需要重新计算。我会选择这个作为噪声模型。对于一个真正的机器人，你需要选择一个能准确描述系统错误的模型。

$$\mathbf{M}=\left[\begin{array}{cc}{\sigma }_{vel}^2& 0\\ 0& {\sigma }_{\alpha }^2\end{array}\right]$$

如果这是一个线性问题，我们可以使用现在熟悉的${\mathbf{FMF}}^T$形式，从控制空间转换到状态空间。但由于我们的运动模型是非线性的，需要使用雅可比矩阵将其线性化，我们将其命名为$\mathbf{V}$。

$$\mathbf{V}=\frac{\partial f(x,u)}{\partial u}=\left[\begin{array}{cc}\frac{\partial f_1}{\partial v}& \frac{\partial f_1}{\partial \alpha }\\ \frac{\partial f_2}{\partial v}& \frac{\partial f_2}{\partial \alpha }\\ \frac{\partial f_3}{\partial v}& \frac{\partial f_3}{\partial \alpha }\end{array}\right]$$

这些偏导数有时会变得很难处理，让我们用SymPy计算它们。

V = fxu.jacobian(Matrix([v, alpha]))
V = V.subs(sympy.tan(alpha)/w, 1/R) 
V = V.subs(time*v/R, B)
V = V.subs(time*v, 'd')
V

$\left[\begin{array}{cc}tcos(\beta +\theta )& \frac{d(ta{n}^2(\alpha )+1)cos(\beta +\theta )}{(tan(\alpha )}-\frac{w(-ta{n}^2(\alpha )-1)sin(\theta )}{(ta{n}^2(\alpha )}+\frac{w(-ta{n}^2(\alpha )-1)sin(\beta +\theta )}{(ta{n}^2(\alpha )}\\ tsin(\beta +\theta )& \frac{d(ta{n}^2(\alpha )+1)cos(\beta +\theta )}{(tan(\alpha )}+\frac{w(-ta{n}^2(\alpha )-1)cos(\theta )}{(ta{n}^2(\alpha )}-\frac{w(-ta{n}^2(\alpha )-1)cos(\beta +\theta )}{(ta{n}^2(\alpha )}\\ \frac{t}{R}& \frac{d(ta{n}^2(\alpha )+1)}{w}\end{array}\right]$

这为我们提供了预测方程的最终形式：

$$\bar{\mathbf{x}}=\mathbf{x}+\left[\begin{array}{c}-Rsin(\theta )+Rsin(\theta +\beta )\\ Rcos(\theta )-Rcos(\theta +\beta )\\ \beta \end{array}\right]$$

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

这种形式的线性化并不是预测$\mathbf{x}$的唯一方法。例如，我们可以使用数值积分技术，如Runge Kutta来计算机器人的运动。如果时间步长相对较大，则需要这样做。对于卡尔曼滤波器来说，使用EKF并没有KF那样简单。对于一个实际问题，你必须仔细地用微分方程对你的系统建模，然后确定解决这个系统的最合适的方法。正确的方法取决于所需的精度、方程的非线性程度、处理器预算和数值稳定性。

设计观测模型

传感器提供了相对于已知地标的、有噪声的方向和距离观测结果。观测模型必须转换状态量${\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{x}$处的雅可比矩阵，来线性化$h$。我们用Symphy来计算。

px, py = symbols('p_x, p_y')
z = Matrix([[sympy.sqrt((px-x)**2 + (py-y)**2)],
            [sympy.atan2(py-y, px-x) - theta]])
z.jacobian(Matrix([x, y, theta]))

$\left[\begin{array}{ccc}\frac{-p_x+x}{\sqrt{(p_x-x{)}^2+(p_y-y{)}^2}}& \frac{-p_y+y}{\sqrt{(p_x-x{)}^2+(p_y-y{)}^2}}& 0\\ -\frac{-p_y+y}{(p_x-x{)}^2+(p_y-y{)}^2}& -\frac{p_x-x}{(p_x-x{)}^2+(p_y-y{)}^2}& -1\end{array}\right]$

现在我们需要将其编写为Python函数。例如，我们可以写：

from math import sqrt

def H_of(x, landmark_pos):
    """ compute Jacobian of H matrix where h(x) computes 
    the range and bearing to a landmark for state x """
    px = landmark_pos[0]
    py = landmark_pos[1]
    hyp = (px - x[0, 0])**2 + (py - x[1, 0])**2
    dist = sqrt(hyp)

    H = array(
        [[-(px - x[0, 0]) / dist, -(py - x[1, 0]) / dist, 0],
         [ (py - x[1, 0]) / hyp,  -(px - x[0, 0]) / hyp, -1]])
    return H

我们还需要定义一个将状态量转换为观测量的函数。

from math import atan2

def Hx(x, landmark_pos):
    """ takes a state variable and returns the measurement
    that would correspond to that state.
    """
    px = landmark_pos[0]
    py = landmark_pos[1]
    dist = sqrt((px - x[0, 0])**2 + (py - x[1, 0])**2)

    Hx = array([[dist],
                [atan2(py - x[1, 0], px - x[0, 0]) - x[2, 0]]])
    return Hx

设计观测噪声

可以合理地假设距离观测和方向观测的噪声是独立的，因此：

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

实现

我们将使用FilterPy的ExtendedKalmanFilter类来实现该滤波器。它的predict()方法使用标准线性方程组的过程模型。而我们的是非线性的，所以我们必须用自己的实现覆盖predict()。我还想使用这个类来模拟机器人，所以我将添加一个move()方法来计算机器人的位置。

预测步骤的矩阵相当大。evalf给变量设置特定值来计算Symphy矩阵。我向你演示这种用法，并在卡尔曼滤波代码中使用它。你需要了解几点。

首先，evalf使用dict类型来指定值。例如，如果矩阵包含$x$和$y$，则可以编写：

M.evalf(subs={x:3, y:17})

来计算$x=3$和$y=17$的矩阵。

其次，evalf返回一个sympy.Matrix对象。使用numpy.array(M).astype(float)将其转换为NumPy数组。numpy.array(M)创建一个object类型的Numpy数组，这并不是你想要的。

以下是EKF的代码：

from filterpy.kalman import ExtendedKalmanFilter as EKF
from numpy import array, sqrt

class RobotEKF(EKF):
    def __init__(self, dt, wheelbase, std_vel, std_steer):
        EKF.__init__(self, 3, 2, 2)
        self.dt = dt
        self.wheelbase = wheelbase
        self.std_vel = std_vel
        self.std_steer = std_steer

        a, x, y, v, w, theta, time = symbols(
            'a, x, y, v, w, theta, t')
        d = v*time
        beta = (d/w)*sympy.tan(a)
        r = w/sympy.tan(a)
    
        self.fxu = Matrix(
            [[x-r*sympy.sin(theta)+r*sympy.sin(theta+beta)],
             [y+r*sympy.cos(theta)-r*sympy.cos(theta+beta)],
             [theta+beta]])

        self.F_j = self.fxu.jacobian(Matrix([x, y, theta]))
        self.V_j = self.fxu.jacobian(Matrix([v, a]))

        # save dictionary and it's variables for later use
        self.subs = {x: 0, y: 0, v:0, a:0, 
                     time:dt, w:wheelbase, theta:0}
        self.x_x, self.x_y, = x, y 
        self.v, self.a, self.theta = v, a, theta

    def predict(self, u):
        self.x = self.move(self.x, u, self.dt)

        self.subs[self.theta] = self.x[2, 0]
        self.subs[self.v] = u[0]
        self.subs[self.a] = u[1]

        F = array(self.F_j.evalf(subs=self.subs)).astype(float)
        V = array(self.V_j.evalf(subs=self.subs)).astype(float)

        # covariance of motion noise in control space
        M = array([[self.std_vel*u[0]**2, 0], 
                   [0, self.std_steer**2]])

        self.P = F @ self.P @ F.T + V @ M @ V.T

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

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

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

现在我们还有另一个问题要处理。残差的计算理论上为$y=z-h(x)$，但这不起作用，因为我们的观测包含一个角度。假设$z$的方向为$1^{\circ }$， $h(x)$是$359^{\circ }$。直接利用残差公式计算得到的残差为$-358^{\circ }$，而正确的角度差是$2^{\circ }$。因此，我们必须编写代码来正确计算方向残差。

def residual(a, b):
    """ compute residual (a-b) between measurements containing 
    [range, bearing]. Bearing is normalized to [-pi, pi)"""
    y = a - b
    y[1] = y[1] % (2 * np.pi)    # force in range [0, 2 pi)
    if y[1] > np.pi:             # move to [-pi, pi)
        y[1] -= 2 * np.pi
    return y

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

from filterpy.stats import plot_covariance_ellipse
from math import sqrt, tan, cos, sin, atan2
import matplotlib.pyplot as plt

dt = 1.0

def z_landmark(lmark, sim_pos, std_rng, std_brg):
    x, y = sim_pos[0, 0], sim_pos[1, 0]
    d = np.sqrt((lmark[0] - x)**2 + (lmark[1] - y)**2)  
    a = atan2(lmark[1] - y, lmark[0] - x) - sim_pos[2, 0]
    z = np.array([[d + randn()*std_rng],
                  [a + randn()*std_brg]])
    return z

def ekf_update(ekf, z, landmark):
    ekf.update(z, HJacobian=H_of, Hx=Hx, 
               residual=residual,
               args=(landmark), hx_args=(landmark))
    
                
def run_localization(landmarks, std_vel, std_steer, 
                     std_range, std_bearing,
                     step=10, ellipse_step=20, ylim=None):
    ekf = RobotEKF(dt, wheelbase=0.5, std_vel=std_vel, 
                   std_steer=std_steer)
    ekf.x = array([[2, 6, .3]]).T # x, y, steer angle
    ekf.P = np.diag([.1, .1, .1])
    ekf.R = np.diag([std_range**2, std_bearing**2])

    sim_pos = ekf.x.copy() # simulated position
    # steering command (vel, steering angle radians)
    u = array([1.1, .01]) 

    plt.figure()
    plt.scatter(landmarks[:, 0], landmarks[:, 1],
                marker='s', s=60)
    
    track = []
    for i in range(200):
        sim_pos = ekf.move(sim_pos, u, dt/10.) # simulate robot
        track.append(sim_pos)

        if i % step == 0:
            ekf.predict(u=u)

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

            x, y = sim_pos[0, 0], sim_pos[1, 0]
            for lmark in landmarks:
                z = z_landmark(lmark, sim_pos,
                               std_range, std_bearing)
                ekf_update(ekf, z, lmark)

            if i % ellipse_step == 0:
                plot_covariance_ellipse(
                    (ekf.x[0,0], ekf.x[1,0]), ekf.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("EKF Robot localization")
    if ylim is not None: plt.ylim(*ylim)
    plt.show()
    return ekf

landmarks = array([[5, 10], [10, 5], [15, 15]])

ekf = run_localization(
    landmarks, std_vel=0.1, std_steer=np.radians(1),
    std_range=0.3, std_bearing=0.1)
print('Final P:', ekf.P.diagonal())

Final P: [0.025 0.042 0.002]

我把这些地标画成了实心正方形，机器人的路径是用一条黑线表示。预测步骤的协方差椭圆为浅灰色，更新的协方差椭圆以绿色显示。为了使它们在这个比例下可见，我将椭圆边界设置为$6\sigma$。

我们可以看到，我们的运动模型增加了很多不确定性，而且大部分误差都发生在运动方向上（通过绿色椭圆的形状可以确定）。经过几步之后，我们可以看到滤波器结合了地标测量值，误差得到了改善。

我在UKF章节中使用了相同的初始条件和地标位置。UKF在误差椭圆方面获得了更好的精度。就$\mathbf{x}$的估计而言，两者的表现大致相同。

现在，让我们添加另一个地标。

landmarks = array([[5, 10], [10, 5], [15, 15], [20, 5]])

ekf = run_localization(
    landmarks, std_vel=0.1, std_steer=np.radians(1),
    std_range=0.3, std_bearing=0.1)
plt.show()
print('Final P:', ekf.P.diagonal())

Final P: [0.02  0.02  0.002]

轨道末端附近估计值的不确定性较小，我们可以看到使用多个地标对我们的不确定性的影响还是很大的。下面仅使用前两个地标：

ekf = run_localization(
    landmarks[0:2], std_vel=1.e-10, std_steer=1.e-10,
    std_range=1.4, std_bearing=.05)
print('Final P:', ekf.P.diagonal())

Final P: [0.022 0.045 0.   ]

经过地标后，估计值很快偏离机器人的路径，协方差也快速增长。让我们看看只有一个地标会发生什么：

ekf = run_localization(
    landmarks[0:1], std_vel=1.e-10, std_steer=1.e-10,
    std_range=1.4, std_bearing=.05)
print('Final P:', ekf.P.diagonal())

Final P: [0.263 0.798 0.004]

正如你所猜测的那样，一个地标会产生非常糟糕的结果。相反，大量的地标使我们能够做出非常准确的估计。

landmarks = array([[5, 10], [10,  5], [15, 15], [20,  5], [15, 10], 
                   [10,14], [23, 14], [25, 20], [10, 20]])

ekf = run_localization(
    landmarks, std_vel=0.1, std_steer=np.radians(1),
    std_range=0.3, std_bearing=0.1, ylim=(0, 21))
print('Final P:', ekf.P.diagonal())

Final P: [0.008 0.009 0.001]

讨论

我说这是一个真正的现实问题，在某些方面确实如此。我看到过使用机器人运动模型的其他演示，那些模型使用了更简单的雅可比矩阵。其实，我的运动模型从某个角度上看也是简化了的——它使用自行车模型。一辆真正的汽车有两套轮胎，每一套都以不同的半径行驶。车轮也不能很好地抓住路面。我还假设机器人会即时响应控制输入。Sebastian Thrun在《概率机器人》（Probabilistic Robots）中写道，这种简化模型是合理的，因为滤波器在用于跟踪真实车辆时表现良好。这里的结论是，虽然你必须有一个较精确的非线性模型，但它并不需要非常完美才能很好地运行。作为一名设计师，你需要平衡模型的保真度、数学难度以及执行线性代数所需的CPU时间。

这个问题被简单化的另一个点是，我们知道地标和观测值之间的对应关系。但是假设我们使用的是雷达——我们怎么知道特定的信号返回对应于本地场景中的哪个特定地标？这个问题暗示了SLAM算法——同步定位和建图。SLAM不是本书的重点，所以我不会详细阐述这个话题。

UKF与EKF

在上一章中，我使用UKF来解决这个问题。两者实现上的差异应该非常明显。尽管有一个基本的运动模型，但计算状态模型和观测模型的雅可比矩阵并不是一件小事。不同的问题，可能导致雅可比矩阵难以或不可能通过解析的方法得到。相比之下，UKF只需要提供一个状态模型的函数和另一个观测模型的函数。

在许多情况下，雅可比矩阵无法解析地找到。具体的细节超出了本书的范围，但你必须通过数值的方法来计算雅可比矩阵。这项工作并非简单，你可能需要花费相当大一部分精力学习技术来处理此类情况。即使这样，你也可能只能解决与你的领域相关的问题——航空工程师对Navier Stokes方程了解很多，但对化学反应速率模型可能就了解不多了。

因此，UKF很容易。那么，它准确吗？在实践中，它通常比EKF表现更好。你可以找到大量的研究论文，证明UKF在各种问题领域的表现优于EKF。不难理解为什么这是真的。EKF通过在单点处线性化系统模型和观测模型来工作，UKF使用$2n+1$个sigma点。

让我们看一个具体的例子。拿$f(x)=x^3$并通过高斯分布，我将使用蒙特卡罗模拟计算准确答案。我根据高斯分布随机生成50000个点，每个点通过$f(x)$，然后计算结果的均值和方差。

EKF通过在求值点$x$处求导数来线性化函数，该斜率成为我们用于变换高斯函数的线性函数。

import kf_book.nonlinear_plots as nonlinear_plots

nonlinear_plots.plot_ekf_vs_mc()

actual mean=1.30, std=1.14
EKF    mean=1.00, std=0.95

EKF的计算相当不准确。相比之下，以下是UKF的表现：

nonlinear_plots.plot_ukf_vs_mc(alpha=0.001, beta=3., kappa=1.)

actual mean=1.30, std=1.12
UKF    mean=1.30, std=1.08

这里我们可以看到UKF平均值的计算精确到小数点后2位。标准偏差稍有偏差，但你也可以通过使用生成sigma点的$\alpha$、$\beta$和$\kappa$参数来微调UKF计算分布的方式。这里我使用了$\alpha =0.001$，$\beta =3$，$\kappa =1$。可以随意修改它们以查看结果，你可能能得到比我更好的结果。但是，避免针对特定测试过度调整UKF。对于你的测试用例，它的性能可能更好，但总体上却可能更差。

相关阅读

• Kalman-and-Bayesian-Filters-in-Python/blob/master/11-Extended-Kalman-Filters