update PF and add navigation

g_density.m

 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,:));

navigation.m

+% 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;

state_machine.m

-% 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

track_audio_pf.m

@@ -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;