Skip to content

Added support for unmeasured disturbances at model manipulated inputs. #15

New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Merged
merged 4 commits into from
Sep 7, 2023
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
2 changes: 1 addition & 1 deletion Project.toml
Original file line number Diff line number Diff line change
@@ -1,7 +1,7 @@
name = "ModelPredictiveControl"
uuid = "61f9bdb8-6ae4-484a-811f-bbf86720c31c"
authors = ["Francis Gagnon"]
version = "0.7.1"
version = "0.8.0"

[deps]
ControlSystemsBase = "aaaaaaaa-a6ca-5380-bf3e-84a91bcd477e"
Expand Down
6 changes: 4 additions & 2 deletions README.md
Original file line number Diff line number Diff line change
Expand Up @@ -93,7 +93,7 @@ for more detailed examples.
- [x] supported feedback strategy:
- [x] state estimator (see State Estimation features)
- [x] internal model structure with a custom stochastic model
- [x] offset-free tracking with a single or multiple integrators on measured outputs
- [x] automatic model augmentation with integrating states for offset-free tracking
- [x] support for unmeasured model outputs
- [x] feedforward action with measured disturbances that supports direct transmission
- [x] custom predictions for:
Expand All @@ -115,7 +115,9 @@ for more detailed examples.
- [x] extended Kalman filter
- [x] unscented Kalman filter
- [ ] moving horizon estimator
- [x] automatic model augmentation for offset-free tracking
- [x] easily estimate unmeasured disturbances by adding one or more integrators at the:
- [x] manipulated inputs
- [x] measured outputs
- [x] observers in predictor form to ease control applications
- [ ] moving horizon estimator that supports:
- [ ] inequality state constraints
Expand Down
1 change: 1 addition & 0 deletions docs/src/internals/state_estim.md
Original file line number Diff line number Diff line change
Expand Up @@ -3,6 +3,7 @@
## Estimator Initialization

```@docs
ModelPredictiveControl.init_estimstoch
ModelPredictiveControl.init_integrators
ModelPredictiveControl.augment_model
ModelPredictiveControl.init_ukf
Expand Down
4 changes: 2 additions & 2 deletions docs/src/manual/nonlinmpc.md
Original file line number Diff line number Diff line change
Expand Up @@ -59,7 +59,7 @@ plot(sim!(model, 60, u), plotu=false)
An [`UnscentedKalmanFilter`](@ref) estimates the plant state :

```@example 1
estim = UnscentedKalmanFilter(model, σQ=[0.1, 0.5], σR=[0.5], nint_ym=[1], σQ_int=[5.0])
estim = UnscentedKalmanFilter(model, σQ=[0.1, 0.5], σR=[0.5], nint_ym=[1], σQint_ym=[5.0])
```

The standard deviation of the angular velocity ``ω`` is higher here (`σQ` second value)
Expand All @@ -75,7 +75,7 @@ plot(res, plotu=false, plotxwithx̂=true)
```

The estimate ``x̂_3`` is the integrator state that compensates for static errors (`nint_ym`
and `σQ_int` parameters of [`UnscentedKalmanFilter`](@ref)). The Kalman filter performance
and `σQint_ym` parameters of [`UnscentedKalmanFilter`](@ref)). The Kalman filter performance
seems sufficient for control. As the motor torque is limited to -1.5 to 1.5 N m, we
incorporate the input constraints in a [`NonLinMPC`](@ref):

Expand Down
2 changes: 1 addition & 1 deletion src/controller/nonlinmpc.jl
Original file line number Diff line number Diff line change
Expand Up @@ -168,7 +168,7 @@ Use custom state estimator `estim` to construct `NonLinMPC`.
```jldoctest
julia> model = NonLinModel((x,u,_)->0.5x+u, (x,_)->2x, 10.0, 1, 1, 1);

julia> estim = UnscentedKalmanFilter(model, σQ_int=[0.05]);
julia> estim = UnscentedKalmanFilter(model, σQint_ym=[0.05]);

julia> mpc = NonLinMPC(estim, Hp=20, Hc=1, Cwt=1e6)
NonLinMPC controller with a sample time Ts = 10.0 s, Ipopt optimizer, UnscentedKalmanFilter estimator and:
Expand Down
213 changes: 127 additions & 86 deletions src/estimator/kalman.jl

Large diffs are not rendered by default.

24 changes: 15 additions & 9 deletions src/estimator/luenberger.jl
Original file line number Diff line number Diff line change
Expand Up @@ -7,8 +7,10 @@ struct Luenberger <: StateEstimator
nym::Int
nyu::Int
nxs::Int
As::Matrix{Float64}
Cs::Matrix{Float64}
As ::Matrix{Float64}
Cs_u::Matrix{Float64}
Cs_y::Matrix{Float64}
nint_u ::Vector{Int}
nint_ym::Vector{Int}
 ::Matrix{Float64}
B̂u ::Matrix{Float64}
Expand All @@ -18,9 +20,11 @@ struct Luenberger <: StateEstimator
Ĉm ::Matrix{Float64}
D̂dm ::Matrix{Float64}
K::Matrix{Float64}
function Luenberger(model, i_ym, nint_ym, p̂)
nym, nyu, nxs, nx̂, As, Cs, nint_ym = init_estimstoch(model, i_ym, nint_ym)
Â, B̂u, Ĉ, B̂d, D̂d = augment_model(model, As, Cs)
function Luenberger(model, i_ym, nint_u, nint_ym, p̂)
nym, nyu = validate_ym(model, i_ym)
As, Cs_u, Cs_y, nxs, nint_u, nint_ym = init_estimstoch(model, i_ym, nint_u, nint_ym)
nx̂ = model.nx + nxs
Â, B̂u, Ĉ, B̂d, D̂d = augment_model(model, As, Cs_u, Cs_y)
K = try
place(Â, Ĉ, p̂, :o)[:, i_ym]
catch
Expand All @@ -33,7 +37,7 @@ struct Luenberger <: StateEstimator
model,
lastu0, x̂,
i_ym, nx̂, nym, nyu, nxs,
As, Cs, nint_ym,
As, Cs_u, Cs_y, nint_u, nint_ym,
Â, B̂u, B̂d, Ĉ, D̂d,
Ĉm, D̂dm,
K
Expand All @@ -45,6 +49,7 @@ end
Luenberger(
model::LinModel;
i_ym = 1:model.ny,
nint_u = 0,
nint_ym = default_nint(model, i_ym),
p̂ = 1e-3*(0:(model.nx + sum(nint_ym)-1)) .+ 0.5)
)
Expand All @@ -53,7 +58,7 @@ Construct a Luenberger observer with the [`LinModel`](@ref) `model`.

`i_ym` provides the `model` output indices that are measured ``\mathbf{y^m}``, the rest are
unmeasured ``\mathbf{y^u}``. `model` matrices are augmented with the stochastic model, which
is specified by the numbers of output integrator `nint_ym` (see [`SteadyKalmanFilter`](@ref)
is specified by the numbers of integrator `nint_u` and `nint_ym` (see [`SteadyKalmanFilter`](@ref)
Extended Help). The argument `p̂` is a vector of `model.nx + sum(nint_ym)` elements
specifying the observer poles/eigenvalues (near ``z=0.5`` by default). The method computes
the observer gain ``\mathbf{K}`` with [`place`](https://juliacontrol.github.io/ControlSystems.jl/stable/lib/synthesis/#ControlSystemsBase.place).
Expand All @@ -74,15 +79,16 @@ Luenberger estimator with a sample time Ts = 0.5 s, LinModel and:
function Luenberger(
model::LinModel;
i_ym::IntRangeOrVector = 1:model.ny,
nint_ym::IntVectorOrInt = default_nint(model, i_ym),
nint_u ::IntVectorOrInt = 0,
nint_ym::IntVectorOrInt = default_nint(model, i_ym, nint_u),
p̂ = 1e-3*(0:(model.nx + sum(nint_ym)-1)) .+ 0.5
)
nx = model.nx
if length(p̂) ≠ model.nx + sum(nint_ym)
error("p̂ length ($(length(p̂))) ≠ nx ($nx) + integrator quantity ($(sum(nint_ym)))")
end
any(abs.(p̂) .≥ 1) && error("Observer poles p̂ should be inside the unit circles.")
return Luenberger(model, i_ym, nint_ym, p̂)
return Luenberger(model, i_ym, nint_u, nint_ym, p̂)
end


Expand Down
4 changes: 2 additions & 2 deletions src/predictive_control.jl
Original file line number Diff line number Diff line change
Expand Up @@ -913,14 +913,14 @@ with [`getestimates!`](@ref). The method also returns an empty ``\mathbf{P_s}``
it is useless except for [`InternalModel`](@ref) estimators.
"""
function init_stochpred(estim::StateEstimator, Hp)
As, Cs = estim.As, estim.Cs
As, Cs_y = estim.As, estim.Cs_y
nxs = estim.nxs
Ms = Matrix{Float64}(undef, Hp*nxs, nxs)
for i = 1:Hp
iRow = (1:nxs) .+ nxs*(i-1)
Ms[iRow, :] = As^i
end
Js = repeatdiag(Cs, Hp)
Js = repeatdiag(Cs_y, Hp)
Ks = Js*Ms
Ps = zeros(estim.model.ny*Hp, 0)
return Ks, Ps
Expand Down
157 changes: 73 additions & 84 deletions src/state_estim.jl
Original file line number Diff line number Diff line change
Expand Up @@ -70,26 +70,36 @@ function remove_op!(estim::StateEstimator, u, d, ym)
end

"""
init_estimstoch(model, i_ym, nint_ym)
init_estimstoch(model, i_ym, nint_u, nint_ym) -> As, Cs_u, Cs_y, nxs, nint_u, nint_ym

Init stochastic model matrices from integrator specifications for state estimation.

TBW.

The function [`init_integrators`](@ref) builds the state-space matrice of the unmeasured
disturbance models.
"""
function init_estimstoch(model, i_ym, nint_ym)
validate_ym(model, i_ym)
nx, ny = model.nx, model.ny
nym, nyu = length(i_ym), ny - length(i_ym)
Asm, Csm, nint_ym = init_integrators(i_ym, nint_ym)
nxs = size(Asm,1)
nx̂ = nx + nxs
As, _ , Cs = stoch_ym2y(model, i_ym, Asm, [], Csm, [])
return nym, nyu, nxs, nx̂, As, Cs, nint_ym
function init_estimstoch(model, i_ym, nint_u::IntVectorOrInt, nint_ym::IntVectorOrInt)
nu, ny, nym = model.nu, model.ny, length(i_ym)
As_u , Cs_u , nint_u = init_integrators(nint_u , nu , "u")
As_ym, Cs_ym, nint_ym = init_integrators(nint_ym, nym, "ym")
As_y, _ , Cs_y = stoch_ym2y(model, i_ym, As_ym, [], Cs_ym, [])
nxs_u, nxs_y = size(As_u, 1), size(As_y, 1)
# combines input and output stochastic models:
As = [As_u zeros(nxs_u, nxs_y); zeros(nxs_y, nxs_u) As_y]
Cs_u = [Cs_u zeros(nu, nxs_y)]
Cs_y = [zeros(ny, nxs_u) Cs_y]
nxs = nxs_u + nxs_y
return As, Cs_u, Cs_y, nxs, nint_u, nint_ym
end

"Validate the specified measured output indices `i_ym`."
function validate_ym(model::SimModel, i_ym)
if length(unique(i_ym)) ≠ length(i_ym) || maximum(i_ym) > model.ny
error("Measured output indices i_ym should contains valid and unique indices")
end
nym, nyu = length(i_ym), model.ny - length(i_ym)
return nym, nyu
end

"Convert the measured outputs stochastic model `stoch_ym` to all outputs `stoch_y`."
Expand All @@ -108,74 +118,49 @@ function stoch_ym2y(model::SimModel, i_ym, Asm, Bsm, Csm, Dsm)
end

@doc raw"""
init_integrators(i_ym, nint_ym::Vector{Int}) -> Asm, Csm, nint_ym
init_integrators(nint, nys, varname::String) -> As, Cs, nint

Calc stochastic model matrices from output integrators specifications for state estimation.
Calc state-space matrices `As, Cs` (stochastic part) from integrator specifications `nint`.

For closed-loop state estimators. `nint_ym` is a vector providing how many integrator should
be added for each measured output ``\mathbf{y^m}``. The argument generates the `Asm` and
`Csm` matrices:
This function is used to initialize the stochastic part of the augmented model for the
design of state estimators. The vector `nint` provides how many integrators (in series)
should be incorporated for each stochastic output ``\mathbf{y_s}``:
```math
\begin{aligned}
\mathbf{x_s}(k+1) &= \mathbf{A_s^m x_s}(k) + \mathbf{B_s^m e}(k) \\
\mathbf{y_s^m}(k) &= \mathbf{C_s^m x_s}(k)
\mathbf{x_s}(k+1) &= \mathbf{A_s x_s}(k) + \mathbf{B_s e}(k) \\
\mathbf{y_s}(k) &= \mathbf{C_s x_s}(k)
\end{aligned}
```
where ``\mathbf{e}(k)`` is a conceptual and unknown zero mean white noise.
``\mathbf{B_s^m}`` is not used for closed-loop state estimators thus ignored.
"""
function init_integrators(i_ym, nint_ym)
if nint_ym == 0 # alias for no output integrator at all
nint_ym = fill(0, length(i_ym))
end
nym = length(i_ym);
if length(nint_ym) ≠ nym
error("nint_ym size ($(length(nint_ym))) ≠ measured output quantity nym ($nym)")
end
any(nint_ym .< 0) && error("nint_ym values should be ≥ 0")
nxs = sum(nint_ym)
Asm, Csm = zeros(nxs, nxs), zeros(nym, nxs)
if nxs ≠ 0 # construct stochastic model state-space matrices (integrators) :
i_Asm, i_Csm = 1, 1
for iym = 1:nym
nint = nint_ym[iym]
if nint ≠ 0
rows_Asm = (i_Asm):(i_Asm + nint - 1)
Asm[rows_Asm, rows_Asm] = Bidiagonal(ones(nint), ones(nint-1), :L)
Csm[iym, i_Csm+nint-1] = 1
i_Asm += nint
i_Csm += nint
end
end
end
return Asm, Csm, nint_ym
end
where ``\mathbf{e}(k)`` is an unknown zero mean white noise. The estimations does not use
``\mathbf{B_s}``, it is thus ignored. Note that this function is called twice :

function init_integrators_u(nu, nint_u)
if nint_u == 0 # alias for no output integrator at all
nint_u = fill(0, length(nu))
1. for the unmodeled disturbances at measured outputs ``\mathbf{y^m}``
2. for the unmodeled disturbances at manipulated inputs ``\mathbf{u}``
"""
function init_integrators(nint::IntVectorOrInt, nys, varname::String)
if nint == 0 # alias for no integrator at all
nint = fill(0, nys)
end
#nym = length(i_ym);
if length(nint_u) ≠ nu
error("nint_u size ($(length(nint_u))) ≠ manipulated input quantity nu ($nu)")
if length(nint) ≠ nys
error("nint_$(varname) size ($(length(nint))) ≠ n$(varname) ($nys)")
end
any(nint_u .< 0) && error("nint_u values should be ≥ 0")
nxs = sum(nint_u)
Asm, Csm = zeros(nxs, nxs), zeros(nym, nxs)
any(nint .< 0) && error("nint_$(varname) values should be ≥ 0")
nxs = sum(nint)
As, Cs = zeros(nxs, nxs), zeros(nys, nxs)
if nxs ≠ 0 # construct stochastic model state-space matrices (integrators) :
i_Asm, i_Csm = 1, 1
for iym = 1:nym
nint = nint_u[iym]
if nint ≠ 0
rows_Asm = (i_Asm):(i_Asm + nint - 1)
Asm[rows_Asm, rows_Asm] = Bidiagonal(ones(nint), ones(nint-1), :L)
Csm[iym, i_Csm+nint-1] = 1
i_Asm += nint
i_Csm += nint
i_As, i_Cs = 1, 1
for i = 1:nys
nint_i = nint[i]
if nint_i ≠ 0
rows_As = (i_As):(i_As + nint_i - 1)
As[rows_As, rows_As] = Bidiagonal(ones(nint_i), ones(nint_i-1), :L)
Cs[i, i_Cs+nint_i-1] = 1
i_As += nint_i
i_Cs += nint_i
end
end
end
return Asm, Csm, nint_u
return As, Cs, nint
end

@doc raw"""
Expand All @@ -195,26 +180,27 @@ returns the augmented matrices `Â`, `B̂u`, `Ĉ`, `B̂d` and `D̂d`:
```
An error is thrown if the augmented model is not observable and `verify_obsv == true`.
"""
function augment_model(model::LinModel, As, Cs; verify_obsv=true)
function augment_model(model::LinModel, As, Cs_u, Cs_y; verify_obsv=true)
nu, nx, nd = model.nu, model.nx, model.nd
nxs = size(As, 1)
 = [model.A zeros(nx,nxs); zeros(nxs,nx) As]
B̂u = [model.Bu; zeros(nxs,nu)]
Ĉ = [model.C Cs]
B̂d = [model.Bd; zeros(nxs,nd)]
 = [model.A model.Bu*Cs_u; zeros(nxs,nx) As]
B̂u = [model.Bu; zeros(nxs, nu)]
Ĉ = [model.C Cs_y]
B̂d = [model.Bd; zeros(nxs, nd)]
D̂d = model.Dd
# observability on Ĉ instead of Ĉm, since it would always return false when nym ≠ ny:
if verify_obsv && !observability(Â, Ĉ)[:isobservable]
error("The augmented model is unobservable. You may try to use 0 "*
"integrator on model integrating outputs with nint_ym parameter.")
error("The augmented model is unobservable. You may try to use 0 integrator on "*
"model integrating outputs with nint_ym parameter. Adding integrators at both "*
"inputs (nint_u) and outputs (nint_ym) can also violate observability.")
end
return Â, B̂u, Ĉ, B̂d, D̂d
end
"No need to augment the model if `model` is not a [`LinModel`](@ref)."
augment_model(::SimModel, _ , _ ) = nothing
augment_model(::SimModel, _ , _ , _ ) = nothing

@doc raw"""
default_nint(model::LinModel, i_ym=1:model.ny)
default_nint(model::LinModel, i_ym=1:model.ny, nint_u)

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

Expand All @@ -234,24 +220,27 @@ julia> nint_ym = default_nint(model)
1
```
"""
function default_nint(model::LinModel, i_ym::IntRangeOrVector = 1:model.ny)
function default_nint(model::LinModel, i_ym=1:model.ny, nint_u=0)
validate_ym(model, i_ym)
nint_ym = fill(0, length(i_ym))
for i in eachindex(i_ym)
nint_ym[i] = 1
Asm, Csm = init_integrators(i_ym, nint_ym)
As , _ , Cs = stoch_ym2y(model, i_ym, Asm, [], Csm, [])
 , _ , Ĉ = augment_model(model, As, Cs, verify_obsv=false)
As, Cs_u, Cs_y = init_estimstoch(model, i_ym, nint_u, nint_ym)
Â, _ , Ĉ = augment_model(model, As, Cs_u, Cs_y, verify_obsv=false)
# observability on Ĉ instead of Ĉm, since it would always return false when nym ≠ ny
observability(Â, Ĉ)[:isobservable] || (nint_ym[i] = 0)
end
return nint_ym
end
"""
default_nint(model::SimModel, i_ym=1:model.ny)
default_nint(model::SimModel, i_ym=1:model.ny, nint_u=0)

One integrator on each measured output by default if `model` is not a [`LinModel`](@ref).
"""
default_nint(::SimModel, i_ym::IntRangeOrVector = 1:model.ny) = fill(1, length(i_ym))
function default_nint(model::SimModel, i_ym=1:model.ny, nint_u=0)
validate_ym(model, i_ym)
return fill(1, length(i_ym))
end

@doc raw"""
f̂(estim::StateEstimator, x̂, u, d)
Expand All @@ -269,8 +258,8 @@ function returns the next state of the augmented model, defined as:
"""
function f̂(estim::StateEstimator, x̂, u, d)
# `@views` macro avoid copies with matrix slice operator e.g. [a:b]
nx = estim.model.nx
@views return [f(estim.model, x̂[1:nx], u, d); estim.As*x̂[nx+1:end]]
@views x̂d, x̂s = x̂[1:estim.model.nx], x̂[estim.model.nx+1:end]
return [f(estim.model, x̂d, u + estim.Cs_u*x̂s, d); estim.As*x̂s]
end

@doc raw"""
Expand All @@ -280,8 +269,8 @@ Output function ``\mathbf{ĥ}`` of the augmented model, see [`f̂`](@ref) for d
"""
function ĥ(estim::StateEstimator, x̂, d)
# `@views` macro avoid copies with matrix slice operator e.g. [a:b]
nx = estim.model.nx
@views return h(estim.model, x̂[1:nx], d) + estim.Cs*x̂[nx+1:end]
@views x̂d, x̂s = x̂[1:estim.model.nx], x̂[estim.model.nx+1:end]
return h(estim.model, x̂d, d) + estim.Cs_y*x̂s
end


Expand Down
Loading