From 2555bfb9d6798fa873c988f61bc1aae0aba39c70 Mon Sep 17 00:00:00 2001 From: franckgaga Date: Thu, 12 Jun 2025 17:54:52 -0400 Subject: [PATCH 01/10] wip: new `KalmanCovariance` similar to `ControllerWeights` --- src/estimator/construct.jl | 67 ++++++++++++++++++++++++++++++ src/estimator/kalman.jl | 85 +++++++++++++++----------------------- 2 files changed, 101 insertions(+), 51 deletions(-) diff --git a/src/estimator/construct.jl b/src/estimator/construct.jl index 59dd0cedb..b3f416b3e 100644 --- a/src/estimator/construct.jl +++ b/src/estimator/construct.jl @@ -38,6 +38,73 @@ function StateEstimatorBuffer{NT}( return StateEstimatorBuffer{NT}(u, û, k, x̂, P̂, Q̂, R̂, K̂, ym, ŷ, d, empty) end +"Include all the covariance matrices for the Kalman filters and moving horizon estimator." +struct KalmanCovariances{ + NT<:Real, + # parameters to support both dense and Diagonal matrices (with specialization): + Q̂C<:AbstractMatrix{NT}, + R̂C<:AbstractMatrix{NT}, +} + P̂_0::Hermitian{NT, Matrix{NT}} + P̂::Hermitian{NT, Matrix{NT}} + Q̂::Hermitian{NT, Q̂C} + R̂::Hermitian{NT, R̂C} + invP̄::Hermitian{NT, Matrix{NT}} + invQ̂_He::Hermitian{NT, Q̂C} + invR̂_He::Hermitian{NT, R̂C} + function KalmanCovariances{NT}( + model, i_ym, nint_u, nint_ym, Q̂::Q̂C, R̂::R̂C, P̂_0=nothing, He=1 + ) where {NT<:Real, Q̂C<:AbstractMatrix{NT}, R̂C<:AbstractMatrix{NT}} + validate_kfcov(model, i_ym, nint_u, nint_ym, Q̂, R̂, P̂_0) + if isnothing(P̂_0) + P̂_0 = zeros(NT, 0, 0) + end + P̂_0 = Hermitian(P̂_0, :L) + P̂ = copy(P̂_0) + Q̂ = Hermitian(Q̂, :L) + R̂ = Hermitian(R̂, :L) + # the following variables are only for the moving horizon estimator: + invP̄, invQ̂, invR̂ = copy(P̂_0), copy(Q̂), copy(R̂) + inv!(invP̄) + inv!(invQ̂) + inv!(invR̂) + invQ̂_He = repeatdiag(invQ̂, He) + invR̂_He = repeatdiag(invR̂, He) + isdiag(invQ̂_He) && (invQ̂_He = Diagonal(invQ̂_He)) # Q̂C(invQ̂_He) does not work on Julia 1.10 + isdiag(invR̂_He) && (invR̂_He = Diagonal(invR̂_He)) # R̂C(invR̂_He) does not work on Julia 1.10 + invQ̂_He = Hermitian(invQ̂_He, :L) + invR̂_He = Hermitian(invR̂_He, :L) + return new{NT, Q̂C, R̂C}(P̂_0, P̂, Q̂, R̂, invP̄, invQ̂_He, invR̂_He) + end +end + +"Outer constructor to convert covariance matrix number type to `NT` if necessary." +function KalmanCovariances( + model::SimModel{NT}, i_ym, nint_u, nint_ym, Q̂, R̂, P̂_0=nothing, He=1 + ) where {NT<:Real} + return KalmanCovariances{NT}(model, i_ym, nint_u, nint_ym, NT.(Q̂), NT.(R̂), P̂_0, He) +end + +""" + validate_kfcov(model, i_ym, nint_u, nint_ym, Q̂, R̂, P̂_0=nothing) + +Validate sizes and Hermitianity of process `Q̂`` and sensor `R̂` noises covariance matrices. + +Also validate initial estimate covariance `P̂_0`, if provided. +""" +function validate_kfcov(model, i_ym, nint_u, nint_ym, Q̂, R̂, P̂_0=nothing) + nym = length(i_ym) + nx̂ = model.nx + sum(nint_u) + sum(nint_ym) + 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 + @doc raw""" init_estimstoch(model, i_ym, nint_u, nint_ym) -> As, Cs_u, Cs_y, nxs, nint_u, nint_ym diff --git a/src/estimator/kalman.jl b/src/estimator/kalman.jl index e9bf2f389..8b47d7c10 100644 --- a/src/estimator/kalman.jl +++ b/src/estimator/kalman.jl @@ -1,5 +1,10 @@ -struct SteadyKalmanFilter{NT<:Real, SM<:LinModel} <: StateEstimator{NT} +struct SteadyKalmanFilter{ + NT<:Real, + SM<:LinModel, + KC<:KalmanCovariances +} <: StateEstimator{NT} model::SM + cov ::KC x̂op ::Vector{NT} f̂op ::Vector{NT} x̂0 ::Vector{NT} @@ -20,15 +25,13 @@ struct SteadyKalmanFilter{NT<:Real, SM<:LinModel} <: StateEstimator{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} direct::Bool corrected::Vector{Bool} buffer::StateEstimatorBuffer{NT} function SteadyKalmanFilter{NT}( - model::SM, i_ym, nint_u, nint_ym, Q̂, R̂; direct=true - ) where {NT<:Real, SM<:LinModel} + model::SM, i_ym, nint_u, nint_ym, cov::KC; direct=true + ) where {NT<:Real, SM<:LinModel, KC<:KalmanCovariances} nu, ny, nd, nk = model.nu, model.ny, model.nd, model.nk nym, nyu = validate_ym(model, i_ym) As, Cs_u, Cs_y, nint_u, nint_ym = init_estimstoch(model, i_ym, nint_u, nint_ym) @@ -36,7 +39,7 @@ struct SteadyKalmanFilter{NT<:Real, SM<:LinModel} <: StateEstimator{NT} 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̂) + R̂, Q̂ = cov.R̂, cov.Q̂ if ny == nym R̂_y = R̂ else @@ -56,16 +59,15 @@ struct SteadyKalmanFilter{NT<:Real, SM<:LinModel} <: StateEstimator{NT} end end x̂0 = [zeros(NT, model.nx); zeros(NT, nxs)] - Q̂, R̂ = Hermitian(Q̂, :L), Hermitian(R̂, :L) corrected = [false] buffer = StateEstimatorBuffer{NT}(nu, nx̂, nym, ny, nd, nk) - return new{NT, SM}( - model, + return new{NT, SM, KC}( + model, + cov, 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, Ĉm, D̂dm, - Q̂, R̂, K̂, direct, corrected, buffer @@ -184,9 +186,9 @@ function SteadyKalmanFilter( σQint_ym = sigmaQint_ym, ) where {NT<:Real, SM<:LinModel{NT}} # estimated covariances matrices (variance = σ²) : - Q̂ = Hermitian(diagm(NT[σQ; σQint_u; σQint_ym ].^2), :L) - R̂ = Hermitian(diagm(NT[σR;].^2), :L) - return SteadyKalmanFilter{NT}(model, i_ym, nint_u, nint_ym, Q̂, R̂; direct) + Q̂ = Diagonal([σQ; σQint_u; σQint_ym].^2) + R̂ = Diagonal([σR;].^2) + return SteadyKalmanFilter(model, i_ym, nint_u, nint_ym, Q̂, R̂; direct) end @doc raw""" @@ -200,7 +202,8 @@ function SteadyKalmanFilter( model::SM, i_ym, nint_u, nint_ym, Q̂, R̂; direct=true ) where {NT<:Real, SM<:LinModel{NT}} Q̂, R̂ = to_mat(Q̂), to_mat(R̂) - return SteadyKalmanFilter{NT}(model, i_ym, nint_u, nint_ym, Q̂, R̂; direct) + cov = KalmanCovariances(model, i_ym, nint_u, nint_ym, Q̂, R̂) + return SteadyKalmanFilter{NT}(model, i_ym, nint_u, nint_ym, cov; direct) end "Throw an error if `setmodel!` is called on a SteadyKalmanFilter w/o the default values." @@ -281,12 +284,16 @@ function predict_estimate_obsv!(estim::StateEstimator, _ , d0, u0) return nothing end -struct KalmanFilter{NT<:Real, SM<:LinModel} <: StateEstimator{NT} +struct KalmanFilter{ + NT<:Real, + SM<:LinModel, + KC<:KalmanCovariances +} <: StateEstimator{NT} model::SM + cov ::KC x̂op::Vector{NT} f̂op::Vector{NT} x̂0 ::Vector{NT} - P̂::Hermitian{NT, Matrix{NT}} i_ym::Vector{Int} nx̂ ::Int nym::Int @@ -304,16 +311,13 @@ struct KalmanFilter{NT<:Real, SM<:LinModel} <: StateEstimator{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} direct::Bool corrected::Vector{Bool} buffer::StateEstimatorBuffer{NT} function KalmanFilter{NT}( - model::SM, i_ym, nint_u, nint_ym, P̂_0, Q̂, R̂; direct=true - ) where {NT<:Real, SM<:LinModel} + model::SM, i_ym, nint_u, nint_ym, cov::KC; direct=true + ) where {NT<:Real, SM<:LinModel, KC<:KalmanCovariances} nu, ny, nd, nk = model.nu, model.ny, model.nd, model.nk nym, nyu = validate_ym(model, i_ym) As, Cs_u, Cs_y, nint_u, nint_ym = init_estimstoch(model, i_ym, nint_u, nint_ym) @@ -321,21 +325,17 @@ struct KalmanFilter{NT<:Real, SM<:LinModel} <: StateEstimator{NT} 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) x̂0 = [zeros(NT, model.nx); zeros(NT, nxs)] - Q̂, R̂ = Hermitian(Q̂, :L), Hermitian(R̂, :L) - P̂_0 = Hermitian(P̂_0, :L) - P̂ = Hermitian(copy(P̂_0.data), :L) # copy on P̂_0.data necessary for Julia Nightly K̂ = zeros(NT, nx̂, nym) corrected = [false] buffer = StateEstimatorBuffer{NT}(nu, nx̂, nym, ny, nd, nk) - return new{NT, SM}( + return new{NT, SM, KC}( model, - x̂op, f̂op, x̂0, P̂, + cov, + 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, Ĉm, D̂dm, - P̂_0, Q̂, R̂, K̂, direct, corrected, buffer @@ -419,10 +419,10 @@ function KalmanFilter( σQint_ym = sigmaQint_ym, ) where {NT<:Real, SM<:LinModel{NT}} # estimated covariances matrices (variance = σ²) : - P̂_0 = Hermitian(diagm(NT[σP_0; σPint_u_0; σPint_ym_0].^2), :L) - Q̂ = Hermitian(diagm(NT[σQ; σQint_u; σQint_ym ].^2), :L) - R̂ = Hermitian(diagm(NT[σR;].^2), :L) - return KalmanFilter{NT}(model, i_ym, nint_u, nint_ym, P̂_0, Q̂ , R̂; direct) + P̂_0 = Diagonal([σP_0; σPint_u_0; σPint_ym_0].^2) + Q̂ = Diagonal([σQ; σQint_u; σQint_ym ].^2) + R̂ = Diagonal([σR;].^2) + return KalmanFilter(model, i_ym, nint_u, nint_ym, P̂_0, Q̂, R̂; direct) end @doc raw""" @@ -436,7 +436,8 @@ function KalmanFilter( model::SM, i_ym, nint_u, nint_ym, P̂_0, Q̂, R̂; direct=true ) where {NT<:Real, SM<:LinModel{NT}} P̂_0, Q̂, R̂ = to_mat(P̂_0), to_mat(Q̂), to_mat(R̂) - return KalmanFilter{NT}(model, i_ym, nint_u, nint_ym, P̂_0, Q̂, R̂; direct) + cov = KalmanCovariances(model, i_ym, nint_u, nint_ym, Q̂, R̂, P̂_0) + return KalmanFilter{NT}(model, i_ym, nint_u, nint_ym, cov; direct) end @doc raw""" @@ -1177,24 +1178,6 @@ function init_estimate_cov!( return nothing end -""" - validate_kfcov(nym, nx̂, Q̂, R̂, P̂_0=nothing) - -Validate sizes and Hermitianity of process `Q̂`` and sensor `R̂` noises covariance matrices. - -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 - """ correct_estimate_kf!(estim::Union{KalmanFilter, ExtendedKalmanFilter}, y0m, d0, Ĉm) From 2655b9fe5ef7d0ff6a490cd28c860b48e5dd073f Mon Sep 17 00:00:00 2001 From: franckgaga Date: Thu, 12 Jun 2025 21:25:18 -0400 Subject: [PATCH 02/10] wip: `KalmanFilter` with new `cov` struct --- src/estimator/construct.jl | 2 -- src/estimator/kalman.jl | 14 +++++++------- 2 files changed, 7 insertions(+), 9 deletions(-) diff --git a/src/estimator/construct.jl b/src/estimator/construct.jl index b3f416b3e..2ea9c2293 100644 --- a/src/estimator/construct.jl +++ b/src/estimator/construct.jl @@ -70,8 +70,6 @@ struct KalmanCovariances{ inv!(invR̂) invQ̂_He = repeatdiag(invQ̂, He) invR̂_He = repeatdiag(invR̂, He) - isdiag(invQ̂_He) && (invQ̂_He = Diagonal(invQ̂_He)) # Q̂C(invQ̂_He) does not work on Julia 1.10 - isdiag(invR̂_He) && (invR̂_He = Diagonal(invR̂_He)) # R̂C(invR̂_He) does not work on Julia 1.10 invQ̂_He = Hermitian(invQ̂_He, :L) invR̂_He = Hermitian(invR̂_He, :L) return new{NT, Q̂C, R̂C}(P̂_0, P̂, Q̂, R̂, invP̄, invQ̂_He, invR̂_He) diff --git a/src/estimator/kalman.jl b/src/estimator/kalman.jl index 8b47d7c10..5fcc456fc 100644 --- a/src/estimator/kalman.jl +++ b/src/estimator/kalman.jl @@ -1174,7 +1174,7 @@ end function init_estimate_cov!( estim::Union{KalmanFilter, UnscentedKalmanFilter, ExtendedKalmanFilter}, _ , _ , _ ) - estim.P̂ .= estim.P̂_0 + estim.cov.P̂ .= estim.cov.P̂_0 return nothing end @@ -1187,8 +1187,8 @@ Allows code reuse for [`KalmanFilter`](@ref), [`ExtendedKalmanFilterKalmanFilter See [`update_estimate_kf!`](@ref) for more information. """ function correct_estimate_kf!(estim::Union{KalmanFilter, ExtendedKalmanFilter}, y0m, d0, Ĉm) - R̂, K̂ = estim.R̂, estim.K̂ - x̂0, P̂ = estim.x̂0, estim.P̂ + R̂, K̂ = estim.cov.R̂, estim.K̂ + x̂0, P̂ = estim.x̂0, estim.cov.P̂ # in-place operations to reduce allocations: P̂_Ĉmᵀ = K̂ mul!(P̂_Ĉmᵀ, P̂, Ĉm') @@ -1213,7 +1213,7 @@ function correct_estimate_kf!(estim::Union{KalmanFilter, ExtendedKalmanFilter}, end P̂corr = estim.buffer.P̂ mul!(P̂corr, I_minus_K̂_Ĉm, P̂) - estim.P̂ .= Hermitian(P̂corr, :L) + estim.cov.P̂ .= Hermitian(P̂corr, :L) return nothing end @@ -1227,8 +1227,8 @@ 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}, u0, d0, Â) - x̂0corr, P̂corr = estim.x̂0, estim.P̂ - Q̂ = estim.Q̂ + x̂0corr, P̂corr = estim.x̂0, estim.cov.P̂ + Q̂ = estim.cov.Q̂ x̂0next, û0, k0 = estim.buffer.x̂, estim.buffer.û, estim.buffer.k # in-place operations to reduce allocations: f̂!(x̂0next, û0, k0, estim, estim.model, x̂0corr, u0, d0) @@ -1240,6 +1240,6 @@ function predict_estimate_kf!(estim::Union{KalmanFilter, ExtendedKalmanFilter}, P̂next .= Â_P̂corr_Âᵀ .+ Q̂ x̂0next .+= estim.f̂op .- estim.x̂op estim.x̂0 .= x̂0next - estim.P̂ .= Hermitian(P̂next, :L) + estim.cov.P̂ .= Hermitian(P̂next, :L) return nothing end From 9860b06151d7437f619abf991530ec5791e99d18 Mon Sep 17 00:00:00 2001 From: franckgaga Date: Fri, 13 Jun 2025 10:08:09 -0400 Subject: [PATCH 03/10] wip: Kalman Filters and MHE with `KalmanCovariance` --- src/ModelPredictiveControl.jl | 2 +- src/controller/construct.jl | 42 ++++++++--------- src/estimator/construct.jl | 4 +- src/estimator/kalman.jl | 86 +++++++++++++++------------------- src/estimator/mhe/construct.jl | 44 ++++++----------- src/estimator/mhe/execute.jl | 34 +++++++------- 6 files changed, 93 insertions(+), 119 deletions(-) diff --git a/src/ModelPredictiveControl.jl b/src/ModelPredictiveControl.jl index 38b7623bd..b30c7323d 100644 --- a/src/ModelPredictiveControl.jl +++ b/src/ModelPredictiveControl.jl @@ -54,7 +54,7 @@ include("plot_sim.jl") @compile_workload begin # all calls in this block will be precompiled, regardless of whether # they belong to your package or not (on Julia 1.8 and higher) - include("precompile.jl") + # include("precompile.jl") end end diff --git a/src/controller/construct.jl b/src/controller/construct.jl index 69e9917d9..49e181ae6 100644 --- a/src/controller/construct.jl +++ b/src/controller/construct.jl @@ -94,6 +94,27 @@ function ControllerWeights( return ControllerWeights{NT}(NT.(M_Hp), NT.(N_Hc), NT.(L_Hp), Cwt, Ewt) end +"Validate predictive controller weight and horizon specified values." +function validate_weights(model, Hp, Hc, M_Hp, N_Hc, L_Hp, C=Inf, E=nothing) + nu, ny = model.nu, model.ny + nM, nN, nL = ny*Hp, nu*Hc, nu*Hp + Hp < 1 && throw(ArgumentError("Prediction horizon Hp should be ≥ 1")) + Hc < 1 && throw(ArgumentError("Control horizon Hc should be ≥ 1")) + Hc > Hp && throw(ArgumentError("Control horizon Hc should be ≤ prediction horizon Hp")) + size(M_Hp) ≠ (nM,nM) && throw(ArgumentError("M_Hp size $(size(M_Hp)) ≠ (ny*Hp, ny*Hp) ($nM,$nM)")) + size(N_Hc) ≠ (nN,nN) && throw(ArgumentError("N_Hc size $(size(N_Hc)) ≠ (nu*Hc, nu*Hc) ($nN,$nN)")) + size(L_Hp) ≠ (nL,nL) && throw(ArgumentError("L_Hp size $(size(L_Hp)) ≠ (nu*Hp, nu*Hp) ($nL,$nL)")) + (isdiag(M_Hp) && any(diag(M_Hp) .< 0)) && throw(ArgumentError("Mwt values should be nonnegative")) + (isdiag(N_Hc) && any(diag(N_Hc) .< 0)) && throw(ArgumentError("Nwt values should be nonnegative")) + (isdiag(L_Hp) && any(diag(L_Hp) .< 0)) && throw(ArgumentError("Lwt values should be nonnegative")) + !ishermitian(M_Hp) && throw(ArgumentError("M_Hp should be hermitian")) + !ishermitian(N_Hc) && throw(ArgumentError("N_Hc should be hermitian")) + !ishermitian(L_Hp) && throw(ArgumentError("L_Hp should be hermitian")) + size(C) ≠ () && throw(ArgumentError("Cwt should be a real scalar")) + C < 0 && throw(ArgumentError("Cwt weight should be ≥ 0")) + !isnothing(E) && size(E) ≠ () && throw(ArgumentError("Ewt should be a real scalar")) +end + "Include all the data for the constraints of [`PredictiveController`](@ref)" struct ControllerConstraint{NT<:Real, GCfunc<:Union{Nothing, Function}} # matrices for the terminal constraints: @@ -867,25 +888,4 @@ end "Return empty matrices if `estim` is not a [`InternalModel`](@ref)." function init_stochpred(estim::StateEstimator{NT}, _ ) where NT<:Real return zeros(NT, 0, estim.nxs), zeros(NT, 0, estim.model.ny) -end - -"Validate predictive controller weight and horizon specified values." -function validate_weights(model, Hp, Hc, M_Hp, N_Hc, L_Hp, C=Inf, E=nothing) - nu, ny = model.nu, model.ny - nM, nN, nL = ny*Hp, nu*Hc, nu*Hp - Hp < 1 && throw(ArgumentError("Prediction horizon Hp should be ≥ 1")) - Hc < 1 && throw(ArgumentError("Control horizon Hc should be ≥ 1")) - Hc > Hp && throw(ArgumentError("Control horizon Hc should be ≤ prediction horizon Hp")) - size(M_Hp) ≠ (nM,nM) && throw(ArgumentError("M_Hp size $(size(M_Hp)) ≠ (ny*Hp, ny*Hp) ($nM,$nM)")) - size(N_Hc) ≠ (nN,nN) && throw(ArgumentError("N_Hc size $(size(N_Hc)) ≠ (nu*Hc, nu*Hc) ($nN,$nN)")) - size(L_Hp) ≠ (nL,nL) && throw(ArgumentError("L_Hp size $(size(L_Hp)) ≠ (nu*Hp, nu*Hp) ($nL,$nL)")) - (isdiag(M_Hp) && any(diag(M_Hp) .< 0)) && throw(ArgumentError("Mwt values should be nonnegative")) - (isdiag(N_Hc) && any(diag(N_Hc) .< 0)) && throw(ArgumentError("Nwt values should be nonnegative")) - (isdiag(L_Hp) && any(diag(L_Hp) .< 0)) && throw(ArgumentError("Lwt values should be nonnegative")) - !ishermitian(M_Hp) && throw(ArgumentError("M_Hp should be hermitian")) - !ishermitian(N_Hc) && throw(ArgumentError("N_Hc should be hermitian")) - !ishermitian(L_Hp) && throw(ArgumentError("L_Hp should be hermitian")) - size(C) ≠ () && throw(ArgumentError("Cwt should be a real scalar")) - C < 0 && throw(ArgumentError("Cwt weight should be ≥ 0")) - !isnothing(E) && size(E) ≠ () && throw(ArgumentError("Ewt should be a real scalar")) end \ No newline at end of file diff --git a/src/estimator/construct.jl b/src/estimator/construct.jl index 2ea9c2293..f78e663d7 100644 --- a/src/estimator/construct.jl +++ b/src/estimator/construct.jl @@ -55,7 +55,6 @@ struct KalmanCovariances{ function KalmanCovariances{NT}( model, i_ym, nint_u, nint_ym, Q̂::Q̂C, R̂::R̂C, P̂_0=nothing, He=1 ) where {NT<:Real, Q̂C<:AbstractMatrix{NT}, R̂C<:AbstractMatrix{NT}} - validate_kfcov(model, i_ym, nint_u, nint_ym, Q̂, R̂, P̂_0) if isnothing(P̂_0) P̂_0 = zeros(NT, 0, 0) end @@ -76,10 +75,11 @@ struct KalmanCovariances{ end end -"Outer constructor to convert covariance matrix number type to `NT` if necessary." +"Outer constructor to validate and convert covariance matrices if necessary." function KalmanCovariances( model::SimModel{NT}, i_ym, nint_u, nint_ym, Q̂, R̂, P̂_0=nothing, He=1 ) where {NT<:Real} + validate_kfcov(model, i_ym, nint_u, nint_ym, Q̂, R̂, P̂_0) return KalmanCovariances{NT}(model, i_ym, nint_u, nint_ym, NT.(Q̂), NT.(R̂), P̂_0, He) end diff --git a/src/estimator/kalman.jl b/src/estimator/kalman.jl index 5fcc456fc..ceafc2690 100644 --- a/src/estimator/kalman.jl +++ b/src/estimator/kalman.jl @@ -494,12 +494,16 @@ function update_estimate!(estim::KalmanFilter, y0m, d0, u0) end -struct UnscentedKalmanFilter{NT<:Real, SM<:SimModel} <: StateEstimator{NT} +struct UnscentedKalmanFilter{ + NT<:Real, + SM<:SimModel, + KC<:KalmanCovariances +} <: StateEstimator{NT} model::SM + cov ::KC x̂op ::Vector{NT} f̂op ::Vector{NT} x̂0 ::Vector{NT} - P̂::Hermitian{NT, Matrix{NT}} i_ym::Vector{Int} nx̂ ::Int nym::Int @@ -517,9 +521,6 @@ struct UnscentedKalmanFilter{NT<:Real, SM<:SimModel} <: StateEstimator{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} @@ -534,8 +535,8 @@ struct UnscentedKalmanFilter{NT<:Real, SM<:SimModel} <: StateEstimator{NT} corrected::Vector{Bool} buffer::StateEstimatorBuffer{NT} function UnscentedKalmanFilter{NT}( - model::SM, i_ym, nint_u, nint_ym, P̂_0, Q̂, R̂, α, β, κ; direct=true - ) where {NT<:Real, SM<:SimModel{NT}} + model::SM, i_ym, nint_u, nint_ym, cov::KC, α, β, κ; direct=true + ) where {NT<:Real, SM<:SimModel{NT}, KC<:KalmanCovariances} nu, ny, nd, nk = model.nu, model.ny, model.nd, model.nk nym, nyu = validate_ym(model, i_ym) As, Cs_u, Cs_y, nint_u, nint_ym = init_estimstoch(model, i_ym, nint_u, nint_ym) @@ -543,25 +544,21 @@ struct UnscentedKalmanFilter{NT<:Real, SM<:SimModel} <: StateEstimator{NT} 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̂, α, β, κ) x̂0 = [zeros(NT, model.nx); zeros(NT, nxs)] - Q̂, R̂ = Hermitian(Q̂, :L), Hermitian(R̂, :L) - P̂_0 = Hermitian(P̂_0, :L) - P̂ = Hermitian(copy(P̂_0.data), :L) # copy on P̂_0.data necessary for Julia Nightly K̂ = zeros(NT, nx̂, nym) M̂ = Hermitian(zeros(NT, nym, nym), :L) 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, nk) - return new{NT, SM}( + return new{NT, SM, KC}( model, - x̂op, f̂op, x̂0, P̂, + cov, + 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, Ĉm, D̂dm, - P̂_0, Q̂, R̂, K̂, M̂, X̂0, X̄0, Ŷ0m, Ȳ0m, nσ, γ, m̂, Ŝ, @@ -681,12 +678,10 @@ function UnscentedKalmanFilter( κ = kappa, ) where {NT<:Real, SM<:SimModel{NT}} # estimated covariances matrices (variance = σ²) : - P̂_0 = Hermitian(diagm(NT[σP_0; σPint_u_0; σPint_ym_0].^2), :L) - Q̂ = Hermitian(diagm(NT[σQ; σQint_u; σQint_ym ].^2), :L) - R̂ = Hermitian(diagm(NT[σR;].^2), :L) - return UnscentedKalmanFilter{NT}( - model, i_ym, nint_u, nint_ym, P̂_0, Q̂, R̂, α, β, κ; direct - ) + P̂_0 = Diagonal([σP_0; σPint_u_0; σPint_ym_0].^2) + Q̂ = Diagonal([σQ; σQint_u; σQint_ym ].^2) + R̂ = Diagonal([σR;].^2) + return UnscentedKalmanFilter(model, i_ym, nint_u, nint_ym, P̂_0, Q̂, R̂, α, β, κ; direct) end @doc raw""" @@ -702,9 +697,8 @@ function UnscentedKalmanFilter( model::SM, i_ym, nint_u, nint_ym, P̂_0, Q̂, R̂, α=1e-3, β=2, κ=0; direct=true ) where {NT<:Real, SM<:SimModel{NT}} P̂_0, Q̂, R̂ = to_mat(P̂_0), to_mat(Q̂), to_mat(R̂) - return UnscentedKalmanFilter{NT}( - model, i_ym, nint_u, nint_ym, P̂_0, Q̂ , R̂, α, β, κ; direct - ) + cov = KalmanCovariances(model, i_ym, nint_u, nint_ym, Q̂, R̂, P̂_0) + return UnscentedKalmanFilter{NT}(model, i_ym, nint_u, nint_ym, cov, α, β, κ; direct) end @@ -746,7 +740,7 @@ end Do the same but for the [`UnscentedKalmanFilter`](@ref). """ function correct_estimate!(estim::UnscentedKalmanFilter, y0m, d0) - x̂0, P̂, R̂, K̂ = estim.x̂0, estim.P̂, estim.R̂, estim.K̂ + x̂0, P̂, R̂, K̂ = estim.x̂0, estim.cov.P̂, estim.cov.R̂, estim.K̂ nx̂ = estim.nx̂ γ, m̂, Ŝ = estim.γ, estim.m̂, estim.Ŝ # in-place operations to reduce allocations: @@ -781,7 +775,7 @@ function correct_estimate!(estim::UnscentedKalmanFilter, y0m, d0) M̂ = estim.M̂ v̂ = ŷ0m v̂ .= y0m .- ŷ0m - x̂0corr, P̂corr = estim.x̂0, estim.P̂ + x̂0corr, P̂corr = estim.x̂0, estim.cov.P̂ mul!(x̂0corr, K̂, v̂, 1, 1) K̂_M̂ = estim.buffer.K̂ mul!(K̂_M̂, K̂, M̂) @@ -789,7 +783,7 @@ function correct_estimate!(estim::UnscentedKalmanFilter, y0m, d0) 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) + estim.cov.P̂ .= Hermitian(P̂corr, :L) return nothing end @@ -841,8 +835,8 @@ function update_estimate!(estim::UnscentedKalmanFilter, y0m, d0, u0) if !estim.direct correct_estimate!(estim, y0m, d0) end - x̂0corr, X̂0corr, P̂corr = estim.x̂0, estim.X̂0, estim.P̂ - Q̂, nx̂ = estim.Q̂, estim.nx̂ + x̂0corr, X̂0corr, P̂corr = estim.x̂0, estim.X̂0, estim.cov.P̂ + Q̂, nx̂ = estim.cov.Q̂, estim.nx̂ γ, m̂, Ŝ = estim.γ, estim.m̂, estim.Ŝ x̂0next, û0, k0 = estim.buffer.x̂, estim.buffer.û, estim.buffer.k # in-place operations to reduce allocations: @@ -869,21 +863,22 @@ function update_estimate!(estim::UnscentedKalmanFilter, y0m, d0, u0) P̂next .+= Q̂ x̂0next .+= estim.f̂op .- estim.x̂op estim.x̂0 .= x̂0next - estim.P̂ .= Hermitian(P̂next, :L) + estim.cov.P̂ .= Hermitian(P̂next, :L) return nothing end struct ExtendedKalmanFilter{ NT<:Real, SM<:SimModel, + KC<:KalmanCovariances, JB<:AbstractADType, LF<:Function } <: StateEstimator{NT} model::SM + cov ::KC x̂op ::Vector{NT} f̂op ::Vector{NT} x̂0 ::Vector{NT} - P̂::Hermitian{NT, Matrix{NT}} i_ym::Vector{Int} nx̂ ::Int nym::Int @@ -901,9 +896,6 @@ struct ExtendedKalmanFilter{ 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} @@ -915,8 +907,8 @@ struct ExtendedKalmanFilter{ corrected::Vector{Bool} buffer::StateEstimatorBuffer{NT} function ExtendedKalmanFilter{NT}( - model::SM, i_ym, nint_u, nint_ym, P̂_0, Q̂, R̂; jacobian::JB, linfunc!::LF, direct=true - ) where {NT<:Real, SM<:SimModel, JB<:AbstractADType, LF<:Function} + model::SM, i_ym, nint_u, nint_ym, cov::KC; jacobian::JB, linfunc!::LF, direct=true + ) where {NT<:Real, SM<:SimModel, KC<:KalmanCovariances, JB<:AbstractADType, LF<:Function} nu, ny, nd, nk = model.nu, model.ny, model.nd, model.nk nym, nyu = validate_ym(model, i_ym) As, Cs_u, Cs_y, nint_u, nint_ym = init_estimstoch(model, i_ym, nint_u, nint_ym) @@ -924,23 +916,19 @@ struct ExtendedKalmanFilter{ 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) x̂0 = [zeros(NT, model.nx); zeros(NT, nxs)] - Q̂, R̂ = Hermitian(Q̂, :L), Hermitian(R̂, :L) - P̂_0 = Hermitian(P̂_0, :L) - P̂ = Hermitian(copy(P̂_0.data), :L) # copy on P̂_0.data necessary for Julia Nightly K̂ = zeros(NT, nx̂, nym) 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, nk) - return new{NT, SM, JB, LF}( + return new{NT, SM, KC, JB, LF}( model, - x̂op, f̂op, x̂0, P̂, + cov, + 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, Ĉm, D̂dm, - P̂_0, Q̂, R̂, K̂, F̂_û, F̂, Ĥ, Ĥm, jacobian, linfunc!, @@ -1029,12 +1017,11 @@ function ExtendedKalmanFilter( σQint_ym = sigmaQint_ym, ) where {NT<:Real, SM<:SimModel{NT}} # estimated covariances matrices (variance = σ²) : - P̂_0 = Hermitian(diagm(NT[σP_0; σPint_u_0; σPint_ym_0].^2), :L) - Q̂ = Hermitian(diagm(NT[σQ; σQint_u; σQint_ym ].^2), :L) - R̂ = Hermitian(diagm(NT[σR;].^2), :L) - linfunc! = get_ekf_linfunc(NT, model, i_ym, nint_u, nint_ym, jacobian) - return ExtendedKalmanFilter{NT}( - model, i_ym, nint_u, nint_ym, P̂_0, Q̂, R̂; jacobian, linfunc!, direct + P̂_0 = Diagonal([σP_0; σPint_u_0; σPint_ym_0].^2) + Q̂ = Diagonal([σQ; σQint_u; σQint_ym ].^2) + R̂ = Diagonal([σR;].^2) + return ExtendedKalmanFilter( + model, i_ym, nint_u, nint_ym, P̂_0, Q̂, R̂; jacobian, direct ) end @@ -1051,9 +1038,10 @@ function ExtendedKalmanFilter( model::SM, i_ym, nint_u, nint_ym, P̂_0, Q̂, R̂; jacobian=AutoForwardDiff(), direct=true ) where {NT<:Real, SM<:SimModel{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) linfunc! = get_ekf_linfunc(NT, model, i_ym, nint_u, nint_ym, jacobian) return ExtendedKalmanFilter{NT}( - model, i_ym, nint_u, nint_ym, P̂_0, Q̂, R̂; jacobian, direct, linfunc! + model, i_ym, nint_u, nint_ym, cov; jacobian, direct, linfunc! ) end diff --git a/src/estimator/mhe/construct.jl b/src/estimator/mhe/construct.jl index 4af87b140..4a3c18730 100644 --- a/src/estimator/mhe/construct.jl +++ b/src/estimator/mhe/construct.jl @@ -47,12 +47,14 @@ end struct MovingHorizonEstimator{ NT<:Real, SM<:SimModel, + KC<:KalmanCovariances, JM<:JuMP.GenericModel, GB<:AbstractADType, JB<:AbstractADType, CE<:StateEstimator, } <: StateEstimator{NT} model::SM + cov ::KC # note: `NT` and the number type `JNT` in `JuMP.GenericModel{JNT}` can be # different since solvers that support non-Float64 are scarce. optim::JM @@ -94,12 +96,6 @@ struct MovingHorizonEstimator{ H̃::Hermitian{NT, Matrix{NT}} q̃::Vector{NT} r::Vector{NT} - P̂_0::Hermitian{NT, Matrix{NT}} - Q̂::Hermitian{NT, Matrix{NT}} - R̂::Hermitian{NT, Matrix{NT}} - invP̄::Hermitian{NT, Matrix{NT}} - invQ̂_He::Hermitian{NT, Matrix{NT}} - invR̂_He::Hermitian{NT, Matrix{NT}} C::NT X̂op::Vector{NT} X̂0 ::Vector{NT} @@ -115,12 +111,13 @@ struct MovingHorizonEstimator{ buffer::StateEstimatorBuffer{NT} function MovingHorizonEstimator{NT}( model::SM, - He, i_ym, nint_u, nint_ym, P̂_0, Q̂, R̂, Cwt, + He, i_ym, nint_u, nint_ym, cov::KC, Cwt, optim::JM, gradient::GB, jacobian::JB, covestim::CE; direct=true ) where { NT<:Real, SM<:SimModel{NT}, + KC<:KalmanCovariances, JM<:JuMP.GenericModel, GB<:AbstractADType, JB<:AbstractADType, @@ -135,7 +132,6 @@ struct MovingHorizonEstimator{ 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)] r = direct ? 0 : 1 @@ -157,25 +153,14 @@ struct MovingHorizonEstimator{ U0, D0 = zeros(NT, nu*He), zeros(NT, nD0) Ŵ = zeros(NT, nx̂*He) buffer = StateEstimatorBuffer{NT}(nu, nx̂, nym, ny, nd, nk) - P̂_0 = Hermitian(P̂_0, :L) - Q̂, R̂ = Hermitian(Q̂, :L), Hermitian(R̂, :L) - invP̄ = Hermitian(buffer.P̂, :L) - invP̄ .= P̂_0 - inv!(invP̄) - invQ̂ = Hermitian(buffer.Q̂, :L) - invQ̂ .= Q̂ - inv!(invQ̂) - invR̂ = Hermitian(buffer.R̂, :L) - invR̂ .= R̂ - inv!(invR̂) - 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) + P̂arr_old = copy(cov.P̂_0) Nk = [0] corrected = [false] - estim = new{NT, SM, JM, GB, JB, CE}( - model, optim, con, + estim = new{NT, SM, KC, JM, GB, JB, CE}( + model, + cov, + optim, con, gradient, jacobian, covestim, Z̃, lastu0, x̂op, f̂op, x̂0, @@ -185,7 +170,7 @@ struct MovingHorizonEstimator{ Â, B̂u, Ĉ, B̂d, D̂d, Ĉm, D̂dm, Ẽ, F, G, J, B, ẽx̄, fx̄, H̃, q̃, r, - P̂_0, Q̂, R̂, invP̄, invQ̂_He, invR̂_He, Cwt, + Cwt, X̂op, X̂0, Y0m, U0, D0, Ŵ, x̂0arr_old, P̂arr_old, Nk, direct, corrected, @@ -405,9 +390,9 @@ function MovingHorizonEstimator( σQint_ym = sigmaQint_ym, ) where {NT<:Real, SM<:SimModel{NT}, JM<:JuMP.GenericModel} # estimated covariances matrices (variance = σ²) : - P̂_0 = Hermitian(diagm(NT[σP_0; σPint_u_0; σPint_ym_0].^2), :L) - Q̂ = Hermitian(diagm(NT[σQ; σQint_u; σQint_ym ].^2), :L) - R̂ = Hermitian(diagm(NT[σR;].^2), :L) + P̂_0 = Diagonal([σP_0; σPint_u_0; σPint_ym_0].^2) + Q̂ = Diagonal([σQ; σQint_u; σQint_ym ].^2) + R̂ = Diagonal([σR;].^2) isnothing(He) && throw(ArgumentError("Estimation horizon He must be explicitly specified")) return MovingHorizonEstimator( model, He, i_ym, nint_u, nint_ym, P̂_0, Q̂, R̂, Cwt; direct, optim, gradient, jacobian @@ -445,9 +430,10 @@ function MovingHorizonEstimator( covestim::CE = default_covestim_mhe(model, i_ym, nint_u, nint_ym, P̂_0, Q̂, R̂; direct) ) 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) return MovingHorizonEstimator{NT}( model, - He, i_ym, nint_u, nint_ym, P̂_0, Q̂ , R̂, Cwt, + He, i_ym, nint_u, nint_ym, cov, Cwt, optim, gradient, jacobian, covestim; direct ) diff --git a/src/estimator/mhe/execute.jl b/src/estimator/mhe/execute.jl index ec79a783b..e820325a0 100644 --- a/src/estimator/mhe/execute.jl +++ b/src/estimator/mhe/execute.jl @@ -17,8 +17,8 @@ function init_estimate_cov!(estim::MovingHorizonEstimator, _ , d0, u0) end estim.lastu0 .= u0 # estim.P̂_0 is in fact P̂(-1|-1) is estim.direct==false, else P̂(-1|0) - invert_cov!(estim, estim.P̂_0) - estim.P̂arr_old .= estim.P̂_0 + invert_cov!(estim, estim.cov.P̂_0) + estim.P̂arr_old .= estim.cov.P̂_0 estim.x̂0arr_old .= 0 return nothing end @@ -249,6 +249,7 @@ of the time-varying ``\mathbf{P̄}`` covariance . The computed variables are: ``` """ function initpred!(estim::MovingHorizonEstimator, model::LinModel) + invP̄, invQ̂_He, invR̂_He = estim.cov.invP̄, estim.cov.invQ̂_He, estim.cov.invR̂_He F, C, optim = estim.F, estim.C, estim.optim nx̂, nŵ, nym, nϵ, Nk = estim.nx̂, estim.nx̂, estim.nym, estim.nϵ, estim.Nk[] nYm, nŴ = nym*Nk, nŵ*Nk @@ -263,8 +264,8 @@ function initpred!(estim::MovingHorizonEstimator, model::LinModel) # --- update H̃, q̃ and p vectors for quadratic optimization --- ẼZ̃ = @views [estim.ẽx̄[:, 1:nZ̃]; estim.Ẽ[1:nYm, 1:nZ̃]] FZ̃ = @views [estim.fx̄; estim.F[1:nYm]] - invQ̂_Nk, invR̂_Nk = @views estim.invQ̂_He[1:nŴ, 1:nŴ], estim.invR̂_He[1:nYm, 1:nYm] - M_Nk = [estim.invP̄ zeros(nx̂, nYm); zeros(nYm, nx̂) invR̂_Nk] + invQ̂_Nk, invR̂_Nk = @views invQ̂_He[1:nŴ, 1:nŴ], invR̂_He[1:nYm, 1:nYm] + M_Nk = [invP̄ zeros(nx̂, nYm); zeros(nYm, nx̂) invR̂_Nk] Ñ_Nk = [fill(C, nϵ, nϵ) zeros(nϵ, nx̂+nŴ); zeros(nx̂, nϵ+nx̂+nŴ); zeros(nŴ, nϵ+nx̂) invQ̂_Nk] M_Nk_ẼZ̃ = M_Nk*ẼZ̃ @views mul!(estim.q̃[1:nZ̃], M_Nk_ẼZ̃', FZ̃) @@ -480,12 +481,12 @@ function correct_cov!(estim::MovingHorizonEstimator) y0marr, d0arr = buffer.ym, buffer.d y0marr .= @views estim.Y0m[1:nym] d0arr .= @views estim.D0[1:nd] - estim.covestim.x̂0 .= estim.x̂0arr_old - estim.covestim.P̂ .= estim.P̂arr_old + estim.covestim.x̂0 .= estim.x̂0arr_old + estim.covestim.cov.P̂ .= estim.P̂arr_old try correct_estimate!(estim.covestim, y0marr, d0arr) - all(isfinite, estim.covestim.P̂) || error("Arrival covariance P̄ is not finite") - estim.P̂arr_old .= estim.covestim.P̂ + all(isfinite, estim.covestim.cov.P̂) || error("Arrival covariance P̄ is not finite") + estim.P̂arr_old .= estim.covestim.cov.P̂ invert_cov!(estim, estim.P̂arr_old) catch err if err isa PosDefException @@ -507,12 +508,12 @@ function update_cov!(estim::MovingHorizonEstimator) u0arr .= @views estim.U0[1:nu] y0marr .= @views estim.Y0m[1:nym] d0arr .= @views estim.D0[1:nd] - estim.covestim.x̂0 .= estim.x̂0arr_old - estim.covestim.P̂ .= estim.P̂arr_old + estim.covestim.x̂0 .= estim.x̂0arr_old + estim.covestim.cov.P̂ .= estim.P̂arr_old try update_estimate!(estim.covestim, y0marr, d0arr, u0arr) - all(isfinite, estim.covestim.P̂) || error("Arrival covariance P̄ is not finite") - estim.P̂arr_old .= estim.covestim.P̂ + all(isfinite, estim.covestim.cov.P̂) || error("Arrival covariance P̄ is not finite") + estim.P̂arr_old .= estim.covestim.cov.P̂ invert_cov!(estim, estim.P̂arr_old) catch err if err isa PosDefException @@ -528,10 +529,9 @@ end "Invert the covariance estimate at arrival `P̄`." function invert_cov!(estim::MovingHorizonEstimator, P̄) - invP̄ = Hermitian(estim.buffer.P̂, :L) - invP̄ .= P̄ + estim.cov.invP̄ .= P̄ try - inv!(invP̄) + inv!(estim.cov.invP̄) catch err if err isa PosDefException @error("Arrival covariance P̄ is not invertible: keeping the old one") @@ -568,9 +568,9 @@ function obj_nonlinprog!( x̄, estim::MovingHorizonEstimator, ::SimModel, V̂, Z̃::AbstractVector{NT} ) where NT<:Real nϵ, Nk = estim.nϵ, estim.Nk[] - nYm, nŴ, nx̂, invP̄ = Nk*estim.nym, Nk*estim.nx̂, estim.nx̂, estim.invP̄ + nYm, nŴ, nx̂, invP̄ = Nk*estim.nym, Nk*estim.nx̂, estim.nx̂, estim.cov.invP̄ nx̃ = nϵ + nx̂ - invQ̂_Nk, invR̂_Nk = @views estim.invQ̂_He[1:nŴ, 1:nŴ], estim.invR̂_He[1:nYm, 1:nYm] + invQ̂_Nk, invR̂_Nk = @views estim.cov.invQ̂_He[1:nŴ, 1:nŴ], estim.cov.invR̂_He[1:nYm, 1:nYm] x̂0arr, Ŵ, V̂ = @views Z̃[nx̃-nx̂+1:nx̃], Z̃[nx̃+1:nx̃+nŴ], V̂[1:nYm] x̄ .= estim.x̂0arr_old .- x̂0arr Jϵ = nϵ ≠ 0 ? estim.C*Z̃[begin]^2 : zero(NT) From 336d0326402d1b200eec4624b12c3a9f375ff162 Mon Sep 17 00:00:00 2001 From: franckgaga Date: Fri, 13 Jun 2025 10:13:13 -0400 Subject: [PATCH 04/10] debug: adding `.cov` where needed --- src/controller/execute.jl | 4 ++-- src/estimator/execute.jl | 12 ++++++------ src/estimator/kalman.jl | 10 +++++----- src/estimator/mhe/execute.jl | 10 +++++----- test/3_test_predictive_control.jl | 4 ++-- 5 files changed, 20 insertions(+), 20 deletions(-) diff --git a/src/controller/execute.jl b/src/controller/execute.jl index 7bd29861d..a5ea51563 100644 --- a/src/controller/execute.jl +++ b/src/controller/execute.jl @@ -535,12 +535,12 @@ prediction horizon ``H_p``. ```jldoctest julia> mpc = LinMPC(KalmanFilter(LinModel(ss(0.1, 0.5, 1, 0, 4.0)), σR=[√25]), Hp=1, Hc=1); -julia> mpc.estim.model.A[1], mpc.estim.R̂[1], mpc.weights.M_Hp[1], mpc.weights.Ñ_Hc[1] +julia> mpc.estim.model.A[1], mpc.estim.cov.R̂[1], mpc.weights.M_Hp[1], mpc.weights.Ñ_Hc[1] (0.1, 25.0, 1.0, 0.1) julia> setmodel!(mpc, LinModel(ss(0.42, 0.5, 1, 0, 4.0)); R̂=[9], M_Hp=[10], Nwt=[0.666]); -julia> mpc.estim.model.A[1], mpc.estim.R̂[1], mpc.weights.M_Hp[1], mpc.weights.Ñ_Hc[1] +julia> mpc.estim.model.A[1], mpc.estim.cov.R̂[1], mpc.weights.M_Hp[1], mpc.weights.Ñ_Hc[1] (0.42, 9.0, 10.0, 0.666) ``` """ diff --git a/src/estimator/execute.jl b/src/estimator/execute.jl index 560eab897..8de7c4336 100644 --- a/src/estimator/execute.jl +++ b/src/estimator/execute.jl @@ -112,7 +112,7 @@ removes the operating points with [`remove_op!`](@ref) and call [`init_estimate! bumpless manual to automatic transfer. See [`init_estimate!`](@ref) for details. - Else, `estim.x̂0` is left unchanged. Use [`setstate!`](@ref) to manually modify it. -If applicable, it also sets the error covariance `estim.P̂` to `estim.P̂_0`. +If applicable, it also sets the error covariance `estim.cov.P̂` to `estim.cov.P̂_0`. # Examples ```jldoctest @@ -327,7 +327,7 @@ end """ setstate!(estim::StateEstimator, x̂[, P̂]) -> estim -Set `estim.x̂0` to `x̂ - estim.x̂op` from the argument `x̂`, and `estim.P̂` to `P̂` if applicable. +Set `estim.x̂0` to `x̂ - estim.x̂op` from the argument `x̂`, and `estim.cov.P̂` to `P̂` if applicable. The covariance error estimate `P̂` can be set only if `estim` is a [`StateEstimator`](@ref) that computes it. @@ -339,11 +339,11 @@ function setstate!(estim::StateEstimator, x̂, P̂=nothing) return estim end -"Set the covariance error estimate `estim.P̂` to `P̂`." +"Set the covariance error estimate `estim.cov.P̂` to `P̂`." function setstate_cov!(estim::StateEstimator, P̂) if !isnothing(P̂) size(P̂) == (estim.nx̂, estim.nx̂) || error("P̂ size must be $((estim.nx̂, estim.nx̂))") - estim.P̂ .= to_hermitian(P̂) + estim.cov.P̂ .= to_hermitian(P̂) end return nothing end @@ -449,7 +449,7 @@ function setmodel_estimator!(estim::StateEstimator, model, _ , _ , _ , Q̂, R̂) estim.f̂op .= f̂op estim.x̂0 .-= estim.x̂op # convert x̂ to x̂0 with the new operating point # --- update covariance matrices --- - !isnothing(Q̂) && (estim.Q̂ .= to_hermitian(Q̂)) - !isnothing(R̂) && (estim.R̂ .= to_hermitian(R̂)) + !isnothing(Q̂) && (estim.cov.Q̂ .= to_hermitian(Q̂)) + !isnothing(R̂) && (estim.cov.R̂ .= to_hermitian(R̂)) return nothing end diff --git a/src/estimator/kalman.jl b/src/estimator/kalman.jl index ceafc2690..e886b3f1d 100644 --- a/src/estimator/kalman.jl +++ b/src/estimator/kalman.jl @@ -443,7 +443,7 @@ end @doc raw""" correct_estimate!(estim::KalmanFilter, y0m, d0) -Correct `estim.x̂0` and `estim.P̂` using the time-varying [`KalmanFilter`](@ref). +Correct `estim.x̂0` and `estim.cov.P̂` using the time-varying [`KalmanFilter`](@ref). It computes the corrected state estimate ``\mathbf{x̂}_{k}(k)`` estimation covariance ``\mathbf{P̂}_{k}(k)``. @@ -456,7 +456,7 @@ end @doc raw""" update_estimate!(estim::KalmanFilter, y0m, d0, u0) -Update [`KalmanFilter`](@ref) state `estim.x̂0` and estimation error covariance `estim.P̂`. +Update [`KalmanFilter`](@ref) state `estim.x̂0` and estimation error covariance `estim.cov.P̂`. It implements the classical time-varying Kalman Filter based on the process model described in [`SteadyKalmanFilter`](@ref). If `estim.direct == false`, it first corrects the estimate @@ -790,7 +790,7 @@ end @doc raw""" update_estimate!(estim::UnscentedKalmanFilter, y0m, d0, u0) -Update [`UnscentedKalmanFilter`](@ref) state `estim.x̂0` and covariance estimate `estim.P̂`. +Update [`UnscentedKalmanFilter`](@ref) state `estim.x̂0` and covariance estimate `estim.cov.P̂`. It implements the unscented Kalman Filter based on the generalized unscented transform[^3]. See [`init_ukf`](@ref) for the definition of the constants ``\mathbf{m̂, Ŝ}`` and ``γ``. The @@ -1107,7 +1107,7 @@ end @doc raw""" update_estimate!(estim::ExtendedKalmanFilter, y0m, d0, u0) -Update [`ExtendedKalmanFilter`](@ref) state `estim.x̂0` and covariance `estim.P̂`. +Update [`ExtendedKalmanFilter`](@ref) state `estim.x̂0` and covariance `estim.cov.P̂`. The equations are similar to [`update_estimate!(::KalmanFilter)`](@ref) but with the substitutions ``\mathbf{Ĉ^m = Ĥ^m}(k)`` and ``\mathbf{Â = F̂}(k)``, the Jacobians of the @@ -1158,7 +1158,7 @@ function update_estimate!(estim::ExtendedKalmanFilter{NT}, y0m, d0, u0) where NT return predict_estimate_kf!(estim, u0, d0, estim.F̂) end -"Set `estim.P̂` to `estim.P̂_0` for the time-varying Kalman Filters." +"Set `estim.cov.P̂` to `estim.cov.P̂_0` for the time-varying Kalman Filters." function init_estimate_cov!( estim::Union{KalmanFilter, UnscentedKalmanFilter, ExtendedKalmanFilter}, _ , _ , _ ) diff --git a/src/estimator/mhe/execute.jl b/src/estimator/mhe/execute.jl index e820325a0..eaeffe7d4 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.lastu0 .= u0 - # estim.P̂_0 is in fact P̂(-1|-1) is estim.direct==false, else P̂(-1|0) + # estim.cov.P̂_0 is in fact P̂(-1|-1) is estim.direct==false, else P̂(-1|0) invert_cov!(estim, estim.cov.P̂_0) estim.P̂arr_old .= estim.cov.P̂_0 estim.x̂0arr_old .= 0 @@ -793,16 +793,16 @@ function setmodel_estimator!( estim.x̂0arr_old .-= x̂op # --- covariance matrices --- if !isnothing(Q̂) - estim.Q̂ .= to_hermitian(Q̂) + estim.cov.Q̂ .= to_hermitian(Q̂) invQ̂ = Hermitian(estim.buffer.Q̂, :L) - invQ̂ .= estim.Q̂ + invQ̂ .= estim.cov.Q̂ inv!(invQ̂) estim.invQ̂_He .= Hermitian(repeatdiag(invQ̂, He), :L) end if !isnothing(R̂) - estim.R̂ .= to_hermitian(R̂) + estim.cov.R̂ .= to_hermitian(R̂) invR̂ = Hermitian(estim.buffer.R̂, :L) - invR̂ .= estim.R̂ + invR̂ .= estim.cov.R̂ inv!(invR̂) estim.invR̂_He .= Hermitian(repeatdiag(invR̂, He), :L) end diff --git a/test/3_test_predictive_control.jl b/test/3_test_predictive_control.jl index 95b171c2e..5d6b89df6 100644 --- a/test/3_test_predictive_control.jl +++ b/test/3_test_predictive_control.jl @@ -215,8 +215,8 @@ end @test initstate!(mpc1, [10, 50], [50, 30+1]) ≈ [zeros(3); [1]] setstate!(mpc1, [1,2,3,4], diagm(.1:.1:.4)) @test mpc1.estim.x̂0 ≈ [1,2,3,4] - @test mpc1.estim.P̂ ≈ diagm(.1:.1:.4) - setstate!(mpc1, [0,0,0,0], mpc1.estim.P̂_0) + @test mpc1.estim.cov.P̂ ≈ diagm(.1:.1:.4) + setstate!(mpc1, [0,0,0,0], mpc1.estim.cov.P̂_0) preparestate!(mpc1, [50, 30]) updatestate!(mpc1, mpc1.estim.model.uop, [50, 30]) @test mpc1.estim.x̂0 ≈ [0,0,0,0] From 63b4387cf0656556c6ce0d7b1a2b3a55cafb2a61 Mon Sep 17 00:00:00 2001 From: franckgaga Date: Fri, 13 Jun 2025 11:23:32 -0400 Subject: [PATCH 05/10] test: adapt tests to new `cov` field --- src/ModelPredictiveControl.jl | 2 +- src/estimator/construct.jl | 32 ++++- src/estimator/internal_model.jl | 4 +- src/estimator/kalman.jl | 6 + src/estimator/luenberger.jl | 4 +- src/estimator/manual.jl | 8 +- src/estimator/mhe/execute.jl | 4 +- test/2_test_state_estim.jl | 226 ++++++++++++++++---------------- 8 files changed, 161 insertions(+), 125 deletions(-) diff --git a/src/ModelPredictiveControl.jl b/src/ModelPredictiveControl.jl index b30c7323d..38b7623bd 100644 --- a/src/ModelPredictiveControl.jl +++ b/src/ModelPredictiveControl.jl @@ -54,7 +54,7 @@ include("plot_sim.jl") @compile_workload begin # all calls in this block will be precompiled, regardless of whether # they belong to your package or not (on Julia 1.8 and higher) - # include("precompile.jl") + include("precompile.jl") end end diff --git a/src/estimator/construct.jl b/src/estimator/construct.jl index f78e663d7..e2eedf5a3 100644 --- a/src/estimator/construct.jl +++ b/src/estimator/construct.jl @@ -64,9 +64,33 @@ struct KalmanCovariances{ R̂ = Hermitian(R̂, :L) # the following variables are only for the moving horizon estimator: invP̄, invQ̂, invR̂ = copy(P̂_0), copy(Q̂), copy(R̂) - inv!(invP̄) - inv!(invQ̂) - inv!(invR̂) + try + inv!(invP̄) + catch err + if err isa PosDefException + error("P̂_0 is not positive definite") + else + rethrow() + end + end + try + inv!(invQ̂) + catch err + if err isa PosDefException + error("Q̂ is not positive definite") + else + rethrow() + end + end + try + inv!(invR̂) + catch err + if err isa PosDefException + error("R̂ is not positive definite") + else + rethrow() + end + end invQ̂_He = repeatdiag(invQ̂, He) invR̂_He = repeatdiag(invR̂, He) invQ̂_He = Hermitian(invQ̂_He, :L) @@ -80,7 +104,7 @@ function KalmanCovariances( model::SimModel{NT}, i_ym, nint_u, nint_ym, Q̂, R̂, P̂_0=nothing, He=1 ) where {NT<:Real} validate_kfcov(model, i_ym, nint_u, nint_ym, Q̂, R̂, P̂_0) - return KalmanCovariances{NT}(model, i_ym, nint_u, nint_ym, NT.(Q̂), NT.(R̂), P̂_0, He) + return KalmanCovariances{NT}(model, i_ym, nint_u, nint_ym, NT.(Q̂), NT.(R̂), NT.(P̂_0), He) end """ diff --git a/src/estimator/internal_model.jl b/src/estimator/internal_model.jl index 62eef27ed..94dde0b53 100644 --- a/src/estimator/internal_model.jl +++ b/src/estimator/internal_model.jl @@ -225,8 +225,8 @@ function init_internalmodel(As, Bs, Cs, Ds) end "Throw an error if P̂ != nothing." -function setstate_cov!(estim::InternalModel, P̂) - P̂ == nothing || error("InternalModel does not compute an estimation covariance matrix P̂.") +function setstate_cov!(::InternalModel, P̂) + isnothing(P̂) || error("InternalModel does not compute an estimation covariance matrix P̂.") return nothing end diff --git a/src/estimator/kalman.jl b/src/estimator/kalman.jl index e886b3f1d..55c31d8bd 100644 --- a/src/estimator/kalman.jl +++ b/src/estimator/kalman.jl @@ -214,6 +214,12 @@ 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) diff --git a/src/estimator/luenberger.jl b/src/estimator/luenberger.jl index c186bf157..c63e7f4a1 100644 --- a/src/estimator/luenberger.jl +++ b/src/estimator/luenberger.jl @@ -135,8 +135,8 @@ function update_estimate!(estim::Luenberger, y0m, d0, u0) end "Throw an error if P̂ != nothing." -function setstate_cov!(estim::Luenberger, P̂) - P̂ == nothing || error("Luenberger does not compute an estimation covariance matrix P̂.") +function setstate_cov!(::Luenberger, P̂) + isnothing(P̂) || error("Luenberger does not compute an estimation covariance matrix P̂.") return nothing end diff --git a/src/estimator/manual.jl b/src/estimator/manual.jl index 1c4d21a14..bc848385d 100644 --- a/src/estimator/manual.jl +++ b/src/estimator/manual.jl @@ -147,4 +147,10 @@ end Do nothing for [`ManualEstimator`](@ref). """ -update_estimate!(estim::ManualEstimator, y0m, d0, u0) = nothing \ No newline at end of file +update_estimate!(::ManualEstimator, y0m, d0, u0) = nothing + +"Throw an error if P̂ != nothing." +function setstate_cov!(::ManualEstimator, P̂) + isnothing(P̂) || error("ManualEstimator does not compute an estimation covariance matrix P̂.") + return nothing +end \ No newline at end of file diff --git a/src/estimator/mhe/execute.jl b/src/estimator/mhe/execute.jl index eaeffe7d4..727f50f79 100644 --- a/src/estimator/mhe/execute.jl +++ b/src/estimator/mhe/execute.jl @@ -686,8 +686,8 @@ end con_nonlinprog!(g, ::MovingHorizonEstimator, ::LinModel, _ , _ , _ ) = g "Throw an error if P̂ != nothing." -function setstate_cov!(estim::MovingHorizonEstimator, P̂) - P̂ == nothing || error("MovingHorizonEstimator does not compute an estimation covariance matrix P̂.") +function setstate_cov!(::MovingHorizonEstimator, P̂) + isnothing(P̂) || error("MovingHorizonEstimator does not compute an estimation covariance matrix P̂.") return nothing end diff --git a/test/2_test_state_estim.jl b/test/2_test_state_estim.jl index b85d009e7..7f4751db2 100644 --- a/test/2_test_state_estim.jl +++ b/test/2_test_state_estim.jl @@ -1,33 +1,33 @@ @testitem "SteadyKalmanFilter construction" setup=[SetupMPCtests] begin using .SetupMPCtests, ControlSystemsBase, LinearAlgebra linmodel = LinModel(sys,Ts,i_u=[1,2]) - manual1 = SteadyKalmanFilter(linmodel) - @test manual1.nym == 2 - @test manual1.nyu == 0 - @test manual1.nxs == 2 - @test manual1.nx̂ == 4 - @test manual1.nint_ym == [1, 1] + kalmanfilter1 = SteadyKalmanFilter(linmodel) + @test kalmanfilter1.nym == 2 + @test kalmanfilter1.nyu == 0 + @test kalmanfilter1.nxs == 2 + @test kalmanfilter1.nx̂ == 4 + @test kalmanfilter1.nint_ym == [1, 1] linmodel2 = LinModel(sys,Ts,i_d=[3]) - manual2 = SteadyKalmanFilter(linmodel2, i_ym=[2]) - @test manual2.nym == 1 - @test manual2.nyu == 1 - @test manual2.nxs == 1 - @test manual2.nx̂ == 5 - @test manual2.nint_ym == [1] + kalmanfilter2 = SteadyKalmanFilter(linmodel2, i_ym=[2]) + @test kalmanfilter2.nym == 1 + @test kalmanfilter2.nyu == 1 + @test kalmanfilter2.nxs == 1 + @test kalmanfilter2.nx̂ == 5 + @test kalmanfilter2.nint_ym == [1] - manual3 = SteadyKalmanFilter(linmodel, nint_ym=0) - @test manual3.nxs == 0 - @test manual3.nx̂ == 2 - @test manual3.nint_ym == [0, 0] + kalmanfilter3 = SteadyKalmanFilter(linmodel, nint_ym=0) + @test kalmanfilter3.nxs == 0 + @test kalmanfilter3.nx̂ == 2 + @test kalmanfilter3.nint_ym == [0, 0] - manual4 = SteadyKalmanFilter(linmodel, nint_ym=[2,2]) - @test manual4.nxs == 4 - @test manual4.nx̂ == 6 + kalmanfilter4 = SteadyKalmanFilter(linmodel, nint_ym=[2,2]) + @test kalmanfilter4.nxs == 4 + @test kalmanfilter4.nx̂ == 6 skalmanfilter5 = SteadyKalmanFilter(linmodel2, σQ=[1,2,3,4], σQint_ym=[5, 6], σR=[7, 8]) - @test skalmanfilter5.Q̂ ≈ Hermitian(diagm(Float64[1, 4, 9 ,16, 25, 36])) - @test skalmanfilter5.R̂ ≈ Hermitian(diagm(Float64[49, 64])) + @test skalmanfilter5.cov.Q̂ ≈ Hermitian(diagm(Float64[1, 4, 9 ,16, 25, 36])) + @test skalmanfilter5.cov.R̂ ≈ Hermitian(diagm(Float64[49, 64])) linmodel3 = LinModel(append(tf(1,[1, 0]),tf(1,[10, 1]),tf(1,[-1, 1])), 0.1) skalmanfilter6 = SteadyKalmanFilter(linmodel3) @@ -35,19 +35,19 @@ @test skalmanfilter6.nx̂ == 5 @test skalmanfilter6.nint_ym == [0, 1, 1] - manual5 = SteadyKalmanFilter(linmodel, nint_u=[1,1]) - @test manual5.nxs == 2 - @test manual5.nx̂ == 4 - @test manual5.nint_u == [1, 1] - @test manual5.nint_ym == [0, 0] + kalmanfilter5 = SteadyKalmanFilter(linmodel, nint_u=[1,1]) + @test kalmanfilter5.nxs == 2 + @test kalmanfilter5.nx̂ == 4 + @test kalmanfilter5.nint_u == [1, 1] + @test kalmanfilter5.nint_ym == [0, 0] linmodel2 = LinModel{Float32}(0.5*ones(1,1), ones(1,1), ones(1,1), zeros(1,0), zeros(1,0), 1.0) - manual6 = SteadyKalmanFilter(linmodel2) - @test isa(manual6, SteadyKalmanFilter{Float32}) + kalmanfilter6 = SteadyKalmanFilter(linmodel2) + @test isa(kalmanfilter6, SteadyKalmanFilter{Float32}) skalmanfilter9 = SteadyKalmanFilter(linmodel, 1:2, 0, [1, 1], I(4), I(2)) - @test skalmanfilter9.Q̂ ≈ I(4) - @test skalmanfilter9.R̂ ≈ I(2) + @test skalmanfilter9.cov.Q̂ ≈ I(4) + @test skalmanfilter9.cov.R̂ ≈ I(2) @test_throws ErrorException SteadyKalmanFilter(linmodel, nint_ym=[1,1,1]) @test_throws ErrorException SteadyKalmanFilter(linmodel, nint_ym=[-1,0]) @@ -64,58 +64,58 @@ end @testitem "SteadyKalmanFilter estimator methods" setup=[SetupMPCtests] begin using .SetupMPCtests, ControlSystemsBase, LinearAlgebra linmodel = setop!(LinModel(sys,Ts,i_u=[1,2]), uop=[10,50], yop=[50,30]) - manual1 = SteadyKalmanFilter(linmodel, nint_ym=[1, 1]) + kalmanfilter1 = SteadyKalmanFilter(linmodel, nint_ym=[1, 1]) u, y, d = [10, 50], [50, 30], Float64[] - preparestate!(manual1, y) - @test updatestate!(manual1, u, y) ≈ zeros(4) - preparestate!(manual1, y) - @test updatestate!(manual1, u, y, d) ≈ zeros(4) - @test manual1.x̂0 ≈ zeros(4) - @test_skip @allocations(preparestate!(manual1, y)) == 0 - @test_skip @allocations(updatestate!(manual1, u, y)) == 0 - preparestate!(manual1, y) - @test evaloutput(manual1) ≈ manual1() ≈ [50, 30] - @test evaloutput(manual1, d) ≈ manual1(d) ≈ [50, 30] - @test_skip @allocations(evaloutput(manual1, d)) == 0 - @test initstate!(manual1, [10, 50], [50, 30+1]) ≈ [zeros(3); [1]] + preparestate!(kalmanfilter1, y) + @test updatestate!(kalmanfilter1, u, y) ≈ zeros(4) + preparestate!(kalmanfilter1, y) + @test updatestate!(kalmanfilter1, u, y, d) ≈ zeros(4) + @test kalmanfilter1.x̂0 ≈ zeros(4) + @test_skip @allocations(preparestate!(kalmanfilter1, y)) == 0 + @test_skip @allocations(updatestate!(kalmanfilter1, u, y)) == 0 + preparestate!(kalmanfilter1, y) + @test evaloutput(kalmanfilter1) ≈ kalmanfilter1() ≈ [50, 30] + @test evaloutput(kalmanfilter1, d) ≈ kalmanfilter1(d) ≈ [50, 30] + @test_skip @allocations(evaloutput(kalmanfilter1, d)) == 0 + @test initstate!(kalmanfilter1, [10, 50], [50, 30+1]) ≈ [zeros(3); [1]] linmodel2 = LinModel(append(tf(1, [1, 0]), tf(2, [10, 1])), 1.0) - manual2 = SteadyKalmanFilter(linmodel2, nint_u=[1, 1], direct=false) - x = initstate!(manual2, [10, 3], [0.5, 6+0.1]) - @test evaloutput(manual2) ≈ [0.5, 6+0.1] - @test updatestate!(manual2, [10, 3], [0.5, 6+0.1]) ≈ x - setstate!(manual1, [1,2,3,4]) - @test manual1.x̂0 ≈ [1,2,3,4] + kalmanfilter2 = SteadyKalmanFilter(linmodel2, nint_u=[1, 1], direct=false) + x = initstate!(kalmanfilter2, [10, 3], [0.5, 6+0.1]) + @test evaloutput(kalmanfilter2) ≈ [0.5, 6+0.1] + @test updatestate!(kalmanfilter2, [10, 3], [0.5, 6+0.1]) ≈ x + setstate!(kalmanfilter1, [1,2,3,4]) + @test kalmanfilter1.x̂0 ≈ [1,2,3,4] for i in 1:40 - preparestate!(manual1, [50, 30]) - updatestate!(manual1, [11, 52], [50, 30]) + preparestate!(kalmanfilter1, [50, 30]) + updatestate!(kalmanfilter1, [11, 52], [50, 30]) end - preparestate!(manual1, [50, 30]) - @test manual1() ≈ [50, 30] atol=1e-3 + preparestate!(kalmanfilter1, [50, 30]) + @test kalmanfilter1() ≈ [50, 30] atol=1e-3 for i in 1:40 - preparestate!(manual1, [51, 32]) - updatestate!(manual1, [10, 50], [51, 32]) + preparestate!(kalmanfilter1, [51, 32]) + updatestate!(kalmanfilter1, [10, 50], [51, 32]) end - preparestate!(manual1, [51, 32]) - @test manual1() ≈ [51, 32] atol=1e-3 - manual2 = SteadyKalmanFilter(linmodel, nint_u=[1, 1], direct=false) + preparestate!(kalmanfilter1, [51, 32]) + @test kalmanfilter1() ≈ [51, 32] atol=1e-3 + kalmanfilter2 = SteadyKalmanFilter(linmodel, nint_u=[1, 1], direct=false) for i in 1:40 - preparestate!(manual2, [50, 30]) - updatestate!(manual2, [11, 52], [50, 30]) + preparestate!(kalmanfilter2, [50, 30]) + updatestate!(kalmanfilter2, [11, 52], [50, 30]) end - @test manual2() ≈ [50, 30] atol=1e-3 + @test kalmanfilter2() ≈ [50, 30] atol=1e-3 for i in 1:40 - preparestate!(manual2, [51, 32]) - updatestate!(manual2, [10, 50], [51, 32]) + preparestate!(kalmanfilter2, [51, 32]) + updatestate!(kalmanfilter2, [10, 50], [51, 32]) end - @test manual2() ≈ [51, 32] atol=1e-3 + @test kalmanfilter2() ≈ [51, 32] atol=1e-3 linmodel3 = LinModel{Float32}(0.5*ones(1,1), ones(1,1), ones(1,1), zeros(1,0), zeros(1,0), 1.0) - manual3 = SteadyKalmanFilter(linmodel3) - preparestate!(manual3, [0]) - x̂ = updatestate!(manual3, [0], [0]) + kalmanfilter3 = SteadyKalmanFilter(linmodel3) + preparestate!(kalmanfilter3, [0]) + x̂ = updatestate!(kalmanfilter3, [0], [0]) @test x̂ ≈ [0, 0] @test isa(x̂, Vector{Float32}) - @test_throws ArgumentError updatestate!(manual1, [10, 50]) - @test_throws ErrorException setstate!(manual1, [1,2,3,4], diagm(.1:.1:.4)) + @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 @@ -132,13 +132,13 @@ end @testitem "SteadyKalmanFilter real-time simulations" setup=[SetupMPCtests] begin using .SetupMPCtests, ControlSystemsBase, LinearAlgebra linmodel = LinModel(tf(2, [10, 1]), 0.25) - manual1 = SteadyKalmanFilter(linmodel) + kalmanfilter1 = SteadyKalmanFilter(linmodel) times1 = zeros(5) for i=1:5 - times1[i] = savetime!(manual1) - preparestate!(manual1, [1]) - updatestate!(manual1, [1], [1]) - periodsleep(manual1, true) + times1[i] = savetime!(kalmanfilter1) + preparestate!(kalmanfilter1, [1]) + updatestate!(kalmanfilter1, [1], [1]) + periodsleep(kalmanfilter1, true) end @test all(isapprox.(diff(times1[2:end]), 0.25, atol=0.01)) end @@ -169,13 +169,13 @@ end @test kalmanfilter4.nx̂ == 6 kalmanfilter5 = KalmanFilter(linmodel2, σQ=[1,2,3,4], σQint_ym=[5, 6], σR=[7, 8]) - @test kalmanfilter5.Q̂ ≈ Hermitian(diagm(Float64[1, 4, 9 ,16, 25, 36])) - @test kalmanfilter5.R̂ ≈ Hermitian(diagm(Float64[49, 64])) + @test kalmanfilter5.cov.Q̂ ≈ Hermitian(diagm(Float64[1, 4, 9 ,16, 25, 36])) + @test kalmanfilter5.cov.R̂ ≈ Hermitian(diagm(Float64[49, 64])) kalmanfilter6 = KalmanFilter(linmodel2, σP_0=[1,2,3,4], σPint_ym_0=[5,6]) - @test kalmanfilter6.P̂_0 ≈ Hermitian(diagm(Float64[1, 4, 9 ,16, 25, 36])) - @test kalmanfilter6.P̂ ≈ Hermitian(diagm(Float64[1, 4, 9 ,16, 25, 36])) - @test kalmanfilter6.P̂_0 !== kalmanfilter6.P̂ + @test kalmanfilter6.cov.P̂_0 ≈ Hermitian(diagm(Float64[1, 4, 9 ,16, 25, 36])) + @test kalmanfilter6.cov.P̂ ≈ Hermitian(diagm(Float64[1, 4, 9 ,16, 25, 36])) + @test kalmanfilter6.cov.P̂_0 !== kalmanfilter6.cov.P̂ kalmanfilter7 = KalmanFilter(linmodel, nint_u=[1,1]) @test kalmanfilter7.nxs == 2 @@ -184,9 +184,9 @@ end @test kalmanfilter7.nint_ym == [0, 0] kalmanfilter8 = KalmanFilter(linmodel, 1:2, 0, [1, 1], I(4), I(4), I(2)) - @test kalmanfilter8.P̂_0 ≈ I(4) - @test kalmanfilter8.Q̂ ≈ I(4) - @test kalmanfilter8.R̂ ≈ I(2) + @test kalmanfilter8.cov.P̂_0 ≈ I(4) + @test kalmanfilter8.cov.Q̂ ≈ I(4) + @test kalmanfilter8.cov.R̂ ≈ I(2) linmodel2 = LinModel{Float32}(0.5*ones(1,1), ones(1,1), ones(1,1), zeros(1,0), zeros(1,0), 1.0) kalmanfilter8 = KalmanFilter(linmodel2) @@ -214,7 +214,7 @@ end @test initstate!(kalmanfilter1, [10, 50], [50, 30+1]) ≈ [zeros(3); [1]] setstate!(kalmanfilter1, [1,2,3,4], diagm(.1:.1:.4)) @test kalmanfilter1.x̂0 ≈ [1,2,3,4] - @test kalmanfilter1.P̂ ≈ diagm(.1:.1:.4) + @test kalmanfilter1.cov.P̂ ≈ diagm(.1:.1:.4) for i in 1:40 preparestate!(kalmanfilter1, [50, 30]) updatestate!(kalmanfilter1, [11, 52], [50, 30]) @@ -271,8 +271,8 @@ end setmodel!(kalmanfilter, newlinmodel) @test kalmanfilter.x̂0 ≈ [3.0 - 8.0] setmodel!(kalmanfilter, Q̂=[1e-3], R̂=[1e-6]) - @test kalmanfilter.Q̂ ≈ [1e-3] - @test kalmanfilter.R̂ ≈ [1e-6] + @test kalmanfilter.cov.Q̂ ≈ [1e-3] + @test kalmanfilter.cov.R̂ ≈ [1e-6] end @testitem "Luenberger construction" setup=[SetupMPCtests] begin @@ -540,17 +540,17 @@ end @test ukf3.nx̂ == 5 ukf4 = UnscentedKalmanFilter(nonlinmodel, σQ=[1,2,3,4], σQint_ym=[5, 6], σR=[7, 8]) - @test ukf4.Q̂ ≈ Hermitian(diagm(Float64[1, 4, 9 ,16, 25, 36])) - @test ukf4.R̂ ≈ Hermitian(diagm(Float64[49, 64])) + @test ukf4.cov.Q̂ ≈ Hermitian(diagm(Float64[1, 4, 9 ,16, 25, 36])) + @test ukf4.cov.R̂ ≈ Hermitian(diagm(Float64[49, 64])) ukf5 = UnscentedKalmanFilter(nonlinmodel, nint_ym=[2,2]) @test ukf5.nxs == 4 @test ukf5.nx̂ == 8 ukf6 = UnscentedKalmanFilter(nonlinmodel, σP_0=[1,2,3,4], σPint_ym_0=[5,6]) - @test ukf6.P̂_0 ≈ Hermitian(diagm(Float64[1, 4, 9 ,16, 25, 36])) - @test ukf6.P̂ ≈ Hermitian(diagm(Float64[1, 4, 9 ,16, 25, 36])) - @test ukf6.P̂_0 !== ukf6.P̂ + @test ukf6.cov.P̂_0 ≈ Hermitian(diagm(Float64[1, 4, 9 ,16, 25, 36])) + @test ukf6.cov.P̂ ≈ Hermitian(diagm(Float64[1, 4, 9 ,16, 25, 36])) + @test ukf6.cov.P̂_0 !== ukf6.cov.P̂ ukf7 = UnscentedKalmanFilter(nonlinmodel, α=0.1, β=4, κ=0.2) @test ukf7.γ ≈ 0.1*√(ukf7.nx̂+0.2) @@ -563,9 +563,9 @@ end @test ukf8.nint_ym == [0, 0] ukf9 = UnscentedKalmanFilter(nonlinmodel, 1:2, 0, [1, 1], I(6), I(6), I(2), 0.1, 2, 0) - @test ukf9.P̂_0 ≈ I(6) - @test ukf9.Q̂ ≈ I(6) - @test ukf9.R̂ ≈ I(2) + @test ukf9.cov.P̂_0 ≈ I(6) + @test ukf9.cov.Q̂ ≈ I(6) + @test ukf9.cov.R̂ ≈ I(2) linmodel2 = LinModel{Float32}(0.5*ones(1,1), ones(1,1), ones(1,1), zeros(1,0), zeros(1,0), 1.0) ukf10 = UnscentedKalmanFilter(linmodel2) @@ -602,7 +602,7 @@ end @test initstate!(ukf1, [10, 50], [50, 30+1]) ≈ zeros(4) atol=1e-9 setstate!(ukf1, [1,2,3,4], diagm(.1:.1:.4)) @test ukf1.x̂0 ≈ [1,2,3,4] - @test ukf1.P̂ ≈ diagm(.1:.1:.4) + @test ukf1.cov.P̂ ≈ diagm(.1:.1:.4) for i in 1:40 preparestate!(ukf1, [50, 30]) updatestate!(ukf1, [11, 52], [50, 30]) @@ -658,15 +658,15 @@ end setmodel!(ukf1, newlinmodel) @test ukf1.x̂0 ≈ [3.0 - 8.0] setmodel!(ukf1, Q̂=[1e-3], R̂=[1e-6]) - @test ukf1.Q̂ ≈ [1e-3] - @test ukf1.R̂ ≈ [1e-6] + @test ukf1.cov.Q̂ ≈ [1e-3] + @test ukf1.cov.R̂ ≈ [1e-6] f(x,u,d,model) = model.A*x + model.Bu*u + model.Bd*d h(x,d,model) = model.C*x + model.Du*d nonlinmodel = NonLinModel(f, h, 10.0, 1, 1, 1, solver=nothing, p=linmodel) ukf2 = UnscentedKalmanFilter(nonlinmodel, nint_ym=0) setmodel!(ukf2, Q̂=[1e-3], R̂=[1e-6]) - @test ukf2.Q̂ ≈ [1e-3] - @test ukf2.R̂ ≈ [1e-6] + @test ukf2.cov.Q̂ ≈ [1e-3] + @test ukf2.cov.R̂ ≈ [1e-6] @test_throws ErrorException setmodel!(ukf2, deepcopy(nonlinmodel)) end @@ -698,17 +698,17 @@ end @test ekf3.nx̂ == 5 ekf4 = ExtendedKalmanFilter(nonlinmodel, σQ=[1,2,3,4], σQint_ym=[5, 6], σR=[7, 8]) - @test ekf4.Q̂ ≈ Hermitian(diagm(Float64[1, 4, 9 ,16, 25, 36])) - @test ekf4.R̂ ≈ Hermitian(diagm(Float64[49, 64])) + @test ekf4.cov.Q̂ ≈ Hermitian(diagm(Float64[1, 4, 9 ,16, 25, 36])) + @test ekf4.cov.R̂ ≈ Hermitian(diagm(Float64[49, 64])) ekf5 = ExtendedKalmanFilter(nonlinmodel, nint_ym=[2,2]) @test ekf5.nxs == 4 @test ekf5.nx̂ == 8 ekf6 = ExtendedKalmanFilter(nonlinmodel, σP_0=[1,2,3,4], σPint_ym_0=[5,6]) - @test ekf6.P̂_0 ≈ Hermitian(diagm(Float64[1, 4, 9 ,16, 25, 36])) - @test ekf6.P̂ ≈ Hermitian(diagm(Float64[1, 4, 9 ,16, 25, 36])) - @test ekf6.P̂_0 !== ekf6.P̂ + @test ekf6.cov.P̂_0 ≈ Hermitian(diagm(Float64[1, 4, 9 ,16, 25, 36])) + @test ekf6.cov.P̂ ≈ Hermitian(diagm(Float64[1, 4, 9 ,16, 25, 36])) + @test ekf6.cov.P̂_0 !== ekf6.cov.P̂ ekf7 = ExtendedKalmanFilter(nonlinmodel, nint_u=[1,1], nint_ym=[0,0]) @test ekf7.nxs == 2 @@ -717,9 +717,9 @@ end @test ekf7.nint_ym == [0, 0] ekf8 = ExtendedKalmanFilter(nonlinmodel, 1:2, 0, [1, 1], I(6), I(6), I(2)) - @test ekf8.P̂_0 ≈ I(6) - @test ekf8.Q̂ ≈ I(6) - @test ekf8.R̂ ≈ I(2) + @test ekf8.cov.P̂_0 ≈ I(6) + @test ekf8.cov.Q̂ ≈ I(6) + @test ekf8.cov.R̂ ≈ I(2) ekf9 = ExtendedKalmanFilter(nonlinmodel, jacobian=AutoFiniteDiff()) @test ekf9.jacobian === AutoFiniteDiff() @@ -761,7 +761,7 @@ end @test initstate!(ekf1, [10, 50], [50, 30+1]) ≈ zeros(4); setstate!(ekf1, [1,2,3,4], diagm(.1:.1:.4)) @test ekf1.x̂0 ≈ [1,2,3,4] - @test ekf1.P̂ ≈ diagm(.1:.1:.4) + @test ekf1.cov.P̂ ≈ diagm(.1:.1:.4) for i in 1:40 preparestate!(ekf1, [50, 30]) updatestate!(ekf1, [11, 52], [50, 30]) @@ -822,15 +822,15 @@ end setmodel!(ekf1, newlinmodel) @test ekf1.x̂0 ≈ [3.0 - 8.0] setmodel!(ekf1, Q̂=[1e-3], R̂=[1e-6]) - @test ekf1.Q̂ ≈ [1e-3] - @test ekf1.R̂ ≈ [1e-6] + @test ekf1.cov.Q̂ ≈ [1e-3] + @test ekf1.cov.R̂ ≈ [1e-6] f(x,u,d,_) = linmodel.A*x + linmodel.Bu*u + linmodel.Bd*d h(x,d,_) = linmodel.C*x + linmodel.Du*d nonlinmodel = NonLinModel(f, h, 10.0, 1, 1, 1) ekf2 = ExtendedKalmanFilter(nonlinmodel, nint_ym=0) setmodel!(ekf2, Q̂=[1e-3], R̂=[1e-6]) - @test ekf2.Q̂ ≈ [1e-3] - @test ekf2.R̂ ≈ [1e-6] + @test ekf2.cov.Q̂ ≈ [1e-3] + @test ekf2.cov.R̂ ≈ [1e-6] @test_throws ErrorException setmodel!(ekf2, deepcopy(nonlinmodel)) end @@ -1097,7 +1097,7 @@ end 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̄) + invP̄_copy = deepcopy(mhe.cov.invP̄) @test_logs( (:error, "Arrival covariance P̄ is not positive definite: keeping the old one"), preparestate!(mhe, [50, 30], [5]) @@ -1379,7 +1379,7 @@ end kf = KalmanFilter(linmodel, nint_ym=0, direct=true) # recuperate P̂(-1|-1) exact value using the Kalman filter: preparestate!(kf, [50, 30], [20]) - σP̂ = sqrt.(diag(kf.P̂)) + σP̂ = sqrt.(diag(kf.cov.P̂)) mhe = MovingHorizonEstimator(linmodel, He=3, nint_ym=0, direct=true, σP_0=σP̂) updatestate!(kf, [10, 50], [50, 30], [20]) X̂_mhe = zeros(4, 6) From d60715077f21417b075245306be25ae3ff0c8dfa Mon Sep 17 00:00:00 2001 From: franckgaga Date: Fri, 13 Jun 2025 11:43:59 -0400 Subject: [PATCH 06/10] =?UTF-8?q?debug:=20do=20not=20convert=20`P=CC=82=5F?= =?UTF-8?q?0`=20if=20`isnothing`?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- src/controller/construct.jl | 3 ++- src/estimator/construct.jl | 6 ++++-- test/3_test_predictive_control.jl | 1 - 3 files changed, 6 insertions(+), 4 deletions(-) diff --git a/src/controller/construct.jl b/src/controller/construct.jl index 49e181ae6..711cb3040 100644 --- a/src/controller/construct.jl +++ b/src/controller/construct.jl @@ -91,7 +91,8 @@ function ControllerWeights( model::SimModel{NT}, Hp, Hc, M_Hp, N_Hc, L_Hp, Cwt=Inf, Ewt=0 ) where {NT<:Real} validate_weights(model, Hp, Hc, M_Hp, N_Hc, L_Hp, Cwt, Ewt) - return ControllerWeights{NT}(NT.(M_Hp), NT.(N_Hc), NT.(L_Hp), Cwt, Ewt) + M_Hp, N_Hc, L_Hp = NT.(M_Hp), NT.(N_Hc), NT.(L_Hp) + return ControllerWeights{NT}(M_Hp, N_Hc, L_Hp, Cwt, Ewt) end "Validate predictive controller weight and horizon specified values." diff --git a/src/estimator/construct.jl b/src/estimator/construct.jl index e2eedf5a3..5f7642a22 100644 --- a/src/estimator/construct.jl +++ b/src/estimator/construct.jl @@ -53,7 +53,7 @@ struct KalmanCovariances{ invQ̂_He::Hermitian{NT, Q̂C} invR̂_He::Hermitian{NT, R̂C} function KalmanCovariances{NT}( - model, i_ym, nint_u, nint_ym, Q̂::Q̂C, R̂::R̂C, P̂_0=nothing, He=1 + 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) @@ -104,7 +104,9 @@ function KalmanCovariances( model::SimModel{NT}, i_ym, nint_u, nint_ym, Q̂, R̂, P̂_0=nothing, He=1 ) where {NT<:Real} validate_kfcov(model, i_ym, nint_u, nint_ym, Q̂, R̂, P̂_0) - return KalmanCovariances{NT}(model, i_ym, nint_u, nint_ym, NT.(Q̂), NT.(R̂), NT.(P̂_0), He) + Q̂, R̂ = NT.(Q̂), NT.(R̂) + !isnothing(P̂_0) && (P̂_0 .= NT.(P̂_0)) + return KalmanCovariances{NT}(Q̂, R̂, P̂_0, He) end """ diff --git a/test/3_test_predictive_control.jl b/test/3_test_predictive_control.jl index 5d6b89df6..bd6bb35d0 100644 --- a/test/3_test_predictive_control.jl +++ b/test/3_test_predictive_control.jl @@ -822,7 +822,6 @@ end preparestate!(nmpc11, y, [0]) moveinput!(nmpc11, [10], [0]) ΔU_diff = diff(getinfo(nmpc11)[:U]) - println(ΔU_diff) @test ΔU_diff[[2, 4, 5, 7, 8, 9]] ≈ zeros(6) atol=1e-9 @test_nowarn ModelPredictiveControl.info2debugstr(info) From 8f4522108a4cd9d6fa783f01517a70f403432a96 Mon Sep 17 00:00:00 2001 From: franckgaga Date: Fri, 13 Jun 2025 12:11:12 -0400 Subject: [PATCH 07/10] test: debug --- src/estimator/construct.jl | 7 ++++++- src/estimator/mhe/execute.jl | 4 ++-- test/2_test_state_estim.jl | 8 ++++---- 3 files changed, 12 insertions(+), 7 deletions(-) diff --git a/src/estimator/construct.jl b/src/estimator/construct.jl index 5f7642a22..b6a70b618 100644 --- a/src/estimator/construct.jl +++ b/src/estimator/construct.jl @@ -105,7 +105,12 @@ function KalmanCovariances( ) where {NT<:Real} validate_kfcov(model, i_ym, nint_u, nint_ym, Q̂, R̂, P̂_0) Q̂, R̂ = NT.(Q̂), NT.(R̂) - !isnothing(P̂_0) && (P̂_0 .= NT.(P̂_0)) + if !isnothing(P̂_0) + P̂_0 .= NT.(P̂_0) + end + println(Q̂) + println(R̂) + println(P̂_0) return KalmanCovariances{NT}(Q̂, R̂, P̂_0, He) end diff --git a/src/estimator/mhe/execute.jl b/src/estimator/mhe/execute.jl index 727f50f79..a9f1fd52e 100644 --- a/src/estimator/mhe/execute.jl +++ b/src/estimator/mhe/execute.jl @@ -797,14 +797,14 @@ function setmodel_estimator!( invQ̂ = Hermitian(estim.buffer.Q̂, :L) invQ̂ .= estim.cov.Q̂ inv!(invQ̂) - estim.invQ̂_He .= Hermitian(repeatdiag(invQ̂, He), :L) + estim.cov.invQ̂_He .= Hermitian(repeatdiag(invQ̂, He), :L) end if !isnothing(R̂) estim.cov.R̂ .= to_hermitian(R̂) invR̂ = Hermitian(estim.buffer.R̂, :L) invR̂ .= estim.cov.R̂ inv!(invR̂) - estim.invR̂_He .= Hermitian(repeatdiag(invR̂, He), :L) + estim.cov.invR̂_He .= Hermitian(repeatdiag(invR̂, He), :L) end return nothing end diff --git a/test/2_test_state_estim.jl b/test/2_test_state_estim.jl index 7f4751db2..359492a75 100644 --- a/test/2_test_state_estim.jl +++ b/test/2_test_state_estim.jl @@ -864,8 +864,8 @@ end @test mhe3.nx̂ == 5 mhe4 = MovingHorizonEstimator(nonlinmodel, He=5, σQ=[1,2,3,4], σQint_ym=[5, 6], σR=[7, 8]) - @test mhe4.Q̂ ≈ Hermitian(diagm(Float64[1, 4, 9 ,16, 25, 36])) - @test mhe4.R̂ ≈ Hermitian(diagm(Float64[49, 64])) + @test mhe4.cov.Q̂ ≈ Hermitian(diagm(Float64[1, 4, 9 ,16, 25, 36])) + @test mhe4.cov.R̂ ≈ Hermitian(diagm(Float64[49, 64])) mhe5 = MovingHorizonEstimator(nonlinmodel, He=5, nint_ym=[2,2]) @test mhe5.nxs == 4 @@ -1116,7 +1116,7 @@ end ) mhe.P̂arr_old[1, 1] = Inf # Inf to trigger fallback P̂arr_old_copy = deepcopy(mhe.P̂arr_old) - invP̄_copy = deepcopy(mhe.invP̄) + invP̄_copy = deepcopy(mhe.cov.invP̄) @test_logs( (:error, "Arrival covariance P̄ is not finite: keeping the old one"), preparestate!(mhe, [50, 30], [5]) @@ -1425,7 +1425,7 @@ end # recuperate P̂(-1|-1) exact value using the Unscented Kalman filter: preparestate!(ukf, [50, 30], [20]) preparestate!(ekf, [50, 30], [20]) - σP̂ = sqrt.(diag(ukf.P̂)) + σP̂ = sqrt.(diag(ukf.cov.P̂)) mhe = MovingHorizonEstimator(nonlinmodel, He=5, nint_ym=0, direct=true, σP_0=σP̂) updatestate!(ukf, [10, 50], [50, 30], [20]) updatestate!(ekf, [10, 50], [50, 30], [20]) From dea3c393d57d0bc3293aabd6d7e251cd952e5980 Mon Sep 17 00:00:00 2001 From: franckgaga Date: Fri, 13 Jun 2025 12:28:58 -0400 Subject: [PATCH 08/10] test: debug --- src/estimator/construct.jl | 7 +------ test/2_test_state_estim.jl | 6 +++--- 2 files changed, 4 insertions(+), 9 deletions(-) diff --git a/src/estimator/construct.jl b/src/estimator/construct.jl index b6a70b618..66e0847ce 100644 --- a/src/estimator/construct.jl +++ b/src/estimator/construct.jl @@ -105,12 +105,7 @@ function KalmanCovariances( ) where {NT<:Real} validate_kfcov(model, i_ym, nint_u, nint_ym, Q̂, R̂, P̂_0) Q̂, R̂ = NT.(Q̂), NT.(R̂) - if !isnothing(P̂_0) - P̂_0 .= NT.(P̂_0) - end - println(Q̂) - println(R̂) - println(P̂_0) + !isnothing(P̂_0) && (P̂_0 = NT.(P̂_0)) return KalmanCovariances{NT}(Q̂, R̂, P̂_0, He) end diff --git a/test/2_test_state_estim.jl b/test/2_test_state_estim.jl index 359492a75..2338513d9 100644 --- a/test/2_test_state_estim.jl +++ b/test/2_test_state_estim.jl @@ -1128,7 +1128,7 @@ end updatestate!(mhe, [10, 50], [50, 30], [5]) ) @test mhe.P̂arr_old ≈ P̂arr_old_copy - @test mhe.invP̄ ≈ invP̄_copy + @test mhe.cov.invP̄ ≈ invP̄_copy end @testitem "MovingHorizonEstimator set constraints" setup=[SetupMPCtests] begin @@ -1354,8 +1354,8 @@ end nonlinmodel = NonLinModel(f, h, 10.0, 1, 1, 1, p=linmodel, solver=nothing) mhe2 = MovingHorizonEstimator(nonlinmodel; He, nint_ym=0) setmodel!(mhe2, Q̂=[1e-3], R̂=[1e-6]) - @test mhe2.Q̂ ≈ [1e-3] - @test mhe2.R̂ ≈ [1e-6] + @test mhe2.cov.Q̂ ≈ [1e-3] + @test mhe2.cov.R̂ ≈ [1e-6] @test_throws ErrorException setmodel!(mhe2, deepcopy(nonlinmodel)) end From 3de4919201a752f4009968064aca5e6fc477c6d6 Mon Sep 17 00:00:00 2001 From: franckgaga Date: Fri, 13 Jun 2025 15:32:19 -0400 Subject: [PATCH 09/10] test: debug --- test/2_test_state_estim.jl | 20 ++++++++++---------- 1 file changed, 10 insertions(+), 10 deletions(-) diff --git a/test/2_test_state_estim.jl b/test/2_test_state_estim.jl index 2338513d9..24134f355 100644 --- a/test/2_test_state_estim.jl +++ b/test/2_test_state_estim.jl @@ -872,9 +872,9 @@ end @test mhe5.nx̂ == 8 mhe6 = MovingHorizonEstimator(nonlinmodel, He=5, σP_0=[1,2,3,4], σPint_ym_0=[5,6]) - @test mhe6.P̂_0 ≈ Hermitian(diagm(Float64[1, 4, 9 ,16, 25, 36])) + @test mhe6.cov.P̂_0 ≈ Hermitian(diagm(Float64[1, 4, 9 ,16, 25, 36])) @test mhe6.P̂arr_old ≈ Hermitian(diagm(Float64[1, 4, 9 ,16, 25, 36])) - @test mhe6.P̂_0 !== mhe6.P̂arr_old + @test mhe6.cov.P̂_0 !== mhe6.P̂arr_old mhe7 = MovingHorizonEstimator(nonlinmodel, He=10) @test mhe7.He == 10 @@ -894,9 +894,9 @@ end I_2 = Matrix{Float64}(I, 2, 2) optim = Model(Ipopt.Optimizer) mhe9 = MovingHorizonEstimator(nonlinmodel, 5, 1:2, 0, [1, 1], I_6, I_6, I_2, 1e5; optim) - @test mhe9.P̂_0 ≈ I(6) - @test mhe9.Q̂ ≈ I(6) - @test mhe9.R̂ ≈ I(2) + @test mhe9.cov.P̂_0 ≈ I(6) + @test mhe9.cov.Q̂ ≈ I(6) + @test mhe9.cov.R̂ ≈ I(2) optim = JuMP.Model(optimizer_with_attributes(Ipopt.Optimizer, "nlp_scaling_max_gradient"=>1.0)) covestim = ExtendedKalmanFilter(nonlinmodel, 1:2, 0, [1, 1], I_6, I_6, I_2) @@ -1103,13 +1103,13 @@ end preparestate!(mhe, [50, 30], [5]) ) @test mhe.P̂arr_old ≈ P̂arr_old_copy - @test mhe.invP̄ ≈ invP̄_copy + @test mhe.cov.invP̄ ≈ invP̄_copy @test_logs( (:error, "Arrival covariance P̄ 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 mhe.cov.invP̄ ≈ invP̄_copy @test_logs( (:error, "Arrival covariance P̄ is not invertible: keeping the old one"), ModelPredictiveControl.invert_cov!(mhe, Hermitian(zeros(mhe.nx̂, mhe.nx̂),:L)) @@ -1122,7 +1122,7 @@ end preparestate!(mhe, [50, 30], [5]) ) @test mhe.P̂arr_old ≈ P̂arr_old_copy - @test mhe.invP̄ ≈ invP̄_copy + @test mhe.cov.invP̄ ≈ invP̄_copy @test_logs( (:error, "Arrival covariance P̄ is not finite: keeping the old one"), updatestate!(mhe, [10, 50], [50, 30], [5]) @@ -1347,8 +1347,8 @@ end @test mhe.con.x̃0min ≈ [-1000 - 8.0] @test mhe.con.x̃0max ≈ [+1000 - 8.0] setmodel!(mhe, Q̂=[1e-3], R̂=[1e-6]) - @test mhe.Q̂ ≈ [1e-3] - @test mhe.R̂ ≈ [1e-6] + @test mhe.cov.Q̂ ≈ [1e-3] + @test mhe.cov.R̂ ≈ [1e-6] f(x,u,d,model) = model.A*x + model.Bu*u + model.Bd*d h(x,d,model) = model.C*x + model.Du*d nonlinmodel = NonLinModel(f, h, 10.0, 1, 1, 1, p=linmodel, solver=nothing) From 73e36ce8dc4cf5badc00e88ed212dab706e521e6 Mon Sep 17 00:00:00 2001 From: franckgaga Date: Fri, 13 Jun 2025 17:05:18 -0400 Subject: [PATCH 10/10] doc: debug jldoctest --- src/estimator/execute.jl | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/estimator/execute.jl b/src/estimator/execute.jl index 8de7c4336..03f99b3d4 100644 --- a/src/estimator/execute.jl +++ b/src/estimator/execute.jl @@ -375,12 +375,12 @@ augmented model is not verified (see Extended Help for more info). ```jldoctest julia> kf = KalmanFilter(LinModel(ss(0.1, 0.5, 1, 0, 4.0)), σQ=[√4.0], σQint_ym=[√0.25]); -julia> kf.model.A[], kf.Q̂[1, 1], kf.Q̂[2, 2] +julia> kf.model.A[], kf.cov.Q̂[1, 1], kf.cov.Q̂[2, 2] (0.1, 4.0, 0.25) julia> setmodel!(kf, LinModel(ss(0.42, 0.5, 1, 0, 4.0)), Q̂=[1 0;0 0.5]); -julia> kf.model.A[], kf.Q̂[1, 1], kf.Q̂[2, 2] +julia> kf.model.A[], kf.cov.Q̂[1, 1], kf.cov.Q̂[2, 2] (0.42, 1.0, 0.5) ```