@@ -48,13 +48,17 @@ struct MovingHorizonEstimator{
4848 NT<: Real ,
4949 SM<: SimModel ,
5050 JM<: JuMP.GenericModel ,
51+ GB<: AbstractADType ,
52+ JB<: AbstractADType ,
5153 CE<: StateEstimator ,
5254} <: StateEstimator{NT}
5355 model:: SM
5456 # note: `NT` and the number type `JNT` in `JuMP.GenericModel{JNT}` can be
5557 # different since solvers that support non-Float64 are scarce.
5658 optim:: JM
5759 con:: EstimatorConstraint{NT}
60+ gradient:: GB
61+ jacobian:: JB
5862 covestim:: CE
5963 Z̃:: Vector{NT}
6064 lastu0:: Vector{NT}
@@ -112,9 +116,16 @@ struct MovingHorizonEstimator{
112116 function MovingHorizonEstimator {NT} (
113117 model:: SM ,
114118 He, i_ym, nint_u, nint_ym, P̂_0, Q̂, R̂, Cwt,
115- optim:: JM , gradient, jacobian, covestim:: CE ;
119+ optim:: JM , gradient:: GB , jacobian:: JB , covestim:: CE ;
116120 direct= true
117- ) where {NT<: Real , SM<: SimModel{NT} , JM<: JuMP.GenericModel , CE<: StateEstimator{NT} }
121+ ) where {
122+ NT<: Real ,
123+ SM<: SimModel{NT} ,
124+ JM<: JuMP.GenericModel ,
125+ GB<: AbstractADType ,
126+ JB<: AbstractADType ,
127+ CE<: StateEstimator{NT}
128+ }
118129 nu, ny, nd = model. nu, model. ny, model. nd
119130 He < 1 && throw (ArgumentError (" Estimation horizon He should be ≥ 1" ))
120131 Cwt < 0 && throw (ArgumentError (" Cwt weight should be ≥ 0" ))
@@ -158,8 +169,10 @@ struct MovingHorizonEstimator{
158169 P̂arr_old = copy (P̂_0)
159170 Nk = [0 ]
160171 corrected = [false ]
161- estim = new {NT, SM, JM, CE} (
162- model, optim, con, covestim,
172+ estim = new {NT, SM, JM, GB, JB, CE} (
173+ model, optim, con,
174+ gradient, jacobian,
175+ covestim,
163176 Z̃, lastu0, x̂op, f̂op, x̂0,
164177 He, nϵ,
165178 i_ym, nx̂, nym, nyu, nxs,
@@ -173,7 +186,7 @@ struct MovingHorizonEstimator{
173186 direct, corrected,
174187 buffer
175188 )
176- init_optimization! (estim, model, optim, gradient, jacobian )
189+ init_optimization! (estim, model, optim)
177190 return estim
178191 end
179192end
@@ -261,9 +274,9 @@ transcription for now.
261274 nonlinear optimizer for solving (default to [`Ipopt`](https://github.com/jump-dev/Ipopt.jl),
262275 or [`OSQP`](https://osqp.org/docs/parsers/jump.html) if `model` is a [`LinModel`](@ref)).
263276- `gradient=AutoForwardDiff()` : an `AbstractADType` backend for the gradient of the objective
264- function if `model` is not a [`LinModel`](@ref), see [`DifferentiationInterface` doc](@extref DifferentiationInterface List).
277+ function when `model` is not a [`LinModel`](@ref), see [`DifferentiationInterface` doc](@extref DifferentiationInterface List).
265278- `jacobian=AutoForwardDiff()` : an `AbstractADType` backend for the Jacobian of the
266- constraints if `model` is not a [`LinModel`](@ref), see `gradient` above for the options.
279+ constraints when `model` is not a [`LinModel`](@ref), see `gradient` above for the options.
267280- `direct=true`: construct with a direct transmission from ``\m athbf{y^m}`` (a.k.a. current
268281 estimator, in opposition to the delayed/predictor form).
269282
@@ -1197,22 +1210,18 @@ end
11971210
11981211"""
11991212 init_optimization!(
1200- estim::MovingHorizonEstimator, model::SimModel , optim::JuMP.GenericModel, _ , _
1213+ estim::MovingHorizonEstimator, model::LinModel , optim::JuMP.GenericModel
12011214 )
12021215
12031216Init the quadratic optimization of [`MovingHorizonEstimator`](@ref).
12041217"""
12051218function init_optimization! (
1206- estim:: MovingHorizonEstimator ,
1207- :: LinModel ,
1208- optim:: JuMP.GenericModel ,
1209- :: AbstractADType ,
1210- :: AbstractADType
1219+ estim:: MovingHorizonEstimator , model:: LinModel , optim:: JuMP.GenericModel ,
12111220)
12121221 nZ̃ = length (estim. Z̃)
12131222 JuMP. num_variables (optim) == 0 || JuMP. empty! (optim)
12141223 JuMP. set_silent (optim)
1215- limit_solve_time (estim . optim, estim . model. Ts)
1224+ limit_solve_time (optim, model. Ts)
12161225 @variable (optim, Z̃var[1 : nZ̃])
12171226 A = estim. con. A[estim. con. i_b, :]
12181227 b = estim. con. b[estim. con. i_b]
@@ -1223,28 +1232,20 @@ end
12231232
12241233"""
12251234 init_optimization!(
1226- estim::MovingHorizonEstimator,
1227- model::SimModel,
1228- optim::JuMP.GenericModel,
1229- gradient::AbstractADType,
1230- jacobian::AbstractADType
1235+ estim::MovingHorizonEstimator, model::SimModel, optim::JuMP.GenericModel,
12311236 ) -> nothing
12321237
12331238Init the nonlinear optimization of [`MovingHorizonEstimator`](@ref).
12341239"""
12351240function init_optimization! (
1236- estim:: MovingHorizonEstimator ,
1237- model:: SimModel ,
1238- optim:: JuMP.GenericModel{JNT} ,
1239- gradient:: AbstractADType ,
1240- jacobian:: AbstractADType
1241+ estim:: MovingHorizonEstimator , model:: SimModel , optim:: JuMP.GenericModel{JNT}
12411242) where JNT<: Real
12421243 C, con = estim. C, estim. con
12431244 nZ̃ = length (estim. Z̃)
12441245 # --- variables and linear constraints ---
12451246 JuMP. num_variables (optim) == 0 || JuMP. empty! (optim)
12461247 JuMP. set_silent (optim)
1247- limit_solve_time (estim . optim, estim . model. Ts)
1248+ limit_solve_time (optim, model. Ts)
12481249 @variable (optim, Z̃var[1 : nZ̃])
12491250 A = estim. con. A[con. i_b, :]
12501251 b = estim. con. b[con. i_b]
@@ -1258,9 +1259,7 @@ function init_optimization!(
12581259 JuMP. set_attribute (optim, " nlp_scaling_max_gradient" , 10.0 / C)
12591260 end
12601261 end
1261- Jfunc, ∇Jfunc!, gfuncs, ∇gfuncs! = get_optim_functions (
1262- estim, optim, gradient, jacobian
1263- )
1262+ Jfunc, ∇Jfunc!, gfuncs, ∇gfuncs! = get_optim_functions (estim, optim)
12641263 @operator (optim, J, nZ̃, Jfunc, ∇Jfunc!)
12651264 @objective (optim, Min, J (Z̃var... ))
12661265 nV̂, nX̂ = estim. He* estim. nym, estim. He* estim. nx̂
@@ -1301,10 +1300,7 @@ end
13011300
13021301"""
13031302 get_optim_functions(
1304- estim::MovingHorizonEstimator,
1305- optim::JuMP.GenericModel,
1306- grad_backend::AbstractADType,
1307- jac_backend::AbstractADType
1303+ estim::MovingHorizonEstimator, optim::JuMP.GenericModel,
13081304 ) -> Jfunc, ∇Jfunc!, gfuncs, ∇gfuncs!
13091305
13101306Return the functions for the nonlinear optimization of [`MovingHorizonEstimator`](@ref).
@@ -1327,10 +1323,7 @@ This method is really intricate and I'm not proud of it. That's because of 3 ele
13271323Inspired from: [User-defined operators with vector outputs](@extref JuMP User-defined-operators-with-vector-outputs)
13281324"""
13291325function get_optim_functions (
1330- estim:: MovingHorizonEstimator ,
1331- optim:: JuMP.GenericModel{JNT} ,
1332- grad_backend:: AbstractADType ,
1333- jac_backend:: AbstractADType
1326+ estim:: MovingHorizonEstimator , :: JuMP.GenericModel{JNT} ,
13341327) where {JNT <: Real }
13351328 # ---------- common cache for Jfunc, gfuncs called with floats ------------------------
13361329 model, con = estim. model, estim. con
@@ -1363,13 +1356,13 @@ function get_optim_functions(
13631356 Cache (g),
13641357 Cache (x̄),
13651358 )
1366- ∇J_prep = prepare_gradient (Jfunc!, grad_backend , Z̃_∇J, ∇J_context... ; strict)
1359+ ∇J_prep = prepare_gradient (Jfunc!, estim . gradient , Z̃_∇J, ∇J_context... ; strict)
13671360 ∇J = Vector {JNT} (undef, nZ̃)
13681361 ∇Jfunc! = function (∇J:: AbstractVector{T} , Z̃arg:: Vararg{T, N} ) where {N, T<: Real }
13691362 # only the multivariate syntax of JuMP.@operator, univariate is impossible for MHE
13701363 # since Z̃ comprises the arrival state estimate AND the estimated process noise
13711364 Z̃_∇J .= Z̃arg
1372- gradient! (Jfunc!, ∇J, ∇J_prep, grad_backend , Z̃_∇J, ∇J_context... )
1365+ gradient! (Jfunc!, ∇J, ∇J_prep, estim . gradient , Z̃_∇J, ∇J_context... )
13731366 return ∇J
13741367 end
13751368
@@ -1397,17 +1390,17 @@ function get_optim_functions(
13971390 # temporarily enable all the inequality constraints for sparsity detection:
13981391 estim. con. i_g .= true
13991392 estim. Nk[] = He
1400- ∇g_prep = prepare_jacobian (gfunc!, g, jac_backend , Z̃_∇g, ∇g_context... ; strict)
1393+ ∇g_prep = prepare_jacobian (gfunc!, g, estim . jacobian , Z̃_∇g, ∇g_context... ; strict)
14011394 estim. con. i_g .= false
14021395 estim. Nk[] = 0
1403- ∇g = init_diffmat (JNT, jac_backend , ∇g_prep, nZ̃, ng)
1396+ ∇g = init_diffmat (JNT, estim . jacobian , ∇g_prep, nZ̃, ng)
14041397 ∇gfuncs! = Vector {Function} (undef, ng)
14051398 for i in eachindex (∇gfuncs!)
14061399 ∇gfuncs![i] = function (∇g_i, Z̃arg:: Vararg{T, N} ) where {N, T<: Real }
14071400 # only the multivariate syntax of JuMP.@operator, see above for the explanation
14081401 if isdifferent (Z̃arg, Z̃_∇g)
14091402 Z̃_∇g .= Z̃arg
1410- jacobian! (gfunc!, g, ∇g, ∇g_prep, jac_backend , Z̃_∇g, ∇g_context... )
1403+ jacobian! (gfunc!, g, ∇g, ∇g_prep, estim . jacobian , Z̃_∇g, ∇g_context... )
14111404 end
14121405 return ∇g_i .= @views ∇g[i, :]
14131406 end
0 commit comments