clear;
addpath('functions');
T=500;
B1=0.2;
B2=0.9;
C1=1;
C2=-1;
S1=3;
S2=1;
P=[0.95 0.05;0.05 0.95];
strue=zeros(T,2);
strue(1,1)=1;
strue=simS(strue,P);
e=randn(T,1);
Y=zeros(T,1);
X=zeros(T,1);
for i=2:T;
X(i,:)=Y(i-1,:);
if strue(i,1)==1
Y(i)=[X(i,:) 1]*[B1 C1]'+e(i)*sqrt(S1);
else
Y(i)=[X(i,:) 1]*[B2 C2]'+e(i)*sqrt(S2);
end
end
A = [(eye(2)-P);ones(1,2)];
EN=[0;0;1];
ett11= pinv(A'*A)*A'*EN;
iS1=1/S1;
iS2=1/S2;
lik=0;
filter=zeros(T,2);
for j=1:T
em1=Y(j)-[X(j,:) 1]*[B1 C1]';
em2=Y(j)-[X(j,:) 1]*[B2 C2]';
neta1=(1/sqrt(S1))*exp(-0.5*(em1*iS1*em1'));
neta2=(1/sqrt(S2))*exp(-0.5*(em2*iS2*em2'));
ett10=P*ett11;
ett11=ett10.*[neta1;neta2];
fit=sum(ett11);
ett11=(ett11)/fit;
filter(j,1:2)=ett11';
lik=lik+log(fit);
end
S=zeros(T,1);
p1=filter(T,1);
p2=filter(T,2);
p=p1/(p1+p2);
u=rand(1,1);
S(T,1)=(u>=p);
for t=T-1:-1:1
if S(t+1)==0
p00=P(1,1)*filter(t,1);
p01=P(1,2)*filter(t,2);
elseif S(t+1)==1
p00=P(2,1)*filter(t,1);
p01=P(2,2)*filter(t,2);
end
u=rand(1,1);
p=p00/(p00+p01);
if u<p
S(t)=0;
else
S(t)=1;
end
end