From 016695fc3ec913a6468a7f90c050d6f887e3d60a Mon Sep 17 00:00:00 2001 From: Paul Riseborough Date: Mon, 22 Feb 2016 22:12:47 +1100 Subject: [PATCH 1/3] EKF: Reduce startup transients Update initial state variance values --- EKF/covariance.cpp | 35 +++++++++++++++++++---------------- 1 file changed, 19 insertions(+), 16 deletions(-) diff --git a/EKF/covariance.cpp b/EKF/covariance.cpp index c5dbf51512..59a87db520 100644 --- a/EKF/covariance.cpp +++ b/EKF/covariance.cpp @@ -52,34 +52,37 @@ void Ekf::initialiseCovariance() } } + // calculate average prediction time step in sec + float dt = 0.001f * (float)FILTER_UPDATE_PERRIOD_MS; + // XXX use initial guess for the diagonal elements for the covariance matrix // angle error - P[0][0] = 0.001f; - P[1][1] = 0.001f; - P[2][2] = 0.001f; + P[0][0] = 0.1f; + P[1][1] = 0.1f; + P[2][2] = 0.1f; // velocity - P[3][3] = 0.1f; - P[4][4] = 0.1f; - P[5][5] = 0.1f; + P[3][3] = sq(fmaxf(_params.gps_vel_noise, 0.01f)); + P[4][4] = P[3][3]; + P[5][5] = sq(1.5f) * P[3][3]; // position - P[6][6] = 0.1f; - P[7][7] = 0.1f; - P[8][8] = 0.1f; + P[6][6] = sq(fmaxf(_params.gps_pos_noise, 0.01f)); + P[7][7] = P[6][6]; + P[8][8] = sq(fmaxf(_params.baro_noise, 0.01f)); // gyro bias - P[9][9] = 0.00001f; - P[10][10] = 0.00001f; - P[11][11] = 0.00001f; + P[9][9] = sq(0.035f * dt); + P[10][10] = P[9][9]; + P[11][11] = P[9][9]; // gyro scale - P[12][12] = 0.0001f; - P[13][13] = 0.0001f; - P[14][14] = 0.0001f; + P[12][12] = sq(0.001f); + P[13][13] = P[12][12]; + P[14][14] = P[12][12]; // accel z bias - P[15][15] = 0.0001f; + P[15][15] = sq(0.2f * dt); // variances for optional states // these state variances are set to zero until the states are required, then they must be initialised From 36affe3cd844ec145d81cf36e12cb13f8777cb51 Mon Sep 17 00:00:00 2001 From: Paul Riseborough Date: Wed, 24 Feb 2016 11:56:12 +1100 Subject: [PATCH 2/3] EKF: Fix bug causing incorrect initial roll when inverted --- EKF/ekf.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/EKF/ekf.cpp b/EKF/ekf.cpp index fe7e617990..1d20868f34 100644 --- a/EKF/ekf.cpp +++ b/EKF/ekf.cpp @@ -265,7 +265,7 @@ bool Ekf::initialiseFilter(void) if (_delVel_sum.norm() > 0.001f) { _delVel_sum.normalize(); pitch = asinf(_delVel_sum(0)); - roll = -asinf(_delVel_sum(1) / cosf(pitch)); + roll = atan2f(-_delVel_sum(1), -_delVel_sum(2)); } else { return false; From 0ad5329caff79720efa2891a87e8a08793678530 Mon Sep 17 00:00:00 2001 From: Paul Riseborough Date: Wed, 24 Feb 2016 08:20:29 +1100 Subject: [PATCH 3/3] EKF: Update comments in heading fusion to clarify calculation of magnetic heading --- EKF/mag_fusion.cpp | 4 +++- 1 file changed, 3 insertions(+), 1 deletion(-) diff --git a/EKF/mag_fusion.cpp b/EKF/mag_fusion.cpp index c31ad0cf58..857d1c019c 100644 --- a/EKF/mag_fusion.cpp +++ b/EKF/mag_fusion.cpp @@ -610,9 +610,11 @@ void Ekf::fuseHeading() // TODO - enable use of an off-board heading measurement - // rotate the magnetometer measurement into earth frame using an assumed zero yaw angle + // rotate the magnetometer measurement into earth frame matrix::Euler euler(_state.quat_nominal); float predicted_hdg = euler(2); // we will need the predicted heading to calculate the innovation + + // Set the yaw angle to zero and rotate the measurements into earth frame using the zero yaw angle euler(2) = 0.0f; matrix::Dcm R_to_earth(euler); matrix::Vector3f mag_earth_pred = R_to_earth * _mag_sample_delayed.mag;