estimator_test.cpp
Go to the documentation of this file.
1 #include "common.h"
2 #include "math.h"
3 #include "rosflight.h"
4 #include "mavlink.h"
5 #include "test_board.h"
6 #include "eigen3/unsupported/Eigen/MatrixFunctions"
7 #include <cmath>
8 #include <fstream>
9 
10 //#define DEBUG
11 
12 using namespace rosflight_firmware;
13 
14 double sign(double y)
15 {
16  return (y > 0.0) - (y < 0.0);
17 }
18 
19 double run_estimator_test(std::string filename, ROSflight &rf, testBoard &board, std::vector<double> params)
20 {
21 #ifndef DEBUG
22  (void) filename;
23 #endif
24 
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];
32 
33  double dt = 0.001;
34 
35  Eigen::Vector3d gravity;
36  gravity << 0.0, 0.0, -9.80665;
37  Eigen::Matrix3d rotation = Eigen::Matrix3d::Identity();
38 
39 #ifdef DEBUG
40  // File for saving simulation results
41  std::ofstream file;
42  file.open(filename.c_str());
43 #endif
44 
45  double max_error = 0.0;
46  volatile double t = 0.0;
47  while (t < tmax)
48  {
49  // euler integration of S03 (probably a better way that isn't so intensive)
50  double ddt = 0.00005;
51  Eigen::Matrix3d omega_skew;
52  double step = t + dt;
53  while (t < step)
54  {
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);
58 
59  omega_skew << 0.0, -r, q,
60  r, 0.0, -p,
61  -q, p, 0.0;
62  rotation = rotation*(omega_skew*ddt).exp();
63  t += ddt;
64  }
65 
66  // Extract Accel Orientation
67  Eigen::Vector3d y_acc = rotation.transpose() * gravity;
68 
69  // Create gyro measurement
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);
73 
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)};
76 
77  // Simulate measurements
78  board.set_imu(acc, gyro, static_cast<uint64_t>(t*1e6));
79 
80  // Run firmware
81  rf.run();
82 
83  Eigen::Quaternionf eig_quat(rotation.cast<float>());
84  turbomath::Quaternion estimate = rf.estimator_.state().attitude;
85  if (eig_quat.w() < 0.0)
86  {
87  eig_quat.coeffs() *= -1.0;
88  }
89 
90  double pos_error = quaternion_error(eig_quat, estimate);
91  eig_quat.coeffs() *= -1.0;
92  double neg_error = quaternion_error(eig_quat, estimate);
93 
94  double error = 0.0;
95  if (pos_error < neg_error)
96  {
97  error = pos_error;
98  eig_quat.coeffs() *= -1.0;
99  }
100  else
101  {
102  error = neg_error;
103  }
104 
105  // output to file for plotting
106 #ifdef DEBUG
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() << ", ";
110  file << rf.estimator_.state().roll << ", " << rf.estimator_.state().pitch << ", " <<rf.estimator_.state().yaw << ", ";
111  file << rf.estimator_.state().angular_velocity.x << ", " << rf.estimator_.state().angular_velocity.y << ", " <<
112  rf.estimator_.state().angular_velocity.z << ", ";
113  file << p << ", " << q << ", " << r << ", ";
114  file << error << "\n";
115 #endif
116 
117  if (std::isfinite(error))
118  {
119  if (error > max_error)
120  max_error = error;
121  }
122  }
123  return max_error;
124 }
125 
126 
127 TEST(estimator_test, linear_gyro_integration)
128 {
129  testBoard board;
130  Mavlink mavlink(board);
131  ROSflight rf(board, mavlink);
132 
133  std::vector<double> params =
134  {
135  10.0, // xfreq
136  5.0, // yfreq
137  0.3, // zfreq
138  1.0, // xamp
139  0.75, // yamp
140  1.0, // zamp
141  30.0, // tmax
142  0.0003932 // error_limit
143  };
144 
145  // Initialize the firmware
146  rf.init();
147 
148  rf.params_.set_param_int(PARAM_FILTER_USE_ACC, false);
149  rf.params_.set_param_int(PARAM_FILTER_USE_QUAD_INT, false);
150  rf.params_.set_param_int(PARAM_FILTER_USE_MAT_EXP, false);
151  rf.params_.set_param_int(PARAM_ACC_ALPHA, 0);
152  rf.params_.set_param_int(PARAM_GYRO_XY_ALPHA, 0);
153  rf.params_.set_param_int(PARAM_GYRO_Z_ALPHA, 0);
154 
155  double max_error = run_estimator_test("linear_gyro_sim.csv", rf, board, params);
156 
157  EXPECT_LE(max_error, params[7]);
158 
159 #ifdef DEBUG
160  printf("max_error = %.7f\n", max_error);
161 #endif
162 }
163 
164 
165 TEST(estimator_test, quadratic_gyro_integration)
166 {
167  testBoard board;
168  Mavlink mavlink(board);
169  ROSflight rf(board, mavlink);
170 
171  std::vector<double> params =
172  {
173  10.0, // xfreq
174  0.1, // yfreq
175  0.5, // zfreq
176  1.5, // xamp
177  0.4, // yamp
178  1.0, // zamp
179  30.0, // tmax
180  0.0000363 // error_limit
181  };
182 
183 
184  // Initialize the firmware
185  rf.init();
186 
187  rf.params_.set_param_int(PARAM_FILTER_USE_ACC, false);
188  rf.params_.set_param_int(PARAM_FILTER_USE_QUAD_INT, true);
189  rf.params_.set_param_int(PARAM_FILTER_USE_MAT_EXP, false);
190  rf.params_.set_param_int(PARAM_ACC_ALPHA, 0);
191  rf.params_.set_param_int(PARAM_GYRO_XY_ALPHA, 0);
192  rf.params_.set_param_int(PARAM_GYRO_Z_ALPHA, 0);
193 
194  double max_error = run_estimator_test("quad_int_sim.csv", rf, board, params);
195 
196  EXPECT_LE(max_error, params[7]);
197 
198 #ifdef DEBUG
199  printf("max_error = %.7f\n", max_error);
200 #endif
201 }
202 
203 TEST(estimator_test, mat_exp_integration)
204 {
205  testBoard board;
206  Mavlink mavlink(board);
207  ROSflight rf(board, mavlink);
208 
209  std::vector<double> params =
210  {
211  10.0, // xfreq
212  0.1, // yfreq
213  0.5, // zfreq
214  1.5, // xamp
215  0.4, // yamp
216  1.0, // zamp
217  30.0, // tmax
218  0.0005029 // error_limit
219  };
220 
221  // Initialize the firmware
222  rf.init();
223 
224  rf.params_.set_param_int(PARAM_FILTER_USE_ACC, false);
225  rf.params_.set_param_int(PARAM_FILTER_USE_QUAD_INT, false);
226  rf.params_.set_param_int(PARAM_FILTER_USE_MAT_EXP, true);
227  rf.params_.set_param_int(PARAM_ACC_ALPHA, 0);
228  rf.params_.set_param_int(PARAM_GYRO_XY_ALPHA, 0);
229  rf.params_.set_param_int(PARAM_GYRO_Z_ALPHA, 0);
230 
231  double max_error = run_estimator_test("mat_exp_sim.csv", rf, board, params);
232  EXPECT_LE(max_error, params[7]);
233 #ifdef DEBUG
234  printf("max_error = %.7f\n", max_error);
235 #endif
236 }
237 
238 TEST(estimator_test, mat_exp_quad_int)
239 {
240  testBoard board;
241  Mavlink mavlink(board);
242  ROSflight rf(board, mavlink);
243 
244  std::vector<double> params =
245  {
246  10.0, // xfreq
247  0.1, // yfreq
248  0.5, // zfreq
249  1.5, // xamp
250  0.4, // yamp
251  1.0, // zamp
252  30.0, // tmax
253  0.0000403 // error_limit
254  };
255 
256  // Initialize the firmware
257  rf.init();
258 
259  rf.params_.set_param_int(PARAM_FILTER_USE_ACC, false);
260  rf.params_.set_param_int(PARAM_FILTER_USE_QUAD_INT, true);
261  rf.params_.set_param_int(PARAM_FILTER_USE_MAT_EXP, true);
262  rf.params_.set_param_int(PARAM_ACC_ALPHA, 0);
263  rf.params_.set_param_int(PARAM_GYRO_XY_ALPHA, 0);
264  rf.params_.set_param_int(PARAM_GYRO_Z_ALPHA, 0);
265 
266  double max_error = run_estimator_test("mat_exp_quad_sim.csv", rf, board, params);
267  EXPECT_LE(max_error, params[7]);
268 
269 #ifdef DEBUG
270  printf("max_error = %.7f\n", max_error);
271 #endif
272 }
273 
274 
275 TEST(estimator_test, accel)
276 {
277  testBoard board;
278  Mavlink mavlink(board);
279  ROSflight rf(board, mavlink);
280 
281  std::vector<double> params =
282  {
283  10.0, // xfreq
284  0.1, // yfreq
285  0.5, // zfreq
286  1.5, // xamp
287  0.4, // yamp
288  1.0, // zamp
289  30.0, // tmax
290  0.0280459 // error_limit
291  };
292 
293 
294  // Initialize the firmware
295  rf.init();
296 
297  rf.params_.set_param_int(PARAM_FILTER_USE_ACC, true);
298  rf.params_.set_param_int(PARAM_FILTER_USE_QUAD_INT, false);
299  rf.params_.set_param_int(PARAM_FILTER_USE_MAT_EXP, false);
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  rf.params_.set_param_int(PARAM_FILTER_KP, 3.0f);
304  rf.params_.set_param_int(PARAM_INIT_TIME, 0.0f);
305 
306  double max_error = run_estimator_test("accel_sim.csv", rf, board, params);
307  EXPECT_LE(max_error, params[7]);
308 #ifdef DEBUG
309  printf("max_error = %.7f\n", max_error);
310 #endif
311 }
312 
313 TEST(estimator_test, all_features)
314 {
315  testBoard board;
316  Mavlink mavlink(board);
317  ROSflight rf(board, mavlink);
318 
319  std::vector<double> params =
320  {
321  10.0, // xfreq
322  0.1, // yfreq
323  0.5, // zfreq
324  1.5, // xamp
325  0.4, // yamp
326  1.0, // zamp
327  30.0, // tmax
328  0.0632316 // error_limit
329  };
330 
331  // Initialize the firmware
332  rf.init();
333 
334  rf.params_.set_param_int(PARAM_FILTER_USE_ACC, true);
335  rf.params_.set_param_int(PARAM_FILTER_USE_QUAD_INT, true);
336  rf.params_.set_param_int(PARAM_FILTER_USE_MAT_EXP, true);
337  rf.params_.set_param_float(PARAM_FILTER_KP, 2.0f);
338  rf.params_.set_param_float(PARAM_ACC_ALPHA, 0.0f);
339  rf.params_.set_param_float(PARAM_GYRO_XY_ALPHA, 0.0f);
340  rf.params_.set_param_float(PARAM_GYRO_Z_ALPHA, 0.0f);
341  rf.params_.set_param_float(PARAM_GYRO_X_BIAS, 0.0);
342  rf.params_.set_param_float(PARAM_GYRO_Y_BIAS, 0.0);
343  rf.params_.set_param_float(PARAM_GYRO_Z_BIAS, 0.0); // We don't converge on z bias
344 
345  double max_error = run_estimator_test("full_estimator_sim.csv", rf, board, params);
346  EXPECT_LE(max_error, params[7]);
347 
348 #ifdef DEBUG
349  printf("max_error = %.7f\n", max_error);
350 #endif
351 }
352 
353 TEST(estimator_test, level_bias_sim)
354 {
355  testBoard board;
356  Mavlink mavlink(board);
357  ROSflight rf(board, mavlink);
358 
359 
360  std::vector<double> params =
361  {
362  0.0, // xfreq
363  0.0, // yfreq
364  0.0, // zfreq
365  0.0, // xamp
366  0.0, // yamp
367  0.0, // zamp
368  60.0, // tmax
369  0.0280459 // error_limit
370  };
371 
372  // Initialize the firmware
373  rf.init();
374 
375  turbomath::Vector true_bias = {0.25, -0.15, 0.0};
376 
377  rf.params_.set_param_int(PARAM_FILTER_USE_ACC, true);
378  rf.params_.set_param_int(PARAM_FILTER_USE_QUAD_INT, true);
379  rf.params_.set_param_int(PARAM_FILTER_USE_MAT_EXP, true);
380  rf.params_.set_param_float(PARAM_FILTER_KP, 2.0f);
381  rf.params_.set_param_float(PARAM_FILTER_KI, 0.2f);
382  rf.params_.set_param_float(PARAM_ACC_ALPHA, 0.0f);
383  rf.params_.set_param_float(PARAM_GYRO_XY_ALPHA, 0.0f);
384  rf.params_.set_param_float(PARAM_GYRO_Z_ALPHA, 0.0f);
385  rf.params_.set_param_float(PARAM_GYRO_X_BIAS, true_bias.x);
386  rf.params_.set_param_float(PARAM_GYRO_Y_BIAS, true_bias.y);
387  rf.params_.set_param_float(PARAM_GYRO_Z_BIAS, 0.0); // We don't converge on z bias
388 
389  run_estimator_test("level_bias_sim.csv", rf, board, params);
390 
391  // Check bias at the end
392  turbomath::Vector bias = rf.estimator_.state().angular_velocity - rf.sensors_.data().gyro;
393  turbomath::Vector error_vec = bias - true_bias;
394  float error_mag = error_vec.norm();
395  EXPECT_LE(error_mag, 0.001);
396 
397 #ifdef DEBUG
398  printf("estimated_bias = %.7f, %.7f\n", bias.x, bias.y);
399 #endif
400 }
401 
402 TEST(estimator_test, moving_bias_sim)
403 {
404  testBoard board;
405  Mavlink mavlink(board);
406  ROSflight rf(board, mavlink);
407 
408 
409  std::vector<double> params =
410  {
411  5.0, // xfreq
412  0.5, // yfreq
413  0.0, // zfreq
414  0.02, // xamp
415  0.01, // yamp
416  0.0, // zamp
417  60.0, // tmax
418  0.0280459 // error_limit
419  };
420 
421  // Initialize the firmware
422  rf.init();
423 
424  turbomath::Vector true_bias(0.01, -0.005, 0.0);
425 
426  rf.params_.set_param_int(PARAM_FILTER_USE_ACC, true);
427  rf.params_.set_param_int(PARAM_FILTER_USE_QUAD_INT, true);
428  rf.params_.set_param_int(PARAM_FILTER_USE_MAT_EXP, true);
429  rf.params_.set_param_float(PARAM_FILTER_KP, 0.5f);
430  rf.params_.set_param_float(PARAM_FILTER_KI, 0.05f);
431  rf.params_.set_param_float(PARAM_ACC_ALPHA, 0.0f);
432  rf.params_.set_param_float(PARAM_GYRO_XY_ALPHA, 0.0f);
433  rf.params_.set_param_float(PARAM_GYRO_Z_ALPHA, 0.0f);
434  rf.params_.set_param_float(PARAM_GYRO_X_BIAS, true_bias.x);
435  rf.params_.set_param_float(PARAM_GYRO_Y_BIAS, true_bias.y);
436  rf.params_.set_param_float(PARAM_GYRO_Z_BIAS, 0.0); // We don't converge on z bias
437 
438  run_estimator_test("moving_bias_sim.csv", rf, board, params);
439 
440  // Check bias at the end
441  turbomath::Vector bias = rf.estimator_.state().angular_velocity - rf.sensors_.data().gyro;
442  turbomath::Vector error_vec = bias - true_bias;
443  float error_mag = error_vec.norm();
444  EXPECT_LE(error_mag, params[7]);
445 
446 #ifdef DEBUG
447  printf("estimated_bias = %.7f, %.7f\n", bias.x, bias.y);
448 #endif
449 }
run_estimator_test
double run_estimator_test(std::string filename, ROSflight &rf, testBoard &board, std::vector< double > params)
Definition: estimator_test.cpp:19
rosflight_firmware::ROSflight::estimator_
Estimator estimator_
Definition: include/rosflight.h:66
gyro
static volatile int16_t gyro[3]
Definition: drv_mpu6050.c:277
rosflight_firmware::PARAM_GYRO_Z_BIAS
Definition: param.h:138
rosflight_firmware::PARAM_GYRO_XY_ALPHA
Definition: param.h:132
printf
#define printf
Definition: printf.h:119
error
static volatile bool error
Definition: drv_i2c.c:92
rosflight_firmware::PARAM_FILTER_KI
Definition: param.h:123
rosflight_firmware::testBoard
Definition: test_board.h:41
M_PI
#define M_PI
Definition: ublox.cpp:9
sign
double sign(double y)
Definition: estimator_test.cpp:14
turbomath::sin
float sin(float x)
Definition: turbomath.cpp:427
rosflight_firmware::testBoard::set_imu
void set_imu(float *acc, float *gyro, uint64_t time_us)
Definition: test_board.cpp:58
rosflight_firmware::ROSflight::sensors_
Sensors sensors_
Definition: include/rosflight.h:69
quaternion_error
double quaternion_error(Eigen::Quaternionf q_eig, turbomath::Quaternion q)
Definition: common.cpp:3
rosflight_firmware
Definition: airbourne_board.cpp:34
rosflight_firmware::Mavlink
Definition: mavlink.h:50
param_parser.params
list params
Definition: param_parser.py:18
rosflight_firmware::Params::set_param_float
bool set_param_float(uint16_t id, float value)
Sets the value of a floating point parameter by ID and calls the parameter callback.
Definition: param.cpp:389
rosflight_firmware::Estimator::State::roll
float roll
Definition: estimator.h:54
rosflight_firmware::Estimator::state
const State & state() const
Definition: estimator.h:62
test_board.h
accel
static volatile int16_t accel[3]
Definition: drv_mpu6050.c:276
rosflight_firmware::PARAM_FILTER_USE_ACC
Definition: param.h:128
rosflight_firmware::ROSflight::run
void run()
Main loop for the ROSflight autopilot flight stack.
Definition: rosflight.cpp:92
TEST
TEST(estimator_test, linear_gyro_integration)
Definition: estimator_test.cpp:127
rosflight_firmware::ROSflight
Definition: include/rosflight.h:50
rosflight_firmware::PARAM_GYRO_Z_ALPHA
Definition: param.h:133
rosflight_firmware::Estimator::State::yaw
float yaw
Definition: estimator.h:56
rosflight_firmware::PARAM_GYRO_Y_BIAS
Definition: param.h:137
rosflight_firmware::Estimator::State::pitch
float pitch
Definition: estimator.h:55
rosflight_firmware::PARAM_FILTER_KP
Definition: param.h:122
rosflight_firmware::Estimator::State::attitude
turbomath::Quaternion attitude
Definition: estimator.h:53
rosflight_firmware::PARAM_ACC_ALPHA
Definition: param.h:134
turbomath::Vector::norm
float norm() const
Definition: turbomath.cpp:58
rosflight_firmware::Estimator::State::angular_velocity
turbomath::Vector angular_velocity
Definition: estimator.h:52
rosflight_firmware::ROSflight::params_
Params params_
Definition: include/rosflight.h:62
param_parser.f
f
Definition: param_parser.py:12
rosflight_firmware::PARAM_INIT_TIME
Definition: param.h:121
rosflight_firmware::ROSflight::init
void init()
Main initialization routine for the ROSflight autopilot flight stack.
Definition: rosflight.cpp:52
rosflight_firmware::PARAM_FILTER_USE_MAT_EXP
Definition: param.h:127
rosflight_firmware::Sensors::data
const Data & data() const
Definition: sensors.h:160
rosflight_firmware::Params::set_param_int
bool set_param_int(uint16_t id, int32_t value)
Sets the value of a parameter by ID and calls the parameter change callback.
Definition: param.cpp:377
rosflight_firmware::PARAM_GYRO_X_BIAS
Definition: param.h:136
rosflight_firmware::Sensors::Data::gyro
turbomath::Vector gyro
Definition: sensors.h:126
rosflight_firmware::PARAM_FILTER_USE_QUAD_INT
Definition: param.h:126

rosflight_firmware
Author(s): Daniel Koch , James Jackson
autogenerated on Wed Jul 3 2019 19:59:24