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 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 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
'''
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)
ra, rb = d.range_of(pos)
rx, ry = d.range_of((i + f1.x[0, 0], i + f1.x[2, 0]))
print('range =', ra, rb)
'''
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)
ra, rb = d.range_of(pos)
rx, ry = d.range_of((i + f1.x[0, 0], i + f1.x[2, 0]))
print('range =', ra, rb)
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
