% Mink-muskrat data set
y = [  0.10609     0.16794
      -0.16852     0.06242
      -0.23700    -0.13344
      -0.18022    -0.50616
       0.18094    -0.37943
       0.65983    -0.40132
       0.65235     0.08789
       0.21594     0.23877
      -0.11515     0.40043
      -0.00067     0.37758
      -0.00387     0.55735
      -0.25202     0.34444
      -0.65011    -0.02749
      -0.53646    -0.41519
      -0.08462     0.02591
      -0.05640    -0.11348
       0.26630     0.20544
       0.03641     0.16331
      -0.26030    -0.01498
      -0.03995     0.09657
       0.33612     0.31096
      -0.11672     0.30681
      -0.69775    -0.69351
      -0.07569    -0.56212
       0.36149    -0.36799
       0.42341    -0.24725
       0.26721     0.04478
      -0.00363     0.21637
       0.08333     0.30188
      -0.22480     0.29493
      -0.13728     0.35463
      -0.12698     0.05490
      -0.18770    -0.52573
       0.34741    -0.49541
       0.54947    -0.26250
       0.57423    -0.21936
       0.57493    -0.12012
       0.28188     0.63556
      -0.58438     0.27067
      -0.50236     0.10386
      -0.60766     0.36748
      -1.04784    -0.33493
      -0.68857    -0.46525
      -0.11450    -0.63648
       0.22005    -0.26335
       0.36533     0.07017
      -0.00151    -0.04977
       0.03740    -0.02411
       0.22438     0.30790
      -0.16196     0.41050
      -0.12862     0.34929
       0.08448    -0.14995
       0.17945    -0.03320
       0.37502     0.02953
       0.95727     0.24090
       0.86188     0.41096
       0.39464     0.24157
       0.53794     0.29385
       0.13054     0.39336
      -0.39138    -0.00323
      -1.23825    -0.56953
      -0.66286    -0.72363  ]';
% mean adjust series
t = size(y,2);
Ny = size(y,1);
Nz = Ny;
F = eye(Nz);
H = eye(Ny);
% observation noise variance is diagonal
Rt = 1e-5*eye(Ny);
% transition noise variance
Vt = .1*eye(Nz);
a = zeros(Nz,1);
b = zeros(Ny,1);
myu = zeros(Nz,1);
sigma = .1*eye(Nz);
itermat = [];
for iter=1:100
   % construct big cov matrix
   var = [Vt zeros(Nz,Ny); zeros(Ny,Nz) Rt];

   % initial values are changed
   z0 = F*myu;
   vz0 = F*sigma*F'+Vt;

   % filtering to get one-step prediction and filtered value
   [logl, pred, vpred, filt, vfilt] = kalcvf(y, 0, a, F, b, H, var, z0, vz0);
   logl = (-2*logl-Ny*log(2*pi))*t;

   % smoothing using one-step prediction values
   [sm, vsm] = kalcvs(y, a, F, b, H, var, pred, vpred);

   % store old parameters and function values
   myu0 = myu;
   F0 = F;
   Vt0 = Vt;
   Rt0 = Rt;
   logl0 = logl;
   itermat = [itermat; iter logl0 F0(:)' myu0'];

   % obtain P*(t) to get P_T_0 and Z_T_0
   % these values are not usually needed
   % See Harvey (1991 p154) or Shumway (1988, p177)
   jt1 = sigma*F'/vpred(:,:,1);
   p_t_0  = sigma+jt1*(vsm(:,:,1)-vpred(:,:,1))*jt1';
   z_t_0  = myu+jt1*(sm(:,1)-pred(:,1));
   p_t1_t = vpred(:,:,t);
   p_t1_t1 = vfilt(:,:,t-1);
   Kt = p_t1_t*H'/(H*p_t1_t*H'+Rt);

   % obtain P_T_TT1. See Shumway (1988, p180)
   p_t_ii1 = (eye(Nz)-Kt*H)*F*p_t1_t1;
   st0 = vsm(:,:,t)+sm(:,t)*sm(:,t)';
   st1 = p_t_ii1+sm(:,t)*sm(:,t-1)';
   st00 = p_t_0+z_t_0*z_t_0';
   cov = (y(:,t)-H*sm(:,t))*(y(:,t)-H*sm(:,t))'+H*vsm(:,:,t)*H';
   for i=t:-1:2
      p_i1_i1 = vfilt(:,:,i-1);
      p_i1_i  = vpred(:,:,i);
      jt1 = p_i1_i1*F'/p_i1_i;
      p_i1_i  = vpred(:,:,i-1);
      if (i>2)
         p_i2_i2 = vfilt(:,:,i-2);
      else
         p_i2_i2 = sigma;
      end
      jt2 = p_i2_i2*F'/p_i1_i;
      p_t_i1i2 = p_i1_i1*jt2'+jt1*(p_t_ii1-F*p_i1_i1)*jt2';
      p_t_ii1 = p_t_i1i2;
      temp = vsm(:,:,i-1);
      sm1 = sm(:,i-1);
      st0 = st0+(temp+sm1*sm1');
      if (i>2)
         st1 = st1+(p_t_ii1+sm1*sm(:,i-2)');
      else
         st1 = st1+(p_t_ii1+sm1*z_t_0');
      end
      st00 = st00+(temp+sm1*sm1');
      cov = cov+H*temp*H'+(y(:,i-1)-H*sm1)*(y(:,i-1)-H*sm1)';
   end

   % M-step: update the parameters
   myu = z_t_0;
   F = st1/st00;
   Vt = (st0-F*st1')/t;
   Rt = cov/t;

   % check convergence
   if norm((myu-myu0)./(myu0+1e-6),inf) < 1e-2 && ...
      norm((F-F0)./(F0+1e-6),inf) < 1e-2 && ...
      norm((Vt-Vt0)./(Vt0+1e-6),inf) < 1e-2 && ...
      norm((Rt-Rt0)./(Rt0+1e-6),inf) < 1e-2 && ...
      abs((logl-logl0)/(logl0+1e-6)) < 1e-3
      break
   end
end

fprintf('\n   SSM Estimation using EM Algorithm\n');
fprintf('\n   Iter  -2*log L      F11       F21       F12       F22      MYU11     MYU22\n');
fprintf('%6d   % .3f   % .4f   % .4f   % .4f   % .4f   % .4f   % .4f\n',itermat');
eval = eig(F0);
fprintf('\n   Real         Imag         MOD\n');
eval = [real(eval) imag(eval) abs(eval)];
fprintf('% .7f   % .7f   % .7f\n',eval');
var = [Vt zeros(Nz,Ny); zeros(Ny,Nz) Rt];

% initial values are changed
z0  = F*myu;
vz0 = F*sigma*F'+Vt;
itermat=[];

% multistep prediction
[logl, pred, vpred] = kalcvf(y, 15, a, F, b, H, var, z0, vz0);
for i=1:15
   itermat = [itermat; i pred(:,t+i)' sqrt(diag(vpred(:,:,t+i)))'];
end
fprintf('\n  n-Step    Z1_T_n       Z2_T_n        SE_Z1        SE_Z2\n');
fprintf('%6d    % .7f   % .7f   % .7f   % .7f\n',itermat');
fprintf('\n');