altb_pmic / Mbed 2 deprecated Test_ekf

Testing ekf implementation for Quadro_1.

Dependencies:   mbed Eigen

main.cpp@0:a0e9705be9c4, 2019-10-18 (annotated)

Committer:
pmic
Date:
Fri Oct 18 14:55:53 2019 +0000
Revision:
0:a0e9705be9c4
Child:
1:6b803652d032
First commit. Nothing is working.

Who changed what in which revision?

UserRevisionLine numberNew contents of line
pmic 0:a0e9705be9c4 1 #include "mbed.h"
pmic 0:a0e9705be9c4 2 //#include "Eigen/Dense.h"
pmic 0:a0e9705be9c4 3 #include "Eigen/Core.h"
pmic 0:a0e9705be9c4 4 #include "Eigen/Geometry.h"
pmic 0:a0e9705be9c4 5 #include "EKF_RP.h"
pmic 0:a0e9705be9c4 6
pmic 0:a0e9705be9c4 7 using namespace Eigen;
pmic 0:a0e9705be9c4 8
pmic 0:a0e9705be9c4 9 Serial pc(SERIAL_TX, SERIAL_RX);
pmic 0:a0e9705be9c4 10
pmic 0:a0e9705be9c4 11 // EKF_RP ekf_rp(0.02f); // initialize counter on PB_6 and PB_7
pmic 0:a0e9705be9c4 12
pmic 0:a0e9705be9c4 13 Timer timer; // timer for time measurement
pmic 0:a0e9705be9c4 14 float dt = 0.0f;
pmic 0:a0e9705be9c4 15
pmic 0:a0e9705be9c4 16 uint32_t counter;
pmic 0:a0e9705be9c4 17
pmic 0:a0e9705be9c4 18 Matrix<float, 3, 3> A;
pmic 0:a0e9705be9c4 19 Matrix<float, 3, 1> b;
pmic 0:a0e9705be9c4 20 DiagonalMatrix<float, 3, 3> I;
pmic 0:a0e9705be9c4 21
pmic 0:a0e9705be9c4 22 // Matrix<float,4,1> mat = state.block(6,0,4,1);
pmic 0:a0e9705be9c4 23
pmic 0:a0e9705be9c4 24 int main()
pmic 0:a0e9705be9c4 25 {
pmic 0:a0e9705be9c4 26 pc.baud(2000000);
pmic 0:a0e9705be9c4 27
pmic 0:a0e9705be9c4 28 timer.start();
pmic 0:a0e9705be9c4 29
pmic 0:a0e9705be9c4 30 counter = 0;
pmic 0:a0e9705be9c4 31
pmic 0:a0e9705be9c4 32 A << 1, 2, 3,
pmic 0:a0e9705be9c4 33 4, 5, 6,
pmic 0:a0e9705be9c4 34 7, 8, 10;
pmic 0:a0e9705be9c4 35 b << 3,
pmic 0:a0e9705be9c4 36 3,
pmic 0:a0e9705be9c4 37 4;
pmic 0:a0e9705be9c4 38
pmic 0:a0e9705be9c4 39 // I.setIdentity();
pmic 0:a0e9705be9c4 40
pmic 0:a0e9705be9c4 41 I.diagonal()[0] = 1;
pmic 0:a0e9705be9c4 42 I.diagonal()[1] = 1;
pmic 0:a0e9705be9c4 43 I.diagonal()[2] = 1;
pmic 0:a0e9705be9c4 44
pmic 0:a0e9705be9c4 45 while(1) {
pmic 0:a0e9705be9c4 46
pmic 0:a0e9705be9c4 47 Vector3f x = I * A.inverse() * b;
pmic 0:a0e9705be9c4 48
pmic 0:a0e9705be9c4 49 // ekf_rp.update(0.0f, 0.0f, 0.0f, 0.0f);
pmic 0:a0e9705be9c4 50 dt = timer.read();
pmic 0:a0e9705be9c4 51 timer.reset();
pmic 0:a0e9705be9c4 52
pmic 0:a0e9705be9c4 53 pc.printf("%i; %.6f; %.6f; %.6f; %.6f; \r\n", counter, b(0), b(1), b(2), 1.0f/dt);
pmic 0:a0e9705be9c4 54 pc.printf("%i; %.6f; %.6f; %.6f;\r\n", counter, x(0), x(1), x(2));
pmic 0:a0e9705be9c4 55
pmic 0:a0e9705be9c4 56 counter++;
pmic 0:a0e9705be9c4 57
pmic 0:a0e9705be9c4 58 wait_us(1000000);
pmic 0:a0e9705be9c4 59 }
pmic 0:a0e9705be9c4 60 }
pmic 0:a0e9705be9c4 61