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')
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 = 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) ra, rb = d.range_of(pos)
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
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()