altb_pmic / Mbed 2 deprecated Test_ekf

Testing ekf implementation for Quadro_1.

Dependencies:   mbed Eigen

main.cpp@16:c39e084f05ed, 2019-10-24 (annotated)

Committer:
pmic
Date:
Thu Oct 24 06:43:17 2019 +0000
Revision:
16:c39e084f05ed
Parent:
15:53485bd1ff28
Child:
17:1d98928f7681
Get recursion running. Everything is fine now : - )

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 4:e50e18eac72b 25 Matrix<float, 4, 1> gyro_x;
pmic 4:e50e18eac72b 26 Matrix<float, 4, 1> gyro_y;
pmic 7:bcbcc23983de 27 Matrix<float, 4, 1> gyro_z;
pmic 4:e50e18eac72b 28 Matrix<float, 4, 1> accel_x;
pmic 4:e50e18eac72b 29 Matrix<float, 4, 1> accel_y;
pmic 7:bcbcc23983de 30 Matrix<float, 4, 1> magnet_x;
pmic 7:bcbcc23983de 31 Matrix<float, 4, 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 7:bcbcc23983de 82 gyro_x << 0.01, 0.05, -0.02, 0.02;
pmic 7:bcbcc23983de 83 gyro_y << -0.01, -0.03, 0.01, 0.01;
pmic 7:bcbcc23983de 84 gyro_z << 0.05, 0.04, -0.03, 0.02;
pmic 7:bcbcc23983de 85 accel_x << 0.03, -0.01, 0.07, -0.03;
pmic 7:bcbcc23983de 86 accel_y << 0.01, 0.05, -0.04, -0.02;
pmic 7:bcbcc23983de 87 magnet_x << 0.01, -0.02, -0.08, 0.02;
pmic 7:bcbcc23983de 88 magnet_y << 0.06, -0.04, 0.04, 0.07;
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 4:e50e18eac72b 106 if( i < 4 ) {
pmic 8:a72cd6356bab 107 /* ekf_rp */
pmic 16:c39e084f05ed 108 // ekf_rp.update(gyro_x(i), gyro_y(i), accel_x(i), accel_y(i));
pmic 16:c39e084f05ed 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 8:a72cd6356bab 110 /* ekf_rpy */
pmic 16:c39e084f05ed 111 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 16:c39e084f05ed 112 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 4:e50e18eac72b 113 }
pmic 0:a0e9705be9c4 114
pmic 4:e50e18eac72b 115 i++;
pmic 0:a0e9705be9c4 116
pmic 4:e50e18eac72b 117 wait_us(1000000);
pmic 0:a0e9705be9c4 118 }
pmic 0:a0e9705be9c4 119 }