EKF: move calculation of optical flow observation variance into a function

Allows it to be used when calculating initial state variance

EKF/control.cpp

@@ -106,8 +106,8 @@ void Ekf::controlFusionModes()
 				zeroRows(P,4,5);
 				zeroCols(P,4,5);
 
-				// reset the horizontal velocity variance using the optical flow noise
+				// reset the horizontal velocity variance using the optical flow noise variance
-				P[5][5] = P[4][4] = sq(range * _params.flow_noise_qual_min);
+				P[5][5] = P[4][4] = sq(range) * calcOptFlowMeasVar();
 
 				if (!_in_air) {
 					// we are likely starting OF for the first time so reset the position and states

EKF/ekf.h

@@ -347,4 +347,7 @@ private:
 	// zero the specified range of columns in the state covariance matrix
 	void zeroCols(float (&cov_mat)[_k_num_states][_k_num_states], uint8_t first, uint8_t last);
 
+	// calculate the measurement variance for the optical flow sensor
+	float calcOptFlowMeasVar();
+
 };

EKF/optflow_fusion.cpp

@@ -59,23 +59,8 @@ void Ekf::fuseOptFlow()
 	float ve = _state.vel(1);
 	float vd = _state.vel(2);
 
-	// calculate the observation noise variance - scaling noise linearly across flow quality range
+	// calculate the optical flow observation variance
-	float R_LOS_best = fmaxf(_params.flow_noise, 0.05f);
+	float R_LOS = calcOptFlowMeasVar();
-	float R_LOS_worst = fmaxf(_params.flow_noise_qual_min, 0.05f);
-
-	// calculate a weighting that varies between 1 when flow quality is best and 0 when flow quality is worst
-	float weighting = (255.0f - (float)_params.flow_qual_min);
-
-	if (weighting >= 1.0f) {
-		weighting = math::constrain(((float)_flow_sample_delayed.quality - (float)_params.flow_qual_min) / weighting, 0.0f,
-					    1.0f);
-
-	} else {
-		weighting = 0.0f;
-	}
-
-	// take the weighted average of the observation noie for the best and wort flow quality
-	float R_LOS = sq(R_LOS_best * weighting + R_LOS_worst * (1.0f - weighting));
 
 	float H_LOS[2][24] = {}; // Optical flow observation Jacobians
 	float Kfusion[24][2] = {}; // Optical flow Kalman gains
@@ -548,3 +533,27 @@ void Ekf::calcOptFlowBias()
 		_delta_time_of = 0.0f;
 	}
 }
+
+// calculate the measurement variance for the optical flow sensor (rad/sec)^2
+float Ekf::calcOptFlowMeasVar()
+{
+	// calculate the observation noise variance - scaling noise linearly across flow quality range
+	float R_LOS_best = fmaxf(_params.flow_noise, 0.05f);
+	float R_LOS_worst = fmaxf(_params.flow_noise_qual_min, 0.05f);
+
+	// calculate a weighting that varies between 1 when flow quality is best and 0 when flow quality is worst
+	float weighting = (255.0f - (float)_params.flow_qual_min);
+
+	if (weighting >= 1.0f) {
+		weighting = math::constrain(((float)_flow_sample_delayed.quality - (float)_params.flow_qual_min) / weighting, 0.0f,
+					    1.0f);
+
+	} else {
+		weighting = 0.0f;
+	}
+
+	// take the weighted average of the observation noie for the best and wort flow quality
+	float R_LOS = sq(R_LOS_best * weighting + R_LOS_worst * (1.0f - weighting));
+
+	return R_LOS;
+}