Skip to content

AP_Follow: fix vertical estimate not tracking the target #33109

New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Merged
rmackay9 merged 3 commits into from
May 22, 2026
Merged

AP_Follow: fix vertical estimate not tracking the target #33109

Changes from all commits
Commits
Show all changes
3 commits
Select commit Hold shift + click to select a range
7f7f4c2
AP_Follow: Fix vertical interpolation.
lthall May 19, 2026
146a791
AP_Follow: Fix comments
lthall May 22, 2026
6b38405
AP_Follow: Improve performance of default parameters
lthall May 22, 2026
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
27 changes: 15 additions & 12 deletions libraries/AP_Follow/AP_Follow.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -168,47 +168,47 @@ const AP_Param::GroupInfo AP_Follow::var_info[] = {
// @Range: 0 5
// @Units: m/s/s
// @User: Advanced
AP_GROUPINFO("_ACCEL_NE", 12, AP_Follow, _accel_max_ne_mss, 2.5),
AP_GROUPINFO("_ACCEL_NE", 12, AP_Follow, _accel_max_ne_mss, 5.0),

// @Param: _JERK_NE
// @DisplayName: Jerk limit for the horizontal kinematic input shaping
// @Description: Jerk limit of the horizontal kinematic path generation used to determine how quickly the estimate varies in acceleration
// @Range: 0 20
// @Units: m/s/s/s
// @User: Advanced
AP_GROUPINFO("_JERK_NE", 13, AP_Follow, _jerk_max_ne_msss, 5.0),
AP_GROUPINFO("_JERK_NE", 13, AP_Follow, _jerk_max_ne_msss, 10.0),

// @Param: _ACCEL_D
// @DisplayName: Acceleration limit for the vertical kinematic input shaping
// @Description: Acceleration limit of the vertical kinematic path generation used to determine how quickly the estimate varies in velocity
// @Range: 0 2.5
// @Units: m/s/s
// @User: Advanced
AP_GROUPINFO("_ACCEL_D", 14, AP_Follow, _accel_max_d_mss, 2.5),
AP_GROUPINFO("_ACCEL_D", 14, AP_Follow, _accel_max_d_mss, 5.0),

// @Param: _JERK_D
// @DisplayName: Jerk limit for the vertical kinematic input shaping
// @Description: Jerk limit of the vertical kinematic path generation used to determine how quickly the estimate varies in acceleration
// @Range: 0 5
// @Units: m/s/s/s
// @User: Advanced
AP_GROUPINFO("_JERK_D", 15, AP_Follow, _jerk_max_d_msss, 5.0),
AP_GROUPINFO("_JERK_D", 15, AP_Follow, _jerk_max_d_msss, 10.0),

// @Param: _ACCEL_H
// @DisplayName: Angular acceleration limit for the heading kinematic input shaping
// @Description: Angular acceleration limit of the heading kinematic path generation used to determine how quickly the estimate varies in angular velocity
// @Range: 0 90
// @Units: deg/s/s
// @User: Advanced
AP_GROUPINFO("_ACCEL_H", 16, AP_Follow, _accel_max_h_degss, 90.0),
AP_GROUPINFO("_ACCEL_H", 16, AP_Follow, _accel_max_h_degss, 360.0),

// @Param: _JERK_H
// @DisplayName: Angular jerk limit for the heading kinematic input shaping
// @Description: Angular jerk limit of the heading kinematic path generation used to determine how quickly the estimate varies in angular acceleration
// @Range: 0 360
// @Units: deg/s/s/s
// @User: Advanced
AP_GROUPINFO("_JERK_H", 17, AP_Follow, _jerk_max_h_degsss, 360.0),
AP_GROUPINFO("_JERK_H", 17, AP_Follow, _jerk_max_h_degsss, 720.0),

// @Param: _TIMEOUT
// @DisplayName: Follow timeout
Expand Down Expand Up @@ -272,14 +272,20 @@ void AP_Follow::update_estimates()
// update X/Y position, velocity, acceleration with shaping
update_pos_vel_accel_xy(_estimate_pos_ned_m.xy(), _estimate_vel_ned_ms.xy(), _estimate_accel_ned_mss.xy(), e_dt, Vector2f(), Vector2f(), Vector2f());

// update Z axis position, velocity, acceleration without shaping (direct update)
// integrate the Z axis estimate forward by e_dt
update_pos_vel_accel(_estimate_pos_ned_m.z, _estimate_vel_ned_ms.z, _estimate_accel_ned_mss.z, e_dt, 0.0, 0.0, 0.0);

// apply horizontal shaping to refine estimate toward projected target state
shape_pos_vel_accel_xy(_target_pos_ned_m.xy() + delta_pos_m.xy().topostype(), _target_vel_ned_ms.xy() + delta_vel_ms.xy(), _target_accel_ned_mss.xy(),
_estimate_pos_ned_m.xy(), _estimate_vel_ned_ms.xy(), _estimate_accel_ned_mss.xy(),
0.0, _accel_max_ne_mss, _jerk_max_ne_msss, e_dt, false);

// apply vertical shaping to refine the Z estimate toward the projected
// target state (jerk-limited, tuned via FOLL_ACCEL_D / FOLL_JERK_D)
shape_pos_vel_accel(_target_pos_ned_m.z + delta_pos_m.z, _target_vel_ned_ms.z + delta_vel_ms.z, _target_accel_ned_mss.z,
_estimate_pos_ned_m.z, _estimate_vel_ned_ms.z, _estimate_accel_ned_mss.z,
0.0, 0.0, -_accel_max_d_mss, _accel_max_d_mss, _jerk_max_d_msss, e_dt, false);
Comment thread
rmackay9 marked this conversation as resolved.

// apply angular shaping for heading estimate
shape_angle_vel_accel(radians(_target_heading_deg) + delta_heading_rad, radians(_target_heading_rate_degs), 0.0,
_estimate_heading_rad, _estimate_heading_rate_rads, _estimate_heading_accel_radss,
Expand Down Expand Up @@ -870,11 +876,8 @@ void AP_Follow::update_dist_and_bearing_to_target()
// if unable to retrieve local position, clear distance/bearing info
clear_dist_and_bearing_to_target();
} else {
// convert vehicle position to NED meters (NEU -> NED and cm -> m)
current_position_ned_m.z = -current_position_ned_m.z; // NEU to NED
current_position_ned_m *= 0.01; // convert cm to m

// calculate distance vectors to target, both with and without offsets
// get_relative_position_NED_origin() already returns metres in the NED
// frame, matching _ofs_estimate_pos_ned_m, so no conversion is required.
const Vector3p ofs_dist_vec = _ofs_estimate_pos_ned_m - current_position_ned_m;

// record distance and bearing to target for reporting/logging
Expand Down
Loading