NonLinMHE continuation

Author: franckgaga

docs/make.jl

@@ -14,6 +14,7 @@ DocMeta.setdocmeta!(
 )
 makedocs(
     sitename    = "ModelPredictiveControl.jl",
+    #format = Documenter.LaTeX(platform = "none"),
     doctest     = true,
     format      = Documenter.HTML(
         prettyurls = get(ENV, "CI", nothing) == "true",

docs/src/manual/installation.md

@@ -6,10 +6,8 @@ To install the `ModelPredictiveControl` package, run this command in the Julia R
 using Pkg; Pkg.add("ModelPredictiveControl")
 ```
 
-It will also automatically install all the dependencies, including [`JuMP.jl`](https://github.com/jump-dev/JuMP.jl)
-and [`ControlSystemsBase.jl`](https://github.com/JuliaControl/ControlSystems.jl). Note that
-that the construction of linear models typically requires `ss` or `tf` functions, it is thus
-recommended to load the package with:
+Note that that the construction of linear models typically requires `ss` or `tf` functions,
+it is thus recommended to load the package with:
 
 ```julia
 using ModelPredictiveControl, ControlSystemsBase

example/juMPC.jl

@@ -57,6 +57,11 @@ updatestate!(luenberger, [0,0], [0,0])
 
 mpcluen = LinMPC(luenberger)
 
+mhe = NonLinMHE(nonLinModel2)
+
+updatestate!(mhe, [0, 0], [0, 0], [1])
+
+#=
 kalmanFilter1 = KalmanFilter(linModel1)
 kalmanFilter2 = KalmanFilter(linModel1,nint_ym=0)
 
@@ -207,4 +212,5 @@ display(pd)
 display(pu)
 display(py)
 
+=#
 =#

src/ModelPredictiveControl.jl

@@ -14,9 +14,13 @@ import OSQP, Ipopt
 export SimModel, LinModel, NonLinModel, setop!, setstate!, updatestate!, evaloutput
 export StateEstimator, InternalModel, Luenberger
 export SteadyKalmanFilter, KalmanFilter, UnscentedKalmanFilter, ExtendedKalmanFilter
+export NonLinMHE
 export initstate!
 export PredictiveController, LinMPC, NonLinMPC, setconstraint!, moveinput!, getinfo, sim!
 
+"Generate a block diagonal matrix repeating `n` times the matrix `A`."
+repeatdiag(A, n::Int) = kron(I(n), A)
+
 include("sim_model.jl")
 include("state_estim.jl")
 include("predictive_control.jl")

src/controller/nonlinmpc.jl

@@ -324,7 +324,7 @@ end
 """
     obj_nonlinprog(mpc::NonLinMPC, model::LinModel, ΔŨ::Vector{Real})
 
-Objective function for [`NonLinMPC`] when `model` is a [`LinModel`](@ref).
+Objective function for [`NonLinMPC`](@ref) when `model` is a [`LinModel`](@ref).
 """
 function obj_nonlinprog(mpc::NonLinMPC, model::LinModel, Ŷ, ΔŨ::Vector{T}) where {T<:Real}
     J = obj_quadprog(ΔŨ, mpc.P̃, mpc.q̃)
@@ -341,7 +341,7 @@ end
 """
     obj_nonlinprog(mpc::NonLinMPC, model::SimModel, ΔŨ::Vector{Real})
 
-Objective function for [`NonLinMPC`] when `model` is not a [`LinModel`](@ref).
+Objective function for [`NonLinMPC`](@ref) when `model` is not a [`LinModel`](@ref).
 """
 function obj_nonlinprog(mpc::NonLinMPC, model::SimModel, Ŷ, ΔŨ::Vector{T}) where {T<:Real}
     # --- output setpoint tracking term ---

src/estimator/mhe.jl

@@ -1,6 +1,7 @@
 struct NonLinMHE{M<:SimModel} <: StateEstimator
     model::M
     optim::JuMP.Model
+    W̃::Vector{Float64}
     lastu0::Vector{Float64}
     x̂::Vector{Float64}
     P̂::Hermitian{Float64, Matrix{Float64}}
@@ -12,36 +13,51 @@ struct NonLinMHE{M<:SimModel} <: StateEstimator
     As::Matrix{Float64}
     Cs::Matrix{Float64}
     nint_ym::Vector{Int}
+    He::Int
     P̂0::Hermitian{Float64, Matrix{Float64}}
-    Q̂::Hermitian{Float64, Matrix{Float64}}
-    R̂::Hermitian{Float64, Matrix{Float64}}
-    He::Int 
-    X̂max::Vector{Float64}
+    Q̂_He::Hermitian{Float64, Matrix{Float64}}
+    R̂_He::Hermitian{Float64, Matrix{Float64}}
     X̂min::Vector{Float64}
-    function NonLinMHE{M}(model::M, i_ym, nint_ym, P̂0, Q̂, R̂, He) where {M<:SimModel}
-        nu, nx, ny = model.nu, model.nx, model.ny
+    X̂max::Vector{Float64}
+    X̂ ::Vector{Float64}
+    Ym::Vector{Float64}
+    U ::Vector{Float64}
+    D ::Vector{Float64}
+    Ŵ ::Vector{Float64}
+    x̂0_past::Vector{Float64}
+    function NonLinMHE{M}(model::M, i_ym, nint_ym, He, P̂0, Q̂, R̂, optim) where {M<:SimModel}
+        nu, nd, nx, ny = model.nu, model.nd, model.nx, model.ny
+        He < 1  && error("Estimation horizon He should be ≥ 1")
         nym, nyu = length(i_ym), ny - length(i_ym)
         Asm, Csm, nint_ym = init_estimstoch(i_ym, nint_ym)
         nxs = size(Asm,1)
         nx̂ = nx + nxs
         validate_kfcov(nym, nx̂, Q̂, R̂, P̂0)
         As, _ , Cs, _  = stoch_ym2y(model, i_ym, Asm, [], Csm, [])
-        nσ, γ, m̂, Ŝ = init_mhe(nx̂, He)
         i_ym = collect(i_ym)
         lastu0 = zeros(nu)
         x̂ = [zeros(model.nx); zeros(nxs)]
         P̂0 = Hermitian(P̂0, :L)
-        Q̂ = Hermitian(Q̂, :L)
-        R̂ = Hermitian(R̂, :L)
+        Q̂_He = Hermitian(repeatdiag(Q̂, He), :L)
+        R̂_He = Hermitian(repeatdiag(R̂, He), :L)
         P̂ = copy(P̂0)
-        return new(
-            model,
+        X̂min, X̂max = fill(-Inf, nx̂*He), fill(+Inf, nx̂*He)
+        nvar = nx̂*(He + 1) 
+        W̃ = zeros(nvar)
+        X̂, Ym, U, D, Ŵ = zeros(nx̂*He), zeros(nym*He), zeros(nu*He), zeros(nd*He), zeros(nx̂*He)
+        x̂0_past = zeros(nx̂)
+        estim = new(
+            model, optim, W̃,
             lastu0, x̂, P̂, 
             i_ym, nx̂, nym, nyu, nxs, 
             As, Cs, nint_ym,
-            P̂0, Q̂, R̂,
-            nσ, γ, m̂, Ŝ
+            He, P̂0, Q̂_He, R̂_He,
+            X̂min, X̂max,
+            X̂, Ym, U, D, Ŵ,
+            x̂0_past
         )
+        init_optimization!(estim)
+        return estim
     end
 end
 
@@ -68,19 +84,20 @@ julia> model = NonLinModel((x,u,_)->0.1x+u, (x,_)->2x, 10.0, 1, 1, 1);
 function NonLinMHE(
     model::M;
     i_ym::IntRangeOrVector = 1:model.ny,
+    He::Int = 10,
     σP0::Vector = fill(1/model.nx, model.nx),
     σQ::Vector  = fill(1/model.nx, model.nx),
     σR::Vector  = fill(1, length(i_ym)),
     nint_ym::IntVectorOrInt = fill(1, length(i_ym)),
     σP0_int::Vector = fill(1, max(sum(nint_ym), 0)),
     σQ_int::Vector  = fill(1, max(sum(nint_ym), 0)),
-    He::Int = 10
+    optim::JuMP.Model = JuMP.Model(optimizer_with_attributes(Ipopt.Optimizer,"sb"=>"yes"))
 ) where {M<:SimModel}
     # estimated covariances matrices (variance = σ²) :
     P̂0 = Diagonal{Float64}([σP0  ; σP0_int   ].^2);
     Q̂  = Diagonal{Float64}([σQ   ; σQ_int    ].^2);
     R̂  = Diagonal{Float64}(σR.^2);
-    return NonLinMHE{M}(model, i_ym, nint_ym, P̂0, Q̂ , R̂, He)
+    return NonLinMHE{M}(model, i_ym, nint_ym, He, P̂0, Q̂, R̂, optim)
 end
 
 @doc raw"""
@@ -90,4 +107,143 @@ Construct the estimator from the augmented covariance matrices `P̂0`, `Q̂` and
 
 This syntax allows nonzero off-diagonal elements in ``\mathbf{P̂}_{-1}(0), \mathbf{Q̂, R̂}``.
 """
-NonLinMHE{M}(model::M, i_ym, nint_ym, P̂0, Q̂, R̂, He) where {M<:SimModel}
+NonLinMHE{M}(model::M, i_ym, nint_ym, P̂0, Q̂, R̂, He) where {M<:SimModel}
+
+@doc raw"""
+    init_stochpred(As, Cs, He)
+
+Construct stochastic model prediction matrices for nonlinear observers.
+   
+It allows separate simulations of the stochastic model (integrators). Stochastic model 
+states and outputs are simulated using :
+```math
+\begin{aligned}
+    \mathbf{X̂_s} &= \mathbf{M_s x̂_s}(k) + \mathbf{N_s Ŵ_s} 
+    \mathbf{Ŷ_s} &= \mathbf{P_s X̂_s}
+\end{aligned}
+```
+where ``\mathbf{X̂_s}`` and ``\mathbf{Ŷ_s}`` are integrator states and outputs from ``k + 1`` 
+to ``k + H_e`` inclusively, and ``\mathbf{Ŵ_s}`` are integrator process noises from ``k`` to
+``k + H_e - 1``. Stochastic simulations are combined with ``\mathbf{f, h}`` results to 
+simulate the augmented model:
+```math
+\begin{aligned}
+    \mathbf{X̂} &= \mathbf{M_s x̂_s}(k) + \mathbf{N_s Ŵ_s} 
+    \mathbf{Ŷ_s} &= \mathbf{P_s X̂_s}
+\end{aligned}
+```
+       Xhat = [XhatD; XhatS]
+       Yhat = YhatD + YhatS
+where XhatD and YhatD are SimulFunc states and outputs from ``k + 1`` to ``k + H_e``.
+"""
+function init_stochpred(As, Cs, He)
+    nxs = size(As,1);
+    Ms = zeros(He*nxs, nxs);
+    for i = 1:Ho
+        iRow = (1:nxs) + nxs*(i-1);
+        Ms[iRow, :] = As^i;
+    end
+    Ns = zeros(Ho*nxs, He*nxs);
+    for i = 1:Ho
+        iCol = (1:nxs) + nxs*(i-1);
+        Ns[nxs*(i-1)+1:end, iCol] = [eye(nxs); Ms(1:nxs*(Ho-i),:)];
+    end
+    Ps = kron(eye(Ho),Cs);
+    return (Ms, Ns, Ps)
+end
+    
+    
+
+"""
+    init_optimization!(estim::NonLinMHE)
+
+Init the nonlinear optimization for [`NonLinMHE`](@ref).
+"""
+function init_optimization!(estim::NonLinMHE)
+       # --- variables and linear constraints ---
+       optim = estim.optim
+       nvar = length(estim.W̃)
+       set_silent(optim)
+       @variable(optim, W̃var[1:nvar])
+       W̃var = optim[:W̃var]
+       # --- nonlinear optimization init ---
+       model = estim.model
+       nym, nx̂, He = estim.nym, estim.nx̂, estim.He
+       # inspired from https://jump.dev/JuMP.jl/stable/tutorials/nonlinear/tips_and_tricks/#User-defined-functions-with-vector-outputs
+       Jfunc = let estim=estim, model=model, nvar=nvar , nŶm=He*nym, nX̂=He*nx̂
+            last_W̃tup_float, last_W̃tup_dual = nothing, nothing
+            Ŷm_cache::DiffCacheType = DiffCache(zeros(nŶm), nvar + 3)
+            X̂_cache ::DiffCacheType = DiffCache(zeros(nX̂) , nvar + 3)
+            function Jfunc(W̃tup::Float64...)
+                Ŷm, X̂ = get_tmp(Ŷm_cache, W̃tup[1]), get_tmp(X̂_cache, W̃tup[1])
+                W̃ = collect(W̃tup)
+                if W̃tup != last_W̃tup_float
+                    Ŷm[:], X̂[:] = predict(estim, model, W̃)
+                    last_W̃tup_float = W̃tup
+                end
+                return obj_nonlinprog(estim, model, Ŷm, W̃)
+            end
+            function Jfunc(W̃tup::Real...)
+                Ŷm, X̂ = get_tmp(Ŷm_cache, W̃tup[1]), get_tmp(X̂_cache, W̃tup[1])
+                W̃ = collect(W̃tup)
+                if W̃tup != last_W̃tup_dual
+                    Ŷm[:], X̂[:] = predict(estim, model, W̃)
+                    last_W̃tup_dual = W̃tup
+                end
+                return obj_nonlinprog(estim, model, Ŷm, W̃)
+            end
+            Jfunc
+       end
+       register(optim, :Jfunc, nvar, Jfunc, autodiff=true)
+       @NLobjective(optim, Min, Jfunc(W̃var...))
+       return nothing
+end
+
+"""
+    obj_nonlinprog(estim::NonLinMHE, model::SimModel, ΔŨ::Vector{Real})
+
+Objective function for [`NonLinMHE`] when `model` is not a [`LinModel`](@ref).
+"""
+function obj_nonlinprog(estim::NonLinMHE, ::SimModel, Ŷm, W̃::Vector{T}) where {T<:Real}
+    # `@views` macro avoid copies with matrix slice operator e.g. [a:b]
+    @views x̄0 = W̃[1:estim.nx̂] - estim.x̂0_past
+    V̂  = estim.win_Ym - Ŷm
+    @views Ŵ = W̃[estim.nx̂+1:end]
+    return x̄0'/estim.P̂0*x̄0 + Ŵ'/Q̂_He*Ŵ + V̂'/R̂_He*V̂
+end
+
+function predict(estim::NonLinMHE, model::SimModel, W̃::Vector{T}) where {T<:Real}
+    nu, nd, nym, nx̂, He = model.nu, model.nd, estim.nym, estim.nx̂, estim.He
+    Ŷm::Vector{T} = Vector{T}(undef, nym*(He+1))
+    X̂ ::Vector{T} = Vector{T}(undef, nx̂*(He+1))
+    Ŵ ::Vector{T} = @views W̃[nx̂+1:end]
+    u ::Vector{T} = Vector{T}(undef, nu)
+    d ::Vector{T} = Vector{T}(undef, nu)
+    ŵ ::Vector{T} = Vector{T}(undef, nx̂)
+    x̂ ::Vector{T} = W̃[1:nx̂]
+    for j=1:He
+        u[:] = @views estim.U[(1 + nu*(j-1)):(nu*j)]
+        d[:] = @views estim.D[(1 + nd*(j-1)):(nd*j)]
+        ŵ[:] = @views Ŵ[(1 + nx̂*(j-1)):(nx̂*j)]
+        X̂[(1 + nx̂*(j-1)):(nx̂*j)] = x̂
+        Ŷm[(1 + ny*(j-1)):(ny*j)] = @views ĥ(estim, x̂, d)[estim.i_ym]
+        x̂[:] = f̂(estim, x̂, u, d) + ŵ
+    end
+    X̂[end-nx̂+1:end] = x̂
+    Ŷm[end-nx̂+1:end] = @views ĥ(estim, x̂, estin.D[end-nd+1:end])[estim.i_ym]
+    return Ŷm, X̂
+end
+
+function update_estimate!(estim::NonLinMHE, u, ym, d)
+    model, x̂ = estim.model, estim.x̂
+    nx̂, nym, nu, nd, nŵ = estim.nx̂, estim.nym, model.nu, model.nd, estim.nx̂
+    ŵ = zeros(nŵ) # ŵ(k) = 0 for warm-starting
+    # --- adding new data in time windows ---
+    estim.X̂[:]  = @views [estim.X̂[nx̂+1:end]  ; x̂]
+    estim.Ym[:] = @views [estim.Ym[nym+1:end]; ym]
+    estim.U[:]  = @views [estim.U[nu+1:end]  ; u]
+    estim.D[:]  = @views [estim.D[nd+1:end]  ; d]
+    estim.Ŵ[:]  = @views [estim.Ŵ[nŵ+1:end]  ; ŵ]
+    
+    #estim.x̂0_past[:] =
+end

src/predictive_control.jl

@@ -449,19 +449,19 @@ function initpred!(mpc::PredictiveController, model::SimModel, d, D̂, R̂y)
     return nothing
 end
 
-"""
+@doc raw"""
     predict(mpc::PredictiveController, model::LinModel, ΔŨ)
 
-Evaluate the outputs predictions ``\\mathbf{Ŷ}`` when `model` is a [`LinModel`](@ref).
+Evaluate the outputs predictions ``\mathbf{Ŷ}`` when `model` is a [`LinModel`](@ref).
 """
 function predict(mpc::PredictiveController, ::LinModel, ΔŨ::Vector{T}) where {T<:Real}
     return mpc.Ẽ*ΔŨ + mpc.F
 end
 
-"""
+@doc raw"""
     predict(mpc::PredictiveController, model::SimModel, ΔŨ)
 
-Evaluate  ``\\mathbf{Ŷ}`` when `model` is not a [`LinModel`](@ref).
+Evaluate  ``\mathbf{Ŷ}`` when `model` is not a [`LinModel`](@ref).
 """
 function predict(mpc::PredictiveController, model::SimModel, ΔŨ::Vector{T}) where {T<:Real}
     nu, ny, nd, Hp = model.nu, model.ny, model.nd, mpc.Hp
@@ -1000,9 +1000,6 @@ function validate_weights(model, Hp, Hc, Mwt, Nwt, Lwt, Cwt, ru, Ewt=nothing)
     !isnothing(Ewt) && size(Ewt) ≠ () && error("Ewt should be a real scalar")
 end
 
-"Generate a block diagonal matrix repeating `n` times the matrix `A`."
-repeatdiag(A, n::Int) = kron(I(n), A)
-
 function Base.show(io::IO, mpc::PredictiveController)
     Hp, Hc = mpc.Hp, mpc.Hc
     nu, nd = mpc.estim.model.nu, mpc.estim.model.nd

src/state_estim.jl

@@ -303,6 +303,7 @@ end
 include("estimator/kalman.jl")
 include("estimator/luenberger.jl")
 include("estimator/internal_model.jl")
+include("estimator/mhe.jl")
 
 "Get [`InternalModel`](@ref) output `ŷ` from current measured outputs `ym` and dist. `d`."
 evalŷ(estim::InternalModel, ym, d) = evaloutput(estim,ym, d)