Merge pull request #39 from JuliaControl/doc_mistake

franckgaga · web-flow · commit 383ce4b4c04d · 2024-03-21T16:16:11.000-04:00
Minor doc correction and code cleanup
diff --git a/Project.toml b/Project.toml
@@ -1,7 +1,7 @@
 name = "ModelPredictiveControl"
 uuid = "61f9bdb8-6ae4-484a-811f-bbf86720c31c"
 authors = ["Francis Gagnon"]
-version = "0.19.0"
+version = "0.19.1"
 
 [deps]
 ControlSystemsBase = "aaaaaaaa-a6ca-5380-bf3e-84a91bcd477e"
diff --git a/docs/src/manual/nonlinmpc.md b/docs/src/manual/nonlinmpc.md
@@ -35,8 +35,9 @@ The plant model is nonlinear:
 
 in which ``g`` is the gravitational acceleration in m/s², ``L``, the pendulum length in m,
 ``K``, the friction coefficient at the pivot point in kg/s, and ``m``, the mass attached at
-the end of the pendulum in kg. Here, a [fourth order Runge-Kutta method](https://en.wikipedia.org/wiki/Runge%E2%80%93Kutta_methods)
-solves the differential equations to construct a [`NonLinModel`](@ref):
+the end of the pendulum in kg. The [`NonLinModel`](@ref) constructor assumes by default
+that the state function `f` is continuous in time, that is, an ordinary differential
+equation system (like here):
 
 ```@example 1
 using ModelPredictiveControl
diff --git a/src/controller/execute.jl b/src/controller/execute.jl
@@ -160,26 +160,26 @@ function initpred!(mpc::PredictiveController, model::LinModel, d, ym, D̂, R̂y,
     ŷ, F, q̃, p = mpc.ŷ, mpc.F, mpc.q̃, mpc.p
     ŷ .= evalŷ(mpc.estim, ym, d)
     predictstoch!(mpc, mpc.estim, d, ym) # init mpc.Ŷop for InternalModel
-    F_LHS = similar(F) 
-    F  .= mul!(F_LHS, mpc.K, mpc.estim.x̂) 
-    F .+= mul!(F_LHS, mpc.V, mpc.estim.lastu0)
-    F .+= mpc.Ŷop
+    F .= mpc.Ŷop
+    mul!(F, mpc.K, mpc.estim.x̂, 1, 1) 
+    mul!(F, mpc.V, mpc.estim.lastu0, 1, 1)
     if model.nd ≠ 0
         mpc.d0 .= d .- model.dop
         mpc.D̂0 .= D̂ .- mpc.Dop
         mpc.D̂E[1:model.nd]     .= mpc.d0
         mpc.D̂E[model.nd+1:end] .= mpc.D̂0
-        F .+= mul!(F_LHS, mpc.G, mpc.d0)
-        F .+= mul!(F_LHS, mpc.J, mpc.D̂0)
+        mul!(F, mpc.G, mpc.d0, 1, 1)
+        mul!(F, mpc.J, mpc.D̂0, 1, 1)
     end
     mpc.R̂y .= R̂y
-    C_y  = F_LHS
+    C_y  = similar(F)
     C_y .= mpc.F .- mpc.R̂y
     q̃   .= lmul!(2,(mpc.M_Hp*mpc.Ẽ)'*C_y)
     p   .= dot(C_y, mpc.M_Hp, C_y)
     if ~mpc.noR̂u
         mpc.R̂u .= R̂u
-        C_u = mpc.T_lastu .- mpc.R̂u
+        C_u  = similar(mpc.T_lastu)
+        C_u .= mpc.T_lastu .- mpc.R̂u
         mpc.q̃ .+= lmul!(2, (mpc.L_Hp*mpc.S̃)'*C_u)
         mpc.p .+= dot(C_u, mpc.L_Hp, C_u)
     end
@@ -232,12 +232,11 @@ function predictstoch!(
     ŷd .+= estim.model.yop 
     ŷs = zeros(NT, estim.model.ny)
     ŷs[estim.i_ym] .= @views ym .- ŷd[estim.i_ym]  # ŷs=0 for unmeasured outputs
-    Ŷop_LHS = similar(Ŷop)
-    Ŷop  .= mul!(Ŷop_LHS, mpc.Ks, estim.x̂s)
-    Ŷop .+= mul!(Ŷop_LHS, mpc.Ps, ŷs)
     for j=1:mpc.Hp
-        Ŷop[(1 + ny*(j-1)):(ny*j)] .+= yop
+        Ŷop[(1 + ny*(j-1)):(ny*j)] .= yop
     end
+    mul!(Ŷop, mpc.Ks, estim.x̂s, 1, 1)
+    mul!(Ŷop, mpc.Ps, ŷs, 1, 1)
     return nothing
 end
 "Separate stochastic predictions are not needed if `estim` is not [`InternalModel`](@ref)."
@@ -255,12 +254,11 @@ function linconstraint!(mpc::PredictiveController, model::LinModel)
     nU, nΔŨ, nY = length(mpc.con.Umin), length(mpc.con.ΔŨmin), length(mpc.con.Ymin)
     nx̂ = mpc.estim.nx̂
     fx̂ = mpc.con.fx̂
-    fx̂_LHS = similar(fx̂)
-    fx̂  .= mul!(fx̂_LHS, mpc.con.kx̂, mpc.estim.x̂)
-    fx̂ .+= mul!(fx̂_LHS, mpc.con.vx̂, mpc.estim.lastu0)
+    mul!(fx̂, mpc.con.kx̂, mpc.estim.x̂)
+    mul!(fx̂, mpc.con.vx̂, mpc.estim.lastu0, 1, 1)
     if model.nd ≠ 0
-        fx̂ .+= mul!(fx̂_LHS, mpc.con.gx̂, mpc.d0)
-        fx̂ .+= mul!(fx̂_LHS, mpc.con.jx̂, mpc.D̂0)
+        mul!(fx̂, mpc.con.gx̂, mpc.d0, 1, 1)
+        mul!(fx̂, mpc.con.jx̂, mpc.D̂0, 1, 1)
     end
     n = 0
     mpc.con.b[(n+1):(n+nU)]  .= @. -mpc.con.Umin + mpc.T_lastu
diff --git a/src/estimator/execute.jl b/src/estimator/execute.jl
@@ -43,8 +43,7 @@ end
 
 "Use the augmented model matrices if `model` is a [`LinModel`](@ref)."
 function f̂!(x̂next, estim::StateEstimator, ::LinModel, x̂, u, d)
-    x̂next .= 0
-    mul!(x̂next, estim.Â,  x̂, 1, 1)
+    mul!(x̂next, estim.Â,  x̂)
     mul!(x̂next, estim.B̂u, u, 1, 1)
     mul!(x̂next, estim.B̂d, d, 1, 1)
     return nothing
@@ -64,8 +63,7 @@ function ĥ!(ŷ, estim::StateEstimator, model::SimModel, x̂, d)
 end
 "Use the augmented model matrices if `model` is a [`LinModel`](@ref)."
 function ĥ!(ŷ, estim::StateEstimator, ::LinModel, x̂, d)
-    ŷ .= 0
-    mul!(ŷ, estim.Ĉ,  x̂, 1, 1)
+    mul!(ŷ, estim.Ĉ,  x̂)
     mul!(ŷ, estim.D̂d, d, 1, 1)
     return nothing
 end
diff --git a/src/estimator/kalman.jl b/src/estimator/kalman.jl
@@ -182,15 +182,17 @@ The [`SteadyKalmanFilter`](@ref) updates it with the precomputed Kalman gain ``\
 function update_estimate!(estim::SteadyKalmanFilter, u, ym, d=empty(estim.x̂))
     Â, B̂u, B̂d, Ĉm, D̂dm = estim.Â, estim.B̂u, estim.B̂d, estim.Ĉm, estim.D̂dm
     x̂, K̂ = estim.x̂, estim.K̂
-    v̂, ŷm, x̂LHS = similar(ym), similar(ym), similar(x̂)
-    # in-place operations to recuce allocations:
-    ŷm  .= mul!(v̂, Ĉm, x̂) 
-    ŷm .+= mul!(v̂, D̂dm, d)
-    v̂   .= ym .- ŷm
-    x̂   .= mul!(x̂LHS, Â, x̂)
-    x̂  .+= mul!(x̂LHS, B̂u, u)
-    x̂  .+= mul!(x̂LHS, B̂d, d)
-    x̂  .+= mul!(x̂LHS, K̂, v̂)
+    ŷm, x̂next = similar(ym), similar(x̂)
+    # in-place operations to reduce allocations:
+    mul!(ŷm, Ĉm, x̂) 
+    mul!(ŷm, D̂dm, d, 1, 1)
+    v̂  = ŷm
+    v̂ .= ym .- ŷm
+    mul!(x̂next, Â, x̂)
+    mul!(x̂next, B̂u, u, 1, 1)
+    mul!(x̂next, B̂d, d, 1, 1)
+    mul!(x̂next, K̂, v̂, 1, 1)
+    x̂ .= x̂next
     return nothing
 end
 
diff --git a/src/estimator/luenberger.jl b/src/estimator/luenberger.jl
@@ -108,14 +108,16 @@ Same than [`update_estimate!(::SteadyKalmanFilter)`](@ref) but using [`Luenberge
 function update_estimate!(estim::Luenberger, u, ym, d=empty(estim.x̂))
     Â, B̂u, B̂d, Ĉm, D̂dm = estim.Â, estim.B̂u, estim.B̂d, estim.Ĉm, estim.D̂dm
     x̂, K̂ = estim.x̂, estim.K̂
-    v̂, ŷm, x̂LHS = similar(ym), similar(ym), similar(x̂)
-    # in-place operations to recuce allocations:
-    ŷm  .= mul!(v̂, Ĉm, x̂) 
-    ŷm .+= mul!(v̂, D̂dm, d)
-    v̂   .= ym .- ŷm
-    x̂   .= mul!(x̂LHS, Â, x̂)
-    x̂  .+= mul!(x̂LHS, B̂u, u)
-    x̂  .+= mul!(x̂LHS, B̂d, d)
-    x̂  .+= mul!(x̂LHS, K̂, v̂)
+    ŷm, x̂next = similar(ym), similar(x̂)
+    # in-place operations to reduce allocations:
+    mul!(ŷm, Ĉm, x̂) 
+    mul!(ŷm, D̂dm, d, 1, 1)
+    v̂  = ŷm
+    v̂ .= ym .- ŷm
+    mul!(x̂next, Â, x̂)
+    mul!(x̂next, B̂u, u, 1, 1)
+    mul!(x̂next, B̂d, d, 1, 1)
+    mul!(x̂next, K̂, v̂, 1, 1)
+    x̂ .= x̂next
     return nothing
 end
diff --git a/src/estimator/mhe/construct.jl b/src/estimator/mhe/construct.jl
@@ -187,7 +187,7 @@ N_k =                     \begin{cases}
 The vectors ``\mathbf{Ŵ}`` and ``\mathbf{V̂}`` encompass the estimated process noise
 ``\mathbf{ŵ}(k-j)`` and sensor noise ``\mathbf{v̂}(k-j)`` from ``j=N_k-1`` to ``0``. The 
 Extended Help defines the two vectors and the scalar ``ϵ``. See [`UnscentedKalmanFilter`](@ref)
-for details on he augmented process model and ``\mathbf{R̂}, \mathbf{Q̂}`` covariances. The
+for details on the augmented process model and ``\mathbf{R̂}, \mathbf{Q̂}`` covariances. The
 matrix ``\mathbf{P̂}_{k-N_k}(k-N_k+1)`` is estimated with an [`ExtendedKalmanFilter`](@ref).
 
 !!! warning
diff --git a/src/estimator/mhe/execute.jl b/src/estimator/mhe/execute.jl
@@ -221,17 +221,16 @@ of the time-varying ``\mathbf{P̄}`` covariance . The computed variables are:
 ```
 """
 function initpred!(estim::MovingHorizonEstimator, model::LinModel)
-    C, optim = estim.C, estim.optim
+    F, C, optim = estim.F, estim.C, estim.optim
     nϵ = isinf(C) ? 0 : 1
     nx̂, nŵ, nym, Nk = estim.nx̂, estim.nx̂, estim.nym, estim.Nk[]
     nYm, nŴ = nym*Nk, nŵ*Nk
     nZ̃ = nϵ + nx̂ + nŴ
     # --- update F and fx̄ vectors for MHE predictions ---
-    F_LHS = similar(estim.F)
-    estim.F  .= mul!(F_LHS, estim.G, estim.U)
-    estim.F .+= estim.Ym
+    F .= estim.Ym
+    mul!(F, estim.G, estim.U, 1, 1)
     if model.nd ≠ 0
-        estim.F .+= mul!(F_LHS, estim.J, estim.D)
+        mul!(F, estim.J, estim.D, 1, 1)
     end
     estim.fx̄ .= estim.x̂arr_old
     # --- update H̃, q̃ and p vectors for quadratic optimization ---
@@ -261,10 +260,10 @@ Also init ``\mathbf{F_x̂ = G_x̂ U + J_x̂ D}`` vector for the state constraint
 [`init_predmat_mhe`](@ref).
 """
 function linconstraint!(estim::MovingHorizonEstimator, model::LinModel)
-    Fx̂_LHS = similar(estim.con.Fx̂)
-    estim.con.Fx̂ .= mul!(Fx̂_LHS, estim.con.Gx̂, estim.U)
+    Fx̂ = estim.con.Fx̂
+    mul!(Fx̂, estim.con.Gx̂, estim.U)
     if model.nd ≠ 0
-        estim.con.Fx̂ .+= mul!(Fx̂_LHS, estim.con.Jx̂, estim.D)
+        mul!(Fx̂, estim.con.Jx̂, estim.D, 1, 1)
     end
     X̂min, X̂max = trunc_bounds(estim, estim.con.X̂min, estim.con.X̂max, estim.nx̂)
     Ŵmin, Ŵmax = trunc_bounds(estim, estim.con.Ŵmin, estim.con.Ŵmax, estim.nx̂)
@@ -276,9 +275,9 @@ function linconstraint!(estim::MovingHorizonEstimator, model::LinModel)
     n += nx̃
     estim.con.b[(n+1):(n+nx̃)] .= @. +estim.con.x̃max
     n += nx̃
-    estim.con.b[(n+1):(n+nX̂)] .= @. -X̂min + estim.con.Fx̂
+    estim.con.b[(n+1):(n+nX̂)] .= @. -X̂min + Fx̂
     n += nX̂
-    estim.con.b[(n+1):(n+nX̂)] .= @. +X̂max - estim.con.Fx̂
+    estim.con.b[(n+1):(n+nX̂)] .= @. +X̂max - Fx̂
     n += nX̂
     estim.con.b[(n+1):(n+nŴ)] .= @. -Ŵmin
     n += nŴ
diff --git a/src/model/linmodel.jl b/src/model/linmodel.jl
@@ -229,7 +229,9 @@ disturbances ``\mathbf{d_0 = d - d_{op}}``. The Moore-Penrose pseudo-inverse com
 ``\mathbf{(I - A)^{-1}}`` to support integrating `model` (integrator states will be 0).
 """
 function steadystate!(model::LinModel, u, d)
-    model.x .= pinv(I - model.A)*(model.Bu*(u - model.uop) + model.Bd*(d - model.dop))
+    M = I - model.A
+    rtol = sqrt(eps(real(float(oneunit(eltype(M)))))) # pinv docstring recommendation
+    model.x .= pinv(M; rtol)*(model.Bu*(u - model.uop) + model.Bd*(d - model.dop))
     return nothing
 end
 
@@ -239,8 +241,7 @@ end
 Evaluate `xnext = A*x + Bu*u + Bd*d` in-place when `model` is a [`LinModel`](@ref).
 """
 function f!(xnext, model::LinModel, x, u, d)
-    xnext .= 0
-    mul!(xnext, model.A,  x, 1, 1)
+    mul!(xnext, model.A,  x)
     mul!(xnext, model.Bu, u, 1, 1)
     mul!(xnext, model.Bd, d, 1, 1)
     return nothing
@@ -253,8 +254,7 @@ end
 Evaluate `y = C*x + Dd*d` in-place when `model` is a [`LinModel`](@ref).
 """
 function h!(y, model::LinModel, x, d)
-    y .= 0
-    mul!(y, model.C,  x, 1, 1)
+    mul!(y, model.C,  x)
     mul!(y, model.Dd, d, 1, 1)
     return nothing
 end
diff --git a/test/test_predictive_control.jl b/test/test_predictive_control.jl
@@ -83,7 +83,7 @@ end
     r = [15]
     outdist = [5]
     mpc_im = LinMPC(InternalModel(linmodel))
-    linmodel.x[:] .= 0
+    linmodel.x .= 0
     ym, u = linmodel() - outdist, [0.0]
     for i=1:25
         ym = linmodel() - outdist
@@ -94,7 +94,7 @@ end
     @test u  ≈ [2] atol=1e-2
     @test ym ≈ r   atol=1e-2
     mpc_nint_u = LinMPC(SteadyKalmanFilter(LinModel(tf(5, [2, 1]), 3), nint_u=[1]))
-    linmodel.x[:] .= 0
+    linmodel.x .= 0
     ym, u = linmodel() - outdist, [0.0]
     for i=1:25
         ym = linmodel() - outdist
@@ -105,7 +105,7 @@ end
     @test u  ≈ [2] atol=1e-2
     @test ym ≈ r   atol=1e-2 
     mpc_nint_ym = LinMPC(SteadyKalmanFilter(LinModel(tf(5, [2, 1]), 3), nint_ym=[1]))
-    linmodel.x[:] .= 0
+    linmodel.x .= 0
     ym, u = linmodel() - outdist, [0.0]
     for i=1:25
         ym = linmodel() - outdist
@@ -313,7 +313,7 @@ end
     r = [15]
     outdist = [5]
     mpc_im = ExplicitMPC(InternalModel(linmodel))
-    linmodel.x[:] .= 0
+    linmodel.x .= 0
     ym, u = linmodel() - outdist, [0.0]
     for i=1:25
         ym = linmodel() - outdist
@@ -324,7 +324,7 @@ end
     @test u  ≈ [2] atol=1e-2
     @test ym ≈ r   atol=1e-2
     mpc_nint_u = ExplicitMPC(SteadyKalmanFilter(LinModel(tf(5, [2, 1]), 3), nint_u=[1]))
-    linmodel.x[:] .= 0
+    linmodel.x .= 0
     ym, u = linmodel() - outdist, [0.0]
     for i=1:25
         ym = linmodel() - outdist
@@ -335,7 +335,7 @@ end
     @test u  ≈ [2] atol=1e-2
     @test ym ≈ r   atol=1e-2 
     mpc_nint_ym = ExplicitMPC(SteadyKalmanFilter(LinModel(tf(5, [2, 1]), 3), nint_ym=[1]))
-    linmodel.x[:] .= 0
+    linmodel.x .= 0
     ym, u = linmodel() - outdist, [0.0]
     for i=1:25
         ym = linmodel() - outdist
@@ -475,7 +475,7 @@ end
     r = [15]
     outdist = [5]
     nmpc_im = NonLinMPC(InternalModel(linmodel))
-    linmodel.x[:] .= 0
+    linmodel.x .= 0
     ym, u = linmodel() - outdist, [0.0]
     for i=1:25
         ym = linmodel() - outdist
@@ -486,7 +486,7 @@ end
     @test u  ≈ [2] atol=1e-2
     @test ym ≈ r   atol=1e-2
     nmpc_nint_u = NonLinMPC(SteadyKalmanFilter(linmodel, nint_u=[1]))
-    linmodel.x[:] .= 0
+    linmodel.x .= 0
     ym, u = linmodel() - outdist, [0.0]
     for i=1:25
         ym = linmodel() - outdist
@@ -497,7 +497,7 @@ end
     @test u  ≈ [2] atol=1e-2
     @test ym ≈ r   atol=1e-2 
     nmpc_nint_ym = NonLinMPC(SteadyKalmanFilter(linmodel, nint_ym=[1]))
-    linmodel.x[:] .= 0
+    linmodel.x .= 0
     ym, u = linmodel() - outdist, [0.0]
     for i=1:25
         ym = linmodel() - outdist
diff --git a/test/test_sim_model.jl b/test/test_sim_model.jl
@@ -137,15 +137,13 @@ end
     @test isa(nonlinmodel3, NonLinModel{Float32})
 
     function f1!(xnext, x, u, d)
-        xnext .= 0
-        mul!(xnext, linmodel2.A,  x, 1, 1)
+        mul!(xnext, linmodel2.A,  x)
         mul!(xnext, linmodel2.Bu, u, 1, 1)
         mul!(xnext, linmodel2.Bd, d, 1, 1)
         return nothing
     end 
     function h1!(y, x, d)
-        y .= 0
-        mul!(y, linmodel2.C,  x, 1, 1)
+        mul!(y, linmodel2.C,  x)
         mul!(y, linmodel2.Dd, d, 1, 1)
         return nothing
     end
@@ -171,15 +169,13 @@ end
     @test y ≈ zeros(1)
 
     function f2!(ẋ, x, u , d)
-        ẋ .= 0
-        mul!(ẋ, A, x, 1, 1)
+        mul!(ẋ, A, x)
         mul!(ẋ, Bu, u, 1, 1)
         mul!(ẋ, Bd, d, 1, 1)
         return nothing
     end
     function h2!(y, x, d)
-        y .= 0
-        mul!(y, C, x, 1, 1)
+        mul!(y, C, x)
         mul!(y, Dd, d, 1, 1)
         return nothing
     end
