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

Merge pull request #323 from PX4/pr-ekfQuatMultOrder 

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
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

6
EKF/control.cpp
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++;

6
EKF/ekf.cpp
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

22
EKF/ekf_helper.cpp
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++;

2
matrix

@ -1 +1 @@
Subproject commit cf924956d7d62ce18bfc4f8441e9177ddb69c0dc
Subproject commit e595ebb9a704c24aeae990dac768d26949fcaee0