Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

AP_NavEKF: tidy creation of vectors #27241

Merged
merged 1 commit into from
Jun 10, 2024
Merged
Changes from all commits
Commits
File filter

Filter by extension

Filter by extension

Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
15 changes: 9 additions & 6 deletions libraries/AP_NavEKF/EKFGSF_yaw.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -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]);
Expand Down Expand Up @@ -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) {

Expand All @@ -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;
}
Expand Down
Loading