@@ -461,80 +461,95 @@ end
461461
462462
463463" Update the augmented model matrices of `estim` by default."
464- function setmodel_estimator! (estim:: MovingHorizonEstimator , model:: LinModel )
465- As, Cs_u, Cs_y = estim. As, estim. Cs_u, estim. Cs_y
466- Â, B̂u, Ĉ, B̂d, D̂d = augment_model (model, As, Cs_u, Cs_y, verify_obsv= false )
464+ function setmodel_estimator! (
465+ estim:: MovingHorizonEstimator , model:: LinModel , uop_old, yop_old, dop_old
466+ )
467+ con = estim. con
468+ nx̂, nym, nu, nd, He = estim. nx̂, estim. nym, model. nu, model. nd, estim. He
469+ nϵ = isinf (estim. C) ? 0 : 1
470+ As, Cs_u, Cs_y = estim. As, estim. Cs_u, estim. Cs_y
471+ Â, B̂u, Ĉ, B̂d, D̂d, x̂op, f̂op = augment_model (model, As, Cs_u, Cs_y, verify_obsv= false )
472+ # --- update augmented state-space matrices ---
467473 estim. Â .= Â
468474 estim. B̂u .= B̂u
469475 estim. Ĉ .= Ĉ
470476 estim. B̂d .= B̂d
471477 estim. D̂d .= D̂d
472- # TODO : re-construct the MHE prediction matrices here:
473- error (" setmodel! for MovingHorizonEstimator is not implemented yet."
474-
475-
476-
477-
478-
479-
480-
481-
482-
483-
484-
485- # LINMPC:
478+ # --- update state estimate and its operating points ---
479+ x̂op_old = copy (estim. x̂op)
480+ X̂op_old = copy (estim. X̂op)
481+ estim. x̂0 .+ = estim. x̂op # convert x̂0 to x̂ with the old operating point
482+ estim. x̂op .= x̂op
483+ estim. f̂op .= f̂op
484+ estim. x̂0 .- = estim. x̂op # convert x̂ to x̂0 with the new operating point
486485 # --- predictions matrices ---
487- E, G, J, K, V, B, ex̂, gx̂, jx̂, kx̂, vx̂, bx̂ = init_predmat (estim, model, Hp, Hc)
488- A_Ymin, A_Ymax, Ẽ = relaxŶ ( model, mpc . C, con . C_ymin, con . C_ymax, E)
489- A_x̂min, A_x̂max, ẽx̂ = relaxterminal (model, mpc . C, con . c_x̂min, con . c_x̂max, ex̂ )
490- mpc . Ẽ . = Ẽ
491- mpc . G . = G
492- mpc . J .= J
493- mpc . K .= K
494- mpc . V .= V
495- mpc . B .= B
486+ E, G, J, B, _ , Ex̂, Gx̂, Jx̂, Bx̂ = init_predmat_mhe (
487+ model, He, estim . i_ym, Â, B̂u, Ĉ, B̂d, D̂d, x̂op, f̂op
488+ )
489+ A_X̂min, A_X̂max, Ẽx̂ = relaxX̂ (model, estim . C, con . C_x̂min, con . C_x̂max, Ex̂)
490+ A_V̂min, A_V̂max, Ẽ = relaxV̂ (model, estim . C, con . C_v̂min, con . C_v̂max, E)
491+ estim . Ẽ .= Ẽ
492+ estim . G .= G
493+ estim . J .= J
494+ estim . B .= B
496495 # --- linear inequality constraints ---
497- con. ẽx̂ .= ẽx̂
498- con. gx̂ .= gx̂
499- con. jx̂ .= jx̂
500- con. kx̂ .= kx̂
501- con. vx̂ .= vx̂
502- con. bx̂ .= bx̂
503- con. U0min .+ = mpc. Uop # convert U0 to U with the old operating point
504- con. U0max .+ = mpc. Uop # convert U0 to U with the old operating point
505- con. Y0min .+ = mpc. Yop # convert Y0 to Y with the old operating point
506- con. Y0max .+ = mpc. Yop # convert Y0 to Y with the old operating point
507- con. x̂0min .+ = x̂op_old # convert x̂0 to x̂ with the old operating point
508- con. x̂0max .+ = x̂op_old # convert x̂0 to x̂ with the old operating point
509- # --- operating points ---
510- for i in 0 : Hp- 1
511- mpc. Uop[(1 + nu* i): (nu+ nu* i)] .= model. uop
512- mpc. Yop[(1 + ny* i): (ny+ ny* i)] .= model. yop
513- mpc. Dop[(1 + nd* i): (nd+ nd* i)] .= model. dop
496+ con. Ẽx̂ .= Ẽx̂
497+ con. Gx̂ .= Gx̂
498+ con. Jx̂ .= Jx̂
499+ con. Bx̂ .= Bx̂
500+ # convert x̃0 to x̃ with the old operating point:
501+ con. x̃0min[end - nx̂+ 1 : end ] .- = x̂op_old
502+ con. x̃0max[end - nx̂+ 1 : end ] .- = x̂op_old
503+ # convert X̂0 to X̂ with the old operating point:
504+ con. X̂0min .- = X̂op_old
505+ con. X̂0max .- = X̂op_old
506+ for i in 0 : He- 1
507+ estim. X̂op[(1 + nx̂* i): (nx̂+ nx̂* i)] .= estim. x̂op
514508 end
515- con. U0min .- = mpc. Uop # convert U0 to U with the new operating point
516- con. U0max .- = mpc. Uop # convert U0 to U with the new operating point
517- con. Y0min .- = mpc. Yop # convert Y0 to Y with the new operating point
518- con. Y0max .- = mpc. Yop # convert Y0 to Y with the new operating point
519- con. x̂0min .- = estim. x̂op # convert x̂0 to x̂ with the new operating point
520- con. x̂0max .- = estim. x̂op # convert x̂0 to x̂ with the new operating point
521- con. A_Ymin .= A_Ymin
522- con. A_Ymax .= A_Ymax
523- con. A_x̂min .= A_x̂min
524- con. A_x̂max .= A_x̂max
525- nUandΔŨ = length (con. U0min) + length (con. U0max) + length (con. ΔŨmin) + length (con. ΔŨmax)
526- con. A[nUandΔŨ+ 1 : end , :] = [con. A_Ymin; con. A_Ymax; con. A_x̂min; con. A_x̂max]
509+ # convert x̃ to x̃0 with the new operating point:
510+ con. x̃0min[end - nx̂+ 1 : end ] .+ = estim. x̂op
511+ con. x̃0max[end - nx̂+ 1 : end ] .+ = estim. x̂op
512+ # convert X̂ to X̂0 with the new operating point:
513+ con. X̂0min .+ = estim. X̂op
514+ con. X̂0max .+ = estim. X̂op
515+ con. A
516+ con. A_X̂min .= A_X̂min
517+ con. A_X̂max .= A_X̂max
518+ con. A_V̂min .= A_V̂min
519+ con. A_V̂max .= A_V̂max
520+ nx̃ = length (con. x̃0min) + length (con. x̃0max)
521+ nX̂ = length (con. X̂0min) + length (con. X̂0max)
522+ con. A[nx̃+ 1 : nx̃+ nX̂,:] .= [con. A_X̂min; con. A_X̂max]
523+ nŴ = length (con. Ŵmin) + length (con. Ŵmax)
524+ con. A[nx̃+ nX̂+ nŴ+ 1 : end ,:] .= [con. A_V̂min; con. A_V̂max]
527525 A = con. A[con. i_b, :]
528526 b = con. b[con. i_b]
529- ΔŨvar:: Vector{JuMP.VariableRef} = optim[:ΔŨvar ]
530- JuMP. delete (optim, optim[:linconstraint ])
531- JuMP. unregister (optim, :linconstraint )
532- @constraint (optim, linconstraint, A* ΔŨvar .≤ b)
533- # --- quadratic programming Hessian matrix ---
534- H̃ = init_quadprog (model, mpc. Ẽ, mpc. S̃, mpc. M_Hp, mpc. Ñ_Hc, mpc. L_Hp)
535- mpc. H̃ .= H̃
536- set_objective_hessian! (mpc, ΔŨvar)
537-
538-
527+ Z̃var:: Vector{JuMP.VariableRef} = estim. optim[:Z̃var ]
528+ JuMP. delete (estim. optim, estim. optim[:linconstraint ])
529+ JuMP. unregister (estim. optim, :linconstraint )
530+ @constraint (estim. optim, linconstraint, A* Z̃var .≤ b)
531+ # --- data windows ---
532+ for i in 1 : He
533+ # convert x̂0 to x̂ with the old operating point:
534+ estim. X̂0[(1 + nx̂* (i- 1 )): (nx̂* i)] .- = x̂op_old
535+ # convert ym0 to ym with the old operating point:
536+ estim. Y0m[(1 + nym* (i- 1 )): (nym* i)] .- = @views yop_old[estim. i_ym]
537+ # convert u0 to u with the old operating point:
538+ estim. U0[(1 + nu* (i- 1 )): (nu* i)] .- = uop_old
539+ # convert d0 to d with the old operating point:
540+ estim. D0[(1 + nd* (i- 1 )): (nd* i)] .- = dop_old
541+ # convert x̂ to x̂0 with the new operating point:
542+ estim. X̂0[(1 + nx̂* (i- 1 )): (nx̂* i)] .+ = x̂op
543+ # convert ym to y0m with the new operating point:
544+ estim. Y0m[(1 + nym* (i- 1 )): (nym* i)] .+ = @views yop_old[estim. i_ym]
545+ # convert u to u0 with the new operating point:
546+ estim. U0[(1 + nu* (i- 1 )): (nu* i)] .+ = uop_old
547+ # convert d to d0 with the new operating point:
548+ estim. D0[(1 + nd* (i- 1 )): (nd* i)] .+ = dop_old
549+ end
550+ estim. Z̃[nϵ+ 1 : nϵ+ nx̂] .- = x̂op_old
551+ estim. Z̃[nϵ+ 1 : nϵ+ nx̂] .+ = x̂op
552+ estim. x̂0arr_old .- = x̂op_old
553+ estim. x̂0arr_old .+ = x̂op
539554 return nothing
540555end
0 commit comments