diff --git a/g_density.m b/g_density.m
index def884ec2c199580a5015840d11760b026620add..92369761ca7fa0a0d481ea860d08271f6bd0739a 100644
--- a/g_density.m
+++ b/g_density.m
@@ -1,5 +1,5 @@
 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,:));
diff --git a/navigation.m b/navigation.m
new file mode 100644
index 0000000000000000000000000000000000000000..fd0a557f0a339c8b49eb3c2cf9db6da007ea319d
--- /dev/null
+++ b/navigation.m
@@ -0,0 +1,18 @@
+% 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;
diff --git a/state_machine.m b/state_machine.m
index 6ce1edaefb943f395ba0c35330e58238494dfbd3..105f33bc1e70bd3447a597c22dab501dd576e2c7 100644
--- a/state_machine.m
+++ b/state_machine.m
@@ -1,22 +1,25 @@
-% 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
diff --git a/track_audio_pf.m b/track_audio_pf.m
index 3d116150e8c014e04508f6ec619a2fd9bf8367a5..30454c5611ba929ddde4bc88f1f55aeb32768371 100644
--- a/track_audio_pf.m
+++ b/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;