import kalman import matplotlib.pyplot as plt from numpy import * from scipy.stats import norm 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)
[0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 10., 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, 1.0]]), ) process = kalman.process_model( F=array([ [1.0, dt, 0.0, 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, 0.0, 0.0, 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, dt, 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, 1.0, dt, 0.0], [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],
import kalman import matplotlib.pyplot as plt from numpy import * from scipy.stats import norm 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)
[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, 10., 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, 1.0]]), ) process = kalman.process_model( F=array([[1.0, dt, 0.0, 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, 0.0, 0.0, 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, dt, 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, 1.0, dt, 0.0], [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],