CN103697894A - 基于滤波器方差阵修正的多源信息非等间隔联邦滤波方法 - Google Patents

Abstract

本发明公开了一种基于滤波器方差阵修正的多源信息非等间隔联邦滤波方法，具体步骤为：首先建立航空机载惯性导航系统与其他导航系统的融合无重置模式联邦滤波器，随后利用基于子滤波器方差阵修正的多源信息非等间隔联邦滤波方法，提高子滤波器滤波精度。以高空长航无人机为应用对象，结合实际传感器使用特点，设计了适用于高空长航无人机的INS/CNS/SAR/TER组合非等间隔联邦滤波实现方案。本发明能够在几乎不增加额外计算负担的情况下，通过修正子滤波器滤波方差阵来改善滤波器的融合精度，在保持无重置结构容错性强特性的同时，解决无重置联邦滤波器在应用于多源信息非等间隔滤波时的适应性问题，进一步提升系统导航精度，增强组合导航系统的鲁棒性。

Description

基于滤波器方差阵修正的多源信息非等间隔联邦滤波方法

技术领域

本发明公开了一种基于滤波器方差阵修正的多源信息非等间隔联邦滤波方法，涉及飞行器组合导航技术领域。

背景技术

无人机技术以其独特的不受人的生理条件限制、空勤保障简单及零伤亡率优势成为各国航空领域的研究热点。随着高空长航无人机技术的发展，对导航定位的精度和可靠性都提出了更高的要求，单一导航系统已难以满足要求。以美国等军事强国的典型高空长航无人机为例，主要以惯性导航系统和卫星导航系统相结合，另外根据执行任务外加其它导航系统电子吊舱。

现代战争电磁环境恶劣，武器系统会因受到电磁压制而效能大大削弱，卫星导航系统与惯性导航系统的相结合虽然可以弥补各自的不足，但是在受到强电磁压制的情况下，卫星导航系统容易受到干扰，将无法使用。除卫星导航系统外，现有可以应用于高空长航无人机的其他自主导航手段主要有惯性、地形/景象匹配辅助、天文导航系统等。考虑到战时存在失去卫星导航信息的威胁，为此研究不依赖于卫星导航的无人机自主导航实现方法就尤为重要。把两种或多种导航系统组合起来，应用最优估计理论形成最优组合导航系统，有利于充分运用各导航系统的信息进行信息互补和信息融合，使组合后的导航系统在精度和可靠性方面都有所提高，这已成为无人机导航定位技术的发展方向。

由于多源组合导航中引入了多个导航设备，随之会引入各导航子滤波系统数据更新频率不一致所导致的滤波器量测信息非等间隔处理的问题。国内外学者提出改进措施包括在有重置结构下，证明其非等间隔算法与集中卡尔曼算法等价；子滤波器同时采样并以相同的采样率对目标进行观测；外推内插将各子滤波器时刻推到主滤波器融合时间等各种方法，但限制过多限制实际应用。

发明内容

本发明所要解决的技术问题是：针对现有技术的缺陷，提供一种基于滤波器方差阵修正的多源信息非等间隔联邦滤波方法。

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

所述基于滤波器方差阵修正的多源信息非等间隔联邦滤波方法包括以下步

骤：

步骤一、选取东北天地理坐标系，惯性导航系统误差状态量定义为：

X＝[φ_E,φ_N,φ_U,δv_E,δv_N,δv_U,δL,δλ,δh,ε_bx,ε_by,ε_bz,ε_rx,ε_ry,ε_rz,▽_x,▽_y,▽_z]^T    （1）

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

步骤二、采用地理坐标系下位置、速度、姿态线性化观测原理，根据子滤波系统的工作特性，建立地理坐标系下子滤波系统的量测方程，所述子滤波系统具体包括INS/CNS姿态量测系统、INS/SAR图像匹配水平位置量测系统、INS/TER地形匹配水平位置量测系统：

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

其中，X(t)为步骤一中

X＝[φ_E,φ_N,φ_U,δv_E,δv_N,δv_U,δL,δλ,δh,ε_bx,ε_by,ε_bz,ε_rx,ε_ry,ε_rz,▽_x,▽_y,▽_z]^T加入时间参数t的表达形式，

为X(t)的微分形式，F(t)表示INS/CNS/SAR/TER组合导航系统状态方程的一步状态转移矩阵；G(t)表示INS/CNS/SAR/TER组合导航系统状态方程的系统白噪声误差矩阵；W(t)为惯性/卫星组合导航系统状态方程的系统误差白噪声矢量；

步骤三、将各子滤波系统量测方程中的子滤波系统误差状态量进行KF滤波，计算子滤波器的滤波估计均方差阵；增加修正方差阵ΔP_i，调整最终信息融合时的冗余子滤波系统信息的权重分配，具体如下：

在离散系统的卡尔曼滤波误差分析中，

${\tilde{x}}_{i\left(k\right)}^P\stackrel{\mathrm{def}}{=}{x}_{i\left(k\right)}^r-{\hat{x}}_{i\left(k\right)}---\left(3\right)$

$P_{i\left(k\right)}^P\stackrel{\mathrm{def}}{=}E\left[{\tilde{x}}_{i\left(k\right)}^P{\tilde{x}}_{i\left(k\right)}^{\mathrm{PT}}\right]---\left(4\right)$

其中，

分别为第i个子滤波器的第k时刻状态真实量和滤波估计量，

为理想状态值与滤波估计值的差值，

为子系统理想估计均方差阵，E[.]为取均值运算，T为转置运算；

在子滤波器的滤波估计均方差阵的基础上增加修正方差阵ΔP_i后，得到经过修正的系统滤波器输出的估计均方差阵Pi(k)：

P_i(k)＝P_i(k)+ΔP_i           （5）

最终得到改进后的主滤波器信息融合算法为：

$\left\{\begin{array}{c}{P}_{i\left(k\right)}=P_{i\left(k\right)}+{\mathrm{\ΔP}}_i\\ P_{f\left(k\right)}^{-1}=\underset{i=1}{\overset{n}{\mathrm{\Σ}}}{P}_{i(k/k)}^{-1}\\ {\hat{x}}_{f\left(k\right)}=P_{f\left(k\right)}\left(\underset{i=1}{\overset{n}{\mathrm{\Σ}}}{P}_{i(k/k)}^{-1}{\hat{x}}_{i(k/k)}\right)\end{array}\right.---\left(6\right)$

其中，

P_f(k)分别为主滤波器滤波输出的滤波估计量和误差方差阵；

为主滤波器滤波输出的误差方差阵P_f(k)的逆，

为第i个子滤波器k时刻的估计均方差阵P_i(k)的逆，

为第i个子滤波器k时刻滤波估计量；

步骤四、采用无重置联邦滤波器非等间隔修正算法，将一个滤波周期内的卡尔曼滤波分为两个信息更新过程：时间更新和量测更新；

各子滤波器在信息输出时，若未到量测时间则输出时间更新信息，若到量测时间时则输出量测更新信息；

步骤五、采用联邦滤波器对步骤三中子滤波系统得出的滤波结果进行数据融合，输出全局最优估计值。

作为本发明的进一步优选方案，所述子滤波系统测量方程具体如下：（1）INS/CNS姿态量测方程：

${Z\left(t\right)}_{\mathrm{CNS}}\begin{array}{c}=\left[\begin{array}{c}{\mathrm{\γ}}_{rINS}-{\mathrm{\γ}}_{\mathrm{rCNS}}\\ {\mathrm{\θ}}_{\mathrm{pINS}}-{\mathrm{\θ}}_{\mathrm{pCNS}}\\ {\mathrm{\ψ}}_{\mathrm{hINS}}-{\mathrm{\ψ}}_{\mathrm{hCNS}}\end{array}\right]=\left[\begin{array}{c}{\mathrm{\δ\γ}}_r+O_{\mathrm{rCNS}}\\ {\mathrm{\δ\θ}}_p+O_{\mathrm{pCNS}}\\ {\mathrm{\δ\ψ}}_h+O_{\mathrm{hCNS}}\end{array}\right]\\ =H_a\left(t\right)X\left(t\right)+N_{\mathrm{CNS}}\left(t\right)\end{array}---\left(7\right)$

其中，γ_rINS、θ_pINS和ψ_hINS分别为惯性导航系统的横滚角、俯仰角和航向角，γ_rCNS、θ_pCNS、ψ_hCNS分别为天文导航系统的横滚角、俯仰角和航向角，δγ_r、δθ_p和δψ_h分别为惯性导航系统和天文导航系统中横滚角、俯仰角和航向角的差值，O_rCNS、O_pCNS和O_hCNS为相减产生的误差小量，H_a(t)为天文导航子系统的量测阵，N_CNS(t)为天文子系统的量测噪声矩阵；

（2）INS/SAR图像匹配水平位置量测方程：

$Z_{\mathrm{SAR}}\left(t\right)\begin{array}{c}=\left[\begin{array}{c}(L_{\mathrm{INS}}-L_{\mathrm{SAR}})R_M\\ ({\mathrm{\λ}}_{\mathrm{INS}}-{\mathrm{\λ}}_{\mathrm{SAR}})R_N\cos L\end{array}\right]=\left[\begin{array}{c}{R}_M\mathrm{\δL}+N_{M^{\mathrm{SAR}}}\\ R_N\cos \mathrm{L\δ\λ}+N_{N^{\mathrm{SAR}}}\end{array}\right]\\ =H_{\mathrm{hp}}\left(t\right)X\left(t\right)+N_{\mathrm{SAR}}\left(t\right)\end{array}---\left(8\right)$

其中，L_INS、λ_INS分别为惯性导航系统的纬度和经度，L_SAR、λ_SAR分别为景象导航系统的纬度和经度，R_M为地球曲率半径子午圈，R_N地球曲率半径卯酉圈，δL、δλ分别为惯性导航系统和景象导航系统中维度和经度的差值，

为子午圈下量测噪声矩阵，为卯酉圈下量测噪声矩阵，H_hp(t)为景象导航子系统的量测阵，N_SAR(t)为景象子系统的量测噪声矩阵；

（3）INS/TER地形匹配水平位置量测方程：

$Z_{\mathrm{TER}}\left(t\right)\begin{array}{c}=\left[\begin{array}{c}(L_{\mathrm{INS}}-L_{\mathrm{TER}})R_M\\ ({\mathrm{\λ}}_{\mathrm{INS}}-{\mathrm{\λ}}_{\mathrm{TER}})R_N\cos L\end{array}\right]=\left[\begin{array}{c}{R}_M\mathrm{\δL}+N_{M^{\mathrm{TER}}}\\ R_N\cos \mathrm{L\δ\λ}+N_{N^{\mathrm{TER}}}\end{array}\right]\\ =H_{\mathrm{tp}}\left(t\right)X\left(t\right)+N_{\mathrm{TER}}\left(t\right)\end{array}---\left(9\right)$

其中，L_TER、λ_TER分别为地形导航系统的纬度和经度，H_tp(t)为地形导航子系统的量测阵，N_TER(t)为地形子系统的量测噪声矩阵。

作为本发明的进一步优选方案，步骤三中所述子滤波系统修正方差阵ΔP_i的构造方法如下所示：

${\tilde{x}}_{i\left(k\right)}^P\stackrel{\mathrm{def}}{=}{x}_{i\left(k\right)}^r-{\hat{x}}_{i\left(k\right)}---\left(10\right)$

${\tilde{x}}_{i(k/k-1)}^P\stackrel{\mathrm{def}}{=}{x}_{i\left(k\right)}^r-{\hat{x}}_{i(k/k-1)}---\left(11\right)$

$P_{i\left(k\right)}^P\stackrel{\mathrm{def}}{=}E\left[{\tilde{x}}_{i\left(k\right)}^P{\tilde{x}}_{i\left(k\right)}^{\mathrm{PT}}\right]---\left(12\right)$

$P_{i(k/k-1)}^P\stackrel{\mathrm{def}}{=}E\left[{\tilde{x}}_{i(k/k-1)}^P{\tilde{x}}_{i(k/k-1)}^{\mathrm{PT}}\right]---\left(13\right)$

上述公式中，公式（11）表示k-1时刻理想状态值与滤波估计值一步预测值的差值；公式（13）表示k-1时刻一步预测估计均方差阵；

通过离散型卡尔曼滤波方程知，反映真实估计误差和一步预测误差的均方差阵的具体计算公式为：

$P_{i\left(k\right)}^P=(I-K_{i\left(k\right)}{H}_{i\left(k\right)})P_{i(k/k-1)}^P{(I-K_{i\left(k\right)}{H}_{i\left(k\right)})}^T+K_{i\left(k\right)}{R}_k^r{K}_{i\left(k\right)}^T---\left(14\right)$

$P_{i(k/k-1)}^P={\mathrm{\φ}}_{k,k-1}{P}_{i(k-1)}^P{\mathrm{\φ}}_{k,k-1}^T+{\mathrm{\Γ}}_{k-1}{Q}_{k-1}^r{\mathrm{\Γ}}_{k-1}^T---\left(15\right)$

上式中I为单位矩阵，K_i(k)是第k时刻的滤波增益，H_i(k)是第k时刻的量测矩阵，

为k时刻理想量测噪声方差阵，φ_k,k-1为k-1时刻至k时刻的一步状态转移矩阵，

为k-1时刻系统理想噪声方差阵，Γ_k-1为系统噪声驱动阵；

而相应滤波计算中获得的计算均方差阵为：

$P_{i\left(k\right)}=(I-K_{i\left(k\right)}{H}_{i\left(k\right)})P_{i(k/k-1)}{(I-K_{i\left(k\right)}{H}_{i\left(k\right)})}^T+K_{i\left(k\right)}{R}_k{K}_{i\left(k\right)}^T---\left(16\right)$

$P_{i(k/k-1)}={\mathrm{\φ}}_{k,k-1}{P}_{i(k-1)}{\mathrm{\φ}}_{k,k-1}^T+{\mathrm{\Γ}}_{k-1}{Q}_{k-1}{\mathrm{\Γ}}_{k-1}^T---\left(17\right)$

根据公式（12）至公式（15），整理可得：

$\begin{array}{c}{\mathrm{\ΔP}}_{i\left(k\right)}=P_{i\left(k\right)}^P-P_{i\left(k\right)}\\ =(I-K_{i\left(k\right)}{H}_{i\left(k\right)})P_{k/k-1}{(I-K_{i\left(k\right)}{H}_{i\left(k\right)})}^T+K_{i\left(k\right)}(R_k^r-R_k)K_{i\left(k\right)}^T\end{array}---\left(18\right)$

$\begin{array}{c}{\mathrm{\ΔP}}_{i(k/k-1)}=P_{i(k/k-1)}^P-P_{i(k/k-1)}\\ ={\mathrm{\φ}}_{k,k-1}{P}_{k-1}{\mathrm{\φ}}_{k,k-1}^T+{\mathrm{\Γ}}_{k-1}(Q_{k-1}^r-Q_{k-1}){\mathrm{\Γ}}_{k-1}^T\end{array}---\left(19\right)$

其中，P_i(k)是系统第i子滤波器第k时刻估计均方差阵，P_i(k/k-1)是系统第i子滤波器第k-1时刻一步预测估计均方差阵，ΔP_i(k)、ΔP_i(k/k-1)是在系统模型和量测模型正确的情况下，因系统噪声方差阵、量测噪声方差阵及初始误差阵选取的误差而造成的滤波误计算的误差方差阵以及一步预测误差方差阵；

在滤波器的实际设计中参数选择存在误差，使得

$P_{i\left(0\right)}\≠P_0^P,Q\≠Q^r,R\≠R^r$

其中，P_i(0)、Q和R为系统实际使用配置的值，

Q^r和R^r为系统无误差的理想值，因此出现ΔP_i(k)≠0，在滤波过程中P_i(k)逐渐达到稳态值，使得相应的ΔP_i(k)也会随之趋于常量；

通过在子滤波器输出端增加修正方差阵ΔP_i，可得系统实际估计均方差阵P_i(k)：

P_i(k)＝P_i(k)+ΔP_i         （20）

其中，添加的修正方差阵ΔP_i的理想对角矩阵形式为：

${\mathrm{\ΔP}}_i=\frac{1}{K}\underset{k=k_0}{\overset{K+k_0-1}{\mathrm{\Σ}}}({\hat{x}}_{i\left(k\right)}-x_{i\left(k\right)}^r){({\hat{x}}_{i\left(k\right)}-x_{i\left(k\right)}^r)}^T-P_{i\left(k\right)}---\left(21\right)$

在实际中的近似表达形式为：

${\mathrm{\ΔP}}_i=\frac{1}{K}\underset{k=k_0}{\overset{K+k_0-1}{\mathrm{\Σ}}}({\hat{x}}_{i\left(k\right)}-{\stackrel{\‾}{x}}_{i\left(k\right)}){({\hat{x}}_{i\left(k\right)}-{\stackrel{\‾}{x}}_{i\left(k\right)})}^T-{\hat{P}}_{i\left(k\right)}---\left(22\right)$

其中，K为在滤波器收敛后要采样的点数，k₀为对

采样的起始采样点，为采样中第K时刻子滤波器i的输出信息，为K个采样点

的均值，

为相对应K时刻子滤波器i的误差方差阵输出信息。

作为本发明的进一步优选方案，步骤四中所述非等间隔修正算法具体如下所示：

当多个传感器均有新的量测信息时，各个子滤波器进行卡尔曼滤波，过程如下：

${\hat{X}}_{i(k/k-1)}={\mathrm{\φ}}_{k-k-1}{\hat{X}}_{i(k-1)}---\left(23\right)$

$P_{i(k/k-1)}={\mathrm{\φ}}_{k,k-1}{P}_{i(k-1)}{\mathrm{\φ}}_{k,k-1}^T+{\mathrm{\Γ}}_{k-1}{Q}_{k-1}{\mathrm{\Γ}}_{k-1}^T---\left(24\right)$

$K_{i\left(k\right)}=P_{i(k/k-1)}{H}_{i\left(k\right)}^T{[H_{i\left(k\right)}{P}_{i(k/k-1)}{H}_{i\left(k\right)}^T+R_{i\left(k\right)}]}^{-1}---\left(25\right)$

${\hat{X}}_{i\left(k\right)}={\hat{X}}_{i(k/k-1)}+K_{i\left(k\right)}[Z_{i\left(k\right)}-H_{i\left(k\right)}{\hat{X}}_{i(k/k-1)}]---\left(26\right)$

$P_{i\left(k\right)}=[I-K_{i\left(k\right)}{H}_{i\left(k\right)}]P_{i(k/k-1)}---\left(27\right)$

其中，Z_i(k)为第i子滤波器第k时刻的量测信息矩阵；

各子传感器不全有新的量测信息，若到达量测输出时刻，则对应子滤波器则利用式（26）到式（27）进行卡尔曼滤波；若未到达量测输出时刻，则利用式（28）到式（29）状态转移矩阵进行时间更新；

主滤波器利用各子滤波器的信息进行处理，过程如下：

${\hat{X}}_{i(k/k-1)}={\mathrm{\φ}}_{k/k-1}{\hat{X}}_{i(k-1)}---\left(28\right)$

$P_{i(k/k-1)}={\mathrm{\φ}}_{k,k-1}{P}_{i(k-1)}{\mathrm{\φ}}_{k,k-1}^T+{\mathrm{\Γ}}_{k-1}{Q}_{k-1}{\mathrm{\Γ}}_{k-1}^T---\left(29\right)$

联邦滤波主滤波器算法实现步骤如下：

$P_{f\left(k\right)}^{-1}=\underset{i=1}{\overset{n}{\mathrm{\Σ}}}{P}_{i\left(k\right)}^{-1}---\left(30\right)$

${\hat{X}}_{f\left(k\right)}=P_{f\left(k\right)}\underset{i=1}{\overset{n}{\mathrm{\Σ}}}{P}_{i\left(k\right)}^{-1}{\hat{x}}_{i\left(k\right)}---\left(31\right).$

本发明采用以上技术方案与现有技术相比，具有以下技术效果：本发明解决了无重置联邦滤波器结构不能对子滤波器进行反馈修正，进而不能提高子滤波器估计精度的问题，同时针对无重置联邦滤波器在非等间隔情形下的适应性问题，设计了适用于高空长航无人机的INS/CNS/SAR/TER组合非等间隔联邦滤波自主导航实现方法，它具有以下优点：

(1)根据不同子滤波器精度不一致，以及滤波趋于稳定的特点在无重置联邦滤波器的设计中，通过添加合适的ΔP_i(k)，来调整最终信息融合时的冗余子滤波系统信息的权重分配，使得子滤波器的滤波估计精度提高，进而提高主滤波器的融合精度。

(2)设计了适用于高空长航无人机的INS/CNS/SAR/TER组合非等间隔联邦滤波自主导航实现方案，针对无重置联邦滤波器在非等间隔情形下的适应性问题，设计了主/子滤波器的状态更新方法，通过对其在仅有一个子滤波器有量测输出时的不适应性进行针对性改进，使系统算法在保持容错性强、运算速度快的特性的同时，提高了自主导航系统的导航融合精度。

(3)针对高空长航无人机多源信息导航系统组合多、具备冗余信息的特点，针对无重置联邦滤波器结构不能对子滤波器进行反馈修正，进而不能提高子滤波器估计精度的问题；通过修正子滤波器的滤波方差阵，进而修正相应子滤波器在主滤波器最优估计中的比重，使其多源冗余信息的融合分布更趋合理，也改善了融合精度，并应用于高空长航无人机的INS/CNS/SAR/TER组合非等间隔联邦滤波自主导航实现方案。

与传统的故障检测及隔离相比，本发明的基于滤波器方差阵修正的多源信息非等间隔联邦滤波方法在几乎不增加额外的计算负担的情况下在保持容错性强、运算速度快的特性的同时，提高了自主导航系统的导航融合精度。

附图说明

图1为本发明的基于滤波器方差阵修正的多源信息非等间隔联邦滤波方法的结构图。

图2为本发明的多传感器非等间隔时间关系示意图。

图3为本发明的无重置非等间隔联邦滤波结构图。

图4为本发明的子滤波器有修正下和无修正情况下无重置联邦滤波器在等间隔情况下经度误差对比曲线。

图5为本发明的子滤波器有修正下和无修正情况下无重置联邦滤波器在等间隔情况下纬度误差对比曲线。

图6为本发明的子滤波器有修正下和无修正情况下无重置联邦滤波器在非等间隔情况下横滚角对比曲线。

图7为本发明的子滤波器有修正下和无修正情况下无重置联邦滤波器在非等间隔情况下俯仰角对比曲线。

图8为本发明的子滤波器有修正下和无修正情况下无重置联邦滤波器在非等间隔情况下航向角对比曲线。

图9为本发明的子滤波器有修正下和无修正情况下无重置联邦滤波器在非等间隔情况下经度误差对比曲线。

图10为本发明的子滤波器有修正下和无修正情况下无重置联邦滤波器在非等间隔情况下纬度误差对比曲线。

具体实施方式

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

如图1所示，本发明的原理是：针对高空长航无人机的INS/CNS/SAR/TER（惯性导航系统/天文导航系统/图像匹配导航系统/地形匹配导航系统）组合非等间隔联邦滤波自主导航特点，从地理系导航的角度入手，依据系统状态方程和各子滤波系统的线性化量测方程，构成滤波子滤波系统。利用联邦滤波信息不同子滤波器的精度不一致时，由于滤波趋于稳定的特点因此在无重置联邦滤波器的设计中，应该通过添加合适的ΔP_i(k)，使得子滤波器的滤波估计精度提高，通过修正子滤波器的滤波方差阵，进而修正相应子滤波器在主滤波器最优估计中的比重，使其多源冗余信息的融合分布更趋合理，在此基础上针对无重置联邦滤波器在非等间隔情形下的适应性问题，设计了主/子滤波器的状态更新方法，通过对其在仅有一个子滤波器有量测输出时的不适应性进行针对性改进，使系统算法在保持容错性强、运算速度快的特性的同时提高自主导航系统的导航融合精度。

具体实施方法如下：

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

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

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

其中，F(t)为惯导系统误差方程所对应的状态系数矩阵，G(t)为惯导系统误差方程所对应的白噪声误差系数矩阵，W(t)为惯导系统误差方程所对应的白噪声随机误差向量，惯性导航系统误差状态量为：

X＝[φ_E,φ_N,φ_U,δv_E,δv_N,δv_U,δL,δλ,δh,ε_bx,ε_by,ε_bz,ε_rx,ε_ry,ε_rz,▽_x,▽_y,▽_z]^T     （2）

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

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

采用地理系下位置、速度、姿态线性化观测原理，根据各子滤波系统不同工作特性，建立地理系下各子滤波系统的量测方程，如下式所示：

1、INS/CNS姿态量测方程

CNS采用姿态型天文导航系统，量测方程中的量测信息由INS输出的三维姿态信息分别与天文输出的横滚角、俯仰角和航向角的差值构成，如式（3）。

${Z\left(t\right)}_{\mathrm{CNS}}\begin{array}{c}=\left[\begin{array}{c}{\mathrm{\γ}}_{rINS}-{\mathrm{\γ}}_{\mathrm{rCNS}}\\ {\mathrm{\θ}}_{\mathrm{pINS}}-{\mathrm{\θ}}_{\mathrm{pCNS}}\\ {\mathrm{\ψ}}_{\mathrm{hINS}}-{\mathrm{\ψ}}_{\mathrm{hCNS}}\end{array}\right]=\left[\begin{array}{c}{\mathrm{\δ\γ}}_r+O_{\mathrm{rCNS}}\\ {\mathrm{\δ\θ}}_p+O_{\mathrm{pCNS}}\\ {\mathrm{\δ\ψ}}_h+O_{\mathrm{hCNS}}\end{array}\right]\\ =H_a\left(t\right)X\left(t\right)+N_{\mathrm{CNS}}\left(t\right)\end{array}---\left(3\right)$

其中，γ_rINS、θ_pINS和ψ_hINS分别为惯性导航系统的横滚角、俯仰角和航向角，γ_rCNS、θ_pCNS、ψ_hCNS分别为天文导航系统的横滚角、俯仰角和航向角，δγ_r、δθ_p和δψ_h分别为二者的差值，O_rCNS、O_pCNS和O_hCNS为相减产生的误差小量，H_a(t)为天文导航子系统的量测阵，N_CNS(t)为天文子系统的量测噪声矩阵；

2、INS/SAR图像匹配水平位置量测方程

量测信息由INS和SAR图像匹配导航系统输出的纬度、经度差值构成，具体为：

$Z_{\mathrm{SAR}}\left(t\right)\begin{array}{c}=\left[\begin{array}{c}(L_{\mathrm{INS}}-L_{\mathrm{SAR}})R_M\\ ({\mathrm{\λ}}_{\mathrm{INS}}-{\mathrm{\λ}}_{\mathrm{SAR}})R_N\cos L\end{array}\right]=\left[\begin{array}{c}{R}_M\mathrm{\δL}+N_{M^{\mathrm{SAR}}}\\ R_N\cos \mathrm{L\δ\λ}+N_{N^{\mathrm{SAR}}}\end{array}\right]\\ =H_{\mathrm{hp}}\left(t\right)X\left(t\right)+N_{\mathrm{SAR}}\left(t\right)\end{array}---\left(4\right)$

其中，L_INS、λ_INS分别为惯性导航系统的纬度和经度，L_SAR、λ_SAR分别为景象导航系统的纬度和经度，R_M为地球曲率半径子午圈，R_N地球曲率半径卯酉圈，δL、δλ分别为二者的差值，H_hp(t)为景象导航子系统的量测阵，N_SAR(t)为景象子系统的量测噪声矩阵；

3、INS/TER水平位置量测方程

量测信息由INS和TER地形匹配导航系统输出的纬度、经度差值构成，具体为：

$Z_{\mathrm{TER}}\left(t\right)\begin{array}{c}=\left[\begin{array}{c}(L_{\mathrm{INS}}-L_{\mathrm{TER}})R_M\\ ({\mathrm{\λ}}_{\mathrm{INS}}-{\mathrm{\λ}}_{\mathrm{TER}})R_N\cos L\end{array}\right]=\left[\begin{array}{c}{R}_M\mathrm{\δL}+N_{M^{\mathrm{TER}}}\\ R_N\cos \mathrm{L\δ\λ}+N_{N^{\mathrm{TER}}}\end{array}\right]\\ =H_{\mathrm{tp}}\left(t\right)X\left(t\right)+N_{\mathrm{TER}}\left(t\right)\end{array}---\left(5\right)$

其中，L_TER、λ_TER分别为地形导航系统的纬度和经度，H_tp(t)为地形导航子系统的量测阵，N_TER(t)为地形子系统的量测噪声矩阵。

三、进行子滤波系统KF(Kalman Filter)滤波，构造合理的修正量ΔP_i，调整信息融合的冗余子滤波系统信息的权重分配，以期达到更好的融合效果

在离散系统的卡尔曼滤波误差分析中，令

为第i子滤波器的第k时刻状态真实量，

分别为系统实际估计均方差阵和系统实际一步预测误差方差阵。其表达式为：

${\tilde{x}}_{i\left(k\right)}^P\stackrel{\mathrm{def}}{=}{x}_{i\left(k\right)}^r-{\hat{x}}_{i\left(k\right)}---\left(6\right)$

${\tilde{x}}_{i(k/k-1)}^P\stackrel{\mathrm{def}}{=}{x}_{i\left(k\right)}^r-{\hat{x}}_{i(k/k-1)}---\left(7\right)$

$P_{i\left(k\right)}^P\stackrel{\mathrm{def}}{=}E\left[{\tilde{x}}_{i\left(k\right)}^P{\tilde{x}}_{i\left(k\right)}^{\mathrm{PT}}\right]---\left(8\right)$

$P_{i(k/k-1)}^P\stackrel{\mathrm{def}}{=}E\left[{\tilde{x}}_{i(k/k-1)}^P{\tilde{x}}_{i(k/k-1)}^{\mathrm{PT}}\right]---\left(9\right)$

式中

为理想状态值与滤波估计值一步预测值的差值，

分别是一步预测估计均方差阵，

为

为理想状态值与滤波估计值差值的转置。

通过离散型卡尔曼滤波方程知，反映真实估计误差和一步预测误差的均方差阵的具体计算公式为：

$P_{i\left(k\right)}^P=(I-K_{i\left(k\right)}{H}_{i\left(k\right)})P_{i(k/k-1)}^P{(I-K_{i\left(k\right)}{H}_{i\left(k\right)})}^T+K_{i\left(k\right)}{R}_k^r{K}_{i\left(k\right)}^T---\left(10\right)$

$P_{i(k/k-1)}^P={\mathrm{\φ}}_{k,k-1}{P}_{i(k-1)}^P{\mathrm{\φ}}_{k,k-1}^T+{\mathrm{\Γ}}_{k-1}{Q}_{k-1}^r{\mathrm{\Γ}}_{k-1}^T---\left(11\right)$

上式中I为单位矩阵，K_k是第k时刻的滤波增益，H_i(k)是第k时刻的量测矩阵，

为k时刻理想量测噪声方差阵，φ_k,k-1为k-1时刻至k时刻的一步状态转移矩阵，

为k-1时刻系统理想噪声方差阵，Γ_k-1为系统噪声驱动阵；

而相应滤波计算中获得的计算均方误差阵为：

$P_{i\left(k\right)}=(I-K_{i\left(k\right)}{H}_{i\left(k\right)})P_{i(k/k-1)}{(I-K_{i\left(k\right)}{H}_{i\left(k\right)})}^T+K_{i\left(k\right)}{R}_k{K}_{i\left(k\right)}^T---\left(12\right)$

$P_{i(k/k-1)}={\mathrm{\φ}}_{k,k-1}{P}_{i(k-1)}{\mathrm{\φ}}_{k,k-1}^T+{\mathrm{\Γ}}_{k-1}{Q}_{k-1}{\mathrm{\Γ}}_{k-1}^T---\left(13\right)$

根据公式（10）至公式（13），可得：

$\begin{array}{c}{\mathrm{\ΔP}}_{i\left(k\right)}=P_{i\left(k\right)}^P-P_{i\left(k\right)}\\ =(I-K_{i\left(k\right)}{H}_{i\left(k\right)})P_{k/k-1}{(I-K_{i\left(k\right)}{H}_{i\left(k\right)})}^T+K_{i\left(k\right)}(R_k^r-R_k)K_{i\left(k\right)}^T\end{array}---\left(14\right)$

$\begin{array}{c}{\mathrm{\ΔP}}_{i(k/k-1)}=P_{i(k/k-1)}^P-P_{i(k/k-1)}\\ ={\mathrm{\φ}}_{k,k-1}{P}_{k-1}{\mathrm{\φ}}_{k,k-1}^T+{\mathrm{\Γ}}_{k-1}(Q_{k-1}^r-Q_{k-1}){\mathrm{\Γ}}_{k-1}^T\end{array}---\left(15\right)$

其中，P_i(k)是系统第i子滤波器第k时刻估计均方差阵，P_i(k/k-1)是系统第i子滤波器第k-1时刻一步预测估计均方差阵，ΔP_i(k)、ΔP_i(k/k-1)是在系统模型和量测模型正确的情况下，因系统噪声方差阵、量测噪声方差阵及初始误差阵选取的误差而造成的滤波误计算的误差方差阵以及一步预测误差方差阵；

在滤波器的实际设计中参数选择会有一定误差，使得

P_i(0)、Q和R为系统实际使用配置的值，

Q^r和R^r为系统无误差的理想值，因此会出现ΔP_i(k)≠0，在滤波过程中，P_i(k)逐渐达到稳态值，相应的ΔP_i(k)也会随之趋于常量。上述的信息融合过程，当不同子滤波器的精度不一致时，由于滤波趋于稳定的特点因此在无重置联邦滤波器的设计中，应该通过添加合适的ΔP_i(k)，使得子滤波器的滤波估计精度提高，进而提高主滤波器的融合精度。

在存在冗余信息的结构中，多源信息的融合提高不仅仅依靠精度最高的子滤波系统，同时还需要对其他冗余导航子滤波系统的信息进行融合。针对精度较高的子滤波器的偏小的误差方差阵Pi，通过增加一个合理的ΔP_i来修正，来调整最终信息融合时的冗余子滤波系统信息的权重分配，以期达到更好的融合效果。

在改进方案中，通过在子滤波器输出端增加修正方差阵ΔP_i，可得系统实际估计均方差阵P_i(k)：

P_i(k)＝P_i(k)+ΔP_i        （16）

其中，添加的修正方差阵ΔP_i的理想对角矩阵形式为：

${\mathrm{\ΔP}}_i=\frac{1}{K}\underset{k=k_0}{\overset{K+k_0-1}{\mathrm{\Σ}}}({\hat{x}}_{i\left(k\right)}-x_{i\left(k\right)}^r){({\hat{x}}_{i\left(k\right)}-x_{i\left(k\right)}^r)}^T-P_{i\left(k\right)}---\left(17\right)$

在实际中的表达形式为：

${\mathrm{\ΔP}}_i=\frac{1}{K}\underset{k=k_0}{\overset{K+k_0-1}{\mathrm{\Σ}}}({\hat{x}}_{i\left(k\right)}-{\stackrel{\‾}{x}}_{i\left(k\right)}){({\hat{x}}_{i\left(k\right)}-{\stackrel{\‾}{x}}_{i\left(k\right)})}^T-{\hat{P}}_{i\left(k\right)}---\left(18\right)$

其中，K为在滤波器收敛后要采样的点数，k₀为对

采样的起始采样点，为采样中第K时刻子滤波器i的输出信息，为K个采样点

的均值，为

相对应K时刻子滤波器i的误差方差阵输出信息。

改进的主滤波器信息融合算法为：

$\left\{\begin{array}{c}{P}_{i\left(k\right)}=P_{i\left(k\right)}+{\mathrm{\ΔP}}_i\\ P_{f\left(k\right)}^{-1}=\underset{i=1}{\overset{n}{\mathrm{\Σ}}}{P}_{i(k/k)}^{-1}\\ {\hat{x}}_{f\left(k\right)}=P_{f\left(k\right)}\left(\underset{i=1}{\overset{n}{\mathrm{\Σ}}}{P}_{i(k/k)}^{-1}{\hat{x}}_{i(k/k)}\right)\end{array}\right.---\left(19\right)$

其中，P_f(k)分别为主滤波器滤波输出的滤波估计量和误差方差阵；

为第i个子滤波器k时刻的估计均方差阵P_i(k)的逆，

为主滤波器滤波输出的误差方差阵P_f(k)的逆。

本文中考虑到输出信息冗余的情况下，SAR和TER输出信息均为位置信息，且无重置结构中子滤波器的次优性，同时SAR/INS子滤波器估计精度较高，在位置精度上的融合权重分配影响远大于TER/INS，因此本文中仅对SAR/INS进行修正，其余子滤波器不进行修正，以期达到较好的融合对比效果。

四、无重置联邦滤波器非等间隔修正算法

联邦滤波系统中以INS导航系统的导航时间为基准，定义INS的解算周期为T_INS，T为主滤波器的融合周期，其中T=nT_INS，景象匹配传感器量测周期为T_SAR，地形匹配传感器量测周期为T_TER，天文传感器量测周期为T_CNS。多传感器非等间隔量测输出关系图如图2所示。

联邦滤波系统中各导航传感器如惯导、地形、景象和天文均有不同的频率的量测输出周期。为此，无法按照同一个滤波周期进行全局信息融合。考虑到一个滤波周期内常规的卡尔曼滤波可分为两个信息更新过程：时间更新和量测更新。各子滤波器在信息输出时，若未到量测时间则输出时间更新信息，而到量测时间时则输出量测更新信息，主滤波器利用各子滤波器输出信息进行信息融合，流程图如图3。

拟设计和采用的解决非等间隔量测的联邦滤波子滤波器算法实现步骤如下：

当多个传感器均有新的量测信息时，各个子滤波器进行卡尔曼滤波，过程如下：

${\hat{X}}_{i(k/k-1)}={\mathrm{\φ}}_{k,k-1}{\hat{X}}_{i(k-1)}---\left(20\right)$

$P_{i(k/k-1)}={\mathrm{\φ}}_{k,k-1}{P}_{i(k-1)}{\mathrm{\φ}}_{k,k-1}^T+{\mathrm{\Γ}}_{k-1}{Q}_{k-1}{\mathrm{\Γ}}_{k-1}^T---\left(21\right)$

$K_{i\left(k\right)}=P_{i(k/k-1)}{H}_{i\left(k\right)}^T{[H_{i\left(k\right)}{P}_{i(k/k-1)}{H}_{i\left(k\right)}^T+R_{i\left(k\right)}]}^{-1}---\left(21\right)$

${\hat{X}}_{i\left(k\right)}={\hat{X}}_{i(k/k-1)}+K_{i\left(k\right)}[Z_{i\left(k\right)}-H_{i\left(k\right)}{\hat{X}}_{i(k/k-1)}]---\left(23\right)$

$P_{i\left(k\right)}=[I-K_{i\left(k\right)}{H}_{i\left(k\right)}]P_{i(k/k-1)}---\left(24\right)$

式中Z_i(k)为第i子滤波器第k时刻的量测信息矩阵。

当各传感器不全有新的量测信息时，若量测输出时刻到对应子滤波器则利用式（23）到式（24）进行卡尔曼滤波，若未到，则利用式（25）到式（26）状态转移矩阵进行时间更新；

主滤波器利用各子滤波器的信息进行处理，过程如下：

${\hat{X}}_{i(k/k-1)}={\mathrm{\φ}}_{k/k-1}{\hat{X}}_{i(k-1)}---\left(25\right)$

$P_{i(k/k-1)}={\mathrm{\φ}}_{k,k-1}{P}_{i(k-1)}{\mathrm{\φ}}_{k,k-1}^T+{\mathrm{\Γ}}_{k-1}{Q}_{k-1}{\mathrm{\Γ}}_{k-1}^T---\left(26\right)$

联邦滤波主滤波器算法实现步骤如下：

$P_{f\left(k\right)}^{-1}=\underset{i=1}{\overset{n}{\mathrm{\Σ}}}{P}_{i\left(k\right)}^{-1}---\left(27\right)$

${\hat{X}}_{f\left(k\right)}=P_{f\left(k\right)}\underset{i=1}{\overset{n}{\mathrm{\Σ}}}{P}_{i\left(k\right)}^{-1}{\hat{x}}_{i\left(k\right)}---\left(28\right)$

图4至图5为本发明的子滤波器有修正下和无修正情况下无重置联邦滤波器在等间隔情况下经度、纬度误差对比曲线，为突出仿真对比效果，CNS、TER、SAR子滤波系统的数据量测输出间隔均为1秒，将CNS、TER、SAR三个子滤波系统全程组合，为保证滤波器收敛，本文中针对INS/SAR子滤波的输出信息采样从400秒开始，采集100个信息点，并利用步骤（3）计算获得修正量ΔP_i，并在主滤波器融合各子滤波器输出信息是采用步骤（3）中的融合方式进行信息融合。NR曲线为在传统无重置联邦滤波在等间隔情形下绘制的曲线，NR_modi为在将子滤波器修正后，无重置联邦滤波器在等间隔情况下绘制的曲线，图4、图5为二者对比曲线。经过仿真可以看出，在保持无重置联邦滤波器容错性强的优势下，能够提高整体滤波器的估计精度，在位置精度上有较大改善。

图6至图10的仿真结果表明，本发明提出的提出的修非等间隔无重置结构联邦滤波算法与传统联邦滤波算法相比，在经过改进后的无重置联邦滤波器在非等间隔情形下，在保持无重置联邦滤波器容错性强的优势下，还能够进一步提高整体滤波器的估计精度，在姿态上有一定的修正效果，同时尤其是在位置精度上，较传统算法有较大提高，提高导航系统的鲁棒性。

上面结合附图对本发明的实施方式作了详细说明，但是本发明并不限于上述实施方式，在本领域普通技术人员所具备的知识范围内，还可以在不脱离本发明宗旨的前提下做出各种变化。

Claims (4)

1.基于滤波器方差阵修正的多源信息非等间隔联邦滤波方法，其特征在于，包括以下步骤：

步骤一、选取东北天地理坐标系，惯性导航系统误差状态量定义为：

X＝[φ_E,φ_N,φ_U,δv_E,δv_N,δv_U,δL,δλ,δh,ε_bx,ε_by,ε_bz,ε_rx,ε_ry,ε_rz,▽_x,▽_y,▽_z]^T     （1）

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

步骤二、采用地理坐标系下位置、速度、姿态线性化观测原理，根据子滤波系统的工作特性，建立地理坐标系下子滤波系统的量测方程，所述子滤波系统具体包括INS/CNS姿态量测系统、INS/SAR图像匹配水平位置量测系统、INS/TER地形匹配水平位置量测系统：

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

其中，X(t)为步骤一中

X＝[φ_E,φ_N,φ_U,δv_E,δv_N,δv_U,δL,δλ,δh,ε_bx,ε_by,ε_bz,ε_rx,ε_ry,ε_rz,▽_x,▽_y,▽_z]^T加入时间参数t的表达形式，

为X(t)的微分形式，F(t)表示INS/CNS/SAR/TER组合导航系统状态方程的一步状态转移矩阵；G(t)表示INS/CNS/SAR/TER组合导航系统状态方程的系统白噪声误差矩阵；W(t)为惯性/卫星组合导航系统状态方程的系统误差白噪声矢量；

步骤三、将各子滤波系统量测方程中的子滤波系统误差状态量进行KF滤波，计算子滤波器的滤波估计均方差阵；增加修正方差阵ΔP_i，调整最终信息融合时的冗余子滤波系统信息的权重分配，具体如下：

在离散系统的卡尔曼滤波误差分析中，

${\tilde{x}}_{i\left(k\right)}^P\stackrel{\mathrm{def}}{=}{x}_{i\left(k\right)}^r-{\hat{x}}_{i\left(k\right)}---\left(3\right)$

$P_{i\left(k\right)}^P\stackrel{\mathrm{def}}{=}E\left[{\tilde{x}}_{i\left(k\right)}^P{\tilde{x}}_{i\left(k\right)}^{\mathrm{PT}}\right]---\left(4\right)$

其中，

分别为第i个子滤波器的第k时刻状态真实量和滤波估计量，

为理想状态值与滤波估计值的差值，

为子系统理想估计均方差阵，E[.]为取均值运算，T为转置运算；

在子滤波器的滤波估计均方差阵的基础上增加修正方差阵ΔP_i后，得到经过修正的系统滤波器输出的估计均方差阵P_i(k)：

P_i(k)＝P_i(k)+ΔP_i        （5）

最终得到改进后的主滤波器信息融合算法为：

$\left\{\begin{array}{c}{P}_{i\left(k\right)}=P_{i\left(k\right)}+{\mathrm{\ΔP}}_i\\ P_{f\left(k\right)}^{-1}=\underset{i=1}{\overset{n}{\mathrm{\Σ}}}{P}_{i(k/k)}^{-1}\\ {\hat{x}}_{f\left(k\right)}=P_{f\left(k\right)}\left(\underset{i=1}{\overset{n}{\mathrm{\Σ}}}{P}_{i(k/k)}^{-1}{\hat{x}}_{i(k/k)}\right)\end{array}\right.---\left(6\right)$

其中，

P_f(k)分别为主滤波器滤波输出的滤波估计量和误差方差阵；

为主滤波器滤波输出的误差方差阵P_f(k)的逆，

为第i个子滤波器k时刻的估计均方差阵P_i(k)的逆，

为第i个子滤波器k时刻滤波估计量；

步骤四、采用无重置联邦滤波器非等间隔修正算法，将一个滤波周期内的卡尔曼滤波分为两个信息更新过程：时间更新和量测更新；

各子滤波器在信息输出时，若未到量测时间则输出时间更新信息，若到量测时间时则输出量测更新信息；

步骤五、采用联邦滤波器对步骤三中子滤波系统得出的滤波结果进行数据融合，输出全局最优估计值。

2.根据权利要求1所述的基于滤波器方差阵修正的多源信息非等间隔联邦滤波方法，其特征在于，所述子滤波系统测量方程具体如下：

（1）INS/CNS姿态量测方程：

${Z\left(t\right)}_{\mathrm{CNS}}\begin{array}{c}=\left[\begin{array}{c}{\mathrm{\γ}}_{rINS}-{\mathrm{\γ}}_{\mathrm{rCNS}}\\ {\mathrm{\θ}}_{\mathrm{pINS}}-{\mathrm{\θ}}_{\mathrm{pCNS}}\\ {\mathrm{\ψ}}_{\mathrm{hINS}}-{\mathrm{\ψ}}_{\mathrm{hCNS}}\end{array}\right]=\left[\begin{array}{c}{\mathrm{\δ\γ}}_r+O_{\mathrm{rCNS}}\\ {\mathrm{\δ\θ}}_p+O_{\mathrm{pCNS}}\\ {\mathrm{\δ\ψ}}_h+O_{\mathrm{hCNS}}\end{array}\right]\\ =H_a\left(t\right)X\left(t\right)+N_{\mathrm{CNS}}\left(t\right)\end{array}---\left(7\right)$

其中，γ_rINS、θ_pINS和ψ_hINS分别为惯性导航系统的横滚角、俯仰角和航向角，γ_rCNS、θ_pCNS、ψ_hCNS分别为天文导航系统的横滚角、俯仰角和航向角，δγ_r、δθ_p和δψ_h分别为惯性导航系统和天文导航系统中横滚角、俯仰角和航向角的差值，O_rCNS、O_pCNS和O_hCNS为相减产生的误差小量，H_a(t)为天文导航子系统的量测阵，N_CNS(t)为天文子系统的量测噪声矩阵；

（2）INS/SAR图像匹配水平位置量测方程：

$Z_{\mathrm{SAR}}\left(t\right)\begin{array}{c}=\left[\begin{array}{c}(L_{\mathrm{INS}}-L_{\mathrm{SAR}})R_M\\ ({\mathrm{\λ}}_{\mathrm{INS}}-{\mathrm{\λ}}_{\mathrm{SAR}})R_N\cos L\end{array}\right]=\left[\begin{array}{c}{R}_M\mathrm{\δL}+N_{M^{\mathrm{SAR}}}\\ R_N\cos \mathrm{L\δ\λ}+N_{N^{\mathrm{SAR}}}\end{array}\right]\\ =H_{\mathrm{hp}}\left(t\right)X\left(t\right)+N_{\mathrm{SAR}}\left(t\right)\end{array}---\left(8\right)$

其中，L_INS、λ_INS分别为惯性导航系统的纬度和经度，L_SAR、λ_SAR分别为景象导航系统的纬度和经度，R_M为地球曲率半径子午圈，R_N地球曲率半径卯酉圈，δL、δλ分别为惯性导航系统和景象导航系统中维度和经度的差值，

为子午圈下量测噪声矩阵，

为卯酉圈下量测噪声矩阵，H_hp(t)为景象导航子系统的量测阵，N_SAR(t)为景象子系统的量测噪声矩阵；

（3）INS/TER地形匹配水平位置量测方程：

$Z_{\mathrm{TER}}\left(t\right)\begin{array}{c}=\left[\begin{array}{c}(L_{\mathrm{INS}}-L_{\mathrm{TER}})R_M\\ ({\mathrm{\λ}}_{\mathrm{INS}}-{\mathrm{\λ}}_{\mathrm{TER}})R_N\cos L\end{array}\right]=\left[\begin{array}{c}{R}_M\mathrm{\δL}+N_{M^{\mathrm{TER}}}\\ R_N\cos \mathrm{L\δ\λ}+N_{N^{\mathrm{TER}}}\end{array}\right]\\ =H_{\mathrm{tp}}\left(t\right)X\left(t\right)+N_{\mathrm{TER}}\left(t\right)\end{array}---\left(9\right)$

其中，L_TER、λ_TER分别为地形导航系统的纬度和经度，H_tp(t)为地形导航子系统的量测阵，N_TER(t)为地形子系统的量测噪声矩阵。

3.根据权利要求1所述的基于滤波器方差阵修正的多源信息非等间隔联邦滤波方法，其特征在于：步骤三中所述子滤波系统修正方差阵ΔP_i的构造方法如下所示：

${\tilde{x}}_{i\left(k\right)}^P\stackrel{\mathrm{def}}{=}{x}_{i\left(k\right)}^r-{\hat{x}}_{i\left(k\right)}---\left(10\right)$

${\tilde{x}}_{i(k/k-1)}^P\stackrel{\mathrm{def}}{=}{x}_{i\left(k\right)}^r-{\hat{x}}_{i(k/k-1)}---\left(11\right)$

$P_{i\left(k\right)}^P\stackrel{\mathrm{def}}{=}E\left[{\tilde{x}}_{i\left(k\right)}^P{\tilde{x}}_{i\left(k\right)}^{\mathrm{PT}}\right]---\left(12\right)$

$P_{i(k/k-1)}^P\stackrel{\mathrm{def}}{=}E\left[{\tilde{x}}_{i(k/k-1)}^P{\tilde{x}}_{i(k/k-1)}^{\mathrm{PT}}\right]---\left(13\right)$

上述公式中，公式（11）表示k-1时刻理想状态值与滤波估计值一步预测值的差值；公式（13）表示k-1时刻一步预测估计均方差阵；

通过离散型卡尔曼滤波方程知，反映真实估计误差和一步预测误差的均方差阵的具体计算公式为：

$P_{i\left(k\right)}^P=(I-K_{i\left(k\right)}{H}_{i\left(k\right)})P_{i(k/k-1)}^P{(I-K_{i\left(k\right)}{H}_{i\left(k\right)})}^T+K_{i\left(k\right)}{R}_k^r{K}_{i\left(k\right)}^T---\left(14\right)$

$P_{i(k/k-1)}^P={\mathrm{\φ}}_{k,k-1}{P}_{i(k-1)}^P{\mathrm{\φ}}_{k,k-1}^T+{\mathrm{\Γ}}_{k-1}{Q}_{k-1}^r{\mathrm{\Γ}}_{k-1}^T---\left(15\right)$

上式中I为单位矩阵，K_i(k)是第k时刻的滤波增益，H_i(k)是第k时刻的量测矩阵，为k时刻理想量测噪声方差阵，φ_k,k-1为k-1时刻至k时刻的一步状态转移矩阵，

为k-1时刻系统理想噪声方差阵，Γ_k-1为系统噪声驱动阵；

而相应滤波计算中获得的计算均方差阵为：

$P_{i\left(k\right)}=(I-K_{i\left(k\right)}{H}_{i\left(k\right)})P_{i(k/k-1)}{(I-K_{i\left(k\right)}{H}_{i\left(k\right)})}^T+K_{i\left(k\right)}{R}_k{K}_{i\left(k\right)}^T---\left(16\right)$

$P_{i(k/k-1)}={\mathrm{\φ}}_{k,k-1}{P}_{i(k-1)}{\mathrm{\φ}}_{k,k-1}^T+{\mathrm{\Γ}}_{k-1}{Q}_{k-1}{\mathrm{\Γ}}_{k-1}^T---\left(17\right)$

根据公式（12）至公式（15），整理可得：

$\begin{array}{c}{\mathrm{\ΔP}}_{i\left(k\right)}=P_{i\left(k\right)}^P-P_{i\left(k\right)}\\ =(I-K_{i\left(k\right)}{H}_{i\left(k\right)})P_{k/k-1}{(I-K_{i\left(k\right)}{H}_{i\left(k\right)})}^T+K_{i\left(k\right)}(R_k^r-R_k)K_{i\left(k\right)}^T\end{array}---\left(18\right)$

$\begin{array}{c}{\mathrm{\ΔP}}_{i(k/k-1)}=P_{i(k/k-1)}^P-P_{i(k/k-1)}\\ ={\mathrm{\φ}}_{k,k-1}{P}_{k-1}{\mathrm{\φ}}_{k,k-1}^T+{\mathrm{\Γ}}_{k-1}(Q_{k-1}^r-Q_{k-1}){\mathrm{\Γ}}_{k-1}^T\end{array}---\left(19\right)$

其中，P_i(k)是系统第i子滤波器第k时刻估计均方差阵，P_i(k/k-1)是系统第i子滤波器第k-1时刻一步预测估计均方差阵，ΔP_i(k)、ΔP_i(k/k-1)是在系统模型和量测模型正确的情况下，因系统噪声方差阵、量测噪声方差阵及初始误差阵选取的误差而造成的滤波误计算的误差方差阵以及一步预测误差方差阵；

在滤波器的实际设计中参数选择存在误差，使得

$P_{i\left(0\right)}\≠P_0^P,Q\≠Q^r,R\≠R^r$

其中，P_i(0)、Q和R为系统实际使用配置的值，Q^r和R^r为系统无误差的理想值，因此出现ΔP_i(k)≠0，在滤波过程中P_i(k)逐渐达到稳态值，使得相应的ΔP_i(k)也会随之趋于常量；

通过在子滤波器输出端增加修正方差阵ΔP_i，可得系统实际估计均方差阵P_i(k)：

P_i(k)＝P_i(k)+ΔP_i         （20）

其中，添加的修正方差阵ΔP_i的理想对角矩阵形式为：

${\mathrm{\ΔP}}_i=\frac{1}{K}\underset{k=k_0}{\overset{K+k_0-1}{\mathrm{\Σ}}}({\hat{x}}_{i\left(k\right)}-x_{i\left(k\right)}^r){({\hat{x}}_{i\left(k\right)}-x_{i\left(k\right)}^r)}^T-P_{i\left(k\right)}---\left(21\right)$

在实际中的近似表达形式为：

${\mathrm{\ΔP}}_i=\frac{1}{K}\underset{k=k_0}{\overset{K+k_0-1}{\mathrm{\Σ}}}({\hat{x}}_{i\left(k\right)}-{\stackrel{\‾}{x}}_{i\left(k\right)}){({\hat{x}}_{i\left(k\right)}-{\stackrel{\‾}{x}}_{i\left(k\right)})}^T-{\hat{P}}_{i\left(k\right)}---\left(22\right)$

其中，K为在滤波器收敛后要采样的点数，k₀为对

采样的起始采样点，

为采样中第K时刻子滤波器i的输出信息，为K个采样点的均值，

为

相对应K时刻子滤波器i的误差方差阵输出信息。

4.根据权利要求1所述的基于滤波器方差阵修正的多源信息非等间隔联邦滤波方法，其特征在于，步骤四中所述非等间隔修正算法具体如下所示：

当多个传感器均有新的量测信息时，各个子滤波器进行卡尔曼滤波，过程如下：

${\hat{X}}_{i(k/k-1)}={\mathrm{\φ}}_{k,k-1}{\hat{X}}_{i(k-1)}---\left(23\right)$

$P_{i(k/k-1)}={\mathrm{\φ}}_{k,k-1}{P}_{i(k-1)}{\mathrm{\φ}}_{k,k-1}^T+{\mathrm{\Γ}}_{k-1}{Q}_{k-1}{\mathrm{\Γ}}_{k-1}^T---\left(24\right)$

$K_{i\left(k\right)}=P_{i(k/k-1)}{H}_{i\left(k\right)}^T{[H_{i\left(k\right)}{P}_{i(k/k-1)}{H}_{i\left(k\right)}^T+R_{i\left(k\right)}]}^{-1}---\left(25\right)$

${\hat{X}}_{i\left(k\right)}={\hat{X}}_{i(k/k-1)}+K_{i\left(k\right)}[Z_{i\left(k\right)}-H_{i\left(k\right)}{\hat{X}}_{i(k/k-1)}]---\left(26\right)$

$P_{i\left(k\right)}=[I-K_{i\left(k\right)}{H}_{i\left(k\right)}]P_{i(k/k-1)}---\left(27\right)$

其中，Z_i(k)为第i子滤波器第k时刻的量测信息矩阵；

各子传感器不全有新的量测信息，若到达量测输出时刻，则对应子滤波器则利用式（26）到式（27）进行卡尔曼滤波；若未到达量测输出时刻，则利用式（28）到式（29）状态转移矩阵进行时间更新；

主滤波器利用各子滤波器的信息进行处理，过程如下：

${\hat{X}}_{i(k/k-1)}={\mathrm{\φ}}_{k/k-1}{\hat{X}}_{i(k-1)}---\left(28\right)$

$P_{i(k/k-1)}={\mathrm{\φ}}_{k,k-1}{P}_{i(k-1)}{\mathrm{\φ}}_{k,k-1}^T+{\mathrm{\Γ}}_{k-1}{Q}_{k-1}{\mathrm{\Γ}}_{k-1}^T---\left(29\right)$

联邦滤波主滤波器算法实现步骤如下：

$P_{f\left(k\right)}^{-1}=\underset{i=1}{\overset{n}{\mathrm{\Σ}}}{P}_{i\left(k\right)}^{-1}---\left(30\right)$

${\hat{X}}_{f\left(k\right)}=P_{f\left(k\right)}\underset{i=1}{\overset{n}{\mathrm{\Σ}}}{P}_{i\left(k\right)}^{-1}{\hat{x}}_{i\left(k\right)}---\left(31\right).$

Images