From fce4958d4f31265297f8b9f553dea95c2a787c54 Mon Sep 17 00:00:00 2001 From: kamilritz Date: Sat, 27 Jun 2020 11:49:20 +0200 Subject: [PATCH] Random cleanup --- EKF/ekf.cpp | 9 +---- EKF/ekf_helper.cpp | 91 +++++++++++++++++++++++----------------------- EKF/gps_checks.cpp | 4 +- 3 files changed, 49 insertions(+), 55 deletions(-) diff --git a/EKF/ekf.cpp b/EKF/ekf.cpp index 770dee41fa..18bd27431b 100644 --- a/EKF/ekf.cpp +++ b/EKF/ekf.cpp @@ -408,14 +408,7 @@ void Ekf::calculateOutputStates() const Quatf q_error( (_state.quat_nominal.inversed() * _output_sample_delayed.quat_nominal).normalized() ); // convert the quaternion delta to a delta angle - float scalar; - - if (q_error(0) >= 0.0f) { - scalar = -2.0f; - - } else { - scalar = 2.0f; - } + const float scalar = (q_error(0) >= 0.0f) ? -2.f : 2.f; const Vector3f delta_ang_error{scalar * q_error(1), scalar * q_error(2), scalar * q_error(3)}; diff --git a/EKF/ekf_helper.cpp b/EKF/ekf_helper.cpp index 30cca7f95b..acb2288a8d 100644 --- a/EKF/ekf_helper.cpp +++ b/EKF/ekf_helper.cpp @@ -1621,62 +1621,62 @@ void Ekf::stopFlowFusion() void Ekf::resetQuatStateYaw(float yaw, float yaw_variance, bool update_buffer) { - // save a copy of the quaternion state for later use in calculating the amount of reset change - const Quatf quat_before_reset = _state.quat_nominal; + // save a copy of the quaternion state for later use in calculating the amount of reset change + const Quatf quat_before_reset = _state.quat_nominal; - // update transformation matrix from body to world frame using the current estimate - _R_to_earth = Dcmf(_state.quat_nominal); + // update transformation matrix from body to world frame using the current estimate + _R_to_earth = Dcmf(_state.quat_nominal); - // update the rotation matrix using the new yaw value - // determine if a 321 or 312 Euler sequence is best - if (fabsf(_R_to_earth(2, 0)) < fabsf(_R_to_earth(2, 1))) { - // use a 321 sequence - Eulerf euler321(_R_to_earth); - euler321(2) = yaw; - _R_to_earth = Dcmf(euler321); + // update the rotation matrix using the new yaw value + // determine if a 321 or 312 Euler sequence is best + if (fabsf(_R_to_earth(2, 0)) < fabsf(_R_to_earth(2, 1))) { + // use a 321 sequence + Eulerf euler321(_R_to_earth); + euler321(2) = yaw; + _R_to_earth = Dcmf(euler321); - } else { - // Calculate the 312 Tait-Bryan rotation sequence that rotates from earth to body frame - // We use a 312 sequence as an alternate when there is more pitch tilt than roll tilt - // to avoid gimbal lock - Vector3f rot312; - rot312(0) = yaw; - rot312(1) = asinf(_R_to_earth(2, 1)); - rot312(2) = atan2f(-_R_to_earth(2, 0), _R_to_earth(2, 2)); - _R_to_earth = taitBryan312ToRotMat(rot312); + } else { + // Calculate the 312 Tait-Bryan rotation sequence that rotates from earth to body frame + // We use a 312 sequence as an alternate when there is more pitch tilt than roll tilt + // to avoid gimbal lock + Vector3f rot312; + rot312(0) = yaw; + rot312(1) = asinf(_R_to_earth(2, 1)); + rot312(2) = atan2f(-_R_to_earth(2, 0), _R_to_earth(2, 2)); + _R_to_earth = taitBryan312ToRotMat(rot312); - } + } - // calculate the amount that the quaternion has changed by - const Quatf quat_after_reset(_R_to_earth); - const Quatf q_error((quat_after_reset * quat_before_reset.inversed()).normalized()); + // calculate the amount that the quaternion has changed by + const Quatf quat_after_reset(_R_to_earth); + const Quatf q_error((quat_after_reset * quat_before_reset.inversed()).normalized()); - // update quaternion states - _state.quat_nominal = quat_after_reset; - uncorrelateQuatFromOtherStates(); + // update quaternion states + _state.quat_nominal = quat_after_reset; + uncorrelateQuatFromOtherStates(); - // record the state change - _state_reset_status.quat_change = q_error; + // record the state change + _state_reset_status.quat_change = q_error; - // update the yaw angle variance - if (yaw_variance > FLT_EPSILON) { - increaseQuatYawErrVariance(yaw_variance); - } + // update the yaw angle variance + if (yaw_variance > FLT_EPSILON) { + increaseQuatYawErrVariance(yaw_variance); + } - // add the reset amount to the output observer buffered data - if (update_buffer) { - for (uint8_t i = 0; i < _output_buffer.get_length(); i++) { - _output_buffer[i].quat_nominal = _state_reset_status.quat_change * _output_buffer[i].quat_nominal; - } + // add the reset amount to the output observer buffered data + if (update_buffer) { + for (uint8_t i = 0; i < _output_buffer.get_length(); i++) { + _output_buffer[i].quat_nominal = _state_reset_status.quat_change * _output_buffer[i].quat_nominal; + } - // apply the change in attitude quaternion to our newest quaternion estimate - // which was already taken out from the output buffer - _output_new.quat_nominal = _state_reset_status.quat_change * _output_new.quat_nominal; + // apply the change in attitude quaternion to our newest quaternion estimate + // which was already taken out from the output buffer + _output_new.quat_nominal = _state_reset_status.quat_change * _output_new.quat_nominal; - } + } - // capture the reset event - _state_reset_status.quat_counter++; + // capture the reset event + _state_reset_status.quat_counter++; } // Resets the main Nav EKf yaw to the estimator from the EKF-GSF yaw estimator @@ -1738,7 +1738,8 @@ void Ekf::runYawEKFGSF() yawEstimator.update(_imu_sample_delayed, _control_status.flags.in_air, TAS, imu_gyro_bias); // basic sanity check on GPS velocity data - if (_gps_data_ready && _gps_sample_delayed.vacc > FLT_EPSILON && ISFINITE(_gps_sample_delayed.vel(0)) && ISFINITE(_gps_sample_delayed.vel(1))) { + if (_gps_data_ready && _gps_sample_delayed.vacc > FLT_EPSILON && + ISFINITE(_gps_sample_delayed.vel(0)) && ISFINITE(_gps_sample_delayed.vel(1))) { yawEstimator.setVelocity(_gps_sample_delayed.vel.xy(), _gps_sample_delayed.vacc); } } diff --git a/EKF/gps_checks.cpp b/EKF/gps_checks.cpp index e0ccb8beb5..3113dd84f5 100644 --- a/EKF/gps_checks.cpp +++ b/EKF/gps_checks.cpp @@ -62,8 +62,8 @@ bool Ekf::collect_gps(const gps_message &gps) _gps_checks_passed = gps_is_good(gps); if (!_NED_origin_initialised && _gps_checks_passed) { // If we have good GPS data set the origin's WGS-84 position to the last gps fix - const double lat = gps.lat / 1.0e7; - const double lon = gps.lon / 1.0e7; + const double lat = gps.lat * 1.0e-7; + const double lon = gps.lon * 1.0e-7; map_projection_init_timestamped(&_pos_ref, lat, lon, _time_last_imu); // if we are already doing aiding, correct for the change in position since the EKF started navigationg