On Approximate Nonlinear Gaussian Message Passing On Factor Graphs

TL;DR

This paper develops approximate Gaussian message passing rules for nonlinear transformations in factor graphs, enabling efficient filtering and smoothing algorithms for nonlinear systems using numerical quadrature and Rauch-Tung-Striebel approximations.

Contribution

It introduces general forward and backward message passing rules for nonlinear nodes in factor graphs, expanding their applicability to nonlinear filtering and smoothing.

Findings

Derived Gaussian message passing rules for nonlinear transformations.

Proposed a nonlinear modified Bryson-Frazier smoother.

Demonstrated the rules' utility in nonlinear estimation algorithms.

Abstract

Factor graphs have recently gained increasing attention as a unified framework for representing and constructing algorithms for signal processing, estimation, and control. One capability that does not seem to be well explored within the factor graph tool kit is the ability to handle deterministic nonlinear transformations, such as those occurring in nonlinear filtering and smoothing problems, using tabulated message passing rules. In this contribution, we provide general forward (filtering) and backward (smoothing) approximate Gaussian message passing rules for deterministic nonlinear transformation nodes in arbitrary factor graphs fulfilling a Markov property, based on numerical quadrature procedures for the forward pass and a Rauch-Tung-Striebel-type approximation of the backward pass. These message passing rules can be employed for deriving many algorithms for solving nonlinear problems using factor graphs, as is illustrated by the proposition of a nonlinear modified Bryson-Frazier (MBF) smoother based on the presented message passing rules.

Equations

$$y=f(x)\in {\mathbb{R}}^m\iff p(y|x)=\delta \left(y-f(x)\right)$$

$$p(y)=\underset{{\mathbb{R}}^n}{\int }p(y|x)p(x)\mathrm{d}x=\underset{{\mathbb{R}}^n}{\int }\delta (y-f(x))p(x)\mathrm{d}x,$$

$$m_Y:=E\left[Y\right]=\underset{{\mathbb{R}}^m}{\int }yp(y)\mathrm{d}y=\underset{{\mathbb{R}}^n}{\int }f(x)p(x)\mathrm{d}x$$

$$\begin{gathered} V_Y=W_Y^{-1}:=\mathrm{Cov}\left[Y\right]=\underset{{\mathbb{R}}^m}{\int }(y-m_Y)(y-m_Y{)}^{T}p(y)\mathrm{d}y \\ =\underset{{\mathbb{R}}^n}{\int }(f(x)-m_Y)(f(x)-m_Y{)}^{T}p(x)\mathrm{d}x. \end{gathered}$$

$$\underset{{\mathbb{R}}^n}{\int }g(x)N\left(x;0,\mathcal{I}\right)\mathrm{d}x\approx \sum _{i=1}^{\ell }{w}_{i}g({\xi }_i),$$

$$g(x)\equiv (f(x)-m_Y)(f(x)-m_Y{)}^T,$$

$$p(x|u)$$

$$p(y|u)$$

$$p(y|u,z)\sim N(y;m_Y,V_Y)$$

$$p(x|u,z)\sim N(x;m_X,V_X),$$

$$p(z|x,y)=p(z|y)\text{and}p(y|u,x)=p(y|x).$$

$$p(x,y|u)=p(x|u)p(y|x)\text{and}$$

$$p(x|u,y)=p(x|u,y,z)$$

$$p(x,y|u,z)$$

$$=\frac{p(x,y|u)p(y|u,z)}{p(y|u)}$$

$$p(x|u,z)=\underset{{\mathbb{R}}^m}{\int }\frac{p(x,y|u)p(y|u,z)}{p(y|u)}\mathrm{d}y.$$

$$p(x,y|u)\sim N\left(\left(\begin{array}{c}x\\ y\end{array}\right);\left(\begin{array}{c}{\overrightarrow{m}}_X\\ {\overrightarrow{m}}_Y\end{array}\right),\left(\begin{array}{cc}{\overrightarrow{V}}_X& \overrightarrow{C}\\ {\overrightarrow{C}}^T& {\overrightarrow{V}}_Y\end{array}\right)\right),$$

$$p(x|u,z)\sim N\left(x;m_X,V_X\right)$$

$$x_i$$

$$y_i$$

$$p(y_1,\dots ,y_N,x_0,\dots ,x_N|u_1,\dots ,u_N)$$

$$=p(x_0)\cdot \prod _{i=1}^{N}p(y_i,x_i|x_{i-1},u_i)$$

$${\tilde{W}}_X=({\overrightarrow{V}}_X+{\overleftarrow{V}}_X{)}^{-1},{\tilde{\xi }}_X={\tilde{W}}_X({\overrightarrow{m}}_X-{\overleftarrow{m}}_X),$$

$${\tilde{W}}_X$$

$$={\overrightarrow{W}}_X-{\overrightarrow{W}}_X\left({\overrightarrow{V}}_X+\overrightarrow{D}(V_Y-{\overrightarrow{V}}_Y){\overrightarrow{D}}^T\right){\overrightarrow{W}}_X$$

$$=-{\overrightarrow{W}}_X\overrightarrow{C}{\overrightarrow{W}}_Y\left(-{\overrightarrow{V}}_Y{\tilde{W}}_Y{\overrightarrow{V}}_Y\right){\overrightarrow{W}}_Y{\overrightarrow{C}}^T{\overrightarrow{W}}_X$$

$$={\overrightarrow{W}}_X\overrightarrow{C}{\tilde{W}}_Y{\overrightarrow{C}}^T{\overrightarrow{W}}_X,$$

$$m_X={\overrightarrow{m}}_X-{\overrightarrow{V}}_X{\tilde{\xi }}_X={\overleftarrow{m}}_X+{\overleftarrow{V}}_X{\tilde{\xi }}_X,$$

$${\tilde{\xi }}_X$$

$$={\tilde{W}}_X\left(\overrightarrow{C}{\tilde{\xi }}_Y+\left({\tilde{W}}_X^{-1}-{\overrightarrow{V}}_X\right){\tilde{\xi }}_X\right)$$

$$={\tilde{W}}_X\overrightarrow{C}{\tilde{\xi }}_Y+{\tilde{\xi }}_X-{\tilde{W}}_X{\overrightarrow{V}}_X{\tilde{\xi }}_X={\overrightarrow{W}}_X\overrightarrow{C}{\tilde{\xi }}_Y$$

$${\overrightarrow{m}}_Y$$

$${\overrightarrow{V}}_Y$$

$$m_X$$

$$V_X$$

$${\tilde{W}}_X$$

$${\tilde{\xi }}_X$$

Full text

**Index Terms— ** Nonlinear Filtering, Nonlinear Smoothing, Sigma Point Filtering, Factor Graphs, Message Passing

1 Introduction

Factor graphs are a graphical representation of statistical independence statements regarding probability distributions. These probabilistic graphical models facilitate the application of inference algorithms by means of message passing on the graph [1, 2, 3, 4]. Many classical algorithms such as recursive least squares (RLS) [4], linear Kalman filtering and smoothing [5], and expectation maximization [6] have already been formulated as message passing on a factor graph. This enables the simple generation of novel algorithms by adapting the factor graph of known algorithms to a given problem, or by combining multiple known algorithms into a single graph and, hence, a joint algorithm. Algorithms adapted to a variety of problems such as cooperative localization [7], sparse input estimation [8], and motion planning [9] have been derived.

Despite the vast amount of literature on nonlinear Gaussian filtering and smoothing algorithms [10, 11, 12, 13, 14], to the authors’ knowledge very few efforts have been made to represent existing nonlinear filtering algorithms in the factor graph framework. Meyer, Hlinka, and Hlawatsch [15] describe sigma-point belief propagation (SPBP) algorithms on factor graphs, but they consider a non-sequential system model with observations depending on pairs of states, hence making their results inapplicable to sequential filtering and smoothing problems. Deisenroth and Mohamed [16] propose the use of expectation propagation (EP) as a general framework for Gaussian smoothers, similarly to [17], in which moments of distributions from the univariate exponential family are approximated using Gaussian quadrature. Both approaches result in iterative update rules with respect to the marginals.

In the present paper, we make an attempt at providing concise update rules for performing approximate Gaussian message passing through deterministic nonlinear nodes in factor graphs, thus enabling the representation of known nonlinear filtering and smoothing algorithms, and facilitating the derivation of new algorithms for various nonlinear problems. Similarly to [17, 16], we propose the use of numerical quadrature for computing moments, however we focus our exposition on deriving efficient approximate message passing rules for the directed messages instead of the marginals, which results in non-iterative filtering and smoothing schemes for graphs without loops.

2 Nonlinear Transformations

Consider a deterministic, nonlinear transformation

[TABLE]

of a random variable $X$ with values in $\mathbb{R}^{n}$. The probability density function (PDF) of $Y$ is given by

[TABLE]

the expected value of $Y$ results as

[TABLE]

and the covariance as

[TABLE]

For arbitrary nonlinear functions $f$ and PDFs $\mathop{{}p}(x)$, the integrals in eqs. (3) and (4) rarely admit closed-form analytical solutions. Hence, the prior density $\mathop{{}p}(x)$ is usually assumed to be normally distributed, and numerical quadrature procedures are employed to approximate both integrals, such as the unscented transform (UT) [10], Gauss-Hermite quadrature (GHQ) [11], spherical-radial transform (SRT), which in a filtering setting results in the Cubature Kalman Filter (CKF) [13], or sparse-grid quadrature using the Smolyak rule [14]. The resulting estimates for $\mathop{{}\mathrm{E}}\left[Y\right]$ and $\mathop{{}\mathrm{Cov}}\left[Y\right]$ are then used as the parameters of a normal approximation to the PDF $\mathop{{}p}(y)$, which amounts to approximate moment matching and, hence, approximate minimization of the Kullback-Leiber divergence between the actual distribution $\mathop{{}p}(y)$ and its normal approximation [18].

All of the methods mentioned above have in common that they perform the approximation

[TABLE]

of a normally weighted integral, where the number $\ell$ of integration points $\xi_{i}$ and the weights $w_{i}$ differ between methods, and $\mathop{{}\mathcal{N}}\left(x;m,V\right)$ denotes the PDF of a normally distributed variable $x$ with mean $m$ and covariance matrix $V$. Note that in order to solve eq. (4) using the approximation (5), one chooses

[TABLE]

and hence $\mathop{{}g}(x)$ is a polynomial of degree $2k$ if $f$ is a polynomial of degree $k$. As a consequence, both the UT and the SRT quadrature formulas, which yield exact results for polynomials up to and including degree $3$, do not calculate the covariance exactly for polynomials $f$ of order $2$ [13]. In particular, this includes bilinear functions $f$ resulting from a multiplication of two Gaussian random variables. Gauss-Hermite quadrature formulas of arbitrary order can be readily constructed, but unfortunately these formulas suffer heavily from the curse of dimensionality [13]. Sparse-grid quadrature rules on the other hand, of which the classical UT has been shown to be a subset, can be flexibly adjusted in their degree of precision with a number of quadrature points growing polynomially in the number of dimensions, hence alleviating the curse of dimensionality [14].

Having described a feasible strategy for performing the forwards (filtering) pass through a nonlinear transformation, we will now consider the backwards (smoothing) pass. If the inverse $f^{-1}$ of the nonlinear transformation is available, the same approach that has been described so far can also be used to implement the backward pass. If, however, this is not the case, nonlinear Rauch-Tung-Striebel-type (RTS) smoothers can be derived, such as the unscented RTS smoother proposed by Särkkä [12]. In the following, a general nonlinear Gaussian RTS smoother-type backward pass through a nonlinear function node in a factor graph is derived, following the derivation in [12] but employing a slightly more general setting to facilitate local interpretation in a factor graph context.

Consider again the deterministic nonlinear transformation (1), and assume the parameters $\overrightarrow{m}_{X},\overrightarrow{m}_{Y},m_{Y},\overrightarrow{V}_{X},\overrightarrow{V}_{Y}$, and $V_{Y}$ of the filtering distributions

[TABLE]

and the smoothing distribution of $y$

[TABLE]

known, where $u$ denotes knowledge on $x$, and $z$ denotes knowledge on $y$. The aim is now to compute the parameters $m_{X},V_{X}$ of a Gaussian approximation

[TABLE]

to the smoothing distribution of $x$ incorporating all available data. To this end, further assume that the model satisfies the Markov conditions

[TABLE]

It then holds that

[TABLE]

due to the assumed Markov property (10). It follows that

[TABLE]

and by marginalization of $y$

[TABLE]

If the joint distribution $\mathop{{}p}(x,y|u)$ is approximated by a normal distribution

[TABLE]

with suitably chosen covariance matrix $\overrightarrow{C}$, the marginalization (11) can be evaluated analytically to [12]

[TABLE]

with $m_{X}$, $V_{X}$, $\overrightarrow{D}$, and $\overrightarrow{C}$ defined as in eqs. (17), (18), (21) and (22) in table 1, where $\overrightarrow{C}$ has been approximated using any of the quadrature methods described previously.

3 Message passing on factor graphs

A factor graph is a graphical representation of a factorization of an arbitrary function. Forney-style factor graphs (FFGs) consist of nodes, which represent factors, and edges connecting these nodes, which represent the variables that each factor depends on [4]. Inference can be efficiently performed by means of message passing along the edges of the factor graph. Edges are undirected, but arrows are introduced to disambiguate between messages $\overrightarrow{\mu}$ and $\overleftarrow{\mu}$ in and against the direction of an edge, respectively. Figure 1 shows an FFG representation of the PDF of the nonlinear state space model

[TABLE]

with deterministic nonlinear functions $f$, $g$, and $h$, inputs $u_{i}$ and process and measurement noise $w_{i}\sim\mathop{{}\mathcal{N}}\left(0,\sigma_{W}^{2}\right)$ and $v_{i}\sim\mathop{{}\mathcal{N}}\left(0,\sigma_{V}^{2}\right)$, respectively. One of the main benefits of the factor graph framework is that it allows for the easy combination of existing algorithms from different fields, such as filtering/smoothing, sparse input estimation, parameter estimation, and control [6, 8, 19, 20] to derive powerful new algorithms.

The dashed box in fig. 1 represents the joint probability $\mathop{{}p}(x_{i},y_{i},x_{i-1},u_{i})$, or equivalently the conditional distribution $\mathop{{}p}(x_{i},y_{i}\lvert x_{i-1},u_{i})=\frac{\mathop{{}p}(x_{i},y_{i},x_{i-1},u_{i})}{\mathop{{}p}(x_{i-1})\mathop{{}p}(u_{i})}$. Accordingly, for $N$ samples and initial state $x_{0}$ this results in the Markov chain

[TABLE]

representing the PDF of the complete sequential model. For linear functions $f$, $g$, and $h$, well-known algorithms such as Kalman filtering and smoothing can be understood as special cases of the sum-product message passing algorithm on this model [4], effectively performing Gaussian message passing. Inspired by the conciseness of [4], the aim of the present contribution is to provide tabulated rules for approximate Gaussian message passing through the deterministic nonlinear transformation node depicted in fig. 2, representing the factor $\mathop{{}\delta}\left(y-\mathop{{}f}(x)\right)$.

To this end, the approximation methods presented in the previous section may now be formulated as message passing on factor graphs, the rules for which are summarized in table 1. To obtain these rules, we identify the forward message $\overrightarrow{\mu}_{X}$ with the filtering distribution $\mathop{{}p}(x|u)$ and the marginal message $\mu_{X}$ used in the backward pass with the smoothing distribution $\mathop{{}p}(x|u,z)$, with the parameters of the Gaussian distributions defined as in eqs. (6)-(9), and $u$ and $z$ denoting available knowledge on $x$ and $y$, respectively, as before. Eqs. (3)-(5) then directly yield the update rules (15) and (16) for the forward pass. Eq. (12) yields the update rules (17), (18) for the backward pass in the $(m_{X},V_{X})$ parameterization of the Gaussian messages. This parameterization is, however, not necessarily desirable to use for practical applications since passing backwards through a filtering graph in this parameterization may require multiple matrix inversions for each time slice [5]. Therefore, we additionally derive message passing rules in the

[TABLE]

parameterization, which has proven advantageous for efficient realization of the (backwards) smoothing pass on factor graphs such as the one shown in fig. 1 [5]. Using the Woodbury formula [21] and $V_{X}=(\overrightarrow{W}_{X}+\overleftarrow{W}_{X})^{-1}$ [4], we obtain

[TABLE]

corresponding to update rule (19). For $\tilde{\xi}_{X}$, using [4]

[TABLE]

we obtain

[TABLE]

as summarized in update rule (20).

4 Nonlinear filtering and smoothing

The approach described in the previous sections can now be used to describe various new and existing nonlinear filtering and smoothing algorithms by performing forward and backward message passing along factor graphs such as the one shown in fig. 1. One instance of the class of algorithms that can be derived from this framework is given by the following nonlinear Modified Bryson-Frazier (MBF) smoother for state space models of the form (13) with linear output $h(x)=Hx$:

Nonlinear MBF Smoother:

Perform forward message passing using equations (15) and (16), as well as the previously proposed update rules (II.1), (II.2), (V.1), and (V.2) from [8]. 2. 2.

Perform backward message passing using equations (19) and (20) as well as the previously proposed update rules (II.6), (II.7), and either (V.4), (V.6), (V.8) or (V.5), (V.7), (V.9) from [8].

The smoother is adapted from the factor graph formulation of the MBF smoother [22] for linear systems provided by Loeliger, Bruderer, Malmber, et al. [8]. It requires just a single matrix inversion for each backward time step and is included here mainly to demonstrate the utility of the presented factor graph representation of nonlinear Gaussian message passing for deriving various nonlinear filters and smoothers. The smoother may be implemented using any kind of numerical quadrature procedure and amounts to standard message passing on a statistically linearized factor graph [23].

5 Conclusion

In this contribution, local message passing rules for factor graph nodes representing deterministic nonlinear transformations have been derived. For the forward pass, a linearization is performed using any numerical quadrature method,and for the backward pass, general Rauch-Tung-Striebel (RTS)-type update rules have been derived in two different message parameterizations.The resulting message passing rules can be employed in any factor graphand in particular can be used to perform filtering and smoothing on state space models with state transition, input, and measurement nonlinearities. Demonstrating the usefulness of transferring results from classical nonlinear filtering theory to the factor graph framework, the modified Bryson-Frazier (MBF) smoother is easily augmented to incorporate nonlinear state transitions and input nonlinearities, requiring only a single matrix inversion in each time step. In this way, the present contribution adds the capability of handling nonlinear systems to a range of existing algorithms, hence enabling a factor graph description of a whole range of various new and existing algorithms.

Acknowledgements

The authors would like to thank Maximilian Pilz for interesting discussions on the subject of this article.