Skip to content
Merged
Show file tree
Hide file tree
Changes from all commits
Commits
File filter

Filter by extension

Filter by extension

Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
4 changes: 2 additions & 2 deletions src/controller/explicitmpc.jl
Original file line number Diff line number Diff line change
Expand Up @@ -181,9 +181,9 @@ function Base.show(io::IO, mpc::ExplicitMPC)
nu, nd = mpc.estim.model.nu, mpc.estim.model.nd
nx̂, nym, nyu = mpc.estim.nx̂, mpc.estim.nym, mpc.estim.nyu
n = maximum(ndigits.((Hp, Hc, nu, nx̂, nym, nyu, nd))) + 1
println(io, "$(typeof(mpc).name.name) controller with a sample time Ts = "*
println(io, "$(nameof(typeof(mpc))) controller with a sample time Ts = "*
"$(mpc.estim.model.Ts) s, "*
"$(typeof(mpc.estim).name.name) estimator and:")
"$(nameof(typeof(mpc.estim))) estimator and:")
println(io, "$(lpad(Hp, n)) prediction steps Hp")
println(io, "$(lpad(Hc, n)) control steps Hc")
print_estim_dim(io, mpc.estim, n)
Expand Down
22 changes: 11 additions & 11 deletions src/controller/transcription.jl
Original file line number Diff line number Diff line change
Expand Up @@ -1194,9 +1194,9 @@ Compute vectors if `model` is a [`NonLinModel`](@ref) and other [`TranscriptionM
The method mutates `Ŷ0` and `x̂0end` arguments. The augmented output function [`ĥ!`](@ref)
is called multiple times in a `for` loop:
```math
\mathbf{ŷ_0}(k) = \mathbf{ĥ}\Big(\mathbf{x̂_0^†}(k+j), \mathbf{d_0}(k) \Big)
\mathbf{ŷ_0}(k) = \mathbf{ĥ}\Big(\mathbf{x̂_0}(k), \mathbf{d_0}(k) \Big)
```
in which ``\mathbf{x̂_0^†}`` is the augmented state extracted from the decision variable `Z̃`.
in which ``\mathbf{x̂_0}`` is the augmented state extracted from the decision variable `Z̃`.
"""
function predict!(
Ŷ0, x̂0end, _, _, _,
Expand Down Expand Up @@ -1316,10 +1316,10 @@ The method mutates the `geq`, `X̂0`, `Û0` and `K0` vectors in argument. The n
equality constraints `geq` only includes the augmented state defects, computed with:
```math
\mathbf{ŝ}(k+1) = \mathbf{f̂}\Big(\mathbf{x̂_0}(k), \mathbf{u_0}(k), \mathbf{d_0}(k)\Big)
- \mathbf{x̂_0^†}(k+1)
- \mathbf{x̂_0}(k+1)
```
in which ``\mathbf{x̂_0^†}`` is the augmented state extracted from the decision variables
`Z̃`, and ``\mathbf{f̂}``, the augmented state function defined in [`f̂!`](@ref).
in which the augmented state ``\mathbf{x̂_0}`` are extracted from the decision variables
`Z̃`, and ``\mathbf{f̂}`` is the augmented state function defined in [`f̂!`](@ref).
"""
function con_nonlinprogeq!(
geq, X̂0, Û0, K0,
Expand Down Expand Up @@ -1366,18 +1366,18 @@ time state-space models, and the stochastic model of the unmeasured disturbances
is discrete-time. The deterministic and stochastic defects are respectively computed with:
```math
\begin{aligned}
\mathbf{s_d}(k+1) &= \mathbf{x_0}(k) - \mathbf{x_0^†}(k+1)
\mathbf{s_d}(k+1) &= \mathbf{x_0}(k) - \mathbf{x_0}(k+1)
+ 0.5 T_s (\mathbf{k}_1 + \mathbf{k}_2) \\
\mathbf{s_s}(k+1) &= \mathbf{A_s x_s}(k) - \mathbf{x_s^†}(k+1)
\mathbf{s_s}(k+1) &= \mathbf{A_s x_s}(k) - \mathbf{x_s}(k+1)
\end{aligned}
```
in which ``\mathbf{x_0^†}`` and ``\mathbf{x_s^†}`` are the deterministic and stochastic
states extracted from the decision variables `Z̃`. The ``\mathbf{k}`` coefficients are
in which ``\mathbf{x_0}`` and ``\mathbf{x_s}`` are the deterministic and stochastic states
extracted from the decision variables `Z̃`. The ``\mathbf{k}`` coefficients are
evaluated from the continuous-time function `model.f!` and:
```math
\begin{aligned}
\mathbf{k}_1 &= \mathbf{f}\Big(\mathbf{x_0}(k), \mathbf{û_0}(k), \mathbf{d_0}(k) \Big) \\
\mathbf{k}_2 &= \mathbf{f}\Big(\mathbf{x_0^†}(k+1), \mathbf{û_0}(k), \mathbf{d_0}(k+1)\Big)
\mathbf{k}_1 &= \mathbf{f}\Big(\mathbf{x_0}(k), \mathbf{û_0}(k), \mathbf{d_0}(k) \Big) \\
\mathbf{k}_2 &= \mathbf{f}\Big(\mathbf{x_0}(k+1), \mathbf{û_0}(k), \mathbf{d_0}(k+1)\Big)
\end{aligned}
```
and the input of the augmented model is:
Expand Down
104 changes: 52 additions & 52 deletions src/estimator/kalman.jl
Original file line number Diff line number Diff line change
Expand Up @@ -877,7 +877,8 @@ struct ExtendedKalmanFilter{
SM<:SimModel,
KC<:KalmanCovariances,
JB<:AbstractADType,
LF<:Function
FF<:Function,
HF<:Function
} <: StateEstimator{NT}
model::SM
cov ::KC
Expand Down Expand Up @@ -907,13 +908,23 @@ struct ExtendedKalmanFilter{
Ĥ ::Matrix{NT}
Ĥm ::Matrix{NT}
jacobian::JB
linfunc!::LF
linfuncF̂!::FF
linfuncĤ!::HF
direct::Bool
corrected::Vector{Bool}
buffer::StateEstimatorBuffer{NT}
function ExtendedKalmanFilter{NT}(
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}
model::SM,
i_ym, nint_u, nint_ym, cov::KC;
jacobian::JB, linfuncF̂!::FF, linfuncĤ!::HF, direct=true
) where {
NT<:Real,
SM<:SimModel,
KC<:KalmanCovariances,
JB<:AbstractADType,
FF<:Function,
HF<: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)
Expand All @@ -927,7 +938,7 @@ struct ExtendedKalmanFilter{
Ĥ, Ĥm = zeros(NT, ny, nx̂), zeros(NT, nym, nx̂)
corrected = [false]
buffer = StateEstimatorBuffer{NT}(nu, nx̂, nym, ny, nd, nk)
return new{NT, SM, KC, JB, LF}(
return new{NT, SM, KC, JB, FF, HF}(
model,
cov,
x̂op, f̂op, x̂0,
Expand All @@ -936,7 +947,7 @@ struct ExtendedKalmanFilter{
Â, B̂u, Ĉ, B̂d, D̂d, Ĉm, D̂dm,
K̂,
F̂_û, F̂, Ĥ, Ĥm,
jacobian, linfunc!,
jacobian, linfuncF̂!, linfuncĤ!,
direct, corrected,
buffer
)
Expand Down Expand Up @@ -1044,60 +1055,54 @@ function ExtendedKalmanFilter(
) 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)
linfuncF̂!, linfuncĤ! = get_ekf_linfuncs(NT, model, i_ym, nint_u, nint_ym, jacobian)
return ExtendedKalmanFilter{NT}(
model, i_ym, nint_u, nint_ym, cov; jacobian, direct, linfunc!
model, i_ym, nint_u, nint_ym, cov; jacobian, linfuncF̂!, linfuncĤ!, direct
)
end

"""
get_ekf_linfunc(NT, model, i_ym, nint_u, nint_ym, jacobian) -> linfunc!
get_ekf_linfuncs(NT, model, i_ym, nint_u, nint_ym, jacobian) -> linfuncF̂!, linfuncĤ!

Return the `linfunc!` function that computes the Jacobians of the augmented model.
Return the functions that computes the `F̂` and `Ĥ` Jacobians of the augmented model.

The function has the two following methods:
The functions has the following signatures:
```
linfunc!(x̂0next , ::Nothing, F̂ , ::Nothing, backend, x̂0, cst_u0, cst_d0) -> nothing
linfunc!(::Nothing, ŷ0 , ::Nothing, Ĥ , backend, x̂0, _ , cst_d0) -> nothing
linfuncF̂!(F̂, x̂0next , backend, x̂0, cst_u0, cst_d0) -> nothing
linfuncĤ!(Ĥ, ŷ0 , 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
They mutates all the arguments before `backend`, which 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)
function get_ekf_linfuncs(NT, model, i_ym, nint_u, nint_ym, jacobian)
As, Cs_u, Cs_y = init_estimstoch(model, i_ym, nint_u, nint_ym)
nxs = size(As, 1)
x̂op, f̂op = [model.xop; zeros(nxs)], [model.fop; zeros(nxs)]
f̂_ekf!(x̂0next, x̂0, û0, k0, u0, d0) = f̂!(
x̂0next, û0, k0, model, As, Cs_u, f̂op, x̂op, x̂0, u0, d0
)
ĥ_ekf!(ŷ0, x̂0, d0) = ĥ!(
ŷ0, model, Cs_y, x̂0, d0
)
strict = Val(true)
nu, ny, nd, nk = model.nu, model.ny, model.nd, model.nk
nx̂ = model.nx + nxs
nx̂ = model.nx + size(As, 1)
x̂op = f̂op = zeros(nx̂) # not important for Jacobian computations
function f̂_ekf!(x̂0next, x̂0, û0, k0, u0, d0)
return f̂!(x̂0next, û0, k0, model, As, Cs_u, f̂op, x̂op, x̂0, u0, d0)
end
ĥ_ekf!(ŷ0, x̂0, d0) = ĥ!(ŷ0, model, Cs_y, x̂0, d0)
strict = Val(true)
x̂0next = zeros(NT, nx̂)
ŷ0 = zeros(NT, ny)
x̂0 = zeros(NT, nx̂)
tmp_û0 = Cache(zeros(NT, nu))
tmp_x0i = Cache(zeros(NT, nk))
û0 = Cache(zeros(NT, nu))
k0 = Cache(zeros(NT, nk))
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, tmp_x0i, cst_u0, cst_d0; strict
F̂prep = prepare_jacobian(
f̂_ekf!, x̂0next, jacobian, x̂0, û0, k0, 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, tmp_x0i, cst_u0, cst_d0
)
Ĥprep = prepare_jacobian(ĥ_ekf!, ŷ0, jacobian, x̂0, cst_d0; strict)
function linfuncF̂!(F̂, x̂0next, backend, x̂0, cst_u0, cst_d0)
return jacobian!(f̂_ekf!, x̂0next, F̂, F̂prep, backend, x̂0, û0, k0, 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)
function linfuncĤ!(Ĥ, ŷ0, backend, x̂0, cst_d0)
return jacobian!(ĥ_ekf!, ŷ0, Ĥ, Ĥprep, backend, x̂0, cst_d0)
end
return linfunc!
return linfuncF̂!, linfuncĤ!
end

"""
Expand All @@ -1106,12 +1111,12 @@ end
Do the same but for the [`ExtendedKalmanFilter`](@ref).
"""
function correct_estimate!(estim::ExtendedKalmanFilter, y0m, d0)
model, x̂0 = estim.model, estim.x̂0
x̂0 = estim.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)
ŷ0, Ĥ, Ĥm = estim.buffer.ŷ, estim.Ĥ, estim.Ĥm
estim.linfuncĤ!(Ĥ, ŷ0, estim.jacobian, x̂0, cst_d0)
Ĥm .= @views Ĥ[estim.i_ym, :]
return correct_estimate_kf!(estim, y0m, d0, Ĥm)
end


Expand Down Expand Up @@ -1154,19 +1159,14 @@ and prediction step equations are provided below. The correction step is skipped
```
"""
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.ŷ, 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)
correct_estimate!(estim, y0m, d0)
end
cst_u0, cst_d0 = Constant(u0), Constant(d0)
x̂0corr = estim.x̂0
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̂)
estim.linfuncF̂!(F̂, x̂0next, estim.jacobian, x̂0corr, cst_u0, cst_d0)
return predict_estimate_kf!(estim, u0, d0, F̂)
end

"Set `estim.cov.P̂` to `estim.cov.P̂_0` for the time-varying Kalman Filters."
Expand Down
4 changes: 2 additions & 2 deletions src/estimator/mhe.jl
Original file line number Diff line number Diff line change
Expand Up @@ -5,9 +5,9 @@ function Base.show(io::IO, estim::MovingHorizonEstimator)
nu, nd = estim.model.nu, estim.model.nd
nx̂, nym, nyu = estim.nx̂, estim.nym, estim.nyu
n = maximum(ndigits.((nu, nx̂, nym, nyu, nd))) + 1
println(io, "$(typeof(estim).name.name) estimator with a sample time "*
println(io, "$(nameof(typeof(estim))) estimator with a sample time "*
"Ts = $(estim.model.Ts) s, $(JuMP.solver_name(estim.optim)) optimizer, "*
"$(typeof(estim.model).name.name) and:")
"$(nameof(typeof(estim.model))) and:")
print_estim_dim(io, estim, n)
end

Expand Down
6 changes: 3 additions & 3 deletions src/plot_sim.jl
Original file line number Diff line number Diff line change
Expand Up @@ -101,7 +101,7 @@ get_nx̂(mpc::PredictiveController) = mpc.estim.nx̂

function Base.show(io::IO, res::SimResult)
N = length(res.T_data)
print(io, "Simulation results of $(typeof(res.obj).name.name) with $N time steps.")
print(io, "Simulation results of $(nameof(typeof(res.obj))) with $N time steps.")
end


Expand Down Expand Up @@ -137,7 +137,7 @@ function sim!(
D_data = Matrix{NT}(undef, plant.nd, N)
X_data = Matrix{NT}(undef, plant.nx, N)
setstate!(plant, x_0)
@progress name="$(typeof(plant).name.name) simulation" for i=1:N
@progress name="$(nameof(typeof(plant))) simulation" for i=1:N
y = evaloutput(plant, d)
Y_data[:, i] .= y
U_data[:, i] .= u
Expand Down Expand Up @@ -282,7 +282,7 @@ function sim_closedloop!(
lastd, lasty = d, evaloutput(plant, d)
initstate!(est_mpc, lastu, lasty[estim.i_ym], lastd)
isnothing(x̂_0) || setstate!(est_mpc, x̂_0)
@progress name="$(typeof(est_mpc).name.name) simulation" for i=1:N
@progress name="$(nameof(typeof(est_mpc))) simulation" for i=1:N
d = lastd + d_step + d_noise.*randn(plant.nd)
y = evaloutput(plant, d) + y_step + y_noise.*randn(plant.ny)
ym = y[estim.i_ym]
Expand Down
6 changes: 3 additions & 3 deletions src/predictive_control.jl
Original file line number Diff line number Diff line change
Expand Up @@ -31,10 +31,10 @@ function Base.show(io::IO, mpc::PredictiveController)
nu, nd = mpc.estim.model.nu, mpc.estim.model.nd
nx̂, nym, nyu = mpc.estim.nx̂, mpc.estim.nym, mpc.estim.nyu
n = maximum(ndigits.((Hp, Hc, nu, nx̂, nym, nyu, nd))) + 1
println(io, "$(typeof(mpc).name.name) controller with a sample time Ts = "*
println(io, "$(nameof(typeof(mpc))) controller with a sample time Ts = "*
"$(mpc.estim.model.Ts) s, $(JuMP.solver_name(mpc.optim)) optimizer, "*
"$(typeof(mpc.transcription).name.name) transcription, "*
"$(typeof(mpc.estim).name.name) estimator and:")
"$(nameof(typeof(mpc.transcription))) transcription, "*
"$(nameof(typeof(mpc.estim))) estimator and:")
println(io, "$(lpad(Hp, n)) prediction steps Hp")
println(io, "$(lpad(Hc, n)) control steps Hc")
println(io, "$(lpad(nϵ, n)) slack variable ϵ (control constraints)")
Expand Down
2 changes: 1 addition & 1 deletion src/sim_model.jl
Original file line number Diff line number Diff line change
Expand Up @@ -372,7 +372,7 @@ function Base.show(io::IO, model::SimModel)
nu, nd = model.nu, model.nd
nx, ny = model.nx, model.ny
n = maximum(ndigits.((nu, nx, ny, nd))) + 1
println(io, "$(typeof(model).name.name) with a sample time Ts = $(model.Ts) s"*
println(io, "$(nameof(typeof(model))) with a sample time Ts = $(model.Ts) s"*
"$(detailstr(model)) and:")
println(io, "$(lpad(nu, n)) manipulated inputs u")
println(io, "$(lpad(nx, n)) states x")
Expand Down
4 changes: 2 additions & 2 deletions src/state_estim.jl
Original file line number Diff line number Diff line change
Expand Up @@ -32,8 +32,8 @@ function Base.show(io::IO, estim::StateEstimator)
nu, nd = estim.model.nu, estim.model.nd
nx̂, nym, nyu = estim.nx̂, estim.nym, estim.nyu
n = maximum(ndigits.((nu, nx̂, nym, nyu, nd))) + 1
println(io, "$(typeof(estim).name.name) estimator with a sample time "*
"Ts = $(estim.model.Ts) s, $(typeof(estim.model).name.name) and:")
println(io, "$(nameof(typeof(estim))) estimator with a sample time "*
"Ts = $(estim.model.Ts) s, $(nameof(typeof(estim.model))) and:")
print_estim_dim(io, estim, n)
end

Expand Down