Correct jacobian for h_gps

2025-04-27 21:03:34 -04:00 · 2025-04-27 21:03:34 -04:00 · 5bc01081ee
parent 87c4445f53
commit 5bc01081ee
1 changed files with 1 additions and 2 deletions
--- a/examples/LIEKF_NavstateExample.cpp
+++ b/examples/LIEKF_NavstateExample.cpp
@ -46,8 +46,7 @@ Vector9 dynamics(const Vector6& imu) {
 * @return     3×1 position vector.
 */
 Vector3 h_gps(const NavState& X, OptionalJacobian<3, 9> H = {}) {
-  if (H) *H << Z_3x3, Z_3x3, X.R().matrix();
-  return X.t();
+  return X.position(H);
 }

 int main() {