diff --git a/src/controller/transcription.jl b/src/controller/transcription.jl index 2ced9fed..2b14aee6 100644 --- a/src/controller/transcription.jl +++ b/src/controller/transcription.jl @@ -343,7 +343,7 @@ They are defined in the Extended Help section. \mathbf{E} &= [\begin{smallmatrix}\mathbf{0} & \mathbf{E^†} \end{smallmatrix}] \\ \mathbf{E^†} &= \text{diag}\mathbf{(Ĉ,Ĉ,...,Ĉ)} \\ \mathbf{J} &= \text{diag}\mathbf{(D̂_d,D̂_d,...,D̂_d)} \\ - \mathbf{ex̂} &= [\begin{smallmatrix}\mathbf{0} & \mathbf{I}\end{smallmatrix}] + \mathbf{e_x̂} &= [\begin{smallmatrix}\mathbf{0} & \mathbf{I}\end{smallmatrix}] \end{aligned} ``` """ diff --git a/src/model/linearization.jl b/src/model/linearization.jl index d68995e1..6fc47130 100644 --- a/src/model/linearization.jl +++ b/src/model/linearization.jl @@ -174,22 +174,15 @@ function linearize!( linmodel::LinModel{NT}, model::SimModel; x=model.x0+model.xop, u=model.uop, d=model.dop ) where NT<:Real nonlinmodel = model - buffer, linbuffer = nonlinmodel.buffer, nonlinmodel.linbuffer + buffer = nonlinmodel.buffer # --- remove the operating points of the nonlinear model (typically zeros) --- x0, u0, d0 = buffer.x, buffer.u, buffer.d + x0 .= x .- nonlinmodel.xop u0 .= u .- nonlinmodel.uop d0 .= d .- nonlinmodel.dop - x0 .= x .- nonlinmodel.xop # --- compute the Jacobians at linearization points --- xnext0::Vector{NT}, y0::Vector{NT} = linmodel.buffer.x, linmodel.buffer.y - linbuffer.x .= x0 - linbuffer.u .= u0 - linbuffer.d .= d0 - jacobian!(linmodel.A, linbuffer.buffer_f_at_u_d, xnext0, x0) - jacobian!(linmodel.Bu, linbuffer.buffer_f_at_x_d, xnext0, u0) - jacobian!(linmodel.Bd, linbuffer.buffer_f_at_x_u, xnext0, d0) - jacobian!(linmodel.C, linbuffer.buffer_h_at_d, y0, x0) - jacobian!(linmodel.Dd, linbuffer.buffer_h_at_x, y0, d0) + get_jacobians!(linmodel, xnext0, y0, nonlinmodel, x0, u0, d0) # --- compute the nonlinear model output at operating points --- xnext0, y0 = linmodel.buffer.x, linmodel.buffer.y h!(y0, nonlinmodel, x0, d0, model.p) @@ -208,4 +201,28 @@ function linearize!( # --- reset the state of the linear model --- linmodel.x0 .= 0 # state deviation vector is always x0=0 after a linearization return linmodel +end + +"Compute the 5 Jacobians of `model` at the linearization point and write them in `linmodel`." +function get_jacobians!(linmodel::LinModel, xnext0, y0, model::SimModel, x0, u0, d0) + linbuffer = model.linbuffer # a LinearizationBuffer object + linbuffer.x .= x0 + linbuffer.u .= u0 + linbuffer.d .= d0 + jacobian!(linmodel.A, linbuffer.buffer_f_at_u_d, xnext0, x0) + jacobian!(linmodel.Bu, linbuffer.buffer_f_at_x_d, xnext0, u0) + jacobian!(linmodel.Bd, linbuffer.buffer_f_at_x_u, xnext0, d0) + jacobian!(linmodel.C, linbuffer.buffer_h_at_d, y0, x0) + jacobian!(linmodel.Dd, linbuffer.buffer_h_at_x, y0, d0) + return nothing +end + +"Copy the state-space matrices of `model` to `linmodel` if `model` is already linear." +function get_jacobians!(linmodel::LinModel, _ , _ , model::LinModel, _ , _ , _) + linmodel.A .= model.A + linmodel.Bu .= model.Bu + linmodel.C .= model.C + linmodel.Bd .= model.Bd + linmodel.Dd .= model.Dd + return nothing end \ No newline at end of file diff --git a/test/1_test_sim_model.jl b/test/1_test_sim_model.jl index 7a181ddc..d21eb66b 100644 --- a/test/1_test_sim_model.jl +++ b/test/1_test_sim_model.jl @@ -118,6 +118,17 @@ end @test_throws DimensionMismatch evaloutput(linmodel1, zeros(1)) end +@testitem "LinModel linearization" setup=[SetupMPCtests] begin + using .SetupMPCtests + linmodel1 = LinModel(sys, Ts, i_d=[3]) + linmodel2 = linearize(linmodel1, x=[1,2,3,4], u=[5,6], d=[7]) + @test linmodel2.A ≈ linmodel1.A + @test linmodel2.Bu ≈ linmodel1.Bu + @test linmodel2.Bd ≈ linmodel1.Bd + @test linmodel2.C ≈ linmodel1.C + @test linmodel2.Dd ≈ linmodel1.Dd +end + @testitem "LinModel real time simulations" setup=[SetupMPCtests] begin using .SetupMPCtests, ControlSystemsBase, LinearAlgebra linmodel1 = LinModel(tf(2, [10, 1]), 0.25)