fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-05-02 02:54:08 +08:00
Code Issues Packages Projects Releases Wiki Activity

WIP: ekf2 filtered innovation hacks

Browse Source
This commit is contained in:
Daniel Agar 2022-09-13 15:02:31 -04:00
parent 82b28bc72f
commit 32d6081826
No known key found for this signature in database
GPG Key ID: FD3CBA98017A69DE
20 changed files with 159 additions and 641 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

1
msg/estimator_status.msg
View File

@ -116,6 +116,7 @@ float32 time_slip # cumulative amount of time in seconds that the EKF inertial c
bool pre_flt_fail_innov_heading
bool pre_flt_fail_innov_vel_horiz
bool pre_flt_fail_innov_vel_vert
bool pre_flt_fail_innov_pos_horiz
bool pre_flt_fail_innov_height
bool pre_flt_fail_mag_field_disturbed

7
msg/estimator_status_flags.msg
View File

@ -73,3 +73,10 @@ bool reject_sideslip # 9 - true if the synthetic sideslip
bool reject_hagl # 10 - true if the height above ground observation has been rejected
bool reject_optflow_x # 11 - true if the X optical flow observation has been rejected
bool reject_optflow_y # 12 - true if the Y optical flow observation has been rejected
# preflight failure checks
bool pre_flt_fail_innov_heading
bool pre_flt_fail_innov_vel_horiz
bool pre_flt_fail_innov_vel_vert
bool pre_flt_fail_innov_pos_horiz
bool pre_flt_fail_innov_height

15
src/lib/hysteresis/hysteresis.cpp
View File

@ -42,8 +42,7 @@
namespace systemlib
{
void
Hysteresis::set_hysteresis_time_from(const bool from_state, const hrt_abstime new_hysteresis_time_us)
void Hysteresis::set_hysteresis_time_from(const bool from_state, const hrt_abstime new_hysteresis_time_us)
{
if (from_state) {
_time_from_true_us = new_hysteresis_time_us;
@ -53,8 +52,7 @@ Hysteresis::set_hysteresis_time_from(const bool from_state, const hrt_abstime ne
}
}
void
Hysteresis::set_state_and_update(const bool new_state, const hrt_abstime &now_us)
bool Hysteresis::set_state_and_update(const bool new_state, const hrt_abstime &now_us)
{
if (new_state != _state) {
if (new_state != _requested_state) {
@ -66,12 +64,13 @@ Hysteresis::set_state_and_update(const bool new_state, const hrt_abstime &now_us
_requested_state = _state;
}
update(now_us);
return update(now_us);
}
void
Hysteresis::update(const hrt_abstime &now_us)
bool Hysteresis::update(const hrt_abstime &now_us)
{
const bool state_prev = _state;
if (_requested_state != _state) {
const hrt_abstime elapsed = now_us - _last_time_to_change_state;
@ -89,6 +88,8 @@ Hysteresis::update(const hrt_abstime &now_us)
}
}
}
return _state != state_prev;
}
} // namespace systemlib

6
src/lib/hysteresis/hysteresis.h
View File

@ -61,9 +61,11 @@ public:
void set_hysteresis_time_from(const bool from_state, const hrt_abstime new_hysteresis_time_us);
void set_state_and_update(const bool new_state, const hrt_abstime &now_us);
// returns true if state changed
bool set_state_and_update(const bool new_state, const hrt_abstime &now_us);
void update(const hrt_abstime &now_us);
// returns true if state changed
bool update(const hrt_abstime &now_us);
private:

2
src/lib/mathlib/math/filter/AlphaFilter.hpp
View File

@ -52,7 +52,7 @@ class AlphaFilter
{
public:
AlphaFilter() = default;
explicit AlphaFilter(float alpha) : _alpha(alpha) {}
AlphaFilter(float alpha) : _alpha(alpha) {}
~AlphaFilter() = default;

3
src/modules/ekf2/CMakeLists.txt
View File

@ -31,8 +31,6 @@
#
#############################################################################
add_subdirectory(Utility)
px4_add_module(
MODULE modules__ekf2
MAIN ekf2
@ -90,7 +88,6 @@ px4_add_module(
geo
hysteresis
perf
EKF2Utility
px4_work_queue
world_magnetic_model
UNITY_BUILD

42
src/modules/ekf2/EKF/ekf.h
View File

@ -114,6 +114,10 @@ public:
const Vector3f getFlowGyro() const { return _flow_sample_delayed.gyro_xyz * (1.f / _flow_sample_delayed.dt); }
const Vector3f &getFlowGyroIntegral() const { return _flow_sample_delayed.gyro_xyz; }
const auto& filteredVelocityInnovation(int index) const { return _vel_innov_lpf[index].getState(); }
const auto& filteredPositionInnovation(int index) const { return _pos_innov_lpf[index].getState(); }
const auto& filteredHeadingInnovation() const { return _heading_innov_lpf.getState(); }
void getHeadingInnov(float &heading_innov) const {
if (_control_status.flags.mag_hdg) {
heading_innov = _aid_src_mag_heading.innovation;
@ -275,6 +279,32 @@ public:
return (!_deadreckon_time_exceeded && !_control_status.flags.fake_pos);
}
bool horizontalPositionValid(const float innov_limit = 5.f) const
{
return (!_deadreckon_time_exceeded && !_control_status.flags.fake_pos)
&& isRecent(_time_last_hor_pos_fuse, _params.no_aid_timeout_max);
}
bool verticalPositionValid(const float innov_limit = 5.f) const
{
return isRecent(_time_last_hgt_fuse, _params.no_aid_timeout_max);
}
bool horizontalVelocityValid(const float innov_limit = 1.f) const
{
return isRecent(_time_last_hor_vel_fuse, _params.no_aid_timeout_max);
}
bool verticalVelocityValid(const float innov_limit = 1.f) const
{
return isRecent(_time_last_ver_vel_fuse, _params.no_aid_timeout_max);
}
bool headingValid(const float innov_limit = math::radians(30.f)) const
{
return isRecent(_time_last_heading_fuse, _params.no_aid_timeout_max);
}
bool isTerrainEstimateValid() const { return _hagl_valid; };
bool isYawFinalAlignComplete() const
@ -479,6 +509,13 @@ private:
uint64_t _time_last_healthy_rng_data{0};
uint8_t _nb_gps_yaw_reset_available{0}; ///< remaining number of resets allowed before switching to another aiding source
// Preflight low pass filter time constant inverse (1/sec)
static constexpr float INNOV_LPF_TAU_INV = 0.2f;
AlphaFilter<float> _vel_innov_lpf[3]{{INNOV_LPF_TAU_INV}, {INNOV_LPF_TAU_INV}, {INNOV_LPF_TAU_INV}}; ///< filtered velocity innovations (m/s)
AlphaFilter<float> _pos_innov_lpf[3]{{INNOV_LPF_TAU_INV}, {INNOV_LPF_TAU_INV}, {INNOV_LPF_TAU_INV}}; ///< filtered position innovations (m)
AlphaFilter<float> _heading_innov_lpf{{INNOV_LPF_TAU_INV}}; ///< filtered heading innovations (rad)
Vector3f _last_known_pos{}; ///< last known local position vector (m)
uint64_t _time_acc_bias_check{0}; ///< last time the accel bias check passed (uSec)
@ -1045,6 +1082,9 @@ private:
return sensor_timestamp + acceptance_interval > _newest_high_rate_imu_sample.time_us;
}
bool horizontalVelocityFusedRecently() const { return isTimedOut(_time_last_hor_vel_fuse, _params.valid_timeout_max); }
void startAirspeedFusion();
void stopAirspeedFusion();
@ -1074,7 +1114,7 @@ private:
void stopFakePosFusion();
void fuseFakePosition();
void setVelPosStatus(const int index, const bool healthy);
void setVelPosStatus(const int index, const float innov, const bool healthy);
// reset the quaternion states and covariances to the new yaw value, preserving the roll and pitch
// yaw : Euler yaw angle (rad)

1
src/modules/ekf2/EKF/ekf_helper.cpp
View File

@ -224,7 +224,6 @@ bool Ekf::isHeightResetRequired() const
return (continuous_bad_accel_hgt || hgt_fusion_timeout);
}
void Ekf::resetVerticalPositionTo(const float new_vert_pos)
{
const float old_vert_pos = _state.pos(2);

1
src/modules/ekf2/EKF/gps_yaw_fusion.cpp
View File

@ -198,6 +198,7 @@ void Ekf::fuseGpsYaw(const gpsSample& gps_sample)
if (is_fused) {
_time_last_heading_fuse = _imu_sample_delayed.time_us;
_aid_src_gnss_yaw.time_last_fuse = _imu_sample_delayed.time_us;
_heading_innov_lpf.update(heading_innov);
}
}

2
src/modules/ekf2/EKF/mag_fusion.cpp
View File

@ -676,6 +676,8 @@ bool Ekf::fuseYaw(const float innovation, const float variance, estimator_aid_so
aid_src_status.time_last_fuse = _imu_sample_delayed.time_us;
aid_src_status.fused = true;
_heading_innov_lpf.update(aid_src_status.innovation);
return true;
}

3
src/modules/ekf2/EKF/optflow_fusion.cpp
View File

@ -329,6 +329,9 @@ void Ekf::fuseOptFlow()
if (is_fused) {
_time_last_of_fuse = _imu_sample_delayed.time_us;
const float vel_innov = _flow_innov(obs_index) * range;
_vel_innov_lpf[obs_index].update(vel_innov);
}
}
}

10
src/modules/ekf2/EKF/vel_pos_fusion.cpp
View File

@ -287,7 +287,7 @@ bool Ekf::fuseVelPosHeight(const float innov, const float innov_var, const int o
}
}
setVelPosStatus(obs_index, healthy);
setVelPosStatus(obs_index, innov, healthy);
if (healthy) {
// apply the covariance corrections
@ -304,13 +304,14 @@ bool Ekf::fuseVelPosHeight(const float innov, const float innov_var, const int o
return false;
}
void Ekf::setVelPosStatus(const int index, const bool healthy)
void Ekf::setVelPosStatus(const int index, const float innov, const bool healthy)
{
switch (index) {
case 0:
if (healthy) {
_fault_status.flags.bad_vel_N = false;
_time_last_hor_vel_fuse = _imu_sample_delayed.time_us;
_vel_innov_lpf[0].update(innov);
} else {
_fault_status.flags.bad_vel_N = true;
@ -322,6 +323,7 @@ void Ekf::setVelPosStatus(const int index, const bool healthy)
if (healthy) {
_fault_status.flags.bad_vel_E = false;
_time_last_hor_vel_fuse = _imu_sample_delayed.time_us;
_vel_innov_lpf[1].update(innov);
} else {
_fault_status.flags.bad_vel_E = true;
@ -333,6 +335,7 @@ void Ekf::setVelPosStatus(const int index, const bool healthy)
if (healthy) {
_fault_status.flags.bad_vel_D = false;
_time_last_ver_vel_fuse = _imu_sample_delayed.time_us;
_vel_innov_lpf[2].update(innov);
} else {
_fault_status.flags.bad_vel_D = true;
@ -344,6 +347,7 @@ void Ekf::setVelPosStatus(const int index, const bool healthy)
if (healthy) {
_fault_status.flags.bad_pos_N = false;
_time_last_hor_pos_fuse = _imu_sample_delayed.time_us;
_pos_innov_lpf[0].update(innov);
} else {
_fault_status.flags.bad_pos_N = true;
@ -355,6 +359,7 @@ void Ekf::setVelPosStatus(const int index, const bool healthy)
if (healthy) {
_fault_status.flags.bad_pos_E = false;
_time_last_hor_pos_fuse = _imu_sample_delayed.time_us;
_pos_innov_lpf[1].update(innov);
} else {
_fault_status.flags.bad_pos_E = true;
@ -366,6 +371,7 @@ void Ekf::setVelPosStatus(const int index, const bool healthy)
if (healthy) {
_fault_status.flags.bad_pos_D = false;
_time_last_hgt_fuse = _imu_sample_delayed.time_us;
_pos_innov_lpf[2].update(innov);
} else {
_fault_status.flags.bad_pos_D = true;

100
src/modules/ekf2/EKF2.cpp
View File

@ -180,6 +180,18 @@ EKF2::EKF2(bool multi_mode, const px4::wq_config_t &config, bool replay_mode):
_estimator_states_pub.advertise();
_estimator_status_flags_pub.advertise();
_estimator_status_pub.advertise();
_pre_flt_fail_innov_heading.set_hysteresis_time_from(false, 100_ms);
_pre_flt_fail_innov_vel_horiz.set_hysteresis_time_from(false, 100_ms);
_pre_flt_fail_innov_vel_vert.set_hysteresis_time_from(false, 100_ms);
_pre_flt_fail_innov_pos_horiz.set_hysteresis_time_from(false, 100_ms);
_pre_flt_fail_innov_height.set_hysteresis_time_from(false, 100_ms);
_pre_flt_fail_innov_heading.set_hysteresis_time_from(true, 100_ms);
_pre_flt_fail_innov_vel_horiz.set_hysteresis_time_from(true, 100_ms);
_pre_flt_fail_innov_vel_vert.set_hysteresis_time_from(true, 100_ms);
_pre_flt_fail_innov_pos_horiz.set_hysteresis_time_from(true, 100_ms);
_pre_flt_fail_innov_height.set_hysteresis_time_from(true, 100_ms);
}
EKF2::~EKF2()
@ -537,9 +549,6 @@ void EKF2::Run()
// let the EKF know if the vehicle motion is that of a fixed wing (forward flight only relative to wind)
_ekf.set_is_fixed_wing(is_fixed_wing);
_preflt_checker.setVehicleCanObserveHeadingInFlight(vehicle_status.vehicle_type !=
vehicle_status_s::VEHICLE_TYPE_ROTARY_WING);
}
}
@ -566,8 +575,6 @@ void EKF2::Run()
_gyro_cal = {};
_mag_cal = {};
_preflt_checker.reset();
} else if (was_in_air && !in_air) {
// landed
_ekf.set_in_air_status(false);
@ -603,6 +610,34 @@ void EKF2::Run()
if (_ekf.update()) {
perf_set_elapsed(_ecl_ekf_update_full_perf, hrt_elapsed_time(&ekf_update_start));
bool pre_flt_fail_updated = false;
if (!_ekf.control_status_flags().in_air) {
bool pre_flt_fail_innov_heading = _ekf.filteredHeadingInnovation() > kHeadingInnovationTestLimit;
bool pre_flt_fail_innov_vel_horiz = (_ekf.filteredVelocityInnovation(0) > kVelocityInnovationTestLimit)
|| (_ekf.filteredVelocityInnovation(1) > kVelocityInnovationTestLimit);
bool pre_flt_fail_innov_vel_vert = (_ekf.filteredVelocityInnovation(2) > kVelocityInnovationTestLimit);
bool pre_flt_fail_innov_pos_horiz = (_ekf.filteredPositionInnovation(0) > kPositionInnovationTestLimit)
|| (_ekf.filteredPositionInnovation(1) > kPositionInnovationTestLimit);
bool pre_flt_fail_innov_height = (_ekf.filteredPositionInnovation(2) > kPositionInnovationTestLimit);
pre_flt_fail_updated |= _pre_flt_fail_innov_heading.set_state_and_update(pre_flt_fail_innov_heading, now);
pre_flt_fail_updated |= _pre_flt_fail_innov_vel_horiz.set_state_and_update(pre_flt_fail_innov_vel_horiz, now);
pre_flt_fail_updated |= _pre_flt_fail_innov_vel_vert.set_state_and_update(pre_flt_fail_innov_vel_vert, now);
pre_flt_fail_updated |= _pre_flt_fail_innov_pos_horiz.set_state_and_update(pre_flt_fail_innov_pos_horiz, now);
pre_flt_fail_updated |= _pre_flt_fail_innov_height.set_state_and_update(pre_flt_fail_innov_height, now);
} else if (_ekf.control_status_prev_flags().in_air != _ekf.control_status_flags().in_air) {
// reset
_pre_flt_fail_innov_heading.set_state_and_update(false, now);
_pre_flt_fail_innov_vel_horiz.set_state_and_update(false, now);
_pre_flt_fail_innov_vel_vert.set_state_and_update(false, now);
_pre_flt_fail_innov_pos_horiz.set_state_and_update(false, now);
_pre_flt_fail_innov_height.set_state_and_update(false, now);
}
PublishLocalPosition(now);
PublishOdometry(now);
PublishGlobalPosition(now);
@ -621,7 +656,7 @@ void EKF2::Run()
PublishOpticalFlowVel(now);
PublishStates(now);
PublishStatus(now);
PublishStatusFlags(now);
PublishStatusFlags(now, pre_flt_fail_updated);
PublishYawEstimatorStatus(now);
UpdateAccelCalibration(now);
@ -1000,22 +1035,6 @@ void EKF2::PublishInnovations(const hrt_abstime &timestamp)
innovations.timestamp = _replay_mode ? timestamp : hrt_absolute_time();
_estimator_innovations_pub.publish(innovations);
// calculate noise filtered velocity innovations which are used for pre-flight checking
if (!_ekf.control_status_flags().in_air) {
// TODO: move to run before publications
_preflt_checker.setUsingGpsAiding(_ekf.control_status_flags().gps);
_preflt_checker.setUsingFlowAiding(_ekf.control_status_flags().opt_flow);
_preflt_checker.setUsingEvPosAiding(_ekf.control_status_flags().ev_pos);
_preflt_checker.setUsingEvVelAiding(_ekf.control_status_flags().ev_vel);
_preflt_checker.setUsingBaroHgtAiding(_ekf.control_status_flags().baro_hgt);
_preflt_checker.setUsingGpsHgtAiding(_ekf.control_status_flags().gps_hgt);
_preflt_checker.setUsingRngHgtAiding(_ekf.control_status_flags().rng_hgt);
_preflt_checker.setUsingEvHgtAiding(_ekf.control_status_flags().ev_hgt);
_preflt_checker.update(_ekf.get_imu_sample_delayed().delta_ang_dt, innovations);
}
}
void EKF2::PublishInnovationTestRatios(const hrt_abstime &timestamp)
@ -1096,11 +1115,18 @@ void EKF2::PublishLocalPosition(const hrt_abstime &timestamp)
lpos.ay = vel_deriv(1);
lpos.az = vel_deriv(2);
// TODO: better status reporting
lpos.xy_valid = _ekf.local_position_is_valid();
lpos.z_valid = !_preflt_checker.hasVertFailed();
lpos.v_xy_valid = _ekf.local_position_is_valid();
lpos.v_z_valid = !_preflt_checker.hasVertFailed();
if (!_ekf.control_status_flags().in_air) {
lpos.xy_valid = _ekf.horizontalPositionValid() && _pre_flt_fail_innov_pos_horiz.get_state();
lpos.z_valid = _ekf.verticalPositionValid() && _pre_flt_fail_innov_height.get_state();
lpos.v_xy_valid = _ekf.horizontalVelocityValid() && _pre_flt_fail_innov_vel_horiz.get_state();
lpos.v_z_valid = _ekf.verticalVelocityValid() && _pre_flt_fail_innov_vel_vert.get_state();
} else {
lpos.xy_valid = _ekf.horizontalPositionValid();
lpos.z_valid = _ekf.verticalPositionValid();
lpos.v_xy_valid = _ekf.horizontalVelocityValid();
lpos.v_z_valid = _ekf.verticalVelocityValid();
}
// Position of local NED origin in GPS / WGS84 frame
if (_ekf.global_origin_valid()) {
@ -1351,10 +1377,11 @@ void EKF2::PublishStatus(const hrt_abstime &timestamp)
status.time_slip = _last_time_slip_us * 1e-6f;
status.pre_flt_fail_innov_heading = _preflt_checker.hasHeadingFailed();
status.pre_flt_fail_innov_vel_horiz = _preflt_checker.hasHorizVelFailed();
status.pre_flt_fail_innov_vel_vert = _preflt_checker.hasVertVelFailed();
status.pre_flt_fail_innov_height = _preflt_checker.hasHeightFailed();
status.pre_flt_fail_innov_heading = _pre_flt_fail_innov_heading.get_state();
status.pre_flt_fail_innov_vel_horiz = _pre_flt_fail_innov_vel_horiz.get_state();
status.pre_flt_fail_innov_vel_vert = _pre_flt_fail_innov_vel_vert.get_state();
status.pre_flt_fail_innov_pos_horiz = _pre_flt_fail_innov_pos_horiz.get_state();
status.pre_flt_fail_innov_height = _pre_flt_fail_innov_height.get_state();
status.pre_flt_fail_mag_field_disturbed = _ekf.control_status_flags().mag_field_disturbed;
status.accel_device_id = _device_id_accel;
@ -1366,7 +1393,7 @@ void EKF2::PublishStatus(const hrt_abstime &timestamp)
_estimator_status_pub.publish(status);
}
void EKF2::PublishStatusFlags(const hrt_abstime &timestamp)
void EKF2::PublishStatusFlags(const hrt_abstime &timestamp, bool force)
{
// publish at ~ 1 Hz (or immediately if filter control status or fault status changes)
bool update = (timestamp >= _last_status_flags_publish + 1_s);
@ -1404,7 +1431,7 @@ void EKF2::PublishStatusFlags(const hrt_abstime &timestamp)
status_flags.cs_mag_hdg = _ekf.control_status_flags().mag_hdg;
status_flags.cs_mag_3d = _ekf.control_status_flags().mag_3D;
status_flags.cs_mag_dec = _ekf.control_status_flags().mag_dec;
status_flags.cs_in_air = _ekf.control_status_flags().in_air;
status_flags.cs_in_air = _ekf.control_status_flags().in_air;
status_flags.cs_wind = _ekf.control_status_flags().wind;
status_flags.cs_baro_hgt = _ekf.control_status_flags().baro_hgt;
status_flags.cs_rng_hgt = _ekf.control_status_flags().rng_hgt;
@ -1464,6 +1491,12 @@ void EKF2::PublishStatusFlags(const hrt_abstime &timestamp)
status_flags.reject_optflow_x = _ekf.innov_check_fail_status_flags().reject_optflow_X;
status_flags.reject_optflow_y = _ekf.innov_check_fail_status_flags().reject_optflow_Y;
status_flags.pre_flt_fail_innov_heading = _pre_flt_fail_innov_heading.get_state();
status_flags.pre_flt_fail_innov_vel_horiz = _pre_flt_fail_innov_vel_horiz.get_state();
status_flags.pre_flt_fail_innov_vel_vert = _pre_flt_fail_innov_vel_vert.get_state();
status_flags.pre_flt_fail_innov_pos_horiz = _pre_flt_fail_innov_pos_horiz.get_state();
status_flags.pre_flt_fail_innov_height = _pre_flt_fail_innov_height.get_state();
status_flags.timestamp = _replay_mode ? timestamp : hrt_absolute_time();
_estimator_status_flags_pub.publish(status_flags);
@ -2085,7 +2118,6 @@ void EKF2::UpdateAccelCalibration(const hrt_abstime &timestamp)
// the EKF is operating in the correct mode and there are no filter faults
if ((_ekf.fault_status().value == 0)
&& !_ekf.accel_bias_inhibited()
&& !_preflt_checker.hasHorizFailed() && !_preflt_checker.hasVertFailed()
&& (_ekf.control_status_flags().baro_hgt || _ekf.control_status_flags().rng_hgt
|| _ekf.control_status_flags().gps_hgt || _ekf.control_status_flags().ev_hgt)
&& !_ekf.warning_event_flags().height_sensor_timeout && !_ekf.warning_event_flags().invalid_accel_bias_cov_reset

20
src/modules/ekf2/EKF2.hpp
View File

@ -42,7 +42,6 @@
#define EKF2_HPP
#include "EKF/ekf.h"
#include "Utility/PreFlightChecker.hpp"
#include "EKF2Selector.hpp"
@ -158,7 +157,7 @@ private:
void PublishSensorBias(const hrt_abstime &timestamp);
void PublishStates(const hrt_abstime &timestamp);
void PublishStatus(const hrt_abstime &timestamp);
void PublishStatusFlags(const hrt_abstime &timestamp);
void PublishStatusFlags(const hrt_abstime &timestamp, bool force = false);
void PublishWindEstimate(const hrt_abstime &timestamp);
void PublishYawEstimatorStatus(const hrt_abstime &timestamp);
@ -209,6 +208,20 @@ private:
uint64_t _start_time_us = 0; ///< system time at EKF start (uSec)
int64_t _last_time_slip_us = 0; ///< Last time slip (uSec)
// Maximum permissible velocity innovation to pass pre-flight checks (m/sec)
static constexpr float kVelocityInnovationTestLimit = 0.5f;
// Maximum permissible position innovation to pass pre-flight checks (m)
static constexpr float kPositionInnovationTestLimit = 1.5f;
// Maximum permissible yaw innovation to pass pre-flight checks when not aiding inertial nav using NE frame observations (rad)
static constexpr float kHeadingInnovationTestLimit = 0.5f;
// preflight failure checks
systemlib::Hysteresis _pre_flt_fail_innov_heading{false};
systemlib::Hysteresis _pre_flt_fail_innov_vel_horiz{false};
systemlib::Hysteresis _pre_flt_fail_innov_vel_vert{false};
systemlib::Hysteresis _pre_flt_fail_innov_pos_horiz{false};
systemlib::Hysteresis _pre_flt_fail_innov_height{false};
perf_counter_t _ecl_ekf_update_perf{perf_alloc(PC_ELAPSED, MODULE_NAME": ECL update")};
perf_counter_t _ecl_ekf_update_full_perf{perf_alloc(PC_ELAPSED, MODULE_NAME": ECL full update")};
perf_counter_t _msg_missed_imu_perf{perf_alloc(PC_COUNT, MODULE_NAME": IMU message missed")};
@ -368,9 +381,6 @@ private:
uORB::PublicationMulti<vehicle_odometry_s> _odometry_pub;
uORB::PublicationMulti<wind_s> _wind_pub;
PreFlightChecker _preflt_checker;
Ekf _ekf;
parameters *_params; ///< pointer to ekf parameter struct (located in _ekf class instance)

45
src/modules/ekf2/Utility/CMakeLists.txt
View File

@ -1,45 +0,0 @@
############################################################################
#
# 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.
#
#############################################################################
px4_add_library(EKF2Utility
PreFlightChecker.cpp
)
target_include_directories(EKF2Utility
PUBLIC
${CMAKE_CURRENT_SOURCE_DIR}
)
target_link_libraries(EKF2Utility PRIVATE mathlib)
px4_add_unit_gtest(SRC PreFlightCheckerTest.cpp LINKLIBS EKF2Utility)

79
src/modules/ekf2/Utility/InnovationLpf.hpp
View File

@ -1,79 +0,0 @@
/****************************************************************************
*
* 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 with input saturation
*/
#include <mathlib/mathlib.h>
class InnovationLpf final
{
public:
InnovationLpf() = default;
~InnovationLpf() = default;
void reset(float val = 0.f) { _x = val; }
/**
* Update the filter with a new value and returns the filtered state
* The new value is constained by the limit set in setSpikeLimit
* @param val new input
* @param alpha normalized weight of the new input
* @param spike_limit the amplitude of the saturation at the input of the filter
* @return filtered output
*/
float update(float val, float alpha, float spike_limit)
{
float val_constrained = math::constrain(val, -spike_limit, spike_limit);
float beta = 1.f - alpha;
_x = beta * _x + alpha * val_constrained;
return _x;
}
/**
* Helper function to compute alpha from dt and the inverse of tau
* @param dt sampling time in seconds
* @param tau_inv inverse of the time constant of the filter
* @return alpha, the normalized weight of a new measurement
*/
static float computeAlphaFromDtAndTauInv(float dt, float tau_inv)
{
return math::constrain(dt * tau_inv, 0.f, 1.f);
}
private:
float _x{}; ///< current state of the filter
};

167
src/modules/ekf2/Utility/PreFlightChecker.cpp
View File

@ -1,167 +0,0 @@
/****************************************************************************
*
* 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.
*
****************************************************************************/
/**
* @file PreFlightCheckHelper.cpp
* Class handling the EKF2 innovation pre flight checks
*/
#include "PreFlightChecker.hpp"
void PreFlightChecker::update(const float dt, const estimator_innovations_s &innov)
{
const float alpha = InnovationLpf::computeAlphaFromDtAndTauInv(dt, _innov_lpf_tau_inv);
_has_heading_failed = preFlightCheckHeadingFailed(innov, alpha);
_has_horiz_vel_failed = preFlightCheckHorizVelFailed(innov, alpha);
_has_vert_vel_failed = preFlightCheckVertVelFailed(innov, alpha);
_has_height_failed = preFlightCheckHeightFailed(innov, alpha);
}
bool PreFlightChecker::preFlightCheckHeadingFailed(const estimator_innovations_s &innov, const float alpha)
{
const float heading_test_limit = selectHeadingTestLimit();
const float heading_innov_spike_lim = 2.0f * heading_test_limit;
const float heading_innov_lpf = _filter_heading_innov.update(innov.heading, alpha, heading_innov_spike_lim);
return checkInnovFailed(heading_innov_lpf, innov.heading, heading_test_limit, heading_innov_spike_lim);
}
float PreFlightChecker::selectHeadingTestLimit()
{
// Select the max allowed heading innovaton depending on whether we are not aiding navigation using
// observations in the NE reference frame and if the vehicle can use GPS course to realign in flight (fixedwing sideslip fusion).
const bool is_ne_aiding = _is_using_gps_aiding || _is_using_ev_pos_aiding;
return (is_ne_aiding && !_can_observe_heading_in_flight)
? _nav_heading_innov_test_lim // more restrictive test limit
: _heading_innov_test_lim; // less restrictive test limit
}
bool PreFlightChecker::preFlightCheckHorizVelFailed(const estimator_innovations_s &innov, const float alpha)
{
bool has_failed = false;
if (_is_using_gps_aiding || _is_using_ev_vel_aiding) {
const Vector2f vel_ne_innov = Vector2f(fmaxf(fabsf(innov.gps_hvel[0]), fabsf(innov.ev_hvel[0])),
fmaxf(fabsf(innov.gps_hvel[1]), fabsf(innov.ev_hvel[1])));
Vector2f vel_ne_innov_lpf;
vel_ne_innov_lpf(0) = _filter_vel_n_innov.update(vel_ne_innov(0), alpha, _vel_innov_spike_lim);
vel_ne_innov_lpf(1) = _filter_vel_n_innov.update(vel_ne_innov(1), alpha, _vel_innov_spike_lim);
has_failed |= checkInnov2DFailed(vel_ne_innov_lpf, vel_ne_innov, _vel_innov_test_lim, _vel_innov_spike_lim);
}
if (_is_using_flow_aiding) {
const Vector2f flow_innov = Vector2f(innov.flow);
Vector2f flow_innov_lpf;
flow_innov_lpf(0) = _filter_flow_x_innov.update(flow_innov(0), alpha, _flow_innov_spike_lim);
flow_innov_lpf(1) = _filter_flow_y_innov.update(flow_innov(1), alpha, _flow_innov_spike_lim);
has_failed |= checkInnov2DFailed(flow_innov_lpf, flow_innov, _flow_innov_test_lim, 5.f * _flow_innov_spike_lim);
}
return has_failed;
}
bool PreFlightChecker::preFlightCheckVertVelFailed(const estimator_innovations_s &innov, const float alpha)
{
const float vel_d_innov = fmaxf(fabsf(innov.gps_vvel), fabs(innov.ev_vvel)); // only temporary solution
const float vel_d_innov_lpf = _filter_vel_d_innov.update(vel_d_innov, alpha, _vel_innov_spike_lim);
return checkInnovFailed(vel_d_innov_lpf, vel_d_innov, _vel_innov_test_lim, _vel_innov_spike_lim);
}
bool PreFlightChecker::preFlightCheckHeightFailed(const estimator_innovations_s &innov, const float alpha)
{
bool has_failed = false;
if (_is_using_baro_hgt_aiding) {
const float baro_hgt_innov_lpf = _filter_baro_hgt_innov.update(innov.baro_vpos, alpha, _hgt_innov_spike_lim);
has_failed |= checkInnovFailed(baro_hgt_innov_lpf, innov.baro_vpos, _hgt_innov_test_lim, _hgt_innov_spike_lim);
}
if (_is_using_gps_hgt_aiding) {
const float gps_hgt_innov_lpf = _filter_gps_hgt_innov.update(innov.gps_vpos, alpha, _hgt_innov_spike_lim);
has_failed |= checkInnovFailed(gps_hgt_innov_lpf, innov.gps_vpos, _hgt_innov_test_lim, _hgt_innov_spike_lim);
}
if (_is_using_rng_hgt_aiding) {
const float rng_hgt_innov_lpf = _filter_rng_hgt_innov.update(innov.rng_vpos, alpha, _hgt_innov_spike_lim);
has_failed |= checkInnovFailed(rng_hgt_innov_lpf, innov.rng_vpos, _hgt_innov_test_lim, _hgt_innov_spike_lim);
}
if (_is_using_ev_hgt_aiding) {
const float ev_hgt_innov_lpf = _filter_ev_hgt_innov.update(innov.ev_vpos, alpha, _hgt_innov_spike_lim);
has_failed |= checkInnovFailed(ev_hgt_innov_lpf, innov.ev_vpos, _hgt_innov_test_lim, _hgt_innov_spike_lim);
}
return has_failed;
}
bool PreFlightChecker::checkInnovFailed(const float innov_lpf, const float innov, const float test_limit,
const float spike_limit)
{
return fabsf(innov_lpf) > test_limit || fabsf(innov) > spike_limit;
}
bool PreFlightChecker::checkInnov2DFailed(const Vector2f &innov_lpf, const Vector2f &innov, const float test_limit,
const float spike_limit)
{
return innov_lpf.norm_squared() > sq(test_limit)
|| innov.norm_squared() > sq(spike_limit);
}
void PreFlightChecker::reset()
{
_is_using_gps_aiding = false;
_is_using_flow_aiding = false;
_is_using_ev_pos_aiding = false;
_is_using_ev_vel_aiding = false;
_is_using_baro_hgt_aiding = false;
_is_using_gps_hgt_aiding = false;
_is_using_rng_hgt_aiding = false;
_is_using_ev_hgt_aiding = false;
_has_heading_failed = false;
_has_horiz_vel_failed = false;
_has_vert_vel_failed = false;
_has_height_failed = false;
_filter_vel_n_innov.reset();
_filter_vel_e_innov.reset();
_filter_vel_d_innov.reset();
_filter_baro_hgt_innov.reset();
_filter_gps_hgt_innov.reset();
_filter_rng_hgt_innov.reset();
_filter_ev_hgt_innov.reset();
_filter_heading_innov.reset();
_filter_flow_x_innov.reset();
_filter_flow_y_innov.reset();
}

195
src/modules/ekf2/Utility/PreFlightChecker.hpp
View File

@ -1,195 +0,0 @@
/****************************************************************************
*
* 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.
*
****************************************************************************/
/**
* @file PreFlightChecker.hpp
* Class handling the EKF2 innovation pre flight checks
*
* First call the update(...) function and then get the results
* using the hasXxxFailed() getters
*/
#ifndef EKF2_PREFLIGHTCHECKER_HPP
#define EKF2_PREFLIGHTCHECKER_HPP
#include <uORB/topics/vehicle_status.h>
#include <uORB/topics/estimator_innovations.h>
#include <matrix/matrix/math.hpp>
#include "InnovationLpf.hpp"
using matrix::Vector2f;
class PreFlightChecker
{
public:
PreFlightChecker() = default;
~PreFlightChecker() = default;
/*
* Reset all the internal states of the class to their default value
*/
void reset();
/*
* Update the internal states
* @param dt the sampling time
* @param innov the ekf2_innovation_s struct containing the current innovations
*/
void update(float dt, const estimator_innovations_s &innov);
/*
* If set to true, the checker will use a less conservative heading innovation check
*/
void setVehicleCanObserveHeadingInFlight(bool val) { _can_observe_heading_in_flight = val; }
void setUsingGpsAiding(bool val) { _is_using_gps_aiding = val; }
void setUsingFlowAiding(bool val) { _is_using_flow_aiding = val; }
void setUsingEvPosAiding(bool val) { _is_using_ev_pos_aiding = val; }
void setUsingEvVelAiding(bool val) { _is_using_ev_vel_aiding = val; }
void setUsingBaroHgtAiding(bool val) { _is_using_baro_hgt_aiding = val; }
void setUsingGpsHgtAiding(bool val) { _is_using_gps_hgt_aiding = val; }
void setUsingRngHgtAiding(bool val) { _is_using_rng_hgt_aiding = val; }
void setUsingEvHgtAiding(bool val) { _is_using_ev_hgt_aiding = val; }
bool hasHeadingFailed() const { return _has_heading_failed; }
bool hasHorizVelFailed() const { return _has_horiz_vel_failed; }
bool hasVertVelFailed() const { return _has_vert_vel_failed; }
bool hasHeightFailed() const { return _has_height_failed; }
/*
* Overall state of the pre fligh checks
* @return true if any of the check failed
*/
bool hasFailed() const { return hasHorizFailed() || hasVertFailed(); }
/*
* Horizontal checks overall result
* @return true if one of the horizontal checks failed
*/
bool hasHorizFailed() const { return _has_heading_failed || _has_horiz_vel_failed; }
/*
* Vertical checks overall result
* @return true if one of the vertical checks failed
*/
bool hasVertFailed() const { return _has_vert_vel_failed || _has_height_failed; }
/*
* Check if the innovation fails the test
* To pass the test, the following conditions should be true:
* innov_lpf <= test_limit
* innov <= spike_limit
* @param innov_lpf the low-pass filtered innovation
* @param innov the current unfiltered innovation
* @param test_limit the magnitude test limit for innov_lpf
* @param spike_limit the magnitude test limit for innov
* @return true if the check failed the test, false otherwise
*/
static bool checkInnovFailed(float innov_lpf, float innov, float test_limit, float spike_limit);
/*
* Check if the a innovation of a 2D vector fails the test
* To pass the test, the following conditions should be true:
* innov_lpf <= test_limit
* innov <= spike_limit
* @param innov_lpf the low-pass filtered innovation
* @param innov the current unfiltered innovation
* @param test_limit the magnitude test limit for innov_lpf
* @param spike_limit the magnitude test limit for innov
* @return true if the check failed the test, false otherwise
*/
static bool checkInnov2DFailed(const Vector2f &innov_lpf, const Vector2f &innov, float test_limit, float spike_limit);
static constexpr float sq(float var) { return var * var; }
private:
bool preFlightCheckHeadingFailed(const estimator_innovations_s &innov, float alpha);
float selectHeadingTestLimit();
bool preFlightCheckHorizVelFailed(const estimator_innovations_s &innov, float alpha);
bool preFlightCheckVertVelFailed(const estimator_innovations_s &innov, float alpha);
bool preFlightCheckHeightFailed(const estimator_innovations_s &innov, float alpha);
bool _has_heading_failed{};
bool _has_horiz_vel_failed{};
bool _has_vert_vel_failed{};
bool _has_height_failed{};
bool _can_observe_heading_in_flight{};
bool _is_using_gps_aiding{};
bool _is_using_flow_aiding{};
bool _is_using_ev_pos_aiding{};
bool _is_using_ev_vel_aiding{};
bool _is_using_baro_hgt_aiding{};
bool _is_using_gps_hgt_aiding{};
bool _is_using_rng_hgt_aiding{};
bool _is_using_ev_hgt_aiding{};
// Low-pass filters for innovation pre-flight checks
InnovationLpf _filter_vel_n_innov; ///< Preflight low pass filter N axis velocity innovations (m/sec)
InnovationLpf _filter_vel_e_innov; ///< Preflight low pass filter E axis velocity innovations (m/sec)
InnovationLpf _filter_vel_d_innov; ///< Preflight low pass filter D axis velocity innovations (m/sec)
InnovationLpf _filter_heading_innov; ///< Preflight low pass filter heading innovation magntitude (rad)
InnovationLpf _filter_flow_x_innov; ///< Preflight low pass filter optical flow innovation (rad)
InnovationLpf _filter_flow_y_innov; ///< Preflight low pass filter optical flow innovation (rad)
// Preflight low pass filter height innovation (m)
InnovationLpf _filter_baro_hgt_innov;
InnovationLpf _filter_gps_hgt_innov;
InnovationLpf _filter_rng_hgt_innov;
InnovationLpf _filter_ev_hgt_innov;
// Preflight low pass filter time constant inverse (1/sec)
static constexpr float _innov_lpf_tau_inv = 0.2f;
// Maximum permissible velocity innovation to pass pre-flight checks (m/sec)
static constexpr float _vel_innov_test_lim = 0.5f;
// Maximum permissible height innovation to pass pre-flight checks (m)
static constexpr float _hgt_innov_test_lim = 1.5f;
// Maximum permissible yaw innovation to pass pre-flight checks when aiding inertial nav using NE frame observations (rad)
static constexpr float _nav_heading_innov_test_lim = 0.25f;
// Maximum permissible yaw innovation to pass pre-flight checks when not aiding inertial nav using NE frame observations (rad)
static constexpr float _heading_innov_test_lim = 0.52f;
// Maximum permissible flow innovation to pass pre-flight checks
static constexpr float _flow_innov_test_lim = 0.25f;
// Preflight velocity innovation spike limit (m/sec)
static constexpr float _vel_innov_spike_lim = 2.0f * _vel_innov_test_lim;
// Preflight position innovation spike limit (m)
static constexpr float _hgt_innov_spike_lim = 2.0f * _hgt_innov_test_lim;
// Preflight flow innovation spike limit (rad)
static constexpr float _flow_innov_spike_lim = 2.0f * _flow_innov_test_lim;
};
#endif // !EKF2_PREFLIGHTCHECKER_HPP

97
src/modules/ekf2/Utility/PreFlightCheckerTest.cpp
View File

@ -1,97 +0,0 @@
/****************************************************************************
*
* 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.
*
****************************************************************************/
/**
* Test code for PreFlightChecker class
* Run this test only using make tests TESTFILTER=PreFlightChecker
*/
#include <gtest/gtest.h>
#include "PreFlightChecker.hpp"
class PreFlightCheckerTest : public ::testing::Test
{
};
TEST_F(PreFlightCheckerTest, testInnovFailed)
{
const float test_limit = 1.0; ///< is the limit for innovation_lpf
const float spike_limit = 2.f * test_limit; ///< is the limit for innovation_lpf
const float innovations[9] = {0.0, 1.5, 2.5, -1.5, -2.5, 1.5, -1.5, -2.5, -2.5};
const float innovations_lpf[9] = {0.0, 0.9, 0.9, -0.9, -0.9, 1.1, -1.1, -1.1, 1.1};
const bool expected_result[9] = {false, false, true, false, true, true, true, true, true};
for (int i = 0; i < 9; i++) {
EXPECT_EQ(PreFlightChecker::checkInnovFailed(innovations_lpf[i], innovations[i], test_limit, spike_limit),
expected_result[i]);
}
// Smaller test limit, all the checks should fail except the first
EXPECT_FALSE(PreFlightChecker::checkInnovFailed(innovations_lpf[0], innovations[0], 0.0, 0.0));
for (int i = 1; i < 9; i++) {
EXPECT_TRUE(PreFlightChecker::checkInnovFailed(innovations_lpf[i], innovations[i], 0.0, 0.0));
}
// Larger test limit, none of the checks should fail
for (int i = 0; i < 9; i++) {
EXPECT_FALSE(PreFlightChecker::checkInnovFailed(innovations_lpf[i], innovations[i], 2.0, 4.0));
}
}
TEST_F(PreFlightCheckerTest, testInnov2dFailed)
{
const float test_limit = 1.0;
const float spike_limit = 2.0;
Vector2f innovations[4] = {{0.0, 0.0}, {0.0, 0.0}, {0.0, -2.5}, {1.5, -1.5}};
Vector2f innovations_lpf[4] = {{0.0, 0.0}, {1.1, 0.0}, {0.5, 0.5}, {1.0, -1.0}};
const bool expected_result[4] = {false, true, true, true};
for (int i = 0; i < 4; i++) {
EXPECT_EQ(PreFlightChecker::checkInnov2DFailed(innovations_lpf[i], innovations[i], test_limit, spike_limit),
expected_result[i]);
}
// Smaller test limit, all the checks should fail except the first
EXPECT_FALSE(PreFlightChecker::checkInnov2DFailed(innovations[0], innovations_lpf[0], 0.0, 0.0));
for (int i = 1; i < 4; i++) {
EXPECT_TRUE(PreFlightChecker::checkInnov2DFailed(innovations_lpf[i], innovations[i], 0.0, 0.0));
}
// Larger test limit, none of the checks should fail
for (int i = 0; i < 4; i++) {
EXPECT_FALSE(PreFlightChecker::checkInnov2DFailed(innovations_lpf[i], innovations[i], 1.42, 2.84));
}
}

4
src/modules/simulation/simulator_ignition_bridge/SimulatorIgnitionBridge.cpp
View File

@ -76,14 +76,14 @@ int SimulatorIgnitionBridge::init()
if (!_model_pose.empty()) {
PX4_INFO("Requested Model Position: %s", _model_pose.c_str());
std::vector<float> model_pose_v;
std::vector<double> model_pose_v;
std::stringstream ss(_model_pose);
while (ss.good()) {
std::string substr;
std::getline(ss, substr, ',');
model_pose_v.push_back(std::stof(substr));
model_pose_v.push_back(std::stod(substr));
}
while (model_pose_v.size() < 6) {