Skip to content

Documentation + Order of Error Changes #26

New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Open
wants to merge 1 commit into
base: main
Choose a base branch
from
Open

Documentation + Order of Error Changes #26

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
1 commit
Select commit
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
59 changes: 40 additions & 19 deletions lib/estimator/MEKF.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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;
Expand All @@ -17,11 +17,18 @@ Eigen::Matrix3d mag_bias_cov_mat;
Eigen::Matrix<double, 18, 18> estimate_covariance;
Eigen::Matrix<double, 6, 6> observation_covariance;
Eigen::Matrix<double, 18, 18> G; // part of the state transition matrix x-dot = Gx
Eigen::Matrix<double, 18, 18> F_mat;
Eigen::Matrix<double, 6, 18> H;
Eigen::Matrix<double, 6, 6> inverse_cov;
Eigen::Matrix<double, 18, 6> K;
Eigen::Matrix<double, 18, 1> aposteriori_state;
Eigen::Matrix<double, 18, 18> F_mat; // discretized state transition matrix
Eigen::Matrix<double, 6, 18> H; // observation matrix
Eigen::Matrix<double, 6, 6> innov_cov; // innovation / residual covariance matrix (S_k in the wiki page of EKF)
Eigen::Matrix<double, 18, 6> K; // kalman gain
Eigen::Matrix<double, 18, 1> 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){
Expand All @@ -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);
Expand All @@ -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<double, 18, 18> process_covariance(double time_delta){
Eigen::Matrix<double, 18, 18> 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);
Expand All @@ -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));

Expand All @@ -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<double, 18, 6> 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<double, 1, 6> observation = Eigen::MatrixXd::Zero(1, 6);
Expand Down Expand Up @@ -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();
Expand All @@ -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),
Expand Down
8 changes: 8 additions & 0 deletions lib/estimator/MEKFEstimatorModule.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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");
}