def __init__(self, b1, b2, b3, dt):
EKF.__init__(self, 6, self.f, self.F, self.h, self.H)
self.b1 = b1
self.b2 = b2
self.b3 = b3
self.dt = dt
# self.Q = PROCESS_NOISE_VARIANCE * np.eye(6)
w_psd = PROCESS_NOISE_VARIANCE
self.Q = np.array([[0, 0, 0, -0.5 * dt**2 * w_psd, 0, 0],
[0, 0, 0, 0, -0.5 * dt**2 * w_psd, 0],
[0, 0, 0, 0, 0, -0.5 * dt**2 * w_psd],
[0, 0, 0, -0.5 * dt**3 * w_psd + dt * w_psd, 0, 0],
[0, 0, 0, 0, -0.5 * dt**3 * w_psd + dt * w_psd, 0],
[0, 0, 0, 0, 0, -0.5 * dt**3 * w_psd + dt * w_psd]
]) + w_psd * np.eye(self.n)
def __init__(self, b1, b2, b3, dt):
EKF.__init__(self, 6, self.f, self.F, self.h, self.H)
self.b1 = b1
self.b2 = b2
self.b3 = b3
self.dt = dt
# self.Q = PROCESS_NOISE_VARIANCE * np.eye(6)
w_psd = PROCESS_NOISE_VARIANCE
self.Q = np.array([
[0, 0, 0, -0.5*dt**2*w_psd, 0, 0],
[0, 0, 0, 0, -0.5*dt**2*w_psd, 0],
[0, 0, 0, 0, 0, -0.5*dt**2*w_psd],
[0, 0, 0, -0.5*dt**3*w_psd + dt*w_psd, 0, 0],
[0, 0, 0, 0, -0.5*dt**3*w_psd + dt*w_psd, 0],
[0, 0, 0, 0, 0, -0.5*dt**3*w_psd + dt*w_psd]
]) + w_psd * np.eye(self.n)
