Example #1
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)
measurement_rv = norm(0, measurement_sigma)
Example #2
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],
Example #3
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)
Example #4
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, 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],