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

ekf2: estimator aid source status (starting with fake position)

Browse Source
This commit is contained in:
Daniel Agar
2022-05-23 16:21:49 -04:00
parent 79a34b5aed
commit 1ae467e9cd
13 changed files with 287 additions and 97 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/ekf2/EKF/ekf.h
+68 -2
View File
@@ -48,6 +48,10 @@
#include "EKFGSF_yaw.h"
#include "baro_bias_estimator.hpp"
#include <uORB/topics/estimator_aid_source_1d.h>
#include <uORB/topics/estimator_aid_source_2d.h>
#include <uORB/topics/estimator_aid_source_3d.h>
class Ekf final : public EstimatorInterface
{
public:
@@ -336,6 +340,8 @@ public:
const BaroBiasEstimator::status &getBaroBiasEstimatorStatus() const { return _baro_b_est.getStatus(); }
const auto &aid_src_fake_pos() const { return _aid_src_fake_pos; }
private:
// set the internal states and status to their default value
@@ -394,7 +400,6 @@ private:
uint64_t _time_last_flow_terrain_fuse{0}; ///< time the last fusion of optical flow measurements for terrain estimation were performed (uSec)
uint64_t _time_last_arsp_fuse{0}; ///< time the last fusion of airspeed measurements were performed (uSec)
uint64_t _time_last_beta_fuse{0}; ///< time the last fusion of synthetic sideslip measurements were performed (uSec)
uint64_t _time_last_fake_pos_fuse{0}; ///< last time we faked position measurements to constrain tilt errors during operation without external aiding (uSec)
uint64_t _time_last_zero_velocity_fuse{0}; ///< last time of zero velocity update (uSec)
uint64_t _time_last_gps_yaw_fuse{0}; ///< time the last fusion of GPS yaw measurements were performed (uSec)
uint64_t _time_last_gps_yaw_data{0}; ///< time the last GPS yaw measurement was available (uSec)
@@ -491,6 +496,8 @@ private:
bool _inhibit_flow_use{false}; ///< true when use of optical flow and range finder is being inhibited
Vector2f _flow_compensated_XY_rad{}; ///< measured delta angle of the image about the X and Y body axes after removal of body rotation (rad), RH rotation is positive
estimator_aid_source_2d_s _aid_src_fake_pos{};
// output predictor states
Vector3f _delta_angle_corr{}; ///< delta angle correction vector (rad)
Vector3f _vel_err_integ{}; ///< integral of velocity tracking error (m)
@@ -676,7 +683,7 @@ private:
Vector3f &innov_var, Vector2f &test_ratio);
bool fuseHorizontalPosition(const Vector3f &innov, float innov_gate, const Vector3f &obs_var,
Vector3f &innov_var, Vector2f &test_ratiov, bool inhibit_gate = false);
Vector3f &innov_var, Vector2f &test_ratiov);
bool fuseVerticalPosition(float innov, float innov_gate, float obs_var,
float &innov_var, float &test_ratio);
@@ -1048,6 +1055,65 @@ private:
bool isYawEmergencyEstimateAvailable() const;
void resetGpsDriftCheckFilters();
bool resetEstimatorAidStatusFlags(estimator_aid_source_1d_s &status)
{
if (status.timestamp_sample != 0) {
status.timestamp_sample = 0;
status.fusion_enabled = false;
status.innovation_rejected = false;
status.fused = false;
return true;
}
return false;
}
void resetEstimatorAidStatus(estimator_aid_source_1d_s &status)
{
if (resetEstimatorAidStatusFlags(status)) {
status.observation = 0;
status.observation_variance = 0;
status.innovation = 0;
status.innovation_variance = 0;
status.test_ratio = 0;
}
}
template <typename T>
bool resetEstimatorAidStatusFlags(T &status)
{
if (status.timestamp_sample != 0) {
status.timestamp_sample = 0;
for (size_t i = 0; i < (sizeof(status.fusion_enabled) / sizeof(status.fusion_enabled[0])); i++) {
status.fusion_enabled[i] = false;
status.innovation_rejected[i] = false;
status.fused[i] = false;
}
return true;
}
return false;
}
template <typename T>
void resetEstimatorAidStatus(T &status)
{
if (resetEstimatorAidStatusFlags(status)) {
for (size_t i = 0; i < (sizeof(status.fusion_enabled) / sizeof(status.fusion_enabled[0])); i++) {
status.observation[i] = 0;
status.observation_variance[i] = 0;
status.innovation[i] = 0;
status.innovation_variance[i] = 0;
status.test_ratio[i] = 0;
}
}
}
};
#endif // !EKF_EKF_H
src/modules/ekf2/EKF/fake_pos_control.cpp
+46 -13
View File
@@ -40,9 +40,14 @@
void Ekf::controlFakePosFusion()
{
auto &fake_pos = _aid_src_fake_pos;
// clear
resetEstimatorAidStatusFlags(fake_pos);
// If we aren't doing any aiding, fake position measurements at the last known position to constrain drift
// During intial tilt aligment, fake position is used to perform a "quasi-stationary" leveling of the EKF
const bool fake_pos_data_ready = isTimedOut(_time_last_fake_pos_fuse, (uint64_t)2e5); // Fuse fake position at a limited rate
const bool fake_pos_data_ready = isTimedOut(fake_pos.time_last_fuse[0], (uint64_t)2e5); // Fuse fake position at a limited rate
if (fake_pos_data_ready) {
const bool continuing_conditions_passing = !isHorizontalAidingActive();
@@ -52,7 +57,7 @@ void Ekf::controlFakePosFusion()
if (continuing_conditions_passing) {
fuseFakePosition();
const bool is_fusion_failing = isTimedOut(_time_last_fake_pos_fuse, (uint64_t)4e5);
const bool is_fusion_failing = isTimedOut(fake_pos.time_last_fuse[0], (uint64_t)4e5);
if (is_fusion_failing) {
resetFakePosFusion();
@@ -79,6 +84,7 @@ void Ekf::controlFakePosFusion()
void Ekf::startFakePosFusion()
{
if (!_using_synthetic_position) {
ECL_INFO("start fake position fusion");
_using_synthetic_position = true;
_fuse_hpos_as_odom = false; // TODO: needed?
resetFakePosFusion();
@@ -87,39 +93,66 @@ void Ekf::startFakePosFusion()
void Ekf::resetFakePosFusion()
{
ECL_INFO("reset fake position fusion");
_last_known_posNE = _state.pos.xy();
resetHorizontalPositionToLastKnown();
resetHorizontalVelocityToZero();
_time_last_fake_pos_fuse = _time_last_imu;
_aid_src_fake_pos.time_last_fuse[0] = _time_last_imu;
_aid_src_fake_pos.time_last_fuse[1] = _time_last_imu;
}
void Ekf::stopFakePosFusion()
{
_using_synthetic_position = false;
if (_using_synthetic_position) {
ECL_INFO("stop fake position fusion");
_using_synthetic_position = false;
resetEstimatorAidStatus(_aid_src_fake_pos);
}
}
void Ekf::fuseFakePosition()
{
Vector3f fake_pos_obs_var;
Vector2f obs_var;
if (_control_status.flags.in_air && _control_status.flags.tilt_align) {
fake_pos_obs_var(0) = fake_pos_obs_var(1) = sq(fmaxf(_params.pos_noaid_noise, _params.gps_pos_noise));
obs_var(0) = obs_var(1) = sq(fmaxf(_params.pos_noaid_noise, _params.gps_pos_noise));
} else if (!_control_status.flags.in_air && _control_status.flags.vehicle_at_rest) {
// Accelerate tilt fine alignment by fusing more
// aggressively when the vehicle is at rest
fake_pos_obs_var(0) = fake_pos_obs_var(1) = sq(0.01f);
obs_var(0) = obs_var(1) = sq(0.01f);
} else {
fake_pos_obs_var(0) = fake_pos_obs_var(1) = sq(0.5f);
obs_var(0) = obs_var(1) = sq(0.5f);
}
_gps_pos_innov.xy() = Vector2f(_state.pos) - _last_known_posNE;
const float innov_gate = 3.f;
const float fake_pos_innov_gate = 3.f;
auto &fake_pos = _aid_src_fake_pos;
if (fuseHorizontalPosition(_gps_pos_innov, fake_pos_innov_gate, fake_pos_obs_var,
_gps_pos_innov_var, _gps_pos_test_ratio, true)) {
_time_last_fake_pos_fuse = _time_last_imu;
for (int i = 0; i < 2; i++) {
fake_pos.observation[i] = _last_known_posNE(i);
fake_pos.observation_variance[i] = obs_var(i);
fake_pos.innovation[i] = _state.pos(i) - _last_known_posNE(i);
fake_pos.innovation_variance[i] = P(7 + i, 7 + i) + obs_var(i);
fake_pos.test_ratio[i] = sq(fake_pos.innovation[i]) / (sq(innov_gate) * fake_pos.innovation_variance[i]);
fake_pos.innovation_rejected[i] = (fake_pos.test_ratio[i] > 1.f);
// always protect against extreme values that could result in a NaN
fake_pos.fusion_enabled[i] = fake_pos.test_ratio[i] < sq(100.0f / innov_gate);
if (fake_pos.fusion_enabled[i] && !fake_pos.innovation_rejected[i]) {
if (fuseVelPosHeight(fake_pos.innovation[i], fake_pos.innovation_variance[i], 3 + i)) {
fake_pos.fused[i] = true;
fake_pos.time_last_fuse[i] = _time_last_imu;
}
}
}
fake_pos.timestamp_sample = _time_last_imu;
}
src/modules/ekf2/EKF/vel_pos_fusion.cpp
+2 -7
View File
@@ -102,7 +102,7 @@ bool Ekf::fuseVerticalVelocity(const Vector3f &innov, const float innov_gate, co
}
bool Ekf::fuseHorizontalPosition(const Vector3f &innov, const float innov_gate, const Vector3f &obs_var,
Vector3f &innov_var, Vector2f &test_ratio, bool inhibit_gate)
Vector3f &innov_var, Vector2f &test_ratio)
{
innov_var(0) = P(7, 7) + obs_var(0);
@@ -112,12 +112,7 @@ bool Ekf::fuseHorizontalPosition(const Vector3f &innov, const float innov_gate,
const bool innov_check_pass = test_ratio(0) <= 1.0f;
if (innov_check_pass || inhibit_gate) {
if (inhibit_gate && test_ratio(0) > sq(100.0f / innov_gate)) {
// always protect against extreme values that could result in a NaN
return false;
}
if (innov_check_pass) {
_innov_check_fail_status.flags.reject_hor_pos = false;
bool fuse_x = fuseVelPosHeight(innov(0), innov_var(0), 3);