From d65d06f82dc91ecf2b948c10027f24e2caa788de Mon Sep 17 00:00:00 2001 From: Nicolas MARTIN Date: Wed, 3 Feb 2021 15:22:16 +0100 Subject: [PATCH] SIH/gps: express gps position noise in meters and reduce noise value Previous horizontal position noise was a white Gaussian noise with std=0.8m It results in a noise with high frequencies too high making some ekf position tests fail (test ratio to allow arming). The new noise values are below real GPS errors but as theses errors are generally low frequency, so they cannot be modeled with a white noise. --- src/modules/sih/sih.cpp | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/src/modules/sih/sih.cpp b/src/modules/sih/sih.cpp index 9e7e8a5943..987687c43c 100644 --- a/src/modules/sih/sih.cpp +++ b/src/modules/sih/sih.cpp @@ -333,9 +333,9 @@ void Sih::reconstruct_sensors_signals() _gps_lon_noiseless = _LON0 + degrees((double)_p_I(1) / CONSTANTS_RADIUS_OF_EARTH) / _COS_LAT0; _gps_alt_noiseless = _H0 - _p_I(2); - _gps_lat = _gps_lat_noiseless + (double)(generate_wgn() * 7.2e-6f); // latitude in degrees - _gps_lon = _gps_lon_noiseless + (double)(generate_wgn() * 1.75e-5f); // longitude in degrees - _gps_alt = _gps_alt_noiseless + generate_wgn() * 1.78f; + _gps_lat = _gps_lat_noiseless + degrees((double)generate_wgn() * 0.2 / CONSTANTS_RADIUS_OF_EARTH); + _gps_lon = _gps_lon_noiseless + degrees((double)generate_wgn() * 0.2 / CONSTANTS_RADIUS_OF_EARTH) / _COS_LAT0; + _gps_alt = _gps_alt_noiseless + generate_wgn() * 0.5f; _gps_vel = _v_I + noiseGauss3f(0.06f, 0.077f, 0.158f); }