【读书笔记 | 自动驾驶中的雷达信号处理（第7章 目标滤波与跟踪）】

本文编辑：调皮哥的小助理

大家好，又和大家见面了，时间过得很快，到目前为止，如下面的目录所示，我们已经阅读过汽车雷达目标检测的一些基本的原理了，特别是距离、速度和角度。虽然这些表示瞬时目标状态的信息可以作为雷达处理的主要目标，但在汽车雷达处理中，跟踪运动目标是是一个非常重要的功能。

今天是《自动驾驶中的雷达处理算法》专栏的第8篇文章，其余文章可以在下面的链接中阅读：

目录如下：

1.调皮连续波：读书笔记 | 自动驾驶中的雷达信号处理（引言）

2.调皮连续波：读书笔记 | 自动驾驶中的雷达信号处理（第1章 雷达系统基础）

3.调皮连续波：读书笔记 | 自动驾驶中的雷达信号处理（第2章 雷达方程）

4.调皮连续波：读书笔记 | 自动驾驶中的雷达信号处理（第3章 雷达系统信号处理）

5.调皮连续波：读书笔记 | 自动驾驶中的雷达信号处理（第4章 雷达波形及其数学模型）

6.调皮连续波：读书笔记 | 自动驾驶中的雷达信号处理（第5章 雷达目标检测 ）

7.调皮连续波：读书笔记 | 自动驾驶中的雷达信号处理（第6章 到达方向(DOA)估计算法 ）

7.1介绍

本文的目的是利用滤波和跟踪的算法对探测到的雷达目标进行处理，以捕获目标的运动状态。其中，卡尔曼滤波作为传统的目标跟踪方法被广泛采用。然而，贝叶斯算法最近获得了关注，因为它们解决了卡尔曼滤波的一些缺点。

本文将概述这些算法，并指出在实际应用中存在的一些困难，另外本文不打算详细介绍相关的原理，关于这些内容可以在下面分享的书籍中找到：

调皮连续波：【书籍推荐】(第1期) 学习目标检测、定位、跟踪的3本书！

7.2卡尔曼滤波

7.2.1 基本概念

卡尔曼滤波器是一种基于线性动态系统理论的计算高效递归滤波器，它从噪声测量中估计离散时间线性动态系统的状态 ， 自提出以来，卡尔曼滤波器 (KF) 已被大量研究用于广泛的应用。

KF考虑到是离散时间线性动态系统，过程模型描述如下:

$\mathbf{x}(k)=\mathbf{F}(k-1)\mathbf{x}(k-1)+\mathbf{G}(k-1)\mathbf{u}(k-1)+\mathbf{v}(k-1)$（7.1）

其中，$x(k)$ 是时间 $k$ 的状态向量，$F(k-1)$ 是状态转移矩阵，$G(k-1)$ 是输入控制矩阵，$u(k-1)$ 是控制输入，$v(k-1)$ 零均值高斯白噪声。 时间 $k$ 处过程噪声的协方差矩阵由 $E\left(v(k)v(k{)}^H\right)=Q(k)$ 定义。

测量方程由下式给出:

$\mathbf{z}(k)=\mathbf{H}(k)\mathbf{x}(k)+w(k)$（7.2）

其中，$z(k)$ 是测量向量，$w(k)$ 是零均值高斯白噪声。 测量噪声的协方差矩阵由 $E\left(w(k)w(k{)}^H\right)=R(k)$ 定义。

为了将上述内容放到汽车雷达处理的维度，且 $x(k)$ 是不可观测的，故而必须使用 $z(k)$ 来估计。 $z(k)$由目标状态测量得到，即距离、速度和角度(DOA)。

对于滤波和跟踪，状态向量 $x(k)$ 包括目标在$x$ 和 $y$方向上的位置、速度和加速度。

如果只跟踪目标的位置 $[x,y{]}^T$ , 则 $x(k)$ 是二维向量。
如果跟踪目标的位置和速度${\left[\begin{array}{llll}x& y& v_x& v_y\end{array}\right]}^{\mathrm{T}}$，则$x(k)$是一个四维向量。
如果跟踪目标的位置、速度和加速度 ${\left[\begin{array}{llllll}x& y& v_x& v_y& a_x& a_y\end{array}\right]}^{\mathrm{T}}$ ，则$x(k)$是一个六维向量。
其实还可以类推，位置、速度、加速度、高度等，那么就是九维向量 ${\left[\begin{array}{lllllllll}x& y& z& v_x& v_y& v_z& a_x& a_y& a_z\end{array}\right]}^{\mathrm{T}}$ 。

假设矩阵$F、G、Q、H$ 和 $R$ 是已知的。虽然$G(k)$ 和 $u(k)$在控制的应用中很重要，但它们在汽车应用中不太相关。

KF操作要求基于各自状态估计的误差协方差矩阵的先验$P(k\mid k-1)$ 和后验$P(k\mid k)$ 估计。 先验状态估计 $\hat{\mathbf{x}}(k\mid k-1)$ 是时间的状态估计，仅基于到 $k-1$ 的可用测量值，而不考虑当前测量值 $z(k)$ 。

另一方面，后验状态估计$\hat{\mathbf{X}}(k\mid k)$是在考虑当前测量值 $z(k)$ 的情况下计算的系统在时间 $k$ 的估计。 根据这些值，估计误差协方差如下所示：

$\mathbf{P}(k\mid k-1)=E\left[(\mathbf{x}(k)-\hat{\mathbf{x}}(k\mid k-1))(\mathbf{x}(k)-\hat{\mathbf{x}}(k\mid k-1){)}^{\mathrm{T}}\right]$
$\mathbf{P}(k\mid k)=E\left[(\mathbf{x}(k)-\hat{\mathbf{x}}(k\mid k))(\mathbf{x}(k)-\hat{\mathbf{x}}(k\mid k){)}^{\mathrm{T}}\right]$（7.3）

初始化之后，KF计算分为两个不同的步骤进行，分别是预测和更新，如下所述：

（1）初始化
将状态估计和协方差估计初始化为$\hat{\mathbf{x}}(0)$ 和 $P(0)$
（2）预测
状态预测(先验估计):
$\hat{\mathbf{x}}(k\mid k-1)=\mathbf{F}(k-1)\hat{\mathbf{x}}(k-1\mid k-1)$（7.4）
误差协方差预测(先验估计):
$\mathbf{P}(k\mid k-1)=\mathbf{F}(k-1)\mathbf{P}(k-1\mid k-1)\mathbf{F}(k-1{)}^{\mathrm{T}}+\mathbf{Q}(k-1)$（7.5）
（3）更新
假设测量值 是可用的，则卡尔曼增益计算如下：
$\mathbf{K}(k)=\mathbf{P}(k\mid k-1)\mathbf{H}(k{)}^{\mathrm{T}}{\left(\mathbf{H}(k)\mathbf{P}(k\mid k-1)\mathbf{H}(k{)}^{\mathrm{T}}+\mathbf{R}(k)\right)}^{-1}$（7.6）
更新状态估计(后验估计):
$\hat{\mathbf{x}}(k\mid k)=\hat{\mathbf{x}}(k\mid k-1)+\mathbf{K}(k)(\mathbf{z}(k)-\mathbf{H}(k)\hat{\mathbf{x}}(k\mid k-1))$（7.7）
更新误差协方差估计(后验估计):
$\mathbf{P}(k\mid k)=\mathbf{P}(k\mid k-1)-\mathbf{K}(k)\mathbf{H}(k)\mathbf{P}(k\mid k-1)$(7.8)

上述公式中的最优卡尔曼增益 $K(k)$ 使后验误差协方差最小， 详细推导可以在文献 [3, 6] 中找到。 后期处理可以应用于 $\hat{\mathbf{x}}(k\mid k)$ 以获得平滑估计。

上面讨论的 KF滤波算法应用于雷达测量时存在几个缺点：

第一、上述讨论是基于“假设底层过程是线性的”，卡尔曼滤波仅能对线性的过程模型和测量模型进行精确的估计，在非线性的场景中并不能达到最优的估计效果，为了能够设定线性的环境，我们假定了我们的过程模型为恒定速度模型，但是显然在显示的应用中不是这样的，不论是过程模型还是测量模型，都是非线性的。
第二、过程噪声和观测噪声都是高斯噪声的假设并不总是正确的。当噪声为统计特性已知的白噪声时, Kalman 滤波器的滤波效果是很好的。当噪声为统计特性未知的有色噪声或系统具有不确定性时,Kalman 滤波的结果就不太令人满意。卡尔曼滤波器也不能有效地跟踪快速加速或减速的目标，因此需要改进基本假设。

基于上述事实，文献[7-12]提出了扩展卡尔曼滤波器（EKF）和无迹卡尔曼滤波器（UKF）。 在汽车雷达环境中，非线性可以通过将距离和 DOA 估计转换为笛卡尔坐标进行位置估计来引入。

示例：以卡尔曼滤波器跟踪为例，使用模型 $x(t)=1+2t+2t^2$ 生成 2000 点测试位置数据。 假设测量噪声方差为 0.01，过程噪声方差为 0.18。 采样周期设置为 1,跟踪结果如图 1 所示。

（图1 卡尔曼滤波跟踪示例）

预测误差如图2所示，对于这个例子，可以认为跟踪误差很小。

（图2 卡尔曼滤波预测误差示例）

7.2.2 扩展卡尔曼滤波器（EKF）

EKF 将状态转换和测量建模为状态的可微函数，这些函数不是明确的线性，如下所示。

$\mathbf{x}(k)=f(\mathbf{x}(k-1),u(k))+v(k)$
$\mathbf{z}(k)=f(\mathbf{x}(k))+w(k)$ （7.9）

EKF模型中没有使用线性状态转移矩阵 $F(k)$ ，而是使用了更通用的函数 $f(x(k-1),u(k))$ 。 对噪声的假设与 KF 相似。

EKF过滤器的预测和更新步骤如下所述进行：

（1）预测
状态预测(先验估计):
$\hat{\mathbf{x}}(k\mid k-1)=f(\hat{\mathbf{x}}(k-1\mid k-1),u(k-1))$（7.10）
误差协方差预测(先验估计)：
$\mathbf{P}(k\mid k-1)=\mathbf{F}(k)\mathbf{P}(k-1\mid k-1)\mathbf{F}(k{)}^{\mathrm{T}}+\mathbf{Q}(k-1)$（7.11）
其中，$F(k)$ 由 $f(\hat{\mathbf{x}}(k-1\mid k-1),\mathbf{u}(k-1))$的雅可比矩阵定义在 $\hat{\mathbf{x}}(k-1\mid k-1)$ 处的值。
$\mathbf{F}(k)=\frac{\partial f}{\partial x}\mid \hat{\mathbf{x}}(k-1\mid k-1)$（7.12）
（2）更新
假设测量值 是可用的，则卡尔曼增益计算如下:
$\mathbf{K}(k)=\mathbf{P}(k\mid k-1)\mathbf{H}(k{)}^{\mathrm{T}}{\left(\mathbf{H}(k)\mathbf{P}(k\mid k-1)\mathbf{H}(k{)}^{\mathrm{T}}+\mathbf{R}(k)\right)}^{-1}$（7.13）
其中$H(k)$ 由 $\mathbf{h}(\hat{\mathbf{x}}(k\mid k-1))$的雅可比矩阵定义在 $\hat{\mathbf{x}}(k\mid k-1)$ 处的值：
$\mathbf{H}(k)=\frac{\partial h}{\partial \mathbf{x}}\mid \hat{\mathbf{x}}(k\mid k-1)$（7.14）
更新状态估计(后验估计):
$\mathbf{P}(k\mid k)=\mathbf{P}(k\mid k-1)-\mathbf{K}(k)\mathbf{H}(k)\mathbf{P}(k\mid k-1)$（7.15）

EKF虽然在导航和GPS跟踪中应用了很长时间，但也存在几个问题：

（1）状态估计在最小均方误差（MMSE）上不是最优的。
（2）错误的初始状态估计可能导致滤波器发散。
（3）对于大多数应用，尤其是汽车应用，与雅可比矩阵相关的计算复杂度很高。 最后，过程和测量模型的高斯假设仍然存在，对于 KF可能与现实生活中的情况不符。

7.2.3 无迹卡尔曼滤波器（UKF）

无迹卡尔曼滤波器 (UKF) 利用非线性无迹变换来弥补EKF 线性模型的不足，并尝试处理与 EKF 相关的问题，同时保持 KF的基本模型不变。

Unscented Kalman Filter是解决非线性卡尔曼滤波的另一种思路，它利用Unscented Transform来解决概率分布非线性变换的问题。UnScented Kalman Filter不需要像Extended Kalman Filter一样计算Jacobin矩阵，在计算量大致相当的情况下，能够获得更加精确非线性处理效果。

无迹变换（UT）

无迹变换 (UT) 是一种计算经历了非线性变换的随机变量的统计量的方法 ，UT背后的动机是使用一个精确的非线性函数来精确逼近状态的概率分布，而不是EKF中使用的雅可比矩阵近似。

考虑通过非线性函数 $z=f(x)$ 计算随机变量 $x$（维度 L）， 假设$x$的均值为 $\bar{x}$ ，协方差为 $P_x$。 下一步计算所谓的sigma 向量或sigma点（在 L 维空间中）${\chi }_i$ ，如下所示：
$\begin{array}{rl}{\chi }_0& =\overline{\mathbf{x}}\\ {\chi }_i& =\overline{\mathbf{x}}+{\left(\sqrt{(L+\lambda ){\mathbf{P}}_{\mathbf{x}}}\right)}_i,i=1,\dots ,L\\ {\chi }_i& =\overline{\mathbf{x}}-{\left(\sqrt{(L+\lambda ){\mathbf{P}}_{\mathbf{x}}}\right)}_{i-L},i=L+1,\dots ,2L\end{array}$（7.16）

其中$\lambda ={\alpha }^2(\kappa +L)$是比例因子，$\alpha$ 表示sigma 点在 $x$周围的分布， 是通常设置为 3-L 的调整参数。 符号 $(\cdot {)}_i$表示矩阵的第 列。 矩阵的平方根可以通过 Cholesky 分解来计算。 计算sigma 点的非线性函数由下式给出：

${\mathbf{Z}}_i=f\left({\chi }_i\right),i=0,\dots ,2L$（7.17）

由(7.17)，$Z$ 的均值和协方差可以用以下公式计算：

$\overline{\mathbf{z}}\cong {\sum }_{i=0}^{2L}{W}_i^{(m)}{\mathbf{Z}}_i$
${\mathbf{P}}_z\cong {\sum }_{i=0}^{2L}{W}_i^{(c)}\left({\mathbf{z}}_{\mathbf{i}}-\overline{\mathbf{z}}\right){\left({\mathbf{z}}_i-\overline{\mathbf{z}}\right)}^{\mathrm{T}}$（7.18）

其中计算平均值的权重 $W_i^{(m)}$ 和计算协方差的权重 $W_i^{(c)}$ 由下式给出:

$\begin{array}{rl}{W}_0^{(m)}& =\frac{\lambda }{L+\lambda }\\ W_0^{(c)}& =\frac{\lambda }{L+\lambda }+1-{\alpha }^2+\beta \\ W_i^{(m)}& =W_i^{(c)}=\frac{\lambda }{2(L+\lambda )},i=0,\dots ,2L\end{array}$（7.19）

其中，$\beta$ 结合了分布的先验知识，并设置高斯假设为 2。

UT 应用于 UKF的系统方程如下所述：

$\mathbf{x}(k)=f(\mathbf{x}(k-1),\mathbf{u}(k))+\mathbf{v}(k)$
$\mathbf{z}(k)=h(\mathbf{x}(k))+\mathbf{w}(k)$（7.20）

算法使用:

将连接状态和噪声向量的向量定义为${\mathbf{x}}^a(k)={\left[{\mathbf{x}}^{\mathrm{T}}(k){\mathbf{v}}^{\mathrm{T}}(k)\right]}^{\mathrm{T}}$ 。
（1）初始化:
将状态估计和协方差估计初始化为$\hat{\mathbf{x}}(0)$ 和 $P(0)$ 。
$\begin{array}{rl}\hat{\mathbf{x}}(0)& =E[\mathbf{x}(0)]\\ \mathbf{P}(0)& =E\left[(\mathbf{x}(0)-\hat{\mathbf{x}}(0))(\mathbf{x}(0)-\hat{\mathbf{x}}(0){)}^{\mathrm{T}}\right]\\ {\hat{\mathbf{x}}}^a(0)& ={\left[{\hat{\mathbf{x}}}^{\mathrm{T}}(0)E\left[{\mathbf{v}}^{\mathrm{T}}(0)\right]\right]}^{\mathrm{T}}\\ {\mathbf{P}}^a(0)& =E\left[\left({\mathbf{x}}^a(0)-{\hat{\mathbf{x}}}^a(0)\right){\left({\mathbf{x}}^a(0)-{\hat{\mathbf{x}}}^a(0)\right)}^{\mathrm{T}}\right]\end{array}$（7.21）
（2）预测
${\hat{\mathbf{x}}}^a(k-1)={\left[{\hat{\mathbf{x}}}^{\mathrm{T}}(k-1)E\left[{\mathbf{v}}^{\mathrm{T}}(k-1)\right]\right]}^{\mathrm{T}}$
${\mathbf{P}}^a(k-1)=\left[\begin{array}{cc}\mathbf{P}(k-1)& 0\\ 0& \mathbf{Q}(k-1)\end{array}\right]$（7.22）
（3）计算sigma点：
${\chi }_0(k-1)={\hat{\mathbf{x}}}^a(k-1)$
${\chi }_i(k-1)={\hat{\mathbf{x}}}^a(k-1)+{\left(\sqrt{(L+\lambda ){\mathbf{P}}^a(k-1)}\right)}_i,i=1,\dots ,L$
${\chi }_i(k-1)={\hat{\mathbf{x}}}^a(k-1)-{\left(\sqrt{(L+\lambda ){\mathbf{P}}^a(k-1)}\right)}_{i-L},i=L+1,\dots ,2L$（7.23）
权重$W_i^{(m)}$ 和 $W_i^{(c)}$的计算方法如上所述。计算 sigma 点、状态和协方差如下所示：
$\begin{array}{rl}{\chi }_i(k\mid k-1)& =f\left({\chi }_i(k-1)\right),i=0,\dots ,2L\\ \hat{\mathbf{x}}(k\mid k-1)& =\sum _{i=0}^{2L}{W}_i^{(m)}{\chi }_i(k\mid k-1)\\ \mathbf{P}(k\mid k-1)& =\sum _{i=0}^{2L}{W}_i^{(c)}\left[\left({\chi }_i(k\mid k-1)-\hat{\mathbf{x}}(k\mid k-1)\right){\left({\chi }_i(k\mid k-1)-\hat{\mathbf{x}}(k\mid k-1)\right)}^{\mathrm{T}}\right]\end{array}$（7.24）
（4）更新
${\mathbf{x}}^a(k\mid k-1)={\left[{\hat{\mathbf{x}}}^T(k\mid k-1)E\left[{\mathbf{v}}^{\mathrm{T}}(k)\right]\right]}^T$
${\mathbf{P}}^a(k\mid k-1)=\left[\begin{array}{cc}\mathbf{P}(k\mid k-1)& 0\\ 0& \mathbf{Q}(k)\end{array}\right]$
${\chi }_0(k\mid k-1)={\mathbf{x}}^a(k\mid k-1)$
${\chi }_i(k\mid k-1)={\mathbf{x}}^a(k\mid k-1)+{\left(\sqrt{(L+\lambda ){\mathbf{P}}^a(k\mid k-1)}\right)}_i,i=1,\dots ,L$
${\chi }_i(k\mid k-1)={\mathbf{x}}^a(k\mid k-1)-{\left(\sqrt{(L+\lambda ){\mathbf{P}}^a(k\mid k-1)}\right)}_{i-L},i=L+1,\dots$,$2L$（7.25）
通过测量模型预测 sigma 点：
${\gamma }_i(k)=h\left({\chi }_i(k\mid k-1)\right),i=0,\dots ,2L$（7.26）
根据 (7.26)，计算预测测量值及其协方差：
$\hat{z}(k)={\sum }_{i=1}^{2L}{W}_i^{(m)}{\gamma }_i(k)$
$P_{z(k)z(k)}={\sum }_{i=1}^{2L}{W}_i^{(c)}\left({\gamma }_i(k)-\hat{z}(k)\right){\left({\gamma }_i(k)-\hat{z}(k)\right)}^{\mathrm{T}}$（7.27）
计算用于估计卡尔曼增益的互协方差：
${\mathbf{P}}_{\mathbf{x}(k)z(k)}={\sum }_{i=1}^{2L}{W}_i^{(c)}\left({\chi }_i(k\mid k-1)-\hat{x}(k\mid k-1)\right){\left({\gamma }_i(k)-\hat{z}(k)\right)}^{\mathrm{T}}$（7.27）
卡尔曼增益：
$\mathbf{K}(k)=P_{x(k)z(k)}{P}_{x(k)z(k)}^{-1}$（7.28）
状态更新：
$\hat{\mathbf{x}}(k\mid k)=\hat{\mathbf{x}}(k\mid k-1)+\mathbf{K}(k)(\mathbf{z}(k)-\hat{z}(k))$（7.29）
协方差更新：
$\mathbf{P}(k\mid k)=\mathbf{P}(k\mid k-1)-\mathbf{K}(k){\mathbf{P}}_{z(k)z(k)}\mathbf{K}(k{)}^{\mathrm{T}}$（7.30）

总的来说：

（1）KF处理线性模型。
（2）EKF 通过雅克比和偏导数近似(处理）非线性模型，但是忽略了高阶导数(当然，高阶导数也可以通过泰勒级数继续求，只是计算量的问题而已。我没试过，但据说其实UKF的精度和二阶EKF差不多）
（3）UKF 通过定义sigma点的方式近似非线性模型，因为没有用雅克比和偏导数，让计算变得更加简单，同时也没有忽略高阶导数项。所以相对EKF,UKF计算量少，且效果好。唯一的问题就是，看着太复杂（实际上也复杂）。

7.2.4 其他 KF 变体

存在其他 KF 变体，例如集合卡尔曼滤波器和快速卡尔曼滤波器，但目前卡尔曼滤波方面的最新技术是 UKF。 UKF 已被证明比其前身 EKF 表现更好，但仍然存在与参数选择相关的问题。但在某些情况下， EKF 的性能更好。

7.3贝叶斯滤波(序贯蒙特卡洛)

7.3.1 基本概念 （粒子滤波）

当系统动力学模型和测量模型均为线性时，KF给出了最优的最小均方误差(MMSE)状态估计，然而，KF并不适用于非线性/非高斯系统。因此，提出了EKF和UKF，不过EKF已经被证明很难实现，所以UKF是跟踪和导航系统推荐的实用解决方案。

尽管UKF提供了更好的基于KF的性能，但它仍然依赖于基本KF假设，为了完全处理非线性/非高斯系统，需要另一种方法，这就是贝叶斯滤波算法。

贝叶斯滤波算法，尤其是粒子滤波在大多数实际情况下不需要遵守线性/高斯模型，放宽了对系统动力学的假设，从而适应更广泛的应用领域，包括线性/非线性和高斯/非高斯过程。

在本文中，我们将概述贝叶斯滤波方法，重点是汽车应用。

关于粒子滤波的内容，还是推荐大家阅读下面这本书：

调皮连续波：【书籍推荐】(第1期) 学习目标检测、定位、跟踪的3本书！

7.3.2 递推贝叶斯估计

与 EKF 和 UKF 相反，贝叶斯滤波算法使用一组随机样本递归地生成状态概率密度函数的近似值。

系统模型可以表示为：

$\mathbf{x}(k)=f(\mathbf{x}(k-1),\mathbf{v}(k-1))$（7.31）

其中 $f(\cdot )$ 是转换函数， $v(\cdot )$ 是已知 PDF 的零均值白噪声。 测量方程由下式给出：

$\mathbf{z}(k)=h(\mathbf{x}(k),\mathbf{w}(k))$（7.32）

其中 $h(\cdot )$ 是转换函数，$w(\cdot )$ 是已知 PDF 的零均值白噪声。

PF 算法通过一组加权随机样本（称为粒子）来近似后验概率密度函数$p(x(k)\mid z(1:k))$ 。 假设状态 $p(x(0))$ 的初始先验分布是已知的，在前一时刻 $k-1$ 的概率密度函数 $p(x(k-1)\mid z(1:k-1))$ 可用，则PDF $p(x(k)\mid z(1:k))$ 可以计算如下：

$p(x(k)\mid z(1:k-1))=\int p(x(k)\mid x(1:k-1))p(x(k-1)\mid z(1:k-\mid 1))\mathrm{d}x(k$
$-1)$(7.33)

可以使用基于贝叶斯定理的当前测量值 $y(k)$ 更新先验（或预测）：

$p(x(k)\mid z(1:k))=\frac{p(y(k)\mid x(k))p(x(k)\mid z(1:k-1))}{p(z(k)\mid z(1:k-1))}$（7.34）

其中，$p(y(z)\mid z(1:k-1))$ 被认为是归一化常数（与状态无关）。 在上述方程中 (7.31)-(7.34) 是最优贝叶斯估计的基础。 以 MMSE 准则为例，可以得到最优状态估计为：

$E[x(k)\mid z(1:k)]=\int x(k)p(x(k)\mid z(1:k)\mathrm{d}x(k)$（7.35）

或者，可以计算使 $p(x(k)\mid z(1:k)$ 最大化的最大后验 (MAP) 估计。

上述方法的最大缺点是不定积分在计算上难以处理，因此需要作近似值，这些近似包括构成粒子滤波器基础的序列蒙特卡罗方法 (SMC)。

7.3.3 粒子滤波器(PF)

PF 通过使用一组随机样本（粒子）$x_i(k),i=1,\dots ,N$ 来表示后验概率密度函数，以获得状态估计，其中 N 是粒子数，与权重 $w_i^{(k)}$ 相关。

在大多数情况下，重要性采样与重采样策略一起用于确定权重 ，特别是在一些重采样策略包括多项重采样、分层重采样、系统重采样和残差重采样中。

权重被归一化，使得它们的总和等于 1。如果假设可以从与 $p(x)$ 相似的密度$q(x)$生成样本，则后验密度近似为：

$p(x(k)\mid z(1:k))\cong {\sum }_{i=1}^N{w}_i(k)\delta \left(x(k)-x_i(k)\right)$（7.36）

其中 $\delta (x)$ 是狄拉克函数，并且存在：

$w_i(k)\propto \frac{p\left(x_i(k)\mid z(1:k)\right)}{q\left(x_i(k)\mid z(1:k)\right)}=w_i(k-1)\frac{p\left(z(k)\mid x_i(k)\right)p\left(x_i(k)\mid x_i(k-1)\right)}{q\left(x_i(k)\mid x_i(k-1),z(k)\right)}$（7.37）

其中， $p(z(k)\mid x(k))$ 是测量模型定义的似然函数，$p\left(x_i(k)\mid x_i(k-1)\right)$ 是过程模型的过渡函数。 PF 的一个常见问题是粒子退化，其中权重集中在一个粒子中，而其他粒子的权重接近于零，因此对后验分布没有任何贡献，简并度是通过给出的有效样本量 (ESS) 来衡量的。

$N_{\mathrm{eff}}=\frac{1}{{\sum }_{i=1}^N{w}_i^2(k)}$（7.38）

重采样策略通过设置阈值来使用 $N_{eff}$ ，以便在$N_{eff}$低于阈值时执行重采样。

Bootstrap （自举滤波器）PF 算法可以总结如下，自举滤波器是最基本的 PF 算法, 可以通过以下步骤来描述:

（1）初始化

$k=0$ ，当 $i=1，\dots N$ 时，样本 $x_i(0)\sim p(x(0))$ 。

（2）重要性采样

(2.1) 当 $i=1，\dots N$ 时，样本 ${\hat{\mathbf{x}}}_l(k)\sim p\left({\mathbf{x}}_i(k)\mid {\mathbf{x}}_i(k-1)\right)$ ，且使得 ${\hat{\mathbf{x}}}_l(0:k)=\left[{\hat{\mathbf{x}}}_l(0:k-1),{\hat{\mathbf{x}}}_l(k)\right]。$

（2.2）计算重要性权重：

${\hat{\mathbf{w}}}_t(k)=p\left(z(k)\mid {\hat{\mathbf{x}}}_t(k)\right)$（7.39）

（2.3）权重归一化

${\hat{\mathbf{w}}}_{\iota }(k)=\frac{{\hat{\mathbf{w}}}_{\iota }(k)}{{\sum }_{i=-1}^N{\hat{\mathbf{w}}}_{(}(k)}$（7.40）

（3）重采样

（3.1）计算由下式给出的有效样本量 (ESS)：

$N_{\mathrm{eff}}=\frac{1}{{\sum }_{i=1}^N{w}_i^2(k)}$（7.41）

（3.2）如果 $N_{eff}$ 小于预定阈值，则使用替换$N$ 个粒子 $\left[x_i(0:k),i=1,\dots ,N\right]$根据重要性权重设置 $\left({\hat{\mathbf{x}}}_l(0:k),i=1,\dots ,N\right)$ 。

（4）增加时间步并从步骤 2 开始重复该过程。

对于重采样步骤（步骤 3），可以应用文献 [19, 20] 中详述的方法。 在这些方法中，系统重采样是最简单和常用的方法，图 3 说明了在自举过滤器中执行的计算。

（图3 使用N = 10个粒子的粒子计算流程示意图）

7.3.4 PF 的变体

PF 的一些变体包括auxiliary PF（辅助粒子滤波器） 、Rao-Blackwellized 、分布式并行粒子滤波器和无迹 PF (UPF) 。

7.4多目标跟踪中的数据关联

在多目标跟踪环境下，观测期间会有新的目标出现和消失，数据关联是一个重要的课题。在汽车雷达跟踪中，由于存在障碍物(杂波)、虚警、模糊测量，以及在某些情况下丢失连续测量值，将测量值与目标相关联更具挑战性。

根据定义，数据关联的主要任务是将测量目标与现有目标或新目标相关联。多目标处理的一般流程如图4所示，利用初始轨迹或现有数据进行预测，从而确定预期的测量数据空间。

（图4 多目标处理一般流程）

当确定与现有目标的关联后，对现有目标轨迹进行更新。另一方面，当没有目标可以与测量相关联时，新的目标轨迹被创建。在这个过程中，不存在的目标必须从跟踪列表中删除。

例如，假设我们有两个目标轨迹和三个测量值，目标航迹与测量值之间的关系如图5所示。

（图5 目标航迹与测量值的关系）

为了实现上述数据关联的任务，文献中提出了各种各样的方法。其中包括最近邻、广义最近邻(GNN)、概率数据关联(PDA)、多重假设检验(MHT)、匹配算法等。这些内容的详细原理超出了本文的范围，但我们将给出一些可以考虑用于汽车雷达跟踪的主要算法的大致流程，作为一个雷达小白入门的阅读教材。

7.4.1 广义最近邻(GNN)

GNN使用距离度量，如马氏距离或欧几里得距离，将轨迹与测量值关联起来，因此存在如下的最小化问题：

$\mathrm{minimize}\sum d_{jk}^2{w}_{jk}$ subject to ${\sum }_j{w}_{jk}=1,{\sum }_k{w}_{jk}=1$（7.42）

其中 $d_{jk}^2$ 是轨迹$j$ 和测量 $k$ 之间的距离度量，并且 $w_{jk}$ 可以被认为是相关的权重。 GNN 方法的优点是实现起来非常简单， 但是当测量中存在歧义并且有时会导致局部最小值时，GNN 表现出较差的性能。

7.4.2 联合概率数据关联滤波器 (JPDAF)

PDAF 用于单个目标跟踪，其中对于每个有效测量，计算它来自所考虑轨道的概率${\beta }_i$。${\beta }_i$ 的计算包含检测和误报率。 使用计算的概率，可以更新目标数据。 JPDAF是从单目标到多目标跟踪的扩展。 在这种情况下，计算测量 $i$ 来自目标轨道 $j$ 的概率${\beta }_{ij}$ 。

7.4.3 多重假设检验

多重假设过滤器通过考虑三种可能的关联来生成假设树，其中测量值要么属于一个现有的轨迹，要么是由于虚警或新的。基于贝叶斯规则，可以评估每个假设的概率。最后，计算每种可能关联的可能性[3]。虽然它可以被认为是最通用的数据关联方法，但MHT方法的计算成本很高，并且难以实时实现。此外，只能计算近似的概率，增加了跟踪误差的可能性。

7.4.4 匹配算法

匹配算法起源于运筹学，目的是在成本方面优化任务分配到可用资源，最优的解决方案可以使总成本最小化，其中匈牙利匹配算法是解决分配问题的代表和知名算法之一。

匈牙利匹配算法最初由 Kuhn以及进一步改进的 James Munkres 开发成为 Munkres 算法 ， 解决相同问题的其他算法包括 Murty 算法 、 Jonker-Volgenant 算法 。

由于匈牙利匹配算法很好理解，并且很容易适用于数据关联中的目标分配问题，所以我们将简要描述该算法所涉及的步骤，并给出一个简单的例子。

基于 $N\times N$ 代价矩阵，匈牙利算法按照以下步骤进行 ：

(1) 行缩减：从其行的所有条目中减去每一行中最小的条目。
(2)列缩减：从其列的所有条目中减去每一列中最小的条目。
(3) 零分配：通过适当的行和列画线，以便覆盖代价矩阵的所有零项，并使用此类线的最小数量。
(4) 最优性检验：
(i) 如果覆盖线的最小数量是 N，那么最优的零分配是可能的，我们就完成了。
(ii) 如果覆盖线的最小数量小于 n，则零的最佳分配是不可能的。 在这种情况下，进行步骤 (5)。
(5) 确定未被任何行覆盖的最小条目。 从每个未覆盖的行中减去此条目，然后将其添加到每个覆盖的列。 返回步骤（3）。

假设，我们有三个有效测量值以及三个有效目标轨道，代价矩阵 $C$如下：

$C=\left[\begin{array}{lll}0.81& 0.91& 0.28\\ 0.90& 0.63& 0.55\\ 0.13& 0.09& 0.95\end{array}\right]$（7.43）

在代价矩阵 $C$ 中应用上述步骤，我们得到匹配矩阵 $M$：

$M=\left[\begin{array}{lll}0& 0& 1\\ 0& 1& 0\\ 1& 0& 0\end{array}\right]$（7.44）

其中1表示赋值，如果行表示目标轨迹，列表示测量值，那么测量值3被分配给目标1，测量值2被分配给目标2，测量值1被分配给目标3，则最小成本可以计算为: $0.13+0.63+0.28=1.04$ 。

同样的想法可以扩展到三个以上的目标和测量，这种看似简单而直接的方法产生了许多实际问题：

第一、观测或测量的数量并不总是等于轨道的数量，原因之一可能是漏检。
第二、存在对一个目标进行多重检测的可能性，以及其他因素等等。

当测量值大于目标轨迹或反之亦然时，首先将代价矩阵转换为方阵，然后进行匹配。当缺失数据或噪声测量得到，算法必须修改，例如，使用门控函数或阈值方法。

7.5 目标过滤和跟踪的困难

当所研究过程的动态和统计信息不准确时，粒子滤波器可提供一些最佳滤波结果。如本节介绍中所述，它可以在许多应用中实现更好的目标跟踪。这对于汽车雷达跟踪也是如此，在这种情况下，突然的机动可能会给 KF 方法带来严重挑战。

与 PF 的实际使用相关的挑战之一包括为重要性采样选择建议分布，一种有效算法是 UPF，它应用 UKF 来生成建议分布 。然而，对于大多数汽车目标跟踪应用来说，计算复杂度过高，而且还将 UKF 的负担带入了粒子滤波，从而使初始化和参数选择更加复杂。

更具实际意义的问题是数据关联，尽管有多种方法可供考虑，但在其他挑战中出现了准确性和计算负载问题。大多数情况下，算法为了对可用数据有用，必须调整参数。

虽然不是 PF 特有的，但目标状态更新和数据关联在滤波处理中占很大比例，因此有必要寻找创新方法。然而，对于行人检测，PF 最近被视为可行的方法之一 ，这可以归因于它跟踪非线性动力学的能力。

感谢阅读，如果本文对你有帮助，希望能够给我点一个赞！

参考文献

1. Kalman, R.E.: A new approach to linear filtering and prediction problems. J. Basic Eng.
   82(Series D), 35–45 (1960)
2. Gordon, N.J., Salmond, D.J., Smith, A.F.M.: Novel approach to nonlinear/non-Gaussian
   Bayesian state estimation. In: Radar and Signal Processing, IEE Proceedings F, vol. 140, issue
   2, pp. 107–113, Apr 1993
3. Bar-Shalom, Y., Li, X., Kirubarajan, T.: Estimation with Applications to Tracking and Navigation:
   Theory, Algorithms and Software. Wiley (2002)
4. Doucet, A., de Freitas, N., Gordon, N. (eds.): Sequential Monte Carlo Methods in Practice
   (2001)
5. Welch, G., Bishop, G.: An Introduction to the Kalman Filter. University of North Carolina at
   Chapel Hill, Chapel Hill, NC (2006)
6. Haykin, S.: Adaptive Filter Theory, 3rd edn. Prentice-Hall, Inc., Upper Saddle River, NJ, USA
   (1996)
7. Julier, S.J., Uhlmann, J.K.: Unscented filtering and nonlinear estimation. Proc. IEEE 92(3),
   401–422 (2004)
8. Julier, S.J., Uhlmann, J.K., Durrant-Whyte, H.: A new approach for filtering nonlinear systems.
   In: Proceedings of the American Control Conference, pp. 1628–1632 (1995)
9. Julier, S.J., Uhlmann, J.K.: A general method for approximating nonlinear transformations of
   probability distributions. Technical Report, RRG, Department of Engineering Science, University
   of Oxford, Nov 1996
10. Julier, S.J., Uhlmann, J.K.: A new extension of the Kalman filter to nonlinear systems. In:
    Proceedings of AeroSense: The 11th International Symposium onAerospace/Defence Sensing,
    Simulation and Controls (1997)
11. Julier, S.J., Uhlmann, J.K., Durrant-Whyte, H.: A new method for the nonlinear transformation
    of means and covariances in filters and estimators. IEEE Trans. Autom. Control 45(3), 477–482
    (2000)
12. Wan, E.A., van der Merwe, R.: The Unscented Kalman Filter. In: Haykin, S. (ed.) Kalman
    Filtering and Neural Networks. Wiley (2001)
13. LaViola, J.J.: A comparison of unscented and extended Kalman filtering for estimating quaternion
    motion. In: Proceedings on American Control Conference, vol. 3, pp. 2435–2440 (2003)
14. Andrieu, C., Doucet, A., Holenstein, R.: Particle Markov chain Monte Carlo methods. J. R.
    Stat. Soc. Ser. B 72(Part 3), 269–342 (2010)
15. Doucet, A., Johansen, A.M.: A tutorial on particle filtering and smoothing: fifteen years later.
    In: Crisan, D., Rozovsky, B. (eds.) Handbook of Nonlinear Filtering. Cambridge University
    Press, Cambridge (2009)
16. Gustafsson, Fredrik: Particle filter theory and practice with positioning applications. IEEE
    Aerosp. Electron. Syst. Mag. 25(7), 53–82 (2010)
17. van der Merwe, R., Doucet, A., de Freitas, J.F.G., Wan, E.: The unscented particle filter.
    Technical Report CUED/F-INFENG/TR 380, Cambridge University Engineering Department
    (2000)
18. Gamba, J., Yamano, C.: Target tracking using parallel partition particle filtering. In: Society
    Conference of IEICE, Hokkaido University (Sapporo), Japan, 20–23 Sept 2005
19. Hol, J.D., Schon, T.B., Gustafsson, F.: On resampling algorithms for particle filters. In: 2006
    IEEE Nonlinear Statistical Signal Processing Workshop, 13–15 Sept 2006
20. Li, T., Bolic,M., Djuric, P.M.: Resampling methods for particle filtering: classification, implementation,
    and strategies. IEEE Signal Process. Mag. 32(3), 70–86 (2015)
21. Särkkä, S., Vehtari, A., Lampinen, J.: Rao-Blackwellized particle filter for multiple target
    tracking. Inf. Fusion 8(1), 2–15 (2007)
22. Kirubarajan, T., Bar-Shalom, Y.: Probabilistic data association techniques for target tracking
    in clutter. Proc. IEEE 92(3), 536–557 (2004)
23. Kuhn, H.W.: The Hungarian method for the assignment problem. Naval Res. Logistics Q.
    2(1–2), 83–97 (1955)
24. Munkres, J.: Algorithms for assignment and transportation problems. J. Soc. Ind. Appl. Math.
    5(1), 32–38 (1957)
25. Murty, K.G.: An algorithm for ranking all the assignments in order of increasing cost. Oper.
    Res. 16(3), 682–687 (1968)
26. Jonker, R., Volgenant, T.: A shortest augmenting path algorithm for dense and sparse linear
    assignment problems. Computing 38(11), 325–340 (1987)
27. Poore, A.B., Gadaleta, S.: Some assignment problems arising from multiple target tracking.
    Math. Comput. Model. 43, 1074–1091 (2006)
28. The Assignment Problem and the Hungarian Method. http://www.math.harvard.edu/archive/
    20_spring_05/handouts/assignment_overheads.pdf
29. Heuer, M., Al-Hamadi, A., Rain, A., Meinecke, M., Rohling, H.: Pedestrian tracking with
    occlusion using a 24 GHz automotive radar. In: 2014 15th International Radar Symposium
    (IRS), 16–18 June 2014
30. Gaddigoudar, P.K., Balihalli, T.R., Ijantkar, S.S., Iyer, N.C., Maralappanavar, S.: Pedestrian
    detection and tracking using particle filtering. In: 2017 InternationalConference on Computing,
    Communication and Automation (ICCCA), pp. 110–115 (2017)