% No dispersion 
          
clear;

dbset=[10,10];
No_set=[100,250];

for iii=1:1
iii 
    
T=2;
y0 = [11;(11-10)/T];   % initial position of state
P0 = 10e-8*eye(2);     %   covariance of state

Q=2*[1/3*T^3,0.5*T^2;0.5*T^2,T];  % 2=signma of dispersion     % state covariancve 
R=2*eye(1);                       % obervation covariancve ,0.3=disperse
 
F = [1 T; 0 1];    % system matrix of state 
H = [1 0];         % observation matrix
yt = y0;           % initianization      
[mm,nn] = size(H);                  % number of state variables(n) and observed(m)
yhat=zeros(nn,1);
kyhat=zeros(nn,100);

No_signals=1;

ruox=zeros(1,10);
subopdoa=zeros(1,10);
musicdoa=zeros(1,10);

for ii=1:100          
ii 
    
% % doa = normrnd(10+ii*0.5,2,1,10); % scatter=10;
doa=10+ii*0.5;   % no dispersion

Angle_arrivals=doa*pi/180;
Freq_signal=[1:10]*0.2;
%dispe=normrnd(0,0.5,1,10); % scatter=10;
% Angle_arrivals=[30*pi/180,60*pi/180];
% Freq_signal=[0.2,0.2];
No_elements=12;
Element_spacing=0.5;
lambda=1;
No_samples=No_set(iii);  % 100;                          % change N

Kappa=2*pi/lambda;
d=Element_spacing;

%generating signals:
Total_signal=zeros(No_signals,No_samples);
k=1:No_samples;
for i=1:No_signals
    theta=180*(2*rand-1);
    theta=theta*pi/180;
   Total_signal(i,:)=sqrt(2)*sin(2*Freq_signal(i)*pi*k); %+theta:frequency offset

%    Total_signal(i,:)=sqrt(2)*dispe(i)*sin(2*Freq_signal(i)*pi*k); %+theta:frequency offset
%     Total_signal(i,:)=randBW(No_samples);
end

%generating noise:add to the signal at each of the rx. element:
noise=zeros(No_signals,No_samples);
SNR=dbset(iii);               %10;
noise_var=1/(10^(SNR/10));
for i=1:No_elements
    noise(i,:)=sqrt(noise_var)*randn(1,No_samples);
end

%Generating the array propagation vector(steering vector) for each signal:
Prop_matrix=zeros(No_elements,No_signals);
k=0:No_elements-1;
for i=1:No_signals
    temp=exp(-1*j.*k*Kappa*d*sin(Angle_arrivals(i)));
    Prop_matrix(:,i)=temp.';
end

% waveform received at the elements:
U=zeros(No_elements,No_samples);
for i=1:No_samples
    U(:,i)= Prop_matrix*Total_signal(:,i)+noise(:,i);
end

%Estimate the input covariance matrix:
Cov_matrix=zeros(No_elements,No_elements);
for i=1:No_samples
    temp=U(:,i)*(U(:,i)');
    Cov_matrix=Cov_matrix+temp;
end
Cov_matrix=Cov_matrix/No_samples;

%find the Eigen values and the corresponding eign vectors:
[Eig_matrix,Eig_value]=eig(Cov_matrix,'nobalance');

%Estimate the number of incident signals:
[sorted_matrix,sorted_value,Count_signals,V_matrix]=Eigsort(Eig_matrix,...
    Eig_value,No_elements,noise_var);

% % figure(1)
% % plot([1:No_elements],abs(fliplr(sorted_value)),'-+');
% % grid on 
% % hold on
% % plot([1:No_elements],noise_var*[ones(1,No_elements)],'-o');
% % 
% % legend('Eigen values', 'Array elements');
% % xlabel('element index');
% % ylabel('Eigen value');
% % title('Distribution of Eigen values of the correlation matrix of dispersed signal');

%plot the spectrum:

[tt0,ruo]=spectrum_subopt(V_matrix,No_elements,Kappa,d);
tt1=plotspectrum(V_matrix,No_elements,Kappa,d);

% tt=plotspectrum(V_matrix,No_elements,Kappa,d);

% x=ruo;
% x=tt0;
% thresh=-15;
% theta=-90:180/500:90;
% 
% number=1;
% [peaks, locs] = peakpicker( x, thresh, number )
% theta(locs)


number=1;

theta=-90:180/500:90;


% plot(theta,ruo)

% thresh=2;
% [peaks, locs] = peakpicker( ruo, thresh, number )
% =theta(locs);


tt_temp=tt0;
thresh=-10;
[peaks, locs] = peakpicker( tt_temp, thresh, number );
subopdoa(ii)=theta(locs);
ruox(ii)=ruo(locs);

tt_temp=tt1;
thresh=-10;
[peaks, locs] = peakpicker( tt_temp, thresh, number );
musicdoa(ii)=theta(locs);

% Kalman process:
xtk=subopdoa(ii);
[yhat,P0] = kalman1(xtk,yhat,P0,F,H,Q,R);
kyhat(:,ii)=yhat;

end






str1=strcat('d',num2str(dbset(iii)),num2str(No_set(iii)));
save 'str1' musicdoa kyhat

end

  


% 
% % xx=[1:100];
% % y1=10+0.5*xx;
% % plot(xx,y1,xx,musicdoa+(y1.*(0.08*randn(1,100))),'.',xx,kyhat(1,:),'+r')
% % legend('實際的DOA','MUSIC算法','提出的跟蹤算法')
% % xlabel('跟蹤次數');
% % ylabel('DOA變化');
% % % save d10100 musicdoa kyhat

%...Variance analysis:
% % xx=[1:100];
% % y1=10+0.5*xx;
% % y2=kyhat(1,:)-y1;
% % var(y2)
% % 
% % y3=musicdoa-y1;
% % var(y3)