CN104833968A - 一种用于再入弹道目标跟踪的有限差分滤波方法 - Google Patents

Abstract

本发明公开了一种用于再入弹道目标跟踪的有限差分滤波方法，包括以下步骤：在FDEKF的基础上，在预测误差协方差矩阵中引入强跟踪的次优渐消因子及遗忘因子实现对预测误差协方差的修正，再引入Cholesky分解就可以得到目标状态下一步的预测、预测误差协方差矩阵、增益、状态估计及估计误差协方差矩阵，从而实现对目标机动的跟踪，本发明对目标机动的跟踪能力强，跟踪精度高，并且计算量小。

Description

一种用于再入弹道目标跟踪的有限差分滤波方法

技术领域

本发明属于再入阶段的弹道目标跟踪技术领域，涉及一种用于再入弹道目标跟踪的有限差分滤波方法。

背景技术

弹道目标的跟踪，尤其是弹道导弹的跟踪，是弹道防御系统中极其重要的任务，现代战争条件下，弹道导弹以其射程远、精度高、速度快、威力大等优点，成为战争中不可或缺的重要武器，为各国所重视[1]。弹道导弹的跟踪是个相当复杂的问题，它是落点预报及制导拦截的基础。再入段是导弹飞行三个阶段中时间最短的一个阶段，在该阶段，除地球引力外，导弹还受空气动力矩的作用。

对于再入阶段的弹道目标跟踪问题，由于目标运动模型内在的非线性，我们需要使用非线性的滤波技术来估计目标的状态。现有的方法包括扩展卡尔曼滤波(EKF)、无味卡尔曼滤波(UKF)、粒子滤波(PF)及有限差分扩展卡尔曼滤波算法(FDEKF)，EKF算法虽然简单，但其需要计算Jacobia矩阵和/或Hessians矩阵；UKF效果较好，但对高维情况，也较复杂；PF算法精度很高，但计算量相当大；有限差分扩展卡尔曼滤波算法(FDEKF)，很适合对再入弹道目标的跟踪，但对目标机动跟踪能力及跟踪精度较差。

发明内容

本发明的目的在于克服上述现有技术的缺点，提供了一种用于再入弹道目标跟踪的有限差分滤波方法，该方法对目标机动的跟踪能力强，跟踪精度高，并且计算量小。

为达到上述目的，本发明所述的用于再入弹道目标跟踪的有限差分滤波方法包括以下步骤：

在弹道目标跟踪模型中，弹道目标受到空气阻力和地球引力的作用，假设地球为平的，则目标运动的离散时间非线性状态方程为

x_k+1＝Ψ(x_k)+w_k   (1)

其中，k为非负整数，T为连续两个雷达量测的时间间隔，状态向量x_k及y_k为笛卡儿坐标系(x,y)中目标的位置，及为笛卡儿坐标系(x,y)中目标在x轴方向上及y轴方向上的速度，w_k为过程噪声序列，Ψ(x_k)为非线性函数；

在FDEKF算法基础上，在预测误差协方差矩阵中引入强跟踪的次优渐消因子λ(k+1)，则将预测误差协方差修正为：

$\begin{array}{c}{\stackrel{\‾}{P}}_{k+1}=\mathrm{\λ}(k+1)[F_x{\hat{P}}_k{F}_x^T+F_w{Q}_k{F}_w^T]\\ =\mathrm{\λ}(k+1)[F_x{\hat{S}}_x{\hat{S}}_x^T{F}_x^T+F_w{S}_w{S}_w^T{F}_w^T]\\ =\mathrm{\λ}(k+1)[S_{x\hat{x}}{S}_{x\hat{x}}^T+S_{\mathrm{xw}}{S}_{\mathrm{xw}}^T]\end{array}---\left(8\right)$

其中，F_x为非线性函数f_k(x_k,u,w_k)对x_k的一阶偏导数，为_k时刻的估计误差协方差，Q_k为过程噪声协方差矩阵，为的Cholesky分解，F_w为非线性函数f_k(x_k,u,w_k)对w_k的一阶偏导数，S_w为Q_k的Cholesky分解，λ(k+1)为次优渐消因子，其中

$\mathrm{\&lambda;}(k+1)=\left\{\begin{array}{cc}{\mathrm{\&lambda;}}_0,& {\mathrm{\&lambda;}}_0\&GreaterEqual;1\\ 1,& {\mathrm{\&lambda;}}_0<1\end{array}\right.---\left(9\right)$

${\mathrm{\&lambda;}}_0=\frac{\mathrm{tr}\left[N(k+1)\right]}{\mathrm{tr}\left[M(k+1)\right]}---\left(10\right)$

N(k+1)＝V₀(k+1)-βR(k+1)-H(k+1)Q(k)H^T(k+1)   (11)

M(k+1)＝H(k+1)F(k)P(k|k)F^T(k)H^T(k+1)   (12)其中，R(k+1)为残差序列，H(k+1)为量测矩阵，F(k)为非线性函数f(x_k)的雅可比矩阵，P(k|k)为k时刻的估计误差协方差；

V₀(k+1)为残差方差矩阵，且

$\begin{array}{c}{V}_0(k+1)=E\left[r(k+1)r^T(k+1)\right]\\ =\left\{\begin{array}{cc}r\left(1\right)r^T\left(1\right),& k=0\\ \frac{\mathrm{\&rho;}{V}_0\left(k\right)+r(k+1)r^T(k+1)}{1+\mathrm{\&rho;}}& k\&GreaterEqual;1\end{array}\right.\end{array}---\left(13\right)$

r(k+1)为残差序列，r(1)为初始残差，tr[·]表示对矩阵求秩运算，ρ为遗忘因子，且0<ρ≤1；

引入以下Cholesky分解，得：

${\stackrel{\&OverBar;}{S}}_{x\hat{x}}=\left\{{\stackrel{\&OverBar;}{S}}_{x\hat{x}}(i,j)\right\}=\{(f_i(\hat{x}+h{\hat{s}}_{x,j},u,\stackrel{\&OverBar;}{w})-f_i(\hat{x}-h{\hat{s}}_{x,j},u,\stackrel{\&OverBar;}{w}))/2h\}---\left(14\right)$

${\stackrel{\&OverBar;}{S}}_{\mathrm{xw}}=\left\{{\stackrel{\&OverBar;}{S}}_{\mathrm{xw}}(i,j)\right\}=\{(f_i(\hat{x},u,\stackrel{\&OverBar;}{w}+h{s}_{w,j})-f_i(\hat{x},u,\stackrel{\&OverBar;}{w}-h{s}_{w,j}))/2h\}---\left(15\right)$

$\begin{array}{c}{S}_{y\stackrel{\&OverBar;}{x}}=\left\{S_{y\stackrel{\&OverBar;}{x}}(i,j)\right\}\\ =\{(g_i(\stackrel{\&OverBar;}{x}+h{\stackrel{\&OverBar;}{s}}_{x,j},\stackrel{\&OverBar;}{v})-g_i(\stackrel{\&OverBar;}{x}-h{\stackrel{\&OverBar;}{s}}_{x,j},\stackrel{\&OverBar;}{v}))/2h\}\end{array}---\left(16\right)$

$\begin{array}{c}{S}_{\mathrm{yv}}=\left\{S_{\mathrm{yv}}(i,j)\right\}\\ =\{(g_i(\stackrel{\&OverBar;}{x},\stackrel{\&OverBar;}{v}+h{s}_{v,j})-g_i(\stackrel{\&OverBar;}{x},\stackrel{\&OverBar;}{v}-h{s}_{v,j}))/2h\}\end{array}---\left(17\right)$

其中，为矩阵的第i行、第j列的元素；u为状态过程输入，为过程噪声一步预测，h为区间长度参数，为中心差分公式，为量测噪声一步预测；

由式(8)、(14)、(15)、(16)及(17)得目标状态下一步预测预测误差协方差矩阵为：

${\stackrel{\&OverBar;}{x}}_{k+1}=f({\hat{x}}_k,u_k,{\stackrel{\&OverBar;}{w}}_k)---\left(18\right)$

${\stackrel{\&OverBar;}{P}}_{k+1}=\mathrm{\&lambda;}(k+1)[S_{x\hat{x}}{S}_{x\hat{x}}^T+S_{\mathrm{xw}}{S}_{\mathrm{xw}}^T]---\left(19\right)$

其中，为状态估计，u_k为过程输入，为过程噪声一步预测；

目标状态下一步的预测增益K_k+1、状态估计及估计误差协方差矩阵分别为：

${\stackrel{\&OverBar;}{y}}_{k+1}=g({\stackrel{\&OverBar;}{x}}_{k+1},{\stackrel{\&OverBar;}{v}}_{k+1})---\left(20\right)$

$K_{k+1}={\stackrel{\&OverBar;}{S}}_x{S}_{y\stackrel{\&OverBar;}{x}}^T{[S_{y\stackrel{\&OverBar;}{x}}{S}_{y\stackrel{\&OverBar;}{x}}^T+S_{\mathrm{yv}}{S}_{\mathrm{yv}}^T]}^{-1}---\left(21\right)$

${\hat{x}}_{k+1}={\stackrel{\&OverBar;}{x}}_{k+1}+K_{k+1}[y_{k+1}-{\stackrel{\&OverBar;}{y}}_{k+1}]---\left(22\right)$

$\begin{array}{c}{\hat{P}}_{k+1}=\begin{array}{}\end{array}\left[\begin{array}{cc}{\stackrel{\&OverBar;}{S}}_x-K_{k+1}{S}_{y\stackrel{\&OverBar;}{x}}& K_{k+1}{S}_{\mathrm{yv}}\end{array}\right]\\ \&times;{\left[\begin{array}{cc}{\stackrel{\&OverBar;}{S}}_x-K_{k+1}{S}_{y\stackrel{\&OverBar;}{x}}& K_{k+1}{S}_{\mathrm{yv}}\end{array}\right]}^T\end{array}---\left(23\right)$

其中，为系统量测的非线性函数，为预测误差协方差矩阵的乔里斯基分解，K_k+1为增益，y_k+1为量测。

设过程噪声序列w_k是零均值的高斯白噪声，则过程噪声序列w_k的协方差矩阵Q为：

$Q=q\left[\begin{array}{cc}\mathrm{\&Theta;}& 0\\ 0& \mathrm{\&Theta;}\end{array}\right],\mathrm{\&Theta;}=\left[\begin{array}{cc}{T}^3/3& T^2/2\\ T^2/2& T\end{array}\right]---\left(2\right)$

其中，q是过程噪声强度参数。

非线性函数Ψ(x_k)受到空气阻力和地球引力的作用，非线性函数Ψ(x_k)的表达式为：

$\mathrm{\&Psi;}\left(x_k\right)=\mathrm{\&Phi;}{x}_k+\mathrm{Gf}\left(x_k\right)+G\left[\begin{array}{c}0\\ -g\end{array}\right]---\left(3\right)$

其中，g为重力加速度，矩阵Φ和G分别为：

$\mathrm{\&Phi;}=\left[\begin{array}{cccc}1& T& 0& 0\\ 0& 1& 0& 0\\ 0& 0& 1& T\\ 0& 0& 0& 1\end{array}\right],G\left[\begin{array}{cc}{T}^2/2& 0\\ T& 0\\ 0& T^2/2\\ 0& T\end{array}\right]---\left(4\right)$

f(x_k)为代表气动阻力的影响的非线性函数，其中f(x_k)的表达式为：

$f\left(x_k\right)=-0.5\frac{g}{\mathrm{\&beta;}}\mathrm{\&rho;}\left(y_k\right){({\stackrel{\&CenterDot;}{x}}_k^2+{\stackrel{\&CenterDot;}{y}}_k^2)}^{1/2}\left[\begin{array}{c}{\stackrel{\&CenterDot;}{x}}_k\\ {\stackrel{\&CenterDot;}{y}}_k\end{array}\right]---\left(5\right)$

其中，ρ(y_k)是空气密度，β为用来改进状态估计光滑性的弱化因子。

所述遗忘因子ρ＝0.95。

本发明具有以下有益效果：

本发明所述的用于再入弹道目标跟踪的有限差分滤波方法在对目标机动跟踪的过程中，在FDEKF的基础上，在预测误差协方差矩阵中引入强跟踪的次优渐消因子及遗忘因子实现对预测误差协方差的修正，提高目标机动跟踪的精度，再引入Cholesky分解就可以得到目标状态下一步的预测、预测误差协方差矩阵、增益、状态估计及估计误差协方差矩阵，从而实现对目标机动的跟踪，在此过程中，避免计算Jacobia矩阵和/或Hessians矩阵带来的不必要运算，计算过程简单，易于实现。另外，引入遗忘因子进一步对过去的老数据渐消，从而突出最新残差向量的影响，进一步提高强跟踪滤波器的快速跟踪能力。

附图说明

图1(a)验证性试验中目标真实轨线；

图1(b)验证性试验中目标的位置示意图；

图1(c)验证性试验中目标的加速度示意图；

图2为验证性实验中q＝0.1时各方法的位置均方根误差示意图；

图3为验证性实验中q＝0.1时各方法的速度均方根误差示意图；

图4为验证性实验中q＝0.1时本发明与ANEES比较示意图；

图5为验证性实验中q＝1时各方法的位置均方根误差示意图；

图6为验证性实验中q＝1时各方法的速度均方根误差示意图；

图7为验证性实验中q＝1时本发明与ANEES比较示意图；

图8为验证性实验中q＝5时各方法的位置均方根误差示意图；

图9为验证性实验中q＝5时各方法的速度均方根误差示意图；

图10为验证性实验中q＝5时本发明与ANEES比较示意图。

具体实施方式

下面结合附图对本发明做进一步详细描述：

本发明所述的用于再入弹道目标跟踪的有限差分滤波方法包括以下步骤：

在弹道目标跟踪模型，弹道目标收到空气阻力和地球引力的作用，假设地球为平的，则目标运动的离散时间非线性状态方程为

x_k+1＝Ψ(x_k)+w_k   (1)

其中k是非负整数，T为连续两个雷达量测的时间间隔，状态向量x_k及y_k为笛卡儿坐标系(x,y)中目标的位置；及为目标速度；{w_k},k≥0，w_k为过程噪声序列，且k≥0，Ψ(x_k)为非线性函数。

设过程噪声序列是零均值的高斯白噪声，则过程噪声序列的协方差矩阵为：

$Q=q\left[\begin{array}{cc}\mathrm{\&Theta;}& 0\\ 0& \mathrm{\&Theta;}\end{array}\right],\mathrm{\&Theta;}=\left[\begin{array}{cc}{T}^3/3& T^2/2\\ T^2/2& T\end{array}\right]---\left(2\right)$

其中，q是过程噪声强度参数。

等式(1)中的非线性函数Ψ(x_k)受到空气阻力和地球引力的作用，其表达式为：

$\mathrm{\&Psi;}\left(x_k\right)=\mathrm{\&Phi;}{x}_k+\mathrm{Gf}\left(x_k\right)+G\left[\begin{array}{c}0\\ -g\end{array}\right]---\left(3\right)$

其中，g为重力加速度，矩阵Φ和G分别为：

$\mathrm{\&Phi;}=\left[\begin{array}{cccc}1& T& 0& 0\\ 0& 1& 0& 0\\ 0& 0& 1& T\\ 0& 0& 0& 1\end{array}\right],G\left[\begin{array}{cc}{T}^2/2& 0\\ T& 0\\ 0& T^2/2\\ 0& T\end{array}\right]---\left(4\right)$

f(x_k)为代表气动阻力的影响的非线性函数，其中f(x_k)的表达式为：

$f\left(x_k\right)=-0.5\frac{g}{\mathrm{\&beta;}}\mathrm{\&rho;}\left(y_k\right){({\stackrel{\&CenterDot;}{x}}_k^2+{\stackrel{\&CenterDot;}{y}}_k^2)}^{1/2}\left[\begin{array}{c}{\stackrel{\&CenterDot;}{x}}_k\\ {\stackrel{\&CenterDot;}{y}}_k\end{array}\right]---\left(5\right)$

其中，ρ(y_k)是空气密度。

雷达观测量有两个，斜距r和俯仰角ε；斜距和俯仰角的量测误差标准离差分别为σ_r和σ_ε；雷达坐标始终为x_R＝0,y_R＝0；将雷达量测转换到笛卡尔坐标系下为横坐标d＝r cosε和纵坐标h＝r sinε，因此量测的线性等式为：

z_k＝Hx_k+v_k   (6)

其中

$z_k={\left[\begin{array}{cc}{d}_k& h_k\end{array}\right]}^T,H=\left[\begin{array}{cccc}1& 0& 0& 0\\ 0& 0& 1& 0\end{array}\right]---\left(7\right)$

其中，d_k,h_k分别是雷达量测转换到笛卡尔坐标系下的横坐标和纵坐标；H是观测矩阵；v_k是笛卡尔坐标系中的量测噪声，与过程噪声和初始状态独立，为零均值的高斯白噪声，其协方差矩阵为R。

在FDEKF算法基础上，在预测误差协方差矩阵中引入强跟踪的次优渐消因子λ(k+1)，则将预测误差协方差修正为：

$\begin{array}{c}{\stackrel{\&OverBar;}{P}}_{k+1}=\mathrm{\&lambda;}(k+1)[F_x{\hat{P}}_k{F}_x^T+F_w{Q}_k{F}_w^T]\\ =\mathrm{\&lambda;}(k+1)[F_x{\hat{S}}_x{\hat{S}}_x^T{F}_x^T+F_w{S}_w{S}_w^T{F}_w^T]\\ =\mathrm{\&lambda;}(k+1)[S_{x\hat{x}}{S}_{x\hat{x}}^T+S_{\mathrm{xw}}{S}_{\mathrm{xw}}^T]\end{array}---\left(8\right)$

其中，次优渐消因子λ(k+1)为：

$\mathrm{\&lambda;}(k+1)=\left\{\begin{array}{cc}{\mathrm{\&lambda;}}_0,& {\mathrm{\&lambda;}}_0\&GreaterEqual;1\\ 1,& {\mathrm{\&lambda;}}_0<1\end{array}\right.---\left(9\right)$

式中：

${\mathrm{\&lambda;}}_0=\frac{\mathrm{tr}\left[N(k+1)\right]}{\mathrm{tr}\left[M(k+1)\right]}---\left(10\right)$

N(k+1)＝V₀(k+1)-βR(k+1)-H(k+1)Q(k)H^T(k+1)   (11)

M(k+1)＝H(k+1)F(k)P(k|k)F^T(k)H^T(k+1)   (12)

V₀(k+1)为残差方差矩阵，且

$\begin{array}{c}{V}_0(k+1)=E\left[r(k+1)r^T(k+1)\right]\\ =\left\{\begin{array}{cc}r\left(1\right)r^T\left(1\right),& k=0\\ \frac{\mathrm{\&rho;}{V}_0\left(k\right)+r(k+1)r^T(k+1)}{1+\mathrm{\&rho;}}& k\&GreaterEqual;1\end{array}\right.\end{array}---\left(13\right)$

其中，r(1)为初始残差，tr[·]表示对矩阵求秩运算，ρ为遗忘因子，且0<ρ≤1，引入遗忘因子的目的是为了进一步对过去的老数据渐消从而突出最新残差向量的影响，进一步提高强跟踪滤波器的快速跟踪能力，通常选择ρ＝0.95，β为用来改进状态估计光滑性的弱化因子。

引入以下Cholesky分解：

$Q_k=S_w{S}_w^T,R_k=S_v{S}_v^T,{\stackrel{\&OverBar;}{P}}_k={\stackrel{\&OverBar;}{S}}_x{\stackrel{\&OverBar;}{S}}_x^T,{\hat{P}}_k={\hat{S}}_x{\hat{S}}_x^T$

得：

${\stackrel{\&OverBar;}{S}}_{x\hat{x}}=\left\{{\stackrel{\&OverBar;}{S}}_{x\hat{x}}(i,j)\right\}=\{(f_i(\hat{x}+h{\hat{s}}_{x,j},u,\stackrel{\&OverBar;}{w})-f_i(\hat{x}-h{\hat{s}}_{x,j},u,\stackrel{\&OverBar;}{w}))/2h\}---\left(14\right)$

$\begin{array}{c}{\stackrel{\&OverBar;}{S}}_{\mathrm{xw}}=\left\{{\stackrel{\&OverBar;}{S}}_{\mathrm{xw}}(i,j)\right\}\\ =\{(f_i(\hat{x},u,\stackrel{\&OverBar;}{w}+h{s}_{w,j})-f_i(\hat{x},u,\stackrel{\&OverBar;}{w}-h{s}_{w,j}))/2h\}\end{array}---\left(15\right)$

$\begin{array}{c}{S}_{y\stackrel{\&OverBar;}{x}}=\left\{S_{y\stackrel{\&OverBar;}{x}}(i,j)\right\}\\ =\{(g_i(\stackrel{\&OverBar;}{x}+h{\stackrel{\&OverBar;}{s}}_{x,j},\stackrel{\&OverBar;}{v})-g_i(\stackrel{\&OverBar;}{x}-h{\stackrel{\&OverBar;}{s}}_{x,j},\stackrel{\&OverBar;}{v}))/2h\}\end{array}---\left(16\right)$

$\begin{array}{c}{S}_{\mathrm{yv}}=\left\{S_{\mathrm{yv}}(i,j)\right\}\\ =\{(g_i(\stackrel{\&OverBar;}{x},\stackrel{\&OverBar;}{v}+h{s}_{v,j})-g_i(\stackrel{\&OverBar;}{x},\stackrel{\&OverBar;}{v}-h{s}_{v,j}))/2h\}\end{array}---\left(17\right)$

得目标状态下一步预测预测误差协方差矩阵为：

${\stackrel{\&OverBar;}{x}}_{k+1}=f({\hat{x}}_k,u_k,{\stackrel{\&OverBar;}{w}}_k)---\left(18\right)$

${\stackrel{\&OverBar;}{P}}_{k+1}=\mathrm{\&lambda;}(k+1)[S_{x\hat{x}}{S}_{x\hat{x}}^T+S_{\mathrm{xw}}{S}_{\mathrm{xw}}^T]---\left(19\right)$

得目标状态下一步的预测增益K_k+1，状态估计及估计误差协方差矩阵分别为：

${\stackrel{\&OverBar;}{y}}_{k+1}=g({\stackrel{\&OverBar;}{x}}_{k+1},{\stackrel{\&OverBar;}{v}}_{k+1})---\left(20\right)$

$K_{k+1}={\stackrel{\&OverBar;}{S}}_x{S}_{y\stackrel{\&OverBar;}{x}}^T{[S_{y\stackrel{\&OverBar;}{x}}{S}_{y\stackrel{\&OverBar;}{x}}^T+S_{\mathrm{yv}}{S}_{\mathrm{yv}}^T]}^{-1}---\left(21\right)$

${\hat{x}}_{k+1}={\stackrel{\&OverBar;}{x}}_{k+1}+K_{k+1}[y_{k+1}-{\stackrel{\&OverBar;}{y}}_{k+1}]---\left(22\right)$

$\begin{array}{c}{\hat{P}}_{k+1}=\begin{array}{}\end{array}\left[\begin{array}{cc}{\stackrel{\&OverBar;}{S}}_x-K_{k+1}{S}_{y\stackrel{\&OverBar;}{x}}& K_{k+1}{S}_{\mathrm{yv}}\end{array}\right]\\ \&times;{\left[\begin{array}{cc}{\stackrel{\&OverBar;}{S}}_x-K_{k+1}{S}_{y\stackrel{\&OverBar;}{x}}& K_{k+1}{S}_{\mathrm{yv}}\end{array}\right]}^T\end{array}---\left(23\right)$

验证性实验

1)实验场景：通过Monte Carlo仿真比较了几种算法的性能。仿真场景设为：扫描周期T＝2s，过程噪声强度为q，雷达量测的伪距误差标准离差为σ_r＝200m，俯仰角误差标准离差为σ_ε＝0.05rad，目标弹道系数设为已知，β＝40000kgm^-1s^-2。初始状态x₀为高斯随机向量，其均值和方差已知，均值m₀＝[232000 2290cos(190°) 88000 2290sin(190°)]，其中，距离的单位为m，速度的单位为m/s；方差为∑＝diag([1000² 20² 1000² 20²])，仿真目标跟踪步长为120步，运行500次Monte Carlo仿真，并假定目标检测概率为1，虚警概率为0。

2)实验结果:比较的标准为各个算法的位置均方根误差，速度均方根误差，滤波可靠性，以及各算法的执行时间等。本例中，目标状态为是个二维问题，所以位置及速度分量的均方根误差有如下定义:

位置均方根误差的计算方法为：

${\mathrm{RMSE}}_{{\mathrm{Pos}}_k}=\sqrt{\frac{1}{M}\underset{i=1}{\overset{M}{\mathrm{\&Sigma;}}}[{(x_k^i-{\hat{x}}_{k|k}^i)}^2+{(y_k^i-{\hat{y}}_{k|k}^i)}^2]}---\left(24\right)$

其中，M为Monte-Carlo仿真次数；为k时刻真实的位置信息；为k时刻滤波器估计的位置信息。

速度均方根误差的计算方法为：

${\mathrm{RMSE}}_{{\mathrm{Vel}}_k}=\sqrt{\frac{1}{M}\underset{i=1}{\overset{M}{\mathrm{\&Sigma;}}}[{({\stackrel{\&CenterDot;}{x}}_k^i-{\stackrel{\widehat{\&CenterDot;}}{x}}_{k|k}^i)}^2+{({\stackrel{\&CenterDot;}{y}}_k^i-{\stackrel{\widehat{\&CenterDot;}}{y}}_{k|k}^i)}^2]}---\left(25\right)$

其中，M为Monte-Carlo仿真次数；为k时刻真实的速度信息；为k时刻滤波器对速度的估计值；

用平均标准化估计误差平方(Average Normalized EstimationError Square，ANEES)比较几种算法的滤波可靠性。ANEES的定义如下：

$\mathrm{ANEES}=\frac{1}{\mathrm{Mn}}\underset{i=1}{\overset{M}{\mathrm{\&Sigma;}}}{(x_i-{\hat{x}}_i)}^T{P_i}^{-1}(x_i-{\hat{x}}_i)---\left(26\right)$

其中，n为状态维数；M为Monte-Carlo仿真次数；x_i为第i次MonteCarlo运行时的真实的状态；为第i次Monte Carlo运行时的状态估计值；P_i为第i次Monte Carlo运行时的状态估计误差协方差矩阵；

则有，ANEES曲线越接近于1，表示滤波可靠性越高。

下面，首先给出一个典型的目标位置，速度及加速度曲线，如图1所示。

接下来以本发明分别与EKF、UKF和FDEKF算法相比较，通过增加过程噪声强度，包括q＝0.1、q＝1及q＝5三种情况，对四种算法的性能进行比较，比较的准则是均方根误差、滤波可靠性及计算复杂性。

图2至图10中分别给出各种算法的位置和速度的均方根误差比较以及滤波可靠性比较，最后在表1中给出各种算法的计算复杂性比较。

由图1(c)的加速度曲线可以看到，由于空气阻力的影响，该再入弹道目标在40s到60s之间有一个很大的负加速度，目标发生了机动。

情况1：过程噪声强度q＝0.1，运行500次Monte Carlo仿真

由图2至图4看到，在q＝0.1的情况下，FDEKF算法与EKF算法估计精度不相上下，但随着时间增加FDEKF表现出的估计误差小于EKF。而本发明的估计误差与UKF的接近，这二者均方根误差均小于EKF和FDEKF。

滤波可靠性上，本发明和UKF的ANEES曲线都在1附近波动，二者可靠性都比较高，FDEKF和EKF的可靠性较差。

情况2：增大过程噪声强度，q＝1，运行500次Monte Carlo仿真

在图5至图7中，通过观察，可以看出，增大过程噪声强度后，本发明无论是在位置RMSE还是速度RMSE上，均很明显的比FDEKF的小了，其跟踪精度远高于FDEKF的精度。另外，其滤波可靠性也比FDEKF高，STFDEKF的估计精度与UKF仍是很接近。

情况3：增大过程噪声强度，q＝5，运行500次Monte Carlo仿真

在图8至图10中，通过观察，可以看出，继续增大过程噪声强度后，本发明的估计精度与UKF仍是很接近，而且其可靠性与UKF不相上下，跟踪精度和滤波可靠性仍明显高于FDEKF.

分析三种情况，随着噪声的增加，也就是对于模型不确定性增大的情况下，本发明不论是目标发生机动和非机动时，估计精度及滤波可靠性都要高于FDEKF，接近于UKF算法。而FDEKF的估计精度及滤波可靠明显高于EKF。

在计算复杂性上，各种算法的500次Monte Carlo仿真的计算时间如表1所示。从表1中可见，本发明比FDEKF耗时稍多，而比UKF耗时要少。

表1

结果表明本发明计算量适中，而且精度较高，对于再入阶段的弹道目标跟踪是个很好的选择。

Claims (4)

1.一种用于再入弹道目标跟踪的有限差分滤波方法，其特征在于，包括以下步骤：

在弹道目标跟踪模型中，弹道目标受到空气阻力和地球引力的作用，假设地球为平的，则目标运动的离散时间非线性状态方程为

x_k+1＝Ψ(x_k)+w_k              (1)

其中，k为非负整数，T为连续两个雷达量测的时间间隔，状态向量x_k及y_k为笛卡儿坐标系(x,y)中目标的位置，及为笛卡儿坐标系(x,y)中目标在x轴方向上及y轴方向上的速度，w_k为过程噪声序列，Ψ(x_k)为非线性函数；

在FDEKF算法基础上，在预测误差协方差矩阵中引入强跟踪的次优渐消因子λ(k+1)，则将预测误差协方差修正为：

$\begin{array}{c}{\stackrel{\&OverBar;}{P}}_{k+1}=\mathrm{\&lambda;}(k+1)[F_x{\hat{P}}_x{F}_x^T+F_w{Q}_k{F}_w^T]\\ =\mathrm{\&lambda;}(k+1)[F_x{\hat{S}}_x{\hat{S}}_x^T{F}_x^T+F_w{S}_w{S}_w^T{F}_w^T]\\ =\mathrm{\&lambda;}(k+1)[S_{x\hat{x}}{S}_{x\hat{x}}^T+S_{\mathrm{xw}}{S}_{\mathrm{xw}}^T]\end{array}---\left(8\right)$

其中，F_x为非线性函数f_k(x_k,u,w_k)对x_k的一阶偏导数，为k时刻的估计误差协方差，Q_k为过程噪声协方差矩阵，为的Cholesky分解，F_w为非线性函数f_k(x_k,u,w_k)对w_k的一阶偏导数，S_w为Q_k的Cholesky分解，λ(k+1)为次优渐消因子，其中

$\mathrm{\&lambda;}(k+1)=\left\{\begin{array}{cc}{\mathrm{\&lambda;}}_0,& {\mathrm{\&lambda;}}_0\&GreaterEqual;1\\ 1,& {\mathrm{\&lambda;}}_0<1\end{array}\right.---\left(9\right)$

${\mathrm{\&lambda;}}_0=\frac{\mathrm{tr}\left[N(k+1)\right]}{\mathrm{tr}\left[M(k+1)\right]}---\left(10\right)$

N(k+1)＝V₀(k+1)-βR(k+1)-H(k+1)Q(k)H^T(k+1)        (11)

M(k+1)＝H(k+1)F(k)P(k|k)F^T(k)H^T(k+1)           (12)

其中，R(k+1)为残差序列，H(k+1)为量测矩阵，F(k)为非线性函数f(x_k)的雅可比矩阵，P(k|k)为k时刻的估计误差协方差；

V₀(k+1)为残差方差矩阵，且

$\begin{array}{c}{V}_0(k+1)=E\left[r(k+1)r^T(k+1)\right]\\ =\left\{\begin{array}{cc}r\left(1\right)r^T\left(1\right),& k=0\\ \frac{\mathrm{\&rho;}{V}_0\left(k\right)+r(k+1)r^T(k+1)}{1+\mathrm{\&rho;}}& k\&GreaterEqual;1\end{array}\right.\end{array}---\left(13\right)$

r(k+1)为残差序列，r(1)为初始残差，tr[·]表示对矩阵求秩运算，ρ为遗忘因子，且0<ρ≤1；

引入以下Cholesky分解，得：

${\stackrel{\&OverBar;}{S}}_{x\hat{x}}=\left\{{\stackrel{\&OverBar;}{S}}_{x\hat{x}}(i,j)\right\}=\{(f_i(\hat{x}+h{\hat{s}}_{x,j},u,\stackrel{\&OverBar;}{w})-f_i(\hat{x}-h{\hat{s}}_{x,j},u,\stackrel{\&OverBar;}{w}))/2h\}---\left(14\right)$

${\stackrel{\&OverBar;}{S}}_{\mathrm{xw}}=\left\{{\stackrel{\&OverBar;}{S}}_{\mathrm{xw}}(i,j)\right\}=\{(f_i(\hat{x},u,\stackrel{\&OverBar;}{w}+h{s}_{w,j})-f_i(\hat{x},u,\stackrel{\&OverBar;}{w}-h{s}_{w,j}))/2h\}---\left(15\right)$

$\begin{array}{c}{S}_{y\stackrel{\&OverBar;}{x}}=\left\{S_{y\stackrel{\&OverBar;}{x}}(i,j)\right\}\\ =\{(g_i(\stackrel{\&OverBar;}{x}+h{\stackrel{\&OverBar;}{s}}_{x,j},\stackrel{\&OverBar;}{v})-g_i(\stackrel{\&OverBar;}{x}-h{\stackrel{\&OverBar;}{s}}_{x,j},\stackrel{\&OverBar;}{v}))/2h\}\end{array}---\left(16\right)$

$\begin{array}{c}{S}_{\mathrm{yv}}=\left\{S_{\mathrm{yv}}(i,j)\right\}\\ =\{(g_i(\stackrel{\&OverBar;}{x},\stackrel{\&OverBar;}{v}+h{s}_{v,j})-g_i(\stackrel{\&OverBar;}{x},\stackrel{\&OverBar;}{v}-h{s}_{v,j}))/2h\}\end{array}---\left(17\right)$

其中，为矩阵的第i行、第j列的元素；u为状态过程输入，为过程噪声一步预测，h为区间长度参数，为中心差分公式，为量测噪声一步预测；

由式(8)、(14)、(15)、(16)及(17)得目标状态下一步预测预测误差协方差矩阵为：

${\stackrel{\&OverBar;}{x}}_{k+1}=f({\hat{x}}_k,u_k,{\stackrel{\&OverBar;}{w}}_k)---\left(18\right)$

${\stackrel{\&OverBar;}{P}}_{k+1}=\mathrm{\&lambda;}(k+1)[S_{x\hat{x}}{S}_{x\hat{x}}^T+S_{\mathrm{xw}}{S}_{\mathrm{xw}}^T]---\left(19\right)$

其中，为状态估计，u_k为过程输入，为过程噪声一步预测；

目标状态下一步的预测增益K_k+1、状态估计及估计误差协方差矩阵分别为：

${\stackrel{\&OverBar;}{y}}_{k+1}=g({\stackrel{-}{x}}_{k+1},{\stackrel{\&OverBar;}{v}}_{k+1})---\left(20\right)$

$K_{k+1}={\stackrel{\&OverBar;}{S}}_x{S}_{y\stackrel{\&OverBar;}{x}}^T{[S_{y\stackrel{\&OverBar;}{x}}{S}_{y\stackrel{\&OverBar;}{x}}^T+S_{\mathrm{yv}}{S}_{\mathrm{yv}}^T]}^{-1}---\left(21\right)$

${\hat{x}}_{k+1}={\stackrel{\&OverBar;}{x}}_{k+1}+K_{k+1}[y_{k+1}-{\stackrel{\&OverBar;}{y}}_{k+1}]---\left(22\right)$

$\begin{array}{c}{\hat{P}}_{k+1}=\left[\begin{array}{cc}{\stackrel{\&OverBar;}{S}}_x-K_{k+1}{S}_{y\stackrel{\&OverBar;}{x}}& K_{k+1}{S}_{\mathrm{yv}}\end{array}\right]\\ \&times;{\left[\begin{array}{cc}{\stackrel{\&OverBar;}{S}}_x-K_{k+1}{S}_{y\stackrel{\&OverBar;}{x}}& K_{k+1}{S}_{\mathrm{yv}}\end{array}\right]}^T\end{array}---\left(23\right)$

其中，为系统量测的非线性函数，为预测误差协方差矩阵的乔里斯基分解，K_k+1为增益，y_k+1为量测。

2.根据权利要求1所述的用于再入弹道目标跟踪的有限差分滤波方法，其特征在于，

设过程噪声序列w_k是零均值的高斯白噪声，则过程噪声序列w_k的协方差矩阵Q为：

$Q=q\left[\begin{array}{cc}\mathrm{\&Theta;}& 0\\ 0& \mathrm{\&Theta;}\end{array}\right],\mathrm{\&Theta;}=\left[\begin{array}{cc}{T}^3/3& T^2/2\\ T^2/2& T\end{array}\right]---\left(2\right)$

其中，q是过程噪声强度参数。

3.根据权利要求1所述的用于再入弹道目标跟踪的有限差分滤波方法，其特征在于，

非线性函数Ψ(x_k)受到空气阻力和地球引力的作用，非线性函数Ψ(x_k)的表达式为：

$\mathrm{\&Psi;}\left(x_k\right)=\mathrm{\&Phi;}{x}_k+\mathrm{Gf}\left(x_k\right)+G\left[\begin{array}{c}0\\ -g\end{array}\right]---\left(3\right)$

其中，g为重力加速度，矩阵Φ和G分别为：

$\mathrm{\&Phi;}=\left[\begin{array}{cccc}1& T& 0& 0\\ 0& 1& 0& 0\\ 0& 0& 1& T\\ 0& 0& 0& 1\end{array}\right],G=\left[\begin{array}{cc}{T}^2/2& 0\\ T& 0\\ 0& T^2/2\\ 0& T\end{array}\right]---\left(4\right)$

f(x_k)为代表气动阻力的影响的非线性函数，其中f(x_k)的表达式为：

$f\left(x_k\right)=-0.5\frac{g}{\mathrm{\&beta;}}\mathrm{\&rho;}\left(y_k\right){({\stackrel{\&CenterDot;}{x}}_k^2+{\stackrel{\&CenterDot;}{y}}_k^2)}^{1/2}\left[\begin{array}{c}{\stackrel{\&CenterDot;}{x}}_k\\ {\stackrel{\&CenterDot;}{y}}_k\end{array}\right]---\left(5\right)$

其中，ρ(y_k)是空气密度，β为用来改进状态估计光滑性的弱化因子。

4.根据权利要求1所述的用于再入弹道目标跟踪的有限差分滤波方法，其特征在于，所述遗忘因子ρ＝0.95。