add try catch blocks in filter recursions

LAMPSPUC · Oct 9, 2024 · 07f2871 · 07f2871
1 parent 82ff7ec
commit 07f2871
Show file tree

Hide file tree

Showing 5 changed files with 268 additions and 233 deletions.
diff --git a/src/filters/multivariate_kalman_filter.jl b/src/filters/multivariate_kalman_filter.jl
@@ -187,15 +187,10 @@ function update_P!(
 end
 
 function update_llk!(kalman_state::MultivariateKalmanState{Fl}) where Fl
-    try 
-        kalman_state.llk -= (
-            HALF_LOG_2_PI + 0.5 * (logdet(kalman_state.F) +
-            kalman_state.v' * inv(kalman_state.F) * kalman_state.v)
-        )
-    catch
-        @error("Numerical error in the log-likelihood calculation. F = $(kalman_state.F), v = $(kalman_state.v). det(F) can only be positive.")
-        rethrow()
-    end
+    kalman_state.llk -= (
+        HALF_LOG_2_PI + 0.5 * (logdet(kalman_state.F) +
+        kalman_state.v' * inv(kalman_state.F) * kalman_state.v)
+    )
     return kalman_state
 end
 
@@ -307,21 +302,26 @@ function filter_recursions!(
     steadystate_tol::Fl,
     skip_llk_instants::Int,
 ) where Fl
-    RQR = sys.R * sys.Q * sys.R'
-    @inbounds for t in 1:size(sys.y, 1)
-        update_kalman_state!(
-            kalman_state,
-            sys.y[t, :],
-            sys.Z,
-            sys.T,
-            sys.H,
-            RQR,
-            sys.d,
-            sys.c,
-            skip_llk_instants,
-            steadystate_tol,
-            t,
-        )
+    try
+        RQR = sys.R * sys.Q * sys.R'
+        @inbounds for t in 1:size(sys.y, 1)
+            update_kalman_state!(
+                kalman_state,
+                sys.y[t, :],
+                sys.Z,
+                sys.T,
+                sys.H,
+                RQR,
+                sys.d,
+                sys.c,
+                skip_llk_instants,
+                steadystate_tol,
+                t,
+            )
+        end
+    catch
+        @error("Numerical error when applying Kalman filter euqations, the current state is: $kalman_state")
+        rethrow()
     end
     return kalman_state.llk
 end
@@ -333,20 +333,25 @@ function filter_recursions!(
     steadystate_tol::Fl,
     skip_llk_instants::Int,
 ) where Fl
-    @inbounds for t in 1:size(sys.y, 1)
-        update_kalman_state!(
-            kalman_state,
-            sys.y[t, :],
-            sys.Z[t],
-            sys.T[t],
-            sys.H[t],
-            sys.R[t],
-            sys.Q[t],
-            sys.d[t],
-            sys.c[t],
-            skip_llk_instants,
-            t,
-        )
+    try
+        @inbounds for t in 1:size(sys.y, 1)
+            update_kalman_state!(
+                kalman_state,
+                sys.y[t, :],
+                sys.Z[t],
+                sys.T[t],
+                sys.H[t],
+                sys.R[t],
+                sys.Q[t],
+                sys.d[t],
+                sys.c[t],
+                skip_llk_instants,
+                t,
+            )
+        end
+    catch
+        @error("Numerical error when applying Kalman filter euqations, the current state is: $kalman_state")
+        rethrow()
     end
     return kalman_state.llk
 end
@@ -358,23 +363,28 @@ function filter_recursions!(
     steadystate_tol::Fl,
     skip_llk_instants::Int,
 ) where Fl
-    RQR = sys.R * sys.Q * sys.R'
-    save_a1_P1_in_filter_output!(filter_output, kalman_state)
-    @inbounds for t in 1:size(sys.y, 1)
-        update_kalman_state!(
-            kalman_state,
-            sys.y[t, :],
-            sys.Z,
-            sys.T,
-            sys.H,
-            RQR,
-            sys.d,
-            sys.c,
-            skip_llk_instants,
-            steadystate_tol,
-            t,
-        )
-        save_kalman_state_in_filter_output!(filter_output, kalman_state, t)
+    try
+        RQR = sys.R * sys.Q * sys.R'
+        save_a1_P1_in_filter_output!(filter_output, kalman_state)
+        @inbounds for t in 1:size(sys.y, 1)
+            update_kalman_state!(
+                kalman_state,
+                sys.y[t, :],
+                sys.Z,
+                sys.T,
+                sys.H,
+                RQR,
+                sys.d,
+                sys.c,
+                skip_llk_instants,
+                steadystate_tol,
+                t,
+            )
+            save_kalman_state_in_filter_output!(filter_output, kalman_state, t)
+        end
+    catch
+        @error("Numerical error when applying Kalman filter euqations, the current state is: $kalman_state")
+        rethrow()
     end
     return filter_output
 end
@@ -386,22 +396,27 @@ function filter_recursions!(
     steadystate_tol::Fl,
     skip_llk_instants::Int,
 ) where Fl
-    save_a1_P1_in_filter_output!(filter_output, kalman_state)
-    @inbounds for t in 1:size(sys.y, 1)
-        update_kalman_state!(
-            kalman_state,
-            sys.y[t, :],
-            sys.Z[t],
-            sys.T[t],
-            sys.H[t],
-            sys.R[t],
-            sys.Q[t],
-            sys.d[t],
-            sys.c[t],
-            skip_llk_instants,
-            t,
-        )
-        save_kalman_state_in_filter_output!(filter_output, kalman_state, t)
+    try 
+        save_a1_P1_in_filter_output!(filter_output, kalman_state)
+        @inbounds for t in 1:size(sys.y, 1)
+            update_kalman_state!(
+                kalman_state,
+                sys.y[t, :],
+                sys.Z[t],
+                sys.T[t],
+                sys.H[t],
+                sys.R[t],
+                sys.Q[t],
+                sys.d[t],
+                sys.c[t],
+                skip_llk_instants,
+                t,
+            )
+            save_kalman_state_in_filter_output!(filter_output, kalman_state, t)
+        end
+    catch
+        @error("Numerical error when applying Kalman filter euqations, the current state is: $kalman_state")
+        rethrow()
     end
     return filter_output
 end
diff --git a/src/filters/regression_kalman_filter.jl b/src/filters/regression_kalman_filter.jl
@@ -58,14 +58,9 @@ function regression_update_v!(
 end
 
 function update_llk!(kalman_state::RegressionKalmanState{Fl}) where Fl
-    try 
-        kalman_state.llk -= (
-            HALF_LOG_2_PI + 0.5 * (log(kalman_state.F) + kalman_state.v^2 / kalman_state.F)
-        )
-    catch
-        @error("Numerical error in the log-likelihood calculation. F = $(kalman_state.F), v = $(kalman_state.v). F can only be positive.")
-        rethrow()
-    end
+    kalman_state.llk -= (
+        HALF_LOG_2_PI + 0.5 * (log(kalman_state.F) + kalman_state.v^2 / kalman_state.F)
+    )
     return kalman_state
 end
 

diff --git a/src/filters/scalar_kalman_filter.jl b/src/filters/scalar_kalman_filter.jl
@@ -125,14 +125,9 @@ function scalar_update_P!(kalman_state::ScalarKalmanState{Fl}, T::Fl, RQR::Fl) w
 end
 
 function update_llk!(kalman_state::ScalarKalmanState{Fl}) where Fl
-    try 
-        kalman_state.llk -= (
-            HALF_LOG_2_PI + 0.5 * (log(kalman_state.F) + kalman_state.v^2 / kalman_state.F)
-        )
-    catch
-        @error("Numerical error in the log-likelihood calculation. F = $(kalman_state.F), v = $(kalman_state.v). F can only be positive.")
-        rethrow()
-    end
+    kalman_state.llk -= (
+        HALF_LOG_2_PI + 0.5 * (log(kalman_state.F) + kalman_state.v^2 / kalman_state.F)
+    )
     return kalman_state
 end
 
@@ -196,21 +191,26 @@ function filter_recursions!(
     steadystate_tol::Fl,
     skip_llk_instants::Int,
 ) where Fl
-    RQR = sys.R[1] * sys.Q[1] * sys.R[1]
-    @inbounds for t in eachindex(sys.y)
-        scalar_update_kalman_state!(
-            kalman_state,
-            sys.y[t],
-            sys.Z[1],
-            sys.T[1],
-            sys.H,
-            RQR,
-            sys.d,
-            sys.c[1],
-            skip_llk_instants,
-            steadystate_tol,
-            t,
-        )
+    try 
+        RQR = sys.R[1] * sys.Q[1] * sys.R[1]
+        @inbounds for t in eachindex(sys.y)
+            scalar_update_kalman_state!(
+                kalman_state,
+                sys.y[t],
+                sys.Z[1],
+                sys.T[1],
+                sys.H,
+                RQR,
+                sys.d,
+                sys.c[1],
+                skip_llk_instants,
+                steadystate_tol,
+                t,
+            )
+        end
+    catch
+        @error("Numerical error when applying Kalman filter euqations, the current state is: $kalman_state")
+        rethrow()
     end
     return kalman_state.llk
 end
@@ -222,23 +222,28 @@ function filter_recursions!(
     steadystate_tol::Fl,
     skip_llk_instants::Int,
 ) where Fl
-    RQR = sys.R[1] * sys.Q[1] * sys.R[1]
-    save_a1_P1_in_filter_output!(filter_output, kalman_state)
-    @inbounds for t in eachindex(sys.y)
-        scalar_update_kalman_state!(
-            kalman_state,
-            sys.y[t],
-            sys.Z[1],
-            sys.T[1],
-            sys.H,
-            RQR,
-            sys.d,
-            sys.c[1],
-            skip_llk_instants,
-            steadystate_tol,
-            t,
-        )
-        save_kalman_state_in_filter_output!(filter_output, kalman_state, t)
+    try
+        RQR = sys.R[1] * sys.Q[1] * sys.R[1]
+        save_a1_P1_in_filter_output!(filter_output, kalman_state)
+        @inbounds for t in eachindex(sys.y)
+            scalar_update_kalman_state!(
+                kalman_state,
+                sys.y[t],
+                sys.Z[1],
+                sys.T[1],
+                sys.H,
+                RQR,
+                sys.d,
+                sys.c[1],
+                skip_llk_instants,
+                steadystate_tol,
+                t,
+            )
+            save_kalman_state_in_filter_output!(filter_output, kalman_state, t)
+        end
+    catch
+        @error("Numerical error when applying Kalman filter euqations, the current state is: $kalman_state")
+        rethrow()
     end
     return filter_output
 end