Progressive Gaussian Filtering  Uwe D. Hanebeck, Jannik Steinbring Intelligent Sensor-Actuator-Systems Laboratory (ISAS) Institute for Anthropomatics Karlsruhe Institute of Technology (KIT), Germany  uwe.hanebeck@ieee.org,jannik.steinbring@student.kit.edu  Abstract  In this paper, we propose a progressive Bayesian procedure, where the measurement information is continuously included into the given prior estimate (although we perform observations at discrete time steps). The key idea is to derive a system of ordinary first-order differential equations (ODE) by employing a new coupled density representation comprising a Gaussian density and its Dirac Mixture approximation. The ODE is used for continuously tracking the true non-Gaussian posterior by its best matching Gaussian approximation. The performance of the new filter is evaluated in comparison with state-of-the-art filters by means of a canonical benchmark example, the discrete-time cubic sensor problem.  1. Introduction  We consider state estimation in discrete-time stochastic nonlinear dynamic systems. Thanks to their simplicity, Gaussian filters, i.e., filters representing all state densities by Gaussians, are an attractive tool for solving this type of estimation problem. However, in general, their full estimation performance is not exploited due to additional assumptions and simplifications. In this paper, we focus on Gaussian filters that operate by finding the best-matching Gaussian to the true posterior by means of an explicit shape optimization after every processing step. A filter of this type is called Gaussian-assumed density filter (GADF). In case of the filter step, the actual measurement is considered during the approximation, which leads to better results than the commonly used type of Gaussian filter, the so called Linear Regression Kalman Filter (LRKF) [   1 ]. LRKFs approximate the (non-Gausian) joint density of state and measurement corresponding to the prior density, the noise density, and the given nonlinear measurement equation by a jointly Gaussian density, which is independent of the actual measurement and typically is only a very rough approximation of the true joint density. Examples of LRKFs [   2 ] are the Unscented Kalman Filter (UKF) [   3 ], and its scaled version [ 4 ], its higher-order generalization [   5 ], a generalization to an arbitrary number of deterministic samples placed along the coordinate axes [ 6 ], filters performing an analytic or semi-analytic calculation of the required moments [ 7 ], [   8 ] based on a decomposition into parts that can be calculated in closed form or via a sample approximation [   9 ], and filters based on numerical integration for calculating the required nonlinear moments of the prior Gaussian density [   10 ]. Of course, GADFs are more complicated to implement in comparison to the LRKFs and there are various options for minimizing the shape deviation between the true posterior and its Gaussian approximation. One option is to employ moment matching, i.e., using the mean and covariance matrix of the true posterior as parameters for the desired Gaussian, as this is  arXiv:1204.0133v1 [cs.SY] 31 Mar 2012
known to minimize the Kullback-Leibler distance between the two densities. Unfortunately, in the case of nonlinear measurement equations and the corresponding complicated Likelihood function, it is in general not a simple task to calculate mean and covariance matrix of the true posterior, as analytic solutions are a rare exception. In most cases, numerical integration over the true posterior, i.e., the product of the (Gaussian) prior density and the Likelihood function, is in order, such as Monte Carlo integration [11]. In this paper, we propose a progressive Bayesian procedure for Gaussian-assumed density filtering, where the measurement information is continuously included into the given prior estimate (although we perform observations at discrete time steps).   The first progressive filtering procedure has been developed in 2003 [   12 ] for state estimation with a Gaussian Mixture representation in the scalar case, where a homotopy continuation approach was proposed for tracking the true posterior with its approximation minimizing a squared-integral distance measure. The multi-dimensional case was treated in [   13 ]. A generalization of this method to various another distance measures is proposed in [   14 ]. Besides state estimation, the progressive processing idea has been applied to moment calculation [   15 ] and Gaussian Mixture reduction [   16 ]. A homotopy-based filtering method operating on a particle representation is given in [17]. This paper is a further development of the procedure [ 13 ] that was developed for state estimation with a Gaussian Mixture representation.   The key idea is to derive a system of ordinary first-order differential equations (ODEs) by employing a new coupled density representation comprising a continuous and a discrete part, specifically a Gaussian density and its Dirac Mixture approximation [ 18 ]. Dirac Mixture approximations for nonlinear estimation and filtering have been proposed for the case of scalar continuous densities in [ 19 ], [   20 ] An algorithm for sequentially increasing the number of components is given in [   21   ] and applied to recursive nonlinear prediction in [   22 ]. Multi-dimensional Gaussian densities are treated in [   18 ]. Of course, more complicated continuous densities can be handled with this approach such as Gaussian Mixture densities, which is outside the scope of this paper. The system of ordinary first-order differential equations is used for continuously tracking the true non-Gaussian posterior by its best matching Gaussian approximation. In contrast to [   13 ], where a squared-integral distance measure between the true and the approximate posterior was minimized, here we derive ODEs for directly tracking the mean and covariance of the true posterior density by its Gaussian approximation. The new progressive estimation method allows for arbitrary noise structures, even for noise structures that cannot easily be treated by LRKFs such as multiplicative noise. The required integrals are solved by employing the discrete part, i.e., the Dirac Mixture, of the coupled discrete-continuous-approximation, which allows to automatically place Dirac components solely in the interesting regions of the state space, that is the support of the true posterior. As a result, the new filtering method is fast, efficient, and robust. Its performance is evaluated in comparison with state-of-the-art filters by means of a canonical benchmark example, the discrete-time cubic sensor problem.  2. Problem Formulation  We consider the general problem of estimating the hidden state of a discrete-time stochastic nonlinear dynamic system based on noisy measurements, which consists of a prediction step (or time update) employing a system model for propagating the estimated state from time step to time step and filter step (or measurement update) for including observations taken at a given time step into the state estimate. Here, the focus is on the filter step that is typically 2
considered harder compared to the prediction step. The insights obtained for the filter step can, however, be used for the prediction step as well. A generative measurement equation ˆ y   =   h ( x, v )   (1) is investigated, with state   x , a specific measurement   ˆ y , and measurement noise   v   with corresponding noise density   f v ( v ). The special case of additive noise ˆ y   =   h ( x ) +   v   (2) is also of interest, as it usually simplifies matters. We assume that the generative model can somehow be converted to a probabilistic model represented by the conditional density   f   ( y | x ).   For a given specific measurement   ˆ y , this conditional density is the so called Likelihood function 1   abbreviated as  f L ( x ) =   f   (ˆ y | x ) For the case of additive noise, the Likelihood function is given by  f L ( x ) =   f v (ˆ y   −   h ( x ))   .  Other noise structures result in different Likelihood functions.  Gaussian Filters  For a Gaussian-assumed density filter, we have a Gaussian prior density   f   p ( x ) that undergoes a Bayesian filter step according to the following multiplication with the Likelihood function    ̃ f   e ( x ) =   c   ·   f   p ( x )   ·   f L ( x )   ,  where the resulting posterior density is denoted by    ̃ f   e ( x ).   c   is a normalization constant ensuring that the posterior density integrates to one. The tilde is used to underline that this is the true density resulting from performing a single filter step. Obviously, the true posterior density in general is not Gaussian anymore. In order to enable recursive processing without increase in computational complexity, the true posterior has to be approximated by a Gaussian density for the next processing step. The state-of-the-art for doing so will be reviewed in the next section.  3. Density approximations  Estimating the state of stochastic nonlinear dynamic systems poses three challenges: 1. For a given finite-parameter representation of the state density, the density type is usually not maintained across the filter or the prediction step, so that continuous re-approximations are necessary to allow for recursive processing. 2. By doing so, the number of parameters of the selected representation typically increases without bound. 3. The approximation of the true density by the selected representation cannot be efficiently performed or the true density cannot even be calculated at all.  1 The Likelihood function is not necessarily a valid density function. Although it is always non-negative, it does not necessarily integrate to one, i.e., it usually is not normalized or even cannot be normalized.  3
Continuous Density Representations  For representation purposes, continuous representations such as mixtures of Gaussians are convenient when it comes to presenting and using the results of the state estimation process. They smoothly cover the considered part of the state space, easily represent complicated density shapes, and require relatively few parameters for satisfactory approximation results. However, re-approximating the true density resulting from a certain processing step by a continuous is usually hard.   One example in the context of the Bayesian filter step is the re-approximation of the product of a prior Gaussian Mixture and a complicated Likelihood function by a posterior Gaussian Mixture. Another example concerning the prediction step is the propagation of a Gaussian Mixture through a nonlinear dynamic system.  Discrete Density Representations  The difficulties with processing continuous densities led to the development of several types of filters employing discrete density representations over continuous domains. Using discrete samples instead of a continuous representation simplifies both processing steps significantly. Especially the prediction step becomes almost trivial as it just consists of propagating point values independently through the system equation. The filter step, on the other hand, is more challenging. At first, it seems simple to implement as it just consists of multiplying the discrete samples with the Likelihood function. However, it 1. requires an explicit Likelihood function that is not always available and 2. in a na ̈ ıve implementation usually leads to a degeneration of the sample set as most samples obtain zero weights. Furthermore, it is usually difficult to assess the quality of a sample representation, which is related to specifying the number of samples required for a given task. Although summary statistics such as mean and covariance of the sample set can easily be calculated, more useful measures such as entropy or smoothness cannot easily be obtained. Although the solution to the stochastic state estimation problem consists of fully determin- istic densities, particle filters employing independent random samples become popular due to their simplicity. However, random samples of small size do not provide a homogeneous coverage of the underlying density. As a result, the samples usually do not adequately represent the underlying true density. In addition, the quality of the results depends on the specific realization and can only be assessed via Monte Carlo simulations and appropriate averaging.  Alternating Density Representations  This includes sample-based Gaussian filters, such as the Unscented Kalman Filter (UKF) [ 23 ] and the Gaussian filter in [ 6 ], where a prior Gaussian density is approximated by deter- ministic samples that undergo the processing step. The processing result is approximated by a Gaussian density again and the process is repeated. This procedure cannot be easily generalized to more general continuous representations such as Gaussian Mixtures. This is due to the fact that approximating the discrete representation by a continuous representation is equivalent to density estimation from samples, which is a tough problem.  New coupled discrete-continuous-approximation  We now introduce a new class of density representations, the so called coupled discrete- continuous-approximations, where two types of density approximations are maintained simul- taneously. Instead of switching back and forth between representations convenient for the current task at hand, the desired representation is always available when needed. In doing so, we can make use of the best features of both worlds. The continuous part is employed for maintaining the smoothness of the representation and for taking the derivatives with respect to 4
the density parameters. The discrete part is employed for evaluating and comparing densities and for solving integrals in a quasi Monte-Carlo fashion. The principle is to have an inner core representation, a continuous one called   f c , that controls an outer representation, a discrete one called   f d . Typically, the parameters   η c   of the inner representation are specified and lead to appropriate parameters   η d   of the outer representation that is then used for, e.g., numerical integration. The two approximations are tied in such a way that changing the shape of one representation directly influences the shape of the other one and its corresponding parameters. The degrees of freedom of the coupled discrete-continuous-approximation are given by the number of parameters of the inner representation. The number of parameters of the outer approximation is a least equal to the number of parameters of the inner approximation, but in most cases will be larger. As a specific inner representation   f c ( x, η c ), we will focus on Gaussian densities, where the parameter vector   η c   contains the elements of the mean and the covariance matrix. The outer representation is given by the Dirac Mixture approximation of the inner Gaussian calculated as described in [18]. Modifying the inner parameters of the new coupled discrete-continuous-approximation based on employing the outer parameters for numerical integration is demonstrated in the next section.  4. Progressive Filter Step  The first step is to redefine the Likelihood function in order to achieve a continuous execution of the filter step. This progressive Likelihood is defined by  f L ( x, γ )   ,  where   γ   is an artificial time with   γ   ∈   [0 ,   1]. It is desired that  f L ( x, γ ) =      1   γ   = 0  f L ( x )   γ   = 1 holds. Several options for defining progressive Likelihood functions exist. This includes progres- sively modifying the given nonlinear mapping   h ( x ) of the underlying generative model (1) or varying the noise variance as in [ 13 ]. Here, we use the exponentiation of the Likelihood function as used in [17], [14]. The modified Likelihood function is then given by  f L ( x, γ ) = [ f L ( x )] γ   .   (3)  Remark 4.1 (Additive Noise)   For the special case of generative systems suffering from additive noise in (2), we obtain   2  f L ( x, γ ) = exp  {  − 1 2   γ   (  y   −   h ( x ) ) T  C − 1  v  (  y   −   h ( x ) )}  ,  2 For the additive noise case, exponentiation is equivalent to progressive modification of the noise covariance matrix as proposed in [13] since we have  γ   ( y   −   h ( x ) ) T   C − 1  v  ( y   −   h ( x ) )   =   ( y   −   h ( x ) ) T  (   1  γ   C v  ) − 1   ( y   −   h ( x ) )   .  5
where multipliers have been omitted to achieve   f L ( x, γ   = 0) = 1 .  The second step is the continuous execution of the filter step, for now written with a general prior density   f   p ( x ) and a modified posterior density   f   e ( x, γ ) depending on the artificial time  γ   introduced above    ̃ f   e ( x, γ ) =   f   p ( x )   ·   f L ( x, γ ) for   γ   ∈   [0 ,   1].   For the final time 3   γ   = 1, the modified Likelihood reaches the original Likelihood and as a result, the original posterior is attained by the modified posterior, i.e.,  ̃ f   e ( x, γ   = 1) =    ̃ f   e ( x ). On the other extreme, for the start time   γ   = 0, the modified posterior is desired to be identical to the prior density, i.e., we have    ̃ f   e ( x, γ   = 0) =   f   p ( x ). Hence, we have  ̃ f   e ( x, γ ) =  ̃ f   e ( x, γ   = 0)   ·   f L ( x, γ ) for   γ   ∈   [0 ,   1]. Plugging in the continuous part of the coupled discrete-continuous-approximation intro- duced in the previous Section gives  f   e c   ( x, η c ( γ ))   ≈    ̃ f   e ( x, γ ) =   f   p ( x )   ·   f L ( x, γ )   (4) for   γ   ∈   [0 ,   1], where left-hand-side and right-hand-side now become equal only for   γ   = 0, but only approximately 4   equal for   γ >   0. In order to find the best matching Gaussian approximation   f   e c   ( x, η c ( γ )) in (4) we desired the first moments to be equal as  ∫  I R N   m f   e c   ( x, η c ( γ ))   dx   =  ∫  I R N   m f   p ( x )   ·   f L ( x, γ )   dx ,  with   m   = [1 , x, x 2 ] T   . Taking the derivative with respect to   γ   on both sides gives  ∫  I R N   m ∂f   e c   ( x, η c ( γ ))  ∂γ   dx   =  ∫  I R N   m f   p ( x )   ·   ∂f L ( x, γ )  ∂γ  ︸   ︷︷   ︸   ̇ f L ( x,γ )  dx ,  with   ∂f   e c   ( x, η c ( γ ))  ∂γ   =   ∂f   e c   ( x, η c ( γ ))  ∂ η T c   ( γ )  ︸   ︷︷   ︸  p T   ( x,γ )  ·   ∂ η c ( γ )  ∂γ  ︸   ︷︷   ︸   ̇ η c ( γ )  .  We obtain   ∫  I R N   m p T   ( x, γ )   dx  ︸   ︷︷   ︸  P ( γ )   ̇ η c ( γ ) =  ∫  I R N   m f   p ( x )   ·    ̇ f L ( x, γ )   dx ,  which leads to  P ( γ )  ̇ η c ( γ ) =   b ( γ )   .   (5)  3 Of course, time here denotes the artificial time   γ   introduced above.  4 As long as working with the infinite-dimensional functional representation    ̃ f   e   of the true density, the two sides in (4) are equal as    ̃ f   e ( x, γ ) is capable of following changes in the right-hand-side exactly.   On the other hand, for a finite-dimensional representation   f   e c   ( x, η c ( γ )), i.e., a density function depending on a finite-dimensional parameter vector   η c ( γ ), the left-hand-side cannot necessarily exactly follow the changes of the right-hand-side as the product on the right-hand-side typically is not of the same density type.  6
For the specific progression from (3), we obtain  ̇ f L ( x, γ ) =   ∂f L ( x, γ )  ∂ γ   =   ∂f   γ L   ( x )  ∂ γ   =   f   γ L   ( x )   ·   log ( f L ( x )) =   f L ( x, γ )   ·   log ( f L ( x ))   .  Remark 4.2 (Additive Noise)   A further simplification is achieved by focusing on the case of additive noise. Taking the derivative of the Likelihood now gives   ̇ f L ( x, γ ) =   − 1 2   f L ( x, γ )   (  y   −   h ( x ) ) T  C − 1  v  (  y   −   h ( x ) )  .  5. Scalar Gaussian-Assumed Density Filter  This section is devoted to deriving specific formulas for scalar Gaussian densities.  Closed-form Expressions for Matrix   P ( γ ) We start with a scalar Gaussian density for representing   f   e c   given by  f   e c   ( x, η c ( γ )) =   w ( γ )   1  √ 2 π σ ( γ ) exp  (  − 1 2 ( x   −   m ( γ )) 2  ( σ ( γ )) 2  )  ,  with  η c ( γ ) = [ w ( γ ) , m ( γ ) , σ ( γ )] T   .  The required derivatives of the continuous part   f   e c   of the CoDiCo-approximation with respect to the density parameters collected in the vector   p ( x, γ ) given by  p ( x, γ ) =   ∂f   e c   ( x, η c ( γ ))  ∂ η c ( γ )   =          ∂f   e c   ( x,η c ( γ ))  ∂ w ( γ )  ∂f   e c   ( x,η c ( γ ))  ∂ m ( γ )  ∂f   e c   ( x,η c ( γ ))  ∂ σ ( γ )          or  p ( x, γ ) =         1  w ( γ )  x − m ( γ ) ( σ ( γ )) 2  ( x − m ( γ )) 2 − ( σ ( γ )) 2  ( σ ( γ )) 3          f   e c   ( x, η c ( γ ))   .   (6) For this specific choice of continuous representation   f   e c   , the matrix   P ( γ ) is given by  P ( γ ) =        1   0   0  m ( γ )   w ( γ )   0  m 2 ( γ ) +   σ 2 ( γ )   2   w ( γ )   m ( γ )   2   w ( γ )   σ ( γ )         .  Solving for    ̇ η c ( γ ) in (5) could be performed based on the matrix   P ( γ ) directly. However, it is possible to give its inverse in closed form, which will be denoted by  Q ( γ ) =   P ( γ ) − 1   ,  7
with  Q ( γ ) =         1   0   0  − m ( γ )  w ( γ ) 1  w ( γ )   0  m 2 ( γ ) − σ 2 ( γ ) 2   w ( γ )   σ ( γ )   −   m ( γ )  w ( γ )   s ( γ ) 1 2   w ( γ )   σ ( γ )          .  Hence, we obtain  ̇ η c ( γ ) =   Q ( γ )   b ( γ )   ,   (7) with  ̇ η c ( γ ) = [  ̇ w ( γ ) ,    ̇ m ( γ ) ,    ̇ σ ( γ )] T   .  The right-hand-side of (7) that we will now denote by   r ( γ ) can now be written as  r ( γ ) =   Q ( γ )   b ( γ ) =   Q ( γ )  ∫  I R  m   ·    ̃ f   e ( x, γ )   ·   log ( f L ( x ))   dx  =  ∫  I R  q ( x, γ )   ·    ̃ f   e ( x, γ )   ·   log ( f L ( x ))   dx ,  (8) with  q ( x, γ ) =         1  x − m ( γ )  w ( γ ) ( x − m ( γ )) 2 − ( σ ( γ )) 2  2   w ( γ )   σ ( γ )          .  Approximate Expressions for Vector   b ( γ ) Calculating   r ( γ ) in (8) amounts to calculating certain nonlinear moments of the true posterior density. as closed-form solutions are a rare occasion, we have to be content with approximate integration, which is pursued further here. One viable option would be to replace the prior density   f   p ( x ) by its Dirac Mixture approximation and thus, turn the integration into summation. However, this is only efficient for small values of   γ   as long as the progressive Likelihood function is wide and does not modify the prior density too much. For larger   γ , the Likelihood function typically becomes narrower and narrower and would force many Dirac components to zero, which is similar to the degeneration problem in particle filtering. By doing so, only very few Dirac components would really contribute to the integration, especially for larger   γ , which is not desired. Thanks to progressive processing, we have an approximate posterior   f   e c   ( x, η c ( γ )) available at every   γ . This approximate posterior now allows to integrate only over those portions of the state space that contains the true posterior. For doing so, we replace the continuous part  f   e c   ( x, η c ( γ )) of the CoDiCo-approximation in (8) by the coupled discrete density   f   e d   , i.e., a scalar Dirac Mixture approximation according to  f   e d   ( x, η c ( γ )) =  L d ∑  i =1  w i ( η c ( γ ))   δ ( x   −   ˆ x i ( η c ( γ )))   .  As a result, we obtain  r ( γ ) =  L d ∑  i =1  w i   ·   q (ˆ x i , γ )    ̃ f   e (ˆ x i , γ )  f   e c   ( x, η c ( γ ))   ·   log ( f L (ˆ x i ))   dx ,   (9) 8
where the explicit dependency of the parameters of the discrete density representation upon the parameters of the continuous part has been omitted. It is important to note that by employing Dirac components solely in regions of the state space where the true posterior    ̃ f   e (   ˆ x i , γ ) is represented by its Gaussian approximation  f   e c   ( x, η c ( γ )), all the components contribute to the integration.   As a result, by far fewer components are required to achieve a good accuracy. For the special case of a scalar measurement equation with additive Gaussian noise leading to a scalar Likelihood function  f L ( x ) = exp  (  − 1 2 ( y   −   h ( x )) 2  σ 2  v  )  ,  we obtain log ( f L (ˆ x i )) =   − 1 2 ( y   −   h (ˆ x i )) 2  σ 2  v  .  for   i   = 1 , . . . L d .  6. Evaluation  For demonstrating the significant increase in performance achieved by the new filter, it is evaluated in comparison with the state-of-the-art filters. As a benchmark example, we consider the discrete-time cubic sensor problem  y   =   x 3   +   v ,  where   v   is zero-mean Gaussian measurement noise. This is a canonical example, simple to understand, and, compared to some contrived artificial example, allows a good assessment of filter performance. For comparison purposes, the proposed new filter is compared to  •   the class of Linear Regression Kalman Filters (LRKF), specifically to  –   the Unscented Kalman Filter (UKF),  –   the Gaussian Filter (GF),  –   the Analytic LRKF,  •   and an assumed-Gaussian density filter using moment matching for fitting a Gaussian to the true posterior employing Monte Carlo integration for approximating the desired moments (MC). For a measurement   ˆ y   = 3, a prior Gaussian density with mean   − 1 and variance 1, and a cubic measurement equation corrupted by zero-mean Gaussian noise with variance 1 . 2, the results of one filter step for the different filters are shown in Figure 1. The true posterior of the Bayesian filter step is calculated on a fine grid with 30 . 000 grid points. The ground truth for comparison purposes is the Gaussian density with mean and variance identical to the true posterior. A standard Unscented Kalman Filter (UKF) is employed. The Gaussian Filter (GF) is employed based on an optimal Dirac Mixture approximation of the prior Gaussian density according to [ 6 ], where the filter step is performed by assuming that measurement and state are jointly Gaussian. The Analytic LRKF computes the moments in closed form and 9
−1   0   1   2 0 0.2 0.4 0.6 0.8  x   →  f ( x )   →  Best Gaussian Approximation −1   0   1   2 0 0.2 0.4 0.6 0.8  x   →  f ( x )   →  Gaussian Filter (30 samples) −1   0   1   2 0 0.2 0.4 0.6 0.8  x   →  f ( x )   →  UKF −1   0   1   2 0 0.2 0.4 0.6 0.8  x   →  f ( x )   →  Analytic LRKF −1   0   1   2 0 0.2 0.4 0.6 0.8  x   →  f ( x )   →  GPF (10 6   samples) −1   0   1   2 0 0.2 0.4 0.6 0.8  x   →  f ( x )   →  Progressive (30 samples) Figure 1: Comparison of the results for one filter step for the ground truth, i.e., the best Gaussian approximation computed numerically, compared with the true posterior, the Gaussian Filter (GF), the Unscented Kalman Filter (UKF), the Analytic LRKF, the Gaussian Particle Filter (GPF), and the new progressive Gaussian filter.  additionally assumes that measurement and state are jointly Gaussian. The Gaussian Particle Filter (GPF) computes posterior mean and variance by means of 10 6   random samples drawn from the prior Gaussian density that are used for evaluating the product of Likelihood and prior density. Subsequently, the resulting weighted samples are used for moment matching. The new progressive Gaussian filter is applied with a discrete density approximation using 30 Dirac components. Figure 1 clearly shows that the LRKFs have difficulties approximating the true posterior. In contrast, the GPF for 10 6   samples and the new progressive Gaussian filter produce results almost identical to the ground truth. To illustrate the savings in the number of samples used by the new progressive Gaussian filter, its estimation quality is compared to the Gaussian particle filter in Figure 2 for the same experimental parameters as above. It is obvious that very good estimation results are already obtained for very few (deterministic) samples. In contrast, the results of the Gaussian particle filter are non-deterministic and the average convergence is much slower. The results of recursive filtering over 50 times steps are shown in Figure 3. The recursion is started with a prior Gaussian density with mean   − 1 and variance 30. Noise mean is 0, the noise variance is 1 . 2. From time step 1 to 19, the true state is 1. At time step 20, the true state is changed to 0 and Gaussian noise with variance of 9 is added to all estimates. The top plot in Figure 3 shows the estimated means of the true posterior, its best Gaussian approximation 5 , the new progressive Gaussian filter, and the Analytic LRKF. The other variants of LRKF have been omitted as the Analytic LRKF provides their lower bound in terms of estimation quality.  5 The best Gaussian approximation used as the ground truth at every time step is also recursively calculated on the grid based on the previous best Gaussian approximation. The accumulating error between the true posterior and the ground truth is unavoidable due to the Gaussian assumption.  10
50   100   150   200   250   300  10 −10  10 −8  10 −6  10 −4  10 −2  10 0  10 2  Number of samples Absolute Mean Error  Progressive  RMSE GPF Figure 2: Comparison of the estimation quality as a function of the number of samples used between the Gaussian particle filter (GPF) and the proposed progressive Gaussian filter.   As the GPF provides non- deterministic results, 100 Monte-Carlo runs have been performed to produce the RMSE and the min-max error bars.  The bottom plot in Figure 3 shows the absolute mean error of the best Gaussian approxi- mation, the new progressive filter, and the Analytic LRKF with respect to the mean of the true posterior. The two plots in Figure 3 show that the proposed progressive Gaussian filter provides results very close to the best Gaussian approximation, which itself is rather close to the mean of the true posterior. The analytic LRKFs represented by their highest-quality variant, the Analytic LRKF, show a much larger deviation.  7. Conclusions  A progressive Gaussian filtering method has been introduced that is based on a coupled discrete-continuous-representation with a Gaussian density as the continuous part and its Dirac Mixture approximation as the discrete part. For the moment calculation, The Dirac Mixture part is employed to evaluate the true posterior density at discrete points only. In contrast to Monte-Carlo methods available for that purpose, the Dirac components all contribute to the integration as progressive processing is exploited to solely place components in the important regions of the state space. As a result, the new filter requires significantly less samples, is fast, efficient, and robust, and achieves a high estimation quality compared to state-of-the-art Gaussian filters. It can easily be used as a replacement for standard Gaussian filters such as the UKF.  References  [1]   T. Lefebvre, H. Bruyninckx, and J. De Schutter, “The linear regression kalman filter,” in   Nonlinear Kalman Filtering for Force-Controlled Robot Tasks , ser. Springer Tracts in Advanced Robotics, 2005, vol. 19. [2]   ——, “Kalman filters for non-linear systems: a comparison of performance,”   International Journal of Control , vol. 77, no. 7, pp. 639–653, 2004. 11
0   5   10   15   20   25   30   35   40   45   50 −1 −0.5 0 0.5 1 1.5 Time Estimated Mean True posterior Best Gaussian Approx. Progressive (30 samples) Analytic LRKF 0   5   10   15   20   25   30   35   40   45   50 0 0.2 0.4 0.6 0.8 1 Time Absolute Mean Error Best Gaussian Approx. Progressive (30 samples) Analytic LRKF Figure 3: Results of recursive filtering over 50 times steps, where the recursion is started with a prior Gaussian density with mean   − 1 and variance 30. From time step 1 to 19, the true state is 1, from time step 20 to 50, the true state is changed to 0. Top: Estimated mean sequences. Bottom: Absolute mean error of the best Gaussian approximation, the new progressive filter, and the Analytic LRKF with respect to the mean of the true posterior.  [3]   S. Julier, J. Uhlmann, and H. F. Durrant-Whyte, “A new method for the nonlinear transformation of means and covariances in filters and estimators,”   IEEE Transactions on Automatic Control , vol. 45, no. 3, pp. 477–482, Mar. 2000. [4]   S. J. Julier, “The scaled unscented transformation,” in   American Control Conference, 2002. Proceedings of the 2002 , vol. 6.   IEEE, 2002, pp. 4555– 4559 vol.6. [5]   D. Tenne and T. Singh, “The higher order unscented filter,” in   American Control Con- ference, 2003. Proceedings of the 2003 , vol. 3.   IEEE, Jun. 2003, pp. 2441– 2446 vol.3. [6]   M. F. Huber and U. D. Hanebeck, “Gaussian Filter based on Deterministic Sampling for High Quality Nonlinear Estimation,” in   Proceedings of the 17th IFAC World Congress (IFAC 2008) , vol. 17, no. 2, Seoul, Republic of Korea, Jul. 2008. [7]   M. F. Huber, F. Beutler, and U. D. Hanebeck, “Semi-Analytic Gaussian Assumed Density Filter,” in   Proceedings of the 2011 American Control Conference (ACC 2011) , San Francisco, California, USA, Jun. 2011. 12
[8]   F. Beutler, M. F. Huber, and U. D. Hanebeck, “Semi-Analytic Stochastic Linearization for Range-Based Pose Tracking,” in   Proceedings of the 2010 IEEE International Conference on Multisensor Fusion and Integration for Intelligent Systems (MFI 2010) , Salt Lake City, Utah, USA, Sep. 2010. [9]   ——, “Gaussian Filtering using State Decomposition Methods,” in   Proceedings of the 12th International Conference on Information Fusion (Fusion 2009) , Seattle, Washington, USA, Jul. 2009. [10]   K. Ito and K. Xiong, “Gaussian filters for nonlinear filtering problems,”   IEEE Transactions on Automatic Control , vol. 45, no. 5, pp. 910–927, May 2000. [11]   J. H. Kotecha and P. M. Djuric, “Gaussian particle filtering,”   IEEE Transactions on Signal Processing , vol. 51, no. 10, pp. 2592– 2601, Oct. 2003. [12]   U. D. Hanebeck, K. Briechle, and A. Rauh, “Progressive Bayes: A New Framework for Nonlinear State Estimation,” in   Proceedings of SPIE, AeroSense Symposium , vol. 5099, Orlando, Florida, USA, May 2003, pp. 256 – 267. [13]   U. D. Hanebeck and O. Feiermann, “Progressive Bayesian Estimation for Nonlinear Discrete-Time Systems: The Filter Step for Scalar Measurements and Multidimensional States,” in   Proceedings of the 2003 IEEE Conference on Decision and Control (CDC 2003) , Maui, Hawaii, USA, Dec. 2003, pp. 5366–5371. [14]   J. Hagmar, M. Jirstrand, L. Svensson, and M. Morelande, “Optimal parameterization of posterior densities using homotopy,” in   2011 Proceedings of the 14th International Conference on Information Fusion (FUSION) .   IEEE, Jul. 2011, pp. 1–8. [15]   A. Rauh and U. D. Hanebeck, “Calculating Moments of Exponential Densities Using Differential Algebraic Equations,”   IEEE Signal Processing Letters , vol. 10, no. 5, pp. 144–147, May 2003. [16]   M. F. Huber and U. D. Hanebeck, “Progressive Gaussian Mixture Reduction,” in   Proceed- ings of the 11th International Conference on Information Fusion (Fusion 2008) , Cologne, Germany, Jul. 2008, pp. 1–8. [17]   F. Daum and J. Huang, “Nonlinear filters with particle flow induced by log-homotopy,”  Proceedings of SPIE , vol. 7336, no. 1, pp. 733 603–733 603–12, May 2009. [18]   U. D. Hanebeck, M. F. Huber, and V. Klumpp, “Dirac Mixture Approximation of Multivariate Gaussian Densities,” in   Proceedings of the 2009 IEEE Conference on Decision and Control (CDC 2009) , Shanghai, China, Dec. 2009. [19]   O. C. Schrempf, D. Brunn, and U. D. Hanebeck, “Density Approximation Based on Dirac Mixtures with Regard to Nonlinear Estimation and Filtering,” in   Proceedings of the 2006 IEEE Conference on Decision and Control (CDC 2006) , San Diego, California, USA, Dec. 2006. [20]   ——, “Dirac Mixture Density Approximation Based on Minimization of the Weighted Cram ́ er-von Mises Distance,” in   Proceedings of the 2006 IEEE International Conference on Multisensor Fusion and Integration for Intelligent Systems (MFI 2006) , Heidelberg, Germany, Sep. 2006, pp. 512–517. 13
[21]   U. D. Hanebeck and O. C. Schrempf, “Greedy Algorithms for Dirac Mixture Approximation of Arbitrary Probability Density Functions,” in   Proceedings of the 2007 IEEE Conference on Decision and Control (CDC 2007) , New Orleans, Louisiana, USA, Dec. 2007, pp. 3065–3071. [22]   O. C. Schrempf and U. D. Hanebeck, “Recursive Prediction of Stochastic Nonlinear Systems Based on Optimal Dirac Mixture Approximations,” in   Proceedings of the 2007 American Control Conference (ACC 2007) , New York, New York, USA, Jul. 2007, pp. 1768–1774. [23]   S. J. Julier and J. K. Uhlmann, “Unscented filtering and nonlinear estimation,”   Proceedings of the IEEE , vol. 92, no. 3, pp. 401– 422, Mar. 2004. 14