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

摘要

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

主权项

基于滤波器方差阵修正的多源信息非等间隔联邦滤波方法，其特征在于，包括以下步骤：步骤一、选取东北天地理坐标系，惯性导航系统误差状态量定义为：$X={[{\phi }_E,{\phi }_N,{\phi }_U,{\mathrm{\delta v}}_E,{\mathrm{\delta v}}_N,{\mathrm{\delta v}}_U,\delta L,\delta \lambda ,\delta h,{ϵ}_{bx},{ϵ}_{by},{ϵ}_{bz},{ϵ}_{rx},{ϵ}_{ry},{ϵ}_{rz},{▿}_x,{▿}_y,{▿}_z]}^T---\left(1\right)$其中，φ_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轴方向加速度计零偏，上标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={[{\phi }_E,{\phi }_N,{\phi }_U,{\mathrm{\delta v}}_E,{\mathrm{\delta v}}_N,{\mathrm{\delta v}}_U,\delta L,\delta \lambda ,\delta 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{def}{=}{x}_{i\left(k\right)}^r-{\hat{x}}_{i\left(k\right)}---\left(3\right)$$P_{i\left(k\right)}^P\stackrel{def}{=}E\left[{\tilde{x}}_{i\left(k\right)}^P{\tilde{x}}_{i\left(k\right)}^{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{\Delta P}}_i\\ P_{f\left(k\right)}^{-1}=\underset{i=1}{\overset{n}{\Sigma }}{P}_{i\left(k/k\right)}^{-1}\\ {\hat{x}}_{f\left(k\right)}=P_{f\left(k\right)}\left(\underset{i=1}{\overset{n}{\Sigma }}{P}_{i\left(k/k\right)}^{-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时刻滤波估计量；步骤四、采用无重置联邦滤波器非等间隔修正算法，将一个滤波周期内的卡尔曼滤波分为两个信息更新过程：时间更新和量测更新；各子滤波器在信息输出时，若未到量测时间则输出时间更新信息，若到量测时间时则输出量测更新信息；步骤五、采用联邦滤波器对步骤三中子滤波系统得出的滤波结果进行数据融合，输出全局最优估计值。