diff --git a/.gitattributes b/.gitattributes
new file mode 100644
index 0000000..dfe0770
--- /dev/null
+++ b/.gitattributes
@@ -0,0 +1,2 @@
+# Auto detect text files and perform LF normalization
+* text=auto
diff --git a/README.md b/README.md
new file mode 100644
index 0000000..1313b34
--- /dev/null
+++ b/README.md
@@ -0,0 +1,2 @@
+# stateSpacePhasePredictor
+ Estimating phase in real time with a state space model tracking the analytic signal 
diff --git a/causalPhaseEM_MKmdl.m b/causalPhaseEM_MKmdl.m
new file mode 100644
index 0000000..4fe7fca
--- /dev/null
+++ b/causalPhaseEM_MKmdl.m
@@ -0,0 +1,142 @@
+% causal phase estimates using the SP model and EM with fixed interval
+% smoothing across windows of data
+% primary benefit: assume a transitory burst of oscillatory activity in the
+% range of your bandpass filter/ assume a peak shift in the data towards the edge
+% of the band pass filter. These are problems unaddressed for instantaneous
+% phase estimation right now
+% Potential extension: a diffuse prior on x to estimate all frequencies? 
+
+% Algorithm:
+% after estimating reasonable initialization points we need to run EM on
+% the data - at whatever rate makes it possible to run it again before
+% getting the next window of data. So while it might be slow here, a C++
+% implementation is likely going to be much faster meaning we have have
+% small windows (up to the frequency limits of course)
+
+% for the prototype all I need to do i run it on data already collected.
+% And split it into whatever sized segments make sense. And then run three
+% methods on it to show that the EM approach works best given a certain
+% type of data. 
+
+% all the pieces should be run together:
+% 1. Initialization - Using the MK initialization approach
+% 2. Use the EM to estimate parameters from the first window
+% 3 Kalman filter with the latest parameter estiamtes
+% Last edit: Ani Wodeyar 3/17/2020
+
+function [phase,phaseBounds,allX_full] = causalPhaseEM_MKmdl(y,initParams)
+
+freqs = initParams.freqs;
+Fs = initParams.Fs;
+ampVec = initParams.ampVec;
+sigmaFreqs = initParams.sigmaFreqs;
+sigmaObs = initParams.sigmaObs;
+windowSize = initParams.window;
+lowFreqBand = initParams.lowFreqBand;
+
+if windowSize < Fs
+    disp('The window size needs to be different. Setting it equal to sampling rate')
+    windowSize = Fs;
+end
+
+if length(y) < 2*windowSize
+    disp('please enter a larger vector of observations (should be at leas 2x as big as window size)')
+    return
+end
+
+numSegments = floor(length(y)/windowSize);
+
+data = y(1:windowSize);
+% first run to set up parameters
+[omega, ampEst, allQ, R, stateVec, stateCov] = fit_MKModel_multSines(data,freqs, Fs,ampVec, sigmaFreqs,sigmaObs);
+lowFreqLoc = find((omega>lowFreqBand(1)) & (omega<lowFreqBand(2)),1);
+ 
+if isempty(lowFreqLoc)
+    disp('Low freq band limits incorrect OR there is no low freq signal; retaining initial params')
+    omega = freqs;
+    ampEst = ampVec;
+    allQ = sigmaFreqs;
+    [~,lowFreqLoc] = min(abs(freqs-mean(lowFreqBand))); % pick frequency closest to middle of low frequency range
+end
+
+% for loop that runs through rest of the data reestimating parameters after
+% generating phase estimates for the whole period using past parameter ests
+% and the kalman filter
+[phi, Q, M] = genParametersSoulatMdl_sspp(omega, Fs, ampEst, allQ);
+phase = zeros(numSegments, windowSize);
+phaseBounds = zeros(numSegments, windowSize,2);
+allX_full = zeros(numSegments, windowSize, 2);
+
+for seg = 2:numSegments
+    
+    y_thisRun = y((seg-1)*windowSize + 1: seg*windowSize);
+    % running Kalman filter over one window before re-running EM
+    % start below with the end of the EM run x and stae cov
+    allX = zeros(length(freqs)*2, windowSize);
+    allP = zeros(length(freqs)*2,length(freqs)*2, windowSize);
+    
+    x = stateVec(:,end);
+    P = squeeze(stateCov(:,:,end));
+    
+    for i = 1:(length(y_thisRun)-1)
+        % kalman update
+        [x_new,P_new] = oneStepKFupdate_sspp(x,y_thisRun(i),phi,M,Q,R,P);
+        allX(:,i) = x_new;
+        P_new = (P_new + P_new') /2; % forcing symmetry to kill off rounding errors
+        allP(:,:,i) = P_new; 
+        
+        % estimate phase
+        phase(seg, i) = angle(x_new(lowFreqLoc*2-1) + 1i* x_new(lowFreqLoc*2));
+        samples = mvnrnd(x_new(lowFreqLoc*2-1:lowFreqLoc*2), P_new(lowFreqLoc*2-1:lowFreqLoc*2,lowFreqLoc*2-1:lowFreqLoc*2),1000);
+        sampleAngles = angle(samples(:,1) + 1i*samples(:,2));
+        tmpAngleStd = abs(wrapToPi((prctile(sampleAngles,97.5) - prctile(sampleAngles,2.5)))/2);
+        phaseBounds(seg,i,:) = sort([(phase(seg,i)- tmpAngleStd), (phase(seg,i) + tmpAngleStd)]);
+        
+%         sampleAmp = abs(samples(:,1) + 1i*samples(:,2));
+%         amp(seg,i) = std(sampleAmp);
+
+        % update state and state cov
+        P = P_new;
+        x = x_new;
+    end
+    % ending it on N-1
+    [x_new,P_new] = oneStepKFupdate_sspp(x,y_thisRun(i),phi,M,Q,R,P);
+    allX(:,i+1) = x_new;
+    allP(:,:,i+1) = P_new;
+    
+    % estimate phase
+    phase(seg, i) = angle(x_new(lowFreqLoc*2-1) + 1i* x_new(lowFreqLoc*2));
+    samples = mvnrnd(x_new(lowFreqLoc*2-1:lowFreqLoc*2), P_new(lowFreqLoc*2-1:lowFreqLoc*2,lowFreqLoc*2-1:lowFreqLoc*2),2000);
+    sampleAngles = angle(samples(:,1) + 1i*samples(:,2));
+    tmpAngleStd = abs(wrapToPi((prctile(sampleAngles,97.5) - prctile(sampleAngles,2.5)))/2);
+    phaseBounds(seg,i,:) = sort([(phase(seg,i)- tmpAngleStd), (phase(seg,i) + tmpAngleStd)]);
+    % sset up bounds:
+    tmpLowerBounds = phaseBounds(seg,:,1);
+    tmpUpperBounds = phaseBounds(seg,:,2);
+    phaseBounds(seg,tmpLowerBounds>pi,1) = pi;
+    phaseBounds(seg,tmpLowerBounds<-pi,1) = -pi;
+    phaseBounds(seg,tmpUpperBounds>pi,2) = pi;
+    phaseBounds(seg,tmpUpperBounds<-pi,2) = -pi;
+    
+    allX_full(seg,:,:) = allX(lowFreqLoc*2-1:lowFreqLoc*2,:)';
+    
+    % update below to take in the updated x start and cov start 
+   [freqs, ampVec, sigmaFreqs, R, stateVec, stateCov,] = fit_MKModel_multSines(y_thisRun,omega, Fs, ampEst, allQ, R);
+   tmp = find((freqs>lowFreqBand(1)) & (freqs<lowFreqBand(2)),1);
+
+    if isempty(tmp)
+        disp('Low freq band limits incorrect OR there is no low freq signal; retaining old parameters')       
+    else
+        lowFreqLoc = tmp;
+        omega = freqs;
+        ampEst = ampVec;
+        allQ = sigmaFreqs;
+    end
+   
+   [phi, Q, M] = genParametersSoulatMdl_sspp(omega, Fs, ampEst, allQ);
+    
+end
+    
+
+
+
diff --git a/demo.asv b/demo.asv
new file mode 100644
index 0000000..3682f18
--- /dev/null
+++ b/demo.asv
@@ -0,0 +1,45 @@
+% this is a demo of the causalPhaseEM_MK
+% first generate some data
+% creating a pure sine with some amplitude noise and added pink noise:
+Vlo = (10+ 1*randn(1,(time*Fs))).*cos(2*pi*(6).*[1/Fs:1/Fs:time]); % random movement in amplitude, creates a little bump in PSD
+
+[pn] = make_pink_noise(1,1e4,1/Fs);
+pn = 10*pn;
+data = Vlo +pn;
+truePhase =  wrapToPi((2*pi*(6).*[1/Fs:1/Fs:time]))';
+
+%%
+
+% following helps to initialize more reasonable estimates for the scaling
+% parameter 'a' and the variances for the state vector
+[init_f, init_a,init_sigma,R0] = initializeParams(data,1,1,2000); % s
+
+% setting up initial parameters to start the causalPhaseEM code
+initParams.freqs = 4; 
+initParams.Fs = 1000;
+initParams.ampVec = init_a;
+initParams.sigmaFreqs = init_sigma;
+initParams.sigmaObs = R0;
+initParams.window = 2000;
+initParams.lowFreqBand = [4,8];
+
+[phase,phaseBounds, fullX] = causalPhaseEM_MKmdl(data, initParams);
+phase = reshape(phase', size(phase,1) * size(phase,2),1);
+phaseBounds = reshape(permute(phaseBounds,[2,1,3]), size(phaseBounds,1) * size(phaseBounds,2),size(phaseBounds,3));
+fullX = reshape(permute(fullX,[2,1,3]), size(fullX,1) * size(fullX,2),size(fullX,3));
+
+figure
+err_Spcausal = angle(mean(exp(1i*(phase(2001:end) - truePhase(2001:end)))))*(180/pi);
+imagesc( 1:10000,-3:3, (abs(fullX(:,1) + 1i*fullX(:,2)))', 'AlphaData', .5)
+colormap(summer)
+hold on; plot(1:10000, phase, 'Linewidth', 2, 'Color', 'b')
+plot(squeeze(phaseBounds(:,1)), 'Linewidth', 2, 'color','r')
+hold on
+plot(squeeze(phaseBounds(:,2)), 'Linewidth', 2,'color','r')
+set(gca,'Fontsize', 16)
+xlabel('Time (in samples)')
+ylabel('Phase')
+h = colorbar;
+ylabel(h,'Amplitude')
+title(['Causal Phase Estimate with SP-EM, Error = ',num2str(err_Spcausal)])
+xlim([2000,10000])
diff --git a/demo.m b/demo.m
new file mode 100644
index 0000000..070692c
--- /dev/null
+++ b/demo.m
@@ -0,0 +1,49 @@
+% this is a demo of the causalPhaseEM_MK
+% first generate some data
+% creating a pure sine with some amplitude noise and added pink noise at
+% sampling frequency of 1000 Hz for 10 seconds
+
+Fs = 1000;
+time = 10; 
+Vlo = (10+ 1*randn(1,(time*Fs))).*cos(2*pi*(6).*[1/Fs:1/Fs:time]); % random movement in amplitude, creates a little bump in PSD
+
+[pn] = make_pink_noise(1,1e4,1/Fs);
+pn = 10*pn;
+data = Vlo +pn;
+truePhase =  wrapToPi((2*pi*(6).*[1/Fs:1/Fs:time]))';
+
+%%
+
+% following helps to initialize more reasonable estimates for the scaling
+% parameter 'a' and the variances for the state vector
+[init_f, init_a,init_sigma,R0] = initializeParams(data,1,1,2000); % s
+
+% setting up initial parameters to start the causalPhaseEM code
+initParams.freqs = 4; % initialization using above not great at identifying starting freq
+initParams.Fs = 1000;
+initParams.ampVec = init_a; % in a pinch this can be initialized to 0.99 to start
+initParams.sigmaFreqs = init_sigma; % its important to use a value of this that is in the same ballpark scale
+initParams.sigmaObs = R0;
+initParams.window = 2000;
+initParams.lowFreqBand = [4,8];
+
+[phase,phaseBounds, fullX] = causalPhaseEM_MKmdl(data, initParams);
+phase = reshape(phase', size(phase,1) * size(phase,2),1);
+phaseBounds = reshape(permute(phaseBounds,[2,1,3]), size(phaseBounds,1) * size(phaseBounds,2),size(phaseBounds,3));
+fullX = reshape(permute(fullX,[2,1,3]), size(fullX,1) * size(fullX,2),size(fullX,3));
+
+figure
+err_Spcausal = angle(mean(exp(1i*(phase(2001:end) - truePhase(2001:end)))))*(180/pi);
+imagesc( 1:10000,-3:3, (abs(fullX(:,1) + 1i*fullX(:,2)))', 'AlphaData', .5)
+colormap(summer)
+hold on; plot(1:10000, phase, 'Linewidth', 2, 'Color', 'b')
+plot(squeeze(phaseBounds(:,1)), 'Linewidth', 2, 'color','r')
+hold on
+plot(squeeze(phaseBounds(:,2)), 'Linewidth', 2,'color','r')
+set(gca,'Fontsize', 16)
+xlabel('Time (in samples)')
+ylabel('Phase')
+h = colorbar;
+ylabel(h,'Amplitude')
+title(['Causal Phase Estimate with SP-EM, Error = ',num2str(err_Spcausal)])
+xlim([2000,10000])
diff --git a/fit_MKModel_multSines.m b/fit_MKModel_multSines.m
new file mode 100644
index 0000000..9f90f96
--- /dev/null
+++ b/fit_MKModel_multSines.m
@@ -0,0 +1,139 @@
+
+function [omega, ampEst, allQ, R,stateVec, stateCov] = fit_MKModel_multSines(data,freqs, Fs,ampVec, sigmaFreqs,sigmaObs)
+% fitting the soulat model using the shumway-stoffer method (basically
+% assuming no harmonics); EM has an analytic solution that needs the
+% covariance structure between adjacent time points (this is what we need
+% the fixed interval smoother for) 
+% % This is the extension of the fitSoulatModel code to fit multiple
+% sinusoids, still with no harmonics. 
+% INPUTS:
+% data - currently only accepts vector data so n x 1
+% freqs - initialize with some frequencies that are appropriate for data
+% Fs - sampling frequency
+% sigmaFreqs - what are variances at each of the frequencies specified
+% sibmaObs - what is the expected observation noise variance?
+% Last edit: Ani Wodeyar 3/17/2020
+
+if isempty(sigmaFreqs)
+    sigmaFreqs = 0.1*ones(length(freqs),1);
+end
+if isempty(sigmaObs)
+    sigmaObs = 10;
+end
+y = data;
+
+freqEst = freqs/Fs;
+% need to initialize all my parameters
+
+xstart = zeros(2*length(freqs),1);
+Pstart = .001 * eye(2*length(freqs));
+x = xstart;
+P = Pstart;
+
+[phi, Q, M] = genParametersSoulatMdl_sspp(freqs, Fs, ampVec, sigmaFreqs); 
+R = sigmaObs;
+
+iter = 1;
+errorVal = Inf;
+%iterate though maximizing likelihood
+
+while iter < 400 && errorVal(iter)>1e-3
+%% run forward KF
+for i = 1:(length(y)-1)
+    
+    [x_new,P_new] = oneStepKFupdate_sspp(x,y(i),phi,M,Q,R,P);
+    allX(:,i) = x_new;
+    allP(:,:,i) = P_new;
+    
+    P = P_new;
+    x = x_new;
+end
+% ending it on N-1
+[x_new,P_new] = oneStepKFupdate_sspp(x,y(i),phi,M,Q,R,P);
+allX(:,i+1) = x_new;
+allP(:,:,i+1) = P_new;
+
+% now we have P_new as P_N_N and P as P_(N-1)_(N-1)
+% same for x
+% can now run the fixed interval smoother
+
+%% run the fixed interval smoother across all data
+
+x_n = x_new;
+P_n = P_new;
+
+newAllX(:, length(y)) = allX(:,length(y));
+newAllP(:,:,length(y)) = allP(:,:,length(y));
+for i = length(y)-1:-1:1
+    [x_backone,P_backone, J_one] =  fixedIntervalSmoother_sspp(x, x_n, P, P_n, phi, Q);
+    x_n = x_backone;
+    P_n = P_backone;
+    x = allX(:,i);
+    P = squeeze(allP(:,:,i));    
+    
+    newAllX(:,i) = x_backone;
+    newAllP(:,:,i) = P_backone;
+    allJ(:,:,i) = J_one;
+end
+% need to estimate P_t_(t-1) for t = 1:N
+P_tmp = phi * squeeze(allP(:,:,end-1)) * phi' +Q;
+K = P_tmp * M * (1/(M' * P_tmp*M + R));
+P_N_N1 = (eye(length(P_tmp)) - K * M') * phi * squeeze(allP(:,:,end-1));
+
+allP_N_N1(:,:,length(y)) = P_N_N1;
+
+for i = length(y)-1:-1:2
+   allP_N_N1(:,:,i)=squeeze(allP(:,:,i)) * squeeze(allJ(:,:,i-1))' + ...
+                    squeeze(allJ(:,:,i))*(squeeze(allP_N_N1(:,:,i+1)) - phi * squeeze(allP(:,:,i))) * squeeze(allJ(:,:,i-1))';
+end
+
+
+%% update the parameter estimate from optimizing exp cond likelihood
+% the portion here is a direct lift from shumway and stoffer rather than
+% Soulat and Purdon
+
+A = Pstart + xstart * xstart';
+B = zeros(size(newAllP,1),size(newAllP,2));
+C = zeros(size(newAllP,1),size(newAllP,2));
+R = zeros(1);
+
+for i = 1:length(y)
+    if i > 1
+        A = A + squeeze(newAllP(:,:,i-1)) + newAllX(:,i-1) *newAllX(:,i-1)';
+        B = B + squeeze(allP_N_N1(:,:,i)) + newAllX(:,i) *newAllX(:,i-1)';
+    end
+    C = C + squeeze(newAllP(:,:,i)) + newAllX(:,i) *newAllX(:,i)';
+    R = R+ M'* squeeze(newAllP(:,:,i)) * M + (y(i) - M'*newAllX(:,i)) *(y(i) - M'*newAllX(:,i))' ;
+end
+
+R = (1/(length(y))) * (R);
+
+oldFreq = freqEst * 1000 /(2*pi);
+
+freqEst = zeros(1,length(freqs));
+ampEst = zeros(1,length(freqs));
+allQ = zeros(1,length(freqs));
+
+for numFreqs = 1: length(freqs)
+    B_tmp = B((numFreqs-1)*2 + 1: numFreqs*2,(numFreqs-1)*2 + 1: numFreqs*2);
+    A_tmp = A((numFreqs-1)*2 + 1: numFreqs*2,(numFreqs-1)*2 + 1: numFreqs*2);
+    C_tmp = C((numFreqs-1)*2 + 1: numFreqs*2,(numFreqs-1)*2 + 1: numFreqs*2);
+    freqEst(numFreqs) = (atan((B_tmp(2,1) - B_tmp(1,2))/trace(B_tmp)));
+    ampEst(numFreqs) = sqrt((B_tmp(2,1) - B_tmp(1,2))^2 + trace(B_tmp)^2)/trace(A_tmp);
+    allQ(numFreqs) = 1/(2*length(y)) * (trace(C_tmp) - ampEst(numFreqs).^2 * trace(A_tmp));
+end
+    
+[phi, Q] = genParametersSoulatMdl_sspp(freqEst * 1000 /(2*pi), Fs, ampEst, allQ); 
+
+% disp('Frequency is:')
+% freqEst * 1000 /(2*pi)
+omega = freqEst * 1000 /(2*pi);
+stateVec = newAllX;
+stateCov = newAllP;
+% phase = angle(newAllX(1,:) + 1i * newAllX(2,:));
+% disp('Error in phase (in ms)')
+% (mean((abs(phase - realPhase))/(2*pi) * (1/6) *1000))
+iter = iter + 1;
+errorVal(iter) =sum(abs(omega - oldFreq));
+end
+
diff --git a/fixedIntervalSmoother_sspp.m b/fixedIntervalSmoother_sspp.m
new file mode 100644
index 0000000..5c4115f
--- /dev/null
+++ b/fixedIntervalSmoother_sspp.m
@@ -0,0 +1,9 @@
+function [x_backone,P_backone, J_one] =  fixedIntervalSmoother_sspp(x,x_n,P, P_n,phi,Q)
+% given a set of estimates generated by the forward Kalman filter, this
+% will work backwards to smooth the estimates
+
+P_one = phi *P * phi' + Q;
+
+J_one = P* phi' * inv(P_one);
+x_backone = x + J_one * (x_n - phi*x);
+P_backone = P + J_one * (P_n - P_one) * J_one';
\ No newline at end of file
diff --git a/genParametersSoulatMdl_sspp.m b/genParametersSoulatMdl_sspp.m
new file mode 100644
index 0000000..2a5f7ac
--- /dev/null
+++ b/genParametersSoulatMdl_sspp.m
@@ -0,0 +1,33 @@
+function [phi, Q, M] = genParametersSoulatMdl_sspp(freqs, Fs,ampVector, sigmaFreqs)
+
+    rotMat = zeros(length(freqs), 2,2);
+    varMat = zeros(length(freqs),2,2);
+    cnt = 1;
+
+    for freqRunThrough = freqs
+        rotMat(cnt,:,:) = createRotMat(freqRunThrough,Fs);
+        rotMat(cnt,:,:) = rotMat(cnt,:,:) * ampVector(cnt);
+        varMat(cnt, :,:) = [sigmaFreqs(cnt), 0; 0 , sigmaFreqs(cnt)];
+        cnt = cnt + 1;
+    end
+    phi = stackBlockMat(rotMat);
+    Q = stackBlockMat(varMat);
+
+    M = reshape([ones(length(freqs),1) , zeros(length(freqs),1)]', length(freqs)*2,1); % for extracting real part of signal from analytic 'x'
+
+     function rotMat = createRotMat(freq, Fs)
+        % create the appropriate rotation matrix for the frequency
+        rotMat = [cos(2*pi*freq/Fs), -sin(2*pi*freq/Fs); ...
+                  sin(2*pi*freq/Fs), cos(2*pi*freq/Fs)];
+    end        
+
+    function fullBlockMat = stackBlockMat(allMat)
+        % pull all the rotation matrices together into a block diagonal
+        % structure -> improves efficiency in computation
+        for i  = 1:size(allMat,1)
+           fullBlockMat(size(allMat,2) * (i-1) + 1:size(allMat,2) * (i), ...
+               size(allMat,2) * (i-1) + 1:size(allMat,2) * (i)) = squeeze(allMat(i,:,:)); %don't do squeeze, do a reshape and extract points we want.
+        end
+    end
+
+end
\ No newline at end of file
diff --git a/initializeParams.m b/initializeParams.m
new file mode 100644
index 0000000..074aaee
--- /dev/null
+++ b/initializeParams.m
@@ -0,0 +1,120 @@
+function [init_f,init_a,init_sigma,R0] = initializeParams(y0,num_component,T1,T2)
+    % borrowed code from the MK model approach to initialize parameters
+    % appropriately.
+    % Last Edit: Ani Wodeyar 2/1/2020
+
+    T0 = length(y0);
+    y = y0(T1:T2);
+    T = length(y);
+    MAX_AR = 2*num_component+1;
+    AR_fit;
+
+    minAIC = inf;
+    minAIC2 = inf;
+    for ARdeg=num_component:2*num_component
+        tmp = roots([1 ARwithnoise_param(ARdeg,1:ARdeg)]);
+        if ARdeg-nnz(imag(tmp))/2 == num_component && AIC_ARwithnoise(ARdeg) < minAIC
+            z0 = tmp;
+            E0 = ARwithnoise_param(ARdeg,ARdeg+1);
+            R0 = ARwithnoise_param(ARdeg,ARdeg+2);
+            minAIC = AIC_ARwithnoise(ARdeg);
+            optARdeg = ARdeg;
+        end
+        if ARdeg-nnz(imag(tmp))/2 >= num_component && AIC_ARwithnoise(ARdeg) < minAIC2
+            z1 = tmp;
+            E1 = ARwithnoise_param(ARdeg,ARdeg+1);
+            R1 = ARwithnoise_param(ARdeg,ARdeg+2);
+            minAIC2 = AIC_ARwithnoise(ARdeg);
+            optARdeg2 = ARdeg;
+        end
+    end
+    if minAIC == inf
+        if minAIC2 == inf
+            warning('no AR model with %d oscillators',num_component);
+        end
+        z0 = z1;
+        E0 = E1;
+        R0 = R1;
+        optARdeg = optARdeg2;
+        [B,I] = sort(abs(z0),'descend');
+        z0 = z0(I);
+    end
+    [tmp,ARdeg] = min(AIC_ARwithnoise);
+    tmp = roots([1 ARwithnoise_param(ARdeg,1:ARdeg)]);
+    if nnz(imag(tmp)>=0) >= num_component
+        z0 = tmp;
+        E0 = ARwithnoise_param(ARdeg,ARdeg+1);
+        R0 = ARwithnoise_param(ARdeg,ARdeg+2);
+        optARdeg = ARdeg;
+    end
+    VV = zeros(optARdeg,optARdeg);
+    for j=1:optARdeg
+        for i=1:optARdeg
+            VV(i,j) = z0(j)^(1-i);
+        end
+    end
+    QQ = inv(VV)*[E0 zeros(1,optARdeg-1); zeros(optARdeg-1,optARdeg)]*inv(VV)';
+    [B,I] = sort(diag(real(QQ))./(1-abs(z0).^2),'descend');
+    z0 = z0(I);
+
+%     init_a = [];
+%     init_f = [];
+%     z_complex = (imag(z0) ~=0);
+%     if sum(z_complex) >0
+%         init_a = (abs(z0(z_complex)));
+%         init_f = unique((1/(2*pi))*acos(abs(real(z0(z_complex)))./init_a));
+%         init_a = unique(init_a);
+%     end
+%     % covering cases where we have no imaginary root
+%     if length(init_a) < num_component
+%        init_f = [init_f, zeros(1, num_component - length(init_f))];
+%        tmp  =abs(z0(~z_complex));
+%        init_a = [init_a, tmp(1:num_component - length(init_a))];
+%     end
+    
+    init_a = zeros(1,num_component);
+    init_f = zeros(1,num_component);
+    kk = 1;
+    for k=1:num_component
+        init_a(k) = abs(z0(kk));
+        init_f(k) = abs(angle(z0(kk)));
+        if isreal(z0(kk)) && z0(kk) < 0
+            init_f(k) = 4; % for pi
+        end
+        if imag(z0(kk)) == 0
+            kk = kk+1;
+        else
+            kk = kk+2;
+        end
+    end
+
+    [B,I] = sort(init_f);
+    init_a = init_a(I);
+    init_f = init_f(I);
+
+    if mod(T,2) == 0
+        freq = [2*pi/T*(0:T/2-1) pi];
+    else
+        freq = [2*pi/T*(0:(T-1)/2)];
+    end
+    init_f(init_f==4) = pi;
+    P = zeros(length(freq),num_component);
+    for k=1:num_component
+        a = init_a(k);
+        theta = init_f(k);
+        A = (1-2*a^2*cos(theta)^2+a^4*cos(2*theta))/a/(a^2-1)/cos(theta);
+        b = (A-2*a*cos(theta)+sign(cos(theta))*sqrt((A-2*a*cos(theta))^2-4))/2;
+        for j=1:length(freq)
+            P(j,k) = -a*cos(theta)/b*abs(1+b*exp(-1i*freq(j)))^2/abs(1-2*a*cos(theta)*exp(-1i*freq(j))+a^2*exp(-2*1i*freq(j))).^2;
+        end
+    end
+    p = zeros(length(freq),1);
+    for j=1:length(freq)
+        p(j) = abs(y*exp(-1i*freq(j)*(0:T-1)'))^2/T;
+    end
+    if cond(P'*P) < 10^6    % modified 2017/01/14
+        init_sigma = (P'*P)\(P'*p);
+    else
+        init_sigma = expGLMfit(P,p);
+    end
+    init_sigma(init_sigma<0) = R0;
\ No newline at end of file
diff --git a/make_pink_noise.m b/make_pink_noise.m
new file mode 100644
index 0000000..fdea3d4
--- /dev/null
+++ b/make_pink_noise.m
@@ -0,0 +1,21 @@
+
+function [x1new] = make_pink_noise(alpha,L,dt)
+
+  %alpha=0.33;
+
+  x1 = randn(L,1);
+  xf1 = fft(x1);
+  A = abs(xf1);
+  phase = angle(xf1);
+
+  df = 1.0 / (dt*length(x1));
+  faxis = (0:length(x1)/2)*df;
+  faxis = [faxis, faxis(end-1:-1:2)];  %(end-1:-1:2)
+  oneOverf = 1.0 ./ faxis.^alpha;
+  oneOverf(1)=0.0;
+
+  Anew = sqrt((A.^2).*oneOverf');
+  xf1new = Anew .* exp(1i*phase);
+  x1new = real(ifft(xf1new))';
+  
+end
\ No newline at end of file
diff --git a/oneStepKFupdate_sspp.m b/oneStepKFupdate_sspp.m
new file mode 100644
index 0000000..cc4dea8
--- /dev/null
+++ b/oneStepKFupdate_sspp.m
@@ -0,0 +1,11 @@
+function [x_new,P_new] = oneStepKFupdate_sspp(x,y,phi,M,Q,R,P)
+% one step kalman filter update given the current time point's x, the
+% observed y, the variance paramter estimates of Q, R and P (For x itself)
+
+x_one = phi *x;
+P_one = phi *P * phi' + Q;
+
+K_one = (P_one*M) /(M'*P_one*M + R); 
+
+x_new = x_one + K_one * (y - M'*x_one);
+P_new = P_one - K_one * M' * P_one;
diff --git a/osc_decomp/AR_MLE.m b/osc_decomp/AR_MLE.m
new file mode 100644
index 0000000..15c4c3d
--- /dev/null
+++ b/osc_decomp/AR_MLE.m
@@ -0,0 +1,33 @@
+function [A,E] = AR_MLE(y,ARdeg)
+    [A,E] = aryule(y,ARdeg);
+    a = zeros(ARdeg,ARdeg);
+    a(:,ARdeg) = -A(2:ARdeg+1);
+    for m=ARdeg:-1:2
+        for i=1:m-1
+            a(i,m-1) = (a(i,m)+a(m,m)*a(m-i,m))/(1-a(m,m)^2);
+        end
+    end
+    c = zeros(ARdeg,1); % PARCOR
+    for m=1:ARdeg
+        c(m) = a(m,m);
+    end
+    init = log(1+c)-log(1-c);
+    init(init>20) = 20;
+    init(init<-20) = -20;
+    init(c>=1) = 20;
+    init(c<=-1) = -20;
+    options = optimoptions('fminunc','Algorithm','quasi-newton','MaxFunEvals',10000);
+    tmp = fminunc(@(p)AR_ll(y,p),init,options);
+    c = (exp(tmp)-1)./(exp(tmp)+1);
+    [tmp,E] = AR_ll(y,tmp);
+    for m=1:ARdeg
+        a(m,m) = c(m);
+    end
+    for m=2:ARdeg
+        for i=1:m-1
+            a(i,m) = a(i,m-1)-c(m)*a(m-i,m-1);
+        end
+    end
+    A = [1 -a(:,ARdeg)'];
+end
+
diff --git a/osc_decomp/AR_fit.m b/osc_decomp/AR_fit.m
new file mode 100644
index 0000000..5026a25
--- /dev/null
+++ b/osc_decomp/AR_fit.m
@@ -0,0 +1,34 @@
+options = optimoptions('fminunc','Algorithm','quasi-newton','MaxFunEvals',10000 ,'UseParallel', ~0);
+AIC_ARraw = zeros(1,MAX_AR);
+ARraw_param = zeros(MAX_AR,MAX_AR+1);
+AIC_ARwithnoise = zeros(1,MAX_AR);
+ARwithnoise_param = zeros(MAX_AR,MAX_AR+2);
+for ARdeg=1:MAX_AR
+    [A,E] = AR_MLE(y,ARdeg);
+    a = zeros(ARdeg,ARdeg);
+    a(:,ARdeg) = -A(2:ARdeg+1);
+    for m=ARdeg:-1:2
+        for i=1:m-1
+            a(i,m-1) = (a(i,m)+a(m,m)*a(m-i,m))/(1-a(m,m)^2);
+        end
+    end
+    c = zeros(ARdeg,1);
+    for m=1:ARdeg
+        c(m) = a(m,m);
+    end
+    AIC_ARraw(ARdeg) = 2*AR_ll(y,log(1+c)-log(1-c))+2*(ARdeg+1);
+    ARraw_param(ARdeg,1:ARdeg+1) = [A(2:ARdeg+1) E];
+    z = roots(A);
+    k = 1;
+    while max(abs(z)) >= 1
+        [A,E] = AR_MLE(y,ARdeg-k);
+        z = roots(A);
+        A = [A zeros(1,k)];
+        k = k+1;
+    end
+    [A,E,R] = armyule(y,ARdeg);
+    param = [A(2:ARdeg+1) log(E)-log(R)];
+    [mll,R] = ARwithnoise_ll(y,param);
+    AIC_ARwithnoise(ARdeg) = 2*mll+2*(ARdeg+2);
+    ARwithnoise_param(ARdeg,1:ARdeg+2) = [param(1:ARdeg) exp(param(ARdeg+1))*R R];
+end
diff --git a/osc_decomp/AR_ll.m b/osc_decomp/AR_ll.m
new file mode 100644
index 0000000..aad9696
--- /dev/null
+++ b/osc_decomp/AR_ll.m
@@ -0,0 +1,64 @@
+function [mll,Ehat] = AR_ll(y,p)
+    T = length(y);
+    ARdeg = length(p);
+    c = (exp(p)-1)./(exp(p)+1);
+    a = zeros(ARdeg,ARdeg);
+    for m=1:ARdeg
+        a(m,m) = c(m);
+    end
+    for m=2:ARdeg
+        for i=1:m-1
+            a(i,m) = a(i,m-1)-c(m)*a(m-i,m-1);
+        end
+    end
+    K = zeros(ARdeg+1,ARdeg+1);
+    A = [1 -a(:,ARdeg)'];
+    for i=1:ARdeg+1
+        K(i,i:-1:2) = K(i,i:-1:2)+A(1:i-1);
+        K(i,1:ARdeg-i+2) = K(i,1:ARdeg-i+2)+A(i:ARdeg+1);
+    end
+    C = K\[1; zeros(ARdeg,1)];
+    F = zeros(ARdeg,ARdeg);
+    F(:,1) = a(:,ARdeg);
+    F(1:ARdeg-1,2:ARdeg) = eye(ARdeg-1);
+    G = [1; zeros(ARdeg-1,1)];
+    Q = G*G';
+    H = [1 zeros(1,ARdeg-1)];
+    R = 0;
+    x_pred1 = zeros(ARdeg,T);
+    x_filt = zeros(ARdeg,T);
+    V_pred1 = zeros(ARdeg,ARdeg,T);
+    V_filt = zeros(ARdeg,ARdeg,T);
+    x_pred1(:,1) = zeros(ARdeg,1);
+    V_pred1(1,1,1) = C(1);
+    for i=2:ARdeg
+        V_pred1(i,1,1) = C(2:ARdeg-i+2)'*a(i:ARdeg,ARdeg);
+        V_pred1(1,i,1) = V_pred1(i,1,1);
+    end
+    for i=2:ARdeg
+        for j=i:ARdeg
+            for p=i:ARdeg
+                for q=j:ARdeg
+                    V_pred1(i,j,1) = V_pred1(i,j,1)+a(p,ARdeg)*a(q,ARdeg)*C(abs(q-j-p+i)+1);
+                end
+            end
+            V_pred1(j,i,1) = V_pred1(i,j,1);
+        end
+    end
+    for t=1:T-1
+        x_filt(:,t) = x_pred1(:,t) + V_pred1(:,:,t)*H'*((H*V_pred1(:,:,t)*H'+R)\(y(t)-H*x_pred1(:,t)));
+        V_filt(:,:,t) = (eye(ARdeg)-V_pred1(:,:,t)*(H'*H)/(H*V_pred1(:,:,t)*H'+R))*V_pred1(:,:,t);
+        x_pred1(:,t+1) = F*x_filt(:,t);
+        V_pred1(:,:,t+1) = F*V_filt(:,:,t)*F'+Q;
+    end
+    Ehat = 0;
+    for t=1:T
+        Ehat = Ehat*(t-1)/t+(y(t)-H*x_pred1(:,t))^2/(H*V_pred1(:,:,t)*H')/t;
+    end
+    ll = -T*log(Ehat)/2-T/2;
+    for t=1:T
+        ll = ll-log(H*V_pred1(:,:,t)*H')/2;
+    end
+    mll = -ll;
+end
+
diff --git a/osc_decomp/ARwithnoise_ll.m b/osc_decomp/ARwithnoise_ll.m
new file mode 100644
index 0000000..75b0725
--- /dev/null
+++ b/osc_decomp/ARwithnoise_ll.m
@@ -0,0 +1,39 @@
+function [mll,Rhat] = ARwithnoise_ll(y,param)
+    T = length(y);
+    ARdeg = length(param)-1;
+    A = [1 param(1:ARdeg)];
+    E = exp(param(ARdeg+1));
+    R = 1;
+    x_pred1 = zeros(ARdeg,T);
+    x_filt = zeros(ARdeg,T);
+    V_pred1 = zeros(ARdeg,ARdeg,T);
+    V_filt = zeros(ARdeg,ARdeg,T);
+    F = [-A(2:ARdeg+1); eye(ARdeg-1) zeros(ARdeg-1,1)];
+    Q = [E zeros(1,ARdeg-1); zeros(ARdeg-1,ARdeg)];
+    H = [1 zeros(1,ARdeg-1)];
+    x_pred1(:,1) = zeros(ARdeg,1);
+    K = zeros(ARdeg+1,ARdeg+1);
+    for i=1:ARdeg+1
+        K(i,i:-1:2) = K(i,i:-1:2)+A(1:i-1);
+        K(i,1:ARdeg-i+2) = K(i,1:ARdeg-i+2)+A(i:ARdeg+1);
+    end
+    c = K\[E; zeros(ARdeg,1)];
+    V_pred1(:,:,1) = toeplitz(c(1:ARdeg));
+    for t=1:T-1
+        x_filt(:,t) = x_pred1(:,t) + V_pred1(:,:,t)*H'*((H*V_pred1(:,:,t)*H'+R)\(y(t)-H*x_pred1(:,t)));
+        V_filt(:,:,t) = V_pred1(:,:,t) - V_pred1(:,:,t)*H'*((H*V_pred1(:,:,t)*H'+R)\H)*V_pred1(:,:,t);
+        x_pred1(:,t+1) = F*x_filt(:,t);
+        V_pred1(:,:,t+1) = F*V_filt(:,:,t)*F'+Q;
+    end
+    Rhat = 0;
+    for t=1:T
+        Rhat = Rhat*(t-1)/t+(y(t)-H*x_pred1(:,t))^2/(H*V_pred1(:,:,t)*H'+R)/t;
+    end
+    ll = -T*log(Rhat)/2-T/2;
+    for t=1:T
+        ll = ll-log(H*V_pred1(:,:,t)*H'+R)/2;
+    end
+    % for minimization
+    mll = -ll;
+end
+
diff --git a/osc_decomp/ReadMe.txt b/osc_decomp/ReadMe.txt
new file mode 100644
index 0000000..4bdea7c
--- /dev/null
+++ b/osc_decomp/ReadMe.txt
@@ -0,0 +1,12 @@
+demo_osc.m is the script for decomposing the Wolfer sunspot data.
+
+Reference: T. Matsuda and F. Komaki. Time series decomposition into oscillation components and phase estimation. Neural Computation, Vol. 29, pp. 332--367, 2017. 
+
+If you have any comments or bug reports, please contact Takeru Matsuda (matsuda@mist.i.u-tokyo.ac.jp).
+
+Update history
+-program uploaded on Mar 1, 2017
+-initial value setting for sigma_k^2 modified on Jan 14, 2018
+-initial value setting modified on Jun 8, 2018 (thanks to Johan Abrahamsson)
+-optimization method modified on Aug 13, 2018
+-osc_decomp_fast added on Dec 29, 2018
diff --git a/osc_decomp/WolferSunspotData.mat b/osc_decomp/WolferSunspotData.mat
new file mode 100644
index 0000000..b97a647
Binary files /dev/null and b/osc_decomp/WolferSunspotData.mat differ
diff --git a/osc_decomp/__MACOSX/._osc_decomp b/osc_decomp/__MACOSX/._osc_decomp
new file mode 100644
index 0000000..87ff9fd
Binary files /dev/null and b/osc_decomp/__MACOSX/._osc_decomp differ
diff --git a/osc_decomp/__MACOSX/osc_decomp/._AR_MLE.m b/osc_decomp/__MACOSX/osc_decomp/._AR_MLE.m
new file mode 100644
index 0000000..4031f61
Binary files /dev/null and b/osc_decomp/__MACOSX/osc_decomp/._AR_MLE.m differ
diff --git a/osc_decomp/__MACOSX/osc_decomp/._AR_fit.m b/osc_decomp/__MACOSX/osc_decomp/._AR_fit.m
new file mode 100644
index 0000000..624df71
Binary files /dev/null and b/osc_decomp/__MACOSX/osc_decomp/._AR_fit.m differ
diff --git a/osc_decomp/__MACOSX/osc_decomp/._AR_ll.m b/osc_decomp/__MACOSX/osc_decomp/._AR_ll.m
new file mode 100644
index 0000000..9a6e99b
Binary files /dev/null and b/osc_decomp/__MACOSX/osc_decomp/._AR_ll.m differ
diff --git a/osc_decomp/__MACOSX/osc_decomp/._ARwithnoise_ll.m b/osc_decomp/__MACOSX/osc_decomp/._ARwithnoise_ll.m
new file mode 100644
index 0000000..046f768
Binary files /dev/null and b/osc_decomp/__MACOSX/osc_decomp/._ARwithnoise_ll.m differ
diff --git a/osc_decomp/__MACOSX/osc_decomp/._ReadMe.txt b/osc_decomp/__MACOSX/osc_decomp/._ReadMe.txt
new file mode 100644
index 0000000..09d51e3
Binary files /dev/null and b/osc_decomp/__MACOSX/osc_decomp/._ReadMe.txt differ
diff --git a/osc_decomp/__MACOSX/osc_decomp/._WolferSunspotData.mat b/osc_decomp/__MACOSX/osc_decomp/._WolferSunspotData.mat
new file mode 100644
index 0000000..45dd8b6
Binary files /dev/null and b/osc_decomp/__MACOSX/osc_decomp/._WolferSunspotData.mat differ
diff --git a/osc_decomp/__MACOSX/osc_decomp/._angle_conf_MC.m b/osc_decomp/__MACOSX/osc_decomp/._angle_conf_MC.m
new file mode 100644
index 0000000..7f1028d
Binary files /dev/null and b/osc_decomp/__MACOSX/osc_decomp/._angle_conf_MC.m differ
diff --git a/osc_decomp/__MACOSX/osc_decomp/._armyule.m b/osc_decomp/__MACOSX/osc_decomp/._armyule.m
new file mode 100644
index 0000000..52ea675
Binary files /dev/null and b/osc_decomp/__MACOSX/osc_decomp/._armyule.m differ
diff --git a/osc_decomp/__MACOSX/osc_decomp/._demo_osc.m b/osc_decomp/__MACOSX/osc_decomp/._demo_osc.m
new file mode 100644
index 0000000..d53cc6f
Binary files /dev/null and b/osc_decomp/__MACOSX/osc_decomp/._demo_osc.m differ
diff --git a/osc_decomp/__MACOSX/osc_decomp/._expGLM_ll.m b/osc_decomp/__MACOSX/osc_decomp/._expGLM_ll.m
new file mode 100644
index 0000000..ed76796
Binary files /dev/null and b/osc_decomp/__MACOSX/osc_decomp/._expGLM_ll.m differ
diff --git a/osc_decomp/__MACOSX/osc_decomp/._expGLMfit.m b/osc_decomp/__MACOSX/osc_decomp/._expGLMfit.m
new file mode 100644
index 0000000..f83506d
Binary files /dev/null and b/osc_decomp/__MACOSX/osc_decomp/._expGLMfit.m differ
diff --git a/osc_decomp/__MACOSX/osc_decomp/._osc_decomp.m b/osc_decomp/__MACOSX/osc_decomp/._osc_decomp.m
new file mode 100644
index 0000000..39ae5f2
Binary files /dev/null and b/osc_decomp/__MACOSX/osc_decomp/._osc_decomp.m differ
diff --git a/osc_decomp/__MACOSX/osc_decomp/._osc_decomp_fast.m b/osc_decomp/__MACOSX/osc_decomp/._osc_decomp_fast.m
new file mode 100644
index 0000000..7f3a1bb
Binary files /dev/null and b/osc_decomp/__MACOSX/osc_decomp/._osc_decomp_fast.m differ
diff --git a/osc_decomp/__MACOSX/osc_decomp/._plot_decomp.m b/osc_decomp/__MACOSX/osc_decomp/._plot_decomp.m
new file mode 100644
index 0000000..3ee713b
Binary files /dev/null and b/osc_decomp/__MACOSX/osc_decomp/._plot_decomp.m differ
diff --git a/osc_decomp/__MACOSX/osc_decomp/._plot_decomp_one.m b/osc_decomp/__MACOSX/osc_decomp/._plot_decomp_one.m
new file mode 100644
index 0000000..fc8673a
Binary files /dev/null and b/osc_decomp/__MACOSX/osc_decomp/._plot_decomp_one.m differ
diff --git a/osc_decomp/__MACOSX/osc_decomp/._plot_phase.m b/osc_decomp/__MACOSX/osc_decomp/._plot_phase.m
new file mode 100644
index 0000000..e9f1a9b
Binary files /dev/null and b/osc_decomp/__MACOSX/osc_decomp/._plot_phase.m differ
diff --git a/osc_decomp/__MACOSX/osc_decomp/._plot_phase_area.m b/osc_decomp/__MACOSX/osc_decomp/._plot_phase_area.m
new file mode 100644
index 0000000..684c678
Binary files /dev/null and b/osc_decomp/__MACOSX/osc_decomp/._plot_phase_area.m differ
diff --git a/osc_decomp/__MACOSX/osc_decomp/._plot_phase_nocross.m b/osc_decomp/__MACOSX/osc_decomp/._plot_phase_nocross.m
new file mode 100644
index 0000000..8459e6e
Binary files /dev/null and b/osc_decomp/__MACOSX/osc_decomp/._plot_phase_nocross.m differ
diff --git a/osc_decomp/__MACOSX/osc_decomp/._prop_ll.m b/osc_decomp/__MACOSX/osc_decomp/._prop_ll.m
new file mode 100644
index 0000000..afa3b7e
Binary files /dev/null and b/osc_decomp/__MACOSX/osc_decomp/._prop_ll.m differ
diff --git a/osc_decomp/angle_conf_MC.m b/osc_decomp/angle_conf_MC.m
new file mode 100644
index 0000000..5b6b2b2
--- /dev/null
+++ b/osc_decomp/angle_conf_MC.m
@@ -0,0 +1,18 @@
+function conf = angle_conf_MC(mu,Sigma,prob,seeds)
+    nmc = size(seeds,2);
+    phi = atan2(mu(2),mu(1));
+    P = [cos(phi) sin(phi); -sin(phi) cos(phi)];
+    Sigmasqrt = sqrtm(Sigma);
+    seeds = norm(mu)*[ones(1,nmc); zeros(1,nmc)]+P*Sigmasqrt*seeds;
+    phases = atan2(seeds(2,:),seeds(1,:));
+    [tmp,I] = sort(abs(phases));
+    phases = sort(phases(I(1:ceil(prob*nmc))));
+    conf = [phi+phases(1) phi+phases(length(phases))];
+    if conf(1)<-pi
+        conf(1) = conf(1)+2*pi;
+    end
+    if conf(2)>pi
+        conf(2) = conf(2)-2*pi;
+    end
+end
+
diff --git a/osc_decomp/armyule.m b/osc_decomp/armyule.m
new file mode 100644
index 0000000..280eb1a
--- /dev/null
+++ b/osc_decomp/armyule.m
@@ -0,0 +1,20 @@
+function [A,E,R] = armyule(y,ARdeg)
+    T = length(y);
+
+    acov = zeros(ARdeg+1,1);
+    for k=1:ARdeg+1
+        acov(k) = y(1:T-k+1)*y(k:T)'/T;
+    end
+    C = zeros(ARdeg,ARdeg);
+    for k=1:ARdeg
+        C(k,1:k) = acov(k:-1:1);
+        C(k,k+1:ARdeg) = acov(2:ARdeg-k+1);
+    end
+    c = acov(2:ARdeg+1);
+    eigs = eig([C flipud(c); fliplr(c') acov(1)]);
+    R = fminbnd(@(R)ARwithnoise_ll(y,[-(C-R*eye(ARdeg))\c; log(acov(1)-R-((C-R*eye(ARdeg))\c)'*acov(2:ARdeg+1))-log(R)]'),0,min(eigs));
+    A = (C-R*eye(ARdeg))\c;
+    E = acov(1)-R-A'*acov(2:ARdeg+1);
+    A = [1 -A'];
+end
+
diff --git a/osc_decomp/demo_osc.m b/osc_decomp/demo_osc.m
new file mode 100644
index 0000000..359713f
--- /dev/null
+++ b/osc_decomp/demo_osc.m
@@ -0,0 +1,22 @@
+load('WolferSunspotData');
+y = log(sunspot+1);
+fs = 1;
+MAX_COMPONENT = 5;
+tic
+[decomp_phase,decomp_mu,decomp_cov,AIC_osc,osc_param] = osc_decomp(y,fs,MAX_COMPONENT);
+time2=toc
+[tmp,K] = min(AIC_osc);
+plot_decomp(y,fs,decomp_mu,decomp_cov,osc_param,K);
+plot_phase(y,fs,decomp_phase,decomp_mu,decomp_cov,K);
+osc_a = osc_param(K,1:K);
+osc_f = osc_param(K,K+1:2*K);
+osc_sigma = osc_param(K,2*K+1:3*K);
+osc_r = osc_param(K,3*K+1);
+str = sprintf('The number of oscillators is K=%d.',K);
+disp(str);
+str = 'The periods of K oscillators are ';
+for k=1:K-1
+    str = [str sprintf('%.2f, ',1./osc_f(k))];
+end
+str = [str sprintf('%.2f years.',1./osc_f(K))];
+disp(str);
diff --git a/osc_decomp/expGLM_ll.m b/osc_decomp/expGLM_ll.m
new file mode 100644
index 0000000..011a512
--- /dev/null
+++ b/osc_decomp/expGLM_ll.m
@@ -0,0 +1,9 @@
+function [mll,g] = expGLM_ll(X,y,beta)
+    mll = 0;
+    g = zeros(size(X,2),1);
+    for i=1:length(y)
+        mll = mll+log(X(i,:)*beta)+y(i)/(X(i,:)*beta);
+        g = g+X(i,:)'/(X(i,:)*beta)-y(i)*X(i,:)'/(X(i,:)*beta)^2;
+    end
+end
+
diff --git a/osc_decomp/expGLMfit.m b/osc_decomp/expGLMfit.m
new file mode 100644
index 0000000..af39e61
--- /dev/null
+++ b/osc_decomp/expGLMfit.m
@@ -0,0 +1,5 @@
+function beta = expGLMfit(X,y)
+    options = optimoptions('fminunc','Algorithm','quasi-newton','MaxFunEvals',10000);
+    beta = exp(fminunc(@(b)expGLM_ll(X,y,exp(b)),zeros(size(X,2),1),options));
+end
+
diff --git a/osc_decomp/osc_decomp.m b/osc_decomp/osc_decomp.m
new file mode 100644
index 0000000..f2ca32b
--- /dev/null
+++ b/osc_decomp/osc_decomp.m
@@ -0,0 +1,189 @@
+function [phi_prop,decomp_mu,decomp_cov,AIC_osc,osc_param] = osc_decomp(y,fs,MAX_COMPONENT)
+%
+% Input:
+%    y:             time series (1 times T)
+%    fs:            sampling frequency (scalar)
+%    MAX_COMPONENT: maximum number of oscillation components (scalar)
+%
+% Output:
+%    phi_prop:      estimated phase of each oscillation component (MAX_COMPONENT times T times MAX_COMPONENT)
+%                   phi_prop(k,:,K) is the estimated phase of the k-th
+%                   oscillator in the decomposition into K components
+%    decomp_mu:     smoothed coordinate of each oscillator (2*MAX_COMPONENT times T times MAX_COMPONENT)
+%                   decomp_mu(2*k-1:2*k,:,K) is the smoothed coordinate of the k-th oscillator in the decomposition into K components
+%    decomp_cov:    smoothed covariance of each oscillator (2*MAX_COMPONENT times 2*MAX_COMPONENT times T times MAX_COMPONENT)
+%                   decomp_cov(2*k-1:2*k,2*k-1:2*k,:,K) is the smoothed covariance of the k-th oscillator in the decomposition into K components
+%    AIC_osc:       AIC of the oscillator model (1 times MAX_COMPONENT)
+%                   AIC_osc(K) is the AIC of the oscillator model with K oscillation components
+%    osc_param:     estimated parameters of the oscillator model (MAX_COMPONENT times 3*MAX_COMPONENT+1)
+%                   osc_param(K,1:K) is the estimated a_1,...,a_K of the oscillator model with K oscillation components
+%                   osc_param(K,K+1:2*K) is the estimated f_1,...f_K of the oscillator model with K oscillation components
+%                   osc_param(K,2*K+1:3*K) is the estimated sigma_1^2,...,sigma_K^2 of the oscillator model with K oscillation components
+%                   osc_param(K,3*K+1) is the estimated tau^2 of the oscillator model with K oscillation components
+%
+    options = optimoptions('fminunc','Algorithm','quasi-newton','MaxFunEvals',10000, 'UseParallel',~0);
+    T = length(y);
+    phi_prop = zeros(MAX_COMPONENT,T,MAX_COMPONENT);
+    decomp_mu = zeros(2*MAX_COMPONENT,T,MAX_COMPONENT);
+    decomp_cov = zeros(2*MAX_COMPONENT,2*MAX_COMPONENT,T,MAX_COMPONENT);
+    AIC_osc = zeros(1,MAX_COMPONENT);
+    osc_param = zeros(MAX_COMPONENT,3*MAX_COMPONENT+1);
+    MAX_AR = 2*MAX_COMPONENT;
+    AR_fit;
+    for num_component=1:MAX_COMPONENT
+        num_component
+        minAIC = inf;
+        minAIC2 = inf;
+        for ARdeg=num_component:2*num_component
+            tmp = roots([1 ARwithnoise_param(ARdeg,1:ARdeg)]);
+            if ARdeg-nnz(imag(tmp))/2 == num_component && AIC_ARwithnoise(ARdeg) < minAIC
+                z0 = tmp;
+                E0 = ARwithnoise_param(ARdeg,ARdeg+1);
+                R0 = ARwithnoise_param(ARdeg,ARdeg+2);
+                minAIC = AIC_ARwithnoise(ARdeg);
+                optARdeg = ARdeg;
+            end
+            if ARdeg-nnz(imag(tmp))/2 >= num_component && AIC_ARwithnoise(ARdeg) < minAIC2
+                z1 = tmp;
+                E1 = ARwithnoise_param(ARdeg,ARdeg+1);
+                R1 = ARwithnoise_param(ARdeg,ARdeg+2);
+                minAIC2 = AIC_ARwithnoise(ARdeg);
+                optARdeg2 = ARdeg;
+            end
+        end
+        if minAIC == inf
+            if minAIC2 == inf
+                warning('no AR model with %d oscillators',num_component);
+            end
+            z0 = z1;
+            E0 = E1;
+            R0 = R1;
+            optARdeg = optARdeg2;
+            [B,I] = sort(abs(z0),'descend');
+            z0 = z0(I);
+        end
+        [tmp,ARdeg] = min(AIC_ARwithnoise);
+        tmp = roots([1 ARwithnoise_param(ARdeg,1:ARdeg)]);
+        if nnz(imag(tmp)>=0) >= num_component
+            z0 = tmp;
+            E0 = ARwithnoise_param(ARdeg,ARdeg+1);
+            R0 = ARwithnoise_param(ARdeg,ARdeg+2);
+            optARdeg = ARdeg;
+        end
+        VV = zeros(optARdeg,optARdeg);
+        for j=1:optARdeg
+            for i=1:optARdeg
+                VV(i,j) = z0(j)^(1-i);
+            end
+        end
+        QQ = inv(VV)*[E0 zeros(1,optARdeg-1); zeros(optARdeg-1,optARdeg)]*inv(VV)';
+        [B,I] = sort(diag(real(QQ))./(1-abs(z0).^2),'descend');
+        z0 = z0(I);
+        
+        init_a = zeros(1,num_component);
+        init_f = zeros(1,num_component);
+        kk = 1;
+        for k=1:num_component
+            init_a(k) = abs(z0(kk));
+            init_f(k) = abs(angle(z0(kk)));
+	        if isreal(z0(kk)) && z0(kk) < 0
+        	    init_f(k) = 4; % for pi
+	        end
+            if imag(z0(kk)) == 0
+                kk = kk+1;
+            else
+                kk = kk+2;
+            end
+        end
+        [B,I] = sort(init_f);
+        init_a = init_a(I);
+        init_f = init_f(I);
+%    	nf0 = nnz(init_f==0);
+%    	nf1 = nnz(init_f==4);
+%	    freq = init_f;
+%    	freq(2:nf0) = pi*rand(1,nf0-1);
+%	    freq(num_component-nf1+2:num_component) = pi*rand(1,nf1-1);
+        if mod(T,2) == 0
+        	freq = [2*pi/T*(0:T/2-1) pi];
+	    else
+    	    freq = [2*pi/T*(0:(T-1)/2)];
+        end
+        init_f(init_f==4) = pi;
+        P = zeros(length(freq),num_component);
+        for k=1:num_component
+            a = init_a(k);
+            theta = init_f(k);
+            A = (1-2*a^2*cos(theta)^2+a^4*cos(2*theta))/a/(a^2-1)/cos(theta);
+            b = (A-2*a*cos(theta)+sign(cos(theta))*sqrt((A-2*a*cos(theta))^2-4))/2;
+            for j=1:length(freq)
+                P(j,k) = -a*cos(theta)/b*abs(1+b*exp(-1i*freq(j)))^2/abs(1-2*a*cos(theta)*exp(-1i*freq(j))+a^2*exp(-2*1i*freq(j))).^2;
+            end
+        end
+        p = zeros(length(freq),1);
+        for j=1:length(freq)
+            p(j) = abs(y*exp(-1i*freq(j)*(0:T-1)'))^2/T;
+        end
+        if cond(P'*P) < 10^6    % modified 2017/01/14
+            init_sigma = (P'*P)\(P'*p);
+        else
+            init_sigma = expGLMfit(P,p);
+        end
+        init_sigma(init_sigma<0) = R0;
+        
+        param = fminunc(@(param)prop_ll(y,param,init_f),[atanh(2*init_a-1) zeros(1,num_component) log(init_sigma'/R0)],options);
+        [mll,sigman] = prop_ll(y,param,init_f);
+        param(num_component+1:2*num_component) = abs(init_f+tanh(param(num_component+1:2*num_component))*pi);
+        AIC_osc(num_component) = 2*mll+2*(3*num_component+1);
+	    [tmp,I] = sort(param(num_component+1:2*num_component));
+        a = (tanh(param(I))+1)/2;
+        theta = param(num_component+I);
+        sigma = exp(param(2*num_component+I))*sigman;
+        osc_param(num_component,1:3*num_component+1) = [a theta*fs/2/pi sigma sigman];
+    
+        m = 2*num_component;
+        x_pred1 = zeros(m,T);
+        x_filt = zeros(m,T);
+        x_smooth = zeros(m,T);
+        V_pred1 = zeros(m,m,T);
+        V_filt = zeros(m,m,T);
+        V_smooth = zeros(m,m,T);
+        F = zeros(m,m);
+        Q = zeros(m,m);
+        H = zeros(1,m);
+        for k=1:num_component
+            F(2*k-1:2*k,2*k-1:2*k) = a(k)*[cos(theta(k)) -sin(theta(k)); sin(theta(k)) cos(theta(k))];
+            Q(2*k-1,2*k-1) = 1-a(k)^2;
+            Q(2*k,2*k) = 1-a(k)^2;
+        end
+        H(1:2:m) = sqrt(sigma)./sqrt(1-a.^2);
+        R = sigman;
+        x_pred1(:,1) = zeros(m,1);
+        for k=1:num_component
+            V_pred1(2*k-1:2*k,2*k-1:2*k,1) = eye(2);
+        end
+        for t=1:T-1
+            Kg = V_pred1(:,:,t)*H'*inv(H*V_pred1(:,:,t)*H'+R);
+            x_filt(:,t) = x_pred1(:,t) + Kg*(y(t)-H*x_pred1(:,t));
+            V_filt(:,:,t) = (eye(m)-Kg*H)*V_pred1(:,:,t);
+            x_pred1(:,t+1) = F*x_filt(:,t);
+            V_pred1(:,:,t+1) = F*V_filt(:,:,t)*F'+Q;
+        end
+        Kg = V_pred1(:,:,T)*H'*inv(H*V_pred1(:,:,T)*H'+R);
+        x_filt(:,T) = x_pred1(:,T) + Kg*(y(T)-H*x_pred1(:,T));
+        V_filt(:,:,T) = (eye(m)-Kg*H)*V_pred1(:,:,T);
+        x_smooth(:,T) = x_filt(:,T);
+        V_smooth(:,:,T) = V_filt(:,:,T);
+        for t=T-1:-1:1
+            x_smooth(:,t) = x_filt(:,t) + V_filt(:,:,t)*F'*(V_pred1(:,:,t+1)\(x_smooth(:,t+1)-x_pred1(:,t+1)));
+            CC = V_filt(:,:,t)*F'*inv(V_pred1(:,:,t+1));
+            V_smooth(:,:,t) = V_filt(:,:,t) + CC*(V_smooth(:,:,t+1)-V_pred1(:,:,t+1))*CC';
+        end
+        for k=1:num_component
+            phi_prop(k,:,num_component) = atan2(x_smooth(2*k,:),x_smooth(2*k-1,:));
+        end
+        decomp_mu(1:m,:,num_component) = x_smooth;
+        decomp_cov(1:m,1:m,:,num_component) = V_smooth;
+    end
+end
+
+
diff --git a/osc_decomp/osc_decomp_fast.m b/osc_decomp/osc_decomp_fast.m
new file mode 100644
index 0000000..51168df
--- /dev/null
+++ b/osc_decomp/osc_decomp_fast.m
@@ -0,0 +1,198 @@
+function [phi_prop,decomp_mu,decomp_cov,AIC_osc,osc_param] = osc_decomp_fast(y0,fs,MAX_COMPONENT,T1,T2)
+%
+% fast version of osc_decomp
+%
+% This function reduces computational cost by using only part of the input time series for parameter estimation
+%
+% Input:
+%    y0:            time series (1 times T0)
+%    fs:            sampling frequency (scalar)
+%    MAX_COMPONENT: maximum number of oscillation components (scalar)
+%    T1:            first time point of the interval for parameter estimation (scalar)
+%    T2:            last time point of the interval for parameter estimation (scalar)
+%                   (Namely, y0(T1:T2) is used for parameter estimation)
+%
+% Output:
+%    phi_prop:      estimated phase of each oscillation component (MAX_COMPONENT times T0 times MAX_COMPONENT)
+%                   phi_prop(k,:,K) is the estimated phase of the k-th
+%                   oscillator in the decomposition into K components
+%    decomp_mu:     smoothed coordinate of each oscillator (2*MAX_COMPONENT times T0 times MAX_COMPONENT)
+%                   decomp_mu(2*k-1:2*k,:,K) is the smoothed coordinate of the k-th oscillator in the decomposition into K components
+%    decomp_cov:    smoothed covariance of each oscillator (2*MAX_COMPONENT times 2*MAX_COMPONENT times T0 times MAX_COMPONENT)
+%                   decomp_cov(2*k-1:2*k,2*k-1:2*k,:,K) is the smoothed covariance of the k-th oscillator in the decomposition into K components
+%    AIC_osc:       AIC of the oscillator model (1 times MAX_COMPONENT)
+%                   AIC_osc(K) is the AIC of the oscillator model with K oscillation components
+%    osc_param:     estimated parameters of the oscillator model (MAX_COMPONENT times 3*MAX_COMPONENT+1)
+%                   osc_param(K,1:K) is the estimated a_1,...,a_K of the oscillator model with K oscillation components
+%                   osc_param(K,K+1:2*K) is the estimated f_1,...f_K of the oscillator model with K oscillation components
+%                   osc_param(K,2*K+1:3*K) is the estimated sigma_1^2,...,sigma_K^2 of the oscillator model with K oscillation components
+%                   osc_param(K,3*K+1) is the estimated tau^2 of the oscillator model with K oscillation components
+%
+    options = optimoptions('fminunc','Algorithm','quasi-newton','MaxFunEvals',10000,'UseParallel', ~0 );
+    T0 = length(y0);
+    phi_prop = zeros(MAX_COMPONENT,T0,MAX_COMPONENT);
+    decomp_mu = zeros(2*MAX_COMPONENT,T0,MAX_COMPONENT);
+    decomp_cov = zeros(2*MAX_COMPONENT,2*MAX_COMPONENT,T0,MAX_COMPONENT);
+    AIC_osc = zeros(1,MAX_COMPONENT);
+    osc_param = zeros(MAX_COMPONENT,3*MAX_COMPONENT+1);
+    y = y0(T1:T2);
+    T = length(y);
+    MAX_AR = 2*MAX_COMPONENT;
+    AR_fit;
+    for num_component=1:MAX_COMPONENT
+        num_component
+        minAIC = inf;
+        minAIC2 = inf;
+        for ARdeg=num_component:2*num_component
+            tmp = roots([1 ARwithnoise_param(ARdeg,1:ARdeg)]);
+            if ARdeg-nnz(imag(tmp))/2 == num_component && AIC_ARwithnoise(ARdeg) < minAIC
+                z0 = tmp;
+                E0 = ARwithnoise_param(ARdeg,ARdeg+1);
+                R0 = ARwithnoise_param(ARdeg,ARdeg+2);
+                minAIC = AIC_ARwithnoise(ARdeg);
+                optARdeg = ARdeg;
+            end
+            if ARdeg-nnz(imag(tmp))/2 >= num_component && AIC_ARwithnoise(ARdeg) < minAIC2
+                z1 = tmp;
+                E1 = ARwithnoise_param(ARdeg,ARdeg+1);
+                R1 = ARwithnoise_param(ARdeg,ARdeg+2);
+                minAIC2 = AIC_ARwithnoise(ARdeg);
+                optARdeg2 = ARdeg;
+            end
+        end
+        if minAIC == inf
+            if minAIC2 == inf
+                warning('no AR model with %d oscillators',num_component);
+            end
+            z0 = z1;
+            E0 = E1;
+            R0 = R1;
+            optARdeg = optARdeg2;
+            [B,I] = sort(abs(z0),'descend');
+            z0 = z0(I);
+        end
+        [tmp,ARdeg] = min(AIC_ARwithnoise);
+        tmp = roots([1 ARwithnoise_param(ARdeg,1:ARdeg)]);
+        if nnz(imag(tmp)>=0) >= num_component
+            z0 = tmp;
+            E0 = ARwithnoise_param(ARdeg,ARdeg+1);
+            R0 = ARwithnoise_param(ARdeg,ARdeg+2);
+            optARdeg = ARdeg;
+        end
+        VV = zeros(optARdeg,optARdeg);
+        for j=1:optARdeg
+            for i=1:optARdeg
+                VV(i,j) = z0(j)^(1-i);
+            end
+        end
+        QQ = inv(VV)*[E0 zeros(1,optARdeg-1); zeros(optARdeg-1,optARdeg)]*inv(VV)';
+        [B,I] = sort(diag(real(QQ))./(1-abs(z0).^2),'descend');
+        z0 = z0(I);
+        
+        init_a = zeros(1,num_component);
+        init_f = zeros(1,num_component);
+        kk = 1;
+        for k=1:num_component
+            init_a(k) = abs(z0(kk));
+            init_f(k) = abs(angle(z0(kk)));
+	        if isreal(z0(kk)) && z0(kk) < 0
+        	    init_f(k) = 4; % for pi
+	        end
+            if imag(z0(kk)) == 0
+                kk = kk+1;
+            else
+                kk = kk+2;
+            end
+        end
+        [B,I] = sort(init_f);
+        init_a = init_a(I);
+        init_f = init_f(I);
+%    	nf0 = nnz(init_f==0);
+%    	nf1 = nnz(init_f==4);
+%	    freq = init_f;
+%    	freq(2:nf0) = pi*rand(1,nf0-1);
+%	    freq(num_component-nf1+2:num_component) = pi*rand(1,nf1-1);
+        if mod(T,2) == 0
+        	freq = [2*pi/T*(0:T/2-1) pi];
+	    else
+    	    freq = [2*pi/T*(0:(T-1)/2)];
+        end
+        init_f(init_f==4) = pi;
+        P = zeros(length(freq),num_component);
+        for k=1:num_component
+            a = init_a(k);
+            theta = init_f(k);
+            A = (1-2*a^2*cos(theta)^2+a^4*cos(2*theta))/a/(a^2-1)/cos(theta);
+            b = (A-2*a*cos(theta)+sign(cos(theta))*sqrt((A-2*a*cos(theta))^2-4))/2;
+            for j=1:length(freq)
+                P(j,k) = -a*cos(theta)/b*abs(1+b*exp(-1i*freq(j)))^2/abs(1-2*a*cos(theta)*exp(-1i*freq(j))+a^2*exp(-2*1i*freq(j))).^2;
+            end
+        end
+        p = zeros(length(freq),1);
+        for j=1:length(freq)
+            p(j) = abs(y*exp(-1i*freq(j)*(0:T-1)'))^2/T;
+        end
+        if cond(P'*P) < 10^6    % modified 2017/01/14
+            init_sigma = (P'*P)\(P'*p);
+        else
+            init_sigma = expGLMfit(P,p);
+        end
+        init_sigma(init_sigma<0) = R0;
+        
+        param = fminunc(@(param)prop_ll(y,param,init_f),[atanh(2*init_a-1) zeros(1,num_component) log(init_sigma'/R0)],options);
+        [mll,sigman] = prop_ll(y,param,init_f);
+        param(num_component+1:2*num_component) = abs(init_f+tanh(param(num_component+1:2*num_component))*pi);
+        AIC_osc(num_component) = 2*mll+2*(3*num_component+1);
+	    [tmp,I] = sort(param(num_component+1:2*num_component));
+        a = (tanh(param(I))+1)/2;
+        theta = param(num_component+I);
+        sigma = exp(param(2*num_component+I))*sigman;
+        osc_param(num_component,1:3*num_component+1) = [a theta*fs/2/pi sigma sigman];
+    
+        m = 2*num_component;
+        x_pred1 = zeros(m,T0);
+        x_filt = zeros(m,T0);
+        x_smooth = zeros(m,T0);
+        V_pred1 = zeros(m,m,T0);
+        V_filt = zeros(m,m,T0);
+        V_smooth = zeros(m,m,T0);
+        F = zeros(m,m);
+        Q = zeros(m,m);
+        H = zeros(1,m);
+        for k=1:num_component
+            F(2*k-1:2*k,2*k-1:2*k) = a(k)*[cos(theta(k)) -sin(theta(k)); sin(theta(k)) cos(theta(k))];
+            Q(2*k-1,2*k-1) = 1-a(k)^2;
+            Q(2*k,2*k) = 1-a(k)^2;
+        end
+        H(1:2:m) = sqrt(sigma)./sqrt(1-a.^2);
+        R = sigman;
+        x_pred1(:,1) = zeros(m,1);
+        for k=1:num_component
+            V_pred1(2*k-1:2*k,2*k-1:2*k,1) = eye(2);
+        end
+        for t=1:T0-1
+            Kg = V_pred1(:,:,t)*H'*inv(H*V_pred1(:,:,t)*H'+R);
+            x_filt(:,t) = x_pred1(:,t) + Kg*(y0(t)-H*x_pred1(:,t));
+            V_filt(:,:,t) = (eye(m)-Kg*H)*V_pred1(:,:,t);
+            x_pred1(:,t+1) = F*x_filt(:,t);
+            V_pred1(:,:,t+1) = F*V_filt(:,:,t)*F'+Q;
+        end
+        Kg = V_pred1(:,:,T0)*H'*inv(H*V_pred1(:,:,T0)*H'+R);
+        x_filt(:,T0) = x_pred1(:,T0) + Kg*(y0(T)-H*x_pred1(:,T0));
+        V_filt(:,:,T0) = (eye(m)-Kg*H)*V_pred1(:,:,T0);
+        x_smooth(:,T0) = x_filt(:,T0);
+        V_smooth(:,:,T0) = V_filt(:,:,T0);
+        for t=T0-1:-1:1
+            x_smooth(:,t) = x_filt(:,t) + V_filt(:,:,t)*F'*(V_pred1(:,:,t+1)\(x_smooth(:,t+1)-x_pred1(:,t+1)));
+            CC = V_filt(:,:,t)*F'*inv(V_pred1(:,:,t+1));
+            V_smooth(:,:,t) = V_filt(:,:,t) + CC*(V_smooth(:,:,t+1)-V_pred1(:,:,t+1))*CC';
+        end
+        for k=1:num_component
+            phi_prop(k,:,num_component) = atan2(x_smooth(2*k,:),x_smooth(2*k-1,:));
+        end
+        decomp_mu(1:m,:,num_component) = x_smooth;
+        decomp_cov(1:m,1:m,:,num_component) = V_smooth;
+    end
+end
+
+
diff --git a/osc_decomp/plot_decomp.m b/osc_decomp/plot_decomp.m
new file mode 100644
index 0000000..1fde133
--- /dev/null
+++ b/osc_decomp/plot_decomp.m
@@ -0,0 +1,27 @@
+function []=plot_decomp(y,fs,decomp_mu,decomp_cov,osc_param,num_component)
+    T = length(y);
+    T1 = 1;
+    T2 = T;
+    Tstart = 0;
+    figure;
+    subplot(num_component+2,1,1),plot(Tstart+(T1:T2)/fs,y(T1:T2)),xlim(Tstart+[T1 T2]/fs);
+    set(gca,'FontSize',12);
+    vars = zeros(num_component,T2-T1+1);
+    comp_sum = zeros(1,T2-T1+1);
+    for i=1:num_component
+        c = sqrt(osc_param(num_component,2*num_component+i))./sqrt(1-osc_param(num_component,i).^2);
+        subplot(num_component+2,1,i+1);
+        hold on
+        std_bar = reshape(sqrt(decomp_cov(2*i-1,2*i-1,T1:T2,num_component)),1,T2-T1+1);
+        vars(i,:) = decomp_cov(2*i-1,2*i-1,T1:T2,num_component);
+        xx = [Tstart+(T1:T2)/fs Tstart+(T2:-1:T1)/fs]';
+        yy = [c*(decomp_mu(2*i-1,T1:T2,num_component)-chi2inv(0.9,1)*std_bar) c*(decomp_mu(2*i-1,T2:-1:T1,num_component)+chi2inv(0.9,1)*std_bar(T2-T1+1:-1:1))]';
+        fill(xx,yy,[.8,.8,.8],'EdgeColor','none');
+        plot(Tstart+(T1:T2)/fs,c*decomp_mu(2*i-1,T1:T2,num_component));
+        xlim(Tstart+[T1 T2]/fs);
+        set(gca,'FontSize',12);
+        comp_sum = comp_sum+c*decomp_mu(2*i-1,T1:T2,num_component);
+    end
+    subplot(num_component+2,1,num_component+2),plot(Tstart+(T1:T2)/fs,y(T1:T2)-comp_sum),xlim(Tstart+[T1 T2]/fs);
+    set(gca,'FontSize',12);
+end
diff --git a/osc_decomp/plot_decomp_one.m b/osc_decomp/plot_decomp_one.m
new file mode 100644
index 0000000..94fe8bb
--- /dev/null
+++ b/osc_decomp/plot_decomp_one.m
@@ -0,0 +1,17 @@
+function []=plot_decomp_one(y,fs,decomp_mu,decomp_cov,osc_param,num_component,k)
+    T = length(y);
+    T1 = 1;
+    T2 = T;
+    Tstart = 0;
+    vars = zeros(num_component,T2-T1+1);
+    i = k;
+        c = sqrt(osc_param(num_component,2*num_component+i))./sqrt(1-osc_param(num_component,i).^2);
+        hold on
+        std_bar = reshape(sqrt(decomp_cov(2*i-1,2*i-1,T1:T2,num_component)),1,T2-T1+1);
+        xx = [Tstart+(T1:T2)/fs Tstart+(T2:-1:T1)/fs]';
+        yy = [c*(decomp_mu(2*i-1,T1:T2,num_component)-chi2inv(0.9,1)*std_bar) c*(decomp_mu(2*i-1,T2:-1:T1,num_component)+chi2inv(0.9,1)*std_bar(T2-T1+1:-1:1))]';
+        fill(xx,yy,[.8,.8,.8],'EdgeColor','none');
+        plot(Tstart+(T1:T2)/fs,c*decomp_mu(2*i-1,T1:T2,num_component));
+        xlim(Tstart+[T1 T2]/fs);
+        set(gca,'FontSize',12);
+end
diff --git a/osc_decomp/plot_phase.m b/osc_decomp/plot_phase.m
new file mode 100644
index 0000000..4cbbd04
--- /dev/null
+++ b/osc_decomp/plot_phase.m
@@ -0,0 +1,27 @@
+function []=plot_phase(y,fs,phi_prop,decomp_mu,decomp_cov,num_component)
+    nmc = 10^4;
+    T = length(y);
+    T1 = 1;
+    T2 = T;
+    Tstart = 0;
+    phase1 = zeros(1,T2-T1+1);
+    phase2 = zeros(1,T2-T1+1);
+    seeds = randn(2,nmc);
+    figure;
+    for k=1:num_component
+        for t=T1:T2
+            tmp = angle_conf_MC(decomp_mu(2*k-1:2*k,t,num_component),decomp_cov(2*k-1:2*k,2*k-1:2*k,t,num_component),2*normcdf(1)-1,seeds);
+            phase1(t-T1+1) = tmp(1);
+            phase2(t-T1+1) = tmp(2);
+        end
+        subplot(num_component,1,k)
+        hold on
+        plot_phase_area(Tstart+(T1:T2)/fs,phi_prop(k,T1:T2,num_component),phase1,phase2,[.8,.8,.8]);
+        plot_phase_nocross(Tstart+(T1:T2)/fs,phi_prop(k,T1:T2,num_component),'b',2);
+        xlim(Tstart+[T1 T2]/fs);
+        set(gca,'FontSize',12);
+        ax = gca;
+        set(ax,'YTick',[-pi 0 pi]);
+        set(ax,'YTickLabel',{'-3.14', '0', '3.14'});
+    end
+end
diff --git a/osc_decomp/plot_phase_area.m b/osc_decomp/plot_phase_area.m
new file mode 100644
index 0000000..b8d4b21
--- /dev/null
+++ b/osc_decomp/plot_phase_area.m
@@ -0,0 +1,106 @@
+function ret = plot_phase_area(t,phase,lphase,uphase,C)
+    for i=1:length(t)-1
+        lturn = 0;
+        uturn = 0;
+        if (phase(i)-lphase(i))*(phase(i+1)-lphase(i+1))<0
+            if lphase(i)<phase(i) && phase(i)-phase(i+1)<pi
+                lturn = -1;
+            end
+            if lphase(i)>phase(i) && phase(i)-phase(i+1)>-pi
+                lturn = 1;
+            end
+        else
+            if phase(i)-phase(i+1)>pi
+                lturn = 1;
+            end
+            if phase(i)-phase(i+1)<-pi
+                lturn = -1;
+            end
+        end
+        if (phase(i)-uphase(i))*(phase(i+1)-uphase(i+1))<0
+            if uphase(i)<phase(i) && phase(i)-phase(i+1)<pi
+                uturn = -1;
+            end
+            if uphase(i)>phase(i) && phase(i)-phase(i+1)>-pi
+                uturn = 1;
+            end
+        else
+            if phase(i)-phase(i+1)>pi
+                uturn = 1;
+            end
+            if phase(i)-phase(i+1)<-pi
+                uturn = -1;
+            end
+        end
+        if lphase(i)<uphase(i)
+            if lphase(i+1)<uphase(i+1)
+                ld = lphase(i);
+                lu = uphase(i);
+                rd = lphase(i+1)+lturn*2*pi;
+                ru = uphase(i+1)+uturn*2*pi;
+                fill([t(i) t(i+1) t(i+1) t(i)]',[ld rd ru lu]',C,'EdgeColor','none');
+                ld = lphase(i)-lturn*2*pi;
+                lu = uphase(i)-uturn*2*pi;
+                rd = lphase(i+1);
+                ru = uphase(i+1);
+                fill([t(i) t(i+1) t(i+1) t(i)]',[ld rd ru lu]',C,'EdgeColor','none');
+            else
+                ld = lphase(i);
+                lu = uphase(i);
+                rd = lphase(i+1)+lturn*2*pi;
+                ru = uphase(i+1)+uturn*2*pi;
+                fill([t(i) t(i+1) t(i+1) t(i)]',[ld rd ru lu]',C,'EdgeColor','none');
+                ld = lphase(i)-(lturn+uturn)*2*pi;
+                lu = uphase(i)-(lturn+uturn)*2*pi;
+                rd = lphase(i+1)-uturn*2*pi;
+                ru = uphase(i+1)-lturn*2*pi;
+                fill([t(i) t(i+1) t(i+1) t(i)]',[ld rd ru lu]',C,'EdgeColor','none');
+            end
+        else
+            if lphase(i+1)<uphase(i+1)
+                ld = lphase(i)-lturn*2*pi;
+                lu = uphase(i)-uturn*2*pi;
+                rd = lphase(i+1);
+                ru = uphase(i+1);
+                fill([t(i) t(i+1) t(i+1) t(i)]',[ld rd ru lu]',C,'EdgeColor','none');
+                ld = lphase(i)+uturn*2*pi;
+                lu = uphase(i)+lturn*2*pi;
+                rd = lphase(i+1)+(lturn+uturn)*2*pi;
+                ru = uphase(i+1)+(lturn+uturn)*2*pi;
+                fill([t(i) t(i+1) t(i+1) t(i)]',[ld rd ru lu]',C,'EdgeColor','none');
+            else
+                if lturn == 0
+                    ld = lphase(i);
+                    lu = uphase(i)+2*pi;
+                    rd = lphase(i+1);
+                    ru = uphase(i+1)+2*pi;
+                    fill([t(i) t(i+1) t(i+1) t(i)]',[ld rd ru lu]',C,'EdgeColor','none');
+                    ld = lphase(i)-2*pi;
+                    lu = uphase(i);
+                    rd = lphase(i+1)-2*pi;
+                    ru = uphase(i+1);
+                    fill([t(i) t(i+1) t(i+1) t(i)]',[ld rd ru lu]',C,'EdgeColor','none');
+                else
+                    ld = lphase(i);
+                    lu = uphase(i)+lturn*2*pi;
+                    rd = lphase(i+1)+lturn*2*pi;
+                    ru = uphase(i+1)+lturn*4*pi;
+                    fill([t(i) t(i+1) t(i+1) t(i)]',[ld rd ru lu]',C,'EdgeColor','none');
+                    ld = lphase(i)-lturn*2*pi;
+                    lu = uphase(i);
+                    rd = lphase(i+1);
+                    ru = uphase(i+1)+lturn*2*pi;
+                    fill([t(i) t(i+1) t(i+1) t(i)]',[ld rd ru lu]',C,'EdgeColor','none');
+                    ld = lphase(i)-lturn*4*pi;
+                    lu = uphase(i)-lturn*2*pi;
+                    rd = lphase(i+1)-lturn*2*pi;
+                    ru = uphase(i+1);
+                    fill([t(i) t(i+1) t(i+1) t(i)]',[ld rd ru lu]',C,'EdgeColor','none');
+                end
+            end
+        end
+    end
+    ylim([-pi pi]);
+    ret = 0;
+end
+
diff --git a/osc_decomp/plot_phase_nocross.m b/osc_decomp/plot_phase_nocross.m
new file mode 100644
index 0000000..0253fcc
--- /dev/null
+++ b/osc_decomp/plot_phase_nocross.m
@@ -0,0 +1,19 @@
+function ret = plot_phase_nocross(t,phase,lineoption,linewidth)
+    for i=1:length(t)-1
+        if phase(i)-phase(i+1) > pi
+            turn = t(i)+(pi-phase(i))/(phase(i+1)+2*pi-phase(i))*(t(i+1)-t(i));
+            plot([t(i) turn],[phase(i) pi],lineoption,'LineWidth',linewidth);
+            plot([turn t(i+1)],[-pi phase(i+1)],lineoption,'LineWidth',linewidth);
+        else
+            if phase(i)-phase(i+1) < -pi
+                turn = t(i)+(phase(i)+pi)/(phase(i)-phase(i+1)+2*pi)*(t(i+1)-t(i));
+                plot([t(i) turn],[phase(i) -pi],lineoption,'LineWidth',linewidth);
+                plot([turn t(i+1)],[pi phase(i+1)],lineoption,'LineWidth',linewidth);
+            else
+                plot([t(i) t(i+1)],[phase(i) phase(i+1)],lineoption,'LineWidth',linewidth);
+            end
+        end
+    end
+    ret = 0;
+end
+
diff --git a/osc_decomp/prop_ll.m b/osc_decomp/prop_ll.m
new file mode 100644
index 0000000..8958efd
--- /dev/null
+++ b/osc_decomp/prop_ll.m
@@ -0,0 +1,39 @@
+function [mll,Rhat] = prop_ll(y,param,init_f)
+    T = length(y);
+    num_osc = length(param)/3;
+    param(num_osc+1:2*num_osc) = init_f+tanh(param(num_osc+1:2*num_osc))*pi;
+    F = zeros(2*num_osc,2*num_osc);
+    Q = zeros(2*num_osc,2*num_osc);
+    for i=1:num_osc
+        F(2*i-1:2*i,2*i-1:2*i) = (tanh(param(i))+1)/2*[cos(param(num_osc+i)) -sin(param(num_osc+i)); sin(param(num_osc+i)) cos(param(num_osc+i))];
+        Q(2*i-1:2*i,2*i-1:2*i) = exp(param(2*num_osc+i))*eye(2);
+    end
+    H = zeros(1,2*num_osc);
+    H(1:2:2*num_osc) = 1;
+    R = 1;
+
+    x_pred1 = zeros(2*num_osc,T);
+    x_filt = zeros(2*num_osc,T);
+    V_pred1 = zeros(2*num_osc,2*num_osc,T);
+    V_filt = zeros(2*num_osc,2*num_osc,T);
+    x_pred1(:,1) = zeros(2*num_osc,1);
+    for i=1:num_osc
+        V_pred1(2*i-1:2*i,2*i-1:2*i,1) = Q(2*i-1:2*i,2*i-1:2*i)/(1-F(2*i-1,2*i-1)^2-F(2*i-1,2*i)^2);
+    end
+    for t=1:T-1
+        x_filt(:,t) = x_pred1(:,t) + V_pred1(:,:,t)*H'*((H*V_pred1(:,:,t)*H'+R)\(y(t)-H*x_pred1(:,t)));
+        V_filt(:,:,t) = V_pred1(:,:,t) - V_pred1(:,:,t)*H'*((H*V_pred1(:,:,t)*H'+R)\H)*V_pred1(:,:,t);
+        x_pred1(:,t+1) = F*x_filt(:,t);
+        V_pred1(:,:,t+1) = F*V_filt(:,:,t)*F'+Q;
+    end
+    Rhat = 0;
+    for t=1:T
+        Rhat = Rhat*(t-1)/t+(y(t)-H*x_pred1(:,t))^2/(H*V_pred1(:,:,t)*H'+R)/t;
+    end
+    ll = -T*log(Rhat)/2-T/2;
+    for t=1:T
+        ll = ll-log(H*V_pred1(:,:,t)*H'+R)/2;
+    end
+    mll = -ll;
+end
+