Increase matrix library usage even more

EKF/airspeed_fusion.cpp

@@ -215,7 +215,7 @@ void Ekf::fuseAirspeed()
 		// only apply covariance and state corrections if healthy
 		if (healthy) {
 			// apply the covariance corrections
-			P = P - KHP;
+			P -= KHP;
 
 			// correct the covariance matrix for gross errors
 			fixCovarianceErrors(true);

EKF/drag_fusion.cpp

@@ -292,11 +292,8 @@ void Ekf::fuseDrag()
 			// only apply covariance and state corrections if healthy
 			if (healthy) {
 				// apply the covariance corrections
-				for (unsigned row = 0; row < _k_num_states; row++) {
-					for (unsigned column = 0; column < _k_num_states; column++) {
-						P(row,column) = P(row,column) - KHP(row,column);
-					}
-				}
+				P -= KHP;
+
 
 				// correct the covariance matrix for gross errors
 				fixCovarianceErrors(true);

EKF/gps_yaw_fusion.cpp

@@ -261,11 +261,7 @@ void Ekf::fuseGpsAntYaw()
 	if (healthy) {
 		_time_last_gps_yaw_fuse = _time_last_imu;
 		// apply the covariance corrections
-		for (unsigned row = 0; row < _k_num_states; row++) {
-			for (unsigned column = 0; column < _k_num_states; column++) {
-				P(row,column) = P(row,column) - KHP(row,column);
-			}
-		}
+		P -= KHP;
 
 		// correct the covariance matrix for gross errors
 		fixCovarianceErrors(true);

EKF/mag_fusion.cpp

@@ -416,11 +416,7 @@ void Ekf::fuseMag()
 		// only apply covariance and state corrections if healthy
 		if (healthy) {
 			// apply the covariance corrections
-			for (unsigned row = 0; row < _k_num_states; row++) {
-				for (unsigned column = 0; column < _k_num_states; column++) {
-					P(row,column) = P(row,column) - KHP(row,column);
-				}
-			}
+			P -= KHP;
 
 			// correct the covariance matrix for gross errors
 			fixCovarianceErrors(true);
@@ -752,11 +748,7 @@ void Ekf::updateQuaternion(const float innovation, const float variance, const f
 	// only apply covariance and state corrections if healthy
 	if (healthy) {
 		// apply the covariance corrections
-		for (unsigned row = 0; row < _k_num_states; row++) {
-			for (unsigned column = 0; column < _k_num_states; column++) {
-				P(row,column) = P(row,column) - KHP(row,column);
-			}
-		}
+		P -= KHP;
 
 		// correct the covariance matrix for gross errors
 		fixCovarianceErrors(true);
@@ -1045,11 +1037,7 @@ void Ekf::fuseDeclination(float decl_sigma)
 	// only apply covariance and state corrections if healthy
 	if (healthy) {
 		// apply the covariance corrections
-		for (unsigned row = 0; row < _k_num_states; row++) {
-			for (unsigned column = 0; column < _k_num_states; column++) {
-				P(row,column) = P(row,column) - KHP(row,column);
-			}
-		}
+		P -= KHP;
 
 		// correct the covariance matrix for gross errors
 		fixCovarianceErrors(true);

EKF/optflow_fusion.cpp

@@ -494,11 +494,7 @@ void Ekf::fuseOptFlow()
 		// only apply covariance and state corrections if healthy
 		if (healthy) {
 			// apply the covariance corrections
-			for (unsigned row = 0; row < _k_num_states; row++) {
-				for (unsigned column = 0; column < _k_num_states; column++) {
-					P(row,column) = P(row,column) - KHP(row,column);
-				}
-			}
+			P -= KHP;
 
 			// correct the covariance matrix for gross errors
 			fixCovarianceErrors(true);

EKF/sideslip_fusion.cpp

@@ -263,11 +263,7 @@ void Ekf::fuseSideslip()
 		// only apply covariance and state corrections if healthy
 		if (healthy) {
 			// apply the covariance corrections
-			for (unsigned row = 0; row < _k_num_states; row++) {
-				for (unsigned column = 0; column < _k_num_states; column++) {
-					P(row,column) = P(row,column) - KHP(row,column);
-				}
-			}
+			P -= KHP;
 
 			// correct the covariance matrix for gross errors
 			fixCovarianceErrors(true);

EKF/vel_pos_fusion.cpp

@@ -190,11 +190,7 @@ void Ekf::fuseVelPosHeight(const float innov, const float innov_var, const int o
 	// only apply covariance and state corrections if healthy
 	if (healthy) {
 		// apply the covariance corrections
-		for (unsigned row = 0; row < _k_num_states; row++) {
-			for (unsigned column = 0; column < _k_num_states; column++) {
-				P(row, column) = P(row, column) - KHP(row, column);
-			}
-		}
+		P -= KHP;
 
 		// correct the covariance matrix for gross errors
 		fixCovarianceErrors(true);