Skip to content

Commit

Permalink
AP_NavEKF: tidy creation of vectors
Browse files Browse the repository at this point in the history
  • Loading branch information
peterbarker authored and tridge committed Jun 10, 2024
1 parent 4fb8408 commit 2d7a4b3
Showing 1 changed file with 9 additions and 6 deletions.
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

0 comments on commit 2d7a4b3

Please sign in to comment.