12345qiupeng
/
gtsam
1
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

Correct jacobian for h_gps

release/4.3a0
Frank Dellaert 2025-04-27 21:03:34 -04:00
parent 87c4445f53
commit 5bc01081ee
1 changed files with 1 additions and 2 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

3
examples/LIEKF_NavstateExample.cpp
View File

@ -46,8 +46,7 @@ Vector9 dynamics(const Vector6& imu) {
* @return 3×1 position vector. * @return 3×1 position vector.
*/ */
Vector3 h_gps(const NavState& X, OptionalJacobian<3, 9> H = {}) { Vector3 h_gps(const NavState& X, OptionalJacobian<3, 9> H = {}) {
if (H) *H << Z_3x3, Z_3x3, X.R().matrix(); return X.position(H);
return X.t();
} }
int main() { int main() {