![](/media/cache/group/default_image.jpg.50x50_q85.jpg)
Testing ekf implementation for Quadro_1.
Diff: main.cpp
- Revision:
- 0:a0e9705be9c4
- Child:
- 1:6b803652d032
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/main.cpp Fri Oct 18 14:55:53 2019 +0000 @@ -0,0 +1,61 @@ +#include "mbed.h" +//#include "Eigen/Dense.h" +#include "Eigen/Core.h" +#include "Eigen/Geometry.h" +#include "EKF_RP.h" + +using namespace Eigen; + +Serial pc(SERIAL_TX, SERIAL_RX); + +// EKF_RP ekf_rp(0.02f); // initialize counter on PB_6 and PB_7 + +Timer timer; // timer for time measurement +float dt = 0.0f; + +uint32_t counter; + +Matrix<float, 3, 3> A; +Matrix<float, 3, 1> b; +DiagonalMatrix<float, 3, 3> I; + +// Matrix<float,4,1> mat = state.block(6,0,4,1); + +int main() +{ + pc.baud(2000000); + + timer.start(); + + counter = 0; + + A << 1, 2, 3, + 4, 5, 6, + 7, 8, 10; + b << 3, + 3, + 4; + + // I.setIdentity(); + + I.diagonal()[0] = 1; + I.diagonal()[1] = 1; + I.diagonal()[2] = 1; + + while(1) { + + Vector3f x = I * A.inverse() * b; + + // ekf_rp.update(0.0f, 0.0f, 0.0f, 0.0f); + dt = timer.read(); + timer.reset(); + + pc.printf("%i; %.6f; %.6f; %.6f; %.6f; \r\n", counter, b(0), b(1), b(2), 1.0f/dt); + pc.printf("%i; %.6f; %.6f; %.6f;\r\n", counter, x(0), x(1), x(2)); + + counter++; + + wait_us(1000000); + } +} +