From 1e766c75100e6d35d0d94607dfea99368ef589e6 Mon Sep 17 00:00:00 2001 From: CarlOlsson Date: Fri, 12 Feb 2016 15:54:32 +0100 Subject: [PATCH] moved fuse function to ekf_helper.cpp --- EKF/ekf_helper.cpp | 38 ++++++++++++++++++++++++++++++++++++++ EKF/vel_pos_fusion.cpp | 36 ------------------------------------ 2 files changed, 38 insertions(+), 36 deletions(-) diff --git a/EKF/ekf_helper.cpp b/EKF/ekf_helper.cpp index c5c830a326..5796cd8628 100644 --- a/EKF/ekf_helper.cpp +++ b/EKF/ekf_helper.cpp @@ -275,3 +275,41 @@ void Ekf::get_ekf_origin(uint64_t *origin_time, map_projection_reference_s *orig memcpy(origin_pos, &_pos_ref, sizeof(map_projection_reference_s)); memcpy(origin_alt, &_gps_alt_ref, sizeof(float)); } + +// fuse measurement +void Ekf::fuse(float *K, float innovation) +{ + for (unsigned i = 0; i < 3; i++) { + _state.ang_error(i) = _state.ang_error(i) - K[i] * innovation; + } + + for (unsigned i = 0; i < 3; i++) { + _state.vel(i) = _state.vel(i) - K[i + 3] * innovation; + } + + for (unsigned i = 0; i < 3; i++) { + _state.pos(i) = _state.pos(i) - K[i + 6] * innovation; + } + + for (unsigned i = 0; i < 3; i++) { + _state.gyro_bias(i) = _state.gyro_bias(i) - K[i + 9] * innovation; + } + + for (unsigned i = 0; i < 3; i++) { + _state.gyro_scale(i) = _state.gyro_scale(i) - K[i + 12] * innovation; + } + + _state.accel_z_bias -= K[15] * innovation; + + for (unsigned i = 0; i < 3; i++) { + _state.mag_I(i) = _state.mag_I(i) - K[i + 16] * innovation; + } + + for (unsigned i = 0; i < 3; i++) { + _state.mag_B(i) = _state.mag_B(i) - K[i + 19] * innovation; + } + + for (unsigned i = 0; i < 2; i++) { + _state.wind_vel(i) = _state.wind_vel(i) - K[i + 22] * innovation; + } +} diff --git a/EKF/vel_pos_fusion.cpp b/EKF/vel_pos_fusion.cpp index bce1fd1221..dcc695262f 100644 --- a/EKF/vel_pos_fusion.cpp +++ b/EKF/vel_pos_fusion.cpp @@ -190,39 +190,3 @@ void Ekf::fuseVelPosHeight() } -void Ekf::fuse(float *K, float innovation) -{ - for (unsigned i = 0; i < 3; i++) { - _state.ang_error(i) = _state.ang_error(i) - K[i] * innovation; - } - - for (unsigned i = 0; i < 3; i++) { - _state.vel(i) = _state.vel(i) - K[i + 3] * innovation; - } - - for (unsigned i = 0; i < 3; i++) { - _state.pos(i) = _state.pos(i) - K[i + 6] * innovation; - } - - for (unsigned i = 0; i < 3; i++) { - _state.gyro_bias(i) = _state.gyro_bias(i) - K[i + 9] * innovation; - } - - for (unsigned i = 0; i < 3; i++) { - _state.gyro_scale(i) = _state.gyro_scale(i) - K[i + 12] * innovation; - } - - _state.accel_z_bias -= K[15] * innovation; - - for (unsigned i = 0; i < 3; i++) { - _state.mag_I(i) = _state.mag_I(i) - K[i + 16] * innovation; - } - - for (unsigned i = 0; i < 3; i++) { - _state.mag_B(i) = _state.mag_B(i) - K[i + 19] * innovation; - } - - for (unsigned i = 0; i < 2; i++) { - _state.wind_vel(i) = _state.wind_vel(i) - K[i + 22] * innovation; - } -}