pcaBleach2

0001 function [x, tRng, pos] = pcaBleach2(res, tRng)
0002   if nargin<2
0003     [pos, tRng, bPos, head] = resInterpl( res );
0004   else
0005     [pos, tRng, bPos, head] = resInterpl( res, tRng );
0006   end
0007   
0008   [lpb,lpa] = butter(1,0.25);
0009   
0010   % Positions are lowpass version of bPos coordinates
0011   lpx = filtfilt(lpb,lpa,real(bPos).');
0012   [x,xs,x0] = bleach(lpx);
0013   x = fixAt( x, isnan(x) );
0014   
0015   lpy = filtfilt(lpb,lpa,imag(bPos).');
0016   [y,ys,y0] = bleach(lpy);
0017   y = fixAt( y, isnan(y) );
0018 
0019   % Body velocity in body frame - rotate back along heading
0020   bv = diff(pos(7:8,:).') .* exp(-(head(2:end)+head(1:end-1))*[i i]/2);
0021 
0022   % Velocities are differences of position
0023   %  with extra columns for body velocity. Because the real part of
0024   %  bv must be the same for both entries, we use the average
0025   [vx,vxs,vx0] = bleach(diff(lpx));
0026   [vy,vys,vy0] = bleach(diff(lpy));
0027   bv = [ mean(real(bv),2) imag(bv) ];
0028 
0029   % Find outlier velocities and other entries to be fixed
0030   badv = isnan(vx) | isnan(vy);
0031   badv = [ badv badv ];
0032   x = [fixAt( [x(2:end,:) y(2:end,:)], badv), ...
0033        fixAt( [vx vy], badv), ...
0034        bv ];
0035   gx = ~any(isnan(x),2);
0036 
0037   x = x(gx,:);
0038   tRng = tRng(gx);
0039   pos = pos(:,gx);
0040   
0041 function [x,xs,x0] = bleach( x )
0042   % Remove outliers -- anything outside 3 stddevs in each column
0043     xn = x -  repmat(mean(x), size(x,1), 1);
0044   xn = xn ./ repmat( std(xn), size(xn,1), 1);
0045   x(abs(xn)>3) = nan;
0046   
0047   for col=1:size(x,2)
0048     % Obtain average and stddev of all non-outliers
0049     good = ~isnan(x(:,col)); % note: may have been nan-s in original data
0050     x0(col) = mean(x(good,col));
0051     xs(col) = std(x(good,col));
0052   end
0053 
0054 function x = fixAt( x, fix, varargin )
0055   if length(varargin)==0
0056     varargin={'linear'};
0057   end
0058   idx = 1:size(x,1);
0059   for k=1:size(x,2)
0060     if any(fix(:,k))
0061       good = ~fix(:,k);
0062       bad = ~good;
0063       x(bad,k) = interp1( idx(good), x(good,k), find(bad), varargin{:} );
0064     end
0065   end