diff --git a/src/modules/simulation/gz_bridge/GZBridge.cpp b/src/modules/simulation/gz_bridge/GZBridge.cpp index 3ef65e186f..8a93b8b378 100644 --- a/src/modules/simulation/gz_bridge/GZBridge.cpp +++ b/src/modules/simulation/gz_bridge/GZBridge.cpp @@ -410,6 +410,43 @@ void GZBridge::airPressureCallback(const gz::msgs::FluidPressure &msg) { const uint64_t timestamp = hrt_absolute_time(); + // To simulate atmospheric pressure drift, we use a time-discretised + // Ornstein-Uhlenbeck process (= integral of gaussian white noise with a + // linear stabilising feedback term). This has the advantage that we + // retain the statistical properties (stationary distribution, mixing + // time) regardless of dt. + + // Ref: Gillespie, Daniel T.. (1996). Exact numerical simulation of the + // Ornstein-Uhlenbeck process and its integral. Physical Review E, + // 54(2), 2084–2091. doi:10.1103/PhysRevE.54.2084 + + const float dt = (float)(timestamp - _last_baro_pressure_drift_update) / 1_s; + const bool init = dt > 10; + + // Parameterisation used in Gillespie 1996, but instead of c we use + // sigma_stationary - equivalent with c = 2 * sigma_stationary^2 / tau + + // Relaxation time [sec] + const float tau = 3600.f * 24; + + // Standard deviation of stationary distribution [Pa] + // 1000 Pa corresponds to about 80m of altitude error + const float sigma_stationary = 200; + + const float mu = exp(-dt / tau); // (3.3) + const float sigma_update = sigma_stationary * sqrtf(1 - mu * mu); // (3.4a) + + if (init) { + // Sample from the stationary distribution to match long time behaviour + _baro_pressure_drift = sigma_stationary * generate_wgn(); + + } else { + // Update equation (3.5a) + _baro_pressure_drift = mu * _baro_pressure_drift + sigma_update * generate_wgn(); + } + + _last_baro_pressure_drift_update = timestamp; + device::Device::DeviceId id{}; id.devid_s.bus_type = device::Device::DeviceBusType::DeviceBusType_SIMULATION; id.devid_s.devtype = DRV_BARO_DEVTYPE_BAROSIM; @@ -420,7 +457,7 @@ void GZBridge::airPressureCallback(const gz::msgs::FluidPressure &msg) report.timestamp = timestamp; report.timestamp_sample = timestamp; report.device_id = id.devid; - report.pressure = msg.pressure(); + report.pressure = (float) msg.pressure() + _baro_pressure_drift; report.temperature = this->_temperature; _sensor_baro_pub.publish(report); } diff --git a/src/modules/simulation/gz_bridge/GZBridge.hpp b/src/modules/simulation/gz_bridge/GZBridge.hpp index 9f90f4dfea..8a58ae3566 100644 --- a/src/modules/simulation/gz_bridge/GZBridge.hpp +++ b/src/modules/simulation/gz_bridge/GZBridge.hpp @@ -193,6 +193,9 @@ private: const float _vel_noise_density = 0.2f; // Velocity noise process density const float _vel_markov_time = 0.85f; // Velocity Markov process coefficient + float _baro_pressure_drift{}; + hrt_abstime _last_baro_pressure_drift_update; + DEFINE_PARAMETERS( (ParamInt) _sim_gps_used, (ParamInt) _sim_gz_en_lidar,