diff --git a/EKF/airspeed_fusion.cpp b/EKF/airspeed_fusion.cpp index a6306ebc90..361afb6d7e 100644 --- a/EKF/airspeed_fusion.cpp +++ b/EKF/airspeed_fusion.cpp @@ -148,7 +148,7 @@ void Ekf::fuseAirspeed() _state.ang_error.setZero(); // Fuse airspeed measurement - fuse(Kfusion, _airspeed_innov); //Why calculate angle error when it is always zero? + fuse(Kfusion, _airspeed_innov); // correct the nominal quaternion Quaternion dq; @@ -156,10 +156,6 @@ void Ekf::fuseAirspeed() _state.quat_nominal = dq * _state.quat_nominal; _state.quat_nominal.normalize(); - // update covariance matrix via Pnew = (I - KH)P = P - KHP - float KH[_k_num_states][_k_num_states] = {}; - 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++) { // Here it will be a lot of zeros, should optimize that... KH[row][column] = Kfusion[row] * H_TAS[column];