def testVee(self):
for i in range(100):
omega_true = np.random.uniform(-np.pi, np.pi, size=3)
logR = np.array([[0, -omega_true[2], omega_true[1]],
[omega_true[2], 0, -omega_true[0]],
[-omega_true[1], omega_true[0], 0]])
omega = SO3.vee(logR)
np.testing.assert_allclose(omega_true, omega)
def attUpdate(self, quad, z):
z_hat = quad.q_i_from_b.R
H = np.block([np.zeros((3, 6)), np.eye(3)])
r = SO3.vee(z_hat @ z)
S = H @ quad.P_ @ H.T + self.R_att_
K = quad.P_ @ H.T @ np.linalg.inv(S)
dx = K @ r
quad.boxplusr(dx)
M = np.eye(9) - K @ H
quad.P_ = M @ quad.P_ @ M.T + K @ self.R_att_ @ K.T
return quad