% Compare with a Switching Kalman Filter
  
intra = zeros(3);
intra(1,2) = 1; intra(1,3)=1;
intra(2,3) = 1;
inter = zeros(3);
inter(1,1) = 1;
inter(2,2) = 1;
M = 2;
ss = 2;
os = 1;
ns = [M ss os];
eclass = [1 2 3 4 5 3];
dnodes = 1;
onodes = 3;
bnet = mk_dbn(intra, inter, ns, dnodes, 'jtree', {}, eclass, onodes);


A = rand(ss,ss,M);
C = rand(os,ss,M);
Q = repmat(eye(ss), [1 1 M]);
R = repmat(0.1*eye(os), [1 1 M]);
init_x = ones(ss,M);
init_V = repmat(eye(ss,ss), [1 1 M]);
trans_prob = [0.5 0.5; 0.5 0.5];
prior = [0.6 0.4];

bnet.bnodes{1} = mk_tabular_node(bnet, 1);
bnet.bnodes{1}.CPT = prior;
bnet.bnodes{2} = mk_gaussian_node(bnet, 2);
bnet.bnodes{2}.mean = init_x;
bnet.bnodes{2}.cov = init_V;
bnet.bnodes{3} = mk_gaussian_node(bnet, 3);
bnet.bnodes{3}.mean = repmat(zeros(os,1), [1 1 M]);
bnet.bnodes{3}.cov = R;
bnet.bnodes{3}.weights = C;
bnet.bnodes{4} = mk_tabular_node(bnet, 4);
bnet.bnodes{4}.CPT = trans_prob;
bnet.bnodes{5} = mk_gaussian_node(bnet, 5);
bnet.bnodes{5}.mean = repmat(zeros(ss,1),[1 1 M]);
bnet.bnodes{5}.cov = Q;
bnet.bnodes{5}.weights = A;
bnet = compile_params(bnet);

models = [1 1 2 2 1 1];
T=length(models);
[x,y] = sample_lds(A, C, Q, R, init_x, T, models);

%y = [-2.5011   -2.1964   -1.1582];
[bnet, ll] = enter_evidence(bnet, y, obs_nodes);

T = length(y);
xj = zeros(ss,T);
Vj = zeros(ss,ss,T);
for t=1:T
  m = marginal_nodes(bnet,2,t);
  xj(:,t) = m.mu;
  Vj(:,:,t) = m.Sigma;
end

[xsmooth, Vsmooth, VVsmooth, loglik, MM, M] =  sks1(y, A, C, Q, R, init_x, init_V, prior, trans_prob);

% Since SKS and BNT make different approximations in the backwards pass,
% they will only agree at time T, which is filtered, not smoothed.
assert(approxeq(xj(:,end), xsmooth(:,end)));
assert(approxeq(Vj(:,end), Vsmooth(:,end)));

