CN108226887B - 一种观测量短暂丢失情况下的水面目标救援状态估计方法 - Google Patents

Abstract

一种观测量短暂丢失情况下的水面目标救援状态估计方法，涉及水面目标救援跟踪技术领域。本发明的目的是为了提高观测量短暂丢失时的水面目标救援实时性和准确性。技术要点：建立基于历史量测的状态转移模型；设计历史测量数据量M的自适应调整策略；基于状态转移模型和M值自适应调整策略建立观测量短暂丢失情况下的目标状态估计系统模型；基于Chapman‑Kolmogorov方程的权值自适应更新策略设计改进高斯混合容积卡尔曼滤波，以克服常规高斯混合容积卡尔曼滤波器在观测量丢失时高斯分量权值保持不变的问题。在观测量正常时采用常规高斯混合容积卡尔曼滤波，当观测量短暂丢失时，加入高斯分量权值自适应调整策略，提高状态估计精度。

Description

一种观测量短暂丢失情况下的水面目标救援状态估计方法

技术领域

本发明涉及水面目标救援跟踪技术领域，具体涉及水面目标救援状态估计方法。

背景技术

当失事潜艇由于事故在海面上失去动力时，会随风、浪、流等环境因素漂浮运动，为了保证潜艇人员及财产安全，需及时对其进行救援。救援船通过雷达以及安装在失事潜艇上的应答器来实时获得潜艇的位姿数据，进而对其进行状态估计。在实际的海洋环境中，由于传感器与应答器存在一定的安装高度差，以及传感器的扫描范围有限，在四级海况下，考虑到救援船与目标艇的相对运动，包括横摇和升沉，可能会出现观测量短暂丢失的情况，观测量丢失对救援作业的安全实施会造成巨大威胁。因此，研究观测量短暂丢失时的水面目标救援状态估计方法具有非常重要的工程意义。

现有的文献中介绍了很多针对观测量缺失条件下的目标状态估计方法，例如《Kalman Filtering with Intermittent Observations》(IEEE Transactions onAutomatic Control,2004,49(9))、《On the performance of unscented Kalman filterwith intermittent observations》(proceedings of the IEEE InternationalConference on Industrial Informatics,2014)和《Modified Extended KalmanFiltering for Tracking with Insufficient and Intermittent Observations》(Mathematical Problems in Engineering,2015)等，但是这些方法都有一个共同点，即当观测量缺失时，系统只利用目标的运动模型对目标状态进行预测。这是建立在目标运动模型准确的前提上的，但是在应用中尤其是基于雷达的目标状态估计，由于缺乏被跟踪目标和跟踪环境的先验知识，如果目标模型参数与实际情况相差较大，就会造成目标状态估计算法性能急剧下降，甚至无法正常工作，这样就无法为水面目标跟踪控制系统提供准确的控制输入，这对于水面目标救援而言是非常危险的。

发明内容

本发明的目的是提出一种观测量短暂丢失情况下的水面目标救援状态估计方法，以提高观测量短暂丢失时的水面目标救援实时性和准确性。

本发明为解决上述技术问题采取的技术方案是：

一种观测量短暂丢失情况下的水面目标救援状态估计方法，所述方法的具体过程为：

步骤1、基于多项式拟合原理，建立基于历史量测的状态转移模型；

步骤2、步骤1建立的状态转移模型中的关键问题是历史测量数据量M的选取，基于拟合误差设计M的自适应调整策略；

步骤3、基于步骤1的状态转移模型和步骤2的M值自适应调整策略，建立观测量短暂丢失情况下的目标状态估计系统模型；

步骤4、基于Chapman-Kolmogorov方程的权值自适应更新策略设计改进高斯混合容积卡尔曼滤波；为了解决常规高斯混合容积卡尔曼滤波器在观测量丢失时高斯分量权值保持不变的问题；

步骤5、在所述目标状态估计系统模型的基础上采用改进高斯混合容积卡尔曼滤波对观测量短暂丢失条件下的水面目标进行状态估计。

进一步地，步骤1中基于历史量测的状态转移模型为：

采用从当前点向前取M个连续的历史量测数据来确定多项式的拟合系数，进而建立目标的状态转移模型：

其中m为索引序号，x(m)表示第m个历史量测数据，p(m)表示利用多项式拟合获得的历史量测拟合值，W_x(m)表示状态转移模型的噪声。

进一步地，在步骤2中，M的自适应调整策略为(即M是如何调整的)：

令状态转移模型初始化时选取的量测拟合数据量为M_min个，X_M(m)表示拟合数据中的第m个历史量测值，拟合后得出的模型为g_M(X)，其对应的拟合估计值为g_M(X,m)，则拟合估计误差为

令均方根拟合误差为ε_M，则：

最小的量测数据拟合误差ε_min可表示为：

根据图6所示流程图循环计算，直到以下条件被满足：

ε_M＞κε_min (6)

其中κ表示门限调整系数；当满足式(6)时，停止循环计算，当前的拟合历史量测数据量个数为M-1，认为在最近的M-1次测量中，目标做的运动是相对稳定的。

进一步地，步骤3中的目标状态估计系统模型的建立过程为：

对救援船与失事目标的横向距离X进行一阶差分，并且利用二阶多项式进行拟合逼近后可以得到：

这种根据已有的雷达观测量进行状态转移模型建立的方法，能够比较准确地跟踪目标状态的变化，可以适用于非机动和慢机动等多种目标运动状态；由于采用了二阶多项式，使得模型结果较为简单，多项式参数可以根据选择的数据量实现实时估计，不需要其他模型中所需的多种参数，使用比较简单方便。

由式(7)可得出观测量短暂缺失时的目标状态估计系统模型为

其中

表示前面介绍的目标状态自适应拟合值，

分别表示救援船与失事目标的相对横向距离、速度及加速度的变化量；

分别表示救援船与失事目标的相对纵向距离、速度及加速度的变化量；其上波浪表示相对的意思；

m为索引序号；x_k为k时刻的状态量，表示失事目标当前位姿估计值；z_k为k时刻的观测量，表示救援船对失事目标的测量值；F表示状态转移矩阵，h(x_k)表示观测函数；w_k-1表示过程噪声；v_k表示观测噪声；

γ＝0时，观测量无缺失情况；γ＝1表示观测量缺失；

表示救援船与失事目标的相对的横向距离，

表示救援船与失事目标的相对的纵向距离。

在已有的文献中，对于观测量缺失时的观测方程的建模都是基于观测量缺失服从伯努利分布，基于这种思想假设γ表示与观测量缺失相关联的系数，令γ＝0时，观测量无缺失情况；γ＝1表示观测量缺失。

进一步地，基于Chapman-Kolmogorov方程设计高斯混合容积卡尔曼滤波器中高斯分量权值的自适应更新策略，过程如下：

假设目标状态估计系统的初始条件概率密度函数为p(x₀)，给出所述系统状态概率密度函数的高斯分量近似为：

表示时间更新阶段的权值；

表示均值为x_k(i)、协方差为P_k(i)的第i个高斯分量的值；

根据Chapman-Kolmogorov方程可得出系统状态的实际概率密度函数为：

表示n维的实数域；

利用最小二乘法对式(9)表示的概率密度函数近似值和式(10)表示的概率密度函数实际值求解均方最优可得：

其中w_k|k-1＝[ω_k|k-1(1) ω_k|k-1(2) ... ω_k|k-1(I)]^T表示k时刻各高斯分量的权值向量，并且

为了求解式(11)，给出其代价函数：

其中第一项

表示实际概率密度函数的自相关性，该项主要表示不确定性传递误差的整体趋势，(因此在更新过程中可以不需要考虑)第二项

表示实际概率密度函数和近似值的互相关性；最后一项

表示近似概率密度函数的自相关性；

假设k-1时刻的高斯混合近似概率密度函数与真实值相等，令

即

对代价函数中的每一项进行推导计算可得：

Q_k表示过程噪声的协方差矩阵，f(x_k-1)表示状态转移函数；

同理可得：

其中每一项矩阵的元素可计算为：

其中(ω_c,ξ_c)表示容积点的权值和容积点值；式(18)的计算依据是式(10)介绍的chapman-Kolmogorov方程；式(19)的计算依据是两个高斯密度函数的乘积仍为高斯密度函数；

通过上面的计算，可得出式(11)的计算结果：

式(20)的结果表示的是分量权值更新后的结果。

进一步地，根据所述的高斯混合容积卡尔曼滤波器中高斯分量权值的自适应更新策略，设计改进高斯混合容积卡尔曼滤波，其过程为：

(1)状态初始化

对所述系统状态、协方差进行初始化，

P₀＝cov(x₀)：

(2)时间更新

对k-1时刻的状态误差协方差矩阵进行Cholesky分解：

其中r＝(i-1)I+j，I表示系统状态分解为高斯分量的个数，J表示系统过程噪声分解成的高斯分量个数；j取值从1到J；

计算容积点

利用状态方程计算容积点预测估计值：

ξ_c,k|k-1(i)＝Fξ_c,k-1|k-1(i) (24)

k时刻系统状态的预测估计值为：

项中的m表示容积点个数，γΔx_m项中的m索引序号；

k时刻系统状态误差协方差矩阵预测估计值为：

k时刻高斯分量权值计算如下：

表示时间更新以后的权值；

表示系统过程噪声分解成的高斯分量的权值；

其中

的计算方式为

为了提高计算效率以满足实时目标跟踪的需要，本发明设计的改进高斯混合容积卡尔曼滤波器将在观测量丢失时，调用高斯分量权值自适应调整策略；当观测量未丢失时，仍将采用正常的高斯混合容积卡尔曼滤波器。

(3)量测更新

对式(26)的状态误差协方差矩阵预测值进行Cholesky分解：

计算容积点：

利用观测方程对容积点进行传递：

ε_c,k|k-1(r)＝h(ξ_c,k|k-1(r)) (31)

计算k时刻的观测预测值：

计算k时刻观测预测自相关协方差矩阵：

R_k(l)表示观测噪声的协方差矩阵；

计算k时刻互相关协方差矩阵：

计算k时刻的卡尔曼增益：

计算k时刻的状态更新值：

计算k时刻状态误差协方差矩阵：

其中n＝(r-1)L+l；

L表示系统观测噪声分解成的高斯分量个数；l取值从1到L；

(4)计算滤波器估计输出

k时刻的系统状态和协方差的估计值分别为：

其中ω_k(n)表示第n个高斯分量的权值，在每次迭代时根据下式计算：

式中，

表示观测噪声第l个高斯分量的权值，p(z_k|x_k,n)表示第n个高斯分量的观测似然分布，其计算方式可根据式获得：

σ_n表示高斯分布的标准差；

(5)高斯分量缩减合并

缩减后的第I个高斯分量的权值为：

其均值和协方差分别为：

其中

表示被缩减的高斯分量的归一化权值。

本发明相对于常规观测量丢失时的状态估计方法有如下优点：

1.提出基于历史量测数据的状态转移模型，相比于传统的状态转移模型，本发明在观测量短暂丢失时，不单纯利用模型预测，而是加入了历史数据的影响。

2.提出历史数据拟合滑窗长度自适应调整策略，相比固定长度的滑窗宽度，本发明能够在状态估计过程根据历史量测拟合误差不断自适应调整滑窗长度，可以在误差较小时增大滑窗长度，降低计算量；在误差较大时减小滑窗长度，提高拟合精度。

3.常规高斯混合滤波在状态估计过程中保持高斯分量权值不变，这一特点在观测量丢失时会造成估计精度降低的问题，本发明基于Chapman-Kolmogorvo方程设计了高斯分量权值自适应调整策略，在观测量正常时采用常规高斯混合容积卡尔曼滤波，当观测量短暂丢失时，加入高斯分量权值自适应调整策略，提高状态估计精度。

本发明在观测量短暂丢失时，在原有状态转移模型的基础上，加入了基于历史观测数据拟合预测，即考虑了过去一段时间内目标的运动状态，以此来预测未来观测量丢失时间内的目标运动趋势。

文献《观测数据不完全的动态定位算法研究》(中南大学，2011)针对GPS动态定位中出现的观测数据丢失问题，设计了几种基于卡尔曼滤波的补偿预测算法，而本发明则是基于高斯混合容积卡尔曼滤波器，可以应用于实际观测数据并不符合理想高斯分布的情况，并且本发明对拟合数据滑窗长度设计了自适应调整策略，可以在提高拟合精度的同时降低计算量。文献《带有量测数据丢失的虚观测鲁棒卡尔曼滤波算法》(华中科技大学学报(自然科学版),2016 44(9))针对传感器量测数据丢失的卡尔曼滤波问题进行了研究，应用趋势移动平均法来预估构造丢失数据时的虚观测值，建立等效的传感器观测模型，但该文献利用的拟合历史数据同样是固定长度，本发明设计了历史数据拟合长度自适应调整策略，可以在估计过程中自适应调整拟合数据的长度，当拟合精度较高时，可以增加数据滑窗的长度，降低计算量，进而提高状态估计算法的实时性，这在实际应用中是非常重要的。

附图说明

图1观测量短暂丢失时的目标状态估计流程图，

图2雷达观测量短暂丢失示意图，

图3红色应答器的距离和方位测量丢失图，

图4测量丢失局部放大图，

图5不同M值时距离和方位拟合曲线，

图6滑窗自适应调整流程图，

图7滑窗宽度与拟合误差关系图，

图8滑窗宽度自适应调整图，

图9位置拟合值与估计值曲线，

图10速度拟合值与估计值曲线，

图11位置拟合均方误差，

图12速度拟合均方误差，

图13观测量丢失时目标状态估计曲线，

图14北向位置均方根误差曲线，

图15东向位置均方根误差曲线，

图16北向速度均方根误差曲线，

图17东向速度均方根误差曲线。

具体实施方式

本发明的观测量短暂丢失情况下的水面目标救援状态估计的具体实施方式如下：

步骤1：基于历史量测的状态转移模型设计

目标艇在风、浪、流共同作用下的运动属于慢机动状态。而且微波雷达的测量周期为一秒，不能够直接得到目标的具体机动方式，只能够得到目标机动后的结果。因此，本发明将研究如何利用雷达量测数据建立慢机动目标状态转移模型，并将其应用于雷达量测数据丢失情况下的目标状态估计方法设计中。

目标在二维平面直角坐标系内的位置信息x(t)可以通过基于时间t的某种函数来进行拟合，进而通过p(t)来建立目标的状态估计模型：

x(t)＝p(t)+w(t) (1)

其中p(t)表示x(t)的拟合函数，w(t)表示拟合的误差。对于拟合函数p(t)的选择，比较常用的是多项式拟合p(t)＝a₀+a₁t+...+a_kt^k，其中a_i，(i＝0,...,k)是特定的多项式系数，根据实际的数据来确定。根据前面的介绍，水面目标漂浮运动属于慢机动状态，所以采用较低阶多项式就可以得到比较理想的拟合效果。

假设量测数据序列为x(n),n＝1,2,...,N，对其每一项进行差分处理，则其结果可以表示为：

Δx(n)＝x(n+1)-x(n) (2)

用多项式函数p(n)对测量数据的差分函数进行拟合，则Δx(n)可以重新表示为：

其中

表示Δx(n)的拟合估计值，ε(n)表示拟合误差，因此x(n+1)的拟合估计值

可以表示为：

假设测量数据的采样时间为T，则n＝t/T，t为T的整数倍。式(4)两边对时间t求导：

则状态估计模型的离散形式可以表示为：

基于多项式的量测数据拟合方法需要一定数量的量测数据才能实现对其拟合逼近。为了能够比较实时准确地反应目标的状态变化，在实际应用中一般是采用从当前点向前取M个连续的量测数据来确定多项式的拟合系数，进而建立目标的状态转移预测方程，即限定记忆数据建模。为了与式(6)容易区分，将模型中的索引序号改为m，则式(6)可以改写为：

其中用多项式拟合估计p(m)来替换

步骤2：观测量拟合滑窗M自适应调整策略

令系统初始化时选取的量测拟合数据量为M_min个，X_M(m)表示拟合数据中的第m个量测，拟合后得出的模型为g_M(X)，其对应的拟合估计值为g_M(X,m)，则拟合估计误差为

令均方根拟合误差为ε_M，则：

最小的量测数据拟合误差ε_min可表示为：

根据图6所示流程图循环计算，直到以下条件被满足：

ε_M＞κε_min (11)

其中κ表示门限调整系数。当满足式(11)时，停止循环计算，当前的拟合量测数据量个数为M＝M-1，可以近似认为在最近的M次测量中，目标做的运动时相对稳定的。

步骤3：观测量短暂丢失时状态估计系统模型设计：

雷达对目标的量测为极坐标的形式，为了便于计算将其转换到笛卡尔坐标系下。这样就可以得到目标在救援船船体坐标系下横向和纵向的位置信息。这里分别对目标的横向和纵向位置信息进行分析建模。

基于前面的仿真假设，目标属于慢机动状态，为了便于计算，用二阶多项式函数来对目标运动状态进行拟合分析。采用的拟合多项式阶数越高，越能反应目标运动的复杂程度，但是计算量就会越大，可能导致拟合产生较大的滞后。

对横向距离X进行一阶差分，并且利用二阶多项式进行拟合逼近后可以得到：

这种根据已有的雷达观测量进行状态转移模型建立的方法，能够比较准确地跟踪目标状态的变化，可以适用于非机动和慢机动等多种目标运动状态。由于采用了二阶多项式，使得模型结果较为简单，多项式参数可以根据选择的数据量实现实时估计，不需要其他模型中所需的多种参数，使用比较简单方便。

可以得出观测量短暂缺失时的目标状态估计系统模型为

其中

表示前面介绍的目标状态自适应拟合值。在已有的文献中，对于观测量缺失时的观测方程的建模都是基于观测量缺失服从伯努利分布，基于这种思想假设γ表示与观测量缺失相关联的系数，令γ＝0时，观测量无缺失情况；γ＝1表示观测量缺失。

步骤4：高斯分量权值自适应调整策略设计

假设非线性系统的初始条件概率密度函数为p(x₀)，给出系统状态概率密度函数的高斯分量近似为：

根据Chapman-Kolmogorov方程可以得出系统状态的实际概率密度函数为：

利用最小二乘法对式(14)表示的概率密度函数近似值和式(15)表示的概率密度函数实际值求解均方最优可得：

其中w_k|k-1＝[ω_k|k-1(1) ω_k|k-1(2) ... ω_k|k-1(I)]^T表示k时刻各高斯分量的权值向量，并且

为了求解式(16)，给出其代价函数：

其中第一项

表示实际概率密度函数的自相关性，该项主要表示不确定性传递误差的整体趋势，因此在更相信过程中可以不需要考虑。第二项

表示实际概率密度函数和近似值的互相关性。最后一项

表示近似概率密度函数的自相关性。

假设k-1时刻的高斯混合近似概率密度函数与真实值相等，令

即

对代价函数中的每一项进行推导计算可得：

同理可得：

其中每一项矩阵的元素可计算为：

其中(ω_c,ξ_c)表示容积点的权值和容积点值。式(23)的计算依据是式(15)介绍的chapman-Kolmogorov方程；式(24)的计算依据是两个高斯密度函数的乘积仍为高斯密度函数。

通过上面的计算，可以得出式(16)的计算结果：

式(25)的结果表示的是分量权值更新后的结果。

步骤5：基于改进高斯混合容积卡尔曼滤波的水面目标状态估计方法设计

根据前述设计的目标状态转移模型和自适应权值更新策略来设计用于观测量缺失条件下的目标状态估计滤波器。针对观测量缺失时的目标状态估计模型(20)，基于改进混合容积卡尔曼滤波的目标状态估计方法具体步骤如下：

(1)状态初始化

对系统状态，协方差等进行初始化，

P₀＝cov(x₀)：

(2)时间更新

对k-1时刻的状态误差协方差矩阵进行Cholesky分解：

其中r＝(i-1)I+j，I表示系统状态分解为高斯分量的个数，J表示系统过程噪声分解成的高斯分量个数。

计算容积点

利用状态方程计算容积点预测估计值：

ξ_c,k|k-1(i)＝Fξ_c,k-1|k-1(i) (28)

k时刻系统状态的预测估计值为：

k时刻系统状态误差协方差矩阵预测估计值为：

k时刻高斯分量权值计算如下：

其中

的计算方式为

为了提高计算效率以满足实时目标跟踪的需要，本发明设计的改进高斯混合容积卡尔曼滤波器将在观测量丢失时，调用高斯分量权值自适应调整策略；当观测量未丢失时，仍将采用正常的高斯混合容积卡尔曼滤波器。

(3)量测更新

对式(30)的状态误差协方差矩阵预测值进行Cholesky分解：

计算容积点：

利用观测方程对容积点进行传递：

ε_c,k|k-1(r)＝h(ξ_c,k|k-1(r)) (35)

计算k时刻的观测预测值：

计算k时刻观测预测自相关协方差矩阵：

计算k时刻互相关协方差矩阵：

计算k时刻的卡尔曼增益：

计算k时刻的状态更新值：

计算k时刻状态误差协方差矩阵：

其中n＝(r-1)L+l。

(4)计算滤波器估计输出

k时刻的系统状态和协方差的估计值分别为：

其中ω_k(n)表示第n个高斯分量的权值，在每次迭代时根据式(44)计算：

式中，

表示观测噪声第l个高斯分量的权值，p(z_k|x_k,n)表示第n个高斯分量的观测似然分布，其计算方式可根据式获得：

(5)高斯分量缩减合并

缩减后的第I个高斯分量的权值为：

其均值和协方差分别为：

其中

表示被缩减的高斯分量的归一化权值。

数值仿真验证

将本发明提出的观测量缺失条件下的目标状态估计方法进行仿真验证：

仿真内容分为两部分，第一部分为拟合滑窗宽度自适应调整仿真实验；第二部分为观测量缺失时的目标状态估计方法仿真实验，并将其常规GM-CKF进行观测量缺失条件下的估计精度对比，采用的数据集是失事目标艇自由漂浮轨迹，其他仿真参数设置如下。

由于目标艇在水面自由漂浮属于慢机动状态，可以假设目标的加速度是一个平稳的、事件相关的随机过程，因此在本发明的仿真实验中，将采用Singer模型来进行目标运动状态的传递和估计。假设目标运动加速度的时间相关函数为：

R(τ)＝E[a(τ)a(t+τ)]＝σ²e^-α|τ| (49)

其中τ表示目标机动相关时间变量；a(t)表示目标的机动加速度，σ²表示目标机动的加速度方差。α表示目标机动加速度时间常数的倒数。

采用Singer模型的前提是假设认为目标的加速度服从均匀分布，目标的加速度方差可以表示为：

其中a_max表示目标机动的最大加速度，P_max表示a_max出现的概率，-a_max出现的概率也是P_max，P₀表示加速度为0时的概率。可以看出加速度的方差主要取决与a_max,P_max,P₀。

针对目标状态估计系统方程，令目标状态向量

包括目标在二维平面内的位置，速度和加速度信息，则目标状态估计模型可以表示为：

其中α表示X方向的加速度信息，β表示Y方向的加速度，过程噪声协方差矩阵的各项元素可表示为：

在本文中将非高斯观测噪声建模为闪烁噪声。闪烁噪声一般可以建模为高斯噪声和拉普拉斯噪声的混合形式或者多个不同协方差高斯分量的混合。在本文的仿真中，为了简单起见，将其建模为两个不同协方差的高斯噪声的加和，其表达形式为：

p(v)＝(1-ε)N(v；u₁,R₁)+εN(v；u₂,R₂) (52)

其中观测噪声的协方差为u₁＝u₂＝0，R₁＝R，R₂＝100R₁，R＝diag(10,10)闪烁系数ε＝0.1。

根据式(11)确定拟合滑窗的长度，令ε_min＝0.5，即当位置拟合误差ε_M＞0.5时，滑窗长度为M＝M-1。

为了确保观测量的可靠性，假设仿真初始对目标的观测可以被连续三次观测到。因此可以利用前三个时刻的观测数据来确定滤波器的初始值，即

在仿真中设置了三段观测量缺失的时间段，分别为(250～300)s，(450～520)s和(650～700)s。在上述三段时间内，令式(13)中的γ＝1，则会在观测量缺失时利用本文提出的自适应状态转移模型对目标状态进行预测，同时对高斯分量的权值进行自适应更新

从图8中可以看出，根据图7的仿真结果，滑窗宽度初始化为16，随着仿真的进行，滑窗宽度进行自适应调整，到实验后期，滑窗宽度变大，这是由于目标艇在受风浪流影响下自由漂浮运动进入稳定阶段，做近似于匀速直线运动。对于目标状态的估计也进入误差收敛阶段，因此拟合误差变小，滑窗宽度相应变大。

图9和图10分别表示当滑窗长度为16时对目标位置和速度估计值的拟合结果，图11和图12分别表示对目标位置和速度的拟合误差，从图中可以看出当滑窗长度为16时，对于位置的拟合误差能够保持在0.5米以内。

图13给出了观测量缺失条件下本发明提出的IGM-CKF目标状态估计算法与GM-CKF算法的估计轨迹曲线。从图中可以看出，当观测量正常时，两种算法的估计轨迹一致，当观测量缺失时，GM-CKF则会出现较大偏差。在仿真中设置了3段观测量丢失时间，而且图中的GM-CKF相对应的出现了3段估计轨迹偏离较大的阶段。而本发明提出的基于自适应状态转移模型和自适应高斯分量权值更新的IGM-CKF则能够在观测量丢失时保持较高的估计精度。

图14和图15分别给出了两种方法在北向和东向的位置RMSE，从图中可以看出，当观测量丢失时，未加自适应状态转移模型的GM-CKF算法的估计误差直线增大，这是因为该方法仅仅利用常规的状态转移模型对目标状态进行预测，类似于常规的航位推算法，因此会出现累计误差不断增大的情况。当观测量正常时，由于有了观测更新，则会出现跳变，从较大误差直接跳变到正常的目标状态估计。而本发明提出的基于自适应状态转移模型和自适应高斯分量权值更新的IGM-CKF则能够在观测量丢失时，根据已有观测量得出的目标运动趋势，对目标状态进行预测，提高了预测估计的精度。

图16和图17分别给出了观测量缺失时不同方法在北向和东向对目标速度的估计均方差曲线。从图中可以看出，在观测量缺失时，两种方法在北向速度估计均方根误差差别不大，本发明提出的改进的估计方法在估计精度上稍有提高，这是因为本文所使用的失事艇漂浮状态在北向上的速度比较低，因此估计误差也会比较小。在东向速度估计方面，由于使用的目标模型中包含加速度项，因此为改进的估计方法的速度会呈现出逐渐增加的趋势，而本发明提出的改进的估计方法在观测量丢失时能够将估计均方根误差保持在一个较低的范围。

Claims (5)

1.一种观测量短暂丢失情况下的水面目标救援状态估计方法，其特征在于，所述方法的具体过程为：

步骤1、基于多项式拟合原理，建立基于历史量测的状态转移模型；

步骤2、步骤1建立的状态转移模型中的关键问题是历史测量数据量M的选取，基于拟合误差设计M的自适应调整策略；

M的自适应调整策略为：

令状态转移模型初始化时选取的量测拟合数据量为M_min个，X_M(m)表示拟合数据中的第m个历史量测值，拟合后得出的模型为g_M(X)，其对应的拟合估计值为g_M(X,m)，则拟合估计误差为

令均方根拟合误差为ε_M，则：

最小的量测数据拟合误差ε_min可表示为：

根据循环计算，直到以下条件被满足：

ε_M＞κε_min (6)

其中κ表示门限调整系数；当满足式(6)时，停止循环计算，当前的拟合历史量测数据量个数为M-1，认为在最近的M-1次测量中，目标做的运动是相对稳定的；

步骤3、基于步骤1的状态转移模型和步骤2的M值自适应调整策略，建立观测量短暂丢失情况下的目标状态估计系统模型；

步骤4、基于Chapman-Kolmogorov方程的权值自适应更新策略设计改进高斯混合容积卡尔曼滤波；

步骤5、在所述目标状态估计系统模型的基础上采用改进高斯混合容积卡尔曼滤波对观测量短暂丢失条件下的水面目标进行状态估计。

2.根据权利要求1所述的一种观测量短暂丢失情况下的水面目标救援状态估计方法，其特征在于：步骤1中基于历史量测的状态转移模型为：

采用从当前点向前取M个连续的历史量测数据来确定多项式的拟合系数，进而建立目标的状态转移模型：

其中m为索引序号，x(m)表示第m个历史量测数据，p(m)表示利用多项式拟合获得的历史量测拟合值，W_x(m)表示状态转移模型的噪声。

3.根据权利要求2所述的一种观测量短暂丢失情况下的水面目标救援状态估计方法，其特征在于：步骤3中的目标状态估计系统模型的建立过程为：

对救援船与失事目标的横向距离X进行一阶差分，并且利用二阶多项式进行拟合逼近后可以得到：

由式(7)可得出观测量短暂缺失时的目标状态估计系统模型为

其中

表示前面介绍的目标状态自适应拟合值，

分别表示救援船与失事目标的相对横向距离、速度及加速度的变化量；

分别表示救援船与失事目标的相对纵向距离、速度及加速度的变化量；其上波浪表示相对的意思；

m为索引序号；x_k为k时刻的状态量，表示失事目标当前位姿估计值；z_k为k时刻的观测量，表示救援船对失事目标的测量值；F表示状态转移矩阵，h(x_k)表示观测函数；w_k-1表示过程噪声；v_k表示观测噪声；

γ＝0时，观测量无缺失情况；γ＝1表示观测量缺失；

表示救援船与失事目标的相对的横向距离，

表示救援船与失事目标的相对的纵向距离。

4.根据权利要求1、2或3所述的一种观测量短暂丢失情况下的水面目标救援状态估计方法，其特征在于：

基于Chapman-Kolmogorov方程设计高斯混合容积卡尔曼滤波器中高斯分量权值的自适应更新策略，过程如下：

假设目标状态估计系统的初始条件概率密度函数为p(x₀)，给出所述系统状态概率密度函数的高斯分量近似为：

表示时间更新阶段的权值；

表示均值为x_k(i)、协方差为P_k(i)的第i个高斯分量的值；

根据Chapman-Kolmogorov方程可得出系统状态的实际概率密度函数为：

表示n维的实数域；

利用最小二乘法对式(9)表示的概率密度函数近似值和式(10)表示的概率密度函数实际值求解均方最优可得：

其中w_k|k-1＝[ω_k|k-1(1) ω_k|k-1(2) ... ω_k|k-1(I)]^T表示k时刻各高斯分量的权值向量，并且

为了求解式(11)，给出其代价函数：

其中第一项

表示实际概率密度函数的自相关性，该项主要表示不确定性传递误差的整体趋势，第二项

表示实际概率密度函数和近似值的互相关性；最后一项

表示近似概率密度函数的自相关性；

假设k-1时刻的高斯混合近似概率密度函数与真实值相等，令

即

对代价函数中的每一项进行推导计算可得：

Q_k表示过程噪声的协方差矩阵，f(x_k-1)表示状态转移函数；

同理可得：

其中每一项矩阵的元素可计算为：

其中(ω_c,ξ_c)表示容积点的权值和容积点值；式(18)的计算依据是式(10)介绍的chapman-Kolmogorov方程；式(19)的计算依据是两个高斯密度函数的乘积仍为高斯密度函数；

通过上面的计算，可得出式(11)的计算结果：

式(20)的结果表示的是分量权值更新后的结果。

5.根据权利要求4所述的一种观测量短暂丢失情况下的水面目标救援状态估计方法，其特征在于：根据所述的高斯混合容积卡尔曼滤波器中高斯分量权值的自适应更新策略，设计改进高斯混合容积卡尔曼滤波，其过程为：

(1)状态初始化

对所述系统状态、协方差进行初始化，

P₀＝cov(x₀)：

(2)时间更新

对k-1时刻的状态误差协方差矩阵进行Cholesky分解：

其中r＝(i-1)I+j，I表示系统状态分解为高斯分量的个数，J表示系统过程噪声分解成的高斯分量个数；j取值从1到J；

计算容积点

利用状态方程计算容积点预测估计值：

ξ_c,k|k-1(i)＝Fξ_c,k-1|k-1(i) (24)

k时刻系统状态的预测估计值为：

项中的m表示容积点个数，γΔx_m项中的m索引序号；

k时刻系统状态误差协方差矩阵预测估计值为：

k时刻高斯分量权值计算如下：

表示时间更新以后的权值；

表示系统过程噪声分解成的高斯分量的权值；

其中

的计算方式为

(3)量测更新

对式(26)的状态误差协方差矩阵预测值进行Cholesky分解：

计算容积点：

利用观测方程对容积点进行传递：

ε_c,k|k-1(r)＝h(ξ_c,k|k-1(r)) (31)

计算k时刻的观测预测值：

计算k时刻观测预测自相关协方差矩阵：

R_k(l)表示观测噪声的协方差矩阵；

计算k时刻互相关协方差矩阵：

计算k时刻的卡尔曼增益：

计算k时刻的状态更新值：

计算k时刻状态误差协方差矩阵：

其中n＝(r-1)L+l；

L表示系统观测噪声分解成的高斯分量个数；l取值从1到L；

(4)计算滤波器估计输出

k时刻的系统状态和协方差的估计值分别为：

其中ω_k(n)表示第n个高斯分量的权值，在每次迭代时根据下式计算：

式中，

表示观测噪声第l个高斯分量的权值，p(z_k|x_k,n)表示第n个高斯分量的观测似然分布，其计算方式可根据式获得：

σ_n表示高斯分布的标准差；

(5)高斯分量缩减合并

缩减后的第I个高斯分量的权值为：

其均值和协方差分别为：

其中

表示被缩减的高斯分量的归一化权值。

Images