2013/uns13/projects: flatmap2.m

File flatmap2.m, 19.5 KB (added by timmer, 5 years ago)

MATLAB code for the SLAM/mapping project. - will also need MakeQTMovie

Line 
1%
2% flatmap2 - adding in fake place cells, action cells, "associagram"
3% flatmap1 - branched from 'openspace6', grid/place cell simulation
4%                removed.   Arena for mapping experiments created.
5%
6%  This 'openspace' code simulates obstacle avoidance.
7% to see a neuromorphic VLSI implementation variant, see:
8% T. Horiuchi, "A Spike-Latency Model for Sonar-Based Navigation in
9% Obstacle Fields", IEEE Transactions in Circuits and Systems I, 56(11),
10% pp.2393- 2401 , 2009
11
12%  Code originated by:  Timmer Horiuchi
13%  University of Maryland, 2013
14%
15%  Mappers:   PLACE = place cells, ActWts are the eligibility for the 32
16%  possible directions of travel.  Associagram is the binary neighborhood
17%  matrix.  placex_log(k), placey_log(k), placei_log(k)  - is the location of the
18%  place cells in space with k indicating the order in which they were
19%  assigned.    1 <= k <= numPLACE. 
20
21close all;
22
23moviep = 0;  % 0 means that we aren't creating a movie of figure (1) - requires MakeQTMovie.m
24goalp = 0;   % 0 means that we aren't implementing a moving target object
25                  % This part of the code is not relevant for the mapping problem
26
27% initialize the bat's position
28batpos(1,1) = 50;  % previous location x
29batpos(1,2) = 20;  % previous location y
30batpos(2,1) = 50;  % new location x
31batpos(2,2) = 20;  % new location y
32
33% odox and odoy simulate 
34odox(1) = batpos (1,1);
35odoy(1) = batpos (1,2);
36odox(2) = batpos (2,1);
37odoy(2) = batpos (2,2);
38
39% all angles are in radians with 0 pointed off to the right and +pi/2
40% straight up.
41headingchange = 0;   % bat's initial prior direction - given in radians
42brad = 1.9;   % bat's initial orientation
43hrad = 1.9;
44bvel = 2.0;       % bat's speed in spatial units/sec.  The bat flies with constant speed.
45
46% simulation global parameters
47dt = 0.3;   % fixed simulation time step (sec)
48asym = 0.1;   % asymmetry used in assigning the 'prior' gaussian
49
50%% parameters for place / hd / cells
51Nhd = 32;
52Nplace = 200;
53psig = 40;        % place field Gaussian sigma
54
55HD = zeros (Nhd, 1);        % head direction cell activations
56PLACE = zeros (Nplace, 1);   % place cell activations
57PBflag = zeros (Nplace, 1);     % Place/Border type flag (the "border" type is initiated by the presence of objects
58oldPLACE = zeros(Nplace, 1);   
59
60% keeps track of the place cells in the order they are allocated.
61placex_log = zeros(Nplace);     % list of place cell x
62placey_log = zeros(Nplace);     % list of place cell y
63placei_log = zeros(Nplace);     % list of place cell index...   out of a possible Nplace
64PlaceLoc = zeros (Nplace, 2) - 50 + rand (Nplace, 2);  % this is the real location in x and y  (index 1=x and 2=y) - redundant with placex_log, etc.  (can eliminate)
65PlaceRecency = zeros(Nplace,1);   % should be positive.
66associagram = zeros(Nplace, Nplace);  % co-activation matrix
67ActWts = zeros (Nplace, Nhd);  % "action" neurons that indicate what actions (i.e., directions of travel) are available at a given place cell.
68
69%% initialize variables for mapping
70numPLACE = 0;           % how many place cells have been allocated.
71
72%%
73
74% legacy code... not needed for mapping.
75% initialize the goal's position
76if (goalp == 1)     
77    goalx = 50;
78    goaly = 50;
79    gvel = 0.3;         % goal is moving too!
80    grad = -0.1;
81    gcurve = 0.005;     % change in goal direction / timestep
82end
83
84% initialize the obstacles
85obstnum = 300;      % we don't actually use all 300.
86killzone = 1;     % how close the bat needs to get to the goal
87
88% Handle graphics setup
89Hf_1 = figure('Position',[300 300 600 800]);
90set (Hf_1,'Color','white');
91set (Hf_1,'Name','OpenSpace Bat Flight');
92a = axes ('Position',[0.11 0.38 0.8 0.55]);
93xlabel ('Space: X');
94ylabel ('Space: Y');
95b = axes ('Position',[0.05 0.05 0.4 0.26]);
96c = axes ('Position',[0.55 0.1 0.35 0.2]);
97% the other graphs (b and c) get redrawn fully each time so we need to relabel those
98% each time.
99figure (2);
100if (moviep == 1)
101    pause ();
102end
103% rand('state',0);
104% space graph : draw obstacles
105axes(a);   
106hold on;
107obsti = 0;
108for i = 1 : 35
109    obsti = obsti + 1;
110    tree (obsti, 1) = - 70 + (4 * i);
111    tree (obsti, 2) = 0;
112    plot (tree(obsti,1),tree(obsti,2),'k.');
113    obsti = obsti + 1;
114    tree (obsti, 1) = - 70 + (4 * i);
115    tree (obsti, 2) = 120;
116    plot (tree(obsti,1),tree(obsti,2),'k.');
117end
118for i = 1 : 30
119    obsti = obsti + 1;
120    tree (obsti, 1) = -70;
121    tree (obsti, 2) = (4 * i);
122    plot (tree(obsti,1),tree(obsti,2),'k.');
123    obsti = obsti + 1;
124    tree (obsti, 1) = 70;
125    tree (obsti, 2) = 4 * i;
126    plot (tree(obsti,1),tree(obsti,2),'k.');
127end
128for i = 1 : 30
129    obsti = obsti + 1;
130    tree (obsti, 1) = -70;
131    tree (obsti, 2) = (4 * i);
132    plot (tree(obsti,1),tree(obsti,2),'k.');
133    obsti = obsti + 1;
134    tree (obsti, 1) = 70;
135    tree (obsti, 2) = 4 * i;
136    plot (tree(obsti,1),tree(obsti,2),'k.');
137end
138for i = 1 : 10
139    obsti = obsti + 1;
140    tree (obsti, 1) = -50 + 4 * i;
141    tree (obsti, 2) = 80;
142    plot (tree(obsti,1),tree(obsti,2),'k.');
143    obsti = obsti + 1;
144    tree (obsti, 1) = -50 + 4 * i;
145    tree (obsti, 2) = 84;
146    plot (tree(obsti,1),tree(obsti,2),'k.');
147end
148for i = 1 : 22
149    obsti = obsti + 1;
150    tree (obsti, 1) = -20 + 4 * i;
151    tree (obsti, 2) = 40;
152    plot (tree(obsti,1),tree(obsti,2),'k.');
153    obsti = obsti + 1;
154    tree (obsti, 1) = -20 + 4 * i;
155    tree (obsti, 2) = 44;
156    plot (tree(obsti,1),tree(obsti,2),'k.');
157end
158for i = 1 : 5
159    obsti = obsti + 1;
160    tree (obsti, 1) = -50;
161    tree (obsti, 2) = 20 + (4 * i);
162    plot (tree(obsti,1),tree(obsti,2),'k.');
163    obsti = obsti + 1;
164    tree (obsti, 1) = -54;
165    tree (obsti, 2) = 20 + (4 * i);
166    plot (tree(obsti,1),tree(obsti,2),'k.');
167end
168obsti = obsti + 1;
169tree (obsti, 1) = 20;  tree (obsti, 2) = 70;
170plot (tree(obsti,1),tree(obsti,2),'k.');
171obsti = obsti + 1;
172tree (obsti, 1) = 0;  tree (obsti, 2) = 20;
173plot (tree(obsti,1),tree(obsti,2),'k.');
174obsti = obsti + 1;
175tree (obsti, 1) = -60;  tree (obsti, 2) = 100;
176plot (tree(obsti,1),tree(obsti,2),'k.');
177obsti = obsti + 1;
178tree (obsti, 1) = 30;  tree (obsti, 2) = 90;
179plot (tree(obsti,1),tree(obsti,2),'k.');
180obsti = obsti + 1;
181tree (obsti, 1) = -65;  tree (obsti, 2) = 60;
182plot (tree(obsti,1),tree(obsti,2),'k.');
183
184if (goalp == 1)
185    plot (goalx, goaly, 'r.');
186end
187axis ([-80 80 -10 130]);
188
189% start the quicktime movie
190if (moviep == 1)
191    MakeQTMovie start temp.qt;
192end
193 
194% ---------------------------- BEGIN THE MAIN LOOP!! ---------------------------------
195iter = 1;
196
197% place cell recency signal decay
198% in theory, this allows us to reuse infrequently used place cells once we
199% have assigned all of the unused place cells.
200PlaceRecency = PlaceRecency * 0.9999;   
201
202% use these even if goalp = 0.  Set to be out of the arena coordinates.
203goalx = 10000;
204goaly = 10000;
205
206% loop until timeout or goal has been achieved
207% (ignore killzone for mapping project)
208while (((abs(batpos(2,1)-goalx) > killzone) || (abs(batpos(2,2)-goaly) > killzone)) && (iter < 1000))
209   
210    brad = brad + (rand() - 0.5)*0.2;  % add a little noise to the actual direction of travel
211   
212    batpos(1,1) = batpos(2,1);   % old x position
213    batpos(1,2) = batpos(2,2);   % old y position
214    odox(1) = odox(2);      % old odometry estimate of x
215    odoy(1) = odoy(2);      % old odometry estimate of y
216    batpos (2,1) = batpos(1,1) + bvel * dt * cos(brad);     % new x position
217    batpos (2,2) = batpos(1,2) + bvel * dt * sin(brad);     % new y position
218   
219    % simulating a noisy odometry system   (I believe there is a bug in
220    % this piece of code... the odometry system doesn't seem to be drifting
221    % as much as it should.)
222    rad_noise = (rand () - 0.5) * 0.3;
223    vel_noise = (1 + (rand() - 0.5)*0.3);
224    odox(2) = odox(1) + vel_noise*bvel  * dt * cos(rad_noise+brad);
225    odoy(2) = odoy(1) + vel_noise*bvel * dt * sin(rad_noise+brad);
226   
227    if (goalp == 1)
228        ogx = goalx;    % old goal x
229        ogy = goaly;    % old goal y
230        goalx = goalx + dt * gvel * cos(grad);  % new goal x position
231        goaly = goaly + dt * gvel * sin(grad);  % new goal y position
232        goal = [ogx ogy; goalx goaly];
233           
234        separation = sqrt(power(goaly-batpos(2,2),2)+power(goalx-batpos(2,1),2));  % this is used for the random goal movement
235        if (separation < 20)
236            grad = grad + dt*(rand - 0.5);
237        else
238            grad = grad - dt*gcurve;
239        end
240    end
241   
242    % update the bat and goal position on graph
243    axes(a);
244    hold on;
245    plot (batpos(:,1),batpos(:,2),'k-');  % these are lines from previous to new position
246    plot (odox(:),odoy(:), 'b-');  % draw the noisy odometry track
247    title ('Black = True Location; Blue = Estimated Location');
248    if (goalp == 1)
249        plot (goal(:,1), goal(:,2),'r-');  % these are lines from previous to new position
250    end
251    hold off;
252   
253    % generate the openspace (obstacle avoidance) evaluation
254
255    % update the bat's direction using the heading change
256    % we will eventually combine information about the goal and from the obstacle
257    % avoidance system to get the new heading.
258   
259    % there two forms in which relative directions are kept: a discrete
260    % form related to the bin number and in radians.  they are related by a
261    % factor of discrete = (rad * 10) + 17
262    % heading starts off as the desired heading (i.e. toward goal) and ends
263    % up with what the collision avoidance tells us.
264   
265    % update the bat's direction given last recommendation
266    % this only implements part of the requested heading change.
267    % The head (hrad) follows the desired direction of travel
268    % and the body (brad) follows the head.
269   
270    hrad = hrad - headingchange;
271   
272    if (hrad > pi)                  % keep hrad in limits
273        hrad = hrad - 2 * pi;
274    end
275    if (hrad < -pi)
276        hrad = hrad + 2 * pi;
277    end
278
279    deltarad = hrad - brad;
280    if (deltarad > pi)
281        deltarad = deltarad - 2 * pi;
282    end
283    if (deltarad < -pi)
284        deltarad = deltarad + 2 * pi;
285    end
286    brad = brad + dt*0.50*(deltarad);  % move the body to match the head.
287    if (brad > pi)                                % keep brad within limits
288        brad = brad - 2 * pi;
289    end
290    if (brad < -pi)
291        brad = brad + 2 * pi;
292    end
293   
294    % figure out direction to goal
295    if (goalp == 1)
296        gheading = atan2(goaly-batpos(2,2),goalx-batpos(2,1));
297        % fix the radians to a limited range
298        relgheading = mod(gheading - hrad + pi, 2*pi) - pi;   % this puts relgheading between -pi and pi
299    else
300        relgheading = 0;  % this is the default for the mapping project.
301    end
302   
303    % bias is where you will bias your looking next.  [for mapping, we look
304    % forwards.
305    % bias = round((relgheading*(1-dt)*0.3 + (1-dt)*headingchange) * 10) % only bias to 1/5 at goal - relghead is in discrete units
306    bias = round(relgheading * 10 * 0.3);
307   
308    if (bias > 15) bias = 15; end
309    if (bias < -15) bias = -15; end   % limit the bias to +/- 8.
310 
311    for k = 2 : 33
312        ospace(k) = 5 + 2*exp (-power((k-17)+ bias + asym, 2) / 10 / 10);  % use relghead as the prior
313    end
314    ospace(1) = 4.9;
315   
316    axes(b); 
317    polar (0,10);
318    axis ([-0.1 10 -10 10]);
319    grid off;
320    hold on;
321   
322    % target?
323    if (goalp == 1)
324        if ((separation < 10) && (relgheading > -pi/2) && (relgheading < pi/2))
325            z = polar (relgheading, separation, 'ro');
326            set (z, 'LineWidth', 2);
327        end;
328    end
329   
330    %% Incorporate Obstacles into "openspace"
331    numinprox = 0;
332    for i = 1 : obsti         % for each obstacle...
333        relvec = [tree(i,1)-batpos(2,1) tree(i,2)-batpos(2,2)];
334        relrad = atan2(relvec(2), relvec(1));
335        relmag = sqrt(relvec * relvec');
336        dir = [cos(hrad) sin(hrad)];
337       
338        dotp = dir*relvec';   % the dot product > 0 means it's in front of the bat
339
340        if (relmag < 10)
341            numinprox = numinprox + 1;
342        end
343       
344        if ((dotp > 0) && (relmag < 10))
345            diffrad = mod(hrad - relrad + pi/2, pi) - pi/2;
346            z = polar (-diffrad, relmag,'ko');  % <---- this is the old type of azra plot.
347            set (z,'LineWidth',2);
348         
349            whichdir = round(diffrad*10)+17;
350            for i = -9 : 9
351                if ((whichdir + i > 1) && (whichdir + i < 34))
352                    invdist = 10 - relmag;
353                    ospace (whichdir+i) = ospace (whichdir+i) - exp(-i*i/power(2.5+invdist/2,2)) * invdist;
354                end
355            end
356        end
357    end
358   
359    hold off;
360   
361    axes(c);
362    bar (ospace);
363    xlabel ('Azimuth Unit (17=center)');
364    ylabel ('Evaluation');
365    hold on;
366    maxospace = find(ospace == max (ospace));
367    if (length(maxospace) > 1) maxospace = max(maxospace); end   % if more than one max is picked, take only one.
368 
369    % mostly towards last heading, partly towards new direction
370    % because the heaading change doesn't change instantaneously, we
371    % implement only part of the requested change in heading by dt.
372    %    headingchange = headingchange - dt*0.9*headingchange + dt*0.9*(maxospace-17)/10;
373    headingchange = 0.9 *(maxospace-17)/10;
374   
375    z = bar (maxospace, ospace(maxospace),'r');
376
377    axis ([1 33 0 22]);
378    if (goalp == 1)
379        plot (17-round(relgheading*10),15,'r.');  % draw the dot where the target is
380    end
381    hold off;
382   
383    %% - HEAD DIRECTION CELLS
384    % compute the head-direction cell array from brad
385    hdsig = 0.8;
386    for hdi = 1 : Nhd
387        hdr (hdi) = -pi + (hdi-1)*2*pi/Nhd;
388        diffrad = min([abs(hdr(hdi) - brad), abs(hdr(hdi) - (brad + 2*pi)), abs(hdr(hdi) - (brad - 2*pi))]);
389        HD (hdi) = exp(-power(diffrad,2)/hdsig/hdsig);
390    end
391    figure (2);
392    subplot(2,2,2);
393    plot (hdr, HD, 'ko-');
394    axis ([-4 4 -0.1 1.1]);
395    title ('Head Direction Cells'); xlabel('Direction (radians)');
396   
397    %% - PLACE & BORDER CELL ACTIVATION CALCULATIONS
398    % Gaussian implementation
399    for z = 1 : Nplace
400        dx = abs(batpos(2,1) - PlaceLoc (z, 1));
401        dy = abs(batpos(2,2) - PlaceLoc (z, 2));
402        tmp = sqrt(dx*dx+dy*dy);
403        Pinput = exp(-tmp*tmp/psig/psig);       % Gaussian place field
404        PLACE (z) = (tanh(10*Pinput - 8) + 1)/2;   % sigmoid
405    end
406    figure (2);
407    subplot (2,2,1);
408    bar (PLACE);  title ('Place Cells Activation');
409    axis ([-1 Nplace+1 -0.1 1.1]);
410    colormap (gray);
411   
412   
413    %% DO WE NEED to instantiate a NEW place cell ??
414    placemax = max(PLACE(:));
415    if ((placemax < 0.5) | ((numinprox > 2) & (placemax < 0.8)))
416        tmpPLACE = PLACE(:) - PlaceRecency;
417        placemax = max(tmpPLACE);
418        numPLACE = numPLACE + 1;
419        figure (1);
420        axes (a);
421        hold on;
422        if (numinprox > 2)  % numinprox comes from the openspace algorithm
423            PBflag (numPLACE) = 2;       % it's a border cell
424            r = plot (batpos(2,1), batpos(2,2), 'bo');
425        else
426            PBflag (numPLACE) = 1;  % it's a place cell
427            r = plot (batpos(2,1), batpos(2,2), 'ro');
428        end
429        set (r, 'LineWidth', 2);
430        zmax = find (tmpPLACE == placemax);
431        placex_log(numPLACE) = batpos(2,1);
432        placey_log(numPLACE) = batpos(2,2);
433        placei_log(numPLACE) = zmax(1);
434        PlaceLoc(zmax(1), 1) = batpos(2,1);
435        PlaceLoc(zmax(1), 2) = batpos(2,2);
436        PlaceRecency(zmax(1)) = 2;     
437        s = sprintf('Created Place Cell # %d (%d)',numPLACE,zmax(1));
438        disp(s);
439    end
440   
441    %% Associagram
442    ASSOC = (1 + sign (PLACE - 0.3))/2;     % create a binarized coactivation vector.
443    % ASSOC = PLACE;
444    associagram = max(associagram, ASSOC*ASSOC');   % add up the cross correlation matrix
445    figure (5);
446    % figure
447%  subplot (2,2,3);
448    imagesc (associagram);
449    axis xy;
450    colormap (gray);
451   
452    %% 
453    %% Compute the Action Cell activations
454    % training the most strongly activated place cell on possible actions
455   
456    act_gamma = 1.0;
457    dPLACE = PLACE - oldPLACE;   % derivatives of the PLACE cells.
458    iPeak = find (dPLACE == max(dPLACE));  % the place cell with the most positive derivative represents the place we are heading towards
459    iTrough = find (dPLACE == min(dPLACE)); % the place cell with the most negative derivative represents the place we are leaving
460    PeakPlace = zeros(Nplace,1);  % an array of zeros and one 1.
461    PeakPlace(iPeak(1)) = 1;
462    TroughPlace = zeros(Nplace,1);  % an array of zeros and one 1.
463    TroughPlace(iTrough(1)) = 1;
464    actsig = 0.2; % the possible actions bump is narrow. (should be up at the top of the code outside the loop)
465    PossibleFwdAct = zeros(Nhd,1);
466    for acti = 1 : Nhd  % make the possible actions ring/bump for the
467        actr(acti) = -pi + (acti-1)*2*pi/Nhd;
468        diffrad = min([abs(actr(acti) - brad), abs(actr(acti) - (brad + 2*pi)), abs(actr(acti) - (brad - 2*pi))]);
469        PossibleFwdAct (acti) = exp(-power(diffrad,2)/actsig/actsig);
470    end
471    actbrad = brad + pi;   % point the possible actions backwards to where we came from
472    if (actbrad > pi)
473        actbrad = actbrad - 2*pi;
474    end
475    PossibleBkwdAct = zeros(Nhd,1);
476   
477    for acti = 1 : Nhd  % make the possible actions ring/bump
478        actr(acti) = -pi + (acti-1)*2*pi/Nhd;
479        diffrad = min([abs(actr(acti) - actbrad), abs(actr(acti) - (actbrad + 2*pi)), abs(actr(acti) - (actbrad - 2*pi))]);
480        PossibleBkwdAct (acti) = exp(-power(diffrad,2)/actsig/actsig);
481    end
482    figure (2);
483    subplot (2,2,4);
484    plot (actr, PossibleBkwdAct,'bo-'); hold on;
485    plot (actr, PossibleFwdAct,'ro-'); hold off;
486    title ('Possible Fwd and Bkwd Actions from here');
487   
488    if (abs(min(dPLACE)) > 0.05)   % arbitrary threshold
489        ActWts = ActWts + act_gamma * (ones(Nplace,Nhd) - ActWts) .* (TroughPlace * PossibleFwdAct');
490    end
491%         s = sprintf ('Leaving place# %d and approaching place# %d',iPeak(1),iTrough(1));
492%         disp(s);
493    if (max(dPLACE) > 0.05)  % arbitrary threshold.
494        ActWts = ActWts + act_gamma * (ones(Nplace,Nhd) - ActWts) .* (PeakPlace * PossibleBkwdAct');
495    end
496 
497    oldPLACE = PLACE;
498   
499
500    %% MakeQT Movie
501    if (moviep == 1)
502        figure (1);
503        MakeQTMovie addfigure;
504    end
505   
506    drawnow;
507    iter = iter + 1;
508   
509end
510%% Neighborhood Lines Plot
511figure (3);  hold on;
512 for i = 1 : numPLACE
513      plot (placex_log(i), placey_log(i),'ko');
514 end
515 axis ([-80 80 -10 130]);
516for i = 1 : Nplace
517    for k = 1 : Nplace
518        if (associagram (i, k) == 1)
519            indexi = find(placei_log == i);
520            indexk = find(placei_log == k);
521            xvec = [placex_log(indexi) placex_log(indexk)];
522            yvec = [placey_log(indexi) placey_log(indexk)];
523            s = plot (xvec, yvec,'b-');
524        end
525    end
526end
527%% Whisker Plot of Arrival Directions
528figure (6); hold on;
529for k = 1 : numPLACE
530    s = plot (placex_log(k),placey_log(k),'ro-');
531    set (s,'LineWidth',2);
532end
533for k = 1 : numPLACE
534    ActWtsScale = max(ActWts(placei_log(k),:));
535    for j = 1 : Nhd
536        ActMag = ActWts(placei_log(k), j);
537        rx = 10 * ActMag * cos(-pi + (j * 2 * pi / Nhd)) /ActWtsScale;
538        ry = 10 * ActMag * sin(-pi + (j * 2 * pi / Nhd)) /ActWtsScale;
539        plot ([placex_log(k) placex_log(k)+rx], [placey_log(k) placey_log(k)+ry],'k-');
540    end
541end
542axis ([-80 80 -10 130]);
543if (moviep == 1)
544    MakeQTMovie finish;
545end