An Auxiliary Particle Filtering Algorithm With Inequality Constraints

For nonlinear non-Gaussian stochastic dynamic systems with inequality state constraints, this technical note presents an efficient particle filtering algorithm, constrained auxiliary particle filtering algorithm. To deal with the state constraints, the proposed algorithm probabilistically selects particles such that those particles far away from the feasible area are less likely to propagate into the next time step. To improve on the sampling efficiency in the presence of inequality constraints, it uses a highly effective method to perform a series of constrained optimization so that the importance distributions are constructed efficiently based on the state constraints. The caused approximation errors are corrected using the importance sampling method. This ensures that the obtained particles constitute a representative sample of the true posterior distribution. A simulation study on vehicle tracking is used to illustrate the proposed approach.


An Auxiliary Particle Filtering Algorithm With Inequality Constraints
Baibing Li, Member, IEEE, Cunjia Liu, Member, IEEE, and Wen-Hua Chen, Senior Member, IEEE Abstract-For nonlinear non-Gaussian stochastic dynamic systems with inequality state constraints, this technical note presents an efficient particle filtering algorithm, constrained auxiliary particle filtering algorithm.To deal with the state constraints, the proposed algorithm probabilistically selects particles such that those particles far away from the feasible area are less likely to propagate into the next time step.To improve on the sampling efficiency in the presence of inequality constraints, it uses a highly effective method to perform a series of constrained optimization so that the importance distributions are constructed efficiently based on the state constraints.The caused approximation errors are corrected using the importance sampling method.This ensures that the obtained particles constitute a representative sample of the true posterior distribution.A simulation study on vehicle tracking is used to illustrate the proposed approach.Index Terms-Auxiliary particle filter, Bayesian inference, inequality constraints, sequential Monte Carlo, state space models.

I. INTRODUCTION
In practice, nonlinear stochastic dynamic systems are often restricted to a sub-area of the state space.This is usually the consequence of some physical restrictions on the systems of interests.For example, in vehicle tracking, vehicles on a road are expected to move within the boundaries of the road and to follow the speed limit (see, e.g., [1]).Because state constraints reduce the variability of the state vector, incorporating the constraintrelated information into state estimation can usually improve on the accuracy of state filtering [2].
State estimation with state constraints is very challenging and has attracted a number of researchers.With the conventional filtering approaches such as the extended Kalman filter (EKF) and unscented Kalman filter (UKF), many approaches have been developed to deal with state constraints but most of them focus on linear and/or equality constraints.This includes the approaches of reparameterizing, pseudo-measurement, optimization, projection and truncation.Recently a truncationbased method has been developed in [3].It is based on the above conventional filters but a novel Monte Carlo method is used to deal with the inequality constraints.Moving horizon estimator (MHE), on the other hand, performs state estimation by solving an optimization problem that can inherently handle constraints.However, the multi-stage nonlinear optimization in MHE incurs excessive computational burden for online applications [4].See [5] for an overview on the conventional filtering approaches with state constraints.
In the literature, there are only a few particle filters developed for dealing with state constraints, among which the pioneering research work in [6] and [7] is particularly worth noting.In [6], a simple algorithm using the acceptance-rejection method was developed.The acceptance-rejection method is commonly used in offline Monte Carlo methods where computation time is not an issue.However, it is in general not suitable for online applications because the time required for drawing a particle that satisfies the state constraints could be prohibitively long.To circumvent this problem, a novel approach was proposed in [7] that consists of two stages.First, particle candidates are drawn without using the state constraints; then the candidates that violate the constraints are projected into the feasible area defined by the constraints using a series of optimization.This method incurs a high computational cost and the particles obtained using the deterministic projection are no longer a representative sample of the posterior distribution of the state vector.
Recently, [8] has made some solid advances in particle filtering with state constraints and proposed three algorithms.The first method is a Gibbs-sampling-based algorithm that is more relevant to this technical note as the second method in [8] is based on the acceptance-rejection and the posterior updating in the third method does not use the constraint information.To draw each particle at each time step, a Gibbs sampler is embedded in the Gibbs-sampling-based algorithm so that only one variable is considered in each iteration of the Gibbs sampler.As a result, sampling within the state space reduces to a univariate problem, i.e. sampling within an interval, that can be dealt with straightforwardly.This, however, comes at a price of high computational demand.This is because the Gibbs sampler is an iterative procedure that is usually used as an offline method.Embedding a Gibbs sampler for generating particles incurs excessive computation costs.Moreover, within each iteration of the Gibbs sampler, it needs to solve the state constraints for the end-points of the constraint interval of each state variable.This has a high computational demand when the analytical solutions are not available.
The Gibbs-sampling-based algorithm also has some restrictive requirements on the systems under investigation.It is required that: (a) both the conditional cumulative distribution of each state variable and its inverse function can be evaluated This work is licensed under a Creative Commons Attribution 3.0 License.For more information, see http://creativecommons.org/licenses/by/3.0/analytically; (b) these conditional cumulative distributions can be sampled easily; and (c) the constraint region for the univariate sampling is a single interval.In practice these assumptions may not be valid.Take requirement (c) as an example: the obtained constraint region may consist of several non-overlapped intervals because a nonlinear constraint function in general leads to more than one solution.
In this technical note, we propose a new method, constrained auxiliary particle filtering algorithm (CAPFA).Similar to the ordinary auxiliary particle filter (APF), the CAPFA consists of resampling and sampling stages at each time step.In the resampling stage, it probabilistically selects particles such that particles with a lower likelihood of output measurements and/or far away from the feasible area are less likely to survive and propagate into the next time step.For this end, we carry out a series of constrained optimization to work out the center (as measured by the mode) of each transition distribution within the feasible area from which the particles are to be drawn.To improve on the sampling efficiency in the sampling stage, we form each importance distribution in a way such that it is centered at the mode of the corresponding restricted transition distribution.In doing so, the chance of drawing a feasible particle is much higher as the mode lies within the feasible area.Finally we use the importance sampling method to correct the errors caused in the previous stages.
In comparison with [8], the proposed method is computationally more efficient and hence suitable for online applications.Its assumptions on the system are also less restrictive and hence it can be applied to a wider range of problems.
This technical note is structured as follows.Section II is devoted to problem formulation.In Section III we present the main results and in Section IV we conduct a simulation study for algorithm evaluation.Finally we conclude this technical note in Section V.

II. PROBLEM FORMULATION
Consider a nonlinear stochastic dynamic system: with the observation equation where We denote the distribution of x k conditional on x k −1 by p(x k |x k −1 ) which can be derived from (1).Likewise, we denote the distribution of z k conditional on x k by p(z k |x k ) which can be derived from (2); see, e.g., [9] and [10] for further discussion.
Suppose that x k satisfies the following constraints: where g k = [g 1,k , . . ., g M ,k ] T : n → M is a vector of possibly nonlinear functions of x k .Each g j,k (x k ) is assumed to have continuous second-order partial derivatives.Let B k = {x k |g k (x k ) ≤ 0} denote the feasible area of x k that satisfies the constraints.Following the literature (e.g., [2], [10]), the inequality (3) is treated as pseudo-measurements:

III. MAIN RESULTS
We aim to obtain the posterior distribution p(x 0:k |z 1:k ) of the sequence of state vector x 0:k at each time step k for given observation sequence z 1:k , the initial state vector x 0 , constraints (3), and knowledge about system (1)-(2).
When the system is nonlinear/non-Gaussian, usually p(x 0:k |z 1:k ) is analytically intractable, and hence a numerical solution is required in practice.Theoretically this can be obtained by drawing a representative sample of size N from p(x 0:k |z 1:k ), termed particles.Unfortunately, for many complex practical problems, it is usually impossible to draw such a sample directly from p(x 0:k |z 1:k ).In the particle filtering, importance sampling is used to circumvent this difficulty, where the particles x i 0:k (i = 1, . . ., N) are drawn from an importance distribution π(x 0:k |z 1:k ) instead of p(x 0:k |z 1:k ) per se.A correction step with a set of weights w i k (i = 1, . . ., N) is required to ensure that the obtained particles constitute a representative sample of the true posterior distribution: where δ(.) is the Dirac measure and the importance weights are given by w The importance weights need to be normalized, i.e. w i k are replaced with w i k / N j =1 w j k , so that the normalized weights sum to one.
The approximation in ( 5) is based on the law of large number.A common problem with the particle filtering is the degeneracy phenomenon, where after a few iterations, all but one particle will have a negligible weight, leading to a very poor approximation to the true posterior distribution in (5) and hence poor quality of statistical inference for the state vector.The degree of degeneracy is characterized by the effective sample size (ESS) defined to be where w k (p, π) = p(x 0:k |z 1:k )/π(x 0:k |z 1:k ) is the true weight function [11].In numerical computation, ESS is estimated by Neff = [ N i=1 (w i k ) 2 ] −1 .When designing algorithms for particle filtering, one should endeavour to ensure that the ESS is as large as possible [12].

A. Why Use Inequality Constraints in State Filtering?
Following [2] and [10], we consider a dynamic system (1) and treat the constraints (3) as pseudo measurements.During the filtering, the pseudo measurements (4) together with the output measurements (2) will be used to form the likelihood function to draw inference about the state vector.We will show that, for any importance distribution for which state constraints (3) are not taken into account, one can always construct a modified importance distribution so that the particles drawn from the modified importance distribution can better represent the posterior distribution.For ease of exposition, the measurements z 1 , . . ., z k are not written explicitly and the subscript for time step k is dropped in this subsection.
Consider an approximate posterior distribution which pools the prior information and the observation equation ( 2) without consideration of constraints (3).Let p(x) with p(x) dx = 1 denote the probability density function of this approximate posterior distribution.Suppose that an importance distribution π(x) with π(x)dx = 1 is used to draw the particles.
From Bayes' theorem, the true posterior probability density function where the pseudo-measurements are taken into consideration is given by p(x|y) = c 0 p(x)p g (y|x) with c 0 = [ p(x)p g (y|x) dx] −1 .Now we take into account the constraints to construct a modified importance distribution: π(x) = cπ(x)p g (y|x), where c = [ π(x)p g (y|x) dx] −1 ≥ 1.The corresponding weight function is w(x) = p(x|y)/π(x).In addition, when using the importance distribution π(x), the weight function is w(x) = p(x|y)/π(x).The following theorem shows that Var[w(x)] ≤ Var[ w(x)].
Theorem 1: Let p g (y|x) be the probability density function of the pseudo-measurements defined in (4), and p(x) and π(x) be the approximate posterior distribution and the corresponding importance distribution where the constraints are ignored.Let p(x|y) = c 0 p(x)p g (y|x) with c 0 = [ p(x)p g (y|x) dx] −1 be the true posterior distribution where the constraints are taken into consideration.Then one can always construct a modified importance distribution π is the weight function of the importance distribution π(x).
From Theorem 1, by taking into account the state constraints during the sampling, the weights are always more evenly distributed.
Corollary 1: Let N eff and Ñeff denote the ESS values corresponding to weight functions w(x) and w(x) respectively.Then under the assumptions of Theorem 1, we have N eff ≥ Ñeff .
The proof is immediate from Theorem 1. Corollary 1 indicates that the ESS is always larger when the state constraints are taken into consideration.Because particle filtering is based on the law of large numbers, Corollary 1 has important implications: a smaller ESS of particles will in general lead to a poorer representation of the posterior distribution with inferior quality of state estimation.Now define G eff = 100 × [(N eff − Ñeff )/ Ñeff ]% as the gain in ESS when the state constraints are taken into account in the particle filtering.The benefits of using the constraint-related information in particle filters can be measured as follows.
Corollary 2: Under the assumptions of Theorem 1, the gain in ESS by using the constraint-related pseudo-measurement information in the particle filtering is given by Then by definition, we obtain N eff = cN/a and Ñeff = N/a.
When the state constraints are accounted for in particle filtering, Corollary 2 shows that the smaller the feasible area is, the more the gain in ESS can be obtained when the state constraints are taken into account.

B. A Brief Summary of the Auxiliary Particle Filter
Consider system (1)-( 2) without any constraints.The posterior distribution at time k can be derived using Bayes' rule: In the sequential Monte Carlo particle filtering, the importance weights for system (1)-( 2) is updated recursively: for this choice.This importance distribution is chosen in a way that does not take into account the output measurements z 1:k .Hence, it may be inefficient since the state space is explored without any knowledge of the observations.
A distinctive nature of the APF is that its importance distributions incorporate such information.The auxiliary particle filter (APF) was initially proposed in [13] and [14].The APF algorithm draws particles on the basis of (6) in two stages [12], [13]: (a) draw indices I i with the particle selection probabilities proportional to w i k −1 p(z k |x i k −1 ); and (b) draw each particle In many applications, it is difficult to evaluate this predictive likelihood.One simple solution is to use the approximation p(z k |λ i k ), where λ i k is the center (as measured by mean, median, or mode) of p(x k |x i k −1 ) (e.g., [9]).Note that when p ω (ω k −1 ) is Gaussian, p(x k |x i k −1 ) is also Gaussian.In this case, the mean, median, and mode of p(x k |x i k −1 ) are all equal, and the centers of the importance distributions can be obtained straightforwardly, i.e.
) is required to ensure the set of obtained particles represents the true posterior distribution.
The APF can be regarded as an interchange of the importance sampling and resampling procedures in the generic particle filtering (see, e.g., [12], [15]): the resampling stage is performed prior to the sampling stage.Consequently only those particles with a higher particle selection probability are likely to survive and propagate into the next step.

C. Design of the CAPFA
We now take into account state constraints and develop a new filtering algorithm CAPFA based on the APF.The design of the CAPFA will take advantages of the APF to improve on the sampling efficiency in the presence of state constraints.
Following [2], [10], we consider the inequality constraints (3) as pseudo-measurements with the density function p g (y k |x k ).This leads to a generalized likelihood function p(z k |x k )p g (y k |x k ).Applying Bayes' rule to pool this generalized likelihood with the prior distribution p(x k |x k −1 ), we obtain the posterior distribution of x 0:k at time step k: The key idea of the CAPFA is to select particles to propagate into the next time step by using the constraint-related information.Bearing in mind that the constraints are now treated as pseudo-measurements that modify the likelihood to be p(z k |x k )p g (y k |x k ), we follow the design of the APF and select the particles in the CAPFA with probabilities proportional to this modified likelihood multiplied by the corresponding weight w i k −1 .In addition, similar to the APF, the unknown x k at this stage is replaced with the center of the restricted transition distribution p g (y k |x k ) p(x k |x i k −1 ), denoted as λ i k .In summary, the particles in the resampling stage of the CAPFA are selected with probabilities proportional to . The overall structure of the CAPFA is designed in a similar way to the APF: (a) in the resampling stage: we find the centers λ i k of the restricted transition distributions p g (y k |x k ) p(x k |x i k −1 ), and draw indices I i with the particle selection probabilities proportional to ψ i k ; and (b) in the sampling stage: we draw a particle from each p g (y k |x k )p(x k |x I i k −1 ) for given I i .These two issues will be investigated in the next two subsections.

D. Resampling Stage: Locating the Center of Each
Mathematically, the mode λ i k is defined to be the point within the feasible area B k that minimises − log[p(x k |x i k −1 )] (i = 1, . . ., N).At the first glance, the task of searching the mode λ i k looks daunting as N is usually large.However, there is no need to get the exact solution for each optimization problem because the solution is used only for forming an importance distribution, and any approximation error can be corrected using the weight correction method in the importance sampling.In our experience, one-step iteration will usually suffice; the benefit of using more iterations, if any, is marginal.See the simulation study in Section IV for details.Specifically, we define ] about f i k to its second-order and use this second-order expansion to approximate the original optimization problem for λ i k .We suppress subscript k for notational simplicity.The original optimization problem for λ i can be approximated by a series of constrained optimization problems P i (i = 1, . . ., N), each having a quadratic objective function: For problems P i (i = 1, . . ., N), if f i − (V i ) −1 D i falls into the feasible area, it is the exact optimal solution.Let A denote all the indices i that f i − (V i ) −1 D i is outside the feasible area, i.e.
In the rest of this subsection, we only focus on problems P i for i ∈ A.
We use a simple interior-point method (SIPM) described below to solve problem (8) for i ∈ A; see [16] for an overview on various interior-point methods.We choose a barrier function as B(x) = −1/x.Then solving problem (8) can be converted to solving the following problem for a suitably chosen γ > 0: Suppose that an initial value x (0) ∈ B has been chosen.The Newton method can be used to solve (9) and it terminates after the first iteration.The one-step solution is taken as an approximate solution: where α * is the step length.
is the search direction and H i ∇ 2 J i , where J i denotes the objective function of problem (9).There are two issues to be addressed: (a) the choice of step length α * ; and (b) the choice of the initial value x (0) .
For the selection of α * , we use the widely-used linearization method, i.e. we linearize constraint functions g j (x) and solve (8) with the linearized constraint functions.It is easy to show that the optimal solution to this problem is given by Occasionally for the step length α * obtained in (11), we have In this case, we shrink α * by a preselected factor β (0 < β < 1) for a couple of times until all the constraints are satisfied.As this is a linear search problem, we suggest using either the bisection method β = 0.5 or golden section method β = 0.618.
Since all the optimization problems P i have a common feasible area, a common initial point x (0) can be used for all P i .We suggest using the weighted mean or mode (for the case that the mean lies outside the feasible area) of all the feasible f i as the initial point x (0) .Specifically, let us re-introduce the subscript k for each time step for the clarity purposes.We define

E. Sampling Stage: Drawing Particles From Restricted Transition Distributions
Now we turn to consider how to efficiently sample from each unnormalized restricted transition distribution p g (y k |x k ) p(x k |x I i k −1 ).A simple approach would be to use the acceptance-rejection method.However, the computational cost for this method could be prohibitively high if the center of p(x k |x I i k −1 ) is far away from B k .To address this issue, we use an importance-samplingbased method as follows.Consider any particle with index I i .We construct an importance distribution π(x k |λ I i k ) for the particle as p(x k |x I i k −1 ) with its mean translated to λ . Hence, rather than to draw a particle from the unnormalized distribution p g (y is more likely that the candidate particle drawn from π(x k |λ I i k ) will fall into the feasible area, hence improving on the sampling efficiency, as illustrated in Fig. 1.
Because particle x i k is drawn from π(x k |λ I i k ), the importance weight needs to be adjusted by a factor to ensure that the obtained particles as a whole constitute a representative sample of the true posterior distribution.Combining ψ i k with ρ i k , we obtain the importance weight for each particle x i k (i = 1, . . ., N) in the CAPFA: Finally, due to the indicator function in (12), we resample the obtained particles.After the resampling, all particles fall into the feasible area with a non-zero weight.The CAPFA is outlined in Algorithm 1.
In some applications, the covariance matrix of distribution p ω (ω k ) is rank-deficient.If this is the case, we follow [17] (Appendix B) to transform the problem into a sub-space in which the random variables have a full-rank covariance matrix and to draw the relevant samples in the sub-space.

IV. SIMULATION STUDY
In this section, we evaluate the proposed CAPFA using a vehicle-tracking simulation study.Similar problems were also investigated in many previous studies such as [3].

A. Simulation Settings
Consider a typical vehicle tracking problem with a moving vehicle traveling on a bend road section.The boundaries of the road are defined by two ellipse curves centred at the origin of a Cartesian coordinate system in the first quadrant with major radius of a 1 = 96 m and a 2 = 100 m, and minor radius b 1 = 76 m and b 2 = 80 m.The example vehicle trajectory is depicted in Fig. 2, where the vehicle starts from the position (98, 0) and travels first in the central area of the road, and then moves close to the right and left part of the road respectively.
The vehicle dynamics are described by the following white noise acceleration motion model: where the state vector x k = [ x 1,k x 2,k ẋ1,k ẋ2,k ] T consists of the vehicle position and velocity components in x 1 and x 2 directions, T = 1 s is the sampling interval, and ω k is a two-dimensional Gaussian process noise with zero mean and identity covariance matrix.Given the road boundaries, the state constraints can be written as The vehicle is tracked by a range and bearing sensor modeled as: where υ k is a two-dimensional Gaussian noise vector with zeromean and covariance matrix R = diag{8, 0.002}.
For the above tracking problem, simulation experiments were carried out on a PC with 2.5 GHz CPU. 100 Monte Carlo runs were undertaken where different realisations of the noises were generated based on a pre-specified trajectory.In the state estimation, the initial prior distribution was chosen as a Gaussian  distribution with mean x0 = [95, 5, 0, 10] T and covariance matrix P 0 = diag{25, 25, 1, 1}.
The performance of the CAPFA was assessed in terms of accuracy measured by the average mean square error (MSE) of the position-related variables and the corresponding standard deviation (SD) of the MSE over the 100 runs; the latter characterizes the reliability and stability of the filtering results from realization to realization.
The quality of the obtained particles was measured by Percentage of ESS (PESS) defined as the ratio of ESS to N , Percentage of Feasible Particles (PFP) prior to the final resampling at each time step, and computation time (CT) measured by the time required in one run of the experiment.

B. The CAPFA With Different Optimization Methods
First, we compare the performance of the CAPFA using the one-step iteration SIPM with γ = 20 to that of the CAPFA where the optimization was undertaken using a Matlab standard solver, 'fmincon', which uses the Sequential Quadratic Programming (SQP) algorithm to solve a constrained optimization problem.The results are summarised in Table I.
It can be seen from Table I that, for the same particle number N , the benefit of using the standard 'fmincon' solver in the CAPFA was marginal in terms of average MSE, SD and PESS.The main drawback of using the standard optimization technique is the high computational cost, which may be problematic in some real-time applications where the state information needs to be updated rapidly.We also see that the benefit of the improved MSE using the 'fmincon' solver with N = 150 or N = 250 can be achieved by using the SIPM with a larger N (say N = 500) at a much lower computational cost.The case of N = 500 with the SQP optimization method is not reported here due to the high computational demand.
We also tried other choices for γ, i.e. γ = 5, 10, 40.The obtained results for N = 250 are displayed in Table II which indicates that the CAPFA is not sensitive to the choice of γ.This robustness property is important in practice.In the rest of the simulations in this section, we set γ = 20.

C. Measuring the Benefits of Employing State Constraints
Now we gauge the gain of using the state constraints in the particle filtering.We compared the APF with and without the constraints during the sampling.The numerical results are displayed in the first two panels of Table III.
The vehicle was required to move within the road.The PFP values in Table III, however, show that about 55% of the APF particles fell outside the road.Hence, the ordinary APF may provide biased information on the position of the vehicle.
The PFP of the CAPFA is about 88% prior to the final resampling at each time step.After the final re-sampling, all the particles obtained by the CAPFA fall into the feasible area, leading to PFP = 100%.The use of the road constraints in the CAPFA substantially increased the ESS.For N = 250, the gain in ESS by using the CAPFA compared to the APF was G eff = 100% × (75.3 − 42.7)/42.7 = 76.4%.For the estimation error, the average MSE of the CAPFA was only about 100% × 5.022/7.051= 71.2% of the APF.

D. Comparison With Other Algorithms
In this subsection we compare the proposed CAPFA to some existing particle filters with state constraints.
It was shown in [7] that the constrained particle filter (CPF) in [7] outperformed the acceptance-rejection method in [6] and several conventional filtering methods including the EKF, constrained EKF, UKF, constrained UKE and MHE.Hence, we first consider CPF Algorithm 2 in [7] (denoted as CPF(alg.2 [7])) for which α = 0.05 was used in the chi-square test.The results are displayed in the third panel of Table III.
It can be seen that the CPF Algorithm 2 in [7] had a reasonable computation time.This is mainly because the chi-square test is used to reduce the times that the optimization routine is called: when the test is passed, no optimization is undertaken even if there are still infeasible particles.In our simulation study, we noted that this chi-square test was effective for the reduction of computation time at the price that a substantial proportion of the particles did not satisfied the constraints: its PFP was about 59%, hence there were about 41% of the particles lying outside the feasible area.This further caused another important issue: the corresponding PESS of the CPF Algorithm 2 was very low.It was only about 1/3 of the nominal size N .Low PFP and PESS levels affect the quality of inference.This can be seen from Table III that when the total number of particles N was not large (150 and 250), both the MSEs and SDs were relatively high.Note that a high level of SD indicates that the quality of the state filtering was not stable.This problem was alleviated when N (and hence the corresponding effective sample size) became large.This suggests that particle number N need to be set large for the CPF to cope with the lower percentages of ESS and feasible particles.We also tested Algorithms 3 and 4 of the CPF in [7]; they had a similar performances to the CPF Algorithm 2.
We next turn to consider the Gibbs-sampler-based particle filter in [8], denoted as CPF(Gibbs [8]) in Table III.First, we note that this method can guarantee all particles fall into the feasible area and hence its PFP is 100%.It can also be seen from Table III that the PESS and accuracy as measured by MSE and SD were better than that of the CPF Algorithms in [7], but not as good as the CAPFA.More importantly, its computational cost was much higher than all the other filters.The Gibbs-sampler-based particle filter involves: (a) the iterative approach of the Gibbssampler for every particle; and (b) determining the constraint interval within each iteration of the Gibbs sampling.To speed up the computation process, the constraint interval was solved analytically in our simulation study and hence the computation in (b) was kept at the minimum level.If it was impossible to analytically solve problem (b), then searching a numerical solution for (b) would incur a much higher computational cost.Overall, it seems that the Gibbs-sampler-based particle filter in [8] is more suitable to offline applications due to its computational demand.
Finally, we considered two particle filters recently proposed in [18] and [19], denoted as tUPF( [18]) and tUPF ( [19]) respectively in Table III.For both methods, the importance distributions are based on the UKF approach that is further supplemented with a truncation technique to accommodate the state constraints.They have 100% PFP because all the particles outside the feasible are redrawn.We can see from Table III that, in comparison with tUPF( [18]), tUPF( [19]) is more accurate but it has a higher computational cost.The CAPFA outperforms both of them in terms of MSE, SD, and PESS.The CAPFA has a comparable computation time to tUPF( [18]), and is much faster than tUPF( [19]).Note that for tUPF( [19]), the cases for N = 150 and 250 are not reported here because it did not work properly due to a very low ESS.

E. Impact of a Poor Initial Condition
It is well known that a poorly chosen initial point for a filter can have a negative impact on the performance of the filter, in particular during an initial time period.Next, we consider a poor initial guess chosen as a Gaussian distribution with mean x0 = [90, 15, 0, 10] T and covariance P 0 = diag{100, 100, 1, 1}.Due to the poor initial guess, most of the algorithms in Table III large particle numbers (N = 500 or above) to obtain a reasonable result in the test.However, the proposed CAPFA performed well even with N = 250 which is therefore included here for the comparison purposes.The results are displayed in IV.
Table IV shows that with a poorer initial guess: (a) the MSE became larger for all the filters; (b) the CAPFA with both N = 250 and 500 outperformed the others; (c) the CAPFA was fairly robust against the poor initial condition.
In summary, the simulation study shows that the PESS level of the proposed CAPFA is superior over the existing algorithms, leading to a satisfactory filtering accuracy with a reasonably low computational cost.

V. CONCLUDING REMARKS
In this technical note we have proposed a new particle filtering algorithm, CAPFA, to deal with state estimation with state constraints.We have shown that the CAPFA has three distinguished features for dealing with the state constraints: (a) a highly selective method that chooses particles for propagating into the next time step; (b) an effective optimization method for locating the center of each restricted transition distribution; and (c) a method that efficiently constructs the importance distributions using knowledge of the state constraints.
The simulation study shows that the proposed SIPM method took only a fraction of time used by the 'standard' optimization method with little price of performance degradation.It also demonstrates the benefits of incorporating state constraints in the particle sampling: the CAPFA had a much larger ESS in comparison with the existing particle filters.
This technical note assumes that the probability density function of the system noise and the constraint functions have continuous second-order partial derivatives.Primarily these conditions are used in the SIPM.Hence, if necessary, these assumptions can be relaxed by replacing the SIPM with a suitable derivativefree optimization method.We also note that the assumptions of the proposed method exclude some realistic models of practi-cal interests where (a) the uncertainty in the state equation is modeled as a non-additive noise without a density; and (b) the state constraint is defined implicitly by a feasible set, where it is just possible to check whether a point is inside or outside the feasible set, but no simple explicit expression is available in terms of regular enough constraint functions.Further research is required to overcome these limitations.
Let x 0:k = {x 0 , x 1 , . . ., x k } and z 1:k = {z 1 , . . ., z k } denote the set of all states and the set of all output measurements up to time k respectively.
the unnormalized density function of the pseudo-measurements y k , where I{x k ∈ B k } is an indicator function, having 1 if x k ∈ B k and 0 otherwise.

TABLE IV COMPARISON
OF THE PERFORMANCES OF SEVERAL PARTICLE FILTERS WITH A POOR INITIAL GUESS AVERAGED OVER 100 RUNS