Python observation_model Examples

Example #1

File: cexp.py Project: imclab/filters

		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)

Example #2

File: train.py Project: psas/filters

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 = []

Example #3

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 = []

Example #4

File: lv2.py Project: imclab/filters

		[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':

Example #5

File: lv2.py Project: imclab/filters

             [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

Example #6

File: sine.py Project: imclab/filters

		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)

Example #7

File: cexp.py Project: imclab/filters

        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)

Example #8

File: sine.py Project: imclab/filters

        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)