altb_pmic / Mbed 2 deprecated Test_ekf

Testing ekf implementation for Quadro_1.

Dependencies:   mbed Eigen

main.cpp@17:1d98928f7681, 2019-10-25 (annotated)

Committer:
pmic
Date:
Fri Oct 25 08:44:32 2019 +0000
Revision:
17:1d98928f7681
Parent:
16:c39e084f05ed
Child:
21:aab1ac72095b
Recursion and inversion deliver same results. Time measurements show inversion is faster (???). So EKF changes back to inversion.

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 1:6b803652d032 21 Matrix<float, 8, 8> A;
pmic 1:6b803652d032 22 Matrix<float, 8, 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 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 16:c39e084f05ed 80 // I = I.transpose();
pmic 4:e50e18eac72b 81
pmic 17:1d98928f7681 82 gyro_x << 0.1, 0.5, -0.2, 0.2, 0.3, 0.2, -0.7;
pmic 17:1d98928f7681 83 gyro_y << -0.1, -0.3, 0.1, 0.1, 0.1, -0.1, -0.9;
pmic 17:1d98928f7681 84 gyro_z << 0.5, 0.4, -0.3, 0.2, 0.8, -0.6, 0.2;
pmic 17:1d98928f7681 85 accel_x << 0.3, -0.1, 0.7, -0.3, -0.9, 0.1, 0.2;
pmic 17:1d98928f7681 86 accel_y << 0.1, 0.5, -0.4, -0.2, -0.4, 0.5, 0.8;
pmic 17:1d98928f7681 87 magnet_x << 0.1, -0.2, -0.8, 0.2, 0.6, 0.4, -0.4;
pmic 17:1d98928f7681 88 magnet_y << 0.6, -0.4, 0.4, 0.7, 0.7, 0.5, 0.3;
pmic 4:e50e18eac72b 89
pmic 4:e50e18eac72b 90 pc.printf("\r\n");
pmic 1:6b803652d032 91
pmic 0:a0e9705be9c4 92 while(1) {
pmic 0:a0e9705be9c4 93
pmic 3:121647a7cddf 94 // Matrix<float, 8, 1> x = A.inverse() * b;
pmic 0:a0e9705be9c4 95
pmic 4:e50e18eac72b 96 // float temp0 = (float)(rand()%10000-4999)*0.00001f;
pmic 4:e50e18eac72b 97 // float temp1 = (float)(rand()%10000-4999)*0.00001f;
pmic 4:e50e18eac72b 98 // float temp2 = (float)(rand()%10000-4999)*0.00001f;
pmic 4:e50e18eac72b 99 // float temp3 = (float)(rand()%10000-4999)*0.00001f;
pmic 4:e50e18eac72b 100 // float temp4 = (float)(rand()%10000-4999)*0.00001f;
pmic 4:e50e18eac72b 101 // float temp5 = (float)(rand()%10000-4999)*0.00001f;
pmic 4:e50e18eac72b 102 // ekf_rp.update(gyro_x(i), gyro_y(i), accel_x(i), accel_y(i));
pmic 0:a0e9705be9c4 103 dt = timer.read();
pmic 0:a0e9705be9c4 104 timer.reset();
pmic 0:a0e9705be9c4 105
pmic 17:1d98928f7681 106 if( i < 7 ) {
pmic 8:a72cd6356bab 107 /* ekf_rp */
pmic 17:1d98928f7681 108 ekf_rp.update(gyro_x(i), gyro_y(i), accel_x(i), accel_y(i));
pmic 17:1d98928f7681 109 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 110 // pc.printf("%.7f; \r\n", dt);
pmic 8:a72cd6356bab 111 /* ekf_rpy */
pmic 17:1d98928f7681 112 // 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 113 // 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 114 // pc.printf("%.7f; \r\n", dt);
pmic 4:e50e18eac72b 115 }
pmic 17:1d98928f7681 116 i++;
pmic 4:e50e18eac72b 117 wait_us(1000000);
pmic 0:a0e9705be9c4 118 }
pmic 0:a0e9705be9c4 119 }