CN103744057B - 基于输出相关自适应卡尔曼滤波的弹道轨迹形成方法 - Google Patents

Abstract

本发明公开了一种基于输出相关自适应卡尔曼滤波的弹道轨迹形成方法，在弹道轨迹形成过程中，利用弹丸的飞行参数估计输出相关函数矩阵，由相关函数矩阵计算出卡尔曼滤波增益矩阵的最优稳态解，使得稳态增益与实际观测值相适应；利用递推的方法根据观测值不断地调整相关函数矩阵，实现对弹道数据的自适应滤波估计和修正，从而得到准确的弹道轨迹。本发明有效抑制了由于弹道属性模型与实际物理模型不完全相符、系统噪声和观测噪声参数估计不精确等因素造成的滤波发散问题，提高了滤波的精度和弹道轨迹的准确度。

Description

基于输出相关自适应卡尔曼滤波的弹道轨迹形成方法

技术领域

本发明涉及一种弹道轨迹形成方法，尤其涉及一种基于输出相关自适应卡尔曼滤波的弹道轨迹形成方法。

背景技术

在获得弹丸的飞行参数后，炮位雷达利用卡尔曼滤波对数据进行滤波从而形成弹道轨迹。卡尔曼滤波器在已知系统随机噪声精确的统计特性和系统的精确数学模型的前提下可以获得最小均方误差准则下的最优值。在炮位雷达的弹道轨迹形成过程中，由于对弹丸飞行的环境认识不精确，所建弹道模型与实际物理过程有差异；干扰及噪声的统计特性也是未知的，这些因素会影响卡尔曼滤波算法的精度，导致卡尔曼滤器无法获得最优估计结果，甚至引起滤波结果发散，炮位雷达无法形成弹道轨迹。

发明内容

本发明所要解决的技术问题是提供一种基于输出相关自适应卡尔曼滤波的弹道轨迹形成发散抑制方法，利用递推的方法根据观测值不断地调整相关函数矩阵，实现对弹道数据的自适应滤波估计和修正。

本发明为解决上述技术问题采用以下技术方案：

一种基于输出相关自适应卡尔曼滤波的弹道轨迹形成方法，具体步骤如下：

步骤1，根据弹道模型建立基于状态变量的弹道方程及观测方程，将状态变量与标称状态作差得到状态偏差，从而建立基于状态偏差的近似线性方程；

在地面坐标系中建立弹道方程：

$\frac{\mathrm{dX}}{\mathrm{dt}}=\left[\begin{array}{c}\frac{\mathrm{dx}}{\mathrm{dt}}\\ \frac{\mathrm{dy}}{\mathrm{dt}}\\ \frac{\mathrm{dz}}{\mathrm{dt}}\\ \frac{{\mathrm{dv}}_x}{\mathrm{dt}}\\ \frac{{\mathrm{dv}}_y}{\mathrm{dt}}\\ \frac{{\mathrm{dv}}_z}{\mathrm{dt}}\end{array}\right]=\left[\begin{array}{c}{v}_x\\ v_y\\ v_z\\ -K_D\frac{{\mathrm{SC}}_x\left(\mathrm{Ma}\right)}{2m}{\mathrm{\ρv}}_r(v_x-w_x)+K_D\frac{{\mathrm{SC}}_x\left(\mathrm{Ma}\right)}{2m}{\mathrm{\ρv}}_r{w}_x^{\′}\\ {-K}_D\frac{{\mathrm{SC}}_x\left(\mathrm{Ma}\right)}{2m}{\mathrm{\ρv}}_r{v}_y-g\\ {-K}_D\frac{{\mathrm{SC}}_x\left(\mathrm{Ma}\right)}{2m}{\mathrm{\ρv}}_r(v_z-w_z)+K_L{B}_Z+K_D\frac{{\mathrm{SC}}_x\left(\mathrm{Ma}\right)}{2m}{\mathrm{\ρv}}_r{w}_z^{\′}\end{array}\right]$

式中：X为t时刻的状态变量；表示对时间t求导数；x，y，z分别为弹丸的距离、高度、侧偏；v_x,v_y,v_z分别为弹丸速度的三轴分量；v_r为含风速在内的弹丸实际速度；w_x,w_z分别为纵风和横风风速，在标准条件下均取为0；w'_x,w'_z分别为纵风和横风的随机量；S为弹丸最大截断面积，d为弹径；m为弹体质量；Ma为马赫数，C_s为声速，其中k为比热比，对于空气k＝1.404；R_d为干空气气体常数；τ为虚温，是把湿空气折合成干空气时对气温的修正；C_x(Ma)为空气阻力系数，它是马赫数Ma的函数；ρ为空气密度，其中，p为气体压强；g为海拔y处的重力加速度，其中g₀为海平面重力加速度，r₀为地球半径；K_D为阻力符合系数；B_Z为侧向升力加速度；K_L为由动力平衡角引起的侧向加速度时弥补误差的修正系数，又称静力矩符合系数；

令状态变量X＝[x,y,z,v_x,v_y,v_z,c,ac]^T＝[x₁,x₂,x₃,x₄,x₅,x₆,x₇,x₈]^T，式中c为弹道系数，ac为修正的弹道系数，x₁～x₈为状态变量X的8个状态，分别与x,y,z,v_x,v_y,v_z,c,ac对应；

令x_4r＝x₄-w_x，表示弹丸在x方向受风速影响后的实际速度；x_6r＝x₆-w_z，表示弹丸在z方向的实际飞行速度，则

取作为中间变量；同时将声速带入，马赫数表示为 $\mathrm{Ma}=\frac{v_r}{C_s}=\frac{v_r}{20.0468\sqrt{\mathrm{\τ}}},$ 将弹道方程改写为：

$\frac{\mathrm{dX}}{\mathrm{dt}}=f\left(X\right)+\mathrm{\ΓW}=\left[\begin{array}{c}{x}_4\\ x_5\\ x_6\\ {-x}_7{\mathrm{Nx}}_{4r}\\ {-x}_7{\mathrm{Nx}}_5-g\\ {-x}_7{\mathrm{Nx}}_{6r}+x_8{B}_Z\\ 0\\ 0\end{array}\right]+\left[\begin{array}{c}{0}_{3\×3}\\ I_{3\×3}\\ 0_{2\×3}\end{array}\right]x_{7}N\left[\begin{array}{c}{w}_x^{\′}\\ 0\\ w_z^{\′}\end{array}\right]$

式中，f(X)为状态变量X的函数；Γ为噪声驱动阵，Γ＝[0_3×3I_3×30_2×3]^Tx₇N；W为系统噪声，W＝[w'_x0w'_z]^T；0_3×3表示3×3的零矩阵；I_3×3表示3×3的单位阵；0_2×3表示2×3的零矩阵；

建立观测方程为：

Z＝h(X)+V

式中， $h\left(X\right)={\left[\sqrt{x^2+y^2+z^2}\arctan \frac{z}{x}\arctan \frac{y}{\sqrt{x^2+z^2}}\right]}^T,$ V为雷达量测噪声；

在采样区间[t_k,t_k+1]上将f(X(t))近似展开并将弹道方程离散化，得到：

X_k+1＝G(X_k)+Γ_kq_k

Z_k+1＝h(X_k+1)+V_k+1

式中，X_k为k时刻的状态变量；X_k+1为k+1时刻的状态变量； $G\left(X_k\right)=X_k+f\left(X_k\right)T+A\left(X_k\right)f\left(X_k\right)\frac{T^2}{2},$ A(X)是f(X)关于X的偏导，即 $A\left(X\right)=\frac{\∂f\left(X\right)}{\∂X};$ T为时间间隔，T＝t_k+1-t_k；模型噪声Z_k+1为k+1时刻的观测变量；V_k+1为k+1时刻的雷达量测噪声；

若不考虑系统噪声，则系统k时刻的状态变量、观测变量的标称状态为

$X_{K+1}^{*}=G\left(X_k\right)$

$Z_{k+1}^{*}=h\left(X_{k+1}^{*}\right)+V_{k+1}$

将状态变量与标称状态作差得到状态偏差，则：

$\mathrm{\ΔX}=X_k-X_k^{*}$

$\mathrm{\ΔZ}=Z_k-Z_k^{*}$

围绕标称状态将函数h(X_k+1)和G(X_k)泰勒展开，并取一阶近似，得到基于状态偏差的近似线性方程为：

${\mathrm{\ΔX}}_{k+1}=\frac{\∂G\left(X_k^{*}\right)}{{\∂X}_k^{*}}{\mathrm{\ΔX}}_k+\mathrm{\Γ}\left(X_k^{*}\right)q_k$

${\mathrm{\ΔZ}}_{k+1}=\frac{\∂h\left(X_k^{*}\right)}{{\∂X}_k^{*}}{\mathrm{\ΔX}}_{k+1}+V_{k+1}.$

步骤2，由观测数据计算出输出相关函数序列；

令输出相关函数序列为{C_i}：

$C_0=E\left[Z_k{Z}_k^T\right]=\mathrm{B\γ}{B}^T+R$

$C_i=E\left[Z_k{Z}_{k-i}^T\right]=B{\mathrm{\Φ}}^i{\mathrm{\γB}}^T$

式中，下标i表示时间间隔；上标T表示矩阵的转置；Z_k为观测数据；R为量测噪声V_k+1的协方差矩阵；E为数学期望；Υ、B、Φ均为中间变量，Φⁱ为Φ的i次方，

设Z_k的自相关函数C_i的估计值是计算公式为：

${\hat{C}}_i^k={\hat{C}}_i^{k-1}+\frac{1}{k}(Z_k{Z}_{k-i}^T-{\hat{C}}_i^{k-1})$

由C_i的表达式推出：

$\left[\begin{array}{c}{C}_1\\ C_2\\ \·\\ \·\\ C_n\end{array}\right]=\left[\begin{array}{c}{\mathrm{B\Φ\γB}}^T\\ {\mathrm{B\Φ}}^2{\mathrm{\γB}}^T\\ \·\\ \·\\ {\mathrm{B\Φ}}^n{\mathrm{\γB}}^T\end{array}\right]={\mathrm{D\γB}}^T$

式中，n为观测量相关函数状态维数；D为线性系统的可观测矩阵；ΥB^T阵与卡尔曼滤波增益矩阵K相关；

D＝[BΦ,BΦ²,...,BΦⁿ]^T

${\mathrm{\γ}}_{k+1}{B}^T={\left[D^{T}D\right]}^{-1}{D}^T{[{\hat{C}}_1^{k+1},{\hat{C}}_2^{k+1}...{\hat{C}}_n^{k+1}]}^T$

步骤3，由输出相关函数序列推算出卡尔曼滤波增益矩阵的最优稳态解；

卡尔曼滤波增益矩阵表达式如下，通过{C_i}和ΥB^T，使用近似解法求出F阵的相似值，最终得到K的最优稳态解；

K＝(ΥB^T-FB^T)[C₀-BFB^T]^-1＝(ΥB^T-FB^T)[BΥB^T+R-BFB^T]^-1

式中，F为中间过程变量，F＝Φ[F+(ΥB^T-FB^T)(C₀-BFB^T)^-1(ΥB^T-FB^T)^T]Φ^T。

步骤4，将卡尔曼滤波增益矩阵的最优稳态解代入卡尔曼滤波算法递推方程中的状态估计方程，得到输出相关自适应滤波的卡尔曼滤波算法递推方程，对雷达观测的一段飞行弹道参数进行滤波，实现对弹道数据的自适应卡尔曼滤波估计和修正；

所述输出相关自适应滤波的卡尔曼滤波算法递推方程如下：

状态一步预测方程：

$\mathrm{\Δ}{\hat{X}}_{k+1/k}=\frac{\∂G(X_k^{*},k)}{{\∂X}_k^{*}}{\mathrm{\ΔX}}_k$

新息方程：

$\mathrm{\Δ}{\tilde{Z}}_{k+1}={\mathrm{\ΔZ}}_{k+1}-\frac{\∂h(X_{k+1}^{*},k+1)}{{\∂X}_{k+1}^{*}}{\mathrm{\ΔX}}_{k+1/k}$

状态估计方程：

$\mathrm{\Δ}{\hat{X}}_{k+1}=\mathrm{\Δ}{\hat{X}}_{k+1/k}+K_{k+1}[{\mathrm{\ΔZ}}_{k+1}-\frac{\∂h(X_{k+1}^{*},k+1)}{{\∂X}_{k+1}^{*}}\mathrm{\Δ}{\hat{X}}_{k+1/k}]$

${\hat{X}}_{k+1}=X_{k+1}^{*}+\mathrm{\Δ}{\hat{X}}_{k+1}$

式中，为状态偏差的一步预测估计值；ΔX_k+1/k为状态偏差的一步预测值，为状态偏差的新息估计值；为状态偏差的状态估计值；为状态估计值。

步骤5，根据步骤4滤波后的飞行弹道参数，利用弹道模型外推弹道落点，形成弹道轨迹。

本发明采用以上技术方案与现有技术相比，利用炮位雷达获得弹丸的轨迹参数，自适应调整输出吸纳骨干函数矩阵，得出稳态增益矩阵，然后利用卡尔曼滤波得到最优滤波值，有效抑制了由于弹道属性模型与实际物理模型不完全相符、系统噪声和观测噪声参数估计不精确等因素造成的滤波发散问题，提高了滤波精度，从而能够准确得到弹道轨迹。

附图说明

图1是本发明的流程图。

图2是高度Y方向在普通卡尔曼滤波下的仿真图。

图3是高度Y方向在自适应卡尔曼滤波下的仿真图。

具体实施方式

下面结合附图对本发明的技术方案做进一步的详细说明：

一种基于输出相关自适应卡尔曼滤波的弹道轨迹形成方法，如图1所示，具体步骤如下：

步骤1，根据弹道模型建立基于状态变量的弹道方程及观测方程，将状态变量与标称状态作差得到状态偏差，从而建立基于状态偏差的近似线性方程；

在地面坐标系中建立弹道方程：

$\frac{\mathrm{dX}}{\mathrm{dt}}=\left[\begin{array}{c}\frac{\mathrm{dx}}{\mathrm{dt}}\\ \frac{\mathrm{dy}}{\mathrm{dt}}\\ \frac{\mathrm{dz}}{\mathrm{dt}}\\ \frac{{\mathrm{dv}}_x}{\mathrm{dt}}\\ \frac{{\mathrm{dv}}_y}{\mathrm{dt}}\\ \frac{{\mathrm{dv}}_z}{\mathrm{dt}}\end{array}\right]=\left[\begin{array}{c}{v}_x\\ v_y\\ v_z\\ -K_D\frac{{\mathrm{SC}}_x\left(\mathrm{Ma}\right)}{2m}{\mathrm{\ρv}}_r(v_x-w_x)+K_D\frac{{\mathrm{SC}}_x\left(\mathrm{Ma}\right)}{2m}{\mathrm{\ρv}}_r{w}_x^{\′}\\ {-K}_D\frac{{\mathrm{SC}}_x\left(\mathrm{Ma}\right)}{2m}{\mathrm{\ρv}}_r{v}_y-g\\ {-K}_D\frac{{\mathrm{SC}}_x\left(\mathrm{Ma}\right)}{2m}{\mathrm{\ρv}}_r(v_z-w_z)+K_L{B}_Z+K_D\frac{{\mathrm{SC}}_x\left(\mathrm{Ma}\right)}{2m}{\mathrm{\ρv}}_r{w}_z^{\′}\end{array}\right]$

式中：X为t时刻的状态变量；表示对时间t求导数；x，y，z分别为弹丸的距离、高度、侧偏；v_x,v_y,v_z分别为弹丸速度的三轴分量；v_r为含风速在内的弹丸实际速度；w_x,w_z分别为纵风和横风风速，在标准条件下均取为0；w'_x,w'_z分别为纵风和横风的随机量；S为弹丸最大截断面积，d为弹径；m为弹体质量；Ma为马赫数，C_s为声速，其中k为比热比，对于空气k＝1.404；R_d为干空气气体常数；τ为虚温，是把湿空气折合成干空气时对气温的修正；C_x(Ma)为空气阻力系数，它是马赫数Ma的函数；ρ为空气密度，其中，p为气体压强；g为海拔y处的重力加速度，其中g₀为海平面重力加速度，r₀为地球半径；K_D为阻力符合系数；B_Z为侧向升力加速度；K_L为由动力平衡角引起的侧向加速度时弥补误差的修正系数，又称静力矩符合系数；

令状态变量X＝[x,y,z,v_x,v_y,v_z,c,ac]^T＝[x₁,x₂,x₃,x₄,x₅,x₆,x₇,x₈]^T，式中c为弹道系数，ac为修正的弹道系数，x₁～x₈为状态变量X的8个状态，分别与x,y,z,v_x,v_y,v_z,c,ac对应；

令x_4r＝x₄-w_x，表示弹丸在x方向受风速影响后的实际速度；x_6r＝x₆-w_z，表示弹丸在z方向的实际飞行速度，则

取作为中间变量；同时将声速带入，马赫数表示为 $\mathrm{Ma}=\frac{v_r}{C_s}=\frac{v_r}{20.0468\sqrt{\mathrm{\τ}}},$ 将弹道方程改写为：

$\frac{\mathrm{dX}}{\mathrm{dt}}=f\left(X\right)+\mathrm{\ΓW}=\left[\begin{array}{c}{x}_4\\ x_5\\ x_6\\ {-x}_7{\mathrm{Nx}}_{4r}\\ {-x}_7{\mathrm{Nx}}_5-g\\ {-x}_7{\mathrm{Nx}}_{6r}+x_8{B}_Z\\ 0\\ 0\end{array}\right]+\left[\begin{array}{c}{0}_{3\×3}\\ I_{3\×3}\\ 0_{2\×3}\end{array}\right]x_{7}N\left[\begin{array}{c}{w}_x^{\′}\\ 0\\ w_z^{\′}\end{array}\right]$

式中，f(X)为状态变量X的函数；Γ为噪声驱动阵，Γ＝[0_3×3I_3×30_2×3]^Tx₇N；W为系统噪声，W＝[w'_x0w'_z]^T；0_3×3表示3×3的零矩阵；I_3×3表示3×3的单位阵；0_2×3表示2×3的零矩阵；

建立观测方程为：

Z＝h(X)+V

式中， $h\left(X\right)={\left[\sqrt{x^2+y^2+z^2}\arctan \frac{z}{x}\arctan \frac{y}{x^2+z^2}\right]}^T,$ V为雷达量测噪声；

在采样区间[t_k,t_k+1]上将f(X(t))近似展开并将弹道方程离散化，得到：

X_k+1＝G(X_k)+Γ_kq_k

Z_k+1＝h(X_k+1)+V_k+1

式中，X_k为k时刻的状态变量；X_k+1为k+1时刻的状态变量； $G\left(X_k\right)=X_k+f\left(X_k\right)T+A\left(X_k\right)f\left(X_k\right)\frac{T^2}{2},$ A(X)是f(X)关于X的偏导，即 $A\left(X\right)=\frac{\∂f\left(X\right)}{\∂X};$ T为时间间隔，T＝t_k+1-t_k；模型噪声Z_k+1为k+1时刻的观测变量；V_k+1为k+1时刻的雷达量测噪声；

若不考虑系统噪声，则系统k时刻的状态变量、观测变量的标称状态为

$X_{K+1}^{*}=G\left(X_k\right)$

$Z_{k+1}^{*}=h\left(X_{k+1}^{*}\right)+V_{k+1}$

将状态变量与标称状态作差得到状态偏差，则：

$\mathrm{\ΔX}=X_k-X_k^{*}$

$\mathrm{\ΔZ}=Z_k-Z_k^{*}$

围绕标称状态将函数h(X_k+1)和G(X_k)泰勒展开，并取一阶近似，得到基于状态偏差的近似线性方程为：

${\mathrm{\ΔX}}_{k+1}=\frac{\∂G\left(X_k^{*}\right)}{{\∂X}_k^{*}}{\mathrm{\ΔX}}_k+\mathrm{\Γ}\left(X_k^{*}\right)q_k$

${\mathrm{\ΔZ}}_{k+1}=\frac{\∂h\left(X_k^{*}\right)}{{\∂X}_k^{*}}{\mathrm{\ΔX}}_{k+1}+V_{k+1}.$

步骤2，由观测数据计算出输出相关函数序列；

令输出相关函数序列为{C_i}：

$C_0=E\left[Z_k{Z}_k^T\right]=\mathrm{B\γ}{B}^T+R$

$C_i=E\left[Z_k{Z}_{k-i}^T\right]=B{\mathrm{\Φ}}^i{\mathrm{\γB}}^T$

式中，下标i表示时间间隔；上标T表示矩阵的转置；Z_k为观测数据；R为量测噪声V_k+1的协方差矩阵；E为数学期望；Υ、B、Φ均为中间变量，Φⁱ为Φ的i次方，

设Z_k的自相关函数C_i的估计值是计算公式为：

${\hat{C}}_i^k={\hat{C}}_i^{k-1}+\frac{1}{k}(Z_k{Z}_{k-i}^T-{\hat{C}}_i^{k-1})$

由C_i的表达式推出：

$\left[\begin{array}{c}{C}_1\\ C_2\\ \·\\ \·\\ C_n\end{array}\right]=\left[\begin{array}{c}{\mathrm{B\Φ\γB}}^T\\ {\mathrm{B\Φ}}^2{\mathrm{\γB}}^T\\ \·\\ \·\\ {\mathrm{B\Φ}}^n{\mathrm{\γB}}^T\end{array}\right]={\mathrm{D\γB}}^T$

式中，n为观测量相关函数状态维数；D为线性系统的可观测矩阵；ΥB^T阵与卡尔曼滤波增益矩阵K相关；

D＝[BΦ,BΦ²,...,BΦⁿ]^T

${\mathrm{\γ}}_{k+1}{B}^T={\left[D^{T}D\right]}^{-1}{D}^T{[{\hat{C}}_1^{k+1},{\hat{C}}_2^{k+1}...{\hat{C}}_n^{k+1}]}^T.$

步骤3，由输出相关函数序列推算出卡尔曼滤波增益矩阵的最优稳态解；

卡尔曼滤波增益矩阵表达式如下，通过{C_i}和ΥB^T，使用近似解法求出F阵的相似值，最终得到K的最优稳态解；

K＝(ΥB^T-FB^T)[C₀-BFB^T]^-1＝(ΥB^T-FB^T)[BΥB^T+R-BFB^T]^-1

式中，F为中间过程变量，F＝Φ[F+(ΥB^T-FB^T)(C₀-BFB^T)^-1(ΥB^T-FB^T)^T]Φ^T。

步骤4，将卡尔曼滤波增益矩阵的最优稳态解代入卡尔曼滤波算法递推方程中的状态估计方程，得到输出相关自适应滤波的卡尔曼滤波算法递推方程，对雷达观测的一段飞行弹道参数进行滤波，实现对弹道数据的自适应卡尔曼滤波估计和修正；

所述输出相关自适应滤波的卡尔曼滤波算法递推方程如下：

状态一步预测方程：

$\mathrm{\Δ}{\hat{X}}_{k+1/k}=\frac{\∂G(X_k^{*},k)}{{\∂X}_k^{*}}{\mathrm{\ΔX}}_k$

新息方程：

$\mathrm{\Δ}{\tilde{Z}}_{k+1}={\mathrm{\ΔZ}}_{k+1}-\frac{\∂h(X_{k+1}^{*},k+1)}{{\∂X}_{k+1}^{*}}{\mathrm{\ΔX}}_{k+1/k}$

状态估计方程：

$\mathrm{\Δ}{\hat{X}}_{k+1}=\mathrm{\Δ}{\hat{X}}_{k+1/k}+K_{k+1}[{\mathrm{\ΔZ}}_{k+1}-\frac{\∂h(X_{k+1}^{*},k+1)}{{\∂X}_{k+1}^{*}}\mathrm{\Δ}{\hat{X}}_{k+1/k}]$

${\hat{X}}_{k+1}=X_{k+1}^{*}+\mathrm{\Δ}{\hat{X}}_{k+1}$

式中，为状态偏差的一步预测估计值；ΔX_k+1/k为状态偏差的一步预测值，为状态偏差的新息估计值；为状态偏差的状态估计值；为状态估计值。

步骤5，根据步骤4滤波后的飞行弹道参数，利用弹道模型外推弹道落点，形成弹道轨迹。该步骤为本领域的常规技术手段，在此不再赘述。

为了验证本发明的自适应滤波算法的有效性，首先仿真获取雷达跟踪弹丸的飞行参数，具体为：假设雷达在距离方向的噪声方差方向角的噪声方差俯仰角的噪声方差在雷达阵面法线方向的方位角Na＝-45°，天线横向倾斜角d＝0，天线纵向倾斜角t＝45°，利用计算机随机函数模拟产生雷达的量测，将这些噪声与仿真得到的雷达仿真数据叠加，就得到计算机模拟的弹道跟踪的测量数据。

分别采用普通卡尔曼滤波算法和本发明的自适应卡尔曼滤波算法对测量数据进行滤波处理。在对测量数据进行滤波过程中，由于噪声等因素的存在以及对模型误差估计不准确，采用普通卡尔曼滤波会出现滤波发散现象，如图2所示，在第11s处出现发散现象，不能准确得到弹道轨迹。而采用本发明的自适应卡尔曼滤波则能解决上述原因引起的滤波发散问题，得到较为准确的弹道轨迹，如图3所示。

以上所述，仅为本发明中的具体实施方式，但本发明的保护范围并不局限于此，任何熟悉该技术的人在本发明所揭露的技术范围内，可理解想到的变换或替换，都应涵盖在本发明的包含范围之内，因此，本发明的保护范围应该以权利要求书的保护范围为准。

Claims (1)

1.一种基于输出相关自适应卡尔曼滤波的弹道轨迹形成方法，其特征在于，具体步骤如下：

步骤1，根据弹道模型建立基于状态变量的弹道方程及观测方程，将状态变量与标称状态作差得到状态偏差，从而建立基于状态偏差的近似线性方程；

步骤2，由观测数据计算出输出相关函数序列；

步骤3，由输出相关函数序列推算出卡尔曼滤波增益矩阵的最优稳态解；

步骤4，将卡尔曼滤波增益矩阵的最优稳态解代入卡尔曼滤波算法递推方程中的状态估计方程，得到输出相关自适应滤波的卡尔曼滤波算法递推方程，对雷达观测的一段飞行弹道参数进行滤波，实现对弹道数据的自适应卡尔曼滤波估计和修正；

步骤5，根据步骤4滤波后的飞行弹道参数，利用弹道模型外推弹道落点，形成弹道轨迹；

其中，步骤1中所述建立基于状态偏差的近似线性方程具体如下：

在地面坐标系中建立弹道方程：

$\frac{dX}{dt}=\left[\begin{array}{c}\frac{dx}{dt}\\ \frac{dy}{dt}\\ \frac{dz}{dt}\\ \frac{{\mathrm{dv}}_x}{dt}\\ \frac{{\mathrm{dv}}_y}{dt}\\ \frac{{\mathrm{dv}}_z}{dt}\end{array}\right]=\left[\begin{array}{c}{v}_x\\ v_y\\ v_z\\ -K_D\frac{{\mathrm{SC}}_x\left(Ma\right)}{2m}{\mathrm{\ρv}}_r\left(v_x-w_x\right)+K_D\frac{{\mathrm{SC}}_x\left(Ma\right)}{2m}{\mathrm{\ρv}}_r{w}_x^{\′}\\ -K_D\frac{{\mathrm{SC}}_x\left(Ma\right)}{2m}{\mathrm{\ρv}}_r{v}_y-g\\ -K_D\frac{{\mathrm{SC}}_x\left(Ma\right)}{2m}{\mathrm{\ρv}}_r\left(v_z-w_z\right)+K_L{B}_Z+K_D\frac{{\mathrm{SC}}_x\left(Ma\right)}{2m}{\mathrm{\ρv}}_r{w}_z^{\′}\end{array}\right]$

式中：X为t时刻的状态变量；表示对时间t求导数；x，y，z分别为弹丸的距离、高度、侧偏；v_x,v_y,v_z分别为弹丸速度的三轴分量；v_r为含风速在内的弹丸实际速度；w_x,w_z分别为纵风和横风的风速，在标准条件下均取为0；w'_x,w'_z分别为纵风和横风的随机量；S为弹丸最大截断面积，d为弹径；m为弹体质量；Ma为马赫数，C_s为声速，其中k为比热比，对于空气k＝1.404；R_d为干空气气体常数；τ为虚温，是把湿空气折合成干空气时对气温的修正；C_x(Ma)为空气阻力系数，它是马赫数Ma的函数；ρ为空气密度，其中，p为气体压强；g为海拔y处的重力加速度，其中g₀为海平面重力加速度，r₀为地球半径；K_D为阻力符合系数；B_Z为侧向升力加速度；K_L为由动力平衡角引起的侧向加速度时弥补误差的修正系数，又称静力矩符合系数；

令状态变量X＝[x,y,z,v_x,v_y,v_z,c,ac]^T＝[x₁,x₂,x₃,x₄,x₅,x₆,x₇,x₈]^T，式中c为弹道系数，ac为修正的弹道系数，x₁～x₈为状态变量X的8个状态，分别与x,y,z,v_x,v_y,v_z,c,ac对应；

令x_4r＝x₄-w_x，表示弹丸在x方向受风速影响后的实际速度；x_6r＝x₆-w_z，表示弹丸在z方向的实际飞行速度，则

取作为中间变量；同时将声速代入，马赫数表示为 $Ma=\frac{v_r}{C_s}=\frac{v_r}{20.0468\sqrt{\mathrm{\τ}}},$ 将弹道方程改写为：

$\frac{dX}{dt}=f\left(X\right)+\mathrm{\Γ}W=\left[\begin{array}{c}{x}_4\\ x_5\\ x_6\\ -x_7{\mathrm{Nx}}_{4r}\\ -x_7{\mathrm{Nx}}_5-g\\ -x_7{\mathrm{Nx}}_{6r}+x_8{B}_Z\\ 0\\ 0\end{array}\right]+\left[\begin{array}{c}{0}_{3\×3}\\ I_{3\×3}\\ 0_{2\×3}\end{array}\right]x_{7}N\left[\begin{array}{c}{w}_x^{\′}\\ 0\\ w_z^{\′}\end{array}\right]$

式中，f(X)为状态变量X的函数；Γ为噪声驱动阵，Γ＝[0_3×3I_3×30_2×3]^Tx₇N；W为系统噪声，W＝[w'_x0w'_z]^T；0_3×3表示3×3的零矩阵；I_3×3表示3×3的单位阵；0_2×3表示2×3的零矩阵；

建立观测方程为：

Z＝h(X)+V

式中， $h\left(X\right)={\left[\begin{array}{ccc}\sqrt{x^2+y^2+z^2}& \arctan \frac{z}{x}& \arctan \frac{y}{\sqrt{x^2+z^2}}\end{array}\right]}^T,$ V为雷达量测噪声；

在采样区间[t_k,t_k+1]上将f(X)近似展开并将弹道方程离散化，得到：

X_k+1＝G(X_k)+Γ_kW_k

Z_k+1＝h(X_k+1)+V_k+1

式中，X_k为k时刻的状态变量；X_k+1为k+1时刻的状态变量； $G\left(X_k\right)=X_k+f\left(X_k\right)T+A\left(X_k\right)f\left(X_k\right)\frac{T^2}{2},$ A(X)是f(X)关于X的偏导，即 $A\left(X\right)=\frac{\∂f\left(X\right)}{\∂X};$ T为时间间隔，T＝t_k+1-t_k；模型噪声X(t)表示状态变量X是时间t的函数,Γ(X(t))表示状态变量为X(t)时的噪声驱动阵Γ,W(t)表示系统噪声W是时间t的函数；Z_k+1为k+1时刻的观测变量；V_k+1为k+1时刻的雷达量测噪声；

若不考虑系统噪声，则系统k+1时刻的状态变量、观测变量的标称状态为

$X_{k+1}^{*}=G\left(X_k\right)$

$Z_{k+1}^{*}=h\left(X_{k+1}^{*}\right)+V_{k+1}$

将状态变量与标称状态作差得到状态偏差，则：

$\mathrm{\Δ}X=X_k-X_k^{*}$

$\mathrm{\Δ}Z=Z_k-Z_k^{*}$

围绕标称状态将函数h(X_k+1)和G(X_k)泰勒展开，并取一阶近似，得到基于状态偏差的近似线性方程为：

${\mathrm{\ΔX}}_{k+1}=\frac{\∂G\left(X_k^{*}\right)}{\∂X_k^{*}}{\mathrm{\ΔX}}_k+\mathrm{\Γ}\left(X_k^{*}\right)W_k$

${\mathrm{\ΔZ}}_{k+1}=\frac{\∂h\left(X_k^{*}\right)}{\∂X_k^{*}}{\mathrm{\ΔX}}_{k+1}+V_{k+1};$

步骤2中所述由观测数据计算出输出相关函数序列具体如下：

令输出相关函数序列为{C_i}：

式中，下标i表示时间间隔；上标T表示矩阵的转置；Z_k为观测数据；R为量测噪声V_k+1的协方差矩阵；E为数学期望；Υ、B、Φ均为中间变量，Φⁱ为Φ的i次方， $\mathrm{\Φ}=\frac{\∂G}{\∂X_k^{*}};$

设Z_k的自相关函数C_i的估计值是计算公式为：

${\hat{C}}_i^k={\hat{C}}_i^{k-1}+\frac{1}{k}(Z_k{Z}_{k-i}^T-{\hat{C}}_i^{k-1})$

由C_i的表达式推出：

式中，n为观测量相关函数状态维数；D为线性系统的可观测矩阵；ΥB^T阵与卡尔曼滤波增益矩阵K相关；

D＝[BΦ,BΦ²,...,BΦⁿ]^T

步骤3中所述由输出相关函数序列推算出卡尔曼滤波增益矩阵K的最优稳态解，具体如下：

卡尔曼滤波增益矩阵表达式如下，通过{C_i}和ΥB^T，使用近似解法求出F阵的相似值，最终得到K的最优稳态解；

K＝(ΥB^T-FB^T)[C₀-BFB^T]^-1＝(ΥB^T-FB^T)[BΥB^T+R-BFB^T]^-1

式中，F为中间过程变量，F＝Φ[F+(ΥB^T-FB^T)(C₀-BFB^T)^-1(ΥB^T-FB^T)^T]Φ^T；

步骤4中所述卡尔曼滤波算法递推方程包括状态一步预测方程、新息方程、状态估计方程，具体表达式如下：

状态一步预测方程：

$\mathrm{\Δ}{\hat{X}}_{k+1/k}=\frac{\∂G(X_k^{*},k)}{\∂X_k^{*}}{\mathrm{\ΔX}}_k$

新息方程：

$\mathrm{\Δ}{\tilde{Z}}_{k+1}={\mathrm{\ΔZ}}_{k+1}-\frac{\∂h(X_{k+1}^{*},k+1)}{\∂X_{k+1}^{*}}{\mathrm{\ΔX}}_{k+1/k}$

状态估计方程：

$\mathrm{\Δ}{\hat{X}}_{k+1}=\mathrm{\Δ}{\hat{X}}_{k+1/k}+K_{k+1}\[{\mathrm{\ΔZ}}_{k+1}-\frac{\∂h(X_{k+1}^{*},k+1)}{\∂X_{k+1}^{*}}\mathrm{\Δ}{\hat{X}}_{k+1/k}\]$

${\hat{X}}_{k+1}=X_{k+1}^{*}+\mathrm{\Δ}{\hat{X}}_{k+1}$

式中，为状态偏差的一步预测估计值；ΔX_k+1/k为状态偏差的一步预测值，为状态偏差的新息估计值；为状态偏差的状态估计值；为状态估计值。