Make flow_innov/-var a matrix Vector2f

EKF/ekf.h

@@ -428,8 +428,8 @@ private:
 	float _hagl_innov_var{0.0f};		///< innovation variance for the last height above terrain measurement (m**2)
 
 	// optical flow processing
-	float _flow_innov[2] {};	///< flow measurement innovation (rad/sec)
-	float _flow_innov_var[2] {};	///< flow innovation variance ((rad/sec)**2)
+	Vector2f _flow_innov;		///< flow measurement innovation (rad/sec)
+	Vector2f _flow_innov_var;	///< flow innovation variance ((rad/sec)**2)
 	Vector3f _flow_gyro_bias;	///< bias errors in optical flow sensor rate gyro outputs (rad/sec)
 	Vector3f _imu_del_ang_of;	///< bias corrected delta angle measurements accumulated across the same time frame as the optical flow rates (rad)
 	float _delta_time_of{0.0f};	///< time in sec that _imu_del_ang_of was accumulated over (sec)

EKF/ekf_helper.cpp

@@ -727,12 +727,12 @@ void Ekf::getAuxVelInnovRatio(float &aux_vel_innov_ratio) const
 
 void Ekf::getFlowInnov(float flow_innov[2]) const
 {
-	memcpy(flow_innov, _flow_innov, sizeof(_flow_innov));
+	_flow_innov.copyTo(flow_innov);
 }
 
 void Ekf::getFlowInnovVar(float flow_innov_var[2]) const
 {
-	memcpy(flow_innov_var, _flow_innov_var, sizeof(_flow_innov_var));
+	_flow_innov_var.copyTo(flow_innov_var);
 }
 
 void Ekf::getFlowInnovRatio(float &flow_innov_ratio) const
@@ -983,7 +983,7 @@ void Ekf::get_ekf_vel_accuracy(float *ekf_evh, float *ekf_evv)
 
 		if (_control_status.flags.opt_flow) {
 			float gndclearance = math::max(_params.rng_gnd_clearance, 0.1f);
-			vel_err_conservative = math::max((_terrain_vpos - _state.pos(2)), gndclearance) * sqrtf(sq(_flow_innov[0]) + sq(_flow_innov[1]));
+			vel_err_conservative = math::max((_terrain_vpos - _state.pos(2)), gndclearance) * _flow_innov.norm();
 		}
 
 		if (_control_status.flags.gps) {
@@ -1667,9 +1667,9 @@ void Ekf::stopAuxVelFusion()
 void Ekf::stopFlowFusion()
 {
 	_control_status.flags.opt_flow = false;
-	memset(_flow_innov,0.0f,sizeof(_flow_innov));
-	memset(_flow_innov_var,0.0f,sizeof(_flow_innov_var));
-	memset(&_optflow_test_ratio,0.0f,sizeof(_optflow_test_ratio));
+	_flow_innov.setZero();
+	_flow_innov_var.setZero();
+	_optflow_test_ratio = 0.0f;
 }
 
 void Ekf::resetQuatStateYaw(float yaw, float yaw_variance, bool update_buffer)

EKF/optflow_fusion.cpp

@@ -104,8 +104,8 @@ void Ekf::fuseOptFlow()
 	const Vector2f opt_flow_rate = _flow_compensated_XY_rad / _flow_sample_delayed.dt + Vector2f(_flow_gyro_bias);
 
 	if (opt_flow_rate.norm() < _flow_max_rate) {
-		_flow_innov[0] =  vel_body(1) / range - opt_flow_rate(0); // flow around the X axis
-		_flow_innov[1] = -vel_body(0) / range - opt_flow_rate(1); // flow around the Y axis
+		_flow_innov(0) =  vel_body(1) / range - opt_flow_rate(0); // flow around the X axis
+		_flow_innov(1) = -vel_body(0) / range - opt_flow_rate(1); // flow around the Y axis
 
 	} else {
 		return;
@@ -225,7 +225,7 @@ void Ekf::fuseOptFlow()
 			// calculate innovation variance for X axis observation and protect against a badly conditioned calculation
 			if (t77 >= R_LOS) {
 				t78 = 1.0f / t77;
-				_flow_innov_var[0] = t77;
+				_flow_innov_var(0) = t77;
 
 			} else {
 				// we need to reinitialise the covariance matrix and abort this fusion step
@@ -367,7 +367,7 @@ void Ekf::fuseOptFlow()
 			// calculate innovation variance for Y axis observation and protect against a badly conditioned calculation
 			if (t77 >= R_LOS) {
 				t78 = 1.0f / t77;
-				_flow_innov_var[1] = t77;
+				_flow_innov_var(1) = t77;
 
 			} else {
 				// we need to reinitialise the covariance matrix and abort this fusion step
@@ -407,8 +407,8 @@ void Ekf::fuseOptFlow()
 	// run the innovation consistency check and record result
 	bool flow_fail = false;
 	float test_ratio[2];
-	test_ratio[0] = sq(_flow_innov[0]) / (sq(math::max(_params.flow_innov_gate, 1.0f)) * _flow_innov_var[0]);
-	test_ratio[1] = sq(_flow_innov[1]) / (sq(math::max(_params.flow_innov_gate, 1.0f)) * _flow_innov_var[1]);
+	test_ratio[0] = sq(_flow_innov(0)) / (sq(math::max(_params.flow_innov_gate, 1.0f)) * _flow_innov_var(0));
+	test_ratio[1] = sq(_flow_innov(1)) / (sq(math::max(_params.flow_innov_gate, 1.0f)) * _flow_innov_var(1));
 	_optflow_test_ratio = math::max(test_ratio[0],test_ratio[1]);
 
 	for (uint8_t obs_index = 0; obs_index <= 1; obs_index++) {
@@ -493,7 +493,7 @@ void Ekf::fuseOptFlow()
 			fixCovarianceErrors(true);
 
 			// apply the state corrections
-			fuse(gain, _flow_innov[obs_index]);
+			fuse(gain, _flow_innov(obs_index));
 
 			_time_last_of_fuse = _time_last_imu;
 		}

EKF/terrain_estimator.cpp

@@ -231,27 +231,27 @@ void Ekf::fuseFlowForTerrain()
 	_terrain_var = fmaxf(_terrain_var, 0.0f);
 
 	// Cacluate innovation variance
-	_flow_innov_var[0] = Hx * Hx * _terrain_var + R_LOS;
+	_flow_innov_var(0) = Hx * Hx * _terrain_var + R_LOS;
 
 	// calculate the kalman gain for the flow x measurement
-	const float Kx = _terrain_var * Hx / _flow_innov_var[0];
+	const float Kx = _terrain_var * Hx / _flow_innov_var(0);
 
 	// calculate prediced optical flow about x axis
 	const float pred_flow_x = vel_body(1) * earth_to_body(2, 2) * pred_hagl_inv;
 
 	// calculate flow innovation (x axis)
-	_flow_innov[0] = pred_flow_x - opt_flow_rate(0);
+	_flow_innov(0) = pred_flow_x - opt_flow_rate(0);
 
 	// calculate correction term for terrain variance
 	const float KxHxP =  Kx * Hx * _terrain_var;
 
 	// innovation consistency check
 	const float gate_size = fmaxf(_params.flow_innov_gate, 1.0f);
-	float flow_test_ratio = sq(_flow_innov[0]) / (sq(gate_size) * _flow_innov_var[0]);
+	float flow_test_ratio = sq(_flow_innov(0)) / (sq(gate_size) * _flow_innov_var(0));
 
 	// do not perform measurement update if badly conditioned
 	if (flow_test_ratio <= 1.0f) {
-		_terrain_vpos += Kx * _flow_innov[0];
+		_terrain_vpos += Kx * _flow_innov(0);
 		// guard against negative variance
 		_terrain_var = fmaxf(_terrain_var - KxHxP, 0.0f);
 		_time_last_of_fuse = _time_last_imu;
@@ -261,25 +261,25 @@ void Ekf::fuseFlowForTerrain()
 	const float Hy = -vel_body(0) * t0 * pred_hagl_inv * pred_hagl_inv;
 
 	// Calculuate innovation variance
-	_flow_innov_var[1] = Hy * Hy * _terrain_var + R_LOS;
+	_flow_innov_var(1) = Hy * Hy * _terrain_var + R_LOS;
 
 	// calculate the kalman gain for the flow y measurement
-	const float Ky = _terrain_var * Hy / _flow_innov_var[1];
+	const float Ky = _terrain_var * Hy / _flow_innov_var(1);
 
 	// calculate prediced optical flow about y axis
 	const float pred_flow_y = -vel_body(0) * earth_to_body(2, 2) * pred_hagl_inv;
 
 	// calculate flow innovation (y axis)
-	_flow_innov[1] = pred_flow_y - opt_flow_rate(1);
+	_flow_innov(1) = pred_flow_y - opt_flow_rate(1);
 
 	// calculate correction term for terrain variance
 	const float KyHyP =  Ky * Hy * _terrain_var;
 
 	// innovation consistency check
-	flow_test_ratio = sq(_flow_innov[1]) / (sq(gate_size) * _flow_innov_var[1]);
+	flow_test_ratio = sq(_flow_innov(1)) / (sq(gate_size) * _flow_innov_var(1));
 
 	if (flow_test_ratio <= 1.0f) {
-		_terrain_vpos += Ky * _flow_innov[1];
+		_terrain_vpos += Ky * _flow_innov(1);
 		// guard against negative variance
 		_terrain_var = fmaxf(_terrain_var - KyHyP, 0.0f);
 		_time_last_of_fuse = _time_last_imu;