altb_pmic / Mbed 2 deprecated Test_ekf

Testing ekf implementation for Quadro_1.

Dependencies:   mbed Eigen

main.cpp@3:121647a7cddf, 2019-10-18 (annotated)

Committer:
pmic
Date:
Fri Oct 18 21:49:59 2019 +0000
Revision:
3:121647a7cddf
Parent:
2:756446014084
Child:
4:e50e18eac72b
Change Eigen.lib (just uncomment -huge). And get EKF_RP running.

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 0:a0e9705be9c4 17 uint32_t counter;
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 0:a0e9705be9c4 23 int main()
pmic 0:a0e9705be9c4 24 {
pmic 0:a0e9705be9c4 25 pc.baud(2000000);
pmic 0:a0e9705be9c4 26
pmic 0:a0e9705be9c4 27 timer.start();
pmic 0:a0e9705be9c4 28
pmic 0:a0e9705be9c4 29 counter = 0;
pmic 0:a0e9705be9c4 30
pmic 1:6b803652d032 31 /*
pmic 1:6b803652d032 32 A << 1, 2, 3, 5, 1, 8,10, 1, 3,
pmic 1:6b803652d032 33 4, 5, 6, 8, 4, 2, 1, 9, 4,
pmic 1:6b803652d032 34 7, 8,10, 5, 6, 8, 4, 5, 1,
pmic 1:6b803652d032 35 4, 2,10, 8,10, 5, 6, 7, 8,
pmic 1:6b803652d032 36 1, 8, 7, 3, 4, 6, 5, 1, 7,
pmic 1:6b803652d032 37 4, 2, 7, 5, 7, 6, 9, 2, 1,
pmic 1:6b803652d032 38 5, 5, 1, 7, 4, 2, 1, 1, 9,
pmic 1:6b803652d032 39 8, 9, 7, 4, 5, 6, 1, 2, 2,
pmic 1:6b803652d032 40 1, 5, 9, 4, 8, 7, 2, 6, 3;
pmic 0:a0e9705be9c4 41 b << 3,
pmic 0:a0e9705be9c4 42 3,
pmic 1:6b803652d032 43 4,
pmic 1:6b803652d032 44 2,
pmic 1:6b803652d032 45 7,
pmic 1:6b803652d032 46 5,
pmic 1:6b803652d032 47 1,
pmic 1:6b803652d032 48 8,
pmic 1:6b803652d032 49 1;
pmic 1:6b803652d032 50 */
pmic 1:6b803652d032 51
pmic 1:6b803652d032 52 A << 1, 2, 3, 5, 1, 8,10, 1,
pmic 1:6b803652d032 53 4, 5, 6, 8, 4, 2, 1, 9,
pmic 1:6b803652d032 54 7, 8,10, 5, 6, 8, 4, 5,
pmic 1:6b803652d032 55 4, 2,10, 8,10, 5, 6, 7,
pmic 1:6b803652d032 56 1, 8, 7, 3, 4, 6, 5, 1,
pmic 1:6b803652d032 57 4, 2, 7, 5, 7, 6, 9, 2,
pmic 1:6b803652d032 58 5, 5, 1, 7, 4, 2, 1, 1,
pmic 1:6b803652d032 59 8, 9, 7, 4, 5, 6, 1, 2;
pmic 1:6b803652d032 60 b << 3,
pmic 1:6b803652d032 61 3,
pmic 1:6b803652d032 62 4,
pmic 1:6b803652d032 63 2,
pmic 1:6b803652d032 64 7,
pmic 1:6b803652d032 65 5,
pmic 1:6b803652d032 66 1,
pmic 1:6b803652d032 67 8;
pmic 0:a0e9705be9c4 68
pmic 0:a0e9705be9c4 69 // I.setIdentity();
pmic 1:6b803652d032 70
pmic 0:a0e9705be9c4 71 while(1) {
pmic 0:a0e9705be9c4 72
pmic 3:121647a7cddf 73 // Matrix<float, 8, 1> x = A.inverse() * b;
pmic 0:a0e9705be9c4 74
pmic 3:121647a7cddf 75 ekf_rp.update(0.0f, 0.0f, 0.0f, 0.0f);
pmic 0:a0e9705be9c4 76 dt = timer.read();
pmic 0:a0e9705be9c4 77 timer.reset();
pmic 0:a0e9705be9c4 78
pmic 1:6b803652d032 79 pc.printf("%i; %.6f; %.6f; %.6f; %.6f; \r\n", counter, b(0), b(1), b(2), dt);
pmic 3:121647a7cddf 80 // pc.printf("%i; %.6f; %.6f; %.6f; %.6f; \r\n", counter, b(0), b(1), b(2), dt);
pmic 0:a0e9705be9c4 81
pmic 0:a0e9705be9c4 82 counter++;
pmic 0:a0e9705be9c4 83
pmic 3:121647a7cddf 84 // wait_us(1000000);
pmic 0:a0e9705be9c4 85 }
pmic 0:a0e9705be9c4 86 }