EKF: Add method to set diagonals in covariance matrix

EKF/ekf_helper.cpp

@@ -1227,6 +1227,20 @@ void Ekf::zeroOffDiag(float (&cov_mat)[_k_num_states][_k_num_states], uint8_t fi
 
 }
 
+void Ekf::setDiag(float (&cov_mat)[_k_num_states][_k_num_states], uint8_t first, uint8_t last, float variance)
+{
+	// zero rows and columns
+	zeroRows(cov_mat, first, last);
+	zeroCols(cov_mat, first, last);
+
+	// set diagonals
+	uint8_t row;
+	for (row = first; row <= last; row++) {
+		cov_mat[row][row] = variance;
+	}
+
+}
+
 bool Ekf::global_position_is_valid()
 {
 	// return true if we are not doing unconstrained free inertial navigation and the origin is set