Mirror
/
ardupilot
mirror of https://github.com/ArduPilot/ardupilot
5
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

AP_NavEKF : remove the space around the operator

This commit is contained in:
RuffaloVM 2022-03-05 22:29:50 +09:00 committed by Randy Mackay
parent 302c8e4b98
commit 4d40ef5d0a
1 changed files with 9 additions and 9 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

18
libraries/AP_NavEKF/EKFGSF_yaw.cpp
View File

@ -93,7 +93,7 @@ void EKFGSF_yaw::update(const Vector3F &delAng,
}
// Always run the AHRS prediction cycle for each model
for (uint8_t mdl_idx = 0; mdl_idx < N_MODELS_EKFGSF; mdl_idx ++) {
for (uint8_t mdl_idx = 0; mdl_idx < N_MODELS_EKFGSF; mdl_idx++) {
predict(mdl_idx);
}
@ -105,7 +105,7 @@ void EKFGSF_yaw::update(const Vector3F &delAng,
// 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 = {};
for (uint8_t mdl_idx = 0; mdl_idx < N_MODELS_EKFGSF; mdl_idx ++) {
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]);
}
@ -114,7 +114,7 @@ void EKFGSF_yaw::update(const Vector3F &delAng,
// Example for future reference showing how a full GSF covariance matrix could be calculated if required
/*
memset(&GSF.P, 0, sizeof(GSF.P));
for (uint8_t mdl_idx = 0; mdl_idx < N_MODELS_EKFGSF; mdl_idx ++) {
for (uint8_t mdl_idx = 0; mdl_idx < N_MODELS_EKFGSF; mdl_idx++) {
ftype delta[3];
for (uint8_t row = 0; row < 3; row++) {
delta[row] = EKF[mdl_idx].X[row] - GSF.X[row];
@ -128,7 +128,7 @@ void EKFGSF_yaw::update(const Vector3F &delAng,
*/
GSF.yaw_variance = 0.0f;
for (uint8_t mdl_idx = 0; mdl_idx < N_MODELS_EKFGSF; mdl_idx ++) {
for (uint8_t mdl_idx = 0; mdl_idx < N_MODELS_EKFGSF; mdl_idx++) {
ftype yawDelta = wrap_PI(EKF[mdl_idx].X[2] - GSF.yaw);
GSF.yaw_variance += GSF.weights[mdl_idx] * (EKF[mdl_idx].P[2][2] + sq(yawDelta));
}
@ -144,7 +144,7 @@ void EKFGSF_yaw::fuseVelData(const Vector2F &vel, const ftype velAcc)
if (!vel_fuse_running) {
// Perform in-flight alignment
resetEKFGSF();
for (uint8_t mdl_idx = 0; mdl_idx < N_MODELS_EKFGSF; mdl_idx ++) {
for (uint8_t mdl_idx = 0; mdl_idx < N_MODELS_EKFGSF; mdl_idx++) {
// Use the firstGPS measurement to set the velocities and corresponding variances
EKF[mdl_idx].X[0] = vel[0];
EKF[mdl_idx].X[1] = vel[1];
@ -157,7 +157,7 @@ void EKFGSF_yaw::fuseVelData(const Vector2F &vel, const ftype velAcc)
ftype total_w = 0.0f;
ftype newWeight[(uint8_t)N_MODELS_EKFGSF];
bool state_update_failed = false;
for (uint8_t mdl_idx = 0; mdl_idx < N_MODELS_EKFGSF; mdl_idx ++) {
for (uint8_t mdl_idx = 0; mdl_idx < N_MODELS_EKFGSF; mdl_idx++) {
// Update states and covariances using GPS NE velocity measurements fused as direct state observations
if (!correct(mdl_idx, vel, velObsVar)) {
state_update_failed = true;
@ -168,7 +168,7 @@ void EKFGSF_yaw::fuseVelData(const Vector2F &vel, const ftype velAcc)
// Calculate weighting for each model assuming a normal error distribution
const ftype min_weight = 1e-5f;
uint8_t n_clips = 0;
for (uint8_t mdl_idx = 0; mdl_idx < N_MODELS_EKFGSF; mdl_idx ++) {
for (uint8_t mdl_idx = 0; mdl_idx < N_MODELS_EKFGSF; mdl_idx++) {
newWeight[mdl_idx] = gaussianDensity(mdl_idx) * GSF.weights[mdl_idx];
if (newWeight[mdl_idx] < min_weight) {
n_clips++;
@ -181,7 +181,7 @@ void EKFGSF_yaw::fuseVelData(const Vector2F &vel, const ftype velAcc)
// Reset the filters if all weights have underflowed due to excessive innovation variances
if (vel_fuse_running && n_clips < N_MODELS_EKFGSF) {
ftype total_w_inv = 1.0f / total_w;
for (uint8_t mdl_idx = 0; mdl_idx < N_MODELS_EKFGSF; mdl_idx ++) {
for (uint8_t mdl_idx = 0; mdl_idx < N_MODELS_EKFGSF; mdl_idx++) {
GSF.weights[mdl_idx] = newWeight[mdl_idx] * total_w_inv;
}
} else {
@ -641,7 +641,7 @@ bool EKFGSF_yaw::getVelInnovLength(ftype &velInnovLength) const
return false;
}
velInnovLength = 0.0f;
for (uint8_t mdl_idx = 0; mdl_idx < N_MODELS_EKFGSF; mdl_idx ++) {
for (uint8_t mdl_idx = 0; mdl_idx < N_MODELS_EKFGSF; mdl_idx++) {
velInnovLength += GSF.weights[mdl_idx] * sqrtF((sq(EKF[mdl_idx].innov[0]) + sq(EKF[mdl_idx].innov[1])));
}
return true;