fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-10-26 00:03:33 +08:00
Code Issues Packages Projects Releases Wiki Activity

Add const modifier

Browse Source 
const modifier


Add missing const modifier
This commit is contained in:
kamilritz
2020-07-16 18:25:06 +02:00
committed by Mathieu Bresciani
parent e357376797
commit b5e1397c0f
11 changed files with 65 additions and 70 deletions
Download Patch File Download Diff File Expand all files Collapse all files
EKF/EKFGSF_yaw.cpp
+11 -16
View File
@@ -236,10 +236,10 @@ void EKFGSF_yaw::ahrsAlignYaw()
} else {
// Calculate the 312 Tait-Bryan rotation sequence that rotates from earth to body frame
Vector3f rot312;
rot312(0) = wrap_pi(_ekf_gsf[model_index].X(2)); // first rotation (yaw) taken from EKF model state
rot312(1) = asinf(_ahrs_ekf_gsf[model_index].R(2, 1)); // second rotation (roll)
rot312(2) = atan2f(-_ahrs_ekf_gsf[model_index].R(2, 0), _ahrs_ekf_gsf[model_index].R(2, 2)); // third rotation (pitch)
const Vector3f rot312(wrap_pi(_ekf_gsf[model_index].X(2)), // yaw
asinf(_ahrs_ekf_gsf[model_index].R(2, 1)), // roll
atan2f(-_ahrs_ekf_gsf[model_index].R(2, 0),
_ahrs_ekf_gsf[model_index].R(2, 2))); // pitch
// Calculate the body to earth frame rotation matrix
_ahrs_ekf_gsf[model_index].R = taitBryan312ToRotMat(rot312);
@@ -312,7 +312,7 @@ void EKFGSF_yaw::predictEKF(const uint8_t model_index)
const float t15 = P22*t10;
const float t16 = P12+t15;
const float min_var = 1e-6f;
constexpr float min_var = 1e-6f;
_ekf_gsf[model_index].P(0,0) = fmaxf(P00-P20*t6+dvxVar*t14+dvyVar*t13-t6*t7 , min_var);
_ekf_gsf[model_index].P(0,1) = P01+t12-P21*t6+t7*t10-dvyVar*t2*t3;
_ekf_gsf[model_index].P(0,2) = t7;
@@ -356,17 +356,14 @@ bool EKFGSF_yaw::updateEKF(const uint8_t model_index)
// Update the inverse of the innovation covariance matrix S_inverse
updateInnovCovMatInv(model_index, S);
// Perform a chi-square innovation consistency test and calculate a compression scale factor that limits the magnitude of innovations to 5-sigma
float innov_comp_scale_factor = 1.0f;
// test ratio = transpose(innovation) * inverse(innovation variance) * innovation = [1x2] * [2,2] * [2,1] = [1,1]
const float test_ratio = _ekf_gsf[model_index].innov * (_ekf_gsf[model_index].S_inverse * _ekf_gsf[model_index].innov);
// Perform a chi-square innovation consistency test and calculate a compression scale factor
// that limits the magnitude of innovations to 5-sigma
// If the test ratio is greater than 25 (5 Sigma) then reduce the length of the innovation vector to clip it at 5-Sigma
// This protects from large measurement spikes
if (test_ratio > 25.0f) {
innov_comp_scale_factor = sqrtf(25.0f / test_ratio);
}
const float innov_comp_scale_factor = test_ratio > 25.f ? sqrtf(25.0f / test_ratio) : 1.f;
// calculate Kalman gain K nd covariance matrix P
// autocode from https://github.com/priseborough/3_state_filter/blob/flightLogReplay-wip/calcK.txt
@@ -377,13 +374,11 @@ bool EKFGSF_yaw::updateEKF(const uint8_t model_index)
const float t5 = P00*P11;
const float t9 = P01*P10;
const float t6 = t2+t3+t4+t5-t9;
float t7;
if (fabsf(t6) > 1e-6f) {
t7 = 1.0f/t6;
} else {
// skip this fusion step
if (fabsf(t6) < 1e-6f) {
// skip the fusion step
return false;
}
const float t7 = 1.f / t6;
const float t8 = P11+velObsVar;
const float t10 = P00+velObsVar;