[wpilib] Add function to adjust LQR controller gain for pure time delay (#2878)

There were three options for where to put this function:

1. A free function in LinearQuadraticRegulator.h. Returning a K matrix
   means the user can't use the LinearQuadraticRegulator in a loop
   anymore.
2. A default argument added to ctors in LinearQuadraticRegulator for a
   time delay (default of 0). This has the smallest API footprint from
   the user perspective, but it bloats the already substantial
   constructor overload set of LinearQuadraticRegulator.
3. A member function in LinearQuadraticRegulator that modifies the
   internal K. This would still have to take in a LinearSystem or (A, B)
   pair because the ctor doesn't store it. Storing it internally feels
   like paying for what we don't use most of the time.

I went with option 3.

I verified the tests's expected values in Python with
scipy.linalg.fractional_matrix_power().

Closes #2877.
This commit is contained in:
Tyler Veness
2020-11-20 15:28:00 -08:00
committed by GitHub
parent 2816b06c05
commit c8ea1b6c38
7 changed files with 154 additions and 2 deletions

View File

@@ -113,4 +113,26 @@ public class LinearQuadraticRegulatorTest {
assertEquals(19.16, k.get(0, 0), 0.1);
assertEquals(3.32, k.get(0, 1), 0.1);
}
@Test
public void testLatencyCompensate() {
var dt = 0.02;
var plant = LinearSystemId.createElevatorSystem(
DCMotor.getVex775Pro(4),
8.0,
0.75 * 25.4 / 1000.0,
14.67);
var regulator = new LinearQuadraticRegulator<>(
plant,
VecBuilder.fill(0.1, 0.2),
VecBuilder.fill(12.0),
dt);
regulator.latencyCompensate(plant, dt, 0.01);
assertEquals(8.97115941, regulator.getK().get(0, 0), 1e-3);
assertEquals(0.07904881, regulator.getK().get(0, 1), 1e-3);
}
}