From 1f8e47a34f547f10b78c017ccab9e5dda1a47520 Mon Sep 17 00:00:00 2001 From: Gabriele Gilardi Date: Mon, 15 Jun 2020 21:08:33 +0900 Subject: [PATCH] save work --- Code_Python/KalmanCourse.py | 183 +++++++++++++++++++++++++++++++++ Code_Python/KalmanFilter.py | 195 ++++++++---------------------------- Code_Python/filters.py | 1 - 3 files changed, 225 insertions(+), 154 deletions(-) create mode 100644 Code_Python/KalmanCourse.py diff --git a/Code_Python/KalmanCourse.py b/Code_Python/KalmanCourse.py new file mode 100644 index 0000000..123e4ae --- /dev/null +++ b/Code_Python/KalmanCourse.py @@ -0,0 +1,183 @@ + +# import numpy as np + +# measurements = np.array([5., 6., 7., 9., 10.]) +# motion = np.array([1., 1., 2., 1., 1.]) +# measurement_sigma = 4. +# motion_sigma = 2. +# mu = 0. +# sigma = 1000. + +# # Measurement +# def Update( mean1, var1, mean2, var2 ): +# mean = (var2*mean1 + var1*mean2) / (var1 + var2) +# var = 1.0 / (1.0/var1 + 1.0/var2) +# return [mean, var] + +# # Motion +# def Predict( mean1, var1, U, varU ): +# mean = mean1 + U +# var = var1 + varU +# return [mean, var] + +# for n in range(len(measurements)): +# [mu, sigma] = Update(mu, sigma, measurements[n], measurement_sigma) +# print('Update : ', n, [mu, sigma]) +# [mu, sigma] = Predict(mu, sigma, motion[n],motion_sigma) +# print('Predict: ', n, [mu, sigma]) + +# print(' ') +# print(Update(1,1,3,1)) + +# ------------------------------------------------------- + +import numpy as np + +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) + + +def filter(x, P): + + 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)) + + # Prediction + x = F.dot(xp) + U + Ftra = F.T + P = F.dot(Pp.dot(Ftra)) + + print('x =') + print(x) + print('P =') + print(P) + +filter(x, P) + +# # ------------------------------------------------------- + +# 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 = 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)) + + +# def filter(x, P): + +# for n in range(len(measurements)): + +# 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)) + +# # 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 + + +# 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 99e8f07..7302e29 100644 --- a/Code_Python/KalmanFilter.py +++ b/Code_Python/KalmanFilter.py @@ -1,178 +1,67 @@ +""" +- generalize to N and M +M = 1 +N = 2 +""" import numpy as np -measurements = np.array([5., 6., 7., 9., 10.]) -motion = np.array([1., 1., 2., 1., 1.]) -measurement_sigma = 4. -motion_sigma = 2. -mu = 0. -sigma = 1000. - -# Measurement -def Update( mean1, var1, mean2, var2 ): - mean = (var2*mean1 + var1*mean2) / (var1 + var2) - var = 1.0 / (1.0/var1 + 1.0/var2) - return [mean, var] - -# Motion -def Predict( mean1, var1, U, varU ): - mean = mean1 + U - var = var1 + varU - return [mean, var] - -for n in range(len(measurements)): - [mu, sigma] = Update(mu, sigma, measurements[n], measurement_sigma) - print('Update : ', n, [mu, sigma]) - [mu, sigma] = Predict(mu, sigma, motion[n],motion_sigma) - print('Predict: ', n, [mu, sigma]) - -print(' ') -print(Update(1,1,3,1)) - -# ------------------------------------------------------- - -import numpy as np - -measurements = [ 1., 2., 3. ] dt = 1. -# Initial state (location and velocity) +measurements = np.array([ 1., 2., 3., 4., 5., 6., 7., 8, 9, 10]) + +# State vector +# (N, 1) x = np.array([[ 0. ], [ 0. ]]) -# Initial uncertainty + +# Prediction uncertainty (covariance matrix of x) +# (N, N) P = np.array([[ 1000., 0. ], [ 0., 1000. ]]) + # External motion +# (N, 1) U = np.array([[ 0. ], [ 0. ]]) -# Next state function + +# Update matrix (state transition matrix) +# (N, N) 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) +# 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) def filter(x, P): - for z in (measurements): + 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)) + 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 # Prediction - x = F.dot(xp) + U - Ftra = F.T - P = F.dot(Pp.dot(Ftra)) + x = F @ xp + U + P = F @ Pp @ F.T - print('x = ',x) - print('P = ',P) + print('x =') + print(x) + print('P =') + print(P) filter(x, P) - -# ------------------------------------------------------- - -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 = 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)) - - -def filter(x, P): - - for n in range(len(measurements)): - - 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)) - - # 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 - - -x_final, P_final = filter(x, P) -print('x = ') -print(x_final) -print('P = ') -print(P_final) diff --git a/Code_Python/filters.py b/Code_Python/filters.py index d429f8d..ec5d223 100644 --- a/Code_Python/filters.py +++ b/Code_Python/filters.py @@ -25,7 +25,6 @@ Notes: - non filtered data are set equal to the original input, i.e. Y[0:idx-1,:] = X[0:idx-1,:] - Filters: Generic b,a Generic case