diff --git a/benchmark/2_bench_state_estim.jl b/benchmark/2_bench_state_estim.jl index 982aeeaa2..c53c0e444 100644 --- a/benchmark/2_bench_state_estim.jl +++ b/benchmark/2_bench_state_estim.jl @@ -146,6 +146,7 @@ UNIT_ESTIM["ExtendedKalmanFilter"]["evaloutput"]["NonLinModel"] = mhe_lin_curr = MovingHorizonEstimator(linmodel, He=10, direct=true) mhe_lin_pred = MovingHorizonEstimator(linmodel, He=10, direct=false) +mhe_lin_skf = MovingHorizonEstimator(linmodel, He=10, covestim=SteadyKalmanFilter(linmodel)) mhe_nonlin_curr = MovingHorizonEstimator(nonlinmodel, He=10, direct=true) mhe_nonlin_pred = MovingHorizonEstimator(nonlinmodel, He=10, direct=false) @@ -161,6 +162,12 @@ UNIT_ESTIM["MovingHorizonEstimator"]["updatestate!"]["LinModel"]["Current form"] setup=preparestate!($mhe_lin_curr, $y, $d), samples=samples, evals=evals, seconds=seconds, ) +UNIT_ESTIM["MovingHorizonEstimator"]["updatestate!"]["LinModel"]["Constant arr. cov."] = + @benchmarkable( + updatestate!($mhe_lin_skf, $u, $y, $d), + setup=preparestate!($mhe_lin_skf, $y, $d), + samples=samples, evals=evals, seconds=seconds, + ) UNIT_ESTIM["MovingHorizonEstimator"]["preparestate!"]["LinModel"]["Prediction form"] = @benchmarkable( preparestate!($mhe_lin_pred, $y, $d), diff --git a/check_syntax.jl b/check_syntax.jl new file mode 100644 index 000000000..fcb2c74ce --- /dev/null +++ b/check_syntax.jl @@ -0,0 +1,2 @@ +import JuliaFormatter +include("src/estimator/mhe/execute.jl") diff --git a/src/estimator/mhe/execute.jl b/src/estimator/mhe/execute.jl index c5f8a515d..670fdb953 100644 --- a/src/estimator/mhe/execute.jl +++ b/src/estimator/mhe/execute.jl @@ -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) + update_arrival_cov!(estim) 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) + update_arrival_cov!(estim) catch err if err isa PosDefException @error("Arrival covariance P̄ is not positive definite: keeping the old one") @@ -764,6 +764,19 @@ function invert_cov!(estim::MovingHorizonEstimator, P̄) return nothing end +"Update the arrival covariance matrix at the next time step based on the covariance estimator type." +function update_arrival_cov!(estim::MovingHorizonEstimator) + _update_arrival_cov!(estim, estim.covestim) +end + +function _update_arrival_cov!(estim::MovingHorizonEstimator, ::StateEstimator) + invert_cov!(estim, estim.P̂arr_old) +end + +function _update_arrival_cov!(estim::MovingHorizonEstimator, ::SteadyKalmanFilter) + return nothing +end + """ obj_nonlinprog(estim::MovingHorizonEstimator, ::LinModel, _ , _ , _ , Z̃)