Source code for cpsim.controllers.LQR

import numpy as np
from scipy.linalg import solve_continuous_are, inv

from .controller_base import Controller


[docs] class LQR(Controller): def __init__(self, A: np.ndarray, B: np.ndarray, Q: np.ndarray, R: np.ndarray): self.update_gain(A, B, Q, R) self.ref = 0 self.control_lo = None self.control_up = None
[docs] def update_gain(self, A: np.ndarray, B: np.ndarray, Q: np.ndarray, R: np.ndarray): P = solve_continuous_are(A, B, Q, R) self.K = inv(R) @ (B.T @ P)
[docs] def update(self, feedback_value: np.ndarray, current_time=None) -> np.ndarray: cin = -self.K @ (feedback_value - self.ref) if self.control_lo and self.control_up: for i in range(len(cin)): cin[i] = np.clip(cin[i], self.control_lo, self.control_up) return cin
[docs] def set_control_limit(self, control_lo: np.ndarray, control_up: np.ndarray): self.control_lo = control_lo self.control_up = control_up
[docs] def set_reference(self, ref: np.ndarray): self.ref = ref