ekf2: optimize KHP computation

Calculating K(HP) takes less operations than (KH)P because K and H are
vectors.

Without considering the sparsity optimization:
- KH (24*24 operations) is then a 24x24 matrix an it takes
24^3 operations to multiply it with P. Total: 14400 op

- HP (24*(24+24-1) operations) is a row vector
and it takes 24 operations to left-multiply it by K. Total:1152 op

src/modules/ekf2/EKF/ekf.h

@@ -742,24 +742,20 @@ private:
 	template <size_t ...Idxs>
 	SquareMatrix24f computeKHP(const Vector24f &K, const SparseVector24f<Idxs...> &H) const
 	{
-		SquareMatrix24f KHP;
+		// K(HP) and (KH)P are equivalent (matrix multiplication is associative)
-		constexpr size_t non_zeros = sizeof...(Idxs);
+		// but K(HP) is computationally much less expensive
-		float KH[non_zeros];
+		Vector24f HP;
-
+		for (unsigned i = 0; i < H.non_zeros(); i++) {
-		for (unsigned row = 0; row < _k_num_states; row++) {
+			const size_t row = H.index(i);
-			for (unsigned i = 0; i < H.non_zeros(); i++) {
+			for (unsigned col = 0; col < _k_num_states; col++) {
-				KH[i] = K(row) * H.atCompressedIndex(i);
+				HP(col) = HP(col) + H.atCompressedIndex(i) * P(row, col);
 			}
+		}
 
-			for (unsigned column = 0; column < _k_num_states; column++) {
+		SquareMatrix24f KHP;
-				float tmp = 0.f;
+		for (unsigned row = 0; row < _k_num_states; row++) {
-
+			for (unsigned col = 0; col < _k_num_states; col++) {
-				for (unsigned i = 0; i < H.non_zeros(); i++) {
+				KHP(row, col) = K(row) * HP(col);
-					const size_t index = H.index(i);
-					tmp += KH[i] * P(index, column);
-				}
-
-				KHP(row, column) = tmp;
 			}
 		}