一种基于多级Kalman滤波的高精度距离估计方法

唐 玲，杜雨洺

(成都信息工程学院电子工程学院，四川成都610225)

0 引言

在对慢速运动目标进行距离估计时，由于目标运动速度过慢，短时间内可以认为其处于静止状态，在强干扰环境中，目标测量误差可能远远大于其实际运动的距离，在这一情况下，测量值可能很难落在误差允许范围之内，从而造成数据无法真实反应目标运动轨迹[1]。在真实复杂环境下，由于障碍物和多径干扰的原因，测量值中可能出现大量野值点，直接对其进行滤波会导致滤波估计值明显地偏离系统真实状态，导致目标运动轨迹失真[2]。因此研究对慢速目标的高精度距离估计具有重要的实际意义。

通过对慢速运动目标的特点进行分析，提出一种两级Kalman滤波的距离估计方法，以被测目标的当前时刻距离测量值为输入，对一分钟内的测量值取均值进行差值检测，上一时刻最优估计与当前时刻测量值的绝对差作为差值门限检测的标准距离差以判断下一时刻数据点是否为野值点，计算当前时刻与标准距离的差值，对当前时刻落在门限范围外的值进行修正，以修正后的数据作为第一级Kalman滤波的初始状态向量，这样避免了野值点对滤波效果的不利影响，同时不会对距离变化响应时间有较大影响。

1 卡尔曼滤波

1.1 Kalman滤波理论

Kalman滤波器是线性、无偏、最小方差估计器，是一种通过迭代算法实现的估算器[3]，原理是从观测到的向量中对随着时间不断变化的状态向量进行估计，并且希望估计值与状态向量的真实值的误差越小越好[4]，Kalman滤波在处理动态观测数据时以其计算量少及实时性在很多方面得到了广泛应用[5]。在跟踪运动目标的系统中，可以把运动目标的位置信号看成一个离散动态系统，用k+1时刻的目标状态方程X(k+1)和测量方程Z(k)描述该系统。

式中，X(k)为k时刻的目标状态向量，W(k)为k时刻目标运动过程噪声，U(k)为k时刻对系统的控制量，A、B、H为系统参数，Z(k)为k时刻观测值，V(k)为k时刻测量噪声。在测距系统中有

(1){W(k)}、{V(k)}是均值为零的高斯白噪声随机序列，即

(2)状态向量、观测噪声、过程噪声互不相关，即

式中，k，j均表示时刻，上角标T表示矩阵的转置。

Kalman滤波是结合当前时刻的测量值，对上一时刻的预测进行校正，得到校正后的最优估计，该估计具有最小方差。一般来说，Kalman滤波分为两个步骤:预测、校正。预测即对时间状态向量进行更新，校正即对测量状态向量进行更新[6]。当Q、R值确定时，由下列递推公式得到目标的状态估计值:

1.2 多级Kalman滤波方法应用于慢速运动目标距离估计

在对慢速运动目标距离估计数据处理时，经典Kalman滤波存在不足。

(1)在应用Kalman对数据进行滤波处理时，只需要知道被研究对象的数学模型和噪声统计的先验知识，并不考虑测量值是否有现实意义，即只需要根据验前确定的系统参数和噪声方差的初始值，按照滤波公式递推下一时刻的估计值[8]。当目标运动状态量中有野值点出现时，Kalman滤波也会跟踪野值，这对估计结果有很大影响[9]。因此，直接对测量值进行Kalman滤波可以剔除孤立型野值点，但是对连续型野值点进行剔除时[10]，由于Kalman滤波也会对每一个野值点按权重累积，这对滤波结果影响很大，可能严重降低滤波精度。

(2)慢速运动目标的特点是目标与观测点之间的相对运动速度很慢，若观测点处于静止状态，短时间内可以忽略目标运动速度，把运动目标看成是静止状态建模，这样系统具有可观测性[11]，但是当目标运动到一定距离时，那么测量值偏离真实值的误差有可能远远大于目标实际运动的距离，因此目标的运动给测量带来了误差[12]。在这种情况下，经过Kalman滤波后却不能立即在数据上反应出来，即不能在实时对距离变化做出响应，因此不能判断目标状态是否发生改变。

(3)对于慢速运动目标而言，当改变滤波参数提高测距精度时，相邻估计值之间的差值受滤波精度影响，如果被测目标位置发生变化，一个数据率时间内变化的距离值为厘米级以下，滤波精度过高时被平滑处理，不能在数据上体现出距离的变化。因此，滤波精度越高系统反应时间越长，可以看出系统反应时间和滤波精度相互影响并且相互矛盾。

考虑到以上几个问题，采用多级Kalman滤波来对慢速运动目标距离估计数据进行处理，通过改变第一级和第二级滤波参数来调和系统反应时间和滤波精度不能同时优化的矛盾。Kalman滤波前对初始值进行预处理，取一组数据的均值进行野值点的判定，当数据超过判定门限时，用一个分段函数加权改变新旧数据比重，降低漏判和误判的概率;否则，进行第一次滤波。取第一级Kalman滤波结果的最小值作为第二级滤波的先验估计值，从而得到最优估计，这样既减小了滤波误差，同时在不同场合将反应时间控制在允许范围内。多级Kalman滤波流程如图1所示。

图1 多级Kalman滤波流程图

2 实验仿真及结果分析

以德国Nanotron公司的nanoLOC通讯测距模块[13]为例，利用实验数据仿真验证多级Kalman滤波方法的有效性。理论上，nanoLOC距离估计精度在3 m左右[14]，这在对精度要求很高的环境下不能满足要求。为了验证多级Kalman滤波在提高慢速目标距离估计精度的效果，以15 m实际观测值作为一级Kalman滤波的先验估计值，在MATLAB上进行仿真实验。该模块的数据率为0.02 s，目标的速度0.001 m/s，在二维平面里，目标的运动距离为2×10－5m，而测量值偏离真实值的距离(测量误差)为0.6～3 m，测量误差远远大于目标的运动距离。

2.1 滤波模型建立

Kalman滤波最主要的一步就是建立系统的状态模型，在实际应用中很难对系统进行精确的描述，因此在实际中常用近似模型代替。为了数据精度得到较大改善的同时不影响响应时间，设计了两级Kalman滤波，第一级滤波对系统测量得到的距离计数据进行第一次估计，得到改进后的估计值，这个估计值作为第二级滤波的先验估计值，进行第二级滤波，最终完成对距离信息的完整估计。

2.2 主要参数选取

在实际工程中，滤波初始值和估计误差方差初值都会影响系统的稳定性，且精确的初始值可以获得最优的滤波估计值。如果初始值选取不恰当，长时间之后，可能导致滤波发散，所以选取适合的滤波参数对滤波精度起着至关重要的作用。

2.2.1 滤波初始值选定

在距离测量系统中，由于噪声的不确定性，滤波初始值对整体滤波效果的影响很大，Kalman滤波器中初始值为后续估计的起始参考值，唯一地确定了估计模型的起始估计点，的误差将直接影响滤波器的收敛速度，误差越大则滤波器的收敛速度越慢。因此，为了确保数据较快的收敛初始值应尽可能的准确，取系统开启后前5分钟的数据均值，即=，其中N为5分钟内数据样本量。

2.2.2 过程噪声方差Q值选取

过程噪声方差Q反应连续两个时刻的距离方差，它的好坏对滤波精度有直接的影响，Q值选取的一个基本原则是使Q值大小与动态模型的精度相匹配。由于距离测量系统的目标不断运动，外界环境也在不断发生变化，所以采用自适应的方法估计过程噪声方差[15]。

通过对实验数据的分析可知，一级Kalman滤波中Q值应该比二级Kalman滤波中Q值小一个数量级，通过先进行较低精度滤波对距离数据中差距较大的值进行粗略的估计，在进行较高精度的滤波获取较好的估计，这样达到对滤波精度的较好控制。最终选取Q1=0.85 × 10－3，Q2=10－4。

2.2.3 测量噪声方差R值选取

测量噪声方差R同样是很重要的一个参数，反应目标距离的测量精度，通过对测量数据和距离测量系统动力学方程建模得知，当R=3时，系统的收敛性和测量精度能达到较好的平衡。

2.3 仿真实验及数据分析

2.3.1 滤波精度

图2为经nanoLOC通讯测距模块测量得到的原始测量信息，可以看出由测距模块测量得到的数据中斑点型野值点较多，导致测量数据误差高达3.6 m;由前面分析可知，直接进行Kalman滤波滤除斑点型野值点可能导致系统对距离变化的反应时间变慢，因此对图2中的数据进行差值检测，图3经差值检测后的数据即第一级Kalman滤波的初始状态值，从图3看出剔除原始数据中斑点型野值点后误差从3.6 m减小到1.78 m，误差下降1.82 m;图4为经第一级Kalman滤波后的数据，误差从1.78 m减小到0.87 m，下降0.91 m;图5为经过第二级Kalman滤波后的距离信息，此时误差已经减小到0.45 m。

对比分析可以看出，Kalman滤波的方法可以有效地对数据进行预估和校正，使输出变得更平滑。表1为不同时期滤波精度值。

图2 原始距离信息

图3 差值检测后的值

图4 一级Kalman滤波后的结果

图5 二级Kalman滤波后的结果

表1 各阶段滤波数据精度值比较

2.3.2 反应时间

对慢速运动目标而言，系统反应时间是很重要的指标，决定了系统是否能及时的对目标位置变化做出反应。以初始距离12.5 m为例，系统处于稳定状态时移动一段距离(测得移动距离为0.8 m)，若反应时间在10分钟以内则认为符合要求，对测量误差和系统反应时间进行分析。图6为原始距离信息，在距离变化前测量误差为2.12 m，经过5.42分钟系统对该变化做出反应，并且距离变化后的误差为3.23 m。可以看出测量值偏离真实值的误差远大于目标实际运动的距离，对于慢速运动目标来说，不能达到测量要求。经过多级Kalman滤波处理后，测量误差越来越小，系统反应时间也能控制在规定时间内。最终结果如图9所示，在距离变化前测量误差为0.47 m，经过9.89分钟对距离变化做出反应，距离变化后的误差为0.37 m，可以看出测量值偏离真实值的误差小于目标实际运动的距离。对于慢速运动目标来说，测量精度和对距离变化的反应时间都符合系统测量要求。

表2 距离变化中各阶段数据反应时间

图6 原始距离信息

图7 差值处理结果

图8 第一级Kalman滤波结果

图9 第二级Kalman滤波结果

3 结束语

通过对nanoLOC测量数据的分析，提出对距离测量系统的目标距离信息进行两级Kalman滤波处理，建立对应的滤波模型，且对主要的滤波器参数的选取进行讨论。实验结果表明，方法简单有效，在实际应用中针对具体要求可以增加Kalman滤波级数，具有较强的实用性。

[1] 易哲，王国宏，张翔宇，陈泽元.一种高数据率雷达跟踪慢速目标的数据互联算法[J].电学与控制，2014，21(7):31－35.

[2] 祝转民，秋宏兴，李济生，等.动态测量数据野值的辨识与剔除[J].系统工程与电子技术，2004，26(2):147－149.

[3] 黄鹤，张会生，许家栋，等.一种基于二级变维Kalman滤波的跟踪算法[J].航空计算技术，2008，38(3):97－100.

[4] 张国权，倪重匡.应用Kalman滤波提高雷达测量精度[J].电子学报，1981，3:19－25.

[5] 樊建新，董绪荣，罗定开，等.动态测量问题数据处理的一个快速滤波算法[J].武汉测绘科技大学学报，1998，23(2):111－114.

[6] 余翔，冯璐，漆晶.一种组合式的Kalman滤波算法[J].电视技术，2013，37(9):168－170.

[7] 赵军.基于C语言的测量数据修正技术[J].计算机与网络，2013，15:48－50.

[8] 秦庆强，张晓安，李艾华.基于自适应Kalman滤波的加速度计动态数据处理[J].计算机工程与设计，2009，30(17):3943－3945.

[9] 朱学锋，韩荣阁，杨若红.基于模糊预测系统的观测数据野值剔除方法[J].系统工程与电子技术，2006，28(3):478－482.

[10] 张帆，卢峥.自适应抗野值Kalman滤波[J].电机与控制学报，2007，11(2):188－190.

[11] 张凯，刘洋.慢速目标仅测角无源定位算法研究[J].雷达科学与技术，2014，12(3):291－296.

[12] 刘爱东，倪永强，王建国.一种实时剔除雷达测量数据中野值的方法分析[J].火力与指挥控制，2004，29(S1):17－19.

[13] 余文芳，胡旭科.基于线性调频的nanoLOC新技术与应用研究[J].信息通信，2011，2:4－6.

[14] 巩家昌，张军.Nanotron技术在无线测距系统中的应用[J].现代电子技术，2011，34(3):56－58.

[15] 祝转民，秋宏兴，李济生，等.Kalman滤波工程应用问题分析及改进方法研究[J].宇航学报，2002，23(3)，44－47.