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

EKF: Improve tilt alignment monitoring

Browse Source 
Convert quaternion covariances into an angular alignment variance vector and discard the z component so that yaw uncertainty does not affect the result.
This commit is contained in:
Paul Riseborough
2016-05-17 11:17:24 +10:00
parent 6ca8a36782
commit 1540e937b1
3 changed files with 52 additions and 5 deletions
Download Patch File Download Diff File Expand all files Collapse all files
EKF/control.cpp
+10 -5
View File
@@ -49,11 +49,16 @@ void Ekf::controlFusionModes()
// Get the magnetic declination
calcMagDeclination();
// Once the angular uncertainty has reduced sufficiently, initialise the yaw and magnetic field states
float total_angle_variance = P[0][0] + P[1][1] + P[2][2] + P[3][3];
if (total_angle_variance < 0.002f && !_control_status.flags.tilt_align) {
_control_status.flags.tilt_align = true;
_control_status.flags.yaw_align = resetMagHeading(_mag_sample_delayed.mag);
// monitor the tilt alignment
if (!_control_status.flags.tilt_align) {
// whilst we are aligning the tilt, monitor the variances
Vector3f angle_err_var_vec = calcRotVecVariances();
// Once the tilt variances have reduced sufficiently, initialise the yaw and magnetic field states
if ((angle_err_var_vec(0) + angle_err_var_vec(1)) < 0.002f) {
_control_status.flags.tilt_align = true;
_control_status.flags.yaw_align = resetMagHeading(_mag_sample_delayed.mag);
}
}
// control use of various external souces for positon and velocity aiding