diff --git a/src/modules/ekf2/EKF/aid_sources/external_vision/ev_height_control.cpp b/src/modules/ekf2/EKF/aid_sources/external_vision/ev_height_control.cpp index 6d2067423d..5f3eee5fe5 100644 --- a/src/modules/ekf2/EKF/aid_sources/external_vision/ev_height_control.cpp +++ b/src/modules/ekf2/EKF/aid_sources/external_vision/ev_height_control.cpp @@ -67,10 +67,9 @@ void Ekf::controlEvHeightFusion(const imuSample &imu_sample, const extVisionSamp pos = R_ev_to_ekf * ev_sample.pos; pos_cov = R_ev_to_ekf * matrix::diag(ev_sample.position_var) * R_ev_to_ekf.transpose(); - // increase minimum variance to include EV orientation variance - // TODO: do this properly - const float orientation_var_max = math::max(ev_sample.orientation_var(0), ev_sample.orientation_var(1)); - pos_cov(2, 2) = math::max(pos_cov(2, 2), orientation_var_max); + // Position variance contribution from orientation uncertainty: δp_z = δθ_roll·py - δθ_pitch·px + pos_cov(2, 2) += sq(ev_sample.pos(1)) * ev_sample.orientation_var(0) // roll + + sq(ev_sample.pos(0)) * ev_sample.orientation_var(1); // pitch } } diff --git a/src/modules/ekf2/EKF/aid_sources/external_vision/ev_pos_control.cpp b/src/modules/ekf2/EKF/aid_sources/external_vision/ev_pos_control.cpp index d4e6b875ab..070c2d716f 100644 --- a/src/modules/ekf2/EKF/aid_sources/external_vision/ev_pos_control.cpp +++ b/src/modules/ekf2/EKF/aid_sources/external_vision/ev_pos_control.cpp @@ -101,13 +101,11 @@ void Ekf::controlEvPosFusion(const imuSample &imu_sample, const extVisionSample pos = R_ev_to_ekf * ev_sample.pos - pos_offset_earth; pos_cov = R_ev_to_ekf * matrix::diag(ev_sample.position_var) * R_ev_to_ekf.transpose(); - // increase minimum variance to include EV orientation variance - // TODO: do this properly - const float orientation_var_max = ev_sample.orientation_var.max(); - - for (int i = 0; i < 2; i++) { - pos_cov(i, i) = math::max(pos_cov(i, i), orientation_var_max); - } + // Position variance contribution from orientation uncertainty: δp = δθ × p + pos_cov(0, 0) += sq(ev_sample.pos(2)) * ev_sample.orientation_var(1) // pitch + + sq(ev_sample.pos(1)) * ev_sample.orientation_var(2); // yaw + pos_cov(1, 1) += sq(ev_sample.pos(0)) * ev_sample.orientation_var(2) // yaw + + sq(ev_sample.pos(2)) * ev_sample.orientation_var(0); // roll if (_control_status.flags.gnss_pos) { _ev_pos_b_est.setFusionActive(); diff --git a/src/modules/ekf2/EKF/aid_sources/external_vision/ev_vel.h b/src/modules/ekf2/EKF/aid_sources/external_vision/ev_vel.h index 10d6b64cf4..3f7cf93666 100644 --- a/src/modules/ekf2/EKF/aid_sources/external_vision/ev_vel.h +++ b/src/modules/ekf2/EKF/aid_sources/external_vision/ev_vel.h @@ -166,7 +166,11 @@ public: _measurement = rotation_ev_to_ekf * _sample.vel - velocity_offset_earth; _measurement_var = matrix::SquareMatrix3f(rotation_ev_to_ekf * matrix::diag( _sample.velocity_var) * rotation_ev_to_ekf.transpose()).diag(); - _min_variance = math::max(_min_variance, _sample.orientation_var.max()); + // Velocity variance contribution from orientation uncertainty: δv = δθ × v + const float vx = _sample.vel(0), vy = _sample.vel(1), vz = _sample.vel(2); + _measurement_var(0) += sq(vz) * _sample.orientation_var(1) + sq(vy) * _sample.orientation_var(2); + _measurement_var(1) += sq(vx) * _sample.orientation_var(2) + sq(vz) * _sample.orientation_var(0); + _measurement_var(2) += sq(vy) * _sample.orientation_var(0) + sq(vx) * _sample.orientation_var(1); } enforceMinimumVariance(); diff --git a/src/modules/ekf2/EKF/estimator_interface.cpp b/src/modules/ekf2/EKF/estimator_interface.cpp index a6d6ca8038..a25256fd15 100644 --- a/src/modules/ekf2/EKF/estimator_interface.cpp +++ b/src/modules/ekf2/EKF/estimator_interface.cpp @@ -740,7 +740,8 @@ bool EstimatorInterface::isNorthEastAidingActive() const { return _control_status.flags.gnss_pos || _control_status.flags.gnss_vel - || _control_status.flags.aux_gpos; + || _control_status.flags.aux_gpos + || (_control_status.flags.ev_pos && _control_status.flags.yaw_align); } void EstimatorInterface::printBufferAllocationFailed(const char *buffer_name)