altb_pmic
/
Test_ekf
Testing ekf implementation for Quadro_1.
main.cpp
- Committer:
- pmic
- Date:
- 2020-02-05
- Revision:
- 24:e5188a2d72ca
- Parent:
- 23:226f18dd30a5
File content as of revision 24:e5188a2d72ca:
#include "mbed.h" #include "iostream" #include "Eigen/Dense.h" // #include "Eigen/Core.h" // #include "Eigen/Geometry.h" #include "EKF_RP.h" #include "EKF_RPY.h" using namespace Eigen; Serial pc(SERIAL_TX, SERIAL_RX); EKF_RP ekf_rp(0.02f); EKF_RPY ekf_rpy(0.02f, 0.1554364f, -0.0879864f, 0.3507829f); Timer timer; // timer for time measurement float dt = 0.0f; uint32_t i; Matrix<float, 9, 9> A; Matrix<float, 9, 1> b; Matrix<float, 8, 8> I; // Matrix<float, 7, 1> gyro_x; // Matrix<float, 7, 1> gyro_y; // Matrix<float, 7, 1> gyro_z; // Matrix<float, 7, 1> accel_x; // Matrix<float, 7, 1> accel_y; // Matrix<float, 7, 1> magnet_x; // Matrix<float, 7, 1> magnet_y; Matrix<float, 14, 1> gyro_x; Matrix<float, 14, 1> gyro_y; Matrix<float, 14, 1> gyro_z; Matrix<float, 14, 1> accel_x; Matrix<float, 14, 1> accel_y; Matrix<float, 14, 1> magnet_x; Matrix<float, 14, 1> magnet_y; int main() { pc.baud(2000000); timer.start(); i = 0; ///* A << 1, 2, 3, 5, 1, 8,10, 1, 3, 4, 5, 6, 8, 4, 2, 1, 9, 4, 7, 8,10, 5, 6, 8, 4, 5, 1, 4, 2,10, 8,10, 5, 6, 7, 8, 1, 8, 7, 3, 4, 6, 5, 1, 7, 4, 2, 7, 5, 7, 6, 9, 2, 1, 5, 5, 1, 7, 4, 2, 1, 1, 9, 8, 9, 7, 4, 5, 6, 1, 2, 2, 1, 5, 9, 4, 8, 7, 2, 6, 3; b << 3, 3, 4, 2, 7, 5, 1, 8, 1; //*/ /* A << 1, 2, 3, 5, 1, 8,10, 1, 4, 5, 6, 8, 4, 2, 1, 9, 7, 8,10, 5, 6, 8, 4, 5, 4, 2,10, 8,10, 5, 6, 7, 1, 8, 7, 3, 4, 6, 5, 1, 4, 2, 7, 5, 7, 6, 9, 2, 5, 5, 1, 7, 4, 2, 1, 1, 8, 9, 7, 4, 5, 6, 1, 2; b << 3, 3, 4, 2, 7, 5, 1, 8; */ I.setIdentity(); // I = I.transpose(); // gyro_x << 0.1, 0.5, -0.2, 0.2, 0.3, 0.2, -0.7; // gyro_y << -0.1, -0.3, 0.1, 0.1, 0.1, -0.1, -0.9; // gyro_z << 0.5, 0.4, -0.3, 0.2, 0.8, -0.6, 0.2; // accel_x << 0.3, -0.1, 0.7, -0.3, -0.9, 0.1, 0.2; // accel_y << 0.1, 0.5, -0.4, -0.2, -0.4, 0.5, 0.8; // magnet_x << 0.1, -0.2, -0.8, 0.2, 0.6, 0.4, -0.4; // magnet_y << 0.6, -0.4, 0.4, 0.7, 0.7, 0.5, 0.3; gyro_x << 0.1f, 0.5f, -0.2f, 0.2f, 0.3f, 0.2f, -0.7f, 0.5f, 0.4f, -0.3f, 0.2f, 0.8f, -0.6f, 0.2f; gyro_y << -0.1f, -0.3f, 0.1f, 0.1f, 0.1f, -0.1f, -0.9f, 0.1f, 0.5f, -0.4f, -0.2f, -0.4f, 0.5f, 0.8f; gyro_z << 0.5f, 0.4f, -0.3f, 0.2f, 0.8f, -0.6f, 0.2f,-0.1f, -0.3f, 0.1f, 0.1f, 0.1f, -0.1f, -0.9f; accel_x << 0.3f, -0.1f, 0.7f, -0.3f, -0.9f, 0.1f, 0.2f, 0.1f, 0.5f, -0.2f, 0.2f, 0.3f, 0.2f, -0.7f; accel_y << 0.1f, 0.5f, -0.4f, -0.2f, -0.4f, 0.5f, 0.8f, 0.1f, -0.2f, -0.8f, 0.2f, 0.6f, 0.4f, -0.4f; magnet_x << 0.1f, -0.2f, -0.8f, 0.2f, 0.6f, 0.4f, -0.4f, 0.6f, -0.4f, 0.4f, 0.7f, 0.7f, 0.5f, 0.3f; magnet_y << 0.6f, -0.4f, 0.4f, 0.7f, 0.7f, 0.5f, 0.3f, 0.3f, -0.1f, 0.7f, -0.3f, -0.9f, 0.1f, 0.2f; pc.printf("\r\n"); while(1) { // Matrix<float, 9, 1> x = A.inverse() * b; // float temp0 = (float)(rand()%10000-4999)*0.00001f; // float temp1 = (float)(rand()%10000-4999)*0.00001f; // float temp2 = (float)(rand()%10000-4999)*0.00001f; // float temp3 = (float)(rand()%10000-4999)*0.00001f; // float temp4 = (float)(rand()%10000-4999)*0.00001f; // float temp5 = (float)(rand()%10000-4999)*0.00001f; // ekf_rp.update(gyro_x(i), gyro_y(i), accel_x(i), accel_y(i)); dt = timer.read(); timer.reset(); if( i < 14 ) { /* ekf_rp */ ekf_rp.update(gyro_x(i), gyro_y(i), accel_x(i), accel_y(i)); // 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); pc.printf("%.7f; \r\n", dt); /* ekf_rpy */ // ekf_rpy.update(gyro_x(i), gyro_y(i), gyro_z(i), accel_x(i), accel_y(i), magnet_x(i), magnet_y(i)); // 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); // pc.printf("%.7f; \r\n", dt); } i++; // wait_us(1000000); } }