EKF: save some stack memory

EKF/airspeed_fusion.cpp

@@ -161,6 +161,7 @@ void Ekf::fuseAirspeed()
 		// apply covariance correction via P_new = (I -K*H)*P
 		// first calculate expression for KHP
 		// then calculate P - KHP
+		float KH[_k_num_states][_k_num_states];
 		for (unsigned row = 0; row < _k_num_states; row++) {
 			KH[row][4] = Kfusion[row] * H_TAS[4];
 			KH[row][5] = Kfusion[row] * H_TAS[5];

EKF/ekf.h

@@ -238,7 +238,6 @@ private:
 	matrix::Dcm<float> _R_to_earth;	// transformation matrix from body frame to earth frame from last EKF predition
 
 	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 _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

@@ -309,6 +309,7 @@ void Ekf::fuseMag()
 		// apply covariance correction via P_new = (I -K*H)*P
 		// first calculate expression for KHP
 		// then calculate P - KHP
+		float KH[_k_num_states][_k_num_states];
 		for (unsigned row = 0; row < _k_num_states; row++) {
 			for (unsigned column = 0; column <= 3; column++) {
 				KH[row][column] = Kfusion[row] * H_MAG[column];
@@ -638,6 +639,7 @@ void Ekf::fuseHeading()
 	// apply covariance correction via P_new = (I -K*H)*P
 	// first calculate expression for KHP
 	// then calculate P - KHP
+	float KH[_k_num_states][_k_num_states];
 	for (unsigned row = 0; row < _k_num_states; row++) {
 		for (unsigned column = 0; column <= 3; column++) {
 			KH[row][column] = Kfusion[row] * H_YAW[column];
@@ -775,6 +777,7 @@ void Ekf::fuseDeclination()
 	// first calculate expression for KHP
 	// then calculate P - KHP
 	// take advantage of the empty columns in KH to reduce the number of operations
+	float KH[_k_num_states][_k_num_states];
 	for (unsigned row = 0; row < _k_num_states; row++) {
 		for (unsigned column = 16; column <= 17; column++) {
 			KH[row][column] = Kfusion[row] * H_DECL[column];

EKF/optflow_fusion.cpp

@@ -433,6 +433,7 @@ void Ekf::fuseOptFlow()
 		// apply covariance correction via P_new = (I -K*H)*P
 		// first calculate expression for KHP
 		// then calculate P - KHP
+		float KH[_k_num_states][_k_num_states];
 		for (unsigned row = 0; row < _k_num_states; row++) {
 			for (unsigned column = 0; column <= 6; column++) {
 				KH[row][column] = gain[row] * H_LOS[obs_index][column];