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

ekf2: do not fuse ZVU if other velocity source is active

Browse Source
This commit is contained in:
bresch 2022-11-15 17:30:49 +01:00 committed by Daniel Agar
parent 228def6081
commit 72becdf407
3 changed files with 19 additions and 2 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

11
src/modules/ekf2/EKF/estimator_interface.cpp
View File

@ -528,6 +528,17 @@ bool EstimatorInterface::isHorizontalAidingActive() const
return getNumberOfActiveHorizontalAidingSources() > 0;
}
bool EstimatorInterface::isVerticalVelocityAidingActive() const
{
return getNumberOfActiveVerticalVelocityAidingSources() > 0;
}
int EstimatorInterface::getNumberOfActiveVerticalVelocityAidingSources() const
{
return int(_control_status.flags.gps)
+ int(_control_status.flags.ev_vel);
}
void EstimatorInterface::printBufferAllocationFailed(const char *buffer_name)
{
if (buffer_name) {

3
src/modules/ekf2/EKF/estimator_interface.h
View File

@ -181,6 +181,9 @@ public:
int getNumberOfActiveHorizontalAidingSources() const;
bool isVerticalVelocityAidingActive() const;
int getNumberOfActiveVerticalVelocityAidingSources() const;
const matrix::Quatf &getQuaternion() const { return _output_new.quat_nominal; }
// get the velocity of the body frame origin in local NED earth frame

7
src/modules/ekf2/EKF/zero_velocity_update.cpp
View File

@ -45,13 +45,16 @@ void Ekf::controlZeroVelocityUpdate()
if (zero_velocity_update_data_ready) {
const bool continuing_conditions_passing = _control_status.flags.vehicle_at_rest
&& _control_status_prev.flags.vehicle_at_rest;
&& _control_status_prev.flags.vehicle_at_rest
&& !isVerticalVelocityAidingActive(); // otherwise the filter is "too rigid" to follow a position drift
if (continuing_conditions_passing) {
Vector3f vel_obs{0, 0, 0};
Vector3f innovation = _state.vel - vel_obs;
const float obs_var = sq(0.001f);
// Set a low variance initially for faster leveling and higher
// later to let the states follow the measurements
const float obs_var = _control_status.flags.tilt_align ? sq(0.2f) : sq(0.001f);
Vector3f innov_var{
P(4, 4) + obs_var,
P(5, 5) + obs_var,