Extended Kalman Filter

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)