Question about "A mciro lie theory..." and "Quaternion kinematics for the ESKF"

[knightshrub]

This doesn't directly relate to the manif library, but I think it fits into here because Joan has written/contributed to both these papers and other people stumbling across this might find an answer useful.

In the ESKF paper [1] the Jacobian of the true state with respect to the error state is derived in [1, (279)] in order to perform the ESKF update using measurements. The result is trivial in all cases where the nominal and error state composition is the regular vector addition. However, the true orientation $q_t$ is defined in terms of the quaternion composition $\otimes$ of the nominal orientation $q$ and the error quaternion $\delta q = \exp \delta \theta/2 = \operatorname{Exp} (\delta\theta)$ as

$$ q_t = q \otimes \delta q $$

The Jacobian of $q_t$ w.r.t the orientation error $\delta \theta$ is derived in the ESKF paper as [1, (281)]

$$ \frac{\partial q_t}{\partial \delta\theta} = \frac{\partial (q \otimes \delta q)}{\partial \delta\theta} = \begin{bmatrix} q \end{bmatrix}L \frac{1}{2} \begin{bmatrix} \mathbf{0}{1\times3} \ \mathbf{I}_3 \end{bmatrix} \quad\quad\quad(1) $$

Given the definition of the true orientation $q_t$ above, it could be written in the language of [2] using the right $\oplus$ operator as

$$ q_t = q \otimes \delta q = q \otimes \operatorname{Exp}(\delta \theta) = q \oplus \delta \theta $$

which according to [2, (81)]

$$ \frac{\partial qt}{\partial \delta\theta} = \frac{\partial (q \oplus \delta \theta)}{\partial \delta\theta} = \mathbf{J}^{q \oplus \delta \theta}{\delta \theta} = \mathbf{J}_{r}(\delta \theta) \quad\quad\quad(2) $$

i.e. the right Jacobian evaluated at the orientation error $\delta \theta$, whose expression is given in [2, (143)] and looks a lot different from [1, (281)].

Why are these expressions different? Am I missing something here (maybe the vector spaces being mapped aren't the same)?

[1] Solà, J., “Quaternion kinematics for the error-state Kalman filter”, arXiv e-prints, 2017. doi:10.48550/arXiv.1711.02508.

[2] Solà, J., Deray, J., and Atchuthan, D., “A micro Lie theory for state estimation in robotics”, arXiv e-prints, 2018. doi:10.48550/arXiv.1812.01537.

[joansola]

Hello. Yes, these two Jacobians are different.

In  [1], equation (1) above, we differentiate the 4-vector of the quaternion wrt the angular error. The result is a 4x3 matrix. This is regular differentiation in vector spaces.

In [2], equation (2) above, we use the differentiation definition given by Lie theory. It relates infinitesimal variations in the tangent space of the free variable (in this case, the angular error, which is itself a regular vector in this case), with infinitesimal variations in the tangent space of the output, in this case the quaternion manifold. These latter variations have 3 DOF, and therefore the resulting Jacobian matrix is 3x3.

But there is another difference:

In (1), the evaluation of the derivative is at q_t = q, therefore considering the angular error zero, d_theta=0.

In (2), the evaluation of the derivative is at q_t = q (+) d_theta = q * Exp(d_theta) with d_theta not necessarily zero. If d_theta = 0, then J_r(d_theta) is the identity matrix.

The fact that both Jacobians share the same notation, by the same author, is probably unfortunate, and it is my fault. However, I am a strong defender of explicit notations that favor the chain rule, and the notations I use are such that this rule is clearly visible. Having said this, I think in [2] I use D q_t / D d_theta and not d q_t / d d_theta precisely to break this ambiguity. Never take notation lightly.

[knightshrub]

Hello Joan, sorry for the late reply and thank you for the detailed answer!

Looking at it a second time, it should've been immediately clear to me from the 4x3 dimensions of the Jacobian that were mapping from the tangent space directly to the vector representation of a quaternion (the manifold element).

Also, thanks for clarifying my doubts about the right Jacobian, together with the reminder that the Jacobians as defined in [2] always map between tangent spaces (not from tangent space to manifold as above) I see now where my confusion came from.

In fact example A in section V that shows this quite nicely where the Jacobian of the measurement model is given as

$$ \mathbf{H} = \mathbf{J}^{\mathcal{X}^{-1} \cdot \mathbf{b}k}{\mathcal{X}} = \mathbf{J}^{\mathcal{X}^{-1} \cdot \mathbf{b}k}{\mathcal{X}^{-1}} \mathbf{J}^{\mathcal{X}^{-1}}_{\mathcal{X}} $$

which in plain english would be

"How does a small change $\delta \boldsymbol{\theta}$ in the tangent space of $\mathcal{X}$ affect the tangent space of $\mathcal{X}^{-1} \cdot \mathbf{b}_k$?"

such that a linear approximation for $\mathcal{X}^{-1} \cdot \mathbf{b}_k$ can be written as

$$ \left(\mathcal{X} \oplus \delta \boldsymbol{\theta}\right)^{-1} \cdot \mathbf{b}_k \approx \mathcal{X}^{-1} \cdot \mathbf{b}_k + \mathbf{H} \delta \boldsymbol{\theta} $$

where regular $+$ is used for composition (both manifold and tangent space are $\in \mathbb{R}^3$)

Now the usefulness of manifpy becomes apparent

import numpy as np
from manifpy import SO3, SO3Tangent

X = SO3.Random()
bk = np.array([1,0,0])
dtheta = np.array([0,0,0.05])

J_Xi_act_b_Xi = np.zeros((3,3))
J_Xi_X = np.zeros((3,3))

# use manif's automatic Jacobian feature for individual operation's Jacobians
b_= X.inverse(J_Xi_X).act(bk, J_Xi_act_b_Xi)
# use the chain rule to compute the Jacobian of the chained operations
J_Xi_act_b_X = J_Xi_act_b_Xi @ J_Xi_X

# linear approximation
b_appr = b_ + J_Xi_act_b_X @ dtheta
# full non-linear expression
b_full = X.rplus(SO3Tangent(dtheta)).inverse().act(bk)

delta = b_appr - b_full

# should be small
print(delta)