MATLAB and C Programs - Digital Signal Processing: A Practitioner's Approach - page 152

xlabel(' Time ');
ylabel(' angle ');
pause
% print -depsc f6_5
A.3.16 Program f 3_6_9
%Digital Signal Processing:A Practitioner's Approach
%Dr.Kaluri Venkata Ranga Rao, [e-mail]
%Generates figure 3.34
clear;clf;
nmax=2000;
t=1:nmax+9; f=0.05;
xreal(1:1000)=2*sin(2*pi*f*t(1:1000));
xreal(1001:2000)=2*sin(2*pi*f*t(1001:2000));
theta=2*pi*f;
% plot(u);grid;pause;
% plot(xreal);pause;
noise=randn(size(xreal));
y=xreal+0.5*noise;
a=[2*cos(theta) -1; 1 0];
b=[0 0]'; u=0;
c=[1 0];
x_prev=[.01. 01]';
p_prev=[10^6 0 ; 0 10^6];
N=0; s=100;
for i=1:s
N=N+1;
pred_x = a*x_prev+b*u;
pred_p = a*p_prev*a';
err = c*pred_x - y(i);
Y(i)=y(i)
arr(i)=err;
xhat = pred_x - ((pred_p*c')/(1+c*pred_p*c'))*err;
phat = pred_p-(pred_p*c'*c*pred_p)/(1+c*pred_p*c');
Xfilter(i)=xhat(1);
x_prev=xhat;
p_prev=phat;
end
k=1:N;
% plot(k,xreal(1:N), k,y(1:N),k,Xfilter);grid;
subplot(211)
plot(k,Y,'o',k,Xfilter);grid;