A camera gimbal on a film drone turns the camera towards a subject. Its controller logged the pointing error e (target angle minus camera angle, in degrees) every dt seconds. A proportional term Kp * e pushes harder the further off the camera points, so it is still pushing hard just before the camera arrives, and the camera swings past. A derivative term looks at how fast the error is changing:
slope = (e - e_previous) / dt (degrees per second; for the first sample, slope = 0)
u = Kp * e + Kd * slope (PD command)
When the camera is closing on the target the error is shrinking, the slope is negative and the derivative term brakes: it has the opposite sign to the proportional term and takes some push away before the camera gets there. That braking is what damps the swing.
Write pd_terms(errors, dt, Kp, Kd) that returns a tuple (commands, braking):
commands: the list of PD commands, one per error;braking: the number of samples in which the proportional term and the derivative term have opposite signs (both non-zero, one positive and one negative).
Examples
Input: errors = [10, 6, 3, 1, 0, -0.5], dt = 0.1, Kp = 2, Kd = 0.3
Output: ([20.0, 0.0, -3.0, -4.0, -3.0, -2.5], 3)
Explanation: the slopes are 0, -40, -30, -20, -10 and -5 degrees per second. At the second
sample the P term (12) and the D term (-12) cancel: the controller already lets go.
The samples 2, 3 and 4 brake; at sample 5 the P term is 0, and at sample 6 both are negative.
Input: errors = [], dt = 0.01, Kp = 1, Kd = 1
Output: ([], 0)
Constraints
- answers are compared with a tolerance of
1e-6; do not round
Goals
- Estimate the rate of change of the error from two neighbouring samples
- Add a derivative term to a proportional command
- See that the derivative term pushes against the proportional term while the error is closing