def test_ball_path():
y = 15
x = 0
omega = 0.
noise = [1,1]
v0 = 100.
ball = BallPath (x0=x, y0=y, omega_deg=omega, velocity=v0, noise=noise)
dt = 1
f1 = KalmanFilter(dim_x=6, dim_z=2)
dt = 1/30. # time step
ay = -.5*dt**2
f1.F = np.mat ([[1, dt, 0, 0, 0, 0], # x=x0+dx*dt
[0, 1, dt, 0, 0, 0], # dx = dx
[0, 0, 0, 0, 0, 0], # ddx = 0
[0, 0, 0, 1, dt, ay], # y = y0 +dy*dt+1/2*g*dt^2
[0, 0, 0, 0, 1, dt], # dy = dy0 + ddy*dt
[0, 0, 0, 0, 0, 1]]) # ddy = -g
f1.H = np.mat([
[1, 0, 0, 0, 0, 0],
[0, 0, 0, 1, 0, 0]])
f1.R = np.eye(2) * 5
f1.Q = np.eye(6) * 0.
omega = radians(omega)
vx = cos(omega) * v0
vy = sin(omega) * v0
f1.x = np.mat([x,vx,0,y,vy,-9.8]).T
f1.P = np.eye(6) * 500.
z = np.mat([[0,0]]).T
count = 0
markers.MarkerStyle(fillstyle='none')
np.set_printoptions(precision=4)
while f1.x[3,0] > 0:
count += 1
#f1.update (z)
f1.predict()
plt.scatter(f1.x[0,0],f1.x[3,0], color='green')
def initialise_filter(self):
filter_ = KalmanFilter(
dim_x=4,
dim_z=2) # need to instantiate every time to reset all fields
filter_.F = self.matrix_a
filter_.H = self.matrix_c
filter_.B = self.matrix_g
if KalmanParams.use_noise_in_kalman:
u = np.random.normal(loc=0, scale=KalmanParams.var_kalman, size=2)
filter_.u = u
# u = Q_discrete_white_noise(dim=2, var=1)
filter_.Q = self.q
filter_.R = self.r
return filter_
def test_ball_path():
y = 15
x = 0
omega = 0.
noise = [1, 1]
v0 = 100.
ball = BallPath(x0=x, y0=y, omega_deg=omega, velocity=v0, noise=noise)
dt = 1
f1 = KalmanFilter(dim_x=6, dim_z=2)
dt = 1 / 30. # time step
ay = -.5 * dt**2
f1.F = np.mat([
[1, dt, 0, 0, 0, 0], # x=x0+dx*dt
[0, 1, dt, 0, 0, 0], # dx = dx
[0, 0, 0, 0, 0, 0], # ddx = 0
[0, 0, 0, 1, dt, ay], # y = y0 +dy*dt+1/2*g*dt^2
[0, 0, 0, 0, 1, dt], # dy = dy0 + ddy*dt
[0, 0, 0, 0, 0, 1]
]) # ddy = -g
f1.H = np.mat([[1, 0, 0, 0, 0, 0], [0, 0, 0, 1, 0, 0]])
f1.R = np.eye(2) * 5
f1.Q = np.eye(6) * 0.
omega = radians(omega)
vx = cos(omega) * v0
vy = sin(omega) * v0
f1.x = np.mat([x, vx, 0, y, vy, -9.8]).T
f1.P = np.eye(6) * 500.
z = np.mat([[0, 0]]).T
count = 0
markers.MarkerStyle(fillstyle='none')
np.set_printoptions(precision=4)
while f1.x[3, 0] > 0:
count += 1
#f1.update (z)
f1.predict()
plt.scatter(f1.x[0, 0], f1.x[3, 0], color='green')
"""
from KalmanFilter import KalmanFilter
import numpy as np
import matplotlib.pyplot as plt
import numpy.random as random
f = KalmanFilter(dim=4)
dt = 1
f.F = np.mat([[1, dt, 0, 0], [0, 1, 0, 0], [0, 0, 1, dt], [0, 0, 0, 1]])
f.H = np.mat([[1, 0, 0, 0], [0, 0, 1, 0]])
f.Q *= 4.
f.R = np.mat([[50, 0], [0, 50]])
f.x = np.mat([0, 0, 0, 0]).T
f.P *= 100.
xs = []
ys = []
count = 200
for i in range(count):
z = np.mat([[i + random.randn() * 1], [i + random.randn() * 1]])
f.predict()
f.update(z)
xs.append(f.x[0, 0])
ys.append(f.x[2, 0])
plt.plot(xs, ys)
def test_kf_drag():
y = 1
x = 0
omega = 35.
noise = [0,0]
v0 = 50.
ball = BaseballPath (x0=x, y0=y,
launch_angle_deg=omega,
velocity_ms=v0, noise=noise)
#ball = BallPath (x0=x, y0=y, omega_deg=omega, velocity=v0, noise=noise)
dt = 1
f1 = KalmanFilter(dim_x=6, dim_z=2)
dt = 1/30. # time step
ay = -.5*dt**2
ax = .5*dt**2
f1.F = np.mat ([[1, dt, ax, 0, 0, 0], # x=x0+dx*dt
[0, 1, dt, 0, 0, 0], # dx = dx
[0, 0, 1, 0, 0, 0], # ddx = 0
[0, 0, 0, 1, dt, ay], # y = y0 +dy*dt+1/2*g*dt^2
[0, 0, 0, 0, 1, dt], # dy = dy0 + ddy*dt
[0, 0, 0, 0, 0, 1]]) # ddy = -g
# We will inject air drag using Bu
f1.B = np.mat([[0., 0., 1., 0., 0., 0.],
[0., 0., 0., 0., 0., 1.]]).T
f1.u = np.mat([[0., 0.]]).T
f1.H = np.mat([
[1, 0, 0, 0, 0, 0],
[0, 0, 0, 1, 0, 0]])
f1.R = np.eye(2) * 5
f1.Q = np.eye(6) * 0.
omega = radians(omega)
vx = cos(omega) * v0
vy = sin(omega) * v0
f1.x = np.mat([x,vx,0,y,vy,-9.8]).T
f1.P = np.eye(6) * 500.
z = np.mat([[0,0]]).T
markers.MarkerStyle(fillstyle='none')
np.set_printoptions(precision=4)
t=0
while f1.x[3,0] > 0:
t+=dt
#f1.update (z)
x,y = ball.update(dt)
#x,y = ball.pos_at_t(t)
update_drag(f1, dt)
f1.predict()
print f1.x.T
plt.scatter(f1.x[0,0],f1.x[3,0], color='red', alpha=0.5)
plt.scatter (x,y)
return f1
pos_b = (-100, -20)
f1 = KalmanFilter(dim=4)
dt = 1.0 # time step
'''
f1.F = np.mat ([[1, dt, 0, 0],
[0, 1, 0, 0],
[0, 0, 1, dt],
[0, 0, 0, 1]])
'''
f1.F = np.mat([[0, 1, 0, 0], [0, 0, 0, 0], [0, 0, 0, 1], [0, 0, 0, 0]])
f1.B = 0.
f1.R = np.eye(2) * 1.
f1.Q = np.eye(4) * .1
f1.x = np.mat([1, 0, 1, 0]).T
f1.P = np.eye(4) * 5.
# initialize storage and other variables for the run
count = 30
xs, ys = [], []
pxs, pys = [], []
d = DMESensor(pos_a, pos_b, noise_factor=1.)
pos = [0, 0]
for i in range(count):
pos = (i, i)
pos_b = (-100, -20)
f1 = KalmanFilter(dim=4)
dt = 1.0 # time step
'''
f1.F = np.mat ([[1, dt, 0, 0],
[0, 1, 0, 0],
[0, 0, 1, dt],
[0, 0, 0, 1]])
'''
f1.F = np.mat([[0, 1, 0, 0], [0, 0, 0, 0], [0, 0, 0, 1], [0, 0, 0, 0]])
f1.B = 0.
f1.R = np.eye(2) * 1.
f1.Q = np.eye(4) * .1
f1.x = np.mat([1, 0, 1, 0]).T
f1.P = np.eye(4) * 5.
# initialize storage and other variables for the run
count = 30
xs, ys = [], []
pxs, pys = [], []
d = DMESensor(pos_a, pos_b, noise_factor=1.)
pos = [0, 0]
for i in range(count):
pos = (i, i)
def test_kf_drag():
y = 1
x = 0
omega = 35.
noise = [0, 0]
v0 = 50.
ball = BaseballPath(x0=x,
y0=y,
launch_angle_deg=omega,
velocity_ms=v0,
noise=noise)
#ball = BallPath (x0=x, y0=y, omega_deg=omega, velocity=v0, noise=noise)
dt = 1
f1 = KalmanFilter(dim_x=6, dim_z=2)
dt = 1 / 30. # time step
ay = -.5 * dt**2
ax = .5 * dt**2
f1.F = np.mat([
[1, dt, ax, 0, 0, 0], # x=x0+dx*dt
[0, 1, dt, 0, 0, 0], # dx = dx
[0, 0, 1, 0, 0, 0], # ddx = 0
[0, 0, 0, 1, dt, ay], # y = y0 +dy*dt+1/2*g*dt^2
[0, 0, 0, 0, 1, dt], # dy = dy0 + ddy*dt
[0, 0, 0, 0, 0, 1]
]) # ddy = -g
# We will inject air drag using Bu
f1.B = np.mat([[0., 0., 1., 0., 0., 0.], [0., 0., 0., 0., 0., 1.]]).T
f1.u = np.mat([[0., 0.]]).T
f1.H = np.mat([[1, 0, 0, 0, 0, 0], [0, 0, 0, 1, 0, 0]])
f1.R = np.eye(2) * 5
f1.Q = np.eye(6) * 0.
omega = radians(omega)
vx = cos(omega) * v0
vy = sin(omega) * v0
f1.x = np.mat([x, vx, 0, y, vy, -9.8]).T
f1.P = np.eye(6) * 500.
z = np.mat([[0, 0]]).T
markers.MarkerStyle(fillstyle='none')
np.set_printoptions(precision=4)
t = 0
while f1.x[3, 0] > 0:
t += dt
#f1.update (z)
x, y = ball.update(dt)
#x,y = ball.pos_at_t(t)
update_drag(f1, dt)
f1.predict()
print f1.x.T
plt.scatter(f1.x[0, 0], f1.x[3, 0], color='red', alpha=0.5)
plt.scatter(x, y)
return f1
f = KalmanFilter (dim=4)
dt = 1
f.F = np.mat ([[1, dt, 0, 0],
[0, 1, 0, 0],
[0, 0, 1, dt],
[0, 0, 0, 1]])
f.H = np.mat ([[1, 0, 0, 0],
[0, 0, 1, 0]])
f.Q *= 4.
f.R = np.mat([[50,0],
[0, 50]])
f.x = np.mat([0,0,0,0]).T
f.P *= 100.
xs = []
ys = []
count = 200
for i in range(count):
z = np.mat([[i+random.randn()*1],[i+random.randn()*1]])
f.predict ()
f.update (z)
xs.append (f.x[0,0])
ys.append (f.x[2,0])
import numpy as np
import matplotlib.pyplot as plt
from KalmanFilter import KalmanFilter
timesteps = 100
#create synthetic data
pure = np.linspace(0, 10, timesteps)
noise = np.random.normal(0, 1, timesteps)
signal = pure + noise
plt.scatter(range(100), signal, label='sensor data')
plt.title("Synthetic data")
#create filter instance and run
filter = KalmanFilter(timesteps, signal)
filter.F = 1.015
filter.Q = 1
filter.R = 50
x, P = filter.runFilter()
# visualize
print(x)
print(P)
plt.plot(x, color='red', label='prediction')
plt.plot(pure, color='black', label='actual')
plt.legend()
plt.show()
