diff --git a/Project.toml b/Project.toml index cd78878f3..c23223b59 100644 --- a/Project.toml +++ b/Project.toml @@ -1,7 +1,7 @@ name = "ModelPredictiveControl" uuid = "61f9bdb8-6ae4-484a-811f-bbf86720c31c" authors = ["Francis Gagnon"] -version = "1.2.0" +version = "1.3.0" [deps] ControlSystemsBase = "aaaaaaaa-a6ca-5380-bf3e-84a91bcd477e" diff --git a/src/controller/execute.jl b/src/controller/execute.jl index 29e6c5a53..51a366029 100644 --- a/src/controller/execute.jl +++ b/src/controller/execute.jl @@ -114,7 +114,7 @@ julia> round.(getinfo(mpc)[:Ŷ], digits=3) function getinfo(mpc::PredictiveController{NT}) where NT<:Real model = mpc.estim.model nŶe, nUe = (mpc.Hp+1)*model.ny, (mpc.Hp+1)*model.nu - info = Dict{Symbol, Union{JuMP._SolutionSummary, Vector{NT}, NT}}() + info = Dict{Symbol, Any}() Ŷ0, u0, û0 = similar(mpc.Yop), similar(model.uop), similar(model.uop) Ŷs = similar(mpc.Yop) x̂0, x̂0next = similar(mpc.estim.x̂0), similar(mpc.estim.x̂0) @@ -491,7 +491,8 @@ If supported by `mpc.optim`, it warm-starts the solver at: where ``\mathbf{Δu}_{k-1}(k+j)`` is the input increment for time ``k+j`` computed at the last control period ``k-1``. It then calls `JuMP.optimize!(mpc.optim)` and extract the solution. A failed optimization prints an `@error` log in the REPL and returns the -warm-start value. +warm-start value. A failed optimization also prints [`getinfo`](@ref) results in +the debug log [if activated](https://docs.julialang.org/en/v1/stdlib/Logging/#Example:-Enable-debug-level-messages). """ function optim_objective!(mpc::PredictiveController{NT}) where {NT<:Real} model, optim = mpc.estim.model, mpc.optim @@ -518,13 +519,22 @@ function optim_objective!(mpc::PredictiveController{NT}) where {NT<:Real} if !issolved(optim) status = JuMP.termination_status(optim) if iserror(optim) - @error("MPC terminated without solution: returning last solution shifted", - status) + @error( + "MPC terminated without solution: estimation in open-loop "* + "(more info in debug log)", + status + ) else - @warn("MPC termination status not OPTIMAL or LOCALLY_SOLVED: keeping "* - "solution anyway", status) + @warn( + "MPC termination status not OPTIMAL or LOCALLY_SOLVED: keeping solution "* + "anyway (more info in debug log)", + status + ) end - @debug JuMP.solution_summary(optim, verbose=true) + @debug( + "calling getinfo (use logger with show_limited=false if values are truncated)", + getinfo(mpc) + ) end if iserror(optim) mpc.ΔŨ .= ΔŨ0 diff --git a/src/estimator/luenberger.jl b/src/estimator/luenberger.jl index 8924af2aa..6000cf073 100644 --- a/src/estimator/luenberger.jl +++ b/src/estimator/luenberger.jl @@ -137,7 +137,7 @@ function update_estimate!(estim::Luenberger, y0m, d0, u0) end "Throw an error if `setmodel!` is called on `Luenberger` observer w/o the default values." -function setmodel!(estim::Luenberger, model, args...) +function setmodel_estimator!(estim::Luenberger, model, args...) if estim.model !== model error("Luenberger does not support setmodel!") end diff --git a/src/estimator/mhe/construct.jl b/src/estimator/mhe/construct.jl index ca86c59ba..68c5bf271 100644 --- a/src/estimator/mhe/construct.jl +++ b/src/estimator/mhe/construct.jl @@ -123,11 +123,6 @@ struct MovingHorizonEstimator{ validate_kfcov(nym, nx̂, Q̂, R̂, P̂_0) lastu0 = zeros(NT, nu) x̂0 = [zeros(NT, model.nx); zeros(NT, nxs)] - P̂_0 = Hermitian(P̂_0, :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) r = direct ? 0 : 1 E, G, J, B, ex̄, Ex̂, Gx̂, Jx̂, Bx̂ = init_predmat_mhe( model, He, i_ym, Â, B̂u, Ĉm, B̂d, D̂dm, x̂op, f̂op, r @@ -146,10 +141,18 @@ struct MovingHorizonEstimator{ nD0 = direct ? nd*(He+1) : nd*He U0, D0 = zeros(NT, nu*He), zeros(NT, nD0) Ŵ = zeros(NT, nx̂*He) + buffer = StateEstimatorBuffer{NT}(nu, nx̂, nym, ny, nd) + P̂_0 = Hermitian(P̂_0, :L) + Q̂, R̂ = Hermitian(Q̂, :L), Hermitian(R̂, :L) + P̂_0 = Hermitian(P̂_0, :L) + invP̄ = inv_cholesky!(buffer.P̂, P̂_0) + invQ̂ = inv_cholesky!(buffer.Q̂, Q̂) + invR̂ = inv_cholesky!(buffer.R̂, R̂) + invQ̂_He = Hermitian(repeatdiag(invQ̂, He), :L) + invR̂_He = Hermitian(repeatdiag(invR̂, He), :L) x̂0arr_old = zeros(NT, nx̂) P̂arr_old = copy(P̂_0) Nk = [0] - buffer = StateEstimatorBuffer{NT}(nu, nx̂, nym, ny, nd) corrected = [false] estim = new{NT, SM, JM, CE}( model, optim, con, covestim, diff --git a/src/estimator/mhe/execute.jl b/src/estimator/mhe/execute.jl index bc0c4ef38..5dcb89692 100644 --- a/src/estimator/mhe/execute.jl +++ b/src/estimator/mhe/execute.jl @@ -16,7 +16,7 @@ function init_estimate_cov!(estim::MovingHorizonEstimator, _ , d0, u0) estim.D0[1:estim.model.nd] .= d0 end # estim.P̂_0 is in fact P̂(-1|-1) is estim.direct==false, else P̂(-1|0) - estim.invP̄ .= inv(estim.P̂_0) + invert_cov!(estim, estim.P̂_0) estim.P̂arr_old .= estim.P̂_0 estim.x̂0arr_old .= 0 return nothing @@ -108,8 +108,7 @@ function getinfo(estim::MovingHorizonEstimator{NT}) where NT<:Real nu, ny, nd = model.nu, model.ny, model.nd nx̂, nym, nŵ, nϵ = estim.nx̂, estim.nym, estim.nx̂, estim.nϵ nx̃ = nϵ + nx̂ - MyTypes = Union{JuMP._SolutionSummary, Hermitian{NT, Matrix{NT}}, Vector{NT}, NT} - info = Dict{Symbol, MyTypes}() + info = Dict{Symbol, Any}() V̂, X̂0 = similar(estim.Y0m[1:nym*Nk]), similar(estim.X̂0[1:nx̂*Nk]) û0, ŷ0 = similar(model.uop), similar(model.yop) V̂, X̂0 = predict!(V̂, X̂0, û0, ŷ0, estim, model, estim.Z̃) @@ -370,7 +369,8 @@ If supported by `estim.optim`, it warm-starts the solver at: where ``\mathbf{ŵ}_{k-1}(k-j)`` is the input increment for time ``k-j`` computed at the last time step ``k-1``. It then calls `JuMP.optimize!(estim.optim)` and extract the solution. A failed optimization prints an `@error` log in the REPL and returns the -warm-start value. +warm-start value. A failed optimization also prints [`getinfo`](@ref) results in +the debug log [if activated](https://docs.julialang.org/en/v1/stdlib/Logging/#Example:-Enable-debug-level-messages). """ function optim_objective!(estim::MovingHorizonEstimator{NT}) where NT<:Real model, optim = estim.model, estim.optim @@ -406,13 +406,22 @@ function optim_objective!(estim::MovingHorizonEstimator{NT}) where NT<:Real if !issolved(optim) status = JuMP.termination_status(optim) if iserror(optim) - @error("MHE terminated without solution: estimation in open-loop", - status) + @error( + "MHE terminated without solution: estimation in open-loop "* + "(more info in debug log)", + status + ) else - @warn("MHE termination status not OPTIMAL or LOCALLY_SOLVED: keeping "* - "solution anyway", status) + @warn( + "MHE termination status not OPTIMAL or LOCALLY_SOLVED: keeping solution "* + "anyway (more info in debug log)", + status + ) end - @debug JuMP.solution_summary(optim, verbose=true) + @debug( + "calling getinfo (use logger with show_limited=false if values are truncated)", + getinfo(estim) + ) end if iserror(optim) estim.Z̃ .= Z̃_0 @@ -433,9 +442,17 @@ function correct_cov!(estim::MovingHorizonEstimator) y0marr, d0arr = @views estim.Y0m[1:nym], estim.D0[1:nd] estim.covestim.x̂0 .= estim.x̂0arr_old estim.covestim.P̂ .= estim.P̂arr_old - correct_estimate!(estim.covestim, y0marr, d0arr) - estim.P̂arr_old .= estim.covestim.P̂ - estim.invP̄ .= inv(estim.P̂arr_old) + try + correct_estimate!(estim.covestim, y0marr, d0arr) + estim.P̂arr_old .= estim.covestim.P̂ + invert_cov!(estim, estim.P̂arr_old) + catch err + if err isa PosDefException + @warn("Arrival covariance is not positive definite: keeping the old one") + else + rethrow() + end + end return nothing end @@ -445,9 +462,31 @@ function update_cov!(estim::MovingHorizonEstimator) u0arr, y0marr, d0arr = @views estim.U0[1:nu], estim.Y0m[1:nym], estim.D0[1:nd] estim.covestim.x̂0 .= estim.x̂0arr_old estim.covestim.P̂ .= estim.P̂arr_old - update_estimate!(estim.covestim, y0marr, d0arr, u0arr) - estim.P̂arr_old .= estim.covestim.P̂ - estim.invP̄ .= inv(estim.P̂arr_old) + try + update_estimate!(estim.covestim, y0marr, d0arr, u0arr) + estim.P̂arr_old .= estim.covestim.P̂ + invert_cov!(estim, estim.P̂arr_old) + catch err + if err isa PosDefException + @warn("Arrival covariance is not positive definite: keeping the old one") + else + rethrow() + end + end + return nothing +end + +"Invert the covariance estimate at arrival `P̄`." +function invert_cov!(estim::MovingHorizonEstimator, P̄) + try + estim.invP̄ .= inv_cholesky!(estim.buffer.P̂, P̄) + catch err + if err isa PosDefException + @warn("Arrival covariance is not invertible: keeping the old one") + else + rethrow() + end + end return nothing end diff --git a/src/general.jl b/src/general.jl index 40f5e6264..1e512b62d 100644 --- a/src/general.jl +++ b/src/general.jl @@ -59,4 +59,18 @@ end to_hermitian(A::AbstractVector) = Hermitian(reshape(A, 1, 1), :L) to_hermitian(A::AbstractMatrix) = Hermitian(A, :L) to_hermitian(A::Hermitian) = A -to_hermitian(A) = A \ No newline at end of file +to_hermitian(A) = A + +""" +Compute the inverse of a the Hermitian positive definite matrix `A` using `cholesky`. + +Builtin `inv` function uses LU factorization which is not the best choice for Hermitian +positive definite matrices. The function will mutate `buffer` to reduce memory allocations. +""" +function inv_cholesky!(buffer::Matrix, A::Hermitian) + Achol = Hermitian(buffer, :L) + Achol .= A + chol_obj = cholesky!(Achol) + invA = Hermitian(inv(chol_obj), :L) + return invA +end \ No newline at end of file diff --git a/test/test_state_estim.jl b/test/test_state_estim.jl index 9eec37db7..6a4ce0166 100644 --- a/test/test_state_estim.jl +++ b/test/test_state_estim.jl @@ -975,6 +975,35 @@ end @test info[:Ŷ][end-1:end] ≈ [50, 30] atol=1e-9 end +@testset "MovingHorizonEstimator fallbacks for arrival covariance estimation" begin + linmodel = setop!(LinModel(sys,Ts,i_u=[1,2], i_d=[3]), uop=[10,50], yop=[50,30], dop=[5]) + f(x,u,d,_) = linmodel.A*x + linmodel.Bu*u + linmodel.Bd*d + h(x,d,_) = linmodel.C*x + linmodel.Dd*d + nonlinmodel = setop!(NonLinModel(f, h, Ts, 2, 4, 2, 1, solver=nothing), uop=[10,50], yop=[50,30], dop=[5]) + mhe = MovingHorizonEstimator(nonlinmodel, nint_ym=0, He=1) + preparestate!(mhe, [50, 30], [5]) + updatestate!(mhe, [10, 50], [50, 30], [5]) + mhe.P̂arr_old[1, 1] = -1e-3 # negative eigenvalue to trigger fallback + P̂arr_old_copy = deepcopy(mhe.P̂arr_old) + invP̄_copy = deepcopy(mhe.invP̄) + @test_logs( + (:warn, "Arrival covariance is not positive definite: keeping the old one"), + preparestate!(mhe, [50, 30], [5]) + ) + @test mhe.P̂arr_old ≈ P̂arr_old_copy + @test mhe.invP̄ ≈ invP̄_copy + @test_logs( + (:warn, "Arrival covariance is not positive definite: keeping the old one"), + updatestate!(mhe, [10, 50], [50, 30], [5]) + ) + @test mhe.P̂arr_old ≈ P̂arr_old_copy + @test mhe.invP̄ ≈ invP̄_copy + @test_logs( + (:warn, "Arrival covariance is not invertible: keeping the old one"), + ModelPredictiveControl.invert_cov!(mhe, Hermitian(zeros(mhe.nx̂, mhe.nx̂),:L)) + ) +end + @testset "MovingHorizonEstimator set constraints" begin linmodel1 = setop!(LinModel(sys,Ts,i_u=[1,2]), uop=[10,50], yop=[50,30]) mhe1 = MovingHorizonEstimator(linmodel1, He=1, nint_ym=0, Cwt=1e3)