6 #include "eigen3/unsupported/Eigen/MatrixFunctions" 16 return (y > 0.0) - (y < 0.0);
25 double x_freq = params[0];
26 double y_freq = params[1];
27 double z_freq = params[2];
28 double x_amp = params[3];
29 double y_amp = params[4];
30 double z_amp = params[5];
31 double tmax = params[6];
35 Eigen::Vector3d gravity;
36 gravity << 0.0, 0.0, -9.80665;
37 Eigen::Matrix3d rotation = Eigen::Matrix3d::Identity();
42 file.open(filename.c_str());
45 double max_error = 0.0;
46 volatile double t = 0.0;
51 Eigen::Matrix3d omega_skew;
55 double p = x_amp*
sin(x_freq/(2.0*
M_PI)*t);
56 double q = y_amp*
sin(y_freq/(2.0*
M_PI)*t);
57 double r = z_amp*
sin(z_freq/(2.0*
M_PI)*t);
59 omega_skew << 0.0, -r, q,
62 rotation = rotation*(omega_skew*ddt).exp();
67 Eigen::Vector3d y_acc = rotation.transpose() * gravity;
70 double p = x_amp*
sin(x_freq/(2.0*
M_PI)*t);
71 double q = y_amp*
sin(y_freq/(2.0*
M_PI)*t);
72 double r = z_amp*
sin(z_freq/(2.0*
M_PI)*t);
74 float acc[3] = {
static_cast<float>(y_acc(0)), static_cast<float>(y_acc(1)), static_cast<float>(y_acc(2))};
75 float gyro[3] = {
static_cast<float>(p), static_cast<float>(q),
static_cast<float>(r)};
78 board.
set_imu(acc, gyro, static_cast<uint64_t>(t*1e6));
83 Eigen::Quaternionf eig_quat(rotation.cast<
float>());
85 if (eig_quat.w() < 0.0)
87 eig_quat.coeffs() *= -1.0;
91 eig_quat.coeffs() *= -1.0;
95 if (pos_error < neg_error)
98 eig_quat.coeffs() *= -1.0;
107 file << t <<
", " << (error > error_limit) <<
", ";
108 file << estimate.
w <<
", " << estimate.
x <<
", " << estimate.
y <<
", " << estimate.
z <<
", ";
109 file << eig_quat.w() <<
", " << eig_quat.x() <<
", " << eig_quat.y() <<
", " << eig_quat.z() <<
", ";
113 file << p <<
", " << q <<
", " << r <<
", ";
114 file << error <<
"\n";
117 if (std::isfinite(error))
119 if (error > max_error)
127 TEST(estimator_test, linear_gyro_integration)
133 std::vector<double>
params =
157 EXPECT_LE(max_error, params[7]);
160 printf(
"max_error = %.7f\n", max_error);
165 TEST(estimator_test, quadratic_gyro_integration)
171 std::vector<double>
params =
196 EXPECT_LE(max_error, params[7]);
199 printf(
"max_error = %.7f\n", max_error);
203 TEST(estimator_test, mat_exp_integration)
209 std::vector<double>
params =
232 EXPECT_LE(max_error, params[7]);
234 printf(
"max_error = %.7f\n", max_error);
238 TEST(estimator_test, mat_exp_quad_int)
244 std::vector<double>
params =
267 EXPECT_LE(max_error, params[7]);
270 printf(
"max_error = %.7f\n", max_error);
281 std::vector<double>
params =
307 EXPECT_LE(max_error, params[7]);
309 printf(
"max_error = %.7f\n", max_error);
313 TEST(estimator_test, all_features)
319 std::vector<double>
params =
346 EXPECT_LE(max_error, params[7]);
349 printf(
"max_error = %.7f\n", max_error);
353 TEST(estimator_test, level_bias_sim)
360 std::vector<double>
params =
394 float error_mag = error_vec.
norm();
395 EXPECT_LE(error_mag, 0.001);
398 printf(
"estimated_bias = %.7f, %.7f\n", bias.
x, bias.
y);
402 TEST(estimator_test, moving_bias_sim)
409 std::vector<double>
params =
443 float error_mag = error_vec.
norm();
444 EXPECT_LE(error_mag, params[7]);
447 printf(
"estimated_bias = %.7f, %.7f\n", bias.
x, bias.
y);
double run_estimator_test(std::string filename, ROSflight &rf, testBoard &board, std::vector< double > params)
static volatile int16_t gyro[3]
static volatile bool error
void set_imu(float *acc, float *gyro, uint64_t time_us)
double quaternion_error(Eigen::Quaternionf q_eig, turbomath::Quaternion q)
bool set_param_float(uint16_t id, float value)
Sets the value of a floating point parameter by ID and calls the parameter callback.
const State & state() const
static volatile int16_t accel[3]
void run()
Main loop for the ROSflight autopilot flight stack.
TEST(estimator_test, linear_gyro_integration)
turbomath::Quaternion attitude
turbomath::Vector angular_velocity
void init()
Main initialization routine for the ROSflight autopilot flight stack.
const Data & data() const
bool set_param_int(uint16_t id, int32_t value)
Sets the value of a parameter by ID and calls the parameter change callback.