commit
cc4a0e1804
|
@ -10,7 +10,7 @@
|
|||
* -------------------------------------------------------------------------- */
|
||||
|
||||
/**
|
||||
* @file IEKF_Rot3Example.cpp
|
||||
* @file GEKF_Rot3Example.cpp
|
||||
* @brief Left‐Invariant EKF on SO(3) with state‐dependent pitch/roll control
|
||||
* and a single magnetometer update.
|
||||
* @date April 25, 2025
|
||||
|
|
|
@ -17,6 +17,7 @@
|
|||
*/
|
||||
|
||||
#include <gtsam/base/Matrix.h>
|
||||
#include <gtsam/base/VectorSpace.h>
|
||||
#include <gtsam/base/OptionalJacobian.h>
|
||||
#include <gtsam/navigation/InvariantEKF.h>
|
||||
#include <gtsam/navigation/NavState.h>
|
||||
|
@ -73,23 +74,23 @@ int main() {
|
|||
z2 << 0.6, 0, 0;
|
||||
|
||||
cout << "=== Init ===\nX: " << ekf.state() << "\nP: " << ekf.covariance()
|
||||
<< "\n\n";
|
||||
<< "\n\n";
|
||||
|
||||
// --- first predict/update ---
|
||||
ekf.predict(dynamics(imu1), dt, Q);
|
||||
cout << "--- After predict 1 ---\nX: " << ekf.state()
|
||||
<< "\nP: " << ekf.covariance() << "\n\n";
|
||||
<< "\nP: " << ekf.covariance() << "\n\n";
|
||||
ekf.update(h_gps, z1, R);
|
||||
cout << "--- After update 1 ---\nX: " << ekf.state()
|
||||
<< "\nP: " << ekf.covariance() << "\n\n";
|
||||
<< "\nP: " << ekf.covariance() << "\n\n";
|
||||
|
||||
// --- second predict/update ---
|
||||
ekf.predict(dynamics(imu2), dt, Q);
|
||||
cout << "--- After predict 2 ---\nX: " << ekf.state()
|
||||
<< "\nP: " << ekf.covariance() << "\n\n";
|
||||
<< "\nP: " << ekf.covariance() << "\n\n";
|
||||
ekf.update(h_gps, z2, R);
|
||||
cout << "--- After update 2 ---\nX: " << ekf.state()
|
||||
<< "\nP: " << ekf.covariance() << "\n";
|
||||
<< "\nP: " << ekf.covariance() << "\n";
|
||||
|
||||
return 0;
|
||||
}
|
||||
|
|
|
@ -47,7 +47,7 @@ set (LIBVERSION_BUILD 17.1.2)
|
|||
string (TOLOWER ${PROJECT_NAME} PROJECT_NAME_LOWER)
|
||||
string (TOUPPER ${PROJECT_NAME} PROJECT_NAME_UPPER)
|
||||
|
||||
cmake_minimum_required (VERSION 2.8.4) # This version was released 2011-02-16
|
||||
cmake_minimum_required (VERSION 3.5) # This version was released 2011-02-16
|
||||
|
||||
# User-settable variables
|
||||
|
||||
|
|
Loading…
Reference in New Issue