diff --git a/src/modules/ekf2/ekf2_main.cpp b/src/modules/ekf2/ekf2_main.cpp index 63993eca53944e12fc0badc99b5a0fa1ec90bd7d..3f2ee5f147e6fe39f32b51d7e42dddd7ee923ed3 100644 --- a/src/modules/ekf2/ekf2_main.cpp +++ b/src/modules/ekf2/ekf2_main.cpp @@ -1175,7 +1175,8 @@ void Ekf2::run() if (PX4_ISFINITE(ev_odom.pose_covariance[ev_odom.COVARIANCE_MATRIX_X_VARIANCE])) { ev_data.posErr = fmaxf(_ev_pos_noise.get(), sqrtf(fmaxf(ev_odom.pose_covariance[ev_odom.COVARIANCE_MATRIX_X_VARIANCE], ev_odom.pose_covariance[ev_odom.COVARIANCE_MATRIX_Y_VARIANCE]))); - ev_data.hgtErr = fmaxf(_ev_pos_noise.get(), sqrtf(ev_odom.pose_covariance[ev_odom.COVARIANCE_MATRIX_Z_VARIANCE]))); + ev_data.hgtErr = fmaxf(_ev_pos_noise.get(), sqrtf(ev_odom.pose_covariance[ev_odom.COVARIANCE_MATRIX_Z_VARIANCE])); + } else { ev_data.posErr = _ev_pos_noise.get(); ev_data.hgtErr = _ev_pos_noise.get();