[发明专利]一种改进的鲁棒无迹卡尔曼滤波组合导航方法

权利要求书

1.一种改进的鲁棒无迹卡尔曼滤波组合导航方法，其特征在于包括以下几个步骤：

步骤一：收集UWB系统和MEMS系统输出的数据；

步骤二：选择系统的状态量和观测量，建立UWB/MEMS组合导航系统状态空间模型；

步骤三：对UWB/MEMS组合导航系统进行滤波初始化；

步骤四：在k时刻对UWB/MEMS组合导航系统进行时间更新和量测更新，得到系统状态一步预测的误差协方差和一步预测的互协方差；

步骤五：引入量测误差判据，若量测存在误差，则计算多重因子的鲁棒矩阵，并对状态一步预测的误差协方差进行校正，然后进入步骤六，若量测不存在误差，则直接进入步骤六；

步骤六：若量测存在误差，则利用校正后的状态一步预测误差协方差，计算k时刻系统的状态估计值和状态误差协方差，若量测不存在误差，则利用步骤四中的状态一步预测误差协方差，计算k时刻系统的状态估计值和状态误差协方差；

步骤七：根据步骤六中得到的状态估计值获取MEMS惯导的陀螺仪、加速度计的常值漂移，并将其反馈到组合系统中，输出姿态、速度和位置信息。

2.根据权利要求1所述的一种改进的鲁棒无迹卡尔曼滤波组合导航方法，其特征在于所述UWB/MEMS组合导航系统状态空间模型的系统状态方程表示为：

X_k+1＝F_kX_k+w_k

其中，X_k为15维的惯导解算信息误差，表示为其中，φ_k＝[φ_k,x φ_k,y φ_k,z]^T为k时刻惯导解算的姿态误差，ΔV_k＝[ΔV_k,x ΔV_k,y ΔV_k,z]^T为k时刻惯导解算的地理坐标系下的速度误差，ΔP_k＝[ΔL_k Δλ_k Δh_k]^T为k时刻惯导解算的经纬高误差，ε_k＝[ε_k,x ε_k,y ε_k,z]^T为k时刻陀螺常值漂移，为k时刻加速度计零偏，w_k为k时刻组合系统过程噪声，w_k噪声协方差矩阵为Q_k，F_k为k时刻惯导系统状态转移矩阵。