1- @doc raw """
2- Include all the data for the constraints of [`MovingHorizonEstimator`](@ref).
3-
4- The bounds on the estimated state at arrival ``\m athbf{x̂}_k(k-N_k+1)`` is separated from
5- the other state constraints ``\m athbf{x̂}_k(k-N_k+2), \m athbf{x̂}_k(k-N_k+3), ...`` since
6- the former is always a linear inequality constraint (it's a decision variable). The fields
7- `x̂min` and `x̂max` refer to the bounds at the arrival, and `X̂min` and `X̂max`, the others.
8- """
9- struct EstimatorConstraint{NT<: Real }
10- Ẽx̂ :: Matrix{NT}
11- Fx̂ :: Vector{NT}
12- Gx̂ :: Matrix{NT}
13- Jx̂ :: Matrix{NT}
14- x̂min :: Vector{NT}
15- x̂max :: Vector{NT}
16- X̂min :: Vector{NT}
17- X̂max :: Vector{NT}
18- Ŵmin :: Vector{NT}
19- Ŵmax :: Vector{NT}
20- V̂min :: Vector{NT}
21- V̂max :: Vector{NT}
22- A_x̂min :: Matrix{NT}
23- A_x̂max :: Matrix{NT}
24- A_X̂min :: Matrix{NT}
25- A_X̂max :: Matrix{NT}
26- A_Ŵmin :: Matrix{NT}
27- A_Ŵmax :: Matrix{NT}
28- A_V̂min :: Matrix{NT}
29- A_V̂max :: Matrix{NT}
30- A :: Matrix{NT}
31- b :: Vector{NT}
32- i_b :: BitVector
33- i_g :: BitVector
34- end
35-
36- struct MovingHorizonEstimator{
37- NT<: Real ,
38- SM<: SimModel ,
39- JM<: JuMP.GenericModel
40- } <: StateEstimator{NT}
41- model:: SM
42- # note: `NT` and the number type `JNT` in `JuMP.GenericModel{JNT}` can be
43- # different since solvers that support non-Float64 are scarce.
44- optim:: JM
45- con:: EstimatorConstraint{NT}
46- Z̃:: Vector{NT}
47- lastu0:: Vector{NT}
48- x̂:: Vector{NT}
49- He:: Int
50- i_ym:: Vector{Int}
51- nx̂ :: Int
52- nym:: Int
53- nyu:: Int
54- nxs:: Int
55- As :: Matrix{NT}
56- Cs_u:: Matrix{NT}
57- Cs_y:: Matrix{NT}
58- nint_u :: Vector{Int}
59- nint_ym:: Vector{Int}
60- Â :: Matrix{NT}
61- B̂u :: Matrix{NT}
62- Ĉ :: Matrix{NT}
63- B̂d :: Matrix{NT}
64- D̂d :: Matrix{NT}
65- Ẽ :: Matrix{NT}
66- F :: Vector{NT}
67- G :: Matrix{NT}
68- J :: Matrix{NT}
69- ẽx̄:: Matrix{NT}
70- fx̄:: Vector{NT}
71- H̃:: Hermitian{NT, Matrix{NT}}
72- q̃:: Vector{NT}
73- p:: Vector{NT}
74- P̂0:: Hermitian{NT, Matrix{NT}}
75- Q̂:: Hermitian{NT, Matrix{NT}}
76- R̂:: Hermitian{NT, Matrix{NT}}
77- invP̄:: Hermitian{NT, Matrix{NT}}
78- invQ̂_He:: Hermitian{NT, Matrix{NT}}
79- invR̂_He:: Hermitian{NT, Matrix{NT}}
80- M̂:: Matrix{NT}
81- X̂ :: Union{Vector{NT}, Missing}
82- Ym:: Union{Vector{NT}, Missing}
83- U :: Union{Vector{NT}, Missing}
84- D :: Union{Vector{NT}, Missing}
85- Ŵ :: Union{Vector{NT}, Missing}
86- x̂arr_old:: Vector{NT}
87- P̂arr_old:: Hermitian{NT, Matrix{NT}}
88- Nk:: Vector{Int}
89- function MovingHorizonEstimator {NT, SM, JM} (
90- model:: SM , He, i_ym, nint_u, nint_ym, P̂0, Q̂, R̂, optim:: JM
91- ) where {NT<: Real , SM<: SimModel{NT} , JM<: JuMP.GenericModel }
92- nu, nd = model. nu, model. nd
93- He < 1 && throw (ArgumentError (" Estimation horizon He should be ≥ 1"
94- nym, nyu = validate_ym (model, i_ym)
95- As, Cs_u, Cs_y, nint_u, nint_ym = init_estimstoch (model, i_ym, nint_u, nint_ym)
96- nxs = size (As, 1 )
97- nx̂ = model. nx + nxs
98- nŵ = nx̂
99- Â, B̂u, Ĉ, B̂d, D̂d = augment_model (model, As, Cs_u, Cs_y)
100- validate_kfcov (nym, nx̂, Q̂, R̂, P̂0)
101- lastu0 = zeros (NT, model. nu)
102- x̂ = [zeros (NT, model. nx); zeros (NT, nxs)]
103- P̂0 = Hermitian (P̂0, :L )
104- Q̂, R̂ = Hermitian (Q̂, :L ), Hermitian (R̂, :L )
105- invP̄ = Hermitian (inv (P̂0), :L )
106- invQ̂_He = Hermitian (repeatdiag (inv (Q̂), He), :L )
107- invR̂_He = Hermitian (repeatdiag (inv (R̂), He), :L )
108- M̂ = zeros (NT, nx̂, nym)
109- E, F, G, J, ex̄, fx̄, Ex̂, Fx̂, Gx̂, Jx̂ = init_predmat_mhe (
110- model, He, i_ym, Â, B̂u, Ĉ, B̂d, D̂d
111- )
112- con, Ẽ, ẽx̄ = init_defaultcon_mhe (model, He, nx̂, nym, E, ex̄, Ex̂, Fx̂, Gx̂, Jx̂)
113- nZ̃ = nx̂ + nŵ* He
114- # dummy values, updated before optimization:
115- H̃, q̃, p = Hermitian (zeros (NT, nZ̃, nZ̃), :L ), zeros (NT, nZ̃), zeros (NT, 1 )
116- Z̃ = zeros (NT, nZ̃)
117- X̂, Ym = zeros (NT, nx̂* He), zeros (NT, nym* He)
118- U, D, Ŵ = zeros (NT, nu* He), zeros (NT, nd* He), zeros (NT, nx̂* He)
119- x̂arr_old = zeros (NT, nx̂)
120- P̂arr_old = copy (P̂0)
121- Nk = [0 ]
122- estim = new {NT, SM, JM} (
123- model, optim, con,
124- Z̃, lastu0, x̂,
125- He,
126- i_ym, nx̂, nym, nyu, nxs,
127- As, Cs_u, Cs_y, nint_u, nint_ym,
128- Â, B̂u, Ĉ, B̂d, D̂d,
129- Ẽ, F, G, J, ẽx̄, fx̄,
130- H̃, q̃, p,
131- P̂0, Q̂, R̂, invP̄, invQ̂_He, invR̂_He,
132- M̂,
133- X̂, Ym, U, D, Ŵ,
134- x̂arr_old, P̂arr_old, Nk
135- )
136- init_optimization! (estim, model, optim)
137- return estim
138- end
139- end
1+ include (" mhe/construct.jl"
2+ include (" mhe/execute.jl"
1403
1414function Base. show (io:: IO , estim:: MovingHorizonEstimator )
1425 nu, nd = estim. model. nu, estim. model. nd
@@ -148,7 +11,7 @@ function Base.show(io::IO, estim::MovingHorizonEstimator)
14811 print_estim_dim (io, estim, n)
14912end
15013
151- " Print the overall dimensions of the state estimator `estim` with left padding `n`."
14+ " Print the overall dimensions of the MHE `estim` with left padding `n`."
15215function print_estim_dim (io:: IO , estim:: MovingHorizonEstimator , n)
15316 nu, nd = estim. model. nu, estim. model. nd
15417 nx̂, nym, nyu = estim. nx̂, estim. nym, estim. nyu
@@ -159,7 +22,4 @@ function print_estim_dim(io::IO, estim::MovingHorizonEstimator, n)
15922 println (io, " $(lpad (nym, n)) measured outputs ym ($(sum (estim. nint_ym)) integrating states)"
16023 println (io, " $(lpad (nyu, n)) unmeasured outputs yu"
16124 print (io, " $(lpad (nd, n)) measured disturbances d"
162- end
163-
164- include (" mhe/construct.jl"
165- include (" mhe/execute.jl"
25+ end
0 commit comments