CN112556721B - 导航装置滤波器的随机误差的标定方法及系统 - Google Patents

Abstract

本发明提供一种导航装置滤波器的随机误差的标定方法及系统。所述方法包括：构建导航装置的误差模型，所述误差模型包括惯性器件的随机误差的微分方程；根据所述惯性器件的随机误差的微分方程，构建状态方程，且所述惯性器件的随机误差为所述状态方程的状态变量；根据已知的状态变量，构建观测方程；设置运动路径，并对所述运动路径进行可观测分析，以得到可观测结果；设置模糊控制块用于检测观测噪声；根据与所述模糊控制块相对应的理论方差矩阵的迹和实际测量方差矩阵的迹的比值，构建模糊规则；根据状态方程、观测方程、可观测结果和模糊规则，在所述运动路径中标定出所述滤波器的随机误差。本发明能够提高滤波器的随机误差的标定精度。

Description

导航装置滤波器的随机误差的标定方法及系统

技术领域

本发明涉及导航控制技术领域，尤其涉及一种导航装置滤波器的随机误差的标定方法及系统。

背景技术

随着航空航天技术的飞速发展，现在对飞行器气动力性能指标提出了越来越高的要求，准确预测空气动力特性是设计高性能飞行器控制系统的基础和前提，而气动参数在飞行器运动过程中是随着运动环境和条件的变化而变化的，并且在实际飞行全过程中控制系统本身以及飞行条件的复杂性、传感器及数据采集的非理想性、飞行试验设计的不完善性等因素的影响，使得飞行器系统模型中的气动参数存在偏差，而这种偏差必然会体现在飞行器的控制参数中，因此在实践中通常需要凭借惯性导航系统对可测的控制参数进行正确的估计。

现有的MEMS惯性导航系统在线标定中，通常采用基于Kalman的滤波方法对陀螺和加速计的随机误差进行在线估计，这种方法在外部环境影响较小的情况下效果是比较好的，但是在实际在线标定中，惯性导航系统很容易受到多种外部环境的干扰而导致观测噪声发生变化，从而影响滤波器性能，导致标定出的随机误差精度下降，进而降低MEMS惯性导航系统的导航性能。

发明内容

为解决上述问题，本发明提供的导航装置滤波器的随机误差的标定方法及系统，能够提高滤波器的随机误差的标定精度。

第一方面，本发明提供一种导航装置滤波器的随机误差的标定方法，包括：

构建导航装置的误差模型，所述误差模型包括惯性器件的随机误差的微分方程；

根据所述惯性器件的随机误差的微分方程，构建状态方程，且所述惯性器件的随机误差为所述状态方程的状态变量；

根据已知的状态变量，构建观测方程；

设置运动路径，并对所述运动路径进行可观测分析，以得到可观测结果；

设置模糊控制块用于检测观测噪声；

根据与所述模糊控制块相对应的理论方差矩阵的迹和实际测量方差矩阵的迹的比值，构建模糊规则；

根据状态方程、观测方程、可观测结果和模糊规则，在所述运动路径中标定出所述滤波器的随机误差。

可选地，在所述根据与所述模糊控制块相对应的理论方差矩阵的迹和实际测量方差矩阵的迹的比值，构建模糊规则之前，所述方法还包括：

获取滤波器的残差量测方差和残差理论方差；

对所述残差量测方差进行平滑处理，以得到平滑新息量测方差；

根据所述平滑新息量测方差，确定与所述模糊控制块相对应的实际测量方差矩阵；

根据所述残差理论方差，确定与所述模糊控制块相对应的理论方差矩阵。

可选地，所述述根据与所述模糊控制块相对应的理论方差矩阵的迹和实际测量方差矩阵的迹的比值，构建模糊规则，包括：

在所述比值在第一范围内的情况下，根据预设的第一计算公式计算出观测噪声的修正权值；

在所述比值在第二范围内的情况下，根据预设的第二计算公式计算出观测噪声的修正权值；

在所述比值在第三范围内的情况下，根据预设的第三计算公式计算出观测噪声的修正权值。

可选地，所述第一计算公式为q＝1+0.5·N；

所述第二计算公式为q＝1+0.3·(1-N)·sign(G-1)；

所述第三计算公式为q＝1-0.5·N；

其中，q为所述修正权值，N为根据输入隶属度函数得到的隶属模糊度且N∈(0,1)，G为所述比值。

可选地，所述第一范围为(1.5,2]；

所述第二范围为(0.5,1.5]；

所述第三范围为(0,0.5]。

可选地，在所述根据状态方程、观测方程、可观测结果和模糊规则，在所述运动路径中标定出所述导航装置滤波器的随机误差之后，所述方法还包括：

根据所述标定出的所述滤波器的随机误差，对所述滤波器进行状态值更新和协方差更新，以实时标定出所述滤波器的随机误差。

可选地，所述根据所述标定出的所述滤波器的随机误差，对所述滤波器进行状态值更新和协方差更新，以实时标定出所述滤波器的随机误差，包括：

根据状态矩阵和k时刻状态变量的值，进行状态一步预测，以确定k+1时刻状态变量的预测值；

根据k+1时刻状态变量的预测值、k+1时刻的滤波器的增益、k+1时刻的观测值和观测矩阵，进行状态值更新，以确定k+1时刻状态变量的估计值；

根据k+1时刻的预测值、观测矩阵和观测噪声均方差，进行滤波器的增益的更新，以确定k+1时刻的滤波器的增益；

根据状态矩阵、k时刻的协方差和状态噪声均方差，进行协方差一步预测，以确定k+1时刻协方差的预测值；

根据k+1时刻的滤波器的增益、观测矩阵和k+1时刻协方差的预测值，进行协方差的更新，以确定k+1时刻协方差的估计值；

根据k+1时刻状态变量的预测值、k+1时刻状态变量的估计值、k+1时刻协方差的预测值和k+1时刻协方差的估计值，标定出所述滤波器k+1时刻的随机误差。

可选地，在所述根据观测方程、状态方程和模糊规则，在所述运动路径中标定出所述导航装置滤波器的随机误差之前，所述方法还包括：

对所述状态方程进行离散化处理。

第二方面，本发明提供一种导航装置滤波器的随机误差的标定系统，包括：

第一构建模块，被配置为构建导航装置的误差模型，所述误差模型包括惯性器件的随机误差的微分方程；

第二构建模块，被配置为根据所述惯性器件的随机误差的微分方程，构建状态方程，且所述惯性器件的随机误差为所述状态方程的状态变量；

第三构建模块，被配置为根据已知的状态变量，构建观测方程；

第一设置模块，被配置为设置运动路径，并对所述运动路径进行可观测分析，以得到可观测结果；

第二设置模块，被配置为设置模糊控制块用于检测观测噪声；

第四构建模块，被配置为根据与所述模糊控制块相对应的理论方差矩阵的迹和实际测量方差矩阵的迹的比值，构建模糊规则；

标定模块，被配置为根据状态方程、观测方程、可观测结果和模糊规则，在所述运动路径中标定出所述滤波器的随机误差。

可选地，所述系统还包括：

获取模块，被配置为在所述根据与所述模糊控制块相对应的理论方差矩阵的迹和实际测量方差矩阵的迹的比值，构建模糊规则之前，获取滤波器的残差量测方差和残差理论方差；

平滑处理模块，被配置为对所述残差量测方差进行平滑处理，以得到平滑新息量测方差；

第一确定模块，被配置为根据所述平滑新息量测方差，确定与所述模糊控制块相对应的实际测量方差矩阵；

第二确定模块，被配置为根据所述残差理论方差，确定与所述模糊控制块相对应的理论方差矩阵。

本发明实施例提供的导航装置滤波器的随机误差的标定方法及系统，其中所述方法通过增加模糊规则，能够使滤波器稳定的标定出惯性导航装置的随机误差，即随机零偏，进而能够使惯性导航装置进行准确的估计，并快速的逼近真实值。

附图说明

图1为本申请实施例的导航装置滤波器的随机误差的标定方法示意性流程图；

图2为本申请实施例的导航装置滤波器的随机误差的标定原理图；

图3为本申请实施例的Kalman滤波器原理图；

图4为本申请实施例的导航装置滤波器的随机误差的标定系统的示意性结构图。

具体实施方式

为使本发明实施例的目的、技术方案和优点更加清楚，下面将结合本发明实施例中的附图，对本发明实施例中的技术方案进行清楚、完整地描述，显然，所描述的实施例仅仅是本发明一部分实施例，而不是全部的实施例。基于本发明中的实施例，本领域普通技术人员在没有做出创造性劳动前提下所获得的所有其他实施例，都属于本发明保护的范围。

第一方面，本发明提供一种导航装置滤波器的随机误差的标定方法，应用于MEMS惯性导航系统中的Kalman滤波器，参见图1，图1示出了根据本申请一实施例的所述方法示意性流程图，包括：

步骤S101：构建导航装置的误差模型，所述误差模型包括惯性器件的随机误差的微分方程。

具体的，惯性器件的随机误差的微分方程包括：误差模型系统平台误差角为：

速度误差为：

和位置误差为：

并包含随机误差在内的陀螺和加速计的误差参数模型为：

其中，

表示平台误差角，δV表示速度误差，ε和

为陀螺和加计的随机零偏，

表示载体坐标系到导航坐标系的转换矩阵，b表示载体坐标系，n表示导航坐标系，δL表示纬度误差、δλ表示经度误差、δh表示高度误差，ε^b和

为陀螺仪和加计载体坐标系下的随机零偏。

步骤S102：根据所述惯性器件的随机误差的微分方程，构建状态方程，且所述惯性器件的随机误差为所述状态方程的状态变量。

MEMS惯性导航系统误差建模的状态变量X为：

其中，

和

为系统平台三个方向的误差角，δV_E、δV_N和δV_U为系统三个方向的速度误差，E、N和U表示导航坐标系下的东、北、上三个方向，δL、δλ和δh为系统的纬度、经度、高度误差，ε_bx、ε_by和ε_bz为陀螺的三个不同方面的随机零偏，

和

为加速计三个不同方面的随机零偏。

建立的MEMS惯性导航系统状态方程为：

其中，A为状态矩阵，W为状态噪声，观测矩阵A为：

其中，

步骤S103：根据已知的状态变量，构建观测方程。

采用惯性导航系统解算的航向角、速度、位置信息与参考航向角、GPS速度、位置的差值作为观测量，观测变量为：

其中，V_S、P_S和ψ_S为惯性导航解算的速度、位置及航向角，V_G、P_G和ψ_G为参考的速度、位置及航向角。则建立MEMS惯性导航系统观测方程为：z(t)＝HX(t)+V(t)，其中，H为观测方程，V为观测噪声。

观测方程：H＝[H1；H2]，其中，H1＝[H11 H12 0_1×10]；H11＝-T(1，2)T(3，2)/(T(1，2)²+T(2，2)²；H12＝-T(3，2)T(2，2)/(T(1，2)²+T(2，2)²；

其中，系统参数和已知的状态变量可由GPS(Global Positioning System，全球定位系统)获取。

步骤S104：设置运动路径，并对所述运动路径进行可观测分析，以得到可观测结果。

步骤S105：设置模糊控制块用于检测观测噪声。

步骤S106：根据与所述模糊控制块相对应的理论方差矩阵的迹和实际测量方差矩阵的迹的比值，构建模糊规则。

步骤S107：根据状态方程、观测方程、可观测结果和模糊规则，在所述运动路径中标定出所述滤波器的随机误差。

在本实施例中，设置四种运动路径，包括：指北方位匀速直线运动，速度为20m/s；加速直线运动，加速度为0.5m/s^2；拐弯运动，转弯角速率为0.1rad/s；圆周运动，转速为1rad/s。之后，对这四种路径进行基于奇异值的可观测分析，分析出不同路径对陀螺和加计随机零偏的激励大小，选出合适的路径来标定滤波器的随机误差。

其中，可观测分析的步骤是：首先，建立TOM矩阵；然后根据TOM矩阵建立SOM矩阵，将SOM奇异值分解，得到各个状态变量对应的奇异值，则奇异值的大小表示各观测性的大小。

如根据可观测结果，在匀速直线运动状态下陀螺x，y轴的随机零偏可观测性的数值大于1，因此在该路径下可以标定出陀螺x，y轴的随机零偏。同理，加速直线运动下可标定出加速计x，y轴的随机零偏，圆周运动下标定出陀螺z轴的随机零偏，拐弯运动下加速计的z轴的随机零偏。

在一种可选的实施例中，在所述根据观测方程、状态方程和模糊规则，在所述运动路径中标定出所述导航装置滤波器的随机误差之前，所述方法还包括：

对所述状态方程进行离散化处理。

在本实施中。所述离散化的状态方程和所述观测方程如下：

其中，X_k∈Rⁿ；Z_k∈R^m；F_j∈R^n×n；H_j∈R^m×n；F_j和H_j均为常值矩阵.则线性系统的可观测性矩阵(TOM，Total Observability Matrix)为：

其中，

Q_j为线性时变系统中第j时间段nmxn阶的可观测性矩阵。同时，时变系统的可观测性矩阵(SOM，Stripped Observability Matrix)为：Q_s(r)＝[Q₁Q₂ … Q_r]^T。奇异值的分解如下：

存在矩阵A∈R^m×n，那么存在正交矩阵U₁∈R^m×m和U₂∈R^n×n，使得A＝U₁∑U₂ ^T，其中，

且对角元素等于s＝diag(σ₁，…，σ_r)为对角阵，σ₁≥σ₂≥…≥σ_r≥0顺序排列。

所述方法通过增加模糊规则，能够使滤波器稳定的标定出惯性导航装置的随机误差，即随机零偏，进而能够使惯性导航装置进行准确的估计，并快速的逼近真实值。

在一种可选的实施例中，所述述根据与所述模糊控制块相对应的理论方差矩阵的迹和实际测量方差矩阵的迹的比值，构建模糊规则，包括：

在所述比值在第一范围内的情况下，根据预设的第一计算公式计算出观测噪声的修正权值。

在所述比值在第二范围内的情况下，根据预设的第二计算公式计算出观测噪声的修正权值。

在所述比值在第三范围内的情况下，根据预设的第三计算公式计算出观测噪声的修正权值。

在一种可选的实施例中，所述第一计算公式为q＝1+0.5·N。

所述第二计算公式为q＝1+0.3·(1-N)·sign(G-1)。

所述第三计算公式为q＝1-0.5·N。

其中，q为所述修正权值，N为根据输入隶属度函数得到的隶属模糊度且N∈(0，1)，G为所述比值。

在一种可选的实施例中，所述第一范围为(1.5，2]。

所述第二范围为(0.5，1.5]。

所述第三范围为(0，0.5]。

在一种可选的实施例中，在所述根据状态方程、观测方程、可观测结果和模糊规则，在所述运动路径中标定出所述导航装置滤波器的随机误差之后，所述方法还包括：

根据所述标定出的所述滤波器的随机误差，对所述滤波器进行状态值更新和协方差更新，以实时标定出所述滤波器的随机误差。

在一种可选的实施例中，所述根据所述标定出的所述滤波器的随机误差，对所述滤波器进行状态值更新和协方差更新，以实时标定出所述滤波器的随机误差，包括：

根据状态矩阵和k时刻状态变量的值，进行状态一步预测，以确定k+1时刻状态变量的预测值。

根据k+1时刻状态变量的预测值、k+1时刻的滤波器的增益、k+1时刻的观测值和观测矩阵，进行状态值更新，以确定k+1时刻状态变量的估计值。

根据k+1时刻的预测值、观测矩阵和观测噪声均方差，进行滤波器的增益的更新，以确定k+1时刻的滤波器的增益。

根据状态矩阵、k时刻的协方差和状态噪声均方差，进行协方差一步预测，以确定k+1时刻协方差的预测值。

根据k+1时刻的滤波器的增益、观测矩阵和k+1时刻协方差的预测值，进行协方差的更新，以确定k+1时刻协方差的估计值。

根据k+1时刻状态变量的预测值、k+1时刻状态变量的估计值、k+1时刻协方差的预测值和k+1时刻协方差的估计值，标定出所述滤波器k+1时刻的随机误差。

具体的，所述状态一步预测的方程为：

所述状态值更新的方程为：

所述滤波器的增益的更新方程为：K_k+1＝P_k+1/kH^T[HP_k+1/kH^T+R]^-1；

所述协方差一步预测的方程为：

所述协方差的更新方程为：P_k+1/k+1＝[I_n-K_k+1H]P_k+1/k。

其中，A为状态矩阵，H为观测矩阵，Q为状态噪声均方差，R为观测噪声均方差，X_k/k为k时刻状态变量的值，

为k+1时刻状态变量的预测值，

为k+1时刻状态变量的估计值，z_k+1为k+1时刻的观测值，P_k/k为k时刻的协方差值，P_k+1/k为k+1时刻协方差的预测值，K_k+1为k+1时刻的滤波器的增益，P_k+1/k+1为k+1时刻协方差的估计值。

在本实施例中，所述模块控制模块的数量为两个。在一种可选的实施例中，在所述根据与所述模糊控制块相对应的理论方差矩阵的迹和实际测量方差矩阵的迹的比值，构建模糊规则之前，所述方法还包括：获取滤波器的残差量测方差和残差理论方差；对所述残差量测方差进行平滑处理，以得到平滑新息量测方差；根据所述平滑新息量测方差，确定与所述模糊控制块相对应的实际测量方差矩阵。

根据所述残差理论方差，确定与所述模糊控制块相对应的理论方差矩阵。

具体的，用两个模糊控制块来监测观测噪声，该两个模块为对应理论方差与实测方差迹的比值G₁、G₂作为模糊推理的输入，观测噪声的修正权值q_ψV，q_P作为输出，当观测噪声的统计特性准确时，G₁，G₂的值在1附近，当G₁、G₂大于1时，则系统的理论方差大于实测方差，应该增大权值q_ψV，q_P，使G₁、G₂接近于1，同理，当系统理论方差小于实测方差时，应当减小G₁、G₂。具体步骤如下：首先是求取系统的实测方差和理论方差，新息ε被称为残差向量，它反映了滤波模型的准确性，则残差的量测方差为：C_k＝ε_k·ε_k ^T，其中，ε_k为k时刻所述系统的新息；C_k为k时刻所述残差的量测方差。

由于Kalman滤波中观测噪声是由简单的统计特性得到的，但观测噪声经常受到外部环境的影响，容易导致滤波发散，因此需要对以上量测差进行数据平滑操作，得到平滑新息测量方差为：

其中，d为遗忘因子，b为遗忘因子中的参数，通常b的取值为[0.95，0.995]，d_k为k时刻系统的遗忘因子，b^k+1为k时刻系统的遗忘因子中的参数，ε_k为k时刻系统的新息。

定义残差的理论方差为：S_k＝H_k(A_kP_kA_k ^T+Q_k)H_k ^T+R_k；

由于本发明将观测信息分为两个模块，则残差的量测方差C_k和理论方差S_k可变形为：

其中，C_ψV和S_ψV分别为姿态和速度的

实测方差与理论方差；C_P、S_P分别为位置的实测方差与理论方差；C1、C2、S1和S2可为任意值，在本实施例中C1、C2、S1和S2均为0。那么残差的理论方差与实测方差的迹的比值为：

其中，C_ψV和S_ψV分别为姿态和速度的实测方方差与理论方差，C_P、S_P分别为位置的实测方差与理论方差。G₁、G₂作为模糊推理的输入，通过以上的模糊规则可以求得基于两个权值q_ψV、q_Pd的修正值R_k，修正过程如下式：

其中，R_ψV为k时刻姿态和速度的观测噪声，R_P为k时刻位置的观测噪声，q_ψV为k时刻R_ψV的修正值，q_P为k时刻R_P的修正值。

第二方面，本发明提供一种导航装置滤波器的随机误差的标定方法，应用于MEMS惯性导航系统中的Kalman滤波器，参见图2和图3，图2示出了根据本申请一实施例的所述导航装置滤波器的随机误差的标定原理图，图3示出了根据本申请一实施例的所述Kalman滤波器原理图。

所述标定方法包括：首先，设计载体的四种机动方式，分别是匀速直线运动、加速直线运动、拐弯运动运动和圆周运动；其次，由预设的IMU误差模型对这四种路径进行可观测分析，分析出这四种路径对各个误差参数的激励大小；之后，建立系统误差模型，其中系统误差模型由惯性导航系统解算的结果来表示；接着，通过系统误差模型建立状态方程，GPS的观测信息即外测信息作为观测变量建立观测方程；然后，进行Kalman滤波估计，并将估计出的参数即IMU误差的值和导航误差的值为标定的值；最后，将所述标定的值反馈到系统误差模型的输入，用以提高导航的精度。

在本发明中，Kalman滤波的过程是根据初始状态值计算下一个时刻状态值的预测值，根据初始协方差值计算下一个时刻协方差预测值，然后根据协方差预测值和观测值计算新息，由新息值计算Kalman增益，然后由Kalman增益和观测值更新状态值，以及更新协方差值，由更新的状态值和协方差值和状态值作为下一个时刻初始值，这样就形成了一个循环。其中，在上述Kalman滤波的基础上制定了模糊规则，用两个模糊控制块来监测观测噪声，并将该两个模糊控制块所对应的理论方差与实测方差的迹的比值作为模糊推理的输入，观测噪声的修正权值作为输出。

当观测噪声的统计特性准确时，理论方差与实测方差迹的比值在1附近，当系统的测量噪声的实际值与给定值不一致时，比值将偏离1。因此，当比值大于1时，则系统的理论方差大于实测方差，应该增大修正权值，使比值接近于1，同理，当系统理论方差小于实测方差时，应当减小比值。如此通过监测比值，实现对观测噪声的实时调节，从而提高所述系统的导航精度。

第三方面，本发明提供一种导航装置滤波器的随机误差的标定系统，图4示出了根据本申请一实施例的所述系统的示意性结构图，所述导航装置滤波器的随机误差的标定系统300包括：

第一构建模块301，被配置为构建导航装置的误差模型，所述误差模型包括惯性器件的随机误差的微分方程。

第二构建模块302，被配置为根据所述惯性器件的随机误差的微分方程，构建状态方程，且所述惯性器件的随机误差为所述状态方程的状态变量。

第三构建模块303，被配置为根据已知的状态变量，构建观测方程。

第一设置模块304，被配置为设置运动路径，并对所述运动路径进行可观测分析，以得到可观测结果。

第二设置模块305，被配置为设置模糊控制块用于检测观测噪声。

第四构建模块306，被配置为根据与所述模糊控制块相对应的理论方差矩阵的迹和实际测量方差矩阵的迹的比值，构建模糊规则。

第一标定模块307，被配置为根据状态方程、观测方程、可观测结果和模糊规则，在所述运动路径中标定出所述滤波器的随机误差。

在一种可选的实施例中，所述系统还包括：

获取模块，被配置为在所述根据与所述模糊控制块相对应的理论方差矩阵的迹和实际测量方差矩阵的迹的比值，构建模糊规则之前，获取滤波器的残差量测方差和残差理论方差。

平滑处理模块，被配置为对所述残差量测方差进行平滑处理，以得到平滑新息量测方差。

第一确定模块，被配置为根据所述平滑新息量测方差，确定与所述模糊控制块相对应的实际测量方差矩阵。

第二确定模块，被配置为根据所述残差理论方差，确定与所述模糊控制块相对应的理论方差矩阵。

在一种可选的实施例中，所述述根据与所述模糊控制块相对应的理论方差矩阵的迹和实际测量方差矩阵的迹的比值，构建模糊规则，包括：

第一计算模块，被配置为在所述比值在第一范围内的情况下，根据预设的第一计算公式计算出观测噪声的修正权值；

第二计算模块，被配置为在所述比值在第二范围内的情况下，根据预设的第二计算公式计算出观测噪声的修正权值；

第三计算模块，被配置为在所述比值在第三范围内的情况下，根据预设的第三计算公式计算出观测噪声的修正权值。

在一种可选的实施例中，所述第一计算公式为q＝1+0.5·N；

所述第二计算公式为q＝1+0.3·(1-N)·sign(G-1)；

所述第三计算公式为q＝1-0.5·N；

其中，q为所述修正权值，N为根据输入隶属度函数得到的隶属模糊度且N∈(0,1)，G为所述比值。

在一种可选的实施例中，所述第一范围为(1.5,2]；

所述第二范围为(0.5,1.5]；

所述第三范围为(0,0.5]。

在一种可选的实施例中，所述系统还包括：

第二标定模块，被配置为在所述根据状态方程、观测方程、可观测结果和模糊规则，在所述运动路径中标定出所述导航装置滤波器的随机误差之后，根据所述标定出的所述滤波器的随机误差，对所述滤波器进行状态值更新和协方差更新，以实时标定出所述滤波器的随机误差。

在一种可选的实施例中，所述第二标定模块包括：

第一确定子模块，被配置为根据状态矩阵和k时刻状态变量的值，进行状态一步预测，以确定k+1时刻状态变量的预测值；

第二确定子模块，被配置为根据k+1时刻状态变量的预测值、k+1时刻的滤波器的增益、k+1时刻的观测值和观测矩阵，进行状态值更新，以确定k+1时刻状态变量的估计值；

第三确定子模块，被配置为根据k+1时刻的预测值、观测矩阵和观测噪声均方差，进行滤波器的增益的更新，以确定k+1时刻的滤波器的增益；

第四确定子模块，被配置为根据状态矩阵、k时刻的协方差和状态噪声均方差，进行协方差一步预测，以确定k+1时刻协方差的预测值；

第五确定子模块，被配置为根据k+1时刻的滤波器的增益、观测矩阵和k+1时刻协方差的预测值，进行协方差的更新，以确定k+1时刻协方差的估计值；

标定子模块，被配置为根据k+1时刻状态变量的预测值、k+1时刻状态变量的估计值、k+1时刻协方差的预测值和k+1时刻协方差的估计值，标定出所述滤波器k+1时刻的随机误差。

在一种可选的实施例中，所述系统还包括：离散模块，被配置为在所述根据观测方程、状态方程和模糊规则，在所述运动路径中标定出所述导航装置滤波器的随机误差之前，对所述状态方程进行离散化处理。

本发明实施例提供的导航装置滤波器的随机误差的标定方法及系统，其中所述方法通过增加模糊规则，能够使滤波器稳定的标定出惯性导航装置的随机误差，即随机零偏，进而能够使惯性导航装置进行准确的估计，并快速的逼近真实值。

以上所述，仅为本发明的具体实施方式，但本发明的保护范围并不局限于此，任何熟悉本技术领域的技术人员在本发明揭露的技术范围内，可轻易想到的变化或替换，都应涵盖在本发明的保护范围之内。因此，本发明的保护范围应该以权利要求的保护范围为准。

Claims (7)

1.一种导航装置滤波器的随机误差的标定方法，其特征在于，包括：

构建导航装置的误差模型，所述误差模型包括惯性器件的随机误差的微分方程；

根据所述惯性器件的随机误差的微分方程，构建状态方程，且所述惯性器件的随机误差为所述状态方程的状态变量；

根据已知的状态变量，构建观测方程；

设置运动路径，并对所述运动路径进行可观测分析，以得到可观测结果；

设置模糊控制块用于检测观测噪声；

根据与所述模糊控制块相对应的理论方差矩阵的迹和实际测量方差矩阵的迹的比值，构建模糊规则；

所述根据与所述模糊控制块相对应的理论方差矩阵的迹和实际测量方差矩阵的迹的比值，构建模糊规则，包括：在所述比值在第一范围内的情况下，根据预设的第一计算公式计算出观测噪声的修正权值；在所述比值在第二范围内的情况下，根据预设的第二计算公式计算出观测噪声的修正权值；在所述比值在第三范围内的情况下，根据预设的第三计算公式计算出观测噪声的修正权值；所述第一计算公式为q＝1+0.5·N；所述第二计算公式为q＝1+0.3·(1-N)·sign(G-1)；所述第三计算公式为q＝1-0.5·N；其中，q为所述修正权值，N为根据输入隶属度函数得到的隶属模糊度且N∈(0,1)，G为所述比值，所述第一范围为(1.5,2]，所述第二范围为(0.5,1.5]，所述第三范围为(0,0.5]；

根据状态方程、观测方程、可观测结果和模糊规则，在所述运动路径中标定出所述滤波器的随机误差。

2.根据权利要求1所述的方法，其特征在于，在所述根据与所述模糊控制块相对应的理论方差矩阵的迹和实际测量方差矩阵的迹的比值，构建模糊规则之前，所述方法还包括：

获取滤波器的残差量测方差和残差理论方差；

对所述残差量测方差进行平滑处理，以得到平滑新息量测方差；

根据所述平滑新息量测方差，确定与所述模糊控制块相对应的实际测量方差矩阵；

根据所述残差理论方差，确定与所述模糊控制块相对应的理论方差矩阵。

3.根据权利要求1所述的方法，其特征在于，在所述根据状态方程、观测方程、可观测结果和模糊规则，在所述运动路径中标定出所述导航装置滤波器的随机误差之后，所述方法还包括：

根据所述标定出的所述滤波器的随机误差，对所述滤波器进行状态值更新和协方差更新，以实时标定出所述滤波器的随机误差。

4.根据权利要求3所述的方法，其特征在于，所述根据所述标定出的所述滤波器的随机误差，对所述滤波器进行状态值更新和协方差更新，以实时标定出所述滤波器的随机误差，包括：

根据状态矩阵和k时刻状态变量的值，进行状态一步预测，以确定k+1时刻状态变量的预测值；

根据k+1时刻状态变量的预测值、k+1时刻的滤波器的增益、k+1时刻的观测值和观测矩阵，进行状态值更新，以确定k+1时刻状态变量的估计值；

根据k+1时刻的预测值、观测矩阵和观测噪声均方差，进行滤波器的增益的更新，以确定k+1时刻的滤波器的增益；

根据状态矩阵、k时刻的协方差和状态噪声均方差，进行协方差一步预测，以确定k+1时刻协方差的预测值；

根据k+1时刻的滤波器的增益、观测矩阵和k+1时刻协方差的预测值，进行协方差的更新，以确定k+1时刻协方差的估计值；

根据k+1时刻状态变量的预测值、k+1时刻状态变量的估计值、k+1时刻协方差的预测值和k+1时刻协方差的估计值，标定出所述滤波器k+1时刻的随机误差。

5.根据权利要求1所述的方法，其特征在于，在所述根据观测方程、状态方程和模糊规则，在所述运动路径中标定出所述导航装置滤波器的随机误差之前，所述方法还包括：

对所述状态方程进行离散化处理。

6.一种导航装置滤波器的随机误差的标定系统，其特征在于，包括：

第一构建模块，被配置为构建导航装置的误差模型，所述误差模型包括惯性器件的随机误差的微分方程；

第二构建模块，被配置为根据所述惯性器件的随机误差的微分方程，构建状态方程，且所述惯性器件的随机误差为所述状态方程的状态变量；

第三构建模块，被配置为根据已知的状态变量，构建观测方程；

第一设置模块，被配置为设置运动路径，并对所述运动路径进行可观测分析，以得到可观测结果；

第二设置模块，被配置为设置模糊控制块用于检测观测噪声；

第四构建模块，被配置为根据与所述模糊控制块相对应的理论方差矩阵的迹和实际测量方差矩阵的迹的比值，构建模糊规则；

所述第四构建模块包括：第一计算模块，被配置为在所述比值在第一范围内的情况下，根据预设的第一计算公式计算出观测噪声的修正权值；第二计算模块，被配置为在所述比值在第二范围内的情况下，根据预设的第二计算公式计算出观测噪声的修正权值；第三计算模块，被配置为在所述比值在第三范围内的情况下，根据预设的第三计算公式计算出观测噪声的修正权值；所述第一计算公式为q＝1+0.5·N；所述第二计算公式为q＝1+0.3·(1-N)·sign(G-1)；所述第三计算公式为q＝1-0.5·N；其中，q为所述修正权值，N为根据输入隶属度函数得到的隶属模糊度且N∈(0,1)，G为所述比值，所述第一范围为(1.5,2]，所述第二范围为(0.5,1.5]，所述第三范围为(0,0.5]；

标定模块，被配置为根据状态方程、观测方程、可观测结果和模糊规则，在所述运动路径中标定出所述滤波器的随机误差。

7.根据权利要求6所述的系统，其特征在于，所述系统还包括：

获取模块，被配置为在所述根据与所述模糊控制块相对应的理论方差矩阵的迹和实际测量方差矩阵的迹的比值，构建模糊规则之前，获取滤波器的残差量测方差和残差理论方差；

平滑处理模块，被配置为对所述残差量测方差进行平滑处理，以得到平滑新息量测方差；

第一确定模块，被配置为根据所述平滑新息量测方差，确定与所述模糊控制块相对应的实际测量方差矩阵；

第二确定模块，被配置为根据所述残差理论方差，确定与所述模糊控制块相对应的理论方差矩阵。

Images