diff --git a/lib/estimator/MEKF.cpp b/lib/estimator/MEKF.cpp index 2d7e425..a70b278 100644 --- a/lib/estimator/MEKF.cpp +++ b/lib/estimator/MEKF.cpp @@ -2,7 +2,7 @@ #include "VEigen.h" -Eigen::Quaterniond estimate; +Eigen::Quaterniond estimate; // estimate of orientation ( q hat ) Eigen::Vector3d gyro_bias; Eigen::Vector3d accel_bias; Eigen::Vector3d mag_bias; @@ -17,11 +17,18 @@ Eigen::Matrix3d mag_bias_cov_mat; Eigen::Matrix estimate_covariance; Eigen::Matrix observation_covariance; Eigen::Matrix G; // part of the state transition matrix x-dot = Gx -Eigen::Matrix F_mat; -Eigen::Matrix H; -Eigen::Matrix inverse_cov; -Eigen::Matrix K; -Eigen::Matrix aposteriori_state; +Eigen::Matrix F_mat; // discretized state transition matrix +Eigen::Matrix H; // observation matrix +Eigen::Matrix innov_cov; // innovation / residual covariance matrix (S_k in the wiki page of EKF) +Eigen::Matrix K; // kalman gain +Eigen::Matrix aposteriori_state; // (x hat)_{k|k} +// state vector: +// [0-3] -> alpha = 2 * del q (where del q is orientation error, tending to zero) +// [3-6] -> velocity error +// [6-9] -> position error +// [9-12] -> gyro bias +// [12-15] -> accelerometer bias +// [15-18] -> magnetometer bias void initKalman(Eigen::Quaterniond init_est, double estimate_covar, double gyro_cov, double gyro_bias_cov, double accel_proc_cov, double accel_bias_cov, double mag_proc_cov, double mag_bias_cov, double accel_obs_cov, double mag_obs_cov){ @@ -36,7 +43,7 @@ void initKalman(Eigen::Quaterniond init_est, double estimate_covar, double gyro_ F_mat = Eigen::MatrixXd::Zero(18, 18); H = Eigen::MatrixXd::Zero(6, 18); - inverse_cov = Eigen::MatrixXd::Zero(6, 6); + innov_cov = Eigen::MatrixXd::Zero(6, 6); K = Eigen::MatrixXd::Zero(18, 6); G = Eigen::MatrixXd::Zero(18,18); @@ -51,11 +58,15 @@ void initKalman(Eigen::Quaterniond init_est, double estimate_covar, double gyro_ mag_bias_cov_mat = Eigen::Matrix3d::Identity(3,3) * mag_bias_cov; } -// taylor series approximation of the process covariance matrix, linearized around omega = 0 = accelerometer reading, and rotation = Identity matrix -// what does that mean: I'm only about 50% sure.. +// calculates the process covariance matrix Q_k using a taylor series approximation for F described on line 98 +// linearized around omega = 0, accelerometer reading = 0, and Rotation matrix of q = I => q = [1 0vec] +// "This makes some sense intuitively, works in +// practice, and is used in reference 11, but an analytical reason why this works is left as a subject +// of future research." mentioned in the paper from where this is taken from Eigen::Matrix process_covariance(double time_delta){ Eigen::Matrix Q = Eigen::MatrixXd::Zero(18, 18); - + + // derivation at https://matthewhampsey.github.io/blog/2020/07/18/mekf Q.block<3, 3>(0, 0) = gyro_cov_mat * time_delta + gyro_bias_cov_mat * (pow(time_delta, 3)/3.0); Q.block<3, 3>(0, 9) = gyro_bias_cov_mat * (-pow(time_delta, 2)/2.0); Q.block<3, 3>(3, 3) = accel_cov_mat * time_delta + accel_bias_cov_mat * (pow(time_delta, 3)/3.0); @@ -80,20 +91,28 @@ void updateKalman(Eigen::Vector3d gyro_meas, Eigen::Vector3d acc_meas, Eigen::Ve mag_meas = mag_meas - mag_bias; Eigen::Quaterniond angular_velocity; + Eigen::Quaterniond q_delta; + // angular velocity (omega) definition angular_velocity.w() = 0; angular_velocity.vec() = gyro_meas; + // q dot = 1/2 q cross omega + // q_delta ~ del q dot * delta t angular_velocity = estimate * angular_velocity; - Eigen::Quaterniond q_temp; - q_temp = angular_velocity.coeffs() * time_delta * 0.5; - estimate.coeffs() += q_temp.coeffs(); + q_delta = angular_velocity.coeffs() * time_delta * 0.5; + estimate.coeffs() += q_delta.coeffs(); // maybe add w() and vec() seperately instead of creating temp vector ?? estimate.normalize(); G.block<3, 3>(0, 0) = -skewSymmetric(gyro_meas); G.block<3, 3>(3, 0) = -estimate.toRotationMatrix() * skewSymmetric(acc_meas); G.block<3, 3>(3, 12) = -estimate.toRotationMatrix(); - F_mat = Eigen::MatrixXd::Identity(18, 18) + G * time_delta; - - + // system in continuous time is x dot = G x + // converting to discrete time x_{n+1} = F x_n + // where F = e^(G delta t) + // using taylor series, F = I + G delta t + 1/2 G^2 (delta_t)^2 + O(t^3) + // considered till second order to keep it consistent with calculation of Q matrix + F_mat = Eigen::MatrixXd::Identity(18, 18) + G * time_delta + (0.5 * G * G * time_delta * time_delta); + + // prediction of estimate covariance ( P_{k|k-1} ) estimate_covariance = (F_mat * estimate_covariance * F_mat.transpose()) + process_covariance(time_delta); //Serial.println(process_covariance(time_delta)(0, 0) * pow(10, 7)); @@ -107,9 +126,9 @@ void updateKalman(Eigen::Vector3d gyro_meas, Eigen::Vector3d acc_meas, Eigen::Ve H.block<3, 3>(3, 15) = Eigen::MatrixXd::Identity(3, 3); Eigen::Matrix PH_T = Eigen::MatrixXd::Zero(18, 6); PH_T = estimate_covariance * H.transpose(); - inverse_cov = H * PH_T + observation_covariance; - K = PH_T * inverse_cov.inverse(); - + innov_cov = H * PH_T + observation_covariance; + K = PH_T * innov_cov.inverse(); + // update step for estimate covariance ( P_{k|k} ) estimate_covariance = (Eigen::MatrixXd::Identity(18, 18) - (K * H)) * estimate_covariance; Eigen::Matrix observation = Eigen::MatrixXd::Zero(1, 6); @@ -141,6 +160,7 @@ void updateKalman(Eigen::Vector3d gyro_meas, Eigen::Vector3d acc_meas, Eigen::Ve Eigen::Quaterniond estimate_to_fold; estimate_to_fold.w() = 1.0; + // del q term in our state, must be tending to zero to maintain unitary norm assumption estimate_to_fold.vec() = 0.5 * aposteriori_state.block<3, 1>(0, 0); estimate = estimate * estimate_to_fold; estimate.normalize(); @@ -149,6 +169,7 @@ void updateKalman(Eigen::Vector3d gyro_meas, Eigen::Vector3d acc_meas, Eigen::Ve mag_bias += aposteriori_state.block<3, 1>(15, 0); } +// cross product matrix Eigen::Matrix3d skewSymmetric(Eigen::VectorXd v){ Eigen::Matrix3d m(3, 3); m << 0.0, -v(2), v(1), diff --git a/lib/estimator/MEKFEstimatorModule.cpp b/lib/estimator/MEKFEstimatorModule.cpp index a495a64..c20dc79 100644 --- a/lib/estimator/MEKFEstimatorModule.cpp +++ b/lib/estimator/MEKFEstimatorModule.cpp @@ -70,4 +70,12 @@ void MEKFEstimatorModule::update(unsigned long time) { // flightData::estimatedStateX(3) = roll; // flightData::estimatedStateX(4) = pitch; // flightData::estimatedStateX(5) = yaw; + // Serial.print("Quaternion"); + // Serial.print(","); + // Serial.print(qx); + // Serial.print(","); + // Serial.print(qy); + // Serial.print(","); + // Serial.print(qz); + // Serial.print("\n"); } \ No newline at end of file