ExtendedKalmanFilter

Overview¶

The ExtendedKalmanFilter class in GTSAM is an implementation of the Extended Kalman Filter (EKF), which is a powerful tool for estimating the state of a nonlinear dynamic system.

See also this notebook for the python version of the C++ example below.

Using the ExtendedKalmanFilter Class¶

The ExtendedKalmanFilter class in GTSAM provides a flexible way to implement Kalman filtering using factor graphs. Here’s a step-by-step guide based on the example provided in easyPoint2KalmanFilter.cpp:

Steps to Use the ExtendedKalmanFilter¶

1. Initialize the Filter:

   • Define the initial state (e.g., position) and its covariance.
   • Create a key for the initial state.
   • Instantiate the ExtendedKalmanFilter object with the initial state and covariance.

   Point2 x_initial(0.0, 0.0);
   SharedDiagonal P_initial = noiseModel::Diagonal::Sigmas(Vector2(0.1, 0.1));
   Symbol x0('x', 0);
   ExtendedKalmanFilter<Point2> ekf(x0, x_initial, P_initial);

2. Predict the Next State:

   • Define the motion model using a BetweenFactor.
   • Predict the next state using the predict method.

   Symbol x1('x', 1);
   Point2 difference(1, 0);
   SharedDiagonal Q = noiseModel::Diagonal::Sigmas(Vector2(0.1, 0.1), true);
   BetweenFactor<Point2> factor1(x0, x1, difference, Q);
   Point2 x1_predict = ekf.predict(factor1);

3. Update the State with Measurements:

   • Define the measurement model using a PriorFactor.
   • Update the state using the update method.

   Point2 z1(1.0, 0.0);
   SharedDiagonal R = noiseModel::Diagonal::Sigmas(Vector2(0.25, 0.25), true);
   PriorFactor<Point2> factor2(x1, z1, R);
   Point2 x1_update = ekf.update(factor2);

4. Repeat for Subsequent Time Steps:

   • Repeat the prediction and update steps for subsequent states and measurements.

Example Use Case¶

This example demonstrates tracking a moving 2D point using a simple linear motion model and position measurements. The ExtendedKalmanFilter class allows for flexible modeling of both the motion and measurement processes using GTSAM’s factor graph framework.

For the full implementation, see the easyPoint2KalmanFilter.cpp file.