kopia lustrzana https://github.com/gabrielegilardi/SignalFilters
save work
rodzic
8d18544acd
commit
1f8e47a34f
|
@ -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)
|
|
@ -1,178 +1,67 @@
|
||||||
|
"""
|
||||||
|
- generalize to N and M
|
||||||
|
|
||||||
|
M = 1
|
||||||
|
N = 2
|
||||||
|
"""
|
||||||
import numpy as np
|
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.
|
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. ],
|
x = np.array([[ 0. ],
|
||||||
[ 0. ]])
|
[ 0. ]])
|
||||||
# Initial uncertainty
|
|
||||||
|
# Prediction uncertainty (covariance matrix of x)
|
||||||
|
# (N, N)
|
||||||
P = np.array([[ 1000., 0. ],
|
P = np.array([[ 1000., 0. ],
|
||||||
[ 0., 1000. ]])
|
[ 0., 1000. ]])
|
||||||
|
|
||||||
# External motion
|
# External motion
|
||||||
|
# (N, 1)
|
||||||
U = np.array([[ 0. ],
|
U = np.array([[ 0. ],
|
||||||
[ 0. ]])
|
[ 0. ]])
|
||||||
# Next state function
|
|
||||||
|
# Update matrix (state transition matrix)
|
||||||
|
# (N, N)
|
||||||
F = np.array([[ 1., dt ],
|
F = np.array([[ 1., dt ],
|
||||||
[ 0., 1. ]])
|
[ 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):
|
def filter(x, P):
|
||||||
|
|
||||||
for z in (measurements):
|
step = 0
|
||||||
|
for z in (measurements):
|
||||||
|
step += 1
|
||||||
|
print("step = ", step, " meas. = ", z)
|
||||||
|
|
||||||
# Measurement
|
# Measurement
|
||||||
Htra = H.T
|
S = H @ P @ H.T + R # (M, M)
|
||||||
S = H.dot(P.dot(Htra)) + R
|
K = P @ H.T @ np.linalg.inv(S) # (N, M)
|
||||||
Sinv = np.linalg.inv(S)
|
y = z - H @ x
|
||||||
K = P.dot(Htra.dot(Sinv))
|
xp = x + K @ y
|
||||||
y = z - H.dot(x)
|
Pp = P - K @ H @ P
|
||||||
xp = x +K.dot(y)
|
|
||||||
Pp = P - K.dot(H.dot(P))
|
|
||||||
|
|
||||||
# Prediction
|
# Prediction
|
||||||
x = F.dot(xp) + U
|
x = F @ xp + U
|
||||||
Ftra = F.T
|
P = F @ Pp @ F.T
|
||||||
P = F.dot(Pp.dot(Ftra))
|
|
||||||
|
|
||||||
print('x = ',x)
|
print('x =')
|
||||||
print('P = ',P)
|
print(x)
|
||||||
|
print('P =')
|
||||||
|
print(P)
|
||||||
|
|
||||||
filter(x, 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)
|
|
||||||
|
|
|
@ -25,7 +25,6 @@ Notes:
|
||||||
- non filtered data are set equal to the original input, i.e.
|
- non filtered data are set equal to the original input, i.e.
|
||||||
Y[0:idx-1,:] = X[0:idx-1,:]
|
Y[0:idx-1,:] = X[0:idx-1,:]
|
||||||
|
|
||||||
|
|
||||||
Filters:
|
Filters:
|
||||||
|
|
||||||
Generic b,a Generic case
|
Generic b,a Generic case
|
||||||
|
|
Ładowanie…
Reference in New Issue