Mirrors_Framework/PX4-Autopilot
1
0
Fork 0
mirror of https://github.com/PX4/PX4-Autopilot.git synced 2026-05-23 22:58:10 +08:00
Code Issues Actions 15 Packages Projects Releases Wiki Activity

EKF: Non functional changes arising from review

Browse Source
This commit is contained in:
Paul Riseborough
2020-07-20 18:33:40 +10:00
committed by Paul Riseborough
parent b87778e61d
commit 6f664abc4a
3 changed files with 6 additions and 6 deletions
Download Patch File Download Diff File Expand all files Collapse all files
EKF/control.cpp
+2 -2
View File
@@ -898,7 +898,7 @@ void Ekf::checkVerticalAccelerationHealth()
// Don't use stale innovation data. // Don't use stale innovation data.
bool is_inertial_nav_falling = false; bool is_inertial_nav_falling = false;
bool are_vertical_pos_and_vel_independant = false; bool are_vertical_pos_and_vel_independant = false;
if (isRecent(_vert_pos_fuse_time_us, 1000000)) { if (isRecent(_vert_pos_fuse_attempt_time_us, 1000000)) {
if (isRecent(_vert_vel_fuse_time_us, 1000000)) { if (isRecent(_vert_vel_fuse_time_us, 1000000)) {
// If vertical position and velocity come from independent sensors then we can // If vertical position and velocity come from independent sensors then we can
// trust them more if they disagree with the IMU, but need to check that they agree // trust them more if they disagree with the IMU, but need to check that they agree
@@ -919,7 +919,7 @@ void Ekf::checkVerticalAccelerationHealth()
_imu_sample_delayed.delta_vel_clipping[1] || _imu_sample_delayed.delta_vel_clipping[1] ||
_imu_sample_delayed.delta_vel_clipping[2]; _imu_sample_delayed.delta_vel_clipping[2];
if (is_clipping &&_clip_counter < clip_count_limit) { if (is_clipping &&_clip_counter < clip_count_limit) {
_clip_counter++; _clip_counter++;
} else if (_clip_counter > 0) { } else if (_clip_counter > 0) {
_clip_counter--; _clip_counter--;
} }
EKF/ekf.h
+2 -2
View File
@@ -392,8 +392,8 @@ private:
Vector3f _last_vel_obs; ///< last velocity observation (m/s) Vector3f _last_vel_obs; ///< last velocity observation (m/s)
Vector2f _last_fail_hvel_innov; ///< last failed horizontal velocity innovation (m/s)**2 Vector2f _last_fail_hvel_innov; ///< last failed horizontal velocity innovation (m/s)**2
float _vert_pos_innov_ratio; ///< standard deviation of vertical position innovation float _vert_pos_innov_ratio; ///< vertical position innovation divided by estimated standard deviation of innovation
uint64_t _vert_pos_fuse_time_us; ///< last system time in usec vertical position measurement fuson was attempted uint64_t _vert_pos_fuse_attempt_time_us; ///< last system time in usec vertical position measurement fuson was attempted
float _vert_vel_innov_ratio; ///< standard deviation of vertical velocity innovation float _vert_vel_innov_ratio; ///< standard deviation of vertical velocity innovation
uint64_t _vert_vel_fuse_time_us; ///< last system time in usec time vertical velocity measurement fuson was attempted uint64_t _vert_vel_fuse_time_us; ///< last system time in usec time vertical velocity measurement fuson was attempted
EKF/vel_pos_fusion.cpp
+2 -2
View File
@@ -151,12 +151,12 @@ bool Ekf::fuseVerticalPosition(const Vector3f &innov, const Vector2f &innov_gate
innov_var(2) = P(9, 9) + obs_var(2); innov_var(2) = P(9, 9) + obs_var(2);
test_ratio(1) = sq(innov(2)) / (sq(innov_gate(1)) * innov_var(2)); test_ratio(1) = sq(innov(2)) / (sq(innov_gate(1)) * innov_var(2));
_vert_pos_innov_ratio = innov(2) / sqrtf(innov_var(2)); _vert_pos_innov_ratio = innov(2) / sqrtf(innov_var(2));
_vert_pos_fuse_time_us = _time_last_imu; _vert_pos_fuse_attempt_time_us = _time_last_imu;
bool innov_check_pass = test_ratio(1) <= 1.0f; bool innov_check_pass = test_ratio(1) <= 1.0f;
// if there is bad vertical acceleration data, then don't reject measurement, // if there is bad vertical acceleration data, then don't reject measurement,
// but limit innovation to prevent spikes that could destabilise the filter // but limit innovation to prevent spikes that could destabilise the filter
float innovation = innov(2); float innovation;
if (_bad_vert_accel_detected && !innov_check_pass) { if (_bad_vert_accel_detected && !innov_check_pass) {
const float innov_limit = innov_gate(1) * sqrtf(innov_var(2)); const float innov_limit = innov_gate(1) * sqrtf(innov_var(2));
innovation = math::constrain(innov(2), -innov_limit, innov_limit); innovation = math::constrain(innov(2), -innov_limit, innov_limit);