fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-08-21 11:30:34 +08:00
Code Issues Packages Projects Releases Wiki Activity

Merge pull request #323 from PX4/pr-ekfQuatMultOrder

Browse Source 
EKF: use hamiltonian convention for quaternion multiplication order
This commit is contained in:
Paul Riseborough
2017-09-21 07:49:35 +10:00
committed by GitHub
parent 4411c7fd1c dd5b8525c3
commit 160e4d69c1
4 changed files with 18 additions and 18 deletions
Download Patch File Download Diff File Expand all files Collapse all files
EKF/control.cpp
+3 -3
View File
@@ -187,20 +187,20 @@ void Ekf::controlExternalVisionFusion()
_state.quat_nominal = Quatf(euler_init);
// calculate the amount that the quaternion has changed by
_state_reset_status.quat_change = _state.quat_nominal * quat_before_reset.inversed();
_state_reset_status.quat_change = quat_before_reset.inversed() * _state.quat_nominal;
// add the reset amount to the output observer buffered data
outputSample output_states;
unsigned output_length = _output_buffer.get_length();
for (unsigned i=0; i < output_length; i++) {
output_states = _output_buffer.get_from_index(i);
output_states.quat_nominal *= _state_reset_status.quat_change;
output_states.quat_nominal = _state_reset_status.quat_change * output_states.quat_nominal;
_output_buffer.push_to_index(i, output_states);
}
// 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 = _state_reset_status.quat_change * _output_new.quat_nominal;
// capture the reset event
_state_reset_status.quat_counter++;
EKF/ekf.cpp
+3 -3
View File
@@ -329,7 +329,7 @@ void Ekf::predictState()
dq.from_axis_angle(corrected_delta_ang);
// rotate the previous quaternion by the delta quaternion using a quaternion multiplication
_state.quat_nominal = dq * _state.quat_nominal;
_state.quat_nominal = _state.quat_nominal * dq;
// quaternions must be normalised whenever they are modified
_state.quat_nominal.normalize();
@@ -471,7 +471,7 @@ void Ekf::calculateOutputStates()
// rotate the previous INS quaternion by the delta quaternions
_output_new.time_us = imu_new.time_us;
_output_new.quat_nominal = dq * _output_new.quat_nominal;
_output_new.quat_nominal = _output_new.quat_nominal * dq;
// the quaternions must always be normalised afer modification
_output_new.quat_nominal.normalize();
@@ -524,7 +524,7 @@ void Ekf::calculateOutputStates()
// calculate the quaternion delta between the INS and EKF quaternions at the EKF fusion time horizon
Quatf quat_inv = _state.quat_nominal.inversed();
Quatf q_error = _output_sample_delayed.quat_nominal * quat_inv;
Quatf q_error = quat_inv * _output_sample_delayed.quat_nominal;
q_error.normalize();
// convert the quaternion delta to a delta angle
EKF/ekf_helper.cpp
+11 -11
View File
@@ -319,7 +319,7 @@ void Ekf::alignOutputFilter()
{
// calculate the quaternion delta between the output and EKF quaternions at the EKF fusion time horizon
Quatf quat_inv = _state.quat_nominal.inversed();
Quatf q_delta = _output_sample_delayed.quat_nominal * quat_inv;
Quatf q_delta = quat_inv * _output_sample_delayed.quat_nominal;
q_delta.normalize();
// calculate the velocity and posiiton deltas between the output and EKF at the EKF fusion time horizon
@@ -332,7 +332,7 @@ void Ekf::alignOutputFilter()
for (unsigned i = 0; i < output_length; i++) {
output_states = _output_buffer.get_from_index(i);
output_states.quat_nominal *= q_delta;
output_states.quat_nominal = q_delta * output_states.quat_nominal;
output_states.quat_nominal.normalize();
output_states.vel += vel_delta;
output_states.pos += pos_delta;
@@ -439,20 +439,20 @@ bool Ekf::realignYawGPS()
}
// calculate the amount that the quaternion has changed by
_state_reset_status.quat_change = _state.quat_nominal * quat_before_reset.inversed();
_state_reset_status.quat_change = quat_before_reset.inversed() * _state.quat_nominal;
// add the reset amount to the output observer buffered data
outputSample output_states;
unsigned output_length = _output_buffer.get_length();
for (unsigned i = 0; i < output_length; i++) {
output_states = _output_buffer.get_from_index(i);
output_states.quat_nominal *= _state_reset_status.quat_change;
output_states.quat_nominal = _state_reset_status.quat_change * output_states.quat_nominal;
_output_buffer.push_to_index(i, output_states);
}
// 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 = _state_reset_status.quat_change * _output_new.quat_nominal;
// capture the reset event
_state_reset_status.quat_counter++;
@@ -522,20 +522,20 @@ bool Ekf::resetMagHeading(Vector3f &mag_init)
_state.quat_nominal = Quatf(euler321);
// calculate the amount that the quaternion has changed by
_state_reset_status.quat_change = _state.quat_nominal * quat_before_reset.inversed();
_state_reset_status.quat_change = quat_before_reset.inversed() * _state.quat_nominal;
// add the reset amount to the output observer buffered data
outputSample output_states;
unsigned output_length = _output_buffer.get_length();
for (unsigned i = 0; i < output_length; i++) {
output_states = _output_buffer.get_from_index(i);
output_states.quat_nominal *= _state_reset_status.quat_change;
output_states.quat_nominal = _state_reset_status.quat_change * output_states.quat_nominal;
_output_buffer.push_to_index(i, output_states);
}
// 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 = _state_reset_status.quat_change * _output_new.quat_nominal;
// capture the reset event
_state_reset_status.quat_counter++;
@@ -636,7 +636,7 @@ bool Ekf::resetMagHeading(Vector3f &mag_init)
}
// calculate the amount that the quaternion has changed by
_state_reset_status.quat_change = _state.quat_nominal * quat_before_reset.inversed();
_state_reset_status.quat_change = quat_before_reset.inversed() * _state.quat_nominal;
// add the reset amount to the output observer buffered data
outputSample output_states = {};
@@ -644,13 +644,13 @@ bool Ekf::resetMagHeading(Vector3f &mag_init)
for (unsigned i = 0; i < output_length; i++) {
output_states = _output_buffer.get_from_index(i);
output_states.quat_nominal *= _state_reset_status.quat_change;
output_states.quat_nominal = _state_reset_status.quat_change * output_states.quat_nominal;
_output_buffer.push_to_index(i, output_states);
}
// 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 = _state_reset_status.quat_change * _output_new.quat_nominal;
// capture the reset event
_state_reset_status.quat_counter++;
matrix
+1 -1
Submodule matrix updated: cf924956d7...e595ebb9a7