def __init__(self, Q_gyro=1e-3, Q_bias=1e-12, R=10):
self.q = Quaternion.Identity()
self.bias = np.array([0.0, 0.0, 0.0])
self.sigma = np.diag([0.1, 0.1, 0.1, 0.5, 0.5, 0.5])
# self.Q = 0.02 * np.eye(6) * 0.05
self.Q = np.diag([Q_gyro] * 3 + [Q_bias] * 3)
self.R = R * np.eye(3)
def __init__(self, alpha=0.01):
self.q = Quaternion.Identity()
self.alpha = alpha
