EKF: remove Ekf::KHP and move KHP to the methods where it's used

Everywhere where KHP is used, it is first completely reset, thus making
it unnecessary to keep it as a class member.

This saves 2.3KB RAM.

Stack sizes don't need changing, since there is already a function
Ekf::predictCovariance(), which needs around 3KB of stack and is called
close to where the fuse* functions are called.

EKF/airspeed_fusion.cpp

@@ -169,6 +169,7 @@ void Ekf::fuseAirspeed()
 			KH[row][23] = Kfusion[row] * H_TAS[23];
 		}
 
+		float KHP[_k_num_states][_k_num_states];
 		for (unsigned row = 0; row < _k_num_states; row++) {
 			for (unsigned column = 0; column < _k_num_states; column++) {
 				float tmp = KH[row][4] * P[4][column];

EKF/ekf.h

@@ -239,7 +239,6 @@ private:
 
 	float P[_k_num_states][_k_num_states];	// state covariance matrix
 	float KH[_k_num_states][_k_num_states]; // intermediate variable for the covariance update
-	float KHP[_k_num_states][_k_num_states]; // intermediate variable for the covariance update
 
 	float _vel_pos_innov[6];	// innovations: 0-2 vel,  3-5 pos
 	float _vel_pos_innov_var[6];	// innovation variances: 0-2 vel, 3-5 pos

EKF/mag_fusion.cpp

@@ -320,6 +320,7 @@ void Ekf::fuseMag()
 
 		}
 
+		float KHP[_k_num_states][_k_num_states];
 		for (unsigned row = 0; row < _k_num_states; row++) {
 			for (unsigned column = 0; column < _k_num_states; column++) {
 				float tmp = KH[row][0] * P[0][column];
@@ -643,6 +644,7 @@ void Ekf::fuseHeading()
 		}
 	}
 
+	float KHP[_k_num_states][_k_num_states];
 	for (unsigned row = 0; row < _k_num_states; row++) {
 		for (unsigned column = 0; column < _k_num_states; column++) {
 			float tmp = KH[row][0] * P[0][column];
@@ -779,6 +781,7 @@ void Ekf::fuseDeclination()
 		}
 	}
 
+	float KHP[_k_num_states][_k_num_states];
 	for (unsigned row = 0; row < _k_num_states; row++) {
 		for (unsigned column = 0; column < _k_num_states; column++) {
 			float tmp = KH[row][16] * P[16][column];

EKF/optflow_fusion.cpp

@@ -439,6 +439,7 @@ void Ekf::fuseOptFlow()
 			}
 		}
 
+		float KHP[_k_num_states][_k_num_states];
 		for (unsigned row = 0; row < _k_num_states; row++) {
 			for (unsigned column = 0; column < _k_num_states; column++) {
 				float tmp = KH[row][0] * P[0][column];

EKF/vel_pos_fusion.cpp

@@ -239,6 +239,7 @@ void Ekf::fuseVelPosHeight()
 		}
 
 		// update covarinace matrix via Pnew = (I - KH)P
+		float KHP[_k_num_states][_k_num_states];
 		for (unsigned row = 0; row < _k_num_states; row++) {
 			for (unsigned column = 0; column < _k_num_states; column++) {
 				KHP[row][column] = Kfusion[row] * P[state_index][column];