CN104075710A - 一种基于航迹预测的机动扩展目标轴向姿态实时估计方法 - Google Patents

Abstract

本发明提供一种基于航迹预测的机动扩展目标轴向姿态实时估计方法，根据光电跟踪系统对目标的角位置数据与测距数据，实现根据目标的航迹预测实时对目标的轴向姿态进行估计，得到目标轴线在图像探测器中的俯仰角与偏航角，从而能够为目标的动力学分析、跟踪态势的决策、以及为图像处理所需的方向性基准提供依据。本发明只利用仪器本身的角位置信息和目标的距离信息，不依赖图像处理，不受目标外形调整影响，实时性好，精度高，适应性强。

Description

一种基于航迹预测的机动扩展目标轴向姿态实时估计方法

技术领域

本发明属于靶场测量及其它光电跟踪领域，具体涉及一种基于航迹预测的机动扩展目标的轴向姿态实时估计方法。

背景技术

在光电跟踪ATP(捕获跟综瞄准)系统中，仪器必须对目标进行高精度探测跟踪，才能有效完成跟踪与测量任务。对于机动目标速度快，距离近，在探测视场中一般占据20％以上的视场，表现为扩展目标。系统往往需要实时的获取目标在图像探测器中的姿态信息(俯仰角、偏航角)，为目标的动力学分析、跟踪态势的决策、以及为图像处理提供方向性基准信息提供依据。

目标在图像探测器中的姿态，可以由图像处理算法通过轮廓拟合或多特征点提取的方法给出；当图像轮廓比较规则，特征点清晰且不遮挡时，图像处理算法往往较准确的得到目标的俯仰角。但偏航角的精度一般较低，因为偏航角的计算需要准确知道目标的真实长度与成像投影的长度之比。偏航角的精度对该比值敏感。

实际跟踪中，目标的轮廓往往不规则。特征点在全程飞行中往往会出现遮挡的情况。因此往往不能通过图像连续准确的得到目标的姿态信息。给目标的动力学分析、态势估计等造成困难。

发明内容

为了提高对目标姿态估计的适应性，降低对图像处理的依赖。对于空间轨迹相对比较稳定的目标。考虑利用目标在空间的航迹指向来估计目标轴向的指向。通过解算航迹方向在图像探测器中的姿态，来估计目标在图像探测器中的姿态。

为了提高对机动目标的轨迹的实时响应能力以及削弱测量噪声对估计的影响，采用滤波预测的方法，从解算出的目标空间位置信息中实时滤波预测出其空间速度量指向，用该速度指向代替目标轴向的空间指向。这样处理可以显著提高轨迹预测的响应能力，并提高对机动目标的姿态估计精度。

本发明采用的技术方案为：一种基于航迹预测的机动扩展目标的轴向姿态实时估计方法，具体实现步骤如下：

(1)保证仪器跟踪上目标；

(2)建立视场中跟踪点A，虚拟参考点B，观测点O之间的空间关系；观测点O与参考点B的连线与为参考点的视轴，过A点且垂直与连线的的平面成为虚拟视平面，连线与虚拟视平面的交点为B1，即为AB在虚拟视平面上的投影；

(3)计算跟踪点A(x₀,y₀,z₀)在直角坐标系中的位置：

$\left\{\begin{array}{c}{y}_0=r\cos ({\mathrm{\β}}_0+\mathrm{\Δ}{E}_{\mathrm{tv}})\sin ({\mathrm{\α}}_0+\frac{{\mathrm{\ΔA}}_{\mathrm{tv}}}{\cos {\mathrm{\β}}_0})\\ x_0=r\cos ({\mathrm{\β}}_0+{\mathrm{\ΔE}}_{\mathrm{tv}})\cos ({\mathrm{\α}}_0+\frac{{\mathrm{\ΔA}}_{\mathrm{tv}}}{{\cos \mathrm{\β}}_0})\\ z_0=r\sin ({\mathrm{\β}}_0+{\mathrm{\ΔE}}_{\mathrm{tv}})\end{array}\right.---\left(1\right)$

其中：r：目标距离，α₀：跟踪点仪器方位角，β₀：跟踪点仪器俯仰角，ΔA_tv：跟踪点电视X方向脱靶量，ΔE_tv：跟踪点电视Y方向脱靶量；

(4)使用Kalman滤波在有噪声数据(x₀,y₀,z₀)序列中求准确的目标飞行速度矢量：

Kalman滤波的模型采用CA模型即常加速度模型，在X,Y,Z三个方向上加速度的波动作为零均值白噪声处理，则目标在X方向上的运动可以在状态空间中描述为：

$\left\{\begin{array}{c}{X}_{\left(k\right)}={\mathrm{\Φ}}_{(k,k-1)}{X}_{(k-1)}+{\mathrm{\Γ}}_{(k,k-1)}{W}_{(k-1)},X_{\left(k\right)}{|}_{k=0}=X_{\left(0\right)}\\ Y_{\left(k\right)}=C_{\left(k\right)}{X}_{\left(k\right)}+V_{\left(k\right)}\end{array}\right.---\left(2\right)$

取状态转移矩阵 ${\mathrm{\Φ}}_{(k,k-1)}=\left[\begin{array}{ccc}1& T& \frac{T^2}{2}\\ 0& 1& T\\ 0& 0& 1\end{array}\right],$ 观测矩阵C_(k)＝[1 0 0]，系统过程噪声输入矩阵Γ_(k,k-1)，V_(k)为观测噪声序列，其中T为滤波迭代周期。滤波参数取：模型方差q＝0.01，观测方差r＝1.0。

Kalman滤波的迭代频率为50Hz，经过迭代，即滤得跟踪点在X方向上的当帧速度同理，能够滤得跟踪点在Y方向和Z方向上的速度从而求得跟踪点当帧准确的速度矢量 $V=({\stackrel{\·}{x}}_0,{\stackrel{\·}{y}}_0,{\stackrel{\·}{z}}_0);$

(5)计算虚拟参考点B(x₁,y₁,z₁)的座标：

跟踪点座标与其速度矢量确定后，能够求得参考点B的座标为：

$x_1=x_0-l\frac{{\stackrel{\·}{x}}_0}{\sqrt{{{\stackrel{\·}{x}}_0}^2+{{\stackrel{\·}{y}}_0}^2+{{\stackrel{\·}{z}}_0}^2}}$

$y_1=y_0-l\frac{{\stackrel{\·}{y}}_0}{\sqrt{{{\stackrel{\·}{x}}_0}^2+{{\stackrel{\·}{y}}_0}^2+{{\stackrel{\·}{z}}_0}^2}}---\left(3\right)$

$z_1=z_0-l\frac{{\stackrel{\·}{z}}_0}{\sqrt{{{\stackrel{\·}{x}}_0}^2+{{\stackrel{\·}{y}}_0}^2+{{\stackrel{\·}{z}}_0}^2}}$

或

$x_1=x_0+l\frac{{\stackrel{\·}{x}}_0}{\sqrt{{{\stackrel{\·}{x}}_0}^2+{{\stackrel{\·}{y}}_0}^2+{{\stackrel{\·}{z}}_0}^2}}$

$y_1=y_0+l\frac{{\stackrel{\·}{y}}_0}{\sqrt{{{\stackrel{\·}{x}}_0}^2+{{\stackrel{\·}{y}}_0}^2+{{\stackrel{\·}{z}}_0}^2}}---\left(4\right)$

$z_1=z_0+l\frac{{\stackrel{\·}{z}}_0}{\sqrt{{{\stackrel{\·}{x}}_0}^2+{{\stackrel{\·}{y}}_0}^2+{{\stackrel{\·}{z}}_0}^2}}$

其中式(3)适用于虚拟参考点在跟踪点航迹偏后，式(4)适用于虚拟参考点在跟踪点航迹偏前；

其极坐标为：

${\mathrm{\&alpha;}}_1=\left\{\begin{array}{c}\arctan \left(\frac{y_1}{x_1}\right),x_1>0\\ \arctan \left(\frac{y_1}{x_1}\right)+\mathrm{\&pi;},x_1<0\end{array}\right.$

${\mathrm{\&beta;}}_1=\arctan \left(\frac{z_1}{\sqrt{{x_1}^2+{y_1}^2}}\right)---\left(5\right)$

$r_1=\sqrt{{x_1}^2+{y_1}^2+{z_1}^2}$

(6)计算虚拟参考点在假想视平面上的投影点B₁(x₂,y₂,z₂)的座标：

投影点是OB连线与过A的以OA连线为法线的平面的交点，通过解下列方程组求得B₁(x₂,y₂,z₂)座标：

$\left\{\begin{array}{c}\frac{X-x_1}{x_1}=\frac{Y-y_1}{y_1}\\ \frac{X-x_1}{x_1}=\frac{Z-z_1}{z_1}\\ x_0(X-x_0)+y_0(Y-y_0)+z_0(Z-z_0)=0\end{array}\right.---\left(6\right)$

解之得：

X＝x₁(x₀ ²+y₀ ²+z₀ ²)/(x₀x₁+y₀y₁+z₀z₁)＝x₂

Y＝y₁(x₀ ²+y₀ ²+z₀ ²)/(x₀x₁+y₀y₁+z₀z₁)＝y₂      (7)

Z＝z₁(x₀ ²+y₀ ²+z₀ ²)/(x₀x₁+y₀y₁+z₀z₁)＝z₂

该点极坐标为：

${\mathrm{\&alpha;}}_2=\left\{\begin{array}{c}\arctan \left(\frac{y_2}{x_2}\right),x_2>0\\ \arctan \left(\frac{y_2}{x_2}\right)+\mathrm{\&pi;},x_2<0\end{array}\right.$

${\mathrm{\&beta;}}_2=\arctan \left(\frac{z_2}{\sqrt{{x_2}^2+{y_2}^2}}\right)---\left(8\right)$

$r_2=\sqrt{{x_2}^2+{y_2}^2+{z_2}^2}$

(7)计算视场内的水平线对应的AC连线的方向矢量：

$\stackrel{\&RightArrow;}{\mathrm{AC}}=\left|\begin{array}{ccc}i& j& k\\ 0& 0& 1\\ x_0& y_0& z_0\end{array}\right|=-y_{0}i+x_{0}j---\left(9\right)$

(8)求矢量与矢量之间的夹角，即俯仰角：

设矢量 $\stackrel{\&RightArrow;}{(x_0-x_2,y_0-y_2,z_0-z_2)}=\stackrel{\&RightArrow;}{(x_3,y_3,z_3)}$ 则：

(9)求偏航角：

根据以上结果可得：

$\mathrm{\&gamma;}=\left\{\begin{array}{c}\arcsin \left(\frac{\sqrt{{x_3}^2+{y_3}^2+{z_3}^2}}{l}\right),r_1<r_0\\ \mathrm{\&pi;}-\arcsin \left(\frac{\sqrt{{x_3}^2+{y_3}^2+{z_3}^2}}{l}\right),r_1\&GreaterEqual;r_0\end{array}\right.---\left(11\right).$

进一步的，第(4)步所选用的Kalman滤波器(2)可以是三阶滤波器或更高阶的滤波器；滤波器的性能与r参数关系比较大，50Hz迭代时，一般取1.0；要提高滤波的快速性时，减小r的值，否则增大r的值。

本发明与现有技术相比的优点在于：

(1)本发明的核心是根据目标的航迹信息，实时解算目标的空间速度矢量，并利用Kalman滤波方法，能显著削弱噪声对于估计的影响。

(2)本发明不依赖图像提供的姿态信息，根据仪器角位置信息、距离信息即可完成目标轴向姿态的估计；需要的信息少，鲁棒性好，精度高(附图4为本发明解算的姿态数据与合作目标提供姿态对比)。

(3)本发明的结果，即实时解算出的目标姿态信息，能够为目标的动力学分析、跟踪态势的决策、以及为图像处理所需的方向性基准提供依据。

附图说明

图1为跟踪点，目标轴线上的虚拟参考点的关系示意图；

图2为跟踪点、轴上的虚拟参考点、参考点的投影点、观测点之间的空间几何关系示意图；

图3计算速度矢量Kalman滤波的迭代流程示意图；

图4本发明解算的姿态数据与合作目标提供的姿态的对比。

具体实施方式

下面结合附图以及具体实施例进一步说明本发明。

一种基于航迹预测的机动扩展目标的轴向姿态实时估计方法，基本原理是通过仪器的位置信息(仪器的方位、高低角度)，目标测距信息。通过空间关系实时解算出目标在电视探测器中的姿态信息(俯仰角，偏航角)。为了提高解算的精度，采用预测滤波的方法。工作原理如下：

目标在探测器中的情形如附图1所示，目标上A点一般为图像提取的特征点成为跟踪点，B点为目标轴线上的一个虚拟参考点。A、B间的距离一般为几米。对跟踪点、虚拟参考点，观测点进行抽象并建立空间关系如图2所示，图中A为跟踪点，B为虚拟参考点，O为观测点，观测点O与参考点B的连线与为参考点的视轴。过A点且垂直与连线的的平面成为虚拟视平面。连线与虚拟视平面的交点为B1。即为AB在虚拟视平面上的投影。

对以速度V飞行的机动扩展目标AB的跟踪瞄准过程中，目标相对仪器的方位、高角分别为α₀，β₀，距离r₀都可测量出，已知条件为扩展目标AB上跟踪点A点通过仪器观测及测距装置得到的俯仰角α₀，高低角β₀、距离r₀。需求出扩展目标AB的姿态角，即AB在视场内的投影AB1与视场水平线AC的夹角即俯仰角θ，以及AB矢量与OA矢量的夹角即偏航角γ。

具体实现步骤如下：

(1)保证仪器跟踪上目标。

(2)建立视场中跟踪点A，虚拟参考点B，观测点O之间的空间关系。观测点O与参考点B的连线与为参考点的视轴。过A点且垂直与连线的的平面成为虚拟视平面。连线与虚拟视平面的交点为B1。即为AB在虚拟视平面上的投影。如附图2所示。

(3)计算跟踪点A(x₀,y₀,z₀)在直角坐标系中的位置。

$\left\{\begin{array}{c}{y}_0=r\cos ({\mathrm{\&beta;}}_0+\mathrm{\&Delta;}{E}_{\mathrm{tv}})\sin ({\mathrm{\&alpha;}}_0+\frac{{\mathrm{\&Delta;A}}_{\mathrm{tv}}}{\cos {\mathrm{\&beta;}}_0})\\ x_0=r\cos ({\mathrm{\&beta;}}_0+{\mathrm{\&Delta;E}}_{\mathrm{tv}})\cos ({\mathrm{\&alpha;}}_0+\frac{{\mathrm{\&Delta;A}}_{\mathrm{tv}}}{{\cos \mathrm{\&beta;}}_0})\\ z_0=r\sin ({\mathrm{\&beta;}}_0+{\mathrm{\&Delta;E}}_{\mathrm{tv}})\end{array}\right.---\left(1\right)$

其中：r：目标距离α₀:跟踪点仪器方位角β₀:跟踪点仪器俯仰角ΔA_tv:跟踪点电视X方向脱靶量ΔE_tv：跟踪点电视Y方向脱靶量；

(4)使用Kalman滤波在有噪声数据(x₀,y₀,z₀)序列中求准确的目标飞行的速度矢量：

Kalman滤波的模型采用CA模型(常加速度模型)，在X,Y,Z三个方向上加速度的波动作为零均值白噪声处理。

则目标在某一维(如X方向)上的运动可以在状态空间中描述为：

$\left\{\begin{array}{c}{X}_{\left(k\right)}={\mathrm{\&Phi;}}_{(k,k-1)}{X}_{(k-1)}+{\mathrm{\&Gamma;}}_{(k,k-1)}{W}_{(k-1)},X_{\left(k\right)}{|}_{k=0}=X_{\left(0\right)}\\ Y_{\left(k\right)}=C_{\left(k\right)}{X}_{\left(k\right)}+V_{\left(k\right)}\end{array}\right.---\left(2\right)$

取状态转移矩阵 ${\mathrm{\&Phi;}}_{(k,k-1)}=\left[\begin{array}{ccc}1& T& \frac{T^2}{2}\\ 0& 1& T\\ 0& 0& 1\end{array}\right],$ 观测矩阵C_(k)＝[1 0 0]，系统过程噪声输入矩阵Γ_(k,k-1)，V_(k)为观测噪声序列，其中T为滤波迭代周期。滤波参数取：模型方差q＝0.01，观测方差r＝1.0。

Kalman滤波的迭代频率为50Hz，迭代方法如附图3所示。经过迭代，即滤得跟踪点在X方向上的当帧速度同理，能够滤得跟踪点在Y方向和Z方向上的速度从而求得跟踪点当帧准确的速度矢量

进一步的，第(4)步所选用的Kalman滤波器(2)可以是三阶滤波器或更高阶的滤波器；滤波器的性能与r参数关系比较大，50Hz迭代时，一般取1.0；要提高滤波的快速性时，减小r的值，否则增大r的值。

(5)计算虚拟参考点B(x₁,y₁,z₁)的座标。

跟踪点座标与其速度矢量确定后，可求得参考点B的座标为：

$x_1=x_0-l\frac{{\stackrel{\&CenterDot;}{x}}_0}{\sqrt{{{\stackrel{\&CenterDot;}{x}}_0}^2+{{\stackrel{\&CenterDot;}{y}}_0}^2+{{\stackrel{\&CenterDot;}{z}}_0}^2}},$

$y_1=y_0-l\frac{{\stackrel{\&CenterDot;}{y}}_0}{\sqrt{{{\stackrel{\&CenterDot;}{x}}_0}^2+{{\stackrel{\&CenterDot;}{y}}_0}^2+{{\stackrel{\&CenterDot;}{z}}_0}^2}},$

$z_1=z_0-l\frac{{\stackrel{\&CenterDot;}{z}}_0}{\sqrt{{{\stackrel{\&CenterDot;}{x}}_0}^2+{{\stackrel{\&CenterDot;}{y}}_0}^2+{{\stackrel{\&CenterDot;}{z}}_0}^2}}---\left(3\right)$

或 $x_1=x_0+l\frac{{\stackrel{\&CenterDot;}{x}}_0}{\sqrt{{{\stackrel{\&CenterDot;}{x}}_0}^2+{{\stackrel{\&CenterDot;}{y}}_0}^2+{{\stackrel{\&CenterDot;}{z}}_0}^2}},$

$y_1=y_0+l\frac{{\stackrel{\&CenterDot;}{y}}_0}{\sqrt{{{\stackrel{\&CenterDot;}{x}}_0}^2+{{\stackrel{\&CenterDot;}{y}}_0}^2+{{\stackrel{\&CenterDot;}{z}}_0}^2}},$

$z_1=z_0+l\frac{{\stackrel{\&CenterDot;}{z}}_0}{\sqrt{{{\stackrel{\&CenterDot;}{x}}_0}^2+{{\stackrel{\&CenterDot;}{y}}_0}^2+{{\stackrel{\&CenterDot;}{z}}_0}^2}}---\left(4\right)$

其中式3适用于参考点在跟踪点航迹偏后，式4适用于参考点在跟踪点航迹偏前。

其极坐标为：

${\mathrm{\&alpha;}}_1=\left\{\begin{array}{c}\arctan \left(\frac{y_1}{x_1}\right),x_1>0\\ \arctan \left(\frac{y_1}{x_1}\right)+\mathrm{\&pi;},x_1<0\end{array}\right.$

${\mathrm{\&beta;}}_1=\arctan \left(\frac{z_1}{\sqrt{{x_1}^2+{y_1}^2}}\right)$

$r_1=\sqrt{{x_1}^2+{y_1}^2+{z_1}^2}---\left(5\right)$

(6)计算虚拟参考点在假想视平面上的投影点B₁(x₂,y₂,z₂)的座标。

投影点是OB连线与过A的OA连线的法平面的交点，通过解下列方程组求得B₁(x₂,y₂,z₂)座标：

$\left\{\begin{array}{c}\frac{X-x_1}{x_1}=\frac{Y-y_1}{y_1}\\ \frac{X-x_1}{x_1}=\frac{Z-z_1}{z_1}\\ x_0(X-x_0)+y_0(Y-y_0)+z_0(Z-z_0)=0\end{array}\right.---\left(6\right)$

解之得：

X＝x₁(x₀ ²+y₀ ²+z₀ ²)/(x₀x₁+y₀y₁+z₀z₁)＝x₂

Y＝y₁(x₀ ²+y₀ ²+z₀ ²)/(x₀x₁+y₀y₁+z₀z₁)＝y₂      (7)

Z＝z₁(x₀ ²+y₀ ²+z₀ ²)/(x₀x₁+y₀y₁+z₀z₁)＝z₂

该点极坐标为：

${\mathrm{\&alpha;}}_2=\left\{\begin{array}{c}\arctan \left(\frac{y_2}{x_2}\right),x_2>0\\ \arctan \left(\frac{y_2}{x_2}\right)+\mathrm{\&pi;},x_2<0\end{array}\right.$

${\mathrm{\&beta;}}_2=\arctan \left(\frac{z_2}{\sqrt{{x_2}^2+{y_2}^2}}\right)$

$r_2=\sqrt{{x_2}^2+{y_2}^2+{z_2}^2}---\left(8\right)$

(7)计算视场内的水平线对应的AC连线(如附图1)的方向矢量：

$\stackrel{\&RightArrow;}{\mathrm{AC}}=\left|\begin{array}{ccc}i& j& k\\ 0& 0& 1\\ x_0& y_0& z_0\end{array}\right|=-y_{0}i+x_{0}j---\left(9\right)$

(8)求矢量与矢量之间的夹角，即俯仰角：

设矢量 $\stackrel{\&RightArrow;}{(x_0-x_2,y_0-y_2,z_0-z_2)}=\stackrel{\&RightArrow;}{(x_3,y_3,z_3)}$ 则：

(9)求偏航角。

根据以上结果可得：

$\mathrm{\&gamma;}=\left\{\begin{array}{c}\arcsin \left(\frac{\sqrt{{x_3}^2+{y_3}^2+{z_3}^2}}{l}\right),r_1<r_0\\ \mathrm{\&pi;}-\arcsin \left(\frac{\sqrt{{x_3}^2+{y_3}^2+{z_3}^2}}{l}\right),r_1\&GreaterEqual;r_0\end{array}\right.---\left(11\right).$

Claims (2)

1.一种基于航迹预测的机动扩展目标轴向姿态实时估计方法，其特征在于：实现步骤如下：

(1)保证仪器跟踪上目标；

(2)建立视场中跟踪点A，虚拟参考点B，观测点O之间的空间关系；观测点O与参考点B的连线与为参考点的视轴，过A点且垂直与连线的的平面成为虚拟视平面，连线与虚拟视平面的交点为B1，即为AB在虚拟视平面上的投影；

(3)计算跟踪点A(x₀,y₀,z₀)在直角坐标系中的位置：

$\left\{\begin{array}{c}{y}_0=r\cos ({\mathrm{\&beta;}}_0+\mathrm{\&Delta;}{E}_{\mathrm{tv}})\sin ({\mathrm{\&alpha;}}_0+\frac{{\mathrm{\&Delta;A}}_{\mathrm{tv}}}{\cos {\mathrm{\&beta;}}_0})\\ x_0=r\cos ({\mathrm{\&beta;}}_0+{\mathrm{\&Delta;E}}_{\mathrm{tv}})\cos ({\mathrm{\&alpha;}}_0+\frac{{\mathrm{\&Delta;A}}_{\mathrm{tv}}}{{\cos \mathrm{\&beta;}}_0})\\ z_0=r\sin ({\mathrm{\&beta;}}_0+{\mathrm{\&Delta;E}}_{\mathrm{tv}})\end{array}\right.---\left(1\right)$

其中：r：目标距离，α₀：跟踪点仪器方位角，β₀：跟踪点仪器俯仰角，ΔA_tv：跟踪点电视X方向脱靶量，ΔE_tv：跟踪点电视Y方向脱靶量；

(4)使用Kalman滤波在有噪声数据(x₀,y₀,z₀)序列中求准确的目标飞行速度矢量：

Kalman滤波的模型采用CA模型即常加速度模型，在X,Y,Z三个方向上加速度的波动作为零均值白噪声处理，则目标在X方向上的运动在状态空间中描述为：

$\left\{\begin{array}{c}{X}_{\left(k\right)}={\mathrm{\&Phi;}}_{(k,k-1)}{X}_{(k-1)}+{\mathrm{\&Gamma;}}_{(k,k-1)}{W}_{(k-1)},X_{\left(k\right)}{|}_{k=0}=X_{\left(0\right)}\\ Y_{\left(k\right)}=C_{\left(k\right)}{X}_{\left(k\right)}+V_{\left(k\right)}\end{array}\right.---\left(2\right)$

取状态转移矩阵 ${\mathrm{\&Phi;}}_{(k,k-1)}=\left[\begin{array}{ccc}1& T& \frac{T^2}{2}\\ 0& 1& T\\ 0& 0& 1\end{array}\right],$ 观测矩阵C_(k)＝[1 0 0]，系统过程噪声输入矩阵Γ_(k,k-1)，V_(k)为观测噪声序列，其中T为滤波迭代周期，滤波参数取：模型方差q＝0.01，观测方差r＝1.0；

Kalman滤波的迭代频率为50Hz，经过迭代，即滤得跟踪点在X方向上的当帧速度同理，能够滤得跟踪点在Y方向和Z方向上的速度从而求得跟踪点当帧准确的速度矢量 $V=({\stackrel{\&CenterDot;}{x}}_0,{\stackrel{\&CenterDot;}{y}}_0,{\stackrel{\&CenterDot;}{z}}_0);$

(5)计算虚拟参考点B(x₁,y₁,z₁)的座标：

跟踪点座标与其速度矢量确定后，能够求得参考点B的座标为：

$x_1=x_0-l\frac{{\stackrel{\&CenterDot;}{x}}_0}{\sqrt{{{\stackrel{\&CenterDot;}{x}}_0}^2+{{\stackrel{\&CenterDot;}{y}}_0}^2+{{\stackrel{\&CenterDot;}{z}}_0}^2}}$

$y_1=y_0-l\frac{{\stackrel{\&CenterDot;}{y}}_0}{\sqrt{{{\stackrel{\&CenterDot;}{x}}_0}^2+{{\stackrel{\&CenterDot;}{y}}_0}^2+{{\stackrel{\&CenterDot;}{z}}_0}^2}}---\left(3\right)$

$z_1=z_0-l\frac{{\stackrel{\&CenterDot;}{z}}_0}{\sqrt{{{\stackrel{\&CenterDot;}{x}}_0}^2+{{\stackrel{\&CenterDot;}{y}}_0}^2+{{\stackrel{\&CenterDot;}{z}}_0}^2}}$

或

$x_1=x_0+l\frac{{\stackrel{\&CenterDot;}{x}}_0}{\sqrt{{{\stackrel{\&CenterDot;}{x}}_0}^2+{{\stackrel{\&CenterDot;}{y}}_0}^2+{{\stackrel{\&CenterDot;}{z}}_0}^2}}$

$y_1=y_0+l\frac{{\stackrel{\&CenterDot;}{y}}_0}{\sqrt{{{\stackrel{\&CenterDot;}{x}}_0}^2+{{\stackrel{\&CenterDot;}{y}}_0}^2+{{\stackrel{\&CenterDot;}{z}}_0}^2}}---\left(4\right)$

$z_1=z_0+l\frac{{\stackrel{\&CenterDot;}{z}}_0}{\sqrt{{{\stackrel{\&CenterDot;}{x}}_0}^2+{{\stackrel{\&CenterDot;}{y}}_0}^2+{{\stackrel{\&CenterDot;}{z}}_0}^2}}$

其中式(3)适用于虚拟参考点在跟踪点航迹偏后，式(4)适用于虚拟参考点在跟踪点航迹偏前；

其极坐标为：

${\mathrm{\&alpha;}}_1=\left\{\begin{array}{c}\arctan \left(\frac{y_1}{x_1}\right),x_1>0\\ \arctan \left(\frac{y_1}{x_1}\right)+\mathrm{\&pi;},x_1<0\end{array}\right.$

${\mathrm{\&beta;}}_1=\arctan \left(\frac{z_1}{\sqrt{{x_1}^2+{y_1}^2}}\right)---\left(5\right)$

$r_1=\sqrt{{x_1}^2+{y_1}^2+{z_1}^2}$

(6)计算虚拟参考点在假想视平面上的投影点B₁(x₂,y₂,z₂)的座标：

投影点是OB连线与过A的以OA连线为法线的平面的交点，通过解下列方程组求得B₁(x₂,y₂,z₂)座标：

$\left\{\begin{array}{c}\frac{X-x_1}{x_1}=\frac{Y-y_1}{y_1}\\ \frac{X-x_1}{x_1}=\frac{Z-z_1}{z_1}\\ x_0(X-x_0)+y_0(Y-y_0)+z_0(Z-z_0)=0\end{array}\right.---\left(6\right)$

解之得：

X＝x₁(x₀ ²+y₀ ²+z₀ ²)/(x₀x₁+y₀y₁+z₀z₁)＝x₂

Y＝y₁(x₀ ²+y₀ ²+z₀ ²)/(x₀x₁+y₀y₁+z₀z₁)＝y₂      (7)

Z＝z₁(x₀ ²+y₀ ²+z₀ ²)/(x₀x₁+y₀y₁+z₀z₁)＝z₂

该点极坐标为：

${\mathrm{\&alpha;}}_2=\left\{\begin{array}{c}\arctan \left(\frac{y_2}{x_2}\right),x_2>0\\ \arctan \left(\frac{y_2}{x_2}\right)+\mathrm{\&pi;},x_2<0\end{array}\right.$

${\mathrm{\&beta;}}_2=\arctan \left(\frac{z_2}{\sqrt{{x_2}^2+{y_2}^2}}\right)---\left(8\right)$

$r_2=\sqrt{{x_2}^2+{y_2}^2+{z_2}^2}$

(7)计算视场内的水平线对应的AC连线的方向矢量：

$\stackrel{\&RightArrow;}{\mathrm{AC}}=\left|\begin{array}{ccc}i& j& k\\ 0& 0& 1\\ x_0& y_0& z_0\end{array}\right|=-y_{0}i+x_{0}j---\left(9\right)$

(8)求矢量与矢量之间的夹角，即俯仰角：

设矢量则：

(9)求偏航角：

根据以上结果得：

$\mathrm{\&gamma;}=\left\{\begin{array}{c}\arcsin \left(\frac{\sqrt{{x_3}^2+{y_3}^2+{z_3}^2}}{l}\right),r_1<r_0\\ \mathrm{\&pi;}-\arcsin \left(\frac{\sqrt{{x_3}^2+{y_3}^2+{z_3}^2}}{l}\right),r_1\&GreaterEqual;r_0\end{array}\right.---\left(11\right).$

2.根据权利要求1所述的基于航迹预测的机动扩展目标轴向姿态实时估计方法，其特征在于第(4)步所选用的Kalman滤波器是三阶滤波器或更高阶的滤波器；滤波器的性能与r参数关系比较大，50Hz迭代时，一般取1.0；要提高滤波的快速性时，减小r的值，否则增大r的值。