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

Docs with o4-mini

release/4.3a0
Frank Dellaert 2025-04-26 12:30:55 -04:00
parent f63255be5b
commit 885ab38a7e
1 changed files with 54 additions and 59 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

111
examples/LIEKF_NavstateExample.cpp
View File

@ -11,19 +11,14 @@
/** /**
* @file LIEKF_NavstateExample.cpp * @file LIEKF_NavstateExample.cpp
* @brief A left invariant Extended Kalman Filter example using the LIEKF * @brief Example of a Left-Invariant Extended Kalman Filter on NavState
* on NavState using IMU/GPS measurements. * using IMU (predict) and GPS (update) measurements.
* * @date April 25, 2025
* This example uses the templated LIEKF class to estimate the state of * @authors Scott Baker, Matt Kielo, Frank Dellaert
* an object using IMU/GPS measurements. The prediction stage of the LIEKF uses
* a generic dynamics function to predict the state. This simulates a navigation
* state of (pose, velocity, position)
*
* @date Apr 25, 2025
* @author Scott Baker
* @author Matt Kielo
* @author Frank Dellaert
*/ */
#include <gtsam/base/Matrix.h>
#include <gtsam/base/OptionalJacobian.h>
#include <gtsam/navigation/NavState.h> #include <gtsam/navigation/NavState.h>
#include <gtsam/nonlinear/LIEKF.h> #include <gtsam/nonlinear/LIEKF.h>
@ -32,85 +27,85 @@
using namespace std; using namespace std;
using namespace gtsam; using namespace gtsam;
// Define a dynamics function. /**
// The dynamics function for NavState returns a result vector of * @brief Left-invariant dynamics for NavState.
// size 9 of [angular_velocity, 0, 0, 0, linear_acceleration] as well as * @param X Current state (unused for left-invariant error dynamics).
// a Jacobian of the dynamics function with respect to the state X. * @param imu 6×1 vector [a; ω]: linear accel (first 3) and angular vel (last
// Since this is a left invariant EKF, the error dynamics do not rely on the * 3).
// state * @param H Optional 9×9 Jacobian w.r.t. X (always zero here).
* @return 9×1 tangent: [ω; 0₃; a].
*/
Vector9 dynamics(const NavState& X, const Vector6& imu, Vector9 dynamics(const NavState& X, const Vector6& imu,
OptionalJacobian<9, 9> H = {}) { OptionalJacobian<9, 9> H = {}) {
const auto a = imu.head<3>(); auto a = imu.head<3>();
const auto w = imu.tail<3>(); auto w = imu.tail<3>();
Vector9 result; Vector9 xi;
result << w, Z_3x1, a; xi << w, Vector3::Zero(), a;
if (H) { if (H) *H = Matrix9::Zero();
*H = Matrix::Zero(9, 9); return xi;
}
return result;
} }
// define a GPS measurement processor. The GPS measurement processor returns /**
// the expected measurement h(x) = translation of X with a Jacobian H used in * @brief GPS measurement model: returns position and its Jacobian.
// the update stage of the LIEKF. * @param X Current state.
* @param H Optional 3×9 Jacobian w.r.t. X.
* @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(); if (H) {
// H = [ 0₃×3, 0₃×3, R ]
*H << Z_3x3, Z_3x3, X.R();
}
return X.t(); return X.t();
} }
int main() { int main() {
// Initialize the filter's state, covariance, and time interval values. // Initial state, covariance, and time step
NavState X0; NavState X0;
Matrix9 P0 = Matrix9::Identity() * 0.1; Matrix9 P0 = Matrix9::Identity() * 0.1;
double dt = 1.0; double dt = 1.0;
// Create the filter with the initial state, covariance, and dynamics and // Create the filter with the initial state and covariance.
// measurement functions.
LIEKF<NavState> ekf(X0, P0); LIEKF<NavState> ekf(X0, P0);
// Create the process covariance and measurement covariance matrices Q and R. // Process & measurement noise
Matrix9 Q = Matrix9::Identity() * 0.01; Matrix9 Q = Matrix9::Identity() * 0.01;
Matrix3 R = Matrix3::Identity() * 0.5; Matrix3 R = Matrix3::Identity() * 0.5;
// Create the IMU measurements of the form (linear_acceleration, // Create the IMU measurements of the form (linear_acceleration,
// angular_velocity) // angular_velocity)
Vector6 imu1, imu2; Vector6 imu1, imu2;
imu1 << 0.0, 0.0, 0.0, 0.0, 0.0, 0.0; imu1 << 0.1, 0.0, 0.0, 0.0, 0.2, 0.0;
imu2 << 0.0, 0.0, 0.0, 0.0, 0.0, 0.0; imu2 << 0.0, 0.3, 0.0, 0.4, 0.0, 0.0;
// Create the GPS measurements of the form (px, py, pz) // Create the GPS measurements of the form (px, py, pz)
Vector3 z1, z2; Vector3 z1, z2;
z1 << 0.0, 0.0, 0.0; z1 << 0.3, 0.0, 0.0;
z2 << 0.0, 0.0, 0.0; z2 << 0.6, 0.0, 0.0;
// Run the predict and update stages, and print their results. cout << "=== Initialization ===\n"
cout << "\nInitialization:\n"; << "X0: " << ekf.state() << "\n"
cout << "X0: " << ekf.state() << endl; << "P0: " << ekf.covariance() << "\n\n";
cout << "P0: " << ekf.covariance() << endl;
// First prediction stage
ekf.predict(dynamics, imu1, dt, Q); ekf.predict(dynamics, imu1, dt, Q);
cout << "\nFirst Prediction:\n"; cout << "--- After first predict ---\n"
cout << "X: " << ekf.state() << endl; << "X: " << ekf.state() << "\n"
cout << "P: " << ekf.covariance() << endl; << "P: " << ekf.covariance() << "\n\n";
// First update stage
ekf.update(h_gps, z1, R); ekf.update(h_gps, z1, R);
cout << "\nFirst Update:\n"; cout << "--- After first update ---\n"
cout << "X: " << ekf.state() << endl; << "X: " << ekf.state() << "\n"
cout << "P: " << ekf.covariance() << endl; << "P: " << ekf.covariance() << "\n\n";
// Second prediction stage
ekf.predict(dynamics, imu2, dt, Q); ekf.predict(dynamics, imu2, dt, Q);
cout << "\nSecond Prediction:\n"; cout << "--- After second predict ---\n"
cout << "X: " << ekf.state() << endl; << "X: " << ekf.state() << "\n"
cout << "P: " << ekf.covariance() << endl; << "P: " << ekf.covariance() << "\n\n";
// Second update stage
ekf.update(h_gps, z2, R); ekf.update(h_gps, z2, R);
cout << "\nSecond Update:\n"; cout << "--- After second update ---\n"
cout << "X: " << ekf.state() << endl; << "X: " << ekf.state() << "\n"
cout << "P: " << ekf.covariance() << endl; << "P: " << ekf.covariance() << "\n";
return 0; return 0;
} }