dark0707
/
px4-firmware
forked from Archive/PX4-Autopilot
1
0
Fork 0
Code Pull Requests Activity

GPS Yaw: move isfinite checks in control.cpp

This commit is contained in:
bresch 2020-06-22 14:33:01 +02:00 committed by Mathieu Bresciani
parent 3c6790f5d5
commit fe2a9d3018
3 changed files with 100 additions and 108 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

35
EKF/control.cpp
View File

@ -518,28 +518,31 @@ void Ekf::controlGpsFusion()
// Detect if coming back after significant time without GPS data
bool gps_signal_was_lost = isTimedOut(_time_prev_gps_us, 1000000);
// GPS yaw aiding selection logic
if ((_params.fusion_mode & MASK_USE_GPSYAW)
&& ISFINITE(_gps_sample_delayed.yaw)
&& _control_status.flags.tilt_align
&& !_control_status.flags.gps_yaw
&& !_gps_hgt_intermittent) {
if (!(_params.fusion_mode & MASK_USE_GPSYAW)) {
stopGpsYawFusion();
if (resetGpsAntYaw()) {
// flag the yaw as aligned
_control_status.flags.yaw_align = true;
} else {
if (ISFINITE(_gps_sample_delayed.yaw)) {
if (!_control_status.flags.gps_yaw
&& _control_status.flags.tilt_align
&& !_gps_hgt_intermittent) {
startGpsYawFusion();
// Activate GPS yaw fusion
if (resetGpsAntYaw()) {
_control_status.flags.yaw_align = true;
ECL_INFO_TIMESTAMPED("starting GPS yaw fusion");
startGpsYawFusion();
ECL_INFO_TIMESTAMPED("starting GPS yaw fusion");
}
}
if (_control_status.flags.gps_yaw) {
fuseGpsAntYaw();
}
}
}
// fuse the yaw observation
if (_control_status.flags.gps_yaw) {
fuseGpsAntYaw();
}
// Determine if we should use GPS aiding for velocity and horizontal position
// To start using GPS we need angular alignment completed, the local NED origin set and GPS data that has not failed checks recently
bool gps_checks_passing = isTimedOut(_last_gps_fail_us, (uint64_t)5e6);

2
EKF/ekf.h
View File

@ -351,6 +351,8 @@ private:
uint64_t _time_last_beta_fuse{0}; ///< time the last fusion of synthetic sideslip measurements were performed (uSec)
uint64_t _time_last_fake_pos{0}; ///< last time we faked position measurements to constrain tilt errors during operation without external aiding (uSec)
uint64_t _time_last_gps_yaw_fuse{0}; ///< time the last fusion of GPS yaw measurements were performed (uSec)
Vector2f _last_known_posNE; ///< last known local NE position vector (m)
float _imu_collection_time_adj{0.0f}; ///< the amount of time the IMU collection needs to be advanced to meet the target set by FILTER_UPDATE_PERIOD_MS (sec)

171
EKF/gps_yaw_fusion.cpp
View File

@ -58,83 +58,76 @@ void Ekf::fuseGpsAntYaw()
float H_YAW[4];
float measured_hdg;
// check if data has been set to NAN indicating no measurement
if (ISFINITE(_gps_sample_delayed.yaw)) {
// calculate the observed yaw angle of antenna array, converting a from body to antenna yaw measurement
measured_hdg = _gps_sample_delayed.yaw + _gps_yaw_offset;
// calculate the observed yaw angle of antenna array, converting a from body to antenna yaw measurement
measured_hdg = _gps_sample_delayed.yaw + _gps_yaw_offset;
// define the predicted antenna array vector and rotate into earth frame
Vector3f ant_vec_bf = {cosf(_gps_yaw_offset), sinf(_gps_yaw_offset), 0.0f};
Vector3f ant_vec_ef = _R_to_earth * ant_vec_bf;
// define the predicted antenna array vector and rotate into earth frame
Vector3f ant_vec_bf = {cosf(_gps_yaw_offset), sinf(_gps_yaw_offset), 0.0f};
Vector3f ant_vec_ef = _R_to_earth * ant_vec_bf;
// check if antenna array vector is within 30 degrees of vertical and therefore unable to provide a reliable heading
if (fabsf(ant_vec_ef(2)) > cosf(math::radians(30.0f))) {
return;
}
// calculate predicted antenna yaw angle
predicted_hdg = atan2f(ant_vec_ef(1),ant_vec_ef(0));
// calculate observation jacobian
float t2 = sinf(_gps_yaw_offset);
float t3 = cosf(_gps_yaw_offset);
float t4 = q0*q3*2.0f;
float t5 = q0*q0;
float t6 = q1*q1;
float t7 = q2*q2;
float t8 = q3*q3;
float t9 = q1*q2*2.0f;
float t10 = t5+t6-t7-t8;
float t11 = t3*t10;
float t12 = t4+t9;
float t13 = t3*t12;
float t14 = t5-t6+t7-t8;
float t15 = t2*t14;
float t16 = t13+t15;
float t17 = t4-t9;
float t19 = t2*t17;
float t20 = t11-t19;
float t18 = (t20*t20);
if (t18 < 1e-6f) {
return;
}
t18 = 1.0f / t18;
float t21 = t16*t16;
float t22 = sq(t11-t19);
if (t22 < 1e-6f) {
return;
}
t22 = 1.0f/t22;
float t23 = q1*t3*2.0f;
float t24 = q2*t2*2.0f;
float t25 = t23+t24;
float t26 = 1.0f/t20;
float t27 = q1*t2*2.0f;
float t28 = t21*t22;
float t29 = t28+1.0f;
if (fabsf(t29) < 1e-6f) {
return;
}
float t30 = 1.0f/t29;
float t31 = q0*t3*2.0f;
float t32 = t31-q3*t2*2.0f;
float t33 = q3*t3*2.0f;
float t34 = q0*t2*2.0f;
float t35 = t33+t34;
H_YAW[0] = (t35/(t11-t2*(t4-q1*q2*2.0f))-t16*t18*t32)/(t18*t21+1.0f);
H_YAW[1] = -t30*(t26*(t27-q2*t3*2.0f)+t16*t22*t25);
H_YAW[2] = t30*(t25*t26-t16*t22*(t27-q2*t3*2.0f));
H_YAW[3] = t30*(t26*t32+t16*t22*t35);
// using magnetic heading tuning parameter
R_YAW = sq(fmaxf(_params.mag_heading_noise, 1.0e-2f));
} else {
// there is nothing to fuse
// check if antenna array vector is within 30 degrees of vertical and therefore unable to provide a reliable heading
if (fabsf(ant_vec_ef(2)) > cosf(math::radians(30.0f))) {
return;
}
// calculate predicted antenna yaw angle
predicted_hdg = atan2f(ant_vec_ef(1),ant_vec_ef(0));
// calculate observation jacobian
float t2 = sinf(_gps_yaw_offset);
float t3 = cosf(_gps_yaw_offset);
float t4 = q0*q3*2.0f;
float t5 = q0*q0;
float t6 = q1*q1;
float t7 = q2*q2;
float t8 = q3*q3;
float t9 = q1*q2*2.0f;
float t10 = t5+t6-t7-t8;
float t11 = t3*t10;
float t12 = t4+t9;
float t13 = t3*t12;
float t14 = t5-t6+t7-t8;
float t15 = t2*t14;
float t16 = t13+t15;
float t17 = t4-t9;
float t19 = t2*t17;
float t20 = t11-t19;
float t18 = (t20*t20);
if (t18 < 1e-6f) {
return;
}
t18 = 1.0f / t18;
float t21 = t16*t16;
float t22 = sq(t11-t19);
if (t22 < 1e-6f) {
return;
}
t22 = 1.0f/t22;
float t23 = q1*t3*2.0f;
float t24 = q2*t2*2.0f;
float t25 = t23+t24;
float t26 = 1.0f/t20;
float t27 = q1*t2*2.0f;
float t28 = t21*t22;
float t29 = t28+1.0f;
if (fabsf(t29) < 1e-6f) {
return;
}
float t30 = 1.0f/t29;
float t31 = q0*t3*2.0f;
float t32 = t31-q3*t2*2.0f;
float t33 = q3*t3*2.0f;
float t34 = q0*t2*2.0f;
float t35 = t33+t34;
H_YAW[0] = (t35/(t11-t2*(t4-q1*q2*2.0f))-t16*t18*t32)/(t18*t21+1.0f);
H_YAW[1] = -t30*(t26*(t27-q2*t3*2.0f)+t16*t22*t25);
H_YAW[2] = t30*(t25*t26-t16*t22*(t27-q2*t3*2.0f));
H_YAW[3] = t30*(t26*t32+t16*t22*t35);
// using magnetic heading tuning parameter
R_YAW = sq(fmaxf(_params.mag_heading_noise, 1.0e-2f));
// wrap the heading to the interval between +-pi
measured_hdg = wrap_pi(measured_hdg);
@ -210,7 +203,6 @@ void Ekf::fuseGpsAntYaw()
// we are no longer using 3-axis fusion so set the reported test levels to zero
memset(_mag_test_ratio, 0, sizeof(_mag_test_ratio));
// set the magnetometer unhealthy if the test fails
if (_yaw_test_ratio > 1.0f) {
_innov_check_fail_status.flags.reject_yaw = true;
@ -273,6 +265,7 @@ void Ekf::fuseGpsAntYaw()
// only apply covariance and state corrections if healthy
if (healthy) {
_time_last_gps_yaw_fuse = _time_last_imu;
// apply the covariance corrections
for (unsigned row = 0; row < _k_num_states; row++) {
for (unsigned column = 0; column < _k_num_states; column++) {
@ -291,26 +284,20 @@ void Ekf::fuseGpsAntYaw()
bool Ekf::resetGpsAntYaw()
{
// check if data has been set to NAN indicating no measurement
if (ISFINITE(_gps_sample_delayed.yaw)) {
// define the predicted antenna array vector and rotate into earth frame
const Vector3f ant_vec_bf = {cosf(_gps_yaw_offset), sinf(_gps_yaw_offset), 0.0f};
const Vector3f ant_vec_ef = _R_to_earth * ant_vec_bf;
// define the predicted antenna array vector and rotate into earth frame
const Vector3f ant_vec_bf = {cosf(_gps_yaw_offset), sinf(_gps_yaw_offset), 0.0f};
const Vector3f ant_vec_ef = _R_to_earth * ant_vec_bf;
// check if antenna array vector is within 30 degrees of vertical and therefore unable to provide a reliable heading
if (fabsf(ant_vec_ef(2)) > cosf(math::radians(30.0f))) {
return false;
}
// get measurement and correct for antenna array yaw offset
const float measured_yaw = _gps_sample_delayed.yaw + _gps_yaw_offset;
const float yaw_variance = sq(fmaxf(_params.mag_heading_noise, 1.0e-2f));
resetQuatStateYaw(measured_yaw, yaw_variance, true);
return true;
// check if antenna array vector is within 30 degrees of vertical and therefore unable to provide a reliable heading
if (fabsf(ant_vec_ef(2)) > cosf(math::radians(30.0f))) {
return false;
}
return false;
// get measurement and correct for antenna array yaw offset
const float measured_yaw = _gps_sample_delayed.yaw + _gps_yaw_offset;
const float yaw_variance = sq(fmaxf(_params.mag_heading_noise, 1.0e-2f));
resetQuatStateYaw(measured_yaw, yaw_variance, true);
return true;
}