diff --git a/remote_control/casadi_opt.py b/remote_control/casadi_opt.py
index 12e86bf..87df398 100644
--- a/remote_control/casadi_opt.py
+++ b/remote_control/casadi_opt.py
@@ -213,7 +213,7 @@ class OpenLoopSolver:
         # ---- path constraints -----------
         # limit = lambda pos: 1-sin(2*pi*pos)/2
         # self.opti.subject_to(speed<=limit(pos))   # track speed limit
-        maxcontrol = 0.3
+        maxcontrol = 0.9
         self.opti.subject_to(self.opti.bounded(-maxcontrol, self.U, maxcontrol))  # control is limited
 
         # ---- boundary conditions --------
diff --git a/remote_control/position_controller.py b/remote_control/position_controller.py
index 514d7fe..143112b 100644
--- a/remote_control/position_controller.py
+++ b/remote_control/position_controller.py
@@ -124,14 +124,14 @@ def f_ode(t, x, u):
 class RemoteController:
     def __init__(self):
 
-        #self.robots = [Robot(15, '192.168.1.103')]
-        self.robots = [Robot(14, '192.168.1.102')]
+        self.robots = [Robot(15, '192.168.1.103')]
+        #self.robots = [Robot(14, '192.168.1.102')]
 
         self.robot_ids = {}
         for r in self.robots:
             self.robot_ids[r.id] = r
 
-        obst = [Obstacle(12, 0.25), Obstacle(10, 0.25), Obstacle(13, 0.25)]
+        obst = [Obstacle(12, 0.275), Obstacle(10, 0.275), Obstacle(13, 0.275), Obstacle(14, 0.275)]
 
         self.obstacles = {}
         for r in obst:
@@ -217,7 +217,8 @@ class RemoteController:
         self.ax3.set_ylabel('y-position')
         self.ax3.grid()
 
-        self.ols = OpenLoopSolver()
+        self.mstep = 2
+        self.ols = OpenLoopSolver(N=20, T=4.0)
         self.ols.setup()
         self.dt = self.ols.T / self.ols.N
 
@@ -226,6 +227,7 @@ class RemoteController:
         # integrator
         self.r = scipy.integrate.ode(f_ode)
         self.omega_max = 5.0
+        #self.omega_max = 13.32
 
 
     def ani(self):
@@ -249,7 +251,8 @@ class RemoteController:
         self.track_line_outer.set_data(self.track.outer_poly.exterior.xy)
 
         return self.line, self.line_sim, self.dirm, self.dirs, self.line_ol,\
-               self.track_line_inner, self.track_line_outer, self.line_x,self.line_y, self.circles[0], self.circles[1],self.circles[2],
+               self.track_line_inner, self.track_line_outer, self.line_x,self.line_y, self.circles[0], self.circles[1],\
+               self.circles[2], self.circles[3],
 
     def ani_update(self, frame):
         #print("plotting")
@@ -317,7 +320,7 @@ class RemoteController:
             self.mutex.release()
 
         return self.line, self.line_sim, self.dirm, self.dirs, self.line_ol, self.track_line_inner, self.track_line_outer,\
-               self.line_x, self.line_y, self.circles[0], self.circles[1],self.circles[2],
+               self.line_x, self.line_y, self.circles[0], self.circles[1],self.circles[2],self.circles[3],
 
     def measurement_callback(self, data):
         #print(data)
@@ -410,7 +413,7 @@ class RemoteController:
                 x_pred = self.get_measurement_prediction()
 
                 if auto_control:
-                    if np.linalg.norm(x_pred[0:2]-np.array(self.target[0:2])) < 0.2:
+                    if np.linalg.norm(x_pred[0:2]-np.array(self.target[0:2])) < 0.3:
                         print("close to target!")
                         current_target = (current_target + 1) % 4
                         self.target = targets[current_target]
@@ -440,18 +443,24 @@ class RemoteController:
                     time.sleep(self.dt - dt_mpc)
 
                 # send controls to the robot
-                for i in range(0, 1): # option to use multistep mpc if len(range) > 1
+                for i in range(0, self.mstep): # option to use multistep mpc if len(range) > 1
                     u1 = us1[i]
                     u2 = us2[i]
                     if self.rc_socket:
                         self.rc_socket.send('({},{})\n'.format(u1, u2))
+                        if i < self.mstep:
+                            time.sleep(self.dt)
                     self.t = time.time() # save time the most recent control was applied
 
     def get_measurement_prediction(self):
         # get measurement
         self.mutex.acquire()
         try:
-            last_measurement = copy.deepcopy(self.xms[-1])
+            window = 3
+            last_measurement = copy.deepcopy(self.xms[-window:])
+            #print("last_measurements = {}".format(last_measurement))
+            #print("mean = {}".format(np.mean(last_measurement, axis=0)))
+            last_measurement = np.mean(last_measurement, axis=0)
             last_time = copy.deepcopy(self.tms[-1])
         finally:
             self.mutex.release()