Clarification needed. Left box plus used in Right Invariance filter.

[slovak194]

Dear, @joansola and @artivis

I'm looking into the implementation of the UKF on manifold from this repo.
I've just noticed, that in the case of the Right Invariance sigmas and increments are added to the current state using the left box plus operator. In which frame P (covariance) is expressed then? Local tangent space at X, or global at the group origin?
I expected that with Right Invariance increments and covariance are expressed in the local tangent space at X and thus right box plus operator shall be used.

kalmanif/include/kalmanif/impl/unscented_kalman_filter_manifolds.h

Lines 155 to 161 in 5e0f1f2

	// sigma points on manifold
	State s_j_p, s_j_m;
	Eigen::Matrix<Scalar, StateSize, StateSize*2> xis_new;
	for (int i = 0; i < StateSize; ++i) {
	if constexpr (Iv == Invariance::Right) {
	s_j_p = MapTangent(xis.col(i).data()) + x;
	s_j_m = Tangent(-xis.col(i)) + x;

At the same time, in the example from manif repo, the description matches the operator used to add sigma increments to the state.

*  The estimation error dx and its covariance P are expressed
*  in the tangent space at X.

https://github.com/artivis/manif/blob/805a0b2adf9435dd658fdad5606c342db4d889e8/examples/se2_localization_ukfm.cpp#L311

        // sigma points on manifold
        for (int i = 0; i < DoF; ++i)
        {
          s_j_p = X + manif::SE2Tangentd( xis.col(i));
          s_j_m = X + manif::SE2Tangentd(-xis.col(i));

          xis_new.col(i) = (X_new.lminus(s_j_p + u_est)).coeffs();
          xis_new.col(i + DoF) = (X_new.lminus(s_j_m + u_est)).coeffs();
        }

Thank you in advance for your answer!

Best regards,
Alex

[artivis]

@slovak194,

First of all I have to point that the UKFM examples in manif are wrong and listed on my todo list for a fix. You should therefore refer the kalmanif implementation as the most up to date.

To answer your question clearly, the covariance P is expressed in the global frame at the group origin.
Note that the README states: Both the IEKF and UKFM filters are implemented in their 'right invariant' flavor. However they are able to handle both 'right' and 'left' measurements.
The conditional invariance you've linked to is only related to the measurement model used.

This being said, this topic isn't trivial and a little far in my memories. That's why I have just added some old and partial notes on the invariance topic. They are based on [1] and other publications from the same authors/group.

Mind that the right/left invariance definition comes from this line of work and may be a little confusing with respect to our definition of right/left box-plus. Indeed they are somewhat inversed in that the right invariance is naturally associated with the left box-minus and the left invariance goes in pair with the right box-minus. This is all detailed in eqs (7-10) in the notes.

Let me know if those notes are any useful and if you have any further question.

[1] “The Invariant Extended Kalman Filter as a Stable Observer” A. Barrau and S. Bonnabel.