Initial commit

.gitattributes

@@ -0,0 +1,2 @@
+# Auto detect text files and perform LF normalization
+* text=auto

README.md

@@ -0,0 +1,2 @@
+# stateSpacePhasePredictor
+ Estimating phase in real time with a state space model tracking the analytic signal 

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
+
+
+
+

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])

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])

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
+

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';