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

EKF: update output observer and capture reset event for magnetic heading resets

Browse Source
This commit is contained in:
Paul Riseborough 2016-05-23 20:39:38 +10:00
parent 2b0d5c28f0
commit 9ec09f5f4e
1 changed files with 17 additions and 0 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

17
EKF/ekf_helper.cpp
View File

@ -350,6 +350,8 @@ void Ekf::alignOutputFilter()
// Reset heading and magnetic field states
bool Ekf::resetMagHeading(Vector3f &mag_init)
{
// save a copy of the quaternion state for later use in calculating the amount of reset change
Quaternion quat_before_reset = _state.quat_nominal;
// calculate the variance for the rotation estimate expressed as a rotation vector
// this will be used later to reset the quaternion state covariances
@ -482,6 +484,21 @@ bool Ekf::resetMagHeading(Vector3f &mag_init)
P[index][index] = sq(_params.mag_noise);
}
// calculate the amount that the quaternion has changed by
_state_reset_status.quat_change = _state.quat_nominal * quat_before_reset.inversed();
// 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_buffer.push_to_index(i,output_states);
}
// capture the reset event
_state_reset_status.quat_time_us = _imu_sample_delayed.time_us;
return true;
}