Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-01-08 17:08:28 -04:00
Code Issues Packages Projects Releases Wiki Activity

AP_NavEKF: GSF white space fixes

Browse Source
This commit is contained in:
Randy Mackay 2020-04-23 13:11:03 +09:00 committed by Andrew Tridgell
parent ddaa5dee86
commit a43beaaa23
2 changed files with 27 additions and 27 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

24
libraries/AP_NavEKF/EKFGSF_yaw.cpp
View File

@ -159,16 +159,16 @@ void EKFGSF_yaw::update(const Vector3f &delAng,
/*
memset(&GSF.P, 0, sizeof(GSF.P));
for (uint8_t mdl_idx = 0; mdl_idx < N_MODELS_EKFGSF; mdl_idx ++) {
float delta[3];
for (uint8_t row = 0; row < 3; row++) {
delta[row] = EKF[mdl_idx].X[row] - GSF.X[row];
}
for (uint8_t row = 0; row < 3; row++) {
for (uint8_t col = 0; col < 3; col++) {
GSF.P[row][col] += GSF.weights[mdl_idx] * (EKF[mdl_idx].P[row][col] + delta[row] * delta[col]);
}
}
}
float delta[3];
for (uint8_t row = 0; row < 3; row++) {
delta[row] = EKF[mdl_idx].X[row] - GSF.X[row];
}
for (uint8_t row = 0; row < 3; row++) {
for (uint8_t col = 0; col < 3; col++) {
GSF.P[row][col] += GSF.weights[mdl_idx] * (EKF[mdl_idx].P[row][col] + delta[row] * delta[col]);
}
}
}
*/
GSF.yaw_variance = 0.0f;
@ -628,7 +628,7 @@ Matrix3f EKFGSF_yaw::updateRotMat(const Matrix3f &R, const Vector3f &g)
ret[1][0] += R[1][1] * g[2] - R[1][2] * g[1];
ret[1][1] += R[1][2] * g[0] - R[1][0] * g[2];
ret[1][2] += R[1][0] * g[1] - R[1][1] * g[0];
ret[2][0] += R[2][1] * g[2] - R[2][2] * g[1];
ret[2][0] += R[2][1] * g[2] - R[2][2] * g[1];
ret[2][1] += R[2][2] * g[0] - R[2][0] * g[2];
ret[2][2] += R[2][0] * g[1] - R[2][1] * g[0];
@ -655,4 +655,4 @@ bool EKFGSF_yaw::getYawData(float &yaw, float &yawVariance)
yaw = GSF.yaw;
yawVariance = GSF.yaw_variance;
return true;
}
}

30
libraries/AP_NavEKF/EKFGSF_yaw.h
View File

@ -48,10 +48,10 @@ private:
#endif
// Parameters
const float EKFGSF_gyroNoise{1.0e-1f}; // yaw rate noise used for covariance prediction (rad/sec)
const float EKFGSF_accelNoise{2.0f}; // horizontal accel noise used for covariance prediction (m/sec**2)
const float EKFGSF_tiltGain{0.2f}; // gain from tilt error to gyro correction for complementary filter (1/sec)
const float EKFGSF_gyroBiasGain{0.04f}; // gain applied to integral of gyro correction for complementary filter (1/sec)
const float EKFGSF_gyroNoise{1.0e-1f}; // yaw rate noise used for covariance prediction (rad/sec)
const float EKFGSF_accelNoise{2.0f}; // horizontal accel noise used for covariance prediction (m/sec**2)
const float EKFGSF_tiltGain{0.2f}; // gain from tilt error to gyro correction for complementary filter (1/sec)
const float EKFGSF_gyroBiasGain{0.04f}; // gain applied to integral of gyro correction for complementary filter (1/sec)
const float EKFGSF_accelFiltRatio{10.0f}; // ratio of time constant of AHRS tilt correction to time constant of first order LPF applied to accel data used by ahrs
// Declarations used by the bank of AHRS complementary filters that use IMU data augmented by true
@ -62,20 +62,20 @@ private:
float angle_dt;
float velocity_dt;
struct ahrs_struct {
Matrix3f R; // matrix that rotates a vector from body to earth frame
Vector3f gyro_bias; // gyro bias learned and used by the quaternion calculation
bool aligned{false}; // true when AHRS has been aligned
float accel_FR[2]; // front-right acceleration vector in a horizontal plane (m/s/s)
float vel_NE[2]; // NE velocity vector from last GPS measurement (m/s)
bool fuse_gps; // true when GPS should be fused on that frame
float accel_dt; // time step used when generating _simple_accel_FR data (sec)
Matrix3f R; // matrix that rotates a vector from body to earth frame
Vector3f gyro_bias; // gyro bias learned and used by the quaternion calculation
bool aligned{false}; // true when AHRS has been aligned
float accel_FR[2]; // front-right acceleration vector in a horizontal plane (m/s/s)
float vel_NE[2]; // NE velocity vector from last GPS measurement (m/s)
bool fuse_gps; // true when GPS should be fused on that frame
float accel_dt; // time step used when generating _simple_accel_FR data (sec)
};
ahrs_struct AHRS[N_MODELS_EKFGSF];
bool ahrs_tilt_aligned; // true the initial tilt alignment has been calculated
float accel_gain; // gain from accel vector tilt error to rate gyro correction used by AHRS calculation
Vector3f ahrs_accel; // filtered body frame specific force vector used by AHRS calculation (m/s/s)
float ahrs_accel_norm; // length of body frame specific force vector used by AHRS calculation (m/s/s)
bool ahrs_turn_comp_enabled; // true when compensation for centripetal acceleration in coordinated turns using true airspeed is being used.
float accel_gain; // gain from accel vector tilt error to rate gyro correction used by AHRS calculation
Vector3f ahrs_accel; // filtered body frame specific force vector used by AHRS calculation (m/s/s)
float ahrs_accel_norm; // length of body frame specific force vector used by AHRS calculation (m/s/s)
bool ahrs_turn_comp_enabled; // true when compensation for centripetal acceleration in coordinated turns using true airspeed is being used.
float true_airspeed; // true airspeed used to correct for centripetal acceleratoin in coordinated turns (m/s)
// Runs quaternion prediction for the selected AHRS using IMU (and optionally true airspeed) data