From 36e1d0184cc544b474d3108726e994ae4c737a13 Mon Sep 17 00:00:00 2001 From: franckgaga Date: Mon, 26 Oct 2026 11:48:26 -0400 Subject: [PATCH 1/2] changed: slightly cleaner UKF implementation --- src/estimator/kalman.jl | 14 ++++++++------ 1 file changed, 8 insertions(+), 6 deletions(-) diff --git a/src/estimator/kalman.jl b/src/estimator/kalman.jl index 878f99301..bcdf633ca 100644 --- a/src/estimator/kalman.jl +++ b/src/estimator/kalman.jl @@ -794,8 +794,9 @@ function correct_estimate!(estim::UnscentedKalmanFilter, y0m, d0) X̂0[:, nx̂+2:end] .= @views X̂0[:, nx̂+2:end] .- γ_sqrtP̂ ŷ0 = estim.buffer.ŷ for j in axes(Ŷ0m, 2) - @views ĥ!(ŷ0, estim, estim.model, X̂0[:, j], d0) - @views Ŷ0m[:, j] .= ŷ0[estim.i_ym] + x̂0_j, ŷ0m_j = @views X̂0[:, j], Ŷ0m[:, j] + ĥ!(ŷ0, estim, estim.model, x̂0_j, d0) + ŷ0m_j .= @views ŷ0[estim.i_ym] end ŷ0m = @views ŷ0[estim.i_ym] mul!(ŷ0m, Ŷ0m, m̂) @@ -887,12 +888,13 @@ function update_estimate!(estim::UnscentedKalmanFilter, u0, y0m, d0) X̂0corr .= x̂0corr X̂0corr[:, 2:nx̂+1] .= @views X̂0corr[:, 2:nx̂+1] .+ γ_sqrtP̂corr X̂0corr[:, nx̂+2:end] .= @views X̂0corr[:, nx̂+2:end] .- γ_sqrtP̂corr - X̂0next = X̂0corr + X̂0next = X̂0corr # X̂0next is an alias of X̂0corr for j in axes(X̂0next, 2) - @views x̂0corr .= X̂0corr[:, j] - @views f̂!(X̂0next[:, j], û0, k̄, estim, estim.model, x̂0corr, u0, d0) + x̂0corr_j, x̂0next_j = @views X̂0corr[:, j], X̂0next[:, j] + @views f̂!(x̂0next, û0, k̄, estim, estim.model, x̂0corr_j, u0, d0, u0next, d0next) + x̂0next_j .= x̂0next end - x̂0next .= mul!(x̂0corr, X̂0next, m̂) + mul!(x̂0next, X̂0next, m̂) X̄0next = estim.X̄0 X̄0next .= X̂0next .- x̂0next Ŝ_X̄0nextᵀ = estim.X̂0' From 8fe94ba5ab654bc74edac5a0683ba562a1d4bf87 Mon Sep 17 00:00:00 2001 From: franckgaga Date: Mon, 26 Oct 2026 12:13:13 -0400 Subject: [PATCH 2/2] debug: correct signature --- src/estimator/kalman.jl | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/estimator/kalman.jl b/src/estimator/kalman.jl index bcdf633ca..7e4d69217 100644 --- a/src/estimator/kalman.jl +++ b/src/estimator/kalman.jl @@ -891,7 +891,7 @@ function update_estimate!(estim::UnscentedKalmanFilter, u0, y0m, d0) X̂0next = X̂0corr # X̂0next is an alias of X̂0corr for j in axes(X̂0next, 2) x̂0corr_j, x̂0next_j = @views X̂0corr[:, j], X̂0next[:, j] - @views f̂!(x̂0next, û0, k̄, estim, estim.model, x̂0corr_j, u0, d0, u0next, d0next) + @views f̂!(x̂0next, û0, k̄, estim, estim.model, x̂0corr_j, u0, d0) x̂0next_j .= x̂0next end mul!(x̂0next, X̂0next, m̂)