From a50ef2eb5e40dce4f81e560f8612edcd3cf319e4 Mon Sep 17 00:00:00 2001 From: bresch Date: Tue, 25 Jun 2024 12:05:05 +0200 Subject: [PATCH] ekf2-terrain: make terrain validity based on uncertainty When using optical flow for terrain aiding, we cannot assume that the terrain estimate is valid if flow is fused as is can only be observed during motion. When no direct observation is available, the terrain is assumed to be valid if its 1sigma uncertainty is < 10% of the hagl. --- src/modules/ekf2/EKF/terrain_control.cpp | 15 ++--- .../test/change_indication/ekf_gsf_reset.csv | 62 +++++++++---------- .../ekf2/test/change_indication/iris_gps.csv | 28 ++++----- src/modules/ekf2/test/test_EKF_flow.cpp | 4 +- src/modules/ekf2/test/test_EKF_terrain.cpp | 4 +- 5 files changed, 54 insertions(+), 59 deletions(-) diff --git a/src/modules/ekf2/EKF/terrain_control.cpp b/src/modules/ekf2/EKF/terrain_control.cpp index 4de72ddd49..c21b7b35d8 100644 --- a/src/modules/ekf2/EKF/terrain_control.cpp +++ b/src/modules/ekf2/EKF/terrain_control.cpp @@ -36,7 +36,7 @@ */ #include "ekf.h" -#include "ekf_derivation/generated/state.h" +#include "ekf_derivation/generated/compute_hagl_innov_var.h" #include @@ -86,23 +86,18 @@ void Ekf::controlTerrainFakeFusion() bool Ekf::isTerrainEstimateValid() const { // Assume being valid when the uncertainty is small compared to the height above ground - bool valid = getTerrainVariance() < sq(0.1f * getHagl()); + float hagl_var = INFINITY; + sym::ComputeHaglInnovVar(P, 0.f, &hagl_var); + bool valid = hagl_var < fmaxf(sq(0.1f * getHagl()), 0.2f); #if defined(CONFIG_EKF2_RANGE_FINDER) + // Assume that the terrain estimate is always valid when direct observations are fused if (_hagl_sensor_status.flags.range_finder && isRecent(_aid_src_rng_hgt.time_last_fuse, (uint64_t)5e6)) { valid = true; } #endif // CONFIG_EKF2_RANGE_FINDER -#if defined(CONFIG_EKF2_OPTICAL_FLOW) - - if (_hagl_sensor_status.flags.flow && isRecent(_aid_src_optical_flow.time_last_fuse, (uint64_t)5e6)) { - valid = true; - } - -#endif // CONFIG_EKF2_OPTICAL_FLOW - return valid; } 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 07045ce2c9..2852048266 100644 --- a/src/modules/ekf2/test/change_indication/ekf_gsf_reset.csv +++ b/src/modules/ekf2/test/change_indication/ekf_gsf_reset.csv @@ -358,34 +358,34 @@ Timestamp,state[0],state[1],state[2],state[3],state[4],state[5],state[6],state[7 35590000,0.64,-0.0065,0.02,0.77,-3.3,-3.1,-0.13,-1e+06,1.2e+04,-3.7e+02,-0.0012,-0.0057,0.00014,0.065,0.004,-0.11,-0.2,-0.047,0.46,0.00042,-0.0016,-0.025,0,0,-3.7e+02,0.00013,9.3e-05,0.0011,0.15,0.2,0.011,0.65,0.72,0.05,2.9e-07,4.4e-07,1.8e-06,0.0047,0.0051,8.9e-05,1.7e-05,4.3e-06,0.00038,5.4e-06,5.8e-06,0.00037,1,1,0.01 35690000,0.64,-0.0065,0.02,0.77,-3.3,-3.1,-0.13,-1e+06,1.2e+04,-3.7e+02,-0.0012,-0.0057,0.00014,0.065,0.0038,-0.11,-0.2,-0.047,0.46,0.00047,-0.0016,-0.025,0,0,-3.7e+02,0.00013,9.2e-05,0.0011,0.16,0.22,0.011,0.69,0.77,0.05,2.9e-07,4.4e-07,1.9e-06,0.0047,0.0051,8.9e-05,1.7e-05,4.3e-06,0.00038,5.3e-06,5.6e-06,0.00037,1,1,0.01 35790000,0.64,-0.0065,0.02,0.77,-3.4,-3.2,-0.12,-1e+06,1.2e+04,-3.7e+02,-0.0012,-0.0057,0.00014,0.065,0.0038,-0.11,-0.2,-0.047,0.46,0.00049,-0.0015,-0.025,0,0,-3.7e+02,0.00013,9.2e-05,0.0011,0.17,0.23,0.01,0.74,0.84,0.049,2.9e-07,4.4e-07,1.9e-06,0.0047,0.0051,8.9e-05,1.7e-05,4.3e-06,0.00038,5.1e-06,5.3e-06,0.00037,1,1,0.025 -35890000,0.64,-0.0066,0.02,0.77,-3.4,-3.3,-0.11,-1e+06,1.2e+04,-3.7e+02,-0.0012,-0.0057,0.00013,0.065,0.0039,-0.11,-0.2,-0.047,0.46,0.00049,-0.0015,-0.025,0,0,-3.7e+02,0.00013,9.2e-05,0.0011,0.18,0.25,0.0096,0.79,0.9,0.048,2.9e-07,4.4e-07,1.9e-06,0.0047,0.005,8.9e-05,1.7e-05,4.3e-06,0.00038,5e-06,5.2e-06,0.00037,1,1,0.056 -35990000,0.64,-0.0066,0.02,0.77,-3.4,-3.3,-0.1,-1e+06,1.2e+04,-3.7e+02,-0.0012,-0.0057,0.00013,0.065,0.0038,-0.11,-0.2,-0.047,0.46,0.00047,-0.0016,-0.025,0,0,-3.7e+02,0.00013,9.1e-05,0.0011,0.2,0.26,0.0093,0.84,0.98,0.047,2.9e-07,4.4e-07,1.9e-06,0.0047,0.005,8.9e-05,1.6e-05,4.3e-06,0.00038,4.9e-06,5e-06,0.00037,1,1,0.086 -36090000,0.64,-0.0066,0.02,0.77,-3.4,-3.4,-0.093,-1e+06,1.2e+04,-3.7e+02,-0.0012,-0.0057,0.00013,0.065,0.0038,-0.11,-0.2,-0.047,0.46,0.00049,-0.0016,-0.025,0,0,-3.7e+02,0.00013,9.1e-05,0.0011,0.21,0.28,0.0089,0.91,1.1,0.046,2.9e-07,4.4e-07,1.9e-06,0.0047,0.005,8.9e-05,1.6e-05,4.2e-06,0.00038,4.8e-06,4.8e-06,0.00037,1,1,0.12 -36190000,0.64,-0.0067,0.02,0.77,-3.5,-3.5,-0.083,-1e+06,1.2e+04,-3.7e+02,-0.0012,-0.0057,0.00013,0.065,0.004,-0.11,-0.2,-0.047,0.46,0.00057,-0.0016,-0.025,0,0,-3.7e+02,0.00013,9.1e-05,0.0011,0.22,0.3,0.0086,0.97,1.1,0.045,2.9e-07,4.4e-07,1.9e-06,0.0047,0.005,8.9e-05,1.6e-05,4.2e-06,0.00038,4.7e-06,4.7e-06,0.00037,1,1,0.15 -36290000,0.64,-0.0067,0.02,0.77,-3.5,-3.6,-0.073,-1e+06,1.2e+04,-3.7e+02,-0.0012,-0.0057,0.00012,0.066,0.0038,-0.11,-0.2,-0.047,0.46,0.00058,-0.0016,-0.025,0,0,-3.7e+02,0.00013,9e-05,0.0011,0.24,0.31,0.0083,1,1.2,0.045,2.9e-07,4.4e-07,1.9e-06,0.0047,0.005,8.9e-05,1.6e-05,4.2e-06,0.00038,4.6e-06,4.6e-06,0.00037,1,1,0.18 -36390000,0.64,-0.0067,0.02,0.77,-3.5,-3.6,-0.066,-1e+06,1.2e+04,-3.7e+02,-0.0012,-0.0057,0.00012,0.066,0.0039,-0.11,-0.2,-0.047,0.46,0.00058,-0.0015,-0.025,0,0,-3.7e+02,0.00012,9e-05,0.0011,0.25,0.33,0.0081,1.1,1.4,0.044,2.9e-07,4.4e-07,1.9e-06,0.0047,0.005,8.9e-05,1.6e-05,4.2e-06,0.00038,4.5e-06,4.4e-06,0.00037,1,1,0.21 -36490000,0.64,-0.0068,0.02,0.77,-3.6,-3.7,-0.058,-1e+06,1.2e+04,-3.7e+02,-0.0012,-0.0057,0.00013,0.066,0.0038,-0.11,-0.2,-0.047,0.46,0.00055,-0.0015,-0.025,0,0,-3.7e+02,0.00012,9e-05,0.0011,0.27,0.35,0.0078,1.2,1.5,0.043,2.9e-07,4.4e-07,1.9e-06,0.0047,0.005,9e-05,1.6e-05,4.2e-06,0.00038,4.4e-06,4.3e-06,0.00037,1,1,0.24 -36590000,0.64,-0.0068,0.02,0.77,-3.6,-3.8,-0.048,-1e+06,1.2e+04,-3.7e+02,-0.0012,-0.0057,0.00012,0.066,0.0038,-0.11,-0.2,-0.047,0.46,0.00058,-0.0015,-0.025,0,0,-3.7e+02,0.00012,8.9e-05,0.0011,0.28,0.37,0.0076,1.3,1.6,0.042,2.9e-07,4.4e-07,1.9e-06,0.0047,0.005,9e-05,1.6e-05,4.2e-06,0.00038,4.4e-06,4.2e-06,0.00037,1,1,0.27 -36690000,0.64,-0.0068,0.02,0.77,-3.6,-3.9,-0.04,-1e+06,1.2e+04,-3.7e+02,-0.0012,-0.0057,0.00012,0.066,0.0041,-0.11,-0.2,-0.047,0.46,0.00061,-0.0015,-0.025,0,0,-3.7e+02,0.00012,8.9e-05,0.0011,0.3,0.39,0.0075,1.4,1.7,0.042,3e-07,4.4e-07,1.9e-06,0.0047,0.005,9e-05,1.5e-05,4.2e-06,0.00038,4.3e-06,4.1e-06,0.00037,1,1,0.31 -36790000,0.64,-0.0069,0.02,0.77,-3.6,-3.9,-0.031,-1e+06,1.2e+04,-3.7e+02,-0.0012,-0.0057,0.00011,0.066,0.0039,-0.11,-0.2,-0.047,0.46,0.00064,-0.0015,-0.025,0,0,-3.7e+02,0.00012,8.9e-05,0.0011,0.31,0.41,0.0073,1.5,1.9,0.041,3e-07,4.4e-07,1.9e-06,0.0047,0.005,9e-05,1.5e-05,4.2e-06,0.00038,4.3e-06,4e-06,0.00037,1,1,0.34 -36890000,0.64,-0.0069,0.021,0.77,-3.7,-4,-0.023,-1e+06,1.2e+04,-3.7e+02,-0.0012,-0.0057,0.00011,0.067,0.0041,-0.11,-0.2,-0.047,0.46,0.00067,-0.0015,-0.025,0,0,-3.7e+02,0.00012,8.9e-05,0.0011,0.33,0.43,0.0072,1.6,2,0.04,3e-07,4.4e-07,1.9e-06,0.0047,0.005,9e-05,1.5e-05,4.2e-06,0.00038,4.2e-06,3.9e-06,0.00037,1,1,0.37 -36990000,0.64,-0.0069,0.021,0.77,-3.7,-4.1,-0.015,-1e+06,1.2e+04,-3.7e+02,-0.0012,-0.0057,0.0001,0.067,0.0042,-0.11,-0.2,-0.047,0.46,0.00069,-0.0015,-0.025,0,0,-3.7e+02,0.00012,8.8e-05,0.0011,0.35,0.45,0.0071,1.8,2.2,0.04,3e-07,4.4e-07,1.9e-06,0.0047,0.005,9e-05,1.5e-05,4.2e-06,0.00038,4.2e-06,3.9e-06,0.00037,1,1,0.4 -37090000,0.64,-0.0069,0.021,0.77,-3.7,-4.2,-0.0064,-1e+06,1.2e+04,-3.7e+02,-0.0012,-0.0057,0.0001,0.067,0.0043,-0.11,-0.2,-0.047,0.46,0.00069,-0.0014,-0.025,0,0,-3.7e+02,0.00012,8.8e-05,0.0011,0.37,0.47,0.007,1.9,2.4,0.04,3e-07,4.4e-07,1.8e-06,0.0047,0.005,9e-05,1.5e-05,4.2e-06,0.00038,4.1e-06,3.8e-06,0.00037,1,1,0.44 -37190000,0.64,-0.0069,0.021,0.77,-3.8,-4.2,0.0016,-1e+06,1.2e+04,-3.7e+02,-0.0012,-0.0057,0.0001,0.067,0.0044,-0.11,-0.2,-0.047,0.46,0.00068,-0.0014,-0.025,0,0,-3.7e+02,0.00012,8.8e-05,0.0011,0.38,0.49,0.0069,2,2.6,0.039,3e-07,4.4e-07,1.8e-06,0.0047,0.005,9e-05,1.5e-05,4.2e-06,0.00038,4.1e-06,3.7e-06,0.00037,1,1,0.47 -37290000,0.64,-0.0069,0.021,0.77,-3.8,-4.3,0.0094,-1e+06,1.2e+04,-3.7e+02,-0.0012,-0.0057,9.7e-05,0.067,0.0044,-0.11,-0.2,-0.047,0.46,0.00069,-0.0014,-0.025,0,0,-3.7e+02,0.00012,8.8e-05,0.0011,0.4,0.51,0.0068,2.2,2.8,0.039,3e-07,4.4e-07,1.8e-06,0.0047,0.005,9e-05,1.5e-05,4.2e-06,0.00038,4.1e-06,3.7e-06,0.00037,1,1,0.5 -37390000,0.64,-0.0069,0.021,0.77,-3.8,-4.4,0.017,-1e+06,1.2e+04,-3.7e+02,-0.0012,-0.0057,9.3e-05,0.067,0.0043,-0.11,-0.2,-0.047,0.46,0.00071,-0.0014,-0.025,0,0,-3.7e+02,0.00012,8.7e-05,0.0011,0.42,0.53,0.0068,2.4,3,0.038,3e-07,4.4e-07,1.8e-06,0.0047,0.005,9e-05,1.5e-05,4.2e-06,0.00038,4e-06,3.6e-06,0.00037,1,1,0.53 -37490000,0.64,-0.0069,0.021,0.77,-3.8,-4.4,0.024,-1e+06,1.2e+04,-3.7e+02,-0.0012,-0.0057,8.8e-05,0.067,0.0044,-0.11,-0.2,-0.047,0.46,0.00074,-0.0014,-0.025,0,0,-3.7e+02,0.00012,8.7e-05,0.0011,0.44,0.56,0.0067,2.5,3.2,0.038,3e-07,4.4e-07,1.8e-06,0.0047,0.005,9e-05,1.5e-05,4.2e-06,0.00038,4e-06,3.5e-06,0.00037,1,1,0.57 -37590000,0.64,-0.0069,0.021,0.77,-3.9,-4.5,0.033,-1e+06,1.2e+04,-3.7e+02,-0.0012,-0.0057,8.4e-05,0.067,0.0045,-0.11,-0.2,-0.047,0.46,0.00075,-0.0014,-0.025,0,0,-3.7e+02,0.00012,8.7e-05,0.001,0.46,0.58,0.0067,2.7,3.5,0.038,3e-07,4.4e-07,1.8e-06,0.0047,0.005,9e-05,1.5e-05,4.2e-06,0.00038,4e-06,3.5e-06,0.00037,1,1,0.6 -37690000,0.64,-0.007,0.021,0.77,-3.9,-4.6,0.043,-1e+06,1.2e+04,-3.7e+02,-0.0012,-0.0056,7.8e-05,0.068,0.0045,-0.11,-0.2,-0.047,0.46,0.00076,-0.0014,-0.025,0,0,-3.7e+02,0.00012,8.7e-05,0.001,0.48,0.6,0.0066,2.9,3.7,0.037,3e-07,4.4e-07,1.8e-06,0.0047,0.005,9e-05,1.5e-05,4.2e-06,0.00038,3.9e-06,3.4e-06,0.00037,1,1,0.64 -37790000,0.64,-0.007,0.021,0.77,-3.9,-4.7,0.051,-1e+06,1.2e+04,-3.7e+02,-0.0012,-0.0056,7.6e-05,0.068,0.0044,-0.11,-0.2,-0.047,0.46,0.00076,-0.0014,-0.025,0,0,-3.7e+02,0.00012,8.7e-05,0.001,0.5,0.63,0.0066,3.1,4,0.037,3e-07,4.4e-07,1.8e-06,0.0047,0.005,9.1e-05,1.4e-05,4.2e-06,0.00038,3.9e-06,3.4e-06,0.00037,1,1,0.67 -37890000,0.64,-0.007,0.021,0.77,-3.9,-4.7,0.059,-1e+06,1.2e+04,-3.7e+02,-0.0012,-0.0056,7.4e-05,0.068,0.0046,-0.11,-0.2,-0.047,0.46,0.00077,-0.0014,-0.025,0,0,-3.7e+02,0.00011,8.6e-05,0.001,0.52,0.65,0.0065,3.4,4.3,0.036,3e-07,4.4e-07,1.8e-06,0.0047,0.0049,9.1e-05,1.4e-05,4.2e-06,0.00038,3.9e-06,3.3e-06,0.00037,1,1,0.7 -37990000,0.64,-0.0071,0.021,0.77,-4,-4.8,0.068,-1e+06,1.2e+04,-3.7e+02,-0.0012,-0.0056,7.4e-05,0.068,0.0044,-0.11,-0.2,-0.047,0.46,0.00077,-0.0014,-0.025,0,0,-3.7e+02,0.00011,8.6e-05,0.001,0.54,0.67,0.0066,3.6,4.6,0.036,3.1e-07,4.4e-07,1.8e-06,0.0046,0.0049,9.1e-05,1.4e-05,4.2e-06,0.00038,3.9e-06,3.3e-06,0.00037,1,1,0.74 -38090000,0.64,-0.0071,0.021,0.77,-4,-4.9,0.079,-1e+06,1.2e+04,-3.7e+02,-0.0012,-0.0056,6.9e-05,0.068,0.0044,-0.11,-0.2,-0.047,0.46,0.00078,-0.0014,-0.025,0,0,-3.7e+02,0.00011,8.6e-05,0.001,0.57,0.7,0.0065,3.9,4.9,0.036,3.1e-07,4.4e-07,1.8e-06,0.0046,0.0049,9.1e-05,1.4e-05,4.2e-06,0.00038,3.9e-06,3.2e-06,0.00037,1,1,0.77 -38190000,0.64,-0.0071,0.021,0.77,-4,-5,0.086,-1e+06,1.2e+04,-3.7e+02,-0.0012,-0.0056,6.7e-05,0.068,0.0044,-0.11,-0.2,-0.047,0.46,0.00079,-0.0014,-0.025,0,0,-3.7e+02,0.00011,8.6e-05,0.001,0.59,0.72,0.0065,4.1,5.3,0.036,3.1e-07,4.4e-07,1.8e-06,0.0046,0.0049,9.1e-05,1.4e-05,4.2e-06,0.00038,3.8e-06,3.2e-06,0.00037,1,1,0.81 -38290000,0.64,-0.0071,0.021,0.77,-4.1,-5,0.094,-1e+06,1.2e+04,-3.7e+02,-0.0012,-0.0056,6.7e-05,0.068,0.0044,-0.11,-0.2,-0.047,0.46,0.00079,-0.0014,-0.025,0,0,-3.7e+02,0.00011,8.6e-05,0.001,0.61,0.75,0.0065,4.4,5.6,0.036,3.1e-07,4.4e-07,1.8e-06,0.0046,0.0049,9.1e-05,1.4e-05,4.2e-06,0.00038,3.8e-06,3.1e-06,0.00037,1,1,0.84 -38390000,0.64,-0.007,0.021,0.77,-4.1,-5.1,0.1,-1e+06,1.2e+04,-3.7e+02,-0.0012,-0.0056,6.9e-05,0.068,0.0042,-0.11,-0.2,-0.047,0.46,0.00078,-0.0013,-0.025,0,0,-3.7e+02,0.00011,8.6e-05,0.001,0.64,0.77,0.0065,4.7,6,0.035,3.1e-07,4.4e-07,1.8e-06,0.0046,0.0049,9.1e-05,1.4e-05,4.1e-06,0.00038,3.8e-06,3.1e-06,0.00037,1,1,0.88 -38490000,0.64,-0.007,0.021,0.77,-4.1,-5.2,0.11,-1e+06,1.2e+04,-3.7e+02,-0.0012,-0.0056,6.7e-05,0.068,0.0043,-0.11,-0.2,-0.047,0.46,0.00079,-0.0013,-0.025,0,0,-3.7e+02,0.00011,8.5e-05,0.001,0.66,0.8,0.0065,5.1,6.4,0.035,3.1e-07,4.4e-07,1.8e-06,0.0046,0.0049,9.1e-05,1.4e-05,4.1e-06,0.00038,3.8e-06,3e-06,0.00037,1,1,0.92 -38590000,0.64,-0.007,0.021,0.77,-4.1,-5.2,0.11,-1e+06,1.2e+04,-3.7e+02,-0.0012,-0.0056,6.8e-05,0.068,0.0045,-0.11,-0.2,-0.047,0.46,0.00079,-0.0013,-0.025,0,0,-3.7e+02,0.00011,8.5e-05,0.001,0.68,0.82,0.0066,5.4,6.8,0.035,3.1e-07,4.4e-07,1.8e-06,0.0046,0.0049,9.1e-05,1.4e-05,4.1e-06,0.00038,3.8e-06,3e-06,0.00037,1,1,0.95 -38690000,0.64,-0.007,0.021,0.77,-4.2,-5.3,0.12,-1e+06,1.2e+04,-3.7e+02,-0.0012,-0.0056,6.4e-05,0.067,0.0047,-0.11,-0.2,-0.047,0.46,0.0008,-0.0013,-0.025,0,0,-3.7e+02,0.00011,8.5e-05,0.001,0.71,0.85,0.0065,5.8,7.3,0.035,3.1e-07,4.4e-07,1.8e-06,0.0046,0.0049,9.1e-05,1.4e-05,4.1e-06,0.00038,3.8e-06,3e-06,0.00037,1,1,0.99 -38790000,0.64,-0.007,0.021,0.77,-4.2,-5.4,0.13,-1e+06,1.2e+04,-3.7e+02,-0.0012,-0.0056,6.3e-05,0.067,0.0046,-0.11,-0.2,-0.047,0.46,0.0008,-0.0013,-0.025,0,0,-3.7e+02,0.00011,8.5e-05,0.001,0.73,0.88,0.0066,6.1,7.7,0.035,3.1e-07,4.3e-07,1.8e-06,0.0046,0.0049,9.1e-05,1.4e-05,4.1e-06,0.00038,3.8e-06,2.9e-06,0.00037,1,1,1 -38890000,0.64,-0.0071,0.021,0.77,-4.2,-5.4,0.63,-1e+06,1.2e+04,-3.7e+02,-0.0012,-0.0056,6.3e-05,0.067,0.0047,-0.11,-0.2,-0.047,0.46,0.0008,-0.0013,-0.025,0,0,-3.7e+02,0.00011,8.5e-05,0.001,0.75,0.89,0.0066,6.5,8.2,0.035,3.1e-07,4.3e-07,1.8e-06,0.0046,0.0049,9.1e-05,1.4e-05,4.1e-06,0.00038,3.8e-06,2.9e-06,0.00037,1,1,1.1 +35890000,0.64,-0.0066,0.02,0.77,-3.4,-3.3,-0.12,-1e+06,1.2e+04,-3.7e+02,-0.0012,-0.0057,0.00013,0.065,0.0039,-0.11,-0.2,-0.047,0.46,0.00049,-0.0015,-0.025,0,0,-3.7e+02,0.00013,9.2e-05,0.0011,0.18,0.25,0.0096,0.79,0.9,0.048,2.9e-07,4.4e-07,1.9e-06,0.0047,0.005,8.9e-05,1.7e-05,4.3e-06,0.00038,5e-06,5.2e-06,0.00037,1,1,0.056 +35990000,0.64,-0.0066,0.02,0.77,-3.4,-3.3,-0.11,-1e+06,1.2e+04,-3.7e+02,-0.0012,-0.0057,0.00014,0.065,0.0038,-0.11,-0.2,-0.047,0.46,0.00047,-0.0015,-0.025,0,0,-3.7e+02,0.00013,9.1e-05,0.0011,0.2,0.26,0.0093,0.84,0.98,0.047,2.9e-07,4.4e-07,1.9e-06,0.0047,0.005,8.9e-05,1.6e-05,4.3e-06,0.00038,4.9e-06,5e-06,0.00037,1,1,0.086 +36090000,0.64,-0.0066,0.02,0.77,-3.4,-3.4,-0.11,-1e+06,1.2e+04,-3.7e+02,-0.0012,-0.0057,0.00013,0.065,0.004,-0.11,-0.2,-0.047,0.46,0.00049,-0.0016,-0.025,0,0,-3.7e+02,0.00013,9.1e-05,0.0011,0.21,0.28,0.0089,0.91,1.1,0.046,2.9e-07,4.4e-07,1.9e-06,0.0047,0.005,8.9e-05,1.6e-05,4.2e-06,0.00038,4.8e-06,4.8e-06,0.00037,1,1,0.12 +36190000,0.64,-0.0067,0.02,0.77,-3.5,-3.5,-0.1,-1e+06,1.2e+04,-3.7e+02,-0.0012,-0.0057,0.00013,0.064,0.0042,-0.11,-0.2,-0.047,0.46,0.00057,-0.0016,-0.025,0,0,-3.7e+02,0.00013,9.1e-05,0.0011,0.22,0.3,0.0086,0.97,1.1,0.045,2.9e-07,4.4e-07,1.9e-06,0.0047,0.005,8.9e-05,1.6e-05,4.2e-06,0.00038,4.7e-06,4.7e-06,0.00037,1,1,0.15 +36290000,0.64,-0.0066,0.02,0.77,-3.5,-3.6,-0.09,-1e+06,1.2e+04,-3.7e+02,-0.0012,-0.0057,0.00013,0.065,0.0039,-0.11,-0.2,-0.047,0.46,0.00058,-0.0016,-0.025,0,0,-3.7e+02,0.00013,9e-05,0.0011,0.24,0.31,0.0083,1,1.2,0.045,2.9e-07,4.4e-07,1.9e-06,0.0047,0.005,8.9e-05,1.6e-05,4.2e-06,0.00038,4.6e-06,4.6e-06,0.00037,1,1,0.18 +36390000,0.64,-0.0067,0.02,0.77,-3.5,-3.6,-0.082,-1e+06,1.2e+04,-3.7e+02,-0.0012,-0.0057,0.00013,0.065,0.004,-0.11,-0.2,-0.047,0.46,0.00058,-0.0015,-0.025,0,0,-3.7e+02,0.00012,9e-05,0.0011,0.25,0.33,0.0081,1.1,1.4,0.044,2.9e-07,4.4e-07,1.9e-06,0.0047,0.005,8.9e-05,1.6e-05,4.2e-06,0.00038,4.5e-06,4.4e-06,0.00037,1,1,0.21 +36490000,0.64,-0.0068,0.02,0.77,-3.6,-3.7,-0.073,-1e+06,1.2e+04,-3.7e+02,-0.0012,-0.0057,0.00013,0.065,0.0039,-0.11,-0.2,-0.047,0.46,0.00055,-0.0015,-0.025,0,0,-3.7e+02,0.00012,9e-05,0.0011,0.27,0.35,0.0078,1.2,1.5,0.043,2.9e-07,4.4e-07,1.9e-06,0.0047,0.005,9e-05,1.6e-05,4.2e-06,0.00038,4.4e-06,4.3e-06,0.00037,1,1,0.24 +36590000,0.64,-0.0068,0.02,0.77,-3.6,-3.8,-0.061,-1e+06,1.2e+04,-3.7e+02,-0.0012,-0.0057,0.00012,0.066,0.0039,-0.11,-0.2,-0.047,0.46,0.00058,-0.0015,-0.025,0,0,-3.7e+02,0.00012,8.9e-05,0.0011,0.28,0.37,0.0076,1.3,1.6,0.042,2.9e-07,4.4e-07,1.9e-06,0.0047,0.005,9e-05,1.6e-05,4.2e-06,0.00038,4.4e-06,4.2e-06,0.00037,1,1,0.27 +36690000,0.64,-0.0068,0.02,0.77,-3.6,-3.9,-0.052,-1e+06,1.2e+04,-3.7e+02,-0.0012,-0.0057,0.00012,0.066,0.0041,-0.11,-0.2,-0.047,0.46,0.00061,-0.0014,-0.025,0,0,-3.7e+02,0.00012,8.9e-05,0.0011,0.3,0.39,0.0075,1.4,1.7,0.042,3e-07,4.4e-07,1.9e-06,0.0047,0.005,9e-05,1.5e-05,4.2e-06,0.00038,4.3e-06,4.1e-06,0.00037,1,1,0.31 +36790000,0.64,-0.0068,0.02,0.77,-3.6,-3.9,-0.041,-1e+06,1.2e+04,-3.7e+02,-0.0012,-0.0057,0.00011,0.066,0.0039,-0.11,-0.2,-0.047,0.46,0.00064,-0.0015,-0.025,0,0,-3.7e+02,0.00012,8.9e-05,0.0011,0.31,0.41,0.0073,1.5,1.9,0.041,3e-07,4.4e-07,1.9e-06,0.0047,0.005,9e-05,1.5e-05,4.2e-06,0.00038,4.3e-06,4e-06,0.00037,1,1,0.34 +36890000,0.64,-0.0069,0.02,0.77,-3.7,-4,-0.033,-1e+06,1.2e+04,-3.7e+02,-0.0012,-0.0057,0.00011,0.066,0.0041,-0.11,-0.2,-0.047,0.46,0.00067,-0.0015,-0.025,0,0,-3.7e+02,0.00012,8.9e-05,0.0011,0.33,0.43,0.0072,1.6,2,0.04,3e-07,4.4e-07,1.9e-06,0.0047,0.005,9e-05,1.5e-05,4.2e-06,0.00038,4.2e-06,3.9e-06,0.00037,1,1,0.37 +36990000,0.64,-0.0069,0.021,0.77,-3.7,-4.1,-0.024,-1e+06,1.2e+04,-3.7e+02,-0.0012,-0.0057,0.0001,0.067,0.0042,-0.11,-0.2,-0.047,0.46,0.00069,-0.0015,-0.025,0,0,-3.7e+02,0.00012,8.8e-05,0.0011,0.35,0.45,0.0071,1.8,2.2,0.04,3e-07,4.4e-07,1.9e-06,0.0047,0.005,9e-05,1.5e-05,4.2e-06,0.00038,4.2e-06,3.9e-06,0.00037,1,1,0.4 +37090000,0.64,-0.0069,0.021,0.77,-3.7,-4.2,-0.014,-1e+06,1.2e+04,-3.7e+02,-0.0012,-0.0057,0.0001,0.067,0.0043,-0.11,-0.2,-0.047,0.46,0.00069,-0.0014,-0.025,0,0,-3.7e+02,0.00012,8.8e-05,0.0011,0.37,0.47,0.007,1.9,2.4,0.04,3e-07,4.4e-07,1.8e-06,0.0047,0.005,9e-05,1.5e-05,4.2e-06,0.00038,4.1e-06,3.8e-06,0.00037,1,1,0.43 +37190000,0.64,-0.0069,0.021,0.77,-3.8,-4.2,-0.0055,-1e+06,1.2e+04,-3.7e+02,-0.0012,-0.0057,0.0001,0.067,0.0043,-0.11,-0.2,-0.047,0.46,0.00068,-0.0014,-0.025,0,0,-3.7e+02,0.00012,8.8e-05,0.0011,0.38,0.49,0.0069,2,2.6,0.039,3e-07,4.4e-07,1.8e-06,0.0047,0.005,9e-05,1.5e-05,4.2e-06,0.00038,4.1e-06,3.7e-06,0.00037,1,1,0.47 +37290000,0.64,-0.0069,0.021,0.77,-3.8,-4.3,0.003,-1e+06,1.2e+04,-3.7e+02,-0.0012,-0.0057,9.7e-05,0.067,0.0043,-0.11,-0.2,-0.047,0.46,0.00069,-0.0014,-0.025,0,0,-3.7e+02,0.00012,8.8e-05,0.0011,0.4,0.51,0.0068,2.2,2.8,0.039,3e-07,4.4e-07,1.8e-06,0.0047,0.005,9e-05,1.5e-05,4.2e-06,0.00038,4.1e-06,3.7e-06,0.00037,1,1,0.5 +37390000,0.64,-0.0069,0.021,0.77,-3.8,-4.4,0.011,-1e+06,1.2e+04,-3.7e+02,-0.0012,-0.0057,9.3e-05,0.068,0.0042,-0.11,-0.2,-0.047,0.46,0.00071,-0.0014,-0.025,0,0,-3.7e+02,0.00012,8.7e-05,0.0011,0.42,0.53,0.0068,2.4,3,0.038,3e-07,4.4e-07,1.8e-06,0.0047,0.005,9e-05,1.5e-05,4.2e-06,0.00038,4e-06,3.6e-06,0.00037,1,1,0.53 +37490000,0.64,-0.0069,0.021,0.77,-3.8,-4.4,0.019,-1e+06,1.2e+04,-3.7e+02,-0.0012,-0.0057,8.7e-05,0.068,0.0042,-0.11,-0.2,-0.047,0.46,0.00074,-0.0014,-0.025,0,0,-3.7e+02,0.00012,8.7e-05,0.0011,0.44,0.56,0.0067,2.5,3.2,0.038,3e-07,4.4e-07,1.8e-06,0.0047,0.005,9e-05,1.5e-05,4.2e-06,0.00038,4e-06,3.5e-06,0.00037,1,1,0.57 +37590000,0.64,-0.0069,0.021,0.77,-3.9,-4.5,0.029,-1e+06,1.2e+04,-3.7e+02,-0.0012,-0.0056,8.3e-05,0.068,0.0043,-0.11,-0.2,-0.047,0.46,0.00074,-0.0014,-0.025,0,0,-3.7e+02,0.00012,8.7e-05,0.001,0.46,0.58,0.0067,2.7,3.5,0.038,3e-07,4.4e-07,1.8e-06,0.0047,0.005,9e-05,1.5e-05,4.2e-06,0.00038,4e-06,3.5e-06,0.00037,1,1,0.6 +37690000,0.64,-0.007,0.021,0.77,-3.9,-4.6,0.039,-1e+06,1.2e+04,-3.7e+02,-0.0012,-0.0056,7.7e-05,0.068,0.0043,-0.11,-0.2,-0.047,0.46,0.00076,-0.0014,-0.025,0,0,-3.7e+02,0.00012,8.7e-05,0.001,0.48,0.6,0.0066,2.9,3.7,0.037,3e-07,4.4e-07,1.8e-06,0.0047,0.005,9e-05,1.5e-05,4.2e-06,0.00038,3.9e-06,3.4e-06,0.00037,1,1,0.64 +37790000,0.64,-0.007,0.021,0.77,-3.9,-4.7,0.048,-1e+06,1.2e+04,-3.7e+02,-0.0012,-0.0056,7.5e-05,0.069,0.0041,-0.11,-0.2,-0.047,0.46,0.00076,-0.0014,-0.025,0,0,-3.7e+02,0.00012,8.7e-05,0.001,0.5,0.63,0.0066,3.1,4,0.037,3e-07,4.4e-07,1.8e-06,0.0047,0.005,9.1e-05,1.4e-05,4.2e-06,0.00038,3.9e-06,3.4e-06,0.00037,1,1,0.67 +37890000,0.64,-0.0071,0.021,0.77,-3.9,-4.8,0.056,-1e+06,1.2e+04,-3.7e+02,-0.0012,-0.0056,7.2e-05,0.069,0.0043,-0.11,-0.2,-0.047,0.46,0.00076,-0.0014,-0.025,0,0,-3.7e+02,0.00011,8.6e-05,0.001,0.52,0.65,0.0065,3.4,4.3,0.036,3e-07,4.4e-07,1.8e-06,0.0047,0.0049,9.1e-05,1.4e-05,4.2e-06,0.00038,3.9e-06,3.3e-06,0.00037,1,1,0.7 +37990000,0.64,-0.0071,0.021,0.77,-4,-4.8,0.066,-1e+06,1.2e+04,-3.7e+02,-0.0012,-0.0056,7.2e-05,0.069,0.0041,-0.11,-0.2,-0.047,0.46,0.00076,-0.0014,-0.025,0,0,-3.7e+02,0.00011,8.6e-05,0.001,0.54,0.67,0.0066,3.6,4.6,0.036,3.1e-07,4.4e-07,1.8e-06,0.0046,0.0049,9.1e-05,1.4e-05,4.2e-06,0.00038,3.9e-06,3.3e-06,0.00037,1,1,0.74 +38090000,0.64,-0.0071,0.021,0.77,-4,-4.9,0.077,-1e+06,1.2e+04,-3.7e+02,-0.0012,-0.0056,6.7e-05,0.069,0.004,-0.11,-0.2,-0.047,0.46,0.00078,-0.0014,-0.025,0,0,-3.7e+02,0.00011,8.6e-05,0.001,0.57,0.7,0.0065,3.9,4.9,0.036,3.1e-07,4.4e-07,1.8e-06,0.0046,0.0049,9.1e-05,1.4e-05,4.2e-06,0.00038,3.9e-06,3.2e-06,0.00037,1,1,0.77 +38190000,0.64,-0.0071,0.021,0.77,-4,-5,0.085,-1e+06,1.2e+04,-3.7e+02,-0.0012,-0.0056,6.5e-05,0.069,0.004,-0.11,-0.2,-0.047,0.46,0.00078,-0.0014,-0.025,0,0,-3.7e+02,0.00011,8.6e-05,0.001,0.59,0.72,0.0065,4.1,5.3,0.036,3.1e-07,4.4e-07,1.8e-06,0.0046,0.0049,9.1e-05,1.4e-05,4.2e-06,0.00038,3.8e-06,3.2e-06,0.00037,1,1,0.81 +38290000,0.64,-0.0071,0.021,0.77,-4.1,-5,0.093,-1e+06,1.2e+04,-3.7e+02,-0.0012,-0.0056,6.4e-05,0.069,0.004,-0.11,-0.2,-0.047,0.46,0.00078,-0.0014,-0.025,0,0,-3.7e+02,0.00011,8.6e-05,0.001,0.61,0.75,0.0065,4.4,5.6,0.036,3.1e-07,4.4e-07,1.8e-06,0.0046,0.0049,9.1e-05,1.4e-05,4.2e-06,0.00038,3.8e-06,3.1e-06,0.00037,1,1,0.84 +38390000,0.64,-0.0071,0.021,0.77,-4.1,-5.1,0.1,-1e+06,1.2e+04,-3.7e+02,-0.0012,-0.0056,6.6e-05,0.069,0.0038,-0.11,-0.2,-0.047,0.46,0.00078,-0.0013,-0.025,0,0,-3.7e+02,0.00011,8.6e-05,0.001,0.64,0.77,0.0065,4.7,6,0.035,3.1e-07,4.4e-07,1.8e-06,0.0046,0.0049,9.1e-05,1.4e-05,4.1e-06,0.00038,3.8e-06,3.1e-06,0.00037,1,1,0.88 +38490000,0.64,-0.0071,0.021,0.77,-4.1,-5.2,0.11,-1e+06,1.2e+04,-3.7e+02,-0.0012,-0.0056,6.4e-05,0.069,0.0039,-0.11,-0.2,-0.047,0.46,0.00078,-0.0013,-0.025,0,0,-3.7e+02,0.00011,8.5e-05,0.001,0.66,0.8,0.0065,5.1,6.4,0.035,3.1e-07,4.4e-07,1.8e-06,0.0046,0.0049,9.1e-05,1.4e-05,4.1e-06,0.00038,3.8e-06,3e-06,0.00037,1,1,0.92 +38590000,0.64,-0.007,0.021,0.77,-4.1,-5.3,0.11,-1e+06,1.2e+04,-3.7e+02,-0.0012,-0.0056,6.5e-05,0.069,0.0041,-0.11,-0.2,-0.047,0.46,0.00078,-0.0013,-0.025,0,0,-3.7e+02,0.00011,8.5e-05,0.001,0.68,0.82,0.0066,5.4,6.8,0.035,3.1e-07,4.4e-07,1.8e-06,0.0046,0.0049,9.1e-05,1.4e-05,4.1e-06,0.00038,3.8e-06,3e-06,0.00037,1,1,0.95 +38690000,0.64,-0.007,0.021,0.77,-4.2,-5.3,0.12,-1e+06,1.2e+04,-3.7e+02,-0.0012,-0.0056,6.1e-05,0.069,0.0043,-0.11,-0.2,-0.047,0.46,0.00079,-0.0013,-0.025,0,0,-3.7e+02,0.00011,8.5e-05,0.001,0.71,0.85,0.0066,5.8,7.3,0.035,3.1e-07,4.4e-07,1.8e-06,0.0046,0.0049,9.1e-05,1.4e-05,4.1e-06,0.00038,3.8e-06,3e-06,0.00037,1,1,0.99 +38790000,0.64,-0.007,0.021,0.77,-4.2,-5.4,0.13,-1e+06,1.2e+04,-3.7e+02,-0.0012,-0.0056,6e-05,0.068,0.0041,-0.11,-0.2,-0.047,0.46,0.0008,-0.0013,-0.025,0,0,-3.7e+02,0.00011,8.5e-05,0.001,0.73,0.88,0.0066,6.1,7.7,0.035,3.1e-07,4.3e-07,1.8e-06,0.0046,0.0049,9.1e-05,1.4e-05,4.1e-06,0.00038,3.8e-06,2.9e-06,0.00037,1,1,1 +38890000,0.64,-0.0071,0.021,0.77,-4.2,-5.4,0.63,-1e+06,1.2e+04,-3.7e+02,-0.0012,-0.0056,5.9e-05,0.069,0.0042,-0.11,-0.2,-0.047,0.46,0.0008,-0.0013,-0.025,0,0,-3.7e+02,0.00011,8.5e-05,0.001,0.75,0.89,0.0066,6.5,8.2,0.035,3.1e-07,4.3e-07,1.8e-06,0.0046,0.0049,9.1e-05,1.4e-05,4.1e-06,0.00038,3.8e-06,2.9e-06,0.00037,1,1,1.1 diff --git a/src/modules/ekf2/test/change_indication/iris_gps.csv b/src/modules/ekf2/test/change_indication/iris_gps.csv index af5d55ea15..efd262eaf4 100644 --- a/src/modules/ekf2/test/change_indication/iris_gps.csv +++ b/src/modules/ekf2/test/change_indication/iris_gps.csv @@ -333,19 +333,19 @@ Timestamp,state[0],state[1],state[2],state[3],state[4],state[5],state[6],state[7 33090000,-0.29,0.015,-0.006,0.96,0.0014,0.097,-0.081,0.092,0.0086,0.036,-0.0013,-0.0058,-9.8e-06,0.066,0.012,-0.12,-0.11,-0.025,0.5,0.084,-0.032,-0.067,0,0,0.14,4.2e-05,3.7e-05,0.047,0.022,0.023,0.0051,0.31,0.32,0.03,2.6e-07,2.4e-07,1.5e-06,0.0018,0.002,8e-05,0.0011,6.4e-05,0.0012,0.0011,0.00063,0.0012,1,1,0.01 33190000,-0.29,0.015,-0.0059,0.96,0.0056,0.093,-0.08,0.093,-0.006,0.028,-0.0013,-0.0058,-2.9e-05,0.066,0.012,-0.12,-0.11,-0.025,0.5,0.084,-0.032,-0.067,0,0,0.13,4.1e-05,3.6e-05,0.047,0.021,0.022,0.0051,0.31,0.31,0.03,2.6e-07,2.4e-07,1.5e-06,0.0018,0.002,8e-05,0.0011,6.4e-05,0.0012,0.0011,0.00063,0.0012,1,1,0.01 33290000,-0.29,0.015,-0.0059,0.96,0.0098,0.096,-0.08,0.094,0.0027,0.02,-0.0013,-0.0058,-1.8e-05,0.066,0.012,-0.12,-0.11,-0.025,0.5,0.084,-0.032,-0.067,0,0,0.12,4.2e-05,3.7e-05,0.047,0.021,0.023,0.0051,0.32,0.33,0.03,2.6e-07,2.4e-07,1.5e-06,0.0018,0.002,7.9e-05,0.0011,6.4e-05,0.0012,0.0011,0.00063,0.0012,1,1,0.01 -33390000,-0.3,0.015,-0.0059,0.96,0.014,0.093,-0.078,0.094,-0.0055,0.011,-0.0013,-0.0058,-2.6e-05,0.066,0.013,-0.12,-0.11,-0.025,0.5,0.084,-0.032,-0.067,0,0,0.12,4.1e-05,3.6e-05,0.047,0.021,0.022,0.0051,0.32,0.32,0.03,2.6e-07,2.4e-07,1.4e-06,0.0018,0.002,7.9e-05,0.0011,6.4e-05,0.0012,0.0011,0.00063,0.0012,1,1,0.033 -33490000,-0.3,0.015,-0.0059,0.96,0.019,0.097,-0.077,0.097,0.0039,0.0017,-0.0013,-0.0058,-2.1e-05,0.066,0.013,-0.12,-0.11,-0.025,0.5,0.084,-0.032,-0.067,0,0,0.12,4.2e-05,3.6e-05,0.047,0.021,0.023,0.0051,0.33,0.34,0.03,2.6e-07,2.4e-07,1.4e-06,0.0018,0.002,7.9e-05,0.0011,6.4e-05,0.0012,0.0011,0.00063,0.0012,1,1,0.058 -33590000,-0.3,0.015,-0.0057,0.95,0.023,0.095,-0.074,0.096,-0.009,-0.0062,-0.0013,-0.0058,-2.7e-05,0.066,0.013,-0.12,-0.11,-0.025,0.5,0.084,-0.032,-0.067,0,0,0.12,4.1e-05,3.6e-05,0.047,0.021,0.022,0.005,0.33,0.33,0.03,2.6e-07,2.4e-07,1.4e-06,0.0018,0.002,7.8e-05,0.0011,6.4e-05,0.0012,0.0011,0.00063,0.0012,1,1,0.083 -33690000,-0.3,0.015,-0.0057,0.95,0.026,0.098,-0.075,0.097,-3.1e-05,-0.014,-0.0013,-0.0058,-2.1e-05,0.066,0.013,-0.12,-0.11,-0.025,0.5,0.084,-0.032,-0.067,0,0,0.12,4.1e-05,3.6e-05,0.047,0.021,0.023,0.0051,0.34,0.35,0.03,2.6e-07,2.4e-07,1.4e-06,0.0018,0.002,7.8e-05,0.0011,6.4e-05,0.0012,0.0011,0.00063,0.0012,1,1,0.11 -33790000,-0.3,0.015,-0.0056,0.95,0.028,0.096,-0.069,0.094,-0.013,-0.021,-0.0013,-0.0058,-3.5e-05,0.066,0.013,-0.12,-0.11,-0.025,0.5,0.084,-0.032,-0.067,0,0,0.12,4.1e-05,3.6e-05,0.047,0.021,0.022,0.005,0.34,0.34,0.03,2.6e-07,2.4e-07,1.4e-06,0.0018,0.002,7.8e-05,0.0011,6.4e-05,0.0012,0.0011,0.00063,0.0012,1,1,0.13 -33890000,-0.3,0.015,-0.0056,0.95,0.033,0.098,-0.069,0.097,-0.0041,-0.028,-0.0013,-0.0058,-2.5e-05,0.066,0.013,-0.12,-0.11,-0.025,0.5,0.084,-0.032,-0.067,0,0,0.12,4.1e-05,3.6e-05,0.047,0.021,0.023,0.0051,0.35,0.36,0.03,2.6e-07,2.4e-07,1.4e-06,0.0018,0.002,7.8e-05,0.0011,6.4e-05,0.0012,0.0011,0.00063,0.0012,1,1,0.16 -33990000,-0.3,0.015,-0.0055,0.95,0.035,0.096,-0.065,0.096,-0.012,-0.032,-0.0013,-0.0058,-3.6e-05,0.066,0.013,-0.12,-0.11,-0.025,0.5,0.084,-0.032,-0.067,0,0,0.12,4.1e-05,3.6e-05,0.047,0.02,0.022,0.005,0.35,0.35,0.03,2.5e-07,2.4e-07,1.4e-06,0.0018,0.002,7.7e-05,0.0011,6.4e-05,0.0012,0.0011,0.00063,0.0012,1,1,0.18 -34090000,-0.3,0.015,-0.0055,0.95,0.038,0.1,-0.064,0.099,-0.0028,-0.036,-0.0013,-0.0057,-3.2e-05,0.066,0.013,-0.12,-0.11,-0.025,0.5,0.084,-0.032,-0.067,0,0,0.12,4.1e-05,3.6e-05,0.047,0.021,0.023,0.0051,0.36,0.37,0.03,2.6e-07,2.4e-07,1.4e-06,0.0018,0.002,7.7e-05,0.0011,6.4e-05,0.0012,0.0011,0.00063,0.0012,1,1,0.21 -34190000,-0.3,0.015,-0.0054,0.95,0.039,0.098,-0.061,0.094,-0.014,-0.039,-0.0013,-0.0057,-3.7e-05,0.066,0.014,-0.12,-0.11,-0.025,0.5,0.084,-0.032,-0.067,0,0,0.12,4.1e-05,3.6e-05,0.047,0.02,0.022,0.005,0.36,0.36,0.03,2.5e-07,2.4e-07,1.4e-06,0.0018,0.002,7.7e-05,0.0011,6.4e-05,0.0012,0.0011,0.00062,0.0012,1,1,0.23 -34290000,-0.3,0.015,-0.0053,0.95,0.04,0.1,-0.06,0.098,-0.0048,-0.045,-0.0013,-0.0057,-3e-05,0.066,0.014,-0.12,-0.11,-0.025,0.5,0.084,-0.032,-0.067,0,0,0.12,4.1e-05,3.6e-05,0.047,0.021,0.023,0.005,0.37,0.37,0.03,2.5e-07,2.4e-07,1.4e-06,0.0018,0.002,7.6e-05,0.0011,6.4e-05,0.0012,0.0011,0.00062,0.0012,1,1,0.26 -34390000,-0.3,0.015,-0.0052,0.95,0.042,0.097,-0.055,0.093,-0.016,-0.049,-0.0013,-0.0057,-3.9e-05,0.066,0.014,-0.12,-0.11,-0.025,0.5,0.084,-0.032,-0.067,0,0,0.12,4.1e-05,3.6e-05,0.047,0.02,0.022,0.005,0.37,0.37,0.03,2.5e-07,2.3e-07,1.4e-06,0.0018,0.002,7.6e-05,0.0011,6.4e-05,0.0012,0.0011,0.00062,0.0012,1,1,0.28 -34490000,-0.3,0.015,-0.0052,0.95,0.045,0.1,-0.053,0.097,-0.0069,-0.052,-0.0013,-0.0057,-3e-05,0.066,0.014,-0.12,-0.11,-0.025,0.5,0.085,-0.032,-0.067,0,0,0.12,4.1e-05,3.6e-05,0.047,0.021,0.022,0.005,0.38,0.38,0.03,2.5e-07,2.3e-07,1.4e-06,0.0018,0.002,7.6e-05,0.0011,6.4e-05,0.0012,0.0011,0.00062,0.0012,1,1,0.31 -34590000,-0.3,0.014,-0.0051,0.95,0.045,0.098,0.74,0.092,-0.021,-0.024,-0.0013,-0.0057,-4e-05,0.066,0.014,-0.12,-0.11,-0.025,0.5,0.085,-0.031,-0.067,0,0,0.12,4.1e-05,3.6e-05,0.046,0.02,0.021,0.005,0.38,0.38,0.03,2.5e-07,2.3e-07,1.4e-06,0.0018,0.002,7.5e-05,0.0011,6.4e-05,0.0012,0.0011,0.00062,0.0012,1,1,0.33 -34690000,-0.3,0.014,-0.0051,0.95,0.05,0.1,1.7,0.097,-0.011,0.095,-0.0013,-0.0057,-3.7e-05,0.066,0.014,-0.12,-0.11,-0.025,0.5,0.085,-0.031,-0.067,0,0,0.12,4.1e-05,3.6e-05,0.046,0.02,0.021,0.005,0.39,0.39,0.03,2.5e-07,2.3e-07,1.4e-06,0.0018,0.002,7.5e-05,0.0011,6.4e-05,0.0012,0.0011,0.00062,0.0012,1,1,0.36 +33390000,-0.3,0.015,-0.0059,0.96,0.014,0.093,-0.078,0.094,-0.0055,0.01,-0.0013,-0.0058,-2.6e-05,0.066,0.013,-0.12,-0.11,-0.025,0.5,0.084,-0.032,-0.067,0,0,0.12,4.1e-05,3.6e-05,0.047,0.021,0.022,0.0051,0.32,0.32,0.03,2.6e-07,2.4e-07,1.4e-06,0.0018,0.002,7.9e-05,0.0011,6.4e-05,0.0012,0.0011,0.00063,0.0012,1,1,0.033 +33490000,-0.3,0.015,-0.0059,0.96,0.019,0.097,-0.078,0.097,0.0039,-0.00071,-0.0013,-0.0058,-2.1e-05,0.066,0.013,-0.12,-0.11,-0.025,0.5,0.084,-0.032,-0.067,0,0,0.12,4.2e-05,3.6e-05,0.047,0.021,0.023,0.0051,0.33,0.34,0.03,2.6e-07,2.4e-07,1.4e-06,0.0018,0.002,7.9e-05,0.0011,6.4e-05,0.0012,0.0011,0.00063,0.0012,1,1,0.058 +33590000,-0.3,0.015,-0.0057,0.95,0.023,0.095,-0.075,0.096,-0.009,-0.01,-0.0013,-0.0058,-2.7e-05,0.066,0.013,-0.12,-0.11,-0.025,0.5,0.084,-0.032,-0.067,0,0,0.12,4.1e-05,3.6e-05,0.047,0.021,0.022,0.005,0.33,0.33,0.03,2.6e-07,2.4e-07,1.4e-06,0.0018,0.002,7.8e-05,0.0011,6.4e-05,0.0012,0.0011,0.00063,0.0012,1,1,0.083 +33690000,-0.3,0.015,-0.0057,0.95,0.026,0.098,-0.076,0.097,-3.5e-05,-0.019,-0.0013,-0.0058,-2.1e-05,0.066,0.013,-0.12,-0.11,-0.025,0.5,0.084,-0.032,-0.067,0,0,0.12,4.1e-05,3.6e-05,0.047,0.021,0.023,0.0051,0.34,0.35,0.03,2.6e-07,2.4e-07,1.4e-06,0.0018,0.002,7.8e-05,0.0011,6.4e-05,0.0012,0.0011,0.00063,0.0012,1,1,0.11 +33790000,-0.3,0.015,-0.0056,0.95,0.028,0.096,-0.071,0.094,-0.013,-0.028,-0.0013,-0.0058,-3.5e-05,0.066,0.013,-0.12,-0.11,-0.025,0.5,0.084,-0.032,-0.067,0,0,0.12,4.1e-05,3.6e-05,0.047,0.021,0.022,0.005,0.34,0.34,0.03,2.6e-07,2.4e-07,1.4e-06,0.0018,0.002,7.8e-05,0.0011,6.4e-05,0.0012,0.0011,0.00063,0.0012,1,1,0.13 +33890000,-0.3,0.015,-0.0056,0.95,0.033,0.098,-0.071,0.097,-0.0041,-0.037,-0.0013,-0.0058,-2.5e-05,0.066,0.013,-0.12,-0.11,-0.025,0.5,0.084,-0.032,-0.067,0,0,0.12,4.1e-05,3.6e-05,0.047,0.021,0.023,0.0051,0.35,0.36,0.03,2.6e-07,2.4e-07,1.4e-06,0.0018,0.002,7.8e-05,0.0011,6.4e-05,0.0012,0.0011,0.00063,0.0012,1,1,0.16 +33990000,-0.3,0.015,-0.0055,0.95,0.035,0.096,-0.068,0.096,-0.012,-0.042,-0.0013,-0.0058,-3.6e-05,0.066,0.013,-0.12,-0.11,-0.025,0.5,0.084,-0.032,-0.067,0,0,0.12,4.1e-05,3.6e-05,0.047,0.02,0.022,0.005,0.35,0.35,0.03,2.5e-07,2.4e-07,1.4e-06,0.0018,0.002,7.7e-05,0.0011,6.4e-05,0.0012,0.0011,0.00063,0.0012,1,1,0.18 +34090000,-0.3,0.015,-0.0055,0.95,0.038,0.1,-0.066,0.099,-0.0028,-0.046,-0.0013,-0.0057,-3.2e-05,0.066,0.013,-0.12,-0.11,-0.025,0.5,0.084,-0.032,-0.067,0,0,0.12,4.1e-05,3.6e-05,0.047,0.021,0.023,0.0051,0.36,0.37,0.03,2.6e-07,2.4e-07,1.4e-06,0.0018,0.002,7.7e-05,0.0011,6.4e-05,0.0012,0.0011,0.00063,0.0012,1,1,0.21 +34190000,-0.3,0.015,-0.0054,0.95,0.039,0.098,-0.064,0.094,-0.015,-0.05,-0.0013,-0.0057,-3.7e-05,0.066,0.014,-0.12,-0.11,-0.025,0.5,0.084,-0.032,-0.067,0,0,0.12,4.1e-05,3.6e-05,0.047,0.02,0.022,0.005,0.36,0.36,0.03,2.5e-07,2.4e-07,1.4e-06,0.0018,0.002,7.7e-05,0.0011,6.4e-05,0.0012,0.0011,0.00062,0.0012,1,1,0.23 +34290000,-0.3,0.015,-0.0053,0.95,0.04,0.1,-0.062,0.098,-0.0048,-0.055,-0.0013,-0.0057,-3e-05,0.066,0.014,-0.12,-0.11,-0.025,0.5,0.084,-0.032,-0.067,0,0,0.12,4.1e-05,3.6e-05,0.047,0.021,0.023,0.005,0.37,0.37,0.03,2.5e-07,2.4e-07,1.4e-06,0.0018,0.002,7.6e-05,0.0011,6.4e-05,0.0012,0.0011,0.00062,0.0012,1,1,0.26 +34390000,-0.3,0.015,-0.0052,0.95,0.042,0.097,-0.057,0.093,-0.016,-0.059,-0.0013,-0.0057,-3.9e-05,0.066,0.014,-0.12,-0.11,-0.025,0.5,0.084,-0.032,-0.067,0,0,0.12,4.1e-05,3.6e-05,0.047,0.02,0.022,0.005,0.37,0.37,0.03,2.5e-07,2.3e-07,1.4e-06,0.0018,0.002,7.6e-05,0.0011,6.4e-05,0.0012,0.0011,0.00062,0.0012,1,1,0.28 +34490000,-0.3,0.015,-0.0052,0.95,0.045,0.1,-0.055,0.097,-0.0069,-0.061,-0.0013,-0.0057,-3e-05,0.066,0.014,-0.12,-0.11,-0.025,0.5,0.085,-0.032,-0.067,0,0,0.12,4.1e-05,3.6e-05,0.047,0.021,0.022,0.005,0.38,0.38,0.03,2.5e-07,2.3e-07,1.4e-06,0.0018,0.002,7.6e-05,0.0011,6.4e-05,0.0012,0.0011,0.00062,0.0012,1,1,0.31 +34590000,-0.3,0.014,-0.0051,0.95,0.045,0.098,0.74,0.092,-0.021,-0.033,-0.0013,-0.0057,-4e-05,0.066,0.014,-0.12,-0.11,-0.025,0.5,0.085,-0.031,-0.067,0,0,0.12,4.1e-05,3.6e-05,0.046,0.02,0.021,0.005,0.38,0.38,0.03,2.5e-07,2.3e-07,1.4e-06,0.0018,0.002,7.5e-05,0.0011,6.4e-05,0.0012,0.0011,0.00062,0.0012,1,1,0.33 +34690000,-0.3,0.014,-0.0051,0.95,0.05,0.1,1.7,0.097,-0.011,0.087,-0.0013,-0.0057,-3.7e-05,0.066,0.014,-0.12,-0.11,-0.025,0.5,0.085,-0.031,-0.067,0,0,0.12,4.1e-05,3.6e-05,0.046,0.02,0.021,0.005,0.39,0.39,0.03,2.5e-07,2.3e-07,1.4e-06,0.0018,0.002,7.5e-05,0.0011,6.4e-05,0.0012,0.0011,0.00062,0.0012,1,1,0.36 34790000,-0.3,0.014,-0.005,0.95,0.051,0.099,2.7,0.091,-0.025,0.27,-0.0013,-0.0057,-4.7e-05,0.066,0.014,-0.12,-0.11,-0.025,0.5,0.085,-0.031,-0.067,0,0,0.12,4.1e-05,3.6e-05,0.046,0.019,0.019,0.005,0.39,0.39,0.03,2.5e-07,2.3e-07,1.4e-06,0.0018,0.002,7.5e-05,0.0011,6.3e-05,0.0012,0.0011,0.00062,0.0012,1,1,0.38 34890000,-0.3,0.014,-0.0051,0.95,0.056,0.1,3.7,0.096,-0.015,0.56,-0.0013,-0.0057,-4.3e-05,0.066,0.014,-0.12,-0.11,-0.025,0.5,0.085,-0.031,-0.067,0,0,0.12,4.1e-05,3.6e-05,0.046,0.019,0.019,0.005,0.4,0.4,0.03,2.5e-07,2.3e-07,1.4e-06,0.0018,0.002,7.5e-05,0.0011,6.3e-05,0.0012,0.0011,0.00062,0.0012,1,1,0.41 diff --git a/src/modules/ekf2/test/test_EKF_flow.cpp b/src/modules/ekf2/test/test_EKF_flow.cpp index 42f27028a0..787560ce1b 100644 --- a/src/modules/ekf2/test/test_EKF_flow.cpp +++ b/src/modules/ekf2/test/test_EKF_flow.cpp @@ -178,9 +178,9 @@ TEST_F(EkfFlowTest, resetToFlowVelocityOnGround) EXPECT_TRUE(isEqual(estimated_horz_velocity, Vector2f(0.f, 0.f))) << estimated_horz_velocity(0) << ", " << estimated_horz_velocity(1); - // AND: the velocity isn't reset as it is already correct + // AND: the horizontal velocity is reset to the flow value reset_logging_checker.capturePostResetState(); - EXPECT_TRUE(reset_logging_checker.isHorizontalVelocityResetCounterIncreasedBy(0)); + EXPECT_TRUE(reset_logging_checker.isHorizontalVelocityResetCounterIncreasedBy(1)); EXPECT_TRUE(reset_logging_checker.isVerticalVelocityResetCounterIncreasedBy(0)); } diff --git a/src/modules/ekf2/test/test_EKF_terrain.cpp b/src/modules/ekf2/test/test_EKF_terrain.cpp index 964cd43822..c3c2ad6714 100644 --- a/src/modules/ekf2/test/test_EKF_terrain.cpp +++ b/src/modules/ekf2/test/test_EKF_terrain.cpp @@ -154,7 +154,7 @@ TEST_F(EkfTerrainTest, testFlowForTerrainFusion) // WHEN: the sensors do not agree const float rng_height = 1.f; - const float flow_height = 5.f; + const float flow_height = 8.f; runFlowAndRngScenario(rng_height, flow_height); // THEN: the estimator should use flow for terrain and the estimated terrain height @@ -175,7 +175,7 @@ TEST_F(EkfTerrainTest, testRngForTerrainFusion) // WHEN: the sensors do not agree const float rng_height = 1.f; - const float flow_height = 5.f; + const float flow_height = 8.f; runFlowAndRngScenario(rng_height, flow_height); // THEN: the estimator should use rng for terrain and the estimated terrain height