% misorientation calbiration
clear all;
close all;
clc;

j_2p=j*2*pi;
degrad=pi/180;
snapshot=800;
m=3;


% get calbiration matrix
A=[v_p(10,30),v_p(20,40),v_p(30,60)];
pt=[1 0 0;0 cos(degrad*3) sin(degrad*3);0 -sin(degrad*3) cos(degrad*3)]*[cos(degrad*5) -sin(degrad*5) 0;sin(degrad*5) cos(degrad*5) 0;0 0 1];


s(1,:)=exp(j_2p*5/25*[0:snapshot-1]);
s(2,:)=exp(j_2p*7/25*[0:snapshot-1]);
s(3,:)=exp(j_2p*9/25*[0:snapshot-1]);

ss(1,:)=exp(j_2p*5*[1:snapshot]);
ss(2,:)=exp(j_2p*7*[1:snapshot]);
ss(3,:)=exp(j_2p*9*[1:snapshot]);

A1(:,1)=v_a(10,30,15,90);
A1(:,2)=v_a(20,40,25,90);
A1(:,3)=v_a(30,60,35,90);
A2=A1*[exp(j_2p*5/25*.20),0,0; 0,exp(j_2p*7/25*.20),0; 0,0,exp(j_2p*9/25*.20)];

Noise=(10^(-(20)/20))*v_noise(12,snapshot)/sqrt(2);
%% the first signal
z1=[pt,zeros(3,3);zeros(3,3),pt]*A1(:,1)*s(1,:);
z2=[pt,zeros(3,3);zeros(3,3),pt]*A2(:,1)*s(1,:);
z=[z1;z2]+Noise;

R=1/snapshot*z*z';

[u v ]=eig(R);

a1=u(1:6,1:1);
a2=u(7:12,1:1);

w=inv(a1'*a1)*a1'*a2;
[T WW ]=eig(w);

re_A=a1*inv(T);

mis_p(:,1)=cross(re_A(1:3,1),conj(re_A(4:6,1)))/(norm(re_A(1:3,1))*norm(re_A(4:6,1)));


%%%% the second signal
z1=[pt,zeros(3,3);zeros(3,3),pt]*A1(:,2)*s(2,:);
z2=[pt,zeros(3,3);zeros(3,3),pt]*A2(:,2)*s(2,:);
z=[z1;z2]+Noise;

R=1/snapshot*z*z';

[u v ]=eig(R);

a1=u(1:6,1:1);
a2=u(7:12,1:1);

w=inv(a1'*a1)*a1'*a2;
[T WW ]=eig(w);

re_A=a1*inv(T);

mis_p(:,2)=cross(re_A(1:3,1),conj(re_A(4:6,1)))/(norm(re_A(1:3,1))*norm(re_A(4:6,1)));

% the third signal 
z1=[pt,zeros(3,3);zeros(3,3),pt]*A1(:,3)*s(3,:);
z2=[pt,zeros(3,3);zeros(3,3),pt]*A2(:,3)*s(3,:);
z=[z1;z2]+Noise;

R=1/snapshot*z*z';

[u v ]=eig(R);

a1=u(1:6,1:1);
a2=u(7:12,1:1);

w=inv(a1'*a1)*a1'*a2;
[T WW ]=eig(w);

re_A=a1*inv(T);

mis_p(:,3)=cross(re_A(1:3,1),conj(re_A(4:6,1)))/(norm(re_A(1:3,1))*norm(re_A(4:6,1)));

%%% end of signal 

% get calbiration matrix
R_c=mis_p*inv(A);

% misorientation array parameters estimation
%  sinals



a(:,1)=v_a(30.93,37.09,45,90);
a(:,2)=v_a(50.08,39.71,45,-90);

%Poynting vectors
p(:,1)=cross(a(1:3,1),conj(a(4:6,1)));
p(:,2)=cross(a(1:3,2),conj(a(4:6,2)));

% position and phase delay
p_s=[0 0 0;0.5 0 0;0 0.5 0;1.35 0 0;-1.35 0 0;0 0.5 0;0 -0.5 0;0 1.35 0;0 -1.35 0;2 2 0;2 -2 0;-2 -2 0;-2 2 0];
for i=1:m
    q(i,1)=exp(j_2p*p_s(i,1)*p(1,1))*exp(j_2p*p_s(i,2)*p(2,1))*exp(j_2p*p_s(i,3)*p(3,1));
    q(i,2)=exp(j_2p*p_s(i,1)*p(1,2))*exp(j_2p*p_s(i,2)*p(2,2))*exp(j_2p*p_s(i,3)*p(3,2));
end

% receive signals
%###########
A=[kron(q(:,1),a(:,1)),kron(q(:,2),a(:,2))];

% the fifth vector sensor misorientation
A(7:12,:)=[R_c,zeros(3,3);zeros(3,3),R_c]*A(7:12,:);

%%% monte_loop
%for SNR=0:5:30
for loop=1:500
z=A*s(1:2,:)+(10^(-(20)/20))*v_noise(6*m,snapshot)/sqrt(2);

%%%  reosrt signals
for ii=1:6
    for jj=1:m
        chang((ii-1)*m+jj,:)=z(ii+(jj-1)*6,:);
    end
end
z=chang;

R=1/snapshot*z*z';

%獲得信號子空間的6個子空間
%[u s]=eig(R);
[es bb cc]=svd(R);

Es=es(:,1:2);
Es_1=v_j(1,m)*Es;
Es_2=v_j(2,m)*Es;
Es_3=v_j(3,m)*Es;
Es_4=v_j(4,m)*Es;
Es_5=v_j(5,m)*Es;
Es_6=v_j(6,m)*Es;

%獲得5個旋轉(zhuǎn)不變量
w1_2=inv(Es_1'*Es_1)*(Es_1'*Es_2);
w2_3=inv(Es_2'*Es_2)*(Es_2'*Es_3);
w3_4=inv(Es_3'*Es_3)*(Es_3'*Es_4);
w4_5=inv(Es_4'*Es_4)*(Es_4'*Es_5);
w5_6=inv(Es_5'*Es_5)*(Es_5'*Es_6);

%獲得陣列導向矢量的五個不變量
[u x1_2]=eig(w1_2);
[u x2_3]=eig(w2_3); 
[u x3_4]=eig(w3_4); 
[u x4_5]=eig(w4_5); 
[u x5_6]=eig(w5_6); 

%獲得信號Poynting矢量的估計值
tmp_1_1=[1;x1_2(1,1);x1_2(1,1)*x2_3(1,1)];
tmp_1_2=[x1_2(1,1)*x2_3(1,1)*x3_4(1,1);x1_2(1,1)*x2_3(1,1)*x3_4(1,1)*x4_5(1,1);x1_2(1,1)*x2_3(1,1)*x3_4(1,1)*x4_5(1,1)*x5_6(1,1)];
tmp_2_1=[1;x1_2(2,2);x1_2(2,2)*x2_3(2,2)];
tmp_2_2=[x1_2(2,2)*x2_3(2,2)*x3_4(2,2);x1_2(2,2)*x2_3(2,2)*x3_4(2,2)*x4_5(2,2);x1_2(2,2)*x2_3(2,2)*x3_4(2,2)*x4_5(2,2)*x5_6(2,2)];
p_e1=cross(tmp_1_1,conj(tmp_1_2))/sqrt(tmp_1_1'*tmp_1_1)/sqrt(tmp_1_2'*tmp_1_2);
p_e2=cross(tmp_2_1,conj(tmp_2_2))/sqrt(tmp_2_1'*tmp_2_1)/sqrt(tmp_2_2'*tmp_2_2);
%p_e1=inv(c_R)*p_e1
%p_e2=inv(c_R)*p_e2


%仰俯角和方位角,1為仰俯角，2為方位角


ang(1,1)=asin(norm(p_e1(1:2)))*180/pi;
ang(1,2)=atan((p_e1(2)/p_e1(1)))*180/pi;
ang(2,1)=asin(norm(p_e2(1:2)))*180/pi;
ang(2,2)=atan((p_e2(2)/p_e2(1)))*180/pi;




h1=[cos(ang(1,2)*pi/180)*cos(ang(1,1)*pi/180) -sin(ang(1,2)*pi/180)
    sin(ang(1,2)*pi/180)*cos(ang(1,1)*pi/180)  cos(ang(1,2)*pi/180)
   -sin(ang(1,1)*pi/180)                        0
   -sin(ang(1,2)*pi/180)                       -cos(ang(1,2)*pi/180)*cos(ang(1,1)*pi/180)
    cos(ang(1,2)*pi/180)                       -sin(ang(1,2)*pi/180)*cos(ang(1,1)*pi/180)
    0                                           sin(ang(1,1)*pi/180)];

h2=[cos(ang(2,2)*pi/180)*cos(ang(2,1)*pi/180) -sin(ang(2,2)*pi/180)
    sin(ang(2,2)*pi/180)*cos(ang(2,1)*pi/180)  cos(ang(2,2)*pi/180)
   -sin(ang(2,1)*pi/180)                        0
   -sin(ang(2,2)*pi/180)                       -cos(ang(2,2)*pi/180)*cos(ang(2,1)*pi/180)
    cos(ang(2,2)*pi/180)                       -sin(ang(2,2)*pi/180)*cos(ang(2,1)*pi/180)
    0                                           sin(ang(2,1)*pi/180)];

a1=1/sqrt((tmp_1_1'*tmp_1_1)*(tmp_1_2'*tmp_1_2))*[1;x1_2(1,1);x1_2(1,1)*x2_3(1,1);x1_2(1,1)*x2_3(1,1)*x3_4(1,1);x1_2(1,1)*x2_3(1,1)*x3_4(1,1)*x4_5(1,1);x1_2(1,1)*x2_3(1,1)*x3_4(1,1)*x4_5(1,1)*x5_6(1,1)];
a2=1/sqrt((tmp_2_1'*tmp_2_1)*(tmp_2_2'*tmp_2_2))*[1;x1_2(2,2);x1_2(2,2)*x2_3(2,2);x1_2(2,2)*x2_3(2,2)*x3_4(2,2);x1_2(2,2)*x2_3(2,2)*x3_4(2,2)*x4_5(2,2);x1_2(2,2)*x2_3(2,2)*x3_4(2,2)*x4_5(2,2)*x5_6(2,2)];


G1=inv(h1'*h1)*h1'*a1;
G2=inv(h2'*h2)*h2'*a2;

%計算兩信號的參數(shù)，，3為輔助極化角，4為相位差
ang(1,3)=(atan(abs(G1(1)/G1(2)))*180/pi);
ang(1,4)=((angle(G1(1))-angle(G1(2)))*180/pi);

ang(2,3)=(atan(abs(G2(1)/G2(2)))*180/pi);
ang(2,4)=((angle(G2(1))-angle(G2(2)))*180/pi);

%% sort
if sin(ang(1,1)*degrad)>sin(ang(2,1)*degrad)
    temp=ang(1,:);
    ang(1,:)=ang(2,:);
    ang(2,:)=temp;
end
misorientation_angle(:,:,loop)=(ang);
end

%%% end loop

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%% remedy calbiration
%###############################
for loop=1:500
z=A*s(1:2,:)+(10^(-(20)/20))*v_noise(6*m,snapshot)/sqrt(2);
%  remedy matrix
remedy=[eye(6,6),zeros(6,12);zeros(6,6),inv([R_c,zeros(3,3);zeros(3,3),R_c]),zeros(6,6);zeros(6,12),eye(6,6)];
z_remedy=remedy*A*s(1:2,:)+(10^(-(20)/20))*v_noise(6*m,snapshot)/sqrt(2);
%%%  reosrt signals
for ii=1:6
    for jj=1:m
        chang((ii-1)*m+jj,:)=z_remedy(ii+(jj-1)*6,:);
    end
end
z_remedy=chang;

R=1/snapshot*z_remedy*z_remedy';

%獲得信號子空間的6個子空間
%[u s]=eig(R);
[es bb cc]=svd(R);

Es=es(:,1:2);
Es_1=v_j(1,m)*Es;
Es_2=v_j(2,m)*Es;
Es_3=v_j(3,m)*Es;
Es_4=v_j(4,m)*Es;
Es_5=v_j(5,m)*Es;
Es_6=v_j(6,m)*Es;

%獲得5個旋轉(zhuǎn)不變量
w1_2=inv(Es_1'*Es_1)*(Es_1'*Es_2);
w2_3=inv(Es_2'*Es_2)*(Es_2'*Es_3);
w3_4=inv(Es_3'*Es_3)*(Es_3'*Es_4);
w4_5=inv(Es_4'*Es_4)*(Es_4'*Es_5);
w5_6=inv(Es_5'*Es_5)*(Es_5'*Es_6);

%獲得陣列導向矢量的五個不變量
[u x1_2]=eig(w1_2);
[u x2_3]=eig(w2_3); 
[u x3_4]=eig(w3_4); 
[u x4_5]=eig(w4_5); 
[u x5_6]=eig(w5_6); 

%獲得信號Poynting矢量的估計值
tmp_1_1=[1;x1_2(1,1);x1_2(1,1)*x2_3(1,1)];
tmp_1_2=[x1_2(1,1)*x2_3(1,1)*x3_4(1,1);x1_2(1,1)*x2_3(1,1)*x3_4(1,1)*x4_5(1,1);x1_2(1,1)*x2_3(1,1)*x3_4(1,1)*x4_5(1,1)*x5_6(1,1)];
tmp_2_1=[1;x1_2(2,2);x1_2(2,2)*x2_3(2,2)];
tmp_2_2=[x1_2(2,2)*x2_3(2,2)*x3_4(2,2);x1_2(2,2)*x2_3(2,2)*x3_4(2,2)*x4_5(2,2);x1_2(2,2)*x2_3(2,2)*x3_4(2,2)*x4_5(2,2)*x5_6(2,2)];
p_e1=cross(tmp_1_1,conj(tmp_1_2))/sqrt(tmp_1_1'*tmp_1_1)/sqrt(tmp_1_2'*tmp_1_2);
p_e2=cross(tmp_2_1,conj(tmp_2_2))/sqrt(tmp_2_1'*tmp_2_1)/sqrt(tmp_2_2'*tmp_2_2);
%p_e1=inv(c_R)*p_e1
%p_e2=inv(c_R)*p_e2


%仰俯角和方位角,1為仰俯角，2為方位角


ang(1,1)=asin(norm(p_e1(1:2)))*180/pi;
ang(1,2)=atan((p_e1(2)/p_e1(1)))*180/pi;
ang(2,1)=asin(norm(p_e2(1:2)))*180/pi;
ang(2,2)=atan((p_e2(2)/p_e2(1)))*180/pi;




h1=[cos(ang(1,2)*pi/180)*cos(ang(1,1)*pi/180) -sin(ang(1,2)*pi/180)
    sin(ang(1,2)*pi/180)*cos(ang(1,1)*pi/180)  cos(ang(1,2)*pi/180)
   -sin(ang(1,1)*pi/180)                        0
   -sin(ang(1,2)*pi/180)                       -cos(ang(1,2)*pi/180)*cos(ang(1,1)*pi/180)
    cos(ang(1,2)*pi/180)                       -sin(ang(1,2)*pi/180)*cos(ang(1,1)*pi/180)
    0                                           sin(ang(1,1)*pi/180)];

h2=[cos(ang(2,2)*pi/180)*cos(ang(2,1)*pi/180) -sin(ang(2,2)*pi/180)
    sin(ang(2,2)*pi/180)*cos(ang(2,1)*pi/180)  cos(ang(2,2)*pi/180)
   -sin(ang(2,1)*pi/180)                        0
   -sin(ang(2,2)*pi/180)                       -cos(ang(2,2)*pi/180)*cos(ang(2,1)*pi/180)
    cos(ang(2,2)*pi/180)                       -sin(ang(2,2)*pi/180)*cos(ang(2,1)*pi/180)
    0                                           sin(ang(2,1)*pi/180)];

a1=1/sqrt((tmp_1_1'*tmp_1_1)*(tmp_1_2'*tmp_1_2))*[1;x1_2(1,1);x1_2(1,1)*x2_3(1,1);x1_2(1,1)*x2_3(1,1)*x3_4(1,1);x1_2(1,1)*x2_3(1,1)*x3_4(1,1)*x4_5(1,1);x1_2(1,1)*x2_3(1,1)*x3_4(1,1)*x4_5(1,1)*x5_6(1,1)];
a2=1/sqrt((tmp_2_1'*tmp_2_1)*(tmp_2_2'*tmp_2_2))*[1;x1_2(2,2);x1_2(2,2)*x2_3(2,2);x1_2(2,2)*x2_3(2,2)*x3_4(2,2);x1_2(2,2)*x2_3(2,2)*x3_4(2,2)*x4_5(2,2);x1_2(2,2)*x2_3(2,2)*x3_4(2,2)*x4_5(2,2)*x5_6(2,2)];


G1=inv(h1'*h1)*h1'*a1;
G2=inv(h2'*h2)*h2'*a2;

%計算兩信號的參數(shù)，，3為輔助極化角，4為相位差
ang(1,3)=(atan(abs(G1(1)/G1(2)))*180/pi);
ang(1,4)=((angle(G1(1))-angle(G1(2)))*180/pi);

ang(2,3)=(atan(abs(G2(1)/G2(2)))*180/pi);
ang(2,4)=((angle(G2(1))-angle(G2(2)))*180/pi);

%% sort
if sin(ang(1,1)*degrad)>sin(ang(2,1)*degrad)
    temp=ang(1,:);
    ang(1,:)=ang(2,:);
    ang(2,:)=temp;
end
remedy_angle(:,:,loop)=(ang);
end  % end of loop
%end %end of SNR

%%%%%%%
re_mis_ang(1,1)=mean(misorientation_angle(1,1,:));
re_mis_ang(1,2)=mean(misorientation_angle(1,2,:));
re_mis_ang(1,3)=mean(misorientation_angle(1,3,:));
re_mis_ang(1,4)=mean(misorientation_angle(1,4,:));

re_mis_ang(2,1)=mean(misorientation_angle(2,1,:));
re_mis_ang(2,2)=mean(misorientation_angle(2,2,:));
re_mis_ang(2,3)=mean(misorientation_angle(2,3,:));
re_mis_ang(2,4)=mean(misorientation_angle(2,4,:));


re_re_ang(1,1)=mean(remedy_angle(1,1,:));
re_re_ang(1,2)=mean(remedy_angle(1,2,:));
re_re_ang(1,3)=mean(remedy_angle(1,3,:));
re_re_ang(1,4)=mean(remedy_angle(1,4,:));

re_re_ang(2,1)=mean(remedy_angle(2,1,:));
re_re_ang(2,2)=mean(remedy_angle(2,2,:));
re_re_ang(2,3)=mean(remedy_angle(2,3,:));
re_re_ang(2,4)=mean(remedy_angle(2,4,:));

% bias
bias_mis(1,1)=std(misorientation_angle(1,1,:));
bias_mis(1,2)=std(misorientation_angle(1,2,:));
bias_mis(1,3)=std(misorientation_angle(1,3,:));
bias_mis(1,4)=std(misorientation_angle(1,4,:));

bias_mis(2,1)=std(misorientation_angle(2,1,:));
bias_mis(2,2)=std(misorientation_angle(2,2,:));
bias_mis(2,3)=std(misorientation_angle(2,3,:));
bias_mis(2,4)=std(misorientation_angle(2,4,:));

bias_re(1,1)=std(remedy_angle(1,1,:));
bias_re(1,2)=std(remedy_angle(1,2,:));
bias_re(1,3)=std(remedy_angle(1,3,:));
bias_re(1,4)=std(remedy_angle(1,4,:));

bias_re(2,1)=std(remedy_angle(2,1,:));
bias_re(2,2)=std(remedy_angle(2,2,:));
bias_re(2,3)=std(remedy_angle(2,3,:));
bias_re(2,4)=std(remedy_angle(2,4,:));


% figure
% misorientation
%####################
% the first signal ,elevation ,azimuth
figure
plot(20:40,30:50,'w');
hold on;
plot(re_mis_ang(1,1),re_mis_ang(1,2),'*');
hold on;
plot(30.93,37.09,'square');
xlabel('俯仰角(deg)');
ylabel('方位角(deg)');
title('信號一校正前到達角估計值與真實值關系')
print -djpeg paper_3_1;

% the second signal ,elevation ,azimuth
figure
plot(40:60,30:50,'w');
hold on;
plot(re_mis_ang(2,1),re_mis_ang(2,2),'*');
hold on;
plot(50.08,39.71,'square');
xlabel('俯仰角(deg)');
ylabel('方位角(deg)');
title('信號二校正前到達角估計值與真實值關系')
print -djpeg paper_3_2;

% remedy
%######################
% the first signal ,elevation ,azimuth
figure
plot(20:40,30:50,'w');
hold on;
plot(re_re_ang(1,1),re_re_ang(1,2),'*');
hold on;
plot(30.93,37.09,'square');
xlabel('俯仰角(deg)');
ylabel('方位角(deg)');
title('信號一校正后到達角估計值與真實值關系')
print -djpeg paper_3_3;

% the second signal ,elevation ,azimuth
figure
plot(40:60,30:50,'w');
hold on;
plot(re_re_ang(2,1),re_re_ang(2,2),'*');
hold on;
plot(50.08,39.71,'square');
xlabel('俯仰角(deg)');
ylabel('方位角(deg)');
title('信號二校正后到達角估計值與真實值關系')
print -djpeg paper_3_4;