diff --git a/src/modules/ekf2/EKF/mag_fusion.cpp b/src/modules/ekf2/EKF/mag_fusion.cpp index c6a865569a..3b78e0aeda 100644 --- a/src/modules/ekf2/EKF/mag_fusion.cpp +++ b/src/modules/ekf2/EKF/mag_fusion.cpp @@ -629,48 +629,28 @@ bool Ekf::fuseYaw(const float innovation, const float variance, estimator_aid_so _innov_check_fail_status.flags.reject_yaw = false; } - // apply covariance correction via P_new = (I -K*H)*P - // first calculate expression for KHP - // then calculate P - KHP - SquareMatrix24f KHP; - float KH[4]; + // copy observation jacobian + SparseVector24f<0,1,2,3> Hfusion; + Hfusion.at<0>() = H_YAW(0); + Hfusion.at<1>() = H_YAW(1); + Hfusion.at<2>() = H_YAW(2); + Hfusion.at<3>() = H_YAW(3); - for (unsigned row = 0; row < _k_num_states; row++) { - - KH[0] = Kfusion(row) * H_YAW(0); - KH[1] = Kfusion(row) * H_YAW(1); - KH[2] = Kfusion(row) * H_YAW(2); - KH[3] = Kfusion(row) * H_YAW(3); - - 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); - KHP(row, column) = tmp; - } - } - - const bool healthy = checkAndFixCovarianceUpdate(KHP); - - _fault_status.flags.bad_hdg = !healthy; - - if (healthy) { - // apply the covariance corrections - P -= KHP; - - fixCovarianceErrors(true); - - // apply the state corrections - fuse(Kfusion, aid_src_status.innovation); + if (measurementUpdate(Kfusion, Hfusion, aid_src_status.innovation)) { _time_last_heading_fuse = _imu_sample_delayed.time_us; + aid_src_status.time_last_fuse = _imu_sample_delayed.time_us; aid_src_status.fused = true; + _fault_status.flags.bad_hdg = false; + return true; } + // otherwise + aid_src_status.fused = false; + _fault_status.flags.bad_hdg = true; return false; } diff --git a/src/modules/ekf2/EKF/vel_pos_fusion.cpp b/src/modules/ekf2/EKF/vel_pos_fusion.cpp index fe6af2fa79..fcf1ea3b34 100644 --- a/src/modules/ekf2/EKF/vel_pos_fusion.cpp +++ b/src/modules/ekf2/EKF/vel_pos_fusion.cpp @@ -200,6 +200,12 @@ bool Ekf::fuseVelPosHeight(const float innov, const float innov_var, const int o Kfusion(row) = P(row, state_index) / innov_var; } + for (unsigned i = 0; i < 3; i++) { + if (_accel_bias_inhibit[i]) { + Kfusion(13 + i) = 0.0f; + } + } + SquareMatrix24f KHP; for (unsigned row = 0; row < _k_num_states; row++) { @@ -208,18 +214,7 @@ bool Ekf::fuseVelPosHeight(const float innov, const float innov_var, const int o } } - // if the covariance correction will result in a negative variance, then - // the covariance matrix is unhealthy and must be corrected - bool healthy = true; - - for (int i = 0; i < _k_num_states; i++) { - if (P(i, i) < KHP(i, i)) { - // zero rows and columns - P.uncorrelateCovarianceSetVariance<1>(i, 0.0f); - - healthy = false; - } - } + const bool healthy = checkAndFixCovarianceUpdate(KHP); setVelPosStatus(obs_index, healthy); diff --git a/src/modules/ekf2/test/change_indication/ekf_gsf_reset.csv b/src/modules/ekf2/test/change_indication/ekf_gsf_reset.csv index 0081e4c3f4..7efb25d9b5 100644 --- a/src/modules/ekf2/test/change_indication/ekf_gsf_reset.csv +++ b/src/modules/ekf2/test/change_indication/ekf_gsf_reset.csv @@ -156,14 +156,14 @@ Timestamp,state[0],state[1],state[2],state[3],state[4],state[5],state[6],state[7 15390000,0.705,0.00025,-0.013,0.709,0.00302,-0.000168,-0.0241,0.000544,-0.00202,-365,-1.18e-05,-6.06e-05,1.84e-06,-5.43e-06,2.99e-05,-0.00123,0.207,0.00204,0.435,0,0,0,0,0,9.25e-05,0.000185,0.000185,9e-05,0.04,0.04,0.0278,0.0466,0.0466,0.051,2.12e-09,2.12e-09,1.33e-09,3.53e-06,3.53e-06,3.68e-07,0,0,0,0,0,0,0,0 15490000,0.705,0.000267,-0.013,0.709,0.00428,-0.000538,-0.0241,0.00092,-0.00208,-365,-1.18e-05,-6.06e-05,1.51e-06,-5.25e-06,2.97e-05,-0.00122,0.207,0.00204,0.435,0,0,0,0,0,9.25e-05,0.00019,0.00019,8.99e-05,0.045,0.045,0.0282,0.0533,0.0533,0.0523,2.12e-09,2.12e-09,1.3e-09,3.53e-06,3.53e-06,3.57e-07,0,0,0,0,0,0,0,0 15590000,0.705,0.000283,-0.013,0.709,0.00241,-0.000538,-0.0227,-0.00132,-0.00172,-365,-1.2e-05,-6.05e-05,1.32e-06,-5.38e-06,3.18e-05,-0.00123,0.207,0.00204,0.435,0,0,0,0,0,9.22e-05,0.000183,0.000182,8.96e-05,0.0394,0.0394,0.027,0.0465,0.0465,0.0513,1.96e-09,1.97e-09,1.27e-09,3.5e-06,3.5e-06,3.31e-07,0,0,0,0,0,0,0,0 -15690000,0.705,0.000285,-0.013,0.709,0.00273,-0.000698,-0.0231,-0.00108,-0.00176,-365,-1.2e-05,-6.05e-05,1.35e-06,-5.39e-06,3.18e-05,-0.00123,0.207,0.00204,0.435,0,0,0,0,0,9.19e-05,0.000187,0.000187,8.93e-05,0.0443,0.0443,0.027,0.0531,0.0531,0.0519,1.96e-09,1.97e-09,1.24e-09,3.5e-06,3.5e-06,3.2e-07,0,0,0,0,0,0,0,0 +15690000,0.705,0.000285,-0.013,0.709,0.00273,-0.000699,-0.0231,-0.00108,-0.00176,-365,-1.2e-05,-6.05e-05,1.35e-06,-5.39e-06,3.18e-05,-0.00123,0.207,0.00204,0.435,0,0,0,0,0,9.19e-05,0.000187,0.000187,8.93e-05,0.0443,0.0443,0.027,0.0531,0.0531,0.0519,1.96e-09,1.97e-09,1.24e-09,3.5e-06,3.5e-06,3.2e-07,0,0,0,0,0,0,0,0 15790000,0.705,0.000245,-0.013,0.709,0.00322,-0.00237,-0.0253,-0.000974,-0.00282,-365,-1.21e-05,-6.07e-05,1.38e-06,-7.78e-06,3.21e-05,-0.00122,0.207,0.00204,0.435,0,0,0,0,0,9.15e-05,0.000179,0.000179,8.9e-05,0.0388,0.0388,0.0258,0.0463,0.0463,0.0509,1.82e-09,1.82e-09,1.21e-09,3.47e-06,3.48e-06,2.97e-07,0,0,0,0,0,0,0,0 15890000,0.705,0.000197,-0.013,0.709,0.00414,-0.00286,-0.0237,-0.000577,-0.00309,-365,-1.21e-05,-6.07e-05,1.48e-06,-8e-06,3.23e-05,-0.00123,0.207,0.00204,0.435,0,0,0,0,0,9.15e-05,0.000184,0.000183,8.89e-05,0.0437,0.0437,0.026,0.0529,0.0529,0.052,1.82e-09,1.82e-09,1.18e-09,3.47e-06,3.48e-06,2.87e-07,0,0,0,0,0,0,0,0 15990000,0.705,0.000139,-0.013,0.709,0.004,-0.00378,-0.0192,-0.00065,-0.00385,-365,-1.22e-05,-6.09e-05,1.93e-06,-1.1e-05,3.42e-05,-0.00124,0.207,0.00204,0.435,0,0,0,0,0,9.11e-05,0.000176,0.000176,8.86e-05,0.0383,0.0383,0.0249,0.0462,0.0462,0.051,1.67e-09,1.67e-09,1.16e-09,3.45e-06,3.45e-06,2.67e-07,0,0,0,0,0,0,0,0 16090000,0.705,0.000139,-0.013,0.709,0.00574,-0.00398,-0.0156,-0.000181,-0.00427,-365,-1.22e-05,-6.09e-05,2.54e-06,-1.16e-05,3.48e-05,-0.00125,0.207,0.00204,0.435,0,0,0,0,0,9.08e-05,0.00018,0.00018,8.83e-05,0.0431,0.0431,0.0248,0.0528,0.0528,0.0514,1.67e-09,1.67e-09,1.13e-09,3.45e-06,3.45e-06,2.57e-07,0,0,0,0,0,0,0,0 16190000,0.705,0.000164,-0.0129,0.709,0.00573,-0.00321,-0.0143,-0.000392,-0.00345,-365,-1.23e-05,-6.08e-05,2.65e-06,-1.06e-05,3.7e-05,-0.00125,0.207,0.00204,0.435,0,0,0,0,0,9.07e-05,0.000172,0.000172,8.81e-05,0.0378,0.0378,0.0239,0.0461,0.0461,0.051,1.54e-09,1.54e-09,1.11e-09,3.42e-06,3.42e-06,2.4e-07,0,0,0,0,0,0,0,0 16290000,0.705,0.000182,-0.013,0.709,0.00736,-0.004,-0.0156,0.000268,-0.0038,-365,-1.23e-05,-6.08e-05,3.21e-06,-1.05e-05,3.69e-05,-0.00125,0.207,0.00204,0.435,0,0,0,0,0,9.04e-05,0.000176,0.000176,8.78e-05,0.0425,0.0425,0.0237,0.0526,0.0526,0.0514,1.54e-09,1.54e-09,1.08e-09,3.42e-06,3.42e-06,2.31e-07,0,0,0,0,0,0,0,0 -16390000,0.705,0.000172,-0.013,0.709,0.00624,-0.00424,-0.0148,-6.31e-05,-0.00303,-365,-1.24e-05,-6.07e-05,3.07e-06,-9.28e-06,3.96e-05,-0.00126,0.207,0.00204,0.435,0,0,0,0,0,9.01e-05,0.000168,0.000168,8.75e-05,0.0373,0.0373,0.0227,0.046,0.046,0.0504,1.41e-09,1.41e-09,1.06e-09,3.38e-06,3.39e-06,2.15e-07,0,0,0,0,0,0,0,0 +16390000,0.705,0.000172,-0.013,0.709,0.00624,-0.00424,-0.0148,-6.31e-05,-0.00303,-365,-1.24e-05,-6.07e-05,3.07e-06,-9.27e-06,3.96e-05,-0.00126,0.207,0.00204,0.435,0,0,0,0,0,9.01e-05,0.000168,0.000168,8.75e-05,0.0373,0.0373,0.0227,0.046,0.046,0.0504,1.41e-09,1.41e-09,1.06e-09,3.38e-06,3.39e-06,2.15e-07,0,0,0,0,0,0,0,0 16490000,0.705,0.000188,-0.0129,0.709,0.00545,-0.00377,-0.0177,0.000495,-0.00342,-365,-1.24e-05,-6.07e-05,3.2e-06,-8.96e-06,3.93e-05,-0.00125,0.207,0.00204,0.435,0,0,0,0,0,9e-05,0.000172,0.000172,8.74e-05,0.0419,0.0419,0.0227,0.0525,0.0525,0.0513,1.41e-09,1.41e-09,1.03e-09,3.38e-06,3.39e-06,2.08e-07,0,0,0,0,0,0,0,0 16590000,0.705,0.000448,-0.0129,0.709,0.00184,-0.00103,-0.0181,-0.00246,-3.87e-05,-365,-1.29e-05,-6.02e-05,3.29e-06,-2.12e-06,4.81e-05,-0.00125,0.207,0.00204,0.435,0,0,0,0,0,8.97e-05,0.000164,0.000164,8.71e-05,0.0368,0.0368,0.0217,0.0459,0.0459,0.0503,1.28e-09,1.29e-09,1.01e-09,3.35e-06,3.36e-06,1.94e-07,0,0,0,0,0,0,0,0 16690000,0.705,0.000437,-0.0129,0.709,0.00203,-0.000553,-0.0146,-0.00225,-0.000118,-365,-1.29e-05,-6.02e-05,3.07e-06,-2.7e-06,4.86e-05,-0.00126,0.207,0.00204,0.435,0,0,0,0,0,8.94e-05,0.000168,0.000167,8.68e-05,0.0413,0.0413,0.0215,0.0523,0.0523,0.0505,1.28e-09,1.29e-09,9.88e-10,3.35e-06,3.36e-06,1.87e-07,0,0,0,0,0,0,0,0 @@ -175,7 +175,7 @@ Timestamp,state[0],state[1],state[2],state[3],state[4],state[5],state[6],state[7 17290000,0.705,0.000471,-0.0126,0.709,0.00172,0.00247,-0.00666,-0.00559,-0.000317,-365,-1.35e-05,-6.02e-05,2.79e-06,-5.57e-06,6.01e-05,-0.00128,0.207,0.00204,0.435,0,0,0,0,0,8.79e-05,0.000154,0.000154,8.53e-05,0.0392,0.0392,0.0185,0.052,0.052,0.0491,9.66e-10,9.67e-10,8.66e-10,3.26e-06,3.27e-06,1.38e-07,0,0,0,0,0,0,0,0 17390000,0.705,0.000433,-0.0126,0.709,0.00238,0.00162,-0.00476,-0.00468,-0.00161,-365,-1.35e-05,-6.05e-05,3.09e-06,-9.32e-06,6e-05,-0.00128,0.207,0.00204,0.435,0,0,0,0,0,8.76e-05,0.000147,0.000147,8.5e-05,0.0345,0.0345,0.0178,0.0456,0.0456,0.0482,8.76e-10,8.77e-10,8.47e-10,3.24e-06,3.24e-06,1.29e-07,0,0,0,0,0,0,0,0 17490000,0.705,0.000429,-0.0126,0.709,0.00292,0.00119,-0.00303,-0.00443,-0.00147,-365,-1.35e-05,-6.05e-05,3.14e-06,-9.42e-06,6.01e-05,-0.00129,0.207,0.00204,0.435,0,0,0,0,0,8.74e-05,0.00015,0.00015,8.49e-05,0.0385,0.0385,0.0177,0.0519,0.0519,0.0488,8.76e-10,8.77e-10,8.3e-10,3.24e-06,3.24e-06,1.25e-07,0,0,0,0,0,0,0,0 -17590000,0.705,0.000339,-0.0125,0.709,0.00418,-3.39e-06,0.00241,-0.00372,-0.00258,-365,-1.35e-05,-6.06e-05,3.24e-06,-1.28e-05,6.05e-05,-0.00129,0.207,0.00204,0.435,0,0,0,0,0,8.71e-05,0.000143,0.000143,8.46e-05,0.0339,0.0339,0.017,0.0455,0.0455,0.0478,7.94e-10,7.94e-10,8.12e-10,3.21e-06,3.21e-06,1.18e-07,0,0,0,0,0,0,0,0 +17590000,0.705,0.000339,-0.0125,0.709,0.00418,-3.4e-06,0.00241,-0.00372,-0.00258,-365,-1.35e-05,-6.06e-05,3.24e-06,-1.28e-05,6.05e-05,-0.00129,0.207,0.00204,0.435,0,0,0,0,0,8.71e-05,0.000143,0.000143,8.46e-05,0.0339,0.0339,0.017,0.0455,0.0455,0.0478,7.94e-10,7.94e-10,8.12e-10,3.21e-06,3.21e-06,1.18e-07,0,0,0,0,0,0,0,0 17690000,0.705,0.000309,-0.0125,0.709,0.00506,0.000712,0.00181,-0.00325,-0.00257,-365,-1.35e-05,-6.06e-05,3.38e-06,-1.28e-05,6.05e-05,-0.00129,0.207,0.00204,0.435,0,0,0,0,0,8.68e-05,0.000146,0.000145,8.43e-05,0.0377,0.0377,0.0168,0.0517,0.0517,0.0479,7.94e-10,7.94e-10,7.94e-10,3.21e-06,3.21e-06,1.14e-07,0,0,0,0,0,0,0,0 17790000,0.706,0.000217,-0.0125,0.709,0.00766,0.000423,0.000504,-0.00208,-0.00219,-365,-1.34e-05,-6.06e-05,3.98e-06,-1.23e-05,5.71e-05,-0.00129,0.207,0.00204,0.435,0,0,0,0,0,8.67e-05,0.000139,0.000139,8.41e-05,0.0332,0.0332,0.0162,0.0454,0.0454,0.0475,7.19e-10,7.19e-10,7.79e-10,3.19e-06,3.19e-06,1.08e-07,0,0,0,0,0,0,0,0 17890000,0.706,0.000227,-0.0125,0.708,0.00918,-0.00033,0.00061,-0.00124,-0.00215,-365,-1.33e-05,-6.06e-05,4.22e-06,-1.23e-05,5.71e-05,-0.00129,0.207,0.00204,0.435,0,0,0,0,0,8.64e-05,0.000142,0.000141,8.38e-05,0.0369,0.0369,0.016,0.0516,0.0516,0.0475,7.19e-10,7.2e-10,7.62e-10,3.19e-06,3.19e-06,1.04e-07,0,0,0,0,0,0,0,0 @@ -211,7 +211,7 @@ Timestamp,state[0],state[1],state[2],state[3],state[4],state[5],state[6],state[7 20890000,0.707,0.000421,-0.012,0.707,-0.00384,-0.0135,0.014,0.000977,-0.00373,-365,-1.43e-05,-6.02e-05,7.38e-06,-4.41e-06,7.98e-05,-0.00129,0.207,0.00204,0.435,0,0,0,0,0,7.95e-05,0.000101,0.000101,7.71e-05,0.0253,0.0253,0.00902,0.0486,0.0486,0.0395,1.82e-10,1.82e-10,4.26e-10,2.96e-06,2.96e-06,5e-08,0,0,0,0,0,0,0,0 20990000,0.707,0.000425,-0.0121,0.707,-0.00404,-0.0142,0.0145,0.00263,-0.00305,-365,-1.43e-05,-6.01e-05,7.39e-06,-2.15e-06,7.86e-05,-0.00129,0.207,0.00204,0.435,0,0,0,0,0,7.92e-05,9.94e-05,9.92e-05,7.69e-05,0.0227,0.0227,0.00882,0.0432,0.0432,0.039,1.68e-10,1.68e-10,4.18e-10,2.96e-06,2.96e-06,5e-08,0,0,0,0,0,0,0,0 21090000,0.707,0.000422,-0.0121,0.707,-0.00421,-0.0167,0.0149,0.00221,-0.0046,-365,-1.43e-05,-6.01e-05,7.54e-06,-2.17e-06,7.86e-05,-0.00129,0.207,0.00204,0.435,0,0,0,0,0,7.91e-05,0.0001,9.99e-05,7.68e-05,0.0247,0.0247,0.00883,0.0483,0.0483,0.0393,1.68e-10,1.68e-10,4.11e-10,2.96e-06,2.96e-06,5e-08,0,0,0,0,0,0,0,0 -21190000,0.707,0.00046,-0.0121,0.707,-0.00342,-0.0155,0.014,0.00371,-0.00374,-365,-1.42e-05,-6e-05,7.44e-06,3.82e-07,7.75e-05,-0.00129,0.207,0.00204,0.435,0,0,0,0,0,7.89e-05,9.81e-05,9.79e-05,7.65e-05,0.0221,0.0221,0.00865,0.043,0.043,0.0388,1.56e-10,1.56e-10,4.04e-10,2.95e-06,2.95e-06,5e-08,0,0,0,0,0,0,0,0 +21190000,0.707,0.00046,-0.0121,0.707,-0.00342,-0.0155,0.014,0.00371,-0.00374,-365,-1.42e-05,-6e-05,7.44e-06,3.83e-07,7.75e-05,-0.00129,0.207,0.00204,0.435,0,0,0,0,0,7.89e-05,9.81e-05,9.79e-05,7.65e-05,0.0221,0.0221,0.00865,0.043,0.043,0.0388,1.56e-10,1.56e-10,4.04e-10,2.95e-06,2.95e-06,5e-08,0,0,0,0,0,0,0,0 21290000,0.708,0.0005,-0.0121,0.706,-0.00401,-0.0177,0.016,0.00334,-0.00539,-365,-1.42e-05,-6e-05,7.74e-06,4.08e-07,7.75e-05,-0.00129,0.207,0.00204,0.435,0,0,0,0,0,7.86e-05,9.87e-05,9.85e-05,7.63e-05,0.0241,0.0241,0.0086,0.0481,0.0481,0.0386,1.56e-10,1.56e-10,3.96e-10,2.95e-06,2.95e-06,5e-08,0,0,0,0,0,0,0,0 21390000,0.708,0.000547,-0.0121,0.707,-0.00481,-0.0169,0.0158,0.00281,-0.00336,-365,-1.42e-05,-5.98e-05,7.55e-06,4.33e-06,7.74e-05,-0.00129,0.207,0.00204,0.435,0,0,0,0,0,7.85e-05,9.69e-05,9.67e-05,7.62e-05,0.0216,0.0216,0.00849,0.0429,0.0429,0.0385,1.45e-10,1.45e-10,3.9e-10,2.94e-06,2.94e-06,5e-08,0,0,0,0,0,0,0,0 21490000,0.708,0.000554,-0.0121,0.706,-0.00533,-0.018,0.0154,0.00228,-0.0051,-365,-1.42e-05,-5.98e-05,7.65e-06,4.29e-06,7.75e-05,-0.00129,0.207,0.00204,0.435,0,0,0,0,0,7.83e-05,9.74e-05,9.73e-05,7.6e-05,0.0235,0.0235,0.00846,0.0478,0.0478,0.0384,1.45e-10,1.45e-10,3.83e-10,2.94e-06,2.94e-06,5e-08,0,0,0,0,0,0,0,0 @@ -286,7 +286,7 @@ Timestamp,state[0],state[1],state[2],state[3],state[4],state[5],state[6],state[7 28390000,0.711,0.0115,0.0226,0.702,-2.76,-1.29,0.769,-4.72,-2.56,-370,-8.22e-06,-5.79e-05,4.68e-06,5.36e-05,-0.000142,-0.0012,0.207,0.00204,0.435,0,0,0,0,0,6.63e-05,7.73e-05,7.67e-05,6.53e-05,0.0216,0.021,0.00865,0.1,0.0991,0.0356,3.77e-11,3.76e-11,1.44e-10,2.81e-06,2.8e-06,5e-08,0,0,0,0,0,0,0,0 28490000,0.712,0.00262,0.00443,0.703,-2.71,-1.28,1.06,-4.99,-2.69,-370,-8.22e-06,-5.79e-05,4.63e-06,5.11e-05,-0.000136,-0.00119,0.207,0.00204,0.435,0,0,0,0,0,6.61e-05,7.76e-05,7.7e-05,6.5e-05,0.0227,0.0222,0.00878,0.108,0.107,0.036,3.78e-11,3.76e-11,1.43e-10,2.81e-06,2.8e-06,5e-08,0,0,0,0,0,0,0,0 28590000,0.711,0.000848,0.000917,0.704,-2.64,-1.26,0.959,-5.26,-2.82,-370,-8.21e-06,-5.79e-05,4.62e-06,4.74e-05,-0.000127,-0.00119,0.207,0.00204,0.435,0,0,0,0,0,6.59e-05,7.78e-05,7.72e-05,6.49e-05,0.0239,0.0234,0.00887,0.117,0.115,0.0361,3.79e-11,3.77e-11,1.41e-10,2.81e-06,2.8e-06,5e-08,0,0,0,0,0,0,0,0 -28690000,0.71,0.000143,1.52e-06,0.705,-2.56,-1.25,0.962,-5.52,-2.94,-370,-8.2e-06,-5.79e-05,4.56e-06,4.33e-05,-0.000116,-0.00118,0.207,0.00204,0.435,0,0,0,0,0,6.58e-05,7.79e-05,7.74e-05,6.48e-05,0.0251,0.0247,0.00896,0.126,0.125,0.0362,3.8e-11,3.78e-11,1.39e-10,2.81e-06,2.8e-06,5e-08,0,0,0,0,0,0,0,0 +28690000,0.71,0.000143,1.53e-06,0.705,-2.56,-1.25,0.962,-5.52,-2.94,-370,-8.2e-06,-5.79e-05,4.56e-06,4.33e-05,-0.000116,-0.00118,0.207,0.00204,0.435,0,0,0,0,0,6.58e-05,7.79e-05,7.74e-05,6.48e-05,0.0251,0.0247,0.00896,0.126,0.125,0.0362,3.8e-11,3.78e-11,1.39e-10,2.81e-06,2.8e-06,5e-08,0,0,0,0,0,0,0,0 28790000,0.709,-0.000139,-0.000243,0.705,-2.53,-1.22,0.967,-5.85,-3.05,-370,-8.9e-06,-5.77e-05,4.52e-06,1.26e-05,-0.000185,-0.00117,0.207,0.00204,0.435,0,0,0,0,0,6.55e-05,7.81e-05,7.74e-05,6.46e-05,0.024,0.0237,0.00889,0.127,0.125,0.0359,3.73e-11,3.71e-11,1.38e-10,2.8e-06,2.78e-06,5e-08,0,0,0,0,0,0,0,0 28890000,0.708,-0.000158,-2.47e-05,0.706,-2.46,-1.2,0.956,-6.1,-3.17,-370,-8.89e-06,-5.77e-05,4.48e-06,8.29e-06,-0.000175,-0.00116,0.207,0.00204,0.435,0,0,0,0,0,6.54e-05,7.82e-05,7.76e-05,6.46e-05,0.0252,0.025,0.00902,0.136,0.135,0.0364,3.74e-11,3.72e-11,1.36e-10,2.8e-06,2.78e-06,5e-08,0,0,0,0,0,0,0,0 28990000,0.708,4.28e-05,0.000428,0.706,-2.45,-1.18,0.95,-6.44,-3.28,-370,-9.76e-06,-5.76e-05,4.37e-06,-1.27e-05,-0.000254,-0.00115,0.207,0.00204,0.435,0,0,0,0,0,6.51e-05,7.84e-05,7.76e-05,6.44e-05,0.0241,0.024,0.00895,0.136,0.135,0.0361,3.68e-11,3.65e-11,1.35e-10,2.79e-06,2.77e-06,5e-08,0,0,0,0,0,0,0,0 @@ -329,14 +329,14 @@ Timestamp,state[0],state[1],state[2],state[3],state[4],state[5],state[6],state[7 32690000,0.708,-0.000272,-0.00353,0.706,-1.39,-0.794,0.71,-14.1,-6.77,-367,-1.46e-05,-5.66e-05,2.26e-06,-0.000319,6.42e-05,-0.00094,0.207,0.00204,0.435,0,0,0,0,0,6.15e-05,8.09e-05,7.48e-05,5.94e-05,0.0259,0.026,0.00847,0.32,0.318,0.0363,2.9e-11,2.9e-11,9.33e-11,2.71e-06,2.52e-06,5e-08,0,0,0,0,0,0,0,0 32790000,0.708,-0.000234,-0.00347,0.706,-1.36,-0.783,0.706,-14.2,-6.84,-367,-1.46e-05,-5.66e-05,2.23e-06,-0.000323,7.35e-05,-0.000937,0.207,0.00204,0.435,0,0,0,0,0,6.14e-05,8.07e-05,7.44e-05,5.93e-05,0.0246,0.0247,0.00841,0.315,0.314,0.0364,2.86e-11,2.86e-11,9.25e-11,2.71e-06,2.51e-06,5e-08,0,0,0,0,0,0,0,0 32890000,0.708,-0.000142,-0.00343,0.706,-1.34,-0.773,0.706,-14.4,-6.92,-367,-1.46e-05,-5.66e-05,2.08e-06,-0.000328,8.42e-05,-0.000933,0.207,0.00204,0.435,0,0,0,0,0,6.13e-05,8.09e-05,7.45e-05,5.92e-05,0.0258,0.0259,0.00843,0.329,0.327,0.0364,2.87e-11,2.87e-11,9.17e-11,2.71e-06,2.51e-06,5e-08,0,0,0,0,0,0,0,0 -32990000,0.708,-1.17e-05,-0.00336,0.706,-1.32,-0.765,0.7,-14.5,-7,-367,-1.48e-05,-5.66e-05,2.2e-06,-0.000334,9.76e-05,-0.000928,0.207,0.00204,0.435,0,0,0,0,0,6.11e-05,8.07e-05,7.42e-05,5.91e-05,0.0246,0.0246,0.00833,0.325,0.323,0.0361,2.83e-11,2.84e-11,9.09e-11,2.71e-06,2.5e-06,5e-08,0,0,0,0,0,0,0,0 +32990000,0.708,-1.16e-05,-0.00336,0.706,-1.32,-0.765,0.7,-14.5,-7,-367,-1.48e-05,-5.66e-05,2.2e-06,-0.000334,9.76e-05,-0.000928,0.207,0.00204,0.435,0,0,0,0,0,6.11e-05,8.07e-05,7.42e-05,5.91e-05,0.0246,0.0246,0.00833,0.325,0.323,0.0361,2.83e-11,2.84e-11,9.09e-11,2.71e-06,2.5e-06,5e-08,0,0,0,0,0,0,0,0 33090000,0.708,-4.52e-05,-0.00339,0.706,-1.29,-0.756,0.695,-14.7,-7.08,-367,-1.48e-05,-5.66e-05,2.26e-06,-0.000336,0.000103,-0.000926,0.207,0.00204,0.435,0,0,0,0,0,6.11e-05,8.09e-05,7.42e-05,5.9e-05,0.0258,0.0258,0.0084,0.338,0.336,0.0365,2.84e-11,2.85e-11,9.02e-11,2.71e-06,2.5e-06,5e-08,0,0,0,0,0,0,0,0 33190000,0.704,0.00336,-0.00242,0.71,-1.26,-0.745,0.636,-14.8,-7.15,-367,-1.48e-05,-5.66e-05,2.28e-06,-0.000339,0.00011,-0.000924,0.207,0.00204,0.435,0,0,0,0,0,6.08e-05,8.06e-05,7.39e-05,5.91e-05,0.0246,0.0246,0.0083,0.334,0.332,0.0362,2.81e-11,2.82e-11,8.93e-11,2.71e-06,2.5e-06,5e-08,0,0,0,0,0,0,0,0 33290000,0.652,0.0155,-0.00152,0.758,-1.26,-0.727,0.618,-14.9,-7.22,-367,-1.48e-05,-5.66e-05,2.32e-06,-0.000341,0.000114,-0.000922,0.207,0.00204,0.435,0,0,0,0,0,5.86e-05,8.02e-05,7.45e-05,6.13e-05,0.0258,0.0258,0.00832,0.347,0.345,0.0362,2.82e-11,2.83e-11,8.86e-11,2.71e-06,2.49e-06,5e-08,0,0,0,0,0,0,0,0 33390000,0.549,0.0135,-0.0018,0.836,-1.26,-0.715,0.809,-15.1,-7.3,-366,-1.49e-05,-5.66e-05,2.36e-06,-0.000346,0.000123,-0.000919,0.207,0.00204,0.435,0,0,0,0,0,5.46e-05,7.91e-05,7.53e-05,6.52e-05,0.0243,0.0243,0.00826,0.343,0.341,0.0362,2.78e-11,2.8e-11,8.79e-11,2.7e-06,2.49e-06,5e-08,0,0,0,0,0,0,0,0 33490000,0.411,0.0069,0.00065,0.912,-1.24,-0.71,0.826,-15.2,-7.37,-366,-1.49e-05,-5.66e-05,2.35e-06,-0.000346,0.000124,-0.000919,0.207,0.00204,0.435,0,0,0,0,0,5.03e-05,7.8e-05,7.71e-05,6.96e-05,0.0258,0.0258,0.00809,0.356,0.354,0.0362,2.79e-11,2.81e-11,8.71e-11,2.7e-06,2.49e-06,5e-08,0,0,0,0,0,0,0,0 33590000,0.254,0.00101,-0.00188,0.967,-1.21,-0.708,0.792,-15.3,-7.43,-366,-1.48e-05,-5.66e-05,2.35e-06,-0.000346,0.000124,-0.000919,0.207,0.00204,0.435,0,0,0,0,0,4.67e-05,7.61e-05,7.82e-05,7.32e-05,0.0253,0.0254,0.00778,0.352,0.35,0.0359,2.75e-11,2.77e-11,8.63e-11,2.7e-06,2.49e-06,5e-08,0,0,0,0,0,0,0,0 -33690000,0.0871,-0.00228,-0.00496,0.996,-1.15,-0.705,0.798,-15.4,-7.5,-366,-1.48e-05,-5.66e-05,2.42e-06,-0.000346,0.000124,-0.000919,0.207,0.00204,0.435,0,0,0,0,0,4.47e-05,7.48e-05,8e-05,7.55e-05,0.0278,0.028,0.00764,0.365,0.363,0.0362,2.76e-11,2.78e-11,8.57e-11,2.7e-06,2.49e-06,5e-08,0,0,0,0,0,0,0,0 +33690000,0.0871,-0.00227,-0.00496,0.996,-1.15,-0.705,0.798,-15.4,-7.5,-366,-1.48e-05,-5.66e-05,2.42e-06,-0.000346,0.000124,-0.000919,0.207,0.00204,0.435,0,0,0,0,0,4.47e-05,7.48e-05,8e-05,7.55e-05,0.0278,0.028,0.00764,0.365,0.363,0.0362,2.76e-11,2.78e-11,8.57e-11,2.7e-06,2.49e-06,5e-08,0,0,0,0,0,0,0,0 33790000,-0.0824,-0.00379,-0.00679,0.997,-1.1,-0.688,0.78,-15.6,-7.56,-366,-1.49e-05,-5.65e-05,2.42e-06,-0.000346,0.000124,-0.000919,0.207,0.00204,0.435,0,0,0,0,0,4.43e-05,7.22e-05,7.96e-05,7.59e-05,0.0279,0.0281,0.00737,0.361,0.359,0.0358,2.73e-11,2.75e-11,8.5e-11,2.7e-06,2.49e-06,5e-08,0,0,0,0,0,0,0,0 33890000,-0.249,-0.00494,-0.00753,0.968,-1.03,-0.664,0.765,-15.7,-7.63,-366,-1.49e-05,-5.65e-05,2.43e-06,-0.000346,0.000124,-0.000919,0.207,0.00204,0.435,0,0,0,0,0,4.57e-05,7.15e-05,8.07e-05,7.47e-05,0.0314,0.0318,0.00721,0.374,0.372,0.0357,2.74e-11,2.76e-11,8.42e-11,2.7e-06,2.49e-06,5e-08,0,0,0,0,0,0,0,0 33990000,-0.395,-0.00315,-0.0112,0.919,-0.984,-0.627,0.736,-15.8,-7.69,-366,-1.51e-05,-5.65e-05,2.43e-06,-0.000346,0.000124,-0.000919,0.207,0.00204,0.435,0,0,0,0,0,4.84e-05,6.86e-05,7.77e-05,7.19e-05,0.0315,0.032,0.00698,0.37,0.368,0.0354,2.7e-11,2.72e-11,8.35e-11,2.7e-06,2.49e-06,5e-08,0,0,0,0,0,0,0,0 diff --git a/src/modules/ekf2/test/change_indication/iris_gps.csv b/src/modules/ekf2/test/change_indication/iris_gps.csv index d18ce485b3..cf351c7b4e 100644 --- a/src/modules/ekf2/test/change_indication/iris_gps.csv +++ b/src/modules/ekf2/test/change_indication/iris_gps.csv @@ -260,7 +260,7 @@ Timestamp,state[0],state[1],state[2],state[3],state[4],state[5],state[6],state[7 25790000,0.983,-0.0122,-0.0135,0.182,-0.00107,-0.0298,0.00699,0.171,-0.0923,-3.51,-1.54e-05,-5.82e-05,5.91e-06,0.00068,-0.000461,-0.00122,0.204,0.00201,0.434,0,0,0,0,0,2.04e-06,7.87e-05,7.72e-05,5.75e-05,0.0143,0.0143,0.00798,0.0387,0.0387,0.0352,4.85e-11,4.85e-11,2e-10,2.72e-06,2.74e-06,5e-08,0,0,0,0,0,0,0,0 25890000,0.983,-0.0123,-0.0136,0.182,-0.00674,-0.0275,0.00943,0.171,-0.0951,-3.51,-1.54e-05,-5.82e-05,5.69e-06,0.000681,-0.000461,-0.00123,0.204,0.00201,0.434,0,0,0,0,0,2.03e-06,7.89e-05,7.74e-05,5.74e-05,0.0154,0.0154,0.00807,0.0426,0.0426,0.0356,4.86e-11,4.86e-11,1.97e-10,2.72e-06,2.74e-06,5e-08,0,0,0,0,0,0,0,0 25990000,0.983,-0.0123,-0.0137,0.182,-0.0152,-0.0209,0.00311,0.16,-0.0857,-3.51,-1.55e-05,-5.82e-05,5.59e-06,0.000682,-0.000465,-0.00122,0.204,0.00201,0.434,0,0,0,0,0,2.02e-06,7.89e-05,7.74e-05,5.71e-05,0.0142,0.0142,0.008,0.0386,0.0386,0.0353,4.71e-11,4.71e-11,1.95e-10,2.72e-06,2.74e-06,5e-08,0,0,0,0,0,0,0,0 -26090000,0.983,-0.012,-0.0137,0.182,-0.0203,-0.0206,0.00182,0.158,-0.0877,-3.51,-1.55e-05,-5.82e-05,5.65e-06,0.000682,-0.000465,-0.00122,0.204,0.00201,0.434,0,0,0,0,0,2.01e-06,7.91e-05,7.76e-05,5.68e-05,0.0153,0.0153,0.00804,0.0424,0.0424,0.0354,4.72e-11,4.72e-11,1.92e-10,2.72e-06,2.74e-06,5e-08,0,0,0,0,0,0,0,0 +26090000,0.983,-0.012,-0.0137,0.182,-0.0203,-0.0206,0.00182,0.158,-0.0877,-3.51,-1.55e-05,-5.82e-05,5.64e-06,0.000682,-0.000465,-0.00122,0.204,0.00201,0.434,0,0,0,0,0,2.01e-06,7.91e-05,7.76e-05,5.68e-05,0.0153,0.0153,0.00804,0.0424,0.0424,0.0354,4.72e-11,4.72e-11,1.92e-10,2.72e-06,2.74e-06,5e-08,0,0,0,0,0,0,0,0 26190000,0.983,-0.0119,-0.0142,0.182,-0.0263,-0.0141,-0.00277,0.148,-0.0807,-3.51,-1.56e-05,-5.82e-05,5.66e-06,0.000683,-0.000467,-0.00122,0.204,0.00201,0.434,0,0,0,0,0,2e-06,7.91e-05,7.75e-05,5.65e-05,0.0142,0.0142,0.00797,0.0386,0.0386,0.0351,4.58e-11,4.58e-11,1.89e-10,2.72e-06,2.74e-06,5e-08,0,0,0,0,0,0,0,0 26290000,0.983,-0.012,-0.0145,0.182,-0.0276,-0.0128,-0.00842,0.146,-0.0821,-3.51,-1.56e-05,-5.82e-05,5.52e-06,0.000683,-0.000468,-0.00122,0.204,0.00201,0.434,0,0,0,0,0,1.99e-06,7.93e-05,7.77e-05,5.64e-05,0.0152,0.0152,0.00806,0.0423,0.0423,0.0355,4.59e-11,4.59e-11,1.87e-10,2.72e-06,2.74e-06,5e-08,0,0,0,0,0,0,0,0 26390000,0.983,-0.0114,-0.0144,0.182,-0.0332,-0.00578,-0.00416,0.133,-0.0741,-3.52,-1.57e-05,-5.83e-05,5.37e-06,0.000685,-0.00047,-0.00122,0.204,0.00201,0.434,0,0,0,0,0,1.98e-06,7.93e-05,7.77e-05,5.61e-05,0.0141,0.0141,0.00799,0.0385,0.0385,0.0353,4.46e-11,4.46e-11,1.85e-10,2.72e-06,2.74e-06,5e-08,0,0,0,0,0,0,0,0 @@ -274,7 +274,7 @@ Timestamp,state[0],state[1],state[2],state[3],state[4],state[5],state[6],state[7 27190000,0.983,-0.00893,-0.0145,0.182,-0.0662,0.0358,0.00396,0.0697,-0.0452,-3.52,-1.57e-05,-5.84e-05,4.54e-06,0.000692,-0.000479,-0.00122,0.204,0.00201,0.434,0,0,0,0,0,1.9e-06,7.98e-05,7.82e-05,5.43e-05,0.0137,0.0137,0.008,0.0382,0.0382,0.0354,4.03e-11,4.03e-11,1.67e-10,2.71e-06,2.73e-06,5e-08,0,0,0,0,0,0,0,0 27290000,0.983,-0.00911,-0.0155,0.182,-0.0734,0.042,0.118,0.0628,-0.0413,-3.52,-1.57e-05,-5.84e-05,4.48e-06,0.000692,-0.000479,-0.00122,0.204,0.00201,0.434,0,0,0,0,0,1.89e-06,8e-05,7.84e-05,5.41e-05,0.0146,0.0146,0.00805,0.0419,0.0419,0.0355,4.04e-11,4.04e-11,1.65e-10,2.71e-06,2.73e-06,5e-08,0,0,0,0,0,0,0,0 27390000,0.983,-0.0105,-0.018,0.182,-0.0805,0.0485,0.441,0.0535,-0.0342,-3.5,-1.57e-05,-5.83e-05,4.41e-06,0.000693,-0.000482,-0.00121,0.204,0.00201,0.434,0,0,0,0,0,1.89e-06,7.99e-05,7.83e-05,5.38e-05,0.0133,0.0133,0.00798,0.0381,0.0381,0.0352,3.94e-11,3.94e-11,1.63e-10,2.71e-06,2.73e-06,5e-08,0,0,0,0,0,0,0,0 -27490000,0.983,-0.0119,-0.0199,0.182,-0.0857,0.0545,0.753,0.0451,-0.029,-3.44,-1.57e-05,-5.83e-05,4.22e-06,0.000693,-0.000482,-0.00121,0.204,0.00201,0.434,0,0,0,0,0,1.89e-06,8.01e-05,7.85e-05,5.35e-05,0.0141,0.0141,0.00802,0.0417,0.0417,0.0353,3.95e-11,3.95e-11,1.61e-10,2.71e-06,2.73e-06,5e-08,0,0,0,0,0,0,0,0 +27490000,0.983,-0.0119,-0.0199,0.182,-0.0857,0.0545,0.753,0.0451,-0.029,-3.44,-1.57e-05,-5.83e-05,4.21e-06,0.000693,-0.000482,-0.00121,0.204,0.00201,0.434,0,0,0,0,0,1.89e-06,8.01e-05,7.85e-05,5.35e-05,0.0141,0.0141,0.00802,0.0417,0.0417,0.0353,3.95e-11,3.95e-11,1.61e-10,2.71e-06,2.73e-06,5e-08,0,0,0,0,0,0,0,0 27590000,0.983,-0.0117,-0.0187,0.182,-0.0798,0.0569,0.847,0.037,-0.025,-3.38,-1.56e-05,-5.83e-05,4.16e-06,0.000695,-0.000484,-0.0012,0.204,0.00201,0.434,0,0,0,0,0,1.88e-06,8.01e-05,7.85e-05,5.34e-05,0.0131,0.0131,0.00799,0.038,0.038,0.0353,3.86e-11,3.86e-11,1.59e-10,2.71e-06,2.73e-06,5e-08,0,0,0,0,0,0,0,0 27690000,0.983,-0.0105,-0.0158,0.182,-0.0757,0.0533,0.75,0.0292,-0.0194,-3.3,-1.56e-05,-5.83e-05,4.12e-06,0.000696,-0.000485,-0.0012,0.204,0.00201,0.434,0,0,0,0,0,1.86e-06,8.03e-05,7.87e-05,5.32e-05,0.014,0.014,0.00803,0.0416,0.0416,0.0354,3.87e-11,3.87e-11,1.57e-10,2.71e-06,2.73e-06,5e-08,0,0,0,0,0,0,0,0 27790000,0.983,-0.00917,-0.0144,0.182,-0.0743,0.0508,0.744,0.0233,-0.017,-3.23,-1.55e-05,-5.82e-05,4.1e-06,0.000693,-0.000478,-0.0012,0.204,0.00201,0.434,0,0,0,0,0,1.85e-06,8.04e-05,7.87e-05,5.29e-05,0.013,0.013,0.00796,0.0379,0.0379,0.0351,3.79e-11,3.79e-11,1.55e-10,2.71e-06,2.73e-06,5e-08,0,0,0,0,0,0,0,0 @@ -318,7 +318,7 @@ Timestamp,state[0],state[1],state[2],state[3],state[4],state[5],state[6],state[7 31590000,0.983,-0.00752,-0.0171,0.182,-0.0145,0.00548,0.765,-0.0357,0.0382,-0.452,-1.33e-05,-5.68e-05,4.18e-06,0.000733,-0.000519,-0.00111,0.204,0.00201,0.434,0,0,0,0,0,1.62e-06,8.11e-05,7.94e-05,4.61e-05,0.0125,0.0125,0.00789,0.037,0.037,0.0351,2.87e-11,2.87e-11,1.04e-10,2.69e-06,2.71e-06,5e-08,0,0,0,0,0,0,0,0 31690000,0.983,-0.00751,-0.0176,0.182,-0.0164,0.00432,0.761,-0.0372,0.0386,-0.382,-1.33e-05,-5.68e-05,4.27e-06,0.000734,-0.00052,-0.00111,0.204,0.00201,0.434,0,0,0,0,0,1.61e-06,8.12e-05,7.96e-05,4.59e-05,0.0133,0.0133,0.00793,0.0404,0.0404,0.0352,2.88e-11,2.88e-11,1.03e-10,2.69e-06,2.71e-06,5e-08,0,0,0,0,0,0,0,0 31790000,0.983,-0.00773,-0.0182,0.182,-0.00743,0.00178,0.761,-0.026,0.0368,-0.31,-1.33e-05,-5.67e-05,4.32e-06,0.000739,-0.000519,-0.00111,0.204,0.00201,0.434,0,0,0,0,0,1.61e-06,8.09e-05,7.92e-05,4.58e-05,0.0125,0.0125,0.0079,0.0369,0.0369,0.0353,2.84e-11,2.84e-11,1.02e-10,2.69e-06,2.71e-06,5e-08,0,0,0,0,0,0,0,0 -31890000,0.983,-0.00746,-0.018,0.182,-0.00397,-0.000607,0.759,-0.0265,0.0368,-0.242,-1.33e-05,-5.67e-05,4.36e-06,0.00074,-0.00052,-0.00111,0.204,0.00201,0.434,0,0,0,0,0,1.61e-06,8.1e-05,7.94e-05,4.56e-05,0.0133,0.0133,0.00794,0.0404,0.0404,0.0353,2.85e-11,2.85e-11,1.01e-10,2.69e-06,2.71e-06,5e-08,0,0,0,0,0,0,0,0 +31890000,0.983,-0.00746,-0.018,0.182,-0.00397,-0.000606,0.759,-0.0265,0.0368,-0.242,-1.33e-05,-5.67e-05,4.36e-06,0.00074,-0.00052,-0.00111,0.204,0.00201,0.434,0,0,0,0,0,1.61e-06,8.1e-05,7.94e-05,4.56e-05,0.0133,0.0133,0.00794,0.0404,0.0404,0.0353,2.85e-11,2.85e-11,1.01e-10,2.69e-06,2.71e-06,5e-08,0,0,0,0,0,0,0,0 31990000,0.983,-0.00772,-0.0176,0.182,0.00378,-0.00124,0.755,-0.0149,0.0338,-0.176,-1.33e-05,-5.66e-05,4.3e-06,0.000744,-0.000521,-0.0011,0.204,0.00201,0.434,0,0,0,0,0,1.6e-06,8.07e-05,7.9e-05,4.54e-05,0.0124,0.0124,0.00787,0.0369,0.0369,0.0351,2.81e-11,2.81e-11,1e-10,2.69e-06,2.71e-06,5e-08,0,0,0,0,0,0,0,0 32090000,0.983,-0.00809,-0.0172,0.182,0.00341,-0.00471,0.757,-0.0145,0.0336,-0.105,-1.33e-05,-5.66e-05,4.29e-06,0.000745,-0.000521,-0.0011,0.204,0.00201,0.434,0,0,0,0,0,1.6e-06,8.08e-05,7.92e-05,4.54e-05,0.0133,0.0133,0.00795,0.0403,0.0403,0.0354,2.82e-11,2.82e-11,9.92e-11,2.69e-06,2.71e-06,5e-08,0,0,0,0,0,0,0,0 32190000,0.983,-0.00826,-0.0175,0.182,0.00852,-0.00786,0.757,-0.00364,0.0322,-0.0375,-1.33e-05,-5.66e-05,4.24e-06,0.000749,-0.000519,-0.00109,0.204,0.00201,0.434,0,0,0,0,0,1.59e-06,8.04e-05,7.88e-05,4.52e-05,0.0124,0.0124,0.00789,0.0369,0.0369,0.0352,2.78e-11,2.78e-11,9.83e-11,2.69e-06,2.7e-06,5e-08,0,0,0,0,0,0,0,0 @@ -327,9 +327,9 @@ Timestamp,state[0],state[1],state[2],state[3],state[4],state[5],state[6],state[7 32490000,0.983,-0.0111,-0.0157,0.182,0.0486,-0.0768,-0.118,0.0122,0.0223,0.109,-1.33e-05,-5.65e-05,4.18e-06,0.000753,-0.000518,-0.00109,0.204,0.00201,0.434,0,0,0,0,0,1.57e-06,8.04e-05,7.88e-05,4.48e-05,0.0152,0.0153,0.00778,0.0403,0.0403,0.0353,2.76e-11,2.76e-11,9.57e-11,2.68e-06,2.7e-06,5e-08,0,0,0,0,0,0,0,0 32590000,0.983,-0.0111,-0.0157,0.182,0.0481,-0.0776,-0.12,0.024,0.0187,0.0902,-1.33e-05,-5.65e-05,4.25e-06,0.000753,-0.000518,-0.00109,0.204,0.00201,0.434,0,0,0,0,0,1.56e-06,7.92e-05,7.76e-05,4.46e-05,0.0156,0.0157,0.00749,0.0369,0.0369,0.035,2.72e-11,2.72e-11,9.48e-11,2.68e-06,2.7e-06,5e-08,0,0,0,0,0,0,0,0 32690000,0.983,-0.011,-0.0155,0.182,0.044,-0.0833,-0.122,0.0287,0.0107,0.0753,-1.33e-05,-5.65e-05,4.21e-06,0.000753,-0.000518,-0.00109,0.204,0.00201,0.434,0,0,0,0,0,1.56e-06,7.93e-05,7.77e-05,4.44e-05,0.0185,0.0187,0.00731,0.0406,0.0406,0.035,2.73e-11,2.73e-11,9.39e-11,2.68e-06,2.7e-06,5e-08,0,0,0,0,0,0,0,0 -32790000,0.983,-0.0107,-0.0156,0.182,0.0415,-0.0805,-0.123,0.0379,0.00896,0.0599,-1.34e-05,-5.64e-05,4.28e-06,0.000753,-0.000518,-0.00109,0.204,0.00201,0.434,0,0,0,0,0,1.55e-06,7.67e-05,7.52e-05,4.43e-05,0.0193,0.0195,0.00708,0.0372,0.0372,0.035,2.69e-11,2.69e-11,9.32e-11,2.68e-06,2.7e-06,5e-08,0,0,0,0,0,0,0,0 -32890000,0.983,-0.0107,-0.0156,0.182,0.0413,-0.0865,-0.125,0.0421,0.000592,0.0446,-1.34e-05,-5.64e-05,4.29e-06,0.000753,-0.000518,-0.00109,0.204,0.00201,0.434,0,0,0,0,0,1.55e-06,7.69e-05,7.53e-05,4.42e-05,0.0231,0.0234,0.00692,0.041,0.041,0.035,2.7e-11,2.7e-11,9.24e-11,2.68e-06,2.7e-06,5e-08,0,0,0,0,0,0,0,0 -32990000,0.983,-0.0104,-0.0156,0.182,0.0375,-0.0821,-0.123,0.0493,-0.00241,0.0318,-1.35e-05,-5.64e-05,4.36e-06,0.000753,-0.000518,-0.00109,0.204,0.00201,0.434,0,0,0,0,0,1.54e-06,7.26e-05,7.12e-05,4.4e-05,0.0238,0.0241,0.00668,0.0376,0.0376,0.0346,2.66e-11,2.66e-11,9.15e-11,2.68e-06,2.7e-06,5e-08,0,0,0,0,0,0,0,0 +32790000,0.983,-0.0107,-0.0156,0.182,0.0415,-0.0805,-0.123,0.0379,0.00896,0.0599,-1.34e-05,-5.64e-05,4.27e-06,0.000753,-0.000518,-0.00109,0.204,0.00201,0.434,0,0,0,0,0,1.55e-06,7.67e-05,7.52e-05,4.43e-05,0.0193,0.0195,0.00708,0.0372,0.0372,0.035,2.69e-11,2.69e-11,9.32e-11,2.68e-06,2.7e-06,5e-08,0,0,0,0,0,0,0,0 +32890000,0.983,-0.0107,-0.0156,0.182,0.0413,-0.0865,-0.125,0.0421,0.000593,0.0446,-1.34e-05,-5.64e-05,4.29e-06,0.000753,-0.000518,-0.00109,0.204,0.00201,0.434,0,0,0,0,0,1.55e-06,7.69e-05,7.53e-05,4.42e-05,0.0231,0.0234,0.00692,0.041,0.041,0.035,2.7e-11,2.7e-11,9.24e-11,2.68e-06,2.7e-06,5e-08,0,0,0,0,0,0,0,0 +32990000,0.983,-0.0104,-0.0156,0.182,0.0375,-0.0821,-0.123,0.0493,-0.0024,0.0318,-1.35e-05,-5.64e-05,4.36e-06,0.000753,-0.000518,-0.00109,0.204,0.00201,0.434,0,0,0,0,0,1.54e-06,7.26e-05,7.12e-05,4.4e-05,0.0238,0.0241,0.00668,0.0376,0.0376,0.0346,2.66e-11,2.66e-11,9.15e-11,2.68e-06,2.7e-06,5e-08,0,0,0,0,0,0,0,0 33090000,0.983,-0.0103,-0.0155,0.182,0.0334,-0.0859,-0.12,0.0529,-0.0108,0.0247,-1.35e-05,-5.64e-05,4.32e-06,0.000753,-0.000518,-0.00109,0.204,0.00201,0.434,0,0,0,0,0,1.54e-06,7.27e-05,7.14e-05,4.39e-05,0.0283,0.0288,0.00657,0.0417,0.0418,0.0348,2.67e-11,2.67e-11,9.08e-11,2.68e-06,2.7e-06,5e-08,0,0,0,0,0,0,0,0 33190000,0.983,-0.01,-0.0154,0.182,0.0281,-0.0811,-0.119,0.0581,-0.0124,0.0186,-1.36e-05,-5.64e-05,4.26e-06,0.000753,-0.000522,-0.00109,0.204,0.00201,0.434,0,0,0,0,0,1.53e-06,6.7e-05,6.58e-05,4.38e-05,0.0288,0.0293,0.00637,0.0382,0.0382,0.0345,2.63e-11,2.63e-11,9e-11,2.68e-06,2.7e-06,5e-08,0,0,0,0,0,0,0,0 33290000,0.983,-0.0101,-0.0154,0.182,0.0248,-0.0824,-0.118,0.0608,-0.0206,0.0112,-1.36e-05,-5.64e-05,4.34e-06,0.000753,-0.000522,-0.00109,0.204,0.00201,0.434,0,0,0,0,0,1.53e-06,6.71e-05,6.59e-05,4.36e-05,0.0347,0.0353,0.00626,0.0427,0.0428,0.0343,2.64e-11,2.64e-11,8.92e-11,2.68e-06,2.7e-06,5e-08,0,0,0,0,0,0,0,0