leastsquarea.m

least square estimation of system identification

%@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@
%%%%%%%%%% SYSTEM IDENTIFICATION %%%%%%%%%%
%@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@
close all 
clear all
%*********************************************
%%%%%   INPUT OF FIVE SINUSIODES   %%%%%%%%
%*********************************************
n=1:100;
U=0;
for i=1:5;
U=U+5*sin(i*0.1*pi*n);
end
N=length(U);                         %% N
%*********************************************
%%%%% UNKNOWN FIR FILTER COEFFICIENTS %%%%%%%%
%*********************************************
Wo=fir1(10,0.5);
length(Wo);                         %% M
%*********************************************
%% AWGN WITH ZERO MEANS AND DEFERRENT VARAINCE 
%*********************************************
E_noise=1*randn(1,100);
var(E_noise);
length(E_noise);                    %% N
%*********************************************
%%%%%   UNKNOWN SYSTEM RESPONSE   %%%%%%%%
%*********************************************
Y=filter(Wo,1,U);
length(Y);                          %%% N
[h w]=freqz(Y);
%*********************************************
%%%%%   DESIRED SIGNAL RESPONSE     %%%%%%
%*********************************************
D_desire=Y+E_noise;
length(D_desire);                   %%% N
%*********************************************
%%%%%   PLOTTING OF DESIRED VERSUS OUTPUT  %%%
%*********************************************
figure(1)
plot(1:100,[Y;E_noise;D_desire])
legend('output of unknown system','noise with 0.1 var','desired response')
%*********************************************
%*********************************************
%%%%%LEAST SQUARE ADAPTIVE FILTER%%%%%%%%
%*********************************************
%*********************************************
M=input('estimated filter order');
%********************************************
%%%%%%% COVARIANCE METHOD %%%%%%%%%%%%%%%%%%%
%********************************************
for m=1:M
    for n=1:N-M+1
        A_hermit(m,n)=U(M-m+n);
    end
end
%A_hermit=A_hermit(:,1:N-M+1);
length(A_hermit);                  %% N-M+1
%*******************************************
%%%% TIME AVERAGE CORRELATION MATRIX  %%%%%%
%*******************************************
Phi_autocor=A_hermit*A_hermit';   
length(Phi_autocor);                %% M
%*******************************************
%%%%%%%%%% DESIRED VECTOR %%%%%%%%%%%%%%%%%%
%*******************************************
D_vec_hermit=D_desire(1,M:N);   
length(D_vec_hermit);              %% N-M+1
%*******************************************
%%%%%%% CROSS CORRELATION MATRIX  %%%%%%%%%%
%*******************************************
Z_crosscor=A_hermit*D_vec_hermit';  
length(Z_crosscor);                 %%%% M
%*******************************************
%%%%%%%%%%%%%% EQ 8.36 & EQ 8.39 %%%%%%%%%
%*******************************************
inv_Phi=inv(Phi_autocor);
W_hat=inv_Phi*Z_crosscor;                       %%%% tap wiegth of linear adaptive filter
length(W_hat);
D_energy=D_vec_hermit*D_vec_hermit';            %%% desired response enegy
Est_energy=Z_crosscor'*W_hat;                   %%% estimated respomse enegy    
E_minsq=D_energy - Est_energy;                    %%%% Minimum mean square error
W_hat_norm=norm(W_hat);                           %%%% length of the solution of least square
D_est=filter(W_hat,1,U);                        %%%% desired estimate response
length(D_est);                      %%% N
D_est_1=D_est(1,1:N-M+1)'; 
D_desire1=D_desire(1,1:N-M+1);
%*****************************************
%%%%% projection operator  %%%%%%%%%%%%
%******* EQ 8.51 ********************
%*****************************************
P_opt=A_hermit'*inv_Phi*A_hermit;
length(P_opt);                   %% N-M+1
P_orthognl=eye(N-M+1)-P_opt;
D_est1=P_opt*D_vec_hermit';
length(D_est1);                  %%% N-M+1
E_min_1=P_orthognl*D_vec_hermit';
length(E_min_1);                 %% N-M+1
%*****************************************
%%  SINGULAR VALUE DECOMPOSITION  %%%%%%
%****    PSEUDO INVERSE EQ 8.101  ********
%*****************************************
A_sdinv=inv_Phi*A_hermit;
W_hat_sdinv=A_sdinv*D_vec_hermit';
length(W_hat_sdinv);             %%% M
W_sdinv_norm=norm(W_hat_sdinv);
%*****************************************
%%%%%    SVD THOEREM EQ 8.132 %%%%%%%%%%
%*****************************************
A=A_hermit';
[U,D,V]=svd(A);
D_rank=rank(D);
D1=D(1:D_rank,1:D_rank);
inv_D=inv(D1);
W=length(inv_D);
K=N-M+1;
S=[inv_D zeros(W,M-W);zeros(K-W,W) zeros(K-W,M-W)];
length(S);
length(V);
length(U);
A_sdinv=V*S'*U';
W_hat_svd=A_sdinv*D_vec_hermit';
length(W_hat_svd);             %%% M
D_est_svd=filter(W_hat_svd,1,U);
length(D_est_svd);
D_est_svd=D_est_svd(1:N-M+1,1);
E_min=D_desire1'-D_est_svd;
W_svd_norm=norm(W_hat_svd);
%*****************************************
%%%%%   Orthogonality theorem   %%%%%%%%
%*****************************************
for k=0:M-1
    for i=M:N-M+1
        Vec_orth=U(i-k)*E_min_1(i);
    end
end
Vec_orth
%*****************************************
%% PLOTTING OF ESTIMATED VERSES DESIRED %%
%*****************************************
figure(2)
plot(1:N-M+1,[D_desire1',D_est1,E_min_1])
legend('Desired','Estimated with 1 var','error')