[发明专利]一种飞行器的容错组合导航方法及系统

权利要求书

1.一种飞行器的容错组合导航方法，该方法为：获取主定位传感器的定位信息及各辅助定位传感器的定位信息；将主定位传感器定位信息和每个辅助定位传感器定位信息经过对应的局部滤波器进行滤波估计后获取相关状态向量，各局部滤波器输出的相关状态向量经过故障检测和隔离后，进行全局信息融合；其特征在于，局部滤波器采用自适应无迹卡尔曼滤波算法：将新息向量矩阵的迹与量测向量的预测误差方差矩阵的迹进行比较，若新息向量矩阵的迹大于量测向量的预测误差方差矩阵的迹时，采用构造的自适应因子对无迹卡尔曼滤波算法中的增益矩阵进行修正，所述自适应因子为量测向量的预测误差方差矩阵的迹与新息向量的迹的比值；

对增益矩阵进行修正是通过对量测预测方差阵进行修正及对状态向量与量测向量的交互协方差矩阵进行修正来实现的，增益矩阵K_k的公式为：

量测预测方差阵的修正公式为：

状态向量和量测向量的交互协方差矩阵的修正公式为：

其中，为量测预测方差阵，为量测预测方差阵的逆，2n为采样点个数，i表示采样点序号，k为时刻，为方差所对应的权值，为量测预测值，为引入渐消因子后量测预测值，T表示矩阵的转置，R_k为过程噪声协方差阵，μ_k为自适应因子；P_xz,k为状态向量和量测向量的交互协方差矩阵，为离散化后的状态向量，为状态预测值；

所述自适应无迹卡尔曼滤波算法是在强跟踪滤波与卡尔曼滤波相结合的滤波算法中引入自适应因子得到的，所述强跟踪滤波与卡尔曼滤波相结合的滤波算法具体为：

A)Sigma点选取：

式中：表示第j列均方根的值，α表示比例缩放因子；ξ_j,k+1/k为选取的Sigma点，为一步状态更新后状态预测值，为状态预测误差协方差矩阵，n为状态向量维数，j为矩阵的列数；

B)状态预测值与预测状态协方差阵的时间更新：

χ_i＝f(X_i)

式中：含有符号l的变量为引入渐消因子之前的变量；χ_i为离散化后的状态向量，X_i为状态向量，函数f(·)为离散化公式，为状态预测值，为引入渐消因子前的状态预测误差协方差矩阵，T为矩阵的转置，Q_k为观测噪声协方差矩阵；

式中为均值对应的权值，为方差对应的权值，计算公式为：

C)将次优渐消因子λ_k近似引入卡尔曼滤波算法中进行量测更新，引入次优渐消因子λ_k后的量测预测值量测协方差阵P_zz,k和互协方差阵P_xz,k的计算公式如下：

其中，表示经量测函数h(·)传递后得到的Sigma点集，为引入渐消因子前的状态向量的预测误差协方差矩阵，为增益矩阵的转置；

引入的次优渐消因子λ_k的计算方法如下：

式中：β表示可调节的参数弱化因子，β≥1；η_i为经量测函数h(·)传递后的Sigma点集；N_k为加入可调节的参数弱化因子的新息向量矩阵，V_k为第k时刻的新息向量的协方差阵，R_k过程噪声协方差阵，M_k为预测值协方差阵，C_k为两矩阵求迹后的比值，tr(·)为矩阵的求迹算子，tr(N_k)为新息向量矩阵的迹，tr(M_k)为预测值协方差阵的迹；

新息向量的协方差阵V_k的计算公式如下：

式中：γ_k表示新息向量的矩阵，ρ表示遗忘因子，z_k为量测值，表示引入渐消因子前的量测预测值，V_k-1为第k-1时刻的新息向量协方差阵；

引入次优渐消因子λ_k后新的预测状态协方差阵为：

引入λ_k后得到的相关状态向量进行量测更新：

式中：K_k、P_k分别表示增益矩阵、系统状态向量估计值及状态协方差阵，为量测协方差阵的逆，Q_k为观测噪声协方差矩阵。