From 30bfe0d3791cee90923be95c5871170c7e71a815 Mon Sep 17 00:00:00 2001 From: bresch Date: Mon, 24 Jul 2023 16:11:02 +0200 Subject: [PATCH] ekf2: remove artificial constraint on position uncertainty growth There is no reason to contrsain the position uncertainty, estpecially when flying with velocitiy aiding only. Note that all the variances are already contrained to sane values at the end of the covariance prediction --- src/modules/ekf2/EKF/covariance.cpp | 11 ----------- 1 file changed, 11 deletions(-) diff --git a/src/modules/ekf2/EKF/covariance.cpp b/src/modules/ekf2/EKF/covariance.cpp index b6726fdf6b..86da9086ee 100644 --- a/src/modules/ekf2/EKF/covariance.cpp +++ b/src/modules/ekf2/EKF/covariance.cpp @@ -302,17 +302,6 @@ void Ekf::predictCovariance(const imuSample &imu_delayed) nextP(i, i) += process_noise(i); } - // stop position covariance growth if our total position variance reaches 100m - // this can happen if we lose gps for some time - if ((P(7, 7) + P(8, 8)) > 1e4f) { - for (uint8_t i = 7; i <= 8; i++) { - for (uint8_t j = 0; j < _k_num_states; j++) { - nextP(i, j) = P(i, j); - nextP(j, i) = P(j, i); - } - } - } - // covariance matrix is symmetrical, so copy upper half to lower half for (unsigned row = 0; row <= 15; row++) { for (unsigned column = 0 ; column < row; column++) {