mirror of
https://gitee.com/xiaohuolufeihua/bizhang_-obav.git
synced 2026-05-21 01:12:11 +00:00
ekf2: improve selector reset handling
- handle reset count rollover (uint8_t) - compute full reset delta if primary estimator instance has changed or if we missed a reset
This commit is contained in:
Daniel Agar
GitHub
@@ -1,6 +1,6 @@
|
||||
/****************************************************************************
|
||||
*
|
||||
* Copyright (c) 2020 PX4 Development Team. All rights reserved.
|
||||
* Copyright (c) 2020-2021 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
|
||||
@@ -37,7 +37,6 @@ using namespace time_literals;
|
||||
using matrix::Quatf;
|
||||
using matrix::Vector2f;
|
||||
using math::constrain;
|
||||
using math::max;
|
||||
using math::radians;
|
||||
|
||||
EKF2Selector::EKF2Selector() :
|
||||
@@ -118,12 +117,6 @@ bool EKF2Selector::SelectInstance(uint8_t ekf_instance)
|
||||
_instance[i].relative_test_ratio = 0;
|
||||
}
|
||||
|
||||
// publish new data immediately with resets
|
||||
PublishVehicleAttitude(true);
|
||||
PublishVehicleLocalPosition(true);
|
||||
PublishVehicleGlobalPosition(true);
|
||||
PublishWindEstimate(true);
|
||||
|
||||
return true;
|
||||
}
|
||||
|
||||
@@ -330,104 +323,152 @@ bool EKF2Selector::UpdateErrorScores()
|
||||
return (primary_updated || updated);
|
||||
}
|
||||
|
||||
void EKF2Selector::PublishVehicleAttitude(bool reset)
|
||||
void EKF2Selector::PublishVehicleAttitude()
|
||||
{
|
||||
// selected estimator_attitude -> vehicle_attitude
|
||||
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;
|
||||
if (_instance[_selected_instance].estimator_attitude_sub.update(&attitude)) {
|
||||
bool instance_change = false;
|
||||
|
||||
_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};
|
||||
}
|
||||
if (_instance[_selected_instance].estimator_attitude_sub.get_instance() != _attitude_instance_prev) {
|
||||
_attitude_instance_prev = _instance[_selected_instance].estimator_attitude_sub.get_instance();
|
||||
instance_change = true;
|
||||
}
|
||||
|
||||
// save last primary estimator_attitude
|
||||
if (_attitude_last.timestamp != 0) {
|
||||
if (!instance_change && (attitude.quat_reset_counter == _attitude_last.quat_reset_counter + 1)) {
|
||||
// propogate deltas from estimator data while maintaining the overall reset counts
|
||||
++_quat_reset_counter;
|
||||
_delta_q_reset = Quatf{attitude.delta_q_reset};
|
||||
|
||||
} else if (instance_change || (attitude.quat_reset_counter != _attitude_last.quat_reset_counter)) {
|
||||
// on reset compute deltas from last published data
|
||||
++_quat_reset_counter;
|
||||
_delta_q_reset = (Quatf(attitude.q) * Quatf(_attitude_last.q).inversed()).normalized();
|
||||
}
|
||||
|
||||
} else {
|
||||
_quat_reset_counter = attitude.quat_reset_counter;
|
||||
_delta_q_reset = Quatf{attitude.delta_q_reset};
|
||||
}
|
||||
|
||||
bool publish = true;
|
||||
|
||||
// ensure monotonically increasing timestamp_sample through reset, don't publish
|
||||
// estimator's attitude for system (vehicle_attitude) if it's stale
|
||||
if ((attitude.timestamp_sample <= _attitude_last.timestamp_sample)
|
||||
|| (attitude.timestamp_sample < _instance[_selected_instance].timestamp_sample_last)) {
|
||||
|
||||
publish = false;
|
||||
}
|
||||
|
||||
// save last primary estimator_attitude as published with original resets
|
||||
_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);
|
||||
if (publish) {
|
||||
// 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);
|
||||
|
||||
_instance[_selected_instance].timestamp_sample_last = attitude.timestamp_sample;
|
||||
attitude.timestamp = hrt_absolute_time();
|
||||
_vehicle_attitude_pub.publish(attitude);
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
void EKF2Selector::PublishVehicleLocalPosition(bool reset)
|
||||
void EKF2Selector::PublishVehicleLocalPosition()
|
||||
{
|
||||
// vehicle_local_position
|
||||
// selected estimator_local_position -> 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;
|
||||
if (_instance[_selected_instance].estimator_local_position_sub.update(&local_position)) {
|
||||
bool instance_change = false;
|
||||
|
||||
_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
|
||||
if (_instance[_selected_instance].estimator_local_position_sub.get_instance() != _local_position_instance_prev) {
|
||||
_local_position_instance_prev = _instance[_selected_instance].estimator_local_position_sub.get_instance();
|
||||
instance_change = true;
|
||||
}
|
||||
|
||||
if (_local_position_last.timestamp != 0) {
|
||||
// XY reset
|
||||
if (local_position.xy_reset_counter > _local_position_last.xy_reset_counter) {
|
||||
if (!instance_change && (local_position.xy_reset_counter == _local_position_last.xy_reset_counter + 1)) {
|
||||
++_xy_reset_counter;
|
||||
_delta_xy_reset = Vector2f{local_position.delta_xy};
|
||||
|
||||
} else if (instance_change || (local_position.xy_reset_counter != _local_position_last.xy_reset_counter)) {
|
||||
++_xy_reset_counter;
|
||||
_delta_xy_reset = Vector2f{local_position.x, local_position.y} - Vector2f{_local_position_last.x, _local_position_last.y};
|
||||
}
|
||||
|
||||
// Z reset
|
||||
if (local_position.z_reset_counter > _local_position_last.z_reset_counter) {
|
||||
if (!instance_change && (local_position.z_reset_counter == _local_position_last.z_reset_counter + 1)) {
|
||||
++_z_reset_counter;
|
||||
_delta_z_reset = local_position.delta_z;
|
||||
|
||||
} else if (instance_change || (local_position.z_reset_counter != _local_position_last.z_reset_counter)) {
|
||||
++_z_reset_counter;
|
||||
_delta_z_reset = local_position.z - _local_position_last.z;
|
||||
}
|
||||
|
||||
// VXY reset
|
||||
if (local_position.vxy_reset_counter > _local_position_last.vxy_reset_counter) {
|
||||
if (!instance_change && (local_position.vxy_reset_counter == _local_position_last.vxy_reset_counter + 1)) {
|
||||
++_vxy_reset_counter;
|
||||
_delta_vxy_reset = Vector2f{local_position.delta_vxy};
|
||||
|
||||
} else if (instance_change || (local_position.vxy_reset_counter != _local_position_last.vxy_reset_counter)) {
|
||||
++_vxy_reset_counter;
|
||||
_delta_vxy_reset = Vector2f{local_position.vx, local_position.vy} - Vector2f{_local_position_last.vx, _local_position_last.vy};
|
||||
}
|
||||
|
||||
// VZ reset
|
||||
if (local_position.vz_reset_counter > _local_position_last.vz_reset_counter) {
|
||||
if (!instance_change && (local_position.vz_reset_counter == _local_position_last.vz_reset_counter + 1)) {
|
||||
++_vz_reset_counter;
|
||||
_delta_vz_reset = local_position.delta_vz;
|
||||
|
||||
} else if (instance_change || (local_position.vz_reset_counter != _local_position_last.vz_reset_counter)) {
|
||||
++_vz_reset_counter;
|
||||
_delta_vz_reset = local_position.vz - _local_position_last.vz;
|
||||
}
|
||||
|
||||
// heading reset
|
||||
if (local_position.heading_reset_counter > _local_position_last.heading_reset_counter) {
|
||||
if (!instance_change && (local_position.heading_reset_counter == _local_position_last.heading_reset_counter + 1)) {
|
||||
++_heading_reset_counter;
|
||||
_delta_heading_reset = local_position.delta_heading;
|
||||
|
||||
} else if (instance_change || (local_position.heading_reset_counter != _local_position_last.heading_reset_counter)) {
|
||||
++_heading_reset_counter;
|
||||
_delta_heading_reset = matrix::wrap_pi(local_position.heading - _local_position_last.heading);
|
||||
}
|
||||
|
||||
} else {
|
||||
_xy_reset_counter = local_position.xy_reset_counter;
|
||||
_z_reset_counter = local_position.z_reset_counter;
|
||||
_vxy_reset_counter = local_position.vxy_reset_counter;
|
||||
_vz_reset_counter = local_position.vz_reset_counter;
|
||||
_heading_reset_counter = local_position.heading_reset_counter;
|
||||
|
||||
_delta_xy_reset = Vector2f{local_position.delta_xy};
|
||||
_delta_z_reset = local_position.delta_z;
|
||||
_delta_vxy_reset = Vector2f{local_position.delta_vxy};
|
||||
_delta_vz_reset = local_position.delta_vz;
|
||||
_delta_heading_reset = local_position.delta_heading;
|
||||
}
|
||||
|
||||
// save last primary estimator_local_position
|
||||
bool publish = true;
|
||||
|
||||
// ensure monotonically increasing timestamp_sample through reset, don't publish
|
||||
// estimator's local position for system (vehicle_local_position) if it's stale
|
||||
if ((local_position.timestamp_sample <= _local_position_last.timestamp_sample)
|
||||
|| (local_position.timestamp_sample < _instance[_selected_instance].timestamp_sample_last)) {
|
||||
|
||||
publish = false;
|
||||
}
|
||||
|
||||
// save last primary estimator_local_position as published with original resets
|
||||
_local_position_last = local_position;
|
||||
|
||||
// publish estimator's local position for system (vehicle_local_position) unless it's stale
|
||||
if (local_position.timestamp_sample >= _instance[_selected_instance].timestamp_sample_last) {
|
||||
if (publish) {
|
||||
// republish with total reset count and current timestamp
|
||||
local_position.xy_reset_counter = _xy_reset_counter;
|
||||
local_position.z_reset_counter = _z_reset_counter;
|
||||
@@ -447,47 +488,92 @@ void EKF2Selector::PublishVehicleLocalPosition(bool reset)
|
||||
}
|
||||
}
|
||||
|
||||
void EKF2Selector::PublishVehicleGlobalPosition(bool reset)
|
||||
void EKF2Selector::PublishVehicleOdometry()
|
||||
{
|
||||
// selected estimator_odometry -> vehicle_odometry
|
||||
vehicle_odometry_s odometry;
|
||||
|
||||
if (_instance[_selected_instance].estimator_odometry_sub.update(&odometry)) {
|
||||
bool publish = true;
|
||||
|
||||
// ensure monotonically increasing timestamp_sample through reset, don't publish
|
||||
// estimator's odometry for system (vehicle_odometry) if it's stale
|
||||
if ((odometry.timestamp_sample <= _odometry_last.timestamp_sample)
|
||||
|| (odometry.timestamp_sample < _instance[_selected_instance].timestamp_sample_last)) {
|
||||
|
||||
publish = false;
|
||||
}
|
||||
|
||||
// save last primary estimator_odometry
|
||||
_odometry_last = odometry;
|
||||
|
||||
if (publish) {
|
||||
odometry.timestamp = hrt_absolute_time();
|
||||
_vehicle_odometry_pub.publish(odometry);
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
void EKF2Selector::PublishVehicleGlobalPosition()
|
||||
{
|
||||
// selected estimator_global_position -> vehicle_global_position
|
||||
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;
|
||||
if (_instance[_selected_instance].estimator_global_position_sub.update(&global_position)) {
|
||||
bool instance_change = false;
|
||||
|
||||
_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
|
||||
if (_instance[_selected_instance].estimator_global_position_sub.get_instance() != _global_position_instance_prev) {
|
||||
_global_position_instance_prev = _instance[_selected_instance].estimator_global_position_sub.get_instance();
|
||||
instance_change = true;
|
||||
}
|
||||
|
||||
if (_global_position_last.timestamp != 0) {
|
||||
// lat/lon reset
|
||||
if (global_position.lat_lon_reset_counter > _global_position_last.lat_lon_reset_counter) {
|
||||
if (!instance_change && (global_position.lat_lon_reset_counter == _global_position_last.lat_lon_reset_counter + 1)) {
|
||||
++_lat_lon_reset_counter;
|
||||
|
||||
// TODO: delta latitude/longitude
|
||||
//_delta_lat_reset = global_position.delta_lat;
|
||||
//_delta_lon_reset = global_position.delta_lon;
|
||||
_delta_lat_reset = global_position.lat - _global_position_last.lat;
|
||||
_delta_lon_reset = global_position.lon - _global_position_last.lon;
|
||||
|
||||
} else if (instance_change || (global_position.lat_lon_reset_counter != _global_position_last.lat_lon_reset_counter)) {
|
||||
++_lat_lon_reset_counter;
|
||||
|
||||
_delta_lat_reset = global_position.lat - _global_position_last.lat;
|
||||
_delta_lon_reset = global_position.lon - _global_position_last.lon;
|
||||
}
|
||||
|
||||
// alt reset
|
||||
if (global_position.alt_reset_counter > _global_position_last.alt_reset_counter) {
|
||||
if (!instance_change && (global_position.alt_reset_counter == _global_position_last.alt_reset_counter + 1)) {
|
||||
++_alt_reset_counter;
|
||||
_delta_alt_reset = global_position.delta_alt;
|
||||
|
||||
} else if (instance_change || (global_position.alt_reset_counter != _global_position_last.alt_reset_counter)) {
|
||||
++_alt_reset_counter;
|
||||
_delta_alt_reset = global_position.delta_alt - _global_position_last.delta_alt;
|
||||
}
|
||||
|
||||
} else {
|
||||
_lat_lon_reset_counter = global_position.lat_lon_reset_counter;
|
||||
_alt_reset_counter = global_position.alt_reset_counter;
|
||||
|
||||
_delta_alt_reset = global_position.delta_alt;
|
||||
}
|
||||
|
||||
// save last primary estimator_global_position
|
||||
bool publish = true;
|
||||
|
||||
// ensure monotonically increasing timestamp_sample through reset, don't publish
|
||||
// estimator's global position for system (vehicle_global_position) if it's stale
|
||||
if ((global_position.timestamp_sample <= _global_position_last.timestamp_sample)
|
||||
|| (global_position.timestamp_sample < _instance[_selected_instance].timestamp_sample_last)) {
|
||||
|
||||
publish = false;
|
||||
}
|
||||
|
||||
// save last primary estimator_global_position as published with original resets
|
||||
_global_position_last = global_position;
|
||||
|
||||
// publish estimator's global position for system (vehicle_global_position) unless it's stale
|
||||
if (global_position.timestamp_sample >= _instance[_selected_instance].timestamp_sample_last) {
|
||||
if (publish) {
|
||||
// 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;
|
||||
@@ -499,22 +585,31 @@ void EKF2Selector::PublishVehicleGlobalPosition(bool reset)
|
||||
}
|
||||
}
|
||||
|
||||
void EKF2Selector::PublishWindEstimate(bool reset)
|
||||
void EKF2Selector::PublishWindEstimate()
|
||||
{
|
||||
// selected estimator_wind -> wind
|
||||
wind_s wind;
|
||||
|
||||
if (_instance[_selected_instance].estimator_wind_sub.copy(&wind)) {
|
||||
if (reset) {
|
||||
// ensure monotonically increasing timestamp_sample through reset
|
||||
wind.timestamp_sample = max(wind.timestamp_sample, _wind_last.timestamp_sample);
|
||||
if (_instance[_selected_instance].estimator_wind_sub.update(&wind)) {
|
||||
bool publish = true;
|
||||
|
||||
// ensure monotonically increasing timestamp_sample through reset, don't publish
|
||||
// estimator's wind for system (wind) if it's stale
|
||||
if ((wind.timestamp_sample <= _wind_last.timestamp_sample)
|
||||
|| (wind.timestamp_sample < _instance[_selected_instance].timestamp_sample_last)) {
|
||||
|
||||
publish = false;
|
||||
}
|
||||
|
||||
// save last primary wind
|
||||
_wind_last = wind;
|
||||
|
||||
// republish with current timestamp
|
||||
wind.timestamp = hrt_absolute_time();
|
||||
_wind_pub.publish(wind);
|
||||
// publish estimator's wind for system unless it's stale
|
||||
if (publish) {
|
||||
// republish with current timestamp
|
||||
wind.timestamp = hrt_absolute_time();
|
||||
_wind_pub.publish(wind);
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
@@ -655,38 +750,11 @@ void EKF2Selector::Run()
|
||||
}
|
||||
|
||||
// republish selected estimator data for system
|
||||
|
||||
// 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_wind -> wind
|
||||
if (_instance[_selected_instance].estimator_wind_sub.updated()) {
|
||||
PublishWindEstimate();
|
||||
}
|
||||
|
||||
// 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_sample >= _instance[_selected_instance].timestamp_sample_last) {
|
||||
vehicle_odometry.timestamp = hrt_absolute_time();
|
||||
_vehicle_odometry_pub.publish(vehicle_odometry);
|
||||
}
|
||||
}
|
||||
}
|
||||
PublishVehicleAttitude();
|
||||
PublishVehicleLocalPosition();
|
||||
PublishVehicleGlobalPosition();
|
||||
PublishVehicleOdometry();
|
||||
PublishWindEstimate();
|
||||
}
|
||||
|
||||
void EKF2Selector::PrintStatus()
|
||||
|
||||
@@ -1,6 +1,6 @@
|
||||
/****************************************************************************
|
||||
*
|
||||
* Copyright (c) 2020 PX4 Development Team. All rights reserved.
|
||||
* Copyright (c) 2020-2021 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
|
||||
@@ -78,10 +78,11 @@ private:
|
||||
static constexpr uint64_t FILTER_UPDATE_PERIOD{10_ms};
|
||||
|
||||
void Run() override;
|
||||
void PublishVehicleAttitude(bool reset = false);
|
||||
void PublishVehicleLocalPosition(bool reset = false);
|
||||
void PublishVehicleGlobalPosition(bool reset = false);
|
||||
void PublishWindEstimate(bool reset = false);
|
||||
void PublishVehicleAttitude();
|
||||
void PublishVehicleLocalPosition();
|
||||
void PublishVehicleGlobalPosition();
|
||||
void PublishVehicleOdometry();
|
||||
void PublishWindEstimate();
|
||||
bool SelectInstance(uint8_t instance);
|
||||
|
||||
// Update the error scores for all available instances
|
||||
@@ -191,6 +192,9 @@ private:
|
||||
uint8_t _vz_reset_counter{0};
|
||||
uint8_t _heading_reset_counter{0};
|
||||
|
||||
// vehicle_odometry
|
||||
vehicle_odometry_s _odometry_last{};
|
||||
|
||||
// vehicle_global_position: reset counters
|
||||
vehicle_global_position_s _global_position_last{};
|
||||
double _delta_lat_reset{0};
|
||||
@@ -202,6 +206,10 @@ private:
|
||||
// wind estimate
|
||||
wind_s _wind_last{};
|
||||
|
||||
uint8_t _attitude_instance_prev{INVALID_INSTANCE};
|
||||
uint8_t _local_position_instance_prev{INVALID_INSTANCE};
|
||||
uint8_t _global_position_instance_prev{INVALID_INSTANCE};
|
||||
|
||||
uORB::SubscriptionInterval _parameter_update_sub{ORB_ID(parameter_update), 1_s};
|
||||
uORB::Subscription _sensors_status_imu{ORB_ID(sensors_status_imu)};
|
||||
|
||||
|
||||
Reference in New Issue
Block a user