altb_pmic / Mbed 2 deprecated Test_ekf

Testing ekf implementation for Quadro_1.

Dependencies:   mbed Eigen

main.cpp@4:e50e18eac72b, 2019-10-20 (annotated)

Committer:
pmic
Date:
Sun Oct 20 08:27:25 2019 +0000
Revision:
4:e50e18eac72b
Parent:
3:121647a7cddf
Child:
6:f9569a07aff5
Commit before set double for test purposes.

Who changed what in which revision?

UserRevisionLine numberNew 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 2:756446014084 12 EKF_RP ekf_rp(0.02f); // initialize counter on PB_6 and PB_7
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 }