Move baro downsampling and dynamic pressure comp to ECL

EKF/common.h

@@ -342,6 +342,15 @@ struct parameters {
 
 	int32_t valid_timeout_max{5000000};	///< amount of time spent inertial dead reckoning before the estimator reports the state estimates as invalid (uSec)
 
+	// static barometer pressure position error coefficient along body axes
+	float static_pressure_coef_xp {0.0f};
+	float static_pressure_coef_xn {0.0f};
+	float static_pressure_coef_yp {0.0f};
+	float static_pressure_coef_yn {0.0f};
+	float static_pressure_coef_z {0.0f};
+	// upper limit on airspeed used for correction  (m/s**2)
+	float max_correction_airspeed {20.0f};
+
 	// multi-rotor drag specific force fusion
 	float drag_noise{2.5f};			///< observation noise variance for drag specific force measurements (m/sec**2)**2
 	float bcoef_x{25.0f};			///< ballistic coefficient along the X-axis (kg/m**2)
@@ -371,7 +380,7 @@ struct stateSample {
 	Vector3f    delta_vel_bias;	///< delta velocity bias estimate in m/s
 	Vector3f    mag_I;	///< NED earth magnetic field in gauss
 	Vector3f    mag_B;	///< magnetometer bias estimate in body frame in gauss
-	Vector2f    wind_vel;	///< wind velocity in m/s
+	Vector2f    wind_vel;	///< horizontal wind velocity in earth frame in m/s
 };
 
 union fault_status_u {

EKF/ekf.h

@@ -635,6 +635,8 @@ private:
 	// and a scalar innovation value
 	void fuse(float *K, float innovation);
 
+	float compensateBaroForDynamicPressure(float baro_alt_uncompensated) override;
+
 	// calculate the earth rotation vector from a given latitude
 	Vector3f calcEarthRateNED(float lat_rad) const;
 

EKF/ekf_helper.cpp

@@ -787,6 +787,34 @@ void Ekf::constrainStates()
 	}
 }
 
+float Ekf::compensateBaroForDynamicPressure(const float baro_alt_uncompensated)
+{
+	// calculate static pressure error = Pmeas - Ptruth
+	// model position error sensitivity as a body fixed ellipse with a different scale in the positive and
+	// negative X and Y directions. Used to correct baro data for positional errors
+	const matrix::Dcmf R_to_body(_output_new.quat_nominal.inversed());
+
+	// Calculate airspeed in body frame
+	const Vector3f velocity_earth = _output_new.vel - _vel_imu_rel_body_ned;
+
+	const Vector3f wind_velocity_earth(_state.wind_vel(0), _state.wind_vel(1), 0.0f);
+
+	const Vector3f airspeed_earth = velocity_earth - wind_velocity_earth;
+
+	const Vector3f airspeed_body = R_to_body * airspeed_earth;
+
+	const Vector3f K_pstatic_coef(airspeed_body(0) >= 0.0f ? _params.static_pressure_coef_xp : _params.static_pressure_coef_xn,
+				      airspeed_body(1) >= 0.0f ? _params.static_pressure_coef_yp : _params.static_pressure_coef_yn,
+				      _params.static_pressure_coef_z);
+
+	const Vector3f airspeed_squared = matrix::min(airspeed_body.emult(airspeed_body), sq(_params.max_correction_airspeed));
+
+	const float pstatic_err = 0.5f * _air_density * (airspeed_squared.dot(K_pstatic_coef));
+
+	// correct baro measurement using pressure error estimate and assuming sea level gravity
+	return baro_alt_uncompensated + pstatic_err / (_air_density * CONSTANTS_ONE_G);
+}
+
 // calculate the earth rotation vector
 Vector3f Ekf::calcEarthRateNED(float lat_rad) const
 {

EKF/estimator_interface.cpp

@@ -243,7 +243,7 @@ void EstimatorInterface::setGpsData(uint64_t time_usec, const gps_message &gps)
 	}
 }
 
-void EstimatorInterface::setBaroData(uint64_t time_usec, float data)
+void EstimatorInterface::setBaroData(uint64_t time_usec, float baro_alt_meter)
 {
 	if (!_initialised || _baro_buffer_fail) {
 		return;
@@ -260,19 +260,30 @@ void EstimatorInterface::setBaroData(uint64_t time_usec, float data)
 		}
 	}
 
+	// downsample to highest possible sensor rate
+	// by baro data by taking the average of incoming sample
+	_baro_sample_count++;
+	_baro_alt_sum += baro_alt_meter;
+
 	// limit data rate to prevent data being lost
 	if ((time_usec - _time_last_baro) > _min_obs_interval_us) {
 
+		const float baro_alt_avg = _baro_alt_sum / (float)_baro_sample_count;
+
 		baroSample baro_sample_new;
-		baro_sample_new.hgt = data;
-		baro_sample_new.time_us = time_usec - _params.baro_delay_ms * 1000;
+		baro_sample_new.hgt = compensateBaroForDynamicPressure(baro_alt_avg);
 
+		// Use the time in the middle of the downsampling interval for the sample
+		baro_sample_new.time_us = _time_last_baro + (time_usec - _time_last_baro) / 2;
+		baro_sample_new.time_us -= _params.baro_delay_ms * 1000;
 		baro_sample_new.time_us -= FILTER_UPDATE_PERIOD_MS * 1000 / 2;
-		_time_last_baro = time_usec;
-
 		baro_sample_new.time_us = math::max(baro_sample_new.time_us, _imu_sample_delayed.time_us);
 
 		_baro_buffer.push(baro_sample_new);
+
+		_time_last_baro = time_usec;
+		_baro_sample_count = 0;
+		_baro_alt_sum = 0.0f;
 	}
 }
 

EKF/estimator_interface.h

@@ -549,6 +549,10 @@ protected:
 	uint8_t _mag_sample_count {0};
 	uint64_t _mag_timestamp_sum {0};
 
+	// Used to down sample barometer data
+	float _baro_alt_sum {0.0f};			///< summed pressure altitude readings (m)
+	uint8_t _baro_sample_count {0};		///< number of barometric altitude measurements summed
+
 	fault_status_u _fault_status{};
 
 	// allocate data buffers and initialize interface variables
@@ -575,5 +579,7 @@ protected:
 	inline void computeVibrationMetric();
 	inline bool checkIfVehicleAtRest(float dt);
 
+	virtual float compensateBaroForDynamicPressure(const float baro_alt_uncompensated) = 0;
+
 	void printBufferAllocationFailed(const char * buffer_name);
 };