Mirrors_Framework/ardupilot
1
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot.git synced 2026-10-06 19:00:27 +08:00
Code Issues Packages Projects Releases Wiki Activity

AC_PosControl: Improve EKF reset handling

Browse Source 
Co-authored-by: Randy Mackay <rmackay9@yahoo.com>
This commit is contained in:
Leonard Hall
2025-11-04 12:27:47 +09:00
committed by Randy Mackay
co-authored by Randy Mackay
parent 985ef73542
commit 7857d32d3f
2 changed files with 55 additions and 26 deletions
Download Patch File Download Diff File Expand all files Collapse all files
libraries/AC_AttitudeControl/AC_PosControl.cpp
+47 -26
View File
@@ -1672,23 +1672,34 @@ void AC_PosControl::init_ekf_NE_reset()
void AC_PosControl::handle_ekf_NE_reset()
{
// Check for EKF-reported NE position shift since last update
Vector2f pos_shift;
uint32_t reset_ms = _ahrs.getLastPosNorthEastReset(pos_shift);
Vector2f pos_shift_ne_m;
uint32_t reset_ms = _ahrs.getLastPosNorthEastReset(pos_shift_ne_m);
// todo: the actual difference in position and velocity estimation.
// This will prevent the need to pause error calculation for one cycle.
if (reset_ms != _ekf_ne_reset_ms) {
// Reset NE controller to preserve relative position control during Loiter, PosHold, etc.
// This ensures controller output remains continuous after EKF realigns the origin.
// ToDo: move EKF steps into the offsets for modes setting absolute position and velocity
// for this we need some sort of switch to select what type of EKF handling we want to use
// Reconstruct position target relative to the to new EKF estimation to maintain the current position error
Vector2p delta_pos_estimate_ne_m = _p_pos_ne_m.get_error().topostype() - (_pos_target_neu_m.xy() - _pos_estimate_neu_m.xy());
_pos_target_neu_m.xy() += delta_pos_estimate_ne_m;
// Reconstruct target and desired positions relative to new EKF origin
_pos_target_neu_m.xy() = _pos_estimate_neu_m.xy() + _p_pos_ne_m.get_error().topostype();
_pos_desired_neu_m.xy() = _pos_target_neu_m.xy() - _pos_offset_neu_m.xy();
// Reconstruct velocity targets based on PID error and velocity estimate
_vel_target_neu_ms.xy() = _vel_estimate_neu_ms.xy() + _pid_vel_ne_cm.get_error() * 0.01;
_vel_desired_neu_ms.xy() = _vel_target_neu_ms.xy() - _vel_offset_neu_ms.xy();
// Reconstruct velocity target relative to the to new EKF estimation to maintain the current velocity error
Vector2f delta_vel_estimate_ne_ms = _pid_vel_ne_cm.get_error() * 0.01 - (_vel_target_neu_ms.xy() - _vel_estimate_neu_ms.xy());
_vel_target_neu_ms.xy() += delta_vel_estimate_ne_ms;
switch (_ekf_reset_method) {
case EKFResetMethod::MoveTarget:
// Reset NE controller desired position and velocity to preserve actual position control during Loiter, PosHold, etc.
_pos_desired_neu_m.xy() += delta_pos_estimate_ne_m;
_vel_desired_neu_ms.xy() += delta_vel_estimate_ne_ms;
break;
case EKFResetMethod::MoveVehicle:
// Move the change in estimate into the offsest to move the aircraft to our new estimate smoothly during Auto, Guided, etc.
_pos_offset_neu_m.xy() += delta_pos_estimate_ne_m;
_vel_offset_neu_ms.xy() += delta_vel_estimate_ne_ms;
break;
}
_ekf_ne_reset_ms = reset_ms;
}
}
@@ -1704,23 +1715,33 @@ void AC_PosControl::init_ekf_U_reset()
void AC_PosControl::handle_ekf_U_reset()
{
// Check for EKF-reported Down-axis shift since last update
float alt_shift_d_m;
uint32_t reset_ms = _ahrs.getLastPosDownReset(alt_shift_d_m);
float pos_shift_d_m;
uint32_t reset_ms = _ahrs.getLastPosDownReset(pos_shift_d_m);
// todo: the actual difference in position and velocity estimation.
// This will prevent the need to pause error calculation for one cycle.
if (reset_ms != 0 && reset_ms != _ekf_u_reset_ms) {
// Reset U controller to preserve continuity during relative-altitude modes (e.g., Loiter, PosHold).
// Compensates for EKF origin shift without abrupt position or velocity discontinuities.
// This ensures controller output remains continuous after EKF realigns the origin.
// Reconstruct position target relative to the to new EKF estimation to maintain the current position error
postype_t delta_pos_estimate_u_m = _p_pos_u_m.get_error() - (_pos_target_neu_m.z - _pos_estimate_neu_m.z);
_pos_target_neu_m.z += delta_pos_estimate_u_m;
// ToDo: move EKF steps into the offsets for modes setting absolute position and velocity
// for this we need some sort of switch to select what type of EKF handling we want to use
// Reconstruct vertical position targets from measured altitude + P error
_pos_target_neu_m.z = _pos_estimate_neu_m.z + _p_pos_u_m.get_error();
_pos_desired_neu_m.z = _pos_target_neu_m.z - (_pos_offset_neu_m.z + _pos_terrain_u_m);
// Reconstruct vertical velocity targets from measured velocity + PID error
_vel_target_neu_ms.z = _vel_estimate_neu_ms.z + _pid_vel_u_cm.get_error() * 0.01;
_vel_desired_neu_ms.z = _vel_target_neu_ms.z - (_vel_offset_neu_ms.z + _vel_terrain_u_ms);
// Reconstruct velocity target relative to the to new EKF estimation to maintain the current velocity error
float delta_vel_estimate_u_ms = _pid_vel_u_cm.get_error() * 0.01 - (_vel_target_neu_ms.z - _vel_estimate_neu_ms.z);
_vel_target_neu_ms.z += delta_vel_estimate_u_ms;
switch (_ekf_reset_method) {
case EKFResetMethod::MoveTarget:
// Reset U controller desired position and velocity to preserve actual position control during Loiter, PosHold, etc.
_pos_desired_neu_m.z += delta_pos_estimate_u_m;
_vel_desired_neu_ms.z += delta_vel_estimate_u_ms;
break;
case EKFResetMethod::MoveVehicle:
// Move the change in estimate into the offsest to move the aircraft to our new estimate smoothly during Auto, Guided, etc.
_pos_offset_neu_m.z += delta_pos_estimate_u_m;
_vel_offset_neu_ms.z += delta_vel_estimate_u_ms;
break;
}
_ekf_u_reset_ms = reset_ms;
}
}
libraries/AC_AttitudeControl/AC_PosControl.h
+8
View File
@@ -566,6 +566,13 @@ public:
// When enabled, disables use of horizontal velocity estimates.
void set_vibe_comp(bool on_off) { _vibe_comp_enabled = on_off; }
// Reset handling method
enum class EKFResetMethod : uint8_t {
MoveTarget = 0, // the target position is reset so the vehicle does not physically move
MoveVehicle = 1 // the target position is smoothly transitioned so the vehicle moves to its previous position coordinates
};
void set_reset_handling_method(EKFResetMethod reset_method) { _ekf_reset_method = reset_method; }
// Returns confidence (0–1) in vertical control authority based on output usage.
// Used to assess throttle margin and PID effectiveness.
float get_vel_U_control_ratio() const { return constrain_float(_vel_u_control_ratio, 0.0f, 1.0f); }
@@ -749,6 +756,7 @@ protected:
// ekf reset handling
uint32_t _ekf_ne_reset_ms; // system time of last recorded ekf ne position reset
uint32_t _ekf_u_reset_ms; // system time of last recorded ekf altitude reset
EKFResetMethod _ekf_reset_method = EKFResetMethod::MoveTarget; // EKF reset handling method. Loiter should use MoveTarget, Auto should use MoveVehicle
// high vibration handling
bool _vibe_comp_enabled; // true when high vibration compensation is on