debug UKF

Author: franckgaga

src/estimator/kalman.jl

@@ -567,7 +567,7 @@ function update_estimate!(estim::UnscentedKalmanFilter{NT}, u, ym, d) where NT<:
     nym, nx̂, nσ = estim.nym, estim.nx̂, estim.nσ
     γ, m̂, Ŝ = estim.γ, estim.m̂, estim.Ŝ
     # --- initialize matrices ---
-    X̂  = Matrix{NT}(undef, nx̂, nσ)
+    X̂, X̂_next = Matrix{NT}(undef, nx̂, nσ), Matrix{NT}(undef, nx̂, nσ)
     ŷm = Vector{NT}(undef, nym)
     ŷ  = Vector{NT}(undef, estim.model.ny)
     Ŷm = Matrix{NT}(undef, nym, nσ)
@@ -600,7 +600,7 @@ function update_estimate!(estim::UnscentedKalmanFilter{NT}, u, ym, d) where NT<:
     X̂_cor .= x̂_cor
     X̂_cor[:, 2:nx̂+1]   .+= γ_sqrt_P̂_cor
     X̂_cor[:, nx̂+2:end] .-= γ_sqrt_P̂_cor
-    X̂_next = X̂_cor
+    X̂_next = similar(X̂_cor)
     for j in axes(X̂_next, 2)
         @views f̂!(X̂_next[:, j], estim, estim.model, X̂_cor[:, j], u, d)
     end

src/model/nonlinmodel.jl

@@ -130,3 +130,19 @@ h!(y, model::NonLinModel, x, d) = model.h!(y, x, d)
 
 typestr(model::NonLinModel) = "nonlinear"
 
+function rk4!(x, u, d)
+    xterm .= x
+    fc!(ẋ, xterm, u, d)
+    k1 .= ẋ
+    xterm .= @. x + k1 * Ts/2
+    fc!(ẋ, xterm, u, d)
+    k2 .= ẋ 
+    xterm .= @. x + k2 * Ts/2
+    fc!(ẋ, xterm, u, d)
+    k3 .= ẋ
+    xterm .= @. x + k3 * Ts
+    fc!(ẋ, xterm, u, d)
+    k4 .= ẋ
+    x .+= @. (k1 + 2k2 + 2k3 + k4)*Ts/6
+    return x
+end