[Question] The formular in UpdaterWheel::preintegration_3D

[qpc001]

I don't understand why p0_dot = R_Gto0.transpose() * v_hat; but not p0_dot = R_Gto0 * v_hat;

  // k1 ================
  Vector4d dq_0 = {0, 0, 0, 1};
  Vector4d q0_dot = 0.5 * Omega(w_hat) * dq_0;
  Matrix3d R_Gto0 = quat_2_Rot(quat_multiply(dq_0, q_local));
  Vector3d p0_dot = R_Gto0.transpose() * v_hat;

Is it because of the quaternion is in JPL style?

[WoosikLee2510]

No, it is because v_hat is in the wheel frame, thus you need to multiply R_OtoG to convert it to global.

[qpc001]

No, it is because v_hat is in the wheel frame, thus you need to multiply R_OtoG to convert it to global.

What if I change the quaternion to Hamilton, and follow the attitude update like : q_new = q_old Δq,

then p0_dot = R_Gto0.transpose() * v_hat; will change to p0_dot = R_Gto0 * v_hat; ?

[WoosikLee2510]

No, R_GtoO is a rotation matrix. It has nothing to do with quaternions and representation type, i.e., JPL or Hamilton.

[qpc001]

No, R_GtoO is a rotation matrix. It has nothing to do with quaternions and representation type, i.e., JPL or Hamilton.

I add the following debug code :

  Vector4d dq_00 = quatnorm(dq_0 +  k1_q);
  Matrix3d R_Gto00 = quat_2_Rot(quat_multiply(dq_00, q_local));
  std::cout << "rot0 " << R_Gto00 << std::endl;

  Eigen::Quaterniond qq0 = Eigen::Quaterniond(R_3D)*theta2q(w_hat,dt);
  std::cout << "qq0 " << qq0.toRotationMatrix() << std::endl;

  // k2 ================

and result is :

rot0           
 1                          0.000583668            0
-0.000583668            1            0
           0            0            1

qq0            
1                        -0.000583668            0
 0.000583668            1            0
           0            0            1

the JPL style quaternion rotation matrix is a transpose() of Hamilton style quaternion rotation matrix.

[WoosikLee2510]

Yes, it is because you are checking 'how to convert quaternions to rotation matrix' which should be related to quaternion representation. Eigen library assumes the quaternion is Hamilton and we use it JPL so there should be a difference when you convert it to a Rotation matrix. However, I am re-emphasizing, the rotation matrix should be the same if you convert JPL quat to Rot using JPL conversion and H. quat to Rot using H. conversion.

[qpc001]

Thanks, you are right about that. hhh.

[qpc001]

Quaternion kinematics for the error-state Kalman filter of Joan Sol`a explained this. So JPL quaternion need a transpose() [or conjugate()].

[WoosikLee2510]

To be precise, JPL does not need to transpose or conjugate (because it is just a vector), it's the corresponding rotation matrixes from JPL and H. to make them equal.