Repository: ArduPilot/ardupilot
Pull request #29898: Follow kinematic shaping
Author: login: lthall
Labels: name: AllVehicles
Created: 2025-04-26T07:37:56Z | Merged: 2025-05-22T02:53:02Z

This pr provides a kinematic estimate of the target location and heading.

I have added time out's and resets based on the kinematic limits and errors.

We need to decide on how we choose between the different data message sources. Ideally I would like to change the name and extend the _ALT_TYPE parameter to be 0: GPI absolute, 1: GPI relative, 2: Follow Target (GPI = global position int).

We also need to decide what the options and default settings should be. We can leave the kinematic interpolation off by default, we may choose not to process acceleration to reduce noise.


Diff:
diff --git a/ArduCopter/mode_follow.cpp b/ArduCopter/mode_follow.cpp
index 181c20ecb5813..d68d8ca2e4216 100644
--- a/ArduCopter/mode_follow.cpp
+++ b/ArduCopter/mode_follow.cpp
@@ -47,6 +47,10 @@ void ModeFollow::run()
         return;
     }
 
+    // set offsets to current relative position if not already set
+    // this is done here to prevent the vehicle coliding with the target vehicle
+    g2.follow.init_offsets_if_required();
+
     // set motors to full range
     motors->set_desired_spool_state(AP_Motors::DesiredSpoolState::THROTTLE_UNLIMITED);
 
diff --git a/libraries/AP_Follow/AP_Follow.cpp b/libraries/AP_Follow/AP_Follow.cpp
index 72ee496826d95..e8512c36b813b 100644
--- a/libraries/AP_Follow/AP_Follow.cpp
+++ b/libraries/AP_Follow/AP_Follow.cpp
@@ -1,17 +1,27 @@
 /*
-   This program is free software: you can redistribute it and/or modify
-   it under the terms of the GNU General Public License as published by
-   the Free Software Foundation, either version 3 of the License, or
-   (at your option) any later version.
+    This program is free software: you can redistribute it and/or modify
+    it under the terms of the GNU General Public License as published by
+    the Free Software Foundation, either version 3 of the License, or
+    (at your option) any later version.
+
+    This program is distributed in the hope that it will be useful,
+    but WITHOUT ANY WARRANTY; without even the implied warranty of
+    MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+    GNU General Public License for more details.
+
+    You should have received a copy of the GNU General Public License
+    along with this program.  If not, see <http://www.gnu.org/licenses/>.
+*/
+
+//==============================================================================
+// AP_Follow Library
+// Target Following Logic for ArduPilot Vehicles
+//==============================================================================
 
-   This program is distributed in the hope that it will be useful,
-   but WITHOUT ANY WARRANTY; without even the implied warranty of
-   MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
-   GNU General Public License for more details.
 
-   You should have received a copy of the GNU General Public License
-   along with this program.  If not, see <http://www.gnu.org/licenses/>.
- */
+//==============================================================================
+// Includes
+//==============================================================================
 
 #include "AP_Follow_config.h"
 
@@ -26,10 +36,16 @@
 #include <AP_Logger/AP_Logger.h>
 #include <GCS_MAVLink/GCS.h>
 #include <AP_Vehicle/AP_Vehicle_Type.h>
+#include <AP_Scheduler/AP_Scheduler.h>
 
 extern const AP_HAL::HAL& hal;
 
-#define AP_FOLLOW_TIMEOUT_MS    3000    // position estimate timeout after 1 second
+
+//==============================================================================
+// Constants and Definitions
+//==============================================================================
+
+#define AP_FOLLOW_TIMEOUT_MS    3000    // position estimate timeout
 #define AP_FOLLOW_SYSID_TIMEOUT_MS 10000 // forget sysid we are following if we have not heard from them in 10 seconds
 
 #define AP_FOLLOW_OFFSET_TYPE_NED       0   // offsets are in north-east-down frame
@@ -40,11 +56,18 @@ extern const AP_HAL::HAL& hal;
 #define AP_FOLLOW_POS_P_DEFAULT 0.1f    // position error gain default
 
 #if APM_BUILD_TYPE(APM_BUILD_ArduPlane)
-#define AP_FOLLOW_ALT_TYPE_DEFAULT 0
+ #define AP_FOLLOW_ALT_TYPE_DEFAULT 0
+ #define AP_FOLLOW_DIST_MAX_DEFAULT 0
 #else
-#define AP_FOLLOW_ALT_TYPE_DEFAULT AP_FOLLOW_ALTITUDE_TYPE_RELATIVE
+ #define AP_FOLLOW_ALT_TYPE_DEFAULT AP_FOLLOW_ALTITUDE_TYPE_RELATIVE
+ #define AP_FOLLOW_DIST_MAX_DEFAULT 100
 #endif
 
+
+//==============================================================================
+// Constructor
+//==============================================================================
+
 AP_Follow *AP_Follow::_singleton;
 
 // table of user settable parameters
@@ -70,11 +93,11 @@ const AP_Param::GroupInfo AP_Follow::var_info[] = {
 
     // @Param: _DIST_MAX
     // @DisplayName: Follow distance maximum
-    // @Description: Follow distance maximum.  targets further than this will be ignored
+    // @Description: Follow distance maximum. If exceeded, the follow estimate will be considered invalid.
     // @Units: m
     // @Range: 1 1000
     // @User: Standard
-    AP_GROUPINFO("_DIST_MAX", 5, AP_Follow, _dist_max_m, 100),
+    AP_GROUPINFO("_DIST_MAX", 5, AP_Follow, _dist_max_m, AP_FOLLOW_DIST_MAX_DEFAULT),
 
     // @Param: _OFS_TYPE
     // @DisplayName: Follow offset type
@@ -85,7 +108,7 @@ const AP_Param::GroupInfo AP_Follow::var_info[] = {
 
     // @Param: _OFS_X
     // @DisplayName: Follow offsets in meters north/forward
-    // @Description: Follow offsets in meters north/forward.  If positive, this vehicle fly ahead or north of lead vehicle.  Depends on FOLL_OFS_TYPE
+    // @Description: Follow offsets in meters north/forward. If positive, this vehicle fly ahead or north of lead vehicle.  Depends on FOLL_OFS_TYPE
     // @Range: -100 100
     // @Units: m
     // @Increment: 1
@@ -93,7 +116,7 @@ const AP_Param::GroupInfo AP_Follow::var_info[] = {
 
     // @Param: _OFS_Y
     // @DisplayName: Follow offsets in meters east/right
-    // @Description: Follow offsets in meters east/right.  If positive, this vehicle will fly to the right or east of lead vehicle.  Depends on FOLL_OFS_TYPE
+    // @Description: Follow offsets in meters east/right. If positive, this vehicle will fly to the right or east of lead vehicle.  Depends on FOLL_OFS_TYPE
     // @Range: -100 100
     // @Units: m
     // @Increment: 1
@@ -101,12 +124,12 @@ const AP_Param::GroupInfo AP_Follow::var_info[] = {
 
     // @Param: _OFS_Z
     // @DisplayName: Follow offsets in meters down
-    // @Description: Follow offsets in meters down.  If positive, this vehicle will fly below the lead vehicle
+    // @Description: Follow offsets in meters down. If positive, this vehicle will fly below the lead vehicle
     // @Range: -100 100
     // @Units: m
     // @Increment: 1
     // @User: Standard
-    AP_GROUPINFO("_OFS", 7, AP_Follow, _offset_ned_m, 0),
+    AP_GROUPINFO("_OFS", 7, AP_Follow, _offset_m, 0),
 
 #if !(APM_BUILD_TYPE(APM_BUILD_Rover))
     // @Param: _YAW_BEHAVE
@@ -119,7 +142,7 @@ const AP_Param::GroupInfo AP_Follow::var_info[] = {
 
     // @Param: _POS_P
     // @DisplayName: Follow position error P gain
-    // @Description: Follow position error P gain.  Converts the difference between desired vertical speed and actual speed into a desired acceleration that is passed to the throttle acceleration controller
+    // @Description: Follow position error P gain. Converts the difference between desired vertical speed and actual speed into a desired acceleration that is passed to the throttle acceleration controller
     // @Range: 0.01 1.00
     // @Increment: 0.01
     // @User: Standard
@@ -141,412 +164,628 @@ const AP_Param::GroupInfo AP_Follow::var_info[] = {
     // @User: Standard
     AP_GROUPINFO("_OPTIONS", 11, AP_Follow, _options, 0),
 
+    // @Param: _ACCEL_NE
+    // @DisplayName: Acceleration limit for the horizontal kinematic input shaping
+    // @Description: Acceleration limit of the horizontal kinematic path generation used to determine how quickly the estimate varies in velocity
+    // @Range: 0 5
+    // @Units: m/s/s
+    // @User: Advanced
+    AP_GROUPINFO("_ACCEL_NE", 12, AP_Follow, _accel_max_ne_mss, 2.5),
+
+    // @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),
+
+    // @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),
+
+    // @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),
+
+    // @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),
+
+    // @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_GROUPEND
 };
 
-/* 
-   The constructor also initialises the proximity sensor. Note that this
-   constructor is not called until detect() returns true, so we
-   already know that we should setup the proximity sensor
-*/
+// Constructor for AP_Follow. Initializes the position P-controller and sets up parameter defaults.
 AP_Follow::AP_Follow() :
-        _p_pos(AP_FOLLOW_POS_P_DEFAULT)
+    _p_pos(AP_FOLLOW_POS_P_DEFAULT)
 {
     _singleton = this;
     AP_Param::setup_object_defaults(this, var_info);
 }
 
-// restore offsets to zero if necessary, should be called when vehicle exits follow mode
-void AP_Follow::clear_offsets_if_required()
+
+//==============================================================================
+// Target Estimation Update Functions
+//==============================================================================
+
+// Projects and updates the estimated target position, velocity, and heading based on last known data and configured input shaping.
+bool AP_Follow::update_estimate()
 {
-    if (_offsets_were_zero) {
-        _offset_ned_m.set(Vector3f());
-        _offsets_were_zero = false;
+    // check for target: if no valid target, invalidate estimate
+    if (!have_target()) {
+        clear_dist_and_bearing_to_target();
+        _estimate_valid = false;
+        return _estimate_valid;
     }
-}
 
-// get target's estimated location
-bool AP_Follow::get_target_location_and_velocity(Location &loc, Vector3f &vel_ned_ms) const
-{
-    Vector3p pos_ned_m;
-    Vector3f local_vel_ned_m;  // so we either change both return parameters or neither
-    if (!get_target_position_and_velocity_NED_m(pos_ned_m, local_vel_ned_m)) {
-        return false;
+    // skip if update has already been processed this scheduler tick
+    if (_estimate_valid && (AP::scheduler().ticks32() - _last_update_ticks == 0)) {
+        return _estimate_valid;
     }
-    if (!AP::ahrs().get_location_from_origin_offset_NED(loc, pos_ned_m)) {
-        return false;
+    _last_update_ticks = AP::scheduler().ticks32();
+
+    // if sysid changed, reset the estimation state
+    if (_sysid != _sysid_used) {
+        _sysid_used = _sysid;
+        _estimate_valid = false;
     }
-    vel_ned_ms = local_vel_ned_m;
 
-    if (_alt_type == AP_FOLLOW_ALTITUDE_TYPE_RELATIVE) {
-        loc.change_alt_frame(Location::AltFrame::ABOVE_HOME);
+    const uint32_t now = AP_HAL::millis();
+
+    // calculate time since last location update in seconds
+    const float dt = (now - _last_location_update_ms) * 0.001f;
+
+    // project target's position and velocity forward using simple kinematics
+    Vector3f delta_pos_m = _target_vel_ned_ms * dt + _target_accel_ned_mss * 0.5f * sq(dt);
+    Vector3f delta_vel_ms = _target_accel_ned_mss * dt;
+    float delta_heading_rad = radians(_target_heading_rate_degs) * dt;
+
+    // calculate time since last estimation update
+    const float e_dt = (now - _last_estimation_update_ms) * 0.001f;
+
+    const bool valid_kinematic_params = (_accel_max_ne_mss > 0.0f) && (_jerk_max_ne_msss > 0.0f) &&
+                                (_accel_max_d_mss > 0.0f) && (_jerk_max_d_msss > 0.0f) &&
+                                (_accel_max_h_degss > 0.0f) && (_jerk_max_h_degsss > 0.0f);
+    
+    if (_estimate_valid && valid_kinematic_params) {
+        // 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)
+        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 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,
+                              0.0, radians(_accel_max_h_degss),
+                              radians(_jerk_max_h_degsss), e_dt, false);
+
+        // update heading angle separately to maintain proper wrapping [-PI, PI]
+        postype_t estimate_heading_rad = _estimate_heading_rad;
+        update_pos_vel_accel(estimate_heading_rad, _estimate_heading_rate_rads, _estimate_heading_accel_radss, e_dt, 0.0, 0.0, 0.0);
+        _estimate_heading_rad = wrap_PI(estimate_heading_rad);
+    } else {
+        // no valid estimate yet: initialise from latest target position
+        _estimate_pos_ned_m = _target_pos_ned_m + delta_pos_m.topostype();
+        _estimate_vel_ned_ms = _target_vel_ned_ms + delta_vel_ms;
+        _estimate_accel_ned_mss = _target_accel_ned_mss;
+        _estimate_heading_rad = radians(_target_heading_deg) + delta_heading_rad;  
+        _estimate_heading_rate_rads = radians(_target_heading_rate_degs);  
+        _estimate_valid = true;
+    }
+
+    Vector3f offset_m = _offset_m.get();
+
+    // calculate estimated position and velocity with offsets applied
+    if (offset_m.is_zero() || (_offset_type == AP_FOLLOW_OFFSET_TYPE_NED)) {
+        // offsets are in NED frame: simple addition
+        _ofs_estimate_pos_ned_m = _estimate_pos_ned_m + offset_m.topostype();
+        _ofs_estimate_vel_ned_ms = _estimate_vel_ned_ms;
+        _ofs_estimate_accel_ned_mss = _estimate_accel_ned_mss;
+    } else {
+        // offsets are in FRD frame: rotate by heading
+        offset_m.xy().rotate(_estimate_heading_rad);
+        Vector3f offset_cross = offset_m.cross(Vector3f{0.0, 0.0, 1.0});
+        float offset_length_m = offset_m.length();
+        _ofs_estimate_pos_ned_m = _estimate_pos_ned_m + offset_m.topostype();
+        _ofs_estimate_vel_ned_ms = _estimate_vel_ned_ms + offset_cross * offset_length_m * _estimate_heading_rate_rads;
+        _ofs_estimate_accel_ned_mss = _estimate_accel_ned_mss + offset_cross * offset_length_m * _estimate_heading_accel_radss;
     }
 
-    return true;
-}
+    // update the distance and bearing to the target
+    update_dist_and_bearing_to_target();
 
-// get target's estimated location and velocity, both NED SI from origin
-bool AP_Follow::get_target_position_and_velocity_NED_m(Vector3p &pos_ned_m, Vector3f &vel_ned_ms) const
-{
-    // exit immediately if not enabled
-    if (!_enabled) {
+    _last_estimation_update_ms = now;
+
+    // Check if the target is within the maximum distance
+    Vector3f current_position_ned_m;
+    if (!AP::ahrs().get_relative_position_NED_origin(current_position_ned_m)) {
         return false;
     }
-
-    // check for timeout
-    if ((_last_location_update_ms == 0) || (AP_HAL::millis() - _last_location_update_ms > AP_FOLLOW_TIMEOUT_MS)) {
+    const Vector3p dist_vec_ned_m = _target_pos_ned_m - current_position_ned_m.topostype();
+    
+    // If _dist_max_m is not positive, we don't check the distance
+    if (is_positive(_dist_max_m.get()) && (dist_vec_ned_m.length() > _dist_max_m)) {
         return false;
     }
+    
+    return true;
+}
+
 
-    // calculate time since last actual position update
-    const float dt = (AP_HAL::millis() - _last_location_update_ms) * 0.001f;
+//==============================================================================
+// Target Information Retrieval Functions
+//==============================================================================
 
-    // get velocity estimate
-    if (!get_velocity_NED_ms(vel_ned_ms, dt)) {
+// Retrieves the estimated target position, velocity, and acceleration in the NED frame (relative to origin).
+bool AP_Follow::get_target_pos_vel_accel_NED_m(Vector3p &pos_ned_m, Vector3f &vel_ned_ms, Vector3f &accel_ned_mss)
+{
+    if (!update_estimate()) {
         return false;
     }
 
-    // project the vehicle position
-    const Vector3p vel_ned_p { vel_ned_ms.x, vel_ned_ms.y, vel_ned_ms.z };
-    pos_ned_m = _target_position_ned_m + vel_ned_p * dt;
+    pos_ned_m = _estimate_pos_ned_m;
+    vel_ned_ms = _estimate_vel_ned_ms;
+    accel_ned_mss = _estimate_accel_ned_mss;
 
     return true;
 }
 
-// get distance vector to target (in meters) and target's velocity all in NED frame
-bool AP_Follow::get_target_dist_and_vel_NED_m(Vector3f &dist_ned_m, Vector3f &dist_with_offs_m, Vector3f &vel_ned_ms)
+// Retrieves the estimated target position, velocity, and acceleration in the NED frame, including configured offsets.
+bool AP_Follow::get_ofs_pos_vel_accel_NED_m(Vector3p &pos_ofs_ned_m, Vector3f &vel_ofs_ned_ms, Vector3f &accel_ofs_ned_mss)
 {
-    Vector3f current_position_NED;
-    if (!AP::ahrs().get_relative_position_NED_origin(current_position_NED)) {
-        clear_dist_and_bearing_to_target();
-        return false;
-    }
-    Vector3p target_position_ned_m;
-    Vector3f veh_vel_ned_ms;  // NED
-    if (!get_target_position_and_velocity_NED_m(target_position_ned_m, veh_vel_ned_ms)) {
-        clear_dist_and_bearing_to_target();
+    if (!update_estimate()) {
         return false;
     }
 
-    // convert from Vector3p to Vector3f as they can't be subtracted:
-    Vector3f target_position_NED_f;
-    target_position_NED_f.x = target_position_ned_m.x;
-    target_position_NED_f.y = target_position_ned_m.y;
-    target_position_NED_f.z = target_position_ned_m.z;
+    pos_ofs_ned_m = _ofs_estimate_pos_ned_m;
+    vel_ofs_ned_ms = _ofs_estimate_vel_ned_ms;
+    accel_ofs_ned_mss = _ofs_estimate_accel_ned_mss;
 
-    const Vector3f dist_vec_ned_m = target_position_NED_f - current_position_NED;
+    return true;
+}
 
-    // fail if too far
-    if (is_positive(_dist_max_m.get()) && (dist_vec_ned_m.length() > _dist_max_m)) {
-        clear_dist_and_bearing_to_target();
+// Retrieves distance vectors (with and without configured offsets) and the target’s velocity, all in the NED frame.
+bool AP_Follow::get_target_dist_and_vel_NED_m(Vector3f &dist_ned, Vector3f &dist_with_offs, Vector3f &vel_ned)
+{
+    if (!update_estimate()) {
         return false;
     }
 
-    // initialise offsets_ned_m from distance vector if required
-    init_offsets_if_required(dist_vec_ned_m);
-
-    // get offsets_ned_m
-    Vector3f offsets_ned_m;
-    if (!get_offsets_NED_m(offsets_ned_m)) {
-        clear_dist_and_bearing_to_target();
+    Vector3f current_position_ned_m;
+    if (!AP::ahrs().get_relative_position_NED_origin(current_position_ned_m)) {
         return false;
     }
 
-    // calculate results
-    dist_ned_m = dist_vec_ned_m;
-    dist_with_offs_m = dist_vec_ned_m + offsets_ned_m;
-    vel_ned_ms = veh_vel_ned_ms;
+    const Vector3p dist_vec_ned_m = _estimate_pos_ned_m - current_position_ned_m.topostype();
+    const Vector3p ofs_dist_vec = _ofs_estimate_pos_ned_m - current_position_ned_m.topostype();
+    dist_ned = dist_vec_ned_m.tofloat();
+    dist_with_offs = ofs_dist_vec.tofloat();
+    vel_ned = _ofs_estimate_vel_ned_ms;
 
-    // record distance and heading_deg for reporting purposes
-    if (is_zero(dist_with_offs_m.x) && is_zero(dist_with_offs_m.y)) {
-        clear_dist_and_bearing_to_target();
-    } else {
-        _dist_to_target_m = safe_sqrt(sq(dist_with_offs_m.x) + sq(dist_with_offs_m.y));
-        _bearing_to_target_deg = degrees(atan2f(dist_with_offs_m.y, dist_with_offs_m.x));
+    return true;
+}
+
+// Retrieves the estimated target heading and heading rate in radians.
+bool AP_Follow::get_heading_heading_rate_rad(float &heading_rad, float &heading_rate_rads)
+{
+    if (!update_estimate()) {
+        return false;
     }
 
+    // return latest heading estimate
+    heading_rad = _estimate_heading_rad;
+    heading_rate_rads = _estimate_heading_rate_rads;
     return true;
 }
 
-// get target's heading_deg in degrees (0 = north, 90 = east)
-bool AP_Follow::get_target_heading_deg(float &heading_deg) const
+// Retrieves the target's estimated global location and velocity, adjusting altitude frame if relative mode is set (for LUA bindings).
+bool AP_Follow::get_target_location_and_velocity(Location &loc, Vector3f &vel_ned)
 {
-    // exit immediately if not enabled
-    if (!_enabled) {
+    if (!update_estimate()) {
         return false;
     }
 
-    // check for timeout
-    if ((_last_heading_update_ms == 0) || (AP_HAL::millis() - _last_heading_update_ms > AP_FOLLOW_TIMEOUT_MS)) {
+    if (!AP::ahrs().get_location_from_origin_offset_NED(loc, _estimate_pos_ned_m)) {
         return false;
     }
+    vel_ned = _estimate_vel_ned_ms;
+
+    if (_alt_type == AP_FOLLOW_ALTITUDE_TYPE_RELATIVE) {
+        loc.change_alt_frame(Location::AltFrame::ABOVE_HOME);
+    }
 
-    // return latest heading_deg estimate
-    heading_deg = _target_heading_deg;
     return true;
 }
 
-// returns true if we should extract information from msg
-bool AP_Follow::should_handle_message(const mavlink_message_t &msg) const
+// Retrieves the target's estimated global location and velocity, including configured offsets, for LUA bindings.
+bool AP_Follow::get_target_location_and_velocity_ofs(Location &loc, Vector3f &vel_ned)
 {
-    // exit immediately if not enabled
-    if (!_enabled) {
+    if (!update_estimate()) {
         return false;
     }
-
-    // skip our own messages
-    if (msg.sysid == mavlink_system.sysid) {
+    if (!AP::ahrs().get_location_from_origin_offset_NED(loc, _ofs_estimate_pos_ned_m)) {
         return false;
     }
+    if (_alt_type == AP_FOLLOW_ALTITUDE_TYPE_RELATIVE) {
+        loc.change_alt_frame(Location::AltFrame::ABOVE_HOME);
+    }
 
-    // skip message if not from our target
-    if (_sysid != 0 && msg.sysid != _sysid) {
+    vel_ned = _ofs_estimate_vel_ned_ms;
+    return true;
+}
+
+// Retrieves the estimated target heading in degrees (0° = North, 90° = East) for LUA bindings.
+bool AP_Follow::get_target_heading_deg(float &heading)
+{
+    if (!update_estimate()) {
         return false;
     }
 
+    // return latest heading estimate
+    heading = degrees(_estimate_heading_rad);
     return true;
 }
 
-// handle mavlink DISTANCE_SENSOR messages
+
+//==============================================================================
+// MAVLink Message Handling
+//==============================================================================
+
+// Handles incoming MAVLink messages to update the target's position, velocity, and heading.
 void AP_Follow::handle_msg(const mavlink_message_t &msg)
 {
-    // this method should be called from an "update()" method:
-    if (_automatic_sysid) {
-        // maybe timeout who we were following...
-        if ((_last_location_update_ms == 0) ||
-            (AP_HAL::millis() - _last_location_update_ms > AP_FOLLOW_SYSID_TIMEOUT_MS)) {
-            _sysid.set(0);
+    // Invalidate the estimate if no position update has been received within the timeout period.
+    // If using automatic sysid tracking, clear the sysid and reset tracking state.
+    if ((_last_location_update_ms == 0) ||
+        (AP_HAL::millis() - _last_location_update_ms > AP_FOLLOW_SYSID_TIMEOUT_MS)) {
+        if (_automatic_sysid) {
+            _sysid.set(0);         // clear target system ID
+            _sysid_used = 0;       // reset used sysid tracking
         }
+        _estimate_valid = false;   // mark estimate as invalid
+        _using_follow_target = false; // reset follow-target usage flag
     }
 
     if (!should_handle_message(msg)) {
+        // ignore message if filtering rules reject it (e.g., wrong sysid)
         return;
     }
 
-    // decode global-position-int message
+    // decode MAVLink message
     bool updated = false;
 
     switch (msg.msgid) {
     case MAVLINK_MSG_ID_GLOBAL_POSITION_INT: {
+        // handle standard global position messages
         updated = handle_global_position_int_message(msg);
         break;
     }
     case MAVLINK_MSG_ID_FOLLOW_TARGET: {
+        // handle follow-target specific messages
         updated = handle_follow_target_message(msg);
         break;
     }
     }
 
     if (updated) {
+        // Check if estimate needs reset based on position and velocity errors
+        if (estimate_error_too_large()) {
+            _estimate_valid = false;
+        }
+
 #if HAL_LOGGING_ENABLED
+        // log current follow diagnostic data
         Log_Write_FOLL();
 #endif
     }
 }
 
-bool AP_Follow::handle_global_position_int_message(const mavlink_message_t &msg)
+// Returns true if the incoming MAVLink message should be processed for target updates.
+bool AP_Follow::should_handle_message(const mavlink_message_t &msg) const
 {
-        // decode message
-        mavlink_global_position_int_t packet;
-        mavlink_msg_global_position_int_decode(&msg, &packet);
+    // exit immediately if not enabled
+    if (!_enabled) {
+        return false;
+    }
 
-        // ignore message if lat and lon are (exactly) zero
-        if ((packet.lat == 0 && packet.lon == 0)) {
-            return false;
-        }
+    // skip our own messages
+    if (msg.sysid == mavlink_system.sysid) {
+        return false;
+    }
 
-        Location _target_location;
-        _target_location.lat = packet.lat;
-        _target_location.lng = packet.lon;
+    // skip message if not from our target
+    if (_sysid != 0 && msg.sysid != _sysid) {
+        return false;
+    }
 
-        // select altitude source based on FOLL_ALT_TYPE param 
-        if (_alt_type == AP_FOLLOW_ALTITUDE_TYPE_RELATIVE) {
-            // above home alt
-            _target_location.set_alt_cm(packet.relative_alt / 10, Location::AltFrame::ABOVE_HOME);
-        } else {
-            // absolute altitude
-            _target_location.set_alt_cm(packet.alt / 10, Location::AltFrame::ABSOLUTE);
-        }
-        if (!_target_location.get_vector_from_origin_NEU(_target_position_ned_m)) {
-            return false;
-        }
-        _target_position_ned_m.z = -_target_position_ned_m.z; // NEU->NED
-        _target_position_ned_m *= 0.01;  // cm -> m
-
-        _target_velocity_ned_ms.x = packet.vx * 0.01f; // velocity north
-        _target_velocity_ned_ms.y = packet.vy * 0.01f; // velocity east
-        _target_velocity_ned_ms.z = packet.vz * 0.01f; // velocity down
-
-        // get a local timestamp with correction for transport jitter
-        _last_location_update_ms = _jitter.correct_offboard_timestamp_msec(packet.time_boot_ms, AP_HAL::millis());
-        if (packet.hdg <= 36000) {                  // heading_deg (UINT16_MAX if unknown)
-            _target_heading_deg = packet.hdg * 0.01f;   // convert centi-degrees to degrees
-            _last_heading_update_ms = _last_location_update_ms;
-        }
-        // initialise _sysid if zero to sender's id
-        if (_sysid == 0) {
-            _sysid.set(msg.sysid);
-            _automatic_sysid = true;
-        }
-        return true;
+    return true;
 }
 
-bool AP_Follow::handle_follow_target_message(const mavlink_message_t &msg)
+// Checks whether the current estimate should be reset based on position and velocity errors.
+bool AP_Follow::estimate_error_too_large() const
 {
-        // decode message
-        mavlink_follow_target_t packet;
-        mavlink_msg_follow_target_decode(&msg, &packet);
+    const float timeout_sec = AP_FOLLOW_TIMEOUT_MS * 0.001f;
 
-        // ignore message if lat and lon are (exactly) zero
-        if ((packet.lat == 0 && packet.lon == 0)) {
-            return false;
-        }
-        // require at least position
-        if ((packet.est_capabilities & (1<<0)) == 0) {
-            return false;
-        }
+    // Calculate position thresholds based on maximum acceleration then deceleration for the timeout duration
+    const float pos_thresh_horiz_m = _accel_max_ne_mss.get() * sq(timeout_sec * 0.5);
+    const float pos_thresh_vert_m  = _accel_max_d_mss.get()  * sq(timeout_sec * 0.5);
 
-        const Location new_loc {
-            packet.lat,
-            packet.lon,
-            int32_t(packet.alt * 100),  // m -> cm
-            Location::AltFrame::ABSOLUTE
-        };
+    // Calculate velocity thresholds using the new helper function
+    const float vel_thresh_horiz_ms = calc_max_velocity_change(_accel_max_ne_mss.get(), _jerk_max_ne_msss.get(), timeout_sec);
+    const float vel_thresh_vert_ms  = calc_max_velocity_change(_accel_max_d_mss.get(), _jerk_max_d_msss.get(), timeout_sec);
 
-        if (!new_loc.get_vector_from_origin_NEU(_target_position_ned_m)) {
-            return false;
-        }
-        _target_position_ned_m.z = -_target_position_ned_m.z; // NEU->NED
-        _target_position_ned_m *= 0.01;  // cm -> m
+    // Calculate current position and velocity errors
+    const Vector3f pos_error = _estimate_pos_ned_m.tofloat() - _target_pos_ned_m.tofloat();
+    const Vector3f vel_error = _estimate_vel_ned_ms - _target_vel_ned_ms;
 
-        if (packet.est_capabilities & (1<<1)) {
-            _target_velocity_ned_ms.x = packet.vel[0]; // velocity north
-            _target_velocity_ned_ms.y = packet.vel[1]; // velocity east
-            _target_velocity_ned_ms.z = packet.vel[2]; // velocity down
-        } else {
-            _target_velocity_ned_ms.zero();
-        }
+    const Vector2f pos_error_xy = pos_error.xy();
+    const float pos_error_z = pos_error.z;
+    const Vector2f vel_error_xy = vel_error.xy();
+    const float vel_error_z = vel_error.z;
 
-        // get a local timestamp with correction for transport jitter
-        _last_location_update_ms = _jitter.correct_offboard_timestamp_msec(packet.timestamp, AP_HAL::millis());
+    // Check horizontal and vertical separately
+    const bool pos_horiz_bad = pos_error_xy.length() > pos_thresh_horiz_m;
+    const bool vel_horiz_bad = vel_error_xy.length() > vel_thresh_horiz_ms;
+    const bool pos_vert_bad = fabsf(pos_error_z) > pos_thresh_vert_m;
+    const bool vel_vert_bad = fabsf(vel_error_z) > vel_thresh_vert_ms;
 
-        if (packet.est_capabilities & (1<<3)) {
-            Quaternion q{packet.attitude_q[0], packet.attitude_q[1], packet.attitude_q[2], packet.attitude_q[3]};
-            float r, p, y;
-            q.to_euler(r,p,y);
-            _target_heading_deg = degrees(y);
-            _last_heading_update_ms = _last_location_update_ms;
-        }
-
-        // initialise _sysid if zero to sender's id
-        if (_sysid == 0) {
-            _sysid.set(msg.sysid);
-            _automatic_sysid = true;
-        }
+    return pos_horiz_bad || vel_horiz_bad || pos_vert_bad || vel_vert_bad;
+}
 
-        return true;
+// Calculates max velocity change under trapezoidal or triangular acceleration profile (jerk-limited).
+float AP_Follow::calc_max_velocity_change(float accel_max, float jerk_max, float timeout_sec) const
+{
+    const float t_jerk = accel_max / jerk_max;
+    const float t_total_jerk = 2.0f * t_jerk;
+
+    if (timeout_sec >= t_total_jerk) {
+        // time to ramp up, constant accel phase, and ramp down
+        const float t_const = timeout_sec - t_total_jerk;
+        const float delta_v_jerk = 0.5f * accel_max * t_jerk;
+        const float delta_v_const = accel_max * t_const;
+        return 2.0f * delta_v_jerk + delta_v_const;
+    } else {
+        // timeout too short: pure triangle profile
+        const float t_half = timeout_sec * 0.5f;
+        return 0.5f * jerk_max * sq(t_half);
+    }
 }
 
-// write out an onboard-log message to help diagnose follow problems:
-#if HAL_LOGGING_ENABLED
-void AP_Follow::Log_Write_FOLL()
+// Decodes a GLOBAL_POSITION_INT MAVLink message to update the target’s position, velocity, and heading.
+bool AP_Follow::handle_global_position_int_message(const mavlink_message_t &msg)
 {
-        // get estimated location and velocity
-        Location loc_estimate{};
-        Vector3f vel_estimate_ned_ms;
-        UNUSED_RESULT(get_target_location_and_velocity(loc_estimate, vel_estimate_ned_ms));
-
-        Location _target_location;
-        UNUSED_RESULT(AP::ahrs().get_location_from_origin_offset_NED(_target_location, _target_position_ned_m * 0.01));
-        if (_alt_type == AP_FOLLOW_ALTITUDE_TYPE_RELATIVE) {
-            _target_location.change_alt_frame(Location::AltFrame::ABOVE_HOME);
-        }
+    // decode GLOBAL_POSITION_INT message into packet struct
+    mavlink_global_position_int_t packet;
+    mavlink_msg_global_position_int_decode(&msg, &packet);
+
+    // ignore message if latitude and longitude are exactly zero (invalid GPS fix)
+    if ((packet.lat == 0 && packet.lon == 0)) {
+        return false;
+    }
+
+    if (_using_follow_target) {
+        // if we are using follow_target, ignore global_position_int messages
+        return false;
+    }
+
+    Location _target_location;
+    _target_location.lat = packet.lat;
+    _target_location.lng = packet.lon;
+
+    // set target altitude based on configured altitude type
+    if (_alt_type == AP_FOLLOW_ALTITUDE_TYPE_RELATIVE) {
+        // use relative altitude above home
+        _target_location.set_alt_cm(packet.relative_alt / 10, Location::AltFrame::ABOVE_HOME);
+    } else {
+        // use absolute altitude
+        _target_location.set_alt_cm(packet.alt / 10, Location::AltFrame::ABSOLUTE);
+    }
 
-        // log lead's estimated vs reported position
-// @LoggerMessage: FOLL
-// @Description: Follow library diagnostic data
-// @Field: TimeUS: Time since system startup
-// @Field: Lat: Target latitude
-// @Field: Lon: Target longitude
-// @Field: Alt: Target absolute altitude
-// @Field: VelN: Target earth-frame velocity, North
-// @Field: VelE: Target earth-frame velocity, East
-// @Field: VelD: Target earth-frame velocity, Down
-// @Field: LatE: Vehicle latitude
-// @Field: LonE: Vehicle longitude
-// @Field: AltE: Vehicle absolute altitude
-        AP::logger().WriteStreaming("FOLL",
-                                               "TimeUS,Lat,Lon,Alt,VelN,VelE,VelD,LatE,LonE,AltE",  // labels
-                                               "sDUmnnnDUm",    // units
-                                               "F--B000--B",    // mults
-                                               "QLLifffLLi",    // fmt
-                                               AP_HAL::micros64(),
-                                               _target_location.lat,
-                                               _target_location.lng,
-                                               _target_location.alt,
-                                               (double)_target_velocity_ned_ms.x,
-                                               (double)_target_velocity_ned_ms.y,
-                                               (double)_target_velocity_ned_ms.z,
-                                               loc_estimate.lat,
-                                               loc_estimate.lng,
-                                               loc_estimate.alt
-                                               );
+    // convert global location to local NED frame position
+    if (!_target_location.get_vector_from_origin_NEU(_target_pos_ned_m)) {
+        return false;
+    }
+    _target_pos_ned_m.z = -_target_pos_ned_m.z; // convert NEU -> NED
+    _target_pos_ned_m *= 0.01;  // convert from cm to meters
+
+    // decode target velocity components (in m/s)
+    _target_vel_ned_ms.x = packet.vx * 0.01f; // velocity north
+    _target_vel_ned_ms.y = packet.vy * 0.01f; // velocity east
+    _target_vel_ned_ms.z = packet.vz * 0.01f; // velocity down
+
+    // target acceleration not available in GLOBAL_POSITION_INT
+    _target_accel_ned_mss.zero();
+
+    if (packet.hdg <= 36000) {
+        // valid heading field available (in centi-degrees)
+        _target_heading_deg = packet.hdg * 0.01f;
+    } else if (_offset_type == AP_FOLLOW_OFFSET_TYPE_RELATIVE) {
+        // heading missing but relative offset mode requires heading -> reject
+        return false;
+    } else {
+        // no heading available: set heading rate to zero
+        _target_heading_rate_degs = 0.0f;
+    }
+
+    // apply jitter-corrected timestamp to this update
+    _last_location_update_ms = _jitter.correct_offboard_timestamp_msec(packet.time_boot_ms, AP_HAL::millis());
+
+    // if sysid not yet set, adopt sender’s sysid and enable automatic sysid tracking
+    if (_sysid == 0) {
+        _sysid.set(msg.sysid);
+        _sysid_used = 0;
+        _estimate_valid = false;
+        _automatic_sysid = true;
+    }
+
+    return true;
 }
-#endif  // HAL_LOGGING_ENABLED
 
-// get velocity estimate in m/s in NED frame using dt since last update
-bool AP_Follow::get_velocity_NED_ms(Vector3f &vel_ned_ms, float dt) const
+// Decodes a FOLLOW_TARGET MAVLink message to update the target’s position, velocity, acceleration, and orientation.
+bool AP_Follow::handle_follow_target_message(const mavlink_message_t &msg)
 {
-    vel_ned_ms = _target_velocity_ned_ms + (_target_accel_ned_mss * dt);
+    // decode FOLLOW_TARGET message into packet struct
+    mavlink_follow_target_t packet;
+    mavlink_msg_follow_target_decode(&msg, &packet);
+
+    // ignore message if latitude and longitude are exactly zero (invalid GPS fix)
+    if ((packet.lat == 0 && packet.lon == 0)) {
+        return false;
+    }
+
+    // require that at least position is estimated (bit 0 of est_capabilities)
+    if ((packet.est_capabilities & (1<<0)) == 0) {
+        return false;
+    }
+
+    // build Location object from latitude, longitude, and altitude (alt in meters)
+    const Location _target_location {
+        packet.lat,
+        packet.lon,
+        int32_t(packet.alt * 100),  // convert meters to centimeters
+        Location::AltFrame::ABSOLUTE
+    };
+
+    // convert global location to local NED frame position
+    if (!_target_location.get_vector_from_origin_NEU(_target_pos_ned_m)) {
+        return false;
+    }
+    _target_pos_ned_m *= 0.01; // convert from cm to meters
+
+    // adjust Z coordinate to NED frame (NEU altitude -> NED)
+    Location origin;
+    if (!AP::ahrs().get_origin(origin)) {
+        return false;
+    }
+    _target_pos_ned_m.z = -packet.alt + origin.alt * 0.01;
+
+    // decode velocity if available (bit 1 of est_capabilities)
+    if (packet.est_capabilities & (1<<1)) {
+        _target_vel_ned_ms.x = packet.vel[0]; // velocity north
+        _target_vel_ned_ms.y = packet.vel[1]; // velocity east
+        _target_vel_ned_ms.z = packet.vel[2]; // velocity down
+    } else {
+        _target_vel_ned_ms.zero();
+    }
+
+    // decode acceleration if available (bit 2 of est_capabilities)
+    if (packet.est_capabilities & (1 << 2)) {
+        _target_accel_ned_mss.x = packet.acc[0]; // acceleration north
+        _target_accel_ned_mss.y = packet.acc[1]; // acceleration east
+        _target_accel_ned_mss.z = packet.acc[2]; // acceleration down
+    } else {
+        _target_accel_ned_mss.zero();
+    }
+
+    // decode attitude if available (bit 3 of est_capabilities)
+    if (packet.est_capabilities & (1 << 3)) {
+        // reconstruct quaternion from packet
+        Quaternion q{packet.attitude_q[0], packet.attitude_q[1], packet.attitude_q[2], packet.attitude_q[3]};
+        float roll, pitch, yaw;
+        q.to_euler(roll, pitch, yaw);
+
+        // store heading in degrees, wrapped 0–360
+        _target_heading_deg = wrap_360(degrees(yaw));
+
+        // transform body rates (roll, pitch, yaw) to earth frame rates
+        Matrix3f R;
+        q.rotation_matrix(R);
+        Vector3f omega_body = Vector3f{packet.rates[0], packet.rates[1], packet.rates[2]};
+        Vector3f omega_world = R * omega_body; // rotate rates into earth frame
+
+        // store heading rate (yaw rate in world frame) in degrees/sec
+        _target_heading_rate_degs = degrees(omega_world.z);
+    } else if (_offset_type == AP_FOLLOW_OFFSET_TYPE_RELATIVE) {
+        // if attitude unavailable and using relative frame, cannot compute — abort
+        return false;
+    } else {
+        // otherwise, default heading rate to zero
+        _target_heading_rate_degs = 0.0f;
+    }
+
+    // apply jitter-corrected timestamp to this update
+    _last_location_update_ms = _jitter.correct_offboard_timestamp_msec(packet.timestamp, AP_HAL::millis());
+
+    // if sysid not yet assigned, adopt sender's sysid and enable automatic sysid tracking
+    if (_sysid == 0) {
+        _sysid.set(msg.sysid);
+        _automatic_sysid = true;
+    }
+
+    // we are using follow_target: set sysid to sender's sysid
+    _using_follow_target = true;
+
     return true;
 }
 
-// initialise offsets_ned_m to provided distance vector to other vehicle (in meters in NED frame) if required
-void AP_Follow::init_offsets_if_required(const Vector3f &dist_vec_ned)
+
+//==============================================================================
+// Offset Initialization and Adjustment Functions
+//==============================================================================
+
+// Initializes the positional offsets from the target vehicle if not already set.
+void AP_Follow::init_offsets_if_required()
 {
-    // return immediately if offsets_ned_m have already been set
-    if (!_offset_ned_m.get().is_zero()) {
+    // return immediately if offsets have already been set
+    if (!_offset_m.get().is_zero()) {
         return;
     }
     _offsets_were_zero = true;
 
+    if (!update_estimate()) {
+        return;
+    }
+
+    // Check if the target is within the maximum distance
+    Vector3f current_position_ned_m;
+    if (!AP::ahrs().get_relative_position_NED_origin(current_position_ned_m)) {
+        return;
+    }
+    const Vector3f dist_vec_ned_m = (_target_pos_ned_m - current_position_ned_m.topostype()).tofloat();
+
     float target_heading_deg;
     if ((_offset_type == AP_FOLLOW_OFFSET_TYPE_RELATIVE) && get_target_heading_deg(target_heading_deg)) {
-        // rotate offsets_ned_m from north facing to vehicle's perspective
-        _offset_ned_m.set(rotate_vector(-dist_vec_ned, -target_heading_deg));
+        // rotate offset into vehicle-relative frame based on heading
+        _offset_m.set(rotate_vector(-dist_vec_ned_m, -target_heading_deg));
         GCS_SEND_TEXT(MAV_SEVERITY_INFO, "Relative follow offset loaded");
     } else {
-        // initialise offset in NED frame
-        _offset_ned_m.set(-dist_vec_ned);
-        // ensure offset_type used matches frame of offsets_ned_m saved
+        // initialize offset in NED frame (no heading rotation)
+        _offset_m.set(-dist_vec_ned_m);
+
+        // ensure offset type is set to NED frame if initialized this way
         _offset_type.set(AP_FOLLOW_OFFSET_TYPE_NED);
         GCS_SEND_TEXT(MAV_SEVERITY_INFO, "N-E-D follow offset loaded");
     }
 }
 
-// get offsets_ned_m in meters in NED frame
-bool AP_Follow::get_offsets_NED_m(Vector3f &offset) const
-{
-    const Vector3f &off = _offset_ned_m.get();
-
-    // if offsets_ned_m are zero or type is NED, simply return offset vector
-    if (off.is_zero() || (_offset_type == AP_FOLLOW_OFFSET_TYPE_NED)) {
-        offset = off;
-        return true;
-    }
-
-    // offset type is relative, exit if we cannot get vehicle's heading_deg
-    float target_heading_deg;
-    if (!get_target_heading_deg(target_heading_deg)) {
-        return false;
-    }
-
-    // rotate offsets_ned_m from vehicle's perspective to NED
-    offset = rotate_vector(off, target_heading_deg);
-    return true;
-}
-
-// rotate 3D vector clockwise by specified angle (in degrees)
+// Rotates a 3D vector clockwise by the specified angle (degrees).
 Vector3f AP_Follow::rotate_vector(const Vector3f &vec, float angle_deg) const
 {
     // rotate roll, pitch input from north facing to vehicle's perspective
@@ -556,50 +795,111 @@ Vector3f AP_Follow::rotate_vector(const Vector3f &vec, float angle_deg) const
     return ret;
 }
 
-// set recorded distance and bearing to target to zero
+// Resets follow mode offsets to zero if they were automatically initialized.
+void AP_Follow::clear_offsets_if_required()
+{
+    if (_offsets_were_zero) {
+        _offset_m.set(Vector3f());
+        _offsets_were_zero = false;
+    }
+}
+
+
+//==============================================================================
+// Distance and Bearing Management
+//==============================================================================
+
+// Resets the recorded distance and bearing to the target to zero.
 void AP_Follow::clear_dist_and_bearing_to_target()
 {
     _dist_to_target_m = 0.0f;
     _bearing_to_target_deg = 0.0f;
 }
 
-// get target's estimated origin-relative-position and velocity (both in SI NED), with offsets_ned_m added
-bool AP_Follow::get_target_position_and_velocity_ofs_NED_m(Vector3p &pos_ned_m, Vector3f &vel_ned_ms) const
+// Updates the recorded distance and bearing to the target to zero.
+void AP_Follow::update_dist_and_bearing_to_target()
 {
-    Vector3f ofs_ned;
-    if (!get_offsets_NED_m(ofs_ned)) {
-        return false;
-    }
-    if (!get_target_position_and_velocity_NED_m(pos_ned_m, vel_ned_ms)) {
-        return false;
+    Vector3f current_position_ned_m;
+    if (!AP::ahrs().get_relative_position_NED_origin(current_position_ned_m)) {
+        // 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
+        const Vector3p ofs_dist_vec = _ofs_estimate_pos_ned_m - current_position_ned_m.topostype();
+
+        // record distance and bearing to target for reporting/logging
+        if (ofs_dist_vec.xy().is_zero()) {
+            clear_dist_and_bearing_to_target();
+        } else {
+            _dist_to_target_m = ofs_dist_vec.xy().length();
+            _bearing_to_target_deg = degrees(ofs_dist_vec.xy().angle());
+        }
     }
-    // can't add Vector3p and Vector3f directly:
-    pos_ned_m.x += ofs_ned.x;
-    pos_ned_m.y += ofs_ned.y;
-    pos_ned_m.z += ofs_ned.z;
-    return true;
 }
 
-// get target's estimated location and velocity (in NED), with offsets_ned_m added
-bool AP_Follow::get_target_location_and_velocity_ofs(Location &loc, Vector3f &vel_ned_ms) const
+
+//==============================================================================
+// Logging
+//==============================================================================
+
+// Writes a diagnostic onboard log message containing target and vehicle tracking data for Follow mode.
+#if HAL_LOGGING_ENABLED
+void AP_Follow::Log_Write_FOLL()
 {
-    Vector3p pos_ned_m;
-    Vector3f local_vel_ned_m;  // so we either change both return parameters or neither
-    if (!get_target_position_and_velocity_ofs_NED_m(pos_ned_m, local_vel_ned_m)) {
-        return false;
-    }
-    if (!AP::ahrs().get_location_from_origin_offset_NED(loc, pos_ned_m)) {
-        return false;
-    }
-    if (_alt_type == AP_FOLLOW_ALTITUDE_TYPE_RELATIVE) {
-        loc.change_alt_frame(Location::AltFrame::ABOVE_HOME);
-    }
+    // retrieve latest estimated location and velocity
+    Location loc_estimate{};
+    Vector3f vel_estimate;
+    UNUSED_RESULT(get_target_location_and_velocity(loc_estimate, vel_estimate));
 
-    vel_ned_ms = local_vel_ned_m;
-    return true;
+    Location _target_location;
+    UNUSED_RESULT(AP::ahrs().get_location_from_origin_offset_NED(_target_location, _target_pos_ned_m));
+
+    if (_alt_type == AP_FOLLOW_ALTITUDE_TYPE_RELATIVE) {
+        _target_location.change_alt_frame(Location::AltFrame::ABOVE_HOME);
+    }
+
+    // log the lead target's reported position and vehicle's estimated position
+    // @LoggerMessage: FOLL
+    // @Description: Follow library diagnostic data
+    // @Field: TimeUS: Time since system startup (microseconds)
+    // @Field: Lat: Target latitude (degrees * 1E7)
+    // @Field: Lon: Target longitude (degrees * 1E7)
+    // @Field: Alt: Target absolute altitude (centimeters)
+    // @Field: VelN: Target velocity, North (m/s)
+    // @Field: VelE: Target velocity, East (m/s)
+    // @Field: VelD: Target velocity, Down (m/s)
+    // @Field: LatE: Vehicle estimated latitude (degrees * 1E7)
+    // @Field: LonE: Vehicle estimated longitude (degrees * 1E7)
+    // @Field: AltE: Vehicle estimated absolute altitude (centimeters)
+    AP::logger().WriteStreaming("FOLL",
+                                "TimeUS,Lat,Lon,Alt,VelN,VelE,VelD,LatE,LonE,AltE",  // labels
+                                "sDUmnnnDUm",    // units
+                                "F--B000--B",    // mults
+                                "QLLifffLLi",    // fmt
+                                AP_HAL::micros64(),
+                                _target_location.lat,
+                                _target_location.lng,
+                                _target_location.alt,
+                                (double)_target_vel_ned_ms.x,
+                                (double)_target_vel_ned_ms.y,
+                                (double)_target_vel_ned_ms.z,
+                                loc_estimate.lat,
+                                loc_estimate.lng,
+                                loc_estimate.alt
+                                );
 }
+#endif  // HAL_LOGGING_ENABLED
+
 
-// return true if we have a target
+//==============================================================================
+// Accessors and Helpers
+//==============================================================================
+
+// Returns true if following is enabled and a recent target update has been received.
 bool AP_Follow::have_target(void) const
 {
     if (!_enabled) {
@@ -613,13 +913,16 @@ bool AP_Follow::have_target(void) const
     return true;
 }
 
-namespace AP {
-
-AP_Follow &follow()
-{
-    return *AP_Follow::get_singleton();
-}
+//==============================================================================
+// AP_Follow Accessor
+//==============================================================================
 
+// Accessor for the AP_Follow singleton instance.
+namespace AP {
+    AP_Follow &follow()
+    {
+        return *AP_Follow::get_singleton();
+    }
 }
 
 #endif  // AP_FOLLOW_ENABLED
diff --git a/libraries/AP_Follow/AP_Follow.h b/libraries/AP_Follow/AP_Follow.h
index fd82fee1ca2db..7ef7f297e4f32 100644
--- a/libraries/AP_Follow/AP_Follow.h
+++ b/libraries/AP_Follow/AP_Follow.h
@@ -26,10 +26,18 @@
 #include <AC_PID/AC_P.h>
 #include <AP_RTC/JitterCorrection.h>
 
+//==============================================================================
+// AP_Follow Class
+// Target Following Logic for ArduPilot Vehicles
+//==============================================================================
+
 class AP_Follow
 {
 
 public:
+    //==========================================================================
+    // Public Enums
+    //==========================================================================
 
     // enum for FOLLOW_OPTIONS parameter
     enum class Option {
@@ -44,6 +52,10 @@ class AP_Follow
         YAW_BEHAVE_DIR_OF_FLIGHT = 3
     };
 
+    //==========================================================================
+    // Constructor and Singleton Access
+    //==========================================================================
+
     // constructor
     AP_Follow();
 
@@ -58,57 +70,70 @@ class AP_Follow
     // set which target to follow
     void set_target_sysid(uint8_t sysid) { _sysid.set(sysid); }
 
-    // restore offsets to zero if necessary, should be called when vehicle exits follow mode
+    // Resets the follow mode offsets to zero if they were automatically initialized. Should be called when exiting Follow mode.
     void clear_offsets_if_required();
 
-    //
-    // position tracking related methods
-    //
+    //==========================================================================
+    // Target Estimation and Tracking Methods
+    //==========================================================================
 
-    // true if we have a valid target location estimate
+    // Returns true if following is enabled and a recent target location update has been received.
     bool have_target() const;
 
-    // get target's estimated position (NED-from-origin) and velocity (in NED)
-    // from position-from-origin.  metres and metres/second
-    bool get_target_position_and_velocity_NED_m(Vector3p &pos_ned_m, Vector3f &vel_ned_ms) const;
+    // Projects the target’s position, velocity, and heading forward using the latest updates, smoothing with input shaping if necessary 
+    bool update_estimate();
 
-    // get target's estimated position (NED-from-origin) and velocity (in NED)
-    // from position-from-origin with offsets added.  metres and metres/second
-    bool get_target_position_and_velocity_ofs_NED_m(Vector3p &pos_ned, Vector3f &vel_ned) const;
+    // Retrieves the estimated target position, velocity, and acceleration in the NED frame relative to the origin (units: meters and meters/second).
+    bool get_target_pos_vel_accel_NED_m(Vector3p &pos_ned_m, Vector3f &vel_ned_ms, Vector3f &accel_ned_mss);
 
-    // get target's estimated location and velocity (in NED).  Derived
-    // from position-from-origin.
-    bool get_target_location_and_velocity(Location &loc, Vector3f &vel_ned) const;
+    // Retrieves the estimated target position, velocity, and acceleration in the NED frame, including configured positional offsets.
+    bool get_ofs_pos_vel_accel_NED_m(Vector3p &pos_ofs_ned_m, Vector3f &vel_ofs_ned_ms, Vector3f &accel_ofs_ned_mss);
 
-    // get target's estimated location and velocity (in NED), with
-    // offsets added.  Derived from position-from-origin.
-    bool get_target_location_and_velocity_ofs(Location &loc, Vector3f &vel_ned) const;
-    
-    // get distance vector to target (in meters), target plus offsets, and target's velocity all in NED frame
+    // Retrieves the estimated target heading and heading rate in radians.
+    bool get_heading_heading_rate_rad(float &heading_rad, float &heading_rate_rads);
+
+    //==========================================================================
+    // Global Location and Velocity Retrieval (LUA Bindings)
+    //==========================================================================
+
+    // Retrieves the estimated global location and velocity of the target. Adjusts altitude frame to relative if configured (for LUA bindings).
+    bool get_target_location_and_velocity(Location &loc, Vector3f &vel_ned);
+
+    // Retrieves the estimated global location and velocity of the target, including configured positional offsets (for LUA bindings).
+    bool get_target_location_and_velocity_ofs(Location &loc, Vector3f &vel_ned);
+
+    // Retrieves the estimated target heading in degrees (0° = North, 90° = East) for LUA bindings.
+    bool get_target_heading_deg(float &heading);
+
+    // Retrieves the distance vector to the target, the distance vector including configured offsets, and the target’s velocity in the NED frame (units: meters).
     bool get_target_dist_and_vel_NED_m(Vector3f &dist_ned, Vector3f &dist_with_ofs, Vector3f &vel_ned);
 
+    //==========================================================================
+    // Accessor Methods
+    //==========================================================================
+
     // get target sysid
     uint8_t get_target_sysid() const { return _sysid.get(); }
 
     // get position controller.  this controller is not used within this library but it is convenient to hold it here
     const AC_P& get_pos_p() const { return _p_pos; }
 
-    //
-    // yaw/heading related methods
-    //
-
     // get user defined yaw behaviour
     YawBehave get_yaw_behave() const { return (YawBehave)_yaw_behave.get(); }
 
-    // get target's heading in degrees (0 = north, 90 = east)
-    bool get_target_heading_deg(float &heading) const;
+    // initialise offsets to provided distance vector to other vehicle (in meters in NED frame) if required
+    void init_offsets_if_required();
+
+    //==========================================================================
+    // MAVLink Message Handling
+    //==========================================================================
 
     // parse mavlink messages which may hold target's position, velocity and attitude
     void handle_msg(const mavlink_message_t &msg);
 
-    //
-    // GCS reporting functions
-    //
+    //==========================================================================
+    // GCS Reporting Methods
+    //==========================================================================
 
     // get horizontal distance to target (including offset) in meters (for reporting purposes)
     float get_distance_to_target_m() const { return _dist_to_target_m; }
@@ -117,34 +142,42 @@ class AP_Follow
     float get_bearing_to_target_deg() const { return _bearing_to_target_deg; }
 
     // get system time of last position update
+    // LUA bindings
     uint32_t get_last_update_ms() const { return _last_location_update_ms; }
 
     // returns true if a follow option enabled
     bool option_is_enabled(Option option) const { return (_options.get() & (uint16_t)option) != 0; }
 
+    //==========================================================================
+    // Parameter Group Info
+    //==========================================================================
+
     // parameter list
     static const struct AP_Param::GroupInfo var_info[];
 
 private:
+    //==========================================================================
+    // Private Helper Functions and Singleton
+    //==========================================================================
+
+    // Singleton instance of the AP_Follow class.
     static AP_Follow *_singleton;
 
     // returns true if we should extract information from msg
     bool should_handle_message(const mavlink_message_t &msg) const;
 
-    // get velocity estimate in m/s in NED frame using dt since last update
-    bool get_velocity_NED_ms(Vector3f &vel_ned, float dt) const;
-
-    // initialise offsets to provided distance vector to other vehicle (in meters in NED frame) if required
-    void init_offsets_if_required(const Vector3f &dist_vec_ned);
-
-    // get offsets in meters in NED frame
-    bool get_offsets_NED_m(Vector3f &offsets) const;
+    // Checks whether the current estimate should be reset based on position and velocity errors.
+    bool estimate_error_too_large() const;
+    
+    // Calculates max velocity change under trapezoidal or triangular acceleration profile (jerk-limited).
+    float calc_max_velocity_change(float accel_max, float jerk_max, float timeout_sec) const;
 
-    // rotate 3D vector clockwise by specified angle (in degrees)
+    // Rotates a 3D vector clockwise by the specified angle in degrees.
     Vector3f rotate_vector(const Vector3f &vec, float angle_deg) const;
 
-    // set recorded distance and bearing to target to zero
+    // Resets the recorded distance and bearing to the target to zero.
     void clear_dist_and_bearing_to_target();
+    void update_dist_and_bearing_to_target();
 
     // handle various mavlink messages supplying position:
     bool handle_global_position_int_message(const mavlink_message_t &msg);
@@ -153,33 +186,72 @@ class AP_Follow
     // write out an onboard-log message to help diagnose follow problems:
     void Log_Write_FOLL();
 
-    // parameters
-    AP_Int8     _enabled;           // 1 if this subsystem is enabled
-    AP_Int16    _sysid;             // target's mavlink system id (0 to use first sysid seen)
-    AP_Float    _dist_max_m;          // maximum distance to target.  targets further than this will be ignored
-    AP_Int8     _offset_type;       // offset frame type (0:North-East-Down, 1:RelativeToLeadVehicleHeading)
-    AP_Vector3f _offset_ned_m;            // offset from lead vehicle in meters
-    AP_Int8     _yaw_behave;        // following vehicle's yaw/heading behaviour (see YAW_BEHAVE enum)
-    AP_Int8     _alt_type;          // altitude source for follow mode
-    AC_P        _p_pos;             // position error P controller
-    AP_Int16    _options;           // options for mount behaviour follow mode
-
-    // local variables
-    uint32_t _last_location_update_ms;  // system time of last position update
-    Vector3p _target_position_ned_m;  // last known position of target
-    Vector3f _target_velocity_ned_ms;  // last known velocity of target in NED frame in m/s
-    Vector3f _target_accel_ned_mss;     // last known acceleration of target in NED frame in m/s/s
-    uint32_t _last_heading_update_ms;   // system time of last heading update
-    float _target_heading_deg;          // heading in degrees
-    bool _automatic_sysid;          // did we lock onto a sysid automatically?
-    float _dist_to_target_m;          // latest distance to target in meters (for reporting purposes)
-    float _bearing_to_target_deg;       // latest bearing to target in degrees (for reporting purposes)
-    bool _offsets_were_zero;        // true if offsets were originally zero and then initialised to the offset from lead vehicle
-
-    // setup jitter correction with max transport lag of 3s
-    JitterCorrection _jitter{3000};
+    //==========================================================================
+    // Parameters
+    //==========================================================================
+
+    AP_Int8     _enabled;           // 1 = Follow mode is enabled; 0 = disabled
+    AP_Int16    _sysid;             // MAVLink system ID of the target (0 = auto-select first sender)
+    AP_Float    _dist_max_m;        // Maximum allowed distance to target in meters; if exceeded, estimation is rejected
+    AP_Int8     _offset_type;       // Offset frame type: 0 = NED, 1 = relative to lead vehicle heading
+    AP_Vector3f _offset_m;          // Offset from lead vehicle (meters), in NED or FRD frame depending on _offset_type
+    AP_Int8     _yaw_behave;        // Yaw behavior mode (see YawBehave enum)
+    AP_Int8     _alt_type;          // Altitude reference: 0 = absolute, 1 = relative to home
+    AC_P        _p_pos;             // Position error P-controller for optional altitude following
+    AP_Int16    _options;           // Bitmask of follow behavior options (e.g., mount follow, etc.)
+
+    AP_Float    _accel_max_ne_mss;  // Max horizontal acceleration for kinematic shaping (m/s²)
+    AP_Float    _jerk_max_ne_msss;  // Max horizontal jerk for kinematic shaping (m/s³)
+    AP_Float    _accel_max_d_mss;   // Max vertical acceleration for kinematic shaping (m/s²)
+    AP_Float    _jerk_max_d_msss;   // Max vertical jerk for kinematic shaping (m/s³)
+    AP_Float    _accel_max_h_degss; // Max angular acceleration for heading shaping (deg/s²)
+    AP_Float    _jerk_max_h_degsss; // Max angular jerk for heading shaping (deg/s³)
+
+    //==========================================================================
+    // Internal State Variables
+    //==========================================================================
+
+    uint32_t    _last_location_update_ms;       // Time of last target position update (ms)
+    uint32_t    _last_estimation_update_ms;     // Time of last estimate update (ms)
+    uint32_t    _last_update_ticks;             // Scheduler tick count at last estimate update
+
+    Vector3p    _target_pos_ned_m;              // Latest received target position (NED frame, meters)
+    Vector3f    _target_vel_ned_ms;             // Latest received target velocity (NED frame, m/s)
+    Vector3f    _target_accel_ned_mss;          // Latest received target acceleration (NED frame, m/s²)
+    float       _target_heading_deg;            // Latest received target heading (degrees, 0 = North)
+    float       _target_heading_rate_degs;      // Latest received target yaw rate (deg/s)
+
+    bool        _estimate_valid;                // True if internal estimate is valid and in sync with updates
+    Vector3p    _estimate_pos_ned_m;            // Estimated target position (NED frame, meters)
+    Vector3f    _estimate_vel_ned_ms;           // Estimated target velocity (NED frame, m/s)
+    Vector3f    _estimate_accel_ned_mss;        // Estimated target acceleration (NED frame, m/s²)
+    float       _estimate_heading_rad;          // Estimated heading (radians, range -PI to PI)
+    float       _estimate_heading_rate_rads;    // Estimated heading rate (rad/s)
+    float       _estimate_heading_accel_radss;  // Estimated heading acceleration (rad/s²)
+
+    Vector3p    _ofs_estimate_pos_ned_m;        // Estimated position with offsets applied (NED frame)
+    Vector3f    _ofs_estimate_vel_ned_ms;       // Estimated velocity with offsets applied (NED frame)
+    Vector3f    _ofs_estimate_accel_ned_mss;    // Estimated acceleration with offsets applied (NED frame)
+
+    bool        _automatic_sysid;               // True if target sysid was automatically selected
+    int16_t     _sysid_used;                    // Currently active sysid used for updates
+    float       _dist_to_target_m;              // Horizontal distance to target, for reporting (meters)
+    float       _bearing_to_target_deg;         // Bearing to target from vehicle (degrees, 0 = North)
+    bool        _offsets_were_zero;             // True if initial offset was zero before being initialized
+    bool        _using_follow_target;           // True if FOLLOW_TARGET messages are being used instead of GLOBAL_POSITION_INT
+
+    //==========================================================================
+    // Utilities
+    //==========================================================================
+
+    // Jitter correction helper for smoothing offboard timestamps.
+    JitterCorrection _jitter{500};
 };
 
+//==============================================================================
+// AP_Follow Accessor
+//==============================================================================
+
 namespace AP {
     AP_Follow &follow();
 };
diff --git a/libraries/AP_Math/control.cpp b/libraries/AP_Math/control.cpp
index 3c84e8a299b16..a1b7d0b46f804 100644
--- a/libraries/AP_Math/control.cpp
+++ b/libraries/AP_Math/control.cpp
@@ -197,7 +197,7 @@ void shape_vel_accel(float vel_input, float accel_input,
     // velocity correction with input velocity
     accel_target += accel_input;
 
-    // constrain total acceleration from accel_min to accel_max
+    // Constrain total acceleration if limiting is enabled
     if (limit_total_accel) {
         accel_target = constrain_float(accel_target, accel_min, accel_max);
     }
@@ -252,7 +252,7 @@ void shape_vel_accel_xy(const Vector2f& vel_input, const Vector2f& accel_input,
 
     accel_target += accel_input;
 
-    // limit total acceleration to accel_max
+    // Constrain total acceleration if limiting is enabled 
     if (limit_total_accel) {
         accel_target.limit_length(accel_max);
     }
@@ -312,8 +312,8 @@ void shape_pos_vel_accel(postype_t pos_input, float vel_input, float accel_input
     // velocity correction with input velocity
     vel_target += vel_input;
 
-    // limit velocity between vel_min and vel_max
-    if (limit_total) {
+    // Constrain total velocity if limiting is enabled and the velocity range is valid (non-zero and min < max)
+    if (limit_total && (vel_max > vel_min)) {
         vel_target = constrain_float(vel_target, vel_min, vel_max);
     }
 
@@ -351,14 +351,68 @@ void shape_pos_vel_accel_xy(const Vector2p& pos_input, const Vector2f& vel_input
     // velocity correction with input velocity
     vel_target = vel_target + vel_input;
     
-    // limit velocity to vel_max
-    if (limit_total) {
+    // Constrain total velocity if limiting is enabled and vel_max is positive 
+    if (limit_total && is_positive(vel_max)) {
         vel_target.limit_length(vel_max);
     }
 
     shape_vel_accel_xy(vel_target, accel_input, vel, accel, accel_max, jerk_max, dt, limit_total);
 }
 
+/* shape_angle_vel_accel calculate a jerk limited path from the current angle, angular velocity and angular acceleration to an input angle, angular velocity and angular acceleration.           
+ The function takes the current angle, angular velocity, and angular acceleration and calculates the required jerk limited adjustment to the angular acceleration for the next time dt.
+ The kinematic path is constrained by :
+    maximum angular velocity - angle_vel_max,
+    maximum angular acceleration - angle_accel_max,
+    maximum angular jerk - angle_jerk_max.
+ The function alters the variable accel to follow a jerk limited kinematic path to angle_input, angle_vel_input and angle_accel_input.
+ The angle_vel_max limit can be removed by setting the desired limit to zero.
+ The correction angular acceleration can is limited to angle_accel_max. If limit_total_accel is true the total angular acceleration is limited to angle_accel_max.
+*/            
+void shape_angle_vel_accel(float angle_input, float angle_vel_input, float angle_accel_input,
+                         float angle, float angle_vel, float& angle_accel,
+                         float angle_vel_max, float angle_accel_max,
+                         float angle_jerk_max, float dt, bool limit_total)
+{
+    // sanity check accel_max
+    if (!is_positive(angle_accel_max)) {
+        INTERNAL_ERROR(AP_InternalError::error_t::invalid_arg_or_result);
+        return;
+    }
+
+    // calculate angle error to be corrected
+    // unwrap angle error to achieve fastest path
+    float stopping_time = fabsf(angle_vel / angle_accel_max);
+    float angle_error = angle_input - angle - angle_vel * stopping_time;
+    angle_error = wrap_PI(angle_error);
+    angle_error += angle_vel * stopping_time;
+
+    // Calculate time constants and limits to ensure stable operation
+    // The negative acceleration limit is used here because the square root controller
+    // manages the approach to the setpoint. Therefore the acceleration is in the opposite
+    // direction to the position error.
+    const float angle_accel_tc_max = 0.5 * angle_accel_max;
+    const float KPv = 0.5 * angle_jerk_max / angle_accel_max;
+
+    // velocity to correct position
+    float angle_vel_target = sqrt_controller(angle_error, KPv, angle_accel_tc_max, dt);
+
+    // limit velocity to vel_max
+    if (is_positive(angle_vel_max)){
+        angle_vel_target = constrain_float(angle_vel_target, -angle_vel_max, angle_vel_max);
+    }
+
+    // velocity correction with input velocity
+    angle_vel_target += angle_vel_input;
+
+    // Constrain total velocity if limiting is enabled and angle_vel_max is positive 
+    if (limit_total && is_positive(angle_vel_max)){
+        angle_vel_target = constrain_float(angle_vel_target, -angle_vel_max, angle_vel_max);
+    }
+
+    shape_vel_accel(angle_vel_target, angle_accel_input, angle_vel, angle_accel, -angle_accel_max, angle_accel_max, angle_jerk_max, dt, limit_total);
+}
+
 /* limit_accel_xy limits the acceleration to prioritise acceleration perpendicular to the provided velocity vector.
  Input parameters are:
     vel is the velocity vector used to define the direction acceleration limit is biased in.
diff --git a/libraries/AP_Math/control.h b/libraries/AP_Math/control.h
index 7567decf6b33e..441f7c2291d68 100644
--- a/libraries/AP_Math/control.h
+++ b/libraries/AP_Math/control.h
@@ -103,6 +103,21 @@ void shape_pos_vel_accel_xy(const Vector2p& pos_input, const Vector2f& vel_input
                             float vel_max, float accel_max,
                             float jerk_max, float dt, bool limit_total);
 
+/* shape_angle_vel_accel calculate a jerk limited path from the current angle, angular velocity and angular acceleration to an input angle, angular velocity and angular acceleration.           
+ The function takes the current angle, angular velocity, and angular acceleration and calculates the required jerk limited adjustment to the angular acceleration for the next time dt.
+ The kinematic path is constrained by :
+    maximum angular velocity - angle_vel_max,
+    maximum angular acceleration - angle_accel_max,
+    maximum angular jerk - angle_jerk_max.
+ The function alters the variable accel to follow a jerk limited kinematic path to angle_input, angle_vel_input and angle_accel_input.
+ The angle_vel_max limit can be removed by setting the desired limit to zero.
+ The correction angular velocity is limited to angle_vel_max. If limit_total is true the total angular velocity is limited to angle_vel_max.
+ The correction angular acceleration can is limited to angle_accel_max. If limit_total is true the total angular acceleration is limited to angle_accel_max.
+*/            
+void shape_angle_vel_accel(float angle_input, float angle_vel_input, float angle_accel_input,
+                         float angle, float angle_vel, float& angle_accel,
+                         float angle_vel_max, float angle_accel_max,
+                         float angle_jerk_max, float dt, bool limit_total);
 
 /* limit_accel_xy limits the acceleration to prioritise acceleration perpendicular to the provided velocity vector.
  Input parameters are:


Reviews:
user: login: timtuxworth
body: Some initial comments.
state: CHANGES_REQUESTED
author_association: CONTRIBUTOR
submitted_at: 2025-04-29T08:01:48Z
user: login: lthall
state: COMMENTED
author_association: CONTRIBUTOR
submitted_at: 2025-05-01T12:03:47Z
user: login: lthall
state: COMMENTED
author_association: CONTRIBUTOR
submitted_at: 2025-05-01T12:07:08Z
user: login: lthall
state: COMMENTED
author_association: CONTRIBUTOR
submitted_at: 2025-05-01T12:41:58Z
user: login: lthall
state: COMMENTED
author_association: CONTRIBUTOR
submitted_at: 2025-05-01T12:42:18Z
user: login: lthall
state: COMMENTED
author_association: CONTRIBUTOR
submitted_at: 2025-05-01T12:43:18Z
user: login: lthall
state: COMMENTED
author_association: CONTRIBUTOR
submitted_at: 2025-05-01T12:43:31Z
user: login: lthall
state: COMMENTED
author_association: CONTRIBUTOR
submitted_at: 2025-05-01T12:44:41Z
user: login: timtuxworth
state: COMMENTED
author_association: CONTRIBUTOR
submitted_at: 2025-05-01T23:38:52Z
user: login: lthall
state: COMMENTED
author_association: CONTRIBUTOR
submitted_at: 2025-05-02T00:38:36Z
user: login: timtuxworth
state: COMMENTED
author_association: CONTRIBUTOR
submitted_at: 2025-05-02T02:08:25Z
user: login: lthall
state: COMMENTED
author_association: CONTRIBUTOR
submitted_at: 2025-05-02T03:50:45Z
user: login: lthall
state: COMMENTED
author_association: CONTRIBUTOR
submitted_at: 2025-05-15T11:19:45Z
user: login: lthall
state: COMMENTED
author_association: CONTRIBUTOR
submitted_at: 2025-05-15T12:31:56Z
user: login: timtuxworth
state: COMMENTED
author_association: CONTRIBUTOR
submitted_at: 2025-05-19T02:31:41Z
user: login: timtuxworth
body: Looking forward to getting this in, but is there a problem with that if statement in control.cpp?
state: COMMENTED
author_association: CONTRIBUTOR
submitted_at: 2025-05-19T02:34:41Z
user: login: lthall
state: COMMENTED
author_association: CONTRIBUTOR
submitted_at: 2025-05-19T06:16:04Z
user: login: tridge
state: COMMENTED
author_association: CONTRIBUTOR
submitted_at: 2025-05-19T22:50:49Z
user: login: IamPete1
state: COMMENTED
author_association: MEMBER
submitted_at: 2025-05-19T23:26:51Z
user: login: IamPete1
state: COMMENTED
author_association: MEMBER
submitted_at: 2025-05-19T23:35:10Z
user: login: rmackay9
state: COMMENTED
author_association: CONTRIBUTOR
submitted_at: 2025-05-19T23:54:35Z
user: login: tridge
state: COMMENTED
author_association: CONTRIBUTOR
submitted_at: 2025-05-19T23:57:33Z
user: login: rmackay9
state: COMMENTED
author_association: CONTRIBUTOR
submitted_at: 2025-05-20T00:00:35Z
user: login: rmackay9
body: LGTM, just a request from me to restore some comments. thanks!
state: COMMENTED
author_association: CONTRIBUTOR
submitted_at: 2025-05-20T00:06:41Z
user: login: lthall
state: COMMENTED
author_association: CONTRIBUTOR
submitted_at: 2025-05-20T01:00:21Z
user: login: lthall
state: COMMENTED
author_association: CONTRIBUTOR
submitted_at: 2025-05-20T01:03:13Z
user: login: rmackay9
state: APPROVED
author_association: CONTRIBUTOR
submitted_at: 2025-05-20T05:23:51Z
user: login: timtuxworth
body: I'm looking forward to rebasing Plane Follow on this. 
state: APPROVED
author_association: CONTRIBUTOR
submitted_at: 2025-05-20T08:49:29Z
user: login: IamPete1
state: COMMENTED
author_association: MEMBER
submitted_at: 2025-05-20T12:47:41Z
user: login: lthall
state: COMMENTED
author_association: CONTRIBUTOR
submitted_at: 2025-05-20T13:18:53Z
user: login: lthall
state: COMMENTED
author_association: CONTRIBUTOR
submitted_at: 2025-05-21T08:08:05Z

Review comments:
diff_hunk: @@ -134,419 +155,638 @@ const AP_Param::GroupInfo AP_Follow::var_info[] = {
     AP_GROUPINFO("_ALT_TYPE", 10, AP_Follow, _alt_type, AP_FOLLOW_ALT_TYPE_DEFAULT),
 #endif
 
+// May add option to turn on input shaping, turn on acceleration due to rotation of offsest
+
     // @Param: _OPTIONS
     // @DisplayName: Follow options
     // @Description: Follow options bitmask
     // @Values: 0:None,1: Mount Follows lead vehicle on mode enter
     // @User: Standard
     AP_GROUPINFO("_OPTIONS", 11, AP_Follow, _options, 0),
 
+    // @Param: _ACCEL_NE
+    // @DisplayName: Acceleration limit for the horizontal kinematic input shaping
+    // @Description: Acceleration limit of the horizontal kinematic path generation used to determine how quickly the ship varies in velocity
+    // @Range: 0 5
+    // @Units: m/s/s
+    // @User: Advanced
+    AP_GROUPINFO("_ACCEL_NE", 12, AP_Follow, _accel_max_ne_mss, 2.5),
+
+    // @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 ship 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),
+
+    // @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 ship 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),
+
+    // @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 ship 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),
+
+    // @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 ship 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),
+
+    // @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 ship 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_GROUPEND
 };
 
-/* 
-   The constructor also initialises the proximity sensor. Note that this
-   constructor is not called until detect() returns true, so we
-   already know that we should setup the proximity sensor
-*/
+// Constructor for AP_Follow. Initializes the position P-controller and sets up parameter defaults.
 AP_Follow::AP_Follow() :
-        _p_pos(AP_FOLLOW_POS_P_DEFAULT)
+    _p_pos(AP_FOLLOW_POS_P_DEFAULT)
 {
     _singleton = this;
     AP_Param::setup_object_defaults(this, var_info);
 }
 
-// restore offsets to zero if necessary, should be called when vehicle exits follow mode
-void AP_Follow::clear_offsets_if_required()
+
+//==============================================================================
+// Target Estimation Update Functions
+//==============================================================================
+
+// Projects and updates the estimated target position, velocity, and heading based on last known data and configured input shaping.
+bool AP_Follow::update_estimate()
 {
-    if (_offsets_were_zero) {
-        _offset.set(Vector3f());
-        _offsets_were_zero = false;
+    // exit immediately if not enabled
+    if (!_enabled) {
path: libraries/AP_Follow/AP_Follow.cpp
user: login: timtuxworth
body: suggest using have_target() (which also checks _enabled) - this applies to all the cases below where you do this.
created_at: 2025-04-26T09:52:34Z
author_association: CONTRIBUTOR
diff_hunk: @@ -134,419 +155,638 @@ const AP_Param::GroupInfo AP_Follow::var_info[] = {
     AP_GROUPINFO("_ALT_TYPE", 10, AP_Follow, _alt_type, AP_FOLLOW_ALT_TYPE_DEFAULT),
 #endif
 
+// May add option to turn on input shaping, turn on acceleration due to rotation of offsest
+
     // @Param: _OPTIONS
     // @DisplayName: Follow options
     // @Description: Follow options bitmask
     // @Values: 0:None,1: Mount Follows lead vehicle on mode enter
     // @User: Standard
     AP_GROUPINFO("_OPTIONS", 11, AP_Follow, _options, 0),
 
+    // @Param: _ACCEL_NE
+    // @DisplayName: Acceleration limit for the horizontal kinematic input shaping
+    // @Description: Acceleration limit of the horizontal kinematic path generation used to determine how quickly the ship varies in velocity
+    // @Range: 0 5
+    // @Units: m/s/s
+    // @User: Advanced
+    AP_GROUPINFO("_ACCEL_NE", 12, AP_Follow, _accel_max_ne_mss, 2.5),
+
+    // @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 ship 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),
+
+    // @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 ship 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),
+
+    // @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 ship 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),
+
+    // @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 ship 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),
+
+    // @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 ship 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_GROUPEND
 };
 
-/* 
-   The constructor also initialises the proximity sensor. Note that this
-   constructor is not called until detect() returns true, so we
-   already know that we should setup the proximity sensor
-*/
+// Constructor for AP_Follow. Initializes the position P-controller and sets up parameter defaults.
 AP_Follow::AP_Follow() :
-        _p_pos(AP_FOLLOW_POS_P_DEFAULT)
+    _p_pos(AP_FOLLOW_POS_P_DEFAULT)
 {
     _singleton = this;
     AP_Param::setup_object_defaults(this, var_info);
 }
 
-// restore offsets to zero if necessary, should be called when vehicle exits follow mode
-void AP_Follow::clear_offsets_if_required()
+
+//==============================================================================
+// Target Estimation Update Functions
+//==============================================================================
+
+// Projects and updates the estimated target position, velocity, and heading based on last known data and configured input shaping.
+bool AP_Follow::update_estimate()
 {
-    if (_offsets_were_zero) {
-        _offset.set(Vector3f());
-        _offsets_were_zero = false;
+    // exit immediately if not enabled
+    if (!_enabled) {
+        _estimate_valid = false;
+        return _estimate_valid;
     }
-}
 
-// get target's estimated location
-bool AP_Follow::get_target_location_and_velocity(Location &loc, Vector3f &vel_ned) const
-{
-    Vector3p pos_ned;
-    Vector3f local_vel_ned;  // so we either change both return parameters or neither
-    if (!get_target_position_and_velocity(pos_ned, local_vel_ned)) {
-        return false;
+    // check for timeout: if no recent location update, invalidate estimate
+    if ((_last_location_update_ms == 0) || (AP_HAL::millis() - _last_location_update_ms > AP_FOLLOW_TIMEOUT_MS)) {
+        _estimate_valid = false;
+        return _estimate_valid;
     }
-    if (!AP::ahrs().get_location_from_origin_offset_NED(loc, pos_ned)) {
-        return false;
+
+    // skip if update has already been processed this scheduler tick
+    if (_estimate_valid && (AP::scheduler().ticks32() - _last_update_ticks == 0)) {
+        return _estimate_valid;
     }
-    vel_ned = local_vel_ned;
+    _last_update_ticks = AP::scheduler().ticks32();
 
-    if (_alt_type == AP_FOLLOW_ALTITUDE_TYPE_RELATIVE) {
-        loc.change_alt_frame(Location::AltFrame::ABOVE_HOME);
+    // if sysid changed, reset the estimation state
+    if (_sysid != _sysid_used) {
+        _sysid_used = _sysid;
+        _estimate_valid = false;
     }
 
-    return true;
-}
+    const uint32_t now = AP_HAL::millis();
 
-// get target's estimated location and velocity, both NED SI from origin
-bool AP_Follow::get_target_position_and_velocity(Vector3p &pos_ned, Vector3f &vel_ned) const
-{
-    // exit immediately if not enabled
-    if (!_enabled) {
-        return false;
+    // calculate time since last location update in seconds
+    const float dt = (now - _last_location_update_ms) * 0.001f;
+
+    // project target's position and velocity forward using simple kinematics
+    Vector3f delta_pos = _target_vel_ned_ms * dt + _target_accel_ned_mss * 0.5f * sq(dt);
+    Vector3f delta_vel = _target_accel_ned_mss * dt;
+
+    // calculate time since last estimation update
+    const float e_dt = (now - _last_estimation_update_ms) * 0.001f;
+    
+    if (_estimate_valid) {
+        // 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)
+        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.xy().topostype(), _target_vel_ned_ms.xy() + delta_vel.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 angular shaping for heading estimate
+        shape_angle_vel_accel(radians(_target_heading_deg), radians(_target_heading_rate_degs), 0.0,
+                              _estimate_heading_rad, _estimate_heading_rate_rads, _estimate_heading_accel_radss,
+                              0.0, radians(_accel_max_h_degss),
+                              radians(_jerk_max_h_degsss), e_dt, false);
+
+        // update heading angle separately to maintain proper wrapping [-PI, PI]
+        postype_t estimate_heading_rad = _estimate_heading_rad;
+        update_pos_vel_accel(estimate_heading_rad, _estimate_heading_rate_rads, _estimate_heading_accel_radss, e_dt, 0.0, 0.0, 0.0);
+        _estimate_heading_rad = wrap_PI(estimate_heading_rad);
+    } else {
+        // no valid estimate yet: initialise from latest target position
+        _estimate_pos_ned_m = _target_pos_ned_m + delta_pos.topostype();
+        _estimate_vel_ned_ms = _target_vel_ned_ms + delta_vel;
+        _estimate_accel_ned_mss = _target_accel_ned_mss;
+        _estimate_valid = true;
     }
 
-    // check for timeout
-    if ((_last_location_update_ms == 0) || (AP_HAL::millis() - _last_location_update_ms > AP_FOLLOW_TIMEOUT_MS)) {
-        return false;
+    Vector3f offset_m = _offset_XXD_m.get();
+
+    // calculate estimated position and velocity with offsets applied
+    if (offset_m.is_zero() || (_offset_type == AP_FOLLOW_OFFSET_TYPE_NED)) {
+        // offsets are in NED frame: simple addition
+        _ofs_estimate_pos_ned_m = _estimate_pos_ned_m + offset_m.topostype();
+        _ofs_estimate_vel_ned_ms = _estimate_vel_ned_ms;
+        _ofs_estimate_accel_ned_mss = _estimate_accel_ned_mss;
+    } else {
+        // offsets are in FRD frame: rotate by heading
+        offset_m.xy().rotate(_estimate_heading_rad);
+        Vector3f offset_cross = offset_m.cross(Vector3f{0.0, 0.0, 1.0});
+        float offset_length_m = offset_m.length();
+        _ofs_estimate_pos_ned_m = _estimate_pos_ned_m + offset_m.topostype();
+        _ofs_estimate_vel_ned_ms = _estimate_vel_ned_ms + offset_cross * offset_length_m * _estimate_heading_rate_rads;
+        _ofs_estimate_accel_ned_mss = _estimate_accel_ned_mss + offset_cross * offset_length_m * _estimate_heading_accel_radss;
     }
 
-    // calculate time since last actual position update
-    const float dt = (AP_HAL::millis() - _last_location_update_ms) * 0.001f;
+    _last_estimation_update_ms = now;
+    return _estimate_valid;
+}
+
 
-    // get velocity estimate
-    if (!get_velocity_ned(vel_ned, dt)) {
+//==============================================================================
+// Target Information Retrieval Functions
+//==============================================================================
+
+// Retrieves the estimated target position, velocity, and acceleration in the NED frame (relative to origin).
+bool AP_Follow::get_target_pos_vel_accel_NED_m(Vector3p &pos_ned_m, Vector3f &vel_ned_ms, Vector3f &accel_ned_mss)
+{
+    if (!update_estimate()) {
         return false;
     }
 
-    // project the vehicle position
-    const Vector3p vel_ned_p { vel_ned.x, vel_ned.y, vel_ned.z };
-    pos_ned = _target_position_ned + vel_ned_p * dt;
+    pos_ned_m = _estimate_pos_ned_m;
+    vel_ned_ms = _estimate_vel_ned_ms;
+    accel_ned_mss = _estimate_accel_ned_mss;
 
     return true;
 }
 
-// get distance vector to target (in meters) and target's velocity all in NED frame
-bool AP_Follow::get_target_dist_and_vel_ned(Vector3f &dist_ned, Vector3f &dist_with_offs, Vector3f &vel_ned)
+// Retrieves the estimated target position, velocity, and acceleration in the NED frame, including configured offsets.
+bool AP_Follow::get_ofs_pos_vel_accel_NED_m(Vector3p &pos_ofs_ned_m, Vector3f &vel_ofs_ned_ms, Vector3f &accel_ofs_ned_mss)
path: libraries/AP_Follow/AP_Follow.cpp
user: login: timtuxworth
body: This should probably be called get_target_pos_vel_accel_ofs_ned_m() I think. or maybe get_target_pos_vel_accel_ned_ofs_m()
created_at: 2025-04-27T00:38:58Z
author_association: CONTRIBUTOR
diff_hunk: @@ -134,419 +155,638 @@ const AP_Param::GroupInfo AP_Follow::var_info[] = {
     AP_GROUPINFO("_ALT_TYPE", 10, AP_Follow, _alt_type, AP_FOLLOW_ALT_TYPE_DEFAULT),
 #endif
 
+// May add option to turn on input shaping, turn on acceleration due to rotation of offsest
+
     // @Param: _OPTIONS
     // @DisplayName: Follow options
     // @Description: Follow options bitmask
     // @Values: 0:None,1: Mount Follows lead vehicle on mode enter
     // @User: Standard
     AP_GROUPINFO("_OPTIONS", 11, AP_Follow, _options, 0),
 
+    // @Param: _ACCEL_NE
+    // @DisplayName: Acceleration limit for the horizontal kinematic input shaping
+    // @Description: Acceleration limit of the horizontal kinematic path generation used to determine how quickly the ship varies in velocity
+    // @Range: 0 5
+    // @Units: m/s/s
+    // @User: Advanced
+    AP_GROUPINFO("_ACCEL_NE", 12, AP_Follow, _accel_max_ne_mss, 2.5),
+
+    // @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 ship 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),
+
+    // @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 ship 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),
+
+    // @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 ship 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),
+
+    // @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 ship 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),
+
+    // @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 ship 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_GROUPEND
 };
 
-/* 
-   The constructor also initialises the proximity sensor. Note that this
-   constructor is not called until detect() returns true, so we
-   already know that we should setup the proximity sensor
-*/
+// Constructor for AP_Follow. Initializes the position P-controller and sets up parameter defaults.
 AP_Follow::AP_Follow() :
-        _p_pos(AP_FOLLOW_POS_P_DEFAULT)
+    _p_pos(AP_FOLLOW_POS_P_DEFAULT)
 {
     _singleton = this;
     AP_Param::setup_object_defaults(this, var_info);
 }
 
-// restore offsets to zero if necessary, should be called when vehicle exits follow mode
-void AP_Follow::clear_offsets_if_required()
+
+//==============================================================================
+// Target Estimation Update Functions
+//==============================================================================
+
+// Projects and updates the estimated target position, velocity, and heading based on last known data and configured input shaping.
+bool AP_Follow::update_estimate()
 {
-    if (_offsets_were_zero) {
-        _offset.set(Vector3f());
-        _offsets_were_zero = false;
+    // exit immediately if not enabled
+    if (!_enabled) {
+        _estimate_valid = false;
+        return _estimate_valid;
     }
-}
 
-// get target's estimated location
-bool AP_Follow::get_target_location_and_velocity(Location &loc, Vector3f &vel_ned) const
-{
-    Vector3p pos_ned;
-    Vector3f local_vel_ned;  // so we either change both return parameters or neither
-    if (!get_target_position_and_velocity(pos_ned, local_vel_ned)) {
-        return false;
+    // check for timeout: if no recent location update, invalidate estimate
+    if ((_last_location_update_ms == 0) || (AP_HAL::millis() - _last_location_update_ms > AP_FOLLOW_TIMEOUT_MS)) {
+        _estimate_valid = false;
+        return _estimate_valid;
     }
-    if (!AP::ahrs().get_location_from_origin_offset_NED(loc, pos_ned)) {
-        return false;
+
+    // skip if update has already been processed this scheduler tick
+    if (_estimate_valid && (AP::scheduler().ticks32() - _last_update_ticks == 0)) {
+        return _estimate_valid;
     }
-    vel_ned = local_vel_ned;
+    _last_update_ticks = AP::scheduler().ticks32();
 
-    if (_alt_type == AP_FOLLOW_ALTITUDE_TYPE_RELATIVE) {
-        loc.change_alt_frame(Location::AltFrame::ABOVE_HOME);
+    // if sysid changed, reset the estimation state
+    if (_sysid != _sysid_used) {
+        _sysid_used = _sysid;
+        _estimate_valid = false;
     }
 
-    return true;
-}
+    const uint32_t now = AP_HAL::millis();
 
-// get target's estimated location and velocity, both NED SI from origin
-bool AP_Follow::get_target_position_and_velocity(Vector3p &pos_ned, Vector3f &vel_ned) const
-{
-    // exit immediately if not enabled
-    if (!_enabled) {
-        return false;
+    // calculate time since last location update in seconds
+    const float dt = (now - _last_location_update_ms) * 0.001f;
+
+    // project target's position and velocity forward using simple kinematics
+    Vector3f delta_pos = _target_vel_ned_ms * dt + _target_accel_ned_mss * 0.5f * sq(dt);
+    Vector3f delta_vel = _target_accel_ned_mss * dt;
+
+    // calculate time since last estimation update
+    const float e_dt = (now - _last_estimation_update_ms) * 0.001f;
+    
+    if (_estimate_valid) {
+        // 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)
+        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.xy().topostype(), _target_vel_ned_ms.xy() + delta_vel.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 angular shaping for heading estimate
+        shape_angle_vel_accel(radians(_target_heading_deg), radians(_target_heading_rate_degs), 0.0,
+                              _estimate_heading_rad, _estimate_heading_rate_rads, _estimate_heading_accel_radss,
+                              0.0, radians(_accel_max_h_degss),
+                              radians(_jerk_max_h_degsss), e_dt, false);
+
+        // update heading angle separately to maintain proper wrapping [-PI, PI]
+        postype_t estimate_heading_rad = _estimate_heading_rad;
+        update_pos_vel_accel(estimate_heading_rad, _estimate_heading_rate_rads, _estimate_heading_accel_radss, e_dt, 0.0, 0.0, 0.0);
+        _estimate_heading_rad = wrap_PI(estimate_heading_rad);
+    } else {
+        // no valid estimate yet: initialise from latest target position
+        _estimate_pos_ned_m = _target_pos_ned_m + delta_pos.topostype();
+        _estimate_vel_ned_ms = _target_vel_ned_ms + delta_vel;
+        _estimate_accel_ned_mss = _target_accel_ned_mss;
+        _estimate_valid = true;
     }
 
-    // check for timeout
-    if ((_last_location_update_ms == 0) || (AP_HAL::millis() - _last_location_update_ms > AP_FOLLOW_TIMEOUT_MS)) {
-        return false;
+    Vector3f offset_m = _offset_XXD_m.get();
+
+    // calculate estimated position and velocity with offsets applied
+    if (offset_m.is_zero() || (_offset_type == AP_FOLLOW_OFFSET_TYPE_NED)) {
+        // offsets are in NED frame: simple addition
+        _ofs_estimate_pos_ned_m = _estimate_pos_ned_m + offset_m.topostype();
+        _ofs_estimate_vel_ned_ms = _estimate_vel_ned_ms;
+        _ofs_estimate_accel_ned_mss = _estimate_accel_ned_mss;
+    } else {
+        // offsets are in FRD frame: rotate by heading
+        offset_m.xy().rotate(_estimate_heading_rad);
+        Vector3f offset_cross = offset_m.cross(Vector3f{0.0, 0.0, 1.0});
+        float offset_length_m = offset_m.length();
+        _ofs_estimate_pos_ned_m = _estimate_pos_ned_m + offset_m.topostype();
+        _ofs_estimate_vel_ned_ms = _estimate_vel_ned_ms + offset_cross * offset_length_m * _estimate_heading_rate_rads;
+        _ofs_estimate_accel_ned_mss = _estimate_accel_ned_mss + offset_cross * offset_length_m * _estimate_heading_accel_radss;
     }
 
-    // calculate time since last actual position update
-    const float dt = (AP_HAL::millis() - _last_location_update_ms) * 0.001f;
+    _last_estimation_update_ms = now;
+    return _estimate_valid;
+}
+
 
-    // get velocity estimate
-    if (!get_velocity_ned(vel_ned, dt)) {
+//==============================================================================
+// Target Information Retrieval Functions
+//==============================================================================
+
+// Retrieves the estimated target position, velocity, and acceleration in the NED frame (relative to origin).
+bool AP_Follow::get_target_pos_vel_accel_NED_m(Vector3p &pos_ned_m, Vector3f &vel_ned_ms, Vector3f &accel_ned_mss)
+{
+    if (!update_estimate()) {
         return false;
     }
 
-    // project the vehicle position
-    const Vector3p vel_ned_p { vel_ned.x, vel_ned.y, vel_ned.z };
-    pos_ned = _target_position_ned + vel_ned_p * dt;
+    pos_ned_m = _estimate_pos_ned_m;
+    vel_ned_ms = _estimate_vel_ned_ms;
+    accel_ned_mss = _estimate_accel_ned_mss;
 
     return true;
 }
 
-// get distance vector to target (in meters) and target's velocity all in NED frame
-bool AP_Follow::get_target_dist_and_vel_ned(Vector3f &dist_ned, Vector3f &dist_with_offs, Vector3f &vel_ned)
+// Retrieves the estimated target position, velocity, and acceleration in the NED frame, including configured offsets.
+bool AP_Follow::get_ofs_pos_vel_accel_NED_m(Vector3p &pos_ofs_ned_m, Vector3f &vel_ofs_ned_ms, Vector3f &accel_ofs_ned_mss)
 {
-    Vector3f current_position_NED;
-    if (!AP::ahrs().get_relative_position_NED_origin(current_position_NED)) {
-        clear_dist_and_bearing_to_target();
-        return false;
-    }
-    Vector3p target_position_NED;
-    Vector3f veh_vel;  // NED
-    if (!get_target_position_and_velocity(target_position_NED, veh_vel)) {
-        clear_dist_and_bearing_to_target();
+    if (!update_estimate()) {
         return false;
     }
 
-    // convert from Vector3p to Vector3f as they can't be subtracted:
-    Vector3f target_position_NED_f;
-    target_position_NED_f.x = target_position_NED.x;
-    target_position_NED_f.y = target_position_NED.y;
-    target_position_NED_f.z = target_position_NED.z;
+    pos_ofs_ned_m = _ofs_estimate_pos_ned_m;
+    vel_ofs_ned_ms = _ofs_estimate_vel_ned_ms;
+    accel_ofs_ned_mss = _ofs_estimate_accel_ned_mss;
 
-    const Vector3f dist_vec = target_position_NED_f - current_position_NED;
+    return true;
+}
 
-    // fail if too far
-    if (is_positive(_dist_max.get()) && (dist_vec.length() > _dist_max)) {
-        clear_dist_and_bearing_to_target();
+// Retrieves distance vectors (with and without configured offsets) and the target’s velocity, all in the NED frame.
+bool AP_Follow::get_target_dist_and_vel_NED_m(Vector3f &dist_ned, Vector3f &dist_with_offs, Vector3f &vel_ned)
+{
+    if (!update_estimate()) {
         return false;
     }
 
-    // initialise offsets from distance vector if required
-    init_offsets_if_required(dist_vec);
-
-    // get offsets
-    Vector3f offsets;
-    if (!get_offsets_ned(offsets)) {
+    Vector3f current_position_ned_m;
+    if (!AP::ahrs().get_relative_position_NED_origin(current_position_ned_m)) {
         clear_dist_and_bearing_to_target();
         return false;
     }
 
-    // calculate results
-    dist_ned = dist_vec;
-    dist_with_offs = dist_vec + offsets;
-    vel_ned = veh_vel;
+    const Vector3p dist_vec_ned_m = _estimate_pos_ned_m - current_position_ned_m.topostype();
+    const Vector3p ofs_dist_vec = _ofs_estimate_pos_ned_m - current_position_ned_m.topostype();
+    dist_ned = dist_vec_ned_m.tofloat();
+    dist_with_offs = ofs_dist_vec.tofloat();
+    vel_ned = _ofs_estimate_vel_ned_ms;
 
-    // record distance and heading for reporting purposes
-    if (is_zero(dist_with_offs.x) && is_zero(dist_with_offs.y)) {
-        clear_dist_and_bearing_to_target();
-    } else {
-        _dist_to_target = safe_sqrt(sq(dist_with_offs.x) + sq(dist_with_offs.y));
-        _bearing_to_target = degrees(atan2f(dist_with_offs.y, dist_with_offs.x));
+    return true;
+}
+
+// Retrieves the estimated target heading and heading rate in radians.
+bool AP_Follow::get_heading_heading_rate_rad(float &heading_rad, float &heading_rate_rads)
+{
+    if (!update_estimate()) {
+        return false;
     }
 
+    // return latest heading estimate
+    heading_rad = _estimate_heading_rad;
+    heading_rate_rads = _estimate_heading_rate_rads;
     return true;
 }
 
-// get target's heading in degrees (0 = north, 90 = east)
-bool AP_Follow::get_target_heading_deg(float &heading) const
+// Retrieves the target's estimated global location and velocity, adjusting altitude frame if relative mode is set (for LUA bindings).
+bool AP_Follow::get_target_location_and_velocity(Location &loc, Vector3f &vel_ned)
 {
-    // exit immediately if not enabled
-    if (!_enabled) {
+    if (!update_estimate()) {
         return false;
     }
 
-    // check for timeout
-    if ((_last_heading_update_ms == 0) || (AP_HAL::millis() - _last_heading_update_ms > AP_FOLLOW_TIMEOUT_MS)) {
+    if (!AP::ahrs().get_location_from_origin_offset_NED(loc, _estimate_pos_ned_m)) {
         return false;
     }
+    vel_ned = _estimate_vel_ned_ms;
+
+    if (_alt_type == AP_FOLLOW_ALTITUDE_TYPE_RELATIVE) {
+        loc.change_alt_frame(Location::AltFrame::ABOVE_HOME);
+    }
 
-    // return latest heading estimate
-    heading = _target_heading;
     return true;
 }
 
-// returns true if we should extract information from msg
-bool AP_Follow::should_handle_message(const mavlink_message_t &msg) const
+// Retrieves the target's estimated global location and velocity, including configured offsets, for LUA bindings.
+bool AP_Follow::get_target_location_and_velocity_ofs(Location &loc, Vector3f &vel_ned)
 {
-    // exit immediately if not enabled
-    if (!_enabled) {
+    if (!update_estimate()) {
         return false;
     }
-
-    // skip our own messages
-    if (msg.sysid == mavlink_system.sysid) {
+    if (!AP::ahrs().get_location_from_origin_offset_NED(loc, _ofs_estimate_pos_ned_m)) {
         return false;
     }
+    if (_alt_type == AP_FOLLOW_ALTITUDE_TYPE_RELATIVE) {
+        loc.change_alt_frame(Location::AltFrame::ABOVE_HOME);
+    }
 
-    // skip message if not from our target
-    if (_sysid != 0 && msg.sysid != _sysid) {
+    vel_ned = _ofs_estimate_vel_ned_ms;
+    return true;
+}
+
+// Retrieves the estimated target heading in degrees (0° = North, 90° = East) for LUA bindings.
+bool AP_Follow::get_target_heading_deg(float &heading)
+{
+    if (!update_estimate()) {
         return false;
     }
 
+    // return latest heading estimate
+    heading = degrees(_estimate_heading_rad);
     return true;
 }
 
-// handle mavlink DISTANCE_SENSOR messages
+
+//==============================================================================
+// MAVLink Message Handling
+//==============================================================================
+
+// Handles incoming MAVLink messages to update the target's position, velocity, and heading.
 void AP_Follow::handle_msg(const mavlink_message_t &msg)
 {
-    // this method should be called from an "update()" method:
     if (_automatic_sysid) {
-        // maybe timeout who we were following...
+        // timeout check: if no position update received within timeout, clear automatic sysid
         if ((_last_location_update_ms == 0) ||
             (AP_HAL::millis() - _last_location_update_ms > AP_FOLLOW_SYSID_TIMEOUT_MS)) {
             _sysid.set(0);
+            _sysid_used = 0;
+            _estimate_valid = false;
         }
     }
 
     if (!should_handle_message(msg)) {
+        // ignore message if filtering rules reject it (e.g., wrong sysid)
         return;
     }
 
-    // decode global-position-int message
+    // decode MAVLink message
     bool updated = false;
 
     switch (msg.msgid) {
     case MAVLINK_MSG_ID_GLOBAL_POSITION_INT: {
+        // handle standard global position messages
         updated = handle_global_position_int_message(msg);
         break;
     }
     case MAVLINK_MSG_ID_FOLLOW_TARGET: {
+        // handle follow-target specific messages
         updated = handle_follow_target_message(msg);
         break;
     }
     }
 
     if (updated) {
+        // message provided updated position information
+        Vector3f offset_m = _offset_XXD_m.get();
+
+        // if using relative frame, rotate offsets based on estimated heading
+        if (_offset_type == AP_FOLLOW_OFFSET_TYPE_RELATIVE) {
+            offset_m.xy().rotate(_estimate_heading_rad);
+        }
+
+        Vector3f current_position_ned_m;
+        if (!AP::ahrs().get_relative_position_NED_origin(current_position_ned_m)) {
+            // 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
+            const Vector3p dist_vec_ned_m = _estimate_pos_ned_m - current_position_ned_m.topostype();
+            const Vector3p ofs_dist_vec = _ofs_estimate_pos_ned_m - current_position_ned_m.topostype();
+
+            // initialise offsets dynamically if needed
+            init_offsets_if_required(dist_vec_ned_m.tofloat());
+
+            // record distance and bearing to target for reporting/logging
+            if (ofs_dist_vec.xy().is_zero()) {
+                clear_dist_and_bearing_to_target();
+            } else {
+                _dist_to_target_m = ofs_dist_vec.xy().length();
path: libraries/AP_Follow/AP_Follow.cpp
user: login: timtuxworth
body: I think calculating this here means _dist_to_target_m ( which is returned by the get_distance_to_target() method is now returning the actual distance whereas before it was returning the estimated distance, in other words now it doesn't seem like it will update between messages.

This is important to me because I have a binding for get_distance_to_target() and get_bearing_to_target() that I used in my follow Lua script.

Maybe some of  these calculations should be moved into update_estimates() and handle_msg() should call update_estimates().
created_at: 2025-04-27T00:45:33Z
author_association: CONTRIBUTOR
diff_hunk: @@ -134,419 +155,638 @@ const AP_Param::GroupInfo AP_Follow::var_info[] = {
     AP_GROUPINFO("_ALT_TYPE", 10, AP_Follow, _alt_type, AP_FOLLOW_ALT_TYPE_DEFAULT),
 #endif
 
+// May add option to turn on input shaping, turn on acceleration due to rotation of offsest
+
     // @Param: _OPTIONS
     // @DisplayName: Follow options
     // @Description: Follow options bitmask
     // @Values: 0:None,1: Mount Follows lead vehicle on mode enter
     // @User: Standard
     AP_GROUPINFO("_OPTIONS", 11, AP_Follow, _options, 0),
 
+    // @Param: _ACCEL_NE
+    // @DisplayName: Acceleration limit for the horizontal kinematic input shaping
+    // @Description: Acceleration limit of the horizontal kinematic path generation used to determine how quickly the ship varies in velocity
+    // @Range: 0 5
+    // @Units: m/s/s
+    // @User: Advanced
+    AP_GROUPINFO("_ACCEL_NE", 12, AP_Follow, _accel_max_ne_mss, 2.5),
+
+    // @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 ship 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),
+
+    // @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 ship 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),
+
+    // @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 ship 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),
+
+    // @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 ship 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),
+
+    // @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 ship 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_GROUPEND
 };
 
-/* 
-   The constructor also initialises the proximity sensor. Note that this
-   constructor is not called until detect() returns true, so we
-   already know that we should setup the proximity sensor
-*/
+// Constructor for AP_Follow. Initializes the position P-controller and sets up parameter defaults.
 AP_Follow::AP_Follow() :
-        _p_pos(AP_FOLLOW_POS_P_DEFAULT)
+    _p_pos(AP_FOLLOW_POS_P_DEFAULT)
 {
     _singleton = this;
     AP_Param::setup_object_defaults(this, var_info);
 }
 
-// restore offsets to zero if necessary, should be called when vehicle exits follow mode
-void AP_Follow::clear_offsets_if_required()
+
+//==============================================================================
+// Target Estimation Update Functions
+//==============================================================================
+
+// Projects and updates the estimated target position, velocity, and heading based on last known data and configured input shaping.
+bool AP_Follow::update_estimate()
 {
-    if (_offsets_were_zero) {
-        _offset.set(Vector3f());
-        _offsets_were_zero = false;
+    // exit immediately if not enabled
+    if (!_enabled) {
+        _estimate_valid = false;
+        return _estimate_valid;
     }
-}
 
-// get target's estimated location
-bool AP_Follow::get_target_location_and_velocity(Location &loc, Vector3f &vel_ned) const
-{
-    Vector3p pos_ned;
-    Vector3f local_vel_ned;  // so we either change both return parameters or neither
-    if (!get_target_position_and_velocity(pos_ned, local_vel_ned)) {
-        return false;
+    // check for timeout: if no recent location update, invalidate estimate
+    if ((_last_location_update_ms == 0) || (AP_HAL::millis() - _last_location_update_ms > AP_FOLLOW_TIMEOUT_MS)) {
+        _estimate_valid = false;
+        return _estimate_valid;
     }
-    if (!AP::ahrs().get_location_from_origin_offset_NED(loc, pos_ned)) {
-        return false;
+
+    // skip if update has already been processed this scheduler tick
+    if (_estimate_valid && (AP::scheduler().ticks32() - _last_update_ticks == 0)) {
+        return _estimate_valid;
     }
-    vel_ned = local_vel_ned;
+    _last_update_ticks = AP::scheduler().ticks32();
 
-    if (_alt_type == AP_FOLLOW_ALTITUDE_TYPE_RELATIVE) {
-        loc.change_alt_frame(Location::AltFrame::ABOVE_HOME);
+    // if sysid changed, reset the estimation state
+    if (_sysid != _sysid_used) {
+        _sysid_used = _sysid;
+        _estimate_valid = false;
     }
 
-    return true;
-}
+    const uint32_t now = AP_HAL::millis();
 
-// get target's estimated location and velocity, both NED SI from origin
-bool AP_Follow::get_target_position_and_velocity(Vector3p &pos_ned, Vector3f &vel_ned) const
-{
-    // exit immediately if not enabled
-    if (!_enabled) {
-        return false;
+    // calculate time since last location update in seconds
+    const float dt = (now - _last_location_update_ms) * 0.001f;
+
+    // project target's position and velocity forward using simple kinematics
+    Vector3f delta_pos = _target_vel_ned_ms * dt + _target_accel_ned_mss * 0.5f * sq(dt);
+    Vector3f delta_vel = _target_accel_ned_mss * dt;
+
+    // calculate time since last estimation update
+    const float e_dt = (now - _last_estimation_update_ms) * 0.001f;
+    
+    if (_estimate_valid) {
+        // 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)
+        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.xy().topostype(), _target_vel_ned_ms.xy() + delta_vel.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 angular shaping for heading estimate
+        shape_angle_vel_accel(radians(_target_heading_deg), radians(_target_heading_rate_degs), 0.0,
+                              _estimate_heading_rad, _estimate_heading_rate_rads, _estimate_heading_accel_radss,
+                              0.0, radians(_accel_max_h_degss),
+                              radians(_jerk_max_h_degsss), e_dt, false);
+
+        // update heading angle separately to maintain proper wrapping [-PI, PI]
+        postype_t estimate_heading_rad = _estimate_heading_rad;
+        update_pos_vel_accel(estimate_heading_rad, _estimate_heading_rate_rads, _estimate_heading_accel_radss, e_dt, 0.0, 0.0, 0.0);
+        _estimate_heading_rad = wrap_PI(estimate_heading_rad);
+    } else {
+        // no valid estimate yet: initialise from latest target position
+        _estimate_pos_ned_m = _target_pos_ned_m + delta_pos.topostype();
+        _estimate_vel_ned_ms = _target_vel_ned_ms + delta_vel;
+        _estimate_accel_ned_mss = _target_accel_ned_mss;
+        _estimate_valid = true;
     }
 
-    // check for timeout
-    if ((_last_location_update_ms == 0) || (AP_HAL::millis() - _last_location_update_ms > AP_FOLLOW_TIMEOUT_MS)) {
-        return false;
+    Vector3f offset_m = _offset_XXD_m.get();
+
+    // calculate estimated position and velocity with offsets applied
+    if (offset_m.is_zero() || (_offset_type == AP_FOLLOW_OFFSET_TYPE_NED)) {
+        // offsets are in NED frame: simple addition
+        _ofs_estimate_pos_ned_m = _estimate_pos_ned_m + offset_m.topostype();
+        _ofs_estimate_vel_ned_ms = _estimate_vel_ned_ms;
+        _ofs_estimate_accel_ned_mss = _estimate_accel_ned_mss;
+    } else {
+        // offsets are in FRD frame: rotate by heading
+        offset_m.xy().rotate(_estimate_heading_rad);
+        Vector3f offset_cross = offset_m.cross(Vector3f{0.0, 0.0, 1.0});
+        float offset_length_m = offset_m.length();
+        _ofs_estimate_pos_ned_m = _estimate_pos_ned_m + offset_m.topostype();
+        _ofs_estimate_vel_ned_ms = _estimate_vel_ned_ms + offset_cross * offset_length_m * _estimate_heading_rate_rads;
+        _ofs_estimate_accel_ned_mss = _estimate_accel_ned_mss + offset_cross * offset_length_m * _estimate_heading_accel_radss;
     }
 
-    // calculate time since last actual position update
-    const float dt = (AP_HAL::millis() - _last_location_update_ms) * 0.001f;
+    _last_estimation_update_ms = now;
+    return _estimate_valid;
+}
+
 
-    // get velocity estimate
-    if (!get_velocity_ned(vel_ned, dt)) {
+//==============================================================================
+// Target Information Retrieval Functions
+//==============================================================================
+
+// Retrieves the estimated target position, velocity, and acceleration in the NED frame (relative to origin).
+bool AP_Follow::get_target_pos_vel_accel_NED_m(Vector3p &pos_ned_m, Vector3f &vel_ned_ms, Vector3f &accel_ned_mss)
+{
+    if (!update_estimate()) {
         return false;
     }
 
-    // project the vehicle position
-    const Vector3p vel_ned_p { vel_ned.x, vel_ned.y, vel_ned.z };
-    pos_ned = _target_position_ned + vel_ned_p * dt;
+    pos_ned_m = _estimate_pos_ned_m;
+    vel_ned_ms = _estimate_vel_ned_ms;
+    accel_ned_mss = _estimate_accel_ned_mss;
 
     return true;
 }
 
-// get distance vector to target (in meters) and target's velocity all in NED frame
-bool AP_Follow::get_target_dist_and_vel_ned(Vector3f &dist_ned, Vector3f &dist_with_offs, Vector3f &vel_ned)
+// Retrieves the estimated target position, velocity, and acceleration in the NED frame, including configured offsets.
+bool AP_Follow::get_ofs_pos_vel_accel_NED_m(Vector3p &pos_ofs_ned_m, Vector3f &vel_ofs_ned_ms, Vector3f &accel_ofs_ned_mss)
 {
-    Vector3f current_position_NED;
-    if (!AP::ahrs().get_relative_position_NED_origin(current_position_NED)) {
-        clear_dist_and_bearing_to_target();
-        return false;
-    }
-    Vector3p target_position_NED;
-    Vector3f veh_vel;  // NED
-    if (!get_target_position_and_velocity(target_position_NED, veh_vel)) {
-        clear_dist_and_bearing_to_target();
+    if (!update_estimate()) {
         return false;
     }
 
-    // convert from Vector3p to Vector3f as they can't be subtracted:
-    Vector3f target_position_NED_f;
-    target_position_NED_f.x = target_position_NED.x;
-    target_position_NED_f.y = target_position_NED.y;
-    target_position_NED_f.z = target_position_NED.z;
+    pos_ofs_ned_m = _ofs_estimate_pos_ned_m;
+    vel_ofs_ned_ms = _ofs_estimate_vel_ned_ms;
+    accel_ofs_ned_mss = _ofs_estimate_accel_ned_mss;
 
-    const Vector3f dist_vec = target_position_NED_f - current_position_NED;
+    return true;
+}
 
-    // fail if too far
-    if (is_positive(_dist_max.get()) && (dist_vec.length() > _dist_max)) {
-        clear_dist_and_bearing_to_target();
+// Retrieves distance vectors (with and without configured offsets) and the target’s velocity, all in the NED frame.
+bool AP_Follow::get_target_dist_and_vel_NED_m(Vector3f &dist_ned, Vector3f &dist_with_offs, Vector3f &vel_ned)
+{
+    if (!update_estimate()) {
         return false;
     }
 
-    // initialise offsets from distance vector if required
-    init_offsets_if_required(dist_vec);
-
-    // get offsets
-    Vector3f offsets;
-    if (!get_offsets_ned(offsets)) {
+    Vector3f current_position_ned_m;
+    if (!AP::ahrs().get_relative_position_NED_origin(current_position_ned_m)) {
         clear_dist_and_bearing_to_target();
         return false;
     }
 
-    // calculate results
-    dist_ned = dist_vec;
-    dist_with_offs = dist_vec + offsets;
-    vel_ned = veh_vel;
+    const Vector3p dist_vec_ned_m = _estimate_pos_ned_m - current_position_ned_m.topostype();
+    const Vector3p ofs_dist_vec = _ofs_estimate_pos_ned_m - current_position_ned_m.topostype();
+    dist_ned = dist_vec_ned_m.tofloat();
+    dist_with_offs = ofs_dist_vec.tofloat();
+    vel_ned = _ofs_estimate_vel_ned_ms;
 
-    // record distance and heading for reporting purposes
-    if (is_zero(dist_with_offs.x) && is_zero(dist_with_offs.y)) {
-        clear_dist_and_bearing_to_target();
-    } else {
-        _dist_to_target = safe_sqrt(sq(dist_with_offs.x) + sq(dist_with_offs.y));
-        _bearing_to_target = degrees(atan2f(dist_with_offs.y, dist_with_offs.x));
+    return true;
+}
+
+// Retrieves the estimated target heading and heading rate in radians.
+bool AP_Follow::get_heading_heading_rate_rad(float &heading_rad, float &heading_rate_rads)
+{
+    if (!update_estimate()) {
+        return false;
     }
 
+    // return latest heading estimate
+    heading_rad = _estimate_heading_rad;
+    heading_rate_rads = _estimate_heading_rate_rads;
     return true;
 }
 
-// get target's heading in degrees (0 = north, 90 = east)
-bool AP_Follow::get_target_heading_deg(float &heading) const
+// Retrieves the target's estimated global location and velocity, adjusting altitude frame if relative mode is set (for LUA bindings).
+bool AP_Follow::get_target_location_and_velocity(Location &loc, Vector3f &vel_ned)
 {
-    // exit immediately if not enabled
-    if (!_enabled) {
+    if (!update_estimate()) {
         return false;
     }
 
-    // check for timeout
-    if ((_last_heading_update_ms == 0) || (AP_HAL::millis() - _last_heading_update_ms > AP_FOLLOW_TIMEOUT_MS)) {
+    if (!AP::ahrs().get_location_from_origin_offset_NED(loc, _estimate_pos_ned_m)) {
         return false;
     }
+    vel_ned = _estimate_vel_ned_ms;
+
+    if (_alt_type == AP_FOLLOW_ALTITUDE_TYPE_RELATIVE) {
+        loc.change_alt_frame(Location::AltFrame::ABOVE_HOME);
+    }
 
-    // return latest heading estimate
-    heading = _target_heading;
     return true;
 }
 
-// returns true if we should extract information from msg
-bool AP_Follow::should_handle_message(const mavlink_message_t &msg) const
+// Retrieves the target's estimated global location and velocity, including configured offsets, for LUA bindings.
+bool AP_Follow::get_target_location_and_velocity_ofs(Location &loc, Vector3f &vel_ned)
 {
-    // exit immediately if not enabled
-    if (!_enabled) {
+    if (!update_estimate()) {
         return false;
     }
-
-    // skip our own messages
-    if (msg.sysid == mavlink_system.sysid) {
+    if (!AP::ahrs().get_location_from_origin_offset_NED(loc, _ofs_estimate_pos_ned_m)) {
         return false;
     }
+    if (_alt_type == AP_FOLLOW_ALTITUDE_TYPE_RELATIVE) {
+        loc.change_alt_frame(Location::AltFrame::ABOVE_HOME);
+    }
 
-    // skip message if not from our target
-    if (_sysid != 0 && msg.sysid != _sysid) {
+    vel_ned = _ofs_estimate_vel_ned_ms;
+    return true;
+}
+
+// Retrieves the estimated target heading in degrees (0° = North, 90° = East) for LUA bindings.
+bool AP_Follow::get_target_heading_deg(float &heading)
+{
+    if (!update_estimate()) {
         return false;
     }
 
+    // return latest heading estimate
+    heading = degrees(_estimate_heading_rad);
     return true;
 }
 
-// handle mavlink DISTANCE_SENSOR messages
+
+//==============================================================================
+// MAVLink Message Handling
+//==============================================================================
+
+// Handles incoming MAVLink messages to update the target's position, velocity, and heading.
 void AP_Follow::handle_msg(const mavlink_message_t &msg)
 {
-    // this method should be called from an "update()" method:
     if (_automatic_sysid) {
-        // maybe timeout who we were following...
+        // timeout check: if no position update received within timeout, clear automatic sysid
         if ((_last_location_update_ms == 0) ||
             (AP_HAL::millis() - _last_location_update_ms > AP_FOLLOW_SYSID_TIMEOUT_MS)) {
             _sysid.set(0);
+            _sysid_used = 0;
+            _estimate_valid = false;
         }
     }
 
     if (!should_handle_message(msg)) {
+        // ignore message if filtering rules reject it (e.g., wrong sysid)
         return;
     }
 
-    // decode global-position-int message
+    // decode MAVLink message
     bool updated = false;
 
     switch (msg.msgid) {
     case MAVLINK_MSG_ID_GLOBAL_POSITION_INT: {
+        // handle standard global position messages
         updated = handle_global_position_int_message(msg);
         break;
     }
     case MAVLINK_MSG_ID_FOLLOW_TARGET: {
+        // handle follow-target specific messages
         updated = handle_follow_target_message(msg);
         break;
     }
     }
 
     if (updated) {
+        // message provided updated position information
+        Vector3f offset_m = _offset_XXD_m.get();
+
+        // if using relative frame, rotate offsets based on estimated heading
+        if (_offset_type == AP_FOLLOW_OFFSET_TYPE_RELATIVE) {
+            offset_m.xy().rotate(_estimate_heading_rad);
+        }
+
+        Vector3f current_position_ned_m;
+        if (!AP::ahrs().get_relative_position_NED_origin(current_position_ned_m)) {
+            // 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
+            const Vector3p dist_vec_ned_m = _estimate_pos_ned_m - current_position_ned_m.topostype();
+            const Vector3p ofs_dist_vec = _ofs_estimate_pos_ned_m - current_position_ned_m.topostype();
+
+            // initialise offsets dynamically if needed
+            init_offsets_if_required(dist_vec_ned_m.tofloat());
+
+            // record distance and bearing to target for reporting/logging
+            if (ofs_dist_vec.xy().is_zero()) {
+                clear_dist_and_bearing_to_target();
+            } else {
+                _dist_to_target_m = ofs_dist_vec.xy().length();
+                _bearing_to_target_deg = degrees(ofs_dist_vec.xy().angle());
path: libraries/AP_Follow/AP_Follow.cpp
user: login: timtuxworth
body: ditto _bearing_to_target - now it is the bearing to the actual (last received) target position, whereas in the old code, I'm pretty sure it was the bearing to the estimated target position.
created_at: 2025-04-27T00:48:55Z
author_association: CONTRIBUTOR
diff_hunk: @@ -134,419 +155,638 @@ const AP_Param::GroupInfo AP_Follow::var_info[] = {
     AP_GROUPINFO("_ALT_TYPE", 10, AP_Follow, _alt_type, AP_FOLLOW_ALT_TYPE_DEFAULT),
 #endif
 
+// May add option to turn on input shaping, turn on acceleration due to rotation of offsest
+
     // @Param: _OPTIONS
     // @DisplayName: Follow options
     // @Description: Follow options bitmask
     // @Values: 0:None,1: Mount Follows lead vehicle on mode enter
     // @User: Standard
     AP_GROUPINFO("_OPTIONS", 11, AP_Follow, _options, 0),
 
+    // @Param: _ACCEL_NE
+    // @DisplayName: Acceleration limit for the horizontal kinematic input shaping
+    // @Description: Acceleration limit of the horizontal kinematic path generation used to determine how quickly the ship varies in velocity
+    // @Range: 0 5
+    // @Units: m/s/s
+    // @User: Advanced
+    AP_GROUPINFO("_ACCEL_NE", 12, AP_Follow, _accel_max_ne_mss, 2.5),
+
+    // @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 ship 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),
+
+    // @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 ship 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),
+
+    // @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 ship 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),
+
+    // @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 ship 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),
+
+    // @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 ship 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_GROUPEND
 };
 
-/* 
-   The constructor also initialises the proximity sensor. Note that this
-   constructor is not called until detect() returns true, so we
-   already know that we should setup the proximity sensor
-*/
+// Constructor for AP_Follow. Initializes the position P-controller and sets up parameter defaults.
 AP_Follow::AP_Follow() :
-        _p_pos(AP_FOLLOW_POS_P_DEFAULT)
+    _p_pos(AP_FOLLOW_POS_P_DEFAULT)
 {
     _singleton = this;
     AP_Param::setup_object_defaults(this, var_info);
 }
 
-// restore offsets to zero if necessary, should be called when vehicle exits follow mode
-void AP_Follow::clear_offsets_if_required()
+
+//==============================================================================
+// Target Estimation Update Functions
+//==============================================================================
+
+// Projects and updates the estimated target position, velocity, and heading based on last known data and configured input shaping.
+bool AP_Follow::update_estimate()
 {
-    if (_offsets_were_zero) {
-        _offset.set(Vector3f());
-        _offsets_were_zero = false;
+    // exit immediately if not enabled
+    if (!_enabled) {
+        _estimate_valid = false;
+        return _estimate_valid;
     }
-}
 
-// get target's estimated location
-bool AP_Follow::get_target_location_and_velocity(Location &loc, Vector3f &vel_ned) const
-{
-    Vector3p pos_ned;
-    Vector3f local_vel_ned;  // so we either change both return parameters or neither
-    if (!get_target_position_and_velocity(pos_ned, local_vel_ned)) {
-        return false;
+    // check for timeout: if no recent location update, invalidate estimate
+    if ((_last_location_update_ms == 0) || (AP_HAL::millis() - _last_location_update_ms > AP_FOLLOW_TIMEOUT_MS)) {
+        _estimate_valid = false;
+        return _estimate_valid;
     }
-    if (!AP::ahrs().get_location_from_origin_offset_NED(loc, pos_ned)) {
-        return false;
+
+    // skip if update has already been processed this scheduler tick
+    if (_estimate_valid && (AP::scheduler().ticks32() - _last_update_ticks == 0)) {
+        return _estimate_valid;
     }
-    vel_ned = local_vel_ned;
+    _last_update_ticks = AP::scheduler().ticks32();
 
-    if (_alt_type == AP_FOLLOW_ALTITUDE_TYPE_RELATIVE) {
-        loc.change_alt_frame(Location::AltFrame::ABOVE_HOME);
+    // if sysid changed, reset the estimation state
+    if (_sysid != _sysid_used) {
+        _sysid_used = _sysid;
+        _estimate_valid = false;
     }
 
-    return true;
-}
+    const uint32_t now = AP_HAL::millis();
 
-// get target's estimated location and velocity, both NED SI from origin
-bool AP_Follow::get_target_position_and_velocity(Vector3p &pos_ned, Vector3f &vel_ned) const
-{
-    // exit immediately if not enabled
-    if (!_enabled) {
-        return false;
+    // calculate time since last location update in seconds
+    const float dt = (now - _last_location_update_ms) * 0.001f;
+
+    // project target's position and velocity forward using simple kinematics
+    Vector3f delta_pos = _target_vel_ned_ms * dt + _target_accel_ned_mss * 0.5f * sq(dt);
+    Vector3f delta_vel = _target_accel_ned_mss * dt;
+
+    // calculate time since last estimation update
+    const float e_dt = (now - _last_estimation_update_ms) * 0.001f;
+    
+    if (_estimate_valid) {
+        // 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)
+        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.xy().topostype(), _target_vel_ned_ms.xy() + delta_vel.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 angular shaping for heading estimate
+        shape_angle_vel_accel(radians(_target_heading_deg), radians(_target_heading_rate_degs), 0.0,
+                              _estimate_heading_rad, _estimate_heading_rate_rads, _estimate_heading_accel_radss,
+                              0.0, radians(_accel_max_h_degss),
+                              radians(_jerk_max_h_degsss), e_dt, false);
+
+        // update heading angle separately to maintain proper wrapping [-PI, PI]
+        postype_t estimate_heading_rad = _estimate_heading_rad;
+        update_pos_vel_accel(estimate_heading_rad, _estimate_heading_rate_rads, _estimate_heading_accel_radss, e_dt, 0.0, 0.0, 0.0);
+        _estimate_heading_rad = wrap_PI(estimate_heading_rad);
+    } else {
+        // no valid estimate yet: initialise from latest target position
+        _estimate_pos_ned_m = _target_pos_ned_m + delta_pos.topostype();
+        _estimate_vel_ned_ms = _target_vel_ned_ms + delta_vel;
+        _estimate_accel_ned_mss = _target_accel_ned_mss;
+        _estimate_valid = true;
     }
 
-    // check for timeout
-    if ((_last_location_update_ms == 0) || (AP_HAL::millis() - _last_location_update_ms > AP_FOLLOW_TIMEOUT_MS)) {
-        return false;
+    Vector3f offset_m = _offset_XXD_m.get();
+
+    // calculate estimated position and velocity with offsets applied
+    if (offset_m.is_zero() || (_offset_type == AP_FOLLOW_OFFSET_TYPE_NED)) {
+        // offsets are in NED frame: simple addition
+        _ofs_estimate_pos_ned_m = _estimate_pos_ned_m + offset_m.topostype();
+        _ofs_estimate_vel_ned_ms = _estimate_vel_ned_ms;
+        _ofs_estimate_accel_ned_mss = _estimate_accel_ned_mss;
+    } else {
+        // offsets are in FRD frame: rotate by heading
+        offset_m.xy().rotate(_estimate_heading_rad);
+        Vector3f offset_cross = offset_m.cross(Vector3f{0.0, 0.0, 1.0});
+        float offset_length_m = offset_m.length();
+        _ofs_estimate_pos_ned_m = _estimate_pos_ned_m + offset_m.topostype();
+        _ofs_estimate_vel_ned_ms = _estimate_vel_ned_ms + offset_cross * offset_length_m * _estimate_heading_rate_rads;
+        _ofs_estimate_accel_ned_mss = _estimate_accel_ned_mss + offset_cross * offset_length_m * _estimate_heading_accel_radss;
     }
 
-    // calculate time since last actual position update
-    const float dt = (AP_HAL::millis() - _last_location_update_ms) * 0.001f;
+    _last_estimation_update_ms = now;
+    return _estimate_valid;
+}
+
 
-    // get velocity estimate
-    if (!get_velocity_ned(vel_ned, dt)) {
+//==============================================================================
+// Target Information Retrieval Functions
+//==============================================================================
+
+// Retrieves the estimated target position, velocity, and acceleration in the NED frame (relative to origin).
+bool AP_Follow::get_target_pos_vel_accel_NED_m(Vector3p &pos_ned_m, Vector3f &vel_ned_ms, Vector3f &accel_ned_mss)
+{
+    if (!update_estimate()) {
         return false;
     }
 
-    // project the vehicle position
-    const Vector3p vel_ned_p { vel_ned.x, vel_ned.y, vel_ned.z };
-    pos_ned = _target_position_ned + vel_ned_p * dt;
+    pos_ned_m = _estimate_pos_ned_m;
+    vel_ned_ms = _estimate_vel_ned_ms;
+    accel_ned_mss = _estimate_accel_ned_mss;
 
     return true;
 }
 
-// get distance vector to target (in meters) and target's velocity all in NED frame
-bool AP_Follow::get_target_dist_and_vel_ned(Vector3f &dist_ned, Vector3f &dist_with_offs, Vector3f &vel_ned)
+// Retrieves the estimated target position, velocity, and acceleration in the NED frame, including configured offsets.
+bool AP_Follow::get_ofs_pos_vel_accel_NED_m(Vector3p &pos_ofs_ned_m, Vector3f &vel_ofs_ned_ms, Vector3f &accel_ofs_ned_mss)
 {
-    Vector3f current_position_NED;
-    if (!AP::ahrs().get_relative_position_NED_origin(current_position_NED)) {
-        clear_dist_and_bearing_to_target();
-        return false;
-    }
-    Vector3p target_position_NED;
-    Vector3f veh_vel;  // NED
-    if (!get_target_position_and_velocity(target_position_NED, veh_vel)) {
-        clear_dist_and_bearing_to_target();
+    if (!update_estimate()) {
         return false;
     }
 
-    // convert from Vector3p to Vector3f as they can't be subtracted:
-    Vector3f target_position_NED_f;
-    target_position_NED_f.x = target_position_NED.x;
-    target_position_NED_f.y = target_position_NED.y;
-    target_position_NED_f.z = target_position_NED.z;
+    pos_ofs_ned_m = _ofs_estimate_pos_ned_m;
+    vel_ofs_ned_ms = _ofs_estimate_vel_ned_ms;
+    accel_ofs_ned_mss = _ofs_estimate_accel_ned_mss;
 
-    const Vector3f dist_vec = target_position_NED_f - current_position_NED;
+    return true;
+}
 
-    // fail if too far
-    if (is_positive(_dist_max.get()) && (dist_vec.length() > _dist_max)) {
-        clear_dist_and_bearing_to_target();
+// Retrieves distance vectors (with and without configured offsets) and the target’s velocity, all in the NED frame.
+bool AP_Follow::get_target_dist_and_vel_NED_m(Vector3f &dist_ned, Vector3f &dist_with_offs, Vector3f &vel_ned)
+{
+    if (!update_estimate()) {
         return false;
     }
 
-    // initialise offsets from distance vector if required
-    init_offsets_if_required(dist_vec);
-
-    // get offsets
-    Vector3f offsets;
-    if (!get_offsets_ned(offsets)) {
+    Vector3f current_position_ned_m;
+    if (!AP::ahrs().get_relative_position_NED_origin(current_position_ned_m)) {
         clear_dist_and_bearing_to_target();
         return false;
     }
 
-    // calculate results
-    dist_ned = dist_vec;
-    dist_with_offs = dist_vec + offsets;
-    vel_ned = veh_vel;
+    const Vector3p dist_vec_ned_m = _estimate_pos_ned_m - current_position_ned_m.topostype();
+    const Vector3p ofs_dist_vec = _ofs_estimate_pos_ned_m - current_position_ned_m.topostype();
+    dist_ned = dist_vec_ned_m.tofloat();
+    dist_with_offs = ofs_dist_vec.tofloat();
+    vel_ned = _ofs_estimate_vel_ned_ms;
 
-    // record distance and heading for reporting purposes
-    if (is_zero(dist_with_offs.x) && is_zero(dist_with_offs.y)) {
-        clear_dist_and_bearing_to_target();
-    } else {
-        _dist_to_target = safe_sqrt(sq(dist_with_offs.x) + sq(dist_with_offs.y));
-        _bearing_to_target = degrees(atan2f(dist_with_offs.y, dist_with_offs.x));
+    return true;
+}
+
+// Retrieves the estimated target heading and heading rate in radians.
+bool AP_Follow::get_heading_heading_rate_rad(float &heading_rad, float &heading_rate_rads)
+{
+    if (!update_estimate()) {
+        return false;
     }
 
+    // return latest heading estimate
+    heading_rad = _estimate_heading_rad;
+    heading_rate_rads = _estimate_heading_rate_rads;
     return true;
 }
 
-// get target's heading in degrees (0 = north, 90 = east)
-bool AP_Follow::get_target_heading_deg(float &heading) const
+// Retrieves the target's estimated global location and velocity, adjusting altitude frame if relative mode is set (for LUA bindings).
+bool AP_Follow::get_target_location_and_velocity(Location &loc, Vector3f &vel_ned)
 {
-    // exit immediately if not enabled
-    if (!_enabled) {
+    if (!update_estimate()) {
         return false;
     }
 
-    // check for timeout
-    if ((_last_heading_update_ms == 0) || (AP_HAL::millis() - _last_heading_update_ms > AP_FOLLOW_TIMEOUT_MS)) {
+    if (!AP::ahrs().get_location_from_origin_offset_NED(loc, _estimate_pos_ned_m)) {
         return false;
     }
+    vel_ned = _estimate_vel_ned_ms;
+
+    if (_alt_type == AP_FOLLOW_ALTITUDE_TYPE_RELATIVE) {
+        loc.change_alt_frame(Location::AltFrame::ABOVE_HOME);
+    }
 
-    // return latest heading estimate
-    heading = _target_heading;
     return true;
 }
 
-// returns true if we should extract information from msg
-bool AP_Follow::should_handle_message(const mavlink_message_t &msg) const
+// Retrieves the target's estimated global location and velocity, including configured offsets, for LUA bindings.
+bool AP_Follow::get_target_location_and_velocity_ofs(Location &loc, Vector3f &vel_ned)
 {
-    // exit immediately if not enabled
-    if (!_enabled) {
+    if (!update_estimate()) {
         return false;
     }
-
-    // skip our own messages
-    if (msg.sysid == mavlink_system.sysid) {
+    if (!AP::ahrs().get_location_from_origin_offset_NED(loc, _ofs_estimate_pos_ned_m)) {
         return false;
     }
+    if (_alt_type == AP_FOLLOW_ALTITUDE_TYPE_RELATIVE) {
+        loc.change_alt_frame(Location::AltFrame::ABOVE_HOME);
+    }
 
-    // skip message if not from our target
-    if (_sysid != 0 && msg.sysid != _sysid) {
+    vel_ned = _ofs_estimate_vel_ned_ms;
+    return true;
+}
+
+// Retrieves the estimated target heading in degrees (0° = North, 90° = East) for LUA bindings.
+bool AP_Follow::get_target_heading_deg(float &heading)
+{
+    if (!update_estimate()) {
         return false;
     }
 
+    // return latest heading estimate
+    heading = degrees(_estimate_heading_rad);
     return true;
 }
 
-// handle mavlink DISTANCE_SENSOR messages
+
+//==============================================================================
+// MAVLink Message Handling
+//==============================================================================
+
+// Handles incoming MAVLink messages to update the target's position, velocity, and heading.
 void AP_Follow::handle_msg(const mavlink_message_t &msg)
 {
-    // this method should be called from an "update()" method:
     if (_automatic_sysid) {
-        // maybe timeout who we were following...
+        // timeout check: if no position update received within timeout, clear automatic sysid
         if ((_last_location_update_ms == 0) ||
             (AP_HAL::millis() - _last_location_update_ms > AP_FOLLOW_SYSID_TIMEOUT_MS)) {
             _sysid.set(0);
+            _sysid_used = 0;
+            _estimate_valid = false;
         }
     }
 
     if (!should_handle_message(msg)) {
+        // ignore message if filtering rules reject it (e.g., wrong sysid)
         return;
     }
 
-    // decode global-position-int message
+    // decode MAVLink message
     bool updated = false;
 
     switch (msg.msgid) {
     case MAVLINK_MSG_ID_GLOBAL_POSITION_INT: {
+        // handle standard global position messages
         updated = handle_global_position_int_message(msg);
         break;
     }
     case MAVLINK_MSG_ID_FOLLOW_TARGET: {
+        // handle follow-target specific messages
         updated = handle_follow_target_message(msg);
         break;
     }
     }
 
     if (updated) {
+        // message provided updated position information
+        Vector3f offset_m = _offset_XXD_m.get();
+
+        // if using relative frame, rotate offsets based on estimated heading
+        if (_offset_type == AP_FOLLOW_OFFSET_TYPE_RELATIVE) {
+            offset_m.xy().rotate(_estimate_heading_rad);
+        }
+
+        Vector3f current_position_ned_m;
+        if (!AP::ahrs().get_relative_position_NED_origin(current_position_ned_m)) {
+            // 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
+            const Vector3p dist_vec_ned_m = _estimate_pos_ned_m - current_position_ned_m.topostype();
+            const Vector3p ofs_dist_vec = _ofs_estimate_pos_ned_m - current_position_ned_m.topostype();
path: libraries/AP_Follow/AP_Follow.cpp
user: login: timtuxworth
body: If we just processed a new message, but we haven't called update_estimate() yet, are these calculations using stale estimates?
created_at: 2025-04-27T01:13:20Z
author_association: CONTRIBUTOR
diff_hunk: @@ -134,419 +155,638 @@ const AP_Param::GroupInfo AP_Follow::var_info[] = {
     AP_GROUPINFO("_ALT_TYPE", 10, AP_Follow, _alt_type, AP_FOLLOW_ALT_TYPE_DEFAULT),
 #endif
 
+// May add option to turn on input shaping, turn on acceleration due to rotation of offsest
+
     // @Param: _OPTIONS
     // @DisplayName: Follow options
     // @Description: Follow options bitmask
     // @Values: 0:None,1: Mount Follows lead vehicle on mode enter
     // @User: Standard
     AP_GROUPINFO("_OPTIONS", 11, AP_Follow, _options, 0),
 
+    // @Param: _ACCEL_NE
+    // @DisplayName: Acceleration limit for the horizontal kinematic input shaping
+    // @Description: Acceleration limit of the horizontal kinematic path generation used to determine how quickly the ship varies in velocity
+    // @Range: 0 5
+    // @Units: m/s/s
+    // @User: Advanced
+    AP_GROUPINFO("_ACCEL_NE", 12, AP_Follow, _accel_max_ne_mss, 2.5),
+
+    // @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 ship 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),
+
+    // @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 ship 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),
+
+    // @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 ship 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),
+
+    // @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 ship 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),
+
+    // @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 ship 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_GROUPEND
 };
 
-/* 
-   The constructor also initialises the proximity sensor. Note that this
-   constructor is not called until detect() returns true, so we
-   already know that we should setup the proximity sensor
-*/
+// Constructor for AP_Follow. Initializes the position P-controller and sets up parameter defaults.
 AP_Follow::AP_Follow() :
-        _p_pos(AP_FOLLOW_POS_P_DEFAULT)
+    _p_pos(AP_FOLLOW_POS_P_DEFAULT)
 {
     _singleton = this;
     AP_Param::setup_object_defaults(this, var_info);
 }
 
-// restore offsets to zero if necessary, should be called when vehicle exits follow mode
-void AP_Follow::clear_offsets_if_required()
+
+//==============================================================================
+// Target Estimation Update Functions
+//==============================================================================
+
+// Projects and updates the estimated target position, velocity, and heading based on last known data and configured input shaping.
+bool AP_Follow::update_estimate()
 {
-    if (_offsets_were_zero) {
-        _offset.set(Vector3f());
-        _offsets_were_zero = false;
+    // exit immediately if not enabled
+    if (!_enabled) {
+        _estimate_valid = false;
+        return _estimate_valid;
     }
-}
 
-// get target's estimated location
-bool AP_Follow::get_target_location_and_velocity(Location &loc, Vector3f &vel_ned) const
-{
-    Vector3p pos_ned;
-    Vector3f local_vel_ned;  // so we either change both return parameters or neither
-    if (!get_target_position_and_velocity(pos_ned, local_vel_ned)) {
-        return false;
+    // check for timeout: if no recent location update, invalidate estimate
+    if ((_last_location_update_ms == 0) || (AP_HAL::millis() - _last_location_update_ms > AP_FOLLOW_TIMEOUT_MS)) {
+        _estimate_valid = false;
+        return _estimate_valid;
     }
-    if (!AP::ahrs().get_location_from_origin_offset_NED(loc, pos_ned)) {
-        return false;
+
+    // skip if update has already been processed this scheduler tick
+    if (_estimate_valid && (AP::scheduler().ticks32() - _last_update_ticks == 0)) {
+        return _estimate_valid;
     }
-    vel_ned = local_vel_ned;
+    _last_update_ticks = AP::scheduler().ticks32();
 
-    if (_alt_type == AP_FOLLOW_ALTITUDE_TYPE_RELATIVE) {
-        loc.change_alt_frame(Location::AltFrame::ABOVE_HOME);
+    // if sysid changed, reset the estimation state
+    if (_sysid != _sysid_used) {
+        _sysid_used = _sysid;
+        _estimate_valid = false;
     }
 
-    return true;
-}
+    const uint32_t now = AP_HAL::millis();
 
-// get target's estimated location and velocity, both NED SI from origin
-bool AP_Follow::get_target_position_and_velocity(Vector3p &pos_ned, Vector3f &vel_ned) const
-{
-    // exit immediately if not enabled
-    if (!_enabled) {
-        return false;
+    // calculate time since last location update in seconds
+    const float dt = (now - _last_location_update_ms) * 0.001f;
+
+    // project target's position and velocity forward using simple kinematics
+    Vector3f delta_pos = _target_vel_ned_ms * dt + _target_accel_ned_mss * 0.5f * sq(dt);
+    Vector3f delta_vel = _target_accel_ned_mss * dt;
+
+    // calculate time since last estimation update
+    const float e_dt = (now - _last_estimation_update_ms) * 0.001f;
+    
+    if (_estimate_valid) {
+        // 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)
+        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.xy().topostype(), _target_vel_ned_ms.xy() + delta_vel.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 angular shaping for heading estimate
+        shape_angle_vel_accel(radians(_target_heading_deg), radians(_target_heading_rate_degs), 0.0,
+                              _estimate_heading_rad, _estimate_heading_rate_rads, _estimate_heading_accel_radss,
+                              0.0, radians(_accel_max_h_degss),
+                              radians(_jerk_max_h_degsss), e_dt, false);
+
+        // update heading angle separately to maintain proper wrapping [-PI, PI]
+        postype_t estimate_heading_rad = _estimate_heading_rad;
+        update_pos_vel_accel(estimate_heading_rad, _estimate_heading_rate_rads, _estimate_heading_accel_radss, e_dt, 0.0, 0.0, 0.0);
+        _estimate_heading_rad = wrap_PI(estimate_heading_rad);
+    } else {
+        // no valid estimate yet: initialise from latest target position
+        _estimate_pos_ned_m = _target_pos_ned_m + delta_pos.topostype();
+        _estimate_vel_ned_ms = _target_vel_ned_ms + delta_vel;
+        _estimate_accel_ned_mss = _target_accel_ned_mss;
+        _estimate_valid = true;
     }
 
-    // check for timeout
-    if ((_last_location_update_ms == 0) || (AP_HAL::millis() - _last_location_update_ms > AP_FOLLOW_TIMEOUT_MS)) {
-        return false;
+    Vector3f offset_m = _offset_XXD_m.get();
+
+    // calculate estimated position and velocity with offsets applied
+    if (offset_m.is_zero() || (_offset_type == AP_FOLLOW_OFFSET_TYPE_NED)) {
+        // offsets are in NED frame: simple addition
+        _ofs_estimate_pos_ned_m = _estimate_pos_ned_m + offset_m.topostype();
+        _ofs_estimate_vel_ned_ms = _estimate_vel_ned_ms;
+        _ofs_estimate_accel_ned_mss = _estimate_accel_ned_mss;
+    } else {
+        // offsets are in FRD frame: rotate by heading
+        offset_m.xy().rotate(_estimate_heading_rad);
+        Vector3f offset_cross = offset_m.cross(Vector3f{0.0, 0.0, 1.0});
+        float offset_length_m = offset_m.length();
+        _ofs_estimate_pos_ned_m = _estimate_pos_ned_m + offset_m.topostype();
+        _ofs_estimate_vel_ned_ms = _estimate_vel_ned_ms + offset_cross * offset_length_m * _estimate_heading_rate_rads;
+        _ofs_estimate_accel_ned_mss = _estimate_accel_ned_mss + offset_cross * offset_length_m * _estimate_heading_accel_radss;
     }
 
-    // calculate time since last actual position update
-    const float dt = (AP_HAL::millis() - _last_location_update_ms) * 0.001f;
+    _last_estimation_update_ms = now;
+    return _estimate_valid;
+}
+
 
-    // get velocity estimate
-    if (!get_velocity_ned(vel_ned, dt)) {
+//==============================================================================
+// Target Information Retrieval Functions
+//==============================================================================
+
+// Retrieves the estimated target position, velocity, and acceleration in the NED frame (relative to origin).
+bool AP_Follow::get_target_pos_vel_accel_NED_m(Vector3p &pos_ned_m, Vector3f &vel_ned_ms, Vector3f &accel_ned_mss)
+{
+    if (!update_estimate()) {
         return false;
     }
 
-    // project the vehicle position
-    const Vector3p vel_ned_p { vel_ned.x, vel_ned.y, vel_ned.z };
-    pos_ned = _target_position_ned + vel_ned_p * dt;
+    pos_ned_m = _estimate_pos_ned_m;
+    vel_ned_ms = _estimate_vel_ned_ms;
+    accel_ned_mss = _estimate_accel_ned_mss;
 
     return true;
 }
 
-// get distance vector to target (in meters) and target's velocity all in NED frame
-bool AP_Follow::get_target_dist_and_vel_ned(Vector3f &dist_ned, Vector3f &dist_with_offs, Vector3f &vel_ned)
+// Retrieves the estimated target position, velocity, and acceleration in the NED frame, including configured offsets.
+bool AP_Follow::get_ofs_pos_vel_accel_NED_m(Vector3p &pos_ofs_ned_m, Vector3f &vel_ofs_ned_ms, Vector3f &accel_ofs_ned_mss)
 {
-    Vector3f current_position_NED;
-    if (!AP::ahrs().get_relative_position_NED_origin(current_position_NED)) {
-        clear_dist_and_bearing_to_target();
-        return false;
-    }
-    Vector3p target_position_NED;
-    Vector3f veh_vel;  // NED
-    if (!get_target_position_and_velocity(target_position_NED, veh_vel)) {
-        clear_dist_and_bearing_to_target();
+    if (!update_estimate()) {
         return false;
     }
 
-    // convert from Vector3p to Vector3f as they can't be subtracted:
-    Vector3f target_position_NED_f;
-    target_position_NED_f.x = target_position_NED.x;
-    target_position_NED_f.y = target_position_NED.y;
-    target_position_NED_f.z = target_position_NED.z;
+    pos_ofs_ned_m = _ofs_estimate_pos_ned_m;
+    vel_ofs_ned_ms = _ofs_estimate_vel_ned_ms;
+    accel_ofs_ned_mss = _ofs_estimate_accel_ned_mss;
 
-    const Vector3f dist_vec = target_position_NED_f - current_position_NED;
+    return true;
+}
 
-    // fail if too far
-    if (is_positive(_dist_max.get()) && (dist_vec.length() > _dist_max)) {
-        clear_dist_and_bearing_to_target();
+// Retrieves distance vectors (with and without configured offsets) and the target’s velocity, all in the NED frame.
+bool AP_Follow::get_target_dist_and_vel_NED_m(Vector3f &dist_ned, Vector3f &dist_with_offs, Vector3f &vel_ned)
+{
+    if (!update_estimate()) {
         return false;
     }
 
-    // initialise offsets from distance vector if required
-    init_offsets_if_required(dist_vec);
-
-    // get offsets
-    Vector3f offsets;
-    if (!get_offsets_ned(offsets)) {
+    Vector3f current_position_ned_m;
+    if (!AP::ahrs().get_relative_position_NED_origin(current_position_ned_m)) {
         clear_dist_and_bearing_to_target();
         return false;
     }
 
-    // calculate results
-    dist_ned = dist_vec;
-    dist_with_offs = dist_vec + offsets;
-    vel_ned = veh_vel;
+    const Vector3p dist_vec_ned_m = _estimate_pos_ned_m - current_position_ned_m.topostype();
+    const Vector3p ofs_dist_vec = _ofs_estimate_pos_ned_m - current_position_ned_m.topostype();
+    dist_ned = dist_vec_ned_m.tofloat();
+    dist_with_offs = ofs_dist_vec.tofloat();
+    vel_ned = _ofs_estimate_vel_ned_ms;
 
-    // record distance and heading for reporting purposes
-    if (is_zero(dist_with_offs.x) && is_zero(dist_with_offs.y)) {
-        clear_dist_and_bearing_to_target();
-    } else {
-        _dist_to_target = safe_sqrt(sq(dist_with_offs.x) + sq(dist_with_offs.y));
-        _bearing_to_target = degrees(atan2f(dist_with_offs.y, dist_with_offs.x));
+    return true;
+}
+
+// Retrieves the estimated target heading and heading rate in radians.
+bool AP_Follow::get_heading_heading_rate_rad(float &heading_rad, float &heading_rate_rads)
+{
+    if (!update_estimate()) {
+        return false;
     }
 
+    // return latest heading estimate
+    heading_rad = _estimate_heading_rad;
+    heading_rate_rads = _estimate_heading_rate_rads;
     return true;
 }
 
-// get target's heading in degrees (0 = north, 90 = east)
-bool AP_Follow::get_target_heading_deg(float &heading) const
+// Retrieves the target's estimated global location and velocity, adjusting altitude frame if relative mode is set (for LUA bindings).
+bool AP_Follow::get_target_location_and_velocity(Location &loc, Vector3f &vel_ned)
 {
-    // exit immediately if not enabled
-    if (!_enabled) {
+    if (!update_estimate()) {
         return false;
     }
 
-    // check for timeout
-    if ((_last_heading_update_ms == 0) || (AP_HAL::millis() - _last_heading_update_ms > AP_FOLLOW_TIMEOUT_MS)) {
+    if (!AP::ahrs().get_location_from_origin_offset_NED(loc, _estimate_pos_ned_m)) {
         return false;
     }
+    vel_ned = _estimate_vel_ned_ms;
+
+    if (_alt_type == AP_FOLLOW_ALTITUDE_TYPE_RELATIVE) {
+        loc.change_alt_frame(Location::AltFrame::ABOVE_HOME);
+    }
 
-    // return latest heading estimate
-    heading = _target_heading;
     return true;
 }
 
-// returns true if we should extract information from msg
-bool AP_Follow::should_handle_message(const mavlink_message_t &msg) const
+// Retrieves the target's estimated global location and velocity, including configured offsets, for LUA bindings.
+bool AP_Follow::get_target_location_and_velocity_ofs(Location &loc, Vector3f &vel_ned)
 {
-    // exit immediately if not enabled
-    if (!_enabled) {
+    if (!update_estimate()) {
         return false;
     }
-
-    // skip our own messages
-    if (msg.sysid == mavlink_system.sysid) {
+    if (!AP::ahrs().get_location_from_origin_offset_NED(loc, _ofs_estimate_pos_ned_m)) {
         return false;
     }
+    if (_alt_type == AP_FOLLOW_ALTITUDE_TYPE_RELATIVE) {
+        loc.change_alt_frame(Location::AltFrame::ABOVE_HOME);
+    }
 
-    // skip message if not from our target
-    if (_sysid != 0 && msg.sysid != _sysid) {
+    vel_ned = _ofs_estimate_vel_ned_ms;
+    return true;
+}
+
+// Retrieves the estimated target heading in degrees (0° = North, 90° = East) for LUA bindings.
+bool AP_Follow::get_target_heading_deg(float &heading)
+{
+    if (!update_estimate()) {
         return false;
     }
 
+    // return latest heading estimate
+    heading = degrees(_estimate_heading_rad);
     return true;
 }
 
-// handle mavlink DISTANCE_SENSOR messages
+
+//==============================================================================
+// MAVLink Message Handling
+//==============================================================================
+
+// Handles incoming MAVLink messages to update the target's position, velocity, and heading.
 void AP_Follow::handle_msg(const mavlink_message_t &msg)
 {
-    // this method should be called from an "update()" method:
     if (_automatic_sysid) {
-        // maybe timeout who we were following...
+        // timeout check: if no position update received within timeout, clear automatic sysid
         if ((_last_location_update_ms == 0) ||
             (AP_HAL::millis() - _last_location_update_ms > AP_FOLLOW_SYSID_TIMEOUT_MS)) {
             _sysid.set(0);
+            _sysid_used = 0;
+            _estimate_valid = false;
         }
     }
 
     if (!should_handle_message(msg)) {
+        // ignore message if filtering rules reject it (e.g., wrong sysid)
         return;
     }
 
-    // decode global-position-int message
+    // decode MAVLink message
     bool updated = false;
 
     switch (msg.msgid) {
     case MAVLINK_MSG_ID_GLOBAL_POSITION_INT: {
+        // handle standard global position messages
         updated = handle_global_position_int_message(msg);
         break;
     }
     case MAVLINK_MSG_ID_FOLLOW_TARGET: {
+        // handle follow-target specific messages
         updated = handle_follow_target_message(msg);
         break;
     }
     }
 
     if (updated) {
+        // message provided updated position information
+        Vector3f offset_m = _offset_XXD_m.get();
+
+        // if using relative frame, rotate offsets based on estimated heading
+        if (_offset_type == AP_FOLLOW_OFFSET_TYPE_RELATIVE) {
+            offset_m.xy().rotate(_estimate_heading_rad);
path: libraries/AP_Follow/AP_Follow.cpp
user: login: timtuxworth
body: also using a stale estimate?
created_at: 2025-04-27T01:29:43Z
author_association: CONTRIBUTOR
diff_hunk: @@ -556,51 +796,93 @@ Vector3f AP_Follow::rotate_vector(const Vector3f &vec, float angle_deg) const
     return ret;
 }
 
-// set recorded distance and bearing to target to zero
-void AP_Follow::clear_dist_and_bearing_to_target()
+// Resets follow mode offsets to zero if they were automatically initialized.
+void AP_Follow::clear_offsets_if_required()
 {
-    _dist_to_target = 0.0f;
-    _bearing_to_target = 0.0f;
+    if (_offsets_were_zero) {
+        _offset_XXD_m.set(Vector3f());
+        _offsets_were_zero = false;
+    }
 }
 
-// get target's estimated origin-relative-position and velocity (both
-// in SI NED), with offsets added
-bool AP_Follow::get_target_position_and_velocity_ofs(Vector3p &pos_ned, Vector3f &vel_ned) const
+
+//==============================================================================
+// Distance and Bearing Management
+//==============================================================================
+
+// Resets the recorded distance and bearing to the target to zero.
+void AP_Follow::clear_dist_and_bearing_to_target()
 {
-    Vector3f ofs_ned;
-    if (!get_offsets_ned(ofs_ned)) {
-        return false;
-    }
-    if (!get_target_position_and_velocity(pos_ned, vel_ned)) {
-        return false;
-    }
-    // can't add Vector3p and Vector3f directly:
-    pos_ned.x += ofs_ned.x;
-    pos_ned.y += ofs_ned.y;
-    pos_ned.z += ofs_ned.z;
-    return true;
+    _dist_to_target_m = 0.0f;
+    _bearing_to_target_deg = 0.0f;
 }
 
-// get target's estimated location and velocity (in NED), with offsets added
-bool AP_Follow::get_target_location_and_velocity_ofs(Location &loc, Vector3f &vel_ned) const
+
+//==============================================================================
+// Logging
+//==============================================================================
+
+// Writes a diagnostic onboard log message containing target and vehicle tracking data for Follow mode.
+#if HAL_LOGGING_ENABLED
+void AP_Follow::Log_Write_FOLL()
 {
-    Vector3p pos_ned;
-    Vector3f local_vel_ned;  // so we either change both return parameters or neither
-    if (!get_target_position_and_velocity_ofs(pos_ned, local_vel_ned)) {
-        return false;
-    }
-    if (!AP::ahrs().get_location_from_origin_offset_NED(loc, pos_ned)) {
-        return false;
-    }
+    // retrieve latest estimated location and velocity
+    Location loc_estimate{};
+    Vector3f vel_estimate;
+    UNUSED_RESULT(get_target_location_and_velocity(loc_estimate, vel_estimate));
+
+    Location _target_location;
+    UNUSED_RESULT(AP::ahrs().get_location_from_origin_offset_NED(_target_location, _target_pos_ned_m * 0.01));
+
     if (_alt_type == AP_FOLLOW_ALTITUDE_TYPE_RELATIVE) {
-        loc.change_alt_frame(Location::AltFrame::ABOVE_HOME);
-    }
+        _target_location.change_alt_frame(Location::AltFrame::ABOVE_HOME);
+    }
+
+    // log the lead target's reported position and vehicle's estimated position
+    // @LoggerMessage: FOLL
+    // @Description: Follow library diagnostic data
+    // @Field: TimeUS: Time since system startup (microseconds)
+    // @Field: Lat: Target latitude (degrees * 1E7)
+    // @Field: Lon: Target longitude (degrees * 1E7)
+    // @Field: Alt: Target absolute altitude (centimeters)
+    // @Field: VelN: Target velocity, North (m/s)
+    // @Field: VelE: Target velocity, East (m/s)
+    // @Field: VelD: Target velocity, Down (m/s)
+    // @Field: LatE: Vehicle estimated latitude (degrees * 1E7)
+    // @Field: LonE: Vehicle estimated longitude (degrees * 1E7)
+    // @Field: AltE: Vehicle estimated absolute altitude (centimeters)
+    AP::logger().WriteStreaming("FOLL",
+                                "TimeUS,Lat,Lon,Alt,VelN,VelE,VelD,LatE,LonE,AltE",  // labels
+                                "sDUmnnnDUm",    // units
+                                "F--B000--B",    // mults
+                                "QLLifffLLi",    // fmt
+                                AP_HAL::micros64(),
+                                _target_location.lat,
+                                _target_location.lng,
+                                _target_location.alt,
+                                (double)_target_vel_ned_ms.x,
+                                (double)_target_vel_ned_ms.y,
+                                (double)_target_vel_ned_ms.z,
+                                loc_estimate.lat,
+                                loc_estimate.lng,
+                                loc_estimate.alt
+                                );
+}
+#endif  // HAL_LOGGING_ENABLED
+
+
+//==============================================================================
+// Accessors and Helpers
+//==============================================================================
 
-    vel_ned = local_vel_ned;
+// Computes the estimated velocity in the NED frame by applying acceleration over the given time delta.
path: libraries/AP_Follow/AP_Follow.cpp
user: login: timtuxworth
body: If I read the code, it seems to be the velocity of the target. The comment should say that I think. Is it the estimated **current** velocity of the target vehicle, or the estimated velocity required to **achieve** the target location?
created_at: 2025-04-27T03:42:59Z
author_association: CONTRIBUTOR
diff_hunk: @@ -134,419 +155,638 @@ const AP_Param::GroupInfo AP_Follow::var_info[] = {
     AP_GROUPINFO("_ALT_TYPE", 10, AP_Follow, _alt_type, AP_FOLLOW_ALT_TYPE_DEFAULT),
 #endif
 
+// May add option to turn on input shaping, turn on acceleration due to rotation of offsest
+
     // @Param: _OPTIONS
     // @DisplayName: Follow options
     // @Description: Follow options bitmask
     // @Values: 0:None,1: Mount Follows lead vehicle on mode enter
     // @User: Standard
     AP_GROUPINFO("_OPTIONS", 11, AP_Follow, _options, 0),
 
+    // @Param: _ACCEL_NE
+    // @DisplayName: Acceleration limit for the horizontal kinematic input shaping
+    // @Description: Acceleration limit of the horizontal kinematic path generation used to determine how quickly the ship varies in velocity
+    // @Range: 0 5
+    // @Units: m/s/s
+    // @User: Advanced
+    AP_GROUPINFO("_ACCEL_NE", 12, AP_Follow, _accel_max_ne_mss, 2.5),
+
+    // @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 ship 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),
+
+    // @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 ship 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),
+
+    // @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 ship 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),
+
+    // @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 ship 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),
+
+    // @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 ship 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_GROUPEND
 };
 
-/* 
-   The constructor also initialises the proximity sensor. Note that this
-   constructor is not called until detect() returns true, so we
-   already know that we should setup the proximity sensor
-*/
+// Constructor for AP_Follow. Initializes the position P-controller and sets up parameter defaults.
 AP_Follow::AP_Follow() :
-        _p_pos(AP_FOLLOW_POS_P_DEFAULT)
+    _p_pos(AP_FOLLOW_POS_P_DEFAULT)
 {
     _singleton = this;
     AP_Param::setup_object_defaults(this, var_info);
 }
 
-// restore offsets to zero if necessary, should be called when vehicle exits follow mode
-void AP_Follow::clear_offsets_if_required()
+
+//==============================================================================
+// Target Estimation Update Functions
+//==============================================================================
+
+// Projects and updates the estimated target position, velocity, and heading based on last known data and configured input shaping.
+bool AP_Follow::update_estimate()
 {
-    if (_offsets_were_zero) {
-        _offset.set(Vector3f());
-        _offsets_were_zero = false;
+    // exit immediately if not enabled
+    if (!_enabled) {
+        _estimate_valid = false;
+        return _estimate_valid;
     }
-}
 
-// get target's estimated location
-bool AP_Follow::get_target_location_and_velocity(Location &loc, Vector3f &vel_ned) const
-{
-    Vector3p pos_ned;
-    Vector3f local_vel_ned;  // so we either change both return parameters or neither
-    if (!get_target_position_and_velocity(pos_ned, local_vel_ned)) {
-        return false;
+    // check for timeout: if no recent location update, invalidate estimate
+    if ((_last_location_update_ms == 0) || (AP_HAL::millis() - _last_location_update_ms > AP_FOLLOW_TIMEOUT_MS)) {
+        _estimate_valid = false;
+        return _estimate_valid;
     }
-    if (!AP::ahrs().get_location_from_origin_offset_NED(loc, pos_ned)) {
-        return false;
+
+    // skip if update has already been processed this scheduler tick
+    if (_estimate_valid && (AP::scheduler().ticks32() - _last_update_ticks == 0)) {
+        return _estimate_valid;
     }
-    vel_ned = local_vel_ned;
+    _last_update_ticks = AP::scheduler().ticks32();
 
-    if (_alt_type == AP_FOLLOW_ALTITUDE_TYPE_RELATIVE) {
-        loc.change_alt_frame(Location::AltFrame::ABOVE_HOME);
+    // if sysid changed, reset the estimation state
+    if (_sysid != _sysid_used) {
+        _sysid_used = _sysid;
+        _estimate_valid = false;
     }
 
-    return true;
-}
+    const uint32_t now = AP_HAL::millis();
 
-// get target's estimated location and velocity, both NED SI from origin
-bool AP_Follow::get_target_position_and_velocity(Vector3p &pos_ned, Vector3f &vel_ned) const
-{
-    // exit immediately if not enabled
-    if (!_enabled) {
-        return false;
+    // calculate time since last location update in seconds
+    const float dt = (now - _last_location_update_ms) * 0.001f;
+
+    // project target's position and velocity forward using simple kinematics
+    Vector3f delta_pos = _target_vel_ned_ms * dt + _target_accel_ned_mss * 0.5f * sq(dt);
+    Vector3f delta_vel = _target_accel_ned_mss * dt;
+
+    // calculate time since last estimation update
+    const float e_dt = (now - _last_estimation_update_ms) * 0.001f;
+    
+    if (_estimate_valid) {
+        // 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)
+        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.xy().topostype(), _target_vel_ned_ms.xy() + delta_vel.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 angular shaping for heading estimate
+        shape_angle_vel_accel(radians(_target_heading_deg), radians(_target_heading_rate_degs), 0.0,
+                              _estimate_heading_rad, _estimate_heading_rate_rads, _estimate_heading_accel_radss,
+                              0.0, radians(_accel_max_h_degss),
+                              radians(_jerk_max_h_degsss), e_dt, false);
+
+        // update heading angle separately to maintain proper wrapping [-PI, PI]
+        postype_t estimate_heading_rad = _estimate_heading_rad;
+        update_pos_vel_accel(estimate_heading_rad, _estimate_heading_rate_rads, _estimate_heading_accel_radss, e_dt, 0.0, 0.0, 0.0);
+        _estimate_heading_rad = wrap_PI(estimate_heading_rad);
+    } else {
+        // no valid estimate yet: initialise from latest target position
+        _estimate_pos_ned_m = _target_pos_ned_m + delta_pos.topostype();
+        _estimate_vel_ned_ms = _target_vel_ned_ms + delta_vel;
+        _estimate_accel_ned_mss = _target_accel_ned_mss;
+        _estimate_valid = true;
     }
 
-    // check for timeout
-    if ((_last_location_update_ms == 0) || (AP_HAL::millis() - _last_location_update_ms > AP_FOLLOW_TIMEOUT_MS)) {
-        return false;
+    Vector3f offset_m = _offset_XXD_m.get();
+
+    // calculate estimated position and velocity with offsets applied
+    if (offset_m.is_zero() || (_offset_type == AP_FOLLOW_OFFSET_TYPE_NED)) {
+        // offsets are in NED frame: simple addition
+        _ofs_estimate_pos_ned_m = _estimate_pos_ned_m + offset_m.topostype();
+        _ofs_estimate_vel_ned_ms = _estimate_vel_ned_ms;
+        _ofs_estimate_accel_ned_mss = _estimate_accel_ned_mss;
+    } else {
+        // offsets are in FRD frame: rotate by heading
+        offset_m.xy().rotate(_estimate_heading_rad);
+        Vector3f offset_cross = offset_m.cross(Vector3f{0.0, 0.0, 1.0});
+        float offset_length_m = offset_m.length();
+        _ofs_estimate_pos_ned_m = _estimate_pos_ned_m + offset_m.topostype();
+        _ofs_estimate_vel_ned_ms = _estimate_vel_ned_ms + offset_cross * offset_length_m * _estimate_heading_rate_rads;
+        _ofs_estimate_accel_ned_mss = _estimate_accel_ned_mss + offset_cross * offset_length_m * _estimate_heading_accel_radss;
     }
 
-    // calculate time since last actual position update
-    const float dt = (AP_HAL::millis() - _last_location_update_ms) * 0.001f;
+    _last_estimation_update_ms = now;
+    return _estimate_valid;
+}
+
 
-    // get velocity estimate
-    if (!get_velocity_ned(vel_ned, dt)) {
+//==============================================================================
+// Target Information Retrieval Functions
+//==============================================================================
+
+// Retrieves the estimated target position, velocity, and acceleration in the NED frame (relative to origin).
+bool AP_Follow::get_target_pos_vel_accel_NED_m(Vector3p &pos_ned_m, Vector3f &vel_ned_ms, Vector3f &accel_ned_mss)
+{
+    if (!update_estimate()) {
         return false;
     }
 
-    // project the vehicle position
-    const Vector3p vel_ned_p { vel_ned.x, vel_ned.y, vel_ned.z };
-    pos_ned = _target_position_ned + vel_ned_p * dt;
+    pos_ned_m = _estimate_pos_ned_m;
+    vel_ned_ms = _estimate_vel_ned_ms;
+    accel_ned_mss = _estimate_accel_ned_mss;
 
     return true;
 }
 
-// get distance vector to target (in meters) and target's velocity all in NED frame
-bool AP_Follow::get_target_dist_and_vel_ned(Vector3f &dist_ned, Vector3f &dist_with_offs, Vector3f &vel_ned)
+// Retrieves the estimated target position, velocity, and acceleration in the NED frame, including configured offsets.
+bool AP_Follow::get_ofs_pos_vel_accel_NED_m(Vector3p &pos_ofs_ned_m, Vector3f &vel_ofs_ned_ms, Vector3f &accel_ofs_ned_mss)
 {
-    Vector3f current_position_NED;
-    if (!AP::ahrs().get_relative_position_NED_origin(current_position_NED)) {
-        clear_dist_and_bearing_to_target();
-        return false;
-    }
-    Vector3p target_position_NED;
-    Vector3f veh_vel;  // NED
-    if (!get_target_position_and_velocity(target_position_NED, veh_vel)) {
-        clear_dist_and_bearing_to_target();
+    if (!update_estimate()) {
         return false;
     }
 
-    // convert from Vector3p to Vector3f as they can't be subtracted:
-    Vector3f target_position_NED_f;
-    target_position_NED_f.x = target_position_NED.x;
-    target_position_NED_f.y = target_position_NED.y;
-    target_position_NED_f.z = target_position_NED.z;
+    pos_ofs_ned_m = _ofs_estimate_pos_ned_m;
+    vel_ofs_ned_ms = _ofs_estimate_vel_ned_ms;
+    accel_ofs_ned_mss = _ofs_estimate_accel_ned_mss;
 
-    const Vector3f dist_vec = target_position_NED_f - current_position_NED;
+    return true;
+}
 
-    // fail if too far
-    if (is_positive(_dist_max.get()) && (dist_vec.length() > _dist_max)) {
-        clear_dist_and_bearing_to_target();
+// Retrieves distance vectors (with and without configured offsets) and the target’s velocity, all in the NED frame.
+bool AP_Follow::get_target_dist_and_vel_NED_m(Vector3f &dist_ned, Vector3f &dist_with_offs, Vector3f &vel_ned)
+{
+    if (!update_estimate()) {
         return false;
     }
 
-    // initialise offsets from distance vector if required
-    init_offsets_if_required(dist_vec);
-
-    // get offsets
-    Vector3f offsets;
-    if (!get_offsets_ned(offsets)) {
+    Vector3f current_position_ned_m;
+    if (!AP::ahrs().get_relative_position_NED_origin(current_position_ned_m)) {
         clear_dist_and_bearing_to_target();
         return false;
     }
 
-    // calculate results
-    dist_ned = dist_vec;
-    dist_with_offs = dist_vec + offsets;
-    vel_ned = veh_vel;
+    const Vector3p dist_vec_ned_m = _estimate_pos_ned_m - current_position_ned_m.topostype();
+    const Vector3p ofs_dist_vec = _ofs_estimate_pos_ned_m - current_position_ned_m.topostype();
+    dist_ned = dist_vec_ned_m.tofloat();
+    dist_with_offs = ofs_dist_vec.tofloat();
+    vel_ned = _ofs_estimate_vel_ned_ms;
 
-    // record distance and heading for reporting purposes
-    if (is_zero(dist_with_offs.x) && is_zero(dist_with_offs.y)) {
-        clear_dist_and_bearing_to_target();
-    } else {
-        _dist_to_target = safe_sqrt(sq(dist_with_offs.x) + sq(dist_with_offs.y));
-        _bearing_to_target = degrees(atan2f(dist_with_offs.y, dist_with_offs.x));
+    return true;
+}
+
+// Retrieves the estimated target heading and heading rate in radians.
+bool AP_Follow::get_heading_heading_rate_rad(float &heading_rad, float &heading_rate_rads)
+{
+    if (!update_estimate()) {
+        return false;
     }
 
+    // return latest heading estimate
+    heading_rad = _estimate_heading_rad;
+    heading_rate_rads = _estimate_heading_rate_rads;
     return true;
 }
 
-// get target's heading in degrees (0 = north, 90 = east)
-bool AP_Follow::get_target_heading_deg(float &heading) const
+// Retrieves the target's estimated global location and velocity, adjusting altitude frame if relative mode is set (for LUA bindings).
+bool AP_Follow::get_target_location_and_velocity(Location &loc, Vector3f &vel_ned)
 {
-    // exit immediately if not enabled
-    if (!_enabled) {
+    if (!update_estimate()) {
         return false;
     }
 
-    // check for timeout
-    if ((_last_heading_update_ms == 0) || (AP_HAL::millis() - _last_heading_update_ms > AP_FOLLOW_TIMEOUT_MS)) {
+    if (!AP::ahrs().get_location_from_origin_offset_NED(loc, _estimate_pos_ned_m)) {
         return false;
     }
+    vel_ned = _estimate_vel_ned_ms;
+
+    if (_alt_type == AP_FOLLOW_ALTITUDE_TYPE_RELATIVE) {
+        loc.change_alt_frame(Location::AltFrame::ABOVE_HOME);
+    }
 
-    // return latest heading estimate
-    heading = _target_heading;
     return true;
 }
 
-// returns true if we should extract information from msg
-bool AP_Follow::should_handle_message(const mavlink_message_t &msg) const
+// Retrieves the target's estimated global location and velocity, including configured offsets, for LUA bindings.
+bool AP_Follow::get_target_location_and_velocity_ofs(Location &loc, Vector3f &vel_ned)
 {
-    // exit immediately if not enabled
-    if (!_enabled) {
+    if (!update_estimate()) {
         return false;
     }
-
-    // skip our own messages
-    if (msg.sysid == mavlink_system.sysid) {
+    if (!AP::ahrs().get_location_from_origin_offset_NED(loc, _ofs_estimate_pos_ned_m)) {
         return false;
     }
+    if (_alt_type == AP_FOLLOW_ALTITUDE_TYPE_RELATIVE) {
+        loc.change_alt_frame(Location::AltFrame::ABOVE_HOME);
+    }
 
-    // skip message if not from our target
-    if (_sysid != 0 && msg.sysid != _sysid) {
+    vel_ned = _ofs_estimate_vel_ned_ms;
+    return true;
+}
+
+// Retrieves the estimated target heading in degrees (0° = North, 90° = East) for LUA bindings.
+bool AP_Follow::get_target_heading_deg(float &heading)
+{
+    if (!update_estimate()) {
         return false;
     }
 
+    // return latest heading estimate
+    heading = degrees(_estimate_heading_rad);
     return true;
 }
 
-// handle mavlink DISTANCE_SENSOR messages
+
+//==============================================================================
+// MAVLink Message Handling
+//==============================================================================
+
+// Handles incoming MAVLink messages to update the target's position, velocity, and heading.
 void AP_Follow::handle_msg(const mavlink_message_t &msg)
 {
-    // this method should be called from an "update()" method:
     if (_automatic_sysid) {
-        // maybe timeout who we were following...
+        // timeout check: if no position update received within timeout, clear automatic sysid
         if ((_last_location_update_ms == 0) ||
             (AP_HAL::millis() - _last_location_update_ms > AP_FOLLOW_SYSID_TIMEOUT_MS)) {
             _sysid.set(0);
+            _sysid_used = 0;
+            _estimate_valid = false;
         }
     }
 
     if (!should_handle_message(msg)) {
+        // ignore message if filtering rules reject it (e.g., wrong sysid)
         return;
     }
 
-    // decode global-position-int message
+    // decode MAVLink message
     bool updated = false;
 
     switch (msg.msgid) {
     case MAVLINK_MSG_ID_GLOBAL_POSITION_INT: {
+        // handle standard global position messages
         updated = handle_global_position_int_message(msg);
         break;
     }
     case MAVLINK_MSG_ID_FOLLOW_TARGET: {
+        // handle follow-target specific messages
         updated = handle_follow_target_message(msg);
         break;
     }
     }
 
     if (updated) {
+        // message provided updated position information
+        Vector3f offset_m = _offset_XXD_m.get();
+
+        // if using relative frame, rotate offsets based on estimated heading
+        if (_offset_type == AP_FOLLOW_OFFSET_TYPE_RELATIVE) {
+            offset_m.xy().rotate(_estimate_heading_rad);
+        }
+
+        Vector3f current_position_ned_m;
+        if (!AP::ahrs().get_relative_position_NED_origin(current_position_ned_m)) {
+            // 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
+            const Vector3p dist_vec_ned_m = _estimate_pos_ned_m - current_position_ned_m.topostype();
+            const Vector3p ofs_dist_vec = _ofs_estimate_pos_ned_m - current_position_ned_m.topostype();
+
+            // initialise offsets dynamically if needed
+            init_offsets_if_required(dist_vec_ned_m.tofloat());
+
+            // record distance and bearing to target for reporting/logging
+            if (ofs_dist_vec.xy().is_zero()) {
+                clear_dist_and_bearing_to_target();
+            } else {
+                _dist_to_target_m = ofs_dist_vec.xy().length();
+                _bearing_to_target_deg = degrees(ofs_dist_vec.xy().angle());
+            }
+
+            // clear distance and bearing if outside configured max follow distance
+            if (is_positive(_dist_max_m.get()) && (dist_vec_ned_m.length() > _dist_max_m)) {
+                clear_dist_and_bearing_to_target();
+            }
+        }
+
+        // Check if estimate needs reset based on position and velocity errors
+        if (estimate_error_too_large()) {
+            _estimate_valid = false;
+        }
+
 #if HAL_LOGGING_ENABLED
+        // log current follow diagnostic data
         Log_Write_FOLL();
 #endif
     }
 }
 
-bool AP_Follow::handle_global_position_int_message(const mavlink_message_t &msg)
+// Returns true if the incoming MAVLink message should be processed for target updates.
+bool AP_Follow::should_handle_message(const mavlink_message_t &msg) const
 {
-        // decode message
-        mavlink_global_position_int_t packet;
-        mavlink_msg_global_position_int_decode(&msg, &packet);
+    // exit immediately if not enabled
+    if (!_enabled) {
+        return false;
+    }
 
-        // ignore message if lat and lon are (exactly) zero
-        if ((packet.lat == 0 && packet.lon == 0)) {
-            return false;
-        }
+    // skip our own messages
+    if (msg.sysid == mavlink_system.sysid) {
+        return false;
+    }
 
-        Location _target_location;
-        _target_location.lat = packet.lat;
-        _target_location.lng = packet.lon;
+    // skip message if not from our target
+    if (_sysid != 0 && msg.sysid != _sysid) {
+        return false;
+    }
 
-        // select altitude source based on FOLL_ALT_TYPE param 
-        if (_alt_type == AP_FOLLOW_ALTITUDE_TYPE_RELATIVE) {
-            // above home alt
-            _target_location.set_alt_cm(packet.relative_alt / 10, Location::AltFrame::ABOVE_HOME);
-        } else {
-            // absolute altitude
-            _target_location.set_alt_cm(packet.alt / 10, Location::AltFrame::ABSOLUTE);
-        }
-        if (!_target_location.get_vector_from_origin_NEU(_target_position_ned)) {
-            return false;
-        }
-        _target_position_ned.z = -_target_position_ned.z; // NEU->NED
-        _target_position_ned *= 0.01;  // cm -> m
-
-        _target_velocity_ned.x = packet.vx * 0.01f; // velocity north
-        _target_velocity_ned.y = packet.vy * 0.01f; // velocity east
-        _target_velocity_ned.z = packet.vz * 0.01f; // velocity down
-
-        // get a local timestamp with correction for transport jitter
-        _last_location_update_ms = _jitter.correct_offboard_timestamp_msec(packet.time_boot_ms, AP_HAL::millis());
-        if (packet.hdg <= 36000) {                  // heading (UINT16_MAX if unknown)
-            _target_heading = packet.hdg * 0.01f;   // convert centi-degrees to degrees
-            _last_heading_update_ms = _last_location_update_ms;
-        }
-        // initialise _sysid if zero to sender's id
-        if (_sysid == 0) {
-            _sysid.set(msg.sysid);
-            _automatic_sysid = true;
-        }
-        return true;
+    return true;
 }
 
-bool AP_Follow::handle_follow_target_message(const mavlink_message_t &msg)
+// Checks whether the current estimate should be reset based on position and velocity errors.
+bool AP_Follow::estimate_error_too_large() const
 {
-        // decode message
-        mavlink_follow_target_t packet;
-        mavlink_msg_follow_target_decode(&msg, &packet);
+    const float timeout_sec = AP_FOLLOW_TIMEOUT_MS * 0.001f;
+    const float half_timeout_sec = timeout_sec * 0.5f;
 
-        // ignore message if lat and lon are (exactly) zero
-        if ((packet.lat == 0 && packet.lon == 0)) {
-            return false;
-        }
-        // require at least position
-        if ((packet.est_capabilities & (1<<0)) == 0) {
-            return false;
-        }
+    // Calculate position thresholds
+    const float pos_thresh_horiz_m = _accel_max_ne_mss.get() * sq(half_timeout_sec);
+    const float pos_thresh_vert_m  = _accel_max_d_mss.get()  * sq(half_timeout_sec);
 
-        const Location new_loc {
-            packet.lat,
-            packet.lon,
-            int32_t(packet.alt * 100),  // m -> cm
-            Location::AltFrame::ABSOLUTE
-        };
-
-        if (!new_loc.get_vector_from_origin_NEU(_target_position_ned)) {
-            return false;
-        }
-        _target_position_ned.z = -_target_position_ned.z; // NEU->NED
-        _target_position_ned *= 0.01;  // cm -> m
+    // Calculate velocity thresholds using the new helper function
+    const float vel_thresh_horiz_ms = calc_max_velocity_change(_accel_max_ne_mss.get(), _jerk_max_ne_msss.get(), timeout_sec);
+    const float vel_thresh_vert_ms  = calc_max_velocity_change(_accel_max_d_mss.get(), _jerk_max_d_msss.get(), timeout_sec);
 
-        if (packet.est_capabilities & (1<<1)) {
-            _target_velocity_ned.x = packet.vel[0]; // velocity north
-            _target_velocity_ned.y = packet.vel[1]; // velocity east
-            _target_velocity_ned.z = packet.vel[2]; // velocity down
-        } else {
-            _target_velocity_ned.zero();
-        }
+    // Calculate current position and velocity errors
+    Vector3f pos_error = _estimate_pos_ned_m.tofloat() - _target_pos_ned_m.tofloat();
+    Vector3f vel_error = _estimate_vel_ned_ms - _target_vel_ned_ms;
 
-        // get a local timestamp with correction for transport jitter
-        _last_location_update_ms = _jitter.correct_offboard_timestamp_msec(packet.timestamp, AP_HAL::millis());
+    Vector2f pos_error_xy = pos_error.xy();
+    float pos_error_z = pos_error.z;
+    Vector2f vel_error_xy = vel_error.xy();
+    float vel_error_z = vel_error.z;
 
-        if (packet.est_capabilities & (1<<3)) {
-            Quaternion q{packet.attitude_q[0], packet.attitude_q[1], packet.attitude_q[2], packet.attitude_q[3]};
-            float r, p, y;
-            q.to_euler(r,p,y);
-            _target_heading = degrees(y);
-            _last_heading_update_ms = _last_location_update_ms;
-        }
+    // Check horizontal and vertical separately
+    const bool pos_horiz_bad = pos_error_xy.length() > pos_thresh_horiz_m;
+    const bool vel_horiz_bad = vel_error_xy.length() > vel_thresh_horiz_ms;
+    const bool pos_vert_bad = fabsf(pos_error_z) > pos_thresh_vert_m;
+    const bool vel_vert_bad = fabsf(vel_error_z) > vel_thresh_vert_ms;
 
-        // initialise _sysid if zero to sender's id
-        if (_sysid == 0) {
-            _sysid.set(msg.sysid);
-            _automatic_sysid = true;
-        }
+    return pos_horiz_bad || vel_horiz_bad || pos_vert_bad || vel_vert_bad;
+}
 
-        return true;
+// Calculates max velocity change under trapezoidal or triangular acceleration profile (jerk-limited).
+float AP_Follow::calc_max_velocity_change(float accel_max, float jerk_max, float timeout_sec) const
+{
+    const float t_jerk = accel_max / jerk_max;
+    const float t_total_jerk = 2.0f * t_jerk;
+
+    if (timeout_sec >= t_total_jerk) {
+        // time to ramp up, constant accel phase, and ramp down
+        const float t_const = timeout_sec - t_total_jerk;
+        const float delta_v_jerk = 0.5f * accel_max * t_jerk;
+        const float delta_v_const = accel_max * t_const;
+        return 2.0f * delta_v_jerk + delta_v_const;
+    } else {
+        // timeout too short: pure triangle profile
+        const float t_half = timeout_sec * 0.5f;
+        return 0.5f * jerk_max * sq(t_half);
+    }
 }
 
-// write out an onboard-log message to help diagnose follow problems:
-#if HAL_LOGGING_ENABLED
-void AP_Follow::Log_Write_FOLL()
+// Decodes a GLOBAL_POSITION_INT MAVLink message to update the target’s position, velocity, and heading.
+bool AP_Follow::handle_global_position_int_message(const mavlink_message_t &msg)
 {
-        // get estimated location and velocity
-        Location loc_estimate{};
-        Vector3f vel_estimate;
-        UNUSED_RESULT(get_target_location_and_velocity(loc_estimate, vel_estimate));
-
-        Location _target_location;
-        UNUSED_RESULT(AP::ahrs().get_location_from_origin_offset_NED(_target_location, _target_position_ned * 0.01));
-        if (_alt_type == AP_FOLLOW_ALTITUDE_TYPE_RELATIVE) {
-            _target_location.change_alt_frame(Location::AltFrame::ABOVE_HOME);
-        }
+    // decode GLOBAL_POSITION_INT message into packet struct
+    mavlink_global_position_int_t packet;
+    mavlink_msg_global_position_int_decode(&msg, &packet);
+
+    // ignore message if latitude and longitude are exactly zero (invalid GPS fix)
+    if ((packet.lat == 0 && packet.lon == 0)) {
+        return false;
+    }
+
+    Location _target_location;
+    _target_location.lat = packet.lat;
+    _target_location.lng = packet.lon;
+
+    // set target altitude based on configured altitude type
+    if (_alt_type == AP_FOLLOW_ALTITUDE_TYPE_RELATIVE) {
+        // use relative altitude above home
+        _target_location.set_alt_cm(packet.relative_alt / 10, Location::AltFrame::ABOVE_HOME);
+    } else {
+        // use absolute altitude
+        _target_location.set_alt_cm(packet.alt / 10, Location::AltFrame::ABSOLUTE);
+    }
+
+    // convert global location to local NED frame position
+    if (!_target_location.get_vector_from_origin_NEU(_target_pos_ned_m)) {
+        return false;
+    }
+    _target_pos_ned_m.z = -_target_pos_ned_m.z; // convert NEU -> NED
+    _target_pos_ned_m *= 0.01;  // convert from cm to meters
+
+    // decode target velocity components (in m/s)
+    _target_vel_ned_ms.x = packet.vx * 0.01f; // velocity north
+    _target_vel_ned_ms.y = packet.vy * 0.01f; // velocity east
+    _target_vel_ned_ms.z = packet.vz * 0.01f; // velocity down
+
+    // target acceleration not available in GLOBAL_POSITION_INT
+    _target_accel_ned_mss.zero();
+
+    if (packet.hdg <= 36000) {
+        // valid heading field available (in centi-degrees)
+        _target_heading_deg = packet.hdg * 0.01f;
+    } else if (_offset_type == AP_FOLLOW_OFFSET_TYPE_RELATIVE) {
+        // heading missing but relative offset mode requires heading -> reject
+        return false;
+    } else {
+        // no heading available: set heading rate to zero
+        _target_heading_rate_degs = 0.0f;
+    }
+
+    // apply jitter-corrected timestamp to this update
+    _last_location_update_ms = _jitter.correct_offboard_timestamp_msec(packet.time_boot_ms, AP_HAL::millis());
 
-        // log lead's estimated vs reported position
-// @LoggerMessage: FOLL
-// @Description: Follow library diagnostic data
-// @Field: TimeUS: Time since system startup
-// @Field: Lat: Target latitude
-// @Field: Lon: Target longitude
-// @Field: Alt: Target absolute altitude
-// @Field: VelN: Target earth-frame velocity, North
-// @Field: VelE: Target earth-frame velocity, East
-// @Field: VelD: Target earth-frame velocity, Down
-// @Field: LatE: Vehicle latitude
-// @Field: LonE: Vehicle longitude
-// @Field: AltE: Vehicle absolute altitude
-        AP::logger().WriteStreaming("FOLL",
-                                               "TimeUS,Lat,Lon,Alt,VelN,VelE,VelD,LatE,LonE,AltE",  // labels
-                                               "sDUmnnnDUm",    // units
-                                               "F--B000--B",    // mults
-                                               "QLLifffLLi",    // fmt
-                                               AP_HAL::micros64(),
-                                               _target_location.lat,
-                                               _target_location.lng,
-                                               _target_location.alt,
-                                               (double)_target_velocity_ned.x,
-                                               (double)_target_velocity_ned.y,
-                                               (double)_target_velocity_ned.z,
-                                               loc_estimate.lat,
-                                               loc_estimate.lng,
-                                               loc_estimate.alt
-                                               );
+    // if sysid not yet set, adopt sender’s sysid and enable automatic sysid tracking
+    if (_sysid == 0) {
+        _sysid.set(msg.sysid);
+        _sysid_used = 0;
+        _estimate_valid = false;
+        _automatic_sysid = true;
+    }
+
+    return true;
 }
-#endif  // HAL_LOGGING_ENABLED
 
-// get velocity estimate in m/s in NED frame using dt since last update
-bool AP_Follow::get_velocity_ned(Vector3f &vel_ned, float dt) const
+// Decodes a FOLLOW_TARGET MAVLink message to update the target’s position, velocity, acceleration, and orientation.
+bool AP_Follow::handle_follow_target_message(const mavlink_message_t &msg)
 {
-    vel_ned = _target_velocity_ned + (_target_accel_ned * dt);
+    // decode FOLLOW_TARGET message into packet struct
+    mavlink_follow_target_t packet;
+    mavlink_msg_follow_target_decode(&msg, &packet);
+
+    // ignore message if latitude and longitude are exactly zero (invalid GPS fix)
+    if ((packet.lat == 0 && packet.lon == 0)) {
+        return false;
+    }
+
+    // require that at least position is estimated (bit 0 of est_capabilities)
+    if ((packet.est_capabilities & (1<<0)) == 0) {
+        return false;
+    }
+
+    // build Location object from latitude, longitude, and altitude (alt in meters)
+    const Location _target_location {
+        packet.lat,
+        packet.lon,
+        int32_t(packet.alt * 100),  // convert meters to centimeters
+        Location::AltFrame::ABSOLUTE
+    };
+
+    // convert global location to local NED frame position
+    if (!_target_location.get_vector_from_origin_NEU(_target_pos_ned_m)) {
+        return false;
+    }
+    _target_pos_ned_m *= 0.01; // convert from cm to meters
+
+    // adjust Z coordinate to NED frame (NEU altitude -> NED)
+    Location origin;
+    if (!AP::ahrs().get_origin(origin)) {
+        return false;
+    }
+    _target_pos_ned_m.z = -packet.alt + origin.alt * 0.01;
+
+    // decode velocity if available (bit 1 of est_capabilities)
+    if (packet.est_capabilities & (1<<1)) {
+        _target_vel_ned_ms.x = packet.vel[0]; // velocity north
+        _target_vel_ned_ms.y = packet.vel[1]; // velocity east
+        _target_vel_ned_ms.z = packet.vel[2]; // velocity down
+    } else {
+        _target_vel_ned_ms.zero();
path: libraries/AP_Follow/AP_Follow.cpp
user: login: timtuxworth
body: Should not zero the _target_vel_ned_ms as this will thump any values set in handle_global_position_int_message()
created_at: 2025-04-29T07:58:50Z
author_association: CONTRIBUTOR
diff_hunk: @@ -134,419 +155,638 @@ const AP_Param::GroupInfo AP_Follow::var_info[] = {
     AP_GROUPINFO("_ALT_TYPE", 10, AP_Follow, _alt_type, AP_FOLLOW_ALT_TYPE_DEFAULT),
 #endif
 
+// May add option to turn on input shaping, turn on acceleration due to rotation of offsest
+
     // @Param: _OPTIONS
     // @DisplayName: Follow options
     // @Description: Follow options bitmask
     // @Values: 0:None,1: Mount Follows lead vehicle on mode enter
     // @User: Standard
     AP_GROUPINFO("_OPTIONS", 11, AP_Follow, _options, 0),
 
+    // @Param: _ACCEL_NE
+    // @DisplayName: Acceleration limit for the horizontal kinematic input shaping
+    // @Description: Acceleration limit of the horizontal kinematic path generation used to determine how quickly the ship varies in velocity
+    // @Range: 0 5
+    // @Units: m/s/s
+    // @User: Advanced
+    AP_GROUPINFO("_ACCEL_NE", 12, AP_Follow, _accel_max_ne_mss, 2.5),
+
+    // @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 ship 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),
+
+    // @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 ship 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),
+
+    // @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 ship 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),
+
+    // @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 ship 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),
+
+    // @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 ship 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_GROUPEND
 };
 
-/* 
-   The constructor also initialises the proximity sensor. Note that this
-   constructor is not called until detect() returns true, so we
-   already know that we should setup the proximity sensor
-*/
+// Constructor for AP_Follow. Initializes the position P-controller and sets up parameter defaults.
 AP_Follow::AP_Follow() :
-        _p_pos(AP_FOLLOW_POS_P_DEFAULT)
+    _p_pos(AP_FOLLOW_POS_P_DEFAULT)
 {
     _singleton = this;
     AP_Param::setup_object_defaults(this, var_info);
 }
 
-// restore offsets to zero if necessary, should be called when vehicle exits follow mode
-void AP_Follow::clear_offsets_if_required()
+
+//==============================================================================
+// Target Estimation Update Functions
+//==============================================================================
+
+// Projects and updates the estimated target position, velocity, and heading based on last known data and configured input shaping.
+bool AP_Follow::update_estimate()
 {
-    if (_offsets_were_zero) {
-        _offset.set(Vector3f());
-        _offsets_were_zero = false;
+    // exit immediately if not enabled
+    if (!_enabled) {
+        _estimate_valid = false;
+        return _estimate_valid;
     }
-}
 
-// get target's estimated location
-bool AP_Follow::get_target_location_and_velocity(Location &loc, Vector3f &vel_ned) const
-{
-    Vector3p pos_ned;
-    Vector3f local_vel_ned;  // so we either change both return parameters or neither
-    if (!get_target_position_and_velocity(pos_ned, local_vel_ned)) {
-        return false;
+    // check for timeout: if no recent location update, invalidate estimate
+    if ((_last_location_update_ms == 0) || (AP_HAL::millis() - _last_location_update_ms > AP_FOLLOW_TIMEOUT_MS)) {
+        _estimate_valid = false;
+        return _estimate_valid;
     }
-    if (!AP::ahrs().get_location_from_origin_offset_NED(loc, pos_ned)) {
-        return false;
+
+    // skip if update has already been processed this scheduler tick
+    if (_estimate_valid && (AP::scheduler().ticks32() - _last_update_ticks == 0)) {
+        return _estimate_valid;
     }
-    vel_ned = local_vel_ned;
+    _last_update_ticks = AP::scheduler().ticks32();
 
-    if (_alt_type == AP_FOLLOW_ALTITUDE_TYPE_RELATIVE) {
-        loc.change_alt_frame(Location::AltFrame::ABOVE_HOME);
+    // if sysid changed, reset the estimation state
+    if (_sysid != _sysid_used) {
+        _sysid_used = _sysid;
+        _estimate_valid = false;
     }
 
-    return true;
-}
+    const uint32_t now = AP_HAL::millis();
 
-// get target's estimated location and velocity, both NED SI from origin
-bool AP_Follow::get_target_position_and_velocity(Vector3p &pos_ned, Vector3f &vel_ned) const
-{
-    // exit immediately if not enabled
-    if (!_enabled) {
-        return false;
+    // calculate time since last location update in seconds
+    const float dt = (now - _last_location_update_ms) * 0.001f;
+
+    // project target's position and velocity forward using simple kinematics
+    Vector3f delta_pos = _target_vel_ned_ms * dt + _target_accel_ned_mss * 0.5f * sq(dt);
+    Vector3f delta_vel = _target_accel_ned_mss * dt;
+
+    // calculate time since last estimation update
+    const float e_dt = (now - _last_estimation_update_ms) * 0.001f;
+    
+    if (_estimate_valid) {
+        // 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)
+        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.xy().topostype(), _target_vel_ned_ms.xy() + delta_vel.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 angular shaping for heading estimate
+        shape_angle_vel_accel(radians(_target_heading_deg), radians(_target_heading_rate_degs), 0.0,
+                              _estimate_heading_rad, _estimate_heading_rate_rads, _estimate_heading_accel_radss,
+                              0.0, radians(_accel_max_h_degss),
+                              radians(_jerk_max_h_degsss), e_dt, false);
+
+        // update heading angle separately to maintain proper wrapping [-PI, PI]
+        postype_t estimate_heading_rad = _estimate_heading_rad;
+        update_pos_vel_accel(estimate_heading_rad, _estimate_heading_rate_rads, _estimate_heading_accel_radss, e_dt, 0.0, 0.0, 0.0);
+        _estimate_heading_rad = wrap_PI(estimate_heading_rad);
+    } else {
+        // no valid estimate yet: initialise from latest target position
+        _estimate_pos_ned_m = _target_pos_ned_m + delta_pos.topostype();
+        _estimate_vel_ned_ms = _target_vel_ned_ms + delta_vel;
+        _estimate_accel_ned_mss = _target_accel_ned_mss;
+        _estimate_valid = true;
     }
 
-    // check for timeout
-    if ((_last_location_update_ms == 0) || (AP_HAL::millis() - _last_location_update_ms > AP_FOLLOW_TIMEOUT_MS)) {
-        return false;
+    Vector3f offset_m = _offset_XXD_m.get();
+
+    // calculate estimated position and velocity with offsets applied
+    if (offset_m.is_zero() || (_offset_type == AP_FOLLOW_OFFSET_TYPE_NED)) {
+        // offsets are in NED frame: simple addition
+        _ofs_estimate_pos_ned_m = _estimate_pos_ned_m + offset_m.topostype();
+        _ofs_estimate_vel_ned_ms = _estimate_vel_ned_ms;
+        _ofs_estimate_accel_ned_mss = _estimate_accel_ned_mss;
+    } else {
+        // offsets are in FRD frame: rotate by heading
+        offset_m.xy().rotate(_estimate_heading_rad);
+        Vector3f offset_cross = offset_m.cross(Vector3f{0.0, 0.0, 1.0});
+        float offset_length_m = offset_m.length();
+        _ofs_estimate_pos_ned_m = _estimate_pos_ned_m + offset_m.topostype();
+        _ofs_estimate_vel_ned_ms = _estimate_vel_ned_ms + offset_cross * offset_length_m * _estimate_heading_rate_rads;
+        _ofs_estimate_accel_ned_mss = _estimate_accel_ned_mss + offset_cross * offset_length_m * _estimate_heading_accel_radss;
     }
 
-    // calculate time since last actual position update
-    const float dt = (AP_HAL::millis() - _last_location_update_ms) * 0.001f;
+    _last_estimation_update_ms = now;
+    return _estimate_valid;
+}
+
 
-    // get velocity estimate
-    if (!get_velocity_ned(vel_ned, dt)) {
+//==============================================================================
+// Target Information Retrieval Functions
+//==============================================================================
+
+// Retrieves the estimated target position, velocity, and acceleration in the NED frame (relative to origin).
+bool AP_Follow::get_target_pos_vel_accel_NED_m(Vector3p &pos_ned_m, Vector3f &vel_ned_ms, Vector3f &accel_ned_mss)
+{
+    if (!update_estimate()) {
         return false;
     }
 
-    // project the vehicle position
-    const Vector3p vel_ned_p { vel_ned.x, vel_ned.y, vel_ned.z };
-    pos_ned = _target_position_ned + vel_ned_p * dt;
+    pos_ned_m = _estimate_pos_ned_m;
+    vel_ned_ms = _estimate_vel_ned_ms;
+    accel_ned_mss = _estimate_accel_ned_mss;
 
     return true;
 }
 
-// get distance vector to target (in meters) and target's velocity all in NED frame
-bool AP_Follow::get_target_dist_and_vel_ned(Vector3f &dist_ned, Vector3f &dist_with_offs, Vector3f &vel_ned)
+// Retrieves the estimated target position, velocity, and acceleration in the NED frame, including configured offsets.
+bool AP_Follow::get_ofs_pos_vel_accel_NED_m(Vector3p &pos_ofs_ned_m, Vector3f &vel_ofs_ned_ms, Vector3f &accel_ofs_ned_mss)
 {
-    Vector3f current_position_NED;
-    if (!AP::ahrs().get_relative_position_NED_origin(current_position_NED)) {
-        clear_dist_and_bearing_to_target();
-        return false;
-    }
-    Vector3p target_position_NED;
-    Vector3f veh_vel;  // NED
-    if (!get_target_position_and_velocity(target_position_NED, veh_vel)) {
-        clear_dist_and_bearing_to_target();
+    if (!update_estimate()) {
         return false;
     }
 
-    // convert from Vector3p to Vector3f as they can't be subtracted:
-    Vector3f target_position_NED_f;
-    target_position_NED_f.x = target_position_NED.x;
-    target_position_NED_f.y = target_position_NED.y;
-    target_position_NED_f.z = target_position_NED.z;
+    pos_ofs_ned_m = _ofs_estimate_pos_ned_m;
+    vel_ofs_ned_ms = _ofs_estimate_vel_ned_ms;
+    accel_ofs_ned_mss = _ofs_estimate_accel_ned_mss;
 
-    const Vector3f dist_vec = target_position_NED_f - current_position_NED;
+    return true;
+}
 
-    // fail if too far
-    if (is_positive(_dist_max.get()) && (dist_vec.length() > _dist_max)) {
-        clear_dist_and_bearing_to_target();
+// Retrieves distance vectors (with and without configured offsets) and the target’s velocity, all in the NED frame.
+bool AP_Follow::get_target_dist_and_vel_NED_m(Vector3f &dist_ned, Vector3f &dist_with_offs, Vector3f &vel_ned)
+{
+    if (!update_estimate()) {
         return false;
     }
 
-    // initialise offsets from distance vector if required
-    init_offsets_if_required(dist_vec);
-
-    // get offsets
-    Vector3f offsets;
-    if (!get_offsets_ned(offsets)) {
+    Vector3f current_position_ned_m;
+    if (!AP::ahrs().get_relative_position_NED_origin(current_position_ned_m)) {
         clear_dist_and_bearing_to_target();
         return false;
     }
 
-    // calculate results
-    dist_ned = dist_vec;
-    dist_with_offs = dist_vec + offsets;
-    vel_ned = veh_vel;
+    const Vector3p dist_vec_ned_m = _estimate_pos_ned_m - current_position_ned_m.topostype();
+    const Vector3p ofs_dist_vec = _ofs_estimate_pos_ned_m - current_position_ned_m.topostype();
+    dist_ned = dist_vec_ned_m.tofloat();
+    dist_with_offs = ofs_dist_vec.tofloat();
+    vel_ned = _ofs_estimate_vel_ned_ms;
 
-    // record distance and heading for reporting purposes
-    if (is_zero(dist_with_offs.x) && is_zero(dist_with_offs.y)) {
-        clear_dist_and_bearing_to_target();
-    } else {
-        _dist_to_target = safe_sqrt(sq(dist_with_offs.x) + sq(dist_with_offs.y));
-        _bearing_to_target = degrees(atan2f(dist_with_offs.y, dist_with_offs.x));
+    return true;
+}
+
+// Retrieves the estimated target heading and heading rate in radians.
+bool AP_Follow::get_heading_heading_rate_rad(float &heading_rad, float &heading_rate_rads)
+{
+    if (!update_estimate()) {
+        return false;
     }
 
+    // return latest heading estimate
+    heading_rad = _estimate_heading_rad;
+    heading_rate_rads = _estimate_heading_rate_rads;
     return true;
 }
 
-// get target's heading in degrees (0 = north, 90 = east)
-bool AP_Follow::get_target_heading_deg(float &heading) const
+// Retrieves the target's estimated global location and velocity, adjusting altitude frame if relative mode is set (for LUA bindings).
+bool AP_Follow::get_target_location_and_velocity(Location &loc, Vector3f &vel_ned)
 {
-    // exit immediately if not enabled
-    if (!_enabled) {
+    if (!update_estimate()) {
         return false;
     }
 
-    // check for timeout
-    if ((_last_heading_update_ms == 0) || (AP_HAL::millis() - _last_heading_update_ms > AP_FOLLOW_TIMEOUT_MS)) {
+    if (!AP::ahrs().get_location_from_origin_offset_NED(loc, _estimate_pos_ned_m)) {
         return false;
     }
+    vel_ned = _estimate_vel_ned_ms;
+
+    if (_alt_type == AP_FOLLOW_ALTITUDE_TYPE_RELATIVE) {
+        loc.change_alt_frame(Location::AltFrame::ABOVE_HOME);
+    }
 
-    // return latest heading estimate
-    heading = _target_heading;
     return true;
 }
 
-// returns true if we should extract information from msg
-bool AP_Follow::should_handle_message(const mavlink_message_t &msg) const
+// Retrieves the target's estimated global location and velocity, including configured offsets, for LUA bindings.
+bool AP_Follow::get_target_location_and_velocity_ofs(Location &loc, Vector3f &vel_ned)
 {
-    // exit immediately if not enabled
-    if (!_enabled) {
+    if (!update_estimate()) {
         return false;
     }
-
-    // skip our own messages
-    if (msg.sysid == mavlink_system.sysid) {
+    if (!AP::ahrs().get_location_from_origin_offset_NED(loc, _ofs_estimate_pos_ned_m)) {
         return false;
     }
+    if (_alt_type == AP_FOLLOW_ALTITUDE_TYPE_RELATIVE) {
+        loc.change_alt_frame(Location::AltFrame::ABOVE_HOME);
+    }
 
-    // skip message if not from our target
-    if (_sysid != 0 && msg.sysid != _sysid) {
+    vel_ned = _ofs_estimate_vel_ned_ms;
+    return true;
+}
+
+// Retrieves the estimated target heading in degrees (0° = North, 90° = East) for LUA bindings.
+bool AP_Follow::get_target_heading_deg(float &heading)
+{
+    if (!update_estimate()) {
         return false;
     }
 
+    // return latest heading estimate
+    heading = degrees(_estimate_heading_rad);
     return true;
 }
 
-// handle mavlink DISTANCE_SENSOR messages
+
+//==============================================================================
+// MAVLink Message Handling
+//==============================================================================
+
+// Handles incoming MAVLink messages to update the target's position, velocity, and heading.
 void AP_Follow::handle_msg(const mavlink_message_t &msg)
 {
-    // this method should be called from an "update()" method:
     if (_automatic_sysid) {
-        // maybe timeout who we were following...
+        // timeout check: if no position update received within timeout, clear automatic sysid
         if ((_last_location_update_ms == 0) ||
             (AP_HAL::millis() - _last_location_update_ms > AP_FOLLOW_SYSID_TIMEOUT_MS)) {
             _sysid.set(0);
+            _sysid_used = 0;
+            _estimate_valid = false;
         }
     }
 
     if (!should_handle_message(msg)) {
+        // ignore message if filtering rules reject it (e.g., wrong sysid)
         return;
     }
 
-    // decode global-position-int message
+    // decode MAVLink message
     bool updated = false;
 
     switch (msg.msgid) {
     case MAVLINK_MSG_ID_GLOBAL_POSITION_INT: {
+        // handle standard global position messages
         updated = handle_global_position_int_message(msg);
         break;
     }
     case MAVLINK_MSG_ID_FOLLOW_TARGET: {
+        // handle follow-target specific messages
         updated = handle_follow_target_message(msg);
         break;
     }
     }
 
     if (updated) {
+        // message provided updated position information
+        Vector3f offset_m = _offset_XXD_m.get();
+
+        // if using relative frame, rotate offsets based on estimated heading
+        if (_offset_type == AP_FOLLOW_OFFSET_TYPE_RELATIVE) {
+            offset_m.xy().rotate(_estimate_heading_rad);
+        }
+
+        Vector3f current_position_ned_m;
+        if (!AP::ahrs().get_relative_position_NED_origin(current_position_ned_m)) {
+            // 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
+            const Vector3p dist_vec_ned_m = _estimate_pos_ned_m - current_position_ned_m.topostype();
+            const Vector3p ofs_dist_vec = _ofs_estimate_pos_ned_m - current_position_ned_m.topostype();
+
+            // initialise offsets dynamically if needed
+            init_offsets_if_required(dist_vec_ned_m.tofloat());
+
+            // record distance and bearing to target for reporting/logging
+            if (ofs_dist_vec.xy().is_zero()) {
+                clear_dist_and_bearing_to_target();
+            } else {
+                _dist_to_target_m = ofs_dist_vec.xy().length();
+                _bearing_to_target_deg = degrees(ofs_dist_vec.xy().angle());
+            }
+
+            // clear distance and bearing if outside configured max follow distance
+            if (is_positive(_dist_max_m.get()) && (dist_vec_ned_m.length() > _dist_max_m)) {
+                clear_dist_and_bearing_to_target();
+            }
+        }
+
+        // Check if estimate needs reset based on position and velocity errors
+        if (estimate_error_too_large()) {
+            _estimate_valid = false;
+        }
+
 #if HAL_LOGGING_ENABLED
+        // log current follow diagnostic data
         Log_Write_FOLL();
 #endif
     }
 }
 
-bool AP_Follow::handle_global_position_int_message(const mavlink_message_t &msg)
+// Returns true if the incoming MAVLink message should be processed for target updates.
+bool AP_Follow::should_handle_message(const mavlink_message_t &msg) const
 {
-        // decode message
-        mavlink_global_position_int_t packet;
-        mavlink_msg_global_position_int_decode(&msg, &packet);
+    // exit immediately if not enabled
+    if (!_enabled) {
+        return false;
+    }
 
-        // ignore message if lat and lon are (exactly) zero
-        if ((packet.lat == 0 && packet.lon == 0)) {
-            return false;
-        }
+    // skip our own messages
+    if (msg.sysid == mavlink_system.sysid) {
+        return false;
+    }
 
-        Location _target_location;
-        _target_location.lat = packet.lat;
-        _target_location.lng = packet.lon;
+    // skip message if not from our target
+    if (_sysid != 0 && msg.sysid != _sysid) {
+        return false;
+    }
 
-        // select altitude source based on FOLL_ALT_TYPE param 
-        if (_alt_type == AP_FOLLOW_ALTITUDE_TYPE_RELATIVE) {
-            // above home alt
-            _target_location.set_alt_cm(packet.relative_alt / 10, Location::AltFrame::ABOVE_HOME);
-        } else {
-            // absolute altitude
-            _target_location.set_alt_cm(packet.alt / 10, Location::AltFrame::ABSOLUTE);
-        }
-        if (!_target_location.get_vector_from_origin_NEU(_target_position_ned)) {
-            return false;
-        }
-        _target_position_ned.z = -_target_position_ned.z; // NEU->NED
-        _target_position_ned *= 0.01;  // cm -> m
-
-        _target_velocity_ned.x = packet.vx * 0.01f; // velocity north
-        _target_velocity_ned.y = packet.vy * 0.01f; // velocity east
-        _target_velocity_ned.z = packet.vz * 0.01f; // velocity down
-
-        // get a local timestamp with correction for transport jitter
-        _last_location_update_ms = _jitter.correct_offboard_timestamp_msec(packet.time_boot_ms, AP_HAL::millis());
-        if (packet.hdg <= 36000) {                  // heading (UINT16_MAX if unknown)
-            _target_heading = packet.hdg * 0.01f;   // convert centi-degrees to degrees
-            _last_heading_update_ms = _last_location_update_ms;
-        }
-        // initialise _sysid if zero to sender's id
-        if (_sysid == 0) {
-            _sysid.set(msg.sysid);
-            _automatic_sysid = true;
-        }
-        return true;
+    return true;
 }
 
-bool AP_Follow::handle_follow_target_message(const mavlink_message_t &msg)
+// Checks whether the current estimate should be reset based on position and velocity errors.
+bool AP_Follow::estimate_error_too_large() const
 {
-        // decode message
-        mavlink_follow_target_t packet;
-        mavlink_msg_follow_target_decode(&msg, &packet);
+    const float timeout_sec = AP_FOLLOW_TIMEOUT_MS * 0.001f;
+    const float half_timeout_sec = timeout_sec * 0.5f;
 
-        // ignore message if lat and lon are (exactly) zero
-        if ((packet.lat == 0 && packet.lon == 0)) {
-            return false;
-        }
-        // require at least position
-        if ((packet.est_capabilities & (1<<0)) == 0) {
-            return false;
-        }
+    // Calculate position thresholds
+    const float pos_thresh_horiz_m = _accel_max_ne_mss.get() * sq(half_timeout_sec);
+    const float pos_thresh_vert_m  = _accel_max_d_mss.get()  * sq(half_timeout_sec);
 
-        const Location new_loc {
-            packet.lat,
-            packet.lon,
-            int32_t(packet.alt * 100),  // m -> cm
-            Location::AltFrame::ABSOLUTE
-        };
-
-        if (!new_loc.get_vector_from_origin_NEU(_target_position_ned)) {
-            return false;
-        }
-        _target_position_ned.z = -_target_position_ned.z; // NEU->NED
-        _target_position_ned *= 0.01;  // cm -> m
+    // Calculate velocity thresholds using the new helper function
+    const float vel_thresh_horiz_ms = calc_max_velocity_change(_accel_max_ne_mss.get(), _jerk_max_ne_msss.get(), timeout_sec);
+    const float vel_thresh_vert_ms  = calc_max_velocity_change(_accel_max_d_mss.get(), _jerk_max_d_msss.get(), timeout_sec);
 
-        if (packet.est_capabilities & (1<<1)) {
-            _target_velocity_ned.x = packet.vel[0]; // velocity north
-            _target_velocity_ned.y = packet.vel[1]; // velocity east
-            _target_velocity_ned.z = packet.vel[2]; // velocity down
-        } else {
-            _target_velocity_ned.zero();
-        }
+    // Calculate current position and velocity errors
+    Vector3f pos_error = _estimate_pos_ned_m.tofloat() - _target_pos_ned_m.tofloat();
+    Vector3f vel_error = _estimate_vel_ned_ms - _target_vel_ned_ms;
 
-        // get a local timestamp with correction for transport jitter
-        _last_location_update_ms = _jitter.correct_offboard_timestamp_msec(packet.timestamp, AP_HAL::millis());
+    Vector2f pos_error_xy = pos_error.xy();
+    float pos_error_z = pos_error.z;
+    Vector2f vel_error_xy = vel_error.xy();
+    float vel_error_z = vel_error.z;
 
-        if (packet.est_capabilities & (1<<3)) {
-            Quaternion q{packet.attitude_q[0], packet.attitude_q[1], packet.attitude_q[2], packet.attitude_q[3]};
-            float r, p, y;
-            q.to_euler(r,p,y);
-            _target_heading = degrees(y);
-            _last_heading_update_ms = _last_location_update_ms;
-        }
+    // Check horizontal and vertical separately
+    const bool pos_horiz_bad = pos_error_xy.length() > pos_thresh_horiz_m;
+    const bool vel_horiz_bad = vel_error_xy.length() > vel_thresh_horiz_ms;
+    const bool pos_vert_bad = fabsf(pos_error_z) > pos_thresh_vert_m;
+    const bool vel_vert_bad = fabsf(vel_error_z) > vel_thresh_vert_ms;
 
-        // initialise _sysid if zero to sender's id
-        if (_sysid == 0) {
-            _sysid.set(msg.sysid);
-            _automatic_sysid = true;
-        }
+    return pos_horiz_bad || vel_horiz_bad || pos_vert_bad || vel_vert_bad;
+}
 
-        return true;
+// Calculates max velocity change under trapezoidal or triangular acceleration profile (jerk-limited).
+float AP_Follow::calc_max_velocity_change(float accel_max, float jerk_max, float timeout_sec) const
+{
+    const float t_jerk = accel_max / jerk_max;
+    const float t_total_jerk = 2.0f * t_jerk;
+
+    if (timeout_sec >= t_total_jerk) {
+        // time to ramp up, constant accel phase, and ramp down
+        const float t_const = timeout_sec - t_total_jerk;
+        const float delta_v_jerk = 0.5f * accel_max * t_jerk;
+        const float delta_v_const = accel_max * t_const;
+        return 2.0f * delta_v_jerk + delta_v_const;
+    } else {
+        // timeout too short: pure triangle profile
+        const float t_half = timeout_sec * 0.5f;
+        return 0.5f * jerk_max * sq(t_half);
+    }
 }
 
-// write out an onboard-log message to help diagnose follow problems:
-#if HAL_LOGGING_ENABLED
-void AP_Follow::Log_Write_FOLL()
+// Decodes a GLOBAL_POSITION_INT MAVLink message to update the target’s position, velocity, and heading.
+bool AP_Follow::handle_global_position_int_message(const mavlink_message_t &msg)
 {
-        // get estimated location and velocity
-        Location loc_estimate{};
-        Vector3f vel_estimate;
-        UNUSED_RESULT(get_target_location_and_velocity(loc_estimate, vel_estimate));
-
-        Location _target_location;
-        UNUSED_RESULT(AP::ahrs().get_location_from_origin_offset_NED(_target_location, _target_position_ned * 0.01));
-        if (_alt_type == AP_FOLLOW_ALTITUDE_TYPE_RELATIVE) {
-            _target_location.change_alt_frame(Location::AltFrame::ABOVE_HOME);
-        }
+    // decode GLOBAL_POSITION_INT message into packet struct
+    mavlink_global_position_int_t packet;
+    mavlink_msg_global_position_int_decode(&msg, &packet);
+
+    // ignore message if latitude and longitude are exactly zero (invalid GPS fix)
+    if ((packet.lat == 0 && packet.lon == 0)) {
+        return false;
+    }
+
+    Location _target_location;
+    _target_location.lat = packet.lat;
+    _target_location.lng = packet.lon;
+
+    // set target altitude based on configured altitude type
+    if (_alt_type == AP_FOLLOW_ALTITUDE_TYPE_RELATIVE) {
+        // use relative altitude above home
+        _target_location.set_alt_cm(packet.relative_alt / 10, Location::AltFrame::ABOVE_HOME);
+    } else {
+        // use absolute altitude
+        _target_location.set_alt_cm(packet.alt / 10, Location::AltFrame::ABSOLUTE);
+    }
+
+    // convert global location to local NED frame position
+    if (!_target_location.get_vector_from_origin_NEU(_target_pos_ned_m)) {
+        return false;
+    }
+    _target_pos_ned_m.z = -_target_pos_ned_m.z; // convert NEU -> NED
+    _target_pos_ned_m *= 0.01;  // convert from cm to meters
+
+    // decode target velocity components (in m/s)
+    _target_vel_ned_ms.x = packet.vx * 0.01f; // velocity north
+    _target_vel_ned_ms.y = packet.vy * 0.01f; // velocity east
+    _target_vel_ned_ms.z = packet.vz * 0.01f; // velocity down
+
+    // target acceleration not available in GLOBAL_POSITION_INT
+    _target_accel_ned_mss.zero();
+
+    if (packet.hdg <= 36000) {
+        // valid heading field available (in centi-degrees)
+        _target_heading_deg = packet.hdg * 0.01f;
+    } else if (_offset_type == AP_FOLLOW_OFFSET_TYPE_RELATIVE) {
+        // heading missing but relative offset mode requires heading -> reject
+        return false;
+    } else {
+        // no heading available: set heading rate to zero
+        _target_heading_rate_degs = 0.0f;
+    }
+
+    // apply jitter-corrected timestamp to this update
+    _last_location_update_ms = _jitter.correct_offboard_timestamp_msec(packet.time_boot_ms, AP_HAL::millis());
 
-        // log lead's estimated vs reported position
-// @LoggerMessage: FOLL
-// @Description: Follow library diagnostic data
-// @Field: TimeUS: Time since system startup
-// @Field: Lat: Target latitude
-// @Field: Lon: Target longitude
-// @Field: Alt: Target absolute altitude
-// @Field: VelN: Target earth-frame velocity, North
-// @Field: VelE: Target earth-frame velocity, East
-// @Field: VelD: Target earth-frame velocity, Down
-// @Field: LatE: Vehicle latitude
-// @Field: LonE: Vehicle longitude
-// @Field: AltE: Vehicle absolute altitude
-        AP::logger().WriteStreaming("FOLL",
-                                               "TimeUS,Lat,Lon,Alt,VelN,VelE,VelD,LatE,LonE,AltE",  // labels
-                                               "sDUmnnnDUm",    // units
-                                               "F--B000--B",    // mults
-                                               "QLLifffLLi",    // fmt
-                                               AP_HAL::micros64(),
-                                               _target_location.lat,
-                                               _target_location.lng,
-                                               _target_location.alt,
-                                               (double)_target_velocity_ned.x,
-                                               (double)_target_velocity_ned.y,
-                                               (double)_target_velocity_ned.z,
-                                               loc_estimate.lat,
-                                               loc_estimate.lng,
-                                               loc_estimate.alt
-                                               );
+    // if sysid not yet set, adopt sender’s sysid and enable automatic sysid tracking
+    if (_sysid == 0) {
+        _sysid.set(msg.sysid);
+        _sysid_used = 0;
+        _estimate_valid = false;
+        _automatic_sysid = true;
+    }
+
+    return true;
 }
-#endif  // HAL_LOGGING_ENABLED
 
-// get velocity estimate in m/s in NED frame using dt since last update
-bool AP_Follow::get_velocity_ned(Vector3f &vel_ned, float dt) const
+// Decodes a FOLLOW_TARGET MAVLink message to update the target’s position, velocity, acceleration, and orientation.
+bool AP_Follow::handle_follow_target_message(const mavlink_message_t &msg)
 {
-    vel_ned = _target_velocity_ned + (_target_accel_ned * dt);
+    // decode FOLLOW_TARGET message into packet struct
+    mavlink_follow_target_t packet;
+    mavlink_msg_follow_target_decode(&msg, &packet);
+
+    // ignore message if latitude and longitude are exactly zero (invalid GPS fix)
+    if ((packet.lat == 0 && packet.lon == 0)) {
+        return false;
+    }
+
+    // require that at least position is estimated (bit 0 of est_capabilities)
+    if ((packet.est_capabilities & (1<<0)) == 0) {
+        return false;
+    }
+
+    // build Location object from latitude, longitude, and altitude (alt in meters)
+    const Location _target_location {
+        packet.lat,
+        packet.lon,
+        int32_t(packet.alt * 100),  // convert meters to centimeters
+        Location::AltFrame::ABSOLUTE
+    };
+
+    // convert global location to local NED frame position
+    if (!_target_location.get_vector_from_origin_NEU(_target_pos_ned_m)) {
+        return false;
+    }
+    _target_pos_ned_m *= 0.01; // convert from cm to meters
+
+    // adjust Z coordinate to NED frame (NEU altitude -> NED)
+    Location origin;
+    if (!AP::ahrs().get_origin(origin)) {
+        return false;
+    }
+    _target_pos_ned_m.z = -packet.alt + origin.alt * 0.01;
+
+    // decode velocity if available (bit 1 of est_capabilities)
+    if (packet.est_capabilities & (1<<1)) {
+        _target_vel_ned_ms.x = packet.vel[0]; // velocity north
+        _target_vel_ned_ms.y = packet.vel[1]; // velocity east
+        _target_vel_ned_ms.z = packet.vel[2]; // velocity down
+    } else {
+        _target_vel_ned_ms.zero();
+    }
+
+    // decode acceleration if available (bit 2 of est_capabilities)
+    if (packet.est_capabilities & (1 << 2)) {
+        _target_accel_ned_mss.x = packet.acc[0]; // acceleration north
+        _target_accel_ned_mss.y = packet.acc[1]; // acceleration east
+        _target_accel_ned_mss.z = packet.acc[2]; // acceleration down
+    } else {
+        _target_accel_ned_mss.zero();
path: libraries/AP_Follow/AP_Follow.cpp
user: login: timtuxworth
body: should not zero _target_accel_ned_mss as this will thump any values calculated elsewhere.
created_at: 2025-04-29T07:59:21Z
author_association: CONTRIBUTOR
diff_hunk: @@ -134,419 +155,638 @@ const AP_Param::GroupInfo AP_Follow::var_info[] = {
     AP_GROUPINFO("_ALT_TYPE", 10, AP_Follow, _alt_type, AP_FOLLOW_ALT_TYPE_DEFAULT),
 #endif
 
+// May add option to turn on input shaping, turn on acceleration due to rotation of offsest
+
     // @Param: _OPTIONS
     // @DisplayName: Follow options
     // @Description: Follow options bitmask
     // @Values: 0:None,1: Mount Follows lead vehicle on mode enter
     // @User: Standard
     AP_GROUPINFO("_OPTIONS", 11, AP_Follow, _options, 0),
 
+    // @Param: _ACCEL_NE
+    // @DisplayName: Acceleration limit for the horizontal kinematic input shaping
+    // @Description: Acceleration limit of the horizontal kinematic path generation used to determine how quickly the ship varies in velocity
+    // @Range: 0 5
+    // @Units: m/s/s
+    // @User: Advanced
+    AP_GROUPINFO("_ACCEL_NE", 12, AP_Follow, _accel_max_ne_mss, 2.5),
+
+    // @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 ship 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),
+
+    // @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 ship 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),
+
+    // @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 ship 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),
+
+    // @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 ship 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),
+
+    // @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 ship 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_GROUPEND
 };
 
-/* 
-   The constructor also initialises the proximity sensor. Note that this
-   constructor is not called until detect() returns true, so we
-   already know that we should setup the proximity sensor
-*/
+// Constructor for AP_Follow. Initializes the position P-controller and sets up parameter defaults.
 AP_Follow::AP_Follow() :
-        _p_pos(AP_FOLLOW_POS_P_DEFAULT)
+    _p_pos(AP_FOLLOW_POS_P_DEFAULT)
 {
     _singleton = this;
     AP_Param::setup_object_defaults(this, var_info);
 }
 
-// restore offsets to zero if necessary, should be called when vehicle exits follow mode
-void AP_Follow::clear_offsets_if_required()
+
+//==============================================================================
+// Target Estimation Update Functions
+//==============================================================================
+
+// Projects and updates the estimated target position, velocity, and heading based on last known data and configured input shaping.
+bool AP_Follow::update_estimate()
 {
-    if (_offsets_were_zero) {
-        _offset.set(Vector3f());
-        _offsets_were_zero = false;
+    // exit immediately if not enabled
+    if (!_enabled) {
path: libraries/AP_Follow/AP_Follow.cpp
user: login: lthall
body: Good suggestion.
created_at: 2025-05-01T12:03:47Z
author_association: CONTRIBUTOR
diff_hunk: @@ -134,419 +155,638 @@ const AP_Param::GroupInfo AP_Follow::var_info[] = {
     AP_GROUPINFO("_ALT_TYPE", 10, AP_Follow, _alt_type, AP_FOLLOW_ALT_TYPE_DEFAULT),
 #endif
 
+// May add option to turn on input shaping, turn on acceleration due to rotation of offsest
+
     // @Param: _OPTIONS
     // @DisplayName: Follow options
     // @Description: Follow options bitmask
     // @Values: 0:None,1: Mount Follows lead vehicle on mode enter
     // @User: Standard
     AP_GROUPINFO("_OPTIONS", 11, AP_Follow, _options, 0),
 
+    // @Param: _ACCEL_NE
+    // @DisplayName: Acceleration limit for the horizontal kinematic input shaping
+    // @Description: Acceleration limit of the horizontal kinematic path generation used to determine how quickly the ship varies in velocity
+    // @Range: 0 5
+    // @Units: m/s/s
+    // @User: Advanced
+    AP_GROUPINFO("_ACCEL_NE", 12, AP_Follow, _accel_max_ne_mss, 2.5),
+
+    // @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 ship 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),
+
+    // @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 ship 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),
+
+    // @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 ship 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),
+
+    // @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 ship 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),
+
+    // @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 ship 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_GROUPEND
 };
 
-/* 
-   The constructor also initialises the proximity sensor. Note that this
-   constructor is not called until detect() returns true, so we
-   already know that we should setup the proximity sensor
-*/
+// Constructor for AP_Follow. Initializes the position P-controller and sets up parameter defaults.
 AP_Follow::AP_Follow() :
-        _p_pos(AP_FOLLOW_POS_P_DEFAULT)
+    _p_pos(AP_FOLLOW_POS_P_DEFAULT)
 {
     _singleton = this;
     AP_Param::setup_object_defaults(this, var_info);
 }
 
-// restore offsets to zero if necessary, should be called when vehicle exits follow mode
-void AP_Follow::clear_offsets_if_required()
+
+//==============================================================================
+// Target Estimation Update Functions
+//==============================================================================
+
+// Projects and updates the estimated target position, velocity, and heading based on last known data and configured input shaping.
+bool AP_Follow::update_estimate()
 {
-    if (_offsets_were_zero) {
-        _offset.set(Vector3f());
-        _offsets_were_zero = false;
+    // exit immediately if not enabled
+    if (!_enabled) {
+        _estimate_valid = false;
+        return _estimate_valid;
     }
-}
 
-// get target's estimated location
-bool AP_Follow::get_target_location_and_velocity(Location &loc, Vector3f &vel_ned) const
-{
-    Vector3p pos_ned;
-    Vector3f local_vel_ned;  // so we either change both return parameters or neither
-    if (!get_target_position_and_velocity(pos_ned, local_vel_ned)) {
-        return false;
+    // check for timeout: if no recent location update, invalidate estimate
+    if ((_last_location_update_ms == 0) || (AP_HAL::millis() - _last_location_update_ms > AP_FOLLOW_TIMEOUT_MS)) {
+        _estimate_valid = false;
+        return _estimate_valid;
     }
-    if (!AP::ahrs().get_location_from_origin_offset_NED(loc, pos_ned)) {
-        return false;
+
+    // skip if update has already been processed this scheduler tick
+    if (_estimate_valid && (AP::scheduler().ticks32() - _last_update_ticks == 0)) {
+        return _estimate_valid;
     }
-    vel_ned = local_vel_ned;
+    _last_update_ticks = AP::scheduler().ticks32();
 
-    if (_alt_type == AP_FOLLOW_ALTITUDE_TYPE_RELATIVE) {
-        loc.change_alt_frame(Location::AltFrame::ABOVE_HOME);
+    // if sysid changed, reset the estimation state
+    if (_sysid != _sysid_used) {
+        _sysid_used = _sysid;
+        _estimate_valid = false;
     }
 
-    return true;
-}
+    const uint32_t now = AP_HAL::millis();
 
-// get target's estimated location and velocity, both NED SI from origin
-bool AP_Follow::get_target_position_and_velocity(Vector3p &pos_ned, Vector3f &vel_ned) const
-{
-    // exit immediately if not enabled
-    if (!_enabled) {
-        return false;
+    // calculate time since last location update in seconds
+    const float dt = (now - _last_location_update_ms) * 0.001f;
+
+    // project target's position and velocity forward using simple kinematics
+    Vector3f delta_pos = _target_vel_ned_ms * dt + _target_accel_ned_mss * 0.5f * sq(dt);
+    Vector3f delta_vel = _target_accel_ned_mss * dt;
+
+    // calculate time since last estimation update
+    const float e_dt = (now - _last_estimation_update_ms) * 0.001f;
+    
+    if (_estimate_valid) {
+        // 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)
+        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.xy().topostype(), _target_vel_ned_ms.xy() + delta_vel.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 angular shaping for heading estimate
+        shape_angle_vel_accel(radians(_target_heading_deg), radians(_target_heading_rate_degs), 0.0,
+                              _estimate_heading_rad, _estimate_heading_rate_rads, _estimate_heading_accel_radss,
+                              0.0, radians(_accel_max_h_degss),
+                              radians(_jerk_max_h_degsss), e_dt, false);
+
+        // update heading angle separately to maintain proper wrapping [-PI, PI]
+        postype_t estimate_heading_rad = _estimate_heading_rad;
+        update_pos_vel_accel(estimate_heading_rad, _estimate_heading_rate_rads, _estimate_heading_accel_radss, e_dt, 0.0, 0.0, 0.0);
+        _estimate_heading_rad = wrap_PI(estimate_heading_rad);
+    } else {
+        // no valid estimate yet: initialise from latest target position
+        _estimate_pos_ned_m = _target_pos_ned_m + delta_pos.topostype();
+        _estimate_vel_ned_ms = _target_vel_ned_ms + delta_vel;
+        _estimate_accel_ned_mss = _target_accel_ned_mss;
+        _estimate_valid = true;
     }
 
-    // check for timeout
-    if ((_last_location_update_ms == 0) || (AP_HAL::millis() - _last_location_update_ms > AP_FOLLOW_TIMEOUT_MS)) {
-        return false;
+    Vector3f offset_m = _offset_XXD_m.get();
+
+    // calculate estimated position and velocity with offsets applied
+    if (offset_m.is_zero() || (_offset_type == AP_FOLLOW_OFFSET_TYPE_NED)) {
+        // offsets are in NED frame: simple addition
+        _ofs_estimate_pos_ned_m = _estimate_pos_ned_m + offset_m.topostype();
+        _ofs_estimate_vel_ned_ms = _estimate_vel_ned_ms;
+        _ofs_estimate_accel_ned_mss = _estimate_accel_ned_mss;
+    } else {
+        // offsets are in FRD frame: rotate by heading
+        offset_m.xy().rotate(_estimate_heading_rad);
+        Vector3f offset_cross = offset_m.cross(Vector3f{0.0, 0.0, 1.0});
+        float offset_length_m = offset_m.length();
+        _ofs_estimate_pos_ned_m = _estimate_pos_ned_m + offset_m.topostype();
+        _ofs_estimate_vel_ned_ms = _estimate_vel_ned_ms + offset_cross * offset_length_m * _estimate_heading_rate_rads;
+        _ofs_estimate_accel_ned_mss = _estimate_accel_ned_mss + offset_cross * offset_length_m * _estimate_heading_accel_radss;
     }
 
-    // calculate time since last actual position update
-    const float dt = (AP_HAL::millis() - _last_location_update_ms) * 0.001f;
+    _last_estimation_update_ms = now;
+    return _estimate_valid;
+}
+
 
-    // get velocity estimate
-    if (!get_velocity_ned(vel_ned, dt)) {
+//==============================================================================
+// Target Information Retrieval Functions
+//==============================================================================
+
+// Retrieves the estimated target position, velocity, and acceleration in the NED frame (relative to origin).
+bool AP_Follow::get_target_pos_vel_accel_NED_m(Vector3p &pos_ned_m, Vector3f &vel_ned_ms, Vector3f &accel_ned_mss)
+{
+    if (!update_estimate()) {
         return false;
     }
 
-    // project the vehicle position
-    const Vector3p vel_ned_p { vel_ned.x, vel_ned.y, vel_ned.z };
-    pos_ned = _target_position_ned + vel_ned_p * dt;
+    pos_ned_m = _estimate_pos_ned_m;
+    vel_ned_ms = _estimate_vel_ned_ms;
+    accel_ned_mss = _estimate_accel_ned_mss;
 
     return true;
 }
 
-// get distance vector to target (in meters) and target's velocity all in NED frame
-bool AP_Follow::get_target_dist_and_vel_ned(Vector3f &dist_ned, Vector3f &dist_with_offs, Vector3f &vel_ned)
+// Retrieves the estimated target position, velocity, and acceleration in the NED frame, including configured offsets.
+bool AP_Follow::get_ofs_pos_vel_accel_NED_m(Vector3p &pos_ofs_ned_m, Vector3f &vel_ofs_ned_ms, Vector3f &accel_ofs_ned_mss)
path: libraries/AP_Follow/AP_Follow.cpp
user: login: lthall
body: We have two things we can follow, the target and the offset from the target. I felt calling one target and the other ofs was ok. Maybe target and target_ofs.

However putting the ofs after the pos_vel_accel, I think that would be changing the pattern I normally use.
created_at: 2025-05-01T12:07:07Z
author_association: CONTRIBUTOR
diff_hunk: @@ -134,419 +155,638 @@ const AP_Param::GroupInfo AP_Follow::var_info[] = {
     AP_GROUPINFO("_ALT_TYPE", 10, AP_Follow, _alt_type, AP_FOLLOW_ALT_TYPE_DEFAULT),
 #endif
 
+// May add option to turn on input shaping, turn on acceleration due to rotation of offsest
+
     // @Param: _OPTIONS
     // @DisplayName: Follow options
     // @Description: Follow options bitmask
     // @Values: 0:None,1: Mount Follows lead vehicle on mode enter
     // @User: Standard
     AP_GROUPINFO("_OPTIONS", 11, AP_Follow, _options, 0),
 
+    // @Param: _ACCEL_NE
+    // @DisplayName: Acceleration limit for the horizontal kinematic input shaping
+    // @Description: Acceleration limit of the horizontal kinematic path generation used to determine how quickly the ship varies in velocity
+    // @Range: 0 5
+    // @Units: m/s/s
+    // @User: Advanced
+    AP_GROUPINFO("_ACCEL_NE", 12, AP_Follow, _accel_max_ne_mss, 2.5),
+
+    // @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 ship 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),
+
+    // @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 ship 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),
+
+    // @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 ship 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),
+
+    // @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 ship 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),
+
+    // @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 ship 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_GROUPEND
 };
 
-/* 
-   The constructor also initialises the proximity sensor. Note that this
-   constructor is not called until detect() returns true, so we
-   already know that we should setup the proximity sensor
-*/
+// Constructor for AP_Follow. Initializes the position P-controller and sets up parameter defaults.
 AP_Follow::AP_Follow() :
-        _p_pos(AP_FOLLOW_POS_P_DEFAULT)
+    _p_pos(AP_FOLLOW_POS_P_DEFAULT)
 {
     _singleton = this;
     AP_Param::setup_object_defaults(this, var_info);
 }
 
-// restore offsets to zero if necessary, should be called when vehicle exits follow mode
-void AP_Follow::clear_offsets_if_required()
+
+//==============================================================================
+// Target Estimation Update Functions
+//==============================================================================
+
+// Projects and updates the estimated target position, velocity, and heading based on last known data and configured input shaping.
+bool AP_Follow::update_estimate()
 {
-    if (_offsets_were_zero) {
-        _offset.set(Vector3f());
-        _offsets_were_zero = false;
+    // exit immediately if not enabled
+    if (!_enabled) {
+        _estimate_valid = false;
+        return _estimate_valid;
     }
-}
 
-// get target's estimated location
-bool AP_Follow::get_target_location_and_velocity(Location &loc, Vector3f &vel_ned) const
-{
-    Vector3p pos_ned;
-    Vector3f local_vel_ned;  // so we either change both return parameters or neither
-    if (!get_target_position_and_velocity(pos_ned, local_vel_ned)) {
-        return false;
+    // check for timeout: if no recent location update, invalidate estimate
+    if ((_last_location_update_ms == 0) || (AP_HAL::millis() - _last_location_update_ms > AP_FOLLOW_TIMEOUT_MS)) {
+        _estimate_valid = false;
+        return _estimate_valid;
     }
-    if (!AP::ahrs().get_location_from_origin_offset_NED(loc, pos_ned)) {
-        return false;
+
+    // skip if update has already been processed this scheduler tick
+    if (_estimate_valid && (AP::scheduler().ticks32() - _last_update_ticks == 0)) {
+        return _estimate_valid;
     }
-    vel_ned = local_vel_ned;
+    _last_update_ticks = AP::scheduler().ticks32();
 
-    if (_alt_type == AP_FOLLOW_ALTITUDE_TYPE_RELATIVE) {
-        loc.change_alt_frame(Location::AltFrame::ABOVE_HOME);
+    // if sysid changed, reset the estimation state
+    if (_sysid != _sysid_used) {
+        _sysid_used = _sysid;
+        _estimate_valid = false;
     }
 
-    return true;
-}
+    const uint32_t now = AP_HAL::millis();
 
-// get target's estimated location and velocity, both NED SI from origin
-bool AP_Follow::get_target_position_and_velocity(Vector3p &pos_ned, Vector3f &vel_ned) const
-{
-    // exit immediately if not enabled
-    if (!_enabled) {
-        return false;
+    // calculate time since last location update in seconds
+    const float dt = (now - _last_location_update_ms) * 0.001f;
+
+    // project target's position and velocity forward using simple kinematics
+    Vector3f delta_pos = _target_vel_ned_ms * dt + _target_accel_ned_mss * 0.5f * sq(dt);
+    Vector3f delta_vel = _target_accel_ned_mss * dt;
+
+    // calculate time since last estimation update
+    const float e_dt = (now - _last_estimation_update_ms) * 0.001f;
+    
+    if (_estimate_valid) {
+        // 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)
+        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.xy().topostype(), _target_vel_ned_ms.xy() + delta_vel.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 angular shaping for heading estimate
+        shape_angle_vel_accel(radians(_target_heading_deg), radians(_target_heading_rate_degs), 0.0,
+                              _estimate_heading_rad, _estimate_heading_rate_rads, _estimate_heading_accel_radss,
+                              0.0, radians(_accel_max_h_degss),
+                              radians(_jerk_max_h_degsss), e_dt, false);
+
+        // update heading angle separately to maintain proper wrapping [-PI, PI]
+        postype_t estimate_heading_rad = _estimate_heading_rad;
+        update_pos_vel_accel(estimate_heading_rad, _estimate_heading_rate_rads, _estimate_heading_accel_radss, e_dt, 0.0, 0.0, 0.0);
+        _estimate_heading_rad = wrap_PI(estimate_heading_rad);
+    } else {
+        // no valid estimate yet: initialise from latest target position
+        _estimate_pos_ned_m = _target_pos_ned_m + delta_pos.topostype();
+        _estimate_vel_ned_ms = _target_vel_ned_ms + delta_vel;
+        _estimate_accel_ned_mss = _target_accel_ned_mss;
+        _estimate_valid = true;
     }
 
-    // check for timeout
-    if ((_last_location_update_ms == 0) || (AP_HAL::millis() - _last_location_update_ms > AP_FOLLOW_TIMEOUT_MS)) {
-        return false;
+    Vector3f offset_m = _offset_XXD_m.get();
+
+    // calculate estimated position and velocity with offsets applied
+    if (offset_m.is_zero() || (_offset_type == AP_FOLLOW_OFFSET_TYPE_NED)) {
+        // offsets are in NED frame: simple addition
+        _ofs_estimate_pos_ned_m = _estimate_pos_ned_m + offset_m.topostype();
+        _ofs_estimate_vel_ned_ms = _estimate_vel_ned_ms;
+        _ofs_estimate_accel_ned_mss = _estimate_accel_ned_mss;
+    } else {
+        // offsets are in FRD frame: rotate by heading
+        offset_m.xy().rotate(_estimate_heading_rad);
+        Vector3f offset_cross = offset_m.cross(Vector3f{0.0, 0.0, 1.0});
+        float offset_length_m = offset_m.length();
+        _ofs_estimate_pos_ned_m = _estimate_pos_ned_m + offset_m.topostype();
+        _ofs_estimate_vel_ned_ms = _estimate_vel_ned_ms + offset_cross * offset_length_m * _estimate_heading_rate_rads;
+        _ofs_estimate_accel_ned_mss = _estimate_accel_ned_mss + offset_cross * offset_length_m * _estimate_heading_accel_radss;
     }
 
-    // calculate time since last actual position update
-    const float dt = (AP_HAL::millis() - _last_location_update_ms) * 0.001f;
+    _last_estimation_update_ms = now;
+    return _estimate_valid;
+}
+
 
-    // get velocity estimate
-    if (!get_velocity_ned(vel_ned, dt)) {
+//==============================================================================
+// Target Information Retrieval Functions
+//==============================================================================
+
+// Retrieves the estimated target position, velocity, and acceleration in the NED frame (relative to origin).
+bool AP_Follow::get_target_pos_vel_accel_NED_m(Vector3p &pos_ned_m, Vector3f &vel_ned_ms, Vector3f &accel_ned_mss)
+{
+    if (!update_estimate()) {
         return false;
     }
 
-    // project the vehicle position
-    const Vector3p vel_ned_p { vel_ned.x, vel_ned.y, vel_ned.z };
-    pos_ned = _target_position_ned + vel_ned_p * dt;
+    pos_ned_m = _estimate_pos_ned_m;
+    vel_ned_ms = _estimate_vel_ned_ms;
+    accel_ned_mss = _estimate_accel_ned_mss;
 
     return true;
 }
 
-// get distance vector to target (in meters) and target's velocity all in NED frame
-bool AP_Follow::get_target_dist_and_vel_ned(Vector3f &dist_ned, Vector3f &dist_with_offs, Vector3f &vel_ned)
+// Retrieves the estimated target position, velocity, and acceleration in the NED frame, including configured offsets.
+bool AP_Follow::get_ofs_pos_vel_accel_NED_m(Vector3p &pos_ofs_ned_m, Vector3f &vel_ofs_ned_ms, Vector3f &accel_ofs_ned_mss)
 {
-    Vector3f current_position_NED;
-    if (!AP::ahrs().get_relative_position_NED_origin(current_position_NED)) {
-        clear_dist_and_bearing_to_target();
-        return false;
-    }
-    Vector3p target_position_NED;
-    Vector3f veh_vel;  // NED
-    if (!get_target_position_and_velocity(target_position_NED, veh_vel)) {
-        clear_dist_and_bearing_to_target();
+    if (!update_estimate()) {
         return false;
     }
 
-    // convert from Vector3p to Vector3f as they can't be subtracted:
-    Vector3f target_position_NED_f;
-    target_position_NED_f.x = target_position_NED.x;
-    target_position_NED_f.y = target_position_NED.y;
-    target_position_NED_f.z = target_position_NED.z;
+    pos_ofs_ned_m = _ofs_estimate_pos_ned_m;
+    vel_ofs_ned_ms = _ofs_estimate_vel_ned_ms;
+    accel_ofs_ned_mss = _ofs_estimate_accel_ned_mss;
 
-    const Vector3f dist_vec = target_position_NED_f - current_position_NED;
+    return true;
+}
 
-    // fail if too far
-    if (is_positive(_dist_max.get()) && (dist_vec.length() > _dist_max)) {
-        clear_dist_and_bearing_to_target();
+// Retrieves distance vectors (with and without configured offsets) and the target’s velocity, all in the NED frame.
+bool AP_Follow::get_target_dist_and_vel_NED_m(Vector3f &dist_ned, Vector3f &dist_with_offs, Vector3f &vel_ned)
+{
+    if (!update_estimate()) {
         return false;
     }
 
-    // initialise offsets from distance vector if required
-    init_offsets_if_required(dist_vec);
-
-    // get offsets
-    Vector3f offsets;
-    if (!get_offsets_ned(offsets)) {
+    Vector3f current_position_ned_m;
+    if (!AP::ahrs().get_relative_position_NED_origin(current_position_ned_m)) {
         clear_dist_and_bearing_to_target();
         return false;
     }
 
-    // calculate results
-    dist_ned = dist_vec;
-    dist_with_offs = dist_vec + offsets;
-    vel_ned = veh_vel;
+    const Vector3p dist_vec_ned_m = _estimate_pos_ned_m - current_position_ned_m.topostype();
+    const Vector3p ofs_dist_vec = _ofs_estimate_pos_ned_m - current_position_ned_m.topostype();
+    dist_ned = dist_vec_ned_m.tofloat();
+    dist_with_offs = ofs_dist_vec.tofloat();
+    vel_ned = _ofs_estimate_vel_ned_ms;
 
-    // record distance and heading for reporting purposes
-    if (is_zero(dist_with_offs.x) && is_zero(dist_with_offs.y)) {
-        clear_dist_and_bearing_to_target();
-    } else {
-        _dist_to_target = safe_sqrt(sq(dist_with_offs.x) + sq(dist_with_offs.y));
-        _bearing_to_target = degrees(atan2f(dist_with_offs.y, dist_with_offs.x));
+    return true;
+}
+
+// Retrieves the estimated target heading and heading rate in radians.
+bool AP_Follow::get_heading_heading_rate_rad(float &heading_rad, float &heading_rate_rads)
+{
+    if (!update_estimate()) {
+        return false;
     }
 
+    // return latest heading estimate
+    heading_rad = _estimate_heading_rad;
+    heading_rate_rads = _estimate_heading_rate_rads;
     return true;
 }
 
-// get target's heading in degrees (0 = north, 90 = east)
-bool AP_Follow::get_target_heading_deg(float &heading) const
+// Retrieves the target's estimated global location and velocity, adjusting altitude frame if relative mode is set (for LUA bindings).
+bool AP_Follow::get_target_location_and_velocity(Location &loc, Vector3f &vel_ned)
 {
-    // exit immediately if not enabled
-    if (!_enabled) {
+    if (!update_estimate()) {
         return false;
     }
 
-    // check for timeout
-    if ((_last_heading_update_ms == 0) || (AP_HAL::millis() - _last_heading_update_ms > AP_FOLLOW_TIMEOUT_MS)) {
+    if (!AP::ahrs().get_location_from_origin_offset_NED(loc, _estimate_pos_ned_m)) {
         return false;
     }
+    vel_ned = _estimate_vel_ned_ms;
+
+    if (_alt_type == AP_FOLLOW_ALTITUDE_TYPE_RELATIVE) {
+        loc.change_alt_frame(Location::AltFrame::ABOVE_HOME);
+    }
 
-    // return latest heading estimate
-    heading = _target_heading;
     return true;
 }
 
-// returns true if we should extract information from msg
-bool AP_Follow::should_handle_message(const mavlink_message_t &msg) const
+// Retrieves the target's estimated global location and velocity, including configured offsets, for LUA bindings.
+bool AP_Follow::get_target_location_and_velocity_ofs(Location &loc, Vector3f &vel_ned)
 {
-    // exit immediately if not enabled
-    if (!_enabled) {
+    if (!update_estimate()) {
         return false;
     }
-
-    // skip our own messages
-    if (msg.sysid == mavlink_system.sysid) {
+    if (!AP::ahrs().get_location_from_origin_offset_NED(loc, _ofs_estimate_pos_ned_m)) {
         return false;
     }
+    if (_alt_type == AP_FOLLOW_ALTITUDE_TYPE_RELATIVE) {
+        loc.change_alt_frame(Location::AltFrame::ABOVE_HOME);
+    }
 
-    // skip message if not from our target
-    if (_sysid != 0 && msg.sysid != _sysid) {
+    vel_ned = _ofs_estimate_vel_ned_ms;
+    return true;
+}
+
+// Retrieves the estimated target heading in degrees (0° = North, 90° = East) for LUA bindings.
+bool AP_Follow::get_target_heading_deg(float &heading)
+{
+    if (!update_estimate()) {
         return false;
     }
 
+    // return latest heading estimate
+    heading = degrees(_estimate_heading_rad);
     return true;
 }
 
-// handle mavlink DISTANCE_SENSOR messages
+
+//==============================================================================
+// MAVLink Message Handling
+//==============================================================================
+
+// Handles incoming MAVLink messages to update the target's position, velocity, and heading.
 void AP_Follow::handle_msg(const mavlink_message_t &msg)
 {
-    // this method should be called from an "update()" method:
     if (_automatic_sysid) {
-        // maybe timeout who we were following...
+        // timeout check: if no position update received within timeout, clear automatic sysid
         if ((_last_location_update_ms == 0) ||
             (AP_HAL::millis() - _last_location_update_ms > AP_FOLLOW_SYSID_TIMEOUT_MS)) {
             _sysid.set(0);
+            _sysid_used = 0;
+            _estimate_valid = false;
         }
     }
 
     if (!should_handle_message(msg)) {
+        // ignore message if filtering rules reject it (e.g., wrong sysid)
         return;
     }
 
-    // decode global-position-int message
+    // decode MAVLink message
     bool updated = false;
 
     switch (msg.msgid) {
     case MAVLINK_MSG_ID_GLOBAL_POSITION_INT: {
+        // handle standard global position messages
         updated = handle_global_position_int_message(msg);
         break;
     }
     case MAVLINK_MSG_ID_FOLLOW_TARGET: {
+        // handle follow-target specific messages
         updated = handle_follow_target_message(msg);
         break;
     }
     }
 
     if (updated) {
+        // message provided updated position information
+        Vector3f offset_m = _offset_XXD_m.get();
+
+        // if using relative frame, rotate offsets based on estimated heading
+        if (_offset_type == AP_FOLLOW_OFFSET_TYPE_RELATIVE) {
+            offset_m.xy().rotate(_estimate_heading_rad);
path: libraries/AP_Follow/AP_Follow.cpp
user: login: lthall
body: Fixed
created_at: 2025-05-01T12:41:58Z
author_association: CONTRIBUTOR
diff_hunk: @@ -134,419 +155,638 @@ const AP_Param::GroupInfo AP_Follow::var_info[] = {
     AP_GROUPINFO("_ALT_TYPE", 10, AP_Follow, _alt_type, AP_FOLLOW_ALT_TYPE_DEFAULT),
 #endif
 
+// May add option to turn on input shaping, turn on acceleration due to rotation of offsest
+
     // @Param: _OPTIONS
     // @DisplayName: Follow options
     // @Description: Follow options bitmask
     // @Values: 0:None,1: Mount Follows lead vehicle on mode enter
     // @User: Standard
     AP_GROUPINFO("_OPTIONS", 11, AP_Follow, _options, 0),
 
+    // @Param: _ACCEL_NE
+    // @DisplayName: Acceleration limit for the horizontal kinematic input shaping
+    // @Description: Acceleration limit of the horizontal kinematic path generation used to determine how quickly the ship varies in velocity
+    // @Range: 0 5
+    // @Units: m/s/s
+    // @User: Advanced
+    AP_GROUPINFO("_ACCEL_NE", 12, AP_Follow, _accel_max_ne_mss, 2.5),
+
+    // @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 ship 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),
+
+    // @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 ship 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),
+
+    // @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 ship 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),
+
+    // @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 ship 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),
+
+    // @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 ship 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_GROUPEND
 };
 
-/* 
-   The constructor also initialises the proximity sensor. Note that this
-   constructor is not called until detect() returns true, so we
-   already know that we should setup the proximity sensor
-*/
+// Constructor for AP_Follow. Initializes the position P-controller and sets up parameter defaults.
 AP_Follow::AP_Follow() :
-        _p_pos(AP_FOLLOW_POS_P_DEFAULT)
+    _p_pos(AP_FOLLOW_POS_P_DEFAULT)
 {
     _singleton = this;
     AP_Param::setup_object_defaults(this, var_info);
 }
 
-// restore offsets to zero if necessary, should be called when vehicle exits follow mode
-void AP_Follow::clear_offsets_if_required()
+
+//==============================================================================
+// Target Estimation Update Functions
+//==============================================================================
+
+// Projects and updates the estimated target position, velocity, and heading based on last known data and configured input shaping.
+bool AP_Follow::update_estimate()
 {
-    if (_offsets_were_zero) {
-        _offset.set(Vector3f());
-        _offsets_were_zero = false;
+    // exit immediately if not enabled
+    if (!_enabled) {
+        _estimate_valid = false;
+        return _estimate_valid;
     }
-}
 
-// get target's estimated location
-bool AP_Follow::get_target_location_and_velocity(Location &loc, Vector3f &vel_ned) const
-{
-    Vector3p pos_ned;
-    Vector3f local_vel_ned;  // so we either change both return parameters or neither
-    if (!get_target_position_and_velocity(pos_ned, local_vel_ned)) {
-        return false;
+    // check for timeout: if no recent location update, invalidate estimate
+    if ((_last_location_update_ms == 0) || (AP_HAL::millis() - _last_location_update_ms > AP_FOLLOW_TIMEOUT_MS)) {
+        _estimate_valid = false;
+        return _estimate_valid;
     }
-    if (!AP::ahrs().get_location_from_origin_offset_NED(loc, pos_ned)) {
-        return false;
+
+    // skip if update has already been processed this scheduler tick
+    if (_estimate_valid && (AP::scheduler().ticks32() - _last_update_ticks == 0)) {
+        return _estimate_valid;
     }
-    vel_ned = local_vel_ned;
+    _last_update_ticks = AP::scheduler().ticks32();
 
-    if (_alt_type == AP_FOLLOW_ALTITUDE_TYPE_RELATIVE) {
-        loc.change_alt_frame(Location::AltFrame::ABOVE_HOME);
+    // if sysid changed, reset the estimation state
+    if (_sysid != _sysid_used) {
+        _sysid_used = _sysid;
+        _estimate_valid = false;
     }
 
-    return true;
-}
+    const uint32_t now = AP_HAL::millis();
 
-// get target's estimated location and velocity, both NED SI from origin
-bool AP_Follow::get_target_position_and_velocity(Vector3p &pos_ned, Vector3f &vel_ned) const
-{
-    // exit immediately if not enabled
-    if (!_enabled) {
-        return false;
+    // calculate time since last location update in seconds
+    const float dt = (now - _last_location_update_ms) * 0.001f;
+
+    // project target's position and velocity forward using simple kinematics
+    Vector3f delta_pos = _target_vel_ned_ms * dt + _target_accel_ned_mss * 0.5f * sq(dt);
+    Vector3f delta_vel = _target_accel_ned_mss * dt;
+
+    // calculate time since last estimation update
+    const float e_dt = (now - _last_estimation_update_ms) * 0.001f;
+    
+    if (_estimate_valid) {
+        // 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)
+        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.xy().topostype(), _target_vel_ned_ms.xy() + delta_vel.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 angular shaping for heading estimate
+        shape_angle_vel_accel(radians(_target_heading_deg), radians(_target_heading_rate_degs), 0.0,
+                              _estimate_heading_rad, _estimate_heading_rate_rads, _estimate_heading_accel_radss,
+                              0.0, radians(_accel_max_h_degss),
+                              radians(_jerk_max_h_degsss), e_dt, false);
+
+        // update heading angle separately to maintain proper wrapping [-PI, PI]
+        postype_t estimate_heading_rad = _estimate_heading_rad;
+        update_pos_vel_accel(estimate_heading_rad, _estimate_heading_rate_rads, _estimate_heading_accel_radss, e_dt, 0.0, 0.0, 0.0);
+        _estimate_heading_rad = wrap_PI(estimate_heading_rad);
+    } else {
+        // no valid estimate yet: initialise from latest target position
+        _estimate_pos_ned_m = _target_pos_ned_m + delta_pos.topostype();
+        _estimate_vel_ned_ms = _target_vel_ned_ms + delta_vel;
+        _estimate_accel_ned_mss = _target_accel_ned_mss;
+        _estimate_valid = true;
     }
 
-    // check for timeout
-    if ((_last_location_update_ms == 0) || (AP_HAL::millis() - _last_location_update_ms > AP_FOLLOW_TIMEOUT_MS)) {
-        return false;
+    Vector3f offset_m = _offset_XXD_m.get();
+
+    // calculate estimated position and velocity with offsets applied
+    if (offset_m.is_zero() || (_offset_type == AP_FOLLOW_OFFSET_TYPE_NED)) {
+        // offsets are in NED frame: simple addition
+        _ofs_estimate_pos_ned_m = _estimate_pos_ned_m + offset_m.topostype();
+        _ofs_estimate_vel_ned_ms = _estimate_vel_ned_ms;
+        _ofs_estimate_accel_ned_mss = _estimate_accel_ned_mss;
+    } else {
+        // offsets are in FRD frame: rotate by heading
+        offset_m.xy().rotate(_estimate_heading_rad);
+        Vector3f offset_cross = offset_m.cross(Vector3f{0.0, 0.0, 1.0});
+        float offset_length_m = offset_m.length();
+        _ofs_estimate_pos_ned_m = _estimate_pos_ned_m + offset_m.topostype();
+        _ofs_estimate_vel_ned_ms = _estimate_vel_ned_ms + offset_cross * offset_length_m * _estimate_heading_rate_rads;
+        _ofs_estimate_accel_ned_mss = _estimate_accel_ned_mss + offset_cross * offset_length_m * _estimate_heading_accel_radss;
     }
 
-    // calculate time since last actual position update
-    const float dt = (AP_HAL::millis() - _last_location_update_ms) * 0.001f;
+    _last_estimation_update_ms = now;
+    return _estimate_valid;
+}
+
 
-    // get velocity estimate
-    if (!get_velocity_ned(vel_ned, dt)) {
+//==============================================================================
+// Target Information Retrieval Functions
+//==============================================================================
+
+// Retrieves the estimated target position, velocity, and acceleration in the NED frame (relative to origin).
+bool AP_Follow::get_target_pos_vel_accel_NED_m(Vector3p &pos_ned_m, Vector3f &vel_ned_ms, Vector3f &accel_ned_mss)
+{
+    if (!update_estimate()) {
         return false;
     }
 
-    // project the vehicle position
-    const Vector3p vel_ned_p { vel_ned.x, vel_ned.y, vel_ned.z };
-    pos_ned = _target_position_ned + vel_ned_p * dt;
+    pos_ned_m = _estimate_pos_ned_m;
+    vel_ned_ms = _estimate_vel_ned_ms;
+    accel_ned_mss = _estimate_accel_ned_mss;
 
     return true;
 }
 
-// get distance vector to target (in meters) and target's velocity all in NED frame
-bool AP_Follow::get_target_dist_and_vel_ned(Vector3f &dist_ned, Vector3f &dist_with_offs, Vector3f &vel_ned)
+// Retrieves the estimated target position, velocity, and acceleration in the NED frame, including configured offsets.
+bool AP_Follow::get_ofs_pos_vel_accel_NED_m(Vector3p &pos_ofs_ned_m, Vector3f &vel_ofs_ned_ms, Vector3f &accel_ofs_ned_mss)
 {
-    Vector3f current_position_NED;
-    if (!AP::ahrs().get_relative_position_NED_origin(current_position_NED)) {
-        clear_dist_and_bearing_to_target();
-        return false;
-    }
-    Vector3p target_position_NED;
-    Vector3f veh_vel;  // NED
-    if (!get_target_position_and_velocity(target_position_NED, veh_vel)) {
-        clear_dist_and_bearing_to_target();
+    if (!update_estimate()) {
         return false;
     }
 
-    // convert from Vector3p to Vector3f as they can't be subtracted:
-    Vector3f target_position_NED_f;
-    target_position_NED_f.x = target_position_NED.x;
-    target_position_NED_f.y = target_position_NED.y;
-    target_position_NED_f.z = target_position_NED.z;
+    pos_ofs_ned_m = _ofs_estimate_pos_ned_m;
+    vel_ofs_ned_ms = _ofs_estimate_vel_ned_ms;
+    accel_ofs_ned_mss = _ofs_estimate_accel_ned_mss;
 
-    const Vector3f dist_vec = target_position_NED_f - current_position_NED;
+    return true;
+}
 
-    // fail if too far
-    if (is_positive(_dist_max.get()) && (dist_vec.length() > _dist_max)) {
-        clear_dist_and_bearing_to_target();
+// Retrieves distance vectors (with and without configured offsets) and the target’s velocity, all in the NED frame.
+bool AP_Follow::get_target_dist_and_vel_NED_m(Vector3f &dist_ned, Vector3f &dist_with_offs, Vector3f &vel_ned)
+{
+    if (!update_estimate()) {
         return false;
     }
 
-    // initialise offsets from distance vector if required
-    init_offsets_if_required(dist_vec);
-
-    // get offsets
-    Vector3f offsets;
-    if (!get_offsets_ned(offsets)) {
+    Vector3f current_position_ned_m;
+    if (!AP::ahrs().get_relative_position_NED_origin(current_position_ned_m)) {
         clear_dist_and_bearing_to_target();
         return false;
     }
 
-    // calculate results
-    dist_ned = dist_vec;
-    dist_with_offs = dist_vec + offsets;
-    vel_ned = veh_vel;
+    const Vector3p dist_vec_ned_m = _estimate_pos_ned_m - current_position_ned_m.topostype();
+    const Vector3p ofs_dist_vec = _ofs_estimate_pos_ned_m - current_position_ned_m.topostype();
+    dist_ned = dist_vec_ned_m.tofloat();
+    dist_with_offs = ofs_dist_vec.tofloat();
+    vel_ned = _ofs_estimate_vel_ned_ms;
 
-    // record distance and heading for reporting purposes
-    if (is_zero(dist_with_offs.x) && is_zero(dist_with_offs.y)) {
-        clear_dist_and_bearing_to_target();
-    } else {
-        _dist_to_target = safe_sqrt(sq(dist_with_offs.x) + sq(dist_with_offs.y));
-        _bearing_to_target = degrees(atan2f(dist_with_offs.y, dist_with_offs.x));
+    return true;
+}
+
+// Retrieves the estimated target heading and heading rate in radians.
+bool AP_Follow::get_heading_heading_rate_rad(float &heading_rad, float &heading_rate_rads)
+{
+    if (!update_estimate()) {
+        return false;
     }
 
+    // return latest heading estimate
+    heading_rad = _estimate_heading_rad;
+    heading_rate_rads = _estimate_heading_rate_rads;
     return true;
 }
 
-// get target's heading in degrees (0 = north, 90 = east)
-bool AP_Follow::get_target_heading_deg(float &heading) const
+// Retrieves the target's estimated global location and velocity, adjusting altitude frame if relative mode is set (for LUA bindings).
+bool AP_Follow::get_target_location_and_velocity(Location &loc, Vector3f &vel_ned)
 {
-    // exit immediately if not enabled
-    if (!_enabled) {
+    if (!update_estimate()) {
         return false;
     }
 
-    // check for timeout
-    if ((_last_heading_update_ms == 0) || (AP_HAL::millis() - _last_heading_update_ms > AP_FOLLOW_TIMEOUT_MS)) {
+    if (!AP::ahrs().get_location_from_origin_offset_NED(loc, _estimate_pos_ned_m)) {
         return false;
     }
+    vel_ned = _estimate_vel_ned_ms;
+
+    if (_alt_type == AP_FOLLOW_ALTITUDE_TYPE_RELATIVE) {
+        loc.change_alt_frame(Location::AltFrame::ABOVE_HOME);
+    }
 
-    // return latest heading estimate
-    heading = _target_heading;
     return true;
 }
 
-// returns true if we should extract information from msg
-bool AP_Follow::should_handle_message(const mavlink_message_t &msg) const
+// Retrieves the target's estimated global location and velocity, including configured offsets, for LUA bindings.
+bool AP_Follow::get_target_location_and_velocity_ofs(Location &loc, Vector3f &vel_ned)
 {
-    // exit immediately if not enabled
-    if (!_enabled) {
+    if (!update_estimate()) {
         return false;
     }
-
-    // skip our own messages
-    if (msg.sysid == mavlink_system.sysid) {
+    if (!AP::ahrs().get_location_from_origin_offset_NED(loc, _ofs_estimate_pos_ned_m)) {
         return false;
     }
+    if (_alt_type == AP_FOLLOW_ALTITUDE_TYPE_RELATIVE) {
+        loc.change_alt_frame(Location::AltFrame::ABOVE_HOME);
+    }
 
-    // skip message if not from our target
-    if (_sysid != 0 && msg.sysid != _sysid) {
+    vel_ned = _ofs_estimate_vel_ned_ms;
+    return true;
+}
+
+// Retrieves the estimated target heading in degrees (0° = North, 90° = East) for LUA bindings.
+bool AP_Follow::get_target_heading_deg(float &heading)
+{
+    if (!update_estimate()) {
         return false;
     }
 
+    // return latest heading estimate
+    heading = degrees(_estimate_heading_rad);
     return true;
 }
 
-// handle mavlink DISTANCE_SENSOR messages
+
+//==============================================================================
+// MAVLink Message Handling
+//==============================================================================
+
+// Handles incoming MAVLink messages to update the target's position, velocity, and heading.
 void AP_Follow::handle_msg(const mavlink_message_t &msg)
 {
-    // this method should be called from an "update()" method:
     if (_automatic_sysid) {
-        // maybe timeout who we were following...
+        // timeout check: if no position update received within timeout, clear automatic sysid
         if ((_last_location_update_ms == 0) ||
             (AP_HAL::millis() - _last_location_update_ms > AP_FOLLOW_SYSID_TIMEOUT_MS)) {
             _sysid.set(0);
+            _sysid_used = 0;
+            _estimate_valid = false;
         }
     }
 
     if (!should_handle_message(msg)) {
+        // ignore message if filtering rules reject it (e.g., wrong sysid)
         return;
     }
 
-    // decode global-position-int message
+    // decode MAVLink message
     bool updated = false;
 
     switch (msg.msgid) {
     case MAVLINK_MSG_ID_GLOBAL_POSITION_INT: {
+        // handle standard global position messages
         updated = handle_global_position_int_message(msg);
         break;
     }
     case MAVLINK_MSG_ID_FOLLOW_TARGET: {
+        // handle follow-target specific messages
         updated = handle_follow_target_message(msg);
         break;
     }
     }
 
     if (updated) {
+        // message provided updated position information
+        Vector3f offset_m = _offset_XXD_m.get();
+
+        // if using relative frame, rotate offsets based on estimated heading
+        if (_offset_type == AP_FOLLOW_OFFSET_TYPE_RELATIVE) {
+            offset_m.xy().rotate(_estimate_heading_rad);
+        }
+
+        Vector3f current_position_ned_m;
+        if (!AP::ahrs().get_relative_position_NED_origin(current_position_ned_m)) {
+            // 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
+            const Vector3p dist_vec_ned_m = _estimate_pos_ned_m - current_position_ned_m.topostype();
+            const Vector3p ofs_dist_vec = _ofs_estimate_pos_ned_m - current_position_ned_m.topostype();
path: libraries/AP_Follow/AP_Follow.cpp
user: login: lthall
body: fixed
created_at: 2025-05-01T12:42:18Z
author_association: CONTRIBUTOR
diff_hunk: @@ -134,419 +155,638 @@ const AP_Param::GroupInfo AP_Follow::var_info[] = {
     AP_GROUPINFO("_ALT_TYPE", 10, AP_Follow, _alt_type, AP_FOLLOW_ALT_TYPE_DEFAULT),
 #endif
 
+// May add option to turn on input shaping, turn on acceleration due to rotation of offsest
+
     // @Param: _OPTIONS
     // @DisplayName: Follow options
     // @Description: Follow options bitmask
     // @Values: 0:None,1: Mount Follows lead vehicle on mode enter
     // @User: Standard
     AP_GROUPINFO("_OPTIONS", 11, AP_Follow, _options, 0),
 
+    // @Param: _ACCEL_NE
+    // @DisplayName: Acceleration limit for the horizontal kinematic input shaping
+    // @Description: Acceleration limit of the horizontal kinematic path generation used to determine how quickly the ship varies in velocity
+    // @Range: 0 5
+    // @Units: m/s/s
+    // @User: Advanced
+    AP_GROUPINFO("_ACCEL_NE", 12, AP_Follow, _accel_max_ne_mss, 2.5),
+
+    // @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 ship 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),
+
+    // @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 ship 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),
+
+    // @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 ship 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),
+
+    // @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 ship 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),
+
+    // @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 ship 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_GROUPEND
 };
 
-/* 
-   The constructor also initialises the proximity sensor. Note that this
-   constructor is not called until detect() returns true, so we
-   already know that we should setup the proximity sensor
-*/
+// Constructor for AP_Follow. Initializes the position P-controller and sets up parameter defaults.
 AP_Follow::AP_Follow() :
-        _p_pos(AP_FOLLOW_POS_P_DEFAULT)
+    _p_pos(AP_FOLLOW_POS_P_DEFAULT)
 {
     _singleton = this;
     AP_Param::setup_object_defaults(this, var_info);
 }
 
-// restore offsets to zero if necessary, should be called when vehicle exits follow mode
-void AP_Follow::clear_offsets_if_required()
+
+//==============================================================================
+// Target Estimation Update Functions
+//==============================================================================
+
+// Projects and updates the estimated target position, velocity, and heading based on last known data and configured input shaping.
+bool AP_Follow::update_estimate()
 {
-    if (_offsets_were_zero) {
-        _offset.set(Vector3f());
-        _offsets_were_zero = false;
+    // exit immediately if not enabled
+    if (!_enabled) {
+        _estimate_valid = false;
+        return _estimate_valid;
     }
-}
 
-// get target's estimated location
-bool AP_Follow::get_target_location_and_velocity(Location &loc, Vector3f &vel_ned) const
-{
-    Vector3p pos_ned;
-    Vector3f local_vel_ned;  // so we either change both return parameters or neither
-    if (!get_target_position_and_velocity(pos_ned, local_vel_ned)) {
-        return false;
+    // check for timeout: if no recent location update, invalidate estimate
+    if ((_last_location_update_ms == 0) || (AP_HAL::millis() - _last_location_update_ms > AP_FOLLOW_TIMEOUT_MS)) {
+        _estimate_valid = false;
+        return _estimate_valid;
     }
-    if (!AP::ahrs().get_location_from_origin_offset_NED(loc, pos_ned)) {
-        return false;
+
+    // skip if update has already been processed this scheduler tick
+    if (_estimate_valid && (AP::scheduler().ticks32() - _last_update_ticks == 0)) {
+        return _estimate_valid;
     }
-    vel_ned = local_vel_ned;
+    _last_update_ticks = AP::scheduler().ticks32();
 
-    if (_alt_type == AP_FOLLOW_ALTITUDE_TYPE_RELATIVE) {
-        loc.change_alt_frame(Location::AltFrame::ABOVE_HOME);
+    // if sysid changed, reset the estimation state
+    if (_sysid != _sysid_used) {
+        _sysid_used = _sysid;
+        _estimate_valid = false;
     }
 
-    return true;
-}
+    const uint32_t now = AP_HAL::millis();
 
-// get target's estimated location and velocity, both NED SI from origin
-bool AP_Follow::get_target_position_and_velocity(Vector3p &pos_ned, Vector3f &vel_ned) const
-{
-    // exit immediately if not enabled
-    if (!_enabled) {
-        return false;
+    // calculate time since last location update in seconds
+    const float dt = (now - _last_location_update_ms) * 0.001f;
+
+    // project target's position and velocity forward using simple kinematics
+    Vector3f delta_pos = _target_vel_ned_ms * dt + _target_accel_ned_mss * 0.5f * sq(dt);
+    Vector3f delta_vel = _target_accel_ned_mss * dt;
+
+    // calculate time since last estimation update
+    const float e_dt = (now - _last_estimation_update_ms) * 0.001f;
+    
+    if (_estimate_valid) {
+        // 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)
+        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.xy().topostype(), _target_vel_ned_ms.xy() + delta_vel.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 angular shaping for heading estimate
+        shape_angle_vel_accel(radians(_target_heading_deg), radians(_target_heading_rate_degs), 0.0,
+                              _estimate_heading_rad, _estimate_heading_rate_rads, _estimate_heading_accel_radss,
+                              0.0, radians(_accel_max_h_degss),
+                              radians(_jerk_max_h_degsss), e_dt, false);
+
+        // update heading angle separately to maintain proper wrapping [-PI, PI]
+        postype_t estimate_heading_rad = _estimate_heading_rad;
+        update_pos_vel_accel(estimate_heading_rad, _estimate_heading_rate_rads, _estimate_heading_accel_radss, e_dt, 0.0, 0.0, 0.0);
+        _estimate_heading_rad = wrap_PI(estimate_heading_rad);
+    } else {
+        // no valid estimate yet: initialise from latest target position
+        _estimate_pos_ned_m = _target_pos_ned_m + delta_pos.topostype();
+        _estimate_vel_ned_ms = _target_vel_ned_ms + delta_vel;
+        _estimate_accel_ned_mss = _target_accel_ned_mss;
+        _estimate_valid = true;
     }
 
-    // check for timeout
-    if ((_last_location_update_ms == 0) || (AP_HAL::millis() - _last_location_update_ms > AP_FOLLOW_TIMEOUT_MS)) {
-        return false;
+    Vector3f offset_m = _offset_XXD_m.get();
+
+    // calculate estimated position and velocity with offsets applied
+    if (offset_m.is_zero() || (_offset_type == AP_FOLLOW_OFFSET_TYPE_NED)) {
+        // offsets are in NED frame: simple addition
+        _ofs_estimate_pos_ned_m = _estimate_pos_ned_m + offset_m.topostype();
+        _ofs_estimate_vel_ned_ms = _estimate_vel_ned_ms;
+        _ofs_estimate_accel_ned_mss = _estimate_accel_ned_mss;
+    } else {
+        // offsets are in FRD frame: rotate by heading
+        offset_m.xy().rotate(_estimate_heading_rad);
+        Vector3f offset_cross = offset_m.cross(Vector3f{0.0, 0.0, 1.0});
+        float offset_length_m = offset_m.length();
+        _ofs_estimate_pos_ned_m = _estimate_pos_ned_m + offset_m.topostype();
+        _ofs_estimate_vel_ned_ms = _estimate_vel_ned_ms + offset_cross * offset_length_m * _estimate_heading_rate_rads;
+        _ofs_estimate_accel_ned_mss = _estimate_accel_ned_mss + offset_cross * offset_length_m * _estimate_heading_accel_radss;
     }
 
-    // calculate time since last actual position update
-    const float dt = (AP_HAL::millis() - _last_location_update_ms) * 0.001f;
+    _last_estimation_update_ms = now;
+    return _estimate_valid;
+}
+
 
-    // get velocity estimate
-    if (!get_velocity_ned(vel_ned, dt)) {
+//==============================================================================
+// Target Information Retrieval Functions
+//==============================================================================
+
+// Retrieves the estimated target position, velocity, and acceleration in the NED frame (relative to origin).
+bool AP_Follow::get_target_pos_vel_accel_NED_m(Vector3p &pos_ned_m, Vector3f &vel_ned_ms, Vector3f &accel_ned_mss)
+{
+    if (!update_estimate()) {
         return false;
     }
 
-    // project the vehicle position
-    const Vector3p vel_ned_p { vel_ned.x, vel_ned.y, vel_ned.z };
-    pos_ned = _target_position_ned + vel_ned_p * dt;
+    pos_ned_m = _estimate_pos_ned_m;
+    vel_ned_ms = _estimate_vel_ned_ms;
+    accel_ned_mss = _estimate_accel_ned_mss;
 
     return true;
 }
 
-// get distance vector to target (in meters) and target's velocity all in NED frame
-bool AP_Follow::get_target_dist_and_vel_ned(Vector3f &dist_ned, Vector3f &dist_with_offs, Vector3f &vel_ned)
+// Retrieves the estimated target position, velocity, and acceleration in the NED frame, including configured offsets.
+bool AP_Follow::get_ofs_pos_vel_accel_NED_m(Vector3p &pos_ofs_ned_m, Vector3f &vel_ofs_ned_ms, Vector3f &accel_ofs_ned_mss)
 {
-    Vector3f current_position_NED;
-    if (!AP::ahrs().get_relative_position_NED_origin(current_position_NED)) {
-        clear_dist_and_bearing_to_target();
-        return false;
-    }
-    Vector3p target_position_NED;
-    Vector3f veh_vel;  // NED
-    if (!get_target_position_and_velocity(target_position_NED, veh_vel)) {
-        clear_dist_and_bearing_to_target();
+    if (!update_estimate()) {
         return false;
     }
 
-    // convert from Vector3p to Vector3f as they can't be subtracted:
-    Vector3f target_position_NED_f;
-    target_position_NED_f.x = target_position_NED.x;
-    target_position_NED_f.y = target_position_NED.y;
-    target_position_NED_f.z = target_position_NED.z;
+    pos_ofs_ned_m = _ofs_estimate_pos_ned_m;
+    vel_ofs_ned_ms = _ofs_estimate_vel_ned_ms;
+    accel_ofs_ned_mss = _ofs_estimate_accel_ned_mss;
 
-    const Vector3f dist_vec = target_position_NED_f - current_position_NED;
+    return true;
+}
 
-    // fail if too far
-    if (is_positive(_dist_max.get()) && (dist_vec.length() > _dist_max)) {
-        clear_dist_and_bearing_to_target();
+// Retrieves distance vectors (with and without configured offsets) and the target’s velocity, all in the NED frame.
+bool AP_Follow::get_target_dist_and_vel_NED_m(Vector3f &dist_ned, Vector3f &dist_with_offs, Vector3f &vel_ned)
+{
+    if (!update_estimate()) {
         return false;
     }
 
-    // initialise offsets from distance vector if required
-    init_offsets_if_required(dist_vec);
-
-    // get offsets
-    Vector3f offsets;
-    if (!get_offsets_ned(offsets)) {
+    Vector3f current_position_ned_m;
+    if (!AP::ahrs().get_relative_position_NED_origin(current_position_ned_m)) {
         clear_dist_and_bearing_to_target();
         return false;
     }
 
-    // calculate results
-    dist_ned = dist_vec;
-    dist_with_offs = dist_vec + offsets;
-    vel_ned = veh_vel;
+    const Vector3p dist_vec_ned_m = _estimate_pos_ned_m - current_position_ned_m.topostype();
+    const Vector3p ofs_dist_vec = _ofs_estimate_pos_ned_m - current_position_ned_m.topostype();
+    dist_ned = dist_vec_ned_m.tofloat();
+    dist_with_offs = ofs_dist_vec.tofloat();
+    vel_ned = _ofs_estimate_vel_ned_ms;
 
-    // record distance and heading for reporting purposes
-    if (is_zero(dist_with_offs.x) && is_zero(dist_with_offs.y)) {
-        clear_dist_and_bearing_to_target();
-    } else {
-        _dist_to_target = safe_sqrt(sq(dist_with_offs.x) + sq(dist_with_offs.y));
-        _bearing_to_target = degrees(atan2f(dist_with_offs.y, dist_with_offs.x));
+    return true;
+}
+
+// Retrieves the estimated target heading and heading rate in radians.
+bool AP_Follow::get_heading_heading_rate_rad(float &heading_rad, float &heading_rate_rads)
+{
+    if (!update_estimate()) {
+        return false;
     }
 
+    // return latest heading estimate
+    heading_rad = _estimate_heading_rad;
+    heading_rate_rads = _estimate_heading_rate_rads;
     return true;
 }
 
-// get target's heading in degrees (0 = north, 90 = east)
-bool AP_Follow::get_target_heading_deg(float &heading) const
+// Retrieves the target's estimated global location and velocity, adjusting altitude frame if relative mode is set (for LUA bindings).
+bool AP_Follow::get_target_location_and_velocity(Location &loc, Vector3f &vel_ned)
 {
-    // exit immediately if not enabled
-    if (!_enabled) {
+    if (!update_estimate()) {
         return false;
     }
 
-    // check for timeout
-    if ((_last_heading_update_ms == 0) || (AP_HAL::millis() - _last_heading_update_ms > AP_FOLLOW_TIMEOUT_MS)) {
+    if (!AP::ahrs().get_location_from_origin_offset_NED(loc, _estimate_pos_ned_m)) {
         return false;
     }
+    vel_ned = _estimate_vel_ned_ms;
+
+    if (_alt_type == AP_FOLLOW_ALTITUDE_TYPE_RELATIVE) {
+        loc.change_alt_frame(Location::AltFrame::ABOVE_HOME);
+    }
 
-    // return latest heading estimate
-    heading = _target_heading;
     return true;
 }
 
-// returns true if we should extract information from msg
-bool AP_Follow::should_handle_message(const mavlink_message_t &msg) const
+// Retrieves the target's estimated global location and velocity, including configured offsets, for LUA bindings.
+bool AP_Follow::get_target_location_and_velocity_ofs(Location &loc, Vector3f &vel_ned)
 {
-    // exit immediately if not enabled
-    if (!_enabled) {
+    if (!update_estimate()) {
         return false;
     }
-
-    // skip our own messages
-    if (msg.sysid == mavlink_system.sysid) {
+    if (!AP::ahrs().get_location_from_origin_offset_NED(loc, _ofs_estimate_pos_ned_m)) {
         return false;
     }
+    if (_alt_type == AP_FOLLOW_ALTITUDE_TYPE_RELATIVE) {
+        loc.change_alt_frame(Location::AltFrame::ABOVE_HOME);
+    }
 
-    // skip message if not from our target
-    if (_sysid != 0 && msg.sysid != _sysid) {
+    vel_ned = _ofs_estimate_vel_ned_ms;
+    return true;
+}
+
+// Retrieves the estimated target heading in degrees (0° = North, 90° = East) for LUA bindings.
+bool AP_Follow::get_target_heading_deg(float &heading)
+{
+    if (!update_estimate()) {
         return false;
     }
 
+    // return latest heading estimate
+    heading = degrees(_estimate_heading_rad);
     return true;
 }
 
-// handle mavlink DISTANCE_SENSOR messages
+
+//==============================================================================
+// MAVLink Message Handling
+//==============================================================================
+
+// Handles incoming MAVLink messages to update the target's position, velocity, and heading.
 void AP_Follow::handle_msg(const mavlink_message_t &msg)
 {
-    // this method should be called from an "update()" method:
     if (_automatic_sysid) {
-        // maybe timeout who we were following...
+        // timeout check: if no position update received within timeout, clear automatic sysid
         if ((_last_location_update_ms == 0) ||
             (AP_HAL::millis() - _last_location_update_ms > AP_FOLLOW_SYSID_TIMEOUT_MS)) {
             _sysid.set(0);
+            _sysid_used = 0;
+            _estimate_valid = false;
         }
     }
 
     if (!should_handle_message(msg)) {
+        // ignore message if filtering rules reject it (e.g., wrong sysid)
         return;
     }
 
-    // decode global-position-int message
+    // decode MAVLink message
     bool updated = false;
 
     switch (msg.msgid) {
     case MAVLINK_MSG_ID_GLOBAL_POSITION_INT: {
+        // handle standard global position messages
         updated = handle_global_position_int_message(msg);
         break;
     }
     case MAVLINK_MSG_ID_FOLLOW_TARGET: {
+        // handle follow-target specific messages
         updated = handle_follow_target_message(msg);
         break;
     }
     }
 
     if (updated) {
+        // message provided updated position information
+        Vector3f offset_m = _offset_XXD_m.get();
+
+        // if using relative frame, rotate offsets based on estimated heading
+        if (_offset_type == AP_FOLLOW_OFFSET_TYPE_RELATIVE) {
+            offset_m.xy().rotate(_estimate_heading_rad);
+        }
+
+        Vector3f current_position_ned_m;
+        if (!AP::ahrs().get_relative_position_NED_origin(current_position_ned_m)) {
+            // 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
+            const Vector3p dist_vec_ned_m = _estimate_pos_ned_m - current_position_ned_m.topostype();
+            const Vector3p ofs_dist_vec = _ofs_estimate_pos_ned_m - current_position_ned_m.topostype();
+
+            // initialise offsets dynamically if needed
+            init_offsets_if_required(dist_vec_ned_m.tofloat());
+
+            // record distance and bearing to target for reporting/logging
+            if (ofs_dist_vec.xy().is_zero()) {
+                clear_dist_and_bearing_to_target();
+            } else {
+                _dist_to_target_m = ofs_dist_vec.xy().length();
+                _bearing_to_target_deg = degrees(ofs_dist_vec.xy().angle());
+            }
+
+            // clear distance and bearing if outside configured max follow distance
+            if (is_positive(_dist_max_m.get()) && (dist_vec_ned_m.length() > _dist_max_m)) {
+                clear_dist_and_bearing_to_target();
+            }
+        }
+
+        // Check if estimate needs reset based on position and velocity errors
+        if (estimate_error_too_large()) {
+            _estimate_valid = false;
+        }
+
 #if HAL_LOGGING_ENABLED
+        // log current follow diagnostic data
         Log_Write_FOLL();
 #endif
     }
 }
 
-bool AP_Follow::handle_global_position_int_message(const mavlink_message_t &msg)
+// Returns true if the incoming MAVLink message should be processed for target updates.
+bool AP_Follow::should_handle_message(const mavlink_message_t &msg) const
 {
-        // decode message
-        mavlink_global_position_int_t packet;
-        mavlink_msg_global_position_int_decode(&msg, &packet);
+    // exit immediately if not enabled
+    if (!_enabled) {
+        return false;
+    }
 
-        // ignore message if lat and lon are (exactly) zero
-        if ((packet.lat == 0 && packet.lon == 0)) {
-            return false;
-        }
+    // skip our own messages
+    if (msg.sysid == mavlink_system.sysid) {
+        return false;
+    }
 
-        Location _target_location;
-        _target_location.lat = packet.lat;
-        _target_location.lng = packet.lon;
+    // skip message if not from our target
+    if (_sysid != 0 && msg.sysid != _sysid) {
+        return false;
+    }
 
-        // select altitude source based on FOLL_ALT_TYPE param 
-        if (_alt_type == AP_FOLLOW_ALTITUDE_TYPE_RELATIVE) {
-            // above home alt
-            _target_location.set_alt_cm(packet.relative_alt / 10, Location::AltFrame::ABOVE_HOME);
-        } else {
-            // absolute altitude
-            _target_location.set_alt_cm(packet.alt / 10, Location::AltFrame::ABSOLUTE);
-        }
-        if (!_target_location.get_vector_from_origin_NEU(_target_position_ned)) {
-            return false;
-        }
-        _target_position_ned.z = -_target_position_ned.z; // NEU->NED
-        _target_position_ned *= 0.01;  // cm -> m
-
-        _target_velocity_ned.x = packet.vx * 0.01f; // velocity north
-        _target_velocity_ned.y = packet.vy * 0.01f; // velocity east
-        _target_velocity_ned.z = packet.vz * 0.01f; // velocity down
-
-        // get a local timestamp with correction for transport jitter
-        _last_location_update_ms = _jitter.correct_offboard_timestamp_msec(packet.time_boot_ms, AP_HAL::millis());
-        if (packet.hdg <= 36000) {                  // heading (UINT16_MAX if unknown)
-            _target_heading = packet.hdg * 0.01f;   // convert centi-degrees to degrees
-            _last_heading_update_ms = _last_location_update_ms;
-        }
-        // initialise _sysid if zero to sender's id
-        if (_sysid == 0) {
-            _sysid.set(msg.sysid);
-            _automatic_sysid = true;
-        }
-        return true;
+    return true;
 }
 
-bool AP_Follow::handle_follow_target_message(const mavlink_message_t &msg)
+// Checks whether the current estimate should be reset based on position and velocity errors.
+bool AP_Follow::estimate_error_too_large() const
 {
-        // decode message
-        mavlink_follow_target_t packet;
-        mavlink_msg_follow_target_decode(&msg, &packet);
+    const float timeout_sec = AP_FOLLOW_TIMEOUT_MS * 0.001f;
+    const float half_timeout_sec = timeout_sec * 0.5f;
 
-        // ignore message if lat and lon are (exactly) zero
-        if ((packet.lat == 0 && packet.lon == 0)) {
-            return false;
-        }
-        // require at least position
-        if ((packet.est_capabilities & (1<<0)) == 0) {
-            return false;
-        }
+    // Calculate position thresholds
+    const float pos_thresh_horiz_m = _accel_max_ne_mss.get() * sq(half_timeout_sec);
+    const float pos_thresh_vert_m  = _accel_max_d_mss.get()  * sq(half_timeout_sec);
 
-        const Location new_loc {
-            packet.lat,
-            packet.lon,
-            int32_t(packet.alt * 100),  // m -> cm
-            Location::AltFrame::ABSOLUTE
-        };
-
-        if (!new_loc.get_vector_from_origin_NEU(_target_position_ned)) {
-            return false;
-        }
-        _target_position_ned.z = -_target_position_ned.z; // NEU->NED
-        _target_position_ned *= 0.01;  // cm -> m
+    // Calculate velocity thresholds using the new helper function
+    const float vel_thresh_horiz_ms = calc_max_velocity_change(_accel_max_ne_mss.get(), _jerk_max_ne_msss.get(), timeout_sec);
+    const float vel_thresh_vert_ms  = calc_max_velocity_change(_accel_max_d_mss.get(), _jerk_max_d_msss.get(), timeout_sec);
 
-        if (packet.est_capabilities & (1<<1)) {
-            _target_velocity_ned.x = packet.vel[0]; // velocity north
-            _target_velocity_ned.y = packet.vel[1]; // velocity east
-            _target_velocity_ned.z = packet.vel[2]; // velocity down
-        } else {
-            _target_velocity_ned.zero();
-        }
+    // Calculate current position and velocity errors
+    Vector3f pos_error = _estimate_pos_ned_m.tofloat() - _target_pos_ned_m.tofloat();
+    Vector3f vel_error = _estimate_vel_ned_ms - _target_vel_ned_ms;
 
-        // get a local timestamp with correction for transport jitter
-        _last_location_update_ms = _jitter.correct_offboard_timestamp_msec(packet.timestamp, AP_HAL::millis());
+    Vector2f pos_error_xy = pos_error.xy();
+    float pos_error_z = pos_error.z;
+    Vector2f vel_error_xy = vel_error.xy();
+    float vel_error_z = vel_error.z;
 
-        if (packet.est_capabilities & (1<<3)) {
-            Quaternion q{packet.attitude_q[0], packet.attitude_q[1], packet.attitude_q[2], packet.attitude_q[3]};
-            float r, p, y;
-            q.to_euler(r,p,y);
-            _target_heading = degrees(y);
-            _last_heading_update_ms = _last_location_update_ms;
-        }
+    // Check horizontal and vertical separately
+    const bool pos_horiz_bad = pos_error_xy.length() > pos_thresh_horiz_m;
+    const bool vel_horiz_bad = vel_error_xy.length() > vel_thresh_horiz_ms;
+    const bool pos_vert_bad = fabsf(pos_error_z) > pos_thresh_vert_m;
+    const bool vel_vert_bad = fabsf(vel_error_z) > vel_thresh_vert_ms;
 
-        // initialise _sysid if zero to sender's id
-        if (_sysid == 0) {
-            _sysid.set(msg.sysid);
-            _automatic_sysid = true;
-        }
+    return pos_horiz_bad || vel_horiz_bad || pos_vert_bad || vel_vert_bad;
+}
 
-        return true;
+// Calculates max velocity change under trapezoidal or triangular acceleration profile (jerk-limited).
+float AP_Follow::calc_max_velocity_change(float accel_max, float jerk_max, float timeout_sec) const
+{
+    const float t_jerk = accel_max / jerk_max;
+    const float t_total_jerk = 2.0f * t_jerk;
+
+    if (timeout_sec >= t_total_jerk) {
+        // time to ramp up, constant accel phase, and ramp down
+        const float t_const = timeout_sec - t_total_jerk;
+        const float delta_v_jerk = 0.5f * accel_max * t_jerk;
+        const float delta_v_const = accel_max * t_const;
+        return 2.0f * delta_v_jerk + delta_v_const;
+    } else {
+        // timeout too short: pure triangle profile
+        const float t_half = timeout_sec * 0.5f;
+        return 0.5f * jerk_max * sq(t_half);
+    }
 }
 
-// write out an onboard-log message to help diagnose follow problems:
-#if HAL_LOGGING_ENABLED
-void AP_Follow::Log_Write_FOLL()
+// Decodes a GLOBAL_POSITION_INT MAVLink message to update the target’s position, velocity, and heading.
+bool AP_Follow::handle_global_position_int_message(const mavlink_message_t &msg)
 {
-        // get estimated location and velocity
-        Location loc_estimate{};
-        Vector3f vel_estimate;
-        UNUSED_RESULT(get_target_location_and_velocity(loc_estimate, vel_estimate));
-
-        Location _target_location;
-        UNUSED_RESULT(AP::ahrs().get_location_from_origin_offset_NED(_target_location, _target_position_ned * 0.01));
-        if (_alt_type == AP_FOLLOW_ALTITUDE_TYPE_RELATIVE) {
-            _target_location.change_alt_frame(Location::AltFrame::ABOVE_HOME);
-        }
+    // decode GLOBAL_POSITION_INT message into packet struct
+    mavlink_global_position_int_t packet;
+    mavlink_msg_global_position_int_decode(&msg, &packet);
+
+    // ignore message if latitude and longitude are exactly zero (invalid GPS fix)
+    if ((packet.lat == 0 && packet.lon == 0)) {
+        return false;
+    }
+
+    Location _target_location;
+    _target_location.lat = packet.lat;
+    _target_location.lng = packet.lon;
+
+    // set target altitude based on configured altitude type
+    if (_alt_type == AP_FOLLOW_ALTITUDE_TYPE_RELATIVE) {
+        // use relative altitude above home
+        _target_location.set_alt_cm(packet.relative_alt / 10, Location::AltFrame::ABOVE_HOME);
+    } else {
+        // use absolute altitude
+        _target_location.set_alt_cm(packet.alt / 10, Location::AltFrame::ABSOLUTE);
+    }
+
+    // convert global location to local NED frame position
+    if (!_target_location.get_vector_from_origin_NEU(_target_pos_ned_m)) {
+        return false;
+    }
+    _target_pos_ned_m.z = -_target_pos_ned_m.z; // convert NEU -> NED
+    _target_pos_ned_m *= 0.01;  // convert from cm to meters
+
+    // decode target velocity components (in m/s)
+    _target_vel_ned_ms.x = packet.vx * 0.01f; // velocity north
+    _target_vel_ned_ms.y = packet.vy * 0.01f; // velocity east
+    _target_vel_ned_ms.z = packet.vz * 0.01f; // velocity down
+
+    // target acceleration not available in GLOBAL_POSITION_INT
+    _target_accel_ned_mss.zero();
+
+    if (packet.hdg <= 36000) {
+        // valid heading field available (in centi-degrees)
+        _target_heading_deg = packet.hdg * 0.01f;
+    } else if (_offset_type == AP_FOLLOW_OFFSET_TYPE_RELATIVE) {
+        // heading missing but relative offset mode requires heading -> reject
+        return false;
+    } else {
+        // no heading available: set heading rate to zero
+        _target_heading_rate_degs = 0.0f;
+    }
+
+    // apply jitter-corrected timestamp to this update
+    _last_location_update_ms = _jitter.correct_offboard_timestamp_msec(packet.time_boot_ms, AP_HAL::millis());
 
-        // log lead's estimated vs reported position
-// @LoggerMessage: FOLL
-// @Description: Follow library diagnostic data
-// @Field: TimeUS: Time since system startup
-// @Field: Lat: Target latitude
-// @Field: Lon: Target longitude
-// @Field: Alt: Target absolute altitude
-// @Field: VelN: Target earth-frame velocity, North
-// @Field: VelE: Target earth-frame velocity, East
-// @Field: VelD: Target earth-frame velocity, Down
-// @Field: LatE: Vehicle latitude
-// @Field: LonE: Vehicle longitude
-// @Field: AltE: Vehicle absolute altitude
-        AP::logger().WriteStreaming("FOLL",
-                                               "TimeUS,Lat,Lon,Alt,VelN,VelE,VelD,LatE,LonE,AltE",  // labels
-                                               "sDUmnnnDUm",    // units
-                                               "F--B000--B",    // mults
-                                               "QLLifffLLi",    // fmt
-                                               AP_HAL::micros64(),
-                                               _target_location.lat,
-                                               _target_location.lng,
-                                               _target_location.alt,
-                                               (double)_target_velocity_ned.x,
-                                               (double)_target_velocity_ned.y,
-                                               (double)_target_velocity_ned.z,
-                                               loc_estimate.lat,
-                                               loc_estimate.lng,
-                                               loc_estimate.alt
-                                               );
+    // if sysid not yet set, adopt sender’s sysid and enable automatic sysid tracking
+    if (_sysid == 0) {
+        _sysid.set(msg.sysid);
+        _sysid_used = 0;
+        _estimate_valid = false;
+        _automatic_sysid = true;
+    }
+
+    return true;
 }
-#endif  // HAL_LOGGING_ENABLED
 
-// get velocity estimate in m/s in NED frame using dt since last update
-bool AP_Follow::get_velocity_ned(Vector3f &vel_ned, float dt) const
+// Decodes a FOLLOW_TARGET MAVLink message to update the target’s position, velocity, acceleration, and orientation.
+bool AP_Follow::handle_follow_target_message(const mavlink_message_t &msg)
 {
-    vel_ned = _target_velocity_ned + (_target_accel_ned * dt);
+    // decode FOLLOW_TARGET message into packet struct
+    mavlink_follow_target_t packet;
+    mavlink_msg_follow_target_decode(&msg, &packet);
+
+    // ignore message if latitude and longitude are exactly zero (invalid GPS fix)
+    if ((packet.lat == 0 && packet.lon == 0)) {
+        return false;
+    }
+
+    // require that at least position is estimated (bit 0 of est_capabilities)
+    if ((packet.est_capabilities & (1<<0)) == 0) {
+        return false;
+    }
+
+    // build Location object from latitude, longitude, and altitude (alt in meters)
+    const Location _target_location {
+        packet.lat,
+        packet.lon,
+        int32_t(packet.alt * 100),  // convert meters to centimeters
+        Location::AltFrame::ABSOLUTE
+    };
+
+    // convert global location to local NED frame position
+    if (!_target_location.get_vector_from_origin_NEU(_target_pos_ned_m)) {
+        return false;
+    }
+    _target_pos_ned_m *= 0.01; // convert from cm to meters
+
+    // adjust Z coordinate to NED frame (NEU altitude -> NED)
+    Location origin;
+    if (!AP::ahrs().get_origin(origin)) {
+        return false;
+    }
+    _target_pos_ned_m.z = -packet.alt + origin.alt * 0.01;
+
+    // decode velocity if available (bit 1 of est_capabilities)
+    if (packet.est_capabilities & (1<<1)) {
+        _target_vel_ned_ms.x = packet.vel[0]; // velocity north
+        _target_vel_ned_ms.y = packet.vel[1]; // velocity east
+        _target_vel_ned_ms.z = packet.vel[2]; // velocity down
+    } else {
+        _target_vel_ned_ms.zero();
path: libraries/AP_Follow/AP_Follow.cpp
user: login: lthall
body: We need to only use one of these messages. This is something we need to add.
created_at: 2025-05-01T12:43:18Z
author_association: CONTRIBUTOR
diff_hunk: @@ -134,419 +155,638 @@ const AP_Param::GroupInfo AP_Follow::var_info[] = {
     AP_GROUPINFO("_ALT_TYPE", 10, AP_Follow, _alt_type, AP_FOLLOW_ALT_TYPE_DEFAULT),
 #endif
 
+// May add option to turn on input shaping, turn on acceleration due to rotation of offsest
+
     // @Param: _OPTIONS
     // @DisplayName: Follow options
     // @Description: Follow options bitmask
     // @Values: 0:None,1: Mount Follows lead vehicle on mode enter
     // @User: Standard
     AP_GROUPINFO("_OPTIONS", 11, AP_Follow, _options, 0),
 
+    // @Param: _ACCEL_NE
+    // @DisplayName: Acceleration limit for the horizontal kinematic input shaping
+    // @Description: Acceleration limit of the horizontal kinematic path generation used to determine how quickly the ship varies in velocity
+    // @Range: 0 5
+    // @Units: m/s/s
+    // @User: Advanced
+    AP_GROUPINFO("_ACCEL_NE", 12, AP_Follow, _accel_max_ne_mss, 2.5),
+
+    // @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 ship 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),
+
+    // @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 ship 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),
+
+    // @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 ship 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),
+
+    // @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 ship 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),
+
+    // @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 ship 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_GROUPEND
 };
 
-/* 
-   The constructor also initialises the proximity sensor. Note that this
-   constructor is not called until detect() returns true, so we
-   already know that we should setup the proximity sensor
-*/
+// Constructor for AP_Follow. Initializes the position P-controller and sets up parameter defaults.
 AP_Follow::AP_Follow() :
-        _p_pos(AP_FOLLOW_POS_P_DEFAULT)
+    _p_pos(AP_FOLLOW_POS_P_DEFAULT)
 {
     _singleton = this;
     AP_Param::setup_object_defaults(this, var_info);
 }
 
-// restore offsets to zero if necessary, should be called when vehicle exits follow mode
-void AP_Follow::clear_offsets_if_required()
+
+//==============================================================================
+// Target Estimation Update Functions
+//==============================================================================
+
+// Projects and updates the estimated target position, velocity, and heading based on last known data and configured input shaping.
+bool AP_Follow::update_estimate()
 {
-    if (_offsets_were_zero) {
-        _offset.set(Vector3f());
-        _offsets_were_zero = false;
+    // exit immediately if not enabled
+    if (!_enabled) {
+        _estimate_valid = false;
+        return _estimate_valid;
     }
-}
 
-// get target's estimated location
-bool AP_Follow::get_target_location_and_velocity(Location &loc, Vector3f &vel_ned) const
-{
-    Vector3p pos_ned;
-    Vector3f local_vel_ned;  // so we either change both return parameters or neither
-    if (!get_target_position_and_velocity(pos_ned, local_vel_ned)) {
-        return false;
+    // check for timeout: if no recent location update, invalidate estimate
+    if ((_last_location_update_ms == 0) || (AP_HAL::millis() - _last_location_update_ms > AP_FOLLOW_TIMEOUT_MS)) {
+        _estimate_valid = false;
+        return _estimate_valid;
     }
-    if (!AP::ahrs().get_location_from_origin_offset_NED(loc, pos_ned)) {
-        return false;
+
+    // skip if update has already been processed this scheduler tick
+    if (_estimate_valid && (AP::scheduler().ticks32() - _last_update_ticks == 0)) {
+        return _estimate_valid;
     }
-    vel_ned = local_vel_ned;
+    _last_update_ticks = AP::scheduler().ticks32();
 
-    if (_alt_type == AP_FOLLOW_ALTITUDE_TYPE_RELATIVE) {
-        loc.change_alt_frame(Location::AltFrame::ABOVE_HOME);
+    // if sysid changed, reset the estimation state
+    if (_sysid != _sysid_used) {
+        _sysid_used = _sysid;
+        _estimate_valid = false;
     }
 
-    return true;
-}
+    const uint32_t now = AP_HAL::millis();
 
-// get target's estimated location and velocity, both NED SI from origin
-bool AP_Follow::get_target_position_and_velocity(Vector3p &pos_ned, Vector3f &vel_ned) const
-{
-    // exit immediately if not enabled
-    if (!_enabled) {
-        return false;
+    // calculate time since last location update in seconds
+    const float dt = (now - _last_location_update_ms) * 0.001f;
+
+    // project target's position and velocity forward using simple kinematics
+    Vector3f delta_pos = _target_vel_ned_ms * dt + _target_accel_ned_mss * 0.5f * sq(dt);
+    Vector3f delta_vel = _target_accel_ned_mss * dt;
+
+    // calculate time since last estimation update
+    const float e_dt = (now - _last_estimation_update_ms) * 0.001f;
+    
+    if (_estimate_valid) {
+        // 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)
+        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.xy().topostype(), _target_vel_ned_ms.xy() + delta_vel.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 angular shaping for heading estimate
+        shape_angle_vel_accel(radians(_target_heading_deg), radians(_target_heading_rate_degs), 0.0,
+                              _estimate_heading_rad, _estimate_heading_rate_rads, _estimate_heading_accel_radss,
+                              0.0, radians(_accel_max_h_degss),
+                              radians(_jerk_max_h_degsss), e_dt, false);
+
+        // update heading angle separately to maintain proper wrapping [-PI, PI]
+        postype_t estimate_heading_rad = _estimate_heading_rad;
+        update_pos_vel_accel(estimate_heading_rad, _estimate_heading_rate_rads, _estimate_heading_accel_radss, e_dt, 0.0, 0.0, 0.0);
+        _estimate_heading_rad = wrap_PI(estimate_heading_rad);
+    } else {
+        // no valid estimate yet: initialise from latest target position
+        _estimate_pos_ned_m = _target_pos_ned_m + delta_pos.topostype();
+        _estimate_vel_ned_ms = _target_vel_ned_ms + delta_vel;
+        _estimate_accel_ned_mss = _target_accel_ned_mss;
+        _estimate_valid = true;
     }
 
-    // check for timeout
-    if ((_last_location_update_ms == 0) || (AP_HAL::millis() - _last_location_update_ms > AP_FOLLOW_TIMEOUT_MS)) {
-        return false;
+    Vector3f offset_m = _offset_XXD_m.get();
+
+    // calculate estimated position and velocity with offsets applied
+    if (offset_m.is_zero() || (_offset_type == AP_FOLLOW_OFFSET_TYPE_NED)) {
+        // offsets are in NED frame: simple addition
+        _ofs_estimate_pos_ned_m = _estimate_pos_ned_m + offset_m.topostype();
+        _ofs_estimate_vel_ned_ms = _estimate_vel_ned_ms;
+        _ofs_estimate_accel_ned_mss = _estimate_accel_ned_mss;
+    } else {
+        // offsets are in FRD frame: rotate by heading
+        offset_m.xy().rotate(_estimate_heading_rad);
+        Vector3f offset_cross = offset_m.cross(Vector3f{0.0, 0.0, 1.0});
+        float offset_length_m = offset_m.length();
+        _ofs_estimate_pos_ned_m = _estimate_pos_ned_m + offset_m.topostype();
+        _ofs_estimate_vel_ned_ms = _estimate_vel_ned_ms + offset_cross * offset_length_m * _estimate_heading_rate_rads;
+        _ofs_estimate_accel_ned_mss = _estimate_accel_ned_mss + offset_cross * offset_length_m * _estimate_heading_accel_radss;
     }
 
-    // calculate time since last actual position update
-    const float dt = (AP_HAL::millis() - _last_location_update_ms) * 0.001f;
+    _last_estimation_update_ms = now;
+    return _estimate_valid;
+}
+
 
-    // get velocity estimate
-    if (!get_velocity_ned(vel_ned, dt)) {
+//==============================================================================
+// Target Information Retrieval Functions
+//==============================================================================
+
+// Retrieves the estimated target position, velocity, and acceleration in the NED frame (relative to origin).
+bool AP_Follow::get_target_pos_vel_accel_NED_m(Vector3p &pos_ned_m, Vector3f &vel_ned_ms, Vector3f &accel_ned_mss)
+{
+    if (!update_estimate()) {
         return false;
     }
 
-    // project the vehicle position
-    const Vector3p vel_ned_p { vel_ned.x, vel_ned.y, vel_ned.z };
-    pos_ned = _target_position_ned + vel_ned_p * dt;
+    pos_ned_m = _estimate_pos_ned_m;
+    vel_ned_ms = _estimate_vel_ned_ms;
+    accel_ned_mss = _estimate_accel_ned_mss;
 
     return true;
 }
 
-// get distance vector to target (in meters) and target's velocity all in NED frame
-bool AP_Follow::get_target_dist_and_vel_ned(Vector3f &dist_ned, Vector3f &dist_with_offs, Vector3f &vel_ned)
+// Retrieves the estimated target position, velocity, and acceleration in the NED frame, including configured offsets.
+bool AP_Follow::get_ofs_pos_vel_accel_NED_m(Vector3p &pos_ofs_ned_m, Vector3f &vel_ofs_ned_ms, Vector3f &accel_ofs_ned_mss)
 {
-    Vector3f current_position_NED;
-    if (!AP::ahrs().get_relative_position_NED_origin(current_position_NED)) {
-        clear_dist_and_bearing_to_target();
-        return false;
-    }
-    Vector3p target_position_NED;
-    Vector3f veh_vel;  // NED
-    if (!get_target_position_and_velocity(target_position_NED, veh_vel)) {
-        clear_dist_and_bearing_to_target();
+    if (!update_estimate()) {
         return false;
     }
 
-    // convert from Vector3p to Vector3f as they can't be subtracted:
-    Vector3f target_position_NED_f;
-    target_position_NED_f.x = target_position_NED.x;
-    target_position_NED_f.y = target_position_NED.y;
-    target_position_NED_f.z = target_position_NED.z;
+    pos_ofs_ned_m = _ofs_estimate_pos_ned_m;
+    vel_ofs_ned_ms = _ofs_estimate_vel_ned_ms;
+    accel_ofs_ned_mss = _ofs_estimate_accel_ned_mss;
 
-    const Vector3f dist_vec = target_position_NED_f - current_position_NED;
+    return true;
+}
 
-    // fail if too far
-    if (is_positive(_dist_max.get()) && (dist_vec.length() > _dist_max)) {
-        clear_dist_and_bearing_to_target();
+// Retrieves distance vectors (with and without configured offsets) and the target’s velocity, all in the NED frame.
+bool AP_Follow::get_target_dist_and_vel_NED_m(Vector3f &dist_ned, Vector3f &dist_with_offs, Vector3f &vel_ned)
+{
+    if (!update_estimate()) {
         return false;
     }
 
-    // initialise offsets from distance vector if required
-    init_offsets_if_required(dist_vec);
-
-    // get offsets
-    Vector3f offsets;
-    if (!get_offsets_ned(offsets)) {
+    Vector3f current_position_ned_m;
+    if (!AP::ahrs().get_relative_position_NED_origin(current_position_ned_m)) {
         clear_dist_and_bearing_to_target();
         return false;
     }
 
-    // calculate results
-    dist_ned = dist_vec;
-    dist_with_offs = dist_vec + offsets;
-    vel_ned = veh_vel;
+    const Vector3p dist_vec_ned_m = _estimate_pos_ned_m - current_position_ned_m.topostype();
+    const Vector3p ofs_dist_vec = _ofs_estimate_pos_ned_m - current_position_ned_m.topostype();
+    dist_ned = dist_vec_ned_m.tofloat();
+    dist_with_offs = ofs_dist_vec.tofloat();
+    vel_ned = _ofs_estimate_vel_ned_ms;
 
-    // record distance and heading for reporting purposes
-    if (is_zero(dist_with_offs.x) && is_zero(dist_with_offs.y)) {
-        clear_dist_and_bearing_to_target();
-    } else {
-        _dist_to_target = safe_sqrt(sq(dist_with_offs.x) + sq(dist_with_offs.y));
-        _bearing_to_target = degrees(atan2f(dist_with_offs.y, dist_with_offs.x));
+    return true;
+}
+
+// Retrieves the estimated target heading and heading rate in radians.
+bool AP_Follow::get_heading_heading_rate_rad(float &heading_rad, float &heading_rate_rads)
+{
+    if (!update_estimate()) {
+        return false;
     }
 
+    // return latest heading estimate
+    heading_rad = _estimate_heading_rad;
+    heading_rate_rads = _estimate_heading_rate_rads;
     return true;
 }
 
-// get target's heading in degrees (0 = north, 90 = east)
-bool AP_Follow::get_target_heading_deg(float &heading) const
+// Retrieves the target's estimated global location and velocity, adjusting altitude frame if relative mode is set (for LUA bindings).
+bool AP_Follow::get_target_location_and_velocity(Location &loc, Vector3f &vel_ned)
 {
-    // exit immediately if not enabled
-    if (!_enabled) {
+    if (!update_estimate()) {
         return false;
     }
 
-    // check for timeout
-    if ((_last_heading_update_ms == 0) || (AP_HAL::millis() - _last_heading_update_ms > AP_FOLLOW_TIMEOUT_MS)) {
+    if (!AP::ahrs().get_location_from_origin_offset_NED(loc, _estimate_pos_ned_m)) {
         return false;
     }
+    vel_ned = _estimate_vel_ned_ms;
+
+    if (_alt_type == AP_FOLLOW_ALTITUDE_TYPE_RELATIVE) {
+        loc.change_alt_frame(Location::AltFrame::ABOVE_HOME);
+    }
 
-    // return latest heading estimate
-    heading = _target_heading;
     return true;
 }
 
-// returns true if we should extract information from msg
-bool AP_Follow::should_handle_message(const mavlink_message_t &msg) const
+// Retrieves the target's estimated global location and velocity, including configured offsets, for LUA bindings.
+bool AP_Follow::get_target_location_and_velocity_ofs(Location &loc, Vector3f &vel_ned)
 {
-    // exit immediately if not enabled
-    if (!_enabled) {
+    if (!update_estimate()) {
         return false;
     }
-
-    // skip our own messages
-    if (msg.sysid == mavlink_system.sysid) {
+    if (!AP::ahrs().get_location_from_origin_offset_NED(loc, _ofs_estimate_pos_ned_m)) {
         return false;
     }
+    if (_alt_type == AP_FOLLOW_ALTITUDE_TYPE_RELATIVE) {
+        loc.change_alt_frame(Location::AltFrame::ABOVE_HOME);
+    }
 
-    // skip message if not from our target
-    if (_sysid != 0 && msg.sysid != _sysid) {
+    vel_ned = _ofs_estimate_vel_ned_ms;
+    return true;
+}
+
+// Retrieves the estimated target heading in degrees (0° = North, 90° = East) for LUA bindings.
+bool AP_Follow::get_target_heading_deg(float &heading)
+{
+    if (!update_estimate()) {
         return false;
     }
 
+    // return latest heading estimate
+    heading = degrees(_estimate_heading_rad);
     return true;
 }
 
-// handle mavlink DISTANCE_SENSOR messages
+
+//==============================================================================
+// MAVLink Message Handling
+//==============================================================================
+
+// Handles incoming MAVLink messages to update the target's position, velocity, and heading.
 void AP_Follow::handle_msg(const mavlink_message_t &msg)
 {
-    // this method should be called from an "update()" method:
     if (_automatic_sysid) {
-        // maybe timeout who we were following...
+        // timeout check: if no position update received within timeout, clear automatic sysid
         if ((_last_location_update_ms == 0) ||
             (AP_HAL::millis() - _last_location_update_ms > AP_FOLLOW_SYSID_TIMEOUT_MS)) {
             _sysid.set(0);
+            _sysid_used = 0;
+            _estimate_valid = false;
         }
     }
 
     if (!should_handle_message(msg)) {
+        // ignore message if filtering rules reject it (e.g., wrong sysid)
         return;
     }
 
-    // decode global-position-int message
+    // decode MAVLink message
     bool updated = false;
 
     switch (msg.msgid) {
     case MAVLINK_MSG_ID_GLOBAL_POSITION_INT: {
+        // handle standard global position messages
         updated = handle_global_position_int_message(msg);
         break;
     }
     case MAVLINK_MSG_ID_FOLLOW_TARGET: {
+        // handle follow-target specific messages
         updated = handle_follow_target_message(msg);
         break;
     }
     }
 
     if (updated) {
+        // message provided updated position information
+        Vector3f offset_m = _offset_XXD_m.get();
+
+        // if using relative frame, rotate offsets based on estimated heading
+        if (_offset_type == AP_FOLLOW_OFFSET_TYPE_RELATIVE) {
+            offset_m.xy().rotate(_estimate_heading_rad);
+        }
+
+        Vector3f current_position_ned_m;
+        if (!AP::ahrs().get_relative_position_NED_origin(current_position_ned_m)) {
+            // 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
+            const Vector3p dist_vec_ned_m = _estimate_pos_ned_m - current_position_ned_m.topostype();
+            const Vector3p ofs_dist_vec = _ofs_estimate_pos_ned_m - current_position_ned_m.topostype();
+
+            // initialise offsets dynamically if needed
+            init_offsets_if_required(dist_vec_ned_m.tofloat());
+
+            // record distance and bearing to target for reporting/logging
+            if (ofs_dist_vec.xy().is_zero()) {
+                clear_dist_and_bearing_to_target();
+            } else {
+                _dist_to_target_m = ofs_dist_vec.xy().length();
+                _bearing_to_target_deg = degrees(ofs_dist_vec.xy().angle());
+            }
+
+            // clear distance and bearing if outside configured max follow distance
+            if (is_positive(_dist_max_m.get()) && (dist_vec_ned_m.length() > _dist_max_m)) {
+                clear_dist_and_bearing_to_target();
+            }
+        }
+
+        // Check if estimate needs reset based on position and velocity errors
+        if (estimate_error_too_large()) {
+            _estimate_valid = false;
+        }
+
 #if HAL_LOGGING_ENABLED
+        // log current follow diagnostic data
         Log_Write_FOLL();
 #endif
     }
 }
 
-bool AP_Follow::handle_global_position_int_message(const mavlink_message_t &msg)
+// Returns true if the incoming MAVLink message should be processed for target updates.
+bool AP_Follow::should_handle_message(const mavlink_message_t &msg) const
 {
-        // decode message
-        mavlink_global_position_int_t packet;
-        mavlink_msg_global_position_int_decode(&msg, &packet);
+    // exit immediately if not enabled
+    if (!_enabled) {
+        return false;
+    }
 
-        // ignore message if lat and lon are (exactly) zero
-        if ((packet.lat == 0 && packet.lon == 0)) {
-            return false;
-        }
+    // skip our own messages
+    if (msg.sysid == mavlink_system.sysid) {
+        return false;
+    }
 
-        Location _target_location;
-        _target_location.lat = packet.lat;
-        _target_location.lng = packet.lon;
+    // skip message if not from our target
+    if (_sysid != 0 && msg.sysid != _sysid) {
+        return false;
+    }
 
-        // select altitude source based on FOLL_ALT_TYPE param 
-        if (_alt_type == AP_FOLLOW_ALTITUDE_TYPE_RELATIVE) {
-            // above home alt
-            _target_location.set_alt_cm(packet.relative_alt / 10, Location::AltFrame::ABOVE_HOME);
-        } else {
-            // absolute altitude
-            _target_location.set_alt_cm(packet.alt / 10, Location::AltFrame::ABSOLUTE);
-        }
-        if (!_target_location.get_vector_from_origin_NEU(_target_position_ned)) {
-            return false;
-        }
-        _target_position_ned.z = -_target_position_ned.z; // NEU->NED
-        _target_position_ned *= 0.01;  // cm -> m
-
-        _target_velocity_ned.x = packet.vx * 0.01f; // velocity north
-        _target_velocity_ned.y = packet.vy * 0.01f; // velocity east
-        _target_velocity_ned.z = packet.vz * 0.01f; // velocity down
-
-        // get a local timestamp with correction for transport jitter
-        _last_location_update_ms = _jitter.correct_offboard_timestamp_msec(packet.time_boot_ms, AP_HAL::millis());
-        if (packet.hdg <= 36000) {                  // heading (UINT16_MAX if unknown)
-            _target_heading = packet.hdg * 0.01f;   // convert centi-degrees to degrees
-            _last_heading_update_ms = _last_location_update_ms;
-        }
-        // initialise _sysid if zero to sender's id
-        if (_sysid == 0) {
-            _sysid.set(msg.sysid);
-            _automatic_sysid = true;
-        }
-        return true;
+    return true;
 }
 
-bool AP_Follow::handle_follow_target_message(const mavlink_message_t &msg)
+// Checks whether the current estimate should be reset based on position and velocity errors.
+bool AP_Follow::estimate_error_too_large() const
 {
-        // decode message
-        mavlink_follow_target_t packet;
-        mavlink_msg_follow_target_decode(&msg, &packet);
+    const float timeout_sec = AP_FOLLOW_TIMEOUT_MS * 0.001f;
+    const float half_timeout_sec = timeout_sec * 0.5f;
 
-        // ignore message if lat and lon are (exactly) zero
-        if ((packet.lat == 0 && packet.lon == 0)) {
-            return false;
-        }
-        // require at least position
-        if ((packet.est_capabilities & (1<<0)) == 0) {
-            return false;
-        }
+    // Calculate position thresholds
+    const float pos_thresh_horiz_m = _accel_max_ne_mss.get() * sq(half_timeout_sec);
+    const float pos_thresh_vert_m  = _accel_max_d_mss.get()  * sq(half_timeout_sec);
 
-        const Location new_loc {
-            packet.lat,
-            packet.lon,
-            int32_t(packet.alt * 100),  // m -> cm
-            Location::AltFrame::ABSOLUTE
-        };
-
-        if (!new_loc.get_vector_from_origin_NEU(_target_position_ned)) {
-            return false;
-        }
-        _target_position_ned.z = -_target_position_ned.z; // NEU->NED
-        _target_position_ned *= 0.01;  // cm -> m
+    // Calculate velocity thresholds using the new helper function
+    const float vel_thresh_horiz_ms = calc_max_velocity_change(_accel_max_ne_mss.get(), _jerk_max_ne_msss.get(), timeout_sec);
+    const float vel_thresh_vert_ms  = calc_max_velocity_change(_accel_max_d_mss.get(), _jerk_max_d_msss.get(), timeout_sec);
 
-        if (packet.est_capabilities & (1<<1)) {
-            _target_velocity_ned.x = packet.vel[0]; // velocity north
-            _target_velocity_ned.y = packet.vel[1]; // velocity east
-            _target_velocity_ned.z = packet.vel[2]; // velocity down
-        } else {
-            _target_velocity_ned.zero();
-        }
+    // Calculate current position and velocity errors
+    Vector3f pos_error = _estimate_pos_ned_m.tofloat() - _target_pos_ned_m.tofloat();
+    Vector3f vel_error = _estimate_vel_ned_ms - _target_vel_ned_ms;
 
-        // get a local timestamp with correction for transport jitter
-        _last_location_update_ms = _jitter.correct_offboard_timestamp_msec(packet.timestamp, AP_HAL::millis());
+    Vector2f pos_error_xy = pos_error.xy();
+    float pos_error_z = pos_error.z;
+    Vector2f vel_error_xy = vel_error.xy();
+    float vel_error_z = vel_error.z;
 
-        if (packet.est_capabilities & (1<<3)) {
-            Quaternion q{packet.attitude_q[0], packet.attitude_q[1], packet.attitude_q[2], packet.attitude_q[3]};
-            float r, p, y;
-            q.to_euler(r,p,y);
-            _target_heading = degrees(y);
-            _last_heading_update_ms = _last_location_update_ms;
-        }
+    // Check horizontal and vertical separately
+    const bool pos_horiz_bad = pos_error_xy.length() > pos_thresh_horiz_m;
+    const bool vel_horiz_bad = vel_error_xy.length() > vel_thresh_horiz_ms;
+    const bool pos_vert_bad = fabsf(pos_error_z) > pos_thresh_vert_m;
+    const bool vel_vert_bad = fabsf(vel_error_z) > vel_thresh_vert_ms;
 
-        // initialise _sysid if zero to sender's id
-        if (_sysid == 0) {
-            _sysid.set(msg.sysid);
-            _automatic_sysid = true;
-        }
+    return pos_horiz_bad || vel_horiz_bad || pos_vert_bad || vel_vert_bad;
+}
 
-        return true;
+// Calculates max velocity change under trapezoidal or triangular acceleration profile (jerk-limited).
+float AP_Follow::calc_max_velocity_change(float accel_max, float jerk_max, float timeout_sec) const
+{
+    const float t_jerk = accel_max / jerk_max;
+    const float t_total_jerk = 2.0f * t_jerk;
+
+    if (timeout_sec >= t_total_jerk) {
+        // time to ramp up, constant accel phase, and ramp down
+        const float t_const = timeout_sec - t_total_jerk;
+        const float delta_v_jerk = 0.5f * accel_max * t_jerk;
+        const float delta_v_const = accel_max * t_const;
+        return 2.0f * delta_v_jerk + delta_v_const;
+    } else {
+        // timeout too short: pure triangle profile
+        const float t_half = timeout_sec * 0.5f;
+        return 0.5f * jerk_max * sq(t_half);
+    }
 }
 
-// write out an onboard-log message to help diagnose follow problems:
-#if HAL_LOGGING_ENABLED
-void AP_Follow::Log_Write_FOLL()
+// Decodes a GLOBAL_POSITION_INT MAVLink message to update the target’s position, velocity, and heading.
+bool AP_Follow::handle_global_position_int_message(const mavlink_message_t &msg)
 {
-        // get estimated location and velocity
-        Location loc_estimate{};
-        Vector3f vel_estimate;
-        UNUSED_RESULT(get_target_location_and_velocity(loc_estimate, vel_estimate));
-
-        Location _target_location;
-        UNUSED_RESULT(AP::ahrs().get_location_from_origin_offset_NED(_target_location, _target_position_ned * 0.01));
-        if (_alt_type == AP_FOLLOW_ALTITUDE_TYPE_RELATIVE) {
-            _target_location.change_alt_frame(Location::AltFrame::ABOVE_HOME);
-        }
+    // decode GLOBAL_POSITION_INT message into packet struct
+    mavlink_global_position_int_t packet;
+    mavlink_msg_global_position_int_decode(&msg, &packet);
+
+    // ignore message if latitude and longitude are exactly zero (invalid GPS fix)
+    if ((packet.lat == 0 && packet.lon == 0)) {
+        return false;
+    }
+
+    Location _target_location;
+    _target_location.lat = packet.lat;
+    _target_location.lng = packet.lon;
+
+    // set target altitude based on configured altitude type
+    if (_alt_type == AP_FOLLOW_ALTITUDE_TYPE_RELATIVE) {
+        // use relative altitude above home
+        _target_location.set_alt_cm(packet.relative_alt / 10, Location::AltFrame::ABOVE_HOME);
+    } else {
+        // use absolute altitude
+        _target_location.set_alt_cm(packet.alt / 10, Location::AltFrame::ABSOLUTE);
+    }
+
+    // convert global location to local NED frame position
+    if (!_target_location.get_vector_from_origin_NEU(_target_pos_ned_m)) {
+        return false;
+    }
+    _target_pos_ned_m.z = -_target_pos_ned_m.z; // convert NEU -> NED
+    _target_pos_ned_m *= 0.01;  // convert from cm to meters
+
+    // decode target velocity components (in m/s)
+    _target_vel_ned_ms.x = packet.vx * 0.01f; // velocity north
+    _target_vel_ned_ms.y = packet.vy * 0.01f; // velocity east
+    _target_vel_ned_ms.z = packet.vz * 0.01f; // velocity down
+
+    // target acceleration not available in GLOBAL_POSITION_INT
+    _target_accel_ned_mss.zero();
+
+    if (packet.hdg <= 36000) {
+        // valid heading field available (in centi-degrees)
+        _target_heading_deg = packet.hdg * 0.01f;
+    } else if (_offset_type == AP_FOLLOW_OFFSET_TYPE_RELATIVE) {
+        // heading missing but relative offset mode requires heading -> reject
+        return false;
+    } else {
+        // no heading available: set heading rate to zero
+        _target_heading_rate_degs = 0.0f;
+    }
+
+    // apply jitter-corrected timestamp to this update
+    _last_location_update_ms = _jitter.correct_offboard_timestamp_msec(packet.time_boot_ms, AP_HAL::millis());
 
-        // log lead's estimated vs reported position
-// @LoggerMessage: FOLL
-// @Description: Follow library diagnostic data
-// @Field: TimeUS: Time since system startup
-// @Field: Lat: Target latitude
-// @Field: Lon: Target longitude
-// @Field: Alt: Target absolute altitude
-// @Field: VelN: Target earth-frame velocity, North
-// @Field: VelE: Target earth-frame velocity, East
-// @Field: VelD: Target earth-frame velocity, Down
-// @Field: LatE: Vehicle latitude
-// @Field: LonE: Vehicle longitude
-// @Field: AltE: Vehicle absolute altitude
-        AP::logger().WriteStreaming("FOLL",
-                                               "TimeUS,Lat,Lon,Alt,VelN,VelE,VelD,LatE,LonE,AltE",  // labels
-                                               "sDUmnnnDUm",    // units
-                                               "F--B000--B",    // mults
-                                               "QLLifffLLi",    // fmt
-                                               AP_HAL::micros64(),
-                                               _target_location.lat,
-                                               _target_location.lng,
-                                               _target_location.alt,
-                                               (double)_target_velocity_ned.x,
-                                               (double)_target_velocity_ned.y,
-                                               (double)_target_velocity_ned.z,
-                                               loc_estimate.lat,
-                                               loc_estimate.lng,
-                                               loc_estimate.alt
-                                               );
+    // if sysid not yet set, adopt sender’s sysid and enable automatic sysid tracking
+    if (_sysid == 0) {
+        _sysid.set(msg.sysid);
+        _sysid_used = 0;
+        _estimate_valid = false;
+        _automatic_sysid = true;
+    }
+
+    return true;
 }
-#endif  // HAL_LOGGING_ENABLED
 
-// get velocity estimate in m/s in NED frame using dt since last update
-bool AP_Follow::get_velocity_ned(Vector3f &vel_ned, float dt) const
+// Decodes a FOLLOW_TARGET MAVLink message to update the target’s position, velocity, acceleration, and orientation.
+bool AP_Follow::handle_follow_target_message(const mavlink_message_t &msg)
 {
-    vel_ned = _target_velocity_ned + (_target_accel_ned * dt);
+    // decode FOLLOW_TARGET message into packet struct
+    mavlink_follow_target_t packet;
+    mavlink_msg_follow_target_decode(&msg, &packet);
+
+    // ignore message if latitude and longitude are exactly zero (invalid GPS fix)
+    if ((packet.lat == 0 && packet.lon == 0)) {
+        return false;
+    }
+
+    // require that at least position is estimated (bit 0 of est_capabilities)
+    if ((packet.est_capabilities & (1<<0)) == 0) {
+        return false;
+    }
+
+    // build Location object from latitude, longitude, and altitude (alt in meters)
+    const Location _target_location {
+        packet.lat,
+        packet.lon,
+        int32_t(packet.alt * 100),  // convert meters to centimeters
+        Location::AltFrame::ABSOLUTE
+    };
+
+    // convert global location to local NED frame position
+    if (!_target_location.get_vector_from_origin_NEU(_target_pos_ned_m)) {
+        return false;
+    }
+    _target_pos_ned_m *= 0.01; // convert from cm to meters
+
+    // adjust Z coordinate to NED frame (NEU altitude -> NED)
+    Location origin;
+    if (!AP::ahrs().get_origin(origin)) {
+        return false;
+    }
+    _target_pos_ned_m.z = -packet.alt + origin.alt * 0.01;
+
+    // decode velocity if available (bit 1 of est_capabilities)
+    if (packet.est_capabilities & (1<<1)) {
+        _target_vel_ned_ms.x = packet.vel[0]; // velocity north
+        _target_vel_ned_ms.y = packet.vel[1]; // velocity east
+        _target_vel_ned_ms.z = packet.vel[2]; // velocity down
+    } else {
+        _target_vel_ned_ms.zero();
+    }
+
+    // decode acceleration if available (bit 2 of est_capabilities)
+    if (packet.est_capabilities & (1 << 2)) {
+        _target_accel_ned_mss.x = packet.acc[0]; // acceleration north
+        _target_accel_ned_mss.y = packet.acc[1]; // acceleration east
+        _target_accel_ned_mss.z = packet.acc[2]; // acceleration down
+    } else {
+        _target_accel_ned_mss.zero();
path: libraries/AP_Follow/AP_Follow.cpp
user: login: lthall
body: We need to only use one of these messages. This is something we need to add.
created_at: 2025-05-01T12:43:31Z
author_association: CONTRIBUTOR
diff_hunk: @@ -556,51 +796,93 @@ Vector3f AP_Follow::rotate_vector(const Vector3f &vec, float angle_deg) const
     return ret;
 }
 
-// set recorded distance and bearing to target to zero
-void AP_Follow::clear_dist_and_bearing_to_target()
+// Resets follow mode offsets to zero if they were automatically initialized.
+void AP_Follow::clear_offsets_if_required()
 {
-    _dist_to_target = 0.0f;
-    _bearing_to_target = 0.0f;
+    if (_offsets_were_zero) {
+        _offset_XXD_m.set(Vector3f());
+        _offsets_were_zero = false;
+    }
 }
 
-// get target's estimated origin-relative-position and velocity (both
-// in SI NED), with offsets added
-bool AP_Follow::get_target_position_and_velocity_ofs(Vector3p &pos_ned, Vector3f &vel_ned) const
+
+//==============================================================================
+// Distance and Bearing Management
+//==============================================================================
+
+// Resets the recorded distance and bearing to the target to zero.
+void AP_Follow::clear_dist_and_bearing_to_target()
 {
-    Vector3f ofs_ned;
-    if (!get_offsets_ned(ofs_ned)) {
-        return false;
-    }
-    if (!get_target_position_and_velocity(pos_ned, vel_ned)) {
-        return false;
-    }
-    // can't add Vector3p and Vector3f directly:
-    pos_ned.x += ofs_ned.x;
-    pos_ned.y += ofs_ned.y;
-    pos_ned.z += ofs_ned.z;
-    return true;
+    _dist_to_target_m = 0.0f;
+    _bearing_to_target_deg = 0.0f;
 }
 
-// get target's estimated location and velocity (in NED), with offsets added
-bool AP_Follow::get_target_location_and_velocity_ofs(Location &loc, Vector3f &vel_ned) const
+
+//==============================================================================
+// Logging
+//==============================================================================
+
+// Writes a diagnostic onboard log message containing target and vehicle tracking data for Follow mode.
+#if HAL_LOGGING_ENABLED
+void AP_Follow::Log_Write_FOLL()
 {
-    Vector3p pos_ned;
-    Vector3f local_vel_ned;  // so we either change both return parameters or neither
-    if (!get_target_position_and_velocity_ofs(pos_ned, local_vel_ned)) {
-        return false;
-    }
-    if (!AP::ahrs().get_location_from_origin_offset_NED(loc, pos_ned)) {
-        return false;
-    }
+    // retrieve latest estimated location and velocity
+    Location loc_estimate{};
+    Vector3f vel_estimate;
+    UNUSED_RESULT(get_target_location_and_velocity(loc_estimate, vel_estimate));
+
+    Location _target_location;
+    UNUSED_RESULT(AP::ahrs().get_location_from_origin_offset_NED(_target_location, _target_pos_ned_m * 0.01));
+
     if (_alt_type == AP_FOLLOW_ALTITUDE_TYPE_RELATIVE) {
-        loc.change_alt_frame(Location::AltFrame::ABOVE_HOME);
-    }
+        _target_location.change_alt_frame(Location::AltFrame::ABOVE_HOME);
+    }
+
+    // log the lead target's reported position and vehicle's estimated position
+    // @LoggerMessage: FOLL
+    // @Description: Follow library diagnostic data
+    // @Field: TimeUS: Time since system startup (microseconds)
+    // @Field: Lat: Target latitude (degrees * 1E7)
+    // @Field: Lon: Target longitude (degrees * 1E7)
+    // @Field: Alt: Target absolute altitude (centimeters)
+    // @Field: VelN: Target velocity, North (m/s)
+    // @Field: VelE: Target velocity, East (m/s)
+    // @Field: VelD: Target velocity, Down (m/s)
+    // @Field: LatE: Vehicle estimated latitude (degrees * 1E7)
+    // @Field: LonE: Vehicle estimated longitude (degrees * 1E7)
+    // @Field: AltE: Vehicle estimated absolute altitude (centimeters)
+    AP::logger().WriteStreaming("FOLL",
+                                "TimeUS,Lat,Lon,Alt,VelN,VelE,VelD,LatE,LonE,AltE",  // labels
+                                "sDUmnnnDUm",    // units
+                                "F--B000--B",    // mults
+                                "QLLifffLLi",    // fmt
+                                AP_HAL::micros64(),
+                                _target_location.lat,
+                                _target_location.lng,
+                                _target_location.alt,
+                                (double)_target_vel_ned_ms.x,
+                                (double)_target_vel_ned_ms.y,
+                                (double)_target_vel_ned_ms.z,
+                                loc_estimate.lat,
+                                loc_estimate.lng,
+                                loc_estimate.alt
+                                );
+}
+#endif  // HAL_LOGGING_ENABLED
+
+
+//==============================================================================
+// Accessors and Helpers
+//==============================================================================
 
-    vel_ned = local_vel_ned;
+// Computes the estimated velocity in the NED frame by applying acceleration over the given time delta.
path: libraries/AP_Follow/AP_Follow.cpp
user: login: lthall
body: Not used
created_at: 2025-05-01T12:44:41Z
author_association: CONTRIBUTOR
diff_hunk: @@ -134,419 +155,638 @@ const AP_Param::GroupInfo AP_Follow::var_info[] = {
     AP_GROUPINFO("_ALT_TYPE", 10, AP_Follow, _alt_type, AP_FOLLOW_ALT_TYPE_DEFAULT),
 #endif
 
+// May add option to turn on input shaping, turn on acceleration due to rotation of offsest
+
     // @Param: _OPTIONS
     // @DisplayName: Follow options
     // @Description: Follow options bitmask
     // @Values: 0:None,1: Mount Follows lead vehicle on mode enter
     // @User: Standard
     AP_GROUPINFO("_OPTIONS", 11, AP_Follow, _options, 0),
 
+    // @Param: _ACCEL_NE
+    // @DisplayName: Acceleration limit for the horizontal kinematic input shaping
+    // @Description: Acceleration limit of the horizontal kinematic path generation used to determine how quickly the ship varies in velocity
+    // @Range: 0 5
+    // @Units: m/s/s
+    // @User: Advanced
+    AP_GROUPINFO("_ACCEL_NE", 12, AP_Follow, _accel_max_ne_mss, 2.5),
+
+    // @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 ship 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),
+
+    // @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 ship 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),
+
+    // @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 ship 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),
+
+    // @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 ship 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),
+
+    // @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 ship 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_GROUPEND
 };
 
-/* 
-   The constructor also initialises the proximity sensor. Note that this
-   constructor is not called until detect() returns true, so we
-   already know that we should setup the proximity sensor
-*/
+// Constructor for AP_Follow. Initializes the position P-controller and sets up parameter defaults.
 AP_Follow::AP_Follow() :
-        _p_pos(AP_FOLLOW_POS_P_DEFAULT)
+    _p_pos(AP_FOLLOW_POS_P_DEFAULT)
 {
     _singleton = this;
     AP_Param::setup_object_defaults(this, var_info);
 }
 
-// restore offsets to zero if necessary, should be called when vehicle exits follow mode
-void AP_Follow::clear_offsets_if_required()
+
+//==============================================================================
+// Target Estimation Update Functions
+//==============================================================================
+
+// Projects and updates the estimated target position, velocity, and heading based on last known data and configured input shaping.
+bool AP_Follow::update_estimate()
 {
-    if (_offsets_were_zero) {
-        _offset.set(Vector3f());
-        _offsets_were_zero = false;
+    // exit immediately if not enabled
+    if (!_enabled) {
+        _estimate_valid = false;
+        return _estimate_valid;
     }
-}
 
-// get target's estimated location
-bool AP_Follow::get_target_location_and_velocity(Location &loc, Vector3f &vel_ned) const
-{
-    Vector3p pos_ned;
-    Vector3f local_vel_ned;  // so we either change both return parameters or neither
-    if (!get_target_position_and_velocity(pos_ned, local_vel_ned)) {
-        return false;
+    // check for timeout: if no recent location update, invalidate estimate
+    if ((_last_location_update_ms == 0) || (AP_HAL::millis() - _last_location_update_ms > AP_FOLLOW_TIMEOUT_MS)) {
+        _estimate_valid = false;
+        return _estimate_valid;
     }
-    if (!AP::ahrs().get_location_from_origin_offset_NED(loc, pos_ned)) {
-        return false;
+
+    // skip if update has already been processed this scheduler tick
+    if (_estimate_valid && (AP::scheduler().ticks32() - _last_update_ticks == 0)) {
+        return _estimate_valid;
     }
-    vel_ned = local_vel_ned;
+    _last_update_ticks = AP::scheduler().ticks32();
 
-    if (_alt_type == AP_FOLLOW_ALTITUDE_TYPE_RELATIVE) {
-        loc.change_alt_frame(Location::AltFrame::ABOVE_HOME);
+    // if sysid changed, reset the estimation state
+    if (_sysid != _sysid_used) {
+        _sysid_used = _sysid;
+        _estimate_valid = false;
     }
 
-    return true;
-}
+    const uint32_t now = AP_HAL::millis();
 
-// get target's estimated location and velocity, both NED SI from origin
-bool AP_Follow::get_target_position_and_velocity(Vector3p &pos_ned, Vector3f &vel_ned) const
-{
-    // exit immediately if not enabled
-    if (!_enabled) {
-        return false;
+    // calculate time since last location update in seconds
+    const float dt = (now - _last_location_update_ms) * 0.001f;
+
+    // project target's position and velocity forward using simple kinematics
+    Vector3f delta_pos = _target_vel_ned_ms * dt + _target_accel_ned_mss * 0.5f * sq(dt);
+    Vector3f delta_vel = _target_accel_ned_mss * dt;
+
+    // calculate time since last estimation update
+    const float e_dt = (now - _last_estimation_update_ms) * 0.001f;
+    
+    if (_estimate_valid) {
+        // 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)
+        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.xy().topostype(), _target_vel_ned_ms.xy() + delta_vel.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 angular shaping for heading estimate
+        shape_angle_vel_accel(radians(_target_heading_deg), radians(_target_heading_rate_degs), 0.0,
+                              _estimate_heading_rad, _estimate_heading_rate_rads, _estimate_heading_accel_radss,
+                              0.0, radians(_accel_max_h_degss),
+                              radians(_jerk_max_h_degsss), e_dt, false);
+
+        // update heading angle separately to maintain proper wrapping [-PI, PI]
+        postype_t estimate_heading_rad = _estimate_heading_rad;
+        update_pos_vel_accel(estimate_heading_rad, _estimate_heading_rate_rads, _estimate_heading_accel_radss, e_dt, 0.0, 0.0, 0.0);
+        _estimate_heading_rad = wrap_PI(estimate_heading_rad);
+    } else {
+        // no valid estimate yet: initialise from latest target position
+        _estimate_pos_ned_m = _target_pos_ned_m + delta_pos.topostype();
+        _estimate_vel_ned_ms = _target_vel_ned_ms + delta_vel;
+        _estimate_accel_ned_mss = _target_accel_ned_mss;
+        _estimate_valid = true;
     }
 
-    // check for timeout
-    if ((_last_location_update_ms == 0) || (AP_HAL::millis() - _last_location_update_ms > AP_FOLLOW_TIMEOUT_MS)) {
-        return false;
+    Vector3f offset_m = _offset_XXD_m.get();
+
+    // calculate estimated position and velocity with offsets applied
+    if (offset_m.is_zero() || (_offset_type == AP_FOLLOW_OFFSET_TYPE_NED)) {
+        // offsets are in NED frame: simple addition
+        _ofs_estimate_pos_ned_m = _estimate_pos_ned_m + offset_m.topostype();
+        _ofs_estimate_vel_ned_ms = _estimate_vel_ned_ms;
+        _ofs_estimate_accel_ned_mss = _estimate_accel_ned_mss;
+    } else {
+        // offsets are in FRD frame: rotate by heading
+        offset_m.xy().rotate(_estimate_heading_rad);
+        Vector3f offset_cross = offset_m.cross(Vector3f{0.0, 0.0, 1.0});
+        float offset_length_m = offset_m.length();
+        _ofs_estimate_pos_ned_m = _estimate_pos_ned_m + offset_m.topostype();
+        _ofs_estimate_vel_ned_ms = _estimate_vel_ned_ms + offset_cross * offset_length_m * _estimate_heading_rate_rads;
+        _ofs_estimate_accel_ned_mss = _estimate_accel_ned_mss + offset_cross * offset_length_m * _estimate_heading_accel_radss;
     }
 
-    // calculate time since last actual position update
-    const float dt = (AP_HAL::millis() - _last_location_update_ms) * 0.001f;
+    _last_estimation_update_ms = now;
+    return _estimate_valid;
+}
+
 
-    // get velocity estimate
-    if (!get_velocity_ned(vel_ned, dt)) {
+//==============================================================================
+// Target Information Retrieval Functions
+//==============================================================================
+
+// Retrieves the estimated target position, velocity, and acceleration in the NED frame (relative to origin).
+bool AP_Follow::get_target_pos_vel_accel_NED_m(Vector3p &pos_ned_m, Vector3f &vel_ned_ms, Vector3f &accel_ned_mss)
+{
+    if (!update_estimate()) {
         return false;
     }
 
-    // project the vehicle position
-    const Vector3p vel_ned_p { vel_ned.x, vel_ned.y, vel_ned.z };
-    pos_ned = _target_position_ned + vel_ned_p * dt;
+    pos_ned_m = _estimate_pos_ned_m;
+    vel_ned_ms = _estimate_vel_ned_ms;
+    accel_ned_mss = _estimate_accel_ned_mss;
 
     return true;
 }
 
-// get distance vector to target (in meters) and target's velocity all in NED frame
-bool AP_Follow::get_target_dist_and_vel_ned(Vector3f &dist_ned, Vector3f &dist_with_offs, Vector3f &vel_ned)
+// Retrieves the estimated target position, velocity, and acceleration in the NED frame, including configured offsets.
+bool AP_Follow::get_ofs_pos_vel_accel_NED_m(Vector3p &pos_ofs_ned_m, Vector3f &vel_ofs_ned_ms, Vector3f &accel_ofs_ned_mss)
 {
-    Vector3f current_position_NED;
-    if (!AP::ahrs().get_relative_position_NED_origin(current_position_NED)) {
-        clear_dist_and_bearing_to_target();
-        return false;
-    }
-    Vector3p target_position_NED;
-    Vector3f veh_vel;  // NED
-    if (!get_target_position_and_velocity(target_position_NED, veh_vel)) {
-        clear_dist_and_bearing_to_target();
+    if (!update_estimate()) {
         return false;
     }
 
-    // convert from Vector3p to Vector3f as they can't be subtracted:
-    Vector3f target_position_NED_f;
-    target_position_NED_f.x = target_position_NED.x;
-    target_position_NED_f.y = target_position_NED.y;
-    target_position_NED_f.z = target_position_NED.z;
+    pos_ofs_ned_m = _ofs_estimate_pos_ned_m;
+    vel_ofs_ned_ms = _ofs_estimate_vel_ned_ms;
+    accel_ofs_ned_mss = _ofs_estimate_accel_ned_mss;
 
-    const Vector3f dist_vec = target_position_NED_f - current_position_NED;
+    return true;
+}
 
-    // fail if too far
-    if (is_positive(_dist_max.get()) && (dist_vec.length() > _dist_max)) {
-        clear_dist_and_bearing_to_target();
+// Retrieves distance vectors (with and without configured offsets) and the target’s velocity, all in the NED frame.
+bool AP_Follow::get_target_dist_and_vel_NED_m(Vector3f &dist_ned, Vector3f &dist_with_offs, Vector3f &vel_ned)
+{
+    if (!update_estimate()) {
         return false;
     }
 
-    // initialise offsets from distance vector if required
-    init_offsets_if_required(dist_vec);
-
-    // get offsets
-    Vector3f offsets;
-    if (!get_offsets_ned(offsets)) {
+    Vector3f current_position_ned_m;
+    if (!AP::ahrs().get_relative_position_NED_origin(current_position_ned_m)) {
         clear_dist_and_bearing_to_target();
         return false;
     }
 
-    // calculate results
-    dist_ned = dist_vec;
-    dist_with_offs = dist_vec + offsets;
-    vel_ned = veh_vel;
+    const Vector3p dist_vec_ned_m = _estimate_pos_ned_m - current_position_ned_m.topostype();
+    const Vector3p ofs_dist_vec = _ofs_estimate_pos_ned_m - current_position_ned_m.topostype();
+    dist_ned = dist_vec_ned_m.tofloat();
+    dist_with_offs = ofs_dist_vec.tofloat();
+    vel_ned = _ofs_estimate_vel_ned_ms;
 
-    // record distance and heading for reporting purposes
-    if (is_zero(dist_with_offs.x) && is_zero(dist_with_offs.y)) {
-        clear_dist_and_bearing_to_target();
-    } else {
-        _dist_to_target = safe_sqrt(sq(dist_with_offs.x) + sq(dist_with_offs.y));
-        _bearing_to_target = degrees(atan2f(dist_with_offs.y, dist_with_offs.x));
+    return true;
+}
+
+// Retrieves the estimated target heading and heading rate in radians.
+bool AP_Follow::get_heading_heading_rate_rad(float &heading_rad, float &heading_rate_rads)
+{
+    if (!update_estimate()) {
+        return false;
     }
 
+    // return latest heading estimate
+    heading_rad = _estimate_heading_rad;
+    heading_rate_rads = _estimate_heading_rate_rads;
     return true;
 }
 
-// get target's heading in degrees (0 = north, 90 = east)
-bool AP_Follow::get_target_heading_deg(float &heading) const
+// Retrieves the target's estimated global location and velocity, adjusting altitude frame if relative mode is set (for LUA bindings).
+bool AP_Follow::get_target_location_and_velocity(Location &loc, Vector3f &vel_ned)
 {
-    // exit immediately if not enabled
-    if (!_enabled) {
+    if (!update_estimate()) {
         return false;
     }
 
-    // check for timeout
-    if ((_last_heading_update_ms == 0) || (AP_HAL::millis() - _last_heading_update_ms > AP_FOLLOW_TIMEOUT_MS)) {
+    if (!AP::ahrs().get_location_from_origin_offset_NED(loc, _estimate_pos_ned_m)) {
         return false;
     }
+    vel_ned = _estimate_vel_ned_ms;
+
+    if (_alt_type == AP_FOLLOW_ALTITUDE_TYPE_RELATIVE) {
+        loc.change_alt_frame(Location::AltFrame::ABOVE_HOME);
+    }
 
-    // return latest heading estimate
-    heading = _target_heading;
     return true;
 }
 
-// returns true if we should extract information from msg
-bool AP_Follow::should_handle_message(const mavlink_message_t &msg) const
+// Retrieves the target's estimated global location and velocity, including configured offsets, for LUA bindings.
+bool AP_Follow::get_target_location_and_velocity_ofs(Location &loc, Vector3f &vel_ned)
 {
-    // exit immediately if not enabled
-    if (!_enabled) {
+    if (!update_estimate()) {
         return false;
     }
-
-    // skip our own messages
-    if (msg.sysid == mavlink_system.sysid) {
+    if (!AP::ahrs().get_location_from_origin_offset_NED(loc, _ofs_estimate_pos_ned_m)) {
         return false;
     }
+    if (_alt_type == AP_FOLLOW_ALTITUDE_TYPE_RELATIVE) {
+        loc.change_alt_frame(Location::AltFrame::ABOVE_HOME);
+    }
 
-    // skip message if not from our target
-    if (_sysid != 0 && msg.sysid != _sysid) {
+    vel_ned = _ofs_estimate_vel_ned_ms;
+    return true;
+}
+
+// Retrieves the estimated target heading in degrees (0° = North, 90° = East) for LUA bindings.
+bool AP_Follow::get_target_heading_deg(float &heading)
+{
+    if (!update_estimate()) {
         return false;
     }
 
+    // return latest heading estimate
+    heading = degrees(_estimate_heading_rad);
     return true;
 }
 
-// handle mavlink DISTANCE_SENSOR messages
+
+//==============================================================================
+// MAVLink Message Handling
+//==============================================================================
+
+// Handles incoming MAVLink messages to update the target's position, velocity, and heading.
 void AP_Follow::handle_msg(const mavlink_message_t &msg)
 {
-    // this method should be called from an "update()" method:
     if (_automatic_sysid) {
-        // maybe timeout who we were following...
+        // timeout check: if no position update received within timeout, clear automatic sysid
         if ((_last_location_update_ms == 0) ||
             (AP_HAL::millis() - _last_location_update_ms > AP_FOLLOW_SYSID_TIMEOUT_MS)) {
             _sysid.set(0);
+            _sysid_used = 0;
+            _estimate_valid = false;
         }
     }
 
     if (!should_handle_message(msg)) {
+        // ignore message if filtering rules reject it (e.g., wrong sysid)
         return;
     }
 
-    // decode global-position-int message
+    // decode MAVLink message
     bool updated = false;
 
     switch (msg.msgid) {
     case MAVLINK_MSG_ID_GLOBAL_POSITION_INT: {
+        // handle standard global position messages
         updated = handle_global_position_int_message(msg);
         break;
     }
     case MAVLINK_MSG_ID_FOLLOW_TARGET: {
+        // handle follow-target specific messages
         updated = handle_follow_target_message(msg);
         break;
     }
     }
 
     if (updated) {
+        // message provided updated position information
+        Vector3f offset_m = _offset_XXD_m.get();
+
+        // if using relative frame, rotate offsets based on estimated heading
+        if (_offset_type == AP_FOLLOW_OFFSET_TYPE_RELATIVE) {
+            offset_m.xy().rotate(_estimate_heading_rad);
+        }
+
+        Vector3f current_position_ned_m;
+        if (!AP::ahrs().get_relative_position_NED_origin(current_position_ned_m)) {
+            // 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
+            const Vector3p dist_vec_ned_m = _estimate_pos_ned_m - current_position_ned_m.topostype();
+            const Vector3p ofs_dist_vec = _ofs_estimate_pos_ned_m - current_position_ned_m.topostype();
+
+            // initialise offsets dynamically if needed
+            init_offsets_if_required(dist_vec_ned_m.tofloat());
+
+            // record distance and bearing to target for reporting/logging
+            if (ofs_dist_vec.xy().is_zero()) {
+                clear_dist_and_bearing_to_target();
+            } else {
+                _dist_to_target_m = ofs_dist_vec.xy().length();
+                _bearing_to_target_deg = degrees(ofs_dist_vec.xy().angle());
+            }
+
+            // clear distance and bearing if outside configured max follow distance
+            if (is_positive(_dist_max_m.get()) && (dist_vec_ned_m.length() > _dist_max_m)) {
+                clear_dist_and_bearing_to_target();
+            }
+        }
+
+        // Check if estimate needs reset based on position and velocity errors
+        if (estimate_error_too_large()) {
+            _estimate_valid = false;
+        }
+
 #if HAL_LOGGING_ENABLED
+        // log current follow diagnostic data
         Log_Write_FOLL();
 #endif
     }
 }
 
-bool AP_Follow::handle_global_position_int_message(const mavlink_message_t &msg)
+// Returns true if the incoming MAVLink message should be processed for target updates.
+bool AP_Follow::should_handle_message(const mavlink_message_t &msg) const
 {
-        // decode message
-        mavlink_global_position_int_t packet;
-        mavlink_msg_global_position_int_decode(&msg, &packet);
+    // exit immediately if not enabled
+    if (!_enabled) {
+        return false;
+    }
 
-        // ignore message if lat and lon are (exactly) zero
-        if ((packet.lat == 0 && packet.lon == 0)) {
-            return false;
-        }
+    // skip our own messages
+    if (msg.sysid == mavlink_system.sysid) {
+        return false;
+    }
 
-        Location _target_location;
-        _target_location.lat = packet.lat;
-        _target_location.lng = packet.lon;
+    // skip message if not from our target
+    if (_sysid != 0 && msg.sysid != _sysid) {
+        return false;
+    }
 
-        // select altitude source based on FOLL_ALT_TYPE param 
-        if (_alt_type == AP_FOLLOW_ALTITUDE_TYPE_RELATIVE) {
-            // above home alt
-            _target_location.set_alt_cm(packet.relative_alt / 10, Location::AltFrame::ABOVE_HOME);
-        } else {
-            // absolute altitude
-            _target_location.set_alt_cm(packet.alt / 10, Location::AltFrame::ABSOLUTE);
-        }
-        if (!_target_location.get_vector_from_origin_NEU(_target_position_ned)) {
-            return false;
-        }
-        _target_position_ned.z = -_target_position_ned.z; // NEU->NED
-        _target_position_ned *= 0.01;  // cm -> m
-
-        _target_velocity_ned.x = packet.vx * 0.01f; // velocity north
-        _target_velocity_ned.y = packet.vy * 0.01f; // velocity east
-        _target_velocity_ned.z = packet.vz * 0.01f; // velocity down
-
-        // get a local timestamp with correction for transport jitter
-        _last_location_update_ms = _jitter.correct_offboard_timestamp_msec(packet.time_boot_ms, AP_HAL::millis());
-        if (packet.hdg <= 36000) {                  // heading (UINT16_MAX if unknown)
-            _target_heading = packet.hdg * 0.01f;   // convert centi-degrees to degrees
-            _last_heading_update_ms = _last_location_update_ms;
-        }
-        // initialise _sysid if zero to sender's id
-        if (_sysid == 0) {
-            _sysid.set(msg.sysid);
-            _automatic_sysid = true;
-        }
-        return true;
+    return true;
 }
 
-bool AP_Follow::handle_follow_target_message(const mavlink_message_t &msg)
+// Checks whether the current estimate should be reset based on position and velocity errors.
+bool AP_Follow::estimate_error_too_large() const
 {
-        // decode message
-        mavlink_follow_target_t packet;
-        mavlink_msg_follow_target_decode(&msg, &packet);
+    const float timeout_sec = AP_FOLLOW_TIMEOUT_MS * 0.001f;
+    const float half_timeout_sec = timeout_sec * 0.5f;
 
-        // ignore message if lat and lon are (exactly) zero
-        if ((packet.lat == 0 && packet.lon == 0)) {
-            return false;
-        }
-        // require at least position
-        if ((packet.est_capabilities & (1<<0)) == 0) {
-            return false;
-        }
+    // Calculate position thresholds
+    const float pos_thresh_horiz_m = _accel_max_ne_mss.get() * sq(half_timeout_sec);
+    const float pos_thresh_vert_m  = _accel_max_d_mss.get()  * sq(half_timeout_sec);
 
-        const Location new_loc {
-            packet.lat,
-            packet.lon,
-            int32_t(packet.alt * 100),  // m -> cm
-            Location::AltFrame::ABSOLUTE
-        };
-
-        if (!new_loc.get_vector_from_origin_NEU(_target_position_ned)) {
-            return false;
-        }
-        _target_position_ned.z = -_target_position_ned.z; // NEU->NED
-        _target_position_ned *= 0.01;  // cm -> m
+    // Calculate velocity thresholds using the new helper function
+    const float vel_thresh_horiz_ms = calc_max_velocity_change(_accel_max_ne_mss.get(), _jerk_max_ne_msss.get(), timeout_sec);
+    const float vel_thresh_vert_ms  = calc_max_velocity_change(_accel_max_d_mss.get(), _jerk_max_d_msss.get(), timeout_sec);
 
-        if (packet.est_capabilities & (1<<1)) {
-            _target_velocity_ned.x = packet.vel[0]; // velocity north
-            _target_velocity_ned.y = packet.vel[1]; // velocity east
-            _target_velocity_ned.z = packet.vel[2]; // velocity down
-        } else {
-            _target_velocity_ned.zero();
-        }
+    // Calculate current position and velocity errors
+    Vector3f pos_error = _estimate_pos_ned_m.tofloat() - _target_pos_ned_m.tofloat();
+    Vector3f vel_error = _estimate_vel_ned_ms - _target_vel_ned_ms;
 
-        // get a local timestamp with correction for transport jitter
-        _last_location_update_ms = _jitter.correct_offboard_timestamp_msec(packet.timestamp, AP_HAL::millis());
+    Vector2f pos_error_xy = pos_error.xy();
+    float pos_error_z = pos_error.z;
+    Vector2f vel_error_xy = vel_error.xy();
+    float vel_error_z = vel_error.z;
 
-        if (packet.est_capabilities & (1<<3)) {
-            Quaternion q{packet.attitude_q[0], packet.attitude_q[1], packet.attitude_q[2], packet.attitude_q[3]};
-            float r, p, y;
-            q.to_euler(r,p,y);
-            _target_heading = degrees(y);
-            _last_heading_update_ms = _last_location_update_ms;
-        }
+    // Check horizontal and vertical separately
+    const bool pos_horiz_bad = pos_error_xy.length() > pos_thresh_horiz_m;
+    const bool vel_horiz_bad = vel_error_xy.length() > vel_thresh_horiz_ms;
+    const bool pos_vert_bad = fabsf(pos_error_z) > pos_thresh_vert_m;
+    const bool vel_vert_bad = fabsf(vel_error_z) > vel_thresh_vert_ms;
 
-        // initialise _sysid if zero to sender's id
-        if (_sysid == 0) {
-            _sysid.set(msg.sysid);
-            _automatic_sysid = true;
-        }
+    return pos_horiz_bad || vel_horiz_bad || pos_vert_bad || vel_vert_bad;
+}
 
-        return true;
+// Calculates max velocity change under trapezoidal or triangular acceleration profile (jerk-limited).
+float AP_Follow::calc_max_velocity_change(float accel_max, float jerk_max, float timeout_sec) const
+{
+    const float t_jerk = accel_max / jerk_max;
+    const float t_total_jerk = 2.0f * t_jerk;
+
+    if (timeout_sec >= t_total_jerk) {
+        // time to ramp up, constant accel phase, and ramp down
+        const float t_const = timeout_sec - t_total_jerk;
+        const float delta_v_jerk = 0.5f * accel_max * t_jerk;
+        const float delta_v_const = accel_max * t_const;
+        return 2.0f * delta_v_jerk + delta_v_const;
+    } else {
+        // timeout too short: pure triangle profile
+        const float t_half = timeout_sec * 0.5f;
+        return 0.5f * jerk_max * sq(t_half);
+    }
 }
 
-// write out an onboard-log message to help diagnose follow problems:
-#if HAL_LOGGING_ENABLED
-void AP_Follow::Log_Write_FOLL()
+// Decodes a GLOBAL_POSITION_INT MAVLink message to update the target’s position, velocity, and heading.
+bool AP_Follow::handle_global_position_int_message(const mavlink_message_t &msg)
 {
-        // get estimated location and velocity
-        Location loc_estimate{};
-        Vector3f vel_estimate;
-        UNUSED_RESULT(get_target_location_and_velocity(loc_estimate, vel_estimate));
-
-        Location _target_location;
-        UNUSED_RESULT(AP::ahrs().get_location_from_origin_offset_NED(_target_location, _target_position_ned * 0.01));
-        if (_alt_type == AP_FOLLOW_ALTITUDE_TYPE_RELATIVE) {
-            _target_location.change_alt_frame(Location::AltFrame::ABOVE_HOME);
-        }
+    // decode GLOBAL_POSITION_INT message into packet struct
+    mavlink_global_position_int_t packet;
+    mavlink_msg_global_position_int_decode(&msg, &packet);
+
+    // ignore message if latitude and longitude are exactly zero (invalid GPS fix)
+    if ((packet.lat == 0 && packet.lon == 0)) {
+        return false;
+    }
+
+    Location _target_location;
+    _target_location.lat = packet.lat;
+    _target_location.lng = packet.lon;
+
+    // set target altitude based on configured altitude type
+    if (_alt_type == AP_FOLLOW_ALTITUDE_TYPE_RELATIVE) {
+        // use relative altitude above home
+        _target_location.set_alt_cm(packet.relative_alt / 10, Location::AltFrame::ABOVE_HOME);
+    } else {
+        // use absolute altitude
+        _target_location.set_alt_cm(packet.alt / 10, Location::AltFrame::ABSOLUTE);
+    }
+
+    // convert global location to local NED frame position
+    if (!_target_location.get_vector_from_origin_NEU(_target_pos_ned_m)) {
+        return false;
+    }
+    _target_pos_ned_m.z = -_target_pos_ned_m.z; // convert NEU -> NED
+    _target_pos_ned_m *= 0.01;  // convert from cm to meters
+
+    // decode target velocity components (in m/s)
+    _target_vel_ned_ms.x = packet.vx * 0.01f; // velocity north
+    _target_vel_ned_ms.y = packet.vy * 0.01f; // velocity east
+    _target_vel_ned_ms.z = packet.vz * 0.01f; // velocity down
+
+    // target acceleration not available in GLOBAL_POSITION_INT
+    _target_accel_ned_mss.zero();
+
+    if (packet.hdg <= 36000) {
+        // valid heading field available (in centi-degrees)
+        _target_heading_deg = packet.hdg * 0.01f;
+    } else if (_offset_type == AP_FOLLOW_OFFSET_TYPE_RELATIVE) {
+        // heading missing but relative offset mode requires heading -> reject
+        return false;
+    } else {
+        // no heading available: set heading rate to zero
+        _target_heading_rate_degs = 0.0f;
+    }
+
+    // apply jitter-corrected timestamp to this update
+    _last_location_update_ms = _jitter.correct_offboard_timestamp_msec(packet.time_boot_ms, AP_HAL::millis());
 
-        // log lead's estimated vs reported position
-// @LoggerMessage: FOLL
-// @Description: Follow library diagnostic data
-// @Field: TimeUS: Time since system startup
-// @Field: Lat: Target latitude
-// @Field: Lon: Target longitude
-// @Field: Alt: Target absolute altitude
-// @Field: VelN: Target earth-frame velocity, North
-// @Field: VelE: Target earth-frame velocity, East
-// @Field: VelD: Target earth-frame velocity, Down
-// @Field: LatE: Vehicle latitude
-// @Field: LonE: Vehicle longitude
-// @Field: AltE: Vehicle absolute altitude
-        AP::logger().WriteStreaming("FOLL",
-                                               "TimeUS,Lat,Lon,Alt,VelN,VelE,VelD,LatE,LonE,AltE",  // labels
-                                               "sDUmnnnDUm",    // units
-                                               "F--B000--B",    // mults
-                                               "QLLifffLLi",    // fmt
-                                               AP_HAL::micros64(),
-                                               _target_location.lat,
-                                               _target_location.lng,
-                                               _target_location.alt,
-                                               (double)_target_velocity_ned.x,
-                                               (double)_target_velocity_ned.y,
-                                               (double)_target_velocity_ned.z,
-                                               loc_estimate.lat,
-                                               loc_estimate.lng,
-                                               loc_estimate.alt
-                                               );
+    // if sysid not yet set, adopt sender’s sysid and enable automatic sysid tracking
+    if (_sysid == 0) {
+        _sysid.set(msg.sysid);
+        _sysid_used = 0;
+        _estimate_valid = false;
+        _automatic_sysid = true;
+    }
+
+    return true;
 }
-#endif  // HAL_LOGGING_ENABLED
 
-// get velocity estimate in m/s in NED frame using dt since last update
-bool AP_Follow::get_velocity_ned(Vector3f &vel_ned, float dt) const
+// Decodes a FOLLOW_TARGET MAVLink message to update the target’s position, velocity, acceleration, and orientation.
+bool AP_Follow::handle_follow_target_message(const mavlink_message_t &msg)
 {
-    vel_ned = _target_velocity_ned + (_target_accel_ned * dt);
+    // decode FOLLOW_TARGET message into packet struct
+    mavlink_follow_target_t packet;
+    mavlink_msg_follow_target_decode(&msg, &packet);
+
+    // ignore message if latitude and longitude are exactly zero (invalid GPS fix)
+    if ((packet.lat == 0 && packet.lon == 0)) {
+        return false;
+    }
+
+    // require that at least position is estimated (bit 0 of est_capabilities)
+    if ((packet.est_capabilities & (1<<0)) == 0) {
+        return false;
+    }
+
+    // build Location object from latitude, longitude, and altitude (alt in meters)
+    const Location _target_location {
+        packet.lat,
+        packet.lon,
+        int32_t(packet.alt * 100),  // convert meters to centimeters
+        Location::AltFrame::ABSOLUTE
+    };
+
+    // convert global location to local NED frame position
+    if (!_target_location.get_vector_from_origin_NEU(_target_pos_ned_m)) {
+        return false;
+    }
+    _target_pos_ned_m *= 0.01; // convert from cm to meters
+
+    // adjust Z coordinate to NED frame (NEU altitude -> NED)
+    Location origin;
+    if (!AP::ahrs().get_origin(origin)) {
+        return false;
+    }
+    _target_pos_ned_m.z = -packet.alt + origin.alt * 0.01;
+
+    // decode velocity if available (bit 1 of est_capabilities)
+    if (packet.est_capabilities & (1<<1)) {
+        _target_vel_ned_ms.x = packet.vel[0]; // velocity north
+        _target_vel_ned_ms.y = packet.vel[1]; // velocity east
+        _target_vel_ned_ms.z = packet.vel[2]; // velocity down
+    } else {
+        _target_vel_ned_ms.zero();
path: libraries/AP_Follow/AP_Follow.cpp
user: login: timtuxworth
body: When I send FOLLOW_TARGET from a plane, it doesn't have FOLLOW_TARGET_CAPABILITIES.VEL and FOLLOW_TARGET_CAPABILITIES.ACCEL and this breaks the follow code, I've seen it in testing. These lines (710,719) need to go.
created_at: 2025-05-01T23:38:52Z
author_association: CONTRIBUTOR
diff_hunk: @@ -134,419 +155,638 @@ const AP_Param::GroupInfo AP_Follow::var_info[] = {
     AP_GROUPINFO("_ALT_TYPE", 10, AP_Follow, _alt_type, AP_FOLLOW_ALT_TYPE_DEFAULT),
 #endif
 
+// May add option to turn on input shaping, turn on acceleration due to rotation of offsest
+
     // @Param: _OPTIONS
     // @DisplayName: Follow options
     // @Description: Follow options bitmask
     // @Values: 0:None,1: Mount Follows lead vehicle on mode enter
     // @User: Standard
     AP_GROUPINFO("_OPTIONS", 11, AP_Follow, _options, 0),
 
+    // @Param: _ACCEL_NE
+    // @DisplayName: Acceleration limit for the horizontal kinematic input shaping
+    // @Description: Acceleration limit of the horizontal kinematic path generation used to determine how quickly the ship varies in velocity
+    // @Range: 0 5
+    // @Units: m/s/s
+    // @User: Advanced
+    AP_GROUPINFO("_ACCEL_NE", 12, AP_Follow, _accel_max_ne_mss, 2.5),
+
+    // @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 ship 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),
+
+    // @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 ship 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),
+
+    // @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 ship 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),
+
+    // @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 ship 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),
+
+    // @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 ship 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_GROUPEND
 };
 
-/* 
-   The constructor also initialises the proximity sensor. Note that this
-   constructor is not called until detect() returns true, so we
-   already know that we should setup the proximity sensor
-*/
+// Constructor for AP_Follow. Initializes the position P-controller and sets up parameter defaults.
 AP_Follow::AP_Follow() :
-        _p_pos(AP_FOLLOW_POS_P_DEFAULT)
+    _p_pos(AP_FOLLOW_POS_P_DEFAULT)
 {
     _singleton = this;
     AP_Param::setup_object_defaults(this, var_info);
 }
 
-// restore offsets to zero if necessary, should be called when vehicle exits follow mode
-void AP_Follow::clear_offsets_if_required()
+
+//==============================================================================
+// Target Estimation Update Functions
+//==============================================================================
+
+// Projects and updates the estimated target position, velocity, and heading based on last known data and configured input shaping.
+bool AP_Follow::update_estimate()
 {
-    if (_offsets_were_zero) {
-        _offset.set(Vector3f());
-        _offsets_were_zero = false;
+    // exit immediately if not enabled
+    if (!_enabled) {
+        _estimate_valid = false;
+        return _estimate_valid;
     }
-}
 
-// get target's estimated location
-bool AP_Follow::get_target_location_and_velocity(Location &loc, Vector3f &vel_ned) const
-{
-    Vector3p pos_ned;
-    Vector3f local_vel_ned;  // so we either change both return parameters or neither
-    if (!get_target_position_and_velocity(pos_ned, local_vel_ned)) {
-        return false;
+    // check for timeout: if no recent location update, invalidate estimate
+    if ((_last_location_update_ms == 0) || (AP_HAL::millis() - _last_location_update_ms > AP_FOLLOW_TIMEOUT_MS)) {
+        _estimate_valid = false;
+        return _estimate_valid;
     }
-    if (!AP::ahrs().get_location_from_origin_offset_NED(loc, pos_ned)) {
-        return false;
+
+    // skip if update has already been processed this scheduler tick
+    if (_estimate_valid && (AP::scheduler().ticks32() - _last_update_ticks == 0)) {
+        return _estimate_valid;
     }
-    vel_ned = local_vel_ned;
+    _last_update_ticks = AP::scheduler().ticks32();
 
-    if (_alt_type == AP_FOLLOW_ALTITUDE_TYPE_RELATIVE) {
-        loc.change_alt_frame(Location::AltFrame::ABOVE_HOME);
+    // if sysid changed, reset the estimation state
+    if (_sysid != _sysid_used) {
+        _sysid_used = _sysid;
+        _estimate_valid = false;
     }
 
-    return true;
-}
+    const uint32_t now = AP_HAL::millis();
 
-// get target's estimated location and velocity, both NED SI from origin
-bool AP_Follow::get_target_position_and_velocity(Vector3p &pos_ned, Vector3f &vel_ned) const
-{
-    // exit immediately if not enabled
-    if (!_enabled) {
-        return false;
+    // calculate time since last location update in seconds
+    const float dt = (now - _last_location_update_ms) * 0.001f;
+
+    // project target's position and velocity forward using simple kinematics
+    Vector3f delta_pos = _target_vel_ned_ms * dt + _target_accel_ned_mss * 0.5f * sq(dt);
+    Vector3f delta_vel = _target_accel_ned_mss * dt;
+
+    // calculate time since last estimation update
+    const float e_dt = (now - _last_estimation_update_ms) * 0.001f;
+    
+    if (_estimate_valid) {
+        // 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)
+        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.xy().topostype(), _target_vel_ned_ms.xy() + delta_vel.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 angular shaping for heading estimate
+        shape_angle_vel_accel(radians(_target_heading_deg), radians(_target_heading_rate_degs), 0.0,
+                              _estimate_heading_rad, _estimate_heading_rate_rads, _estimate_heading_accel_radss,
+                              0.0, radians(_accel_max_h_degss),
+                              radians(_jerk_max_h_degsss), e_dt, false);
+
+        // update heading angle separately to maintain proper wrapping [-PI, PI]
+        postype_t estimate_heading_rad = _estimate_heading_rad;
+        update_pos_vel_accel(estimate_heading_rad, _estimate_heading_rate_rads, _estimate_heading_accel_radss, e_dt, 0.0, 0.0, 0.0);
+        _estimate_heading_rad = wrap_PI(estimate_heading_rad);
+    } else {
+        // no valid estimate yet: initialise from latest target position
+        _estimate_pos_ned_m = _target_pos_ned_m + delta_pos.topostype();
+        _estimate_vel_ned_ms = _target_vel_ned_ms + delta_vel;
+        _estimate_accel_ned_mss = _target_accel_ned_mss;
+        _estimate_valid = true;
     }
 
-    // check for timeout
-    if ((_last_location_update_ms == 0) || (AP_HAL::millis() - _last_location_update_ms > AP_FOLLOW_TIMEOUT_MS)) {
-        return false;
+    Vector3f offset_m = _offset_XXD_m.get();
+
+    // calculate estimated position and velocity with offsets applied
+    if (offset_m.is_zero() || (_offset_type == AP_FOLLOW_OFFSET_TYPE_NED)) {
+        // offsets are in NED frame: simple addition
+        _ofs_estimate_pos_ned_m = _estimate_pos_ned_m + offset_m.topostype();
+        _ofs_estimate_vel_ned_ms = _estimate_vel_ned_ms;
+        _ofs_estimate_accel_ned_mss = _estimate_accel_ned_mss;
+    } else {
+        // offsets are in FRD frame: rotate by heading
+        offset_m.xy().rotate(_estimate_heading_rad);
+        Vector3f offset_cross = offset_m.cross(Vector3f{0.0, 0.0, 1.0});
+        float offset_length_m = offset_m.length();
+        _ofs_estimate_pos_ned_m = _estimate_pos_ned_m + offset_m.topostype();
+        _ofs_estimate_vel_ned_ms = _estimate_vel_ned_ms + offset_cross * offset_length_m * _estimate_heading_rate_rads;
+        _ofs_estimate_accel_ned_mss = _estimate_accel_ned_mss + offset_cross * offset_length_m * _estimate_heading_accel_radss;
     }
 
-    // calculate time since last actual position update
-    const float dt = (AP_HAL::millis() - _last_location_update_ms) * 0.001f;
+    _last_estimation_update_ms = now;
+    return _estimate_valid;
+}
+
 
-    // get velocity estimate
-    if (!get_velocity_ned(vel_ned, dt)) {
+//==============================================================================
+// Target Information Retrieval Functions
+//==============================================================================
+
+// Retrieves the estimated target position, velocity, and acceleration in the NED frame (relative to origin).
+bool AP_Follow::get_target_pos_vel_accel_NED_m(Vector3p &pos_ned_m, Vector3f &vel_ned_ms, Vector3f &accel_ned_mss)
+{
+    if (!update_estimate()) {
         return false;
     }
 
-    // project the vehicle position
-    const Vector3p vel_ned_p { vel_ned.x, vel_ned.y, vel_ned.z };
-    pos_ned = _target_position_ned + vel_ned_p * dt;
+    pos_ned_m = _estimate_pos_ned_m;
+    vel_ned_ms = _estimate_vel_ned_ms;
+    accel_ned_mss = _estimate_accel_ned_mss;
 
     return true;
 }
 
-// get distance vector to target (in meters) and target's velocity all in NED frame
-bool AP_Follow::get_target_dist_and_vel_ned(Vector3f &dist_ned, Vector3f &dist_with_offs, Vector3f &vel_ned)
+// Retrieves the estimated target position, velocity, and acceleration in the NED frame, including configured offsets.
+bool AP_Follow::get_ofs_pos_vel_accel_NED_m(Vector3p &pos_ofs_ned_m, Vector3f &vel_ofs_ned_ms, Vector3f &accel_ofs_ned_mss)
 {
-    Vector3f current_position_NED;
-    if (!AP::ahrs().get_relative_position_NED_origin(current_position_NED)) {
-        clear_dist_and_bearing_to_target();
-        return false;
-    }
-    Vector3p target_position_NED;
-    Vector3f veh_vel;  // NED
-    if (!get_target_position_and_velocity(target_position_NED, veh_vel)) {
-        clear_dist_and_bearing_to_target();
+    if (!update_estimate()) {
         return false;
     }
 
-    // convert from Vector3p to Vector3f as they can't be subtracted:
-    Vector3f target_position_NED_f;
-    target_position_NED_f.x = target_position_NED.x;
-    target_position_NED_f.y = target_position_NED.y;
-    target_position_NED_f.z = target_position_NED.z;
+    pos_ofs_ned_m = _ofs_estimate_pos_ned_m;
+    vel_ofs_ned_ms = _ofs_estimate_vel_ned_ms;
+    accel_ofs_ned_mss = _ofs_estimate_accel_ned_mss;
 
-    const Vector3f dist_vec = target_position_NED_f - current_position_NED;
+    return true;
+}
 
-    // fail if too far
-    if (is_positive(_dist_max.get()) && (dist_vec.length() > _dist_max)) {
-        clear_dist_and_bearing_to_target();
+// Retrieves distance vectors (with and without configured offsets) and the target’s velocity, all in the NED frame.
+bool AP_Follow::get_target_dist_and_vel_NED_m(Vector3f &dist_ned, Vector3f &dist_with_offs, Vector3f &vel_ned)
+{
+    if (!update_estimate()) {
         return false;
     }
 
-    // initialise offsets from distance vector if required
-    init_offsets_if_required(dist_vec);
-
-    // get offsets
-    Vector3f offsets;
-    if (!get_offsets_ned(offsets)) {
+    Vector3f current_position_ned_m;
+    if (!AP::ahrs().get_relative_position_NED_origin(current_position_ned_m)) {
         clear_dist_and_bearing_to_target();
         return false;
     }
 
-    // calculate results
-    dist_ned = dist_vec;
-    dist_with_offs = dist_vec + offsets;
-    vel_ned = veh_vel;
+    const Vector3p dist_vec_ned_m = _estimate_pos_ned_m - current_position_ned_m.topostype();
+    const Vector3p ofs_dist_vec = _ofs_estimate_pos_ned_m - current_position_ned_m.topostype();
+    dist_ned = dist_vec_ned_m.tofloat();
+    dist_with_offs = ofs_dist_vec.tofloat();
+    vel_ned = _ofs_estimate_vel_ned_ms;
 
-    // record distance and heading for reporting purposes
-    if (is_zero(dist_with_offs.x) && is_zero(dist_with_offs.y)) {
-        clear_dist_and_bearing_to_target();
-    } else {
-        _dist_to_target = safe_sqrt(sq(dist_with_offs.x) + sq(dist_with_offs.y));
-        _bearing_to_target = degrees(atan2f(dist_with_offs.y, dist_with_offs.x));
+    return true;
+}
+
+// Retrieves the estimated target heading and heading rate in radians.
+bool AP_Follow::get_heading_heading_rate_rad(float &heading_rad, float &heading_rate_rads)
+{
+    if (!update_estimate()) {
+        return false;
     }
 
+    // return latest heading estimate
+    heading_rad = _estimate_heading_rad;
+    heading_rate_rads = _estimate_heading_rate_rads;
     return true;
 }
 
-// get target's heading in degrees (0 = north, 90 = east)
-bool AP_Follow::get_target_heading_deg(float &heading) const
+// Retrieves the target's estimated global location and velocity, adjusting altitude frame if relative mode is set (for LUA bindings).
+bool AP_Follow::get_target_location_and_velocity(Location &loc, Vector3f &vel_ned)
 {
-    // exit immediately if not enabled
-    if (!_enabled) {
+    if (!update_estimate()) {
         return false;
     }
 
-    // check for timeout
-    if ((_last_heading_update_ms == 0) || (AP_HAL::millis() - _last_heading_update_ms > AP_FOLLOW_TIMEOUT_MS)) {
+    if (!AP::ahrs().get_location_from_origin_offset_NED(loc, _estimate_pos_ned_m)) {
         return false;
     }
+    vel_ned = _estimate_vel_ned_ms;
+
+    if (_alt_type == AP_FOLLOW_ALTITUDE_TYPE_RELATIVE) {
+        loc.change_alt_frame(Location::AltFrame::ABOVE_HOME);
+    }
 
-    // return latest heading estimate
-    heading = _target_heading;
     return true;
 }
 
-// returns true if we should extract information from msg
-bool AP_Follow::should_handle_message(const mavlink_message_t &msg) const
+// Retrieves the target's estimated global location and velocity, including configured offsets, for LUA bindings.
+bool AP_Follow::get_target_location_and_velocity_ofs(Location &loc, Vector3f &vel_ned)
 {
-    // exit immediately if not enabled
-    if (!_enabled) {
+    if (!update_estimate()) {
         return false;
     }
-
-    // skip our own messages
-    if (msg.sysid == mavlink_system.sysid) {
+    if (!AP::ahrs().get_location_from_origin_offset_NED(loc, _ofs_estimate_pos_ned_m)) {
         return false;
     }
+    if (_alt_type == AP_FOLLOW_ALTITUDE_TYPE_RELATIVE) {
+        loc.change_alt_frame(Location::AltFrame::ABOVE_HOME);
+    }
 
-    // skip message if not from our target
-    if (_sysid != 0 && msg.sysid != _sysid) {
+    vel_ned = _ofs_estimate_vel_ned_ms;
+    return true;
+}
+
+// Retrieves the estimated target heading in degrees (0° = North, 90° = East) for LUA bindings.
+bool AP_Follow::get_target_heading_deg(float &heading)
+{
+    if (!update_estimate()) {
         return false;
     }
 
+    // return latest heading estimate
+    heading = degrees(_estimate_heading_rad);
     return true;
 }
 
-// handle mavlink DISTANCE_SENSOR messages
+
+//==============================================================================
+// MAVLink Message Handling
+//==============================================================================
+
+// Handles incoming MAVLink messages to update the target's position, velocity, and heading.
 void AP_Follow::handle_msg(const mavlink_message_t &msg)
 {
-    // this method should be called from an "update()" method:
     if (_automatic_sysid) {
-        // maybe timeout who we were following...
+        // timeout check: if no position update received within timeout, clear automatic sysid
         if ((_last_location_update_ms == 0) ||
             (AP_HAL::millis() - _last_location_update_ms > AP_FOLLOW_SYSID_TIMEOUT_MS)) {
             _sysid.set(0);
+            _sysid_used = 0;
+            _estimate_valid = false;
         }
     }
 
     if (!should_handle_message(msg)) {
+        // ignore message if filtering rules reject it (e.g., wrong sysid)
         return;
     }
 
-    // decode global-position-int message
+    // decode MAVLink message
     bool updated = false;
 
     switch (msg.msgid) {
     case MAVLINK_MSG_ID_GLOBAL_POSITION_INT: {
+        // handle standard global position messages
         updated = handle_global_position_int_message(msg);
         break;
     }
     case MAVLINK_MSG_ID_FOLLOW_TARGET: {
+        // handle follow-target specific messages
         updated = handle_follow_target_message(msg);
         break;
     }
     }
 
     if (updated) {
+        // message provided updated position information
+        Vector3f offset_m = _offset_XXD_m.get();
+
+        // if using relative frame, rotate offsets based on estimated heading
+        if (_offset_type == AP_FOLLOW_OFFSET_TYPE_RELATIVE) {
+            offset_m.xy().rotate(_estimate_heading_rad);
+        }
+
+        Vector3f current_position_ned_m;
+        if (!AP::ahrs().get_relative_position_NED_origin(current_position_ned_m)) {
+            // 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
+            const Vector3p dist_vec_ned_m = _estimate_pos_ned_m - current_position_ned_m.topostype();
+            const Vector3p ofs_dist_vec = _ofs_estimate_pos_ned_m - current_position_ned_m.topostype();
+
+            // initialise offsets dynamically if needed
+            init_offsets_if_required(dist_vec_ned_m.tofloat());
+
+            // record distance and bearing to target for reporting/logging
+            if (ofs_dist_vec.xy().is_zero()) {
+                clear_dist_and_bearing_to_target();
+            } else {
+                _dist_to_target_m = ofs_dist_vec.xy().length();
+                _bearing_to_target_deg = degrees(ofs_dist_vec.xy().angle());
+            }
+
+            // clear distance and bearing if outside configured max follow distance
+            if (is_positive(_dist_max_m.get()) && (dist_vec_ned_m.length() > _dist_max_m)) {
+                clear_dist_and_bearing_to_target();
+            }
+        }
+
+        // Check if estimate needs reset based on position and velocity errors
+        if (estimate_error_too_large()) {
+            _estimate_valid = false;
+        }
+
 #if HAL_LOGGING_ENABLED
+        // log current follow diagnostic data
         Log_Write_FOLL();
 #endif
     }
 }
 
-bool AP_Follow::handle_global_position_int_message(const mavlink_message_t &msg)
+// Returns true if the incoming MAVLink message should be processed for target updates.
+bool AP_Follow::should_handle_message(const mavlink_message_t &msg) const
 {
-        // decode message
-        mavlink_global_position_int_t packet;
-        mavlink_msg_global_position_int_decode(&msg, &packet);
+    // exit immediately if not enabled
+    if (!_enabled) {
+        return false;
+    }
 
-        // ignore message if lat and lon are (exactly) zero
-        if ((packet.lat == 0 && packet.lon == 0)) {
-            return false;
-        }
+    // skip our own messages
+    if (msg.sysid == mavlink_system.sysid) {
+        return false;
+    }
 
-        Location _target_location;
-        _target_location.lat = packet.lat;
-        _target_location.lng = packet.lon;
+    // skip message if not from our target
+    if (_sysid != 0 && msg.sysid != _sysid) {
+        return false;
+    }
 
-        // select altitude source based on FOLL_ALT_TYPE param 
-        if (_alt_type == AP_FOLLOW_ALTITUDE_TYPE_RELATIVE) {
-            // above home alt
-            _target_location.set_alt_cm(packet.relative_alt / 10, Location::AltFrame::ABOVE_HOME);
-        } else {
-            // absolute altitude
-            _target_location.set_alt_cm(packet.alt / 10, Location::AltFrame::ABSOLUTE);
-        }
-        if (!_target_location.get_vector_from_origin_NEU(_target_position_ned)) {
-            return false;
-        }
-        _target_position_ned.z = -_target_position_ned.z; // NEU->NED
-        _target_position_ned *= 0.01;  // cm -> m
-
-        _target_velocity_ned.x = packet.vx * 0.01f; // velocity north
-        _target_velocity_ned.y = packet.vy * 0.01f; // velocity east
-        _target_velocity_ned.z = packet.vz * 0.01f; // velocity down
-
-        // get a local timestamp with correction for transport jitter
-        _last_location_update_ms = _jitter.correct_offboard_timestamp_msec(packet.time_boot_ms, AP_HAL::millis());
-        if (packet.hdg <= 36000) {                  // heading (UINT16_MAX if unknown)
-            _target_heading = packet.hdg * 0.01f;   // convert centi-degrees to degrees
-            _last_heading_update_ms = _last_location_update_ms;
-        }
-        // initialise _sysid if zero to sender's id
-        if (_sysid == 0) {
-            _sysid.set(msg.sysid);
-            _automatic_sysid = true;
-        }
-        return true;
+    return true;
 }
 
-bool AP_Follow::handle_follow_target_message(const mavlink_message_t &msg)
+// Checks whether the current estimate should be reset based on position and velocity errors.
+bool AP_Follow::estimate_error_too_large() const
 {
-        // decode message
-        mavlink_follow_target_t packet;
-        mavlink_msg_follow_target_decode(&msg, &packet);
+    const float timeout_sec = AP_FOLLOW_TIMEOUT_MS * 0.001f;
+    const float half_timeout_sec = timeout_sec * 0.5f;
 
-        // ignore message if lat and lon are (exactly) zero
-        if ((packet.lat == 0 && packet.lon == 0)) {
-            return false;
-        }
-        // require at least position
-        if ((packet.est_capabilities & (1<<0)) == 0) {
-            return false;
-        }
+    // Calculate position thresholds
+    const float pos_thresh_horiz_m = _accel_max_ne_mss.get() * sq(half_timeout_sec);
+    const float pos_thresh_vert_m  = _accel_max_d_mss.get()  * sq(half_timeout_sec);
 
-        const Location new_loc {
-            packet.lat,
-            packet.lon,
-            int32_t(packet.alt * 100),  // m -> cm
-            Location::AltFrame::ABSOLUTE
-        };
-
-        if (!new_loc.get_vector_from_origin_NEU(_target_position_ned)) {
-            return false;
-        }
-        _target_position_ned.z = -_target_position_ned.z; // NEU->NED
-        _target_position_ned *= 0.01;  // cm -> m
+    // Calculate velocity thresholds using the new helper function
+    const float vel_thresh_horiz_ms = calc_max_velocity_change(_accel_max_ne_mss.get(), _jerk_max_ne_msss.get(), timeout_sec);
+    const float vel_thresh_vert_ms  = calc_max_velocity_change(_accel_max_d_mss.get(), _jerk_max_d_msss.get(), timeout_sec);
 
-        if (packet.est_capabilities & (1<<1)) {
-            _target_velocity_ned.x = packet.vel[0]; // velocity north
-            _target_velocity_ned.y = packet.vel[1]; // velocity east
-            _target_velocity_ned.z = packet.vel[2]; // velocity down
-        } else {
-            _target_velocity_ned.zero();
-        }
+    // Calculate current position and velocity errors
+    Vector3f pos_error = _estimate_pos_ned_m.tofloat() - _target_pos_ned_m.tofloat();
+    Vector3f vel_error = _estimate_vel_ned_ms - _target_vel_ned_ms;
 
-        // get a local timestamp with correction for transport jitter
-        _last_location_update_ms = _jitter.correct_offboard_timestamp_msec(packet.timestamp, AP_HAL::millis());
+    Vector2f pos_error_xy = pos_error.xy();
+    float pos_error_z = pos_error.z;
+    Vector2f vel_error_xy = vel_error.xy();
+    float vel_error_z = vel_error.z;
 
-        if (packet.est_capabilities & (1<<3)) {
-            Quaternion q{packet.attitude_q[0], packet.attitude_q[1], packet.attitude_q[2], packet.attitude_q[3]};
-            float r, p, y;
-            q.to_euler(r,p,y);
-            _target_heading = degrees(y);
-            _last_heading_update_ms = _last_location_update_ms;
-        }
+    // Check horizontal and vertical separately
+    const bool pos_horiz_bad = pos_error_xy.length() > pos_thresh_horiz_m;
+    const bool vel_horiz_bad = vel_error_xy.length() > vel_thresh_horiz_ms;
+    const bool pos_vert_bad = fabsf(pos_error_z) > pos_thresh_vert_m;
+    const bool vel_vert_bad = fabsf(vel_error_z) > vel_thresh_vert_ms;
 
-        // initialise _sysid if zero to sender's id
-        if (_sysid == 0) {
-            _sysid.set(msg.sysid);
-            _automatic_sysid = true;
-        }
+    return pos_horiz_bad || vel_horiz_bad || pos_vert_bad || vel_vert_bad;
+}
 
-        return true;
+// Calculates max velocity change under trapezoidal or triangular acceleration profile (jerk-limited).
+float AP_Follow::calc_max_velocity_change(float accel_max, float jerk_max, float timeout_sec) const
+{
+    const float t_jerk = accel_max / jerk_max;
+    const float t_total_jerk = 2.0f * t_jerk;
+
+    if (timeout_sec >= t_total_jerk) {
+        // time to ramp up, constant accel phase, and ramp down
+        const float t_const = timeout_sec - t_total_jerk;
+        const float delta_v_jerk = 0.5f * accel_max * t_jerk;
+        const float delta_v_const = accel_max * t_const;
+        return 2.0f * delta_v_jerk + delta_v_const;
+    } else {
+        // timeout too short: pure triangle profile
+        const float t_half = timeout_sec * 0.5f;
+        return 0.5f * jerk_max * sq(t_half);
+    }
 }
 
-// write out an onboard-log message to help diagnose follow problems:
-#if HAL_LOGGING_ENABLED
-void AP_Follow::Log_Write_FOLL()
+// Decodes a GLOBAL_POSITION_INT MAVLink message to update the target’s position, velocity, and heading.
+bool AP_Follow::handle_global_position_int_message(const mavlink_message_t &msg)
 {
-        // get estimated location and velocity
-        Location loc_estimate{};
-        Vector3f vel_estimate;
-        UNUSED_RESULT(get_target_location_and_velocity(loc_estimate, vel_estimate));
-
-        Location _target_location;
-        UNUSED_RESULT(AP::ahrs().get_location_from_origin_offset_NED(_target_location, _target_position_ned * 0.01));
-        if (_alt_type == AP_FOLLOW_ALTITUDE_TYPE_RELATIVE) {
-            _target_location.change_alt_frame(Location::AltFrame::ABOVE_HOME);
-        }
+    // decode GLOBAL_POSITION_INT message into packet struct
+    mavlink_global_position_int_t packet;
+    mavlink_msg_global_position_int_decode(&msg, &packet);
+
+    // ignore message if latitude and longitude are exactly zero (invalid GPS fix)
+    if ((packet.lat == 0 && packet.lon == 0)) {
+        return false;
+    }
+
+    Location _target_location;
+    _target_location.lat = packet.lat;
+    _target_location.lng = packet.lon;
+
+    // set target altitude based on configured altitude type
+    if (_alt_type == AP_FOLLOW_ALTITUDE_TYPE_RELATIVE) {
+        // use relative altitude above home
+        _target_location.set_alt_cm(packet.relative_alt / 10, Location::AltFrame::ABOVE_HOME);
+    } else {
+        // use absolute altitude
+        _target_location.set_alt_cm(packet.alt / 10, Location::AltFrame::ABSOLUTE);
+    }
+
+    // convert global location to local NED frame position
+    if (!_target_location.get_vector_from_origin_NEU(_target_pos_ned_m)) {
+        return false;
+    }
+    _target_pos_ned_m.z = -_target_pos_ned_m.z; // convert NEU -> NED
+    _target_pos_ned_m *= 0.01;  // convert from cm to meters
+
+    // decode target velocity components (in m/s)
+    _target_vel_ned_ms.x = packet.vx * 0.01f; // velocity north
+    _target_vel_ned_ms.y = packet.vy * 0.01f; // velocity east
+    _target_vel_ned_ms.z = packet.vz * 0.01f; // velocity down
+
+    // target acceleration not available in GLOBAL_POSITION_INT
+    _target_accel_ned_mss.zero();
+
+    if (packet.hdg <= 36000) {
+        // valid heading field available (in centi-degrees)
+        _target_heading_deg = packet.hdg * 0.01f;
+    } else if (_offset_type == AP_FOLLOW_OFFSET_TYPE_RELATIVE) {
+        // heading missing but relative offset mode requires heading -> reject
+        return false;
+    } else {
+        // no heading available: set heading rate to zero
+        _target_heading_rate_degs = 0.0f;
+    }
+
+    // apply jitter-corrected timestamp to this update
+    _last_location_update_ms = _jitter.correct_offboard_timestamp_msec(packet.time_boot_ms, AP_HAL::millis());
 
-        // log lead's estimated vs reported position
-// @LoggerMessage: FOLL
-// @Description: Follow library diagnostic data
-// @Field: TimeUS: Time since system startup
-// @Field: Lat: Target latitude
-// @Field: Lon: Target longitude
-// @Field: Alt: Target absolute altitude
-// @Field: VelN: Target earth-frame velocity, North
-// @Field: VelE: Target earth-frame velocity, East
-// @Field: VelD: Target earth-frame velocity, Down
-// @Field: LatE: Vehicle latitude
-// @Field: LonE: Vehicle longitude
-// @Field: AltE: Vehicle absolute altitude
-        AP::logger().WriteStreaming("FOLL",
-                                               "TimeUS,Lat,Lon,Alt,VelN,VelE,VelD,LatE,LonE,AltE",  // labels
-                                               "sDUmnnnDUm",    // units
-                                               "F--B000--B",    // mults
-                                               "QLLifffLLi",    // fmt
-                                               AP_HAL::micros64(),
-                                               _target_location.lat,
-                                               _target_location.lng,
-                                               _target_location.alt,
-                                               (double)_target_velocity_ned.x,
-                                               (double)_target_velocity_ned.y,
-                                               (double)_target_velocity_ned.z,
-                                               loc_estimate.lat,
-                                               loc_estimate.lng,
-                                               loc_estimate.alt
-                                               );
+    // if sysid not yet set, adopt sender’s sysid and enable automatic sysid tracking
+    if (_sysid == 0) {
+        _sysid.set(msg.sysid);
+        _sysid_used = 0;
+        _estimate_valid = false;
+        _automatic_sysid = true;
+    }
+
+    return true;
 }
-#endif  // HAL_LOGGING_ENABLED
 
-// get velocity estimate in m/s in NED frame using dt since last update
-bool AP_Follow::get_velocity_ned(Vector3f &vel_ned, float dt) const
+// Decodes a FOLLOW_TARGET MAVLink message to update the target’s position, velocity, acceleration, and orientation.
+bool AP_Follow::handle_follow_target_message(const mavlink_message_t &msg)
 {
-    vel_ned = _target_velocity_ned + (_target_accel_ned * dt);
+    // decode FOLLOW_TARGET message into packet struct
+    mavlink_follow_target_t packet;
+    mavlink_msg_follow_target_decode(&msg, &packet);
+
+    // ignore message if latitude and longitude are exactly zero (invalid GPS fix)
+    if ((packet.lat == 0 && packet.lon == 0)) {
+        return false;
+    }
+
+    // require that at least position is estimated (bit 0 of est_capabilities)
+    if ((packet.est_capabilities & (1<<0)) == 0) {
+        return false;
+    }
+
+    // build Location object from latitude, longitude, and altitude (alt in meters)
+    const Location _target_location {
+        packet.lat,
+        packet.lon,
+        int32_t(packet.alt * 100),  // convert meters to centimeters
+        Location::AltFrame::ABSOLUTE
+    };
+
+    // convert global location to local NED frame position
+    if (!_target_location.get_vector_from_origin_NEU(_target_pos_ned_m)) {
+        return false;
+    }
+    _target_pos_ned_m *= 0.01; // convert from cm to meters
+
+    // adjust Z coordinate to NED frame (NEU altitude -> NED)
+    Location origin;
+    if (!AP::ahrs().get_origin(origin)) {
+        return false;
+    }
+    _target_pos_ned_m.z = -packet.alt + origin.alt * 0.01;
+
+    // decode velocity if available (bit 1 of est_capabilities)
+    if (packet.est_capabilities & (1<<1)) {
+        _target_vel_ned_ms.x = packet.vel[0]; // velocity north
+        _target_vel_ned_ms.y = packet.vel[1]; // velocity east
+        _target_vel_ned_ms.z = packet.vel[2]; // velocity down
+    } else {
+        _target_vel_ned_ms.zero();
path: libraries/AP_Follow/AP_Follow.cpp
user: login: lthall
body: Sorry @timtuxworth but I must be missunderstanding what you are saying. We have to set these to zero if they are not provided by the lead aircraft.

This PR currently doesn't let you turn off the consumption of GLOBAL_POSITION_INT when you provide FOLLOW_TARGET so using both will cause problems if one provides more information than the other. This is something I need to take to the Dev Call to get some guidance on how to implement this. Maybe I should do it the way I think it should be done so at least this PR is fully functional.
created_at: 2025-05-02T00:38:36Z
author_association: CONTRIBUTOR
diff_hunk: @@ -134,419 +155,638 @@ const AP_Param::GroupInfo AP_Follow::var_info[] = {
     AP_GROUPINFO("_ALT_TYPE", 10, AP_Follow, _alt_type, AP_FOLLOW_ALT_TYPE_DEFAULT),
 #endif
 
+// May add option to turn on input shaping, turn on acceleration due to rotation of offsest
+
     // @Param: _OPTIONS
     // @DisplayName: Follow options
     // @Description: Follow options bitmask
     // @Values: 0:None,1: Mount Follows lead vehicle on mode enter
     // @User: Standard
     AP_GROUPINFO("_OPTIONS", 11, AP_Follow, _options, 0),
 
+    // @Param: _ACCEL_NE
+    // @DisplayName: Acceleration limit for the horizontal kinematic input shaping
+    // @Description: Acceleration limit of the horizontal kinematic path generation used to determine how quickly the ship varies in velocity
+    // @Range: 0 5
+    // @Units: m/s/s
+    // @User: Advanced
+    AP_GROUPINFO("_ACCEL_NE", 12, AP_Follow, _accel_max_ne_mss, 2.5),
+
+    // @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 ship 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),
+
+    // @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 ship 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),
+
+    // @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 ship 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),
+
+    // @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 ship 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),
+
+    // @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 ship 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_GROUPEND
 };
 
-/* 
-   The constructor also initialises the proximity sensor. Note that this
-   constructor is not called until detect() returns true, so we
-   already know that we should setup the proximity sensor
-*/
+// Constructor for AP_Follow. Initializes the position P-controller and sets up parameter defaults.
 AP_Follow::AP_Follow() :
-        _p_pos(AP_FOLLOW_POS_P_DEFAULT)
+    _p_pos(AP_FOLLOW_POS_P_DEFAULT)
 {
     _singleton = this;
     AP_Param::setup_object_defaults(this, var_info);
 }
 
-// restore offsets to zero if necessary, should be called when vehicle exits follow mode
-void AP_Follow::clear_offsets_if_required()
+
+//==============================================================================
+// Target Estimation Update Functions
+//==============================================================================
+
+// Projects and updates the estimated target position, velocity, and heading based on last known data and configured input shaping.
+bool AP_Follow::update_estimate()
 {
-    if (_offsets_were_zero) {
-        _offset.set(Vector3f());
-        _offsets_were_zero = false;
+    // exit immediately if not enabled
+    if (!_enabled) {
+        _estimate_valid = false;
+        return _estimate_valid;
     }
-}
 
-// get target's estimated location
-bool AP_Follow::get_target_location_and_velocity(Location &loc, Vector3f &vel_ned) const
-{
-    Vector3p pos_ned;
-    Vector3f local_vel_ned;  // so we either change both return parameters or neither
-    if (!get_target_position_and_velocity(pos_ned, local_vel_ned)) {
-        return false;
+    // check for timeout: if no recent location update, invalidate estimate
+    if ((_last_location_update_ms == 0) || (AP_HAL::millis() - _last_location_update_ms > AP_FOLLOW_TIMEOUT_MS)) {
+        _estimate_valid = false;
+        return _estimate_valid;
     }
-    if (!AP::ahrs().get_location_from_origin_offset_NED(loc, pos_ned)) {
-        return false;
+
+    // skip if update has already been processed this scheduler tick
+    if (_estimate_valid && (AP::scheduler().ticks32() - _last_update_ticks == 0)) {
+        return _estimate_valid;
     }
-    vel_ned = local_vel_ned;
+    _last_update_ticks = AP::scheduler().ticks32();
 
-    if (_alt_type == AP_FOLLOW_ALTITUDE_TYPE_RELATIVE) {
-        loc.change_alt_frame(Location::AltFrame::ABOVE_HOME);
+    // if sysid changed, reset the estimation state
+    if (_sysid != _sysid_used) {
+        _sysid_used = _sysid;
+        _estimate_valid = false;
     }
 
-    return true;
-}
+    const uint32_t now = AP_HAL::millis();
 
-// get target's estimated location and velocity, both NED SI from origin
-bool AP_Follow::get_target_position_and_velocity(Vector3p &pos_ned, Vector3f &vel_ned) const
-{
-    // exit immediately if not enabled
-    if (!_enabled) {
-        return false;
+    // calculate time since last location update in seconds
+    const float dt = (now - _last_location_update_ms) * 0.001f;
+
+    // project target's position and velocity forward using simple kinematics
+    Vector3f delta_pos = _target_vel_ned_ms * dt + _target_accel_ned_mss * 0.5f * sq(dt);
+    Vector3f delta_vel = _target_accel_ned_mss * dt;
+
+    // calculate time since last estimation update
+    const float e_dt = (now - _last_estimation_update_ms) * 0.001f;
+    
+    if (_estimate_valid) {
+        // 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)
+        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.xy().topostype(), _target_vel_ned_ms.xy() + delta_vel.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 angular shaping for heading estimate
+        shape_angle_vel_accel(radians(_target_heading_deg), radians(_target_heading_rate_degs), 0.0,
+                              _estimate_heading_rad, _estimate_heading_rate_rads, _estimate_heading_accel_radss,
+                              0.0, radians(_accel_max_h_degss),
+                              radians(_jerk_max_h_degsss), e_dt, false);
+
+        // update heading angle separately to maintain proper wrapping [-PI, PI]
+        postype_t estimate_heading_rad = _estimate_heading_rad;
+        update_pos_vel_accel(estimate_heading_rad, _estimate_heading_rate_rads, _estimate_heading_accel_radss, e_dt, 0.0, 0.0, 0.0);
+        _estimate_heading_rad = wrap_PI(estimate_heading_rad);
+    } else {
+        // no valid estimate yet: initialise from latest target position
+        _estimate_pos_ned_m = _target_pos_ned_m + delta_pos.topostype();
+        _estimate_vel_ned_ms = _target_vel_ned_ms + delta_vel;
+        _estimate_accel_ned_mss = _target_accel_ned_mss;
+        _estimate_valid = true;
     }
 
-    // check for timeout
-    if ((_last_location_update_ms == 0) || (AP_HAL::millis() - _last_location_update_ms > AP_FOLLOW_TIMEOUT_MS)) {
-        return false;
+    Vector3f offset_m = _offset_XXD_m.get();
+
+    // calculate estimated position and velocity with offsets applied
+    if (offset_m.is_zero() || (_offset_type == AP_FOLLOW_OFFSET_TYPE_NED)) {
+        // offsets are in NED frame: simple addition
+        _ofs_estimate_pos_ned_m = _estimate_pos_ned_m + offset_m.topostype();
+        _ofs_estimate_vel_ned_ms = _estimate_vel_ned_ms;
+        _ofs_estimate_accel_ned_mss = _estimate_accel_ned_mss;
+    } else {
+        // offsets are in FRD frame: rotate by heading
+        offset_m.xy().rotate(_estimate_heading_rad);
+        Vector3f offset_cross = offset_m.cross(Vector3f{0.0, 0.0, 1.0});
+        float offset_length_m = offset_m.length();
+        _ofs_estimate_pos_ned_m = _estimate_pos_ned_m + offset_m.topostype();
+        _ofs_estimate_vel_ned_ms = _estimate_vel_ned_ms + offset_cross * offset_length_m * _estimate_heading_rate_rads;
+        _ofs_estimate_accel_ned_mss = _estimate_accel_ned_mss + offset_cross * offset_length_m * _estimate_heading_accel_radss;
     }
 
-    // calculate time since last actual position update
-    const float dt = (AP_HAL::millis() - _last_location_update_ms) * 0.001f;
+    _last_estimation_update_ms = now;
+    return _estimate_valid;
+}
+
 
-    // get velocity estimate
-    if (!get_velocity_ned(vel_ned, dt)) {
+//==============================================================================
+// Target Information Retrieval Functions
+//==============================================================================
+
+// Retrieves the estimated target position, velocity, and acceleration in the NED frame (relative to origin).
+bool AP_Follow::get_target_pos_vel_accel_NED_m(Vector3p &pos_ned_m, Vector3f &vel_ned_ms, Vector3f &accel_ned_mss)
+{
+    if (!update_estimate()) {
         return false;
     }
 
-    // project the vehicle position
-    const Vector3p vel_ned_p { vel_ned.x, vel_ned.y, vel_ned.z };
-    pos_ned = _target_position_ned + vel_ned_p * dt;
+    pos_ned_m = _estimate_pos_ned_m;
+    vel_ned_ms = _estimate_vel_ned_ms;
+    accel_ned_mss = _estimate_accel_ned_mss;
 
     return true;
 }
 
-// get distance vector to target (in meters) and target's velocity all in NED frame
-bool AP_Follow::get_target_dist_and_vel_ned(Vector3f &dist_ned, Vector3f &dist_with_offs, Vector3f &vel_ned)
+// Retrieves the estimated target position, velocity, and acceleration in the NED frame, including configured offsets.
+bool AP_Follow::get_ofs_pos_vel_accel_NED_m(Vector3p &pos_ofs_ned_m, Vector3f &vel_ofs_ned_ms, Vector3f &accel_ofs_ned_mss)
 {
-    Vector3f current_position_NED;
-    if (!AP::ahrs().get_relative_position_NED_origin(current_position_NED)) {
-        clear_dist_and_bearing_to_target();
-        return false;
-    }
-    Vector3p target_position_NED;
-    Vector3f veh_vel;  // NED
-    if (!get_target_position_and_velocity(target_position_NED, veh_vel)) {
-        clear_dist_and_bearing_to_target();
+    if (!update_estimate()) {
         return false;
     }
 
-    // convert from Vector3p to Vector3f as they can't be subtracted:
-    Vector3f target_position_NED_f;
-    target_position_NED_f.x = target_position_NED.x;
-    target_position_NED_f.y = target_position_NED.y;
-    target_position_NED_f.z = target_position_NED.z;
+    pos_ofs_ned_m = _ofs_estimate_pos_ned_m;
+    vel_ofs_ned_ms = _ofs_estimate_vel_ned_ms;
+    accel_ofs_ned_mss = _ofs_estimate_accel_ned_mss;
 
-    const Vector3f dist_vec = target_position_NED_f - current_position_NED;
+    return true;
+}
 
-    // fail if too far
-    if (is_positive(_dist_max.get()) && (dist_vec.length() > _dist_max)) {
-        clear_dist_and_bearing_to_target();
+// Retrieves distance vectors (with and without configured offsets) and the target’s velocity, all in the NED frame.
+bool AP_Follow::get_target_dist_and_vel_NED_m(Vector3f &dist_ned, Vector3f &dist_with_offs, Vector3f &vel_ned)
+{
+    if (!update_estimate()) {
         return false;
     }
 
-    // initialise offsets from distance vector if required
-    init_offsets_if_required(dist_vec);
-
-    // get offsets
-    Vector3f offsets;
-    if (!get_offsets_ned(offsets)) {
+    Vector3f current_position_ned_m;
+    if (!AP::ahrs().get_relative_position_NED_origin(current_position_ned_m)) {
         clear_dist_and_bearing_to_target();
         return false;
     }
 
-    // calculate results
-    dist_ned = dist_vec;
-    dist_with_offs = dist_vec + offsets;
-    vel_ned = veh_vel;
+    const Vector3p dist_vec_ned_m = _estimate_pos_ned_m - current_position_ned_m.topostype();
+    const Vector3p ofs_dist_vec = _ofs_estimate_pos_ned_m - current_position_ned_m.topostype();
+    dist_ned = dist_vec_ned_m.tofloat();
+    dist_with_offs = ofs_dist_vec.tofloat();
+    vel_ned = _ofs_estimate_vel_ned_ms;
 
-    // record distance and heading for reporting purposes
-    if (is_zero(dist_with_offs.x) && is_zero(dist_with_offs.y)) {
-        clear_dist_and_bearing_to_target();
-    } else {
-        _dist_to_target = safe_sqrt(sq(dist_with_offs.x) + sq(dist_with_offs.y));
-        _bearing_to_target = degrees(atan2f(dist_with_offs.y, dist_with_offs.x));
+    return true;
+}
+
+// Retrieves the estimated target heading and heading rate in radians.
+bool AP_Follow::get_heading_heading_rate_rad(float &heading_rad, float &heading_rate_rads)
+{
+    if (!update_estimate()) {
+        return false;
     }
 
+    // return latest heading estimate
+    heading_rad = _estimate_heading_rad;
+    heading_rate_rads = _estimate_heading_rate_rads;
     return true;
 }
 
-// get target's heading in degrees (0 = north, 90 = east)
-bool AP_Follow::get_target_heading_deg(float &heading) const
+// Retrieves the target's estimated global location and velocity, adjusting altitude frame if relative mode is set (for LUA bindings).
+bool AP_Follow::get_target_location_and_velocity(Location &loc, Vector3f &vel_ned)
 {
-    // exit immediately if not enabled
-    if (!_enabled) {
+    if (!update_estimate()) {
         return false;
     }
 
-    // check for timeout
-    if ((_last_heading_update_ms == 0) || (AP_HAL::millis() - _last_heading_update_ms > AP_FOLLOW_TIMEOUT_MS)) {
+    if (!AP::ahrs().get_location_from_origin_offset_NED(loc, _estimate_pos_ned_m)) {
         return false;
     }
+    vel_ned = _estimate_vel_ned_ms;
+
+    if (_alt_type == AP_FOLLOW_ALTITUDE_TYPE_RELATIVE) {
+        loc.change_alt_frame(Location::AltFrame::ABOVE_HOME);
+    }
 
-    // return latest heading estimate
-    heading = _target_heading;
     return true;
 }
 
-// returns true if we should extract information from msg
-bool AP_Follow::should_handle_message(const mavlink_message_t &msg) const
+// Retrieves the target's estimated global location and velocity, including configured offsets, for LUA bindings.
+bool AP_Follow::get_target_location_and_velocity_ofs(Location &loc, Vector3f &vel_ned)
 {
-    // exit immediately if not enabled
-    if (!_enabled) {
+    if (!update_estimate()) {
         return false;
     }
-
-    // skip our own messages
-    if (msg.sysid == mavlink_system.sysid) {
+    if (!AP::ahrs().get_location_from_origin_offset_NED(loc, _ofs_estimate_pos_ned_m)) {
         return false;
     }
+    if (_alt_type == AP_FOLLOW_ALTITUDE_TYPE_RELATIVE) {
+        loc.change_alt_frame(Location::AltFrame::ABOVE_HOME);
+    }
 
-    // skip message if not from our target
-    if (_sysid != 0 && msg.sysid != _sysid) {
+    vel_ned = _ofs_estimate_vel_ned_ms;
+    return true;
+}
+
+// Retrieves the estimated target heading in degrees (0° = North, 90° = East) for LUA bindings.
+bool AP_Follow::get_target_heading_deg(float &heading)
+{
+    if (!update_estimate()) {
         return false;
     }
 
+    // return latest heading estimate
+    heading = degrees(_estimate_heading_rad);
     return true;
 }
 
-// handle mavlink DISTANCE_SENSOR messages
+
+//==============================================================================
+// MAVLink Message Handling
+//==============================================================================
+
+// Handles incoming MAVLink messages to update the target's position, velocity, and heading.
 void AP_Follow::handle_msg(const mavlink_message_t &msg)
 {
-    // this method should be called from an "update()" method:
     if (_automatic_sysid) {
-        // maybe timeout who we were following...
+        // timeout check: if no position update received within timeout, clear automatic sysid
         if ((_last_location_update_ms == 0) ||
             (AP_HAL::millis() - _last_location_update_ms > AP_FOLLOW_SYSID_TIMEOUT_MS)) {
             _sysid.set(0);
+            _sysid_used = 0;
+            _estimate_valid = false;
         }
     }
 
     if (!should_handle_message(msg)) {
+        // ignore message if filtering rules reject it (e.g., wrong sysid)
         return;
     }
 
-    // decode global-position-int message
+    // decode MAVLink message
     bool updated = false;
 
     switch (msg.msgid) {
     case MAVLINK_MSG_ID_GLOBAL_POSITION_INT: {
+        // handle standard global position messages
         updated = handle_global_position_int_message(msg);
         break;
     }
     case MAVLINK_MSG_ID_FOLLOW_TARGET: {
+        // handle follow-target specific messages
         updated = handle_follow_target_message(msg);
         break;
     }
     }
 
     if (updated) {
+        // message provided updated position information
+        Vector3f offset_m = _offset_XXD_m.get();
+
+        // if using relative frame, rotate offsets based on estimated heading
+        if (_offset_type == AP_FOLLOW_OFFSET_TYPE_RELATIVE) {
+            offset_m.xy().rotate(_estimate_heading_rad);
+        }
+
+        Vector3f current_position_ned_m;
+        if (!AP::ahrs().get_relative_position_NED_origin(current_position_ned_m)) {
+            // 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
+            const Vector3p dist_vec_ned_m = _estimate_pos_ned_m - current_position_ned_m.topostype();
+            const Vector3p ofs_dist_vec = _ofs_estimate_pos_ned_m - current_position_ned_m.topostype();
+
+            // initialise offsets dynamically if needed
+            init_offsets_if_required(dist_vec_ned_m.tofloat());
+
+            // record distance and bearing to target for reporting/logging
+            if (ofs_dist_vec.xy().is_zero()) {
+                clear_dist_and_bearing_to_target();
+            } else {
+                _dist_to_target_m = ofs_dist_vec.xy().length();
+                _bearing_to_target_deg = degrees(ofs_dist_vec.xy().angle());
+            }
+
+            // clear distance and bearing if outside configured max follow distance
+            if (is_positive(_dist_max_m.get()) && (dist_vec_ned_m.length() > _dist_max_m)) {
+                clear_dist_and_bearing_to_target();
+            }
+        }
+
+        // Check if estimate needs reset based on position and velocity errors
+        if (estimate_error_too_large()) {
+            _estimate_valid = false;
+        }
+
 #if HAL_LOGGING_ENABLED
+        // log current follow diagnostic data
         Log_Write_FOLL();
 #endif
     }
 }
 
-bool AP_Follow::handle_global_position_int_message(const mavlink_message_t &msg)
+// Returns true if the incoming MAVLink message should be processed for target updates.
+bool AP_Follow::should_handle_message(const mavlink_message_t &msg) const
 {
-        // decode message
-        mavlink_global_position_int_t packet;
-        mavlink_msg_global_position_int_decode(&msg, &packet);
+    // exit immediately if not enabled
+    if (!_enabled) {
+        return false;
+    }
 
-        // ignore message if lat and lon are (exactly) zero
-        if ((packet.lat == 0 && packet.lon == 0)) {
-            return false;
-        }
+    // skip our own messages
+    if (msg.sysid == mavlink_system.sysid) {
+        return false;
+    }
 
-        Location _target_location;
-        _target_location.lat = packet.lat;
-        _target_location.lng = packet.lon;
+    // skip message if not from our target
+    if (_sysid != 0 && msg.sysid != _sysid) {
+        return false;
+    }
 
-        // select altitude source based on FOLL_ALT_TYPE param 
-        if (_alt_type == AP_FOLLOW_ALTITUDE_TYPE_RELATIVE) {
-            // above home alt
-            _target_location.set_alt_cm(packet.relative_alt / 10, Location::AltFrame::ABOVE_HOME);
-        } else {
-            // absolute altitude
-            _target_location.set_alt_cm(packet.alt / 10, Location::AltFrame::ABSOLUTE);
-        }
-        if (!_target_location.get_vector_from_origin_NEU(_target_position_ned)) {
-            return false;
-        }
-        _target_position_ned.z = -_target_position_ned.z; // NEU->NED
-        _target_position_ned *= 0.01;  // cm -> m
-
-        _target_velocity_ned.x = packet.vx * 0.01f; // velocity north
-        _target_velocity_ned.y = packet.vy * 0.01f; // velocity east
-        _target_velocity_ned.z = packet.vz * 0.01f; // velocity down
-
-        // get a local timestamp with correction for transport jitter
-        _last_location_update_ms = _jitter.correct_offboard_timestamp_msec(packet.time_boot_ms, AP_HAL::millis());
-        if (packet.hdg <= 36000) {                  // heading (UINT16_MAX if unknown)
-            _target_heading = packet.hdg * 0.01f;   // convert centi-degrees to degrees
-            _last_heading_update_ms = _last_location_update_ms;
-        }
-        // initialise _sysid if zero to sender's id
-        if (_sysid == 0) {
-            _sysid.set(msg.sysid);
-            _automatic_sysid = true;
-        }
-        return true;
+    return true;
 }
 
-bool AP_Follow::handle_follow_target_message(const mavlink_message_t &msg)
+// Checks whether the current estimate should be reset based on position and velocity errors.
+bool AP_Follow::estimate_error_too_large() const
 {
-        // decode message
-        mavlink_follow_target_t packet;
-        mavlink_msg_follow_target_decode(&msg, &packet);
+    const float timeout_sec = AP_FOLLOW_TIMEOUT_MS * 0.001f;
+    const float half_timeout_sec = timeout_sec * 0.5f;
 
-        // ignore message if lat and lon are (exactly) zero
-        if ((packet.lat == 0 && packet.lon == 0)) {
-            return false;
-        }
-        // require at least position
-        if ((packet.est_capabilities & (1<<0)) == 0) {
-            return false;
-        }
+    // Calculate position thresholds
+    const float pos_thresh_horiz_m = _accel_max_ne_mss.get() * sq(half_timeout_sec);
+    const float pos_thresh_vert_m  = _accel_max_d_mss.get()  * sq(half_timeout_sec);
 
-        const Location new_loc {
-            packet.lat,
-            packet.lon,
-            int32_t(packet.alt * 100),  // m -> cm
-            Location::AltFrame::ABSOLUTE
-        };
-
-        if (!new_loc.get_vector_from_origin_NEU(_target_position_ned)) {
-            return false;
-        }
-        _target_position_ned.z = -_target_position_ned.z; // NEU->NED
-        _target_position_ned *= 0.01;  // cm -> m
+    // Calculate velocity thresholds using the new helper function
+    const float vel_thresh_horiz_ms = calc_max_velocity_change(_accel_max_ne_mss.get(), _jerk_max_ne_msss.get(), timeout_sec);
+    const float vel_thresh_vert_ms  = calc_max_velocity_change(_accel_max_d_mss.get(), _jerk_max_d_msss.get(), timeout_sec);
 
-        if (packet.est_capabilities & (1<<1)) {
-            _target_velocity_ned.x = packet.vel[0]; // velocity north
-            _target_velocity_ned.y = packet.vel[1]; // velocity east
-            _target_velocity_ned.z = packet.vel[2]; // velocity down
-        } else {
-            _target_velocity_ned.zero();
-        }
+    // Calculate current position and velocity errors
+    Vector3f pos_error = _estimate_pos_ned_m.tofloat() - _target_pos_ned_m.tofloat();
+    Vector3f vel_error = _estimate_vel_ned_ms - _target_vel_ned_ms;
 
-        // get a local timestamp with correction for transport jitter
-        _last_location_update_ms = _jitter.correct_offboard_timestamp_msec(packet.timestamp, AP_HAL::millis());
+    Vector2f pos_error_xy = pos_error.xy();
+    float pos_error_z = pos_error.z;
+    Vector2f vel_error_xy = vel_error.xy();
+    float vel_error_z = vel_error.z;
 
-        if (packet.est_capabilities & (1<<3)) {
-            Quaternion q{packet.attitude_q[0], packet.attitude_q[1], packet.attitude_q[2], packet.attitude_q[3]};
-            float r, p, y;
-            q.to_euler(r,p,y);
-            _target_heading = degrees(y);
-            _last_heading_update_ms = _last_location_update_ms;
-        }
+    // Check horizontal and vertical separately
+    const bool pos_horiz_bad = pos_error_xy.length() > pos_thresh_horiz_m;
+    const bool vel_horiz_bad = vel_error_xy.length() > vel_thresh_horiz_ms;
+    const bool pos_vert_bad = fabsf(pos_error_z) > pos_thresh_vert_m;
+    const bool vel_vert_bad = fabsf(vel_error_z) > vel_thresh_vert_ms;
 
-        // initialise _sysid if zero to sender's id
-        if (_sysid == 0) {
-            _sysid.set(msg.sysid);
-            _automatic_sysid = true;
-        }
+    return pos_horiz_bad || vel_horiz_bad || pos_vert_bad || vel_vert_bad;
+}
 
-        return true;
+// Calculates max velocity change under trapezoidal or triangular acceleration profile (jerk-limited).
+float AP_Follow::calc_max_velocity_change(float accel_max, float jerk_max, float timeout_sec) const
+{
+    const float t_jerk = accel_max / jerk_max;
+    const float t_total_jerk = 2.0f * t_jerk;
+
+    if (timeout_sec >= t_total_jerk) {
+        // time to ramp up, constant accel phase, and ramp down
+        const float t_const = timeout_sec - t_total_jerk;
+        const float delta_v_jerk = 0.5f * accel_max * t_jerk;
+        const float delta_v_const = accel_max * t_const;
+        return 2.0f * delta_v_jerk + delta_v_const;
+    } else {
+        // timeout too short: pure triangle profile
+        const float t_half = timeout_sec * 0.5f;
+        return 0.5f * jerk_max * sq(t_half);
+    }
 }
 
-// write out an onboard-log message to help diagnose follow problems:
-#if HAL_LOGGING_ENABLED
-void AP_Follow::Log_Write_FOLL()
+// Decodes a GLOBAL_POSITION_INT MAVLink message to update the target’s position, velocity, and heading.
+bool AP_Follow::handle_global_position_int_message(const mavlink_message_t &msg)
 {
-        // get estimated location and velocity
-        Location loc_estimate{};
-        Vector3f vel_estimate;
-        UNUSED_RESULT(get_target_location_and_velocity(loc_estimate, vel_estimate));
-
-        Location _target_location;
-        UNUSED_RESULT(AP::ahrs().get_location_from_origin_offset_NED(_target_location, _target_position_ned * 0.01));
-        if (_alt_type == AP_FOLLOW_ALTITUDE_TYPE_RELATIVE) {
-            _target_location.change_alt_frame(Location::AltFrame::ABOVE_HOME);
-        }
+    // decode GLOBAL_POSITION_INT message into packet struct
+    mavlink_global_position_int_t packet;
+    mavlink_msg_global_position_int_decode(&msg, &packet);
+
+    // ignore message if latitude and longitude are exactly zero (invalid GPS fix)
+    if ((packet.lat == 0 && packet.lon == 0)) {
+        return false;
+    }
+
+    Location _target_location;
+    _target_location.lat = packet.lat;
+    _target_location.lng = packet.lon;
+
+    // set target altitude based on configured altitude type
+    if (_alt_type == AP_FOLLOW_ALTITUDE_TYPE_RELATIVE) {
+        // use relative altitude above home
+        _target_location.set_alt_cm(packet.relative_alt / 10, Location::AltFrame::ABOVE_HOME);
+    } else {
+        // use absolute altitude
+        _target_location.set_alt_cm(packet.alt / 10, Location::AltFrame::ABSOLUTE);
+    }
+
+    // convert global location to local NED frame position
+    if (!_target_location.get_vector_from_origin_NEU(_target_pos_ned_m)) {
+        return false;
+    }
+    _target_pos_ned_m.z = -_target_pos_ned_m.z; // convert NEU -> NED
+    _target_pos_ned_m *= 0.01;  // convert from cm to meters
+
+    // decode target velocity components (in m/s)
+    _target_vel_ned_ms.x = packet.vx * 0.01f; // velocity north
+    _target_vel_ned_ms.y = packet.vy * 0.01f; // velocity east
+    _target_vel_ned_ms.z = packet.vz * 0.01f; // velocity down
+
+    // target acceleration not available in GLOBAL_POSITION_INT
+    _target_accel_ned_mss.zero();
+
+    if (packet.hdg <= 36000) {
+        // valid heading field available (in centi-degrees)
+        _target_heading_deg = packet.hdg * 0.01f;
+    } else if (_offset_type == AP_FOLLOW_OFFSET_TYPE_RELATIVE) {
+        // heading missing but relative offset mode requires heading -> reject
+        return false;
+    } else {
+        // no heading available: set heading rate to zero
+        _target_heading_rate_degs = 0.0f;
+    }
+
+    // apply jitter-corrected timestamp to this update
+    _last_location_update_ms = _jitter.correct_offboard_timestamp_msec(packet.time_boot_ms, AP_HAL::millis());
 
-        // log lead's estimated vs reported position
-// @LoggerMessage: FOLL
-// @Description: Follow library diagnostic data
-// @Field: TimeUS: Time since system startup
-// @Field: Lat: Target latitude
-// @Field: Lon: Target longitude
-// @Field: Alt: Target absolute altitude
-// @Field: VelN: Target earth-frame velocity, North
-// @Field: VelE: Target earth-frame velocity, East
-// @Field: VelD: Target earth-frame velocity, Down
-// @Field: LatE: Vehicle latitude
-// @Field: LonE: Vehicle longitude
-// @Field: AltE: Vehicle absolute altitude
-        AP::logger().WriteStreaming("FOLL",
-                                               "TimeUS,Lat,Lon,Alt,VelN,VelE,VelD,LatE,LonE,AltE",  // labels
-                                               "sDUmnnnDUm",    // units
-                                               "F--B000--B",    // mults
-                                               "QLLifffLLi",    // fmt
-                                               AP_HAL::micros64(),
-                                               _target_location.lat,
-                                               _target_location.lng,
-                                               _target_location.alt,
-                                               (double)_target_velocity_ned.x,
-                                               (double)_target_velocity_ned.y,
-                                               (double)_target_velocity_ned.z,
-                                               loc_estimate.lat,
-                                               loc_estimate.lng,
-                                               loc_estimate.alt
-                                               );
+    // if sysid not yet set, adopt sender’s sysid and enable automatic sysid tracking
+    if (_sysid == 0) {
+        _sysid.set(msg.sysid);
+        _sysid_used = 0;
+        _estimate_valid = false;
+        _automatic_sysid = true;
+    }
+
+    return true;
 }
-#endif  // HAL_LOGGING_ENABLED
 
-// get velocity estimate in m/s in NED frame using dt since last update
-bool AP_Follow::get_velocity_ned(Vector3f &vel_ned, float dt) const
+// Decodes a FOLLOW_TARGET MAVLink message to update the target’s position, velocity, acceleration, and orientation.
+bool AP_Follow::handle_follow_target_message(const mavlink_message_t &msg)
 {
-    vel_ned = _target_velocity_ned + (_target_accel_ned * dt);
+    // decode FOLLOW_TARGET message into packet struct
+    mavlink_follow_target_t packet;
+    mavlink_msg_follow_target_decode(&msg, &packet);
+
+    // ignore message if latitude and longitude are exactly zero (invalid GPS fix)
+    if ((packet.lat == 0 && packet.lon == 0)) {
+        return false;
+    }
+
+    // require that at least position is estimated (bit 0 of est_capabilities)
+    if ((packet.est_capabilities & (1<<0)) == 0) {
+        return false;
+    }
+
+    // build Location object from latitude, longitude, and altitude (alt in meters)
+    const Location _target_location {
+        packet.lat,
+        packet.lon,
+        int32_t(packet.alt * 100),  // convert meters to centimeters
+        Location::AltFrame::ABSOLUTE
+    };
+
+    // convert global location to local NED frame position
+    if (!_target_location.get_vector_from_origin_NEU(_target_pos_ned_m)) {
+        return false;
+    }
+    _target_pos_ned_m *= 0.01; // convert from cm to meters
+
+    // adjust Z coordinate to NED frame (NEU altitude -> NED)
+    Location origin;
+    if (!AP::ahrs().get_origin(origin)) {
+        return false;
+    }
+    _target_pos_ned_m.z = -packet.alt + origin.alt * 0.01;
+
+    // decode velocity if available (bit 1 of est_capabilities)
+    if (packet.est_capabilities & (1<<1)) {
+        _target_vel_ned_ms.x = packet.vel[0]; // velocity north
+        _target_vel_ned_ms.y = packet.vel[1]; // velocity east
+        _target_vel_ned_ms.z = packet.vel[2]; // velocity down
+    } else {
+        _target_vel_ned_ms.zero();
path: libraries/AP_Follow/AP_Follow.cpp
user: login: timtuxworth
body: I see your point, but I still don't think zeroing these values is safe. Surely even stale values would be better than zeros.

BTW - for plane, I can get the same data with GLOBAL_POSITION_INT and ATTITUDE (existing messages) without needing FOLLOW_TARGET. I implemented an ATTITUDE receiver in my Lua script.
created_at: 2025-05-02T02:08:25Z
author_association: CONTRIBUTOR
diff_hunk: @@ -134,419 +155,638 @@ const AP_Param::GroupInfo AP_Follow::var_info[] = {
     AP_GROUPINFO("_ALT_TYPE", 10, AP_Follow, _alt_type, AP_FOLLOW_ALT_TYPE_DEFAULT),
 #endif
 
+// May add option to turn on input shaping, turn on acceleration due to rotation of offsest
+
     // @Param: _OPTIONS
     // @DisplayName: Follow options
     // @Description: Follow options bitmask
     // @Values: 0:None,1: Mount Follows lead vehicle on mode enter
     // @User: Standard
     AP_GROUPINFO("_OPTIONS", 11, AP_Follow, _options, 0),
 
+    // @Param: _ACCEL_NE
+    // @DisplayName: Acceleration limit for the horizontal kinematic input shaping
+    // @Description: Acceleration limit of the horizontal kinematic path generation used to determine how quickly the ship varies in velocity
+    // @Range: 0 5
+    // @Units: m/s/s
+    // @User: Advanced
+    AP_GROUPINFO("_ACCEL_NE", 12, AP_Follow, _accel_max_ne_mss, 2.5),
+
+    // @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 ship 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),
+
+    // @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 ship 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),
+
+    // @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 ship 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),
+
+    // @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 ship 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),
+
+    // @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 ship 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_GROUPEND
 };
 
-/* 
-   The constructor also initialises the proximity sensor. Note that this
-   constructor is not called until detect() returns true, so we
-   already know that we should setup the proximity sensor
-*/
+// Constructor for AP_Follow. Initializes the position P-controller and sets up parameter defaults.
 AP_Follow::AP_Follow() :
-        _p_pos(AP_FOLLOW_POS_P_DEFAULT)
+    _p_pos(AP_FOLLOW_POS_P_DEFAULT)
 {
     _singleton = this;
     AP_Param::setup_object_defaults(this, var_info);
 }
 
-// restore offsets to zero if necessary, should be called when vehicle exits follow mode
-void AP_Follow::clear_offsets_if_required()
+
+//==============================================================================
+// Target Estimation Update Functions
+//==============================================================================
+
+// Projects and updates the estimated target position, velocity, and heading based on last known data and configured input shaping.
+bool AP_Follow::update_estimate()
 {
-    if (_offsets_were_zero) {
-        _offset.set(Vector3f());
-        _offsets_were_zero = false;
+    // exit immediately if not enabled
+    if (!_enabled) {
+        _estimate_valid = false;
+        return _estimate_valid;
     }
-}
 
-// get target's estimated location
-bool AP_Follow::get_target_location_and_velocity(Location &loc, Vector3f &vel_ned) const
-{
-    Vector3p pos_ned;
-    Vector3f local_vel_ned;  // so we either change both return parameters or neither
-    if (!get_target_position_and_velocity(pos_ned, local_vel_ned)) {
-        return false;
+    // check for timeout: if no recent location update, invalidate estimate
+    if ((_last_location_update_ms == 0) || (AP_HAL::millis() - _last_location_update_ms > AP_FOLLOW_TIMEOUT_MS)) {
+        _estimate_valid = false;
+        return _estimate_valid;
     }
-    if (!AP::ahrs().get_location_from_origin_offset_NED(loc, pos_ned)) {
-        return false;
+
+    // skip if update has already been processed this scheduler tick
+    if (_estimate_valid && (AP::scheduler().ticks32() - _last_update_ticks == 0)) {
+        return _estimate_valid;
     }
-    vel_ned = local_vel_ned;
+    _last_update_ticks = AP::scheduler().ticks32();
 
-    if (_alt_type == AP_FOLLOW_ALTITUDE_TYPE_RELATIVE) {
-        loc.change_alt_frame(Location::AltFrame::ABOVE_HOME);
+    // if sysid changed, reset the estimation state
+    if (_sysid != _sysid_used) {
+        _sysid_used = _sysid;
+        _estimate_valid = false;
     }
 
-    return true;
-}
+    const uint32_t now = AP_HAL::millis();
 
-// get target's estimated location and velocity, both NED SI from origin
-bool AP_Follow::get_target_position_and_velocity(Vector3p &pos_ned, Vector3f &vel_ned) const
-{
-    // exit immediately if not enabled
-    if (!_enabled) {
-        return false;
+    // calculate time since last location update in seconds
+    const float dt = (now - _last_location_update_ms) * 0.001f;
+
+    // project target's position and velocity forward using simple kinematics
+    Vector3f delta_pos = _target_vel_ned_ms * dt + _target_accel_ned_mss * 0.5f * sq(dt);
+    Vector3f delta_vel = _target_accel_ned_mss * dt;
+
+    // calculate time since last estimation update
+    const float e_dt = (now - _last_estimation_update_ms) * 0.001f;
+    
+    if (_estimate_valid) {
+        // 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)
+        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.xy().topostype(), _target_vel_ned_ms.xy() + delta_vel.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 angular shaping for heading estimate
+        shape_angle_vel_accel(radians(_target_heading_deg), radians(_target_heading_rate_degs), 0.0,
+                              _estimate_heading_rad, _estimate_heading_rate_rads, _estimate_heading_accel_radss,
+                              0.0, radians(_accel_max_h_degss),
+                              radians(_jerk_max_h_degsss), e_dt, false);
+
+        // update heading angle separately to maintain proper wrapping [-PI, PI]
+        postype_t estimate_heading_rad = _estimate_heading_rad;
+        update_pos_vel_accel(estimate_heading_rad, _estimate_heading_rate_rads, _estimate_heading_accel_radss, e_dt, 0.0, 0.0, 0.0);
+        _estimate_heading_rad = wrap_PI(estimate_heading_rad);
+    } else {
+        // no valid estimate yet: initialise from latest target position
+        _estimate_pos_ned_m = _target_pos_ned_m + delta_pos.topostype();
+        _estimate_vel_ned_ms = _target_vel_ned_ms + delta_vel;
+        _estimate_accel_ned_mss = _target_accel_ned_mss;
+        _estimate_valid = true;
     }
 
-    // check for timeout
-    if ((_last_location_update_ms == 0) || (AP_HAL::millis() - _last_location_update_ms > AP_FOLLOW_TIMEOUT_MS)) {
-        return false;
+    Vector3f offset_m = _offset_XXD_m.get();
+
+    // calculate estimated position and velocity with offsets applied
+    if (offset_m.is_zero() || (_offset_type == AP_FOLLOW_OFFSET_TYPE_NED)) {
+        // offsets are in NED frame: simple addition
+        _ofs_estimate_pos_ned_m = _estimate_pos_ned_m + offset_m.topostype();
+        _ofs_estimate_vel_ned_ms = _estimate_vel_ned_ms;
+        _ofs_estimate_accel_ned_mss = _estimate_accel_ned_mss;
+    } else {
+        // offsets are in FRD frame: rotate by heading
+        offset_m.xy().rotate(_estimate_heading_rad);
+        Vector3f offset_cross = offset_m.cross(Vector3f{0.0, 0.0, 1.0});
+        float offset_length_m = offset_m.length();
+        _ofs_estimate_pos_ned_m = _estimate_pos_ned_m + offset_m.topostype();
+        _ofs_estimate_vel_ned_ms = _estimate_vel_ned_ms + offset_cross * offset_length_m * _estimate_heading_rate_rads;
+        _ofs_estimate_accel_ned_mss = _estimate_accel_ned_mss + offset_cross * offset_length_m * _estimate_heading_accel_radss;
     }
 
-    // calculate time since last actual position update
-    const float dt = (AP_HAL::millis() - _last_location_update_ms) * 0.001f;
+    _last_estimation_update_ms = now;
+    return _estimate_valid;
+}
+
 
-    // get velocity estimate
-    if (!get_velocity_ned(vel_ned, dt)) {
+//==============================================================================
+// Target Information Retrieval Functions
+//==============================================================================
+
+// Retrieves the estimated target position, velocity, and acceleration in the NED frame (relative to origin).
+bool AP_Follow::get_target_pos_vel_accel_NED_m(Vector3p &pos_ned_m, Vector3f &vel_ned_ms, Vector3f &accel_ned_mss)
+{
+    if (!update_estimate()) {
         return false;
     }
 
-    // project the vehicle position
-    const Vector3p vel_ned_p { vel_ned.x, vel_ned.y, vel_ned.z };
-    pos_ned = _target_position_ned + vel_ned_p * dt;
+    pos_ned_m = _estimate_pos_ned_m;
+    vel_ned_ms = _estimate_vel_ned_ms;
+    accel_ned_mss = _estimate_accel_ned_mss;
 
     return true;
 }
 
-// get distance vector to target (in meters) and target's velocity all in NED frame
-bool AP_Follow::get_target_dist_and_vel_ned(Vector3f &dist_ned, Vector3f &dist_with_offs, Vector3f &vel_ned)
+// Retrieves the estimated target position, velocity, and acceleration in the NED frame, including configured offsets.
+bool AP_Follow::get_ofs_pos_vel_accel_NED_m(Vector3p &pos_ofs_ned_m, Vector3f &vel_ofs_ned_ms, Vector3f &accel_ofs_ned_mss)
 {
-    Vector3f current_position_NED;
-    if (!AP::ahrs().get_relative_position_NED_origin(current_position_NED)) {
-        clear_dist_and_bearing_to_target();
-        return false;
-    }
-    Vector3p target_position_NED;
-    Vector3f veh_vel;  // NED
-    if (!get_target_position_and_velocity(target_position_NED, veh_vel)) {
-        clear_dist_and_bearing_to_target();
+    if (!update_estimate()) {
         return false;
     }
 
-    // convert from Vector3p to Vector3f as they can't be subtracted:
-    Vector3f target_position_NED_f;
-    target_position_NED_f.x = target_position_NED.x;
-    target_position_NED_f.y = target_position_NED.y;
-    target_position_NED_f.z = target_position_NED.z;
+    pos_ofs_ned_m = _ofs_estimate_pos_ned_m;
+    vel_ofs_ned_ms = _ofs_estimate_vel_ned_ms;
+    accel_ofs_ned_mss = _ofs_estimate_accel_ned_mss;
 
-    const Vector3f dist_vec = target_position_NED_f - current_position_NED;
+    return true;
+}
 
-    // fail if too far
-    if (is_positive(_dist_max.get()) && (dist_vec.length() > _dist_max)) {
-        clear_dist_and_bearing_to_target();
+// Retrieves distance vectors (with and without configured offsets) and the target’s velocity, all in the NED frame.
+bool AP_Follow::get_target_dist_and_vel_NED_m(Vector3f &dist_ned, Vector3f &dist_with_offs, Vector3f &vel_ned)
+{
+    if (!update_estimate()) {
         return false;
     }
 
-    // initialise offsets from distance vector if required
-    init_offsets_if_required(dist_vec);
-
-    // get offsets
-    Vector3f offsets;
-    if (!get_offsets_ned(offsets)) {
+    Vector3f current_position_ned_m;
+    if (!AP::ahrs().get_relative_position_NED_origin(current_position_ned_m)) {
         clear_dist_and_bearing_to_target();
         return false;
     }
 
-    // calculate results
-    dist_ned = dist_vec;
-    dist_with_offs = dist_vec + offsets;
-    vel_ned = veh_vel;
+    const Vector3p dist_vec_ned_m = _estimate_pos_ned_m - current_position_ned_m.topostype();
+    const Vector3p ofs_dist_vec = _ofs_estimate_pos_ned_m - current_position_ned_m.topostype();
+    dist_ned = dist_vec_ned_m.tofloat();
+    dist_with_offs = ofs_dist_vec.tofloat();
+    vel_ned = _ofs_estimate_vel_ned_ms;
 
-    // record distance and heading for reporting purposes
-    if (is_zero(dist_with_offs.x) && is_zero(dist_with_offs.y)) {
-        clear_dist_and_bearing_to_target();
-    } else {
-        _dist_to_target = safe_sqrt(sq(dist_with_offs.x) + sq(dist_with_offs.y));
-        _bearing_to_target = degrees(atan2f(dist_with_offs.y, dist_with_offs.x));
+    return true;
+}
+
+// Retrieves the estimated target heading and heading rate in radians.
+bool AP_Follow::get_heading_heading_rate_rad(float &heading_rad, float &heading_rate_rads)
+{
+    if (!update_estimate()) {
+        return false;
     }
 
+    // return latest heading estimate
+    heading_rad = _estimate_heading_rad;
+    heading_rate_rads = _estimate_heading_rate_rads;
     return true;
 }
 
-// get target's heading in degrees (0 = north, 90 = east)
-bool AP_Follow::get_target_heading_deg(float &heading) const
+// Retrieves the target's estimated global location and velocity, adjusting altitude frame if relative mode is set (for LUA bindings).
+bool AP_Follow::get_target_location_and_velocity(Location &loc, Vector3f &vel_ned)
 {
-    // exit immediately if not enabled
-    if (!_enabled) {
+    if (!update_estimate()) {
         return false;
     }
 
-    // check for timeout
-    if ((_last_heading_update_ms == 0) || (AP_HAL::millis() - _last_heading_update_ms > AP_FOLLOW_TIMEOUT_MS)) {
+    if (!AP::ahrs().get_location_from_origin_offset_NED(loc, _estimate_pos_ned_m)) {
         return false;
     }
+    vel_ned = _estimate_vel_ned_ms;
+
+    if (_alt_type == AP_FOLLOW_ALTITUDE_TYPE_RELATIVE) {
+        loc.change_alt_frame(Location::AltFrame::ABOVE_HOME);
+    }
 
-    // return latest heading estimate
-    heading = _target_heading;
     return true;
 }
 
-// returns true if we should extract information from msg
-bool AP_Follow::should_handle_message(const mavlink_message_t &msg) const
+// Retrieves the target's estimated global location and velocity, including configured offsets, for LUA bindings.
+bool AP_Follow::get_target_location_and_velocity_ofs(Location &loc, Vector3f &vel_ned)
 {
-    // exit immediately if not enabled
-    if (!_enabled) {
+    if (!update_estimate()) {
         return false;
     }
-
-    // skip our own messages
-    if (msg.sysid == mavlink_system.sysid) {
+    if (!AP::ahrs().get_location_from_origin_offset_NED(loc, _ofs_estimate_pos_ned_m)) {
         return false;
     }
+    if (_alt_type == AP_FOLLOW_ALTITUDE_TYPE_RELATIVE) {
+        loc.change_alt_frame(Location::AltFrame::ABOVE_HOME);
+    }
 
-    // skip message if not from our target
-    if (_sysid != 0 && msg.sysid != _sysid) {
+    vel_ned = _ofs_estimate_vel_ned_ms;
+    return true;
+}
+
+// Retrieves the estimated target heading in degrees (0° = North, 90° = East) for LUA bindings.
+bool AP_Follow::get_target_heading_deg(float &heading)
+{
+    if (!update_estimate()) {
         return false;
     }
 
+    // return latest heading estimate
+    heading = degrees(_estimate_heading_rad);
     return true;
 }
 
-// handle mavlink DISTANCE_SENSOR messages
+
+//==============================================================================
+// MAVLink Message Handling
+//==============================================================================
+
+// Handles incoming MAVLink messages to update the target's position, velocity, and heading.
 void AP_Follow::handle_msg(const mavlink_message_t &msg)
 {
-    // this method should be called from an "update()" method:
     if (_automatic_sysid) {
-        // maybe timeout who we were following...
+        // timeout check: if no position update received within timeout, clear automatic sysid
         if ((_last_location_update_ms == 0) ||
             (AP_HAL::millis() - _last_location_update_ms > AP_FOLLOW_SYSID_TIMEOUT_MS)) {
             _sysid.set(0);
+            _sysid_used = 0;
+            _estimate_valid = false;
         }
     }
 
     if (!should_handle_message(msg)) {
+        // ignore message if filtering rules reject it (e.g., wrong sysid)
         return;
     }
 
-    // decode global-position-int message
+    // decode MAVLink message
     bool updated = false;
 
     switch (msg.msgid) {
     case MAVLINK_MSG_ID_GLOBAL_POSITION_INT: {
+        // handle standard global position messages
         updated = handle_global_position_int_message(msg);
         break;
     }
     case MAVLINK_MSG_ID_FOLLOW_TARGET: {
+        // handle follow-target specific messages
         updated = handle_follow_target_message(msg);
         break;
     }
     }
 
     if (updated) {
+        // message provided updated position information
+        Vector3f offset_m = _offset_XXD_m.get();
+
+        // if using relative frame, rotate offsets based on estimated heading
+        if (_offset_type == AP_FOLLOW_OFFSET_TYPE_RELATIVE) {
+            offset_m.xy().rotate(_estimate_heading_rad);
+        }
+
+        Vector3f current_position_ned_m;
+        if (!AP::ahrs().get_relative_position_NED_origin(current_position_ned_m)) {
+            // 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
+            const Vector3p dist_vec_ned_m = _estimate_pos_ned_m - current_position_ned_m.topostype();
+            const Vector3p ofs_dist_vec = _ofs_estimate_pos_ned_m - current_position_ned_m.topostype();
+
+            // initialise offsets dynamically if needed
+            init_offsets_if_required(dist_vec_ned_m.tofloat());
+
+            // record distance and bearing to target for reporting/logging
+            if (ofs_dist_vec.xy().is_zero()) {
+                clear_dist_and_bearing_to_target();
+            } else {
+                _dist_to_target_m = ofs_dist_vec.xy().length();
+                _bearing_to_target_deg = degrees(ofs_dist_vec.xy().angle());
+            }
+
+            // clear distance and bearing if outside configured max follow distance
+            if (is_positive(_dist_max_m.get()) && (dist_vec_ned_m.length() > _dist_max_m)) {
+                clear_dist_and_bearing_to_target();
+            }
+        }
+
+        // Check if estimate needs reset based on position and velocity errors
+        if (estimate_error_too_large()) {
+            _estimate_valid = false;
+        }
+
 #if HAL_LOGGING_ENABLED
+        // log current follow diagnostic data
         Log_Write_FOLL();
 #endif
     }
 }
 
-bool AP_Follow::handle_global_position_int_message(const mavlink_message_t &msg)
+// Returns true if the incoming MAVLink message should be processed for target updates.
+bool AP_Follow::should_handle_message(const mavlink_message_t &msg) const
 {
-        // decode message
-        mavlink_global_position_int_t packet;
-        mavlink_msg_global_position_int_decode(&msg, &packet);
+    // exit immediately if not enabled
+    if (!_enabled) {
+        return false;
+    }
 
-        // ignore message if lat and lon are (exactly) zero
-        if ((packet.lat == 0 && packet.lon == 0)) {
-            return false;
-        }
+    // skip our own messages
+    if (msg.sysid == mavlink_system.sysid) {
+        return false;
+    }
 
-        Location _target_location;
-        _target_location.lat = packet.lat;
-        _target_location.lng = packet.lon;
+    // skip message if not from our target
+    if (_sysid != 0 && msg.sysid != _sysid) {
+        return false;
+    }
 
-        // select altitude source based on FOLL_ALT_TYPE param 
-        if (_alt_type == AP_FOLLOW_ALTITUDE_TYPE_RELATIVE) {
-            // above home alt
-            _target_location.set_alt_cm(packet.relative_alt / 10, Location::AltFrame::ABOVE_HOME);
-        } else {
-            // absolute altitude
-            _target_location.set_alt_cm(packet.alt / 10, Location::AltFrame::ABSOLUTE);
-        }
-        if (!_target_location.get_vector_from_origin_NEU(_target_position_ned)) {
-            return false;
-        }
-        _target_position_ned.z = -_target_position_ned.z; // NEU->NED
-        _target_position_ned *= 0.01;  // cm -> m
-
-        _target_velocity_ned.x = packet.vx * 0.01f; // velocity north
-        _target_velocity_ned.y = packet.vy * 0.01f; // velocity east
-        _target_velocity_ned.z = packet.vz * 0.01f; // velocity down
-
-        // get a local timestamp with correction for transport jitter
-        _last_location_update_ms = _jitter.correct_offboard_timestamp_msec(packet.time_boot_ms, AP_HAL::millis());
-        if (packet.hdg <= 36000) {                  // heading (UINT16_MAX if unknown)
-            _target_heading = packet.hdg * 0.01f;   // convert centi-degrees to degrees
-            _last_heading_update_ms = _last_location_update_ms;
-        }
-        // initialise _sysid if zero to sender's id
-        if (_sysid == 0) {
-            _sysid.set(msg.sysid);
-            _automatic_sysid = true;
-        }
-        return true;
+    return true;
 }
 
-bool AP_Follow::handle_follow_target_message(const mavlink_message_t &msg)
+// Checks whether the current estimate should be reset based on position and velocity errors.
+bool AP_Follow::estimate_error_too_large() const
 {
-        // decode message
-        mavlink_follow_target_t packet;
-        mavlink_msg_follow_target_decode(&msg, &packet);
+    const float timeout_sec = AP_FOLLOW_TIMEOUT_MS * 0.001f;
+    const float half_timeout_sec = timeout_sec * 0.5f;
 
-        // ignore message if lat and lon are (exactly) zero
-        if ((packet.lat == 0 && packet.lon == 0)) {
-            return false;
-        }
-        // require at least position
-        if ((packet.est_capabilities & (1<<0)) == 0) {
-            return false;
-        }
+    // Calculate position thresholds
+    const float pos_thresh_horiz_m = _accel_max_ne_mss.get() * sq(half_timeout_sec);
+    const float pos_thresh_vert_m  = _accel_max_d_mss.get()  * sq(half_timeout_sec);
 
-        const Location new_loc {
-            packet.lat,
-            packet.lon,
-            int32_t(packet.alt * 100),  // m -> cm
-            Location::AltFrame::ABSOLUTE
-        };
-
-        if (!new_loc.get_vector_from_origin_NEU(_target_position_ned)) {
-            return false;
-        }
-        _target_position_ned.z = -_target_position_ned.z; // NEU->NED
-        _target_position_ned *= 0.01;  // cm -> m
+    // Calculate velocity thresholds using the new helper function
+    const float vel_thresh_horiz_ms = calc_max_velocity_change(_accel_max_ne_mss.get(), _jerk_max_ne_msss.get(), timeout_sec);
+    const float vel_thresh_vert_ms  = calc_max_velocity_change(_accel_max_d_mss.get(), _jerk_max_d_msss.get(), timeout_sec);
 
-        if (packet.est_capabilities & (1<<1)) {
-            _target_velocity_ned.x = packet.vel[0]; // velocity north
-            _target_velocity_ned.y = packet.vel[1]; // velocity east
-            _target_velocity_ned.z = packet.vel[2]; // velocity down
-        } else {
-            _target_velocity_ned.zero();
-        }
+    // Calculate current position and velocity errors
+    Vector3f pos_error = _estimate_pos_ned_m.tofloat() - _target_pos_ned_m.tofloat();
+    Vector3f vel_error = _estimate_vel_ned_ms - _target_vel_ned_ms;
 
-        // get a local timestamp with correction for transport jitter
-        _last_location_update_ms = _jitter.correct_offboard_timestamp_msec(packet.timestamp, AP_HAL::millis());
+    Vector2f pos_error_xy = pos_error.xy();
+    float pos_error_z = pos_error.z;
+    Vector2f vel_error_xy = vel_error.xy();
+    float vel_error_z = vel_error.z;
 
-        if (packet.est_capabilities & (1<<3)) {
-            Quaternion q{packet.attitude_q[0], packet.attitude_q[1], packet.attitude_q[2], packet.attitude_q[3]};
-            float r, p, y;
-            q.to_euler(r,p,y);
-            _target_heading = degrees(y);
-            _last_heading_update_ms = _last_location_update_ms;
-        }
+    // Check horizontal and vertical separately
+    const bool pos_horiz_bad = pos_error_xy.length() > pos_thresh_horiz_m;
+    const bool vel_horiz_bad = vel_error_xy.length() > vel_thresh_horiz_ms;
+    const bool pos_vert_bad = fabsf(pos_error_z) > pos_thresh_vert_m;
+    const bool vel_vert_bad = fabsf(vel_error_z) > vel_thresh_vert_ms;
 
-        // initialise _sysid if zero to sender's id
-        if (_sysid == 0) {
-            _sysid.set(msg.sysid);
-            _automatic_sysid = true;
-        }
+    return pos_horiz_bad || vel_horiz_bad || pos_vert_bad || vel_vert_bad;
+}
 
-        return true;
+// Calculates max velocity change under trapezoidal or triangular acceleration profile (jerk-limited).
+float AP_Follow::calc_max_velocity_change(float accel_max, float jerk_max, float timeout_sec) const
+{
+    const float t_jerk = accel_max / jerk_max;
+    const float t_total_jerk = 2.0f * t_jerk;
+
+    if (timeout_sec >= t_total_jerk) {
+        // time to ramp up, constant accel phase, and ramp down
+        const float t_const = timeout_sec - t_total_jerk;
+        const float delta_v_jerk = 0.5f * accel_max * t_jerk;
+        const float delta_v_const = accel_max * t_const;
+        return 2.0f * delta_v_jerk + delta_v_const;
+    } else {
+        // timeout too short: pure triangle profile
+        const float t_half = timeout_sec * 0.5f;
+        return 0.5f * jerk_max * sq(t_half);
+    }
 }
 
-// write out an onboard-log message to help diagnose follow problems:
-#if HAL_LOGGING_ENABLED
-void AP_Follow::Log_Write_FOLL()
+// Decodes a GLOBAL_POSITION_INT MAVLink message to update the target’s position, velocity, and heading.
+bool AP_Follow::handle_global_position_int_message(const mavlink_message_t &msg)
 {
-        // get estimated location and velocity
-        Location loc_estimate{};
-        Vector3f vel_estimate;
-        UNUSED_RESULT(get_target_location_and_velocity(loc_estimate, vel_estimate));
-
-        Location _target_location;
-        UNUSED_RESULT(AP::ahrs().get_location_from_origin_offset_NED(_target_location, _target_position_ned * 0.01));
-        if (_alt_type == AP_FOLLOW_ALTITUDE_TYPE_RELATIVE) {
-            _target_location.change_alt_frame(Location::AltFrame::ABOVE_HOME);
-        }
+    // decode GLOBAL_POSITION_INT message into packet struct
+    mavlink_global_position_int_t packet;
+    mavlink_msg_global_position_int_decode(&msg, &packet);
+
+    // ignore message if latitude and longitude are exactly zero (invalid GPS fix)
+    if ((packet.lat == 0 && packet.lon == 0)) {
+        return false;
+    }
+
+    Location _target_location;
+    _target_location.lat = packet.lat;
+    _target_location.lng = packet.lon;
+
+    // set target altitude based on configured altitude type
+    if (_alt_type == AP_FOLLOW_ALTITUDE_TYPE_RELATIVE) {
+        // use relative altitude above home
+        _target_location.set_alt_cm(packet.relative_alt / 10, Location::AltFrame::ABOVE_HOME);
+    } else {
+        // use absolute altitude
+        _target_location.set_alt_cm(packet.alt / 10, Location::AltFrame::ABSOLUTE);
+    }
+
+    // convert global location to local NED frame position
+    if (!_target_location.get_vector_from_origin_NEU(_target_pos_ned_m)) {
+        return false;
+    }
+    _target_pos_ned_m.z = -_target_pos_ned_m.z; // convert NEU -> NED
+    _target_pos_ned_m *= 0.01;  // convert from cm to meters
+
+    // decode target velocity components (in m/s)
+    _target_vel_ned_ms.x = packet.vx * 0.01f; // velocity north
+    _target_vel_ned_ms.y = packet.vy * 0.01f; // velocity east
+    _target_vel_ned_ms.z = packet.vz * 0.01f; // velocity down
+
+    // target acceleration not available in GLOBAL_POSITION_INT
+    _target_accel_ned_mss.zero();
+
+    if (packet.hdg <= 36000) {
+        // valid heading field available (in centi-degrees)
+        _target_heading_deg = packet.hdg * 0.01f;
+    } else if (_offset_type == AP_FOLLOW_OFFSET_TYPE_RELATIVE) {
+        // heading missing but relative offset mode requires heading -> reject
+        return false;
+    } else {
+        // no heading available: set heading rate to zero
+        _target_heading_rate_degs = 0.0f;
+    }
+
+    // apply jitter-corrected timestamp to this update
+    _last_location_update_ms = _jitter.correct_offboard_timestamp_msec(packet.time_boot_ms, AP_HAL::millis());
 
-        // log lead's estimated vs reported position
-// @LoggerMessage: FOLL
-// @Description: Follow library diagnostic data
-// @Field: TimeUS: Time since system startup
-// @Field: Lat: Target latitude
-// @Field: Lon: Target longitude
-// @Field: Alt: Target absolute altitude
-// @Field: VelN: Target earth-frame velocity, North
-// @Field: VelE: Target earth-frame velocity, East
-// @Field: VelD: Target earth-frame velocity, Down
-// @Field: LatE: Vehicle latitude
-// @Field: LonE: Vehicle longitude
-// @Field: AltE: Vehicle absolute altitude
-        AP::logger().WriteStreaming("FOLL",
-                                               "TimeUS,Lat,Lon,Alt,VelN,VelE,VelD,LatE,LonE,AltE",  // labels
-                                               "sDUmnnnDUm",    // units
-                                               "F--B000--B",    // mults
-                                               "QLLifffLLi",    // fmt
-                                               AP_HAL::micros64(),
-                                               _target_location.lat,
-                                               _target_location.lng,
-                                               _target_location.alt,
-                                               (double)_target_velocity_ned.x,
-                                               (double)_target_velocity_ned.y,
-                                               (double)_target_velocity_ned.z,
-                                               loc_estimate.lat,
-                                               loc_estimate.lng,
-                                               loc_estimate.alt
-                                               );
+    // if sysid not yet set, adopt sender’s sysid and enable automatic sysid tracking
+    if (_sysid == 0) {
+        _sysid.set(msg.sysid);
+        _sysid_used = 0;
+        _estimate_valid = false;
+        _automatic_sysid = true;
+    }
+
+    return true;
 }
-#endif  // HAL_LOGGING_ENABLED
 
-// get velocity estimate in m/s in NED frame using dt since last update
-bool AP_Follow::get_velocity_ned(Vector3f &vel_ned, float dt) const
+// Decodes a FOLLOW_TARGET MAVLink message to update the target’s position, velocity, acceleration, and orientation.
+bool AP_Follow::handle_follow_target_message(const mavlink_message_t &msg)
 {
-    vel_ned = _target_velocity_ned + (_target_accel_ned * dt);
+    // decode FOLLOW_TARGET message into packet struct
+    mavlink_follow_target_t packet;
+    mavlink_msg_follow_target_decode(&msg, &packet);
+
+    // ignore message if latitude and longitude are exactly zero (invalid GPS fix)
+    if ((packet.lat == 0 && packet.lon == 0)) {
+        return false;
+    }
+
+    // require that at least position is estimated (bit 0 of est_capabilities)
+    if ((packet.est_capabilities & (1<<0)) == 0) {
+        return false;
+    }
+
+    // build Location object from latitude, longitude, and altitude (alt in meters)
+    const Location _target_location {
+        packet.lat,
+        packet.lon,
+        int32_t(packet.alt * 100),  // convert meters to centimeters
+        Location::AltFrame::ABSOLUTE
+    };
+
+    // convert global location to local NED frame position
+    if (!_target_location.get_vector_from_origin_NEU(_target_pos_ned_m)) {
+        return false;
+    }
+    _target_pos_ned_m *= 0.01; // convert from cm to meters
+
+    // adjust Z coordinate to NED frame (NEU altitude -> NED)
+    Location origin;
+    if (!AP::ahrs().get_origin(origin)) {
+        return false;
+    }
+    _target_pos_ned_m.z = -packet.alt + origin.alt * 0.01;
+
+    // decode velocity if available (bit 1 of est_capabilities)
+    if (packet.est_capabilities & (1<<1)) {
+        _target_vel_ned_ms.x = packet.vel[0]; // velocity north
+        _target_vel_ned_ms.y = packet.vel[1]; // velocity east
+        _target_vel_ned_ms.z = packet.vel[2]; // velocity down
+    } else {
+        _target_vel_ned_ms.zero();
path: libraries/AP_Follow/AP_Follow.cpp
user: login: lthall
body: Absolutly not. Stale values are worse than zero. Stale valuse will a random cause a random offset error. Zeros will result in a sensible and predictable following error.

There is no point getting the same data from multiple message sources when we can simply select the prefered one. In my implementation of the FOLLOW_TARGET message the data will be different to the GLOBAL_POSITION_INT for example.
created_at: 2025-05-02T03:50:44Z
author_association: CONTRIBUTOR
diff_hunk: @@ -134,419 +155,638 @@ const AP_Param::GroupInfo AP_Follow::var_info[] = {
     AP_GROUPINFO("_ALT_TYPE", 10, AP_Follow, _alt_type, AP_FOLLOW_ALT_TYPE_DEFAULT),
 #endif
 
+// May add option to turn on input shaping, turn on acceleration due to rotation of offsest
+
     // @Param: _OPTIONS
     // @DisplayName: Follow options
     // @Description: Follow options bitmask
     // @Values: 0:None,1: Mount Follows lead vehicle on mode enter
     // @User: Standard
     AP_GROUPINFO("_OPTIONS", 11, AP_Follow, _options, 0),
 
+    // @Param: _ACCEL_NE
+    // @DisplayName: Acceleration limit for the horizontal kinematic input shaping
+    // @Description: Acceleration limit of the horizontal kinematic path generation used to determine how quickly the ship varies in velocity
+    // @Range: 0 5
+    // @Units: m/s/s
+    // @User: Advanced
+    AP_GROUPINFO("_ACCEL_NE", 12, AP_Follow, _accel_max_ne_mss, 2.5),
+
+    // @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 ship 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),
+
+    // @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 ship 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),
+
+    // @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 ship 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),
+
+    // @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 ship 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),
+
+    // @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 ship 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_GROUPEND
 };
 
-/* 
-   The constructor also initialises the proximity sensor. Note that this
-   constructor is not called until detect() returns true, so we
-   already know that we should setup the proximity sensor
-*/
+// Constructor for AP_Follow. Initializes the position P-controller and sets up parameter defaults.
 AP_Follow::AP_Follow() :
-        _p_pos(AP_FOLLOW_POS_P_DEFAULT)
+    _p_pos(AP_FOLLOW_POS_P_DEFAULT)
 {
     _singleton = this;
     AP_Param::setup_object_defaults(this, var_info);
 }
 
-// restore offsets to zero if necessary, should be called when vehicle exits follow mode
-void AP_Follow::clear_offsets_if_required()
+
+//==============================================================================
+// Target Estimation Update Functions
+//==============================================================================
+
+// Projects and updates the estimated target position, velocity, and heading based on last known data and configured input shaping.
+bool AP_Follow::update_estimate()
 {
-    if (_offsets_were_zero) {
-        _offset.set(Vector3f());
-        _offsets_were_zero = false;
+    // exit immediately if not enabled
+    if (!_enabled) {
+        _estimate_valid = false;
+        return _estimate_valid;
     }
-}
 
-// get target's estimated location
-bool AP_Follow::get_target_location_and_velocity(Location &loc, Vector3f &vel_ned) const
-{
-    Vector3p pos_ned;
-    Vector3f local_vel_ned;  // so we either change both return parameters or neither
-    if (!get_target_position_and_velocity(pos_ned, local_vel_ned)) {
-        return false;
+    // check for timeout: if no recent location update, invalidate estimate
+    if ((_last_location_update_ms == 0) || (AP_HAL::millis() - _last_location_update_ms > AP_FOLLOW_TIMEOUT_MS)) {
+        _estimate_valid = false;
+        return _estimate_valid;
     }
-    if (!AP::ahrs().get_location_from_origin_offset_NED(loc, pos_ned)) {
-        return false;
+
+    // skip if update has already been processed this scheduler tick
+    if (_estimate_valid && (AP::scheduler().ticks32() - _last_update_ticks == 0)) {
+        return _estimate_valid;
     }
-    vel_ned = local_vel_ned;
+    _last_update_ticks = AP::scheduler().ticks32();
 
-    if (_alt_type == AP_FOLLOW_ALTITUDE_TYPE_RELATIVE) {
-        loc.change_alt_frame(Location::AltFrame::ABOVE_HOME);
+    // if sysid changed, reset the estimation state
+    if (_sysid != _sysid_used) {
+        _sysid_used = _sysid;
+        _estimate_valid = false;
     }
 
-    return true;
-}
+    const uint32_t now = AP_HAL::millis();
 
-// get target's estimated location and velocity, both NED SI from origin
-bool AP_Follow::get_target_position_and_velocity(Vector3p &pos_ned, Vector3f &vel_ned) const
-{
-    // exit immediately if not enabled
-    if (!_enabled) {
-        return false;
+    // calculate time since last location update in seconds
+    const float dt = (now - _last_location_update_ms) * 0.001f;
+
+    // project target's position and velocity forward using simple kinematics
+    Vector3f delta_pos = _target_vel_ned_ms * dt + _target_accel_ned_mss * 0.5f * sq(dt);
+    Vector3f delta_vel = _target_accel_ned_mss * dt;
+
+    // calculate time since last estimation update
+    const float e_dt = (now - _last_estimation_update_ms) * 0.001f;
+    
+    if (_estimate_valid) {
+        // 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)
+        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.xy().topostype(), _target_vel_ned_ms.xy() + delta_vel.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 angular shaping for heading estimate
+        shape_angle_vel_accel(radians(_target_heading_deg), radians(_target_heading_rate_degs), 0.0,
+                              _estimate_heading_rad, _estimate_heading_rate_rads, _estimate_heading_accel_radss,
+                              0.0, radians(_accel_max_h_degss),
+                              radians(_jerk_max_h_degsss), e_dt, false);
+
+        // update heading angle separately to maintain proper wrapping [-PI, PI]
+        postype_t estimate_heading_rad = _estimate_heading_rad;
+        update_pos_vel_accel(estimate_heading_rad, _estimate_heading_rate_rads, _estimate_heading_accel_radss, e_dt, 0.0, 0.0, 0.0);
+        _estimate_heading_rad = wrap_PI(estimate_heading_rad);
+    } else {
+        // no valid estimate yet: initialise from latest target position
+        _estimate_pos_ned_m = _target_pos_ned_m + delta_pos.topostype();
+        _estimate_vel_ned_ms = _target_vel_ned_ms + delta_vel;
+        _estimate_accel_ned_mss = _target_accel_ned_mss;
+        _estimate_valid = true;
     }
 
-    // check for timeout
-    if ((_last_location_update_ms == 0) || (AP_HAL::millis() - _last_location_update_ms > AP_FOLLOW_TIMEOUT_MS)) {
-        return false;
+    Vector3f offset_m = _offset_XXD_m.get();
+
+    // calculate estimated position and velocity with offsets applied
+    if (offset_m.is_zero() || (_offset_type == AP_FOLLOW_OFFSET_TYPE_NED)) {
+        // offsets are in NED frame: simple addition
+        _ofs_estimate_pos_ned_m = _estimate_pos_ned_m + offset_m.topostype();
+        _ofs_estimate_vel_ned_ms = _estimate_vel_ned_ms;
+        _ofs_estimate_accel_ned_mss = _estimate_accel_ned_mss;
+    } else {
+        // offsets are in FRD frame: rotate by heading
+        offset_m.xy().rotate(_estimate_heading_rad);
+        Vector3f offset_cross = offset_m.cross(Vector3f{0.0, 0.0, 1.0});
+        float offset_length_m = offset_m.length();
+        _ofs_estimate_pos_ned_m = _estimate_pos_ned_m + offset_m.topostype();
+        _ofs_estimate_vel_ned_ms = _estimate_vel_ned_ms + offset_cross * offset_length_m * _estimate_heading_rate_rads;
+        _ofs_estimate_accel_ned_mss = _estimate_accel_ned_mss + offset_cross * offset_length_m * _estimate_heading_accel_radss;
     }
 
-    // calculate time since last actual position update
-    const float dt = (AP_HAL::millis() - _last_location_update_ms) * 0.001f;
+    _last_estimation_update_ms = now;
+    return _estimate_valid;
+}
+
 
-    // get velocity estimate
-    if (!get_velocity_ned(vel_ned, dt)) {
+//==============================================================================
+// Target Information Retrieval Functions
+//==============================================================================
+
+// Retrieves the estimated target position, velocity, and acceleration in the NED frame (relative to origin).
+bool AP_Follow::get_target_pos_vel_accel_NED_m(Vector3p &pos_ned_m, Vector3f &vel_ned_ms, Vector3f &accel_ned_mss)
+{
+    if (!update_estimate()) {
         return false;
     }
 
-    // project the vehicle position
-    const Vector3p vel_ned_p { vel_ned.x, vel_ned.y, vel_ned.z };
-    pos_ned = _target_position_ned + vel_ned_p * dt;
+    pos_ned_m = _estimate_pos_ned_m;
+    vel_ned_ms = _estimate_vel_ned_ms;
+    accel_ned_mss = _estimate_accel_ned_mss;
 
     return true;
 }
 
-// get distance vector to target (in meters) and target's velocity all in NED frame
-bool AP_Follow::get_target_dist_and_vel_ned(Vector3f &dist_ned, Vector3f &dist_with_offs, Vector3f &vel_ned)
+// Retrieves the estimated target position, velocity, and acceleration in the NED frame, including configured offsets.
+bool AP_Follow::get_ofs_pos_vel_accel_NED_m(Vector3p &pos_ofs_ned_m, Vector3f &vel_ofs_ned_ms, Vector3f &accel_ofs_ned_mss)
 {
-    Vector3f current_position_NED;
-    if (!AP::ahrs().get_relative_position_NED_origin(current_position_NED)) {
-        clear_dist_and_bearing_to_target();
-        return false;
-    }
-    Vector3p target_position_NED;
-    Vector3f veh_vel;  // NED
-    if (!get_target_position_and_velocity(target_position_NED, veh_vel)) {
-        clear_dist_and_bearing_to_target();
+    if (!update_estimate()) {
         return false;
     }
 
-    // convert from Vector3p to Vector3f as they can't be subtracted:
-    Vector3f target_position_NED_f;
-    target_position_NED_f.x = target_position_NED.x;
-    target_position_NED_f.y = target_position_NED.y;
-    target_position_NED_f.z = target_position_NED.z;
+    pos_ofs_ned_m = _ofs_estimate_pos_ned_m;
+    vel_ofs_ned_ms = _ofs_estimate_vel_ned_ms;
+    accel_ofs_ned_mss = _ofs_estimate_accel_ned_mss;
 
-    const Vector3f dist_vec = target_position_NED_f - current_position_NED;
+    return true;
+}
 
-    // fail if too far
-    if (is_positive(_dist_max.get()) && (dist_vec.length() > _dist_max)) {
-        clear_dist_and_bearing_to_target();
+// Retrieves distance vectors (with and without configured offsets) and the target’s velocity, all in the NED frame.
+bool AP_Follow::get_target_dist_and_vel_NED_m(Vector3f &dist_ned, Vector3f &dist_with_offs, Vector3f &vel_ned)
+{
+    if (!update_estimate()) {
         return false;
     }
 
-    // initialise offsets from distance vector if required
-    init_offsets_if_required(dist_vec);
-
-    // get offsets
-    Vector3f offsets;
-    if (!get_offsets_ned(offsets)) {
+    Vector3f current_position_ned_m;
+    if (!AP::ahrs().get_relative_position_NED_origin(current_position_ned_m)) {
         clear_dist_and_bearing_to_target();
         return false;
     }
 
-    // calculate results
-    dist_ned = dist_vec;
-    dist_with_offs = dist_vec + offsets;
-    vel_ned = veh_vel;
+    const Vector3p dist_vec_ned_m = _estimate_pos_ned_m - current_position_ned_m.topostype();
+    const Vector3p ofs_dist_vec = _ofs_estimate_pos_ned_m - current_position_ned_m.topostype();
+    dist_ned = dist_vec_ned_m.tofloat();
+    dist_with_offs = ofs_dist_vec.tofloat();
+    vel_ned = _ofs_estimate_vel_ned_ms;
 
-    // record distance and heading for reporting purposes
-    if (is_zero(dist_with_offs.x) && is_zero(dist_with_offs.y)) {
-        clear_dist_and_bearing_to_target();
-    } else {
-        _dist_to_target = safe_sqrt(sq(dist_with_offs.x) + sq(dist_with_offs.y));
-        _bearing_to_target = degrees(atan2f(dist_with_offs.y, dist_with_offs.x));
+    return true;
+}
+
+// Retrieves the estimated target heading and heading rate in radians.
+bool AP_Follow::get_heading_heading_rate_rad(float &heading_rad, float &heading_rate_rads)
+{
+    if (!update_estimate()) {
+        return false;
     }
 
+    // return latest heading estimate
+    heading_rad = _estimate_heading_rad;
+    heading_rate_rads = _estimate_heading_rate_rads;
     return true;
 }
 
-// get target's heading in degrees (0 = north, 90 = east)
-bool AP_Follow::get_target_heading_deg(float &heading) const
+// Retrieves the target's estimated global location and velocity, adjusting altitude frame if relative mode is set (for LUA bindings).
+bool AP_Follow::get_target_location_and_velocity(Location &loc, Vector3f &vel_ned)
 {
-    // exit immediately if not enabled
-    if (!_enabled) {
+    if (!update_estimate()) {
         return false;
     }
 
-    // check for timeout
-    if ((_last_heading_update_ms == 0) || (AP_HAL::millis() - _last_heading_update_ms > AP_FOLLOW_TIMEOUT_MS)) {
+    if (!AP::ahrs().get_location_from_origin_offset_NED(loc, _estimate_pos_ned_m)) {
         return false;
     }
+    vel_ned = _estimate_vel_ned_ms;
+
+    if (_alt_type == AP_FOLLOW_ALTITUDE_TYPE_RELATIVE) {
+        loc.change_alt_frame(Location::AltFrame::ABOVE_HOME);
+    }
 
-    // return latest heading estimate
-    heading = _target_heading;
     return true;
 }
 
-// returns true if we should extract information from msg
-bool AP_Follow::should_handle_message(const mavlink_message_t &msg) const
+// Retrieves the target's estimated global location and velocity, including configured offsets, for LUA bindings.
+bool AP_Follow::get_target_location_and_velocity_ofs(Location &loc, Vector3f &vel_ned)
 {
-    // exit immediately if not enabled
-    if (!_enabled) {
+    if (!update_estimate()) {
         return false;
     }
-
-    // skip our own messages
-    if (msg.sysid == mavlink_system.sysid) {
+    if (!AP::ahrs().get_location_from_origin_offset_NED(loc, _ofs_estimate_pos_ned_m)) {
         return false;
     }
+    if (_alt_type == AP_FOLLOW_ALTITUDE_TYPE_RELATIVE) {
+        loc.change_alt_frame(Location::AltFrame::ABOVE_HOME);
+    }
 
-    // skip message if not from our target
-    if (_sysid != 0 && msg.sysid != _sysid) {
+    vel_ned = _ofs_estimate_vel_ned_ms;
+    return true;
+}
+
+// Retrieves the estimated target heading in degrees (0° = North, 90° = East) for LUA bindings.
+bool AP_Follow::get_target_heading_deg(float &heading)
+{
+    if (!update_estimate()) {
         return false;
     }
 
+    // return latest heading estimate
+    heading = degrees(_estimate_heading_rad);
     return true;
 }
 
-// handle mavlink DISTANCE_SENSOR messages
+
+//==============================================================================
+// MAVLink Message Handling
+//==============================================================================
+
+// Handles incoming MAVLink messages to update the target's position, velocity, and heading.
 void AP_Follow::handle_msg(const mavlink_message_t &msg)
 {
-    // this method should be called from an "update()" method:
     if (_automatic_sysid) {
-        // maybe timeout who we were following...
+        // timeout check: if no position update received within timeout, clear automatic sysid
         if ((_last_location_update_ms == 0) ||
             (AP_HAL::millis() - _last_location_update_ms > AP_FOLLOW_SYSID_TIMEOUT_MS)) {
             _sysid.set(0);
+            _sysid_used = 0;
+            _estimate_valid = false;
         }
     }
 
     if (!should_handle_message(msg)) {
+        // ignore message if filtering rules reject it (e.g., wrong sysid)
         return;
     }
 
-    // decode global-position-int message
+    // decode MAVLink message
     bool updated = false;
 
     switch (msg.msgid) {
     case MAVLINK_MSG_ID_GLOBAL_POSITION_INT: {
+        // handle standard global position messages
         updated = handle_global_position_int_message(msg);
         break;
     }
     case MAVLINK_MSG_ID_FOLLOW_TARGET: {
+        // handle follow-target specific messages
         updated = handle_follow_target_message(msg);
         break;
     }
     }
 
     if (updated) {
+        // message provided updated position information
+        Vector3f offset_m = _offset_XXD_m.get();
+
+        // if using relative frame, rotate offsets based on estimated heading
+        if (_offset_type == AP_FOLLOW_OFFSET_TYPE_RELATIVE) {
+            offset_m.xy().rotate(_estimate_heading_rad);
+        }
+
+        Vector3f current_position_ned_m;
+        if (!AP::ahrs().get_relative_position_NED_origin(current_position_ned_m)) {
+            // 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
+            const Vector3p dist_vec_ned_m = _estimate_pos_ned_m - current_position_ned_m.topostype();
+            const Vector3p ofs_dist_vec = _ofs_estimate_pos_ned_m - current_position_ned_m.topostype();
+
+            // initialise offsets dynamically if needed
+            init_offsets_if_required(dist_vec_ned_m.tofloat());
+
+            // record distance and bearing to target for reporting/logging
+            if (ofs_dist_vec.xy().is_zero()) {
+                clear_dist_and_bearing_to_target();
+            } else {
+                _dist_to_target_m = ofs_dist_vec.xy().length();
+                _bearing_to_target_deg = degrees(ofs_dist_vec.xy().angle());
path: libraries/AP_Follow/AP_Follow.cpp
user: login: lthall
body: The original code was:
```
    // record distance and heading_deg for reporting purposes
    if (is_zero(dist_with_offs_m.x) && is_zero(dist_with_offs_m.y)) {
        clear_dist_and_bearing_to_target();
    } else {
        _dist_to_target_m = safe_sqrt(sq(dist_with_offs_m.x) + sq(dist_with_offs_m.y));
        _bearing_to_target_deg = degrees(atan2f(dist_with_offs_m.y, dist_with_offs_m.x));
    }
```

So it was the offset location.
created_at: 2025-05-15T11:19:44Z
author_association: CONTRIBUTOR
diff_hunk: @@ -134,419 +155,638 @@ const AP_Param::GroupInfo AP_Follow::var_info[] = {
     AP_GROUPINFO("_ALT_TYPE", 10, AP_Follow, _alt_type, AP_FOLLOW_ALT_TYPE_DEFAULT),
 #endif
 
+// May add option to turn on input shaping, turn on acceleration due to rotation of offsest
+
     // @Param: _OPTIONS
     // @DisplayName: Follow options
     // @Description: Follow options bitmask
     // @Values: 0:None,1: Mount Follows lead vehicle on mode enter
     // @User: Standard
     AP_GROUPINFO("_OPTIONS", 11, AP_Follow, _options, 0),
 
+    // @Param: _ACCEL_NE
+    // @DisplayName: Acceleration limit for the horizontal kinematic input shaping
+    // @Description: Acceleration limit of the horizontal kinematic path generation used to determine how quickly the ship varies in velocity
+    // @Range: 0 5
+    // @Units: m/s/s
+    // @User: Advanced
+    AP_GROUPINFO("_ACCEL_NE", 12, AP_Follow, _accel_max_ne_mss, 2.5),
+
+    // @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 ship 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),
+
+    // @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 ship 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),
+
+    // @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 ship 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),
+
+    // @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 ship 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),
+
+    // @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 ship 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_GROUPEND
 };
 
-/* 
-   The constructor also initialises the proximity sensor. Note that this
-   constructor is not called until detect() returns true, so we
-   already know that we should setup the proximity sensor
-*/
+// Constructor for AP_Follow. Initializes the position P-controller and sets up parameter defaults.
 AP_Follow::AP_Follow() :
-        _p_pos(AP_FOLLOW_POS_P_DEFAULT)
+    _p_pos(AP_FOLLOW_POS_P_DEFAULT)
 {
     _singleton = this;
     AP_Param::setup_object_defaults(this, var_info);
 }
 
-// restore offsets to zero if necessary, should be called when vehicle exits follow mode
-void AP_Follow::clear_offsets_if_required()
+
+//==============================================================================
+// Target Estimation Update Functions
+//==============================================================================
+
+// Projects and updates the estimated target position, velocity, and heading based on last known data and configured input shaping.
+bool AP_Follow::update_estimate()
 {
-    if (_offsets_were_zero) {
-        _offset.set(Vector3f());
-        _offsets_were_zero = false;
+    // exit immediately if not enabled
+    if (!_enabled) {
+        _estimate_valid = false;
+        return _estimate_valid;
     }
-}
 
-// get target's estimated location
-bool AP_Follow::get_target_location_and_velocity(Location &loc, Vector3f &vel_ned) const
-{
-    Vector3p pos_ned;
-    Vector3f local_vel_ned;  // so we either change both return parameters or neither
-    if (!get_target_position_and_velocity(pos_ned, local_vel_ned)) {
-        return false;
+    // check for timeout: if no recent location update, invalidate estimate
+    if ((_last_location_update_ms == 0) || (AP_HAL::millis() - _last_location_update_ms > AP_FOLLOW_TIMEOUT_MS)) {
+        _estimate_valid = false;
+        return _estimate_valid;
     }
-    if (!AP::ahrs().get_location_from_origin_offset_NED(loc, pos_ned)) {
-        return false;
+
+    // skip if update has already been processed this scheduler tick
+    if (_estimate_valid && (AP::scheduler().ticks32() - _last_update_ticks == 0)) {
+        return _estimate_valid;
     }
-    vel_ned = local_vel_ned;
+    _last_update_ticks = AP::scheduler().ticks32();
 
-    if (_alt_type == AP_FOLLOW_ALTITUDE_TYPE_RELATIVE) {
-        loc.change_alt_frame(Location::AltFrame::ABOVE_HOME);
+    // if sysid changed, reset the estimation state
+    if (_sysid != _sysid_used) {
+        _sysid_used = _sysid;
+        _estimate_valid = false;
     }
 
-    return true;
-}
+    const uint32_t now = AP_HAL::millis();
 
-// get target's estimated location and velocity, both NED SI from origin
-bool AP_Follow::get_target_position_and_velocity(Vector3p &pos_ned, Vector3f &vel_ned) const
-{
-    // exit immediately if not enabled
-    if (!_enabled) {
-        return false;
+    // calculate time since last location update in seconds
+    const float dt = (now - _last_location_update_ms) * 0.001f;
+
+    // project target's position and velocity forward using simple kinematics
+    Vector3f delta_pos = _target_vel_ned_ms * dt + _target_accel_ned_mss * 0.5f * sq(dt);
+    Vector3f delta_vel = _target_accel_ned_mss * dt;
+
+    // calculate time since last estimation update
+    const float e_dt = (now - _last_estimation_update_ms) * 0.001f;
+    
+    if (_estimate_valid) {
+        // 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)
+        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.xy().topostype(), _target_vel_ned_ms.xy() + delta_vel.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 angular shaping for heading estimate
+        shape_angle_vel_accel(radians(_target_heading_deg), radians(_target_heading_rate_degs), 0.0,
+                              _estimate_heading_rad, _estimate_heading_rate_rads, _estimate_heading_accel_radss,
+                              0.0, radians(_accel_max_h_degss),
+                              radians(_jerk_max_h_degsss), e_dt, false);
+
+        // update heading angle separately to maintain proper wrapping [-PI, PI]
+        postype_t estimate_heading_rad = _estimate_heading_rad;
+        update_pos_vel_accel(estimate_heading_rad, _estimate_heading_rate_rads, _estimate_heading_accel_radss, e_dt, 0.0, 0.0, 0.0);
+        _estimate_heading_rad = wrap_PI(estimate_heading_rad);
+    } else {
+        // no valid estimate yet: initialise from latest target position
+        _estimate_pos_ned_m = _target_pos_ned_m + delta_pos.topostype();
+        _estimate_vel_ned_ms = _target_vel_ned_ms + delta_vel;
+        _estimate_accel_ned_mss = _target_accel_ned_mss;
+        _estimate_valid = true;
     }
 
-    // check for timeout
-    if ((_last_location_update_ms == 0) || (AP_HAL::millis() - _last_location_update_ms > AP_FOLLOW_TIMEOUT_MS)) {
-        return false;
+    Vector3f offset_m = _offset_XXD_m.get();
+
+    // calculate estimated position and velocity with offsets applied
+    if (offset_m.is_zero() || (_offset_type == AP_FOLLOW_OFFSET_TYPE_NED)) {
+        // offsets are in NED frame: simple addition
+        _ofs_estimate_pos_ned_m = _estimate_pos_ned_m + offset_m.topostype();
+        _ofs_estimate_vel_ned_ms = _estimate_vel_ned_ms;
+        _ofs_estimate_accel_ned_mss = _estimate_accel_ned_mss;
+    } else {
+        // offsets are in FRD frame: rotate by heading
+        offset_m.xy().rotate(_estimate_heading_rad);
+        Vector3f offset_cross = offset_m.cross(Vector3f{0.0, 0.0, 1.0});
+        float offset_length_m = offset_m.length();
+        _ofs_estimate_pos_ned_m = _estimate_pos_ned_m + offset_m.topostype();
+        _ofs_estimate_vel_ned_ms = _estimate_vel_ned_ms + offset_cross * offset_length_m * _estimate_heading_rate_rads;
+        _ofs_estimate_accel_ned_mss = _estimate_accel_ned_mss + offset_cross * offset_length_m * _estimate_heading_accel_radss;
     }
 
-    // calculate time since last actual position update
-    const float dt = (AP_HAL::millis() - _last_location_update_ms) * 0.001f;
+    _last_estimation_update_ms = now;
+    return _estimate_valid;
+}
+
 
-    // get velocity estimate
-    if (!get_velocity_ned(vel_ned, dt)) {
+//==============================================================================
+// Target Information Retrieval Functions
+//==============================================================================
+
+// Retrieves the estimated target position, velocity, and acceleration in the NED frame (relative to origin).
+bool AP_Follow::get_target_pos_vel_accel_NED_m(Vector3p &pos_ned_m, Vector3f &vel_ned_ms, Vector3f &accel_ned_mss)
+{
+    if (!update_estimate()) {
         return false;
     }
 
-    // project the vehicle position
-    const Vector3p vel_ned_p { vel_ned.x, vel_ned.y, vel_ned.z };
-    pos_ned = _target_position_ned + vel_ned_p * dt;
+    pos_ned_m = _estimate_pos_ned_m;
+    vel_ned_ms = _estimate_vel_ned_ms;
+    accel_ned_mss = _estimate_accel_ned_mss;
 
     return true;
 }
 
-// get distance vector to target (in meters) and target's velocity all in NED frame
-bool AP_Follow::get_target_dist_and_vel_ned(Vector3f &dist_ned, Vector3f &dist_with_offs, Vector3f &vel_ned)
+// Retrieves the estimated target position, velocity, and acceleration in the NED frame, including configured offsets.
+bool AP_Follow::get_ofs_pos_vel_accel_NED_m(Vector3p &pos_ofs_ned_m, Vector3f &vel_ofs_ned_ms, Vector3f &accel_ofs_ned_mss)
 {
-    Vector3f current_position_NED;
-    if (!AP::ahrs().get_relative_position_NED_origin(current_position_NED)) {
-        clear_dist_and_bearing_to_target();
-        return false;
-    }
-    Vector3p target_position_NED;
-    Vector3f veh_vel;  // NED
-    if (!get_target_position_and_velocity(target_position_NED, veh_vel)) {
-        clear_dist_and_bearing_to_target();
+    if (!update_estimate()) {
         return false;
     }
 
-    // convert from Vector3p to Vector3f as they can't be subtracted:
-    Vector3f target_position_NED_f;
-    target_position_NED_f.x = target_position_NED.x;
-    target_position_NED_f.y = target_position_NED.y;
-    target_position_NED_f.z = target_position_NED.z;
+    pos_ofs_ned_m = _ofs_estimate_pos_ned_m;
+    vel_ofs_ned_ms = _ofs_estimate_vel_ned_ms;
+    accel_ofs_ned_mss = _ofs_estimate_accel_ned_mss;
 
-    const Vector3f dist_vec = target_position_NED_f - current_position_NED;
+    return true;
+}
 
-    // fail if too far
-    if (is_positive(_dist_max.get()) && (dist_vec.length() > _dist_max)) {
-        clear_dist_and_bearing_to_target();
+// Retrieves distance vectors (with and without configured offsets) and the target’s velocity, all in the NED frame.
+bool AP_Follow::get_target_dist_and_vel_NED_m(Vector3f &dist_ned, Vector3f &dist_with_offs, Vector3f &vel_ned)
+{
+    if (!update_estimate()) {
         return false;
     }
 
-    // initialise offsets from distance vector if required
-    init_offsets_if_required(dist_vec);
-
-    // get offsets
-    Vector3f offsets;
-    if (!get_offsets_ned(offsets)) {
+    Vector3f current_position_ned_m;
+    if (!AP::ahrs().get_relative_position_NED_origin(current_position_ned_m)) {
         clear_dist_and_bearing_to_target();
         return false;
     }
 
-    // calculate results
-    dist_ned = dist_vec;
-    dist_with_offs = dist_vec + offsets;
-    vel_ned = veh_vel;
+    const Vector3p dist_vec_ned_m = _estimate_pos_ned_m - current_position_ned_m.topostype();
+    const Vector3p ofs_dist_vec = _ofs_estimate_pos_ned_m - current_position_ned_m.topostype();
+    dist_ned = dist_vec_ned_m.tofloat();
+    dist_with_offs = ofs_dist_vec.tofloat();
+    vel_ned = _ofs_estimate_vel_ned_ms;
 
-    // record distance and heading for reporting purposes
-    if (is_zero(dist_with_offs.x) && is_zero(dist_with_offs.y)) {
-        clear_dist_and_bearing_to_target();
-    } else {
-        _dist_to_target = safe_sqrt(sq(dist_with_offs.x) + sq(dist_with_offs.y));
-        _bearing_to_target = degrees(atan2f(dist_with_offs.y, dist_with_offs.x));
+    return true;
+}
+
+// Retrieves the estimated target heading and heading rate in radians.
+bool AP_Follow::get_heading_heading_rate_rad(float &heading_rad, float &heading_rate_rads)
+{
+    if (!update_estimate()) {
+        return false;
     }
 
+    // return latest heading estimate
+    heading_rad = _estimate_heading_rad;
+    heading_rate_rads = _estimate_heading_rate_rads;
     return true;
 }
 
-// get target's heading in degrees (0 = north, 90 = east)
-bool AP_Follow::get_target_heading_deg(float &heading) const
+// Retrieves the target's estimated global location and velocity, adjusting altitude frame if relative mode is set (for LUA bindings).
+bool AP_Follow::get_target_location_and_velocity(Location &loc, Vector3f &vel_ned)
 {
-    // exit immediately if not enabled
-    if (!_enabled) {
+    if (!update_estimate()) {
         return false;
     }
 
-    // check for timeout
-    if ((_last_heading_update_ms == 0) || (AP_HAL::millis() - _last_heading_update_ms > AP_FOLLOW_TIMEOUT_MS)) {
+    if (!AP::ahrs().get_location_from_origin_offset_NED(loc, _estimate_pos_ned_m)) {
         return false;
     }
+    vel_ned = _estimate_vel_ned_ms;
+
+    if (_alt_type == AP_FOLLOW_ALTITUDE_TYPE_RELATIVE) {
+        loc.change_alt_frame(Location::AltFrame::ABOVE_HOME);
+    }
 
-    // return latest heading estimate
-    heading = _target_heading;
     return true;
 }
 
-// returns true if we should extract information from msg
-bool AP_Follow::should_handle_message(const mavlink_message_t &msg) const
+// Retrieves the target's estimated global location and velocity, including configured offsets, for LUA bindings.
+bool AP_Follow::get_target_location_and_velocity_ofs(Location &loc, Vector3f &vel_ned)
 {
-    // exit immediately if not enabled
-    if (!_enabled) {
+    if (!update_estimate()) {
         return false;
     }
-
-    // skip our own messages
-    if (msg.sysid == mavlink_system.sysid) {
+    if (!AP::ahrs().get_location_from_origin_offset_NED(loc, _ofs_estimate_pos_ned_m)) {
         return false;
     }
+    if (_alt_type == AP_FOLLOW_ALTITUDE_TYPE_RELATIVE) {
+        loc.change_alt_frame(Location::AltFrame::ABOVE_HOME);
+    }
 
-    // skip message if not from our target
-    if (_sysid != 0 && msg.sysid != _sysid) {
+    vel_ned = _ofs_estimate_vel_ned_ms;
+    return true;
+}
+
+// Retrieves the estimated target heading in degrees (0° = North, 90° = East) for LUA bindings.
+bool AP_Follow::get_target_heading_deg(float &heading)
+{
+    if (!update_estimate()) {
         return false;
     }
 
+    // return latest heading estimate
+    heading = degrees(_estimate_heading_rad);
     return true;
 }
 
-// handle mavlink DISTANCE_SENSOR messages
+
+//==============================================================================
+// MAVLink Message Handling
+//==============================================================================
+
+// Handles incoming MAVLink messages to update the target's position, velocity, and heading.
 void AP_Follow::handle_msg(const mavlink_message_t &msg)
 {
-    // this method should be called from an "update()" method:
     if (_automatic_sysid) {
-        // maybe timeout who we were following...
+        // timeout check: if no position update received within timeout, clear automatic sysid
         if ((_last_location_update_ms == 0) ||
             (AP_HAL::millis() - _last_location_update_ms > AP_FOLLOW_SYSID_TIMEOUT_MS)) {
             _sysid.set(0);
+            _sysid_used = 0;
+            _estimate_valid = false;
         }
     }
 
     if (!should_handle_message(msg)) {
+        // ignore message if filtering rules reject it (e.g., wrong sysid)
         return;
     }
 
-    // decode global-position-int message
+    // decode MAVLink message
     bool updated = false;
 
     switch (msg.msgid) {
     case MAVLINK_MSG_ID_GLOBAL_POSITION_INT: {
+        // handle standard global position messages
         updated = handle_global_position_int_message(msg);
         break;
     }
     case MAVLINK_MSG_ID_FOLLOW_TARGET: {
+        // handle follow-target specific messages
         updated = handle_follow_target_message(msg);
         break;
     }
     }
 
     if (updated) {
+        // message provided updated position information
+        Vector3f offset_m = _offset_XXD_m.get();
+
+        // if using relative frame, rotate offsets based on estimated heading
+        if (_offset_type == AP_FOLLOW_OFFSET_TYPE_RELATIVE) {
+            offset_m.xy().rotate(_estimate_heading_rad);
+        }
+
+        Vector3f current_position_ned_m;
+        if (!AP::ahrs().get_relative_position_NED_origin(current_position_ned_m)) {
+            // 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
+            const Vector3p dist_vec_ned_m = _estimate_pos_ned_m - current_position_ned_m.topostype();
+            const Vector3p ofs_dist_vec = _ofs_estimate_pos_ned_m - current_position_ned_m.topostype();
+
+            // initialise offsets dynamically if needed
+            init_offsets_if_required(dist_vec_ned_m.tofloat());
+
+            // record distance and bearing to target for reporting/logging
+            if (ofs_dist_vec.xy().is_zero()) {
+                clear_dist_and_bearing_to_target();
+            } else {
+                _dist_to_target_m = ofs_dist_vec.xy().length();
path: libraries/AP_Follow/AP_Follow.cpp
user: login: lthall
body: Fixed
created_at: 2025-05-15T12:31:55Z
author_association: CONTRIBUTOR
diff_hunk: @@ -313,7 +313,7 @@ void shape_pos_vel_accel(postype_t pos_input, float vel_input, float accel_input
     vel_target += vel_input;
 
     // limit velocity between vel_min and vel_max
-    if (limit_total) {
+    if (limit_total && (is_negative(vel_min) || is_positive(vel_max))) {
path: libraries/AP_Math/control.cpp
user: login: timtuxworth
body: This test would pass if vel_min > vel_max as long as vel_max was positive, or if vel_min is negative and vel_max < vel_min. I don't know what the constrain would return in that case. Perhaps you mean || to be && or alternatively you need to add && vel_min < vel_max.
created_at: 2025-05-19T02:31:41Z
author_association: CONTRIBUTOR
diff_hunk: @@ -313,7 +313,7 @@ void shape_pos_vel_accel(postype_t pos_input, float vel_input, float accel_input
     vel_target += vel_input;
 
     // limit velocity between vel_min and vel_max
-    if (limit_total) {
+    if (limit_total && (is_negative(vel_min) || is_positive(vel_max))) {
path: libraries/AP_Math/control.cpp
user: login: lthall
body: On line 282 we ensure that:
- vel_min is negative or zero, 
- vel_max is positive or zero.

Here we ensure that one of them is not zero. So we can't get a situation where vel_min > vel_max.

I do see the point that this may not be as clear as it could be. I could add a comment or I could change the check to:
` if (limit_total && is_positive(vel_max-vel_min)) {`

Or both?
created_at: 2025-05-19T06:16:03Z
author_association: CONTRIBUTOR
diff_hunk: @@ -1568,6 +1568,7 @@ def ShipLanding(self):
             "Q_WP_SPEED": 700,
             "SIM_SHIP_DSIZE": 10,
             "FOLL_ENABLE": 1,
+            "FOLL_DIST_MAX": 1000,
path: Tools/autotest/quadplane.py
user: login: tridge
body: we should discuss if we should require this. The test passes without it, but with a lot of "No target location/velocity" messages. Previously the APIs that ship landing used didn't depend on FOLL_DIST_MAX being set
created_at: 2025-05-19T22:50:43Z
author_association: CONTRIBUTOR
diff_hunk: @@ -20,7 +20,7 @@ bool ModeFollow::init(const bool ignore_checks)
         gcs().send_text(MAV_SEVERITY_WARNING, "Set FOLL_ENABLE = 1");
         return false;
     }
-
+    
path: ArduCopter/mode_follow.cpp
user: login: IamPete1
body: ```suggestion

```
created_at: 2025-05-19T23:26:51Z
author_association: MEMBER
diff_hunk: @@ -134,419 +155,634 @@ const AP_Param::GroupInfo AP_Follow::var_info[] = {
     AP_GROUPINFO("_ALT_TYPE", 10, AP_Follow, _alt_type, AP_FOLLOW_ALT_TYPE_DEFAULT),
 #endif
 
+// May add option to turn on input shaping, turn on acceleration due to rotation of offset
+
     // @Param: _OPTIONS
     // @DisplayName: Follow options
     // @Description: Follow options bitmask
     // @Values: 0:None,1: Mount Follows lead vehicle on mode enter
     // @User: Standard
     AP_GROUPINFO("_OPTIONS", 11, AP_Follow, _options, 0),
 
+    // @Param: _ACCEL_NE
+    // @DisplayName: Acceleration limit for the horizontal kinematic input shaping
+    // @Description: Acceleration limit of the horizontal kinematic path generation used to determine how quickly the ship varies in velocity
path: libraries/AP_Follow/AP_Follow.cpp
user: login: IamPete1
body: ```suggestion
    // @Description: Acceleration limit of the horizontal kinematic path generation used to determine how quickly the target varies in velocity
```
Not everyone is landing on ships ;)
created_at: 2025-05-19T23:29:40Z
author_association: MEMBER
diff_hunk: @@ -134,419 +155,634 @@ const AP_Param::GroupInfo AP_Follow::var_info[] = {
     AP_GROUPINFO("_ALT_TYPE", 10, AP_Follow, _alt_type, AP_FOLLOW_ALT_TYPE_DEFAULT),
 #endif
 
+// May add option to turn on input shaping, turn on acceleration due to rotation of offset
+
     // @Param: _OPTIONS
     // @DisplayName: Follow options
     // @Description: Follow options bitmask
     // @Values: 0:None,1: Mount Follows lead vehicle on mode enter
     // @User: Standard
     AP_GROUPINFO("_OPTIONS", 11, AP_Follow, _options, 0),
 
+    // @Param: _ACCEL_NE
+    // @DisplayName: Acceleration limit for the horizontal kinematic input shaping
+    // @Description: Acceleration limit of the horizontal kinematic path generation used to determine how quickly the ship varies in velocity
+    // @Range: 0 5
+    // @Units: m/s/s
+    // @User: Advanced
+    AP_GROUPINFO("_ACCEL_NE", 12, AP_Follow, _accel_max_ne_mss, 2.5),
+
+    // @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 ship varies in acceleration
path: libraries/AP_Follow/AP_Follow.cpp
user: login: IamPete1
body: ship again 
created_at: 2025-05-19T23:29:56Z
author_association: MEMBER
diff_hunk: @@ -134,419 +155,634 @@ const AP_Param::GroupInfo AP_Follow::var_info[] = {
     AP_GROUPINFO("_ALT_TYPE", 10, AP_Follow, _alt_type, AP_FOLLOW_ALT_TYPE_DEFAULT),
 #endif
 
+// May add option to turn on input shaping, turn on acceleration due to rotation of offset
+
     // @Param: _OPTIONS
     // @DisplayName: Follow options
     // @Description: Follow options bitmask
     // @Values: 0:None,1: Mount Follows lead vehicle on mode enter
     // @User: Standard
     AP_GROUPINFO("_OPTIONS", 11, AP_Follow, _options, 0),
 
+    // @Param: _ACCEL_NE
+    // @DisplayName: Acceleration limit for the horizontal kinematic input shaping
+    // @Description: Acceleration limit of the horizontal kinematic path generation used to determine how quickly the ship varies in velocity
+    // @Range: 0 5
+    // @Units: m/s/s
+    // @User: Advanced
+    AP_GROUPINFO("_ACCEL_NE", 12, AP_Follow, _accel_max_ne_mss, 2.5),
+
+    // @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 ship 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),
+
+    // @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 ship varies in velocity
path: libraries/AP_Follow/AP_Follow.cpp
user: login: IamPete1
body: ship again 
created_at: 2025-05-19T23:30:00Z
author_association: MEMBER
diff_hunk: @@ -134,419 +155,634 @@ const AP_Param::GroupInfo AP_Follow::var_info[] = {
     AP_GROUPINFO("_ALT_TYPE", 10, AP_Follow, _alt_type, AP_FOLLOW_ALT_TYPE_DEFAULT),
 #endif
 
+// May add option to turn on input shaping, turn on acceleration due to rotation of offset
+
     // @Param: _OPTIONS
     // @DisplayName: Follow options
     // @Description: Follow options bitmask
     // @Values: 0:None,1: Mount Follows lead vehicle on mode enter
     // @User: Standard
     AP_GROUPINFO("_OPTIONS", 11, AP_Follow, _options, 0),
 
+    // @Param: _ACCEL_NE
+    // @DisplayName: Acceleration limit for the horizontal kinematic input shaping
+    // @Description: Acceleration limit of the horizontal kinematic path generation used to determine how quickly the ship varies in velocity
+    // @Range: 0 5
+    // @Units: m/s/s
+    // @User: Advanced
+    AP_GROUPINFO("_ACCEL_NE", 12, AP_Follow, _accel_max_ne_mss, 2.5),
+
+    // @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 ship 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),
+
+    // @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 ship 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),
+
+    // @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 ship varies in acceleration
path: libraries/AP_Follow/AP_Follow.cpp
user: login: IamPete1
body: ship
created_at: 2025-05-19T23:30:10Z
author_association: MEMBER
diff_hunk: @@ -134,419 +155,634 @@ const AP_Param::GroupInfo AP_Follow::var_info[] = {
     AP_GROUPINFO("_ALT_TYPE", 10, AP_Follow, _alt_type, AP_FOLLOW_ALT_TYPE_DEFAULT),
 #endif
 
+// May add option to turn on input shaping, turn on acceleration due to rotation of offset
+
     // @Param: _OPTIONS
     // @DisplayName: Follow options
     // @Description: Follow options bitmask
     // @Values: 0:None,1: Mount Follows lead vehicle on mode enter
     // @User: Standard
     AP_GROUPINFO("_OPTIONS", 11, AP_Follow, _options, 0),
 
+    // @Param: _ACCEL_NE
+    // @DisplayName: Acceleration limit for the horizontal kinematic input shaping
+    // @Description: Acceleration limit of the horizontal kinematic path generation used to determine how quickly the ship varies in velocity
+    // @Range: 0 5
+    // @Units: m/s/s
+    // @User: Advanced
+    AP_GROUPINFO("_ACCEL_NE", 12, AP_Follow, _accel_max_ne_mss, 2.5),
+
+    // @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 ship 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),
+
+    // @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 ship 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),
+
+    // @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 ship 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),
+
+    // @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 ship varies in angular velocity
path: libraries/AP_Follow/AP_Follow.cpp
user: login: IamPete1
body: ship
created_at: 2025-05-19T23:30:14Z
author_association: MEMBER
diff_hunk: @@ -134,419 +155,634 @@ const AP_Param::GroupInfo AP_Follow::var_info[] = {
     AP_GROUPINFO("_ALT_TYPE", 10, AP_Follow, _alt_type, AP_FOLLOW_ALT_TYPE_DEFAULT),
 #endif
 
+// May add option to turn on input shaping, turn on acceleration due to rotation of offset
+
     // @Param: _OPTIONS
     // @DisplayName: Follow options
     // @Description: Follow options bitmask
     // @Values: 0:None,1: Mount Follows lead vehicle on mode enter
     // @User: Standard
     AP_GROUPINFO("_OPTIONS", 11, AP_Follow, _options, 0),
 
+    // @Param: _ACCEL_NE
+    // @DisplayName: Acceleration limit for the horizontal kinematic input shaping
+    // @Description: Acceleration limit of the horizontal kinematic path generation used to determine how quickly the ship varies in velocity
+    // @Range: 0 5
+    // @Units: m/s/s
+    // @User: Advanced
+    AP_GROUPINFO("_ACCEL_NE", 12, AP_Follow, _accel_max_ne_mss, 2.5),
+
+    // @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 ship 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),
+
+    // @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 ship 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),
+
+    // @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 ship 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),
+
+    // @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 ship 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),
+
+    // @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 ship varies in angular acceleration
path: libraries/AP_Follow/AP_Follow.cpp
user: login: IamPete1
body: ship
created_at: 2025-05-19T23:30:17Z
author_association: MEMBER
diff_hunk: @@ -613,13 +910,16 @@ bool AP_Follow::have_target(void) const
     return true;
 }
 
-namespace AP {
-
-AP_Follow &follow()
-{
-    return *AP_Follow::get_singleton();
-}
+//==============================================================================
+// AP_Follow Accessor
+//==============================================================================
 
+// Accessor for the AP_Follow singleton instance.
+namespace AP {
+    AP_Follow &follow()
+    {
+        return *AP_Follow::get_singleton();
+    }
 }
 
-#endif  // AP_FOLLOW_ENABLED
+#endif  // AP_FOLLOW_ENABLED
path: libraries/AP_Follow/AP_Follow.cpp
user: login: IamPete1
body: ```suggestion
#endif  // AP_FOLLOW_ENABLED

```
created_at: 2025-05-19T23:32:32Z
author_association: MEMBER
diff_hunk: @@ -153,33 +186,71 @@ class AP_Follow
     // write out an onboard-log message to help diagnose follow problems:
     void Log_Write_FOLL();
 
-    // parameters
-    AP_Int8     _enabled;           // 1 if this subsystem is enabled
-    AP_Int16    _sysid;             // target's mavlink system id (0 to use first sysid seen)
-    AP_Float    _dist_max_m;          // maximum distance to target.  targets further than this will be ignored
-    AP_Int8     _offset_type;       // offset frame type (0:North-East-Down, 1:RelativeToLeadVehicleHeading)
-    AP_Vector3f _offset_ned_m;            // offset from lead vehicle in meters
-    AP_Int8     _yaw_behave;        // following vehicle's yaw/heading behaviour (see YAW_BEHAVE enum)
-    AP_Int8     _alt_type;          // altitude source for follow mode
-    AC_P        _p_pos;             // position error P controller
-    AP_Int16    _options;           // options for mount behaviour follow mode
-
-    // local variables
-    uint32_t _last_location_update_ms;  // system time of last position update
-    Vector3p _target_position_ned_m;  // last known position of target
-    Vector3f _target_velocity_ned_ms;  // last known velocity of target in NED frame in m/s
-    Vector3f _target_accel_ned_mss;     // last known acceleration of target in NED frame in m/s/s
-    uint32_t _last_heading_update_ms;   // system time of last heading update
-    float _target_heading_deg;          // heading in degrees
-    bool _automatic_sysid;          // did we lock onto a sysid automatically?
-    float _dist_to_target_m;          // latest distance to target in meters (for reporting purposes)
-    float _bearing_to_target_deg;       // latest bearing to target in degrees (for reporting purposes)
-    bool _offsets_were_zero;        // true if offsets were originally zero and then initialised to the offset from lead vehicle
-
-    // setup jitter correction with max transport lag of 3s
-    JitterCorrection _jitter{3000};
+    //==========================================================================
+    // Parameters
+    //==========================================================================
+
+    AP_Int8     _enabled;
+    AP_Int16    _sysid;
+    AP_Float    _dist_max_m;
+    AP_Int8     _offset_type;
+    AP_Vector3f _offset_XXD_m;
path: libraries/AP_Follow/AP_Follow.h
user: login: IamPete1
body: XXD? XYD?
created_at: 2025-05-19T23:33:20Z
author_association: MEMBER
diff_hunk: @@ -352,13 +352,69 @@ void shape_pos_vel_accel_xy(const Vector2p& pos_input, const Vector2f& vel_input
     vel_target = vel_target + vel_input;
     
     // limit velocity to vel_max
-    if (limit_total) {
+    if (limit_total && is_positive(vel_max)) {
         vel_target.limit_length(vel_max);
     }
 
     shape_vel_accel_xy(vel_target, accel_input, vel, accel, accel_max, jerk_max, dt, limit_total);
 }
 
+/* shape_angle_vel_accel calculate a jerk limited path from the current angle, angular velocity and angular acceleration to an input angle, angular velocity and angular acceleration.           
+ The function takes the current angle, angular velocity, and angular acceleration and calculates the required jerk limited adjustment to the angular acceleration for the next time dt.
+ The kinematic path is constrained by :
+    maximum angular velocity - angle_vel_max,
+    maximum angular acceleration - angle_accel_max,
+    maximum angular jerk - angle_jerk_max.
+ The function alters the variable accel to follow a jerk limited kinematic path to angle_input, angle_vel_input and angle_accel_input.
+ The angle_vel_max limit can be removed by setting the desired limit to zero.
+ The correction angular acceleration can is limited to angle_accel_max. If limit_total_accel is true the total angular acceleration is limited to angle_accel_max.
+*/            
+void shape_angle_vel_accel(float angle_input, float angle_vel_input, float angle_accel_input,
+                         float angle, float angle_vel, float& angle_accel,
+                         float angle_vel_max, float angle_accel_max,
+                         float angle_jerk_max, float dt, bool limit_total)
+{
+    // sanity check accel_max
+    if (!is_positive(angle_accel_max)) {
+        INTERNAL_ERROR(AP_InternalError::error_t::invalid_arg_or_result);
+        return;
+    }
+
+    // calculate angle error to be corrected
+    // unwrap angle error to achieve fastest path
+    float stopping_time = fabsf(angle_vel / angle_accel_max);
+    float angle_error = angle_input - angle - angle_vel * stopping_time;
+    angle_error = wrap_PI(angle_error);
+    angle_error += angle_vel * stopping_time;
+
+    // Calculate time constants and limits to ensure stable operation
+    // The negative acceleration limit is used here because the square root controller
+    // manages the approach to the setpoint. Therefore the acceleration is in the opposite
+    // direction to the position error.
+    float angle_accel_tc_max;
+    float KPv;
+    angle_accel_tc_max = 0.5 * angle_accel_max;
+    KPv = 0.5 * angle_jerk_max / angle_accel_max;
path: libraries/AP_Math/control.cpp
user: login: IamPete1
body: ```suggestion
    const float angle_accel_tc_max = 0.5 * angle_accel_max;
    const float KPv = 0.5 * angle_jerk_max / angle_accel_max;
```
Const if possible. 
created_at: 2025-05-19T23:34:59Z
author_association: MEMBER
diff_hunk: @@ -153,33 +186,71 @@ class AP_Follow
     // write out an onboard-log message to help diagnose follow problems:
     void Log_Write_FOLL();
 
-    // parameters
-    AP_Int8     _enabled;           // 1 if this subsystem is enabled
-    AP_Int16    _sysid;             // target's mavlink system id (0 to use first sysid seen)
-    AP_Float    _dist_max_m;          // maximum distance to target.  targets further than this will be ignored
-    AP_Int8     _offset_type;       // offset frame type (0:North-East-Down, 1:RelativeToLeadVehicleHeading)
-    AP_Vector3f _offset_ned_m;            // offset from lead vehicle in meters
-    AP_Int8     _yaw_behave;        // following vehicle's yaw/heading behaviour (see YAW_BEHAVE enum)
-    AP_Int8     _alt_type;          // altitude source for follow mode
-    AC_P        _p_pos;             // position error P controller
-    AP_Int16    _options;           // options for mount behaviour follow mode
-
-    // local variables
-    uint32_t _last_location_update_ms;  // system time of last position update
-    Vector3p _target_position_ned_m;  // last known position of target
-    Vector3f _target_velocity_ned_ms;  // last known velocity of target in NED frame in m/s
-    Vector3f _target_accel_ned_mss;     // last known acceleration of target in NED frame in m/s/s
-    uint32_t _last_heading_update_ms;   // system time of last heading update
-    float _target_heading_deg;          // heading in degrees
-    bool _automatic_sysid;          // did we lock onto a sysid automatically?
-    float _dist_to_target_m;          // latest distance to target in meters (for reporting purposes)
-    float _bearing_to_target_deg;       // latest bearing to target in degrees (for reporting purposes)
-    bool _offsets_were_zero;        // true if offsets were originally zero and then initialised to the offset from lead vehicle
-
-    // setup jitter correction with max transport lag of 3s
-    JitterCorrection _jitter{3000};
+    //==========================================================================
+    // Parameters
+    //==========================================================================
+
+    AP_Int8     _enabled;
+    AP_Int16    _sysid;
+    AP_Float    _dist_max_m;
+    AP_Int8     _offset_type;
+    AP_Vector3f _offset_XXD_m;
+    AP_Int8     _yaw_behave;
+    AP_Int8     _alt_type;
+    AC_P        _p_pos;
+    AP_Int16    _options;
+    AP_Float    _accel_max_ne_mss;
path: libraries/AP_Follow/AP_Follow.h
user: login: rmackay9
body: overall this looks pretty good but I wonder why we've removed all the comments to the right of the variables and parameters?
created_at: 2025-05-19T23:54:35Z
author_association: CONTRIBUTOR
diff_hunk: @@ -1568,6 +1568,7 @@ def ShipLanding(self):
             "Q_WP_SPEED": 700,
             "SIM_SHIP_DSIZE": 10,
             "FOLL_ENABLE": 1,
+            "FOLL_DIST_MAX": 1000,
path: Tools/autotest/quadplane.py
user: login: tridge
body: maybe default FOLL_DIST_MAX to 0 on plane? and make 0 mean no limit?
created_at: 2025-05-19T23:57:33Z
author_association: CONTRIBUTOR
diff_hunk: @@ -134,419 +155,634 @@ const AP_Param::GroupInfo AP_Follow::var_info[] = {
     AP_GROUPINFO("_ALT_TYPE", 10, AP_Follow, _alt_type, AP_FOLLOW_ALT_TYPE_DEFAULT),
 #endif
 
+// May add option to turn on input shaping, turn on acceleration due to rotation of offset
path: libraries/AP_Follow/AP_Follow.cpp
user: login: rmackay9
body: we should probably prefix, "ToDo:" on this comment
created_at: 2025-05-20T00:00:35Z
author_association: CONTRIBUTOR
diff_hunk: @@ -134,419 +155,634 @@ const AP_Param::GroupInfo AP_Follow::var_info[] = {
     AP_GROUPINFO("_ALT_TYPE", 10, AP_Follow, _alt_type, AP_FOLLOW_ALT_TYPE_DEFAULT),
 #endif
 
+// May add option to turn on input shaping, turn on acceleration due to rotation of offset
+
     // @Param: _OPTIONS
     // @DisplayName: Follow options
     // @Description: Follow options bitmask
     // @Values: 0:None,1: Mount Follows lead vehicle on mode enter
     // @User: Standard
     AP_GROUPINFO("_OPTIONS", 11, AP_Follow, _options, 0),
 
+    // @Param: _ACCEL_NE
+    // @DisplayName: Acceleration limit for the horizontal kinematic input shaping
+    // @Description: Acceleration limit of the horizontal kinematic path generation used to determine how quickly the ship varies in velocity
+    // @Range: 0 5
+    // @Units: m/s/s
+    // @User: Advanced
+    AP_GROUPINFO("_ACCEL_NE", 12, AP_Follow, _accel_max_ne_mss, 2.5),
+
+    // @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 ship 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),
+
+    // @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 ship 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),
+
+    // @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 ship 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),
+
+    // @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 ship 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),
+
+    // @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 ship varies in angular acceleration
path: libraries/AP_Follow/AP_Follow.cpp
user: login: lthall
body: Thanks, good catch!
created_at: 2025-05-20T01:00:21Z
author_association: CONTRIBUTOR
diff_hunk: @@ -153,33 +186,71 @@ class AP_Follow
     // write out an onboard-log message to help diagnose follow problems:
     void Log_Write_FOLL();
 
-    // parameters
-    AP_Int8     _enabled;           // 1 if this subsystem is enabled
-    AP_Int16    _sysid;             // target's mavlink system id (0 to use first sysid seen)
-    AP_Float    _dist_max_m;          // maximum distance to target.  targets further than this will be ignored
-    AP_Int8     _offset_type;       // offset frame type (0:North-East-Down, 1:RelativeToLeadVehicleHeading)
-    AP_Vector3f _offset_ned_m;            // offset from lead vehicle in meters
-    AP_Int8     _yaw_behave;        // following vehicle's yaw/heading behaviour (see YAW_BEHAVE enum)
-    AP_Int8     _alt_type;          // altitude source for follow mode
-    AC_P        _p_pos;             // position error P controller
-    AP_Int16    _options;           // options for mount behaviour follow mode
-
-    // local variables
-    uint32_t _last_location_update_ms;  // system time of last position update
-    Vector3p _target_position_ned_m;  // last known position of target
-    Vector3f _target_velocity_ned_ms;  // last known velocity of target in NED frame in m/s
-    Vector3f _target_accel_ned_mss;     // last known acceleration of target in NED frame in m/s/s
-    uint32_t _last_heading_update_ms;   // system time of last heading update
-    float _target_heading_deg;          // heading in degrees
-    bool _automatic_sysid;          // did we lock onto a sysid automatically?
-    float _dist_to_target_m;          // latest distance to target in meters (for reporting purposes)
-    float _bearing_to_target_deg;       // latest bearing to target in degrees (for reporting purposes)
-    bool _offsets_were_zero;        // true if offsets were originally zero and then initialised to the offset from lead vehicle
-
-    // setup jitter correction with max transport lag of 3s
-    JitterCorrection _jitter{3000};
+    //==========================================================================
+    // Parameters
+    //==========================================================================
+
+    AP_Int8     _enabled;
+    AP_Int16    _sysid;
+    AP_Float    _dist_max_m;
+    AP_Int8     _offset_type;
+    AP_Vector3f _offset_XXD_m;
path: libraries/AP_Follow/AP_Follow.h
user: login: lthall
body: Changed to _offset_m
created_at: 2025-05-20T01:03:13Z
author_association: CONTRIBUTOR
diff_hunk: @@ -141,412 +164,625 @@ const AP_Param::GroupInfo AP_Follow::var_info[] = {
     // @User: Standard
     AP_GROUPINFO("_OPTIONS", 11, AP_Follow, _options, 0),
 
+    // @Param: _ACCEL_NE
+    // @DisplayName: Acceleration limit for the horizontal kinematic input shaping
+    // @Description: Acceleration limit of the horizontal kinematic path generation used to determine how quickly the estimate varies in velocity
+    // @Range: 0 5
+    // @Units: m/s/s
+    // @User: Advanced
+    AP_GROUPINFO("_ACCEL_NE", 12, AP_Follow, _accel_max_ne_mss, 2.5),
+
+    // @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),
+
+    // @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),
+
+    // @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),
+
+    // @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),
+
+    // @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_GROUPEND
 };
 
-/* 
-   The constructor also initialises the proximity sensor. Note that this
-   constructor is not called until detect() returns true, so we
-   already know that we should setup the proximity sensor
-*/
+// Constructor for AP_Follow. Initializes the position P-controller and sets up parameter defaults.
 AP_Follow::AP_Follow() :
-        _p_pos(AP_FOLLOW_POS_P_DEFAULT)
+    _p_pos(AP_FOLLOW_POS_P_DEFAULT)
 {
     _singleton = this;
     AP_Param::setup_object_defaults(this, var_info);
 }
 
-// restore offsets to zero if necessary, should be called when vehicle exits follow mode
-void AP_Follow::clear_offsets_if_required()
+
+//==============================================================================
+// Target Estimation Update Functions
+//==============================================================================
+
+// Projects and updates the estimated target position, velocity, and heading based on last known data and configured input shaping.
+bool AP_Follow::update_estimate()
 {
-    if (_offsets_were_zero) {
-        _offset_ned_m.set(Vector3f());
-        _offsets_were_zero = false;
+    // check for target: if no valid target, invalidate estimate
+    if (!have_target()) {
+        clear_dist_and_bearing_to_target();
+        _estimate_valid = false;
+        return _estimate_valid;
     }
-}
 
-// get target's estimated location
-bool AP_Follow::get_target_location_and_velocity(Location &loc, Vector3f &vel_ned_ms) const
-{
-    Vector3p pos_ned_m;
-    Vector3f local_vel_ned_m;  // so we either change both return parameters or neither
-    if (!get_target_position_and_velocity_NED_m(pos_ned_m, local_vel_ned_m)) {
-        return false;
+    // skip if update has already been processed this scheduler tick
+    if (_estimate_valid && (AP::scheduler().ticks32() - _last_update_ticks == 0)) {
+        return _estimate_valid;
     }
-    if (!AP::ahrs().get_location_from_origin_offset_NED(loc, pos_ned_m)) {
-        return false;
+    _last_update_ticks = AP::scheduler().ticks32();
+
+    // if sysid changed, reset the estimation state
+    if (_sysid != _sysid_used) {
+        _sysid_used = _sysid;
+        _estimate_valid = false;
     }
-    vel_ned_ms = local_vel_ned_m;
 
-    if (_alt_type == AP_FOLLOW_ALTITUDE_TYPE_RELATIVE) {
-        loc.change_alt_frame(Location::AltFrame::ABOVE_HOME);
+    const uint32_t now = AP_HAL::millis();
+
+    // calculate time since last location update in seconds
+    const float dt = (now - _last_location_update_ms) * 0.001f;
+
+    // project target's position and velocity forward using simple kinematics
+    Vector3f delta_pos = _target_vel_ned_ms * dt + _target_accel_ned_mss * 0.5f * sq(dt);
+    Vector3f delta_vel = _target_accel_ned_mss * dt;
+
+    // calculate time since last estimation update
+    const float e_dt = (now - _last_estimation_update_ms) * 0.001f;
+
+    const bool valid_kinematic_params = (_accel_max_ne_mss > 0.0f) && (_jerk_max_ne_msss > 0.0f) &&
+                                (_accel_max_d_mss > 0.0f) && (_jerk_max_d_msss > 0.0f) &&
+                                (_accel_max_h_degss > 0.0f) && (_jerk_max_h_degsss > 0.0f);
+    
+    if (_estimate_valid && valid_kinematic_params) {
+        // 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)
+        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.xy().topostype(), _target_vel_ned_ms.xy() + delta_vel.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 angular shaping for heading estimate
+        shape_angle_vel_accel(radians(_target_heading_deg), radians(_target_heading_rate_degs), 0.0,
+                              _estimate_heading_rad, _estimate_heading_rate_rads, _estimate_heading_accel_radss,
+                              0.0, radians(_accel_max_h_degss),
+                              radians(_jerk_max_h_degsss), e_dt, false);
+
+        // update heading angle separately to maintain proper wrapping [-PI, PI]
+        postype_t estimate_heading_rad = _estimate_heading_rad;
+        update_pos_vel_accel(estimate_heading_rad, _estimate_heading_rate_rads, _estimate_heading_accel_radss, e_dt, 0.0, 0.0, 0.0);
+        _estimate_heading_rad = wrap_PI(estimate_heading_rad);
+    } else {
+        // no valid estimate yet: initialise from latest target position
+        _estimate_pos_ned_m = _target_pos_ned_m + delta_pos.topostype();
+        _estimate_vel_ned_ms = _target_vel_ned_ms + delta_vel;
+        _estimate_accel_ned_mss = _target_accel_ned_mss;
+        _estimate_valid = true;
path: libraries/AP_Follow/AP_Follow.cpp
user: login: IamPete1
body: I think we need to update `_estimate_heading_rad` and `_estimate_heading_rate_rads` here too. A easy test would be to see what happens when the new input shaping params are invalid. 

I think it need this:
```
_estimate_heading_rad = radians(_target_heading_deg);
_estimate_heading_rate_rads = radians(_target_heading_rate_degs);
```
created_at: 2025-05-20T12:25:02Z
author_association: MEMBER
diff_hunk: @@ -141,412 +164,625 @@ const AP_Param::GroupInfo AP_Follow::var_info[] = {
     // @User: Standard
     AP_GROUPINFO("_OPTIONS", 11, AP_Follow, _options, 0),
 
+    // @Param: _ACCEL_NE
+    // @DisplayName: Acceleration limit for the horizontal kinematic input shaping
+    // @Description: Acceleration limit of the horizontal kinematic path generation used to determine how quickly the estimate varies in velocity
+    // @Range: 0 5
+    // @Units: m/s/s
+    // @User: Advanced
+    AP_GROUPINFO("_ACCEL_NE", 12, AP_Follow, _accel_max_ne_mss, 2.5),
+
+    // @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),
+
+    // @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),
+
+    // @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),
+
+    // @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),
+
+    // @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_GROUPEND
 };
 
-/* 
-   The constructor also initialises the proximity sensor. Note that this
-   constructor is not called until detect() returns true, so we
-   already know that we should setup the proximity sensor
-*/
+// Constructor for AP_Follow. Initializes the position P-controller and sets up parameter defaults.
 AP_Follow::AP_Follow() :
-        _p_pos(AP_FOLLOW_POS_P_DEFAULT)
+    _p_pos(AP_FOLLOW_POS_P_DEFAULT)
 {
     _singleton = this;
     AP_Param::setup_object_defaults(this, var_info);
 }
 
-// restore offsets to zero if necessary, should be called when vehicle exits follow mode
-void AP_Follow::clear_offsets_if_required()
+
+//==============================================================================
+// Target Estimation Update Functions
+//==============================================================================
+
+// Projects and updates the estimated target position, velocity, and heading based on last known data and configured input shaping.
+bool AP_Follow::update_estimate()
 {
-    if (_offsets_were_zero) {
-        _offset_ned_m.set(Vector3f());
-        _offsets_were_zero = false;
+    // check for target: if no valid target, invalidate estimate
+    if (!have_target()) {
+        clear_dist_and_bearing_to_target();
+        _estimate_valid = false;
+        return _estimate_valid;
     }
-}
 
-// get target's estimated location
-bool AP_Follow::get_target_location_and_velocity(Location &loc, Vector3f &vel_ned_ms) const
-{
-    Vector3p pos_ned_m;
-    Vector3f local_vel_ned_m;  // so we either change both return parameters or neither
-    if (!get_target_position_and_velocity_NED_m(pos_ned_m, local_vel_ned_m)) {
-        return false;
+    // skip if update has already been processed this scheduler tick
+    if (_estimate_valid && (AP::scheduler().ticks32() - _last_update_ticks == 0)) {
+        return _estimate_valid;
     }
-    if (!AP::ahrs().get_location_from_origin_offset_NED(loc, pos_ned_m)) {
-        return false;
+    _last_update_ticks = AP::scheduler().ticks32();
+
+    // if sysid changed, reset the estimation state
+    if (_sysid != _sysid_used) {
+        _sysid_used = _sysid;
+        _estimate_valid = false;
     }
-    vel_ned_ms = local_vel_ned_m;
 
-    if (_alt_type == AP_FOLLOW_ALTITUDE_TYPE_RELATIVE) {
-        loc.change_alt_frame(Location::AltFrame::ABOVE_HOME);
+    const uint32_t now = AP_HAL::millis();
+
+    // calculate time since last location update in seconds
+    const float dt = (now - _last_location_update_ms) * 0.001f;
+
+    // project target's position and velocity forward using simple kinematics
+    Vector3f delta_pos = _target_vel_ned_ms * dt + _target_accel_ned_mss * 0.5f * sq(dt);
+    Vector3f delta_vel = _target_accel_ned_mss * dt;
+
+    // calculate time since last estimation update
+    const float e_dt = (now - _last_estimation_update_ms) * 0.001f;
+
+    const bool valid_kinematic_params = (_accel_max_ne_mss > 0.0f) && (_jerk_max_ne_msss > 0.0f) &&
+                                (_accel_max_d_mss > 0.0f) && (_jerk_max_d_msss > 0.0f) &&
+                                (_accel_max_h_degss > 0.0f) && (_jerk_max_h_degsss > 0.0f);
+    
+    if (_estimate_valid && valid_kinematic_params) {
+        // 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)
+        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.xy().topostype(), _target_vel_ned_ms.xy() + delta_vel.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 angular shaping for heading estimate
+        shape_angle_vel_accel(radians(_target_heading_deg), radians(_target_heading_rate_degs), 0.0,
+                              _estimate_heading_rad, _estimate_heading_rate_rads, _estimate_heading_accel_radss,
+                              0.0, radians(_accel_max_h_degss),
+                              radians(_jerk_max_h_degsss), e_dt, false);
+
+        // update heading angle separately to maintain proper wrapping [-PI, PI]
+        postype_t estimate_heading_rad = _estimate_heading_rad;
+        update_pos_vel_accel(estimate_heading_rad, _estimate_heading_rate_rads, _estimate_heading_accel_radss, e_dt, 0.0, 0.0, 0.0);
+        _estimate_heading_rad = wrap_PI(estimate_heading_rad);
+    } else {
+        // no valid estimate yet: initialise from latest target position
+        _estimate_pos_ned_m = _target_pos_ned_m + delta_pos.topostype();
+        _estimate_vel_ned_ms = _target_vel_ned_ms + delta_vel;
+        _estimate_accel_ned_mss = _target_accel_ned_mss;
+        _estimate_valid = true;
path: libraries/AP_Follow/AP_Follow.cpp
user: login: lthall
body: Great catch!
created_at: 2025-05-20T13:18:52Z
author_association: CONTRIBUTOR
diff_hunk: @@ -313,7 +313,7 @@ void shape_pos_vel_accel(postype_t pos_input, float vel_input, float accel_input
     vel_target += vel_input;
 
     // limit velocity between vel_min and vel_max
-    if (limit_total) {
+    if (limit_total && (is_negative(vel_min) || is_positive(vel_max))) {
path: libraries/AP_Math/control.cpp
user: login: lthall
body: ` if (limit_total && (vel_max > vel_min) {`
created_at: 2025-05-21T08:08:05Z
author_association: CONTRIBUTOR

PR comments:
user: login: timtuxworth
created_at: 2025-05-02T14:13:10Z
body: > We need to decide on how we choose between the different data message sources. Ideally I would like to change the name and extend the _ALT_TYPE parameter to be 0: GPI absolute, 1: GPI relative, 2: Follow Target (GPI = global position int).

I'd like to see the _ALT_TYPE parameter align with Location::AltFrame, and in particular, ABOVE_TERRAIN = 3. Following at terrain altitude is an important use case. I've already included this change in my Plane Follow PR.

> We also need to decide what the options and default settings should be. We can leave the kinimatic interpolation off by default, we may choose not to process acceleration to reduce noise.

Why would we choose to leave kinematic off?

author_association: CONTRIBUTOR
user: login: lthall
created_at: 2025-05-02T14:21:57Z
body: > Following at terrain altitude is an important use case. I've already included this change in my Plane Follow PR.

Currently I have no plans to handle terain altitude. I am not sure how we would specify this.

> Why would we choose to leave kinematic off?

The main reason is to maintain the current behaviour.
author_association: CONTRIBUTOR
user: login: timtuxworth
created_at: 2025-05-03T00:25:23Z
body: > > Following at terrain altitude is an important use case. I've already included this change in my Plane Follow PR.
> 
> Currently I have no plans to handle terain altitude. I am not sure how we would specify this.

I do it one of two ways, depending on a parameter
1. Explicitly specify (via parameter) a fixed altitude value that the following vehicle will use for it's terrain altitude or
2. Use change_alt_frame to lookup the terrain altitude of the target (with offsets) before using this value as the target altitude for the follow vehicle

author_association: CONTRIBUTOR
user: login: lthall
created_at: 2025-05-03T12:38:47Z
body: > > > Following at terrain altitude is an important use case. I've already included this change in my Plane Follow PR.
> > 
> > 
> > Currently I have no plans to handle terain altitude. I am not sure how we would specify this.
> 
> I do it one of two ways, depending on a parameter
> 
> 1. Explicitly specify (via parameter) a fixed altitude value that the following vehicle will use for it's terrain altitude or
> 2. Use change_alt_frame to lookup the terrain altitude of the target (with offsets) before using this value as the target altitude for the follow vehicle

I really don't understand why you would do this. What does this enable?
author_association: CONTRIBUTOR
user: login: timtuxworth
created_at: 2025-05-04T00:57:24Z
body: > > > > Following at terrain altitude is an important use case. I've already included this change in my Plane Follow PR.
> > > 
> > > 
> > > Currently I have no plans to handle terain altitude. I am not sure how we would specify this.
> > 
> > 
> > I do it one of two ways, depending on a parameter
> > 
> > 1. Explicitly specify (via parameter) a fixed altitude value that the following vehicle will use for it's terrain altitude or
> > 2. Use change_alt_frame to lookup the terrain altitude of the target (with offsets) before using this value as the target altitude for the follow vehicle
> 
> I really don't understand why you would do this. What does this enable?

I work with two clients who use fixed wing VTOLs and quadcopter for magnetometry surveys. For them it's very important that each vehicle to follow the terrain at a specific height, often around 40m AGL. For use case #1 the planes are typically flying over mountainous areas. The two planes fly with an Y offset of around 50m -75m and the terrain height can be quite different even only 50m apart. It would give invalid results if the two drones flew at the same AMSL altitude. For example if the two planes are flying along a valley, one must fly much higher to be 40m above the ground if it is flying further up the slope. To be clear as well, because the terrain undulates, the vehicle cannot fly AMSL as it will likely hit something and maintaining a consistent 40m above the ground is important for the correct data to be collected.

For the second use case, the client flies quadcopter drones over remote areas. They fly with magnetometers hanging below the drone and need (again) to maintain 40m AGL. The terrain is not as extreme as the first use case, but flying multiple quadcopters allows them to collect more data faster, but each quad must fly a consistent path above the terrain at it's own location. This client asked me to write a Lua script to overcome the existing Copter follow code to allow it to follow AGL, but it would be much better IMO if the c++ code knew how to follow the terrain, this script is really a bit of a hack. 
https://github.com/timtuxworth/ardupilot_scripts/blob/main/copter-follow-terrain.lua 



author_association: CONTRIBUTOR
user: login: lthall
created_at: 2025-05-04T02:02:14Z
body: > > > > > Following at terrain altitude is an important use case. I've already included this change in my Plane Follow PR.
> > > > 
> > > > 
> > > > Currently I have no plans to handle terain altitude. I am not sure how we would specify this.
> > > 
> > > 
> > > I do it one of two ways, depending on a parameter
> > > 
> > > 1. Explicitly specify (via parameter) a fixed altitude value that the following vehicle will use for it's terrain altitude or
> > > 2. Use change_alt_frame to lookup the terrain altitude of the target (with offsets) before using this value as the target altitude for the follow vehicle
> > 
> > 
> > I really don't understand why you would do this. What does this enable?
> 
> I work with two clients who use fixed wing VTOLs and quadcopter for magnetometry surveys. For them it's very important that each vehicle to follow the terrain at a specific height, often around 40m AGL. For use case #1 the planes are typically flying over mountainous areas. The two planes fly with an Y offset of around 50m -75m and the terrain height can be quite different even only 50m apart. It would give invalid results if the two drones flew at the same AMSL altitude. For example if the two planes are flying along a valley, one must fly much higher to be 40m above the ground if it is flying further up the slope. To be clear as well, because the terrain undulates, the vehicle cannot fly AMSL as it will likely hit something and maintaining a consistent 40m above the ground is important for the correct data to be collected.
> 
> For the second use case, the client flies quadcopter drones over remote areas. They fly with magnetometers hanging below the drone and need (again) to maintain 40m AGL. The terrain is not as extreme as the first use case, but flying multiple quadcopters allows them to collect more data faster, but each quad must fly a consistent path above the terrain at it's own location. This client asked me to write a Lua script to overcome the existing Copter follow code to allow it to follow AGL, but it would be much better IMO if the c++ code knew how to follow the terrain, this script is really a bit of a hack. https://github.com/timtuxworth/ardupilot_scripts/blob/main/copter-follow-terrain.lua

Thanks for the application example. I can see how that could be important. However, I think we are getting out of the scope of the current follow messages. Neither GPI or FT messages provide terrain altitude. Terrain is also not subject to any kinematic limitations so it poses additional (but manageable) challenges.

As for follow mode terrain following in copter, that has some more obvious and elegant solutions. I have considered that it may be useful for aircraft to only follow in the horizontal while having manual control of altitude. This could be extended to use terrain following or use a default altitude. All this is a separate problem to this PR or the update to follow mode I have planned after this.
author_association: CONTRIBUTOR
user: login: timtuxworth
created_at: 2025-05-04T04:31:44Z
body: > As for follow mode terrain following in copter, that has some more obvious and elegent solutions. I have considered that it may be useful for aircraft to only follow in the horizontal while having manual control of altitude. This could be extended to use terrain following or use a default altitude. All this is a seperate problem to this PR or the update to follow mode I have planned after this.

Yes I agree it's outside the scope of this PR, just wanted to sure make the _ALT_TYPE parameter aligns with Location::AltFrame, to cover this future usage when we add Location:AltFrame:ABOVE_TERRAIN to the mix.
author_association: CONTRIBUTOR
user: login: lthall
created_at: 2025-05-16T05:00:08Z
body: I have opened up the test for the QuadPlane.ShipLanding test because it has a fundamental accuracy error. The test in this PR has the following errors in the east direction:

- Follow estimation ~ 10mm
- Position Control error ~ 36mm
- LUA based error ~ 1.85m

 ShipLanding (ship landing test) (Did not land within 1m of ship distance=2.297590104696072)
 
 I think this is caused by the use of velocity follow target without providing an instantaneous position target. So if the initial position is off then it will maintain that error.
 
 I have left the debug logging in https://github.com/lthall/Leonard_ardupilot/tree/20250424_Follow_Kinematic_Shaping_debug
author_association: CONTRIBUTOR
user: login: tridge
created_at: 2025-05-16T20:04:29Z
body: I think the key reason for the incorrect landing point for the ship landing is the AP_Follow::init_offsets_if_required() function. The quadplane ShipLanding autotest gets the distance from the location of the ship and the aircraft, but the AP_Follow::init_offsets_if_required() function auto-initialises an offset, setting FOLL_OFS_{X,Y,Z}. The plane lands at 0.8 from the target if you add FOLL_OFS_X=0.01 to the set of parameters in the test, as this prevents the automatic offset code from running.

author_association: CONTRIBUTOR
user: login: lthall
created_at: 2025-05-17T11:38:05Z
body: > I think the key reason for the incorrect landing point for the ship landing is the AP_Follow::init_offsets_if_required() function. The quadplane ShipLanding autotest gets the distance from the location of the ship and the aircraft, but the AP_Follow::init_offsets_if_required() function auto-initialises an offset, setting FOLL_OFS_{X,Y,Z}. The plane lands at 0.8 from the target if you add FOLL_OFS_X=0.01 to the set of parameters in the test, as this prevents the automatic offset code from running.

I have confirmed this. I would have expected this to happen before but looking at the old code we only automatically set the offsets if we were using get_target_dist_and_vel_NED_m. The shiplanding LUA script is using get_target_location_and_velocity_ofs so it isn't set automatically if the offsets are zero.

The question is what do we do from here. This seems like a big inconsistency in the library. Do we:
1. Set it on all functions like I have here
2. Call the function in Follow Mode in copter (I assume plane doesn't use it in the same way)
3. Some other suggestion I can't think of???
author_association: CONTRIBUTOR
user: login: rmackay9
created_at: 2025-05-18T00:01:36Z
body: I haven't read everything above but in general Follow's behaviour of interpreting all zero offsets as meaning the user wants to follow at the current distance is not good and we could remove this defaulting.  It seemed like a good idea at the time but in case it helps we should change this behaviour to be used based on a parameter or or an argument in a mavlink message that initiates the follow behaviour
author_association: CONTRIBUTOR
user: login: timtuxworth
created_at: 2025-05-18T12:06:19Z
body: > I haven't read everything above but in general Follow's behaviour of interpreting all zero offsets as meaning the user wants to follow at the current distance is not good and we could remove this defaulting. It seemed like a good idea at the time but in case it helps we should change this behaviour to be used based on a parameter or or an argument in a mavlink message that initiates the follow behaviour

Makes sense @rmackay9, but in that case what should happen if the user doesn't set the offsets? Should it fail to follow? I could maybe add a pre-arm for this case to my scripted arming checks.
author_association: CONTRIBUTOR
user: login: lthall
created_at: 2025-05-18T23:27:02Z
body: > > I haven't read everything above but in general Follow's behaviour of interpreting all zero offsets as meaning the user wants to follow at the current distance is not good and we could remove this defaulting. It seemed like a good idea at the time but in case it helps we should change this behaviour to be used based on a parameter or or an argument in a mavlink message that initiates the follow behaviour
> 
> Makes sense @rmackay9, but in that case what should happen if the user doesn't set the offsets? Should it fail to follow? I could maybe add a pre-arm for this case to my scripted arming checks.

For this PR I have maintained the current behaviour but called it directly from the mode rather than automatically as part of only one interface.
author_association: CONTRIBUTOR
user: login: tridge
created_at: 2025-05-20T00:06:43Z
body: with defaulting FOLL_OFS_MAX to 0 on plane I'm happy, thanks!
author_association: CONTRIBUTOR
user: login: rmackay9
created_at: 2025-05-22T02:53:11Z
body: Merged, thanks!
author_association: CONTRIBUTOR
user: login: timtuxworth
created_at: 2025-05-22T23:35:22Z
body: Great to see this. I'll be running Plane Follow on this ASAP. Thanks @lthall !
author_association: CONTRIBUTOR