Second-Order Recursive Filtering on the Rigid-Motion Lie Group \({\text {SE}}_{3}\) Based on Nonlinear Observations

Camera motion estimation from observed scene features is an important task in image processing to increase the accuracy of many methods, e.g., optical flow and structure-from-motion. Due to the curved geometry of the state space \({\text {SE}}_{3}\) and the nonlinear relation to the observed optical flow, many recent filtering approaches use a first-order approximation and assume a Gaussian a posteriori distribution or restrict the state to Euclidean geometry. The physical model is usually also limited to uniform motions. We propose a second-order optimal minimum energy filter that copes with the full geometry of \({\text {SE}}_{3}\) as well as with the nonlinear dependencies between the state space and observations., which results in a recursive description of the optimal state and the corresponding second-order operator. The derived filter enables reconstructing motions correctly for synthetic and real scenes, e.g., from the KITTI benchmark. Our experiments confirm that the derived minimum energy filter with higher-order state differential equation copes with higher-order kinematics and is also able to minimize model noise. We also show that the proposed filter is superior to state-of-the-art extended Kalman filters on Lie groups in the case of linear observations and that our method reaches the accuracy of modern visual odometry methods.

This work was supported by the DFG (German Research Foundation), Grant GRK 1653.

Authors and Affiliations

1. Research Training Group 1653, Image and Pattern Analysis Group, Heidelberg University (HCI), Im Neuenheimer Feld 205, 69120, Heidelberg, Germany

   Johannes Berger

2. Heidelberg Collaboratory for Image Processing, Heidelberg University (HCI), Im Neuenheimer Feld 205, 69120, Heidelberg, Germany

   Frank Lenzen & Florian Becker

3. Image and Pattern Analysis Group, Heidelberg University (HCI), Im Neuenheimer Feld 205, 69120 , Heidelberg, Germany

   Andreas Neufeld & Christoph Schnörr

Corresponding author

Correspondence to Johannes Berger.

Appendix 1: Properties of \({\text {SE}}_{3}\) and \(\mathcal {G}\)

1.1 Projection onto \(\mathfrak {se}_{3}\)

The projection \({\text {Pr}}: \mathbb {R}^{4\times 4} \rightarrow \mathfrak {se}_{3}\) is given by

1.2 Adjoints, Exponential and Logarithmic Map

The adjoint operator \({\text {ad}}_{\mathfrak {se}} (\left[ v\right] _{\mathfrak {se}}^{\wedge })\) can be computed for a vector \(v\in \mathbb {R}^{6}\) as follows

where \(\left[ v_{1:3}\right] _{\mathfrak {so}}^{\wedge }:=(\left[ v\right] _{\mathfrak {se}}^{\wedge })_{1:3,1:3}\).

Since \(\mathbb {R}^{6}\) is trivial, the adjoint representation on \(\mathfrak {g}\) parametrized by a vector \(v \in \mathbb {R}^{12}\) is

The exponential map \({\text {Exp}}_{{\text {SE}}_{3}}: \mathfrak {se}_{3}\rightarrow {\text {SE}}_{3}\) and the logarithmic map on \({\text {SE}}_{3}\) can be computed by the matrix exponential and matrix logarithm or more efficiently by the Rodrigues’ formula as in [39, p. 413f ].

Then, the exponential map \({\text {Exp}}_{\mathcal {G}}:\mathfrak {se}_{3}\rightarrow {\text {SE}}_{3}\) for a tangent vector \(\eta = (\eta _{1}, \eta _{2})\in \mathfrak {g}\) and the logarithmic map \({\text {Log}}_{G}:{\text {SE}}_{3}\rightarrow \mathfrak {se}_{3}\) for \(x =(E,v) \in \mathcal {G}\) are simply

and similar for higher-order state spaces.

1.3 Vectorization of Connection Function

Following [1, Section  5.2], we can vectorize the connection function \(\omega \) of the Levi-Civita connection \(\nabla \) for constant \(\eta ,\) \(\xi \in \mathfrak {g}\) in the following way:

where \({\varGamma (\gamma )}\) is the matrix whose (i, j) element is the real-valued function

and \(\varGamma _{jk}^{i}\) are the Christoffel symbols of the connection function \(\omega \) for a vector \(\gamma \in \mathbb {R}^{12}.\) Similarly, permuting indices, we can define the adjoint matrix \({\varGamma ^{*}(\gamma )}\), whose (i, j)-th element is given by

This leads to the following equality:

If the expression \(\xi \) in (85) is non-constant, we will obtain the following vectorization from [1, Eq. (5.7)] for the case of the Lie algebra \(\mathfrak {se}_{3}\), i.e.,

where the entries of the matrix \(D \in \mathbb {R}^{6\times 6}\) can be computed as

where \(e_{j}^{6}\) denotes the j-th unit vector in \(\mathbb {R}^{6}\).

Appendix 2: Proofs

Proof (of Proposition 1)

The tangent map is simply the differential or directional derivative. For \(x_{1}=(E_{1},v_{1}) ,x_{2}=(E_{2},v_{2})\in \mathcal {G}\) it holds \(T_{x_{2}}L_{x_{1}}:T_{x_{2}}\mathcal {G}\rightarrow T_{L_{x_{1}}(x_{2})}\mathcal {G}.\) Thus, we can compute it for a \(\eta = (E_{2}\eta _{1},\eta _{2})\in T_{x_{2}}\mathcal {G}= T_{E_{2}}{\text {SE}}_{3}\times \mathbb {R}^{6}\) as follows

For \(x_{2}={\text {Id}}= (\mathbbm {1}_{4}, \mathbf {0}_{6})\) and \(\eta = (\eta _{1}, \eta _{2}) \in \mathfrak {g}\), we gain

Note that the adjoint of the tangent map of \(L_{x}\) at identity can be expressed as inverse of \(x=(E,v)\), i.e., for \(\eta =(\eta _{1},\eta _{2}) \in T_{x}\mathcal {G}\) and \(\xi =(\xi _{1}, \xi _{2}) \in \mathfrak {g}\)

Thus, \(T_{{\text {Id}}}L_{x}^{*} \eta = L_{(E^{-1}, \mathbf {0}_{6})}\eta \). We will use the shorthand \(x^{-1}\eta := T_{{\text {Id}}}L_{x}^{*}\eta \) for the dual of the tangent map of \(L_{x}\) at identity. \(\square \)

Proof (of Lemma 1)

Since \(\mu = (\mu _{1}, \mu _{2})\) and v are independent of E, the gradient \(\mathbf {d}_{1} \mathcal {H}(x=(E,v), \mu , t)\) can be computed separately in terms of E and v, i.e., for \(\eta = (E \eta _{1}, \eta _{2}) \in T_{x}\mathcal {G}\)

The directional derivative regarding v can be computed by the usual gradient on \(\mathbb {R}^{6}\), which is given by

such that \(\mathbf {d}_{v} \langle \mu _{1}, \left[ v\right] _{\mathfrak {se}}^{\wedge } \rangle = -{\left[ \mu _{1}\right] _{\mathfrak {se}}^{\vee }}\). For the directional derivative of \(\mathcal {H}\), we first consider the directional derivative of \(h_{z}(E).\) Since \(h_{z}(E)\) can also be written as

with \(\quad \hat{I}:=\left( {\begin{matrix}1 &{} 0 &{} 0 &{} 0 \\ 0 &{} 1 &{} 0 &{} 0 \end{matrix}}\right) \) and \(\mathbf {e}:= \left( {\begin{matrix}0&0&1&0\end{matrix}}\right) \), the directional derivative (into direction \(\xi \)) can be derived by the following matrix calculus:

where \(\kappa _{z} = \kappa _{z}(E):= {\mathbf {e}}E^{-1} g_{z}.\) Then for the choice \(\xi = E\eta _{1}\) we find that

Here we used the property that the trace is cyclic. We obtain the Riemannian gradient on \({\text {SE}}_{3}\) by projecting (cf. [1, Section  3.6.1]) the left-hand side of the Riemannian metric in (96) onto \(T_{E}{\text {SE}}_{3}\), which is for \(x=(E,v)\)

with \(\hat{G}_{z}(E):=\hat{G}_{z}(E,y):= \bigl (\kappa _{z}^{-1}\hat{I} - \kappa _{z}^{-2} \hat{I} E^{-1} g_{z}{\mathbf {e}} \bigr )^{\top } Q(y_{z} - h_{z}(E)) g_{z}^{\top } E^{-\top }\), and \({\text {Pr}}_{E}:{\text {GL}}_{4} \rightarrow T_{E}{\text {SE}}_{3}\) denotes the projection onto the tangential space \(T_{E}{\text {SE}}_{3}\) that can be expressed in terms of \({\text {Pr}}_{E}(E \cdot ) = E {\text {Pr}}(\cdot )\). Besides, \({\text {Pr}}:{\text {GL}}_{4} \rightarrow \mathfrak {se}_{3}\) denotes the projection onto the Lie algebra \(\mathfrak {se}_{3}\) as given in (80).

Putting together (91) and (97) results in

\(\square \)

Proof (of Lemma 2)

Eq. (52) can be easily found by considering a basis of \(\mathfrak {se}_{3}\) and by the fact that Z is a linear operator on the Lie algebra. Since the resulting matrix \(K(t) {\left[ \eta \right] _{\mathfrak {g}}^{\vee }} := Z(x)\circ \eta \) depends only on t, equation (53). Eq.(54) is trivial since Z is linear.

1. 1.

   With the symmetry of the Levi-Civita connection, i.e.,

   $$\begin{aligned}{}[\eta ,\xi ] = \nabla _{\eta }\xi - \nabla _{\xi }\eta , \end{aligned}$$

   (99)

   we gain the following equalities

   $$\begin{aligned}&{\left[ Z(x)[ \omega _{x^{-1} \dot{x}}\eta ] + Z(x)\bigl [\omega _{\mathbf {d}_{2}\mathcal {H}(x,\mathbf {0},t)}^{\leftrightharpoons }\eta \bigr ]\right] _{\mathfrak {g}}^{\vee }} \nonumber \\ \mathop {=}\limits ^{(52)}&K(t) {\left[ \omega _{x^{-1} \dot{x}}\eta + \omega _{\mathbf {d}_{2}\mathcal {H}(x,\mathbf {0},t)}^{\leftrightharpoons }\eta \right] _{\mathfrak {g}}^{\vee }} \nonumber \\ \mathop {=}\limits ^{(49)}&K(t)\Bigl ( {\left[ \nabla _{ -\mathbf {d}_{2} \mathcal {H}(x,\mathbf {0},t)}\eta \right] _{\mathfrak {g}}^{\vee }} \nonumber \\&- {\left[ \nabla _{e^{-\alpha (t-t_{0})} Z(x)^{-1}[G(x)]}\eta + \nabla _{\eta }\mathbf {d}_{2}\mathcal {H}(x,\mathbf {0},t)\right] _{\mathfrak {g}}^{\vee }} \Bigr )\nonumber \\ \mathop {=}\limits ^{(99)}&K(t) \Bigl ({\left[ -[\mathbf {d}_{2}\mathcal {H}(x,\mathbf {0},t),\eta ]\right] _{\mathfrak {g}}^{\vee }} \nonumber \\&- {\left[ \nabla _{e^{-\alpha (t-t_{0})} Z(x)^{-1} [G(x)]}\eta \right] _{\mathfrak {g}}^{\vee }} \Bigr ) \nonumber \\ \mathop {=}\limits ^{(88)}&K(t) \Bigl ( {\left[ [f(x),\eta ]\right] _{\mathfrak {g}}^{\vee }} \nonumber \\&- \varGamma ^{*}\bigl ({\left[ e^{-\alpha (t-t_{0})} Z(x)^{-1}[G(x)]\right] _{\mathfrak {g}}^{\vee }}\bigr ){\left[ \eta \right] _{\mathfrak {g}}^{\vee }} \Bigr ) \nonumber \\ \mathop {=}\limits ^{(52)}&K(t) \Bigl ( {\left[ (f(x),\eta )\right] _{\mathfrak {g}}^{\vee }} \nonumber \\&+ \varGamma ^{*}\bigl (-e^{-\alpha (t-t_{0})} K(t)^{-1}{\left[ G(x)\right] _{\mathfrak {g}}^{\vee }}\bigr ) {\left[ \eta \right] _{\mathfrak {g}}^{\vee }}\Bigr ) \nonumber \\ \mathop {=}\limits ^{(82)}&K(t) \Bigl ( {\text {ad}}_{\mathfrak {g}}^{\vee } (f(x)) \nonumber \\&+ \varGamma ^{*}\bigl (-e^{-\alpha (t-t_{0})} K(t)^{-1}{\left[ G(x)\right] _{\mathfrak {g}}^{\vee }}\bigr ) \Bigr ){\left[ \eta \right] _{\mathfrak {g}}^{\vee }} \nonumber \\ =:&K(t) B(x) {\left[ \eta \right] _{\mathfrak {g}}^{\vee }}. \end{aligned}$$

   (100)

   The claim follows from the fact that the adjoints and the Christoffel symbols on \(\mathbb {R}^{6}\) are zero, such that B reads as i.e.,

   $$\begin{aligned} B(x) = \left( {\begin{matrix}-C_{1,1}^{t} (x)&{} \mathbf {0}_{6 \times 6} \\ \mathbf {0}_{6 \times 6} &{} \mathbf {0}_{6 \times 6} \end{matrix}}\right) . \end{aligned}$$

   (101)

   with \(C_{1,1}^t\) from Theorem 1.

2. 2.

   Since this expression is dual to the expression in 1., the claim follows by using its transpose.

3. 3.

   Recall that the Hamiltonian in (32) is given by

   $$\begin{aligned} \mathcal {H}((E,v), \mu , t)&= \tfrac{1}{2}e^{-\alpha (t-t_{0})} \Bigl ( \sum _{z \in \varOmega } ||y_{z}-h_{z}(E)||_{Q}^{2} \Bigr ) \\&\quad - \tfrac{1}{2}e^{\alpha (t-t_{0})}\Bigg ( \left\langle \mu _{1}, \left[ S_{1}^{-1}{\left[ \mu _{1}\right] _{\mathfrak {se}}^{\vee }}\right] _{\mathfrak {se}}^{\wedge } \right\rangle _{{\text {Id}}} \\&\quad + \big \langle \mu _{2}, S_{2}^{-1} \mu _{2} \big \rangle \Bigr ) - \big \langle \mu _{1}, \left[ v\right] _{\mathfrak {se}}^{\wedge }\big \rangle _{{\text {Id}}} . \end{aligned}$$

   The Riemannian Hessian w.r.t. the first component can be computed for \(x=(E,v) \in \mathcal {G}\), \(\eta =(\eta _{1},\eta _{2}) \in \mathfrak {g}\) and the choice \(\mu = (\mu _{1},\mu _{2})=(\mathbf {0}_{4\times 4}, \mathbf {0}_{6})\) as

   $$\begin{aligned}&e^{\alpha (t-t_{0})}{\left[ x^{-1} {\text {Hess}}_{1} \mathcal {H}(x,\mu ,t)[x\eta ]\right] _{\mathfrak {g}}^{\vee }} \nonumber \\&=e^{\alpha (t-t_{0})} {\left[ x^{-1} \nabla _{x\eta } \mathbf {d}_{1} \mathcal {H}(x,\mathbf {0},t)\right] _{\mathfrak {g}}^{\vee }} \end{aligned}$$

   (102)
   $$\begin{aligned}&= e^{\alpha (t-t_{0})} {\left[ \nabla _{\eta } x^{-1} \mathbf {d}_{1} \mathcal {H}(x,\mathbf {0},t)\right] _{\mathfrak {g}}^{\vee }} \end{aligned}$$

   (103)
   $$\begin{aligned}&= {\left[ \nabla _{\eta } \bigl (\sum _{z \in \varOmega }{\text {Pr}}(\hat{G}_{z}(E)), -e^{\alpha (t-t_{0})} {\left[ \mathbf {0}_{4\times 4}\right] _{\mathfrak {se}}^{\vee }} \bigr )\right] _{\mathfrak {g}}^{\vee }} \end{aligned}$$

   (104)
   $$\begin{aligned}&= \Bigl (\sum _{z \in \varOmega } {\left[ \nabla _{\eta _{1}} {\text {Pr}}\bigl (\hat{G}_{z}(E)\bigr )\right] _{\mathfrak {se}}^{\vee }}, \mathbf {0}_{6}\Bigr ) \nonumber \\&= \sum _{z \in \varOmega }\Bigl ( \varGamma \left( {\left[ {\text {Pr}}(\hat{G}_{z}(E))\right] _{\mathfrak {g}}^{\vee }}\right) {\left[ \eta _{1}\right] _{\mathfrak {se}}^{\vee }} \nonumber \\&\qquad + \sum _{i} ({\left[ \eta _{1}\right] _{\mathfrak {se}}^{\vee }})_{i} {\left[ \mathbf {d}{Pr}\bigl (\hat{G}_{z}(E)\bigr ))[E^{i}]\right] _{\mathfrak {se}}^{\vee }} , \mathbf {0}_{6} \Bigr ). \end{aligned}$$

   (105)

   Here, line (102) follows from the general definition of the Hessian (cf. [1, Def. 5.5.1]). Line (103) is valid because of the linearity of the affine connection, equation (104) results from insertion of the expression in Lemma 1 and (105) can be achieved with (89).

   Next, we calculate the differential \( \mathbf {d}{Pr}(\hat{G}_{z}(E)) [\eta _{1}]\) in (105) for an arbitrary direction \(\eta _{1}\). Since the projection is a linear operation (cf. (80)), i.e., \(\mathbf {d}{Pr}(\hat{G}_{z}(E))[\eta _{1}] = {\text {Pr}}(\mathbf {d}\hat{G}_{z}(E)[\eta _{1}])\), we require calculating \(\mathbf {d}\hat{G}_{z}(E)[\eta _{1}]\). By using the product rule and the definition of \(\hat{G}_{z}\) from (37), we obtain

   $$\begin{aligned}&\mathbf {d}\hat{G}_{z}(E) [\eta _{1}] \nonumber \\&\quad = \mathbf {d}\bigl (\bigl (\kappa _{z}^{-1}\hat{I} - \kappa _{z}^{-2} \hat{I} E^{-1} g_{z}{\mathbf {e}} \bigr )^{\top }Q(y_{z} - h_{z}(E)) g_{z}^{\top } E^{-\top } \bigr )[\eta _{1}] \nonumber \\&\quad = \bigl (\mathbf {d}\bigl (\kappa _{z}^{-1}\hat{I} - \kappa _{z}^{-2} \hat{I} E^{-1} g_{z}{\mathbf {e}} \bigr )^{\top }[\eta _{1}]Q(y_{z} - h_{z}(E)) g_{z}^{\top } E^{-\top } \bigr ) \nonumber \\&\qquad + \bigl (\kappa _{z}^{-1}\hat{I} - \kappa _{z}^{-2} \hat{I} E^{-1} g_{z}{\mathbf {e}} \bigr )^{\top }Q \Bigl (\Bigl ( - \mathbf {d}h_{z}(E)[\eta _{1}]\Bigr ) g_{z}^{\top } E^{-\top } \Bigr ) \nonumber \\&\qquad + \bigl ((y_{z} - h_{z}(E)) g_{z}^{\top } \mathbf {d}E^{-\top }[\eta _{1}] \bigr ) \Bigr ) . \end{aligned}$$

   (106)

   The directional derivative of \(\bigl (\kappa _{z}^{-1}\hat{I} - \kappa _{z}^{-2} \hat{I} E^{-1} g_{z}{\mathbf {e}} \bigr )\) is

   $$\begin{aligned}&\mathbf {d}\bigl (\kappa _{z}^{-1}\hat{I} - \kappa _{z}^{-2} \hat{I} E^{-1} g_{z}{\mathbf {e}} \bigr )[\eta _{1}] \nonumber \\&\quad = -\kappa _{z}^{-2} {\mathbf {e}} \mathbf {d}E^{-1}[\eta _{1}] g_{z}\hat{I} + 2 \kappa _{z}^{-3}{\mathbf {e}} \mathbf {d}E^{-1}[\eta _{1}]g_{z} \hat{I}E^{-1}g_{z}{\mathbf {e}} \nonumber \\&\qquad - \kappa _{z}^{-2} \hat{I} \mathbf {d}E^{-1} [\eta _{1}] g_{z}{\mathbf {e}} \nonumber \\&\quad = \kappa _{z}^{-2} {\mathbf {e}} E^{-1}\eta E^{-1} g_{z} \hat{I} - 2\kappa _{z}^{-3}{\mathbf {e}} E^{-1}\eta _{1} E^{-1} g_{z} \hat{I}E^{-1}g_{z}{\mathbf {e}}\nonumber \\&\qquad + \kappa _{z}^{-2} \hat{I}E^{-1} \eta _{1} E^{-1} g_{z}{\mathbf {e}} . \end{aligned}$$

   (107)

   By inserting the directional derivatives (107), (94) and \(\mathbf {d}E^{-\top }[\eta _{1}] = -(E^{-1}\eta _{1}E^{-1})^{\top }\) into (106), we obtain the vector-valued function \(\zeta ^{k}(E)(\cdot ): \mathfrak {se}_{3}\rightarrow \mathbb {R}^{6}\) defined as

   $$\begin{aligned}&\left[ \zeta ^{k}(E)(\eta _{1})\right] _{\mathfrak {se}}^{\wedge } := {\text {Pr}}\bigl (\mathbf {d}\hat{G}_{z}(E)[\eta _{1}] \bigr ) \nonumber \\&\quad = {\text {Pr}} \biggl (\Bigl (\kappa _{z}^{-2} {\mathbf {e}} E^{-1}\eta _{1} E^{-1} g_{z} \hat{I} - 2\kappa _{z}^{-3}{\mathbf {e}} E^{-1}\eta _{1} E^{-1} g_{z} \hat{I}E^{-1}g_{z}{\mathbf {e}} \nonumber \\&\qquad + \kappa _{z}^{-2} \hat{I}E^{-1} \eta _{1} E^{-1} g_{z}{\mathbf {e}}\Bigr )^{\top }Q\bigl (y_{z} - h_{z}(E)\bigr ) g_{z}^{\top } E^{-\top } \nonumber \\&\qquad + \bigl (\kappa _{z}^{-1}\hat{I} - \kappa _{z}^{-2} \hat{I} E^{-1} g_{z}{\mathbf {e}} \bigr )^{\top }Q \Bigl (\bigl (\kappa _{z}^{-1} \hat{I}E^{-1} \eta _{1} E^{-1} g_{z} \nonumber \\&\qquad - \kappa _{z}^{-2}{\mathbf {e}}E^{-1}\eta _{1} E^{-1}g_{z}\hat{I} E^{-1} g_{z} \bigr ) g_{z}^{\top }E^{-\top } \nonumber \\&\qquad - \bigl (y_{z}-h_{z}(E)\bigr )g_{z}^{\top } E^{-\top } \eta _{1}^{\top } E^{-\top } \Bigr )\biggr ) . \end{aligned}$$

   (108)

   Using the basis \(\{E^{j}\}_{j=1}^{6}\) of \(\mathfrak {se}_{3}\), with \(E^{j}:= \left[ e_{j}^{6}\right] _{\mathfrak {se}}^{\wedge }\) we define, as in (90), the following matrix \(D_{z}(E) \in \mathbb {R}^{6\times 6}\) with components

   $$\begin{aligned} (D_{z}(E))_{i,j}:= \zeta _{i}^{k}(E^{j}). \end{aligned}$$

   (109)

   By using Eq. (89), we find that

   $$\begin{aligned} {\left[ \nabla _{\eta _{1}} {\text {Pr}}(\hat{G}_{z}(E))\right] _{\mathfrak {se}}^{\vee }} = \bigl (\varGamma ({\text {Pr}}(\hat{G}_{z}(E))) + D_{z}(E)\bigr ) {\left[ \eta _{1}\right] _{\mathfrak {se}}^{\vee }}. \end{aligned}$$

   Insertion of this expression into (105) finally leads to the desired result, i.e.,

   $$\begin{aligned}&e^{\alpha (t-t_{0})}{\left[ x^{-1} {\text {Hess}}_{1} \mathcal {H}(x,\mu ,t)[x\eta ]\right] _{\mathfrak {g}}^{\vee }} \nonumber \\&\quad = \begin{pmatrix} \sum _{z \in \varOmega } \bigl ( \varGamma \bigl ({\left[ {\text {Pr}}\bigl (\hat{G}_{z}(E)\bigr )\right] _{\mathfrak {se}}^{\vee }}\bigr ) + D_{z}(E)\bigr ) &{} \mathbf {0}_{6\times 6} \\ \mathbf {0}_{6\times 6} &{} \mathbf {0}_{6\times 6} \end{pmatrix} {\left[ \eta \right] _{\mathfrak {g}}^{\vee }} . \end{aligned}$$
4. 4.

   The Riemannian gradient of the Hamiltonian regarding the second component is zero, thus we obtain

   $$\begin{aligned} \mathbf {d}_{2} \mathcal {H} (x,\mathbf {0}, t) = \bigl ( - \left[ v\right] _{\mathfrak {se}}^{\wedge } , \mathbf {0} \bigr ) = - f(x). \end{aligned}$$

   (110)

   Computation of the differential regarding the first component at \(\eta = (E\eta _{1},\eta _{2}) \in T_{x}\mathcal {G}\) results in

   $$\begin{aligned} \mathbf {d}_{1}(\mathbf {d}_{2} \mathcal {H}(x,\mathbf {0},t))[\eta ] =&-\mathbf {d}f(x)[\eta ] \\ =&-\mathbf {d}_{(E,v)} (\left[ v\right] _{\mathfrak {se}}^{\wedge }, \mathbf {0})[\eta ] \\ =&-(\left[ \eta _{2}\right] _{\mathfrak {se}}^{\wedge } , \mathbf {0}). \end{aligned}$$

   Finally, we compute the complete expression, which is for \(\eta =(\eta _{1},\eta _{2}) \in \mathfrak {g}\) and \(x=(E,v) \in \mathcal {G}\)

   $$\begin{aligned}&{\left[ Z(x) \bigl [\mathbf {d}_{1} (\mathbf {d}_{2} \mathcal {H}(x, \mathbf {0},t))[x\eta ] \bigr ]\right] _{\mathfrak {g}}^{\vee }} \nonumber \\&\quad = K {\left[ \mathbf {d}_{1} (\mathbf {d}_{2} \mathcal {H}(x,\mathbf {0},t)) [E\eta _{1},\eta _{2}]\right] _{\mathfrak {g}}^{\vee }} \end{aligned}$$

   (111)
   $$\begin{aligned}&\quad = -K {\left[ (\left[ \eta _{2}\right] _{\mathfrak {se}}^{\wedge } , \mathbf {0})\right] _{\mathfrak {g}}^{\vee }} \end{aligned}$$

   (112)
   $$\begin{aligned}&\quad = -K \begin{pmatrix} \mathbf {0}_{6\times 6} &{} \mathbbm {1}_{6} \\ \mathbf {0}_{6\times 6} &{} \mathbf {0}_{6\times 6} \end{pmatrix} {\left[ \eta \right] _{\mathfrak {g}}^{\vee }}. \end{aligned}$$

   (113)
5. 5.

   The following duality is vaild

   $$\begin{aligned} \begin{aligned} \mathbf {d}_{2}(\mathbf {d}_{1} \mathcal {H}(x,\mathbf {0},t)) =&(\mathbf {d}_{1}(\mathbf {d}_{2} \mathcal {H}(x,\mathbf {0},t)))^{*} \\ =&- (\mathbf {d}_{x}f(x))^{*}, \end{aligned} \end{aligned}$$

   (114)

   as well as the following duality rule for linear operators \(f,g: \mathfrak {g}\rightarrow \mathfrak {g}^{*}\) (i.e., \(f^{*},g^{*}: \mathfrak {g}\rightarrow \mathfrak {g}^{*}\) by the identification \(\mathfrak {g}^{**} = \mathfrak {g}\)) and \(\eta , \xi \in \mathfrak {g}\),

   $$\begin{aligned} \begin{aligned}&\langle (g^{*} \circ f^{*})(\eta ) ,\xi \rangle _{{\text {Id}}} = \langle f^{*}(\eta ), g(\xi ) \rangle _{{\text {Id}}} \\&\quad = \langle \eta , (f\circ g)(\xi ) \rangle _{ {\text {Id}}} = \langle (f\circ g)^{*}(\eta ), \xi \rangle _{{\text {Id}}}, \end{aligned} \end{aligned}$$

   (115)

   resulting in

   $$\begin{aligned} (g^{*} \circ f^{*}) = (f \circ g)^{*}. \end{aligned}$$

   (116)

   Note that for \(\mathfrak {g}= \mathfrak {se}_{3}\) we replace the Riemannian metric \(\langle \cdot , \cdot \rangle \) by the trace, and that the dual notation can be replaced by the transpose.

   Applying the \({\left[ \cdot \right] _{\mathfrak {g}}^{\vee }}\) operation for \(\eta \in \mathfrak {g}\) results in

   $$\begin{aligned}&{\left[ T_{{\text {Id}}} L_{x}^{*} \circ \mathbf {d}_{2}(\mathbf {d}_{1} \mathcal {H}(x,0,t)) \circ Z(x)[\eta ]\right] _{\mathfrak {g}}^{\vee }} \\&\quad \mathop {=}\limits ^{(114)} -{\left[ T_{{\text {Id}}} L_{x}^{*} \circ (\mathbf {d}f(x))^{*} \circ Z(x)[\eta ]\right] _{\mathfrak {g}}^{\vee }} \\&\quad \mathop {=}\limits ^{(116)} -{\left[ (\mathbf {d}f(x) \circ T_{{\text {Id}}} L_{x})^{*} \circ Z(x)[\eta ]\right] _{\mathfrak {g}}^{\vee }} \\&\quad \mathop {=}\limits ^{(113)} - \begin{pmatrix} \mathbf {0}_{6\times 6} &{}\quad \mathbf {0}_{6\times 6} \\ \mathbbm {1}_{6} &{}\quad \mathbf {0}_{6\times 6} \end{pmatrix} {\left[ Z(x)[\eta ]\right] _{\mathfrak {g}}^{\vee }} \\&\quad = - \begin{pmatrix} \mathbf {0}_{6\times 6} &{}\quad \mathbf {0}_{6\times 6} \\ \mathbbm {1}_{6} &{}\quad \mathbf {0}_{6\times 6} \end{pmatrix}K(t){\left[ \eta \right] _{\mathfrak {g}}^{\vee }} . \end{aligned}$$
6. 6.

   For \(\eta = (\eta _{1}, \eta _{2}) \in \mathfrak {g}\) and the definition of the Riemannian Hessian, we obtain

   $$\begin{aligned} {\text {Hess}}_{2} \mathcal {H} (x,\mu ,t) [ \eta ] = \nabla _{(\eta _{1},\eta _{2})} \mathbf {d}_{2} \mathcal {H}(x,\mu ,t) . \end{aligned}$$

   (117)

   The Riemannian gradient of the Hamiltonian regarding the second component can be computed for \(x=(E,v)\in \mathcal {G}\) as

   $$\begin{aligned}&\mathbf {d}_{2} \mathcal {H} (x,\mu , t) \nonumber \\&\quad = \left( - e^{\alpha (t-t_{0})} \left[ S_{1}^{-1}{\left[ \mu _{1}\right] _{\mathfrak {se}}^{\vee }}\right] _{\mathfrak {se}}^{\wedge } - \left[ v\right] _{\mathfrak {se}}^{\wedge }, - e^{\alpha (t-t_{0})} S_{2}^{-1} \mu _{2} \right) . \end{aligned}$$

   (118)

   Inserting (118) into (117) results in

   $$\begin{aligned}&e^{-\alpha (t-t_{0})}{\text {Hess}}_{2} \mathcal {H} (x,\mu ,t) [\eta ] \\&\quad = - \nabla _{(\eta _{1}, \eta _{2})} \left( \left[ S_{1}^{-1}{\left[ \mu _{1}\right] _{\mathfrak {se}}^{\vee }}\right] _{\mathfrak {se}}^{\wedge } + \left[ v\right] _{\mathfrak {se}}^{\wedge } , S_{2}^{-1} \mu _{2} \right) \\&\quad = - {\text {Pr}}_{\mathfrak {g}}\left( \mathbf {d}_{\mu }\left( \left[ S_{1}^{-1}{\left[ \mu _{1}\right] _{\mathfrak {se}}^{\vee }}\right] _{\mathfrak {se}}^{\wedge } + \left[ v\right] _{\mathfrak {se}}^{\wedge }\right) [\eta ],\mathbf {d}_{\mu }(S_{2}^{-1}\mu _{2})[\eta ] \right) \\&\quad = - \left( {\text {Pr}} \left( \left[ S_{1}^{-1}{\left[ \eta _{1}\right] _{\mathfrak {se}}^{\vee }}\right] _{\mathfrak {se}}^{\wedge } \right) , S_{2}^{-1}\eta _{2} \right) \\&\quad = - \left( \left[ S_{1}^{-1} {\left[ \eta _{1}\right] _{\mathfrak {se}}^{\vee }}\right] _{\mathfrak {se}}^{\wedge } , S_{2}^{-1}\eta _{2} \right) , \end{aligned}$$

   where \({\text {Pr}}_{\mathfrak {g}}: \mathbb {R}^{4\times 4} \times \mathbb {R}^{6} \rightarrow \mathfrak {g}\) denotes the projection onto the Lie algebra \(\mathfrak {g}\). Note that the second component of the projection is trivial.

   This result coincides with [46] where the Hessian of the Hamiltonian regarding the second component is computed directly. Applying the \({\left[ \cdot \right] _{\mathfrak {g}}^{\vee }}\)operation leads to

   $$\begin{aligned}&{\left[ {\text {Hess}}_{2} \mathcal {H} (x,\mu ,t) [T_{{\text {Id}}}L_{x} \eta ]\right] _{\mathfrak {g}}^{\vee }} \\&\quad = - e^{\alpha (t-t_{0})} {\left[ \left[ S_{1}^{-1} {\left[ \eta _{1}\right] _{\mathfrak {se}}^{\vee }}\right] _{\mathfrak {se}}^{\wedge } , S_{2}^{-1}\eta _{2}\right] _{\mathfrak {g}}^{\vee }} \\&\quad = -e^{\alpha (t-t_{0})} ((S_{1}^{-1} {\left[ \eta _{1}\right] _{\mathfrak {se}}^{\vee }})^{\top }, (S_{2}^{-1}\eta _{2})^{\top })^{\top } \\&\quad = -e^{\alpha (t-t_{0})} \underbrace{\begin{pmatrix} S_{1}^{-1} &{} \mathbf {0}_{6\times 6} \\ \mathbf {0}_{6 \times 6} &{} S_{2}^{-1} \end{pmatrix}}_{=:S^{-1}} {\left[ \eta \right] _{\mathfrak {g}}^{\vee }}. \end{aligned}$$

   Now we apply the \({\left[ \cdot \right] _{\mathfrak {g}}^{\vee }}\)-operation to the expression \(Z(x) \circ {\text {Hess}}_{2} \mathcal {H}(x,\mathbf {0},t) \circ Z(x)\):

   $$\begin{aligned}&{\left[ Z(x)\bigl [ {\text {Hess}}_{2} \mathcal {H}(x,\mathbf {0},t)\bigl [Z(x)[\eta ]\bigr ]\bigr ]\right] _{\mathfrak {g}}^{\vee }} \\&\quad = K(t) {\left[ {\text {Hess}}_{2} \mathcal {H}(x,\mathbf {0},t)[Z(x)[\eta ]]\right] _{\mathfrak {g}}^{\vee }} \\&\quad = -e^{\alpha (t-t_{0})} K(t)S^{-1} {\left[ Z(x)[\eta ]\right] _{\mathfrak {g}}^{\vee }} \\&\quad = -e^{\alpha (t-t_{0})} K(t)S^{-1}K(t){\left[ \eta \right] _{\mathfrak {g}}^{\vee }} . \end{aligned}$$

\(\square \)

Appendix 3: Christoffel Symbols

The Christoffel symbols \(\varGamma _{ij}^{k}, i,j,k \in \{1,\dots ,6\}\) for the Riemannian connection on \({\text {SE}}_{3}\) are given by

and zero otherwise. Note that this Christoffel symbols are similar to these of the kinematic connection in [60]. However, for the Riemannian connection, we need to switch the indexes i and j.

Appendix 4: Derivations for Extended Kalman Filter

The function \(\varPhi : \mathbb {R}^{12} \rightarrow \mathbb {R}^{12\times 12}\) in Alg. 2 is

whereas the function \(\varPhi _{{\text {SE}}_{3}}\) is given in [49, Section  10] (cf. [13, Eq. (17)]).

1.1 Derivations for Nonlinear Observations

The expression of \(H_{l}\) that is defined in [13, Eq. (59)] is simply the Riemannian gradient of the observation function \(h_{z}\), i.e.,

where \(h_k\) is defined as in (92); and the \(\mathbf {d}h_{z}\) can be computed component-wise (for \(j=1,2\)) for \(x(t_{l})=(E(t_{l}),v(t_{l}))\) by the directional derivative for a direction \(x\eta \in T_{x}\mathcal {G}.\)

where the third line follows from the definition of the Riemannian metric on \({\text {SE}}_{3}\), i.e., \(\langle \eta , \xi \rangle _{{\text {Id}}} = \eta ^{\top }\xi \), and the fact that the trace is cyclic. By projection of \(\rho _{k}^{1}(x(t_{l}))\) onto the Lie algebra \(\mathfrak {se}_{3}\) and by vectorization, we obtain the Riemannian gradient. Stacking the vectors leads to the Jacobian \(H_{l} \in \mathbb {R}^{2\times 12}\), which is provided through

Next, we consider the calculation of the function J(t) in Alg. 2 in line 3. Following [13], J(t) can be calculated as

where the differential of \(F(t) = \mathbf {d}f(x(t))\) can be computed as

For a diagonal weighting matrix S, we find that in (124) the function C can be computed for diagonal weighting matrices S as

where \(\varXi = -{\text {diag}}((S_{22}+S_{33},S_{11}+S_{33}, S_{11}+S_{22} )^{\top })\), and the adjoint in (124) can be computed with (82).

Reprints and permissions