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>());
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 
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 
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 
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 
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 
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 
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 
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
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 
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
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 }
double run_estimator_test(std::string filename, ROSflight &rf, testBoard &board, std::vector< double > params)
static volatile int16_t gyro[3]
Definition: drv_mpu6050.c:277
#define printf
Definition: printf.h:119
static volatile bool error
Definition: drv_i2c.c:92
double sign(double y)
float sin(float x)
Definition: turbomath.cpp:402
void set_imu(float *acc, float *gyro, uint64_t time_us)
Definition: test_board.cpp:58
double quaternion_error(Eigen::Quaternionf q_eig, turbomath::Quaternion q)
Definition: common.cpp:3
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:369
const State & state() const
Definition: estimator.h:64
static volatile int16_t accel[3]
Definition: drv_mpu6050.c:276
void run()
Main loop for the ROSflight autopilot flight stack.
Definition: rosflight.cpp:95
TEST(estimator_test, linear_gyro_integration)
turbomath::Quaternion attitude
Definition: estimator.h:55
float norm() const
Definition: turbomath.cpp:46
turbomath::Vector angular_velocity
Definition: estimator.h:54
#define M_PI
Definition: drv_mpu6050.c:28
void init()
Main initialization routine for the ROSflight autopilot flight stack.
Definition: rosflight.cpp:55
const Data & data() const
Definition: sensors.h:169
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:357


rosflight_firmware
Author(s): Daniel Koch , James Jackson
autogenerated on Thu Oct 24 2019 03:17:18