Unscented Kalman Filtering on Riemannian Manifolds

Abstract

In recent years there has been a growing interest in problems, where either the observed data or hidden state variables are confined to a known Riemannian manifold. In sequential data analysis this interest has also been growing, but rather crude algorithms have been applied: either Monte Carlo filters or brute-force discretisations. These approaches scale poorly and clearly show a missing gap: no generic analogues to Kalman filters are currently available in non-Euclidean domains. In this paper, we remedy this issue by first generalising the unscented transform and then the unscented Kalman filter to Riemannian manifolds. As the Kalman filter can be viewed as an optimisation algorithm akin to the Gauss-Newton method, our algorithm also provides a general-purpose optimisation framework on manifolds. We illustrate the suggested method on synthetic data to study robustness and convergence, on a region tracking problem using covariance features, an articulated tracking problem, a mean value optimisation and a pose optimisation problem.

Appendices

Appendix A: Definitions from Differential Geometry

We give definitions of some concepts from differential geometry that we use in the paper (mainly from [5]) for the convenience of the reader.

• 1. Differentiable Manifolds: A differentiable manifold of dimension M is a set $\mathcal{M}$ and a family of injective mappings $\mathcal{T}$ = $\{x_{i}: U_{i} \subset\mathbb {R}^{M} \to\mathcal{M}\}$ of open sets U _i of ℝ^M into $\mathcal{M}$ such that

  • $\bigcup_{i} x_{i}(U_{i}) = \mathcal{M}$, i.e. the open sets cover $\mathcal{M}$.

  • For any pair i,j with x _i (U _i )⋂x _j (U _j )=W≠ϕ, the mapping $x_{j}^{-1} \circ x_{i}$ is differentiable.

  • The family $\mathcal{T}$ is maximal, which means that if (y,V), $y:V\subset \mathbb{R}^{M}\to\mathcal{M}$ is such that: for each element of $\mathcal{T}$, (x _i ,U _i ) with $x_{i}(U_{i})\cap y(V)\not= 0$ implies that y ⁻¹∘x _i is a diffeomorphism, then in fact $(y,V)\in\mathcal{T}$.

• 2. Directional derivative of a function along a vector field: A vector field X on $\mathcal{M}$ is a map that associates to each $p\in\mathcal{M}$ an element $X(p)\in T_{p}\mathcal{M}$, where $T_{p}\mathcal{M}$ is the tangent space of $\mathcal{M}$ at p. The space of smooth vector fields on $\mathcal{M}$ is denoted ${\frak{X}}(\mathcal{M})$. Let $f:\mathcal{M}\to\mathbb{R}$ be a differentiable function of $\mathcal{M}$ and X a vector field on $\mathcal{M}$. The directional derivative X.f is the function $\mathcal{M}\to\mathbb{R}$,

  

  (53)

  the differential of f at p evaluated at vector X(p).

• 3. Covariant tensors: A p-covariant tensor h is a $\mathcal{C}^{\infty}$ p-linear map

  

  (54)

  i.e., for all $x\in\mathcal{M}$, x↦h _x :

  

  (55)

  is p-linear and for vector fields $X_{1},\dots,X_{p} \in{\frak{X}}$, the map x↦h _x (X ₁(x),…,X _p (x)) is smooth.

• 4. Riemannian Metric: A Riemannian metric on a manifold $\mathcal{M}$ is a covariant 2-tensor g which associates to each point $p\in\mathcal{M}$ an inner product g _p =〈−,−〉 _p on the tangent space $T_{p}\mathcal{M}$, i.e., not only is it bilinear, but symmetric and positive definite and thus define a Euclidean distance on each tangent space. In terms of local coordinates, the metric at each point x is given by a matrix, g _ij =〈X _i ,X _j 〉 _x , where X _i ,X _j are tangent vectors to $\mathcal{M}$ at x, and it varies smoothly with x. A Geodesic curve is a local minimiser of arc-length computed with a Riemannian metric.

• 5. Affine connection: An affine connection ∇ on a differentiable manifold $\mathcal {M}$ is a mapping

  

  (56)

  which is denoted by ∇(X,Y)→∇ _X Y and which satisfies the following properties:

  • ∇ _fX+gY Z=f∇ _X Z+g∇ _Y Z

  • ∇ _X (Y+Z)=∇ _X Y+∇ _X Z

  • ∇ _X (fY)=f∇ _X Y+X(f)Y

  in which $X,Y,Z \in{\frak{X}}(\mathcal{M})$ and f,g are $\mathcal {C}^{\infty}(\mathcal{M})$. This gives a notion of directional derivative of a vector field defined on the manifold. An affine connection extends naturally to more than vector fields, and especially of interest here, covariant tensors: if h is a covariant p-tensor and $X\in{\frak{X}}(\mathcal {M})$, ∇ _X h is defined as follows. Given p vector fields $Y_{1},\dots,Y_{p}\in{\frak{X}} (\mathcal{M})$,

  

  (57)

• 6. Covariant derivatives: Let $\mathcal{M}$ be a differentiable manifold with affine connection ∇. There exists a unique correspondence which associates to a vector field V along the differentiable curve $c:I\to\mathcal{M}$ another vector field $\frac{DV}{dt}$ along c, called the covariant derivative of V along c, such that

  • $\frac{D}{dt}(V+W) = \frac{DV}{dt} + \frac{DW}{dt}$, where W is a vector field along c.

  • $\frac{D}{dt}(fV) = \frac{df}{dt}V + f\frac{DV}{dt}$, where f is a differentiable function on I.

  • If V is induced by a vector field Y, a member of the tangent bundle of $\mathcal{M}$, i.e. V(t)=Y(c(t)), then $\frac{DV}{dt} = \nabla_{\frac{dc}{dt}} Y$.

  The covariant derivative extends to covariant tensors via the extension of the connection to them: Given a covariant p-tensor h defined along c and vector fields U ₁,…,U _p along c,

  

  (58)

• 7. Parallel transport: Given a vector $P\in T_{c(0)}\mathcal{M}$, the differential equation

  

  (59)

  admits a unique solution, called the parallel transport of P along c. The induced map P↦P(t) from $T_{c(0)}\mathcal{M}$ to $T_{c(t)}\mathcal {M}$ is a linear isomorphism.

  The parallel transport extends to covariant tensors in the same way: Given a p-linear mapping $h:T_{c(0)}\mathcal{M}\times\dots\times T_{c(0)}\mathcal{M}\to \mathbb{R}$, the differential equation

  

  (60)

  admits a unique solution, called the parallel transport of h along c. As for vectors, the mapping h↦h(t) is a linear isomorphism between p-linear maps on $T_{c(0)}\mathcal{M}$ and p-linear maps on $T_{c(t)}\mathcal{M}$.

• 8. Levi-Civita connection: Given a Riemannian metric g on the manifold $\mathcal{M}$, there exists a unique affine connection ∇ such that

  • compatibility with the metric:

    $$ X.g(Y,Z) = g(\nabla_XY,Z) + g(X, \nabla_X Z) $$

    (61)

  • symmetry:

    

    (62)

    ([X,Y] is the Lie bracket of X and Y).

  ∇ is the Levi-Civita connection associated to g. Note that from the previous items, one has ∇ _X g=0 for any $X\in{\frak{X}}(\mathcal {M})$ and that the parallel transport in that case is a linear isometry.

  The compatibility of ∇ and the metric g can be expressed in term of covariant derivatives: if X(t)=X(c(t)) and Y(t)=Y(c(t)) are two vector fields along the curve c, and D/dt is the covariant derivative along c,

  $$ \frac{d}{dt}g\bigl(X(t),Y(t)\bigr) = g \biggl( \frac{DX(t)}{dt},Y(t) \biggr) + g \biggl(X(t),\frac{DY(t)}{dt} \biggr). $$

  (63)

• 9. Christoffel symbols: In a parametrised manifold, where the curve c(t) is represented as (x ¹(t),…,x ^M(t)), the covariant derivative of a vector field v becomes

  $$ \frac{Dv}{dt} = \sum_m \biggl\{ \frac{dv^m}{dt} + \sum_{i,j}\varGamma_{ij}^m v^j\frac{dx^i}{dt} \biggr\} \frac{\partial}{\partial x_m}, $$

  (64)

  where the $\varGamma_{ij}^{m}$ are the coefficients of the connection also known as the Christoffel symbols Γ. In particular, the parallel transport equation above becomes the first-order linear system

  

  (65)

  For the Levi-Civita connection associated with the metric g, the corresponding Christoffel symbols are given by

  $$ \varGamma_{ij}^m = \frac{1}{2}\sum _{l} \biggl\{\frac{\partial}{\partial x_i}g_{jm} + \frac{\partial}{\partial x_j}g_{mi} - \frac{\partial}{\partial x_m}g_{ij} \biggr \}g^{ml} $$

  (66)

  g _ij is the ijth element of the metric, and g ^ij is the ijth element of its inverse. A curve is geodesic if the covariant derivative of its tangent vector field is zero everywhere on it, which means that a geodesic curve has zero tangential acceleration. Such a curve c satisfies the second order system of ODEs, which, with the above parametrisation becomes

  $$ \frac{d^2x^m}{dt^2} + \sum_{ij} \varGamma^m_{ij}\frac{dx^i}{dt}\frac {dx^j}{dt} = 0,\quad m=1\dots M. $$

  (67)

• 10. Exponential map: The exponential map is a map $\operatorname{Exp}: T\mathcal{M}\to\mathcal{M}$, that maps $v \in T_{q}\mathcal{M}$ for $q \in\mathcal{M}$, to a point $\operatorname{Exp}_{q}v$ in $\mathcal{M}$ obtained by going out the length equal to |v|, starting from q, along a geodesic which passes through q with velocity equal to $\frac{v}{|v|}$. Given $q \in\mathcal{M}$ and $v\in T_{q}\mathcal{M}$, and a parametrisation (x ₁,…,x _n ) around q, $\operatorname{Exp}_{q}(v)$ can be defined as the solution at time 1 of the above system of ODEs (67) with initial conditions (x ^m(0))=q and $(\frac{dx^{m}}{dt}(0)) = v$, m=1,…,M. The geodesic starting at q with initial velocity t can thus be parametrised as

  

  (68)

• 11. Logarithm map: For $\tilde{q}$ in a sufficiently small neighbourhood of q, the length minimising curve joining q and $\tilde{q}$ is unique as well. Given q and $\tilde{q}$, the direction in which to travel geodesically from q in order to reach $\tilde{q}$ is given by the result of the logarithm map $\operatorname{Log}_{q}(\tilde{q})$. We get the corresponding geodesics as the curve $t\mapsto \operatorname{Exp}_{q}(t\operatorname{Log}_{q}\tilde{q})$. In other words, $\operatorname{Log}$ is the inverse of $\operatorname{Exp}$ in the neighbourhood.

Appendix B: Numerical Implementation of Exponential Maps and Parallel Transports

In this appendix, we briefly review some techniques for numerical implementation of exponential Maps and parallel transports. We have applied them in the articulated tracking example in Sect. 4.3. As logarithm maps are not used, they will not be described here. It is worth noticing that the numerical techniques presented in the following are easily adapted to other manifolds, though care should be taken in, e.g., step size selection when the manifold is highly curved.

2.1 B.1 Computing Exponential Maps

Assume $\mathcal{M}$ is an M-dimensional sub-manifold of ℝ^N and that the metric is inherited from the standard inner product in ℝ^N. We can then discretise the geodesics in a straight-forward manner. We remind the reader that, given a vector v in the tangent space of the point x ₀, the exponential map seeks a point $\operatorname{Exp}_{\mathbf{x}_{0}} (\mathbf{v})$ on the geodesic curve starting at x ₀ with the same length and direction as v.

We then discretise the geodesics in a straightforward way by applying a standard forward Euler scheme based on the standard projection method [11]. The method repeatedly takes a discrete step in the tangent space in the direction encoded by v followed by a projection back onto the manifold. The latter is necessary as the discrete step will “fall of the manifold” unless $\mathcal{M}$ is flat. This scheme is illustrated in Fig. 16. The missing piece is a scheme for projecting a point in the embedding space back onto $\mathcal{M}$. Defining projection of $\hat{\mathbf{x}} \in\mathbb{R}^{N}$ as finding the nearest point on $\mathcal{M}$ reduces projection to a non-linear least-squares problem [15],

(69)

which can easily be solved using gradient descent [15]. Specifically, we apply a projected steepest descent with line-search, as empirical results have shown it to be both fast and stable [7]. This scheme usually finds a local optimum within a few iterations as it is warm-started with the results from the previous iteration.

Fig. 16

An illustration of a 4-step standard projection method for approximating the exponential map

In the practical implementation, we use 10 discrete steps in the Euler scheme, where the step length is controlled by the length of the tangent vector and the length of the geodesic segment computed so far. More sophisticated schemes that take local curvature into account might prove beneficial, but we have not experimented with this. We have tried with more discrete steps, but did not notice much improvement; we speculate that this is because we only need to compute short geodesic segments due to the sequential nature of the tracking problem.

2.2 B.2 Computing the Parallel Transport

Given two points x ₀ and x _I in $\mathcal{M}$ and the geodesic segment α that joins them, we describe a classical approximation of the parallel transport of a vector $\mathbf{v}_{0} \in T_{\mathbf{x}_{0}}\mathcal{M}$ to a vector $\mathbf {v}_{i} \in T_{\mathbf{x}_{I}}\mathcal{M}$ along α, known as Schild’s Ladder [29]. This scheme places points along the geodesic and approximately parallel transports v ₀ to these by forming approximate parallelograms on $\mathcal{M}$.

Let {x ₁,…,x _I−1} denote points along the geodesic segment joining x ₀ and x _I . Then start by computing $\mathbf {a}_{0} = \operatorname{Exp}_{\mathbf{x}_{0}} (\mathbf{v}_{0})$ and the midpoint b ₁ of the geodesic segment joining x ₁ and a ₀ (see the left side of Fig. 17). Follow the geodesic from x ₀ through b ₁ for twice its length to the point a ₁. This scheme is repeated for all sampled points along the geodesic from x ₀ to x _I (see the right side of Fig. 17). The final parallel transport of v ₀ can then be evaluated as the logarithm map $\operatorname{Log}_{\mathbf{x}_{I}} (\mathbf{a}_{I})$. When sampled points are available along the geodesic segment from x _I to a _I , this logarithm map can easily be approximated using the finite difference of the velocity at x _I .

Fig. 17

An illustration of Schild’s Ladder for approximating the parallel transport

As we use the standard projection method for computing the geodesic which we transport along, we form approximate parallelograms between the discrete points on the geodesic. As with the exponential maps we do not seem to experience numerical problems caused by curvature of the manifold, most likely due to the short geodesics that occur as part of tracking. For more difficult parallel transportation problems, we believe that more sophisticated numerical schemes are needed.

Appendix C: Proof of Proposition 1

We need to show that $\frac{DM_{t}}{dt} = 0$. It will be sufficient to show that

$$\biggl(\frac{DM_t}{dt} \biggr) \bigl(v_m(t),v_{m'}(t) \bigr) = 0,\quad m, m' = 1,\dots, M $$

for the vectors v _m (t),m=1,…,M defined in the proposition. Indeed, for each t, they form an orthonormal basis of $T_{\alpha(t)}\mathcal{M}$:

(70)

because the v _m (t)s are parallel along α, and therefore their covariant derivative vanish. By definition of the covariant derivative for a tensor,

(71)

The last two terms vanish, again because the v _m (t)s are parallel. On the other hand, a simple calculation gives

$$ M_t \bigl(v_m(t),v_{m'}(t) \bigr) = \lambda_m\delta_{mm'} $$

(72)

(δ _mm′ is the usual Kronecker symbol) and this quantity is thus independent of t. This concludes the proof.  □