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