卡尔曼滤波

基于修正卡尔曼滤波的目标跟踪摘要：目前卡尔曼滤波算法被广泛应用于目标跟踪算法。标准卡尔曼滤波算法是建立在状态空间上的线性高斯滤波算法，是以精确的数学模型为前提的，然而，在实际工程中，常常发生模型和系统部匹配的情况，在该情况下采用标准卡尔曼滤波算法进行跟踪必然无法实现最优估计，通常会发生跟踪精度下降。本文提出一种修正的卡尔曼滤波算法，通过设置判定准则和修正准则，实时修正预测值，在滤波初始阶段可迅速降低估计误差，提高稳态是的滤波精度。仿真实例说明了算法的有效性。关键词：卡尔曼滤波；修正预测值；运动模型一、引言精确而可靠地对机动目标进行跟踪是目标跟踪系统设计的一个主要目的，跟踪算法不仅要满足系统实时处理的要求，同时要保证较高的跟踪精度。目前卡尔曼滤波算法被广泛应用于目标跟踪。标准卡尔曼滤波算法是建立在状态空间上的线性高斯滤波算法，是以精确的数学模型为前提的，然而，在实际工程中，常常发生模型和系统部匹配的情况，在该情况下采用标准卡尔曼滤波算法进行跟踪必然无法实现最优估计，通常会发生跟踪精度下降。造成卡尔曼滤波发散的主要原因是模型误差引起的预测值不准确，因此本文在分析造成卡尔曼滤波算法滤波精度下降或发散的基础原因之上，通过新息r(k+1)和量测噪声协方差R设置判定门限准则和修正准则，直接对卡尔曼滤波算法的一步预测值进行修正，从而提高了算法的滤波精度，改善了算法的性能。这种修正的方法理论简单，计算量小。能有效解决由于模型误差带来的误差精度下降问题。二、卡尔曼滤波原理2.1卡尔曼滤波基本介绍卡尔曼滤波是解决以均方误差最小为准则的最佳线性滤波问题，它根据前一个估计值和最近一个观察数据来估计信号的当前值。它是用状态方程和递推方法进行估计的，而它的解是以估计值（常常是状态变量的估计值）的形式给出其信号模型是从状态方程和量测方程得到的。卡尔曼过滤中信号和噪声是用状态方程和测量方程来表示的。因此设计卡尔曼滤波器要求已知状态方程和测量方程。它不需要知道全部过去的数据，采用递推的方法计算，它既可以用于平稳和不平稳的随机过程，同时也可以应用解决非时变和时变系统，因而它比维纳过滤有更广泛的应用。2.2离散系统的卡尔曼滤波1）确定控制系统的状态空间方程：𝑥(𝑡)̇=𝐴𝑥(𝑡)+𝐵𝑢(𝑡)+𝐹𝑊(𝑡)y(t)=Cx(t)+Du(t)+V(t)其中W为过程噪声，V为观测噪声，它们被假设为高斯白噪声，它们的方差分别为Q和R。2）将上述系统离散化为如下所示：𝑥𝑘=𝑘|k−1𝑥𝑘−1+𝑘|𝑘−1𝑊𝑘𝑦𝑘=𝐶𝑘𝑥𝑘+𝑉𝑘上两式子中，𝑥𝑘是k时刻的系统状态，𝑢𝑘是k时刻对系统的控制量，𝑦𝑘是k时刻系统的观测量。3）我们要利用系统的过程模型，来预测下一状态的系统。假设现在的系统状态是k，根据系统的模型，可以基于系统的上一状态而预测出现在状态：𝑥̂𝑘|𝑘−1=𝑘|𝑘−1𝑥̂𝑘−1（1）式(1)中，𝑥̂𝑘|𝑘−1是利用上一状态预测的结果，𝑥̂𝑘−1是上一状态最优的结果。4）到现在为止，我们的系统结果已经更新了，现在求预报误差方差矩阵：𝑃𝑘|𝑘−1=𝑘|𝑘−1𝑃𝑘−1𝑘|𝑘−1′+𝑘|𝑘−1𝑄𝑘|𝑘−1′（2）式（2）和式（1）是对系统的预测，Q为系统过程噪声的方差5）现在我们有了现在状态的预测结果，然后我们再收集现在状态的测量值。结合预测值和测量值，我们可以得到现在状态的最优化估算值𝑥̂𝑘：𝑥̂𝑘=𝑥̂𝑘|𝑘−1+𝐾𝑘(𝑦𝑘−𝐶𝑘𝑥̂𝑘|𝑘−1)（3）其中，𝐾𝑘为卡尔曼增益：𝐾𝑘=𝑃𝑘|𝑘−1𝐶𝑘𝑇(𝐶𝑘𝑃𝑘|𝑘−1𝐶𝑘𝑇+𝑅)−1（4）6）到现在为止，我们已经得到了k状态下最优的估算值𝑥̂𝑘。但是为了要另卡尔曼滤波器不断的运行下去直到系统过程结束，我们还要更新k状态下𝑥̂𝑘的滤波误差方差：𝑃𝑘=(𝐼−𝐾𝑘𝐶𝑘)𝑃𝑘|𝑘−1（5）其中，𝐼为单位矩阵。按照上述5式自回归运算下去就可以实现滤波。三、目标跟踪建模与估计设状态向量𝑋=[𝑥𝑥̇𝑥̈]𝑇，𝑥，𝑥̇，𝑥̈分别为目标在运动方向上的位置、速度、加速度。设采样周期为𝑇，则离散时间状态方程如下：𝑋(𝑘)=(𝑘−1)𝑋(𝑘−1)+𝐺(𝑘−1)𝑊(𝑘−1)𝑍(𝑘)=H(𝑘)𝑋(𝑘)+𝑉(𝑘−1)（6）式中，𝑋(𝑘)是𝑘时刻的目标状态向量；𝑊(𝑘)为状态噪声，𝐸(𝑊(𝑘))=0，𝐸(𝑊(𝑘)𝑊𝑇(𝑗))=𝑄(𝑘)𝛿𝑘𝑗；𝑍(𝑘)是𝑘时刻的量测向量，𝑉(𝑘)为量测噪声，𝐸(𝑉(𝑘))=0，𝐸(𝑉(𝑘)𝑉𝑇(𝑗))=𝑅(𝑘)𝛿𝑘𝑗，与𝑊(𝑘)相互独立;(𝑘)和H(𝑘)分别为𝑘时刻状态转移矩阵和量测矩阵。对于机动目标跟踪，因所建模型的不同，其表达式也不同，因本文采用的是匀加速直线运动和匀速直线运动，所以匀加速运动采用CA（常加速度）模型。(𝑘)=[1𝑇𝑇2/201𝑇001]H(𝑘)=[100]（7）匀速运动采用的模型为：(𝑘)=[1𝑇0010001]H(𝑘)=[100]（8）根据上述卡尔曼滤波公式（1）~（5），预先给定状态初始值𝑋̂(0|0)和滤波估计状态向量的协方差矩阵P(0|0)，整个卡尔曼滤波过程就可以并持续的递推下去，即可得到最优估值。四、预测值修正方法4.1标准卡尔曼滤波算法的不足及改进思路当目标状态方程和量测方程已知，且状态噪声和量测噪声的统计特性已知时，造成卡尔曼滤波算法精度下降或发散的主要原因如下：(1)卡尔曼滤波通过迭代来消除估计误差，但在迭代过程中估计误差具有传递性，这种误差的传递影响达到稳态所需的迭代次数和稳态后估计的精确性。初始阶段，估计误差越小，达到稳态所需的迭代次数越小，估计精度越高。(2)采用卡尔曼滤波算法时，当系统达到稳态后，预测协方差趋于极小值，滤波增益也趋于极小值。此时，若目标状态发生突变，预测值与量测值之间的误差(即新息)增大，预测协方差和滤波器的增益不能随新息同步改变，从而失去对突变状态的跟踪能力或估计误差增加。(3)当目标运动模型不准确时，预测值偏离真实值过大，卡尔曼滤波算法失效。通过以上分析，对卡尔曼滤波算法的修正在保证原算法优势的基础上应满足：①在初始滤波阶段能够迅速降低估计误差；②在稳态阶段能够有效地消除由于目标状态突变造成的估计精度下降或跟踪失效的问题；③在目标运动建模不准确时，能够有效消除由于建模不准确带来的模型误差。由式(3)可以看出，影响滤波输出的因素有两个：预测值和修正值。修正值即新息就是预报误差，即是量测值与预测值的误差。预测值越准确则预报误差越小。因此要提高滤波的精度，就应该对预测值进行实时的修正。4.2基于修正预测值的卡尔曼滤波算法根据式(1)和式(3)可以看出，k+1时刻的预测值由k+1时刻的状态估计值决定，且k+1时刻的预测值影响k+1时刻的状态估计值。因此，要提高k+1时刻的状态估计精应提高k+1时刻预测值的精度。新息反映了预测值偏离量测值的程度，当模型不准确或目标运动状态发生突变时，预测精度较低，为了提高估计精度，应增加对量测值的信任度。若k+1时刻预测值与量测值之间的误差ε(k+1)较大，则k时刻的状态估计值𝑥̂𝑘误差较大，应对𝑥̂𝑘进行修正，通过修正的𝑥̂𝑘重新计算k+1的预测值𝑥̂𝑘+1|𝑘，从而提高k+1时刻的状态估计值𝑥̂𝑘+1，达到实时修正的目的。其流程图如图1所示。要解决以上问题，首先，要确定预测值与量测值之间误差较大的一个判断准则；其次，要制定对(l)修正的准则。在不考虑杂波和干扰的情况下，每个时刻的量测值是唯一确定的。如果以量测值为中心，以量测值的最大误差(即量测误差协方差的均方值)为半径作一圆，为了方便描述，称之为量测圆。可以肯定，真实值一定在量测圆内。如果当前时刻的预测值在量测圆内，则可以判定目标状态未发生突变；如果当前时刻的预测值在量测圆外，说明上一时刻的估计误差太大或目标状态发生突变，造成当前时刻预测误差增大；这与目标机动检测的原理是一致的。因此，通过修正预测值，使修正后的预测值落入量测圆内，再使用卡尔曼滤波算法进行估计，则必然能够消除或降低由于目标机动、误差累积和建模不准确带来的标准卡尔曼滤波算法失效或精度不高的问题，且这种方法比对增益项或新息项进行修正更容易直观理解。修正原理如图2所示。4.2.1判定准则的设置𝘀(𝑘)反映了预测值与量测值的误差程度，由于量测值本身是有误差的，因此，为了更贴近真实值，修正初始条件应该是建立在预测值与量测值之间的误差大于某一修正门限的基础之上的。令𝑟′(𝑘)=√𝑅1,1(𝑘)，𝑅1,1(𝑘)表示k时刻位置的量测误差(即R(k)中第一行第一列的数值)。因为文中只取一维量测，所以R(k)=𝑅1,1(𝑘)，𝘀(𝑘)和𝑟′(𝑘)均为标量值，对于多维量测，则需分别取对应量测维的具体量测协方差数值。若|𝘀(𝑘+1)|𝑔𝑎𝑡𝑒，则𝑥̂𝑘+1|𝑘不准确。修正𝑥̂𝑘，通过式（1）重新计算𝑥̂𝑘+1|𝑘。为了降低累积误差的影响，同时增加估计值对量测值得信任度，门限的设置为：gate=0.7𝑟′(𝑘+1)（系数0.7的选取并不固定，为了平衡量测值与预测值对修正值得影响，其值应在[0.5,0.9]区间内）。图2中实线的圆周，即判定门限gate，该圆称之为修正圆。从图中可以看出，修正准则应该保证落在修正圆外的点通过修正尽可能的落入修正圆内。4.2.2修正准则的设置具体的修正规则如下：(1)修正位置估计值。根据新息的误差，对位置估计通过补偿进行修正；(2)修正速度估计值。根据新息的误差，对速度估计通过补偿进行修正；(3)修正加速度估计值。根据修正前后速度估计的变化值，对加速度估计通过补偿进行修正。令δ=𝑟′(𝑘+1)/𝑅1,1(𝑘)，则具体修正公式如下：{𝑥̂𝑘=𝑥̂𝑘+𝑟(𝑘+1)∙𝑐1𝑥̇̂𝑘=𝑥̇̂𝑘+𝑟(𝑘+1)∙𝑐2/𝑇𝑥̈̂𝑘=𝑥̈̂𝑘+𝑟(𝑘+1)∙𝑐3/𝑇2（9）式中，𝑐1=0.7𝛿，𝑐2=0.1𝛿，𝑐3=0.3𝛿。𝑐1，𝑐2，𝑐3的选取与速度、加速度、位移的表达式有关，即𝑠=𝑣0𝑡+12𝑎𝑡2，𝑐1即公式中的位移，𝑐2为速度，𝑐3为加速度。目标跟踪过程中，如果要对速度或加速度修正较多，则𝑐2或𝑐3的取值应大于0.5，其余两个参数，一个取值小于0.5，另一个从公式推导计算得出。本文主要是对位置修正，因此𝑐1取值较大。可以看出，修正的算法与标准卡尔曼滤波算法相比，每次迭代增加了一次比较和一次修正。因此，修正的算法计算量比卡尔曼滤波算法稍大，但精度更高。五、仿真验证仿真1匀加速运动目标跟踪假设目标沿某一个方向做匀加速直线运动，状态初始值为X=[0;10;20]，目标运动时间持续时间100s，采样间隔1s。系统噪声协方差区固定值Q=diag[1,1,1],观测噪声协方差为R=100，初始估计值取第一次量测值。利用上述的CA模型，分别进行标准卡尔曼滤波与修正卡尔曼滤波。图3~图5分别为标准卡尔曼滤波和修正卡尔曼滤波对目标跟踪时的位置、速度、加速度的跟踪曲线与误差曲线。图3标准卡尔曼滤波与修正卡尔曼滤波位置最优估计值图4标准卡尔曼滤波与修正卡尔曼滤波速度最优估计值图5标准卡尔曼滤波与修正卡尔曼滤波加速度最优估计值图6标准卡尔曼滤波与修正卡尔曼滤波位置最优估值与真实值误差图7标准卡尔曼滤波与修正卡尔曼滤波速度最优估值与真实值误差图8标准卡尔曼滤波与修正卡尔曼滤波加速度最优估值与真实值误差从上面的图形可以看出，经过修正的卡尔曼滤波在位置估计上精度远远高于标准的卡尔曼滤波。虽然在加速度的精度与标准卡尔曼滤波差不多。但在目标跟踪的过程中，我们往往只在乎位置的