Just some restructuring to make the mechanical engineering student Milo angry

src/algorithms/kalmanFilter.cpp

@@ -65,6 +65,7 @@ KalmanState KalmanFilter::correct(KalmanState state, matrix z)
     matrix K = state.error * H_.T() * linalg::inv(H_ * state.error * H_.T() + R_);
     // Update the state based on the measurement
     state.x = state.x + K * (z - H_ * state.x); //TODO check transpose
+
     // Update the error covariance matrix
     state.error = (identity_ - K * H_) * state.error;
     return state;