CN109470251A - 一种用于组合导航系统中的部分反馈滤波方法 - Google Patents

Abstract

一种用于组合导航系统中的部分反馈滤波方法，包括GPS接收机、惯性导航系统和组合滤波器，步骤包括：将GPS接收机和惯性导航系统接收到速度及位置信息的误差作为卡尔曼滤波估计值，组合滤波器将卡尔曼滤波估计值乘以数因子a后再反馈给惯性导航系统进行参数校正，惯性导航系统进行初始姿态矩阵和四元数之间的转换，获得初始四元数矩阵，组合滤波器输出校正惯性导航系统的初始四元数矩阵计算出组合导航的输出结果。本发明使得导航系统位置输出结果精度提高，还对卡尔曼滤波方差阵进行了限制，使得输出结果不会出现失真现象，具有更强的适应性。

Description

一种用于组合导航系统中的部分反馈滤波方法

技术领域

本发明涉及导航算法技术领域，具体涉及一种用于组合导航系统中的部分反馈滤波方法。

背景技术

组合导航算法是组合导航系统中的核心技术，不仅决定了整个导航系统的功能能否实现，而且对整个组合导航系统的性能起着决定性作用。

组合导航算法，一般是使用卡尔曼滤波，从概率统计最优的角度估算出系统误差并消除，组合导航过程中需将卡尔曼滤波的状态估计值不断反馈给惯导系统，进行惯导修正。传统的反馈方法是：选定某些状态参与反馈，将卡尔曼滤波器中的这些状态的估计值一次性全部反馈给惯导系统，修正惯导参数，再将滤波器中所有参与反馈的状态清零。但如果卡尔曼滤波器建模不准或者出现干扰时，滤波器估计难以实现实时调整，容易出现方差阵严重失真甚至溢出，最终导致滤波失效，失去组合导航功能。

发明内容

为解决上述现有技术的缺陷，本发明提供一种用于组合导航系统中的部分反馈滤波方法。

本发明的技术方案是：一种用于组合导航系统中的部分反馈滤波方法，包括GPS接收机、惯性导航系统和组合滤波器，GPS接收机和惯性导航系统连接组合滤波器，实现组合导航系统中的部分反馈滤波，包括如下步骤：

S1.由GPS接收机和惯性导航系统分别接受各自的速度及位置信息，将GPS接收机和惯性导航系统的速度及位置信息误差作为卡尔曼滤波估计值；

S2.根据卡尔曼滤波估计值设系数因子a，组合滤波器将卡尔曼滤波估计值乘以系数因子a后再反馈给惯性导航系统进行参数校正；

S3.通过惯性导航系统进行初始姿态矩阵和四元数之间的转换，获得初始四元数矩阵。

S4.组合滤波器输出校正惯性导航系统的初始四元数矩阵计算出组合导航的输出结果。

优选的，组合滤波器将卡尔曼滤波估计值乘以(1-a)后，作为下一周期卡尔曼滤波的初始值。

本发明的有益效果：组合滤波器将卡尔曼滤波估计值乘以系数因子a后再反馈给惯性导航系统进行参数校正，使得导航系统位置输出结果精度提高，还对卡尔曼滤波方差阵进行了限制，使得输出结果不会出现失真现象，具有更强的适应性。

附图说明

图1为组合导航系统框图。

具体实施方式

为使本发明要解决的技术问题、技术方案和优点更加清楚，下面将结合附图及具体实施例进行详细描述。

本发明技术方案如下：一种用于组合导航系统中的部分反馈滤波方法，包括GPS接收机、惯性导航系统和组合滤波器，GPS接收机和惯性导航系统连接组合滤波器，实现组合导航系统中的部分反馈滤波，包括如下步骤：

S1.由GPS接收机和惯性导航系统分别接受各自的速度及位置信息，将GPS接收机和惯性导航系统的速度及位置信息误差作为卡尔曼滤波估计值；

S2.根据卡尔曼滤波估计值设系数因子a，组合滤波器将卡尔曼滤波估计值乘以系数因子a后再反馈给惯性导航系统进行参数校正；

S3.通过惯性导航系统进行初始姿态矩阵和四元数之间的转换，获得初始四元数矩阵。

S4.组合滤波器输出校正惯性导航系统的初始四元数矩阵计算出组合导航的输出结果。

组合滤波器将卡尔曼滤波估计值乘以(1-a)后，作为下一周期卡尔曼滤波的初始值。

具体实施例：组合导航系统是采用线性卡尔曼滤波对观测量进行的滤波处理，需要建立组合导航的卡尔曼滤波状态转移方程和量测方程。

(1)状态转移方程

X＝[φ_e φ_n φ_u δV_e δV_n δV_u δL δλ δH ε_x ε_y ε_z ▽_x ▽_y ▽_z]^T

取惯性导航输出参数的误差作为状态估计量，参数分别为：3个惯性导航平台角误差，3个接收机速度误差，3个接收机位置误差，3个陀螺仪三轴零偏误差和3个加速度计三轴零偏误差，共15个状态估计参数。

(2)量测方程

在组合导航中，通过将GPS接收机给出的定位，定速结果与惯性导航系统定位，定速结果的差值作为卡尔曼滤波器的观测向量。

在GPS接收机中，位置信息可表示为：

惯性导航系统位置信息可表示为：

在上式中λ_t，L_t，h_t为载体位置由经纬高表示的真实值；N_E，N_N，N_U为GPS接收机分别沿东北天方向的位置误差，为了统一惯性导航系统量纲，需要将N_E和N_N进行转换，并定义位置量测方程为：

其中：

H_p＝[0_3×6 diag[(R_e+h) (R_e+h)cosL 1] 0_3×9]

V_p＝[N_N N_E N_U]^T

GPS接收机速度信息可表示为：

惯性导航系统速度信息可表示为：

在上式中，V_E，V_N，V_U分别表示为载体沿东北天坐标系各轴的速度真实值；

M_E，M_N，M_U分别为GPS接收机测速误差，则速度量测方程为:

其中：

H_v＝[0_3×3 diag[1 1 1] 0_3×12]

V_v＝[M_E M_N M_U]^T

则由此可知组合导航量测方程为：

(3)卡尔曼滤波

在建立状态转移方程和观测方程后，则可利用前一时刻的估计量X_K-1和当前时刻观测量Z_K，通过迭代方式进行下一个时刻的系统状态估计量X_K的计算。组合导航系统Kalman滤波算法求解过程如下：

一步状态预测：

一步误差方差矩阵预测：

滤波增益矩阵：

估计误差方差矩阵：

P_k＝[I-K_k·H_k]·P_k，k-1

以上即为组合导航系统Kalman滤波算法求解过程，在给定初始系统状态和初始状态误差方差矩阵后就可以通过递推计算得到K时刻的状态估计值

(4)部分反馈滤波方法

在经过递推得到K时刻的状态估计值后，组合滤波器将卡尔曼滤波估计值乘以系数因子a后再反馈给惯性导航系统进行参数校正，再将卡尔曼滤波估计值乘以(1-a)后，再反馈给原状态。

以速度误差的状态反馈修正为例，部分反馈方法表示为：

其中为惯导更新算法的速度，v_n为优化算法修正后的速度输出，为卡尔曼滤波器的速度误差估计值，为经过优化算法修正后的剩余估计值，可用于后续的部分反馈修正或下一步的滤波初值；a为计算修正比例的系数因子。

组合导航方法还包含对卡尔曼方差矩阵进行限制，防止出现失真现象。表示为：

如果P_K(i,j)＜m，则P_K(i,j)＝m，m为失真界限值。

以上所述仅为本发明的较佳实施例，并非用于限定本发明的保护范围。凡在本发明的精神和原则之内所作的任何修改、等同替换、改进等，均包含在本发明的保护范围内。

Claims (2)

1.一种用于组合导航系统中的部分反馈滤波方法，其特征在于：包括GPS接收机、惯性导航系统和组合滤波器，GPS接收机和惯性导航系统连接组合滤波器，实现组合导航系统中的部分反馈滤波，包括如下步骤：

S1.由GPS接收机和惯性导航系统分别接受各自的速度及位置信息，将GPS接收机和惯性导航系统的速度及位置信息误差作为卡尔曼滤波估计值；

S2.根据卡尔曼滤波估计值设系数因子a，组合滤波器将卡尔曼滤波估计值乘以系数因子a后再反馈给惯性导航系统进行参数校正；

S3.通过惯性导航系统进行初始姿态矩阵和四元数之间的转换，获得初始四元数矩阵。

S4.组合滤波器输出校正惯性导航系统的初始四元数矩阵计算出组合导航的输出结果。

2.根据权利要求1所述的一种用于组合导航系统中的部分反馈滤波方法，其特征在于：组合滤波器将卡尔曼滤波估计值乘以(1-a)后，作为下一周期卡尔曼滤波的初始值。