diff --git a/src/estimator/mhe/execute.jl b/src/estimator/mhe/execute.jl index c5f8a515d..d8625e8c4 100644 --- a/src/estimator/mhe/execute.jl +++ b/src/estimator/mhe/execute.jl @@ -30,8 +30,8 @@ function init_estimate_cov!(estim::MovingHorizonEstimator, y0m, d0, u0) nd > 0 && (estim.D0[1:nd] .= d0) # add d0(-1) to the data window estim.lastu0 .= u0 # estim.cov.P̂_0 is P̂(-1|-1) if estim.direct==false, else P̂(-1|0) - invert_cov!(estim, estim.cov.P̂_0) estim.P̂arr_old .= estim.cov.P̂_0 + invert_cov!(estim, estim.covestim) estim.x̂0arr_old .= 0 return nothing end @@ -709,7 +709,7 @@ function correct_cov!(estim::MovingHorizonEstimator) correct_estimate!(estim.covestim, y0marr, d0arr) all(isfinite, estim.covestim.cov.P̂) || error("Arrival covariance P̄ is not finite") estim.P̂arr_old .= estim.covestim.cov.P̂ - invert_cov!(estim, estim.P̂arr_old) + invert_cov!(estim, estim.covestim) catch err if err isa PosDefException @error("Arrival covariance P̄ is not positive definite: keeping the old one") @@ -736,7 +736,7 @@ function update_cov!(estim::MovingHorizonEstimator) update_estimate!(estim.covestim, y0marr, d0arr, u0arr) all(isfinite, estim.covestim.cov.P̂) || error("Arrival covariance P̄ is not finite") estim.P̂arr_old .= estim.covestim.cov.P̂ - invert_cov!(estim, estim.P̂arr_old) + invert_cov!(estim, estim.covestim) catch err if err isa PosDefException @error("Arrival covariance P̄ is not positive definite: keeping the old one") @@ -749,8 +749,9 @@ function update_cov!(estim::MovingHorizonEstimator) return nothing end -"Invert the covariance estimate at arrival `P̄`." -function invert_cov!(estim::MovingHorizonEstimator, P̄) +"Invert the covariance estimate at arrival `P̄` and store it in `estim.cov.invP̄`." +function invert_cov!(estim::MovingHorizonEstimator, covestim::StateEstimator) + P̄ = estim.P̂arr_old estim.cov.invP̄ .= P̄ try inv!(estim.cov.invP̄) @@ -763,6 +764,9 @@ function invert_cov!(estim::MovingHorizonEstimator, P̄) end return nothing end +"Do nothing if `covestim` is a [`SteadyKalmanFilter`]." +invert_cov!(::MovingHorizonEstimator, ::SteadyKalmanFilter) = nothing + """ obj_nonlinprog(estim::MovingHorizonEstimator, ::LinModel, _ , _ , _ , Z̃) diff --git a/test/2_test_state_estim.jl b/test/2_test_state_estim.jl index 597e4ff79..8b37902db 100644 --- a/test/2_test_state_estim.jl +++ b/test/2_test_state_estim.jl @@ -1011,6 +1011,17 @@ end info = getinfo(mhe3) @test info[:x̂] ≈ x̂ atol=1e-9 @test info[:Ŷ][end-1:end] ≈ [50, 30] atol=1e-9 + covestim = SteadyKalmanFilter(linmodel) + mhe3b = MovingHorizonEstimator(linmodel; He=1, σP_0=nothing, covestim) + preparestate!(mhe3b, [50, 30], [5]) + x̂ = updatestate!(mhe3b, [10, 50], [50, 30], [5]) + @test x̂ ≈ zeros(6) atol=1e-9 + @test mhe3b.x̂0 ≈ zeros(6) atol=1e-9 + @test mhe3b.cov.invP̄ ≈ inv(covestim.cov.P̂) + preparestate!(mhe3b, [50, 30], [5]) + info = getinfo(mhe3b) + @test info[:x̂] ≈ x̂ atol=1e-9 + @test info[:Ŷ][end-1:end] ≈ [50, 30] atol=1e-9 linmodel3 = LinModel{Float32}(0.5*ones(1,1), ones(1,1), ones(1,1), zeros(1,0), zeros(1,0), 1.0) mhe3 = MovingHorizonEstimator(linmodel3, He=1) @@ -1181,7 +1192,7 @@ end @test mhe.cov.invP̄ ≈ invP̄_copy @test_logs( (:error, "Arrival covariance P̄ is not invertible: keeping the old one"), - ModelPredictiveControl.invert_cov!(mhe, Hermitian(zeros(mhe.nx̂, mhe.nx̂),:L)) + ModelPredictiveControl.invert_cov!(mhe, mhe.covestim) ) mhe.P̂arr_old[1, 1] = Inf # Inf to trigger fallback P̂arr_old_copy = deepcopy(mhe.P̂arr_old)