Kalman filter built in python 3 that will output the predicted path of an object based off old data. It's just a normal kalman filter really :)

Kalman-filter.py

# Scalable Kalman Filter written in python 3 that takes in multiple matrixes and outputs both a predicted state estimate and predicted estimate covariance.
# NOTE: matrixes passed into the Kalman filter MUST adhear to linear algebra matrix multiplication rules.

# Uncomment import if matrix has not been imported yet.
# from Matrix import matrix

# inputs:
# measurements: Sensor world data.
# x: Initial state.
# P: Initial uncertainty.
# H: Measurement function.
# R: Measurement uncertainty.
# I: Identitiy matrix.
# u: External motion data.
# F: Next state function

# mock inputs (make sure you import matrix functions):
# measurements = measurements = [[5., 10.], [6., 8.], [7., 6.], [8., 4.], [9., 2.], [10., 0.]]
# initial_xy = initial_xy = [4., 12.]
# dt = 0.1 #time interval
# x = matrix([[initial_xy[0]], [initial_xy[1]], [0.], [0.]])
# u = u = matrix([[0.], [0.], [0.], [0.]])
# P = matrix([[0., 0., 0., 0.],[0., 0., 0., 0.],[0., 0., 1000., 0.],[0., 0., 0., 1000.]])
# F = matrix([[1., 0., dt, 0.],[0., 1., 0., dt],[0., 0., 1., 0.],[0., 0., 0., 1.]])
# H = matrix([[1., 0., 0., 0.],[0., 1., 0., 0.]])
# R = matrix([[0.1, 0.], [0., 0.1]])
# I = matrix([[1., 0., 0., 0.],[0., 1., 0., 0.],[0., 0., 1., 0.],[0., 0., 0., 1.]])

# print(kalman_filter(x, P, H, R, I, u, F))

def kalman_filter(measurements, x, P, H, R, I, u, F):
    
    for n in range(len(measurements)):

        # Update Filter based on new world measurements
        # Need to update data based on new world data passed in.
        z = matrix([[measurements[n]]])                                 # Measurement of the true state x
        y = z.transpose() - (H * x)                                     # Measurement pre-fit residual
        S = H*P*H.transpose() + R                                       # Pre-fit residual covariance
        K = P * H.transpose() * S.inverse()                             # Optimal Kalman Gain


        x = x + (K*y)                                                   # Updated state estimate (a posteriori)
        P = (I - (K*H))*P*(I - K*H).transpose() + K*R*K.transpose()     # Updated estimate covariance (a posteriori)

        # Calculate Prediction
        x = F*x + u                                                     # Predicted state estimate (a priori)
        P = F*P*F.transpose()                                           # Predicted estimate covariance (a priori)

        # Uncomment below if you'd like to see prediction calculations for each iteration.
        #print('x = ')
        #x.show()
        #print('P = ')
        #P.show()
        
    # Return final prediction calculations for Predicted state estimate and Predicted estimate covariance.
    return x, P