fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-10-18 09:03:34 +08:00
Code Issues Packages Projects Releases Wiki Activity

ekf2: GNSS yaw, update->fuse pattern

Browse Source
This commit is contained in:
bresch
2022-11-22 09:59:31 -05:00
committed by Daniel Agar
parent 4cb327fba3
commit b25bc1b982
7 changed files with 63 additions and 108 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/ekf2/CMakeLists.txt
-1
View File
@@ -98,7 +98,6 @@ px4_add_module(
EKF/gnss_height_control.cpp
EKF/gps_checks.cpp
EKF/gps_control.cpp
EKF/gps_fusion.cpp
EKF/gps_yaw_fusion.cpp
EKF/height_control.cpp
EKF/imu_down_sampler.cpp
src/modules/ekf2/EKF/CMakeLists.txt
-1
View File
@@ -49,7 +49,6 @@ add_library(ecl_EKF
gnss_height_control.cpp
gps_checks.cpp
gps_control.cpp
gps_fusion.cpp
gps_yaw_fusion.cpp
height_control.cpp
imu_down_sampler.cpp
src/modules/ekf2/EKF/control.cpp
+3 -1
View File
@@ -429,6 +429,8 @@ void Ekf::controlGpsYawFusion(const gpsSample &gps_sample, bool gps_checks_passi
return;
}
updateGpsYaw(gps_sample);
const bool is_new_data_available = PX4_ISFINITE(gps_sample.yaw);
if (is_new_data_available) {
@@ -447,7 +449,7 @@ void Ekf::controlGpsYawFusion(const gpsSample &gps_sample, bool gps_checks_passi
if (continuing_conditions_passing) {
fuseGpsYaw(gps_sample);
fuseGpsYaw();
const bool is_fusion_failing = isTimedOut(_aid_src_gnss_yaw.time_last_fuse, _params.reset_timeout_max);
src/modules/ekf2/EKF/ekf.h
+1 -1
View File
@@ -677,7 +677,7 @@ private:
bool fuseYaw(const float innovation, const float variance, estimator_aid_source1d_s &aid_src_status);
// fuse the yaw angle obtained from a dual antenna GPS unit
void fuseGpsYaw(const gpsSample &gps_sample);
void fuseGpsYaw();
// reset the quaternions states using the yaw angle obtained from a dual antenna GPS unit
// return true if the reset was successful
src/modules/ekf2/EKF/gps_control.cpp
-2
View File
@@ -51,8 +51,6 @@ void Ekf::controlGpsFusion()
const gpsSample &gps_sample{_gps_sample_delayed};
updateGpsYaw(gps_sample);
const bool gps_checks_passing = isTimedOut(_last_gps_fail_us, (uint64_t)5e6);
const bool gps_checks_failing = isTimedOut(_last_gps_pass_us, (uint64_t)5e6);
src/modules/ekf2/EKF/gps_fusion.cpp
-66
View File
@@ -1,66 +0,0 @@
/****************************************************************************
*
* Copyright (c) 2021-2022 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 <mathlib/mathlib.h> */
#include "ekf.h"
void Ekf::updateGpsYaw(const gpsSample &gps_sample)
{
auto &gps_yaw = _aid_src_gnss_yaw;
resetEstimatorAidStatus(gps_yaw);
if (PX4_ISFINITE(gps_sample.yaw)) {
// initially populate for estimator_aid_src_gnss_yaw logging
// calculate the observed yaw angle of antenna array, converting a from body to antenna yaw measurement
const float measured_hdg = wrap_pi(gps_sample.yaw + _gps_yaw_offset);
gps_yaw.observation = measured_hdg;
gps_yaw.observation_variance = sq(fmaxf(_params.gps_heading_noise, 1.0e-2f));
// define the predicted antenna array vector and rotate into earth frame
const Vector3f ant_vec_bf = {cosf(_gps_yaw_offset), sinf(_gps_yaw_offset), 0.0f};
const Vector3f ant_vec_ef = _R_to_earth * ant_vec_bf;
const float predicted_hdg = atan2f(ant_vec_ef(1), ant_vec_ef(0));
gps_yaw.innovation = wrap_pi(predicted_hdg - gps_yaw.observation);
gps_yaw.fusion_enabled = _control_status.flags.gps_yaw;
gps_yaw.timestamp_sample = gps_sample.time_us;
}
}
src/modules/ekf2/EKF/gps_yaw_fusion.cpp
+59 -36
View File
@@ -46,33 +46,67 @@
#include <mathlib/mathlib.h>
#include <cstdlib>
void Ekf::fuseGpsYaw(const gpsSample& gps_sample)
void Ekf::updateGpsYaw(const gpsSample &gps_sample)
{
// calculate the observed yaw angle of antenna array, converting a from body to antenna yaw measurement
const float measured_hdg = wrap_pi(gps_sample.yaw + _gps_yaw_offset);
auto &gnss_yaw = _aid_src_gnss_yaw;
resetEstimatorAidStatus(gnss_yaw);
// using magnetic heading process noise
// TODO extend interface to use yaw uncertainty provided by GPS if available
const float R_YAW = sq(fmaxf(_params.gps_heading_noise, 1.0e-2f));
if (PX4_ISFINITE(gps_sample.yaw)) {
// initially populate for estimator_aid_src_gnss_yaw logging
// calculate the observed yaw angle of antenna array, converting a from body to antenna yaw measurement
const float measured_hdg = wrap_pi(gps_sample.yaw + _gps_yaw_offset);
// using magnetic heading process noise
// TODO extend interface to use yaw uncertainty provided by GPS if available
const float R_YAW = sq(fmaxf(_params.gps_heading_noise, 1.0e-2f));
float heading_innov;
float heading_innov_var;
{
Vector24f H;
sym::ComputeGnssYawInnonInnovVarAndH(getStateAtFusionHorizonAsVector(), P, _gps_yaw_offset, measured_hdg, R_YAW, FLT_EPSILON, &heading_innov, &heading_innov_var, &H);
}
gnss_yaw.observation = measured_hdg;
gnss_yaw.observation_variance = R_YAW;
gnss_yaw.innovation = heading_innov;
gnss_yaw.innovation_variance = heading_innov_var;
gnss_yaw.fusion_enabled = _control_status.flags.gps_yaw;
gnss_yaw.timestamp_sample = gps_sample.time_us;
const float innov_gate = math::max(_params.heading_innov_gate, 1.0f);
setEstimatorAidStatusTestRatio(gnss_yaw, innov_gate);
}
}
void Ekf::fuseGpsYaw()
{
auto &gnss_yaw = _aid_src_gnss_yaw;
if (gnss_yaw.innovation_rejected) {
_innov_check_fail_status.flags.reject_yaw = true;
return;
}
float heading_innov;
float heading_innov_var;
Vector24f H;
sym::ComputeGnssYawInnonInnovVarAndH(getStateAtFusionHorizonAsVector(), P, _gps_yaw_offset, measured_hdg, R_YAW, FLT_EPSILON, &heading_innov, &heading_innov_var, &H);
{
float heading_innov;
float heading_innov_var;
// Note: we recompute innov and innov_var because it doesn't cost much more than just computing H
// making a separate function just for H uses more flash space without reducing CPU load significantly
sym::ComputeGnssYawInnonInnovVarAndH(getStateAtFusionHorizonAsVector(), P, _gps_yaw_offset, gnss_yaw.observation, gnss_yaw.observation_variance, FLT_EPSILON, &heading_innov, &heading_innov_var, &H);
}
const SparseVector24f<0,1,2,3> Hfusion(H);
_aid_src_gnss_yaw.timestamp_sample = gps_sample.time_us;
_aid_src_gnss_yaw.observation = measured_hdg;
_aid_src_gnss_yaw.observation_variance = R_YAW;
_aid_src_gnss_yaw.innovation = heading_innov;
_aid_src_gnss_yaw.fusion_enabled = _control_status.flags.gps_yaw;
_aid_src_gnss_yaw.innovation_variance = heading_innov_var;
// check if the innovation variance calculation is badly conditioned
if (heading_innov_var < R_YAW) {
if (gnss_yaw.innovation_variance < gnss_yaw.observation_variance) {
// the innovation variance contribution from the state covariances is negative which means the covariance matrix is badly conditioned
_fault_status.flags.bad_hdg = true;
@@ -83,23 +117,12 @@ void Ekf::fuseGpsYaw(const gpsSample& gps_sample)
}
_fault_status.flags.bad_hdg = false;
_innov_check_fail_status.flags.reject_yaw = false;
const float innov_gate = math::max(_params.heading_innov_gate, 1.0f);
setEstimatorAidStatusTestRatio(_aid_src_gnss_yaw, innov_gate);
if (_aid_src_gnss_yaw.innovation_rejected) {
_innov_check_fail_status.flags.reject_yaw = true;
return;
} else {
_innov_check_fail_status.flags.reject_yaw = false;
}
_gnss_yaw_signed_test_ratio_lpf.update(matrix::sign(heading_innov) * _aid_src_gnss_yaw.test_ratio);
_gnss_yaw_signed_test_ratio_lpf.update(matrix::sign(gnss_yaw.innovation) * gnss_yaw.test_ratio);
if ((fabsf(_gnss_yaw_signed_test_ratio_lpf.getState()) > 0.2f)
&& !_control_status.flags.in_air && isTimedOut(_aid_src_gnss_yaw.time_last_fuse, (uint64_t)1e6)) {
&& !_control_status.flags.in_air && isTimedOut(gnss_yaw.time_last_fuse, (uint64_t)1e6)) {
// A constant large signed test ratio is a sign of wrong gyro bias
// Reset the yaw gyro variance to converge faster and avoid
@@ -109,15 +132,15 @@ void Ekf::fuseGpsYaw(const gpsSample& gps_sample)
// calculate the Kalman gains
// only calculate gains for states we are using
Vector24f Kfusion = P * Hfusion / _aid_src_gnss_yaw.innovation_variance;
Vector24f Kfusion = P * Hfusion / gnss_yaw.innovation_variance;
const bool is_fused = measurementUpdate(Kfusion, Hfusion, heading_innov);
const bool is_fused = measurementUpdate(Kfusion, Hfusion, gnss_yaw.innovation);
_fault_status.flags.bad_hdg = !is_fused;
_aid_src_gnss_yaw.fused = is_fused;
gnss_yaw.fused = is_fused;
if (is_fused) {
_time_last_heading_fuse = _imu_sample_delayed.time_us;
_aid_src_gnss_yaw.time_last_fuse = _imu_sample_delayed.time_us;
gnss_yaw.time_last_fuse = _imu_sample_delayed.time_us;
}
}