image thumbnail

Neural Network (MLP) Robot Localization

by

 

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

kksim(varargin)
function varargout = kksim(varargin)
% KKSIM M-file for kksim.fig
%      KKSIM, by itself, creates a new KKSIM or raises the existing
%      singleton*.
%
%      H = KKSIM returns the handle to a new KKSIM or the handle to
%      the existing singleton*.
%
%      KKSIM('CALLBACK',hObject,eventData,handles,...) calls the local
%      function named CALLBACK in KKSIM.M with the given input arguments.
%
%      KKSIM('Property','Value',...) creates a new KKSIM or raises the
%      existing singleton*.  Starting from the left, property value pairs are
%      applied to the GUI before kksim_OpeningFunction gets called.  An
%      unrecognized property name or invalid value makes property application
%      stop.  All inputs are passed to kksim_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

% Copyright 2002-2003 The MathWorks, Inc.

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

% Last Modified by GUIDE v2.5 18-Apr-2009 11:14:13

% Begin initialization code - DO NOT EDIT
gui_Singleton = 1;
gui_State = struct('gui_Name',       mfilename, ...
                   'gui_Singleton',  gui_Singleton, ...
                   'gui_OpeningFcn', @kksim_OpeningFcn, ...
                   'gui_OutputFcn',  @kksim_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 kksim is made visible.
function kksim_OpeningFcn(hObject, eventdata, 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 kksim (see VARARGIN)

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

% Update handles structure
guidata(hObject, handles);

%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 v w pose goal i kP kA kB run;

%INITIALIZING ROBOT STATE VARIABLES
v=0;
w=0;

pose = [];
pose(1,:) = [0 0 0];

goal =[];
goal(1,:) = [0 0 0];

i=2;
kP=0;
kA=0;
kB=0;

run = 0;

plotRobot(handles.axes,pose(1,1),pose(1,2),pose(1,3));

% UIWAIT makes kksim wait for user response (see UIRESUME)
% uiwait(handles.figure1);


% --- Outputs from this function are returned to the command line.
function varargout = kksim_OutputFcn(hObject, eventdata, 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 slider movement.
function kPslider_Callback(hObject, eventdata, handles)
% hObject    handle to kPslider (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,'Value') returns position of slider
%        get(hObject,'Min') and get(hObject,'Max') to determine range of slider

global kP kA;
kP = get(hObject,'Value');
kP = num2str(kP);
set(handles.kPvalue,'String',kP);

kP = str2num(kP);

if ((kP < 0) || (kA <= kP))
    set(handles.kPtext,'ForegroundColor','r');
else
    set(handles.kPtext,'ForegroundColor','k');
end

if(kA <= kP)
    set(handles.kAtext,'ForegroundColor','r');
else
    set(handles.kAtext,'ForegroundColor','k');
end

guidata(hObject,handles);

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

% Hint: slider controls usually have a light gray background.
if isequal(get(hObject,'BackgroundColor'), get(0,'defaultUicontrolBackgroundColor'))
    set(hObject,'BackgroundColor',[.9 .9 .9]);
end


% --- Executes on slider movement.
function kAslider_Callback(hObject, eventdata, handles)
% hObject    handle to kAslider (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,'Value') returns position of slider
%        get(hObject,'Min') and get(hObject,'Max') to determine range of slider

global kP kA;
kA = get(hObject,'Value');
kA = num2str(kA);
set(handles.kAvalue,'String',kA);

kA = str2num(kA);

if (kP < 0 || kA <= kP)
    set(handles.kPtext,'ForegroundColor','r');
else
    set(handles.kPtext,'ForegroundColor','k');
end

if(kA <= kP)
    set(handles.kAtext,'ForegroundColor','r');
else
    set(handles.kAtext,'ForegroundColor','k');
end

guidata(hObject,handles);

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

% Hint: slider controls usually have a light gray background.
if isequal(get(hObject,'BackgroundColor'), get(0,'defaultUicontrolBackgroundColor'))
    set(hObject,'BackgroundColor',[.9 .9 .9]);
end


% --- Executes on slider movement.
function kBslider_Callback(hObject, eventdata, handles)
% hObject    handle to kBslider (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,'Value') returns position of slider
%        get(hObject,'Min') and get(hObject,'Max') to determine range of slider

global kB;
kB = get(hObject,'Value');
kB = num2str(kB);
set(handles.kBvalue,'String',kB);

kB = str2num(kB);
if (kB < 0)
    set(handles.kBtext,'ForegroundColor','k');
else
    set(handles.kBtext,'ForegroundColor','r');
end

guidata(hObject,handles);

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

% Hint: slider controls usually have a light gray background.
if isequal(get(hObject,'BackgroundColor'), get(0,'defaultUicontrolBackgroundColor'))
    set(hObject,'BackgroundColor',[.9 .9 .9]);
end



function xValue_Callback(hObject, eventdata, handles)
% hObject    handle to xValue (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 xValue as text
%        str2double(get(hObject,'String')) returns contents of xValue as a double


% --- Executes during object creation, after setting all properties.
function xValue_CreateFcn(hObject, eventdata, handles)
% hObject    handle to xValue (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 yValue_Callback(hObject, eventdata, handles)
% hObject    handle to yValue (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 yValue as text
%        str2double(get(hObject,'String')) returns contents of yValue as a double


% --- Executes during object creation, after setting all properties.
function yValue_CreateFcn(hObject, eventdata, handles)
% hObject    handle to yValue (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 playButton.
function playButton_Callback(hObject, eventdata, handles)
% hObject    handle to playButton (see GCBO)
% eventdata  reserved - to be defined in a future version of MATLAB
% handles    structure with handles and user data (see GUIDATA)

global v w pose goal i kP kA kB run
global rho alpha beta

run = 1;
t = 0.01;
k = i;

if(strcmp(get(hObject,'String'),'Play'))
    set(hObject,'String','Pause');
else
    set(hObject,'String','Play');
    run = 0;
    return;
end

while(run)
    t
    
    x = pose(i-1,1);
    y = pose(i-1,2);
    theta = pose(i-1,3) ;
    
    xr = goal(1,1);
    yr = goal(1,2);
    thetar = goal(1,3);
    
    var = [(xr-x) (yr-y) (thetar-theta)];
    %noise... interesting!
    %var(:) = var(:) + (rand-0.5)*0.0000000000000000000001;
    %var(:) = var(:) + 0.001*exp(i/10000);
    
    if(var(1) == 0 && var(2) == 0)
        var(1) == 1;
    end
    
    if(abs(var) < 0.00001)
        run = 0;
        break;
    end
    
    p = sqrt(var(1)^2+var(2)^2);
    a = -theta + atan2(var(2),var(1));
    b = thetar-theta-a;
     
    while(a<-pi)
        a = a + 2*pi;
    end
    
    while(a>pi)
        a = a - 2*pi;
    end
    
    rho(i) = p;
    alpha(i) = a;
    beta(i) = b;

    dp = -kP*p*cos(a);
    da = kP*sin(a) - kA*a - kB*b;
    db = -kP*sin(a);
    
    if (a>-pi/2 && a<=pi/2)
        v = -dp/cos(a);
        w = -da + v*sin(a)/p;
    else
        v = -dp/cos(a);
        w = -da - v*sin(a)/p;
    end
    
    xnew = x + v*cos(theta)*t;
    ynew = y + v*sin(theta)*t;
    thetanew = theta + w*t;
    
    while(thetanew<-pi)
        thetanew = thetanew + 2*pi;
    end
    
    while(thetanew>pi)
        thetanew = thetanew - 2*pi;
    end

    pose(i,:) = [xnew, ynew, thetanew];
    i = i+1;
    t = (i-1)*0.01;

    cla;
    plotRobot(handles.axes,pose(i-1,1),pose(i-1,2),pose(i-1,3));
    plot(handles.axes,pose(:,1),pose(:,2),'--m');
    plot(handles.axes,goal(1,1),goal(1,2),'om');
    plotArrow(handles.axes,goal(1,1),goal(1,2),goal(1,3));
    %plotMap(handles.axes);
    %plotSensor(handles.axes,pose(i-1,1),pose(i-1,2),pose(i-1,3));
    
    drawnow;
    
    status = sprintf('v = %2.3f m/s\nw = %2.3f rad/s\nx = %2.3f m\ny = %2.3f m\ntheta = %2.3f deg',v,w,xnew,ynew,thetanew*(180/pi));
    set(handles.status,'String',status);
    
    simTime = sprintf('Simulation Time = %.2f s',(i-2)/100);
    set(handles.simTime,'String',simTime);
    
    guidata(hObject,handles);   
    
end

set(hObject,'String','Play');
plotK;

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

global v w pose goal i
global rho alpha beta

v=0;
w=0;

pose = [];
pose(1,:) = [0 0 0];

goal =[];
goal(1,:) = [0 0 0];

rho = [];
alpha = [];
beta = [];

i=2;

cla;
plotRobot(handles.axes,pose(1,1),pose(1,2),pose(1,3));

status = sprintf('v = %2.3f m/s\nw = %2.3f rad/s\nx = %2.3f m\ny = %2.3f m\ntheta = %2.3f deg',v,w,0,0,0);
set(handles.status,'String',status);
    
simTime = sprintf('Simulation Time = %.2f s',(i-2)/100);
set(handles.simTime,'String',simTime);


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

global pose goal i

'REPLAYING..'

for I=1:i-1
    cla;
    plotRobot(handles.axes,pose(I,1),pose(I,2),pose(I,3));
    plot(handles.axes,pose(:,1),pose(:,2),'--m');
    plot(handles.axes,goal(1,1),goal(1,2),'om');
    plotArrow(handles.axes,goal(1,1),goal(1,2),goal(1,3));
    drawnow;
end

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

global pose goal i

x = get(handles.xValue,'String'); % Pega o string no xValue
y = get(handles.yValue,'String'); % Pega o string no yValue
t = get(handles.tValue,'String'); % Pega o string no tValue

goal(1,1) = str2num([x]);
goal(1,2) = str2num([y]);
goal(1,3) = str2num([t])*(pi/180);

cla;
plotRobot(handles.axes,pose(i-1,1),pose(i-1,2),pose(i-1,3));
plot(handles.axes,pose(:,1),pose(:,2),'--m');
plot(handles.axes,goal(1,1),goal(1,2),'om');
plotArrow(handles.axes,goal(1,1),goal(1,2),goal(1,3));
%plotMap(handles.axes);
%plotSensor(handles.axes,pose(i-1,1),pose(i-1,2),pose(i-1,3));

guidata(hObject,handles);

function tValue_Callback(hObject, eventdata, handles)
% hObject    handle to tValue (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 tValue as text
%        str2double(get(hObject,'String')) returns contents of tValue as a double


% --- Executes during object creation, after setting all properties.
function tValue_CreateFcn(hObject, eventdata, handles)
% hObject    handle to tValue (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 File_Callback(hObject, eventdata, handles)
% hObject    handle to File (see GCBO)
% eventdata  reserved - to be defined in a future version of MATLAB
% handles    structure with handles and user data (see GUIDATA)


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


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


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

Contact us