x, _, y, _, omega = ravel(state) vw = -2 * omega * Ts vwx = vw * x vwy = vw * y return process_sigmasq * Ts * array([ [0.0, 0.0, 0.0, 0.0, 0.0], [0.0, vwx*vwx/3, 0.0, vwx*vwy/3, vwx/2], [0.0, 0.0, 0.0, 0.0, 0.0], [0.0, vwy*vwx/3, 0.0, vwy*vwy/3, vwy/2], [0.0, vwx/2, 0.0, vwy/2, 1.0], ]) process = SineProcess() measurement = kalman.observation_model( H=[[1.0, 0.0, 0.0, 0.0, 0.0], [0.0, 0.0, 1.0, 0.0, 0.0]], R=measurement_sigmasq * eye(2), ) results = [] true_amplitude = 1 true_rv = norm(0, 0.3) true_theta = 0 for t in xrange(int(400 / Ts) + 1): true_omega = 0.75 + 0.5 * sin(t * Ts * math.pi / 100) true_theta = true_theta + Ts * true_omega filt.predict(process) true = true_amplitude * array([math.cos(true_theta), math.sin(true_theta)]) noisy = true + true_rv.rvs(size=2) filt.update(measurement, *noisy)
accel_sigma = 2.0 measurement_sigma = 1.0 process = kalman.process_model( F=array([ [1, dt], [0, 1], ]), Q=array([ [(dt**4)/4, (dt**3)/2], [(dt**3)/2, dt**2], ]) * (accel_sigma ** 2) ) measure = kalman.observation_model( H=[[1.0, 0.0]], R=[[measurement_sigma ** 2]], ) filt = kalman.kalman( x=zeros((2,1)), P=zeros((2,2)), ) accel_rv = norm(0, accel_sigma) measurement_rv = norm(0, measurement_sigma) accelerations = (accel_rv.rvs() for _ in xrange(1000)) true_positions = [] true_velocities = [] estimated_positions = [] estimated_velocities = []
dt = 0.1 accel_sigma = 2.0 measurement_sigma = 1.0 process = kalman.process_model(F=array([ [1, dt], [0, 1], ]), Q=array([ [(dt**4) / 4, (dt**3) / 2], [(dt**3) / 2, dt**2], ]) * (accel_sigma**2)) measure = kalman.observation_model( H=[[1.0, 0.0]], R=[[measurement_sigma**2]], ) filt = kalman.kalman( x=zeros((2, 1)), P=zeros((2, 2)), ) accel_rv = norm(0, accel_sigma) measurement_rv = norm(0, measurement_sigma) accelerations = (accel_rv.rvs() for _ in xrange(1000)) true_positions = [] true_velocities = [] estimated_positions = [] estimated_velocities = []
[dt ** 4 / 4, dt ** 3 / 3, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0], [dt ** 3 / 3, dt ** 2, dt ** 3 / 3, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0], [ 0.0, dt ** 3 / 3, dt, dt ** 3 / 3, 0.0, 0.0, 0.0, 0.0, 0.0], [ 0.0, 0.0, dt ** 3 / 3, dt ** 4 / 4, dt ** 3 / 3, 0.0, 0.0, 0.0, 0.0], [ 0.0, 0.0, 0.0, dt ** 3 / 3, dt ** 2, dt ** 3 / 3, 0.0, 0.0, 0.0], [ 0.0, 0.0, 0.0, 0.0, dt ** 3 / 3, dt, dt ** 3 / 3, 0.0, 0.0], [ 0.0, 0.0, 0.0, 0.0, 0.0, dt ** 3 / 3, dt ** 4 / 4, dt ** 3 / 3, 0.0], [ 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, dt ** 3 / 3, dt ** 2, dt ** 3 / 3], [ 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, dt ** 3 / 3, dt]]) * process_sigmasq ) accel = kalman.observation_model( H = [ [0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0], [0.0, 0.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0], [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1.0]], R = [ [measurement_sigmasq, 0.0, 0.0], [0.0, measurement_sigmasq, 0.0], [0.0, 0.0, measurement_sigmasq]] ) gps = kalman.observation_model( H = [[0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0]], R = [[measurement_sigmasq]] ) for line in sys.stdin: li = line.split('\t') if li[0] == 'accel': t.update(accel, float(li[2]), float(li[3]), -float(li[4]) - 9.80665) elif li[0] == 'pressure':
[0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1.0, dt], [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1.0]]), Q=array([[dt**4 / 4, dt**3 / 3, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0], [dt**3 / 3, dt**2, dt**3 / 3, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0], [0.0, dt**3 / 3, dt, dt**3 / 3, 0.0, 0.0, 0.0, 0.0, 0.0], [0.0, 0.0, dt**3 / 3, dt**4 / 4, dt**3 / 3, 0.0, 0.0, 0.0, 0.0], [0.0, 0.0, 0.0, dt**3 / 3, dt**2, dt**3 / 3, 0.0, 0.0, 0.0], [0.0, 0.0, 0.0, 0.0, dt**3 / 3, dt, dt**3 / 3, 0.0, 0.0], [0.0, 0.0, 0.0, 0.0, 0.0, dt**3 / 3, dt**4 / 4, dt**3 / 3, 0.0], [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, dt**3 / 3, dt**2, dt**3 / 3], [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, dt**3 / 3, dt]]) * process_sigmasq) accel = kalman.observation_model( H=[[0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0], [0.0, 0.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0], [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1.0]], R=[[measurement_sigmasq, 0.0, 0.0], [0.0, measurement_sigmasq, 0.0], [0.0, 0.0, measurement_sigmasq]]) gps = kalman.observation_model( H=[[0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0]], R=[[measurement_sigmasq]]) for line in sys.stdin: li = line.split('\t') if li[0] == 'accel': t.update(accel, float(li[2]), float(li[3]), -float(li[4]) - 9.80665) elif li[0] == 'pressure': t.update(gps, float(li[2])) else: continue
results, info = odeint(func, [xdot, x], [0, Ts], full_output=True) self.stepsused.append(info['nst'][0]) xdot, x = results[1] return array([[x, xdot, omega]]).T def Q(self, state): deriv = state[2][0] * state[0][0] * Ts return process_sigmasq * Ts * array([ [0.0, 0.0, 0.0], [0.0, 4.0/3.0 * deriv ** 2, -deriv], [0.0, -deriv, 1.0]]) process = SineProcess() measurement = kalman.observation_model( H=[[1.0, 0.0, 0.0]], R=[[measurement_sigmasq]] ) results = [] true_amplitude = 1 true_rv = norm(0, 0.3) true_theta = 0 for t in xrange(int(400 / Ts) + 1): true_omega = 0.75 + 0.5 * sin(t * Ts * math.pi / 100) true_theta = true_theta + Ts * true_omega filt.predict(process) true = true_amplitude * math.sin(true_theta) noisy = true + true_rv.rvs() filt.update(measurement, noisy)
vw = -2 * omega * Ts vwx = vw * x vwy = vw * y return process_sigmasq * Ts * array([ [0.0, 0.0, 0.0, 0.0, 0.0], [0.0, vwx * vwx / 3, 0.0, vwx * vwy / 3, vwx / 2], [0.0, 0.0, 0.0, 0.0, 0.0], [0.0, vwy * vwx / 3, 0.0, vwy * vwy / 3, vwy / 2], [0.0, vwx / 2, 0.0, vwy / 2, 1.0], ]) process = SineProcess() measurement = kalman.observation_model( H=[[1.0, 0.0, 0.0, 0.0, 0.0], [0.0, 0.0, 1.0, 0.0, 0.0]], R=measurement_sigmasq * eye(2), ) results = [] true_amplitude = 1 true_rv = norm(0, 0.3) true_theta = 0 for t in xrange(int(400 / Ts) + 1): true_omega = 0.75 + 0.5 * sin(t * Ts * math.pi / 100) true_theta = true_theta + Ts * true_omega filt.predict(process) true = true_amplitude * array([math.cos(true_theta), math.sin(true_theta)]) noisy = true + true_rv.rvs(size=2) filt.update(measurement, *noisy)
results, info = odeint(func, [xdot, x], [0, Ts], full_output=True) self.stepsused.append(info['nst'][0]) xdot, x = results[1] return array([[x, xdot, omega]]).T def Q(self, state): deriv = state[2][0] * state[0][0] * Ts return process_sigmasq * Ts * array( [[0.0, 0.0, 0.0], [0.0, 4.0 / 3.0 * deriv**2, -deriv], [0.0, -deriv, 1.0]]) process = SineProcess() measurement = kalman.observation_model(H=[[1.0, 0.0, 0.0]], R=[[measurement_sigmasq]]) results = [] true_amplitude = 1 true_rv = norm(0, 0.3) true_theta = 0 for t in xrange(int(400 / Ts) + 1): true_omega = 0.75 + 0.5 * sin(t * Ts * math.pi / 100) true_theta = true_theta + Ts * true_omega filt.predict(process) true = true_amplitude * math.sin(true_theta) noisy = true + true_rv.rvs() filt.update(measurement, noisy)