Example code in Matlab

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

matlab file

Lost target search simplified Probabilistic Question

The overall question is the following:
Which actions have to perform the sensing agents to find the lost target?

Without taking into account the time, we can simplify the question to:
Which actions have to perform the sensing agents to maximize the probability of detecting the target?
Or its inverse question:
Which actions have to perform the sensing agents to minimize the probability of non-detecting the target?

From the probabilistic reasoning point of view, this question is formally defined as:

    \[ P(U|\tau,S,Z,\pi) \]

The problem as a joint probability distribution is then stated as:

    \[ P(\tau,S,Z,U|\pi) \]

The question depends on the estimation of the target location P(\tau|Z,U,S), the sensing platform states P(S|U,S=s) and the sensor model P(Z|S,\tau)

1. Assuming no uncertainty within the agents movements that is it exist a deterministic function f_s: S \times U \rightarrow S that makes agent go from state S=s by applying the action U=u to state S=s' unequivocally, we can avoid the using of P(S|U,S=s).

2. Assuming that the sensing platforms while they are searching, the measurements obtained by the sensors are always not detection Z=\overline{D}

3. Assuming that the platforms counts with a prior information of the possible location of the target P(\tau^0|\pi).

4. Assuming that the observations made by the agents are independent (i.e. only depends on the phenomenon and not on the rest of the sensors)

The final question is the following:

    \[ P(\overline{D}|\tau,S,U) = \int_\tau \prod_i P(\overline{D}|\tau,S) P(\tau^0|\pi) \]