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