def ball_kf_noay(x, y, omega, v0, dt, r=0.5, q=0.02): g = 9.8 # gravitational constant f1 = KalmanFilter(dim_x=5, dim_z=2) ay = .5*dt**2 f1.F = np.mat ([[1, dt, 0, 0, 0], # x = x0+dx*dt [0, 1, 0, 0, 0], # dx = dx [0, 0, 1, dt, 0], # y = y0 +dy*dt [0, 0, 0, 1, dt], # dy = dy0 + ddy*dt [0, 0, 0, 0, 1]]) # ddy = -g. f1.H = np.mat([ [1, 0, 0, 0, 0], [0, 0, 1, 0, 0]]) f1.R *= r f1.Q *= q omega = radians(omega) vx = cos(omega) * v0 vy = sin(omega) * v0 f1.x = np.mat([x,vx,y,vy,-9.8]).T return f1
def ball_kf_noay(x, y, omega, v0, dt, r=0.5, q=0.02): g = 9.8 # gravitational constant f1 = KalmanFilter(dim_x=5, dim_z=2) ay = .5 * dt**2 f1.F = np.mat([ [1, dt, 0, 0, 0], # x = x0+dx*dt [0, 1, 0, 0, 0], # dx = dx [0, 0, 1, dt, 0], # y = y0 +dy*dt [0, 0, 0, 1, dt], # dy = dy0 + ddy*dt [0, 0, 0, 0, 1] ]) # ddy = -g. f1.H = np.mat([[1, 0, 0, 0, 0], [0, 0, 1, 0, 0]]) f1.R *= r f1.Q *= q omega = radians(omega) vx = cos(omega) * v0 vy = sin(omega) * v0 f1.x = np.mat([x, vx, y, vy, -9.8]).T return f1
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')
Created on Sat May 24 14:42:55 2014 @author: rlabbe """ 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])
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
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) z = np.mat([[ra - rx], [rb - ry]]) print('z =', z) f1.H = H_of(pos, pos_a, pos_b) print('H =', f1.H) ##f1.update (z) print(f1.x) xs.append(f1.x[0, 0] + i) ys.append(f1.x[2, 0] + i) pxs.append(pos[0]) pys.append(pos[1]) f1.predict() print(f1.H * f1.x) print(z) print(f1.x) f1.update(z) print(f1.x)
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
""" 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):