%%%%%%@@@@@@@@@
%%%%%%書本63-64頁，線性預測算法◎◎◎◎◎◎◎◎◎◎◎◎◎◎◎◎◎◎◎
clc;
clear;
close all;
tic
M=16;%陣元數目
N=3;%信源數目
B=8;%形成的波束個數
snap=1000;%快拍數目
C=3e8;
f0=10e6;
lamda=C/f0;
d=0.5*lamda;
% k=d/lamda;
theta0=20;
theta1=30;
theta2=-10;
fs=1000;
ts=1/fs;
t=(0:snap-1)*ts;
a=[0:M-1]';%陣列矢量
u0=5;
u1=10;
u2=20;


s0=exp(j*2*pi*0.5*u0*t.^2);
s1=exp(j*2*pi*0.5*u0*t.^2);
s2=exp(j*2*pi*0.5*u0*t.^2);

%陣列流行矢量

a_theta0=exp(j*2*pi*d/lamda*a*sin(theta0/180*pi));
a_theta1=exp(j*2*pi*d/lamda*a*sin(theta1/180*pi));
a_theta2=exp(j*2*pi*d/lamda*a*sin(theta2/180*pi));

A=[a_theta0 a_theta1 a_theta2];
S=[s0;s1;s2];
X0=A*S;
SNR=15;
randn('state',0);
real_noise=randn(size(X0));
randn('state',3);
imag_noise=randn(size(X0));
noise0=(real_noise+j*imag_noise)/2^0.5;
noise=10^(-SNR/20)*noise0;
X=X0+noise;%%%%%%%%%%------------=========完整的基帶信號
Rx=X*X'/length(t);
%%%@@@@@@@@@@@@@@@@下面為一階線性預測算法
%%%%%%%%%%前向預測算法
Xf=flipud(X(1:M-1,:));Xft=transpose(Xf);Xfh=Xf';
Rf=Xf*Xfh/snap;rf=Xf*X(M,:)'/snap;
Wflp=conj((inv(Rf)*rf));
%%%%%%%%%%后向預測算法
Xb=X(2:M,:);Xbt=transpose(Xb);Xbh=Xb';
Rb=Xb*Xbh/snap;rb=Xb*X(1,:)'/snap;
Wblp=inv(Rb)*rb;
%%%%%%%%%%雙向預測算法
Xfbt=[Xft;conj(Xbt)];Xfb=transpose(Xfbt);Xfbh=Xfb';
Rfb=Xfb*Xfbh/snap;rfb=Xfb*[X(M,:)';transpose(X(1,:))]/snap;
Wfblp=conj(inv(Rfb)*rfb);

%%%@@@@@@@@@@@@@@@@下面為多階線性預測算法
p=10;P=M-p;%線性預測的階數
J=fliplr(eye(P));%P階交換矩陣
RF0=zeros(P,P);%前向預測的協方差陣
Rf0=zeros(P,1);
af0=zeros(P,1);
RB=zeros(P,P);%后向預測的協方差陣
Rb=zeros(P,1);


theta=-90:0.01:90;
for ii=1:length(theta)
    a_theta=exp(j*2*pi*d/lamda*a*sin(theta(ii)/180*pi));
    Pflp(ii)=10*log10(1/(abs(a_theta'*[1;-Wflp]))^2);
    Pblp(ii)=10*log10(1/(abs(a_theta'*[1;-Wblp]))^2);   
    Pfblp(ii)=10*log10(1/(abs(a_theta'*[1;-Wfblp]))^2); 
    for jj=1:p
        RF=J*Rx(jj:P+jj-1,jj:P+jj-1)*J;
        Rf=J*Rx(jj:P+jj-1,P+jj);
        WFLP=conj((inv(RF)*Rf));
        PFLP(jj,ii)=10*log10(1/(abs(a_theta(1:P+1)'*[1;-WFLP]))^2);
        RB=Rx(jj+1:P+jj,jj+1:P+jj);
        Rb=Rx(jj+1:P+jj,jj);
        WBLP=(inv(RB)*Rb);
        PBLP(jj,ii)=10*log10(1/(abs(a_theta(1:P+1)'*[1;-WBLP]))^2); 
        RFB=RF+conj(RB);
        Rfb=Rf+conj(Rb);
        WFBLP=conj((inv(RFB)*Rfb));   
        PFBLP(jj,ii)=10*log10(1/(abs(a_theta(1:P+1)'*[1;-WFBLP]))^2);
    end          
end

figure(1);
plot(theta,Pflp);grid on;xlabel('角度');ylabel('峰值');title('一階前向預測');
figure(2);
plot(theta,Pblp);grid on;xlabel('角度');ylabel('峰值');title('一階后向預測');
figure(3);
plot(theta,Pfblp);grid on;xlabel('角度');ylabel('峰值');title('一階雙向預測');
figure(4);
plot(theta,sum(PFLP,1)/p);grid on;xlabel('角度');ylabel('峰值');title('多階前向預測');
figure(5);
plot(theta,sum(PBLP,1)/p);grid on;xlabel('角度');ylabel('峰值');title('多階后向預測');
figure(6);
plot(theta,sum(PFBLP,1)/p);grid on;xlabel('角度');ylabel('峰值');title('多階雙向預測');
toc