From 689ab12845ae689b493939950652ed2d889da269 Mon Sep 17 00:00:00 2001 From: bresch Date: Mon, 25 Oct 2021 18:24:12 +0200 Subject: [PATCH] ekf2: refactor gps vel/pos fusion control logic --- src/modules/ekf2/CMakeLists.txt | 1 + src/modules/ekf2/EKF/CMakeLists.txt | 1 + src/modules/ekf2/EKF/control.cpp | 308 ++++++++++++---------------- src/modules/ekf2/EKF/ekf.h | 6 + src/modules/ekf2/EKF/gps_fusion.cpp | 91 ++++++++ 5 files changed, 230 insertions(+), 177 deletions(-) create mode 100644 src/modules/ekf2/EKF/gps_fusion.cpp diff --git a/src/modules/ekf2/CMakeLists.txt b/src/modules/ekf2/CMakeLists.txt index eb5b40b0d3..31b2946f19 100644 --- a/src/modules/ekf2/CMakeLists.txt +++ b/src/modules/ekf2/CMakeLists.txt @@ -54,6 +54,7 @@ px4_add_module( EKF/EKFGSF_yaw.cpp EKF/estimator_interface.cpp EKF/gps_checks.cpp + EKF/gps_fusion.cpp EKF/gps_yaw_fusion.cpp EKF/imu_down_sampler.cpp EKF/mag_control.cpp diff --git a/src/modules/ekf2/EKF/CMakeLists.txt b/src/modules/ekf2/EKF/CMakeLists.txt index daeb559a8f..8b11a0dcb1 100644 --- a/src/modules/ekf2/EKF/CMakeLists.txt +++ b/src/modules/ekf2/EKF/CMakeLists.txt @@ -42,6 +42,7 @@ add_library(ecl_EKF EKFGSF_yaw.cpp estimator_interface.cpp gps_checks.cpp + gps_fusion.cpp gps_yaw_fusion.cpp imu_down_sampler.cpp mag_control.cpp diff --git a/src/modules/ekf2/EKF/control.cpp b/src/modules/ekf2/EKF/control.cpp index fd125244a4..d50c547527 100644 --- a/src/modules/ekf2/EKF/control.cpp +++ b/src/modules/ekf2/EKF/control.cpp @@ -521,6 +521,11 @@ void Ekf::resetOnGroundMotionForOpticalFlowChecks() void Ekf::controlGpsFusion() { + if (!(_params.fusion_mode & MASK_USE_GPS)) { + stopGpsFusion(); + return; + } + // Check for new GPS data that has fallen behind the fusion time horizon if (_gps_data_ready) { @@ -531,8 +536,91 @@ void Ekf::controlGpsFusion() // Determine if we should use GPS aiding for velocity and horizontal position // To start using GPS we need angular alignment completed, the local NED origin set and GPS data that has not failed checks recently - if ((_params.fusion_mode & MASK_USE_GPS) && !_control_status.flags.gps) { - if (_control_status.flags.tilt_align && _NED_origin_initialised && gps_checks_passing) { + const bool mandatory_conditions_passing = _control_status.flags.tilt_align + && _control_status.flags.yaw_align + && _NED_origin_initialised; + const bool continuing_conditions_passing = mandatory_conditions_passing + && !gps_checks_failing; + const bool starting_conditions_passing = continuing_conditions_passing + && gps_checks_passing; + + if (_control_status.flags.gps) { + if (mandatory_conditions_passing) { + if (continuing_conditions_passing + || !isOtherSourceOfHorizontalAidingThan(_control_status.flags.gps)) { + fuseGpsVelPos(); + + // We are relying on aiding to constrain drift so after a specified time + // with no aiding we need to do something + const bool do_vel_pos_reset = hasHorizontalAidingTimedOut() + || isTimedOut(_time_last_hor_pos_fuse, 2 * _params.reset_timeout_max); + + /* Logic controlling the reset of navigation filter yaw to the EKF-GSF estimate to recover from loss of + navigation casued by a bad yaw estimate. + + A rapid reset to the EKF-GSF estimate is performed after a recent takeoff if horizontal velocity + innovation checks fail. This enables recovery from a bad yaw estimate. After 30 seconds from takeoff, + different test criteria are used that take longer to trigger and reduce false positives. A reset is + not performed if the fault condition was present before flight to prevent triggering due to GPS glitches + or other sensor errors. + + The yaw reset to the EKF-GSF estimate can be requested externally at any time during flight. + + The total number of resets allowed per boot cycle is limited. + + The minimum time interval between resets to the EKF-GSF estimate is limited to allow the EKF-GSF time + to improve its estimate if the previous reset was not successful. + + A reset is not performed when getting GPS back after a significant period of no data because the timeout + could have been caused by bad GPS. + */ + const bool is_recent_takeoff_nav_failure = isTimedOut(_time_last_hor_vel_fuse, _params.EKFGSF_reset_delay) + && (_time_last_hor_vel_fuse > _time_last_on_ground_us) + && isRecentTakeoff(); + + + const bool is_inflight_nav_failure = _control_status.flags.in_air && + do_vel_pos_reset && + (_time_last_hor_vel_fuse > _time_last_on_ground_us) && + (_time_last_hor_pos_fuse > _time_last_on_ground_us); + + if (is_recent_takeoff_nav_failure || is_inflight_nav_failure){ + const bool is_yaw_failure = !isVelStateAlignedWithObs(); + const bool was_gps_signal_lost = isTimedOut(_time_prev_gps_us, 1000000); + + if (is_yaw_failure + && !was_gps_signal_lost + && _ekfgsf_yaw_reset_count < _params.EKFGSF_reset_count_limit) { + + _do_ekfgsf_yaw_reset = true; + _ekfgsf_yaw_reset_count++; + + } else { + + // use GPS velocity data to check and correct yaw angle if a FW vehicle + if (_control_status.flags.fixed_wing && _control_status.flags.in_air) { + // if flying a fixed wing aircraft, do a complete reset that includes yaw + _control_status.flags.mag_aligned_in_flight = realignYawGPS(); + } + + _warning_events.flags.gps_fusion_timout = true; + ECL_WARN("GPS fusion timeout - resetting"); + _velpos_reset_request = true; + } + } + + } else { + stopGpsFusion(); + _warning_events.flags.gps_quality_poor = true; + ECL_WARN("GPS quality poor - stopping use"); + } + + } else { // mandatory conditions are not passing + stopGpsFusion(); + } + + } else { + if (starting_conditions_passing) { // If the heading is not aligned, reset the yaw and magnetic field states // Do not use external vision for yaw if using GPS because yaw needs to be // defined relative to an NED reference frame @@ -546,198 +634,37 @@ void Ekf::controlGpsFusion() stopEvYawFusion(); _inhibit_ev_yaw_use = true; - } else if (_control_status.flags.yaw_align) { - // If the heading is valid start using gps aiding + } else { startGpsFusion(); } } - - } else if (_control_status.flags.gps - && (!(_params.fusion_mode & MASK_USE_GPS) || !_control_status.flags.yaw_align)) { - - stopGpsFusion(); } - // Handle the case where we are using GPS and another source of aiding and GPS is failing checks - if (_control_status.flags.gps && gps_checks_failing && isOtherSourceOfHorizontalAidingThan(_control_status.flags.gps)) { - stopGpsFusion(); - - // Reset position state to external vision if we are going to use absolute values - if (_control_status.flags.ev_pos && !(_params.fusion_mode & MASK_ROTATE_EV)) { - resetHorizontalPosition(); - } - - _warning_events.flags.gps_quality_poor = true; - ECL_WARN("GPS quality poor - stopping use"); - } - - // handle case where we are not currently using GPS, but need to align yaw angle using EKF-GSF before - // we can start using GPS const bool align_yaw_using_gsf = !_control_status.flags.yaw_align && (_params.mag_fusion_type == MAG_FUSE_TYPE_NONE); + _do_ekfgsf_yaw_reset = _do_ekfgsf_yaw_reset || align_yaw_using_gsf; - if ((align_yaw_using_gsf || _do_ekfgsf_yaw_reset) + // Process yaw estimator reset request + if (_do_ekfgsf_yaw_reset && isTimedOut(_ekfgsf_yaw_reset_time, 5000000)){ if (resetYawToEKFGSF()) { _ekfgsf_yaw_reset_time = _time_last_imu; - _do_ekfgsf_yaw_reset = false; - } - } - - // handle the case when we now have GPS, but have not been fusing it for an extended period - if (_control_status.flags.gps) { - // We are relying on aiding to constrain drift so after a specified time - // with no aiding we need to do something - bool do_vel_pos_reset = isTimedOut(_time_last_hor_pos_fuse, _params.reset_timeout_max) - && isTimedOut(_time_last_delpos_fuse, _params.reset_timeout_max) - && isTimedOut(_time_last_hor_vel_fuse, _params.reset_timeout_max) - && isTimedOut(_time_last_of_fuse, _params.reset_timeout_max); - - // We haven't had an absolute position fix for a longer time so need to do something - do_vel_pos_reset = do_vel_pos_reset || isTimedOut(_time_last_hor_pos_fuse, 2 * _params.reset_timeout_max); - - /* Logic controlling the reset of navigation filter yaw to the EKF-GSF estimate to recover from loss of - navigation casued by a bad yaw estimate. - - A rapid reset to the EKF-GSF estimate is performed after a recent takeoff if horizontal velocity - innovation checks fail. This enables recovery from a bad yaw estimate. After 30 seconds from takeoff, - different test criteria are used that take longer to trigger and reduce false positives. A reset is - not performed if the fault condition was present before flight to prevent triggering due to GPS glitches - or other sensor errors. - - The yaw reset to the EKF-GSF estimate can be requested externally at any time during flight. - - The total number of resets allowed per boot cycle is limited. - - The minimum time interval between resets to the EKF-GSF estimate is limited to allow the EKF-GSF time - to improve its estimate if the previous reset was not successful. - - A reset is not performed when getting GPS back after a significant period of no data because the timeout - could have been caused by bad GPS. - */ - - const bool recent_takeoff_nav_failure = _control_status.flags.in_air && - !isTimedOut(_time_last_on_ground_us, 30000000) && - isTimedOut(_time_last_hor_vel_fuse, _params.EKFGSF_reset_delay) && - (_time_last_hor_vel_fuse > _time_last_on_ground_us); - - const bool inflight_nav_failure = _control_status.flags.in_air && - do_vel_pos_reset && - (_time_last_hor_vel_fuse > _time_last_on_ground_us) && - (_time_last_hor_pos_fuse > _time_last_on_ground_us); - - bool is_yaw_failure = false; - - if ((recent_takeoff_nav_failure || inflight_nav_failure) && _time_last_hor_vel_fuse > 0) { - // Do sanity check to see if the innovation failures is likely caused by a yaw angle error - // by measuring the angle between the velocity estimate and the last velocity observation - // Only use those vectors if their norm if they are larger than 4 times their noise standard deviation - const float vel_obs_xy_norm_sq = _last_vel_obs.xy().norm_squared(); - const float vel_state_xy_norm_sq = _state.vel.xy().norm_squared(); - - const float vel_obs_threshold_sq = fmaxf(sq(4.f) * (_last_vel_obs_var(0) + _last_vel_obs_var(1)), sq(0.4f)); - const float vel_state_threshold_sq = fmaxf(sq(4.f) * (P(4, 4) + P(5, 5)), sq(0.4f)); - - if (vel_obs_xy_norm_sq > vel_obs_threshold_sq && vel_state_xy_norm_sq > vel_state_threshold_sq) { - const float obs_dot_vel = Vector2f(_last_vel_obs).dot(_state.vel.xy()); - const float cos_sq = sq(obs_dot_vel) / (vel_state_xy_norm_sq * vel_obs_xy_norm_sq); - - if (cos_sq < sq(cosf(math::radians(25.f))) || obs_dot_vel < 0.f) { - // The angle between the observation and the velocity estimate is greater than 25 degrees - is_yaw_failure = true; - } - } - } - - // Detect if coming back after significant time without GPS data - const bool gps_signal_was_lost = isTimedOut(_time_prev_gps_us, 1000000); - const bool do_yaw_vel_pos_reset = (_do_ekfgsf_yaw_reset || is_yaw_failure) && - _ekfgsf_yaw_reset_count < _params.EKFGSF_reset_count_limit && - isTimedOut(_ekfgsf_yaw_reset_time, 5000000) && - !gps_signal_was_lost; - - if (do_yaw_vel_pos_reset) { - if (resetYawToEKFGSF()) { - _ekfgsf_yaw_reset_time = _time_last_imu; - _do_ekfgsf_yaw_reset = false; - _ekfgsf_yaw_reset_count++; - - // Reset the timeout counters - _time_last_hor_pos_fuse = _time_last_imu; - _time_last_delpos_fuse = _time_last_imu; - _time_last_hor_vel_fuse = _time_last_imu; - _time_last_of_fuse = _time_last_imu; - } - - } else if (do_vel_pos_reset) { - // use GPS velocity data to check and correct yaw angle if a FW vehicle - if (_control_status.flags.fixed_wing && _control_status.flags.in_air) { - // if flying a fixed wing aircraft, do a complete reset that includes yaw - _control_status.flags.mag_aligned_in_flight = realignYawGPS(); - } - - resetVelocity(); - resetHorizontalPosition(); - _velpos_reset_request = false; - _warning_events.flags.gps_fusion_timout = true; - ECL_WARN("GPS fusion timeout - reset to GPS"); - - // Reset the timeout counters _time_last_hor_pos_fuse = _time_last_imu; _time_last_hor_vel_fuse = _time_last_imu; + + _do_ekfgsf_yaw_reset = false; + _velpos_reset_request = false; // included in yaw reset } } - // Only use GPS data for position and velocity aiding if enabled - if (_control_status.flags.gps) { + if (_velpos_reset_request) { + resetVelocity(); + resetHorizontalPosition(); + _velpos_reset_request = false; - Vector2f gps_vel_innov_gates; // [horizontal vertical] - Vector2f gps_pos_innov_gates; // [horizontal vertical] - Vector3f gps_pos_obs_var; - - // correct velocity for offset relative to IMU - const Vector3f pos_offset_body = _params.gps_pos_body - _params.imu_pos_body; - const Vector3f vel_offset_body = _ang_rate_delayed_raw % pos_offset_body; - const Vector3f vel_offset_earth = _R_to_earth * vel_offset_body; - _gps_sample_delayed.vel -= vel_offset_earth; - - // correct position and height for offset relative to IMU - const Vector3f pos_offset_earth = _R_to_earth * pos_offset_body; - _gps_sample_delayed.pos -= pos_offset_earth.xy(); - _gps_sample_delayed.hgt += pos_offset_earth(2); - - const float lower_limit = fmaxf(_params.gps_pos_noise, 0.01f); - - if (isOtherSourceOfHorizontalAidingThan(_control_status.flags.gps)) { - // if we are using other sources of aiding, then relax the upper observation - // noise limit which prevents bad GPS perturbing the position estimate - gps_pos_obs_var(0) = gps_pos_obs_var(1) = sq(fmaxf(_gps_sample_delayed.hacc, lower_limit)); - - } else { - // if we are not using another source of aiding, then we are reliant on the GPS - // observations to constrain attitude errors and must limit the observation noise value. - float upper_limit = fmaxf(_params.pos_noaid_noise, lower_limit); - gps_pos_obs_var(0) = gps_pos_obs_var(1) = sq(math::constrain(_gps_sample_delayed.hacc, lower_limit, upper_limit)); - } - - _gps_sample_delayed.sacc = fmaxf(_gps_sample_delayed.sacc, _params.gps_vel_noise); - - _last_vel_obs_var.setAll(sq(_gps_sample_delayed.sacc)); - _last_vel_obs_var(2) *= sq(1.5f); - - // calculate innovations - _last_vel_obs = _gps_sample_delayed.vel; - _gps_vel_innov = _state.vel - _last_vel_obs; - _gps_pos_innov.xy() = Vector2f(_state.pos) - _gps_sample_delayed.pos; - - // set innovation gate size - gps_pos_innov_gates(0) = fmaxf(_params.gps_pos_innov_gate, 1.0f); - gps_vel_innov_gates(0) = gps_vel_innov_gates(1) = fmaxf(_params.gps_vel_innov_gate, 1.0f); - - // fuse GPS measurement - fuseHorizontalVelocity(_gps_vel_innov, gps_vel_innov_gates, _last_vel_obs_var, _gps_vel_innov_var, _gps_vel_test_ratio); - fuseVerticalVelocity(_gps_vel_innov, gps_vel_innov_gates, _last_vel_obs_var, _gps_vel_innov_var, _gps_vel_test_ratio); - fuseHorizontalPosition(_gps_pos_innov, gps_pos_innov_gates, gps_pos_obs_var, _gps_pos_innov_var, _gps_pos_test_ratio); + // Reset the timeout counters + _time_last_hor_pos_fuse = _time_last_imu; + _time_last_hor_vel_fuse = _time_last_imu; } } else if (_control_status.flags.gps && (_imu_sample_delayed.time_us - _gps_sample_delayed.time_us > (uint64_t)10e6)) { @@ -753,6 +680,33 @@ void Ekf::controlGpsFusion() _warning_events.flags.gps_data_stopped_using_alternate = true; ECL_WARN("GPS data stopped, using only EV, OF or air data"); } + //TODO: move that resetHorizontalPosition to EV + /* // Handle the case where we are using GPS and another source of aiding and GPS is failing checks */ + /* if (_control_status.flags.gps && gps_checks_failing && isOtherSourceOfHorizontalAidingThan(_control_status.flags.gps)) { */ + /* stopGpsFusion(); */ + + /* // Reset position state to external vision if we are going to use absolute values */ + /* if (_control_status.flags.ev_pos && !(_params.fusion_mode & MASK_ROTATE_EV)) { */ + /* resetHorizontalPosition(); */ + /* } */ + + /* _warning_events.flags.gps_quality_poor = true; */ + /* ECL_WARN("GPS quality poor - stopping use"); */ + /* } */ +} + +bool Ekf::hasHorizontalAidingTimedOut() const +{ + return isTimedOut(_time_last_hor_pos_fuse, _params.reset_timeout_max) + && isTimedOut(_time_last_delpos_fuse, _params.reset_timeout_max) + && isTimedOut(_time_last_hor_vel_fuse, _params.reset_timeout_max) + && isTimedOut(_time_last_of_fuse, _params.reset_timeout_max); +} + +bool Ekf::isRecentTakeoff() const +{ + return _control_status.flags.in_air + && !isTimedOut(_time_last_on_ground_us, 30000000); } void Ekf::controlGpsYawFusion(bool gps_checks_passing, bool gps_checks_failing) diff --git a/src/modules/ekf2/EKF/ekf.h b/src/modules/ekf2/EKF/ekf.h index a7442fa097..9c479164d7 100644 --- a/src/modules/ekf2/EKF/ekf.h +++ b/src/modules/ekf2/EKF/ekf.h @@ -661,6 +661,9 @@ private: bool fuseVerticalPosition(const Vector3f &innov, const Vector2f &innov_gate, const Vector3f &obs_var, Vector3f &innov_var, Vector2f &test_ratio); + void fuseGpsVelPos(); + bool isVelStateAlignedWithObs() const; + // calculate optical flow body angular rate compensation // returns false if bias corrected body rate data is unavailable bool calcOptFlowBodyRateComp(); @@ -804,6 +807,9 @@ private: void controlGpsFusion(); void controlGpsYawFusion(bool gps_checks_passing, bool gps_checks_failing); + bool hasHorizontalAidingTimedOut() const; + bool isRecentTakeoff() const; + // control fusion of magnetometer observations void controlMagFusion(); diff --git a/src/modules/ekf2/EKF/gps_fusion.cpp b/src/modules/ekf2/EKF/gps_fusion.cpp new file mode 100644 index 0000000000..4dc986fe8e --- /dev/null +++ b/src/modules/ekf2/EKF/gps_fusion.cpp @@ -0,0 +1,91 @@ +/**************************************************************************** + * + * Copyright (c) 2021 PX4. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +/** + * @file gps_fusion.cpp + * Function for fusing gps measurements + */ + +/* #include */ +#include "ekf.h" + +void Ekf::fuseGpsVelPos() +{ + Vector2f gps_vel_innov_gates; // [horizontal vertical] + Vector2f gps_pos_innov_gates; // [horizontal vertical] + Vector3f gps_pos_obs_var; + + // correct velocity for offset relative to IMU + const Vector3f pos_offset_body = _params.gps_pos_body - _params.imu_pos_body; + const Vector3f vel_offset_body = _ang_rate_delayed_raw % pos_offset_body; + const Vector3f vel_offset_earth = _R_to_earth * vel_offset_body; + _gps_sample_delayed.vel -= vel_offset_earth; + + // correct position and height for offset relative to IMU + const Vector3f pos_offset_earth = _R_to_earth * pos_offset_body; + _gps_sample_delayed.pos -= pos_offset_earth.xy(); + _gps_sample_delayed.hgt += pos_offset_earth(2); + + const float lower_limit = fmaxf(_params.gps_pos_noise, 0.01f); + + if (isOtherSourceOfHorizontalAidingThan(_control_status.flags.gps)) { + // if we are using other sources of aiding, then relax the upper observation + // noise limit which prevents bad GPS perturbing the position estimate + gps_pos_obs_var(0) = gps_pos_obs_var(1) = sq(fmaxf(_gps_sample_delayed.hacc, lower_limit)); + + } else { + // if we are not using another source of aiding, then we are reliant on the GPS + // observations to constrain attitude errors and must limit the observation noise value. + float upper_limit = fmaxf(_params.pos_noaid_noise, lower_limit); + gps_pos_obs_var(0) = gps_pos_obs_var(1) = sq(math::constrain(_gps_sample_delayed.hacc, lower_limit, upper_limit)); + } + + _gps_sample_delayed.sacc = fmaxf(_gps_sample_delayed.sacc, _params.gps_vel_noise); + + _last_vel_obs_var.setAll(sq(_gps_sample_delayed.sacc)); + _last_vel_obs_var(2) *= sq(1.5f); + + // calculate innovations + _last_vel_obs = _gps_sample_delayed.vel; + _gps_vel_innov = _state.vel - _last_vel_obs; + _gps_pos_innov.xy() = Vector2f(_state.pos) - _gps_sample_delayed.pos; + + // set innovation gate size + gps_pos_innov_gates(0) = fmaxf(_params.gps_pos_innov_gate, 1.0f); + gps_vel_innov_gates(0) = gps_vel_innov_gates(1) = fmaxf(_params.gps_vel_innov_gate, 1.0f); + + // fuse GPS measurement + fuseHorizontalVelocity(_gps_vel_innov, gps_vel_innov_gates, _last_vel_obs_var, _gps_vel_innov_var, _gps_vel_test_ratio); + fuseVerticalVelocity(_gps_vel_innov, gps_vel_innov_gates, _last_vel_obs_var, _gps_vel_innov_var, _gps_vel_test_ratio); + fuseHorizontalPosition(_gps_pos_innov, gps_pos_innov_gates, gps_pos_obs_var, _gps_pos_innov_var, _gps_pos_test_ratio); +}