Skip to content

Instantly share code, notes, and snippets.

@DecoderMG
Last active May 24, 2018 18:38
Show Gist options
  • Save DecoderMG/aebe956ba688ac9452cc34b08d06e1d3 to your computer and use it in GitHub Desktop.
Save DecoderMG/aebe956ba688ac9452cc34b08d06e1d3 to your computer and use it in GitHub Desktop.
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 :)
# 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
Sign up for free to join this conversation on GitHub. Already have an account? Sign in to comment