Path-Relative LQR for trajectory following
Implement a basic LQR controller that operates on a path-relative state
for spline control. This formulation also helps to leave the path open
for changes around how we manage gains (e.g., setting different cost
matrices at different points along the path) as well as leaving room
to move into a more MPC-like formulation where we more explicitly
compensate for saturation.
Left the tuning a bit loose so far, since the control
scheme implicitly assumes that saturation is not an issue.
Change-Id: I792bc939735b405b09ba4b8af777a1b2b242d325
diff --git a/frc971/control_loops/drivetrain/drivetrain.cc b/frc971/control_loops/drivetrain/drivetrain.cc
index f517e83..8c0599f 100644
--- a/frc971/control_loops/drivetrain/drivetrain.cc
+++ b/frc971/control_loops/drivetrain/drivetrain.cc
@@ -299,7 +299,7 @@
dt_spline_.Update(
output != nullptr && controller_type == ControllerType::SPLINE_FOLLOWER,
- trajectory_state);
+ trajectory_state, kf_.X_hat().block<2, 1>(4, 0));
dt_line_follow_.Update(monotonic_now, trajectory_state);