CN101826856A - Particle filtering method based on spherical simplex unscented Kalman filter - Google Patents

Abstract

The invention provides a particle filtering method based on spherical simplex unscented Kalman filter. The particle filtering method comprises the steps of: initializing a particle and a weight value thereof; generating a particle through importance sampling; updating and normalizing the particle weight value; sampling again; outputting a result; and entering a next time step. The invention mainly improves an importance sampling step and obtains importance probability density by adopting a SSUKF (Spherical Simplex Unscented Kalman Filter) algorithm based on SSUT (Spherical Simplex Unscented Transformation). Compared with PF (Particle Filter), EKPF (Extended Kalman Particle Filter) and standard unscented particle filter, SSUPF (Spherical Simplex Unscented Particle Filter) can acquire the precision equivalent to UPF (Unscented Particle Filter). On the other hand, because the SSUT adopts sampling points, i.e. sigma points, which are distributed in a spherical way, the quantity of the sampling points is far less than the UT (Unscented Transformation), and the advantage on the aspect of computing efficiency is gradually obvious in a high dimensional system.

Description

Particle filter method based on spherical simplex unscented Kalman filter

Technical field

The invention belongs to signal processing, artificial intelligence, target following and computer vision field, specifically a kind of particle filter method.

Background technology

Non-linear filtering method all is widely used at numerous areas such as navigational guidance, location, signal processing, finance, artificial intelligence.EKF (EKF) is a kind of method that proposes early, and this method has higher computational efficiency, but filtering accuracy is limited, and the model that is suitable for is also restricted.Along with development of computer, Unscented kalman filtering (UKF) and particle filter become the focus of research gradually.Compare with EKF, UKF need not carry out linearisation with model, directly uses nonlinear model, and the error of having avoided local linearization to introduce is avoided occurring dispersing in strongly non-linear system.But EKF and UKF are based on Gauss's hypothesis, so inapplicable a lot of non-Gauss model in engineering is used.A kind of effective ways that remedy above-mentioned deficiency are the particle filters (PF) based on the Monte Carlo simulation method of imparametrization.The core of PF method is to utilize some random samples (particle) to represent the posterior probability density of system's stochastic variable, can obtain the near-optimization numerical solution based on physical model, rather than the pairing approximation model carries out optimal filter.

The modal problem of particle filter is the particle degradation phenomena, promptly through iteration several times, except that a particle, all particles all only have small weights, this means that a large amount of evaluation works all is used to upgrade those estimations to posterior probability density and does not almost have on the particle of influence.Select suitable significance distribution, can reduce of the influence of particle degradation phenomena to a certain extent arithmetic accuracy.Unscented kalman filtering (UPF) method is that to produce the importance probability density by UKF be a kind of suggestion distribution production method comparatively commonly used at present, and this method has been introduced the new measured value of current time, thereby can obtain high estimation accuracy.But because the amount of calculation of UKF depends on the sampled point number in the no mark conversion (UT) to a great extent, therefore for the higher-dimension system, the amount of calculation of UPF can become huge along with the increase of sampled point.

Summary of the invention

The object of the present invention is to provide and a kind ofly can guarantee to reduce the particle filter method based on spherical simplex unscented particle filter of amount of calculation significantly under the prerequisite of filtering performance.

The state-space model of supposing nonlinear dynamic system is:

x _k＝F(x _k-1，v _k-1)

y _k＝H(x _k，u _k)

X wherein _kThe k of expression system is residing state constantly, y _kExpression k measured value constantly.Function F () and H () are the state transitions of system and measure model, v _kAnd u _kBe respectively system noise and measurement noise.

The object of the present invention is achieved like this:

The first step, initialization particle and weights thereof;

In second step, produce particle by importance sampling;

The 3rd step, more new particle weights and it is carried out normalization;

The 4th step, sampling step again;

The 5th step, the output result;

In the 6th step, enter next time step;

Described method by importance sampling generation particle is:

(1) do not have mark conversion (SSUT) by the hypersphere simple form and obtain sampled point (sigma point):

Select the weight w of correspondence at zero point ₀, satisfy: 0≤w ₀≤ 1

Determine the weight w that other point is corresponding _i: w _i=(1-w ₀The i=1 of)/(n+1) ..., n+1

Introduce a sequence vector and construct the hypersphere sampled point

The initialization vector sequence:

The spread vector sequence, j=2 ..., n:

$e_i^j=\left\{\begin{array}{cc}\left[\begin{array}{c}{e}_0^{j-1}\\ 0\end{array}\right]& i=0\\ \left[\begin{array}{c}{e}_i^{j-1}\\ -1/\sqrt{j(j+1)w_j}\end{array}\right]& i=1,...,j\\ \left[\begin{array}{c}{0}^{j-1}\\ j/\sqrt{j(j+1)w_j}\end{array}\right]& i=j+1\end{array}\right.$

In the formula, n is the state vector dimension, e _i ^jI sampled point of expression j n-dimensional random variable n; 0 ^jExpression j dimension null vector; Obtain n+2 the sampled point e of y by step _i ⁿ, i=1 ..., n+1; For average be

Mean square deviation is P _XxThe hypersphere profile samples dot matrix of n n-dimensional random variable n x obtain by following formula:

${\mathrm{\χ}}_i^n=\hat{x}+\sqrt{P_{\mathrm{xx}}}{e}_i^n$ i＝0，...，n+1；

(2) time upgrades:

Transmit the sigma point according to system's nonlinear equation:

x _i，k|k-1＝F(x _i，k-1，u _k-1，k-1) i＝0，...，n+1

y _i，k|k-1＝H(x _i，k|k-1，k) i＝0，...，n+1

Calculate premeasuring:

${\hat{x}}_{k|k-1}=\underset{i=0}{\overset{n+1}{\mathrm{\Σ}}}{W}_i{\mathrm{\χ}}_{i,k|k-1}$

${\hat{y}}_{k|k-1}=\underset{i=0}{\overset{n+1}{\mathrm{\Σ}}}{W}_i{y}_{i,k|k-1};$

$P_{k|k-1}=\underset{i=0}{\overset{n+1}{\mathrm{\Σ}}}{W}_i({\mathrm{\χ}}_{i,k|k-1}-{\hat{x}}_{k|k-1}){({\mathrm{\χ}}_{i,k|k-1}-{\hat{x}}_{k|k-1})}^T+P_{\mathrm{\ω}}$

(3) measure renewal: the measured value y that obtains current time _kAfter, further quantity of state is upgraded

${\hat{x}}_k={\hat{x}}_{k|k-1}+K_k(y_k-{\hat{y}}_{k|k-1})$

Wherein,

Be kalman gain

$P_{{\tilde{y}}_k}=\underset{i=0}{\overset{n+1}{\mathrm{\Σ}}}{W}_i(y_{i,k|k-1}-{\hat{y}}_{k|k-1}){(y_{i,k|k-1}-{\hat{y}}_{k|k-1})}^T+P_v$

$P_{x_k{y}_k}=\underset{i=0}{\overset{n+1}{\mathrm{\Σ}}}{W}_i({\mathrm{\χ}}_{i,k|k-1}-{\hat{x}}_{k|k-1}){(y_{i,k|k-1}-{\hat{y}}_{k|k-1})}^T{K}_k=P_{x_k{y}_k}{P}_{{\tilde{y}}_k}^{-1}$

$P_k=P_{k|k-1}-K_k{P}_{{\tilde{y}}_k}{K}_k^T;$

Wherein, x expands dimension by state variable and forms, by system mode, system noise v _k, observation noise u _kForm.The sigma dot matrix of x for being obtained by x, dimension is identical with x.x _K-1Expression k-1 sigma dot matrix constantly.W _iThe weights of expression sigma point vector.

(4) to each sampled point x _K-1 ⁱ, use the average that spherical simplex unscented particle filter (SSUKF) obtains the particle collection

With variance P _k ⁱ

(5) the suggestion distribution N that obtains from SSUKF (

P _k ⁱ) N particle of middle generation.

In the step of particle filter, of paramount importance is importance sampling and sampling step again.At present common way is that to choose importance density be prior probability, advantage is to be easy to realize and avoided complicated integral operation, but owing to do not use up-to-date measuring value, have only the particle of only a few to have bigger weights, cause the performance of filter variation making.Utilize EKF and UKF to produce the precision that significance distribution can improve PF to a certain extent, but under many circumstances, owing to can cause very big model error after strong non-linear of dynamical system, first-order linearization, thereby the EKF estimated performance is descended rapidly even cause filtering divergence.And the UKF that changes based on UT goes for this situation, and it does not need non linear system equation and measurement equation approximately linearization.Therefore the filtering performance of UPF is better than EKPF, but amount of calculation is much higher than EKPF, and this makes the rapidity of algorithm be subjected to influence.

The present invention improves the importance sampling step, adopts and obtains the importance probability density based on the SSUKF algorithm of SSUT conversion.Than the UPF of PF, EKPF and standard, SSUPF can obtain the precision suitable with UPF.On the other hand, because the sigma point sampling that the SSUT conversion adopts hypersphere to distribute, sampled point quantity is less than the UT conversion greatly, and is so amount of calculation also is significantly smaller than UPF, obvious all the more in the higher-dimension system in the advantage aspect the computational efficiency.The present invention proposes a kind of particle filter method based on SSUT (SphericalSimplex Unscented Transformation), promptly replace the sampling that is symmetrically distributed with the hypersphere profile samples, reduced the sigma number of spots, under the prerequisite that guarantees filtering performance, reduced the amount of calculation of algorithm significantly.

The present invention has the following advantages:

The first, owing in SSUKF, introduced the new observed quantity of current time, thus on precision, be better than PF and EKPF method, suitable with UPF.

The second, the sigma number of spots of UKF is 2n+1, and the sigma number of spots of the SSUKF method of the present invention's utilization is n+2, and therefore for high order system, the amount of calculation of this method is less than the UPF algorithm greatly.

Description of drawings

Fig. 1 is the particle filter method flow chart based on SSUKF;

Fig. 2 is the SSUKF algorithm flow chart;

The state estimation (N=100) that Fig. 3 is PF, EKPF and SSUPF in independent experiment;

The state estimation (N=100) that Fig. 4 is PF, EKPF and UPF in independent experiment;

The state estimation (N=200) that Fig. 5 is PF, EKPF and SSUPF in independent experiment;

The state estimation (N=200) that Fig. 6 is PF, EKPF and UPF in independent experiment.

Embodiment

For example the present invention is done description in more detail below in conjunction with accompanying drawing:

The first step, initial time is by initial distribution p (x ₀) in obtain one group of primary, and its initial average and variance is set.

In second step, use more new particle of SSUKF:

(1) obtain the sigma point by the SSUT conversion:

Select the weight w of correspondence at zero point ₀, satisfy: 0≤w ₀≤ 1

Determine weight w _i: w _i=(1-w ₀The i=1 of)/(n+1) ..., n+1

The initialization vector sequence:

The spread vector sequence (j=2 ..., n):

$e_i^j=\left\{\begin{array}{cc}\left[\begin{array}{c}{e}_0^{j-1}\\ 0\end{array}\right]& i=0\\ \left[\begin{array}{c}{e}_i^{j-1}\\ -1/\sqrt{j(j+1)w_j}\end{array}\right]& i=1,...,j\\ \left[\begin{array}{c}{0}^{j-1}\\ j/\sqrt{j(j+1)w_j}\end{array}\right]& i=j+1\end{array}\right.$

In the formula, e _i ^jI sampled point of expression j n-dimensional random variable n; 0 ^jExpression j dimension null vector.Obtain n+2 the sampled point e of y by above-mentioned algorithm _i ⁿ, i=1 ..., n+1.For average be Mean square deviation is P _XxThe hypersphere profile samples point of n n-dimensional random variable n x can obtain by following formula:

${\mathrm{\χ}}_i^n=\hat{x}+\sqrt{P_{\mathrm{xx}}}{e}_i^n$ i＝0，...，n+1

(2) time upgrades:

x _i，k|k+1＝F(x _i，k-1，u _k-1，k-1) i＝0，...，n+1

${\hat{x}}_{k|k-1}=\underset{i=0}{\overset{n+1}{\mathrm{\Σ}}}{W}_i{\mathrm{\χ}}_{i,k|k-1}$

$P_{k|k-1}=\underset{i=0}{\overset{n+1}{\mathrm{\Σ}}}{W}_i({\mathrm{\χ}}_{i,k|k-1}-{\hat{x}}_{k|k-1}){({\mathrm{\χ}}_{i,k|k-1}-{\hat{x}}_{k|k-1})}^T+P_{\mathrm{\ω}}$

y _i，k|k-1＝H(x _i，k|k-1，k) i＝0，...，n+1

${\hat{y}}_{k|k-1}=\underset{i=0}{\overset{n+1}{\mathrm{\Σ}}}{W}_i{y}_{i,k|k-1}$

(3) measure renewal

$P_{{\tilde{y}}_k}=\underset{i=0}{\overset{n+1}{\mathrm{\Σ}}}{W}_i(y_{i,k|k-1}-{\hat{y}}_{k|k-1}){(y_{i,k|k-1}-{\hat{y}}_{k|k-1})}^T+P_v$

$P_{x_k{y}_k}=\underset{i=0}{\overset{n+1}{\mathrm{\Σ}}}{W}_i({\mathrm{\χ}}_{i,k|k-1}-{\hat{x}}_{k|k-1}){(y_{i,k|k-1}-{\hat{y}}_{k|k-1})}^T{K}_k=P_{x_k{y}_k}{P}_{{\tilde{y}}_k}^{-1}$

${\hat{x}}_k={\hat{x}}_{k|k-1}+K_k(y_k-{\hat{y}}_{k|k-1})$

$P_k=P_{k|k-1}-K_k{P}_{{\tilde{y}}_k}{K}_k^T$

(4) to each sampled point x _K-1 ⁱ, use the average that SSUKF obtains the particle collection

With variance P _k ⁱ

(5) from SSUKF as a result N (

P _k ⁱ) N particle of middle generation

The 3rd step, right value update

$w_k^i=w_{k-1}^i\frac{p\left(y_k\right|x_k^i)p\left(x_k^i\right|x_{k-1}^i)}{\mathrm{\π}\left(x_k^i\right|x_{k-1}^i,y_{1:k})}$

Importance probability density function in the following formula

Introduce up-to-date measured value, therefore improved the performance of filter.

In the 4th step, obtain normalized weights

The 5th step, sampling again

Definition threshold value N _EffWeigh effective number of particles

As effective population N _Eff＜N _Thr, promptly to

I=1 ..., the N} resampling produces new set { x _k ⁱ, i=1 ..., N}, the weights that reset particle are:

In the 6th step, state upgrades:

So far, the computing of a time step of algorithm finishes, and the computing that enters next time step changes the operation of second step over to.

Embodiment one

Nonlinear model below utilizing verifies that to filtering performance its state equation and observational equation are as follows:

$\left\{\begin{array}{c}{x}_k=1+\sin \left((4e-2)\mathrm{\&pi;}(k-1)\right)+0.5x_{k-1}+v_{k-1}\\ y_k=\left\{\begin{array}{cc}0.2x_{\mathrm{<figure-callout\; id="2"\; label="k"\; filenames="HSA00000055154400041.png,HSA00000055154400042.png"\; state="\{\{state\}\}">k</figure-callout>}}^2+n_k& k\&le;30\\ 0.5x_k-2+n_k& k>30\end{array}\right.\end{array}\right.$

Process noise v wherein _tObey Gamma distribution Gamma (3,2), measurement noise n _kGaussian distributed N (0,10 ^-5).Being estimated as of system mode The mean square error of an independent experiment is: $\mathrm{Var}={\left[\frac{1}{T}\underset{k=1}{\overset{T}{\mathrm{\&Sigma;}}}{({\hat{x}}_k-x_k)}^2\right]}^{1/2}.$

In experiment, adopt PF respectively, the SSUPF algorithm that EKPF and UPF and this paper propose compares.The population N that adopts is respectively 100 and 200, and the measurement time is T=60, carries out 100 independently experiments, and the UT transformation parameter of UPF is α=1, β=0, κ=2.

It is 100 o'clock that Fig. 3 has provided population, PF, EKPF and the SSUPF state estimation result in independent experiment.It is 100 o'clock that Fig. 4 has provided population, PF, EKPF and the UPF state estimation result in independent experiment.It is 200 o'clock that Fig. 5 has provided population, PF, EKPF and the SSUPF state estimation result in independent experiment.It is 200 o'clock that Fig. 6 has provided population, PF, EKPF and the UPF state estimation result in independent experiment.From above figure as can be seen, for non-linear, non-Gauss's problem, the state estimation that the PF algorithm obtains constantly can the substantial deviation actual value in part, the state estimation that EKPF, SSUPF and UPF algorithm the obtain time of day that can coincide preferably, and then the precision of two kinds of UPF algorithms is better than EKPF again.

Compare by the performance of above-mentioned model the present invention and PF, EKPF and three kinds of particle filter methods of UPF.Main correction data is through the root-mean-square error (RMSE) of four kinds of particle filter algorithms of 100 operations, effective population and average calculating operation time.Can obtain by the comparison to these performance index, in four kinds of filtering algorithms, EKPF, UPF and SSUPF will be significantly better than PF for the adaptability of nonlinear problem, and the precision and the UKF of the SSUPF algorithm that this paper proposes are suitable, comparatively significantly are better than EKPF.But owing to improvement, increased the amount of calculation of algorithm, relative conventional P F algorithm operation time of above-mentioned three kinds of algorithms is obviously increased in the importance sampling link.The sigma number of spots of SSUT conversion is less than symmetrical distribution UT conversion, so the operation efficiency of this paper algorithm is better than UPF, is less than UPF average calculating operation time.Aspect effective population, the effective sample of two kinds of UPF algorithms is more than EKPF and PF, this shows that two kinds of UPF algorithms are suppressing to be better than other algorithms on the particle degradation phenomena, and non-linear and noise that can more efficiently answering system model is factors such as non-Gaussian Profile.In addition, be that two kinds of experimental results of 100 and 200 can obtain by the longitudinal comparison population, the precision of various filtering algorithms can improve along with the increase of population, but also can increase operation time simultaneously thereupon.This explanation is used the particle of larger amt for the not high occasion of rapidity requirement, can obtain higher filtering accuracy.The SSUPF algorithm that the present invention proposes can obtain the filtering accuracy of UPF with the operation efficiency that is equivalent to EKPF.

Claims (1)

1. the particle filter method based on spherical simplex unscented particle filter comprises the steps:

The first step, initialization particle and weights thereof;

In second step, produce particle by importance sampling;

The 3rd step, more new particle weights and it is carried out normalization;

The 4th step, sampling step again;

The 5th step, the output result;

In the 6th step, enter next time step;

It is characterized in that described method by importance sampling generation particle is:

(1) obtain the sigma point by the SSUT conversion:

Select the weight w of correspondence at zero point ₀, satisfy: 0≤w ₀≤ 1

Determine the weight w of other sampled point correspondence _i: w _i=(1-w ₀The i=1 of)/(n+1) ..., n+1 initialization vector sequence: $e_0^1=\left[0\right],$ $e_1^1=[-1/\sqrt{{2w}_1}],$ $e_2^1=[1/\sqrt{2w_1}]$

The spread vector sequence, j=2 ..., n:

$e_i^j=\left\{\begin{array}{cc}\left[\begin{array}{c}{e}_0^{j-1}\\ 0\end{array}\right]& i=0\\ \left[\begin{array}{c}{e}_i^{j-1}\\ -1/\sqrt{j(j+1)w_j}\end{array}\right]& i=1,...,j\\ \left[\begin{array}{c}{0}^{j-1}\\ j/\sqrt{j(j+1)w_j}\end{array}\right]& i=j+1\end{array}\right.$

In the formula, n is the state vector dimension, e _i ^jI sampled point of expression j n-dimensional random variable n; 0 ^jExpression j dimension null vector;

Obtain n+2 the sampled point e of y by step _i ⁿ, i=1 ..., n+1; For average be

, mean square deviation is P _XxThe hypersphere profile samples point of n n-dimensional random variable n x obtain by following formula:

${\mathrm{\&chi;}}_i^n=\hat{x}+\sqrt{P_{\mathrm{xx}}}{e}_i^n$ i＝0，...，n+1；

(2) time upgrades:

χ _i，k|k+1＝F(χ _i,k-1，u _k-1，k-1) i＝0，...，n+1

${\hat{x}}_{k|k-1}=\underset{i=0}{\overset{n+1}{\mathrm{\&Sigma;}}}{W}_i{\mathrm{\&chi;}}_{i,k|k-1}$

$P_{k|k-1}=\underset{i=0}{\overset{n+1}{\mathrm{\&Sigma;}}}{W}_i({\mathrm{\&chi;}}_{i,k|k-1}-{\hat{x}}_{k|k-1}){({\mathrm{\&chi;}}_{i,k|k-1}-{\hat{x}}_{k|k-1})}^T+P_{\mathrm{\&omega;}}$

y _i，k|k-1＝H(χ _i，k|k-1，k) i＝0，...，n+1

${\hat{y}}_{k|k-1}=\underset{i=0}{\overset{n+1}{\mathrm{\&Sigma;}}}{W}_i{y}_{i,k|k-1};$

(3) measure renewal

$P_{{\tilde{y}}_k}=\underset{i=0}{\overset{n+1}{\mathrm{\&Sigma;}}}{W}_i(y_{i,k|k-1}-{\hat{y}}_{k|k-1}){(y_{i,k|k-1}-{\hat{y}}_{k|k-1})}^T+P_v$

$P_{x_k{y}_k}=\underset{i=0}{\overset{n+1}{\mathrm{\&Sigma;}}}{W}_i({\mathrm{\&chi;}}_{i,k|k-1}-{\hat{x}}_{k|k-1}){(y_{i,k|k-1}-{\hat{y}}_{k|k-1})}^T{K}_k=P_{x_k{y}_k}{P}_{{\tilde{y}}_k}^{-1}$

${\hat{x}}_k={\hat{x}}_{k|k-1}+K_k(y_k-{\hat{y}}_{k|k-1})$

$P_k=P_{k|k-1}-K_k{P}_{{\tilde{y}}_k}{K}_k^T;$

(4) to each sampled point x _K-1 ⁱ, use the average that ssuKF obtains the particle collection

With variance P _k ⁱ

(5) from SSUKF result

N particle of middle generation.

Images