3#include "math/eigen_interface.h"
4#include "../vendor/eigen/unsupported/Eigen/MatrixFunctions"
6#include "math/systems/dare_solver.h"
7#include "math/systems/discretization.h"
8#include "math/systems/linear_system.h"
21template <
int DIM> EMat<DIM, DIM> cost_matrix(
const EVec<DIM> &tolerances) {
22 EMat<DIM, DIM> Q = EMat<DIM, DIM>::Zero();
23 for (
int i = 0; i < DIM; i++) {
24 Q(i, i) = 1.0 / (tolerances(i) * tolerances(i));
49 using MatrixA = EMat<STATES, STATES>;
50 using MatrixB = EMat<STATES, INPUTS>;
51 using VectorX = EVec<STATES>;
52 using VectorU = EVec<INPUTS>;
62 template <
int OUTPUTS>
79 const MatrixA &A,
const MatrixB &B,
const VectorX &Qtolerances,
const VectorU &Rtolerances,
const double &dt
93 const MatrixA &A,
const MatrixB &B,
const EMat<STATES, STATES> &Q,
const EMat<INPUTS, INPUTS> &R,
const double &dt
95 auto [Ad, Bd] = discretize_AB(A, B, dt);
97 MatrixA S = DARE<STATES, INPUTS>(Ad, Bd, Q, R);
100 K_ = (Bd.transpose() * S * Bd + R).llt().solve(Bd.transpose() * S * Ad);
111 VectorU
calculate(
const VectorX &x,
const VectorX &r) {
return K_ * (r - x); }
123 template <
int OUTPUTS>
125 auto [Ad, Bd] = discretize_AB(plant.
A(), plant.
B(), dt);
128 K_ = K_ * (Ad - Bd * K_).pow(input_delay / dt);
132 EMat<INPUTS, STATES> K_;
VectorU calculate(const VectorX &x, const VectorX &r)
Definition linear_quadratic_regulator.h:111
void latency_compensate(LinearSystem< STATES, INPUTS, OUTPUTS > &plant, const double &dt, const double &input_delay)
Definition linear_quadratic_regulator.h:124
LinearQuadraticRegulator(LinearSystem< STATES, INPUTS, OUTPUTS > &plant, const VectorX &Qtolerances, const VectorU &Rtolerances, const double &dt)
Definition linear_quadratic_regulator.h:63
LinearQuadraticRegulator(const MatrixA &A, const MatrixB &B, const EMat< STATES, STATES > &Q, const EMat< INPUTS, INPUTS > &R, const double &dt)
Definition linear_quadratic_regulator.h:92
LinearQuadraticRegulator(const MatrixA &A, const MatrixB &B, const VectorX &Qtolerances, const VectorU &Rtolerances, const double &dt)
Definition linear_quadratic_regulator.h:78
Definition linear_system.h:17
MatrixB B()
Definition linear_system.h:46
MatrixA A()
Definition linear_system.h:41