mirror of
https://gitee.com/mirrors_PX4/PX4-Autopilot.git
synced 2026-05-01 23:44:07 +08:00
ekf2: split GNSS velocity and position fusion
This commit is contained in:
Daniel Agar
parent
451b0a3db8
commit
0eb9d05fe9
@ -174,8 +174,10 @@ endif()
|
||||
if(CONFIG_EKF2_GNSS)
|
||||
list(APPEND EKF_SRCS
|
||||
EKF/aid_sources/gnss/gnss_height_control.cpp
|
||||
EKF/aid_sources/gnss/gps_checks.cpp
|
||||
EKF/aid_sources/gnss/gps_control.cpp
|
||||
EKF/aid_sources/gnss/gnss_checks.cpp
|
||||
EKF/aid_sources/gnss/gnss_control.cpp
|
||||
EKF/aid_sources/gnss/gnss_position.cpp
|
||||
EKF/aid_sources/gnss/gnss_velocity.cpp
|
||||
)
|
||||
|
||||
if(CONFIG_EKF2_GNSS_YAW)
|
||||
|
||||
@ -91,8 +91,10 @@ endif()
|
||||
if(CONFIG_EKF2_GNSS)
|
||||
list(APPEND EKF_SRCS
|
||||
aid_sources/gnss/gnss_height_control.cpp
|
||||
aid_sources/gnss/gps_checks.cpp
|
||||
aid_sources/gnss/gps_control.cpp
|
||||
aid_sources/gnss/gnss_checks.cpp
|
||||
aid_sources/gnss/gnss_control.cpp
|
||||
aid_sources/gnss/gnss_position.cpp
|
||||
aid_sources/gnss/gnss_velocity.cpp
|
||||
)
|
||||
|
||||
if(CONFIG_EKF2_GNSS_YAW)
|
||||
|
||||
@ -167,6 +167,18 @@ bool Ekf::runGnssChecks(const gnssSample &gps)
|
||||
_last_gps_fail_us = _time_delayed_us;
|
||||
}
|
||||
|
||||
if (
|
||||
_gps_check_fail_status.flags.fix ||
|
||||
(_gps_check_fail_status.flags.nsats && (_params.gps_check_mask & MASK_GPS_NSATS)) ||
|
||||
(_gps_check_fail_status.flags.pdop && (_params.gps_check_mask & MASK_GPS_PDOP)) ||
|
||||
(_gps_check_fail_status.flags.spoofed && (_params.gps_check_mask & MASK_GPS_SPOOFED))
|
||||
) {
|
||||
_gnss_common_checks_passed = false;
|
||||
|
||||
} else {
|
||||
_gnss_common_checks_passed = true;
|
||||
}
|
||||
|
||||
// if any user selected checks have failed, record the fail time
|
||||
if (
|
||||
_gps_check_fail_status.flags.fix ||
|
||||
163
src/modules/ekf2/EKF/aid_sources/gnss/gnss_control.cpp
Normal file
163
src/modules/ekf2/EKF/aid_sources/gnss/gnss_control.cpp
Normal file
@ -0,0 +1,163 @@
|
||||
/****************************************************************************
|
||||
*
|
||||
* Copyright (c) 2021-2025 PX4 Development Team. 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.
|
||||
*
|
||||
****************************************************************************/
|
||||
|
||||
#include "ekf.h"
|
||||
#include <mathlib/mathlib.h>
|
||||
|
||||
void Ekf::controlGnssFusion(const imuSample &imu_sample)
|
||||
{
|
||||
if (!_gps_buffer || (_params.gnss_ctrl == 0)) {
|
||||
stopGnssFusion();
|
||||
return;
|
||||
}
|
||||
|
||||
if (!gyro_bias_inhibited()) {
|
||||
_yawEstimator.setGyroBias(getGyroBias(), _control_status.flags.vehicle_at_rest);
|
||||
}
|
||||
|
||||
// run EKF-GSF yaw estimator once per IMU update
|
||||
_yawEstimator.predict(imu_sample.delta_ang, imu_sample.delta_ang_dt,
|
||||
imu_sample.delta_vel, imu_sample.delta_vel_dt,
|
||||
(_control_status.flags.in_air && !_control_status.flags.vehicle_at_rest));
|
||||
|
||||
|
||||
// check for arrival of new sensor data at the fusion time horizon
|
||||
gnssSample gnss_sample;
|
||||
|
||||
if (_gps_buffer->pop_first_older_than(imu_sample.time_us, &gnss_sample)) {
|
||||
|
||||
if (runGnssChecks(gnss_sample)
|
||||
&& isTimedOut(_last_gps_fail_us, max((uint64_t)1e6, (uint64_t)_min_gps_health_time_us / 10))) {
|
||||
if (isTimedOut(_last_gps_fail_us, (uint64_t)_min_gps_health_time_us)) {
|
||||
// First time checks are passing, latching.
|
||||
if (!_gps_checks_passed) {
|
||||
_information_events.flags.gps_checks_passed = true;
|
||||
}
|
||||
|
||||
_gps_checks_passed = true;
|
||||
}
|
||||
|
||||
} else {
|
||||
// Skip this sample
|
||||
_gps_data_ready = false;
|
||||
|
||||
if ((_control_status.flags.gnss_vel || _control_status.flags.gnss_pos)
|
||||
&& isTimedOut(_last_gps_pass_us, _params.reset_timeout_max)) {
|
||||
stopGnssFusion();
|
||||
ECL_WARN("GNSS quality poor - stopping use");
|
||||
}
|
||||
}
|
||||
|
||||
controlGnssHeightFusion(gnss_sample);
|
||||
|
||||
#if defined(CONFIG_EKF2_GNSS_YAW)
|
||||
controlGnssYawFusion(gnss_sample);
|
||||
#endif // CONFIG_EKF2_GNSS_YAW
|
||||
|
||||
controlGnssYawEstimator(_aid_src_gnss_vel);
|
||||
|
||||
bool do_vel_pos_reset = false;
|
||||
|
||||
if (_control_status.flags.gnss_vel || _control_status.flags.gnss_pos) {
|
||||
// Reset checks need to run together, before each control function runs because the result would change
|
||||
// after the first one runs
|
||||
do_vel_pos_reset = shouldResetGpsFusion();
|
||||
|
||||
if (_control_status.flags.in_air
|
||||
&& isYawFailure()
|
||||
&& isTimedOut(_time_last_hor_vel_fuse, _params.EKFGSF_reset_delay)
|
||||
&& (_time_last_hor_vel_fuse > _time_last_on_ground_us)) {
|
||||
do_vel_pos_reset = tryYawEmergencyReset();
|
||||
}
|
||||
}
|
||||
|
||||
controlGnssVelocityFusion(imu_sample, gnss_sample, do_vel_pos_reset);
|
||||
controlGnssPositionFusion(imu_sample, gnss_sample, do_vel_pos_reset);
|
||||
|
||||
|
||||
} else if (_control_status.flags.gnss_vel || _control_status.flags.gnss_pos) {
|
||||
if (!isNewestSampleRecent(_time_last_gps_buffer_push, _params.reset_timeout_max)) {
|
||||
stopGnssFusion();
|
||||
ECL_WARN("GNSS data stopped");
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
bool Ekf::shouldResetGpsFusion() const
|
||||
{
|
||||
/* We are relying on aiding to constrain drift so after a specified time
|
||||
* with no aiding we need to do something
|
||||
*/
|
||||
bool has_horizontal_aiding_timed_out = isTimedOut(_time_last_hor_pos_fuse, _params.reset_timeout_max)
|
||||
&& isTimedOut(_time_last_hor_vel_fuse, _params.reset_timeout_max);
|
||||
|
||||
#if defined(CONFIG_EKF2_OPTICAL_FLOW)
|
||||
|
||||
if (has_horizontal_aiding_timed_out) {
|
||||
// horizontal aiding hasn't timed out if optical flow still active
|
||||
if (_control_status.flags.opt_flow && isRecent(_aid_src_optical_flow.time_last_fuse, _params.reset_timeout_max)) {
|
||||
has_horizontal_aiding_timed_out = false;
|
||||
}
|
||||
}
|
||||
|
||||
#endif // CONFIG_EKF2_OPTICAL_FLOW
|
||||
|
||||
const bool is_reset_required = has_horizontal_aiding_timed_out
|
||||
|| (isTimedOut(_time_last_hor_pos_fuse, 2 * _params.reset_timeout_max)
|
||||
&& (_params.gnss_ctrl & static_cast<int32_t>(GnssCtrl::HPOS)));
|
||||
|
||||
const bool is_inflight_nav_failure = _control_status.flags.in_air
|
||||
&& isTimedOut(_time_last_hor_vel_fuse, _params.reset_timeout_max)
|
||||
&& isTimedOut(_time_last_hor_pos_fuse, _params.reset_timeout_max)
|
||||
&& (_time_last_hor_vel_fuse > _time_last_on_ground_us)
|
||||
&& (_time_last_hor_pos_fuse > _time_last_on_ground_us);
|
||||
|
||||
return (is_reset_required || is_inflight_nav_failure);
|
||||
}
|
||||
|
||||
void Ekf::stopGnssFusion()
|
||||
{
|
||||
if (_control_status.flags.gnss_vel || _control_status.flags.gnss_pos) {
|
||||
_last_gps_fail_us = 0;
|
||||
_last_gps_pass_us = 0;
|
||||
}
|
||||
|
||||
stopGnssVelFusion();
|
||||
stopGnssPosFusion();
|
||||
stopGpsHgtFusion();
|
||||
#if defined(CONFIG_EKF2_GNSS_YAW)
|
||||
stopGnssYawFusion();
|
||||
#endif // CONFIG_EKF2_GNSS_YAW
|
||||
|
||||
_yawEstimator.reset();
|
||||
}
|
||||
142
src/modules/ekf2/EKF/aid_sources/gnss/gnss_position.cpp
Normal file
142
src/modules/ekf2/EKF/aid_sources/gnss/gnss_position.cpp
Normal file
@ -0,0 +1,142 @@
|
||||
/****************************************************************************
|
||||
*
|
||||
* Copyright (c) 2021-2025 PX4 Development Team. 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.
|
||||
*
|
||||
****************************************************************************/
|
||||
|
||||
#include "ekf.h"
|
||||
#include <mathlib/mathlib.h>
|
||||
|
||||
void Ekf::controlGnssPositionFusion(const imuSample &imu_sample, const gnssSample &gnss_sample, bool reset)
|
||||
{
|
||||
static constexpr const char *AID_SRC_NAME = "GNSS position";
|
||||
|
||||
auto &aid_src = _aid_src_gnss_pos;
|
||||
|
||||
// correct position and height for offset relative to IMU
|
||||
const Vector3f pos_offset_body = _params.gps_pos_body - _params.imu_pos_body;
|
||||
const Vector3f pos_offset_earth = Vector3f(_R_to_earth * pos_offset_body);
|
||||
const LatLonAlt measurement(gnss_sample.lat, gnss_sample.lon, gnss_sample.alt);
|
||||
const LatLonAlt measurement_corrected = measurement + (-pos_offset_earth);
|
||||
const Vector2f innovation = (_gpos - measurement_corrected).xy();
|
||||
|
||||
// relax the upper observation noise limit which prevents bad GPS perturbing the position estimate
|
||||
float pos_noise = math::max(gnss_sample.hacc, _params.gps_pos_noise);
|
||||
|
||||
if (!isOtherSourceOfHorizontalAidingThan(_control_status.flags.gnss_pos)) {
|
||||
// if we are not using another source of aiding, then we are reliant on the GNSS
|
||||
// observations to constrain attitude errors and must limit the observation noise value.
|
||||
if (pos_noise > _params.pos_noaid_noise) {
|
||||
pos_noise = _params.pos_noaid_noise;
|
||||
}
|
||||
}
|
||||
|
||||
const float pos_var = math::max(sq(pos_noise), sq(0.01f));
|
||||
const Vector2f pos_obs_var(pos_var, pos_var);
|
||||
const matrix::Vector2d observation(measurement_corrected.latitude_deg(), measurement_corrected.longitude_deg());
|
||||
|
||||
updateAidSourceStatus(aid_src,
|
||||
gnss_sample.time_us, // sample timestamp
|
||||
observation, // observation
|
||||
pos_obs_var, // observation variance
|
||||
innovation, // innovation
|
||||
Vector2f(getStateVariance<State::pos>()) + pos_obs_var, // innovation variance
|
||||
math::max(_params.gps_pos_innov_gate, 1.f)); // innovation gate
|
||||
|
||||
const bool gnss_pos_enabled = (_params.gnss_ctrl & static_cast<int32_t>(GnssCtrl::HPOS));
|
||||
|
||||
const bool continuing_conditions_passing = gnss_pos_enabled
|
||||
&& _control_status.flags.tilt_align
|
||||
&& _control_status.flags.yaw_align
|
||||
&& (gnss_sample.hacc < _params.req_hacc);
|
||||
|
||||
const bool starting_conditions_passing = continuing_conditions_passing
|
||||
&& _gnss_common_checks_passed;
|
||||
|
||||
const bool gpos_init_conditions_passing = gnss_pos_enabled && _gps_checks_passed;
|
||||
|
||||
if (_control_status.flags.gnss_pos) {
|
||||
if (continuing_conditions_passing) {
|
||||
if (reset) {
|
||||
ECL_WARN("%s fusion timeout, resetting", AID_SRC_NAME);
|
||||
|
||||
_information_events.flags.reset_pos_to_gps = true;
|
||||
resetLatLonTo(aid_src.observation[0], aid_src.observation[1],
|
||||
aid_src.observation_variance[0] + aid_src.observation_variance[1]);
|
||||
|
||||
resetAidSourceStatusZeroInnovation(aid_src);
|
||||
|
||||
} else {
|
||||
fuseHorizontalPosition(aid_src);
|
||||
}
|
||||
|
||||
} else {
|
||||
stopGnssPosFusion();
|
||||
}
|
||||
|
||||
} else {
|
||||
if (starting_conditions_passing) {
|
||||
ECL_INFO("%s starting fusion", AID_SRC_NAME);
|
||||
_information_events.flags.starting_gps_fusion = true;
|
||||
|
||||
_information_events.flags.reset_pos_to_gps = true;
|
||||
resetLatLonTo(aid_src.observation[0], aid_src.observation[1],
|
||||
aid_src.observation_variance[0] +
|
||||
aid_src.observation_variance[1]);
|
||||
|
||||
resetAidSourceStatusZeroInnovation(aid_src);
|
||||
|
||||
_control_status.flags.gnss_pos = true;
|
||||
|
||||
} else if (gpos_init_conditions_passing && !_local_origin_lat_lon.isInitialized()) {
|
||||
|
||||
_information_events.flags.reset_pos_to_gps = true;
|
||||
resetLatLonTo(aid_src.observation[0], aid_src.observation[1],
|
||||
aid_src.observation_variance[0] +
|
||||
aid_src.observation_variance[1]);
|
||||
|
||||
resetAidSourceStatusZeroInnovation(aid_src);
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
void Ekf::stopGnssPosFusion()
|
||||
{
|
||||
if (_control_status.flags.gnss_pos) {
|
||||
ECL_INFO("stopping GNSS position fusion");
|
||||
_control_status.flags.gnss_pos = false;
|
||||
|
||||
//TODO: what if gnss yaw or height is used?
|
||||
if (!_control_status.flags.gnss_vel) {
|
||||
_last_gps_fail_us = 0;
|
||||
_last_gps_pass_us = 0;
|
||||
}
|
||||
}
|
||||
}
|
||||
283
src/modules/ekf2/EKF/aid_sources/gnss/gnss_velocity.cpp
Normal file
283
src/modules/ekf2/EKF/aid_sources/gnss/gnss_velocity.cpp
Normal file
@ -0,0 +1,283 @@
|
||||
/****************************************************************************
|
||||
*
|
||||
* Copyright (c) 2021-2025 PX4 Development Team. 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.
|
||||
*
|
||||
****************************************************************************/
|
||||
|
||||
#include "ekf.h"
|
||||
#include <mathlib/mathlib.h>
|
||||
|
||||
void Ekf::controlGnssVelocityFusion(const imuSample &imu_sample, const gnssSample &gnss_sample, bool reset)
|
||||
{
|
||||
static constexpr const char *AID_SRC_NAME = "GNSS velocity";
|
||||
|
||||
auto &aid_src = _aid_src_gnss_vel;
|
||||
|
||||
// correct velocity for offset relative to IMU
|
||||
const Vector3f pos_offset_body = _params.gps_pos_body - _params.imu_pos_body;
|
||||
|
||||
const Vector3f angular_velocity = imu_sample.delta_ang / imu_sample.delta_ang_dt - _state.gyro_bias;
|
||||
const Vector3f vel_offset_body = angular_velocity % pos_offset_body;
|
||||
const Vector3f vel_offset_earth = _R_to_earth * vel_offset_body;
|
||||
const Vector3f velocity = gnss_sample.vel - vel_offset_earth;
|
||||
|
||||
const float vel_var = sq(math::max(gnss_sample.sacc, _params.gps_vel_noise, 0.01f));
|
||||
const Vector3f vel_obs_var(vel_var, vel_var, vel_var * sq(1.5f));
|
||||
|
||||
const float innovation_gate = math::max(_params.gps_vel_innov_gate, 1.f);
|
||||
|
||||
updateAidSourceStatus(aid_src,
|
||||
gnss_sample.time_us, // sample timestamp
|
||||
velocity, // observation
|
||||
vel_obs_var, // observation variance
|
||||
_state.vel - velocity, // innovation
|
||||
getVelocityVariance() + vel_obs_var, // innovation variance
|
||||
innovation_gate); // innovation gate
|
||||
|
||||
// vz special case if there is bad vertical acceleration data, then don't reject measurement if GNSS reports velocity accuracy is acceptable,
|
||||
// but limit innovation to prevent spikes that could destabilise the filter
|
||||
bool bad_acc_vz_rejected = _fault_status.flags.bad_acc_vertical
|
||||
&& (aid_src.test_ratio[2] > 1.f) // vz rejected
|
||||
&& (aid_src.test_ratio[0] < 1.f) && (aid_src.test_ratio[1] < 1.f); // vx & vy accepted
|
||||
|
||||
if (bad_acc_vz_rejected
|
||||
&& (gnss_sample.sacc < _params.req_sacc)
|
||||
) {
|
||||
const float innov_limit = innovation_gate * sqrtf(aid_src.innovation_variance[2]);
|
||||
aid_src.innovation[2] = math::constrain(aid_src.innovation[2], -innov_limit, innov_limit);
|
||||
aid_src.innovation_rejected = false;
|
||||
}
|
||||
|
||||
if (_control_status.flags.gnss_vel) {
|
||||
|
||||
if (_control_status.flags.in_air
|
||||
&& isYawFailure()
|
||||
&& isTimedOut(_time_last_hor_vel_fuse, _params.EKFGSF_reset_delay)
|
||||
&& (_time_last_hor_vel_fuse > _time_last_on_ground_us)) {
|
||||
|
||||
if (tryYawEmergencyReset()) {
|
||||
reset = true;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
const bool continuing_conditions_passing = (_params.gnss_ctrl & static_cast<int32_t>(GnssCtrl::VEL))
|
||||
&& _control_status.flags.tilt_align
|
||||
&& _control_status.flags.yaw_align
|
||||
&& velocity.isAllFinite()
|
||||
&& (gnss_sample.sacc < _params.req_sacc)
|
||||
&& !_gps_check_fail_status.flags.hspeed;
|
||||
|
||||
const bool starting_conditions_passing = continuing_conditions_passing
|
||||
&& _gnss_common_checks_passed
|
||||
&& velocity.longerThan(0.f);
|
||||
|
||||
if (_control_status.flags.gnss_vel) {
|
||||
if (continuing_conditions_passing) {
|
||||
if (reset) {
|
||||
ECL_WARN("%s fusion timeout, resetting", AID_SRC_NAME);
|
||||
|
||||
_information_events.flags.reset_vel_to_gps = true;
|
||||
resetVelocityTo(Vector3f(aid_src.observation), Vector3f(aid_src.observation_variance));
|
||||
|
||||
resetAidSourceStatusZeroInnovation(aid_src);
|
||||
|
||||
} else {
|
||||
fuseVelocity(aid_src);
|
||||
|
||||
if (isHeightResetRequired()) {
|
||||
// reset vertical velocity if height is failing
|
||||
resetVerticalVelocityTo(aid_src.observation[2], aid_src.observation_variance[2]);
|
||||
}
|
||||
}
|
||||
|
||||
} else {
|
||||
stopGnssVelFusion();
|
||||
}
|
||||
|
||||
} else {
|
||||
if (starting_conditions_passing) {
|
||||
ECL_INFO("%s starting fusion", AID_SRC_NAME);
|
||||
_information_events.flags.starting_gps_fusion = true;
|
||||
|
||||
// when already using another velocity source velocity reset is not necessary
|
||||
if (!isHorizontalAidingActive()
|
||||
|| isTimedOut(_time_last_hor_vel_fuse, _params.reset_timeout_max)
|
||||
|| !_control_status_prev.flags.yaw_align
|
||||
) {
|
||||
// reset velocity
|
||||
_information_events.flags.reset_vel_to_gps = true;
|
||||
resetVelocityTo(Vector3f(aid_src.observation), Vector3f(aid_src.observation_variance));
|
||||
|
||||
resetAidSourceStatusZeroInnovation(aid_src);
|
||||
|
||||
}
|
||||
|
||||
_control_status.flags.gnss_vel = true;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
void Ekf::controlGnssYawEstimator(estimator_aid_source3d_s &aid_src_vel)
|
||||
{
|
||||
// update yaw estimator velocity (basic sanity check on GNSS velocity data)
|
||||
const float vel_var = aid_src_vel.observation_variance[0];
|
||||
const Vector2f vel_xy(aid_src_vel.observation);
|
||||
|
||||
if ((vel_var > 0.f)
|
||||
&& (vel_var < _params.req_sacc)
|
||||
&& vel_xy.isAllFinite()
|
||||
) {
|
||||
_yawEstimator.fuseVelocity(vel_xy, vel_var, _control_status.flags.in_air);
|
||||
|
||||
// Try to align yaw using estimate if available
|
||||
if ((_params.gnss_ctrl & static_cast<int32_t>(GnssCtrl::VEL))
|
||||
&& !_control_status.flags.yaw_align
|
||||
&& _control_status.flags.tilt_align
|
||||
) {
|
||||
|
||||
if (resetYawToEKFGSF()) {
|
||||
ECL_INFO("GNSS yaw aligned using IMU");
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
bool Ekf::tryYawEmergencyReset()
|
||||
{
|
||||
bool success = false;
|
||||
|
||||
/* A rapid reset to the yaw emergency estimate is performed if horizontal velocity innovation checks continuously
|
||||
* fails while the difference between the yaw emergency estimator and the yaw estimate is large.
|
||||
* This enables recovery from a bad yaw estimate. A reset is not performed if the fault condition was
|
||||
* present before flight to prevent triggering due to GPS glitches or other sensor errors.
|
||||
*/
|
||||
if (resetYawToEKFGSF()) {
|
||||
ECL_WARN("GNSS emergency yaw reset");
|
||||
|
||||
if (_control_status.flags.mag_hdg || _control_status.flags.mag_3D) {
|
||||
// stop using the magnetometer in the main EKF otherwise its fusion could drag the yaw around
|
||||
// and cause another navigation failure
|
||||
_control_status.flags.mag_fault = true;
|
||||
}
|
||||
|
||||
#if defined(CONFIG_EKF2_GNSS_YAW)
|
||||
|
||||
if (_control_status.flags.gnss_yaw) {
|
||||
_control_status.flags.gnss_yaw_fault = true;
|
||||
}
|
||||
|
||||
#endif // CONFIG_EKF2_GNSS_YAW
|
||||
|
||||
#if defined(CONFIG_EKF2_EXTERNAL_VISION)
|
||||
|
||||
if (_control_status.flags.ev_yaw) {
|
||||
_control_status.flags.ev_yaw_fault = true;
|
||||
}
|
||||
|
||||
#endif // CONFIG_EKF2_EXTERNAL_VISION
|
||||
|
||||
success = true;
|
||||
}
|
||||
|
||||
return success;
|
||||
}
|
||||
|
||||
void Ekf::stopGnssVelFusion()
|
||||
{
|
||||
if (_control_status.flags.gnss_vel) {
|
||||
ECL_INFO("stopping GNSS velocity fusion");
|
||||
_control_status.flags.gnss_vel = false;
|
||||
|
||||
//TODO: what if gnss yaw or height is used?
|
||||
if (!_control_status.flags.gnss_pos) {
|
||||
_last_gps_fail_us = 0;
|
||||
_last_gps_pass_us = 0;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
bool Ekf::isYawEmergencyEstimateAvailable() const
|
||||
{
|
||||
// don't allow reet using the EKF-GSF estimate until the filter has started fusing velocity
|
||||
// data and the yaw estimate has converged
|
||||
if (!_yawEstimator.isActive()) {
|
||||
return false;
|
||||
}
|
||||
|
||||
const float yaw_var = _yawEstimator.getYawVar();
|
||||
|
||||
return (yaw_var > 0.f)
|
||||
&& (yaw_var < sq(_params.EKFGSF_yaw_err_max))
|
||||
&& PX4_ISFINITE(yaw_var);
|
||||
}
|
||||
|
||||
bool Ekf::isYawFailure() const
|
||||
{
|
||||
if (!isYawEmergencyEstimateAvailable()) {
|
||||
return false;
|
||||
}
|
||||
|
||||
const float euler_yaw = getEulerYaw(_R_to_earth);
|
||||
const float yaw_error = wrap_pi(euler_yaw - _yawEstimator.getYaw());
|
||||
|
||||
return fabsf(yaw_error) > math::radians(25.f);
|
||||
}
|
||||
|
||||
bool Ekf::resetYawToEKFGSF()
|
||||
{
|
||||
if (!isYawEmergencyEstimateAvailable()) {
|
||||
return false;
|
||||
}
|
||||
|
||||
// don't allow reset if there's just been a yaw reset
|
||||
const bool yaw_alignment_changed = (_control_status_prev.flags.yaw_align != _control_status.flags.yaw_align);
|
||||
const bool quat_reset = (_state_reset_status.reset_count.quat != _state_reset_count_prev.quat);
|
||||
|
||||
if (yaw_alignment_changed || quat_reset) {
|
||||
return false;
|
||||
}
|
||||
|
||||
ECL_INFO("yaw estimator reset heading %.3f -> %.3f rad",
|
||||
(double)getEulerYaw(_R_to_earth), (double)_yawEstimator.getYaw());
|
||||
|
||||
resetQuatStateYaw(_yawEstimator.getYaw(), _yawEstimator.getYawVar());
|
||||
|
||||
_control_status.flags.yaw_align = true;
|
||||
_information_events.flags.yaw_aligned_to_imu_gps = true;
|
||||
|
||||
return true;
|
||||
}
|
||||
|
||||
bool Ekf::getDataEKFGSF(float *yaw_composite, float *yaw_variance, float yaw[N_MODELS_EKFGSF],
|
||||
float innov_VN[N_MODELS_EKFGSF], float innov_VE[N_MODELS_EKFGSF], float weight[N_MODELS_EKFGSF])
|
||||
{
|
||||
return _yawEstimator.getLogData(yaw_composite, yaw_variance, yaw, innov_VN, innov_VE, weight);
|
||||
}
|
||||
@ -67,8 +67,7 @@ void Ekf::controlGnssYawFusion(const gnssSample &gnss_sample)
|
||||
|
||||
const bool starting_conditions_passing = continuing_conditions_passing
|
||||
&& _gps_checks_passed
|
||||
&& !is_gnss_yaw_data_intermittent
|
||||
&& !_gps_intermittent;
|
||||
&& !is_gnss_yaw_data_intermittent;
|
||||
|
||||
if (_control_status.flags.gnss_yaw) {
|
||||
if (continuing_conditions_passing) {
|
||||
|
||||
@ -1,495 +0,0 @@
|
||||
/****************************************************************************
|
||||
*
|
||||
* Copyright (c) 2021-2022 PX4 Development Team. 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_control.cpp
|
||||
* Control functions for ekf GNSS fusion
|
||||
*/
|
||||
|
||||
#include "ekf.h"
|
||||
#include <mathlib/mathlib.h>
|
||||
|
||||
void Ekf::controlGpsFusion(const imuSample &imu_delayed)
|
||||
{
|
||||
if (!_gps_buffer || (_params.gnss_ctrl == 0)) {
|
||||
stopGnssFusion();
|
||||
return;
|
||||
}
|
||||
|
||||
if (!gyro_bias_inhibited()) {
|
||||
_yawEstimator.setGyroBias(getGyroBias(), _control_status.flags.vehicle_at_rest);
|
||||
}
|
||||
|
||||
// run EKF-GSF yaw estimator once per imu_delayed update
|
||||
_yawEstimator.predict(imu_delayed.delta_ang, imu_delayed.delta_ang_dt,
|
||||
imu_delayed.delta_vel, imu_delayed.delta_vel_dt,
|
||||
(_control_status.flags.in_air && !_control_status.flags.vehicle_at_rest));
|
||||
|
||||
_gps_intermittent = !isNewestSampleRecent(_time_last_gps_buffer_push, 2 * GNSS_MAX_INTERVAL);
|
||||
|
||||
// check for arrival of new sensor data at the fusion time horizon
|
||||
_gps_data_ready = _gps_buffer->pop_first_older_than(imu_delayed.time_us, &_gps_sample_delayed);
|
||||
|
||||
if (_gps_data_ready) {
|
||||
const gnssSample &gnss_sample = _gps_sample_delayed;
|
||||
|
||||
if (runGnssChecks(gnss_sample)
|
||||
&& isTimedOut(_last_gps_fail_us, max((uint64_t)1e6, (uint64_t)_min_gps_health_time_us / 10))) {
|
||||
if (isTimedOut(_last_gps_fail_us, (uint64_t)_min_gps_health_time_us)) {
|
||||
// First time checks are passing, latching.
|
||||
if (!_gps_checks_passed) {
|
||||
_information_events.flags.gps_checks_passed = true;
|
||||
}
|
||||
|
||||
_gps_checks_passed = true;
|
||||
}
|
||||
|
||||
} else {
|
||||
// Skip this sample
|
||||
_gps_data_ready = false;
|
||||
|
||||
if ((_control_status.flags.gnss_vel || _control_status.flags.gnss_pos)
|
||||
&& isTimedOut(_last_gps_pass_us, _params.reset_timeout_max)) {
|
||||
stopGnssFusion();
|
||||
ECL_WARN("GNSS quality poor - stopping use");
|
||||
}
|
||||
}
|
||||
|
||||
updateGnssPos(gnss_sample, _aid_src_gnss_pos);
|
||||
updateGnssVel(imu_delayed, gnss_sample, _aid_src_gnss_vel);
|
||||
|
||||
} else if (_control_status.flags.gnss_vel || _control_status.flags.gnss_pos) {
|
||||
if (!isNewestSampleRecent(_time_last_gps_buffer_push, _params.reset_timeout_max)) {
|
||||
stopGnssFusion();
|
||||
ECL_WARN("GNSS data stopped");
|
||||
}
|
||||
}
|
||||
|
||||
if (_gps_data_ready) {
|
||||
#if defined(CONFIG_EKF2_GNSS_YAW)
|
||||
const gnssSample &gnss_sample = _gps_sample_delayed;
|
||||
controlGnssYawFusion(gnss_sample);
|
||||
#endif // CONFIG_EKF2_GNSS_YAW
|
||||
|
||||
controlGnssYawEstimator(_aid_src_gnss_vel);
|
||||
|
||||
bool do_vel_pos_reset = false;
|
||||
|
||||
if (_control_status.flags.gnss_vel || _control_status.flags.gnss_pos) {
|
||||
// Reset checks need to run together, before each control function runs because the result would change
|
||||
// after the first one runs
|
||||
do_vel_pos_reset = shouldResetGpsFusion();
|
||||
|
||||
if (_control_status.flags.in_air
|
||||
&& isYawFailure()
|
||||
&& isTimedOut(_time_last_hor_vel_fuse, _params.EKFGSF_reset_delay)
|
||||
&& (_time_last_hor_vel_fuse > _time_last_on_ground_us)) {
|
||||
do_vel_pos_reset = tryYawEmergencyReset();
|
||||
}
|
||||
}
|
||||
|
||||
controlGnssVelFusion(_aid_src_gnss_vel, do_vel_pos_reset);
|
||||
controlGnssPosFusion(_aid_src_gnss_pos, do_vel_pos_reset);
|
||||
}
|
||||
}
|
||||
|
||||
void Ekf::controlGnssVelFusion(estimator_aid_source3d_s &aid_src, const bool force_reset)
|
||||
{
|
||||
const bool continuing_conditions_passing = (_params.gnss_ctrl & static_cast<int32_t>(GnssCtrl::VEL))
|
||||
&& _control_status.flags.tilt_align
|
||||
&& _control_status.flags.yaw_align;
|
||||
const bool starting_conditions_passing = continuing_conditions_passing && _gps_checks_passed;
|
||||
|
||||
if (_control_status.flags.gnss_vel) {
|
||||
if (continuing_conditions_passing) {
|
||||
if (force_reset) {
|
||||
ECL_WARN("GNSS fusion timeout, resetting");
|
||||
resetVelocityToGnss(aid_src);
|
||||
|
||||
} else {
|
||||
fuseVelocity(aid_src);
|
||||
|
||||
if (isHeightResetRequired()) {
|
||||
// reset vertical velocity if height is failing
|
||||
resetVerticalVelocityTo(aid_src.observation[2], aid_src.observation_variance[2]);
|
||||
}
|
||||
}
|
||||
|
||||
} else {
|
||||
stopGnssVelFusion();
|
||||
}
|
||||
|
||||
} else {
|
||||
if (starting_conditions_passing) {
|
||||
ECL_INFO("starting GNSS velocity fusion");
|
||||
_information_events.flags.starting_gps_fusion = true;
|
||||
|
||||
// when already using another velocity source velocity reset is not necessary
|
||||
if (!isHorizontalAidingActive()
|
||||
|| isTimedOut(_time_last_hor_vel_fuse, _params.reset_timeout_max)
|
||||
|| !_control_status_prev.flags.yaw_align
|
||||
) {
|
||||
// reset velocity
|
||||
resetVelocityToGnss(aid_src);
|
||||
}
|
||||
|
||||
_control_status.flags.gnss_vel = true;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
void Ekf::controlGnssPosFusion(estimator_aid_source2d_s &aid_src, const bool force_reset)
|
||||
{
|
||||
const bool gnss_pos_enabled = (_params.gnss_ctrl & static_cast<int32_t>(GnssCtrl::HPOS));
|
||||
|
||||
const bool continuing_conditions_passing = gnss_pos_enabled
|
||||
&& _control_status.flags.tilt_align
|
||||
&& _control_status.flags.yaw_align;
|
||||
const bool starting_conditions_passing = continuing_conditions_passing && _gps_checks_passed;
|
||||
const bool gpos_init_conditions_passing = gnss_pos_enabled && _gps_checks_passed;
|
||||
|
||||
if (_control_status.flags.gnss_pos) {
|
||||
if (continuing_conditions_passing) {
|
||||
if (force_reset) {
|
||||
ECL_WARN("GNSS fusion timeout, resetting");
|
||||
resetHorizontalPositionToGnss(aid_src);
|
||||
|
||||
} else {
|
||||
fuseHorizontalPosition(aid_src);
|
||||
}
|
||||
|
||||
} else {
|
||||
stopGnssPosFusion();
|
||||
}
|
||||
|
||||
} else {
|
||||
if (starting_conditions_passing) {
|
||||
ECL_INFO("starting GNSS position fusion");
|
||||
_information_events.flags.starting_gps_fusion = true;
|
||||
|
||||
resetHorizontalPositionToGnss(aid_src);
|
||||
_control_status.flags.gnss_pos = true;
|
||||
|
||||
} else if (gpos_init_conditions_passing && !_local_origin_lat_lon.isInitialized()) {
|
||||
resetHorizontalPositionToGnss(aid_src);
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
void Ekf::updateGnssVel(const imuSample &imu_sample, const gnssSample &gnss_sample, estimator_aid_source3d_s &aid_src)
|
||||
{
|
||||
// correct velocity for offset relative to IMU
|
||||
const Vector3f pos_offset_body = _params.gps_pos_body - _params.imu_pos_body;
|
||||
|
||||
const Vector3f angular_velocity = imu_sample.delta_ang / imu_sample.delta_ang_dt - _state.gyro_bias;
|
||||
const Vector3f vel_offset_body = angular_velocity % pos_offset_body;
|
||||
const Vector3f vel_offset_earth = _R_to_earth * vel_offset_body;
|
||||
const Vector3f velocity = gnss_sample.vel - vel_offset_earth;
|
||||
|
||||
const float vel_var = sq(math::max(gnss_sample.sacc, _params.gps_vel_noise, 0.01f));
|
||||
const Vector3f vel_obs_var(vel_var, vel_var, vel_var * sq(1.5f));
|
||||
|
||||
const float innovation_gate = math::max(_params.gps_vel_innov_gate, 1.f);
|
||||
|
||||
updateAidSourceStatus(aid_src,
|
||||
gnss_sample.time_us, // sample timestamp
|
||||
velocity, // observation
|
||||
vel_obs_var, // observation variance
|
||||
_state.vel - velocity, // innovation
|
||||
getVelocityVariance() + vel_obs_var, // innovation variance
|
||||
innovation_gate); // innovation gate
|
||||
|
||||
// vz special case if there is bad vertical acceleration data, then don't reject measurement if GNSS reports velocity accuracy is acceptable,
|
||||
// but limit innovation to prevent spikes that could destabilise the filter
|
||||
bool bad_acc_vz_rejected = _fault_status.flags.bad_acc_vertical
|
||||
&& (aid_src.test_ratio[2] > 1.f) // vz rejected
|
||||
&& (aid_src.test_ratio[0] < 1.f) && (aid_src.test_ratio[1] < 1.f); // vx & vy accepted
|
||||
|
||||
if (bad_acc_vz_rejected
|
||||
&& (gnss_sample.sacc < _params.req_sacc)
|
||||
) {
|
||||
const float innov_limit = innovation_gate * sqrtf(aid_src.innovation_variance[2]);
|
||||
aid_src.innovation[2] = math::constrain(aid_src.innovation[2], -innov_limit, innov_limit);
|
||||
aid_src.innovation_rejected = false;
|
||||
}
|
||||
}
|
||||
|
||||
void Ekf::updateGnssPos(const gnssSample &gnss_sample, estimator_aid_source2d_s &aid_src)
|
||||
{
|
||||
// correct position and height for offset relative to IMU
|
||||
const Vector3f pos_offset_body = _params.gps_pos_body - _params.imu_pos_body;
|
||||
const Vector3f pos_offset_earth = Vector3f(_R_to_earth * pos_offset_body);
|
||||
const LatLonAlt measurement(gnss_sample.lat, gnss_sample.lon, gnss_sample.alt);
|
||||
const LatLonAlt measurement_corrected = measurement + (-pos_offset_earth);
|
||||
const Vector2f innovation = (_gpos - measurement_corrected).xy();
|
||||
|
||||
// relax the upper observation noise limit which prevents bad GPS perturbing the position estimate
|
||||
float pos_noise = math::max(gnss_sample.hacc, _params.gps_pos_noise);
|
||||
|
||||
if (!isOtherSourceOfHorizontalAidingThan(_control_status.flags.gnss_pos)) {
|
||||
// if we are not using another source of aiding, then we are reliant on the GNSS
|
||||
// observations to constrain attitude errors and must limit the observation noise value.
|
||||
if (pos_noise > _params.pos_noaid_noise) {
|
||||
pos_noise = _params.pos_noaid_noise;
|
||||
}
|
||||
}
|
||||
|
||||
const float pos_var = math::max(sq(pos_noise), sq(0.01f));
|
||||
const Vector2f pos_obs_var(pos_var, pos_var);
|
||||
const matrix::Vector2d observation(measurement_corrected.latitude_deg(), measurement_corrected.longitude_deg());
|
||||
|
||||
updateAidSourceStatus(aid_src,
|
||||
gnss_sample.time_us, // sample timestamp
|
||||
observation, // observation
|
||||
pos_obs_var, // observation variance
|
||||
innovation, // innovation
|
||||
Vector2f(getStateVariance<State::pos>()) + pos_obs_var, // innovation variance
|
||||
math::max(_params.gps_pos_innov_gate, 1.f)); // innovation gate
|
||||
}
|
||||
|
||||
void Ekf::controlGnssYawEstimator(estimator_aid_source3d_s &aid_src_vel)
|
||||
{
|
||||
// update yaw estimator velocity (basic sanity check on GNSS velocity data)
|
||||
const float vel_var = aid_src_vel.observation_variance[0];
|
||||
const Vector2f vel_xy(aid_src_vel.observation);
|
||||
|
||||
if ((vel_var > 0.f)
|
||||
&& (vel_var < _params.req_sacc)
|
||||
&& vel_xy.isAllFinite()) {
|
||||
|
||||
_yawEstimator.fuseVelocity(vel_xy, vel_var, _control_status.flags.in_air);
|
||||
|
||||
// Try to align yaw using estimate if available
|
||||
if (((_params.gnss_ctrl & static_cast<int32_t>(GnssCtrl::VEL))
|
||||
|| (_params.gnss_ctrl & static_cast<int32_t>(GnssCtrl::HPOS)))
|
||||
&& !_control_status.flags.yaw_align
|
||||
&& _control_status.flags.tilt_align) {
|
||||
if (resetYawToEKFGSF()) {
|
||||
ECL_INFO("GPS yaw aligned using IMU");
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
bool Ekf::tryYawEmergencyReset()
|
||||
{
|
||||
bool success = false;
|
||||
|
||||
/* A rapid reset to the yaw emergency estimate is performed if horizontal velocity innovation checks continuously
|
||||
* fails while the difference between the yaw emergency estimator and the yaw estimate is large.
|
||||
* This enables recovery from a bad yaw estimate. A reset is not performed if the fault condition was
|
||||
* present before flight to prevent triggering due to GPS glitches or other sensor errors.
|
||||
*/
|
||||
if (resetYawToEKFGSF()) {
|
||||
ECL_WARN("GPS emergency yaw reset");
|
||||
|
||||
if (_control_status.flags.mag_hdg || _control_status.flags.mag_3D) {
|
||||
// stop using the magnetometer in the main EKF otherwise its fusion could drag the yaw around
|
||||
// and cause another navigation failure
|
||||
_control_status.flags.mag_fault = true;
|
||||
}
|
||||
|
||||
#if defined(CONFIG_EKF2_GNSS_YAW)
|
||||
|
||||
if (_control_status.flags.gnss_yaw) {
|
||||
_control_status.flags.gnss_yaw_fault = true;
|
||||
}
|
||||
|
||||
#endif // CONFIG_EKF2_GNSS_YAW
|
||||
|
||||
#if defined(CONFIG_EKF2_EXTERNAL_VISION)
|
||||
|
||||
if (_control_status.flags.ev_yaw) {
|
||||
_control_status.flags.ev_yaw_fault = true;
|
||||
}
|
||||
|
||||
#endif // CONFIG_EKF2_EXTERNAL_VISION
|
||||
|
||||
success = true;
|
||||
}
|
||||
|
||||
return success;
|
||||
}
|
||||
|
||||
void Ekf::resetVelocityToGnss(estimator_aid_source3d_s &aid_src)
|
||||
{
|
||||
_information_events.flags.reset_vel_to_gps = true;
|
||||
resetVelocityTo(Vector3f(aid_src.observation), Vector3f(aid_src.observation_variance));
|
||||
|
||||
resetAidSourceStatusZeroInnovation(aid_src);
|
||||
}
|
||||
|
||||
void Ekf::resetHorizontalPositionToGnss(estimator_aid_source2d_s &aid_src)
|
||||
{
|
||||
_information_events.flags.reset_pos_to_gps = true;
|
||||
resetLatLonTo(aid_src.observation[0], aid_src.observation[1],
|
||||
aid_src.observation_variance[0] +
|
||||
aid_src.observation_variance[1]);
|
||||
|
||||
resetAidSourceStatusZeroInnovation(aid_src);
|
||||
}
|
||||
|
||||
bool Ekf::shouldResetGpsFusion() const
|
||||
{
|
||||
/* We are relying on aiding to constrain drift so after a specified time
|
||||
* with no aiding we need to do something
|
||||
*/
|
||||
bool has_horizontal_aiding_timed_out = isTimedOut(_time_last_hor_pos_fuse, _params.reset_timeout_max)
|
||||
&& isTimedOut(_time_last_hor_vel_fuse, _params.reset_timeout_max);
|
||||
|
||||
#if defined(CONFIG_EKF2_OPTICAL_FLOW)
|
||||
|
||||
if (has_horizontal_aiding_timed_out) {
|
||||
// horizontal aiding hasn't timed out if optical flow still active
|
||||
if (_control_status.flags.opt_flow && isRecent(_aid_src_optical_flow.time_last_fuse, _params.reset_timeout_max)) {
|
||||
has_horizontal_aiding_timed_out = false;
|
||||
}
|
||||
}
|
||||
|
||||
#endif // CONFIG_EKF2_OPTICAL_FLOW
|
||||
|
||||
const bool is_reset_required = has_horizontal_aiding_timed_out
|
||||
|| (isTimedOut(_time_last_hor_pos_fuse, 2 * _params.reset_timeout_max)
|
||||
&& (_params.gnss_ctrl & static_cast<int32_t>(GnssCtrl::HPOS)));
|
||||
|
||||
const bool is_inflight_nav_failure = _control_status.flags.in_air
|
||||
&& isTimedOut(_time_last_hor_vel_fuse, _params.reset_timeout_max)
|
||||
&& isTimedOut(_time_last_hor_pos_fuse, _params.reset_timeout_max)
|
||||
&& (_time_last_hor_vel_fuse > _time_last_on_ground_us)
|
||||
&& (_time_last_hor_pos_fuse > _time_last_on_ground_us);
|
||||
|
||||
return (is_reset_required || is_inflight_nav_failure);
|
||||
}
|
||||
|
||||
void Ekf::stopGnssFusion()
|
||||
{
|
||||
if (_control_status.flags.gnss_vel || _control_status.flags.gnss_pos) {
|
||||
_last_gps_fail_us = 0;
|
||||
_last_gps_pass_us = 0;
|
||||
}
|
||||
|
||||
stopGnssVelFusion();
|
||||
stopGnssPosFusion();
|
||||
stopGpsHgtFusion();
|
||||
#if defined(CONFIG_EKF2_GNSS_YAW)
|
||||
stopGnssYawFusion();
|
||||
#endif // CONFIG_EKF2_GNSS_YAW
|
||||
|
||||
_yawEstimator.reset();
|
||||
}
|
||||
|
||||
void Ekf::stopGnssVelFusion()
|
||||
{
|
||||
if (_control_status.flags.gnss_vel) {
|
||||
ECL_INFO("stopping GNSS velocity fusion");
|
||||
_control_status.flags.gnss_vel = false;
|
||||
|
||||
//TODO: what if gnss yaw or height is used?
|
||||
if (!_control_status.flags.gnss_pos) {
|
||||
_last_gps_fail_us = 0;
|
||||
_last_gps_pass_us = 0;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
void Ekf::stopGnssPosFusion()
|
||||
{
|
||||
if (_control_status.flags.gnss_pos) {
|
||||
ECL_INFO("stopping GNSS position fusion");
|
||||
_control_status.flags.gnss_pos = false;
|
||||
|
||||
//TODO: what if gnss yaw or height is used?
|
||||
if (!_control_status.flags.gnss_vel) {
|
||||
_last_gps_fail_us = 0;
|
||||
_last_gps_pass_us = 0;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
bool Ekf::isYawEmergencyEstimateAvailable() const
|
||||
{
|
||||
// don't allow reet using the EKF-GSF estimate until the filter has started fusing velocity
|
||||
// data and the yaw estimate has converged
|
||||
if (!_yawEstimator.isActive()) {
|
||||
return false;
|
||||
}
|
||||
|
||||
const float yaw_var = _yawEstimator.getYawVar();
|
||||
|
||||
return (yaw_var > 0.f)
|
||||
&& (yaw_var < sq(_params.EKFGSF_yaw_err_max))
|
||||
&& PX4_ISFINITE(yaw_var);
|
||||
}
|
||||
|
||||
bool Ekf::isYawFailure() const
|
||||
{
|
||||
if (!isYawEmergencyEstimateAvailable()) {
|
||||
return false;
|
||||
}
|
||||
|
||||
const float euler_yaw = getEulerYaw(_R_to_earth);
|
||||
const float yaw_error = wrap_pi(euler_yaw - _yawEstimator.getYaw());
|
||||
|
||||
return fabsf(yaw_error) > math::radians(25.f);
|
||||
}
|
||||
|
||||
bool Ekf::resetYawToEKFGSF()
|
||||
{
|
||||
if (!isYawEmergencyEstimateAvailable()) {
|
||||
return false;
|
||||
}
|
||||
|
||||
// don't allow reset if there's just been a yaw reset
|
||||
const bool yaw_alignment_changed = (_control_status_prev.flags.yaw_align != _control_status.flags.yaw_align);
|
||||
const bool quat_reset = (_state_reset_status.reset_count.quat != _state_reset_count_prev.quat);
|
||||
|
||||
if (yaw_alignment_changed || quat_reset) {
|
||||
return false;
|
||||
}
|
||||
|
||||
ECL_INFO("yaw estimator reset heading %.3f -> %.3f rad",
|
||||
(double)getEulerYaw(_R_to_earth), (double)_yawEstimator.getYaw());
|
||||
|
||||
resetQuatStateYaw(_yawEstimator.getYaw(), _yawEstimator.getYawVar());
|
||||
|
||||
_control_status.flags.yaw_align = true;
|
||||
_information_events.flags.yaw_aligned_to_imu_gps = true;
|
||||
|
||||
return true;
|
||||
}
|
||||
|
||||
bool Ekf::getDataEKFGSF(float *yaw_composite, float *yaw_variance, float yaw[N_MODELS_EKFGSF],
|
||||
float innov_VN[N_MODELS_EKFGSF], float innov_VE[N_MODELS_EKFGSF], float weight[N_MODELS_EKFGSF])
|
||||
{
|
||||
return _yawEstimator.getLogData(yaw_composite, yaw_variance, yaw, innov_VN, innov_VE, weight);
|
||||
}
|
||||
@ -112,7 +112,7 @@ void Ekf::controlFusionModes(const imuSample &imu_delayed)
|
||||
#endif // CONFIG_EKF2_OPTICAL_FLOW
|
||||
|
||||
#if defined(CONFIG_EKF2_GNSS)
|
||||
controlGpsFusion(imu_delayed);
|
||||
controlGnssFusion(imu_delayed);
|
||||
#endif // CONFIG_EKF2_GNSS
|
||||
|
||||
#if defined(CONFIG_EKF2_AUX_GLOBAL_POSITION) && defined(MODULE_NAME)
|
||||
|
||||
@ -562,11 +562,11 @@ private:
|
||||
uint64_t _last_gps_fail_us{0}; ///< last system time in usec that the GPS failed it's checks
|
||||
uint64_t _last_gps_pass_us{0}; ///< last system time in usec that the GPS passed it's checks
|
||||
uint32_t _min_gps_health_time_us{10000000}; ///< GPS is marked as healthy only after this amount of time
|
||||
|
||||
bool _gps_checks_passed{false}; ///> true when all active GPS checks have passed
|
||||
bool _gnss_common_checks_passed{false};
|
||||
|
||||
gps_check_fail_status_u _gps_check_fail_status{};
|
||||
// height sensor status
|
||||
bool _gps_intermittent{true}; ///< true if data into the buffer is intermittent
|
||||
|
||||
HeightBiasEstimator _gps_hgt_b_est{HeightSensor::GNSS, _height_sensor_ref};
|
||||
|
||||
@ -861,19 +861,8 @@ private:
|
||||
|
||||
#if defined(CONFIG_EKF2_GNSS)
|
||||
// control fusion of GPS observations
|
||||
void controlGpsFusion(const imuSample &imu_delayed);
|
||||
void controlGnssVelFusion(estimator_aid_source3d_s &aid_src, bool force_reset);
|
||||
void controlGnssPosFusion(estimator_aid_source2d_s &aid_src, const bool force_reset);
|
||||
void controlGnssFusion(const imuSample &imu_sample);
|
||||
void stopGnssFusion();
|
||||
void stopGnssVelFusion();
|
||||
void stopGnssPosFusion();
|
||||
void updateGnssVel(const imuSample &imu_sample, const gnssSample &gnss_sample, estimator_aid_source3d_s &aid_src);
|
||||
void updateGnssPos(const gnssSample &gnss_sample, estimator_aid_source2d_s &aid_src);
|
||||
void controlGnssYawEstimator(estimator_aid_source3d_s &aid_src_vel);
|
||||
bool tryYawEmergencyReset();
|
||||
void resetVelocityToGnss(estimator_aid_source3d_s &aid_src);
|
||||
void resetHorizontalPositionToGnss(estimator_aid_source2d_s &aid_src);
|
||||
bool shouldResetGpsFusion() const;
|
||||
|
||||
/*
|
||||
* Return true if the GPS solution quality is adequate.
|
||||
@ -882,6 +871,18 @@ private:
|
||||
*/
|
||||
bool runGnssChecks(const gnssSample &gps);
|
||||
|
||||
|
||||
void controlGnssVelocityFusion(const imuSample &imu_sample, const gnssSample &gnss_sample, const bool force_reset);
|
||||
void stopGnssVelFusion();
|
||||
void controlGnssYawEstimator(estimator_aid_source3d_s &aid_src_vel);
|
||||
bool tryYawEmergencyReset();
|
||||
|
||||
void controlGnssPositionFusion(const imuSample &imu_sample, const gnssSample &gnss_sample, const bool force_reset);
|
||||
void stopGnssPosFusion();
|
||||
|
||||
bool shouldResetGpsFusion() const;
|
||||
|
||||
|
||||
void controlGnssHeightFusion(const gnssSample &gps_sample);
|
||||
void stopGpsHgtFusion();
|
||||
|
||||
|
||||
@ -87,8 +87,6 @@ public:
|
||||
#if defined(CONFIG_EKF2_GNSS)
|
||||
void setGpsData(const gnssSample &gnss_sample);
|
||||
|
||||
const gnssSample &get_gps_sample_delayed() const { return _gps_sample_delayed; }
|
||||
|
||||
float gps_horizontal_position_drift_rate_m_s() const { return _gps_horizontal_position_drift_rate_m_s; }
|
||||
float gps_vertical_position_drift_rate_m_s() const { return _gps_vertical_position_drift_rate_m_s; }
|
||||
float gps_filtered_horizontal_velocity_m_s() const { return _gps_filtered_horizontal_velocity_m_s; }
|
||||
@ -395,8 +393,6 @@ protected:
|
||||
RingBuffer<gnssSample> *_gps_buffer {nullptr};
|
||||
uint64_t _time_last_gps_buffer_push{0};
|
||||
|
||||
gnssSample _gps_sample_delayed{};
|
||||
|
||||
float _gps_horizontal_position_drift_rate_m_s{NAN}; // Horizontal position drift rate (m/s)
|
||||
float _gps_vertical_position_drift_rate_m_s{NAN}; // Vertical position drift rate (m/s)
|
||||
float _gps_filtered_horizontal_velocity_m_s{NAN}; // Filtered horizontal velocity (m/s)
|
||||
|
||||
@ -47,10 +47,6 @@ void Ekf::controlHeightFusion(const imuSample &imu_delayed)
|
||||
controlBaroHeightFusion(imu_delayed);
|
||||
#endif // CONFIG_EKF2_BAROMETER
|
||||
|
||||
#if defined(CONFIG_EKF2_GNSS)
|
||||
controlGnssHeightFusion(_gps_sample_delayed);
|
||||
#endif // CONFIG_EKF2_GNSS
|
||||
|
||||
#if defined(CONFIG_EKF2_RANGE_FINDER)
|
||||
controlRangeHaglFusion(imu_delayed);
|
||||
#endif // CONFIG_EKF2_RANGE_FINDER
|
||||
|
||||
@ -1053,11 +1053,11 @@ void EKF2::PublishBaroBias(const hrt_abstime ×tamp)
|
||||
#if defined(CONFIG_EKF2_GNSS)
|
||||
void EKF2::PublishGnssHgtBias(const hrt_abstime ×tamp)
|
||||
{
|
||||
if (_ekf.get_gps_sample_delayed().time_us != 0) {
|
||||
if ( _ekf.aid_src_gnss_hgt().timestamp_sample != 0) {
|
||||
const BiasEstimator::status &status = _ekf.getGpsHgtBiasEstimatorStatus();
|
||||
|
||||
if (fabsf(status.bias - _last_gnss_hgt_bias_published) > 0.001f) {
|
||||
_estimator_gnss_hgt_bias_pub.publish(fillEstimatorBiasMsg(status, _ekf.get_gps_sample_delayed().time_us, timestamp));
|
||||
_estimator_gnss_hgt_bias_pub.publish(fillEstimatorBiasMsg(status, _ekf.aid_src_gnss_hgt().timestamp_sample, timestamp));
|
||||
|
||||
_last_gnss_hgt_bias_published = status.bias;
|
||||
}
|
||||
@ -1221,7 +1221,7 @@ void EKF2::PublishGlobalPosition(const hrt_abstime ×tamp)
|
||||
#if defined(CONFIG_EKF2_GNSS)
|
||||
void EKF2::PublishGpsStatus(const hrt_abstime ×tamp)
|
||||
{
|
||||
const hrt_abstime timestamp_sample = _ekf.get_gps_sample_delayed().time_us;
|
||||
const hrt_abstime timestamp_sample = _ekf.aid_src_gnss_pos().timestamp_sample;
|
||||
|
||||
if (timestamp_sample == _last_gps_status_published) {
|
||||
return;
|
||||
|
||||
Loading…
x
Reference in New Issue
Block a user