altb_pmic
/
Test_ekf
Testing ekf implementation for Quadro_1.
main.cpp@6:f9569a07aff5, 2019-10-20 (annotated)
- Committer:
- pmic
- Date:
- Sun Oct 20 10:20:05 2019 +0000
- Revision:
- 6:f9569a07aff5
- Parent:
- 4:e50e18eac72b
- Child:
- 7:bcbcc23983de
No update on Eigen!
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 | 0:a0e9705be9c4 | 7 | |
pmic | 0:a0e9705be9c4 | 8 | using namespace Eigen; |
pmic | 0:a0e9705be9c4 | 9 | |
pmic | 0:a0e9705be9c4 | 10 | Serial pc(SERIAL_TX, SERIAL_RX); |
pmic | 0:a0e9705be9c4 | 11 | |
pmic | 6:f9569a07aff5 | 12 | EKF_RP ekf_rp(0.02f); |
pmic | 0:a0e9705be9c4 | 13 | |
pmic | 0:a0e9705be9c4 | 14 | Timer timer; // timer for time measurement |
pmic | 0:a0e9705be9c4 | 15 | float dt = 0.0f; |
pmic | 0:a0e9705be9c4 | 16 | |
pmic | 4:e50e18eac72b | 17 | uint32_t i; |
pmic | 0:a0e9705be9c4 | 18 | |
pmic | 1:6b803652d032 | 19 | Matrix<float, 8, 8> A; |
pmic | 1:6b803652d032 | 20 | Matrix<float, 8, 1> b; |
pmic | 3:121647a7cddf | 21 | Matrix<float, 8, 8> I; |
pmic | 0:a0e9705be9c4 | 22 | |
pmic | 4:e50e18eac72b | 23 | Matrix<float, 4, 1> gyro_x; |
pmic | 4:e50e18eac72b | 24 | Matrix<float, 4, 1> gyro_y; |
pmic | 4:e50e18eac72b | 25 | Matrix<float, 4, 1> accel_x; |
pmic | 4:e50e18eac72b | 26 | Matrix<float, 4, 1> accel_y; |
pmic | 4:e50e18eac72b | 27 | |
pmic | 0:a0e9705be9c4 | 28 | int main() |
pmic | 0:a0e9705be9c4 | 29 | { |
pmic | 0:a0e9705be9c4 | 30 | pc.baud(2000000); |
pmic | 0:a0e9705be9c4 | 31 | |
pmic | 0:a0e9705be9c4 | 32 | timer.start(); |
pmic | 0:a0e9705be9c4 | 33 | |
pmic | 4:e50e18eac72b | 34 | i = 0; |
pmic | 0:a0e9705be9c4 | 35 | |
pmic | 1:6b803652d032 | 36 | /* |
pmic | 1:6b803652d032 | 37 | A << 1, 2, 3, 5, 1, 8,10, 1, 3, |
pmic | 1:6b803652d032 | 38 | 4, 5, 6, 8, 4, 2, 1, 9, 4, |
pmic | 1:6b803652d032 | 39 | 7, 8,10, 5, 6, 8, 4, 5, 1, |
pmic | 1:6b803652d032 | 40 | 4, 2,10, 8,10, 5, 6, 7, 8, |
pmic | 1:6b803652d032 | 41 | 1, 8, 7, 3, 4, 6, 5, 1, 7, |
pmic | 1:6b803652d032 | 42 | 4, 2, 7, 5, 7, 6, 9, 2, 1, |
pmic | 1:6b803652d032 | 43 | 5, 5, 1, 7, 4, 2, 1, 1, 9, |
pmic | 1:6b803652d032 | 44 | 8, 9, 7, 4, 5, 6, 1, 2, 2, |
pmic | 1:6b803652d032 | 45 | 1, 5, 9, 4, 8, 7, 2, 6, 3; |
pmic | 0:a0e9705be9c4 | 46 | b << 3, |
pmic | 0:a0e9705be9c4 | 47 | 3, |
pmic | 1:6b803652d032 | 48 | 4, |
pmic | 1:6b803652d032 | 49 | 2, |
pmic | 1:6b803652d032 | 50 | 7, |
pmic | 1:6b803652d032 | 51 | 5, |
pmic | 1:6b803652d032 | 52 | 1, |
pmic | 1:6b803652d032 | 53 | 8, |
pmic | 1:6b803652d032 | 54 | 1; |
pmic | 1:6b803652d032 | 55 | */ |
pmic | 1:6b803652d032 | 56 | |
pmic | 1:6b803652d032 | 57 | A << 1, 2, 3, 5, 1, 8,10, 1, |
pmic | 1:6b803652d032 | 58 | 4, 5, 6, 8, 4, 2, 1, 9, |
pmic | 1:6b803652d032 | 59 | 7, 8,10, 5, 6, 8, 4, 5, |
pmic | 1:6b803652d032 | 60 | 4, 2,10, 8,10, 5, 6, 7, |
pmic | 1:6b803652d032 | 61 | 1, 8, 7, 3, 4, 6, 5, 1, |
pmic | 1:6b803652d032 | 62 | 4, 2, 7, 5, 7, 6, 9, 2, |
pmic | 1:6b803652d032 | 63 | 5, 5, 1, 7, 4, 2, 1, 1, |
pmic | 1:6b803652d032 | 64 | 8, 9, 7, 4, 5, 6, 1, 2; |
pmic | 1:6b803652d032 | 65 | b << 3, |
pmic | 1:6b803652d032 | 66 | 3, |
pmic | 1:6b803652d032 | 67 | 4, |
pmic | 1:6b803652d032 | 68 | 2, |
pmic | 1:6b803652d032 | 69 | 7, |
pmic | 1:6b803652d032 | 70 | 5, |
pmic | 1:6b803652d032 | 71 | 1, |
pmic | 1:6b803652d032 | 72 | 8; |
pmic | 0:a0e9705be9c4 | 73 | |
pmic | 0:a0e9705be9c4 | 74 | // I.setIdentity(); |
pmic | 4:e50e18eac72b | 75 | |
pmic | 4:e50e18eac72b | 76 | gyro_x << 0.01, 0.05, -0.02, 0.02; |
pmic | 4:e50e18eac72b | 77 | gyro_y << -0.01, -0.03, 0.01, 0.01; |
pmic | 4:e50e18eac72b | 78 | accel_x << 0.03, -0.01, 0.07, -0.03; |
pmic | 4:e50e18eac72b | 79 | accel_y << 0.01, 0.05, -0.04, 0.02; |
pmic | 4:e50e18eac72b | 80 | |
pmic | 4:e50e18eac72b | 81 | pc.printf("\r\n"); |
pmic | 1:6b803652d032 | 82 | |
pmic | 0:a0e9705be9c4 | 83 | while(1) { |
pmic | 0:a0e9705be9c4 | 84 | |
pmic | 3:121647a7cddf | 85 | // Matrix<float, 8, 1> x = A.inverse() * b; |
pmic | 0:a0e9705be9c4 | 86 | |
pmic | 4:e50e18eac72b | 87 | // float temp0 = (float)(rand()%10000-4999)*0.00001f; |
pmic | 4:e50e18eac72b | 88 | // float temp1 = (float)(rand()%10000-4999)*0.00001f; |
pmic | 4:e50e18eac72b | 89 | // float temp2 = (float)(rand()%10000-4999)*0.00001f; |
pmic | 4:e50e18eac72b | 90 | // float temp3 = (float)(rand()%10000-4999)*0.00001f; |
pmic | 4:e50e18eac72b | 91 | // float temp4 = (float)(rand()%10000-4999)*0.00001f; |
pmic | 4:e50e18eac72b | 92 | // float temp5 = (float)(rand()%10000-4999)*0.00001f; |
pmic | 4:e50e18eac72b | 93 | // ekf_rp.update(gyro_x(i), gyro_y(i), accel_x(i), accel_y(i)); |
pmic | 0:a0e9705be9c4 | 94 | dt = timer.read(); |
pmic | 0:a0e9705be9c4 | 95 | timer.reset(); |
pmic | 0:a0e9705be9c4 | 96 | |
pmic | 4:e50e18eac72b | 97 | if( i < 4 ) { |
pmic | 4:e50e18eac72b | 98 | ekf_rp.update(gyro_x(i), gyro_y(i), accel_x(i), accel_y(i)); |
pmic | 4:e50e18eac72b | 99 | pc.printf("%i; %.6f; %.6f; %.6f; %.6f; %.6f; %.6f; %.6f; \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 | 4:e50e18eac72b | 100 | // pc.printf("%i; %.6f; %.6f; %.6f; %.6f; %.6f; %.6f; %.6f; %.6f; \r\n", i, ekf_rp.read_Q(0,0), ekf_rp.read_Q(1,1), ekf_rp.read_Q(2,2), ekf_rp.read_Q(3,3), ekf_rp.read_Q(4,4), ekf_rp.read_Q(5,5), ekf_rp.read_Q(0,1), ekf_rp.read_Q(1,0)); |
pmic | 4:e50e18eac72b | 101 | } |
pmic | 0:a0e9705be9c4 | 102 | |
pmic | 4:e50e18eac72b | 103 | i++; |
pmic | 0:a0e9705be9c4 | 104 | |
pmic | 4:e50e18eac72b | 105 | wait_us(1000000); |
pmic | 0:a0e9705be9c4 | 106 | } |
pmic | 0:a0e9705be9c4 | 107 | } |