function [VM,VU]=classify_submodes_cmsec_degsec(VR,VT,VS,V1,VE,varargin) % OUTPUT: labeled trajectories, showing submode and mode classification at % VR extrema, in VU and VM, respectively. % INPUT: instantaneous velocity components (rotational, translational, % side-slip, VR,VT,VS) and indices of trajectory start and end (V1,VE). % Optionally, heading angular velocity (VRTH) can be included in inputs or % computed from (VR,VT,VS) when not included. % UNITS: cm/sec (VT,VS); deg/sec (VR,VRTH) % SAMPLING RATE: 30 samples/sec. For data acquired at other rates, either % (1) modify fragment window variables, which are stored in % classification_params_r.mat and are named taup, c1taup, c3taup, c2taup, % c2taup2; or (2) resample data to 30 samples/sec. %% load classification_params_r; sample_rate = 30; % samples per second %% % initialize TU = zeros(size(VR)); % compute heading angular velocity if it is not supplied if isempty(varargin) VRTH = nan(size(VR)); dTH = atan2(VS,VT); % angle between body axis and heading direction, in radians thisi = 0; while thisi(end) < length(VR) thisi = V1(thisi(end)+1):VE(thisi(end)+1); dTHdt = diff(dTH(thisi)); % d/dt of angle dTH, rad/frame dTHdt = atan2( sin(dTHdt), cos(dTHdt) ); dTHdts = smoof2(dTHdt,{[1:length(dTHdt)]},5,1); VRTH(thisi) = VR(thisi)-scaleR*[dTHdts(1);dTHdts]; end; else VRTH = varargin{1}; end; %% % loop through individual trajectories thisi = 0; while thisi(end) < length(VR) % get indices for one trajectory thisi = V1(thisi(end)+1):VE(thisi(end)+1); % find extrema lm3 = localmaxm(VR(thisi), wloc, medge); lm4 = localminm(VR(thisi), wloc, medge); lmh = lm3|lm4; % initialize submode logical indices wanted1 = false(size(lm3)); wanted2 = false(size(lm4)); wanted3 = false(size(lm4)); % wanted19 = false(size(lm3)); wanted18 = false(size(lm3)); wanted17 = false(size(lm3)); wanted16 = false(size(lm3)); % wanted15 = false(size(lm3)); wanted14 = false(size(lm3)); wanted13 = false(size(lm3)); wanted12 = false(size(lm3)); wanted11 = false(size(lm3)); % wanted131 = false(size(lm3)); wanted132 = false(size(lm3)); % wanted121 = false(size(thisi)); wanted122 = false(size(thisi)); wanted123 = false(size(thisi)); wanted124 = false(size(thisi)); % get indices of VR extrema, and eliminate any that are too close to % trajectory beginning or end idw = find(lmh); idw(idw-taup<1)=[]; idw(idw+taup>length(thisi))=[]; % cull measurement errors, if necessary idw(abs(VR(thisi(idw)))<=dhcrit(1))=[]; %% % 1st branchpoint % classify movement around each extremum for w = 1:length(idw) % get a trajectory fragment around an extremum idseg = (thisi(idw(w))-taup:thisi(idw(w))+taup); aseg = ([sign(VR(thisi(idw(w)))).*VRTH(idseg); VT(idseg)]-(scale0.*mavg)')./(scale0.*mstd)'; % project into PCs, then ICs pcs = ( PCM' * aseg ); ics = UMM * pcs(1:6); pic = ((ics(1:3)'-msavg)./msstd)*PIM; % histogram trajectory parameter values in 2 ICs [~, IB1]=histc(pic(1),YED); [~, IB2]=histc(pic(2),YED); % segment clusters in IC space if IB1>0 & IB2>0 IL3 = pic(3) >= xi4(IB1,IB2); ZN2 = abs(VR(thisi(idw(w))))<(scaleR*0.07) & VT(thisi(idw(w)))<(scaleT*0.1); T2S = ( ((VT(thisi(idw(w)))-(scaleT*0.6))./(scaleT*1)).^2 + ( abs( VR( thisi( idw(w) ) ) )./(scaleR*0.062) ).^2 ) >= 1; if IL3 & T2S & ~ZN2 wanted2(idw(w))=true; elseif IL3 & ~T2S & ~ZN2 wanted3(idw(w))=true; else wanted1(idw(w))=true; end; end; clear IL3 ZN*; end; % cull measurement errors if necessary tt2out = (abs(VR(thisi(idw)))<=dhcrit2(1)); wanted2(idw(tt2out)) = false; idw(tt2out) = []; %% % 2nd branchpoint, in tt1 tt1 = find(wanted1); for w1 = 1:length(tt1) idseg = (thisi(tt1(w1))-taup:thisi(tt1(w1))+taup); % same taup interval as in original class. aseg = ([sign(VR(thisi(tt1(w1)))).*VR(idseg); VT(idseg)]-(scale0.*par2.mavg)')./(scale0.*par2.mstd)'; sparm = abs( VS(thisi(tt1(w1))+1) ); pcs = ( par2.PCM' * aseg ); ics = par2.UMM * pcs(1:6); PJ2 = ( ics(1:6)' - mp2 )*uvn2'; PJ22 = ( ics(1:6)' - mp22 )*uvn22'; PJ21 = ( ics(1:6)' - mp21 )*uvn21'; PJ211 = ( ics(1:6)' - mp211 )*uvn211'; if PJ2 >= PJ2C % id22 if PJ22 >= PJ22C & sparm < scaleT*0.12 wanted18(tt1(w1))=true; % stop id222\slide elseif PJ22 >= PJ22C & sparm >= scaleT*0.12 wanted19(tt1(w1))=true; % slide end; elseif PJ21 < PJ21C % id211 if PJ211 >= PJ211C wanted17(tt1(w1))=true; % id2112 else wanted16(tt1(w1))=true; % id2111 end; end; end; tt1 = find( wanted1 & ~(wanted16|wanted17|wanted18|wanted19) ); tt16 = ( wanted16 ); tt17 = ( wanted17 ); tt18 = ( wanted18 ); tt19 = ( wanted19 ); %% % 3rd branchpoint, in tt1 clear w1; for w1 = 1:length(tt1) idseg = (thisi(tt1(w1))-c1taup:thisi(tt1(w1))+c1taup); % different taup interval than in original class. asegd = ([sign(VR(thisi(tt1(w1)))).*VR(idseg); VT(idseg)]-(scale1.*c1d.mavg)')./(scale1.*c1d.mstd)'; asege = ([sign(VR(thisi(tt1(w1)))).*VR(idseg); VT(idseg)]-(scale1.*c1e.mavg)')./(scale1.*c1e.mstd)'; asegf = ([sign(VR(thisi(tt1(w1)))).*VR(idseg); VT(idseg)]-(scale1.*c1f.mavg)')./(scale1.*c1f.mstd)'; idseg3 = (thisi(tt1(w1))-c3taup:thisi(tt1(w1))+c3taup); % different taup interval than in original class. aseg3 = ([VT(idseg3); VS(idseg3); ]-(scale3.*par3.mavg)')./(scale3.*par3.mstd)'; pcs = ( ctt1d' * asegd ); ics = Wd * pcs(1:5); PJ6 = ( ics(1:5)' - mp6 )*uvn6'; if PJ6 < PJ6C pcs = ( ctt1e' * asege ); ics = We * pcs(1:6); PJ61 = ( ics(1:6)' - mp61 )*uvn61'; if PJ61 < PJ61C wanted11(tt1(w1))=true; % id611 else wanted12(tt1(w1))=true; % id612 end; else pcs = ( ctt1f' * asegf ); ics = Wf * pcs(1:6); PJ62 = ( ics(1:6)' - mp62 )*uvn62'; if PJ62 < PJ62C % subdivide pcs3 = ( par3.ctt3a' * aseg3 ); ics3 = par3.W1a * pcs3(1:7); PJ3 = ( ics3(1:5)' - mp3 )*uvn3'; if PJ3 < PJ3C wanted131(tt1(w1))=true; % id621(id31) elseif PJ3 >= PJ3C % elseif because might be unclassified wanted132(tt1(w1))=true; % id621(id32) else wanted13(tt1(w1))=true; % id621 end; else PJ622 = ( ics(1:6)' - mp622 )*uvn622'; if PJ622 < PJ622C wanted14(tt1(w1))=true; % id6221 else wanted15(tt1(w1))=true; % id6222 end; end; end; end; %% % 4th branchpoint, in tt12 idw = find(wanted12); wanted12 = false(size(thisi)); for w = 1:length(idw) idseg = (thisi(idw(w))-c2taup:thisi(idw(w))+c2taup2); aseg = ([sign(VR(thisi(idw(w)))).*VR(idseg); VT(idseg)]-(scale2.*par4.mavg)')./(scale2.*par4.mstd)'; pcs = ( par4.ctt12b' * aseg ); ics = par4.Wb * pcs(1:5); PJ12 = ( ics(2:3)' - mp12 )*uvn12'; PJ12S = ics(1); if PJ12 < PJ12C && PJ12S < PJ12C2 wanted121(idw(w))=true; elseif PJ12 < PJ12C && PJ12S >= PJ12C2 wanted122(idw(w))=true; elseif PJ12 >= PJ12C && PJ12S < PJ12C2 wanted123(idw(w))=true; elseif PJ12 >= PJ12C && PJ12S >= PJ12C2 wanted124(idw(w))=true; else wanted12(idw(w))=true; end; end; tt11 = ( wanted11 ); tt13 = ( wanted13 ); tt131 = ( wanted131 ); tt132 = ( wanted132 ); tt14 = ( wanted14 ); tt15 = ( wanted15 ); tt121 = ( wanted121 ); tt122 = ( wanted122 ); tt123 = ( wanted123 ); tt124 = ( wanted124 ); tt12 = ( wanted12 ); tt1 = (wanted1); % comprises all tt1 subclasses. tt2 = (wanted2); tt3 = (wanted3); TU(thisi(tt1)) = 1; TU(thisi(tt2)) = 2; TU(thisi(tt3)) = 3; TU(thisi(tt11)) = 11; TU(thisi(tt13)) = 13; TU(thisi(tt131)) = 131; TU(thisi(tt132)) = 132; TU(thisi(tt14)) = 14; TU(thisi(tt15)) = 15; TU(thisi(tt16)) = 16; TU(thisi(tt17)) = 17; TU(thisi(tt18)) = 18; TU(thisi(tt19)) = 19; TU(thisi(tt12)) = 12; TU(thisi(tt121)) = 121; TU(thisi(tt122)) = 122; TU(thisi(tt123)) = 123; TU(thisi(tt124)) = 124; end; VU = TU; % group submodes to modes, according to MM5 uid = [2 3 11 14 15 16 17 18 19 121 122 123 124 131 132]; S1 = TU==uid(8); S2 = TU==uid(3) | TU==uid(7) | TU==uid(10) | TU==uid(11) | TU==uid(12) | TU==uid(13); S3 = TU==uid(4) | TU==uid(5) | TU==uid(9) | TU==uid(14) | TU==uid(15); S4 = TU==uid(2); S5 = TU==uid(1) | TU==uid(6); VM = 1*S1 + 2*S2 + 3*S3 + 4*S4 + 5*S5; return; function index = localmaxm(x, w, medge) % INPUTS: x is column vector; w is half-window length % % roger jang 1999 % ak 2004 generalized % [m,n] = size(x); % if ( m < n ) % row vector % assume column vector b1 = zeros(length(x), w); b2 = zeros(length(x), w); % continuity constraints b3 = zeros(length(x), w); b4 = zeros(length(x), w); % >0 condition ensures search on pos values only; % revise for valley search. for i = 1:w b1(1+w:end-w,i) = x(1+w:end-w) > x(1+w-i:end-w-i); b2(1+w:end-w,i) = x(1+w:end-w) > x(1+w+i:end-w+i); b3(1+w:end-w,i) = x(1+w-i:end-w-i) > medge; b4(1+w:end-w,i) = x(1+w+i:end-w+i) > medge; end; b5 = x > medge; index = ones(size(x)); for j = 1:w index = index & b1(:,j) & b2(:,j) & ( b3(:,j) | b4(:,j) ); end; index = index & b5; index = logical(index); return; function index = localminm(x, w, medge) % INPUT: x is column vector; w is half-window length % % roger jang 1999 % ak 2004 generalized % [m,n] = size(x); % if ( m < n ) % row vector % assume column vector b1 = zeros(length(x), w); b2 = zeros(length(x), w); b3 = zeros(length(x), w); b4 = zeros(length(x), w); for i = 1:w b1(1+w:end-w,i) = x(1+w:end-w) < x(1+w-i:end-w-i); b2(1+w:end-w,i) = x(1+w:end-w) < x(1+w+i:end-w+i); b3(1+w:end-w,i) = x(1+w-i:end-w-i) < -medge; b4(1+w:end-w,i) = x(1+w+i:end-w+i) < -medge; end; b5 = x < -medge; index = ones(size(x)); for j = 1:w index = index & b1(:,j) & b2(:,j) & ( b3(:,j) | b4(:,j) ); end; index = index & b5; index = logical(index); return; function [FHS, countskipped] = smoof2(FH, iTH, win, s) % smooth all trajectories sizef = size(FH); sizeit = size(iTH); nit = max(sizeit); FXS = nan * zeros(sizef); FYS = nan * zeros(sizef); FHS = nan * zeros(sizef); FX = cos(FH); FY = sin(FH); delay = floor(win/2); filtx = [-delay:delay]; gfilt = (1/(s.*sqrt(2*pi))) .* exp( -0.5*( ( filtx )./s ).^2 ); gfilt = gfilt./sum(gfilt); countskipped = zeros(500,1); csk = 0; for tjn = 1:nit ids = iTH{tjn}; ids(isnan(FX(ids))|isnan(FY(ids)))=[]; if any(diff(ids)>1) csk = csk+1; countskipped(csk) = tjn; end; if length(ids)>3*(win-1) FXS( ids ) = filtfilt( gfilt , 1 , FX( ids ) ); FYS( ids ) = filtfilt( gfilt , 1 , FY( ids ) ); FHS( ids ) = atan2( FYS( ids ), FXS( ids ) ); end; end; return;