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/splinedrivetrain.h b/frc971/control_loops/drivetrain/splinedrivetrain.h
index 1ce0e31..145060d 100644
--- a/frc971/control_loops/drivetrain/splinedrivetrain.h
+++ b/frc971/control_loops/drivetrain/splinedrivetrain.h
@@ -36,7 +36,8 @@
void SetGoal(const ::frc971::control_loops::drivetrain::Goal *goal);
- void Update(bool enabled, const ::Eigen::Matrix<double, 5, 1> &state);
+ void Update(bool enabled, const ::Eigen::Matrix<double, 5, 1> &state,
+ const ::Eigen::Matrix<double, 2, 1> &voltage_error);
void SetOutput(::frc971::control_loops::drivetrain::OutputT *output);
@@ -132,22 +133,6 @@
bool future_drive_spline_backwards_ = false;
int32_t future_spline_idx_ = 0; // Current spline being computed.
::std::atomic<int32_t> planning_spline_idx_{-1};
-
- // TODO(alex): pull this out of dt_config.
- const ::Eigen::DiagonalMatrix<double, 5> Q =
- 0.2 *
- (::Eigen::DiagonalMatrix<double, 5>().diagonal()
- << 1.0 / ::std::pow(0.12, 2),
- 1.0 / ::std::pow(0.12, 2), 1.0 / ::std::pow(0.1, 2),
- 1.0 / ::std::pow(1.0, 2), 1.0 / ::std::pow(1.0, 2))
- .finished()
- .asDiagonal();
- const ::Eigen::DiagonalMatrix<double, 2> R =
- (::Eigen::DiagonalMatrix<double, 2>().diagonal()
- << 1.0 / ::std::pow(12.0, 2),
- 1.0 / ::std::pow(12.0, 2))
- .finished()
- .asDiagonal();
};
} // namespace drivetrain