Add option to use measurement derivative for PID

[TheTripleV]

In the current pid controller implementation, the derivative of error is used for the D term. This causes discontinuity in that value if the setpoint is changed on an active controller. Using the derivative of the measurement makes this continuous across setpoint changes.

[calcmogul]

We use the setpoint derivative instead of the measurement derivative on purpose. If you have a jump discontinuity in the setpoint, that means the setpoint is running away from you and you need to apply max control input to catch up.

Also, the error derivative and measurement derivative aren't always the same value. The former properly tracks a nonzero velocity setpoint while the latter does not. Given the PID derivation at https://docs.wpilib.org/en/stable/docs/software/advanced-controls/introduction/introduction-to-pid.html#derivative-term, using measurement derivative instead of error derivative is like zeroing out the setpoint derivative part of the Kd term. This means the velocity will always be driven to zero, which breaks motion profile tracking.

The discontinuity stems from a PD controller's velocity setpoint and measurement being implicitly computed from finite differences of the position setpoint and position measurement respectively. This can be avoided with separate position and velocity P controllers with individually settable setpoints and measurements; they would handle motion profiles correctly too. Maybe we could add this as a new wrapper class because it doesn't meet the definition of a PID controller.

[TheTripleV]

I think it should be an option in the existing PIDController class like how continuous input is. For non-continuous changes of the setpoint, I don't want mechanisms facing derivative kick. If anything, I think eliminating derivative kick by using measurement derivative should be the default with the option to switch to the error derivative since I imagine most user calls of PIDController.setSetpoint are not continuous.

Consider the case of a PID controller holding an elevator at 0 feet, then later being set to 3 feet. There shouldn't be a large derivate kick because the pid controller was already running.

Regarding motion profile tracking: The PID derivation you linked is

$$u = K_p (r - x) + K_d \left(\frac{dr}{dt} - \frac{dx}{dt}\right)$$

Since ProfiledPIDController calculates both the desired position and velocity, wouldn't it be better to directly supply the desired velocity into $\frac{dr}{dt}$ into that equation instead calculating the setpoint derivative? This would effectively be exactly what you stated

This can be avoided with separate position and velocity P controllers with individually settable setpoints and measurements

but with $K_d$ of the position controller and the $K_p$ of the velocity controller at the same value.

TLDR: I think it's ok to assume that the measurement is a continuous variable but it's not ok to assume the setpoint is a continuous variable.

[calcmogul]

Consider the case of a PID controller holding an elevator at 0 feet, then later being set to 3 feet. There shouldn't be a large derivate kick because the pid controller was already running.

I already explained why there should be. If you don't want a derivative kick, use a motion profile that moves the setpoint in a manner the elevator can actually achieve.

wouldn't it be better to directly supply the desired velocity into into that equation instead calculating the setpoint derivative?

A PID controller inherently computes the setpoint derivative internally. Doing otherwise by accepting a velocity setpoint from the user is no longer a PID controller. Again, we can make a separate class that takes setpoints for both, but it wouldn't be a canonical PID controller.

Also, making the suggested change to ProfiledPIDController causes code duplication since we can't reuse PIDController for the calculation anymore, and it doesn't fix PIDController being broken for motion profiles if it uses the measurement derivative.

[BenCaunt]

If one wants to use the velocity measurement as a derivative term isn't the same as full state back?

https://github.com/wpilibsuite/allwpilib/blob/main/wpimath/src/main/java/edu/wpi/first/math/controller/LinearQuadraticRegulator.java

You can just use the Linear Quadratic Regulator with the DcMotor model and achieve that rather than modifying the PID controller.

Perhaps some sort of profiled LQR controller for SISO systems might be useful.

[calcmogul]

If one wants to use the velocity measurement as a derivative term isn't the same as full state back?

Specifically, full-state feedback requires a position reference, position measurement, velocity reference, and velocity measurement.

Perhaps some sort of profiled LQR controller for SISO systems might be useful.

I agree with the sentiment, but if we make more "profiled" versions of classes, we'd be starting to violate separation of concerns because there's many kinds of profiles you could mix-and-match. The plumbing between the profile and controller are trivial for users to write, so we should probably focus on making profile generators that are easier to use for the replan-every-loop use case than TrapezoidProfile.

I prototyped a discrete trapezoid profile filter, but it still has some kinks inherent to the discretization: trapezoid_profile_filter.py.txt