Browse Source
- ekf2 can now run in multi-instance mode (currently up to 9 instances)
- in multi mode all estimates are published to alternate topics (eg estimator_attitude instead of vehicle_attitude)
- new ekf2 selector runs in multi-instance mode to monitor and compare all instances, selecting a primary (eg N x estimator_attitude => vehicle_attitude)
- sensors module accel & gyro inconsistency checks are now relative to the mean of all instances, rather than the current primary (when active ekf2 selector is responsible for choosing primary accel & gyro)
- existing consumers of estimator_status must check estimator_selector_status to select current primary instance status
- ekf2 single instance mode is still fully supported and the default
Co-authored-by: Paul Riseborough <gncsolns@gmail.com>
release/1.12
Daniel Agar
4 years ago
committed by
GitHub
56 changed files with 1752 additions and 300 deletions
@ -0,0 +1,22 @@
@@ -0,0 +1,22 @@
|
||||
uint64 timestamp # time since system start (microseconds) |
||||
|
||||
uint8 primary_instance |
||||
|
||||
uint8 instances_available |
||||
|
||||
uint32 instance_changed_count |
||||
uint64 last_instance_change |
||||
|
||||
uint32 accel_device_id |
||||
uint32 baro_device_id |
||||
uint32 gyro_device_id |
||||
uint32 mag_device_id |
||||
|
||||
float32[9] combined_test_ratio |
||||
float32[9] relative_test_ratio |
||||
bool[9] healthy |
||||
|
||||
float32[4] accumulated_gyro_error |
||||
float32[4] accumulated_accel_error |
||||
bool gyro_fault_detected |
||||
bool accel_fault_detected |
||||
@ -0,0 +1,628 @@
@@ -0,0 +1,628 @@
|
||||
/****************************************************************************
|
||||
* |
||||
* Copyright (c) 2020 PX4 Development Team. All rights reserved. |
||||
* |
||||
* Redistribution and use in source and binary forms, with or without |
||||
* modification, are permitted provided that the following conditions |
||||
* are met: |
||||
* |
||||
* 1. Redistributions of source code must retain the above copyright |
||||
* notice, this list of conditions and the following disclaimer. |
||||
* 2. Redistributions in binary form must reproduce the above copyright |
||||
* notice, this list of conditions and the following disclaimer in |
||||
* the documentation and/or other materials provided with the |
||||
* distribution. |
||||
* 3. Neither the name PX4 nor the names of its contributors may be |
||||
* used to endorse or promote products derived from this software |
||||
* without specific prior written permission. |
||||
* |
||||
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS |
||||
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT |
||||
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS |
||||
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE |
||||
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, |
||||
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, |
||||
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS |
||||
* OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED |
||||
* AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT |
||||
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN |
||||
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE |
||||
* POSSIBILITY OF SUCH DAMAGE. |
||||
* |
||||
****************************************************************************/ |
||||
|
||||
#include "EKF2Selector.hpp" |
||||
|
||||
using namespace time_literals; |
||||
using matrix::Quatf; |
||||
using matrix::Vector2f; |
||||
using math::constrain; |
||||
using math::max; |
||||
using math::radians; |
||||
|
||||
EKF2Selector::EKF2Selector() : |
||||
ModuleParams(nullptr), |
||||
ScheduledWorkItem("ekf2_selector", px4::wq_configurations::nav_and_controllers) |
||||
{ |
||||
} |
||||
|
||||
EKF2Selector::~EKF2Selector() |
||||
{ |
||||
Stop(); |
||||
|
||||
px4_lockstep_unregister_component(_lockstep_component); |
||||
} |
||||
|
||||
bool EKF2Selector::Start() |
||||
{ |
||||
// default to first instance
|
||||
_selected_instance = 0; |
||||
_instance[0].estimator_status_sub.registerCallback(); |
||||
_instance[0].estimator_attitude_sub.registerCallback(); |
||||
|
||||
return true; |
||||
} |
||||
|
||||
void EKF2Selector::Stop() |
||||
{ |
||||
for (int i = 0; i < EKF2_MAX_INSTANCES; i++) { |
||||
_instance[i].estimator_attitude_sub.unregisterCallback(); |
||||
_instance[i].estimator_status_sub.unregisterCallback(); |
||||
} |
||||
|
||||
ScheduleClear(); |
||||
} |
||||
|
||||
void EKF2Selector::SelectInstance(uint8_t ekf_instance) |
||||
{ |
||||
if (ekf_instance != _selected_instance) { |
||||
|
||||
// update sensor_selection immediately
|
||||
sensor_selection_s sensor_selection{}; |
||||
sensor_selection.accel_device_id = _instance[ekf_instance].estimator_status.accel_device_id; |
||||
sensor_selection.gyro_device_id = _instance[ekf_instance].estimator_status.gyro_device_id; |
||||
sensor_selection.timestamp = hrt_absolute_time(); |
||||
_sensor_selection_pub.publish(sensor_selection); |
||||
|
||||
if (_selected_instance != INVALID_INSTANCE) { |
||||
// switch callback registration
|
||||
_instance[_selected_instance].estimator_attitude_sub.unregisterCallback(); |
||||
_instance[_selected_instance].estimator_status_sub.unregisterCallback(); |
||||
|
||||
if (!_instance[_selected_instance].healthy) { |
||||
PX4_WARN("primary EKF changed %d (unhealthy) -> %d", _selected_instance, ekf_instance); |
||||
} |
||||
} |
||||
|
||||
_instance[ekf_instance].estimator_attitude_sub.registerCallback(); |
||||
_instance[ekf_instance].estimator_status_sub.registerCallback(); |
||||
|
||||
_selected_instance = ekf_instance; |
||||
_instance_changed_count++; |
||||
_last_instance_change = sensor_selection.timestamp; |
||||
_instance[ekf_instance].time_last_selected = _last_instance_change; |
||||
|
||||
// reset all relative test ratios
|
||||
for (auto &inst : _instance) { |
||||
inst.relative_test_ratio = 0; |
||||
} |
||||
|
||||
// publish new data immediately with resets
|
||||
PublishVehicleAttitude(true); |
||||
PublishVehicleLocalPosition(true); |
||||
PublishVehicleGlobalPosition(true); |
||||
} |
||||
} |
||||
|
||||
bool EKF2Selector::UpdateErrorScores() |
||||
{ |
||||
bool updated = false; |
||||
|
||||
// first check imu inconsistencies
|
||||
_gyro_fault_detected = false; |
||||
uint32_t faulty_gyro_id = 0; |
||||
_accel_fault_detected = false; |
||||
uint32_t faulty_accel_id = 0; |
||||
|
||||
if (_sensors_status_imu.updated()) { |
||||
sensors_status_imu_s sensors_status_imu; |
||||
|
||||
if (_sensors_status_imu.copy(&sensors_status_imu)) { |
||||
const float angle_rate_threshold = radians(_param_ekf2_sel_imu_angle_rate.get()); |
||||
const float angle_threshold = radians(_param_ekf2_sel_imu_angle.get()); |
||||
const float accel_threshold = _param_ekf2_sel_imu_accel.get(); |
||||
const float velocity_threshold = _param_ekf2_sel_imu_velocity.get(); |
||||
const float time_step_s = constrain((sensors_status_imu.timestamp - _last_update_us) * 1e-6f, 0.f, 0.02f); |
||||
_last_update_us = sensors_status_imu.timestamp; |
||||
|
||||
uint8_t n_gyros = 0; |
||||
uint8_t n_accels = 0; |
||||
uint8_t n_gyro_exceedances = 0; |
||||
uint8_t n_accel_exceedances = 0; |
||||
float largest_accumulated_gyro_error = 0.0f; |
||||
float largest_accumulated_accel_error = 0.0f; |
||||
uint8_t largest_gyro_error_index = 0; |
||||
uint8_t largest_accel_error_index = 0; |
||||
|
||||
for (unsigned i = 0; i < IMU_STATUS_SIZE; i++) { |
||||
// check for gyros with excessive difference to mean using accumulated error
|
||||
if (sensors_status_imu.gyro_device_ids[i] != 0) { |
||||
n_gyros++; |
||||
_accumulated_gyro_error[i] += (sensors_status_imu.gyro_inconsistency_rad_s[i] - angle_rate_threshold) * time_step_s; |
||||
_accumulated_gyro_error[i] = fmaxf(_accumulated_gyro_error[i], 0.f); |
||||
|
||||
if (_accumulated_gyro_error[i] > angle_threshold) { |
||||
n_gyro_exceedances++; |
||||
} |
||||
|
||||
if (_accumulated_gyro_error[i] > largest_accumulated_gyro_error) { |
||||
largest_accumulated_gyro_error = _accumulated_gyro_error[i]; |
||||
largest_gyro_error_index = i; |
||||
} |
||||
|
||||
} else { |
||||
// no sensor
|
||||
_accumulated_gyro_error[i] = 0.f; |
||||
} |
||||
|
||||
// check for accelerometers with excessive difference to mean using accumulated error
|
||||
if (sensors_status_imu.accel_device_ids[i] != 0) { |
||||
n_accels++; |
||||
_accumulated_accel_error[i] += (sensors_status_imu.accel_inconsistency_m_s_s[i] - accel_threshold) * time_step_s; |
||||
_accumulated_accel_error[i] = fmaxf(_accumulated_accel_error[i], 0.f); |
||||
|
||||
if (_accumulated_accel_error[i] > velocity_threshold) { |
||||
n_accel_exceedances++; |
||||
} |
||||
|
||||
if (_accumulated_accel_error[i] > largest_accumulated_accel_error) { |
||||
largest_accumulated_accel_error = _accumulated_accel_error[i]; |
||||
largest_accel_error_index = i; |
||||
} |
||||
|
||||
} else { |
||||
// no sensor
|
||||
_accumulated_accel_error[i] = 0.f; |
||||
} |
||||
} |
||||
|
||||
if (n_gyro_exceedances > 0) { |
||||
if (n_gyros >= 3) { |
||||
// If there are 3 or more sensors, the one with the largest accumulated error is faulty
|
||||
_gyro_fault_detected = true; |
||||
faulty_gyro_id = _instance[largest_gyro_error_index].estimator_status.gyro_device_id; |
||||
|
||||
} else if (n_gyros == 2) { |
||||
// A fault is present, but the faulty sensor identity cannot be determined
|
||||
_gyro_fault_detected = true; |
||||
} |
||||
} |
||||
|
||||
if (n_accel_exceedances > 0) { |
||||
if (n_accels >= 3) { |
||||
// If there are 3 or more sensors, the one with the largest accumulated error is faulty
|
||||
_accel_fault_detected = true; |
||||
faulty_accel_id = _instance[largest_accel_error_index].estimator_status.accel_device_id; |
||||
|
||||
} else if (n_accels == 2) { |
||||
// A fault is present, but the faulty sensor identity cannot be determined
|
||||
_accel_fault_detected = true; |
||||
} |
||||
} |
||||
} |
||||
} |
||||
|
||||
|
||||
bool primary_updated = false; |
||||
|
||||
// calculate individual error scores
|
||||
for (uint8_t i = 0; i < EKF2_MAX_INSTANCES; i++) { |
||||
const bool prev_healthy = _instance[i].healthy; |
||||
|
||||
if (_instance[i].estimator_status_sub.update(&_instance[i].estimator_status)) { |
||||
|
||||
if ((i + 1) > _available_instances) { |
||||
_available_instances = i + 1; |
||||
updated = true; |
||||
} |
||||
|
||||
if (i == _selected_instance) { |
||||
primary_updated = true; |
||||
} |
||||
|
||||
const estimator_status_s &status = _instance[i].estimator_status; |
||||
|
||||
const bool tilt_align = status.control_mode_flags & (1 << estimator_status_s::CS_TILT_ALIGN); |
||||
const bool yaw_align = status.control_mode_flags & (1 << estimator_status_s::CS_YAW_ALIGN); |
||||
|
||||
_instance[i].combined_test_ratio = 0.0f; |
||||
|
||||
if (tilt_align && yaw_align) { |
||||
_instance[i].combined_test_ratio = fmaxf(_instance[i].combined_test_ratio, |
||||
0.5f * (status.vel_test_ratio + status.pos_test_ratio)); |
||||
_instance[i].combined_test_ratio = fmaxf(_instance[i].combined_test_ratio, status.hgt_test_ratio); |
||||
} |
||||
|
||||
_instance[i].healthy = tilt_align && yaw_align && (status.filter_fault_flags == 0); |
||||
|
||||
} else if (hrt_elapsed_time(&_instance[i].estimator_status.timestamp) > 20_ms) { |
||||
_instance[i].healthy = false; |
||||
} |
||||
|
||||
// if the gyro used by the EKF is faulty, declare the EKF unhealthy without delay
|
||||
if (_gyro_fault_detected && _instance[i].estimator_status.gyro_device_id == faulty_gyro_id) { |
||||
_instance[i].healthy = false; |
||||
} |
||||
|
||||
// if the accelerometer used by the EKF is faulty, declare the EKF unhealthy without delay
|
||||
if (_accel_fault_detected && _instance[i].estimator_status.accel_device_id == faulty_accel_id) { |
||||
_instance[i].healthy = false; |
||||
} |
||||
|
||||
if (prev_healthy != _instance[i].healthy) { |
||||
updated = true; |
||||
} |
||||
} |
||||
|
||||
// update relative test ratios if primary has updated
|
||||
if (primary_updated) { |
||||
for (uint8_t i = 0; i < _available_instances; i++) { |
||||
if (i != _selected_instance) { |
||||
|
||||
const float error_delta = _instance[i].combined_test_ratio - _instance[_selected_instance].combined_test_ratio; |
||||
|
||||
// reduce error only if its better than the primary instance by at least EKF2_SEL_ERR_RED to prevent unnecessary selection changes
|
||||
const float threshold = _gyro_fault_detected ? fmaxf(_param_ekf2_sel_err_red.get(), 0.05f) : 0.0f; |
||||
|
||||
if (error_delta > 0 || error_delta < -threshold) { |
||||
_instance[i].relative_test_ratio += error_delta; |
||||
_instance[i].relative_test_ratio = constrain(_instance[i].relative_test_ratio, -_rel_err_score_lim, _rel_err_score_lim); |
||||
} |
||||
} |
||||
} |
||||
} |
||||
|
||||
return (primary_updated || updated); |
||||
} |
||||
|
||||
void EKF2Selector::PublishVehicleAttitude(bool reset) |
||||
{ |
||||
vehicle_attitude_s attitude; |
||||
|
||||
if (_instance[_selected_instance].estimator_attitude_sub.copy(&attitude)) { |
||||
if (reset) { |
||||
// on reset compute deltas from last published data
|
||||
++_quat_reset_counter; |
||||
|
||||
_delta_q_reset = (Quatf(attitude.q) * Quatf(_attitude_last.q).inversed()).normalized(); |
||||
|
||||
// ensure monotonically increasing timestamp_sample through reset
|
||||
attitude.timestamp_sample = max(attitude.timestamp_sample, _attitude_last.timestamp_sample); |
||||
|
||||
} else { |
||||
// otherwise propogate deltas from estimator data while maintaining the overall reset counts
|
||||
if (attitude.quat_reset_counter > _attitude_last.quat_reset_counter) { |
||||
++_quat_reset_counter; |
||||
_delta_q_reset = Quatf{attitude.delta_q_reset}; |
||||
} |
||||
} |
||||
|
||||
// save last primary estimator_attitude
|
||||
_attitude_last = attitude; |
||||
|
||||
// republish with total reset count and current timestamp
|
||||
attitude.quat_reset_counter = _quat_reset_counter; |
||||
_delta_q_reset.copyTo(attitude.delta_q_reset); |
||||
|
||||
attitude.timestamp = hrt_absolute_time(); |
||||
_vehicle_attitude_pub.publish(attitude); |
||||
} |
||||
} |
||||
|
||||
void EKF2Selector::PublishVehicleLocalPosition(bool reset) |
||||
{ |
||||
// vehicle_local_position
|
||||
vehicle_local_position_s local_position; |
||||
|
||||
if (_instance[_selected_instance].estimator_local_position_sub.copy(&local_position)) { |
||||
if (reset) { |
||||
// on reset compute deltas from last published data
|
||||
++_xy_reset_counter; |
||||
++_z_reset_counter; |
||||
++_vxy_reset_counter; |
||||
++_vz_reset_counter; |
||||
++_heading_reset_counter; |
||||
|
||||
_delta_xy_reset = Vector2f{local_position.x, local_position.y} - Vector2f{_local_position_last.x, _local_position_last.y}; |
||||
_delta_z_reset = local_position.z - _local_position_last.z; |
||||
_delta_vxy_reset = Vector2f{local_position.vx, local_position.vy} - Vector2f{_local_position_last.vx, _local_position_last.vy}; |
||||
_delta_vz_reset = local_position.vz - _local_position_last.vz; |
||||
_delta_heading_reset = matrix::wrap_2pi(local_position.heading - _local_position_last.heading); |
||||
|
||||
// ensure monotonically increasing timestamp_sample through reset
|
||||
local_position.timestamp_sample = max(local_position.timestamp_sample, _local_position_last.timestamp_sample); |
||||
|
||||
} else { |
||||
// otherwise propogate deltas from estimator data while maintaining the overall reset counts
|
||||
|
||||
// XY reset
|
||||
if (local_position.xy_reset_counter > _local_position_last.xy_reset_counter) { |
||||
++_xy_reset_counter; |
||||
_delta_xy_reset = Vector2f{local_position.delta_xy}; |
||||
} |
||||
|
||||
// Z reset
|
||||
if (local_position.z_reset_counter > _local_position_last.z_reset_counter) { |
||||
++_z_reset_counter; |
||||
_delta_z_reset = local_position.delta_z; |
||||
} |
||||
|
||||
// VXY reset
|
||||
if (local_position.vxy_reset_counter > _local_position_last.vxy_reset_counter) { |
||||
++_vxy_reset_counter; |
||||
_delta_vxy_reset = Vector2f{local_position.delta_vxy}; |
||||
} |
||||
|
||||
// VZ reset
|
||||
if (local_position.vz_reset_counter > _local_position_last.vz_reset_counter) { |
||||
++_vz_reset_counter; |
||||
_delta_z_reset = local_position.delta_vz; |
||||
} |
||||
|
||||
// heading reset
|
||||
if (local_position.heading_reset_counter > _local_position_last.heading_reset_counter) { |
||||
++_heading_reset_counter; |
||||
_delta_heading_reset = local_position.delta_heading; |
||||
} |
||||
} |
||||
|
||||
// save last primary estimator_local_position
|
||||
_local_position_last = local_position; |
||||
|
||||
// publish estimator's local position for system (vehicle_local_position) unless it's stale
|
||||
if (local_position.timestamp >= _instance[_selected_instance].estimator_status.timestamp_sample) { |
||||
// republish with total reset count and current timestamp
|
||||
local_position.xy_reset_counter = _xy_reset_counter; |
||||
local_position.z_reset_counter = _z_reset_counter; |
||||
local_position.vxy_reset_counter = _vxy_reset_counter; |
||||
local_position.vz_reset_counter = _vz_reset_counter; |
||||
local_position.heading_reset_counter = _heading_reset_counter; |
||||
|
||||
_delta_xy_reset.copyTo(local_position.delta_xy); |
||||
local_position.delta_z = _delta_z_reset; |
||||
_delta_vxy_reset.copyTo(local_position.delta_vxy); |
||||
local_position.delta_vz = _delta_vz_reset; |
||||
local_position.delta_heading = _delta_heading_reset; |
||||
|
||||
local_position.timestamp = hrt_absolute_time(); |
||||
_vehicle_local_position_pub.publish(local_position); |
||||
} |
||||
} |
||||
} |
||||
|
||||
void EKF2Selector::PublishVehicleGlobalPosition(bool reset) |
||||
{ |
||||
vehicle_global_position_s global_position; |
||||
|
||||
if (_instance[_selected_instance].estimator_global_position_sub.copy(&global_position)) { |
||||
if (reset) { |
||||
// on reset compute deltas from last published data
|
||||
++_lat_lon_reset_counter; |
||||
++_alt_reset_counter; |
||||
|
||||
_delta_lat_reset = global_position.lat - _global_position_last.lat; |
||||
_delta_lon_reset = global_position.lon - _global_position_last.lon; |
||||
_delta_alt_reset = global_position.delta_alt - _global_position_last.delta_alt; |
||||
|
||||
// ensure monotonically increasing timestamp_sample through reset
|
||||
global_position.timestamp_sample = max(global_position.timestamp_sample, _global_position_last.timestamp_sample); |
||||
|
||||
} else { |
||||
// otherwise propogate deltas from estimator data while maintaining the overall reset counts
|
||||
|
||||
// lat/lon reset
|
||||
if (global_position.lat_lon_reset_counter > _global_position_last.lat_lon_reset_counter) { |
||||
++_lat_lon_reset_counter; |
||||
|
||||
// TODO: delta latitude/longitude
|
||||
//_delta_lat_reset = global_position.delta_lat;
|
||||
//_delta_lon_reset = global_position.delta_lon;
|
||||
} |
||||
|
||||
// alt reset
|
||||
if (global_position.alt_reset_counter > _global_position_last.alt_reset_counter) { |
||||
++_alt_reset_counter; |
||||
_delta_alt_reset = global_position.delta_alt; |
||||
} |
||||
} |
||||
|
||||
// save last primary estimator_global_position
|
||||
_global_position_last = global_position; |
||||
|
||||
// publish estimator's global position for system (vehicle_global_position) unless it's stale
|
||||
if (global_position.timestamp >= _instance[_selected_instance].estimator_status.timestamp_sample) { |
||||
// republish with total reset count and current timestamp
|
||||
global_position.lat_lon_reset_counter = _lat_lon_reset_counter; |
||||
global_position.alt_reset_counter = _alt_reset_counter; |
||||
global_position.delta_alt = _delta_alt_reset; |
||||
|
||||
global_position.timestamp = hrt_absolute_time(); |
||||
_vehicle_global_position_pub.publish(global_position); |
||||
} |
||||
} |
||||
} |
||||
|
||||
void EKF2Selector::Run() |
||||
{ |
||||
// re-schedule as backup timeout
|
||||
ScheduleDelayed(10_ms); |
||||
|
||||
// check for parameter updates
|
||||
if (_parameter_update_sub.updated()) { |
||||
// clear update
|
||||
parameter_update_s pupdate; |
||||
_parameter_update_sub.copy(&pupdate); |
||||
|
||||
// update parameters from storage
|
||||
updateParams(); |
||||
} |
||||
|
||||
// update combined test ratio for all estimators
|
||||
const bool updated = UpdateErrorScores(); |
||||
|
||||
if (updated) { |
||||
bool lower_error_available = false; |
||||
float alternative_error = 0.f; // looking for instances that have error lower than the current primary
|
||||
uint8_t best_ekf_instance = _selected_instance; |
||||
|
||||
// loop through all available instances to find if an alternative is available
|
||||
for (int i = 0; i < _available_instances; i++) { |
||||
// Use an alternative instance if -
|
||||
// (healthy and has updated recently)
|
||||
// AND
|
||||
// (has relative error less than selected instance and has not been the selected instance for at least 10 seconds
|
||||
// OR
|
||||
// selected instance has stopped updating
|
||||
if (_instance[i].healthy && (i != _selected_instance)) { |
||||
const float relative_error = _instance[i].relative_test_ratio; |
||||
|
||||
if (relative_error < alternative_error) { |
||||
|
||||
alternative_error = relative_error; |
||||
best_ekf_instance = i; |
||||
|
||||
// relative error less than selected instance and has not been the selected instance for at least 10 seconds
|
||||
if ((relative_error <= -_rel_err_thresh) && hrt_elapsed_time(&_instance[i].time_last_selected) > 10_s) { |
||||
lower_error_available = true; |
||||
} |
||||
} |
||||
} |
||||
} |
||||
|
||||
if ((_selected_instance == INVALID_INSTANCE) |
||||
|| (!_instance[_selected_instance].healthy && (best_ekf_instance == _selected_instance))) { |
||||
|
||||
// force initial selection
|
||||
uint8_t best_instance = _selected_instance; |
||||
float best_test_ratio = FLT_MAX; |
||||
|
||||
for (int i = 0; i < _available_instances; i++) { |
||||
if (_instance[i].healthy) { |
||||
const float test_ratio = _instance[i].combined_test_ratio; |
||||
|
||||
if ((test_ratio > 0) && (test_ratio < best_test_ratio)) { |
||||
best_instance = i; |
||||
best_test_ratio = test_ratio; |
||||
} |
||||
} |
||||
} |
||||
|
||||
SelectInstance(best_instance); |
||||
|
||||
} else if (best_ekf_instance != _selected_instance) { |
||||
// if this instance has a significantly lower relative error to the active primary, we consider it as a
|
||||
// better instance and would like to switch to it even if the current primary is healthy
|
||||
// switch immediately if the current selected is no longer healthy
|
||||
if ((lower_error_available && hrt_elapsed_time(&_last_instance_change) > 10_s) |
||||
|| !_instance[_selected_instance].healthy) { |
||||
|
||||
SelectInstance(best_ekf_instance); |
||||
} |
||||
} |
||||
|
||||
// publish selector status
|
||||
estimator_selector_status_s selector_status{}; |
||||
selector_status.primary_instance = _selected_instance; |
||||
selector_status.instances_available = _available_instances; |
||||
selector_status.instance_changed_count = _instance_changed_count; |
||||
selector_status.last_instance_change = _last_instance_change; |
||||
selector_status.accel_device_id = _instance[_selected_instance].estimator_status.accel_device_id; |
||||
selector_status.baro_device_id = _instance[_selected_instance].estimator_status.baro_device_id; |
||||
selector_status.gyro_device_id = _instance[_selected_instance].estimator_status.gyro_device_id; |
||||
selector_status.mag_device_id = _instance[_selected_instance].estimator_status.mag_device_id; |
||||
selector_status.gyro_fault_detected = _gyro_fault_detected; |
||||
selector_status.accel_fault_detected = _accel_fault_detected; |
||||
|
||||
for (int i = 0; i < EKF2_MAX_INSTANCES; i++) { |
||||
selector_status.combined_test_ratio[i] = _instance[i].combined_test_ratio; |
||||
selector_status.relative_test_ratio[i] = _instance[i].relative_test_ratio; |
||||
selector_status.healthy[i] = _instance[i].healthy; |
||||
} |
||||
|
||||
for (int i = 0; i < IMU_STATUS_SIZE; i++) { |
||||
selector_status.accumulated_gyro_error[i] = _accumulated_gyro_error[i]; |
||||
selector_status.accumulated_accel_error[i] = _accumulated_accel_error[i]; |
||||
} |
||||
|
||||
selector_status.timestamp = hrt_absolute_time(); |
||||
_estimator_selector_status_pub.publish(selector_status); |
||||
} |
||||
|
||||
// republish selected estimator data for system
|
||||
if (_selected_instance != INVALID_INSTANCE) { |
||||
// selected estimator_attitude -> vehicle_attitude
|
||||
if (_instance[_selected_instance].estimator_attitude_sub.updated()) { |
||||
PublishVehicleAttitude(); |
||||
} |
||||
|
||||
// selected estimator_local_position -> vehicle_local_position
|
||||
if (_instance[_selected_instance].estimator_local_position_sub.updated()) { |
||||
PublishVehicleLocalPosition(); |
||||
} |
||||
|
||||
// selected estimator_global_position -> vehicle_global_position
|
||||
if (_instance[_selected_instance].estimator_global_position_sub.updated()) { |
||||
PublishVehicleGlobalPosition(); |
||||
} |
||||
|
||||
// selected estimator_odometry -> vehicle_odometry
|
||||
if (_instance[_selected_instance].estimator_odometry_sub.updated()) { |
||||
vehicle_odometry_s vehicle_odometry; |
||||
|
||||
if (_instance[_selected_instance].estimator_odometry_sub.update(&vehicle_odometry)) { |
||||
if (vehicle_odometry.timestamp >= _instance[_selected_instance].estimator_status.timestamp_sample) { |
||||
vehicle_odometry.timestamp = hrt_absolute_time(); |
||||
_vehicle_odometry_pub.publish(vehicle_odometry); |
||||
} |
||||
} |
||||
} |
||||
|
||||
// selected estimator_visual_odometry_aligned -> vehicle_visual_odometry_aligned
|
||||
if (_instance[_selected_instance].estimator_visual_odometry_aligned_sub.updated()) { |
||||
vehicle_odometry_s vehicle_visual_odometry_aligned; |
||||
|
||||
if (_instance[_selected_instance].estimator_visual_odometry_aligned_sub.update(&vehicle_visual_odometry_aligned)) { |
||||
if (vehicle_visual_odometry_aligned.timestamp >= _instance[_selected_instance].estimator_status.timestamp_sample) { |
||||
vehicle_visual_odometry_aligned.timestamp = hrt_absolute_time(); |
||||
_vehicle_visual_odometry_aligned_pub.publish(vehicle_visual_odometry_aligned); |
||||
} |
||||
} |
||||
} |
||||
} |
||||
|
||||
if (_lockstep_component == -1) { |
||||
_lockstep_component = px4_lockstep_register_component(); |
||||
} |
||||
|
||||
px4_lockstep_progress(_lockstep_component); |
||||
} |
||||
|
||||
void EKF2Selector::PrintStatus() |
||||
{ |
||||
PX4_INFO("available instances: %d", _available_instances); |
||||
|
||||
if (_selected_instance == INVALID_INSTANCE) { |
||||
PX4_WARN("selected instance: None"); |
||||
} |
||||
|
||||
for (int i = 0; i < _available_instances; i++) { |
||||
const EstimatorInstance &inst = _instance[i]; |
||||
|
||||
PX4_INFO("%d: ACC: %d, GYRO: %d, MAG: %d, %s, test ratio: %.7f (%.5f) %s", |
||||
inst.instance, inst.estimator_status.accel_device_id, inst.estimator_status.gyro_device_id, |
||||
inst.estimator_status.mag_device_id, |
||||
inst.healthy ? "healthy" : "unhealthy", |
||||
(double)inst.combined_test_ratio, (double)inst.relative_test_ratio, |
||||
(_selected_instance == i) ? "*" : ""); |
||||
} |
||||
} |
||||
@ -0,0 +1,199 @@
@@ -0,0 +1,199 @@
|
||||
/****************************************************************************
|
||||
* |
||||
* Copyright (c) 2020 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. |
||||
* |
||||
****************************************************************************/ |
||||
|
||||
#pragma once |
||||
|
||||
#include <px4_platform_common/px4_config.h> |
||||
#include <px4_platform_common/log.h> |
||||
#include <px4_platform_common/module.h> |
||||
#include <px4_platform_common/module_params.h> |
||||
#include <px4_platform_common/time.h> |
||||
#include <lib/mathlib/mathlib.h> |
||||
#include <px4_platform_common/px4_work_queue/ScheduledWorkItem.hpp> |
||||
#include <uORB/Subscription.hpp> |
||||
#include <uORB/SubscriptionCallback.hpp> |
||||
#include <uORB/Publication.hpp> |
||||
#include <uORB/topics/estimator_selector_status.h> |
||||
#include <uORB/topics/estimator_status.h> |
||||
#include <uORB/topics/parameter_update.h> |
||||
#include <uORB/topics/sensor_selection.h> |
||||
#include <uORB/topics/sensors_status_imu.h> |
||||
#include <uORB/topics/vehicle_attitude.h> |
||||
#include <uORB/topics/vehicle_local_position.h> |
||||
#include <uORB/topics/vehicle_global_position.h> |
||||
#include <uORB/topics/vehicle_odometry.h> |
||||
|
||||
static constexpr uint8_t EKF2_MAX_INSTANCES{9}; |
||||
static_assert(EKF2_MAX_INSTANCES <= ORB_MULTI_MAX_INSTANCES, "EKF2_MAX_INSTANCES must be <= ORB_MULTI_MAX_INSTANCES"); |
||||
|
||||
class EKF2Selector : public ModuleParams, public px4::ScheduledWorkItem |
||||
{ |
||||
public: |
||||
EKF2Selector(); |
||||
~EKF2Selector() override; |
||||
|
||||
bool Start(); |
||||
void Stop(); |
||||
|
||||
void PrintStatus(); |
||||
|
||||
private: |
||||
static constexpr uint8_t INVALID_INSTANCE = UINT8_MAX; |
||||
void Run() override; |
||||
void PublishVehicleAttitude(bool reset = false); |
||||
void PublishVehicleLocalPosition(bool reset = false); |
||||
void PublishVehicleGlobalPosition(bool reset = false); |
||||
void SelectInstance(uint8_t instance); |
||||
|
||||
// Update the error scores for all available instances
|
||||
bool UpdateErrorScores(); |
||||
|
||||
// Subscriptions (per estimator instance)
|
||||
struct EstimatorInstance { |
||||
EstimatorInstance(EKF2Selector *selector, uint8_t i) : |
||||
estimator_attitude_sub{selector, ORB_ID(estimator_attitude), i}, |
||||
estimator_status_sub{selector, ORB_ID(estimator_status), i}, |
||||
estimator_local_position_sub{ORB_ID(estimator_local_position), i}, |
||||
estimator_global_position_sub{ORB_ID(estimator_global_position), i}, |
||||
estimator_odometry_sub{ORB_ID(estimator_odometry), i}, |
||||
estimator_visual_odometry_aligned_sub{ORB_ID(estimator_visual_odometry_aligned), i}, |
||||
instance(i) |
||||
{} |
||||
|
||||
uORB::SubscriptionCallbackWorkItem estimator_attitude_sub; |
||||
uORB::SubscriptionCallbackWorkItem estimator_status_sub; |
||||
|
||||
uORB::Subscription estimator_local_position_sub; |
||||
uORB::Subscription estimator_global_position_sub; |
||||
uORB::Subscription estimator_odometry_sub; |
||||
uORB::Subscription estimator_visual_odometry_aligned_sub; |
||||
|
||||
estimator_status_s estimator_status{}; |
||||
|
||||
hrt_abstime time_last_selected{0}; |
||||
|
||||
float combined_test_ratio{0.f}; |
||||
float relative_test_ratio{0.f}; |
||||
|
||||
bool healthy{false}; |
||||
|
||||
const uint8_t instance; |
||||
}; |
||||
|
||||
static constexpr float _rel_err_score_lim{1.0f}; // +- limit applied to the relative error score
|
||||
static constexpr float _rel_err_thresh{0.5f}; // the relative score difference needs to be greater than this to switch from an otherwise healthy instance
|
||||
|
||||
EstimatorInstance _instance[EKF2_MAX_INSTANCES] { |
||||
{this, 0}, |
||||
{this, 1}, |
||||
{this, 2}, |
||||
{this, 3}, |
||||
{this, 4}, |
||||
{this, 5}, |
||||
{this, 6}, |
||||
{this, 7}, |
||||
{this, 8}, |
||||
}; |
||||
|
||||
static constexpr uint8_t IMU_STATUS_SIZE = (sizeof(sensors_status_imu_s::gyro_inconsistency_rad_s) / sizeof( |
||||
sensors_status_imu_s::gyro_inconsistency_rad_s[0])); |
||||
static_assert(IMU_STATUS_SIZE == sizeof(estimator_selector_status_s::accumulated_gyro_error) / sizeof( |
||||
estimator_selector_status_s::accumulated_gyro_error[0]), |
||||
"increase estimator_selector_status_s::accumulated_gyro_error size"); |
||||
static_assert(IMU_STATUS_SIZE == sizeof(estimator_selector_status_s::accumulated_accel_error) / sizeof( |
||||
estimator_selector_status_s::accumulated_accel_error[0]), |
||||
"increase estimator_selector_status_s::accumulated_accel_error size"); |
||||
static_assert(EKF2_MAX_INSTANCES == sizeof(estimator_selector_status_s::combined_test_ratio) / sizeof( |
||||
estimator_selector_status_s::combined_test_ratio[0]), |
||||
"increase estimator_selector_status_s::combined_test_ratio size"); |
||||
|
||||
float _accumulated_gyro_error[IMU_STATUS_SIZE] {}; |
||||
float _accumulated_accel_error[IMU_STATUS_SIZE] {}; |
||||
hrt_abstime _last_update_us{0}; |
||||
bool _gyro_fault_detected{false}; |
||||
bool _accel_fault_detected{false}; |
||||
|
||||
uint8_t _available_instances{0}; |
||||
uint8_t _selected_instance{INVALID_INSTANCE}; |
||||
|
||||
uint32_t _instance_changed_count{0}; |
||||
hrt_abstime _last_instance_change{0}; |
||||
|
||||
// vehicle_attitude: reset counters
|
||||
vehicle_attitude_s _attitude_last{}; |
||||
matrix::Quatf _delta_q_reset{}; |
||||
uint8_t _quat_reset_counter{0}; |
||||
|
||||
// vehicle_local_position: reset counters
|
||||
vehicle_local_position_s _local_position_last{}; |
||||
matrix::Vector2f _delta_xy_reset{}; |
||||
float _delta_z_reset{0.f}; |
||||
matrix::Vector2f _delta_vxy_reset{}; |
||||
float _delta_vz_reset{0.f}; |
||||
float _delta_heading_reset{0}; |
||||
uint8_t _xy_reset_counter{0}; |
||||
uint8_t _z_reset_counter{0}; |
||||
uint8_t _vxy_reset_counter{0}; |
||||
uint8_t _vz_reset_counter{0}; |
||||
uint8_t _heading_reset_counter{0}; |
||||
|
||||
// vehicle_global_position: reset counters
|
||||
vehicle_global_position_s _global_position_last{}; |
||||
double _delta_lat_reset{0}; |
||||
double _delta_lon_reset{0}; |
||||
float _delta_alt_reset{0.f}; |
||||
uint8_t _lat_lon_reset_counter{0}; |
||||
uint8_t _alt_reset_counter{0}; |
||||
|
||||
int _lockstep_component{-1}; |
||||
|
||||
uORB::Subscription _parameter_update_sub{ORB_ID(parameter_update)}; |
||||
uORB::Subscription _sensors_status_imu{ORB_ID(sensors_status_imu)}; |
||||
|
||||
// Publications
|
||||
uORB::Publication<estimator_selector_status_s> _estimator_selector_status_pub{ORB_ID(estimator_selector_status)}; |
||||
uORB::Publication<sensor_selection_s> _sensor_selection_pub{ORB_ID(sensor_selection)}; |
||||
uORB::Publication<vehicle_attitude_s> _vehicle_attitude_pub{ORB_ID(vehicle_attitude)}; |
||||
uORB::Publication<vehicle_global_position_s> _vehicle_global_position_pub{ORB_ID(vehicle_global_position)}; |
||||
uORB::Publication<vehicle_local_position_s> _vehicle_local_position_pub{ORB_ID(vehicle_local_position)}; |
||||
uORB::Publication<vehicle_odometry_s> _vehicle_odometry_pub{ORB_ID(vehicle_odometry)}; |
||||
uORB::Publication<vehicle_odometry_s> _vehicle_visual_odometry_aligned_pub{ORB_ID(vehicle_visual_odometry_aligned)}; |
||||
|
||||
DEFINE_PARAMETERS( |
||||
(ParamFloat<px4::params::EKF2_SEL_ERR_RED>) _param_ekf2_sel_err_red, |
||||
(ParamFloat<px4::params::EKF2_SEL_IMU_RAT>) _param_ekf2_sel_imu_angle_rate, |
||||
(ParamFloat<px4::params::EKF2_SEL_IMU_ANG>) _param_ekf2_sel_imu_angle, |
||||
(ParamFloat<px4::params::EKF2_SEL_IMU_ACC>) _param_ekf2_sel_imu_accel, |
||||
(ParamFloat<px4::params::EKF2_SEL_IMU_VEL>) _param_ekf2_sel_imu_velocity |
||||
) |
||||
}; |
||||
@ -0,0 +1,58 @@
@@ -0,0 +1,58 @@
|
||||
/****************************************************************************
|
||||
* |
||||
* Copyright (c) 2020 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. |
||||
* |
||||
****************************************************************************/ |
||||
|
||||
/**
|
||||
* Multi-EKF IMUs |
||||
* |
||||
* Maximum number of IMUs to use for Multi-EKF. Set 0 to disable. |
||||
* Requires SENS_IMU_MODE 0. |
||||
* |
||||
* @group EKF2 |
||||
* @reboot_required true |
||||
* @min 0 |
||||
* @max 3 |
||||
*/ |
||||
PARAM_DEFINE_INT32(EKF2_MULTI_IMU, 0); |
||||
|
||||
/**
|
||||
* Multi-EKF Magnetometers. |
||||
* |
||||
* Maximum number of magnetometers to use for Multi-EKF. Set 0 to disable. |
||||
* Requires SENS_MAG_MODE 0. |
||||
* |
||||
* @group EKF2 |
||||
* @reboot_required true |
||||
* @min 0 |
||||
* @max 4 |
||||
*/ |
||||
PARAM_DEFINE_INT32(EKF2_MULTI_MAG, 0); |
||||
@ -0,0 +1,81 @@
@@ -0,0 +1,81 @@
|
||||
/****************************************************************************
|
||||
* |
||||
* Copyright (c) 2020 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. |
||||
* |
||||
****************************************************************************/ |
||||
|
||||
/**
|
||||
* Selector error reduce threshold |
||||
* |
||||
* EKF2 instances have to be better than the selected by at least this amount before their relative score can be reduced. |
||||
* |
||||
* @group EKF2 |
||||
*/ |
||||
PARAM_DEFINE_FLOAT(EKF2_SEL_ERR_RED, 0.2f); |
||||
|
||||
/**
|
||||
* Selector angular rate threshold |
||||
* |
||||
* EKF2 selector angular rate error threshold for comparing gyros. Angular rate vector differences larger than this will result in accumulated angular error. |
||||
* |
||||
* @group EKF2 |
||||
* @unit deg/s |
||||
*/ |
||||
PARAM_DEFINE_FLOAT(EKF2_SEL_IMU_RAT, 7.0f); |
||||
|
||||
/**
|
||||
* Selector angular threshold. |
||||
* |
||||
* EKF2 selector maximum accumulated angular error threshold for comparing gyros. Accumulated angular error larger than this will result in the sensor being declared faulty. |
||||
* |
||||
* @group EKF2 |
||||
* @unit deg |
||||
*/ |
||||
PARAM_DEFINE_FLOAT(EKF2_SEL_IMU_ANG, 15.0f); |
||||
|
||||
/**
|
||||
* Selector acceleration threshold |
||||
* |
||||
* EKF2 selector acceleration error threshold for comparing accelerometers. Acceleration vector differences larger than this will result in accumulated velocity error. |
||||
* |
||||
* @group EKF2 |
||||
* @unit m/s^2 |
||||
*/ |
||||
PARAM_DEFINE_FLOAT(EKF2_SEL_IMU_ACC, 1.0f); |
||||
|
||||
/**
|
||||
* Selector angular threshold. |
||||
* |
||||
* EKF2 selector maximum accumulated velocity threshold for comparing accelerometers. Accumulated velocity error larger than this will result in the sensor being declared faulty. |
||||
* |
||||
* @group EKF2 |
||||
* @unit m/s |
||||
*/ |
||||
PARAM_DEFINE_FLOAT(EKF2_SEL_IMU_VEL, 2.0f); |
||||
Loading…
Reference in new issue