2.3 滤波方法

在概述章节，我们按照时间点把估计问题分为滤波、外推和平滑三类，在离散时间系统（如目标跟踪系统）中，可以更清晰地表示出来。

设已知 j 和 j 为以前时刻的量测值， 为对 k 时刻状态 做出的某种估计，则有

（1）当 时，称为滤波问题，称 为 的最优滤波估计量；

（2）当 时，称为预测问题，称 为 的最优预测估计量；

（3）当 时，称为平滑问题，称 为 的最优平滑估计量。

可见，滤波和预测是目标跟踪问题的核心环节。所谓滤波问题，就是要在测量噪声存在的环境下，通过测量结果估计目标当前的状态向量，通常是利用上周期外推所得的本周期目标的可能位置，结合本周期实时测量到的目标坐标，按一定的算法实现估计的。这种算法即为滤波算法，为了保障实时性和快速性，通常采用递推算法形式。预测也叫外推，它是根据已有的目标运动数据预测下一周期目标可能状态的算法。显然，实际上由于各种不确定性未知因素，实现理想的外推并非易事。

为了解决目标跟踪中的滤波外推问题，下面详细介绍离散时间系统的滤波方法，包括线性递推最小二乘滤波、线性卡尔曼滤波、常增益滤波（ 和 滤波）、非线性滤波等。这些方法是目标跟踪中的基本方法，是设计其他滤波方法的基础。

2.3.1 线性系统的概念

在之前所讨论的统计估计问题的模型，可以认为是一种“静态”的模型。即通过量测 所估计的随机向量 是不依赖于时间 的。但是在很多实际问题中，被估计的 ，一方面仍是随机向量，另一方面它随时间不断变化，例如一个飞行体的状态（位置、速度等），就是随时间 变化的随机向量 ，或者说，是一个随机向量过程。我们把 称为动态系统在 时刻的状态。如果我们感兴趣的只是在某些离散时刻（抽样时刻） 的状态，量测也只是在这些时刻进行，那么状态和量测就分别构成两个随机序列 和 。所谓离散时间系统的滤波，就是要求基于量测序列，对状态序列做出尽可能好的估计。

一个动态系统的状态变化，是遵循一定的物理规律的，因而不同时刻对状态的量测也是互相联系的。这些规律或联系，在对状态估计时，显然应当充分加以利用。状态随时间变化的规律，对于连续线性系统，通常是用一个具有随机初始状态的向量微分方程来描述，称为动态方程（也称系统方程、状态方程）。进一步还可以假定，这个方程受到某种随机扰动的影响（如大气湍流对飞机飞行轨迹的干扰等），这种扰动统称为动态噪声（也称系统噪声、状态噪声）。量测变量与状态变量之间，可一般地假定有某种函数依赖关系，同时还存在着随机量测误差，这种误差称为量测噪声。于是，如果用 ， 分别表示 维状态变量和 维量测变量，则动态方程和量测方程一般可表示成如下的形式：

式中： 和 分别为已知的 维和 向量函数； 和 分别为 维随机动态噪声和 维随机量测噪声，而初始状态 是一个有确定概率分布的 维随机向量。特别地，如果式（2.3.1）中的两个方程对 、 和 都是线性的，即有

式中： 、 和 分别为已知的、随时间 连续变化的 、 和 矩阵，把式（2.3.2）所描述的动态系统和量测系统称为线性系统。有不少的物理过程可以用线性方程来近似地描述，所以它是简单而常用的系统。

对于离散时间系统，随时间变化的状态满足的动态方程，一般可表示成随机初始状态，并带有随机扰动的差分方程（递推方程），通常可由连续时间系统的动态微分方程经过离散化得到。我们特别讨论一下线性系统（2.3.2）的离散化问题。为了把式（2.3.2）中的动态方程离散化，要用到线性常微分方程的一般理论。设 是下列 阶方阵微分方程的解：

由常微分方程解的存在唯一性定理知道，这样的 是存在而且唯一的，它具有下面的性质：

矩阵 称为状态转移矩阵。容易验证，线性系统（2.3.2）在 时刻的状态 ，可用状态转移矩阵 与初始状态 ，表示如下：

现在考虑在离散抽样时刻 诸状态 之间的变化规律。由式（2.3.7a）可得

令

由此，即得离散时间的线性动态方程

这是一个以 为初始状态的线性差分方程（递推方程），其中， 是 阶可逆阵，称为第 时刻到第 时刻的状态（一步）转移阵； 是 维随机序列，称为动态噪声。同样，令 ， ， ，则由式（2.3.2）中连续时间的线性量测方程，可直接导出离散时间的线性量测方程

式中： 是 矩阵，称为第 时刻的量测阵； 是 维随机序列，称为量测噪声。式（2.3.10）和式（2.3.11）一起，就描述了离散时间的线性系统。

下面举例说明：卫星在空间飞行的过程中，由于受到大气阻力的影响，要产生阻力加速度，而阻力加速度的主要影响是改变卫星沿轨道切线方向的运动，使卫星产生轨道偏差，不能按照预定轨道（标称轨道）飞行。为了对卫星的实际飞行轨道进行修正，需要通过设在地球各地的卫星观测站得到卫星瞬时位置的量测值，然后经过适当的数学处理，尽可能精确地求出和预测卫星飞行轨道的偏差。我们考虑这个问题的简化情况。设在 时刻大气阻力引起的卫星沿轨道切线方向的角位置偏差为 ，角速度偏差为 ，随机阻力角加速度为 ，则易知它们之间的相互关系，可以用如下的微分方程组来描述：

令 , ， ，则上面的方程组可表示成

这是一个连续时间系统的线性动态方程，容易求出它的状态转移矩阵 为

于是对于离散抽样时刻 ，动态方程为

其中， ， ， ，而动态噪声 的统计性质由大气扰动的统计规律所确定。如果量测量是角位置偏差，则测量方程为

式中： ， ，而量测噪声 的统计特性由量测方式的误差统计规律所确定。

在解决一般的滤波问题时，我们总是假定初始状态 ，动态噪声 和量测噪声 的统计性质（它们的联合概率分布或其一阶、二阶矩）是已知的。考虑到实际情况，我们常常将线性动态方程（2.3.10）一般化为如下形式

式中： 为非随机的 维向量序列（通常看作动态系统的系统输入项或控制项等）； 为均值为零的 维随机动态噪声； 和 分别为 和 矩阵系数序列。关于 的统计特性，最简单和最基本的类型是假定它为零均值的正态白噪声或白噪声序列，即

如果我们进一步令

则从式（2.3.18）出发，对于 ，容易推出 与 之间的如下关系：

两边左乘 再移项，可得

特别是在式（2.3.21）中，取 时，则有

这是 由初始状态、系统输入和动态噪声表达的非递推关系式，也就是动态方程（2.3.18）的解。

类似地，线性量测方程（2.3.17）也可一般化为

式中： 为非随机的 维向量序列（通常看作量测的系统误差项等）； 为均值为零的 维随机量测噪声。同样地，关于 的统计特性，最简单和最基本的类型是假定它为零均值的正态白噪声或白噪声序列，即

如果式（2.3.18）中的 ， ， ，以及 为平稳随机序列（在白噪声情形下等价于 ），则式（2.3.18）所描述的线性动态系统称为定常的；同样，如果式（2.3.24）中的 ，以及 为平稳随机序列（在白噪声情形下等价于 ），则式（2.3.24）所描述的线性量测系统称为定常的，统称为定常线性系统。

2.3.2 状态方程和观测方程

1．状态方程

一个系统或过程的特性，可以用一些参量表示出来。这样的参量中的一组最小的集合，称为状态变量。例如，一个目标的运动状态，可以用其坐标、速度、加速度等表示出来。在 X 方向的坐标、速度、加速度分别用 、 、 表示时，则

就是一组状态变量，也称为状态变量。

用来表示一个动态系统状态变化规律的方程，一般可用时间 的一阶向量微分方程来描述。目标的各种典型运动变化规律就是通过采用坐标 、速度 、加速度 等参量表示的动态方程来描述的。

以目标做等加速直线运动为例，目标坐标 的变化规律可用二次项多项式表示，即

目标速度、加速度的变化规律分别为

初始状态 时

于是，可以用 时刻的状态值，即

式中： 。

同理，可以导出由 时刻的状态计算 时刻状态值的一般式。为了简明起见，以后采用以下符号表示：

用矩阵表示一般状态方程，则为

或

式中： ，称为状态转移矩阵，转移矩阵代表目标的运动规律，不同的目标运动将有不同的转移矩阵。

在真实的目标运动中，任何一个运动系统难免受到某种随机干扰，或者由于方程描述物理现实不够全面、不够准确，而使状态并不完全按照方程变化。这时，就需要对状态方程进行适当修正。通常的处理方法是在上述的状态方程中，引入系统噪声 来进行补偿。即系统的状态方程写成：

式中： 表示时刻 的系统状态值，都是 维向量； 是 的系统状态转移矩阵； 表示 的系统噪声系数矩阵； 表示 维向量，表示时刻 的系统噪声。

系统噪声 包括了外界干扰，如气流不稳对飞机的影响、目标的有意机动以及描述方程的不完善等。

最后，对状态方程的物理意义简述如下：状态方程也称为动态方程或系统模型，它是线性离散系统的状态差分方程，它描述了物理系统本身的运动规律。对于一个动态系统，根据它的状态方程就可以导出它的未来状态。实际系统模型不可能与真实的完全一致，为了表示这种欠缺，增加系统噪声项。系统噪声项中一般包括目标的有意机动或无意机动、环境噪声（气流、风、浪等干扰）等。此外，也包括系统方程在描述物理真实机理方面存在的其他欠缺。这样，一般的系统方程就由确定性部分和随机部分组成。有了系统方程，如在 时刻，根据已知状态 和系统噪声 ，则 时刻的系统状态 就可以表示为 和 线性组合。

2．测量方程

测量方程用来表示测量值与状态值之间的关系。

测量方程也称为测量模型。对于一个动态系统，它的各个状态不一定都观测到，如目标位置坐标可以直接观测到，而目标速度、加速度常常不能直接测量出来。可以观测到的量，一般是含有噪声的，而且不一定就是系统状态，常是状态变量的线性组合，即 。比如测量到的球坐标斜距离 、高低角 和方位角 ，而状态采用直角坐标 、 和 ，它们之间必须通过转换。

测量方程的矩阵表示如下：

式中： 表示 时刻测量的 维向量； 表示 转换矩阵； 表示 时刻测量的 维测量误差向量。

例如，目标做匀速直线运动，而观测仪器只能观测目标的坐标 ，在没有测量误差的情况下，有

这是因为此时观测值是 ，而速度和加速度都没有观测到。

2.3.3 两点外推滤波

两点外推也称为线性外推，是一种最简单的滤波和外推方法。它的基本思想是：将当前提取到的点迹作为目标当前坐标，利用当前及前一时刻目标的两个点迹数据，确定目标的速度，并依此外推下一点，进行相关处理。

两点外推的滤波方程为：

以上三个式子分别为位置滤波、速度滤波和外推公式，其中：

—— k 时刻位置的测量值；

—— k 时刻的位置滤波值；

—— k 时刻速度滤波值；

—— k 时刻外推至 k +1时刻状态向量的外推值。

对于匀速直线运动的目标，由于 ， ，故上式可改写为如下形式：

以上过程可以具体描述如下：假定目标做匀速直线运动，已知第一点坐标 和第二点坐标 ，要求外推第三点，如图2.1所示。

目标运动方程可写为：

令 k =2，即 ，则：

以上两式结合后可得到一般的外推公式：

令 k =3，可得外推第三点的外推公式：

以上两式就是由第1、2点数据求出任一点和第3点的外推公式。因为都是只根据两点数据向前外推另一点的数据，故称为“两点外推”。

当 在测量中存在误差，且各自的均方根误差各为 ，由式 求 对 的偏导，可得因 导致外推 的误差分别为：

外推总误差的均方值为：

如 ，则：

可见，一步外推所带来的外推均方误差将增加为原误差的5倍。

两点外推中坐标滤波只与当前点迹有关，速度滤波及外推则仅由 k -1及 k 时刻提取的点迹坐标决定，与 k -1以前的点迹毫无关系。因此，这种方法实际上没有记忆作用，其精度只与当前及前一时刻点迹数据的精度有关，并且对机动和非机动目标均能产生同样好或同样坏的估计效果。这是一种精度最低的滤波方法，但这种方法对状态噪声和测量噪声的统计特性毫无要求，计算极为简单。

2.3.4 线性递推最小二乘滤波

2.2.6节介绍的线性最小二乘估计要根据测量数据的累加值来计算，因而又被称为累加格式最小二乘法。这种估计方法在计算过程中需要存储每一次的观测数据，随着 k 的增大，对计算装置的存储量要求提高，计算的实时性逐渐变差。为了避免这一缺点，产生了递推格式的最小二乘滤波法。为了理解线性递推滤波的概念，有必要先介绍一个简单的线性递推滤波器。

1．线性递推滤波

设有一常数标量 ，我们根据 次观测所得的测量值 来估计这一未知标量。根据最小二乘估计原理，首先建立极小化目标函数，即 次误差的平方和

能使 取极小的估计值 就是未知量 的最小二乘估计，经计算

即常数变量的最小二乘估计是测量值的算术平均。

当 时刻得到一个新的测量值 时，应得到新的估计

式（2.3.48）和式（2.3.49）都是累加格式的最小二乘滤波。通过数学变换，就可以变成递推格式的最小二乘滤波。

式（2.3.49）可化为用先前估值 和新测量值 表示的形式，即

上式表明，新的最小二乘估计，等于前一时刻估值与测量值的线性组合。

对比式（2.3.49）和式（2.3.50）可以看出，采用后式计算 就不需要存储过去的测量值，只用一个新测量值和前一时刻的估计值线性组合就够了。这是因为所有以前的信息都包含在前次估值中了。式（2.3.50）所表示的就是一种线性递推滤波器。

式（2.3.50）还可以改写成另一种递推形式：

以后遇到的递推滤波器都是这种格式，因此，了解此式各项的意义很重要。

（1）新息。上式右端括弧项表示新的测量值 与根据以前各次测量值而定的估值 之差，亦称为残差或误差。这里面包含了新的信息，所以也称为新息。

（2）增益。 是一个加权系数，也称为增益。它的大小表示我们对新息的重视程度。原来的估值 在有新的测量值 后应做一定的校正，那么校正多少才合适呢？这里的加权是按最小二乘的原则确定的。当 增大时，加权变小，即新的测量值对新的估值影响变小，这是合乎情理的。

以上就是递推格式最小二乘滤波，只不过是最简单的，也即 是固定不变的标量的情况。然而，在大多数的应用场合中，未知估计量都是向量，有必要介绍向量形式的递推最小二乘滤波公式。

2．算法公式推导

累加格式的最小二乘估计的缺点是要求每一时刻都要计算 矩阵 的逆矩阵，引起较大计算负担。从计算实时应用角度来看，要求估计值 的递推算法避免求逆矩阵。下面应用矩阵求逆定理导出线性递推最小二乘算法。

设 次观测后，基于观测序列 的线性最小二乘估计为：

定义 矩阵 为：

同理，在 n +1次观测后，基于观测序列 的线性最小二乘估计为：

由于存在关系 ， ，所以

则有

根据矩阵求逆公式：

取 ， ， ，有

令

则

因此，根据下式

可以得到 的递推计算式

线性最小二乘估计协方差为

假设每次观测噪声的协方差均相等，记为 ，有 ， 表示对角矩阵。

所以

综上各式，递推最小二乘滤波算法为：

当先验统计特性一无所知时，一般采用最小二乘滤波。如果仅掌握测量误差的统计特性，可以采用最优加权阵为观测误差协方差矩阵的逆（ ）的最小二乘估计（也称马尔可夫估计）。而在目前的雷达等跟踪系统中，探测跟踪设备的探测统计特性一般是可以事先统计得到的，因此，在跟踪系统中采用最优加权的最小二乘滤波方法是非常普遍的。递推形式的最优加权最小二乘估计的计算流程如图2.2所示。

从上述推导过程可以看出：

（1）最小二乘滤波算法适用于参数估计，即估计量是固定的，不随时间变化的量。

（2）最小二乘滤波算法直接利用量测信息来估计未知参数，而没有利用该参数的历史先验信息。

实际上，有一类系统其未知参数是随时间发生变化的，称为状态，状态的变化刻画了其历史的运动规律或变化规律，典型的例子是对运动目标的跟踪系统。如何对此类系统的运动状态进行估计呢？下面学习该类估计方法——线性卡尔曼滤波算法。

2.3.5 线性卡尔曼滤波

卡尔曼滤波算法是一种以无偏最小方差为最优准则，并采用递推算法的线性滤波方法。目前已广泛应用于各种跟踪测量系统、导航系统、航天以及工业控制系统中。卡尔曼滤波技术在对机动目标的跟踪中优点尤为突出。由于卡尔曼滤波对计算工具的要求较高，其应用曾受到一定的限制。不过，随着计算机技术的飞速发展，卡尔曼滤波已在实际应用中更加受到人们的青睐。

1．算法设计思想

卡尔曼滤波器的设计思想有下列特点：

1）最优估计准则——无偏最小方差估计

如图2.3所示，图中实线表示目标的实际航迹，虚线表示目标航迹的估计值。图2.3（a）估计航迹较长时间偏于实际航迹的一侧，称为有偏；图2.3（b）估计航迹虽未较长时间偏于实际航迹的一侧，但在两侧摆动剧烈，此种摆动以“方差”表示，意即此种估计虽然是无偏的，但方差很大；图2.3（c）所示的是无偏和方差最小，称为无偏最小方差估计。卡尔曼滤波就是采用这种最优估计准则。这种准则可表示为：

无偏：

最小方差：

2）线性递推滤波

卡尔曼滤波采用线性递推滤波，即当第 个采样测量值 获得后，把它和前一个采样的估值 作某种线性组合，得出第 k +1个采样的滤波估值 ，即：

式中： 是 时刻状态矢量的估值； 是 时刻接收到的测量矢量； 是已知的 时刻输入量。

3）考虑状态方程及观测方程

与最小二乘滤波只考虑观测方程不同，线性卡尔曼滤波还考虑了系统状态方程的影响。如我们之前所述，状态方程描述了系统内部状态的变化规律。因此，卡尔曼滤波的值不仅依赖于观测数据，还和被估计量的变化规律有关，显然，如果这一规律描述是准确的，则估计值会比仅依赖观测数据所得到的估计值更可靠。同时，将状态方程纳入滤波算法，使得卡尔曼滤波算法能够实现变化状态估计。

2．前提条件

1）状态噪声假定

在实际系统中，系统噪声 是随机过程，一般假设服从均值为零、方差为 的白噪声随机过程。其统计特性如下：

式中： 表示 系统噪声方差阵。

2）观测噪声假定

一般地，在卡尔曼滤波中，假定观测噪声是以零均值、方差为 的白噪声，其统计特性为：

式中：量测误差方差矩阵 已知。

3）初始状态假定

状态噪声序列 、量测噪声序列 和初始状态 互不相关，即对任意 和 有

3．离散卡尔曼公式系的推导

1）计算状态估值

离散的线性递推滤波的原始方程为：

系统方程：

观测方程：

这两个方程，一个描述被估计量本身的物理特性；另一个表示观测系统与被估计量间的关系。现在，假设我们已知 时刻的状态估值为 。根据状态方程，我们可以在没有传来新的测量值时，先把 外推一步，即一步预测，得 。当传来新的测量值时，再利用这一新息把原来只靠状态方程本身外推得出的 修正一下，成为 ，这就是卡尔曼滤波的简要过程。这里定义以下符号：

表示验前估计值，即预测值，它是在 时刻，只根据系统状态转移矩阵外推得到的，没有经过测量检验的估计值；

表示验后估计值，即滤波值，它是在 基础上，用新测量值检验后作适当修正后的估计值；

表示 时刻的状态真值，我们不能直接得到，只能依靠估计得到。

设我们已知 步，即 时刻的状态估计值 ，在线性递推滤波器中，我们可以设递推滤波的一般形式是

式中： 、 是待定的时变加权矩阵。

将式（2.3.76）代入此式，则有

令

代入式（2.3.77），得

或

两边取数学期望，因 ，则有

设在 时刻的状态预测估计值是无偏估计，即 ；如果同样要求在 时刻的状态估计值是无偏估计，则只有当 时， 。

因此，我们令 （这是无偏估计的必要条件），把 代入式（2.3.77）则得无偏估计

或

这就是线性递推滤波计算式，它是从无偏估计条件得出的，也是量测的线性组合。当然，还有 需要确定。

2）计算误差协方差

（1）验前误差协方差。

验前误差协方差是在不参考新测量值的情况下，只依靠系统方程作外推时的滤波误差协方差。

设

只根据系统的状态转移矩阵，可外推出状态的新值来，即

上边的后式减前式，得这次外推的误差

则

那么，这次外推的误差协方差则为

利用 与 的不相关性，则上式交叉项的乘积的数学期望等于零。则有

式中： 为 时刻的验后误差协方差； 为 时刻的系统噪声协方差。

式（2.3.91）即计算验前滤波误差协方差的公式，也称为误差传播公式或误差外推公式。它根据已知的 ，外推出 来；但这里只是根据动态方程外推的，并没有参考新的测量值做适当修正。式（2.3.91）中 是状态方程的随机误差方差，它不可能是负的。因此可以说，由于存在 的不确定性，误差协方差在外推中要增大。

（2）验后误差协方差。

验后误差为

验后误差协方差为

这就是有新的测量后的误差协方差，它是对 的修正。但是，这里的 不一定是最优的。注意，公式是对称的。

3）增益的最优选择

在以上的计算中用到了 ，它只是一个待定的加权矩阵，现在讨论 的最优选择。首先选定性能指标函数

只要选择适当的 ，使 为最小，则估计误差的方差也就最小的了。

把式（2.3.93）右端展开

这里用到了对矩阵的迹的微分公式：

令式（2.3.95）等于零，得

这就是卡尔曼滤波中的增益公式。选用这个增益就会使滤波误差协方差最小，在这个意义上，滤波是最优的。

用 的公式还可以把前面的 表达式简化为

把 的计算式（2.3.101）代入，为此先把式（2.3.101）变形

或

把此式代入 的公式中，则有

这是当 是最优增益时的滤波误差协方差。注意公式是不对称的。

式（2.3.105）还可利用 式改写成

根据矩阵求逆公式，有

从此式可以看出， 是非奇异的，而且 要比 小，也就是说，经过修正后的误差协方差是减少了。当测量较准确时， 小，即 大， 减少得多些。

在卡尔曼滤波中用到的误差协方差公式是式（2.3.106）和式（2.3.107）。其中前者虽然比后者长些，但它不限定 是最优的，而且它的计算式具有对称性，因而在反复的计算过程中，可保持误差协方差矩阵的对称性和正定性。

如果把式（2.3.105）代入式（2.3.104），可得 的另一表达式

这样 也有两个表达式。表面上看来后者要比前者简单，但是求 的式（2.3.109）需要知道 ，而求 要知道 和 ，因此式（2.3.109）只做分析用。式（2.3.109）说明，增益矩阵是由滤波误差协方差 和测量误差方差 之比组成的，而矩阵 不过是从状态到测量的转换。这点正是增益的物理意义。如果 比 大很多，则 大，多考虑新息在估计中的分量；反之当滤波误差较小，而测量误差较大时，则 小，即对滤波的校正量取小些。

以上计算了状态估值、误差协方差，这两个式子都分验前和验后，再加上增益公式，共有5个公式。这5个公式就是卡尔曼滤波的公式系。

4．离散线性卡尔曼滤波公式系

1）公式系及计算程序

把上述导出的离散卡尔曼滤波公式系列在一起，以供参考。

验前状态估计（预测估计）：

最优增益：

验后状态估计（滤波估计）：

已知条件：动态系统噪声及观测噪声的统计特性： 和 。

初值： 即 ，取 ， 取任意假定的非零值即可， 为任意常数。

2）公式说明

（1） 是半正定矩阵，意味着不是所有状态都受到干扰，这在物理上是合理的假定。

（2） 是正定矩阵，表示观测向量的每一元素都有不确定性，这是合乎物理实际的。

（3）动态系统必须是线性的，观测方程必须是状态的线性组合，干扰（噪声）必须是附加性噪声，而且是正态的随机变量。

实际上，这些要求可能不完全满足。这时，卡尔曼滤波公式仍可使用，只不过不是“最优”，而是“次优”。

（4） 称为测量前的误差协方差矩阵。 称为测量后的误差协方差矩阵。从公式可以看出

式中：右端第二项是非负矩阵，说明 是绝不会大于 的。因此，可以认为，平均来看，测量将减少对估值了解的不确定性。

（5）在卡尔曼滤波中，协方差的计算量很大，其目的主要是为了求得增益 ，然后利用 来求得滤波 。从图2.4中可以看出，从状态到协方差计算没有反馈，即协方差的计算循环是独立的。

（6）增益是按照这样的准则确定的：协方差 的对角线元素的加权和为最小，实际要求每个元素都必须是最小。这就是对每一个状态的滤波误差方差都是最小。

增益中考虑了状态的不确定性和测量的不确定性，一般它是变化的。

如果增益用一个简单的常数代替，则不需要计算协方差了。这时卡尔曼滤波公式只剩①和②了，计算量大减。当然这时已经不是“最优”的了。

（7）若 、 、 、 、 等都是与时间无关的常数矩阵，则称为定常系统。在一定的条件下，当 时，误差协方差矩阵 、增益 均趋于一常数矩阵。这类估计称为定常系统的稳态卡尔曼滤波。

3）计算顺序

参看图2.4。首先把 和 的初值送入，然后即可求出 和 ，计算 。有了测量 后，进一步求出状态滤波 ，同时对协方差修正，得 ，存储 直到第二次测量，重复以上计算。注意协方差的计算循环有独立性，它不依赖于状态估值（见图2.5、图2.6）。

5．线性卡尔曼滤波的特点与性质

1）卡尔曼滤波的特点

（1）由于它将被估计的信号看作在白噪声作用下的一个线性系统的输出，并且其输入/输出关系是由状态方程和观测方程在时域内给出的。因此，这种方法不仅适用于单输入/单输出的平稳序列的滤波，而且特别是适用于MIMO（Multiple Input Multiple Output）非平稳或平稳马尔可夫序列或高斯马尔可夫序列的滤波，因此，其应用范围十分广泛。

（2）由于其计算过程是一个不断地“预测-修正”的过程，在求解中不需要存储大量数据，并且一旦观测到了新的数据，随时就可以计算新的滤波值。因此，这种方法便于实时处理。

（3）由于最优滤波增益 与观测值 是无关的，可以离线计算，从而减少实时在线计算量，并且在求解过程中，可以随时计算出滤波器的精度指标 ，其对角线上的元素就是滤波误差向量各分量的方差。

2）卡尔曼滤波器具有的性质

性质2-3： 卡尔曼滤波误差协方差阵 与最优增益矩阵 具有另外几种形式：

式（2.3.110）具有较好的保持对称性和非负定性的能力。

性质2-4： 对于一个马尔可夫序列模型，卡尔曼滤波估计 是基于观测数据 上的线性最小方差估计，误差协方差阵 是所有线性估计中的最小方差阵；而对于高斯-马尔可夫序列模型，卡尔曼滤波估计 是基于观测数据 上的最小方差无偏估计，误差协方差阵 是所有估计中的最小方差阵。

性质2-5： 最优滤波误差序列 是一个均值为零的马尔可夫序列或高斯-马尔可夫序列。

证明：

又

所以：

令

则

令

则有：

从假设条件知， 是零均值的白噪声序列，或高斯-白噪声序列，其协方差为：

并且 ， 。

因此， 是具有零均值方差为 ，且同 具有相同概率分布形式的随机变量。此外， 与 互不相关，因此， 是一个马尔可夫序列或高斯-马尔可夫序列。

性质2-6： 增益矩阵 与初始方差阵 、系统噪声方差阵 、观测噪声方差阵 存在如下关系：

增益矩阵 与 成正比关系，而与 成反比关系。

现在来进一步了解卡尔曼滤波方程的意义。先研究滤波方程，它说明了怎样求得滤波估计 （它是预测值和新息量的线性组合）。卡尔曼滤波增益是这两项的相对加权，增益 可以写成：

式中： 是新息量序列 的协方差。既然 仅仅是一个从状态空间映射到观测空间的运算符，所以 可以看作两个协方差矩阵 和 之比。前一个矩阵是衡量预测的不确定性的，后一个矩阵则是衡量新息量序列 的不确定性。当 很大，即 “大”于 时，置信度放在观测值 上，而 依赖于 的程度很小。相反， “小”于 时，可以预料到观测中会有很大误差，而 取决于外推估计 。

上面引入的新息过程具有某些适宜的特点。它们将用在自适应卡尔曼滤波中。新息过程是一个协方差矩阵为 的零均值的“白色”过程。当 和 、 都是高斯分布时，新息过程也是高斯分布的。

现在对协方差方程作进一步说明。下式成立：

因此，在能得到可靠观测的场合（ 很小）， 值会显著增加，也就是说， 的不确定性得到明显减小。最后，若 ，则新息协方差 可看作 。因此，由于从观测数据中不能得到更多的信息， 几乎等于 ，同时认为预测值足够准确。

总之，卡尔曼滤波器提供了过程 的最小方差估计，如果过程本身和观测误差都是高斯分布的，那么也达到了极大似然估计意义上的最佳估计。在这种情况下，卡尔曼滤波器得到的是一个有效无偏估计，其协方差达到克拉美-罗下限。

6．卡尔曼滤波需要注意的问题

1）卡尔曼滤波的收敛性

卡尔曼滤波结果的好坏与过程噪声和量测噪声的统计特性（零均值和协方差）、状态初始条件等因素有关。实际上这些量都是未知的，在滤波时对它们进行了假设。如果假设的模型和真实模型比较相符，则滤波结果就会和真实值很相近，而且随着滤波时间的增长，二者之间的差值会越来越小。但如果假设的模型和真实模型不相符，则会出现滤波发散现象。滤波发散是指滤波器实际的均方误差比估计值大很多，并且其差值随着时间的增加而无限增长。

滤波结果的发散情况分为两类：显式发散（Apparent Divergence）和真发散（True Divergence）。显式发散情况下，状态参数的真实误差有界；在真发散情况下，虽然滤波器的估计方差趋于零或某一稳态值，但滤波状态参数的真实误差却越来越大。图2.7为两种发散的表示。一般在算法上提到的发散都是第二种发散。一旦出现发散现象，滤波就失去了意义。因此，在实际应用中，应克服这种现象。

引起发散的主要原因包括：

（1）系统过程噪声和量测噪声参数的选取与实际物理过程不符，特别是过程噪声的影响较大。

（2）系统的初始状态与初始协方差的假设值偏差过大。

（3）不适当的线性化处理或降维处理。

（4）计算误差。这是由于计算机的有限字长引起的，计算机的舍入、截断等计算误差会使预测方差矩阵 或更新方差阵 失去正定性，造成计算值与理论值之差越来越大，从而产生滤波数值不稳定问题。滤波运算中其他部分的误差累计，也会严重地影响滤波精度。在计算机字长较短的系统中，采用双倍字长可以减少运算误差，但是会使计算量成倍增加，大大降低滤波的实时能力。

克服前三种滤波发散的方法主要有限定下界滤波、衰减记忆滤波、限定记忆滤波和自适应滤波等，这些方法都是充分利用系统新的测量值对估计值进行修正；也是以牺牲滤波最佳性为代价而换取滤波收敛性的。而克服滤波数值不稳定性的主要方法有协方差平方根滤波与平滑、信息平方根滤波与平滑、序列平方根滤波与平滑等。

在一定条件下滤波模型的不精确引起的误差是允许的，这种误差随着时间的推移能够逐渐消失。滤波器是否具有这些特性，可以通过对滤波模型进行灵敏度分析来验证。如果模型误差超出了允许的范围，或者要求较高的滤波精度，就需要对滤波模型进行修改。这时可采用模式辨识和自适应滤波。可以说，将卡尔曼滤波应用于实际问题时，主要的工作就是建立滤波数学模型和寻求适用的自适应滤波算法。

2）卡尔曼滤波的收敛速度

卡尔曼滤波应用应注意的另一个问题是滤波的实时能力，即收敛速度。卡尔曼滤波算法中都很注意滤波的收敛速度问题，滤波收敛快慢直接影响到目标跟踪的稳定度和对目标的锁定速度。因此，滤波的收敛速度是评价一个滤波器性能的重要指标。

尽管卡尔曼滤波具有递推形式，为实时处理提供了有利条件，但它的运算量还是比较大的。为了实时跟踪，滤波运算就需要采用高性能计算机，这往往会使应用卡尔曼滤波失去实用价值。从另一个方面讲，提高卡尔曼滤波的实时能力，可减轻计算机的负担，提高计算效率，降低对计算机的要求。提高卡尔曼滤波实时能力有以下几种途径：

（1）改进计算技术，如采用计算量较小的序贯处理和信息滤波算法。

（2）减少状态维数，这可以通过压缩状态维数或将系统解耦成几个子系统来实现。

（3）采用简化增益，利用常增益或者分段增益。

（4）降低数据率，特别是脉冲多普勒雷达，其数据率高达几十或者几百千赫，而典型的滤波速度是每秒几十次。为了解决高数据率与滤波实时能力的矛盾，可以采用简单的算法对数据进行预处理，这样既能降低数据率，又能基本保持数据中的有用信息。

当目标做机动运动时，如果滤波器收敛慢就很容易产生丢失跟踪的状况，而一个快收敛性的滤波器能够很好地解决这个问题。现代航海雷达目标跟踪时从滤波起始过渡到稳定跟踪一般需要约40个采样周期。对于一些海上小型船只，由于其机动速度快，所以有必要关注收敛速度问题。如果应用多普勒技术来实现跟踪，其收敛速度会非常快，甚至可以做到一步收敛。但由于相当一部分雷达是非相参的边扫描边跟踪体制，目前接收机又是模拟式的，使得多普勒技术在航海雷达中未能得到应用。

2.3.6 常增益滤波

由卡尔曼滤波的状态更新方程可以看出，某个时刻位置的更新值等于该时刻的预测值再加上一个与增益有关的修正项，而要计算增益 ，就必须计算协方差的一步预测、新息协方差和更新协方差，因而在卡尔曼滤波中增益 的计算占了大部分的工作量。为了减少计算量，就必须改变增益矩阵的计算方法，为此人们提出了常增益滤波器，此时增益不再与协方差有关，而是在滤波过程中可以离线计算，这样就大大减少了计算量，易于工程实现。常见的常增益滤波方法主要是指 和 滤波。

和 滤波实质是卡尔曼滤波的稳态解形式。由于 和 滤波是简单并且易于工程实现的常增益滤波方法，已被广泛应用于跟踪滤波器的设计过程中，最大的优点是在与增益矩阵的离线计算，而且计算量相对于卡尔曼滤波来说非常小。

1．二阶 α - β 滤波

滤波主要应用在匀速直线目标跟踪系统中， 和 分别对应目标位置和速度滤波方程的残差分量加权，滤波方程为：

式中： 为滤波增益阵。

也可以写成如下形式：

式中： ； 。

滤波与卡尔曼滤波最大的不同点就在于增益的计算不同， 和 是无量纲的量，分别为目标状态的位置和速度分量的常滤波增益，这两个系数一旦确定，增益就是个确定的量。所以此时协方差和目标状态估计的计算不再通过增益使它们交织在一起，它们是两个独立的分支，在单目标情况下不再需要计算协方差的一步预测、新息协方差和更新协方差。

2．三阶 滤波

与 滤波类似的，三阶 滤波用于对匀加速运动目标进行跟踪，目标的状态向量中包含位置、速度和加速度三项分量。其滤波方程为：

式中： 为滤波增益阵。

也可以写成如下形式：

式中： ；

；

。

3．增益系数的确定

如何选择 、 以及 的数值，进而获得较好的滤波性能呢？应考虑的因素有：系统的稳定性；暂态特性；暂态与稳态误差；对目标机动的适应性，等等。

从滤波方程式可看出，当取 时，意味着以本次测量值作为本次滤波值，前次预测值未起作用，这显然适用于刚刚起始建立跟踪的时刻，或目标跟踪丢失重新捕获后再次建立跟踪的时刻，还适用于目标快速大幅度机动的时刻；当取 时，意味着以前次预测值作为本次滤波值，本次测量值未起作用，这显然适用于目标跟踪丢失、无测量值输入的时刻，或测量值存在随机干扰的时刻。由此分析可见：

、 取较大值时适用于：

（1）起始建立跟踪或目标跟踪丢失重新建立跟踪的时期。

（2）目标做快速大幅度机动时期。

、 取较小值时适用于：

（1）目标进入稳态跟踪时期。

（2）目标无机动或机动速率很小的时期。

（3）目标跟踪丢失，惯性外推时期，应取 。

有多种可用的方法，但常用的是按照最小二乘的准则来确定 、 以及 的数值。这时的 和 滤波实质上是匀速直线运动和匀加速直线运动时的递推格式的最小二乘滤波。同时， 、 以及 的数值还可用卡尔曼滤波的稳态增益计算出来。

对于匀速直线运动，采用最小二乘准则来确定 值，根据最小二乘估计方法的讨论，可以确定：

当扫描次数 n 增加时， 、 皆减小；当航迹刚刚建立时， 取1，然后渐渐减小，最后趋向稳态值。实际上 ，因为如果 ，相当于预测值等于观测值，观测值不起作用了。

、 随 n 的增加而变化的情况可如表2.1和图2.8所示。

下面用连续系统卡尔曼滤波的稳态增益矩阵来揭示参数 、 和 的值，以及系统噪声方差阵 和观测噪声矩阵 的关系。

设连续系统的状态方程和观测方程为：

式中：状态噪声与观测噪声互不相关，其统计特性为：

对于 滤波有：

对于 滤波有：

由于上述系统是线性定常的，且假定滤波器已达到稳态，于是滤波协方差阵 满足黎卡提（Riccati）方程：

并且稳态增益矩阵为：

将各参数代入，可分别得到两种滤波器的稳态增益为：

滤波为：

滤波为：

式中： 称为信噪比。比较式（2.3.130）和式（2.3.131）和 滤波以及 滤波的增益，可得参数 与 、 的关系如下：

滤波为：

滤波为：

因此，从上述公式可以看出，增益 的优化选取，同 密切相关，即系统噪声的估计必须准确，这一要求在实际应用中往往是难以满足的，这样，依据上述思路来选取增益 ，一般是不可行的，但只具有理论分析意义。

那么，在折中考虑系统噪声动态特性的基础上，如何来优化选择增益 的数值呢？许多学者为此做了大量的工作，主要思路是在 之间找出合适的关系，根据此关系来确定 的值，这样就避开了系统噪声的限制。

卡塔拉（Katala）证明了最优参数应满足下列关系：

其中，方程（2.3.134）对两种滤波器均适用，式（2.3.135）是 滤波的附加条件。

贝尼泰克（Benetic）等人在考虑方差减小比的加权和瞬态均方差的基础上，推导了 滤波的另一种最优参数关系，即

不难证明，当 时，上式与式（2.3.134）和式（2.3.135）相近似。

在式（2.3.136）中，令 ，可得 滤波的最优参数关系为：

从上述各式可以看到，在 中至少有一个自由参数必须被指定，它可以用相应的卡尔曼滤波稳态增益来确定。

在实际应用中， 、 的调整可以按采样序号 作自适应调整，当目标机动时，按目标机动量进行调整。下面通过例子给予说明：

首先确定 值的自适应跳变选择方法。

（1）起始建立跟踪或目标跟踪丢失重新建立跟踪时，按采样序号 作自适应调整：

，取 ；

，取 ；

，取 ；

，取 ；

，取 ；

虽然进入稳态时， 值应该更小，并趋于0，但为了能随时适应目标的机动，不致因种种随机因素丢失目标，在 以后， 值不能选得太小，应不低于0.1。

（2）当目标发生机动时，按目标在采样周期中的机动量，分等级做自适应跳变调整。每个采样周期中目标运动的机动量可由本次测量值与前次预测值之差来衡量，即

当 时，可视为目标无机动，不做调整；

当 时，取 ，做一级调整；

当 时，取 ，做二级调整；

当 时，取 ，做三级调整。

当 值选定后， 可按式（2.3.134）求解。

滤波是航迹滤波与外推的常用方法，它简单且易于工程实现，具有较好的实时性和较高的滤波精度。而其最大的特点是滤波增益 、 可以离线计算。与卡尔曼滤波相比， 滤波的每次滤波循环大约可以节约计算量70%。

2.3.7 非线性滤波 ^*

1．非线性系统

前面所讨论的卡尔曼滤波要求系统的状态方程和观测方程是线性的，然而，在许多问题中，往往是不能用简单的线性系统来描述的。例如，导弹控制问题、惯性导航问题等，状态方程往往不是线性的。因此，有必要研究非线性系统的滤波问题。

对于非线性系统的滤波问题，理论上还没有严格的滤波方程。一般情况下，都是将非线性方程进行线性化，然后利用线性系统的卡尔曼滤波基本方程。本节介绍非线性系统的卡尔曼滤波问题的处理方法。有关此方面的理论，有大量的文献可以查阅。

一般离散非线性系统的状态方程和观测方程如下：

式中： 是 n 维状态向量； 是 m 维观测向量； 是非线性函数； 是非线性观测方程。

式（2.3.138）和式（2.3.139）描述相当广泛的一类非线性系统，由于此类问题的最优估计求解是非常困难和复杂的，因此，为了使估计问题得到可行的解，我们对上述模型进行进一步的简化，仅限于讨论下列情况下的非线性系统：

其中：

（1） 和 均为零均值白噪声向量， 和 不相关，即：

其中， 是Kronecker函数，即：

是对称的非负定方差阵； 是对称的正定方差阵。

（2） 的初始值 是一个随机变量。 的统计特征已知，即：

（3） 与 、 不相关，则：

下面介绍两种线性化滤波方法：

（1）围绕标称轨道的线性化方法。

（2）推广卡尔曼滤波方法。

2．围绕标称轨道线性化的卡尔曼滤波

考虑如式（2.3.140）描述的系统，所谓标称轨道就是指在不考虑噪声的情况下，系统状态方程的解：

式中： 称为标称状态变量。

真实状态与标称状态变量之间的差为：

称为状态偏差。

把状态方程中的非线性函数 围绕标称状态 进行泰勒展开，略去二次上项，可得：

又

则：

移项整理，并把 用 ，可得：

即：

其中

为 矩阵，称为 的雅克比矩阵。

下面将观测方程线性化，在不考虑观测噪声 时，可得观测值 ，即：

同样将观测方程 围绕标称状态 进行泰勒级数展开，略去二次或二次以上项，可得：

即

设 ，则可得到观测方程的线性化形式：

其中

是 的矩阵，称为 的雅克比矩阵。

因此线性化的式（2.3.150）和式（2.3.155）就是卡尔曼滤波方程所需要的系统状态方程和观测方程。因此，运用卡尔曼滤波基本方程，可以得到状态偏差得卡尔曼滤波方程 。然后计算出系统状态的滤波值：

这种线性化方法，只能在可以得到标称状态 ，且 比较小时效果较好。但实际上，标称状态 是很难得到的。这时可以使用推广卡尔曼滤波方法（Extended kalman filtering，EKF）。

3．推广卡尔曼滤波方法

上面所讲的方法是将非线性函数 围绕标称状态 展开成泰勒级数，略去二次或二次以上项后得到的。推广卡尔曼滤波（EKF）是将非线性函数 围绕滤波值 展开成泰勒级数，略去二次或二次以上项后，得到线性化模型方法。

记

所以式（2.3.158）整理得：

初始值 ， 。

这就是一个带有控制项的系统状态方程，其他条件同标准的卡尔曼滤波方程所需求的条件一样。

把观测方程 围绕预测值 进行泰勒级数展开，略去二次或二次以上项后，得到线性化模型：

记

则新的观测方程为：

1）EKF滤波算法

运用卡尔曼滤波方程，可得到：

（1）确定初值：

（2）一步最优预测 和 ：

（3）计算增益 ：

（4）最优滤波 和 ：

EKF滤波的优点是不必预先计算标称轨道，同样，EKF一般在滤波误差较小时才适用。一阶扩展卡尔曼滤波的协方差预测公式与线性滤波中的类似，不过这里雅可比矩阵 类似于系统转移矩阵 。如果泰勒级数展开式中保留到三阶项或四阶项，则可得到三阶或四阶扩展卡尔曼滤波。通过对不同阶数的扩展卡尔曼滤波性能进行仿真分析，结果表明，二阶扩展卡尔曼滤波的性能远比一阶的要好，而二阶以上的扩展卡尔曼滤波性能与二阶相比并没有明显的提高，所以超过二阶以上的扩展卡尔曼滤波一般都不采用。二阶扩展卡尔曼滤波的性能虽然要优于一阶的，但二阶的计算量很大，所以一般情况只采用一阶扩展卡尔曼滤波。

2）线性化EKF 滤波的误差补偿

因为扩展卡尔曼滤波算法是由泰勒级数的一阶或二阶展开式获得的，并忽略了高阶项，这样在滤波过程中不可避免地要引入线性化误差，对于这些误差可采用以下补偿方法：

（1）为补偿状态预测中的误差，附加“人为过程噪声”，即通过增大过程噪声协方差

来实现这一点，即用 代替 。

（2）用标量加边因子 Φ ＞1乘以状态预测协方差矩阵，即

然后在协方差更新方程中使用 。

（3）利用对角矩阵 ， ＞1乘以状态预测协方差矩阵，即

（4）采用迭代滤波，即通过平滑技术改进参考估计来降低线性化误差。

3）扩展卡尔曼滤波应用中应注意的一些问题

扩展卡尔曼滤波是一种比较常用的非线性滤波方法，在这种滤波方法中，非线性因子的存在对滤波稳定性和状态估计精度都有很大的影响，其滤波结果的好坏与过程噪声协方差 和量测噪声协方差 在滤波过程中一直保持不变，如果这两个噪声协方差矩阵估计得不太准确的话，这样在滤波过程中就容易产生误差积累，导致滤波发散，而且对于维数较大得非线性系统，估计的过程噪声协方差矩阵和量测噪声协方差矩阵量出现异常现象，即 丢失半正定性， 失去正定性，也容易导致滤波发散。利用扩展卡尔曼滤波对目标进行跟踪，只有当系统的动态模型和观测模型都接近线性时，也就是线性化模型误差较小时，扩展卡尔曼的滤波结果才有可能接近于真实值。而且扩展卡尔曼滤波还有一个缺点就是状态的始值不太好确定，如果假设的状态初始值和初始协方差误差较大的话，也容易导致滤波发散。

EKF是传统非线性估计的代表，其基本思想是围绕状态估值 对非线性模型进行一阶Taylor展开，然后应用线性系统卡尔曼滤波公式。它的主要缺陷有两点：一是必须满足小扰动假设，即假设非线性方程的理论解与实际解之差为小量，即EKF只适合弱非线性系统，对于强非线性系统，该假设不成立，此时EKF滤波性能极不稳定，甚至发散；二是必须计算Jacobian矩阵及其幂，这是一件计算复杂、极易出错的工作。

针对EKF的缺陷，众多学者提出了各种分解及补偿算法，如：U-D分解、奇异值分解、L-D分解、平方根滤波等。这些努力在一定程度上解决了EKF数值稳健性问题，相应地提高了计算效率，但仍无法避免上述EKF的两个缺陷。另外，标准EKF是取Taylor展开式的一阶近似，为提高估计精度也可取二阶近似，构成SONF（Second Order Nonlinear Filter）滤波，但其实现复杂性和计算量大大增加。级数展开法中还有一种称为统计线性化方法，将非线性模型按某种不带导数的级数（如幂级数）展开，避免了求导计算，不要求 、 必须可导。从统计的观点来看，所得的表达式比Taylor级数更为精确。但该方法需要计算多重无穷积分，因计算量太大而妨碍了其推广应用。

4．无迹卡尔曼滤波（Unscented Kalman Filter, UKF）

目前，扩展卡尔曼滤波虽然被广泛用于解决非线性系统的状态估计问题，但其滤波效果在很多复杂系统中并不能令人满意。模型的线性化误差往往会严重影响最终的滤波精度，甚至导致滤波发散。此外，在许多实际应用中，模型的线性化过程比较烦杂，而且也不容易得到。

Unscented滤波，国内翻译为不敏卡尔曼滤波或无迹变换卡尔曼滤波（UKF）。UKF对状态向量的PDF进行近似化，表现为一系列选取好的 采样点完全体现了高斯密度的真实均值和协方差。当这些点经过任何非线性系统的传递后，得到的后验均值和协方差都能够精确到二阶（对系统的非线性强度不敏感）。由于不需要对非线性系统进行线性化，并可以很容易地应用于非线性系统的状态估计，因此，UKF方法在许多方面都得到了广泛应用，例如模型参数估计、人头或手的方位跟踪、飞行器的状态或参数估计、目标的方位跟踪等。

Unscented滤波是一种典型的非线性变换估计方法。在施加非线性变换之后，仍采用标准卡尔曼滤波，所以也称为UKF（Unscented Kalman Filtering）。Unscented滤波由牛津大学Julier、Uhlmann等于1995年首次提出，其后又得到美国学者Wan、Van derMerwe的进一步发展。其核心是通过一种非线性变换——u变换（Unscented变换）来进行非线性模型的状态与误差协方差的递推和更新，所以UF的关键在于u变换。

与EKF不同，UKF不是对非线性模型做近似，而是对状态的概率密度函数（Probability Density Function，PDF）做近似。U变换原理如图2.9所示，首先选择有限个近似高斯分布离散点（称为 点），它们的均值为 、方差为 。对每个 点施以非线性变换（经过非线性系统的状态方程和量测方程传播后），得到一簇变换后的点，将它们的均值和方差经过加权处理，可求出非线性系统状态估值的均值和协方差。UF算法有以下特点：

（1）UF可以准确估计均值和协方差达到Taylor级数的四阶精度，而EKF估计均值达到二阶精度，协方差达到四阶精度。

（2） 点俘获到的均值和协方差不会因采取不同的平方根分解方法而改变，因此可以采用效率高、鲁棒性强的Cholesky分解方法，在实时应用场合这将显得尤为重要。

（3）经过u变换后就不需计算状态方程与测量方程的Jacobian矩阵，实现相对简单。

UF表面上看似乎与粒子滤波一样是一种蒙特卡罗方法，其实不然。首先， 点不是随机抽取的，它有确定的含义（有给定的均值和方差），因此状态变量的一、二阶矩才能被这些数量有限的 点俘获；再者， 点的加权方式与粒子滤波中样本点的分配方式不一样，它不是通常意义上的加权，而是一种“广义”加权，其权系数不一定都为正，不一定分布在[0，1]区间。所以虽然U变换也需要采样，但不能将其理解为通常的抽样统计。

1）Unscented变换

Unscented卡尔曼滤波是在不敏变换的基础上发展起来的。Unscented变换（Unscented Transformation，UT）的基本思想是由Juiler等首先提出的。Unscented变换是用于计算经过非线性变换的随机变量统计的一种新方法。Unscented变换不需要对非线性状态和测量模型进行线性化，而是对状态向量的PDF进行近似化。近似化的PDF仍然是高斯的，但它表现为一系列选取好的 采样点。

假设 X 为一个 维随机向量， 为一非线性函数，并且 。 X 的均值和协方差分别为 和 P _x 。计算UT变换的步骤可简单叙述如下。

（1）首先计算（2 n _x +1）个 采样点 X _i 和相对应的权值 W _i ：

式中： k 是一个尺度参数，可以为任何数值，只要 。 是 均方根矩阵的第 i 行或第 i 列， n _x 为状态向量的维数。

（2）每个 采样点通过非线性函数传播，得到

（3） y 的估计均值和协方差估计如下：

2）滤波模型

假设 k 时刻融合中心的状态估计向量和状态估计协方差分别为 和 ，则可以利用式（2.3.170）和式（2.3.171）计算出相应 点 和其对应的权值 。根据状态方程，可以得到 点的一步预测：

利用一步预测 点 ，以及权值 W _i ，根据式（2.3.172）和式（2.3.173），可得到状态预测估计和状态预测协方差：

式中： 。

根据量测方程，可得到预测量测 点：

式中： 。

同样，我们可以得到测量和状态向量的交互协方差：

如果 时刻传感器所提供的测量为 ，则状态更新和状态更新协方差可表示为：

5．粒子滤波（Particle filtering，PF）

EKF和UKF都是递推滤波算法，其基本思想是通过采用参数化的解析形式对系统的非线性进行近似，而且都是基于高斯假设。在实际情况中非线性、非高斯随机系统估计问题更具普遍意义，解决这一问题的一种有效方法是以非参数化的蒙特卡罗模拟为特色的粒子滤波（Particle filtering）法。粒子滤波是英国学者Cordon、Salmond等于1993年提出的基于Bayes原理的序贯蒙特卡罗模拟方法。该方法的核心是利用一些随机样本（粒子）来表示系统随机变量的后验概率密度，能得到基于物理模型的近似最优数值解，而不是对近似模型进行最优滤波，最适合于强非线性、非高斯噪声系统模型的滤波。

粒子滤波是指通过寻找一组在状态空间中传播的随机样本对概率密度函数 进行近似，以样本均值代替积分运算，从而获得状态最小方差估计的过程，这些样本即称为“粒子”。采用数学语言描述如下：对于平稳的随机过程，假定

时刻系统的后验概率密度为 ，依据一定原则选取 个随机样本点， 时刻获得测量信息后，经过状态和时间更新过程， 个粒子的后验概率密度可近似为 。随着粒子数目的增加，粒子的概率密度函数逐渐逼近状态的概率密度函数，粒子滤波估计即达到了最优贝叶斯估计的效果。粒子滤波算法摆脱了解决非线性滤波问题时随机量必须满足高斯分布的制约条件，并在一定程度上解决了粒子数样本匮乏问题，因此近年来该算法日益受到重视，并在许多领域得到成功应用。

1）最优贝叶斯估计

假定动态时变系统描述如下：

若已知状态的初始概率密度函数为 ，则状态预测方程为

状态更新方程为

式中归一化常数

式（2.3.185）～式（2.3.187）描述了最优贝叶斯估计的基本思想，式（2.3.187）取决于由模型式（2.3.184）所定义的似然函数 。在更新公式（2.3.187）中，测量值 被用来修正后验概率密度，以获得当前状态的后验概率密度函数。式（2.3.185）和式（2.3.186）是最优贝叶斯估计的一般概念表达式，通常不可能对它进行精确的分析。在满足一定的条件下，可以得到最优贝叶斯解。但如果条件不满足，特别是在非线性状态估计条件下，可以利用UKF 或PF 滤波算法获得次优贝叶斯解。

2）粒子滤波算法

粒子滤波算法属于序贯蒙特卡罗算法的范畴，它的核心思想是用一系列带有权值的随机采样点来表示要求的后验概率密度函数，再利用这些采样点和权值来得到最终的估计量 。当采样点数足够多时，可以认为采样值的统计特性近似于后验概率密度的统计特性。注意，这种算法没有要求概率密度函数一定为高斯分布。

其具体计算过程如下：

（1）构造采样点集。

初始重要性权值：

（2）采样点和权值更新。

构造采样点集：

权值更新：

归一化权值：

（3）重新采样。

定义有效采样尺度

如 ，对 ，使它近似于分布 ，重新设定粒子的权值为 ；否则， 。

（4）状态更新。

粒子滤波器最常见的问题就是退化（degeneracy）现象，即经过几次迭代，除一个粒子外，所有粒子都只具有微小的权值。退化程度可以用有效样本个数进行度量，退化现象意味着大量的计算工作都被用来更新那些对后验概率密度的估计几乎没有影响的粒子上。减小这一不利影响的一个方法是增加粒子数，但这通常是有限度的。所以主要还是依靠选取适当的重要性概率密度和再采样两种方法解决：

（1）重要性概率密度的选择。通常选取系统状态的转移概率作为重要性概率密度，但用转移概率分布来产生预测样本没有考虑系统状态的最新观测，由此产生的样本同真实的后验概率产生的样本偏差较大。袁泽剑、郑南宁等提出一种确定的次优化方法——高斯-厄米特粒子滤波器（GHPF），高斯-厄米特滤波（GHF）是一种基于高斯-厄米特数值积分的Bayes滤波方法，它不用计算Jacobian矩阵，没有非线性映射必须为可微映射的限制。Vander Merwe等提出将Unscented滤波与粒子滤波相结合，用U变换来获取重要性概率密度，从而构成一种新的粒子滤波器UPF（Unscented Particle Filter）。该方法有两个特点，一是将最新观测信息加入到先验信息更新循环中，二是通过U变换产生的重要性概率密度更接近真实后验概率密度。Higuchi采用遗传算法产生重要性概率密度函数，仿真结果表现出良好的估计性能。

（2）再采样。其基本思想是消除小权值粒子而集中大权值粒子，方法是对后验概率密度再次采样，生成新的粒子集。再采样又会带来采样枯竭问题，即权值较大的粒子被多次选取，采样结果中包含了许多重复点，从而失去粒子的多样性。已经提出一些方法来解决该问题，例如：增加马尔可夫链蒙特卡罗移动步骤、粒子正则再采样、非等权值粒子确定性算法等。 fjvcYREHwZY4S3nJT7Fw1D3sjQiPmFb3t25wZUWX0BW9bl6qPp2D1Froj2Jywkaq