altb_pmic
/
Test_ekf
Testing ekf implementation for Quadro_1.
main.cpp@24:e5188a2d72ca, 2020-02-05 (annotated)
- Committer:
- pmic
- Date:
- Wed Feb 05 15:35:01 2020 +0000
- Revision:
- 24:e5188a2d72ca
- Parent:
- 23:226f18dd30a5
Update EKF_RP 6 states to EKF_RP 8 states.
Who changed what in which revision?
User | Revision | Line number | New 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 | 21:aab1ac72095b | 21 | Matrix<float, 9, 9> A; |
pmic | 21:aab1ac72095b | 22 | Matrix<float, 9, 1> b; |
pmic | 15:53485bd1ff28 | 23 | Matrix<float, 8, 8> I; |
pmic | 0:a0e9705be9c4 | 24 | |
pmic | 23:226f18dd30a5 | 25 | // Matrix<float, 7, 1> gyro_x; |
pmic | 23:226f18dd30a5 | 26 | // Matrix<float, 7, 1> gyro_y; |
pmic | 23:226f18dd30a5 | 27 | // Matrix<float, 7, 1> gyro_z; |
pmic | 23:226f18dd30a5 | 28 | // Matrix<float, 7, 1> accel_x; |
pmic | 23:226f18dd30a5 | 29 | // Matrix<float, 7, 1> accel_y; |
pmic | 23:226f18dd30a5 | 30 | // Matrix<float, 7, 1> magnet_x; |
pmic | 23:226f18dd30a5 | 31 | // Matrix<float, 7, 1> magnet_y; |
pmic | 23:226f18dd30a5 | 32 | |
pmic | 23:226f18dd30a5 | 33 | Matrix<float, 14, 1> gyro_x; |
pmic | 23:226f18dd30a5 | 34 | Matrix<float, 14, 1> gyro_y; |
pmic | 23:226f18dd30a5 | 35 | Matrix<float, 14, 1> gyro_z; |
pmic | 23:226f18dd30a5 | 36 | Matrix<float, 14, 1> accel_x; |
pmic | 23:226f18dd30a5 | 37 | Matrix<float, 14, 1> accel_y; |
pmic | 23:226f18dd30a5 | 38 | Matrix<float, 14, 1> magnet_x; |
pmic | 23:226f18dd30a5 | 39 | Matrix<float, 14, 1> magnet_y; |
pmic | 4:e50e18eac72b | 40 | |
pmic | 0:a0e9705be9c4 | 41 | int main() |
pmic | 0:a0e9705be9c4 | 42 | { |
pmic | 0:a0e9705be9c4 | 43 | pc.baud(2000000); |
pmic | 0:a0e9705be9c4 | 44 | |
pmic | 0:a0e9705be9c4 | 45 | timer.start(); |
pmic | 0:a0e9705be9c4 | 46 | |
pmic | 4:e50e18eac72b | 47 | i = 0; |
pmic | 0:a0e9705be9c4 | 48 | |
pmic | 21:aab1ac72095b | 49 | ///* |
pmic | 1:6b803652d032 | 50 | A << 1, 2, 3, 5, 1, 8,10, 1, 3, |
pmic | 1:6b803652d032 | 51 | 4, 5, 6, 8, 4, 2, 1, 9, 4, |
pmic | 1:6b803652d032 | 52 | 7, 8,10, 5, 6, 8, 4, 5, 1, |
pmic | 1:6b803652d032 | 53 | 4, 2,10, 8,10, 5, 6, 7, 8, |
pmic | 1:6b803652d032 | 54 | 1, 8, 7, 3, 4, 6, 5, 1, 7, |
pmic | 1:6b803652d032 | 55 | 4, 2, 7, 5, 7, 6, 9, 2, 1, |
pmic | 1:6b803652d032 | 56 | 5, 5, 1, 7, 4, 2, 1, 1, 9, |
pmic | 1:6b803652d032 | 57 | 8, 9, 7, 4, 5, 6, 1, 2, 2, |
pmic | 1:6b803652d032 | 58 | 1, 5, 9, 4, 8, 7, 2, 6, 3; |
pmic | 0:a0e9705be9c4 | 59 | b << 3, |
pmic | 0:a0e9705be9c4 | 60 | 3, |
pmic | 1:6b803652d032 | 61 | 4, |
pmic | 1:6b803652d032 | 62 | 2, |
pmic | 1:6b803652d032 | 63 | 7, |
pmic | 1:6b803652d032 | 64 | 5, |
pmic | 1:6b803652d032 | 65 | 1, |
pmic | 1:6b803652d032 | 66 | 8, |
pmic | 1:6b803652d032 | 67 | 1; |
pmic | 21:aab1ac72095b | 68 | //*/ |
pmic | 1:6b803652d032 | 69 | |
pmic | 21:aab1ac72095b | 70 | /* |
pmic | 1:6b803652d032 | 71 | A << 1, 2, 3, 5, 1, 8,10, 1, |
pmic | 1:6b803652d032 | 72 | 4, 5, 6, 8, 4, 2, 1, 9, |
pmic | 1:6b803652d032 | 73 | 7, 8,10, 5, 6, 8, 4, 5, |
pmic | 1:6b803652d032 | 74 | 4, 2,10, 8,10, 5, 6, 7, |
pmic | 1:6b803652d032 | 75 | 1, 8, 7, 3, 4, 6, 5, 1, |
pmic | 1:6b803652d032 | 76 | 4, 2, 7, 5, 7, 6, 9, 2, |
pmic | 1:6b803652d032 | 77 | 5, 5, 1, 7, 4, 2, 1, 1, |
pmic | 1:6b803652d032 | 78 | 8, 9, 7, 4, 5, 6, 1, 2; |
pmic | 1:6b803652d032 | 79 | b << 3, |
pmic | 1:6b803652d032 | 80 | 3, |
pmic | 1:6b803652d032 | 81 | 4, |
pmic | 1:6b803652d032 | 82 | 2, |
pmic | 1:6b803652d032 | 83 | 7, |
pmic | 1:6b803652d032 | 84 | 5, |
pmic | 1:6b803652d032 | 85 | 1, |
pmic | 1:6b803652d032 | 86 | 8; |
pmic | 21:aab1ac72095b | 87 | */ |
pmic | 0:a0e9705be9c4 | 88 | |
pmic | 10:dd5ea359fb97 | 89 | I.setIdentity(); |
pmic | 16:c39e084f05ed | 90 | // I = I.transpose(); |
pmic | 4:e50e18eac72b | 91 | |
pmic | 23:226f18dd30a5 | 92 | // gyro_x << 0.1, 0.5, -0.2, 0.2, 0.3, 0.2, -0.7; |
pmic | 23:226f18dd30a5 | 93 | // gyro_y << -0.1, -0.3, 0.1, 0.1, 0.1, -0.1, -0.9; |
pmic | 23:226f18dd30a5 | 94 | // gyro_z << 0.5, 0.4, -0.3, 0.2, 0.8, -0.6, 0.2; |
pmic | 23:226f18dd30a5 | 95 | // accel_x << 0.3, -0.1, 0.7, -0.3, -0.9, 0.1, 0.2; |
pmic | 23:226f18dd30a5 | 96 | // accel_y << 0.1, 0.5, -0.4, -0.2, -0.4, 0.5, 0.8; |
pmic | 23:226f18dd30a5 | 97 | // magnet_x << 0.1, -0.2, -0.8, 0.2, 0.6, 0.4, -0.4; |
pmic | 23:226f18dd30a5 | 98 | // magnet_y << 0.6, -0.4, 0.4, 0.7, 0.7, 0.5, 0.3; |
pmic | 23:226f18dd30a5 | 99 | |
pmic | 23:226f18dd30a5 | 100 | 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; |
pmic | 23:226f18dd30a5 | 101 | 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; |
pmic | 23:226f18dd30a5 | 102 | 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; |
pmic | 23:226f18dd30a5 | 103 | 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; |
pmic | 23:226f18dd30a5 | 104 | 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; |
pmic | 23:226f18dd30a5 | 105 | 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; |
pmic | 23:226f18dd30a5 | 106 | 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; |
pmic | 4:e50e18eac72b | 107 | |
pmic | 4:e50e18eac72b | 108 | pc.printf("\r\n"); |
pmic | 1:6b803652d032 | 109 | |
pmic | 0:a0e9705be9c4 | 110 | while(1) { |
pmic | 0:a0e9705be9c4 | 111 | |
pmic | 23:226f18dd30a5 | 112 | // Matrix<float, 9, 1> x = A.inverse() * b; |
pmic | 0:a0e9705be9c4 | 113 | |
pmic | 4:e50e18eac72b | 114 | // float temp0 = (float)(rand()%10000-4999)*0.00001f; |
pmic | 4:e50e18eac72b | 115 | // float temp1 = (float)(rand()%10000-4999)*0.00001f; |
pmic | 4:e50e18eac72b | 116 | // float temp2 = (float)(rand()%10000-4999)*0.00001f; |
pmic | 4:e50e18eac72b | 117 | // float temp3 = (float)(rand()%10000-4999)*0.00001f; |
pmic | 4:e50e18eac72b | 118 | // float temp4 = (float)(rand()%10000-4999)*0.00001f; |
pmic | 4:e50e18eac72b | 119 | // float temp5 = (float)(rand()%10000-4999)*0.00001f; |
pmic | 4:e50e18eac72b | 120 | // ekf_rp.update(gyro_x(i), gyro_y(i), accel_x(i), accel_y(i)); |
pmic | 0:a0e9705be9c4 | 121 | dt = timer.read(); |
pmic | 0:a0e9705be9c4 | 122 | timer.reset(); |
pmic | 0:a0e9705be9c4 | 123 | |
pmic | 23:226f18dd30a5 | 124 | if( i < 14 ) { |
pmic | 8:a72cd6356bab | 125 | /* ekf_rp */ |
pmic | 17:1d98928f7681 | 126 | ekf_rp.update(gyro_x(i), gyro_y(i), accel_x(i), accel_y(i)); |
pmic | 23:226f18dd30a5 | 127 | // 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 | 23:226f18dd30a5 | 128 | pc.printf("%.7f; \r\n", dt); |
pmic | 8:a72cd6356bab | 129 | /* ekf_rpy */ |
pmic | 17:1d98928f7681 | 130 | // 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 | 17:1d98928f7681 | 131 | // 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 | 17:1d98928f7681 | 132 | // pc.printf("%.7f; \r\n", dt); |
pmic | 4:e50e18eac72b | 133 | } |
pmic | 17:1d98928f7681 | 134 | i++; |
pmic | 23:226f18dd30a5 | 135 | // wait_us(1000000); |
pmic | 0:a0e9705be9c4 | 136 | } |
pmic | 0:a0e9705be9c4 | 137 | } |