From bd72f9dd70b480b6d54ffa68e036775641f154f2 Mon Sep 17 00:00:00 2001 From: franckgaga Date: Fri, 24 Nov 2023 13:02:56 -0500 Subject: [PATCH 01/20] starting MHE, take 2 --- src/ModelPredictiveControl.jl | 1 + src/estimator/kalman.jl | 22 ++-- src/estimator/mhe.jl | 233 ++++++++++++++++++++++++++++++++++ src/state_estim.jl | 1 + 4 files changed, 246 insertions(+), 11 deletions(-) create mode 100644 src/estimator/mhe.jl diff --git a/src/ModelPredictiveControl.jl b/src/ModelPredictiveControl.jl index 0d1405dbf..2fdef3e1c 100644 --- a/src/ModelPredictiveControl.jl +++ b/src/ModelPredictiveControl.jl @@ -15,6 +15,7 @@ export SimModel, LinModel, NonLinModel export setop!, setstate!, updatestate!, evaloutput, linearize export StateEstimator, InternalModel, Luenberger export SteadyKalmanFilter, KalmanFilter, UnscentedKalmanFilter, ExtendedKalmanFilter +export MovingHorizonEstimator export default_nint, initstate! export PredictiveController, ExplicitMPC, LinMPC, NonLinMPC, setconstraint!, moveinput! export SimResult, getinfo, sim! diff --git a/src/estimator/kalman.jl b/src/estimator/kalman.jl index 5fef5282a..20163e89c 100644 --- a/src/estimator/kalman.jl +++ b/src/estimator/kalman.jl @@ -148,8 +148,8 @@ function SteadyKalmanFilter( σQint_ym::Vector = fill(1, max(sum(nint_ym), 0)) ) where {NT<:Real, SM<:LinModel{NT}} # estimated covariances matrices (variance = σ²) : - Q̂ = Diagonal{NT}([σQ; σQint_u; σQint_ym].^2); - R̂ = Diagonal{NT}(σR.^2); + Q̂ = Diagonal{NT}([σQ; σQint_u; σQint_ym].^2) + R̂ = Diagonal{NT}(σR.^2) return SteadyKalmanFilter{NT, SM}(model, i_ym, nint_u, nint_ym, Q̂ , R̂) end @@ -288,9 +288,9 @@ function KalmanFilter( σP0int_ym::Vector = fill(1, max(sum(nint_ym), 0)) ) where {NT<:Real, SM<:LinModel{NT}} # estimated covariances matrices (variance = σ²) : - P̂0 = Diagonal{NT}([σP0; σP0int_u; σP0int_ym].^2); - Q̂ = Diagonal{NT}([σQ; σQint_u; σQint_ym].^2); - R̂ = Diagonal{NT}(σR.^2); + P̂0 = Diagonal{NT}([σP0; σP0int_u; σP0int_ym].^2) + Q̂ = Diagonal{NT}([σQ; σQint_u; σQint_ym].^2) + R̂ = Diagonal{NT}(σR.^2) return KalmanFilter{NT, SM}(model, i_ym, nint_u, nint_ym, P̂0, Q̂ , R̂) end @@ -457,9 +457,9 @@ function UnscentedKalmanFilter( κ::Real = 0 ) where {NT<:Real, SM<:SimModel{NT}} # estimated covariances matrices (variance = σ²) : - P̂0 = Diagonal{NT}([σP0; σP0int_u; σP0int_ym].^2); - Q̂ = Diagonal{NT}([σQ; σQint_u; σQint_ym].^2); - R̂ = Diagonal{NT}(σR.^2); + P̂0 = Diagonal{NT}([σP0; σP0int_u; σP0int_ym].^2) + Q̂ = Diagonal{NT}([σQ; σQint_u; σQint_ym].^2) + R̂ = Diagonal{NT}(σR.^2) return UnscentedKalmanFilter{NT, SM}(model, i_ym, nint_u, nint_ym, P̂0, Q̂, R̂, α, β, κ) end @@ -680,9 +680,9 @@ function ExtendedKalmanFilter( σP0int_ym::Vector = fill(1, max(sum(nint_ym), 0)) ) where {NT<:Real, SM<:SimModel{NT}} # estimated covariances matrices (variance = σ²) : - P̂0 = Diagonal{NT}([σP0; σP0int_u; σP0int_ym].^2); - Q̂ = Diagonal{NT}([σQ; σQint_u; σQint_ym].^2); - R̂ = Diagonal{NT}(σR.^2); + P̂0 = Diagonal{NT}([σP0; σP0int_u; σP0int_ym].^2) + Q̂ = Diagonal{NT}([σQ; σQint_u; σQint_ym].^2) + R̂ = Diagonal{NT}(σR.^2) return ExtendedKalmanFilter{NT, SM}(model, i_ym, nint_u, nint_ym, P̂0, Q̂ , R̂) end diff --git a/src/estimator/mhe.jl b/src/estimator/mhe.jl new file mode 100644 index 000000000..54e59e4e2 --- /dev/null +++ b/src/estimator/mhe.jl @@ -0,0 +1,233 @@ +const DEFAULT_MHE_OPTIMIZER = optimizer_with_attributes(Ipopt.Optimizer,"sb"=>"yes") + +struct MovingHorizonEstimator{ + NT<:Real, + SM<:SimModel, + JM<:JuMP.GenericModel +} <: StateEstimator{NT} + model::SM + # note: `NT` and the number type `JNT` in `JuMP.GenericModel{JNT}` can be + # different since solvers that support non-Float64 are scarce. + optim::JM + lastu0::Vector{NT} + x̂::Vector{NT} + P̂::Hermitian{NT, Matrix{NT}} + He::Int + i_ym::Vector{Int} + nx̂ ::Int + nym::Int + nyu::Int + nxs::Int + As ::Matrix{NT} + Cs_u::Matrix{NT} + Cs_y::Matrix{NT} + nint_u ::Vector{Int} + nint_ym::Vector{Int} + Â ::Matrix{NT} + B̂u::Matrix{NT} + Ĉ ::Matrix{NT} + B̂d::Matrix{NT} + D̂d::Matrix{NT} + P̂0::Hermitian{NT, Matrix{NT}} + Q̂::Hermitian{NT, Matrix{NT}} + R̂::Hermitian{NT, Matrix{NT}} + function MovingHorizonEstimator{NT, SM, JM}( + model::SM, He, i_ym, nint_u, nint_ym, P̂0, Q̂, R̂, optim::JM + ) where {NT<:Real, SM<:SimModel{NT}, JM<:JuMP.GenericModel} + nym, nyu = validate_ym(model, i_ym) + As, Cs_u, Cs_y, nint_u, nint_ym = init_estimstoch(model, i_ym, nint_u, nint_ym) + nxs = size(As, 1) + nx̂ = model.nx + nxs + Â, B̂u, Ĉ, B̂d, D̂d = augment_model(model, As, Cs_u, Cs_y) + validate_kfcov(nym, nx̂, Q̂, R̂, P̂0) + lastu0 = zeros(NT, model.nu) + x̂ = [zeros(NT, model.nx); zeros(NT, nxs)] + Q̂, R̂ = Hermitian(Q̂, :L), Hermitian(R̂, :L) + P̂0 = Hermitian(P̂0, :L) + P̂ = copy(P̂0) + estim = new{NT, SM, JM}( + model, optim, + lastu0, x̂, P̂, He, + i_ym, nx̂, nym, nyu, nxs, + As, Cs_u, Cs_y, nint_u, nint_ym, + Â, B̂u, Ĉ, B̂d, D̂d, + P̂0, Q̂, R̂, + ) + init_optimization!(estim, optim) + return mhe + end +end + +function MovingHorizonEstimator( + model::SM; + He::Int=nothing, + i_ym::IntRangeOrVector = 1:model.ny, + σP0::Vector = fill(1/model.nx, model.nx), + σQ::Vector = fill(1/model.nx, model.nx), + σR::Vector = fill(1, length(i_ym)), + nint_u ::IntVectorOrInt = 0, + σQint_u ::Vector = fill(1, max(sum(nint_u), 0)), + σP0int_u ::Vector = fill(1, max(sum(nint_u), 0)), + nint_ym ::IntVectorOrInt = default_nint(model, i_ym, nint_u), + σQint_ym ::Vector = fill(1, max(sum(nint_ym), 0)), + σP0int_ym::Vector = fill(1, max(sum(nint_ym), 0)), + optim::JM = JuMP.Model(DEFAULT_MHE_OPTIMIZER, add_bridges=false), +) where {NT<:Real, SM<:SimModel{NT}, JM<:JuMP.GenericModel} + # estimated covariances matrices (variance = σ²) : + P̂0 = Diagonal{NT}([σP0; σP0int_u; σP0int_ym].^2) + Q̂ = Diagonal{NT}([σQ; σQint_u; σQint_ym].^2) + R̂ = Diagonal{NT}(σR.^2) + return MovingHorizonEstimator{NT, SM, JM}( + model, He, i_ym, nint_u, nint_ym, P̂0, Q̂, R̂, optim + ) +end + +""" + init_optimization!(estim::MovingHorizonEstimator, optim::JuMP.GenericModel) + +Init the nonlinear optimization of [`MovingHorizonEstimator`](@ref). +""" +function init_optimization!( + estim::MovingHorizonEstimator, optim::JuMP.GenericModel{JNT} +) where JNT<:Real + # --- variables and linear constraints --- + nvar = length(estim.ΔŨ) + set_silent(optim) + limit_solve_time(estim) + @variable(optim, ΔŨvar[1:nvar]) + ΔŨvar = optim[:ΔŨvar] + A = con.A[con.i_b, :] + b = con.b[con.i_b] + @constraint(optim, linconstraint, A*ΔŨvar .≤ b) + # --- nonlinear optimization init --- + model = estim.estim.model + ny, nx̂, Hp, ng = model.ny, estim.estim.nx̂, estim.Hp, length(con.i_g) + # inspired from https://jump.dev/JuMP.jl/stable/tutorials/nonlinear/tips_and_tricks/#User-defined-operators-with-vector-outputs + Jfunc, gfunc = let mpc=estim, model=model, ng=ng, nvar=nvar , nŶ=Hp*ny, nx̂=nx̂ + last_ΔŨtup_float, last_ΔŨtup_dual = nothing, nothing + Ŷ_cache::DiffCache{Vector{JNT}, Vector{JNT}} = DiffCache(zeros(JNT, nŶ), nvar + 3) + g_cache::DiffCache{Vector{JNT}, Vector{JNT}} = DiffCache(zeros(JNT, ng), nvar + 3) + x̂_cache::DiffCache{Vector{JNT}, Vector{JNT}} = DiffCache(zeros(JNT, nx̂), nvar + 3) + function Jfunc(ΔŨtup::JNT...) + Ŷ = get_tmp(Ŷ_cache, ΔŨtup[1]) + ΔŨ = collect(ΔŨtup) + if ΔŨtup != last_ΔŨtup_float + x̂ = get_tmp(x̂_cache, ΔŨtup[1]) + g = get_tmp(g_cache, ΔŨtup[1]) + Ŷ, x̂end = predict!(Ŷ, x̂, mpc, model, ΔŨ) + con_nonlinprog!(g, mpc, model, x̂end, Ŷ, ΔŨ) + last_ΔŨtup_float = ΔŨtup + end + return obj_nonlinprog(mpc, model, Ŷ, ΔŨ) + end + function Jfunc(ΔŨtup::ForwardDiff.Dual...) + Ŷ = get_tmp(Ŷ_cache, ΔŨtup[1]) + ΔŨ = collect(ΔŨtup) + if ΔŨtup != last_ΔŨtup_dual + x̂ = get_tmp(x̂_cache, ΔŨtup[1]) + g = get_tmp(g_cache, ΔŨtup[1]) + Ŷ, x̂end = predict!(Ŷ, x̂, mpc, model, ΔŨ) + con_nonlinprog!(g, mpc, model, x̂end, Ŷ, ΔŨ) + last_ΔŨtup_dual = ΔŨtup + end + return obj_nonlinprog(mpc, model, Ŷ, ΔŨ) + end + function gfunc_i(i, ΔŨtup::NTuple{N, JNT}) where N + g = get_tmp(g_cache, ΔŨtup[1]) + if ΔŨtup != last_ΔŨtup_float + x̂ = get_tmp(x̂_cache, ΔŨtup[1]) + Ŷ = get_tmp(Ŷ_cache, ΔŨtup[1]) + ΔŨ = collect(ΔŨtup) + Ŷ, x̂end = predict!(Ŷ, x̂, mpc, model, ΔŨ) + g = con_nonlinprog!(g, mpc, model, x̂end, Ŷ, ΔŨ) + last_ΔŨtup_float = ΔŨtup + end + return g[i] + end + function gfunc_i(i, ΔŨtup::NTuple{N, ForwardDiff.Dual}) where N + g = get_tmp(g_cache, ΔŨtup[1]) + if ΔŨtup != last_ΔŨtup_dual + x̂ = get_tmp(x̂_cache, ΔŨtup[1]) + Ŷ = get_tmp(Ŷ_cache, ΔŨtup[1]) + ΔŨ = collect(ΔŨtup) + Ŷ, x̂end = predict!(Ŷ, x̂, mpc, model, ΔŨ) + g = con_nonlinprog!(g, mpc, model, x̂end, Ŷ, ΔŨ) + last_ΔŨtup_dual = ΔŨtup + end + return g[i] + end + gfunc = [(ΔŨ...) -> gfunc_i(i, ΔŨ) for i in 1:ng] + (Jfunc, gfunc) + end + register(optim, :Jfunc, nvar, Jfunc, autodiff=true) + @NLobjective(optim, Min, Jfunc(ΔŨvar...)) + if ng ≠ 0 + i_end_Ymin, i_end_Ymax, i_end_x̂min = 1Hp*ny, 2Hp*ny, 2Hp*ny + nx̂ + for i in eachindex(con.Ymin) + sym = Symbol("g_Ymin_$i") + register(optim, sym, nvar, gfunc[i], autodiff=true) + end + for i in eachindex(con.Ymax) + sym = Symbol("g_Ymax_$i") + register(optim, sym, nvar, gfunc[i_end_Ymin+i], autodiff=true) + end + for i in eachindex(con.x̂min) + sym = Symbol("g_x̂min_$i") + register(optim, sym, nvar, gfunc[i_end_Ymax+i], autodiff=true) + end + for i in eachindex(con.x̂max) + sym = Symbol("g_x̂max_$i") + register(optim, sym, nvar, gfunc[i_end_x̂min+i], autodiff=true) + end + end + return nothing +end + + +@doc raw""" + update_estimate!(estim::UnscentedKalmanFilter, u, ym, d) + +Update [`UnscentedKalmanFilter`](@ref) state `estim.x̂` and covariance estimate `estim.P̂`. + +A ref[^4]: + +```math +\begin{aligned} + \mathbf{Ŷ^m}(k) &= \bigg[\begin{matrix} \mathbf{ĥ^m}\Big( \mathbf{X̂}_{k-1}^{1}(k) \Big) & \mathbf{ĥ^m}\Big( \mathbf{X̂}_{k-1}^{2}(k) \Big) & \cdots & \mathbf{ĥ^m}\Big( \mathbf{X̂}_{k-1}^{n_σ}(k) \Big) \end{matrix}\bigg] \\ + \mathbf{ŷ^m}(k) &= \mathbf{Ŷ^m}(k) \mathbf{m̂} +\end{aligned} +``` + +[^4]: TODO +""" +function update_estimate!(estim::MovingHorizonEstimator, u, ym, d) where NT<:Real + x̂, P̂, Q̂, R̂, K̂ = estim.x̂, estim.P̂, estim.Q̂, estim.R̂, estim.K̂ + nym, nx̂, nσ = estim.nym, estim.nx̂, estim.nσ + γ, m̂, Ŝ = estim.γ, estim.m̂, estim.Ŝ + # --- correction step --- + sqrt_P̂ = cholesky(P̂).L + X̂ = repeat(x̂, 1, nσ) + [zeros(NT, nx̂) +γ*sqrt_P̂ -γ*sqrt_P̂] + Ŷm = Matrix{NT}(undef, nym, nσ) + for j in axes(Ŷm, 2) + Ŷm[:, j] = ĥ(estim, estim.model, X̂[:, j], d)[estim.i_ym] + end + ŷm = Ŷm * m̂ + X̄ = X̂ .- x̂ + Ȳm = Ŷm .- ŷm + M̂ = Hermitian(Ȳm * Ŝ * Ȳm' + R̂, :L) + mul!(K̂, X̄, lmul!(Ŝ, Ȳm')) + rdiv!(K̂, cholesky(M̂)) + x̂_cor = x̂ + K̂ * (ym - ŷm) + P̂_cor = P̂ - Hermitian(K̂ * M̂ * K̂', :L) + # --- prediction step --- + sqrt_P̂_cor = cholesky(P̂_cor).L + X̂_cor = repeat(x̂_cor, 1, nσ) + [zeros(NT, nx̂) +γ*sqrt_P̂_cor -γ*sqrt_P̂_cor] + X̂_next = Matrix{NT}(undef, nx̂, nσ) + for j in axes(X̂_next, 2) + X̂_next[:, j] = f̂(estim, estim.model, X̂_cor[:, j], u, d) + end + x̂[:] = X̂_next * m̂ + X̄_next = X̂_next .- x̂ + P̂.data[:] = X̄_next * Ŝ * X̄_next' + Q̂ # .data is necessary for Hermitians + return x̂, P̂ +end \ No newline at end of file diff --git a/src/state_estim.jl b/src/state_estim.jl index c3f1024e0..7660b10e3 100644 --- a/src/state_estim.jl +++ b/src/state_estim.jl @@ -429,6 +429,7 @@ updatestate!(::StateEstimator, _ ) = throw(ArgumentError("missing measured outpu include("estimator/kalman.jl") include("estimator/luenberger.jl") include("estimator/internal_model.jl") +include("estimator/mhe.jl") """ evalŷ(estim::StateEstimator, _ , d) -> ŷ From e7a57c0b4e44de140f40c6bdc6ce06791dcba168 Mon Sep 17 00:00:00 2001 From: franckgaga Date: Fri, 24 Nov 2023 14:11:23 -0500 Subject: [PATCH 02/20] idem --- src/controller/nonlinmpc.jl | 1 - src/estimator/mhe.jl | 199 +++++++++++++++++++++++------------- 2 files changed, 128 insertions(+), 72 deletions(-) diff --git a/src/controller/nonlinmpc.jl b/src/controller/nonlinmpc.jl index d7f7605f3..35ee5b76e 100644 --- a/src/controller/nonlinmpc.jl +++ b/src/controller/nonlinmpc.jl @@ -275,7 +275,6 @@ function init_optimization!(mpc::NonLinMPC, optim::JuMP.GenericModel{JNT}) where set_silent(optim) limit_solve_time(mpc) @variable(optim, ΔŨvar[1:nvar]) - ΔŨvar = optim[:ΔŨvar] A = con.A[con.i_b, :] b = con.b[con.i_b] @constraint(optim, linconstraint, A*ΔŨvar .≤ b) diff --git a/src/estimator/mhe.jl b/src/estimator/mhe.jl index 54e59e4e2..9889d3f2e 100644 --- a/src/estimator/mhe.jl +++ b/src/estimator/mhe.jl @@ -9,6 +9,7 @@ struct MovingHorizonEstimator{ # note: `NT` and the number type `JNT` in `JuMP.GenericModel{JNT}` can be # different since solvers that support non-Float64 are scarce. optim::JM + W̃::Vector{NT} lastu0::Vector{NT} x̂::Vector{NT} P̂::Hermitian{NT, Matrix{NT}} @@ -29,11 +30,21 @@ struct MovingHorizonEstimator{ B̂d::Matrix{NT} D̂d::Matrix{NT} P̂0::Hermitian{NT, Matrix{NT}} - Q̂::Hermitian{NT, Matrix{NT}} - R̂::Hermitian{NT, Matrix{NT}} + Q̂_He::Hermitian{Float64, Matrix{Float64}} + R̂_He::Hermitian{Float64, Matrix{Float64}} + X̂min::Vector{Float64} + X̂max::Vector{Float64} + X̂ ::Vector{Float64} + Ym::Vector{Float64} + U ::Vector{Float64} + D ::Vector{Float64} + Ŵ ::Vector{Float64} + x̂0_past::Vector{Float64} function MovingHorizonEstimator{NT, SM, JM}( model::SM, He, i_ym, nint_u, nint_ym, P̂0, Q̂, R̂, optim::JM ) where {NT<:Real, SM<:SimModel{NT}, JM<:JuMP.GenericModel} + nu, nd, nx, ny = model.nu, model.nd, model.nx, model.ny + He < 1 && throw(ArgumentError("Estimation horizon He should be ≥ 1")) nym, nyu = validate_ym(model, i_ym) As, Cs_u, Cs_y, nint_u, nint_ym = init_estimstoch(model, i_ym, nint_u, nint_ym) nxs = size(As, 1) @@ -44,17 +55,27 @@ struct MovingHorizonEstimator{ x̂ = [zeros(NT, model.nx); zeros(NT, nxs)] Q̂, R̂ = Hermitian(Q̂, :L), Hermitian(R̂, :L) P̂0 = Hermitian(P̂0, :L) + Q̂_He = Hermitian(repeatdiag(Q̂, He), :L) + R̂_He = Hermitian(repeatdiag(R̂, He), :L) P̂ = copy(P̂0) + 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{NT, SM, JM}( - model, optim, + model, optim, W̃, lastu0, x̂, P̂, He, i_ym, nx̂, nym, nyu, nxs, As, Cs_u, Cs_y, nint_u, nint_ym, Â, B̂u, Ĉ, B̂d, D̂d, - P̂0, Q̂, R̂, + P̂0, Q̂_He, R̂_He, + X̂min, X̂max, + X̂, Ym, U, D, Ŵ, + x̂0_past ) - init_optimization!(estim, optim) - return mhe + #init_optimization!(estim, optim) + return estim end end @@ -82,6 +103,51 @@ function MovingHorizonEstimator( ) end +#= +@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::MovingHorizonEstimator, optim::JuMP.GenericModel) @@ -91,22 +157,17 @@ function init_optimization!( estim::MovingHorizonEstimator, optim::JuMP.GenericModel{JNT} ) where JNT<:Real # --- variables and linear constraints --- - nvar = length(estim.ΔŨ) + nvar = length(estim.W̃) set_silent(optim) limit_solve_time(estim) - @variable(optim, ΔŨvar[1:nvar]) - ΔŨvar = optim[:ΔŨvar] - A = con.A[con.i_b, :] - b = con.b[con.i_b] - @constraint(optim, linconstraint, A*ΔŨvar .≤ b) + @variable(optim, W̃var[1:nvar]) # --- nonlinear optimization init --- - model = estim.estim.model - ny, nx̂, Hp, ng = model.ny, estim.estim.nx̂, estim.Hp, length(con.i_g) + model = estim.model + ny, nx̂, He = model.ny, estim.estim.nx̂, estim.He#, length(i_g) # inspired from https://jump.dev/JuMP.jl/stable/tutorials/nonlinear/tips_and_tricks/#User-defined-operators-with-vector-outputs - Jfunc, gfunc = let mpc=estim, model=model, ng=ng, nvar=nvar , nŶ=Hp*ny, nx̂=nx̂ + Jfunc, gfunc = let mpc=estim, model=model, nvar=nvar , nŶ=Hp*ny, nx̂=nx̂ last_ΔŨtup_float, last_ΔŨtup_dual = nothing, nothing Ŷ_cache::DiffCache{Vector{JNT}, Vector{JNT}} = DiffCache(zeros(JNT, nŶ), nvar + 3) - g_cache::DiffCache{Vector{JNT}, Vector{JNT}} = DiffCache(zeros(JNT, ng), nvar + 3) x̂_cache::DiffCache{Vector{JNT}, Vector{JNT}} = DiffCache(zeros(JNT, nx̂), nvar + 3) function Jfunc(ΔŨtup::JNT...) Ŷ = get_tmp(Ŷ_cache, ΔŨtup[1]) @@ -132,32 +193,7 @@ function init_optimization!( end return obj_nonlinprog(mpc, model, Ŷ, ΔŨ) end - function gfunc_i(i, ΔŨtup::NTuple{N, JNT}) where N - g = get_tmp(g_cache, ΔŨtup[1]) - if ΔŨtup != last_ΔŨtup_float - x̂ = get_tmp(x̂_cache, ΔŨtup[1]) - Ŷ = get_tmp(Ŷ_cache, ΔŨtup[1]) - ΔŨ = collect(ΔŨtup) - Ŷ, x̂end = predict!(Ŷ, x̂, mpc, model, ΔŨ) - g = con_nonlinprog!(g, mpc, model, x̂end, Ŷ, ΔŨ) - last_ΔŨtup_float = ΔŨtup - end - return g[i] - end - function gfunc_i(i, ΔŨtup::NTuple{N, ForwardDiff.Dual}) where N - g = get_tmp(g_cache, ΔŨtup[1]) - if ΔŨtup != last_ΔŨtup_dual - x̂ = get_tmp(x̂_cache, ΔŨtup[1]) - Ŷ = get_tmp(Ŷ_cache, ΔŨtup[1]) - ΔŨ = collect(ΔŨtup) - Ŷ, x̂end = predict!(Ŷ, x̂, mpc, model, ΔŨ) - g = con_nonlinprog!(g, mpc, model, x̂end, Ŷ, ΔŨ) - last_ΔŨtup_dual = ΔŨtup - end - return g[i] - end - gfunc = [(ΔŨ...) -> gfunc_i(i, ΔŨ) for i in 1:ng] - (Jfunc, gfunc) + Jfunc end register(optim, :Jfunc, nvar, Jfunc, autodiff=true) @NLobjective(optim, Min, Jfunc(ΔŨvar...)) @@ -200,34 +236,55 @@ A ref[^4]: [^4]: TODO """ -function update_estimate!(estim::MovingHorizonEstimator, u, ym, d) where NT<:Real - x̂, P̂, Q̂, R̂, K̂ = estim.x̂, estim.P̂, estim.Q̂, estim.R̂, estim.K̂ - nym, nx̂, nσ = estim.nym, estim.nx̂, estim.nσ - γ, m̂, Ŝ = estim.γ, estim.m̂, estim.Ŝ - # --- correction step --- - sqrt_P̂ = cholesky(P̂).L - X̂ = repeat(x̂, 1, nσ) + [zeros(NT, nx̂) +γ*sqrt_P̂ -γ*sqrt_P̂] - Ŷm = Matrix{NT}(undef, nym, nσ) - for j in axes(Ŷm, 2) - Ŷm[:, j] = ĥ(estim, estim.model, X̂[:, j], d)[estim.i_ym] - end - ŷm = Ŷm * m̂ - X̄ = X̂ .- x̂ - Ȳm = Ŷm .- ŷm - M̂ = Hermitian(Ȳm * Ŝ * Ȳm' + R̂, :L) - mul!(K̂, X̄, lmul!(Ŝ, Ȳm')) - rdiv!(K̂, cholesky(M̂)) - x̂_cor = x̂ + K̂ * (ym - ŷm) - P̂_cor = P̂ - Hermitian(K̂ * M̂ * K̂', :L) - # --- prediction step --- - sqrt_P̂_cor = cholesky(P̂_cor).L - X̂_cor = repeat(x̂_cor, 1, nσ) + [zeros(NT, nx̂) +γ*sqrt_P̂_cor -γ*sqrt_P̂_cor] - X̂_next = Matrix{NT}(undef, nx̂, nσ) - for j in axes(X̂_next, 2) - X̂_next[:, j] = f̂(estim, estim.model, X̂_cor[:, j], u, d) +function update_estimate!(estim::MovingHorizonEstimator, u, ym, d) + return esti.x̂, estim.P̂ +end + +""" + obj_nonlinprog(estim::MovingHorizonEstimator, model::SimModel, ΔŨ::Vector{Real}) + +Objective function for [`NonLinMHE`] when `model` is not a [`LinModel`](@ref). +""" +function obj_nonlinprog(estim::MovingHorizonEstimator, ::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̂[:] = X̂_next * m̂ - X̄_next = X̂_next .- x̂ - P̂.data[:] = X̄_next * Ŝ * X̄_next' + Q̂ # .data is necessary for Hermitians - return x̂, P̂ + 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 \ No newline at end of file From b6a8be1bd7a0a6723412e76b330d02f5250a92e7 Mon Sep 17 00:00:00 2001 From: franckgaga Date: Fri, 24 Nov 2023 16:05:28 -0500 Subject: [PATCH 03/20] main structure of MHE --- src/estimator/mhe.jl | 154 +++++++++++++++++++------------------------ 1 file changed, 68 insertions(+), 86 deletions(-) diff --git a/src/estimator/mhe.jl b/src/estimator/mhe.jl index 9889d3f2e..fd634749e 100644 --- a/src/estimator/mhe.jl +++ b/src/estimator/mhe.jl @@ -29,17 +29,18 @@ struct MovingHorizonEstimator{ Ĉ ::Matrix{NT} B̂d::Matrix{NT} D̂d::Matrix{NT} - P̂0::Hermitian{NT, Matrix{NT}} - Q̂_He::Hermitian{Float64, Matrix{Float64}} - R̂_He::Hermitian{Float64, Matrix{Float64}} - X̂min::Vector{Float64} - X̂max::Vector{Float64} - X̂ ::Vector{Float64} - Ym::Vector{Float64} - U ::Vector{Float64} - D ::Vector{Float64} - Ŵ ::Vector{Float64} - x̂0_past::Vector{Float64} + P̂0 ::Hermitian{NT, Matrix{NT}} + Q̂_He::Hermitian{NT, Matrix{NT}} + R̂_He::Hermitian{NT, Matrix{NT}} + X̂min::Vector{NT} + X̂max::Vector{NT} + X̂ ::Vector{NT} + Ym::Vector{NT} + U ::Vector{NT} + D ::Vector{NT} + Ŵ ::Vector{NT} + x̂0_past::Vector{NT} + Nk::Vector{Int} function MovingHorizonEstimator{NT, SM, JM}( model::SM, He, i_ym, nint_u, nint_ym, P̂0, Q̂, R̂, optim::JM ) where {NT<:Real, SM<:SimModel{NT}, JM<:JuMP.GenericModel} @@ -63,6 +64,7 @@ struct MovingHorizonEstimator{ 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̂) + Nk = [1] estim = new{NT, SM, JM}( model, optim, W̃, lastu0, x̂, P̂, He, @@ -72,9 +74,9 @@ struct MovingHorizonEstimator{ P̂0, Q̂_He, R̂_He, X̂min, X̂max, X̂, Ym, U, D, Ŵ, - x̂0_past + x̂0_past, Nk ) - #init_optimization!(estim, optim) + init_optimization!(estim, optim) return estim end end @@ -159,63 +161,37 @@ function init_optimization!( # --- variables and linear constraints --- nvar = length(estim.W̃) set_silent(optim) - limit_solve_time(estim) + #limit_solve_time(estim) @variable(optim, W̃var[1:nvar]) # --- nonlinear optimization init --- - model = estim.model - ny, nx̂, He = model.ny, estim.estim.nx̂, estim.He#, length(i_g) + nym, nx̂, He = estim.nym, estim.nx̂, estim.He #, length(i_g) # inspired from https://jump.dev/JuMP.jl/stable/tutorials/nonlinear/tips_and_tricks/#User-defined-operators-with-vector-outputs - Jfunc, gfunc = let mpc=estim, model=model, nvar=nvar , nŶ=Hp*ny, nx̂=nx̂ - last_ΔŨtup_float, last_ΔŨtup_dual = nothing, nothing - Ŷ_cache::DiffCache{Vector{JNT}, Vector{JNT}} = DiffCache(zeros(JNT, nŶ), nvar + 3) - x̂_cache::DiffCache{Vector{JNT}, Vector{JNT}} = DiffCache(zeros(JNT, nx̂), nvar + 3) - function Jfunc(ΔŨtup::JNT...) - Ŷ = get_tmp(Ŷ_cache, ΔŨtup[1]) - ΔŨ = collect(ΔŨtup) - if ΔŨtup != last_ΔŨtup_float - x̂ = get_tmp(x̂_cache, ΔŨtup[1]) - g = get_tmp(g_cache, ΔŨtup[1]) - Ŷ, x̂end = predict!(Ŷ, x̂, mpc, model, ΔŨ) - con_nonlinprog!(g, mpc, model, x̂end, Ŷ, ΔŨ) - last_ΔŨtup_float = ΔŨtup + Jfunc = let estim=estim, model=estim.model, nvar=nvar , nŶm=He*nym, nX̂=He*nx̂ + last_W̃tup_float, last_W̃tup_dual = nothing, nothing + Ŷm_cache::DiffCache{Vector{JNT}, Vector{JNT}} = DiffCache(zeros(nŶm), nvar + 3) + X̂_cache ::DiffCache{Vector{JNT}, Vector{JNT}} = 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, _ = predict!(Ŷm, X̂, estim, model, W̃) + last_W̃tup_float = W̃tup end - return obj_nonlinprog(mpc, model, Ŷ, ΔŨ) + return obj_nonlinprog(estim, model, Ŷm, W̃) end - function Jfunc(ΔŨtup::ForwardDiff.Dual...) - Ŷ = get_tmp(Ŷ_cache, ΔŨtup[1]) - ΔŨ = collect(ΔŨtup) - if ΔŨtup != last_ΔŨtup_dual - x̂ = get_tmp(x̂_cache, ΔŨtup[1]) - g = get_tmp(g_cache, ΔŨtup[1]) - Ŷ, x̂end = predict!(Ŷ, x̂, mpc, model, ΔŨ) - con_nonlinprog!(g, mpc, model, x̂end, Ŷ, ΔŨ) - last_ΔŨtup_dual = ΔŨtup + 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, _ = predict!(Ŷm, X̂, estim, model, W̃) + last_W̃tup_dual = W̃tup end - return obj_nonlinprog(mpc, model, Ŷ, ΔŨ) + return obj_nonlinprog(estim, model, Ŷm, W̃) end Jfunc end register(optim, :Jfunc, nvar, Jfunc, autodiff=true) - @NLobjective(optim, Min, Jfunc(ΔŨvar...)) - if ng ≠ 0 - i_end_Ymin, i_end_Ymax, i_end_x̂min = 1Hp*ny, 2Hp*ny, 2Hp*ny + nx̂ - for i in eachindex(con.Ymin) - sym = Symbol("g_Ymin_$i") - register(optim, sym, nvar, gfunc[i], autodiff=true) - end - for i in eachindex(con.Ymax) - sym = Symbol("g_Ymax_$i") - register(optim, sym, nvar, gfunc[i_end_Ymin+i], autodiff=true) - end - for i in eachindex(con.x̂min) - sym = Symbol("g_x̂min_$i") - register(optim, sym, nvar, gfunc[i_end_Ymax+i], autodiff=true) - end - for i in eachindex(con.x̂max) - sym = Symbol("g_x̂max_$i") - register(optim, sym, nvar, gfunc[i_end_x̂min+i], autodiff=true) - end - end + @NLobjective(optim, Min, Jfunc(W̃var...)) return nothing end @@ -248,43 +224,49 @@ Objective function for [`NonLinMHE`] when `model` is not a [`LinModel`](@ref). function obj_nonlinprog(estim::MovingHorizonEstimator, ::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 + V̂ = estim.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̂] +function predict!( + Ŷm, X̂, estim::MovingHorizonEstimator, model::SimModel, W̃::Vector{T} +) where {T<:Real} + nu, nd, nx̂, He = model.nu, model.nd, estim.nx̂, estim.He + 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)] + u[:] = estim.U[(1 + nu*(j-1)):(nu*j)] + d[:] = estim.D[(1 + nd*(j-1)):(nd*j)] + i = j+1 + ŵ[:] = W̃[(1 + nx̂*(i-1)):(nx̂*i)] 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) + ŵ + Ŷm[(1 + ny*(j-1)):(ny*j)] = ĥ(estim, model, x̂, d)[estim.i_ym] + x̂[:] = f̂(estim, model, 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̂ +function update_estimate!(estim::MovingHorizonEstimator, u, ym, d) + model, x̂, P̂ = estim.model, estim.x̂, estim.P̂ 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] ; ŵ] - + # --- adding new data in the windows --- + estim.X̂[:] = [estim.X̂[nx̂+1:end] ; x̂] + estim.Ym[:] = [estim.Ym[nym+1:end]; ym] + estim.U[:] = [estim.U[nu+1:end] ; u] + estim.D[:] = [estim.D[nd+1:end] ; d] + estim.Ŵ[:] = [estim.Ŵ[nŵ+1:end] ; ŵ] + + + + + + + Nk, He = estim.Nk, estim.He + Nk[] = Nk[] < He ? Nk[] + 1 : He + return x̂, P̂ #estim.x̂0_past[:] = end \ No newline at end of file From 7edfa49a5561df4a8a3cd1e45fdd7d6fbca28fe0 Mon Sep 17 00:00:00 2001 From: franckgaga Date: Wed, 29 Nov 2023 09:54:05 -0500 Subject: [PATCH 04/20] MHE continuation --- src/estimator/mhe.jl | 101 ++++++++++++++++++++++++------------------- 1 file changed, 57 insertions(+), 44 deletions(-) diff --git a/src/estimator/mhe.jl b/src/estimator/mhe.jl index fd634749e..9e2766995 100644 --- a/src/estimator/mhe.jl +++ b/src/estimator/mhe.jl @@ -196,48 +196,30 @@ function init_optimization!( end -@doc raw""" - update_estimate!(estim::UnscentedKalmanFilter, u, ym, d) - -Update [`UnscentedKalmanFilter`](@ref) state `estim.x̂` and covariance estimate `estim.P̂`. - -A ref[^4]: - -```math -\begin{aligned} - \mathbf{Ŷ^m}(k) &= \bigg[\begin{matrix} \mathbf{ĥ^m}\Big( \mathbf{X̂}_{k-1}^{1}(k) \Big) & \mathbf{ĥ^m}\Big( \mathbf{X̂}_{k-1}^{2}(k) \Big) & \cdots & \mathbf{ĥ^m}\Big( \mathbf{X̂}_{k-1}^{n_σ}(k) \Big) \end{matrix}\bigg] \\ - \mathbf{ŷ^m}(k) &= \mathbf{Ŷ^m}(k) \mathbf{m̂} -\end{aligned} -``` - -[^4]: TODO -""" -function update_estimate!(estim::MovingHorizonEstimator, u, ym, d) - return esti.x̂, estim.P̂ -end - """ obj_nonlinprog(estim::MovingHorizonEstimator, model::SimModel, ΔŨ::Vector{Real}) Objective function for [`NonLinMHE`] when `model` is not a [`LinModel`](@ref). """ -function obj_nonlinprog(estim::MovingHorizonEstimator, ::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.Ym - Ŷm - @views Ŵ = W̃[estim.nx̂+1:end] - return x̄0'/estim.P̂0*x̄0 + Ŵ'/Q̂_He*Ŵ + V̂'/R̂_He*V̂ +function obj_nonlinprog( + estim::MovingHorizonEstimator, model::SimModel, Ŷm, W̃::Vector{T} +) where {T<:Real} + nYm, nŴ, nx̂ = estim.Nk[]*estim.nym, estim.Nk[]*estim.nx̂, estim.nx̂ + x̄0 = W̃[1:nx̂] - estim.x̂0_past # W̃ = [x̂(k-Nk|k); Ŵ] + V̂ = estim.Ym[1:nYm] - Ŷm[1:nYm] + Ŵ = W̃[nx̂+1:nx̂+nŴ] + return x̄0'/estim.P̂0*x̄0 + Ŵ'/Q̂_He[1:nŴ, 1:nŴ]*Ŵ + V̂'/R̂_He[1:nYm, 1:nYm]*V̂ end function predict!( Ŷm, X̂, estim::MovingHorizonEstimator, model::SimModel, W̃::Vector{T} ) where {T<:Real} - nu, nd, nx̂, He = model.nu, model.nd, estim.nx̂, estim.He + nu, nd, nx̂, Nk = model.nu, model.nd, estim.nx̂, estim.Nk[] 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 + x̂::Vector{T} = W̃[1:nx̂] # W̃ = [x̂(k-Nk|k); Ŵ] + for j=1:Nk u[:] = estim.U[(1 + nu*(j-1)):(nu*j)] d[:] = estim.D[(1 + nd*(j-1)):(nd*j)] i = j+1 @@ -249,24 +231,55 @@ function predict!( return Ŷm, X̂ end -function update_estimate!(estim::MovingHorizonEstimator, u, ym, d) - model, x̂, P̂ = estim.model, estim.x̂, estim.P̂ - 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 the windows --- - estim.X̂[:] = [estim.X̂[nx̂+1:end] ; x̂] - estim.Ym[:] = [estim.Ym[nym+1:end]; ym] - estim.U[:] = [estim.U[nu+1:end] ; u] - estim.D[:] = [estim.D[nd+1:end] ; d] - estim.Ŵ[:] = [estim.Ŵ[nŵ+1:end] ; ŵ] - - - +@doc raw""" + update_estimate!(estim::UnscentedKalmanFilter, u, ym, d) + +Update [`UnscentedKalmanFilter`](@ref) state `estim.x̂` and covariance estimate `estim.P̂`. +A ref[^4]: +```math +\begin{aligned} + \mathbf{Ŷ^m}(k) &= \bigg[\begin{matrix} \mathbf{ĥ^m}\Big( \mathbf{X̂}_{k-1}^{1}(k) \Big) & \mathbf{ĥ^m}\Big( \mathbf{X̂}_{k-1}^{2}(k) \Big) & \cdots & \mathbf{ĥ^m}\Big( \mathbf{X̂}_{k-1}^{n_σ}(k) \Big) \end{matrix}\bigg] \\ + \mathbf{ŷ^m}(k) &= \mathbf{Ŷ^m}(k) \mathbf{m̂} +\end{aligned} +``` - Nk, He = estim.Nk, estim.He +[^4]: TODO +""" +function update_estimate!(estim::MovingHorizonEstimator, u, ym, d) + model, optim, x̂, P̂ = estim.model, estim.optim, estim.x̂, estim.P̂ + nx̂, nym, nu, nd, nŵ = estim.nx̂, estim.nym, model.nu, model.nd, estim.nx̂ + Nk, He = estim.Nk[], estim.He + W̃var::Vector{VariableRef} = optim[:W̃var] + ŵ = zeros(nŵ) # ŵ(k) = 0 for warm-starting + if Nk < He + estim.X̂[ (1 + nx̂*(Nk-1)):(nx̂*Nk)] = x̂ + estim.Ym[(1 + nym*(Nk-1)):(nym*Nk)] = ym + estim.U[ (1 + nu*(Nk-1)):(nu*Nk)] = u + estim.D[ (1 + nd*(Nk-1)):(nd*Nk)] = d + estim.Ŵ[ (1 + nŵ*(Nk-1)):(nŵ*Nk)] = ŵ + else + estim.X̂[:] = [estim.X̂[nx̂+1:end] ; x̂] + estim.Ym[:] = [estim.Ym[nym+1:end]; ym] + estim.U[:] = [estim.U[nu+1:end] ; u] + estim.D[:] = [estim.D[nd+1:end] ; d] + estim.Ŵ[:] = [estim.Ŵ[nŵ+1:end] ; ŵ] + end + estim.x̂0_past[:] = estim.X̂[1:nx̂] + W̃0 = [estim.x̂0_past; estim.Ŵ] + set_start_value.(W̃var, W̃0) + try + optimize!(optim) + catch err + if isa(err, MOI.UnsupportedAttribute{MOI.VariablePrimalStart}) + # reset_optimizer to unset warm-start, set_start_value.(nothing) seems buggy + MOIU.reset_optimizer(optim) + optimize!(optim) + else + rethrow(err) + end + end Nk[] = Nk[] < He ? Nk[] + 1 : He return x̂, P̂ - #estim.x̂0_past[:] = end \ No newline at end of file From 83212a3d38813cb344e618db773cb79b37a291d2 Mon Sep 17 00:00:00 2001 From: franckgaga Date: Wed, 29 Nov 2023 10:33:25 -0500 Subject: [PATCH 05/20] debug MHE --- src/estimator/mhe.jl | 36 +++++++++++++++++++----------------- 1 file changed, 19 insertions(+), 17 deletions(-) diff --git a/src/estimator/mhe.jl b/src/estimator/mhe.jl index 9e2766995..7981d9114 100644 --- a/src/estimator/mhe.jl +++ b/src/estimator/mhe.jl @@ -166,7 +166,7 @@ function init_optimization!( # --- nonlinear optimization init --- nym, nx̂, He = estim.nym, estim.nx̂, estim.He #, length(i_g) # inspired from https://jump.dev/JuMP.jl/stable/tutorials/nonlinear/tips_and_tricks/#User-defined-operators-with-vector-outputs - Jfunc = let estim=estim, model=estim.model, nvar=nvar , nŶm=He*nym, nX̂=He*nx̂ + Jfunc = let estim=estim, model=estim.model, nvar=nvar , nŶm=He*nym, nX̂=(He+1)*nx̂ last_W̃tup_float, last_W̃tup_dual = nothing, nothing Ŷm_cache::DiffCache{Vector{JNT}, Vector{JNT}} = DiffCache(zeros(nŶm), nvar + 3) X̂_cache ::DiffCache{Vector{JNT}, Vector{JNT}} = DiffCache(zeros(nX̂) , nvar + 3) @@ -174,6 +174,7 @@ function init_optimization!( Ŷ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 + println(length(X̂)) Ŷm, _ = predict!(Ŷm, X̂, estim, model, W̃) last_W̃tup_float = W̃tup end @@ -183,6 +184,7 @@ function init_optimization!( Ŷ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 + println(length(X̂)) Ŷm, _ = predict!(Ŷm, X̂, estim, model, W̃) last_W̃tup_dual = W̃tup end @@ -204,19 +206,20 @@ Objective function for [`NonLinMHE`] when `model` is not a [`LinModel`](@ref). function obj_nonlinprog( estim::MovingHorizonEstimator, model::SimModel, Ŷm, W̃::Vector{T} ) where {T<:Real} + P̂0, Q̂_He, R̂_He = estim.P̂0, estim.Q̂_He, estim.R̂_He nYm, nŴ, nx̂ = estim.Nk[]*estim.nym, estim.Nk[]*estim.nx̂, estim.nx̂ x̄0 = W̃[1:nx̂] - estim.x̂0_past # W̃ = [x̂(k-Nk|k); Ŵ] V̂ = estim.Ym[1:nYm] - Ŷm[1:nYm] Ŵ = W̃[nx̂+1:nx̂+nŴ] - return x̄0'/estim.P̂0*x̄0 + Ŵ'/Q̂_He[1:nŴ, 1:nŴ]*Ŵ + V̂'/R̂_He[1:nYm, 1:nYm]*V̂ + return x̄0'*inv(estim.P̂0)*x̄0 + Ŵ'*inv(Q̂_He[1:nŴ, 1:nŴ])*Ŵ + V̂'*inv(R̂_He[1:nYm, 1:nYm])*V̂ end function predict!( Ŷm, X̂, estim::MovingHorizonEstimator, model::SimModel, W̃::Vector{T} ) where {T<:Real} - nu, nd, nx̂, Nk = model.nu, model.nd, estim.nx̂, estim.Nk[] + nu, nd, nx̂, nym, Nk = model.nu, model.nd, estim.nx̂, estim.nym, estim.Nk[] u::Vector{T} = Vector{T}(undef, nu) - d::Vector{T} = Vector{T}(undef, nu) + d::Vector{T} = Vector{T}(undef, nd) ŵ::Vector{T} = Vector{T}(undef, nx̂) x̂::Vector{T} = W̃[1:nx̂] # W̃ = [x̂(k-Nk|k); Ŵ] for j=1:Nk @@ -225,9 +228,11 @@ function predict!( i = j+1 ŵ[:] = W̃[(1 + nx̂*(i-1)):(nx̂*i)] X̂[(1 + nx̂*(j-1)):(nx̂*j)] = x̂ - Ŷm[(1 + ny*(j-1)):(ny*j)] = ĥ(estim, model, x̂, d)[estim.i_ym] + Ŷm[(1 + nym*(j-1)):(nym*j)] = ĥ(estim, model, x̂, d)[estim.i_ym] x̂[:] = f̂(estim, model, x̂, u, d) + ŵ end + j = Nk + 1 + X̂[(1 + nx̂*(j-1)):(nx̂*j)] = x̂ return Ŷm, X̂ end @@ -269,17 +274,14 @@ function update_estimate!(estim::MovingHorizonEstimator, u, ym, d) estim.x̂0_past[:] = estim.X̂[1:nx̂] W̃0 = [estim.x̂0_past; estim.Ŵ] set_start_value.(W̃var, W̃0) - try - optimize!(optim) - catch err - if isa(err, MOI.UnsupportedAttribute{MOI.VariablePrimalStart}) - # reset_optimizer to unset warm-start, set_start_value.(nothing) seems buggy - MOIU.reset_optimizer(optim) - optimize!(optim) - else - rethrow(err) - end - end - Nk[] = Nk[] < He ? Nk[] + 1 : He + optimize!(optim) + println(solution_summary(optim)) + + W̃ = value.(W̃var) + Ŷm = zeros(nym*Nk) + X̂ = zeros(nx̂*(Nk+1)) + Ŷm, X̂ = predict!(Ŷm, X̂, estim, model, W̃) + x̂[:] = X̂[(1 + nx̂*Nk):(nx̂*(Nk+1))] + estim.Nk[] = Nk < He ? Nk + 1 : He return x̂, P̂ end \ No newline at end of file From 10523859e25a6d2784ff425e44f49f4d40acdf39 Mon Sep 17 00:00:00 2001 From: franckgaga Date: Wed, 29 Nov 2023 17:50:04 -0500 Subject: [PATCH 06/20] MHE continuation --- src/ModelPredictiveControl.jl | 7 ++ src/estimator/kalman.jl | 7 +- src/estimator/mhe.jl | 174 +++++++++++++++++----------------- src/predictive_control.jl | 7 -- 4 files changed, 99 insertions(+), 96 deletions(-) diff --git a/src/ModelPredictiveControl.jl b/src/ModelPredictiveControl.jl index 2fdef3e1c..306636cfa 100644 --- a/src/ModelPredictiveControl.jl +++ b/src/ModelPredictiveControl.jl @@ -20,6 +20,13 @@ export default_nint, initstate! export PredictiveController, ExplicitMPC, LinMPC, NonLinMPC, setconstraint!, moveinput! export SimResult, getinfo, sim! +"Termination status that means 'no solution available'." +const FATAL_STATUSES = [ + INFEASIBLE, DUAL_INFEASIBLE, LOCALLY_INFEASIBLE, INFEASIBLE_OR_UNBOUNDED, + NUMERICAL_ERROR, INVALID_MODEL, INVALID_OPTION, INTERRUPTED, + OTHER_ERROR +] + "Generate a block diagonal matrix repeating `n` times the matrix `A`." repeatdiag(A, n::Int) = kron(I(n), A) diff --git a/src/estimator/kalman.jl b/src/estimator/kalman.jl index 20163e89c..b36955f65 100644 --- a/src/estimator/kalman.jl +++ b/src/estimator/kalman.jl @@ -746,15 +746,18 @@ end """ validate_kfcov(nym, nx̂, Q̂, R̂, P̂0=nothing) -Validate sizes of process `Q̂`` and sensor `R̂` noises covariance matrices. +Validate sizes and Hermitianity of process `Q̂`` and sensor `R̂` noises covariance matrices. -Also validate initial estimate covariance size, if provided. +Also validate initial estimate covariance `P̂0`, if provided. """ function validate_kfcov(nym, nx̂, Q̂, R̂, P̂0=nothing) size(Q̂) ≠ (nx̂, nx̂) && error("Q̂ size $(size(Q̂)) ≠ nx̂, nx̂ $((nx̂, nx̂))") + !ishermitian(Q̂) && error("Q̂ is not Hermitian") size(R̂) ≠ (nym, nym) && error("R̂ size $(size(R̂)) ≠ nym, nym $((nym, nym))") + !ishermitian(R̂) && error("R̂ is not Hermitian") if ~isnothing(P̂0) size(P̂0) ≠ (nx̂, nx̂) && error("P̂0 size $(size(P̂0)) ≠ nx̂, nx̂ $((nx̂, nx̂))") + !ishermitian(P̂0) && error("P̂0 is not Hermitian") end end diff --git a/src/estimator/mhe.jl b/src/estimator/mhe.jl index 7981d9114..59658d88f 100644 --- a/src/estimator/mhe.jl +++ b/src/estimator/mhe.jl @@ -30,17 +30,20 @@ struct MovingHorizonEstimator{ B̂d::Matrix{NT} D̂d::Matrix{NT} P̂0 ::Hermitian{NT, Matrix{NT}} - Q̂_He::Hermitian{NT, Matrix{NT}} - R̂_He::Hermitian{NT, Matrix{NT}} + Q̂::Hermitian{NT, Matrix{NT}} + R̂::Hermitian{NT, Matrix{NT}} + invP̄::Hermitian{NT, Matrix{NT}} + invQ̂_He::Hermitian{NT, Matrix{NT}} + invR̂_He::Hermitian{NT, Matrix{NT}} X̂min::Vector{NT} X̂max::Vector{NT} - X̂ ::Vector{NT} - Ym::Vector{NT} - U ::Vector{NT} - D ::Vector{NT} - Ŵ ::Vector{NT} + X̂ ::Union{Vector{NT}, Missing} + Ym::Union{Vector{NT}, Missing} + U ::Union{Vector{NT}, Missing} + D ::Union{Vector{NT}, Missing} + Ŵ ::Union{Vector{NT}, Missing} x̂0_past::Vector{NT} - Nk::Vector{Int} + Nk::Vector{Int} # TODO: remove this field and count missing values in X̂ function MovingHorizonEstimator{NT, SM, JM}( model::SM, He, i_ym, nint_u, nint_ym, P̂0, Q̂, R̂, optim::JM ) where {NT<:Real, SM<:SimModel{NT}, JM<:JuMP.GenericModel} @@ -54,12 +57,13 @@ struct MovingHorizonEstimator{ validate_kfcov(nym, nx̂, Q̂, R̂, P̂0) lastu0 = zeros(NT, model.nu) x̂ = [zeros(NT, model.nx); zeros(NT, nxs)] - Q̂, R̂ = Hermitian(Q̂, :L), Hermitian(R̂, :L) P̂0 = Hermitian(P̂0, :L) - Q̂_He = Hermitian(repeatdiag(Q̂, He), :L) - R̂_He = Hermitian(repeatdiag(R̂, He), :L) + Q̂, R̂ = Hermitian(Q̂, :L), Hermitian(R̂, :L) + invP̄ = Hermitian(inv(P̂0), :L) + invQ̂_He = Hermitian(repeatdiag(inv(Q̂), He), :L) + invR̂_He = Hermitian(repeatdiag(inv(R̂), He), :L) P̂ = copy(P̂0) - X̂min, X̂max = fill(-Inf, nx̂*He), fill(+Inf, nx̂*He) + X̂min, X̂max = fill(-Inf, nx̂*(He+1)), fill(+Inf, nx̂*(He+1)) 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) @@ -71,7 +75,7 @@ struct MovingHorizonEstimator{ i_ym, nx̂, nym, nyu, nxs, As, Cs_u, Cs_y, nint_u, nint_ym, Â, B̂u, Ĉ, B̂d, D̂d, - P̂0, Q̂_He, R̂_He, + P̂0, Q̂, R̂, invP̄, invQ̂_He, invR̂_He, X̂min, X̂max, X̂, Ym, U, D, Ŵ, x̂0_past, Nk @@ -105,50 +109,6 @@ function MovingHorizonEstimator( ) end -#= -@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::MovingHorizonEstimator, optim::JuMP.GenericModel) @@ -170,21 +130,19 @@ function init_optimization!( last_W̃tup_float, last_W̃tup_dual = nothing, nothing Ŷm_cache::DiffCache{Vector{JNT}, Vector{JNT}} = DiffCache(zeros(nŶm), nvar + 3) X̂_cache ::DiffCache{Vector{JNT}, Vector{JNT}} = DiffCache(zeros(nX̂) , nvar + 3) - function Jfunc(W̃tup::Float64...) + function Jfunc(W̃tup::JNT...) Ŷ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 - println(length(X̂)) Ŷm, _ = predict!(Ŷm, X̂, estim, model, W̃) last_W̃tup_float = W̃tup end return obj_nonlinprog(estim, model, Ŷm, W̃) end - function Jfunc(W̃tup::Real...) + function Jfunc(W̃tup::ForwardDiff.Dual...) Ŷ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 - println(length(X̂)) Ŷm, _ = predict!(Ŷm, X̂, estim, model, W̃) last_W̃tup_dual = W̃tup end @@ -198,41 +156,51 @@ function init_optimization!( end +"Print the overall dimensions of the state estimator `estim` with left padding `n`." +function print_estim_dim(io::IO, estim::MovingHorizonEstimator, n) + nu, nd = estim.model.nu, estim.model.nd + nx̂, nym, nyu = estim.nx̂, estim.nym, estim.nyu + He = estim.He + println(io, "$(lpad(He, n)) estimation steps He") + 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)) integrating states)") + println(io, "$(lpad(nyu, n)) unmeasured outputs yu") + print(io, "$(lpad(nd, n)) measured disturbances d") +end + + """ obj_nonlinprog(estim::MovingHorizonEstimator, model::SimModel, ΔŨ::Vector{Real}) -Objective function for [`NonLinMHE`] when `model` is not a [`LinModel`](@ref). +Objective function for [`MovingHorizonEstimator`](@ref). + +The function `dot(x, A, x)` is a performant way of calculating `x'*A*x`. """ function obj_nonlinprog( - estim::MovingHorizonEstimator, model::SimModel, Ŷm, W̃::Vector{T} + estim::MovingHorizonEstimator, ::SimModel, Ŷm, W̃::Vector{T} ) where {T<:Real} - P̂0, Q̂_He, R̂_He = estim.P̂0, estim.Q̂_He, estim.R̂_He - nYm, nŴ, nx̂ = estim.Nk[]*estim.nym, estim.Nk[]*estim.nx̂, estim.nx̂ - x̄0 = W̃[1:nx̂] - estim.x̂0_past # W̃ = [x̂(k-Nk|k); Ŵ] - V̂ = estim.Ym[1:nYm] - Ŷm[1:nYm] - Ŵ = W̃[nx̂+1:nx̂+nŴ] - return x̄0'*inv(estim.P̂0)*x̄0 + Ŵ'*inv(Q̂_He[1:nŴ, 1:nŴ])*Ŵ + V̂'*inv(R̂_He[1:nYm, 1:nYm])*V̂ + nYm, nŴ, nx̂, invP̄ = estim.Nk[]*estim.nym, estim.Nk[]*estim.nx̂, estim.nx̂, estim.invP̄ + invQ̂_Nk, invR̂_Nk = @views estim.invQ̂_He[1:nŴ, 1:nŴ], estim.invR̂_He[1:nYm, 1:nYm] + x̄0 = @views W̃[1:nx̂] - estim.x̂0_past # W̃ = [x̂(k-Nk|k); Ŵ] + V̂ = @views estim.Ym[1:nYm] - Ŷm[1:nYm] + Ŵ = @views W̃[nx̂+1:nx̂+nŴ] + return dot(x̄0, invP̄, x̄0) + dot(Ŵ, invQ̂_Nk, Ŵ) + dot(V̂, invR̂_Nk, V̂) end function predict!( Ŷm, X̂, estim::MovingHorizonEstimator, model::SimModel, W̃::Vector{T} ) where {T<:Real} nu, nd, nx̂, nym, Nk = model.nu, model.nd, estim.nx̂, estim.nym, estim.Nk[] - u::Vector{T} = Vector{T}(undef, nu) - d::Vector{T} = Vector{T}(undef, nd) - ŵ::Vector{T} = Vector{T}(undef, nx̂) - x̂::Vector{T} = W̃[1:nx̂] # W̃ = [x̂(k-Nk|k); Ŵ] + X̂[1:nx̂] = W̃[1:nx̂] # W̃ = [x̂(k-Nk|k); Ŵ] for j=1:Nk - u[:] = estim.U[(1 + nu*(j-1)):(nu*j)] - d[:] = estim.D[(1 + nd*(j-1)):(nd*j)] - i = j+1 - ŵ[:] = W̃[(1 + nx̂*(i-1)):(nx̂*i)] - X̂[(1 + nx̂*(j-1)):(nx̂*j)] = x̂ + u = @views estim.U[(1 + nu*(j-1)):(nu*j)] + d = @views estim.D[(1 + nd*(j-1)):(nd*j)] + ŵ = @views W̃[(1 + nx̂*j):(nx̂*(j+1))] + x̂ = @views X̂[(1 + nx̂*(j-1)):(nx̂*j)] Ŷm[(1 + nym*(j-1)):(nym*j)] = ĥ(estim, model, x̂, d)[estim.i_ym] - x̂[:] = f̂(estim, model, x̂, u, d) + ŵ + X̂[(1 + nx̂*j):(nx̂*(j+1))] = f̂(estim, model, x̂, u, d) + ŵ end - j = Nk + 1 - X̂[(1 + nx̂*(j-1)):(nx̂*j)] = x̂ return Ŷm, X̂ end @@ -252,10 +220,11 @@ A ref[^4]: [^4]: TODO """ -function update_estimate!(estim::MovingHorizonEstimator, u, ym, d) +function update_estimate!(estim::MovingHorizonEstimator{NT}, u, ym, d) where NT<:Real model, optim, x̂, P̂ = estim.model, estim.optim, estim.x̂, estim.P̂ nx̂, nym, nu, nd, nŵ = estim.nx̂, estim.nym, model.nu, model.nd, estim.nx̂ Nk, He = estim.Nk[], estim.He + nŴ, nYm, nX̂ = nx̂*Nk, nym*Nk, nx̂*(Nk+1) W̃var::Vector{VariableRef} = optim[:W̃var] ŵ = zeros(nŵ) # ŵ(k) = 0 for warm-starting if Nk < He @@ -271,15 +240,46 @@ function update_estimate!(estim::MovingHorizonEstimator, u, ym, d) estim.D[:] = [estim.D[nd+1:end] ; d] estim.Ŵ[:] = [estim.Ŵ[nŵ+1:end] ; ŵ] end + Ŷm = Vector{NT}(undef, nYm) + X̂ = Vector{NT}(undef, nX̂) estim.x̂0_past[:] = estim.X̂[1:nx̂] W̃0 = [estim.x̂0_past; estim.Ŵ] + Ŷm, X̂ = predict!(Ŷm, X̂, estim, model, W̃0) + J0 = obj_nonlinprog(estim, model, Ŷm, W̃0) + # initial W̃0 with Ŵ=0 if objective or constraint function not finite : + isfinite(J0) || (W̃0 = [estim.x̂0_past; zeros(NT, nŴ)]) set_start_value.(W̃var, W̃0) - optimize!(optim) - println(solution_summary(optim)) + unfix.(W̃var[is_fixed.(W̃var)]) + fix.(W̃var[(nx̂*(Nk+1)+1):end], 0.0) + try + optimize!(optim) + catch err + if isa(err, MOI.UnsupportedAttribute{MOI.VariablePrimalStart}) + # reset_optimizer to unset warm-start, set_start_value.(nothing) seems buggy + MOIU.reset_optimizer(optim) + optimize!(optim) + else + rethrow(err) + end + end + status = termination_status(optim) + W̃curr = value.(W̃var) + if !(status == OPTIMAL || status == LOCALLY_SOLVED) + if isfatal(status) + @error("MHE terminated without solution: estimation in open-loop", + status) + else + @warn("MHE termination status not OPTIMAL or LOCALLY_SOLVED: keeping "* + "solution anyway", status) + end + @debug solution_summary(optim, verbose=true) + end + #if !isfatal(status) + #esimt.W̃[:] = + W̃ = value.(W̃var) + + estim.Ŵ[1:nŴ] = W̃[nx̂+1:nx̂+nŴ] # W̃ = [x̂(k-Nk|k); Ŵ] - W̃ = value.(W̃var) - Ŷm = zeros(nym*Nk) - X̂ = zeros(nx̂*(Nk+1)) Ŷm, X̂ = predict!(Ŷm, X̂, estim, model, W̃) x̂[:] = X̂[(1 + nx̂*Nk):(nx̂*(Nk+1))] estim.Nk[] = Nk < He ? Nk + 1 : He diff --git a/src/predictive_control.jl b/src/predictive_control.jl index b116a565a..178858abd 100644 --- a/src/predictive_control.jl +++ b/src/predictive_control.jl @@ -27,13 +27,6 @@ const DEFAULT_LWT = 0.0 const DEFAULT_CWT = 1e5 const DEFAULT_EWT = 0.0 -"Termination status that means 'no solution available'." -const FATAL_STATUSES = [ - INFEASIBLE, DUAL_INFEASIBLE, LOCALLY_INFEASIBLE, INFEASIBLE_OR_UNBOUNDED, - NUMERICAL_ERROR, INVALID_MODEL, INVALID_OPTION, INTERRUPTED, - OTHER_ERROR -] - "Include all the data for the constraints of [`PredictiveController`](@ref)" struct ControllerConstraint{NT<:Real} ẽx̂ ::Matrix{NT} From 4ac73790defdcca15532d149da4d61ef209521ae Mon Sep 17 00:00:00 2001 From: franckgaga Date: Fri, 1 Dec 2023 12:19:13 -0500 Subject: [PATCH 07/20] Nk field replaced with `missing`s --- src/estimator/mhe.jl | 31 ++++++++++++++++++------------- 1 file changed, 18 insertions(+), 13 deletions(-) diff --git a/src/estimator/mhe.jl b/src/estimator/mhe.jl index 59658d88f..1b9dba1c6 100644 --- a/src/estimator/mhe.jl +++ b/src/estimator/mhe.jl @@ -37,13 +37,12 @@ struct MovingHorizonEstimator{ invR̂_He::Hermitian{NT, Matrix{NT}} X̂min::Vector{NT} X̂max::Vector{NT} - X̂ ::Union{Vector{NT}, Missing} - Ym::Union{Vector{NT}, Missing} - U ::Union{Vector{NT}, Missing} - D ::Union{Vector{NT}, Missing} - Ŵ ::Union{Vector{NT}, Missing} + X̂ ::Vector{Union{NT, Missing}} + Ym::Vector{Union{NT, Missing}} + U ::Vector{Union{NT, Missing}} + D ::Vector{Union{NT, Missing}} + Ŵ ::Vector{Union{NT, Missing}} x̂0_past::Vector{NT} - Nk::Vector{Int} # TODO: remove this field and count missing values in X̂ function MovingHorizonEstimator{NT, SM, JM}( model::SM, He, i_ym, nint_u, nint_ym, P̂0, Q̂, R̂, optim::JM ) where {NT<:Real, SM<:SimModel{NT}, JM<:JuMP.GenericModel} @@ -66,9 +65,12 @@ struct MovingHorizonEstimator{ X̂min, X̂max = fill(-Inf, nx̂*(He+1)), fill(+Inf, nx̂*(He+1)) 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̂ = fill(missing, nx̂*He) + Ym = fill(missing, nym*He) + U = fill(missing, nu*He) + D = fill(missing, nd*He) + Ŵ = fill(missing, nx̂*He) x̂0_past = zeros(nx̂) - Nk = [1] estim = new{NT, SM, JM}( model, optim, W̃, lastu0, x̂, P̂, He, @@ -78,7 +80,7 @@ struct MovingHorizonEstimator{ P̂0, Q̂, R̂, invP̄, invQ̂_He, invR̂_He, X̂min, X̂max, X̂, Ym, U, D, Ŵ, - x̂0_past, Nk + x̂0_past ) init_optimization!(estim, optim) return estim @@ -180,7 +182,8 @@ The function `dot(x, A, x)` is a performant way of calculating `x'*A*x`. function obj_nonlinprog( estim::MovingHorizonEstimator, ::SimModel, Ŷm, W̃::Vector{T} ) where {T<:Real} - nYm, nŴ, nx̂, invP̄ = estim.Nk[]*estim.nym, estim.Nk[]*estim.nx̂, estim.nx̂, estim.invP̄ + Nk = estim.He - count(ismissing, estim.X̂) ÷ estim.nx̂ + 1 + nYm, nŴ, nx̂, invP̄ = Nk*estim.nym, Nk*estim.nx̂, estim.nx̂, estim.invP̄ invQ̂_Nk, invR̂_Nk = @views estim.invQ̂_He[1:nŴ, 1:nŴ], estim.invR̂_He[1:nYm, 1:nYm] x̄0 = @views W̃[1:nx̂] - estim.x̂0_past # W̃ = [x̂(k-Nk|k); Ŵ] V̂ = @views estim.Ym[1:nYm] - Ŷm[1:nYm] @@ -191,7 +194,8 @@ end function predict!( Ŷm, X̂, estim::MovingHorizonEstimator, model::SimModel, W̃::Vector{T} ) where {T<:Real} - nu, nd, nx̂, nym, Nk = model.nu, model.nd, estim.nx̂, estim.nym, estim.Nk[] + Nk = estim.He - count(ismissing, estim.X̂) ÷ estim.nx̂ + 1 + nu, nd, nx̂, nym = model.nu, model.nd, estim.nx̂, estim.nym X̂[1:nx̂] = W̃[1:nx̂] # W̃ = [x̂(k-Nk|k); Ŵ] for j=1:Nk u = @views estim.U[(1 + nu*(j-1)):(nu*j)] @@ -223,7 +227,8 @@ A ref[^4]: function update_estimate!(estim::MovingHorizonEstimator{NT}, u, ym, d) where NT<:Real model, optim, x̂, P̂ = estim.model, estim.optim, estim.x̂, estim.P̂ nx̂, nym, nu, nd, nŵ = estim.nx̂, estim.nym, model.nu, model.nd, estim.nx̂ - Nk, He = estim.Nk[], estim.He + He = estim.He + Nk = He - count(ismissing, estim.X̂) ÷ nx̂ + 1 nŴ, nYm, nX̂ = nx̂*Nk, nym*Nk, nx̂*(Nk+1) W̃var::Vector{VariableRef} = optim[:W̃var] ŵ = zeros(nŵ) # ŵ(k) = 0 for warm-starting @@ -282,6 +287,6 @@ function update_estimate!(estim::MovingHorizonEstimator{NT}, u, ym, d) where NT< Ŷm, X̂ = predict!(Ŷm, X̂, estim, model, W̃) x̂[:] = X̂[(1 + nx̂*Nk):(nx̂*(Nk+1))] - estim.Nk[] = Nk < He ? Nk + 1 : He + #estim.Nk[] = Nk < He ? Nk + 1 : He return x̂, P̂ end \ No newline at end of file From 6e4eb0d27bcc219bd9b3a9caecbeebd7344adfc7 Mon Sep 17 00:00:00 2001 From: franckgaga Date: Fri, 1 Dec 2023 15:35:01 -0500 Subject: [PATCH 08/20] debug windows with `missing`s --- src/estimator/mhe.jl | 36 ++++++++++++++++++++---------------- 1 file changed, 20 insertions(+), 16 deletions(-) diff --git a/src/estimator/mhe.jl b/src/estimator/mhe.jl index 1b9dba1c6..a7680a464 100644 --- a/src/estimator/mhe.jl +++ b/src/estimator/mhe.jl @@ -181,8 +181,8 @@ The function `dot(x, A, x)` is a performant way of calculating `x'*A*x`. """ function obj_nonlinprog( estim::MovingHorizonEstimator, ::SimModel, Ŷm, W̃::Vector{T} -) where {T<:Real} - Nk = estim.He - count(ismissing, estim.X̂) ÷ estim.nx̂ + 1 +) where {T<:Union{Real, Missing}} + Nk = estim.He - count(ismissing, estim.X̂) ÷ estim.nx̂ nYm, nŴ, nx̂, invP̄ = Nk*estim.nym, Nk*estim.nx̂, estim.nx̂, estim.invP̄ invQ̂_Nk, invR̂_Nk = @views estim.invQ̂_He[1:nŴ, 1:nŴ], estim.invR̂_He[1:nYm, 1:nYm] x̄0 = @views W̃[1:nx̂] - estim.x̂0_past # W̃ = [x̂(k-Nk|k); Ŵ] @@ -193,8 +193,9 @@ end function predict!( Ŷm, X̂, estim::MovingHorizonEstimator, model::SimModel, W̃::Vector{T} -) where {T<:Real} - Nk = estim.He - count(ismissing, estim.X̂) ÷ estim.nx̂ + 1 +) where {T<:Union{Real, Missing}} + Nk = estim.He - count(ismissing, estim.X̂) ÷ estim.nx̂ + println(Nk) nu, nd, nx̂, nym = model.nu, model.nd, estim.nx̂, estim.nym X̂[1:nx̂] = W̃[1:nx̂] # W̃ = [x̂(k-Nk|k); Ŵ] for j=1:Nk @@ -226,33 +227,37 @@ A ref[^4]: """ function update_estimate!(estim::MovingHorizonEstimator{NT}, u, ym, d) where NT<:Real model, optim, x̂, P̂ = estim.model, estim.optim, estim.x̂, estim.P̂ - nx̂, nym, nu, nd, nŵ = estim.nx̂, estim.nym, model.nu, model.nd, estim.nx̂ - He = estim.He - Nk = He - count(ismissing, estim.X̂) ÷ nx̂ + 1 - nŴ, nYm, nX̂ = nx̂*Nk, nym*Nk, nx̂*(Nk+1) - W̃var::Vector{VariableRef} = optim[:W̃var] + nx̂, nym, nu, nd, nŵ, He = estim.nx̂, estim.nym, model.nu, model.nd, estim.nx̂, estim.He ŵ = zeros(nŵ) # ŵ(k) = 0 for warm-starting - if Nk < He - estim.X̂[ (1 + nx̂*(Nk-1)):(nx̂*Nk)] = x̂ - estim.Ym[(1 + nym*(Nk-1)):(nym*Nk)] = ym - estim.U[ (1 + nu*(Nk-1)):(nu*Nk)] = u - estim.D[ (1 + nd*(Nk-1)):(nd*Nk)] = d - estim.Ŵ[ (1 + nŵ*(Nk-1)):(nŵ*Nk)] = ŵ + n_missing = count(ismissing, estim.X̂) ÷ nx̂ + if n_missing ≠ 0 + j = He - n_missing + estim.X̂[ (1 + nx̂*j):(nx̂*(j+1))] = x̂ + estim.Ym[(1 + nym*j):(nym*(j+1))] = ym + estim.U[ (1 + nu*j):(nu*(j+1))] = u + estim.D[ (1 + nd*j):(nd*(j+1))] = d + estim.Ŵ[ (1 + nŵ*j):(nŵ*(j+1))] = ŵ + Nk = j + 1 else estim.X̂[:] = [estim.X̂[nx̂+1:end] ; x̂] estim.Ym[:] = [estim.Ym[nym+1:end]; ym] estim.U[:] = [estim.U[nu+1:end] ; u] estim.D[:] = [estim.D[nd+1:end] ; d] estim.Ŵ[:] = [estim.Ŵ[nŵ+1:end] ; ŵ] + Nk = He end + nŴ, nYm, nX̂ = nx̂*Nk, nym*Nk, nx̂*(Nk+1) Ŷm = Vector{NT}(undef, nYm) X̂ = Vector{NT}(undef, nX̂) estim.x̂0_past[:] = estim.X̂[1:nx̂] W̃0 = [estim.x̂0_past; estim.Ŵ] + Ŷm, X̂ = predict!(Ŷm, X̂, estim, model, W̃0) J0 = obj_nonlinprog(estim, model, Ŷm, W̃0) # initial W̃0 with Ŵ=0 if objective or constraint function not finite : isfinite(J0) || (W̃0 = [estim.x̂0_past; zeros(NT, nŴ)]) + + W̃var::Vector{VariableRef} = optim[:W̃var] set_start_value.(W̃var, W̃0) unfix.(W̃var[is_fixed.(W̃var)]) fix.(W̃var[(nx̂*(Nk+1)+1):end], 0.0) @@ -268,7 +273,6 @@ function update_estimate!(estim::MovingHorizonEstimator{NT}, u, ym, d) where NT< end end status = termination_status(optim) - W̃curr = value.(W̃var) if !(status == OPTIMAL || status == LOCALLY_SOLVED) if isfatal(status) @error("MHE terminated without solution: estimation in open-loop", From a3a117c194d0cc7eb9115511b47940230fbc76d9 Mon Sep 17 00:00:00 2001 From: franckgaga Date: Fri, 1 Dec 2023 15:51:16 -0500 Subject: [PATCH 09/20] debug : type-stable `update_estimate!` --- src/estimator/internal_model.jl | 2 +- src/estimator/kalman.jl | 6 +++--- src/estimator/luenberger.jl | 2 +- src/estimator/mhe.jl | 21 +++++++-------------- 4 files changed, 12 insertions(+), 19 deletions(-) diff --git a/src/estimator/internal_model.jl b/src/estimator/internal_model.jl index 3a1237844..5de35d356 100644 --- a/src/estimator/internal_model.jl +++ b/src/estimator/internal_model.jl @@ -229,7 +229,7 @@ function update_estimate!( ŷs = zeros(NT, model.ny) ŷs[estim.i_ym] = ym - ŷd[estim.i_ym] # ŷs=0 for unmeasured outputs x̂s[:] = estim.Âs*x̂s + estim.B̂s*ŷs - return x̂d + return nothing end @doc raw""" diff --git a/src/estimator/kalman.jl b/src/estimator/kalman.jl index b36955f65..eea0e55dd 100644 --- a/src/estimator/kalman.jl +++ b/src/estimator/kalman.jl @@ -181,7 +181,7 @@ function update_estimate!(estim::SteadyKalmanFilter, u, ym, d=empty(estim.x̂)) Â, B̂u, B̂d, Ĉm, D̂dm = estim.Â, estim.B̂u, estim.B̂d, estim.Ĉm, estim.D̂dm x̂, K̂ = estim.x̂, estim.K̂ x̂[:] = Â*x̂ + B̂u*u + B̂d*d + K̂*(ym - Ĉm*x̂ - D̂dm*d) - return x̂ + return nothing end struct KalmanFilter{NT<:Real, SM<:LinModel} <: StateEstimator{NT} @@ -578,7 +578,7 @@ function update_estimate!(estim::UnscentedKalmanFilter{NT}, u, ym, d) where NT<: x̂[:] = X̂_next * m̂ X̄_next = X̂_next .- x̂ P̂.data[:] = X̄_next * Ŝ * X̄_next' + Q̂ # .data is necessary for Hermitians - return x̂, P̂ + return nothing end struct ExtendedKalmanFilter{NT<:Real, SM<:SimModel} <: StateEstimator{NT} @@ -780,5 +780,5 @@ function update_estimate_kf!(estim, Â, Ĉm, u, ym, d) ŷm = @views ĥ(estim, estim.model, x̂, d)[estim.i_ym] x̂[:] = f̂(estim, estim.model, x̂, u, d) + K̂ * (ym - ŷm) P̂.data[:] = Â * (P̂ - M̂ * Ĉm * P̂) * Â' + Q̂ # .data is necessary for Hermitians - return x̂, P̂ + return nothing end diff --git a/src/estimator/luenberger.jl b/src/estimator/luenberger.jl index 301717a80..71d3717d3 100644 --- a/src/estimator/luenberger.jl +++ b/src/estimator/luenberger.jl @@ -109,5 +109,5 @@ function update_estimate!(estim::Luenberger, u, ym, d=empty(estim.x̂)) Â, B̂u, B̂d, Ĉm, D̂dm = estim.Â, estim.B̂u, estim.B̂d, estim.Ĉm, estim.D̂dm x̂, K̂ = estim.x̂, estim.K̂ x̂[:] = Â*x̂ + B̂u*u + B̂d*d + K̂*(ym - Ĉm*x̂ - D̂dm*d) - return x̂ + return nothing end \ No newline at end of file diff --git a/src/estimator/mhe.jl b/src/estimator/mhe.jl index a7680a464..1520da4a2 100644 --- a/src/estimator/mhe.jl +++ b/src/estimator/mhe.jl @@ -9,7 +9,7 @@ struct MovingHorizonEstimator{ # note: `NT` and the number type `JNT` in `JuMP.GenericModel{JNT}` can be # different since solvers that support non-Float64 are scarce. optim::JM - W̃::Vector{NT} + W̃::Vector{Union{NT, Missing}} lastu0::Vector{NT} x̂::Vector{NT} P̂::Hermitian{NT, Matrix{NT}} @@ -195,7 +195,6 @@ function predict!( Ŷm, X̂, estim::MovingHorizonEstimator, model::SimModel, W̃::Vector{T} ) where {T<:Union{Real, Missing}} Nk = estim.He - count(ismissing, estim.X̂) ÷ estim.nx̂ - println(Nk) nu, nd, nx̂, nym = model.nu, model.nd, estim.nx̂, estim.nym X̂[1:nx̂] = W̃[1:nx̂] # W̃ = [x̂(k-Nk|k); Ŵ] for j=1:Nk @@ -246,17 +245,15 @@ function update_estimate!(estim::MovingHorizonEstimator{NT}, u, ym, d) where NT< estim.Ŵ[:] = [estim.Ŵ[nŵ+1:end] ; ŵ] Nk = He end - nŴ, nYm, nX̂ = nx̂*Nk, nym*Nk, nx̂*(Nk+1) + nŴ, nYm, nX̂ = nŵ*Nk, nym*Nk, nx̂*(Nk+1) Ŷm = Vector{NT}(undef, nYm) X̂ = Vector{NT}(undef, nX̂) estim.x̂0_past[:] = estim.X̂[1:nx̂] W̃0 = [estim.x̂0_past; estim.Ŵ] - Ŷm, X̂ = predict!(Ŷm, X̂, estim, model, W̃0) J0 = obj_nonlinprog(estim, model, Ŷm, W̃0) # initial W̃0 with Ŵ=0 if objective or constraint function not finite : isfinite(J0) || (W̃0 = [estim.x̂0_past; zeros(NT, nŴ)]) - W̃var::Vector{VariableRef} = optim[:W̃var] set_start_value.(W̃var, W̃0) unfix.(W̃var[is_fixed.(W̃var)]) @@ -273,6 +270,7 @@ function update_estimate!(estim::MovingHorizonEstimator{NT}, u, ym, d) where NT< end end status = termination_status(optim) + W̃curr, W̃last = value.(W̃var), W̃0 if !(status == OPTIMAL || status == LOCALLY_SOLVED) if isfatal(status) @error("MHE terminated without solution: estimation in open-loop", @@ -283,14 +281,9 @@ function update_estimate!(estim::MovingHorizonEstimator{NT}, u, ym, d) where NT< end @debug solution_summary(optim, verbose=true) end - #if !isfatal(status) - #esimt.W̃[:] = - W̃ = value.(W̃var) - - estim.Ŵ[1:nŴ] = W̃[nx̂+1:nx̂+nŴ] # W̃ = [x̂(k-Nk|k); Ŵ] - - Ŷm, X̂ = predict!(Ŷm, X̂, estim, model, W̃) + estim.W̃[:] = !isfatal(status) ? W̃curr : W̃last + estim.Ŵ[1:nŴ] = estim.W̃[nx̂+1:nx̂+nŴ] # update Ŵ with optimum for next time step + Ŷm, X̂ = predict!(Ŷm, X̂, estim, model, estim.W̃) x̂[:] = X̂[(1 + nx̂*Nk):(nx̂*(Nk+1))] - #estim.Nk[] = Nk < He ? Nk + 1 : He - return x̂, P̂ + return nothing end \ No newline at end of file From 42705470e28d5cada4621bdf8180ddc888b625e1 Mon Sep 17 00:00:00 2001 From: franckgaga Date: Fri, 1 Dec 2023 16:57:19 -0500 Subject: [PATCH 10/20] mhe stop working ?? --- src/estimator/mhe.jl | 33 +++++++++++++++++++++++++-------- 1 file changed, 25 insertions(+), 8 deletions(-) diff --git a/src/estimator/mhe.jl b/src/estimator/mhe.jl index 1520da4a2..47906d36d 100644 --- a/src/estimator/mhe.jl +++ b/src/estimator/mhe.jl @@ -9,7 +9,7 @@ struct MovingHorizonEstimator{ # note: `NT` and the number type `JNT` in `JuMP.GenericModel{JNT}` can be # different since solvers that support non-Float64 are scarce. optim::JM - W̃::Vector{Union{NT, Missing}} + W̃::Vector{NT} lastu0::Vector{NT} x̂::Vector{NT} P̂::Hermitian{NT, Matrix{NT}} @@ -123,7 +123,7 @@ function init_optimization!( # --- variables and linear constraints --- nvar = length(estim.W̃) set_silent(optim) - #limit_solve_time(estim) + #limit_solve_time(estim) #TODO: add this feature @variable(optim, W̃var[1:nvar]) # --- nonlinear optimization init --- nym, nx̂, He = estim.nym, estim.nx̂, estim.He #, length(i_g) @@ -181,19 +181,20 @@ The function `dot(x, A, x)` is a performant way of calculating `x'*A*x`. """ function obj_nonlinprog( estim::MovingHorizonEstimator, ::SimModel, Ŷm, W̃::Vector{T} -) where {T<:Union{Real, Missing}} +) where {T<:Real} Nk = estim.He - count(ismissing, estim.X̂) ÷ estim.nx̂ nYm, nŴ, nx̂, invP̄ = Nk*estim.nym, Nk*estim.nx̂, estim.nx̂, estim.invP̄ invQ̂_Nk, invR̂_Nk = @views estim.invQ̂_He[1:nŴ, 1:nŴ], estim.invR̂_He[1:nYm, 1:nYm] x̄0 = @views W̃[1:nx̂] - estim.x̂0_past # W̃ = [x̂(k-Nk|k); Ŵ] V̂ = @views estim.Ym[1:nYm] - Ŷm[1:nYm] Ŵ = @views W̃[nx̂+1:nx̂+nŴ] + println(dot(x̄0, invP̄, x̄0) + dot(Ŵ, invQ̂_Nk, Ŵ) + dot(V̂, invR̂_Nk, V̂)) return dot(x̄0, invP̄, x̄0) + dot(Ŵ, invQ̂_Nk, Ŵ) + dot(V̂, invR̂_Nk, V̂) end function predict!( Ŷm, X̂, estim::MovingHorizonEstimator, model::SimModel, W̃::Vector{T} -) where {T<:Union{Real, Missing}} +) where {T<:Real} Nk = estim.He - count(ismissing, estim.X̂) ÷ estim.nx̂ nu, nd, nx̂, nym = model.nu, model.nd, estim.nx̂, estim.nym X̂[1:nx̂] = W̃[1:nx̂] # W̃ = [x̂(k-Nk|k); Ŵ] @@ -208,6 +209,20 @@ function predict!( return Ŷm, X̂ end +"Reset `estim.P̂`, `estim.invP̄` and the time windows for the moving horizon estimator." +function init_estimate_cov!(estim::MovingHorizonEstimator, _ , _ , _ ) + estim.P̂.data[:] = estim.P̂0 # .data is necessary for Hermitians + estim.invP̄.data[:] = Hermitian(inv(estim.P̂0), :L) + estim.x̂0_past .= 0 + estim.W̃ .= 0 + estim.X̂ .= missing + estim.Ym .= missing + estim.U .= missing + estim.D .= missing + estim.Ŵ .= missing + return nothing +end + @doc raw""" update_estimate!(estim::UnscentedKalmanFilter, u, ym, d) @@ -245,19 +260,21 @@ function update_estimate!(estim::MovingHorizonEstimator{NT}, u, ym, d) where NT< estim.Ŵ[:] = [estim.Ŵ[nŵ+1:end] ; ŵ] Nk = He end + estim.x̂0_past[:] = estim.X̂[1:nx̂] + W̃var::Vector{VariableRef} = optim[:W̃var] nŴ, nYm, nX̂ = nŵ*Nk, nym*Nk, nx̂*(Nk+1) Ŷm = Vector{NT}(undef, nYm) X̂ = Vector{NT}(undef, nX̂) - estim.x̂0_past[:] = estim.X̂[1:nx̂] - W̃0 = [estim.x̂0_past; estim.Ŵ] + W̃0 = zeros(length(W̃var)) + W̃0[1:nx̂] = estim.x̂0_past + W̃0[nx̂+1:nx̂+nŴ] = estim.Ŵ[1:nŴ] Ŷm, X̂ = predict!(Ŷm, X̂, estim, model, W̃0) J0 = obj_nonlinprog(estim, model, Ŷm, W̃0) # initial W̃0 with Ŵ=0 if objective or constraint function not finite : isfinite(J0) || (W̃0 = [estim.x̂0_past; zeros(NT, nŴ)]) - W̃var::Vector{VariableRef} = optim[:W̃var] set_start_value.(W̃var, W̃0) unfix.(W̃var[is_fixed.(W̃var)]) - fix.(W̃var[(nx̂*(Nk+1)+1):end], 0.0) + fix.(W̃var[(nx̂*(Nk+1)+1):end], 0.0) try optimize!(optim) catch err From e1dbf96400bf674498b1bf6ce042de3bc4b5e590 Mon Sep 17 00:00:00 2001 From: franckgaga Date: Sat, 2 Dec 2023 12:15:02 -0500 Subject: [PATCH 11/20] back to `Nk` field, simpler --- src/estimator/mhe.jl | 67 ++++++++++++++++++++------------------------ 1 file changed, 30 insertions(+), 37 deletions(-) diff --git a/src/estimator/mhe.jl b/src/estimator/mhe.jl index 47906d36d..fd640bf18 100644 --- a/src/estimator/mhe.jl +++ b/src/estimator/mhe.jl @@ -37,12 +37,13 @@ struct MovingHorizonEstimator{ invR̂_He::Hermitian{NT, Matrix{NT}} X̂min::Vector{NT} X̂max::Vector{NT} - X̂ ::Vector{Union{NT, Missing}} - Ym::Vector{Union{NT, Missing}} - U ::Vector{Union{NT, Missing}} - D ::Vector{Union{NT, Missing}} - Ŵ ::Vector{Union{NT, Missing}} + X̂ ::Union{Vector{NT}, Missing} + Ym::Union{Vector{NT}, Missing} + U ::Union{Vector{NT}, Missing} + D ::Union{Vector{NT}, Missing} + Ŵ ::Union{Vector{NT}, Missing} x̂0_past::Vector{NT} + Nk::Vector{Int} function MovingHorizonEstimator{NT, SM, JM}( model::SM, He, i_ym, nint_u, nint_ym, P̂0, Q̂, R̂, optim::JM ) where {NT<:Real, SM<:SimModel{NT}, JM<:JuMP.GenericModel} @@ -65,12 +66,9 @@ struct MovingHorizonEstimator{ X̂min, X̂max = fill(-Inf, nx̂*(He+1)), fill(+Inf, nx̂*(He+1)) nvar = nx̂*(He + 1) W̃ = zeros(nvar) - X̂ = fill(missing, nx̂*He) - Ym = fill(missing, nym*He) - U = fill(missing, nu*He) - D = fill(missing, nd*He) - Ŵ = fill(missing, nx̂*He) + X̂, Ym, U, D, Ŵ = zeros(nx̂*He), zeros(nym*He), zeros(nu*He), zeros(nd*He), zeros(nx̂*He) x̂0_past = zeros(nx̂) + Nk = [1] estim = new{NT, SM, JM}( model, optim, W̃, lastu0, x̂, P̂, He, @@ -80,7 +78,7 @@ struct MovingHorizonEstimator{ P̂0, Q̂, R̂, invP̄, invQ̂_He, invR̂_He, X̂min, X̂max, X̂, Ym, U, D, Ŵ, - x̂0_past + x̂0_past, Nk ) init_optimization!(estim, optim) return estim @@ -182,21 +180,19 @@ The function `dot(x, A, x)` is a performant way of calculating `x'*A*x`. function obj_nonlinprog( estim::MovingHorizonEstimator, ::SimModel, Ŷm, W̃::Vector{T} ) where {T<:Real} - Nk = estim.He - count(ismissing, estim.X̂) ÷ estim.nx̂ + Nk = estim.Nk[] nYm, nŴ, nx̂, invP̄ = Nk*estim.nym, Nk*estim.nx̂, estim.nx̂, estim.invP̄ invQ̂_Nk, invR̂_Nk = @views estim.invQ̂_He[1:nŴ, 1:nŴ], estim.invR̂_He[1:nYm, 1:nYm] x̄0 = @views W̃[1:nx̂] - estim.x̂0_past # W̃ = [x̂(k-Nk|k); Ŵ] V̂ = @views estim.Ym[1:nYm] - Ŷm[1:nYm] Ŵ = @views W̃[nx̂+1:nx̂+nŴ] - println(dot(x̄0, invP̄, x̄0) + dot(Ŵ, invQ̂_Nk, Ŵ) + dot(V̂, invR̂_Nk, V̂)) return dot(x̄0, invP̄, x̄0) + dot(Ŵ, invQ̂_Nk, Ŵ) + dot(V̂, invR̂_Nk, V̂) end function predict!( Ŷm, X̂, estim::MovingHorizonEstimator, model::SimModel, W̃::Vector{T} ) where {T<:Real} - Nk = estim.He - count(ismissing, estim.X̂) ÷ estim.nx̂ - nu, nd, nx̂, nym = model.nu, model.nd, estim.nx̂, estim.nym + nu, nd, nx̂, nym, Nk = model.nu, model.nd, estim.nx̂, estim.nym, estim.Nk[] X̂[1:nx̂] = W̃[1:nx̂] # W̃ = [x̂(k-Nk|k); Ŵ] for j=1:Nk u = @views estim.U[(1 + nu*(j-1)):(nu*j)] @@ -215,11 +211,12 @@ function init_estimate_cov!(estim::MovingHorizonEstimator, _ , _ , _ ) estim.invP̄.data[:] = Hermitian(inv(estim.P̂0), :L) estim.x̂0_past .= 0 estim.W̃ .= 0 - estim.X̂ .= missing - estim.Ym .= missing - estim.U .= missing - estim.D .= missing - estim.Ŵ .= missing + estim.X̂ .= 0 + estim.Ym .= 0 + estim.U .= 0 + estim.D .= 0 + estim.Ŵ .= 0 + estim.Nk .= 1 return nothing end @@ -241,33 +238,28 @@ A ref[^4]: """ function update_estimate!(estim::MovingHorizonEstimator{NT}, u, ym, d) where NT<:Real model, optim, x̂, P̂ = estim.model, estim.optim, estim.x̂, estim.P̂ - nx̂, nym, nu, nd, nŵ, He = estim.nx̂, estim.nym, model.nu, model.nd, estim.nx̂, estim.He + nx̂, nym, nu, nd, nŵ = estim.nx̂, estim.nym, model.nu, model.nd, estim.nx̂ + Nk, He = estim.Nk[], estim.He + nŴ, nYm, nX̂ = nx̂*Nk, nym*Nk, nx̂*(Nk+1) + W̃var::Vector{VariableRef} = optim[:W̃var] ŵ = zeros(nŵ) # ŵ(k) = 0 for warm-starting - n_missing = count(ismissing, estim.X̂) ÷ nx̂ - if n_missing ≠ 0 - j = He - n_missing - estim.X̂[ (1 + nx̂*j):(nx̂*(j+1))] = x̂ - estim.Ym[(1 + nym*j):(nym*(j+1))] = ym - estim.U[ (1 + nu*j):(nu*(j+1))] = u - estim.D[ (1 + nd*j):(nd*(j+1))] = d - estim.Ŵ[ (1 + nŵ*j):(nŵ*(j+1))] = ŵ - Nk = j + 1 + if Nk < He + estim.X̂[ (1 + nx̂*(Nk-1)):(nx̂*Nk)] = x̂ + estim.Ym[(1 + nym*(Nk-1)):(nym*Nk)] = ym + estim.U[ (1 + nu*(Nk-1)):(nu*Nk)] = u + estim.D[ (1 + nd*(Nk-1)):(nd*Nk)] = d + estim.Ŵ[ (1 + nŵ*(Nk-1)):(nŵ*Nk)] = ŵ else estim.X̂[:] = [estim.X̂[nx̂+1:end] ; x̂] estim.Ym[:] = [estim.Ym[nym+1:end]; ym] estim.U[:] = [estim.U[nu+1:end] ; u] estim.D[:] = [estim.D[nd+1:end] ; d] estim.Ŵ[:] = [estim.Ŵ[nŵ+1:end] ; ŵ] - Nk = He end - estim.x̂0_past[:] = estim.X̂[1:nx̂] - W̃var::Vector{VariableRef} = optim[:W̃var] - nŴ, nYm, nX̂ = nŵ*Nk, nym*Nk, nx̂*(Nk+1) Ŷm = Vector{NT}(undef, nYm) X̂ = Vector{NT}(undef, nX̂) - W̃0 = zeros(length(W̃var)) - W̃0[1:nx̂] = estim.x̂0_past - W̃0[nx̂+1:nx̂+nŴ] = estim.Ŵ[1:nŴ] + estim.x̂0_past[:] = estim.X̂[1:nx̂] + W̃0 = [estim.x̂0_past; estim.Ŵ] Ŷm, X̂ = predict!(Ŷm, X̂, estim, model, W̃0) J0 = obj_nonlinprog(estim, model, Ŷm, W̃0) # initial W̃0 with Ŵ=0 if objective or constraint function not finite : @@ -302,5 +294,6 @@ function update_estimate!(estim::MovingHorizonEstimator{NT}, u, ym, d) where NT< estim.Ŵ[1:nŴ] = estim.W̃[nx̂+1:nx̂+nŴ] # update Ŵ with optimum for next time step Ŷm, X̂ = predict!(Ŷm, X̂, estim, model, estim.W̃) x̂[:] = X̂[(1 + nx̂*Nk):(nx̂*(Nk+1))] + estim.Nk[] = Nk < He ? Nk + 1 : He return nothing end \ No newline at end of file From 46f4f7a6086f261e3ad46bd91319f8f617140fa1 Mon Sep 17 00:00:00 2001 From: franckgaga Date: Tue, 5 Dec 2023 17:15:18 -0500 Subject: [PATCH 12/20] added : MHE constraints --- docs/src/public/state_estim.md | 3 +- src/controller/nonlinmpc.jl | 4 +- src/estimator/mhe.jl | 120 +++++++++++++++++++++++++++++---- 3 files changed, 111 insertions(+), 16 deletions(-) diff --git a/docs/src/public/state_estim.md b/docs/src/public/state_estim.md index 656a8cc1e..7a2d899e7 100644 --- a/docs/src/public/state_estim.md +++ b/docs/src/public/state_estim.md @@ -20,7 +20,8 @@ error with closed-loop control (offset-free tracking). The estimators are all implemented in the predictor form (a.k.a. observer form), that is, they all estimates at each discrete time ``k`` the states of the next period ``\mathbf{x̂}_k(k+1)``[^1]. In contrast, the filter form that estimates ``\mathbf{x̂}_k(k)`` -is sometimes slightly more accurate. +is sometimes slightly more accurate. The documentation of the function [`update_estimates!`](@ref) +details how the estimators predicts them. [^1]: also denoted ``\mathbf{x̂}_{k+1|k}`` [elsewhere](https://en.wikipedia.org/wiki/Kalman_filter). diff --git a/src/controller/nonlinmpc.jl b/src/controller/nonlinmpc.jl index 35ee5b76e..7705edd06 100644 --- a/src/controller/nonlinmpc.jl +++ b/src/controller/nonlinmpc.jl @@ -294,7 +294,7 @@ function init_optimization!(mpc::NonLinMPC, optim::JuMP.GenericModel{JNT}) where x̂ = get_tmp(x̂_cache, ΔŨtup[1]) g = get_tmp(g_cache, ΔŨtup[1]) Ŷ, x̂end = predict!(Ŷ, x̂, mpc, model, ΔŨ) - con_nonlinprog!(g, mpc, model, x̂end, Ŷ, ΔŨ) + g = con_nonlinprog!(g, mpc, model, x̂end, Ŷ, ΔŨ) last_ΔŨtup_float = ΔŨtup end return obj_nonlinprog(mpc, model, Ŷ, ΔŨ) @@ -306,7 +306,7 @@ function init_optimization!(mpc::NonLinMPC, optim::JuMP.GenericModel{JNT}) where x̂ = get_tmp(x̂_cache, ΔŨtup[1]) g = get_tmp(g_cache, ΔŨtup[1]) Ŷ, x̂end = predict!(Ŷ, x̂, mpc, model, ΔŨ) - con_nonlinprog!(g, mpc, model, x̂end, Ŷ, ΔŨ) + g = con_nonlinprog!(g, mpc, model, x̂end, Ŷ, ΔŨ) last_ΔŨtup_dual = ΔŨtup end return obj_nonlinprog(mpc, model, Ŷ, ΔŨ) diff --git a/src/estimator/mhe.jl b/src/estimator/mhe.jl index fd640bf18..d0dc3cfda 100644 --- a/src/estimator/mhe.jl +++ b/src/estimator/mhe.jl @@ -37,6 +37,7 @@ struct MovingHorizonEstimator{ invR̂_He::Hermitian{NT, Matrix{NT}} X̂min::Vector{NT} X̂max::Vector{NT} + i_g::BitVector X̂ ::Union{Vector{NT}, Missing} Ym::Union{Vector{NT}, Missing} U ::Union{Vector{NT}, Missing} @@ -64,6 +65,9 @@ struct MovingHorizonEstimator{ invR̂_He = Hermitian(repeatdiag(inv(R̂), He), :L) P̂ = copy(P̂0) X̂min, X̂max = fill(-Inf, nx̂*(He+1)), fill(+Inf, nx̂*(He+1)) + X̂max = fill(100, nx̂*(He+1)) + i_X̂min, i_X̂max = .!isinf.(X̂min), .!isinf.(X̂max) + i_g = [i_X̂min; i_X̂max] 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) @@ -76,7 +80,7 @@ struct MovingHorizonEstimator{ As, Cs_u, Cs_y, nint_u, nint_ym, Â, B̂u, Ĉ, B̂d, D̂d, P̂0, Q̂, R̂, invP̄, invQ̂_He, invR̂_He, - X̂min, X̂max, + X̂min, X̂max, i_g, X̂, Ym, U, D, Ŵ, x̂0_past, Nk ) @@ -118,40 +122,86 @@ Init the nonlinear optimization of [`MovingHorizonEstimator`](@ref). function init_optimization!( estim::MovingHorizonEstimator, optim::JuMP.GenericModel{JNT} ) where JNT<:Real + He = estim.He + nŶm, nX̂, ng = He*estim.nym, (He+1)*estim.nx̂, length(estim.i_g) + println(ng) # --- variables and linear constraints --- nvar = length(estim.W̃) set_silent(optim) + #set_attribute(optim, "max_iter", 1000*nvar) #limit_solve_time(estim) #TODO: add this feature @variable(optim, W̃var[1:nvar]) # --- nonlinear optimization init --- - nym, nx̂, He = estim.nym, estim.nx̂, estim.He #, length(i_g) - # inspired from https://jump.dev/JuMP.jl/stable/tutorials/nonlinear/tips_and_tricks/#User-defined-operators-with-vector-outputs - Jfunc = let estim=estim, model=estim.model, nvar=nvar , nŶm=He*nym, nX̂=(He+1)*nx̂ + # see init_optimization!(mpc::NonLinMPC, optim) for details on the inspiration + Jfunc, gfunc = let estim=estim, model=estim.model, nvar=nvar , nŶm=nŶm, nX̂=nX̂, ng=ng last_W̃tup_float, last_W̃tup_dual = nothing, nothing - Ŷm_cache::DiffCache{Vector{JNT}, Vector{JNT}} = DiffCache(zeros(nŶm), nvar + 3) - X̂_cache ::DiffCache{Vector{JNT}, Vector{JNT}} = DiffCache(zeros(nX̂) , nvar + 3) + Ŷm_cache::DiffCache{Vector{JNT}, Vector{JNT}} = DiffCache(zeros(JNT, nŶm), nvar + 3) + g_cache ::DiffCache{Vector{JNT}, Vector{JNT}} = DiffCache(zeros(JNT, ng) , nvar + 3) + X̂_cache ::DiffCache{Vector{JNT}, Vector{JNT}} = DiffCache(zeros(JNT, nX̂) , nvar + 3) function Jfunc(W̃tup::JNT...) - Ŷm, X̂ = get_tmp(Ŷm_cache, W̃tup[1]), get_tmp(X̂_cache, W̃tup[1]) - W̃ = collect(W̃tup) + Ŷm = get_tmp(Ŷm_cache, W̃tup[1]) + W̃ = collect(W̃tup) if W̃tup != last_W̃tup_float - Ŷm, _ = predict!(Ŷm, X̂, estim, model, W̃) + g = get_tmp(g_cache, W̃tup[1]) + X̂ = get_tmp(X̂_cache, W̃tup[1]) + Ŷm, X̂ = predict!(Ŷm, X̂, estim, model, W̃) + g = con_nonlinprog!(g, estim, model, X̂) last_W̃tup_float = W̃tup end return obj_nonlinprog(estim, model, Ŷm, W̃) end function Jfunc(W̃tup::ForwardDiff.Dual...) - Ŷm, X̂ = get_tmp(Ŷm_cache, W̃tup[1]), get_tmp(X̂_cache, W̃tup[1]) - W̃ = collect(W̃tup) + Ŷm = get_tmp(Ŷm_cache, W̃tup[1]) + W̃ = collect(W̃tup) if W̃tup != last_W̃tup_dual - Ŷm, _ = predict!(Ŷm, X̂, estim, model, W̃) + g = get_tmp(g_cache, W̃tup[1]) + X̂ = get_tmp(X̂_cache, W̃tup[1]) + Ŷm, X̂ = predict!(Ŷm, X̂, estim, model, W̃) + g = con_nonlinprog!(g, estim, model, X̂) last_W̃tup_dual = W̃tup end return obj_nonlinprog(estim, model, Ŷm, W̃) end - Jfunc + function gfunc_i(i, W̃tup::NTuple{N, JNT}) where N + g = get_tmp(g_cache, W̃tup[1]) + if W̃tup != last_W̃tup_dual + Ŷm = get_tmp(Ŷm_cache, W̃tup[1]) + X̂ = get_tmp(X̂_cache, W̃tup[1]) + W̃ = collect(W̃tup) + Ŷm, X̂ = predict!(Ŷm, X̂, estim, model, W̃) + g = con_nonlinprog!(g, estim, model, X̂) + last_W̃tup_float = ΔŨtup + end + return g[i] + end + function gfunc_i(i, W̃tup::NTuple{N, ForwardDiff.Dual}) where N + g = get_tmp(g_cache, W̃tup[1]) + if W̃tup != last_W̃tup_dual + Ŷm = get_tmp(Ŷm_cache, W̃tup[1]) + X̂ = get_tmp(X̂_cache, W̃tup[1]) + W̃ = collect(W̃tup) + Ŷm, X̂ = predict!(Ŷm, X̂, estim, model, W̃) + g = con_nonlinprog!(g, estim, model, X̂) + last_W̃tup_dual = ΔŨtup + end + return g[i] + end + gfunc = [(W̃...) -> gfunc_i(i, W̃) for i in 1:ng] + Jfunc, gfunc end register(optim, :Jfunc, nvar, Jfunc, autodiff=true) @NLobjective(optim, Min, Jfunc(W̃var...)) + if ng ≠ 0 + i_end_X̂min = nX̂ + for i in eachindex(estim.X̂min) + sym = Symbol("g_X̂min_$i") + register(optim, sym, nvar, gfunc[i], autodiff=true) + end + for i in eachindex(estim.X̂max) + sym = Symbol("g_X̂max_$i") + register(optim, sym, nvar, gfunc[i_end_X̂min+i], autodiff=true) + end + end return nothing end @@ -296,4 +346,48 @@ function update_estimate!(estim::MovingHorizonEstimator{NT}, u, ym, d) where NT< x̂[:] = X̂[(1 + nx̂*Nk):(nx̂*(Nk+1))] estim.Nk[] = Nk < He ? Nk + 1 : He return nothing +end + +"Set the nonlinear constraints on the output predictions `Ŷ` and terminal states `x̂end`." +function setnonlincon!(mpc::NonLinMPC, ::NonLinModel) + optim = mpc.optim + ΔŨvar = mpc.optim[:ΔŨvar] + con = mpc.con + map(con -> delete(optim, con), all_nonlinear_constraints(optim)) + for i in findall(.!isinf.(con.Ymin)) + f_sym = Symbol("g_Ymin_$(i)") + add_nonlinear_constraint(optim, :($(f_sym)($(ΔŨvar...)) <= 0)) + end + for i in findall(.!isinf.(con.Ymax)) + f_sym = Symbol("g_Ymax_$(i)") + add_nonlinear_constraint(optim, :($(f_sym)($(ΔŨvar...)) <= 0)) + end + for i in findall(.!isinf.(con.x̂min)) + f_sym = Symbol("g_x̂min_$(i)") + add_nonlinear_constraint(optim, :($(f_sym)($(ΔŨvar...)) <= 0)) + end + for i in findall(.!isinf.(con.x̂max)) + f_sym = Symbol("g_x̂max_$(i)") + add_nonlinear_constraint(optim, :($(f_sym)($(ΔŨvar...)) <= 0)) + end + return nothing +end + + +function con_nonlinprog!(g, estim::MovingHorizonEstimator, ::SimModel, X̂) + nX̂ = length(X̂) + for i in eachindex(g) + estim.i_g[i] || continue + if i ≤ nX̂ + j = i + g[i] = (estim.X̂min[j] - X̂[j]) + else + j = i - nX̂ + g[i] = (X̂[j] - estim.X̂max[j]) + end + end + if isa(g, Vector{Float64}) + println(g) + end + return g end \ No newline at end of file From bfdac979a23d3318059a7efef3b7e1818987797f Mon Sep 17 00:00:00 2001 From: franckgaga Date: Tue, 5 Dec 2023 18:10:51 -0500 Subject: [PATCH 13/20] debug MHE constraints --- src/controller/explicitmpc.jl | 2 +- src/controller/nonlinmpc.jl | 4 +- src/estimator/mhe.jl | 124 ++++++++++++++++------------------ src/predictive_control.jl | 3 + 4 files changed, 63 insertions(+), 70 deletions(-) diff --git a/src/controller/explicitmpc.jl b/src/controller/explicitmpc.jl index 696947be4..0c4138260 100644 --- a/src/controller/explicitmpc.jl +++ b/src/controller/explicitmpc.jl @@ -98,7 +98,7 @@ state estimator, a [`SteadyKalmanFilter`](@ref) with default arguments. - `Lwt=fill(0.0,model.nu)` : main diagonal of ``\mathbf{L}`` weight matrix (vector). - `M_Hp` / `N_Hc` / `L_Hp` : diagonal matrices ``\mathbf{M}_{H_p}, \mathbf{N}_{H_c}, \mathbf{L}_{H_p}``, for time-varying weights (generated from `Mwt/Nwt/Lwt` args if omitted). -- additionnal keyword arguments are passed to [`SteadyKalmanFilter`](@ref) constructor. +- additional keyword arguments are passed to [`SteadyKalmanFilter`](@ref) constructor. # Examples ```jldoctest diff --git a/src/controller/nonlinmpc.jl b/src/controller/nonlinmpc.jl index 7705edd06..800019c0e 100644 --- a/src/controller/nonlinmpc.jl +++ b/src/controller/nonlinmpc.jl @@ -134,7 +134,7 @@ This method uses the default state estimator : - `optim=JuMP.Model(Ipopt.Optimizer)` : nonlinear optimizer used in the predictive controller, provided as a [`JuMP.Model`](https://jump.dev/JuMP.jl/stable/api/JuMP/#JuMP.Model) (default to [`Ipopt.jl`](https://github.com/jump-dev/Ipopt.jl) optimizer). -- additionnal keyword arguments are passed to [`UnscentedKalmanFilter`](@ref) constructor +- additional keyword arguments are passed to [`UnscentedKalmanFilter`](@ref) constructor (or [`SteadyKalmanFilter`](@ref), for [`LinModel`](@ref)). # Examples @@ -365,7 +365,7 @@ end "Set the nonlinear constraints on the output predictions `Ŷ` and terminal states `x̂end`." function setnonlincon!(mpc::NonLinMPC, ::NonLinModel) optim = mpc.optim - ΔŨvar = mpc.optim[:ΔŨvar] + ΔŨvar = optim[:ΔŨvar] con = mpc.con map(con -> delete(optim, con), all_nonlinear_constraints(optim)) for i in findall(.!isinf.(con.Ymin)) diff --git a/src/estimator/mhe.jl b/src/estimator/mhe.jl index d0dc3cfda..2f9c99cb0 100644 --- a/src/estimator/mhe.jl +++ b/src/estimator/mhe.jl @@ -65,7 +65,8 @@ struct MovingHorizonEstimator{ invR̂_He = Hermitian(repeatdiag(inv(R̂), He), :L) P̂ = copy(P̂0) X̂min, X̂max = fill(-Inf, nx̂*(He+1)), fill(+Inf, nx̂*(He+1)) - X̂max = fill(100, nx̂*(He+1)) + X̂min = fill(-50, nx̂*(He+1)) + X̂max = fill(50, nx̂*(He+1)) i_X̂min, i_X̂max = .!isinf.(X̂min), .!isinf.(X̂max) i_g = [i_X̂min; i_X̂max] nvar = nx̂*(He + 1) @@ -124,7 +125,6 @@ function init_optimization!( ) where JNT<:Real He = estim.He nŶm, nX̂, ng = He*estim.nym, (He+1)*estim.nx̂, length(estim.i_g) - println(ng) # --- variables and linear constraints --- nvar = length(estim.W̃) set_silent(optim) @@ -164,13 +164,13 @@ function init_optimization!( end function gfunc_i(i, W̃tup::NTuple{N, JNT}) where N g = get_tmp(g_cache, W̃tup[1]) - if W̃tup != last_W̃tup_dual + if W̃tup != last_W̃tup_float Ŷm = get_tmp(Ŷm_cache, W̃tup[1]) X̂ = get_tmp(X̂_cache, W̃tup[1]) W̃ = collect(W̃tup) Ŷm, X̂ = predict!(Ŷm, X̂, estim, model, W̃) g = con_nonlinprog!(g, estim, model, X̂) - last_W̃tup_float = ΔŨtup + last_W̃tup_float = W̃tup end return g[i] end @@ -182,7 +182,7 @@ function init_optimization!( W̃ = collect(W̃tup) Ŷm, X̂ = predict!(Ŷm, X̂, estim, model, W̃) g = con_nonlinprog!(g, estim, model, X̂) - last_W̃tup_dual = ΔŨtup + last_W̃tup_dual = W̃tup end return g[i] end @@ -202,9 +202,26 @@ function init_optimization!( register(optim, sym, nvar, gfunc[i_end_X̂min+i], autodiff=true) end end + # TODO: moved this call to setconstraint! when the function will handle MHE + setnonlincon!(estim, estim.model) return nothing end +"Set the nonlinear constraints on the output predictions `Ŷ` and terminal states `x̂end`." +function setnonlincon!(estim::MovingHorizonEstimator, ::SimModel) + optim = estim.optim + W̃var = optim[:W̃var] + map(con -> delete(optim, con), all_nonlinear_constraints(optim)) + for i in findall(.!isinf.(estim.X̂min)) + f_sym = Symbol("g_X̂min_$(i)") + add_nonlinear_constraint(optim, :($(f_sym)($(W̃var...)) <= 0)) + end + for i in findall(.!isinf.(estim.X̂max)) + f_sym = Symbol("g_X̂max_$(i)") + add_nonlinear_constraint(optim, :($(f_sym)($(W̃var...)) <= 0)) + end + return nothing +end "Print the overall dimensions of the state estimator `estim` with left padding `n`." function print_estim_dim(io::IO, estim::MovingHorizonEstimator, n) @@ -219,42 +236,6 @@ function print_estim_dim(io::IO, estim::MovingHorizonEstimator, n) print(io, "$(lpad(nd, n)) measured disturbances d") end - -""" - obj_nonlinprog(estim::MovingHorizonEstimator, model::SimModel, ΔŨ::Vector{Real}) - -Objective function for [`MovingHorizonEstimator`](@ref). - -The function `dot(x, A, x)` is a performant way of calculating `x'*A*x`. -""" -function obj_nonlinprog( - estim::MovingHorizonEstimator, ::SimModel, Ŷm, W̃::Vector{T} -) where {T<:Real} - Nk = estim.Nk[] - nYm, nŴ, nx̂, invP̄ = Nk*estim.nym, Nk*estim.nx̂, estim.nx̂, estim.invP̄ - invQ̂_Nk, invR̂_Nk = @views estim.invQ̂_He[1:nŴ, 1:nŴ], estim.invR̂_He[1:nYm, 1:nYm] - x̄0 = @views W̃[1:nx̂] - estim.x̂0_past # W̃ = [x̂(k-Nk|k); Ŵ] - V̂ = @views estim.Ym[1:nYm] - Ŷm[1:nYm] - Ŵ = @views W̃[nx̂+1:nx̂+nŴ] - return dot(x̄0, invP̄, x̄0) + dot(Ŵ, invQ̂_Nk, Ŵ) + dot(V̂, invR̂_Nk, V̂) -end - -function predict!( - Ŷm, X̂, estim::MovingHorizonEstimator, model::SimModel, W̃::Vector{T} -) where {T<:Real} - nu, nd, nx̂, nym, Nk = model.nu, model.nd, estim.nx̂, estim.nym, estim.Nk[] - X̂[1:nx̂] = W̃[1:nx̂] # W̃ = [x̂(k-Nk|k); Ŵ] - for j=1:Nk - u = @views estim.U[(1 + nu*(j-1)):(nu*j)] - d = @views estim.D[(1 + nd*(j-1)):(nd*j)] - ŵ = @views W̃[(1 + nx̂*j):(nx̂*(j+1))] - x̂ = @views X̂[(1 + nx̂*(j-1)):(nx̂*j)] - Ŷm[(1 + nym*(j-1)):(nym*j)] = ĥ(estim, model, x̂, d)[estim.i_ym] - X̂[(1 + nx̂*j):(nx̂*(j+1))] = f̂(estim, model, x̂, u, d) + ŵ - end - return Ŷm, X̂ -end - "Reset `estim.P̂`, `estim.invP̄` and the time windows for the moving horizon estimator." function init_estimate_cov!(estim::MovingHorizonEstimator, _ , _ , _ ) estim.P̂.data[:] = estim.P̂0 # .data is necessary for Hermitians @@ -348,31 +329,40 @@ function update_estimate!(estim::MovingHorizonEstimator{NT}, u, ym, d) where NT< return nothing end -"Set the nonlinear constraints on the output predictions `Ŷ` and terminal states `x̂end`." -function setnonlincon!(mpc::NonLinMPC, ::NonLinModel) - optim = mpc.optim - ΔŨvar = mpc.optim[:ΔŨvar] - con = mpc.con - map(con -> delete(optim, con), all_nonlinear_constraints(optim)) - for i in findall(.!isinf.(con.Ymin)) - f_sym = Symbol("g_Ymin_$(i)") - add_nonlinear_constraint(optim, :($(f_sym)($(ΔŨvar...)) <= 0)) - end - for i in findall(.!isinf.(con.Ymax)) - f_sym = Symbol("g_Ymax_$(i)") - add_nonlinear_constraint(optim, :($(f_sym)($(ΔŨvar...)) <= 0)) - end - for i in findall(.!isinf.(con.x̂min)) - f_sym = Symbol("g_x̂min_$(i)") - add_nonlinear_constraint(optim, :($(f_sym)($(ΔŨvar...)) <= 0)) - end - for i in findall(.!isinf.(con.x̂max)) - f_sym = Symbol("g_x̂max_$(i)") - add_nonlinear_constraint(optim, :($(f_sym)($(ΔŨvar...)) <= 0)) - end - return nothing +""" + obj_nonlinprog(estim::MovingHorizonEstimator, model::SimModel, ΔŨ::Vector{Real}) + +Objective function for [`MovingHorizonEstimator`](@ref). + +The function `dot(x, A, x)` is a performant way of calculating `x'*A*x`. +""" +function obj_nonlinprog( + estim::MovingHorizonEstimator, ::SimModel, Ŷm, W̃::Vector{T} +) where {T<:Real} + Nk = estim.Nk[] + nYm, nŴ, nx̂, invP̄ = Nk*estim.nym, Nk*estim.nx̂, estim.nx̂, estim.invP̄ + invQ̂_Nk, invR̂_Nk = @views estim.invQ̂_He[1:nŴ, 1:nŴ], estim.invR̂_He[1:nYm, 1:nYm] + x̄0 = @views W̃[1:nx̂] - estim.x̂0_past # W̃ = [x̂(k-Nk|k); Ŵ] + V̂ = @views estim.Ym[1:nYm] - Ŷm[1:nYm] + Ŵ = @views W̃[nx̂+1:nx̂+nŴ] + return dot(x̄0, invP̄, x̄0) + dot(Ŵ, invQ̂_Nk, Ŵ) + dot(V̂, invR̂_Nk, V̂) end +function predict!( + Ŷm, X̂, estim::MovingHorizonEstimator, model::SimModel, W̃::Vector{T} +) where {T<:Real} + nu, nd, nx̂, nym, Nk = model.nu, model.nd, estim.nx̂, estim.nym, estim.Nk[] + X̂[1:nx̂] = W̃[1:nx̂] # W̃ = [x̂(k-Nk|k); Ŵ] + for j=1:Nk + u = @views estim.U[(1 + nu*(j-1)):(nu*j)] + d = @views estim.D[(1 + nd*(j-1)):(nd*j)] + ŵ = @views W̃[(1 + nx̂*j):(nx̂*(j+1))] + x̂ = @views X̂[(1 + nx̂*(j-1)):(nx̂*j)] + Ŷm[(1 + nym*(j-1)):(nym*j)] = ĥ(estim, model, x̂, d)[estim.i_ym] + X̂[(1 + nx̂*j):(nx̂*(j+1))] = f̂(estim, model, x̂, u, d) + ŵ + end + return Ŷm, X̂ +end function con_nonlinprog!(g, estim::MovingHorizonEstimator, ::SimModel, X̂) nX̂ = length(X̂) @@ -386,8 +376,8 @@ function con_nonlinprog!(g, estim::MovingHorizonEstimator, ::SimModel, X̂) g[i] = (X̂[j] - estim.X̂max[j]) end end - if isa(g, Vector{Float64}) - println(g) - end + #if isa(g, Vector{Float64}) + # println(g) + #end return g end \ No newline at end of file diff --git a/src/predictive_control.jl b/src/predictive_control.jl index 4ba885eb4..a9628b9a1 100644 --- a/src/predictive_control.jl +++ b/src/predictive_control.jl @@ -170,11 +170,13 @@ function setconstraint!( C_umax = nothing, C_umin = nothing, C_Δumax = nothing, C_Δumin = nothing, C_ymax = nothing, C_ymin = nothing, + # TODO: # ------------ will be deleted in the future --------------- ŷmin = nothing, ŷmax = nothing, c_ŷmin = nothing, c_ŷmax = nothing, # ---------------------------------------------------------- ) + # TODO: # ----- these 4 `if`s will be deleted in the future -------- if !isnothing(ŷmin) Base.depwarn("keyword arg ŷmin is deprecated, use ymin instead", :setconstraint!) @@ -509,6 +511,7 @@ Throw an error if `isnothing(Hp)` when model is not a [`LinModel`](@ref). """ function default_Hp(::SimModel, Hp) if isnothing(Hp) + # TODO: # ------------ will be deleted in the future ------------------------------------ Base.depwarn("Hp=nothing is deprecated for NonLinModel, explicitly specify an "* "integer value", :NonLinMPC) From a95d91828e26393d4eef0a2b25da231177a9322a Mon Sep 17 00:00:00 2001 From: franckgaga Date: Wed, 6 Dec 2023 12:01:22 -0500 Subject: [PATCH 14/20] added : `setconstraint!` for MHE --- docs/src/public/generic_func.md | 6 ++ docs/src/public/predictive_control.md | 6 -- docs/src/public/state_estim.md | 9 +- src/estimator/mhe.jl | 122 +++++++++++++++++++++++--- 4 files changed, 123 insertions(+), 20 deletions(-) diff --git a/docs/src/public/generic_func.md b/docs/src/public/generic_func.md index a4be3d12a..626caa72e 100644 --- a/docs/src/public/generic_func.md +++ b/docs/src/public/generic_func.md @@ -7,6 +7,12 @@ Pages = ["generic_func.md"] This page contains the documentation of functions that are generic to [`SimModel`](@ref), [`StateEstimator`](@ref) and [`PredictiveController`](@ref) types. +## Set Constraint + +```@docs +setconstraint! +``` + ## Evaluate Output y ```@docs diff --git a/docs/src/public/predictive_control.md b/docs/src/public/predictive_control.md index 2092fa5fc..1b98ab5e2 100644 --- a/docs/src/public/predictive_control.md +++ b/docs/src/public/predictive_control.md @@ -78,12 +78,6 @@ ExplicitMPC NonLinMPC ``` -## Set Constraint - -```@docs -setconstraint! -``` - ## Move Manipulated Input u ```@docs diff --git a/docs/src/public/state_estim.md b/docs/src/public/state_estim.md index 7a2d899e7..d22ce1771 100644 --- a/docs/src/public/state_estim.md +++ b/docs/src/public/state_estim.md @@ -20,8 +20,7 @@ error with closed-loop control (offset-free tracking). The estimators are all implemented in the predictor form (a.k.a. observer form), that is, they all estimates at each discrete time ``k`` the states of the next period ``\mathbf{x̂}_k(k+1)``[^1]. In contrast, the filter form that estimates ``\mathbf{x̂}_k(k)`` -is sometimes slightly more accurate. The documentation of the function [`update_estimates!`](@ref) -details how the estimators predicts them. +is sometimes slightly more accurate. [^1]: also denoted ``\mathbf{x̂}_{k+1|k}`` [elsewhere](https://en.wikipedia.org/wiki/Kalman_filter). @@ -72,6 +71,12 @@ UnscentedKalmanFilter ExtendedKalmanFilter ``` +## MovingHorizonEstimator + +```@docs +MovingHorizonEstimator +``` + ## InternalModel ```@docs diff --git a/src/estimator/mhe.jl b/src/estimator/mhe.jl index 2f9c99cb0..d917ebd8b 100644 --- a/src/estimator/mhe.jl +++ b/src/estimator/mhe.jl @@ -48,7 +48,7 @@ struct MovingHorizonEstimator{ function MovingHorizonEstimator{NT, SM, JM}( model::SM, He, i_ym, nint_u, nint_ym, P̂0, Q̂, R̂, optim::JM ) where {NT<:Real, SM<:SimModel{NT}, JM<:JuMP.GenericModel} - nu, nd, nx, ny = model.nu, model.nd, model.nx, model.ny + nu, nd = model.nu, model.nd He < 1 && throw(ArgumentError("Estimation horizon He should be ≥ 1")) nym, nyu = validate_ym(model, i_ym) As, Cs_u, Cs_y, nint_u, nint_ym = init_estimstoch(model, i_ym, nint_u, nint_ym) @@ -64,15 +64,14 @@ struct MovingHorizonEstimator{ invQ̂_He = Hermitian(repeatdiag(inv(Q̂), He), :L) invR̂_He = Hermitian(repeatdiag(inv(R̂), He), :L) P̂ = copy(P̂0) - X̂min, X̂max = fill(-Inf, nx̂*(He+1)), fill(+Inf, nx̂*(He+1)) - X̂min = fill(-50, nx̂*(He+1)) - X̂max = fill(50, nx̂*(He+1)) - i_X̂min, i_X̂max = .!isinf.(X̂min), .!isinf.(X̂max) + nvar, nX̂ = nx̂*(He + 1), nx̂*(He + 1) + X̂min , X̂max = fill(-Inf, nX̂), fill(+Inf, nX̂) + i_X̂min, i_X̂max = .!isinf.(X̂min) , .!isinf.(X̂max) i_g = [i_X̂min; i_X̂max] - 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̂) + W̃ = zeros(NT, nvar) + X̂, Ym = zeros(NT, nx̂*He), zeros(NT, nym*He) + U, D, Ŵ = zeros(NT, nu*He), zeros(NT, nd*He), zeros(NT, nx̂*He) + x̂0_past = zeros(NT, nx̂) Nk = [1] estim = new{NT, SM, JM}( model, optim, W̃, @@ -90,6 +89,45 @@ struct MovingHorizonEstimator{ end end + +@doc raw""" + MovingHorizonEstimator(model::SimModel; ) + +Construct a moving horizon estimator based on `model` ([`LinModel`](@ref) or [`NonLinModel`](@ref)). + +It can handle constraints on the estimates. Additionally, `model` is not linearized like +the [`ExtendedKalmanFilter`](@ref) and the probability distribution is not approximated +like the [`UnscentedKalmanFilter`](@ref). The computational costs are drastically higher, +however, since it minimizes the following objective function at each discrete time ``k``: +```math +\min_{\mathbf{x̂}_k(k-N_k+1), Ŵ} \mathbf{x̄}' \mathbf{P̄}^{-1} \mathbf{x̄} + + \mathbf{Ŵ}' \mathbf{Q̂}_{N_k}^{-1} \mathbf{Ŵ} + + \mathbf{V̂}' \mathbf{R̂}_{N_k}^{-1} \mathbf{V̂} +``` +in which the covariance matrices are repeated ``N_k`` times: +```math +\begin{aligned} + \mathbf{Q̂}_{N_k} &= \text{diag}\mathbf{(Q̂,Q̂,...,Q̂)} \\ + \mathbf{R̂}_{N_k} &= \text{diag}\mathbf{(R̂,R̂,...,R̂)} +\end{aligned} +```` +The window length at time ``k`` is truncated at the estimation horizon ``H_e``: +```math +N_k = +\begin{cases} + k + 1 & k < H_e \\ + H_e & k ≥ H_e +\end{cases} +``` +The vectors ``\mathbf{Ŵ}`` and ``\mathbf{V̂}`` incorporate the estimated process noise +and sensor noise over the time window ``N_k``. The arrival costs are defined by: +```math +\begin{aligned} + \mathbf{x̄}(k-N_k+1) &= \mathbf{x̂}_k(k-N_k+1) - \mathbf{x̂}_{k-N_k}(k-N_k+1) \\ + \mathbf{P̄} &= \mathbf{P̂}_k(k-N_k+1) +\end{aligned} +``` +""" function MovingHorizonEstimator( model::SM; He::Int=nothing, @@ -207,6 +245,65 @@ function init_optimization!( return nothing end +@doc raw""" + setconstraint!(estim::MovingHorizonEstimator; ) -> estim + +Set the constraint parameters of `estim` moving horizon estimator. + +The moving horizon estimator constraints are defined by: +```math +\mathbf{x̂_{min}} ≤ \mathbf{x̂}_k(k-j+1) ≤ \mathbf{x̂_{max}} \qquad j = 0, 1, ... , N_k \\ +``` +in which N_k. +Note that state constraints are applied on the augmented state vector ``\mathbf{x̂}`` (see +[`SteadyKalmanFilter`](@ref) extended help for details on augmentation). + +# Arguments +!!! info + The default constraints are mentioned here for clarity but omitting a keyword argument + will not re-assign to its default value (defaults are set at construction only). + +- `estim::MovingHorizonEstimator` : moving horizon estimator to set constraints. +- `x̂min = fill(-Inf,nx̂)` : augmented state vector lower bounds ``\mathbf{x̂_{min}}``. +- `x̂max = fill(+Inf,nx̂)` : augmented state vector upper bounds ``\mathbf{x̂_{max}}``. + +# Examples +```jldoctest +julia> a = 1; +``` +""" +function setconstraint!( + estim::MovingHorizonEstimator; + x̂min = nothing, x̂max = nothing, + X̂min = nothing, X̂max = nothing, +) + model, optim = estim.model, estim.optim + nx̂, He = estim.nx̂, estim.He + nX̂ = nx̂*(He+1) + notSolvedYet = (termination_status(optim) == OPTIMIZE_NOT_CALLED) + isnothing(X̂min) && !isnothing(x̂min) && (X̂min = repeat(x̂min, He+1)) + isnothing(X̂max) && !isnothing(x̂max) && (X̂max = repeat(x̂max, He+1)) + if !isnothing(X̂min) + size(X̂min) == (nX̂,) || throw(ArgumentError("X̂min size must be $((nX̂,))")) + estim.X̂min[:] = X̂min + end + if !isnothing(X̂max) + size(X̂max) == (nX̂,) || throw(ArgumentError("X̂max size must be $((nX̂,))")) + estim.X̂max[:] = X̂max + end + i_X̂min, i_X̂max = .!isinf.(estim.X̂min) , .!isinf.(estim.X̂max) + i_g = [i_X̂min; i_X̂max] + if notSolvedYet + estim.i_g[:] = i_g + setnonlincon!(estim, model) + else + if i_g ≠ estim.i_g + error("Cannot modify ±Inf constraints after calling updatestate!") + end + end + return estim +end + "Set the nonlinear constraints on the output predictions `Ŷ` and terminal states `x̂end`." function setnonlincon!(estim::MovingHorizonEstimator, ::SimModel) optim = estim.optim @@ -296,6 +393,7 @@ function update_estimate!(estim::MovingHorizonEstimator{NT}, u, ym, d) where NT< # initial W̃0 with Ŵ=0 if objective or constraint function not finite : isfinite(J0) || (W̃0 = [estim.x̂0_past; zeros(NT, nŴ)]) set_start_value.(W̃var, W̃0) + # at start, when time windows are not filled, some decision variables are fixed at 0: unfix.(W̃var[is_fixed.(W̃var)]) fix.(W̃var[(nx̂*(Nk+1)+1):end], 0.0) try @@ -330,7 +428,7 @@ function update_estimate!(estim::MovingHorizonEstimator{NT}, u, ym, d) where NT< end """ - obj_nonlinprog(estim::MovingHorizonEstimator, model::SimModel, ΔŨ::Vector{Real}) + obj_nonlinprog(estim::MovingHorizonEstimator, model::SimModel, W̃) Objective function for [`MovingHorizonEstimator`](@ref). @@ -370,10 +468,10 @@ function con_nonlinprog!(g, estim::MovingHorizonEstimator, ::SimModel, X̂) estim.i_g[i] || continue if i ≤ nX̂ j = i - g[i] = (estim.X̂min[j] - X̂[j]) + g[i] = estim.X̂min[j] - X̂[j] else j = i - nX̂ - g[i] = (X̂[j] - estim.X̂max[j]) + g[i] = X̂[j] - estim.X̂max[j] end end #if isa(g, Vector{Float64}) From d97385b685f126ada92f266c72ed52b118e092e7 Mon Sep 17 00:00:00 2001 From: franckgaga Date: Wed, 6 Dec 2023 16:54:55 -0500 Subject: [PATCH 15/20] added: update MHE arrival covariance with EKF --- docs/src/public/generic_func.md | 12 ++-- src/estimator/kalman.jl | 15 +++-- src/estimator/mhe.jl | 102 ++++++++++++++++++-------------- src/predictive_control.jl | 3 +- 4 files changed, 73 insertions(+), 59 deletions(-) diff --git a/docs/src/public/generic_func.md b/docs/src/public/generic_func.md index 626caa72e..af5c2b1e3 100644 --- a/docs/src/public/generic_func.md +++ b/docs/src/public/generic_func.md @@ -7,12 +7,6 @@ Pages = ["generic_func.md"] This page contains the documentation of functions that are generic to [`SimModel`](@ref), [`StateEstimator`](@ref) and [`PredictiveController`](@ref) types. -## Set Constraint - -```@docs -setconstraint! -``` - ## Evaluate Output y ```@docs @@ -37,6 +31,12 @@ initstate! setstate! ``` +## Set Constraint + +```@docs +setconstraint! +``` + ## Quick Simulation ### Simulate diff --git a/src/estimator/kalman.jl b/src/estimator/kalman.jl index eea0e55dd..1b2be8c69 100644 --- a/src/estimator/kalman.jl +++ b/src/estimator/kalman.jl @@ -762,7 +762,7 @@ function validate_kfcov(nym, nx̂, Q̂, R̂, P̂0=nothing) end """ - update_estimate_kf!(estim, Â, Ĉm, u, ym, d) -> x̂, P̂ + update_estimate_kf!(estim, Â, Ĉm, u, ym, d; updatestate=true) Update time-varying/extended Kalman Filter estimates with augmented `Â` and `Ĉm` matrices. @@ -770,15 +770,18 @@ Allows code reuse for [`KalmanFilter`](@ref) and [`ExtendedKalmanFilterKalmanFil They update the state `x̂` and covariance `P̂` with the same equations. The extended filter substitutes the augmented model matrices with its Jacobians (`Â = F̂` and `Ĉm = Ĥm`). The implementation uses in-place operations and explicit factorization to reduce -allocations. See e.g. [`KalmanFilter`](@ref) docstring for the equations. +allocations. See e.g. [`KalmanFilter`](@ref) docstring for the equations. If `updatestate` +is `false`, only the covariance `P̂` is updated. """ -function update_estimate_kf!(estim, Â, Ĉm, u, ym, d) +function update_estimate_kf!(estim, Â, Ĉm, u, ym, d; updatestate=true) x̂, P̂, Q̂, R̂, K̂, M̂ = estim.x̂, estim.P̂, estim.Q̂, estim.R̂, estim.K̂, estim.M̂ mul!(M̂, P̂, Ĉm') rdiv!(M̂, cholesky!(Hermitian(Ĉm * P̂ * Ĉm' + R̂))) - mul!(K̂, Â, M̂) - ŷm = @views ĥ(estim, estim.model, x̂, d)[estim.i_ym] - x̂[:] = f̂(estim, estim.model, x̂, u, d) + K̂ * (ym - ŷm) + if updatestate + mul!(K̂, Â, M̂) + ŷm = @views ĥ(estim, estim.model, x̂, d)[estim.i_ym] + x̂[:] = f̂(estim, estim.model, x̂, u, d) + K̂ * (ym - ŷm) + end P̂.data[:] = Â * (P̂ - M̂ * Ĉm * P̂) * Â' + Q̂ # .data is necessary for Hermitians return nothing end diff --git a/src/estimator/mhe.jl b/src/estimator/mhe.jl index d917ebd8b..091d1f944 100644 --- a/src/estimator/mhe.jl +++ b/src/estimator/mhe.jl @@ -29,12 +29,14 @@ struct MovingHorizonEstimator{ Ĉ ::Matrix{NT} B̂d::Matrix{NT} D̂d::Matrix{NT} - P̂0 ::Hermitian{NT, Matrix{NT}} + P̂0::Hermitian{NT, Matrix{NT}} Q̂::Hermitian{NT, Matrix{NT}} R̂::Hermitian{NT, Matrix{NT}} invP̄::Hermitian{NT, Matrix{NT}} invQ̂_He::Hermitian{NT, Matrix{NT}} invR̂_He::Hermitian{NT, Matrix{NT}} + K̂::Matrix{NT} + M̂::Matrix{NT} X̂min::Vector{NT} X̂max::Vector{NT} i_g::BitVector @@ -64,6 +66,7 @@ struct MovingHorizonEstimator{ invQ̂_He = Hermitian(repeatdiag(inv(Q̂), He), :L) invR̂_He = Hermitian(repeatdiag(inv(R̂), He), :L) P̂ = copy(P̂0) + K̂, M̂ = zeros(NT, nx̂, nym), zeros(NT, nx̂, nym) nvar, nX̂ = nx̂*(He + 1), nx̂*(He + 1) X̂min , X̂max = fill(-Inf, nX̂), fill(+Inf, nX̂) i_X̂min, i_X̂max = .!isinf.(X̂min) , .!isinf.(X̂max) @@ -80,6 +83,7 @@ struct MovingHorizonEstimator{ As, Cs_u, Cs_y, nint_u, nint_ym, Â, B̂u, Ĉ, B̂d, D̂d, P̂0, Q̂, R̂, invP̄, invQ̂_He, invR̂_He, + K̂, M̂, X̂min, X̂max, i_g, X̂, Ym, U, D, Ŵ, x̂0_past, Nk @@ -95,42 +99,42 @@ end Construct a moving horizon estimator based on `model` ([`LinModel`](@ref) or [`NonLinModel`](@ref)). -It can handle constraints on the estimates. Additionally, `model` is not linearized like -the [`ExtendedKalmanFilter`](@ref) and the probability distribution is not approximated -like the [`UnscentedKalmanFilter`](@ref). The computational costs are drastically higher, -however, since it minimizes the following objective function at each discrete time ``k``: +This estimator can handle constraints on the estimates, see [`setconstraint!`](@ref). +Additionally, `model` is not linearized like the [`ExtendedKalmanFilter`](@ref), and the +probability distribution is not approximated like the [`UnscentedKalmanFilter`](@ref). The +computational costs are drastically higher, however, since it minimizes the following +nonlinear objective function at each discrete time ``k``: ```math -\min_{\mathbf{x̂}_k(k-N_k+1), Ŵ} \mathbf{x̄}' \mathbf{P̄}^{-1} \mathbf{x̄} - + \mathbf{Ŵ}' \mathbf{Q̂}_{N_k}^{-1} \mathbf{Ŵ} - + \mathbf{V̂}' \mathbf{R̂}_{N_k}^{-1} \mathbf{V̂} +\min_{\mathbf{x̂}_k(k-N_k+1), \mathbf{Ŵ}} \mathbf{x̄}' \mathbf{P̄}^{-1} \mathbf{x̄} + + \mathbf{Ŵ}' \mathbf{Q̂}_{N_k}^{-1} \mathbf{Ŵ} + + \mathbf{V̂}' \mathbf{R̂}_{N_k}^{-1} \mathbf{V̂} ``` -in which the covariance matrices are repeated ``N_k`` times: +in which the arrival costs are defined by: +```math +\begin{aligned} + \mathbf{x̄} &= \mathbf{x̂}_k(k-N_k+1) - \mathbf{x̂}_{k-N_k}(k-N_k+1) \\ + \mathbf{P̄} &= \mathbf{P̂}_k(k-N_k+1) +\end{aligned} +``` +and the covariances are repeated ``N_k`` times: ```math \begin{aligned} \mathbf{Q̂}_{N_k} &= \text{diag}\mathbf{(Q̂,Q̂,...,Q̂)} \\ \mathbf{R̂}_{N_k} &= \text{diag}\mathbf{(R̂,R̂,...,R̂)} \end{aligned} -```` -The window length at time ``k`` is truncated at the estimation horizon ``H_e``: -```math -N_k = -\begin{cases} - k + 1 & k < H_e \\ - H_e & k ≥ H_e -\end{cases} ``` The vectors ``\mathbf{Ŵ}`` and ``\mathbf{V̂}`` incorporate the estimated process noise -and sensor noise over the time window ``N_k``. The arrival costs are defined by: +``\mathbf{ŵ}(k-j)`` and sensor noise ``\mathbf{v̂}(k-j)`` from ``j=0`` to ``N_k-1``. The +estimation horizon ``H_e`` limits the window length: ```math -\begin{aligned} - \mathbf{x̄}(k-N_k+1) &= \mathbf{x̂}_k(k-N_k+1) - \mathbf{x̂}_{k-N_k}(k-N_k+1) \\ - \mathbf{P̄} &= \mathbf{P̂}_k(k-N_k+1) -\end{aligned} +N_k = \begin{cases} + k + 1 & k < H_e \\ + H_e & k ≥ H_e \end{cases} ``` """ function MovingHorizonEstimator( model::SM; - He::Int=nothing, + He::Union{Int, Nothing}=nothing, i_ym::IntRangeOrVector = 1:model.ny, σP0::Vector = fill(1/model.nx, model.nx), σQ::Vector = fill(1/model.nx, model.nx), @@ -147,6 +151,7 @@ function MovingHorizonEstimator( P̂0 = Diagonal{NT}([σP0; σP0int_u; σP0int_ym].^2) Q̂ = Diagonal{NT}([σQ; σQint_u; σQint_ym].^2) R̂ = Diagonal{NT}(σR.^2) + isnothing(He) && throw(ArgumentError("Estimation horizon He must be explicitly specified")) return MovingHorizonEstimator{NT, SM, JM}( model, He, i_ym, nint_u, nint_ym, P̂0, Q̂, R̂, optim ) @@ -248,15 +253,14 @@ end @doc raw""" setconstraint!(estim::MovingHorizonEstimator; ) -> estim -Set the constraint parameters of `estim` moving horizon estimator. +Set the constraint parameters of `estim` [`MovingHorizonEstimator`](@ref). -The moving horizon estimator constraints are defined by: +The constraints of the moving horizon estimator are: ```math \mathbf{x̂_{min}} ≤ \mathbf{x̂}_k(k-j+1) ≤ \mathbf{x̂_{max}} \qquad j = 0, 1, ... , N_k \\ ``` -in which N_k. Note that state constraints are applied on the augmented state vector ``\mathbf{x̂}`` (see -[`SteadyKalmanFilter`](@ref) extended help for details on augmentation). +the extended help of [`SteadyKalmanFilter`](@ref) for details on augmentation). # Arguments !!! info @@ -266,10 +270,21 @@ Note that state constraints are applied on the augmented state vector ``\mathbf{ - `estim::MovingHorizonEstimator` : moving horizon estimator to set constraints. - `x̂min = fill(-Inf,nx̂)` : augmented state vector lower bounds ``\mathbf{x̂_{min}}``. - `x̂max = fill(+Inf,nx̂)` : augmented state vector upper bounds ``\mathbf{x̂_{max}}``. +- all the keyword arguments above but with a capital letter, e.g. `X̂max` or `X̂min` : for + time-varying constraints (see Extended Help). # Examples ```jldoctest -julia> a = 1; +julia> estim = MovingHorizonEstimator(LinModel(ss(0.5,1,1,0,1)), He=3); + +julia> estim = setconstraint!(estim, x̂min=[-50, -50], x̂max=[50, 50]) +MovingHorizonEstimator estimator with a sample time Ts = 1.0 s, LinModel and: + 2 estimation steps He + 1 manipulated inputs u (0 integrating states) + 1 states x̂ + 1 measured outputs ym (1 integrating states) + 0 unmeasured outputs yu + 0 measured disturbances d ``` """ function setconstraint!( @@ -335,7 +350,6 @@ end "Reset `estim.P̂`, `estim.invP̄` and the time windows for the moving horizon estimator." function init_estimate_cov!(estim::MovingHorizonEstimator, _ , _ , _ ) - estim.P̂.data[:] = estim.P̂0 # .data is necessary for Hermitians estim.invP̄.data[:] = Hermitian(inv(estim.P̂0), :L) estim.x̂0_past .= 0 estim.W̃ .= 0 @@ -349,23 +363,14 @@ function init_estimate_cov!(estim::MovingHorizonEstimator, _ , _ , _ ) end @doc raw""" - update_estimate!(estim::UnscentedKalmanFilter, u, ym, d) + update_estimate!(estim::MovingHorizonEstimator, u, ym, d) -Update [`UnscentedKalmanFilter`](@ref) state `estim.x̂` and covariance estimate `estim.P̂`. - -A ref[^4]: +Update [`MovingHorizonEstimator`](@ref) state `estim.x̂`. -```math -\begin{aligned} - \mathbf{Ŷ^m}(k) &= \bigg[\begin{matrix} \mathbf{ĥ^m}\Big( \mathbf{X̂}_{k-1}^{1}(k) \Big) & \mathbf{ĥ^m}\Big( \mathbf{X̂}_{k-1}^{2}(k) \Big) & \cdots & \mathbf{ĥ^m}\Big( \mathbf{X̂}_{k-1}^{n_σ}(k) \Big) \end{matrix}\bigg] \\ - \mathbf{ŷ^m}(k) &= \mathbf{Ŷ^m}(k) \mathbf{m̂} -\end{aligned} -``` - -[^4]: TODO +TBW """ function update_estimate!(estim::MovingHorizonEstimator{NT}, u, ym, d) where NT<:Real - model, optim, x̂, P̂ = estim.model, estim.optim, estim.x̂, estim.P̂ + model, optim, x̂ = estim.model, estim.optim, estim.x̂ nx̂, nym, nu, nd, nŵ = estim.nx̂, estim.nym, model.nu, model.nd, estim.nx̂ Nk, He = estim.Nk[], estim.He nŴ, nYm, nX̂ = nx̂*Nk, nym*Nk, nx̂*(Nk+1) @@ -384,6 +389,16 @@ function update_estimate!(estim::MovingHorizonEstimator{NT}, u, ym, d) where NT< estim.D[:] = [estim.D[nd+1:end] ; d] estim.Ŵ[:] = [estim.Ŵ[nŵ+1:end] ; ŵ] end + if Nk ≥ He + # update the arrival covariance with an Extended Kalman Filter: + # TODO: also support UnscentedKalmanFilter, and KalmanFilter for LinModel + F̂ = ForwardDiff.jacobian(x̂ -> f̂(estim, estim.model, x̂, u, d), estim.x̂) + Ĥ = ForwardDiff.jacobian(x̂ -> ĥ(estim, estim.model, x̂, d), estim.x̂) + Ĥm = Ĥ[estim.i_ym, :] + update_estimate_kf!(estim, F̂, Ĥm, u, ym, d, updatestate=false) + P̄ = estim.P̂ + estim.invP̄.data[:] = Hermitian(inv(P̄), :L) # .data is necessary for Hermitian + end Ŷm = Vector{NT}(undef, nYm) X̂ = Vector{NT}(undef, nX̂) estim.x̂0_past[:] = estim.X̂[1:nx̂] @@ -474,8 +489,5 @@ function con_nonlinprog!(g, estim::MovingHorizonEstimator, ::SimModel, X̂) g[i] = X̂[j] - estim.X̂max[j] end end - #if isa(g, Vector{Float64}) - # println(g) - #end return g end \ No newline at end of file diff --git a/src/predictive_control.jl b/src/predictive_control.jl index a9628b9a1..f9ffc49b2 100644 --- a/src/predictive_control.jl +++ b/src/predictive_control.jl @@ -84,8 +84,7 @@ constraints are all soft by default. See Extended Help for time-varying constrai # Arguments !!! info - The default constraints are mentioned here for clarity but omitting a keyword argument - will not re-assign to its default value (defaults are set at construction only). + Same as above for the keyword arguments and default constraints. - `mpc::PredictiveController` : predictive controller to set constraints. - `umin = fill(-Inf,nu)` : manipulated input lower bounds ``\mathbf{u_{min}}``. From bb51f01fb0ba5b722d24abd3a92fe97fd3090ce0 Mon Sep 17 00:00:00 2001 From: franckgaga Date: Wed, 6 Dec 2023 17:06:19 -0500 Subject: [PATCH 16/20] =?UTF-8?q?modif:=20intial=20covariance=20`P=CC=820`?= =?UTF-8?q?=20default=20to=20`Q=CC=82`=20arg?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- src/estimator/kalman.jl | 42 ++++++++++++++++++++--------------------- src/estimator/mhe.jl | 14 +++++++------- 2 files changed, 28 insertions(+), 28 deletions(-) diff --git a/src/estimator/kalman.jl b/src/estimator/kalman.jl index 1b2be8c69..89ca0b1fd 100644 --- a/src/estimator/kalman.jl +++ b/src/estimator/kalman.jl @@ -253,13 +253,13 @@ its initial value with ``\mathbf{P̂}_{-1}(0) = # Arguments - `model::LinModel` : (deterministic) model for the estimations. -- `σ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). -- `σ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). - `` of [`SteadyKalmanFilter`](@ref) constructor. +- `σP0=σQ` : main diagonal of the initial estimate covariance ``\mathbf{P}(0)``, specified + as a standard deviation vector. +- `σP0int_u=σQint_u` : same than `σP0` but for the unmeasured disturbances at manipulated + inputs ``\mathbf{P_{int_u}}(0)`` (composed of integrators). +- `σP0int_ym=σQint_ym` : same than `σP0` but for the unmeasured disturbances at measured + outputs ``\mathbf{P_{int_{ym}}}(0)`` (composed of integrators). # Examples ```jldoctest @@ -277,15 +277,15 @@ KalmanFilter estimator with a sample time Ts = 0.5 s, LinModel and: function KalmanFilter( model::SM; i_ym::IntRangeOrVector = 1:model.ny, - σP0::Vector = fill(1/model.nx, model.nx), - σQ ::Vector = fill(1/model.nx, model.nx), - σR ::Vector = fill(1, length(i_ym)), + σQ ::Vector = fill(1/model.nx, model.nx), + σR ::Vector = fill(1, length(i_ym)), + σP0::Vector = σQ, nint_u ::IntVectorOrInt = 0, σQint_u ::Vector = fill(1, max(sum(nint_u), 0)), - σP0int_u ::Vector = fill(1, max(sum(nint_u), 0)), + σP0int_u ::Vector = σQint_u, nint_ym ::IntVectorOrInt = default_nint(model, i_ym, nint_u), σQint_ym ::Vector = fill(1, max(sum(nint_ym), 0)), - σP0int_ym::Vector = fill(1, max(sum(nint_ym), 0)) + σP0int_ym::Vector = σQint_ym, ) where {NT<:Real, SM<:LinModel{NT}} # estimated covariances matrices (variance = σ²) : P̂0 = Diagonal{NT}([σP0; σP0int_u; σP0int_ym].^2) @@ -443,15 +443,15 @@ responsibility to ensure that the augmented model is still observable. function UnscentedKalmanFilter( model::SM; i_ym::IntRangeOrVector = 1:model.ny, - σP0::Vector = fill(1/model.nx, model.nx), - σQ::Vector = fill(1/model.nx, model.nx), - σR::Vector = fill(1, length(i_ym)), + σQ ::Vector = fill(1/model.nx, model.nx), + σR ::Vector = fill(1, length(i_ym)), + σP0::Vector = σQ, nint_u ::IntVectorOrInt = 0, σQint_u ::Vector = fill(1, max(sum(nint_u), 0)), - σP0int_u ::Vector = fill(1, max(sum(nint_u), 0)), + σP0int_u ::Vector = σQint_u, nint_ym ::IntVectorOrInt = default_nint(model, i_ym, nint_u), σQint_ym ::Vector = fill(1, max(sum(nint_ym), 0)), - σP0int_ym::Vector = fill(1, max(sum(nint_ym), 0)), + σP0int_ym::Vector = σQint_ym, α::Real = 1e-3, β::Real = 2, κ::Real = 0 @@ -669,15 +669,15 @@ ExtendedKalmanFilter estimator with a sample time Ts = 5.0 s, NonLinModel and: function ExtendedKalmanFilter( model::SM; i_ym::IntRangeOrVector = 1:model.ny, - σP0::Vector = fill(1/model.nx, model.nx), - σQ::Vector = fill(1/model.nx, model.nx), - σR::Vector = fill(1, length(i_ym)), + σQ ::Vector = fill(1/model.nx, model.nx), + σR ::Vector = fill(1, length(i_ym)), + σP0::Vector = σQ, nint_u ::IntVectorOrInt = 0, σQint_u ::Vector = fill(1, max(sum(nint_u), 0)), - σP0int_u ::Vector = fill(1, max(sum(nint_u), 0)), + σP0int_u ::Vector = σQint_u, nint_ym ::IntVectorOrInt = default_nint(model, i_ym, nint_u), σQint_ym ::Vector = fill(1, max(sum(nint_ym), 0)), - σP0int_ym::Vector = fill(1, max(sum(nint_ym), 0)) + σP0int_ym::Vector = σQint_ym, ) where {NT<:Real, SM<:SimModel{NT}} # estimated covariances matrices (variance = σ²) : P̂0 = Diagonal{NT}([σP0; σP0int_u; σP0int_ym].^2) diff --git a/src/estimator/mhe.jl b/src/estimator/mhe.jl index 091d1f944..def9600e3 100644 --- a/src/estimator/mhe.jl +++ b/src/estimator/mhe.jl @@ -136,15 +136,15 @@ function MovingHorizonEstimator( model::SM; He::Union{Int, Nothing}=nothing, i_ym::IntRangeOrVector = 1:model.ny, - σP0::Vector = fill(1/model.nx, model.nx), - σQ::Vector = fill(1/model.nx, model.nx), - σR::Vector = fill(1, length(i_ym)), + σQ ::Vector = fill(1/model.nx, model.nx), + σR ::Vector = fill(1, length(i_ym)), + σP0::Vector = σQ, nint_u ::IntVectorOrInt = 0, σQint_u ::Vector = fill(1, max(sum(nint_u), 0)), - σP0int_u ::Vector = fill(1, max(sum(nint_u), 0)), + σP0int_u ::Vector = σQint_u, nint_ym ::IntVectorOrInt = default_nint(model, i_ym, nint_u), σQint_ym ::Vector = fill(1, max(sum(nint_ym), 0)), - σP0int_ym::Vector = fill(1, max(sum(nint_ym), 0)), + σP0int_ym::Vector = σQint_ym, optim::JM = JuMP.Model(DEFAULT_MHE_OPTIMIZER, add_bridges=false), ) where {NT<:Real, SM<:SimModel{NT}, JM<:JuMP.GenericModel} # estimated covariances matrices (variance = σ²) : @@ -279,9 +279,9 @@ julia> estim = MovingHorizonEstimator(LinModel(ss(0.5,1,1,0,1)), He=3); julia> estim = setconstraint!(estim, x̂min=[-50, -50], x̂max=[50, 50]) MovingHorizonEstimator estimator with a sample time Ts = 1.0 s, LinModel and: - 2 estimation steps He + 3 estimation steps He 1 manipulated inputs u (0 integrating states) - 1 states x̂ + 2 states x̂ 1 measured outputs ym (1 integrating states) 0 unmeasured outputs yu 0 measured disturbances d From 70112d0d3dedfeca20c5f3d1a69888cb2bcfa17a Mon Sep 17 00:00:00 2001 From: franckgaga Date: Thu, 7 Dec 2023 19:50:01 -0500 Subject: [PATCH 17/20] test for debugging mhe --- src/estimator/mhe.jl | 71 ++++++++++++++++++++++++++++++-------------- 1 file changed, 49 insertions(+), 22 deletions(-) diff --git a/src/estimator/mhe.jl b/src/estimator/mhe.jl index def9600e3..e5ec3887f 100644 --- a/src/estimator/mhe.jl +++ b/src/estimator/mhe.jl @@ -172,6 +172,8 @@ function init_optimization!( nvar = length(estim.W̃) set_silent(optim) #set_attribute(optim, "max_iter", 1000*nvar) + set_attribute(optim, "nlp_scaling_method", "gradient-based") + #set_attribute(optim, "nlp_scaling_max_gradient", 1000) #limit_solve_time(estim) #TODO: add this feature @variable(optim, W̃var[1:nvar]) # --- nonlinear optimization init --- @@ -184,7 +186,7 @@ function init_optimization!( function Jfunc(W̃tup::JNT...) Ŷm = get_tmp(Ŷm_cache, W̃tup[1]) W̃ = collect(W̃tup) - if W̃tup != last_W̃tup_float + if W̃tup !== last_W̃tup_float g = get_tmp(g_cache, W̃tup[1]) X̂ = get_tmp(X̂_cache, W̃tup[1]) Ŷm, X̂ = predict!(Ŷm, X̂, estim, model, W̃) @@ -196,7 +198,7 @@ function init_optimization!( function Jfunc(W̃tup::ForwardDiff.Dual...) Ŷm = get_tmp(Ŷm_cache, W̃tup[1]) W̃ = collect(W̃tup) - if W̃tup != last_W̃tup_dual + if W̃tup !== last_W̃tup_dual g = get_tmp(g_cache, W̃tup[1]) X̂ = get_tmp(X̂_cache, W̃tup[1]) Ŷm, X̂ = predict!(Ŷm, X̂, estim, model, W̃) @@ -207,7 +209,7 @@ function init_optimization!( end function gfunc_i(i, W̃tup::NTuple{N, JNT}) where N g = get_tmp(g_cache, W̃tup[1]) - if W̃tup != last_W̃tup_float + if W̃tup !== last_W̃tup_float Ŷm = get_tmp(Ŷm_cache, W̃tup[1]) X̂ = get_tmp(X̂_cache, W̃tup[1]) W̃ = collect(W̃tup) @@ -219,7 +221,7 @@ function init_optimization!( end function gfunc_i(i, W̃tup::NTuple{N, ForwardDiff.Dual}) where N g = get_tmp(g_cache, W̃tup[1]) - if W̃tup != last_W̃tup_dual + if W̃tup !== last_W̃tup_dual Ŷm = get_tmp(Ŷm_cache, W̃tup[1]) X̂ = get_tmp(X̂_cache, W̃tup[1]) W̃ = collect(W̃tup) @@ -351,6 +353,7 @@ end "Reset `estim.P̂`, `estim.invP̄` and the time windows for the moving horizon estimator." function init_estimate_cov!(estim::MovingHorizonEstimator, _ , _ , _ ) estim.invP̄.data[:] = Hermitian(inv(estim.P̂0), :L) + estim.P̂.data[:] = estim.P̂0 estim.x̂0_past .= 0 estim.W̃ .= 0 estim.X̂ .= 0 @@ -389,16 +392,9 @@ function update_estimate!(estim::MovingHorizonEstimator{NT}, u, ym, d) where NT< estim.D[:] = [estim.D[nd+1:end] ; d] estim.Ŵ[:] = [estim.Ŵ[nŵ+1:end] ; ŵ] end - if Nk ≥ He - # update the arrival covariance with an Extended Kalman Filter: - # TODO: also support UnscentedKalmanFilter, and KalmanFilter for LinModel - F̂ = ForwardDiff.jacobian(x̂ -> f̂(estim, estim.model, x̂, u, d), estim.x̂) - Ĥ = ForwardDiff.jacobian(x̂ -> ĥ(estim, estim.model, x̂, d), estim.x̂) - Ĥm = Ĥ[estim.i_ym, :] - update_estimate_kf!(estim, F̂, Ĥm, u, ym, d, updatestate=false) - P̄ = estim.P̂ - estim.invP̄.data[:] = Hermitian(inv(P̄), :L) # .data is necessary for Hermitian - end + # TODO: vérifier pourquoi ya une discontinuité exactement quand k = He, + # tout est linéaire ici et les contraintes sont éloignées, ça devrait marcher il me + # semble ! Ŷm = Vector{NT}(undef, nYm) X̂ = Vector{NT}(undef, nX̂) estim.x̂0_past[:] = estim.X̂[1:nx̂] @@ -427,7 +423,7 @@ function update_estimate!(estim::MovingHorizonEstimator{NT}, u, ym, d) where NT< if !(status == OPTIMAL || status == LOCALLY_SOLVED) if isfatal(status) @error("MHE terminated without solution: estimation in open-loop", - status) + status) else @warn("MHE termination status not OPTIMAL or LOCALLY_SOLVED: keeping "* "solution anyway", status) @@ -438,6 +434,32 @@ function update_estimate!(estim::MovingHorizonEstimator{NT}, u, ym, d) where NT< estim.Ŵ[1:nŴ] = estim.W̃[nx̂+1:nx̂+nŴ] # update Ŵ with optimum for next time step Ŷm, X̂ = predict!(Ŷm, X̂, estim, model, estim.W̃) x̂[:] = X̂[(1 + nx̂*Nk):(nx̂*(Nk+1))] + + + if Nk == He + # update the arrival covariance with an Extended Kalman Filter: + # TODO: also support UnscentedKalmanFilter, and KalmanFilter for LinModel + F̂ = ForwardDiff.jacobian(x̂ -> f̂(estim, estim.model, x̂, u, d), estim.x̂) + println(F̂) + Ĥ = ForwardDiff.jacobian(x̂ -> ĥ(estim, estim.model, x̂, d), estim.x̂) + Ĥm = Ĥ[estim.i_ym, :] + update_estimate_kf!(estim, F̂, Ĥm, u, ym, d, updatestate=false) + P̄ = estim.P̂ + estim.invP̄.data[:] = Hermitian(inv(P̄), :L) # .data is necessary for Hermitian + end + + #ng = length(estim.i_g) + #println(ng) + #g = zeros(ng) + #g = con_nonlinprog!(g, estim, model, X̂) + #println(g) + J = obj_nonlinprog(estim, model, Ŷm, W̃curr) + + println(J) + println(solution_summary(optim, verbose=false)) + + + estim.Nk[] = Nk < He ? Nk + 1 : He return nothing end @@ -455,7 +477,8 @@ function obj_nonlinprog( Nk = estim.Nk[] nYm, nŴ, nx̂, invP̄ = Nk*estim.nym, Nk*estim.nx̂, estim.nx̂, estim.invP̄ invQ̂_Nk, invR̂_Nk = @views estim.invQ̂_He[1:nŴ, 1:nŴ], estim.invR̂_He[1:nYm, 1:nYm] - x̄0 = @views W̃[1:nx̂] - estim.x̂0_past # W̃ = [x̂(k-Nk|k); Ŵ] + x̂0 = @views W̃[1:nx̂] # W̃ = [x̂(k-Nk+1|k); Ŵ] + x̄0 = x̂0 - estim.x̂0_past V̂ = @views estim.Ym[1:nYm] - Ŷm[1:nYm] Ŵ = @views W̃[nx̂+1:nx̂+nŴ] return dot(x̄0, invP̄, x̄0) + dot(Ŵ, invQ̂_Nk, Ŵ) + dot(V̂, invR̂_Nk, V̂) @@ -465,7 +488,7 @@ function predict!( Ŷm, X̂, estim::MovingHorizonEstimator, model::SimModel, W̃::Vector{T} ) where {T<:Real} nu, nd, nx̂, nym, Nk = model.nu, model.nd, estim.nx̂, estim.nym, estim.Nk[] - X̂[1:nx̂] = W̃[1:nx̂] # W̃ = [x̂(k-Nk|k); Ŵ] + X̂[1:nx̂] = W̃[1:nx̂] # W̃ = [x̂(k-Nk+1|k); Ŵ] for j=1:Nk u = @views estim.U[(1 + nu*(j-1)):(nu*j)] d = @views estim.D[(1 + nd*(j-1)):(nd*j)] @@ -478,15 +501,19 @@ function predict!( end function con_nonlinprog!(g, estim::MovingHorizonEstimator, ::SimModel, X̂) - nX̂ = length(X̂) + nX̂con, nX̂ = length(estim.X̂min), estim.Nk[]*estim.nx̂ for i in eachindex(g) estim.i_g[i] || continue - if i ≤ nX̂ + if i ≤ nX̂con j = i - g[i] = estim.X̂min[j] - X̂[j] + if j ≤ nX̂ + g[i] = estim.X̂min[j] - X̂[j] + end else - j = i - nX̂ - g[i] = X̂[j] - estim.X̂max[j] + j = i - nX̂con + if j ≤ nX̂ + g[i] = X̂[j] - estim.X̂max[j] + end end end return g From eec8d128b073b68ca97d8c95fe1fff9f0b142f14 Mon Sep 17 00:00:00 2001 From: franckgaga Date: Thu, 7 Dec 2023 21:45:13 -0500 Subject: [PATCH 18/20] debug: MHE now work s as expected! --- src/estimator/mhe.jl | 49 +++++++++++++++----------------------------- 1 file changed, 16 insertions(+), 33 deletions(-) diff --git a/src/estimator/mhe.jl b/src/estimator/mhe.jl index e5ec3887f..4970a4c6a 100644 --- a/src/estimator/mhe.jl +++ b/src/estimator/mhe.jl @@ -75,7 +75,7 @@ struct MovingHorizonEstimator{ X̂, Ym = zeros(NT, nx̂*He), zeros(NT, nym*He) U, D, Ŵ = zeros(NT, nu*He), zeros(NT, nd*He), zeros(NT, nx̂*He) x̂0_past = zeros(NT, nx̂) - Nk = [1] + Nk = [0] estim = new{NT, SM, JM}( model, optim, W̃, lastu0, x̂, P̂, He, @@ -361,7 +361,7 @@ function init_estimate_cov!(estim::MovingHorizonEstimator, _ , _ , _ ) estim.U .= 0 estim.D .= 0 estim.Ŵ .= 0 - estim.Nk .= 1 + estim.Nk .= 0 return nothing end @@ -374,27 +374,27 @@ TBW """ function update_estimate!(estim::MovingHorizonEstimator{NT}, u, ym, d) where NT<:Real model, optim, x̂ = estim.model, estim.optim, estim.x̂ - nx̂, nym, nu, nd, nŵ = estim.nx̂, estim.nym, model.nu, model.nd, estim.nx̂ - Nk, He = estim.Nk[], estim.He - nŴ, nYm, nX̂ = nx̂*Nk, nym*Nk, nx̂*(Nk+1) - W̃var::Vector{VariableRef} = optim[:W̃var] + nx̂, nym, nu, nd, nŵ, He = estim.nx̂, estim.nym, model.nu, model.nd, estim.nx̂, estim.He ŵ = zeros(nŵ) # ŵ(k) = 0 for warm-starting - if Nk < He - estim.X̂[ (1 + nx̂*(Nk-1)):(nx̂*Nk)] = x̂ - estim.Ym[(1 + nym*(Nk-1)):(nym*Nk)] = ym - estim.U[ (1 + nu*(Nk-1)):(nu*Nk)] = u - estim.D[ (1 + nd*(Nk-1)):(nd*Nk)] = d - estim.Ŵ[ (1 + nŵ*(Nk-1)):(nŵ*Nk)] = ŵ - else + estim.Nk .+= 1 + Nk = estim.Nk[] + if Nk > He estim.X̂[:] = [estim.X̂[nx̂+1:end] ; x̂] estim.Ym[:] = [estim.Ym[nym+1:end]; ym] estim.U[:] = [estim.U[nu+1:end] ; u] estim.D[:] = [estim.D[nd+1:end] ; d] estim.Ŵ[:] = [estim.Ŵ[nŵ+1:end] ; ŵ] + estim.Nk[:] = [He] + else + estim.X̂[(1 + nx̂*(Nk-1)):(nx̂*Nk)] = x̂ + estim.Ym[(1 + nym*(Nk-1)):(nym*Nk)] = ym + estim.U[(1 + nu*(Nk-1)):(nu*Nk)] = u + estim.D[(1 + nd*(Nk-1)):(nd*Nk)] = d + estim.Ŵ[(1 + nŵ*(Nk-1)):(nŵ*Nk)] = ŵ end - # TODO: vérifier pourquoi ya une discontinuité exactement quand k = He, - # tout est linéaire ici et les contraintes sont éloignées, ça devrait marcher il me - # semble ! + Nk = estim.Nk[] + nŴ, nYm, nX̂ = nx̂*Nk, nym*Nk, nx̂*(Nk+1) + W̃var::Vector{VariableRef} = optim[:W̃var] Ŷm = Vector{NT}(undef, nYm) X̂ = Vector{NT}(undef, nX̂) estim.x̂0_past[:] = estim.X̂[1:nx̂] @@ -434,33 +434,16 @@ function update_estimate!(estim::MovingHorizonEstimator{NT}, u, ym, d) where NT< estim.Ŵ[1:nŴ] = estim.W̃[nx̂+1:nx̂+nŴ] # update Ŵ with optimum for next time step Ŷm, X̂ = predict!(Ŷm, X̂, estim, model, estim.W̃) x̂[:] = X̂[(1 + nx̂*Nk):(nx̂*(Nk+1))] - - if Nk == He # update the arrival covariance with an Extended Kalman Filter: # TODO: also support UnscentedKalmanFilter, and KalmanFilter for LinModel F̂ = ForwardDiff.jacobian(x̂ -> f̂(estim, estim.model, x̂, u, d), estim.x̂) - println(F̂) Ĥ = ForwardDiff.jacobian(x̂ -> ĥ(estim, estim.model, x̂, d), estim.x̂) Ĥm = Ĥ[estim.i_ym, :] update_estimate_kf!(estim, F̂, Ĥm, u, ym, d, updatestate=false) P̄ = estim.P̂ estim.invP̄.data[:] = Hermitian(inv(P̄), :L) # .data is necessary for Hermitian end - - #ng = length(estim.i_g) - #println(ng) - #g = zeros(ng) - #g = con_nonlinprog!(g, estim, model, X̂) - #println(g) - J = obj_nonlinprog(estim, model, Ŷm, W̃curr) - - println(J) - println(solution_summary(optim, verbose=false)) - - - - estim.Nk[] = Nk < He ? Nk + 1 : He return nothing end From 6e3f41768babb5b8b4bb2bef3d06c3257581d6a4 Mon Sep 17 00:00:00 2001 From: franckgaga Date: Thu, 7 Dec 2023 21:46:55 -0500 Subject: [PATCH 19/20] removed useless jump settings --- src/estimator/mhe.jl | 3 --- 1 file changed, 3 deletions(-) diff --git a/src/estimator/mhe.jl b/src/estimator/mhe.jl index 4970a4c6a..cdbad3d5c 100644 --- a/src/estimator/mhe.jl +++ b/src/estimator/mhe.jl @@ -171,9 +171,6 @@ function init_optimization!( # --- variables and linear constraints --- nvar = length(estim.W̃) set_silent(optim) - #set_attribute(optim, "max_iter", 1000*nvar) - set_attribute(optim, "nlp_scaling_method", "gradient-based") - #set_attribute(optim, "nlp_scaling_max_gradient", 1000) #limit_solve_time(estim) #TODO: add this feature @variable(optim, W̃var[1:nvar]) # --- nonlinear optimization init --- From a26242000431cabb8ca78512ebf5d8094dcc63c6 Mon Sep 17 00:00:00 2001 From: franckgaga Date: Thu, 7 Dec 2023 22:30:52 -0500 Subject: [PATCH 20/20] doc: arguments MHE --- README.md | 4 +-- src/estimator/mhe.jl | 54 ++++++++++++++++++++++++++++++++++++++-- test/test_state_estim.jl | 2 +- 3 files changed, 55 insertions(+), 5 deletions(-) diff --git a/README.md b/README.md index ee83486f7..537c33bce 100644 --- a/README.md +++ b/README.md @@ -121,6 +121,6 @@ for more detailed examples. - [x] measured outputs - [x] bumpless manual to automatic transfer for control with a proper intial estimate - [x] observers in predictor form to ease control applications -- [ ] moving horizon estimator that supports: - - [ ] inequality state constraints +- [x] moving horizon estimator that supports: + - [x] inequality state constraints - [ ] zero process noise equality constraint to reduce the problem size diff --git a/src/estimator/mhe.jl b/src/estimator/mhe.jl index cdbad3d5c..728c104cc 100644 --- a/src/estimator/mhe.jl +++ b/src/estimator/mhe.jl @@ -131,6 +131,31 @@ N_k = \begin{cases} k + 1 & k < H_e \\ H_e & k ≥ H_e \end{cases} ``` +See [`SteadyKalmanFilter`](@ref) for details on ``\mathbf{R̂}, \mathbf{Q̂}`` covariances and +model augmentation. + +# Arguments +- `model::SimModel` : (deterministic) model for the estimations. +- `He=nothing`: estimation horizon ``H_e``, must be specified. +- `optim=JuMP.Model(Ipopt.Optimizer)` : nonlinear optimizer used in the moving horizon + estimator, provided as a [`JuMP.Model`](https://jump.dev/JuMP.jl/stable/api/JuMP/#JuMP.Model) + (default to [`Ipopt.jl`](https://github.com/jump-dev/Ipopt.jl) optimizer). +- `` of [`SteadyKalmanFilter`](@ref) constructor. +- `` of [`KalmanFilter`](@ref) constructor. + +# Examples +```jldoctest +julia> model = NonLinModel((x,u,_)->0.1x+u, (x,_)->2x, 10.0, 1, 1, 1); + +julia> estim = MovingHorizonEstimator(model, He=5, σR=[1], σP0=[0.01]) +MovingHorizonEstimator estimator with a sample time Ts = 10.0 s, NonLinModel and: + 5 estimation steps He + 1 manipulated inputs u (0 integrating states) + 2 states x̂ + 1 measured outputs ym (1 integrating states) + 0 unmeasured outputs yu + 0 measured disturbances d +``` """ function MovingHorizonEstimator( model::SM; @@ -157,6 +182,21 @@ function MovingHorizonEstimator( ) end +@doc raw""" + MovingHorizonEstimator(model, He, i_ym, nint_u, nint_ym, P̂0, Q̂, R̂, optim) + +Construct the estimator from the augmented covariance matrices `P̂0`, `Q̂` and `R̂`. + +This syntax allows nonzero off-diagonal elements in ``\mathbf{P̂}_{-1}(0), \mathbf{Q̂, R̂}``. +""" +function MovingHorizonEstimator( + model::SM, He, i_ym, nint_u, nint_ym, P̂0, Q̂, R̂, optim +) where {NT<:Real, SM<:SimModel{NT}} + return MovingHorizonEstimator{NT, SM}( + model, He, i_ym, nint_u, nint_ym, P̂0, Q̂ , R̂, optim + ) +end + """ init_optimization!(estim::MovingHorizonEstimator, optim::JuMP.GenericModel) @@ -244,8 +284,6 @@ function init_optimization!( register(optim, sym, nvar, gfunc[i_end_X̂min+i], autodiff=true) end end - # TODO: moved this call to setconstraint! when the function will handle MHE - setnonlincon!(estim, estim.model) return nothing end @@ -464,6 +502,13 @@ function obj_nonlinprog( return dot(x̄0, invP̄, x̄0) + dot(Ŵ, invQ̂_Nk, Ŵ) + dot(V̂, invR̂_Nk, V̂) end +""" + predict!(Ŷm, X̂, estim::MovingHorizonEstimator, model::SimModel, W̃) -> Ŷm, X̂ + +Compute the predicted measured outputs `Ŷm` and states `X̂` for the `MovingHorizonEstimator`. + +The method mutates `Ŷm` and `X̂` vector arguments. +""" function predict!( Ŷm, X̂, estim::MovingHorizonEstimator, model::SimModel, W̃::Vector{T} ) where {T<:Real} @@ -480,6 +525,11 @@ function predict!( return Ŷm, X̂ end +""" + con_nonlinprog!(g, estim::MovingHorizonEstimator, model::SimModel, X̂) + +Nonlinear constrains for [`MovingHorizonEstimator`](@ref). +""" function con_nonlinprog!(g, estim::MovingHorizonEstimator, ::SimModel, X̂) nX̂con, nX̂ = length(estim.X̂min), estim.Nk[]*estim.nx̂ for i in eachindex(g) diff --git a/test/test_state_estim.jl b/test/test_state_estim.jl index 50592ba11..c3ca9c57a 100644 --- a/test/test_state_estim.jl +++ b/test/test_state_estim.jl @@ -453,7 +453,7 @@ end linmodel3 = LinModel{Float32}(0.5*ones(1,1), ones(1,1), ones(1,1), zeros(1,0), zeros(1,0), 1.0) ukf3 = UnscentedKalmanFilter(linmodel3) x̂ = updatestate!(ukf3, [0], [0]) - @test x̂ ≈ [0, 0] + @test x̂ ≈ [0, 0] atol=1e-3 @test isa(x̂, Vector{Float32}) end