|
|
|
@ -93,7 +93,7 @@ void EKFGSF_yaw::update(const Vector3F &delAng,
@@ -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,
@@ -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,
@@ -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,
@@ -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)
@@ -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)
@@ -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)
@@ -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)
@@ -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
@@ -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; |
|
|
|
|