CN106446422B - 一种基于对数似然估计的无源定位跟踪新方法 - Google Patents

Abstract

一种基于对数似然估计的无源定位跟踪新方法，利用对数似然估计将多站测角无源定位非线性观测方程进行伪线性化处理，用最大似然估计求出目标的位置并给出了目标定位算法的闭式解，接着利用“当前”统计机动模型和卡尔曼滤波，在线实时计算目标位置、速度和加速度，实现对目标的实时精确定位跟踪。

Description

一种基于对数似然估计的无源定位跟踪新方法

技术领域

本发明涉及自动控制和信息技术领域，可用于在军事和民用领域中对高机动运动目标的实时定位跟踪，具体涉及一种基于对数似然估计的无源定位跟踪新方法。

背景技术

无源定位技术是一种定位设备本身不发射信号，仅仅是依靠被动地接收辐射源的信息来实现定位的技术。它可以利用未知位置的辐射源的辐射信息，确定出该辐射源的类型、空间和地理位置；或者利用已知地理位置的辐射源来确定航行中物体的空间和地理位置，这也是导航和制导定位中的一项重要技术手段。与有源定位技术相比，无源定位技术具有作用距离远、隐蔽接收、不易被对方发觉等优点，是现代一体化防空***、机载对地对海攻击以及对付隐身目标的远程预警***的重要组成部分，对于提高***在电子战环境下的生存能力和作战能力具有重要作用，因此无源定位跟踪技术一直是研究的热点和难点。

无源定位是通过观测站接收来自目标的无线电信号，并从信号中挖掘出用于定位的观测量。一般的观测量包括到达时间(Time of Arrival，TOA)，接受信号强度(ReceivedSignal Strength，RSS)，到达时间差(Time Difference of Arrival，TDOA)，到达方位角和俯仰角(Angle of Arrival，AOA)[8,9],到达频率差(FrequencyDifference of Arrival，FDOA)等。根据上述观测信息均能够建立关于目标位置或速度与观测站位置之间的(非线性)方程，再通过优化求解该方程即可获得关于目标位置或速度的参数信息。近些年来，基于上述观测量的目标定位算法已相继提出，其中包括Taylor级数迭代算法，总体最小二乘(Total Least Squares，TLS)算法,约束加权最小二乘(Constrained Weighted LeastSquares，CWLS)算法，约束总体最小二乘(Constrained Total Least Squares，CTLS)算法，结构总体最小二乘(Structured Total Least Squares，STLS)算法。然而，上述算法大都需要迭代运算，这除了带来较复杂的运算量外，还会出现迭代发散和局部收敛等问题。

发明内容

为了克服上述现有技术的不足，本发明的目的是提供一种基于对数似然估计的无源定位跟踪新方法，解决了传统无源定位跟踪方法中迭代运算出现迭代发散、收敛于局部解以及运算量大的问题。本发明针对多基站测角无源定位跟踪技术，利用对数似然估计将多站测角无源定位非线性观测方程进行伪线性化处理，用最大似然估计求出目标的位置并给出了目标定位算法的闭式解，接着利用“当前”统计机动模型和卡尔曼滤波，在线实时计算目标位置、速度和加速度，实现对目标的实时精确定位跟踪。通过理论证明和仿真实验证明本发明提出闭式解的定位性能均能够达到克拉美罗下限(Cramér-Rao Bound，CRB)，从而验证文中理论分析的有效性。

一种基于对数似然估计的无源定位跟踪新方法，其特征在于，包括如下步骤：

1)根据基站测角与目标位置之间数学关系，建立目标定位跟踪测量方程，测量噪声服从高斯分布，同时确定目标状态变量；

在直角坐标系下，假设观测站i的位置分别为(x_i,y_i,z_i)，i＝1,2,…,N，飞行目标的空间位置在k时刻为(x_k,y_k,z_k)，观测站i测得飞行目标在k时刻的方位角为A_i,k和俯仰角为E_i,k，在存在测量噪声情况下，A_i,k和E_i,k可以用下式(1)、式(2)表示：

这里定义和以式(3)、式(4)表示：

和表示观测站i测得方位角和俯仰角的真值，n_i,k和e_i,k是互不相关的测量噪声序列，服从均值为0的高斯分布，其方差分别由下式给出：

重新定义测量序列，用向量a_k表示为式(5)，

a_k＝[A_1,k,A_2,k,…,A_N,k,E_1,k,E_2,k,…,E_N,k]^T (5)

同时测量真值序列用向量表示为式(6)，

根据式(2)、(5)、(6)，可以得出下式(7),

这里

b_k＝[n_1,k,n_2,k,…,n_N,k,e_1,k,e_2,k,…,e_N,k]^T

由于噪声序列n_i,k和e_i,k是互不相关的零均值高斯随机变量，方差分别为和噪声序列b_k是2N维向量，均值为0，方差矩阵由式(8)给出：

这里

为了利用含噪声项向量a_k估计飞行目标在k时刻的位置(x_k,y_k,z_k)，定义如下向量s_k，向量s_k由式(9)表示：

向量元素(x_k,y_k,z_k)给出了飞行器在k时刻的位置，给出了k时刻飞行器在(x,y,z)方向上的飞行速度，给出了k时刻飞行器在(x,y,z)方向上的飞行加速度，

假定s_k的最大似然估计和它的误差协方差矩阵p_k/k-1先于a_k被计算出来，和p_k/k-1定义如下形式向量，由式(10)、式(11)表示，

2)利用新的量测序列a_k更新s_k的最大似然估计和误差协方差矩阵p_k/k-1。s_k的更新估计将用表示，更新误差协方差矩阵用p_k/k表示，根据最大似然估计原理，建立求解目标状态变量的对数似然函数，通过求导可以计算出目标位置的全局最优解；

因为最大似然估计是渐近的高斯函数分布，因此我们可以假定估计值的概率密度函数是均值为s_k，方差为p_k/k-1的高斯函数，的概率密度函数定义由式(12)表示：

这里ρ₁为：

因为在式(7)中，b_k是均值为0，方差为R_k的高斯高斯随机向量，所以向量a_k是均值为方差为R_k的高斯随机向量，因此a_k的概率密度函数g(a_k)由式(13)给出，

这里ρ₂为：

在给定和a_k的条件下，s_k的似然函数l(s_k)表示如式(14)：

由此，负对数似然函数L(s_k)经计算等于式(15)：

这里常数项被省略，在给定和a_k的条件下，为了得到s_k的最大似然估计，必须最小化-L(s_k)，由式(16)表示：

3)为了便于快速实时滤波，利用对数似然估计将多站测角无源定位非线性观测方程进行伪线性化处理，给出了目标位置计算的闭式解；

根据(13)式和(14)式，飞行目标位置坐标状态变量x_k，y_k，z_k与测量值A_i,k和E_i,k存在非线性形式，为了得到状态变量与测量之间的线性关系，我们将在线性化这两个等式，

对(13)式在进行泰勒级数展开，忽略掉高次项，得式(17)：

这里

符号和表示估计的观测站与目标的方位角和水平距离，观测站位于(x_i,y_i,z_i),为从(17)式估计目标在k时刻的位置，将(17)式进行数学变换得到下式：

这里

对(14)式在进行泰勒级数展开，忽 略掉高次项，得到下式：

其中

和同上，表示估计的观测站与目标的距离和方位角。为从(19)式估计目标在k时刻的位置，将(19)式进行数学变换，得到下面的关系式(20)：

其中

通过对量测A_i,k和E_i,k重复上面提到的线性化过程，可以得到下面伪测量向量，由式(21)表示：

向量也可以表示成矩阵形式，如式(22)，

这里

将上式计算结果代入(15)式，可以得到-L(s_k)另外一个表达式(23)，

因此，为了得到状态向量s_k在k时刻的最大似然估计，必须最小化-L(s_k)；

4)通过伪线性化处理，给出了目标位置状态变量真实值与预测值之间的关系式，在此基础上建立目标定位跟踪状态方程；

为了求解s_k的估计值求-L(s_k)对s_k的梯度▽(-L(s_k))，令▽(-L(s_k))＝0，得到式(24)，

因为

所以(23)式求出的是全局最小值，在(23)式中G_k由式(25)表示，

矩阵G_k叫增益矩阵，经变换后得到下面的等式(26)，

的误差协方差矩阵p_k/k由式(27)给出

由上述推导结果可以看出，在给定p_k/k-1和a_k时，和p_k/k计算过程如下：

第一步：计算

第二步：计算

第三步：计算p_k/k＝(I-G_kH_k)p_k/k-1；

5)运用“当前”统计模型和卡尔曼滤波，结合测量方程和状态方程，建立机动目标定位跟踪的递推关系式，实现基于测角的机动目标无源定位跟踪；

在上述中，当计算和p_k/k，假定和它的误差协方差矩阵p_k/k-1已经给定，因此在k+1时刻计算和p_k+1/k+1时，必须事先给定和p_k+1/k的最大似然估计，在这部分将给出如何利用和p_k/k估计和p_k+1/k值，运动目标从k时刻到k+1时刻机动模型采用“当前”统计模型，即式(28)所表示，

这里状态转移矩阵F由式(29)表示，

其中

U由

U＝[u₁ u₂ u₃ u₄ u₅ u₆ u₇ u₈ u₉]^T (30)

其中

这里，F表示状态转移矩阵，T表示采样间隔，(a_x,a_y,a_z)表示(x,y,z)方向机动，表示随机加速度的均值(加速据)时间常数的倒数，W表示状态噪声，服从零均值高斯分布，方差为Q，U表示加速度系数矩阵。

是加速度的方差，由估计理论，可以求出和p_k+1/k，由式(32)、式(33)表示，

本发明的有益效果是：

1)运用最大似然估计及对数似然函数伪线性化处理，使得目标位置计算具有闭式解，而且计算结果具有全局最优解，避免了传统无源定位方法中迭代运算出现迭代发散、收敛于局部解的问题；

2)在利用对数似然估计将多站测角无源定位非线性观测方程进行伪线性化处理的基础上，运用“当前”统计模型和卡尔曼滤波算法，实现了在线实时的目标定位跟踪，解决了传统迭代计算复杂度高的问题；

3)通过理论证明和仿真实验验证，本发明提出闭式解的定位性能均能够达到克拉美罗下限(Cramér-Rao Bound，CRB)，从而验证本方法的有效性。

附图说明

图1为本发明的第一种仿真位置估计曲线；

图2为本发明的第一种仿真速度估计曲线；

图3为本发明的第一种仿真加速的估计曲线；

图4为本发明的第二种仿真位置估计曲线；

图5为本发明的第二种仿真速度估计曲线；

图6为本发明的第二种仿真加速度估计曲线；

图7为本发明的位置估计RMS与Cramér Rao Bound下限。

具体实施方式

以下结合实施例对本发明进一步叙述。

一种基于对数似然估计的无源定位跟踪新方法，其特征在于，包括如下步骤：

1)根据基站测角与目标位置之间数学关系，建立目标定位跟踪测量方程，测量噪声服从高斯分布，同时确定目标状态变量；

在直角坐标系下，假设观测站i的位置分别为(x_i,y_i,z_i)，i＝1,2,…,N，飞行目标的空间位置在k时刻为(x_k,y_k,z_k)，观测站i测得飞行目标在k时刻的方位角为A_i,k和俯仰角为E_i,k，在存在测量噪声情况下，A_i,k和E_i,k可以用下式(1)、式(2)表示：

这里定义和以式(3)、式(4)表示：

和表示观测站i测得方位角和俯仰角的真值，n_i,k和e_i,k是互不相关的测量噪声序列，服从均值为0的高斯分布，其方差分别由下式给出：

重新定义测量序列，用向量a_k表示为式(5)，

同时测量真值序列用向量表示为式(6)，

根据式(2)、(5)、(6)，可以得出下式(7),

这里

b_k＝[n_1,k,n_2,k,…,n_N,k,e_1,k,e_2,k,…,e_N,k]^T

由于噪声序列n_i,k和e_i,k是互不相关的零均值高斯随机变量，方差分别为和噪声序列b_k是2N维向量，均值为0，方差矩阵由式(8)给出：

E[b_kb_l]＝δ_klR_k (8)

这里

为了利用含噪声项向量a_k估计飞行目标在k时刻的位置(x_k,y_k,z_k)，定义如下向量s_k，向量s_k由式(9)表示：

向量元素(x_k,y_k,z_k)给出了飞行器在k时刻的位置，给出了k时刻飞行器在(x,y,z)方向上的飞行速度，给出了k时刻飞行器在(x,y,z)方向上的飞行加速度，

假定s_k的最大似然估计和它的误差协方差矩阵p_k/k-1先于a_k被计算出来，和p_k/k-1定义如下形式向量，由式(10)、式(11)表示，

2)利用新的量测序列a_k更新s_k的最大似然估计和误差协方差矩阵p_k/k-1。s_k的更新估计将用表示，更新误差协方差矩阵用p_k/k表示，根据最大似然估计原理，建立求解目标状态变量的对数似然函数，通过求导可以计算出目标位置的全局最优解；

因为最大似然估计是渐近的高斯函数分布，因此我们可以假定估计值的概率密度函数是均值为s_k，方差为p_k/k-1的高斯函数，的概率密度函数定义由式(12)表示：

这里ρ₁为：

因为在式(7)中，b_k是均值为0，方差为R_k的高斯高斯随机向量，所以向量a_k是均值为方差为R_k的高斯随机向量，因此a_k的概率密度函数g(a_k)由式(13)给出，

这里ρ₂为：

在给定和a_k的条件下，s_k的似然函数l(s_k)表示如式(14)：

由此，负对数似然函数L(s_k)经计算等于式(15)：

这里常数项被省略，在给定和a_k的条件下，为了得到s_k的最大似然估计，必须最小化-L(s_k)，由式(16)表示：

3)为了便于快速实时滤波，利用对数似然估计将多站测角无源定位非线性观测方程进行伪线性化处理，给出了目标位置计算的闭式解；

根据(13)式和(14)式，飞行目标位置坐标状态变量x_k，y_k，z_k与测量值A_i,k和E_i,k存在非线性形式，为了得到状态变量与测量之间的线性关系，我们将在线性化这两个等式，

对(13)式在进行泰勒级数展开，忽略掉高次项，得式(17)：

这里

符号和表示估计的观测站与目标的方位角和水平距离，观测站位于(x_i,y_i,z_i),为从(17)式估计目标在k时刻的位置，将(17)式进行数学变换得到下式：

这里

对(14)式在进行泰勒级数展开，忽略掉高次项，得到下式：

其中

和同上，表示估计的观测站与目标的距离和方位角。为从(19)式估计目标在k时刻的位置，将(19)式进行数学变换，得到下面的关系式(20)：

其中

通过对量测A_i,k和E_i,k重复上面提到的线性化过程，可以得到下面伪测量向量，由式(21)表示：

向量也可以表示成矩阵形式，如式(22)，

这里

将上式计算结果代入(15)式，可以得到-L(s_k)另外一个表达式(23)，

因此，为了得到状态向量s_k在k时刻的最大似然估计，必须最小化-L(s_k)；

4)通过伪线性化处理，给出了目标位置状态变量真实值与预测值之间的关系式，在此基础上建立目标定位跟踪状态方程；

为了求解s_k的估计值求-L(s_k)对s_k的梯度▽(-L(s_k))，令▽(-L(s_k))＝0，得到式(24)，

因为

所以(23)式求出的是全局最小值，在(23)式中G_k由式(25)表示，

矩阵G_k叫增益矩阵，经变换后得到下面的等式(26)，

的误差协方差矩阵p_k/k由式(27)给出

由上述推导结果可以看出，在给定p_k/k-1和a_k时，和p_k/k计算过程如下：

第一步：计算

第二步：计算

第三步：计算p_k/k＝(I-G_kH_k)p_k/k-1；

5)运用“当前”统计模型和卡尔曼滤波，结合测量方程和状态方程，建立机动目标定位跟踪的递推关系式，实现基于测角的机动目标无源定位跟踪；

在上述中，当计算和p_k/k，假定和它的误差协方差矩阵p_k/k-1已经给定，因此在k+1时刻计算和p_k+1/k+1时，必须事先给定和p_k+1/k的最大似然估计，在这部分将给出如何利用和p_k/k估计和p_k+1/k值，运动目标从k时刻到k+1时刻机动模型采用“当前”统计模型，即式(28)所表示，

这里状态转移矩阵F由式(29)表示，

其中

U由

U＝[u₁ u₂ u₃ u₄ u₅ u₆ u₇ u₈ u₉]^T (30)

其中

这里，F表示状态转移矩阵，T表示采样间隔，(a_x,a_y,a_z)表示(x,y,z)方向机动，a表示随机加速度的均值(加速据)时间常数的倒数，W表示状态噪声，服从零均值高斯分布，方差为Q，U表示加速度系数矩阵。

是加速度的方差，由估计理论，可以求出和p_k+1/k，由式(32)、式(33)表示，

实施例

为了验证算法的可行性，我们作以下三种仿真分析。

第一种：设目标在平面上作匀加速运动，起始坐标在原点，起始速度v₀＝2m/s，加速度a₀＝0.2m/s2。角度测量误差服从均值为0，方差为0.001o的高斯分布。仿真中取T＝1s，机动常数为10，两个观测站的位置o₁(0,50)、o₂(0,100)。图(1)给出了跟踪曲线结果，图(2)给出了速度估计结果，图(3)给出了加速度估计结果。由图可以看出位置、速度，加速度估计值和理论值很接近。位置的均方根误差0.032，速度的均方根误差为0.0897，加速度均方根误差0.176。

第二种：设目标在平面上作s型运动，起始坐标在原点。其运动曲线由下面方程决定：

x＝10sin(5tpi/180)

初始条件同上。图(4)给出了跟踪曲线结果，图(5)给出了速度估计结果，图(6)给出了加速度估计结果。由图可以看出位置、速度，估计值和理论值很接近，加速度估计值与理论值相差25％，在一定误差范围内，这种估计结果可以接受。经过计算位置的均方根误差0.16，速度的均方根误差为0.267，加速度均方根误差0.47。

在经过100次蒙特卡罗仿真后，虽然每次估计的曲线不同，但统计结果基本相同，尤其是位置估计相对误差小于4％。这说明采用上述不仅可以获得高精度目标定位精度，而且提供了精度较高的速度和加速度估计。但是，当目标运动非常复杂时，加速度的估计精度可能下降。

第三种，仿真条件两观测站的位置坐标分别为：(-40km，0，0)，(-40km，0，0)，采样时间间隔为1s，方位角和俯仰角的观测误差为5mrad，目标的初始位置是(-10km,50km,10km)。运动轨迹分三个阶段：

运动阶段1：匀速直线运动,各方向分速度为：v_x＝0.2m/s，v_y＝0.1m/s，v_z＝0m/s，运行时间为1500s；

运动阶段2：匀速圆周运动,角速度为0.157rard/s，向心加速度为74m/s2，线速度为471m/s，圆半径为3km，运行时间为500s；

运动阶段3：匀速直线运动,各方向分速度为:v_x＝0.2m/s，v_y＝0.2m/s，v_z＝0.2m/s，运行时间为1000s。

本文经过100次蒙特卡罗仿真实验，实验结果如图7所示，由实验结果可以看出，利用本文方法对目标位置进行计算，其位置估计均方根误差(RMS)渐近达到了克拉美罗下限(Cramér-Rao Bound，CRB)。

Claims (1)

1.一种基于对数似然估计的无源定位跟踪新方法，其特征在于，包括如下步骤：

1)根据基站测角与目标位置之间数学关系，建立目标定位跟踪测量方程，测量噪声服从高斯分布，同时确定目标状态变量；

在直角坐标系下，假设观测站i的位置分别为(x_i,y_i,z_i)，i＝1,2,…,N，飞行目标的空间位置在k时刻为(x_k,y_k,z_k)，观测站i测得飞行目标在k时刻的方位角为A_i,k和俯仰角为E_i,k，在存在测量噪声情况下，A_i,k和E_i,k可以用下式(1)、式(2)表示：

这里定义和以式(3)、式(4)表示：

和表示观测站i测得方位角和俯仰角的真值，n_i,k和e_i,k是互不相关的测量噪声序列，服从均值为0的高斯分布，其方差分别由下式给出：

重新定义测量序列，用向量a_k表示为式(5)，

a_k＝[A_1,k,A_2,k,…,A_N,k,E_1,k,E_2,k,…,E_N,k]^T (5)

同时测量真值序列用向量表示为式(6)，

根据式(2)、(5)、(6)，可以得出下式(7),

这里

b_k＝[n_1,k,n_2,k,…,n_N,k,e_1,k,e_2,k,…,e_N,k]^T

由于噪声序列n_i,k和e_i,k是互不相关的零均值高斯随机变量，方差分别为和噪声序列b_k是2N维向量，均值为0，方差矩阵由式(8)给出：

E[b_kb_l]＝δ_klR_k (8)

这里

为了利用含噪声项向量a_k估计飞行目标在k时刻的位置(x_k,y_k,z_k)，定义如下向量s_k，向量s_k由式(9)表示：

向量元素(x_k,y_k,z_k)给出了飞行器在k时刻的位置，给出了k时刻飞行器在(x,y,z)方向上的飞行速度，给出了k时刻飞行器在(x,y,z)方向上的飞行加速度，

假定s_k的最大似然估计和它的误差协方差矩阵p_k/k-1先于a_k被计算出来，和p_k/k-1定义如下形式向量，由式(10)、式(11)表示，

2)利用新的量测序列a_k更新s_k的最大似然估计和误差协方差矩阵p_k/k-1，s_k的更新估计将用表示，更新误差协方差矩阵用p_k/k表示，根据最大似然估计原理，建立求解目标状态变量的对数似然函数，通过求导可以计算出目标位置的全局最优解；

因为最大似然估计是渐近的高斯函数分布，因此我们可以假定估计值的概率密度函数是均值为s_k，方差为p_k/k-1的高斯函数，的概率密度函数定义由式(12)表示：

这里ρ₁为：

因为在式(7)中，b_k是均值为0，方差为R_k的高斯高斯随机向量，所以向量a_k是均值为方差为R_k的高斯随机向量，因此a_k的概率密度函数g(a_k)由式(13)给出，

这里ρ₂为：

在给定和a_k的条件下，s_k的似然函数l(s_k)表示如式(14)：

由此，负对数似然函数L(s_k)经计算等于式(15)：

这里常数项被省略，在给定和a_k的条件下，为了得到s_k的最大似然估计，必须最小化-L(s_k)，由式(16)表示：

3)为了便于快速实时滤波，利用对数似然估计将多站测角无源定位非线性观测方程进行伪线性化处理，给出了目标位置计算的闭式解；

根据(13)式和(14)式，飞行目标位置坐标状态变量x_k，y_k，z_k与测量值A_i,k和E_i,k存在非线性形式，为了得到状态变量与测量之间的线性关系，我们将在线性化这两个等式，

对(13)式在进行泰勒级数展开，忽略掉高次项，得式(17)：

这里

符号和表示估计的观测站与目标的方位角和水平距离，观测站位于(x_i,y_i,z_i),为从(17)式估计目标在k时刻的位置，将(17)式进行数学变换得到下式：

这里

对(14)式在进行泰勒级数展开，忽略掉高次项，得到下式：

其中

和同上，表示估计的观测站与目标的距离和方位角，为从(19)式估计目标在k时刻的位置，将(19)式进行数学变换，得到下面的关系式(20)：

其中

通过对量测A_i,k和E_i,k重复上面提到的线性化过程，可以得到下面伪测量向量，由式(21)表示：

向量也可以表示成矩阵形式，如式(22)，

这里

将上式计算结果代入(15)式，可以得到-L(s_k)另外一个表达式(23)，

因此，为了得到状态向量s_k在k时刻的最大似然估计，必须最小化-L(s_k)；

4)通过伪线性化处理，给出了目标位置状态变量真实值与预测值之间的关系式，在此基础上建立目标定位跟踪状态方程；

为了求解s_k的估计值求-L(s_k)对s_k的梯度▽(-L(s_k))，令▽(-L(s_k))＝0，得到式(24)，

因为

所以(23)式求出的是全局最小值，在(23)式中G_k由式(25)表示，

矩阵G_k叫增益矩阵，经变换后得到下面的等式(26)，

的误差协方差矩阵p_k/k由式(27)给出

由上述推导结果可以看出，在给定p_k/k-1和a_k时，和p_k/k计算过程如下：

第一步：计算

第二步：计算

第三步：计算p_k/k＝(I-G_kH_k)p_k/k-1；

5)运用“当前”统计模型和卡尔曼滤波，结合测量方程和状态方程，建立机动目标定位跟踪的递推关系式，实现基于测角的机动目标无源定位跟踪；

在上述中，当计算和p_k/k，假定和它的误差协方差矩阵p_k/k-1已经给定，因此在k+1时刻计算和p_k+1/k+1时，必须事先给定和p_k+1/k的最大似然估计，在这部分将给出如何利用和p_k/k估计和p_k+1/k值，运动目标从k时刻到k+1时刻机动模型采用“当前”统计模型，即式(28)所表示，

这里状态转移矩阵F由式(29)表示，

其中

U＝[u₁ u₂ u₃ u₄ u₅ u₆ u₇ u₈ u₉]^T (30)

其中

这里，F表示状态转移矩阵，T表示采样间隔，(a_x,a_y,a_z)表示(x,y,z)方向机动，表示随机加速度的均值，即加速据时间常数的倒数，W表示状态噪声，服从零均值高斯分布，方差为Q，U表示加速度系数矩阵；

是加速度的方差，由估计理论，可以求出和p_k+1/k，由式(32)、式(33)表示，