motion

 En dynamisk modell som bygger på att det troligaste är att objektet rör sig 
 rakt fram med konstant hastighet. Här kan man istället få trolig fart och 
 riktning från en terrängmodell. Tex, en partikel på väg åker längre sträcka 
 än en partikel i skogen, och det är troligare att partikeln följer vägen än 
 att den åker ut i skog.

 Hedvig Sidenbladh
 Januari 2004

0001 function priorpartiklar = motion(target, tidssteg);
0002 
0003 % En dynamisk modell som bygger på att det troligaste är att objektet rör sig
0004 % rakt fram med konstant hastighet. Här kan man istället få trolig fart och
0005 % riktning från en terrängmodell. Tex, en partikel på väg åker längre sträcka
0006 % än en partikel i skogen, och det är troligare att partikeln följer vägen än
0007 % att den åker ut i skog.
0008 %
0009 % Hedvig Sidenbladh
0010 % Januari 2004
0011 
0012 % Konstanter
0013 global N;
0014 global R_BRUS;
0015 global localAreaofInterest;
0016 global roads_struct;
0017 %global struct_own_forces;
0018 
0019 partiklar = target.particles; 
0020 
0021 
0022 max_speed = target.max_speed;
0023 localAreaofInterest = 3;
0024 R_BRUS = [0.5 0.5 0.1 pi/20];
0025 N = length(partiklar(:,1)); 
0026 priorpartiklar = [];
0027 alive_priorpartiklar = [];
0028 
0029 
0030 %---------------slask kod---------------------------------
0031 %own_force = 1; currEnemy = 1
0032 %[struct_currentData] = getCurrentEsimatedData(own_force, currEnemy);
0033 
0034 %[struct_own_forces,noOfOwn_forces] = initializeOwn_forces;
0035 noOfDeadParticles = 0;
0036 for t = 1:tidssteg
0037     for p = 1:N
0038         [new_particle] = propagateParticle(round(partiklar(p,:)));
0039         if ~isempty(new_particle)
0040            priorpartiklar = [priorpartiklar; new_particle];
0041         else
0042             noOfDeadParticles = noOfDeadParticles + 1;
0043         end;
0044     end; 
0045 %    priorpartiklar
0046 %    noOfDeadParticles
0047     priorpartiklar = [priorpartiklar; reIncarnation(noOfDeadParticles,priorpartiklar)];
0048     priorpartiklar = priorpartiklar + randn(N, 4).*(ones(N, 1)*R_BRUS);
0049     % max speed of targets used in white noise step
0050     priorpartiklar(:,3) = max_speed.*rand(N, 1);
0051     noOfDeadParticles = 0;
0052     for p = 1:N 
0053       if inMatrix(roads_struct.costMap, round(priorpartiklar(p,1)),round(priorpartiklar(p,2)))
0054         alive_priorpartiklar = [alive_priorpartiklar; priorpartiklar(p,:)];
0055       else
0056         noOfDeadParticles = noOfDeadParticles + 1;
0057     end;
0058   end;
0059   alive_priorpartiklar = [alive_priorpartiklar; reIncarnation(noOfDeadParticles, alive_priorpartiklar)]; 
0060   partiklar = alive_priorpartiklar; 
0061   noOfDeadParticles = 0;
0062   priorpartiklar = [];
0063   alive_priorpartiklar = [];
0064 end;% end for each time step
0065 priorpartiklar = partiklar;