diff --git a/Project.toml b/Project.toml index 18d4df711..104d13da8 100644 --- a/Project.toml +++ b/Project.toml @@ -21,7 +21,7 @@ SparseConnectivityTracer = "9f842d2f-2579-4b1d-911e-f412cf18a3f5" SparseMatrixColorings = "0a514795-09f3-496d-8182-132a7b665d35" [compat] -ControlSystemsBase = "1.9" +ControlSystemsBase = "1.18.2" DAQP = "0.6, 0.7.1" DifferentiationInterface = "0.6.45, 0.7" Documenter = "1" diff --git a/src/estimator/construct.jl b/src/estimator/construct.jl index 8ae2558b4..897d06c9b 100644 --- a/src/estimator/construct.jl +++ b/src/estimator/construct.jl @@ -56,10 +56,13 @@ struct KalmanCovariances{ Q̂::Q̂C, R̂::R̂C, P̂_0, He ) where {NT<:Real, Q̂C<:AbstractMatrix{NT}, R̂C<:AbstractMatrix{NT}} if isnothing(P̂_0) - P̂_0 = zeros(NT, 0, 0) + P̂_0 = zeros(NT, 0, 0) # does not apply to steady-state Kalman filter + P̂ = zeros(NT, size(Q̂)) # will hold the steady-state error covariance + else + P̂ = copy(P̂_0) end P̂_0 = Hermitian(P̂_0, :L) - P̂ = copy(P̂_0) + P̂ = Hermitian(P̂, :L) Q̂ = Hermitian(Q̂, :L) R̂ = Hermitian(R̂, :L) # the following variables are only for the moving horizon estimator: diff --git a/src/estimator/kalman.jl b/src/estimator/kalman.jl index b7b04315b..9dd3d985e 100644 --- a/src/estimator/kalman.jl +++ b/src/estimator/kalman.jl @@ -1,8 +1,11 @@ +"Abstract supertype of all Kalman-type state estimators." +abstract type KalmanEstimator{NT<:Real} <: StateEstimator{NT} end + struct SteadyKalmanFilter{ NT<:Real, SM<:LinModel, KC<:KalmanCovariances -} <: StateEstimator{NT} +} <: KalmanEstimator{NT} model::SM cov ::KC x̂op ::Vector{NT} @@ -40,24 +43,8 @@ struct SteadyKalmanFilter{ Â, B̂u, Ĉ, B̂d, D̂d, x̂op, f̂op = augment_model(model, As, Cs_u, Cs_y) Ĉm, D̂dm = Ĉ[i_ym, :], D̂d[i_ym, :] R̂, Q̂ = cov.R̂, cov.Q̂ - if ny == nym - R̂_y = R̂ - else - R̂_y = zeros(NT, ny, ny) - R̂_y[i_ym, i_ym] = R̂ - R̂_y = Hermitian(R̂_y, :L) - end - K̂ = try - ControlSystemsBase.kalman(Discrete, Â, Ĉ, Q̂, R̂_y; direct)[:, i_ym] - catch my_error - if isa(my_error, ErrorException) - error("Cannot compute the optimal Kalman gain K̂ for the "* - "SteadyKalmanFilter. You may try to remove integrators with "* - "nint_u/nint_ym parameter or use the time-varying KalmanFilter.") - else - rethrow() - end - end + K̂, P̂ = init_skf(i_ym, Â, Ĉ, Q̂, R̂; direct) + cov.P̂ .= P̂ x̂0 = [zeros(NT, model.nx); zeros(NT, nxs)] corrected = [false] buffer = StateEstimatorBuffer{NT}(nu, nx̂, nym, ny, nd, nk) @@ -208,6 +195,36 @@ function SteadyKalmanFilter( return SteadyKalmanFilter{NT}(model, i_ym, nint_u, nint_ym, cov; direct) end +""" + init_skf(i_ym, Â, Ĉ, Q̂, R̂; direct=true) -> K̂, P̂ + +Initialize the steady-state Kalman gain `K̂` and estimation error covariance `P̂`. +""" +function init_skf(i_ym, Â, Ĉ, Q̂, R̂; direct=true) + ny, nym = size(Ĉ, 1), length(i_ym) + if ny != nym + R̂_y = zeros(eltype(R̂), ny, ny) + R̂_y[i_ym, i_ym] = R̂ + R̂ = Hermitian(R̂_y, :L) + end + K̂, P̂ = try + ControlSystemsBase.kalman(Discrete, Â, Ĉ, Q̂, R̂; direct, extra=Val(true)) + catch my_error + if isa(my_error, ErrorException) + error("Cannot compute the optimal Kalman gain K̂ for the "* + "SteadyKalmanFilter. You may try to remove integrators with "* + "nint_u/nint_ym parameter or use the time-varying KalmanFilter.") + else + rethrow() + end + end + if ny != nym + K̂ = K̂[:, i_ym] + end + P̂ = Hermitian(P̂, :L) + return K̂, P̂ +end + "Throw an error if `setmodel!` is called on a SteadyKalmanFilter w/o the default values." function setmodel_estimator!(estim::SteadyKalmanFilter, model, _ , _ , _ , Q̂, R̂) if estim.model !== model || !isnothing(Q̂) || !isnothing(R̂) @@ -216,12 +233,6 @@ function setmodel_estimator!(estim::SteadyKalmanFilter, model, _ , _ , _ , Q̂, return nothing end -"Throw an error if P̂ != nothing." -function setstate_cov!(::SteadyKalmanFilter, P̂) - isnothing(P̂) || error("SteadyKalmanFilter does not compute an estimation covariance matrix P̂.") - return nothing -end - @doc raw""" correct_estimate!(estim::SteadyKalmanFilter, y0m, d0) @@ -296,7 +307,7 @@ struct KalmanFilter{ NT<:Real, SM<:LinModel, KC<:KalmanCovariances -} <: StateEstimator{NT} +} <: KalmanEstimator{NT} model::SM cov ::KC x̂op::Vector{NT} @@ -508,7 +519,7 @@ struct UnscentedKalmanFilter{ NT<:Real, SM<:SimModel, KC<:KalmanCovariances -} <: StateEstimator{NT} +} <: KalmanEstimator{NT} model::SM cov ::KC x̂op ::Vector{NT} @@ -554,7 +565,7 @@ struct UnscentedKalmanFilter{ nx̂ = model.nx + nxs Â, B̂u, Ĉ, B̂d, D̂d, x̂op, f̂op = augment_model(model, As, Cs_u, Cs_y) Ĉm, D̂dm = Ĉ[i_ym, :], D̂d[i_ym, :] - nσ, γ, m̂, Ŝ = init_ukf(model, nx̂, α, β, κ) + nσ, γ, m̂, Ŝ = init_ukf(nx̂, α, β, κ) x̂0 = [zeros(NT, model.nx); zeros(NT, nxs)] K̂ = zeros(NT, nx̂, nym) M̂ = Hermitian(zeros(NT, nym, nym), :L) @@ -715,7 +726,7 @@ end @doc raw""" - init_ukf(model, nx̂, α, β, κ) -> nσ, γ, m̂, Ŝ + init_ukf(nx̂, α, β, κ) -> nσ, γ, m̂, Ŝ Compute the [`UnscentedKalmanFilter`](@ref) constants from ``α, β`` and ``κ``. @@ -735,14 +746,15 @@ covariance are respectively: ``` See [`update_estimate!(::UnscentedKalmanFilter)`](@ref) for other details. """ -function init_ukf(::SimModel{NT}, nx̂, α, β, κ) where {NT<:Real} - nσ = 2nx̂ + 1 # number of sigma points - γ = α * √(nx̂ + κ) # constant factor of standard deviation √P +function init_ukf(nx̂, α, β, κ) + α, β, κ = promote(α, β, κ) + nσ =2nx̂ + 1 # number of sigma points + γ = α * √(nx̂ + κ) # constant factor of standard deviation √P m̂_0 = 1 - nx̂ / γ^2 Ŝ_0 = m̂_0 + 1 - α^2 + β w = 1 / 2 / γ^2 - m̂ = NT[m̂_0; fill(w, 2 * nx̂)] # weights for the mean - Ŝ = Diagonal(NT[Ŝ_0; fill(w, 2 * nx̂)]) # weights for the covariance + m̂ = [m̂_0; fill(w, 2 * nx̂)] # weights for the mean + Ŝ = Diagonal([Ŝ_0; fill(w, 2 * nx̂)]) # weights for the covariance return nσ, γ, m̂, Ŝ end @@ -885,7 +897,7 @@ struct ExtendedKalmanFilter{ JB<:AbstractADType, FF<:Function, HF<:Function -} <: StateEstimator{NT} +} <: KalmanEstimator{NT} model::SM cov ::KC x̂op ::Vector{NT} diff --git a/src/estimator/mhe.jl b/src/estimator/mhe.jl index 3ae49ea2a..b03414823 100644 --- a/src/estimator/mhe.jl +++ b/src/estimator/mhe.jl @@ -5,6 +5,7 @@ include("mhe/execute.jl") function print_details(io::IO, estim::MovingHorizonEstimator) println(io, "├ optimizer: $(JuMP.solver_name(estim.optim)) ") print_backends(io, estim, estim.model) + println(io, "├ arrival covariance: $(nameof(typeof(estim.covestim))) ") end "Print the differentiation backends for `SimModel`." diff --git a/src/estimator/mhe/construct.jl b/src/estimator/mhe/construct.jl index c5e9fd5c8..648400ec4 100644 --- a/src/estimator/mhe/construct.jl +++ b/src/estimator/mhe/construct.jl @@ -51,7 +51,7 @@ struct MovingHorizonEstimator{ JM<:JuMP.GenericModel, GB<:AbstractADType, JB<:AbstractADType, - CE<:StateEstimator, + CE<:KalmanEstimator, } <: StateEstimator{NT} model::SM cov ::KC @@ -121,7 +121,7 @@ struct MovingHorizonEstimator{ JM<:JuMP.GenericModel, GB<:AbstractADType, JB<:AbstractADType, - CE<:StateEstimator{NT} + CE<:KalmanEstimator{NT} } nu, ny, nd, nk = model.nu, model.ny, model.nd, model.nk He < 1 && throw(ArgumentError("Estimation horizon He should be ≥ 1")) @@ -280,6 +280,7 @@ MovingHorizonEstimator estimator with a sample time Ts = 10.0 s: ├ optimizer: Ipopt ├ gradient: AutoForwardDiff ├ jacobian: AutoForwardDiff +├ arrival covariance: UnscentedKalmanFilter └ dimensions: ├ 5 estimation steps He ├ 0 slack variable ε (estimation constraints) @@ -423,8 +424,10 @@ This syntax allows nonzero off-diagonal elements in ``\mathbf{P̂_i}, \mathbf{Q where ``\mathbf{P̂_i}`` is the initial estimation covariance, provided by `P̂_0` argument. The keyword argument `covestim` also allows specifying a custom [`StateEstimator`](@ref) object for the estimation of covariance at the arrival ``\mathbf{P̂}_{k-N_k}(k-N_k+p)``. The -supported types are [`KalmanFilter`](@ref), [`UnscentedKalmanFilter`](@ref) and -[`ExtendedKalmanFilter`](@ref). +supported types are [`SteadyKalmanFilter`](@ref), [`KalmanFilter`](@ref), +[`UnscentedKalmanFilter`](@ref) and [`ExtendedKalmanFilter`](@ref). A constant arrival +covariance is supported with [`SteadyKalmanFilter`](@ref), and by setting the `P̂` argument +of [`setstate!`](@ref) at the desired value. """ function MovingHorizonEstimator( model::SM, He, i_ym, nint_u, nint_ym, P̂_0, Q̂, R̂, Cwt=Inf; @@ -436,6 +439,7 @@ function MovingHorizonEstimator( ) where {NT<:Real, SM<:SimModel{NT}, JM<:JuMP.GenericModel, CE<:StateEstimator{NT}} P̂_0, Q̂, R̂ = to_mat(P̂_0), to_mat(Q̂), to_mat(R̂) cov = KalmanCovariances(model, i_ym, nint_u, nint_ym, Q̂, R̂, P̂_0, He) + validate_covestim(cov, covestim) return MovingHorizonEstimator{NT}( model, He, i_ym, nint_u, nint_ym, cov, Cwt, @@ -444,6 +448,7 @@ function MovingHorizonEstimator( ) end + function default_covestim_mhe(model::LinModel, i_ym, nint_u, nint_ym, P̂_0, Q̂, R̂; direct) return KalmanFilter(model, i_ym, nint_u, nint_ym, P̂_0, Q̂, R̂; direct) end @@ -451,6 +456,18 @@ function default_covestim_mhe(model::SimModel, i_ym, nint_u, nint_ym, P̂_0, Q̂ return UnscentedKalmanFilter(model, i_ym, nint_u, nint_ym, P̂_0, Q̂, R̂; direct) end +"Validate covestim type and dimensions." +function validate_covestim(cov::KalmanCovariances, covestim::KalmanEstimator) + if size(cov.P̂) != size(covestim.cov.P̂) + throw(ArgumentError("estimation covariance covestim.cov.P̂ size does not match the MHE")) + end + return nothing +end +function validate_covestim(::KalmanCovariances, ::StateEstimator) + error( "covestim argument must be a SteadyKalmanFilter, KalmanFilter, "* + "ExtendedKalmanFilter or UnscentedKalmanFilter") +end + @doc raw""" setconstraint!(estim::MovingHorizonEstimator; ) -> estim @@ -498,6 +515,7 @@ julia> estim = setconstraint!(estim, x̂min=[-50, -50], x̂max=[50, 50]) MovingHorizonEstimator estimator with a sample time Ts = 1.0 s: ├ model: LinModel ├ optimizer: OSQP +├ arrival covariance: KalmanFilter └ dimensions: ├ 3 estimation steps He ├ 0 slack variable ε (estimation constraints) diff --git a/test/2_test_state_estim.jl b/test/2_test_state_estim.jl index 6b05651d2..da0a2facd 100644 --- a/test/2_test_state_estim.jl +++ b/test/2_test_state_estim.jl @@ -112,7 +112,6 @@ end @test x̂ ≈ [0, 0] @test isa(x̂, Vector{Float32}) @test_throws ArgumentError updatestate!(kalmanfilter1, [10, 50]) - @test_throws ErrorException setstate!(kalmanfilter1, [1,2,3,4], diagm(.1:.1:.4)) end @testitem "SteadyKalmanFilter set model" setup=[SetupMPCtests] begin @@ -904,17 +903,26 @@ end @test_throws ArgumentError MovingHorizonEstimator(linmodel) @test_throws ArgumentError MovingHorizonEstimator(linmodel, He=0) @test_throws ArgumentError MovingHorizonEstimator(linmodel, Cwt=-1) + @test_throws ErrorException MovingHorizonEstimator( + nonlinmodel, 5, 1:2, 0, [1, 1], I_6, I_6, I_2, Inf; optim, + covestim = InternalModel(nonlinmodel) + ) + @test_throws ArgumentError MovingHorizonEstimator( + nonlinmodel, 5, 1:2, 0, [1, 1], I_6, I_6, I_2, Inf; optim, + covestim = UnscentedKalmanFilter(nonlinmodel, nint_ym=[2,2]) + ) end @testitem "MovingHorizonEstimator estimation and getinfo" setup=[SetupMPCtests] begin - using .SetupMPCtests, ControlSystemsBase, LinearAlgebra, JuMP, Ipopt, ForwardDiff + using .SetupMPCtests, ControlSystemsBase, LinearAlgebra, ForwardDiff + using JuMP, Ipopt, DAQP linmodel = LinModel(sys,Ts,i_u=[1,2], i_d=[3]) linmodel = setop!(linmodel, uop=[10,50], yop=[50,30], dop=[5]) f(x,u,d,model) = model.A*x + model.Bu*u + model.Bd*d h(x,d,model) = model.C*x + model.Dd*d nonlinmodel = NonLinModel(f, h, Ts, 2, 4, 2, 1, solver=nothing, p=linmodel) nonlinmodel = setop!(nonlinmodel, uop=[10,50], yop=[50,30], dop=[5]) - + mhe1 = MovingHorizonEstimator(nonlinmodel, He=2) JuMP.set_attribute(mhe1.optim, "tol", 1e-7) preparestate!(mhe1, [50, 30], [5]) @@ -950,7 +958,6 @@ end x̂ = updatestate!(mhe1, [10, 50], [50, 30], [5]) @test x̂ ≈ zeros(6) atol=1e-9 @test mhe1.x̂0 ≈ zeros(6) atol=1e-9 - @test evaloutput(mhe1, [5]) ≈ mhe1([5]) ≈ [50, 30] info = getinfo(mhe1) @test info[:x̂] ≈ x̂ atol=1e-9 @test info[:Ŷ][end-1:end] ≈ [50, 30] atol=1e-9 @@ -976,7 +983,6 @@ end @test x̂ ≈ zeros(6) atol=1e-9 @test mhe2.x̂0 ≈ zeros(6) atol=1e-9 preparestate!(mhe2, [50, 30], [5]) - @test evaloutput(mhe2, [5]) ≈ mhe2([5]) ≈ [50, 30] info = getinfo(mhe2) @test info[:x̂] ≈ x̂ atol=1e-9 @test info[:Ŷ][end-1:end] ≈ [50, 30] atol=1e-9 @@ -998,7 +1004,6 @@ end x̂ = updatestate!(mhe2, [10, 50], [50, 30], [5]) @test x̂ ≈ zeros(6) atol=1e-9 @test mhe2.x̂0 ≈ zeros(6) atol=1e-9 - @test evaloutput(mhe2, [5]) ≈ mhe2([5]) ≈ [50, 30] info = getinfo(mhe2) @test info[:x̂] ≈ x̂ atol=1e-9 @test info[:Ŷ][end-1:end] ≈ [50, 30] atol=1e-9 @@ -1012,12 +1017,29 @@ end updatestate!(mhe2, [10, 50], [51, 32], [5]) end @test mhe2([5]) ≈ [51, 32] atol=1e-2 + + Q̂ = diagm([1/4, 1/4, 1/4, 1/4].^2) + R̂ = diagm([1, 1].^2) + optim = Model(DAQP.Optimizer) + covestim = SteadyKalmanFilter(linmodel, 1:2, 0, 0, Q̂, R̂) + P̂_0 = covestim.cov.P̂ + mhe3 = MovingHorizonEstimator(linmodel, 2, 1:2, 0, 0, P̂_0, Q̂, R̂; optim, covestim) + preparestate!(mhe3, [50, 30], [5]) + x̂ = updatestate!(mhe3, [10, 50], [50, 30], [5]) + @test x̂ ≈ zeros(4) atol=1e-9 + @test mhe3.x̂0 ≈ zeros(4) atol=1e-9 + preparestate!(mhe3, [50, 30], [5]) + info = getinfo(mhe3) + @test info[:x̂] ≈ x̂ atol=1e-9 + @test info[:Ŷ][end-1:end] ≈ [50, 30] atol=1e-9 + linmodel3 = LinModel{Float32}(0.5*ones(1,1), ones(1,1), ones(1,1), zeros(1,0), zeros(1,0), 1.0) mhe3 = MovingHorizonEstimator(linmodel3, He=1) preparestate!(mhe3, [0]) x̂ = updatestate!(mhe3, [0], [0]) @test x̂ ≈ [0, 0] atol=1e-3 @test isa(x̂, Vector{Float32}) + mhe4 = setconstraint!(MovingHorizonEstimator(nonlinmodel, He=1, nint_ym=0), v̂max=[50,50]) g_V̂max_end = mhe4.optim[:g_V̂max_2].func # execute update_predictions! branch in `gfunc_i` for coverage: @@ -1027,7 +1049,7 @@ end R̂ = diagm([1, 1].^2) optim = Model(Ipopt.Optimizer) covestim = ExtendedKalmanFilter(nonlinmodel, 1:2, 0, 0, Q̂, Q̂, R̂) - mhe5 = MovingHorizonEstimator(nonlinmodel, 1, 1:2, 0, 0, Q̂, Q̂, R̂, Inf; optim, covestim) + mhe5 = MovingHorizonEstimator(nonlinmodel, 1, 1:2, 0, 0, Q̂, Q̂, R̂; optim, covestim) preparestate!(mhe5, [50, 30], [5]) x̂ = updatestate!(mhe5, [10, 50], [50, 30], [5]) @test x̂ ≈ zeros(4) atol=1e-9