Continuous-Discrete Extended Kalman Filter on Matrix Lie Groups Using Concentrated Gaussian Distributions

Abstract

In this paper we generalize the continuous-discrete extended Kalman filter (CD-EKF) to the case where the state and the observations evolve on connected unimodular matrix Lie groups. We propose a new assumed density filter called continuous-discrete extended Kalman filter on Lie groups (CD-LG-EKF). It is built upon a geometrically meaningful modeling of the concentrated Gaussian distribution on Lie groups. Such a distribution is parametrized by a mean and a covariance matrix defined on the Lie group and in its associated Lie algebra respectively. Our formalism yields tractable equations for both non-linear continuous time propagation and discrete update of the distribution parameters under the assumption that the posterior distribution of the state is a concentrated Gaussian. As a side effect, we contribute to the derivation of the first and second order differential of the matrix Lie group logarithm using left connection. We also show that the CD-LG-EKF reduces to the usual CD-EKF if the state and the observations evolve on Euclidean spaces. Our approach leads to a systematic methodology for the design of filters, which is illustrated by the application to a camera pose filtering problem with observations on Lie group. In this application, the CD-LG-EKF significantly outperforms two constrained non-linear filters (one based on a linearization technique and the other on the unscented transform) applied on the embedding space of the Lie group.

Appendices

Properties and Notations

Property 1

First order Taylor expansion of $\text{ Ad }_{G}\left( \cdot \right)$:

$$\begin{aligned}&\text{ Ad }_{G}\left( \text{ exp }_{G}\left( \left[ a\right] _{G}^{\wedge }\right) \right) =\text{ exp }_{G}\left( \text{ ad }_{G}\left( a\right) \right) \nonumber \\&\quad =\text{ Id }_{p\times p}+\text{ ad }_{G}\left( a\right) +O\left( \left\| a\right\| ^{2}\right) \end{aligned}$$

(134)

where $a\in \mathbb {R}^{p}$.

Property 2

First order Taylor expansion of $\text{ exp }_{G}\left( \cdot \right)$:

$$\begin{aligned} \text{ exp }{}_{G}\left( \epsilon _{\wedge }\right) =\text{ Id }_{n\times n}+\epsilon _{\wedge }+\frac{1}{2} \epsilon _{\wedge }^{2}+O\left( \left\| \epsilon _{\wedge }\right\| ^{3}\right) \end{aligned}$$

(135)

where $\epsilon _{\wedge }\in \mathfrak {g}$

Property 3

Adjoint properties:

$$\begin{aligned}&\left( \text{ ad }_{G}\left( x\right) \right) _{ij} =L_{ij}^{T}x\end{aligned}$$

(136)

$$\begin{aligned}&\left( \text{ ad }_{G}\left( x\right) ^{T}\right) _{ij} =\left( \text{ ad }_{G}\left( x\right) \right) _{ji}=L_{ji}^{T}x\end{aligned}$$

(137)

where $L_{ij}\in \mathbb {R}^{p\times 1}$ and $x\in \mathbb {R}^{p\times 1}$. We have:

$$\begin{aligned}&\left( \text{ ad }_{G}\left( x\right) R\text{ ad }_{G}\left( x\right) ^{T}\right) {}_{ij}\nonumber \\&\quad =\mathbb {E}\left( \sum _{k=1}^{p}\sum _{l=1}^{p} \left( \text{ ad }_{G}\left( x\right) \right) _{ik}R_{kl}\left( \text{ ad }_{G}\left( x\right) ^{T}\right) _{lj}\right) \nonumber \\&\quad =\mathbb {E}\left( \sum _{k=1}^{p}\sum _{l=1}^{p} L_{ik}^{T}xR_{kl}L_{jl}^{T}x\right) \nonumber \\&\quad =\mathbb {E}\left( \sum _{k=1}^{p}\sum _{l=1}^{p} R_{kl}L_{ik}^{T}xx^{T}L_{jl}\right) \nonumber \\&\quad =\sum _{k=1}^{p}\sum _{l=1}^{p}R_{kl}L_{ik}^{T}xx^{T}L_{jl} \end{aligned}$$

(138)

and

$$\begin{aligned} \left( \text{ ad }_{G}\left( x\right) ^{2}\right) _{ij}&=\sum _{k=1}^{p}\left( \text{ ad }_{G}\left( x\right) \right) _{ik}\left( \text{ ad }_{G}\left( x\right) \right) _{kj}\nonumber \\&=\sum _{k=1}^{p}L_{ik}^{T}xL_{kj}^{T}x\nonumber \\&=\sum _{k=1}^{p}L_{ik}^{T}xx^{T}L_{kj} \end{aligned}$$

(139)

Propagation

For the following dynamical equation where $f$ and $G$ are bounded and Lipschitz functions:

$$\begin{aligned} dx=f\left( x\right) dt+G\left( x\right) d\beta \end{aligned}$$

(140)

where $\beta$ is a Brownian process with diffusion matrix $Q\left( t\right)$, and $x\in \mathbb {R}^{m}$, the mean and covariance propagation equations are:

$$\begin{aligned}&\dot{m}_{x}=\mathbb {E}\left[ f\left( x\right) \right] \end{aligned}$$

(141)

$$\begin{aligned}&\dot{P}_{x} =\left( \mathbb {E}\left[ f\left( x\right) x^{T}\right] - \mathbb {E}\left[ f\left( x\right) \right] m_{x}^{T}\right) \nonumber \\&\qquad \quad +\left( \mathbb {E}\left[ xf\left( x\right) ^{T}\right] -m_{x}\mathbb {E}\left[ f\left( x\right) ^{T}\right] \right) \nonumber \\&\qquad \quad +\mathbb {E}\left( G\left( x\right) QG\left( x\right) ^{T}\right) \end{aligned}$$

(142)

see [28] Vol.2 Chap.11 Sec.6.

Update

Let $A\in G$ et $b,c\in \mathbb {R}^{p}$, then:

$$\begin{aligned} A\text{ exp }\left( b+c\right)&=A\text{ exp }\left( b\right) \text{ exp }\left( -b\right) \text{ exp }\left( b+c\right) \nonumber \\&\overset{(16)}{=}A\text{ exp }\left( b\right) \text{ exp }\left( \varPhi \left( b\right) c+O\left( \left\| c\right\| ^{2}\right) \right) \end{aligned}$$

(143)

After the Lie algebraic error update step (3.4.1) we have:

$$\begin{aligned} X_{k|k}&=\mu _{k|k-1}\text{ exp }{}_{G}\left( \left[ \epsilon _{k|k}^{-}\right] _{G}^{\wedge }\right) \nonumber \\&=\mu _{k|k-1}\text{ exp }{}_{G}\left( \left[ m_{k|k}^{-}+r_{k|k}^{-}\right] _{G}^{\wedge }\right) \end{aligned}$$

(144)

where $r_{k|k}^{-}\sim \mathcal {N}_{\mathbb {R}^{p}} \left( \varvec{0}_{p\times 1},P_{k|k}^{-}\right)$. Applying (143), we obtain:

$$\begin{aligned}&X_{k|k}=\mu _{k|k-1}\text{ exp }{}_{G}\left( \left[ m_{k|k}^{-}\right] _{G}^{\wedge }\right) \nonumber \\&\quad \text{ exp }{}_{G}\left( \left[ \varPhi _{G}\left( m_{k|k}^{-}\right) \epsilon _{k|k}^{-}+O\left( \left\| \epsilon _{k|k}^{-}\right\| ^{2}\right) \right] _{G}^{\wedge }\right) \nonumber \\&\quad =\mu _{k|k}\text{ exp }{}_{G}\left( \left[ \epsilon _{k|k}\right] _{G}^{\wedge }\right) \end{aligned}$$

(145)

where

$$\begin{aligned}&\mu _{k|k}=\mu _{k|k-1}\text{ exp }{}_{G}\left( \left[ m_{k|k}^{-}\right] _{G}^{\wedge }\right) \end{aligned}$$

(146)

$$\begin{aligned}&\quad \epsilon _{k|k}=\varPhi _{G}\left( m_{k|k}^{-}\right) \epsilon _{k|k}^{-} +O\left( \left\| \epsilon _{k|k}^{-}\right\| ^{2}\right) \end{aligned}$$

(147)

From the previous expression and neglecting terms in $O\left( \left\| \epsilon _{k|k}^{-}\right\| ^{2}\right)$:

$$\begin{aligned}&\mathbb {E}\left[ \epsilon {}_{k|k}\right] =\varvec{0}_{p\times 1} \end{aligned}$$

(148)

$$\begin{aligned}&P_{k|k}=\mathbb {E}\left[ \epsilon {}_{k|k}\epsilon {}_{k|k}^{T}\right] \nonumber \\&\quad =\mathbb {E}\left[ \varPhi _{G}\left( m_{k|k}^{-}\right) \epsilon _{k|k}^{-}\epsilon _{k|k}^{-T}\varPhi _{G}\left( m_{k|k}^{-}\right) ^{T}\right] \nonumber \\&\quad =\varPhi _{G}\left( m_{k|k}^{-}\right) P_{k|k}^{-} \varPhi _{G}\left( m_{k|k}^{-}\right) ^{T} \end{aligned}$$

(149)

$SO\left( 3\right)$ Properties

• $\text{ exp }_{SO\left( 3\right) }$, $\text{ log }_{SO\left( 3\right) }$ and $\varPhi _{SO\left( 3\right) }$ can be computed efficiently using Rodrigues’ rotation formulae [37]

• Let $a=\left[ \begin{array}{c} a_{1}\\ a_{2}\\ a_{3} \end{array}\right]$ then $\left[ a\right] _{SO\left( 3\right) }^{\wedge }=\left[ \begin{array}{ccc} 0 &{} -a_{3} &{} a_{2}\\ a_{3} &{} 0 &{} -a_{1}\\ -a_{2} &{} a_{1} &{} 0 \end{array}\right]$

• Let $b\in \mathbb {R}^{3}$ then $\left[ a\right] _{SO\left( 3\right) }^{\wedge }b=\left[ b\right] ^{*}a$ where $\left[ b\right] ^{*}=-\left[ b\right] _{SO\left( 3\right) }^{\wedge }$

• $\text{ ad }_{SO\left( 3\right) }\left( a\right) =\left[ a\right] _{SO\left( 3\right) }^{\wedge }$