Merge pull request #330 from LAMPSPUC/gb/treat-numerical-errors

Better treatment for numerical errors

Project.toml

@@ -1,7 +1,7 @@
 name = "StateSpaceModels"
 uuid = "99342f36-827c-5390-97c9-d7f9ee765c78"
 authors = ["raphaelsaavedra <raphael.saavedra93@gmail.com>, guilhermebodin <guilherme.b.moraes@gmail.com>, mariohsouto"]
-version = "0.6.8"
+version = "0.6.9"
 
 [deps]
 Distributions = "31c24e10-a181-5473-b8eb-7969acd0382f"

src/filters/multivariate_kalman_filter.jl

@@ -187,9 +187,10 @@ function update_P!(
 end
 
 function update_llk!(kalman_state::MultivariateKalmanState{Fl}) where Fl
-    kalman_state.llk -=
+    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
 
@@ -301,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
@@ -327,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
@@ -352,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
@@ -380,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

src/filters/scalar_kalman_filter.jl

@@ -191,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
@@ -217,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

src/filters/sparse_univariate_kalman_filter.jl

@@ -180,7 +180,7 @@ end
 
 function update_llk!(kalman_state::SparseUnivariateKalmanState{Fl}) where Fl
     kalman_state.llk -= (
-        HALF_LOG_2_PI + (log(kalman_state.F) + kalman_state.v^2 / kalman_state.F) / 2
+        HALF_LOG_2_PI + 0.5 * (log(kalman_state.F) + kalman_state.v^2 / kalman_state.F)
     )
     return kalman_state
 end
@@ -255,23 +255,28 @@ function filter_recursions!(
     steadystate_tol::Fl,
     skip_llk_instants::Int,
 ) where Fl
-    RQR = sys.R * sys.Q * sys.R'
-    T_sparse = sparse(sys.T) 
-    Z_sparse = sparse(sys.Z)
-    @inbounds for t in eachindex(sys.y)
-        update_kalman_state!(
-            kalman_state,
-            sys.y[t],
-            Z_sparse,
-            T_sparse,
-            sys.H,
-            RQR,
-            sys.d,
-            sys.c,
-            skip_llk_instants,
-            steadystate_tol,
-            t,
-        )
+    try 
+        RQR = sys.R * sys.Q * sys.R'
+        T_sparse = sparse(sys.T) 
+        Z_sparse = sparse(sys.Z)
+        @inbounds for t in eachindex(sys.y)
+            update_kalman_state!(
+                kalman_state,
+                sys.y[t],
+                Z_sparse,
+                T_sparse,
+                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
@@ -283,25 +288,30 @@ function filter_recursions!(
     steadystate_tol::Fl,
     skip_llk_instants::Int,
 ) where Fl
-    RQR = sys.R * sys.Q * sys.R'
-    T_sparse = sparse(sys.T) 
-    Z_sparse = sparse(sys.Z)
-    save_a1_P1_in_filter_output!(filter_output, kalman_state)
-    @inbounds for t in eachindex(sys.y)
-        update_kalman_state!(
-            kalman_state,
-            sys.y[t],
-            Z_sparse,
-            T_sparse,
-            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'
+        T_sparse = sparse(sys.T) 
+        Z_sparse = sparse(sys.Z)
+        save_a1_P1_in_filter_output!(filter_output, kalman_state)
+        @inbounds for t in eachindex(sys.y)
+            update_kalman_state!(
+                kalman_state,
+                sys.y[t],
+                Z_sparse,
+                T_sparse,
+                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