diff --git a/Project.toml b/Project.toml index b5460325..d1dc0525 100644 --- a/Project.toml +++ b/Project.toml @@ -1,7 +1,7 @@ name = "ModelPredictiveControl" uuid = "61f9bdb8-6ae4-484a-811f-bbf86720c31c" authors = ["Francis Gagnon"] -version = "0.23.0" +version = "0.23.1" [deps] ControlSystemsBase = "aaaaaaaa-a6ca-5380-bf3e-84a91bcd477e" diff --git a/README.md b/README.md index 94fa9c69..dc310f02 100644 --- a/README.md +++ b/README.md @@ -126,9 +126,9 @@ for more detailed examples. - [x] manipulated inputs - [x] measured outputs - [x] bumpless manual to automatic transfer for control with a proper initial estimate -- [ ] estimators in two possible forms: +- [x] estimators in two possible forms: + - [x] filter (or current) form to improve accuracy and robustness - [x] predictor (or delayed) form to reduce computational load - - [ ] filter (or current) form to improve accuracy and robustness - [x] moving horizon estimator in two formulations: - [x] linear plant models (quadratic optimization) - [x] nonlinear plant models (nonlinear optimization) diff --git a/docs/src/manual/nonlinmpc.md b/docs/src/manual/nonlinmpc.md index 0434f5b4..8e2ce55f 100644 --- a/docs/src/manual/nonlinmpc.md +++ b/docs/src/manual/nonlinmpc.md @@ -378,7 +378,7 @@ nothing # hide The [`setmodel!`](@ref) method must be called after solving the optimization problem with [`moveinput!`](@ref) and before updating the state estimate with [`updatestate!`](@ref), -that is, when both ``\mathbf{u}(k)`` and ``\mathbf{x̂}_{k-1}(k)`` are available as the new +that is, when both ``\mathbf{u}(k)`` and ``\mathbf{x̂}_{k}(k)`` are available as the new operating point. The [`SimResult`](@ref) object is for plotting purposes only. The adaptive [`LinMPC`](@ref) performances are similar to the nonlinear MPC, both for the 180° setpoint: diff --git a/src/controller/construct.jl b/src/controller/construct.jl index a179b10b..4e1fb86e 100644 --- a/src/controller/construct.jl +++ b/src/controller/construct.jl @@ -94,10 +94,10 @@ LinMPC controller with a sample time Ts = 4.0 s, OSQP optimizer, SteadyKalmanFil Terminal constraints provide closed-loop stability guarantees on the nominal plant model. They can render an unfeasible problem however. In practice, a sufficiently large prediction horizon ``H_p`` without terminal constraints is typically enough for - stability. If `mpc.estim.direct==true`, the state is estimated at ``i = k`` (the current - time step), otherwise at ``i = k - 1``. Note that terminal constraints are applied on the - augmented state vector ``\mathbf{x̂}`` (see [`SteadyKalmanFilter`](@ref) for details on - augmentation). + stability. If `mpc.estim.direct==true`, the estimator computes the states at ``i = k`` + (the current time step), otherwise at ``i = k - 1``. Note that terminal constraints are + applied on the augmented state vector ``\mathbf{x̂}`` (see [`SteadyKalmanFilter`](@ref) + for details on augmentation). For variable constraints, the bounds can be modified after calling [`moveinput!`](@ref), that is, at runtime, but not the softness parameters ``\mathbf{c}``. It is not possible diff --git a/src/estimator/construct.jl b/src/estimator/construct.jl index 92a3bcac..ae45658d 100644 --- a/src/estimator/construct.jl +++ b/src/estimator/construct.jl @@ -146,13 +146,13 @@ end Return empty matrices, and `x̂op` & `f̂op` vectors, if `model` is not a [`LinModel`](@ref). """ function augment_model(model::SimModel{NT}, As, args... ; verify_obsv=false) where NT<:Real - nu, nx, nd = model.nu, model.nx, model.nd + nu, nx, nd, ny = model.nu, model.nx, model.nd, model.ny nxs = size(As, 1) Â = zeros(NT, 0, nx+nxs) B̂u = zeros(NT, 0, nu) - Ĉ = zeros(NT, 0, nx+nxs) + Ĉ = zeros(NT, ny, 0) B̂d = zeros(NT, 0, nd) - D̂d = zeros(NT, 0, nd) + D̂d = zeros(NT, ny, 0) x̂op, f̂op = [model.xop; zeros(nxs)], [model.fop; zeros(nxs)] return Â, B̂u, Ĉ, B̂d, D̂d, x̂op, f̂op end diff --git a/src/estimator/execute.jl b/src/estimator/execute.jl index 4e466cb8..d3866389 100644 --- a/src/estimator/execute.jl +++ b/src/estimator/execute.jl @@ -153,8 +153,8 @@ in which ``\mathbf{Ĉ^m, D̂_d^m}`` are the rows of `estim.Ĉ, estim.D̂d` th measured outputs ``\mathbf{y^m}``. """ function init_estimate!(estim::StateEstimator, ::LinModel, y0m, d0, u0) - Â, B̂u, Ĉ, B̂d, D̂d = estim.Â, estim.B̂u, estim.Ĉ, estim.B̂d, estim.D̂d - Ĉm, D̂dm = @views Ĉ[estim.i_ym, :], D̂d[estim.i_ym, :] # measured outputs ym only + Â, B̂u, B̂d = estim.Â, estim.B̂u, estim.B̂d + Ĉm, D̂dm = estim.Ĉm, estim.D̂dm # TODO: use estim.buffer.x̂ to reduce allocations estim.x̂0 .= [I - Â; Ĉm]\[B̂u*u0 + B̂d*d0 + estim.f̂op - estim.x̂op; y0m - D̂dm*d0] return nothing @@ -390,6 +390,8 @@ function setmodel_estimator!(estim::StateEstimator, model, _ , _ , _ , Q̂, R̂) estim.Ĉ .= Ĉ estim.B̂d .= B̂d estim.D̂d .= D̂d + estim.Ĉm .= @views Ĉ[estim.i_ym, :] + estim.D̂dm .= @views D̂d[estim.i_ym, :] # --- update state estimate and its operating points --- estim.x̂0 .+= estim.x̂op # convert x̂0 to x̂ with the old operating point estim.x̂op .= x̂op diff --git a/src/estimator/internal_model.jl b/src/estimator/internal_model.jl index f26ba5c8..be61da46 100644 --- a/src/estimator/internal_model.jl +++ b/src/estimator/internal_model.jl @@ -16,11 +16,13 @@ struct InternalModel{NT<:Real, SM<:SimModel} <: StateEstimator{NT} Bs::Matrix{NT} Cs::Matrix{NT} Ds::Matrix{NT} - Â ::Matrix{NT} - B̂u::Matrix{NT} - Ĉ ::Matrix{NT} - B̂d::Matrix{NT} - D̂d::Matrix{NT} + Â ::Matrix{NT} + B̂u ::Matrix{NT} + Ĉ ::Matrix{NT} + B̂d ::Matrix{NT} + D̂d ::Matrix{NT} + Ĉm ::Matrix{NT} + D̂dm ::Matrix{NT} Âs::Matrix{NT} B̂s::Matrix{NT} direct::Bool @@ -36,6 +38,7 @@ struct InternalModel{NT<:Real, SM<:SimModel} <: StateEstimator{NT} nxs = size(As,1) nx̂ = model.nx Â, B̂u, Ĉ, B̂d, D̂d, x̂op, f̂op = matrices_internalmodel(model) + Ĉm, D̂dm = Ĉ[i_ym,:], D̂d[i_ym,:] Âs, B̂s = init_internalmodel(As, Bs, Cs, Ds) lastu0 = zeros(NT, nu) # x̂0 and x̂d are same object (updating x̂d will update x̂0): @@ -50,7 +53,7 @@ struct InternalModel{NT<:Real, SM<:SimModel} <: StateEstimator{NT} lastu0, x̂op, f̂op, x̂0, x̂d, x̂s, ŷs, i_ym, nx̂, nym, nyu, nxs, As, Bs, Cs, Ds, - Â, B̂u, Ĉ, B̂d, D̂d, + Â, B̂u, Ĉ, B̂d, D̂d, Ĉm, D̂dm, Âs, B̂s, direct, corrected, buffer @@ -156,8 +159,8 @@ function matrices_internalmodel(model::LinModel) end "Return empty matrices, and `x̂op` & `f̂op` vectors, if `model` is not a [`LinModel`](@ref)." function matrices_internalmodel(model::SimModel{NT}) where NT<:Real - nu, nx, nd = model.nu, model.nx, model.nd - Â, B̂u, Ĉ, B̂d, D̂d = zeros(NT,0,nx), zeros(NT,0,nu), zeros(NT,0,nx), zeros(NT,0,nd), zeros(NT,0,nd) + nu, nx, nd, ny = model.nu, model.nx, model.nd, model.ny + Â, B̂u, Ĉ, B̂d, D̂d = zeros(NT,0,nx), zeros(NT,0,nu), zeros(NT,ny,0), zeros(NT,0,nd), zeros(NT,ny,0) x̂op, f̂op = copy(model.xop), copy(model.fop) return Â, B̂u, Ĉ, B̂d, D̂d, x̂op, f̂op end @@ -225,6 +228,8 @@ function setmodel_estimator!(estim::InternalModel, model, _ , _ , _ , _ , _ ) estim.Ĉ .= Ĉ estim.B̂d .= B̂d estim.D̂d .= D̂d + estim.Ĉm .= @views Ĉ[estim.i_ym,:] + estim.D̂dm .= @views D̂d[estim.i_ym,:] # --- update state estimate and its operating points --- estim.x̂0 .+= estim.x̂op # convert x̂0 to x̂ with the old operating point estim.x̂op .= x̂op diff --git a/src/estimator/kalman.jl b/src/estimator/kalman.jl index ab24db2c..d1bdd3bd 100644 --- a/src/estimator/kalman.jl +++ b/src/estimator/kalman.jl @@ -19,6 +19,8 @@ struct SteadyKalmanFilter{NT<:Real, SM<:LinModel} <: StateEstimator{NT} Ĉ ::Matrix{NT} B̂d ::Matrix{NT} D̂d ::Matrix{NT} + Ĉm ::Matrix{NT} + D̂dm ::Matrix{NT} Q̂::Hermitian{NT, Matrix{NT}} R̂::Hermitian{NT, Matrix{NT}} K̂::Matrix{NT} @@ -34,6 +36,7 @@ struct SteadyKalmanFilter{NT<:Real, SM<:LinModel} <: StateEstimator{NT} nxs = size(As, 1) 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, :] validate_kfcov(nym, nx̂, Q̂, R̂) K̂ = try Q̂_kalman = Matrix(Q̂) # Matrix() required for Julia 1.6 @@ -59,7 +62,7 @@ struct SteadyKalmanFilter{NT<:Real, SM<:LinModel} <: StateEstimator{NT} lastu0, x̂op, f̂op, x̂0, i_ym, nx̂, nym, nyu, nxs, As, Cs_u, Cs_y, nint_u, nint_ym, - Â, B̂u, Ĉ, B̂d, D̂d, + Â, B̂u, Ĉ, B̂d, D̂d, Ĉm, D̂dm, Q̂, R̂, K̂, direct, corrected, @@ -233,7 +236,7 @@ end "Allow code reuse for `SteadyKalmanFilter` and `Luenberger` (observers with constant gain)." function correct_estimate_obsv!(estim::StateEstimator, y0m, d0, K̂) - Ĉm, D̂dm = @views estim.Ĉ[estim.i_ym, :], estim.D̂d[estim.i_ym, :] + Ĉm, D̂dm = estim.Ĉm, estim.D̂dm ŷ0m = @views estim.buffer.ŷ[estim.i_ym] # in-place operations to reduce allocations: mul!(ŷ0m, Ĉm, estim.x̂0) @@ -281,6 +284,8 @@ struct KalmanFilter{NT<:Real, SM<:LinModel} <: StateEstimator{NT} Ĉ ::Matrix{NT} B̂d ::Matrix{NT} D̂d ::Matrix{NT} + Ĉm ::Matrix{NT} + D̂dm ::Matrix{NT} P̂_0::Hermitian{NT, Matrix{NT}} Q̂::Hermitian{NT, Matrix{NT}} R̂::Hermitian{NT, Matrix{NT}} @@ -297,6 +302,7 @@ struct KalmanFilter{NT<:Real, SM<:LinModel} <: StateEstimator{NT} nxs = size(As, 1) 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, :] validate_kfcov(nym, nx̂, Q̂, R̂, P̂_0) lastu0 = zeros(NT, nu) x̂0 = [zeros(NT, model.nx); zeros(NT, nxs)] @@ -311,7 +317,7 @@ struct KalmanFilter{NT<:Real, SM<:LinModel} <: StateEstimator{NT} lastu0, x̂op, f̂op, x̂0, P̂, i_ym, nx̂, nym, nyu, nxs, As, Cs_u, Cs_y, nint_u, nint_ym, - Â, B̂u, Ĉ, B̂d, D̂d, + Â, B̂u, Ĉ, B̂d, D̂d, Ĉm, D̂dm, P̂_0, Q̂, R̂, K̂, direct, corrected, @@ -408,8 +414,7 @@ It computes the corrected state estimate ``\mathbf{x̂}_{k}(k)`` estimation cova ``\mathbf{P̂}_{k}(k)``. """ function correct_estimate!(estim::KalmanFilter, y0m, d0) - Ĉm = @views estim.Ĉ[estim.i_ym, :] - return correct_estimate_kf!(estim, y0m, d0, Ĉm) + return correct_estimate_kf!(estim, y0m, d0, estim.Ĉm) end @@ -427,8 +432,8 @@ provided below, see [^2] for details. # Correction Step ```math \begin{aligned} - \mathbf{Ŝ}(k) &= \mathbf{Ĉ^m P̂}_{k-1}(k)\mathbf{Ĉ^m}' + \mathbf{R̂} \\ - \mathbf{K̂}(k) &= \mathbf{P̂}_{k-1}(k)\mathbf{Ĉ^m}'\mathbf{Ŝ^{-1}}(k) \\ + \mathbf{M̂}(k) &= \mathbf{Ĉ^m P̂}_{k-1}(k)\mathbf{Ĉ^m}' + \mathbf{R̂} \\ + \mathbf{K̂}(k) &= \mathbf{P̂}_{k-1}(k)\mathbf{Ĉ^m}'\mathbf{M̂^{-1}}(k) \\ \mathbf{ŷ^m}(k) &= \mathbf{Ĉ^m x̂}_{k-1}(k) + \mathbf{D̂_d^m d}(k) \\ \mathbf{x̂}_{k}(k) &= \mathbf{x̂}_{k-1}(k) + \mathbf{K̂}(k)[\mathbf{y^m}(k) - \mathbf{ŷ^m}(k)] \\ \mathbf{P̂}_{k}(k) &= [\mathbf{I - K̂}(k)\mathbf{Ĉ^m}]\mathbf{P̂}_{k-1}(k) @@ -447,11 +452,10 @@ provided below, see [^2] for details. , Accessed 2024-08-08. """ function update_estimate!(estim::KalmanFilter, y0m, d0, u0) - Ĉm = @views estim.Ĉ[estim.i_ym, :] if !estim.direct - correct_estimate_kf!(estim, y0m, d0, Ĉm) + correct_estimate_kf!(estim, y0m, d0, estim.Ĉm) end - return predict_estimate_kf!(estim, y0m, d0, u0, Ĉm, estim.Â) + return predict_estimate_kf!(estim, u0, d0, estim.Â) end @@ -472,19 +476,22 @@ struct UnscentedKalmanFilter{NT<:Real, SM<:SimModel} <: StateEstimator{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} + Â ::Matrix{NT} + B̂u ::Matrix{NT} + Ĉ ::Matrix{NT} + B̂d ::Matrix{NT} + D̂d ::Matrix{NT} + Ĉm ::Matrix{NT} + D̂dm ::Matrix{NT} P̂_0::Hermitian{NT, Matrix{NT}} Q̂::Hermitian{NT, Matrix{NT}} R̂::Hermitian{NT, Matrix{NT}} K̂::Matrix{NT} M̂::Hermitian{NT, Matrix{NT}} X̂0::Matrix{NT} + X̄0::Matrix{NT} Ŷ0m::Matrix{NT} - sqrtP̂::LowerTriangular{NT, Matrix{NT}} + Ȳ0m::Matrix{NT} nσ::Int γ::NT m̂::Vector{NT} @@ -501,6 +508,7 @@ struct UnscentedKalmanFilter{NT<:Real, SM<:SimModel} <: StateEstimator{NT} nxs = size(As, 1) 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, :] validate_kfcov(nym, nx̂, Q̂, R̂, P̂_0) nσ, γ, m̂, Ŝ = init_ukf(model, nx̂, α, β, κ) lastu0 = zeros(NT, nu) @@ -510,8 +518,8 @@ struct UnscentedKalmanFilter{NT<:Real, SM<:SimModel} <: StateEstimator{NT} P̂ = copy(P̂_0) K̂ = zeros(NT, nx̂, nym) M̂ = Hermitian(zeros(NT, nym, nym), :L) - X̂0, Ŷ0m = zeros(NT, nx̂, nσ), zeros(NT, nym, nσ) - sqrtP̂ = LowerTriangular(zeros(NT, nx̂, nx̂)) + X̂0, X̄0 = zeros(NT, nx̂, nσ), zeros(NT, nx̂, nσ) + Ŷ0m, Ȳ0m = zeros(NT, nym, nσ), zeros(NT, nym, nσ) corrected = [false] buffer = StateEstimatorBuffer{NT}(nu, nx̂, nym, ny, nd) return new{NT, SM}( @@ -519,9 +527,10 @@ struct UnscentedKalmanFilter{NT<:Real, SM<:SimModel} <: StateEstimator{NT} lastu0, x̂op, f̂op, x̂0, P̂, i_ym, nx̂, nym, nyu, nxs, As, Cs_u, Cs_y, nint_u, nint_ym, - Â, B̂u, Ĉ, B̂d, D̂d, + Â, B̂u, Ĉ, B̂d, D̂d, Ĉm, D̂dm, P̂_0, Q̂, R̂, - K̂, M̂, X̂0, Ŷ0m, sqrtP̂, + K̂, + M̂, X̂0, X̄0, Ŷ0m, Ȳ0m, nσ, γ, m̂, Ŝ, direct, corrected, buffer @@ -676,38 +685,50 @@ end Do the same but for the [`UnscentedKalmanFilter`](@ref). """ function correct_estimate!(estim::UnscentedKalmanFilter, y0m, d0) - x̂0, P̂, R̂, K̂, M̂ = estim.x̂0, estim.P̂, estim.R̂, estim.K̂, estim.M̂ + x̂0, P̂, R̂, K̂ = estim.x̂0, estim.P̂, estim.R̂, estim.K̂ nx̂ = estim.nx̂ γ, m̂, Ŝ = estim.γ, estim.m̂, estim.Ŝ + # in-place operations to reduce allocations: + P̂_temp = Hermitian(estim.buffer.P̂, :L) + P̂_temp .= P̂ + P̂_chol = cholesky!(P̂_temp) # also modifies P̂_temp + sqrtP̂ = P̂_chol.L + γ_sqrtP̂ = lmul!(γ, sqrtP̂) X̂0, Ŷ0m = estim.X̂0, estim.Ŷ0m - sqrtP̂ = estim.sqrtP̂ - ŷ0 = estim.buffer.ŷ - P̂_chol = sqrtP̂.data - P̂_chol .= P̂ - cholesky!(Hermitian(P̂_chol, :L)) # also modifies sqrtP̂ - γ_sqrtP̂ = lmul!(γ, sqrtP̂) X̂0 .= x̂0 - X̂0[:, 2:nx̂+1] .+= γ_sqrtP̂ - X̂0[:, nx̂+2:end] .-= γ_sqrtP̂ + X̂0[:, 2:nx̂+1] .= @views X̂0[:, 2:nx̂+1] .+ γ_sqrtP̂ + X̂0[:, nx̂+2:end] .= @views X̂0[:, nx̂+2:end] .- γ_sqrtP̂ + ŷ0 = estim.buffer.ŷ for j in axes(Ŷ0m, 2) @views ĥ!(ŷ0, estim, estim.model, X̂0[:, j], d0) @views Ŷ0m[:, j] .= ŷ0[estim.i_ym] end ŷ0m = @views ŷ0[estim.i_ym] mul!(ŷ0m, Ŷ0m, m̂) - X̄, Ȳm = X̂0, Ŷ0m - X̄ .= X̂0 .- x̂0 - Ȳm .= Ŷ0m .- ŷ0m - # TODO: use estim.buffer.R̂ here to reduce allocations - M̂.data .= Ȳm * Ŝ * Ȳm' .+ R̂ - mul!(K̂, X̄, lmul!(Ŝ, Ȳm')) - rdiv!(K̂, cholesky(M̂)) - v̂ = ŷ0m + X̄0, Ȳ0m = estim.X̄0, estim.Ȳ0m + X̄0 .= X̂0 .- x̂0 + Ȳ0m .= Ŷ0m .- ŷ0m + Ŝ_Ŷ0mᵀ = estim.Ŷ0m' + mul!(Ŝ_Ŷ0mᵀ, Ŝ, Ȳ0m') + M̂ = estim.buffer.R̂ + mul!(M̂, Ȳ0m, Ŝ_Ŷ0mᵀ) + M̂ .+= R̂ + M̂ = Hermitian(M̂, :L) + estim.M̂ .= M̂ + mul!(K̂, X̄0, Ŝ_Ŷ0mᵀ) + rdiv!(K̂, cholesky!(M̂)) # also modifies M̂ (estim.M̂ contains unmodified M̂, see line below) + M̂ = estim.M̂ + v̂ = ŷ0m v̂ .= y0m .- ŷ0m x̂0corr, P̂corr = estim.x̂0, estim.P̂ mul!(x̂0corr, K̂, v̂, 1, 1) - # TODO: use estim.buffer.P̂ and estim.buffer.Q̂ here to reduce allocations - P̂corr .= Hermitian(P̂ .- K̂ * M̂ * K̂', :L) + K̂_M̂ = estim.buffer.K̂ + mul!(K̂_M̂, K̂, M̂) + K̂_M̂_K̂ᵀ = estim.buffer.Q̂ + mul!(K̂_M̂_K̂ᵀ, K̂_M̂, K̂') + P̂corr = estim.buffer.P̂ + P̂corr .= P̂ .- Hermitian(K̂_M̂_K̂ᵀ, :L) + estim.P̂ .= Hermitian(P̂corr, :L) return nothing end @@ -759,31 +780,35 @@ function update_estimate!(estim::UnscentedKalmanFilter, y0m, d0, u0) if !estim.direct correct_estimate!(estim, y0m, d0) end - x̂0corr, X̂0corr, P̂corr, sqrtP̂corr = estim.x̂0, estim.X̂0, estim.P̂, estim.sqrtP̂ + x̂0corr, X̂0corr, P̂corr = estim.x̂0, estim.X̂0, estim.P̂ Q̂, nx̂ = estim.Q̂, estim.nx̂ γ, m̂, Ŝ = estim.γ, estim.m̂, estim.Ŝ x̂0next, û0 = estim.buffer.x̂, estim.buffer.û - P̂cor_chol = sqrtP̂corr.data - P̂cor_chol .= P̂corr - cholesky!(Hermitian(P̂cor_chol, :L)) # also modifies sqrtP̂cor + # in-place operations to reduce allocations: + P̂corr_temp = Hermitian(estim.buffer.P̂, :L) + P̂corr_temp .= P̂corr + P̂corr_chol = cholesky!(P̂corr_temp) # also modifies P̂corr_temp + sqrtP̂corr = P̂corr_chol.L γ_sqrtP̂corr = lmul!(γ, sqrtP̂corr) X̂0corr .= x̂0corr - X̂0corr[:, 2:nx̂+1] .+= γ_sqrtP̂corr - X̂0corr[:, nx̂+2:end] .-= γ_sqrtP̂corr + X̂0corr[:, 2:nx̂+1] .= @views X̂0corr[:, 2:nx̂+1] .+ γ_sqrtP̂corr + X̂0corr[:, nx̂+2:end] .= @views X̂0corr[:, nx̂+2:end] .- γ_sqrtP̂corr X̂0next = X̂0corr for j in axes(X̂0next, 2) @views x̂0corr .= X̂0corr[:, j] @views f̂!(X̂0next[:, j], û0, estim, estim.model, x̂0corr, u0, d0) end x̂0next .= mul!(x̂0corr, X̂0next, m̂) - X̄next = X̂0next - X̄next .= X̂0next .- x̂0next - P̂next = P̂corr - # TODO: use estim.buffer.P̂ and estim.buffer.Q̂ here to reduce allocations - P̂next.data .= X̄next * Ŝ * X̄next' .+ Q̂ + X̄0next = estim.X̄0 + X̄0next .= X̂0next .- x̂0next + Ŝ_X̄0nextᵀ = estim.X̂0' + mul!(Ŝ_X̄0nextᵀ, Ŝ, X̄0next') + P̂next = estim.buffer.P̂ + mul!(P̂next, X̄0next, Ŝ_X̄0nextᵀ) + P̂next .+= Q̂ x̂0next .+= estim.f̂op .- estim.x̂op estim.x̂0 .= x̂0next - estim.P̂ .= P̂next + estim.P̂ .= Hermitian(P̂next, :L) return nothing end @@ -804,17 +829,21 @@ struct ExtendedKalmanFilter{NT<:Real, SM<:SimModel} <: StateEstimator{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} + Â ::Matrix{NT} + B̂u ::Matrix{NT} + Ĉ ::Matrix{NT} + B̂d ::Matrix{NT} + D̂d ::Matrix{NT} + Ĉm ::Matrix{NT} + D̂dm ::Matrix{NT} P̂_0::Hermitian{NT, Matrix{NT}} Q̂::Hermitian{NT, Matrix{NT}} R̂::Hermitian{NT, Matrix{NT}} K̂::Matrix{NT} F̂_û::Matrix{NT} + F̂ ::Matrix{NT} Ĥ ::Matrix{NT} + Ĥm ::Matrix{NT} direct::Bool corrected::Vector{Bool} buffer::StateEstimatorBuffer{NT} @@ -827,6 +856,7 @@ struct ExtendedKalmanFilter{NT<:Real, SM<:SimModel} <: StateEstimator{NT} nxs = size(As, 1) 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, :] validate_kfcov(nym, nx̂, Q̂, R̂, P̂_0) lastu0 = zeros(NT, nu) x̂0 = [zeros(NT, model.nx); zeros(NT, nxs)] @@ -835,7 +865,8 @@ struct ExtendedKalmanFilter{NT<:Real, SM<:SimModel} <: StateEstimator{NT} R̂ = Hermitian(R̂, :L) P̂ = copy(P̂_0) K̂ = zeros(NT, nx̂, nym) - F̂_û, Ĥ = zeros(NT, nx̂+nu, nx̂), zeros(NT, ny, nx̂) + F̂_û, F̂ = zeros(NT, nx̂+nu, nx̂), zeros(NT, nx̂, nx̂) + Ĥ, Ĥm = zeros(NT, ny, nx̂), zeros(NT, nym, nx̂) corrected = [false] buffer = StateEstimatorBuffer{NT}(nu, nx̂, nym, ny, nd) return new{NT, SM}( @@ -843,10 +874,10 @@ struct ExtendedKalmanFilter{NT<:Real, SM<:SimModel} <: StateEstimator{NT} lastu0, x̂op, f̂op, x̂0, P̂, i_ym, nx̂, nym, nyu, nxs, As, Cs_u, Cs_y, nint_u, nint_ym, - Â, B̂u, Ĉ, B̂d, D̂d, + Â, B̂u, Ĉ, B̂d, D̂d, Ĉm, D̂dm, P̂_0, Q̂, R̂, K̂, - F̂_û, Ĥ, + F̂_û, F̂, Ĥ, Ĥm, direct, corrected, buffer ) @@ -937,8 +968,8 @@ function correct_estimate!(estim::ExtendedKalmanFilter, y0m, d0) ŷ0 = estim.buffer.ŷ ĥAD! = (ŷ0, x̂0) -> ĥ!(ŷ0, estim, model, x̂0, d0) ForwardDiff.jacobian!(estim.Ĥ, ĥAD!, ŷ0, x̂0) - Ĥm = @views estim.Ĥ[estim.i_ym, :] - return correct_estimate_kf!(estim, y0m, d0, Ĥm) + estim.Ĥm .= @views estim.Ĥ[estim.i_ym, :] + return correct_estimate_kf!(estim, y0m, d0, estim.Ĥm) end @@ -983,15 +1014,12 @@ The correction step is skipped if `estim.direct == true` since it's already done function update_estimate!(estim::ExtendedKalmanFilter{NT}, y0m, d0, u0) where NT<:Real model, x̂0 = estim.model, estim.x̂0 nx̂, nu = estim.nx̂, model.nu - Ĥ = estim.Ĥ if !estim.direct ŷ0 = estim.buffer.ŷ ĥAD! = (ŷ0, x̂0) -> ĥ!(ŷ0, estim, model, x̂0, d0) - ForwardDiff.jacobian!(Ĥ, ĥAD!, ŷ0, x̂0) - Ĥm = @views Ĥ[estim.i_ym, :] - correct_estimate_kf!(estim, y0m, d0, Ĥm) - else - Ĥm = @views Ĥ[estim.i_ym, :] + ForwardDiff.jacobian!(estim.Ĥ, ĥAD!, ŷ0, x̂0) + estim.Ĥm .= @views estim.Ĥ[estim.i_ym, :] + correct_estimate_kf!(estim, y0m, d0, estim.Ĥm) end x̂0corr = estim.x̂0 # concatenate x̂0next and û0 vectors to allows û0 vector with dual numbers for AD: @@ -1001,8 +1029,8 @@ function update_estimate!(estim::ExtendedKalmanFilter{NT}, y0m, d0, u0) where NT x̂0nextû[1:nx̂], x̂0nextû[nx̂+1:end], estim, model, x̂0corr, u0, d0 ) ForwardDiff.jacobian!(estim.F̂_û, f̂AD!, x̂0nextû, x̂0corr) - F̂ = @views estim.F̂_û[1:estim.nx̂, :] - return predict_estimate_kf!(estim, y0m, d0, u0, Ĥm, F̂) + estim.F̂ .= @views estim.F̂_û[1:estim.nx̂, :] + return predict_estimate_kf!(estim, u0, d0, estim.F̂) end "Set `estim.P̂` to `estim.P̂_0` for the time-varying Kalman Filters." @@ -1045,11 +1073,11 @@ function correct_estimate_kf!(estim::Union{KalmanFilter, ExtendedKalmanFilter}, # in-place operations to reduce allocations: P̂_Ĉmᵀ = K̂ mul!(P̂_Ĉmᵀ, P̂.data, Ĉm') # the ".data" weirdly removes a type instability in mul! - Ŝ = estim.buffer.R̂ - mul!(Ŝ, Ĉm, P̂_Ĉmᵀ) - Ŝ .+= R̂ + M̂ = estim.buffer.R̂ + mul!(M̂, Ĉm, P̂_Ĉmᵀ) + M̂ .+= R̂ K̂ = P̂_Ĉmᵀ - M̂_chol = cholesky!(Hermitian(Ŝ)) # also modifies Ŝ + M̂_chol = cholesky!(Hermitian(M̂, :L)) # also modifies M̂ rdiv!(K̂, M̂_chol) ŷ0 = estim.buffer.ŷ ĥ!(ŷ0, estim, estim.model, x̂0, d0) @@ -1061,7 +1089,9 @@ function correct_estimate_kf!(estim::Union{KalmanFilter, ExtendedKalmanFilter}, I_minus_K̂_Ĉm = estim.buffer.Q̂ mul!(I_minus_K̂_Ĉm, K̂, Ĉm) lmul!(-1, I_minus_K̂_Ĉm) - I_minus_K̂_Ĉm[diagind(I_minus_K̂_Ĉm)] .+= 1 # compute I - K̂*Ĉm in-place + for i=diagind(I_minus_K̂_Ĉm) + I_minus_K̂_Ĉm[i] += 1 # compute I - K̂*Ĉm in-place + end P̂corr = estim.buffer.P̂ mul!(P̂corr, I_minus_K̂_Ĉm, P̂) estim.P̂ .= Hermitian(P̂corr, :L) @@ -1069,7 +1099,7 @@ function correct_estimate_kf!(estim::Union{KalmanFilter, ExtendedKalmanFilter}, end """ - predict_estimate_kf!(estim::Union{KalmanFilter, ExtendedKalmanFilter}, y0m, d0, u0, Ĉm, Â) + predict_estimate_kf!(estim::Union{KalmanFilter, ExtendedKalmanFilter}, u0, d0, Â) Predict time-varying/extended Kalman Filter estimates with augmented `Ĉm` and `Â` matrices. @@ -1077,14 +1107,14 @@ Allows code reuse for [`KalmanFilter`](@ref), [`ExtendedKalmanFilterKalmanFilter They predict the state `x̂` and covariance `P̂` with the same equations. See [`update_estimate`](@ref) methods for the equations. """ -function predict_estimate_kf!(estim::Union{KalmanFilter, ExtendedKalmanFilter}, y0m, d0, u0, Ĉm, Â) +function predict_estimate_kf!(estim::Union{KalmanFilter, ExtendedKalmanFilter}, u0, d0, Â) x̂0corr, P̂corr = estim.x̂0, estim.P̂ Q̂ = estim.Q̂ x̂0next, û0 = estim.buffer.x̂, estim.buffer.û # in-place operations to reduce allocations: f̂!(x̂0next, û0, estim, estim.model, x̂0corr, u0, d0) P̂corr_Âᵀ = estim.buffer.P̂ - mul!(P̂corr_Âᵀ, P̂corr, Â') + mul!(P̂corr_Âᵀ, P̂corr.data, Â') # the ".data" weirdly removes a type instability in mul! Â_P̂corr_Âᵀ = estim.buffer.Q̂ mul!(Â_P̂corr_Âᵀ, Â, P̂corr_Âᵀ) P̂next = estim.buffer.P̂ diff --git a/src/estimator/luenberger.jl b/src/estimator/luenberger.jl index 872f9e9a..1215caab 100644 --- a/src/estimator/luenberger.jl +++ b/src/estimator/luenberger.jl @@ -19,6 +19,8 @@ struct Luenberger{NT<:Real, SM<:LinModel} <: StateEstimator{NT} Ĉ ::Matrix{NT} B̂d ::Matrix{NT} D̂d ::Matrix{NT} + Ĉm ::Matrix{NT} + D̂dm ::Matrix{NT} K̂::Matrix{NT} direct::Bool corrected::Vector{Bool} @@ -33,6 +35,7 @@ struct Luenberger{NT<:Real, SM<:LinModel} <: StateEstimator{NT} nxs = size(As, 1) 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, :] K̂ = try ControlSystemsBase.place(Â, Ĉ, poles, :o; direct)[:, i_ym] catch @@ -47,7 +50,7 @@ struct Luenberger{NT<:Real, SM<:LinModel} <: StateEstimator{NT} lastu0, x̂op, f̂op, x̂0, i_ym, nx̂, nym, nyu, nxs, As, Cs_u, Cs_y, nint_u, nint_ym, - Â, B̂u, Ĉ, B̂d, D̂d, + Â, B̂u, Ĉ, B̂d, D̂d, Ĉm, D̂dm, K̂, direct, corrected, buffer @@ -111,7 +114,7 @@ end """ - prepare_estimate_obsv!(estim::Luenberger, y0m, d0, _ ) + correct_estimate!(estim::Luenberger, y0m, d0, _ ) Identical to [`correct_estimate!(::SteadyKalmanFilter)`](@ref) but using [`Luenberger`](@ref). """ diff --git a/src/estimator/mhe/construct.jl b/src/estimator/mhe/construct.jl index 6f4144a6..6f2949ff 100644 --- a/src/estimator/mhe/construct.jl +++ b/src/estimator/mhe/construct.jl @@ -76,6 +76,8 @@ struct MovingHorizonEstimator{ Ĉ ::Matrix{NT} B̂d ::Matrix{NT} D̂d ::Matrix{NT} + Ĉm ::Matrix{NT} + D̂dm ::Matrix{NT} Ẽ ::Matrix{NT} F ::Vector{NT} G ::Matrix{NT} @@ -120,6 +122,7 @@ struct MovingHorizonEstimator{ nxs = size(As, 1) 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, :] validate_kfcov(nym, nx̂, Q̂, R̂, P̂_0) lastu0 = zeros(NT, nu) x̂0 = [zeros(NT, model.nx); zeros(NT, nxs)] @@ -129,7 +132,7 @@ struct MovingHorizonEstimator{ invQ̂_He = Hermitian(repeatdiag(inv(Q̂), He), :L) invR̂_He = Hermitian(repeatdiag(inv(R̂), He), :L) E, G, J, B, ex̄, Ex̂, Gx̂, Jx̂, Bx̂ = init_predmat_mhe( - model, He, i_ym, Â, B̂u, Ĉ, B̂d, D̂d, x̂op, f̂op + model, He, i_ym, Â, B̂u, Ĉm, B̂d, D̂dm, x̂op, f̂op ) # dummy values (updated just before optimization): F, fx̄, Fx̂ = zeros(NT, nym*He), zeros(NT, nx̂), zeros(NT, nx̂*He) @@ -155,7 +158,7 @@ struct MovingHorizonEstimator{ He, nϵ, i_ym, nx̂, nym, nyu, nxs, As, Cs_u, Cs_y, nint_u, nint_ym, - Â, B̂u, Ĉ, B̂d, D̂d, + Â, B̂u, Ĉ, B̂d, D̂d, Ĉm, D̂dm, Ẽ, F, G, J, B, ẽx̄, fx̄, H̃, q̃, p, P̂_0, Q̂, R̂, invP̄, invQ̂_He, invR̂_He, Cwt, @@ -891,7 +894,7 @@ end @doc raw""" init_predmat_mhe( - model::LinModel, He, i_ym, Â, B̂u, Ĉ, B̂d, D̂d, x̂op, f̂op + model::LinModel, He, i_ym, Â, B̂u, Ĉm, B̂d, D̂dm, x̂op, f̂op ) -> E, G, J, B, ex̄, Ex̂, Gx̂, Jx̂, Bx̂ Construct the MHE prediction matrices for [`LinModel`](@ref) `model`. @@ -989,11 +992,10 @@ calculated with: All these matrices are truncated when ``N_k < H_e`` (at the beginning). """ function init_predmat_mhe( - model::LinModel{NT}, He, i_ym, Â, B̂u, Ĉ, B̂d, D̂d, x̂op, f̂op + model::LinModel{NT}, He, i_ym, Â, B̂u, Ĉm, B̂d, D̂dm, x̂op, f̂op ) where {NT<:Real} nu, nd = model.nu, model.nd nym, nx̂ = length(i_ym), size(Â, 2) - Ĉm, D̂dm = Ĉ[i_ym,:], D̂d[i_ym,:] # measured outputs ym only nŵ = nx̂ # --- pre-compute matrix powers --- # Apow 3D array : Apow[:,:,1] = A^0, Apow[:,:,2] = A^1, ... , Apow[:,:,He+1] = A^He diff --git a/src/estimator/mhe/execute.jl b/src/estimator/mhe/execute.jl index 6afe507a..9d0bcc4f 100644 --- a/src/estimator/mhe/execute.jl +++ b/src/estimator/mhe/execute.jl @@ -496,6 +496,8 @@ function setmodel_estimator!( estim.Ĉ .= Ĉ estim.B̂d .= B̂d estim.D̂d .= D̂d + estim.Ĉm .= @views Ĉ[estim.i_ym, :] + estim.D̂dm .= @views D̂d[estim.i_ym, :] # --- update state estimate and its operating points --- x̂op_old = copy(estim.x̂op) estim.x̂0 .+= estim.x̂op # convert x̂0 to x̂ with the old operating point @@ -504,7 +506,9 @@ function setmodel_estimator!( estim.x̂0 .-= estim.x̂op # convert x̂ to x̂0 with the new operating point # --- predictions matrices --- E, G, J, B, _ , Ex̂, Gx̂, Jx̂, Bx̂ = init_predmat_mhe( - model, He, estim.i_ym, Â, B̂u, Ĉ, B̂d, D̂d, x̂op, f̂op + model, He, estim.i_ym, + estim.Â, estim.B̂u, estim.Ĉm, estim.B̂d, estim.D̂dm, + estim.x̂op, estim.f̂op ) A_X̂min, A_X̂max, Ẽx̂ = relaxX̂(model, nϵ, con.C_x̂min, con.C_x̂max, Ex̂) A_V̂min, A_V̂max, Ẽ = relaxV̂(model, nϵ, con.C_v̂min, con.C_v̂max, E) diff --git a/src/state_estim.jl b/src/state_estim.jl index fde29ce1..0c801ebe 100644 --- a/src/state_estim.jl +++ b/src/state_estim.jl @@ -25,6 +25,7 @@ struct StateEstimatorBuffer{NT<:Real} P̂ ::Matrix{NT} Q̂ ::Matrix{NT} R̂ ::Matrix{NT} + K̂ ::Matrix{NT} ym::Vector{NT} ŷ ::Vector{NT} d ::Vector{NT} @@ -47,11 +48,12 @@ function StateEstimatorBuffer{NT}( P̂ = Matrix{NT}(undef, nx̂, nx̂) Q̂ = Matrix{NT}(undef, nx̂, nx̂) R̂ = Matrix{NT}(undef, nym, nym) + K̂ = Matrix{NT}(undef, nx̂, nym) ym = Vector{NT}(undef, nym) ŷ = Vector{NT}(undef, ny) d = Vector{NT}(undef, nd) empty = Vector{NT}(undef, 0) - return StateEstimatorBuffer{NT}(u, û, x̂, P̂, Q̂, R̂, ym, ŷ, d, empty) + return StateEstimatorBuffer{NT}(u, û, x̂, P̂, Q̂, R̂, K̂, ym, ŷ, d, empty) end const IntVectorOrInt = Union{Int, Vector{Int}}