wip: KalmanFilter with new cov struct

Author: franckgaga

src/estimator/construct.jl

@@ -70,8 +70,6 @@ struct KalmanCovariances{
         inv!(invR̂)
         invQ̂_He = repeatdiag(invQ̂, He)
         invR̂_He = repeatdiag(invR̂, He)
-        isdiag(invQ̂_He) && (invQ̂_He = Diagonal(invQ̂_He)) # Q̂C(invQ̂_He) does not work on Julia 1.10
-        isdiag(invR̂_He) && (invR̂_He = Diagonal(invR̂_He)) # R̂C(invR̂_He) does not work on Julia 1.10
         invQ̂_He = Hermitian(invQ̂_He, :L)
         invR̂_He = Hermitian(invR̂_He, :L)
         return new{NT, Q̂C, R̂C}(P̂_0, P̂, Q̂, R̂, invP̄, invQ̂_He, invR̂_He)

src/estimator/kalman.jl

@@ -1174,7 +1174,7 @@ end
 function init_estimate_cov!(
     estim::Union{KalmanFilter, UnscentedKalmanFilter, ExtendedKalmanFilter}, _ , _ , _
 ) 
-    estim.P̂ .= estim.P̂_0
+    estim.cov.P̂ .= estim.cov.P̂_0
     return nothing
 end
 
@@ -1187,8 +1187,8 @@ Allows code reuse for [`KalmanFilter`](@ref), [`ExtendedKalmanFilterKalmanFilter
 See [`update_estimate_kf!`](@ref) for more information.
 """
 function correct_estimate_kf!(estim::Union{KalmanFilter, ExtendedKalmanFilter}, y0m, d0, Ĉm)
-    R̂, K̂ = estim.R̂, estim.K̂
-    x̂0, P̂ = estim.x̂0, estim.P̂
+    R̂, K̂ = estim.cov.R̂, estim.K̂
+    x̂0, P̂ = estim.x̂0, estim.cov.P̂
     # in-place operations to reduce allocations:
     P̂_Ĉmᵀ = K̂
     mul!(P̂_Ĉmᵀ, P̂, Ĉm')
@@ -1213,7 +1213,7 @@ function correct_estimate_kf!(estim::Union{KalmanFilter, ExtendedKalmanFilter},
     end
     P̂corr = estim.buffer.P̂
     mul!(P̂corr, I_minus_K̂_Ĉm, P̂)
-    estim.P̂ .= Hermitian(P̂corr, :L)
+    estim.cov.P̂ .= Hermitian(P̂corr, :L)
     return nothing
 end
 
@@ -1227,8 +1227,8 @@ They predict the state `x̂` and covariance `P̂` with the same equations. See
 [`update_estimate`](@ref) methods for the equations.
 """
 function predict_estimate_kf!(estim::Union{KalmanFilter, ExtendedKalmanFilter}, u0, d0, Â)
-    x̂0corr, P̂corr = estim.x̂0, estim.P̂
-    Q̂ = estim.Q̂
+    x̂0corr, P̂corr = estim.x̂0, estim.cov.P̂
+    Q̂ = estim.cov.Q̂
     x̂0next, û0, k0 = estim.buffer.x̂, estim.buffer.û, estim.buffer.k
     # in-place operations to reduce allocations:
     f̂!(x̂0next, û0, k0, estim, estim.model, x̂0corr, u0, d0)
@@ -1240,6 +1240,6 @@ function predict_estimate_kf!(estim::Union{KalmanFilter, ExtendedKalmanFilter},
     P̂next .= Â_P̂corr_Âᵀ .+ Q̂
     x̂0next  .+= estim.f̂op .- estim.x̂op
     estim.x̂0 .= x̂0next
-    estim.P̂  .= Hermitian(P̂next, :L)
+    estim.cov.P̂  .= Hermitian(P̂next, :L)
     return nothing
 end