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

技术特征：

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)表示：

$A_{i,k}=A_{i,k}^o+n_{i,k}---\left(1\right)$

$E_{i,k}=E_{i,k}^o+e_{i,k}---\left(2\right)$

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

$A_{i,k}^o=arctan(y_i-y_k/x_i-x_k)---\left(3\right)$

$E_{i,k}^o=arctan(z_i-z_k/\sqrt{{\left(x_i-x_k\right)}^2+{\left(y_i-y_k\right)}^2})---\left(4\right)$

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

$E\[n_{i,k}{n}_{j,l}\]=\left\{\begin{array}{cc}{\mathrm{\σ}}_{i,k}^2& i=j,k=l\\ 0& i\≠j,k\≠l\end{array}\right.$

$E\[e_{i,k}{e}_{j,l}\]=\left\{\begin{array}{cc}{\mathrm{\ϵ}}_{i,k}^2& i=j,k=l\\ 0& i\≠j,k\≠l\end{array}\right.$

重新定义测量序列，用向量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)，

$a_k^o={\[A_{1,k}^o,A_{2,k}^o,...A_{N,k}^o,E_{1,k}^o,E_{2,k}^o,...E_{N,k}^o\]}^T---\left(6\right)$

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

$a_k=a_k^o+b_k---\left(7\right)$

这里

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)

这里

${\mathrm{\δ}}_{kl}=\left\{\begin{array}{cc}1& k=l\\ 0& k\≠l\end{array}\right.$

$R_k=diag\[{\mathrm{\σ}}_{1,k}^2,{\mathrm{\σ}}_{2,k}^2,...,{\mathrm{\σ}}_{n,k}^2,{\mathrm{\ϵ}}_{1,k}^2,{\mathrm{\ϵ}}_{2,k}^2,...,{\mathrm{\ϵ}}_{N,k}^2\]$

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

$s_k=\[x_k,{\stackrel{\·}{x}}_k,{\stackrel{\·\·}{x}}_k,y_k,{\stackrel{\·}{y}}_k,{\stackrel{\·\·}{y}}_k,z_k,{\stackrel{\·}{z}}_k,{\stackrel{\·\·}{z}}_k\]---\left(9\right)$

向量元素(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)表示，

${\hat{s}}_{k/k-1}={\[{\hat{x}}_{k/k-1},{\widehat{\stackrel{\·}{x}}}_{k/k-1},{\widehat{\stackrel{\·\·}{x}}}_{k/k-1},{\hat{y}}_{k/k-1},{\widehat{\stackrel{\·}{y}}}_{k/k-1},{\widehat{\stackrel{\·\·}{y}}}_{k/k-1},{\hat{z}}_{k/k-1},{\widehat{\stackrel{\·}{z}}}_{k/k-1},{\widehat{\stackrel{\·\·}{z}}}_{k/k-1}\]}^T---\left(10\right)$

$p_{k/k-1}=E\[({\hat{s}}_{k/k-1}-s_k){({\hat{s}}_{k/k-1}-s_k)}^T\]---\left(11\right)$

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

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

这里ρ₁为：

${\mathrm{\ρ}}_1=\frac{1}{{\left(2\mathrm{\π}\right)}^3{\left|p_{k/k-1}\right|}^{1/2}}$

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

$g\left(a_k\right)={\mathrm{\ρ}}_2\exp \{-\frac{1}{2}{(a_k-a_k^o)}^T{R}_k^{-1}(a_k-a_k^o)\}---\left(13\right)$

这里ρ₂为：

${\mathrm{\ρ}}_2=\frac{1}{{\left(2\mathrm{\π}\right)}^N{\left|R_k\right|}^{1/2}}$

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

$l\left(s_k\right)=f\left({\hat{s}}_{k/k-1}\right)g\left(a_k\right)---\left(14\right)$

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

$-L\left(s_k\right)={({\hat{s}}_{k/k-1}-s_k)}^T{p}_{k/k-1}^{-1}({\hat{s}}_{k/k-1}-s_k)+{(a_k-a_k^o)}^T{R}_k^{-1}(a_k-a_k^o)---\left(15\right)$

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

${\hat{s}}_{k/k}=\arg min\{-L\left(s_k\right)\}---\left(16\right);$

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

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

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

$A_{i,k}={\hat{A}}_{i,k}+\frac{\sin {\hat{A}}_{i,k}}{{\hat{d}}_{i,k}}(x_k-{\hat{x}}_{k/k-1})-\frac{\cos {\hat{A}}_{i,k}}{{\hat{d}}_{i,k}}(y_k-{\hat{y}}_{k/k-1})+n_{i,k}---\left(17\right)$

这里

${\hat{A}}_{i,k}=arctan\left(\frac{y_i-{\hat{y}}_{k/k-1}}{x_i-{\hat{x}}_{k/k-1}}\right)$

${\hat{d}}_{i,k}=\sqrt{{(x_i-{\hat{x}}_{k/k-1})}^2+{(y_i-{\hat{y}}_{k/k-1})}^2}$

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

${\tilde{A}}_{i,k}=\frac{sin{\hat{A}}_{i,k}}{{\hat{d}}_{i,k}}{x}_k-\frac{\cos {\hat{A}}_{i,k}}{{\hat{d}}_{i,k}}{y}_k+n_{i,k}---\left(18\right)$

这里

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

$E_{i,k}={\hat{E}}_{i,k}+\frac{{\hat{d}}_{i,k}\cos {\hat{A}}_{i,k}\tan {\hat{E}}_{i,k}}{{\hat{D}}_{i,k}^2}(x_k-{\hat{x}}_{k/k-1})-\frac{{\hat{d}}_{i,k}\sin {\hat{A}}_{i,k}\tan {\hat{E}}_{i,k}}{{\hat{D}}_{i,k}^2}(y_k-{\hat{y}}_{k/k-1})+\frac{\cos {\hat{E}}_{i,k}}{{\hat{D}}_{i,k}}(z_k-{\hat{z}}_{k/k-1})+e_{i,k}---\left(19\right)$

其中

${\hat{E}}_{i,k}=arctan(z_i-{\hat{z}}_{k/k-1}/\sqrt{{\left(x_i-{\hat{x}}_{k/k-1}\right)}^2+{\left(y_i-{\hat{y}}_{k/k-1}\right)}^2})$

${\hat{A}}_{i,k}=\arctan \left(\frac{y_i-{\hat{y}}_{k/k-1}}{x_i-{\hat{x}}_{k/k-1}}\right)$

${\hat{D}}_{i,k}=\sqrt{{(x_k-{\hat{x}}_{k/k-1})}^2+{(y_k-{\hat{y}}_{k/k-1})}^2+{(z_k-{\hat{z}}_{k/k-1})}^2}$

${\hat{d}}_{i,k}=\sqrt{{(x_i-{\hat{x}}_{k/k-1})}^2+{(y_i-{\hat{y}}_{k/k-1})}^2}$

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

${\tilde{E}}_{i,k}=\frac{{\hat{d}}_{i,k}\cos {\hat{A}}_{i,k}\tan {\hat{E}}_{i,k}}{{\hat{D}}_{i,k}^2}{x}_k+\frac{{\hat{d}}_{i,k}\sin {\hat{A}}_{i,k}\tan {\hat{E}}_{i,k}}{{\hat{D}}_{i,k}^2}{y}_k-\frac{\cos {\hat{E}}_{i,k}}{{\hat{D}}_{i,k}}{z}_k+e_{i,k}---\left(20\right)$

其中

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

${\tilde{a}}_k={\[{\tilde{A}}_{1,k},{\tilde{H}}_{2,k},...{\tilde{A}}_{N,k},{\tilde{E}}_{1,k},{\tilde{E}}_{2,k},...{\tilde{E}}_{N,k}\]}^T---\left(21\right)$

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

${\tilde{a}}_k=H_k{s}_k+b_k---\left(22\right)$

这里

${\tilde{a}}_k=a_k-{\hat{a}}_k+H_k{\hat{s}}_{k/k-1}$

${\hat{a}}_k={\[{\hat{A}}_{1,k},{\hat{A}}_{2,k},...{\hat{A}}_{N,k},{\hat{E}}_{1,k},{\hat{E}}_{2,k},...{\hat{E}}_{N,k}\]}^T$

$H_k=\left[\begin{array}{cccccc}\frac{\sin {\hat{A}}_{1,k}}{{\hat{d}}_{1,k}}& 0& -\frac{\cos {\hat{A}}_{1,k}}{{\hat{d}}_{1,k}}& 0& 0& 0\\ \frac{\sin {\hat{A}}_{2,k}}{{\hat{d}}_{2,k}}& 0& -\frac{\cos {\hat{A}}_{2,k}}{{\hat{d}}_{2,k}}& 0& 0& 0\\ .& .& .& .& .& .\\ .& .& .& .& .& .\\ .& .& .& .& .& .\\ \frac{\sin {\hat{A}}_{N,k}}{{\hat{d}}_{N,k}}& 0& -\frac{\cos {\hat{A}}_{N,k}}{{\hat{d}}_{N,k}}& 0& 0& 0\\ \frac{{\hat{d}}_{1,k}\cos {\hat{A}}_{1,k}\tan {\hat{E}}_{1,k}}{{\hat{D}}_{1,k}^2}& 0& \frac{{\hat{d}}_{1,k}\sin {\hat{A}}_{1,k}\tan {\hat{E}}_{1,k}}{{\hat{D}}_{1,k}^2}& 0& -\frac{\cos {\hat{E}}_{1,k}}{{\hat{D}}_{1,k}}& 0\\ \frac{{\hat{d}}_{2,k}\cos {\hat{A}}_{2,k}\tan {\hat{E}}_{2,k}}{{\hat{D}}_{2,k}^2}& 0& \frac{{\hat{d}}_{2,k}\sin {\hat{A}}_{2,k}\tan {\hat{E}}_{2,k}}{{\hat{D}}_{2,k}^2}& 0& -\frac{\cos {\hat{E}}_{2,k}}{{\hat{D}}_{2,k}}& 0\\ .& .& .& .& .& .\\ .& .& .& .& .& .\\ .& .& .& .& .& .\\ \frac{{\hat{d}}_{N,k}\cos {\hat{A}}_{N,k}\tan {\hat{E}}_{N,k}}{{\hat{D}}_{N,k}^2}& 0& \frac{{\hat{d}}_{N,k}\sin {\hat{A}}_{N,k}\tan {\hat{E}}_{N,k}}{{\hat{D}}_{N,k}^2}& 0& -\frac{\cos {\hat{E}}_{N,k}}{{\hat{D}}_{N,k}}& 0\end{array}\right]$

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

$-L\left(s_k\right)={({\hat{s}}_{k/k-1}-s_k)}^T{p}_{k/k-1}^{-1}({\hat{s}}_{k/k-1}-s_k)+{({\tilde{a}}_k-H_k{s}_k)}^T{R}_k^{-1}({\tilde{a}}_k-H_k{s}_k)---\left(23\right)$

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

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

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

因为

${\▿}^2(-L(s_k\left)\right)=2(p_{k/k-1}^{-1}+H_k^T{R}_k^{-1}{H}_k)\≥0,$

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

$G_k={(p_{k/k-1}^{-1}+H_k^T{R}_k^{-1}{H}_k)}^{-1}{H}_k^T{R}_k^{-1}---\left(25\right)$

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

$G_k=p_{k/k-1}{H}_k^T{(H_k{p}_{k/k-1}{H}_k^T+R_k)}^{-1}---\left(26\right)$

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

$p_{k/k}=E\[({\hat{s}}_{k/k}-s_k){({\hat{s}}_{k/k}-s_k)}^T\]={(p_{k/k-1}^{-1}+H_k^T{R}_k^{-1}{H}_k)}^{-1}=(I-G_k{H}_k)p_{k/k-1}---\left(27\right)$

由上述推导结果可以看出，在给定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)所表示，

$s_{k+1}={\mathrm{Fs}}_k+U\stackrel{\‾}{a}+W---\left(28\right)$

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

$F=\left[\begin{array}{ccccccccc}1& T& f_{13}& 0& 0& 0& 0& 0& 0\\ 0& 1& f_{23}& 0& 0& 0& 0& 0& 0\\ 0& 0& f_{33}& 0& 0& 0& 0& 0& 0\\ 0& 0& 0& 1& T& f_{46}& 0& 0& 0\\ 0& 0& 0& 0& 1& f_{56}& 0& 0& 0\\ 0& 0& 0& 0& 0& f_{66}& 0& 0& 0\\ 0& 0& 0& 0& 0& 0& 1& T& f_{79}\\ 0& 0& 0& 0& 0& 0& 0& 1& f_{89}\\ 0& 0& 0& 0& 0& 0& 0& 0& f_{99}\end{array}\right]---\left(29\right)$

其中

$f_{13}=\frac{-1+a_{x}T+e^{-a_{x}T}}{a_x^2};f_{23}=\frac{1-e^{-a_{x}T}}{a_x};$

$f_{33}=1-e^{-a_{x}T};=\frac{-1+a_{y}T+e^{-a_{y}T}}{a_y^2};$

$f_{56}=\frac{1-e^{-a_{y}T}}{a_y};f_{66}=1-e^{-a_{y}T};f_{79}=\frac{-1+a_{z}T+e^{-a_{z}T}}{a_z^2};f_{89}=\frac{1-e^{-a_{z}T}}{a_z}$

$f_{99}=1-e^{-a_{z}T};$

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

其中

$\begin{array}{ccc}{u}_1=\frac{1}{a_x}(-T+\frac{a_x{T}^2}{2}+\frac{1-e^{-a_{x}T}}{a_x})& u_2=T-\frac{1-e^{-a_{x}T}}{a_x}& u_3=1-e^{-a_{x}T}\end{array}$

$\begin{array}{ccc}{u}_4=\frac{1}{a_y}(-T+\frac{a_y{T}^2}{2}+\frac{1-e^{-a_{y}T}}{a_y})& u_5=T-\frac{1-e^{-a_{y}T}}{a_y}& u_6=1-e^{-a_{y}T}\end{array}$

$\begin{array}{ccc}{u}_7=\frac{1}{a_z}(-T+\frac{a_z{T}^2}{2}+\frac{1-e^{-a_{z}T}}{a_z})& u_8=T-\frac{1-e^{-a_{z}T}}{a_z}& u_9=1-e^{-a_{z}T}\end{array}$

$\stackrel{\‾}{a}=\left[\begin{array}{ccc}{\stackrel{\‾}{a}}_x& {\stackrel{\‾}{a}}_y& {\stackrel{\‾}{a}}_z\end{array}\right]$

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

$Q=E\[{\mathrm{WW}}^T\]=2{\mathrm{a\σ}}_a^2{Q}_0---\left(31\right),$

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

${\hat{s}}_{k+1/k}=F{\hat{s}}_{k/k}+U\stackrel{\‾}{a}---\left(32\right),$

$p_{k+1/k}=E\[({\hat{s}}_{k+1/k}-s_{k+1}){({\hat{s}}_{k+1/k}-s_{k+1})}^T\]={\mathrm{FP}}_{k/k}F+Q---\left(33\right).$