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