stepfit1_alvaro.m

function est=stepfit1_alvaro(data,varargin)
%    
%USAGE:
%x=stepfit1(y,'parameter',value)
%
%x           Step Fit
%y           Data on which to fit steps
%Optional Parameters:
%Fs                 Sampling time. Required if tau is being specified.
%tau                Time constant of first order dynamics 
%outputnoise Standard deviation of the noise measured at output, Estimates noise at the output if unspecified.
%                      Noise estimation is not reliable under extreme dynamical distortion and high stepping speed.
%measnoise   Standard deviation of the noise in the measurement system.
%                       Assumes zero measurement noise if unspecifed
%passes          Number of iterations in histogram refinement
%verbose         verbose=1: Display progress of histogram.  verbose=2: Save the progress in fig files


numarg=nargin-1;
Fs=1;
tau=0;
outputnoise=noise_std(data);
measnoise=0;
passes=10;
verbose=0;
for i=1:2:numarg
    msg=[varargin{i} '=' num2str(varargin{i+1})];
    evalc(msg);
%     disp(msg);
end
msg=sprintf('Fs=%.2e \n tau= %.2e \n outputnoise=%.2e \n measnoise=%.2e \n passes= %.2e \n verbose=%.2e',Fs,tau,outputnoise,measnoise,passes,verbose);
disp(msg);
est=stepfit_sub(data,Fs,tau,outputnoise,measnoise,passes,verbose);
LLR=stepfitLLR(data,est);
fprintf('Stepping LLR=%.3f',LLR)
function est=stepfit_sub(data,varargin)
global userargs Fs tau noise outputnoise measnoise resolution
global pass passes verbose b a envelopeflag y endbounding
y=data(:)';
for i=1:6
    userargs{i}=[];
end
for i=1:nargin-1
    userargs{i}=varargin{i};
end
Fs=1;
tau=0;
evalc('outputnoise=noise_std(y)');
measnoise=0;
resolution=.1;
passes=10;
verbose=0;
if(nargin<1);disp('Atleast 1 argument required');return;end   
if(nargin>1);if(~isempty(varargin{1}));Fs=varargin{1};end;end        
if(nargin>2);if(~isempty(varargin{2}));tau=varargin{2};end;end   
if(nargin>3);if(~isempty(varargin{3}));outputnoise=varargin{3};end;end
if(nargin>4);if(~isempty(varargin{4}));measnoise=varargin{4};end;end 
if(nargin>5);if(~isempty(varargin{5}));passes=varargin{5};end;end   
if(nargin>6);if(~isempty(varargin{6}));verbose=varargin{6};end;end   
try%check for parallel processing capability
    s=matlabpool('size');
    if(s<=1)
        disp('starting parallel MATLAB sessions...')
        evalc('matlabpool open');
        disp([char(8) 'Done '])
    end
catch  %#ok<*CTCH>
    disp('Parallel Processing toolbox not found');
end
try  %find coefficients of dynamical model
    if(tau~=0)
        sysc=tf(1,[tau 1]);
        sysd=c2d(sysc,1/Fs,'zoh');
        [b,a]=tfdata(sysd,'v');
    else
        b=1;
        a=[1 0];
    end
catch
    disp('Warning:Control Systems Toolbox not found: Using unoptimized discretization for dynamics representation!');
    b=1/Fs/tau;
    a=[1 1/Fs/tau-1];
end
% if(verbose)
%     disp(['b: [' num2str(b) ']'])
%     disp(['a: [' num2str(a) ']'])
% end
[h,w]=freqz(b,a,1000);
noiseamp=sqrt(sum((abs(h)).^2)/length(h));
noise=max(0,(outputnoise-measnoise)/noiseamp); % this is thermal noise SD
est=y;
envelopeflag=0;
endbounding=0;

wb = waitbar(0,'Starting step fit function calculation...','Units','pixels','Visible','off');
movegui(wb,'center');
pos = get(wb,'Position');
movegui(wb,[pos(1),pos(2)-57]);
set(wb,'Visible','on');

for pass=1:passes
    
    waitbar(pass/passes,wb,['Calculating step fit function...',char(10),'Iteration ',num2str(pass),' of ',num2str(passes)]);
    
    est=vitpass(y,est);
    steps=diff(est);
%     if(verbose)
        disp( [char(8) ', ' num2str(sum(logical(steps))) ' steps']);
%     end
end

close(wb);

if(nargout>1)
    stepfitconfidence(y,est);
end
%---------------------------------------------------------------------
function est=vitpass(y,estin)
global userargs outputnoise measnoise pass wt noise 
global verbose resolution lowsave highsave b a
% partition data into blocks and run viterbi on each block
% global trackcost
%% windowsize
N=length(y);
imp=zeros(1,1000);
imp(1)=1;
impf=filter(b,a,imp);
bw=find(impf>0.2*max(impf),1,'last');
window=max(10,min(length(y),bw));
%% noiseamp
if(pass>=1)%noiseamp
    [h,w]=freqz(b,a,10000);
    noiseamp=sqrt(sum((abs(h)).^2)/length(h));
    noise=(outputnoise-measnoise)/noiseamp;
end
%% ncores
try
    ncores=matlabpool('size');
catch
    ncores=1;
end%ncores
%% Blocksize

steps=diff(estin);
ind=find(steps);
if(pass==1)
    meandwell=500;
else
    meandwell=max(500,round(mean(diff(ind))));
end
blocksize=max(10*meandwell,ceil(N/ncores/ceil(N/10/meandwell)));
r=floor((N-1)/blocksize);
overlap=max(.1*blocksize,2*meandwell);
est=zeros(1,length(y));
%% noise reestimation
if(pass>1 && isempty(userargs{3}))%outputnoise 
    outputnoise=noise_std(y-filter(b,a,estin)); %re-estimate noise
end
%% envelope
if(pass==1)%envelope
    [low high maxstep minstep miny maxy]=findenvelope2(estin,window);
    lowsave=low;
    highsave=high;
    resolution=mean(high-low)/20; % refine resolution 
else
[low high maxstep minstep miny maxy resolution ]=findenvelope3(estin,window,resolution,lowsave,highsave);
lowsave=low;
highsave=high;
end
low=round((low-miny)/resolution)+1;
high=round((high-miny)/resolution)+1;
message=['Pass:' num2str(pass) ', Resolution=' sprintf('%4.3f',resolution) ];
% if(verbose)
    fprintf(message);
% end
%% compute weight 

if(pass<=1 )
    range=minstep-1:resolution:maxstep;
    [val in]=min(abs(range));
    range=range-range(in);
    wt.zeroindex=in;
    wt.pdf=abs(sign(range));
else
   wt=computeweight(estin,minstep,maxstep);
end%compute weight
start=zeros(1,r+1);
stop=zeros(1,r+1);
eststart=zeros(1,r+1);
eststop=zeros(1,r+1);

%% parallel operation
% if(verbose)
    msg=[char(zeros(1,r+1)+'|')  ' '];
    fprintf(msg);
% end
for j=1:r+1
    i=j-1;
    start(j)=max(blocksize*i+1-overlap,1);
    stop(j)=min(blocksize*(i+1)+overlap,N);
    eststart(j)=blocksize*i+1;
    eststop(j)=min(N,blocksize*(i+1));
    yme{j}=y(start(j):stop(j));
    lowtmp{j}=low(start(j):stop(j));
    hightmp{j}=high(start(j):stop(j));
end %%% slice data for parallel operation
param.noise=noise;
param.measnoise=measnoise;
param.outputnoise=outputnoise;
param.wt=wt;
param.resolution=resolution;
param.b=b;
param.a=a;
param.pass=pass;
for j=1:r+1  %*****************parfor**************
    [tmp, costtmp]=viterbistepdetector(yme{j},miny,lowtmp{j},hightmp{j},param);
    myest{j}=tmp;
%     if(verbose)
        disp([char(8) char(8)  ]);
%     end
end
disp([char(8)]);
for j=1:r+1
    tempest=myest{j};
    est(eststart(j):eststop(j))=tempest(eststart(j)-start(j)+1:eststop(j)-start(j)+1);
end  %% merge sliced data 

%% display progress
if(verbose)
    figure(4);clf;
    %         nr=floor(sqrt(passes));
    %         nc=ceil(passes/nr);
    subplot(2,1,1);
    v=1:N;
    rv=N:-1:1;
    polyx=[v rv];
    polyenv=[(low-1)*resolution+miny (fliplr(high)-1)*resolution+miny];
    polyy=[y rv*0];
    py=fill(polyx,polyy,'k');hold on;set(py,'facealpha',0,'edgealpha',0.1);
    pf=fill(polyx,polyenv,'r');set(pf,'facealpha',0.2,'edgealpha',0);hold on;
    plot(est,'r','linewidth',2);
    xlabel('Samples');
    ylabel('Position');
    title(['Iteration ' num2str(pass)]);
%     legend('Data','Envelope','Fit','location','best');
    axis tight;
    
    subplot(2,1,2);
    cla
    x=resolution*((1:length(wt.pdf))-wt.zeroindex);
    smoothedhistogram=exp(-wt.pdf);
    smoothedhistogram=smoothedhistogram-min(smoothedhistogram);
    smoothedhistogram(wt.zeroindex)=0;        
    [bins nhist]=stephist(est,linspace(min(x),max(x),25),gcf,'normalized','bar');    
    if(max(nhist))
        bar(bins,nhist*max(smoothedhistogram)/max(nhist),'b');
    end        
    hold on;
    plot(x-resolution/2,smoothedhistogram,'r','linewidth',1);
    xlabel('Step-size');       
    ylabel('Probability');
    axis tight;
    drawnow();
    if(verbose==2)
        str=sprintf('StepFitIterationProgress %d .fig',pass);
        hgsave(str);
    end
end

function wt=computeweight(est,minstep,maxstep)
global resolution noise pass passes
redfactor=1-0.8*pass/passes;
smooth=max(3,round(redfactor*noise/resolution));
steps=diff(est);
s=minstep-2*smooth*resolution:resolution:maxstep+2*smooth*resolution;
[s0 zindex]=min(abs(s));
s=s-s(zindex);
N=histc(steps,s-resolution/2);

if(smooth)
t=N(zindex);
Ntmp=N;
Ntmp(zindex)=0;
Ntmp(zindex+1)=0 ;
Ntmp(zindex-1)=0 ;
d=smooth;
H=gaussfir(1/d,d,1);
Ntmpright=[Ntmp(zindex+1:end) zeros(1,2*d+1)];
Ntmpleft=[Ntmp(1:zindex-1) zeros(1,2*d+1)];
Ntmpright=filter(H,1,Ntmpright);
Ntmpleft=filter(H,1,Ntmpleft);
Ntmpright(1:end-d)=Ntmpright(d+1:end);
Ntmpleft(1:end-d)=Ntmpleft(d+1:end);
Ntmpleft=Ntmpleft(1:end-2*d-1);
Ntmpright=Ntmpright(1:end-2*d-1);
Ntmp=[Ntmpleft 0 Ntmpright];
Ntmp(zindex)=t*max(H);
N=Ntmp+1e-50;
end
N(zindex+1)=0 ;
N(zindex-1)=0 ;
N=N/sum(N);
N=-log(N);
wt.zeroindex=zindex;
wt.pdf=N;
%---------------------------------------------------------------------
function [est costtrack]=viterbistepdetector(y,miny,low,high,param)

penaltyFactor = 30;

noise=param.noise;
measnoise=param.measnoise;
wt=param.wt;
resolution=param.resolution;
b=param.b;
a=param.a;
pass=param.pass;

if(pass>1 ); %%sigmabar definition
    sigmabar=2*(sum(b.^2)*(noise.^2)+sum(a.^2)*(measnoise^2));
else
    
    sigmabar=penaltyFactor*(noise.^2+measnoise^2+resolution^2);
end
maxstep=max(high(2:end)-low(1:end-1));
minstep=min(low(2:end)-high(1:end-1));
maxband=max(high-low+1); % spread of envelope
S=(low(1):low(1)+maxband-1)'*ones(1,length(y)); % Initialize memory for surviving states.
cost=((S(:,1)-1)*resolution+miny-y(1)).^2; % Initialize cost associated with each end state.
levelsup=0:maxstep;
levelsdown=minstep:1:-1;
levels=max(1,[levelsdown levelsup]'+wt.zeroindex);
levels=min(levels,length(wt.pdf));
pdfcost1=wt.pdf(levels);
pdfcostoffset1=length(levelsdown)+1;
zhat=S*0+y(1); % Initialize memory for surviving output.
costtrack=0;
for t=max(length(a)):length(y)%% Viterbi
    lastrow=(low(t-1):high(t-1))';
    llastrow=length(lastrow);
    row=(low(t):high(t));
    lrow=length(row);
    vr=ones(llastrow,1);
    vc=ones(1,lrow);
    dx=-lastrow*vc+vr*row+pdfcostoffset1;
    z=b(1)*vr*((row-1)*resolution+miny); %************************************
    lastrowtmp=lastrow;
    for i=2:max(length(a),length(b))
        if(t-i>=2)
            idx=lastrowtmp-low(t-i+1)+1;
            if(i<=numel(b)); z=z+b(i)*((lastrowtmp-1)*resolution+miny)*vc; end %********************
            if(i<=numel(a))
                if(pass==1 ); 
                    z=z-a(i)*zhat(idx,t-i+1)*vc;
                else
                    z=z-a(i)*y(t-i+1)*vr*vc ;
                end
            end
            lastrowtmp=low(t-i)-1+S(idx,t-i+1);
        end
    end
    if(min(size(dx))==1)
        noisecost=reshape(pdfcost1(dx),size(dx))*sigmabar;
    else
%         size(pdfcost1(dx))
%         size(sigmabar)
%         sigmabar
        noisecost=pdfcost1(dx)*sigmabar;
    end
    measurementcost=(y(t)-z).^2;
    tempcost= cost(1:llastrow)*vc+measurementcost+noisecost;
    [mincost,ind]=min(tempcost);     %select candidate with least cost
    zhat(1:lrow,t)=z(ind+(0:length(ind)-1)*llastrow)';
    cost(1:lrow)=mincost';
    S(1:lrow,t)=ind;
end
[mincost,ind]=min(cost(1:high(end)-low(end)+1));
est=reconstruction(S,low,ind,resolution,miny);

function [low high maxstep minstep miny maxy]=findenvelope2(signal,window)

low=minfilter(signal,window);
high=maxfilter(signal,window);
mid=(low+high)/2;
low=2*low-mid-2;
high=2*high-mid+2;
maxstep=max(high-low);
minstep=min(low-high);
miny=min(low);
maxy=max(high);
%---------------------------------------------------------------------
function [low high maxstep minstep miny maxy resolution]=findenvelope3(signal,window,resolution,low,high)
global envelopeflag pass topthickness bottomthickness
if(pass<=2)
topthickness=max(5*resolution,mean(high-signal));
bottomthickness=max(5*resolution,mean(signal-low));
end
if(~(min(abs(high-signal))<=resolution || min(abs(low-signal))<=resolution))
    envelopeflag=1;
        topthickness=topthickness/2;
        bottomthickness=bottomthickness/2;
        low=signal-bottomthickness;
high=signal+topthickness;
else
    envelopeflag=0;
%     disp('Envelope too narrow');
    topper=topthickness*(logical(abs(high-signal)<=6*resolution));
    lower=bottomthickness*(logical(abs(signal-low)<=6*resolution));
    low=low-lower;
    high=high+topper;
end
low=min(low,minfilter(signal-2*resolution,window));
high=max(high,maxfilter(signal+2*resolution,window));
finelevel=max(50,1);
resolution=min(resolution,.5*(min(high-low)/10 +max(high-low)/finelevel));

high=max(low+2*resolution,high);
maxstep=max(high(2:end)-low(1:end-1));
minstep=min(low(2:end)-high(1:end-1));
miny=min(low);
maxy=max(high);
%---------------------------------------------------------------------
function y=maxfilter(x,window)
N=length(x);
y=x;
for i=1:N
    start=max(1,i-window);
    stop=min(i+window,N);
    y(i)=max(x(start:stop));
end
%---------------------------------------------------------------------
function y=minfilter(x,window)
N=length(x);
y=x;
for i=1:N
    start=max(1,i-window);
    stop=min(i+window,N);
    y(i)=min(x(start:stop));
end
%---------------------------------------------------------------------
function noiseD=noise_std(y)
y=y(1:min(length(y),10000));
N=floor(length(y)/10);
w=floor(logspace(log10(1),log10(N),50)');
w=unique(w);
m=zeros(length(w),1);
for j=1
    for i=1:length(w);
        v=movingvar(y,w(i)); %compute average noise variance for different window lengths
        m(i,j)=mean(v(w+1:end-w));
    end;
    st=sqrt(mean(m,2));
end;

dst=abs(diff(st)); %compute derivative of variance with respect to window size
[b,a]=butter(1,.6);
dst1=.5*filter(b,a,dst,dst(1))+.5*flipud(filter(b,a,flipud(dst),dst(end)));
dst1=.5*filter(b,a,dst1,dst1(1))+.5*flipud(filter(b,a,flipud(dst1),dst1(end)));
dst1=.5*filter(b,a,dst1,dst1(1))+.5*flipud(filter(b,a,flipud(dst1),dst1(end)));
% [val,in]=min(dst1);%find window size for which corresponds to minimum variation in noise

[val,in]=lmax(-dst1);
noiseD=min(st(in+1));
%---------------------------------------------------------------------
function [v,s]=movingvar(x,m)
offset=floor(m/2);
x=x(:);
s=movingmean(x,m);
N=length(x);
npts=floor(N/m);
locations=round(linspace(1,N,npts));
xt=spline(locations,s(locations),1:N);
xt=xt(:);
x=x-xt;
x=x.^2;
b=ones(1,m)/m;
a=1;
v=filter(b,a,x);
v(offset+1:end-m+offset)=v(m+1:end);
v(1:offset)=v(offset+1);
v(end-m+offset+1:end)=v(end-m+offset);
%---------------------------------------------------------------------
function xm=movingmean(x,m)
x=x(:);
b=ones(1,m)/m;
a=1;
xm=filter(b,a,x);
%---------------------------------------------------------------------
function [y i]=lmax(x)
der=diff(x);
der=sign(der);
i=find(diff(der)<0);
i=i+1;
y=x(i);
%---------------------------------------------------------------------
function c=stepfitconfidence(y,fit)
sm=fit;
sm1=fit;
steplocations=[0 find(diff(fit)) length(fit)];
ind=zeros(length(steplocations)-1,1);
for i=1:length(steplocations)-1
    ind(i)=round((steplocations(i)+1+steplocations(i+1))/2);
    sm(ind(i))=fit(ind(i));
end
sm1(ind(1):ind(end))=interp1(ind,sm(ind),ind(1):ind(end));
c=(-.5*(sum((y-fit).^2)-sum((sm1-y).^2))/var(y-fit))/length(y);

function est=reconstruction(S,low,lastindex,resolution,miny) 
N=size(S,2);
est(N)=(low(N)+lastindex-1-1)*resolution+miny;
ind=lastindex;
for k=N:-1:2
    ind=S(ind,k);
    est(k-1)=(ind+low(k-1)-1-1)*resolution+miny;    
end

function LLR=stepfitLLR(y,fit) % find likelihood ratio of stepping vs. smooth
y=y(:);
fit=fit(:);
sm=fit;
sm1=fit;
steplocations=find(diff(fit));
if(isempty(steplocations))
    LLR=0;
else
    steplocations=[0 ; steplocations ;length(fit)];
    ind=zeros(length(steplocations)-1,1);
    for i=1:length(steplocations)-1
        ind(i)=round((steplocations(i)+1+steplocations(i+1))/2);
        sm(ind(i))=fit(ind(i));
    end
    sm1(ind(1):ind(end))=interp1(ind,sm(ind),ind(1):ind(end));
    LLR=(-.5*(sum((y-fit).^2)-sum((sm1-y).^2))/var(y-fit))/length(y);
end
    


function [bincenters Nhist]=stephist(stepsignal,bincenters,fignum,varargin)
s=size(stepsignal);
l=max(s);
m=min(s);
temp=reshape(stepsignal,l,m);
figure(fignum);
Nhist=zeros(length(bincenters),m);
for j=1:m
    stepsignal=temp(:,j);
    steps=diff(stepsignal);
    steps=steps(logical(steps));
    Nhist(:,j)=histc(steps,bincenters-(bincenters(2)-bincenters(1))/2);
    
    if(nargin>4)
        if((strcmp(varargin{1},'normalized') || strcmp(varargin{1},'Normalized')) && sum(Nhist(:,j))~=0)
            Nhist(:,j)=Nhist(:,j)/sum(Nhist(:,j));
        elseif(strcmp(varargin{1},'smooth') || strcmp(varargin{1},'Smooth'))
            Nhist(:,j)=gaussfir_unbiased(Nhist(:,j),5);

        end
    end
end