ekf2: refactor fuse measurement helpers for consistency

- centralize measurement update methods in ekf.cpp
 - always check if variance calculation is badly conditioned

src/modules/ekf2/EKF/aid_sources/ZeroGyroUpdate.cpp

@@ -61,12 +61,12 @@ bool ZeroGyroUpdate::update(Ekf &ekf, const estimator::imuSample &imu_delayed)
 
 			Vector3f gyro_bias = _zgup_delta_ang / _zgup_delta_ang_dt;
 
-			const float obs_var = sq(math::constrain(ekf.getGyroNoise(), 0.f, 1.f));
+			const float R = sq(math::constrain(ekf.getGyroNoise(), 0.f, 1.f));
 
 			for (unsigned i = 0; i < 3; i++) {
 				const float innovation = ekf.state().gyro_bias(i) - gyro_bias(i);
-				const float innov_var = ekf.getGyroBiasVariance()(i) + obs_var;
-				ekf.fuseDirectStateMeasurement(innovation, innov_var, obs_var, State::gyro_bias.idx + i);
+				const float innov_var = ekf.getGyroBiasVariance()(i) + R;
+				ekf.fuseDirectStateMeasurement(State::gyro_bias.idx + i, R, innovation, innov_var);
 			}
 
 			// Reset the integrators

src/modules/ekf2/EKF/aid_sources/ZeroVelocityUpdate.cpp

@@ -61,12 +61,13 @@ bool ZeroVelocityUpdate::update(Ekf &ekf, const estimator::imuSample &imu_delaye
 
 			// Set a low variance initially for faster leveling and higher
 			// later to let the states follow the measurements
-			const float obs_var = ekf.control_status_flags().tilt_align ? sq(0.2f) : sq(0.001f);
-			Vector3f innov_var = ekf.getVelocityVariance() + obs_var;
+			const float R = ekf.control_status_flags().tilt_align ? sq(0.2f) : sq(0.001f);
 
-			for (unsigned i = 0; i < 3; i++) {
-				const float innovation = ekf.state().vel(i) - vel_obs(i);
-				ekf.fuseDirectStateMeasurement(innovation, innov_var(i), obs_var, State::vel.idx + i);
+			const Vector3f innov = ekf.state().vel - vel_obs;
+			const Vector3f innov_var = ekf.getVelocityVariance() + R;
+
+			for (unsigned i = 0; i <= 2; i++) {
+				ekf.fuseDirectStateMeasurement(State::vel.idx + i, R, innov(i), innov_var(i));
 			}
 
 			_time_last_fuse = imu_delayed.time_us;

src/modules/ekf2/EKF/aid_sources/airspeed/airspeed_fusion.cpp

@@ -210,7 +210,7 @@ void Ekf::fuseAirspeed(const airspeedSample &airspeed_sample, estimator_aid_sour
 		K.slice<State::wind_vel.dof, 1>(State::wind_vel.idx, 0) = K_wind;
 	}
 
-	const bool is_fused = measurementUpdate(K, H, aid_src.observation_variance, aid_src.innovation);
+	const bool is_fused = fuseMeasurement(K, H, aid_src.observation_variance, aid_src.innovation);
 
 	aid_src.fused = is_fused;
 	_fault_status.flags.bad_airspeed = !is_fused;

src/modules/ekf2/EKF/aid_sources/drag/drag_fusion.cpp

@@ -159,10 +159,7 @@ void Ekf::fuseDrag(const dragSample &drag_sample)
 		    && PX4_ISFINITE(innovation_variance(axis_index)) && PX4_ISFINITE(innovation(axis_index))
 		    && (test_ratio < 1.f)
 		   ) {
-
-			VectorState K = P * H / innovation_variance(axis_index);
-
-			if (measurementUpdate(K, H, R_ACC, innovation(axis_index))) {
+			if (fuseMeasurement(H, R_ACC, innovation(axis_index), innovation_variance(axis_index))) {
 				fused[axis_index] = true;
 			}
 		}

src/modules/ekf2/EKF/aid_sources/external_vision/ev_vel_control.cpp

@@ -278,8 +278,8 @@ bool Ekf::fuseEvVelocity(estimator_aid_source3d_s &aid_src, const extVisionSampl
 					      math::max(_params.ev_vel_innov_gate, 1.f));	// innovation gate
 
 			if (!current_aid_src.innovation_rejected) {
-				fuseBodyVelocity(current_aid_src, current_aid_src.innovation_variance, H);
-
+				current_aid_src.fused = fuseMeasurement(H, current_aid_src.observation_variance,
+									current_aid_src.innovation, current_aid_src.innovation_variance);
 			}
 
 			aid_src.innovation[index] = current_aid_src.innovation;

src/modules/ekf2/EKF/aid_sources/gnss/gps_yaw_fusion.cpp

@@ -115,11 +115,7 @@ void Ekf::fuseGpsYaw(float antenna_yaw_offset)
 		resetGyroBiasZCov();
 	}
 
-	// calculate the Kalman gains
-	// only calculate gains for states we are using
-	VectorState Kfusion = P * H / aid_src.innovation_variance;
-
-	const bool is_fused = measurementUpdate(Kfusion, H, aid_src.observation_variance, aid_src.innovation);
+	const bool is_fused = fuseMeasurement(H, aid_src.observation_variance, aid_src.innovation, aid_src.innovation_variance);
 	_fault_status.flags.bad_hdg = !is_fused;
 	aid_src.fused = is_fused;
 

src/modules/ekf2/EKF/aid_sources/gravity/gravity_fusion.cpp

@@ -103,13 +103,11 @@ void Ekf::controlGravityFusion(const imuSample &imu)
 							     -1.f))(index) - measurement(index);
 		}
 
-		VectorState K = P * H / _aid_src_gravity.innovation_variance[index];
-
 		const bool accel_clipping = imu.delta_vel_clipping[0] || imu.delta_vel_clipping[1] || imu.delta_vel_clipping[2];
 
 		if (_control_status.flags.gravity_vector && !_aid_src_gravity.innovation_rejected && !accel_clipping) {
-			fused[index] = measurementUpdate(K, H, _aid_src_gravity.observation_variance[index],
-							 _aid_src_gravity.innovation[index]);
+			fused[index] = fuseMeasurement(H, _aid_src_gravity.observation_variance[index],
+						       _aid_src_gravity.innovation[index], _aid_src_gravity.innovation_variance[index]);
 		}
 	}
 

src/modules/ekf2/EKF/aid_sources/magnetometer/mag_fusion.cpp

@@ -125,7 +125,7 @@ bool Ekf::fuseMag(const Vector3f &mag, const float R_MAG, VectorState &H, estima
 			Kfusion.slice<State::mag_B.dof, 1>(State::mag_B.idx, 0) = K_mag_B;
 		}
 
-		if (measurementUpdate(Kfusion, H, aid_src.observation_variance[index], aid_src.innovation[index])) {
+		if (fuseMeasurement(Kfusion, H, aid_src.observation_variance[index], aid_src.innovation[index])) {
 			fused[index] = true;
 		}
 	}
@@ -179,15 +179,7 @@ bool Ekf::fuseDeclination(float decl_sigma)
 
 		const float innovation = wrap_pi(decl_pred - decl_measurement);
 
-		if (innovation_variance < R_DECL) {
-			// variance calculation is badly conditioned
-			return false;
-		}
-
-		// Calculate the Kalman gains
-		VectorState Kfusion = P * H / innovation_variance;
-
-		const bool is_fused = measurementUpdate(Kfusion, H, R_DECL, innovation);
+		const bool is_fused = fuseMeasurement(H, R_DECL, innovation, innovation_variance);
 
 		_fault_status.flags.bad_mag_decl = !is_fused;
 

src/modules/ekf2/EKF/aid_sources/optical_flow/optical_flow_fusion.cpp

@@ -85,8 +85,9 @@ bool Ekf::fuseOptFlow(VectorState &H, const bool update_terrain)
 			Kfusion(State::terrain.idx) = 0.f;
 		}
 
-		if (measurementUpdate(Kfusion, H, _aid_src_optical_flow.observation_variance[index],
-				      _aid_src_optical_flow.innovation[index])) {
+		if (fuseMeasurement(Kfusion, H, _aid_src_optical_flow.observation_variance[index],
+				    _aid_src_optical_flow.innovation[index])
+		   ) {
 			fused[index] = true;
 		}
 	}

src/modules/ekf2/EKF/aid_sources/range_finder/range_height_fusion.cpp

@@ -58,13 +58,13 @@ bool Ekf::fuseHaglRng(estimator_aid_source1d_s &aid_src, bool update_height, boo
 		K(State::terrain.idx) = k_terrain;
 	}
 
-	measurementUpdate(K, H, aid_src.observation_variance, aid_src.innovation);
+	if (fuseMeasurement(K, H, aid_src.observation_variance, aid_src.innovation)) {
+		// record last successful fusion event
+		_innov_check_fail_status.flags.reject_hagl = false;
 
-	// record last successful fusion event
-	_innov_check_fail_status.flags.reject_hagl = false;
+		aid_src.time_last_fuse = _time_delayed_us;
+		aid_src.fused = true;
+	}
 
-	aid_src.time_last_fuse = _time_delayed_us;
-	aid_src.fused = true;
-
-	return true;
+	return aid_src.fused;
 }

src/modules/ekf2/EKF/aid_sources/sideslip/sideslip_fusion.cpp

@@ -141,12 +141,12 @@ void Ekf::fuseSideslip(estimator_aid_source1d_s &sideslip)
 		K.slice<State::wind_vel.dof, 1>(State::wind_vel.idx, 0) = K_wind;
 	}
 
-	const bool is_fused = measurementUpdate(K, H, sideslip.observation_variance, sideslip.innovation);
-
-	sideslip.fused = is_fused;
-	_fault_status.flags.bad_sideslip = !is_fused;
-
-	if (is_fused) {
+	if (fuseMeasurement(K, H, sideslip.observation_variance, sideslip.innovation)) {
+		sideslip.fused = true;
 		sideslip.time_last_fuse = _time_delayed_us;
+
+		_fault_status.flags.bad_sideslip = false;
 	}
+
+	_fault_status.flags.bad_sideslip = !sideslip.fused;
 }

src/modules/ekf2/EKF/ekf.cpp

@@ -273,6 +273,138 @@ void Ekf::predictState(const imuSample &imu_delayed)
 	_height_rate_lpf = _height_rate_lpf * (1.0f - alpha_height_rate_lpf) + _state.vel(2) * alpha_height_rate_lpf;
 }
 
+bool Ekf::fuseDirectStateMeasurement(const unsigned state_index, const float R, const float innovation,
+				     const float innovation_variance)
+{
+	if (innovation_variance < R) {
+		// variance calculation is badly conditioned
+		return false;
+	}
+
+	VectorState K;  // Kalman gain vector for any single observation - sequential fusion is used.
+
+	// calculate kalman gain K = PHS, where S = 1/innovation variance
+	for (int row = 0; row < State::size; row++) {
+		K(row) = P(row, state_index) / innovation_variance;
+	}
+
+	clearInhibitedStateKalmanGains(K);
+
+#if false
+	// Matrix implementation of the Joseph stabilized covariance update
+	// This is extremely expensive to compute. Use for debugging purposes only.
+	auto A = matrix::eye<float, State::size>();
+	VectorState H;
+	H(state_index) = 1.f;
+	A -= K.multiplyByTranspose(H);
+	P = A * P;
+	P = P.multiplyByTranspose(A);
+
+	const VectorState KR = K * R;
+	P += KR.multiplyByTranspose(K);
+#else
+	// Efficient implementation of the Joseph stabilized covariance update
+	// Based on "G. J. Bierman. Factorization Methods for Discrete Sequential Estimation. Academic Press, Dover Publications, New York, 1977, 2006"
+	// P = (I - K * H) * P * (I - K * H).T   + K * R * K.T
+	//   =      P_temp     * (I - H.T * K.T) + K * R * K.T
+	//   =      P_temp - P_temp * H.T * K.T  + K * R * K.T
+
+	// Step 1: conventional update
+	// Compute P_temp and store it in P to avoid allocating more memory
+	// P is symmetric, so PH == H.T * P.T == H.T * P. Taking the row is faster as matrices are row-major
+	VectorState PH = P.row(state_index);
+
+	for (unsigned i = 0; i < State::size; i++) {
+		for (unsigned j = 0; j < State::size; j++) {
+			P(i, j) -= K(i) * PH(j); // P is now not symmetric if K is not optimal (e.g.: some gains have been zeroed)
+		}
+	}
+
+	// Step 2: stabilized update
+	// P (or "P_temp") is not symmetric so we must take the column
+	PH = P.col(state_index);
+
+	for (unsigned i = 0; i < State::size; i++) {
+		for (unsigned j = 0; j <= i; j++) {
+			P(i, j) = P(i, j) - PH(i) * K(j) + K(i) * R * K(j);
+			P(j, i) = P(i, j);
+		}
+	}
+
+#endif
+
+	constrainStateVariances();
+
+	// apply the state corrections
+	fuse(K, innovation);
+	return true;
+}
+
+bool Ekf::fuseMeasurement(const VectorState &H, const float R, const float innovation, const float innovation_variance)
+{
+	if (innovation_variance < R) {
+		// variance calculation is badly conditioned
+		return false;
+	}
+
+	// calculate the Kalman gains
+	// only calculate gains for states we are using
+	VectorState K = P * H / innovation_variance;
+
+	return fuseMeasurement(K, H, R, innovation);
+}
+
+bool Ekf::fuseMeasurement(VectorState &K, const VectorState &H, const float R, const float innovation)
+{
+	clearInhibitedStateKalmanGains(K);
+
+#if false
+	// Matrix implementation of the Joseph stabilized covariance update
+	// This is extremely expensive to compute. Use for debugging purposes only.
+	auto A = matrix::eye<float, State::size>();
+	A -= K.multiplyByTranspose(H);
+	P = A * P;
+	P = P.multiplyByTranspose(A);
+
+	const VectorState KR = K * R;
+	P += KR.multiplyByTranspose(K);
+#else
+	// Efficient implementation of the Joseph stabilized covariance update
+	// Based on "G. J. Bierman. Factorization Methods for Discrete Sequential Estimation. Academic Press, Dover Publications, New York, 1977, 2006"
+	// P = (I - K * H) * P * (I - K * H).T   + K * R * K.T
+	//   =      P_temp     * (I - H.T * K.T) + K * R * K.T
+	//   =      P_temp - P_temp * H.T * K.T  + K * R * K.T
+
+	// Step 1: conventional update
+	// Compute P_temp and store it in P to avoid allocating more memory
+	// P is symmetric, so PH == H.T * P.T == H.T * P. Taking the row is faster as matrices are row-major
+	VectorState PH = P * H; // H is stored as a column vector. H is in fact H.T
+
+	for (unsigned i = 0; i < State::size; i++) {
+		for (unsigned j = 0; j < State::size; j++) {
+			P(i, j) -= K(i) * PH(j); // P is now not symmetrical if K is not optimal (e.g.: some gains have been zeroed)
+		}
+	}
+
+	// Step 2: stabilized update
+	PH = P * H; // H is stored as a column vector. H is in fact H.T
+
+	for (unsigned i = 0; i < State::size; i++) {
+		for (unsigned j = 0; j <= i; j++) {
+			P(i, j) = P(i, j) - PH(i) * K(j) + K(i) * R * K(j);
+			P(j, i) = P(i, j);
+		}
+	}
+
+#endif
+
+	constrainStateVariances();
+
+	// apply the state corrections
+	fuse(K, innovation);
+	return true;
+}
+
 bool Ekf::resetGlobalPosToExternalObservation(double lat_deg, double lon_deg, float accuracy,
 		uint64_t timestamp_observation)
 {
@@ -301,10 +433,10 @@ bool Ekf::resetGlobalPosToExternalObservation(double lat_deg, double lon_deg, fl
 		pos_corrected += _state.vel.xy() * dt_s;
 	}
 
-	const float obs_var = math::max(sq(accuracy), sq(0.01f));
+	const float R = math::max(sq(accuracy), sq(0.01f));
 
 	const Vector2f innov = Vector2f(_state.pos.xy()) - pos_corrected;
-	const Vector2f innov_var = Vector2f(getStateVariance<State::pos>()) + obs_var;
+	const Vector2f innov_var = Vector2f(getStateVariance<State::pos>()) + R;
 
 	const float sq_gate = sq(5.f); // magic hardcoded gate
 	const Vector2f test_ratio{sq(innov(0)) / (sq_gate * innov_var(0)),
@@ -318,13 +450,13 @@ bool Ekf::resetGlobalPosToExternalObservation(double lat_deg, double lon_deg, fl
 		ECL_INFO("reset position to external observation");
 		_information_events.flags.reset_pos_to_ext_obs = true;
 
-		resetHorizontalPositionTo(pos_corrected, obs_var);
+		resetHorizontalPositionTo(pos_corrected, R);
 		_last_known_pos.xy() = _state.pos.xy();
 		return true;
 
 	} else {
-		if (fuseDirectStateMeasurement(innov(0), innov_var(0), obs_var, State::pos.idx + 0)
-		    && fuseDirectStateMeasurement(innov(1), innov_var(1), obs_var, State::pos.idx + 1)
+		if (fuseDirectStateMeasurement(State::pos.idx + 0, R, innov(0), innov_var(0))
+		    && fuseDirectStateMeasurement(State::pos.idx + 1, R, innov(1), innov_var(1))
 		   ) {
 			ECL_INFO("fused external observation as position measurement");
 			_time_last_hor_pos_fuse = _time_delayed_us;

src/modules/ekf2/EKF/ekf.h

@@ -320,8 +320,12 @@ public:
 #endif
 	}
 
-	// fuse single direct state measurement (eg NED velocity, NED position, mag earth field, etc)
-	bool fuseDirectStateMeasurement(const float innov, const float innov_var, const float R, const int state_index);
+	bool fuseMeasurement(const VectorState &H, const float R, const float innovation, const float innovation_variance);
+	bool fuseMeasurement(VectorState &K, const VectorState &H, const float R, const float innovation);
+
+	// fuse direct state observations
+	bool fuseDirectStateMeasurement(const unsigned state_index, const float R, const float innovation,
+					const float innovation_variance);
 
 	// gyro bias
 	const Vector3f &getGyroBias() const { return _state.gyro_bias; } // get the gyroscope bias in rad/s
@@ -471,57 +475,6 @@ public:
 	const auto &aid_src_aux_vel() const { return _aid_src_aux_vel; }
 #endif // CONFIG_EKF2_AUXVEL
 
-	bool measurementUpdate(VectorState &K, const VectorState &H, const float R, const float innovation)
-	{
-		clearInhibitedStateKalmanGains(K);
-
-#if false
-		// Matrix implementation of the Joseph stabilized covariance update
-		// This is extremely expensive to compute. Use for debugging purposes only.
-		auto A = matrix::eye<float, State::size>();
-		A -= K.multiplyByTranspose(H);
-		P = A * P;
-		P = P.multiplyByTranspose(A);
-
-		const VectorState KR = K * R;
-		P += KR.multiplyByTranspose(K);
-#else
-		// Efficient implementation of the Joseph stabilized covariance update
-		// Based on "G. J. Bierman. Factorization Methods for Discrete Sequential Estimation. Academic Press, Dover Publications, New York, 1977, 2006"
-		// P = (I - K * H) * P * (I - K * H).T   + K * R * K.T
-		//   =      P_temp     * (I - H.T * K.T) + K * R * K.T
-		//   =      P_temp - P_temp * H.T * K.T  + K * R * K.T
-
-		// Step 1: conventional update
-		// Compute P_temp and store it in P to avoid allocating more memory
-		// P is symmetric, so PH == H.T * P.T == H.T * P. Taking the row is faster as matrices are row-major
-		VectorState PH = P * H; // H is stored as a column vector. H is in fact H.T
-
-		for (unsigned i = 0; i < State::size; i++) {
-			for (unsigned j = 0; j < State::size; j++) {
-				P(i, j) -= K(i) * PH(j); // P is now not symmetrical if K is not optimal (e.g.: some gains have been zeroed)
-			}
-		}
-
-		// Step 2: stabilized update
-		PH = P * H; // H is stored as a column vector. H is in fact H.T
-
-		for (unsigned i = 0; i < State::size; i++) {
-			for (unsigned j = 0; j <= i; j++) {
-				P(i, j) = P(i, j) - PH(i) * K(j) + K(i) * R * K(j);
-				P(j, i) = P(i, j);
-			}
-		}
-
-#endif
-
-		constrainStateVariances();
-
-		// apply the state corrections
-		fuse(K, innovation);
-		return true;
-	}
-
 	bool resetGlobalPosToExternalObservation(double lat_deg, double lon_deg, float accuracy,
 			uint64_t timestamp_observation);
 
@@ -963,11 +916,6 @@ private:
 	void stopEvVelFusion();
 	void stopEvYawFusion();
 	bool fuseEvVelocity(estimator_aid_source3d_s &aid_src, const extVisionSample &ev_sample);
-	void fuseBodyVelocity(estimator_aid_source1d_s &aid_src, float &innov_var, VectorState &H)
-	{
-		VectorState Kfusion = P * H / innov_var;
-		aid_src.fused = measurementUpdate(Kfusion, H, aid_src.observation_variance, aid_src.innovation);
-	}
 #endif // CONFIG_EKF2_EXTERNAL_VISION
 
 #if defined(CONFIG_EKF2_GNSS)

src/modules/ekf2/EKF/ekf_helper.cpp

@@ -878,64 +878,3 @@ void Ekf::updateIMUBiasInhibit(const imuSample &imu_delayed)
 		_accel_bias_inhibit[index] = do_inhibit_all_accel_axes || imu_delayed.delta_vel_clipping[index] || !is_bias_observable;
 	}
 }
-
-bool Ekf::fuseDirectStateMeasurement(const float innov, const float innov_var, const float R, const int state_index)
-{
-	VectorState K;  // Kalman gain vector for any single observation - sequential fusion is used.
-
-	// calculate kalman gain K = PHS, where S = 1/innovation variance
-	for (int row = 0; row < State::size; row++) {
-		K(row) = P(row, state_index) / innov_var;
-	}
-
-	clearInhibitedStateKalmanGains(K);
-
-#if false
-	// Matrix implementation of the Joseph stabilized covariance update
-	// This is extremely expensive to compute. Use for debugging purposes only.
-	auto A = matrix::eye<float, State::size>();
-	VectorState H;
-	H(state_index) = 1.f;
-	A -= K.multiplyByTranspose(H);
-	P = A * P;
-	P = P.multiplyByTranspose(A);
-
-	const VectorState KR = K * R;
-	P += KR.multiplyByTranspose(K);
-#else
-	// Efficient implementation of the Joseph stabilized covariance update
-	// Based on "G. J. Bierman. Factorization Methods for Discrete Sequential Estimation. Academic Press, Dover Publications, New York, 1977, 2006"
-	// P = (I - K * H) * P * (I - K * H).T   + K * R * K.T
-	//   =      P_temp     * (I - H.T * K.T) + K * R * K.T
-	//   =      P_temp - P_temp * H.T * K.T  + K * R * K.T
-
-	// Step 1: conventional update
-	// Compute P_temp and store it in P to avoid allocating more memory
-	// P is symmetric, so PH == H.T * P.T == H.T * P. Taking the row is faster as matrices are row-major
-	VectorState PH = P.row(state_index);
-
-	for (unsigned i = 0; i < State::size; i++) {
-		for (unsigned j = 0; j < State::size; j++) {
-			P(i, j) -= K(i) * PH(j); // P is now not symmetric if K is not optimal (e.g.: some gains have been zeroed)
-		}
-	}
-
-	// Step 2: stabilized update
-	// P (or "P_temp") is not symmetric so we must take the column
-	PH = P.col(state_index);
-
-	for (unsigned i = 0; i < State::size; i++) {
-		for (unsigned j = 0; j <= i; j++) {
-			P(i, j) = P(i, j) - PH(i) * K(j) + K(i) * R * K(j);
-			P(j, i) = P(i, j);
-		}
-	}
-
-#endif
-
-	constrainStateVariances();
-
-	// apply the state corrections
-	fuse(K, innov);
-	return true;
-}

src/modules/ekf2/EKF/position_fusion.cpp

@@ -57,10 +57,10 @@ bool Ekf::fuseHorizontalPosition(estimator_aid_source2d_s &aid_src)
 {
 	// x & y
 	if (!aid_src.innovation_rejected
-	    && fuseDirectStateMeasurement(aid_src.innovation[0], aid_src.innovation_variance[0], aid_src.observation_variance[0],
-					  State::pos.idx + 0)
-	    && fuseDirectStateMeasurement(aid_src.innovation[1], aid_src.innovation_variance[1], aid_src.observation_variance[1],
-					  State::pos.idx + 1)
+	    && fuseDirectStateMeasurement(State::pos.idx + 0, aid_src.observation_variance[0], aid_src.innovation[0],
+					  aid_src.innovation_variance[0])
+	    && fuseDirectStateMeasurement(State::pos.idx + 1, aid_src.observation_variance[1], aid_src.innovation[1],
+					  aid_src.innovation_variance[1])
 	   ) {
 		aid_src.fused = true;
 		aid_src.time_last_fuse = _time_delayed_us;
@@ -78,8 +78,8 @@ bool Ekf::fuseVerticalPosition(estimator_aid_source1d_s &aid_src)
 {
 	// z
 	if (!aid_src.innovation_rejected
-	    && fuseDirectStateMeasurement(aid_src.innovation, aid_src.innovation_variance, aid_src.observation_variance,
-					  State::pos.idx + 2)
+	    && fuseDirectStateMeasurement(State::pos.idx + 2, aid_src.observation_variance, aid_src.innovation,
+					  aid_src.innovation_variance)
 	   ) {
 		aid_src.fused = true;
 		aid_src.time_last_fuse = _time_delayed_us;

src/modules/ekf2/EKF/velocity_fusion.cpp

@@ -37,10 +37,10 @@ bool Ekf::fuseHorizontalVelocity(estimator_aid_source2d_s &aid_src)
 {
 	// vx, vy
 	if (!aid_src.innovation_rejected
-	    && fuseDirectStateMeasurement(aid_src.innovation[0], aid_src.innovation_variance[0], aid_src.observation_variance[0],
-					  State::vel.idx + 0)
-	    && fuseDirectStateMeasurement(aid_src.innovation[1], aid_src.innovation_variance[1], aid_src.observation_variance[1],
-					  State::vel.idx + 1)
+	    && fuseDirectStateMeasurement(State::vel.idx + 0, aid_src.observation_variance[0], aid_src.innovation[0],
+					  aid_src.innovation_variance[0])
+	    && fuseDirectStateMeasurement(State::vel.idx + 1, aid_src.observation_variance[1], aid_src.innovation[1],
+					  aid_src.innovation_variance[1])
 	   ) {
 		aid_src.fused = true;
 		aid_src.time_last_fuse = _time_delayed_us;
@@ -58,12 +58,12 @@ bool Ekf::fuseVelocity(estimator_aid_source3d_s &aid_src)
 {
 	// vx, vy, vz
 	if (!aid_src.innovation_rejected
-	    && fuseDirectStateMeasurement(aid_src.innovation[0], aid_src.innovation_variance[0], aid_src.observation_variance[0],
-					  State::vel.idx + 0)
-	    && fuseDirectStateMeasurement(aid_src.innovation[1], aid_src.innovation_variance[1], aid_src.observation_variance[1],
-					  State::vel.idx + 1)
-	    && fuseDirectStateMeasurement(aid_src.innovation[2], aid_src.innovation_variance[2], aid_src.observation_variance[2],
-					  State::vel.idx + 2)
+	    && fuseDirectStateMeasurement(State::vel.idx + 0, aid_src.observation_variance[0], aid_src.innovation[0],
+					  aid_src.innovation_variance[0])
+	    && fuseDirectStateMeasurement(State::vel.idx + 1, aid_src.observation_variance[1], aid_src.innovation[1],
+					  aid_src.innovation_variance[1])
+	    && fuseDirectStateMeasurement(State::vel.idx + 2, aid_src.observation_variance[2], aid_src.innovation[2],
+					  aid_src.innovation_variance[2])
 	   ) {
 		aid_src.fused = true;
 		aid_src.time_last_fuse = _time_delayed_us;

src/modules/ekf2/EKF/yaw_fusion.cpp

@@ -98,7 +98,7 @@ bool Ekf::fuseYaw(estimator_aid_source1d_s &aid_src_status, const VectorState &H
 		_innov_check_fail_status.flags.reject_yaw = false;
 	}
 
-	if (measurementUpdate(Kfusion, H_YAW, aid_src_status.observation_variance, aid_src_status.innovation)) {
+	if (fuseMeasurement(Kfusion, H_YAW, aid_src_status.observation_variance, aid_src_status.innovation)) {
 
 		_time_last_heading_fuse = _time_delayed_us;