CN105486308A - 估计弹目视线角速率的快收敛Kalman滤波器的设计方法 - Google Patents

Abstract

估计弹目视线角速率的快收敛Kalman滤波器的设计方法，属于导弹制导控制技术领域。本发明为了提高现有的目标—导弹视线角速度估计Kalman滤波方法的收敛速度。首先设置滤波器初始估计值；然后测算当前时刻目标与导弹之间的相对距离和相对速度；测算导弹加速度在视线坐标系o′y₄和o′z₄轴方向上的分量；最后由导弹俯仰通道视线运动状态方程和俯仰通道视线角测量方程构造俯仰通道快收敛Kalman滤波器，由导弹偏航通道视线运动状态方程和偏航通道视线角测量方程构造偏航通道快收敛Kalman滤波器，从而分别求出目标与导弹之间的视线俯仰角速率和视线偏航角速率。本发明提高Kalman滤波器的收敛速度，得到高精度的视线角速率估计。

Description

估计弹目视线角速率的快收敛Kalman滤波器的设计方法

技术领域

本发明涉及导弹的精确制导方法，属于导弹制导控制技术领域，具体涉及大目标—导弹相对运动信息精确估计方法。

背景技术

近些年来，为了降低其成本，提高其可靠性，精确制导武器越来越多地采用小型捷联或半捷联导引头，这种导引头的特点是可以测量目标—导弹之间的相对视线角，如果是主动式导引头还可以测量距离信息，但捷联或半捷联导引头不能直接输出实际制导律需要的视线角速率信息。

针对安装捷联导引头的导弹这一应用背景，极坐标形式的目标—导弹相对运动运动学和动力学方程构造二维的Kalman滤波器，可以得到高精度的视线角速率估计。为了进一步提高这种滤波器的收敛速度，我们提出了一种通过自适应调整滤波器中的状态噪声协方差阵和测量噪声协方差阵，并恰当选用初值来提高收敛速度的估计视线角速率的快收敛Kalman滤波器。

发明内容

本发明的目的是提供一种估计弹目视线角速率的快收敛Kalman滤波器的设计方法，以提高现有的目标—导弹视线角速度估计Kalman滤波方法的收敛速度。所述弹目视线角速率是指导弹到目标的视线和发射点惯性坐标的夹角的变化率。

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

一、滤波器初始估计中的视线俯仰角和视线偏航角的初值和取为根据导引头第1拍测量值计算出来的视线俯仰角和视线偏航角，视线俯仰角速率和视线偏航角速率的初值和利用几何方法计算出来，即

${\widehat{\stackrel{\·}{q}}}_{\mathrm{\ϵ}}\left(0\right)=\frac{\left({\mathrm{\Δx}}^2+{\mathrm{\Δz}}^2\right)\mathrm{\Δ}\stackrel{\·}{y}-\mathrm{\Δ}y\left(\mathrm{\Δ}x\mathrm{\Δ}\stackrel{\·}{x}+\mathrm{\Δ}z\mathrm{\Δ}\stackrel{\·}{z}\right)}{({\mathrm{\Δx}}^2+{\mathrm{\Δy}}^2+{\mathrm{\Δz}}^2)\sqrt{{\mathrm{\Δx}}^2+{\mathrm{\Δz}}^2}}---\left(1\right)$

${\widehat{\stackrel{\·}{q}}}_{\mathrm{\β}}\left(0\right)=\frac{\mathrm{\Δ}z\mathrm{\Δ}\stackrel{\·}{x}-\mathrm{\Δ}x\mathrm{\Δ}\stackrel{\·}{z}}{{\mathrm{\Δx}}^2+{\mathrm{\Δz}}^2}---\left(2\right)$

其中，Δx、Δy、Δz为末制导初始时刻目标—导弹相对位置在末制导惯性参考坐标系三个轴上的分量，为末制导初始时刻目标—导弹相对速度在末制导惯性参考坐标系三个轴上的分量，它们的具体计算过程如下：设末制导惯性参考坐标系o₀x₀y₀z₀在末制导过程中相对于惯性空间固化不动，它与地心惯性坐标系以及发射点惯性坐标系的关系如图1所示。

末制导惯性参考坐标系的原点为末制导初始时刻导弹的质心o₀，o₀x₀沿着末制导初始时刻视线方向，指向目标为正，o₀y₀轴位于包含o₀x₀轴的铅垂面内，指向上为正，o₀z₀轴由右手定则确定。发射点惯性坐标系的原点为发射点o_F，o_Fy_F轴铅锤向上，o_Fx_F轴和o_Fz_F轴位于水平面内，其指向按需要选取，通常o_Fx_Fy_F为射击平面。地心惯性坐标系的原点o_I位于地心上，o_Ix_I轴位于赤道平面内，指向某一恒星，o_Iz_I轴指向北极方向，o_Iy_I按右手定则确定。地心惯性坐标系与发射点惯性坐标系之间的转换取决于λ₀和α_F三个角度，它们的定义见图1。图1中,q_ε0和q_β0分别为末制导初始时刻视线相对于发射点惯性坐标系所成的视线仰角和视线偏角。

设导弹在末制导惯性参考坐标系的位置为(x、y、z)，其速度在该坐标系中的投影为这些信息由弹上导航系统提供；目标在末制导惯性参考坐标系的位置为(x_t、y_t、z_t)，其速度在该坐标系中的投影为这些信息由地面上的目标测量系统提供，则Δx＝x_t-x，Δy＝y_t-y，Δz＝z_t-z，

这样，在很大程度上降低了滤波器的初始动态过程。

二、测算当前时刻目标与导弹之间的相对距离和相对速度，具体过程如下：记当前时刻相对位置在末制导惯性参考坐标系中的分量为Δx＝x_t-x，Δy＝y_t-y，Δz＝z_t-z，当前时刻相对速度在末制导惯性参考坐标系中的分量为则目标—导弹相对距离为相对速度为 $\stackrel{\·}{R}=(\mathrm{\Δ}x\mathrm{\Δ}\stackrel{\·}{x}+\mathrm{\Δ}y\mathrm{\Δ}\stackrel{\·}{y}+\mathrm{\Δ}\stackrel{\·}{z}\mathrm{\Δ}\stackrel{\·}{z})/R.;$ 在末制导过程中，始终有R>0，

三、测算导弹加速度在视线坐标系o′x₄y₄z₄(见图2)o′y₄和o′z₄轴方向上的分量a_ε和a_β，以及目标加速度在视线坐标系o′y₄和o′z₄轴方向上的分量a_tε和a_tβ；导弹上加速度计输出的加速度是沿着弹体坐标系ox₁y₁z₁(见图1)投影的，把它投影到视线坐标系即可求得导弹法向加速度a_ε和a_β；由图3所示，导弹弹体坐标系ox₁y₁z₁的原点o位于导弹质心上。ox₁轴与弹体纵轴重合，指向头部为正，oy₁轴在弹体纵向对称平面内，垂直ox₁轴，指向上方为正，oz₁轴方向按右手定则确定；由图2，视线坐标系o′x₄y₄z₄原点o′位于导引头回转中心,o′x₄轴与目标—导弹视线一致,由导引头回转中心指向目标为正，o′y₄轴位于包含o′x₄轴的铅锤面内，与o′x₄轴垂直，指向上方为正，o′z₄轴按右手定则确定。由末制导惯性坐标系转换到视线坐标系定义了视线俯仰角q_ε和视线偏航角q_β(见图2)。根据目标轨道测量数据可以计算出目标加速度在地心惯性坐标系中的三个分量，然后投影到视线坐标系求得a_tε和a_tβ；

四、由导弹的俯仰通道视线运动状态方程和俯仰通道视线角测量方程构造俯仰通道快收敛Kalman滤波算法，从而求出目标与导弹之间的视线俯仰角速率，由导弹的偏航通道视线运动状态方程和偏航通道视线角测量方程构造偏航通道快收敛Kalman滤波算法，从而求出目标与导弹之间的视线偏航角速率。

俯仰通道视线运动离散化状态方程为

$\left\{\begin{array}{c}{q}_{\mathrm{\ϵ}}(k+1)=q_{\mathrm{\ϵ}}\left(k\right)+\mathrm{\Δ}t{\stackrel{\·}{q}}_{\mathrm{\ϵ}}\left(k\right)\\ {\stackrel{\·}{q}}_{\mathrm{\ϵ}}(k+1)=\[1-\frac{2\stackrel{\·}{R}\left(k\right)}{R\left(k\right)}\mathrm{\Δ}t\]{\stackrel{\·}{q}}_{\mathrm{\ϵ}}\left(k\right)+\frac{\mathrm{\Δ}t}{R\left(k\right)}\[a_{t\mathrm{\ϵ}}\left(k\right)-a_{\mathrm{\ϵ}}\left(k\right)\]\end{array}\right.---\left(3\right)$

其中，k代表第k个采样时刻，Δt为采样周期；

俯仰通道视线角测量方程可以写作

$Z_{\mathrm{\ϵ}}\left(k\right)=q_{\mathrm{\ϵ}}\left(k\right)+{\mathrm{\ν}}_{\mathrm{\ϵ}}\left(k\right)=H_{\mathrm{\ϵ}}{\left[\begin{array}{cc}{q}_{\mathrm{\ϵ}}\left(k\right)& {\stackrel{\·}{q}}_{\mathrm{\ϵ}}\left(k\right)\end{array}\right]}^T+{\mathrm{\ν}}_{\mathrm{\ϵ}}\left(k\right)---\left(4\right)$

其中，H_ε＝[10]，ν_ε(k)为视线俯仰角的测量噪声，假设其均值为零。

设 $E\[w_{\mathrm{\ϵ}}\left(k\right)w_{\mathrm{\ϵ}}^T\left(k\right)\]=Q_{\mathrm{\ϵ}0}\left(k\right),E\[{\mathrm{\ν}}_{\mathrm{\ϵ}}\left(k\right){\mathrm{\ν}}_{\mathrm{\ϵ}}^T\left(k\right)\]=R_{\mathrm{\ϵ}0}\left(k\right),$

$X_{\mathrm{\ϵ}}=\left[\begin{array}{c}{q}_{\mathrm{\ϵ}}\\ {\stackrel{\·}{q}}_{\mathrm{\ϵ}}\end{array}\right],u_{\mathrm{\ϵ}}=\left[\begin{array}{c}0\\ a_{t\mathrm{\ϵ}}-a_{\mathrm{\ϵ}}\end{array}\right],{\mathrm{\Φ}}_{\mathrm{\ϵ}}=\left[\begin{array}{cc}1& \mathrm{\Δ}t\\ 0& 1-\mathrm{\Δ}t2\stackrel{\·}{R}/R\end{array}\right],B_{\mathrm{\ϵ}}=\left[\begin{array}{c}0\\ \mathrm{\Δ}t/R\end{array}\right],$

则俯仰通道快收敛Kalman滤波器为

$\left\{\begin{array}{c}{\stackrel{\‾}{X}}_{\mathrm{\ϵ}}\left(k+1\right)={\mathrm{\Φ}}_{\mathrm{\ϵ}}\left(k\right){\hat{X}}_{\mathrm{\ϵ}}\left(k\right)+B_{\mathrm{\ϵ}}\left(k\right)u_{\mathrm{\ϵ}}\left(k\right)\\ Q_{\mathrm{\ϵ}}\left(k\right)={\mathrm{\γe}}_{\mathrm{\ϵ}}^T\left(k\right)e_{\mathrm{\ϵ}}\left(k\right)I_n+Q_{\mathrm{\ϵ}0}\left(k\right)\\ P_{\mathrm{\ϵ}}\left(k+1/k\right)={\mathrm{\Φ}}_{\mathrm{\ϵ}}\left(k\right)P_{\mathrm{\ϵ}}\left(k\right){\mathrm{\Φ}}_{\mathrm{\ϵ}}^T\left(k\right)+Q_{\mathrm{\ϵ}}\left(k\right)\\ R_{\mathrm{\ϵ}}\left(k+1\right)={\mathrm{\λH}}_{\mathrm{\ϵ}}{P}_{\mathrm{\ϵ}}\left(k+1/k\right)H_{\mathrm{\ϵ}}^T+R_{\mathrm{\ϵ}0}\left(k+1\right)\\ K_{\mathrm{\ϵ}}\left(k+1\right)=P_{\mathrm{\ϵ}}\left(k+1/k\right)H_{\mathrm{\ϵ}}^T{\[H_{\mathrm{\ϵ}}{P}_{\mathrm{\ϵ}}\left(k+1/k\right)H_{\mathrm{\ϵ}}^T+R_{\mathrm{\ϵ}}\left(k+1\right)\]}^{-1}\\ e_{\mathrm{\ϵ}}\left(k+1\right)=Z_{\mathrm{\ϵ}}\left(k+1\right)-H_{\mathrm{\ϵ}}{\stackrel{\‾}{X}}_{\mathrm{\ϵ}}\left(k+1\right)\\ {\hat{X}}_{\mathrm{\ϵ}}\left(k+1\right)={\stackrel{\‾}{X}}_{\mathrm{\ϵ}}\left(k+1\right)+K_{\mathrm{\ϵ}}\left(k+1\right)e_{\mathrm{\ϵ}}\left(k+1\right)\\ P_{\mathrm{\ϵ}}\left(k+1\right)=\[I-K_{\mathrm{\ϵ}}\left(k+1\right)H_{\mathrm{\ϵ}}\]P_{\mathrm{\ϵ}}\left(k+1/k\right)\end{array}\right.$

其中，和分别代表X_ε的滤波估计和预报估计，γ选择一个充分大的正值

$P_{\mathrm{\ϵ}}(k+1/k)=E\{\[X_{\mathrm{\ϵ}}(k+1)-{\stackrel{\‾}{X}}_{\mathrm{\ϵ}}(k+1)\]{\[X_{\mathrm{\ϵ}}(k+1)-{\stackrel{\‾}{X}}_{\mathrm{\ϵ}}(k+1)\]}^T\},$

$P_{\mathrm{\ϵ}}(k+1)=E\{\[X_{\mathrm{\ϵ}}(k+1)-{\hat{X}}_{\mathrm{\ϵ}}(k+1)\]{\[X_{\mathrm{\ϵ}}(k+1)-{\hat{X}}_{\mathrm{\ϵ}}(k+1)\]}^T\},$

λ是一个正的标量。若γ选择一个充分大的正值，可以满足在最坏的初始条件下也具有较好的滤波收敛特性。但γ选取得过大会对滤波器的稳态特性有一定影响，在应用中应该采取一种折中的选择。另外，为了确保稳态阶段，R_k更接近于或等于R_k0，λ不易选择过大。

根据导弹的偏航通道视线运动状态方程和偏航通道视线角测量方程可以设计出估计目标与导弹之间偏航通道视线角速率的快收敛Kalman滤波器：

偏航通道视线运动离散化状态方程写作

X_β(k+1)＝Φ_β(k)X_β(k)+Β_β(k)u_β(k)+w_β(k)(5)

其中，

$X_{\mathrm{\β}}=\left[\begin{array}{c}{q}_{\mathrm{\β}}\\ {\stackrel{\·}{q}}_{\mathrm{\β}}\end{array}\right],u_{\mathrm{\β}}=\left[\begin{array}{c}0\\ a_{\mathrm{\β}}-a_{t\mathrm{\β}}\end{array}\right],{\mathrm{\Φ}}_{\mathrm{\β}}=\left[\begin{array}{cc}1& \mathrm{\Δ}t\\ 0& 1-\mathrm{\Δ}t2\stackrel{\·}{R}/R\end{array}\right],B_{\mathrm{\β}}=\left[\begin{array}{c}0\\ \mathrm{\Δ}t/R\end{array}\right]$

w_β(k)是一个零均值随机过程，

偏航视线角测量方程写作

$Z_{\mathrm{\β}}\left(k\right)=q_{\mathrm{\β}}\left(k\right)+{\mathrm{\ν}}_{\mathrm{\β}}\left(k\right)=H_{\mathrm{\β}}{\left[\begin{array}{cc}{q}_{\mathrm{\β}}\left(k\right)& {\stackrel{\·}{q}}_{\mathrm{\β}}\left(k\right)\end{array}\right]}^T+{\mathrm{\ν}}_{\mathrm{\β}}\left(k\right)---\left(6\right)$

其中，H_β＝[10]，ν_β(k)为视线俯仰角的测量噪声，假设其均值为零， $E\[{\mathrm{\ν}}_{\mathrm{\β}}\left(k\right){\mathrm{\ν}}_{\mathrm{\β}}^T\left(k\right)\]=R_{\mathrm{\β}0}\left(k\right);$

偏航通道Kalman滤波器为

$\left\{\begin{array}{c}{\stackrel{\‾}{X}}_{\mathrm{\β}}\left(k+1\right)={\mathrm{\Φ}}_{\mathrm{\β}}\left(k\right){\hat{X}}_{\mathrm{\β}}\left(k\right)+B_{\mathrm{\β}}\left(k\right)u_{\mathrm{\β}}\left(k\right)\\ Q_{\mathrm{\β}}\left(k\right)={\mathrm{\γe}}_{\mathrm{\β}}^T\left(k\right)e_{\mathrm{\β}}\left(k\right)I_n+Q_{\mathrm{\β}0}\left(k\right)\\ P_{\mathrm{\β}}\left(k+1/k\right)={\mathrm{\Φ}}_{\mathrm{\β}}\left(k\right)P_{\mathrm{\β}}\left(k\right){\mathrm{\Φ}}_{\mathrm{\β}}^T\left(k\right)+Q_{\mathrm{\β}}\left(k\right)\\ R_{\mathrm{\β}}\left(k+1\right)={\mathrm{\λH}}_{\mathrm{\β}}{P}_{\mathrm{\β}}\left(k+1/k\right)H_{\mathrm{\β}}^T+R_{\mathrm{\β}0}\left(k+1\right)\\ K_{\mathrm{\β}}\left(k+1\right)=P_{\mathrm{\β}}\left(k+1/k\right)H_{\mathrm{\β}}^T{\[H_{\mathrm{\β}}{P}_{\mathrm{\β}}\left(k+1/k\right)H_{\mathrm{\β}}^T+R_{\mathrm{\β}}\left(k+1\right)\]}^{-1}\\ e_{\mathrm{\β}}\left(k+1\right)=Z_{\mathrm{\β}}\left(k+1\right)-H_{\mathrm{\β}}{\stackrel{\‾}{X}}_{\mathrm{\β}}\left(k+1\right)\\ {\hat{X}}_{\mathrm{\β}}\left(k+1\right)={\stackrel{\‾}{X}}_{\mathrm{\β}}\left(k+1\right)+K_{\mathrm{\β}}\left(k+1\right)e_{\mathrm{\β}}\left(k+1\right)\\ P_{\mathrm{\β}}\left(k+1\right)=\[I-K_{\mathrm{\β}}\left(k+1\right)H_{\mathrm{\β}}\]P_{\mathrm{\β}}\left(k+1/k\right)\end{array}\right.$

其中，和分别代表X_β的滤波估计和预报估计，

$P_{\mathrm{\β}}(k+1/k)=E\{\[X_{\mathrm{\β}}(k+1)-{\stackrel{\‾}{X}}_{\mathrm{\β}}(k+1)\]{\[X_{\mathrm{\β}}(k+1)-{\stackrel{\‾}{X}}_{\mathrm{\β}}(k+1)\]}^T\},$

$P_{\mathrm{\β}}(k+1)=E\{\[X_{\mathrm{\β}}(k+1)-{\hat{X}}_{\mathrm{\β}}(k+1)\]{\[X_{\mathrm{\β}}(k+1)-{\hat{X}}_{\mathrm{\β}}(k+1)\]}^T\},$ λ和γ(为两个参量，而且在滤波过程中取为常量)的取值规律与俯仰通道快收敛Kalman滤波器相同。

本发明的有益效果是：

由导弹的俯仰通道视线运动状态方程和俯仰通道视线角测量方程构造俯仰通道快收敛Kalman滤波器，从而求出目标与导弹之间的视线俯仰角速率，由导弹的偏航通道视线运动状态方程和偏航通道视线角测量方程构造偏航通道快收敛Kalman滤波器，从而求出目标与导弹之间的视线偏航角速率，最终完成目标与导弹之间的视线角速率的估计。

本发明提出的估计视线角速度的快收敛Kalman滤波器的收敛速度高于现有的Kalman滤波器。本发明通过自适应调整滤波器中的状态噪声协方差阵和测量噪声协方差阵，并恰当选用初值来提高收敛速度的估计视线角速率的快收敛Kalman滤波器。提高了极坐标形式的目标—导弹相对运动运动学和动力学方程构造二维的Kalman滤波器的收敛速度，得到了高精度的视线角速率估计。

附图说明

图1是地心惯性坐标系和发射点惯性坐标系关系图；图2是发射点惯性坐标系和视线坐标系关系图；图3是导弹弹体坐标系示意图；

图4是快收敛Kalman滤波器视线俯仰角速率完整跟踪过程图(γ＝10²，)；

图5是快收敛Kalman滤波器视线俯仰角速率初段动态跟踪过程图(γ＝10²，)；

图6是快收敛Kalman滤波器视线俯仰角速率稳态跟踪过程图(γ＝10²，)；

图7是快收敛Kalman滤波器视线俯仰角速率末段动态跟踪过程图(γ＝10²，)；

图8是快收敛Kalman滤波器视线偏航角速率完整跟踪过程图(γ＝10²，)；

图9是快收敛Kalman滤波器视线偏航角速率初段动态跟踪过程图(γ＝10²，)；

图10是快收敛Kalman滤波器视线偏航角速率稳态跟踪过程图(γ＝10²，)；

图11是快收敛Kalman滤波器视线偏航角速率末段动态跟踪过程图(γ＝10²，)；

图12是Kalman滤波器视线俯仰角速率完整跟踪过程图

图13是Kalman滤波器视线俯仰角速率初段动态跟踪过程图

图14是Kalman滤波器视线俯仰角速率稳态跟踪过程图

图15是Kalman滤波器视线俯仰角速率末段动态跟踪过程图

图16是Kalman滤波器视线偏航角速率完整跟踪过程图

图17是Kalman滤波器视线偏航角速率初段动态跟踪过程

图18是Kalman滤波器视线偏航角速率稳态跟踪过程图

图19是Kalman滤波器视线偏航角速率末段动态跟踪过程图

具体实施方式

下面通过数学推导演示本发明Kalman滤波器的由来：

(一)目标—导弹相对运动动力学

设目标—导弹视线矢量可以表示为

其中，和分别代表目标和导弹在发射点惯性坐标系中的位置矢量。上式相对时间微分，得到

其中，和分别代表视线矢量在惯性坐标系和视线坐标系中相对时间求导，代表视线坐标系相对惯性坐标系的旋转角速度，和分别代表目标和导弹速度矢量。上式写成在视线坐标系中的投影形式：

其中，

$L\left(q_{\mathrm{\ϵ}}\right)=\left[\begin{array}{ccc}{\mathrm{cosq}}_{\mathrm{\ϵ}}& {\mathrm{sinq}}_{\mathrm{\ϵ}}& 0\\ -{\mathrm{sinq}}_{\mathrm{\ϵ}}& {\mathrm{cosq}}_{\mathrm{\ϵ}}& 0\\ 0& 0& 1\end{array}\right]$

则

$\begin{array}{c}\left[\begin{array}{c}\stackrel{\·}{R}\\ 0\\ 0\end{array}\right]+\left[\begin{array}{c}{\stackrel{\·}{q}}_{\mathrm{\β}}\sin q_{\mathrm{\ϵ}}\\ {\stackrel{\·}{q}}_{\mathrm{\β}}\cos q_{\mathrm{\ϵ}}\\ {\stackrel{\·}{q}}_{\mathrm{\ϵ}}\end{array}\right]\×\left[\begin{array}{c}R\\ 0\\ 0\end{array}\right]=\left[\begin{array}{c}\stackrel{\·}{R}\\ 0\\ 0\end{array}\right]+\left[\begin{array}{ccc}0& -{\stackrel{\·}{q}}_{\mathrm{\ϵ}}& {\stackrel{\·}{q}}_{\mathrm{\β}}\cos q_{\mathrm{\ϵ}}\\ {\stackrel{\·}{q}}_{\mathrm{\ϵ}}& 0& -{\stackrel{\·}{q}}_{\mathrm{\β}}\sin q_{\mathrm{\ϵ}}\\ -{\stackrel{\·}{q}}_{\mathrm{\β}}\cos q_{\mathrm{\ϵ}}& {\stackrel{\·}{q}}_{\mathrm{\β}}\sin q_{\mathrm{\ϵ}}& 0\end{array}\right]\left[\begin{array}{c}R\\ 0\\ 0\end{array}\right]=\left[\begin{array}{c}\stackrel{\·}{R}\\ R{\stackrel{\·}{q}}_{\mathrm{\ϵ}}\\ -R\cos q_{\mathrm{\ϵ}}{\stackrel{\·}{q}}_{\mathrm{\β}}\end{array}\right]\\ =L\left(q_{\mathrm{\ϵ}}\right)L\left(q_{\mathrm{\β}}\right)\left[\begin{array}{c}{V}_t{\mathrm{cos\ψ}}_{vt}{\mathrm{cos\θ}}_t\\ V_t{\mathrm{sin\θ}}_t\\ -V_t{\mathrm{cos\θ}}_t{\mathrm{sin\ψ}}_{vt}\end{array}\right]-L\left(q_{\mathrm{\ϵ}}\right)L\left(q_{\mathrm{\β}}\right)\left[\begin{array}{c}V{\mathrm{cos\θcos\ψ}}_v\\ V\sin \mathrm{\θ}\\ -V{\mathrm{cos\θsin\ψ}}_v\end{array}\right]\\ =\left[\begin{array}{c}{V}_t{\mathrm{cos\θ}}_t{\mathrm{cosq}}_{\mathrm{\ϵ}}\cos \left(q_{\mathrm{\β}}-{\mathrm{\ψ}}_{vt}\right)+V_t{\mathrm{sin\θ}}_t{\mathrm{sinq}}_{\mathrm{\ϵ}}\\ -V_t{\mathrm{cos\θ}}_t\cos \left(q_{\mathrm{\β}}-{\mathrm{\ψ}}_{vt}\right){\mathrm{sinq}}_{\mathrm{\ϵ}}+V_t{\mathrm{sin\θ}}_t{\mathrm{cosq}}_{\mathrm{\ϵ}}\\ V_t{\mathrm{cos\θ}}_t\sin \left(q_{\mathrm{\β}}-{\mathrm{\ψ}}_{vt}\right)\end{array}\right]-\left[\begin{array}{c}V{\mathrm{cos\θcosq}}_{\mathrm{\ϵ}}\cos \left(q_{\mathrm{\β}}-{\mathrm{\ψ}}_v\right)+V\sin \mathrm{\θ}\sin q_{\mathrm{\ϵ}}\\ -V\cos \mathrm{\θ}\cos \left(q_{\mathrm{\β}}-{\mathrm{\ψ}}_v\right){\mathrm{sinq}}_{\mathrm{\ϵ}}+V\sin \mathrm{\θ}\cos q_{\mathrm{\ϵ}}\\ V\cos \mathrm{\θ}\sin \left(q_{\mathrm{\β}}-{\mathrm{\ψ}}_v\right)\end{array}\right]\end{array}$

把公式

相对时间求导，得到

$\frac{d{\stackrel{\→}{V}}_r}{dt}=\frac{\mathrm{\δ}{\stackrel{\→}{V}}_r}{\mathrm{\δ}t}+\stackrel{\→}{\mathrm{\ω}}\×{\stackrel{\→}{V}}_r={\stackrel{\→}{a}}_t-\stackrel{\→}{a}$

投影到视线坐标系，可得

$\left[\begin{array}{c}\stackrel{\·}{R}\\ R{\stackrel{\·\·}{q}}_{\mathrm{\ϵ}}+\stackrel{\·}{R}{\stackrel{\·}{q}}_{\mathrm{\ϵ}}\\ -{\mathrm{Rcosq}}_{\mathrm{\ϵ}}{\stackrel{\·\·}{q}}_{\mathrm{\β}}-\stackrel{\·}{R}{\mathrm{cosq}}_{\mathrm{\ϵ}}{\stackrel{\·}{q}}_{\mathrm{\β}}+{\mathrm{Rsinq}}_{\mathrm{\ϵ}}{\stackrel{\·}{q}}_{\mathrm{\ϵ}}{\stackrel{\·}{q}}_{\mathrm{\β}}\end{array}\right]+\left[\begin{array}{ccc}0& -{\stackrel{\·}{q}}_{\mathrm{\ϵ}}& {\stackrel{\·}{q}}_{\mathrm{\β}}{\mathrm{cosq}}_{\mathrm{\ϵ}}\\ {\stackrel{\·}{q}}_{\mathrm{\ϵ}}& 0& -{\stackrel{\·}{q}}_{\mathrm{\β}}{\mathrm{sinq}}_{\mathrm{\ϵ}}\\ -{\stackrel{\·}{q}}_{\mathrm{\β}}{\mathrm{cosq}}_{\mathrm{\ϵ}}& {\stackrel{\·}{q}}_{\mathrm{\β}}{\mathrm{sinq}}_{\mathrm{\ϵ}}& 0\end{array}\right]\left[\begin{array}{c}\stackrel{\·}{R}\\ R{\stackrel{\·}{q}}_{\mathrm{\ϵ}}\\ -{\mathrm{Rcosq}}_{\mathrm{\ϵ}}{\stackrel{\·}{q}}_{\mathrm{\β}}\end{array}\right]={\stackrel{\→}{a}}_t-\stackrel{\→}{a}$

即

$\left\{\begin{array}{c}\stackrel{\·\·}{R}-R{\stackrel{\·}{q}}_{\mathrm{\ϵ}}^2-R{\cos }^2{q}_{\mathrm{\ϵ}}{\stackrel{\·}{q}}_{\mathrm{\β}}^2=a_{tr}-a_r\\ R{\stackrel{\·\·}{q}}_{\mathrm{\ϵ}}+2\stackrel{\·}{R}{\stackrel{\·}{q}}_{\mathrm{\ϵ}}+R\sin q_{\mathrm{\ϵ}}{\stackrel{\·}{q}}_{\mathrm{\β}}^2=a_{t\mathrm{\ϵ}}-a_{\mathrm{\ϵ}}\\ -R\cos q_{\mathrm{\ϵ}}{\stackrel{\·\·}{q}}_{\mathrm{\β}}-2\stackrel{\·}{R}\cos q_{\mathrm{\ϵ}}{\stackrel{\·}{q}}_{\mathrm{\β}}+2R\sin q_{\mathrm{\ϵ}}{\stackrel{\·}{q}}_{\mathrm{\ϵ}}{\stackrel{\·}{q}}_{\mathrm{\β}}=a_{t\mathrm{\β}}-a_{\mathrm{\β}}\end{array}\right.---\left(7\right)$

其中，a_r、a_ε和a_β是导弹加速度在视线坐标系三个轴上的分量，a_tr、a_tε和a_tβ是目标加速度在视线坐标系三个轴上的分量。

在末制导过程中，视线角速率为小量，因此，可以忽略式(7)中第二式中的二阶小量则俯仰视线变化动力学方程简化为

${\stackrel{\·\·}{q}}_{\mathrm{\ϵ}}=-\frac{2\stackrel{\·}{R}}{R}{\stackrel{\·}{q}}_{\mathrm{\ϵ}}+\frac{a_{t\mathrm{\ϵ}}-a_{\mathrm{\ϵ}}}{R}---\left(8\right)$

而忽略式(7)中第三式中的二阶小量而且以末制导惯性参考坐标系为基准，q_ε为小角度，则偏航视线变化动力学方程简化为

${\stackrel{\·\·}{q}}_{\mathrm{\β}}=-\frac{2\stackrel{\·}{R}}{R}{\stackrel{\·}{q}}_{\mathrm{\β}}+\frac{a_{\mathrm{\β}}-a_{t\mathrm{\β}}}{R}---\left(9\right)$

俯仰通道视线运动状态方程为

$\left\{\begin{array}{c}{\stackrel{\·}{q}}_{\mathrm{\ϵ}}={\stackrel{\·}{q}}_{\mathrm{\ϵ}}\\ {\stackrel{\·\·}{q}}_{\mathrm{\ϵ}}=-\frac{2\stackrel{\·}{R}}{R}{\stackrel{\·}{q}}_{\mathrm{\ϵ}}+\frac{a_{t\mathrm{\ϵ}}-a_{\mathrm{\ϵ}}}{R}\end{array}\right.---\left(10\right)$

令Δ_t为采样周期，式(10)离散化后就得到式(3),即

$\left\{\begin{array}{c}{q}_{\mathrm{\ϵ}}(k+1)=q_{\mathrm{\ϵ}}\left(k\right)+\mathrm{\Δ}t{\stackrel{\·}{q}}_{\mathrm{\ϵ}}\left(k\right)\\ {\stackrel{\·}{q}}_{\mathrm{\ϵ}}(k+1)=\[1-\frac{2\stackrel{\·}{R}\left(k\right)}{R\left(k\right)}\mathrm{\Δ}t\]{\stackrel{\·}{q}}_{\mathrm{\ϵ}}\left(k\right)+\frac{\mathrm{\Δ}t}{R\left(k\right)}\[a_{\mathrm{\ϵ}}\left(k\right)-a_{t\mathrm{\ϵ}}\left(k\right)\]\end{array}\right.$

又设

$X_{\mathrm{\ϵ}}=\left[\begin{array}{c}{q}_{\mathrm{\ϵ}}\\ {\stackrel{\·}{q}}_{\mathrm{\ϵ}}\end{array}\right],u_{\mathrm{\β}}=\left[\begin{array}{c}0\\ a_{\mathrm{\ϵ}}-a_{t\mathrm{\ϵ}}\end{array}\right],{\mathrm{\Φ}}_{\mathrm{\ϵ}}=\left[\begin{array}{cc}1& \mathrm{\Δ}t\\ 0& 1-\mathrm{\Δ}t2\stackrel{\·}{R}/R\end{array}\right],B_{\mathrm{\ϵ}}=\left[\begin{array}{c}0\\ \mathrm{\Δ}t/R\end{array}\right]$

则式(3)可以写作

X_ε(k+1)＝Φ_ε(k)X_ε(k)+Β_ε(k)u_ε(k)

考虑模型误差等原因，在上式中加入了一个零均值随机过程w_ε(k)，就得到

X_ε(k+1)＝Φ_ε(k)X_ε(k)+Β_ε(k)u_ε(k)+w_ε(k)

俯仰通道视线角测量方程可以写作式(4)，即

$Z_{\mathrm{\ϵ}}\left(k\right)=q_{\mathrm{\ϵ}}\left(k\right)+{\mathrm{\ν}}_{\mathrm{\ϵ}}\left(k\right)=H_{\mathrm{\ϵ}}{\left[\begin{array}{cc}{q}_{\mathrm{\ϵ}}\left(k\right)& {\stackrel{\·}{q}}_{\mathrm{\ϵ}}\left(k\right)\end{array}\right]}^T+{\mathrm{\ν}}_{\mathrm{\ϵ}}\left(k\right)$

其中，H_ε＝[10]，ν_ε(k)为视线俯仰角的测量噪声，假设其均值为零， $E\[w_{\mathrm{\ϵ}}\left(k\right)w_{\mathrm{\ϵ}}^T\left(k\right)\]=Q_{\mathrm{\ϵ}0}\left(k\right),E\[{\mathrm{\ν}}_{\mathrm{\ϵ}}\left(k\right){\mathrm{\ν}}_{\mathrm{\ϵ}}^T\left(k\right)\]=R_{\mathrm{\ϵ}0}\left(k\right),$

同理，偏航通道视线运动状态方程为

$\left\{\begin{array}{c}{\stackrel{\·}{q}}_{\mathrm{\β}}={\stackrel{\·}{q}}_{\mathrm{\β}}\\ {\stackrel{\·\·}{q}}_{\mathrm{\β}}=-\frac{2\stackrel{\·}{R}}{R}{\stackrel{\·}{q}}_{\mathrm{\β}}+\frac{a_{\mathrm{\β}}-a_{t\mathrm{\β}}}{R}\end{array}\right.---\left(11\right)$

式(11)离散化后得到

$\left\{\begin{array}{c}{q}_{\mathrm{\β}}(k+1)=q_{\mathrm{\β}}\left(k\right)+\mathrm{\Δ}t{\stackrel{\·}{q}}_{\mathrm{\β}}\left(k\right)\\ {\stackrel{\·}{q}}_{\mathrm{\β}}(k+1)=\[1-\frac{2\stackrel{\·}{R}\left(k\right)}{R\left(k\right)}\mathrm{\Δ}t\]{\stackrel{\·}{q}}_{\mathrm{\β}}\left(k\right)+\frac{\mathrm{\Δ}t}{R\left(k\right)}\[a_{\mathrm{\β}}\left(k\right)-a_{t\mathrm{\β}}\left(k\right)\]\end{array}\right.---\left(12\right)$

由式(12)，偏航通道视线运动离散化状态方程写作式(5)，即

X_β(k+1)＝Φ_β(k)X_β(k)+Β_β(k)u_β(k)+w_β(k)

其中，

$X_{\mathrm{\β}}=\left[\begin{array}{c}{q}_{\mathrm{\β}}\\ {\stackrel{\·}{q}}_{\mathrm{\β}}\end{array}\right],u_{\mathrm{\β}}=\left[\begin{array}{c}0\\ a_{\mathrm{\β}}-a_{t\mathrm{\β}}\end{array}\right],{\mathrm{\Φ}}_{\mathrm{\β}}=\left[\begin{array}{cc}1& \mathrm{\Δ}t\\ 0& 1-\mathrm{\Δ}t2\stackrel{\·}{R}/R\end{array}\right],B_{\mathrm{\β}}=\left[\begin{array}{c}0\\ \mathrm{\Δ}t/R\end{array}\right]$

w_β(k)是一个零均值随机过程，

偏航视线角测量方程写作式(6)，即

$Z_{\mathrm{\β}}\left(k\right)=q_{\mathrm{\β}}\left(k\right)+{\mathrm{\ν}}_{\mathrm{\β}}\left(k\right)=H_{\mathrm{\β}}{\left[\begin{array}{cc}{q}_{\mathrm{\β}}\left(k\right)& {\stackrel{\·}{q}}_{\mathrm{\β}}\left(k\right)\end{array}\right]}^T+{\mathrm{\ν}}_{\mathrm{\β}}\left(k\right)$

其中，H_β＝[10]，ν_β(k)为视线俯仰角的测量噪声，假设其均值为零， $E\[{\mathrm{\ν}}_{\mathrm{\β}}\left(k\right){\mathrm{\ν}}_{\mathrm{\β}}^T\left(k\right)\]=R_{\mathrm{\β}0}\left(k\right).$

(二)快收敛Kalman滤波算法

在末制导Kalman滤波算法设计中，设和分别代表X_ε(k)和X_β(k)的一步预报估计，定义新息和根据新息自适应地调节滤波算法中的状态噪声协方差阵Q_ε(k)和Q_β(k)，并定义状态一步预报估计协方差阵分别为

$P_{\mathrm{\ϵ}}(k+1/k)=E\{\[X_{\mathrm{\ϵ}}(k+1)-{\stackrel{\‾}{X}}_{\mathrm{\ϵ}}(k+1)\]{\[X_{\mathrm{\ϵ}}(k+1)-{\stackrel{\‾}{X}}_{\mathrm{\ϵ}}(k+1)\]}^T\}$

$P_{\mathrm{\β}}(k+1/k)=E\{\[X_{\mathrm{\β}}(k+1)-{\stackrel{\‾}{X}}_{\mathrm{\β}}(k+1)\]{\[X_{\mathrm{\β}}(k+1)-{\stackrel{\‾}{X}}_{\mathrm{\β}}(k+1)\]}^T\}$

根据它们在线地调整滤波算法中的测量噪声协方差阵R_ε(k)和R_β(k)。通过自适应地调节滤波算法中的这两个矩阵，可以扩大滤波器稳定的吸引区域，令其快速收敛，即构成快速收敛末制导Kalman滤波器。

俯仰通道快收敛Kalman滤波器表达式为

$\left\{\begin{array}{c}{\stackrel{\‾}{X}}_{\mathrm{\ϵ}}\left(k+1\right)={\mathrm{\Φ}}_{\mathrm{\ϵ}}\left(k\right){\hat{X}}_{\mathrm{\ϵ}}\left(k\right)+B_{\mathrm{\ϵ}}\left(k\right)u_{\mathrm{\ϵ}}\left(k\right)\\ Q_{\mathrm{\ϵ}}\left(k\right)={\mathrm{\γe}}_{\mathrm{\ϵ}}^T\left(k\right)e_{\mathrm{\ϵ}}\left(k\right)I_n+Q_{\mathrm{\ϵ}0}\left(k\right)\\ P_{\mathrm{\ϵ}}\left(k+1/k\right)={\mathrm{\Φ}}_{\mathrm{\ϵ}}\left(k\right)P_{\mathrm{\ϵ}}\left(k\right){\mathrm{\Φ}}_{\mathrm{\ϵ}}^T\left(k\right)+Q_{\mathrm{\ϵ}}\left(k\right)\\ R_{\mathrm{\ϵ}}\left(k+1\right)={\mathrm{\λH}}_{\mathrm{\ϵ}}{P}_{\mathrm{\ϵ}}\left(k+1/k\right)H_{\mathrm{\ϵ}}^T+R_{\mathrm{\ϵ}0}\left(k+1\right)\\ K_{\mathrm{\ϵ}}\left(k+1\right)=P_{\mathrm{\ϵ}}\left(k+1/k\right)H_{\mathrm{\ϵ}}^T{\[H_{\mathrm{\ϵ}}{P}_{\mathrm{\ϵ}}\left(k+1/k\right)H_{\mathrm{\ϵ}}^T+R_{\mathrm{\ϵ}}\left(k+1\right)\]}^{-1}\\ e_{\mathrm{\ϵ}}\left(k+1\right)=Z_{\mathrm{\ϵ}}\left(k+1\right)-H_{\mathrm{\ϵ}}{\stackrel{\‾}{X}}_{\mathrm{\ϵ}}\left(k+1\right)\\ {\hat{X}}_{\mathrm{\ϵ}}\left(k+1\right)={\stackrel{\‾}{X}}_{\mathrm{\ϵ}}\left(k+1\right)+K_{\mathrm{\ϵ}}\left(k+1\right)e_{\mathrm{\ϵ}}\left(k+1\right)\\ P_{\mathrm{\ϵ}}\left(k+1\right)=\[I-K_{\mathrm{\ϵ}}\left(k+1\right)H_{\mathrm{\ϵ}}\]P_{\mathrm{\ϵ}}\left(k+1/k\right)\end{array}\right.$

而偏航通道快收敛Kalman滤波器表达式为

$\left\{\begin{array}{c}{\stackrel{\‾}{X}}_{\mathrm{\β}}\left(k+1\right)={\mathrm{\Φ}}_{\mathrm{\β}}\left(k\right){\hat{X}}_{\mathrm{\β}}\left(k\right)+B_{\mathrm{\β}}\left(k\right)u_{\mathrm{\β}}\left(k\right)\\ Q_{\mathrm{\β}}\left(k\right)={\mathrm{\γe}}_{\mathrm{\β}}^T\left(k\right)e_{\mathrm{\β}}\left(k\right)I_n+Q_{\mathrm{\β}0}\left(k\right)\\ P_{\mathrm{\β}}\left(k+1/k\right)={\mathrm{\Φ}}_{\mathrm{\β}}\left(k\right)P_{\mathrm{\β}}\left(k\right){\mathrm{\Φ}}_{\mathrm{\β}}^T\left(k\right)+Q_{\mathrm{\β}}\left(k\right)\\ R_{\mathrm{\β}}\left(k+1\right)={\mathrm{\λH}}_{\mathrm{\β}}{P}_{\mathrm{\β}}\left(k+1/k\right)H_{\mathrm{\β}}^T+R_{\mathrm{\β}0}\left(k+1\right)\\ K_{\mathrm{\β}}\left(k+1\right)=P_{\mathrm{\β}}\left(k+1/k\right)H_{\mathrm{\β}}^T{\[H_{\mathrm{\β}}{P}_{\mathrm{\β}}\left(k+1/k\right)H_{\mathrm{\β}}^T+R_{\mathrm{\β}}\left(k+1\right)\]}^{-1}\\ e_{\mathrm{\β}}\left(k+1\right)=Z_{\mathrm{\β}}\left(k+1\right)-H_{\mathrm{\β}}{\stackrel{\‾}{X}}_{\mathrm{\β}}\left(k+1\right)\\ {\hat{X}}_{\mathrm{\β}}\left(k+1\right)={\stackrel{\‾}{X}}_{\mathrm{\β}}\left(k+1\right)+K_{\mathrm{\β}}\left(k+1\right)e_{\mathrm{\β}}\left(k+1\right)\\ P_{\mathrm{\β}}\left(k+1\right)=\[I-K_{\mathrm{\β}}\left(k+1\right)H_{\mathrm{\β}}\]P_{\mathrm{\β}}\left(k+1/k\right)\end{array}\right.$

在上述滤波算法中，λ是一个正的标量。若γ选择一个充分大的正值，可以满足在最坏的初始条件下也具有较好的滤波收敛特性。但γ选取得过大会对滤波器的稳态特性有一定影响，在应用中应该采取一种折中的选择。另外，为了确保稳态阶段，R_k更接近于R_k0，λ不易选择过大。算法中的P_ε(k+1)和P_β(k+1)代表状态的滤波估计协方差阵，即

$P_{\mathrm{\ϵ}}(k+1)=E\{\[X_{\mathrm{\ϵ}}(k+1)-{\hat{X}}_{\mathrm{\ϵ}}(k+1)\]{\[X_{\mathrm{\ϵ}}(k+1)-{\hat{X}}_{\mathrm{\ϵ}}(k+1)\]}^T\},$

$P_{\mathrm{\β}}(k+1)=E\{\[X_{\mathrm{\β}}(k+1)-{\hat{X}}_{\mathrm{\β}}(k+1)\]{\[X_{\mathrm{\β}}(k+1)-{\hat{X}}_{\mathrm{\β}}(k+1)\]}^T\}.$

实现上述滤波器需要知道相对距离和相对速度信息。记拦截器在发射点惯性坐标系的位置为(x、y、z)，其速度在该坐标系中的投影为这些信息由弹上导航系统提供；目标在发射点惯性坐标系的位置为(x_t、y_t、z_t)，其速度在该坐标系中的投影为这些信息由轨道预报系统提供。记Δx＝x_t-x，Δy＝y_t-y，Δz＝z_t-z， $\mathrm{\Δ}\stackrel{\·}{x}={\stackrel{\·}{x}}_t-\stackrel{\·}{x},\mathrm{\Δ}\stackrel{\·}{y}={\stackrel{\·}{y}}_t-\stackrel{\·}{y},\mathrm{\Δ}\stackrel{\·}{z}={\stackrel{\·}{z}}_t-\stackrel{\·}{z},$ 则目标—导弹相对距离为 $R=\sqrt{{\mathrm{\Δx}}^2+{\mathrm{\Δy}}^2+{\mathrm{\Δz}}^2},$ 相对速度为在末制导过程中，始终有R>0，实现上述滤波器还需要知道导弹法向加速度a_ε和a_β，以及目标法向加速度a_tε和a_tβ。导弹上加速度计输出的加速度是沿着弹体坐标系投影的，把它投影到视线坐标系即可求得导弹法向加速度a_ε和a_β。而根据轨道预报数据可以用公式计算出目标加速度在地心惯性坐标系中的三个分量，然后投影到视线坐标系求得a_tε和a_tβ。

(三)视线俯仰角和视线偏航角的提取算法

导引头输出的只是目标—导弹视线相对导引头测量坐标系的视线角Δq_ε和Δq_β。导引头测量坐标系的圆点在导引头回转中心o′，在不考虑导引头安装误差及导引头安装位置挠性变形的情况下，其o′x₆轴、o′y₆轴和o′z₆轴分别平行于弹体坐标系的ox₁轴、oy₁轴和oz₁轴。

设导引头测量坐标系与弹体坐标系重合，则目标—导弹视线的单位向量在导引头测量坐标系中的投影为

$\left\{\begin{array}{c}{c}_{1x}={\mathrm{cos\Δq}}_{\mathrm{\ϵ}}{\mathrm{cos\Δq}}_{\mathrm{\β}}\\ c_{1y}={\mathrm{sin\Δq}}_{\mathrm{\ϵ}}\\ c_{1z}=-{\mathrm{cos\Δq}}_{\mathrm{\ϵ}}{\mathrm{sin\Δq}}_{\mathrm{\β}}\end{array}\right.---\left(13\right)$

而它在发射点惯性坐标系中的投影为

$\left\{\begin{array}{c}{c}_{0x}=\cos q_{\mathrm{\ϵ}}\cos q_{\mathrm{\β}}\\ c_{0y}=\sin q_{\mathrm{\ϵ}}\\ c_{0z}=-\cos q_{\mathrm{\ϵ}}\sin q_{\mathrm{\β}}\end{array}\right.---\left(14\right)$

则

$\left[\begin{array}{c}{c}_{0x}\\ c_{0y}\\ c_{0z}\end{array}\right]=C_{F\→1}^T\left[\begin{array}{c}{c}_{1x}\\ c_{1y}\\ c_{1z}\end{array}\right]---\left(15\right)$

其中，C_F→1为由发射点惯性坐标系到弹体坐标系的转换矩阵，其定义为

其中，ψ和γ分别为弹体的俯仰角，偏航角和滚动角，由惯导系统测算得到。

根据式(14),可以计算出视线俯仰角和视线偏航角，即

q_ε＝sin^-1c_0y(17)

$q_{\mathrm{\β}}=-{\tan }^{-1}\frac{c_{0z}}{c_{0x}}---\left(18\right)$

滤波器初始估计中的视线俯仰角和视线偏航角的初值和取为根据导引头第1拍测量值计算出来的视线俯仰角和视线偏航角，视线俯仰角速率和视线偏航角速率的初值和利用几何方法计算出来，即

$\begin{array}{c}{\widehat{\stackrel{\·}{q}}}_{\mathrm{\ϵ}}\left(0\right)\frac{({\mathrm{\Δx}}^2+{\mathrm{\Δz}}^2)\mathrm{\Δ}\stackrel{\·}{y}-\mathrm{\Δ}y(\mathrm{\Δ}x\mathrm{\Δ}\stackrel{\·}{x}+\mathrm{\Δ}z\mathrm{\Δ}\stackrel{\·}{z})}{({\mathrm{\Δx}}^2+{\mathrm{\Δy}}^2+{\mathrm{\Δz}}^2)\sqrt{{\mathrm{\Δx}}^2+{\mathrm{\Δz}}^2}}\\ {\widehat{\stackrel{\·}{q}}}_{\mathrm{\β}}\left(0\right)=\frac{\mathrm{\Δ}z\mathrm{\Δ}\stackrel{\·}{x}-\mathrm{\Δ}x\mathrm{\Δ}\stackrel{\·}{z}}{{\mathrm{\Δx}}^2+{\mathrm{\Δz}}^2}\end{array}$

记Δx、Δy、Δz为末制导初始时刻目标—导弹相对位置在末制导惯性参考坐标系三个轴上的分量，为末制导初始时刻目标—导弹相对速度在末制导惯性参考坐标系三个轴上的分量。导弹在末制导惯性参考坐标系的位置为(x、y、z)，其速度在该坐标系中的投影为这些信息由弹上导航系统提供；目标在发射点惯性坐标系的位置为(x_t、y_t、z_t)，其速度在该坐标系中的投影为这些信息由地面上的目标测量系统提供，则Δx＝x_t-x，Δy＝y_t-y，Δz＝z_t-z，

(四)下面通过数值仿真结果说明本实施方式的效果：

设仿真初始时刻，目标的高度为H_t＝307.49km，飞行速度为4242m/s,导弹的高度为254.12km，飞行速度3000m/s，二者相对距离112.196km，相对速度为-6680.7m/s。

导弹的初始姿态角速率取为零，初始姿态大致指向初始视线方向，滚转角基本等于零。

仿真中，取惯导系统数据更新周期为5ms，导引头数据更新周期为15ms，轨控周期15ms，姿控周期5ms。

导引头测量数据Δq_ε和Δq_β中包含0.17mrad量化噪声。导引头误差角中分别包含安装误差角0.2°和-0.2°(常值)，同时又分别包含轨控发动机开关机所引起的导引头安装位置挠性变形产生的误差。

惯导系统初始姿态误差用标准差为0.1°的高斯白噪声描述，初始速度误差用标准差为1m/s的高斯白噪声描述，在发射点惯性坐标系x轴上的初始位置误差在(2km～5km)范围内均匀分布，或在(-2km～-5km)范围内均匀分布；在发射点惯性坐标系y轴和z轴上的初始位置误差在±1km内均匀分布。设目标轨道位置预报数据在地心惯性坐标系x_I轴上的位置误差用均值为1km，标准差为1km的高斯白噪声表述，y_I轴和z_I轴方向上的位置误差分别用均值为1km，标准差为1km的高斯白噪声表述；三个方向上的速度误差均取上述相应数据的1/(1000s)。轨道预报数据每1s输出一次，而滤波器的采样周期为15ms，这样在每两个预报数据之间用插值方法获得更多数据。

在快收敛Kalman滤波器设计中，滤波器初始估计中的视线俯仰角和视线偏航角的初值和取为根据导引头第1拍测量值计算出来的视线俯仰角和视线偏航角，视线俯仰角速率和视线偏航角速率的初值和利用几何方法计算出来。

动态噪声方差阵中的Q_ε0(k)和Q_β0(k)分别取为

$Q_{\mathrm{\ϵ}0}\left(k\right)=\left[\begin{array}{cc}{10}^{-6}& 0\\ 0& {10}^{-6}\end{array}\right],Q_{\mathrm{\β}0}\left(k\right)=\left[\begin{array}{cc}{10}^{-6}& 0\\ 0& {10}^{-6}\end{array}\right]$

测量噪声方差阵中R_ε0(k)和R_β0(k)分别取为

$R_{\mathrm{\ϵ}0}\left(k\right)=\left[\begin{array}{cc}{10}^{-4}& 0\\ 0& {10}^{-4}\end{array}\right],R_{\mathrm{\β}0}\left(k\right)=\left[\begin{array}{cc}{10}^{-4}& 0\\ 0& {10}^{-4}\end{array}\right]$

状态估计方差阵的初值取为

$P_{\mathrm{\ϵ}}\left(0\right)=\left[\begin{array}{cc}{10}^{-2}& 0\\ 0& {10}^{-2}\end{array}\right],P_{\mathrm{\β}}\left(0\right)=\left[\begin{array}{cc}{10}^{-2}& 0\\ 0& {10}^{-2}\end{array}\right]$

在动态噪声方差阵计算公式

$Q_{\mathrm{\ϵ}}\left(k\right)={\mathrm{\γe}}_{\mathrm{\ϵ}}^T\left(k\right)e_{\mathrm{\ϵ}}\left(k\right)I_n+Q_{\mathrm{\ϵ}0}\left(k\right)$

$Q_{\mathrm{\β}}\left(k\right)={\mathrm{\γe}}_{\mathrm{\β}}^T\left(k\right)e_{\mathrm{\β}}\left(k\right)I_n+Q_{\mathrm{\β}0}\left(k\right)$

中，参数γ是一个需要选取的重要参数，γ选取得大有利于扩大滤波器的稳定区域，但可能影响稳态的滤波效果。折中考虑后选取γ＝10²，这是一个较大的参数，它可以保证滤波器的稳定区域足够大。

在测量噪声方差阵计算公式

$R_{\mathrm{\ϵ}}(k+1)={\mathrm{\λH}}_{\mathrm{\ϵ}}{P}_{\mathrm{\ϵ}}(k+1/k)H_{\mathrm{\ϵ}}^T+R_{\mathrm{\ϵ}0}(k+1),R_{\mathrm{\β}}(k+1)={\mathrm{\λH}}_{\mathrm{\β}}{P}_{\mathrm{\β}}(k+1/k)H_{\mathrm{\β}}^T+R_{\mathrm{\β}0}(k+1)$

中，选取λ＝3。

在这种情况下，快收敛Kalman滤波器估计的视线俯仰角速率跟踪其真实值的全过程、初段动态过程、稳态跟踪过程，以及末段跟踪过程分别如图4-7所示，而快收敛Kalman滤波器估计的视线偏航角速率跟踪其真实值的全过程、初段动态过程、稳态跟踪过程，以及末段跟踪过程如图8-11所示。从这些仿真结果图可以看出，滤波器在1s之后已经保证跟踪误差小于0.005°/s。随后的稳态跟踪过程中，由于γ减小后，滤波器的稳态增益上升，消除了静差。在末段跟踪阶段也仅在最后的0.1s内没有跟踪上真值，对制导精度影响不大。

为了进行对比，我们在图12-15中给出了标准Kalman滤波器估计的视线俯仰角速率跟踪其真实值的全过程、初段动态过程、稳态跟踪过程，以及末段跟踪过程，在图16-19中给出了标准Kalman滤波器估计的视线偏航角速率跟踪其真实值的全过程、初段动态过程、稳态跟踪过程，以及末段跟踪过程。从这些图中可以发现，标准Kalman滤波器稳态跟踪性能好，但初段动态跟踪阶段进入稳态的时间稍长，大约为2.5s。其末段跟踪性能尚可，本次仿真脱靶量为0.056m，轨控发动机开机时间5.929s。

Claims (3)

1.估计弹目视线角速率的快收敛Kalman滤波器的设计方法，其特征在于：所述方法的实现过程为：

步骤一、滤波器初始估计值的设定：

定义末制导惯性参考坐标系o₀x₀y₀z₀、发射点惯性坐标系o_Fx_Fy_Fz_F、地心惯性坐标系o_Ix_Iy_Iz_I，以三者之间的关系：

设末制导惯性参考坐标系o₀x₀y₀z₀在末制导过程中相对于惯性空间固化不动，末制导惯性参考坐标系的原点为末制导初始时刻导弹的质心o₀，其o₀x₀轴沿着末制导初始时刻视线方向，指向目标为正，其o₀y₀轴位于包含o₀x₀轴的铅垂面内，指向上为正，o₀z₀轴由右手定则确定；发射点惯性坐标系的原点为发射点o_F，o_Fy_F轴铅锤向上，o_Fx_F轴和o_Fz_F轴位于水平面内，其指向按需要选取，设o_Fx_Fy_F为射击平面；地心惯性坐标系的原点o_I位于地心上，o_Ix_I轴位于赤道平面内，指向某一恒星，o_Iz_I轴指向北极方向，o_Iy_I按右手定则确定；地心惯性坐标系与发射点惯性坐标系之间的转换取决于λ₀和α_F三个角度,q_ε0和q_β0分别为末制导初始时刻视线相对于发射点惯性坐标系所成的视线仰角和视线偏角；

滤波器初始估计中的视线俯仰角和视线偏航角的初值和取为根据导引头第1拍测量值计算出来的视线俯仰角和视线偏航角，视线俯仰角速率和视线偏航角速率的初值和利用几何方法计算出来，即

其中，和括号中的0表示0时刻；

△x、△y、△z为末制导初始时刻目标—导弹相对位置在末制导惯性参考坐标系三个轴上的分量，为末制导初始时刻目标—导弹相对速度在末制导惯性参考坐标系三个轴上的分量，的具体计算过程如下：

设导弹在末制导惯性参考坐标系的位置为(x、y、z)，其速度在该坐标系中的投影为 上述信息由弹上导航系统提供；目标在末制导惯性参考坐标系的位置为(x_t、y_t、z_t)，其速度在该坐标系中的投影为上述信息由地面上的目标测量系统提供，则△x＝x_t-x，△y＝y_t-y，△z＝z_t-z，

步骤二、测算当前时刻目标与导弹之间的相对距离和相对速度，具体过程如下：

记当前时刻相对位置在末制导惯性参考坐标系中的分量为△x＝x_t-x，△y＝y_t-y，△z＝z_t-z，当前时刻相对速度在末制导惯性参考坐标系中的分量为 则目标—导弹相对距离为相对速度为在末制导过程中，始终有R>0，

步骤三、测算导弹加速度在视线坐标系o′x₄y₄z₄的o′y₄和o′z₄轴方向上的分量a_ε和a_β，以及目标加速度在视线坐标系的o′y₄和o′z₄轴方向上的分量a_tε和a_tβ；导弹上加速度计输出的加速度是沿着弹体坐标系ox₁y₁z₁投影的，投影到视线坐标系即可求得导弹法向加速度a_ε和a_β；

导弹弹体坐标系ox₁y₁z₁的原点o位于导弹质心上；ox₁轴与弹体纵轴重合，指向头部为正，oy₁轴在弹体纵向对称平面内，垂直ox₁轴，指向上方为正，oz₁轴方向按右手定则确定；视线坐标系o′x₄y₄z₄原点o′位于导引头回转中心,o′x₄轴与目标—导弹视线一致,由导引头回转中心指向目标为正，o′y₄轴位于包含o′x₄轴的铅锤面内，与o′x₄轴垂直，指向上方为正，o′z₄轴按右手定则确定；由末制导惯性坐标系转换到视线坐标系定义视线俯仰角q_ε和视线偏航角q_β；根据目标轨道测量数据可计算出目标加速度在地心惯性坐标系中的三个分量，然后投影到视线坐标系求得a_tε和a_tβ；

步骤四、由导弹的俯仰通道视线运动状态方程和俯仰通道视线角测量方程构造俯仰通道快收敛Kalman滤波器，从而求出目标与导弹之间的视线俯仰角速率：

俯仰通道视线运动离散化状态方程为：

其中，k代表第k个采样时刻，△t为采样周期；

俯仰通道视线角测量方程为：

其中，H_ε＝[10]，ν_ε(k)为视线俯仰角的测量噪声，假设其均值为零；

设

则俯仰通道快收敛Kalman滤波器为：

其中，和分别代表X_ε的滤波估计和预报估计，γ选择一个充分大的正值；

λ是一个正的标量；

步骤五、根据导弹的偏航通道视线运动状态方程和偏航通道视线角测量方程可设计出估计目标与导弹之间偏航通道视线角速率的快收敛Kalman滤波器：

偏航通道视线运动离散化状态方程为

X_β(k+1)＝Φ_β(k)X_β(k)+Β_β(k)u_β(k)+w_β(k)(5)

其中，

w_β(k)是一个零均值随机过程，

偏航视线角测量方程为

其中，H_β＝[10]，ν_β(k)为视线俯仰角的测量噪声，假设其均值为零，

由导弹的偏航通道视线运动状态方程和偏航通道视线角测量方程构造偏航通道快收敛Kalman滤波器，从而求出目标与导弹之间的视线偏航角速率；

偏航通道Kalman滤波器为：

其中，和分别代表X_β的滤波估计和预报估计，γ选择一个充分大的正值；

λ是一个正的标量。

2.根据权利要求1所述的估计弹目视线角速率的快收敛Kalman滤波器的设计方法，其特征在于：偏航通道快收敛Kalman滤波器与俯仰通道快收敛Kalman滤波器中的λ和γ的取值规律相同。

3.根据权利要求1或2所述的估计弹目视线角速率的快收敛Kalman滤波器的设计方法，其特征在于：在步骤四中，γ选取规则如下：

若γ选择一个充分大的正值，可满足在最坏的初始条件下也具有较好的滤波收敛特性；若γ选取得过大会对滤波器的稳态特性有一定影响，在应用中应该采取一种折中的选择，通常选择不大于300；

为了确保稳态阶段，R_k接近于R_k0，λ不大于10。