Skip to content

Commit 54a626d

Browse files
authored
Merge pull request #64 from JuliaControl/nonunicode_api
Offer an alternative non-unicode API for keyword arguments
2 parents 60a9a18 + 4f2410e commit 54a626d

File tree

9 files changed

+253
-143
lines changed

9 files changed

+253
-143
lines changed

docs/src/manual/nonlinmpc.md

Lines changed: 2 additions & 2 deletions
Original file line numberDiff line numberDiff line change
@@ -73,10 +73,10 @@ plot(res, plotu=false)
7373
savefig(ans, "plot1_NonLinMPC.svg"); nothing # hide
7474
```
7575

76-
The [`setname!`](@ref) function allows customizing the Y-axis labels.
77-
7876
![plot1_NonLinMPC](plot1_NonLinMPC.svg)
7977

78+
The [`setname!`](@ref) function allows customizing the Y-axis labels.
79+
8080
## Nonlinear Model Predictive Controller
8181

8282
An [`UnscentedKalmanFilter`](@ref) estimates the plant state :

src/controller/construct.jl

Lines changed: 28 additions & 14 deletions
Original file line numberDiff line numberDiff line change
@@ -53,6 +53,14 @@ for details on bounds and softness parameters ``\mathbf{c}``. The output and ter
5353
constraints are all soft by default. See Extended Help for time-varying constraints.
5454
5555
# Arguments
56+
!!! info
57+
The keyword arguments `Δumin`, `Δumax`, `c_Δumin`, `c_Δumax`, `x̂min`, `x̂max`, `c_x̂min`,
58+
`c_x̂max` and their capital letter versions have non-Unicode alternatives e.g.
59+
*`Deltaumin`*, *`xhatmax`* and *`C_Deltaumin`*
60+
61+
The default constraints are mentioned here for clarity but omitting a keyword argument
62+
will not re-assign to its default value (defaults are set at construction only).
63+
5664
- `mpc::PredictiveController` : predictive controller to set constraints
5765
- `umin=fill(-Inf,nu)` / `umax=fill(+Inf,nu)` : manipulated input bound ``\mathbf{u_{min/max}}``
5866
- `Δumin=fill(-Inf,nu)` / `Δumax=fill(+Inf,nu)` : manipulated input increment bound ``\mathbf{Δu_{min/max}}``
@@ -114,20 +122,26 @@ LinMPC controller with a sample time Ts = 4.0 s, OSQP optimizer, SteadyKalmanFil
114122
"""
115123
function setconstraint!(
116124
mpc::PredictiveController;
117-
umin = nothing, umax = nothing,
118-
Δumin = nothing, Δumax = nothing,
119-
ymin = nothing, ymax = nothing,
120-
x̂min = nothing, x̂max = nothing,
121-
c_umin = nothing, c_umax = nothing,
122-
c_Δumin = nothing, c_Δumax = nothing,
123-
c_ymin = nothing, c_ymax = nothing,
124-
c_x̂min = nothing, c_x̂max = nothing,
125-
Umin = nothing, Umax = nothing,
126-
ΔUmin = nothing, ΔUmax = nothing,
127-
Ymin = nothing, Ymax = nothing,
128-
C_umax = nothing, C_umin = nothing,
129-
C_Δumax = nothing, C_Δumin = nothing,
130-
C_ymax = nothing, C_ymin = nothing,
125+
umin = nothing, umax = nothing,
126+
Deltaumin = nothing, Deltaumax = nothing,
127+
ymin = nothing, ymax = nothing,
128+
xhatmin = nothing, xhatmax = nothing,
129+
c_umin = nothing, c_umax = nothing,
130+
c_Deltaumin = nothing, c_Deltaumax = nothing,
131+
c_ymin = nothing, c_ymax = nothing,
132+
c_xhatmin = nothing, c_xhatmax = nothing,
133+
Umin = nothing, Umax = nothing,
134+
DeltaUmin = nothing, DeltaUmax = nothing,
135+
Ymin = nothing, Ymax = nothing,
136+
C_umax = nothing, C_umin = nothing,
137+
C_Deltaumax = nothing, C_Deltaumin = nothing,
138+
C_ymax = nothing, C_ymin = nothing,
139+
Δumin = Deltaumin, Δumax = Deltaumax,
140+
x̂min = xhatmin, x̂max = xhatmax,
141+
c_Δumin = c_Deltaumin, c_Δumax = c_Deltaumax,
142+
c_x̂min = c_xhatmin, c_x̂max = c_xhatmax,
143+
ΔUmin = DeltaUmin, ΔUmax = DeltaUmax,
144+
C_Δumin = C_Deltaumin, C_Δumax = C_Deltaumax,
131145
)
132146
model, con, optim = mpc.estim.model, mpc.con, mpc.optim
133147
nu, ny, nx̂, Hp, Hc = model.nu, model.ny, mpc.estim.nx̂, mpc.Hp, mpc.Hc

src/controller/execute.jl

Lines changed: 40 additions & 20 deletions
Original file line numberDiff line numberDiff line change
@@ -24,15 +24,19 @@ Calling a [`PredictiveController`](@ref) object calls this method.
2424
See also [`LinMPC`](@ref), [`ExplicitMPC`](@ref), [`NonLinMPC`](@ref).
2525
2626
# Arguments
27+
28+
Keyword arguments in *`italic`* are non-Unicode alternatives.
29+
2730
- `mpc::PredictiveController` : solve optimization problem of `mpc`.
2831
- `ry=mpc.estim.model.yop` : current output setpoints ``\mathbf{r_y}(k)``.
2932
- `d=[]` : current measured disturbances ``\mathbf{d}(k)``.
30-
- `D̂=repeat(d, mpc.Hp)` : predicted measured disturbances ``\mathbf{D̂}``, constant in the
31-
future by default or ``\mathbf{d̂}(k+j)=\mathbf{d}(k)`` for ``j=1`` to ``H_p``.
32-
- `R̂y=repeat(ry, mpc.Hp)` : predicted output setpoints ``\mathbf{R̂_y}``, constant in the
33-
future by default or ``\mathbf{r̂_y}(k+j)=\mathbf{r_y}(k)`` for ``j=1`` to ``H_p``.
34-
- `R̂u=repeat(mpc.estim.model.uop, mpc.Hp)` : predicted manipulated input setpoints, constant
35-
in the future by default or ``\mathbf{r̂_u}(k+j)=\mathbf{u_{op}}`` for ``j=0`` to ``H_p-1``.
33+
- `D̂=repeat(d, mpc.Hp)` or *`Dhat`* : predicted measured disturbances ``\mathbf{D̂}``, constant
34+
in the future by default or ``\mathbf{d̂}(k+j)=\mathbf{d}(k)`` for ``j=1`` to ``H_p``.
35+
- `R̂y=repeat(ry, mpc.Hp)` or *`Rhaty`* : predicted output setpoints ``\mathbf{R̂_y}``, constant
36+
in the future by default or ``\mathbf{r̂_y}(k+j)=\mathbf{r_y}(k)`` for ``j=1`` to ``H_p``.
37+
- `R̂u=repeat(mpc.estim.model.uop, mpc.Hp)` or *`Rhatu`* : predicted manipulated input
38+
setpoints, constant in the future by default or ``\mathbf{r̂_u}(k+j)=\mathbf{u_{op}}`` for
39+
``j=0`` to ``H_p-1``.
3640
- `ym=nothing` : current measured outputs ``\mathbf{y^m}(k)``, only required if `mpc.estim`
3741
is an [`InternalModel`](@ref).
3842
@@ -49,10 +53,13 @@ function moveinput!(
4953
mpc::PredictiveController,
5054
ry::Vector = mpc.estim.model.yop,
5155
d ::Vector = empty(mpc.estim.x̂0);
52-
::Vector = repeat(d, mpc.Hp),
53-
R̂y::Vector = repeat(ry, mpc.Hp),
54-
R̂u::Vector = mpc.noR̂u ? empty(mpc.estim.x̂0) : repeat(mpc.estim.model.uop, mpc.Hp),
55-
ym::Union{Vector, Nothing} = nothing
56+
Dhat ::Vector = repeat(d, mpc.Hp),
57+
Rhaty::Vector = repeat(ry, mpc.Hp),
58+
Rhatu::Vector = mpc.noR̂u ? empty(mpc.estim.x̂0) : repeat(mpc.estim.model.uop, mpc.Hp),
59+
ym::Union{Vector, Nothing} = nothing,
60+
= Dhat,
61+
R̂y = Rhaty,
62+
R̂u = Rhatu
5663
)
5764
validate_args(mpc, ry, d, D̂, R̂y, R̂u)
5865
initpred!(mpc, mpc.estim.model, d, ym, D̂, R̂y, R̂u)
@@ -73,19 +80,22 @@ Get additional info about `mpc` [`PredictiveController`](@ref) optimum for troub
7380
The function should be called after calling [`moveinput!`](@ref). It returns the dictionary
7481
`info` with the following fields:
7582
76-
- `:ΔU` : optimal manipulated input increments over ``H_c``, ``\mathbf{ΔU}``
77-
- `:ϵ` : optimal slack variable, ``ϵ``
83+
!!! info
84+
Fields in *`italic`* are non-Unicode alternatives.
85+
86+
- `:ΔU` or *`:DeltaU`* : optimal manipulated input increments over ``H_c``, ``\mathbf{ΔU}``
87+
- `:ϵ` or *`:epsilon`* : optimal slack variable, ``ϵ``
88+
- `:D̂` or *`:Dhat`* : predicted measured disturbances over ``H_p``, ``\mathbf{D̂}``
89+
- `:ŷ` or *`:yhat`* : current estimated output, ``\mathbf{ŷ}(k)``
90+
- `:Ŷ` or *`:Yhat`* : optimal predicted outputs over ``H_p``, ``\mathbf{Ŷ}``
91+
- `:Ŷs` or *`:Yhats`* : predicted stochastic output over ``H_p`` of [`InternalModel`](@ref), ``\mathbf{Ŷ_s}``
92+
- `:R̂y` or *`:Rhaty`* : predicted output setpoint over ``H_p``, ``\mathbf{R̂_y}``
93+
- `:R̂u` or *`:Rhatu`* : predicted manipulated input setpoint over ``H_p``, ``\mathbf{R̂_u}``
94+
- `:x̂end` or *`:xhatend`* : optimal terminal states, ``\mathbf{x̂}_{k-1}(k+H_p)``
7895
- `:J` : objective value optimum, ``J``
7996
- `:U` : optimal manipulated inputs over ``H_p``, ``\mathbf{U}``
8097
- `:u` : current optimal manipulated input, ``\mathbf{u}(k)``
8198
- `:d` : current measured disturbance, ``\mathbf{d}(k)``
82-
- `:D̂` : predicted measured disturbances over ``H_p``, ``\mathbf{D̂}``
83-
- `:ŷ` : current estimated output, ``\mathbf{ŷ}(k)``
84-
- `:Ŷ` : optimal predicted outputs over ``H_p``, ``\mathbf{Ŷ}``
85-
- `:x̂end`: optimal terminal states, ``\mathbf{x̂}_{k-1}(k+H_p)``
86-
- `:Ŷs` : predicted stochastic output over ``H_p`` of [`InternalModel`](@ref), ``\mathbf{Ŷ_s}``
87-
- `:R̂y` : predicted output setpoint over ``H_p``, ``\mathbf{R̂_y}``
88-
- `:R̂u` : predicted manipulated input setpoint over ``H_p``, ``\mathbf{R̂_u}``
8999
90100
For [`LinMPC`](@ref) and [`NonLinMPC`](@ref), the field `:sol` also contains the optimizer
91101
solution summary that can be printed. Lastly, the optimal economic cost `:JE` is also
@@ -129,6 +139,16 @@ function getinfo(mpc::PredictiveController{NT}) where NT<:Real
129139
info[:Ŷs] = Ŷs
130140
info[:R̂y] = mpc.R̂y0 + mpc.Yop
131141
info[:R̂u] = mpc.R̂u0 + mpc.Uop
142+
# --- non-Unicode fields ---
143+
info[:DeltaU] = info[:ΔU]
144+
info[:epsilon] = info[]
145+
info[:Dhat] = info[:D̂]
146+
info[:yhat] = info[:ŷ]
147+
info[:Yhat] = info[:Ŷ]
148+
info[:xhatend] = info[:x̂end]
149+
info[:Yhats] = info[:Ŷs]
150+
info[:Rhaty] = info[:R̂y]
151+
info[:Rhatu] = info[:R̂u]
132152
info = addinfo!(info, mpc)
133153
return info
134154
end
@@ -498,7 +518,7 @@ updatestate!(::PredictiveController, _ ) = throw(ArgumentError("missing measured
498518
"""
499519
setstate!(mpc::PredictiveController, x̂) -> mpc
500520
501-
Set the estimate at `mpc.estim.x̂`.
521+
Set `mpc.estim.x̂0` to `x̂ - estim.x̂op` from the argument `x̂`.
502522
"""
503523
setstate!(mpc::PredictiveController, x̂) = (setstate!(mpc.estim, x̂); return mpc)
504524

src/estimator/kalman.jl

Lines changed: 88 additions & 51 deletions
Original file line numberDiff line numberDiff line change
@@ -86,21 +86,24 @@ the rows of ``\mathbf{Ĉ, D̂_d}`` that correspond to measured outputs ``\mathb
8686
unmeasured ones, for ``\mathbf{Ĉ^u, D̂_d^u}``).
8787
8888
# Arguments
89+
!!! info
90+
Keyword arguments in *`italic`* are non-Unicode alternatives.
91+
8992
- `model::LinModel` : (deterministic) model for the estimations.
9093
- `i_ym=1:model.ny` : `model` output indices that are measured ``\mathbf{y^m}``, the rest
9194
are unmeasured ``\mathbf{y^u}``.
92-
- `σQ=fill(1/model.nx,model.nx)` : main diagonal of the process noise covariance
93-
``\mathbf{Q}`` of `model`, specified as a standard deviation vector.
94-
- `σR=fill(1,length(i_ym))` : main diagonal of the sensor noise covariance ``\mathbf{R}``
95-
of `model` measured outputs, specified as a standard deviation vector.
95+
- `σQ=fill(1/model.nx,model.nx)` or *`sigmaQ`* : main diagonal of the process noise
96+
covariance ``\mathbf{Q}`` of `model`, specified as a standard deviation vector.
97+
- `σR=fill(1,length(i_ym))` or *`sigmaR`* : main diagonal of the sensor noise covariance
98+
``\mathbf{R}`` of `model` measured outputs, specified as a standard deviation vector.
9699
- `nint_u=0`: integrator quantity for the stochastic model of the unmeasured disturbances at
97100
the manipulated inputs (vector), use `nint_u=0` for no integrator (see Extended Help).
98-
- `σQint_u=fill(1,sum(nint_u))`: same than `σQ` but for the unmeasured disturbances at
99-
manipulated inputs ``\mathbf{Q_{int_u}}`` (composed of integrators).
100101
- `nint_ym=default_nint(model,i_ym,nint_u)` : same than `nint_u` but for the unmeasured
101102
disturbances at the measured outputs, use `nint_ym=0` for no integrator (see Extended Help).
102-
- `σQint_ym=fill(1,sum(nint_ym))` : same than `σQ` for the unmeasured disturbances at
103-
measured outputs ``\mathbf{Q_{int_{ym}}}`` (composed of integrators).
103+
- `σQint_u=fill(1,sum(nint_u))` or *`sigmaQint_u`* : same than `σQ` but for the unmeasured
104+
disturbances at manipulated inputs ``\mathbf{Q_{int_u}}`` (composed of integrators).
105+
- `σQint_ym=fill(1,sum(nint_ym))` or *`sigmaQint_u`* : same than `σQ` for the unmeasured
106+
disturbances at measured outputs ``\mathbf{Q_{int_{ym}}}`` (composed of integrators).
104107
105108
# Examples
106109
```jldoctest
@@ -139,12 +142,16 @@ SteadyKalmanFilter estimator with a sample time Ts = 0.5 s, LinModel and:
139142
function SteadyKalmanFilter(
140143
model::SM;
141144
i_ym::IntRangeOrVector = 1:model.ny,
142-
σQ::Vector = fill(1/model.nx, model.nx),
143-
σR::Vector = fill(1, length(i_ym)),
145+
sigmaQ = fill(1/model.nx, model.nx),
146+
sigmaR = fill(1, length(i_ym)),
144147
nint_u ::IntVectorOrInt = 0,
145-
σQint_u::Vector = fill(1, max(sum(nint_u), 0)),
146148
nint_ym::IntVectorOrInt = default_nint(model, i_ym, nint_u),
147-
σQint_ym::Vector = fill(1, max(sum(nint_ym), 0))
149+
sigmaQint_u = fill(1, max(sum(nint_u), 0)),
150+
sigmaQint_ym = fill(1, max(sum(nint_ym), 0)),
151+
σQ = sigmaQ,
152+
σR = sigmaR,
153+
σQint_u = sigmaQint_u,
154+
σQint_ym = sigmaQint_ym,
148155
) where {NT<:Real, SM<:LinModel{NT}}
149156
# estimated covariances matrices (variance = σ²) :
150157
= Hermitian(diagm(NT[σQ; σQint_u; σQint_ym ].^2), :L)
@@ -266,13 +273,16 @@ its initial value with ``\mathbf{P̂}_{-1}(0) =
266273
\mathrm{diag}\{ \mathbf{P}(0), \mathbf{P_{int_{u}}}(0), \mathbf{P_{int_{ym}}}(0) \}``.
267274
268275
# Arguments
276+
!!! info
277+
Keyword arguments in *`italic`* are non-Unicode alternatives.
278+
269279
- `model::LinModel` : (deterministic) model for the estimations.
270-
- `σP_0=fill(1/model.nx,model.nx)` : main diagonal of the initial estimate covariance
271-
``\mathbf{P}(0)``, specified as a standard deviation vector.
272-
- `σPint_u_0=fill(1,sum(nint_u))` : same than `σP_0` but for the unmeasured disturbances at
273-
manipulated inputs ``\mathbf{P_{int_u}}(0)`` (composed of integrators).
274-
- `σPint_ym_0=fill(1,sum(nint_ym))` : same than `σP_0` but for the unmeasured disturbances
275-
at measured outputs ``\mathbf{P_{int_{ym}}}(0)`` (composed of integrators).
280+
- `σP_0=fill(1/model.nx,model.nx)` or *`sigmaP_0`* : main diagonal of the initial estimate
281+
covariance ``\mathbf{P}(0)``, specified as a standard deviation vector.
282+
- `σPint_u_0=fill(1,sum(nint_u))` or *`sigmaPint_u_0`* : same than `σP_0` but for the unmeasured
283+
disturbances at manipulated inputs ``\mathbf{P_{int_u}}(0)`` (composed of integrators).
284+
- `σPint_ym_0=fill(1,sum(nint_ym))` or *`sigmaPint_ym_0`* : same than `σP_0` but for the unmeasured
285+
disturbances at measured outputs ``\mathbf{P_{int_{ym}}}(0)`` (composed of integrators).
276286
- `<keyword arguments>` of [`SteadyKalmanFilter`](@ref) constructor.
277287
278288
# Examples
@@ -291,15 +301,22 @@ KalmanFilter estimator with a sample time Ts = 0.5 s, LinModel and:
291301
function KalmanFilter(
292302
model::SM;
293303
i_ym::IntRangeOrVector = 1:model.ny,
294-
σP_0::Vector = fill(1/model.nx, model.nx),
295-
σQ ::Vector = fill(1/model.nx, model.nx),
296-
σR ::Vector = fill(1, length(i_ym)),
297-
nint_u ::IntVectorOrInt = 0,
298-
σQint_u ::Vector = fill(1, max(sum(nint_u), 0)),
299-
σPint_u_0 ::Vector = fill(1, max(sum(nint_u), 0)),
300-
nint_ym ::IntVectorOrInt = default_nint(model, i_ym, nint_u),
301-
σQint_ym ::Vector = fill(1, max(sum(nint_ym), 0)),
302-
σPint_ym_0::Vector = fill(1, max(sum(nint_ym), 0)),
304+
sigmaP_0 = fill(1/model.nx, model.nx),
305+
sigmaQ = fill(1/model.nx, model.nx),
306+
sigmaR = fill(1, length(i_ym)),
307+
nint_u ::IntVectorOrInt = 0,
308+
nint_ym::IntVectorOrInt = default_nint(model, i_ym, nint_u),
309+
sigmaPint_u_0 = fill(1, max(sum(nint_u), 0)),
310+
sigmaQint_u = fill(1, max(sum(nint_u), 0)),
311+
sigmaPint_ym_0 = fill(1, max(sum(nint_ym), 0)),
312+
sigmaQint_ym = fill(1, max(sum(nint_ym), 0)),
313+
σP_0 = sigmaP_0,
314+
σQ = sigmaQ,
315+
σR = sigmaR,
316+
σPint_u_0 = sigmaPint_u_0,
317+
σQint_u = sigmaQint_u,
318+
σPint_ym_0 = sigmaPint_ym_0,
319+
σQint_ym = sigmaQint_ym,
303320
) where {NT<:Real, SM<:LinModel{NT}}
304321
# estimated covariances matrices (variance = σ²) :
305322
P̂_0 = Hermitian(diagm(NT[σP_0; σPint_u_0; σPint_ym_0].^2), :L)
@@ -440,10 +457,13 @@ represents the measured outputs of ``\mathbf{ĥ}`` function (and unmeasured one
440457
``\mathbf{ĥ^u}``).
441458
442459
# Arguments
460+
!!! info
461+
Keyword arguments in *`italic`* are non-Unicode alternatives.
462+
443463
- `model::SimModel` : (deterministic) model for the estimations.
444-
- `α=1e-3` : alpha parameter, spread of the state distribution ``(0 < α ≤ 1)``.
445-
- `β=2` : beta parameter, skewness and kurtosis of the states distribution ``(β ≥ 0)``.
446-
- `κ=0` : kappa parameter, another spread parameter ``(0 ≤ κ ≤ 3)``.
464+
- `α=1e-3` or *`alpha`* : alpha parameter, spread of the state distribution ``(0 < α ≤ 1)``.
465+
- `β=2` or *`beta`* : beta parameter, skewness and kurtosis of the states distribution ``(β ≥ 0)``.
466+
- `κ=0` or *`kappa`* : kappa parameter, another spread parameter ``(0 ≤ κ ≤ 3)``.
447467
- `<keyword arguments>` of [`SteadyKalmanFilter`](@ref) constructor.
448468
- `<keyword arguments>` of [`KalmanFilter`](@ref) constructor.
449469
@@ -470,18 +490,28 @@ UnscentedKalmanFilter estimator with a sample time Ts = 10.0 s, NonLinModel and:
470490
function UnscentedKalmanFilter(
471491
model::SM;
472492
i_ym::IntRangeOrVector = 1:model.ny,
473-
σP_0::Vector = fill(1/model.nx, model.nx),
474-
σQ ::Vector = fill(1/model.nx, model.nx),
475-
σR ::Vector = fill(1, length(i_ym)),
476-
nint_u ::IntVectorOrInt = 0,
477-
σQint_u ::Vector = fill(1, max(sum(nint_u), 0)),
478-
σPint_u_0 ::Vector = fill(1, max(sum(nint_u), 0)),
479-
nint_ym ::IntVectorOrInt = default_nint(model, i_ym, nint_u),
480-
σQint_ym ::Vector = fill(1, max(sum(nint_ym), 0)),
481-
σPint_ym_0::Vector = fill(1, max(sum(nint_ym), 0)),
482-
α::Real = 1e-3,
483-
β::Real = 2,
484-
κ::Real = 0
493+
sigmaP_0 = fill(1/model.nx, model.nx),
494+
sigmaQ = fill(1/model.nx, model.nx),
495+
sigmaR = fill(1, length(i_ym)),
496+
nint_u ::IntVectorOrInt = 0,
497+
nint_ym::IntVectorOrInt = default_nint(model, i_ym, nint_u),
498+
sigmaPint_u_0 = fill(1, max(sum(nint_u), 0)),
499+
sigmaQint_u = fill(1, max(sum(nint_u), 0)),
500+
sigmaPint_ym_0 = fill(1, max(sum(nint_ym), 0)),
501+
sigmaQint_ym = fill(1, max(sum(nint_ym), 0)),
502+
alpha::Real = 1e-3,
503+
beta ::Real = 2,
504+
kappa::Real = 0,
505+
σP_0 = sigmaP_0,
506+
σQ = sigmaQ,
507+
σR = sigmaR,
508+
σPint_u_0 = sigmaPint_u_0,
509+
σQint_u = sigmaQint_u,
510+
σPint_ym_0 = sigmaPint_ym_0,
511+
σQint_ym = sigmaQint_ym,
512+
α = alpha,
513+
β = beta,
514+
κ = kappa,
485515
) where {NT<:Real, SM<:SimModel{NT}}
486516
# estimated covariances matrices (variance = σ²) :
487517
P̂_0 = Hermitian(diagm(NT[σP_0; σPint_u_0; σPint_ym_0].^2), :L)
@@ -732,15 +762,22 @@ ExtendedKalmanFilter estimator with a sample time Ts = 5.0 s, NonLinModel and:
732762
function ExtendedKalmanFilter(
733763
model::SM;
734764
i_ym::IntRangeOrVector = 1:model.ny,
735-
σP_0::Vector = fill(1/model.nx, model.nx),
736-
σQ ::Vector = fill(1/model.nx, model.nx),
737-
σR ::Vector = fill(1, length(i_ym)),
738-
nint_u ::IntVectorOrInt = 0,
739-
σQint_u ::Vector = fill(1, max(sum(nint_u), 0)),
740-
σPint_u_0 ::Vector = fill(1, max(sum(nint_u), 0)),
741-
nint_ym ::IntVectorOrInt = default_nint(model, i_ym, nint_u),
742-
σQint_ym ::Vector = fill(1, max(sum(nint_ym), 0)),
743-
σPint_ym_0::Vector = fill(1, max(sum(nint_ym), 0)),
765+
sigmaP_0 = fill(1/model.nx, model.nx),
766+
sigmaQ = fill(1/model.nx, model.nx),
767+
sigmaR = fill(1, length(i_ym)),
768+
nint_u ::IntVectorOrInt = 0,
769+
nint_ym::IntVectorOrInt = default_nint(model, i_ym, nint_u),
770+
sigmaPint_u_0 = fill(1, max(sum(nint_u), 0)),
771+
sigmaQint_u = fill(1, max(sum(nint_u), 0)),
772+
sigmaPint_ym_0 = fill(1, max(sum(nint_ym), 0)),
773+
sigmaQint_ym = fill(1, max(sum(nint_ym), 0)),
774+
σP_0 = sigmaP_0,
775+
σQ = sigmaQ,
776+
σR = sigmaR,
777+
σPint_u_0 = sigmaPint_u_0,
778+
σQint_u = sigmaQint_u,
779+
σPint_ym_0 = sigmaPint_ym_0,
780+
σQint_ym = sigmaQint_ym,
744781
) where {NT<:Real, SM<:SimModel{NT}}
745782
# estimated covariances matrices (variance = σ²) :
746783
P̂_0 = Hermitian(diagm(NT[σP_0; σPint_u_0; σPint_ym_0].^2), :L)

0 commit comments

Comments
 (0)