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

EKF: Remove use of of quaternion self product operator and fix delta rotation sign

This commit is contained in:
Paul Riseborough 2019-06-14 10:00:45 +10:00 committed by Paul Riseborough
parent 0e946f25fd
commit a036cf82cc
3 changed files with 9 additions and 11 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

5
EKF/control.cpp
View File

@ -226,12 +226,11 @@ void Ekf::controlExternalVisionFusion()
increaseQuatYawErrVariance(sq(fmaxf(_ev_sample_delayed.angErr, 1.0e-2f)));
// calculate the amount that the quaternion has changed by
_state_reset_status.quat_change = quat_before_reset.inversed() * _state.quat_nominal;
_state_reset_status.quat_change = _state.quat_nominal * quat_before_reset.inversed();
// add the reset amount to the output observer buffered data
// Note q1 *= q2 is equivalent to q1 = q2 * q1
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 = _state_reset_status.quat_change * _output_buffer[i].quat_nominal;
}
// apply the change in attitude quaternion to our newest quaternion estimate

10
EKF/ekf_helper.cpp
View File

@ -378,8 +378,8 @@ void Ekf::resetHeight()
// align output filter states to match EKF states at the fusion time horizon
void Ekf::alignOutputFilter()
{
// calculate the quaternion delta between the output and EKF quaternions at the EKF fusion time horizon
Quatf q_delta = _output_sample_delayed.quat_nominal.inversed() * _state.quat_nominal;
// calculate the quaternion rotation delta from the EKF to output observer states at the EKF fusion time horizon
Quatf q_delta = _state.quat_nominal * _output_sample_delayed.quat_nominal.inversed();
q_delta.normalize();
// calculate the velocity and position deltas between the output and EKF at the EKF fusion time horizon
@ -388,16 +388,16 @@ void Ekf::alignOutputFilter()
// loop through the output filter state history and add the deltas
for (uint8_t i = 0; i < _output_buffer.get_length(); i++) {
_output_buffer[i].quat_nominal *= q_delta;
_output_buffer[i].quat_nominal = q_delta * _output_buffer[i].quat_nominal;
_output_buffer[i].quat_nominal.normalize();
_output_buffer[i].vel += vel_delta;
_output_buffer[i].pos += pos_delta;
}
_output_new.quat_nominal *= q_delta;
_output_new.quat_nominal = q_delta * _output_new.quat_nominal;
_output_new.quat_nominal.normalize();
_output_sample_delayed.quat_nominal *= q_delta;
_output_sample_delayed.quat_nominal = q_delta * _output_sample_delayed.quat_nominal;
_output_sample_delayed.quat_nominal.normalize();
}

5
EKF/gps_yaw_fusion.cpp
View File

@ -367,7 +367,7 @@ bool Ekf::resetGpsAntYaw()
}
// calculate the amount that the quaternion has changed by
Quatf q_error = quat_before_reset.inversed() * _state.quat_nominal;
Quatf q_error = _state.quat_nominal * quat_before_reset.inversed();
q_error.normalize();
// convert the quaternion delta to a delta angle
@ -407,8 +407,7 @@ bool Ekf::resetGpsAntYaw()
// add the reset amount to the output observer buffered data
for (uint8_t i = 0; i < _output_buffer.get_length(); i++) {
// Note q1 *= q2 is equivalent to q1 = q2 * q1
_output_buffer[i].quat_nominal *= _state_reset_status.quat_change;
_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