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

ekf2: move Ekf::resetQuatStateYaw() to yaw_fusion.cpp

Browse Source
This commit is contained in:
Daniel Agar
2024-05-02 17:40:58 -04:00
parent 1ca4056b6a
commit 63c2ea33c1
2 changed files with 47 additions and 47 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/ekf2/EKF/ekf_helper.cpp
-47
View File
@@ -652,53 +652,6 @@ void Ekf::updateGroundEffect()
}
#endif // CONFIG_EKF2_BAROMETER
void Ekf::resetQuatStateYaw(float yaw, float yaw_variance)
{
// 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 the yaw angle variance
if (PX4_ISFINITE(yaw_variance) && (yaw_variance > FLT_EPSILON)) {
P.uncorrelateCovarianceSetVariance<1>(2, yaw_variance);
}
// update transformation matrix from body to world frame using the current estimate
// update the rotation matrix using the new yaw value
_R_to_earth = updateYawInRotMat(yaw, Dcmf(_state.quat_nominal));
// 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;
// add the reset amount to the output observer buffered data
_output_predictor.resetQuaternion(q_error);
#if defined(CONFIG_EKF2_EXTERNAL_VISION)
// update EV attitude error filter
if (_ev_q_error_initialized) {
const Quatf ev_q_error_updated = (q_error * _ev_q_error_filt.getState()).normalized();
_ev_q_error_filt.reset(ev_q_error_updated);
}
#endif // CONFIG_EKF2_EXTERNAL_VISION
// record the state change
if (_state_reset_status.reset_count.quat == _state_reset_count_prev.quat) {
_state_reset_status.quat_change = q_error;
} else {
// there's already a reset this update, accumulate total delta
_state_reset_status.quat_change = q_error * _state_reset_status.quat_change;
_state_reset_status.quat_change.normalize();
}
_state_reset_status.reset_count.quat++;
_time_last_heading_fuse = _time_delayed_us;
}
#if defined(CONFIG_EKF2_WIND)
void Ekf::resetWind()
{
src/modules/ekf2/EKF/yaw_fusion.cpp
+47
View File
@@ -136,3 +136,50 @@ void Ekf::computeYawInnovVarAndH(float variance, float &innovation_variance, Vec
{
sym::ComputeYawInnovVarAndH(_state.vector(), P, variance, &innovation_variance, &H_YAW);
}
void Ekf::resetQuatStateYaw(float yaw, float yaw_variance)
{
// 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 the yaw angle variance
if (PX4_ISFINITE(yaw_variance) && (yaw_variance > FLT_EPSILON)) {
P.uncorrelateCovarianceSetVariance<1>(2, yaw_variance);
}
// update transformation matrix from body to world frame using the current estimate
// update the rotation matrix using the new yaw value
_R_to_earth = updateYawInRotMat(yaw, Dcmf(_state.quat_nominal));
// 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;
// add the reset amount to the output observer buffered data
_output_predictor.resetQuaternion(q_error);
#if defined(CONFIG_EKF2_EXTERNAL_VISION)
// update EV attitude error filter
if (_ev_q_error_initialized) {
const Quatf ev_q_error_updated = (q_error * _ev_q_error_filt.getState()).normalized();
_ev_q_error_filt.reset(ev_q_error_updated);
}
#endif // CONFIG_EKF2_EXTERNAL_VISION
// record the state change
if (_state_reset_status.reset_count.quat == _state_reset_count_prev.quat) {
_state_reset_status.quat_change = q_error;
} else {
// there's already a reset this update, accumulate total delta
_state_reset_status.quat_change = q_error * _state_reset_status.quat_change;
_state_reset_status.quat_change.normalize();
}
_state_reset_status.reset_count.quat++;
_time_last_heading_fuse = _time_delayed_us;
}