finally working

Author: baggepinnen

docs/src/batch_linearization.md

@@ -115,18 +115,18 @@ closed_loop_eqs = [
 plot(layout=2)
 
 # Simulate each individual controller
-for C in Cs
-    @named Ci = System(C)
-    eqs = [
-        closed_loop_eqs
-        connect(fb.output, Ci.input)
-        connect(Ci.output, duffing.u)
-    ]
-    @named closed_loop = System(eqs, t, systems=[duffing, Ci, fb, ref, F])
-    prob = ODEProblem(structural_simplify(closed_loop), [F.x => 0, F.xd => 0], (0.0, 8.0))
-    sol = solve(prob, Rodas5P(), abstol=1e-8, reltol=1e-8)
-    plot!(sol, idxs=[duffing.y.u, duffing.u.u], layout=2, lab="")
-end
+# for C in Cs
+#     @named Ci = System(C)
+#     eqs = [
+#         closed_loop_eqs
+#         connect(fb.output, Ci.input)
+#         connect(Ci.output, duffing.u)
+#     ]
+#     @named closed_loop = System(eqs, t, systems=[duffing, Ci, fb, ref, F])
+#     prob = ODEProblem(structural_simplify(closed_loop), [F.x => 0, F.xd => 0], (0.0, 8.0))
+#     sol = solve(prob, Rodas5P(), abstol=1e-8, reltol=1e-8)
+#     plot!(sol, idxs=[duffing.y.u, duffing.u.u], layout=2, lab="")
+# end
 
 # Simulate gain-scheduled controller
 @named Cgs = GainScheduledStateSpace(Cs, xs, interpolator=LinearInterpolation)
@@ -162,11 +162,12 @@ timepoints = 0:0.01:8
 Ps2, ssys, ops2, resolved_ops = trajectory_ss(closed_loop, closed_loop.r, closed_loop.y, sol; t=timepoints, verbose=true);
 bodeplot(Ps2, w, legend=false)
 ```
+Not how the closed-loop system changes very little along the trajectory, this is a good indication that the gain-scheduled controller is able to make the system appear linear.
 
 Internally, [`trajectory_ss`](@ref) works very much the same as [`batch_ss`](@ref), but constructs operating points automatically along the trajectory. This requires that the solution contains the states of the simplified system, accessible through the `idxs` argument like `sol(t, idxs=x)`. By linearizing the same system as we simulated, we ensure that this condition holds, doing so requires that we specify the inputs and outputs as analysis points rather than as variables.
 
 
-Notice how the peak of the transfer function changes along the trajectory. We can replicate the figure above by linearizing the plant and the controller individually, by providing the `loop_openings` argument. When linearizing the plant, we disconnect the controller input by passing `loop_openings=[closed_loop.u]`, and when linearizing the controller, we have various options for disconnecting the the plant:
+We can replicate the figure above by linearizing the plant and the controller individually, by providing the `loop_openings` argument. When linearizing the plant, we disconnect the controller input by passing `loop_openings=[closed_loop.u]`, and when linearizing the controller, we have various options for disconnecting the the plant:
 - Break the connection from plant output to controller input by passing `loop_openings=[closed_loop.y]`
 - Break the connection between the controller and the plant input by passing `loop_openings=[closed_loop.u]`
 - Break the connection `y` as well as the scheduling variable `v` (which is another form of feedback) by passing `loop_openings=[closed_loop.y, closed_loop.v]`
@@ -207,7 +208,7 @@ bodeplot(closed_loops_mimo, w; title="Loop open at MIMO", kwargs...)
 
 
 
-Why the discrepancy? We can understand the difference by comparing the Bode plots of the controllers. 
+Why are the results different depending on where we open the loop? We can understand the difference by comparing the Bode plots of the controllers. 
 
 ```@example BATCHLIN
 plot(
@@ -217,9 +218,23 @@ plot(
     bodeplot(controllersv, w, legend=false, plotphase=false, title="Loop open at v and y"),
 )
 ```
-if we open at both `y` and `v`, we only get the controller designed for the origin. If we open at `u`, we get controllers for the different values of the scheduling variable, and the corresponding measurement feedback (which is the same as the scheduling variable in this case).
+if we open at both `y` and `v` or we open at `u`, we get controllers for the different values of the scheduling variable, and the corresponding measurement feedback (which is the same as the scheduling variable in this case).
+
+However, if we only open at `y` we get controller linearizations that _still contain the closed loop through the scheduling connection_ `v`. We can verify this by looking at what variables are present in the input-output map
+```@example BATCHLIN
+Cy = named_ss(controllersy[end], x=Symbol.(unknowns(ssy)))
+sminreal(Cy).x
+```
+notice how the state of the plant is included in the controller, this is an indication that we didn't fully isolate the controller during the linearizaiton. If we do the same thing for the controller with the loop opened at `u`, we see that the state of the plant is not included in the controller:
+```@example BATCHLIN
+Cu = named_ss(controllersu[end], x=Symbol.(unknowns(ssu)))
+sminreal(Cu).x
+```
+The call to `sminreal` is important here, it removes the states that are not needed to represent the input-output map of the system. The state of the full model, including the plant state, is present in the linearization before this call. 
+
+
 
-However, if we only open at `y` we get controller linearizations that contains the closed loop through the scheduling connection `v`. The easiest way to ensure that the controller is properly disconnected from the plant while taking into account the different scheduling along the trajectory is thus to break at `u`.
+The easiest way to ensure that the controller is properly disconnected from the plant while taking into account the different scheduling along the trajectory is thus to break at `u`.
 
 ## Summary
 We have seen how to

src/ode_system.jl

@@ -405,16 +405,13 @@ function batch_ss(args...; kwargs...)
     [ss(l...) for l in lins], ssys, resolved_ops
 end
 
-function unnamespace(ap)
-    # ap_name = ap.name
-    # new_name = join(ModelingToolkit.namespace_hierarchy(ap_name)[2:end], Symbolics.NAMESPACE_SEPARATOR)
-    # ap2 = ModelingToolkit.@set ap.name = Symbol(new_name)
-    # ap2
-
-    ap_name = ModelingToolkit.SymbolicIndexingInterface.getname(ap.input.u)
-    new_name = join(ModelingToolkit.namespace_hierarchy(ap_name)[2:end], Symbolics.NAMESPACE_SEPARATOR)
-    new_var = Symbolics.rename(ap.input.u, Symbol(new_name))
-end
+# function unnamespace(ap)
+#     map(ap.outputs) do out
+#         ap_name = ModelingToolkit.SymbolicIndexingInterface.getname(out.u) 
+#         new_name = join(ModelingToolkit.namespace_hierarchy(ap_name)[2:end], Symbolics.NAMESPACE_SEPARATOR)
+#         Symbolics.rename(ap.input.u, Symbol(new_name))
+#     end
+# end
 
 """
     linsystems, ssys = trajectory_ss(sys, inputs, outputs, sol; t = _max_100(sol.t), fuzzer=nothing, verbose = true, kwargs...)
@@ -440,11 +437,11 @@ function trajectory_ss(sys, inputs, outputs, sol; t = _max_100(sol.t), allow_inp
 
     # TODO: The value of the output (or input) of the input analysis points should be mapped to the perturbation vars
     perturbation_vars = ModelingToolkit.inputs(ssys)
-    original_inputs = [unnamespace(ap) for ap in vcat(inputs)] # assuming all inputs are analysis points for now
+    # @show original_inputs = reduce(vcat, unnamespace(ap) for ap in vcat(inputs)) # assuming all inputs are analysis points for now
     op_nothing = Dict(unknowns(sys) .=> nothing) # Remove all defaults present in the original system
     defs = ModelingToolkit.defaults(sys)
     ops = map(t) do ti
-        opsol = Dict(x => robust_sol_getindex(sol, ti, x, defs; verbose) for x in [x; original_inputs])
+        opsol = Dict(x => robust_sol_getindex(sol, ti, x, defs; verbose) for x in x)
         # opsolu = Dict(new_u => robust_sol_getindex(sol, ti, u, defs; verbose) for (new_u, u) in zip(perturbation_vars, original_inputs))
         merge(op_nothing, opsol)
     end
@@ -455,11 +452,15 @@ function trajectory_ss(sys, inputs, outputs, sol; t = _max_100(sol.t), allow_inp
         ops = reduce(vcat, opsv)
         t = repeat(t, inner = length(ops) ÷ length(t))
     end
-    lin_fun, ssys = linearization_function(sys, inputs, outputs; op=ops[1], initialization_abstol=1e-2, initialization_reltol=1e-2, kwargs...) # initializealg=ModelingToolkit.SciMLBase.NoInit()
+    lin_fun, ssys = linearization_function(sys, inputs, outputs; op=ops[1], kwargs...)#, initialization_abstol=1e-1, initialization_reltol=1e-1, kwargs...) # initializealg=ModelingToolkit.SciMLBase.NoInit()
+    # Main.lin_fun = lin_fun
+    # Main.op1 = ops[1]
+    # Main.ops = ops 
+    # equations(lin_fun.prob.f.initialization_data.initializeprob.f.sys)
+    # observed(lin_fun.prob.f.initialization_data.initializeprob.f.sys)
     lins_ops = map(zip(ops, t)) do (op, t)
-        @show t
         linearize(ssys, lin_fun; op, t, allow_input_derivatives)
-        # linearize(sys, inputs, outputs; op, t, allow_input_derivatives, initialize=false)[1]
+        # linearize(sys, inputs, outputs; op, t, allow_input_derivatives) # useful for debugging
     end
     lins = first.(lins_ops)
     resolved_ops = last.(lins_ops)