Skip to content

Instantly share code, notes, and snippets.

DecoderMG/Kalman-filter.py

Last active May 24, 2018 18:38
Show Gist options
• 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 :)
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters. Learn more about bidirectional Unicode characters
 # 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
to join this conversation on GitHub. Already have an account? Sign in to comment