RIT VEXU Core API
Loading...
Searching...
No Matches
linear_quadratic_regulator.h
1#pragma once
2
3#include "math/eigen_interface.h"
4#include "../vendor/eigen/unsupported/Eigen/MatrixFunctions"
5
6#include "math/systems/dare_solver.h"
7#include "math/systems/discretization.h"
8#include "math/systems/linear_system.h"
9
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));
25 }
26 return Q;
27}
28
46template <int STATES, int INPUTS> class LinearQuadraticRegulator {
47 public:
48 // Definitions to shorten some lines.
49 using MatrixA = EMat<STATES, STATES>;
50 using MatrixB = EMat<STATES, INPUTS>;
51 using VectorX = EVec<STATES>;
52 using VectorU = EVec<INPUTS>;
53
62 template <int OUTPUTS>
64 LinearSystem<STATES, INPUTS, OUTPUTS> &plant, const VectorX &Qtolerances, const VectorU &Rtolerances,
65 const double &dt
66 )
67 : LinearQuadraticRegulator(plant.A(), plant.B(), Qtolerances, Rtolerances, dt) {}
68
79 const MatrixA &A, const MatrixB &B, const VectorX &Qtolerances, const VectorU &Rtolerances, const double &dt
80 )
81 : LinearQuadraticRegulator(A, B, cost_matrix(Qtolerances), cost_matrix(Rtolerances), dt) {}
82
93 const MatrixA &A, const MatrixB &B, const EMat<STATES, STATES> &Q, const EMat<INPUTS, INPUTS> &R, const double &dt
94 ) {
95 auto [Ad, Bd] = discretize_AB(A, B, dt);
96
97 MatrixA S = DARE<STATES, INPUTS>(Ad, Bd, Q, R);
98
99 // (BᵀSD + R) \ (BᵀSA)
100 K_ = (Bd.transpose() * S * Bd + R).llt().solve(Bd.transpose() * S * Ad);
101 }
102
111 VectorU calculate(const VectorX &x, const VectorX &r) { return K_ * (r - x); }
112
123 template <int OUTPUTS>
124 void latency_compensate(LinearSystem<STATES, INPUTS, OUTPUTS> &plant, const double &dt, const double &input_delay) {
125 auto [Ad, Bd] = discretize_AB(plant.A(), plant.B(), dt);
126
127 // Kdelay = K(A - BK)^(delay / dt)
128 K_ = K_ * (Ad - Bd * K_).pow(input_delay / dt);
129 }
130
131 private:
132 EMat<INPUTS, STATES> K_;
133};
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