一种用于高精度弹道实时定轨的抗差加权观测融合平方根UKF滤波方法与流程

本发明属于多传感器跟踪导弹实时定轨技术领域，属于目标跟踪、定位导航等技术领域，具体涉及一种用于高精度弹道实时定轨的抗差加权观测融合平方根ukf滤波方法。

背景技术：

导弹武器是现代高科技的结晶和化身，具有进攻性武器的突出特点，比如射程远、精度高、突防能力强的显著特性，成为了具有超强进攻性和强大威慑力的武器和维持战略平衡的支柱。在导弹武器发展初级阶段，由于导弹射程较近，对外测弹道测量的精度要求较低，通常是单传感器跟踪测量导弹并解算其飞行轨迹。随着导弹试验技术的发展和需要，射程的增加和测量精度要求的提高，一般由多传感器比如多个雷达和多个经纬仪组成测量体系交会测量导弹、运载火箭的飞行轨迹，多目标运动状态估计的精度及其性能比单一传感器要优越得多，因此多传感器数据融合技术成为解决弹道测试技术的首选方案。即使采用多种高精度弹道跟踪体制对导弹进行跟踪测量，但依旧避免不了由于跟踪或测量设备的冲击以及系统故障、环境干扰等原因，导致测量数据严重偏离目标真值，这部分严重偏离真值的数据称为异常，野值的存在会严重污染算法处理结果，估计的弹道参数不准确，降低算法精度。

为了解决这个问题，国内外学者基于抗差估计理论研究了一系列的方法，sengch,linsf,jwodj等人研究了一种新的基于huber的滤波算法，可以实现更准确的估计和更快的收敛，但使得ukf无需求导、m估计抗异常误差能力强等优点未能保留。付心如等人研究了一种抗差自适应ukf导航算法，该算法利用新息序列与残差序列进行实时观测噪声统计特性匹配，并自适应调节滤波增益，提高了ukf的抗差与自适应能力。李婵，张士峰，等研究了一种适用于再入弹道处理的自适应抗差滤波方法，该方法可实现异常值的分离和维纳模型方差的自适应调整，并能有效减弱测量误和动力学模型误差对弹道处理精度的影响。然而，上述滤波算法可能存在由于误差协方差矩阵出现负定导致数值稳定性较差的问题，并且这几类方法针对单传感器跟踪目标，因此不能直接用于多传感器跟踪目标实时定轨中，在弹道数据处理方面存在一定的局限性。

技术实现要素：

本发明针对现有技术的不足，提供一种用于高精度弹道实时定轨的抗差加权观测融合平方根ukf滤波方法。

本发明通过以下技术方案来实现的：

一种用于高精度弹道实时定轨的抗差加权观测融合平方根ukf滤波方法，包括以下步骤：

步骤1，建立多传感器弹道动力学系统模型；

步骤2，初始化，设定初始时刻传感器的状态向量协方差矩阵的平方根初值sⁱ(0)；

步骤3，集中式融合，计算集中式测量融合z⁽⁰⁾(k)，进而得到集中式融合非线性测量函数h⁽⁰⁾和集中式融合测量噪声v⁽⁰⁾(k)；

步骤4，加权观测融合，使用步骤3得到的h⁽⁰⁾和步骤2中的计算集中式融合测量系数矩阵h⁽⁰⁾和非线性逼近函数h(x(k),k)，对h⁽⁰⁾进行满秩分解得到列满秩矩阵m和行满秩矩阵h⁽ⁱ⁾，得到加权观测测量融合值zⁱ(k)。

步骤5：时间更新，根据步骤2中的和sⁱ(0)计算状态方程预测的采样点计算状态融合预测值根据步骤4中的h⁽ⁱ⁾和h(x(k),k)，计算测量方程预测的采样点计算测量融合预测值计算估计融合误差协方差矩阵的平方根分解修正预测值si(k+1|k)；

步骤6：野值辨识，根据步骤5中的和zⁱ(k+1)计算融合残差向量vⁱ，根据融合残差向量viⁱ和融合测量值z⁰的方差的第i个分量σi计算引入的测量融合误差值di，将di与给定门限值比较辨识野值，自适应计算抗差权重因子wi；

步骤7：抗差修正野值，根据步骤6中的vⁱ和wi计算加入抗差权重因子的测量融合残差向量根据重新计算测量噪声等价协方差值对异常值进行修正处理；

步骤8：测量更新，根据步骤7中的计算新息协方差矩阵平方根计算互协方差矩阵计算增益矩阵kⁱ(k+1)；

步骤9：状态和方差矩阵的平方根矩阵更新，根据步骤8中的kⁱ(k+1)，和步骤5中的zⁱ(k+1)，计算k+1时刻的状态估计状态估计误差协方差矩阵的平方根矩阵sⁱ。

与现有技术相比，本发明的有益效果：

目前存在的加权观测融合方法对来自各个传感器的测量数据直接进行融合估计弹道参数，并未考虑由于目标的复杂性和测量机制自身的问题，测量数据可能存在污染的问题。本发明在加权观测融合算法的基础上引入抗差估计理论，根据观测融合值与融合预测值，计算测量融合残差向量，抗差权重因子和融合观测向量等价协方差阵，实时性地实现了异常值的分离与修正，解决了融合过程中由于测量数据存在污染导致弹道数据处理精度下降的问题。同时引入平方根滤波思想，避免了常规ukf中误差协方差矩阵非正值引起的滤波散度问题，实现了滤波技术的提高。

附图说明

图1为本发明的流程图。

图2为本发明的加权观测融合算法流程图。

图3为本发明的抗差算法的流程图。

图4为单个测量设备状态，真实状态，抗差wmf-srukf算法估计轨迹曲线。

具体实施方式

为了使本发明的目的、技术方案和优点更加清楚，下面将结合实施例对本发明作进一步地详细描述，显然，所描述的实施例仅仅是本发明一部分实施例，而不是全部的实施例。基于本发明中的实施例，本领域普通技术人员在没有做出创造性劳动前提下所获得的所有其它实施例，都属于本发明保护的范围。

如图1所示，本发明是一种抗差加权观测融合平方根ukf的弹道实时定轨方法具体描述如下：

(1)建立多传感器弹道动力学系统模型

再入弹道的状态方程为：x(k+1)＝f(x(k),k)+w(k)(1)

各个测量体系的观测方程为：

其中f(.,.)为已知非线性函数，x(k)为第k个时刻系统状态，w(k)为状态噪声，w(k)为零均值、方差阵为qw的白噪声。(x1j,y1j,z1j)为第j个光电经纬仪的坐标，(x2j,y2j,z2j)为第j个雷达的坐标，(x,y,z)为目标的坐标。

(2)初始化，设定初始时刻传感器的状态向量：协方差矩阵的平方根初值

(3)加权观测融合，如图2所示，本发明采用加权观测融合算法对式(1)和式(2)构成的多传感器弹道模型进行融合，具体描述如下：

基于式(2)构建集中式测量融合方程：

z⁽⁰⁾(k)＝h⁽⁰⁾(x(k),k)+v⁽⁰⁾(k)(5)

其中集中式融合非线性测量函数为

h⁽⁰⁾(x(k),k)＝[h^(1)t(x(k),k),h^(2)t(x(k),k),...,h^(l)t(x^(k),k)]^t，集中式融合测量噪声为

v⁽⁰⁾(k)＝[v^(1)t(k),v^(2)t(k),...,v^(l)t(k)]^t。

对h⁽⁰⁾进行μ阶泰勒展开得到集中式融合测量系数矩阵h⁽⁰⁾表示为：

其中式(6)中为第j个测量方程关于x的i阶导数，可表示为：

对h⁽⁰⁾进行满秩分解得到列满秩矩阵m和行满秩矩阵h⁽ⁱ⁾

h⁽⁰⁾＝mh⁽ⁱ⁾(9)

引入状态真值与状态估计值的偏差计算非线性逼近函数：

h(x(k),k)＝[1(δx)^t((δx)²)^t…((δx)^μ)^t]^t(10)

其中式(11)中的计算融合测量噪声协方差矩阵：

v⁽ⁱ⁾(k)＝(m^tr^(0)-1m)^-1m^tr^(0)-1v⁽⁰⁾(k)(11)

得到加权测量融合系统的最优测量方程：

z⁽ⁱ⁾(k)＝h⁽ⁱ⁾h(x(k),k)+v⁽ⁱ⁾(k)(12)

(4)时间更新，

计算状态方程预测的采样点：

状态融合预测值：

计算测量方程预测的采样点

其中

测量融合预测值：

估计融合误差协方差矩阵的平方根分解修正预测值：

其中式(17)的qr指的是qr分解，具体过程为对于矩阵a，找到一个正交矩阵q和一个上三角矩阵r来使得a＝qr。式(18)的cholupdate表示矩阵cholesky分解的修正，即若s为原始矩阵p的cholesky的分解，修正矩阵的cholesky的分解为s＝cholupdate(s,u,±v)。

(5)抗差处理，如图3所示本发明采用抗差算法对数据融合过程中测量值可能出现的异常值进行辨识以及修正，具体描述如下：

计算融合残差向量

计算引入的测量融合误差值

其中σi为融合测量值z⁰的方差的第i个分量，viⁱ为融合残差向量vⁱ的第i个分量。

引入参数k0和k1，其中k0和k1可分别取1.5～3.0和2.5～5.0。当di满足：

①di≤k0时，表示测量数据为正常值，wi＝1，测量值被接受。

②k0＜di≤k1时，表示测量数据为异常值，被剔除，wi＝k0/di，测量值需进行抗差处理，修正异常值。

③di＞k1时，表示测量数据为异常值，被剔除，wi＝10^-3，测量值需进行抗差处理，修正异常值。

构建抗差权重因子向量：w＝[w1w2...wn](21)

计算加入抗差权重因子的融合残差向量：

计算测量噪声等价协方差值：

(6)测量更新，计算新息协方差矩阵平方根，

计算互协方差矩阵：

计算增益矩阵：

(7)状态和方差矩阵的平方根矩阵更新，计算k+1时刻的状态估计：

计算状态估计误差协方差矩阵的平方根矩阵：

uⁱ(k+1)＝kⁱ(k+1)*syⁱ(k+1)(29)

sⁱ＝cholupdate{sⁱ,uⁱ,-1}(30)

综上所述，本发明在加权观测融合方法上结合了抗差理论和平方根滤波思想，相比于其他融合滤波算法，增加了异常值分离与修正，这是一般融合算法所欠缺的，并结合平方根滤波思想，引入了cholesky和qr分解有效地避免误差协方差矩阵为负值引起的滤波器发散，提高收敛速度和稳定性，实现了多个传感器弹道的高精度跟踪。

实施例：

本例应用光电经纬仪与雷达协同工作跟踪导弹，考虑3个测站构成的传感器网络，采用本发明所提出的方法对导弹进行加权融合式实时轨道的确定，以光电经纬仪的坐标为原点建立三维坐标系，其中

雷达1相对站址：x01＝1400,y01＝1350,z01＝1000；

雷达2相对站址：x02＝-2200,y02＝1500,z02＝3400；

雷达3相对站址：x03＝1700,y03＝-2850,z03＝3000；

初始状态为：xi(:,1)＝[3500,1500,1000,-1100,-150,-50,10,10,10]‘，i＝1，2，3

滤波初始状态为：xukf(:,1)＝[3300,1300,950,-1000,-100,-60,0,0,0]'，

各个测量体系的测量噪声的方差分别为

本发明性能测试：直观上通过采用本发明中的方法得到的弹道轨迹曲线与真实轨迹的接近程度进行评价，接近程度越高，本发明估计的弹道参数越准确；数值上通过各个弹道参数分量的均方根误差(rootmeansquarederrorrmse)均值进行评价，rmse越小，精度越高。单个测量设备状态，真实状态，抗差wmf-srukf算法估计轨迹曲线如图4所示，各方法的弹道参数的平均rmse值如表1所示

表1各方法的弹道参数的平均rmse值

由图4可以看出采用本发明提出的抗差加权观测融合平方根ukf的弹道实时定轨方法可以准确的融合各个测量机制的有效信息，估计的弹道轨迹直观上高度近似于真值，同时由表1可以看出，采用本发明所提出的方法估计的各个弹道参数的平均rmse值均最小，算法精度高，本例验证了本发明所提出的弹道实时定轨方法的准确性与有效性。

以上应用了具体个例对本发明进行阐述，只是用于帮助理解本发明，并不用以限制本发明。任何熟悉该技术的人在本发明所揭露的技术范围内的局部修改或替换，都应涵盖在本发明的包含范围之内。