[发明专利]面向具有数据缺失UWB行人导航的自适应预估EKF滤波算法及系统

权利要求书

1.面向具有数据缺失UWB行人导航的自适应预估EKF滤波算法，其特征在于，包括：

以UWB导航系统在t时刻在导航系下的东方向位置、北方向位置、东方向速度、北方向速度作为状态量，以INS与UWB分别测量的目标节点与参考节点之间距离的差值作为系统观测量，构建滤波模型；

利用自适应EKF滤波算法对位置误差进行预估，预估过程中实时判断UWB测量得到的目标节点与参考节点之间的距离信息是否有缺失，如果有，对缺失的距离信息进行预估；

最终得到当前时刻目标行人最优的导航信息；

所述的预估过程中实时判断UWB测量得到的目标节点与参考节点之间的距离信息是否有缺失，如果有，对缺失的距离信息进行预估，具体为：

引入变量表示UWB测量得到的目标节点与参考节点之间的第i个距离信息；如果第i个距离信息缺失，则重新对进行预估；采用矩阵h(X_t|t-1)的第i行第1列替代缺失的距离信息。

2.如权利要求1所述的面向具有数据缺失UWB行人导航的自适应预估EKF滤波算法，其特征在于，所述自适应EKF预估滤波器的状态方程为：

其中，和分别为t和t-1时刻UWB导航系统的东方向位置、北方向位置、东方向速度、北方向速度；T为采样周期；ω_t-1为t-1时刻的系统噪声。

3.如权利要求1所述的面向具有数据缺失UWB行人导航的自适应预估EKF滤波算法，其特征在于，所述自适应EKF预估滤波器的的观测方程为：

其中，d_i,t,i∈(1,2,...,g)为t时刻UWB分别测量得的目标节点与参考节点之间的距离；g为参考节点的数目；x为UWB解算出的目标节点东方向位置，y为UWB解算出的目标节点北方向位置，x_i,i∈(1,2,...,g)和y_i,i∈(1,2,...,g)分别为参考节点1到i的东向位置和北向位置；ν_t为系统t时刻的观测噪声。

4.如权利要求1所述的面向具有数据缺失UWB行人导航的自适应预估EKF滤波算法，其特征在于，对缺失数据进行预估后，自适应EKF预估滤波器的观测方程变为：

5.如权利要求1所述的面向具有数据缺失UWB行人导航的自适应预估EKF滤波算法，其特征在于，所述利用自适应EKF滤波算法对位置误差进行预估，具体为：

P_t＝(I-K_tH_t)P_t|t-1；

其中，K_t表示EKF在t时刻的误差增益矩阵；I表示单位矩阵，P_t|t-1表示EKF由t-1时刻到t时刻的最小预测均方误差矩阵，表示自适应EKF滤波算法由t-1时刻到t时刻预估的状态向量，P_t表示EKF在t时刻的最小预测均方误差矩阵，ν_t为系统t时刻的观测噪声，其协方差矩阵为R_t；r_t、d_t均为中间变量,