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

ekf2: simplify mag auto (3d select)

Browse Source
This commit is contained in:
Daniel Agar
2022-03-21 16:35:06 -04:00
parent ad1a35f7a4
commit 0df8443f2f
4 changed files with 52 additions and 54 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/ekf2/EKF/ekf.cpp
+3 -17
View File
@@ -276,11 +276,6 @@ void Ekf::predictState()
const Vector3f corrected_delta_vel = _imu_sample_delayed.delta_vel - _state.delta_vel_bias;
const Vector3f corrected_delta_vel_ef = _R_to_earth * corrected_delta_vel;
// calculate a filtered horizontal acceleration with a 1 sec time constant
// this are used for manoeuvre detection elsewhere
const float alpha = 1.0f - _imu_sample_delayed.delta_vel_dt;
_accel_lpf_NE = _accel_lpf_NE * alpha + corrected_delta_vel_ef.xy();
// save the previous value of velocity so we can use trapzoidal integration
const Vector3f vel_last = _state.vel;
@@ -331,15 +326,6 @@ void Ekf::calculateOutputStates(const imuSample &imu)
// Apply corrections to the delta angle required to track the quaternion states at the EKF fusion time horizon
const Vector3f delta_angle(imu.delta_ang - _state.delta_ang_bias * dt_scale_correction + _delta_angle_corr);
// calculate a yaw change about the earth frame vertical
const float spin_del_ang_D = delta_angle.dot(Vector3f(_R_to_earth_now.row(2)));
_yaw_delta_ef += spin_del_ang_D;
// Calculate filtered yaw rate to be used by the magnetometer fusion type selection logic
// Note fixed coefficients are used to save operations. The exact time constant is not important.
_yaw_rate_lpf_ef = 0.95f * _yaw_rate_lpf_ef + 0.05f * spin_del_ang_D / imu.delta_ang_dt;
_output_new.time_us = imu.time_us;
_output_vert_new.time_us = imu.time_us;
@@ -352,13 +338,13 @@ void Ekf::calculateOutputStates(const imuSample &imu)
_output_new.quat_nominal.normalize();
// calculate the rotation matrix from body to earth frame
_R_to_earth_now = Dcmf(_output_new.quat_nominal);
const Dcmf R_to_earth_now{_output_new.quat_nominal};
// correct delta velocity for bias offsets
const Vector3f delta_vel_body{imu.delta_vel - _state.delta_vel_bias * dt_scale_correction};
// rotate the delta velocity to earth frame
Vector3f delta_vel_earth{_R_to_earth_now * delta_vel_body};
Vector3f delta_vel_earth{R_to_earth_now * delta_vel_body};
// correct for measured acceleration due to gravity
delta_vel_earth(2) += CONSTANTS_ONE_G * imu.delta_vel_dt;
@@ -394,7 +380,7 @@ void Ekf::calculateOutputStates(const imuSample &imu)
const Vector3f vel_imu_rel_body = ang_rate % _params.imu_pos_body;
// rotate the relative velocity into earth frame
_vel_imu_rel_body_ned = _R_to_earth_now * vel_imu_rel_body;
_vel_imu_rel_body_ned = R_to_earth_now * vel_imu_rel_body;
}
// store the INS states in a ring buffer with the same length and time coordinates as the IMU data buffer
src/modules/ekf2/EKF/ekf.h
-3
View File
@@ -416,7 +416,6 @@ private:
// used by magnetometer fusion mode selection
Vector2f _accel_lpf_NE{}; ///< Low pass filtered horizontal earth frame acceleration (m/sec**2)
float _yaw_delta_ef{0.0f}; ///< Recent change in yaw angle measured about the earth frame D axis (rad)
float _yaw_rate_lpf_ef{0.0f}; ///< Filtered angular rate about earth frame D axis (rad/sec)
bool _mag_bias_observable{false}; ///< true when there is enough rotation to make magnetometer bias errors observable
bool _yaw_angle_observable{false}; ///< true when there is enough horizontal acceleration to make yaw observable
@@ -851,8 +850,6 @@ private:
void runYawReset();
void selectMagAuto();
void check3DMagFusionSuitability();
void checkYawAngleObservability();
void checkMagBiasObservability();
bool canUse3DMagFusion() const;
src/modules/ekf2/EKF/estimator_interface.h
+1 -2
View File
@@ -195,7 +195,7 @@ public:
Vector3f getPosition() const
{
// rotate the position of the IMU relative to the boy origin into earth frame
const Vector3f pos_offset_earth = _R_to_earth_now * _params.imu_pos_body;
const Vector3f pos_offset_earth = Dcmf(_output_new.quat_nominal) * _params.imu_pos_body;
// subtract from the EKF position (which is at the IMU) to get position at the body origin
return _output_new.pos - pos_offset_earth;
}
@@ -308,7 +308,6 @@ protected:
outputSample _output_new{}; // filter output on the non-delayed time horizon
outputVert _output_vert_new{}; // vertical filter output on the non-delayed time horizon
imuSample _newest_high_rate_imu_sample{}; // imu sample capturing the newest imu data
Matrix3f _R_to_earth_now{}; // rotation matrix from body to earth frame at current time
Vector3f _vel_imu_rel_body_ned{}; // velocity of IMU relative to body origin in NED earth frame
Vector3f _vel_deriv{}; // velocity derivative at the IMU in NED earth frame (m/s/s)
src/modules/ekf2/EKF/mag_control.cpp
+48 -32
View File
@@ -76,6 +76,13 @@ void Ekf::controlMagFusion()
_num_bad_flight_yaw_events = 0;
}
checkYawAngleObservability();
checkMagBiasObservability();
if (_mag_bias_observable || _yaw_angle_observable) {
_time_last_mov_3d_mag_suitable = _imu_sample_delayed.time_us;
}
// When operating without a magnetometer and no other source of yaw aiding is active,
// yaw fusion is run selectively to enable yaw gyro bias learning when stationary on
// ground and to prevent uncontrolled yaw variance growth
@@ -109,7 +116,17 @@ void Ekf::controlMagFusion()
// FALLTHROUGH
case MAG_FUSE_TYPE_AUTO:
selectMagAuto();
// Use of 3D fusion requires an in-air heading alignment but it should not
// be used when the heading and mag biases are not observable for more than 2 seconds
if (_control_status.flags.mag_aligned_in_flight
&& ((_imu_sample_delayed.time_us - _time_last_mov_3d_mag_suitable) < (uint64_t)2e6)
) {
startMag3DFusion();
} else {
startMagHdgFusion();
}
break;
case MAG_FUSE_TYPE_INDOOR:
@@ -206,36 +223,44 @@ void Ekf::runYawReset()
}
}
void Ekf::selectMagAuto()
{
check3DMagFusionSuitability();
canUse3DMagFusion() ? startMag3DFusion() : startMagHdgFusion();
}
void Ekf::check3DMagFusionSuitability()
{
checkYawAngleObservability();
checkMagBiasObservability();
if (_mag_bias_observable || _yaw_angle_observable) {
_time_last_mov_3d_mag_suitable = _imu_sample_delayed.time_us;
}
}
void Ekf::checkYawAngleObservability()
{
// calculate a filtered horizontal acceleration with a 1 sec time constant
// Calculate an earth frame delta velocity
const Vector3f corrected_delta_vel = _imu_sample_delayed.delta_vel - _state.delta_vel_bias;
const Vector3f corrected_delta_vel_ef = _R_to_earth * corrected_delta_vel;
const float alpha = 1.0f - _imu_sample_delayed.delta_vel_dt;
_accel_lpf_NE = _accel_lpf_NE * alpha + corrected_delta_vel_ef.xy();
// Check if there has been enough change in horizontal velocity to make yaw observable
// Apply hysteresis to check to avoid rapid toggling
_yaw_angle_observable = _yaw_angle_observable
? _accel_lpf_NE.norm() > _params.mag_acc_gate
: _accel_lpf_NE.norm() > 2.0f * _params.mag_acc_gate;
if (_control_status.flags.gps) {
if (_yaw_angle_observable) {
_yaw_angle_observable = _accel_lpf_NE.norm() > _params.mag_acc_gate;
_yaw_angle_observable = _yaw_angle_observable
&& (_control_status.flags.gps || _control_status.flags.ev_pos); // Do we have to add ev_vel here?
} else {
_yaw_angle_observable = _accel_lpf_NE.norm() > _params.mag_acc_gate * 2.f;
}
} else {
_yaw_angle_observable = false;
}
}
void Ekf::checkMagBiasObservability()
{
// calculate a yaw change about the earth frame vertical
const Vector3f delta_angle = _imu_sample_delayed.delta_ang - _state.delta_ang_bias;
const float spin_del_ang_D = delta_angle.dot(Vector3f(_R_to_earth.row(2)));
float yaw_delta_ef = spin_del_ang_D;
// Calculate filtered yaw rate to be used by the magnetometer fusion type selection logic
// Note fixed coefficients are used to save operations. The exact time constant is not important.
_yaw_rate_lpf_ef = 0.95f * _yaw_rate_lpf_ef + 0.05f * spin_del_ang_D / _imu_sample_delayed.delta_ang_dt;
// check if there is enough yaw rotation to make the mag bias states observable
if (!_mag_bias_observable && (fabsf(_yaw_rate_lpf_ef) > _params.mag_yaw_rate_gate)) {
// initial yaw motion is detected
@@ -245,21 +270,12 @@ void Ekf::checkMagBiasObservability()
// require sustained yaw motion of 50% the initial yaw rate threshold
const float yaw_dt = 1e-6f * (float)(_imu_sample_delayed.time_us - _time_yaw_started);
const float min_yaw_change_req = 0.5f * _params.mag_yaw_rate_gate * yaw_dt;
_mag_bias_observable = fabsf(_yaw_delta_ef) > min_yaw_change_req;
_mag_bias_observable = fabsf(yaw_delta_ef) > min_yaw_change_req;
}
_yaw_delta_ef = 0.0f;
_time_yaw_started = _imu_sample_delayed.time_us;
}
bool Ekf::canUse3DMagFusion() const
{
// Use of 3D fusion requires an in-air heading alignment but it should not
// be used when the heading and mag biases are not observable for more than 2 seconds
return _control_status.flags.mag_aligned_in_flight
&& ((_imu_sample_delayed.time_us - _time_last_mov_3d_mag_suitable) < (uint64_t)2e6);
}
void Ekf::checkMagDeclRequired()
{
// if we are using 3-axis magnetometer fusion, but without external NE aiding,