diff --git a/EKF/ekf.cpp b/EKF/ekf.cpp index 547d27baf0..49b8910e7d 100644 --- a/EKF/ekf.cpp +++ b/EKF/ekf.cpp @@ -50,7 +50,13 @@ Ekf::Ekf(): _fuse_hor_vel(false), _fuse_vert_vel(false), _mag_fuse_index(0), - _time_last_fake_gps(0) + _time_last_fake_gps(0), + _vel_pos_innov{}, + _mag_innov{}, + _heading_innov{}, + _vel_pos_innov_var{}, + _mag_innov_var{}, + _heading_innov_var{} { _earth_rate_NED.setZero(); _R_prev = matrix::Dcm(); diff --git a/EKF/ekf.h b/EKF/ekf.h index 46728fee9d..7567af22d1 100644 --- a/EKF/ekf.h +++ b/EKF/ekf.h @@ -52,6 +52,32 @@ public: bool update(); + // gets the innovations of velocity and position measurements + // 0-2 vel, 3-5 pos + void get_vel_pos_innov(float vel_pos_innov[6]); + + // gets the innovations of the earth magnetic field measurements + void get_mag_innov(float mag_innov[3]); + + // gets the innovations of the heading measurement + void get_heading_innov(float *heading_innov); + + // gets the innovation variances of velocity and position measurements + // 0-2 vel, 3-5 pos + void get_vel_pos_innov_var(float vel_pos_innov_var[6]); + + // gets the innovation variances of the earth magnetic field measurements + void get_mag_innov_var(float mag_innov_var[3]); + + // gets the innovation variance of the heading measurement + void get_heading_innov_var(float *heading_innov_var); + + // get the state vector at the delayed time horizon + void get_state_delayed(float *state); + + // get the diagonal elements of the covariance matrix + void get_covariances(float *covariances); + private: static const uint8_t _k_num_states = 24; @@ -75,6 +101,14 @@ private: float P[_k_num_states][_k_num_states]; // state covariance matrix + float _vel_pos_innov[6]; // innovations: 0-2 vel, 3-5 pos + float _mag_innov[3]; // earth magnetic field innovations + float _heading_innov; // heading measurement innovation + + float _vel_pos_innov_var[6]; // innovation variances: 0-2 vel, 3-5 pos + float _mag_innov_var[3]; // earth magnetic field innovation variance + float _heading_innov_var; // heading measurement innovation variance + // complementary filter states Vector3f _delta_angle_corr; Vector3f _delta_vel_corr; diff --git a/EKF/ekf_helper.cpp b/EKF/ekf_helper.cpp index a656d59cf8..620740ec42 100644 --- a/EKF/ekf_helper.cpp +++ b/EKF/ekf_helper.cpp @@ -128,33 +128,33 @@ void Ekf::makeSymmetrical() void Ekf::constrainStates() { - for (int i = 0; i < 3; i++) { + for (int i = 0; i < 3; i++) { _state.ang_error(i) = math::constrain(_state.ang_error(i), -1.0f, 1.0f); } - for (int i = 0; i < 3; i++) { + for (int i = 0; i < 3; i++) { _state.vel(i) = math::constrain(_state.vel(i), -1000.0f, 1000.0f); } - for (int i = 0; i < 3; i++) { + for (int i = 0; i < 3; i++) { _state.pos(i) = math::constrain(_state.pos(i), -1.e6f, 1.e6f); } - for (int i = 0; i < 3; i++) { + for (int i = 0; i < 3; i++) { _state.gyro_bias(i) = math::constrain(_state.gyro_bias(i), -0.349066f * _dt_imu_avg, 0.349066f * _dt_imu_avg); } - for (int i = 0; i < 3; i++) { + for (int i = 0; i < 3; i++) { _state.gyro_scale(i) = math::constrain(_state.gyro_scale(i), 0.95f, 1.05f); } _state.accel_z_bias = math::constrain(_state.accel_z_bias, -1.0f * _dt_imu_avg, 1.0f * _dt_imu_avg); - for (int i = 0; i < 3; i++) { + for (int i = 0; i < 3; i++) { _state.mag_I(i) = math::constrain(_state.mag_I(i), -1.0f, 1.0f); } - for (int i = 0; i < 3; i++) { + for (int i = 0; i < 3; i++) { _state.mag_B(i) = math::constrain(_state.mag_B(i), -0.5f, 0.5f); } @@ -170,3 +170,87 @@ void Ekf::calcEarthRateNED(Vector3f &omega, double lat_rad) const omega(1) = 0.0f; omega(2) = -_k_earth_rate * sinf((float)lat_rad); } + +// gets the innovations of velocity and position measurements +// 0-2 vel, 3-5 pos +void Ekf::get_vel_pos_innov(float vel_pos_innov[6]) +{ + memcpy(vel_pos_innov, _vel_pos_innov, sizeof(float) * 6); +} + +// writes the innovations of the earth magnetic field measurements +void Ekf::get_mag_innov(float mag_innov[3]) +{ + memcpy(mag_innov, _mag_innov, 3 * sizeof(float)); +} + +// gets the innovations of the heading measurement +void Ekf::get_heading_innov(float *heading_innov) +{ + memcpy(heading_innov, &_heading_innov, sizeof(float)); +} + +// gets the innovation variances of velocity and position measurements +// 0-2 vel, 3-5 pos +void Ekf::get_vel_pos_innov_var(float vel_pos_innov_var[6]) +{ + memcpy(vel_pos_innov_var, _vel_pos_innov_var, sizeof(float) * 6); +} + +// gets the innovation variances of the earth magnetic field measurements +void Ekf::get_mag_innov_var(float mag_innov_var[3]) +{ + memcpy(mag_innov_var, _mag_innov_var, sizeof(float) * 3); +} + +// gets the innovation variance of the heading measurement +void Ekf::get_heading_innov_var(float *heading_innov_var) +{ + memcpy(heading_innov_var, &_heading_innov_var, sizeof(float)); +} + +// get the state vector at the delayed time horizon +void Ekf::get_state_delayed(float *state) +{ + for (int i = 0; i < 3; i++) { + state[i] = _state.ang_error(i); + } + + for (int i = 0; i < 3; i++) { + state[i + 3] = _state.vel(i); + } + + for (int i = 0; i < 3; i++) { + state[i + 6] = _state.pos(i); + } + + for (int i = 0; i < 3; i++) { + state[i + 9] = _state.gyro_bias(i); + } + + for (int i = 0; i < 3; i++) { + state[i + 12] = _state.gyro_scale(i); + } + + state[15] = _state.accel_z_bias; + + for (int i = 0; i < 3; i++) { + state[i + 16] = _state.mag_I(i); + } + + for (int i = 0; i < 3; i++) { + state[i + 19] = _state.mag_B(i); + } + + for (int i = 0; i < 2; i++) { + state[i + 22] = _state.wind_vel(i); + } +} + +// get the diagonal elements of the covariance matrix +void Ekf::get_covariances(float *covariances) +{ + for (unsigned i = 0; i < _k_num_states; i++) { + covariances[i] = P[i][i]; + } +} diff --git a/EKF/estimator_base.h b/EKF/estimator_base.h index 223182c05f..bd716601b3 100644 --- a/EKF/estimator_base.h +++ b/EKF/estimator_base.h @@ -94,6 +94,30 @@ public: virtual bool update() = 0; + // gets the innovations of velocity and position measurements + // 0-2 vel, 3-5 pos + virtual void get_vel_pos_innov(float vel_pos_innov[6]) = 0; + + // gets the innovations of the earth magnetic field measurements + virtual void get_mag_innov(float mag_innov[3]) = 0; + + // gets the innovations of the heading measurement + virtual void get_heading_innov(float *heading_innov) = 0; + + // gets the innovation variances of velocity and position measurements + // 0-2 vel, 3-5 pos + virtual void get_vel_pos_innov_var(float vel_pos_innov_var[6]) = 0; + + // gets the innovation variances of the earth magnetic field measurements + virtual void get_mag_innov_var(float mag_innov_var[3]) = 0; + + // gets the innovation variance of the heading measurement + virtual void get_heading_innov_var(float *heading_innov_var) = 0; + + virtual void get_state_delayed(float *state) = 0; + + virtual void get_covariances(float *covariances) = 0; + // set delta angle imu data void setIMUData(uint64_t time_usec, uint64_t delta_ang_dt, uint64_t delta_vel_dt, float *delta_ang, float *delta_vel); @@ -244,14 +268,13 @@ protected: // flags capturing information about severe nummerical problems for various fusions struct { - bool bad_mag_x:1; - bool bad_mag_y:1; - bool bad_mag_z:1; - bool bad_airspeed:1; - bool bad_sideslip:1; + bool bad_mag_x: 1; + bool bad_mag_y: 1; + bool bad_mag_z: 1; + bool bad_airspeed: 1; + bool bad_sideslip: 1; } _fault_status; - void initialiseVariables(uint64_t timestamp); void initialiseGPS(struct gps_message *gps); @@ -272,22 +295,26 @@ public: bool position_is_valid(); - void copy_quaternion(float *quat) { + void copy_quaternion(float *quat) + { for (unsigned i = 0; i < 4; i++) { quat[i] = _output_new.quat_nominal(i); } } - void copy_velocity(float *vel) { + void copy_velocity(float *vel) + { for (unsigned i = 0; i < 3; i++) { vel[i] = _output_new.vel(i); } } - void copy_position(float *pos) { + void copy_position(float *pos) + { for (unsigned i = 0; i < 3; i++) { pos[i] = _output_new.pos(i); } } - void copy_timestamp(uint64_t *time_us) { + void copy_timestamp(uint64_t *time_us) + { *time_us = _imu_time_last; } diff --git a/EKF/mag_fusion.cpp b/EKF/mag_fusion.cpp index a592007ab5..91ba8071bf 100644 --- a/EKF/mag_fusion.cpp +++ b/EKF/mag_fusion.cpp @@ -61,12 +61,12 @@ void Ekf::fuseMag(uint8_t index) SH_MAG[0] = sq(q0) - sq(q1) + sq(q2) - sq(q3); SH_MAG[1] = sq(q0) + sq(q1) - sq(q2) - sq(q3); SH_MAG[2] = sq(q0) - sq(q1) - sq(q2) + sq(q3); - SH_MAG[3] = 2*q0*q1 + 2*q2*q3; - SH_MAG[4] = 2*q0*q3 + 2*q1*q2; - SH_MAG[5] = 2*q0*q2 + 2*q1*q3; - SH_MAG[6] = magE*(2*q0*q1 - 2*q2*q3); - SH_MAG[7] = 2*q1*q3 - 2*q0*q2; - SH_MAG[8] = 2*q0*q3; + SH_MAG[3] = 2 * q0 * q1 + 2 * q2 * q3; + SH_MAG[4] = 2 * q0 * q3 + 2 * q1 * q2; + SH_MAG[5] = 2 * q0 * q2 + 2 * q1 * q3; + SH_MAG[6] = magE * (2 * q0 * q1 - 2 * q2 * q3); + SH_MAG[7] = 2 * q1 * q3 - 2 * q0 * q2; + SH_MAG[8] = 2 * q0 * q3; // rotate magnetometer earth field state into body frame matrix::Dcm R_to_body(_state.quat_nominal); @@ -75,10 +75,9 @@ void Ekf::fuseMag(uint8_t index) Vector3f mag_I_rot = R_to_body * _state.mag_I; // compute magnetometer innovations - float mag_innov[3] = {}; - mag_innov[0] = (mag_I_rot(0) + _state.mag_B(0)) - _mag_sample_delayed.mag(0); - mag_innov[1] = (mag_I_rot(1) + _state.mag_B(1)) - _mag_sample_delayed.mag(1); - mag_innov[2] = (mag_I_rot(2) + _state.mag_B(2)) - _mag_sample_delayed.mag(2); + _mag_innov[0] = (mag_I_rot(0) + _state.mag_B(0)) - _mag_sample_delayed.mag(0); + _mag_innov[1] = (mag_I_rot(1) + _state.mag_B(1)) - _mag_sample_delayed.mag(1); + _mag_innov[2] = (mag_I_rot(2) + _state.mag_B(2)) - _mag_sample_delayed.mag(2); // XXX Do mag checks here! @@ -88,8 +87,8 @@ void Ekf::fuseMag(uint8_t index) // index corresponds to a mag axis (x, y, z) if (index == 0) { - H_MAG[1] = SH_MAG[6] - magD*SH_MAG[2] - magN*SH_MAG[5]; - H_MAG[2] = magE*SH_MAG[0] + magD*SH_MAG[3] - magN*(SH_MAG[8] - 2*q1*q2); + H_MAG[1] = SH_MAG[6] - magD * SH_MAG[2] - magN * SH_MAG[5]; + H_MAG[2] = magE * SH_MAG[0] + magD * SH_MAG[3] - magN * (SH_MAG[8] - 2 * q1 * q2); H_MAG[16] = SH_MAG[1]; H_MAG[17] = SH_MAG[4]; H_MAG[18] = SH_MAG[7]; @@ -97,119 +96,241 @@ void Ekf::fuseMag(uint8_t index) // intermediate variables float SK_MX[4] = {}; - SK_MX[0] = 1/(P[19][19] + R_MAG - P[1][19]*(magD*SH_MAG[2] - SH_MAG[6] + magN*SH_MAG[5]) + P[16][19]*SH_MAG[1] + P[17][19]*SH_MAG[4] + P[18][19]*SH_MAG[7] + P[2][19]*(magE*SH_MAG[0] + magD*SH_MAG[3] - magN*(SH_MAG[8] - 2*q1*q2)) - (magD*SH_MAG[2] - SH_MAG[6] + magN*SH_MAG[5])*(P[19][1] - P[1][1]*(magD*SH_MAG[2] - SH_MAG[6] + magN*SH_MAG[5]) + P[16][1]*SH_MAG[1] + P[17][1]*SH_MAG[4] + P[18][1]*SH_MAG[7] + P[2][1]*(magE*SH_MAG[0] + magD*SH_MAG[3] - magN*(SH_MAG[8] - 2*q1*q2))) + SH_MAG[1]*(P[19][16] - P[1][16]*(magD*SH_MAG[2] - SH_MAG[6] + magN*SH_MAG[5]) + P[16][16]*SH_MAG[1] + P[17][16]*SH_MAG[4] + P[18][16]*SH_MAG[7] + P[2][16]*(magE*SH_MAG[0] + magD*SH_MAG[3] - magN*(SH_MAG[8] - 2*q1*q2))) + SH_MAG[4]*(P[19][17] - P[1][17]*(magD*SH_MAG[2] - SH_MAG[6] + magN*SH_MAG[5]) + P[16][17]*SH_MAG[1] + P[17][17]*SH_MAG[4] + P[18][17]*SH_MAG[7] + P[2][17]*(magE*SH_MAG[0] + magD*SH_MAG[3] - magN*(SH_MAG[8] - 2*q1*q2))) + SH_MAG[7]*(P[19][18] - P[1][18]*(magD*SH_MAG[2] - SH_MAG[6] + magN*SH_MAG[5]) + P[16][18]*SH_MAG[1] + P[17][18]*SH_MAG[4] + P[18][18]*SH_MAG[7] + P[2][18]*(magE*SH_MAG[0] + magD*SH_MAG[3] - magN*(SH_MAG[8] - 2*q1*q2))) + (magE*SH_MAG[0] + magD*SH_MAG[3] - magN*(SH_MAG[8] - 2*q1*q2))*(P[19][2] - P[1][2]*(magD*SH_MAG[2] - SH_MAG[6] + magN*SH_MAG[5]) + P[16][2]*SH_MAG[1] + P[17][2]*SH_MAG[4] + P[18][2]*SH_MAG[7] + P[2][2]*(magE*SH_MAG[0] + magD*SH_MAG[3] - magN*(SH_MAG[8] - 2*q1*q2)))); - SK_MX[1] = magE*SH_MAG[0] + magD*SH_MAG[3] - magN*(SH_MAG[8] - 2*q1*q2); - SK_MX[2] = magD*SH_MAG[2] - SH_MAG[6] + magN*SH_MAG[5]; + SK_MX[0] = 1 / (P[19][19] + R_MAG - P[1][19] * (magD * SH_MAG[2] - SH_MAG[6] + magN * SH_MAG[5]) + P[16][19] * SH_MAG[1] + + P[17][19] * SH_MAG[4] + P[18][19] * SH_MAG[7] + P[2][19] * (magE * SH_MAG[0] + magD * SH_MAG[3] - magN * + (SH_MAG[8] - 2 * q1 * q2)) - (magD * SH_MAG[2] - SH_MAG[6] + magN * SH_MAG[5]) * (P[19][1] - P[1][1] * + (magD * SH_MAG[2] - SH_MAG[6] + magN * SH_MAG[5]) + P[16][1] * SH_MAG[1] + P[17][1] * SH_MAG[4] + P[18][1] * SH_MAG[7] + + P[2][1] * (magE * SH_MAG[0] + magD * SH_MAG[3] - magN * (SH_MAG[8] - 2 * q1 * q2))) + SH_MAG[1] * + (P[19][16] - P[1][16] * (magD * SH_MAG[2] - SH_MAG[6] + magN * SH_MAG[5]) + P[16][16] * SH_MAG[1] + P[17][16] * + SH_MAG[4] + P[18][16] * SH_MAG[7] + P[2][16] * (magE * SH_MAG[0] + magD * SH_MAG[3] - magN * + (SH_MAG[8] - 2 * q1 * q2))) + SH_MAG[4] * (P[19][17] - P[1][17] * (magD * SH_MAG[2] - SH_MAG[6] + magN * SH_MAG[5]) + + P[16][17] * SH_MAG[1] + P[17][17] * SH_MAG[4] + P[18][17] * SH_MAG[7] + P[2][17] * (magE * SH_MAG[0] + magD * SH_MAG[3] + - magN * (SH_MAG[8] - 2 * q1 * q2))) + SH_MAG[7] * (P[19][18] - P[1][18] * (magD * SH_MAG[2] - SH_MAG[6] + magN * + SH_MAG[5]) + P[16][18] * SH_MAG[1] + P[17][18] * SH_MAG[4] + P[18][18] * SH_MAG[7] + P[2][18] * + (magE * SH_MAG[0] + magD * SH_MAG[3] - magN * (SH_MAG[8] - 2 * q1 * q2))) + (magE * SH_MAG[0] + magD * SH_MAG[3] - + magN * (SH_MAG[8] - 2 * q1 * q2)) * (P[19][2] - P[1][2] * (magD * SH_MAG[2] - SH_MAG[6] + magN * SH_MAG[5]) + P[16][2] * + SH_MAG[1] + P[17][2] * SH_MAG[4] + P[18][2] * SH_MAG[7] + P[2][2] * (magE * SH_MAG[0] + magD * SH_MAG[3] - magN * + (SH_MAG[8] - 2 * q1 * q2)))); + SK_MX[1] = magE * SH_MAG[0] + magD * SH_MAG[3] - magN * (SH_MAG[8] - 2 * q1 * q2); + SK_MX[2] = magD * SH_MAG[2] - SH_MAG[6] + magN * SH_MAG[5]; SK_MX[3] = SH_MAG[7]; - Kfusion[0] = SK_MX[0]*(P[0][19] + P[0][16]*SH_MAG[1] + P[0][17]*SH_MAG[4] - P[0][1]*SK_MX[2] + P[0][2]*SK_MX[1] + P[0][18]*SK_MX[3]); - Kfusion[1] = SK_MX[0]*(P[1][19] + P[1][16]*SH_MAG[1] + P[1][17]*SH_MAG[4] - P[1][1]*SK_MX[2] + P[1][2]*SK_MX[1] + P[1][18]*SK_MX[3]); - Kfusion[2] = SK_MX[0]*(P[2][19] + P[2][16]*SH_MAG[1] + P[2][17]*SH_MAG[4] - P[2][1]*SK_MX[2] + P[2][2]*SK_MX[1] + P[2][18]*SK_MX[3]); - Kfusion[3] = SK_MX[0]*(P[3][19] + P[3][16]*SH_MAG[1] + P[3][17]*SH_MAG[4] - P[3][1]*SK_MX[2] + P[3][2]*SK_MX[1] + P[3][18]*SK_MX[3]); - Kfusion[4] = SK_MX[0]*(P[4][19] + P[4][16]*SH_MAG[1] + P[4][17]*SH_MAG[4] - P[4][1]*SK_MX[2] + P[4][2]*SK_MX[1] + P[4][18]*SK_MX[3]); - Kfusion[5] = SK_MX[0]*(P[5][19] + P[5][16]*SH_MAG[1] + P[5][17]*SH_MAG[4] - P[5][1]*SK_MX[2] + P[5][2]*SK_MX[1] + P[5][18]*SK_MX[3]); - Kfusion[6] = SK_MX[0]*(P[6][19] + P[6][16]*SH_MAG[1] + P[6][17]*SH_MAG[4] - P[6][1]*SK_MX[2] + P[6][2]*SK_MX[1] + P[6][18]*SK_MX[3]); - Kfusion[7] = SK_MX[0]*(P[7][19] + P[7][16]*SH_MAG[1] + P[7][17]*SH_MAG[4] - P[7][1]*SK_MX[2] + P[7][2]*SK_MX[1] + P[7][18]*SK_MX[3]); - Kfusion[8] = SK_MX[0]*(P[8][19] + P[8][16]*SH_MAG[1] + P[8][17]*SH_MAG[4] - P[8][1]*SK_MX[2] + P[8][2]*SK_MX[1] + P[8][18]*SK_MX[3]); - Kfusion[9] = SK_MX[0]*(P[9][19] + P[9][16]*SH_MAG[1] + P[9][17]*SH_MAG[4] - P[9][1]*SK_MX[2] + P[9][2]*SK_MX[1] + P[9][18]*SK_MX[3]); - Kfusion[10] = SK_MX[0]*(P[10][19] + P[10][16]*SH_MAG[1] + P[10][17]*SH_MAG[4] - P[10][1]*SK_MX[2] + P[10][2]*SK_MX[1] + P[10][18]*SK_MX[3]); - Kfusion[11] = SK_MX[0]*(P[11][19] + P[11][16]*SH_MAG[1] + P[11][17]*SH_MAG[4] - P[11][1]*SK_MX[2] + P[11][2]*SK_MX[1] + P[11][18]*SK_MX[3]); - Kfusion[12] = SK_MX[0]*(P[12][19] + P[12][16]*SH_MAG[1] + P[12][17]*SH_MAG[4] - P[12][1]*SK_MX[2] + P[12][2]*SK_MX[1] + P[12][18]*SK_MX[3]); - Kfusion[13] = SK_MX[0]*(P[13][19] + P[13][16]*SH_MAG[1] + P[13][17]*SH_MAG[4] - P[13][1]*SK_MX[2] + P[13][2]*SK_MX[1] + P[13][18]*SK_MX[3]); - Kfusion[14] = SK_MX[0]*(P[14][19] + P[14][16]*SH_MAG[1] + P[14][17]*SH_MAG[4] - P[14][1]*SK_MX[2] + P[14][2]*SK_MX[1] + P[14][18]*SK_MX[3]); - Kfusion[15] = SK_MX[0]*(P[15][19] + P[15][16]*SH_MAG[1] + P[15][17]*SH_MAG[4] - P[15][1]*SK_MX[2] + P[15][2]*SK_MX[1] + P[15][18]*SK_MX[3]); - Kfusion[16] = SK_MX[0]*(P[16][19] + P[16][16]*SH_MAG[1] + P[16][17]*SH_MAG[4] - P[16][1]*SK_MX[2] + P[16][2]*SK_MX[1] + P[16][18]*SK_MX[3]); - Kfusion[17] = SK_MX[0]*(P[17][19] + P[17][16]*SH_MAG[1] + P[17][17]*SH_MAG[4] - P[17][1]*SK_MX[2] + P[17][2]*SK_MX[1] + P[17][18]*SK_MX[3]); - Kfusion[18] = SK_MX[0]*(P[18][19] + P[18][16]*SH_MAG[1] + P[18][17]*SH_MAG[4] - P[18][1]*SK_MX[2] + P[18][2]*SK_MX[1] + P[18][18]*SK_MX[3]); - Kfusion[19] = SK_MX[0]*(P[19][19] + P[19][16]*SH_MAG[1] + P[19][17]*SH_MAG[4] - P[19][1]*SK_MX[2] + P[19][2]*SK_MX[1] + P[19][18]*SK_MX[3]); - Kfusion[20] = SK_MX[0]*(P[20][19] + P[20][16]*SH_MAG[1] + P[20][17]*SH_MAG[4] - P[20][1]*SK_MX[2] + P[20][2]*SK_MX[1] + P[20][18]*SK_MX[3]); - Kfusion[21] = SK_MX[0]*(P[21][19] + P[21][16]*SH_MAG[1] + P[21][17]*SH_MAG[4] - P[21][1]*SK_MX[2] + P[21][2]*SK_MX[1] + P[21][18]*SK_MX[3]); - Kfusion[22] = SK_MX[0]*(P[22][19] + P[22][16]*SH_MAG[1] + P[22][17]*SH_MAG[4] - P[22][1]*SK_MX[2] + P[22][2]*SK_MX[1] + P[22][18]*SK_MX[3]); - Kfusion[23] = SK_MX[0]*(P[23][19] + P[23][16]*SH_MAG[1] + P[23][17]*SH_MAG[4] - P[23][1]*SK_MX[2] + P[23][2]*SK_MX[1] + P[23][18]*SK_MX[3]); + _mag_innov_var[0] = 1 / SK_MX[0]; + + Kfusion[0] = SK_MX[0] * (P[0][19] + P[0][16] * SH_MAG[1] + P[0][17] * SH_MAG[4] - P[0][1] * SK_MX[2] + P[0][2] * + SK_MX[1] + P[0][18] * SK_MX[3]); + Kfusion[1] = SK_MX[0] * (P[1][19] + P[1][16] * SH_MAG[1] + P[1][17] * SH_MAG[4] - P[1][1] * SK_MX[2] + P[1][2] * + SK_MX[1] + P[1][18] * SK_MX[3]); + Kfusion[2] = SK_MX[0] * (P[2][19] + P[2][16] * SH_MAG[1] + P[2][17] * SH_MAG[4] - P[2][1] * SK_MX[2] + P[2][2] * + SK_MX[1] + P[2][18] * SK_MX[3]); + Kfusion[3] = SK_MX[0] * (P[3][19] + P[3][16] * SH_MAG[1] + P[3][17] * SH_MAG[4] - P[3][1] * SK_MX[2] + P[3][2] * + SK_MX[1] + P[3][18] * SK_MX[3]); + Kfusion[4] = SK_MX[0] * (P[4][19] + P[4][16] * SH_MAG[1] + P[4][17] * SH_MAG[4] - P[4][1] * SK_MX[2] + P[4][2] * + SK_MX[1] + P[4][18] * SK_MX[3]); + Kfusion[5] = SK_MX[0] * (P[5][19] + P[5][16] * SH_MAG[1] + P[5][17] * SH_MAG[4] - P[5][1] * SK_MX[2] + P[5][2] * + SK_MX[1] + P[5][18] * SK_MX[3]); + Kfusion[6] = SK_MX[0] * (P[6][19] + P[6][16] * SH_MAG[1] + P[6][17] * SH_MAG[4] - P[6][1] * SK_MX[2] + P[6][2] * + SK_MX[1] + P[6][18] * SK_MX[3]); + Kfusion[7] = SK_MX[0] * (P[7][19] + P[7][16] * SH_MAG[1] + P[7][17] * SH_MAG[4] - P[7][1] * SK_MX[2] + P[7][2] * + SK_MX[1] + P[7][18] * SK_MX[3]); + Kfusion[8] = SK_MX[0] * (P[8][19] + P[8][16] * SH_MAG[1] + P[8][17] * SH_MAG[4] - P[8][1] * SK_MX[2] + P[8][2] * + SK_MX[1] + P[8][18] * SK_MX[3]); + Kfusion[9] = SK_MX[0] * (P[9][19] + P[9][16] * SH_MAG[1] + P[9][17] * SH_MAG[4] - P[9][1] * SK_MX[2] + P[9][2] * + SK_MX[1] + P[9][18] * SK_MX[3]); + Kfusion[10] = SK_MX[0] * (P[10][19] + P[10][16] * SH_MAG[1] + P[10][17] * SH_MAG[4] - P[10][1] * SK_MX[2] + P[10][2] * + SK_MX[1] + P[10][18] * SK_MX[3]); + Kfusion[11] = SK_MX[0] * (P[11][19] + P[11][16] * SH_MAG[1] + P[11][17] * SH_MAG[4] - P[11][1] * SK_MX[2] + P[11][2] * + SK_MX[1] + P[11][18] * SK_MX[3]); + Kfusion[12] = SK_MX[0] * (P[12][19] + P[12][16] * SH_MAG[1] + P[12][17] * SH_MAG[4] - P[12][1] * SK_MX[2] + P[12][2] * + SK_MX[1] + P[12][18] * SK_MX[3]); + Kfusion[13] = SK_MX[0] * (P[13][19] + P[13][16] * SH_MAG[1] + P[13][17] * SH_MAG[4] - P[13][1] * SK_MX[2] + P[13][2] * + SK_MX[1] + P[13][18] * SK_MX[3]); + Kfusion[14] = SK_MX[0] * (P[14][19] + P[14][16] * SH_MAG[1] + P[14][17] * SH_MAG[4] - P[14][1] * SK_MX[2] + P[14][2] * + SK_MX[1] + P[14][18] * SK_MX[3]); + Kfusion[15] = SK_MX[0] * (P[15][19] + P[15][16] * SH_MAG[1] + P[15][17] * SH_MAG[4] - P[15][1] * SK_MX[2] + P[15][2] * + SK_MX[1] + P[15][18] * SK_MX[3]); + Kfusion[16] = SK_MX[0] * (P[16][19] + P[16][16] * SH_MAG[1] + P[16][17] * SH_MAG[4] - P[16][1] * SK_MX[2] + P[16][2] * + SK_MX[1] + P[16][18] * SK_MX[3]); + Kfusion[17] = SK_MX[0] * (P[17][19] + P[17][16] * SH_MAG[1] + P[17][17] * SH_MAG[4] - P[17][1] * SK_MX[2] + P[17][2] * + SK_MX[1] + P[17][18] * SK_MX[3]); + Kfusion[18] = SK_MX[0] * (P[18][19] + P[18][16] * SH_MAG[1] + P[18][17] * SH_MAG[4] - P[18][1] * SK_MX[2] + P[18][2] * + SK_MX[1] + P[18][18] * SK_MX[3]); + Kfusion[19] = SK_MX[0] * (P[19][19] + P[19][16] * SH_MAG[1] + P[19][17] * SH_MAG[4] - P[19][1] * SK_MX[2] + P[19][2] * + SK_MX[1] + P[19][18] * SK_MX[3]); + Kfusion[20] = SK_MX[0] * (P[20][19] + P[20][16] * SH_MAG[1] + P[20][17] * SH_MAG[4] - P[20][1] * SK_MX[2] + P[20][2] * + SK_MX[1] + P[20][18] * SK_MX[3]); + Kfusion[21] = SK_MX[0] * (P[21][19] + P[21][16] * SH_MAG[1] + P[21][17] * SH_MAG[4] - P[21][1] * SK_MX[2] + P[21][2] * + SK_MX[1] + P[21][18] * SK_MX[3]); + Kfusion[22] = SK_MX[0] * (P[22][19] + P[22][16] * SH_MAG[1] + P[22][17] * SH_MAG[4] - P[22][1] * SK_MX[2] + P[22][2] * + SK_MX[1] + P[22][18] * SK_MX[3]); + Kfusion[23] = SK_MX[0] * (P[23][19] + P[23][16] * SH_MAG[1] + P[23][17] * SH_MAG[4] - P[23][1] * SK_MX[2] + P[23][2] * + SK_MX[1] + P[23][18] * SK_MX[3]); } else if (index == 1) { - H_MAG[0] = magD*SH_MAG[2] - SH_MAG[6] + magN*SH_MAG[5]; - H_MAG[2] = - magE*SH_MAG[4] - magD*SH_MAG[7] - magN*SH_MAG[1]; - H_MAG[16] = 2*q1*q2 - SH_MAG[8]; + H_MAG[0] = magD * SH_MAG[2] - SH_MAG[6] + magN * SH_MAG[5]; + H_MAG[2] = - magE * SH_MAG[4] - magD * SH_MAG[7] - magN * SH_MAG[1]; + H_MAG[16] = 2 * q1 * q2 - SH_MAG[8]; H_MAG[17] = SH_MAG[0]; H_MAG[18] = SH_MAG[3]; H_MAG[20] = 1; // intermediate variables - note SK_MY[0] is 1/(innovation variance) float SK_MY[4]; - SK_MY[0] = 1/(P[20][20] + R_MAG + P[0][20]*(magD*SH_MAG[2] - SH_MAG[6] + magN*SH_MAG[5]) + P[17][20]*SH_MAG[0] + P[18][20]*SH_MAG[3] - (SH_MAG[8] - 2*q1*q2)*(P[20][16] + P[0][16]*(magD*SH_MAG[2] - SH_MAG[6] + magN*SH_MAG[5]) + P[17][16]*SH_MAG[0] + P[18][16]*SH_MAG[3] - P[2][16]*(magE*SH_MAG[4] + magD*SH_MAG[7] + magN*SH_MAG[1]) - P[16][16]*(SH_MAG[8] - 2*q1*q2)) - P[2][20]*(magE*SH_MAG[4] + magD*SH_MAG[7] + magN*SH_MAG[1]) + (magD*SH_MAG[2] - SH_MAG[6] + magN*SH_MAG[5])*(P[20][0] + P[0][0]*(magD*SH_MAG[2] - SH_MAG[6] + magN*SH_MAG[5]) + P[17][0]*SH_MAG[0] + P[18][0]*SH_MAG[3] - P[2][0]*(magE*SH_MAG[4] + magD*SH_MAG[7] + magN*SH_MAG[1]) - P[16][0]*(SH_MAG[8] - 2*q1*q2)) + SH_MAG[0]*(P[20][17] + P[0][17]*(magD*SH_MAG[2] - SH_MAG[6] + magN*SH_MAG[5]) + P[17][17]*SH_MAG[0] + P[18][17]*SH_MAG[3] - P[2][17]*(magE*SH_MAG[4] + magD*SH_MAG[7] + magN*SH_MAG[1]) - P[16][17]*(SH_MAG[8] - 2*q1*q2)) + SH_MAG[3]*(P[20][18] + P[0][18]*(magD*SH_MAG[2] - SH_MAG[6] + magN*SH_MAG[5]) + P[17][18]*SH_MAG[0] + P[18][18]*SH_MAG[3] - P[2][18]*(magE*SH_MAG[4] + magD*SH_MAG[7] + magN*SH_MAG[1]) - P[16][18]*(SH_MAG[8] - 2*q1*q2)) - P[16][20]*(SH_MAG[8] - 2*q1*q2) - (magE*SH_MAG[4] + magD*SH_MAG[7] + magN*SH_MAG[1])*(P[20][2] + P[0][2]*(magD*SH_MAG[2] - SH_MAG[6] + magN*SH_MAG[5]) + P[17][2]*SH_MAG[0] + P[18][2]*SH_MAG[3] - P[2][2]*(magE*SH_MAG[4] + magD*SH_MAG[7] + magN*SH_MAG[1]) - P[16][2]*(SH_MAG[8] - 2*q1*q2))); - SK_MY[1] = magE*SH_MAG[4] + magD*SH_MAG[7] + magN*SH_MAG[1]; - SK_MY[2] = magD*SH_MAG[2] - SH_MAG[6] + magN*SH_MAG[5]; - SK_MY[3] = SH_MAG[8] - 2*q1*q2; + SK_MY[0] = 1 / (P[20][20] + R_MAG + P[0][20] * (magD * SH_MAG[2] - SH_MAG[6] + magN * SH_MAG[5]) + P[17][20] * SH_MAG[0] + + P[18][20] * SH_MAG[3] - (SH_MAG[8] - 2 * q1 * q2) * (P[20][16] + P[0][16] * (magD * SH_MAG[2] - SH_MAG[6] + magN * + SH_MAG[5]) + P[17][16] * SH_MAG[0] + P[18][16] * SH_MAG[3] - P[2][16] * (magE * SH_MAG[4] + magD * SH_MAG[7] + magN * + SH_MAG[1]) - P[16][16] * (SH_MAG[8] - 2 * q1 * q2)) - P[2][20] * (magE * SH_MAG[4] + magD * SH_MAG[7] + magN * + SH_MAG[1]) + (magD * SH_MAG[2] - SH_MAG[6] + magN * SH_MAG[5]) * (P[20][0] + P[0][0] * + (magD * SH_MAG[2] - SH_MAG[6] + magN * SH_MAG[5]) + P[17][0] * SH_MAG[0] + P[18][0] * SH_MAG[3] - P[2][0] * + (magE * SH_MAG[4] + magD * SH_MAG[7] + magN * SH_MAG[1]) - P[16][0] * (SH_MAG[8] - 2 * q1 * q2)) + SH_MAG[0] * + (P[20][17] + P[0][17] * (magD * SH_MAG[2] - SH_MAG[6] + magN * SH_MAG[5]) + P[17][17] * SH_MAG[0] + P[18][17] * + SH_MAG[3] - P[2][17] * (magE * SH_MAG[4] + magD * SH_MAG[7] + magN * SH_MAG[1]) - P[16][17] * + (SH_MAG[8] - 2 * q1 * q2)) + SH_MAG[3] * (P[20][18] + P[0][18] * (magD * SH_MAG[2] - SH_MAG[6] + magN * SH_MAG[5]) + + P[17][18] * SH_MAG[0] + P[18][18] * SH_MAG[3] - P[2][18] * (magE * SH_MAG[4] + magD * SH_MAG[7] + magN * SH_MAG[1]) - + P[16][18] * (SH_MAG[8] - 2 * q1 * q2)) - P[16][20] * (SH_MAG[8] - 2 * q1 * q2) - (magE * SH_MAG[4] + magD * SH_MAG[7] + + magN * SH_MAG[1]) * (P[20][2] + P[0][2] * (magD * SH_MAG[2] - SH_MAG[6] + magN * SH_MAG[5]) + P[17][2] * SH_MAG[0] + + P[18][2] * SH_MAG[3] - P[2][2] * (magE * SH_MAG[4] + magD * SH_MAG[7] + magN * SH_MAG[1]) - P[16][2] * + (SH_MAG[8] - 2 * q1 * q2))); + SK_MY[1] = magE * SH_MAG[4] + magD * SH_MAG[7] + magN * SH_MAG[1]; + SK_MY[2] = magD * SH_MAG[2] - SH_MAG[6] + magN * SH_MAG[5]; + SK_MY[3] = SH_MAG[8] - 2 * q1 * q2; - Kfusion[0] = SK_MY[0]*(P[0][20] + P[0][17]*SH_MAG[0] + P[0][18]*SH_MAG[3] + P[0][0]*SK_MY[2] - P[0][2]*SK_MY[1] - P[0][16]*SK_MY[3]); - Kfusion[1] = SK_MY[0]*(P[1][20] + P[1][17]*SH_MAG[0] + P[1][18]*SH_MAG[3] + P[1][0]*SK_MY[2] - P[1][2]*SK_MY[1] - P[1][16]*SK_MY[3]); - Kfusion[2] = SK_MY[0]*(P[2][20] + P[2][17]*SH_MAG[0] + P[2][18]*SH_MAG[3] + P[2][0]*SK_MY[2] - P[2][2]*SK_MY[1] - P[2][16]*SK_MY[3]); - Kfusion[3] = SK_MY[0]*(P[3][20] + P[3][17]*SH_MAG[0] + P[3][18]*SH_MAG[3] + P[3][0]*SK_MY[2] - P[3][2]*SK_MY[1] - P[3][16]*SK_MY[3]); - Kfusion[4] = SK_MY[0]*(P[4][20] + P[4][17]*SH_MAG[0] + P[4][18]*SH_MAG[3] + P[4][0]*SK_MY[2] - P[4][2]*SK_MY[1] - P[4][16]*SK_MY[3]); - Kfusion[5] = SK_MY[0]*(P[5][20] + P[5][17]*SH_MAG[0] + P[5][18]*SH_MAG[3] + P[5][0]*SK_MY[2] - P[5][2]*SK_MY[1] - P[5][16]*SK_MY[3]); - Kfusion[6] = SK_MY[0]*(P[6][20] + P[6][17]*SH_MAG[0] + P[6][18]*SH_MAG[3] + P[6][0]*SK_MY[2] - P[6][2]*SK_MY[1] - P[6][16]*SK_MY[3]); - Kfusion[7] = SK_MY[0]*(P[7][20] + P[7][17]*SH_MAG[0] + P[7][18]*SH_MAG[3] + P[7][0]*SK_MY[2] - P[7][2]*SK_MY[1] - P[7][16]*SK_MY[3]); - Kfusion[8] = SK_MY[0]*(P[8][20] + P[8][17]*SH_MAG[0] + P[8][18]*SH_MAG[3] + P[8][0]*SK_MY[2] - P[8][2]*SK_MY[1] - P[8][16]*SK_MY[3]); - Kfusion[9] = SK_MY[0]*(P[9][20] + P[9][17]*SH_MAG[0] + P[9][18]*SH_MAG[3] + P[9][0]*SK_MY[2] - P[9][2]*SK_MY[1] - P[9][16]*SK_MY[3]); - Kfusion[10] = SK_MY[0]*(P[10][20] + P[10][17]*SH_MAG[0] + P[10][18]*SH_MAG[3] + P[10][0]*SK_MY[2] - P[10][2]*SK_MY[1] - P[10][16]*SK_MY[3]); - Kfusion[11] = SK_MY[0]*(P[11][20] + P[11][17]*SH_MAG[0] + P[11][18]*SH_MAG[3] + P[11][0]*SK_MY[2] - P[11][2]*SK_MY[1] - P[11][16]*SK_MY[3]); - Kfusion[12] = SK_MY[0]*(P[12][20] + P[12][17]*SH_MAG[0] + P[12][18]*SH_MAG[3] + P[12][0]*SK_MY[2] - P[12][2]*SK_MY[1] - P[12][16]*SK_MY[3]); - Kfusion[13] = SK_MY[0]*(P[13][20] + P[13][17]*SH_MAG[0] + P[13][18]*SH_MAG[3] + P[13][0]*SK_MY[2] - P[13][2]*SK_MY[1] - P[13][16]*SK_MY[3]); - Kfusion[14] = SK_MY[0]*(P[14][20] + P[14][17]*SH_MAG[0] + P[14][18]*SH_MAG[3] + P[14][0]*SK_MY[2] - P[14][2]*SK_MY[1] - P[14][16]*SK_MY[3]); - Kfusion[15] = SK_MY[0]*(P[15][20] + P[15][17]*SH_MAG[0] + P[15][18]*SH_MAG[3] + P[15][0]*SK_MY[2] - P[15][2]*SK_MY[1] - P[15][16]*SK_MY[3]); - Kfusion[16] = SK_MY[0]*(P[16][20] + P[16][17]*SH_MAG[0] + P[16][18]*SH_MAG[3] + P[16][0]*SK_MY[2] - P[16][2]*SK_MY[1] - P[16][16]*SK_MY[3]); - Kfusion[17] = SK_MY[0]*(P[17][20] + P[17][17]*SH_MAG[0] + P[17][18]*SH_MAG[3] + P[17][0]*SK_MY[2] - P[17][2]*SK_MY[1] - P[17][16]*SK_MY[3]); - Kfusion[18] = SK_MY[0]*(P[18][20] + P[18][17]*SH_MAG[0] + P[18][18]*SH_MAG[3] + P[18][0]*SK_MY[2] - P[18][2]*SK_MY[1] - P[18][16]*SK_MY[3]); - Kfusion[19] = SK_MY[0]*(P[19][20] + P[19][17]*SH_MAG[0] + P[19][18]*SH_MAG[3] + P[19][0]*SK_MY[2] - P[19][2]*SK_MY[1] - P[19][16]*SK_MY[3]); - Kfusion[20] = SK_MY[0]*(P[20][20] + P[20][17]*SH_MAG[0] + P[20][18]*SH_MAG[3] + P[20][0]*SK_MY[2] - P[20][2]*SK_MY[1] - P[20][16]*SK_MY[3]); - Kfusion[21] = SK_MY[0]*(P[21][20] + P[21][17]*SH_MAG[0] + P[21][18]*SH_MAG[3] + P[21][0]*SK_MY[2] - P[21][2]*SK_MY[1] - P[21][16]*SK_MY[3]); - Kfusion[22] = SK_MY[0]*(P[22][20] + P[22][17]*SH_MAG[0] + P[22][18]*SH_MAG[3] + P[22][0]*SK_MY[2] - P[22][2]*SK_MY[1] - P[22][16]*SK_MY[3]); - Kfusion[23] = SK_MY[0]*(P[23][20] + P[23][17]*SH_MAG[0] + P[23][18]*SH_MAG[3] + P[23][0]*SK_MY[2] - P[23][2]*SK_MY[1] - P[23][16]*SK_MY[3]); + _mag_innov_var[1] = 1 / SK_MY[0]; + + Kfusion[0] = SK_MY[0] * (P[0][20] + P[0][17] * SH_MAG[0] + P[0][18] * SH_MAG[3] + P[0][0] * SK_MY[2] - P[0][2] * + SK_MY[1] - P[0][16] * SK_MY[3]); + Kfusion[1] = SK_MY[0] * (P[1][20] + P[1][17] * SH_MAG[0] + P[1][18] * SH_MAG[3] + P[1][0] * SK_MY[2] - P[1][2] * + SK_MY[1] - P[1][16] * SK_MY[3]); + Kfusion[2] = SK_MY[0] * (P[2][20] + P[2][17] * SH_MAG[0] + P[2][18] * SH_MAG[3] + P[2][0] * SK_MY[2] - P[2][2] * + SK_MY[1] - P[2][16] * SK_MY[3]); + Kfusion[3] = SK_MY[0] * (P[3][20] + P[3][17] * SH_MAG[0] + P[3][18] * SH_MAG[3] + P[3][0] * SK_MY[2] - P[3][2] * + SK_MY[1] - P[3][16] * SK_MY[3]); + Kfusion[4] = SK_MY[0] * (P[4][20] + P[4][17] * SH_MAG[0] + P[4][18] * SH_MAG[3] + P[4][0] * SK_MY[2] - P[4][2] * + SK_MY[1] - P[4][16] * SK_MY[3]); + Kfusion[5] = SK_MY[0] * (P[5][20] + P[5][17] * SH_MAG[0] + P[5][18] * SH_MAG[3] + P[5][0] * SK_MY[2] - P[5][2] * + SK_MY[1] - P[5][16] * SK_MY[3]); + Kfusion[6] = SK_MY[0] * (P[6][20] + P[6][17] * SH_MAG[0] + P[6][18] * SH_MAG[3] + P[6][0] * SK_MY[2] - P[6][2] * + SK_MY[1] - P[6][16] * SK_MY[3]); + Kfusion[7] = SK_MY[0] * (P[7][20] + P[7][17] * SH_MAG[0] + P[7][18] * SH_MAG[3] + P[7][0] * SK_MY[2] - P[7][2] * + SK_MY[1] - P[7][16] * SK_MY[3]); + Kfusion[8] = SK_MY[0] * (P[8][20] + P[8][17] * SH_MAG[0] + P[8][18] * SH_MAG[3] + P[8][0] * SK_MY[2] - P[8][2] * + SK_MY[1] - P[8][16] * SK_MY[3]); + Kfusion[9] = SK_MY[0] * (P[9][20] + P[9][17] * SH_MAG[0] + P[9][18] * SH_MAG[3] + P[9][0] * SK_MY[2] - P[9][2] * + SK_MY[1] - P[9][16] * SK_MY[3]); + Kfusion[10] = SK_MY[0] * (P[10][20] + P[10][17] * SH_MAG[0] + P[10][18] * SH_MAG[3] + P[10][0] * SK_MY[2] - P[10][2] * + SK_MY[1] - P[10][16] * SK_MY[3]); + Kfusion[11] = SK_MY[0] * (P[11][20] + P[11][17] * SH_MAG[0] + P[11][18] * SH_MAG[3] + P[11][0] * SK_MY[2] - P[11][2] * + SK_MY[1] - P[11][16] * SK_MY[3]); + Kfusion[12] = SK_MY[0] * (P[12][20] + P[12][17] * SH_MAG[0] + P[12][18] * SH_MAG[3] + P[12][0] * SK_MY[2] - P[12][2] * + SK_MY[1] - P[12][16] * SK_MY[3]); + Kfusion[13] = SK_MY[0] * (P[13][20] + P[13][17] * SH_MAG[0] + P[13][18] * SH_MAG[3] + P[13][0] * SK_MY[2] - P[13][2] * + SK_MY[1] - P[13][16] * SK_MY[3]); + Kfusion[14] = SK_MY[0] * (P[14][20] + P[14][17] * SH_MAG[0] + P[14][18] * SH_MAG[3] + P[14][0] * SK_MY[2] - P[14][2] * + SK_MY[1] - P[14][16] * SK_MY[3]); + Kfusion[15] = SK_MY[0] * (P[15][20] + P[15][17] * SH_MAG[0] + P[15][18] * SH_MAG[3] + P[15][0] * SK_MY[2] - P[15][2] * + SK_MY[1] - P[15][16] * SK_MY[3]); + Kfusion[16] = SK_MY[0] * (P[16][20] + P[16][17] * SH_MAG[0] + P[16][18] * SH_MAG[3] + P[16][0] * SK_MY[2] - P[16][2] * + SK_MY[1] - P[16][16] * SK_MY[3]); + Kfusion[17] = SK_MY[0] * (P[17][20] + P[17][17] * SH_MAG[0] + P[17][18] * SH_MAG[3] + P[17][0] * SK_MY[2] - P[17][2] * + SK_MY[1] - P[17][16] * SK_MY[3]); + Kfusion[18] = SK_MY[0] * (P[18][20] + P[18][17] * SH_MAG[0] + P[18][18] * SH_MAG[3] + P[18][0] * SK_MY[2] - P[18][2] * + SK_MY[1] - P[18][16] * SK_MY[3]); + Kfusion[19] = SK_MY[0] * (P[19][20] + P[19][17] * SH_MAG[0] + P[19][18] * SH_MAG[3] + P[19][0] * SK_MY[2] - P[19][2] * + SK_MY[1] - P[19][16] * SK_MY[3]); + Kfusion[20] = SK_MY[0] * (P[20][20] + P[20][17] * SH_MAG[0] + P[20][18] * SH_MAG[3] + P[20][0] * SK_MY[2] - P[20][2] * + SK_MY[1] - P[20][16] * SK_MY[3]); + Kfusion[21] = SK_MY[0] * (P[21][20] + P[21][17] * SH_MAG[0] + P[21][18] * SH_MAG[3] + P[21][0] * SK_MY[2] - P[21][2] * + SK_MY[1] - P[21][16] * SK_MY[3]); + Kfusion[22] = SK_MY[0] * (P[22][20] + P[22][17] * SH_MAG[0] + P[22][18] * SH_MAG[3] + P[22][0] * SK_MY[2] - P[22][2] * + SK_MY[1] - P[22][16] * SK_MY[3]); + Kfusion[23] = SK_MY[0] * (P[23][20] + P[23][17] * SH_MAG[0] + P[23][18] * SH_MAG[3] + P[23][0] * SK_MY[2] - P[23][2] * + SK_MY[1] - P[23][16] * SK_MY[3]); } else if (index == 2) { - H_MAG[0] = magN*(SH_MAG[8] - 2*q1*q2) - magD*SH_MAG[3] - magE*SH_MAG[0]; - H_MAG[1] = magE*SH_MAG[4] + magD*SH_MAG[7] + magN*SH_MAG[1]; + H_MAG[0] = magN * (SH_MAG[8] - 2 * q1 * q2) - magD * SH_MAG[3] - magE * SH_MAG[0]; + H_MAG[1] = magE * SH_MAG[4] + magD * SH_MAG[7] + magN * SH_MAG[1]; H_MAG[16] = SH_MAG[5]; - H_MAG[17] = 2*q2*q3 - 2*q0*q1; + H_MAG[17] = 2 * q2 * q3 - 2 * q0 * q1; H_MAG[18] = SH_MAG[2]; H_MAG[21] = 1; // intermediate variables float SK_MZ[4]; - SK_MZ[0] = 1/(P[21][21] + R_MAG + P[16][21]*SH_MAG[5] + P[18][21]*SH_MAG[2] - (2*q0*q1 - 2*q2*q3)*(P[21][17] + P[16][17]*SH_MAG[5] + P[18][17]*SH_MAG[2] - P[0][17]*(magE*SH_MAG[0] + magD*SH_MAG[3] - magN*(SH_MAG[8] - 2*q1*q2)) + P[1][17]*(magE*SH_MAG[4] + magD*SH_MAG[7] + magN*SH_MAG[1]) - P[17][17]*(2*q0*q1 - 2*q2*q3)) - P[0][21]*(magE*SH_MAG[0] + magD*SH_MAG[3] - magN*(SH_MAG[8] - 2*q1*q2)) + P[1][21]*(magE*SH_MAG[4] + magD*SH_MAG[7] + magN*SH_MAG[1]) + SH_MAG[5]*(P[21][16] + P[16][16]*SH_MAG[5] + P[18][16]*SH_MAG[2] - P[0][16]*(magE*SH_MAG[0] + magD*SH_MAG[3] - magN*(SH_MAG[8] - 2*q1*q2)) + P[1][16]*(magE*SH_MAG[4] + magD*SH_MAG[7] + magN*SH_MAG[1]) - P[17][16]*(2*q0*q1 - 2*q2*q3)) + SH_MAG[2]*(P[21][18] + P[16][18]*SH_MAG[5] + P[18][18]*SH_MAG[2] - P[0][18]*(magE*SH_MAG[0] + magD*SH_MAG[3] - magN*(SH_MAG[8] - 2*q1*q2)) + P[1][18]*(magE*SH_MAG[4] + magD*SH_MAG[7] + magN*SH_MAG[1]) - P[17][18]*(2*q0*q1 - 2*q2*q3)) - (magE*SH_MAG[0] + magD*SH_MAG[3] - magN*(SH_MAG[8] - 2*q1*q2))*(P[21][0] + P[16][0]*SH_MAG[5] + P[18][0]*SH_MAG[2] - P[0][0]*(magE*SH_MAG[0] + magD*SH_MAG[3] - magN*(SH_MAG[8] - 2*q1*q2)) + P[1][0]*(magE*SH_MAG[4] + magD*SH_MAG[7] + magN*SH_MAG[1]) - P[17][0]*(2*q0*q1 - 2*q2*q3)) - P[17][21]*(2*q0*q1 - 2*q2*q3) + (magE*SH_MAG[4] + magD*SH_MAG[7] + magN*SH_MAG[1])*(P[21][1] + P[16][1]*SH_MAG[5] + P[18][1]*SH_MAG[2] - P[0][1]*(magE*SH_MAG[0] + magD*SH_MAG[3] - magN*(SH_MAG[8] - 2*q1*q2)) + P[1][1]*(magE*SH_MAG[4] + magD*SH_MAG[7] + magN*SH_MAG[1]) - P[17][1]*(2*q0*q1 - 2*q2*q3))); - SK_MZ[1] = magE*SH_MAG[0] + magD*SH_MAG[3] - magN*(SH_MAG[8] - 2*q1*q2); - SK_MZ[2] = magE*SH_MAG[4] + magD*SH_MAG[7] + magN*SH_MAG[1]; - SK_MZ[3] = 2*q0*q1 - 2*q2*q3; + SK_MZ[0] = 1 / (P[21][21] + R_MAG + P[16][21] * SH_MAG[5] + P[18][21] * SH_MAG[2] - (2 * q0 * q1 - 2 * q2 * q3) * + (P[21][17] + P[16][17] * SH_MAG[5] + P[18][17] * SH_MAG[2] - P[0][17] * (magE * SH_MAG[0] + magD * SH_MAG[3] - magN * + (SH_MAG[8] - 2 * q1 * q2)) + P[1][17] * (magE * SH_MAG[4] + magD * SH_MAG[7] + magN * SH_MAG[1]) - P[17][17] * + (2 * q0 * q1 - 2 * q2 * q3)) - P[0][21] * (magE * SH_MAG[0] + magD * SH_MAG[3] - magN * + (SH_MAG[8] - 2 * q1 * q2)) + P[1][21] * (magE * SH_MAG[4] + magD * SH_MAG[7] + magN * SH_MAG[1]) + SH_MAG[5] * + (P[21][16] + P[16][16] * SH_MAG[5] + P[18][16] * SH_MAG[2] - P[0][16] * (magE * SH_MAG[0] + magD * SH_MAG[3] - magN * + (SH_MAG[8] - 2 * q1 * q2)) + P[1][16] * (magE * SH_MAG[4] + magD * SH_MAG[7] + magN * SH_MAG[1]) - P[17][16] * + (2 * q0 * q1 - 2 * q2 * q3)) + SH_MAG[2] * (P[21][18] + P[16][18] * SH_MAG[5] + P[18][18] * SH_MAG[2] - P[0][18] * + (magE * SH_MAG[0] + magD * SH_MAG[3] - magN * (SH_MAG[8] - 2 * q1 * q2)) + P[1][18] * + (magE * SH_MAG[4] + magD * SH_MAG[7] + magN * SH_MAG[1]) - P[17][18] * (2 * q0 * q1 - 2 * q2 * q3)) - + (magE * SH_MAG[0] + magD * SH_MAG[3] - magN * (SH_MAG[8] - 2 * q1 * q2)) * (P[21][0] + P[16][0] * SH_MAG[5] + P[18][0] * + SH_MAG[2] - P[0][0] * (magE * SH_MAG[0] + magD * SH_MAG[3] - magN * (SH_MAG[8] - 2 * q1 * q2)) + P[1][0] * + (magE * SH_MAG[4] + magD * SH_MAG[7] + magN * SH_MAG[1]) - P[17][0] * (2 * q0 * q1 - 2 * q2 * q3)) - P[17][21] * + (2 * q0 * q1 - 2 * q2 * q3) + (magE * SH_MAG[4] + magD * SH_MAG[7] + magN * SH_MAG[1]) * + (P[21][1] + P[16][1] * SH_MAG[5] + P[18][1] * SH_MAG[2] - P[0][1] * (magE * SH_MAG[0] + magD * SH_MAG[3] - magN * + (SH_MAG[8] - 2 * q1 * q2)) + P[1][1] * (magE * SH_MAG[4] + magD * SH_MAG[7] + magN * SH_MAG[1]) - P[17][1] * + (2 * q0 * q1 - 2 * q2 * q3))); + SK_MZ[1] = magE * SH_MAG[0] + magD * SH_MAG[3] - magN * (SH_MAG[8] - 2 * q1 * q2); + SK_MZ[2] = magE * SH_MAG[4] + magD * SH_MAG[7] + magN * SH_MAG[1]; + SK_MZ[3] = 2 * q0 * q1 - 2 * q2 * q3; - Kfusion[0] = SK_MZ[0]*(P[0][21] + P[0][18]*SH_MAG[2] + P[0][16]*SH_MAG[5] - P[0][0]*SK_MZ[1] + P[0][1]*SK_MZ[2] - P[0][17]*SK_MZ[3]); - Kfusion[1] = SK_MZ[0]*(P[1][21] + P[1][18]*SH_MAG[2] + P[1][16]*SH_MAG[5] - P[1][0]*SK_MZ[1] + P[1][1]*SK_MZ[2] - P[1][17]*SK_MZ[3]); - Kfusion[2] = SK_MZ[0]*(P[2][21] + P[2][18]*SH_MAG[2] + P[2][16]*SH_MAG[5] - P[2][0]*SK_MZ[1] + P[2][1]*SK_MZ[2] - P[2][17]*SK_MZ[3]); - Kfusion[3] = SK_MZ[0]*(P[3][21] + P[3][18]*SH_MAG[2] + P[3][16]*SH_MAG[5] - P[3][0]*SK_MZ[1] + P[3][1]*SK_MZ[2] - P[3][17]*SK_MZ[3]); - Kfusion[4] = SK_MZ[0]*(P[4][21] + P[4][18]*SH_MAG[2] + P[4][16]*SH_MAG[5] - P[4][0]*SK_MZ[1] + P[4][1]*SK_MZ[2] - P[4][17]*SK_MZ[3]); - Kfusion[5] = SK_MZ[0]*(P[5][21] + P[5][18]*SH_MAG[2] + P[5][16]*SH_MAG[5] - P[5][0]*SK_MZ[1] + P[5][1]*SK_MZ[2] - P[5][17]*SK_MZ[3]); - Kfusion[6] = SK_MZ[0]*(P[6][21] + P[6][18]*SH_MAG[2] + P[6][16]*SH_MAG[5] - P[6][0]*SK_MZ[1] + P[6][1]*SK_MZ[2] - P[6][17]*SK_MZ[3]); - Kfusion[7] = SK_MZ[0]*(P[7][21] + P[7][18]*SH_MAG[2] + P[7][16]*SH_MAG[5] - P[7][0]*SK_MZ[1] + P[7][1]*SK_MZ[2] - P[7][17]*SK_MZ[3]); - Kfusion[8] = SK_MZ[0]*(P[8][21] + P[8][18]*SH_MAG[2] + P[8][16]*SH_MAG[5] - P[8][0]*SK_MZ[1] + P[8][1]*SK_MZ[2] - P[8][17]*SK_MZ[3]); - Kfusion[9] = SK_MZ[0]*(P[9][21] + P[9][18]*SH_MAG[2] + P[9][16]*SH_MAG[5] - P[9][0]*SK_MZ[1] + P[9][1]*SK_MZ[2] - P[9][17]*SK_MZ[3]); - Kfusion[10] = SK_MZ[0]*(P[10][21] + P[10][18]*SH_MAG[2] + P[10][16]*SH_MAG[5] - P[10][0]*SK_MZ[1] + P[10][1]*SK_MZ[2] - P[10][17]*SK_MZ[3]); - Kfusion[11] = SK_MZ[0]*(P[11][21] + P[11][18]*SH_MAG[2] + P[11][16]*SH_MAG[5] - P[11][0]*SK_MZ[1] + P[11][1]*SK_MZ[2] - P[11][17]*SK_MZ[3]); - Kfusion[12] = SK_MZ[0]*(P[12][21] + P[12][18]*SH_MAG[2] + P[12][16]*SH_MAG[5] - P[12][0]*SK_MZ[1] + P[12][1]*SK_MZ[2] - P[12][17]*SK_MZ[3]); - Kfusion[13] = SK_MZ[0]*(P[13][21] + P[13][18]*SH_MAG[2] + P[13][16]*SH_MAG[5] - P[13][0]*SK_MZ[1] + P[13][1]*SK_MZ[2] - P[13][17]*SK_MZ[3]); - Kfusion[14] = SK_MZ[0]*(P[14][21] + P[14][18]*SH_MAG[2] + P[14][16]*SH_MAG[5] - P[14][0]*SK_MZ[1] + P[14][1]*SK_MZ[2] - P[14][17]*SK_MZ[3]); - Kfusion[15] = SK_MZ[0]*(P[15][21] + P[15][18]*SH_MAG[2] + P[15][16]*SH_MAG[5] - P[15][0]*SK_MZ[1] + P[15][1]*SK_MZ[2] - P[15][17]*SK_MZ[3]); - Kfusion[16] = SK_MZ[0]*(P[16][21] + P[16][18]*SH_MAG[2] + P[16][16]*SH_MAG[5] - P[16][0]*SK_MZ[1] + P[16][1]*SK_MZ[2] - P[16][17]*SK_MZ[3]); - Kfusion[17] = SK_MZ[0]*(P[17][21] + P[17][18]*SH_MAG[2] + P[17][16]*SH_MAG[5] - P[17][0]*SK_MZ[1] + P[17][1]*SK_MZ[2] - P[17][17]*SK_MZ[3]); - Kfusion[18] = SK_MZ[0]*(P[18][21] + P[18][18]*SH_MAG[2] + P[18][16]*SH_MAG[5] - P[18][0]*SK_MZ[1] + P[18][1]*SK_MZ[2] - P[18][17]*SK_MZ[3]); - Kfusion[19] = SK_MZ[0]*(P[19][21] + P[19][18]*SH_MAG[2] + P[19][16]*SH_MAG[5] - P[19][0]*SK_MZ[1] + P[19][1]*SK_MZ[2] - P[19][17]*SK_MZ[3]); - Kfusion[20] = SK_MZ[0]*(P[20][21] + P[20][18]*SH_MAG[2] + P[20][16]*SH_MAG[5] - P[20][0]*SK_MZ[1] + P[20][1]*SK_MZ[2] - P[20][17]*SK_MZ[3]); - Kfusion[21] = SK_MZ[0]*(P[21][21] + P[21][18]*SH_MAG[2] + P[21][16]*SH_MAG[5] - P[21][0]*SK_MZ[1] + P[21][1]*SK_MZ[2] - P[21][17]*SK_MZ[3]); - Kfusion[22] = SK_MZ[0]*(P[22][21] + P[22][18]*SH_MAG[2] + P[22][16]*SH_MAG[5] - P[22][0]*SK_MZ[1] + P[22][1]*SK_MZ[2] - P[22][17]*SK_MZ[3]); - Kfusion[23] = SK_MZ[0]*(P[23][21] + P[23][18]*SH_MAG[2] + P[23][16]*SH_MAG[5] - P[23][0]*SK_MZ[1] + P[23][1]*SK_MZ[2] - P[23][17]*SK_MZ[3]); + _mag_innov_var[2] = 1 / SK_MZ[0]; + + Kfusion[0] = SK_MZ[0] * (P[0][21] + P[0][18] * SH_MAG[2] + P[0][16] * SH_MAG[5] - P[0][0] * SK_MZ[1] + P[0][1] * + SK_MZ[2] - P[0][17] * SK_MZ[3]); + Kfusion[1] = SK_MZ[0] * (P[1][21] + P[1][18] * SH_MAG[2] + P[1][16] * SH_MAG[5] - P[1][0] * SK_MZ[1] + P[1][1] * + SK_MZ[2] - P[1][17] * SK_MZ[3]); + Kfusion[2] = SK_MZ[0] * (P[2][21] + P[2][18] * SH_MAG[2] + P[2][16] * SH_MAG[5] - P[2][0] * SK_MZ[1] + P[2][1] * + SK_MZ[2] - P[2][17] * SK_MZ[3]); + Kfusion[3] = SK_MZ[0] * (P[3][21] + P[3][18] * SH_MAG[2] + P[3][16] * SH_MAG[5] - P[3][0] * SK_MZ[1] + P[3][1] * + SK_MZ[2] - P[3][17] * SK_MZ[3]); + Kfusion[4] = SK_MZ[0] * (P[4][21] + P[4][18] * SH_MAG[2] + P[4][16] * SH_MAG[5] - P[4][0] * SK_MZ[1] + P[4][1] * + SK_MZ[2] - P[4][17] * SK_MZ[3]); + Kfusion[5] = SK_MZ[0] * (P[5][21] + P[5][18] * SH_MAG[2] + P[5][16] * SH_MAG[5] - P[5][0] * SK_MZ[1] + P[5][1] * + SK_MZ[2] - P[5][17] * SK_MZ[3]); + Kfusion[6] = SK_MZ[0] * (P[6][21] + P[6][18] * SH_MAG[2] + P[6][16] * SH_MAG[5] - P[6][0] * SK_MZ[1] + P[6][1] * + SK_MZ[2] - P[6][17] * SK_MZ[3]); + Kfusion[7] = SK_MZ[0] * (P[7][21] + P[7][18] * SH_MAG[2] + P[7][16] * SH_MAG[5] - P[7][0] * SK_MZ[1] + P[7][1] * + SK_MZ[2] - P[7][17] * SK_MZ[3]); + Kfusion[8] = SK_MZ[0] * (P[8][21] + P[8][18] * SH_MAG[2] + P[8][16] * SH_MAG[5] - P[8][0] * SK_MZ[1] + P[8][1] * + SK_MZ[2] - P[8][17] * SK_MZ[3]); + Kfusion[9] = SK_MZ[0] * (P[9][21] + P[9][18] * SH_MAG[2] + P[9][16] * SH_MAG[5] - P[9][0] * SK_MZ[1] + P[9][1] * + SK_MZ[2] - P[9][17] * SK_MZ[3]); + Kfusion[10] = SK_MZ[0] * (P[10][21] + P[10][18] * SH_MAG[2] + P[10][16] * SH_MAG[5] - P[10][0] * SK_MZ[1] + P[10][1] * + SK_MZ[2] - P[10][17] * SK_MZ[3]); + Kfusion[11] = SK_MZ[0] * (P[11][21] + P[11][18] * SH_MAG[2] + P[11][16] * SH_MAG[5] - P[11][0] * SK_MZ[1] + P[11][1] * + SK_MZ[2] - P[11][17] * SK_MZ[3]); + Kfusion[12] = SK_MZ[0] * (P[12][21] + P[12][18] * SH_MAG[2] + P[12][16] * SH_MAG[5] - P[12][0] * SK_MZ[1] + P[12][1] * + SK_MZ[2] - P[12][17] * SK_MZ[3]); + Kfusion[13] = SK_MZ[0] * (P[13][21] + P[13][18] * SH_MAG[2] + P[13][16] * SH_MAG[5] - P[13][0] * SK_MZ[1] + P[13][1] * + SK_MZ[2] - P[13][17] * SK_MZ[3]); + Kfusion[14] = SK_MZ[0] * (P[14][21] + P[14][18] * SH_MAG[2] + P[14][16] * SH_MAG[5] - P[14][0] * SK_MZ[1] + P[14][1] * + SK_MZ[2] - P[14][17] * SK_MZ[3]); + Kfusion[15] = SK_MZ[0] * (P[15][21] + P[15][18] * SH_MAG[2] + P[15][16] * SH_MAG[5] - P[15][0] * SK_MZ[1] + P[15][1] * + SK_MZ[2] - P[15][17] * SK_MZ[3]); + Kfusion[16] = SK_MZ[0] * (P[16][21] + P[16][18] * SH_MAG[2] + P[16][16] * SH_MAG[5] - P[16][0] * SK_MZ[1] + P[16][1] * + SK_MZ[2] - P[16][17] * SK_MZ[3]); + Kfusion[17] = SK_MZ[0] * (P[17][21] + P[17][18] * SH_MAG[2] + P[17][16] * SH_MAG[5] - P[17][0] * SK_MZ[1] + P[17][1] * + SK_MZ[2] - P[17][17] * SK_MZ[3]); + Kfusion[18] = SK_MZ[0] * (P[18][21] + P[18][18] * SH_MAG[2] + P[18][16] * SH_MAG[5] - P[18][0] * SK_MZ[1] + P[18][1] * + SK_MZ[2] - P[18][17] * SK_MZ[3]); + Kfusion[19] = SK_MZ[0] * (P[19][21] + P[19][18] * SH_MAG[2] + P[19][16] * SH_MAG[5] - P[19][0] * SK_MZ[1] + P[19][1] * + SK_MZ[2] - P[19][17] * SK_MZ[3]); + Kfusion[20] = SK_MZ[0] * (P[20][21] + P[20][18] * SH_MAG[2] + P[20][16] * SH_MAG[5] - P[20][0] * SK_MZ[1] + P[20][1] * + SK_MZ[2] - P[20][17] * SK_MZ[3]); + Kfusion[21] = SK_MZ[0] * (P[21][21] + P[21][18] * SH_MAG[2] + P[21][16] * SH_MAG[5] - P[21][0] * SK_MZ[1] + P[21][1] * + SK_MZ[2] - P[21][17] * SK_MZ[3]); + Kfusion[22] = SK_MZ[0] * (P[22][21] + P[22][18] * SH_MAG[2] + P[22][16] * SH_MAG[5] - P[22][0] * SK_MZ[1] + P[22][1] * + SK_MZ[2] - P[22][17] * SK_MZ[3]); + Kfusion[23] = SK_MZ[0] * (P[23][21] + P[23][18] * SH_MAG[2] + P[23][16] * SH_MAG[5] - P[23][0] * SK_MZ[1] + P[23][1] * + SK_MZ[2] - P[23][17] * SK_MZ[3]); } else { } @@ -218,7 +339,7 @@ void Ekf::fuseMag(uint8_t index) // by definition our error state is zero at the time of fusion _state.ang_error.setZero(); - fuse(Kfusion, mag_innov[index]); + fuse(Kfusion, _mag_innov[index]); Quaternion q_correction; q_correction.from_axis_angle(_state.ang_error); @@ -243,6 +364,7 @@ void Ekf::fuseMag(uint8_t index) } float KHP[_k_num_states][_k_num_states] = {}; + for (unsigned row = 0; row < _k_num_states; row++) { for (unsigned column = 0; column < _k_num_states; column++) { float tmp = KH[row][0] * P[0][column]; @@ -280,43 +402,43 @@ void Ekf::fuseHeading() float magY = _mag_sample_delayed.mag(1); float magZ = _mag_sample_delayed.mag(2); - float R_mag = _params.mag_heading_noise; + float R_mag = _params.mag_heading_noise; - float t2 = q0*q0; - float t3 = q1*q1; - float t4 = q2*q2; - float t5 = q3*q3; - float t6 = q0*q2*2.0f; - float t7 = q1*q3*2.0f; - float t8 = t6+t7; - float t9 = q0*q3*2.0f; - float t13 = q1*q2*2.0f; - float t10 = t9-t13; - float t11 = t2+t3-t4-t5; - float t12 = magX*t11; - float t14 = magZ*t8; - float t19 = magY*t10; - float t15 = t12+t14-t19; - float t16 = t2-t3+t4-t5; - float t17 = q0*q1*2.0f; - float t24 = q2*q3*2.0f; - float t18 = t17-t24; - float t20 = 1.0f/t15; - float t21 = magY*t16; - float t22 = t9+t13; - float t23 = magX*t22; - float t28 = magZ*t18; - float t25 = t21+t23-t28; - float t29 = t20*t25; + float t2 = q0 * q0; + float t3 = q1 * q1; + float t4 = q2 * q2; + float t5 = q3 * q3; + float t6 = q0 * q2 * 2.0f; + float t7 = q1 * q3 * 2.0f; + float t8 = t6 + t7; + float t9 = q0 * q3 * 2.0f; + float t13 = q1 * q2 * 2.0f; + float t10 = t9 - t13; + float t11 = t2 + t3 - t4 - t5; + float t12 = magX * t11; + float t14 = magZ * t8; + float t19 = magY * t10; + float t15 = t12 + t14 - t19; + float t16 = t2 - t3 + t4 - t5; + float t17 = q0 * q1 * 2.0f; + float t24 = q2 * q3 * 2.0f; + float t18 = t17 - t24; + float t20 = 1.0f / t15; + float t21 = magY * t16; + float t22 = t9 + t13; + float t23 = magX * t22; + float t28 = magZ * t18; + float t25 = t21 + t23 - t28; + float t29 = t20 * t25; float t26 = tan(t29); - float t27 = 1.0f/(t15*t15); - float t30 = t26*t26; - float t31 = t30+1.0f; + float t27 = 1.0f / (t15 * t15); + float t30 = t26 * t26; + float t31 = t30 + 1.0f; float H_MAG[3] = {}; - H_MAG[0] = -t31*(t20*(magZ*t16+magY*t18)+t25*t27*(magY*t8+magZ*t10)); - H_MAG[1] = t31*(t20*(magX*t18+magZ*t22)+t25*t27*(magX*t8-magZ*t11)); - H_MAG[2] = t31*(t20*(magX*t16-magY*t22)+t25*t27*(magX*t10+magY*t11)); + H_MAG[0] = -t31 * (t20 * (magZ * t16 + magY * t18) + t25 * t27 * (magY * t8 + magZ * t10)); + H_MAG[1] = t31 * (t20 * (magX * t18 + magZ * t22) + t25 * t27 * (magX * t8 - magZ * t11)); + H_MAG[2] = t31 * (t20 * (magX * t16 - magY * t22) + t25 * t27 * (magX * t10 + magY * t11)); // calculate innovation matrix::Dcm R_to_earth(_state.quat_nominal); @@ -325,15 +447,19 @@ void Ekf::fuseHeading() float innovation = atan2f(mag_earth_pred(1), mag_earth_pred(0)) - math::radians(_params.mag_declination_deg); innovation = math::constrain(innovation, -0.5f, 0.5f); + _heading_innov = innovation; float innovation_var = R_mag; + _heading_innov_var = innovation_var; // calculate innovation variance float PH[3] = {}; + for (unsigned row = 0; row < 3; row++) { for (unsigned column = 0; column < 3; column++) { - PH[row] += P[row][column]*H_MAG[column]; + PH[row] += P[row][column] * H_MAG[column]; } + innovation_var += H_MAG[row] * PH[row]; } @@ -342,6 +468,7 @@ void Ekf::fuseHeading() _fault_status.bad_mag_x = false; _fault_status.bad_mag_y = false; _fault_status.bad_mag_z = false; + } else { // our innovation variance has decreased, our calculation is thus badly conditioned _fault_status.bad_mag_x = true; @@ -357,15 +484,18 @@ void Ekf::fuseHeading() // calculate kalman gain float Kfusion[_k_num_states] = {}; + for (unsigned row = 0; row < _k_num_states; row++) { for (unsigned column = 0; column < 3; column++) { Kfusion[row] += P[row][column] * H_MAG[column]; } + Kfusion[row] *= innovation_var_inv; } // innovation test ratio - float yaw_test_ratio = sq(innovation) / (sq(math::max(0.01f * (float)_params.heading_innov_gate, 1.0f)) * innovation_var); + float yaw_test_ratio = sq(innovation) / (sq(math::max(0.01f * (float)_params.heading_innov_gate, + 1.0f)) * innovation_var); // set the magnetometer unhealthy if the test fails if (yaw_test_ratio > 1.0f) { @@ -377,6 +507,7 @@ void Ekf::fuseHeading() if (_in_air) { return; } + } else { _mag_healthy = true; } @@ -385,7 +516,7 @@ void Ekf::fuseHeading() fuse(Kfusion, innovation); // correct the nominal quaternion - Quaternion dq; + Quaternion dq; dq.from_axis_angle(_state.ang_error); _state.quat_nominal = dq * _state.quat_nominal; _state.quat_nominal.normalize(); diff --git a/EKF/vel_pos_fusion.cpp b/EKF/vel_pos_fusion.cpp index 0b3632fdd8..1bcd6e230b 100644 --- a/EKF/vel_pos_fusion.cpp +++ b/EKF/vel_pos_fusion.cpp @@ -44,29 +44,28 @@ void Ekf::fuseVelPosHeight() { bool fuse_map[6] = {}; - float innovations[6] = {}; float R[6] = {}; float Kfusion[24] = {}; // calculate innovations if (_fuse_hor_vel) { fuse_map[0] = fuse_map[1] = true; - innovations[0] = _state.vel(0) - _gps_sample_delayed.vel(0); - innovations[1] = _state.vel(1) - _gps_sample_delayed.vel(1); + _vel_pos_innov[0] = _state.vel(0) - _gps_sample_delayed.vel(0); + _vel_pos_innov[1] = _state.vel(1) - _gps_sample_delayed.vel(1); R[0] = _params.gps_vel_noise; R[1] = _params.gps_vel_noise; } if (_fuse_vert_vel) { fuse_map[2] = true; - innovations[2] = _state.vel(2) - _gps_sample_delayed.vel(2); + _vel_pos_innov[2] = _state.vel(2) - _gps_sample_delayed.vel(2); R[2] = _params.gps_vel_noise; } if (_fuse_pos) { fuse_map[3] = fuse_map[4] = true; - innovations[3] = _state.pos(0) - _gps_sample_delayed.pos(0); - innovations[4] = _state.pos(1) - _gps_sample_delayed.pos(1); + _vel_pos_innov[3] = _state.pos(0) - _gps_sample_delayed.pos(0); + _vel_pos_innov[4] = _state.pos(1) - _gps_sample_delayed.pos(1); R[3] = _params.gps_pos_noise; R[4] = _params.gps_pos_noise; @@ -74,7 +73,7 @@ void Ekf::fuseVelPosHeight() if (_fuse_height) { fuse_map[5] = true; - innovations[5] = _state.pos(2) - (-_baro_sample_delayed.hgt); // baro measurement has inversed z axis + _vel_pos_innov[5] = _state.pos(2) - (-_baro_sample_delayed.hgt); // baro measurement has inversed z axis R[5] = _params.baro_noise; } @@ -89,6 +88,7 @@ void Ekf::fuseVelPosHeight() // compute the innovation variance SK = HPH + R float S = P[state_index][state_index] + R[obs_index]; + _vel_pos_innov_var[obs_index] = S; S = 1.0f / S; // calculate kalman gain K = PHS @@ -100,7 +100,7 @@ void Ekf::fuseVelPosHeight() _state.ang_error.setZero(); // fuse the observation - fuse(Kfusion, innovations[obs_index]); + fuse(Kfusion, _vel_pos_innov[obs_index]); // correct the nominal quaternion Quaternion dq;