![](/media/cache/group/default_image.jpg.50x50_q85.jpg)
Testing ekf implementation for Quadro_1.
Diff: main.cpp
- Revision:
- 17:1d98928f7681
- Parent:
- 16:c39e084f05ed
- Child:
- 21:aab1ac72095b
diff -r c39e084f05ed -r 1d98928f7681 main.cpp --- a/main.cpp Thu Oct 24 06:43:17 2019 +0000 +++ b/main.cpp Fri Oct 25 08:44:32 2019 +0000 @@ -22,13 +22,13 @@ Matrix<float, 8, 1> b; Matrix<float, 8, 8> I; -Matrix<float, 4, 1> gyro_x; -Matrix<float, 4, 1> gyro_y; -Matrix<float, 4, 1> gyro_z; -Matrix<float, 4, 1> accel_x; -Matrix<float, 4, 1> accel_y; -Matrix<float, 4, 1> magnet_x; -Matrix<float, 4, 1> magnet_y; +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; int main() { @@ -79,13 +79,13 @@ I.setIdentity(); // I = I.transpose(); - gyro_x << 0.01, 0.05, -0.02, 0.02; - gyro_y << -0.01, -0.03, 0.01, 0.01; - gyro_z << 0.05, 0.04, -0.03, 0.02; - accel_x << 0.03, -0.01, 0.07, -0.03; - accel_y << 0.01, 0.05, -0.04, -0.02; - magnet_x << 0.01, -0.02, -0.08, 0.02; - magnet_y << 0.06, -0.04, 0.04, 0.07; + 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; pc.printf("\r\n"); @@ -103,17 +103,17 @@ dt = timer.read(); timer.reset(); - if( i < 4 ) { + if( i < 7 ) { /* 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); + 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); + // 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++; - + i++; wait_us(1000000); } }