estimator_test.cpp
Go to the documentation of this file.
1 #include "common.h"
2 #include "mavlink.h"
3 #include "test_board.h"
4 
5 #include "rosflight.h"
6 
7 #include <eigen3/unsupported/Eigen/MatrixFunctions>
8 
9 #include <cmath>
10 #include <fstream>
11 
12 // #define DEBUG
13 
14 using namespace rosflight_firmware;
15 using namespace Eigen;
16 
17 class EstimatorTest : public ::testing::Test
18 {
19 public:
23 
24  std::ofstream file_;
25 
26  Vector3d gravity;
27  Quaterniond q_;
28 
29  double x_freq_, y_freq_, z_freq_;
30  double x_amp_, y_amp_, z_amp_;
31  double tmax_;
32  double x_gyro_bias_;
33  double y_gyro_bias_;
34  double z_gyro_bias_;
35  double t_, dt_;
39 
40  EstimatorTest() : mavlink(board), rf(board, mavlink) {}
41 
42  void SetUp() override
43  {
44  gravity.x() = 0.0;
45  gravity.y() = 0.0;
46  gravity.z() = -9.80665;
47  q_.w() = 1;
48  q_.x() = 0;
49  q_.y() = 0;
50  q_.z() = 0;
51 
52  x_freq_ = 10.0;
53  y_freq_ = 5.0;
54  z_freq_ = 0.3;
55  x_amp_ = 1.0;
56  y_amp_ = 0.75;
57  z_amp_ = 1.0;
58  dt_ = 0.001;
59  tmax_ = 30.0;
60  t_ = 0.0;
61  x_gyro_bias_ = 0.0;
62  y_gyro_bias_ = 0.0;
63  z_gyro_bias_ = 0.0;
64  oversampling_factor_ = 10;
65 
66  ext_att_update_rate_ = 0;
67  ext_att_count_ = 0;
68 
69  rf.init();
70  }
71 
72  void initFile(const std::string& filename) { file_.open(filename); }
73 
74  double run()
75  {
76  float acc[3], gyro[3];
77 
78  double max_error = 0.0;
79  t_ = 0.0;
80  while (t_ < tmax_)
81  {
82  for (int i = 0; i < oversampling_factor_; i++)
83  {
84  Vector3d w;
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_);
90  }
91 
92  simulateIMU(acc, gyro);
93  extAttUpdate();
94  board.set_imu(acc, gyro, t_ * 1e6);
95  board.set_time(t_ * 1e6);
96  rf.run();
97  Vector3d err = computeError();
98 
99  double err_norm = err.norm();
100  if (std::abs(err_norm - 2.0 * M_PI) < err_norm)
101  {
102  err_norm = std::abs(err_norm - 2.0 * M_PI);
103  }
104  max_error = (err_norm > max_error) ? err_norm : max_error;
105  }
106  return max_error;
107  }
108 
109  void integrate(Quaterniond& q, const Vector3d& _w, double dt)
110  {
111  Vector3d w = _w * dt;
112 
113  Quaterniond w_exp;
114  double w_norm = w.norm();
115  if (w_norm > 1e-4)
116  {
117  w_exp.w() = std::cos(w_norm / 2.0);
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);
122  w_exp.normalize();
123  }
124  else
125  {
126  w_exp.w() = 1.0;
127  w_exp.x() = w(0) / 2.0;
128  w_exp.y() = w(1) / 2.0;
129  w_exp.z() = w(2) / 2.0;
130  w_exp.normalize();
131  }
132  q = q * w_exp;
133  q.coeffs() *= sign(q.w());
134  }
135 
136  void simulateIMU(float* acc, float* gyro)
137  {
138  Vector3d y_acc = q_.inverse() * gravity;
139  acc[0] = y_acc.x();
140  acc[1] = y_acc.y();
141  acc[2] = y_acc.z();
142 
143  // Create gyro measurement
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_;
147  }
148 
150  {
151  if (ext_att_update_rate_ && ++ext_att_count_ >= ext_att_update_rate_)
152  {
153  ext_att_count_ = 0;
154  turbomath::Quaternion q_ext;
155  q_ext.w = q_.w();
156  q_ext.x = q_.x();
157  q_ext.y = q_.y();
158  q_ext.z = q_.z();
159 
161  }
162  }
163 
164  double sign(double x) { return x < 0 ? -1 : 1; }
165 
166  Vector3d computeError()
167  {
168  Quaterniond rf_quat(rf.estimator_.state().attitude.w, rf.estimator_.state().attitude.x,
170 
171  Quaterniond err = q_ * rf_quat.inverse();
172  Vector3d v(err.x(), err.y(), err.z());
173  float w = err.w();
174  double norm_v = v.norm();
175 
176  if (t_ > 20.698)
177  {
178  int debug = 1;
179  (void)debug;
180  }
181 
182  Vector3d log_err;
183  if (norm_v > 1e-4)
184  {
185  log_err = 2.0 * std::atan(norm_v / w) * v / norm_v;
186  }
187  else
188  {
189  log_err = 2.0 * sign(w) * v;
190  }
191 
192  if (file_.is_open())
193  {
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);
200  }
201 
202  return log_err;
203  }
204 
205  Vector3d eulerError()
206  {
207  Vector3d rpy = getTrueRPY();
208 
209  Vector3d err;
210  err(0) = rpy(0) - rf.estimator_.state().roll;
211  err(1) = rpy(1) - rf.estimator_.state().pitch;
212  err(2) = rpy(2) - rf.estimator_.state().yaw;
213 
214  return err;
215  }
216 
217  double biasError()
218  {
219  double xerr = x_gyro_bias_ - rf.estimator_.bias().x;
220  double yerr = y_gyro_bias_ - rf.estimator_.bias().y;
221  double zerr = z_gyro_bias_ - rf.estimator_.bias().z;
222  return std::sqrt(xerr * xerr + yerr * yerr + zerr * zerr);
223  }
224 
225  Vector3d getTrueRPY()
226  {
227  Vector3d rpy;
228  rpy(0) = std::atan2(2.0f * (q_.w() * q_.x() + q_.y() * q_.z()), 1.0f - 2.0f * (q_.x() * q_.x() + q_.y() * q_.y()));
229  rpy(1) = std::asin(2.0f * (q_.w() * q_.y() - q_.z() * q_.x()));
230  rpy(2) = std::atan2(2.0f * (q_.w() * q_.z() + q_.x() * q_.y()), 1.0f - 2.0f * (q_.y() * q_.y() + q_.z() * q_.z()));
231  return rpy;
232  }
233 };
234 
235 TEST_F(EstimatorTest, LinearGyro)
236 {
237  rf.params_.set_param_int(PARAM_FILTER_USE_ACC, false);
238  rf.params_.set_param_int(PARAM_FILTER_USE_QUAD_INT, false);
239  rf.params_.set_param_int(PARAM_FILTER_USE_MAT_EXP, false);
240  rf.params_.set_param_int(PARAM_ACC_ALPHA, 0);
241  rf.params_.set_param_int(PARAM_GYRO_XY_ALPHA, 0);
242  rf.params_.set_param_int(PARAM_GYRO_Z_ALPHA, 0);
243 
244 #ifdef DEBUG
245  initFile("linearGyro.bin");
246 #endif
247  double error = run();
248  EXPECT_LE(error, 1e-3);
249 
250 #ifdef DEBUG
251  std::cout << "error = " << error << std::endl;
252 #endif
253 }
254 
255 TEST_F(EstimatorTest, QuadraticGyro)
256 {
257  rf.params_.set_param_int(PARAM_FILTER_USE_ACC, false);
258  rf.params_.set_param_int(PARAM_FILTER_USE_QUAD_INT, true);
259  rf.params_.set_param_int(PARAM_FILTER_USE_MAT_EXP, false);
260  rf.params_.set_param_int(PARAM_ACC_ALPHA, 0);
261  rf.params_.set_param_int(PARAM_GYRO_XY_ALPHA, 0);
262  rf.params_.set_param_int(PARAM_GYRO_Z_ALPHA, 0);
263 
264 #ifdef DEBUG
265  initFile("quadGyro.bin");
266 #endif
267  double error = run();
268  EXPECT_LE(error, 1e-4);
269 
270 #ifdef DEBUG
271  std::cout << "error = " << error << std::endl;
272 #endif
273 }
274 
276 {
277  rf.params_.set_param_int(PARAM_FILTER_USE_ACC, false);
278  rf.params_.set_param_int(PARAM_FILTER_USE_QUAD_INT, false);
279  rf.params_.set_param_int(PARAM_FILTER_USE_MAT_EXP, true);
280  rf.params_.set_param_int(PARAM_ACC_ALPHA, 0);
281  rf.params_.set_param_int(PARAM_GYRO_XY_ALPHA, 0);
282  rf.params_.set_param_int(PARAM_GYRO_Z_ALPHA, 0);
283 
284 #ifdef DEBUG
285  initFile("expInt.bin");
286 #endif
287  double error = run();
288  EXPECT_LE(error, 3e-3);
289 
290 #ifdef DEBUG
291  std::cout << "error = " << error << std::endl;
292 #endif
293 }
294 
295 TEST_F(EstimatorTest, MatrixExpQuadInt)
296 {
297  rf.params_.set_param_int(PARAM_FILTER_USE_ACC, false);
298  rf.params_.set_param_int(PARAM_FILTER_USE_QUAD_INT, true);
299  rf.params_.set_param_int(PARAM_FILTER_USE_MAT_EXP, true);
300  rf.params_.set_param_int(PARAM_ACC_ALPHA, 0);
301  rf.params_.set_param_int(PARAM_GYRO_XY_ALPHA, 0);
302  rf.params_.set_param_int(PARAM_GYRO_Z_ALPHA, 0);
303 
304 #ifdef DEBUG
305  initFile("expQuadInt.bin");
306 #endif
307  double error = run();
308  EXPECT_LE(error, 2e-3);
309 
310 #ifdef DEBUG
311  std::cout << "error = " << error << std::endl;
312 #endif
313 }
314 
316 {
317  rf.params_.set_param_int(PARAM_FILTER_USE_ACC, true);
318  rf.params_.set_param_int(PARAM_FILTER_USE_QUAD_INT, true);
319  rf.params_.set_param_int(PARAM_FILTER_USE_MAT_EXP, true);
320  rf.params_.set_param_int(PARAM_ACC_ALPHA, 0);
321  rf.params_.set_param_int(PARAM_GYRO_XY_ALPHA, 0);
322  rf.params_.set_param_int(PARAM_GYRO_Z_ALPHA, 0);
323 
324 #ifdef DEBUG
325  initFile("acc.bin");
326 #endif
327  double error = run();
328  EXPECT_LE(error, 1e-2);
329 
330 #ifdef DEBUG
331  std::cout << "error = " << error << std::endl;
332 #endif
333 }
334 
335 TEST_F(EstimatorTest, EstimateStateAccel)
336 {
337  rf.params_.set_param_int(PARAM_FILTER_USE_ACC, true);
338  rf.params_.set_param_int(PARAM_FILTER_USE_QUAD_INT, true);
339  rf.params_.set_param_int(PARAM_FILTER_USE_MAT_EXP, true);
340  rf.params_.set_param_int(PARAM_ACC_ALPHA, 0);
341  rf.params_.set_param_int(PARAM_GYRO_XY_ALPHA, 0);
342  rf.params_.set_param_int(PARAM_GYRO_Z_ALPHA, 0);
343  rf.params_.set_param_int(PARAM_INIT_TIME, 0.0f);
344 
345  turbomath::Quaternion q_tweaked;
346  q_tweaked.from_RPY(0.2, 0.1, 0.0);
347  q_.w() = q_tweaked.w;
348  q_.x() = q_tweaked.x;
349  q_.y() = q_tweaked.y;
350  q_.z() = q_tweaked.z;
351 
352  x_freq_ = 0.0;
353  y_freq_ = 0.0;
354  z_freq_ = 0.0;
355  x_amp_ = 0.0;
356  y_amp_ = 0.0;
357  z_amp_ = 0.0;
358 
359  tmax_ = 150.0;
360  x_gyro_bias_ = 0.00;
361  y_gyro_bias_ = 0.00;
362 
363  oversampling_factor_ = 1;
364 
365 #ifdef DEBUG
366  initFile("estState.bin");
367 #endif
368  run();
369 
370  // only take error of roll and pitch because yaw is unobservable
371  double rp_err = eulerError().head<2>().norm();
372  EXPECT_LE(rp_err, 1e-3);
373  EXPECT_LE(biasError(), 1e-3);
374 #ifdef DEBUG
375  std::cout << "rp_err = " << rp_err << std::endl;
376  std::cout << "biasError = " << biasError() << std::endl;
377 #endif
378 }
379 
380 TEST_F(EstimatorTest, EstimateBiasAccel)
381 {
382  rf.params_.set_param_int(PARAM_FILTER_USE_ACC, true);
383  rf.params_.set_param_int(PARAM_FILTER_USE_QUAD_INT, true);
384  rf.params_.set_param_int(PARAM_FILTER_USE_MAT_EXP, true);
385  rf.params_.set_param_int(PARAM_ACC_ALPHA, 0);
386  rf.params_.set_param_int(PARAM_GYRO_XY_ALPHA, 0);
387  rf.params_.set_param_int(PARAM_GYRO_Z_ALPHA, 0);
388  rf.params_.set_param_int(PARAM_INIT_TIME, 0.0f);
389 
390  turbomath::Quaternion q_tweaked;
391  q_tweaked.from_RPY(0.2, 0.1, 0.0);
392  q_.w() = q_tweaked.w;
393  q_.x() = q_tweaked.x;
394  q_.y() = q_tweaked.y;
395  q_.z() = q_tweaked.z;
396 
397  x_freq_ = 0.0;
398  y_freq_ = 0.0;
399  z_freq_ = 0.0;
400  x_amp_ = 0.0;
401  y_amp_ = 0.0;
402  z_amp_ = 0.0;
403 
404  tmax_ = 150.0;
405  x_gyro_bias_ = 0.01;
406  y_gyro_bias_ = -0.03;
407  z_gyro_bias_ = 0.00;
408 
409  oversampling_factor_ = 1;
410 
411 #ifdef DEBUG
412  initFile("estBias.bin");
413 #endif
414  run();
415 
416  // only take error of roll and pitch because yaw is unobservable
417  double rp_err = eulerError().head<2>().norm();
418  EXPECT_LE(rp_err, 3e-3);
419  EXPECT_LE(biasError(), 2e-3);
420 #ifdef DEBUG
421  std::cout << "rp_err = " << rp_err << std::endl;
422  std::cout << "biasError = " << biasError() << std::endl;
423 #endif
424 }
425 
426 TEST_F(EstimatorTest, StaticExtAtt)
427 {
428  rf.params_.set_param_int(PARAM_FILTER_USE_ACC, false);
429  rf.params_.set_param_int(PARAM_FILTER_USE_QUAD_INT, true);
430  rf.params_.set_param_int(PARAM_FILTER_USE_MAT_EXP, true);
431  rf.params_.set_param_int(PARAM_ACC_ALPHA, 0);
432  rf.params_.set_param_int(PARAM_GYRO_XY_ALPHA, 0);
433  rf.params_.set_param_int(PARAM_GYRO_Z_ALPHA, 0);
434  rf.params_.set_param_int(PARAM_INIT_TIME, 0.0f);
435 
436  turbomath::Quaternion q_tweaked;
437  q_tweaked.from_RPY(0.2, 0.1, 0.0);
438  q_.w() = q_tweaked.w;
439  q_.x() = q_tweaked.x;
440  q_.y() = q_tweaked.y;
441  q_.z() = q_tweaked.z;
442 
443  x_freq_ = 0.0;
444  y_freq_ = 0.0;
445  z_freq_ = 0.0;
446  x_amp_ = 0.0;
447  y_amp_ = 0.0;
448  z_amp_ = 0.0;
449 
450  tmax_ = 150.0;
451  x_gyro_bias_ = 0.01;
452  y_gyro_bias_ = -0.03;
453  z_gyro_bias_ = 0.01;
454 
455  oversampling_factor_ = 1;
456 
457  ext_att_update_rate_ = 3;
458 
459 #ifdef DEBUG
460  initFile("estStateExtAtt.bin");
461 #endif
462  run();
463 
464  double error = computeError().norm();
465  EXPECT_LE(error, 5e-3);
466  EXPECT_LE(biasError(), 2e-2);
467 #ifdef DEBUG
468  std::cout << "stateError = " << error << std::endl;
469  std::cout << "biasError = " << biasError() << std::endl;
470 #endif
471 }
472 
473 // This test is fixed by #357
474 TEST_F(EstimatorTest, MovingExtAtt)
475 {
476  rf.params_.set_param_int(PARAM_FILTER_USE_ACC, false);
477  rf.params_.set_param_int(PARAM_FILTER_USE_QUAD_INT, true);
478  rf.params_.set_param_int(PARAM_FILTER_USE_MAT_EXP, true);
479  rf.params_.set_param_int(PARAM_ACC_ALPHA, 0);
480  rf.params_.set_param_int(PARAM_GYRO_XY_ALPHA, 0);
481  rf.params_.set_param_int(PARAM_GYRO_Z_ALPHA, 0);
482  rf.params_.set_param_int(PARAM_INIT_TIME, 0.0f);
483 
484  turbomath::Quaternion q_tweaked;
485  q_tweaked.from_RPY(0.2, 0.1, 0.0);
486  q_.w() = q_tweaked.w;
487  q_.x() = q_tweaked.x;
488  q_.y() = q_tweaked.y;
489  q_.z() = q_tweaked.z;
490 
491  x_freq_ = 2.0;
492  y_freq_ = 3.0;
493  z_freq_ = 0.5;
494  x_amp_ = 0.1;
495  y_amp_ = 0.2;
496  z_amp_ = -0.1;
497 
498  tmax_ = 150.0;
499  x_gyro_bias_ = 0.01;
500  y_gyro_bias_ = -0.03;
501  z_gyro_bias_ = 0.01;
502 
503  oversampling_factor_ = 1;
504 
505  ext_att_update_rate_ = 3;
506 
507 #ifdef DEBUG
508  initFile("movingExtAtt.bin");
509 #endif
510  run();
511 
512  double error = computeError().norm();
513  EXPECT_LE(error, 4e-3);
514  EXPECT_LE(biasError(), 2e-2);
515 #ifdef DEBUG
516  std::cout << "stateError = " << error << std::endl;
517  std::cout << "biasError = " << biasError() << std::endl;
518 #endif
519 }
static volatile int16_t gyro[3]
Definition: drv_mpu6050.c:277
Vector3d eulerError()
static volatile bool error
Definition: drv_i2c.c:92
float atan(float x)
Definition: turbomath.cpp:383
void simulateIMU(float *acc, float *gyro)
std::ofstream file_
float cos(float x)
Definition: turbomath.cpp:348
float sin(float x)
Definition: turbomath.cpp:353
float atan2(float y, float x)
Definition: turbomath.cpp:410
static double abs(double a)
Definition: printf.cpp:42
void set_imu(float *acc, float *gyro, uint64_t time_us)
Definition: test_board.cpp:57
Quaternion & from_RPY(float roll, float pitch, float yaw)
Definition: turbomath.cpp:231
Vector3d computeError()
const turbomath::Vector & bias()
Definition: estimator.h:64
static float sign(float x)
Definition: drv_ms4525.c:35
const State & state() const
Definition: estimator.h:62
void initFile(const std::string &filename)
float asin(float x)
Definition: turbomath.cpp:449
void set_external_attitude_update(const turbomath::Quaternion &q)
Definition: estimator.cpp:119
void run()
Main loop for the ROSflight autopilot flight stack.
Definition: rosflight.cpp:100
double sign(double x)
void set_time(uint64_t time_us)
Definition: test_board.cpp:47
Vector3d getTrueRPY()
turbomath::Quaternion attitude
Definition: estimator.h:53
TEST_F(EstimatorTest, LinearGyro)
void SetUp() override
#define M_PI
Definition: drv_mpu6050.c:28
void init()
Main initialization routine for the ROSflight autopilot flight stack.
Definition: rosflight.cpp:55
void integrate(Quaterniond &q, const Vector3d &_w, double dt)
Quaterniond q_


rosflight_firmware
Author(s): Daniel Koch , James Jackson
autogenerated on Thu Apr 15 2021 05:07:46