def correct_beacon(self, beacon_x, beacon_y, distance):
"""
Corrects with the distance measurement provided from the given beacon
position.
"""
corr = ekf.Corrector(
h_b(beacon_x, beacon_y), H_b(beacon_x, beacon_y), self.Q_b)
z = np.array([distance])
self.mu, self.sigma = corr(self.mu, self.sigma, z)
def setUp(self):
self.g = lambda s, u: s
self.G = lambda s, u: np.array([[1]]) # linearized
self.R = np.array([[0.1]]) # noise induced
self.predictor = ekf.Predictor(self.g, self.G, self.R)
self.h = lambda s: s
self.H = lambda _: np.array([[1]])
self.Q = np.array([[0.1]])
self.corrector = ekf.Corrector(self.h, self.H, self.Q)
def __init__(self, dt, R, Q, Q_b):
"""
Creates an instance of the simple model.
R is the update covariance.
Q is the angle update covarance.
Q_b is the beacon update covariance.
"""
self.Q = Q
self.Q_b = Q_b
self.predictor = ekf.Predictor(g(dt), G(dt), R)
self.angle_corrector = ekf.Corrector(h, H, Q)
self.mu = np.array([0, 0, 0, 0, 0])
self.sigma = np.diag([1e-2, 1e-2, 0.01, 0.1, 0.1])
def setUp(self):
self.h = lambda s: s
self.H = lambda _: np.array([[1]])
self.Q = np.array([[0.1]])
self.f = ekf.Corrector(self.h, self.H, self.Q)