fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-06-29 01:30:35 +08:00
Code Issues Packages Projects Releases Wiki Activity

ekf2: move generic functions to control.cpp

Browse Source 
these functions aren't specific to GPS fusion
This commit is contained in:
bresch
2021-10-29 15:24:27 +02:00
committed by Mathieu Bresciani
parent e90734881b
commit 9e54c6d1aa
2 changed files with 47 additions and 46 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/ekf2/EKF/control.cpp
+47
View File
@@ -1229,3 +1229,50 @@ void Ekf::controlAuxVelFusion()
}
}
bool Ekf::isVelStateAlignedWithObs() const
{
/* Do sanity check to see if the innovation failures is likely caused by a yaw angle error
* by measuring the angle between the velocity estimate and the last velocity observation
* Only use those vectors if their norm if they are larger than 4 times their noise standard deviation
*/
const float vel_obs_xy_norm_sq = _last_vel_obs.xy().norm_squared();
const float vel_state_xy_norm_sq = _state.vel.xy().norm_squared();
const float vel_obs_threshold_sq = fmaxf(sq(4.f) * (_last_vel_obs_var(0) + _last_vel_obs_var(1)), sq(0.4f));
const float vel_state_threshold_sq = fmaxf(sq(4.f) * (P(4, 4) + P(5, 5)), sq(0.4f));
if (vel_obs_xy_norm_sq > vel_obs_threshold_sq && vel_state_xy_norm_sq > vel_state_threshold_sq) {
const float obs_dot_vel = Vector2f(_last_vel_obs).dot(_state.vel.xy());
const float cos_sq = sq(obs_dot_vel) / (vel_state_xy_norm_sq * vel_obs_xy_norm_sq);
if (cos_sq < sq(cosf(math::radians(25.f))) || obs_dot_vel < 0.f) {
// The angle between the observation and the velocity estimate is greater than 25 degrees
return false;
}
}
return true;
}
bool Ekf::hasHorizontalAidingTimedOut() const
{
return isTimedOut(_time_last_hor_pos_fuse, _params.reset_timeout_max)
&& isTimedOut(_time_last_delpos_fuse, _params.reset_timeout_max)
&& isTimedOut(_time_last_hor_vel_fuse, _params.reset_timeout_max)
&& isTimedOut(_time_last_of_fuse, _params.reset_timeout_max);
}
void Ekf::processVelPosResetRequest()
{
if (_velpos_reset_request) {
resetVelocity();
resetHorizontalPosition();
_velpos_reset_request = false;
// Reset the timeout counters
_time_last_hor_pos_fuse = _time_last_imu;
_time_last_hor_vel_fuse = _time_last_imu;
}
}
src/modules/ekf2/EKF/gps_control.cpp
-46
View File
@@ -188,39 +188,6 @@ bool Ekf::shouldResetGpsFusion() const
return (is_reset_required || is_recent_takeoff_nav_failure || is_inflight_nav_failure);
}
bool Ekf::hasHorizontalAidingTimedOut() const
{
return isTimedOut(_time_last_hor_pos_fuse, _params.reset_timeout_max)
&& isTimedOut(_time_last_delpos_fuse, _params.reset_timeout_max)
&& isTimedOut(_time_last_hor_vel_fuse, _params.reset_timeout_max)
&& isTimedOut(_time_last_of_fuse, _params.reset_timeout_max);
}
bool Ekf::isVelStateAlignedWithObs() const
{
/* Do sanity check to see if the innovation failures is likely caused by a yaw angle error
* by measuring the angle between the velocity estimate and the last velocity observation
* Only use those vectors if their norm if they are larger than 4 times their noise standard deviation
*/
const float vel_obs_xy_norm_sq = _last_vel_obs.xy().norm_squared();
const float vel_state_xy_norm_sq = _state.vel.xy().norm_squared();
const float vel_obs_threshold_sq = fmaxf(sq(4.f) * (_last_vel_obs_var(0) + _last_vel_obs_var(1)), sq(0.4f));
const float vel_state_threshold_sq = fmaxf(sq(4.f) * (P(4, 4) + P(5, 5)), sq(0.4f));
if (vel_obs_xy_norm_sq > vel_obs_threshold_sq && vel_state_xy_norm_sq > vel_state_threshold_sq) {
const float obs_dot_vel = Vector2f(_last_vel_obs).dot(_state.vel.xy());
const float cos_sq = sq(obs_dot_vel) / (vel_state_xy_norm_sq * vel_obs_xy_norm_sq);
if (cos_sq < sq(cosf(math::radians(25.f))) || obs_dot_vel < 0.f) {
// The angle between the observation and the velocity estimate is greater than 25 degrees
return false;
}
}
return true;
}
void Ekf::processYawEstimatorResetRequest()
{
/* The yaw reset to the EKF-GSF estimate can be requested externally at any time during flight.
@@ -240,16 +207,3 @@ void Ekf::processYawEstimatorResetRequest()
}
}
}
void Ekf::processVelPosResetRequest()
{
if (_velpos_reset_request) {
resetVelocity();
resetHorizontalPosition();
_velpos_reset_request = false;
// Reset the timeout counters
_time_last_hor_pos_fuse = _time_last_imu;
_time_last_hor_vel_fuse = _time_last_imu;
}
}