一种矢量形式信息分配系数的自适应联邦滤波方法

摘要

本发明公开了一种矢量形式信息分配系数的自适应联邦滤波方法，该方法在分析GPS、天文（CNS）和SAR三种导航传感器的工作环境和工作特性基础上，建立地理系下惯性导航系统与各导航传感器的线性化量测方程，构建滤波子系统；然后分别对滤波子系统进行卡尔曼滤波，结合子系统估计误差协方差阵特征值和子系统可观测矩阵奇异值，设计完成了一种矢量信息分配系数计算方案，针对矢量分配系数会导致协方差阵不对称问题，设计了新的信息分配方程；最后采用联邦滤波方法对惯性导航的误差状态进行了最优估计。本发明导航精度高，能充分发挥多传感器组合导航系统对动态环境下惯性导航系统误差状态量的估计作用。

说明书

技术领域

发明涉及一种矢量形式信息分配系数的自适应联邦滤波方法，属于飞行器组合导航技术 领域，可应用于高空长时间飞行的航空飞行器导航参数的确定，用于提高导航系统的导航精 度。

背景技术

随着航空航天技术的发展，对导航与制导系统的性能要求越来越高，组合导航系统已成 为当前重要的导航系统方案。在导航多传感器信息融合领域，分散化滤波技术日益受到人们 重视，Carlson提出的分散化联邦滤波器得到越来越广泛的应用。信息分配是联邦滤波器研究 和设计中的关键，直接影响联邦滤波器的精度和容错性等性能。

Carlson最先提出的信息分配原则是固定比例的，通常根据正常工作子系统的个数进行固 定平均分配。考虑到在实际的高动态导航环境中，各子滤波器的性能和估计质量都是不断变 化的，为进一步提高联邦滤波器的性能，国内外学者提出了多种动态的信息分配方法，主要 包括基于估计协方差特征值法、基于协方差阵Fronenius范数方法、基于可观测矩阵条件数法 等，这些动态信息分配方法从一定程度上实现了对系统状态的实时跟踪，滤波效果较固定比 例的分配方法有所改善。

经分析不难发现，以上研究的信息分配系数均为标量形式。即把状态变量作为一个整体 考虑来进行信息反馈，其实质是把状态变量的每个分量的特性看作是相同的，具有完全相同 的估计精度和收敛速度。但这种观点是不合理的，根据实际情况知，每个传感器功能特性和 精度的不同使得每个局部滤波器建立的状态方程和量测方程是不同的，而状态方程的特性又 决定了状态的每个分量具有不同的可观测度和收敛速度，在基于惯性导航的组合模式中，平 台的方位误差角收敛速度明显低于其它的状态分量。再者，不同的传感器其输出特性也是不 同的，如SAR图像匹配仅能输出经度、纬度信息，大气数据系统仅能提供高度及地向速度信 息，从而导致不同的状态分量具有不同的估计精度。

因此，现有标量形式信息分配系数的联邦滤波方法难以具体实时跟踪反映每个状态变量 的变化特性，导航精度不高，不能充分满足动态长航时飞行器对导航精度的要求。

发明内容

本发明所要解决的技术问题是克服标量形式信息分配系数的联邦滤波不能充分具体反映 每个状态量变化特性的缺陷，提供一种矢量形式信息分配系数的自适应联邦滤波方法。

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

本发明基于系统误差协方差阵特征值和系统可观测矩阵奇异值的矢量形式信息分配系数 的联邦滤波方法，包括以下步骤：

(1)通过建立惯性导航系统INS的误差状态量方程，得到对惯性导航系统误差状态量的数 学描述，惯性导航系统误差状态量X定义为：

$X={[{\phi }_N,{\phi }_E,{\phi }_D,{\mathrm{\delta v}}_N,{\mathrm{\delta v}}_E,{\mathrm{\delta v}}_D,\mathrm{\delta L},\mathrm{\delta \lambda },\mathrm{\delta h},{ϵ}_{\mathrm{bx}},{ϵ}_{\mathrm{by}},{ϵ}_{\mathrm{bz}},{ϵ}_{\mathrm{rx}},{ϵ}_{\mathrm{ry}},{ϵ}_{\mathrm{rz}},{▿}_x,{▿}_y,{▿}_z]}^T,$

其中：φ_N，φ_E，φ_D分别表示惯性导航系统误差状态量中的北向平台误差角状态量、东向平 台误差角状态量和地向平台误差角状态量；δv_N，δv_E，δv_D分别表示惯性导航系统误差状态量中 的北向速度误差状态量、东向速度误差状态量和地向速度误差状态量；δL，δλ，δh分别表示惯性 导航系统误差状态量中的纬度误差状态量、经度误差状态量和高度误差状态量；ε_bx，ε_by，ε_bz分 别表示惯性导航系统误差状态量中的X轴、Y轴、Z轴方向陀螺常值漂移误差状态量； ε_rx，ε_ry，ε_rz分别表示惯性导航系统误差状态量中的X轴、Y轴、Z轴方向陀螺一阶马尔可夫漂 移误差状态量；分别表示惯性导航系统误差状态量中的X轴、Y轴和Z轴方向加速 度计零偏，上标T为转置；

(2)建立地理系下各子系统的量测方程，包括GPS/INS量测方程、CNS/INS量测方程和 SAR/INS量测方程；

(3)将步骤(2)所述的各子系统的量测方程中子系统误差状态量进行KF滤波，将子系统 KF滤波结果送入联邦滤波器；

(4)根据各导航子系统的协方差矩阵以及可观测性矩阵，求取一种矢量形式的联邦滤波信 息分配系数，使每个子系统的每个状态变量得到不同的信息分配系数，根据该矢量形式的联 邦滤波信息分配系数，建立系统的过程信息在各子滤波器之间的分配原则；

(5)联邦滤波器对步骤(3)中子系统送来的滤波结果进行数据融合，输出全局最优估计 值，从而对惯性导航系统的导航误差进行修正。

本发明采用以上技术方案与现有技术相比，具有以下技术效果：

本发明克服了现有技术无法实时具体反映状态变量特性的不足，构建了一种适用于动态 长时间飞行航空飞行器的组合导航系统联邦滤波方法，它具有以下优点：

(1)根据每个滤波子系统每个状态变量的估计精度分别自适应进行分配系数调整；

(2)根据每个滤波子系统每个状态变量的可观测性分别自适应进行分配系数调整；

(3)根据矢量形式信息分配系数建立的过程信息在各子滤波器之间的分配原则，确保系 统的误差协方差阵为对称阵，从而有效改善联邦滤波器的稳定性和精度。

附图说明

图1为本发明的矢量信息分配系数联邦滤波的一种实施例子的流程图；

图2为仿真的一条飞行航迹；

图3为本发明的导航经度误差与传统滤波导航经度误差的仿真对比图；

图4为本发明的导航纬度误差与传统滤波导航纬度误差的仿真对比图；

图5为本发明的导航高度误差与传统滤波导航高度误差的仿真对比图。

具体实施方式

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

如图1所示，本发明的原理是：从地理系导航的角度入手，依据系统状态方程和各子系 统的线性化量测方程，构成滤波子系统，从各滤波子系统中提取估计协方差矩阵P_i和可观测 性矩阵Q_i，综合两者信息求取矢量形式的信息分配系数B_i，从而进一步完成信息分配、最优 融合等步骤，实现对组合导航误差状态量的最优估计。具体实施方法如下：

一、建立惯性导航系统的误差状态量方程

选择导航坐标系为北东地地理水平坐标系(O_nX_nY_nZ_n)，采用线性卡尔曼滤波器进行组合， 系统的状态方程为惯性导航系统的误差状态量方程，通过对惯性导航系统的性能及误差源的 分析，可以获得惯导系统的误差状态量方程为：

$\stackrel{·}{X}\left(t\right)=F\left(t\right)X\left(t\right)+G\left(t\right)W\left(t\right)---\left(1\right)$

式中，$X={[{\phi }_N,{\phi }_E,{\phi }_D,{\mathrm{\delta v}}_N,{\mathrm{\delta v}}_E,{\mathrm{\delta v}}_D,\mathrm{\delta L},\mathrm{\delta \lambda },\mathrm{\delta h},{ϵ}_{\mathrm{bx}},{ϵ}_{\mathrm{by}},{ϵ}_{\mathrm{bz}},{ϵ}_{\mathrm{rx}},{ϵ}_{\mathrm{ry}},{ϵ}_{\mathrm{rz}},{▿}_x,{▿}_y,{▿}_z]}^T$

其中φ_N，φ_E，φ_D为平台误差角；δv_N，δv_E，δv_D为速度误差；δL，δλ，δh为纬度、经度和高度误差； ε_bx，ε_by，ε_bz，ε_rx，ε_ry，ε_rz分别为陀螺常值漂移误差和一阶马尔可夫漂移误差；为加速度 计零偏，上标T为转置。

二、建立地理系下各子系统的量测方程

①GPS/INS量测方程

$Z_{\mathrm{GPS}}\left(t\right)=\left(\begin{array}{c}{v}_{\mathrm{nINS}}-v_{\mathrm{nGPS}}\\ v_{\mathrm{eINS}}-v_{\mathrm{eGPS}}\\ v_{\mathrm{dINS}}-v_{\mathrm{dGPS}}\\ (L_{\mathrm{INS}}-L_{\mathrm{GPS}})R_n\\ ({\lambda }_{\mathrm{INS}}-{\lambda }_{\mathrm{GPS}})R_e\cos L\\ h_{\mathrm{INS}}-h_{\mathrm{GPS}}\end{array}\right)=\left(\begin{array}{cccc}{0}_{3\times 3}& \mathrm{diag}\left(\begin{array}{ccc}1& 1& 1\end{array}\right)& 0_{3\times 3}& 0_{3\times 9}\\ 0_{3\times 3}& 0_{3\times 3}& \mathrm{diag}\left(\begin{array}{cccc}{R}_n& R_e& \cos L& 1\end{array}\right)& 0_{3\times 9}\end{array}\right)X\left(t\right)+\left(\begin{array}{c}{M}_{\mathrm{nGPS}}\\ M_{\mathrm{eGPS}}\\ M_{\mathrm{dGPS}}\\ M_{\mathrm{nGPS}}\\ N_{\mathrm{eGPS}}\\ N_{\mathrm{dGPS}}\end{array}\right)---\left(2\right)$

式中，v_nINS v_eINS v_dINS表示为惯性导航系统的北、东、地方向速度，v_nGPS，v_eGPS，v_dGPS表示 为GPS接收机的北、东、地方向速度，L_INS，λ_INS，h_INS表示为惯性导航系统的纬度、经度、高 度测量值，L_GPS，λ_GPS，h_GPS表示为GPS接收机的纬度、经度、高度测量值，R_n，R_e分别表示北 向、东向地球曲率半径，L表示航迹纬度值，M_nGPS、M_eGPS、M_dGPS表示为GPS接收机的北、 东、地方向的测速误差，N_nGPS，N_eGPS，N_dGPS为GPS接收机沿北、东、地方向的位置测量 误差，均考虑为白噪声。

②CNS/INS量测方程

$Z_{\mathrm{GNS}}\left(t\right)=\left(\begin{array}{c}{\gamma }_{\mathrm{rINS}}-{\gamma }_{\mathrm{rCNS}}\\ {\theta }_{\mathrm{pINS}}-{\theta }_{\mathrm{pCNS}}\\ {\psi }_{\mathrm{hINS}}-{\psi }_{\mathrm{hCNS}}\end{array}\right)={\left(\begin{array}{cccc}{A}_{3\times 3}& 0_{3\times 3}& 0_{3\times 3}& 0_{3\times 9}\end{array}\right)}_{3\times 18}X\left(t\right)+\left(\begin{array}{c}{O}_{\mathrm{rCNS}}\\ O_{\mathrm{pCNS}}\\ O_{\mathrm{hCNS}}\end{array}\right)---\left(3\right)$

其中$A_{3\times 3}=\frac{-1}{\cos \theta }\left(\begin{array}{ccc}\cos \psi & \sin \psi & 0\\ -\sin \psi \cos \theta & \cos \psi \cos \theta & 0\\ \cos \psi \sin \theta & \sin \psi \sin \theta & \cos \theta \end{array}\right),$θ、ψ表示为飞行器的真实俯仰角和 航向角。式中γ_rINS，θ_pINS，ψ_hINS分别表示惯性导航系统的横滚、俯仰、航向角，γ_rCNS，θ_pCNS，ψ_hCNS分别表示天文星敏感器的横滚、俯仰、航向角，O_rCNS、O_pCNS、O_hCNS表示为天文星敏感器对 横滚角、俯仰角与航向角在地理系下的等效测量误差，均考虑为白噪声。

③SAR/INS量测方程

$Z_{\mathrm{SAR}}\left(t\right)=\left(\begin{array}{c}(L_{\mathrm{INS}}-L_{\mathrm{SAR}})R_n\\ ({\lambda }_{\mathrm{INS}}-{\lambda }_{\mathrm{SAR}})R_e\cos L\end{array}\right)={\left(\begin{array}{cccc}{0}_{2\times 3}& 0_{2\times 3}& B_{2\times 3}& 0_{2\times 9}\end{array}\right)}_{3\times 18}X\left(t\right)+\left(\begin{array}{c}{N}_{\mathrm{nSAR}}\\ N_{\mathrm{eSAR}}\end{array}\right)---\left(4\right)$

其中$B_{2\times 3}=\left(\begin{array}{ccc}{R}_n& 0& 0\\ 0& R_e\cos L& 0\end{array}\right),$式(4)中L_INS，λ_INS分别表示惯性导航系统的纬度、经度测 量值，L_SAR，λ_SAR分别表示SAR景象匹配导航的纬度、经度测量值，N_nSAR，N_eSAR为SAR景象 匹配导航沿北、东方向的位置测量误差。

三、子系统KF(Kalman Filter)滤波，估计子系统的误差状态量

状态方程和量测方程的离散化及卡尔曼滤波器

当采用线性卡尔曼滤波器时，需要对上面连续形式的系统状态方程(1)和量测方程(2)、 (3)、(4)进行离散化，从而获得离散形式的系统方程。其离散化形式如下：

$\left(\begin{array}{c}{X}_k={\Phi }_{k,k-1}{X}_{k-1}+{\Gamma }_{k-1}{W}_{k-1}\\ Z_k=H_k{X}_k+V_k\end{array}\right)$

式中${\Phi }_{k,k-1}=\underset{m=0}{\overset{\infty }{\Sigma }}{\left[F\left(t_k\right)T\right]}^m/m!,$${\Gamma }_{k-1}=\left\{\underset{m=1}{\overset{\infty }{\Sigma }}\right[\frac{1}{m!}{\left(F\left(t_k\right)T\right)}^{m-1}\left]\right\}G\left(t_k\right)T_d,$T_d为迭代周期。

从而可以获得系统的线性化卡尔曼滤波器方程如下：

${\hat{X}}_{k|k-1}={\Phi }_{k,k-1}{\hat{X}}_{k-1}$

${\hat{X}}_k={\hat{X}}_{k|k-1}+K_k\left(\begin{array}{ccc}{Z}_k& -H_k& {\hat{X}}_{k|k-1}\end{array}\right)$

$P_{k|k-1}={\Phi }_{k,k-1}{P}_{k-1}{{\Phi }_{k,k-1}}^T+{\Gamma }_{k-1}{Q}_{k-1}{{\Gamma }_{k-1}}^T$

$K_k=P_{k|k-1}{H}_k^T{(H_k{P}_{k|k-1}{H}_k^T+R_k)}^{-1}$

$P_k=(I-K_k{H}_k)P_{k|k-1}{(I-K_k{H}_k)}^T+K_k{R}_k{K}_k^T$

上式中，表示k-1时刻的状态对k时刻的状态的最优估计值，又称一步预测估值，Φ_k，k-1表示k-1时刻到k时刻的状态转移矩阵，表示k-1时刻的系统状态估计值，表示k时刻 的系统状态估计值，K_k表示增益矩阵，Z_k表示k时刻的观测矢量，H_k表示k时刻的观测系数 矩阵，P_k|k-1表示最优预测估计误差协方差阵，P_k-1表示k-1时刻的系统误差协方差阵，Q_k-1表 示k-1时刻的噪声方差矩阵，Γ_k-1表示k时刻的噪声矢量对k+1时刻状态矢量影响的噪声系数 矩阵，R_k表示k时刻的量测方差矩阵，P_k表示k时刻的系统误差协方差阵，I为单位矩阵。

四、矢量形式信息分配系数求取及信息分配

基于子系统协方差矩阵特征值的矢量系数A_i的求解

子系统协方差矩阵P_i按特征值分解为：

$P_i=L_i{\Lambda }_i{L}_i^T$

式中，Λ_i＝diag{λ_i1，λ_i2，Λ，λ_in}，λ_i1，λ_i2，L，λ_in为P_i的特征值，n为P_i阵的阶数。

对X_i的每一个分量x_ij(表示第i个局部滤波器状态估计中的第j个分量)独立进行信息 分配系数计算，信息分配系数为：

${\alpha }_{\mathrm{ij}}=\frac{1/{\lambda }_{\mathrm{ij}}}{1/{\lambda }_{1j}+1/{\lambda }_{2j}+L+1/{\lambda }_{\mathrm{Nj}}}---\left(6\right)$

i＝1，2，L，N；j＝1，2，L，n

式中，λ_ij为状态变量x_ij对应的特征值，N为子系统个数。

X_i对应的信息分配系数为矩阵形式：

$A_i=\left(\begin{array}{cccc}{\alpha }_{i1}& & & \\ & {\alpha }_{i2}& & \\ & & O& \\ & & & {\alpha }_{\mathrm{in}}\end{array}\right)=\mathrm{diag}\left(\begin{array}{cccc}{\alpha }_{i1}& {\alpha }_{i2}& L& {\alpha }_{\mathrm{in}}\end{array}\right)---\left(7\right)$

基于子系统可观测矩阵奇异值的矢量系数Υ_i的求解。

设某时间段动态系统的可观测性矩阵为Q(Q∈R^p×q)，对Q进行奇异值分解，得

Q＝USV^T                  (8)

式中：U＝[u₁ u₂ L u_p]，V＝[v₁ v₂ L v_q]都是正交矩阵；$S=\left(\begin{array}{cc}{\Lambda }_{r\times r}& 0\\ 0& 0\end{array}\right),$

Λ_r×r＝diag(σ₁，σ₂，L，σ_r)，其中r，σ_i(i＝1，2，L，r)分别为矩阵Q的秩和奇异值。对矩阵V进行 分析，得到σ_i为与其对应的右奇异向量V中取得最大绝对值的状态变量所对应的奇异值。若 σ_i的值较大，则相应的系统状态变量具有较好的观测性，可以获得较高精度的估计；若σ_i的 值较小，则相应的系统状态变量可能会出现奇异，落入不可观测区间。

对每个子系统的可观测性矩阵分别进行奇异值分解，进一步对Xi的每一个分量x_ij(表示 第i个局部滤波器状态估计中的第j个分量)独立进行信息分配系数计算，信息分配系数为：

${\gamma }_{\mathrm{ij}}=\left(\begin{array}{cc}\frac{{\sigma }_{\mathrm{ij}}}{{\sigma }_{1j}+{\sigma }_{2j}+L{\sigma }_{3j}}& {\sigma }_{\mathrm{ij}}\ne 0\\ 0& {\sigma }_{\mathrm{ij}}=0\end{array}\right)---\left(9\right)$

i＝1，2，L，N；j＝1，2，L，n

式中，σ_ij为状态变量x_ij对应的奇异值，N为子系统个数。

则X_i对应的信息分配系数为矩阵形式：

融合子系统协方差矩阵特征值和可观测矩阵奇异值的矢量系数Υ_i的求解

即

$\left(\begin{array}{c}{B}_i=\left(\begin{array}{cccc}{\beta }_{i1}& & & \\ & {\beta }_{i2}& & \\ & & O& \\ & & & {\beta }_{\mathrm{in}}\end{array}\right)=\mathrm{diag}\left(\begin{array}{cccc}{\beta }_{i1}& {\beta }_{i2}& L& {\beta }_{\mathrm{in}}\end{array}\right)\\ {\beta }_{\mathrm{ij}}=\frac{1}{2}\times ({\alpha }_{\mathrm{ij}}+{\gamma }_{\mathrm{ij}})\end{array}\right)---\left(12\right)$

信息分配系数满足信息守恒原理：

$\underset{i=1}{\overset{N}{\Sigma }}{B}_i=I_{18\times 18}---\left(13\right)$

主滤波器信息分配：

根据矢量形式信息分配系数将系统的过程信息在各子滤波器之间的进行分配，表达式如 下：

$\left(\begin{array}{c}{P}_i^{-1}\left(k\right)=\sqrt{B_i}{P}_g^{-1}\left(k\right)\sqrt{B_i}\\ Q_i^{-1}\left(k\right)=\sqrt{B_i}{Q}_g^{-1}\left(k\right)\sqrt{B_{\mathrm{iQ}}}(i=\mathrm{1,2},L,N)\\ {\hat{X}}_i={\hat{X}}_g\left(k\right)\end{array}\right)---\left(14\right)$

五、联邦主滤波器信息融合

将各子滤波器估计信息进行融合，得到全局最优估计。

融合算法为：

$\left(\begin{array}{c}{P}_g={\left(\underset{i=1}{\overset{n}{\Sigma }}{P}_i^{-1}\right)}^{-1}\\ {\hat{X}}_g=P_g\left(\underset{i=1}{\overset{n}{\Sigma }}{P}_i^{-1}{\hat{X}}_i\right)\end{array}\right)---\left(15\right)$

图2为仿真的一条飞行航迹，包括滑跑、加速、上升、滚转、转弯等机动方式。

经过仿真得到，对相同的状态变量，不同的子系统获得了不同的信息分配系数；对同一 个子系统，不同的状态变量也得到了不同的信息分配系数；分配系数曲线均随时间动态变化。 该种矢量信息分配方案能够实时具体反映每个状态量的变化特性。

图3～图5的仿真结果表明，该方法能够根据导航子系统中各个状态变量的精度特性和 可观测特性进行实时自适应调节，滤波精度高，对飞行环境变化有更强的适应性。