1515 Nbar — precompensator (n_inputs x n_tracked)
1616 None if closed-loop DC gain is singular (regulates to zero only)
1717"""
18+ # pylint: disable=invalid-name
1819
1920import warnings
2021from dataclasses import dataclass
@@ -40,40 +41,18 @@ class ControllerResult:
4041 tracked : dict [str , float ]
4142
4243
43- def build_controller (
44- plant : StateSpace ,
45- track : dict [str | int , float | None ] | None = None ,
46- p : float = 1.0 ,
47- Q : np .ndarray | None = None ,
48- R : np .ndarray | None = None ,
49- ) -> ControllerResult :
50- """LQR controller for a StateSpace plant. Supports both continuous-
51- and discrete-time systems (uses ``lqr`` or ``dlqr`` depending on
52- ``plant.dt``). The returned closed-loop model preserves the sampling
53- period so that downstream simulation routines behave correctly.
44+ def _parse_tracking (
45+ plant : StateSpace , track : dict | None
46+ ) -> tuple [list [str ], dict [str , float ], np .ndarray ]:
47+ """Interpret the ``track`` argument.
5448
55- Args:
56- plant: Any StateSpace instance from systems.py.
57- track: Dict of {output_name_or_index: target_value | None}.
58- None value = don't care. Omitted outputs = don't care.
59- track=None regulates all outputs to zero.
60- p: Output-error weight (Q = p * C_track' @ C_track). Ignored if Q given.
61- Q: State-cost matrix (n_states x n_states). Overrides p.
62- R: Input-cost matrix (n_inputs x n_inputs). Defaults to identity.
63-
64- Returns:
65- ControllerResult containing ``K``, ``Nbar`` and ``sys_cl`` (plus
66- the ``tracked`` dictionary used to compute the controller). If
67- ``plant`` was discrete-time, the returned ``sys_cl`` has its
68- ``dt`` field set accordingly and ``K`` is computed via ``dlqr``. The
69- precompensator ``Nbar`` is computed with the appropriate steady-
70- state formula for continuous or discrete dynamics.
49+ Returns ``(labels, tracked, C_track)`` where ``labels`` is the list of
50+ plant output names, ``tracked`` maps those names to non-None reference
51+ values, and ``C_track`` is the corresponding rows of the plant output
52+ matrix. This isolates a bulky dictionary comprehension from
53+ ``build_controller``.
7154 """
72- A = np .array (plant .A , dtype = float )
73- B = np .array (plant .B , dtype = float )
7455 C = np .array (plant .C , dtype = float )
75- n_in = B .shape [1 ]
76-
7756 labels = (
7857 list (plant .output_labels )
7958 if plant .output_labels
@@ -87,47 +66,67 @@ def build_controller(
8766 }
8867 idx = [labels .index (lbl ) for lbl in tracked ]
8968 C_track = C [idx , :]
69+ return labels , tracked , C_track
9070
91- # LQR / DLQR depending on plant type
92- Q = p * C_track .T @ C_track if Q is None else np .array (Q , dtype = float )
93- R = np .eye (n_in ) if R is None else np .array (R , dtype = float )
94- is_discrete = plant .dt not in (0 , None )
95- if is_discrete :
96- K , _ , _ = dlqr (A , B , Q , R )
97- else :
98- K , _ , _ = lqr (A , B , Q , R )
9971
100- # Nbar: solves C_track @ DC-gain(A_cl,B) @ Nbar = -I
101- # For continuous DC gain = -C_track @ inv(A_cl) @ B
102- # For discrete DC gain = C_track @ inv(I - A_cl) @ B
72+ def _compute_Nbar (A , B , C_track , K , is_discrete ):
73+ """Calculate the precompensator Nbar from closed-loop data."""
10374 A_cl = A - B @ K
10475 if is_discrete :
105- # discrete-time steady-state: x = inv(I - A_cl) B Nbar r
106- # require C_track @ inv(I - A_cl) B Nbar = I
107- # use pseudo-inverse in case (I-A_cl) is singular
10876 M = C_track @ np .linalg .pinv (np .eye (A_cl .shape [0 ]) - A_cl ) @ B
10977 else :
110- # continuous-time steady-state: x = -inv(A_cl) B Nbar r
111- # require -C_track @ inv(A_cl) B Nbar = I
112- # use pseudo-inverse in case A_cl is singular
11378 M = - C_track @ np .linalg .pinv (A_cl ) @ B
114- Nbar = np .linalg .pinv (M ) if np .all (np .isfinite (M )) else None
115- if Nbar is not None and len (idx ) != n_in :
79+ return np .linalg .pinv (M ) if np .all (np .isfinite (M )) else None
80+
81+
82+ def build_controller (
83+ plant : StateSpace ,
84+ track : dict [str | int , float | None ] | None = None ,
85+ p : float = 1.0 ,
86+ Q : np .ndarray | None = None ,
87+ R : np .ndarray | None = None ,
88+ ) -> ControllerResult :
89+ """LQR controller for a StateSpace plant. Supports both continuous-
90+ and discrete-time systems (uses ``lqr`` or ``dlqr`` depending on
91+ ``plant.dt``). The returned closed-loop model preserves the sampling
92+ period so that downstream simulation routines behave correctly.
93+ """
94+ A = np .array (plant .A , dtype = float )
95+ B = np .array (plant .B , dtype = float )
96+
97+ labels , tracked , C_track = _parse_tracking (plant , track )
98+
99+ # LQR / DLQR depending on plant type; inline Q and R defaults
100+ if plant .dt not in (0 , None ):
101+ K , _ , _ = dlqr (
102+ A ,
103+ B ,
104+ p * C_track .T @ C_track if Q is None else np .array (Q , dtype = float ),
105+ np .eye (B .shape [1 ]) if R is None else np .array (R , dtype = float ),
106+ )
107+ discrete = True
108+ else :
109+ K , _ , _ = lqr (
110+ A ,
111+ B ,
112+ p * C_track .T @ C_track if Q is None else np .array (Q , dtype = float ),
113+ np .eye (B .shape [1 ]) if R is None else np .array (R , dtype = float ),
114+ )
115+ discrete = False
116+
117+ Nbar = _compute_Nbar (A , B , C_track , K , discrete )
118+ if Nbar is not None and len (tracked ) != B .shape [1 ]:
116119 warnings .warn (
117120 "n_tracked != n_inputs: Nbar is a pseudoinverse; "
118121 "exact tracking not guaranteed." ,
119122 stacklevel = 2 ,
120123 )
121124
122- n_tracked = len (idx )
123- B_cl = B @ (Nbar if Nbar is not None else np .zeros ((n_in , n_tracked )))
124- D_cl = np .zeros ((C .shape [0 ], n_tracked ))
125- # preserve sampling period if discrete
126125 sys_cl = ss (
127- A_cl ,
128- B_cl ,
129- C ,
130- D_cl ,
126+ A - B @ K ,
127+ B @ ( Nbar if Nbar is not None else np . zeros (( B . shape [ 1 ], len ( tracked )))) ,
128+ np . array ( plant . C , dtype = float ) ,
129+ np . zeros (( np . array ( plant . C , dtype = float ). shape [ 0 ], len ( tracked ))) ,
131130 inputs = [f"r_{ lbl } " for lbl in tracked ],
132131 outputs = labels ,
133132 dt = plant .dt ,
0 commit comments