return (ra + random.randn()*self.noise_factor,
rb + random.randn()*self.noise_factor)
pos_a = (100,-20)
pos_b = (-100, -20)
f1 = KalmanFilter(dim_x=4, dim_z=2)
f1.F = np.mat ([[0, 1, 0, 0],
[0, 0, 0, 0],
[0, 0, 0, 1],
[0, 0, 0, 0]])
f1.B = 0.
f1.R *= 1.
f1.Q *= .1
f1.x = np.mat([1,0,1,0]).T
f1.P = np.eye(4) * 5.
# initialize storage and other variables for the run
count = 30
xs, ys = [],[]
pxs, pys = [],[]
# create the simulated sensor
d = DMESensor (pos_a, pos_b, noise_factor=1.)
return (ra + random.randn()*self.noise_factor,
rb + random.randn()*self.noise_factor)
pos_a = (100,-20)
pos_b = (-100, -20)
f1 = KalmanFilter(dim_x=4, dim_z=2)
f1.F = np.mat ([[0, 1, 0, 0],
[0, 0, 0, 0],
[0, 0, 0, 1],
[0, 0, 0, 0]])
f1.B = 0.
f1.R *= 1.
f1.Q *= .1
f1.x = np.mat([1,0,1,0]).T
f1.P = np.eye(4) * 5.
# initialize storage and other variables for the run
count = 30
xs, ys = [],[]
pxs, pys = [],[]
# create the simulated sensor
d = DMESensor (pos_a, pos_b, noise_factor=1.)
