%  EKF  Sinusoidal Tracking Example
%  AUTHOR: Balu Santhanam
%  DATE  : 04/26/00
%  COURSE : EECE-542, Spring 2000
%  NOTE : For accurate MMSE the results need to 
%         be averaged over multiple experiments
%***********************************************************
amp = ones(1,1000); N = 1000;
thet = (pi/4)*ones(1,1000); wc = pi/2; wm = 1; 
d = amp.*cos(wc*[0:1:N-1] + thet); 
obs = awn(d,[0,0.05]);
Q = [0.000001 0 ; 0 0.0000005];
R = 0.05^2; 
X_0 = [2;0];
P_0 = [0.001 0 ; 0 0.001];
Phi = eye(2,2); f_v = [wc,wm]; 
[X,K,J] = ekf(obs,Phi,Q,R,f_v,X_0,P_0);
env = X(1,:); 
phas = X(2,:);
% Plot the estimates
plot([0:1:N-1],env,[0:1:N-1],amp,'r--')
axis([0 600 0.5 2])
set(gca,'FontName','Helvetica','FontSize',16)
xlabel('TIME SAMPLE')
ylabel('EKF Amplitude Estimate')
title('EKF, Amplitude Tracking, \sigma_\epsilon = 0.05')
legend(gca,'EST','ACT',4)
pause(1)
print -deps ekf_amp1.eps
clf
plot([0:1:N-1],phas,[0:1:N-1],thet,'r--')
axis([0 600 0 1])
set(gca,'FontName','Helvetica','FontSize',16)
xlabel('TIME SAMPLE')
ylabel('EKF Phase  Estimate')
title('EKF, Phase Tracking, \sigma_\epsilon = 0.05')
legend(gca,'EST','ACT',4)
print -deps ekf_ph1.eps
pause(1)
clf
%*************************************************************
