diff --git a/GPR_based_stream_MPC/get_stream_trajectory.m b/GPR_based_stream_MPC/get_stream_trajectory.m
new file mode 100644
index 0000000..384acf4
--- /dev/null
+++ b/GPR_based_stream_MPC/get_stream_trajectory.m
@@ -0,0 +1,54 @@
+function [ traj ] = get_stream_trajectory(tstep,z,obstacle_type,obstaclepred)
+
+traj= [z;0;0];
+xnew= z(1);
+ynew= z(2);
+% planning for the horizon
+
+%% Initialization for stream function
+a = 2;
+% Set the source/sink strength based on obstacle type
+if obstacle_type == 0
+    % Stationary obstacles
+    C = 1;
+elseif obstacle_type == 1
+    % Moving obstacles
+    C = 12 * sqrt(Vx*Vx + Vy*Vy);
+end
+
+%% Run the horizon
+for i= 1:1:7
+    obs_pose = obstaclepred(i,:);
+    bx= obs_pose(1);
+    by= obs_pose(2);
+    Vx= obs_pose(3);
+    Vy= obs_pose(4);
+    % equations from paper
+    [U, V, psi, U_dot, V_dot] = stream_moving_obstacle();
+    X= xnew;
+    Y= ynew;
+%     Vref=0.015;
+
+    % store the robot states using the stream function with
+    % obstacle states given from prediction model
+    U1= eval(U);
+    V1= eval(V);
+    U1_dot= eval(U_dot);
+    V1_dot= eval(V_dot);
+    theta = atan2(V1,U1);
+%     u = Vref*cos(theta);
+%     v = Vref*sin(theta);
+    xnew= xnew + U1*tstep;
+    ynew= ynew + V1*tstep;
+    % velocity
+    vstep= U1*cos(theta)+V1*sin(theta);
+    traj(4,i)= vstep;
+    % steering = (calculated_step_theta - last_step_theta)/time
+    traj(5,i)= (theta-traj(3,i))/tstep;
+    
+    % store the new predicted robot states of the horizon
+    znew= [xnew;ynew;theta;0;0];
+    traj= [traj,znew];
+end
+
+end
\ No newline at end of file
diff --git a/GPR_based_stream_MPC/get_stream_trajectory_old.m b/GPR_based_stream_MPC/get_stream_trajectory_old.m
new file mode 100644
index 0000000..0927fc9
--- /dev/null
+++ b/GPR_based_stream_MPC/get_stream_trajectory_old.m
@@ -0,0 +1,50 @@
+function [ traj ] = get_stream_trajectory(tstep,z,obstaclepred)
+
+traj= [z;0;0];
+xnew= z(1);
+ynew= z(2);
+% planning for the horizon
+
+%% Initialization for stream function
+a = 2;
+
+%% Run the horizon
+for i= 1:1:7
+    obs_pose = obstaclepred(i,:);
+    bx= obs_pose(1);
+    by= obs_pose(2);
+    Vx= obs_pose(3);
+    Vy= obs_pose(4);
+    % equations from paper
+    
+    % Moving obstacles
+    C = 12 * sqrt(Vx*Vx + Vy*Vy);
+    
+    [U, V, psi, U_dot, V_dot] = stream_moving_obstacle();
+    X= xnew;
+    Y= ynew;
+%     Vref=0.015;
+
+    % store the robot states using the stream function with
+    % obstacle states given from prediction model
+    U1= eval(U);
+    V1= eval(V);
+    U1_dot= eval(U_dot);
+    V1_dot= eval(V_dot);
+    theta = atan2(V1,U1);
+%     u = Vref*cos(theta);
+%     v = Vref*sin(theta);
+    xnew= xnew + U1*tstep;
+    ynew= ynew + V1*tstep;
+    % velocity
+    vstep= U1*cos(theta)+V1*sin(theta);
+    traj(4,i)= vstep;
+    % steering = (calculated_step_theta - last_step_theta)/time
+    traj(5,i)= (theta-traj(3,i))/tstep;
+    
+    % store the new predicted robot states of the horizon
+    znew= [xnew;ynew;theta;0;0];
+    traj= [traj,znew];
+end
+
+end
\ No newline at end of file
diff --git a/GPR_based_stream_MPC/gpr_based_stream.m b/GPR_based_stream_MPC/gpr_based_stream.m
new file mode 100644
index 0000000..9895b87
--- /dev/null
+++ b/GPR_based_stream_MPC/gpr_based_stream.m
@@ -0,0 +1,194 @@
+clc;
+clear;
+close all;
+%initialize
+
+
+% Stream function init block
+tf=100.00;
+tstep = 0.10;
+x0= 9;
+y0= 5;
+theta0= 0;
+bx0= 5.00;
+by0= 5.00;
+vx0= 1.5;
+vy0= -0.55;
+
+% GPR block
+% past obstacle trajectory
+bXtrain= [bx0];
+bYtrain= [by0];
+VXtrain= [vx0];
+VYtrain= [vy0];
+% initialize time simulation from -tstep
+Xtrain= [-0.10];
+% store the robot state
+zinit= [x0;y0;theta0];
+% store the robot state with MPC
+zmpc= [x0;y0;theta0];
+
+figure;
+radius_robot=0.3;
+radius_obs= 1.4;
+
+% MPC block
+% controller params
+N = 7; % length of window
+Q = [1;1;0.5];
+R = [0.1;0.1];
+u_max = [0.47;3.77]; % controller boundary
+warm_start = [0;0;0;0;0;0;0;0;0;0;0;0;0;0];
+
+% calculation of const params
+u_min = -u_max;
+Qhat = diag(repmat(Q,[N,1]));
+Rhat = diag(repmat(R,[N,1]));
+d = [repmat(u_max,[N,1]);repmat(u_min,[N,1])];
+D = [eye(2*N);-eye(2*N)];
+k=1;
+
+% sync the time horizon with ROS 
+for t= 0.00:0.10:tf
+    
+    %% obstacle prediction using GPR
+    
+    % current coordinates and velocity calculation for the obstacle
+    % provide the current state of the obstacle here
+    bx= bx0 + vx0*t; 
+    by= by0 + vy0*t;
+    vx= vx0;
+    vy= vy0;
+    Xtrain =[Xtrain;t];
+    
+    %updating trains for learning
+    % update the X Y positions each time step
+    bXtrain= [bXtrain;bx];
+    bYtrain= [bYtrain;by];
+    VXtrain= [VXtrain;vx];
+    VYtrain= [VYtrain;vy];
+    
+    %creating exact GPR models
+    gprMdbx = fitrgp(Xtrain,bXtrain,'Basis','linear','FitMethod','exact','PredictMethod','exact');
+    gprMdby = fitrgp(Xtrain,bYtrain,'Basis','linear','FitMethod','exact','PredictMethod','exact');
+    gprMdvx = fitrgp(Xtrain,VXtrain,'Basis','linear','FitMethod','exact','PredictMethod','exact');
+    gprMdvy = fitrgp(Xtrain,VYtrain,'Basis','linear','FitMethod','exact','PredictMethod','exact');
+    
+    % create future time step horizon, sync with ROS
+    Xtrain_pred=[t;t+0.1;t+0.2;t+0.3;t+0.4;t+0.5;t+0.6;t+0.7];
+    % prediction from GPR for the horizon
+    [bxtestpred] = predict(gprMdbx,Xtrain_pred);
+    [bytestpred] = predict(gprMdby,Xtrain_pred);
+    [vxtestpred] = predict(gprMdvx,Xtrain_pred);
+    [vytestpred] = predict(gprMdvy,Xtrain_pred);
+    % predicted horizon for obstacle
+    obstaclepred= [bxtestpred,bytestpred,vxtestpred,vytestpred];
+    
+    %% trajectory planning
+    
+    % provide the current state of the robot
+    current_state = [x0;y0;theta0];
+    traj= get_stream_trajectory_old(tstep,current_state,obstaclepred);
+    
+    % get desired robot state from horizon data
+    x0_d= traj(1,2);
+    y0_d= traj(2,2);
+    theta0_d= traj(3,2);
+    
+    z=[x0_d;y0_d;theta0_d];
+    
+    % update robot state
+    zinit=[zinit,z];
+%     figure('Name','Stream Flow');
+%     plot(traj(1,:), traj(2,:),'r-');
+%     hold on
+    if x0*x0 + y0*y0 <= 1
+        break;
+    end
+    
+%     delete(vch);
+%     delete(vch1);
+
+%     vch1 = viscircles([bx,by], radius_obs,'Color','r');
+%     plot(bxtestpred, bytestpred,'r-');
+%     vch = viscircles([x0,y0], radius_robot,'Color','g');
+%     plot(traj(1,:), traj(2,:),'g-');
+%     
+%     hold on;
+%     pause(0.01);
+%     delete(vch);
+%     delete(vch1);
+    
+    
+    
+    %% MPC
+    vr= traj(4,:);
+    qr= traj(1:3,:);
+    
+    % check time step for MPC calculation for turtlebot simulation
+    dt=tstep;
+     % update A and B matrix for the window
+    for i = 1:N
+        A(:,:,i) = [1, 0, -vr(i)*sin(qr(3,i)*dt);
+            0, 1, vr(i)*cos(qr(3,i)*dt)
+            0, 0 ,1];
+        B(:,:,i) = [cos(qr(3,i))*dt, 0;
+            sin(qr(3,i))*dt, 0;
+            0, dt];
+    end
+    
+    
+   % introduce new Ahat(3N,3), Bhat(3N,2N)
+    Ahat = repmat(eye(3,3),N,1);
+    Bhat = repmat(zeros(3,2),N,N);
+    for i = 1:N
+        Bhat(i*3-2:end,i*2-1:i*2) = repmat(B(:,:,i),N+1-i,1);
+        for j = i:N
+            Ahat(j*3-2:j*3,:) = A(:,:,i)*Ahat(j*3-2:j*3,:);
+            for m = i+1:j
+                Bhat(j*3-2:j*3,i*2-1:i*2) = A(:,:,m)*Bhat(j*3-2:j*3,i*2-1:i*2);
+            end
+        end
+    end
+
+    
+    ehat= [0.00001;0.00001;0.00001];
+    H = 2*(Bhat'*Qhat*Bhat+Rhat);
+    f = 2*Bhat'*Qhat*Ahat*ehat;
+    OPTIONS = optimset('Display','off','MaxIter',5);
+%     OPTIONS = optimset('Display','off');
+    q_op = quadprog(H,f,[],[],[],[],[],[],warm_start,OPTIONS)
+    
+    warm_start = q_op;
+    
+    % take velocity from stream function and use current theta for next
+    % step calculations
+    x0= x0+(traj(4,1)+q_op(1))*cos(theta0)*dt;
+    y0= y0+(traj(4,1)+q_op(1))*sin(theta0)*dt;
+    theta0 = theta0+(traj(5,1)+q_op(2))*dt;
+    % final robot state calculated using Stream function and MPC
+    zf=[x0;y0;theta0];
+    % total robot state/trajectory using MPC
+    zmpc=[zmpc,zf];
+    
+    xlim([0 10]);
+    ylim([0 10]);
+    vch1 = viscircles([bx,by], radius_obs,'Color','r');
+    plot(bxtestpred, bytestpred,'r-');
+    vch = viscircles([x0,y0], radius_robot,'Color','g');
+    plot(traj(1,:), traj(2,:),'g-');
+    
+    hold on;
+    pause(0.01);
+    delete(vch);
+    delete(vch1);
+ 
+end
+    figure('Name','Stream Flow');
+    plot(zinit(1,:), zinit(2,:),'r-');
+    hold on
+    plot(zmpc(1,:), zmpc(2,:),'b-');
+    hold on
+    plot(bXtrain, bYtrain,'r-');
+    hold on
+
diff --git a/GPR_based_stream_MPC/gpr_based_stream_turtlebot.m b/GPR_based_stream_MPC/gpr_based_stream_turtlebot.m
new file mode 100644
index 0000000..5b400c3
--- /dev/null
+++ b/GPR_based_stream_MPC/gpr_based_stream_turtlebot.m
@@ -0,0 +1,248 @@
+%% Initialize
+clc;
+clear;
+close all;
+
+%% Select obstacle type 
+% 0 : Stationary Obstacles 
+% 1 : Moving Obstacles
+obstacle_type = 0;
+
+%% Select state feedback type
+% 0 : Fixed positions
+% 1 : VICON motion capture system
+state_fb = 0;
+
+if state_fb == 0    
+    % Initial robot state
+    x0= 9;
+    y0= 5;
+    theta0= 0;
+    % Initial obstacle state
+    bx0= 5.00;
+    by0= 5.00;
+elseif state_fb == 1
+    % Provide the initial/starting state of robot and obstacle
+    state_sub = rossubscriber('/vicon/turtlebot/turtlebot');
+    % Set x0 y0 theta0
+    
+    % Set bx0 by0
+    
+end
+
+if obstacle_type == 0
+    % Stationary obstacles have zero velocity
+    vx0 = 0;
+    vy0 = 0;
+elseif obstacle_type == 1
+    % Moving obstacles have non-zero velocity    
+    vx0= 1.5;
+    vy0= -0.55;
+end
+
+%% GPR block
+% past obstacle trajectory
+bXtrain= [bx0];
+bYtrain= [by0];
+VXtrain= [vx0];
+VYtrain= [vy0];
+% initialize time simulation from -tstep
+Xtrain= [-0.10];
+% store the robot state
+zinit= [x0;y0;theta0];
+% store the robot state with MPC
+zmpc= [x0;y0;theta0];
+
+figure;
+%% Update radii of the robot and obstacle
+radius_robot=0.3;
+radius_obs= 1.4;
+
+%% MPC block
+% controller params
+N = 7; % length of window
+Q = [1;1;0.5];
+R = [0.1;0.1];
+u_max = [0.47;3.77]; % controller boundary
+warm_start = [0;0;0;0;0;0;0;0;0;0;0;0;0;0];
+
+% calculation of const params
+u_min = -u_max;
+Qhat = diag(repmat(Q,[N,1]));
+Rhat = diag(repmat(R,[N,1]));
+d = [repmat(u_max,[N,1]);repmat(u_min,[N,1])];
+D = [eye(2*N);-eye(2*N)];
+k=1;
+
+% % Initialize ROS and connect to TurtleBot
+% ip_turtlebot = '130.215.121.108';
+% rosinit(ip_turtlebot)
+
+% initialize_turtlebot();
+
+% Initialize simulation time and horizon time step
+tf = 100.00;
+tstep = 0.10;
+
+%% Sync the time horizon with ROS 
+
+% Run the simulation for the simulation time with tstep increments
+for t = 0.00:tstep:tf    
+    %% Obstacle prediction using GPR
+    
+    % current coordinates and velocity calculation for the obstacle
+    % provide the current state of the obstacle here
+    bx= bx0 + vx0*t; 
+    by= by0 + vy0*t;
+    vx= vx0;
+    vy= vy0;
+    Xtrain =[Xtrain;t];
+    
+    % updating trains for learning
+    % update the X Y positions each time step
+    bXtrain= [bXtrain;bx];
+    bYtrain= [bYtrain;by];
+    VXtrain= [VXtrain;vx];
+    VYtrain= [VYtrain;vy];
+    
+    %creating exact GPR models
+    gprMdbx = fitrgp(Xtrain,bXtrain,'Basis','linear','FitMethod','exact','PredictMethod','exact');
+    gprMdby = fitrgp(Xtrain,bYtrain,'Basis','linear','FitMethod','exact','PredictMethod','exact');
+    gprMdvx = fitrgp(Xtrain,VXtrain,'Basis','linear','FitMethod','exact','PredictMethod','exact');
+    gprMdvy = fitrgp(Xtrain,VYtrain,'Basis','linear','FitMethod','exact','PredictMethod','exact');
+    
+    %% create future time step horizon, sync with ROS
+    Xtrain_pred=[t;t+0.1;t+0.2;t+0.3;t+0.4;t+0.5;t+0.6;t+0.7];
+    % prediction from GPR for the horizon
+    [bxtestpred] = predict(gprMdbx,Xtrain_pred);
+    [bytestpred] = predict(gprMdby,Xtrain_pred);
+    [vxtestpred] = predict(gprMdvx,Xtrain_pred);
+    [vytestpred] = predict(gprMdvy,Xtrain_pred);
+    % predicted horizon for obstacle
+    obstaclepred= [bxtestpred,bytestpred,vxtestpred,vytestpred];
+    
+    %% trajectory planning
+    
+    % provide the current state of the robot
+    current_state = [x0;y0;theta0];
+    traj= get_stream_trajectory(tstep,current_state,obstacle_type,obstaclepred);
+    
+    % get desired robot state from horizon data
+    x0_d= traj(1,2);
+    y0_d= traj(2,2);
+    theta0_d= traj(3,2);
+    
+    z=[x0_d;y0_d;theta0_d];
+    
+    % update robot state
+    zinit=[zinit,z];
+    % figure('Name','Stream Flow');
+    % plot(traj(1,:), traj(2,:),'r-');
+    % hold on
+    
+    % Stop when robot state is very close to origin
+    if x0*x0 + y0*y0 <= 1
+        break;
+    end
+    
+%     delete(vch);
+%     delete(vch1);
+
+%     vch1 = viscircles([bx,by], radius_obs,'Color','r');
+%     plot(bxtestpred, bytestpred,'r-');
+%     vch = viscircles([x0,y0], radius_robot,'Color','g');
+%     plot(traj(1,:), traj(2,:),'g-');
+%     
+%     hold on;
+%     pause(0.01);
+%     delete(vch);
+%     delete(vch1);
+    
+    
+    
+    %% MPC
+    vr= traj(4,:);
+    qr= traj(1:3,:);
+    
+    % check time step for MPC calculation for turtlebot simulation
+    dt=tstep;
+     % update A and B matrix for the window
+    for i = 1:N
+        A(:,:,i) = [1, 0, -vr(i)*sin(qr(3,i)*dt);
+            0, 1, vr(i)*cos(qr(3,i)*dt)
+            0, 0 ,1];
+        B(:,:,i) = [cos(qr(3,i))*dt, 0;
+            sin(qr(3,i))*dt, 0;
+            0, dt];
+    end
+    
+    
+   % introduce new Ahat(3N,3), Bhat(3N,2N)
+    Ahat = repmat(eye(3,3),N,1);
+    Bhat = repmat(zeros(3,2),N,N);
+    for i = 1:N
+        Bhat(i*3-2:end,i*2-1:i*2) = repmat(B(:,:,i),N+1-i,1);
+        for j = i:N
+            Ahat(j*3-2:j*3,:) = A(:,:,i)*Ahat(j*3-2:j*3,:);
+            for m = i+1:j
+                Bhat(j*3-2:j*3,i*2-1:i*2) = A(:,:,m)*Bhat(j*3-2:j*3,i*2-1:i*2);
+            end
+        end
+    end
+
+    
+    ehat= [0.00001;0.00001;0.00001];
+    H = 2*(Bhat'*Qhat*Bhat+Rhat);
+    f = 2*Bhat'*Qhat*Ahat*ehat;
+    OPTIONS = optimset('Display','off','MaxIter',5);
+    % OPTIONS = optimset('Display','off');
+    q_op = quadprog(H,f,[],[],[],[],[],[],warm_start,OPTIONS);
+    
+    warm_start = q_op;
+    
+    % take velocity from stream function and use current theta for next
+    % step calculations
+    x0= x0+(traj(4,1)+q_op(1))*cos(theta0)*dt;
+    y0= y0+(traj(4,1)+q_op(1))*sin(theta0)*dt;
+    theta0 = theta0+(traj(5,1)+q_op(2))*dt;
+    
+    %% Send control inputs to TurtleBot
+    % Velocity
+    % V = traj(4,1)+q_op(1) 
+    
+    % Steer
+    % theta = theta0
+    
+    
+    % final robot state calculated using Stream function and MPC
+    zf=[x0;y0;theta0];
+    % total robot state/trajectory using MPC
+    zmpc=[zmpc,zf];
+    
+    xlim([0 10]);
+    ylim([0 10]);
+    
+    vch1 = viscircles([bx,by], radius_obs,'Color','r');
+    %figure('Name','Realtime trajectory Plot of Robot and Obstacle');
+    plot(bxtestpred, bytestpred,'r-');
+    vch = viscircles([x0,y0], radius_robot,'Color','g');
+    plot(traj(1,:), traj(2,:),'g-');
+    legend({'Robot','Obstacle'})
+    
+    hold on;
+    pause(0.01);
+    delete(vch);
+    delete(vch1);
+ 
+end
+    figure('Name','Trajectory Plot of Robot and Obstacle');
+    plot(zinit(1,:), zinit(2,:),'b-');
+    hold on
+    plot(zmpc(1,:), zmpc(2,:),'g-');
+    hold on
+    plot(bXtrain, bYtrain,'r-');
+    hold on
+    legend({'Robot trajectory from Stream Function','Robot trajectory using MPC','Obstacle trajectory using GPR'},'Location','southeast')
+
+    
+% terminate_turtlebot();
\ No newline at end of file
diff --git a/GPR_based_stream_MPC/stream_moving_obstacle.m b/GPR_based_stream_MPC/stream_moving_obstacle.m
new file mode 100644
index 0000000..31febe5
--- /dev/null
+++ b/GPR_based_stream_MPC/stream_moving_obstacle.m
@@ -0,0 +1,61 @@
+function [u, v, psi,u_dot, v_dot] = stream_obstacle()
+% Implementation of the stream function for avoiding single stationary and 
+% a single moving obstacle using Circle Theorem
+% Reference: https://doi.org/10.1109/ROBOT.2003.1241966
+% Authors: Mitesh Agrawal and Mohammed Tousif Zaman
+% Contact: msagrawal[at]wpi[dot]edu and mzaman[at]wpi[dot]edu
+
+clc;
+clearvars;
+
+syms bx by X Y a Vx Vy C real
+
+% C = 12;
+% Vx = 2;
+% Vy = 3;
+
+% b = bx + i*by defines the velocity of the moving obstacle
+b = bx + by*1i;
+% b_ is the conjugate of b
+b_ = conj(b);
+% complex variables for defining the potential field
+z = X + Y*1i;
+Z = ((a^2/(z-b)) + b_);
+
+% f = -C*log(z);
+% f_ = -C*log(Z);
+% w_stationary = f + f_;
+
+% considering velocity of moving obstacle for the potential field
+g = -Vx*z + Vy*z*1i;
+g_ = -Vx*Z - Vy*Z*1i;
+w_moving = g + g_;
+
+% alternate calculation
+% w = w_stationary + w_moving;
+% psi = imag(w);
+
+% stream function from imaginary component of complex potential for
+% avoiding a single moving obstacle
+psi2 = imag(w_moving);
+
+% definition from single obstacle avoidance case
+% Y1 and X1 - considering single circular shaped obstacle
+Y1 = (((a^2)*(Y-by))/(((X-bx)^2)+((Y-by)^2)))+by;
+X1 = (((a^2)*(X-bx))/(((X-bx)^2)+((Y-by)^2)))+bx;
+
+% stream function from imaginary component of complex potential for
+% avoiding a single stationary obstacle
+psi1 = -C*(atan2(Y,X)) + C*(atan2(Y1,X1));
+
+% stream function from imaginary component of the total complex potential
+% considering both stationary and moving obstacles
+psi = psi1 + psi2;
+
+% velocity components of the stream function
+u = diff(psi,Y);
+u_dot= diff(u,X);
+v = -1*diff(psi,X);
+v_dot= diff(v,Y);
+
+end
\ No newline at end of file
diff --git a/GPR_based_stream_MPC/test.m b/GPR_based_stream_MPC/test.m
new file mode 100644
index 0000000..44e8d52
--- /dev/null
+++ b/GPR_based_stream_MPC/test.m
@@ -0,0 +1,34 @@
+%% Script to test the connectivity between MATLAB and TurtleBot
+
+% Set ROS_IP and ROS_MASTER_URI
+setenv('ROS_IP','130.215.206.232')
+setenv('ROS_MASTER_URI','http://130.215.121.108:11311')
+
+% Initialize ROS and connect to TurtleBot
+ip_turtlebot = '130.215.121.108';
+rosinit(ip_turtlebot)
+
+velocityX = 0.1;     % meters per second
+velocityY = 0.3;     % meters per second
+
+% Publish velocity message to TurtleBot
+my_turtlebot = rospublisher('/mobile_base/commands/velocity');
+velmsg = rosmessage(my_turtlebot);
+velmsg.Linear.X = velocityX;
+velmsg.Linear.Y = velocityY;
+
+for num=1:10
+    send(my_turtlebot,velmsg);
+end
+
+% Publish and send sound message to TurtleBot
+soundpub = rospublisher('/mobile_base/commands/sound', 'kobuki_msgs/Sound');
+soundmsg = rosmessage('kobuki_msgs/Sound');
+soundmsg.Value = 6;      % Any number 0-6
+send(soundpub,soundmsg);
+
+
+% Clear the workspace of publishers, subscribers and other ROS objects
+% clear
+% Disconnect from the ROS network
+% rosshutdown
diff --git a/LICENSE b/Stream_Function/LICENSE
similarity index 100%
rename from LICENSE
rename to Stream_Function/LICENSE
diff --git a/README.md b/Stream_Function/README.md
similarity index 100%
rename from README.md
rename to Stream_Function/README.md
diff --git a/cumsimp.m b/Stream_Function/cumsimp.m
similarity index 100%
rename from cumsimp.m
rename to Stream_Function/cumsimp.m
diff --git a/flowfun.m b/Stream_Function/flowfun.m
similarity index 100%
rename from flowfun.m
rename to Stream_Function/flowfun.m
diff --git a/stream_moving.m b/Stream_Function/stream_moving.m
similarity index 100%
rename from stream_moving.m
rename to Stream_Function/stream_moving.m
diff --git a/stream_moving_obstacle.m b/Stream_Function/stream_moving_obstacle.m
similarity index 100%
rename from stream_moving_obstacle.m
rename to Stream_Function/stream_moving_obstacle.m
diff --git a/stream_stationary.m b/Stream_Function/stream_stationary.m
similarity index 100%
rename from stream_stationary.m
rename to Stream_Function/stream_stationary.m
diff --git a/stream_stationary_obstacle.m b/Stream_Function/stream_stationary_obstacle.m
similarity index 100%
rename from stream_stationary_obstacle.m
rename to Stream_Function/stream_stationary_obstacle.m
diff --git a/Stream_Function_ODE/cumsimp.m b/Stream_Function_ODE/cumsimp.m
new file mode 100644
index 0000000..0f1e214
--- /dev/null
+++ b/Stream_Function_ODE/cumsimp.m
@@ -0,0 +1,50 @@
+function  f = cumsimp(y)
+
+% F = CUMSIMP(Y)    Simpson-rule column-wise cumulative summation.
+%       Numerical approximation of a function F(x) such that 
+%       Y(X) = dF/dX.  Each column of the input matrix Y represents
+%       the value of the integrand  Y(X)  at equally spaced points
+%       X = 0,1,...size(Y,1).
+%       The output is a matrix  F of the same size as Y.
+%       The first row of F is equal to zero and each following row
+%       is the approximation of the integral of each column of matrix
+%       Y up to the givem row.
+%       CUMSIMP assumes continuity of each column of the function Y(X)
+%       and uses Simpson rule summation.
+%       Similar to the command F = CUMSUM(Y), exept for zero first
+%       row and more accurate summation (under the assumption of
+%       continuous integrand Y(X)).
+% 
+%    See also CUMSUM, SUM, TRAPZ, QUAD
+
+%  Kirill K. Pankratov, March 7, 1994.
+
+ % 3-points interpolation coefficients to midpoints.
+ % Second-order polynomial (parabolic) interpolation coefficients
+ % from  Xbasis = [0 1 2]  to  Xint = [.5 1.5]
+c1 = 3/8; c2 = 6/8; c3 = -1/8;
+
+ % Determine the size of the input and make column if vector
+ist = 0;         % If to be transposed
+lv = size(y,1);
+if lv==1, ist = 1; y = y(:); lv = length(y); end
+f = zeros(size(y));
+
+ % If only 2 elements in columns - simple sum divided by 2
+if lv==2
+  f(2,:) = (y(1,:)+y(2))/2;
+  if ist, f = f'; end   % Transpose output if necessary
+  return
+end
+
+ % If more than two elements in columns - Simpson summation
+num = 1:lv-2;
+   % Interpolate values of Y to all midpoints
+f(num+1,:) = c1*y(num,:)+c2*y(num+1,:)+c3*y(num+2,:);
+f(num+2,:) = f(num+2,:)+c3*y(num,:)+c2*y(num+1,:)+c1*y(num+2,:);
+f(2,:) = f(2,:)*2; f(lv,:) = f(lv,:)*2;
+   % Now Simpson (1,4,1) rule
+f(2:lv,:) = 2*f(2:lv,:)+y(1:lv-1,:)+y(2:lv,:);
+f = cumsum(f)/6;  % Cumulative sum, 6 - denom. from the Simpson rule
+
+if ist, f = f'; end     % Transpose output if necessary
diff --git a/Stream_Function_ODE/flowfun.m b/Stream_Function_ODE/flowfun.m
new file mode 100644
index 0000000..2a02700
--- /dev/null
+++ b/Stream_Function_ODE/flowfun.m
@@ -0,0 +1,92 @@
+function  [phi,psi] = flowfun(u,v,flag)
+
+% FLOWFUN  Computes the potential PHI and the streamfunction PSI
+%     of a 2-dimensional flow defined by the matrices of velocity
+%     components U and V, so that
+%
+%           d(PHI)    d(PSI)          d(PHI)    d(PSI)
+%      u =  -----  -  ----- ,    v =  -----  +  -----
+%            dx        dy              dx        dy
+%
+%     For a potential (irrotational) flow  PSI = 0, and the laplacian
+%     of PSI is equal to the divergence of the velocity field.
+%     A non-divergent flow can be described by the streamfunction
+%     alone, and the laplacian of the streamfunction is equal to
+%     vorticity (curl) of the velocity field.
+%     The stepsizes dx and dy are assumed to equal unity.
+%   [PHI,PSI] = FLOWFUN(U,V), or in a complex form
+%   [PHI,PSI] = FLOWFUN(U+iV)
+%     returns matrices PHI and PSI of the same sizes as U and V,
+%     containing potential and streamfunction given by velocity
+%     components U, V.
+%     Because these potentials are defined up to the integration
+%     constant their absolute values are such that
+%     PHI(1,1) = PSI(1,1) = 0.
+%     If only streamfunction is needed, the flag can be used:
+%   PSI = FLOWFUN(`,FLAG), where FLAG can be a string:
+%     '-', 'psi', 'streamfunction' (abbreviations allowed).
+%     For the potential the FLAG can be  '+', 'phi', 'potential'.
+
+%  Uses command CUMSIMP (Simpson rule summation).
+
+%  Kirill K. Pankratov, March 7, 1994.
+
+% Check input arguments .............................................
+issu=0; issv=0; isflag=0;    % For input checking
+isphi = 1; ispsi = 1;        % Is phi and psi to be computed
+if nargin==1, issu = isstr(u); end
+if nargin==2, issv = isstr(v); end
+if nargin==1&~issu, v=imag(u); end
+if issv, flag = v; v = imag(u); isflag = 1; end 
+if nargin==0|issu            % Not enough input arguments
+  disp([10 '  Error: function must have input arguments:'...
+  10 '  matrivces  U and V  (or complex matrix W = U+iV)' 10 ])
+  return
+end
+if any(size(u)~=size(v))     % Disparate sizes
+  disp([10 '  Error: matrices U and V must be of equal size' 10])
+  return
+end
+if nargin==3, isflag=1; end
+u = real(u);
+
+ % Check the flag string . . . . . . . .
+Dcn = str2mat('+','potential','phi');
+Dcn = str2mat(Dcn,'-','streamfunction','psi');
+if isflag
+  lmin = min(size(flag,2),size(Dcn,2));
+  flag = flag(1,1:lmin);
+  A = flag(ones(size(Dcn,1),1),1:lmin)==Dcn(:,1:lmin);
+  if lmin>1, coinc = sum(A'); else, coinc = A'; end
+  fnd = find(coinc==lmin);
+  if fnd~=[], if fnd<4, ispsi=0; else, isphi=0; end, end
+end
+
+phi = [];        % Create output
+psi = [];
+
+lx = size(u,2);  % Size of the velocity matrices
+ly = size(u,1);
+
+% Now the main computations .........................................
+% Integrate velocity fields to get potential and streamfunction
+% Use Simpson rule summation (function CUMSIMP)
+
+ % Compute potential PHI (potential, non-rotating part)
+if isphi
+  cx = cumsimp(u(1,:));  % Compute x-integration constant
+  cy = cumsimp(v(:,1));  % Compute y-integration constant
+  phi = cumsimp(v)+cx(ones(ly,1),:);
+  phi = (phi+cumsimp(u')'+cy(:,ones(1,lx)))/2;
+end
+
+ % Compute streamfunction PSI (solenoidal part)
+if ispsi
+  cx = cumsimp(v(1,:));  % Compute x-integration constant
+  cy = cumsimp(u(:,1));  % Compute y-integration constant
+  psi = -cumsimp(u)+cx(ones(ly,1),:);
+  psi = (psi+cumsimp(v')'-cy(:,ones(1,lx)))/2;
+end
+
+ % Rename output if need only PSI
+if ~isphi&ispsi&nargout==1, phi = psi; end
diff --git a/Stream_Function_ODE/get_obstacle.m b/Stream_Function_ODE/get_obstacle.m
new file mode 100644
index 0000000..08dabc1
--- /dev/null
+++ b/Stream_Function_ODE/get_obstacle.m
@@ -0,0 +1,14 @@
+function [bx, by, Vx, Vy] = get_obstacle(t)
+% Implementation of the stream function for avoiding stationary obstacles
+% using Circle Theorem
+% Reference: https://doi.org/10.1109/ROBOT.2003.1241966
+% Authors: Mitesh Agrawal and Mohammed Tousif Zaman
+% Contact: msagrawal[at]wpi[dot]edu and mzaman[at]wpi[dot]edu
+
+bx_i= 5;
+by_i= 10;
+Vx= 0;
+Vy= -0.10;
+bx= bx_i + Vx*t
+by= by_i + Vy*t
+end
diff --git a/Stream_Function_ODE/obstacle_gpr_model.m b/Stream_Function_ODE/obstacle_gpr_model.m
new file mode 100644
index 0000000..326ae47
--- /dev/null
+++ b/Stream_Function_ODE/obstacle_gpr_model.m
@@ -0,0 +1,65 @@
+
+x0= 5.00;
+y0= 10.00;
+vx0= 0.00;
+vy0= -1.00;
+
+bXtrain= [x0];
+bYtrain= [y0];
+VXtrain= [vx0];
+VYtrain= [vy0];
+Xtrain= [-0.10];
+for t = 0.00:0.10:3.00
+    bx= x0 + vx0*t;
+    by= y0 + vy0*t+ sin(t)*t;
+    vx= vx0;
+    vy= vy0+sin(t);
+    Xtrain =[Xtrain;t];
+    bXtrain= [bXtrain;bx];
+    bYtrain= [bYtrain;by];
+    VXtrain= [VXtrain;vx];
+    VYtrain= [VYtrain;vy];
+    
+    gprMdby = fitrgp(Xtrain,bYtrain,'Basis','linear','FitMethod','exact','PredictMethod','exact');
+    ypred = resubPredict(gprMdby);
+%     figure;
+%     plot(Xtrain,bYtrain,'b.');
+%     hold on;
+%     plot(Xtrain,ypred,'r','LineWidth',1.5);
+%     xlabel('x');
+%     ylabel('y');
+%     legend('Data','GPR predictions');
+%     hold off
+    Xtrain_pred=[t;t+0.1;t+0.2;t+0.3;t+0.4;t+0.5;t+0.6;t+0.7,];
+    [ytestpred] = predict(gprMdby,Xtrain);
+    [ytestpred2] = predict(gprMdby,Xtrain_pred);
+%     figure;
+%     plot(Xtrain,ytestpred,'b');
+%     hold on;
+%     plot(Xtrain_pred,ytestpred2,'g');
+%     hold off
+    gprMdvy = fitrgp(Xtrain,VYtrain,'Basis','linear','FitMethod','exact','PredictMethod','exact');
+    y_vypred = resubPredict(gprMdvy);
+%     figure;
+%     plot(Xtrain,VYtrain,'b.');
+%     hold on;
+%     plot(Xtrain,y_vypred,'r','LineWidth',1.5);
+%     xlabel('x');
+%     ylabel('y');
+%     legend('Data','GPR predictions');
+%     hold off
+    [y_vytestpred,~,y_vytestci] = predict(gprMdvy,Xtrain);
+    [y_vytestpred2,~,y_vytestci2] = predict(gprMdvy,Xtrain_pred);
+    
+    end
+    figure;
+    plot(Xtrain,y_vytestpred,'b');
+    hold on;
+    plot(Xtrain_pred,y_vytestpred2,'g');
+    xlabel('x');
+    ylabel('y');
+    legend('Data','GPR predictions')
+    hold off;    
+    
+    
+    
\ No newline at end of file
diff --git a/Stream_Function_ODE/stream_moving.m b/Stream_Function_ODE/stream_moving.m
new file mode 100644
index 0000000..417d37f
--- /dev/null
+++ b/Stream_Function_ODE/stream_moving.m
@@ -0,0 +1,23 @@
+% Vehicle path planning using stream function for avoiding stationary
+% and moving obstacles using Circle Theorem as described in the below 
+% reference.
+% Reference: https://doi.org/10.1109/ROBOT.2003.1241966
+% Authors: Mitesh Agrawal and Mohammed Tousif Zaman
+% Contact: msagrawal[at]wpi[dot]edu and mzaman[at]wpi[dot]edu
+clc;
+clear;
+start = [8;9];
+A=0;
+bx = 3;
+by = 3;
+a = 2;
+center = [bx by];
+radius = a;
+tf=100;
+nofigure=0;
+[T,X] = ode23(@(t,x) stream_moving_ode(t,x,A),[0 tf],start,A);
+figure('Name','Stream Flow');
+plot(X(:,1), X(:,2),'r-');
+hold on
+
+
diff --git a/Stream_Function_ODE/stream_moving_obstacle.m b/Stream_Function_ODE/stream_moving_obstacle.m
new file mode 100644
index 0000000..02d8bd6
--- /dev/null
+++ b/Stream_Function_ODE/stream_moving_obstacle.m
@@ -0,0 +1,58 @@
+function [u, v, psi] = stream_moving_obstacle()
+% Implementation of the stream function for avoiding single stationary and 
+% a single moving obstacle using Circle Theorem
+% Reference: https://doi.org/10.1109/ROBOT.2003.1241966
+% Authors: Mitesh Agrawal and Mohammed Tousif Zaman
+% Contact: msagrawal[at]wpi[dot]edu and mzaman[at]wpi[dot]edu
+
+clearvars;
+
+syms bx by X Y a Vx Vy C real
+
+% C = 12;
+% Vx = 2;
+% Vy = 3;
+
+% b = bx + i*by defines the velocity of the moving obstacle
+b = bx + by*1i;
+% b_ is the conjugate of b
+b_ = conj(b);
+% complex variables for defining the potential field
+z = X + Y*1i;
+Z = ((a^2/(z-b)) + b_);
+
+% f = -C*log(z);
+% f_ = -C*log(Z);
+% w_stationary = f + f_;
+
+% considering velocity of moving obstacle for the potential field
+g = -Vx*z + Vy*z*1i;
+g_ = -Vx*Z - Vy*Z*1i;
+w_moving = g + g_;
+
+% alternate calculation
+% w = w_stationary + w_moving;
+% psi = imag(w);
+
+% stream function from imaginary component of complex potential for
+% avoiding a single moving obstacle
+psi2 = imag(w_moving);
+
+% definition from single obstacle avoidance case
+% Y1 and X1 - considering single circular shaped obstacle
+Y1 = (((a^2)*(Y-by))/(((X-bx)^2)+((Y-by)^2)))+by;
+X1 = (((a^2)*(X-bx))/(((X-bx)^2)+((Y-by)^2)))+bx;
+
+% stream function from imaginary component of complex potential for
+% avoiding a single stationary obstacle
+psi1 = -C*(atan2(Y,X)) + C*(atan2(Y1,X1));
+
+% stream function from imaginary component of the total complex potential
+% considering both stationary and moving obstacles
+psi = psi1 + psi2;
+
+% velocity components of the stream function
+u = diff(psi,Y);
+v = -1*diff(psi,X);
+
+end
\ No newline at end of file
diff --git a/Stream_Function_ODE/stream_moving_ode.m b/Stream_Function_ODE/stream_moving_ode.m
new file mode 100644
index 0000000..537ffa3
--- /dev/null
+++ b/Stream_Function_ODE/stream_moving_ode.m
@@ -0,0 +1,17 @@
+function [ dx ] = stream_moving_ode(t,x,A)
+dx = zeros(2,1);
+[bx, by, Vx, Vy] = get_obstacle(t)
+a = 2;
+C = 12* sqrt(Vx*Vx + Vy*Vy);
+[U, V, psi] = stream_moving_obstacle();
+X= x(1);
+Y= x(2);
+Vref=0.15;
+U1= eval(U);
+V1= eval(V);
+theta = atan2(V1,U1);
+u = Vref*cos(theta);
+v = Vref*sin(theta);
+dx(1) = u;
+dx(2) = v;
+end
\ No newline at end of file
diff --git a/Stream_Function_ODE/stream_stationary.m b/Stream_Function_ODE/stream_stationary.m
new file mode 100644
index 0000000..fd6ca8f
--- /dev/null
+++ b/Stream_Function_ODE/stream_stationary.m
@@ -0,0 +1,42 @@
+% Vehicle path planning using stream function for avoiding stationary
+% obstacles using Circle Theorem as described in the below reference.
+% Reference: https://doi.org/10.1109/ROBOT.2003.1241966
+% Authors: Mitesh Agrawal and Mohammed Tousif Zaman
+% Contact: msagrawal[at]wpi[dot]edu and mzaman[at]wpi[dot]edu
+
+clc;
+clear;
+close all;
+
+start = [5;5];% initial condition
+
+% A=0; 
+
+bx_i = 3;
+by_i = 3;
+a_i = 1;
+
+center = [bx_i by_i];
+radius = a_i;
+
+tstart = 0;
+tfinal = 80;
+tdelta = 0.1;
+
+tspan = linspace(tstart,tfinal,tdelta);
+nofigure=0;
+
+[T,X] = ode23(@stream_stationary_ode, tspan, start);
+
+% figure('Name','Stream Flow');
+% viscircles(center, radius);
+% hold on;
+% plot(X(:,1), X(:,2),'g-');
+% hold on;
+% xlabel('x');
+% ylabel('y');
+% legend('Trajectory of robot')
+% hold off;
+
+
+
diff --git a/Stream_Function_ODE/stream_stationary_obstacle.m b/Stream_Function_ODE/stream_stationary_obstacle.m
new file mode 100644
index 0000000..4f72cd4
--- /dev/null
+++ b/Stream_Function_ODE/stream_stationary_obstacle.m
@@ -0,0 +1,20 @@
+function [u, v, psi] = stream_stationary_obstacle()
+% Implementation of the stream function for avoiding stationary obstacles
+% using Circle Theorem
+% Reference: https://doi.org/10.1109/ROBOT.2003.1241966
+% Authors: Mitesh Agrawal and Mohammed Tousif Zaman
+% Contact: msagrawal[at]wpi[dot]edu and mzaman[at]wpi[dot]edu
+
+syms bx by X Y a C
+
+% Y1 and X1 - considering circular shaped obstacle
+Y1 = (((a^2)*(Y-by))/(((X-bx)^2)+((Y-by)^2)))+by;
+X1 = (((a^2)*(X-bx))/(((X-bx)^2)+((Y-by)^2)))+bx;
+
+% stream function from imaginary component of complex potential
+psi = -C*(atan2(Y,X)) + C*(atan2(Y1,X1));
+
+% velocity components
+u = diff(psi,Y);
+v = -1*diff(psi,X);
+end
diff --git a/Stream_Function_ODE/stream_stationary_ode.m b/Stream_Function_ODE/stream_stationary_ode.m
new file mode 100644
index 0000000..0e570ba
--- /dev/null
+++ b/Stream_Function_ODE/stream_stationary_ode.m
@@ -0,0 +1,19 @@
+function dxdt = stream_stationary_ode(t,x)
+dxdt = zeros(size(x));
+bx = 3;
+by = 3;
+a = 1;
+C = 1;
+[U, V, psi] = stream_stationary_obstacle();
+X= x(1);
+Y= x(2);
+Vref=0.15;
+U1= eval(U);
+V1= eval(V);
+theta = atan2(V1,U1);
+u = Vref*cos(theta);
+v = Vref*sin(theta);
+dxdt(1) = u;
+dxdt(2) = v;
+end
+
diff --git a/TurtleBot_Impl/assumptions.txt b/TurtleBot_Impl/assumptions.txt
new file mode 100644
index 0000000..01abafc
--- /dev/null
+++ b/TurtleBot_Impl/assumptions.txt
@@ -0,0 +1,33 @@
+## Assumptions
+
+#1 VICON data is received at 100Hz but the control loop runs at much smaller frequency. 
+The state feedback is therefore not that accurate, however the robot and obstacle are not
+moving fast/moving throughout so I think this should be ok.
+
+#2 For a moving obstacle, a constant velocity is considered in the state feedback part and used for the stream function calculations, assuming that the velocity remains constant at all times.
+
+
+## Observations
+
+#1 Actuator delay is ~1s. No matter what frequency (10Hz-100Hz) is set for
+the calculation/control loop, there is no response for the initial ~1s.
+
+#2 The number of loop iterations were set in such a way that for frequency 
+in range (10Hz-100Hz), the total execution time of this calculation/control
+loop would be atleast ~10s of which the first 1s would have no response 
+from the actuators. Post 1s duration, the actuators begin to respond to the
+control values and therefore a smooth movement is observed.
+
+#3 dT or time step has no significance on the actual time elapsed unless it
+is synchronized to the loop execution rate. 
+e.g. 
+tf = 10;
+dt = 0.01;
+r = rosrate(100);
+for t = 0:dt:tf
+
+number of iterations = 10/0.01 = 1000
+time of each iteration = 1/100 = 0.01s
+total execution time = 0.01*1000 = 10s
+
+
diff --git a/TurtleBot_Impl/get_stream_trajectory.m b/TurtleBot_Impl/get_stream_trajectory.m
new file mode 100644
index 0000000..9e7fd98
--- /dev/null
+++ b/TurtleBot_Impl/get_stream_trajectory.m
@@ -0,0 +1,55 @@
+function [ traj ] = get_stream_trajectory(tstep,z,obstacle_type,obstaclepred)
+
+traj= [z;0;0];
+xnew= z(1);
+ynew= z(2);
+% planning for the horizon
+
+%% Run the horizon
+for i= 1:1:2
+    obs_pose = obstaclepred(i,:);
+    bx= obs_pose(1);
+    by= obs_pose(2);
+    Vx= obs_pose(3);
+    Vy= obs_pose(4);
+    % equations from paper
+    [U, V, psi, U_dot, V_dot] = stream_moving_obstacle();
+    X= xnew;
+    Y= ynew;
+
+    % Vref=0.015;
+
+    %% Initialization for stream function
+    a = 2;
+    % Set the source/sink strength based on obstacle type
+    if obstacle_type == 0
+        % Stationary obstacles
+        C = 22;
+    elseif obstacle_type == 1
+        % Moving obstacles
+        C = 12 * sqrt(Vx*Vx + Vy*Vy);
+    end
+
+    % store the robot states using the stream function with
+    % obstacle states given from prediction model
+    U1= eval(U);
+    V1= eval(V);
+    U1_dot= eval(U_dot);
+    V1_dot= eval(V_dot);
+    theta = atan2(V1,U1);
+%     u = Vref*cos(theta);
+%     v = Vref*sin(theta);
+    xnew= xnew + U1*tstep;
+    ynew= ynew + V1*tstep;
+    % linear velocity
+    vstep= U1*cos(theta)+V1*sin(theta);
+    traj(4,i)= vstep;
+    % angular velocity = dOmega = (current_step_theta - last_step_theta)/dt
+    traj(5,i)= (theta-traj(3,i))/tstep;
+    
+    % store the new predicted robot states of the horizon
+    znew= [xnew;ynew;theta;0;0];
+    traj= [traj,znew];
+end
+
+end
\ No newline at end of file
diff --git a/TurtleBot_Impl/get_stream_trajectory_old.m b/TurtleBot_Impl/get_stream_trajectory_old.m
new file mode 100644
index 0000000..6bc5cef
--- /dev/null
+++ b/TurtleBot_Impl/get_stream_trajectory_old.m
@@ -0,0 +1,50 @@
+function [ traj ] = get_stream_trajectory(tstep,z,obstaclepred)
+
+traj= [z;0;0];
+xnew= z(1);
+ynew= z(2);
+% planning for the horizon
+
+%% Initialization for stream function
+a = 2;
+
+%% Run the horizon
+for i= 1:1:7
+    obs_pose = obstaclepred(i,:);
+    bx= obs_pose(1);
+    by= obs_pose(2);
+    Vx= obs_pose(3);
+    Vy= obs_pose(4);
+    % equations from paper
+    
+    % Moving obstacles
+    C = 12 * sqrt(Vx*Vx + Vy*Vy);
+    
+    [U, V, psi, U_dot, V_dot] = stream_moving_obstacle();
+    X= xnew;
+    Y= ynew;
+%     Vref=0.015;
+
+    % store the robot states using the stream function with
+    % obstacle states given from prediction model
+    U1= eval(U);
+    V1= eval(V);
+    U1_dot= eval(U_dot);
+    V1_dot= eval(V_dot);
+    theta = atan2(V1,U1);
+%     u = Vref*cos(theta);
+%     v = Vref*sin(theta);
+    xnew= xnew + U1*tstep;
+    ynew= ynew + V1*tstep;
+    % linear velocity
+    vstep= U1*cos(theta)+V1*sin(theta);
+    traj(4,i)= vstep;
+    % angular velocity = (current_step_theta - last_step_theta)/time
+    traj(5,i)= (theta-traj(3,i))/tstep;
+    
+    % store the new predicted robot states of the horizon
+    znew= [xnew;ynew;theta;0;0];
+    traj= [traj,znew];
+end
+
+end
\ No newline at end of file
diff --git a/TurtleBot_Impl/gpr_based_stream.m b/TurtleBot_Impl/gpr_based_stream.m
new file mode 100644
index 0000000..9895b87
--- /dev/null
+++ b/TurtleBot_Impl/gpr_based_stream.m
@@ -0,0 +1,194 @@
+clc;
+clear;
+close all;
+%initialize
+
+
+% Stream function init block
+tf=100.00;
+tstep = 0.10;
+x0= 9;
+y0= 5;
+theta0= 0;
+bx0= 5.00;
+by0= 5.00;
+vx0= 1.5;
+vy0= -0.55;
+
+% GPR block
+% past obstacle trajectory
+bXtrain= [bx0];
+bYtrain= [by0];
+VXtrain= [vx0];
+VYtrain= [vy0];
+% initialize time simulation from -tstep
+Xtrain= [-0.10];
+% store the robot state
+zinit= [x0;y0;theta0];
+% store the robot state with MPC
+zmpc= [x0;y0;theta0];
+
+figure;
+radius_robot=0.3;
+radius_obs= 1.4;
+
+% MPC block
+% controller params
+N = 7; % length of window
+Q = [1;1;0.5];
+R = [0.1;0.1];
+u_max = [0.47;3.77]; % controller boundary
+warm_start = [0;0;0;0;0;0;0;0;0;0;0;0;0;0];
+
+% calculation of const params
+u_min = -u_max;
+Qhat = diag(repmat(Q,[N,1]));
+Rhat = diag(repmat(R,[N,1]));
+d = [repmat(u_max,[N,1]);repmat(u_min,[N,1])];
+D = [eye(2*N);-eye(2*N)];
+k=1;
+
+% sync the time horizon with ROS 
+for t= 0.00:0.10:tf
+    
+    %% obstacle prediction using GPR
+    
+    % current coordinates and velocity calculation for the obstacle
+    % provide the current state of the obstacle here
+    bx= bx0 + vx0*t; 
+    by= by0 + vy0*t;
+    vx= vx0;
+    vy= vy0;
+    Xtrain =[Xtrain;t];
+    
+    %updating trains for learning
+    % update the X Y positions each time step
+    bXtrain= [bXtrain;bx];
+    bYtrain= [bYtrain;by];
+    VXtrain= [VXtrain;vx];
+    VYtrain= [VYtrain;vy];
+    
+    %creating exact GPR models
+    gprMdbx = fitrgp(Xtrain,bXtrain,'Basis','linear','FitMethod','exact','PredictMethod','exact');
+    gprMdby = fitrgp(Xtrain,bYtrain,'Basis','linear','FitMethod','exact','PredictMethod','exact');
+    gprMdvx = fitrgp(Xtrain,VXtrain,'Basis','linear','FitMethod','exact','PredictMethod','exact');
+    gprMdvy = fitrgp(Xtrain,VYtrain,'Basis','linear','FitMethod','exact','PredictMethod','exact');
+    
+    % create future time step horizon, sync with ROS
+    Xtrain_pred=[t;t+0.1;t+0.2;t+0.3;t+0.4;t+0.5;t+0.6;t+0.7];
+    % prediction from GPR for the horizon
+    [bxtestpred] = predict(gprMdbx,Xtrain_pred);
+    [bytestpred] = predict(gprMdby,Xtrain_pred);
+    [vxtestpred] = predict(gprMdvx,Xtrain_pred);
+    [vytestpred] = predict(gprMdvy,Xtrain_pred);
+    % predicted horizon for obstacle
+    obstaclepred= [bxtestpred,bytestpred,vxtestpred,vytestpred];
+    
+    %% trajectory planning
+    
+    % provide the current state of the robot
+    current_state = [x0;y0;theta0];
+    traj= get_stream_trajectory_old(tstep,current_state,obstaclepred);
+    
+    % get desired robot state from horizon data
+    x0_d= traj(1,2);
+    y0_d= traj(2,2);
+    theta0_d= traj(3,2);
+    
+    z=[x0_d;y0_d;theta0_d];
+    
+    % update robot state
+    zinit=[zinit,z];
+%     figure('Name','Stream Flow');
+%     plot(traj(1,:), traj(2,:),'r-');
+%     hold on
+    if x0*x0 + y0*y0 <= 1
+        break;
+    end
+    
+%     delete(vch);
+%     delete(vch1);
+
+%     vch1 = viscircles([bx,by], radius_obs,'Color','r');
+%     plot(bxtestpred, bytestpred,'r-');
+%     vch = viscircles([x0,y0], radius_robot,'Color','g');
+%     plot(traj(1,:), traj(2,:),'g-');
+%     
+%     hold on;
+%     pause(0.01);
+%     delete(vch);
+%     delete(vch1);
+    
+    
+    
+    %% MPC
+    vr= traj(4,:);
+    qr= traj(1:3,:);
+    
+    % check time step for MPC calculation for turtlebot simulation
+    dt=tstep;
+     % update A and B matrix for the window
+    for i = 1:N
+        A(:,:,i) = [1, 0, -vr(i)*sin(qr(3,i)*dt);
+            0, 1, vr(i)*cos(qr(3,i)*dt)
+            0, 0 ,1];
+        B(:,:,i) = [cos(qr(3,i))*dt, 0;
+            sin(qr(3,i))*dt, 0;
+            0, dt];
+    end
+    
+    
+   % introduce new Ahat(3N,3), Bhat(3N,2N)
+    Ahat = repmat(eye(3,3),N,1);
+    Bhat = repmat(zeros(3,2),N,N);
+    for i = 1:N
+        Bhat(i*3-2:end,i*2-1:i*2) = repmat(B(:,:,i),N+1-i,1);
+        for j = i:N
+            Ahat(j*3-2:j*3,:) = A(:,:,i)*Ahat(j*3-2:j*3,:);
+            for m = i+1:j
+                Bhat(j*3-2:j*3,i*2-1:i*2) = A(:,:,m)*Bhat(j*3-2:j*3,i*2-1:i*2);
+            end
+        end
+    end
+
+    
+    ehat= [0.00001;0.00001;0.00001];
+    H = 2*(Bhat'*Qhat*Bhat+Rhat);
+    f = 2*Bhat'*Qhat*Ahat*ehat;
+    OPTIONS = optimset('Display','off','MaxIter',5);
+%     OPTIONS = optimset('Display','off');
+    q_op = quadprog(H,f,[],[],[],[],[],[],warm_start,OPTIONS)
+    
+    warm_start = q_op;
+    
+    % take velocity from stream function and use current theta for next
+    % step calculations
+    x0= x0+(traj(4,1)+q_op(1))*cos(theta0)*dt;
+    y0= y0+(traj(4,1)+q_op(1))*sin(theta0)*dt;
+    theta0 = theta0+(traj(5,1)+q_op(2))*dt;
+    % final robot state calculated using Stream function and MPC
+    zf=[x0;y0;theta0];
+    % total robot state/trajectory using MPC
+    zmpc=[zmpc,zf];
+    
+    xlim([0 10]);
+    ylim([0 10]);
+    vch1 = viscircles([bx,by], radius_obs,'Color','r');
+    plot(bxtestpred, bytestpred,'r-');
+    vch = viscircles([x0,y0], radius_robot,'Color','g');
+    plot(traj(1,:), traj(2,:),'g-');
+    
+    hold on;
+    pause(0.01);
+    delete(vch);
+    delete(vch1);
+ 
+end
+    figure('Name','Stream Flow');
+    plot(zinit(1,:), zinit(2,:),'r-');
+    hold on
+    plot(zmpc(1,:), zmpc(2,:),'b-');
+    hold on
+    plot(bXtrain, bYtrain,'r-');
+    hold on
+
diff --git a/TurtleBot_Impl/gpr_based_stream_turtlebot.m b/TurtleBot_Impl/gpr_based_stream_turtlebot.m
new file mode 100644
index 0000000..fdf6d44
--- /dev/null
+++ b/TurtleBot_Impl/gpr_based_stream_turtlebot.m
@@ -0,0 +1,367 @@
+%% DO THIS BEFORE RUNNING THIS SCRIPT
+% Open a terminal on PC and run the below command to launch VICON at the IP
+% address specified (cross-check the IP of the system running VICON)
+
+% roslaunch vicon_bridge vicon.launch ip:= 130.215.206.243
+
+%% Initialize
+clc;
+clear;
+close all;
+
+%% Select ROS Connectivity
+% true : Enable ROS features
+% false : Disable ROS features
+enable_ros = true;
+
+%% Set IP address, initialize ROS and connect to desired ROS nodes
+if enable_ros == true
+    % create a ROS MASTER
+    % master = robotics.ros.Core; % use if master_uri is same as ros_ip
+    ROS_IP = '130.215.206.232';
+    ROS_MASTER_URI = 'http://130.215.121.108:11311'; % URI of TurtleBot
+    % Set ROS environment variables and ROS MASTER at specified IP address
+    initialize_ros(ROS_IP, ROS_MASTER_URI);
+    
+end
+    
+%% Select obstacle type 
+% 0 : Stationary Obstacles 
+% 1 : Moving Obstacles
+obstacle_type = 0;
+
+%% Select state feedback type
+% 0 : Fixed positions
+% 1 : VICON motion capture system
+%% Hard coded for testing
+state_fb = 1;
+% if enable_ros == true
+%     state_fb = 1;
+% else
+%     state_fb = 0;
+% end
+
+if state_fb == 0    
+    % Set initial robot state
+    x0= 4;
+    y0= 4;
+    theta0= 0;
+    % Set initial obstacle state
+    bx0= 1.1;
+    by0= 1.2;
+elseif state_fb == 1
+    % Subscribe to the VICON topic to get TurtleBot state feedback
+    turtlebot_sub = rossubscriber('/vicon/turtlebot_traj_track/turtlebot_traj_track');
+    
+    %% Provide the initial/starting state of robot
+    % Get x0 y0 theta0 of TurtleBot
+    turtlebot_pose_data = receive(turtlebot_sub,3); % timeout of 3s
+    x0 = turtlebot_pose_data.Transform.Translation.X;
+    y0 = turtlebot_pose_data.Transform.Translation.Y;
+
+    quatW = turtlebot_pose_data.Transform.Rotation.W;
+    quatX = turtlebot_pose_data.Transform.Rotation.X;
+    quatY = turtlebot_pose_data.Transform.Rotation.Y;
+    quatZ = turtlebot_pose_data.Transform.Rotation.Z;
+
+    angles = quat2eul([quatW quatX quatY quatZ]); % Euler ZYX
+    theta0 = rad2deg(angles(1));
+    
+    % Subscribe to the VICON topic to get Stationary Obstacle state feedback
+    obstacle_sub = rossubscriber('/vicon/stationary_obs_track/stationary_obs_track');
+    
+    %% Provide the initial/starting state of obstacle
+    % Get bx0 by0 of Obstacle
+    obstacle_pose_data = receive(obstacle_sub,3); % timeout of 3s
+    bx0 = obstacle_pose_data.Transform.Translation.X;
+    by0 = obstacle_pose_data.Transform.Translation.Y;
+    
+end
+
+if obstacle_type == 0
+    % Stationary obstacles have zero velocity
+    vx0 = 0;
+    vy0 = 0;
+elseif obstacle_type == 1
+    % Moving obstacles have non-zero velocity    
+    vx0= 1.5;
+    vy0= -0.55;
+end
+
+%% GPR block
+% past obstacle trajectory
+bXtrain= [bx0];
+bYtrain= [by0];
+VXtrain= [vx0];
+VYtrain= [vy0];
+% initialize time simulation from -tstep
+Xtrain= [-0.10];
+% store the robot state
+zinit= [x0;y0;theta0];
+% store the robot state with MPC
+zmpc= [x0;y0;theta0];
+
+%% Update radii of the robot and obstacle
+if enable_ros == true
+    % Physical dimensions of TurtleBot and obstacle
+    radius_robot = 0.177;   % diameter = 354mm
+    radius_obs = 0.109;    % diameter = 218mm
+else
+    % Dummy dimensions for simulation plots
+    radius_robot = 0.3;
+    radius_obs = 1.4;
+end
+
+%% MPC block
+
+% controller params
+N = 7; % length of window
+Q = [1;1;1];
+R = [1;1];
+% actuator constraints
+u_max = [0.7;180]; % controller boundary; v = 0.7m/s, w = 180deg/s
+warm_start = [0;0;0;0;0;0;0;0;0;0;0;0;0;0];
+
+% calculation of const params
+u_min = -u_max;
+Qhat = diag(repmat(Q,[N,1]));
+Rhat = diag(repmat(R,[N,1]));
+d = [repmat(u_max,[N,1]);repmat(u_min,[N,1])];
+D = [eye(2*N);-eye(2*N)];
+k=1;
+
+%% Initialize simulation time and horizon time step
+tf = 10;
+tstep = 0.1; % For a loop rate = 10Hz
+
+%% Sync the time horizon with ROS
+% r = robotics.ros.Rate(node,10);
+r = rosrate(10);
+tic;
+reset(r)
+
+% Run the simulation for the simulation time with tstep increments
+for t = 0:tstep:tf
+    %% Obstacle prediction using GPR
+    
+    % current coordinates and velocity calculation for the obstacle
+    % assuming the velocity of the obstacles are constant
+    % current velocity of obstacle = initial velocity of obstacle
+    % provide the current state of the obstacle here
+    obstacle_curr_state = receive(obstacle_sub,3); % timeout of 3s
+    bx0_curr = obstacle_curr_state.Transform.Translation.X;
+    by0_curr = obstacle_curr_state.Transform.Translation.Y;
+    
+    bx= bx0_curr + vx0*t; 
+    by= by0_curr + vy0*t;
+    vx= vx0;
+    vy= vy0;
+    Xtrain =[Xtrain;t];
+    
+    % updating trains for learning
+    % update the X Y positions each time step
+    bXtrain= [bXtrain;bx];
+    bYtrain= [bYtrain;by];
+    VXtrain= [VXtrain;vx];
+    VYtrain= [VYtrain;vy];
+    
+    %creating exact GPR models
+    gprMdbx = fitrgp(Xtrain,bXtrain,'Basis','linear','FitMethod','exact','PredictMethod','exact');
+    gprMdby = fitrgp(Xtrain,bYtrain,'Basis','linear','FitMethod','exact','PredictMethod','exact');
+    gprMdvx = fitrgp(Xtrain,VXtrain,'Basis','linear','FitMethod','exact','PredictMethod','exact');
+    gprMdvy = fitrgp(Xtrain,VYtrain,'Basis','linear','FitMethod','exact','PredictMethod','exact');
+    
+    %% create future time step horizon, sync with ROS
+%     Xtrain_pred=[t;t+0.1;t+0.2;t+0.3;t+0.4;t+0.5;t+0.6;t+0.7];
+    
+%   testing
+    Xtrain_pred=[t;t+0.1];
+%   testing
+
+    % prediction from GPR for the horizon
+    [bxtestpred] = predict(gprMdbx,Xtrain_pred);
+    [bytestpred] = predict(gprMdby,Xtrain_pred);
+    [vxtestpred] = predict(gprMdvx,Xtrain_pred);
+    [vytestpred] = predict(gprMdvy,Xtrain_pred);
+    % predicted horizon for obstacle
+    obstaclepred= [bxtestpred,bytestpred,vxtestpred,vytestpred];
+    
+    %% trajectory planning
+    
+    %% provide the current state of the robot
+    turtlebot_curr_state = receive(turtlebot_sub,3); % timeout of 3s
+    x0_curr = turtlebot_curr_state.Transform.Translation.X;
+    y0_curr = turtlebot_curr_state.Transform.Translation.Y;
+
+    quatW_curr = turtlebot_curr_state.Transform.Rotation.W;
+    quatX_curr = turtlebot_curr_state.Transform.Rotation.X;
+    quatY_curr = turtlebot_curr_state.Transform.Rotation.Y;
+    quatZ_curr = turtlebot_curr_state.Transform.Rotation.Z;
+    
+    % Euler ZYX
+    angles_curr = quat2eul([quatW_curr quatX_curr quatY_curr quatZ_curr]);
+    theta0_curr = rad2deg(angles_curr(1));
+    
+    current_state = [x0_curr;y0_curr;theta0_curr];
+    traj= get_stream_trajectory(tstep,current_state,obstacle_type,obstaclepred);
+    
+    % get desired robot state from horizon data
+    x0_d= traj(1,2);
+    y0_d= traj(2,2);
+    theta0_d= traj(3,2);
+    
+    z=[x0_d;y0_d;theta0_d];
+    
+    % update robot state
+    zinit=[zinit,z];
+    % figure('Name','Stream Flow');
+    % plot(traj(1,:), traj(2,:),'r-');
+    % hold on
+    
+    %% Stop when robot is found very close to origin
+    dist_from_origin = sqrt(x0_curr*x0_curr + y0_curr*y0_curr);
+    if dist_from_origin <= 1
+        break;
+    end
+    
+%     delete(vch);
+%     delete(vch1);
+
+%     vch1 = viscircles([bx,by], radius_obs,'Color','r');
+%     plot(bxtestpred, bytestpred,'r-');
+%     vch = viscircles([x0,y0], radius_robot,'Color','g');
+%     plot(traj(1,:), traj(2,:),'g-');
+%     
+%     hold on;
+%     pause(0.01);
+%     delete(vch);
+%     delete(vch1);
+    
+    
+%     
+%     %% MPC
+%     % get the velocity and state values from stream function
+%     vr= traj(4,:);
+%     qr= traj(1:3,:);
+%     
+%     % check time step for MPC calculation for turtlebot simulation
+%     dt=tstep;
+%     % update A and B matrix for the window
+%     for i = 1:N
+%         A(:,:,i) = [1, 0, -vr(i)*sin(qr(3,i)*dt);
+%             0, 1, vr(i)*cos(qr(3,i)*dt)
+%             0, 0 ,1];
+%         B(:,:,i) = [cos(qr(3,i))*dt, 0;
+%             sin(qr(3,i))*dt, 0;
+%             0, dt];
+%     end
+%     
+%     
+%    % introduce new Ahat(3N,3), Bhat(3N,2N)
+%     Ahat = repmat(eye(3,3),N,1);
+%     Bhat = repmat(zeros(3,2),N,N);
+%     for i = 1:N
+%         Bhat(i*3-2:end,i*2-1:i*2) = repmat(B(:,:,i),N+1-i,1);
+%         for j = i:N
+%             Ahat(j*3-2:j*3,:) = A(:,:,i)*Ahat(j*3-2:j*3,:);
+%             for m = i+1:j
+%                 Bhat(j*3-2:j*3,i*2-1:i*2) = A(:,:,m)*Bhat(j*3-2:j*3,i*2-1:i*2);
+%             end
+%         end
+%     end
+% 
+%     % Optimization
+%     % ehat= [0.00001;0.00001;0.00001];
+%     ehat = [x0_curr-x0_d; y0_curr-y0_d; theta0_curr-theta0_d];
+%     H = 2*(Bhat'*Qhat*Bhat+Rhat);
+%     f = 2*Bhat'*Qhat*Ahat*ehat;
+%     OPTIONS = optimset('Display','off','MaxIter',5);
+%     % OPTIONS = optimset('Display','off');
+%     q_op = quadprog(H,f,[],[],[],[],[],[],warm_start,OPTIONS);
+%     
+%     warm_start = q_op;
+    
+%   testing
+    dt=tstep;
+    q_op(1) = 0;
+    q_op(2) = 0;
+%   testing
+    
+    % take velocity from stream function 
+    % use current values of theta for next step calculations ?
+    %% theta0_curr OR traj(3,1) % traj(3,1) = theta0_curr in code
+    x0_curr = x0_curr + (traj(4,1)+q_op(1))*cos(theta0_curr)*dt; 
+    y0_curr = y0_curr + (traj(4,1)+q_op(1))*sin(theta0_curr)*dt;
+    theta0_curr = theta0_curr + (traj(5,1)+q_op(2))*dt;
+    
+    %% Send control inputs to TurtleBot
+    % Control inputs for TurtleBot
+    velocityX = (traj(4,1)+q_op(1))*cos(theta0_curr);
+    velocityY = (traj(4,1)+q_op(1))*sin(theta0_curr);
+    omegaZ = traj(5,1)+q_op(2);
+    
+    if enable_ros == true
+        % Create a ROS publisher and message for velocity topic
+        my_turtlebot = rospublisher('/mobile_base/commands/velocity');
+        velmsg = rosmessage(my_turtlebot);
+
+        % Velocity in X and Y axes
+        % Velocity is limited to safe values
+        %velmsg.Linear.X = limiter_min_max(velocityX, -0.7, 0.7); % 0.7m/s
+        %velmsg.Linear.Y = limiter_min_max(velocityY, -0.7, 0.7); % 0.7m/s
+
+        velmsg.Linear.X = velocityX;
+        velmsg.Linear.Y = velocityY;
+        
+        % Steer about Z-axis
+        %velmsg.Angular.Z = limiter_min_max(omegaZ, -180, 180); % 180deg/s
+        velmsg.Angular.Z = omegaZ;
+        
+        % Publish velocity and steer to the TurtleBot
+        send(my_turtlebot, velmsg);
+        
+        %% Wait the desired Hz time for the actuators to react
+        waitfor(r);
+    end
+    
+    if enable_ros == false
+        %% Final robot state calculated by Stream function and corrected by MPC
+        zf=[x0_curr;y0_curr;theta0_curr];
+        % total robot state/trajectory using MPC
+        zmpc=[zmpc,zf];
+
+        xlim([0 10]);
+        ylim([0 10]);
+
+        vch1 = viscircles([bx,by], radius_obs,'Color','r');
+        %figure('Name','Realtime trajectory Plot of Robot and Obstacle');
+        plot(bxtestpred, bytestpred,'r-');
+        vch = viscircles([x0_curr,y0_curr], radius_robot,'Color','g');
+        plot(traj(1,:), traj(2,:),'g-');
+        legend({'Robot','Obstacle'})
+
+        hold on;
+        pause(0.01);
+        delete(vch);
+        delete(vch1);
+    end
+ 
+end
+toc;
+
+if enable_ros == false
+    %% Plot what is required
+    figure('Name','Trajectory Plot of Robot and Obstacle');
+    plot(zinit(1,:), zinit(2,:),'b-');
+    hold on
+    plot(zmpc(1,:), zmpc(2,:),'g-');
+    hold on
+    plot(bXtrain, bYtrain,'r-');
+    hold on
+    legend({'Robot trajectory from Stream Function','Robot trajectory using MPC','Obstacle trajectory using GPR'},'Location','southeast')
+end
+
+%% Terminate ROS connectivity
+if enable_ros == true
+    terminate_ros();
+end
\ No newline at end of file
diff --git a/TurtleBot_Impl/gpr_based_stream_turtlebot_sf_obs.m b/TurtleBot_Impl/gpr_based_stream_turtlebot_sf_obs.m
new file mode 100644
index 0000000..d8d3c69
--- /dev/null
+++ b/TurtleBot_Impl/gpr_based_stream_turtlebot_sf_obs.m
@@ -0,0 +1,485 @@
+%% DO THIS BEFORE RUNNING THIS SCRIPT
+% Open a terminal on TurtleBot and run the below command to launch VICON 
+% at the IP address specified (cross-check the IP of the system running 
+% VICON)
+
+% roslaunch vicon_bridge vicon.launch ip:=130.215.206.243
+
+%% Initialize
+clc;
+clear;
+close all;
+
+%% Select ROS Connectivity
+% true : Enable ROS features
+% false : Disable ROS features
+enable_ros = true;
+
+%% Terminate previous ROS connectivity, if any
+if enable_ros == true
+    terminate_ros();
+end
+
+%% Set IP address, initialize ROS and connect to desired ROS nodes
+if enable_ros == true
+    % create a ROS MASTER
+    % master = robotics.ros.Core; % use if master_uri is same as ros_ip
+%     ROS_IP = '130.215.206.232';
+%     ROS_MASTER_URI = 'http://130.215.223.68:11311'; % URI of TurtleBot
+    % Set ROS environment variables and ROS MASTER at specified IP address
+%     initialize_ros(ROS_IP, ROS_MASTER_URI);
+
+    % Set ROS_IP and ROS_MASTER_URI
+    setenv('ROS_IP','130.215.206.232')
+    setenv('ROS_MASTER_URI','http://130.215.171.227:11311')
+
+    % Initialize ROS and connect to TurtleBot
+    ip_turtlebot = '130.215.171.227';
+    rosinit(ip_turtlebot)
+    
+end
+
+%% Select obstacle type 
+% 0 : Stationary Obstacles 
+% 1 : Moving Obstacles
+obstacle_type = 0;
+
+%% Select state feedback type
+% 0 : Fixed positions for both robot and obstacle
+% 1 : VICON motion capture system gives feedback for both robot and
+% obstacle
+% 2 : VICON motion capture system gives feedback for the obstacle
+%% Hard coded for testing
+state_fb = 1;
+
+% if enable_ros == true
+%     state_fb = 1;
+%     if obs_fb_alone == true
+%         state_fb = 2;
+% else
+%     state_fb = 0;
+% end
+
+if state_fb == 0    
+    % Set initial robot state
+    x0= 4;
+    y0= 4;
+    theta0= 0;
+    % Set initial obstacle state
+    bx0= 1.1;
+    by0= 1.2;
+elseif state_fb == 1
+    % Subscribe to the VICON topic to get TurtleBot state feedback
+    turtlebot_sub = rossubscriber('/vicon/turtlebot_traj_track/turtlebot_traj_track1');
+    
+    %% Provide the initial/starting state of robot
+    % Get x0 y0 theta0 of TurtleBot
+    turtlebot_pose_data = receive(turtlebot_sub,3); % timeout of 3s
+    x0 = turtlebot_pose_data.Transform.Translation.X;
+    y0 = turtlebot_pose_data.Transform.Translation.Y;
+
+    quatW = turtlebot_pose_data.Transform.Rotation.W;
+    quatX = turtlebot_pose_data.Transform.Rotation.X;
+    quatY = turtlebot_pose_data.Transform.Rotation.Y;
+    quatZ = turtlebot_pose_data.Transform.Rotation.Z;
+
+    angles = quat2eul([quatW quatX quatY quatZ]); % Euler ZYX
+%     theta0 = rad2deg(angles(1));
+    theta0 = angles(1);
+    
+    % Subscribe to the VICON topic to get Stationary Obstacle state feedback
+    obstacle_sub = rossubscriber('/vicon/stationary_obs_track/stationary_obs_track');
+    
+    %% Provide the initial/starting state of obstacle
+    % Get bx0 by0 of Obstacle
+    obstacle_pose_data = receive(obstacle_sub,3); % timeout of 3s
+    bx0 = obstacle_pose_data.Transform.Translation.X;
+    by0 = obstacle_pose_data.Transform.Translation.Y;
+    
+elseif state_fb == 2
+    % Subscribe to the VICON topic to get Stationary Obstacle state feedback
+    obstacle_sub = rossubscriber('/vicon/stationary_obs_track/stationary_obs_track');
+    
+    %% Provide the initial/starting state of obstacle
+    % Get bx0 by0 of Obstacle
+    obstacle_pose_data = receive(obstacle_sub,3); % timeout of 3s
+    bx0 = obstacle_pose_data.Transform.Translation.X;
+    by0 = obstacle_pose_data.Transform.Translation.Y;
+    
+    
+    % Set initial robot state to fixed values
+    x0= 4;
+    y0= 4;
+    theta0= 0;
+end
+
+if obstacle_type == 0
+    % Stationary obstacles have zero velocity
+    vx0 = 0;
+    vy0 = 0;
+elseif obstacle_type == 1
+    % Moving obstacles have non-zero velocity    
+    vx0= 1.5;
+    vy0= -0.55;
+end
+
+%% GPR block
+% past obstacle trajectory
+bXtrain= [bx0];
+bYtrain= [by0];
+VXtrain= [vx0];
+VYtrain= [vy0];
+% initialize time simulation from -tstep
+Xtrain= [-0.10];
+%% added one line below
+% obstaclepred=[bx0,by0,vx0,vy0];
+% store the robot state
+zinit= [x0;y0;theta0];
+% store the robot state with MPC
+zmpc= [x0;y0;theta0];
+
+%% Update radii of the robot and obstacle
+if enable_ros == true
+    % Physical dimensions of TurtleBot and obstacle
+    radius_robot = 0.177;   % diameter = 354mm
+    radius_obs = 0.109;    % diameter = 218mm
+else
+    % Dummy dimensions for simulation plots
+%     radius_robot = 0.3;
+%     radius_obs = 1.4;
+
+    % Physical dimensions of TurtleBot and obstacle
+    radius_robot = 0.177;   % diameter = 354mm
+    radius_obs = 0.109;    % diameter = 218mm
+
+end
+
+%% MPC block
+
+% controller params
+N = 7; % length of window
+Q = [1;1;1];
+R = [1;1];
+% actuator constraints
+% u_max = [0.7;180]; % controller boundary; v = 0.7m/s, w = 180deg/s = pi
+% rad/s
+u_max = [1;pi]; % v = 1m/s, w = pi rad/s
+warm_start = [0;0;0;0;0;0;0;0;0;0;0;0;0;0];
+
+% calculation of const params
+u_min = -u_max;
+Qhat = diag(repmat(Q,[N,1]));
+Rhat = diag(repmat(R,[N,1]));
+d = [repmat(u_max,[N,1]);repmat(u_min,[N,1])];
+D = [eye(2*N);-eye(2*N)];
+k=1;
+
+%% Initialize simulation time and horizon time step
+tf = 10;
+tstep = 1; % For a loop rate = 10Hz
+
+%% Sync the time horizon with ROS
+% r = robotics.ros.Rate(node,10);
+r = rosrate(1);
+tic;
+reset(r)
+pause on
+% Run the simulation for the simulation time with tstep increments
+for t = 0:tstep:tf
+    %% Obstacle prediction using GPR
+    
+    % current coordinates and velocity calculation for the obstacle
+    % assuming the velocity of the obstacles are constant
+    % current velocity of obstacle = initial velocity of obstacle
+    % provide the current state of the obstacle here
+    obstacle_curr_state = receive(obstacle_sub,3); % timeout of 3s
+    bx0_curr = obstacle_curr_state.Transform.Translation.X;
+    by0_curr = obstacle_curr_state.Transform.Translation.Y;
+    
+    
+%     bx= bx0_curr + vx0*t; 
+%     by= by0_curr + vy0*t;
+    
+    vx= vx0;
+    vy= vy0;
+    Xtrain =[Xtrain;t];
+    
+    % updating trains for learning
+    % update the X Y positions each time step
+%     bXtrain= [bXtrain;bx];
+%     bYtrain= [bYtrain;by];
+
+    bXtrain= [bXtrain;bx0_curr];
+    bYtrain= [bYtrain;by0_curr];
+    VXtrain= [VXtrain;vx];
+    VYtrain= [VYtrain;vy];
+    
+    %creating exact GPR models
+    gprMdbx = fitrgp(Xtrain,bXtrain,'Basis','linear','FitMethod','exact','PredictMethod','exact');
+    gprMdby = fitrgp(Xtrain,bYtrain,'Basis','linear','FitMethod','exact','PredictMethod','exact');
+    gprMdvx = fitrgp(Xtrain,VXtrain,'Basis','linear','FitMethod','exact','PredictMethod','exact');
+    gprMdvy = fitrgp(Xtrain,VYtrain,'Basis','linear','FitMethod','exact','PredictMethod','exact');
+    
+    %% create future time step horizon, sync with ROS
+    Xtrain_pred=[t;t+0.1;t+0.2;t+0.3;t+0.4;t+0.5;t+0.6;t+0.7];
+    
+%   testing
+%     Xtrain_pred=[t;t+0.1];
+%   testing
+
+    % prediction from GPR for the horizon
+    [bxtestpred] = predict(gprMdbx,Xtrain_pred);
+    [bytestpred] = predict(gprMdby,Xtrain_pred);
+    [vxtestpred] = predict(gprMdvx,Xtrain_pred);
+    [vytestpred] = predict(gprMdvy,Xtrain_pred);
+    % predicted horizon for obstacle
+    obstaclepred = [bxtestpred,bytestpred,vxtestpred,vytestpred];
+%     obstaclepred = [obstaclepred;bxtestpred,bytestpred,vxtestpred,vytestpred];
+    %% trajectory planning
+    
+    %% provide the current state of the robot
+    turtlebot_curr_state = receive(turtlebot_sub,3); % timeout of 3s
+    x0_curr = turtlebot_curr_state.Transform.Translation.X;
+    y0_curr = turtlebot_curr_state.Transform.Translation.Y;
+
+    quatW_curr = turtlebot_curr_state.Transform.Rotation.W;
+    quatX_curr = turtlebot_curr_state.Transform.Rotation.X;
+    quatY_curr = turtlebot_curr_state.Transform.Rotation.Y;
+    quatZ_curr = turtlebot_curr_state.Transform.Rotation.Z;
+    
+    % Euler ZYX
+    angles_curr = quat2eul([quatW_curr quatX_curr quatY_curr quatZ_curr]);
+    theta0_curr = angles_curr(1);
+%     theta0_curr = rad2deg(angles_curr(1));
+    
+    current_state = [x0_curr;y0_curr;theta0_curr];
+    traj= get_stream_trajectory(tstep,current_state,obstacle_type,obstaclepred);
+
+    % current state of robot from initial values -- testing
+%     current_state = [x0;y0;theta0];
+%     traj= get_stream_trajectory(tstep,current_state,obstacle_type,obstaclepred);
+
+    % get desired robot state from horizon data
+    x0_d= traj(1,2);
+    y0_d= traj(2,2);
+    theta0_d= traj(3,2);
+    
+    z=[x0_d;y0_d;theta0_d];
+    
+    % update robot state
+    zinit=[zinit,z];
+    % figure('Name','Stream Flow');
+    % plot(traj(1,:), traj(2,:),'r-');
+    % hold on
+    
+    %% Stop when robot is found very close to origin
+%     dist_from_origin = sqrt(x0_curr*x0_curr + y0_curr*y0_curr);
+    
+    dist_from_origin = sqrt(x0*x0 + y0*y0);
+    if dist_from_origin <= 1
+        break;
+    end
+    
+%     delete(vch);
+%     delete(vch1);
+
+%     vch1 = viscircles([bx,by], radius_obs,'Color','r');
+%     plot(bxtestpred, bytestpred,'r-');
+%     vch = viscircles([x0,y0], radius_robot,'Color','g');
+%     plot(traj(1,:), traj(2,:),'g-');
+%     
+%     hold on;
+%     pause(0.01);
+%     delete(vch);
+%     delete(vch1);
+    
+    
+    
+    %% MPC
+    % get the velocity and state values from stream function
+    vr= traj(4,:);
+    qr= traj(1:3,:);
+    
+    % check time step for MPC calculation for turtlebot simulation
+    dt=tstep;
+    % update A and B matrix for the window
+    for i = 1:N
+        A(:,:,i) = [1, 0, -vr(i)*sin(qr(3,i)*dt);
+            0, 1, vr(i)*cos(qr(3,i)*dt);
+            0, 0 ,1];
+        B(:,:,i) = [cos(qr(3,i))*dt, 0;
+            sin(qr(3,i))*dt, 0;
+            0, dt];
+    end
+    
+    
+   % introduce new Ahat(3N,3), Bhat(3N,2N)
+    Ahat = repmat(eye(3,3),N,1);
+    Bhat = repmat(zeros(3,2),N,N);
+    for i = 1:N
+        Bhat(i*3-2:end,i*2-1:i*2) = repmat(B(:,:,i),N+1-i,1);
+        for j = i:N
+            Ahat(j*3-2:j*3,:) = A(:,:,i)*Ahat(j*3-2:j*3,:);
+            for m = i+1:j
+                Bhat(j*3-2:j*3,i*2-1:i*2) = A(:,:,m)*Bhat(j*3-2:j*3,i*2-1:i*2);
+            end
+        end
+    end
+
+    % Optimization
+    ehat= [0.00001;0.00001;0.00001];
+    % error = current - desired
+%     ehat = [x0_curr-x0_d; y0_curr-y0_d; theta0_curr-theta0_d];
+    H = 2*(Bhat'*Qhat*Bhat+Rhat);
+    f = 2*Bhat'*Qhat*Ahat*ehat;
+    OPTIONS = optimset('Display','off','MaxIter',5);
+    % OPTIONS = optimset('Display','off');
+    q_op = quadprog(H,f,[],[],[],[],[],[],warm_start,OPTIONS);
+    
+    warm_start = q_op;
+    
+%   testing
+    dt=tstep;
+%   testing
+%     q_op(1) = 0;
+%     q_op(2) = 0;
+    
+    % take velocity from stream function 
+    % use current values of theta for next step calculations ?
+    %% theta0_curr OR traj(3,1) % traj(3,1) = theta0_curr in code
+%     x0 = x0 + (traj(4,1)+q_op(1))*cos(theta0)*dt; 
+%     y0 = y0 + (traj(4,1)+q_op(1))*sin(theta0)*dt;
+%     theta0 = theta0 + (traj(5,1)+q_op(2))*dt;
+    
+    %% Send control inputs to TurtleBot
+    % Control inputs for TurtleBot
+    velocityX = (traj(4,1)+q_op(1))*cos(theta0);
+    velocityY = (traj(4,1)+q_op(1))*sin(theta0);
+    omegaZ = traj(5,1)+q_op(2);
+%     omegaZ_rad = deg2rad(omegaZ);
+    
+    velocityLinTot = sqrt(velocityX^2 + velocityY^2);
+    
+    %% Velocity Limitation Status
+    if abs(velocityLinTot) > 1
+        velocity_limit_reqd = true;
+        disp('Velocity limitation active')
+    else
+        velocity_limit_reqd = false;
+        disp('Velocity limitation not active')
+    end
+    
+    
+    if enable_ros == true
+        % Create a ROS publisher and message for velocity topic
+        my_turtlebot = rospublisher('/mobile_base/commands/velocity');
+        velmsg = rosmessage(my_turtlebot);
+
+%         velmsg = rosmessage('geometry_msgs/Twist');
+%       my_turtlebot = rospublisher('/mobile_base/commands/velocity','geometry_msgs/Twist');
+        
+        % Velocity in X and Y axes
+        % Velocity is limited to safe values
+        %velmsg.Linear.X = limiter_min_max(velocityX, -0.7, 0.7); % 0.7m/s
+        %velmsg.Linear.Y = limiter_min_max(velocityY, -0.7, 0.7); % 0.7m/s
+        %velmsg.Linear.X = velocityX;
+        %velmsg.Linear.Y = velocityY;
+        
+        %% Select and apply velocity limitation loop
+        
+        if velocity_limit_reqd == true
+            
+            %% Velocity Limitation Block
+            % Initialize the limits
+            OldMin = min(nonzeros(traj(4,:)));
+            OldMax = max(nonzeros(traj(4,:)));
+            NewMin = 0;
+            NewMax = 1;
+            row = 1;
+            
+            for OldValue = traj(4,:)    
+
+                if (OldMin ~= OldMax && NewMin ~= NewMax)
+                    NewValue = ((((OldValue - OldMin) * (NewMax - NewMin)) / (OldMax - OldMin)) + NewMin);
+                else
+                    NewValue = (NewMax + NewMin) / 2;
+                end
+
+                VelList(row,:) = NewValue;
+                % Apply the new velocity
+                velmsg.Linear.X = NewValue;
+                row = row + 1;
+
+                % ----
+
+                % Total Linear Velocity
+                % velmsg.Linear.X = velocityLinTot;
+                velmsg.Linear.Y = 0;
+                % Steer about Z-axis
+                %velmsg.Angular.Z = limiter_min_max(omegaZ, -180, 180); % 180deg/s
+%                 velmsg.Angular.Z = omegaZ_rad; % calculation in deg but converted to radians
+                velmsg.Angular.Z = omegaZ; % calculation is in radians
+                % Publish velocity and steer to the TurtleBot
+                send(my_turtlebot, velmsg);
+
+                %% Wait the desired Hz time for the actuators to react
+                waitfor(r);
+
+                % ----
+
+            end
+            
+        else
+            % Total Linear Velocity
+            velmsg.Linear.X = velocityLinTot;
+            velmsg.Linear.Y = 0;
+            % Steer about Z-axis
+            %velmsg.Angular.Z = limiter_min_max(omegaZ, -180, 180); % 180deg/s
+%             velmsg.Angular.Z = omegaZ_rad; % calculation in deg but converted to radians
+            velmsg.Angular.Z = omegaZ; % calculation is in radians
+            % Publish velocity and steer to the TurtleBot
+            send(my_turtlebot, velmsg);
+
+            %% Wait the desired Hz time for the actuators to react
+            waitfor(r);
+            
+        end
+            
+    end
+    
+    if enable_ros == false
+        %% Final robot state calculated by Stream function and corrected by MPC
+        zf=[x0;y0;theta0];
+        % total robot state/trajectory using MPC
+        zmpc=[zmpc,zf];
+
+        xlim([0 10]);
+        ylim([0 10]);
+
+        vch1 = viscircles([bx,by], radius_obs,'Color','r');
+        %figure('Name','Realtime trajectory Plot of Robot and Obstacle');
+        plot(bxtestpred, bytestpred,'r-');
+        vch = viscircles([x0,y0], radius_robot,'Color','g');
+        plot(traj(1,:), traj(2,:),'g-');
+        legend({'Robot','Obstacle'})
+
+        hold on;
+        pause(0.01);
+        delete(vch);
+        delete(vch1);
+    end
+ 
+end
+toc;
+
+if enable_ros == false
+    %% Plot what is required
+    figure('Name','Trajectory Plot of Robot and Obstacle');
+    plot(zinit(1,:), zinit(2,:),'b-');
+    hold on
+    plot(zmpc(1,:), zmpc(2,:),'g-');
+    hold on
+    plot(bXtrain, bYtrain,'r-');
+    hold on
+    legend({'Robot trajectory from Stream Function','Robot trajectory using MPC','Obstacle trajectory using GPR'},'Location','southeast')
+end
diff --git a/TurtleBot_Impl/initialize_ros.m b/TurtleBot_Impl/initialize_ros.m
new file mode 100644
index 0000000..b435b78
--- /dev/null
+++ b/TurtleBot_Impl/initialize_ros.m
@@ -0,0 +1,11 @@
+function [] = initialize_ros(ros_ip, ros_master_uri)
+% INITIALIZE_ROS 
+% Function to initialize ROS and set the ROS MASTER
+% Set ROS_IP
+setenv('ROS_IP',ros_ip)
+% Set the ROS_MASTER_URI to the IP of TurtleBot
+setenv('ROS_MASTER_URI',ros_master_uri)
+% Initialize ROS
+rosinit
+
+end
\ No newline at end of file
diff --git a/TurtleBot_Impl/limiter_min_max.m b/TurtleBot_Impl/limiter_min_max.m
new file mode 100644
index 0000000..2816c54
--- /dev/null
+++ b/TurtleBot_Impl/limiter_min_max.m
@@ -0,0 +1,18 @@
+function [limited_out] = limiter_min_max(input, MIN, MAX)
+% LIMITER_MIN_MAX 
+% This function limits the input to the MIN and MAX limits
+% if MIN <= input <= MAX, limited_out = input
+% if input < MIN, limited_out = MIN
+% if input > MAX, limited_out = MAX
+
+if input <= MAX
+    if input >= MIN
+        limited_out = input;
+    elseif input < MIN
+        limited_out = MIN;
+    end
+elseif input > MAX
+    limited_out = MAX;
+end
+
+end
\ No newline at end of file
diff --git a/TurtleBot_Impl/stream_moving_obstacle.m b/TurtleBot_Impl/stream_moving_obstacle.m
new file mode 100644
index 0000000..31febe5
--- /dev/null
+++ b/TurtleBot_Impl/stream_moving_obstacle.m
@@ -0,0 +1,61 @@
+function [u, v, psi,u_dot, v_dot] = stream_obstacle()
+% Implementation of the stream function for avoiding single stationary and 
+% a single moving obstacle using Circle Theorem
+% Reference: https://doi.org/10.1109/ROBOT.2003.1241966
+% Authors: Mitesh Agrawal and Mohammed Tousif Zaman
+% Contact: msagrawal[at]wpi[dot]edu and mzaman[at]wpi[dot]edu
+
+clc;
+clearvars;
+
+syms bx by X Y a Vx Vy C real
+
+% C = 12;
+% Vx = 2;
+% Vy = 3;
+
+% b = bx + i*by defines the velocity of the moving obstacle
+b = bx + by*1i;
+% b_ is the conjugate of b
+b_ = conj(b);
+% complex variables for defining the potential field
+z = X + Y*1i;
+Z = ((a^2/(z-b)) + b_);
+
+% f = -C*log(z);
+% f_ = -C*log(Z);
+% w_stationary = f + f_;
+
+% considering velocity of moving obstacle for the potential field
+g = -Vx*z + Vy*z*1i;
+g_ = -Vx*Z - Vy*Z*1i;
+w_moving = g + g_;
+
+% alternate calculation
+% w = w_stationary + w_moving;
+% psi = imag(w);
+
+% stream function from imaginary component of complex potential for
+% avoiding a single moving obstacle
+psi2 = imag(w_moving);
+
+% definition from single obstacle avoidance case
+% Y1 and X1 - considering single circular shaped obstacle
+Y1 = (((a^2)*(Y-by))/(((X-bx)^2)+((Y-by)^2)))+by;
+X1 = (((a^2)*(X-bx))/(((X-bx)^2)+((Y-by)^2)))+bx;
+
+% stream function from imaginary component of complex potential for
+% avoiding a single stationary obstacle
+psi1 = -C*(atan2(Y,X)) + C*(atan2(Y1,X1));
+
+% stream function from imaginary component of the total complex potential
+% considering both stationary and moving obstacles
+psi = psi1 + psi2;
+
+% velocity components of the stream function
+u = diff(psi,Y);
+u_dot= diff(u,X);
+v = -1*diff(psi,X);
+v_dot= diff(v,Y);
+
+end
\ No newline at end of file
diff --git a/TurtleBot_Impl/terminate_ros.m b/TurtleBot_Impl/terminate_ros.m
new file mode 100644
index 0000000..6a0366f
--- /dev/null
+++ b/TurtleBot_Impl/terminate_ros.m
@@ -0,0 +1,12 @@
+function [] = terminate_ros()
+% TERMINATE_ROS
+% Function to terminate the connectivity between MATLAB and all ROS
+% objects
+
+% Clear the workspace of publishers, subscribers and other ROS objects
+clear
+% Disconnect from the ROS network
+rosshutdown
+
+end
+
diff --git a/TurtleBot_Impl/test.m b/TurtleBot_Impl/test.m
new file mode 100644
index 0000000..44e8d52
--- /dev/null
+++ b/TurtleBot_Impl/test.m
@@ -0,0 +1,34 @@
+%% Script to test the connectivity between MATLAB and TurtleBot
+
+% Set ROS_IP and ROS_MASTER_URI
+setenv('ROS_IP','130.215.206.232')
+setenv('ROS_MASTER_URI','http://130.215.121.108:11311')
+
+% Initialize ROS and connect to TurtleBot
+ip_turtlebot = '130.215.121.108';
+rosinit(ip_turtlebot)
+
+velocityX = 0.1;     % meters per second
+velocityY = 0.3;     % meters per second
+
+% Publish velocity message to TurtleBot
+my_turtlebot = rospublisher('/mobile_base/commands/velocity');
+velmsg = rosmessage(my_turtlebot);
+velmsg.Linear.X = velocityX;
+velmsg.Linear.Y = velocityY;
+
+for num=1:10
+    send(my_turtlebot,velmsg);
+end
+
+% Publish and send sound message to TurtleBot
+soundpub = rospublisher('/mobile_base/commands/sound', 'kobuki_msgs/Sound');
+soundmsg = rosmessage('kobuki_msgs/Sound');
+soundmsg.Value = 6;      % Any number 0-6
+send(soundpub,soundmsg);
+
+
+% Clear the workspace of publishers, subscribers and other ROS objects
+% clear
+% Disconnect from the ROS network
+% rosshutdown
diff --git a/TurtleBot_Impl/test_turtlebot.m b/TurtleBot_Impl/test_turtlebot.m
new file mode 100644
index 0000000..f624688
--- /dev/null
+++ b/TurtleBot_Impl/test_turtlebot.m
@@ -0,0 +1,48 @@
+%% Script to test the connectivity between MATLAB and TurtleBot
+
+% % Set ROS_IP and ROS_MASTER_URI
+% setenv('ROS_IP','130.215.206.232')
+% setenv('ROS_MASTER_URI','http://130.215.121.108:11311')
+% 
+% % Initialize ROS and connect to TurtleBot
+% ip_turtlebot = '130.215.121.108';
+% rosinit(ip_turtlebot)
+% Alternatively, call these functions
+initialize_ros();
+initialize_turtlebot();
+
+velocityX = 0.1;     % meters per second
+velocityY = 0.3;     % meters per second
+
+% Publish velocity message to TurtleBot
+my_turtlebot = rospublisher('/mobile_base/commands/velocity');
+velmsg = rosmessage(my_turtlebot);
+velmsg.Linear.X = velocityX;
+velmsg.Linear.Y = velocityY;
+
+for num=1:2
+    send(my_turtlebot,velmsg);
+end
+
+% Publish and send sound message to TurtleBot
+soundpub = rospublisher('/mobile_base/commands/sound', 'kobuki_msgs/Sound');
+soundmsg = rosmessage('kobuki_msgs/Sound');
+soundmsg.Value = 6;      % Any number 0-6
+send(soundpub,soundmsg);
+
+% Subscribe to the VICON topic to get Stationary Obstacle state feedback
+obstacle_sub = rossubscriber('/vicon/stationary_obs_track/stationary_obs_track');
+
+% Provide the initial/starting state of obstacle
+%% Get bx0 by0 of Obstacle
+obstacle_pose_data = receive(obstacle_sub,3);
+obstacle_pose = obstacle_pose_data.Pose.Pose;
+bx0 = obstacle_pose.Position.X;
+by0 = obstacle_pose.Position.Y;
+
+% Clear the workspace of publishers, subscribers and other ROS objects
+% clear
+% Disconnect from the ROS network
+% rosshutdown
+% Alternatively, call this function
+terminate_ros();
diff --git a/TurtleBot_Impl/test_vicon.m b/TurtleBot_Impl/test_vicon.m
new file mode 100644
index 0000000..152b333
--- /dev/null
+++ b/TurtleBot_Impl/test_vicon.m
@@ -0,0 +1,85 @@
+%% Script to connect MATLAB and VICON Motion Capture System
+
+%% DO THIS BEFORE RUNNING THIS SCRIPT
+% Open a terminal on PC and run the below command to launch VICON at the IP
+% address specified (cross-check the IP of the system running VICON)
+
+% roslaunch vicon_bridge vicon.launch ip:= 130.215.206.243
+
+%% Clear the workspace
+clc;
+clear;
+
+%% Set ROS environment variables and initialize ROS
+% create a ROS MASTER
+% master = robotics.ros.Core; % use if master_uri is same as ros_ip
+
+ROS_IP = '130.215.206.232';
+ROS_MASTER_URI = 'http://130.215.121.108:11311'; % URI of TurtleBot
+% Initialize ROS with MASTER at the specified IP address
+initialize_ros(ROS_IP, ROS_MASTER_URI);
+
+%% Subscribe to the VICON topic to get Stationary Obstacle state feedback
+obstacle_sub = rossubscriber('/vicon/stationary_obs_track/stationary_obs_track');
+
+%% Provide the initial/starting state of obstacle
+% Get bx0 by0 of Obstacle
+obstacle_pose_data = receive(obstacle_sub,3); % timeout of 3s
+bx0 = obstacle_pose_data.Transform.Translation.X;
+by0 = obstacle_pose_data.Transform.Translation.Y;
+
+%% Subscribe to the VICON topic to get TurtleBot state feedback
+turtlebot_sub = rossubscriber('/vicon/turtlebot_traj_track/turtlebot_traj_track');
+
+%% Provide the initial/starting state of robot
+% Get x0 y0 theta0 of TurtleBot
+turtlebot_pose_data = receive(turtlebot_sub,3); % timeout of 3s
+x0 = turtlebot_pose_data.Transform.Translation.X;
+y0 = turtlebot_pose_data.Transform.Translation.Y;
+
+quatW = turtlebot_pose_data.Transform.Rotation.W;
+quatX = turtlebot_pose_data.Transform.Rotation.X;
+quatY = turtlebot_pose_data.Transform.Rotation.Y;
+quatZ = turtlebot_pose_data.Transform.Rotation.Z;
+
+angles = quat2eul([quatW quatX quatY quatZ]); % Euler ZYX
+theta0 = rad2deg(angles(1));
+
+%% Velocity Publisher
+% Create a ROS publisher and message for velocity topic
+my_turtlebot = rospublisher('/mobile_base/commands/velocity');
+velmsg = rosmessage(my_turtlebot);
+
+%% Move the TurtleBot towards the origin and sync the time with ROS
+% set time step
+tf = 10;
+dt = 0.01;
+
+% r = robotics.ros.Rate(node,10);
+r = rosrate(100);
+tic;
+reset(r)
+for t = 0:dt:tf
+    
+    velocityX = 0.1;
+    velocityY = -0.2;
+    omegaZ = -1;
+
+    % Velocity in X and Y axes
+    % Velocity is limited to safe values
+    velmsg.Linear.X = limiter_min_max(velocityX, -0.7, 0.7); % 0.7m/s
+    velmsg.Linear.Y = limiter_min_max(velocityY, -0.7, 0.7); % 0.7m/s
+
+    % Steer about Z-axis
+    velmsg.Angular.Z = limiter_min_max(omegaZ, -180, 180); % 180deg/s
+
+    % Publish velocity and steer to the TurtleBot
+    send(my_turtlebot, velmsg);
+    % Wait the desired Hz time for the actuators to react
+    waitfor(r);
+    disp(t)
+end
+toc;
+
+%% Terminate ROS connectivity
+terminate_ros();
diff --git a/TurtleBot_Impl/turtlebot_data_mpc_10Hz_junk.mat b/TurtleBot_Impl/turtlebot_data_mpc_10Hz_junk.mat
new file mode 100644
index 0000000..49b4fd7
Binary files /dev/null and b/TurtleBot_Impl/turtlebot_data_mpc_10Hz_junk.mat differ
diff --git a/readme.md b/readme.md
new file mode 100644
index 0000000..5407992
--- /dev/null
+++ b/readme.md
@@ -0,0 +1,65 @@
+## path-planning-stream
+Implementation of vehicle path planning using stream functions on TurtleBot2
+
+### **Folder Structure:**
+#### **Stream_Function**
+Simulation of stream function in MATLAB
+
+#### **Stream_Function_ODE**
+Simulation using ODE function in MATLAB
+
+#### **GPR_based_stream_MPC**
+Simulation of obstacle avoidance with stream function + GPR + MPC in MATLAB
+
+#### **TurtleBot_Impl**
+Implementation of stream function based obstacle avoidance for stationary obstacle on TurtleBot2
+##### **Required Files:**
+- gpr_based_stream_turtlebot.m (main)
+- get_stream_trajectory.m
+- initialize_ros.m
+- terminate_ros.m
+- limiter_min_max.m
+
+### **Prerequisites or Dependencies:**
+
+- MATLAB with Robotics Toolbox [on workstation/ laptop]
+- ROS Kinetic [on workstation/ laptop]
+- vicon_bridge with latest vicon_sdk [on workstation/ laptop]
+
+### **Simulation**:
+
+**Stationary Obstacle:**
+- Run stream_stationary.m
+
+**Moving Obstacle**
+- Run stream_moving.m
+
+
+### **Demonstration:**
+
+Run the below commands in sequence
+
+#### On the TurtleBot2:
+
+Launch a terminal then run
+```
+roslaunch turtlebot_bringup minimal.launch
+```
+#### On the Workstation/Laptop running MATLAB:
+
+Launch a terminal then run
+```
+roslaunch vicon_bridge vicon.launch ip_vicon:=<enter ip address of VICON>
+```
+Launch MATLAB, open TurtleBot_Impl (available on this repo) then run the below M script
+
+- gpr_based_stream_turtlebot.m
+
+### **Additional:**
+- TurtleBot 2 runs on ROS Indigo
+- TurtleBot 2 has turtlebot_bringup, turtlebot_gazebo packages setup already
+- VICON Tracker 3.4.x setup on workstation with 8 overhead + 2 tripod mounted cameras
+- TurtleBot data published at /vicon/turtlebot_traj_track/turtlebot_traj_track
+- Stationary obstacle data published at /vicon/stationary_obs_track/stationary_obs_track
+- IP address of VICON can be set in vicon.launch within vicon_bridge (reference file available in this repo). This is used as the default IP of VICON and is optional on the terminal.
+- IP address of TurtleBot and workstation (running MATLAB) need to be updated in the .m scripts (see comments in gpr_based_stream_turtlebot.m)