A delivery drone holds its altitude with a PID controller. When the pilot changes the target altitude from 2 m to 5 m, the error e = setpoint - altitude jumps by 3 m in a single sample. Its rate of change over that sample is enormous, so a derivative term computed from the error fires a huge one-sample spike of thrust, the derivative kick: the motors lurch, and the payload with them.
The cure: the setpoint is constant between the pilot's changes, so while it is constant the error changes exactly as fast as the altitude does, with the opposite sign. Use the measurement instead:
on the error: D = Kd * (e[k] - e[k-1]) / dt
on the measurement: D = -Kd * (y[k] - y[k-1]) / dt
where y is the altitude reading. The two agree at every sample where the setpoint did not change; only the kick is gone.
The drone logged its setpoints and its altitude readings (metres, one pair per sample, dt seconds apart). For both versions, compute the derivative term for every sample from the second on, and return the tuple (kick_error, kick_measurement): for each version, the derivative term with the largest magnitude, keeping its sign (in newtons). If several share the largest magnitude, take the earliest; with fewer than two samples, return (0.0, 0.0).
Write d_kick(setpoints, readings, dt, Kd).
Examples
Input: setpoints = [2, 2, 2, 5, 5, 5], readings = [2.0, 2.01, 1.99, 2.0, 2.1, 2.3],
dt = 0.02, Kd = 0.5
Output: (74.75, -5.0)
Explanation: at the step the error goes from 0.01 to 3.0, so D = 0.5 * 2.99 / 0.02 = 74.75 N.
Measured, the largest term is the climb from 2.1 to 2.3 m: -0.5 * 0.2 / 0.02 = -5 N, a brake.
Input: setpoints = [3, 3, 3], readings = [2.5, 2.6, 2.8], dt = 0.1, Kd = 1
Output: (-2.0, -2.0)
Explanation: without a setpoint change the two versions are the same.
Constraints
len(setpoints) == len(readings)- answers are compared with a tolerance of
1e-6; do not round
Goals
- Compute the derivative term from the error and from the measurement
- See that a step in the setpoint makes the error's derivative spike
- Explain why taking the derivative of the measurement removes the spike but keeps the braking