EKF: Don't run unnecessary makeRowColSymmetric operation

This operation is expensive when done to the whole covariance matrix and unnecessary after covariance prediction because we calculate the upper diagonal and copy across so it is already symmetric.

EKF/airspeed_fusion.cpp

@@ -222,7 +222,7 @@ void Ekf::fuseAirspeed()
 			P = P - KHP;
 
 			// correct the covariance matrix for gross errors
-			fixCovarianceErrors();
+			fixCovarianceErrors(true);
 
 			// apply the state corrections
 			fuse(Kfusion, _airspeed_innov);

EKF/covariance.cpp

@@ -706,11 +706,11 @@ void Ekf::predictCovariance()
 
 	// fix gross errors in the covariance matrix and ensure rows and
 	// columns for un-used states are zero
-	fixCovarianceErrors();
+	fixCovarianceErrors(false);
 
 }
 
-void Ekf::fixCovarianceErrors()
+void Ekf::fixCovarianceErrors(bool force_symmetry)
 {
 	// NOTE: This limiting is a last resort and should not be relied on
 	// TODO: Split covariance prediction into separate F*P*transpose(F) and Q contributions
@@ -748,7 +748,9 @@ void Ekf::fixCovarianceErrors()
 	}
 
 	// force symmetry on the quaternion, velocity and position state covariances
-	P.makeRowColSymmetric<13>(0);
+	if (force_symmetry) {
+		P.makeRowColSymmetric<13>(0);
+	}
 
 	// the following states are optional and are deactivated when not required
 	// by ensuring the corresponding covariance matrix values are kept at zero
@@ -821,7 +823,7 @@ void Ekf::fixCovarianceErrors()
 			_fault_status.flags.bad_acc_bias = false;
 			ECL_WARN_TIMESTAMPED("invalid accel bias - resetting covariance");
 
-		} else {
+		} else if (force_symmetry) {
 			// ensure the covariance values are symmetrical
 			P.makeRowColSymmetric<3>(13);
 		}
@@ -843,8 +845,10 @@ void Ekf::fixCovarianceErrors()
 		}
 
 		// force symmetry
-		P.makeRowColSymmetric<3>(16);
-		P.makeRowColSymmetric<3>(19);
+		if (force_symmetry) {
+			P.makeRowColSymmetric<3>(16);
+			P.makeRowColSymmetric<3>(19);
+		}
 
 	}
 
@@ -859,7 +863,9 @@ void Ekf::fixCovarianceErrors()
 		}
 
 		// force symmetry
-		P.makeRowColSymmetric<2>(22);
+		if (force_symmetry) {
+			P.makeRowColSymmetric<2>(22);
+		}
 	}
 }
 

EKF/drag_fusion.cpp

@@ -301,7 +301,7 @@ void Ekf::fuseDrag()
 				}
 
 				// correct the covariance matrix for gross errors
-				fixCovarianceErrors();
+				fixCovarianceErrors(true);
 
 				// apply the state corrections
 				fuse(Kfusion, _drag_innov[axis_index]);

EKF/ekf.h

@@ -634,7 +634,8 @@ private:
 	void resetExtVisRotMat();
 
 	// limit the diagonal of the covariance matrix
-	void fixCovarianceErrors();
+	// force symmetry when the argument is true
+	void fixCovarianceErrors(bool force_symmetry);
 
 	// constrain the ekf states
 	void constrainStates();

EKF/gps_yaw_fusion.cpp

@@ -281,7 +281,7 @@ void Ekf::fuseGpsAntYaw()
 		}
 
 		// correct the covariance matrix for gross errors
-		fixCovarianceErrors();
+		fixCovarianceErrors(true);
 
 		// apply the state corrections
 		fuse(Kfusion, _heading_innov);

EKF/mag_fusion.cpp

@@ -423,7 +423,7 @@ void Ekf::fuseMag()
 			}
 
 			// correct the covariance matrix for gross errors
-			fixCovarianceErrors();
+			fixCovarianceErrors(true);
 
 			// apply the state corrections
 			fuse(Kfusion, _mag_innov[index]);
@@ -802,7 +802,7 @@ void Ekf::fuseHeading()
 		}
 
 		// correct the covariance matrix for gross errors
-		fixCovarianceErrors();
+		fixCovarianceErrors(true);
 
 		// apply the state corrections
 		fuse(Kfusion, _heading_innov);
@@ -939,7 +939,7 @@ void Ekf::fuseDeclination(float decl_sigma)
 		}
 
 		// correct the covariance matrix for gross errors
-		fixCovarianceErrors();
+		fixCovarianceErrors(true);
 
 		// apply the state corrections
 		fuse(Kfusion, innovation);

EKF/optflow_fusion.cpp

@@ -501,7 +501,7 @@ void Ekf::fuseOptFlow()
 			}
 
 			// correct the covariance matrix for gross errors
-			fixCovarianceErrors();
+			fixCovarianceErrors(true);
 
 			// apply the state corrections
 			fuse(gain, _flow_innov[obs_index]);

EKF/sideslip_fusion.cpp

@@ -274,7 +274,7 @@ void Ekf::fuseSideslip()
 			}
 
 			// correct the covariance matrix for gross errors
-			fixCovarianceErrors();
+			fixCovarianceErrors(true);
 
 			// apply the state corrections
 			fuse(Kfusion, _beta_innov);

EKF/vel_pos_fusion.cpp

@@ -186,7 +186,7 @@ void Ekf::fuseVelPosHeight(const float innov, const float innov_var, const int o
 		}
 
 		// correct the covariance matrix for gross errors
-		fixCovarianceErrors();
+		fixCovarianceErrors(true);
 
 		// apply the state corrections
 		fuse(Kfusion, innov);