public static Matrix[] KalmanFilter(Matrix y, Matrix Z, Matrix d, Matrix T, Matrix c, Matrix a0,
Matrix P0, Matrix H, Matrix Q, int timeVar)
{
/*
* function [a_smooth,P_smooth,P2_smooth] = Kalman_Smoothing(y,Z,d,T,c,a0,P0,H,Q,timevar)
* % Kalman smoothing
* % code is a Matlab translation of Thierry Roncalli's.
*
* nobs = size(y,1);
* n = size(y,2);
* at = a0;
* Pt = P0;
*
*
* if timevar == 1
* m=size(Z,2)/n;
* g=size(Q,2)/m;
* else
* m=size(Z,2);
* g=size(Q,2);
* end
*
* a_cond = zeros(nobs,m);
* a = zeros(nobs,m);
* P_cond = zeros(nobs,m*m);
* P = zeros(nobs,m*m);
* a_smooth = zeros(nobs,m);
* P_smooth = zeros(nobs,m*m);
*
* if timevar ~= 1 % then matrices are time invariant
* Zt=Z;
* dt=d;
* Ht=H;
* Tt=T;
* ct=c;
* %Rt=R;
* Qt=Q;
* end
*
* for i=1:nobs
*
* yt=y(i,:)'; % y(t)
*
* if timevar == 1
* Zt=reshape(Z(i,:),n,m); % Z(t)
* dt=d(i,:)'; % d(t)
* Ht=reshape(H(i,:),n,n); % H(t)
* Tt=reshape(T(i,:),m,m); % T(t)
* ct=c(i,:)'; % c(t)
* Rt=reshape(R(i,:),m,g); % R(t)
* Qt=reshape(Q(i,:),g,g); % Q(t)
* end;
*
* % Prediction Equations
*
* at_1 = Tt*at + ct ; % a(t|t-1) formula(3.2.2a)
* Pt_1 = Tt*Pt*Tt' + Qt;%Rt*Qt*Rt' ; % P(t|t-1) formula(3.2.2b)
*
* % Innovations
*
* yt_1 = Zt*at_1 + dt ; % y(t|t-1) formula(3.2.18)
* vt = yt-yt_1 ; % v(t) formula(3.2.19)
*
* % Updating Equations
*
* Ft = Zt*Pt_1*Zt' + Ht ; % F(t) formula(3.2.3c)
* inv_Ft = Ft\eye(size(Ft,1)); % Inversion de Ft
*
* at = at_1 + Pt_1*Zt'*inv_Ft*vt ; % a(t) formula(3.2.3a)
* Pt = Pt_1 - Pt_1*Zt'*inv_Ft*Zt*Pt_1 ; % P(t) formula(3.2.3b)
*
* % Save results
*
* a(i,:)=at';
* a_cond(i,:)=at_1';
* P(i,:)=vecr(Pt)';
* P_cond(i,:)=vecr(Pt_1)';
*
* end;
*
* % Smoothing Equations
*
* a_smooth(nobs,:)=at';
* P_smooth(nobs,:)=vecr(Pt)';
*
* for i=(nobs-1):-1:1;
*
* if timevar == 1;
* Tt=reshape(T(i+1,:),m,m); % T(t+1)
* end;
*
* Pt=reshape(P(i,:),m,m); % P(t)
* Pt_1=reshape(P_cond(i+1,:),m,m); % P(t+1|t)
* Pt_1_T=reshape(P_smooth(i+1,:),m,m); % P(t+1|T)
* at=a(i,:)'; % a(t)
* at_1=a_cond(i+1,:)'; % a(t+1|t)
* at_1_T=a_smooth(i+1,:)'; % a(t+1|T)
*
* inv_Pt_1 = Pt_1\eye(size(Pt_1,1)); % Inversion de Ft
*
* P_star = Pt*Tt'*inv_Pt_1;
*
* as = at + P_star*(at_1_T-at_1) ; % a(t|T) formula(3.6.16a)
* Ps = Pt + P_star*(Pt_1_T-Pt_1)*P_star' ; % P(t|T) formula(3.6.16b)
*
* %Added equation P(t|t-1)'
* P2_smooth(i,:)=vecr((Pt_1_T*P_star')');
* a_smooth(i,:)=as';
* P_smooth(i,:)=vecr(Ps)';
*
* end
*/
int nobs = y.R;
int n = y.C;
int m = 0;
int g = 0;
Matrix at = a0;
Matrix Pt = P0;
Matrix logl = new Matrix(nobs, 1);
if (timeVar == 1)
{
m = Z.C / n;
g = Q.C / n;
}
else
{
m = Z.C;
g = Q.C;
}
Matrix y_cond = new Matrix(nobs, n);
Matrix v = new Matrix(nobs, n);
Matrix a_cond = new Matrix(nobs, m);
Matrix a = new Matrix(nobs, m);
Matrix P_cond = new Matrix(nobs, m * m);
Matrix P = new Matrix(nobs, m * m);
Matrix F = new Matrix(nobs, n * n);
Matrix Zt = new Matrix();
Matrix dt = new Matrix();
Matrix Ht = new Matrix();
Matrix Tt = new Matrix();
Matrix ct = new Matrix();
Matrix Qt = new Matrix();
if (timeVar != 1)
{
Zt = Z;
dt = d;
Ht = H;
Tt = T;
ct = c;
Qt = Q;
}
for (int i = 0; i < nobs; i++)
{
Matrix yt = y[i, Range.All];
yt = yt.Transpose();
if (timeVar == 1)
{
Zt = Z[i, Range.All];
Zt = Zt.Reshape(n, m);
dt = d[i, Range.All];
dt = dt.Transpose();
Ht = H[i, Range.All];
Ht = Ht.Reshape(n, n);
Tt = T[i, Range.All];
Tt = Tt.Reshape(m, m);
ct = c[i, Range.All];
ct = ct.Transpose();
Qt = Q[i, Range.All];
Qt = Qt.Reshape(g, g);
}
//% Prediction Equations
Matrix at_1 = Tt * at + ct;
Matrix Pt_1 = Tt * Pt * Tt.Transpose() + Qt;
//% Innovations
Matrix yt_1 = Zt * at_1 + dt;
Matrix vt = yt - yt_1;
//% Updating Equation
Matrix Ft = Zt * Pt_1 * Zt.Transpose() + Ht;
Matrix inv_Ft = Ft.Inverse();
at = at_1 + Pt_1 * Zt.Transpose() * inv_Ft * vt;
Pt = Pt_1 - Pt_1 * Zt.Transpose() * inv_Ft * Zt * Pt_1;
//% Save results
y_cond[i, Range.All] = yt_1.Transpose();
v[i, Range.All] = vt.Transpose();
a[i, Range.All] = at.Transpose();
a_cond[i, Range.All] = at_1.Transpose();
P[i, Range.All] = Vecr(Pt).Transpose();
P_cond[i, Range.All] = Vecr(Pt_1).Transpose();
F[i, Range.All] = Vecr(Ft).Transpose();
double dFt = Ft.Determinant();
if (dFt <= 0)
{
dFt = 1e-10;
}
// The matrix here is 1x1.
logl[i, 0] = (double)(-(n / 2) * Math.Log(2 * Math.PI) - 0.5 * Math.Log(dFt) - 0.5 * vt.Transpose() * inv_Ft * vt);
}
return(new Matrix[] { y_cond, v, a, a_cond, P, P_cond, F, logl });
}