From 9c5aa7274b77af8f8a4d4afa670a082194253f7b Mon Sep 17 00:00:00 2001 From: Ian Reid Date: Tue, 28 Nov 2023 12:25:56 -0700 Subject: [PATCH] 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) {