Kalman弹道滤波器的快速收敛性研究

范　旭，陈国光，白敦卓，杨智杰

(1.中北大学机电工程学院，太原 030051；2.豫西工业集团有限公司，南阳 473000)

0　引言

对于弹道测量，其测量数据中包含着干扰或噪声引起的误差，直接用测量数据进行分析计算将会带来很大的误差甚至是错误，而必须从量测数据中滤取所需要的信息，这种数据处理方法称为滤波。目前，工程技术中使用较多的是最小二乘滤波、最大似然估计和Kalman滤波。其中，Kalman滤波在控制领域，在火箭、炮弹、导弹弹道和飞行状态以及卫星轨道观测的数据处理上得到广泛的应用。Kalman最初提出的滤波基本理论只适用于线性系统，并且要求量测也必须是线性的。如果模型是非线性的，通常在推导滤波方程时，增加线性化步骤。在状态估计时，对系统方程在前一状态值处做实时的线性Taylor近似。在预测步骤中，对量测方程在相应的预测位置也进行线性Taylor近似,得到的Kalman滤波称为扩展Kalman滤波。处理非线性模型的这一思想是很自然的，且滤波过程简单有效[1]。

常规的滤波初值选取方法虽然可使滤波过程及结果收敛，但滤波前期的残差较大且滤波收敛速度慢，需要计算一段时间才可应用，影响弹道修正的实时应用并且使修正过程出现不必要的大幅度修正。为解决此类问题，需提出较好的状态初值估计方法，使滤波过程快速收敛满足弹道修正的实时性需要。

1　外弹道滤波模型

弹道滤波是根据在一段弹道上测得的弹箭飞行弹道数据(坐标、速度、加速度、姿态角和姿态变化率)，利用数学方法从这些数据中提取当前飞行状态及弹道数学模型中的参数的最优估计。而弹道预测则是利用这种最优估计和弹道数学模型计算弹道，求取所需点的弹道参数。弹道滤波是弹道预测的基础，而弹道预测是弹道修正的重要依据，所以滤波的收敛速度及准确度也是弹道修正的重要研究方向。

1.1　滤波状态模型

地面炮位侦查雷达对空中飞行的弹箭进行跟踪和探测，地面计算机对雷达实测到的弹道参数进行数据处理，获得较准确的弹道参数，与目标坐标比较，得到弹道偏差，并由雷达向弹载计算机传输指令，使修正控制机构实现一维或二维弹道修正。根据上述弹道修正弹的执行过程，在雷达量测数据的滤波过程中，需要用到弹道模型，考虑弹道参数获取的快速性、实时性等，本文采用质点弹道模型作为弹道滤波的状态方程[2]：

(1)

式中，x、y、z、vx、vy、vz分别为弹箭在地面坐标系中的坐标分量和速度分量，C为弹道系数，Hτ(y)为空气密度函数，G(vτ)为阻力函数，弹丸速度

为地面标准虚温值，τ为弹丸所处高度y处的虚温值，X=(vx,vy,vz,x,y,z)T作为滤波的状态变量。

则系统状态方程为：

(2)

式中，W(k)是均值为0的Gauss白噪声，且服从方差为Q的正态分布，W～N(0,Q)。

1.2　滤波量测方程

设雷达测量值为斜距r、方位角β和高低角ε，雷达坐标系为球坐标系，可得雷达测量值与地面坐标系的关系[3]：

(3)

令量测变量为Z，即Z=(r,β,ε)T，则得量测方程为：

(4)

式中，V是雷达的量测噪声，假定为零均值Gauss白噪声，且服从方差为R的正态分布。即V～N(0,R)，h(X)为三维矢量函数。

2　Kalman弹道滤波方程

Kalman滤波方程组分为两大部分，第一部分为Kalman滤波方程，这一部分负责向下一时刻推算轨迹状态；第二部分为Kalman滤波器的增益矩阵递推公式，这一部分用于反馈先验估计，并对预测进行修正[4]。

设k时刻的被估计状态Xk受系统噪声W驱动，驱动机理由下述状态方程描述：

Xk=Φk/k-1Xk-1+Γk-1Wk-1

(5)

量测方程为：

Zk=HkXk+Vk

(6)

式中，Φk/k-1为tk-1时刻的一步转移矩阵,Γk-1为系统噪声驱动阵,Hk为量测阵,Vk为量测噪声序列,Wk-1为系统激励噪声序列。

状态的一步预测为：

(7)

量测预测方程为：

(8)

(9)

滤波增益为：

(10)

一步预测均方误差矩阵为：

(11)

估计均方误差为：

Pk=(I-KkHk)Pk/k-1

(12)

3　滤波初值的选取方法

3.1　瞬时量测数据直接应用

工程运算中，对于滤波初值的选取一般为滤波时刻的量测数据，而协方差矩阵P0也是基于量测误差和时间间隔T计算得出，进而形成主对角线矩阵[5-7]。设滤波开始时间为t，将量测数据rt、βt、εt转化为x、y、z三方向数据即xt、yt、zt。而vx、vy、vz可根据时间间隔T与xt、yt、zt得到。

(13)

此种方法存在较大的弊端。首先，这种实时应用的初值具有较大的随机性，量测值受量测误差和系统误差影响，使位置状态初值以及计算速度的结果存在波动，及每次量测的状态初值均不相同使每次滤波过程不尽相同存在偶然性，不能满足实时性应用的要求。其次，这种求得速度初值的方法所得到的速度分量与实际弹道诸元相差较多，进而使协方差矩阵P0的数值过大，影响滤波收敛的快速性。针对这两类问题，提出如下方法。

3.2　滤波初始状态中速度分量与位置分量的估计方法

在实际情况下，弹体在空中的运动受到重力、空气阻力及横风等外界因素影响，其运动轨迹为曲线[8]。在曲线拟合的过程中，可选择线性回归模型，指数类或幂指数类等及其组合形式作为拟合算法的基础。其中，线性回归模型即多项式拟合方法计算复杂度低，计算速度快。同时，分析真实的弹道曲线，可发现弹道曲线形状与变化规律与抛物线类似，从实际情况与模型的相似性出发，可选择多项式进行曲线拟合。从上述两点分析，将采用线性回归模型进行弹道拟合并分析。在曲线拟合的计算过程中，常用到最小二乘法、Gauss消去法、三次样条法和追赶法等方法，来对系数矩阵进行求解，进而得到多项式系数。基于弹道曲线的形状和拟合弹道多项式计算的快速实时性，选取最小二乘法进行弹道多项式拟合。

设拟合多项式为n次幂多项式，其表达形式为：

f(x)=A1·xn+A2·xn-1+…+An·x+An+1

(14)

取m次量测数据(xi,yi)(m>n)，根据最小二乘法估计准则可得目标函数：

(15)

因目标函数为实际问题，其极小值必然存在，由极小值存在条件：

(16)

将上述条件方程转化为矩阵表达形式：

(17)

即AX=b。

由数值分析方法可知，当系数矩阵A非奇异时，则式(17)有唯一解向量X，X向量中的参数A1、A2、…、An+1，就是多项式各次幂的系数。在已知完整多项式的情况下，对所在时刻的速度v估计应为所在时刻的多项式导数，即当x=xm时所对应的f′(xm)。

多项式f(x)的导数f′(x)表达式为：

f′(x)=nA1·xn-1+(n-1)A2·xn-2+…+An

(18)

将xm=t代入，便可获得滤波的速度分量估计初值vx、vy、vz。所以根据多项式拟合得到的曲线方程，代入滤波起始时间就可估计出弹箭所在的速度分量。以滤波开始时刻的量测数据求出的(x,y,z)和速度分量估计初值(vx,vy,vz)作为滤波分量的估计初值(x,y,z,vx,vy,vz)。

4　仿真校验

为验证此种初始状态选取方法的可行性和实用性，需进行计算机仿真计算。以某型火箭弹为例，通过6D弹道方程求解出弹道数据，将弹道数据转化为极坐标的雷达量测数据。并通过随机发生器产生均值为0的Gauss白噪声作为要引入的雷达噪声，加入到原始量测数据，产生受噪声影响的雷达量测数据。

在仿真计算中，取雷达数据刷新间隔为0.1s，径向距离上的方差σr为10m，方向角的噪声方差σr为0.0015rad，俯仰角的噪声方差σr为0.0015rad，即量测噪声矩阵R的主对角元素值。模拟在火箭弹发射3.5s后雷达开始跟踪目标测量，量测数据在40s开始对弹道数据进行滤波处理，持续监控20s左右，至60s时停止滤波处理。

从图1可看出，所估速度残差随着拟合多项式阶数的增加先减少而后略有增加。虽然拟合数据时间长度不同，但一次多项式基本对应残差最大值，造成这种情况的原因在于直线模型与真实三方向轨迹情况存在很大差异，具有较大的模型误差，估计效果差。由一次多项式至二次多项式时，残差变化较大，证明拟合模型与真实轨迹状况的匹配度较高，估计速度残差减小。由二次多项式至五次多项式时，估计残差却略有增长，原因在于拟合数据是带有量测噪声的弹道测量数据。随着多项式阶数的增加，在拟合过程中数据点对拟合结果的影响将增大，也就是将量测噪声所引起的误差逐渐加入到了估计结果中，使得估计位置残差略有增长但较为平缓。由图1可知，随着拟合数据时间长度的增加，所估速度残差趋势基本为先减小后升高。在二次多项式的拟合模型下，所估速度残差最小并且数据长度为3s和5s时，基于二次多项式的速度估计残差变化不大且准确性较高。

从图2 ～图4 可看出，应用多项式估计后，三方向速度滤波初值估计残差大幅度减少，滤波过程平滑，波动较小，可快速收敛。

从图5 ～图7 可看出，应用多项式法估计状态初值与量测数据直接应用相比，其三方向上滤波前期的残差幅值大幅度缩减，滤波收敛速度增加，残差波动平缓，证明此种方法具有较高的实用性和精度。

5　结论

本文针对雷达量测的火箭弹弹道数据，通过对滤波前期的量测数据进行坐标转换，对所量测的三方向数据进行多项式拟合，进而估计滤波起始时刻速度分量，以其作为滤波初始状态。选取滤波前期拟合数据长度和拟合多项式阶数作为变量，通过计算机仿真分析其对弹道参数估计精度的影响，最后选取最优组合确定其为滤波初始状态估计的最优方法。通过滤波仿真，验证此种初始状态选取方法的可行性。由仿真结果可看出，量测数据直接应用后，三方向滤波收敛时间在滤波开始后5.5s左右，而应用此种估计方法的滤波过程基本在3.5s收敛，滤波收敛时间减少1/3且滤波幅值大幅度减少，满足快速准确的要求，实用性强。

[1]秦永元, 张洪钺, 汪叔华.卡尔曼滤波与组合导航原理[M].西安:西北工业大学出版社, 1998.

QIN Yong-yuan, ZHANG Hong-yue, WANG Shu-hua. Kalman filter and integrated navigation principle[M]. Xi’an: Northwestern Polytechnical University Press, 1998.

[2]韩子鹏.弹箭外弹道学[M].北京:北京理工大学出版社, 2014.

HAN Zi-peng. Exterior ballistics of rockets[M]. Beijing: Beijing Institute of Technology Press, 2014.

[3]李岩, 任睿, 王旭刚. 两种卡尔曼滤波模型在修正弹弹道数据处理中的应用比较[J].弹道学报, 2011, 23(1):27-30.

LI Yan, REN Rui,WANG Xu-gang. Application comparison of two Kalman filtering models used in trajectory data processing of correction projectile[J]. Journal of Ballistics, 2011, 23(1):27-30.

[4]闫小龙, 陈国光, 杨东. 抗野值卡尔曼滤波在火箭弹落点估计中的应用[J].弹箭与制导学报, 2016, 36(3):94-98.

YAN Xiao-long, CHEN Guo-guang, YANG Dong. Application of Kalman filter restraining outliers in estimation of rockets impact point[J]. Journal of Projectiles, Rockets, Missiles and Guidance, 2016, 36(3):94-98.

[5]吴汉洲, 宋卫东, 徐敬青. 基于多项式拟合的扩展卡尔曼滤波算法[J].计算机应用, 2016,36(5):1455-1457+1463.

WU Han-zhou, SONG Wei-dong, XU Jing-qing. Ext-ended Kalman filtering algorithm based on polynomial fitting[J]. Journal of Computer Applications, 2016, 36(5):1455-1457+1463.

[6]Yan X L, Chen G G, Bai D Z. Application of adaptive Kalman filter in rocket impact point estimation[J]. Journal of Measurement Science and Instrumentation, 2015, 6(3):212-217.

[7]余小琴, 沈文苗. 扩展卡尔曼滤波算法初值选取方法[J].声学与电子工程, 2012(1): 12-13+17.

YU Xiao-qin, SHEN Wen-miao. Extended Kalman filter algorithm initial value selection method[J]. Acoustics and Electronics Engineering, 2012(1):12-13+17.

[8]秦伟, 田晓丽, 刘超. 基于卡尔曼滤波的导弹姿态最优控制法[J].机械工程与自动化,2016(1):183-185.

QIN Wei, TIAN Xiao-li, LIU Chao. Kalman filter based optimal control approach for attitude control of a missile[J]. Mechanical Engineering and Automation, 2016(1): 183-185.