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_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
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