CN101339610A - Particle filtering resampling method suitable for non-linear probabilistic system posture - Google Patents

Abstract

The invention relates to a particle filter resampling method which is applied to a nonlinear stochastic system state. The invention relates to a filtering method, in particular to a particle filter resampling method for estimating the nonlinear stochastic system state. The invention aims at solving the problem that the filtering precision of the state is poorer as the observation and likelihood function of the system state has double-humped characteristic when the nonlinear stochastic system state is filtered. In the resampling method, the similarity degree of the observation vector of the particles and the observation vector of the system state is utilized, so as to regulate the weight of the particles that are involved in the resampling. The invention is applied to the state filtering problem of the common nonlinear stochastic system, when the observation and likelihood function of the system state has the double-humped characteristic, the method can improve the filtering precision of a particle filter to the nonlinear stochastic system state.

Description

Be applicable to the particle filtering resampling method of non-linear probabilistic system posture

Technical field

The present invention relates to a kind of filtering method, be specifically related to the particle filtering resampling method that non-linear probabilistic system posture is estimated.

Background technology

The non-linear probabilistic system posture estimation problem extensively is present in signal Processing and the association area thereof.Although passed through the development of nearly half a century, the non-linear probabilistic system posture filtering method is also more immature.On the one hand, this is because complicacy, the diversity of nonlinear stochastic system cause; On the other hand, because the existence of various different random noises in the nonlinear stochastic system causes state filtering difficulty more, influenced the accuracy rate of state filtering, for non-linear probabilistic system posture filtering has brought bigger difficulty.Since the eighties in 20th century, the non-linear probabilistic system posture filtering method receives increasing concern, and wherein the most frequently used is extended Kalman filter.Because the core concept of extended Kalman filter is that the nonlinear model of stochastic system is made linear-apporximation, and its noise is based on the Gauss hypothesis, so its filtering performance to the non-gaussian random system state of strong nonlinearity is not good.EKF is only effective to some specific nonlinear stochastic system in the practical application, can not guarantee that for general non-linear non-gaussian random system state EKF its convergence and filtering error are bigger.And non-linear non-gaussian random system state filtering problem has more universal significance in actual conditions.A kind ofly obtaining development rapidly in recent years based on this at new filtering method one particle filter (Particlefiltering) that has original advantage aspect the filtering of non-linear non-gaussian random system state, this method is modeled as characteristic with the Monte-Carlo of imparametrization, has effectively overcome the shortcoming of EKF.Because particle filter is having original advantage aspect the non-linear non-gaussian random system state filtering problem of processing, become a research focus of current non-linear non-gaussian random system state filtering problem, and begun to be applied to video and fields such as Flame Image Process, navigation and location, target following, radio communication, status monitoring and fault diagnosis abroad.

Particle filter is a kind of simulation implementation method of recurrence Bayes state estimation, and its core is to use a random sample with corresponding weight value to gather the posterior probability density of representing the system state of asking.The basic ideas of this method are to choose an importance probability density and therefrom carry out random sampling, after obtaining some state sample (particle) that have relevant weights, size and particle position at the basic adjusted weights of state observation, use these samples to come the posterior probability density of approximate representation system state, utilize this approximate posterior probability density to obtain the valuation of system state at last.The approximate true probability density that will be equal to system state of this probability when population is tending towards infinite.Particle filter adopts sample form rather than functional form that probability distribution over states is described, and makes it not need the probability distribution of state variable is done too much constraint, is the filtering method that is suitable for non-linear non-gaussian random system state at present most.

Particle filter is the new method that occurs in recent years, and its theory and implementation method itself still have a large amount of problems to need to solve.Propose from particle filter, grain word degenerate problem is exactly that people are devoted to the intrinsic major issue of particle filter that solves always, and the particle resampling solves one of main means of particle degenerate problem just.The grain word is degenerated and is meant that most of particle is gradually away from system's time of day in the process that particle filter constantly carries out forward in time, and this makes the weights of most of particle extremely low.The particle that these weights are extremely low works hardly to state filtering, has reduced the effective number of particles in the state filtering, and then has reduced the precision of state filtering.Particle resamples and produces a new particle group according to the particle weights from existing population, and this new particle group's particle is accumulated near system's time of day.Standard particle filtering method (SIR) adopts the polynomial expression method for resampling, has alleviated grain word degenerate problem to a certain extent.Auxiliary particle filtering method (APF) utilizes current time system state observed reading that last one constantly particle is resampled, and therefrom selects at current time more near the particle of system's time of day.The filtering accuracy of this method is better than the standard particle filtering method in the practical application when system noise is less, but precision is not so good as the standard particle filtering method when system noise is big.SIR and APF resample on a discrete probability distribution, thereby all have the deficient problem of particle diversity.Different with the method for resampling of SIR and APF, regularization particle filter method (RPF) utilizes regularization method to obtain a continuous probability distribution and resamples thereon, thereby improved the deficient problem of particle diversity, but it has a theoretic defective, and the discrete probability distribution of the back gained that promptly resamples no longer can guarantee it is to converge to the system state true probability progressively to distribute.In the practical application when the deficient problem of particle diversity is serious this method be better than SIR and APF.Gaussian particle filtering method (GPF) adopts a Gaussian distribution to come approximation system state true probability to distribute on the basis of SIR, and resamples on this Gaussian distribution.This algorithm is progressive optimal algorithm when Gauss assumes immediately.

In sum, more than four kinds of methods characteristics are respectively arranged, all be more effective in the range of application that it was fit to.Yet in actual non-linear system status filtering, system state observation likelihood function usually has double-hump characteristics, and this population that can cause above-mentioned method for resampling to obtain also not exclusively accumulates near system's time of day, and then makes the state filtering precision relatively poor.When the observation likelihood function of system state variables has double-hump characteristics, this observation likelihood function also can form another peak region in certain zone (being called zone 2) away from system's time of day except form a peak region (being called zone 1) near system's time of day.If only utilize this observation likelihood function to compose weights, except near the particle system's time of day has higher weights, also can have higher weights away from the particle in certain zone of system's time of day to particle.At this moment, according to above-mentioned weights resample among the new particle group who obtains particle and not exclusively accumulate near system's time of day.

Summary of the invention

The present invention is in order to solve existing particle filter method when non-linear system status is carried out filtering, because of the observation likelihood function of system state variables has near double-hump characteristics cause the resampling population that obtains and not exclusively accumulating in the real system state, make the relatively poor problem of state filtering precision, a kind of particle filtering resampling method that is applicable to non-linear probabilistic system posture is provided.

The state-space model of described nonlinear stochastic system is as follows:

x _k=f (x _K-1, u _K-1) (system model) (1)

y _k=h (x _k, v _k) (observation equation) (2)

Wherein, f (.) and h (.) are known nonlinear functions, u _kAnd v _kBe respectively known system noise of probability density function and observation noise, x _kBe k system state constantly, y _kBe k x constantly _kObserved reading;

Make y _kThe observed reading of etching system time of day when representing k, { x _k(i): i=1 ..., N} represents k state sample set constantly, { x ^* _k(i): i=1 ..., N} represents from { x _k(i): i=1 ..., the state sample set that resamples among the N} and obtain, x ' _k(i): i=1 ..., N} represents a state sample set, here

x′ _k(i)＝f(x _k-1(i)，0)，i＝1，2，..，N (3)

The observed reading of above-mentioned each state sample can utilize following formula to calculate

$y_k^{*}\left(i\right)=h(x_k^{*}\left(i\right),0)$

y _k(i)＝h(x _k(i)，0)

(4)

y′ _k(i)＝h(x′ _k(i)，0)，i＝1，2，..，N

The step of particle filtering resampling method of the present invention is:

Step 1, the system state observed reading y when obtaining current time k _kAfter, the system state observed reading in nearest three moment is formed system state observation vector Y _k={ y _K-2, y _K-1, y _k; Calculate each state sample observed reading respectively and form state sample observation vector Y _k(i)={ y ^* _K-2(i), y _K-1(i), y ' _k(i) };

Under the similarity degree s (i) of step 2, the above-mentioned two kinds of observation vectors of calculating:

s(i)＝S(Y _k，Y _k(i))，i＝1，2，...，N. (5)

Wherein, on behalf of certain, S (.) can measure the function of two vectorial similaritys; The utilization index function is handled s (i) and is obtained s ^*(i) as follows:

s ^*(i)＝e ^α×s(i)，i＝1，2，...，N (6)

Wherein, α is a preassigned scale factor of need, α＞0, s ^*(i)＞0; Utilize the natural logarithm function to handle s (i) and obtain s ^*(i) as follows:

s ^*(i)＝|ln(s(i)/π+β)|，i＝1，2，...，N (7)

Wherein, β is a preassigned parameter of need, β＞0, s ^*(i) 〉=0; Here, the effect of β is just for fear of when s (i)=0 | and ln (s (i)/π) | → ∞;

Step 3, computing mode sample x _K-1(i) weights ω ^* _K-1(i) as follows:

${\mathrm{\ω}}_{k-1}^{*}\left(i\right)=s^{*}\left(i\right)\×p\left(y_{k-1}\right|x_{k-1}\left(i\right)),i=\mathrm{1,2},..,N---\left(8\right)$

Normalization ω ^* _K-1(i) under:

${\mathrm{\ω}}_{k-1}\left(i\right)={\mathrm{\ω}}_{k-1}^{*}\left(i\right)/\underset{n=1}{\overset{N}{\mathrm{\Σ}}}{\mathrm{\ω}}_{k-1}^{*}\left(n\right),i=\mathrm{1,2},..,N---\left(9\right)$

Step 4, with sample weights for choosing probability, from population { (x _K-1(i), ω _K-1(i)): i=1,2 ..., choosing also among the N}, the replication status sample arrives new state sample set { x ^* _K-1(i): i=1 ..., N}.

Method of the present invention realizes based on following principle: if the observation path of the close system state of the observation path of a particle, then the path of this particle should be near the path of system state.Utilize the similarity of these two kinds of observation path to adjust above-mentioned particle weights, make in the zone 1 particle have big weights and in the zone 2 particle have less weights, resample according to adjusted particle weights again.Thereby overcome system state observation likelihood function and had the relatively poor problem of filtering accuracy that double-hump characteristics brings.The weights of the particle that resamples have been utilized the similarity degree of particle observation vector and system state observation vector to adjust to participate in the inventive method.This method is different from the method for resampling of existing particle filter, is that existing particle filtering resampling method is not available.The present invention is applicable to finding the solution of general nonlinearity probabilistic system posture filtering problem, this method for resampling is better than existing particle filtering resampling method when system state observation likelihood function has double-hump characteristics, can improve the filtering accuracy of non-linear probabilistic system posture effectively.

Description of drawings

Fig. 1 adopts error mean square root (RMSE) to estimate the simulation result synoptic diagram of each algorithm performance.This method result when the Pearson correlation coefficient function is used in the PAP representative among the figure, this method result when the angle function is used in the PAA representative, the simulation result of standard particle filtering method is used in the SIR representative, APF represents the simulation result of auxiliary particle filtering method, RPF represents the simulation result of regularization particle filter method, and GPF represents the simulation result of Gaussian particle filtering method.

Embodiment

Embodiment one: the following particle filtering resampling method that is applicable to non-linear probabilistic system posture that specifies present embodiment.

The state-space model of described nonlinear stochastic system is as follows:

x _k=f (x _K-1, u _K-1) (system model) (1)

y _k=h (x _k, v _k) (observation equation) (2)

Wherein, f (.) and h (.) are known nonlinear functions, u _kAnd v _kBe respectively known system noise of probability density function and observation noise, x _kBe k system state constantly, y _kBe k x constantly _kObserved reading;

Make y _kThe observed reading of etching system time of day when representing k, { x _k(i): i=1 ..., N} represents k state sample set constantly, { x ^* _k(i): i=1 ..., N} represents from { x _k(i): i=1 ..., the state sample set that resamples among the N} and obtain, x ' _k(i): i=1 ..., N} represents a state sample set, here

x′ _k(i)＝f(x _k-1(i)，0)，i＝1，2，..，N (3)

The observed reading of above-mentioned each state sample can utilize following formula to calculate

$y_k^{*}\left(i\right)=h(x_k^{*}\left(i\right),0)$

y _k(i)＝h(x _k(i)，0)

(4)

y′ _k(i)＝h(x′ _k(i)，0)，i＝1，2，..，N

The step of particle filtering resampling method of the present invention is:

Step 1, the system state observed reading y when obtaining current time k _kAfter, the system state observed reading in nearest three moment is formed system state observation vector Y _k={ y _K-2, y _K-1, y _k; Calculate each state sample observed reading respectively and form state sample observation vector Y _k(i)={ y ^* _K-2(i), y _K-1(i), y ' _k(i) };

The similarity degree s (i) of step 2, the above-mentioned two kinds of observation vectors of calculating is as follows:

s(i)＝S(Y _k，Y _k(i))，i＝1，2，...，N. (5)

Wherein, on behalf of certain, S (.) can measure the function of two vectorial similaritys; The utilization index function is handled s (i) and is obtained s ^*(i) as follows:

s ^*(i)＝e ^α×s(i)，i＝1，2，...，N (6)

Wherein, α is a preassigned scale factor of need, α＞0, s ^*(i)＞0; Utilize the natural logarithm function to handle s (i) and obtain s ^*(i) as follows:

s ^*(i)＝|ln(s(i)/π+β)|，i＝1，2，...，N (7)

Wherein, β is a preassigned parameter of need, β＞0, s ^*(i) 〉=0; Here, the effect of β is just for fear of when s (i)=0 | and In (s (i)/π) | → ∞, generally the value of β should be less;

Step 3, computing mode sample x _K-1(i) weights ω ^* _K-1(i) as follows:

${\mathrm{\ω}}_{k-1}^{*}\left(i\right)=s^{*}\left(i\right)\×p\left(y_{k-1}\right|x_{k-1}\left(i\right)),i=\mathrm{1,2},..,N---\left(8\right)$

Normalization ω ^* _K-1(i) as follows:

${\mathrm{\ω}}_{k-1}\left(i\right)={\mathrm{\ω}}_{k-1}^{*}\left(i\right)/\underset{n=1}{\overset{N}{\mathrm{\Σ}}}{\mathrm{\ω}}_{k-1}^{*}\left(n\right),i=\mathrm{1,2},..,N---\left(9\right)$

Step 4, with sample weights for choosing probability, from population { (x _K-1(i), ω _K-1(i)): i=1,2 ..., choosing also among the N}, the replication status sample arrives new state sample set { x ^* _K-1(i): i=1 ..., N}.

Adopt the step of the particle filter of this method for resampling to be:

Steps A, obtain the observed reading y of current system state _kAfter, calculate above-mentioned { ω _K-1(i): i=1,2 ..., N} is from population { (x _K-1(i) ω _K-1(i)): i=1,2 ..., resample among the N} and obtain sample set { x ^* _K-1(i): i=1 ..., N};

Step B, utilize system model transmission { x ^* _K-1(i): i=1 ..., N} obtains sample set { x _k(i): i=1 ..., N};

Step C, calculate each sample x _k(i) likelihood function value p (y _k| x _kAnd normalization weights q (i)) _k(i) as follows:

$q_k\left(i\right)=\frac{p\left(y_k\right|x_k\left(i\right))}{\underset{n=1}{\overset{N}{\mathrm{\Σ}}}p\left(y_k\right|x_k\left(n\right))}---\left(10\right)$

Step D, estimating system state x _kAs follows:

Step e, return steps A.

Embodiment two: the difference of present embodiment and embodiment one is that Pearson correlation coefficient function in step 2 between use two vectors or the angle function between two vectors are as the function of two vectorial similaritys of tolerance.The span of s (i) is-1≤s (i)≤1 when using the Pearson correlation coefficient function, and the span of s (i) is 0≤s (i)≤π when using the angle function.

This method is being applied to following representative observation likelihood function when having in the nonlinear system of double-hump characteristics, and the system state filtering accuracy is better than SIR, APF, RPF and GPF algorithm.

$x_k=\frac{x_{k-1}}{2}+\frac{25x_{k-1}}{1+x_{k-1}^2}+8\cos \left(1.2k\right)+u_{k-1}$

$y_k=\frac{x_k^2}{20}+v_k$

We use total number of particles N=500, u _K-1～N (0,10), v _k～N (0,1), x ₀=0.1.When using the Pearson correlation coefficient function, get α=1; When using the angle function, get β=0.0000001.A simulation run time is 5000 time points.At first carry out 10 simulation runs, adopt error mean square root (RMSE) to estimate each algorithm performance, simulation result is presented among Fig. 1.This method result when the Pearson correlation coefficient function is used in the PAP representative among the figure, this method result when the angle function is used in the PAA representative.

Carry out 100 simulation runs again under similarity condition, adopt mean value and the variance of the RMSE of 100 simulation runs to estimate above-mentioned each method performance, simulation result is presented in the following table 1.

Table 1

	SIR	RPF	APF	GPF	PAP	PAA
							RMSE Mean (mean value of error mean square root)	5.850	5.985	5.433	4.867	4.636	4.655
RMSE Variance (variance of error mean square root)	0.004	0.007	0.018	0.016	0.014	0.015

。

Claims (2)

1, be applicable to the particle filtering resampling method of non-linear probabilistic system posture, the state-space model of described nonlinear stochastic system is as follows:

x _k＝f(x _k-1，u _k-1) (1)

y _k＝h(x _k，v _k) (2)

Wherein, f (.) and h (.) are known nonlinear functions, u _kAnd v _kBe respectively known system noise of probability density function and observation noise, x _kBe k system state constantly, y _kBe k x constantly _kObserved reading;

Make y _kThe observed reading of etching system time of day when representing k, { x _k(i): i=1 ..., N} represents k state sample set constantly, $\{{x^{*}}_k\left(i\right):i=1,...,N\}$ Representative is from { x _k(i): i=1 ..., the state sample set that resamples among the N} and obtain, x ' _k(i): i=1 ..., N} represents a state sample set, here

x′ _k(i)＝f(x _k-1(i)，0)，i＝1，2，..，N (3)

The observed reading of above-mentioned each state sample can utilize following formula to calculate

$y_k^{*}\left(i\right)=h(x_k^{*}\left(i\right),0)$

y _k(i)＝h(x _k(i)，0) (4)

y′ _k(i)＝h(x′ _k(i)，0)，i＝1，2，..，N

The step that it is characterized in that this method is:

Step 1, the system state observed reading y when obtaining current time k _kAfter, the system state observed reading in nearest three moment is formed system state observation vector Y _k={ y _K-2, y _K-1, y _k; Calculate each state sample observed reading respectively and form the state sample observation vector $Y_k\left(i\right)=\{{y^{*}}_{k-2}\left(i\right),y_{k-1}\left(i\right),{y^{\′}}_k\left(i\right)\};$

The similarity degree s (i) of step 2, the above-mentioned two kinds of observation vectors of calculating is as follows:

s(i)＝S(Y _k，Y _k(i))，i＝1，2，...，N. (5)

Wherein, on behalf of certain, S (.) can measure the function of two vectorial similaritys; The utilization index function is handled s (i) and is obtained s ^*(i) as follows:

s ^*(i)＝e ^α×s(i)，i＝1，2，...，N (6)

Wherein, a is a preassigned scale factor of need, a＞0, s ^*(i)＞0; Utilize the natural logarithm function to handle s (i) and obtain s ^*(i) as follows:

s ^*(i)＝|ln(s(i)/π+β)|，i＝1，2，...，N (7)

Wherein, β is a preassigned parameter of need, β＞0, s ^*(i) 〉=0; Here, the effect of β is just for fear of when s (i)=0 | and In (s (i)/π) | → ∞;

Step 3, computing mode sample x _K-1(i) weights ω ^* _K-1(i) as follows:

${\mathrm{\ω}}_{k-1}^{*}\left(i\right)=s^{*}\left(i\right)\×p\left(y_{k-1}\right|x_{k-1}\left(i\right)),i=\mathrm{1,2},..,N---\left(8\right)$

Normalization ω ^* _K-1(i) as follows:

${\mathrm{\ω}}_{k-1}\left(i\right)={\mathrm{\ω}}_{k-1}^{*}/\underset{n=1}{\overset{N}{\mathrm{\Σ}}}{\mathrm{\ω}}_{k-1}^{*}\left(n\right),i=\mathrm{1,2},..,N---\left(9\right)$

Step 4, with sample weights for choosing probability, from population { (x _K-1(i), ω _K-1(i)): i=1,2 ..., choosing also among the N}, the replication status sample arrives the new state sample set $\{{x^{*}}_{k-1}\left(i\right):i=1,...,N\}.$

2, the particle filtering resampling method that is applicable to non-linear probabilistic system posture according to claim 1 is characterized in that in step 2 using Pearson correlation coefficient function between two vectors or the angle function between the two vectors function as two vectorial similaritys of tolerance; The span of s (i) is-1≤s (i)≤1 when using the Pearson correlation coefficient function, and the span of s (i) is 0≤s (i)≤π when using the angle function.

Images