Merge pull request #239 from JuliaControl/debug_linearize

debug: `linearize!` on `NonLinModel` with non-zero operating points now works

Author: franckgaga

src/model/linearization.jl

@@ -157,9 +157,9 @@ julia> linearize!(linmodel, model, x=[20.0], u=[0.0]); linmodel.A
 ```
 """
 function linearize!(
-    linmodel::LinModel{NT}, model::SimModel; 
+    linmodel::LinModel, model::SimModel; 
     x=(model.buffer.x.=model.x0.+model.xop), u=model.uop, d=model.dop
-) where NT<:Real
+)
     nonlinmodel = model
     buffer = nonlinmodel.buffer
     # --- remove the operating points of the nonlinear model (typically zeros) ---
@@ -172,16 +172,19 @@ function linearize!(
     # --- compute the nonlinear model output at operating points ---
     x0next, y0 = linmodel.buffer.x, linmodel.buffer.y
     h!(y0, nonlinmodel, x0, d0, model.p)
-    y  = y0
-    y .= y0 .+ nonlinmodel.yop
+    y0 .+= nonlinmodel.yop
+    y = y0
     # --- compute the nonlinear model next state at operating points ---
     f!(x0next, k0, nonlinmodel, x0, u0, d0, model.p)
-    xnext  = x0next
-    xnext .= x0next .+ nonlinmodel.fop .- nonlinmodel.xop
+    x0next .+= nonlinmodel.fop 
+    xnext = x0next  # xnext = f(x0,u0,d0) + fop - xop + xop = f(x0,u0,d0) + fop
+    # --- recompute x since it was modified in buffer.x ---
+    x0 .+= nonlinmodel.xop
+    x = x0
     # --- modify the linear model operating points ---
     linmodel.uop .= u
-    linmodel.yop .= y
     linmodel.dop .= d
+    linmodel.yop .= y
     linmodel.xop .= x
     linmodel.fop .= xnext
     # --- reset the state of the linear model ---
@@ -190,14 +193,14 @@ function linearize!(
 end
 
 "Call `linfunc!` function to compute the Jacobians of `model` at the linearization point."
-function linearize_core!(linmodel::LinModel, model::SimModel, x0, u0, d0)
-    x0next, y0 = linmodel.buffer.x, linmodel.buffer.y
+function linearize_core!(linmodel::LinModel, model::SimModel, x, u, d)
+    xnext, y = linmodel.buffer.x, linmodel.buffer.y
     A, Bu, C, Bd, Dd = linmodel.A, linmodel.Bu, linmodel.C, linmodel.Bd, linmodel.Dd
-    cst_x = Constant(x0)
-    cst_u = Constant(u0)
-    cst_d = Constant(d0)
+    cst_x = Constant(x)
+    cst_u = Constant(u)
+    cst_d = Constant(d)
     backend = model.jacobian
-    model.linfunc!(x0next, y0, A, Bu, C, Bd, Dd, backend, x0, u0, d0, cst_x, cst_u, cst_d)
+    model.linfunc!(xnext, y, A, Bu, C, Bd, Dd, backend, x, u, d, cst_x, cst_u, cst_d)
     return nothing
 end
 

test/1_test_sim_model.jl

@@ -371,6 +371,22 @@ end
     h2!(y0, x0, _, _) = (y0 .= x0)
     nonlinmodel4 = NonLinModel(f2!,h2!,Ts,1,1,1,0,solver=nothing,jacobian=AutoFiniteDiff())
     @test_nowarn linearize(nonlinmodel4, x=[1], u=[2])
+
+    # test linearization with nonzero operating points in the NonLinModel object:
+    linmodel_op = LinModel(tf(2, [10, 1]),1)
+    linmodel_op = setop!(linmodel_op, uop=[10], yop=[20], xop=[-2], fop=[5])
+    f3!(xnext, x, u, _, p) = (xnext .= p.A*x .+ p.Bu*u)
+    h3!(y, x, _, p) = (y .= p.C*x)
+    nonlinmodel_op = NonLinModel(f3!, h3!, 1.0, 1, 1, 1, p=linmodel_op, solver=nothing)
+    nonlinmodel_op = setop!(linmodel_op, uop=[10], yop=[20], xop=[-2], fop=[5])
+    linmodel_op_2 = linearize(nonlinmodel_op)
+    @test linmodel_op.A   ≈ linmodel_op_2.A
+    @test linmodel_op.Bu  ≈ linmodel_op_2.Bu
+    @test linmodel_op.C   ≈ linmodel_op_2.C
+    @test linmodel_op.uop ≈ linmodel_op_2.uop
+    @test linmodel_op.yop ≈ linmodel_op_2.yop
+    @test linmodel_op.xop ≈ linmodel_op_2.xop
+    @test linmodel_op.fop ≈ linmodel_op_2.fop
 end
 
 @testitem "NonLinModel real time simulations" setup=[SetupMPCtests] begin