Skip to content

Commit 655efb5

Browse files
baggepinnenbaggepinnen
committed
add lqg
1 parent c270f22 commit 655efb5

5 files changed

Lines changed: 476 additions & 275 deletions

File tree

‎Project.toml‎

Lines changed: 0 additions & 6 deletions
Original file line numberDiff line numberDiff line change
@@ -6,19 +6,13 @@ version = "0.1.0"
66
[deps]
77
ControlSystemsMTK = "687d7614-c7e5-45fc-bfc3-9ee385575c88"
88
DyadControlSystems = "6f9e59d0-2838-4fee-9bce-c163a6c9592b"
9-
GLMakie = "e9467ef8-e4e7-5192-8a1a-b1aee30e663a"
109
ModelingToolkit = "961ee093-0014-501f-94e3-6117800e7a78"
1110
ModelingToolkitStandardLibrary = "16a59e39-deab-5bd0-87e4-056b12336739"
12-
Multibody = "e1cad5d1-98ef-44f9-a79a-9ca4547f95b9"
1311
OrdinaryDiffEqDefault = "50262376-6c5a-4cf5-baba-aaf4f84d72d7"
1412
Plots = "91a5bcdd-55d7-5caf-9e0b-520d859cae80"
1513

16-
[sources]
17-
Multibody = {rev = "no_mtkmodel", url = "https://github.com/JuliaComputing/Multibody.jl"}
18-
1914
[compat]
2015
ControlSystemsMTK = "2.5.0"
21-
GLMakie = "0.13.7"
2216
ModelingToolkitStandardLibrary = "2.25.0"
2317
Plots = "1.41.2"
2418

‎src/lqg_control_flat_planar.jl‎

Lines changed: 183 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,183 @@
1+
include("planar_flat.jl") # Get FlatDyadBot
2+
3+
using DyadControlSystems
4+
import DyadControlSystems as JSC
5+
using LinearAlgebra
6+
7+
# Create the plant directly - no wrapper component needed
8+
9+
@component function Add4(; name, K)
10+
__vars = Any[]
11+
__systems = System[]
12+
__guesses = Dict()
13+
__defaults = Dict()
14+
__initialization_eqs = []
15+
__eqs = Equation[]
16+
17+
r,c = size(K)
18+
### Symbolic Parameters
19+
__params = @parameters begin
20+
K[1:r, 1:c] = K
21+
end
22+
append!(__vars, @variables (input1(t)::Real), [input = true])
23+
append!(__vars, @variables (input2(t)::Real), [input = true])
24+
append!(__vars, @variables (input3(t)::Real), [input = true])
25+
append!(__vars, @variables (input4(t)::Real), [input = true])
26+
append!(__vars, @variables (output(t)::Real), [output = true])
27+
__constants = Any[]
28+
push!(__eqs, output ~ dot(K, [input1, input2, input3, input4]))
29+
return System(__eqs, t, __vars, __params; systems=__systems, defaults=__defaults, guesses=__guesses, name, initialization_eqs=__initialization_eqs)
30+
end
31+
32+
@component function LQGFlatDyadBot2(; name)
33+
systems = @named begin
34+
plant = FlatDyadBot()
35+
L = Add4(K = ones(1, 4))
36+
end
37+
38+
eqs = [
39+
# Expose outputs with analysis point names for LQG
40+
connect(plant.x_output.u, :y_x, L.input1) # Position measurement
41+
connect(plant.x_dot_output.u, :y_xd, L.input2)
42+
connect(plant.theta_output.u, :y_theta, L.input3) # Angle measurement
43+
connect(plant.theta_dot_output.u, :y_thetad, L.input4)
44+
connect(L.output, :u, plant.control_input.u) # Control input
45+
]
46+
47+
System(eqs, t, [], []; systems, name)
48+
end
49+
50+
51+
52+
@named plant = LQGFlatDyadBot2()
53+
54+
# LQG Analysis Specification
55+
# - 2 measurements: position (x) and angle (theta)
56+
# - 2 controlled outputs: same as measurements (regulate both)
57+
# - 1 control input: torque (tau)
58+
lqg_spec = JSC.LQGAnalysisSpec(;
59+
name = :SegwayLQG,
60+
model = plant,
61+
measurement = ["y_x", "y_theta"], # What we measure
62+
controlled_output = ["y_x", "y_xd", "y_theta", "y_thetad"], # What we want to control
63+
control_input = ["u"], # Control input
64+
q1_diag = [10.0, 0.1, 1, 0.1], # Penalty on controlled outputs (x, theta)
65+
q2_diag = [0.0001], # Penalty on control input (tau)
66+
r1_diag = [1.0], # Disturbance noise covariance
67+
r2_diag = [0.001, 0.001], # Measurement noise covariance (x, theta)
68+
wl = 1e-2,
69+
wu = 1e3,
70+
num_frequencies = 200,
71+
)
72+
73+
# Run the LQG analysis
74+
lqg_asol = JSC.run_analysis(lqg_spec)
75+
76+
# Visualize results
77+
step_response = JSC.artifacts(lqg_asol, :StepResponse)
78+
gang_of_four = JSC.artifacts(lqg_asol, :GangOfFour)
79+
bode_plot = JSC.artifacts(lqg_asol, :BodePlot)
80+
margin_plot = JSC.artifacts(lqg_asol, :MarginPlot)
81+
controller_gain = lqg_asol.L
82+
observer_gain = lqg_asol.K
83+
84+
display(controller_gain)
85+
display(observer_gain)
86+
87+
##
88+
89+
# cm = complete(plant)
90+
ssys = mtkcompile(plant)
91+
op = [
92+
ssys.L.K => -lqg_asol.L / lqg_asol.P_reduced.C
93+
]
94+
95+
96+
prob = ODEProblem(ssys, op, (0, 1))
97+
sol = solve(prob, Rodas5P())
98+
plot(sol, ylims=(-5, 5)); hline!([π], l=(:dash, :black), primary=false)
99+
100+
101+
##
102+
103+
get_Cfffb(; name) = System(ss(lqg_asol.Cfffb); name)
104+
get_Cff(; name) = System(ss(lqg_asol.Cff); name)
105+
get_Cfb(; name) = System(ss(lqg_asol.Cfb); name)
106+
107+
@component function LQGFlatDyadBot4(; name)
108+
systems = @named begin
109+
plantfffb = FlatDyadBot()
110+
C = get_Cfffb()
111+
plant = FlatDyadBot()
112+
Cff = get_Cff()
113+
Cfb = get_Cfb()
114+
add = Blocks.Add(k1=1, k2=-1)
115+
end
116+
117+
eqs = [
118+
C.input.u[1] ~ ifelse(t>5, 0.15, 0) # rx
119+
C.input.u[2] ~ 0 # rxd
120+
C.input.u[3] ~ 0 # rtheta
121+
C.input.u[4] ~ 0 # rthetad
122+
C.input.u[5] ~ plantfffb.x_output.u # x
123+
C.input.u[6] ~ plantfffb.theta_output.u-pi # theta
124+
connect(C.output, :ufffb, plantfffb.control_input)
125+
126+
Cff.input.u[1] ~ ifelse(t>5, 0.15, 0) # rx
127+
Cff.input.u[2] ~ 0 # rxd
128+
Cff.input.u[3] ~ 0 # rtheta
129+
Cff.input.u[4] ~ 0 # rthetad
130+
Cfb.input.u[1] ~ plant.x_output.u # x
131+
Cfb.input.u[2] ~ plant.theta_output.u-pi # theta
132+
connect(Cff.output, add.input1)
133+
connect(Cfb.output, add.input2)
134+
connect(add.output, :u, plant.control_input)
135+
]
136+
guesses = [
137+
plantfffb.theta_ddot => 0,
138+
plant.theta_ddot => 0
139+
]
140+
System(eqs, t, [], []; systems, name, guesses)
141+
end
142+
143+
@named lqg_cl = LQGFlatDyadBot4()
144+
145+
ssys = mtkcompile(lqg_cl)
146+
147+
op = [
148+
# collect(ssys.C.x) .=> nothing
149+
# ssys.C.output.u => 0
150+
# D(ssys.C.output.u) => 0
151+
ssys.plant.theta => deg2rad(160)
152+
ssys.plantfffb.theta => deg2rad(160)
153+
]
154+
155+
prob = ODEProblem(ssys, op, (0, 20))
156+
sol = solve(prob, Rodas5P())
157+
plot(sol, idxs=[
158+
ssys.plant.theta
159+
ssys.plant.x
160+
ssys.plant.tau
161+
ssys.plantfffb.theta
162+
ssys.plantfffb.x
163+
ssys.plantfffb.tau
164+
]); hline!([π 0.3], l=(:dash, :black), primary=false, ylims=(-1, 4))
165+
166+
167+
##
168+
169+
LT = lqg_asol.Cfb*system_mapping(lqg_asol.P_ext)
170+
171+
plot(diskmargin(LT))
172+
173+
##
174+
L2 = get_named_looptransfer(lqg_cl, [lqg_cl.u])
175+
L2 = -L2#minreal(L2, 1e-12)
176+
177+
bodeplot([LT, L2]) # why not identical?
178+
179+
180+
dmi2 = diskmargin(L2)
181+
plot!(dmi2)
182+
183+
# TODO: test the Cff and Cfb separate contorllers, they do not appear to agree with Cfffb at all

0 commit comments

Comments
 (0)