Damped Posterior Linearization Filter

TL;DR

This paper introduces a double-loop version of the iterated posterior linearization filter (IPLF) that improves convergence and maintains high accuracy in Bayesian state estimation by incorporating an optimization algorithm in the inner loop.

Contribution

The paper proposes a novel double-loop IPLF that enhances convergence properties by integrating an optimization step for the posterior mean.

Findings

The double-loop IPLF converges better than the original IPLF.

The proposed method achieves accuracy comparable or superior to existing algorithms.

Simulation results validate improved convergence and performance.

Abstract

The iterated posterior linearization filter (IPLF) is an algorithm for Bayesian state estimation that performs the measurement update using iterative statistical regression. The main result behind IPLF is that the posterior approximation is more accurate when the statistical regression of measurement function is done in the posterior instead of the prior as is done in non-iterative Kalman filter extensions. In IPLF, each iteration in principle gives a better posterior estimate to obtain a better statistical regression and more accurate posterior estimate in the next iteration. However, IPLF may diverge. IPLF's fixed- points are not described as solutions to an optimization problem, which makes it challenging to improve its convergence properties. In this letter, we introduce a double-loop version of IPLF, where the inner loop computes the posterior mean using an optimization algorithm. Simulation results are presented to show that the proposed algorithm has better convergence than IPLF and its accuracy is similar to or better than other state-of-the-art algorithms.

Tables

Table 1. TABLE I : Mean estimates of first 6 iterations of IPLF and IEKF

Iteration	1	2	3	4	5	6
IPLF	-2.51	5.28	-31.75	17.52	-40.56	11.96
IEKF	-7.64	58.28	-1.77	2.60	-6.66	48.47

Table 2. TABLE II: KLDs of estimates produced by different filters in arctan \arctan test

	GGF	RUF	IPLF	DIPLF
MC	15.9	0.04	88.69	$3\cdot {10}^{-6}$
EKF	4009.10	0.01	65.12	${10}^{-6}$
CKF	3370.78	0.01	64.39	${10}^{-6}$
UKF	92.55	0.01	${10}^{-6}$	${10}^{-6}$

Table 3. TABLE III: Mean KLDs of estimates in range test

	GGF	KLPUKF	RUF	IPLF	DIPLF
MC	0.25	0.17	0.21	0.26	0.17
EKF	0.48	0.60	0.48	0.55	0.55
CKF	0.28	0.22	0.29	0.38	0.23
UKF	0.35	0.34	0.33	0.37	0.26

Equations

$$y=h(x)+\epsilon ,$$

$$\hat{y}$$

$$P_{h(x)x}$$

$$P_{h(x)h(x)}$$

$${\mu }^{+}$$

$$P^{+}$$

$$S$$

$$K$$

$$\hat{h}(x)=J_{i}x+b_i+{{\epsilon }_{\Omega }}_i+\epsilon ,$$

$$J_i$$

$$b_i$$

$${\Omega }_i$$

$${{\epsilon }_{\Omega }}_i$$

$$S$$

$$K_i$$

$${\mu }_{i+1}$$

$$P_{i+1}$$

$$y=\arctan (x)+\epsilon .$$

$$J_i$$

$$b_i$$

$${\Omega }_i$$

$$q(\mu )=$$

$$+\frac{1}{2}(\mu -{\mu }^{-}{)}^T{P}^{-1}(\mu -{\mu }^{-}).$$

$${\mu }_{i+1}=(1-\alpha ){\mu }_i+\alpha ({\mu }_0+K_i(y-h({\mu }_i)))$$

$${\mu }_i$$

$$q({\mu }_i)=$$

$$+\frac{1}{2}({\mu }_i-{\mu }^{-}{)}^T{\left(P^{-}\right)}^{-1}({\mu }_i-{\mu }^{-})$$

$${\mu }_{i+1}=(1-\alpha ){\mu }_i+\alpha \left({\mu }_0+K_i(y-J_i{\mu }_0-b_i)\right)$$

$$q({\mu }_{i+1},P_j)<\beta q({\mu }_i,P_j)$$

$$\frac{1}{\sqrt{(2\pi {)}^{n_y}|R+{\Omega }_j|(2\pi {)}^{n_x}|P^{-}|}}{e}^{-q(x)}$$

$$=$$

$$h(x)=y^2+\epsilon ,$$

$$\hat{y}$$

$$P_{h(x)x}$$

$$P_{h(x)h(x)}$$

$${\mu }_i=$$

$$=$$

$$J({\mu }_i{)}^T(R+{\Omega }_j{)}^{-1}(\hat{y}({\mu }_i)-y)+{P^{-}}^{-1}({\mu }_i-{\mu }^{-})=0.$$

Full text

I Introduction

In Bayesian state estimation, a state that evolves stochastically in time is estimated from noisy measurements. In this letter, we concentrate on the measurement update stage of the Bayesian filter. In the measurement update, a prior (the dynamic model’s state prediction) is updated using information from a measurement. If the measurement model is linear and Gaussian the posterior density can be computed analytically using the Kalman filter [kalman], but for a general measurement model, the computation of the posterior density is intractable. One approximate approach is the general Gaussian filter (GGF), which represents the joint state and measurement distribution by a multivariate Gaussian using moment matching, and then uses standard marginalization and conditioning formulas to compute the conditional state distribution given the measurement to obtain the posterior estimate [sarkka2013bayesian]. The GGF moment matching is also intractable, but there exist several Kalman filter extensions that are approximations of the GGF. The standard way to apply GGF is to do statistical linear regression (SLR), or an approximation of the SLR, in the prior; this can be interpreted as the optimal linearization with the given prior. The GGF approach does not work well for severe nonlinearities or low measurement noise [6584787] so there is interest in developing alternatives.

Iterative algorithms generate approximative solutions based on previous solutions with the goal of improving the approximation. The iterated extended Kalman filter (IEKF) [jazwinski, pp. 349-351] produces a sequence of mean estimates by making first-order Taylor approximations of the measurement function. In [Bell:1993], it was shown that the IEKF measurement update is equivalent to the Gauss-Newton (GN) algorithm for computing the maximum a posteriori (MAP) estimate. IEKF performs well in situations where the true posterior is close to a Gaussian. Convergence of the GN algorithm is not guaranteed. Better convergence is offered by damped (or descending) GN algorithms [Bjork], which ensure that the cost function is nonincreasing at each iteration. An alternative to damped GN is the Levenberg-Marquadt algorithm [Bellaire].

There are many other iterated filters in the literature. The iterated sigma point Kalman filter [Sibley-RSS-06], at each iteration, uses a linearization that mixes SLR with respect to the prior and analytical linearization at the current MAP estimate. The recursive update filter (RUF) uses several updates of the prior with down-weighted Kalman gain [UKFRUF]. Progressive Gaussian Filtering is a homotopy continuation method that updates the prior starting from an easily computable measurement likelihood that gradually evolves into the true measurement likelihood [6290507, hanebeck2003progressive, 7402897]. The Kullback-Leibler partitioned update Kalman filter (KLPUKF) can be used when the measurement is multidimensional [raitoharju2016]. It sequentially updates using nonlinearity-minimizing linear combinations of the measurements.

In this paper, we focus on the iterated posterior linearization filter (IPLF), which uses SLR w.r.t. the posterior density. SLR’s linearization is based on a larger area determined by a probability density function (PDF) instead of a point as in the Taylor linearization, which improves the accuracy of the filter. In [PLF], it was shown that it is better for the linearization to be w.r.t. the posterior instead of the prior. The basic idea in the IPLF is to compute a posterior estimate using the prior, then use this estimate to compute a better linearization and posterior estimate, and so on. In this letter, we observe that the IPLF does not always converge, and we propose a damped version of the algorithm with improved convergence properties.

We show in simulations how the proposed algorithm is less prone to filter divergence than the original IPLF. Furthermore, we compare the posterior estimate accuracy with other iterative Kalman filter methods and show that the proposed algorithm is more accurate in the simulations.

II Background

We consider measurements of the form

[TABLE]

where $y$ is the $n_{y}$ dimensional real measurement value, $h(\cdot)$ is the measurement function, $x$ is the $n_{x}$ dimensional real random state vector and $\varepsilon$ is zero mean Gaussian noise with covariance $R$.

II-A GGF

The GGF [sarkka2013bayesian] for measurement model (1) uses the expected value of predicted measurement $\hat{y}$, cross covariance of measurement and state ${P_{h{(x)x}}}$, and measurement covariance ${P_{h{(x)}h{(x)}}}$. These moments are

[TABLE]

where $p_{\operatorname{N}}(x|\mu^{-},{{P}^{-}})$ is the multivariate normal PDF of the prior that has mean $\mu^{-}$ and covariance ${{P}^{-}}$. The update equations for the mean and covariance of the posterior are then

[TABLE]

where

[TABLE]

The integrals (2)-(4) do not have in general closed form solutions. Many KFEs, such as unscented Kalman filter (UKF) [WANUKF] and cubature Kalman filter (CKF) [cubature], can be interpreted as numerical approximations of GGF.

II-B IPLF

In [PLF], it is shown that a lower bound of the Kullback-Leibler divergence (KLD) of a GGF with respect to the true posterior is minimized if the moments (2)-(4) are computed using the posterior instead of the prior. This update cannot be computed directly with the GGF equations (2)-(8), but instead a linear model that has correct moments is generated and is then applied to the prior.

The IPLF algorithm iteratively approximates the posterior using SLR, creating a sequence of updated means ${\mu}_{i}$ and covariance matrices ${P}_{i}$ as follows. First, set ${\mu}_{1}=\mu^{-}$ and ${P}_{1}={{P}^{-}}$. Then, at the $i$th iteration, compute the SLR of $h(\cdot)$ with respect to (${\mu}_{i}$, ${P}_{i}$) by first computing moments (2)-(4) and then defining the linear measurement that corresponds to those moments [PLF]:

[TABLE]

where

[TABLE]

${\varepsilon_{\Omega}}_{i}$ is an independent noise whose covariance $\Omega_{i}$ is the covariance of the linearization error. For linear systems $\Omega_{i}$ is 0.

Finally, compute the posterior mean and covariance that correspond to the linearized measurement function (9):

[TABLE]

The obtained posterior is used for the next linearization. The process is repeated for a predetermined number of steps or until a convergence criterion is met, e.g. until the KLD of two consecutive estimates is below a threshold.

However, the IPLF, like the GN, can diverge, as illustrated in the next example.

Example 1

The measurement model is

[TABLE]

State $x$ has prior mean 2.75 and variance 1, the measurement value is $y=0$ and measurement noise variance is $R=10^{-4}$. The integrals (2)-(4) are computed using Monte Carlo (MC) integration with $10^{5}$ samples and also with IEKF.

Table I gives the means in the first 6 iterations. Neither of the algorithms converge to the true posterior, which is close to 0, even after 50 iterations. In the supplementary material, we provide another example, whose moments have closed form solutions, where the IPLF does not converge.

II-C IEKF* as a GN algorithm*

IEKF is similar to IPLF, but it uses a first order Taylor series approximation of the measurement function instead of SLR. The IEKF iteration formulas are as in Section II-B, but with (10)-(12) replaced by

[TABLE]

In [Bell:1993], it is shown that the IEKF for measurements of form (1) is equal to the GN algorithm [Bjork] that minimizes the cost function

[TABLE]

To decrease the possibility of the divergence of GN, the damped (or descending) version of the GN algorithm can be used [Bjork]. A damped IEKF that uses line search [Bjork] in minimization of (22) is given in [7266781]. The mean update is

[TABLE]

with step length parameter $\alpha$ selected so that $0\leq\alpha\leq 1$ and $q({\mu}_{i+1})\leq q({\mu}_{i})$. Finally, in the IEKF, the posterior covariance matrix is computed based on the linearization at the MAP point after the last iteration [Bell:1993].

III damped iterated posterior linearization filter (DIPLF)

The main weakness of the IPLF is that it sometimes diverges. In this section, we present a damped version of the IPLF, which has substantially better convergence properties. The key insight is that the update of the mean can be seen as an optimization problem and solved with a damped GN algorithm that decreases the cost function in every iteration. A detailed motivation of the algorithm is presented in Section III-A, followed by a description of line search required to implement it. Further implementation details are discussed in the Supplementary material.

III-A Derivation of DIPLF

Ideally, we want the posterior mean and covariance to be used in the SLR to linearize the measurement function [PLF]. Our approach to iteratively finding the mean and covariance for the linearization is to use a separate loop for the optimization of the mean. Thus, the proposed filter computes the estimate using two nested loops. The inner loop uses a damped GN algorithm to optimize the mean ${\mu}_{i}$ while keeping ${P}_{j}$ and $\Omega_{j}$ fixed. The outer loop updates the covariances ${P}_{j}$ and $\Omega_{j}$. Index $i$ increases in every iteration of the inner loop and $j$ in the outer loop.

III-A1 Inner loop

The purpose of the inner loop is to find optimal ${\mu}_{i}$ when ${P}_{j}$ and $\Omega_{j}$ are fixed. At an optimum $\mu_{i}$ coincides with the mean used in the SLR and we can write:

[TABLE]

where we used $({\mu}_{i})$ to identify vectors and matrices that depend on the mean. In the supplementary material, we show that the above equation is fulfilled when

[TABLE]

achieves its minimum. Therefore, we use it as the cost function for the inner loop. The optimization problem is analogous to the optimization of the cost function of the IEKF (22), but using $\hat{y}$ instead of $h({\mu})$. Because the step is taken in a descent direction of the target function we can use a scaling factor $\alpha$ ($0<\alpha\leq 1$) in the update as in (23):

[TABLE]

and select $\alpha$ using line search so that the value of our target function (25) decreases. Thus, the next mean is a weighted sum of previous mean and the mean provided by IPLF. This makes the algorithm locally convergent on almost all nonlinear least squares problems, provided that the line search is carried out appropriately. In fact, it is usually globally convergent [Bjork, Chapter 9.2.1]. To facilitate the stopping of the inner loop, we propose that the algorithm should repeat the inner loop as long as (25) reduces significantly at each iteration i.e. while

[TABLE]

with, e.g., $\beta=0.9$.

III-A2 Outer loop

The outer loop updates the covariances ${P}_{j}$ and $\Omega_{j}$ for the next iteration of the inner loop. To define a stopping condition for the outer loop, we first exponentiate function (25) and normalize to get a product of normal distributions:

[TABLE]

If we treat $P_{i}$ and $\Omega_{i}$ as constants, (25) achieves its minimum when (28) achieves its maximum. We thus continue the outer loop as long as (28) is increasing significantly. Compared to using (25), the factor containing $\Omega_{i}$ in (28) causes the stopping condition based on (28) to favor estimates with smaller nonlinearity.

III-A3 Full algorithm

The algorithm is summarized in Algorithm 1.

The inner loop does the optimization of the target function (25) while keeping the state covariance and nonlinearity covariance constant. The direction of change in a GN algorithm is a descent direction of the cost function [Bjork, Chapter 9.2.1]. Thus, with small enough $\alpha$ the cost function value should decrease, unless the estimate is in a local extremum or a saddle point. By using $\alpha=1$ and exiting the inner loop after a single iteration, Algorithm 1 becomes equivalent to an IPLF algorithm.

The outer loop changes the optimization problem of the inner loop as $\Omega$ changes and the covariance, for which the expected measurement value $\hat{y}$ is computed, changes.

III-B Line search

The value of $\alpha$ can be computed with different line search techniques [Bjork, Chapter 9.2.1]. Backtracking line search, also known as the Armijo-Goldstein step length principle, is one of the simplest and most widely used options, and is done as follows.

First, $\alpha$ is set to 1, and a value of ${\mu}$ is found using (23). If this value does not decrease (25), w.r.t. the previous value of ${\mu}$, then $\alpha$ is multiplied by a constant factor $0<\tau<1$ and the procedure is repeated. To reduce the computational load one can terminate the inner loop when $\alpha$ is smaller than a predetermined value (we use $2^{-4}$) as then the change is the estimate is negligible.

IV Simulations

In our first test, we evaluate the situation in Example 1. We compare posterior estimates obtained with estimation algorithms that are based on approximating integrals (2)-(4). We use MC integration with $10^{5}$ samples, extended Kalman filter (EKF), UKF with parameters from [WANUKF], and third-degree CKF [cubature] to compute the moment approximations. The algorithms used are GGF, RUF [UKFRUF], IPLF [PLF], and DIPLF. The GGF with different moment computation algorithms is equivalent to the algorithms from which the moment computation was taken. RUF applies the measurement using down-weighted Kalman gains and uses 10 iterations in our tests. When EKF is used to compute the moments GGF is EKF, RUF is equivalent to the algorithm in [RUKF], DIPLF is IEKF, and DIPLF is damped IEKF. We use $\tau=0.5$ for reducing the step length. For the threshold of inner loop, we use $\beta=0.9$ and we exit the outer loop if $0.999p_{\operatorname{N}}(\hat{y}_{i}|y,R+\Omega_{j})$ is not larger than the corresponding likelihood of the previous iteration. A more thorough discussion on the selection of parameters is given in the supplementary material.

Results are given in Table II. DIPLF gives good posterior estimates with all moment computation methods. IPLF converges towards an estimate within 50 iterations only when using UKF for moment computation. Though the UKF and CKF usually have similar accuracy, in this case, the IPLF did not converge when using the CKF; changing the prior mean a little may make it to converge with different moment computation algorithms. RUF has slightly worse estimates than DIPLF and GGF does not provide good estimates with any of the tested moment computation methods.

In our second test, we computed estimates for two dimensional positioning using three range measurements in a single measurement update. The prior has zero mean with covariance $I$. Ranges were computed to beacons located at $\begin{bmatrix}-1&0\end{bmatrix}^{T},\begin{bmatrix}0&1\end{bmatrix}^{T}$ and $\begin{bmatrix}1&-2\end{bmatrix}^{T}$. Measurement noise covariance was $I$ and measurements were generated by sampling a true location from the prior and then generating corresponding measurements. In this test, we used the same filters as in the previous test and KLPUKF [raitoharju2016]. KLPUKF uses a linear transformation to decorrelate measurement elements in such a way that the nonlinearity of a measurement element is minimized and applies measurement elements sequentially. For the first test with only a scalar measurement, KLPUKF would have been identical to GGF.

The test is repeated 1000 times using different true location and measurement values. We analyze only state after one update step, but a more accurate update step can be expected to lead to improved stability in the filtering recursion. The mean KLDs, which are computed numerically using a dense grid, are presented in Table III. The iterative algorithms that use EKF moment computation, except RUF, produce worse estimates than EKF itself. This is probably because the posterior may be multimodal in this test. With other moment computation methods the IPLF is worse than GGF, but DIPLF is better than either of those. In this test, KLPUKF and DIPLF were the most accurate algorithms, except when tested with EKF. The residuals of the mean estimates compared to true locations were similar for all algorithms, thus the largest improvement of using DIPLF comes from better posterior covariance estimation. In this test, the computational time of DIPLF was 8 times higher than the computational cost of KLPUKF. Our implementations of other algorithms had runtimes between these, IPLF taking similar time as DIPLF. The exact comparison of computational complexity of algorithms evaluated in this letter cannot be done, because algorithms have different conditions for loops. These conditions depend on the estimation problem and computational complexity cannot be stated as a function of dimensionalities of the state and the measurement vector.

V Conclusions and future work

We have shown that by fixing the covariance matrices of the IPLF algorithm, the optimal posterior mean in the posterior linearization approach is the solution of an optimization problem. We solved this optimization problem using a damped GN algorithm. Then we updated the covariance matrices ${P}_{j}$ and $\Omega_{j}$ and repeated the mean optimization. Compared to IPLF, this algorithm is less prone to diverge. The proposed algorithm was more accurate than other tested algorithms in our simulations.

One topic for future research is to see how posterior linearization could be done with other optimization algorithms than GN, such as Levenberg-Marquadt. Another interesting research topic would be extending the algorithm to work with non-additive noise. We have done some preliminary simulations in time series, where we found that the proposed algorithm is more accurate than other iterative algorithms, but all iterative algorithms have a risk of converging to a wrong local solution. Investigating this is one future topic.