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

0　引言

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

藤尾追求高尚的文学，想与博学多识的小野交往，常常与小野一起交流文学。她蔑视宗近的妹妹丝子的贤妻良母的意识，敢于放弃封建的女性意识。直接向小野表示自己喜欢小野。

在对照组治疗方案的基础上，再予中药口服治疗，药物组成：熟地黄15 g，山茱萸20 g，山药25 g，白术10 g，补骨脂15 g，枸杞子15 g，当归12 g，赤芍10 g，丹参15 g，延胡索12 g，淫羊藿25 g，牡蛎25 g，牛膝15 g，桑寄生30 g。1剂/d，清水煎取药液300 mL，分早晚2次口服。治疗4周为1个疗程，共3个疗程。

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

1　外弹道滤波模型

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

1.1　滤波状态模型

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

(y)G(vτ)vx (y)G(vτ)vy-g (y)G(vτ)vz

(1)

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

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

则系统状态方程为：

(k)(k)

(2)

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

1.2　滤波量测方程

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

1.2.2 细胞转染 在转染前一天将MDA-MB-231细胞培养于6孔板中，24 h后将质粒按3 μg/孔加入到100 μl无血清的DMEM培养基中混匀。然后加入5 μl的转染试剂，室温静置30 min后，将混合液加入到6孔板中。用5%CO2的培养箱培养4～6 h后换液。培养36～48 h后检测相关蛋白的表达情况。

1.1 资料来源 从2011年1月-2018年2月本院乳腺外科收治的383例行前哨淋巴结活检的原发性乳腺癌患者中筛选研究对象，68例SLN阳性，其中4例为微转移，64例为宏转移。将后者纳入研究对象。64例患者的中位年龄为49(33～89)岁。根据nSLN病理结果将64例患者分为nSLN阳性组和nSLN阴性组，分析对比两组间的临床病理特征。

β=arctan(z/x) ε=arctan()

(3)

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

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

(4)

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

2　Kalman弹道滤波方程

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

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

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

(5)

无醛标签可以贴附在通过无醛认定的纤维板，刨花板，胶合板，细木工板，重组装饰材，单板层积材，集成材，饰面人造板，木质地板，木质墙板，木质门等无醛人造板及其制品上。

Zk=HkXk+Vk

(6)

Z=h(X)(k)

如果被估计状态Xk满足式(5)，对Xk的量测Zk满足式(6)，系统噪声Wk和量测噪声Vk满足式(2)和式(4)，系统噪声Wk方差阵Qk非负定，量测噪声Vk方差阵Rk正定，k时刻的量测为Zk，则Xk的估计按下述方程求解。

量测方程为：

状态的一步预测为：

(7)

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

()

(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得到。

初中的英语语法词法方面首先是词类的介绍，然后是名词的分类、名词的数及其所有格；代词方面有人称代词、物主代词和指示代词；数词包括基数词和序数词；动词有be动词、简单的情态动词和助动词的用法。动词的时态只讲一般现在时、现在进行时和一般过去时，还有简单的介词和连词。句法方面介绍了按照句子用途所分的句子种类，即陈述句、疑问句、祈使句和感叹句。而以陈述句、一般疑问句、特殊疑问句和祈使句为主，讲的都是一些最最基础的语法知识。教材所配的话题都以贴近中学生生活、人们日常生活的题材为主，乐于为学生所接受。在七年级阶段我们可以学习一些简单的句子成分，比如主语、谓语、宾语、定语、状语。

xt=rt·cosβt·cosεt yt=rt·sinεt zt=rt·sinβt·cosεt

(13)

但滤波时刻所得量测数据受随机过程的影响，即每次的量测数据受到量测噪声的影响而上下波动，所以某次的测量值并不能说明问题。引入统计的定义，来计算位置分量x、y、z和速度分量vx、vy、vz的方差δ2，并以3δ作为量测数据波动的范围。以为例，(xt-xt-T)/T2。因为每次量测的过程都为互相独立过程，互不影响，通过量测数据与三方向数据的转换关系，先求出x方向上量测方差在已知的情况下，依据方差的性质和意义，得到同理，可求得δvy、δvz。

资产在评估中的过分夸大或者缩小，都会给整个金融体系运行带来顺周期性风险，怎么来遏制金融机构在资产评估中的随意性，这是国际上很多国家在金融监管中的难题。公允会计计量原则运行不好，容易在金融实践中产生顺周期性，而通过建立科学的公允会计计量来缓释这一评估监管制度带来的顺周期性是各国通行的做法。

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

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

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

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

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

(14)

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

1.4 统计学方法 所有数据经双人核对后录入Excel表格，采用SPSS 16.0统计软件进行分析，计量资料用描述，组间均数比较采用t检验，以P<0.05为差异有统计学意义。

Fmin(x)

(15)

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

(16)

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

(17)

即AX=b。

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

量测预测方程为：

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　多项式阶数与所估速度残差的关系 Fig.1　Relationship between the polynomial order number and the estimated velocity residuals

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

(2)回填土采用3∶7灰土，分层铺摊，每层铺摊后随之耙平，铺土厚度为200～250 mm；采用压路机压实，压实系数应≥0.96。

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

图2　X方向两种估计方法速度滤波曲线 Fig.2　Velocity filtering curve of two estimation methods on X direction

图3　Y方向两种估计方法速度滤波曲线 Fig.3　Velocity filtering curve of two estimation methods on Y direction

图4　Z方向两种估计方法速度滤波曲线 Fig.4　Velocity filtering curve of two estimation methods on Z direction

图5　X方向两种估计方法对滤波的影响 Fig.5　Effect of two estimation methods of X direction on filtering

图6　Y方向两种估计方法对滤波的影响 Fig.6　Effect of two estimation methods of Y direction on filtering

图7　Z方向两种估计方法对滤波的影响 Fig.7　Effect of two estimation methods of Z direction on filtering

从图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.