mirror of
https://gitee.com/mirrors_PX4/PX4-Autopilot.git
synced 2026-05-18 04:17:35 +08:00
Use resetQuatStateYaw during gps yaw reset
This commit is contained in:
Kamil Ritz
committed by
Mathieu Bresciani
Mathieu Bresciani
parent
7f21364844
commit
21f49c22dc
+6
-98
@@ -309,105 +309,13 @@ bool Ekf::resetGpsAntYaw()
|
|||||||
const float measured_yaw = _gps_sample_delayed.yaw + _gps_yaw_offset;
|
const float measured_yaw = _gps_sample_delayed.yaw + _gps_yaw_offset;
|
||||||
|
|
||||||
// calculate the amount the yaw needs to be rotated by
|
// calculate the amount the yaw needs to be rotated by
|
||||||
float yaw_delta = wrap_pi(measured_yaw - predicted_yaw);
|
const float yaw_delta = wrap_pi(measured_yaw - predicted_yaw);
|
||||||
|
|
||||||
// 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;
|
|
||||||
Quatf quat_after_reset;
|
|
||||||
|
|
||||||
// obtain the yaw angle using the best conditioned from either a Tait-Bryan 321 or 312 sequence
|
|
||||||
// to avoid gimbal lock
|
|
||||||
if (fabsf(_R_to_earth(2, 0)) < fabsf(_R_to_earth(2, 1))) {
|
|
||||||
// get the roll, pitch, yaw estimates from the quaternion states using a 321 Tait-Bryan rotation sequence
|
|
||||||
Eulerf euler_init(_state.quat_nominal);
|
|
||||||
|
|
||||||
// correct the yaw angle
|
|
||||||
euler_init(2) += yaw_delta;
|
|
||||||
euler_init(2) = wrap_pi(euler_init(2));
|
|
||||||
|
|
||||||
quat_after_reset = Quatf(euler_init);
|
|
||||||
|
|
||||||
} else {
|
|
||||||
// Calculate the 312 Tait-Bryan sequence euler angles that rotate from earth to body frame
|
|
||||||
// PX4 math library does not support this so are using equations from
|
|
||||||
// http://www.atacolorado.com/eulersequences.doc
|
|
||||||
Vector3f euler312;
|
|
||||||
euler312(0) = atan2f(-_R_to_earth(0, 1), _R_to_earth(1, 1)); // first rotation (yaw)
|
|
||||||
euler312(1) = asinf(_R_to_earth(2, 1)); // second rotation (roll)
|
|
||||||
euler312(2) = atan2f(-_R_to_earth(2, 0), _R_to_earth(2, 2)); // third rotation (pitch)
|
|
||||||
|
|
||||||
// correct the yaw angle
|
|
||||||
euler312(0) += yaw_delta;
|
|
||||||
euler312(0) = wrap_pi(euler312(0));
|
|
||||||
|
|
||||||
// Calculate the body to earth frame rotation matrix from the corrected euler angles
|
|
||||||
float c2 = cosf(euler312(2));
|
|
||||||
float s2 = sinf(euler312(2));
|
|
||||||
float s1 = sinf(euler312(1));
|
|
||||||
float c1 = cosf(euler312(1));
|
|
||||||
float s0 = sinf(euler312(0));
|
|
||||||
float c0 = cosf(euler312(0));
|
|
||||||
|
|
||||||
Dcmf R_to_earth;
|
|
||||||
R_to_earth(0, 0) = c0 * c2 - s0 * s1 * s2;
|
|
||||||
R_to_earth(1, 1) = c0 * c1;
|
|
||||||
R_to_earth(2, 2) = c2 * c1;
|
|
||||||
R_to_earth(0, 1) = -c1 * s0;
|
|
||||||
R_to_earth(0, 2) = s2 * c0 + c2 * s1 * s0;
|
|
||||||
R_to_earth(1, 0) = c2 * s0 + s2 * s1 * c0;
|
|
||||||
R_to_earth(1, 2) = s0 * s2 - s1 * c0 * c2;
|
|
||||||
R_to_earth(2, 0) = -s2 * c1;
|
|
||||||
R_to_earth(2, 1) = s1;
|
|
||||||
|
|
||||||
// update the quaternions
|
|
||||||
quat_after_reset = Quatf(R_to_earth);
|
|
||||||
}
|
|
||||||
|
|
||||||
// calculate the amount that the quaternion has changed by
|
|
||||||
const Quatf q_error( (_state.quat_nominal * quat_before_reset.inversed()).normalized() );
|
|
||||||
|
|
||||||
// convert the quaternion delta to a delta angle
|
|
||||||
Vector3f delta_ang_error;
|
|
||||||
float scalar;
|
|
||||||
|
|
||||||
if (q_error(0) >= 0.0f) {
|
|
||||||
scalar = -2.0f;
|
|
||||||
|
|
||||||
} else {
|
|
||||||
scalar = 2.0f;
|
|
||||||
}
|
|
||||||
|
|
||||||
delta_ang_error(0) = scalar * q_error(1);
|
|
||||||
delta_ang_error(1) = scalar * q_error(2);
|
|
||||||
delta_ang_error(2) = scalar * q_error(3);
|
|
||||||
|
|
||||||
// update the quaternion state estimates and corresponding covariances only if the change in angle has been large or the yaw is not yet aligned
|
|
||||||
if (delta_ang_error.norm() > math::radians(15.0f) || !_control_status.flags.yaw_align) {
|
|
||||||
// update quaternion states
|
|
||||||
_state.quat_nominal = quat_after_reset;
|
|
||||||
uncorrelateQuatFromOtherStates();
|
|
||||||
|
|
||||||
// record the state change
|
|
||||||
_state_reset_status.quat_change = q_error;
|
|
||||||
|
|
||||||
// update transformation matrix from body to world frame using the current estimate
|
|
||||||
_R_to_earth = Dcmf(_state.quat_nominal);
|
|
||||||
|
|
||||||
// update the yaw angle variance using the variance of the measurement
|
|
||||||
increaseQuatYawErrVariance(sq(fmaxf(_params.mag_heading_noise, 1.0e-2f)));
|
|
||||||
|
|
||||||
// add the reset amount to the output observer buffered data
|
|
||||||
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;
|
|
||||||
|
|
||||||
// capture the reset event
|
|
||||||
_state_reset_status.quat_counter++;
|
|
||||||
|
|
||||||
|
// update the quaternion state estimates and corresponding covariances only if
|
||||||
|
// the change in angle has been large or the yaw is not yet aligned
|
||||||
|
if(fabs(yaw_delta) > math::radians(15.0f) || !_control_status.flags.yaw_align) {
|
||||||
|
const float yaw_variance = sq(fmaxf(_params.mag_heading_noise, 1.0e-2f));
|
||||||
|
resetQuatStateYaw(measured_yaw, yaw_variance, true);
|
||||||
}
|
}
|
||||||
|
|
||||||
return true;
|
return true;
|
||||||
|
|||||||
Reference in New Issue
Block a user