Mirrors_Framework/PX4-Autopilot
1
0
Fork 0
mirror of https://github.com/PX4/PX4-Autopilot.git synced 2026-05-25 08:36:08 +08:00
Code Issues Actions 7 Packages Projects Releases Wiki Activity

fix(simulation): use correlated gauss-markov noise for SIH-GPS (#26697)

Browse Source
This commit is contained in:
Marco Hauswirth
2026-03-09 20:39:29 +01:00
committed by GitHub
parent 1a0b7dae9d
commit 48b6ec84bd
2 changed files with 39 additions and 4 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/simulation/sensor_gps_sim/SensorGpsSim.cpp
+23 -4
View File
@@ -116,11 +116,30 @@ void SensorGpsSim::Run()
vehicle_global_position_s gpos{};
_vehicle_global_position_sub.copy(&gpos);
double latitude = gpos.lat + math::degrees((double)generate_wgn() * 0.2 / CONSTANTS_RADIUS_OF_EARTH);
double longitude = gpos.lon + math::degrees((double)generate_wgn() * 0.2 / CONSTANTS_RADIUS_OF_EARTH);
double altitude = (double)(gpos.alt + (generate_wgn() * 0.5f));
// Correlated Markov process position noise (matching GZBridge model)
_gps_pos_noise_n = _pos_markov_time * _gps_pos_noise_n +
_pos_random_walk * generate_wgn() * _pos_noise_amplitude;
Vector3f gps_vel = Vector3f{lpos.vx, lpos.vy, lpos.vz} + noiseGauss3f(0.06f, 0.077f, 0.158f);
_gps_pos_noise_e = _pos_markov_time * _gps_pos_noise_e +
_pos_random_walk * generate_wgn() * _pos_noise_amplitude;
_gps_pos_noise_d = _pos_markov_time * _gps_pos_noise_d +
_pos_random_walk * generate_wgn() * _pos_noise_amplitude * 1.5f;
const double latitude = gpos.lat + math::degrees((double)_gps_pos_noise_n / CONSTANTS_RADIUS_OF_EARTH);
const double longitude = gpos.lon + math::degrees((double)_gps_pos_noise_e / CONSTANTS_RADIUS_OF_EARTH);
const double altitude = (double)(gpos.alt + _gps_pos_noise_d);
_gps_vel_noise_n = _vel_markov_time * _gps_vel_noise_n +
_vel_noise_density * generate_wgn() * _vel_noise_amplitude;
_gps_vel_noise_e = _vel_markov_time * _gps_vel_noise_e +
_vel_noise_density * generate_wgn() * _vel_noise_amplitude;
_gps_vel_noise_d = _vel_markov_time * _gps_vel_noise_d +
_vel_noise_density * generate_wgn() * _vel_noise_amplitude * 1.2f;
const Vector3f gps_vel = Vector3f{lpos.vx + _gps_vel_noise_n, lpos.vy + _gps_vel_noise_e, lpos.vz + _gps_vel_noise_d};
// device id
device::Device::DeviceId device_id;
src/modules/simulation/sensor_gps_sim/SensorGpsSim.hpp
+16
View File
@@ -84,6 +84,22 @@ private:
perf_counter_t _loop_perf{perf_alloc(PC_ELAPSED, MODULE_NAME": cycle")};
// GPS Markov process noise state
float _gps_pos_noise_n{0.0f};
float _gps_pos_noise_e{0.0f};
float _gps_pos_noise_d{0.0f};
float _gps_vel_noise_n{0.0f};
float _gps_vel_noise_e{0.0f};
float _gps_vel_noise_d{0.0f};
// Gauss-Markov noise parameters, rate-corrected from GZBridge (30 Hz) to SIH (8 Hz)
static constexpr float _pos_noise_amplitude{0.8f};
static constexpr float _pos_random_walk{0.02f};
static constexpr float _pos_markov_time{0.76f};
static constexpr float _vel_noise_amplitude{0.05f};
static constexpr float _vel_noise_density{0.4f};
static constexpr float _vel_markov_time{0.54f};
DEFINE_PARAMETERS(
(ParamInt<px4::params::SIM_GPS_USED>) _sim_gps_used
)