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

EKF: Change the EKF initialization to

Browse Source 
1) init tilt
2) init yaw
3) init tilt uncertainty
4) init yaw uncertainty

Signed-off-by: CarlOlsson <carlolsson.co@gmail.com>
This commit is contained in:
CarlOlsson
2019-03-12 19:07:55 +11:00
committed by Paul Riseborough
parent ea259d358d
commit 96c5c14041
3 changed files with 35 additions and 22 deletions
Download Patch File Download Diff File Expand all files Collapse all files
EKF/ekf_helper.cpp
+20 -16
View File
@@ -558,7 +558,7 @@ bool Ekf::realignYawGPS()
}
// Reset heading and magnetic field states
bool Ekf::resetMagHeading(Vector3f &mag_init)
bool Ekf::resetMagHeading(Vector3f &mag_init, bool increase_yaw_var, bool update_buffer)
{
// prevent a reset being performed more than once on the same frame
if (_imu_sample_delayed.time_us == _flt_mag_align_start_time) {
@@ -737,24 +737,28 @@ bool Ekf::resetMagHeading(Vector3f &mag_init)
resetExtVisRotMat();
}
// update the yaw angle variance using the variance of the measurement
if (_control_status.flags.ev_yaw) {
// using error estimate from external vision data
increaseQuatYawErrVariance(sq(fmaxf(_ev_sample_delayed.angErr, 1.0e-2f)));
} else if (_params.mag_fusion_type <= MAG_FUSE_TYPE_AUTOFW) {
// using magnetic heading tuning parameter
increaseQuatYawErrVariance(sq(fmaxf(_params.mag_heading_noise, 1.0e-2f)));
if (increase_yaw_var) {
// update the yaw angle variance using the variance of the measurement
if (_control_status.flags.ev_yaw) {
// using error estimate from external vision data
increaseQuatYawErrVariance(sq(fmaxf(_ev_sample_delayed.angErr, 1.0e-2f)));
} else if (_params.mag_fusion_type <= MAG_FUSE_TYPE_AUTOFW) {
// using magnetic heading tuning parameter
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++) {
// Note q1 *= q2 is equivalent to q1 = q2 * q1
_output_buffer[i].quat_nominal *= _state_reset_status.quat_change;
}
if (update_buffer) {
// 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;
}
// 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;
// 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++;