added steadystate tests for all controllers based on InternalModel, nint_u and nint_ym options.

Author: franckgaga

Project.toml

@@ -1,7 +1,7 @@
 name = "ModelPredictiveControl"
 uuid = "61f9bdb8-6ae4-484a-811f-bbf86720c31c"
 authors = ["Francis Gagnon"]
-version = "0.8.3"
+version = "0.8.4"
 
 [deps]
 ControlSystemsBase = "aaaaaaaa-a6ca-5380-bf3e-84a91bcd477e"

docs/src/manual/linmpc.md

@@ -76,8 +76,8 @@ default, [`LinMPC`](@ref) controllers use [`OSQP`](https://osqp.org/) to solve t
 soft constraints on output predictions ``\mathbf{ŷ}`` to ensure feasibility, and a
 [`SteadyKalmanFilter`](@ref) to estimate the plant states[^1]. An attentive reader will also
 notice that the Kalman filter estimates two additional states compared to the plant model.
-These are the integrators for the unmeasured plant disturbances, and they are automatically
-added to the model outputs by default if feasible (see [`SteadyKalmanFilter`](@ref)
+These are the integrating states for the unmeasured plant disturbances, and they are
+automatically added to the model outputs by default if feasible (see [`SteadyKalmanFilter`](@ref)
 for details).
 
 [^1]: We could have use an [`InternalModel`](@ref) structure with

src/controller/explicitmpc.jl

@@ -104,9 +104,9 @@ julia> mpc = ExplicitMPC(model, Mwt=[0, 1], Nwt=[0.5], Hp=30, Hc=1)
 ExplicitMPC controller with a sample time Ts = 4.0 s, SteadyKalmanFilter estimator and:
  30 prediction steps Hp
   1 control steps Hc
-  1 manipulated inputs u (0 integrators)
+  1 manipulated inputs u (0 integrating states)
   4 states x̂
-  2 measured outputs ym (2 integrators)
+  2 measured outputs ym (2 integrating states)
   0 unmeasured outputs yu
   0 measured disturbances d
 ```
@@ -129,9 +129,9 @@ julia> mpc = ExplicitMPC(estim, Mwt=[0, 1], Nwt=[0.5], Hp=30, Hc=1)
 ExplicitMPC controller with a sample time Ts = 4.0 s, KalmanFilter estimator and:
  30 prediction steps Hp
   1 control steps Hc
-  1 manipulated inputs u (0 integrators)
+  1 manipulated inputs u (0 integrating states)
   3 states x̂
-  1 measured outputs ym (1 integrators)
+  1 measured outputs ym (1 integrating states)
   1 unmeasured outputs yu
   0 measured disturbances d
 ```
@@ -179,7 +179,7 @@ linconstraint!(::ExplicitMPC, ::LinModel) = nothing
 Analytically solve the optimization problem for [`ExplicitMPC`](@ref).
 """
 function optim_objective!(mpc::ExplicitMPC)
-    return ldiv!(mpc.ΔŨ, mpc.P̃_chol, -mpc.q̃)
+    return lmul!(-1, ldiv!(mpc.ΔŨ, mpc.P̃_chol, mpc.q̃))
 end
 
 "For [`ExplicitMPC`](@ref), return an empty summary."

src/controller/linmpc.jl

@@ -120,9 +120,9 @@ julia> mpc = LinMPC(model, Mwt=[0, 1], Nwt=[0.5], Hp=30, Hc=1)
 LinMPC controller with a sample time Ts = 4.0 s, OSQP optimizer, SteadyKalmanFilter estimator and:
  30 prediction steps Hp
   1 control steps Hc
-  1 manipulated inputs u (0 integrators)
+  1 manipulated inputs u (0 integrating states)
   4 states x̂
-  2 measured outputs ym (2 integrators)
+  2 measured outputs ym (2 integrating states)
   0 unmeasured outputs yu
   0 measured disturbances d
 ```
@@ -167,9 +167,9 @@ julia> mpc = LinMPC(estim, Mwt=[0, 1], Nwt=[0.5], Hp=30, Hc=1)
 LinMPC controller with a sample time Ts = 4.0 s, OSQP optimizer, KalmanFilter estimator and:
  30 prediction steps Hp
   1 control steps Hc
-  1 manipulated inputs u (0 integrators)
+  1 manipulated inputs u (0 integrating states)
   3 states x̂
-  1 measured outputs ym (1 integrators)
+  1 measured outputs ym (1 integrating states)
   1 unmeasured outputs yu
   0 measured disturbances d
 ```

src/controller/nonlinmpc.jl

@@ -134,9 +134,9 @@ julia> mpc = NonLinMPC(model, Hp=20, Hc=1, Cwt=1e6)
 NonLinMPC controller with a sample time Ts = 10.0 s, Ipopt optimizer, UnscentedKalmanFilter estimator and:
  20 prediction steps Hp
   1 control steps Hc
-  1 manipulated inputs u (0 integrators)
+  1 manipulated inputs u (0 integrating states)
   2 states x̂
-  1 measured outputs ym (1 integrators)
+  1 measured outputs ym (1 integrating states)
   0 unmeasured outputs yu
   0 measured disturbances d
 ```
@@ -171,9 +171,9 @@ julia> mpc = NonLinMPC(estim, Hp=20, Hc=1, Cwt=1e6)
 NonLinMPC controller with a sample time Ts = 10.0 s, Ipopt optimizer, UnscentedKalmanFilter estimator and:
  20 prediction steps Hp
   1 control steps Hc
-  1 manipulated inputs u (0 integrators)
+  1 manipulated inputs u (0 integrating states)
   2 states x̂
-  1 measured outputs ym (1 integrators)
+  1 measured outputs ym (1 integrating states)
   0 unmeasured outputs yu
   0 measured disturbances d
 ```

src/estimator/kalman.jl

@@ -96,21 +96,21 @@ unmeasured ones, for ``\mathbf{Ĉ^u, D̂_d^u}``).
 - `nint_u=0`: integrator quantity for the stochastic model of the unmeasured disturbances at
     the manipulated inputs (vector), use `nint_u=0` for no integrator (see Extended Help).
 - `σQint_u=fill(1,sum(nint_u))`: same than `σQ` but for the unmeasured disturbances at 
-    manipulated inputs ``\mathbf{Q_{int_u}}`` (composed of integrators).
+    manipulated inputs ``\mathbf{Q_{int_u}}`` (composed of integrating states).
 - `nint_ym=default_nint(model,i_ym,nint_u)` : same than `nint_u` but for the unmeasured 
     disturbances at the measured outputs, use `nint_ym=0` for no integrator (see Extended Help).
 - `σQint_ym=fill(1,sum(nint_ym))` : same than `σQ` for the unmeasured disturbances at 
-    measured outputs ``\mathbf{Q_{int_{ym}}}`` (composed of integrators).
+    measured outputs ``\mathbf{Q_{int_{ym}}}`` (composed of integrating states).
 
 # Examples
 ```jldoctest
 julia> model = LinModel([tf(3, [30, 1]); tf(-2, [5, 1])], 0.5);
 
 julia> estim = SteadyKalmanFilter(model, i_ym=[2], σR=[1], σQint_ym=[0.01])
 SteadyKalmanFilter estimator with a sample time Ts = 0.5 s, LinModel and:
- 1 manipulated inputs u (0 integrators)
+ 1 manipulated inputs u (0 integrating states)
  3 states x̂
- 1 measured outputs ym (1 integrators)
+ 1 measured outputs ym (1 integrating states)
  1 unmeasured outputs yu
  0 measured disturbances d
 ```
@@ -249,9 +249,9 @@ value with ``\mathbf{P̂}_{-1}(0) =
 - `σP0=fill(1/model.nx,model.nx)` : main diagonal of the initial estimate covariance
     ``\mathbf{P}(0)``, specified as a standard deviation vector.
 - `σP0int_u=fill(1,sum(nint_u))` : same than `σP0` but for the unmeasured disturbances at 
-    manipulated inputs ``\mathbf{P_{int_u}}(0)`` (composed of integrators).
+    manipulated inputs ``\mathbf{P_{int_u}}(0)`` (composed of integrating states).
 - `σP0int_ym=fill(1,sum(nint_ym))` : same than `σP0` but for the unmeasured disturbances at 
-    measured outputs ``\mathbf{P_{int_{ym}}}(0)`` (composed of integrators).
+    measured outputs ``\mathbf{P_{int_{ym}}}(0)`` (composed of integrating states).
 - `<keyword arguments>` of [`SteadyKalmanFilter`](@ref) constructor.
 
 # Examples
@@ -260,9 +260,9 @@ julia> model = LinModel([tf(3, [30, 1]); tf(-2, [5, 1])], 0.5);
 
 julia> estim = KalmanFilter(model, i_ym=[2], σR=[1], σP0=[100, 100], σQint_ym=[0.01])
 KalmanFilter estimator with a sample time Ts = 0.5 s, LinModel and:
- 1 manipulated inputs u (0 integrators)
+ 1 manipulated inputs u (0 integrating states)
  3 states x̂
- 1 measured outputs ym (1 integrators)
+ 1 measured outputs ym (1 integrating states)
  1 unmeasured outputs yu
  0 measured disturbances d
 ```
@@ -418,9 +418,9 @@ julia> model = NonLinModel((x,u,_)->0.1x+u, (x,_)->2x, 10.0, 1, 1, 1);
 
 julia> estim = UnscentedKalmanFilter(model, σR=[1], nint_ym=[2], σP0int_ym=[1, 1])
 UnscentedKalmanFilter estimator with a sample time Ts = 10.0 s, NonLinModel and:
- 1 manipulated inputs u (0 integrators)
+ 1 manipulated inputs u (0 integrating states)
  3 states x̂
- 1 measured outputs ym (2 integrators)
+ 1 measured outputs ym (2 integrating states)
  0 unmeasured outputs yu
  0 measured disturbances d
 ```
@@ -646,9 +646,9 @@ julia> model = NonLinModel((x,u,_)->0.2x+u, (x,_)->-3x, 5.0, 1, 1, 1);
 
 julia> estim = ExtendedKalmanFilter(model, σQ=[2], σQint_ym=[2], σP0=[0.1], σP0int_ym=[0.1])
 ExtendedKalmanFilter estimator with a sample time Ts = 5.0 s, NonLinModel and:
- 1 manipulated inputs u (0 integrators)
+ 1 manipulated inputs u (0 integrating states)
  2 states x̂
- 1 measured outputs ym (1 integrators)
+ 1 measured outputs ym (1 integrating states)
  0 unmeasured outputs yu
  0 measured disturbances d
 ```

src/estimator/luenberger.jl

@@ -70,9 +70,9 @@ julia> model = LinModel([tf(3, [30, 1]); tf(-2, [5, 1])], 0.5);
 
 julia> estim = Luenberger(model, nint_ym=[1, 1], p̂=[0.61, 0.62, 0.63, 0.64])
 Luenberger estimator with a sample time Ts = 0.5 s, LinModel and:
- 1 manipulated inputs u (0 integrators)
+ 1 manipulated inputs u (0 integrating states)
  4 states x̂
- 2 measured outputs ym (2 integrators)
+ 2 measured outputs ym (2 integrating states)
  0 unmeasured outputs yu
  0 measured disturbances d
 ```

src/predictive_control.jl

@@ -96,9 +96,9 @@ julia> mpc = setconstraint!(mpc, Δumin=[-10], Δumax=[+10], c_Δumin=[1.0], c_
 LinMPC controller with a sample time Ts = 4.0 s, OSQP optimizer, SteadyKalmanFilter estimator and:
  10 prediction steps Hp
   2 control steps Hc
-  1 manipulated inputs u (0 integrators)
+  1 manipulated inputs u (0 integrating states)
   2 states x̂
-  1 measured outputs ym (1 integrators)
+  1 measured outputs ym (1 integrating states)
   0 unmeasured outputs yu
   0 measured disturbances d
 ```
@@ -296,8 +296,9 @@ end
 
 Get additional information about `mpc` controller optimum to ease troubleshooting.
 
-Return the optimizer solution summary that can be printed, `sol_summary`, and the dictionary 
-`info` with the following fields:
+The function should be called after calling [`moveinput!`](@ref). It returns the optimizer
+solution summary that can be printed, `sol_summary`, and the dictionary `info` with the 
+following fields:
 
 - `:ΔU` : optimal manipulated input increments over `Hc` ``(\mathbf{ΔU})``
 - `:ϵ`  : optimal slack variable ``(ϵ)``
@@ -336,7 +337,7 @@ function getinfo(mpc::PredictiveController)
     info[:D̂]   = mpc.D̂
     info[:ŷ]   = mpc.ŷ
     info[:Ŷ]   = Ŷ
-    info[:Ŷs]  = mpc.Ŷop - repeat(mpc.estim.model.yop, mpc.Hp)
+    info[:Ŷs]  = mpc.Ŷop - repeat(mpc.estim.model.yop, mpc.Hp) # Ŷop = Ŷs + Yop
     info[:R̂y]  = mpc.R̂y
     info[:R̂u]  = mpc.R̂u
     return sol_summary, info

src/state_estim.jl

@@ -46,9 +46,9 @@ end
 function print_estim_dim(io::IO, estim::StateEstimator, n)
     nu, nd = estim.model.nu, estim.model.nd
     nx̂, nym, nyu = estim.nx̂, estim.nym, estim.nyu
-    println(io, "$(lpad(nu, n)) manipulated inputs u ($(sum(estim.nint_u)) integrators)")
+    println(io, "$(lpad(nu, n)) manipulated inputs u ($(sum(estim.nint_u)) integrating states)")
     println(io, "$(lpad(nx̂, n)) states x̂")
-    println(io, "$(lpad(nym, n)) measured outputs ym ($(sum(estim.nint_ym)) integrators)")
+    println(io, "$(lpad(nym, n)) measured outputs ym ($(sum(estim.nint_ym)) integrating states)")
     println(io, "$(lpad(nyu, n)) unmeasured outputs yu")
     print(io,   "$(lpad(nd, n)) measured disturbances d")
 end

test/test_predictive_control.jl

@@ -72,6 +72,36 @@ end
     mpc3 = LinMPC(LinModel(tf(5, [2, 1]), 3), Mwt=[0], Nwt=[0], Lwt=[1], ru=[12])
     u = moveinput!(mpc3, [0])
     @test u ≈ [12] atol=1e-2
+    mpc_im = LinMPC(InternalModel(LinModel(tf(5, [2, 1]), 3)))
+    ym, u = mpc_im.estim.model() - [5], [0.0]
+    for i=1:25
+        ym = mpc_im.estim.model() - [5]
+        u = moveinput!(mpc_im, r; ym)
+        updatestate!(mpc_im, u, ym)
+        updatestate!(mpc_im.estim.model, u)
+    end
+    @test u  ≈ [2] atol=1e-2
+    @test ym ≈ [5] atol=1e-2
+    mpc_nint_u = LinMPC(SteadyKalmanFilter(LinModel(tf(5, [2, 1]), 3), nint_u=[1]))
+    ym, u = mpc_nint_u.estim.model() - [5], [0.0]
+    for i=1:25
+        ym = mpc_nint_u.estim.model() - [5]
+        u = moveinput!(mpc_nint_u, r; ym)
+        updatestate!(mpc_nint_u, u, ym)
+        updatestate!(mpc_nint_u.estim.model, u)
+    end
+    @test u  ≈ [2] atol=1e-2
+    @test ym ≈ [5] atol=1e-2 
+    mpc_nint_ym = LinMPC(SteadyKalmanFilter(LinModel(tf(5, [2, 1]), 3), nint_ym=[1]))
+    ym, u = mpc_nint_ym.estim.model() - [5], [0.0]
+    for i=1:25
+        ym = mpc_nint_ym.estim.model() - [5]
+        u = moveinput!(mpc_nint_ym, r; ym)
+        updatestate!(mpc_nint_ym, u, ym)
+        updatestate!(mpc_nint_ym.estim.model, u)
+    end
+    @test u  ≈ [2] atol=1e-2
+    @test ym ≈ [5] atol=1e-2 
 end
 
 @testset "LinMPC other methods" begin
@@ -136,6 +166,36 @@ end
     mpc3 = ExplicitMPC(LinModel(tf(5, [2, 1]), 3), Mwt=[0], Nwt=[0], Lwt=[1], ru=[12])
     u = moveinput!(mpc3, [0])
     @test u ≈ [12] atol=1e-2
+    mpc_im = ExplicitMPC(InternalModel(LinModel(tf(5, [2, 1]), 3)))
+    ym, u = mpc_im.estim.model() - [5], [0.0]
+    for i=1:25
+        ym = mpc_im.estim.model() - [5]
+        u = moveinput!(mpc_im, r; ym)
+        updatestate!(mpc_im, u, ym)
+        updatestate!(mpc_im.estim.model, u)
+    end
+    @test u  ≈ [2] atol=1e-2
+    @test ym ≈ [5] atol=1e-2
+    mpc_nint_u = ExplicitMPC(SteadyKalmanFilter(LinModel(tf(5, [2, 1]), 3), nint_u=[1]))
+    ym, u = mpc_nint_u.estim.model() - [5], [0.0]
+    for i=1:25
+        ym = mpc_nint_u.estim.model() - [5]
+        u = moveinput!(mpc_nint_u, r; ym)
+        updatestate!(mpc_nint_u, u, ym)
+        updatestate!(mpc_nint_u.estim.model, u)
+    end
+    @test u  ≈ [2] atol=1e-2
+    @test ym ≈ [5] atol=1e-2 
+    mpc_nint_ym = ExplicitMPC(SteadyKalmanFilter(LinModel(tf(5, [2, 1]), 3), nint_ym=[1]))
+    ym, u = mpc_nint_ym.estim.model() - [5], [0.0]
+    for i=1:25
+        ym = mpc_nint_ym.estim.model() - [5]
+        u = moveinput!(mpc_nint_ym, r; ym)
+        updatestate!(mpc_nint_ym, u, ym)
+        updatestate!(mpc_nint_ym.estim.model, u)
+    end
+    @test u  ≈ [2] atol=1e-2
+    @test ym ≈ [5] atol=1e-2
 end
 
 @testset "ExplicitMPC other methods" begin
@@ -231,16 +291,15 @@ end
     nonlinmodel = NonLinModel(f, h, 3.0, 1, 2, 1, 1)
     nmpc2 = NonLinMPC(nonlinmodel, Nwt=[0], Hp=1000, Hc=1)
     d = [0.1]
-    r = 7*d
-    u = moveinput!(nmpc2, r, d)
+    u = moveinput!(nmpc2, 7d, d)
     @test u ≈ [0] atol=5e-2
-    u = nmpc2(r, d)
+    u = nmpc2(7d, d)
     @test u ≈ [0] atol=5e-2
     _ , info = getinfo(nmpc2)
     @test info[:u] ≈ u
-    @test info[:Ŷ][end] ≈ r[1] atol=5e-2
+    @test info[:Ŷ][end] ≈ 7d[1] atol=5e-2
     nmpc3 = NonLinMPC(nonlinmodel, Nwt=[0], Cwt=Inf, Hp=1000, Hc=1)
-    u = moveinput!(nmpc3, r, d)
+    u = moveinput!(nmpc3, 7d, d)
     @test u ≈ [0] atol=5e-2
     nmpc4 = NonLinMPC(nonlinmodel, Mwt=[0], Nwt=[0], Lwt=[1], ru=[12])
     u = moveinput!(nmpc4, [0], d)
@@ -249,6 +308,37 @@ end
     C_Ymax_end = nmpc5.optim.nlp_model.operators.registered_multivariate_operators[end].f
     @test C_Ymax_end(Float64.((1.0, 1.0))) ≤ 0.0 # test con_nonlinprog_i(i,::NTuple{N, Float64})
     @test C_Ymax_end(Float32.((1.0, 1.0))) ≤ 0.0 # test con_nonlinprog_i(i,::NTuple{N, Real})
+    nmpc_im = NonLinMPC(InternalModel(LinModel(tf(5, [2, 1]), 3)))
+    ym, u = nmpc_im.estim.model() - [5], [0.0]
+    for i=1:25
+        ym = nmpc_im.estim.model() - [5]
+        u = moveinput!(nmpc_im, r; ym)
+        updatestate!(nmpc_im, u, ym)
+        updatestate!(nmpc_im.estim.model, u)
+    end
+    @test u  ≈ [2] atol=1e-2
+    @test ym ≈ [5] atol=1e-2    
+    nmpc_nint_u = NonLinMPC(SteadyKalmanFilter(LinModel(tf(5, [2, 1]), 3), nint_u=[1]))
+    ym, u = nmpc_nint_u.estim.model() - [5], [0.0]
+    for i=1:25
+        ym = nmpc_nint_u.estim.model() - [5]
+        u = moveinput!(nmpc_nint_u, r; ym)
+        updatestate!(nmpc_nint_u, u, ym)
+        updatestate!(nmpc_nint_u.estim.model, u)
+    end
+    @test u  ≈ [2] atol=1e-2
+    @test ym ≈ [5] atol=1e-2 
+    nmpc_nint_ym = NonLinMPC(SteadyKalmanFilter(LinModel(tf(5, [2, 1]), 3), nint_ym=[1]))
+    ym, u = nmpc_nint_ym.estim.model() - [5], [0.0]
+    for i=1:25
+        ym = nmpc_nint_ym.estim.model() - [5]
+        u = moveinput!(nmpc_nint_ym, r; ym)
+        updatestate!(nmpc_nint_ym, u, ym)
+        updatestate!(nmpc_nint_ym.estim.model, u)
+    end
+    @test u  ≈ [2] atol=1e-2
+    @test ym ≈ [5] atol=1e-2
+    
 end
 
 @testset "NonLinMPC other methods" begin