diff --git a/src/estimator/mhe/transcription.jl b/src/estimator/mhe/transcription.jl index 36adf555d..0f54f984f 100644 --- a/src/estimator/mhe/transcription.jl +++ b/src/estimator/mhe/transcription.jl @@ -1383,7 +1383,7 @@ function con_nonlinprogeq_mhe!( x̂dnext .+= ŵd sdnext .= @. x̂dnext - x̂dnext_Z̃ end - Nk < He && (geq[nx̂*Nk+1:end] .= 0) + Nk < He && (geq[nx*Nk+1:end] .= 0) return geq end @@ -1462,7 +1462,7 @@ function con_nonlinprogeq_mhe!( sdnext .= @. x̂d_Z̃ - x̂dnext_Z̃ + 0.5*Ts*(k̇1 + k̇2) sdnext .+= ŵd end - Nk < He && (geq[nx̂*Nk+1:end] .= 0) + Nk < He && (geq[nx*Nk+1:end] .= 0) return geq end diff --git a/test/2_test_state_estim.jl b/test/2_test_state_estim.jl index 449380924..0b971315b 100644 --- a/test/2_test_state_estim.jl +++ b/test/2_test_state_estim.jl @@ -1145,6 +1145,7 @@ end using .SetupMPCtests, ControlSystemsBase, LinearAlgebra, ForwardDiff using JuMP, Ipopt, DifferentiationInterface, SparseMatrixColorings, SparseConnectivityTracer import ForwardDiff + linmodel = LinModel(sys,Ts,i_u=[1,2], i_d=[3]) linmodel = setop!(linmodel, uop=[10,50], yop=[50,30], dop=[5]) f(x,u,d,model) = model.A*x + model.Bu*u + model.Bd*d @@ -1237,7 +1238,13 @@ end end preparestate!(mhe3, [50, 30], [5]) @test mhe3([5]) ≈ [50, 30] atol=1e-3 - + initstate!(mhe3, [10, 50], [50, 30], [5]) + setstate!(mhe3, [0.0, 0.0, 0.0, 0.0, 0.0, 0.0]) + preparestate!(mhe3, [50, 30], [5]) + updatestate!(mhe3, [10, 50], [50, 30], [5]) + info = getinfo(mhe3) # test getinfo when Nk