CN107621632A - 用于nshv跟踪滤波的自适应滤波方法及*** - Google Patents

Abstract

本发明公开了用于NSHV跟踪滤波的自适应滤波方法及***，该方法基于未知攻角的动力学模型(状态转移方程)和雷达测量模型(测量方程)；基于次优无偏噪声统计估计器的递推方法；该方法用于临近空间高超声速助推‑滑翔飞行器跟踪可以有效提升滤波稳定性和跟踪精度，速率平均跟踪精度比Sage‑Husa自适应方法可以提升50％，速率平均跟踪精度比Sage‑Husa自适应方法可以提升6.77％，100m内的地心距跟踪精度与6m/s内的速度跟踪满足临近空间高超声速滑翔飞行器的防御需求。

Description

用于NSHV跟踪滤波的自适应滤波方法及***

技术领域

本发明涉及目标跟踪滤波技术领域，特别涉及一种用于NSHV跟踪滤波的自适应滤波方 法及***。

背景技术

临近空间高超声速飞行器(Near Space Hypersonic Vehicle，NSHV)是飞行在距离地 面20km～100km的临近空间，飞行速度一般在5Ma以上的飞行器。基于助推-滑翔弹道概念 的NSHV成为国内外的研究热点之一，该类飞行器经过助推和初始弹道飞行阶段，获得一定 的速度和高度后，在临近空间依靠高升阻比的气动外形做高超声速滑翔式飞行。NSHV由于 飞行速度快、机动能力强，可以在两小时内对全球目标实现精确打击。对防御而言，这类 目标的探测跟踪和威胁评估是很大的难点。

卡尔曼滤波框架是单模型跟踪算法的核心，需要建立带有误差项的状态转移方程和测 量方程，结合初始条件不断递推求取状态的最优估计(线性卡尔曼滤波)或次优估计(非 线性卡尔曼滤波)。数学家Kalman在19世纪60年代提出了线性卡尔曼滤波理论，奠定了现代滤波理论的基础。Bucy和Y.Sunahara等人利用一阶泰勒级数展开来对非线性函数进行局部线性化，提出了扩展卡尔曼滤波器(Extend Kalman Filter，EKF)，把卡尔曼滤波 引入了非线性***状态估计。为了弥补EKF容易发散且只适合弱非线性***的缺陷，S.J.Julier和J.K.Uhlmann从“对概率分布近似要比对非线性函数近似容易得多”的思路出 发，提出了无迹卡尔曼滤波器(Unscented Kalman Filter，UKF)。Ienkaran Arasaratnam 利用严格的数学推导，在容积准则(Cubature Rule)的基础上提出容积卡尔曼滤波(CubatureKalman Filter，CKF)方法。

标准的CKF方法需要知道过程噪声统计特性和测量噪声统计特性，然而在实际应用中 噪声统计特性一般是未知的或者不准确的。如果将不准确的噪声统计特性用于CKF易造成 状态估计误差偏大甚至滤波发散。自适应滤波方法的关键就是对噪声进行估计，现有的主 要分为滤波器在线调整方法和噪声统计特性在线估计方法这两类。滤波器在线调整方法主 要是通过引入相应因子来抑制滤波发散，使滤波稳定，但是较难提高滤波精度。噪声统计 特性在线估计方法虽然实现了对非线性***噪声的估计，但是牺牲了滤波精度导致了误差 偏大，甚至也会出现***发散的可能，难以应用于研究的临近空间高超声速助推-滑翔飞行 器跟踪的高阶***。

发明内容

针对在利用传统的单模型跟踪滤波方法对临近空间高超声速助推-滑翔飞行器跟踪时 误差大、易发散的问题，本发明提供了一种用于NSHV跟踪滤波的自适应滤波方法及***， 可以避免滤波发散，大大提升对NSHV跟踪滤波的数值稳定性和滤波精度。

本发明提供的用于NSHV跟踪滤波的自适应滤波方法，包括以下步骤：

对NSHV跟踪建立离散化的状态转移方程；

对NSHV跟踪建立离散化的测量方程；

获得状态值、测量噪声均值、测量噪声协方差、过程噪声协方差估计值、以及过程噪 声均值估计值的贝叶斯估计；

获得极大后验估计值和噪声统计的极大后验估计器；

依据所述离散化的状态转移方程、所述离散化的测量方程、所述状态值、测量噪声均 值、测量噪声协方差、过程噪声协方差估计值、以及过程噪声均值估计值的贝叶斯估计、以及所述极大后验估计值和噪声统计的极大后验估计器，获得次优无偏噪声统计估计器的递推方法；

依据所述次优无偏噪声统计估计器的递推方法进行滤波。

作为一种可实施方式，所述离散化的状态转移方程为X(k+1)＝f(X(k))+w(k)；所述离散 化的测量方程为Y(k)＝h(X(k))+v₀(k)；

其中，x(k)表示k时刻的状态，y(k)表示k时刻的测量值，w(k)、v₀(k)分别为零均值白噪声，f(x)为非线性的状态转移方程，h(x)为非线性的测量方程。

作为一种可实施方式，对NSHV跟踪建立离散化的状态转移方程包括以下步骤：

Cholesky分解步骤：若k-1时刻后验概率密度函数已知， 则通过式子P(k-1|k-1)＝S(k-1|k-1)[S(k-1|k-1)]^T对误差协方差进行Cholesky分解；其中， P(k|k)表示k时刻滤波误差协方差矩阵，S(k|k)[S(k|k)]^T为Cholesky分解结果；

计算容积点步骤：通过式子计算容积点； 其中，是容积点；

传播容积点步骤：通过式子X^*(i，k|k-1)＝f[X(i，k-1|k-1)]传播容积点；其中，X^*(i，k|k-1) 为传播得到的容积点；

状态预测步骤：通过式子进行状态预测；其中，m＝2n为容积 点总数，n为状态向量的维数，表示由k-1时刻预测k时刻的状态向量；

协方差预测步骤：通过式子进行协方差预测； 其中，P(k|k-1)表示由k-1时刻预测k时刻的预测误差协方差矩阵，Q(k)表示k时刻的过 程噪声协方差矩阵。

作为一种可实施方式，对NSHV跟踪建立离散化的测量方程包括以下步骤：

Cholesky分解步骤：通过式子P(k|k-1)＝S(k|k-1)[S(k|k-1)]^T对误差协方差进行 Cholesky分解；其中，P(k|k)表示k时刻滤波误差协方差矩阵，S(k|k)[S(k|k)]^T为Cholesky 分解结果；

计算容积点步骤：通过式子计算容积点；其中，是容 积点；

传播容积点步骤：通过式子Y(i，k|k-1)＝h[X(i，k|k-1)]传播容积点；其中，X^*(i，k|k-1)为 传播得到的容积点；

测量预测步骤：通过式子进行状态预测；其中，m＝2n为容积点 总数，n为测量向量的维数，表示由k-1时刻预测k时刻的测量向量；

估计自相关协方差阵步骤：通过式子估计自相 关协方差阵；其中，P(yy，k|k-1)为测量估计自相关协方差阵，R(k)表示k时刻的测量噪声 协方差矩阵；

估计互相关协方差步骤：通过式子估计互相关 协方差；其中，P(xy，k|k-1)为状态与测量互协方差阵；

计算Kalman增益步骤：通过式子K_k＝P(xy，k|k-1)P^-1(yy，k|k-1)计算Kalman增益；

状态更新步骤：通过式子进行状态更新；其中，Y(k) 表示k时刻的***测量值；

估计协方差步骤：通过式子估计协方差。

作为一种可实施方式，通过使式子p[XX(k)，q，Q，r，R|YY(k)]的后验概率密度极大化，获得 所述状态值、测量噪声均值、测量噪声协方差、过程噪声协方差估计值、以及过程噪声均 值估计值的贝叶斯估计。

作为一种可实施方式，通过获得式子的非条件概率密度函数 的极大值，获得所述状态值、测量噪声均值、测量噪声协方差、过程噪声协方差估计值、 以及过程噪声均值估计值的贝叶斯估计。

相应地，本发明还提供了用于NSHV跟踪滤波的自适应滤波***，包括状态转移方程建 立单元、测量方程建立单元、贝叶斯估计获得单元、极大后验估计值和噪声统计的极大后 验估计器获得单元、递推单元、以及滤波单元；

所述状态转移方程建立单元用于对NSHV跟踪建立离散化的状态转移方程；

所述测量方程建立单元用于对NSHV跟踪建立离散化的测量方程；

所述贝叶斯估计获得单元用于获得状态值、测量噪声均值、测量噪声协方差、过程噪 声协方差估计值、以及过程噪声均值估计值的贝叶斯估计；

所述极大后验估计值和噪声统计的极大后验估计器获得单元用于获得极大后验估计值 和噪声统计的极大后验估计器；

所述递推单元用于依据所述离散化的状态转移方程、所述离散化的测量方程、所述状 态值、测量噪声均值、测量噪声协方差、过程噪声协方差估计值、以及过程噪声均值估计 值的贝叶斯估计、以及所述极大后验估计值和噪声统计的极大后验估计器，获得次优无偏 噪声统计估计器的递推方法；

所述滤波单元用于依据所述次优无偏噪声统计估计器的递推方法进行滤波。

本发明相比于现有技术的有益效果在于：

本发明提供的用于NSHV跟踪滤波的自适应滤波方法及***，该方法基于未知攻角的动 力学模型(状态转移方程)和雷达测量模型(测量方程)；基于次优无偏噪声统计估计器的 递推方法；该方法用于临近空间高超声速助推-滑翔飞行器跟踪可以有效提升滤波稳定性和 跟踪精度，速率平均跟踪精度比Sage-Husa自适应方法可以提升50％，速率平均跟踪精度比 Sage-Husa自适应方法可以提升6.77％，100m内的地心距跟踪精度与6m/s内的速度跟踪满 足临近空间高超声速滑翔飞行器的防御需求。

附图说明

图1为本发明一实施例提供的用于NSHV跟踪滤波的自适应滤波方法的流程图；

图2为目标飞行高度仿真曲线图；

图3为本发明另一实施例提供的用于NSHV跟踪滤波的自适应滤波***的框图。

附图标记

100、状态转移方程建立单元；200、测量方程建立单元；300、贝叶斯估计获得单元；400、极大后验估计值和噪声统计的极大后验估计器获得单元；500、递推单元；600、滤波 单元。

具体实施方式

以下结合附图，对本发明上述的和另外的技术特征和优点进行清楚、完整地描述，显 然，所描述的实施例仅仅是本发明的部分实施例，而不是全部实施例。

参照图1，示出了本发明提供的用于NSHV跟踪滤波的自适应滤波方法的流程。

本发明提出的用于NSHV跟踪滤波的自适应滤波方法，包括以下步骤；

S100、对NSHV跟踪建立离散化的状态转移方程；

S200、对NSHV跟踪建立离散化的测量方程；

S300、获得状态值、测量噪声均值、测量噪声协方差、过程噪声协方差估计值、以及过程噪声均值估计值的贝叶斯估计；

S400、获得极大后验估计值和噪声统计的极大后验估计器；

S500、依据所述离散化的状态转移方程、所述离散化的测量方程、所述状态值、测量 噪声均值、测量噪声协方差、过程噪声协方差估计值、以及过程噪声均值估计值的贝叶斯 估计、以及所述极大后验估计值和噪声统计的极大后验估计器，获得次优无偏噪声统计估 计器的递推方法；

S600、依据所述次优无偏噪声统计估计器的递推方法进行滤波。

以下具体展开说明这些步骤。

本发明提出的用于NSHV跟踪滤波的自适应滤波方法，利用了滤波器在线调整算法和噪 声统计特性在线估计算法，利用MAP估计器原理推导出了一种新型的自适应CKF算法，利 用NSHV的动力学模型建立状态转移方程，利用雷达测量原理建立测量方程，可以提高对NSHV 跟踪滤波的数值稳定性和滤波精度。

其中，有关NSHV动力学模型的介绍如下：

NSHV再入大气层后凭借自身升力体外形依靠气动力可以大范围机动滑翔。NSHV建模时 地球模型从椭球到圆球的简化误差较小，而在高空(60km)时地球自转引起的哥氏力与气 动力大小相当，在低空(30km)时指数大气模型会带来一定的气动力偏差，故基于考虑哥 氏力的圆球模型和拟合大气模型建立滑翔段的三自由度动力学方程。状态变量中位置用地 心距r、经度λ和纬度φ三个参数来描述，速度用速度大小V、速度倾角θ和速度偏角σ三个参数来描述。速度倾角θ是速度矢量与当地水平面的夹角，速度矢量指向水平面上方则为θ正。速度偏角σ是速度矢量在当地水平面投影与正北方向的夹角，从正北方向到速度 矢量顺时针转时σ为正。由上述参数在半速度坐标系(航迹坐标系)下建立三自由度动力 学方程为：

其中ω_e＝7.292×10^-5rad/s是地球自转角速度，v是倾侧角，表示升力方向与包含速度矢量的 铅垂面之间的夹角，从飞行器尾部向前看，若升力方向向右倾斜，则倾侧角为正。D和L是 阻力加速度和升力加速度，其表达式如下

式(2)和(3)中m为飞行器质量，q为动压，S_M为参考面积；C_D和C_L分别为阻力系数 和升力系数，通常是总攻角η与马赫数Ma的函数。NSHV一般采用倾斜转弯(Back-To-Turn，BTT)，可以认为飞行中侧滑角为零，故总攻角η近似等于攻角α。

根据1976美国标准大气模型，分别对大气密度、声速关于高度进行函数拟合，得到大 气密度函数为ρ＝ρ₀×10^-kh＝1.225×10^-0.1133h (4)

声速函数为

式(5)用h表示高度，单位为km。

NSVH参数采用美国CAV-H(高性能通用飞行器)的设计数据:质量m＝908kg，气动参考面 积S＝0.484m²，升力系数、阻力系数函数为

式(6)中，C_D0～C_D3依次为0.024，0.000724，0.406，-0.323。C_L0～C_L3依次为 -0.2317，0.0513，0.2945，-0.1028。

其中，有关雷达测量模型的介绍如下：

选择雷达接收天线回转中心o`为雷达测量坐标系原点，x`轴在过o`点的水平面内，指向 天文北；y`轴为过о`的铅垂线，指向上万，z`轴在过о`点的水平面内，由右手法则确定。设垂 线测量坐标系原点о`的地心球坐标为[r_o λ_o φ_o]^T，其相应的地心直角坐标为[X_o Y_o Z_o]^T。飞行 器质心的地心球坐标为[r λ φ]^T，飞行器质心的垂线测量坐标为[x yz]，其相应的地心直 角坐标为[X Y Z]^T，则其转换关系为

F＝R_Z(90°-λ_o)R_X(-φ_o)R_Y(90°) (8)

根据地心球坐标系与地心直角坐标系的关系，可得

[X Y Z]^T＝[rcosφcosλ rcosφsinλ rsinφ]^T (9)

[X_o Y_o Z_o]^T＝[r_ocosφ_ocosλ_o r_ocosφ_osinλ_o r_osinφ_o]^T (10)

则可以通过已知条件求得飞行器在雷达测量坐标系中的表示[x y z]^T。

根据雷达测量原理，则有

考虑测量误差，假定测量噪声是不相关的白噪声，则雷达实际测量方程为

记v₀＝[ε_R ε_A ε_E]^T，测量方程简记为

Y＝h(X)+v₀ (13)

测量噪声的统计特性为v₀→N(0，R)，这里

其中分别为距离、方位角和俯仰角的测量误差方差。

接下来，对NSHV跟踪建立离散化的状态转移方程和测量方程为

X(k+1)＝f(X(k))+w(k) (15)

Y(k)＝h(X(k))+v₀(k) (16)

其中x(k)表示k时刻的状态，y(k)表示k时刻的测量值，w(k)、v₀(k)分别为零均值白噪 声。f(x)为非线性的状态转移方程，h(x)为非线性的测量方程。

其中状态噪声w(k)和观测噪声v₀(k)为互不相关的高斯白噪声序列，其统计特性为：

E[w(k)]＝0，E[w(k)w`(j)]＝Q(k)δ_kj (17)

E[v₀(k)]＝0，E[v₀(k)v₀`(j)]＝R(k)δ_kj (18)

其中Q(k)，R(k)分别为状态噪声w(k)和测量噪声v₀(k)的协方差阵，设初始状态X(0)与 w(k)、v₀(k)独立。

以表示k时刻滤波最优估计状态向量(也称状态值，如步骤S300称之为状态值)， P(k|k-1)表示由k-1时刻预测k时刻的预测误差协方差矩阵，P(k|k)表示k时刻滤波误差协方 差矩阵，K_k表示k时刻的卡尔曼滤波增益，H(k)表示k时刻的测量矩阵，Y(k)表示k时刻的系 统测量值，R(k)表示k时刻的测量噪声协方差矩阵。

将容积准则应用于贝叶斯滤波理论即可获得CKF，标准的CKF分为时间更新和测量更新两 部分。

其中，时间更新如下：

1)Cholesky分解

假设k-1时刻后验概率密度函数已知，对误差协方差进 行Cholesky分解，则有

P(k-1|k-1)＝S(k-1|k-1)[S(k-1|k-1)]^T (19)

其中P(k|k)表示k时刻滤波误差协方差矩阵，S(k|k)[S(k|k)]^T为Cholesky分解结果。

2)计算容积点

其中是容积点，参考容积准则求解。

3)传播容积点

X^*(i，k|k-1)＝f[X(i，k-1|k-1)] (21)

其中X^*(i，k|k-1)为传播得到的容积点。

4)状态预测

其中m＝2n为容积点总数，n为状态向量的维数，表示由k-1时刻预测k时刻的状态 向量。

5)协方差预测

其中P(k|k-1)表示由k-1时刻预测k时刻的预测误差协方差矩阵，Q(k)表示k时刻的过程 噪声协方差矩阵。

其中，测量更新如下:

1)Cholesky分解

P(k|k-1)＝S(k|k-1)[S(k|k-1)]^T (24)

2)计算容积点

3)传播容积点

Y(i，k|k-1)＝h[X(i，k|k-1)] (26)

4)测量预测

5)估计自相关协方差阵

其中P(yy，k|k-1)为测量估计自相关协方差阵，R(k)表示k时刻的测量噪声协方差矩阵。

6)估计互相关协方差

其中P(xy，k|k-1)为状态与测量互协方差阵。

7)计算Kalman增益

K_k＝P(xy，k|k-1)P^-1(yy，k|k-1) (30)

8)状态更新

其中Y(k)表示k时刻的***测量值。

9)估计协方差

下一步，获得次优无偏噪声统计估计器的递推方法。

Mehra指出自适应滤波方法可以分为四类：贝叶斯方法、极大似然法、相关函数法和方 差匹配法。Sage-Husa自适应滤波算法就是方差匹配法的一种，但是Mehra指出这一类方法 的收敛性本身并没有得到证明，在某些情况下甚至存在数值发散现象。传统的Sage-Husa 自适应滤波算法是通过实时估计和修正***噪声和测量噪声的统计特性，从而降低***模 型误差、抑制滤波发散。

其过程噪声均值估计值过程噪声协方差估计值测量噪声均值和测量噪 声协方差为：

其中F(k)、H(k)分别为状态转移矩阵和状态测量矩阵，d(k)为一个系数，表达式为

d(k)＝(1-b)/(1-b^(k+1)) (37)

其中b为衰减因子，一般取值范围为：0.95～0.99。

传统的Sage-Husa自适应滤波算法只适用于线性卡尔曼滤波，下文将结合MAP原则推导出 适应于CKF的Sage-Husa自适应准则，并对其进行改进。

考虑一个状态噪声w(k)和观测噪声v₀(k)为互不相关的非零均值的高斯白噪声序列的状 态空间模型，该模型比式(15～18)描述的状态空间模型更具有一般性，即噪声均值非零， 其噪声均值表达式为：

E[w(k)]＝q (38)

E[v₀(k)]＝r (39)

设XX(k)、YY(k)分别为所有状态值和测量值的集合， 的贝叶斯估计可以通过使如下的后验概率密度极大化获得：

p[XX(k)，q，Q，r，R|YY(k)] (40)

在状态噪声和测量噪声互不相关，信号平稳分布的前提下，式(40)可以等价为求解如 下非条件概率密度函数的极大值

式(41)中暂且认为其为一个常数，由先验信息获得。下 面通过分别求解p[ZZ(k)|XX(k)，q，Q，r，R]和p[XX(k)|q，Q，r，R]来求解J，则有

式(42)中l表示测量位数，|R|表示R的行列式，为二次型。

式(43)中n为***状态维数，将式(42)，(43)带入式(41)，则有

式(44)中由于J和lnJ拥有相同的极值 点，故可以通过lnJ进行数学变化求得极大后验估计值。则有

令

则可得噪声统计的极大后验估计器为

通过无偏性证明和CKF滤波公式，可得次优无偏噪声统计估计器的递推公式为

式(54～57)即为适用于CKF的Sage-Husa自适应准则，即本发明中提出的次优无偏噪声 统计估计器的递推方法，利用该方法进行滤波，具体如下：

NSHV跟踪认为测量噪声已知且为零均值的白噪声，状态转移噪声未知且为零均值白噪 声，故只需要对状态转移噪声的协方差阵进行估计。所以次优无偏噪声统计估计器表达式 式(54～57)可以简化为式(57)。

式(57)是一系列历史数据的算术平均，对一个***的统计而言，新近数据的作用影响 强一些，陈旧数据影响小一些。故可以采用渐消记忆指数加权的方式来实现对陈旧数据的 逐渐遗忘。令b为衰减因子，取值为0.95<b<0.99，则有

用式(58)代替式(57)中1/k则有

将式(59)将要改进的两项分别记为式(60)和式(61)，即

CKF滤波中式(60)P(k|k)后面的减号可能导致非正定，从而导致滤波发散，这是 含有噪声估计器的CKF滤波中最棘手的问题之一。式(60)可以写为即k时刻的滤波误差协方差阵减去(k-1)时刻的一步预测滤波误差协方差阵再加上(k-1)时刻的过程噪声协方差阵，因为(k-1)时刻的过程噪声协方差阵在式(59)的前半部分已 经有体现，k时刻的滤波误差协方差阵减去(k-1)时刻的一步预测滤波误差协方差阵再加 上(k-1)时刻的过程噪声协方差阵是一个非常小的矩阵，其形式与P(k|k)一致，可以对 P(k|k)乘以一个较小的倍数来近似式(60)。在滤波递推时间间隔较小时，可以利用式(62) 来近似式(60)

T*P(k|k) (62)

式(62)中T为CKF递推时间间隔。

通过分析式(62)与式(60)的差异可知，式(62)的近似方法在状态噪声协方差变小时会对估计偏大，故设定一个与衰减因子b相关的系数来使式(61)偏小一些，则式(61)则可以改为

在CKF滤波过程中，后期估计出来的会更趋近稳定，滤波残差对调整能力变弱。 为了保证在滤波后期滤波残差有突变时能够及时对估计做出调整，考虑到滤波后期状 态协方差阵的变化也会减小，所以要设计一个系数u，借此逐渐增大式(62)的比重，逐渐 减小式(63)的比重。则式(62、63)分别变为

(2-u)^k-1*T*P(k|k) (64)

其中u为一个略大于1的数，使u^k-1在最大时约为u的2倍。本发明中取u＝1.00001。则的估计表达式最终为：

基于上述的用于NSHV跟踪滤波的自适应滤波方法，进行NSHV无横向机动时的跳跃弹道 (桑格尔弹道)跟踪研究，即飞行器的倾侧角v＝0。假设NSHV初始状态速度V＝6000m/s，速 度倾角θ＝-0.1，速度偏角σ＝-π/3，地心距r＝6.378×10⁶+50×10³m，经度λ＝0，纬度φ＝0，攻 角的变化规律为：

其中α_max＝25°表示最大攻角25°，α_k＝10°表示最大升阻比时的攻角，V1＝5000m/s， V2＝3500m/s。则可以根据所建立的动力学方程和地球大气环境模型即可仿真出一条跳跃弹道， 其目标飞行高度仿真曲线如图2所示。

假设雷达测量中心的位置为地心距r＝6.378×10⁶+200m，经度λ＝-π/3，纬度φ＝π/16，以 攻角的过程表达式为α(k+1)＝α(k)，将攻角的过程表达式与式(1)的动力学方程联立形成新 的状态转移方程，则在卡尔曼滤波体系下利用基于雷达的测量方程和基于含攻角的动力学过 程方程即可实现攻角未知时的飞行器跟踪。

通过仿真轨迹可知初始状态时攻角为25°，为了测试算法的鲁棒性和适应能力，在跟踪 初始时刻设置攻角的估计值为10°。设置过程噪声的初始值为Q0＝diag([1^2 0.001^2 0.001^2 10^2 0.0001^2 0.0001^2 0.5^2])，设置测量噪声中R中σ_R、σ_A、σ_E分别为10m、0.5mrad、0.5mrad。仿真时间间隔T为0.1秒，衰减因子b为0.98，递推12000次。

为了比较是否采用自适应滤波方法、采用不同的自适应滤波方法对NSHV的跟踪效果差 异，采用5种过程噪声协方差阵自适应原则如表1所示。

表1

表1中Curve0采用固定的过程噪声协方差Q0进行跟踪滤波。

Curve1采用未改进的Sage-Husa算法进行滤波自适应，即采用式(56)或式(58)进行 过程噪声自适应。

Curve2采用在1000次递推之前Q＝Q0，之后Q＝Q0*0.1的过程噪声协方差Q的变化规律。

Curve3采用本发明提出的自适应滤波方法。

Curve4采用在1000次递推之前采用本发明提出的自适应滤波方法，之后采用式(58) 所示的Sage-Husa自适应方法。

分别进行50次蒙特卡洛仿真，即可得到仿真数据统计表如表2所示。

表2

由于Curve1程序运行中会出现由于状态协方差阵不正定而导致的滤波发散，故没有出现 在表2中。通过分析的表达式可知是由于中的减号在状态变化较剧烈的情况下会导 致非正定从而引起了滤波发散，标准的Sage-Husa算法具有较差的稳定性。通过分析可 知，初始状态下滤波误差较大，Sage-Husa方法会因为误差较大而导致状态协方差阵不正定 引起滤波发散。

为了比较Sage-Husa方法在不发散时与本发明提出的改进算法的滤波精度，设计了 Curve4。

通过对比Curve0、Curve2、Curve4和Curve3曲线可以发现，Curve3曲线速率、速度倾角、 速度偏角、经度和纬度的估计误差均优于其他曲线，Curve3地心距的估计误差与Curve4相 当。综合来看，本发明提出的自适应滤波方法具备上述方法中最优的速度和位置跟踪精度， 即地心距稳定跟踪精度在100m以内，速率稳定跟踪精度在6m/s以内。故本发明提出的方法 在跟踪滤波精度和稳定性上对上述现有的方法均有一定程度的提升。

Curve2比Curve0跟踪速度精度提升幅度较大，跟踪位置精度基本一致，跟踪攻角精度提 升幅度较大，通过分析可知在滤波初始阶段采用较大的Q值可以让在滤波误差较大的情况下 有较强的调整能力，在滤波稳定后采用较小的Q值可以让滤波误差较小时有较高的滤波精 度。Curve2虽然跟踪精度不如Curve3，但是计算量比较少，不需要实时计算过程噪声协方 差Q，在对精度要求不高的情况下采用采用几种固定的噪声协方差阵切换方式来提高精度。

依据表2进行定量分析来看，本发明提出的方法在滤波稳定性上优于Sage-Husa自适应方 法，且速率平均跟踪精度比Sage-Husa自适应方法提升了50％，速率跟踪误差峰值比 Sage-Husa自适应方法减小了43.69％；地心距平均跟踪比Sage-Husa自适应方法提升了6.77％，地心距误差峰值比Sage-Husa自适应方法减小了3.49％。本发明提出的方法比未进行 自适应的CKF滤波方法的速率平均跟踪精度提升了30多倍，地心距平均跟踪精度提升了1倍。 可以看出本发明提出的方法在速度跟踪精度和位置跟踪精度上均优于上述其他滤波方法。

值得注意的是本发明提出的方法对攻角的跟踪精度低于Curve4，但是这不是传统的速度 和位置跟踪精度的范畴，故在跟踪精度比较时未进行涉及。对攻角的跟踪精度提升，有助 于对飞行器轨迹进行更加准确的预测，故此处值得进一步深入研究。

本发明提出了一种新型的CKF自适应滤波方法，该方法用于临近空间高超声速助推-滑翔 飞行器跟踪可以有效提升滤波稳定性和跟踪精度，速率平均跟踪精度比Sage-Husa自适应方 法可以提升50％，速率平均跟踪精度比Sage-Husa自适应方法可以提升6.77％，100m内的地心 距跟踪精度与6m/s内的速度跟踪满足临近空间高超声速滑翔飞行器的防御需求。本发明成 了：

1)基于未知攻角的动力学模型(状态转移方程)和雷达测量模型(测量方程)的临近 空间高超声速助推-滑翔器跟踪框架建立；

2)基于MAP推导了适用于CKF的Sage-Husa自适应方法，并在数学层面对其进行改进；

3)对多种滤波方法进行仿真比较，证明了本文提出自适应方法在滤波稳定性和跟踪精 度上对Sage-Husa自适应方法及文中提及的其他方法均有一定程度的提升。

本文提出的CKF自适应滤波方法应当有一定的普适性，本文仅证明了其应用于NSHV跟踪 的稳定性和精度方面的优势，下阶段将通过进一步研究探索本文提出的自适应滤波方法在 其他类型的卡尔曼滤波中的跟踪效果，探索研究该方法在其他领域内的应用效果。

参照图3，本发明还提出的用于NSHV跟踪滤波的自适应滤波***，其包括状态转移方程 建立单元100、测量方程建立单元200、贝叶斯估计获得单元300、极大后验估计值和噪声统 计的极大后验估计器获得单元400、递推单元500、以及滤波单元600。

其中，状态转移方程建立单元100用于对NSHV跟踪建立离散化的状态转移方程；测量方 程建立单元200用于对NSHV跟踪建立离散化的测量方程；贝叶斯估计获得单元300用于获得 状态值、测量噪声均值、测量噪声协方差、过程噪声协方差估计值、以及过程噪声均值估 计值的贝叶斯估计；极大后验估计值和噪声统计的极大后验估计器获得单元400用于获得极 大后验估计值和噪声统计的极大后验估计器；递推单元500用于依据所述离散化的状态转移 方程、所述离散化的测量方程、所述状态值、测量噪声均值、测量噪声协方差、过程噪声 协方差估计值、以及过程噪声均值估计值的贝叶斯估计、以及所述极大后验估计值和噪声 统计的极大后验估计器，获得次优无偏噪声统计估计器的递推方法；滤波单元600用于依据 所述次优无偏噪声统计估计器的递推方法进行滤波。

其中，与用于NSHV跟踪滤波的自适应滤波方法相同的部分，此处不再赘述。

以上所述的具体实施例，对本发明的目的、技术方案和有益效果进行了进一步的详细说 明，应当理解，以上所述仅为本发明的具体实施例而已，并不用于限定本发明的保护范围。 特别指出，对于本领域技术人员来说，凡在本发明的精神和原则之内，所做的任何修改、 等同替换、改进等，均应包含在本发明的保护范围之内。

Claims (7)

1.一种用于NSHV跟踪滤波的自适应滤波方法，其特征在于，包括以下步骤：

对NSHV跟踪建立离散化的状态转移方程；

对NSHV跟踪建立离散化的测量方程；

获得状态值、测量噪声均值、测量噪声协方差、过程噪声协方差估计值、以及过程噪声均值估计值的贝叶斯估计；

获得极大后验估计值和噪声统计的极大后验估计器；

依据所述离散化的状态转移方程、所述离散化的测量方程、所述状态值、测量噪声均值、测量噪声协方差、过程噪声协方差估计值、以及过程噪声均值估计值的贝叶斯估计、以及所述极大后验估计值和噪声统计的极大后验估计器，获得次优无偏噪声统计估计器的递推方法；

依据所述次优无偏噪声统计估计器的递推方法进行滤波。

2.根据权利要求1所述的用于NSHV跟踪滤波的自适应滤波方法，其特征在于，所述离散化的状态转移方程为X(k+1)＝f(X(k))+w(k)；所述离散化的测量方程为Y(k)＝h(X(k))+v₀(k)；

其中，x(k)表示k时刻的状态，y(k)表示k时刻的测量值，w(k)、v₀(k)分别为零均值白噪声，f(x)为非线性的状态转移方程，h(x)为非线性的测量方程。

3.根据权利要求1所述的用于NSHV跟踪滤波的自适应滤波方法，其特征在于，对NSHV跟踪建立离散化的状态转移方程包括以下步骤：

Cholesky分解步骤：若k-1时刻后验概率密度函数已知，则通过式子P(k-1|k-1)＝S(k-1|k-1)[S(k-1|k-1)]^T对误差协方差进行Cholesky分解；其中，P(k|k)表示k时刻滤波误差协方差矩阵，S(k|k)[S(k|k)]^T为Cholesky分解结果；

计算容积点步骤：通过式子计算容积点；其中，ξ_i是容积点；

传播容积点步骤：通过式子X^*(i,k|k-1)＝f[X(i,k-1|k-1)]传播容积点；其中，X^*(i,k|k-1)为传播得到的容积点；

状态预测步骤：通过式子进行状态预测；其中，m＝2n为容积点总数,n为状态向量的维数，表示由k-1时刻预测k时刻的状态向量；

协方差预测步骤：通过式子进行协方差预测；其中，P(k|k-1)表示由k-1时刻预测k时刻的预测误差协方差矩阵，Q(k)表示k时刻的过程噪声协方差矩阵。

4.根据权利要求1所述的用于NSHV跟踪滤波的自适应滤波方法，其特征在于，对NSHV跟踪建立离散化的测量方程包括以下步骤：

Cholesky分解步骤：通过式子P(k|k-1)＝S(k|k-1)[S(k|k-1)]^T对误差协方差进行Cholesky分解；其中，P(k|k)表示k时刻滤波误差协方差矩阵，S(k|k)[S(k|k)]^T为Cholesky分解结果；

计算容积点步骤：通过式子计算容积点；其中，ξ_i是容积点；

传播容积点步骤：通过式子Y(i,k|k-1)＝h[X(i,k|k-1)]传播容积点；其中，X^*(i,k|k-1)为传播得到的容积点；

测量预测步骤：通过式子进行状态预测；其中，m＝2n为容积点总数,n为测量向量的维数，表示由k-1时刻预测k时刻的测量向量；

估计自相关协方差阵步骤：通过式子估计自相关协方差阵；其中，P(yy,k|k-1)为测量估计自相关协方差阵，R(k)表示k时刻的测量噪声协方差矩阵；

估计互相关协方差步骤：通过式子估计互相关协方差；其中，P(xy,k|k-1)为状态与测量互协方差阵；

计算Kalman增益步骤：通过式子K_k＝P(xy,k|k-1)P^-1(yy,k|k-1)计算Kalman增益；

状态更新步骤：通过式子进行状态更新；其中，Y(k)表示k时刻的***测量值；

估计协方差步骤：通过式子估计协方差。

5.根据权利要求1所述的用于NSHV跟踪滤波的自适应滤波方法，其特征在于，通过使式子p[XX(k),q,Q,r,R|YY(k)]的后验概率密度极大化，获得所述状态值、测量噪声均值、测量噪声协方差、过程噪声协方差估计值、以及过程噪声均值估计值的贝叶斯估计。

6.根据权利要求1所述的用于NSHV跟踪滤波的自适应滤波方法，其特征在于，通过获得式子的非条件概率密度函数的极大值，获得所述状态值、测量噪声均值、测量噪声协方差、过程噪声协方差估计值、以及过程噪声均值估计值的贝叶斯估计。

7.一种用于NSHV跟踪滤波的自适应滤波***，其特征在于，包括状态转移方程建立单元、测量方程建立单元、贝叶斯估计获得单元、极大后验估计值和噪声统计的极大后验估计器获得单元、递推单元、以及滤波单元；

所述状态转移方程建立单元用于对NSHV跟踪建立离散化的状态转移方程；

所述测量方程建立单元用于对NSHV跟踪建立离散化的测量方程；

所述贝叶斯估计获得单元用于获得状态值、测量噪声均值、测量噪声协方差、过程噪声协方差估计值、以及过程噪声均值估计值的贝叶斯估计；

所述极大后验估计值和噪声统计的极大后验估计器获得单元用于获得极大后验估计值和噪声统计的极大后验估计器；

所述递推单元用于依据所述离散化的状态转移方程、所述离散化的测量方程、所述状态值、测量噪声均值、测量噪声协方差、过程噪声协方差估计值、以及过程噪声均值估计值的贝叶斯估计、以及所述极大后验估计值和噪声统计的极大后验估计器，获得次优无偏噪声统计估计器的递推方法；

所述滤波单元用于依据所述次优无偏噪声统计估计器的递推方法进行滤波。