[发明专利]适用于位姿测量卡尔曼滤波的滤波器更新信息筛选方法

权利要求书

1.一种适用于位姿测量卡尔曼滤波的滤波器更新信息筛选方法，其特征在于，包括以下步骤：

在进行位姿测量的过程中，持续获取并存储带有时间戳信息的IMU测量数据、IMU解算结果和外部位姿传感器的外部位姿观测结果；

选取最新获取的和与之时间戳临近的共两帧外部位姿观测结果；并按照时间戳信息最接近要求，再选取出两帧对应的IMU解算结果；

根据两帧外部位姿观测结果和对应的两帧IMU解算结果求取外部位姿测量与惯性测量的相对偏差；

判断述相对偏差是否超出偏差阈值范围；不超出，则将最新获取的外部位姿观测结果作为卡尔曼滤波的外部观测量，进行该时刻卡尔曼滤波器状态向量的更新；

利用更新的卡尔曼滤波器状态向量和该时刻之后已存储的IMU测量数据重新进行捷联惯导解算，更新位姿测量结果。

2.根据权利要求1所述的滤波器更新信息筛选方法，其特征在于，所述偏差阈值范围为[-2RMSD_k，2RMSD_k]；RMSD_k为均方根偏差基准；

所述其中，D_i，i＝k-n-1，…，k-1，为k时刻前一定时间内，n组有效的相对偏差；

当相对偏差D_k∈[-2RMSD_k，2RMSD_k]时，则将k时刻获取的外部位姿观测结果作为卡尔曼滤波的外部观测量，进行该时刻卡尔曼滤波器状态向量的更新，同时，所述D_k计入k时刻的均方根偏差基准的更新；否则，所述外部位姿观测结果作为无效数据丢弃不用，所述D_k不计入均方根偏差基准。

3.根据权利要求2所述的滤波器更新信息筛选方法，其特征在于，所述相对偏差为根据相邻两帧外部位姿观测结果计算的相对位姿变换矩阵；为与所述两帧相邻外部位姿观测结果时刻最接近的两帧IMU解算结果计算的相对位姿变换矩阵。

4.根据权利要求1所述的滤波器更新信息筛选方法，其特征在于，所述位姿测量为基于双IMU差分测量的被测物体相对于运动载体的相对位姿；

所述被测物体和运动载体布置有各自的IMU，用于分别获取被测物体与运动载体的运动信息。

5.根据权利要求4所述的滤波器更新信息筛选方法，其特征在于，卡尔曼滤波器的状态向量X＝[p_h-p，v_h-p，q_h-p，b_ha，b_pa，b_hω，b_pω]^T；

X中p_h-p、v_h-p、q_h-p定义为双IMU差分获得的载体坐标系下被测物体相对载体的位置、速度、旋转四元数；b_hω、b_ha为被测物体IMU测量角速度与加速度的零偏；b_pω、b_pa为运动载体IMU测量角速度与加速度的零偏。