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 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/execute.jl b/src/estimator/execute.jl index e451f520d..29b968806 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..a41235f01 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 @@ -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} @@ -904,12 +909,14 @@ struct ExtendedKalmanFilter{NT<:Real, SM<:SimModel} <: StateEstimator{NT} F̂ ::Matrix{NT} Ĥ ::Matrix{NT} Ĥm ::Matrix{NT} + 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̂; 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) @@ -928,7 +935,7 @@ struct ExtendedKalmanFilter{NT<:Real, SM<:SimModel} <: StateEstimator{NT} Ĥ, Ĥ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, @@ -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 ) @@ -948,16 +956,44 @@ 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 -differentiation. This estimator allocates memory for the Jacobians. - +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: `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); @@ -983,6 +1019,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 +1033,68 @@ 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(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 + ) 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(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 + +""" + 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 two following methods: +``` +linfunc!(x̂0next , ::Nothing, F̂ , ::Nothing, backend, x̂0, cst_u0, cst_d0) -> nothing +linfunc!(::Nothing, ŷ0 , ::Nothing, Ĥ , backend, x̂0, _ , cst_d0) -> nothing +``` +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) + 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) + 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) + 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 """ @@ -1020,9 +1104,9 @@ 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 @@ -1043,8 +1127,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 @@ -1069,22 +1153,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/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 diff --git a/src/model/linearization.jl b/src/model/linearization.jl index c25156c61..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) @@ -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) 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) 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