From 727a43764f481958130bef3a112f7ee23aea0d4e Mon Sep 17 00:00:00 2001 From: Paul Riseborough Date: Tue, 3 May 2016 20:59:49 +1000 Subject: [PATCH] EKF: update initial angle alignment check --- EKF/control.cpp | 10 +++------- EKF/ekf.cpp | 3 --- EKF/ekf.h | 3 --- EKF/vel_pos_fusion.cpp | 2 -- 4 files changed, 3 insertions(+), 15 deletions(-) diff --git a/EKF/control.cpp b/EKF/control.cpp index 8a604e18f0..c2047af726 100644 --- a/EKF/control.cpp +++ b/EKF/control.cpp @@ -51,13 +51,9 @@ void Ekf::controlFusionModes() // Get the magnetic declination calcMagDeclination(); - // Check for tilt convergence during initial alignment - // filter the tilt error vector using a 1 sec time constant LPF - float filt_coef = 1.0f * _imu_sample_delayed.delta_ang_dt; - _tilt_err_length_filt = filt_coef * _tilt_err_vec.norm() + (1.0f - filt_coef) * _tilt_err_length_filt; - - // Once the tilt error has reduced sufficiently, initialise the yaw and magnetic field states - if (_tilt_err_length_filt < 0.005f && !_control_status.flags.tilt_align) { + // 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.001f && !_control_status.flags.tilt_align) { _control_status.flags.tilt_align = true; _control_status.flags.yaw_align = resetMagHeading(_mag_sample_delayed.mag); } diff --git a/EKF/ekf.cpp b/EKF/ekf.cpp index 421cdf5efa..1b2c5d94d3 100644 --- a/EKF/ekf.cpp +++ b/EKF/ekf.cpp @@ -470,9 +470,6 @@ bool Ekf::initialiseFilter(void) // update transformation matrix from body to world frame _R_to_earth = quat_to_invrotmat(_state.quat_nominal); - // initialise the filtered alignment error estimate to a larger starting value - _tilt_err_length_filt = 1.0f; - // calculate the averaged magnetometer reading Vector3f mag_init = _mag_filt_state; diff --git a/EKF/ekf.h b/EKF/ekf.h index 96b65ab4ce..f496f64046 100644 --- a/EKF/ekf.h +++ b/EKF/ekf.h @@ -180,9 +180,6 @@ private: float _heading_innov; // heading measurement innovation float _heading_innov_var; // heading measurement innovation variance - Vector3f _tilt_err_vec; // Vector of the most recent attitude error correction from velocity and position fusion - float _tilt_err_length_filt; // filtered length of _tilt_err_vec - // optical flow processing float _flow_innov[2]; // flow measurement innovation float _flow_innov_var[2]; // flow innovation variance diff --git a/EKF/vel_pos_fusion.cpp b/EKF/vel_pos_fusion.cpp index 5c53c9077a..2398dce975 100644 --- a/EKF/vel_pos_fusion.cpp +++ b/EKF/vel_pos_fusion.cpp @@ -176,13 +176,11 @@ void Ekf::fuseVelPosHeight() // record the successful velocity fusion time if (vel_check_pass && _fuse_hor_vel) { _time_last_vel_fuse = _time_last_imu; - _tilt_err_vec.setZero(); } // record the successful position fusion time if (pos_check_pass && _fuse_pos) { _time_last_pos_fuse = _time_last_imu; - _tilt_err_vec.setZero(); } // record the successful height fusion time