5 Eigen::Quaternionf est_quat(q.
w, q.
x, q.
y, q.
z);
6 Eigen::Quaternionf q_tilde = q_eig * est_quat.inverse();
7 if (q_tilde.vec().norm() < 0.000001)
11 Eigen::Vector3f v_tilde =
atan2(q_tilde.vec().norm(), q_tilde.w())*q_tilde.vec()/q_tilde.vec().norm();
12 return v_tilde.norm();
18 Eigen::Quaternionf est_quat(q.
w, q.
x, q.
y, q.
z);
19 Eigen::Quaternionf q_eig(q0.
w, q0.
x, q0.
y, q0.
z);
20 Eigen::Quaternionf q_tilde = q_eig * est_quat.inverse();
21 if (q_tilde.vec().norm() < 0.000001)
25 Eigen::Vector3f v_tilde =
atan2(q_tilde.vec().norm(), q_tilde.w())*q_tilde.vec()/q_tilde.vec().norm();
26 return v_tilde.norm();
33 float dummy_acc[3] = {0, 0, -9.80665};
34 float dummy_gyro[3] = {0, 0, 0};
42 int main(
int argc,
char **argv)
44 testing::InitGoogleTest(&argc, argv);
45 return RUN_ALL_TESTS();
float atan2(float y, float x)
void set_imu(float *acc, float *gyro, uint64_t time_us)
double quaternion_error(Eigen::Quaternionf q_eig, turbomath::Quaternion q)
uint64_t clock_micros() override
int main(int argc, char **argv)
void run()
Main loop for the ROSflight autopilot flight stack.
void step_firmware(rosflight_firmware::ROSflight &rf, rosflight_firmware::testBoard &board, uint32_t us)