Mixed left- and right-invariant EKF

[Chris7462]

Thank you for this excellent library and the well-written paper.

I have been exploring the library and working on a SE(3) example for a mixed left- and right-invariant EKF for localization. Here is the setup:

The robot operates in 3D space and receives control inputs as linear and angular velocities. It is equipped with a GNSS sensor, and the latitude, longitude, and altitude readings are converted into the local Cartesian coordinates. The robot's initial position in the local Cartesian coordinate system is (0,0,0).

For the ESKF prediction step, I use the linear and angular velocities to predict the robot's state. For the ESKF update step, I use the GPS measurements to update the robot's state. The prediction follows the same process as the se3_localization example, but the correction step differs.

Since the linear and angular velocities are measured in the body frame, the ESKF prediction step is performed using the right-plus operation. Conversely, since the GNSS data is measured in the fixed frame, the ESKF correction step is performed using the left-plus operation. (Please correct me if I'm thinking this wrong).

Here is my first attempt at a code snippet to implement this approach:

  // State & Error covariance
  manif::SE3d X_;
  X_.setIdentity();
  Matrix6d P_;
  P_.setZero();

  // Control
  manif::SE3Tangentd u_;
  Vector6d u_noisy_;
  u_noisy_ << 0.0, 0.0, 0.0, 0.0, 0.0, 0.0;
  Array6d u_sigmas_;
  u_sigmas_ << 0.1, 0.1, 0.1, 0.1, 0.1, 0.1;
  Matrix6d U_;
  U_ = (u_sigmas_ * u_sigmas_).matrix().asDiagonal();

  // Declare the Jacobians of the motion wrt robot and control
  manif::SE3d::Jacobian J_x_, J_u_;

  // ...

  // IEKF callback
  u_noisy_ << msg->twist.linear.x, msg->twist.linear.y, msg->twist.linear.z,
              msg->twist.angular.x, msg->twist.angular.y, msg->twist.angular.z
  u_ = (u_noisy_ * dt_);  // dt_ = 0.025 is a constant

  // ESKF prediction.
  X_ = X_.rplus(u_, J_x_, J_u_);
  P_ = J_x_ * P_ * J_x_.transpose() + J_u_ * U_ * J_u_.transpose();

  // ESKF correction
  // measurement
  Eigen::Vector3d y(msg->latitude , msg->longitude, msg->altitude); // lat, long, alt are in local Cartesian
  Eigen::Matrix3d R;
  R.setZero();
  R.diagonal() << msg->position_covariance.at(0), msg->position_covariance.at(4), msg->position_covariance.at(8);

  // expection
  Eigen::Matrix<double, 3, 6> J_e_x;
  Eigen::Vector3d e = X_.act(Eigen::Vector3d::Zero(), J_e_x);  // y = x⋅b, b = 0 to get the translation
  Eigen::Matrix<double, 3, 6> H = J_e_x;

  // switch the covariance from right to left
  Matrix6d LP = X_.adj() * P_ * X_.adj().transpose();
  Eigen::Matrix3d E = H * LP * H.transpose();

  // innovation
  Eigen::Vector3d z = y - e;  // innovation
  Eigen::Matrix3d Z = E + R;  // innovation cov

  // Kalman gain
  Eigen::Matrix<double, 6, 3> K = LP * H.transpose() * Z.inverse();

  // Correction step
  manif::SE3Tangentd dx = K * z;

  // Update
  X_ = X_.lplus(dx);
  LP = LP - K * Z * K.transpose();

  // switch the covariance from the left to right
  P_ = X_.adj().inverse() * LP * X_.adj().inverse().transpose();

The ESKF prediction seems reasonable, but the ESKF correction gives me completely wrong updates. I then changed all the ESKF correction steps to use the right-plus operation as shown below. With this adjustment, my second attempt produced correct results.

  // ESKF correction
  //  ...

  // expection
  Eigen::Matrix<double, 3, 6> J_e_x;
  Eigen::Vector3d e = X_.act(Eigen::Vector3d::Zero(), J_e_x);  // y = x⋅b, b = 0 to get the translation
  Eigen::Matrix<double, 3, 6> H = J_e_x;
  Eigen::Matrix3d E = H * P_ * H.transpose();

  // innovation
  Eigen::Vector3d z = y - e;  // innovation
  Eigen::Matrix3d Z = E + R;  // innovation cov

  // Kalman gain
  Eigen::Matrix<double, 6, 3> K = P_ * H.transpose() * Z.inverse();

  // Correction step
  manif::SE3Tangentd dx = K * z;

  // Update
  X_ = X_.rplus(dx);
  P_ = P_ - K * Z * K.transpose();

So, here are my questions:

1. The GPS data is y=X⋅b+v, where b=0 and v is white noise. Therefore, the error is left-invariant. Why does the right-invariant error give me the correct update?
2. In my first attempt, did I correctly switch the covariance from left to right and right to left? Is this the right way of doing it? I couldn't find examples of the mixed left- and right-invariant ESKF for reference.

Thank you for any insights, and I am happy to provide additional details if needed.

[joansola]

Hi Chris,

I never implemented left-invariant filters so I am a bit unsure.

But before that, I noticed you express the measurement in lat-lon-height. Are these numbers in meters? Or are lat-lon in degrees or radians? Because then the comparison you do with the expectation, which seems a Cartesian vector, would be wrong.

Even if lat-lon are in meters, are you sure they are in the same reference frame as your expectation?

I also do not get why you act over the zero vector to compute your expectation, e = X_.act(Eigen::Vector3d::Zero(), J_e_x).

Can you please clarify this point before we dig further into the invariance?

[joansola]

But before that, I noticed you express the measurement in lat-lon-height. Are these numbers in meters? Or are lat-lon in degrees or radians? Because then the comparison you do with the expectation, which seems a Cartesian vector, would be wrong.

Oh, I noticed too late, you already state that these are in Cartesian. OK. It is more common to use NED or ENU for these matters. In fact, strictly speaking, lat-lon-height is a left-handed reference frame if lat is N and lon is E... so make sure you are OK in this regard.

[Chris7462]

Hi Joan,

Sorry for not explaining my setup clearly earlier. Let me clarify the input of my IMU and GPS again. I have preprocessed both inputs before running the ESKF.

For the IMU measurements, the initial IMU orientation is retrieved and stored, and all subsequent IMU orientations are adjusted by the inverse of the initial orientation to obtain the relative orientation. As a result, the first IMU measurement represents zero rotation, the second measurement represents the relative rotation with respect to the first, and so on. Please refer to the IMU preprocessing code here if the explanation is not clear.

For the GPS measurements, since I want the GPS inputs in local Cartesian coordinates, the initial IMU orientation is retrieved and the initial GPS coordinates are set as the origin (both topics are synced). All subsequent GPS coordinates are converted to local coordinates and then rotated by the inverse of the initial orientation to align with the initial reference frame. Please refer to the GPS preprocessing code here if the explanation is not clear.

So, the inputs of the ESKF are essentially the adjusted IMU and GPS coordinates, aligned with the initial reference frame where the vehicle starts. The full implementation can be found at here.

Back to your question: the GPS coordinates are in meters. For the ESKF correction step, since y is the observation (from the GPS coordinates), I use X acting over zero to obtain the expected translation.

I hope this clarifies my setup. I really appreciate your valuable time and help.

[joansola]

Thanks for the clarification.

What about lat-lon-altitude (north-east-up) being a left-haded reference? Is this intended? Usually all references we use in robotics are right-handed. Hence you should use a right-handed reference such as ENU (east-north-up) , which would be lon-lat-altitude.

[Chris7462]

Thank you for pointing out the inconsistency. I made an error with the variable name in my code, but I have double-checked and confirmed that I'm using the ENU right-handed reference for the GPS coordinates.

To recap my setup:

1. All IMU measurements are adjusted using the inverse of the initial orientation to obtain the relative orientation. This means the first IMU measurement has zero rotation, and the subsequent IMU measurements are rotation relative to the initial frame.
2. All GPS coordinates are converted to local coordinates and then rotated by the inverse of the initial orientation to align with the initial frame. As a result, the first GPS coordinate is the origin, and the following GPS coordinates represent positions relative to the initial frame.

So, if I understand correctly, I'm essentially treating the initial frame as the fixed world frame. The adjusted IMU measurements remain in the body frame, while the converted (in local Cartesian) GPS coordinates are in the fixed world frame, which, in my setup, is aligned with the initial frame.

Now, back to my question.

In the ESKF prediction step, I use the linear and angular velocities to predict the robot's state. Since both linear and angular velocities are measured in the body frame, I use the right-plus operation for the prediction. The prediction outputs are reasonable, with the translation of X being close to the GPS coordinates.

The issue arises when I use the converted GPS coordinates for the ESKF correction.
Let y denote the GPS coordinate (the observation). Since we know y=X⋅b+v, and with b=0, e=X_.act(Eigen::Vector3d::Zero(), J_e_x) provides the expected translation.

Based on my understanding, y=X⋅b+v represents a left-invariant observation, so I believe I should be using the left-plus operation to update X. The left-ESKF correction code is shown below:

  // ESKF correction
  // measurement
  Eigen::Vector3d y(msg->local_coordinate.x, msg->local_coordinate.y, msg->local_coordinate.z);
  Eigen::Matrix3d R(msg->position_covariance.data());

  // expection
  Eigen::Matrix<double, 3, 6> J_e_x;
  Eigen::Vector3d e = X_.act(Eigen::Vector3d::Zero(), J_e_x);  // y = x⋅b, b = 0 to get the translation
  Eigen::Matrix<double, 3, 6> H = J_e_x;

  // switch the covariance from right to left
  Matrix6d LP = X_.adj() * P_ * X_.adj().transpose();
  Eigen::Matrix3d E = H * LP * H.transpose();

  // innovation
  Eigen::Vector3d z = y - e;  // innovation
  Eigen::Matrix3d Z = E + R;  // innovation cov

  // Kalman gain
  Eigen::Matrix<double, 6, 3> K = LP * H.transpose() * Z.inverse();

  // Correction step
  manif::SE3Tangentd dx = K * z;

  // Update
  X_ = X_.lplus(dx);
  LP = LP - K * Z * K.transpose();

  // switch the covariance from the left to right
  P_ = X_.adj().inverse() * LP * X_.adj().inverse().transpose();

// Note: The above gives me completely wrong update.
// Somehow the following right-ESKF correction gives me the correct update
//
//  // expection
//  Matrix3x6d J_e_x;
//  Eigen::Vector3d e = X_.act(Eigen::Vector3d::Zero(), J_e_x);
//  Matrix3x6d H = J_e_x;
//  Eigen::Matrix3d E = H * P_ * H.transpose();
//
//  // innovation
//  Eigen::Vector3d z = y - e;  // innovation
//  Eigen::Matrix3d Z = E + R;  // innovation cov
//
//  // Kalman gain
//  Matrix6x3d K = P_ * H.transpose() * Z.inverse();
//  // Correction step
//  manif::SE3Tangentd dx = K * z;
//
//  // Update
//  X_ = X_.rplus(dx);
//  P_ = P_ - K * Z * K.transpose();

So, my questions:

1. Why does the right-invariant error give me the correct update? Isn't it the left-invariant case for the measurement update in this setup?
2. Did I correctly switch the covariance from left to right and right to left? Is this the right way of doing it?

Thank you for your help again. I know this question isn't directly related to the manif package, but I don't know who else has the expertise I can ask. Really appreciate your help. Thank you again.

[Chris7462]

Just found an example of GPS-like measurements for fusions in the kalmanif examples here: https://github.com/artivis/kalmanif/blob/devel/examples/demo_se2.cpp. This is exactly what I was looking for. Thank you for your help!