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

EKF angle constants in degrees for readability (#465)

Browse Source 
* EKF angle constants in degrees for readability

* EKF make FILTER_UPDATE_PERIOD_MS static constexpr and add FILTER_UPDATE_PERIOD_S

* EKF controlOpticalFlowFusion() use constants and update comments

* EKF controlMagFusion() use angle in degrees

* EKF move earth spin rate to geo and update usage

* EKF: Fix numerical constant error and clean up comments

Comments do not need to contain numerical values when the code makes these clear.
This commit is contained in:
Daniel Agar
2018-07-05 08:55:22 +10:00
committed by Paul Riseborough
parent cebdc3d829
commit 41953ab582
7 changed files with 29 additions and 29 deletions
Download Patch File Download Diff File Expand all files Collapse all files
EKF/ekf_helper.cpp
+5 -5
View File
@@ -793,7 +793,7 @@ void Ekf::constrainStates()
}
for (int i = 0; i < 3; i++) {
_state.gyro_bias(i) = math::constrain(_state.gyro_bias(i), -0.349066f * _dt_ekf_avg, 0.349066f * _dt_ekf_avg);
_state.gyro_bias(i) = math::constrain(_state.gyro_bias(i), -math::radians(20.f) * _dt_ekf_avg, math::radians(20.f) * _dt_ekf_avg);
}
for (int i = 0; i < 3; i++) {
@@ -814,11 +814,11 @@ void Ekf::constrainStates()
}
// calculate the earth rotation vector
void Ekf::calcEarthRateNED(Vector3f &omega, double lat_rad) const
void Ekf::calcEarthRateNED(Vector3f &omega, float lat_rad) const
{
omega(0) = _k_earth_rate * cosf((float)lat_rad);
omega(0) = CONSTANTS_EARTH_SPIN_RATE * cosf(lat_rad);
omega(1) = 0.0f;
omega(2) = -_k_earth_rate * sinf((float)lat_rad);
omega(2) = -CONSTANTS_EARTH_SPIN_RATE * sinf(lat_rad);
}
// gets the innovations of velocity and position measurements
@@ -1116,7 +1116,7 @@ bool Ekf::reset_imu_bias()
zeroRows(P, 10, 15);
// Set the corresponding variances to the values use for initial alignment
float dt = 0.001f * (float)FILTER_UPDATE_PERIOD_MS;
float dt = FILTER_UPDATE_PERIOD_S;
P[12][12] = P[11][11] = P[10][10] = sq(_params.switch_on_gyro_bias * dt);
P[15][15] = P[14][14] = P[13][13] = sq(_params.switch_on_accel_bias * dt);
_last_imu_bias_cov_reset_us = _imu_sample_delayed.time_us;