diff --git a/src/estimator/mhe/execute.jl b/src/estimator/mhe/execute.jl index 8a23983e..984e2372 100644 --- a/src/estimator/mhe/execute.jl +++ b/src/estimator/mhe/execute.jl @@ -460,7 +460,7 @@ function con_nonlinprog!(g, estim::MovingHorizonEstimator, ::SimModel, X̂0, V̂ end -"Update the augmented model matrices of `estim` by default." +"Update the augmented model, prediction matrices, constrains and data windows for MHE." function setmodel_estimator!( estim::MovingHorizonEstimator, model::LinModel, uop_old, yop_old, dop_old ) @@ -477,7 +477,6 @@ function setmodel_estimator!( estim.D̂d .= D̂d # --- update state estimate and its operating points --- x̂op_old = copy(estim.x̂op) - X̂op_old = copy(estim.X̂op) estim.x̂0 .+= estim.x̂op # convert x̂0 to x̂ with the old operating point estim.x̂op .= x̂op estim.f̂op .= f̂op @@ -498,21 +497,20 @@ function setmodel_estimator!( con.Jx̂ .= Jx̂ con.Bx̂ .= Bx̂ # convert x̃0 to x̃ with the old operating point: - con.x̃0min[end-nx̂+1:end] .-= x̂op_old - con.x̃0max[end-nx̂+1:end] .-= x̂op_old + con.x̃0min[end-nx̂+1:end] .+= x̂op_old + con.x̃0max[end-nx̂+1:end] .+= x̂op_old # convert X̂0 to X̂ with the old operating point: - con.X̂0min .-= X̂op_old - con.X̂0max .-= X̂op_old + con.X̂0min .+= estim.X̂op + con.X̂0max .+= estim.X̂op for i in 0:He-1 estim.X̂op[(1+nx̂*i):(nx̂+nx̂*i)] .= estim.x̂op end # convert x̃ to x̃0 with the new operating point: - con.x̃0min[end-nx̂+1:end] .+= estim.x̂op - con.x̃0max[end-nx̂+1:end] .+= estim.x̂op + con.x̃0min[end-nx̂+1:end] .-= estim.x̂op + con.x̃0max[end-nx̂+1:end] .-= estim.x̂op # convert X̂ to X̂0 with the new operating point: - con.X̂0min .+= estim.X̂op - con.X̂0max .+= estim.X̂op - con.A + con.X̂0min .-= estim.X̂op + con.X̂0max .-= estim.X̂op con.A_X̂min .= A_X̂min con.A_X̂max .= A_X̂max con.A_V̂min .= A_V̂min @@ -531,25 +529,25 @@ function setmodel_estimator!( # --- data windows --- for i in 1:He # convert x̂0 to x̂ with the old operating point: - estim.X̂0[(1+nx̂*(i-1)):(nx̂*i)] .-= x̂op_old + estim.X̂0[(1+nx̂*(i-1)):(nx̂*i)] .+= x̂op_old # convert ym0 to ym with the old operating point: - estim.Y0m[(1+nym*(i-1)):(nym*i)] .-= @views yop_old[estim.i_ym] + estim.Y0m[(1+nym*(i-1)):(nym*i)] .+= @views yop_old[estim.i_ym] # convert u0 to u with the old operating point: - estim.U0[(1+nu*(i-1)):(nu*i)] .-= uop_old + estim.U0[(1+nu*(i-1)):(nu*i)] .+= uop_old # convert d0 to d with the old operating point: - estim.D0[(1+nd*(i-1)):(nd*i)] .-= dop_old + estim.D0[(1+nd*(i-1)):(nd*i)] .+= dop_old # convert x̂ to x̂0 with the new operating point: - estim.X̂0[(1+nx̂*(i-1)):(nx̂*i)] .+= x̂op + estim.X̂0[(1+nx̂*(i-1)):(nx̂*i)] .-= x̂op # convert ym to y0m with the new operating point: - estim.Y0m[(1+nym*(i-1)):(nym*i)] .+= @views yop_old[estim.i_ym] + estim.Y0m[(1+nym*(i-1)):(nym*i)] .-= @views model.yop[estim.i_ym] # convert u to u0 with the new operating point: - estim.U0[(1+nu*(i-1)):(nu*i)] .+= uop_old + estim.U0[(1+nu*(i-1)):(nu*i)] .-= model.uop # convert d to d0 with the new operating point: - estim.D0[(1+nd*(i-1)):(nd*i)] .+= dop_old + estim.D0[(1+nd*(i-1)):(nd*i)] .-= model.dop end - estim.Z̃[nϵ+1:nϵ+nx̂] .-= x̂op_old - estim.Z̃[nϵ+1:nϵ+nx̂] .+= x̂op - estim.x̂0arr_old .-= x̂op_old - estim.x̂0arr_old .+= x̂op + estim.Z̃[nϵ+1:nϵ+nx̂] .+= x̂op_old + estim.x̂0arr_old .+= x̂op_old + estim.Z̃[nϵ+1:nϵ+nx̂] .-= x̂op + estim.x̂0arr_old .-= x̂op return nothing end \ No newline at end of file diff --git a/test/test_state_estim.jl b/test/test_state_estim.jl index dbc5c5f2..3e6f272d 100644 --- a/test/test_state_estim.jl +++ b/test/test_state_estim.jl @@ -922,4 +922,35 @@ end x̂ = updatestate!(mhe2, [10, 50], [50, 30]) info = getinfo(mhe2) @test info[:V̂] ≈ [-1,-1] atol=5e-2 +end + +@testset "MovingHorizonEstimator set model" begin + linmodel = LinModel(ss(0.5, 0.3, 1.0, 0, 10.0)) + linmodel = setop!(linmodel, uop=[2.0], yop=[50.0], xop=[3.0], fop=[3.0]) + mhe = MovingHorizonEstimator(linmodel, He=1, nint_ym=0) + setconstraint!(mhe, x̂min=[-1000], x̂max=[1000]) + @test mhe. ≈ [0.5] + @test evaloutput(mhe) ≈ [50.0] + x̂ = updatestate!(mhe, [2.0], [50.0]) + @test x̂ ≈ [3.0] + newlinmodel = LinModel(ss(0.2, 0.3, 1.0, 0, 10.0)) + newlinmodel = setop!(newlinmodel, uop=[3.0], yop=[55.0], xop=[3.0], fop=[3.0]) + setmodel!(mhe, newlinmodel) + @test mhe. ≈ [0.2] + @test evaloutput(mhe) ≈ [55.0] + @test mhe.lastu0 ≈ [2.0 - 3.0] + @test mhe.U0 ≈ [2.0 - 3.0] + @test mhe.Y0m ≈ [50.0 - 55.0] + x̂ = updatestate!(mhe, [3.0], [55.0]) + @test x̂ ≈ [3.0] + newlinmodel = setop!(newlinmodel, uop=[3.0], yop=[55.0], xop=[8.0], fop=[8.0]) + setmodel!(mhe, newlinmodel) + @test mhe.x̂0 ≈ [3.0 - 8.0] + @test mhe.Z̃[1] ≈ 3.0 - 8.0 + @test mhe.X̂0 ≈ [3.0 - 8.0] + @test mhe.x̂0arr_old ≈ [3.0 - 8.0] + @test mhe.con.X̂0min ≈ [-1000 - 8.0] + @test mhe.con.X̂0max ≈ [+1000 - 8.0] + @test mhe.con.x̃0min ≈ [-1000 - 8.0] + @test mhe.con.x̃0max ≈ [+1000 - 8.0] end \ No newline at end of file