altb_pmic / Mbed 2 deprecated Test_ekf

Testing ekf implementation for Quadro_1.

Dependencies:   mbed Eigen

main.cpp@21:aab1ac72095b, 2020-01-06 (annotated)

Committer:
pmic
Date:
Mon Jan 06 10:20:10 2020 +0000
Revision:
21:aab1ac72095b
Parent:
17:1d98928f7681
Child:
23:226f18dd30a5
Commit changes in state order (correction).

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 14:c4bc8ba29c2f 3 #include "Eigen/Dense.h"
pmic 14:c4bc8ba29c2f 4 // #include "Eigen/Core.h"
pmic 14:c4bc8ba29c2f 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 21:aab1ac72095b 21 Matrix<float, 9, 9> A;
pmic 21:aab1ac72095b 22 Matrix<float, 9, 1> b;
pmic 15:53485bd1ff28 23 Matrix<float, 8, 8> I;
pmic 0:a0e9705be9c4 24
pmic 17:1d98928f7681 25 Matrix<float, 7, 1> gyro_x;
pmic 17:1d98928f7681 26 Matrix<float, 7, 1> gyro_y;
pmic 17:1d98928f7681 27 Matrix<float, 7, 1> gyro_z;
pmic 17:1d98928f7681 28 Matrix<float, 7, 1> accel_x;
pmic 17:1d98928f7681 29 Matrix<float, 7, 1> accel_y;
pmic 17:1d98928f7681 30 Matrix<float, 7, 1> magnet_x;
pmic 17:1d98928f7681 31 Matrix<float, 7, 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 21:aab1ac72095b 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 21:aab1ac72095b 60 //*/
pmic 1:6b803652d032 61
pmic 21:aab1ac72095b 62 /*
pmic 1:6b803652d032 63 A << 1, 2, 3, 5, 1, 8,10, 1,
pmic 1:6b803652d032 64 4, 5, 6, 8, 4, 2, 1, 9,
pmic 1:6b803652d032 65 7, 8,10, 5, 6, 8, 4, 5,
pmic 1:6b803652d032 66 4, 2,10, 8,10, 5, 6, 7,
pmic 1:6b803652d032 67 1, 8, 7, 3, 4, 6, 5, 1,
pmic 1:6b803652d032 68 4, 2, 7, 5, 7, 6, 9, 2,
pmic 1:6b803652d032 69 5, 5, 1, 7, 4, 2, 1, 1,
pmic 1:6b803652d032 70 8, 9, 7, 4, 5, 6, 1, 2;
pmic 1:6b803652d032 71 b << 3,
pmic 1:6b803652d032 72 3,
pmic 1:6b803652d032 73 4,
pmic 1:6b803652d032 74 2,
pmic 1:6b803652d032 75 7,
pmic 1:6b803652d032 76 5,
pmic 1:6b803652d032 77 1,
pmic 1:6b803652d032 78 8;
pmic 21:aab1ac72095b 79 */
pmic 0:a0e9705be9c4 80
pmic 10:dd5ea359fb97 81 I.setIdentity();
pmic 16:c39e084f05ed 82 // I = I.transpose();
pmic 4:e50e18eac72b 83
pmic 17:1d98928f7681 84 gyro_x << 0.1, 0.5, -0.2, 0.2, 0.3, 0.2, -0.7;
pmic 17:1d98928f7681 85 gyro_y << -0.1, -0.3, 0.1, 0.1, 0.1, -0.1, -0.9;
pmic 17:1d98928f7681 86 gyro_z << 0.5, 0.4, -0.3, 0.2, 0.8, -0.6, 0.2;
pmic 17:1d98928f7681 87 accel_x << 0.3, -0.1, 0.7, -0.3, -0.9, 0.1, 0.2;
pmic 17:1d98928f7681 88 accel_y << 0.1, 0.5, -0.4, -0.2, -0.4, 0.5, 0.8;
pmic 17:1d98928f7681 89 magnet_x << 0.1, -0.2, -0.8, 0.2, 0.6, 0.4, -0.4;
pmic 17:1d98928f7681 90 magnet_y << 0.6, -0.4, 0.4, 0.7, 0.7, 0.5, 0.3;
pmic 4:e50e18eac72b 91
pmic 4:e50e18eac72b 92 pc.printf("\r\n");
pmic 1:6b803652d032 93
pmic 0:a0e9705be9c4 94 while(1) {
pmic 0:a0e9705be9c4 95
pmic 21:aab1ac72095b 96 Matrix<float, 9, 1> x = A.inverse() * b;
pmic 0:a0e9705be9c4 97
pmic 4:e50e18eac72b 98 // float temp0 = (float)(rand()%10000-4999)*0.00001f;
pmic 4:e50e18eac72b 99 // float temp1 = (float)(rand()%10000-4999)*0.00001f;
pmic 4:e50e18eac72b 100 // float temp2 = (float)(rand()%10000-4999)*0.00001f;
pmic 4:e50e18eac72b 101 // float temp3 = (float)(rand()%10000-4999)*0.00001f;
pmic 4:e50e18eac72b 102 // float temp4 = (float)(rand()%10000-4999)*0.00001f;
pmic 4:e50e18eac72b 103 // float temp5 = (float)(rand()%10000-4999)*0.00001f;
pmic 4:e50e18eac72b 104 // ekf_rp.update(gyro_x(i), gyro_y(i), accel_x(i), accel_y(i));
pmic 0:a0e9705be9c4 105 dt = timer.read();
pmic 0:a0e9705be9c4 106 timer.reset();
pmic 0:a0e9705be9c4 107
pmic 17:1d98928f7681 108 if( i < 7 ) {
pmic 8:a72cd6356bab 109 /* ekf_rp */
pmic 17:1d98928f7681 110 ekf_rp.update(gyro_x(i), gyro_y(i), accel_x(i), accel_y(i));
pmic 17:1d98928f7681 111 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 17:1d98928f7681 112 // pc.printf("%.7f; \r\n", dt);
pmic 8:a72cd6356bab 113 /* ekf_rpy */
pmic 17:1d98928f7681 114 // 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 17:1d98928f7681 115 // 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 17:1d98928f7681 116 // pc.printf("%.7f; \r\n", dt);
pmic 4:e50e18eac72b 117 }
pmic 17:1d98928f7681 118 i++;
pmic 4:e50e18eac72b 119 wait_us(1000000);
pmic 0:a0e9705be9c4 120 }
pmic 0:a0e9705be9c4 121 }