cotramarko

# Pseudo code in Python of how the sensor fusion predict-update loop could look like
sf_algo = SFalgo(motion_model, measurement_model)

for (i, y) in enumerate(measurements): 
    if i == 0:
        pred = sf_algo.predict(prior)
        post = sf_algo.update(y, pred)
    else:
        pred = sf_algo.predict(post)
        post = sf_algo.update(y, pred)

cotramarko

import numpy as np
import matplotlib.pyplot as plt


class MotionModel():
    def __init__(self, A, Q):
        self.A = A
        self.Q = Q

        (m, _) = Q.shape

cotramarko

import numpy as np


class KalmanFilter():
    def __init__(self, A, H, Q, R, x_0, P_0):
        # Model parameters
        self.A = A
        self.H = H
        self.Q = Q
        self.R = R

cotramarko

import numpy as np
import matplotlib.pyplot as plt

from kalman_filter import KalmanFilter
from simulate_model import simulate_system, create_model_parameters

np.random.seed(21)
(A, H, Q, R) = create_model_parameters()
K = 20
# initial state