[发明专利]一种基于自适应KLD盒子长度的粒子滤波器的实现方法

摘要

本发明公开了一种基于自适应KLD盒子长度的粒子滤波器的实现方法，通过一系列步骤实现该方法，本发明提出了交替重采样和KLD采样算法，避免了粒子滤波额外的计算量，提高了算法的执行效率，在粒子数量很大的情况下，本发明大大提升了粒子滤波的性能。本发明的自适应KLD盒子长度的实现方法，能够根据每个时刻粒子的状态分布，自动地调整KLD盒子长度的大小。当粒子分布范围比较广时，KLD盒子长度能够自适应地增大，防止KLD采样的粒子数目过多，降低计算复杂度；在粒子分布密集时，KLD盒子长度能够自适应地减少，增加KLD采样的粒子数目，提高估计精度。

主权项

一种基于自适应KLD盒子长度的粒子滤波器的实现方法，其特征在于，包括如下步骤：1)输入：基于KLD准则，从KLD自适应采样截止的判定公式：其中，nx是KLD自适应采样的粒子数目，z1‑δ表示上界为1‑δ的标准正态分布的可信度，k为KLD盒子数目，ε为较小的正实数；KLD盒子数目k与ε、z1‑δ和nx的值有关，KLD盒子数目k与nxε的值近似成正比，其中，z1‑δ表示上界为1‑δ的标准正态分布的可信度；输入t‑1时刻的粒子集为设定ε和δ的值，设定所需粒子数目最小值为nmin，t‑1时刻的粒子个数为nt‑1；2)初始化：在进行KLD采样之前，定义粒子集合，采样粒子数目、KLD盒子数目和权值累加量，令t时刻的预测粒子集nt＝0，KLD盒子数目k＝0，权值累加量α＝0，盒子长度为b；3)重新采样：对粒子集进行重采样，产生第i个粒子4)时间更新：根据粒子的概率密度函数p(xt|xt‑1)，由t‑1时刻的粒子计算得到t时刻的粒子5)量测更新：将t时刻的粒子带入函数得到粒子权值为6)将得到的粒子放入预测粒子集中，7)权值累加不断增加a的值；8)计算盒子个数：利用下面的if函数来进行判断，当粒子分布在一个新的盒子范围内，盒子数目增加1∶If(落入一段空的盒子长度b内)then k＝k+1End If；9)粒子数目进行更新：nt＝nt+1，每做一次循环，增加一个粒子数目；10)判定：将当前时刻的粒子数目nt与设定的最小粒子数目nmin进行比较，当满足条件时，计算KLD采样需要的粒子数目nx，然后进行判断，当前的粒子数达到KLD采样需要的粒子数目时，跳出循环，结束KLD采样：If(nt＞nmin)thenEnd If；If(nt≥nx)then跳出循环End If；11)循环：循环执行步骤3‑步骤10，直到满足步骤10的判定条件，跳出循环；12)归一化权值：