Skip to content
Open
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
2 changes: 1 addition & 1 deletion Project.toml
Original file line number Diff line number Diff line change
@@ -1,6 +1,6 @@
name = "ModelPredictiveControl"
uuid = "61f9bdb8-6ae4-484a-811f-bbf86720c31c"
version = "2.16.4"
version = "2.16.5"
authors = ["Francis Gagnon"]

[deps]
Expand Down
7 changes: 6 additions & 1 deletion src/controller/construct.jl
Original file line number Diff line number Diff line change
Expand Up @@ -854,13 +854,18 @@ end
"Check the condition number of `H̃` and display a `@warn` if `cond(H̃) > warn_cond`."
function verify_cond(::SingleShooting, H̃, warn_cond)
if !isinf(warn_cond)
cond_H̃ = cond(H̃)
cond_H̃ = try
cond(H̃)
catch
NaN
end
cond_H̃ > warn_cond && @warn(
"The Hessian condition number cond_H̃ > $warn_cond. The optimization " *
"problem may be ill-conditioned.\n Consider changing the tunings, using " *
"MultipleShooting, or using an optimizer more robust to this like DAQP.",
cond_H̃,
)
isnan(cond_H̃) && @warn("could not validate the condition number of the QP Hessian.")
end
return nothing
end
Expand Down
7 changes: 6 additions & 1 deletion src/controller/explicitmpc.jl
Original file line number Diff line number Diff line change
Expand Up @@ -167,11 +167,16 @@ function ExplicitMPC(
L_Hp = Diagonal(repeat(Lwt, Hp)),
) where {NT<:Real, SE<:StateEstimator{NT}}
isa(estim.model, LinModel) || error(MSG_LINMODEL_ERR)
nk = estimate_delays(estim.model)
nk = try
estimate_delays(estim.model)
catch myerr
NaN
end
if Hp ≤ nk
@warn("prediction horizon Hp ($Hp) ≤ estimated number of delays in model "*
"($nk), the closed-loop system may be unstable or zero-gain (unresponsive)")
end
isnan(nk) && @warn("could not estimate the model delay, Hp ($Hp) might be too small.")
nb = move_blocking(Hp, Hc)
Hc = get_Hc(nb)
weights = ControllerWeights(estim.model, Hp, Hc, M_Hp, N_Hc, L_Hp)
Expand Down
37 changes: 21 additions & 16 deletions src/estimator/construct.jl
Original file line number Diff line number Diff line change
Expand Up @@ -90,17 +90,12 @@ struct KalmanCovariances{
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) # does not apply to steady-state Kalman filter
P̂ = zeros(NT, size(Q̂)) # will hold the steady-state error covariance
P̂_0 = Hermitian(zeros(NT, 0, 0), :L) # don't apply to SteadyKalmanFilter
P̂ = Hermitian(zeros(NT, size(Q̂)), :L) # will hold the steady-state error covariance
invP̄ = copy(P̂_0)
else
P̂ = copy(P̂_0)
end
P̂_0 = Hermitian(P̂_0, :L)
P̂ = Hermitian(P̂, :L)
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̂)
P̂_0 = Hermitian(P̂_0, :L)
P̂, invP̄ = copy(P̂_0), copy(P̂_0)
try
inv!(invP̄)
catch err
Expand All @@ -110,6 +105,13 @@ struct KalmanCovariances{
rethrow()
end
end
end
P̂_0 = Hermitian(P̂_0, :L)
P̂ = Hermitian(P̂, :L)
Q̂ = Hermitian(Q̂, :L)
R̂ = Hermitian(R̂, :L)
# the following variables are only for the moving horizon estimator:
invQ̂, invR̂ = copy(Q̂), copy(R̂)
try
inv!(invQ̂)
catch err
Expand Down Expand Up @@ -357,15 +359,15 @@ function augment_model(model::SimModel{NT}, As, args... ; verify_obsv=false) whe
end

@doc raw"""
default_nint(model::LinModel, i_ym=1:model.ny, nint_u=0) -> nint_ym
default_nint(model::LinModel, i_ym=1:model.ny, nint_u=0; verify_obsv=true) -> nint_ym

Get default integrator quantity per measured outputs `nint_ym` for [`LinModel`](@ref).

The arguments `i_ym` and `nint_u` are the measured output indices and the integrator
quantity on each manipulated input, respectively. By default, one integrator is added on
each measured outputs. If ``\mathbf{Â, Ĉ}`` matrices of the augmented model become
unobservable, the integrator is removed. This approach works well for stable, integrating
and unstable `model` (see Examples).
and unstable `model` (see Examples). The observability checkup is skipped if `!verify_obsv`.

# Examples
```jldoctest
Expand All @@ -375,9 +377,11 @@ julia> default_nint(model) |> println
[1, 0, 1]
```
"""
function default_nint(model::LinModel, i_ym=1:model.ny, nint_u=0)
function default_nint(model::LinModel, i_ym=1:model.ny, nint_u=0; verify_obsv=true)
validate_ym(model, i_ym)
nint_ym = fill(0, length(i_ym))
nym = length(i_ym)
verify_obsv || return iszero(nint_u) ? fill(1, nym) : fill(0, nym)
nint_ym = fill(0, nym)
for i in eachindex(i_ym)
nint_ym[i] = 1
As, Cs_u, Cs_y = init_estimstoch(model, i_ym, nint_u, nint_ym)
Expand All @@ -389,7 +393,7 @@ function default_nint(model::LinModel, i_ym=1:model.ny, nint_u=0)
end

"""
default_nint(model::SimModel, i_ym=1:model.ny, nint_u=0) -> nint_ym
default_nint(model::SimModel, i_ym=1:model.ny, nint_u=0; verify_obsv=false) -> nint_ym

One integrator on each measured output by default for other cases e.g. [`NonLinModel`](@ref).

Expand All @@ -400,7 +404,8 @@ integrator on each measured output.
Theres is no verification the augmented model remains observable. The resulting
[`StateEstimator`](@ref) object should be assessed separately with e.g.: [`sim!`](@ref).
"""
function default_nint(model::SimModel, i_ym=1:model.ny, nint_u=0)
function default_nint(model::SimModel, i_ym=1:model.ny, nint_u=0; verify_obsv=false)
verify_obsv && error("observability verification of non-LinModel is not supported.")
validate_ym(model, i_ym)
nint_ym = iszero(nint_u) ? fill(1, length(i_ym)) : fill(0, length(i_ym))
return nint_ym
Expand Down
53 changes: 32 additions & 21 deletions src/estimator/kalman.jl
Original file line number Diff line number Diff line change
Expand Up @@ -33,14 +33,15 @@ struct SteadyKalmanFilter{
prepared::Vector{Bool}
buffer::StateEstimatorBuffer{NT}
function SteadyKalmanFilter{NT}(
model::SM, i_ym, nint_u, nint_ym, cov::KC; direct=true
model::SM, i_ym, nint_u, nint_ym, cov::KC; direct=true, verify_obsv=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)
nxs = size(As, 1)
nx̂ = model.nx + nxs
Â, B̂u, Ĉ, B̂d, D̂d, x̂op, f̂op = augment_model(model, As, Cs_u, Cs_y)
Â, B̂u, Ĉ, B̂d, D̂d, x̂op, f̂op =
augment_model(model, As, Cs_u, Cs_y; verify_obsv)
Ĉm, D̂dm = Ĉ[i_ym, :], D̂d[i_ym, :]
R̂, Q̂ = cov.R̂, cov.Q̂
K̂, P̂ = init_skf(i_ym, Â, Ĉ, Q̂, R̂; direct)
Expand Down Expand Up @@ -111,6 +112,7 @@ state of the next time step ``\mathbf{x̂}_k(k+1)``. This estimator is allocatio
disturbances at measured outputs ``\mathbf{Q_{int_{ym}}}`` (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).
- `verify_obsv=true`: verify observability of the augmented [`LinModel`](@ref).

# Examples
```jldoctest
Expand Down Expand Up @@ -166,7 +168,8 @@ function SteadyKalmanFilter(
sigmaQ = fill(1/model.nx, model.nx),
sigmaR = fill(1, length(i_ym)),
nint_u ::IntVectorOrInt = 0,
nint_ym::IntVectorOrInt = default_nint(model, i_ym, nint_u),
verify_obsv = true,
nint_ym::IntVectorOrInt = default_nint(model, i_ym, nint_u; verify_obsv),
sigmaQint_u = fill(1, max(sum(nint_u), 0)),
sigmaQint_ym = fill(1, max(sum(nint_ym), 0)),
direct = true,
Expand All @@ -178,7 +181,7 @@ function SteadyKalmanFilter(
# estimated covariances matrices (variance = σ²) :
Q̂ = Diagonal([σQ; σQint_u; σQint_ym].^2)
R̂ = Diagonal([σR;].^2)
return SteadyKalmanFilter(model, i_ym, nint_u, nint_ym, Q̂, R̂; direct)
return SteadyKalmanFilter(model, i_ym, nint_u, nint_ym, Q̂, R̂; direct, verify_obsv)
end

@doc raw"""
Expand All @@ -189,11 +192,11 @@ Construct the estimator from the augmented covariance matrices `Q̂` and `R̂`.
This syntax allows nonzero off-diagonal elements in ``\mathbf{Q̂, R̂}``.
"""
function SteadyKalmanFilter(
model::SM, i_ym, nint_u, nint_ym, Q̂, R̂; direct=true
model::SM, i_ym, nint_u, nint_ym, Q̂, R̂; direct=true, verify_obsv=true
) where {NT<:Real, SM<:LinModel{NT}}
Q̂, R̂ = to_mat(Q̂), to_mat(R̂)
cov = KalmanCovariances(model, i_ym, nint_u, nint_ym, Q̂, R̂)
return SteadyKalmanFilter{NT}(model, i_ym, nint_u, nint_ym, cov; direct)
return SteadyKalmanFilter{NT}(model, i_ym, nint_u, nint_ym, cov; direct, verify_obsv)
end

"""
Expand Down Expand Up @@ -340,14 +343,15 @@ struct KalmanFilter{
prepared::Vector{Bool}
buffer::StateEstimatorBuffer{NT}
function KalmanFilter{NT}(
model::SM, i_ym, nint_u, nint_ym, cov::KC; direct=true
model::SM, i_ym, nint_u, nint_ym, cov::KC; direct=true, verify_obsv=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)
nxs = size(As, 1)
nx̂ = model.nx + nxs
Â, B̂u, Ĉ, B̂d, D̂d, x̂op, f̂op = augment_model(model, As, Cs_u, Cs_y)
Â, B̂u, Ĉ, B̂d, D̂d, x̂op, f̂op =
augment_model(model, As, Cs_u, Cs_y; verify_obsv)
Ĉm, D̂dm = Ĉ[i_ym, :], D̂d[i_ym, :]
x̂0 = [zeros(NT, model.nx); zeros(NT, nxs)]
K̂ = zeros(NT, nx̂, nym)
Expand Down Expand Up @@ -407,6 +411,7 @@ This estimator is allocation-free.
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).
- `verify_obsv=true`: verify observability of the augmented [`LinModel`](@ref).

# Examples
```jldoctest
Expand All @@ -431,7 +436,8 @@ function KalmanFilter(
sigmaQ = fill(1/model.nx, model.nx),
sigmaR = fill(1, length(i_ym)),
nint_u ::IntVectorOrInt = 0,
nint_ym::IntVectorOrInt = default_nint(model, i_ym, nint_u),
verify_obsv = true,
nint_ym::IntVectorOrInt = default_nint(model, i_ym, nint_u; verify_obsv),
sigmaPint_u_0 = fill(1, max(sum(nint_u), 0)),
sigmaQint_u = fill(1, max(sum(nint_u), 0)),
sigmaPint_ym_0 = fill(1, max(sum(nint_ym), 0)),
Expand All @@ -449,7 +455,7 @@ function KalmanFilter(
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)
return KalmanFilter(model, i_ym, nint_u, nint_ym, P̂_0, Q̂, R̂; direct, verify_obsv)
end

@doc raw"""
Expand All @@ -460,11 +466,11 @@ Construct the estimator from the augmented covariance matrices `P̂_0`, `Q̂` an
This syntax allows nonzero off-diagonal elements in ``\mathbf{P̂}_{-1}(0), \mathbf{Q̂, R̂}``.
"""
function KalmanFilter(
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̂; direct=true, verify_obsv=true
) where {NT<:Real, SM<:LinModel{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)
return KalmanFilter{NT}(model, i_ym, nint_u, nint_ym, cov; direct)
return KalmanFilter{NT}(model, i_ym, nint_u, nint_ym, cov; direct, verify_obsv)
end

@doc raw"""
Expand Down Expand Up @@ -566,14 +572,16 @@ struct UnscentedKalmanFilter{
prepared::Vector{Bool}
buffer::StateEstimatorBuffer{NT}
function UnscentedKalmanFilter{NT}(
model::SM, i_ym, nint_u, nint_ym, cov::KC, α, β, κ; direct=true
model::SM, i_ym, nint_u, nint_ym, cov::KC, α, β, κ;
direct=true, verify_obsv=true
) where {NT<:Real, SM<:SimModelODE{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)
nxs = size(As, 1)
nx̂ = model.nx + nxs
Â, B̂u, Ĉ, B̂d, D̂d, x̂op, f̂op = augment_model(model, As, Cs_u, Cs_y)
Â, B̂u, Ĉ, B̂d, D̂d, x̂op, f̂op =
augment_model(model, As, Cs_u, Cs_y; verify_obsv)
Ĉm, D̂dm = Ĉ[i_ym, :], D̂d[i_ym, :]
nσ, γ, m̂, Ŝ = init_ukf(nx̂, α, β, κ)
x̂0 = [zeros(NT, model.nx); zeros(NT, nxs)]
Expand Down Expand Up @@ -728,11 +736,14 @@ Construct the estimator from the augmented covariance matrices `P̂_0`, `Q̂` an
This syntax allows nonzero off-diagonal elements in ``\mathbf{P̂}_{-1}(0), \mathbf{Q̂, R̂}``.
"""
function UnscentedKalmanFilter(
model::SM, i_ym, nint_u, nint_ym, P̂_0, Q̂, R̂, α=1e-3, β=2, κ=0; direct=true
model::SM, i_ym, nint_u, nint_ym, P̂_0, Q̂, R̂, α=1e-3, β=2, κ=0;
direct=true
) where {NT<:Real, SM<:SimModelODE{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)
return UnscentedKalmanFilter{NT}(model, i_ym, nint_u, nint_ym, cov, α, β, κ; direct)
return UnscentedKalmanFilter{NT}(
model, i_ym, nint_u, nint_ym, cov, α, β, κ; direct
)
end


Expand Down Expand Up @@ -953,7 +964,8 @@ struct ExtendedKalmanFilter{
function ExtendedKalmanFilter{NT}(
model::SM,
i_ym, nint_u, nint_ym, cov::KC;
jacobian::JB, linfuncF̂!::FF, linfuncĤ!::HF, direct=true
jacobian::JB, linfuncF̂!::FF, linfuncĤ!::HF,
direct=true
) where {
NT<:Real,
SM<:SimModelODE,
Expand Down Expand Up @@ -1077,9 +1089,7 @@ function ExtendedKalmanFilter(
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
)
return ExtendedKalmanFilter(model, i_ym, nint_u, nint_ym, P̂_0, Q̂, R̂; jacobian, direct)
end

@doc raw"""
Expand All @@ -1092,7 +1102,8 @@ Construct the estimator from the augmented covariance matrices `P̂_0`, `Q̂` an
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̂; jacobian=AutoForwardDiff(), direct=true
model::SM, i_ym, nint_u, nint_ym, P̂_0, Q̂, R̂;
jacobian=AutoForwardDiff(), direct=true
) where {NT<:Real, SM<:SimModelODE{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)
Expand Down
13 changes: 8 additions & 5 deletions src/estimator/luenberger.jl
Original file line number Diff line number Diff line change
Expand Up @@ -25,15 +25,16 @@ struct Luenberger{NT<:Real, SM<:LinModel} <: StateEstimator{NT}
prepared::Vector{Bool}
buffer::StateEstimatorBuffer{NT}
function Luenberger{NT, SM}(
model, i_ym, nint_u, nint_ym, poles; direct=true
model, i_ym, nint_u, nint_ym, poles; direct=true, verify_obsv=true
) where {NT<:Real, SM<:LinModel}
nu, ny, nd, nk̄ = model.nu, model.ny, model.nd, model.nk̄
nym, nyu = validate_ym(model, i_ym)
validate_luenberger(model, nint_u, nint_ym, poles)
As, Cs_u, Cs_y, nint_u, nint_ym = init_estimstoch(model, i_ym, nint_u, nint_ym)
nxs = size(As, 1)
nx̂ = model.nx + nxs
Â, B̂u, Ĉ, B̂d, D̂d, x̂op, f̂op = augment_model(model, As, Cs_u, Cs_y)
Â, B̂u, Ĉ, B̂d, D̂d, x̂op, f̂op =
augment_model(model, As, Cs_u, Cs_y; verify_obsv)
Ĉm, D̂dm = Ĉ[i_ym, :], D̂d[i_ym, :]
K̂ = try
ControlSystemsBase.place(Â, Ĉ, poles, :o; direct)[:, i_ym]
Expand Down Expand Up @@ -63,7 +64,8 @@ end
nint_u = 0,
nint_ym = default_nint(model, i_ym),
poles = 1e-3*(1:(model.nx + sum(nint_u) + sum(nint_ym))) .+ 0.5,
direct = true
direct = true,
verify_obsv = true
)

Construct a Luenberger observer with the [`LinModel`](@ref) `model`.
Expand Down Expand Up @@ -98,11 +100,12 @@ function Luenberger(
model::SM;
i_ym::AbstractVector{Int} = 1:model.ny,
nint_u ::IntVectorOrInt = 0,
nint_ym::IntVectorOrInt = default_nint(model, i_ym, nint_u),
verify_obsv = true,
nint_ym::IntVectorOrInt = default_nint(model, i_ym, nint_u; verify_obsv),
poles = 1e-3*(1:(model.nx + sum(nint_u) + sum(nint_ym))) .+ 0.5,
direct = true
) where{NT<:Real, SM<:LinModel{NT}}
return Luenberger{NT, SM}(model, i_ym, nint_u, nint_ym, poles; direct)
return Luenberger{NT, SM}(model, i_ym, nint_u, nint_ym, poles; direct, verify_obsv)
end

"Validate the quantity and stability of the Luenberger `poles`."
Expand Down
4 changes: 2 additions & 2 deletions src/estimator/manual.jl
Original file line number Diff line number Diff line change
Expand Up @@ -31,7 +31,7 @@ struct ManualEstimator{NT<:Real, SM<:SimModel} <: StateEstimator{NT}
As, Cs_u, Cs_y, nint_u, nint_ym = init_estimstoch(model, i_ym, nint_u, nint_ym)
nxs = size(As, 1)
nx̂ = model.nx + nxs
Â, B̂u, Ĉ, B̂d, D̂d, x̂op, f̂op = augment_model(model, As, Cs_u, Cs_y)
Â, B̂u, Ĉ, B̂d, D̂d, x̂op, f̂op = augment_model(model, As, Cs_u, Cs_y; verify_obsv=false)
Ĉm, D̂dm = Ĉ[i_ym, :], D̂d[i_ym, :]
x̂0 = [zeros(NT, model.nx); zeros(NT, nxs)]
direct = false
Expand Down Expand Up @@ -100,7 +100,7 @@ ManualEstimator estimator with a sample time Ts = 0.5 s:
julia> function man_sim()
f(x,u,_,_) = 0.5*sin.(x + u)
h(x,_,_) = x
model = NonLinModel(f, h, 100.0, 1, 1, 1, solver=nothing)
model = NonLinModel(f, h, 1000.0, 1, 1, 1, solver=nothing)
linModel = linearize(model, x=[0], u=[0])
man = ManualEstimator(linModel, nint_u=[1])
mpc = LinMPC(man)
Expand Down
Loading
Loading