%%%%%%@@@@@@@@@
%%%%%%書本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=-40;
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*(f0*t+0.5*u2*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=conj((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);

for ii=1:p
    RF0=RF0+Rx(ii:P+ii-1,ii:P+ii-1);
    Rf0=Rx(ii:P+ii-1,P+ii);
    RB=RB+Rx(ii+1:P+ii,ii+1:P+ii);
    Rb=Rx(ii+1:P+ii,ii);
end
RF=J*RF0*J;Rf=J*Rf0;
WFLP=conj((inv(RF)*Rf));
WBLP=conj((inv(RB)*Rb));
RFB=RF+RB;Rfb=Rf+conj(Rb);
WFBLP=conj((inv(RFB)*Rfb));

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);  
    PFLP(ii)=10*log10(1/(abs(a_theta(1:P+1)'*[1;-WFLP]))^2);
    PBLP(ii)=10*log10(1/(abs(a_theta(1:P+1)'*[1;-WBLP]))^2); 
    PFBLP(ii)=10*log10(1/(abs(a_theta(1:P+1)'*[1;-WFBLP]))^2);  
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,PFLP);grid on;xlabel('角度');ylabel('峰值');title('多階前向預測');
figure(5);
plot(theta,PBLP);grid on;xlabel('角度');ylabel('峰值');title('多階后向預測');
figure(6);
plot(theta,PFBLP);grid on;xlabel('角度');ylabel('峰值');title('多階雙向預測');
toc