test: debug

Author: franckgaga

src/estimator/construct.jl

@@ -105,7 +105,12 @@ function KalmanCovariances(
     ) where {NT<:Real}
     validate_kfcov(model, i_ym, nint_u, nint_ym, Q̂, R̂, P̂_0)
     Q̂, R̂ = NT.(Q̂), NT.(R̂)
-    !isnothing(P̂_0) && (P̂_0 .= NT.(P̂_0))
+    if !isnothing(P̂_0) 
+        P̂_0 .= NT.(P̂_0)
+    end
+    println(Q̂)
+    println(R̂)
+    println(P̂_0)
     return KalmanCovariances{NT}(Q̂, R̂, P̂_0, He)
 end
 

src/estimator/mhe/execute.jl

@@ -797,14 +797,14 @@ function setmodel_estimator!(
         invQ̂  = Hermitian(estim.buffer.Q̂, :L)
         invQ̂ .= estim.cov.Q̂
         inv!(invQ̂)
-        estim.invQ̂_He .= Hermitian(repeatdiag(invQ̂, He), :L)
+        estim.cov.invQ̂_He .= Hermitian(repeatdiag(invQ̂, He), :L)
     end
     if !isnothing(R̂) 
         estim.cov.R̂ .= to_hermitian(R̂)
         invR̂  = Hermitian(estim.buffer.R̂, :L)
         invR̂ .= estim.cov.R̂
         inv!(invR̂)
-        estim.invR̂_He .= Hermitian(repeatdiag(invR̂, He), :L)
+        estim.cov.invR̂_He .= Hermitian(repeatdiag(invR̂, He), :L)
     end
     return nothing
 end

test/2_test_state_estim.jl

@@ -864,8 +864,8 @@ end
     @test mhe3.nx̂ == 5
 
     mhe4 = MovingHorizonEstimator(nonlinmodel, He=5, σQ=[1,2,3,4], σQint_ym=[5, 6], σR=[7, 8])
-    @test mhe4.Q̂ ≈ Hermitian(diagm(Float64[1, 4, 9 ,16, 25, 36]))
-    @test mhe4.R̂ ≈ Hermitian(diagm(Float64[49, 64]))
+    @test mhe4.cov.Q̂ ≈ Hermitian(diagm(Float64[1, 4, 9 ,16, 25, 36]))
+    @test mhe4.cov.R̂ ≈ Hermitian(diagm(Float64[49, 64]))
 
     mhe5 = MovingHorizonEstimator(nonlinmodel, He=5, nint_ym=[2,2])
     @test mhe5.nxs == 4
@@ -1116,7 +1116,7 @@ end
     )
     mhe.P̂arr_old[1, 1] = Inf # Inf to trigger fallback
     P̂arr_old_copy = deepcopy(mhe.P̂arr_old)
-    invP̄_copy = deepcopy(mhe.invP̄)
+    invP̄_copy = deepcopy(mhe.cov.invP̄)
     @test_logs(
         (:error, "Arrival covariance P̄ is not finite: keeping the old one"), 
         preparestate!(mhe, [50, 30], [5])
@@ -1425,7 +1425,7 @@ end
     # recuperate P̂(-1|-1) exact value using the Unscented Kalman filter:
     preparestate!(ukf, [50, 30], [20])
     preparestate!(ekf, [50, 30], [20])
-    σP̂ = sqrt.(diag(ukf.P̂))
+    σP̂ = sqrt.(diag(ukf.cov.P̂))
     mhe = MovingHorizonEstimator(nonlinmodel, He=5, nint_ym=0, direct=true, σP_0=σP̂)
     updatestate!(ukf, [10, 50], [50, 30], [20])
     updatestate!(ekf, [10, 50], [50, 30], [20])