[发明专利]一种具有协方差反馈控制的动态自适应导航定位方法

权利要求书

1.一种具有协方差反馈控制的动态自适应导航定位方法，其特征在于，包括以下步骤：

获取惯性导航系统的误差参数和陀螺仪与加速度计的器件误差参数，组合惯性导航系统的误差参数和陀螺仪与加速度计的器件误差参数作为北斗/惯性组合导航系统的状态量，组合状态量的误差模型获得北斗/惯性组合导航系统的状态模型；

采用北斗卫星导航系统和惯性导航系统之间的速度差和位置差，作为北斗/惯性组合导航系统的量测量，根据量测量与状态量之间线性关系，建立北斗/惯性组合导航系统的量测模型；

设置初始状态估计及其误差协方差矩阵对滤波器进行初始化，将北斗/惯性组合导航系统的状态量和量测量输入滤波器，滤波器基于状态模型执行容积卡尔曼滤波进行时间更新，获得北斗/惯性组合导航系统的先验状态及其误差协方差矩阵；

根据先验状态及其误差协方差矩阵，滤波器基于量测模型执行容积卡尔曼滤波进行量测更新，得到北斗/惯性组合导航系统的后验状态及其误差协方差矩阵；

将后验状态及其误差协方差矩阵反馈至滤波器中，滤波器基于极大似然准则，调整先验状态误差协方差矩阵，采用调整后的先验状态误差协方差矩阵重新基于量测模型进行量测更新，获得最终的状态估计及其误差协方差矩阵，修正北斗/惯性组合导航系统的导航误差。

2.如权利要求1的一种具有协方差反馈控制的动态自适应导航定位方法，其特征在于，所述滤波器基于极大似然准则，调整先验状态误差协方差矩阵，包括：

定义滤波器的新息向量；

基于固定长度估计窗口内的新息向量，建立先验状态误差协方差矩阵的似然函数；

求取似然函数的对数关于先验状态误差协方差矩阵的偏导数，并令其为0，使得似然函数最大化；

估计窗口内的滤波过程达到稳态时，先验状态误差协方差矩阵逐渐收敛，得到先验状态误差协方差矩阵的反馈控制策略。

3.如权利要求1的一种具有协方差反馈控制的动态自适应导航定位方法，其特征在于，所述惯性导航系统的误差参数包括：姿态误差、速度误差、位置误差；

所述北斗/惯性组合导航系统的状态量包括姿态误差、速度误差、位置误差、陀螺的输出误差和加速度计的输出误差。

4.如权利要求3的一种具有协方差反馈控制的动态自适应导航定位方法，其特征在于，所述惯性导航系统的误差参数和陀螺仪与加速度计的器件误差采用四元数描述，表示为：

其中，[δq₀ δq₁ δq₂ δq₃]^T表示加性姿态误差四元数，[δv_E δv_N δv_U]^T为速度误差，[δL δλ δh]^T为位置误差，[ε_bx ε_by ε_bz]^T为陀螺仪的常值漂移，为加速度计的零偏。

5.如权利要求4的一种具有协方差反馈控制的动态自适应导航定位方法，其特征在于，所述组合状态量的误差模型获得北斗/惯性组合导航系统的状态模型，包括：

获取姿态误差、速度误差、位置误差、陀螺的输出误差和加速度计的输出误差的传播方程；

组合姿态误差的传播方程、速度误差的传播方程、位置误差的传播方程、陀螺的输出误差方程和加速度计的输出误差方程，形成北斗/惯性组合导航系统的状态模型。

6.如权利要求1的一种具有协方差反馈控制的动态自适应导航定位方法，其特征在于，所述量测量与状态量之间线性关系为：

z_k＝H_kx_k+v_k

其中，z_k表示量测量，x_k表示状态量，为量测噪声矩阵，v_v和v_p为北斗卫星导航系统的速度误差和位置误差，为系统的量测矩阵，其中H_v＝[0_3×4 diag(1,1,1) 0_3×9]、H_p＝[0_3×7 diag(R_M,R_NcosL,1) 0_3×9]；

量测模型中的量测矢量z_k用于表征北斗卫星导航系统和惯性导航系统的速度差和位置差，量测矢量z_k表示为：

z_k＝[v_EI-v_EB v_NI-v_NB v_UI-v_UB λ_I-λ_B L_I-L_B h_I-h_B]^T

其中，[v_EI v_NI v_UI]^T和[λ_I L_I h_I]^T为惯性导航系统输出的无人机速度和位置，[v_EB v_NBv_UB]^T和[λ_B L_B h_B]^T为北斗卫星导航系统输出的无人机速度和位置。