diff --git a/Code_Python/KalmanCourse.py b/Code_Python/KalmanCourse.py index 123e4ae..ef63cdb 100644 --- a/Code_Python/KalmanCourse.py +++ b/Code_Python/KalmanCourse.py @@ -31,153 +31,153 @@ # ------------------------------------------------------- -import numpy as np +# import numpy as np -measurements = [ 1., 2., 3. ] -dt = 1. +# measurements = [ 1., 2., 3. ] +# dt = 1. -# Initial state (location and velocity) -x = np.array([[ 0. ], - [ 0. ]]) -# Initial uncertainty -P = np.array([[ 1000., 0. ], - [ 0., 1000. ]]) -# External motion -U = np.array([[ 0. ], - [ 0. ]]) -# Next state function -F = np.array([[ 1., dt ], - [ 0., 1. ]]) -# Measurement function -H = np.array([[ 1., 0. ]]) -# Measurement uncertainty -R = np.array([[ 1. ]]) -# Identity matrix -I = np.eye(2) +# # Initial state (location and velocity) +# x = np.array([[ 0. ], +# [ 0. ]]) +# # Initial uncertainty +# P = np.array([[ 1000., 0. ], +# [ 0., 1000. ]]) +# # External motion +# U = np.array([[ 0. ], +# [ 0. ]]) +# # Next state function +# F = np.array([[ 1., dt ], +# [ 0., 1. ]]) +# # Measurement function +# H = np.array([[ 1., 0. ]]) +# # Measurement uncertainty +# R = np.array([[ 1. ]]) +# # Identity matrix +# I = np.eye(2) -def filter(x, P): +# def filter(x, P): - step = 0 - for z in (measurements): - step += 1 - print("step = ", step, " meas. = ", z) +# step = 0 +# for z in (measurements): +# step += 1 +# print("step = ", step, " meas. = ", z) - # Measurement - Htra = H.T - S = H.dot(P.dot(Htra)) + R - Sinv = np.linalg.inv(S) - K = P.dot(Htra.dot(Sinv)) - y = z - H.dot(x) - xp = x +K.dot(y) - Pp = P - K.dot(H.dot(P)) +# # Measurement +# Htra = H.T +# S = H.dot(P.dot(Htra)) + R +# Sinv = np.linalg.inv(S) +# K = P.dot(Htra.dot(Sinv)) +# y = z - H.dot(x) +# xp = x +K.dot(y) +# Pp = P - K.dot(H.dot(P)) - # Prediction - x = F.dot(xp) + U - Ftra = F.T - P = F.dot(Pp.dot(Ftra)) +# # Prediction +# x = F.dot(xp) + U +# Ftra = F.T +# P = F.dot(Pp.dot(Ftra)) - print('x =') - print(x) - print('P =') - print(P) +# print('x =') +# print(x) +# print('P =') +# print(P) -filter(x, P) +# filter(x, P) # # ------------------------------------------------------- -# import numpy as np +import numpy as np -# # x0 = 4. -# # y0 = 12. -# # measurements = np.array([[ 5., 10. ], -# # [ 6., 8. ], -# # [ 7., 6. ], -# # [ 8., 4. ], -# # [ 9., 2. ], -# # [ 10., 0. ]]) -# # x0 = -4. -# # y0 = 8. -# # measurements = np.array([[ 1., 4. ], -# # [ 6., 0. ], -# # [ 11., -4. ], -# # [ 16., -8. ]]) -# # x0 = 1. -# # y0 = 19. -# # measurements = np.array([[ 1., 17. ], -# # [ 1., 15. ], -# # [ 1., 13. ], -# # [ 1., 11. ]]) +x0 = 4. +y0 = 12. +measurements = np.array([[ 5., 10. ], + [ 6., 8. ], + [ 7., 6. ], + [ 8., 4. ], + [ 9., 2. ], + [ 10., 0. ]]) +# x0 = -4. +# y0 = 8. +# measurements = np.array([[ 1., 4. ], +# [ 6., 0. ], +# [ 11., -4. ], +# [ 16., -8. ]]) +# x0 = 1. +# y0 = 19. +# measurements = np.array([[ 1., 17. ], +# [ 1., 15. ], +# [ 1., 13. ], +# [ 1., 11. ]]) # x0 = 1. # y0 = 19. # measurements = np.array([[ 2., 17. ], # [ 0., 15. ], # [ 2., 13. ], # [ 0., 11. ]]) -# # Time step -# dt = 0.1 -# # Initial state (location and velocity) -# x = np.array([[ x0 ], -# [ y0 ], -# [ 0. ], -# [ 0. ]]) -# # Initial uncertainty -# P = np.array([[ 0., 0., 0., 0. ], -# [ 0., 0., 0., 0. ], -# [ 0., 0., 1000., 0. ], -# [ 0., 0., 0., 1000. ]]) -# # External motion -# U = np.array([[ 0. ], -# [ 0. ], -# [ 0. ], -# [ 0. ]]) -# # Next state function -# F = np.array([[ 1., 0., dt, 0. ], -# [ 0., 1., 0., dt ], -# [ 0., 0., 1., 0. ], -# [ 0., 0., 0., 1. ]]) -# # Measurement function -# H = np.array([[ 1., 0., 0., 0. ], -# [ 0., 1., 0., 0. ]]) -# # Measurement uncertainty -# R = np.array([[ 0.1, 0. ], -# [ 0. , 0.1 ]]) -# # Measurement vector -# z = np.zeros((2,1)) +# Time step +dt = 0.1 +# Initial state (location and velocity) +x = np.array([[ x0 ], + [ y0 ], + [ 0. ], + [ 0. ]]) +# Initial uncertainty +P = np.array([[ 0., 0., 0., 0. ], + [ 0., 0., 0., 0. ], + [ 0., 0., 1000., 0. ], + [ 0., 0., 0., 1000. ]]) +# External motion +U = np.array([[ 0. ], + [ 0. ], + [ 0. ], + [ 0. ]]) +# Next state function +F = np.array([[ 1., 0., dt, 0. ], + [ 0., 1., 0., dt ], + [ 0., 0., 1., 0. ], + [ 0., 0., 0., 1. ]]) +# Measurement function +H = np.array([[ 1., 0., 0., 0. ], + [ 0., 1., 0., 0. ]]) +# Measurement uncertainty +R = np.array([[ 0.1, 0. ], + [ 0. , 0.1 ]]) +# Measurement vector +z = np.zeros((2,1)) -# def filter(x, P): +def filter(x, P): -# for n in range(len(measurements)): + for n in range(len(measurements)): -# z[0][0] = measurements[n][0] -# z[1][0] = measurements[n][1] + z[0][0] = measurements[n][0] + z[1][0] = measurements[n][1] -# # Prediction -# xp = F.dot(x) + U -# Ftra = F.T -# Pp = F.dot(P.dot(Ftra)) + # Prediction + xp = F.dot(x) + U + Ftra = F.T + Pp = F.dot(P.dot(Ftra)) -# # Measurement -# Htra = H.T -# S = H.dot(Pp.dot(Htra)) + R -# Sinv = np.linalg.inv(S) -# K = Pp.dot(Htra.dot(Sinv)) -# y = z - H.dot(xp) -# x = xp +K.dot(y) -# P = Pp - K.dot(H.dot(Pp)) -# # print(z) -# # print('x = ') -# # print(x) -# # print('P = ') -# # print(P) -# # print(' ') + # Measurement + Htra = H.T + S = H.dot(Pp.dot(Htra)) + R + Sinv = np.linalg.inv(S) + K = Pp.dot(Htra.dot(Sinv)) + y = z - H.dot(xp) + x = xp +K.dot(y) + P = Pp - K.dot(H.dot(Pp)) + # print(z) + # print('x = ') + # print(x) + # print('P = ') + # print(P) + # print(' ') -# return x, P + return x, P -# x_final, P_final = filter(x, P) -# print('x = ') -# print(x_final) -# print('P = ') -# print(P_final) +x_final, P_final = filter(x, P) +print('x = ') +print(x_final) +print('P = ') +print(P_final) diff --git a/Code_Python/KalmanFilter.py b/Code_Python/KalmanFilter.py index 7302e29..a82bb68 100644 --- a/Code_Python/KalmanFilter.py +++ b/Code_Python/KalmanFilter.py @@ -1,45 +1,92 @@ """ -- generalize to N and M -M = 1 -N = 2 +correction = update = measurement +prediction = motion + +X (n_states, 1) State vector +P (n_states, n_states) Covariance matrix of X +F (n_states, n_states) State transition matrix +U (n_states, 1) Input/control/drift vector +Z (n_meas, 1) Measurament vector +H (n_meas, n_states) Measurament matrix +R (n_meas, n_meas) Covariance matrix of Z +S (n_meas, n_meas) Covariance matrix (?) +K (n_states, m) Kalman gain +Q (n_states, n_states) Covariance matrix (?) + +Data (n_meas, n_samples) Measurements +Fext (n_states, n_samples) External driver +X0 (n_states, 1) Initial state vector +P0 (n_states, n_states) Initial covariance matrix of X """ + import numpy as np -dt = 1. -measurements = np.array([ 1., 2., 3., 4., 5., 6., 7., 8, 9, 10]) +class KalmanFilter: -# State vector -# (N, 1) -x = np.array([[ 0. ], - [ 0. ]]) + def __init__(self, F, H, Q, R): + """ + """ + self.F = F + self.Q = Q + self.H = H + self.R = R -# Prediction uncertainty (covariance matrix of x) -# (N, N) -P = np.array([[ 1000., 0. ], - [ 0., 1000. ]]) + + def prediction(self, X, P, U): + X = self.F @ X + U + P = self.F @ P @ self.F.T + self.Q + return X, P + + + def update(self, X, P, Z): + """ + """ + S = self.H @ P @ self.H.T + self.R + K = P @ self.H.T @ np.linalg.inv(S) + X = X + K @ (Z - self.H @ X) + P = P - K @ self.H @ P + return X, P + + + def applyFilter(self, Data, Fext, X0, P0): + """ + """ + pass + + +# Measurements +data = np.array([[5., 6., 7., 8., 9., 10.], + [10., 8., 6., 4., 2., 0.]]) + +# Initial state vector +X0 = np.array([[4. ], + [12.], + [0. ], + [0. ]]) + +# Initial covariance matrix of X +P0 = np.array([[0., 0., 0., 0.], + [0., 0., 0., 0.], + [0., 0., 1000., 0.], + [0., 0., 0., 1000.]]) # External motion -# (N, 1) -U = np.array([[ 0. ], - [ 0. ]]) +Fext = np.zeros_like(data) -# Update matrix (state transition matrix) -# (N, N) -F = np.array([[ 1., dt ], - [ 0., 1. ]]) - -# Measurement function (extraction matrix) -# (M, N) -H = np.array([[ 1., 0. ]]) - -# Measurement uncertainty/noise (covariance matrix of z) -# (M, M) -R = np.array([[ 1. ]]) - -# z = measurament vector -# (M, 1) +# Next state function +dt = 0.1 +F = np.array([[ 1., 0., dt, 0. ], + [ 0., 1., 0., dt ], + [ 0., 0., 1., 0. ], + [ 0., 0., 0., 1. ]]) +# Measurement function +H = np.array([[ 1., 0., 0., 0. ], + [ 0., 1., 0., 0. ]]) +# Measurement uncertainty +R = np.array([[ 0.1, 0. ], + [ 0. , 0.1 ]]) def filter(x, P): @@ -48,16 +95,8 @@ def filter(x, P): step += 1 print("step = ", step, " meas. = ", z) - # Measurement - S = H @ P @ H.T + R # (M, M) - K = P @ H.T @ np.linalg.inv(S) # (N, M) - y = z - H @ x - xp = x + K @ y - Pp = P - K @ H @ P + # Update - # Prediction - x = F @ xp + U - P = F @ Pp @ F.T print('x =') print(x)