From 373d663d3109322e8444a2a680c55ec0364100e2 Mon Sep 17 00:00:00 2001 From: franckgaga Date: Sat, 27 Apr 2024 13:06:38 -0400 Subject: [PATCH 1/2] doc: clarify notation `update_estimate!` and imc block diagram --- docs/src/assets/imc.svg | 844 ++++++++++++++++++++++++++++++ docs/src/internals/state_estim.md | 9 +- src/controller/execute.jl | 42 +- src/estimator/execute.jl | 8 +- src/estimator/internal_model.jl | 21 +- src/estimator/kalman.jl | 157 +++--- src/estimator/luenberger.jl | 26 +- src/estimator/mhe/construct.jl | 1 - src/estimator/mhe/execute.jl | 11 +- 9 files changed, 988 insertions(+), 131 deletions(-) create mode 100644 docs/src/assets/imc.svg diff --git a/docs/src/assets/imc.svg b/docs/src/assets/imc.svg new file mode 100644 index 000000000..29be2d731 --- /dev/null +++ b/docs/src/assets/imc.svg @@ -0,0 +1,844 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/docs/src/internals/state_estim.md b/docs/src/internals/state_estim.md index 90cfa4193..5cda63b99 100644 --- a/docs/src/internals/state_estim.md +++ b/docs/src/internals/state_estim.md @@ -55,9 +55,12 @@ ModelPredictiveControl.init_estimate! ## Update Estimate !!! info - All these methods assume that the operating points are already removed in `u`, `ym` - and `d` arguments. Strickly speaking, the arguments should be called `u0`, `ym0` and - `d0`, following [`setop!`](@ref) notation. The `0` is dropped to simplify the notation. + All these methods assume that the `u0`, `y0m` and `d0` arguments are deviation vectors + from their respective operating points (see [`setop!`](@ref)). The associated equations + in the documentation drops the ``\mathbf{0}`` in subscript to simplify the notation. + Strictly speaking, the manipulated inputs, measured outputs, measured disturbances and + estimated states should be denoted with ``\mathbf{u_0, y_0^m, d_0}`` and + ``\mathbf{x̂_0}``, respectively. ```@docs ModelPredictiveControl.update_estimate! diff --git a/src/controller/execute.jl b/src/controller/execute.jl index fd2ffbc12..c727f6154 100644 --- a/src/controller/execute.jl +++ b/src/controller/execute.jl @@ -348,30 +348,33 @@ function predict!(Ŷ0, x̂0, x̂0next, u0, û0, mpc::PredictiveController, mod end """ - obj_nonlinprog!(U0 , _ , _ , mpc::PredictiveController, model::LinModel, Ŷ0, ΔŨ) + obj_nonlinprog!(U0 , Ȳ, _ , mpc::PredictiveController, model::LinModel, Ŷ0, ΔŨ) Nonlinear programming objective function when `model` is a [`LinModel`](@ref). The function is called by the nonlinear optimizer of [`NonLinMPC`](@ref) controllers. It can also be called on any [`PredictiveController`](@ref)s to evaluate the objective function `J` -at specific input increments `ΔŨ` and predictions `Ŷ0` values. It mutates the `U0` argument. +at specific input increments `ΔŨ` and predictions `Ŷ0` values. It mutates the `U0` and +`Ȳ` arguments. """ function obj_nonlinprog!( - U0, _ , _ , mpc::PredictiveController, model::LinModel, Ŷ0, ΔŨ::AbstractVector{NT} + U0, Ȳ, _ , mpc::PredictiveController, model::LinModel, Ŷ0, ΔŨ::AbstractVector{NT} ) where NT <: Real J = obj_quadprog(ΔŨ, mpc.H̃, mpc.q̃) + mpc.p[] if !iszero(mpc.E) - ny, Hp, D̂E = model.ny, mpc.Hp, mpc.D̂E - U = U0 - mul!(U, mpc.S̃, ΔŨ) - U .+= mpc.T_lastu0 .+ mpc.Uop - UE = @views [U; U[(end - model.nu + 1):end]] - ŶE = Vector{NT}(undef, ny + ny*Hp) - ŶE[1:ny] .= mpc.ŷ - ŶE[1+ny:end] .= Ŷ0 .+ mpc.Yop - J += mpc.E*mpc.JE(UE, ŶE, D̂E) + ny, Hp, ŷ, D̂E = model.ny, mpc.Hp, mpc.ŷ, mpc.D̂E + U = U0 + U .+= mpc.Uop + uend = @views U[(end-model.nu+1):end] + Ŷ = Ȳ + Ŷ .= Ŷ0 .+ mpc.Yop + UE = [U; uend] + ŶE = [ŷ; Ŷ] + E_JE = mpc.E*mpc.JE(UE, ŶE, D̂E) + else + E_JE = 0.0 end - return J + return J + E_JE end """ @@ -403,13 +406,14 @@ function obj_nonlinprog!( end # --- economic term --- if !iszero(mpc.E) - ny, Hp, D̂E = model.ny, mpc.Hp, mpc.D̂E + ny, Hp, ŷ, D̂E = model.ny, mpc.Hp, mpc.ŷ, mpc.D̂E U = U0 - U .+= mpc.Uop - UE = @views [U; U[(end - model.nu + 1):end]] - ŶE = Vector{NT}(undef, ny + ny*Hp) - ŶE[1:ny] .= mpc.ŷ - ŶE[1+ny:end] .= Ŷ0 .+ mpc.Yop + U .+= mpc.Uop + uend = @views U[(end-model.nu+1):end] + Ŷ = Ȳ + Ŷ .= Ŷ0 .+ mpc.Yop + UE = [U; uend] + ŶE = [ŷ; Ŷ] E_JE = mpc.E*mpc.JE(UE, ŶE, D̂E) else E_JE = 0.0 diff --git a/src/estimator/execute.jl b/src/estimator/execute.jl index 09191938f..a062513de 100644 --- a/src/estimator/execute.jl +++ b/src/estimator/execute.jl @@ -245,10 +245,10 @@ Set model and operating points of `estim` [`StateEstimator`](@ref) to `model` va Allows model adaptation of estimators based on [`LinModel`](@ref) at runtime ([`NonLinModel`](@ref) is not supported). Not supported by [`Luenberger`](@ref) and [`SteadyKalmanFilter`](@ref), -use the time-varying [`KalmanFilter`](@ref) instead. The [`MovingHorizonEstimator`] model -is kept constant over the estimation horizon ``H_e``. The matrix dimensions and sample time -must stay the same. Note that the observability and controllability of the new augmented -model is not verified (see Extended Help for details). +use the time-varying [`KalmanFilter`](@ref) instead. The [`MovingHorizonEstimator`](@ref) +model is kept constant over the estimation horizon ``H_e``. The matrix dimensions and sample +time must stay the same. Note that the observability and controllability of the new +augmented model is not verified (see Extended Help for more info). # Examples ```jldoctest diff --git a/src/estimator/internal_model.jl b/src/estimator/internal_model.jl index e118fa363..6161691ac 100644 --- a/src/estimator/internal_model.jl +++ b/src/estimator/internal_model.jl @@ -56,7 +56,7 @@ Construct an internal model estimator based on `model` ([`LinModel`](@ref) or [` unmeasured ``\mathbf{y^u}``. `model` evaluates the deterministic predictions ``\mathbf{ŷ_d}``, and `stoch_ym`, the stochastic predictions of the measured outputs ``\mathbf{ŷ_s^m}`` (the unmeasured ones being ``\mathbf{ŷ_s^u=0}``). The predicted outputs -sum both values : ``\mathbf{ŷ = ŷ_d + ŷ_s}``. +sum both values : ``\mathbf{ŷ = ŷ_d + ŷ_s}``. See the Extended Help for more details. !!! warning `InternalModel` estimator does not work if `model` is integrating or unstable. The @@ -81,7 +81,10 @@ InternalModel estimator with a sample time Ts = 0.5 s, LinModel and: supposes 1 integrator per measured outputs by default, assuming that the current stochastic estimate ``\mathbf{ŷ_s^m}(k) = \mathbf{y^m}(k) - \mathbf{ŷ_d^m}(k)`` is constant in the future. This is the dynamic matrix control (DMC) strategy, which is simple but sometimes - too aggressive. Additional poles and zeros in `stoch_ym` can mitigate this. + too aggressive. Additional poles and zeros in `stoch_ym` can mitigate this. The following + block diagram summarizes the internal model structure. + + ![block diagram of the internal model structure](assets/imc.svg) """ function InternalModel( model::SM; @@ -219,9 +222,9 @@ function setmodel_estimator!(estim::InternalModel, model::LinModel, _ , _ , _) end @doc raw""" - update_estimate!(estim::InternalModel, u, ym, d=[]) + update_estimate!(estim::InternalModel, u0, y0m, d0=[]) -Update `estim.x̂0`/`x̂d`/`x̂s` with current inputs `u`, measured outputs `ym` and dist. `d`. +Update `estim.x̂0`/`x̂d`/`x̂s` with current inputs `u0`, measured outputs `y0m` and dist. `d0`. The [`InternalModel`](@ref) updates the deterministic `x̂d` and stochastic `x̂s` estimates with: ```math @@ -234,19 +237,19 @@ This estimator does not augment the state vector, thus ``\mathbf{x̂ = x̂_d}``. [`init_internalmodel`](@ref) for details. """ function update_estimate!( - estim::InternalModel{NT, SM}, u, ym, d=empty(estim.x̂0) + estim::InternalModel{NT, SM}, u0, y0m, d0=empty(estim.x̂0) ) where {NT<:Real, SM} model = estim.model x̂d, x̂s = estim.x̂d, estim.x̂s # -------------- deterministic model --------------------- - ŷd, x̂dnext = Vector{NT}(undef, model.ny), Vector{NT}(undef, model.nx) - h!(ŷd, model, x̂d, d) - f!(x̂dnext, model, x̂d, u, d) + ŷ0d, x̂dnext = Vector{NT}(undef, model.ny), Vector{NT}(undef, model.nx) + h!(ŷ0d, model, x̂d, d0) + f!(x̂dnext, model, x̂d, u0, d0) x̂d .= x̂dnext # this also updates estim.x̂0 (they are the same object) # --------------- stochastic model ----------------------- x̂snext = Vector{NT}(undef, estim.nxs) ŷs = zeros(NT, model.ny) - ŷs[estim.i_ym] = ym - ŷd[estim.i_ym] # ŷs=0 for unmeasured outputs + ŷs[estim.i_ym] = y0m - ŷ0d[estim.i_ym] # ŷs=0 for unmeasured outputs mul!(x̂snext, estim.Âs, x̂s) mul!(x̂snext, estim.B̂s, ŷs, 1, 1) x̂s .= x̂snext diff --git a/src/estimator/kalman.jl b/src/estimator/kalman.jl index b99bcf3fe..0e499a70b 100644 --- a/src/estimator/kalman.jl +++ b/src/estimator/kalman.jl @@ -171,9 +171,9 @@ end @doc raw""" - update_estimate!(estim::SteadyKalmanFilter, u, ym, d=[]) + update_estimate!(estim::SteadyKalmanFilter, u0, y0m, d0=[]) -Update `estim.x̂0` estimate with current inputs `u`, measured outputs `ym` and dist. `d`. +Update `estim.x̂0` estimate with current inputs `u0`, measured outputs `y0m` and dist. `d0`. The [`SteadyKalmanFilter`](@ref) updates it with the precomputed Kalman gain ``\mathbf{K̂}``: ```math @@ -181,21 +181,21 @@ The [`SteadyKalmanFilter`](@ref) updates it with the precomputed Kalman gain ``\ + \mathbf{K̂}[\mathbf{y^m}(k) - \mathbf{Ĉ^m x̂}_{k-1}(k) - \mathbf{D̂_d^m d}(k)] ``` """ -function update_estimate!(estim::SteadyKalmanFilter, u, ym, d=empty(estim.x̂0)) +function update_estimate!(estim::SteadyKalmanFilter, u0, y0m, d0=empty(estim.x̂0)) Â, B̂u, B̂d = estim.Â, estim.B̂u, estim.B̂d - x̂, K̂ = estim.x̂0, estim.K̂ - ŷm, x̂next = similar(ym), similar(x̂) + x̂0, K̂ = estim.x̂0, estim.K̂ + ŷ0m, x̂0next = similar(y0m), similar(x̂0) Ĉm, D̂dm = @views estim.Ĉ[estim.i_ym, :], estim.D̂d[estim.i_ym, :] # in-place operations to reduce allocations: - mul!(ŷm, Ĉm, x̂) - mul!(ŷm, D̂dm, d, 1, 1) - v̂ = ŷm - v̂ .= ym .- ŷm - mul!(x̂next, Â, x̂) - mul!(x̂next, B̂u, u, 1, 1) - mul!(x̂next, B̂d, d, 1, 1) - mul!(x̂next, K̂, v̂, 1, 1) - estim.x̂0 .= x̂next + mul!(ŷ0m, Ĉm, x̂0) + mul!(ŷ0m, D̂dm, d0, 1, 1) + v̂ = ŷ0m + v̂ .= y0m .- ŷ0m + mul!(x̂0next, Â, x̂0) + mul!(x̂0next, B̂u, u0, 1, 1) + mul!(x̂0next, B̂d, d0, 1, 1) + mul!(x̂0next, K̂, v̂, 1, 1) + estim.x̂0 .= x̂0next return nothing end @@ -320,7 +320,7 @@ function KalmanFilter(model::SM, i_ym, nint_u, nint_ym, P̂_0, Q̂, R̂) where { end @doc raw""" - update_estimate!(estim::KalmanFilter, u, ym, d=[]) + update_estimate!(estim::KalmanFilter, u0, y0m, d0=[]) Update [`KalmanFilter`](@ref) state `estim.x̂0` and estimation error covariance `estim.P̂`. @@ -344,9 +344,9 @@ control period ``k-1``. See [^2] for details. [^2]: Boyd S., "Lecture 8 : The Kalman Filter" (Winter 2008-09) [course slides], *EE363: Linear Dynamical Systems*, . """ -function update_estimate!(estim::KalmanFilter, u, ym, d=empty(estim.x̂0)) +function update_estimate!(estim::KalmanFilter, u0, y0m, d0=empty(estim.x̂0)) Ĉm = @views estim.Ĉ[estim.i_ym, :] - return update_estimate_kf!(estim, u, ym, d, estim.Â, Ĉm, estim.P̂, estim.x̂0) + return update_estimate_kf!(estim, u0, y0m, d0, estim.Â, Ĉm) end @@ -377,8 +377,8 @@ struct UnscentedKalmanFilter{NT<:Real, SM<:SimModel} <: StateEstimator{NT} R̂::Hermitian{NT, Matrix{NT}} K̂::Matrix{NT} M̂::Hermitian{NT, Matrix{NT}} - X̂::Matrix{NT} - Ŷm::Matrix{NT} + X̂0::Matrix{NT} + Ŷ0m::Matrix{NT} sqrtP̂::LowerTriangular{NT, Matrix{NT}} nσ::Int γ::NT @@ -401,7 +401,7 @@ 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̂, Ŷm = zeros(NT, nx̂, nσ), zeros(NT, nym, nσ) + X̂0, Ŷ0m = zeros(NT, nx̂, nσ), zeros(NT, nym, nσ) sqrtP̂ = LowerTriangular(zeros(NT, nx̂, nx̂)) return new{NT, SM}( model, @@ -410,7 +410,7 @@ struct UnscentedKalmanFilter{NT<:Real, SM<:SimModel} <: StateEstimator{NT} As, Cs_u, Cs_y, nint_u, nint_ym, Â, B̂u, Ĉ, B̂d, D̂d, P̂_0, Q̂, R̂, - K̂, M̂, X̂, Ŷm, sqrtP̂, + K̂, M̂, X̂0, Ŷ0m, sqrtP̂, nσ, γ, m̂, Ŝ ) end @@ -537,7 +537,7 @@ function init_ukf(::SimModel{NT}, nx̂, α, β, κ) where {NT<:Real} end @doc raw""" - update_estimate!(estim::UnscentedKalmanFilter, u, ym, d=[]) + update_estimate!(estim::UnscentedKalmanFilter, u0, y0m, d0=[]) Update [`UnscentedKalmanFilter`](@ref) state `estim.x̂0` and covariance estimate `estim.P̂`. @@ -577,63 +577,63 @@ noise, respectively. ISBN9780470045343. """ function update_estimate!( - estim::UnscentedKalmanFilter{NT}, u, ym, d=empty(estim.x̂0) + estim::UnscentedKalmanFilter{NT}, u0, y0m, d0=empty(estim.x̂0) ) where NT<:Real - x̂, P̂, Q̂, R̂, K̂, M̂ = estim.x̂0, estim.P̂, estim.Q̂, estim.R̂, estim.K̂, estim.M̂ + x̂0, P̂, Q̂, R̂, K̂, M̂ = estim.x̂0, estim.P̂, estim.Q̂, estim.R̂, estim.K̂, estim.M̂ nym, nx̂ = estim.nym, estim.nx̂ γ, m̂, Ŝ = estim.γ, estim.m̂, estim.Ŝ - X̂, Ŷm = estim.X̂, estim.Ŷm + X̂0, Ŷ0m = estim.X̂0, estim.Ŷ0m sqrtP̂ = estim.sqrtP̂ # --- initialize matrices --- - x̂next = Vector{NT}(undef, nx̂) - û = Vector{NT}(undef, estim.model.nu) - ŷm = Vector{NT}(undef, nym) - ŷ = Vector{NT}(undef, estim.model.ny) + x̂0next = Vector{NT}(undef, nx̂) + û0 = Vector{NT}(undef, estim.model.nu) + ŷ0m = Vector{NT}(undef, nym) + ŷ0 = Vector{NT}(undef, estim.model.ny) # --- correction step --- P̂_chol = sqrtP̂.data P̂_chol .= P̂ cholesky!(Hermitian(P̂_chol, :L)) # also modifies sqrtP̂ γ_sqrtP̂ = lmul!(γ, sqrtP̂) - X̂ .= x̂ - X̂[:, 2:nx̂+1] .+= γ_sqrtP̂ - X̂[:, nx̂+2:end] .-= γ_sqrtP̂ - for j in axes(Ŷm, 2) - @views ĥ!(ŷ, estim, estim.model, X̂[:, j], d) - @views Ŷm[:, j] .= ŷ[estim.i_ym] + X̂0 .= x̂0 + X̂0[:, 2:nx̂+1] .+= γ_sqrtP̂ + X̂0[:, nx̂+2:end] .-= γ_sqrtP̂ + for j in axes(Ŷ0m, 2) + @views ĥ!(ŷ0, estim, estim.model, X̂0[:, j], d0) + @views Ŷ0m[:, j] .= ŷ0[estim.i_ym] end - mul!(ŷm, Ŷm, m̂) - X̄, Ȳm = X̂, Ŷm - X̄ .= X̂ .- x̂ - Ȳm .= Ŷm .- ŷm + mul!(ŷ0m, Ŷ0m, m̂) + X̄, Ȳm = X̂0, Ŷ0m + X̄ .= X̂0 .- x̂0 + Ȳm .= Ŷ0m .- ŷ0m M̂.data .= Ȳm * Ŝ * Ȳm' .+ R̂ mul!(K̂, X̄, lmul!(Ŝ, Ȳm')) rdiv!(K̂, cholesky(M̂)) - v̂ = ŷm - v̂ .= ym .- ŷm - x̂cor = x̂next - x̂cor .= x̂ - mul!(x̂cor, K̂, v̂, 1, 1) + v̂ = ŷ0m + v̂ .= y0m .- ŷ0m + x̂0cor = x̂0next + x̂0cor .= x̂0 + mul!(x̂0cor, K̂, v̂, 1, 1) P̂cor = Hermitian(P̂ .- K̂ * M̂ * K̂', :L) # --- prediction step --- - X̂cor, sqrtP̂cor = X̂, sqrtP̂ + X̂0cor, sqrtP̂cor = X̂0, sqrtP̂ P̂cor_chol = sqrtP̂cor.data P̂cor_chol .= P̂cor cholesky!(Hermitian(P̂cor_chol, :L)) # also modifies sqrtP̂cor γ_sqrtP̂cor = lmul!(γ, sqrtP̂cor) - X̂cor .= x̂cor - X̂cor[:, 2:nx̂+1] .+= γ_sqrtP̂cor - X̂cor[:, nx̂+2:end] .-= γ_sqrtP̂cor - X̂next = X̂cor - for j in axes(X̂next, 2) - @views x̂cor .= X̂cor[:, j] - @views f̂!(X̂next[:, j], û, estim, estim.model, x̂cor, u, d) + X̂0cor .= x̂0cor + X̂0cor[:, 2:nx̂+1] .+= γ_sqrtP̂cor + X̂0cor[:, nx̂+2:end] .-= γ_sqrtP̂cor + X̂0next = X̂0cor + for j in axes(X̂0next, 2) + @views x̂0cor .= X̂0cor[:, j] + @views f̂!(X̂0next[:, j], û0, estim, estim.model, x̂0cor, u0, d0) end - x̂next .= mul!(x̂, X̂next, m̂) - X̄next = X̂next - X̄next .= X̂next .- x̂next + x̂0next .= mul!(x̂0, X̂0next, m̂) + X̄next = X̂0next + X̄next .= X̂0next .- x̂0next P̂next = P̂cor P̂next.data .= X̄next * Ŝ * X̄next' .+ Q̂ - estim.x̂0 .= x̂next + estim.x̂0 .= x̂0next estim.P̂ .= P̂next return nothing end @@ -763,7 +763,7 @@ end @doc raw""" - update_estimate!(estim::ExtendedKalmanFilter, u, ym, d=[]) + update_estimate!(estim::ExtendedKalmanFilter, u0, y0m, d0=[]) Update [`ExtendedKalmanFilter`](@ref) state `estim.x̂0` and covariance `estim.P̂`. @@ -793,20 +793,22 @@ automatically computes the Jacobians: The matrix ``\mathbf{Ĥ^m}`` is the rows of ``\mathbf{Ĥ}`` that are measured outputs. """ function update_estimate!( - estim::ExtendedKalmanFilter{NT}, u, ym, d=empty(estim.x̂0) + estim::ExtendedKalmanFilter{NT}, u0, y0m, d0=empty(estim.x̂0) ) where NT<:Real model = estim.model nx̂, nu, ny = estim.nx̂, model.nu, model.ny - x̂, P̂ = estim.x̂0, estim.P̂ - # concatenate x̂next and û vectors to allows û vector with dual numbers for auto diff: - x̂nextû, ŷ = Vector{NT}(undef, nx̂ + nu), Vector{NT}(undef, ny) - f̂AD! = (x̂nextû, x̂) -> @views f̂!(x̂nextû[1:nx̂], x̂nextû[nx̂+1:end], estim, model, x̂, u, d) - ĥAD! = (ŷ, x̂) -> ĥ!(ŷ, estim, model, x̂, d) - ForwardDiff.jacobian!(estim.F̂_û, f̂AD!, x̂nextû, x̂) - ForwardDiff.jacobian!(estim.Ĥ, ĥAD!, ŷ, x̂) + x̂0 = estim.x̂0 + # concatenate x̂0next and û0 vectors to allows û0 vector with dual numbers for AD: + x̂0nextû, ŷ0 = Vector{NT}(undef, nx̂ + nu), Vector{NT}(undef, ny) + f̂AD! = (x̂0nextû, x̂0) -> @views f̂!( + x̂0nextû[1:nx̂], x̂0nextû[nx̂+1:end], estim, model, x̂0, u0, d0 + ) + ĥAD! = (ŷ0, x̂0) -> ĥ!(ŷ0, estim, model, x̂0, d0) + ForwardDiff.jacobian!(estim.F̂_û, f̂AD!, x̂0nextû, x̂0) + ForwardDiff.jacobian!(estim.Ĥ, ĥAD!, ŷ0, x̂0) F̂ = @views estim.F̂_û[1:nx̂, :] Ĥm = @views estim.Ĥ[estim.i_ym, :] - return update_estimate_kf!(estim, u, ym, d, F̂, Ĥm, P̂, x̂) + return update_estimate_kf!(estim, u0, y0m, d0, F̂, Ĥm) end "Set `estim.P̂` to `estim.P̂_0` for the time-varying Kalman Filters." @@ -836,7 +838,7 @@ function validate_kfcov(nym, nx̂, Q̂, R̂, P̂_0=nothing) end """ - update_estimate_kf!(estim::StateEstimator, u, ym, d, Â, Ĉm, P̂, x̂) + update_estimate_kf!(estim::StateEstimator, u0, y0m, d0, Â, Ĉm) Update time-varying/extended Kalman Filter estimates with augmented `Â` and `Ĉm` matrices. @@ -846,21 +848,24 @@ substitutes the augmented model matrices with its Jacobians (`Â = F̂` and `C The implementation uses in-place operations and explicit factorization to reduce allocations. See e.g. [`KalmanFilter`](@ref) docstring for the equations. """ -function update_estimate_kf!(estim::StateEstimator{NT}, u, ym, d, Â, Ĉm, P̂, x̂) where NT<:Real +function update_estimate_kf!( + estim::StateEstimator{NT}, u0, y0m, d0, Â, Ĉm +) where NT<:Real Q̂, R̂, M̂, K̂ = estim.Q̂, estim.R̂, estim.M̂, estim.K̂ + x̂0, P̂ = estim.x̂0, estim.P̂ nx̂, nu, ny = estim.nx̂, estim.model.nu, estim.model.ny - x̂next, û, ŷ = Vector{NT}(undef, nx̂), Vector{NT}(undef, nu), Vector{NT}(undef, ny) + x̂0next, û0, ŷ0 = Vector{NT}(undef, nx̂), Vector{NT}(undef, nu), Vector{NT}(undef, ny) mul!(M̂, P̂.data, Ĉm') # the ".data" weirdly removes a type instability in mul! rdiv!(M̂, cholesky!(Hermitian(Ĉm * P̂ * Ĉm' .+ R̂, :L))) mul!(K̂, Â, M̂) - ĥ!(ŷ, estim, estim.model, x̂, d) - ŷm = @views ŷ[estim.i_ym] - v̂ = ŷm - v̂ .= ym .- ŷm - f̂!(x̂next, û, estim, estim.model, x̂, u, d) - mul!(x̂next, K̂, v̂, 1, 1) + ĥ!(ŷ0, estim, estim.model, x̂0, d0) + ŷ0m = @views ŷ0[estim.i_ym] + v̂ = ŷ0m + v̂ .= y0m .- ŷ0m + f̂!(x̂0next, û0, estim, estim.model, x̂0, u0, d0) + mul!(x̂0next, K̂, v̂, 1, 1) P̂next = Hermitian(Â * (P̂ .- M̂ * Ĉm * P̂) * Â' .+ Q̂, :L) - estim.x̂0 .= x̂next + estim.x̂0 .= x̂0next estim.P̂ .= P̂next return nothing end diff --git a/src/estimator/luenberger.jl b/src/estimator/luenberger.jl index 76c421e2d..05504ab9a 100644 --- a/src/estimator/luenberger.jl +++ b/src/estimator/luenberger.jl @@ -99,25 +99,25 @@ end """ - update_estimate!(estim::Luenberger, u, ym, d=[]) + update_estimate!(estim::Luenberger, u0, y0m, d0=[]) Same than [`update_estimate!(::SteadyKalmanFilter)`](@ref) but using [`Luenberger`](@ref). """ -function update_estimate!(estim::Luenberger, u, ym, d=empty(estim.x̂0)) +function update_estimate!(estim::Luenberger, u0, y0m, d0=empty(estim.x̂0)) Â, B̂u, B̂d = estim.Â, estim.B̂u, estim.B̂d - x̂, K̂ = estim.x̂0, estim.K̂ + x̂0, K̂ = estim.x̂0, estim.K̂ Ĉm, D̂dm = @views estim.Ĉ[estim.i_ym, :], estim.D̂d[estim.i_ym, :] - ŷm, x̂next = similar(ym), similar(x̂) + ŷ0m, x̂0next = similar(y0m), similar(x̂0) # in-place operations to reduce allocations: - mul!(ŷm, Ĉm, x̂) - mul!(ŷm, D̂dm, d, 1, 1) - v̂ = ŷm - v̂ .= ym .- ŷm - mul!(x̂next, Â, x̂) - mul!(x̂next, B̂u, u, 1, 1) - mul!(x̂next, B̂d, d, 1, 1) - mul!(x̂next, K̂, v̂, 1, 1) - estim.x̂0 .= x̂next + mul!(ŷ0m, Ĉm, x̂0) + mul!(ŷ0m, D̂dm, d0, 1, 1) + v̂ = ŷ0m + v̂ .= y0m .- ŷ0m + mul!(x̂0next, Â, x̂0) + mul!(x̂0next, B̂u, u0, 1, 1) + mul!(x̂0next, B̂d, d0, 1, 1) + mul!(x̂0next, K̂, v̂, 1, 1) + estim.x̂0 .= x̂0next return nothing end diff --git a/src/estimator/mhe/construct.jl b/src/estimator/mhe/construct.jl index beb38ca03..b91e54a62 100644 --- a/src/estimator/mhe/construct.jl +++ b/src/estimator/mhe/construct.jl @@ -715,7 +715,6 @@ also returns the ``\mathbf{A}`` matrices for the inequality constraints: ``` in which ``\mathbf{X̂_{min}, X̂_{max}}`` and ``\mathbf{X̂_{op}}`` vectors respectively contains ``\mathbf{x̂_{min}, x̂_{max}}`` and ``\mathbf{x̂_{op}}`` repeated ``H_e`` times. -``` """ function relaxX̂(::LinModel{NT}, C, C_x̂min, C_x̂max, Ex̂) where {NT<:Real} if !isinf(C) # Z̃ = [ϵ; Z] diff --git a/src/estimator/mhe/execute.jl b/src/estimator/mhe/execute.jl index 984e2372e..51a6f2c43 100644 --- a/src/estimator/mhe/execute.jl +++ b/src/estimator/mhe/execute.jl @@ -17,23 +17,22 @@ function init_estimate_cov!(estim::MovingHorizonEstimator, _ , _ , _ ) end @doc raw""" - update_estimate!(estim::MovingHorizonEstimator, u, ym, d=[]) + update_estimate!(estim::MovingHorizonEstimator, u0, y0m, d0=[]) -Update [`MovingHorizonEstimator`](@ref) state `estim.x̂`. +Update [`MovingHorizonEstimator`](@ref) state `estim.x̂0`. The optimization problem of [`MovingHorizonEstimator`](@ref) documentation is solved at each discrete time ``k``. Once solved, the next estimate ``\mathbf{x̂}_k(k+1)`` is computed by inserting the optimal values of ``\mathbf{x̂}_k(k-N_k+1)`` and ``\mathbf{Ŵ}`` in the augmented model from ``j = N_k-1`` to ``0`` inclusively. Afterward, if ``k ≥ H_e``, the arrival covariance for the next time step ``\mathbf{P̂}_{k-N_k+1}(k-N_k+2)`` is estimated -with the equations of [`update_estimate!(::ExtendedKalmanFilter)`](@ref), or `KalmanFilter`, -for `LinModel`. +using `estim.covestim` object. """ function update_estimate!( - estim::MovingHorizonEstimator{NT}, u, ym, d=empty(estim.x̂) + estim::MovingHorizonEstimator{NT}, u0, y0m, d0=empty(estim.x̂0) ) where NT<:Real model, optim, x̂0 = estim.model, estim.optim, estim.x̂0 - add_data_windows!(estim::MovingHorizonEstimator, u, d, ym) + add_data_windows!(estim::MovingHorizonEstimator, u0, d0, y0m) initpred!(estim, model) linconstraint!(estim, model) nu, ny, nx̂, nym, nŵ, Nk = model.nu, model.ny, estim.nx̂, estim.nym, estim.nx̂, estim.Nk[] From b685d87f69ec28fd51ff090cc65c43dc79158d88 Mon Sep 17 00:00:00 2001 From: franckgaga Date: Sat, 27 Apr 2024 13:41:06 -0400 Subject: [PATCH 2/2] rescale svg --- docs/src/assets/imc.svg | 151 +++++++++++++++++--------------- src/estimator/internal_model.jl | 2 +- 2 files changed, 82 insertions(+), 71 deletions(-) diff --git a/docs/src/assets/imc.svg b/docs/src/assets/imc.svg index 29be2d731..cd8b83a0f 100644 --- a/docs/src/assets/imc.svg +++ b/docs/src/assets/imc.svg @@ -1,11 +1,11 @@ + inkscape:current-layer="layer2" + fit-margin-top="1" + fit-margin-left="0.5" + fit-margin-right="0" + fit-margin-bottom="0" + lock-margins="true" + height="165.13277pt" + width="342.09381pt" /> @@ -47,103 +54,103 @@ overflow="visible" id="glyph0-1"> @@ -151,39 +158,39 @@ overflow="visible" id="glyph1-1"> @@ -191,31 +198,31 @@ overflow="visible" id="glyph2-1"> @@ -223,15 +230,15 @@ overflow="visible" id="glyph3-1"> @@ -239,31 +246,31 @@ overflow="visible" id="glyph4-1"> @@ -271,8 +278,8 @@ overflow="visible" id="glyph5-1"> @@ -280,22 +287,26 @@ + inkscape:label="Layer 1" + style="display:inline" + transform="translate(74.592868,39.914295)"> + width="370.93448" + height="193.78149" + x="-94.67131" + y="-50.355202" /> + style="display:inline" + transform="translate(74.592868,39.914295)"> + id="surface1" + transform="matrix(1.9999602,0,0,2.0001289,-85.541401,-41.144513)">