The UWB ranging gives you a distance and a bearing. That is enough to know where the person is relative to the wagon. But if you wire those numbers straight to the motors, the wagon jerks forward on every step and dies on every pause. It is not a follower, it is a twitch. What makes it trail smoothly instead is a spring-damper pursuit loop, and tuning it took us longer than anything else on the project.

Feeding distance straight to the motors

At 20 Hz you get a fresh (r, theta) every 50 ms. The obvious thing to do is compute error = r - target_distance and send that straight to acceleration. When the person walks, the error spikes and the motors fire. When they stop, it hits zero and the motors die. The wagon lunges and stalls with every footstep.

Walking is not smooth at the frame level. Each step is a little acceleration followed by a little deceleration. Track that literally and the wagon vibrates in place. You do not want the wagon to follow each step. You want it to follow the trend.

A good follower rides the average. It does not match every step – it matches the fact that you started walking.

The spring-damper model

Picture a spring between the person and the wagon. Its natural length is the target follow distance, say 1.5 m. When the person walks away, the spring stretches past 1.5 m and pulls the wagon forward. When the person stops, the spring settles back to its natural length and the wagon comes to rest.

The pull is proportional to how far off the distance is from target: Kp * (r - target_distance), where Kp is stiffness.

A spring alone bounces forever. The wagon overshoots, snaps back, overshoots again. A damper fixes that. It resists relative velocity, how fast the gap is changing, not the wagon’s own speed: Kd * d(error)/dt. It bleeds energy out of the oscillation so the wagon glides to a stop instead of yo-yoing.

Putting it in one line

# 1. distance smoothing
distance_filtered += alpha * (measured_distance - distance_filtered)
error = distance_filtered - target_distance

# 2. error rate from raw ranging (not the smoothed signal)
error_rate = (measured_distance - prev_raw) / dt   # avoids filter phase lag
prev_error = error
prev_raw = measured_distance

# 3. acceleration and velocity update
accel = Kp * error - Kd * error_rate
accel = clamp(accel, -max_decel, max_accel)   # brake hard, start gentle
v = max(0.0, v + accel * dt)   # non-negative: never reverse

# 4. steering with heading-based speed scaling
turn = Kh * bearing
v_actual = v * cos(radians(bearing))   # bearing in degrees; slow down when turning sharply
drive_motors(v_actual, turn)

Kp is how hard the wagon chases when it falls behind. Kd damps the rate of change of the distance error. Not the wagon’s speed – the gap’s speed. The distinction matters. Damp on the wagon’s own velocity and you get a steady-state lag that grows with walking speed: with Kp=2.4, Kd=1.8 at 1.2 m/s, the wagon settles 0.9 m behind the target and never closes. Damp on the error rate instead and the lag disappears, because once the gap is stable the error rate is zero and the damper pulls no force.

One subtlety in step 2: we compute error_rate from the raw UWB distance, not from the smoothed one. Differentiate the smoothed signal and its 250 ms lag passes straight into the derivative. The damper reacts late, loses its phase lead, and can amplify oscillation instead of killing it. The raw derivative is noisier, so we run it through a light filter (about 50 ms) before it hits Kd.

Step 4 scales forward speed by cos(bearing). If the tag is off to the side, the wagon slows down and turns first, then picks up speed once it is pointed at the tag. Skip this and a 90-degree turn at full speed sends the wagon swinging wide on an arc it cannot recover from.

Tuning the two knobs

It all comes down to two numbers. We spent three months on them, walking the same 20-meter path again and again with each attempt logged to an SD card.

Too much Kp, not enough Kd

Too much spring and the wagon lunges the moment you step away, overshoots, brakes hard, sits there. It makes people flinch. The current spikes are hard on the gearbox too.

Too much Kd, not enough Kp

Too much damper and it barely moves. You walk five meters before it even starts, and by then you are gone. It just sits there.

The sweet spot

With Kp=2.4, Kd=1.8, the wagon starts about a quarter-second after you do, eases into speed, and slows as it approaches the target. The theoretical damping ratio is about 0.58, which is underdamped – a 10% overshoot in the idealized model. In practice the smoothing filter and motor lag eat most of that, so the overshoot is hard to notice. When you stop, it coasts in over half a second and settles. It feels like a slightly elastic leash, not a rod.

The goal is not to match the person’s speed. It is to hold the distance. Those sound the same but they are not.

Why a little lag is the whole point

You could tune the loop to respond within one frame – zero delay. We tried. It is unsettling. A wagon that matches every micro-movement feels less like a tool and more like something is following you. Real followers, a dog on a leash, a friend behind you, always lag a little.

So we add target smoothing. Instead of chasing the raw UWB distance, the wagon chases a low-pass filtered version of it. The filtered signal trails the real distance by about 200-300 ms, which is the amount of lag that feels right. The wagon does not react to a single step. It reacts to the fact that you have started walking.

Five parameters we ended up with

Three months of testing on flat ground, grass, and gravel. These are the values that felt right for a 12 kg wagon at 1.5 m. A heavier wagon or a different gap will shift them, but this is where we landed:

# followwagon pursuit params (12 kg, 1.5 m target)
target_distance = 1.5     # meters
Kp = 2.4                   # spring stiffness
Kd = 1.8                   # damper
alpha = 0.15               # target smoothing (20 Hz)
max_accel = 1.2            # m/s^2 max forward accel
max_decel = 3.0            # m/s^2 max braking decel (stops faster than it starts)
max_jump = 0.2             # max allowed distance change per frame (m)

Starting and braking are not symmetric. The wagon eases into motion at max_accel = 1.2 but brakes at max_decel = 3.0. With a symmetric 1.2 limit, stopping from 1.5 m/s takes nearly a meter. At 3.0 it drops below 0.4 m. The gap matters more than the start.

A clamp does not stop a sustained glitch, though. If the UWB reports a bad reading for two seconds, the wagon brakes at the limit the whole time. So we also reject outliers. At 20 Hz, a person walking at 6 km/h changes distance by at most 0.083 m per frame. A jump over 0.2 m in one frame is not real, so we throw that frame out and hold the last good distance. The Kalman filter does a second pass of outlier rejection through residual gating.

Bearing: the other half of the problem

Distance control keeps the gap right. Steering is separate. The PDOA bearing tells the wagon where to point, and a simple proportional controller nudges the differential drive left or right.

# heading control (separate from distance)
turn = Kh * bearing          # bearing is already body-frame error
drive_motors(v, turn)

The bearing is already in body frame. It tells the wagon how far off its nose the tag sits, so no compass heading is needed. Turn until bearing points forward, then drive. Keeping the two loops separate makes tuning easier: a distance problem and a steering problem have different causes, and mixing them into one controller means you cannot fix one without breaking the other.

What this means for the kit

The loop runs on the wagon’s own controller, not on a phone or a server. It needs to close at 20 Hz minimum, which means the UWB, the IMU, and the motor driver all talk locally. That is why followwagon is self-contained: ranging, filtering, control, and drive all happen on the wagon. You carry the tag. The wagon handles the rest.

Take the loop out and the UWB still works. You still get a distance every 50 ms. But without the spring-damper and the smoothing, the wagon jerks with every step. The loop is what turns raw ranging into something that actually trails you.