Merge pull request #19 from franckgaga/MHE2

Added: `MovingHorizonEstimator` with state constraints

Author: franckgaga

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

docs/src/public/generic_func.md

@@ -31,6 +31,12 @@ initstate!
 setstate!
 ```
 
+## Set Constraint
+
+```@docs
+setconstraint!
+```
+
 ## Quick Simulation
 
 ### Simulate

docs/src/public/predictive_control.md

@@ -78,12 +78,6 @@ ExplicitMPC
 NonLinMPC
 ```
 
-## Set Constraint
-
-```@docs
-setconstraint!
-```
-
 ## Move Manipulated Input u
 
 ```@docs

docs/src/public/state_estim.md

@@ -71,6 +71,12 @@ UnscentedKalmanFilter
 ExtendedKalmanFilter
 ```
 
+## MovingHorizonEstimator
+
+```@docs
+MovingHorizonEstimator
+```
+
 ## InternalModel
 
 ```@docs

src/ModelPredictiveControl.jl

@@ -15,10 +15,18 @@ 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!
 
+"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)
 

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

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
@@ -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)
@@ -295,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, Ŷ, ΔŨ)
@@ -307,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, Ŷ, ΔŨ)
@@ -366,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))

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"""

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
 
@@ -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}
@@ -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).
 - `<keyword arguments>` 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,20 +277,20 @@ 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);
-    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
 
@@ -443,23 +443,23 @@ 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
 ) 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
 
@@ -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}
@@ -669,20 +669,20 @@ 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);
-    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
 
@@ -746,36 +746,42 @@ 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
 
 """
-    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.
 
 Allows code reuse for [`KalmanFilter`](@ref) and [`ExtendedKalmanFilterKalmanFilter`](@ref).
 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 x̂, P̂
+    return nothing
 end

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