7 #include <eigen3/unsupported/Eigen/MatrixFunctions> 15 using namespace Eigen;
46 gravity.z() = -9.80665;
64 oversampling_factor_ = 10;
66 ext_att_update_rate_ = 0;
72 void initFile(
const std::string& filename) { file_.open(filename); }
76 float acc[3],
gyro[3];
78 double max_error = 0.0;
82 for (
int i = 0;
i < oversampling_factor_;
i++)
85 w[0] = x_amp_ *
sin(x_freq_ / (2.0 *
M_PI) * t_);
86 w[1] = y_amp_ *
sin(y_freq_ / (2.0 *
M_PI) * t_);
87 w[2] = z_amp_ *
sin(z_freq_ / (2.0 *
M_PI) * t_);
88 integrate(q_, w, dt_ /
double(oversampling_factor_));
89 t_ += dt_ / double(oversampling_factor_);
92 simulateIMU(acc, gyro);
94 board.
set_imu(acc, gyro, t_ * 1e6);
97 Vector3d err = computeError();
99 double err_norm = err.norm();
104 max_error = (err_norm > max_error) ? err_norm : max_error;
109 void integrate(Quaterniond& q,
const Vector3d& _w,
double dt)
111 Vector3d w = _w * dt;
114 double w_norm = w.norm();
118 double scale =
std::sin(w_norm / 2.0) / w_norm;
119 w_exp.x() = scale * w(0);
120 w_exp.y() = scale * w(1);
121 w_exp.z() = scale * w(2);
127 w_exp.x() = w(0) / 2.0;
128 w_exp.y() = w(1) / 2.0;
129 w_exp.z() = w(2) / 2.0;
133 q.coeffs() *=
sign(q.w());
138 Vector3d y_acc = q_.inverse() * gravity;
144 gyro[0] = x_amp_ *
sin(x_freq_ / (2.0 *
M_PI) * t_) + x_gyro_bias_;
145 gyro[1] = y_amp_ *
sin(y_freq_ / (2.0 *
M_PI) * t_) + y_gyro_bias_;
146 gyro[2] = z_amp_ *
sin(z_freq_ / (2.0 *
M_PI) * t_) + z_gyro_bias_;
151 if (ext_att_update_rate_ && ++ext_att_count_ >= ext_att_update_rate_)
164 double sign(
double x) {
return x < 0 ? -1 : 1; }
171 Quaterniond err = q_ * rf_quat.inverse();
172 Vector3d v(err.x(), err.y(), err.z());
174 double norm_v = v.norm();
185 log_err = 2.0 *
std::atan(norm_v / w) * v / norm_v;
189 log_err = 2.0 *
sign(w) * v;
194 file_.write(reinterpret_cast<char*>(&t_),
sizeof(t_));
195 file_.write(reinterpret_cast<char*>(&q_),
sizeof(
double) * 4);
196 file_.write(reinterpret_cast<const char*>(&rf_quat),
sizeof(
double) * 4);
197 file_.write(reinterpret_cast<char*>(log_err.data()),
sizeof(
double) * 3);
198 file_.write(reinterpret_cast<char*>(eulerError().data()),
sizeof(
double) * 3);
199 file_.write(reinterpret_cast<const char*>(&rf.
estimator_.
bias().
x),
sizeof(
float) * 3);
207 Vector3d rpy = getTrueRPY();
222 return std::sqrt(xerr * xerr + yerr * yerr + zerr * zerr);
228 rpy(0) =
std::atan2(2.0
f * (q_.w() * q_.x() + q_.y() * q_.z()), 1.0
f - 2.0
f * (q_.x() * q_.x() + q_.y() * q_.y()));
229 rpy(1) =
std::asin(2.0
f * (q_.w() * q_.y() - q_.z() * q_.x()));
230 rpy(2) =
std::atan2(2.0
f * (q_.w() * q_.z() + q_.x() * q_.y()), 1.0
f - 2.0
f * (q_.y() * q_.y() + q_.z() * q_.z()));
245 initFile(
"linearGyro.bin");
247 double error = run();
248 EXPECT_LE(error, 1e-3);
251 std::cout <<
"error = " << error << std::endl;
265 initFile(
"quadGyro.bin");
267 double error = run();
268 EXPECT_LE(error, 1e-4);
271 std::cout <<
"error = " << error << std::endl;
285 initFile(
"expInt.bin");
287 double error = run();
288 EXPECT_LE(error, 3e-3);
291 std::cout <<
"error = " << error << std::endl;
305 initFile(
"expQuadInt.bin");
307 double error = run();
308 EXPECT_LE(error, 2e-3);
311 std::cout <<
"error = " << error << std::endl;
327 double error = run();
328 EXPECT_LE(error, 1e-2);
331 std::cout <<
"error = " << error << std::endl;
347 q_.
w() = q_tweaked.
w;
348 q_.x() = q_tweaked.
x;
349 q_.y() = q_tweaked.
y;
350 q_.z() = q_tweaked.
z;
363 oversampling_factor_ = 1;
366 initFile(
"estState.bin");
371 double rp_err = eulerError().head<2>().norm();
372 EXPECT_LE(rp_err, 1e-3);
373 EXPECT_LE(biasError(), 1e-3);
375 std::cout <<
"rp_err = " << rp_err << std::endl;
376 std::cout <<
"biasError = " << biasError() << std::endl;
392 q_.
w() = q_tweaked.
w;
393 q_.x() = q_tweaked.
x;
394 q_.y() = q_tweaked.
y;
395 q_.z() = q_tweaked.
z;
406 y_gyro_bias_ = -0.03;
409 oversampling_factor_ = 1;
412 initFile(
"estBias.bin");
417 double rp_err = eulerError().head<2>().norm();
418 EXPECT_LE(rp_err, 3e-3);
419 EXPECT_LE(biasError(), 2e-3);
421 std::cout <<
"rp_err = " << rp_err << std::endl;
422 std::cout <<
"biasError = " << biasError() << std::endl;
438 q_.
w() = q_tweaked.
w;
439 q_.x() = q_tweaked.
x;
440 q_.y() = q_tweaked.
y;
441 q_.z() = q_tweaked.
z;
452 y_gyro_bias_ = -0.03;
455 oversampling_factor_ = 1;
457 ext_att_update_rate_ = 3;
460 initFile(
"estStateExtAtt.bin");
464 double error = computeError().norm();
465 EXPECT_LE(error, 5e-3);
466 EXPECT_LE(biasError(), 2e-2);
468 std::cout <<
"stateError = " << error << std::endl;
469 std::cout <<
"biasError = " << biasError() << std::endl;
486 q_.
w() = q_tweaked.
w;
487 q_.x() = q_tweaked.
x;
488 q_.y() = q_tweaked.
y;
489 q_.z() = q_tweaked.
z;
500 y_gyro_bias_ = -0.03;
503 oversampling_factor_ = 1;
505 ext_att_update_rate_ = 3;
508 initFile(
"movingExtAtt.bin");
512 double error = computeError().norm();
513 EXPECT_LE(error, 4e-3);
514 EXPECT_LE(biasError(), 2e-2);
516 std::cout <<
"stateError = " << error << std::endl;
517 std::cout <<
"biasError = " << biasError() << std::endl;
static volatile int16_t gyro[3]
static volatile bool error
void simulateIMU(float *acc, float *gyro)
float atan2(float y, float x)
static double abs(double a)
void set_imu(float *acc, float *gyro, uint64_t time_us)
Quaternion & from_RPY(float roll, float pitch, float yaw)
const turbomath::Vector & bias()
static float sign(float x)
const State & state() const
void initFile(const std::string &filename)
void set_external_attitude_update(const turbomath::Quaternion &q)
void run()
Main loop for the ROSflight autopilot flight stack.
void set_time(uint64_t time_us)
turbomath::Quaternion attitude
TEST_F(EstimatorTest, LinearGyro)
void init()
Main initialization routine for the ROSflight autopilot flight stack.
void integrate(Quaterniond &q, const Vector3d &_w, double dt)