convert jacobian into RPY space

Author: suchkristenwow

fuse_models/include/fuse_models/omnidirectional_3d_predict.hpp

@@ -493,9 +493,16 @@ inline void predict(fuse_core::Vector3d const& position1, Eigen::Quaterniond con
           vel_linear2.y(), vel_linear2.z(), vel_angular2.x(), vel_angular2.y(), vel_angular2.z(), acc_linear2.x(),
           acc_linear2.y(), acc_linear2.z(), jacobians.data(), jacobian_quat2rpy);
 
-  // TODO(henrygerardmoore): figure out how to fix this
-  // see https://github.com/locusrobotics/fuse/pull/354#discussion_r1884288806
-  jacobian << J[0], J[1], J[2], J[3], J[4].block<15, 2>(0, 0);
+  // Convert J[1] from quaternion space (15x4) to RPY space (15x3) using the
+  // pseudo-inverse of the quaternion-to-RPY Jacobian.
+  // Chain rule: J[1] = d(state2)/d(quat1) = d(state2)/d(rpy1) * d(rpy)/d(quat)
+  // So: d(state2)/d(rpy1) = J[1] * pinv(d(rpy)/d(quat))
+  Eigen::Map<fuse_core::Matrix<double, 3, 4>> j_quat2rpy_map(jacobian_quat2rpy);
+  fuse_core::Matrix<double, 4, 3> j_quat2rpy_pinv =
+      j_quat2rpy_map.transpose() * (j_quat2rpy_map * j_quat2rpy_map.transpose()).inverse();
+  fuse_core::Matrix<double, 15, 3> J1_rpy = J[1] * j_quat2rpy_pinv;
+
+  jacobian << J[0], J1_rpy, J[2], J[3], J[4];
 
   // Convert back to quaternion
   orientation2 = Eigen::AngleAxisd(rpy[2], Eigen::Vector3d::UnitZ()) *