[发明专利]基于卡尔曼滤波估计方差约束的组合导航方法

权利要求书

1.基于卡尔曼滤波估计方差约束的组合导航方法，其特征在于，所述基于卡尔曼滤波估计方差约束的组合导航方法包括：

对惯导系统进行粗对准；

根据陀螺角速率和加速度计比力进行惯性导航解算，更新姿态四元数、位置和速度；建立系统误差模型；

选取卡尔曼滤波的量测量，设定卡尔曼滤波器初值；

进行卡尔曼滤波计算以获取组合导航数据直至组合导航结束，在卡尔曼滤波计算过程中，根据惯性器件误差约束卡尔曼滤波器的误差协方差取值。

2.根据权利要求1所述的基于卡尔曼滤波估计方差约束的组合导航方法，其特征在于，在卡尔曼滤波计算过程中，根据惯性器件误差约束卡尔曼滤波器的误差协方差取值包括：

根据惯性器件误差设定卡尔曼滤波器的误差协方差P_k+1/k+1的对角线元素的最大值P_Max和最小值P_min，其中，最大值P_Max和最小值P_min均为一维数组，维数与误差协方差P_k+1/k+1的对角线元素个数以及状态量的维数相同，最大值P_Max和最小值P_min中的各元素与状态量中的各变量一一对应，最大值P_Max中的各元素均为元素最大值，最小值P_min中的各元素均为元素最小值；

在进行卡尔曼滤波计算的量测更新过程中，若误差协方差P_k+1/k+1的对角线元素中的任一元素小于最小值P_min中对应的元素最小值，则将当前误差协方差P_k+1/k+1的对角线元素中的该元素设置为最小值P_min中对应的元素最小值；若误差协方差P_k+1/k+1的对角线元素中的任一元素大于最大值P_Max中对应的元素最大值，则将当前误差协方差P_k+1/k+1的对角线元素中的该元素设置为最大值P_Max中对应的元素最大值；

在进行卡尔曼滤波计算的时间更新过程中，若误差协方差P_k+1/k的对角线元素中的任一元素小于最小值P_min中对应的元素最小值，则将当前误差协方差P_k+1/k的对角线元素中的该元素设置为最小值P_min中对应的元素最小值；若误差协方差P_k+1/k的对角线元素中的任一元素大于最大值P_Max中对应的元素最大值，则将当前误差协方差P_k+1/k的对角线元素中的该元素设置为最大值P_Max中对应的元素最大值；

其中，P_k+1/k和P_k+1/k+1分别为k到k+1一步预测时刻、k+1量测更新时刻的误差协方差值，为k时刻系统状态量估计值，k为计算时刻，k＝1,2,...n，n为整数。

3.根据权利要求1或2所述的基于卡尔曼滤波估计方差约束的组合导航方法，其特征在于，基于卡尔曼滤波估计方差约束的组合导航方法可根据构建系统误差模型，其中，Q(k)为k时刻系统激励噪声序列，满足零均值白噪声且方差为Q_k，R(k)为k时刻量测噪声序列，满足零均值白噪声且方差为R_k，Z(k)为k时刻的系统量测量，H(k)为k时刻量测矩阵，Φ(k,k-1)为k-1至k时刻离散系统状态转移矩阵，X(k)、X(k-1)分别为k时刻与k-1时刻的系统的状态量，状态量δL、δh和δλ分别表示惯导系统的纬度误差、高度误差和经度误差，δV_N、δV_U、δV_E分别为惯导北、天、东速度误差，φ_N、φ_U、φ_E分别为惯导系统地理坐标系内北、天、东三个方向的失准角，和分别为x加速度计、y加速度计和z加速度计的加速度计零偏，ε_x、ε_y和ε_z分别为惯导系统测量坐标系内X、Y和Z三个方向的陀螺常值漂移。