一种自复位差分粒子滤波算法的制作方法

本发明涉及滤波估计，特别是涉及一种自复位差分粒子滤波算法。

背景技术：

1、滤波估计领域的研究始于40年前，以贝叶斯滤波递归估计为基础，衍生出多种滤波方法。对于线性系统来说，卡尔曼滤波(kf)可以在系统噪声服从高斯分布时取得良好的定位效果；对于非线性系统，可通过次优的扩展卡尔曼滤波(ekf)和无迹卡尔曼滤波(ukf)进行优化。ekf与ukf都对目标状态进行了线性化处理，不同的是前者采用先验线性化而后者是后验线性化，分别可以达到一阶与二阶的估计精度。相比之下，粒子滤波器(pf)不对状态方程与观测方程进行线性与高斯假设，所以其受系统线性程度可噪声类型的影响较小，因而被广泛应用于室内定位领域。

2、在实际操作中粒子滤波器存在粒子退化问题，虽然重采样操作可在一定程度上缓解粒子退化，但造成了粒子集多样性的降低，并带来了粒子贫化的问题。加之系统建模存在一定的不准确性，当粒子集贫化到一定程度时，如果目标的状态出现了突变，滤波器会因模型状态噪声的修正程度有限而加速发散，无法对目标状态进行有效估计。

3、随着启发式算法在工程领域的应用，各种智能优化方法也集成于pf算法之中。比如遗传优化粒子滤波(gapf)、粒子群优化粒子滤波(psopf)、和免疫粒子滤波(aipf)等等，这类方法原理大同小异，基本是通过改进自寻优以改善粒子集的贫化程度，而且都是在每一代中无差别的对粒子集进行自寻优操作，这无疑增加了估计时间。虽然可通过减少粒子数的方式来降低时间复杂度，但粒子数过少也会导致估计精度的降低。

技术实现思路

1、为了克服现有技术的不足，本发明的目的是提供一种自复位差分粒子滤波算法，本发明解决了现有技术中滤波估计技术精度低下的问题。

2、为实现上述目的，本发明提供了如下方案：

3、一种自复位差分粒子滤波算法，包括：

4、获取滤波器跟踪状态并进行判断，若所述滤波器跟踪状态为正常，则采用标准粒子滤波的随机重采样方法对目标状态进行估计；

5、若所述滤波器跟踪状态为跟踪失败，则将粒子滤波中的随机采样过程转换为差分重采样过程，对目标状态进行估计；

6、判断所述滤波器跟踪状态的步骤为：

7、设置阈值；

8、获取当前时刻的滤波估计值和当前时刻对应的理论最优滤波估计值的距离值；

9、判断所述距离值与所述阈值，若所述距离值小于述阈值，则跟踪状态为跟踪正常；若所述距离值大于所述阈值，则跟踪状态为跟踪失败。

10、根据本发明提供的具体实施例，本发明公开了以下技术效果：

11、本发明提供了一种自复位差分粒子滤波算法，通过结合卡方检验设计了一种跟踪失败的判别机制，设置粒子集分布区间的上界的下界，对跟踪状态进行判定，当检测到滤波估计出现失效，判别跟踪失败时，启用改进的差分优化策略对病态的粒子集进行重置，提高粒子集的多样性提升了滤波估计的准确度。

技术特征：

1.一种自复位差分粒子滤波算法，其特征在于，包括：

2.根据权利要求1所述的一种自复位差分粒子滤波算法，其特征在于，所述阈值为：

3.根据权利要求1所述的一种自复位差分粒子滤波算法，其特征在于，所述当前时刻的滤波估计值和当前时刻对应的理论最优滤波估计值的距离值的计算公式为：

4.根据权利要求3所述的一种自复位差分粒子滤波算法，其特征在于，所述则将粒子滤波中的随机采样过程转换为差分重采样过程，对目标状态进行估计，包括：

5.根据权利要求4所述的一种自复位差分粒子滤波算法，其特征在于，所述对所述低权重粒子集合进行迭代优化，得到优化后的粒子集合，包括：

6.根据权利要求4所述的一种自复位差分粒子滤波算法，其特征在于，所述对所述优化后的粒子集合、高权重粒子集合和中权重粒子集合进行组合，包括：

7.根据权利要求4所述的一种自复位差分粒子滤波算法，其特征在于，所述计算各粒子的权重的计算公式为：

8.根据权利要求7所述的一种自复位差分粒子滤波算法，其特征在于，所述第二后验马氏距离的计算公式为：

9.根据权利要求5所述的一种自复位差分粒子滤波算法，其特征在于，所述对低权重集合内的粒子进行优化，得到第一新粒子的计算公式为：

10.根据权利要求9所述的一种自复位差分粒子滤波算法，其特征在于，所述则对更新后的高权重粒子集合进行优化，得到第二新粒子的计算公式为：

技术总结
本发明提供了一种自复位差分粒子滤波算法，包括：获取滤波器跟踪状态并进行判断，若所述滤波器跟踪状态为正常，则采用标准粒子滤波的随机重采样方法对目标状态进行估计；若所述滤波器跟踪状态为跟踪失败，则将粒子滤波中的随机采样过程转换为差分重采样过程，对目标状态进行估计。本发明解决了现有技术中滤波估计技术精度低下的问题。

技术研发人员：杨永辉,许函铭
受保护的技术使用者：深圳市天工测控技术有限公司
技术研发日：
技术公布日：2024/1/14