mirror of
https://gitee.com/mirrors_PX4/PX4-Autopilot.git
synced 2026-10-16 04:23:35 +08:00
ekf2: add parameters for control of external vision fusion
This commit is contained in:
Paul Riseborough
committed by
Lorenz Meier
Lorenz Meier
parent
2e127a4737
commit
57c1138d28
@@ -168,6 +168,7 @@ private:
|
||||
control::BlockParamFloat _flow_delay_ms;
|
||||
control::BlockParamFloat _rng_delay_ms;
|
||||
control::BlockParamFloat _airspeed_delay_ms;
|
||||
control::BlockParamFloat _ev_delay_ms;
|
||||
|
||||
control::BlockParamFloat _gyro_noise;
|
||||
control::BlockParamFloat _accel_noise;
|
||||
@@ -220,6 +221,11 @@ private:
|
||||
control::BlockParamFloat _range_innov_gate; // range finder fusion innovation consistency gate size (STD)
|
||||
control::BlockParamFloat _rng_gnd_clearance; // minimum valid value for range when on ground (m)
|
||||
|
||||
// vision estimate fusion
|
||||
control::BlockParamFloat _ev_noise; // observation noise for range finder measurements (m)
|
||||
control::BlockParamFloat _ev_innov_gate; // range finder fusion innovation consistency gate size (STD)
|
||||
control::BlockParamFloat _ev_gnd_clearance; // minimum valid value for range when on ground (m)
|
||||
|
||||
// optical flow fusion
|
||||
control::BlockParamFloat
|
||||
_flow_noise; // best quality observation noise for optical flow LOS rate measurements (rad/sec)
|
||||
@@ -242,6 +248,9 @@ private:
|
||||
control::BlockParamFloat _flow_pos_x; // X position of optical flow sensor focal point in body frame (m)
|
||||
control::BlockParamFloat _flow_pos_y; // Y position of optical flow sensor focal point in body frame (m)
|
||||
control::BlockParamFloat _flow_pos_z; // Z position of optical flow sensor focal point in body frame (m)
|
||||
control::BlockParamFloat _ev_pos_x; // X position of VI sensor focal point in body frame (m)
|
||||
control::BlockParamFloat _ev_pos_y; // Y position of VI sensor focal point in body frame (m)
|
||||
control::BlockParamFloat _ev_pos_z; // Z position of VI sensor focal point in body frame (m)
|
||||
|
||||
// output predictor filter time constants
|
||||
control::BlockParamFloat _tau_vel; // time constant used by the output velocity complementary filter (s)
|
||||
@@ -279,6 +288,7 @@ Ekf2::Ekf2():
|
||||
_flow_delay_ms(this, "EKF2_OF_DELAY", false, &_params->flow_delay_ms),
|
||||
_rng_delay_ms(this, "EKF2_RNG_DELAY", false, &_params->range_delay_ms),
|
||||
_airspeed_delay_ms(this, "EKF2_ASP_DELAY", false, &_params->airspeed_delay_ms),
|
||||
_ev_delay_ms(this, "EKF2_EV_DELAY", false, &_params->ev_delay_ms),
|
||||
_gyro_noise(this, "EKF2_GYR_NOISE", false, &_params->gyro_noise),
|
||||
_accel_noise(this, "EKF2_ACC_NOISE", false, &_params->accel_noise),
|
||||
_gyro_bias_p_noise(this, "EKF2_GYR_B_NOISE", false, &_params->gyro_bias_p_noise),
|
||||
@@ -318,6 +328,9 @@ Ekf2::Ekf2():
|
||||
_range_noise(this, "EKF2_RNG_NOISE", false, &_params->range_noise),
|
||||
_range_innov_gate(this, "EKF2_RNG_GATE", false, &_params->range_innov_gate),
|
||||
_rng_gnd_clearance(this, "EKF2_MIN_RNG", false, &_params->rng_gnd_clearance),
|
||||
_ev_noise(this, "EKF2_EV_NOISE", false, &_params->ev_noise),
|
||||
_ev_innov_gate(this, "EKF2_EV_GATE", false, &_params->ev_innov_gate),
|
||||
_ev_gnd_clearance(this, "EKF2_MIN_EV", false, &_params->ev_gnd_clearance),
|
||||
_flow_noise(this, "EKF2_OF_N_MIN", false, &_params->flow_noise),
|
||||
_flow_noise_qual_min(this, "EKF2_OF_N_MAX", false, &_params->flow_noise_qual_min),
|
||||
_flow_qual_min(this, "EKF2_OF_QMIN", false, &_params->flow_qual_min),
|
||||
@@ -335,6 +348,9 @@ Ekf2::Ekf2():
|
||||
_flow_pos_x(this, "EKF2_OF_POS_X", false, &_params->flow_pos_body(0)),
|
||||
_flow_pos_y(this, "EKF2_OF_POS_Y", false, &_params->flow_pos_body(1)),
|
||||
_flow_pos_z(this, "EKF2_OF_POS_Z", false, &_params->flow_pos_body(2)),
|
||||
_ev_pos_x(this, "EKF2_EV_POS_X", false, &_params->ev_pos_body(0)),
|
||||
_ev_pos_y(this, "EKF2_EV_POS_Y", false, &_params->ev_pos_body(1)),
|
||||
_ev_pos_z(this, "EKF2_EV_POS_Z", false, &_params->ev_pos_body(2)),
|
||||
_tau_vel(this, "EKF2_TAU_VEL", false, &_params->vel_Tau),
|
||||
_tau_pos(this, "EKF2_TAU_POS", false, &_params->pos_Tau),
|
||||
_gyr_bias_init(this, "EKF2_GBIAS_INIT", false, &_params->switch_on_gyro_bias),
|
||||
|
||||
@@ -108,6 +108,17 @@ PARAM_DEFINE_FLOAT(EKF2_RNG_DELAY, 5);
|
||||
*/
|
||||
PARAM_DEFINE_FLOAT(EKF2_ASP_DELAY, 200);
|
||||
|
||||
/**
|
||||
* Vision Position Estimator delay relative to IMU measurements
|
||||
*
|
||||
* @group EKF2
|
||||
* @min 0
|
||||
* @max 300
|
||||
* @unit ms
|
||||
* @decimal 1
|
||||
*/
|
||||
PARAM_DEFINE_FLOAT(EKF2_EV_DELAY, 175);
|
||||
|
||||
/**
|
||||
* Integer bitmask controlling GPS checks.
|
||||
*
|
||||
@@ -471,10 +482,12 @@ PARAM_DEFINE_INT32(EKF2_REC_RPL, 0);
|
||||
* 0 : Set to true to use GPS data if available
|
||||
* 1 : Set to true to use optical flow data if available
|
||||
* 2 : Set to true to inhibit IMU bias estimation
|
||||
*
|
||||
* 3 : Set to true to enable vision position fusion
|
||||
* 4 : Set to true to enable vision yaw fusion
|
||||
* *
|
||||
* @group EKF2
|
||||
* @min 0
|
||||
* @max 15
|
||||
* @max 28
|
||||
*/
|
||||
PARAM_DEFINE_INT32(EKF2_AID_MASK, 1);
|
||||
|
||||
@@ -520,6 +533,36 @@ PARAM_DEFINE_FLOAT(EKF2_RNG_GATE, 5.0f);
|
||||
*/
|
||||
PARAM_DEFINE_FLOAT(EKF2_MIN_RNG, 0.1f);
|
||||
|
||||
|
||||
/**
|
||||
* Measurement noise for vision estimate fusion
|
||||
*
|
||||
* @group EKF2
|
||||
* @min 0.01
|
||||
* @unit m
|
||||
* @decimal 2
|
||||
*/
|
||||
PARAM_DEFINE_FLOAT(EKF2_EV_NOISE, 0.05f);
|
||||
|
||||
/**
|
||||
* Gate size for vision estimate fusion
|
||||
*
|
||||
* @group EKF2
|
||||
* @min 1.0
|
||||
* @unit SD
|
||||
* @decimal 1
|
||||
*/
|
||||
PARAM_DEFINE_FLOAT(EKF2_EV_GATE, 5.0f);
|
||||
|
||||
/**
|
||||
* Minimum valid range for the vision estimate
|
||||
*
|
||||
* @group EKF2
|
||||
* @min 0.01
|
||||
* @unit m
|
||||
* @decimal 2
|
||||
*/
|
||||
PARAM_DEFINE_FLOAT(EKF2_MIN_EV, 0.01f);
|
||||
/**
|
||||
* Measurement noise for the optical flow sensor when it's reported quality metric is at the maximum
|
||||
*
|
||||
@@ -701,7 +744,35 @@ PARAM_DEFINE_FLOAT(EKF2_OF_POS_Y, 0.0f);
|
||||
PARAM_DEFINE_FLOAT(EKF2_OF_POS_Z, 0.0f);
|
||||
|
||||
/**
|
||||
* Time constant of the velocity output prediction and smoothing filter. Controls how tightly the velocity output tracks the EKF states. Set to a negative number to disable the EKF velocity state tracking. This will cause the output velocity to track the output position time derivative.
|
||||
* X position of VI sensor focal point in body frame
|
||||
*
|
||||
* @group EKF2
|
||||
* @unit m
|
||||
* @decimal 3
|
||||
*/
|
||||
PARAM_DEFINE_FLOAT(EKF2_EV_POS_X, 0.0f);
|
||||
|
||||
/**
|
||||
* Y position of VI sensor focal point in body frame
|
||||
*
|
||||
* @group EKF2
|
||||
* @unit m
|
||||
* @decimal 3
|
||||
*/
|
||||
PARAM_DEFINE_FLOAT(EKF2_EV_POS_Y, 0.0f);
|
||||
|
||||
/**
|
||||
* Z position of VI sensor focal point in body frame
|
||||
*
|
||||
* @group EKF2
|
||||
* @unit m
|
||||
* @decimal 3
|
||||
*/
|
||||
PARAM_DEFINE_FLOAT(EKF2_EV_POS_Z, 0.0f);
|
||||
|
||||
/**
|
||||
|
||||
* Time constant of the velocity output prediction and smoothing filter
|
||||
*
|
||||
* @group EKF2
|
||||
* @max 1.0
|
||||
|
||||
Reference in New Issue
Block a user