From 73291b0b193db3937cf4c7fcb5506fd332266eeb Mon Sep 17 00:00:00 2001
From: Marco Ratto <marco.ratto@ec.europa.eu>
Date: Fri, 3 Jan 2020 13:19:57 +0100
Subject: [PATCH] before issuing F singularity, check with rescaled F matrix:
 this spares lots of computing time when singularity only happens in the first
 KF step.

---
 matlab/kalman/likelihood/kalman_filter.m               |  9 ++++++++-
 .../likelihood/missing_observations_kalman_filter.m    | 10 +++++++++-
 2 files changed, 17 insertions(+), 2 deletions(-)

diff --git a/matlab/kalman/likelihood/kalman_filter.m b/matlab/kalman/likelihood/kalman_filter.m
index 441ee8525..ec26524eb 100644
--- a/matlab/kalman/likelihood/kalman_filter.m
+++ b/matlab/kalman/likelihood/kalman_filter.m
@@ -158,6 +158,7 @@ else
     LIKK={likk,dlikk};
 end
 
+rescale_prediction_error_covariance0=rescale_prediction_error_covariance;
 while notsteady && t<=last
     s = t-start+1;
     if Zflag
@@ -175,7 +176,12 @@ while notsteady && t<=last
         end
     else
         if rcond(F)<kalman_tol
-            badly_conditioned_F = true;
+            sig=sqrt(diag(F));
+            if any(diag(F)<kalman_tol) || rcond(F./(sig*sig'))<kalman_tol
+                badly_conditioned_F = true;
+            else
+                rescale_prediction_error_covariance=1;
+            end
         end
     end
     if badly_conditioned_F
@@ -191,6 +197,7 @@ while notsteady && t<=last
         if rescale_prediction_error_covariance
             log_dF = log(det(F./(sig*sig')))+2*sum(log(sig));
             iF = inv(F./(sig*sig'))./(sig*sig');
+            rescale_prediction_error_covariance=rescale_prediction_error_covariance0;
         else
             log_dF = log(det(F));
             iF = inv(F);
diff --git a/matlab/kalman/likelihood/missing_observations_kalman_filter.m b/matlab/kalman/likelihood/missing_observations_kalman_filter.m
index f1bd40cb5..a63c2f3f5 100644
--- a/matlab/kalman/likelihood/missing_observations_kalman_filter.m
+++ b/matlab/kalman/likelihood/missing_observations_kalman_filter.m
@@ -84,6 +84,7 @@ oldK = Inf;
 notsteady   = 1;
 F_singular  = true;
 s = 0;
+rescale_prediction_error_covariance0=rescale_prediction_error_covariance;
 
 while notsteady && t<=last
     s  = t-start+1;
@@ -110,7 +111,13 @@ while notsteady && t<=last
             end
         else
             if rcond(F)<kalman_tol
-                badly_conditioned_F = true;
+                sig=sqrt(diag(F));
+                if any(diag(F)<kalman_tol) || rcond(F./(sig*sig'))<kalman_tol
+                    badly_conditioned_F = true;
+                else
+                    rescale_prediction_error_covariance=1;
+                end
+                %                 badly_conditioned_F = true;
             end
         end
         if badly_conditioned_F
@@ -126,6 +133,7 @@ while notsteady && t<=last
             if rescale_prediction_error_covariance
                 log_dF = log(det(F./(sig*sig')))+2*sum(log(sig));
                 iF = inv(F./(sig*sig'))./(sig*sig');
+                rescale_prediction_error_covariance=rescale_prediction_error_covariance0;
             else
                 log_dF = log(det(F));
                 iF = inv(F);