From 00d34c5052a16015ffcb6ce75c76a3cdfa890d4c Mon Sep 17 00:00:00 2001 From: franckgaga Date: Thu, 20 Mar 2025 08:32:56 -0400 Subject: [PATCH 1/7] wip: EKF transition to DI.jl --- src/estimator/execute.jl | 45 +++++++++++++++++------- src/estimator/kalman.jl | 76 +++++++++++++++++++++++++++++++++++----- 2 files changed, 100 insertions(+), 21 deletions(-) diff --git a/src/estimator/execute.jl b/src/estimator/execute.jl index e451f520d..28ee1c1e5 100644 --- a/src/estimator/execute.jl +++ b/src/estimator/execute.jl @@ -36,14 +36,7 @@ where ``\mathbf{x̂_0}(k+1)`` is stored in `x̂next0` argument. The method mutat function signature for conciseness. """ function f̂!(x̂next0, û0, estim::StateEstimator, model::SimModel, x̂0, u0, d0) - # `@views` macro avoid copies with matrix slice operator e.g. [a:b] - @views x̂d, x̂s = x̂0[1:model.nx], x̂0[model.nx+1:end] - @views x̂d_next, x̂s_next = x̂next0[1:model.nx], x̂next0[model.nx+1:end] - mul!(û0, estim.Cs_u, x̂s) - û0 .+= u0 - f!(x̂d_next, model, x̂d, û0, d0, model.p) - mul!(x̂s_next, estim.As, x̂s) - return nothing + return f!(x̂next0, û0, model, estim.As, estim.Cs_u, x̂0, u0, d0) end """ @@ -58,18 +51,31 @@ function f̂!(x̂next0, _ , estim::StateEstimator, ::LinModel, x̂0, u0, d0) return nothing end +""" + f̂!(x̂next0, û0, model::SimModel, As, Cs_u, x̂0, u0, d0) + +Same than [`f̂!`](@ref) for [`SimModel`](@ref) but without the `estim` argument. +""" +function f̂!(x̂next0, û0, model::SimModel, As, Cs_u, x̂0, u0, d0) + # `@views` macro avoid copies with matrix slice operator e.g. [a:b] + @views x̂d, x̂s = x̂0[1:model.nx], x̂0[model.nx+1:end] + @views x̂d_next, x̂s_next = x̂next0[1:model.nx], x̂next0[model.nx+1:end] + mul!(û0, Cs_u, x̂s) + û0 .+= u0 + f!(x̂d_next, model, x̂d, û0, d0, model.p) + mul!(x̂s_next, As, x̂s) + return nothing +end + @doc raw""" ĥ!(ŷ0, estim::StateEstimator, model::SimModel, x̂0, d0) -> nothing Mutating output function ``\mathbf{ĥ}`` of the augmented model, see [`f̂!`](@ref). """ function ĥ!(ŷ0, estim::StateEstimator, model::SimModel, x̂0, d0) - # `@views` macro avoid copies with matrix slice operator e.g. [a:b] - @views x̂d, x̂s = x̂0[1:model.nx], x̂0[model.nx+1:end] - h!(ŷ0, model, x̂d, d0, model.p) - mul!(ŷ0, estim.Cs_y, x̂s, 1, 1) - return nothing + return ĥ!(ŷ0, model, estim.Cs_y, x̂0, d0) end + """ ĥ!(ŷ0, estim::StateEstimator, model::LinModel, x̂0, d0) -> nothing @@ -81,6 +87,19 @@ function ĥ!(ŷ0, estim::StateEstimator, ::LinModel, x̂0, d0) return nothing end +""" + ĥ!(ŷ0, model::SimModel, Cs_y, x̂0, d0) + +Same than [`ĥ!`](@ref) for [`SimModel`](@ref) but without the `estim` argument. +""" +function ĥ!(ŷ0, model::SimModel, Cs_y, x̂0, d0) + # `@views` macro avoid copies with matrix slice operator e.g. [a:b] + @views x̂d, x̂s = x̂0[1:model.nx], x̂0[model.nx+1:end] + h!(ŷ0, model, x̂d, d0, model.p) + mul!(ŷ0, Cs_y, x̂s, 1, 1) + return nothing +end + @doc raw""" initstate!(estim::StateEstimator, u, ym, d=[]) -> x̂ diff --git a/src/estimator/kalman.jl b/src/estimator/kalman.jl index 43e69e5f3..7766c275d 100644 --- a/src/estimator/kalman.jl +++ b/src/estimator/kalman.jl @@ -872,7 +872,12 @@ function update_estimate!(estim::UnscentedKalmanFilter, y0m, d0, u0) return nothing end -struct ExtendedKalmanFilter{NT<:Real, SM<:SimModel} <: StateEstimator{NT} +struct ExtendedKalmanFilter{ + NT<:Real, + SM<:SimModel, + JB<:AbstractADType, + LF<:Function +} <: StateEstimator{NT} model::SM lastu0::Vector{NT} x̂op ::Vector{NT} @@ -906,10 +911,12 @@ struct ExtendedKalmanFilter{NT<:Real, SM<:SimModel} <: StateEstimator{NT} Ĥm ::Matrix{NT} direct::Bool corrected::Vector{Bool} + jacobian::JB + linfunc!::LF buffer::StateEstimatorBuffer{NT} function ExtendedKalmanFilter{NT}( - model::SM, i_ym, nint_u, nint_ym, P̂_0, Q̂, R̂; direct=true - ) where {NT<:Real, SM<:SimModel} + 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} nu, ny, nd = model.nu, model.ny, model.nd 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) @@ -937,6 +944,7 @@ struct ExtendedKalmanFilter{NT<:Real, SM<:SimModel} <: StateEstimator{NT} P̂_0, Q̂, R̂, K̂, F̂_û, F̂, Ĥ, Ĥm, + jacobian, linfunc!, direct, corrected, buffer ) @@ -983,6 +991,7 @@ function ExtendedKalmanFilter( sigmaQint_u = fill(1, max(sum(nint_u), 0)), sigmaPint_ym_0 = fill(1, max(sum(nint_ym), 0)), sigmaQint_ym = fill(1, max(sum(nint_ym), 0)), + jacobian = AutoForwardDiff(), direct = true, σP_0 = sigmaP_0, σQ = sigmaQ, @@ -996,21 +1005,67 @@ function ExtendedKalmanFilter( 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 ExtendedKalmanFilter{NT}(model, i_ym, nint_u, nint_ym, P̂_0, Q̂, R̂; direct) + linfunc! = get_ekf_linfunc(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 + ) end @doc raw""" - ExtendedKalmanFilter(model, i_ym, nint_u, nint_ym, P̂_0, Q̂, R̂; direct=true) + ExtendedKalmanFilter( + model, i_ym, nint_u, nint_ym, P̂_0, Q̂, R̂; jacobian=AutoForwardDiff(), direct=true + ) Construct the estimator from the augmented covariance matrices `P̂_0`, `Q̂` and `R̂`. This syntax allows nonzero off-diagonal elements in ``\mathbf{P̂}_{-1}(0), \mathbf{Q̂, R̂}``. """ function ExtendedKalmanFilter( - model::SM, i_ym, nint_u, nint_ym,P̂_0, Q̂, R̂; direct=true + 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̂) - return ExtendedKalmanFilter{NT}(model, i_ym, nint_u, nint_ym, P̂_0, Q̂, R̂; direct) + P̂_0, Q̂, R̂ = to_mat(P̂_0), to_mat(Q̂), to_mat(R̂) + linfunc! = get_ekf_linfunc(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! + ) +end + +function get_ekf_linfunc(model, i_ym, nint_u, nint_ym, jacobian) + As, Cs_u, Cs_y = init_estimstoch(model, i_ym, nint_u, nint_ym) + function f̂_ekf!(x̂0next, x̂0, model, As, Cs_u, u0, d0, û0) + return f̂!(x̂next0, û0, model, As, Cs_u, x̂0, u0, d0) + end + function ĥ_ekf!(ŷ0, x̂0, model, Cs_y, d0) + return ĥ!(ŷ0, model, Cs_y, x̂0, d0) + end + strict = Val(true) + #TODO: continue here: + xnext = zeros(NT, nx) + y = zeros(NT, ny) + x = zeros(NT, nx) + u = zeros(NT, nu) + d = zeros(NT, nd) + cst_x = Constant(x) + cst_u = Constant(u) + cst_d = Constant(d) + A_prep = prepare_jacobian(f_x!, xnext, backend, x, cst_u, cst_d; strict) + Bu_prep = prepare_jacobian(f_u!, xnext, backend, u, cst_x, cst_d; strict) + Bd_prep = prepare_jacobian(f_d!, xnext, backend, d, cst_x, cst_u; strict) + C_prep = prepare_jacobian(h_x!, y, backend, x, cst_d ; strict) + Dd_prep = prepare_jacobian(h_d!, y, backend, d, cst_x ; strict) + function linfunc!(xnext, y, A, Bu, C, Bd, Dd, backend, x, u, d, cst_x, cst_u, cst_d) + # all the arguments before `x` are mutated in this function + jacobian!(f_x!, xnext, A, A_prep, backend, x, cst_u, cst_d) + jacobian!(f_u!, xnext, Bu, Bu_prep, backend, u, cst_x, cst_d) + jacobian!(f_d!, xnext, Bd, Bd_prep, backend, d, cst_x, cst_u) + jacobian!(h_x!, y, C, C_prep, backend, x, cst_d) + jacobian!(h_d!, y, Dd, Dd_prep, backend, d, cst_x) + return nothing + end + return linfunc! + + + end """ @@ -1021,9 +1076,14 @@ Do the same but for the [`ExtendedKalmanFilter`](@ref). function correct_estimate!(estim::ExtendedKalmanFilter, y0m, d0) model, x̂0 = estim.model, estim.x̂0 ŷ0 = estim.buffer.ŷ + + ĥAD! = (ŷ0, x̂0) -> ĥ!(ŷ0, estim, model, x̂0, d0) ForwardDiff.jacobian!(estim.Ĥ, ĥAD!, ŷ0, x̂0) + + estim.Ĥm .= @views estim.Ĥ[estim.i_ym, :] + return correct_estimate_kf!(estim, y0m, d0, estim.Ĥm) end From f1ff9fe79513a2828c5bb04505da6d0505f8e068 Mon Sep 17 00:00:00 2001 From: franckgaga Date: Thu, 20 Mar 2025 13:30:17 -0400 Subject: [PATCH 2/7] added: `ExtendedKalmanFilter` now uses `DI.jl` Bonus : it is now allocation-free ! --- src/estimator/execute.jl | 2 +- src/estimator/kalman.jl | 103 +++++++++++++++++-------------------- src/model/linearization.jl | 2 +- 3 files changed, 50 insertions(+), 57 deletions(-) diff --git a/src/estimator/execute.jl b/src/estimator/execute.jl index 28ee1c1e5..29b968806 100644 --- a/src/estimator/execute.jl +++ b/src/estimator/execute.jl @@ -36,7 +36,7 @@ where ``\mathbf{x̂_0}(k+1)`` is stored in `x̂next0` argument. The method mutat function signature for conciseness. """ function f̂!(x̂next0, û0, estim::StateEstimator, model::SimModel, x̂0, u0, d0) - return f!(x̂next0, û0, model, estim.As, estim.Cs_u, x̂0, u0, d0) + return f̂!(x̂next0, û0, model, estim.As, estim.Cs_u, x̂0, u0, d0) end """ diff --git a/src/estimator/kalman.jl b/src/estimator/kalman.jl index 7766c275d..1a5f2e96d 100644 --- a/src/estimator/kalman.jl +++ b/src/estimator/kalman.jl @@ -909,10 +909,10 @@ struct ExtendedKalmanFilter{ F̂ ::Matrix{NT} Ĥ ::Matrix{NT} Ĥm ::Matrix{NT} - direct::Bool - corrected::Vector{Bool} jacobian::JB linfunc!::LF + direct::Bool + 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 @@ -935,7 +935,7 @@ struct ExtendedKalmanFilter{ Ĥ, Ĥm = zeros(NT, ny, nx̂), zeros(NT, nym, nx̂) corrected = [false] buffer = StateEstimatorBuffer{NT}(nu, nx̂, nym, ny, nd) - return new{NT, SM}( + return new{NT, SM, JB, LF}( model, lastu0, x̂op, f̂op, x̂0, P̂, i_ym, nx̂, nym, nyu, nxs, @@ -1005,7 +1005,7 @@ function ExtendedKalmanFilter( 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(model, i_ym, nint_u, nint_ym, jacobian) + 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 ) @@ -1024,48 +1024,52 @@ 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̂) - linfunc! = get_ekf_linfunc(model, i_ym, nint_u, nint_ym, jacobian) + 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! ) end -function get_ekf_linfunc(model, i_ym, nint_u, nint_ym, jacobian) +""" + get_ekf_linfunc(NT, model, i_ym, nint_u, nint_ym, jacobian) -> linfunc! + +Return the `linfunc!` function that computes the Jacobians of the augmented model. + +The function has the following signature: +``` + linfunc!(x̂0next, ŷ0, F̂, Ĥ, backend, x̂0, cst_u0, cst_d0) -> nothing +``` +#TODO: continue here +""" +function get_ekf_linfunc(NT, model, i_ym, nint_u, nint_ym, jacobian) As, Cs_u, Cs_y = init_estimstoch(model, i_ym, nint_u, nint_ym) - function f̂_ekf!(x̂0next, x̂0, model, As, Cs_u, u0, d0, û0) - return f̂!(x̂next0, û0, model, As, Cs_u, x̂0, u0, d0) - end - function ĥ_ekf!(ŷ0, x̂0, model, Cs_y, d0) - return ĥ!(ŷ0, model, Cs_y, x̂0, d0) - end + f̂_ekf!(x̂0next, x̂0, û0, u0, d0) = f̂!(x̂0next, û0, model, As, Cs_u, x̂0, u0, d0) + ĥ_ekf!(ŷ0, x̂0, d0) = ĥ!(ŷ0, model, Cs_y, x̂0, d0) strict = Val(true) - #TODO: continue here: - xnext = zeros(NT, nx) - y = zeros(NT, ny) - x = zeros(NT, nx) - u = zeros(NT, nu) - d = zeros(NT, nd) - cst_x = Constant(x) - cst_u = Constant(u) - cst_d = Constant(d) - A_prep = prepare_jacobian(f_x!, xnext, backend, x, cst_u, cst_d; strict) - Bu_prep = prepare_jacobian(f_u!, xnext, backend, u, cst_x, cst_d; strict) - Bd_prep = prepare_jacobian(f_d!, xnext, backend, d, cst_x, cst_u; strict) - C_prep = prepare_jacobian(h_x!, y, backend, x, cst_d ; strict) - Dd_prep = prepare_jacobian(h_d!, y, backend, d, cst_x ; strict) - function linfunc!(xnext, y, A, Bu, C, Bd, Dd, backend, x, u, d, cst_x, cst_u, cst_d) - # all the arguments before `x` are mutated in this function - jacobian!(f_x!, xnext, A, A_prep, backend, x, cst_u, cst_d) - jacobian!(f_u!, xnext, Bu, Bu_prep, backend, u, cst_x, cst_d) - jacobian!(f_d!, xnext, Bd, Bd_prep, backend, d, cst_x, cst_u) - jacobian!(h_x!, y, C, C_prep, backend, x, cst_d) - jacobian!(h_d!, y, Dd, Dd_prep, backend, d, cst_x) + nu, ny, nd = model.nu, model.ny, model.nd + nx̂ = model.nx + size(As, 1) + x̂0next = zeros(NT, nx̂) + ŷ0 = zeros(NT, ny) + x̂0 = zeros(NT, nx̂) + tmp_û0 = Cache(zeros(NT, nu)) + cst_u0 = Constant(zeros(NT, nu)) + cst_d0 = Constant(zeros(NT, nd)) + F̂_prep = prepare_jacobian(f̂_ekf!, x̂0next, jacobian, x̂0, tmp_û0, cst_u0, cst_d0; strict) + Ĥ_prep = prepare_jacobian(ĥ_ekf!, ŷ0, jacobian, x̂0, cst_d0; strict) + # main method to compute both Jacobians, it mutates all args before `backend`: + function linfunc!(x̂0next, ŷ0, F̂, Ĥ, backend, x̂0, cst_u0, cst_d0) + jacobian!(f̂_ekf!, x̂0next, F̂, F̂_prep, backend, x̂0, tmp_û0, cst_u0, cst_d0) + jacobian!(ĥ_ekf!, ŷ0, Ĥ, Ĥ_prep, backend, x̂0, cst_d0) return nothing end + # two additional methods to only compute one of the two Jacobians at a time: + function linfunc!(x̂0next, ŷ0::Nothing, F̂, Ĥ::Nothing, backend, x̂0, cst_u0, cst_d0) + return jacobian!(f̂_ekf!, x̂0next, F̂, F̂_prep, backend, x̂0, tmp_û0, cst_u0, cst_d0) + end + function linfunc!(x̂0next::Nothing, ŷ0, F̂::Nothing, Ĥ, backend, x̂0, _ , cst_d0) + return jacobian!(ĥ_ekf!, ŷ0, Ĥ, Ĥ_prep, backend, x̂0, cst_d0) + end return linfunc! - - - end """ @@ -1075,15 +1079,10 @@ Do the same but for the [`ExtendedKalmanFilter`](@ref). """ function correct_estimate!(estim::ExtendedKalmanFilter, y0m, d0) model, x̂0 = estim.model, estim.x̂0 - ŷ0 = estim.buffer.ŷ - - - ĥAD! = (ŷ0, x̂0) -> ĥ!(ŷ0, estim, model, x̂0, d0) - ForwardDiff.jacobian!(estim.Ĥ, ĥAD!, ŷ0, x̂0) - - + cst_d0 = Constant(d0) + ŷ0, Ĥ = estim.buffer.ŷ, estim.Ĥ + estim.linfunc!(nothing, ŷ0, nothing, Ĥ, estim.jacobian, x̂0, nothing, cst_d0) estim.Ĥm .= @views estim.Ĥ[estim.i_ym, :] - return correct_estimate_kf!(estim, y0m, d0, estim.Ĥm) end @@ -1129,22 +1128,16 @@ prediction step equations are provided below. The correction step is skipped if function update_estimate!(estim::ExtendedKalmanFilter{NT}, y0m, d0, u0) where NT<:Real model, x̂0 = estim.model, estim.x̂0 nx̂, nu = estim.nx̂, model.nu + cst_u0, cst_d0 = Constant(u0), Constant(d0) if !estim.direct - ŷ0 = estim.buffer.ŷ - ĥAD! = (ŷ0, x̂0) -> ĥ!(ŷ0, estim, model, x̂0, d0) - ForwardDiff.jacobian!(estim.Ĥ, ĥAD!, ŷ0, x̂0) + ŷ0, Ĥ = estim.buffer.ŷ, estim.Ĥ + estim.linfunc!(nothing, ŷ0, nothing, Ĥ, estim.jacobian, x̂0, nothing, cst_d0) estim.Ĥm .= @views estim.Ĥ[estim.i_ym, :] correct_estimate_kf!(estim, y0m, d0, estim.Ĥm) end x̂0corr = estim.x̂0 - # concatenate x̂0next and û0 vectors to allows û0 vector with dual numbers for AD: - # TODO: remove this allocation using estim.buffer - x̂0nextû = Vector{NT}(undef, nx̂ + nu) - f̂AD! = (x̂0nextû, x̂0corr) -> @views f̂!( - x̂0nextû[1:nx̂], x̂0nextû[nx̂+1:end], estim, model, x̂0corr, u0, d0 - ) - ForwardDiff.jacobian!(estim.F̂_û, f̂AD!, x̂0nextû, x̂0corr) - estim.F̂ .= @views estim.F̂_û[1:estim.nx̂, :] + x̂0next, F̂ = estim.buffer.x̂, estim.F̂ + estim.linfunc!(x̂0next, nothing, F̂, nothing, estim.jacobian, x̂0corr, cst_u0, cst_d0) return predict_estimate_kf!(estim, u0, d0, estim.F̂) end diff --git a/src/model/linearization.jl b/src/model/linearization.jl index c25156c61..985e5e915 100644 --- a/src/model/linearization.jl +++ b/src/model/linearization.jl @@ -32,7 +32,7 @@ function get_linearization_func(NT, f!, h!, nu, nx, ny, nd, p, backend) C_prep = prepare_jacobian(h_x!, y, backend, x, cst_d ; strict) Dd_prep = prepare_jacobian(h_d!, y, backend, d, cst_x ; strict) function linfunc!(xnext, y, A, Bu, C, Bd, Dd, backend, x, u, d, cst_x, cst_u, cst_d) - # all the arguments before `x` are mutated in this function + # all the arguments before `backend` are mutated in this function jacobian!(f_x!, xnext, A, A_prep, backend, x, cst_u, cst_d) jacobian!(f_u!, xnext, Bu, Bu_prep, backend, u, cst_x, cst_d) jacobian!(f_d!, xnext, Bd, Bd_prep, backend, d, cst_x, cst_u) From 740258f2e60cba672e019a908acfe0262aa51c6a Mon Sep 17 00:00:00 2001 From: franckgaga Date: Thu, 20 Mar 2025 13:51:33 -0400 Subject: [PATCH 3/7] doc: added `jacobian` kwarg to `ExtendedKalmanFilter` --- src/controller/transcription.jl | 9 ++--- src/estimator/kalman.jl | 58 ++++++++++++++++++++++++--------- src/model/linearization.jl | 2 +- 3 files changed, 48 insertions(+), 21 deletions(-) diff --git a/src/controller/transcription.jl b/src/controller/transcription.jl index 9e3f5c0e7..1f5b45473 100644 --- a/src/controller/transcription.jl +++ b/src/controller/transcription.jl @@ -43,11 +43,12 @@ operating point ``\mathbf{x̂_{op}}`` (see [`augment_model`](@ref)): ``` where ``\mathbf{x̂}_i(k+j)`` is the state prediction for time ``k+j``, estimated by the observer at time ``i=k`` or ``i=k-1`` depending on its `direct` flag. Note that -``\mathbf{X̂_0 = X̂}`` if the operating points is zero, which is typically the case in -practice for [`NonLinModel`](@ref). This transcription method is generally more efficient -for large control horizon ``H_c``, unstable or highly nonlinear plant models/constraints. +``\mathbf{X̂_0 = X̂}`` if the operating point is zero, which is typically the case in practice +for [`NonLinModel`](@ref). This transcription method is generally more efficient for large +control horizon ``H_c``, unstable or highly nonlinear plant models/constraints. -Sparse optimizers like `OSQP` or `Ipopt` are recommended for this method. +Sparse optimizers like `OSQP` or `Ipopt` and sparse Jacobian computations are recommended +for this transcription method. """ struct MultipleShooting <: TranscriptionMethod end diff --git a/src/estimator/kalman.jl b/src/estimator/kalman.jl index 1a5f2e96d..759583c84 100644 --- a/src/estimator/kalman.jl +++ b/src/estimator/kalman.jl @@ -624,11 +624,11 @@ This estimator is allocation-free if `model` simulations do not allocate. disturbances at measured outputs ``\mathbf{Q_{int_{ym}}}`` (composed of integrators). - `σPint_ym_0=fill(1,sum(nint_ym))` or *`sigmaPint_ym_0`* : same than `σP_0` but for the unmeasured disturbances at measured outputs ``\mathbf{P_{int_{ym}}}(0)`` (composed of integrators). -- `direct=true`: construct with a direct transmission from ``\mathbf{y^m}`` (a.k.a. current - estimator, in opposition to the delayed/predictor form). - `α=1e-3` or *`alpha`* : alpha parameter, spread of the state distribution ``(0 < α ≤ 1)``. - `β=2` or *`beta`* : beta parameter, skewness and kurtosis of the states distribution ``(β ≥ 0)``. - `κ=0` or *`kappa`* : kappa parameter, another spread parameter ``(0 ≤ κ ≤ 3)``. +- `direct=true`: construct with a direct transmission from ``\mathbf{y^m}`` (a.k.a. current + estimator, in opposition to the delayed/predictor form). # Examples ```jldoctest @@ -960,12 +960,41 @@ Both [`LinModel`](@ref) and [`NonLinModel`](@ref) are supported. The process mod keyword arguments are identical to [`UnscentedKalmanFilter`](@ref), except for `α`, `β` and `κ` which do not apply to the extended Kalman Filter. The Jacobians of the augmented model ``\mathbf{f̂, ĥ}`` are computed with [`ForwardDiff`](@extref ForwardDiff) automatic -differentiation. This estimator allocates memory for the Jacobians. - +differentiation. This estimator is allocation-free if `model` simulations do not allocate. !!! warning See the Extended Help of [`linearize`](@ref) function if you get an error like: `MethodError: no method matching (::var"##")(::Vector{ForwardDiff.Dual})`. +# Arguments +!!! info + Keyword arguments with *`emphasis`* are non-Unicode alternatives. + +- `model::SimModel` : (deterministic) model for the estimations. +- `i_ym=1:model.ny` : `model` output indices that are measured ``\mathbf{y^m}``, the rest + are unmeasured ``\mathbf{y^u}``. +- `σP_0=fill(1/model.nx,model.nx)` or *`sigmaP_0`* : main diagonal of the initial estimate + covariance ``\mathbf{P}(0)``, specified as a standard deviation vector. +- `σQ=fill(1/model.nx,model.nx)` or *`sigmaQ`* : main diagonal of the process noise + covariance ``\mathbf{Q}`` of `model`, specified as a standard deviation vector. +- `σR=fill(1,length(i_ym))` or *`sigmaR`* : main diagonal of the sensor noise covariance + ``\mathbf{R}`` of `model` measured outputs, specified as a standard deviation vector. +- `nint_u=0`: integrator quantity for the stochastic model of the unmeasured disturbances at + the manipulated inputs (vector), use `nint_u=0` for no integrator (see Extended Help). +- `nint_ym=default_nint(model,i_ym,nint_u)` : same than `nint_u` but for the unmeasured + disturbances at the measured outputs, use `nint_ym=0` for no integrator (see Extended Help). +- `σQint_u=fill(1,sum(nint_u))` or *`sigmaQint_u`* : same than `σQ` but for the unmeasured + disturbances at manipulated inputs ``\mathbf{Q_{int_u}}`` (composed of integrators). +- `σPint_u_0=fill(1,sum(nint_u))` or *`sigmaPint_u_0`* : same than `σP_0` but for the unmeasured + disturbances at manipulated inputs ``\mathbf{P_{int_u}}(0)`` (composed of integrators). +- `σQint_ym=fill(1,sum(nint_ym))` or *`sigmaQint_u`* : same than `σQ` for the unmeasured + disturbances at measured outputs ``\mathbf{Q_{int_{ym}}}`` (composed of integrators). +- `σPint_ym_0=fill(1,sum(nint_ym))` or *`sigmaPint_ym_0`* : same than `σP_0` but for the unmeasured + disturbances at measured outputs ``\mathbf{P_{int_{ym}}}(0)`` (composed of integrators). +- `jacobian=AutoForwardDiff()`: an `AbstractADType` backend for the Jacobians of the augmented + model, see [`DifferentiationInterface` doc](@extref DifferentiationInterface List). +- `direct=true`: construct with a direct transmission from ``\mathbf{y^m}`` (a.k.a. current + estimator, in opposition to the delayed/predictor form). + # Examples ```jldoctest julia> model = NonLinModel((x,u,_,_)->0.2x+u, (x,_,_)->-3x, 5.0, 1, 1, 1, solver=nothing); @@ -1035,11 +1064,15 @@ end Return the `linfunc!` function that computes the Jacobians of the augmented model. -The function has the following signature: +The function has the two following methods: ``` - linfunc!(x̂0next, ŷ0, F̂, Ĥ, backend, x̂0, cst_u0, cst_d0) -> nothing +linfunc!(x̂0next , ::Nothing, F̂ , ::Nothing, backend, x̂0, cst_u0, cst_d0) -> nothing +linfunc!(::Nothing, ŷ0 , ::Nothing, Ĥ , backend, x̂0, _ , cst_d0) -> nothing ``` -#TODO: continue here +To respectively compute only `F̂` or `Ĥ` Jacobian. The methods mutate all the arguments +before `backend` argument. The `backend` argument is an `AbstractADType` object from +`DifferentiationInterface`. The `cst_u0` and `cst_d0` are `DifferentiationInterface.Constant` +objects with the linearization points. """ function get_ekf_linfunc(NT, model, i_ym, nint_u, nint_ym, jacobian) As, Cs_u, Cs_y = init_estimstoch(model, i_ym, nint_u, nint_ym) @@ -1056,13 +1089,6 @@ function get_ekf_linfunc(NT, model, i_ym, nint_u, nint_ym, jacobian) cst_d0 = Constant(zeros(NT, nd)) F̂_prep = prepare_jacobian(f̂_ekf!, x̂0next, jacobian, x̂0, tmp_û0, cst_u0, cst_d0; strict) Ĥ_prep = prepare_jacobian(ĥ_ekf!, ŷ0, jacobian, x̂0, cst_d0; strict) - # main method to compute both Jacobians, it mutates all args before `backend`: - function linfunc!(x̂0next, ŷ0, F̂, Ĥ, backend, x̂0, cst_u0, cst_d0) - jacobian!(f̂_ekf!, x̂0next, F̂, F̂_prep, backend, x̂0, tmp_û0, cst_u0, cst_d0) - jacobian!(ĥ_ekf!, ŷ0, Ĥ, Ĥ_prep, backend, x̂0, cst_d0) - return nothing - end - # two additional methods to only compute one of the two Jacobians at a time: function linfunc!(x̂0next, ŷ0::Nothing, F̂, Ĥ::Nothing, backend, x̂0, cst_u0, cst_d0) return jacobian!(f̂_ekf!, x̂0next, F̂, F̂_prep, backend, x̂0, tmp_û0, cst_u0, cst_d0) end @@ -1102,8 +1128,8 @@ augmented process model: \end{aligned} ``` The matrix ``\mathbf{Ĥ^m}`` is the rows of ``\mathbf{Ĥ}`` that are measured outputs. The -Jacobians are computed with [`ForwardDiff`](@extref ForwardDiff). The correction and -prediction step equations are provided below. The correction step is skipped if +Jacobians are computed with [`ForwardDiff`](@extref ForwardDiff) bu default. The correction +and prediction step equations are provided below. The correction step is skipped if `estim.direct == true` since it's already done by the user. # Correction Step diff --git a/src/model/linearization.jl b/src/model/linearization.jl index 985e5e915..a18d58888 100644 --- a/src/model/linearization.jl +++ b/src/model/linearization.jl @@ -8,7 +8,7 @@ The function has the following signature: linfunc!(xnext, y, A, Bu, C, Bd, Dd, backend, x, u, d, cst_x, cst_u, cst_d) -> nothing ``` and it should modifies in-place all the arguments before `backend`. The `backend` argument -is an `AbstractADType` backend from `DifferentiationInterface`. The `cst_x`, `cst_u` and +is an `AbstractADType` object from `DifferentiationInterface`. The `cst_x`, `cst_u` and `cst_d` are `DifferentiationInterface.Constant` objects with the linearization points. """ function get_linearization_func(NT, f!, h!, nu, nx, ny, nd, p, backend) From 4c53e18ce5df4e2ede0114d4b94b7d1ea07172e1 Mon Sep 17 00:00:00 2001 From: franckgaga Date: Thu, 20 Mar 2025 14:29:36 -0400 Subject: [PATCH 4/7] test: new `ExtendedKalmanFilter` tests with `AutoFiniteDiff` --- src/estimator/kalman.jl | 7 ++- test/2_test_state_estim.jl | 89 +++++++++++++++++++++++--------------- 2 files changed, 56 insertions(+), 40 deletions(-) diff --git a/src/estimator/kalman.jl b/src/estimator/kalman.jl index 759583c84..a41235f01 100644 --- a/src/estimator/kalman.jl +++ b/src/estimator/kalman.jl @@ -956,10 +956,9 @@ end Construct an extended Kalman Filter with the [`SimModel`](@ref) `model`. -Both [`LinModel`](@ref) and [`NonLinModel`](@ref) are supported. The process model and the -keyword arguments are identical to [`UnscentedKalmanFilter`](@ref), except for `α`, `β` and -`κ` which do not apply to the extended Kalman Filter. The Jacobians of the augmented model -``\mathbf{f̂, ĥ}`` are computed with [`ForwardDiff`](@extref ForwardDiff) automatic +Both [`LinModel`](@ref) and [`NonLinModel`](@ref) are supported. The process model is +identical to [`UnscentedKalmanFilter`](@ref). By default, the Jacobians of the augmented +model ``\mathbf{f̂, ĥ}`` are computed with [`ForwardDiff`](@extref ForwardDiff) automatic differentiation. This estimator is allocation-free if `model` simulations do not allocate. !!! warning See the Extended Help of [`linearize`](@ref) function if you get an error like: diff --git a/test/2_test_state_estim.jl b/test/2_test_state_estim.jl index 5a807ad81..eef093e91 100644 --- a/test/2_test_state_estim.jl +++ b/test/2_test_state_estim.jl @@ -378,9 +378,9 @@ end @test internalmodel2.nxs == 1 @test internalmodel2.nx̂ == 4 - f(x,u,d,_) = linmodel2.A*x + linmodel2.Bu*u + linmodel2.Bd*d - h(x,d,_) = linmodel2.C*x + linmodel2.Dd*d - nonlinmodel = NonLinModel(f, h, Ts, 2, 4, 2, 2, solver=nothing) + f(x,u,d,model) = model.A*x + model.Bu*u + model.Bd*d + h(x,d,model) = model.C*x + model.Dd*d + nonlinmodel = NonLinModel(f, h, Ts, 2, 4, 2, 2, solver=nothing, p=linmodel2) internalmodel3 = InternalModel(nonlinmodel) @test internalmodel3.nym == 2 @test internalmodel3.nyu == 0 @@ -499,9 +499,9 @@ end @testitem "UnscentedKalmanFilter construction" setup=[SetupMPCtests] begin using .SetupMPCtests, ControlSystemsBase, LinearAlgebra linmodel1 = LinModel(sys,Ts,i_d=[3]) - f(x,u,d,_) = linmodel1.A*x + linmodel1.Bu*u + linmodel1.Bd*d - h(x,d,_) = linmodel1.C*x + linmodel1.Du*d - nonlinmodel = NonLinModel(f, h, Ts, 2, 4, 2, 1, solver=nothing) + 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, Ts, 2, 4, 2, 1, solver=nothing, p=linmodel1) ukf1 = UnscentedKalmanFilter(linmodel1) @test ukf1.nym == 2 @@ -557,9 +557,10 @@ end @testitem "UnscentedKalmanFilter estimator methods" setup=[SetupMPCtests] begin using .SetupMPCtests, ControlSystemsBase, LinearAlgebra linmodel1 = LinModel(sys,Ts,i_u=[1,2]) - f(x,u,_,_) = linmodel1.A*x + linmodel1.Bu*u - h(x,_,_) = linmodel1.C*x - nonlinmodel = setop!(NonLinModel(f, h, Ts, 2, 2, 2, solver=nothing), uop=[10,50], yop=[50,30]) + f(x,u,_,model) = model.A*x + model.Bu*u + h(x,_,model) = model.C*x + nonlinmodel = NonLinModel(f, h, Ts, 2, 2, 2, solver=nothing, p=linmodel1) + nonlinmodel = setop!(nonlinmodel, uop=[10,50], yop=[50,30]) ukf1 = UnscentedKalmanFilter(nonlinmodel) preparestate!(ukf1, [50, 30]) @test updatestate!(ukf1, [10, 50], [50, 30]) ≈ zeros(4) atol=1e-9 @@ -630,9 +631,9 @@ end setmodel!(ukf1, Q̂=[1e-3], R̂=[1e-6]) @test ukf1.Q̂ ≈ [1e-3] @test ukf1.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) + 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] @@ -642,10 +643,12 @@ end @testitem "ExtendedKalmanFilter construction" setup=[SetupMPCtests] begin using .SetupMPCtests, ControlSystemsBase, LinearAlgebra + using DifferentiationInterface + import FiniteDiff linmodel1 = LinModel(sys,Ts,i_d=[3]) - f(x,u,d,_) = linmodel1.A*x + linmodel1.Bu*u + linmodel1.Bd*d - h(x,d,_) = linmodel1.C*x + linmodel1.Du*d - nonlinmodel = NonLinModel(f, h, Ts, 2, 4, 2, 1, solver=nothing) + 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, Ts, 2, 4, 2, 1, solver=nothing, p=linmodel1) ekf1 = ExtendedKalmanFilter(linmodel1) @test ekf1.nym == 2 @@ -689,6 +692,9 @@ end @test ekf8.Q̂ ≈ I(6) @test ekf8.R̂ ≈ I(2) + ekf9 = ExtendedKalmanFilter(nonlinmodel, jacobian=AutoFiniteDiff()) + @test ekf9.jacobian === AutoFiniteDiff() + linmodel2 = LinModel{Float32}(0.5*ones(1,1), ones(1,1), ones(1,1), zeros(1,0), zeros(1,0), 1.0) ekf8 = ExtendedKalmanFilter(linmodel2) @test isa(ekf8, ExtendedKalmanFilter{Float32}) @@ -696,10 +702,13 @@ end @testitem "ExtendedKalmanFilter estimator methods" setup=[SetupMPCtests] begin using .SetupMPCtests, ControlSystemsBase, LinearAlgebra + using DifferentiationInterface + import FiniteDiff linmodel1 = LinModel(sys,Ts,i_u=[1,2]) - f(x,u,_,_) = linmodel1.A*x + linmodel1.Bu*u - h(x,_,_) = linmodel1.C*x - nonlinmodel = setop!(NonLinModel(f, h, Ts, 2, 2, 2, solver=nothing), uop=[10,50], yop=[50,30]) + f(x,u,_,model) = model.A*x + model.Bu*u + h(x,_,model) = model.C*x + nonlinmodel = NonLinModel(f, h, Ts, 2, 2, 2, solver=nothing, p=linmodel1) + nonlinmodel = setop!(nonlinmodel, uop=[10,50], yop=[50,30]) ekf1 = ExtendedKalmanFilter(nonlinmodel) preparestate!(ekf1, [50, 30]) @test updatestate!(ekf1, [10, 50], [50, 30]) ≈ zeros(4) atol=1e-9 @@ -741,6 +750,11 @@ end x̂ = updatestate!(ekf3, [0], [0]) @test x̂ ≈ [0, 0] @test isa(x̂, Vector{Float32}) + ekf4 = ExtendedKalmanFilter(nonlinmodel, jacobian=AutoFiniteDiff()) + preparestate!(ekf4, [50, 30]) + @test updatestate!(ekf4, [10, 50], [50, 30]) ≈ zeros(4) atol=1e-9 + preparestate!(ekf4, [50, 30]) + @test evaloutput(ekf4) ≈ ekf4() ≈ [50, 30] end @testitem "ExtendedKalmanFilter set model" setup=[SetupMPCtests] begin @@ -1008,9 +1022,10 @@ end @testitem "MovingHorizonEstimator fallbacks for arrival covariance estimation" setup=[SetupMPCtests] begin using .SetupMPCtests, ControlSystemsBase, LinearAlgebra linmodel = setop!(LinModel(sys,Ts,i_u=[1,2], i_d=[3]), uop=[10,50], yop=[50,30], dop=[5]) - f(x,u,d,_) = linmodel.A*x + linmodel.Bu*u + linmodel.Bd*d - h(x,d,_) = linmodel.C*x + linmodel.Dd*d - nonlinmodel = setop!(NonLinModel(f, h, Ts, 2, 4, 2, 1, solver=nothing), uop=[10,50], yop=[50,30], dop=[5]) + f(x,u,d,model) = model.A*x + model.Bu*u + model.Bd*d + h(x,d,model) = model.C*x + model.Dd*d + nonlinmodel = NonLinModel(f, h, Ts, 2, 4, 2, 1, p=linmodel, solver=nothing) + nonlinmodel = setop!(nonlinmodel, uop=[10,50], yop=[50,30], dop=[5]) mhe = MovingHorizonEstimator(nonlinmodel, nint_ym=0, He=1) preparestate!(mhe, [50, 30], [5]) updatestate!(mhe, [10, 50], [50, 30], [5]) @@ -1085,9 +1100,10 @@ end setconstraint!(mhe2, C_v̂min=0.05(31:38), C_v̂max=0.06(31:38)) @test all((-mhe2.con.A_V̂min[:, end], -mhe2.con.A_V̂max[:, end]) .≈ (0.05(31:38), 0.06(31:38))) - f(x,u,d,_) = linmodel1.A*x + linmodel1.Bu*u - h(x,d,_) = linmodel1.C*x - nonlinmodel = setop!(NonLinModel(f, h, Ts, 2, 2, 2, solver=nothing), uop=[10,50], yop=[50,30]) + f(x,u,d,model) = model.A*x + model.Bu*u + h(x,d,model) = model.C*x + nonlinmodel = NonLinModel(f, h, Ts, 2, 2, 2, p=linmodel1, solver=nothing) + nonlinmodel = setop!(nonlinmodel, uop=[10,50], yop=[50,30]) mhe3 = MovingHorizonEstimator(nonlinmodel, He=4, nint_ym=0, Cwt=1e3) setconstraint!(mhe3, C_x̂min=0.01(1:10), C_x̂max=0.02(1:10)) @@ -1181,9 +1197,10 @@ end info = getinfo(mhe) @test info[:V̂] ≈ [-1,-1] atol=5e-2 - f(x,u,_,_) = linmodel1.A*x + linmodel1.Bu*u - h(x,_,_) = linmodel1.C*x - nonlinmodel = setop!(NonLinModel(f, h, Ts, 2, 2, 2, solver=nothing), uop=[10,50], yop=[50,30]) + f(x,u,_,model) = model.A*x + model.Bu*u + h(x,_,model) = model.C*x + nonlinmodel = NonLinModel(f, h, Ts, 2, 2, 2, p=linmodel1, solver=nothing) + nonlinmodel = setop!(nonlinmodel, uop=[10,50], yop=[50,30]) mhe2 = MovingHorizonEstimator(nonlinmodel, He=1, nint_ym=0) setconstraint!(mhe2, x̂min=[-100,-100], x̂max=[100,100]) @@ -1266,9 +1283,9 @@ end setmodel!(mhe, Q̂=[1e-3], R̂=[1e-6]) @test mhe.Q̂ ≈ [1e-3] @test mhe.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) + 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) mhe2 = MovingHorizonEstimator(nonlinmodel, He=1, nint_ym=0) setmodel!(mhe2, Q̂=[1e-3], R̂=[1e-6]) @test mhe2.Q̂ ≈ [1e-3] @@ -1307,9 +1324,9 @@ end updatestate!(kf, [11, 50], y, [25]) end @test X̂_mhe ≈ X̂_kf atol=1e-3 rtol=1e-3 - f = (x,u,d,_) -> linmodel1.A*x + linmodel1.Bu*u + linmodel1.Bd*d - h = (x,d,_) -> linmodel1.C*x + linmodel1.Dd*d - nonlinmodel = NonLinModel(f, h, Ts, 2, 4, 2, 1, solver=nothing) + f = (x,u,d,model) -> model.A*x + model.Bu*u + model.Bd*d + h = (x,d,model) -> model.C*x + model.Dd*d + nonlinmodel = NonLinModel(f, h, Ts, 2, 4, 2, 1, p=linmodel1, solver=nothing) nonlinmodel = setop!(nonlinmodel, uop=[10,50], yop=[50,30], dop=[20]) mhe = MovingHorizonEstimator(nonlinmodel, He=5, nint_ym=0, direct=false) ukf = UnscentedKalmanFilter(nonlinmodel, nint_ym=0, direct=false) @@ -1356,9 +1373,9 @@ end @testitem "MovingHorizonEstimator LinModel v.s. NonLinModel" setup=[SetupMPCtests] begin using .SetupMPCtests, ControlSystemsBase, LinearAlgebra, JuMP, Ipopt linmodel = setop!(LinModel(sys,Ts,i_d=[3]), uop=[10,50], yop=[50,30], dop=[20]) - f = (x,u,d,_) -> linmodel.A*x + linmodel.Bu*u + linmodel.Bd*d - h = (x,d,_) -> linmodel.C*x + linmodel.Dd*d - nonlinmodel = NonLinModel(f, h, Ts, 2, 4, 2, 1, solver=nothing) + f = (x,u,d,model) -> model.A*x + model.Bu*u + model.Bd*d + h = (x,d,model) -> model.C*x + model.Dd*d + nonlinmodel = NonLinModel(f, h, Ts, 2, 4, 2, 1, p=linmodel, solver=nothing) nonlinmodel = setop!(nonlinmodel, uop=[10,50], yop=[50,30], dop=[20]) optim = JuMP.Model(optimizer_with_attributes(Ipopt.Optimizer, "sb" => "yes")) mhe_lin = MovingHorizonEstimator(linmodel, He=5, nint_ym=0, direct=true, optim=optim) From e536503d61414e81b20a741c30636f949729a0a0 Mon Sep 17 00:00:00 2001 From: franckgaga Date: Thu, 20 Mar 2025 14:36:58 -0400 Subject: [PATCH 5/7] doc: avoid word repetition --- README.md | 2 +- docs/src/index.md | 2 +- 2 files changed, 2 insertions(+), 2 deletions(-) diff --git a/README.md b/README.md index 63bb8bfc0..45d7cfd46 100644 --- a/README.md +++ b/README.md @@ -11,7 +11,7 @@ package for Julia. The package depends on [`ControlSystemsBase.jl`](https://github.com/JuliaControl/ControlSystems.jl) for the linear systems, [`JuMP.jl`](https://github.com/jump-dev/JuMP.jl) for the optimization and [`DifferentiationInterface.jl`](https://github.com/JuliaDiff/DifferentiationInterface.jl) -for the differentiation. +for the derivatives. ## Installation diff --git a/docs/src/index.md b/docs/src/index.md index 4be5bb160..3ded65373 100644 --- a/docs/src/index.md +++ b/docs/src/index.md @@ -6,7 +6,7 @@ package for Julia. The package depends on [`ControlSystemsBase.jl`](https://github.com/JuliaControl/ControlSystems.jl) for the linear systems, [`JuMP.jl`](https://github.com/jump-dev/JuMP.jl) for the optimization and [`DifferentiationInterface.jl`](https://github.com/JuliaDiff/DifferentiationInterface.jl) -for the differentiation. +for the derivatives. The objective is to provide a simple, clear and modular framework to quickly design model predictive controllers (MPCs) in Julia, while preserving the flexibility for advanced From 5a2b3e0f20504ac2ad7db3dc912edd04f98a5f9b Mon Sep 17 00:00:00 2001 From: franckgaga Date: Thu, 20 Mar 2025 16:04:44 -0400 Subject: [PATCH 6/7] debug: `MovingHorizonEstimator` now uses `covestim` buffer Ensure that the type used in the covariance estimator are not views (does not work with the EKF with `strict=Val(true)` option of DI.jl) --- src/estimator/mhe/execute.jl | 11 +++++++++-- 1 file changed, 9 insertions(+), 2 deletions(-) diff --git a/src/estimator/mhe/execute.jl b/src/estimator/mhe/execute.jl index de73b9411..d26db959b 100644 --- a/src/estimator/mhe/execute.jl +++ b/src/estimator/mhe/execute.jl @@ -445,7 +445,10 @@ end "Correct the covariance estimate at arrival using `covestim` [`StateEstimator`](@ref)." function correct_cov!(estim::MovingHorizonEstimator) nym, nd = estim.nym, estim.model.nd - y0marr, d0arr = @views estim.Y0m[1:nym], estim.D0[1:nd] + buffer = estim.covestim.buffer + 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 try @@ -468,7 +471,11 @@ end "Update the covariance estimate at arrival using `covestim` [`StateEstimator`](@ref)." function update_cov!(estim::MovingHorizonEstimator) nu, nd, nym = estim.model.nu, estim.model.nd, estim.nym - u0arr, y0marr, d0arr = @views estim.U0[1:nu], estim.Y0m[1:nym], estim.D0[1:nd] + buffer = estim.covestim.buffer + u0arr, y0marr, d0arr = buffer.u, buffer.ym, buffer.d + 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 try From 519d4eda03cff09336f3b3c98e4a287f1d97d656 Mon Sep 17 00:00:00 2001 From: franckgaga Date: Thu, 20 Mar 2025 16:45:53 -0400 Subject: [PATCH 7/7] test: remove useless print --- test/3_test_predictive_control.jl | 1 - 1 file changed, 1 deletion(-) diff --git a/test/3_test_predictive_control.jl b/test/3_test_predictive_control.jl index 5dc8e7458..e18dcc3d8 100644 --- a/test/3_test_predictive_control.jl +++ b/test/3_test_predictive_control.jl @@ -698,7 +698,6 @@ end nmpc5 = setconstraint!(nmpc5, ymin=[1]) # execute update_predictions! branch in `gfunc_i` for coverage: g_Y0min_end = nmpc5.optim[:g_Y0min_1].func - println(nmpc5.Z̃) @test_nowarn g_Y0min_end(10.0, 9.0, 8.0, 7.0) # execute update_predictions! branch in `geqfunc_i` for coverage: geq_end = nmpc5.optim[:geq_2].func