ESKF error-state reset in the context of the Lie theory

[knightshrub]

Hello again,

while dotting the i's and crossing the t's for a paper I'm writing I've come across another question regarding the ideas presented in [1] and how to properly express them using the notation introduced in the manif paper [2].

In [1] section 6.3 the ESKF reset is explained and the only non-trivial Jacobian that results is [1, (294)]

$$ \frac{\partial\delta \theta^{+}}{\partial\delta{\theta}} \Big\vert_{\hat{\delta \theta}} = \mathbf{I} - \frac{1}{2} \left[\hat{\delta \theta}\right]_{\times}. $$

As far as I understand [1] tracks the state quaternion as a vector with 4 entries and the orientation error as a vector with 3 entries. From this I would assume the at least the tangent space representations are identical to using the Lie theory as presented in [2].

Following the notation of [1], we have the following equations

1. The true orientation doesn't change on error reset [1, (289)]

$$ q_t^+ = q^{+} \oplus \delta \theta^{+} = q \oplus \delta \theta = q_t $$

2. The observed error mean has been injected into the nominal state [1, (290)]

$$ q^{+} = q \oplus \hat{\delta \theta} $$

Rearranging the two equations yields the expression

$$ \delta \theta^{+} = (q \oplus \delta \theta) \ominus (q \oplus \hat{\delta \theta}) = \mathrm{Exp}(\delta\theta) \ominus \mathrm{Exp}(\hat{\delta\theta}) $$

which when evaluated at $\delta \theta = \hat{\delta \theta}$ gives $\delta \theta^+ = 0$ as expected.
Using the right-Jacobian and right-plus/minus Jacobians for $S^3$ from [2] this gives (the parser somehow breaks when adding the r subscript)

$$ \frac{\partial \delta \theta^+}{\partial \delta \theta} \Big\vert_{\hat{\delta \theta}} = \mathbf{J}^{-1} \left(\delta \theta^+\right) \mathbf{J}\left(\hat{\delta \theta}\right) = \mathbf{J}(\hat{\delta \theta}) = \mathbf{I} - \frac{1 - \cos \hat{\delta \theta}}{\hat{\delta \theta}^2} \left[ \hat{\delta \theta}\right]_{\times} + \dots $$

Is this correct? Am I missing something? Why does this look different from what is presented in [1]?

[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]

Hi Elia,

Although I never did it, I always thought that redeveloping the methods in [1] using lie theory would result in some version of the left or right Jacobian, but only in the first-order approximation which is what ESKF is based upon.

Your development in the last line I think is correct. So you have Jr(dtheta.hat) as the result of this Jacobian.

If you now express cos and sin with their Taylor expressions, and you keep just the first approximation, I am almost sure you get the expression in [1]. Would you do this exercise and report back here?

[knightshrub]

Hello Joan, thanks for the quick reply.

Indeed using the approximation for the cosine around 0 $\cos x \approx 1 - \frac{x^2}{2}$ and substituting into the Jacobian above gives

$$ \mathbf{I} - \frac{1 - \cos \hat{\delta\theta}}{\hat{\delta \theta}^2} \left[\hat{\delta\theta}\right]_{\times} $$

$$ \approx \mathbf{I} - \frac{1 - (1 - \frac{\hat{\delta \theta}^{2}}{2})}{\hat{\delta \theta}^2} \left[\hat{\delta\theta}\right]_{\times} $$

$$ = \mathbf{I} - \frac{1}{2} \left[\hat{\delta\theta}\right]_{\times} $$

[joansola]

Bravo, then!

Thank for the exercise!