-
Notifications
You must be signed in to change notification settings - Fork 13
/
UpdateMap.m
47 lines (40 loc) · 2.51 KB
/
UpdateMap.m
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
%_________________________________________________________________________%
% Motion-encoded Partical Swarm Optimization (MPSO) source codes demo 1.0%
% %
% Developed in MATLAB 2020a %
% %
% Author and programmer: Manh Duong Phung %
% %
% e-Mail: duongpm@gmail.com %
% duongpm@vnu.edu.vn %
% %
% Homepage: https://uet.vnu.edu.vn/~duongpm/ %
% %
% Main paper: Manh Duong Phung, Quang Phuc Ha %
% "Motion-encoded particle swarm optimization for moving %
% target search using UAVs", %
% Applied soft computing , in press, %
% DOI: https://doi.org/10.1016/j.asoc.2020.106705 %
% %
%_________________________________________________________________________%
% Update the belief map given the current position of UAV and its
% measurement
function [scaleFactor, newMap] = UpdateMap(currentStep,pathLength,totalMoves,dir, location, map)
mapSize = size(map);
mapSizeX = mapSize(1);
mapSizeY = mapSize(2);
% Target move direction (totalMoves in the total path)
move = DirToMove(dir);
if totalMoves ~= 0
moveStep = pathLength/totalMoves;
if mod(currentStep,moveStep) == 0 % Shift the belief map every N steps
tmp_map = noncircshift(map, move);
map = tmp_map ./sum(tmp_map(:)); % Scale it to 1
end
end
pSensorNoDetection = ones(mapSizeX,mapSizeY); % Initialize the probability of no detection with ones
pSensorNoDetection(location.y,location.x) = 0; % For binary sensor model, the probability of no detection at UAV location is zero
newMap = pSensorNoDetection .* map; % Update the belief map
scaleFactor = sum(newMap(:)); % Calculate the scaling factor
newMap = newMap ./ scaleFactor; % Scale the updated belief map
end