Extended Kalman Filter

Extended Kalman filter on a moving 2D point, but done using factor graphs. This example uses the ExtendedKalmanFilter class to perform filtering on a linear system, demonstrating the same operations as in elaboratePoint2KalmanFilter.

Author: Matt Kielo

import gtsam
import numpy as np

# Create the Kalman Filter initialization point
X0 = gtsam.Point2(0.0, 0.0)
P0 = gtsam.noiseModel.Diagonal.Sigmas(np.array([0.1, 0.1]))

# Create Key for initial pose
x0 = gtsam.symbol('x', 0)

# Create an ExtendedKalmanFilter object
ekf = gtsam.ExtendedKalmanFilterPoint2(x0, X0, P0)

# For this example, we use a constant-position model where
# controls drive the point to the right at 1 m/s
# F = [1 0; 0 1], B = [1 0; 0 1], and u = [1; 0]
# Process noise Q = [0.1 0; 0 0.1]
Q = gtsam.noiseModel.Diagonal.Sigmas(np.array([0.1, 0.1]), True)

# Measurement noise, assuming a GPS-like sensor
R = gtsam.noiseModel.Diagonal.Sigmas(np.array([0.25, 0.25]), True)

# Motion model - move right by 1.0 units
dX = gtsam.Point2(1.0, 0.0)

last_symbol = x0

for i in range(1, 4):
    # Create symbol for new state
    xi = gtsam.symbol('x', i)
    
    # Prediction step: P(x_i) ~ P(x_i|x_{i-1}) P(x_{i-1})
    # In Kalman Filter notation: x_{t+1|t} and P_{t+1|t}
    motion = gtsam.BetweenFactorPoint2(last_symbol, xi, dX, Q)
    Xi_predict = ekf.predict(motion)
    print(f"X{i} Predict:", Xi_predict)
    
    # Update step: P(x_i|z_i) ~ P(z_i|x_i)*P(x_i)
    # Assuming a measurement model h(x_{t}) = H*x_{t} + v
    # where H is the observation model/matrix and v is noise with covariance R
    measurement = gtsam.Point2(float(i), 0.0)
    meas_factor = gtsam.PriorFactorPoint2(xi, measurement, R)
    Xi_update = ekf.update(meas_factor)
    print(f"X{i} Update:", Xi_update)
    
    # Move to next state
    last_symbol = xi

A = ekf.Density().getA()
information_matrix = A.transpose() @ A
covariance_matrix = np.linalg.inv(information_matrix)
print ("\nEasy Final Covariance (after update):\n", covariance_matrix)

X1 Predict: [1. 0.]
X1 Update: [1. 0.]
X2 Predict: [2. 0.]
X2 Update: [2. 0.]
X3 Predict: [3. 0.]
X3 Update: [3. 0.]

Easy Final Covariance (after update):
 [[0.01930567 0.        ]
 [0.         0.01930567]]

GTSAM Docs

A Discrete Switching System

GTSAM Docs

Elaborate EKF Example