image thumbnail

Neural Network (MLP) Robot Localization

by

 

Simulation of neural robot localization using a Multi-Layer Perceptron Network.

localization_gui(varargin)
function varargout = localization_gui(varargin)
% LOCALIZATION_GUI M-file for localization_gui.fig
%      LOCALIZATION_GUI, by itself, creates a new LOCALIZATION_GUI or raises the existing
%      singleton*.
%
%      H = LOCALIZATION_GUI returns the handle to a new LOCALIZATION_GUI or the handle to
%      the existing singleton*.
%
%      LOCALIZATION_GUI('CALLBACK',hObject,eventData,handles,...) calls the local
%      function named CALLBACK in LOCALIZATION_GUI.M with the given input arguments.
%
%      LOCALIZATION_GUI('Property','Value',...) creates a new LOCALIZATION_GUI or raises the
%      existing singleton*.  Starting from the left, property value pairs are
%      applied to the GUI before localization_gui_OpeningFcn gets called.  An
%      unrecognized property name or invalid value makes property application
%      stop.  All inputs are passed to localization_gui_OpeningFcn via varargin.
%
%      *See GUI Options on GUIDE's Tools menu.  Choose "GUI allows only one
%      instance to run (singleton)".
%
% See also: GUIDE, GUIDATA, GUIHANDLES

% Edit the above text to modify the response to help localization_gui

% Last Modified by GUIDE v2.5 30-Jan-2011 19:08:49

% Begin initialization code - DO NOT EDIT
gui_Singleton = 1;
gui_State = struct('gui_Name',       mfilename, ...
                   'gui_Singleton',  gui_Singleton, ...
                   'gui_OpeningFcn', @localization_gui_OpeningFcn, ...
                   'gui_OutputFcn',  @localization_gui_OutputFcn, ...
                   'gui_LayoutFcn',  [] , ...
                   'gui_Callback',   []);
if nargin && ischar(varargin{1})
    gui_State.gui_Callback = str2func(varargin{1});
end

if nargout
    [varargout{1:nargout}] = gui_mainfcn(gui_State, varargin{:});
else
    gui_mainfcn(gui_State, varargin{:});
end
% End initialization code - DO NOT EDIT

% --- Executes just before localization_gui is made visible.
function localization_gui_OpeningFcn(hObject, ~, handles, varargin)
% This function has no output args, see OutputFcn.
% hObject    handle to figure
% eventdata  reserved - to be defined in a future version of MATLAB
% handles    structure with handles and user data (see GUIDATA)
% varargin   command line arguments to localization_gui (see VARARGIN)

% Choose default command line output for localization_gui
handles.output = hObject;

% Update handles structure
guidata(hObject, handles);

% GUI MODIFICATIONS
%play = importdata('play.png');
%set(handles.pushbutton9,'CDATA',play.cdata);
zoom on
zoom(2)
zoom off

%INITIALIZING ENVIRONMENT
global Landmarks Color Active;
global showRanges showTrajectories showNeural showTri;

Landmarks = [-18 -13; -6 -13; 6 -13; 18 -13; -18 0; 18 0; -18 13; -6 13; 6 13; 18 13];
Color = [1 0 0; 0 1 0; 0 0 1; 0.5 0.5 0; 0 0.5 0.5; 0.5 0 0.5; 0.5 0.25 0.25; 0.25 0.5 0.25; 0.25 0.25 0.5; 0.33 0.33 0.33];
Active = ones(1,length(Landmarks));

showRanges = 0;
showTrajectories = 1;
showNeural = 1;
showTri = 1;

%INITIALIZING NEURAL NETWORK
global scale pattern target;
global net neurons;

scale = str2double(get(handles.scaleValueText,'String'));
pattern = zeros((1+floor(30/scale))*(1+floor(40/scale)), 10);
target = zeros((1+floor(30/scale))*(1+floor(40/scale)), 2);

P = pattern' + randn(size(pattern'));
T = target';
neurons = str2double(get(handles.neuronValueText,'String'));
S = [neurons];

net = newff(P,T,S);

%INITIALIZING ROBOT STATE VARIABLES

%CONTROL LAW

% v = kP*p;
% w = kA*a + kB*b;

%RESULTING EQUATIONS

% p' = -kP*p*cos(a);
% a' = kP*sin(a) - kA*a - kB*b;
% b' = -kP*sin(a);

global k dt run;
global goal activeGoal g;
global pose nnpose tripose v w;
global kP kA kB;

k = 1;
dt = 0.1;

run = 0;

goal = zeros(5,3);
activeGoal = zeros(1,size(goal,1));
g = 0;

pose = [0 0 0];
nnpose = [0 0];
tripose = [0 0];

v = 0;
w = 0;

kP = 1;
kA = 5;
kB = -3;

cla;

plotGoal(handles.axes, goal, activeGoal);
plotRobot(handles.axes, pose(1,1), pose(1,2), pose(1,3));
plotLandmarks(handles.axes, Landmarks, Color);

if(showRanges)
    plotLines(handles.axes, pose(1,1), pose(1,2), Landmarks, Color, Active);
end

if(showTrajectories)
    plotTrajectory(handles.axes, pose, nnpose, tripose);
end

% --- Outputs from this function are returned to the command line.
function varargout = localization_gui_OutputFcn(~, ~, handles) 
% varargout  cell array for returning output args (see VARARGOUT);
% hObject    handle to figure
% eventdata  reserved - to be defined in a future version of MATLAB
% handles    structure with handles and user data (see GUIDATA)

% Get default command line output from handles structure
varargout{1} = handles.output;

% --- Executes on mouse press over axes background.
function axes_ButtonDownFcn(~, ~, handles)
% hObject    handle to axes (see GCBO)
% eventdata  reserved - to be defined in a future version of MATLAB
% handles    structure with handles and user data (see GUIDATA)

global Landmarks Color Active;
global showRanges showTrajectories;
global k;
global pose nnpose tripose;
global goal activeGoal g;

g = g + 1;

if(g > 5)
    g = mod(g,5);
end

activeGoal(1,g) = 1;

goalnew = get(handles.axes,'CurrentPoint');

goal(g,1) = goalnew(1,1);
goal(g,2) = goalnew(1,2);

cla;

plotGoal(handles.axes, goal, activeGoal);
plotRobot(handles.axes, pose(k,1), pose(k,2), pose(k,3));
plotLandmarks(handles.axes, Landmarks, Color);

if(showRanges)
    plotLines(handles.axes, pose(k,1), pose(k,2), Landmarks, Color, Active);
end

if(showTrajectories)
    plotTrajectory(handles.axes, pose, nnpose, tripose);
end

% --- Executes on button press in dataButton.
function dataButton_Callback(~, ~, handles)
% hObject    handle to dataButton (see GCBO)
% eventdata  reserved - to be defined in a future version of MATLAB
% handles    structure with handles and user data (see GUIDATA)

global Landmarks Color;
global scale pattern target;

scale = str2double(get(handles.scaleValueText,'String'));
pattern = zeros((1+floor(30/scale))*(1+floor(40/scale)), 10);
target = zeros((1+floor(30/scale))*(1+floor(40/scale)), 2);

cla;

plotLandmarks(handles.axes, Landmarks, Color);

for i=-15:scale:15
    for j=-20:scale:20
        
        plot(handles.axes, j, i, 'LineStyle','.','MarkerSize',10,'Color',[(i+15)/30 (j+20)/40 (i*j+300)/600]);
        
        target(uint32(((i+15)*(1+floor(40/scale)) + (j+20))/scale + 1), 1) = j;
        target(uint32(((i+15)*(1+floor(40/scale)) + (j+20))/scale + 1), 2) = i;
        
        for m=1:10
            
            dx = abs(Landmarks(m,1) - j);
            dy = abs(Landmarks(m,2) - i);
            
            pattern(uint32(((i+15)*(1+floor(40/scale)) + (j+20))/scale + 1), m) = sqrt(dx^2+dy^2);
            
        end
    end
end

% --- Executes on button press in trainButton.
function trainButton_Callback(~, ~, handles)
% hObject    handle to trainButton (see GCBO)
% eventdata  reserved - to be defined in a future version of MATLAB
% handles    structure with handles and user data (see GUIDATA)

global net;
global pattern target neurons;
global trainNoise;

trainNoise = str2double(get(handles.trainNoiseValueText,'String'));

P = pattern' + randn(size(pattern'));
T = target';
neurons = str2double(get(handles.neuronValueText,'String'));
S = [neurons];

net = newff(P,T,S);
net = train(net,P,T);

% --- Executes on button press in testButton.
function testButton_Callback(hObject, ~, handles)
% hObject    handle to pushbutton9 (see GCBO)
% eventdata  reserved - to be defined in a future version of MATLAB
% handles    structure with handles and user data (see GUIDATA)

global Landmarks Color Active;
global showRanges showTrajectories;

global k dt run;
global goal activeGoal;
global pose nnpose tripose v w;
global kP kA kB;

global testNoise;

global rho alpha beta;

global net;

global nnerror trierror;

run = 1;
nnerror = [0 0 0 0 0 0]';
trierror = [0 0 0 0 0 0]';

t = dt;

v = 0;
w = 0;

testNoise = str2double(get(handles.testNoiseValueText,'String'));

endpoint = size(find(goal(:,1)),1)+1;

set(handles.nn1text,'String','');
set(handles.nn2text,'String','');
set(handles.nn3text,'String','');
set(handles.nn4text,'String','');
set(handles.nn5text,'String','');
set(handles.nnTotalText,'String','');
set(handles.tri1text,'String','');
set(handles.tri2text,'String','');
set(handles.tri3text,'String','');
set(handles.tri4text,'String','');
set(handles.tri5text,'String','');
set(handles.triTotalText,'String','');

for g=1:size(find(goal(:,1)),1)+1
    
    while(run)

        x = pose(k,1);
        y = pose(k,2);
        theta = pose(k,3);

        if(g ~= endpoint)
            xr = goal(g,1);
            yr = goal(g,2);
            thetar = goal(1,3);
        else
            xr = 0;
            yr = 0;
            thetar = 0;
        end

        var = [(xr-x) (yr-y) (thetar-theta)];

        if(var(1) == 0 && var(2) == 0)
            var(1) = 1;
        end

        if(abs(var(1)) < 0.01 && abs(var(2)) < 0.01)
            break;
        end

        p = sqrt(var(1)^2 + var(2)^2);
        a = thetar - theta + atan2(var(2),var(1));
        
        while(a < -pi)
            a = a + 2*pi;
        end
        while(a >= pi)
            a = a - 2*pi;
        end
        
        b = var(3)-a;
        
        while(b < -pi)
            b = b + 2*pi;
        end
        while(b >= pi)
            b = b - 2*pi;
        end
        
        rho(k) = p;
        alpha(k) = a;
        beta(k) = b;
        
        v = kP*p;
        
        if(g ~= endpoint)
            w = kA*a;
        else
            w = kA*a + kB*b;
        end
        
        if(v > (0.5)/dt)
            v = (0.5)/dt;
        end
        
        if(w > (pi/6)/dt)
            w = (pi/6)/dt;
        elseif(w < -(pi/6)/dt)
            w = -(pi/6)/dt;
        end
        
        xnew = x + v*cos(theta)*dt;
        ynew = y + v*sin(theta)*dt;
        thetanew = theta + w*dt;
        
        while(thetanew < -pi)
            thetanew = thetanew + 2*pi;
        end
        
        while(thetanew >= pi)
            thetanew = thetanew - 2*pi;
        end
        
        pose(k+1,:) = [xnew, ynew, thetanew];
        nnpose(k+1,:) = sim(net,distances(xnew, ynew, testNoise)');
        tripose (k+1,:) = triangulation(Landmarks, pose(k+1,:), testNoise);
        
        k = k+1;
        t = t + dt;
        
        cla;
        
        plotGoal(handles.axes, goal, activeGoal);
        plotRobot(handles.axes, pose(k,1), pose(k,2), pose(k,3));
        plotLandmarks(handles.axes, Landmarks, Color);
        
        if(showRanges)
            plotLines(handles.axes, pose(k,1), pose(k,2), Landmarks, Color, Active);
        end
        
        if(showTrajectories)
            plotTrajectory(handles.axes, pose, nnpose, tripose);
        end
        
        drawnow;
        
        guidata(hObject,handles);
        
        nnerror(g,k) = norm(pose(k,1:2) - nnpose(k,1:2));
        trierror(g,k) = norm(pose(k,1:2) - tripose(k,1:2));        
        
    end
    
    v = 0;
    w = 0;
    
    if g==1
        set(handles.nn1text,'String',num2str(sum(nnerror(g,:))));
        set(handles.tri1text,'String',num2str(sum(trierror(g,:))));
    elseif g==2
        set(handles.nn2text,'String',num2str(sum(nnerror(g,:))));
        set(handles.tri2text,'String',num2str(sum(trierror(g,:))));
    elseif g==3
        set(handles.nn3text,'String',num2str(sum(nnerror(g,:))));
        set(handles.tri3text,'String',num2str(sum(trierror(g,:))));
    elseif g==4
        set(handles.nn4text,'String',num2str(sum(nnerror(g,:))));
        set(handles.tri4text,'String',num2str(sum(trierror(g,:))));
    elseif g==5
        set(handles.nn5text,'String',num2str(sum(nnerror(g,:))));
        set(handles.tri5text,'String',num2str(sum(trierror(g,:))));
    elseif g==endpoint
        set(handles.nnTotalText,'String',num2str(sum(nnerror(g,:)) + str2double(get(handles.nn1text,'String')) + str2double(get(handles.nn2text,'String')) + str2double(get(handles.nn3text,'String')) + str2double(get(handles.nn4text,'String')) + str2double(get(handles.nn5text,'String'))));
        set(handles.triTotalText,'String',num2str(sum(trierror(g,:)) + str2double(get(handles.tri1text,'String')) + str2double(get(handles.tri2text,'String')) + str2double(get(handles.tri3text,'String')) + str2double(get(handles.tri4text,'String')) + str2double(get(handles.tri5text,'String'))));
    end
    
end

plotK();
plotError();

run = 0;

% --- Executes on button press in resetButton.
function resetButton_Callback(hObject, eventdata, handles)
% hObject    handle to resetButton (see GCBO)
% eventdata  reserved - to be defined in a future version of MATLAB
% handles    structure with handles and user data (see GUIDATA)

%RESETING ENVIRONMENT

global Landmarks Color Active;
global showRanges;

%RESETING ROBOT STATE VARIABLES

global k dt run;
global goal activeGoal g;
global pose nnpose tripose v w;

k = 1;
dt = 0.1;

run = 0;

goal = zeros(5,3);
activeGoal = zeros(1,size(goal,1));
g = 0;

pose = [0 0 0];
nnpose = [0 0];
tripose = [0 0];

v = 0;
w = 0;

cla;

plotGoal(handles.axes, goal, activeGoal);
plotRobot(handles.axes, pose(1,1), pose(1,2), pose(1,3));
plotLandmarks(handles.axes, Landmarks, Color);

if(showRanges)
    plotLines(handles.axes, pose(1,1), pose(1,2), Landmarks, Color, Active);
end

set(handles.nn1text,'String','');
set(handles.nn2text,'String','');
set(handles.nn3text,'String','');
set(handles.nn4text,'String','');
set(handles.nn5text,'String','');
set(handles.nnTotalText,'String','');
set(handles.tri1text,'String','');
set(handles.tri2text,'String','');
set(handles.tri3text,'String','');
set(handles.tri4text,'String','');
set(handles.tri5text,'String','');
set(handles.triTotalText,'String','');

function neuronValueText_Callback(~, ~, ~)
% hObject    handle to neuronValueText (see GCBO)
% eventdata  reserved - to be defined in a future version of MATLAB
% handles    structure with handles and user data (see GUIDATA)

% Hints: get(hObject,'String') returns contents of neuronValueText as text
%        str2double(get(hObject,'String')) returns contents of neuronValueText as a double


% --- Executes during object creation, after setting all properties.
function neuronValueText_CreateFcn(hObject, ~, ~)
% hObject    handle to neuronValueText (see GCBO)
% eventdata  reserved - to be defined in a future version of MATLAB
% handles    empty - handles not created until after all CreateFcns called

% Hint: edit controls usually have a white background on Windows.
%       See ISPC and COMPUTER.
if ispc && isequal(get(hObject,'BackgroundColor'), get(0,'defaultUicontrolBackgroundColor'))
    set(hObject,'BackgroundColor','white');
end

function scaleValueText_Callback(~, ~, ~)
% hObject    handle to scaleValueText (see GCBO)
% eventdata  reserved - to be defined in a future version of MATLAB
% handles    structure with handles and user data (see GUIDATA)

% Hints: get(hObject,'String') returns contents of scaleValueText as text
%        str2double(get(hObject,'String')) returns contents of scaleValueText as a double


% --- Executes during object creation, after setting all properties.
function scaleValueText_CreateFcn(hObject, ~, ~)
% hObject    handle to scaleValueText (see GCBO)
% eventdata  reserved - to be defined in a future version of MATLAB
% handles    empty - handles not created until after all CreateFcns called

% Hint: edit controls usually have a white background on Windows.
%       See ISPC and COMPUTER.
if ispc && isequal(get(hObject,'BackgroundColor'), get(0,'defaultUicontrolBackgroundColor'))
    set(hObject,'BackgroundColor','white');
end

function trainNoiseValueText_Callback(hObject, eventdata, handles)
% hObject    handle to trainNoiseValueText (see GCBO)
% eventdata  reserved - to be defined in a future version of MATLAB
% handles    structure with handles and user data (see GUIDATA)

% Hints: get(hObject,'String') returns contents of trainNoiseValueText as text
%        str2double(get(hObject,'String')) returns contents of trainNoiseValueText as a double


% --- Executes during object creation, after setting all properties.
function trainNoiseValueText_CreateFcn(hObject, eventdata, handles)
% hObject    handle to trainNoiseValueText (see GCBO)
% eventdata  reserved - to be defined in a future version of MATLAB
% handles    empty - handles not created until after all CreateFcns called

% Hint: edit controls usually have a white background on Windows.
%       See ISPC and COMPUTER.
if ispc && isequal(get(hObject,'BackgroundColor'), get(0,'defaultUicontrolBackgroundColor'))
    set(hObject,'BackgroundColor','white');
end

function testNoiseValueText_Callback(hObject, eventdata, handles)
% hObject    handle to testNoiseValueText (see GCBO)
% eventdata  reserved - to be defined in a future version of MATLAB
% handles    structure with handles and user data (see GUIDATA)

% Hints: get(hObject,'String') returns contents of testNoiseValueText as text
%        str2double(get(hObject,'String')) returns contents of testNoiseValueText as a double


% --- Executes during object creation, after setting all properties.
function testNoiseValueText_CreateFcn(hObject, eventdata, handles)
% hObject    handle to testNoiseValueText (see GCBO)
% eventdata  reserved - to be defined in a future version of MATLAB
% handles    empty - handles not created until after all CreateFcns called

% Hint: edit controls usually have a white background on Windows.
%       See ISPC and COMPUTER.
if ispc && isequal(get(hObject,'BackgroundColor'), get(0,'defaultUicontrolBackgroundColor'))
    set(hObject,'BackgroundColor','white');
end

% --- Executes on button press in trajectoryCBox.
function trajectoryCBox_Callback(hObject, ~, handles)
% hObject    handle to trajectoryCBox (see GCBO)
% eventdata  reserved - to be defined in a future version of MATLAB
% handles    structure with handles and user data (see GUIDATA)

% Hint: get(hObject,'Value') returns toggle state of trajectoryCBox
global Landmarks Color Active;
global showRanges showTrajectories;
global k;
global pose nnpose tripose;
global goal activeGoal g;

showTrajectories = get(hObject,'Value');

cla;

plotGoal(handles.axes, goal, activeGoal);
plotRobot(handles.axes, pose(k,1), pose(k,2), pose(k,3));
plotLandmarks(handles.axes, Landmarks, Color);

if(showRanges)
    plotLines(handles.axes, pose(k,1), pose(k,2), Landmarks, Color, Active);
end

if(showTrajectories)
    plotTrajectory(handles.axes, pose, nnpose, tripose);
end

% --- Executes on button press in neuralCBox.
function neuralCBox_Callback(hObject, eventdata, handles)
% hObject    handle to neuralCBox (see GCBO)
% eventdata  reserved - to be defined in a future version of MATLAB
% handles    structure with handles and user data (see GUIDATA)

% Hint: get(hObject,'Value') returns toggle state of neuralCBox
global Landmarks Color Active;
global showRanges showTrajectories showNeural showTri;
global k;
global pose nnpose tripose;
global goal activeGoal g;

showNeural = get(hObject,'Value');

cla;

plotGoal(handles.axes, goal, activeGoal);
plotRobot(handles.axes, pose(k,1), pose(k,2), pose(k,3));
plotLandmarks(handles.axes, Landmarks, Color);

if(showRanges)
    plotLines(handles.axes, pose(k,1), pose(k,2), Landmarks, Color, Active);
end

if(showTrajectories)
    plotTrajectory(handles.axes, pose, nnpose, tripose);
end

% --- Executes on button press in triCBox.
function triCBox_Callback(hObject, eventdata, handles)
% hObject    handle to triCBox (see GCBO)
% eventdata  reserved - to be defined in a future version of MATLAB
% handles    structure with handles and user data (see GUIDATA)

% Hint: get(hObject,'Value') returns toggle state of triCBox
global Landmarks Color Active;
global showRanges showTrajectories showNeural showTri;
global k;
global pose nnpose tripose;
global goal activeGoal g;

showTri = get(hObject,'Value');

cla;

plotGoal(handles.axes, goal, activeGoal);
plotRobot(handles.axes, pose(k,1), pose(k,2), pose(k,3));
plotLandmarks(handles.axes, Landmarks, Color);

if(showRanges)
    plotLines(handles.axes, pose(k,1), pose(k,2), Landmarks, Color, Active);
end

if(showTrajectories)
    plotTrajectory(handles.axes, pose, nnpose, tripose);
end


% --- Executes on button press in rangeCBox.
function rangeCBox_Callback(hObject, ~, handles)
% hObject    handle to rangeCBox (see GCBO)
% eventdata  reserved - to be defined in a future version of MATLAB
% handles    structure with handles and user data (see GUIDATA)

% Hint: get(hObject,'Value') returns toggle state of rangeCBox
global Landmarks Color Active;
global showRanges showTrajectories showNeural showTri;
global k;
global pose nnpose tripose;
global goal activeGoal g;

showRanges = get(hObject,'Value');

cla;

plotGoal(handles.axes, goal, activeGoal);
plotRobot(handles.axes, pose(k,1), pose(k,2), pose(k,3));
plotLandmarks(handles.axes, Landmarks, Color);

if(showRanges)
    plotLines(handles.axes, pose(k,1), pose(k,2), Landmarks, Color, Active);
end

if(showTrajectories)
    plotTrajectory(handles.axes, pose, nnpose, tripose);
end

% --------------------------------------------------------------------
function Abrir_Callback(~, ~, ~)
% hObject    handle to Abrir (see GCBO)
% eventdata  reserved - to be defined in a future version of MATLAB
% handles    structure with handles and user data (see GUIDATA)


% --------------------------------------------------------------------
function Salvar_Callback(~, ~, ~)
% hObject    handle to Salvar (see GCBO)
% eventdata  reserved - to be defined in a future version of MATLAB
% handles    structure with handles and user data (see GUIDATA)


% --------------------------------------------------------------------
function Fechar_Callback(~, ~, ~)
% hObject    handle to Fechar (see GCBO)
% eventdata  reserved - to be defined in a future version of MATLAB
% handles    structure with handles and user data (see GUIDATA)


% --------------------------------------------------------------------
function Menu_Callback(~, ~, ~)
% hObject    handle to Menu (see GCBO)
% eventdata  reserved - to be defined in a future version of MATLAB
% handles    structure with handles and user data (see GUIDATA)

% --------------------------------------------------------------------
function Zoom_Callback(hObject, eventdata, handles)
% hObject    handle to Zoom (see GCBO)
% eventdata  reserved - to be defined in a future version of MATLAB
% handles    structure with handles and user data (see GUIDATA)

zoom on


% --------------------------------------------------------------------
function Marcador_Callback(hObject, eventdata, handles)
% hObject    handle to Marcador (see GCBO)
% eventdata  reserved - to be defined in a future version of MATLAB
% handles    structure with handles and user data (see GUIDATA)

zoom off

function d = distances(x,y,noise)

global Landmarks;

L = size(Landmarks,1);

d = zeros(1,L);

for I = 1:L
    d(I) = norm([x y]-Landmarks(I,1:2)) + noise*randn;
end

Contact us