Make multivariate kalman filter and smoother robust to badly scaled covariance matrix of observables.
This avoids shifting to univariate filter in most cases.time-shift
parent
d3eca9338b
commit
05fc096569
|
@ -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
|
||||
|
||||
|
|
|
@ -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
|
||||
|
||||
|
|
|
@ -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));
|
||||
|
|
|
@ -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.
|
||||
|
|
|
@ -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;
|
||||
|
|
Loading…
Reference in New Issue