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

Fix include for Retract

release/4.3a0
Frank Dellaert 2025-05-16 15:45:07 -04:00
parent be64222b10
commit f81879aff6
1 changed files with 6 additions and 5 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

11
examples/IEKF_NavstateExample.cpp
View File

@ -17,6 +17,7 @@
*/ */
#include <gtsam/base/Matrix.h> #include <gtsam/base/Matrix.h>
#include <gtsam/base/VectorSpace.h>
#include <gtsam/base/OptionalJacobian.h> #include <gtsam/base/OptionalJacobian.h>
#include <gtsam/navigation/InvariantEKF.h> #include <gtsam/navigation/InvariantEKF.h>
#include <gtsam/navigation/NavState.h> #include <gtsam/navigation/NavState.h>
@ -73,23 +74,23 @@ int main() {
z2 << 0.6, 0, 0; z2 << 0.6, 0, 0;
cout << "=== Init ===\nX: " << ekf.state() << "\nP: " << ekf.covariance() cout << "=== Init ===\nX: " << ekf.state() << "\nP: " << ekf.covariance()
<< "\n\n"; << "\n\n";
// --- first predict/update --- // --- first predict/update ---
ekf.predict(dynamics(imu1), dt, Q); ekf.predict(dynamics(imu1), dt, Q);
cout << "--- After predict 1 ---\nX: " << ekf.state() cout << "--- After predict 1 ---\nX: " << ekf.state()
<< "\nP: " << ekf.covariance() << "\n\n"; << "\nP: " << ekf.covariance() << "\n\n";
ekf.update(h_gps, z1, R); ekf.update(h_gps, z1, R);
cout << "--- After update 1 ---\nX: " << ekf.state() cout << "--- After update 1 ---\nX: " << ekf.state()
<< "\nP: " << ekf.covariance() << "\n\n"; << "\nP: " << ekf.covariance() << "\n\n";
// --- second predict/update --- // --- second predict/update ---
ekf.predict(dynamics(imu2), dt, Q); ekf.predict(dynamics(imu2), dt, Q);
cout << "--- After predict 2 ---\nX: " << ekf.state() cout << "--- After predict 2 ---\nX: " << ekf.state()
<< "\nP: " << ekf.covariance() << "\n\n"; << "\nP: " << ekf.covariance() << "\n\n";
ekf.update(h_gps, z2, R); ekf.update(h_gps, z2, R);
cout << "--- After update 2 ---\nX: " << ekf.state() cout << "--- After update 2 ---\nX: " << ekf.state()
<< "\nP: " << ekf.covariance() << "\n"; << "\nP: " << ekf.covariance() << "\n";
return 0; return 0;
} }