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

Support full vision variance

Browse Source
This commit is contained in:
kamilritz
2020-01-08 14:36:03 +01:00
committed by Mathieu Bresciani
parent 4ab3dd50c9
commit 07e8e88e88
5 changed files with 28 additions and 25 deletions
Download Patch File Download Diff File Expand all files Collapse all files
EKF/ekf_helper.cpp
+3 -3
View File
@@ -100,7 +100,7 @@ bool Ekf::resetVelocity()
_ev_vel = _R_ev_to_ekf *_ev_sample_delayed.vel;
}
_state.vel = _ev_vel;
P.uncorrelateCovarianceSetVariance<3>(4, sq(_ev_sample_delayed.velErr));
P.uncorrelateCovarianceSetVariance<3>(4, _ev_sample_delayed.velVar);
} else {
ECL_INFO_TIMESTAMPED("reset velocity to zero");
// Used when falling back to non-aiding mode of operation
@@ -165,7 +165,7 @@ bool Ekf::resetPosition()
_state.pos(1) = _ev_pos(1);
// use EV accuracy to reset variances
P.uncorrelateCovarianceSetVariance<2>(7, sq(_ev_sample_delayed.posErr));
P.uncorrelateCovarianceSetVariance<2>(7, _ev_sample_delayed.posVar.slice<2, 1>(0, 0));
} else if (_control_status.flags.opt_flow) {
ECL_INFO_TIMESTAMPED("reset position to last known position");
@@ -705,7 +705,7 @@ bool Ekf::resetMagHeading(const Vector3f &mag_init, bool increase_yaw_var, bool
// 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)));
increaseQuatYawErrVariance(fmaxf(_ev_sample_delayed.angVar, sq(1.0e-2f)));
} else if (_params.mag_fusion_type <= MAG_FUSE_TYPE_3D) {
// using magnetic heading tuning parameter
increaseQuatYawErrVariance(sq(fmaxf(_params.mag_heading_noise, 1.0e-2f)));