From 05fc096569e15e89d8d13b08799321c0313b168d Mon Sep 17 00:00:00 2001
From: Marco Ratto <marco.ratto@jrc.ec.europa.eu>
Date: Tue, 13 Oct 2015 17:15:01 +0200
Subject: [PATCH] Make multivariate kalman filter and smoother robust to badly
 scaled covariance matrix of observables. This avoids shifting to univariate
 filter in most cases.

---
 matlab/kalman/likelihood/kalman_filter.m              | 11 ++++++-----
 matlab/kalman/likelihood/kalman_filter_fast.m         |  2 +-
 matlab/kalman/likelihood/kalman_filter_ss.m           |  4 ++--
 .../likelihood/missing_observations_kalman_filter.m   | 11 ++++++-----
 matlab/missing_DiffuseKalmanSmootherH1_Z.m            |  6 ++++--
 5 files changed, 19 insertions(+), 15 deletions(-)

diff --git a/matlab/kalman/likelihood/kalman_filter.m b/matlab/kalman/likelihood/kalman_filter.m
index 8dee95f06..14208f471 100644
--- a/matlab/kalman/likelihood/kalman_filter.m
+++ b/matlab/kalman/likelihood/kalman_filter.m
@@ -169,7 +169,8 @@ while notsteady && t<=last
         v  = Y(:,t)-a(Z);
         F  = P(Z,Z) + H;
     end
-    if rcond(F) < kalman_tol
+    sig=sqrt(diag(F));
+    if any(diag(F)<kalman_tol) || rcond(F./(sig*sig')) < kalman_tol
         if ~all(abs(F(:))<kalman_tol)
             return
         else
@@ -178,9 +179,9 @@ while notsteady && t<=last
         end
     else
         F_singular = 0;
-        dF      = det(F);
-        iF      = inv(F);
-        likk(s) = log(dF)+transpose(v)*iF*v;
+        log_dF      = log(det(F./(sig*sig')))+2*sum(log(sig));
+        iF      = inv(F./(sig*sig'))./(sig*sig');
+        likk(s) = log_dF+transpose(v)*iF*v;
         if Zflag
             K = P*Z'*iF;
             Ptmp = T*(P-K*Z*P)*transpose(T)+QQ;
@@ -245,7 +246,7 @@ if t <= last
             Hess = Hess + tmp{3};
         end
     else
-        [tmp, likk(s+1:end)] = kalman_filter_ss(Y,t,last,a,T,K,iF,dF,Z,pp,Zflag);
+        [tmp, likk(s+1:end)] = kalman_filter_ss(Y,t,last,a,T,K,iF,log_dF,Z,pp,Zflag);
     end
 end
 
diff --git a/matlab/kalman/likelihood/kalman_filter_fast.m b/matlab/kalman/likelihood/kalman_filter_fast.m
index d6a7ba257..6eeb90f76 100644
--- a/matlab/kalman/likelihood/kalman_filter_fast.m
+++ b/matlab/kalman/likelihood/kalman_filter_fast.m
@@ -221,7 +221,7 @@ if t <= last
             Hess = Hess + tmp{3};
         end
     else
-        [tmp, likk(s+1:end)] = kalman_filter_ss(Y,t,last,a,T,K,iF,dF,Z,pp,Zflag);
+        [tmp, likk(s+1:end)] = kalman_filter_ss(Y,t,last,a,T,K,iF,log(dF),Z,pp,Zflag);
     end
 end
 
diff --git a/matlab/kalman/likelihood/kalman_filter_ss.m b/matlab/kalman/likelihood/kalman_filter_ss.m
index 43493e4fa..8e5f92733 100644
--- a/matlab/kalman/likelihood/kalman_filter_ss.m
+++ b/matlab/kalman/likelihood/kalman_filter_ss.m
@@ -1,4 +1,4 @@
-function [LIK, likk, a] = kalman_filter_ss(Y,start,last,a,T,K,iF,dF,Z,pp,Zflag,analytic_derivation,Da,DT,DYss,D2a,D2T,D2Yss)
+function [LIK, likk, a] = kalman_filter_ss(Y,start,last,a,T,K,iF,log_dF,Z,pp,Zflag,analytic_derivation,Da,DT,DYss,D2a,D2T,D2Yss)
 % Computes the likelihood of a stationnary state space model (steady state kalman filter).
 
 %@info:
@@ -130,7 +130,7 @@ while t <= last
 end
 
 % Adding constant determinant of F (prediction error covariance matrix)
-likk = likk + log(dF);
+likk = likk + log_dF;
 
 % Add log-likelihhod constants and divide by two
 likk = .5*(likk + pp*log(2*pi));
diff --git a/matlab/kalman/likelihood/missing_observations_kalman_filter.m b/matlab/kalman/likelihood/missing_observations_kalman_filter.m
index 577d57beb..4c64bd28d 100644
--- a/matlab/kalman/likelihood/missing_observations_kalman_filter.m
+++ b/matlab/kalman/likelihood/missing_observations_kalman_filter.m
@@ -102,7 +102,8 @@ while notsteady & t<=last
             v = Y(d_index,t) - a(z);
             F = P(z,z) + H(d_index,d_index);
         end
-        if rcond(F) < kalman_tol
+        sig=sqrt(diag(F));
+        if any(diag(F)<kalman_tol) || rcond(F./(sig*sig')) < kalman_tol
             if ~all(abs(F(:))<kalman_tol)
                 return
             else
@@ -111,9 +112,9 @@ while notsteady & t<=last
             end
         else
             F_singular = 0;
-            dF     = det(F);
-            iF     = inv(F);
-            lik(s) = log(dF) + transpose(v)*iF*v + length(d_index)*log(2*pi);
+            log_dF      = log(det(F./(sig*sig')))+2*sum(log(sig));
+            iF      = inv(F./(sig*sig'))./(sig*sig');
+            lik(s) = log_dF + transpose(v)*iF*v + length(d_index)*log(2*pi);
             if Zflag
                 K = P*z'*iF;
                 P = T*(P-K*z*P)*transpose(T)+QQ;
@@ -140,7 +141,7 @@ lik(1:s) = .5*lik(1:s);
 
 % Call steady state Kalman filter if needed.
 if t<=last
-    [tmp, lik(s+1:end)] = kalman_filter_ss(Y,t,last,a,T,K,iF,dF,Z,pp,Zflag);
+    [tmp, lik(s+1:end)] = kalman_filter_ss(Y,t,last,a,T,K,iF,log_dF,Z,pp,Zflag);
 end
 
 % Compute minus the log-likelihood.
diff --git a/matlab/missing_DiffuseKalmanSmootherH1_Z.m b/matlab/missing_DiffuseKalmanSmootherH1_Z.m
index 9fc09eb30..b50416072 100644
--- a/matlab/missing_DiffuseKalmanSmootherH1_Z.m
+++ b/matlab/missing_DiffuseKalmanSmootherH1_Z.m
@@ -168,11 +168,13 @@ while notsteady && t<smpl
         ZZ = Z(di,:);
         v(di,t)      = Y(di,t) - ZZ*a(:,t);
         F = ZZ*P(:,:,t)*ZZ' + H(di,di);
-        if rcond(F) < kalman_tol
+        sig=sqrt(diag(F));
+
+        if any(diag(F)<kalman_tol) || rcond(F./(sig*sig')) < kalman_tol
             alphahat = Inf;
             return
         end    
-        iF(di,di,t)   = inv(F);
+        iF(di,di,t)   = inv(F./(sig*sig'))./(sig*sig');
         PZI         = P(:,:,t)*ZZ'*iF(di,di,t);
         atilde(:,t) = a(:,t) + PZI*v(di,t);
         K(:,di,t)    = T*PZI;