mirror of
https://gitee.com/mirrors_PX4/PX4-Autopilot.git
synced 2026-05-01 16:54:07 +08:00
[ekf] controlMagFusion refactor and mag field strength check (#662)
* ekf_control: Inhibit mag fusion when field magnitude is large Move mag inhibition check in separate function * ekf_control: pull out of functionalities out of controlMagFusion - yaw abd mag bias observability checks - mag 3D conditions - load mag covariances - set and clear mag control modes * ekf_control: refactor mag heading/3D start/stop. Move mag declination, mag 3d and mag heading fusion out of the main function * ekf_control: extract mag yaw reset and mag declination fusion requirements * ekf_control: use WMM in isStronMagneticField for mag fusion inhibition - Correct units of WMM strength table * ekf_control: extract mag_state_only functionality of AUTOFW (VTOL custom) Also split inAirYawReset from onGroundYawReset * ekf_control: extract mag automatic selection - transform if-else into switch-case for parameter fusion type selection * ekf_control: extract run3DMagAndDeclFusion, reorganize functions, fix flag naming in Test script * ekf_control: do not run mag fusion if tilt is not aligned. Reset some variables on ground even if mag fusion is not running yet. It could be that it runs later so we need to make sure that those variables are properly set. * ekf_control: move controlMagFusion and related functions to mag_control.cpp * ekf control: check for validity of mag strength from WMM and falls back to average earth mag field with larger gate if not valid * ekf control: remove evyaw check for mag inhibition * ekf control: change nested ternary operator into if-else if * Ekf: create AlphaFilter template class for simple low-pass filtering 0.1/0.9 type low-pass filters are commonly used to smooth data, this class is meant to abstract the computation of this filter * ekf control: reset heading using mag_lpf data to avoid resetting on an outlier fixes ecl issue #525 * ekf control: replace mag_states_only flag with mag_field_disturbed and add parameter to enable or disable mag field strength check * ekf control: remove AUTOFW mag fusion type as not needed This was implemented for VTOL but did not solve the problem and should not be used anymore * ekf control: use start/stop mag functions everywhere instead of setting the flag * ekf control: Run mag fusion depending on yaw_align instead of tilt_align as there is no reason to fuse mag when the ekf isn't aligned * AlphaFilter: add test for float and Vector3f
This commit is contained in:
Mathieu Bresciani
GitHub
parent
a6840655e8
commit
c7bdf25663
71
EKF/AlphaFilter.hpp
Normal file
71
EKF/AlphaFilter.hpp
Normal file
@ -0,0 +1,71 @@
|
||||
/****************************************************************************
|
||||
*
|
||||
* Copyright (c) 2019 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.
|
||||
*
|
||||
****************************************************************************/
|
||||
|
||||
/**
|
||||
* First order "alpha" IIR digital filter
|
||||
* @author Mathieu Bresciani <brescianimathieu@gmail.com>
|
||||
*/
|
||||
|
||||
#pragma once
|
||||
|
||||
template<typename T>
|
||||
class AlphaFilter final
|
||||
{
|
||||
public:
|
||||
AlphaFilter() = default;
|
||||
~AlphaFilter() = default;
|
||||
|
||||
void reset(const T &val) { _x = val; }
|
||||
|
||||
void update(const T &input, float tau, float dt)
|
||||
{
|
||||
const float alpha = dt / tau;
|
||||
update(input, alpha);
|
||||
}
|
||||
|
||||
void update(const T &input, float alpha)
|
||||
{
|
||||
_x = (1.f - alpha) * _x + alpha * input;
|
||||
}
|
||||
|
||||
// Typical 0.9/0.1 lowpass filter
|
||||
void update(const T &input)
|
||||
{
|
||||
update(input, 0.1f);
|
||||
}
|
||||
|
||||
const T& getState() const { return _x; }
|
||||
|
||||
private:
|
||||
T _x{}; ///< current state of the filter
|
||||
};
|
||||
@ -34,6 +34,7 @@
|
||||
add_library(ecl_EKF
|
||||
airspeed_fusion.cpp
|
||||
control.cpp
|
||||
mag_control.cpp
|
||||
covariance.cpp
|
||||
drag_fusion.cpp
|
||||
ekf.cpp
|
||||
|
||||
@ -200,7 +200,7 @@ struct auxVelSample {
|
||||
#define MAG_FUSE_TYPE_AUTO 0 ///< The selection of either heading or 3D magnetometer fusion will be automatic
|
||||
#define MAG_FUSE_TYPE_HEADING 1 ///< Simple yaw angle fusion will always be used. This is less accurate, but less affected by earth field distortions. It should not be used for pitch angles outside the range from -60 to +60 deg
|
||||
#define MAG_FUSE_TYPE_3D 2 ///< Magnetometer 3-axis fusion will always be used. This is more accurate, but more affected by localised earth field distortions
|
||||
#define MAG_FUSE_TYPE_AUTOFW 3 ///< The same as option 0, but if fusing airspeed, magnetometer fusion is only allowed to modify the magnetic field states.
|
||||
#define MAG_FUSE_TYPE_UNUSED 3 ///< Not implemented
|
||||
#define MAG_FUSE_TYPE_INDOOR 4 ///< The same as option 0, but magnetometer or yaw fusion will not be used unless earth frame external aiding (GPS or External Vision) is being used. This prevents inconsistent magnetic fields associated with indoor operation degrading state estimates.
|
||||
#define MAG_FUSE_TYPE_NONE 5 ///< Do not use magnetometer under any circumstance. Other sources of yaw may be used if selected via the EKF2_AID_MASK parameter.
|
||||
|
||||
@ -361,6 +361,7 @@ struct parameters {
|
||||
|
||||
// compute synthetic magnetomter Z value if possible
|
||||
int32_t synthesize_mag_z{0};
|
||||
bool check_mag_strength{false};
|
||||
};
|
||||
|
||||
struct stateSample {
|
||||
@ -453,14 +454,14 @@ union filter_control_status_u {
|
||||
uint32_t ev_yaw : 1; ///< 13 - true when yaw data from external vision measurements is being fused
|
||||
uint32_t ev_hgt : 1; ///< 14 - true when height data from external vision measurements is being fused
|
||||
uint32_t fuse_beta : 1; ///< 15 - true when synthetic sideslip measurements are being fused
|
||||
uint32_t update_mag_states_only : 1; ///< 16 - true when only the magnetometer states are updated by the magnetometer
|
||||
uint32_t mag_field_disturbed : 1; ///< 16 - true when the mag field does not match the expected strength
|
||||
uint32_t fixed_wing : 1; ///< 17 - true when the vehicle is operating as a fixed wing vehicle
|
||||
uint32_t mag_fault : 1; ///< 18 - true when the magnetometer has been declared faulty and is no longer being used
|
||||
uint32_t fuse_aspd : 1; ///< 19 - true when airspeed measurements are being fused
|
||||
uint32_t gnd_effect : 1; ///< 20 - true when protection from ground effect induced static pressure rise is active
|
||||
uint32_t rng_stuck : 1; ///< 21 - true when rng data wasn't ready for more than 10s and new rng values haven't changed enough
|
||||
uint32_t gps_yaw : 1; ///< 22 - true when yaw (not ground course) data from a GPS receiver is being fused
|
||||
uint32_t mag_align_complete : 1; ///< 23 - true when the in-flight mag field alignment has been completed
|
||||
uint32_t mag_aligned_in_flight : 1; ///< 23 - true when the in-flight mag field alignment has been completed
|
||||
uint32_t ev_vel : 1; ///< 24 - true when local earth frame velocity data from external vision measurements are being fused
|
||||
uint32_t synthetic_mag_z : 1; ///< 25 - true when we are using a synthesized measurement for the magnetometer Z component
|
||||
} flags;
|
||||
|
||||
290
EKF/control.cpp
290
EKF/control.cpp
@ -57,7 +57,7 @@ void Ekf::controlFusionModes()
|
||||
// and declare the tilt alignment complete
|
||||
if ((angle_err_var_vec(0) + angle_err_var_vec(1)) < sq(math::radians(3.0f))) {
|
||||
_control_status.flags.tilt_align = true;
|
||||
_control_status.flags.yaw_align = resetMagHeading(_mag_sample_delayed.mag);
|
||||
_control_status.flags.yaw_align = resetMagHeading(_mag_lpf.getState());
|
||||
|
||||
// send alignment status message to the console
|
||||
if (_control_status.flags.baro_hgt) {
|
||||
@ -257,14 +257,10 @@ void Ekf::controlExternalVisionFusion()
|
||||
|
||||
// turn on fusion of external vision yaw measurements and disable all magnetometer fusion
|
||||
_control_status.flags.ev_yaw = true;
|
||||
_control_status.flags.mag_hdg = false;
|
||||
_control_status.flags.mag_dec = false;
|
||||
|
||||
// save covariance data for re-use if currently doing 3-axis fusion
|
||||
if (_control_status.flags.mag_3D) {
|
||||
save_mag_cov_data();
|
||||
_control_status.flags.mag_3D = false;
|
||||
}
|
||||
stopMagHdgFusion();
|
||||
stopMag3DFusion();
|
||||
|
||||
ECL_INFO_TIMESTAMPED("EKF commencing external vision yaw fusion");
|
||||
}
|
||||
@ -506,7 +502,7 @@ void Ekf::controlOpticalFlowFusion()
|
||||
{
|
||||
// If the heading is not aligned, reset the yaw and magnetic field states
|
||||
if (!_control_status.flags.yaw_align) {
|
||||
_control_status.flags.yaw_align = resetMagHeading(_mag_sample_delayed.mag);
|
||||
_control_status.flags.yaw_align = resetMagHeading(_mag_lpf.getState());
|
||||
}
|
||||
|
||||
// If the heading is valid and use is not inhibited , start using optical flow aiding
|
||||
@ -596,14 +592,10 @@ void Ekf::controlGpsFusion()
|
||||
// turn on fusion of external vision yaw measurements and disable all other yaw fusion
|
||||
_control_status.flags.gps_yaw = true;
|
||||
_control_status.flags.ev_yaw = false;
|
||||
_control_status.flags.mag_hdg = false;
|
||||
_control_status.flags.mag_dec = false;
|
||||
|
||||
// save covariance data for re-use if currently doing 3-axis fusion
|
||||
if (_control_status.flags.mag_3D) {
|
||||
save_mag_cov_data();
|
||||
_control_status.flags.mag_3D = false;
|
||||
}
|
||||
stopMagHdgFusion();
|
||||
stopMag3DFusion();
|
||||
|
||||
ECL_INFO_TIMESTAMPED("EKF commencing GPS yaw fusion");
|
||||
}
|
||||
@ -623,9 +615,12 @@ void Ekf::controlGpsFusion()
|
||||
// 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
|
||||
if (!_control_status.flags.yaw_align || _control_status.flags.ev_yaw || _mag_inhibit_yaw_reset_req) {
|
||||
const bool want_to_reset_mag_heading = !_control_status.flags.yaw_align ||
|
||||
_control_status.flags.ev_yaw ||
|
||||
_mag_inhibit_yaw_reset_req;
|
||||
if (want_to_reset_mag_heading && canResetMagHeading()) {
|
||||
_control_status.flags.ev_yaw = false;
|
||||
_control_status.flags.yaw_align = resetMagHeading(_mag_sample_delayed.mag);
|
||||
_control_status.flags.yaw_align = resetMagHeading(_mag_lpf.getState());
|
||||
// Handle the special case where we have not been constraining yaw drift or learning yaw bias due
|
||||
// to assumed invalid mag field associated with indoor operation with a downwards looking flow sensor.
|
||||
if (_mag_inhibit_yaw_reset_req) {
|
||||
@ -690,7 +685,7 @@ void Ekf::controlGpsFusion()
|
||||
// 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_align_complete = realignYawGPS();
|
||||
_control_status.flags.mag_aligned_in_flight = realignYawGPS();
|
||||
}
|
||||
|
||||
resetVelocity();
|
||||
@ -1301,267 +1296,6 @@ void Ekf::controlDragFusion()
|
||||
}
|
||||
}
|
||||
|
||||
void Ekf::controlMagFusion()
|
||||
{
|
||||
if (_params.mag_fusion_type >= MAG_FUSE_TYPE_NONE) {
|
||||
|
||||
// do not use the magnetometer and deactivate magnetic field states
|
||||
// save covariance data for re-use if currently doing 3-axis fusion
|
||||
if (_control_status.flags.mag_3D) {
|
||||
save_mag_cov_data();
|
||||
_control_status.flags.mag_3D = false;
|
||||
}
|
||||
zeroRows(P, 16, 21);
|
||||
zeroCols(P, 16, 21);
|
||||
_mag_decl_cov_reset = false;
|
||||
_control_status.flags.mag_hdg = false;
|
||||
|
||||
return;
|
||||
}
|
||||
|
||||
// If we are on ground, store the local position and time to use as a reference
|
||||
// Also reset the flight alignment flag so that the mag fields will be re-initialised next time we achieve flight altitude
|
||||
if (!_control_status.flags.in_air) {
|
||||
_last_on_ground_posD = _state.pos(2);
|
||||
_control_status.flags.mag_align_complete = false;
|
||||
_num_bad_flight_yaw_events = 0;
|
||||
}
|
||||
|
||||
// check for new magnetometer data that has fallen behind the fusion time horizon
|
||||
// If we are using external vision data for heading then no magnetometer fusion is used
|
||||
if (!_control_status.flags.ev_yaw && !_control_status.flags.gps_yaw && _mag_data_ready) {
|
||||
|
||||
// We need to reset the yaw angle after climbing away from the ground to enable
|
||||
// recovery from ground level magnetic interference.
|
||||
if (!_control_status.flags.mag_align_complete && _control_status.flags.in_air) {
|
||||
// Check if height has increased sufficiently to be away from ground magnetic anomalies
|
||||
// and request a yaw reset if not already requested.
|
||||
float terrain_vpos_estimate = isTerrainEstimateValid() ? _terrain_vpos : _last_on_ground_posD;
|
||||
_mag_yaw_reset_req |= (terrain_vpos_estimate - _state.pos(2)) > 1.5f;
|
||||
}
|
||||
|
||||
// perform a yaw reset if requested by other functions
|
||||
if (_mag_yaw_reset_req && _control_status.flags.tilt_align) {
|
||||
if (!_mag_use_inhibit ) {
|
||||
if (!_control_status.flags.mag_align_complete && _control_status.flags.fixed_wing && _control_status.flags.in_air) {
|
||||
// A fixed wing vehicle can use GPS to bound yaw errors immediately after launch
|
||||
_control_status.flags.mag_align_complete = realignYawGPS();
|
||||
|
||||
if (_velpos_reset_request) {
|
||||
resetVelocity();
|
||||
resetPosition();
|
||||
_velpos_reset_request = false;
|
||||
}
|
||||
|
||||
} else {
|
||||
_control_status.flags.mag_align_complete = resetMagHeading(_mag_sample_delayed.mag) && _control_status.flags.in_air;
|
||||
}
|
||||
}
|
||||
_control_status.flags.yaw_align = _control_status.flags.yaw_align || _control_status.flags.mag_align_complete;
|
||||
_mag_yaw_reset_req = false;
|
||||
}
|
||||
|
||||
// Determine if we should use simple magnetic heading fusion which works better when there are large external disturbances
|
||||
// or the more accurate 3-axis fusion
|
||||
if (_control_status.flags.mag_fault) {
|
||||
// do no magnetometer fusion at all
|
||||
_control_status.flags.mag_hdg = false;
|
||||
_control_status.flags.mag_3D = false;
|
||||
|
||||
} else if (_params.mag_fusion_type == MAG_FUSE_TYPE_AUTO || _params.mag_fusion_type == MAG_FUSE_TYPE_AUTOFW) {
|
||||
// Check if there has been enough change in horizontal velocity to make yaw observable
|
||||
// Apply hysteresis to check to avoid rapid toggling
|
||||
if (_yaw_angle_observable) {
|
||||
_yaw_angle_observable = _accel_lpf_NE.norm() > _params.mag_acc_gate;
|
||||
|
||||
} else {
|
||||
_yaw_angle_observable = _accel_lpf_NE.norm() > 2.0f * _params.mag_acc_gate;
|
||||
}
|
||||
|
||||
_yaw_angle_observable = _yaw_angle_observable && (_control_status.flags.gps || _control_status.flags.ev_pos); // Do we have to add ev_vel here?
|
||||
|
||||
// check if there is enough yaw rotation to make the mag bias states observable
|
||||
if (!_mag_bias_observable && (fabsf(_yaw_rate_lpf_ef) > _params.mag_yaw_rate_gate)) {
|
||||
// initial yaw motion is detected
|
||||
_mag_bias_observable = true;
|
||||
_yaw_delta_ef = 0.0f;
|
||||
_time_yaw_started = _imu_sample_delayed.time_us;
|
||||
|
||||
} else if (_mag_bias_observable) {
|
||||
// monitor yaw rotation in 45 deg sections.
|
||||
// a rotation of 45 deg is sufficient to make the mag bias observable
|
||||
if (fabsf(_yaw_delta_ef) > math::radians(45.0f)) {
|
||||
_time_yaw_started = _imu_sample_delayed.time_us;
|
||||
_yaw_delta_ef = 0.0f;
|
||||
}
|
||||
|
||||
// require sustained yaw motion of 50% the initial yaw rate threshold
|
||||
float min_yaw_change_req = 0.5f * _params.mag_yaw_rate_gate * (1e-6f * (float)(_imu_sample_delayed.time_us - _time_yaw_started));
|
||||
_mag_bias_observable = fabsf(_yaw_delta_ef) > min_yaw_change_req;
|
||||
|
||||
} else {
|
||||
_mag_bias_observable = false;
|
||||
}
|
||||
|
||||
// record the last time that movement was suitable for use of 3-axis magnetometer fusion
|
||||
if (_mag_bias_observable || _yaw_angle_observable) {
|
||||
_time_last_movement = _imu_sample_delayed.time_us;
|
||||
}
|
||||
|
||||
// decide whether 3-axis magnetometer fusion can be used
|
||||
bool use_3D_fusion = _control_status.flags.tilt_align && // Use of 3D fusion requires valid tilt estimates
|
||||
_control_status.flags.in_air && // don't use when on the ground because of magnetic anomalies
|
||||
_control_status.flags.mag_align_complete &&
|
||||
((_imu_sample_delayed.time_us - _time_last_movement) < 2 * 1000 * 1000); // Using 3-axis fusion for a minimum period after to allow for false negatives
|
||||
|
||||
// perform switch-over
|
||||
if (use_3D_fusion) {
|
||||
if (!_control_status.flags.mag_3D) {
|
||||
// reset the mag field covariances
|
||||
zeroRows(P, 16, 21);
|
||||
zeroCols(P, 16, 21);
|
||||
|
||||
// re-instate variances for the D earth axis and XYZ body axis field
|
||||
for (uint8_t index = 0; index <= 3; index ++) {
|
||||
P[index + 18][index + 18] = _saved_mag_bf_variance[index];
|
||||
}
|
||||
// re-instate the NE axis covariance sub-matrix
|
||||
for (uint8_t row = 0; row <= 1; row ++) {
|
||||
for (uint8_t col = 0; col <= 1; col ++) {
|
||||
P[row + 16][col + 16] = _saved_mag_ef_covmat[row][col];
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
// only use one type of mag fusion at the same time
|
||||
_control_status.flags.mag_3D = _control_status.flags.mag_align_complete;
|
||||
_control_status.flags.mag_hdg = !_control_status.flags.mag_3D;
|
||||
|
||||
} else {
|
||||
// save covariance data for re-use if currently doing 3-axis fusion
|
||||
if (_control_status.flags.mag_3D) {
|
||||
save_mag_cov_data();
|
||||
_control_status.flags.mag_3D = false;
|
||||
}
|
||||
|
||||
_control_status.flags.mag_hdg = true;
|
||||
}
|
||||
|
||||
/*
|
||||
Control switch-over between only updating the mag states to updating all states
|
||||
When flying as a fixed wing aircraft, a misaligned magnetometer can cause an error in pitch/roll and accel bias estimates.
|
||||
When MAG_FUSE_TYPE_AUTOFW is selected and the vehicle is flying as a fixed wing, then magnetometer fusion is only allowed
|
||||
to access the magnetic field states.
|
||||
*/
|
||||
_control_status.flags.update_mag_states_only = (_params.mag_fusion_type == MAG_FUSE_TYPE_AUTOFW)
|
||||
&& _control_status.flags.fixed_wing;
|
||||
|
||||
// For the first 5 seconds after switching to 3-axis fusion we allow the magnetic field state estimates to stabilise
|
||||
// before they are used to constrain heading drift
|
||||
_flt_mag_align_converging = ((_imu_sample_delayed.time_us - _flt_mag_align_start_time) < (uint64_t)5e6);
|
||||
|
||||
if (_control_status.flags.mag_3D && _control_status_prev.flags.update_mag_states_only && !_control_status.flags.update_mag_states_only) {
|
||||
// When re-commencing use of magnetometer to correct vehicle states
|
||||
// set the field state variance to the observation variance and zero
|
||||
// the covariance terms to allow the field states re-learn rapidly
|
||||
zeroRows(P, 16, 21);
|
||||
zeroCols(P, 16, 21);
|
||||
_mag_decl_cov_reset = false;
|
||||
|
||||
for (uint8_t index = 0; index <= 5; index ++) {
|
||||
P[index + 16][index + 16] = sq(_params.mag_noise);
|
||||
}
|
||||
|
||||
// save covariance data for re-use when auto-switching between heading and 3-axis fusion
|
||||
save_mag_cov_data();
|
||||
}
|
||||
|
||||
} else if (_params.mag_fusion_type == MAG_FUSE_TYPE_HEADING || _params.mag_fusion_type == MAG_FUSE_TYPE_INDOOR) {
|
||||
// always use heading fusion
|
||||
_control_status.flags.mag_hdg = true;
|
||||
|
||||
// save covariance data for re-use if currently doing 3-axis fusion
|
||||
if (_control_status.flags.mag_3D) {
|
||||
save_mag_cov_data();
|
||||
_control_status.flags.mag_3D = false;
|
||||
}
|
||||
|
||||
} else if (_params.mag_fusion_type == MAG_FUSE_TYPE_3D) {
|
||||
if (!_control_status.flags.mag_3D && _control_status.flags.yaw_align) {
|
||||
// only commence 3-axis fusion when yaw is aligned and field states set
|
||||
_control_status.flags.mag_3D = true;
|
||||
}
|
||||
} else {
|
||||
// do no magnetometer fusion at all
|
||||
_control_status.flags.mag_hdg = false;
|
||||
|
||||
// save covariance data for re-use if currently doing 3-axis fusion
|
||||
if (_control_status.flags.mag_3D) {
|
||||
save_mag_cov_data();
|
||||
_control_status.flags.mag_3D = false;
|
||||
}
|
||||
|
||||
}
|
||||
|
||||
// if we are using 3-axis magnetometer fusion, but without external aiding, then the declination must be fused as an observation to prevent long term heading drift
|
||||
// fusing declination when gps aiding is available is optional, but recommended to prevent problem if the vehicle is static for extended periods of time
|
||||
if (_control_status.flags.mag_3D && (!_control_status.flags.gps || (_params.mag_declination_source & MASK_FUSE_DECL))) {
|
||||
_control_status.flags.mag_dec = true;
|
||||
|
||||
} else {
|
||||
_control_status.flags.mag_dec = false;
|
||||
}
|
||||
|
||||
// If the user has selected auto protection against indoor magnetic field errors, only use the magnetometer
|
||||
// if a yaw angle relative to true North is required for navigation. If no GPS or other earth frame aiding
|
||||
// is available, assume that we are operating indoors and the magnetometer should not be used.
|
||||
bool user_selected = (_params.mag_fusion_type == MAG_FUSE_TYPE_INDOOR);
|
||||
bool not_using_gps = !(_params.fusion_mode & MASK_USE_GPS) || !_control_status.flags.gps;
|
||||
bool not_using_evpos = !(_params.fusion_mode & MASK_USE_EVPOS) || !_control_status.flags.ev_pos;
|
||||
bool not_using_evvel = !(_params.fusion_mode & MASK_USE_EVVEL) || !_control_status.flags.ev_vel;
|
||||
bool not_selected_evyaw = !(_params.fusion_mode & MASK_USE_EVYAW);
|
||||
if (user_selected && not_using_gps && not_using_evpos && not_using_evvel && not_selected_evyaw) {
|
||||
_mag_use_inhibit = true;
|
||||
} else {
|
||||
_mag_use_inhibit = false;
|
||||
_mag_use_not_inhibit_us = _imu_sample_delayed.time_us;
|
||||
}
|
||||
|
||||
// If magnetometer use has been inhibited continuously then a yaw reset is required for a valid heading
|
||||
if (uint32_t(_imu_sample_delayed.time_us - _mag_use_not_inhibit_us) > (uint32_t)5e6) {
|
||||
_mag_inhibit_yaw_reset_req = true;
|
||||
}
|
||||
|
||||
// fuse magnetometer data using the selected methods
|
||||
if (_control_status.flags.mag_3D && _control_status.flags.yaw_align) {
|
||||
if (!_mag_decl_cov_reset) {
|
||||
// After any magnetic field covariance reset event the earth field state
|
||||
// covariances need to be corrected to incorporate knowedge of the declination
|
||||
// before fusing magnetomer data to prevent rapid rotation of the earth field
|
||||
// states for the first few observations.
|
||||
fuseDeclination(0.02f);
|
||||
_mag_decl_cov_reset = true;
|
||||
fuseMag();
|
||||
} else {
|
||||
// The normal sequence is to fuse the magnetometer data first before fusing
|
||||
// declination angle at a higher uncertainty to allow some learning of
|
||||
// declination angle over time.
|
||||
fuseMag();
|
||||
if (_control_status.flags.mag_dec) {
|
||||
fuseDeclination(0.5f);
|
||||
}
|
||||
}
|
||||
|
||||
} else if (_control_status.flags.mag_hdg && _control_status.flags.yaw_align) {
|
||||
// fusion of an Euler yaw angle from either a 321 or 312 rotation sequence
|
||||
fuseHeading();
|
||||
|
||||
} else {
|
||||
// do no fusion at all
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
void Ekf::controlVelPosFusion()
|
||||
{
|
||||
// if we aren't doing any aiding, fake GPS measurements at the last known position to constrain drift
|
||||
|
||||
@ -110,7 +110,7 @@ void Ekf::initialiseCovariance()
|
||||
}
|
||||
|
||||
// save covariance data for re-use when auto-switching between heading and 3-axis fusion
|
||||
save_mag_cov_data();
|
||||
saveMagCovData();
|
||||
|
||||
// wind
|
||||
P[22][22] = sq(_params.initial_wind_uncertainty);
|
||||
@ -896,16 +896,14 @@ void Ekf::fixCovarianceErrors()
|
||||
}
|
||||
}
|
||||
|
||||
void Ekf::resetMagCovariance()
|
||||
void Ekf::resetMagRelatedCovariances()
|
||||
{
|
||||
// set the quaternion covariance terms to zero
|
||||
zeroRows(P, 0, 3);
|
||||
zeroCols(P, 0, 3);
|
||||
|
||||
// set the magnetic field covariance terms to zero
|
||||
zeroRows(P, 16, 21);
|
||||
zeroCols(P, 16, 21);
|
||||
_mag_decl_cov_reset = false;
|
||||
clearMagCov();
|
||||
|
||||
// set the field state variance to the observation variance
|
||||
for (uint8_t rc_index = 16; rc_index <= 21; rc_index ++) {
|
||||
@ -913,7 +911,19 @@ void Ekf::resetMagCovariance()
|
||||
}
|
||||
|
||||
// save covariance data for re-use when auto-switching between heading and 3-axis fusion
|
||||
save_mag_cov_data();
|
||||
saveMagCovData();
|
||||
}
|
||||
|
||||
void Ekf::clearMagCov()
|
||||
{
|
||||
zeroMagCov();
|
||||
_mag_decl_cov_reset = false;
|
||||
}
|
||||
|
||||
void Ekf::zeroMagCov()
|
||||
{
|
||||
zeroRows(P, 16, 21);
|
||||
zeroCols(P, 16, 21);
|
||||
}
|
||||
|
||||
void Ekf::resetWindCovariance()
|
||||
|
||||
10
EKF/ekf.cpp
10
EKF/ekf.cpp
@ -169,12 +169,10 @@ bool Ekf::initialiseFilter()
|
||||
|
||||
// don't start using data until we can be certain all bad initial data has been flushed
|
||||
if (_mag_counter == (uint8_t)(_obs_buffer_length + 1)) {
|
||||
// initialise filter states
|
||||
_mag_filt_state = _mag_sample_delayed.mag;
|
||||
_mag_lpf.reset(_mag_sample_delayed.mag);
|
||||
|
||||
} else if (_mag_counter > (uint8_t)(_obs_buffer_length + 1)) {
|
||||
// noise filter the data
|
||||
_mag_filt_state = _mag_filt_state * 0.9f + _mag_sample_delayed.mag * 0.1f;
|
||||
_mag_lpf.update(_mag_sample_delayed.mag);
|
||||
}
|
||||
}
|
||||
}
|
||||
@ -270,7 +268,7 @@ bool Ekf::initialiseFilter()
|
||||
_R_to_earth = Dcmf(_state.quat_nominal);
|
||||
|
||||
// calculate the initial magnetic field and yaw alignment
|
||||
_control_status.flags.yaw_align = resetMagHeading(_mag_filt_state, false, false);
|
||||
_control_status.flags.yaw_align = resetMagHeading(_mag_lpf.getState(), false, false);
|
||||
|
||||
// initialise the state covariance matrix
|
||||
initialiseCovariance();
|
||||
@ -279,7 +277,7 @@ bool Ekf::initialiseFilter()
|
||||
if (_control_status.flags.ev_yaw) {
|
||||
// using error estimate from external vision data TODO: this is never true
|
||||
increaseQuatYawErrVariance(sq(fmaxf(_ev_sample_delayed.angErr, 1.0e-2f)));
|
||||
} else if (_params.mag_fusion_type <= MAG_FUSE_TYPE_AUTOFW) {
|
||||
} else if (_params.mag_fusion_type <= MAG_FUSE_TYPE_3D) {
|
||||
// using magnetic heading tuning parameter
|
||||
increaseQuatYawErrVariance(sq(fmaxf(_params.mag_heading_noise, 1.0e-2f)));
|
||||
}
|
||||
|
||||
55
EKF/ekf.h
55
EKF/ekf.h
@ -200,7 +200,7 @@ public:
|
||||
// check if the EKF is dead reckoning horizontal velocity using inertial data only
|
||||
void update_deadreckoning_status();
|
||||
|
||||
bool isTerrainEstimateValid() override;
|
||||
bool isTerrainEstimateValid() const override;
|
||||
|
||||
void updateTerrainValidity();
|
||||
|
||||
@ -430,7 +430,7 @@ private:
|
||||
uint32_t _mag_counter{0}; ///< number of magnetometer samples read during initialisation
|
||||
uint32_t _ev_counter{0}; ///< number of external vision samples read during initialisation
|
||||
uint64_t _time_last_mag{0}; ///< measurement time of last magnetomter sample (uSec)
|
||||
Vector3f _mag_filt_state; ///< filtered magnetometer measurement (Gauss)
|
||||
AlphaFilterVector3f _mag_lpf; ///< filtered magnetometer measurement (Gauss)
|
||||
Vector3f _delVel_sum; ///< summed delta velocity (m/sec)
|
||||
float _hgt_sensor_offset{0.0f}; ///< set as necessary if desired to maintain the same height after a height reset (m)
|
||||
float _baro_hgt_offset{0.0f}; ///< baro height reading at the local NED origin (m)
|
||||
@ -439,7 +439,7 @@ private:
|
||||
float _last_on_ground_posD{0.0f}; ///< last vertical position when the in_air status was false (m)
|
||||
bool _flt_mag_align_converging{false}; ///< true when the in-flight mag field post alignment convergence is being performd
|
||||
uint64_t _flt_mag_align_start_time{0}; ///< time that inflight magnetic field alignment started (uSec)
|
||||
uint64_t _time_last_movement{0}; ///< last system time that sufficient movement to use 3-axis magnetometer fusion was detected (uSec)
|
||||
uint64_t _time_last_mov_3d_mag_suitable{0}; ///< last system time that sufficient movement to use 3-axis magnetometer fusion was detected (uSec)
|
||||
float _saved_mag_bf_variance[4] {}; ///< magnetic field state variances that have been saved for use at the next initialisation (Gauss**2)
|
||||
float _saved_mag_ef_covmat[2][2] {}; ///< NE magnetic field state covariance sub-matrix saved for use at the next initialisation (Gauss**2)
|
||||
bool _velpos_reset_request{false}; ///< true when a large yaw error has been fixed and a velocity and position state reset is required
|
||||
@ -561,7 +561,7 @@ private:
|
||||
|
||||
// reset the heading and magnetic field states using the declination and magnetometer/external vision measurements
|
||||
// return true if successful
|
||||
bool resetMagHeading(Vector3f &mag_init, bool increase_yaw_var = true, bool update_buffer=true);
|
||||
bool resetMagHeading(const Vector3f &mag_init, bool increase_yaw_var = true, bool update_buffer=true);
|
||||
|
||||
// Do a forced re-alignment of the yaw angle to align with the horizontal velocity vector from the GPS.
|
||||
// It is used to align the yaw angle after launch or takeoff for fixed wing vehicle.
|
||||
@ -621,6 +621,38 @@ private:
|
||||
|
||||
// control fusion of magnetometer observations
|
||||
void controlMagFusion();
|
||||
void updateMagFilter();
|
||||
|
||||
bool canRunMagFusion() const;
|
||||
|
||||
void checkHaglYawResetReq();
|
||||
float getTerrainVPos() const;
|
||||
|
||||
void runOnGroundYawReset();
|
||||
bool isYawResetAuthorized() const;
|
||||
bool canResetMagHeading() const;
|
||||
void runInAirYawReset();
|
||||
bool canRealignYawUsingGps() const;
|
||||
void runVelPosReset();
|
||||
|
||||
void selectMagAuto();
|
||||
void check3DMagFusionSuitability();
|
||||
void checkYawAngleObservability();
|
||||
void checkMagBiasObservability();
|
||||
bool isYawAngleObservable() const;
|
||||
bool isMagBiasObservable() const;
|
||||
bool canUse3DMagFusion() const;
|
||||
|
||||
void checkMagDeclRequired();
|
||||
void checkMagInhibition();
|
||||
bool shouldInhibitMag() const;
|
||||
void checkMagFieldStrength();
|
||||
bool isStrongMagneticDisturbance() const;
|
||||
bool isMeasuredMatchingGpsMagStrength() const;
|
||||
bool isMeasuredMatchingAverageMagStrength() const;
|
||||
static bool isMeasuredMatchingExpected(float measured, float expected, float gate);
|
||||
void runMagAndMagDeclFusions();
|
||||
void run3DMagAndDeclFusions();
|
||||
|
||||
// control fusion of range finder observations
|
||||
void controlRangeFinderFusion();
|
||||
@ -674,6 +706,12 @@ private:
|
||||
// set control flags to use external vision height
|
||||
void setControlEVHeight();
|
||||
|
||||
void stopMagFusion();
|
||||
void stopMag3DFusion();
|
||||
void stopMagHdgFusion();
|
||||
void startMagHdgFusion();
|
||||
void startMag3DFusion();
|
||||
|
||||
// zero the specified range of rows in the state covariance matrix
|
||||
void zeroRows(float (&cov_mat)[_k_num_states][_k_num_states], uint8_t first, uint8_t last);
|
||||
|
||||
@ -697,7 +735,7 @@ private:
|
||||
void initialiseQuatCovariances(Vector3f &rot_vec_var);
|
||||
|
||||
// perform a limited reset of the magnetic field state covariances
|
||||
void resetMagCovariance();
|
||||
void resetMagRelatedCovariances();
|
||||
|
||||
// perform a limited reset of the wind state covariances
|
||||
void resetWindCovariance();
|
||||
@ -714,8 +752,11 @@ private:
|
||||
// Argument is additional yaw variance in rad**2
|
||||
void increaseQuatYawErrVariance(float yaw_variance);
|
||||
|
||||
// save mag field state covariance data for re-use
|
||||
void save_mag_cov_data();
|
||||
// load and save mag field state covariance data for re-use
|
||||
void loadMagCovData();
|
||||
void saveMagCovData();
|
||||
void clearMagCov();
|
||||
void zeroMagCov();
|
||||
|
||||
// uncorrelate quaternion states from other states
|
||||
void uncorrelateQuatStates();
|
||||
|
||||
@ -433,7 +433,7 @@ bool Ekf::realignYawGPS()
|
||||
ECL_WARN_TIMESTAMPED("EKF bad yaw corrected using GPS course");
|
||||
|
||||
// declare the magnetometer as failed if a bad yaw has occurred more than once
|
||||
if (_control_status.flags.mag_align_complete && (_num_bad_flight_yaw_events >= 2) && !_control_status.flags.mag_fault) {
|
||||
if (_control_status.flags.mag_aligned_in_flight && (_num_bad_flight_yaw_events >= 2) && !_control_status.flags.mag_fault) {
|
||||
ECL_WARN_TIMESTAMPED("EKF stopping magnetometer use");
|
||||
_control_status.flags.mag_fault = true;
|
||||
}
|
||||
@ -448,11 +448,11 @@ bool Ekf::realignYawGPS()
|
||||
Eulerf euler321(_state.quat_nominal);
|
||||
|
||||
// apply yaw correction
|
||||
if (!_control_status.flags.mag_align_complete) {
|
||||
if (!_control_status.flags.mag_aligned_in_flight) {
|
||||
// This is our first flight alignment so we can assume that the recent change in velocity has occurred due to a
|
||||
// forward direction takeoff or launch and therefore the inertial and GPS ground course discrepancy is due to yaw error
|
||||
euler321(2) += courseYawError;
|
||||
_control_status.flags.mag_align_complete = true;
|
||||
_control_status.flags.mag_aligned_in_flight = true;
|
||||
|
||||
} else if (_control_status.flags.wind) {
|
||||
// we have previously aligned yaw in-flight and have wind estimates so set the yaw such that the vehicle nose is
|
||||
@ -488,9 +488,7 @@ bool Ekf::realignYawGPS()
|
||||
increaseQuatYawErrVariance(sq(asinf(sineYawError)));
|
||||
|
||||
// reset the corresponding rows and columns in the covariance matrix and set the variances on the magnetic field states to the measurement variance
|
||||
zeroRows(P, 16, 21);
|
||||
zeroCols(P, 16, 21);
|
||||
_mag_decl_cov_reset = false;
|
||||
clearMagCov();
|
||||
|
||||
if (_control_status.flags.mag_3D) {
|
||||
for (uint8_t index = 16; index <= 21; index ++) {
|
||||
@ -498,7 +496,7 @@ bool Ekf::realignYawGPS()
|
||||
}
|
||||
|
||||
// save covariance data for re-use when auto-switching between heading and 3-axis fusion
|
||||
save_mag_cov_data();
|
||||
saveMagCovData();
|
||||
}
|
||||
|
||||
// record the start time for the magnetic field alignment
|
||||
@ -528,9 +526,7 @@ bool Ekf::realignYawGPS()
|
||||
_state.mag_I = _R_to_earth * _mag_sample_delayed.mag;
|
||||
|
||||
// reset the corresponding rows and columns in the covariance matrix and set the variances on the magnetic field states to the measurement variance
|
||||
zeroRows(P, 16, 21);
|
||||
zeroCols(P, 16, 21);
|
||||
_mag_decl_cov_reset = false;
|
||||
clearMagCov();
|
||||
|
||||
if (_control_status.flags.mag_3D) {
|
||||
for (uint8_t index = 16; index <= 21; index ++) {
|
||||
@ -538,7 +534,7 @@ bool Ekf::realignYawGPS()
|
||||
}
|
||||
|
||||
// save covariance data for re-use when auto-switching between heading and 3-axis fusion
|
||||
save_mag_cov_data();
|
||||
saveMagCovData();
|
||||
}
|
||||
|
||||
// record the start time for the magnetic field alignment
|
||||
@ -549,13 +545,13 @@ bool Ekf::realignYawGPS()
|
||||
|
||||
} else {
|
||||
// attempt a normal alignment using the magnetometer
|
||||
return resetMagHeading(_mag_sample_delayed.mag);
|
||||
return resetMagHeading(_mag_lpf.getState());
|
||||
|
||||
}
|
||||
}
|
||||
|
||||
// Reset heading and magnetic field states
|
||||
bool Ekf::resetMagHeading(Vector3f &mag_init, bool increase_yaw_var, bool update_buffer)
|
||||
bool Ekf::resetMagHeading(const Vector3f &mag_init, bool increase_yaw_var, bool update_buffer)
|
||||
{
|
||||
// prevent a reset being performed more than once on the same frame
|
||||
if (_imu_sample_delayed.time_us == _flt_mag_align_start_time) {
|
||||
@ -563,17 +559,7 @@ bool Ekf::resetMagHeading(Vector3f &mag_init, bool increase_yaw_var, bool update
|
||||
}
|
||||
|
||||
if (_params.mag_fusion_type >= MAG_FUSE_TYPE_NONE) {
|
||||
// do not use the magnetometer and deactivate magnetic field states
|
||||
// save covariance data for re-use if currently doing 3-axis fusion
|
||||
if (_control_status.flags.mag_3D) {
|
||||
save_mag_cov_data();
|
||||
_control_status.flags.mag_3D = false;
|
||||
}
|
||||
zeroRows(P, 16, 21);
|
||||
zeroCols(P, 16, 21);
|
||||
_mag_decl_cov_reset = false;
|
||||
_control_status.flags.mag_hdg = false;
|
||||
|
||||
stopMagFusion();
|
||||
return false;
|
||||
}
|
||||
|
||||
@ -603,7 +589,7 @@ bool Ekf::resetMagHeading(Vector3f &mag_init, bool increase_yaw_var, bool update
|
||||
// calculate the yaw angle for a 312 sequence
|
||||
euler321(2) = atan2f(R_to_earth_ev(1, 0), R_to_earth_ev(0, 0));
|
||||
|
||||
} else if (_params.mag_fusion_type <= MAG_FUSE_TYPE_AUTOFW) {
|
||||
} else if (_params.mag_fusion_type <= MAG_FUSE_TYPE_3D) {
|
||||
// rotate the magnetometer measurements into earth frame using a zero yaw angle
|
||||
Vector3f mag_earth_pred = R_to_earth * mag_init;
|
||||
// the angle of the projection onto the horizontal gives the yaw angle
|
||||
@ -661,7 +647,7 @@ bool Ekf::resetMagHeading(Vector3f &mag_init, bool increase_yaw_var, bool update
|
||||
// calculate the yaw angle for a 312 sequence
|
||||
euler312(0) = atan2f(-R_to_earth_ev(0, 1), R_to_earth_ev(1, 1));
|
||||
|
||||
} else if (_params.mag_fusion_type <= MAG_FUSE_TYPE_AUTOFW) {
|
||||
} else if (_params.mag_fusion_type <= MAG_FUSE_TYPE_3D) {
|
||||
// rotate the magnetometer measurements into earth frame using a zero yaw angle
|
||||
Vector3f mag_earth_pred = R_to_earth * mag_init;
|
||||
// the angle of the projection onto the horizontal gives the yaw angle
|
||||
@ -699,9 +685,7 @@ bool Ekf::resetMagHeading(Vector3f &mag_init, bool increase_yaw_var, bool update
|
||||
_state.mag_I = R_to_earth_after * mag_init;
|
||||
|
||||
// reset the corresponding rows and columns in the covariance matrix and set the variances on the magnetic field states to the measurement variance
|
||||
zeroRows(P, 16, 21);
|
||||
zeroCols(P, 16, 21);
|
||||
_mag_decl_cov_reset = false;
|
||||
clearMagCov();
|
||||
|
||||
if (_control_status.flags.mag_3D) {
|
||||
for (uint8_t index = 16; index <= 21; index ++) {
|
||||
@ -709,7 +693,7 @@ bool Ekf::resetMagHeading(Vector3f &mag_init, bool increase_yaw_var, bool update
|
||||
}
|
||||
|
||||
// save covariance data for re-use when auto-switching between heading and 3-axis fusion
|
||||
save_mag_cov_data();
|
||||
saveMagCovData();
|
||||
}
|
||||
|
||||
// record the time for the magnetic field alignment event
|
||||
@ -739,7 +723,7 @@ bool Ekf::resetMagHeading(Vector3f &mag_init, bool increase_yaw_var, bool update
|
||||
if (_control_status.flags.ev_yaw) {
|
||||
// using error estimate from external vision data
|
||||
increaseQuatYawErrVariance(sq(fmaxf(_ev_sample_delayed.angErr, 1.0e-2f)));
|
||||
} else if (_params.mag_fusion_type <= MAG_FUSE_TYPE_AUTOFW) {
|
||||
} else if (_params.mag_fusion_type <= MAG_FUSE_TYPE_3D) {
|
||||
// using magnetic heading tuning parameter
|
||||
increaseQuatYawErrVariance(sq(fmaxf(_params.mag_heading_noise, 1.0e-2f)));
|
||||
}
|
||||
@ -766,7 +750,7 @@ bool Ekf::resetMagHeading(Vector3f &mag_init, bool increase_yaw_var, bool update
|
||||
float Ekf::getMagDeclination()
|
||||
{
|
||||
// set source of magnetic declination for internal use
|
||||
if (_control_status.flags.mag_align_complete) {
|
||||
if (_control_status.flags.mag_aligned_in_flight) {
|
||||
// Use value consistent with earth field state
|
||||
return atan2f(_state.mag_I(1), _state.mag_I(0));
|
||||
|
||||
@ -1594,6 +1578,43 @@ void Ekf::setControlEVHeight()
|
||||
_control_status.flags.rng_hgt = false;
|
||||
}
|
||||
|
||||
void Ekf::stopMagFusion()
|
||||
{
|
||||
stopMag3DFusion();
|
||||
stopMagHdgFusion();
|
||||
clearMagCov();
|
||||
}
|
||||
|
||||
void Ekf::stopMag3DFusion()
|
||||
{
|
||||
// save covariance data for re-use if currently doing 3-axis fusion
|
||||
if (_control_status.flags.mag_3D) {
|
||||
saveMagCovData();
|
||||
_control_status.flags.mag_3D = false;
|
||||
}
|
||||
}
|
||||
|
||||
void Ekf::stopMagHdgFusion()
|
||||
{
|
||||
_control_status.flags.mag_hdg = false;
|
||||
}
|
||||
|
||||
void Ekf::startMagHdgFusion()
|
||||
{
|
||||
stopMag3DFusion();
|
||||
_control_status.flags.mag_hdg = true;
|
||||
}
|
||||
|
||||
void Ekf::startMag3DFusion()
|
||||
{
|
||||
if (!_control_status.flags.mag_3D) {
|
||||
stopMagHdgFusion();
|
||||
zeroMagCov();
|
||||
loadMagCovData();
|
||||
_control_status.flags.mag_3D = true;
|
||||
}
|
||||
}
|
||||
|
||||
// update the estimated misalignment between the EV navigation frame and the EKF navigation frame
|
||||
// and calculate a rotation matrix which rotates EV measurements into the EKF's navigation frame
|
||||
void Ekf::calcExtVisRotMat()
|
||||
@ -1709,7 +1730,7 @@ void Ekf::increaseQuatYawErrVariance(float yaw_variance)
|
||||
}
|
||||
|
||||
// save covariance data for re-use when auto-switching between heading and 3-axis fusion
|
||||
void Ekf::save_mag_cov_data()
|
||||
void Ekf::saveMagCovData()
|
||||
{
|
||||
// save variances for the D earth axis and XYZ body axis field
|
||||
for (uint8_t index = 0; index <= 3; index ++) {
|
||||
@ -1724,6 +1745,20 @@ void Ekf::save_mag_cov_data()
|
||||
}
|
||||
}
|
||||
|
||||
void Ekf::loadMagCovData()
|
||||
{
|
||||
// re-instate variances for the D earth axis and XYZ body axis field
|
||||
for (uint8_t index = 0; index <= 3; index ++) {
|
||||
P[index + 18][index + 18] = _saved_mag_bf_variance[index];
|
||||
}
|
||||
// re-instate the NE axis covariance sub-matrix
|
||||
for (uint8_t row = 0; row <= 1; row ++) {
|
||||
for (uint8_t col = 0; col <= 1; col ++) {
|
||||
P[row + 16][col + 16] = _saved_mag_ef_covmat[row][col];
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
float Ekf::kahanSummation(float sum_previous, float input, float &accumulator) const
|
||||
{
|
||||
float y = input - accumulator;
|
||||
|
||||
@ -44,6 +44,7 @@
|
||||
#include <ecl.h>
|
||||
#include "common.h"
|
||||
#include "RingBuffer.h"
|
||||
#include "AlphaFilter.hpp"
|
||||
|
||||
#include <geo/geo.h>
|
||||
#include <matrix/math.hpp>
|
||||
@ -55,6 +56,8 @@ class EstimatorInterface
|
||||
{
|
||||
|
||||
public:
|
||||
typedef AlphaFilter<Vector3f> AlphaFilterVector3f;
|
||||
|
||||
EstimatorInterface() = default;
|
||||
virtual ~EstimatorInterface() = default;
|
||||
|
||||
@ -245,9 +248,6 @@ public:
|
||||
_time_last_gnd_effect_on = _time_last_imu;
|
||||
}
|
||||
|
||||
// set flag if only only mag states should be updated by the magnetometer
|
||||
void set_update_mag_states_only_flag(bool update_mag_states_only) {_control_status.flags.update_mag_states_only = update_mag_states_only;}
|
||||
|
||||
// set air density used by the multi-rotor specific drag force fusion
|
||||
void set_air_density(float air_density) {_air_density = air_density;}
|
||||
|
||||
@ -272,7 +272,7 @@ public:
|
||||
// return true if the EKF is dead reckoning the position using inertial data only
|
||||
bool inertial_dead_reckoning() {return _is_dead_reckoning;}
|
||||
|
||||
virtual bool isTerrainEstimateValid() = 0;
|
||||
virtual bool isTerrainEstimateValid() const = 0;
|
||||
//[[deprecated("Replaced by isTerrainEstimateValid")]]
|
||||
bool get_terrain_valid() { return isTerrainEstimateValid(); }
|
||||
|
||||
|
||||
354
EKF/mag_control.cpp
Normal file
354
EKF/mag_control.cpp
Normal file
@ -0,0 +1,354 @@
|
||||
/****************************************************************************
|
||||
*
|
||||
* Copyright (c) 2019 Estimation and Control Library (ECL). 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 ECL 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 mag_control.cpp
|
||||
* Control functions for ekf magnetic field fusion
|
||||
*/
|
||||
|
||||
#include "ekf.h"
|
||||
#include <mathlib/mathlib.h>
|
||||
|
||||
void Ekf::controlMagFusion()
|
||||
{
|
||||
updateMagFilter();
|
||||
checkMagFieldStrength();
|
||||
|
||||
// If we are on ground, store the local position and time to use as a reference
|
||||
// Also reset the flight alignment flag so that the mag fields will be re-initialised next time we achieve flight altitude
|
||||
if (!_control_status.flags.in_air) {
|
||||
_last_on_ground_posD = _state.pos(2);
|
||||
_control_status.flags.mag_aligned_in_flight = false;
|
||||
_num_bad_flight_yaw_events = 0;
|
||||
}
|
||||
|
||||
if ((_params.mag_fusion_type >= MAG_FUSE_TYPE_NONE)
|
||||
|| _control_status.flags.mag_fault
|
||||
|| !_control_status.flags.yaw_align) {
|
||||
stopMagFusion();
|
||||
return;
|
||||
}
|
||||
|
||||
if (canRunMagFusion()) {
|
||||
if (_control_status.flags.in_air) {
|
||||
checkHaglYawResetReq();
|
||||
runInAirYawReset();
|
||||
runVelPosReset();
|
||||
|
||||
} else {
|
||||
runOnGroundYawReset();
|
||||
}
|
||||
|
||||
// Determine if we should use simple magnetic heading fusion which works better when
|
||||
// there are large external disturbances or the more accurate 3-axis fusion
|
||||
switch (_params.mag_fusion_type) {
|
||||
case MAG_FUSE_TYPE_AUTO:
|
||||
selectMagAuto();
|
||||
break;
|
||||
|
||||
case MAG_FUSE_TYPE_INDOOR:
|
||||
/* fallthrough */
|
||||
case MAG_FUSE_TYPE_HEADING:
|
||||
startMagHdgFusion();
|
||||
break;
|
||||
|
||||
case MAG_FUSE_TYPE_3D:
|
||||
startMag3DFusion();
|
||||
break;
|
||||
|
||||
default:
|
||||
selectMagAuto();
|
||||
break;
|
||||
}
|
||||
|
||||
checkMagDeclRequired();
|
||||
checkMagInhibition();
|
||||
|
||||
runMagAndMagDeclFusions();
|
||||
}
|
||||
}
|
||||
|
||||
void Ekf::updateMagFilter()
|
||||
{
|
||||
if (_mag_data_ready) {
|
||||
_mag_lpf.update(_mag_sample_delayed.mag);
|
||||
}
|
||||
}
|
||||
|
||||
bool Ekf::canRunMagFusion() const
|
||||
{
|
||||
// check for new magnetometer data that has fallen behind the fusion time horizon
|
||||
// If we are using external vision data or GPS-heading for heading then no magnetometer fusion is used
|
||||
return !_control_status.flags.ev_yaw && !_control_status.flags.gps_yaw && _mag_data_ready;
|
||||
}
|
||||
|
||||
void Ekf::checkHaglYawResetReq()
|
||||
{
|
||||
// We need to reset the yaw angle after climbing away from the ground to enable
|
||||
// recovery from ground level magnetic interference.
|
||||
if (!_control_status.flags.mag_aligned_in_flight) {
|
||||
// Check if height has increased sufficiently to be away from ground magnetic anomalies
|
||||
// and request a yaw reset if not already requested.
|
||||
static constexpr float mag_anomalies_max_hagl = 1.5f;
|
||||
const bool above_mag_anomalies = (getTerrainVPos() - _state.pos(2)) > mag_anomalies_max_hagl;
|
||||
_mag_yaw_reset_req = _mag_yaw_reset_req || above_mag_anomalies;
|
||||
}
|
||||
}
|
||||
|
||||
float Ekf::getTerrainVPos() const
|
||||
{
|
||||
return isTerrainEstimateValid() ? _terrain_vpos : _last_on_ground_posD;
|
||||
}
|
||||
|
||||
void Ekf::runOnGroundYawReset()
|
||||
{
|
||||
if (_mag_yaw_reset_req && isYawResetAuthorized()) {
|
||||
const bool has_realigned_yaw = canResetMagHeading()
|
||||
? resetMagHeading(_mag_lpf.getState())
|
||||
: false;
|
||||
|
||||
_mag_yaw_reset_req = !has_realigned_yaw;
|
||||
}
|
||||
}
|
||||
|
||||
bool Ekf::isYawResetAuthorized() const
|
||||
{
|
||||
return !_mag_use_inhibit;
|
||||
}
|
||||
|
||||
bool Ekf::canResetMagHeading() const
|
||||
{
|
||||
return !isStrongMagneticDisturbance();
|
||||
}
|
||||
|
||||
void Ekf::runInAirYawReset()
|
||||
{
|
||||
if (_mag_yaw_reset_req && isYawResetAuthorized()) {
|
||||
bool has_realigned_yaw = false;
|
||||
|
||||
if (canRealignYawUsingGps()) { has_realigned_yaw = realignYawGPS(); }
|
||||
else if (canResetMagHeading()) { has_realigned_yaw = resetMagHeading(_mag_lpf.getState()); }
|
||||
|
||||
_mag_yaw_reset_req = !has_realigned_yaw;
|
||||
_control_status.flags.mag_aligned_in_flight = has_realigned_yaw;
|
||||
}
|
||||
}
|
||||
|
||||
bool Ekf::canRealignYawUsingGps() const
|
||||
{
|
||||
return _control_status.flags.fixed_wing;
|
||||
}
|
||||
|
||||
void Ekf::runVelPosReset()
|
||||
{
|
||||
if (_velpos_reset_request) {
|
||||
resetVelocity();
|
||||
resetPosition();
|
||||
_velpos_reset_request = false;
|
||||
}
|
||||
}
|
||||
|
||||
void Ekf::selectMagAuto()
|
||||
{
|
||||
check3DMagFusionSuitability();
|
||||
canUse3DMagFusion() ? startMag3DFusion() : startMagHdgFusion();
|
||||
}
|
||||
|
||||
void Ekf::check3DMagFusionSuitability()
|
||||
{
|
||||
checkYawAngleObservability();
|
||||
checkMagBiasObservability();
|
||||
|
||||
if (isMagBiasObservable() || isYawAngleObservable()) {
|
||||
_time_last_mov_3d_mag_suitable = _imu_sample_delayed.time_us;
|
||||
}
|
||||
}
|
||||
|
||||
void Ekf::checkYawAngleObservability()
|
||||
{
|
||||
// Check if there has been enough change in horizontal velocity to make yaw observable
|
||||
// Apply hysteresis to check to avoid rapid toggling
|
||||
_yaw_angle_observable = _yaw_angle_observable
|
||||
? _accel_lpf_NE.norm() > _params.mag_acc_gate
|
||||
: _accel_lpf_NE.norm() > 2.0f * _params.mag_acc_gate;
|
||||
|
||||
_yaw_angle_observable = _yaw_angle_observable
|
||||
&& (_control_status.flags.gps || _control_status.flags.ev_pos); // Do we have to add ev_vel here?
|
||||
}
|
||||
|
||||
void Ekf::checkMagBiasObservability()
|
||||
{
|
||||
// check if there is enough yaw rotation to make the mag bias states observable
|
||||
if (!_mag_bias_observable && (fabsf(_yaw_rate_lpf_ef) > _params.mag_yaw_rate_gate)) {
|
||||
// initial yaw motion is detected
|
||||
_mag_bias_observable = true;
|
||||
|
||||
} else if (_mag_bias_observable) {
|
||||
// require sustained yaw motion of 50% the initial yaw rate threshold
|
||||
const float yaw_dt = 1e-6f * (float)(_imu_sample_delayed.time_us - _time_yaw_started);
|
||||
const float min_yaw_change_req = 0.5f * _params.mag_yaw_rate_gate * yaw_dt;
|
||||
_mag_bias_observable = fabsf(_yaw_delta_ef) > min_yaw_change_req;
|
||||
}
|
||||
|
||||
_yaw_delta_ef = 0.0f;
|
||||
_time_yaw_started = _imu_sample_delayed.time_us;
|
||||
}
|
||||
|
||||
bool Ekf::isYawAngleObservable() const
|
||||
{
|
||||
return _yaw_angle_observable;
|
||||
}
|
||||
|
||||
bool Ekf::isMagBiasObservable() const
|
||||
{
|
||||
return _mag_bias_observable;
|
||||
}
|
||||
|
||||
bool Ekf::canUse3DMagFusion() const
|
||||
{
|
||||
// Use of 3D fusion requires an in-air heading alignment but it should not
|
||||
// be used when the heading and mag biases are not observable for more than 2 seconds
|
||||
return _control_status.flags.mag_aligned_in_flight
|
||||
&& ((_imu_sample_delayed.time_us - _time_last_mov_3d_mag_suitable) < (uint64_t)2e6);
|
||||
}
|
||||
|
||||
void Ekf::checkMagDeclRequired()
|
||||
{
|
||||
// if we are using 3-axis magnetometer fusion, but without external NE aiding,
|
||||
// then the declination must be fused as an observation to prevent long term heading drift
|
||||
// fusing declination when gps aiding is available is optional, but recommended to prevent
|
||||
// problem if the vehicle is static for extended periods of time
|
||||
const bool user_selected = (_params.mag_declination_source & MASK_FUSE_DECL);
|
||||
const bool not_using_ne_aiding = !_control_status.flags.gps;
|
||||
_control_status.flags.mag_dec = (_control_status.flags.mag_3D && (not_using_ne_aiding || user_selected));
|
||||
}
|
||||
|
||||
void Ekf::checkMagInhibition()
|
||||
{
|
||||
_mag_use_inhibit = shouldInhibitMag();
|
||||
if (!_mag_use_inhibit) {
|
||||
_mag_use_not_inhibit_us = _imu_sample_delayed.time_us;
|
||||
}
|
||||
|
||||
// If magnetometer use has been inhibited continuously then a yaw reset is required for a valid heading
|
||||
if (uint32_t(_imu_sample_delayed.time_us - _mag_use_not_inhibit_us) > (uint32_t)5e6) {
|
||||
_mag_inhibit_yaw_reset_req = true;
|
||||
}
|
||||
}
|
||||
|
||||
bool Ekf::shouldInhibitMag() const
|
||||
{
|
||||
// If the user has selected auto protection against indoor magnetic field errors, only use the magnetometer
|
||||
// if a yaw angle relative to true North is required for navigation. If no GPS or other earth frame aiding
|
||||
// is available, assume that we are operating indoors and the magnetometer should not be used.
|
||||
// Also inhibit mag fusion when a strong magnetic field interference is detected
|
||||
const bool user_selected = (_params.mag_fusion_type == MAG_FUSE_TYPE_INDOOR);
|
||||
|
||||
const bool heading_not_required_for_navigation = !_control_status.flags.gps
|
||||
&& !_control_status.flags.ev_pos
|
||||
&& !_control_status.flags.ev_vel;
|
||||
|
||||
return (user_selected && heading_not_required_for_navigation)
|
||||
|| isStrongMagneticDisturbance();
|
||||
}
|
||||
|
||||
void Ekf::checkMagFieldStrength()
|
||||
{
|
||||
if (_params.check_mag_strength) {
|
||||
_control_status.flags.mag_field_disturbed = _NED_origin_initialised
|
||||
? !isMeasuredMatchingGpsMagStrength()
|
||||
: !isMeasuredMatchingAverageMagStrength();
|
||||
|
||||
} else {
|
||||
_control_status.flags.mag_field_disturbed = false;
|
||||
}
|
||||
}
|
||||
|
||||
bool Ekf::isStrongMagneticDisturbance() const
|
||||
{
|
||||
return _control_status.flags.mag_field_disturbed;
|
||||
}
|
||||
|
||||
bool Ekf::isMeasuredMatchingGpsMagStrength() const
|
||||
{
|
||||
constexpr float wmm_gate_size = 0.2f; // +/- Gauss
|
||||
return isMeasuredMatchingExpected(_mag_sample_delayed.mag.length(), _mag_strength_gps, wmm_gate_size);
|
||||
}
|
||||
|
||||
bool Ekf::isMeasuredMatchingAverageMagStrength() const
|
||||
{
|
||||
constexpr float average_earth_mag_field_strength = 0.45f; // Gauss
|
||||
constexpr float average_earth_mag_gate_size = 0.40f; // +/- Gauss
|
||||
return isMeasuredMatchingExpected(_mag_sample_delayed.mag.length(),
|
||||
average_earth_mag_field_strength,
|
||||
average_earth_mag_gate_size);
|
||||
}
|
||||
|
||||
bool Ekf::isMeasuredMatchingExpected(const float measured, const float expected, const float gate)
|
||||
{
|
||||
return (measured >= expected - gate)
|
||||
&& (measured <= expected + gate);
|
||||
}
|
||||
|
||||
void Ekf::runMagAndMagDeclFusions()
|
||||
{
|
||||
if (_control_status.flags.mag_3D) {
|
||||
run3DMagAndDeclFusions();
|
||||
|
||||
} else if (_control_status.flags.mag_hdg) {
|
||||
fuseHeading();
|
||||
}
|
||||
}
|
||||
|
||||
void Ekf::run3DMagAndDeclFusions()
|
||||
{
|
||||
if (!_mag_decl_cov_reset) {
|
||||
// After any magnetic field covariance reset event the earth field state
|
||||
// covariances need to be corrected to incorporate knowledge of the declination
|
||||
// before fusing magnetomer data to prevent rapid rotation of the earth field
|
||||
// states for the first few observations.
|
||||
fuseDeclination(0.02f);
|
||||
_mag_decl_cov_reset = true;
|
||||
fuseMag();
|
||||
|
||||
} else {
|
||||
// The normal sequence is to fuse the magnetometer data first before fusing
|
||||
// declination angle at a higher uncertainty to allow some learning of
|
||||
// declination angle over time.
|
||||
fuseMag();
|
||||
if (_control_status.flags.mag_dec) {
|
||||
fuseDeclination(0.5f);
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
@ -104,7 +104,7 @@ void Ekf::fuseMag()
|
||||
_fault_status.flags.bad_mag_x = true;
|
||||
|
||||
// we need to re-initialise covariances and abort this fusion step
|
||||
resetMagCovariance();
|
||||
resetMagRelatedCovariances();
|
||||
ECL_ERR_TIMESTAMPED("EKF magX fusion numerical error - covariance reset");
|
||||
return;
|
||||
|
||||
@ -123,7 +123,7 @@ void Ekf::fuseMag()
|
||||
_fault_status.flags.bad_mag_y = true;
|
||||
|
||||
// we need to re-initialise covariances and abort this fusion step
|
||||
resetMagCovariance();
|
||||
resetMagRelatedCovariances();
|
||||
ECL_ERR_TIMESTAMPED("EKF magY fusion numerical error - covariance reset");
|
||||
return;
|
||||
|
||||
@ -142,7 +142,7 @@ void Ekf::fuseMag()
|
||||
_fault_status.flags.bad_mag_z = true;
|
||||
|
||||
// we need to re-initialise covariances and abort this fusion step
|
||||
resetMagCovariance();
|
||||
resetMagRelatedCovariances();
|
||||
ECL_ERR_TIMESTAMPED("EKF magZ fusion numerical error - covariance reset");
|
||||
return;
|
||||
|
||||
@ -171,7 +171,7 @@ void Ekf::fuseMag()
|
||||
return;
|
||||
}
|
||||
|
||||
bool update_all_states = !_control_status.flags.update_mag_states_only && !_flt_mag_align_converging;
|
||||
bool update_all_states = !_flt_mag_align_converging;
|
||||
|
||||
// update the states and covariance using sequential fusion of the magnetometer components
|
||||
for (uint8_t index = 0; index <= 2; index++) {
|
||||
|
||||
@ -284,7 +284,7 @@ void Ekf::fuseFlowForTerrain()
|
||||
}
|
||||
}
|
||||
|
||||
bool Ekf::isTerrainEstimateValid()
|
||||
bool Ekf::isTerrainEstimateValid() const
|
||||
{
|
||||
return _hagl_valid;
|
||||
}
|
||||
|
||||
@ -94,7 +94,7 @@ static constexpr const int8_t inclination_table[13][37] = \
|
||||
{ 71,71,72,73,75,77,78,80,81,81,80,79,77,76,74,73,73,73,73,73,73,74,74,75,76,77,78,78,78,78,77,75,73,72,71,71,71 },
|
||||
};
|
||||
|
||||
// strength data in centi-Tesla
|
||||
// strength data in micro-Tesla or centi-Gauss
|
||||
static constexpr const int8_t strength_table[13][37] = \
|
||||
{
|
||||
{ 62,60,58,56,54,52,49,46,43,41,38,36,34,32,31,31,30,30,30,31,33,35,38,42,46,51,55,59,62,64,66,67,67,66,65,64,62 },
|
||||
|
||||
@ -38,6 +38,7 @@ set(SRCS
|
||||
test_EKF_basics.cpp
|
||||
test_EKF_ringbuffer.cpp
|
||||
test_EKF_imuSampling.cpp
|
||||
test_AlphaFilter.cpp
|
||||
)
|
||||
add_executable(ECL_GTESTS ${SRCS})
|
||||
|
||||
|
||||
190
test/test_AlphaFilter.cpp
Normal file
190
test/test_AlphaFilter.cpp
Normal file
@ -0,0 +1,190 @@
|
||||
/****************************************************************************
|
||||
*
|
||||
* Copyright (c) 2019 ECL 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 <gtest/gtest.h>
|
||||
#include <cmath>
|
||||
#include <matrix/math.hpp>
|
||||
|
||||
#include "EKF/AlphaFilter.hpp"
|
||||
|
||||
using matrix::Vector3f;
|
||||
|
||||
class AlphaFilterTest : public ::testing::Test {
|
||||
public:
|
||||
AlphaFilter<float> filter_float{};
|
||||
AlphaFilter<Vector3f> filter_v3{};
|
||||
};
|
||||
|
||||
TEST_F(AlphaFilterTest, initializeToZero)
|
||||
{
|
||||
ASSERT_EQ(filter_float.getState(), 0.f);
|
||||
}
|
||||
|
||||
TEST_F(AlphaFilterTest, resetToValue)
|
||||
{
|
||||
const float reset_value = 42.42f;
|
||||
filter_float.reset(reset_value);
|
||||
ASSERT_EQ(filter_float.getState(), reset_value);
|
||||
}
|
||||
|
||||
TEST_F(AlphaFilterTest, runZero)
|
||||
{
|
||||
const float input = 0.f;
|
||||
for (int i = 0; i < 10; i++) {
|
||||
filter_float.update(input);
|
||||
}
|
||||
ASSERT_EQ(filter_float.getState(), input);
|
||||
}
|
||||
|
||||
TEST_F(AlphaFilterTest, runPositive)
|
||||
{
|
||||
// GIVEN an input of 1 in a filter with a default time constant of 9 (alpha = 0.1)
|
||||
const float input = 1.f;
|
||||
|
||||
// WHEN we run the filter 9 times
|
||||
for (int i = 0; i < 9; i++) {
|
||||
filter_float.update(input);
|
||||
}
|
||||
|
||||
// THEN the state of the filter should have reached 63%
|
||||
ASSERT_NEAR(filter_float.getState(), 0.63f, 0.02);
|
||||
}
|
||||
|
||||
TEST_F(AlphaFilterTest, runNegative)
|
||||
{
|
||||
// GIVEN an input of 1 in a filter with a default time constant of 9 (alpha = 0.1)
|
||||
const float input = -1.f;
|
||||
|
||||
// WHEN we run the filter 9 times
|
||||
for (int i = 0; i < 9; i++) {
|
||||
filter_float.update(input);
|
||||
}
|
||||
|
||||
// THEN the state of the filter should have reached 63%
|
||||
ASSERT_NEAR(filter_float.getState(), -0.63f, 0.02);
|
||||
}
|
||||
|
||||
TEST_F(AlphaFilterTest, riseTime)
|
||||
{
|
||||
// GIVEN an input of 1 in a filter with a default time constant of 9 (alpha = 0.1)
|
||||
const float input = 1.f;
|
||||
|
||||
// WHEN we run the filter 27 times (3 * time constant)
|
||||
for (int i = 0; i < 3 * 9; i++) {
|
||||
filter_float.update(input);
|
||||
}
|
||||
|
||||
// THEN the state of the filter should have reached 95%
|
||||
ASSERT_NEAR(filter_float.getState(), 0.95f, 0.02f);
|
||||
}
|
||||
|
||||
TEST_F(AlphaFilterTest, convergence)
|
||||
{
|
||||
// GIVEN an input of 1 in a filter with a default time constant of 9 (alpha = 0.1)
|
||||
const float input = 1.f;
|
||||
|
||||
// WHEN we run the filter 45 times (5 * time constant)
|
||||
for (int i = 0; i < 5 * 9; i++) {
|
||||
filter_float.update(input);
|
||||
}
|
||||
|
||||
// THEN the state of the filter should have converged to the input
|
||||
ASSERT_NEAR(filter_float.getState(), 1.f, 0.01f);
|
||||
}
|
||||
|
||||
TEST_F(AlphaFilterTest, convergenceVector3f)
|
||||
{
|
||||
// GIVEN an Vector3f input in a filter with a default time constant of 9 (alpha = 0.1)
|
||||
const Vector3f input = {3.f, 7.f, -11.f};
|
||||
|
||||
// WHEN we run the filter 45 times (5 * time constant)
|
||||
for (int i = 0; i < 5 * 9; i++) {
|
||||
filter_v3.update(input);
|
||||
}
|
||||
|
||||
// THEN the state of the filter should have converged to the input (1% error allowed)
|
||||
Vector3f output = filter_v3.getState();
|
||||
for (int i = 0; i < 3; i++) {
|
||||
ASSERT_NEAR(output(i), input(i), fabsf(0.01f * input(i)));
|
||||
}
|
||||
}
|
||||
|
||||
TEST_F(AlphaFilterTest, convergenceVector3fAlpha)
|
||||
{
|
||||
// GIVEN a Vector3f input in a filter with a defined time constant and the default sampling time
|
||||
const Vector3f input = {3.f, 7.f, -11.f};
|
||||
const float tau = 18.f;
|
||||
const float dt = 1.f;
|
||||
const float alpha = dt / tau;
|
||||
|
||||
// WHEN we run the filter 18 times (1 * time constant)
|
||||
for (int i = 0; i < 18; i++) {
|
||||
filter_v3.update(input, alpha); // dt is assumed equal to 1
|
||||
}
|
||||
|
||||
// THEN the state of the filter should have reached 65% (2% error allowed)
|
||||
Vector3f output = filter_v3.getState();
|
||||
for (int i = 0; i < 3; i++) {
|
||||
ASSERT_NEAR(output(i), 0.63f * input(i), fabsf(0.02f * input(i)));
|
||||
}
|
||||
}
|
||||
|
||||
TEST_F(AlphaFilterTest, convergenceVector3fTauDt)
|
||||
{
|
||||
// GIVEN a Vector3f input in a filter with a defined time constant and sampling time
|
||||
const Vector3f input = {51.f, 7.f, -11.f};
|
||||
const float tau = 2.f;
|
||||
const float dt = 0.1f;
|
||||
|
||||
// WHEN we run the filter (1 * time constant)
|
||||
const float n = tau / dt;
|
||||
for (int i = 0; i < n; i++) {
|
||||
filter_v3.update(input, tau, dt);
|
||||
}
|
||||
|
||||
// THEN the state of the filter should have reached 65% (2% error allowed)
|
||||
Vector3f output = filter_v3.getState();
|
||||
for (int i = 0; i < 3; i++) {
|
||||
ASSERT_NEAR(output(i), 0.63f * input(i), fabsf(0.02f * input(i)));
|
||||
}
|
||||
|
||||
// ALSO when the filter is reset to a specified value
|
||||
const Vector3f reset_vector = {-1.f, 71.f, -42.f};
|
||||
filter_v3.reset(reset_vector);
|
||||
output = filter_v3.getState();
|
||||
|
||||
// THEN the filter should exactly contain those values
|
||||
for (int i = 0; i < 3; i++) {
|
||||
ASSERT_EQ(output(i), reset_vector(i));
|
||||
}
|
||||
}
|
||||
@ -190,13 +190,13 @@ TEST_F(EkfInitializationTest, initialControlMode)
|
||||
EXPECT_EQ(0, (int) control_status.flags.ev_yaw);
|
||||
EXPECT_EQ(0, (int) control_status.flags.ev_hgt);
|
||||
EXPECT_EQ(0, (int) control_status.flags.fuse_beta);
|
||||
EXPECT_EQ(0, (int) control_status.flags.update_mag_states_only);
|
||||
EXPECT_EQ(0, (int) control_status.flags.mag_field_disturbed);
|
||||
EXPECT_EQ(0, (int) control_status.flags.fixed_wing);
|
||||
EXPECT_EQ(0, (int) control_status.flags.mag_fault);
|
||||
EXPECT_EQ(0, (int) control_status.flags.gnd_effect);
|
||||
EXPECT_EQ(0, (int) control_status.flags.rng_stuck);
|
||||
EXPECT_EQ(0, (int) control_status.flags.gps_yaw);
|
||||
EXPECT_EQ(0, (int) control_status.flags.mag_align_complete);
|
||||
EXPECT_EQ(0, (int) control_status.flags.mag_aligned_in_flight);
|
||||
EXPECT_EQ(0, (int) control_status.flags.ev_vel);
|
||||
EXPECT_EQ(0, (int) control_status.flags.synthetic_mag_z);
|
||||
}
|
||||
@ -253,13 +253,13 @@ TEST_F(EkfInitializationTest, gpsFusion)
|
||||
EXPECT_EQ(0, (int) control_status.flags.ev_yaw);
|
||||
EXPECT_EQ(0, (int) control_status.flags.ev_hgt);
|
||||
EXPECT_EQ(0, (int) control_status.flags.fuse_beta);
|
||||
EXPECT_EQ(0, (int) control_status.flags.update_mag_states_only);
|
||||
EXPECT_EQ(0, (int) control_status.flags.mag_field_disturbed);
|
||||
EXPECT_EQ(0, (int) control_status.flags.fixed_wing);
|
||||
EXPECT_EQ(0, (int) control_status.flags.mag_fault);
|
||||
EXPECT_EQ(0, (int) control_status.flags.gnd_effect);
|
||||
EXPECT_EQ(0, (int) control_status.flags.rng_stuck);
|
||||
EXPECT_EQ(0, (int) control_status.flags.gps_yaw);
|
||||
EXPECT_EQ(0, (int) control_status.flags.mag_align_complete);
|
||||
EXPECT_EQ(0, (int) control_status.flags.mag_aligned_in_flight);
|
||||
EXPECT_EQ(0, (int) control_status.flags.ev_vel);
|
||||
EXPECT_EQ(0, (int) control_status.flags.synthetic_mag_z);
|
||||
}
|
||||
|
||||
Loading…
x
Reference in New Issue
Block a user