**Lost target search explained in code**: 1-step greedy planning for static environments using a range sensor in a few lines of Matlab code.

Snippet extracted from *Lanillos, P. (2013): Minimum time search of moving targets in uncertain environments. Ph.D. Dissertation, Universidad Complutense de Madrid. Link*

% SIMPLE SEARCH (1-step ahead)

% for lost targets 2D in a static world using range sensors

% Author: Pablo Lanillos ICS TUM

% If you use the code please cite

% Lanillos, P. (2013): Minimum time search of moving targets in uncertain environments. Ph.D. Dissertation, Universidad Complutense de Madrid.

% http://therobotdecision.com/papers/PhD_MinimumTimeSearch_2013_FinalElectronic_[PabloLanillos].pdf

%

% June 2016 www.therobotdecision.com

```
```function simple_search()

fprintf('-------------------------------------------------\n');

fprintf('> Simple search 2D start (greedy 1-step ahead)...\n');

% sensor params

Pdmax = 0.9;

dmax = 2;

sigma = 0.7;

% agent params

delta = 0.5; % constant displacement

z = 10; % z is considered constant

s = [5,5,z]'; % initial location (x,y,z)

%% discretized prior map 2D, initial belief b0, gaussian centered in the middle of the map

sz = [20,20];

% prepare grid for improve computation efficiency

I=repmat([1:sz(1)],sz(2),1);I=I(:)';

J = repmat([1:sz(2)],[1,sz(1)]);

COV =[2,0;0,4];

x_mu = [I-ceil(sz(1)/2);J-ceil(sz(2)/2)];

bk = zeros(sz(1)*sz(2),1);

for i = 1 : sz(1)*sz(2)

bk(i) = exp(- 1/2 * x_mu(:,i)'/COV * x_mu(:,i));

end

%% Movement params

nsamples = 9; % number of movements

mat = [-delta,-delta; ...% 1

-delta, 0; ...% 2

-delta, delta; ...% 3

0, -delta; ...% 4

0, 0; ...% 5

0, delta; ...% 6

delta,-delta; ...% 7

delta, 0; ...% 8

delta, delta ];% 9

track = [s]; % visualization purpose only

cont = 0; found = 0;

%% start search

while (~found && cont < 100)

%% compute discrete forward states

dirs = randperm(nsamples); % random selection

fs = repmat([s(1), s(2)],nsamples,1) + mat(dirs,:); % forward states

fs = [fs,ones(nsamples,1)*s(3)]; % heigth is constant

% check bounds

mask = fs(:,1) > 0 & fs(:,2) > 0 & fs(:,1) < sz(1) & fs(:,2) < sz(2);

fs = fs(mask,:);

%% compute cost funtion of the potential forward states

info = zeros(size(fs,1),1);

for i = 1 : size(fs,1)

cs = fs(i,:);

dk = sqrt((I(1,:)-cs(1)).^2 + (J(1,:)-cs(2)).^2); % distance

obs = Pdmax.* exp((-sigma.*(dk/dmax).^2)); % No detection range sensor

info(i) = 1-sum((1-obs').*bk); % information gain

end

[v,id] = max(info); % selection of action with max information gain

%% simulate movement

s = fs(id,:)';

%% update belief

% observation model

dk = sqrt((I(1,:)-s(1)).^2 + (J(1,:)-s(2)).^2); % distance

obs = Pdmax.* exp((-sigma.*(dk/dmax).^2)); % No detection range sensor

bk = (1-obs').*bk; % Bayesian update

bk = bk./sum(bk); % normalization

% No prediction step -> assuming static world

%% visualize

track = [track,s];

figure(11); clf; hold on; grid on;

surf(reshape(bk, sz)');

shading interp; colormap hot;

height = 0.1;

plot3(track(2,:),track(1,:),ones(size(track,2),1)*height, '-g', 'LineWidth',2);

plot3(track(2,end),track(1,end),height, 'xg', 'LineWidth',2);

plot3([track(2,end),track(2,end)],[track(1,end),track(1,end)],[height,0], 'o-y', 'LineWidth',2);

xlabel('x');

ylabel('y');

zlabel('P');

view(-6,62);

pause(0.5);

%%

cont = cont +1;

end

`fprintf('> End search \n');`

fprintf('----------------------------------------------\n');

end