altb_pmic / Mbed 2 deprecated Test_ekf

Testing ekf implementation for Quadro_1.

Dependencies:   mbed Eigen

main.cpp@23:226f18dd30a5, 2020-01-30 (annotated)

Committer:
pmic
Date:
Thu Jan 30 14:14:50 2020 +0000
Revision:
23:226f18dd30a5
Parent:
21:aab1ac72095b 
Commit before update EKF_RP 6 states to EKF_RP 8 states.

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 23:226f18dd30a5 25 // Matrix<float, 7, 1> gyro_x;
pmic 23:226f18dd30a5 26 // Matrix<float, 7, 1> gyro_y;
pmic 23:226f18dd30a5 27 // Matrix<float, 7, 1> gyro_z;
pmic 23:226f18dd30a5 28 // Matrix<float, 7, 1> accel_x;
pmic 23:226f18dd30a5 29 // Matrix<float, 7, 1> accel_y;
pmic 23:226f18dd30a5 30 // Matrix<float, 7, 1> magnet_x;
pmic 23:226f18dd30a5 31 // Matrix<float, 7, 1> magnet_y;
pmic 23:226f18dd30a5 32
pmic 23:226f18dd30a5 33 Matrix<float, 14, 1> gyro_x;
pmic 23:226f18dd30a5 34 Matrix<float, 14, 1> gyro_y;
pmic 23:226f18dd30a5 35 Matrix<float, 14, 1> gyro_z;
pmic 23:226f18dd30a5 36 Matrix<float, 14, 1> accel_x;
pmic 23:226f18dd30a5 37 Matrix<float, 14, 1> accel_y;
pmic 23:226f18dd30a5 38 Matrix<float, 14, 1> magnet_x;
pmic 23:226f18dd30a5 39 Matrix<float, 14, 1> magnet_y;
pmic 4:e50e18eac72b 40
pmic 0:a0e9705be9c4 41 int main()
pmic 0:a0e9705be9c4 42 {
pmic 0:a0e9705be9c4 43 pc.baud(2000000);
pmic 0:a0e9705be9c4 44
pmic 0:a0e9705be9c4 45 timer.start();
pmic 0:a0e9705be9c4 46
pmic 4:e50e18eac72b 47 i = 0;
pmic 0:a0e9705be9c4 48
pmic 21:aab1ac72095b 49 ///*
pmic 1:6b803652d032 50 A << 1, 2, 3, 5, 1, 8,10, 1, 3,
pmic 1:6b803652d032 51 4, 5, 6, 8, 4, 2, 1, 9, 4,
pmic 1:6b803652d032 52 7, 8,10, 5, 6, 8, 4, 5, 1,
pmic 1:6b803652d032 53 4, 2,10, 8,10, 5, 6, 7, 8,
pmic 1:6b803652d032 54 1, 8, 7, 3, 4, 6, 5, 1, 7,
pmic 1:6b803652d032 55 4, 2, 7, 5, 7, 6, 9, 2, 1,
pmic 1:6b803652d032 56 5, 5, 1, 7, 4, 2, 1, 1, 9,
pmic 1:6b803652d032 57 8, 9, 7, 4, 5, 6, 1, 2, 2,
pmic 1:6b803652d032 58 1, 5, 9, 4, 8, 7, 2, 6, 3;
pmic 0:a0e9705be9c4 59 b << 3,
pmic 0:a0e9705be9c4 60 3,
pmic 1:6b803652d032 61 4,
pmic 1:6b803652d032 62 2,
pmic 1:6b803652d032 63 7,
pmic 1:6b803652d032 64 5,
pmic 1:6b803652d032 65 1,
pmic 1:6b803652d032 66 8,
pmic 1:6b803652d032 67 1;
pmic 21:aab1ac72095b 68 //*/
pmic 1:6b803652d032 69
pmic 21:aab1ac72095b 70 /*
pmic 1:6b803652d032 71 A << 1, 2, 3, 5, 1, 8,10, 1,
pmic 1:6b803652d032 72 4, 5, 6, 8, 4, 2, 1, 9,
pmic 1:6b803652d032 73 7, 8,10, 5, 6, 8, 4, 5,
pmic 1:6b803652d032 74 4, 2,10, 8,10, 5, 6, 7,
pmic 1:6b803652d032 75 1, 8, 7, 3, 4, 6, 5, 1,
pmic 1:6b803652d032 76 4, 2, 7, 5, 7, 6, 9, 2,
pmic 1:6b803652d032 77 5, 5, 1, 7, 4, 2, 1, 1,
pmic 1:6b803652d032 78 8, 9, 7, 4, 5, 6, 1, 2;
pmic 1:6b803652d032 79 b << 3,
pmic 1:6b803652d032 80 3,
pmic 1:6b803652d032 81 4,
pmic 1:6b803652d032 82 2,
pmic 1:6b803652d032 83 7,
pmic 1:6b803652d032 84 5,
pmic 1:6b803652d032 85 1,
pmic 1:6b803652d032 86 8;
pmic 21:aab1ac72095b 87 */
pmic 0:a0e9705be9c4 88
pmic 10:dd5ea359fb97 89 I.setIdentity();
pmic 16:c39e084f05ed 90 // I = I.transpose();
pmic 4:e50e18eac72b 91
pmic 23:226f18dd30a5 92 // gyro_x << 0.1, 0.5, -0.2, 0.2, 0.3, 0.2, -0.7;
pmic 23:226f18dd30a5 93 // gyro_y << -0.1, -0.3, 0.1, 0.1, 0.1, -0.1, -0.9;
pmic 23:226f18dd30a5 94 // gyro_z << 0.5, 0.4, -0.3, 0.2, 0.8, -0.6, 0.2;
pmic 23:226f18dd30a5 95 // accel_x << 0.3, -0.1, 0.7, -0.3, -0.9, 0.1, 0.2;
pmic 23:226f18dd30a5 96 // accel_y << 0.1, 0.5, -0.4, -0.2, -0.4, 0.5, 0.8;
pmic 23:226f18dd30a5 97 // magnet_x << 0.1, -0.2, -0.8, 0.2, 0.6, 0.4, -0.4;
pmic 23:226f18dd30a5 98 // magnet_y << 0.6, -0.4, 0.4, 0.7, 0.7, 0.5, 0.3;
pmic 23:226f18dd30a5 99
pmic 23:226f18dd30a5 100 gyro_x << 0.1f, 0.5f, -0.2f, 0.2f, 0.3f, 0.2f, -0.7f, 0.5f, 0.4f, -0.3f, 0.2f, 0.8f, -0.6f, 0.2f;
pmic 23:226f18dd30a5 101 gyro_y << -0.1f, -0.3f, 0.1f, 0.1f, 0.1f, -0.1f, -0.9f, 0.1f, 0.5f, -0.4f, -0.2f, -0.4f, 0.5f, 0.8f;
pmic 23:226f18dd30a5 102 gyro_z << 0.5f, 0.4f, -0.3f, 0.2f, 0.8f, -0.6f, 0.2f,-0.1f, -0.3f, 0.1f, 0.1f, 0.1f, -0.1f, -0.9f;
pmic 23:226f18dd30a5 103 accel_x << 0.3f, -0.1f, 0.7f, -0.3f, -0.9f, 0.1f, 0.2f, 0.1f, 0.5f, -0.2f, 0.2f, 0.3f, 0.2f, -0.7f;
pmic 23:226f18dd30a5 104 accel_y << 0.1f, 0.5f, -0.4f, -0.2f, -0.4f, 0.5f, 0.8f, 0.1f, -0.2f, -0.8f, 0.2f, 0.6f, 0.4f, -0.4f;
pmic 23:226f18dd30a5 105 magnet_x << 0.1f, -0.2f, -0.8f, 0.2f, 0.6f, 0.4f, -0.4f, 0.6f, -0.4f, 0.4f, 0.7f, 0.7f, 0.5f, 0.3f;
pmic 23:226f18dd30a5 106 magnet_y << 0.6f, -0.4f, 0.4f, 0.7f, 0.7f, 0.5f, 0.3f, 0.3f, -0.1f, 0.7f, -0.3f, -0.9f, 0.1f, 0.2f;
pmic 4:e50e18eac72b 107
pmic 4:e50e18eac72b 108 pc.printf("\r\n");
pmic 1:6b803652d032 109
pmic 0:a0e9705be9c4 110 while(1) {
pmic 0:a0e9705be9c4 111
pmic 23:226f18dd30a5 112 // Matrix<float, 9, 1> x = A.inverse() * b;
pmic 0:a0e9705be9c4 113
pmic 4:e50e18eac72b 114 // float temp0 = (float)(rand()%10000-4999)*0.00001f;
pmic 4:e50e18eac72b 115 // float temp1 = (float)(rand()%10000-4999)*0.00001f;
pmic 4:e50e18eac72b 116 // float temp2 = (float)(rand()%10000-4999)*0.00001f;
pmic 4:e50e18eac72b 117 // float temp3 = (float)(rand()%10000-4999)*0.00001f;
pmic 4:e50e18eac72b 118 // float temp4 = (float)(rand()%10000-4999)*0.00001f;
pmic 4:e50e18eac72b 119 // float temp5 = (float)(rand()%10000-4999)*0.00001f;
pmic 4:e50e18eac72b 120 // ekf_rp.update(gyro_x(i), gyro_y(i), accel_x(i), accel_y(i));
pmic 0:a0e9705be9c4 121 dt = timer.read();
pmic 0:a0e9705be9c4 122 timer.reset();
pmic 0:a0e9705be9c4 123
pmic 23:226f18dd30a5 124 if( i < 14 ) {
pmic 8:a72cd6356bab 125 /* ekf_rp */
pmic 17:1d98928f7681 126 ekf_rp.update(gyro_x(i), gyro_y(i), accel_x(i), accel_y(i));
pmic 23:226f18dd30a5 127 // 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 23:226f18dd30a5 128 pc.printf("%.7f; \r\n", dt);
pmic 8:a72cd6356bab 129 /* ekf_rpy */
pmic 17:1d98928f7681 130 // 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 131 // 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 132 // pc.printf("%.7f; \r\n", dt);
pmic 4:e50e18eac72b 133 }
pmic 17:1d98928f7681 134 i++;
pmic 23:226f18dd30a5 135 // wait_us(1000000);
pmic 0:a0e9705be9c4 136 }
pmic 0:a0e9705be9c4 137 }