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);
|
v = Y(:,t)-a(Z);
|
||||||
F = P(Z,Z) + H;
|
F = P(Z,Z) + H;
|
||||||
end
|
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)
|
if ~all(abs(F(:))<kalman_tol)
|
||||||
return
|
return
|
||||||
else
|
else
|
||||||
|
@ -178,9 +179,9 @@ while notsteady && t<=last
|
||||||
end
|
end
|
||||||
else
|
else
|
||||||
F_singular = 0;
|
F_singular = 0;
|
||||||
dF = det(F);
|
log_dF = log(det(F./(sig*sig')))+2*sum(log(sig));
|
||||||
iF = inv(F);
|
iF = inv(F./(sig*sig'))./(sig*sig');
|
||||||
likk(s) = log(dF)+transpose(v)*iF*v;
|
likk(s) = log_dF+transpose(v)*iF*v;
|
||||||
if Zflag
|
if Zflag
|
||||||
K = P*Z'*iF;
|
K = P*Z'*iF;
|
||||||
Ptmp = T*(P-K*Z*P)*transpose(T)+QQ;
|
Ptmp = T*(P-K*Z*P)*transpose(T)+QQ;
|
||||||
|
@ -245,7 +246,7 @@ if t <= last
|
||||||
Hess = Hess + tmp{3};
|
Hess = Hess + tmp{3};
|
||||||
end
|
end
|
||||||
else
|
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
|
||||||
end
|
end
|
||||||
|
|
||||||
|
|
|
@ -221,7 +221,7 @@ if t <= last
|
||||||
Hess = Hess + tmp{3};
|
Hess = Hess + tmp{3};
|
||||||
end
|
end
|
||||||
else
|
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
|
||||||
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).
|
% Computes the likelihood of a stationnary state space model (steady state kalman filter).
|
||||||
|
|
||||||
%@info:
|
%@info:
|
||||||
|
@ -130,7 +130,7 @@ while t <= last
|
||||||
end
|
end
|
||||||
|
|
||||||
% Adding constant determinant of F (prediction error covariance matrix)
|
% 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
|
% Add log-likelihhod constants and divide by two
|
||||||
likk = .5*(likk + pp*log(2*pi));
|
likk = .5*(likk + pp*log(2*pi));
|
||||||
|
|
|
@ -102,7 +102,8 @@ while notsteady & t<=last
|
||||||
v = Y(d_index,t) - a(z);
|
v = Y(d_index,t) - a(z);
|
||||||
F = P(z,z) + H(d_index,d_index);
|
F = P(z,z) + H(d_index,d_index);
|
||||||
end
|
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)
|
if ~all(abs(F(:))<kalman_tol)
|
||||||
return
|
return
|
||||||
else
|
else
|
||||||
|
@ -111,9 +112,9 @@ while notsteady & t<=last
|
||||||
end
|
end
|
||||||
else
|
else
|
||||||
F_singular = 0;
|
F_singular = 0;
|
||||||
dF = det(F);
|
log_dF = log(det(F./(sig*sig')))+2*sum(log(sig));
|
||||||
iF = inv(F);
|
iF = inv(F./(sig*sig'))./(sig*sig');
|
||||||
lik(s) = log(dF) + transpose(v)*iF*v + length(d_index)*log(2*pi);
|
lik(s) = log_dF + transpose(v)*iF*v + length(d_index)*log(2*pi);
|
||||||
if Zflag
|
if Zflag
|
||||||
K = P*z'*iF;
|
K = P*z'*iF;
|
||||||
P = T*(P-K*z*P)*transpose(T)+QQ;
|
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.
|
% Call steady state Kalman filter if needed.
|
||||||
if t<=last
|
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
|
end
|
||||||
|
|
||||||
% Compute minus the log-likelihood.
|
% Compute minus the log-likelihood.
|
||||||
|
|
|
@ -168,11 +168,13 @@ while notsteady && t<smpl
|
||||||
ZZ = Z(di,:);
|
ZZ = Z(di,:);
|
||||||
v(di,t) = Y(di,t) - ZZ*a(:,t);
|
v(di,t) = Y(di,t) - ZZ*a(:,t);
|
||||||
F = ZZ*P(:,:,t)*ZZ' + H(di,di);
|
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;
|
alphahat = Inf;
|
||||||
return
|
return
|
||||||
end
|
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);
|
PZI = P(:,:,t)*ZZ'*iF(di,di,t);
|
||||||
atilde(:,t) = a(:,t) + PZI*v(di,t);
|
atilde(:,t) = a(:,t) + PZI*v(di,t);
|
||||||
K(:,di,t) = T*PZI;
|
K(:,di,t) = T*PZI;
|
||||||
|
|
Loading…
Reference in New Issue