AP_Follow: fix vertical estimate not tracking the target

[lthall]

Summary

AP_Follow: shape the vertical estimate toward the target — the missing 1-D shape_pos_vel_accel for Z (was open-loop, sawtoothing at ~5.6 m).
AP_Follow: fix distance and bearing double frame/unit conversion — drop the erroneous NEU→NED + cm→m on an already-metres-NED value.

Classification & Testing (check all that apply and add your own)

• Checked by a human programmer
• Non-functional change
• No-binary change
• Infrastructure change (e.g. unit tests, helper scripts)
• Automated test(s) verify changes (e.g. unit test, autotest)
• Tested manually, description below (e.g. SITL)
• Tested on hardware
• Logs attached
• Logs available on request

[Copilot]

Pull request overview

Note

Copilot was unable to run its full agentic suite in this review.

Fixes two issues in AP_Follow: the vertical position estimate was not being shaped toward the target (open-loop Z integration), and the distance/bearing calculation applied an erroneous NEU→NED and cm→m conversion to a value already in metres NED.

Changes:

• Add shape_pos_vel_accel call for the Z axis using D-axis accel/jerk limits.
• Remove duplicate NEU→NED and cm→m conversion in update_dist_and_bearing_to_target().
• Update comments to reflect the new behavior.

---

💡 Add Copilot custom instructions for smarter, more guided reviews. Learn how to get started.

[Copilot]

Pull request overview

Copilot reviewed 1 out of 1 changed files in this pull request and generated 2 comments.

[rmackay9]

I've tested this and it greatly improves the follower's vertical overshoot of the leader's altitude. Below are before-and-after screenshots of the two vehicle's altitudes. In the "before" we see the follower overshoots the leader's climb by about 5m

In "after we see the follower overshoots by <1m

In addition this fixed the Follower's position target lines drawn on the MAVProxy map. @lthall is this expected?

BTW, testing follow mode in SITL is relatively simple by starting two vehicles as described here on the dev wiki. The exact command I used was:

sim_vehicle.py -v Copter --map --console --count 2 --auto-sysid --location CMAC --auto-offset-line 90,10 --mcast

and then you can switch between the two vehicles using MP's console's "Vehicle" menu

[lthall]

The leader is transmitting position and velocity at 4 Hz, acceleration is zero because it isn't included in the GLOBAL_POSITION_INT.

The follower is configured with the same velocity and acceleration limits as the navigation aircraft, but without access to the leader’s acceleration profile. This means the follower is always tracking a delayed and under-specified trajectory.

As a result, any latency in the follow link cannot be recovered, because the follower has no additional acceleration or velocity authority available to catch back up to the leader’s trajectory. Likewise, any latency or delay in the real aircraft motion cannot be recovered either.

So while this change may reduce overshoot by improving the velocity feed-forward behaviour and reducing the apparent position step seen by the follower, it does not solve the underlying tracking limitation.

To address this I increased the follow model acceleration and jerk limit to twice the default navigation limits. This significantly improves default performance especially for low update rates and poorly chosen limits.

I have a PR to improve the logging data and an autotest to test each axis of the follow library #33164

Horizontal Position:

Vertical Position:

Heading: