From e01ab7e62d9c28d68d846ef39a930e1e19a59213 Mon Sep 17 00:00:00 2001
From: Fredrik Bagge Carlson <fredrikb@control.lth.se>
Date: Fri, 25 Sep 2015 11:18:49 +0200
Subject: [PATCH] update PF and add navigation
---
g_density.m | 2 +-
navigation.m | 18 ++++++++++++++++++
state_machine.m | 16 ++++++++++------
track_audio_pf.m | 6 +++---
4 files changed, 32 insertions(+), 10 deletions(-)
create mode 100644 navigation.m
diff --git a/g_density.m b/g_density.m
index def884e..9236976 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 0000000..fd0a557
--- /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 6ce1eda..105f33b 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 3d11615..30454c5 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;
--
GitLab