ekf2: in bias estimator, use psd instead of var for prediction

PSD is independent from the sampling time while variance isn't

src/modules/ekf2/EKF/bias_estimator.cpp

@@ -43,7 +43,7 @@ void BiasEstimator::predict(const float dt)
 {
 	// State is constant
 	// Predict state covariance only
-	float delta_state_var = _process_var * dt * dt;
+	float delta_state_var = _process_psd * dt;
 
 	if (isOffsetDetected()) {
 		// A bias in the state has been detected by the innovation sequence check

src/modules/ekf2/EKF/bias_estimator.hpp

@@ -80,9 +80,9 @@ public:
 	virtual void fuseBias(float measurement, float measurement_var);
 
 	void setBias(float bias) { _state = bias; }
-	void setProcessNoiseStdDev(float process_noise)
+	void setProcessNoiseSpectralDensity(float nsd)
 	{
-		_process_var = process_noise * process_noise;
+		_process_psd = nsd * nsd;
 	}
 	void setBiasStdDev(float state_noise) { _state_var = state_noise * state_noise; }
 	void setInnovGate(float gate_size) { _gate_size = gate_size; }
@@ -99,7 +99,7 @@ private:
 
 	float _gate_size{3.f}; ///< Used for innovation filtering (innovation test ratio)
 	float _state_var{0.1f}; ///< Initial state uncertainty variance (m^2)
-	float _process_var{25.0e-6f}; ///< State process noise variance (m^2/s^2)
+	float _process_psd{1.25e-6f}; ///< State process power spectral density (m^2/s^2/Hz)
 	float _state_var_max{2.f}; ///< Used to constrain the state variance (m^2)
 
 	// Innovation sequence monitoring; used to detect a bias in the state

src/modules/ekf2/EKF/common.h

@@ -305,9 +305,10 @@ struct parameters {
 	// position and velocity fusion
 	float gps_vel_noise{5.0e-1f};           ///< minimum allowed observation noise for gps velocity fusion (m/sec)
 	float gps_pos_noise{0.5f};              ///< minimum allowed observation noise for gps position fusion (m)
+	float gps_hgt_bias_nsd{0.13f};          ///< process noise for gnss height bias estimation (m/s/sqrt(Hz))
 	float pos_noaid_noise{10.0f};           ///< observation noise for non-aiding position fusion (m)
 	float baro_noise{2.0f};                 ///< observation noise for barometric height fusion (m)
-	float baro_drift_rate{0.005f};          ///< process noise for barometric height bias estimation (m/s)
+	float baro_bias_nsd{0.13f};             ///< process noise for barometric height bias estimation (m/s/sqrt(Hz))
 	float baro_innov_gate{5.0f};            ///< barometric and GPS height innovation consistency gate size (STD)
 	float gps_pos_innov_gate{5.0f};         ///< GPS horizontal position innovation consistency gate size (STD)
 	float gps_vel_innov_gate{5.0f};         ///< GPS velocity innovation consistency gate size (STD)
@@ -342,6 +343,7 @@ struct parameters {
 	// range finder fusion
 	float range_noise{0.1f};                ///< observation noise for range finder measurements (m)
 	float range_innov_gate{5.0f};           ///< range finder fusion innovation consistency gate size (STD)
+	float rng_hgt_bias_nsd{0.13f};          ///< process noise for range height bias estimation (m/s/sqrt(Hz))
 	float rng_gnd_clearance{0.1f};          ///< minimum valid value for range when on ground (m)
 	float rng_sens_pitch{0.0f};             ///< Pitch offset of the range sensor (rad). Sensor points out along Z axis when offset is zero. Positive rotation is RH about Y axis.
 	float range_noise_scaler{0.0f};         ///< scaling from range measurement to noise (m/m)
@@ -356,6 +358,7 @@ struct parameters {
 	// vision position fusion
 	float ev_vel_innov_gate{3.0f};          ///< vision velocity fusion innovation consistency gate size (STD)
 	float ev_pos_innov_gate{5.0f};          ///< vision position fusion innovation consistency gate size (STD)
+	float ev_hgt_bias_nsd{0.13f};           ///< process noise for vision height bias estimation (m/s/sqrt(Hz))
 
 	// optical flow fusion
 	float flow_noise{0.15f};                ///< observation noise for optical flow LOS rate measurements (rad/sec)

src/modules/ekf2/EKF/gps_fusion.cpp

@@ -105,20 +105,12 @@ void Ekf::updateGpsPos(const gpsSample &gps_sample)
 	const float height_measurement = gps_sample.hgt - getEkfGlobalOriginAltitude();
 	const float height_measurement_var = getGpsHeightVariance();
 
-	// save current bias and update bias estimator
-	const float bias = _gps_hgt_b_est.getBias();
-	const float bias_var = _gps_hgt_b_est.getBiasVar();
-
-	_gps_hgt_b_est.setMaxStateNoise(height_measurement_var);
-	_gps_hgt_b_est.setProcessNoiseStdDev(height_measurement_var); //TODO: update this
-	_gps_hgt_b_est.fuseBias(height_measurement - (-_state.pos(2)), height_measurement_var + P(9, 9));
-
 	Vector3f position;
 	position(0) = gps_sample.pos(0);
 	position(1) = gps_sample.pos(1);
 
 	// vertical position - gps measurement has opposite sign to earth z axis
-	position(2) = -(height_measurement - bias);
+	position(2) = -(height_measurement - _gps_hgt_b_est.getBias());
 
 	const float lower_limit = fmaxf(_params.gps_pos_noise, 0.01f);
 
@@ -136,7 +128,7 @@ void Ekf::updateGpsPos(const gpsSample &gps_sample)
 		obs_var(0) = obs_var(1) = sq(math::constrain(gps_sample.hacc, lower_limit, upper_limit));
 	}
 
-	obs_var(2) = height_measurement_var + bias_var;
+	obs_var(2) = height_measurement_var + _gps_hgt_b_est.getBiasVar();
 
 	// innovation gate size
 	float innov_gate = fmaxf(_params.gps_pos_innov_gate, 1.f);
@@ -160,6 +152,12 @@ void Ekf::updateGpsPos(const gpsSample &gps_sample)
 	}
 
 	gps_pos.timestamp_sample = gps_sample.time_us;
+
+	// update the bias estimator before updating the main filter but after
+	// using its current state to compute the vertical position innovation
+	_gps_hgt_b_est.setMaxStateNoise(height_measurement_var);
+	_gps_hgt_b_est.setProcessNoiseSpectralDensity(_params.gps_hgt_bias_nsd);
+	_gps_hgt_b_est.fuseBias(height_measurement - (-_state.pos(2)), height_measurement_var + P(9, 9));
 }
 
 void Ekf::fuseGpsVel()

src/modules/ekf2/EKF/height_fusion.cpp

@@ -91,7 +91,7 @@ void Ekf::updateBaroHgt(const baroSample &baro_sample, estimator_aid_source_1d_s
 	// update the bias estimator before updating the main filter but after
 	// using its current state to compute the vertical position innovation
 	_baro_b_est.setMaxStateNoise(_params.baro_noise);
-	_baro_b_est.setProcessNoiseStdDev(_params.baro_drift_rate);
+	_baro_b_est.setProcessNoiseSpectralDensity(_params.baro_bias_nsd);
 	_baro_b_est.fuseBias(measurement - (-_state.pos(2)) , measurement_var + P(9, 9));
 }
 
@@ -143,7 +143,7 @@ void Ekf::updateRngHgt(estimator_aid_source_1d_s &rng_hgt)
 	// using its current state to compute the vertical position innovation
 	const float rng_noise = sqrtf(measurement_var);
 	_rng_hgt_b_est.setMaxStateNoise(rng_noise);
-	_rng_hgt_b_est.setProcessNoiseStdDev(rng_noise); // TODO: fix
+	_rng_hgt_b_est.setProcessNoiseSpectralDensity(_params.rng_hgt_bias_nsd);
 
 	_rng_hgt_b_est.fuseBias(measurement - (-_state.pos(2)) , measurement_var + P(9, 9));
 }
@@ -167,9 +167,8 @@ void Ekf::fuseEvHgt()
 	const float bias = _ev_hgt_b_est.getBias();
 	const float bias_var = _ev_hgt_b_est.getBiasVar();
 
-	const float ev_noise = sqrtf(measurement_var);
-	_ev_hgt_b_est.setMaxStateNoise(ev_noise);
-	_ev_hgt_b_est.setProcessNoiseStdDev(ev_noise); // TODO: fix
+	_ev_hgt_b_est.setMaxStateNoise(sqrtf(measurement_var));
+	_ev_hgt_b_est.setProcessNoiseSpectralDensity(_params.ev_hgt_bias_nsd);
 	_ev_hgt_b_est.fuseBias(measurement - _state.pos(2), measurement_var + P(9, 9));
 
 	// calculate the innovation assuming the external vision observation is in local NED frame