基于多项式拟合的扩展卡尔曼滤波算法

吴汉洲 宋卫东 徐敬青

摘要：弹道修正弹内的弹载计算机必须实时对卫星定位接收机获取的弹丸状态数据进行滤波降噪，用于预测弹丸落点，传统滤波方法滤波时间长，滤波实时性差，提出一种基于多项式拟合的方法。通过适当降低卫星定位接收机数据更新频率，并用多项式拟合插值出的数据代替数据更新时间间隔内的弹丸状态数据。仿真实验表明，该算法在不降低滤波效果的前提下，较普通扩展卡尔曼滤波时间降低7/8，提高了滤波实时性，对于弹道修正弹关键技术的研究提供了重要参考。同时该方法可推广应用到其他滤波算法当中，具有很强的可移植性。

关键词：滤波算法；多项式拟合；弹道修正弹；卫星定位数据；滤波误差

中图分类号：TP301 文献标志码：A

Abstract： The data acquired by the satellite positioning receiver in the trajectory correction projectile must be filtered in realtime to predict the point. The calculation of traditional filtering method is timeconsuming， and is difficult to meet the requirements of realtime filtering. A kind of extended Kalman filtering algorithm based on polynomial fitting was proposed. The data of projectile flight in the time interval was replaced by the fitting interpolation data. In this way the filter frequency could be reduced. Simulation results show that the computation time of the proposed method can be reduced by 7/8 compared to traditional extended Kalman filtering without reducing the filtering precision， and the realtime performance is improved. This method can provide important reference for the research of key technology of trajectory correction projectile. At the same time， the method can be applied to other filtering algorithms， and has a strong portability.

Key words：filtering algorithm； polynomial fitting； trajectory correction projectile； satellite positioning data； filtering error

0 引言

随着科技的发展，代表科技进步显著成果之一的电子元器件的发展经历了从无到有、从简单到复杂的过程，并不断向集成化、小型化、低成本等多方向发展。随着军事发展，对电子元器件额外提出的高抗过载能力问题也得到很好的解决[1]，这大大促进了弹道修正弹的发展。弹道修正弹内部装有卫星定位系统，能够实时获取弹丸位置、速度等信息，利用该信息计算预测的弹丸落点，通过比较预测落点与目标点距离形成修正指令，修正弹丸飞行弹道，从而使弹丸飞向目标。其中卫星数据准确度直接影响到预测落点的准确度，由于卫星定位系统自身系统误差，其定位数据不能满足直接用来预测弹丸落点的要求，需要对定位数据进行降噪滤波，为满足滤波实时性、准确性要求，基于状态空间法的卡尔曼滤波理论[2-3]被广泛应用。其中应用于非线性系统的主要为扩展卡尔曼滤波（Extended Kalman Filtering， EKF）。

EKF算法主要通过对非线性系统进行泰勒展开，忽略高阶项来实现非线性系统的线性化。但这种方法处理强非线性系统时，会因对系统描述不准确而滤波效果不好。对此有关研究者提出了二阶EKF算法[4-6]，即保留泰勒展开的一、二阶项，忽略更高阶项，但该方法造成了较大的运算量，影响了运算速度。也有研究者提出通过反复迭代状态估计的方法[7-9]优化滤波效果，但该方法同样存在运算量增大的问题。

本文通过适当降低滤波频率，减少滤波次数的方法来整体提升滤波速率，并基于多项式拟合的方法在滤波点之间插值，补偿定位数据更新慢的不足，仿真实验表明该方法能够在不扩大滤波误差的情况下，明显提高滤波效率。

1 弹道模型的选择

弹道修正弹内的弹载计算机进行滤波时，必须以弹道模型为滤波基础，弹道模型越精确滤波算法滤波效果越好，但是弹道模型越精确就意味着弹道模型越复杂，本文以某型100mm一维弹道修正榴弹的弹道为仿真对象，鉴于弹道滤波实时性、快速性的特点，本文以弹道模型较简单的二维质点弹道[10]建立其弹道模型：

2.2 基于多项式拟合的扩展卡尔曼滤波

在工程应用中，弹道修正弹内卡尔曼滤波算法是根据全球定位系统（Global Positioning System， GPS）接收数据的频率进行循环滤波的，这样造成了两个问题：1） 如果GPS的数据更新频率太快，则为了保证滤波实时性，要求滤波时间尽可能地短，这对于简单系统或者简单滤波算法来说一般还能满足，对于复杂系统、复杂滤波算法，则会由于每次滤波时间太长，失去滤波实时性；2）如果降低滤波频率，给滤波计算留出时间，则弹丸在进行落点预测时（落点预测频率与GPS数据更新频率相等）会因每次预测落点相隔时间太长，失去最佳修正时机，造成较大的落点偏差，大大影响射弹精度。

本文提出在保证一定滤波精度基础上适当降低GPS数据更新频率，从而降低滤波频率，对于GPS数据更新之间的时间间隔里通过最小二乘拟合的方式拟合插值出新的更新数据[10]。以射程x为例加以说明，为简化插值算法在短时间内可认为弹丸速度及位置数据变化符合某一元二次抛物线某一段曲线，因此进行数据拟合时最少可仅用三个点，设已滤波降噪得到三个点（x1，x2，x3），利用这三个点求出式（19）中抛物线未知系数a、b、c的值，其中时间t就取弹载计算机内时钟每个点出现时刻：

利用该表达式，可求出上一次滤波时间与下一滤波时间之间更小时间间隔内的x值，弥补了GPS数据更新频率低的不足。

设每一次对GPS数据的滤波处理耗时Tm，设tk表示该时刻要对GPS数据进行第k次滤波，图1为一次完整的基于多项式拟合的扩展卡尔曼滤波流程。由于每次拟合插值计算相对于滤波计算耗时很少，故忽略该部分计算耗时。

3 仿真实验

3.1 仿真背景设置

弹载计算机测得的弹丸位置及速度信息是基于WGS84协议下的地心地球固连坐标系（ECEF）内，在调用该数据时需要将该坐标下的数据通过坐标变化矩阵转换到弹载计算机上的地面坐标系内[11]，本文假设已对GPS测量原始数据进行了坐标变化[12]。具体实现过程为，通过建立该弹丸6D刚体弹道模型，解算出一条弹道，获得的弹道数据包括弹丸不同时刻位置、速度信息，以该弹道数据为原始数据，加上由仿真软件产生的高斯白噪声误差即为模拟GPS定位数据，误差范围设置如表1所示。本文以一条射角为35°仿真弹道为基础， 滤波算法对整条弹道都进行滤波降噪，取弹载计算机GPS数据更新频率为1Hz，对GPS更新时间间隔插值9次，相当于GPS数据更新频率变成了10Hz（弹载计算机GPS数据更新常用频率）。

从图2、图4可看出，虽然基于插值拟合的EKF算法滤波频率只有1Hz，但仍能保持较好的滤波效果，与滤波频率为10Hz的普通EKF算法两者滤波效果相当；图3、图5中GPS定位误差曲线与以10Hz频率滤波误差曲线重合，说明普通EKF算法对误差较小的vx、vy滤波效果不明显，而基于插值拟合的EKF算法滤波误差出现较大的波动，波动幅值与GPS定位误差幅值相当，说明其对vx、vy滤波效果也不明显；表2为不同算法滤波后数据方差表，从表中可看出，以10Hz滤波频率的普通EKF算法与1Hz滤波频率的基于插值拟合的EKF算法对x、y都具有较好的滤波效果，两者对vx、vy滤波效果都不明显。表3列出了基于Matlab软件分别建立EKF模型和基于多项式拟合的EKF模型，并在不同仿真条件下对同一条模拟GPS弹道数据进行滤波所用时间，从表中可看出，1Hz滤波频率的基于插值拟合的EKF算法较滤波频率较1Hz滤波频率的普通EKF算法滤波时间增加14.8%，但仅是滤波频率为10Hz普通EKF滤波时间的1/8，整体滤波速率提升明显。综上所述，基于插值拟合的EKF算法在不影响滤波效果情况下，滤波时间上大大缩短。

4 结语

在弹道修正弹研发过程中，弹载计算机必须实时对卫星接收到的弹丸位置、速度等信息进行滤波降噪，传统滤波方法滤波时间较长，跟不上卫星定位接收机数据更新频率，大大影响了滤波的实时性，本文提出适当降低卫星定位接收机数据更新频率，利用多项式插值拟合的方式，插值拟合出卫星定位接收机数据更新时间间隔内的弹丸位置、速度信息。仿真实验表明，该方法在不降低滤波效果的前提下，能够明显降低滤波时间，提高弹丸位置、速度信息更新的实时性，对于弹道修正弹关键技术的研究提供了重要方法。同时该方法可移植性强，可推广应用到其他滤波算法当中。

参考文献：

[1]GRACE J. GPS guidance system increases projectile accuracy [J]. IEEE Aerospace and Electronic Systems Magazine， 2000， 15（6）： 15-17.

[2]KALMAN R E. A new approach to linear filtering and prediction theory [J]. Journal of Basic Engineering， 1960， 82（1）： 35-45.

[3] KALMAN R E， BUCY R S. New results in linear filtering and prediction theory [J]. Journal of Basic Engineering， 1961， 83（1）： 95-108.

[4]JAWINSKI A H. Filtering for nonlinear dynamical systems [J]. IEEE Transactions on Automatic Control， 1966， 11（4）： 765-766.

[5]范炜，李勇.近似二阶扩展卡尔曼滤波方法研究[J].空间控制技术与应用，2009，35（1）：30-35. （FAN W， LI Y. A method of approximate secondorder extended Kalman filter [J]. Aerospace Control and Application， 2009， 35（1）： 30-35.）

[6]孟真，阎跃鹏，于进勇.基于二阶泰勒展开的扩展卡尔曼滤波测频算法[J].江苏大学学报（自然科学版），2010， 31（5）：564-569. （MENG Z， YAN Y P， YU J Y. Extended Kalman filter for frequency measurement based on twoorder Taylor expansion [J]. Journal of Jiangsu University （Natural Science Edition）， 2010， 31（5）： 564-569.）

[7]BELL B M， CATHEY F W. The iterated Kalman filter update as a GaussNewton method[J]. IEEE Transactions on Automatic Control， 1993， 38（2）： 294-297.

[8]朱建峰.拟线性最优平滑滤波在指令控制——维弹道修正弹上的应用研究[D].南京：南京理工大学，2012：32-34. （ZHU J F. Application research of quasilinear optimum smoothing filtering to onedimensional ballistic in command control [D]. Nanjing： Nanjing University of Science and Technology， 2012： 32-34.）

[9]欧阳广帅，周晶.基于卡尔曼滤波的高精度弹道滤波算法研究[J].电子测量技术，2014，37（11）：16-19，24. （OUYANG G S， ZHOU J. High accuracy ballistic filtering algorithm based on the Kalman filter [J]. Electronic Measurement Technology， 2014， 37（11）： 16-19， 24.）

[10]梁民赞，黄子豪，曹占启.曲线拟合与卡尔曼滤波器的滤波精度评估[J].无线电工程，2013，43（3）：36-39. （LIANG M Z， HUANG Z H， CAO Z Q. Accuracy assessment of filtering based on curve fitting and Kalman filter [J]. Radio Engineering of China， 2013，43（3）： 36-39.）

[11]邱致和，王万义.GPS原理与应用[M].北京：电子工业出版社，2002：10-36. （QIU Z H， WANG W Y. GPS Principle and Application[M]. Beijing： Publishing House of Electronics Industry， 2002： 10-36.）

[12]申强，葛腼，彭博，等.基于GPS弹道测量的卡尔曼滤波参数估计算法[J].北京理工大学学报，2009，29（12）：1048-1051. （SHEN Q， GE M， PENG B， et al. Parameters estimation algorithm by Kalman filtering based on GPS measurement for projectile trajectory [J]. Transactions of Beijing Institute of Technology， 2009， 29（12）： 1048-1051.）