From 71758a68514027584c832df0ade95cb580d7e8aa Mon Sep 17 00:00:00 2001 From: Ian Reid Date: Mon, 6 Nov 2023 12:58:20 -0700 Subject: [PATCH 1/6] Fixed wrapping issue. --- rosplane/src/estimator_example.cpp | 22 +++++++++++++++------- 1 file changed, 15 insertions(+), 7 deletions(-) diff --git a/rosplane/src/estimator_example.cpp b/rosplane/src/estimator_example.cpp index 729b4bb..60441b0 100644 --- a/rosplane/src/estimator_example.cpp +++ b/rosplane/src/estimator_example.cpp @@ -9,6 +9,11 @@ float radians(float degrees) return M_PI*degrees/180.0; } +double wrap(double fixed_heading, double wrapped_heading) { + // wrapped_heading - number_of_times_to_wrap * 2pi + return wrapped_heading - floor((wrapped_heading - fixed_heading) / (2 * M_PI) + 0.5) * 2 * M_PI; +} + estimator_example::estimator_example() : estimator_base(), xhat_a_(Eigen::Vector2f::Zero()), @@ -237,17 +242,19 @@ void estimator_example::estimate(const params_s ¶ms, const input_s &input, o P_p_ = (A_d_*P_p_*A_d_.transpose() + Q_p_*pow(params.Ts/N_, 2)); } - - while(xhat_p_(3) > radians(180.0f)) xhat_p_(3) = xhat_p_(3) - radians(360.0f); - while(xhat_p_(3) < radians(-180.0f)) xhat_p_(3) = xhat_p_(3) + radians(360.0f); + + xhat_p_(3) = wrap(0.0, xhat_p_(3)); + // while(xhat_p_(3) > radians(180.0f)) xhat_p_(3) = xhat_p_(3) - radians(360.0f); + // while(xhat_p_(3) < radians(-180.0f)) xhat_p_(3) = xhat_p_(3) + radians(360.0f); if(xhat_p_(3) > radians(180.0f) || xhat_p_(3) < radians(-180.0f)) { RCLCPP_WARN(this->get_logger(), "Course estimate not wrapped from -pi to pi"); xhat_p_(3) = 0; } - while(xhat_p_(6) > radians(180.0f)) xhat_p_(6) = xhat_p_(6) - radians(360.0f); - while(xhat_p_(6) < radians(-180.0f)) xhat_p_(6) = xhat_p_(6) + radians(360.0f); + xhat_p_(6) = wrap(0.0, xhat_p_(6)); + // while(xhat_p_(6) > radians(180.0f)) xhat_p_(6) = xhat_p_(6) - radians(360.0f); + // while(xhat_p_(6) < radians(-180.0f)) xhat_p_(6) = xhat_p_(6) + radians(360.0f); if(xhat_p_(6) > radians(180.0f) || xhat_p_(6) < radians(-180.0f)) { RCLCPP_WARN(this->get_logger(), "Psi estimate not wrapped from -pi to pi"); @@ -290,8 +297,9 @@ void estimator_example::estimate(const params_s ¶ms, const input_s &input, o //wrap course measurement float gps_course = fmodf(input.gps_course, radians(360.0f)); - while (gps_course - xhat_p_(3) > radians(180.0f)) gps_course = gps_course - radians(360.0f); - while (gps_course - xhat_p_(3) < radians(-180.0f)) gps_course = gps_course + radians(360.0f); + xhat_p_(3) = wrap(xhat_p_(3), gps_course); + // while (gps_course - xhat_p_(3) > radians(180.0f)) gps_course = gps_course - radians(360.0f); + // while (gps_course - xhat_p_(3) < radians(-180.0f)) gps_course = gps_course + radians(360.0f); h_p_ = xhat_p_(3); C_p_ = Eigen::VectorXf::Zero(7); From acd731732d089d38cf7a8caf952cf74ccd8279b7 Mon Sep 17 00:00:00 2001 From: Ian Reid Date: Wed, 15 Nov 2023 14:34:45 -0700 Subject: [PATCH 2/6] Cleaned wrapping functions. --- rosplane/src/estimator_example.cpp | 10 +++------- 1 file changed, 3 insertions(+), 7 deletions(-) diff --git a/rosplane/src/estimator_example.cpp b/rosplane/src/estimator_example.cpp index 60441b0..833359f 100644 --- a/rosplane/src/estimator_example.cpp +++ b/rosplane/src/estimator_example.cpp @@ -9,7 +9,7 @@ float radians(float degrees) return M_PI*degrees/180.0; } -double wrap(double fixed_heading, double wrapped_heading) { +double wrap_within_180(double fixed_heading, double wrapped_heading) { // wrapped_heading - number_of_times_to_wrap * 2pi return wrapped_heading - floor((wrapped_heading - fixed_heading) / (2 * M_PI) + 0.5) * 2 * M_PI; } @@ -243,18 +243,14 @@ void estimator_example::estimate(const params_s ¶ms, const input_s &input, o P_p_ = (A_d_*P_p_*A_d_.transpose() + Q_p_*pow(params.Ts/N_, 2)); } - xhat_p_(3) = wrap(0.0, xhat_p_(3)); - // while(xhat_p_(3) > radians(180.0f)) xhat_p_(3) = xhat_p_(3) - radians(360.0f); - // while(xhat_p_(3) < radians(-180.0f)) xhat_p_(3) = xhat_p_(3) + radians(360.0f); + xhat_p_(3) = wrap_within_180(0.0, xhat_p_(3)); if(xhat_p_(3) > radians(180.0f) || xhat_p_(3) < radians(-180.0f)) { RCLCPP_WARN(this->get_logger(), "Course estimate not wrapped from -pi to pi"); xhat_p_(3) = 0; } - xhat_p_(6) = wrap(0.0, xhat_p_(6)); - // while(xhat_p_(6) > radians(180.0f)) xhat_p_(6) = xhat_p_(6) - radians(360.0f); - // while(xhat_p_(6) < radians(-180.0f)) xhat_p_(6) = xhat_p_(6) + radians(360.0f); + xhat_p_(6) = wrap_within_180(0.0, xhat_p_(6)); if(xhat_p_(6) > radians(180.0f) || xhat_p_(6) < radians(-180.0f)) { RCLCPP_WARN(this->get_logger(), "Psi estimate not wrapped from -pi to pi"); From 123afe1226f07c487ad8a92037b057eee8a59360 Mon Sep 17 00:00:00 2001 From: Ian Reid Date: Tue, 28 Nov 2023 12:03:13 -0700 Subject: [PATCH 3/6] Fixed the wrap syntax issue --- rosplane/src/estimator_example.cpp | 4 +--- 1 file changed, 1 insertion(+), 3 deletions(-) diff --git a/rosplane/src/estimator_example.cpp b/rosplane/src/estimator_example.cpp index 833359f..5aba268 100644 --- a/rosplane/src/estimator_example.cpp +++ b/rosplane/src/estimator_example.cpp @@ -293,9 +293,7 @@ void estimator_example::estimate(const params_s ¶ms, const input_s &input, o //wrap course measurement float gps_course = fmodf(input.gps_course, radians(360.0f)); - xhat_p_(3) = wrap(xhat_p_(3), gps_course); - // while (gps_course - xhat_p_(3) > radians(180.0f)) gps_course = gps_course - radians(360.0f); - // while (gps_course - xhat_p_(3) < radians(-180.0f)) gps_course = gps_course + radians(360.0f); + xhat_p_(3) = wrap_within_180(xhat_p_(3), gps_course); h_p_ = xhat_p_(3); C_p_ = Eigen::VectorXf::Zero(7); From 583dc7f345c52e619b42a0d0690dd59a7a975b26 Mon Sep 17 00:00:00 2001 From: Ian Reid Date: Tue, 28 Nov 2023 12:04:08 -0700 Subject: [PATCH 4/6] Added back the low pass filter for the gyro. --- rosplane/src/estimator_example.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/rosplane/src/estimator_example.cpp b/rosplane/src/estimator_example.cpp index 5aba268..2e2ecea 100644 --- a/rosplane/src/estimator_example.cpp +++ b/rosplane/src/estimator_example.cpp @@ -86,8 +86,8 @@ void estimator_example::estimate(const params_s ¶ms, const input_s &input, o } // low pass filter gyros to estimate angular rates - lpf_gyro_x_ = input.gyro_x; - lpf_gyro_y_ = input.gyro_y; // alpha_*lpf_gyro_y_ + (1 - alpha_)*input.gyro_y; + lpf_gyro_x_ = alpha_*lpf_gyro_x_ + (1 - alpha_)*input.gyro_x; + lpf_gyro_y_ = alpha_*lpf_gyro_y_ + (1 - alpha_)*input.gyro_y; lpf_gyro_z_ = alpha_*lpf_gyro_z_ + (1 - alpha_)*input.gyro_z; float phat = lpf_gyro_x_; From a10954213cb7e3a74414d2925b823d1a8ca8ebc7 Mon Sep 17 00:00:00 2001 From: Ian Reid Date: Tue, 28 Nov 2023 12:11:00 -0700 Subject: [PATCH 5/6] Added the warning messages back into the estimator. --- rosplane/src/estimator_example.cpp | 9 +++------ 1 file changed, 3 insertions(+), 6 deletions(-) diff --git a/rosplane/src/estimator_example.cpp b/rosplane/src/estimator_example.cpp index 2e2ecea..5df6a20 100644 --- a/rosplane/src/estimator_example.cpp +++ b/rosplane/src/estimator_example.cpp @@ -338,7 +338,7 @@ void estimator_example::estimate(const params_s ¶ms, const input_s &input, o } bool problem = false; -// int prob_index; + int prob_index; for (int i = 0; i < 7; i++) { if (!std::isfinite(xhat_p_(i))) @@ -346,7 +346,7 @@ void estimator_example::estimate(const params_s ¶ms, const input_s &input, o if (!problem) { problem = true; -// prob_index = i; + prob_index = i; } switch (i) { @@ -380,7 +380,7 @@ void estimator_example::estimate(const params_s ¶ms, const input_s &input, o } if (problem) { -// RCLCPP_WARN(this->get_logger(), "position estimator reinitialized due to non-finite state %d", prob_index); TODO come back and track down why pn is going infinite. + RCLCPP_WARN(this->get_logger(), "position estimator reinitialized due to non-finite state %d", prob_index); } if (xhat_p_(6) - xhat_p_(3) > radians(360.0f) || xhat_p_(6) - xhat_p_(3) < radians(-360.0f)) { @@ -396,9 +396,6 @@ void estimator_example::estimate(const params_s ¶ms, const input_s &input, o float wehat = xhat_p_(5); float psihat = xhat_p_(6); - -// RCLCPP_INFO_STREAM(this->get_logger(), "chihat: " << chihat); - output.pn = pnhat; output.pe = pehat; output.h = hhat; From 9c5aa7274b77af8f8a4d4afa670a082194253f7b Mon Sep 17 00:00:00 2001 From: Ian Reid Date: Tue, 28 Nov 2023 12:25:56 -0700 Subject: [PATCH 6/6] Cleaned up some of the comments and calculations. --- rosplane/src/estimator_example.cpp | 15 +++++++-------- 1 file changed, 7 insertions(+), 8 deletions(-) diff --git a/rosplane/src/estimator_example.cpp b/rosplane/src/estimator_example.cpp index 5df6a20..4e68401 100644 --- a/rosplane/src/estimator_example.cpp +++ b/rosplane/src/estimator_example.cpp @@ -175,18 +175,18 @@ void estimator_example::estimate(const params_s ¶ms, const input_s &input, o -params.gravity*cp*ct, -rhat*Vahat*st - phat*Vahat*ct + params.gravity*sp*st, params.gravity*sp*ct, (qhat*Vahat + params.gravity*cp) * st; + // This calculates the Kalman Gain for all of the attitude states at once rather than one at a time. + Eigen::Vector3f y; y << lpf_accel_x_, lpf_accel_y_, lpf_accel_z_; Eigen::MatrixXf S_inv = (R_accel_ + C_a_ * P_a_ * C_a_.transpose()).inverse(); + Eigen::MatrixXf L_a_ = P_a_ * C_a_.transpose() * S_inv; + Eigen::MatrixXf temp = Eigen::MatrixXf::Identity(2,2) - L_a_ * C_a_; - if ((y-h_a_).transpose() * S_inv * (y-h_a_) < 10000000.0){ - Eigen::MatrixXf L = P_a_ * C_a_.transpose() * S_inv; - Eigen::MatrixXf temp = Eigen::MatrixXf::Identity(2,2) - L * C_a_; - P_a_ = temp * P_a_ * temp.transpose() + L * R_accel_ * L.transpose(); - xhat_a_ = xhat_a_ + L * (y - h_a_); - } + P_a_ = temp * P_a_ * temp.transpose() + L_a_ * R_accel_ *L_a_.transpose(); + xhat_a_ = xhat_a_ + L_a_ * (y - h_a_); check_xhat_a(); @@ -204,8 +204,6 @@ void estimator_example::estimate(const params_s ¶ms, const input_s &input, o for (int i = 0; i < N_; i++) { -// float pn = xhat_p_(0); -// float pe = xhat_p_(1); float Vg = xhat_p_(2); float chi = xhat_p_(3); float wn = xhat_p_(4); @@ -259,6 +257,7 @@ void estimator_example::estimate(const params_s ¶ms, const input_s &input, o // measurement updates + // These calculate the Kalman gain and applies them to each state individually. if (input.gps_new) {