-
Notifications
You must be signed in to change notification settings - Fork 5
/
Copy pathP_GSL.m
118 lines (102 loc) · 3.38 KB
/
P_GSL.m
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
function mu=P_GSL(Y, fs)
%% The proposed P-GSL method
%% Estimate the period
N_orginal=size(Y,1);
P=1/search_P(Y,fs);
R=fs*P;
L= floor(N_orginal/R);
Ini_pints=[0:L-1]*R+1;
Ini_pints=round(Ini_pints);
R=round(R);
y_new=[];
index_record=[];
for ii=1:length(Ini_pints)
y_new=[y_new;Y(Ini_pints(ii):Ini_pints(ii)+ R-1 )];
index_record=[index_record; [Ini_pints(ii):Ini_pints(ii)+ R-1]' ];
end
%% Normalized
[N,T]=size(y_new);
Norm_y=norm(y_new,'fro')/sqrt(N*T);
y_new=y_new/Norm_y;
%% Initialization
a=1e-10;
b=a;
gamma_with_tau=ones(N,1);
tau=ones(L,1);
Z=ones(N,3)/3;
alpha=1;
iter = 0;
maxiter=100;
rho=0.5;
converged=false;
while ~converged
%% Update x
deltal=[gamma_with_tau(2:length(gamma_with_tau));gamma_with_tau(1)];
deltar=[gamma_with_tau(end);gamma_with_tau(1:length(gamma_with_tau)-1)];
gamma_with_tau=( Z(:,1).*deltar + Z(:,2).*gamma_with_tau + Z(:,3).*deltal );
Sigma=1./(alpha + gamma_with_tau );
mu= alpha*( Sigma.* y_new);
%% Update alpha
resid=y_new-mu;
alpha=( T*N + 2*a )/( 2*b + norm(resid, 'fro')^2 + T* sum(Sigma) );
%% Update gamma
sum_mu=sum( mu.*conj(mu), 2);
mu2=sum_mu + T*real(Sigma);
mu2l=[mu2(2:length(mu2));mu2(1)];
mu2r=[mu2(end);mu2(1:length(mu2)-1)];
Z_2=Z(:,2);
Z_1l=[Z(2:length(mu2),1);Z(1,1)];
Z_3r=[Z(end,3);Z(1:length(mu2)-1,3)];
mu2_combine= Z_1l.*mu2l + Z_2.*mu2 + Z_3r.*mu2r;
mu2_reshape=reshape(mu2_combine, R, L );
Z_shape=reshape((Z_1l + Z_2 + Z_3r), R, L );
c_k=( sum(Z_shape,2)*T+ 2*a );
d_k=( sum(mu2_reshape*diag(tau),2) + 2*b );
gamma_core=c_k./d_k;
gamma_with_tau=kron(tau,gamma_core);
ln_gamma_core= psi( c_k ) - log( d_k );
ln_delta_with_ones=kron(ones(L,1),ln_gamma_core) ;
%% Update Z
Z_old=Z;
ln_deltal=[ln_delta_with_ones(2:length(ln_delta_with_ones));ln_delta_with_ones(1)];
ln_deltar=[ln_delta_with_ones(end);ln_delta_with_ones(1:length(ln_delta_with_ones)-1)];
deltal=[gamma_with_tau(2:length(gamma_with_tau));gamma_with_tau(1)];
deltar=[gamma_with_tau(end);gamma_with_tau(1:length(gamma_with_tau)-1)];
t1 = ln_deltar - mu2.*deltar ;
t2 = ln_delta_with_ones - mu2.*gamma_with_tau ;
t3 = ln_deltal - mu2.*deltal ;
et=[t1,t2,t3]/2;
temp_p= exp(et);
Z= (( 1./sum(temp_p,2) )*ones(1,3)).* temp_p;
Z=rho*Z + (1-rho)*Z_old; % damping
if iter>=30
[~,ind_sort]=sort(gamma_core,'descend');
gamma_core(ind_sort(1:round(end*0.8)))=1e3;
if iter>=maxiter-5
gamma_core(ind_sort(1:round(end*0.8)))=1e5;
end
end
% figure(10);plot(1./gamma_core)
%% Update tau
delta_11=kron(ones(L,1),gamma_core);
deltal=[delta_11(2:length(delta_11));delta_11(1)];
deltar=[delta_11(end);delta_11(1:length(delta_11)-1)];
delta_temp=( Z(:,1).*deltar + Z(:,2).*delta_11 + Z(:,3).*deltal );
temp2=reshape(delta_temp.*mu2, R, L );
cl= sum(temp2,1)';
tau_old=tau;
tau= ( R*T )./( cl );
tau=rho*tau+ (1-rho)*tau_old; % damping
gamma_with_tau=kron(tau,gamma_core);
if iter >= maxiter
converged = true;
end
iter = iter + 1;
end
mu=mu*Norm_y;
%% calibrate the position
y_res=zeros(N_orginal,T);
for ii=1:length(mu)
y_res(index_record(ii))=mu(ii) ;
end
mu=y_res;