mirror of
https://gitee.com/mirrors_PX4/PX4-Autopilot.git
synced 2026-10-20 07:33:34 +08:00
EKF: Convert magnetic field observation methods to use SymPy generated code (#879)
* EKF: Add comparison test for mag field fusion generated code * EKF: convert mag field fusion to use SymPy generated code * EKF: Add test comparison program for yaw fusion equations * Stop setting 0 to 0 * Reduce if/else statement to only if * EKF: more accurate implementation for sequential fusion of magnetometer data * test: update change indicator * Use matrix::SparseVector<float, 24, ...> for observation jacobian * Adapt the auto code generation to allow for different bracket styles * Add auto generated code for mag fusion * Add generic computation of KHP * Apply generic computation of KHP to mag fusion * tests: update change indicator * tests: update change indicator Co-authored-by: kamilritz <kritz@ethz.ch>
This commit is contained in:
Paul Riseborough
co-authored by
kamilritz
kamilritz
parent
ec93490890
commit
21cc46edd7
+397
-418
@@ -34,6 +34,7 @@
|
||||
/**
|
||||
* @file heading_fusion.cpp
|
||||
* Magnetometer fusion methods.
|
||||
* Equations generated using EKF/python/ekf_derivation/main.py
|
||||
*
|
||||
* @author Roman Bast <bapstroman@gmail.com>
|
||||
* @author Paul Riseborough <p_riseborough@live.com.au>
|
||||
@@ -47,29 +48,112 @@
|
||||
void Ekf::fuseMag()
|
||||
{
|
||||
// assign intermediate variables
|
||||
const float q0 = _state.quat_nominal(0);
|
||||
const float q1 = _state.quat_nominal(1);
|
||||
const float q2 = _state.quat_nominal(2);
|
||||
const float q3 = _state.quat_nominal(3);
|
||||
const float &q0 = _state.quat_nominal(0);
|
||||
const float &q1 = _state.quat_nominal(1);
|
||||
const float &q2 = _state.quat_nominal(2);
|
||||
const float &q3 = _state.quat_nominal(3);
|
||||
|
||||
const float magN = _state.mag_I(0);
|
||||
const float magE = _state.mag_I(1);
|
||||
const float magD = _state.mag_I(2);
|
||||
const float &magN = _state.mag_I(0);
|
||||
const float &magE = _state.mag_I(1);
|
||||
const float &magD = _state.mag_I(2);
|
||||
|
||||
// XYZ Measurement uncertainty. Need to consider timing errors for fast rotations
|
||||
const float R_MAG = sq(fmaxf(_params.mag_noise, 0.0f));
|
||||
|
||||
// intermediate variables from algebraic optimisation
|
||||
float SH_MAG[9];
|
||||
SH_MAG[0] = 2.0f*magD*q3 + 2.0f*magE*q2 + 2.0f*magN*q1;
|
||||
SH_MAG[1] = 2.0f*magD*q0 - 2.0f*magE*q1 + 2.0f*magN*q2;
|
||||
SH_MAG[2] = 2.0f*magD*q1 + 2.0f*magE*q0 - 2.0f*magN*q3;
|
||||
SH_MAG[3] = sq(q3);
|
||||
SH_MAG[4] = sq(q2);
|
||||
SH_MAG[5] = sq(q1);
|
||||
SH_MAG[6] = sq(q0);
|
||||
SH_MAG[7] = 2.0f*magN*q0;
|
||||
SH_MAG[8] = 2.0f*magE*q3;
|
||||
// calculate intermediate variables used for X axis innovation variance, observation Jacobians and Kalman gains
|
||||
const char* numerical_error_covariance_reset_string = "numerical error - covariance reset";
|
||||
const float HKX0 = -magD*q2 + magE*q3 + magN*q0;
|
||||
const float HKX1 = magD*q3 + magE*q2 + magN*q1;
|
||||
const float HKX2 = magE*q1;
|
||||
const float HKX3 = magD*q0;
|
||||
const float HKX4 = magN*q2;
|
||||
const float HKX5 = magD*q1 + magE*q0 - magN*q3;
|
||||
const float HKX6 = powf(q0, 2) + powf(q1, 2) - powf(q2, 2) - powf(q3, 2);
|
||||
const float HKX7 = q0*q3 + q1*q2;
|
||||
const float HKX8 = q1*q3;
|
||||
const float HKX9 = q0*q2;
|
||||
const float HKX10 = 2*HKX7;
|
||||
const float HKX11 = -2*HKX8 + 2*HKX9;
|
||||
const float HKX12 = 2*HKX1;
|
||||
const float HKX13 = 2*HKX0;
|
||||
const float HKX14 = -2*HKX2 + 2*HKX3 + 2*HKX4;
|
||||
const float HKX15 = 2*HKX5;
|
||||
const float HKX16 = HKX10*P(0,17) - HKX11*P(0,18) + HKX12*P(0,1) + HKX13*P(0,0) - HKX14*P(0,2) + HKX15*P(0,3) + HKX6*P(0,16) + P(0,19);
|
||||
const float HKX17 = HKX10*P(16,17) - HKX11*P(16,18) + HKX12*P(1,16) + HKX13*P(0,16) - HKX14*P(2,16) + HKX15*P(3,16) + HKX6*P(16,16) + P(16,19);
|
||||
const float HKX18 = HKX10*P(17,18) - HKX11*P(18,18) + HKX12*P(1,18) + HKX13*P(0,18) - HKX14*P(2,18) + HKX15*P(3,18) + HKX6*P(16,18) + P(18,19);
|
||||
const float HKX19 = HKX10*P(2,17) - HKX11*P(2,18) + HKX12*P(1,2) + HKX13*P(0,2) - HKX14*P(2,2) + HKX15*P(2,3) + HKX6*P(2,16) + P(2,19);
|
||||
const float HKX20 = HKX10*P(17,17) - HKX11*P(17,18) + HKX12*P(1,17) + HKX13*P(0,17) - HKX14*P(2,17) + HKX15*P(3,17) + HKX6*P(16,17) + P(17,19);
|
||||
const float HKX21 = HKX10*P(3,17) - HKX11*P(3,18) + HKX12*P(1,3) + HKX13*P(0,3) - HKX14*P(2,3) + HKX15*P(3,3) + HKX6*P(3,16) + P(3,19);
|
||||
const float HKX22 = HKX10*P(1,17) - HKX11*P(1,18) + HKX12*P(1,1) + HKX13*P(0,1) - HKX14*P(1,2) + HKX15*P(1,3) + HKX6*P(1,16) + P(1,19);
|
||||
const float HKX23 = HKX10*P(17,19) - HKX11*P(18,19) + HKX12*P(1,19) + HKX13*P(0,19) - HKX14*P(2,19) + HKX15*P(3,19) + HKX6*P(16,19) + P(19,19);
|
||||
|
||||
_mag_innov_var(0) = HKX10*HKX20 - HKX11*HKX18 + HKX12*HKX22 + HKX13*HKX16 - HKX14*HKX19 + HKX15*HKX21 + HKX17*HKX6 + HKX23 + R_MAG;
|
||||
|
||||
if (_mag_innov_var(0) < R_MAG) {
|
||||
// the innovation variance contribution from the state covariances is negative which means the covariance matrix is badly conditioned
|
||||
_fault_status.flags.bad_mag_x = true;
|
||||
|
||||
// we need to re-initialise covariances and abort this fusion step
|
||||
resetMagRelatedCovariances();
|
||||
ECL_ERR("magX %s", numerical_error_covariance_reset_string);
|
||||
return;
|
||||
}
|
||||
|
||||
_fault_status.flags.bad_mag_x = false;
|
||||
|
||||
const float HKX24 = 1.0F/_mag_innov_var(0);
|
||||
|
||||
// intermediate variables for calculation of innovations variances for Y and Z axes
|
||||
// don't calculate all terms needed for observation jacobians and Kalman gains because
|
||||
// these will have to be recalculated when the X and Y axes are fused
|
||||
const float IV0 = q0*q1;
|
||||
const float IV1 = q2*q3;
|
||||
const float IV2 = 2*IV0 + 2*IV1;
|
||||
const float IV3 = 2*q0*q3 - 2*q1*q2;
|
||||
const float IV4 = 2*magD*q3 + 2*magE*q2 + 2*magN*q1;
|
||||
const float IV5 = 2*magD*q1 + 2*magE*q0 - 2*magN*q3;
|
||||
const float IV6 = 2*magD*q0 - 2*magE*q1 + 2*magN*q2;
|
||||
const float IV7 = -2*magD*q2 + 2*magE*q3 + 2*magN*q0;
|
||||
const float IV8 = powf(q2, 2);
|
||||
const float IV9 = powf(q3, 2);
|
||||
const float IV10 = powf(q0, 2) - powf(q1, 2);
|
||||
const float IV11 = IV10 + IV8 - IV9;
|
||||
const float IV12 = IV7*P(2,3);
|
||||
const float IV13 = IV5*P(0,1);
|
||||
const float IV14 = IV6*P(0,1);
|
||||
const float IV15 = IV4*P(2,3);
|
||||
const float IV16 = 2*q0*q2 + 2*q1*q3;
|
||||
const float IV17 = 2*IV0 - 2*IV1;
|
||||
const float IV18 = IV10 - IV8 + IV9;
|
||||
|
||||
_mag_innov_var(1) = IV11*P(17,20) + IV11*(IV11*P(17,17) + IV2*P(17,18) - IV3*P(16,17) + IV4*P(2,17) + IV5*P(0,17) + IV6*P(1,17) - IV7*P(3,17) + P(17,20)) + IV2*P(18,20) + IV2*(IV11*P(17,18) + IV2*P(18,18) - IV3*P(16,18) + IV4*P(2,18) + IV5*P(0,18) + IV6*P(1,18) - IV7*P(3,18) + P(18,20)) - IV3*P(16,20) - IV3*(IV11*P(16,17) + IV2*P(16,18) - IV3*P(16,16) + IV4*P(2,16) + IV5*P(0,16) + IV6*P(1,16) - IV7*P(3,16) + P(16,20)) + IV4*P(2,20) + IV4*(IV11*P(2,17) - IV12 + IV2*P(2,18) - IV3*P(2,16) + IV4*P(2,2) + IV5*P(0,2) + IV6*P(1,2) + P(2,20)) + IV5*P(0,20) + IV5*(IV11*P(0,17) + IV14 + IV2*P(0,18) - IV3*P(0,16) + IV4*P(0,2) + IV5*P(0,0) - IV7*P(0,3) + P(0,20)) + IV6*P(1,20) + IV6*(IV11*P(1,17) + IV13 + IV2*P(1,18) - IV3*P(1,16) + IV4*P(1,2) + IV6*P(1,1) - IV7*P(1,3) + P(1,20)) - IV7*P(3,20) - IV7*(IV11*P(3,17) + IV15 + IV2*P(3,18) - IV3*P(3,16) + IV5*P(0,3) + IV6*P(1,3) - IV7*P(3,3) + P(3,20)) + P(20,20) + R_MAG;
|
||||
_mag_innov_var(2) = IV16*P(16,21) + IV16*(IV16*P(16,16) - IV17*P(16,17) + IV18*P(16,18) + IV4*P(3,16) - IV5*P(1,16) + IV6*P(0,16) + IV7*P(2,16) + P(16,21)) - IV17*P(17,21) - IV17*(IV16*P(16,17) - IV17*P(17,17) + IV18*P(17,18) + IV4*P(3,17) - IV5*P(1,17) + IV6*P(0,17) + IV7*P(2,17) + P(17,21)) + IV18*P(18,21) + IV18*(IV16*P(16,18) - IV17*P(17,18) + IV18*P(18,18) + IV4*P(3,18) - IV5*P(1,18) + IV6*P(0,18) + IV7*P(2,18) + P(18,21)) + IV4*P(3,21) + IV4*(IV12 + IV16*P(3,16) - IV17*P(3,17) + IV18*P(3,18) + IV4*P(3,3) - IV5*P(1,3) + IV6*P(0,3) + P(3,21)) - IV5*P(1,21) - IV5*(IV14 + IV16*P(1,16) - IV17*P(1,17) + IV18*P(1,18) + IV4*P(1,3) - IV5*P(1,1) + IV7*P(1,2) + P(1,21)) + IV6*P(0,21) + IV6*(-IV13 + IV16*P(0,16) - IV17*P(0,17) + IV18*P(0,18) + IV4*P(0,3) + IV6*P(0,0) + IV7*P(0,2) + P(0,21)) + IV7*P(2,21) + IV7*(IV15 + IV16*P(2,16) - IV17*P(2,17) + IV18*P(2,18) - IV5*P(1,2) + IV6*P(0,2) + IV7*P(2,2) + P(2,21)) + P(21,21) + R_MAG;
|
||||
|
||||
// chedk innovation variances for being badly conditioned
|
||||
|
||||
if (_mag_innov_var(1) < R_MAG) {
|
||||
// the innovation variance contribution from the state covariances is negtive which means the covariance matrix is badly conditioned
|
||||
_fault_status.flags.bad_mag_y = true;
|
||||
|
||||
// we need to re-initialise covariances and abort this fusion step
|
||||
resetMagRelatedCovariances();
|
||||
ECL_ERR("magY %s", numerical_error_covariance_reset_string);
|
||||
return;
|
||||
}
|
||||
|
||||
_fault_status.flags.bad_mag_y = false;
|
||||
|
||||
if (_mag_innov_var(2) < R_MAG) {
|
||||
// the innovation variance contribution from the state covariances is negative which means the covariance matrix is badly conditioned
|
||||
_fault_status.flags.bad_mag_z = true;
|
||||
|
||||
// we need to re-initialise covariances and abort this fusion step
|
||||
resetMagRelatedCovariances();
|
||||
ECL_ERR("magZ %s", numerical_error_covariance_reset_string);
|
||||
return;
|
||||
}
|
||||
|
||||
_fault_status.flags.bad_mag_z = false;
|
||||
|
||||
// rotate magnetometer earth field state into body frame
|
||||
const Dcmf R_to_body = quatToInverseRotMat(_state.quat_nominal);
|
||||
@@ -84,64 +168,6 @@ void Ekf::fuseMag()
|
||||
_mag_innov(2) = 0.0f;
|
||||
}
|
||||
|
||||
// X axis innovation variance
|
||||
_mag_innov_var(0) = (P(19,19) + R_MAG + P(1,19)*SH_MAG[0] - P(2,19)*SH_MAG[1] + P(3,19)*SH_MAG[2] - P(16,19)*(SH_MAG[3] + SH_MAG[4] - SH_MAG[5] - SH_MAG[6]) + (2.0f*q0*q3 + 2.0f*q1*q2)*(P(19,17) + P(1,17)*SH_MAG[0] - P(2,17)*SH_MAG[1] + P(3,17)*SH_MAG[2] - P(16,17)*(SH_MAG[3] + SH_MAG[4] - SH_MAG[5] - SH_MAG[6]) + P(17,17)*(2.0f*q0*q3 + 2.0f*q1*q2) - P(18,17)*(2.0f*q0*q2 - 2.0f*q1*q3) + P(0,17)*(SH_MAG[7] + SH_MAG[8] - 2.0f*magD*q2)) - (2.0f*q0*q2 - 2.0f*q1*q3)*(P(19,18) + P(1,18)*SH_MAG[0] - P(2,18)*SH_MAG[1] + P(3,18)*SH_MAG[2] - P(16,18)*(SH_MAG[3] + SH_MAG[4] - SH_MAG[5] - SH_MAG[6]) + P(17,18)*(2.0f*q0*q3 + 2.0f*q1*q2) - P(18,18)*(2.0f*q0*q2 - 2.0f*q1*q3) + P(0,18)*(SH_MAG[7] + SH_MAG[8] - 2.0f*magD*q2)) + (SH_MAG[7] + SH_MAG[8] - 2.0f*magD*q2)*(P(19,0) + P(1,0)*SH_MAG[0] - P(2,0)*SH_MAG[1] + P(3,0)*SH_MAG[2] - P(16,0)*(SH_MAG[3] + SH_MAG[4] - SH_MAG[5] - SH_MAG[6]) + P(17,0)*(2.0f*q0*q3 + 2.0f*q1*q2) - P(18,0)*(2.0f*q0*q2 - 2.0f*q1*q3) + P(0,0)*(SH_MAG[7] + SH_MAG[8] - 2.0f*magD*q2)) + P(17,19)*(2.0f*q0*q3 + 2.0f*q1*q2) - P(18,19)*(2.0f*q0*q2 - 2.0f*q1*q3) + SH_MAG[0]*(P(19,1) + P(1,1)*SH_MAG[0] - P(2,1)*SH_MAG[1] + P(3,1)*SH_MAG[2] - P(16,1)*(SH_MAG[3] + SH_MAG[4] - SH_MAG[5] - SH_MAG[6]) + P(17,1)*(2.0f*q0*q3 + 2.0f*q1*q2) - P(18,1)*(2.0f*q0*q2 - 2.0f*q1*q3) + P(0,1)*(SH_MAG[7] + SH_MAG[8] - 2.0f*magD*q2)) - SH_MAG[1]*(P(19,2) + P(1,2)*SH_MAG[0] - P(2,2)*SH_MAG[1] + P(3,2)*SH_MAG[2] - P(16,2)*(SH_MAG[3] + SH_MAG[4] - SH_MAG[5] - SH_MAG[6]) + P(17,2)*(2.0f*q0*q3 + 2.0f*q1*q2) - P(18,2)*(2.0f*q0*q2 - 2.0f*q1*q3) + P(0,2)*(SH_MAG[7] + SH_MAG[8] - 2.0f*magD*q2)) + SH_MAG[2]*(P(19,3) + P(1,3)*SH_MAG[0] - P(2,3)*SH_MAG[1] + P(3,3)*SH_MAG[2] - P(16,3)*(SH_MAG[3] + SH_MAG[4] - SH_MAG[5] - SH_MAG[6]) + P(17,3)*(2.0f*q0*q3 + 2.0f*q1*q2) - P(18,3)*(2.0f*q0*q2 - 2.0f*q1*q3) + P(0,3)*(SH_MAG[7] + SH_MAG[8] - 2.0f*magD*q2)) - (SH_MAG[3] + SH_MAG[4] - SH_MAG[5] - SH_MAG[6])*(P(19,16) + P(1,16)*SH_MAG[0] - P(2,16)*SH_MAG[1] + P(3,16)*SH_MAG[2] - P(16,16)*(SH_MAG[3] + SH_MAG[4] - SH_MAG[5] - SH_MAG[6]) + P(17,16)*(2.0f*q0*q3 + 2.0f*q1*q2) - P(18,16)*(2.0f*q0*q2 - 2.0f*q1*q3) + P(0,16)*(SH_MAG[7] + SH_MAG[8] - 2.0f*magD*q2)) + P(0,19)*(SH_MAG[7] + SH_MAG[8] - 2.0f*magD*q2));
|
||||
// check for a badly conditioned covariance matrix
|
||||
const char* numerical_error_covariance_reset_string = "numerical error - covariance reset";
|
||||
|
||||
if (_mag_innov_var(0) >= R_MAG) {
|
||||
// the innovation variance contribution from the state covariances is non-negative - no fault
|
||||
_fault_status.flags.bad_mag_x = false;
|
||||
|
||||
} else {
|
||||
// the innovation variance contribution from the state covariances is negative which means the covariance matrix is badly conditioned
|
||||
_fault_status.flags.bad_mag_x = true;
|
||||
|
||||
// we need to re-initialise covariances and abort this fusion step
|
||||
resetMagRelatedCovariances();
|
||||
ECL_ERR("magX %s", numerical_error_covariance_reset_string);
|
||||
return;
|
||||
|
||||
}
|
||||
|
||||
// Y axis innovation variance
|
||||
_mag_innov_var(1) = (P(20,20) + R_MAG + P(0,20)*SH_MAG[2] + P(1,20)*SH_MAG[1] + P(2,20)*SH_MAG[0] - P(17,20)*(SH_MAG[3] - SH_MAG[4] + SH_MAG[5] - SH_MAG[6]) - (2.0f*q0*q3 - 2.0f*q1*q2)*(P(20,16) + P(0,16)*SH_MAG[2] + P(1,16)*SH_MAG[1] + P(2,16)*SH_MAG[0] - P(17,16)*(SH_MAG[3] - SH_MAG[4] + SH_MAG[5] - SH_MAG[6]) - P(16,16)*(2.0f*q0*q3 - 2.0f*q1*q2) + P(18,16)*(2.0f*q0*q1 + 2.0f*q2*q3) - P(3,16)*(SH_MAG[7] + SH_MAG[8] - 2.0f*magD*q2)) + (2.0f*q0*q1 + 2.0f*q2*q3)*(P(20,18) + P(0,18)*SH_MAG[2] + P(1,18)*SH_MAG[1] + P(2,18)*SH_MAG[0] - P(17,18)*(SH_MAG[3] - SH_MAG[4] + SH_MAG[5] - SH_MAG[6]) - P(16,18)*(2.0f*q0*q3 - 2.0f*q1*q2) + P(18,18)*(2.0f*q0*q1 + 2.0f*q2*q3) - P(3,18)*(SH_MAG[7] + SH_MAG[8] - 2.0f*magD*q2)) - (SH_MAG[7] + SH_MAG[8] - 2.0f*magD*q2)*(P(20,3) + P(0,3)*SH_MAG[2] + P(1,3)*SH_MAG[1] + P(2,3)*SH_MAG[0] - P(17,3)*(SH_MAG[3] - SH_MAG[4] + SH_MAG[5] - SH_MAG[6]) - P(16,3)*(2.0f*q0*q3 - 2.0f*q1*q2) + P(18,3)*(2.0f*q0*q1 + 2.0f*q2*q3) - P(3,3)*(SH_MAG[7] + SH_MAG[8] - 2.0f*magD*q2)) - P(16,20)*(2.0f*q0*q3 - 2.0f*q1*q2) + P(18,20)*(2.0f*q0*q1 + 2.0f*q2*q3) + SH_MAG[2]*(P(20,0) + P(0,0)*SH_MAG[2] + P(1,0)*SH_MAG[1] + P(2,0)*SH_MAG[0] - P(17,0)*(SH_MAG[3] - SH_MAG[4] + SH_MAG[5] - SH_MAG[6]) - P(16,0)*(2.0f*q0*q3 - 2.0f*q1*q2) + P(18,0)*(2.0f*q0*q1 + 2.0f*q2*q3) - P(3,0)*(SH_MAG[7] + SH_MAG[8] - 2.0f*magD*q2)) + SH_MAG[1]*(P(20,1) + P(0,1)*SH_MAG[2] + P(1,1)*SH_MAG[1] + P(2,1)*SH_MAG[0] - P(17,1)*(SH_MAG[3] - SH_MAG[4] + SH_MAG[5] - SH_MAG[6]) - P(16,1)*(2.0f*q0*q3 - 2.0f*q1*q2) + P(18,1)*(2.0f*q0*q1 + 2.0f*q2*q3) - P(3,1)*(SH_MAG[7] + SH_MAG[8] - 2.0f*magD*q2)) + SH_MAG[0]*(P(20,2) + P(0,2)*SH_MAG[2] + P(1,2)*SH_MAG[1] + P(2,2)*SH_MAG[0] - P(17,2)*(SH_MAG[3] - SH_MAG[4] + SH_MAG[5] - SH_MAG[6]) - P(16,2)*(2.0f*q0*q3 - 2.0f*q1*q2) + P(18,2)*(2.0f*q0*q1 + 2.0f*q2*q3) - P(3,2)*(SH_MAG[7] + SH_MAG[8] - 2.0f*magD*q2)) - (SH_MAG[3] - SH_MAG[4] + SH_MAG[5] - SH_MAG[6])*(P(20,17) + P(0,17)*SH_MAG[2] + P(1,17)*SH_MAG[1] + P(2,17)*SH_MAG[0] - P(17,17)*(SH_MAG[3] - SH_MAG[4] + SH_MAG[5] - SH_MAG[6]) - P(16,17)*(2.0f*q0*q3 - 2.0f*q1*q2) + P(18,17)*(2.0f*q0*q1 + 2.0f*q2*q3) - P(3,17)*(SH_MAG[7] + SH_MAG[8] - 2.0f*magD*q2)) - P(3,20)*(SH_MAG[7] + SH_MAG[8] - 2.0f*magD*q2));
|
||||
|
||||
// check for a badly conditioned covariance matrix
|
||||
if (_mag_innov_var(1) >= R_MAG) {
|
||||
// the innovation variance contribution from the state covariances is non-negative - no fault
|
||||
_fault_status.flags.bad_mag_y = false;
|
||||
|
||||
} else {
|
||||
// the innovation variance contribution from the state covariances is negtive which means the covariance matrix is badly conditioned
|
||||
_fault_status.flags.bad_mag_y = true;
|
||||
|
||||
// we need to re-initialise covariances and abort this fusion step
|
||||
resetMagRelatedCovariances();
|
||||
ECL_ERR("magY %s", numerical_error_covariance_reset_string);
|
||||
return;
|
||||
|
||||
}
|
||||
|
||||
// Z axis innovation variance
|
||||
_mag_innov_var(2) = (P(21,21) + R_MAG + P(0,21)*SH_MAG[1] - P(1,21)*SH_MAG[2] + P(3,21)*SH_MAG[0] + P(18,21)*(SH_MAG[3] - SH_MAG[4] - SH_MAG[5] + SH_MAG[6]) + (2.0f*q0*q2 + 2.0f*q1*q3)*(P(21,16) + P(0,16)*SH_MAG[1] - P(1,16)*SH_MAG[2] + P(3,16)*SH_MAG[0] + P(18,16)*(SH_MAG[3] - SH_MAG[4] - SH_MAG[5] + SH_MAG[6]) + P(16,16)*(2.0f*q0*q2 + 2.0f*q1*q3) - P(17,16)*(2.0f*q0*q1 - 2.0f*q2*q3) + P(2,16)*(SH_MAG[7] + SH_MAG[8] - 2.0f*magD*q2)) - (2.0f*q0*q1 - 2.0f*q2*q3)*(P(21,17) + P(0,17)*SH_MAG[1] - P(1,17)*SH_MAG[2] + P(3,17)*SH_MAG[0] + P(18,17)*(SH_MAG[3] - SH_MAG[4] - SH_MAG[5] + SH_MAG[6]) + P(16,17)*(2.0f*q0*q2 + 2.0f*q1*q3) - P(17,17)*(2.0f*q0*q1 - 2.0f*q2*q3) + P(2,17)*(SH_MAG[7] + SH_MAG[8] - 2.0f*magD*q2)) + (SH_MAG[7] + SH_MAG[8] - 2.0f*magD*q2)*(P(21,2) + P(0,2)*SH_MAG[1] - P(1,2)*SH_MAG[2] + P(3,2)*SH_MAG[0] + P(18,2)*(SH_MAG[3] - SH_MAG[4] - SH_MAG[5] + SH_MAG[6]) + P(16,2)*(2.0f*q0*q2 + 2.0f*q1*q3) - P(17,2)*(2.0f*q0*q1 - 2.0f*q2*q3) + P(2,2)*(SH_MAG[7] + SH_MAG[8] - 2.0f*magD*q2)) + P(16,21)*(2.0f*q0*q2 + 2.0f*q1*q3) - P(17,21)*(2.0f*q0*q1 - 2.0f*q2*q3) + SH_MAG[1]*(P(21,0) + P(0,0)*SH_MAG[1] - P(1,0)*SH_MAG[2] + P(3,0)*SH_MAG[0] + P(18,0)*(SH_MAG[3] - SH_MAG[4] - SH_MAG[5] + SH_MAG[6]) + P(16,0)*(2.0f*q0*q2 + 2.0f*q1*q3) - P(17,0)*(2.0f*q0*q1 - 2.0f*q2*q3) + P(2,0)*(SH_MAG[7] + SH_MAG[8] - 2.0f*magD*q2)) - SH_MAG[2]*(P(21,1) + P(0,1)*SH_MAG[1] - P(1,1)*SH_MAG[2] + P(3,1)*SH_MAG[0] + P(18,1)*(SH_MAG[3] - SH_MAG[4] - SH_MAG[5] + SH_MAG[6]) + P(16,1)*(2.0f*q0*q2 + 2.0f*q1*q3) - P(17,1)*(2.0f*q0*q1 - 2.0f*q2*q3) + P(2,1)*(SH_MAG[7] + SH_MAG[8] - 2.0f*magD*q2)) + SH_MAG[0]*(P(21,3) + P(0,3)*SH_MAG[1] - P(1,3)*SH_MAG[2] + P(3,3)*SH_MAG[0] + P(18,3)*(SH_MAG[3] - SH_MAG[4] - SH_MAG[5] + SH_MAG[6]) + P(16,3)*(2.0f*q0*q2 + 2.0f*q1*q3) - P(17,3)*(2.0f*q0*q1 - 2.0f*q2*q3) + P(2,3)*(SH_MAG[7] + SH_MAG[8] - 2.0f*magD*q2)) + (SH_MAG[3] - SH_MAG[4] - SH_MAG[5] + SH_MAG[6])*(P(21,18) + P(0,18)*SH_MAG[1] - P(1,18)*SH_MAG[2] + P(3,18)*SH_MAG[0] + P(18,18)*(SH_MAG[3] - SH_MAG[4] - SH_MAG[5] + SH_MAG[6]) + P(16,18)*(2.0f*q0*q2 + 2.0f*q1*q3) - P(17,18)*(2.0f*q0*q1 - 2.0f*q2*q3) + P(2,18)*(SH_MAG[7] + SH_MAG[8] - 2.0f*magD*q2)) + P(2,21)*(SH_MAG[7] + SH_MAG[8] - 2.0f*magD*q2));
|
||||
|
||||
// check for a badly conditioned covariance matrix
|
||||
if (_mag_innov_var(2) >= R_MAG) {
|
||||
// the innovation variance contribution from the state covariances is non-negative - no fault
|
||||
_fault_status.flags.bad_mag_z = false;
|
||||
|
||||
} else {
|
||||
// the innovation variance contribution from the state covariances is negative which means the covariance matrix is badly conditioned
|
||||
_fault_status.flags.bad_mag_z = true;
|
||||
|
||||
// we need to re-initialise covariances and abort this fusion step
|
||||
resetMagRelatedCovariances();
|
||||
ECL_ERR("magZ %s", numerical_error_covariance_reset_string);
|
||||
return;
|
||||
|
||||
}
|
||||
|
||||
// Perform an innovation consistency check and report the result
|
||||
bool all_innovation_checks_passed = true;
|
||||
|
||||
@@ -170,7 +196,7 @@ void Ekf::fuseMag()
|
||||
const bool update_all_states = ((_imu_sample_delayed.time_us - _flt_mag_align_start_time) > (uint64_t)5e6);
|
||||
|
||||
// Observation jacobian and Kalman gain vectors
|
||||
Vector24f H_MAG;
|
||||
SparseVector24f<0,1,2,3,16,17,18,19,20,21> Hfusion;
|
||||
Vector24f Kfusion;
|
||||
|
||||
// update the states and covariance using sequential fusion of the magnetometer components
|
||||
@@ -179,99 +205,114 @@ void Ekf::fuseMag()
|
||||
// Calculate Kalman gains and observation jacobians
|
||||
if (index == 0) {
|
||||
// Calculate X axis observation jacobians
|
||||
H_MAG.setZero();
|
||||
H_MAG(0) = SH_MAG[7] + SH_MAG[8] - 2.0f*magD*q2;
|
||||
H_MAG(1) = SH_MAG[0];
|
||||
H_MAG(2) = -SH_MAG[1];
|
||||
H_MAG(3) = SH_MAG[2];
|
||||
H_MAG(16) = SH_MAG[5] - SH_MAG[4] - SH_MAG[3] + SH_MAG[6];
|
||||
H_MAG(17) = 2.0f*q0*q3 + 2.0f*q1*q2;
|
||||
H_MAG(18) = 2.0f*q1*q3 - 2.0f*q0*q2;
|
||||
H_MAG(19) = 1.0f;
|
||||
Hfusion.at<0>() = 2*HKX0;
|
||||
Hfusion.at<1>() = 2*HKX1;
|
||||
Hfusion.at<2>() = 2*HKX2 - 2*HKX3 - 2*HKX4;
|
||||
Hfusion.at<3>() = 2*HKX5;
|
||||
Hfusion.at<16>() = HKX6;
|
||||
Hfusion.at<17>() = 2*HKX7;
|
||||
Hfusion.at<18>() = 2*HKX8 - 2*HKX9;
|
||||
Hfusion.at<19>() = 1;
|
||||
|
||||
// Calculate X axis Kalman gains
|
||||
float SK_MX[5];
|
||||
SK_MX[0] = 1.0f / _mag_innov_var(0);
|
||||
SK_MX[1] = SH_MAG[3] + SH_MAG[4] - SH_MAG[5] - SH_MAG[6];
|
||||
SK_MX[2] = SH_MAG[7] + SH_MAG[8] - 2.0f*magD*q2;
|
||||
SK_MX[3] = 2.0f*q0*q2 - 2.0f*q1*q3;
|
||||
SK_MX[4] = 2.0f*q0*q3 + 2.0f*q1*q2;
|
||||
|
||||
if (update_all_states) {
|
||||
Kfusion(0) = SK_MX[0]*(P(0,19) + P(0,1)*SH_MAG[0] - P(0,2)*SH_MAG[1] + P(0,3)*SH_MAG[2] + P(0,0)*SK_MX[2] - P(0,16)*SK_MX[1] + P(0,17)*SK_MX[4] - P(0,18)*SK_MX[3]);
|
||||
Kfusion(1) = SK_MX[0]*(P(1,19) + P(1,1)*SH_MAG[0] - P(1,2)*SH_MAG[1] + P(1,3)*SH_MAG[2] + P(1,0)*SK_MX[2] - P(1,16)*SK_MX[1] + P(1,17)*SK_MX[4] - P(1,18)*SK_MX[3]);
|
||||
Kfusion(2) = SK_MX[0]*(P(2,19) + P(2,1)*SH_MAG[0] - P(2,2)*SH_MAG[1] + P(2,3)*SH_MAG[2] + P(2,0)*SK_MX[2] - P(2,16)*SK_MX[1] + P(2,17)*SK_MX[4] - P(2,18)*SK_MX[3]);
|
||||
Kfusion(3) = SK_MX[0]*(P(3,19) + P(3,1)*SH_MAG[0] - P(3,2)*SH_MAG[1] + P(3,3)*SH_MAG[2] + P(3,0)*SK_MX[2] - P(3,16)*SK_MX[1] + P(3,17)*SK_MX[4] - P(3,18)*SK_MX[3]);
|
||||
Kfusion(4) = SK_MX[0]*(P(4,19) + P(4,1)*SH_MAG[0] - P(4,2)*SH_MAG[1] + P(4,3)*SH_MAG[2] + P(4,0)*SK_MX[2] - P(4,16)*SK_MX[1] + P(4,17)*SK_MX[4] - P(4,18)*SK_MX[3]);
|
||||
Kfusion(5) = SK_MX[0]*(P(5,19) + P(5,1)*SH_MAG[0] - P(5,2)*SH_MAG[1] + P(5,3)*SH_MAG[2] + P(5,0)*SK_MX[2] - P(5,16)*SK_MX[1] + P(5,17)*SK_MX[4] - P(5,18)*SK_MX[3]);
|
||||
Kfusion(6) = SK_MX[0]*(P(6,19) + P(6,1)*SH_MAG[0] - P(6,2)*SH_MAG[1] + P(6,3)*SH_MAG[2] + P(6,0)*SK_MX[2] - P(6,16)*SK_MX[1] + P(6,17)*SK_MX[4] - P(6,18)*SK_MX[3]);
|
||||
Kfusion(7) = SK_MX[0]*(P(7,19) + P(7,1)*SH_MAG[0] - P(7,2)*SH_MAG[1] + P(7,3)*SH_MAG[2] + P(7,0)*SK_MX[2] - P(7,16)*SK_MX[1] + P(7,17)*SK_MX[4] - P(7,18)*SK_MX[3]);
|
||||
Kfusion(8) = SK_MX[0]*(P(8,19) + P(8,1)*SH_MAG[0] - P(8,2)*SH_MAG[1] + P(8,3)*SH_MAG[2] + P(8,0)*SK_MX[2] - P(8,16)*SK_MX[1] + P(8,17)*SK_MX[4] - P(8,18)*SK_MX[3]);
|
||||
Kfusion(9) = SK_MX[0]*(P(9,19) + P(9,1)*SH_MAG[0] - P(9,2)*SH_MAG[1] + P(9,3)*SH_MAG[2] + P(9,0)*SK_MX[2] - P(9,16)*SK_MX[1] + P(9,17)*SK_MX[4] - P(9,18)*SK_MX[3]);
|
||||
Kfusion(10) = SK_MX[0]*(P(10,19) + P(10,1)*SH_MAG[0] - P(10,2)*SH_MAG[1] + P(10,3)*SH_MAG[2] + P(10,0)*SK_MX[2] - P(10,16)*SK_MX[1] + P(10,17)*SK_MX[4] - P(10,18)*SK_MX[3]);
|
||||
Kfusion(11) = SK_MX[0]*(P(11,19) + P(11,1)*SH_MAG[0] - P(11,2)*SH_MAG[1] + P(11,3)*SH_MAG[2] + P(11,0)*SK_MX[2] - P(11,16)*SK_MX[1] + P(11,17)*SK_MX[4] - P(11,18)*SK_MX[3]);
|
||||
Kfusion(12) = SK_MX[0]*(P(12,19) + P(12,1)*SH_MAG[0] - P(12,2)*SH_MAG[1] + P(12,3)*SH_MAG[2] + P(12,0)*SK_MX[2] - P(12,16)*SK_MX[1] + P(12,17)*SK_MX[4] - P(12,18)*SK_MX[3]);
|
||||
Kfusion(13) = SK_MX[0]*(P(13,19) + P(13,1)*SH_MAG[0] - P(13,2)*SH_MAG[1] + P(13,3)*SH_MAG[2] + P(13,0)*SK_MX[2] - P(13,16)*SK_MX[1] + P(13,17)*SK_MX[4] - P(13,18)*SK_MX[3]);
|
||||
Kfusion(14) = SK_MX[0]*(P(14,19) + P(14,1)*SH_MAG[0] - P(14,2)*SH_MAG[1] + P(14,3)*SH_MAG[2] + P(14,0)*SK_MX[2] - P(14,16)*SK_MX[1] + P(14,17)*SK_MX[4] - P(14,18)*SK_MX[3]);
|
||||
Kfusion(15) = SK_MX[0]*(P(15,19) + P(15,1)*SH_MAG[0] - P(15,2)*SH_MAG[1] + P(15,3)*SH_MAG[2] + P(15,0)*SK_MX[2] - P(15,16)*SK_MX[1] + P(15,17)*SK_MX[4] - P(15,18)*SK_MX[3]);
|
||||
Kfusion(22) = SK_MX[0]*(P(22,19) + P(22,1)*SH_MAG[0] - P(22,2)*SH_MAG[1] + P(22,3)*SH_MAG[2] + P(22,0)*SK_MX[2] - P(22,16)*SK_MX[1] + P(22,17)*SK_MX[4] - P(22,18)*SK_MX[3]);
|
||||
Kfusion(23) = SK_MX[0]*(P(23,19) + P(23,1)*SH_MAG[0] - P(23,2)*SH_MAG[1] + P(23,3)*SH_MAG[2] + P(23,0)*SK_MX[2] - P(23,16)*SK_MX[1] + P(23,17)*SK_MX[4] - P(23,18)*SK_MX[3]);
|
||||
Kfusion(0) = HKX16*HKX24;
|
||||
Kfusion(1) = HKX22*HKX24;
|
||||
Kfusion(2) = HKX19*HKX24;
|
||||
Kfusion(3) = HKX21*HKX24;
|
||||
|
||||
for (unsigned row = 4; row <= 15; row++) {
|
||||
Kfusion(row) = HKX24*(HKX10*P(row,17) - HKX11*P(row,18) + HKX12*P(1,row) + HKX13*P(0,row) - HKX14*P(2,row) + HKX15*P(3,row) + HKX6*P(row,16) + P(row,19));
|
||||
}
|
||||
|
||||
for (unsigned row = 22; row <= 23; row++) {
|
||||
Kfusion(row) = HKX24*(HKX10*P(17,row) - HKX11*P(18,row) + HKX12*P(1,row) + HKX13*P(0,row) - HKX14*P(2,row) + HKX15*P(3,row) + HKX6*P(16,row) + P(19,row));
|
||||
}
|
||||
}
|
||||
|
||||
Kfusion(16) = SK_MX[0]*(P(16,19) + P(16,1)*SH_MAG[0] - P(16,2)*SH_MAG[1] + P(16,3)*SH_MAG[2] + P(16,0)*SK_MX[2] - P(16,16)*SK_MX[1] + P(16,17)*SK_MX[4] - P(16,18)*SK_MX[3]);
|
||||
Kfusion(17) = SK_MX[0]*(P(17,19) + P(17,1)*SH_MAG[0] - P(17,2)*SH_MAG[1] + P(17,3)*SH_MAG[2] + P(17,0)*SK_MX[2] - P(17,16)*SK_MX[1] + P(17,17)*SK_MX[4] - P(17,18)*SK_MX[3]);
|
||||
Kfusion(18) = SK_MX[0]*(P(18,19) + P(18,1)*SH_MAG[0] - P(18,2)*SH_MAG[1] + P(18,3)*SH_MAG[2] + P(18,0)*SK_MX[2] - P(18,16)*SK_MX[1] + P(18,17)*SK_MX[4] - P(18,18)*SK_MX[3]);
|
||||
Kfusion(19) = SK_MX[0]*(P(19,19) + P(19,1)*SH_MAG[0] - P(19,2)*SH_MAG[1] + P(19,3)*SH_MAG[2] + P(19,0)*SK_MX[2] - P(19,16)*SK_MX[1] + P(19,17)*SK_MX[4] - P(19,18)*SK_MX[3]);
|
||||
Kfusion(20) = SK_MX[0]*(P(20,19) + P(20,1)*SH_MAG[0] - P(20,2)*SH_MAG[1] + P(20,3)*SH_MAG[2] + P(20,0)*SK_MX[2] - P(20,16)*SK_MX[1] + P(20,17)*SK_MX[4] - P(20,18)*SK_MX[3]);
|
||||
Kfusion(21) = SK_MX[0]*(P(21,19) + P(21,1)*SH_MAG[0] - P(21,2)*SH_MAG[1] + P(21,3)*SH_MAG[2] + P(21,0)*SK_MX[2] - P(21,16)*SK_MX[1] + P(21,17)*SK_MX[4] - P(21,18)*SK_MX[3]);
|
||||
Kfusion(16) = HKX17*HKX24;
|
||||
Kfusion(17) = HKX20*HKX24;
|
||||
Kfusion(18) = HKX18*HKX24;
|
||||
Kfusion(19) = HKX23*HKX24;
|
||||
|
||||
for (unsigned row = 20; row <= 21; row++) {
|
||||
Kfusion(row) = HKX24*(HKX10*P(17,row) - HKX11*P(18,row) + HKX12*P(1,row) + HKX13*P(0,row) - HKX14*P(2,row) + HKX15*P(3,row) + HKX6*P(16,row) + P(19,row));
|
||||
}
|
||||
|
||||
} else if (index == 1) {
|
||||
|
||||
// recalculate innovation variance becasue states and covariances have changed due to previous fusion
|
||||
const float HKY0 = magD*q1 + magE*q0 - magN*q3;
|
||||
const float HKY1 = magD*q0 - magE*q1 + magN*q2;
|
||||
const float HKY2 = magD*q3 + magE*q2 + magN*q1;
|
||||
const float HKY3 = magD*q2;
|
||||
const float HKY4 = magE*q3;
|
||||
const float HKY5 = magN*q0;
|
||||
const float HKY6 = q1*q2;
|
||||
const float HKY7 = q0*q3;
|
||||
const float HKY8 = powf(q0, 2) - powf(q1, 2) + powf(q2, 2) - powf(q3, 2);
|
||||
const float HKY9 = q0*q1 + q2*q3;
|
||||
const float HKY10 = 2*HKY9;
|
||||
const float HKY11 = -2*HKY6 + 2*HKY7;
|
||||
const float HKY12 = 2*HKY2;
|
||||
const float HKY13 = 2*HKY0;
|
||||
const float HKY14 = 2*HKY1;
|
||||
const float HKY15 = -2*HKY3 + 2*HKY4 + 2*HKY5;
|
||||
const float HKY16 = HKY10*P(0,18) - HKY11*P(0,16) + HKY12*P(0,2) + HKY13*P(0,0) + HKY14*P(0,1) - HKY15*P(0,3) + HKY8*P(0,17) + P(0,20);
|
||||
const float HKY17 = HKY10*P(17,18) - HKY11*P(16,17) + HKY12*P(2,17) + HKY13*P(0,17) + HKY14*P(1,17) - HKY15*P(3,17) + HKY8*P(17,17) + P(17,20);
|
||||
const float HKY18 = HKY10*P(16,18) - HKY11*P(16,16) + HKY12*P(2,16) + HKY13*P(0,16) + HKY14*P(1,16) - HKY15*P(3,16) + HKY8*P(16,17) + P(16,20);
|
||||
const float HKY19 = HKY10*P(3,18) - HKY11*P(3,16) + HKY12*P(2,3) + HKY13*P(0,3) + HKY14*P(1,3) - HKY15*P(3,3) + HKY8*P(3,17) + P(3,20);
|
||||
const float HKY20 = HKY10*P(18,18) - HKY11*P(16,18) + HKY12*P(2,18) + HKY13*P(0,18) + HKY14*P(1,18) - HKY15*P(3,18) + HKY8*P(17,18) + P(18,20);
|
||||
const float HKY21 = HKY10*P(1,18) - HKY11*P(1,16) + HKY12*P(1,2) + HKY13*P(0,1) + HKY14*P(1,1) - HKY15*P(1,3) + HKY8*P(1,17) + P(1,20);
|
||||
const float HKY22 = HKY10*P(2,18) - HKY11*P(2,16) + HKY12*P(2,2) + HKY13*P(0,2) + HKY14*P(1,2) - HKY15*P(2,3) + HKY8*P(2,17) + P(2,20);
|
||||
const float HKY23 = HKY10*P(18,20) - HKY11*P(16,20) + HKY12*P(2,20) + HKY13*P(0,20) + HKY14*P(1,20) - HKY15*P(3,20) + HKY8*P(17,20) + P(20,20);
|
||||
|
||||
_mag_innov_var(1) = (HKY10*HKY20 - HKY11*HKY18 + HKY12*HKY22 + HKY13*HKY16 + HKY14*HKY21 - HKY15*HKY19 + HKY17*HKY8 + HKY23 + R_MAG);
|
||||
|
||||
if (_mag_innov_var(1) < R_MAG) {
|
||||
// the innovation variance contribution from the state covariances is negative which means the covariance matrix is badly conditioned
|
||||
_fault_status.flags.bad_mag_y = true;
|
||||
|
||||
// we need to re-initialise covariances and abort this fusion step
|
||||
resetMagRelatedCovariances();
|
||||
ECL_ERR("magY %s", numerical_error_covariance_reset_string);
|
||||
return;
|
||||
}
|
||||
const float HKY24 = 1.0F/_mag_innov_var(1);
|
||||
|
||||
// Calculate Y axis observation jacobians
|
||||
H_MAG.setZero();
|
||||
H_MAG(0) = SH_MAG[2];
|
||||
H_MAG(1) = SH_MAG[1];
|
||||
H_MAG(2) = SH_MAG[0];
|
||||
H_MAG(3) = 2.0f*magD*q2 - SH_MAG[8] - SH_MAG[7];
|
||||
H_MAG(16) = 2.0f*q1*q2 - 2.0f*q0*q3;
|
||||
H_MAG(17) = SH_MAG[4] - SH_MAG[3] - SH_MAG[5] + SH_MAG[6];
|
||||
H_MAG(18) = 2.0f*q0*q1 + 2.0f*q2*q3;
|
||||
H_MAG(20) = 1.0f;
|
||||
Hfusion.setZero();
|
||||
Hfusion.at<0>() = 2*HKY0;
|
||||
Hfusion.at<1>() = 2*HKY1;
|
||||
Hfusion.at<2>() = 2*HKY2;
|
||||
Hfusion.at<3>() = 2*HKY3 - 2*HKY4 - 2*HKY5;
|
||||
Hfusion.at<16>() = 2*HKY6 - 2*HKY7;
|
||||
Hfusion.at<17>() = HKY8;
|
||||
Hfusion.at<18>() = 2*HKY9;
|
||||
Hfusion.at<20>() = 1;
|
||||
|
||||
// Calculate Y axis Kalman gains
|
||||
float SK_MY[5];
|
||||
SK_MY[0] = 1.0f / _mag_innov_var(1);
|
||||
SK_MY[1] = SH_MAG[3] - SH_MAG[4] + SH_MAG[5] - SH_MAG[6];
|
||||
SK_MY[2] = SH_MAG[7] + SH_MAG[8] - 2.0f*magD*q2;
|
||||
SK_MY[3] = 2.0f*q0*q3 - 2.0f*q1*q2;
|
||||
SK_MY[4] = 2.0f*q0*q1 + 2.0f*q2*q3;
|
||||
|
||||
if (update_all_states) {
|
||||
Kfusion(0) = SK_MY[0]*(P(0,20) + P(0,0)*SH_MAG[2] + P(0,1)*SH_MAG[1] + P(0,2)*SH_MAG[0] - P(0,3)*SK_MY[2] - P(0,17)*SK_MY[1] - P(0,16)*SK_MY[3] + P(0,18)*SK_MY[4]);
|
||||
Kfusion(1) = SK_MY[0]*(P(1,20) + P(1,0)*SH_MAG[2] + P(1,1)*SH_MAG[1] + P(1,2)*SH_MAG[0] - P(1,3)*SK_MY[2] - P(1,17)*SK_MY[1] - P(1,16)*SK_MY[3] + P(1,18)*SK_MY[4]);
|
||||
Kfusion(2) = SK_MY[0]*(P(2,20) + P(2,0)*SH_MAG[2] + P(2,1)*SH_MAG[1] + P(2,2)*SH_MAG[0] - P(2,3)*SK_MY[2] - P(2,17)*SK_MY[1] - P(2,16)*SK_MY[3] + P(2,18)*SK_MY[4]);
|
||||
Kfusion(3) = SK_MY[0]*(P(3,20) + P(3,0)*SH_MAG[2] + P(3,1)*SH_MAG[1] + P(3,2)*SH_MAG[0] - P(3,3)*SK_MY[2] - P(3,17)*SK_MY[1] - P(3,16)*SK_MY[3] + P(3,18)*SK_MY[4]);
|
||||
Kfusion(4) = SK_MY[0]*(P(4,20) + P(4,0)*SH_MAG[2] + P(4,1)*SH_MAG[1] + P(4,2)*SH_MAG[0] - P(4,3)*SK_MY[2] - P(4,17)*SK_MY[1] - P(4,16)*SK_MY[3] + P(4,18)*SK_MY[4]);
|
||||
Kfusion(5) = SK_MY[0]*(P(5,20) + P(5,0)*SH_MAG[2] + P(5,1)*SH_MAG[1] + P(5,2)*SH_MAG[0] - P(5,3)*SK_MY[2] - P(5,17)*SK_MY[1] - P(5,16)*SK_MY[3] + P(5,18)*SK_MY[4]);
|
||||
Kfusion(6) = SK_MY[0]*(P(6,20) + P(6,0)*SH_MAG[2] + P(6,1)*SH_MAG[1] + P(6,2)*SH_MAG[0] - P(6,3)*SK_MY[2] - P(6,17)*SK_MY[1] - P(6,16)*SK_MY[3] + P(6,18)*SK_MY[4]);
|
||||
Kfusion(7) = SK_MY[0]*(P(7,20) + P(7,0)*SH_MAG[2] + P(7,1)*SH_MAG[1] + P(7,2)*SH_MAG[0] - P(7,3)*SK_MY[2] - P(7,17)*SK_MY[1] - P(7,16)*SK_MY[3] + P(7,18)*SK_MY[4]);
|
||||
Kfusion(8) = SK_MY[0]*(P(8,20) + P(8,0)*SH_MAG[2] + P(8,1)*SH_MAG[1] + P(8,2)*SH_MAG[0] - P(8,3)*SK_MY[2] - P(8,17)*SK_MY[1] - P(8,16)*SK_MY[3] + P(8,18)*SK_MY[4]);
|
||||
Kfusion(9) = SK_MY[0]*(P(9,20) + P(9,0)*SH_MAG[2] + P(9,1)*SH_MAG[1] + P(9,2)*SH_MAG[0] - P(9,3)*SK_MY[2] - P(9,17)*SK_MY[1] - P(9,16)*SK_MY[3] + P(9,18)*SK_MY[4]);
|
||||
Kfusion(10) = SK_MY[0]*(P(10,20) + P(10,0)*SH_MAG[2] + P(10,1)*SH_MAG[1] + P(10,2)*SH_MAG[0] - P(10,3)*SK_MY[2] - P(10,17)*SK_MY[1] - P(10,16)*SK_MY[3] + P(10,18)*SK_MY[4]);
|
||||
Kfusion(11) = SK_MY[0]*(P(11,20) + P(11,0)*SH_MAG[2] + P(11,1)*SH_MAG[1] + P(11,2)*SH_MAG[0] - P(11,3)*SK_MY[2] - P(11,17)*SK_MY[1] - P(11,16)*SK_MY[3] + P(11,18)*SK_MY[4]);
|
||||
Kfusion(12) = SK_MY[0]*(P(12,20) + P(12,0)*SH_MAG[2] + P(12,1)*SH_MAG[1] + P(12,2)*SH_MAG[0] - P(12,3)*SK_MY[2] - P(12,17)*SK_MY[1] - P(12,16)*SK_MY[3] + P(12,18)*SK_MY[4]);
|
||||
Kfusion(13) = SK_MY[0]*(P(13,20) + P(13,0)*SH_MAG[2] + P(13,1)*SH_MAG[1] + P(13,2)*SH_MAG[0] - P(13,3)*SK_MY[2] - P(13,17)*SK_MY[1] - P(13,16)*SK_MY[3] + P(13,18)*SK_MY[4]);
|
||||
Kfusion(14) = SK_MY[0]*(P(14,20) + P(14,0)*SH_MAG[2] + P(14,1)*SH_MAG[1] + P(14,2)*SH_MAG[0] - P(14,3)*SK_MY[2] - P(14,17)*SK_MY[1] - P(14,16)*SK_MY[3] + P(14,18)*SK_MY[4]);
|
||||
Kfusion(15) = SK_MY[0]*(P(15,20) + P(15,0)*SH_MAG[2] + P(15,1)*SH_MAG[1] + P(15,2)*SH_MAG[0] - P(15,3)*SK_MY[2] - P(15,17)*SK_MY[1] - P(15,16)*SK_MY[3] + P(15,18)*SK_MY[4]);
|
||||
Kfusion(22) = SK_MY[0]*(P(22,20) + P(22,0)*SH_MAG[2] + P(22,1)*SH_MAG[1] + P(22,2)*SH_MAG[0] - P(22,3)*SK_MY[2] - P(22,17)*SK_MY[1] - P(22,16)*SK_MY[3] + P(22,18)*SK_MY[4]);
|
||||
Kfusion(23) = SK_MY[0]*(P(23,20) + P(23,0)*SH_MAG[2] + P(23,1)*SH_MAG[1] + P(23,2)*SH_MAG[0] - P(23,3)*SK_MY[2] - P(23,17)*SK_MY[1] - P(23,16)*SK_MY[3] + P(23,18)*SK_MY[4]);
|
||||
Kfusion(0) = HKY16*HKY24;
|
||||
Kfusion(1) = HKY21*HKY24;
|
||||
Kfusion(2) = HKY22*HKY24;
|
||||
Kfusion(3) = HKY19*HKY24;
|
||||
|
||||
for (unsigned row = 4; row <= 15; row++) {
|
||||
Kfusion(row) = HKY24*(HKY10*P(row,18) - HKY11*P(row,16) + HKY12*P(2,row) + HKY13*P(0,row) + HKY14*P(1,row) - HKY15*P(3,row) + HKY8*P(row,17) + P(row,20));
|
||||
}
|
||||
|
||||
for (unsigned row = 22; row <= 23; row++) {
|
||||
Kfusion(row) = HKY24*(HKY10*P(18,row) - HKY11*P(16,row) + HKY12*P(2,row) + HKY13*P(0,row) + HKY14*P(1,row) - HKY15*P(3,row) + HKY8*P(17,row) + P(20,row));
|
||||
}
|
||||
}
|
||||
|
||||
Kfusion(16) = SK_MY[0]*(P(16,20) + P(16,0)*SH_MAG[2] + P(16,1)*SH_MAG[1] + P(16,2)*SH_MAG[0] - P(16,3)*SK_MY[2] - P(16,17)*SK_MY[1] - P(16,16)*SK_MY[3] + P(16,18)*SK_MY[4]);
|
||||
Kfusion(17) = SK_MY[0]*(P(17,20) + P(17,0)*SH_MAG[2] + P(17,1)*SH_MAG[1] + P(17,2)*SH_MAG[0] - P(17,3)*SK_MY[2] - P(17,17)*SK_MY[1] - P(17,16)*SK_MY[3] + P(17,18)*SK_MY[4]);
|
||||
Kfusion(18) = SK_MY[0]*(P(18,20) + P(18,0)*SH_MAG[2] + P(18,1)*SH_MAG[1] + P(18,2)*SH_MAG[0] - P(18,3)*SK_MY[2] - P(18,17)*SK_MY[1] - P(18,16)*SK_MY[3] + P(18,18)*SK_MY[4]);
|
||||
Kfusion(19) = SK_MY[0]*(P(19,20) + P(19,0)*SH_MAG[2] + P(19,1)*SH_MAG[1] + P(19,2)*SH_MAG[0] - P(19,3)*SK_MY[2] - P(19,17)*SK_MY[1] - P(19,16)*SK_MY[3] + P(19,18)*SK_MY[4]);
|
||||
Kfusion(20) = SK_MY[0]*(P(20,20) + P(20,0)*SH_MAG[2] + P(20,1)*SH_MAG[1] + P(20,2)*SH_MAG[0] - P(20,3)*SK_MY[2] - P(20,17)*SK_MY[1] - P(20,16)*SK_MY[3] + P(20,18)*SK_MY[4]);
|
||||
Kfusion(21) = SK_MY[0]*(P(21,20) + P(21,0)*SH_MAG[2] + P(21,1)*SH_MAG[1] + P(21,2)*SH_MAG[0] - P(21,3)*SK_MY[2] - P(21,17)*SK_MY[1] - P(21,16)*SK_MY[3] + P(21,18)*SK_MY[4]);
|
||||
Kfusion(16) = HKY18*HKY24;
|
||||
Kfusion(17) = HKY17*HKY24;
|
||||
Kfusion(18) = HKY20*HKY24;
|
||||
Kfusion(19) = HKY24*(HKY10*P(18,19) - HKY11*P(16,19) + HKY12*P(2,19) + HKY13*P(0,19) + HKY14*P(1,19) - HKY15*P(3,19) + HKY8*P(17,19) + P(19,20));
|
||||
Kfusion(20) = HKY23*HKY24;
|
||||
Kfusion(21) = HKY24*(HKY10*P(18,21) - HKY11*P(16,21) + HKY12*P(2,21) + HKY13*P(0,21) + HKY14*P(1,21) - HKY15*P(3,21) + HKY8*P(17,21) + P(20,21));
|
||||
|
||||
} else if (index == 2) {
|
||||
|
||||
@@ -280,88 +321,88 @@ void Ekf::fuseMag()
|
||||
continue;
|
||||
}
|
||||
|
||||
// calculate Z axis observation jacobians
|
||||
H_MAG.setZero();
|
||||
H_MAG(0) = SH_MAG[1];
|
||||
H_MAG(1) = -SH_MAG[2];
|
||||
H_MAG(2) = SH_MAG[7] + SH_MAG[8] - 2.0f*magD*q2;
|
||||
H_MAG(3) = SH_MAG[0];
|
||||
H_MAG(16) = 2.0f*q0*q2 + 2.0f*q1*q3;
|
||||
H_MAG(17) = 2.0f*q2*q3 - 2.0f*q0*q1;
|
||||
H_MAG(18) = SH_MAG[3] - SH_MAG[4] - SH_MAG[5] + SH_MAG[6];
|
||||
H_MAG(21) = 1.0f;
|
||||
// recalculate innovation variance becasue states and covariances have changed due to previous fusion
|
||||
const float HKZ0 = magD*q0 - magE*q1 + magN*q2;
|
||||
const float HKZ1 = magN*q3;
|
||||
const float HKZ2 = magD*q1;
|
||||
const float HKZ3 = magE*q0;
|
||||
const float HKZ4 = -magD*q2 + magE*q3 + magN*q0;
|
||||
const float HKZ5 = magD*q3 + magE*q2 + magN*q1;
|
||||
const float HKZ6 = q0*q2 + q1*q3;
|
||||
const float HKZ7 = q2*q3;
|
||||
const float HKZ8 = q0*q1;
|
||||
const float HKZ9 = powf(q0, 2) - powf(q1, 2) - powf(q2, 2) + powf(q3, 2);
|
||||
const float HKZ10 = 2*HKZ6;
|
||||
const float HKZ11 = -2*HKZ7 + 2*HKZ8;
|
||||
const float HKZ12 = 2*HKZ5;
|
||||
const float HKZ13 = 2*HKZ0;
|
||||
const float HKZ14 = -2*HKZ1 + 2*HKZ2 + 2*HKZ3;
|
||||
const float HKZ15 = 2*HKZ4;
|
||||
const float HKZ16 = HKZ10*P(0,16) - HKZ11*P(0,17) + HKZ12*P(0,3) + HKZ13*P(0,0) - HKZ14*P(0,1) + HKZ15*P(0,2) + HKZ9*P(0,18) + P(0,21);
|
||||
const float HKZ17 = HKZ10*P(16,18) - HKZ11*P(17,18) + HKZ12*P(3,18) + HKZ13*P(0,18) - HKZ14*P(1,18) + HKZ15*P(2,18) + HKZ9*P(18,18) + P(18,21);
|
||||
const float HKZ18 = HKZ10*P(16,17) - HKZ11*P(17,17) + HKZ12*P(3,17) + HKZ13*P(0,17) - HKZ14*P(1,17) + HKZ15*P(2,17) + HKZ9*P(17,18) + P(17,21);
|
||||
const float HKZ19 = HKZ10*P(1,16) - HKZ11*P(1,17) + HKZ12*P(1,3) + HKZ13*P(0,1) - HKZ14*P(1,1) + HKZ15*P(1,2) + HKZ9*P(1,18) + P(1,21);
|
||||
const float HKZ20 = HKZ10*P(16,16) - HKZ11*P(16,17) + HKZ12*P(3,16) + HKZ13*P(0,16) - HKZ14*P(1,16) + HKZ15*P(2,16) + HKZ9*P(16,18) + P(16,21);
|
||||
const float HKZ21 = HKZ10*P(3,16) - HKZ11*P(3,17) + HKZ12*P(3,3) + HKZ13*P(0,3) - HKZ14*P(1,3) + HKZ15*P(2,3) + HKZ9*P(3,18) + P(3,21);
|
||||
const float HKZ22 = HKZ10*P(2,16) - HKZ11*P(2,17) + HKZ12*P(2,3) + HKZ13*P(0,2) - HKZ14*P(1,2) + HKZ15*P(2,2) + HKZ9*P(2,18) + P(2,21);
|
||||
const float HKZ23 = HKZ10*P(16,21) - HKZ11*P(17,21) + HKZ12*P(3,21) + HKZ13*P(0,21) - HKZ14*P(1,21) + HKZ15*P(2,21) + HKZ9*P(18,21) + P(21,21);
|
||||
|
||||
// Calculate Z axis Kalman gains
|
||||
float SK_MZ[5];
|
||||
SK_MZ[0] = 1.0f / _mag_innov_var(2);
|
||||
SK_MZ[1] = SH_MAG[3] - SH_MAG[4] - SH_MAG[5] + SH_MAG[6];
|
||||
SK_MZ[2] = SH_MAG[7] + SH_MAG[8] - 2.0f*magD*q2;
|
||||
SK_MZ[3] = 2.0f*q0*q1 - 2.0f*q2*q3;
|
||||
SK_MZ[4] = 2.0f*q0*q2 + 2.0f*q1*q3;
|
||||
_mag_innov_var(2) = (HKZ10*HKZ20 - HKZ11*HKZ18 + HKZ12*HKZ21 + HKZ13*HKZ16 - HKZ14*HKZ19 + HKZ15*HKZ22 + HKZ17*HKZ9 + HKZ23 + R_MAG);
|
||||
|
||||
if (update_all_states) {
|
||||
Kfusion(0) = SK_MZ[0]*(P(0,21) + P(0,0)*SH_MAG[1] - P(0,1)*SH_MAG[2] + P(0,3)*SH_MAG[0] + P(0,2)*SK_MZ[2] + P(0,18)*SK_MZ[1] + P(0,16)*SK_MZ[4] - P(0,17)*SK_MZ[3]);
|
||||
Kfusion(1) = SK_MZ[0]*(P(1,21) + P(1,0)*SH_MAG[1] - P(1,1)*SH_MAG[2] + P(1,3)*SH_MAG[0] + P(1,2)*SK_MZ[2] + P(1,18)*SK_MZ[1] + P(1,16)*SK_MZ[4] - P(1,17)*SK_MZ[3]);
|
||||
Kfusion(2) = SK_MZ[0]*(P(2,21) + P(2,0)*SH_MAG[1] - P(2,1)*SH_MAG[2] + P(2,3)*SH_MAG[0] + P(2,2)*SK_MZ[2] + P(2,18)*SK_MZ[1] + P(2,16)*SK_MZ[4] - P(2,17)*SK_MZ[3]);
|
||||
Kfusion(3) = SK_MZ[0]*(P(3,21) + P(3,0)*SH_MAG[1] - P(3,1)*SH_MAG[2] + P(3,3)*SH_MAG[0] + P(3,2)*SK_MZ[2] + P(3,18)*SK_MZ[1] + P(3,16)*SK_MZ[4] - P(3,17)*SK_MZ[3]);
|
||||
Kfusion(4) = SK_MZ[0]*(P(4,21) + P(4,0)*SH_MAG[1] - P(4,1)*SH_MAG[2] + P(4,3)*SH_MAG[0] + P(4,2)*SK_MZ[2] + P(4,18)*SK_MZ[1] + P(4,16)*SK_MZ[4] - P(4,17)*SK_MZ[3]);
|
||||
Kfusion(5) = SK_MZ[0]*(P(5,21) + P(5,0)*SH_MAG[1] - P(5,1)*SH_MAG[2] + P(5,3)*SH_MAG[0] + P(5,2)*SK_MZ[2] + P(5,18)*SK_MZ[1] + P(5,16)*SK_MZ[4] - P(5,17)*SK_MZ[3]);
|
||||
Kfusion(6) = SK_MZ[0]*(P(6,21) + P(6,0)*SH_MAG[1] - P(6,1)*SH_MAG[2] + P(6,3)*SH_MAG[0] + P(6,2)*SK_MZ[2] + P(6,18)*SK_MZ[1] + P(6,16)*SK_MZ[4] - P(6,17)*SK_MZ[3]);
|
||||
Kfusion(7) = SK_MZ[0]*(P(7,21) + P(7,0)*SH_MAG[1] - P(7,1)*SH_MAG[2] + P(7,3)*SH_MAG[0] + P(7,2)*SK_MZ[2] + P(7,18)*SK_MZ[1] + P(7,16)*SK_MZ[4] - P(7,17)*SK_MZ[3]);
|
||||
Kfusion(8) = SK_MZ[0]*(P(8,21) + P(8,0)*SH_MAG[1] - P(8,1)*SH_MAG[2] + P(8,3)*SH_MAG[0] + P(8,2)*SK_MZ[2] + P(8,18)*SK_MZ[1] + P(8,16)*SK_MZ[4] - P(8,17)*SK_MZ[3]);
|
||||
Kfusion(9) = SK_MZ[0]*(P(9,21) + P(9,0)*SH_MAG[1] - P(9,1)*SH_MAG[2] + P(9,3)*SH_MAG[0] + P(9,2)*SK_MZ[2] + P(9,18)*SK_MZ[1] + P(9,16)*SK_MZ[4] - P(9,17)*SK_MZ[3]);
|
||||
Kfusion(10) = SK_MZ[0]*(P(10,21) + P(10,0)*SH_MAG[1] - P(10,1)*SH_MAG[2] + P(10,3)*SH_MAG[0] + P(10,2)*SK_MZ[2] + P(10,18)*SK_MZ[1] + P(10,16)*SK_MZ[4] - P(10,17)*SK_MZ[3]);
|
||||
Kfusion(11) = SK_MZ[0]*(P(11,21) + P(11,0)*SH_MAG[1] - P(11,1)*SH_MAG[2] + P(11,3)*SH_MAG[0] + P(11,2)*SK_MZ[2] + P(11,18)*SK_MZ[1] + P(11,16)*SK_MZ[4] - P(11,17)*SK_MZ[3]);
|
||||
Kfusion(12) = SK_MZ[0]*(P(12,21) + P(12,0)*SH_MAG[1] - P(12,1)*SH_MAG[2] + P(12,3)*SH_MAG[0] + P(12,2)*SK_MZ[2] + P(12,18)*SK_MZ[1] + P(12,16)*SK_MZ[4] - P(12,17)*SK_MZ[3]);
|
||||
Kfusion(13) = SK_MZ[0]*(P(13,21) + P(13,0)*SH_MAG[1] - P(13,1)*SH_MAG[2] + P(13,3)*SH_MAG[0] + P(13,2)*SK_MZ[2] + P(13,18)*SK_MZ[1] + P(13,16)*SK_MZ[4] - P(13,17)*SK_MZ[3]);
|
||||
Kfusion(14) = SK_MZ[0]*(P(14,21) + P(14,0)*SH_MAG[1] - P(14,1)*SH_MAG[2] + P(14,3)*SH_MAG[0] + P(14,2)*SK_MZ[2] + P(14,18)*SK_MZ[1] + P(14,16)*SK_MZ[4] - P(14,17)*SK_MZ[3]);
|
||||
Kfusion(15) = SK_MZ[0]*(P(15,21) + P(15,0)*SH_MAG[1] - P(15,1)*SH_MAG[2] + P(15,3)*SH_MAG[0] + P(15,2)*SK_MZ[2] + P(15,18)*SK_MZ[1] + P(15,16)*SK_MZ[4] - P(15,17)*SK_MZ[3]);
|
||||
Kfusion(22) = SK_MZ[0]*(P(22,21) + P(22,0)*SH_MAG[1] - P(22,1)*SH_MAG[2] + P(22,3)*SH_MAG[0] + P(22,2)*SK_MZ[2] + P(22,18)*SK_MZ[1] + P(22,16)*SK_MZ[4] - P(22,17)*SK_MZ[3]);
|
||||
Kfusion(23) = SK_MZ[0]*(P(23,21) + P(23,0)*SH_MAG[1] - P(23,1)*SH_MAG[2] + P(23,3)*SH_MAG[0] + P(23,2)*SK_MZ[2] + P(23,18)*SK_MZ[1] + P(23,16)*SK_MZ[4] - P(23,17)*SK_MZ[3]);
|
||||
if (_mag_innov_var(2) < R_MAG) {
|
||||
// the innovation variance contribution from the state covariances is negative which means the covariance matrix is badly conditioned
|
||||
_fault_status.flags.bad_mag_z = true;
|
||||
|
||||
// we need to re-initialise covariances and abort this fusion step
|
||||
resetMagRelatedCovariances();
|
||||
ECL_ERR("magZ %s", numerical_error_covariance_reset_string);
|
||||
return;
|
||||
}
|
||||
|
||||
Kfusion(16) = SK_MZ[0]*(P(16,21) + P(16,0)*SH_MAG[1] - P(16,1)*SH_MAG[2] + P(16,3)*SH_MAG[0] + P(16,2)*SK_MZ[2] + P(16,18)*SK_MZ[1] + P(16,16)*SK_MZ[4] - P(16,17)*SK_MZ[3]);
|
||||
Kfusion(17) = SK_MZ[0]*(P(17,21) + P(17,0)*SH_MAG[1] - P(17,1)*SH_MAG[2] + P(17,3)*SH_MAG[0] + P(17,2)*SK_MZ[2] + P(17,18)*SK_MZ[1] + P(17,16)*SK_MZ[4] - P(17,17)*SK_MZ[3]);
|
||||
Kfusion(18) = SK_MZ[0]*(P(18,21) + P(18,0)*SH_MAG[1] - P(18,1)*SH_MAG[2] + P(18,3)*SH_MAG[0] + P(18,2)*SK_MZ[2] + P(18,18)*SK_MZ[1] + P(18,16)*SK_MZ[4] - P(18,17)*SK_MZ[3]);
|
||||
Kfusion(19) = SK_MZ[0]*(P(19,21) + P(19,0)*SH_MAG[1] - P(19,1)*SH_MAG[2] + P(19,3)*SH_MAG[0] + P(19,2)*SK_MZ[2] + P(19,18)*SK_MZ[1] + P(19,16)*SK_MZ[4] - P(19,17)*SK_MZ[3]);
|
||||
Kfusion(20) = SK_MZ[0]*(P(20,21) + P(20,0)*SH_MAG[1] - P(20,1)*SH_MAG[2] + P(20,3)*SH_MAG[0] + P(20,2)*SK_MZ[2] + P(20,18)*SK_MZ[1] + P(20,16)*SK_MZ[4] - P(20,17)*SK_MZ[3]);
|
||||
Kfusion(21) = SK_MZ[0]*(P(21,21) + P(21,0)*SH_MAG[1] - P(21,1)*SH_MAG[2] + P(21,3)*SH_MAG[0] + P(21,2)*SK_MZ[2] + P(21,18)*SK_MZ[1] + P(21,16)*SK_MZ[4] - P(21,17)*SK_MZ[3]);
|
||||
const float HKZ24 = 1.0F/_mag_innov_var(2);
|
||||
|
||||
// calculate Z axis observation jacobians
|
||||
Hfusion.setZero();
|
||||
Hfusion.at<0>() = 2*HKZ0;
|
||||
Hfusion.at<1>() = 2*HKZ1 - 2*HKZ2 - 2*HKZ3;
|
||||
Hfusion.at<2>() = 2*HKZ4;
|
||||
Hfusion.at<3>() = 2*HKZ5;
|
||||
Hfusion.at<16>() = 2*HKZ6;
|
||||
Hfusion.at<17>() = 2*HKZ7 - 2*HKZ8;
|
||||
Hfusion.at<18>() = HKZ9;
|
||||
Hfusion.at<21>() = 1;
|
||||
|
||||
// Calculate Z axis Kalman gains
|
||||
if (update_all_states) {
|
||||
Kfusion(0) = HKZ16*HKZ24;
|
||||
Kfusion(1) = HKZ19*HKZ24;
|
||||
Kfusion(2) = HKZ22*HKZ24;
|
||||
Kfusion(3) = HKZ21*HKZ24;
|
||||
|
||||
for (unsigned row = 4; row <= 15; row++) {
|
||||
Kfusion(row) = HKZ24*(HKZ10*P(row,16) - HKZ11*P(row,17) + HKZ12*P(3,row) + HKZ13*P(0,row) - HKZ14*P(1,row) + HKZ15*P(2,row) + HKZ9*P(row,18) + P(row,21));
|
||||
}
|
||||
|
||||
for (unsigned row = 22; row <= 23; row++) {
|
||||
Kfusion(row) = HKZ24*(HKZ10*P(16,row) - HKZ11*P(17,row) + HKZ12*P(3,row) + HKZ13*P(0,row) - HKZ14*P(1,row) + HKZ15*P(2,row) + HKZ9*P(18,row) + P(21,row));
|
||||
}
|
||||
}
|
||||
|
||||
Kfusion(16) = HKZ20*HKZ24;
|
||||
Kfusion(17) = HKZ18*HKZ24;
|
||||
Kfusion(18) = HKZ17*HKZ24;
|
||||
|
||||
for (unsigned row = 19; row <= 20; row++) {
|
||||
Kfusion(row) = HKZ24*(HKZ10*P(16,row) - HKZ11*P(17,row) + HKZ12*P(3,row) + HKZ13*P(0,row) - HKZ14*P(1,row) + HKZ15*P(2,row) + HKZ9*P(18,row) + P(row,21));
|
||||
}
|
||||
|
||||
Kfusion(21) = HKZ23*HKZ24;
|
||||
}
|
||||
|
||||
// apply covariance correction via P_new = (I -K*H)*P
|
||||
// first calculate expression for KHP
|
||||
// then calculate P - KHP
|
||||
SquareMatrix24f KHP;
|
||||
float KH[10];
|
||||
|
||||
for (unsigned row = 0; row < _k_num_states; row++) {
|
||||
|
||||
KH[0] = Kfusion(row) * H_MAG(0);
|
||||
KH[1] = Kfusion(row) * H_MAG(1);
|
||||
KH[2] = Kfusion(row) * H_MAG(2);
|
||||
KH[3] = Kfusion(row) * H_MAG(3);
|
||||
KH[4] = Kfusion(row) * H_MAG(16);
|
||||
KH[5] = Kfusion(row) * H_MAG(17);
|
||||
KH[6] = Kfusion(row) * H_MAG(18);
|
||||
KH[7] = Kfusion(row) * H_MAG(19);
|
||||
KH[8] = Kfusion(row) * H_MAG(20);
|
||||
KH[9] = Kfusion(row) * H_MAG(21);
|
||||
|
||||
for (unsigned column = 0; column < _k_num_states; column++) {
|
||||
float tmp = KH[0] * P(0,column);
|
||||
tmp += KH[1] * P(1,column);
|
||||
tmp += KH[2] * P(2,column);
|
||||
tmp += KH[3] * P(3,column);
|
||||
tmp += KH[4] * P(16,column);
|
||||
tmp += KH[5] * P(17,column);
|
||||
tmp += KH[6] * P(18,column);
|
||||
tmp += KH[7] * P(19,column);
|
||||
tmp += KH[8] * P(20,column);
|
||||
tmp += KH[9] * P(21,column);
|
||||
KHP(row,column) = tmp;
|
||||
}
|
||||
}
|
||||
const SquareMatrix24f KHP = computeKHP(Kfusion, Hfusion);
|
||||
|
||||
const bool healthy = checkAndFixCovarianceUpdate(KHP);
|
||||
|
||||
@@ -393,81 +434,66 @@ void Ekf::fuseMag()
|
||||
void Ekf::fuseYaw321(float yaw, float yaw_variance, bool zero_innovation)
|
||||
{
|
||||
// assign intermediate state variables
|
||||
const float q0 = _state.quat_nominal(0);
|
||||
const float q1 = _state.quat_nominal(1);
|
||||
const float q2 = _state.quat_nominal(2);
|
||||
const float q3 = _state.quat_nominal(3);
|
||||
const float &q0 = _state.quat_nominal(0);
|
||||
const float &q1 = _state.quat_nominal(1);
|
||||
const float &q2 = _state.quat_nominal(2);
|
||||
const float &q3 = _state.quat_nominal(3);
|
||||
|
||||
const float R_YAW = fmaxf(yaw_variance, 1.0e-4f);
|
||||
const float measurement = wrap_pi(yaw);
|
||||
|
||||
// calculate observation jacobian when we are observing the first rotation in a 321 sequence
|
||||
float t9 = q0*q3;
|
||||
float t10 = q1*q2;
|
||||
float t2 = t9+t10;
|
||||
float t3 = q0*q0;
|
||||
float t4 = q1*q1;
|
||||
float t5 = q2*q2;
|
||||
float t6 = q3*q3;
|
||||
float t7 = t3+t4-t5-t6;
|
||||
// calculate 321 yaw observation matrix
|
||||
// choose A or B computational paths to avoid singularity in derivation at +-90 degrees yaw
|
||||
bool canUseA = false;
|
||||
const float SA0 = 2*q3;
|
||||
const float SA1 = 2*q2;
|
||||
const float SA2 = SA0*q0 + SA1*q1;
|
||||
const float SA3 = sq(q0) + sq(q1) - sq(q2) - sq(q3);
|
||||
float SA4, SA5_inv;
|
||||
if (sq(SA3) > 1E-6f) {
|
||||
SA4 = 1.0F/sq(SA3);
|
||||
SA5_inv = sq(SA2)*SA4 + 1;
|
||||
canUseA = fabsf(SA5_inv) > 1E-6f;
|
||||
}
|
||||
|
||||
float t16 = q3*t3;
|
||||
float t17 = q3*t5;
|
||||
float t18 = q0*q1*q2*2.0f;
|
||||
float t19 = t16+t17+t18-q3*t4+q3*t6;
|
||||
|
||||
float t24 = q2*t4;
|
||||
float t25 = q2*t6;
|
||||
float t26 = q0*q1*q3*2.0f;
|
||||
float t27 = t24+t25+t26-q2*t3+q2*t5;
|
||||
float t28 = q1*t3;
|
||||
float t29 = q1*t5;
|
||||
float t30 = q0*q2*q3*2.0f;
|
||||
float t31 = t28+t29+t30+q1*t4-q1*t6;
|
||||
float t32 = q0*t4;
|
||||
float t33 = q0*t6;
|
||||
float t34 = q1*q2*q3*2.0f;
|
||||
float t35 = t32+t33+t34+q0*t3-q0*t5;
|
||||
bool canUseB = false;
|
||||
const float SB0 = 2*q0;
|
||||
const float SB1 = 2*q1;
|
||||
const float SB2 = SB0*q3 + SB1*q2;
|
||||
const float SB4 = sq(q0) + sq(q1) - sq(q2) - sq(q3);
|
||||
float SB3, SB5_inv;
|
||||
if (sq(SB2) > 1E-6f) {
|
||||
SB3 = 1.0F/sq(SB2);
|
||||
SB5_inv = SB3*sq(SB4) + 1;
|
||||
canUseB = fabsf(SB5_inv) > 1E-6f;
|
||||
}
|
||||
|
||||
Vector4f H_YAW;
|
||||
// two computational paths are provided to work around singularities in calculation of the Jacobians
|
||||
float t8 = t7*t7;
|
||||
float t15 = t2*t2;
|
||||
if (t8 > t15 && t8 > 1E-6f) {
|
||||
// this path has a singularities at yaw = +-90 degrees
|
||||
t8 = 1.0f/t8;
|
||||
float t11 = t2*t2;
|
||||
float t12 = t8*t11*4.0f;
|
||||
float t13 = t12+1.0f;
|
||||
float t14 = 1.0f/t13;
|
||||
|
||||
H_YAW(0) = t8*t14*t19*(-2.0f);
|
||||
H_YAW(1) = t8*t14*t27*(-2.0f);
|
||||
H_YAW(2) = t8*t14*t31*2.0f;
|
||||
H_YAW(3) = t8*t14*t35*2.0f;
|
||||
if (canUseA && (!canUseB || fabsf(SA5_inv) >= fabsf(SB5_inv))) {
|
||||
const float SA5 = 1.0F/SA5_inv;
|
||||
const float SA6 = 1.0F/SA3;
|
||||
const float SA7 = SA2*SA4;
|
||||
const float SA8 = 2*SA7;
|
||||
const float SA9 = 2*SA6;
|
||||
|
||||
} else if (t15 > 1E-6f) {
|
||||
// this path has singularities at yaw = 0 and +-180 deg
|
||||
t15 = 1.0f/t15;
|
||||
float t20 = t7*t7;
|
||||
float t21 = t15*t20*0.25f;
|
||||
float t22 = t21+1.0f;
|
||||
H_YAW(0) = SA5*(SA0*SA6 - SA8*q0);
|
||||
H_YAW(1) = SA5*(SA1*SA6 - SA8*q1);
|
||||
H_YAW(2) = SA5*(SA1*SA7 + SA9*q1);
|
||||
H_YAW(3) = SA5*(SA0*SA7 + SA9*q0);
|
||||
} else if (canUseB && (!canUseA || fabsf(SB5_inv) > fabsf(SA5_inv))) {
|
||||
const float SB5 = 1.0F/SB5_inv;
|
||||
const float SB6 = 1.0F/SB2;
|
||||
const float SB7 = SB3*SB4;
|
||||
const float SB8 = 2*SB7;
|
||||
const float SB9 = 2*SB6;
|
||||
|
||||
if (fabsf(t22) > 1E-6f) {
|
||||
float t23 = 1.0f/t22;
|
||||
|
||||
H_YAW(0) = t15*t19*t23*(-0.5f);
|
||||
H_YAW(1) = t15*t23*t27*(-0.5f);
|
||||
H_YAW(2) = t15*t23*t31*0.5f;
|
||||
H_YAW(3) = t15*t23*t35*0.5f;
|
||||
|
||||
} else {
|
||||
return;
|
||||
|
||||
}
|
||||
H_YAW(0) = -SB5*(SB0*SB6 - SB8*q3);
|
||||
H_YAW(1) = -SB5*(SB1*SB6 - SB8*q2);
|
||||
H_YAW(2) = -SB5*(-SB1*SB7 - SB9*q2);
|
||||
H_YAW(3) = -SB5*(-SB0*SB7 - SB9*q3);
|
||||
} else {
|
||||
return;
|
||||
|
||||
}
|
||||
|
||||
// calculate the yaw innovation and wrap to the interval between +-pi
|
||||
@@ -496,69 +522,58 @@ void Ekf::fuseYaw312(float yaw, float yaw_variance, bool zero_innovation)
|
||||
const float R_YAW = fmaxf(yaw_variance, 1.0e-4f);
|
||||
const float measurement = wrap_pi(yaw);
|
||||
|
||||
// calculate observation jacobian when we are observing a rotation in a 312 sequence
|
||||
float t9 = q0*q3;
|
||||
float t10 = q1*q2;
|
||||
float t2 = t9-t10;
|
||||
float t3 = q0*q0;
|
||||
float t4 = q1*q1;
|
||||
float t5 = q2*q2;
|
||||
float t6 = q3*q3;
|
||||
float t7 = t3-t4+t5-t6;
|
||||
// calculate 312 yaw observation matrix
|
||||
// choose A or B computational paths to avoid singularity in derivation at +-90 degrees yaw
|
||||
bool canUseA = false;
|
||||
const float SA0 = 2*q3;
|
||||
const float SA1 = 2*q2;
|
||||
const float SA2 = SA0*q0 - SA1*q1;
|
||||
const float SA3 = sq(q0) - sq(q1) + sq(q2) - sq(q3);
|
||||
float SA4, SA5_inv;
|
||||
if (sq(SA3) > 1E-6f) {
|
||||
SA4 = 1.0F/sq(SA3);
|
||||
SA5_inv = sq(SA2)*SA4 + 1;
|
||||
canUseA = fabsf(SA5_inv) > 1E-6f;
|
||||
}
|
||||
|
||||
float t16 = q3*t3;
|
||||
float t17 = q3*t4;
|
||||
float t18 = t16+t17-q3*t5+q3*t6-q0*q1*q2*2.0f;
|
||||
float t23 = q2*t3;
|
||||
float t24 = q2*t4;
|
||||
float t25 = t23+t24+q2*t5-q2*t6-q0*q1*q3*2.0f;
|
||||
float t26 = q1*t5;
|
||||
float t27 = q1*t6;
|
||||
float t28 = t26+t27-q1*t3+q1*t4-q0*q2*q3*2.0f;
|
||||
float t29 = q0*t5;
|
||||
float t30 = q0*t6;
|
||||
float t31 = t29+t30+q0*t3-q0*t4-q1*q2*q3*2.0f;
|
||||
bool canUseB = false;
|
||||
const float SB0 = 2*q0;
|
||||
const float SB1 = 2*q1;
|
||||
const float SB2 = -SB0*q3 + SB1*q2;
|
||||
const float SB4 = -sq(q0) + sq(q1) - sq(q2) + sq(q3);
|
||||
float SB3, SB5_inv;
|
||||
if (sq(SB2) > 1E-6f) {
|
||||
SB3 = 1.0F/sq(SB2);
|
||||
SB5_inv = SB3*sq(SB4) + 1;
|
||||
canUseB = fabsf(SB5_inv) > 1E-6f;
|
||||
}
|
||||
|
||||
Vector4f H_YAW;
|
||||
// two computational paths are provided to work around singularities in calculation of the Jacobians
|
||||
float t8 = t7*t7;
|
||||
float t15 = t2*t2;
|
||||
if (t8 > t15 && t8 > 1E-6f) {
|
||||
// this path has a singularities at yaw = +-90 degrees
|
||||
t8 = 1.0f/t8;
|
||||
float t11 = t2*t2;
|
||||
float t12 = t8*t11*4.0f;
|
||||
float t13 = t12+1.0f;
|
||||
float t14 = 1.0f/t13;
|
||||
|
||||
H_YAW(0) = t8*t14*t18*(-2.0f);
|
||||
H_YAW(1) = t8*t14*t25*(-2.0f);
|
||||
H_YAW(2) = t8*t14*t28*2.0f;
|
||||
H_YAW(3) = t8*t14*t31*2.0f;
|
||||
if (canUseA && (!canUseB || fabsf(SA5_inv) >= fabsf(SB5_inv))) {
|
||||
const float SA5 = 1.0F/SA5_inv;
|
||||
const float SA6 = 1.0F/SA3;
|
||||
const float SA7 = SA2*SA4;
|
||||
const float SA8 = 2*SA7;
|
||||
const float SA9 = 2*SA6;
|
||||
|
||||
} else if (t15 > 1E-6f) {
|
||||
// this path has singularities at yaw = 0 and +-180 deg
|
||||
t15 = 1.0f/t15;
|
||||
float t19 = t7*t7;
|
||||
float t20 = t15*t19*0.25f;
|
||||
float t21 = t20+1.0f;
|
||||
|
||||
if (fabsf(t21) > 1E-6f) {
|
||||
float t22 = 1.0f/t21;
|
||||
|
||||
H_YAW(0) = t15*t18*t22*(-0.5f);
|
||||
H_YAW(1) = t15*t22*t25*(-0.5f);
|
||||
H_YAW(2) = t15*t22*t28*0.5f;
|
||||
H_YAW(3) = t15*t22*t31*0.5f;
|
||||
|
||||
} else {
|
||||
return;
|
||||
|
||||
}
|
||||
H_YAW(0) = SA5*(SA0*SA6 - SA8*q0);
|
||||
H_YAW(1) = SA5*(-SA1*SA6 + SA8*q1);
|
||||
H_YAW(2) = SA5*(-SA1*SA7 - SA9*q1);
|
||||
H_YAW(3) = SA5*(SA0*SA7 + SA9*q0);
|
||||
} else if (canUseB && (!canUseA || fabsf(SB5_inv) > fabsf(SA5_inv))) {
|
||||
const float SB5 = 1.0F/SB5_inv;
|
||||
const float SB6 = 1.0F/SB2;
|
||||
const float SB7 = SB3*SB4;
|
||||
const float SB8 = 2*SB7;
|
||||
const float SB9 = 2*SB6;
|
||||
|
||||
H_YAW(0) = -SB5*(-SB0*SB6 + SB8*q3);
|
||||
H_YAW(1) = -SB5*(SB1*SB6 - SB8*q2);
|
||||
H_YAW(2) = -SB5*(-SB1*SB7 - SB9*q2);
|
||||
H_YAW(3) = -SB5*(SB0*SB7 + SB9*q3);
|
||||
} else {
|
||||
return;
|
||||
|
||||
}
|
||||
|
||||
float innovation;
|
||||
@@ -857,81 +872,57 @@ void Ekf::fuseHeading()
|
||||
void Ekf::fuseDeclination(float decl_sigma)
|
||||
{
|
||||
// assign intermediate state variables
|
||||
const float magN = _state.mag_I(0);
|
||||
const float magE = _state.mag_I(1);
|
||||
const float &magN = _state.mag_I(0);
|
||||
const float &magE = _state.mag_I(1);
|
||||
|
||||
// minimum horizontal field strength before calculation becomes badly conditioned (T)
|
||||
constexpr float h_field_min = 0.001f;
|
||||
// minimum North field strength before calculation becomes badly conditioned (T)
|
||||
constexpr float N_field_min = 0.001f;
|
||||
|
||||
// observation variance (rad**2)
|
||||
const float R_DECL = sq(decl_sigma);
|
||||
|
||||
// Calculate intermediate variables
|
||||
float t2 = magE*magE;
|
||||
float t3 = magN*magN;
|
||||
float t4 = t2+t3;
|
||||
// if the horizontal magnetic field is too small, this calculation will be badly conditioned
|
||||
if (t4 < h_field_min*h_field_min) {
|
||||
if (fabsf(magN) < sq(N_field_min)) {
|
||||
// calculation is badly conditioned close to +-90 deg declination
|
||||
return;
|
||||
}
|
||||
float t5 = P(16,16)*t2;
|
||||
float t6 = P(17,17)*t3;
|
||||
float t7 = t2*t2;
|
||||
float t8 = R_DECL*t7;
|
||||
float t9 = t3*t3;
|
||||
float t10 = R_DECL*t9;
|
||||
float t11 = R_DECL*t2*t3*2.0f;
|
||||
float t14 = P(16,17)*magE*magN;
|
||||
float t15 = P(17,16)*magE*magN;
|
||||
float t12 = t5+t6+t8+t10+t11-t14-t15;
|
||||
float t13;
|
||||
if (fabsf(t12) > 1e-6f) {
|
||||
t13 = 1.0f / t12;
|
||||
} else {
|
||||
return;
|
||||
}
|
||||
float t18 = magE*magE;
|
||||
float t19 = magN*magN;
|
||||
float t20 = t18+t19;
|
||||
float t21;
|
||||
if (fabsf(t20) > 1e-6f) {
|
||||
t21 = 1.0f/t20;
|
||||
const float HK0 = powf(magN, -2);
|
||||
const float HK1 = HK0*powf(magE, 2) + 1.0F;
|
||||
const float HK2 = 1.0F/HK1;
|
||||
const float HK3 = 1.0F/magN;
|
||||
const float HK4 = HK2*HK3;
|
||||
const float HK5 = HK3*magE;
|
||||
const float HK6 = HK5*P(16,17) - P(17,17);
|
||||
const float HK7 = powf(HK1, -2);
|
||||
const float HK8 = HK5*P(16,16) - P(16,17);
|
||||
const float innovation_variance = -HK0*HK6*HK7 + HK7*HK8*magE/powf(magN, 3) + R_DECL;
|
||||
float HK9;
|
||||
if (innovation_variance > R_DECL) {
|
||||
HK9 = HK4/innovation_variance;
|
||||
} else {
|
||||
// variance calculation is badly conditioned
|
||||
return;
|
||||
}
|
||||
|
||||
// Calculate the observation Jacobian
|
||||
// Note only 2 terms are non-zero which can be used in matrix operations for calculation of Kalman gains and covariance update to significantly reduce cost
|
||||
Vector24f H_DECL;
|
||||
H_DECL(16) = -magE*t21;
|
||||
H_DECL(17) = magN*t21;
|
||||
// Note Hfusion indices do not match state indices
|
||||
SparseVector24f<16,17> Hfusion;
|
||||
Hfusion.at<16>() = -HK0*HK2*magE;
|
||||
Hfusion.at<17>() = HK4;
|
||||
|
||||
// Calculate the Kalman gains
|
||||
Vector24f Kfusion;
|
||||
Kfusion(0) = -t4*t13*(P(0,16)*magE-P(0,17)*magN);
|
||||
Kfusion(1) = -t4*t13*(P(1,16)*magE-P(1,17)*magN);
|
||||
Kfusion(2) = -t4*t13*(P(2,16)*magE-P(2,17)*magN);
|
||||
Kfusion(3) = -t4*t13*(P(3,16)*magE-P(3,17)*magN);
|
||||
Kfusion(4) = -t4*t13*(P(4,16)*magE-P(4,17)*magN);
|
||||
Kfusion(5) = -t4*t13*(P(5,16)*magE-P(5,17)*magN);
|
||||
Kfusion(6) = -t4*t13*(P(6,16)*magE-P(6,17)*magN);
|
||||
Kfusion(7) = -t4*t13*(P(7,16)*magE-P(7,17)*magN);
|
||||
Kfusion(8) = -t4*t13*(P(8,16)*magE-P(8,17)*magN);
|
||||
Kfusion(9) = -t4*t13*(P(9,16)*magE-P(9,17)*magN);
|
||||
Kfusion(10) = -t4*t13*(P(10,16)*magE-P(10,17)*magN);
|
||||
Kfusion(11) = -t4*t13*(P(11,16)*magE-P(11,17)*magN);
|
||||
Kfusion(12) = -t4*t13*(P(12,16)*magE-P(12,17)*magN);
|
||||
Kfusion(13) = -t4*t13*(P(13,16)*magE-P(13,17)*magN);
|
||||
Kfusion(14) = -t4*t13*(P(14,16)*magE-P(14,17)*magN);
|
||||
Kfusion(15) = -t4*t13*(P(15,16)*magE-P(15,17)*magN);
|
||||
Kfusion(16) = -t4*t13*(P(16,16)*magE-P(16,17)*magN);
|
||||
Kfusion(17) = -t4*t13*(P(17,16)*magE-P(17,17)*magN);
|
||||
Kfusion(18) = -t4*t13*(P(18,16)*magE-P(18,17)*magN);
|
||||
Kfusion(19) = -t4*t13*(P(19,16)*magE-P(19,17)*magN);
|
||||
Kfusion(20) = -t4*t13*(P(20,16)*magE-P(20,17)*magN);
|
||||
Kfusion(21) = -t4*t13*(P(21,16)*magE-P(21,17)*magN);
|
||||
Kfusion(22) = -t4*t13*(P(22,16)*magE-P(22,17)*magN);
|
||||
Kfusion(23) = -t4*t13*(P(23,16)*magE-P(23,17)*magN);
|
||||
for (unsigned row = 0; row <= 15; row++) {
|
||||
Kfusion(row) = -HK9*(HK5*P(row,16) - P(row,17));
|
||||
}
|
||||
|
||||
Kfusion(16) = -HK8*HK9;
|
||||
Kfusion(17) = -HK6*HK9;
|
||||
|
||||
for (unsigned row = 18; row <= 23; row++) {
|
||||
Kfusion(row) = -HK9*(HK5*P(16,row) - P(17,row));
|
||||
}
|
||||
|
||||
const float innovation = math::constrain(atan2f(magE, magN) - getMagDeclination(), -0.5f, 0.5f);
|
||||
|
||||
@@ -939,19 +930,7 @@ void Ekf::fuseDeclination(float decl_sigma)
|
||||
// first calculate expression for KHP
|
||||
// then calculate P - KHP
|
||||
// take advantage of the empty columns in KH to reduce the number of operations
|
||||
SquareMatrix24f KHP;
|
||||
float KH[2];
|
||||
for (unsigned row = 0; row < _k_num_states; row++) {
|
||||
|
||||
KH[0] = Kfusion(row) * H_DECL(16);
|
||||
KH[1] = Kfusion(row) * H_DECL(17);
|
||||
|
||||
for (unsigned column = 0; column < _k_num_states; column++) {
|
||||
float tmp = KH[0] * P(16,column);
|
||||
tmp += KH[1] * P(17,column);
|
||||
KHP(row,column) = tmp;
|
||||
}
|
||||
}
|
||||
const SquareMatrix24f KHP = computeKHP(Kfusion, Hfusion);
|
||||
|
||||
const bool healthy = checkAndFixCovarianceUpdate(KHP);
|
||||
|
||||
|
||||
Reference in New Issue
Block a user