diff --git a/libraries/AP_NavEKF/EKFGSF_yaw.cpp b/libraries/AP_NavEKF/EKFGSF_yaw.cpp index 7d9e453c7548c..5a9a963f6cf46 100644 --- a/libraries/AP_NavEKF/EKFGSF_yaw.cpp +++ b/libraries/AP_NavEKF/EKFGSF_yaw.cpp @@ -103,7 +103,7 @@ void EKFGSF_yaw::update(const Vector3F &delAng, // Calculate a composite yaw as a weighted average of the states for each model. // To avoid issues with angle wrapping, the yaw state is converted to a vector with legnth // equal to the weighting value before it is summed. - Vector2F yaw_vector = {}; + Vector2F yaw_vector; for (uint8_t mdl_idx = 0; mdl_idx < N_MODELS_EKFGSF; mdl_idx++) { yaw_vector[0] += GSF.weights[mdl_idx] * cosF(EKF[mdl_idx].X[2]); yaw_vector[1] += GSF.weights[mdl_idx] * sinF(EKF[mdl_idx].X[2]); @@ -196,15 +196,15 @@ void EKFGSF_yaw::predictAHRS(const uint8_t mdl_idx) // Generate attitude solution using simple complementary filter for the selected model // Calculate 'k' unit vector of earth frame rotated into body frame - const Vector3F k(AHRS[mdl_idx].R[2][0], AHRS[mdl_idx].R[2][1], AHRS[mdl_idx].R[2][2]); + const Vector3F k{AHRS[mdl_idx].R[2][0], AHRS[mdl_idx].R[2][1], AHRS[mdl_idx].R[2][2]}; // Calculate angular rate vector in rad/sec averaged across last sample interval - Vector3F ang_rate_delayed_raw = delta_angle / angle_dt; + const Vector3F ang_rate_delayed_raw { delta_angle / angle_dt }; // Perform angular rate correction using accel data and reduce correction as accel magnitude moves away from 1 g (reduces drift when vehicle picked up and moved). // During fixed wing flight, compensate for centripetal acceleration assuming coordinated turns and X axis forward - Vector3F tilt_error_gyro_correction = {}; // (rad/sec) + Vector3F tilt_error_gyro_correction; // (rad/sec) if (accel_gain > 0.0f) { @@ -213,8 +213,11 @@ void EKFGSF_yaw::predictAHRS(const uint8_t mdl_idx) if (is_positive(true_airspeed)) { // Calculate centripetal acceleration in body frame from cross product of body rate and body frame airspeed vector // NOTE: this assumes X axis is aligned with airspeed vector - Vector3F centripetal_accel_vec_bf = Vector3F(0.0f, ang_rate_delayed_raw[2] * true_airspeed, - ang_rate_delayed_raw[1] * true_airspeed); - + const Vector3F centripetal_accel_vec_bf { + 0.0f, + ang_rate_delayed_raw[2] * true_airspeed, + - ang_rate_delayed_raw[1] * true_airspeed + }; // Correct measured accel for centripetal acceleration accel -= centripetal_accel_vec_bf; }