Commit e01ab7e6 authored by Fredrik Bagge Carlson's avatar Fredrik Bagge Carlson
Browse files

update PF and add navigation

parent ae8c62a7
function r = g_density(y,xp,xr)
sigma_v = 1;
sigma_v = 20;
N = length(xp);
v = xp - repmat(xr,1,N);
az = atan2d(v(2,:),v(1,:));
......
% Assuming that you already have a 'client' handle, and that the 'binauloc'
% component is running on the turtlebot, load this component.
sendPosition = client.load('sendPosition')
%if you want to move the robot in an absolute position (world frame, check the red arrow on the floor)
absoluteX = 1;
absoluteY = 1;
absoluteOrientation = 90; %Angles are in degrees.
sendPosition.moveAbsolutePosition('map', absoluteX, absoluteY, absoluteOrientation);
%if you want to move the robot in a relative position, from its current position.
moveFront = 0.5;
moveLeft = 0.5;
rotateClockwise = 45;
sendPosition.moveRelativePosition('map', moveFront, moveLeft, rotateClockwise);
%To get the current position:
position = sendPosition.NavigationState().NavigationState.position;
% States
d2r = 180/pi;
th_az = 5; % Degrees
step_distance = 0.1; % meters
while(true)
audio = get_audio();
az = get_azimuth(); % To source
pause(0.01)
audio = get_audio();
set_ref_az(-az); % Relative, otherwise get current az and calc rel. value
az = binauloc.Azimuths()*d2r;
if ~abs(az) < th_az
turn(az); % Relative, otherwise get current az and calc rel. value
continue
end
% The angle to the source is small enough, take a step forward
step_forward(distance);
if ~there()
step_forward(step_distance);
continue
end
......@@ -25,6 +28,7 @@ while(true)
source_label = analyze_source(audio);
display('Source label determined')
disp(source_label)
key = input('Press any key to continue, press c to exit: ','s');
if key == 'c'
break
......
......@@ -43,9 +43,9 @@ par_sub = genParStruct('cc_bBroadband',0,'cc_wSizeSec',winSec,...
mObj = manager(dObj,'localization',par_sub);
%% Particle filter parameters
sigma_w = 1; % State noise std
sigma_v = 1; % Measurement noise std
sigma_w0 = 10;
sigma_w = 0.3; % State noise std
sigma_v = 20; % Measurement noise std
sigma_w0 = 5;
fs = 24414;
chunks = 2048;
dt = chunks/fs;
......
Markdown is supported
0% or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment