![](/media/cache/group/default_image.jpg.50x50_q85.jpg)
Testing ekf implementation for Quadro_1.
main.cpp@11:6a7c3795d8fb, 2019-10-21 (annotated)
- Committer:
- pmic
- Date:
- Mon Oct 21 09:20:25 2019 +0000
- Revision:
- 11:6a7c3795d8fb
- Parent:
- 10:dd5ea359fb97
- Child:
- 12:180e09c4ea26
Make sure right branch.
Who changed what in which revision?
User | Revision | Line number | New contents of line |
---|---|---|---|
pmic | 0:a0e9705be9c4 | 1 | #include "mbed.h" |
pmic | 1:6b803652d032 | 2 | #include "iostream" |
pmic | 0:a0e9705be9c4 | 3 | //#include "Eigen/Dense.h" |
pmic | 0:a0e9705be9c4 | 4 | #include "Eigen/Core.h" |
pmic | 0:a0e9705be9c4 | 5 | #include "Eigen/Geometry.h" |
pmic | 0:a0e9705be9c4 | 6 | #include "EKF_RP.h" |
pmic | 7:bcbcc23983de | 7 | #include "EKF_RPY.h" |
pmic | 0:a0e9705be9c4 | 8 | |
pmic | 0:a0e9705be9c4 | 9 | using namespace Eigen; |
pmic | 0:a0e9705be9c4 | 10 | |
pmic | 0:a0e9705be9c4 | 11 | Serial pc(SERIAL_TX, SERIAL_RX); |
pmic | 0:a0e9705be9c4 | 12 | |
pmic | 6:f9569a07aff5 | 13 | EKF_RP ekf_rp(0.02f); |
pmic | 7:bcbcc23983de | 14 | EKF_RPY ekf_rpy(0.02f, 0.1554364f, -0.0879864f, 0.3507829f); |
pmic | 0:a0e9705be9c4 | 15 | |
pmic | 0:a0e9705be9c4 | 16 | Timer timer; // timer for time measurement |
pmic | 0:a0e9705be9c4 | 17 | float dt = 0.0f; |
pmic | 0:a0e9705be9c4 | 18 | |
pmic | 4:e50e18eac72b | 19 | uint32_t i; |
pmic | 0:a0e9705be9c4 | 20 | |
pmic | 1:6b803652d032 | 21 | Matrix<float, 8, 8> A; |
pmic | 1:6b803652d032 | 22 | Matrix<float, 8, 1> b; |
pmic | 11:6a7c3795d8fb | 23 | Matrix<float, 9, 9> I; |
pmic | 0:a0e9705be9c4 | 24 | |
pmic | 4:e50e18eac72b | 25 | Matrix<float, 4, 1> gyro_x; |
pmic | 4:e50e18eac72b | 26 | Matrix<float, 4, 1> gyro_y; |
pmic | 7:bcbcc23983de | 27 | Matrix<float, 4, 1> gyro_z; |
pmic | 4:e50e18eac72b | 28 | Matrix<float, 4, 1> accel_x; |
pmic | 4:e50e18eac72b | 29 | Matrix<float, 4, 1> accel_y; |
pmic | 7:bcbcc23983de | 30 | Matrix<float, 4, 1> magnet_x; |
pmic | 7:bcbcc23983de | 31 | Matrix<float, 4, 1> magnet_y; |
pmic | 4:e50e18eac72b | 32 | |
pmic | 0:a0e9705be9c4 | 33 | int main() |
pmic | 0:a0e9705be9c4 | 34 | { |
pmic | 0:a0e9705be9c4 | 35 | pc.baud(2000000); |
pmic | 0:a0e9705be9c4 | 36 | |
pmic | 0:a0e9705be9c4 | 37 | timer.start(); |
pmic | 0:a0e9705be9c4 | 38 | |
pmic | 4:e50e18eac72b | 39 | i = 0; |
pmic | 0:a0e9705be9c4 | 40 | |
pmic | 1:6b803652d032 | 41 | /* |
pmic | 1:6b803652d032 | 42 | A << 1, 2, 3, 5, 1, 8,10, 1, 3, |
pmic | 1:6b803652d032 | 43 | 4, 5, 6, 8, 4, 2, 1, 9, 4, |
pmic | 1:6b803652d032 | 44 | 7, 8,10, 5, 6, 8, 4, 5, 1, |
pmic | 1:6b803652d032 | 45 | 4, 2,10, 8,10, 5, 6, 7, 8, |
pmic | 1:6b803652d032 | 46 | 1, 8, 7, 3, 4, 6, 5, 1, 7, |
pmic | 1:6b803652d032 | 47 | 4, 2, 7, 5, 7, 6, 9, 2, 1, |
pmic | 1:6b803652d032 | 48 | 5, 5, 1, 7, 4, 2, 1, 1, 9, |
pmic | 1:6b803652d032 | 49 | 8, 9, 7, 4, 5, 6, 1, 2, 2, |
pmic | 1:6b803652d032 | 50 | 1, 5, 9, 4, 8, 7, 2, 6, 3; |
pmic | 0:a0e9705be9c4 | 51 | b << 3, |
pmic | 0:a0e9705be9c4 | 52 | 3, |
pmic | 1:6b803652d032 | 53 | 4, |
pmic | 1:6b803652d032 | 54 | 2, |
pmic | 1:6b803652d032 | 55 | 7, |
pmic | 1:6b803652d032 | 56 | 5, |
pmic | 1:6b803652d032 | 57 | 1, |
pmic | 1:6b803652d032 | 58 | 8, |
pmic | 1:6b803652d032 | 59 | 1; |
pmic | 1:6b803652d032 | 60 | */ |
pmic | 1:6b803652d032 | 61 | |
pmic | 1:6b803652d032 | 62 | A << 1, 2, 3, 5, 1, 8,10, 1, |
pmic | 1:6b803652d032 | 63 | 4, 5, 6, 8, 4, 2, 1, 9, |
pmic | 1:6b803652d032 | 64 | 7, 8,10, 5, 6, 8, 4, 5, |
pmic | 1:6b803652d032 | 65 | 4, 2,10, 8,10, 5, 6, 7, |
pmic | 1:6b803652d032 | 66 | 1, 8, 7, 3, 4, 6, 5, 1, |
pmic | 1:6b803652d032 | 67 | 4, 2, 7, 5, 7, 6, 9, 2, |
pmic | 1:6b803652d032 | 68 | 5, 5, 1, 7, 4, 2, 1, 1, |
pmic | 1:6b803652d032 | 69 | 8, 9, 7, 4, 5, 6, 1, 2; |
pmic | 1:6b803652d032 | 70 | b << 3, |
pmic | 1:6b803652d032 | 71 | 3, |
pmic | 1:6b803652d032 | 72 | 4, |
pmic | 1:6b803652d032 | 73 | 2, |
pmic | 1:6b803652d032 | 74 | 7, |
pmic | 1:6b803652d032 | 75 | 5, |
pmic | 1:6b803652d032 | 76 | 1, |
pmic | 1:6b803652d032 | 77 | 8; |
pmic | 0:a0e9705be9c4 | 78 | |
pmic | 10:dd5ea359fb97 | 79 | I.setIdentity(); |
pmic | 4:e50e18eac72b | 80 | |
pmic | 7:bcbcc23983de | 81 | gyro_x << 0.01, 0.05, -0.02, 0.02; |
pmic | 7:bcbcc23983de | 82 | gyro_y << -0.01, -0.03, 0.01, 0.01; |
pmic | 7:bcbcc23983de | 83 | gyro_z << 0.05, 0.04, -0.03, 0.02; |
pmic | 7:bcbcc23983de | 84 | accel_x << 0.03, -0.01, 0.07, -0.03; |
pmic | 7:bcbcc23983de | 85 | accel_y << 0.01, 0.05, -0.04, -0.02; |
pmic | 7:bcbcc23983de | 86 | magnet_x << 0.01, -0.02, -0.08, 0.02; |
pmic | 7:bcbcc23983de | 87 | magnet_y << 0.06, -0.04, 0.04, 0.07; |
pmic | 4:e50e18eac72b | 88 | |
pmic | 4:e50e18eac72b | 89 | pc.printf("\r\n"); |
pmic | 1:6b803652d032 | 90 | |
pmic | 0:a0e9705be9c4 | 91 | while(1) { |
pmic | 0:a0e9705be9c4 | 92 | |
pmic | 3:121647a7cddf | 93 | // Matrix<float, 8, 1> x = A.inverse() * b; |
pmic | 0:a0e9705be9c4 | 94 | |
pmic | 4:e50e18eac72b | 95 | // float temp0 = (float)(rand()%10000-4999)*0.00001f; |
pmic | 4:e50e18eac72b | 96 | // float temp1 = (float)(rand()%10000-4999)*0.00001f; |
pmic | 4:e50e18eac72b | 97 | // float temp2 = (float)(rand()%10000-4999)*0.00001f; |
pmic | 4:e50e18eac72b | 98 | // float temp3 = (float)(rand()%10000-4999)*0.00001f; |
pmic | 4:e50e18eac72b | 99 | // float temp4 = (float)(rand()%10000-4999)*0.00001f; |
pmic | 4:e50e18eac72b | 100 | // float temp5 = (float)(rand()%10000-4999)*0.00001f; |
pmic | 4:e50e18eac72b | 101 | // ekf_rp.update(gyro_x(i), gyro_y(i), accel_x(i), accel_y(i)); |
pmic | 0:a0e9705be9c4 | 102 | dt = timer.read(); |
pmic | 0:a0e9705be9c4 | 103 | timer.reset(); |
pmic | 0:a0e9705be9c4 | 104 | |
pmic | 4:e50e18eac72b | 105 | if( i < 4 ) { |
pmic | 8:a72cd6356bab | 106 | /* ekf_rp */ |
pmic | 8:a72cd6356bab | 107 | ekf_rp.update(gyro_x(i), gyro_y(i), accel_x(i), accel_y(i)); |
pmic | 8:a72cd6356bab | 108 | pc.printf("%i; %.7f; %.7f; %.7f; %.7f; %.7f; %.7f; %.7f; \r\n", i, ekf_rp.get_est_state(0), ekf_rp.get_est_state(1), ekf_rp.get_est_state(2), ekf_rp.get_est_state(3), ekf_rp.get_est_state(4), ekf_rp.get_est_state(5), dt); |
pmic | 8:a72cd6356bab | 109 | /* ekf_rpy */ |
pmic | 8:a72cd6356bab | 110 | // ekf_rpy.update(gyro_x(i), gyro_y(i), gyro_z(i), accel_x(i), accel_y(i), magnet_x(i), magnet_y(i)); |
pmic | 8:a72cd6356bab | 111 | // pc.printf("%i; %.7f; %.7f; %.7f; %.7f; %.7f; %.7f; %.7f %.7f; %.7f; \r\n", i, ekf_rpy.get_est_state(0), ekf_rpy.get_est_state(1), ekf_rpy.get_est_state(2), ekf_rpy.get_est_state(3), ekf_rpy.get_est_state(4), ekf_rpy.get_est_state(5), ekf_rpy.get_est_state(6), ekf_rpy.get_est_state(7), dt); |
pmic | 7:bcbcc23983de | 112 | |
pmic | 4:e50e18eac72b | 113 | } |
pmic | 0:a0e9705be9c4 | 114 | |
pmic | 4:e50e18eac72b | 115 | i++; |
pmic | 0:a0e9705be9c4 | 116 | |
pmic | 4:e50e18eac72b | 117 | wait_us(1000000); |
pmic | 0:a0e9705be9c4 | 118 | } |
pmic | 0:a0e9705be9c4 | 119 | } |