├── Casadi_MPC_tutorial.ipynb ├── README.md ├── grid_planner_pid_ctrl.py ├── mpc_cbf.py ├── mpc_dc_fov.py ├── results ├── astar_pid_res.png ├── mpc_dc_simulation.gif ├── mpccbfres.jpg ├── sbmp.png └── state_estimation.png ├── sampling_planner.py └── state_estimation.py /README.md: -------------------------------------------------------------------------------- 1 | # A peek into Robot Planning and Control 2 | This repository serves as an introductory guide to planning and control algorithms commonly used in robotics. We begin with state estimation — not a planning or control technique per se, but a fundamental prerequisite for building autonomous systems. Next, we explore grid-based planners such as A* and Uniform Cost Search (UCS), followed by the implementation of a PID controller to enable the robot to follow planned waypoints. We then transition into sampling-based planning algorithms, and gradually delve into optimal control methods ranging from Quadratic Programming (QP) to Model Predictive Control (MPC) and MPC with Control Barrier Functions (MPC-CBF), which are widely used in autonomous robotics. This tutorial is designed to be accessible for beginners and also introduces powerful optimization libraries like cvxpy and CasADi. 3 | 4 | ## Chapter 1: State estimation using Gaussian Estimators 5 | 6 | State estimation helps robots infer their true state (like position or velocity) from noisy sensor data. Kalman Filter (KF) is optimal for linear systems with Gaussian noise, combining prediction and correction. EKF and UKF extend KF to nonlinear systems—EKF uses linearization, while UKF uses sigma points for better accuracy. 7 | 8 | Algorithms implemented: 9 | - [x] Kalman Filter 10 | - [x] Extended Kalman Filter 11 | - [ ] Unscented Kalman Filter 12 | 13 | ![alt text](https://github.com/RahulHKumar/Robot-Planning-and-Control/blob/main/results/state_estimation.png) 14 | 15 | ## Chapter 2: Grid based planning and PID controller 16 | 17 | Grid-based path planning algorithms can be used to search over a discretized map to find a path from start to goal. Algorithms like **A\***, **UCS**, **Greedy Search**, and **BFS** differ only in their **priority function** used for node expansion. This function combines factors like **path cost (g)**, **heuristic (h)**, or **depth**, e.g., A\* uses \( g + h \), UCS uses \( g \), Greedy uses \( h \), and BFS uses node depth. 18 | 19 | Algorithms implemented: 20 | - [x] A* 21 | - [x] UCS 22 | - [x] Greedy Search 23 | - [x] BFS 24 | - [x] DFS 25 | 26 | ![alt text](https://github.com/RahulHKumar/Robot-Planning-and-Control/blob/main/results/astar_pid_res.png) 27 | 28 | ## Chapter 3: Sampling based methods 29 | 30 | Sampling-based planning algorithms like **PRM**, **RRT**, and their optimal variants (**PRM\***, **RRT\***), build paths by randomly sampling the space rather than searching over a fixed grid. They efficiently handle high-dimensional or continuous spaces where grid-based methods become computationally expensive or infeasible. PRM builds a global roadmap and RRT grows a tree from the start. 31 | 32 | While **RRT\*** finds optimal paths in configuration space but often ignores system dynamics, making it unsuitable for real robots with motion constraints. **Kinodynamic RRT\*** addresses this by incorporating **dynamics and differential constraints**, generating dynamically feasible trajectories. **LQR-RRT\*** further improves performance by using **Linear Quadratic Regulator (LQR)** for local steering, leading to smoother, lower-cost paths and better control-aware exploration. We consider a unicycle model for kinodynamic RRT* and double integrator for LQR-RRT* in this tutorial. This section also introduces the use of **cvxpy**. 33 | 34 | ![alt text](https://github.com/RahulHKumar/Robot-Planning-and-Control/blob/main/results/sbmp.png) 35 | 36 | This section implements: 37 | - [x] PRM 38 | - [x] PRM* 39 | - [x] RRT 40 | - [x] RRT* 41 | - [x] Kinodynamic RRT* 42 | - [x] LQR-RRT* 43 | 44 | ## Chapter 4: Optimization 45 | 46 | Now, the **most important part of this tutorial!**. What is optimization? Check out the [notebook](Casadi_MPC_tutorial.ipynb) for step by step explanation of what is MPC? This section also introduces the use of **Casadi**. 47 | 48 | **Optimization-based techniques** like **Model Predictive Control (MPC)** compute control actions by solving a constrained optimization problem at each timestep. They account for system dynamics, future goals, and input/state constraints, enabling **predictive and constraint-aware** decision making. We also simulate a 2D lidar in matplotlib using Bresenham's algorithm, K-Means Clustering and DBSCAN clustering. 49 | 50 | ![alt text](https://github.com/RahulHKumar/Robot-Planning-and-Control/blob/main/results/mpc_dc_simulation.gif) 51 | 52 | **MPC-CBF** further enhances this by incorporating **Control Barrier Functions (CBFs)** for formal **safety guarantees**, ensuring collision avoidance and constraint satisfaction even in dynamic environments. 53 | 54 | ![alt text](https://github.com/RahulHKumar/Robot-Planning-and-Control/blob/main/results/mpccbfres.jpg) 55 | -------------------------------------------------------------------------------- /grid_planner_pid_ctrl.py: -------------------------------------------------------------------------------- 1 | import numpy as np 2 | import matplotlib.pyplot as plt 3 | from matplotlib.animation import FuncAnimation 4 | from IPython import embed 5 | from matplotlib.patches import Rectangle 6 | import heapq 7 | 8 | class MazeEnvironment: 9 | def __init__(self, start_pos, goal_pos): 10 | # Define maze walls as list of [x, y, width, height] 11 | self.maze_width = 20 12 | self.maze_height = 20 13 | self.start_pos = start_pos 14 | self.goal_pos = goal_pos 15 | self.walls = [ 16 | # Outer walls 17 | [-10, -5, 20, 1], # Bottom 18 | [-10, 15, 20, 1], # Top 19 | [-10, -5, 1, 21], # Left 20 | [9, -5, 1, 21], # Right 21 | 22 | # Inner walls 23 | [-5, 0, 10, 1], # Horizontal wall 24 | [0, 5, 10, 1], # Another horizontal wall 25 | [-5, 5, 1, 5], # Vertical wall 26 | ] 27 | 28 | # # save the maze as a numpy array. Obstacle is 1 and free space is 0. Padding of 1 is added to the maze 29 | # self.pad_size = 1 30 | # self.maze = np.zeros((20, 20)) 31 | # self.maze[1:20, 1:20] = 0 32 | # for wall in self.walls: 33 | # x, y, w, h = wall 34 | # self.maze[y:y+h, x:x+w] = 1 35 | 36 | def get_maze(self): 37 | # Initialize maze with zeros (free space) 38 | maze = np.zeros((self.maze_height, self.maze_width), dtype=int) 39 | 40 | # Add padding 41 | maze[0, :] = 1 # Top padding 42 | maze[-1, :] = 1 # Bottom padding 43 | maze[:, 0] = 1 # Left padding 44 | maze[:, -1] = 1 # Right padding 45 | 46 | # Add walls inside the maze 47 | for wall in self.walls: 48 | x, y, w, h = wall 49 | x_idx = x + 10 # Shift to fit numpy indexing (maze origin at (0,0)) 50 | y_idx = y + 5 51 | maze[y_idx:y_idx+h, x_idx:x_idx+w] = 1 52 | # flipping the maze to match the coordinate system 53 | # maze = np.flipud(maze) 54 | return maze 55 | 56 | 57 | def check_collision(self, x, y, radius): 58 | # Check if robot collides with any wall 59 | for wall in self.walls: 60 | wx, wy, ww, wh = wall 61 | # Expand wall boundaries by robot radius for collision check 62 | if (x + radius > wx and x - radius < wx + ww and 63 | y + radius > wy and y - radius < wy + wh): 64 | return True 65 | return False 66 | 67 | def draw(self, ax): 68 | for wall in self.walls: 69 | x, y, w, h = wall 70 | ax.add_patch(Rectangle((x+1, y+1), w, h, facecolor='gray')) 71 | # Draw red rectangle for start position 72 | ax.add_patch(Rectangle(self.start_pos, 1, 1, facecolor='red', alpha=0.5)) 73 | # Draw green rectangle for goal position 74 | ax.add_patch(Rectangle(self.goal_pos, 1, 1, facecolor='green', alpha=0.5)) 75 | 76 | class Robot: 77 | def __init__(self, radius, mu0, Sigma0, proc_noise_std, obs_noise_std, maze): 78 | # Just for the looks 79 | self.radius = radius 80 | # Adding maze to the robot 81 | self.maze = maze 82 | 83 | # Defining parameters for KF algorithm 84 | self.mu0 = mu0 85 | self.Sigma0 = Sigma0 86 | self.proc_noise_std = proc_noise_std 87 | self.obs_noise_std = obs_noise_std 88 | 89 | # Process noise covariance (R) 90 | self.R = np.diag(self.proc_noise_std ** 2) # process noise covariance 91 | 92 | # Observation model (C) 93 | self.C = np.eye(3) 94 | 95 | # Standard deviations for the noise in x, y, and theta (observation or sensor model noise) 96 | # Observation noise covariance (Q) 97 | self.Q = np.diag(self.obs_noise_std ** 2) 98 | 99 | def action_model(self, At, Bt, ut, xt_1): 100 | # xt = At*xt-1 + Bt*ut + wt 101 | # wt is gaussian noise with covariance R (process noise/ actuators noise) 102 | wt = np.random.multivariate_normal(np.zeros(3), self.R) 103 | xt = At @ xt_1 + Bt @ ut + wt 104 | 105 | # Check for collision 106 | if self.maze.check_collision(xt[0], xt[1], self.radius): 107 | # If collision detected, stay in place 108 | return xt_1 109 | return xt 110 | 111 | def sensor_model(self, Ct, xt): 112 | # zt = Ct*xt + vt 113 | # vt is gaussian noise with covariance Q (sensor noise) 114 | vt = np.random.multivariate_normal(np.zeros(3), self.Q) 115 | zt = Ct @ xt + vt 116 | return zt 117 | 118 | def dynamic_model(self, theta): 119 | # Input: mu_t-1 120 | # At = np.eye(3) 121 | # Bt = np.array([[np.cos(self.mu[2]), 0], [np.sin(self.mu[2]), 0], [0, 1]]) 122 | At = np.eye(3) 123 | Bt = np.array([ 124 | [np.cos(theta), 0], 125 | [np.sin(theta), 0], 126 | [0, 1] 127 | ]) 128 | return At, Bt 129 | 130 | def g_nonlin_motion(self, mu, u): 131 | # mu, u are np vectors of size 3x1 and 2x1 respectively 132 | x, y, theta = mu 133 | v, w = u 134 | x_new = x + v*np.cos(theta) 135 | y_new = y + v*np.sin(theta) 136 | theta_new = theta + w 137 | return np.array([x_new, y_new, theta_new]) 138 | 139 | def G_nonlin_motion(self, mu, u): 140 | # mu, u are np vectors of size 3x1 and 2x1 respectively 141 | x, y, theta = mu 142 | v, w = u 143 | G = np.array([ 144 | [1, 0, -v*np.sin(theta)], 145 | [0, 1, v*np.cos(theta)], 146 | [0, 0, 1] 147 | ]) 148 | return G 149 | 150 | def h_observation(self, mu): 151 | # mu is np vector of size 3x1 152 | return mu 153 | 154 | def H_observation(self): 155 | return np.eye(3) 156 | 157 | def kalman_filter(self, mu, cov, u_t, z_t, At, Bt): 158 | # Prediction step 159 | mu_t_bar = At @ mu + Bt @ u_t 160 | sigma_t_bar = At @ cov @ At.T + self.R 161 | 162 | # Kalman Gain calculation 163 | K = sigma_t_bar @ self.C.T @ np.linalg.inv(self.C @ sigma_t_bar @ self.C.T + self.Q) 164 | 165 | # Update step 166 | mu = mu_t_bar + K @ (z_t - self.C @ mu_t_bar) 167 | sigma = (np.eye(3) - K @ self.C) @ sigma_t_bar 168 | 169 | return mu, sigma 170 | 171 | def extended_kalman_filter(self, mu, cov, u_t, z_t): 172 | # Prediction step 173 | mu_t_bar = self.g_nonlin_motion(mu, u_t) 174 | G = self.G_nonlin_motion(mu, u_t) 175 | sigma_t_bar = G @ cov @ G.T + self.R 176 | 177 | # Observation prediction 178 | h = self.h_observation(mu_t_bar) 179 | H = self.H_observation() 180 | 181 | # Kalman Gain calculation 182 | K = sigma_t_bar @ H.T @ np.linalg.inv(H @ sigma_t_bar @ H.T + self.Q) 183 | 184 | # Update step 185 | mu = mu_t_bar + K @ (z_t - h) 186 | sigma = (np.eye(3) - K @ H) @ sigma_t_bar 187 | 188 | return mu, sigma 189 | 190 | def gnd_truth_dynamic(self, state_gnd, u_t): 191 | # This is only for visualization 192 | At, Bt = self.dynamic_model(state_gnd[2]) 193 | # embed() 194 | xt = At@state_gnd + Bt@u_t 195 | # print(xt) 196 | return xt 197 | 198 | def heuristic(a, b): 199 | # manhattan distance 200 | return abs(a[0] - b[0]) + abs(a[1] - b[1]) 201 | 202 | def get_neighbours(current_node, maze): 203 | neighbours = [] 204 | for new_position in [(0, -1), (0, 1), (-1, 0), (1, 0)]: # Adjacent squares 205 | node_position = (current_node[0] + new_position[0], current_node[1] + new_position[1]) 206 | if node_position[0] > (len(maze) - 1) or node_position[0] < 0 or node_position[1] > (len(maze[len(maze)-1]) -1) or node_position[1] < 0: 207 | continue # Out of bounds. They are outside the walls 208 | if maze[node_position[0]][node_position[1]] != 0: 209 | continue # It is a wall or obstacle 210 | neighbours.append(node_position) 211 | return neighbours 212 | 213 | def path_planning(start_pos, goal_pos, algo, env_maze): 214 | # algo can be Astar or Greedy or UCS or BFS or DFS 215 | # we shall use different priority queues functions for different algorithms 216 | priority_func = { 217 | 'Astar': lambda cost, depth, heuristic: cost + heuristic, 218 | 'Greedy': lambda cost, depth, heuristic: heuristic, 219 | 'UCS': lambda cost, depth, heuristic: cost, 220 | 'BFS': lambda cost, depth, heuristic: depth, 221 | 'DFS': lambda cost, depth, heuristic: -depth 222 | } 223 | 224 | if algo not in priority_func: 225 | raise ValueError('Invalid algorithm. Choose from Astar, Greedy, UCS, BFS, DFS') 226 | 227 | # Convert start and goal positions to tuple 228 | start_pos = tuple(start_pos) 229 | goal_pos = tuple(goal_pos) 230 | 231 | # Initializing the priority queue 232 | priority_queue = [] 233 | heapq.heappush(priority_queue, (0, 0, start_pos, [start_pos])) # cost, depth, position, path 234 | # The above line means initial priority is 0, 235 | # cost to reach the start position is 0, 236 | # position is start_pos and path is [start_pos] 237 | visited = set() 238 | 239 | while len(priority_queue) > 0: 240 | priority, cost, current_node, path = heapq.heappop(priority_queue) 241 | 242 | if current_node in visited: 243 | continue 244 | visited.add(current_node) 245 | 246 | if current_node == goal_pos: 247 | x_cord, y_cord = zip(*path) 248 | return x_cord, y_cord 249 | 250 | for neighbour in get_neighbours(current_node, env_maze): 251 | if neighbour not in visited: 252 | new_cost = cost + 1 253 | heuristic_val = heuristic(neighbour, goal_pos) 254 | depth = len(path) 255 | new_priority = priority_func[algo](new_cost, depth, heuristic_val) 256 | heapq.heappush(priority_queue, (new_priority, new_cost, neighbour, path + [neighbour])) 257 | 258 | # Modified PID Controller Implementation with PID for both linear and angular velocity 259 | def pid_controller(current_state, target_waypoint, 260 | v_Kp=0.5, v_Ki=0.01, v_Kd=0.1, 261 | w_Kp=1.0, w_Ki=0.01, w_Kd=0.1, 262 | prev_heading_error=0, heading_error_integral=0, 263 | prev_distance_error=0, distance_error_integral=0): 264 | # Extract current position and orientation 265 | current_x, current_y, current_theta = current_state 266 | target_x, target_y = target_waypoint 267 | 268 | # Calculate distance to target 269 | dx = target_x - current_x 270 | dy = target_y - current_y 271 | distance = np.sqrt(dx**2 + dy**2) 272 | 273 | # Distance error (for linear velocity control) 274 | # Using target distance of 0 (we want to reach the waypoint) 275 | distance_error = distance 276 | 277 | # Integral term for distance 278 | distance_error_integral += distance_error 279 | 280 | # Derivative term for distance 281 | distance_error_derivative = distance_error - prev_distance_error 282 | 283 | # PID control for linear velocity 284 | v = (v_Kp * distance_error) + (v_Ki * distance_error_integral) + (v_Kd * distance_error_derivative) 285 | 286 | # Calculate desired heading 287 | desired_theta = np.arctan2(dy, dx) 288 | 289 | # Calculate heading error 290 | heading_error = desired_theta - current_theta 291 | 292 | # Normalize heading error to [-pi, pi] range 293 | heading_error = (heading_error + np.pi) % (2 * np.pi) - np.pi 294 | 295 | # Integral term for heading 296 | heading_error_integral += heading_error 297 | 298 | # Derivative term for heading 299 | heading_error_derivative = heading_error - prev_heading_error 300 | 301 | # PID control for angular velocity 302 | w = (w_Kp * heading_error) + (w_Ki * heading_error_integral) + (w_Kd * heading_error_derivative) 303 | 304 | # Limit velocities for safety 305 | v = np.clip(v, 0, 1.2) # Max linear velocity of 1 306 | 307 | # Return control inputs and updated error terms for next iteration 308 | return np.array([v, w]), heading_error, heading_error_integral, distance_error, distance_error_integral 309 | 310 | def get_trajectory_offline(start_pos, goal_pos, algo, env_maze): 311 | # convert start and goal positions to maze coordinates 312 | start_pos = np.array([start_pos[0]+10, start_pos[1]+5]) 313 | goal_pos = np.array([goal_pos[0]+10, goal_pos[1]+5]) 314 | env_maze = np.fliplr(env_maze) 315 | x_cord, y_cord = path_planning(start_pos, goal_pos, algo, env_maze) 316 | # convert x_cord and y_cord to actual coordinates 317 | x_cord = [x-10 for x in x_cord] 318 | y_cord = [y-5 for y in y_cord] 319 | return x_cord, y_cord 320 | 321 | def main(): 322 | # Create maze instance 323 | start_pos = np.array([-6, -2]) 324 | goal_pos = np.array([6, 12]) 325 | maze = MazeEnvironment(start_pos, goal_pos) 326 | env_maze= maze.get_maze() 327 | 328 | # Create robot instance 329 | radius_robot = 0.2 330 | initial_state = np.array([-6, -2, 0]) # x, y, theta 331 | mu0 = initial_state 332 | sigma0 = np.eye(3) 333 | proc_noise_std = np.array([0.02, 0.02, 0.01]) 334 | obs_noise_std = np.array([0.5, 0.5, 0.2]) 335 | radius_circle = 5.0 336 | robot = Robot(radius_robot, mu0, sigma0, proc_noise_std, obs_noise_std, maze) 337 | 338 | # Gnd state for visualization (not used in KF) (not a distribution) 339 | state_gnd = initial_state 340 | 341 | # Get path planning waypoints 342 | algo = 'Astar' 343 | x_cord, y_cord = get_trajectory_offline(start_pos, goal_pos, algo, env_maze) 344 | waypoints = list(zip(x_cord, y_cord)) 345 | steps = len(waypoints) 346 | 347 | # Collect positions over time 348 | gnd_truth_x_positions = [] 349 | gnd_truth_y_positions = [] 350 | noisy_x_positions = [] 351 | noisy_y_positions = [] 352 | filtered_x_positions = [] 353 | filtered_y_positions = [] 354 | ekf_filtered_x_positions = [] 355 | ekf_filtered_y_positions = [] 356 | 357 | # Starting Kalman Filter code 358 | state = initial_state 359 | mu = mu0 360 | mu_ekf = mu0 361 | cov = sigma0 362 | cov_ekf = sigma0 363 | 364 | # Initialize PID error tracking 365 | prev_heading_error = 0 366 | heading_error_integral = 0 367 | prev_distance_error = 0 368 | distance_error_integral = 0 369 | 370 | # Simulate for each waypoint 371 | for i in range(1, steps): # Start from 1 to skip the starting position 372 | # Call PID controller to get control inputs for current waypoint 373 | current_state = np.array([mu_ekf[0], mu_ekf[1], mu_ekf[2]]) # Use filtered state for control 374 | target_waypoint = waypoints[i] 375 | 376 | u_present, prev_heading_error, heading_error_integral, prev_distance_error, distance_error_integral = pid_controller( 377 | current_state, 378 | target_waypoint, 379 | v_Kp=0.5, # Adjust for linear velocity control 380 | v_Ki=0.01, # Small integral gain to avoid overshoot 381 | v_Kd=0.1, # Derivative gain for smoother response 382 | w_Kp=0.8, # Proportional gain for angular velocity 383 | w_Ki=0.01, # Small integral gain for heading 384 | w_Kd=0.1, # Derivative gain for angular velocity 385 | prev_heading_error=prev_heading_error, 386 | heading_error_integral=heading_error_integral, 387 | prev_distance_error=prev_distance_error, 388 | distance_error_integral=distance_error_integral 389 | ) 390 | 391 | # Get the ground truth state (for visualization) 392 | state_gnd = robot.gnd_truth_dynamic(state_gnd, u_present) 393 | # gnd_truth_x, ground_truth_y, _ = state_gnd 394 | # gnd_truth_x_positions.append(gnd_truth_x) 395 | # gnd_truth_y_positions.append(ground_truth_y) 396 | 397 | # Now, call action model to get the next state 398 | At, Bt = robot.dynamic_model(state[2]) 399 | state = robot.action_model(At, Bt, u_present, state) 400 | gnd_truth_x, ground_truth_y, _ = state # from action model 401 | gnd_truth_x_positions.append(gnd_truth_x) 402 | gnd_truth_y_positions.append(ground_truth_y) 403 | 404 | # Getting the sensor reading 405 | z_t = robot.sensor_model(robot.C, state) 406 | x_noisy, y_noisy, _ = z_t 407 | noisy_x_positions.append(x_noisy) 408 | noisy_y_positions.append(y_noisy) 409 | 410 | # Now, call Kalman Filter 411 | mu, cov = robot.kalman_filter(mu, cov, u_present, z_t, At, Bt) 412 | filtered_x, filtered_y, _ = mu 413 | filtered_x_positions.append(filtered_x) 414 | filtered_y_positions.append(filtered_y) 415 | 416 | # Similarly, let's implement EKF 417 | mu_ekf, cov_ekf = robot.extended_kalman_filter(mu_ekf, cov_ekf, u_present, z_t) 418 | ekf_filtered_x, ekf_filtered_y, _ = mu_ekf 419 | ekf_filtered_x_positions.append(ekf_filtered_x) 420 | ekf_filtered_y_positions.append(ekf_filtered_y) 421 | 422 | # Create the figure and axis 423 | fig, ax = plt.subplots(figsize=(5, 5)) 424 | ax.set_xlim(-10, 10) 425 | ax.set_ylim(-5, 15) 426 | ax.set_title('Robot path planning in maze') 427 | ax.set_xlabel('X Position') 428 | ax.set_ylabel('Y Position') 429 | ax.grid(True) 430 | ax.set_xticks(np.arange(-10, 11, 1)) 431 | ax.set_yticks(np.arange(-5, 16, 1)) 432 | ax.axis('equal') 433 | 434 | # Draw maze 435 | maze.draw(ax) 436 | 437 | # Create line and scatter plot objects 438 | ideal_line, = ax.plot([], [], 'b-', label='Ground truth') 439 | noisy_scatter, = ax.plot([], [], 'r-', label='Noisy Sensor Readings') 440 | filtered_line, = ax.plot([], [], 'g-', label='KF Filtered Path') 441 | ekf_filtered_line, = ax.plot([], [], 'm', label='EKF Filtered Path') 442 | robot_dot = ax.plot([], [], 'ko', markersize=2*robot.radius)[0] 443 | 444 | # Drawing the path found by path planning algorithm (saved in x_cord, y_cord) 445 | ax.plot(x_cord, y_cord, 'y--', label='Path found by '+algo) 446 | ax.legend() 447 | 448 | # Animation update function 449 | def update(frame): 450 | # Update ideal path 451 | ideal_line.set_data(gnd_truth_x_positions[:frame+1], gnd_truth_y_positions[:frame+1]) 452 | 453 | # Update noisy scatter points 454 | noisy_scatter.set_data(noisy_x_positions[:frame+1], noisy_y_positions[:frame+1]) 455 | 456 | # Update filtered path 457 | filtered_line.set_data(filtered_x_positions[:frame+1], filtered_y_positions[:frame+1]) 458 | 459 | # Update ekf filtered path 460 | ekf_filtered_line.set_data(ekf_filtered_x_positions[:frame+1], ekf_filtered_y_positions[:frame+1]) 461 | 462 | # Update robot position 463 | if frame < len(filtered_x_positions): 464 | robot_dot.set_data([filtered_x_positions[frame]], [filtered_y_positions[frame]]) 465 | 466 | return ideal_line, noisy_scatter, filtered_line, ekf_filtered_line, robot_dot 467 | 468 | # Create animation 469 | anim = FuncAnimation(fig, update, frames=len(filtered_x_positions), 470 | interval=200, # 200 ms between frames 471 | blit=True, 472 | repeat=False) 473 | 474 | plt.show() 475 | 476 | # Run the main function 477 | if __name__ == "__main__": 478 | main() -------------------------------------------------------------------------------- /mpc_cbf.py: -------------------------------------------------------------------------------- 1 | import casadi as ca 2 | import numpy as np 3 | import matplotlib.pyplot as plt 4 | from matplotlib.animation import FuncAnimation 5 | import cv2 6 | from sklearn.cluster import DBSCAN, KMeans 7 | 8 | # Parameters 9 | T = 0.05 # Time step 10 | N = 10 # Prediction horizon (increased for better planning) 11 | obs_x, obs_y, obs_r = 3.5, 3, 0.75 # Obstacle 1 (center and radius) 12 | obs2_x, obs2_y, obs2_r = 1, 1.0, 0.5 # Obstacle 2 (center and radius) 13 | 14 | # Store obstacles in a list 15 | obstacles = [(obs_x, obs_y, obs_r), (obs2_x, obs2_y, obs2_r), (1, 4.5, 1.0), (5, 0, 0.5)] 16 | 17 | reached_goal_threshold = 0.1 # Threshold to stop the simulation 18 | 19 | goal_x, goal_y = 5, 5 # Goal position 20 | max_iters = 100 # Max iterations to reach the goal 21 | safety_margin = 0.1 # Safety margin for obstacle avoidance 22 | 23 | 24 | # sensor parameters 25 | fov = 120 # Field of view in degrees 26 | sensor_range = 2 # Sensor range in meters 27 | num_ray = 200 # Resolution of the sensor 28 | resolution = 0.01 # Resolution of the sensor in meters 29 | 30 | def cluster_black_pixels(image, method="dbscan", eps=5, min_samples=5, n_clusters=3): 31 | gray = image[:, :, 0] # Use only the first channel 32 | black_pixels = np.column_stack(np.where(gray == 0)) 33 | 34 | if len(black_pixels) == 0: 35 | return [] # No black pixels found 36 | 37 | if method.lower() == "dbscan": 38 | clustering = DBSCAN(eps=eps, min_samples=min_samples) 39 | labels = clustering.fit_predict(black_pixels) 40 | 41 | elif method.lower() == "kmeans": 42 | if len(black_pixels) < n_clusters: 43 | return [] # Not enough points for K-Means 44 | clustering = KMeans(n_clusters=n_clusters, random_state=42, n_init=10) 45 | labels = clustering.fit_predict(black_pixels) 46 | 47 | else: 48 | raise ValueError("Invalid method. Choose 'dbscan' or 'kmeans'.") 49 | 50 | clusters = [] 51 | unique_labels = set(labels) - {-1} # Remove noise (-1) from DBSCAN 52 | 53 | for label in unique_labels: 54 | cluster_points = black_pixels[labels == label] # Get points belonging to the cluster 55 | 56 | if len(cluster_points) < min_samples: 57 | continue # Skip tiny clusters 58 | 59 | # Fit a minimum enclosing circle 60 | (x, y), radius = cv2.minEnclosingCircle(cluster_points.astype(np.float32)) 61 | clusters.append((int(x), int(y), int(radius))) 62 | 63 | return clusters 64 | 65 | def isObstacle(world_x, world_y): 66 | for obs_x, obs_y, obs_r in obstacles: 67 | if (world_x - obs_x)**2 + (world_y - obs_y)**2 <= obs_r**2: 68 | return True 69 | return False 70 | 71 | # Bresenham's Line Algorithm 72 | def bresenham(x1, y1, x2, y2): 73 | points = [] 74 | dx = abs(x2 - x1) 75 | dy = abs(y2 - y1) 76 | sx = 1 if x1 < x2 else -1 77 | sy = 1 if y1 < y2 else -1 78 | err = dx - dy 79 | 80 | while True: 81 | points.append((x1, y1)) 82 | if x1 == x2 and y1 == y2: 83 | break 84 | e2 = 2 * err 85 | if e2 > -dy: 86 | err -= dy 87 | x1 += sx 88 | if e2 < dx: 89 | err += dx 90 | y1 += sy 91 | 92 | return points 93 | 94 | def simulate_lidar(x, y, theta): 95 | # Initialize sensor window with resolution 96 | grid_size = int(6/resolution) 97 | display_window = np.zeros((grid_size, grid_size)) 98 | 99 | # Calculate center of grid (robot position) 100 | grid_center_x = grid_size // 2 101 | grid_center_y = grid_size // 2 102 | 103 | # Fill with background color (gray) 104 | display_window.fill(0.5) 105 | 106 | # For each ray in the field of view 107 | half_fov = fov / 2 108 | angle_increment = fov / (num_ray-1) 109 | 110 | for i in range(num_ray): 111 | # Calculate ray angle 112 | ray_angle = theta - np.deg2rad(half_fov) + np.deg2rad(i * angle_increment) 113 | 114 | # Calculate endpoint of ray based on sensor range (in grid coordinates) 115 | end_x = int(grid_center_x + (sensor_range/resolution) * np.cos(ray_angle)) 116 | end_y = int(grid_center_y + (sensor_range/resolution) * np.sin(ray_angle)) 117 | 118 | # Clamp to grid boundaries 119 | end_x = max(0, min(end_x, grid_size-1)) 120 | end_y = max(0, min(end_y, grid_size-1)) 121 | 122 | # Use Bresenham's algorithm to get all points along the ray 123 | ray_points = bresenham(grid_center_x, grid_center_y, end_x, end_y) 124 | 125 | # Mark ray points in the display window 126 | hit_obstacle = False 127 | for point_x, point_y in ray_points: 128 | if 0 <= point_x < grid_size and 0 <= point_y < grid_size: 129 | # Convert grid coordinates to world coordinates 130 | world_x = x + (point_x - grid_center_x) * resolution 131 | world_y = y + (point_y - grid_center_y) * resolution 132 | 133 | if hit_obstacle: 134 | # After hitting an obstacle, mark the rest of the ray black 135 | display_window[point_y, point_x] = 0.0 136 | else: 137 | # Check if point is an obstacle 138 | if isObstacle(world_x, world_y): 139 | display_window[point_y, point_x] = 0.0 # Mark obstacle as black 140 | hit_obstacle = True # Set flag but don't break 141 | else: 142 | display_window[point_y, point_x] = 1.0 # Mark ray path as white 143 | 144 | # Mark robot position 145 | display_window[grid_center_y, grid_center_x] = 1.0 # Robot position as white 146 | 147 | # Convert to 0-255 range for display 148 | display_window = (display_window * 255).astype(np.uint8) 149 | 150 | # Convert to RGB 151 | display_window = np.stack([display_window]*3, axis=-1) 152 | 153 | # mask black pixels 154 | mask = np.all(display_window == [0, 0, 0], axis=-1) 155 | plain_white = np.ones((grid_size, grid_size)) 156 | plain_white = np.stack([plain_white]*3, axis=-1) 157 | plain_white[mask] = [0, 0, 0] 158 | # cluster maksed image 159 | clusters_pixel = cluster_black_pixels(plain_white, method="dbscan", eps=5, min_samples=5) 160 | # cluster using kmeans 161 | # clusters_pixel = cluster_black_pixels(plain_white, method="kmeans", n_clusters=2) 162 | clusters = [None] * len(clusters_pixel) 163 | # convert pixel coordinates to world coordinates and store in clusters 164 | for i, (pixel_x, pixel_y, pixel_radius) in enumerate(clusters_pixel): 165 | world_x = x + (pixel_x - grid_center_x) * resolution 166 | world_y = y + (pixel_y - grid_center_y) * resolution 167 | world_radius = pixel_radius * resolution # Scale the radius 168 | clusters[i] = (world_x, world_y, world_radius) 169 | print(clusters) 170 | 171 | return display_window, clusters_pixel 172 | 173 | def solve_mpc(x0, y0, theta0): 174 | opti = ca.Opti() 175 | X = opti.variable(N+1) 176 | Y = opti.variable(N+1) 177 | Theta = opti.variable(N+1) 178 | V = opti.variable(N) 179 | Omega = opti.variable(N) 180 | 181 | # System dynamics constraints 182 | for k in range(N): 183 | opti.subject_to(X[k+1] == X[k] + V[k] * ca.cos(Theta[k]) * T) 184 | opti.subject_to(Y[k+1] == Y[k] + V[k] * ca.sin(Theta[k]) * T) 185 | opti.subject_to(Theta[k+1] == Theta[k] + Omega[k] * T) 186 | 187 | # Initial conditions 188 | opti.subject_to(X[0] == x0) 189 | opti.subject_to(Y[0] == y0) 190 | opti.subject_to(Theta[0] == theta0) 191 | 192 | # Control constraints 193 | opti.subject_to(opti.bounded(0, V, 2)) 194 | opti.subject_to(opti.bounded(-1, Omega, 1)) 195 | 196 | # Define CBF constraints for obstacles 197 | gamma = 0.7 # CBF relaxation parameter (tuning required) 198 | 199 | for k in range(N): 200 | # Compute distance functions for obstacles 201 | h1 = (X[k] - obs_x)**2 + (Y[k] - obs_y)**2 - (obs_r + safety_margin)**2 202 | h2 = (X[k] - obs2_x)**2 + (Y[k] - obs2_y)**2 - (obs2_r + safety_margin)**2 203 | 204 | # Compute Lie derivatives (change in h) 205 | Lf_h1 = 2 * (X[k] - obs_x) * V[k] * ca.cos(Theta[k]) + 2 * (Y[k] - obs_y) * V[k] * ca.sin(Theta[k]) 206 | Lf_h2 = 2 * (X[k] - obs2_x) * V[k] * ca.cos(Theta[k]) + 2 * (Y[k] - obs2_y) * V[k] * ca.sin(Theta[k]) 207 | 208 | # Ensure CBF constraints are satisfied 209 | opti.subject_to(Lf_h1 + gamma * h1 >= 0) 210 | opti.subject_to(Lf_h2 + gamma * h2 >= 0) 211 | 212 | # TODO: put conatraints for sensed obstacles 213 | _, obstacle_clusters = simulate_lidar(x0, y0, theta0) 214 | for cluster in obstacle_clusters: 215 | center_x, center_y, radius = cluster 216 | h = (X[k] - center_x)**2 + (Y[k] - center_y)**2 - (radius + safety_margin)**2 217 | Lf_h = 2 * (X[k] - center_x) * V[k] * ca.cos(Theta[k]) + 2 * (Y[k] - center_y) * V[k] * ca.sin(Theta[k]) 218 | opti.subject_to(Lf_h + gamma * h >= 0) 219 | 220 | 221 | # Cost function: Prioritize reaching the goal while minimizing effort 222 | goal_weight = 150 223 | effort_weight = 0.2 224 | cost = effort_weight * (ca.sumsqr(V) + ca.sumsqr(Omega)) + goal_weight * ((X[N] - goal_x)**2 + (Y[N] - goal_y)**2) 225 | opti.minimize(cost) 226 | 227 | # Solve 228 | opti.solver('ipopt') 229 | sol = opti.solve() 230 | return sol.value(V[0]), sol.value(Omega[0]) 231 | 232 | # Simulation setup 233 | x_traj, y_traj, theta_traj = [], [], [] 234 | x, y, theta = 0, 0, 0 # Initial state 235 | 236 | # Run MPC loop 237 | for _ in range(max_iters): 238 | v_opt, omega_opt = solve_mpc(x, y, theta) # Solve MPC 239 | 240 | # # Apply first control input with small perturbation if near obstacle 241 | # if np.linalg.norm([x - obs_x, y - obs_y]) < obs_r + safety_margin + 0.2 or \ 242 | # np.linalg.norm([x - obs2_x, y - obs2_y]) < obs2_r + safety_margin + 0.2: 243 | # omega_opt += np.random.uniform(-0.1, 0.1) # Small perturbation to escape local minima 244 | 245 | x += v_opt * np.cos(theta) * T 246 | y += v_opt * np.sin(theta) * T 247 | theta += omega_opt * T 248 | 249 | # Store trajectory 250 | x_traj.append(x) 251 | y_traj.append(y) 252 | theta_traj.append(theta) 253 | 254 | # Stop if close to goal 255 | if np.linalg.norm([x - goal_x, y - goal_y]) < reached_goal_threshold: 256 | break 257 | 258 | # Plot results 259 | fig, (ax, ax2) = plt.subplots(1, 2, figsize=(8, 4)) 260 | # fig, ax = plt.subplots(figsize=(6, 6)) 261 | ax2.set_title("Sensor Display") 262 | ax.scatter([0, goal_x], [0, goal_y], color='red', label="Start/Goal") 263 | ax.scatter(obs_x, obs_y, color='black', marker='x', label="Obstacle") 264 | ax.scatter(obs2_x, obs2_y, color='black', marker='x', label="Obstacle") 265 | for obstacle in obstacles: 266 | circle = plt.Circle((obstacle[0], obstacle[1]), obstacle[2], color='gray', alpha=0.5) 267 | ax.add_patch(circle) 268 | ax.set_xlim(-1, 6) 269 | ax.set_ylim(-1, 6) 270 | ax.set_xlabel("X Position") 271 | ax.set_ylabel("Y Position") 272 | ax.legend() 273 | ax.grid() 274 | ax.set_title("MPC-CBF for Unicycle Model with Obstacles") 275 | 276 | # Plot robot trajectory 277 | robot, = ax.plot([], [], 'bo-', markersize=5, label="Robot") 278 | arrow = ax.quiver([], [], [], [], scale=10, color='blue') 279 | 280 | def update(frame): 281 | robot.set_data(x_traj[:frame+1], y_traj[:frame+1]) 282 | arrow.set_offsets([[x_traj[frame], y_traj[frame]]]) 283 | arrow.set_UVC([np.cos(theta_traj[frame])], [np.sin(theta_traj[frame])]) 284 | # calling the sensor simulation 285 | sensor_output, obstacle_clusters = simulate_lidar(x_traj[frame], y_traj[frame], theta_traj[frame]) 286 | ax2.clear() 287 | # plot red dot at x_traj[frame], y_traj[frame] 288 | ax2.scatter(sensor_output.shape[1]//2, sensor_output.shape[0]//2, color='red') 289 | # plot clusters 290 | for cluster in obstacle_clusters: 291 | center_x, center_y, radius = cluster 292 | circle = plt.Circle((center_x, center_y), radius, color='red', alpha=0.5) 293 | ax2.add_patch(circle) 294 | ax2.set_title("Sensor Display") 295 | ax2.imshow(sensor_output, cmap='binary') 296 | return robot, arrow 297 | 298 | ani = FuncAnimation(fig, update, frames=len(x_traj), interval=200, blit=False, repeat=False) 299 | plt.show() 300 | -------------------------------------------------------------------------------- /mpc_dc_fov.py: -------------------------------------------------------------------------------- 1 | import casadi as ca 2 | import numpy as np 3 | import matplotlib.pyplot as plt 4 | from matplotlib.animation import FuncAnimation 5 | import cv2 6 | from sklearn.cluster import DBSCAN, KMeans 7 | 8 | # Parameters 9 | T = 0.05 # Time step 10 | N = 10 # Prediction horizon (increased for better planning) 11 | obs_x, obs_y, obs_r = 3.5, 3, 0.75 # Obstacle 1 (center and radius) 12 | obs2_x, obs2_y, obs2_r = 1, 1.0, 0.5 # Obstacle 2 (center and radius) 13 | 14 | # Store obstacles in a list 15 | obstacles = [(obs_x, obs_y, obs_r), (obs2_x, obs2_y, obs2_r), (1, 4.5, 1.0), (3, 0, 0.5)] 16 | 17 | reached_goal_threshold = 0.1 # Threshold to stop the simulation 18 | 19 | goal_x, goal_y = 5, 5 # Goal position 20 | max_iters = 100 # Max iterations to reach the goal 21 | safety_margin = 0.1 # Safety margin for obstacle avoidance 22 | 23 | # sensor parameters 24 | fov = 120 # Field of view in degrees 25 | sensor_range = 2 # Sensor range in meters 26 | num_ray = 200 # Resolution of the sensor 27 | resolution = 0.01 # Resolution of the sensor in meters 28 | 29 | def cluster_black_pixels(image, method="dbscan", eps=5, min_samples=5, n_clusters=3): 30 | gray = image[:, :, 0] # Use only the first channel 31 | black_pixels = np.column_stack(np.where(gray == 0)) 32 | 33 | if len(black_pixels) == 0: 34 | return [] # No black pixels found 35 | 36 | if method.lower() == "dbscan": 37 | clustering = DBSCAN(eps=eps, min_samples=min_samples) 38 | labels = clustering.fit_predict(black_pixels) 39 | 40 | elif method.lower() == "kmeans": 41 | if len(black_pixels) < n_clusters: 42 | return [] # Not enough points for K-Means 43 | clustering = KMeans(n_clusters=n_clusters, random_state=42, n_init=10) 44 | labels = clustering.fit_predict(black_pixels) 45 | 46 | else: 47 | raise ValueError("Invalid method. Choose 'dbscan' or 'kmeans'.") 48 | 49 | clusters = [] 50 | unique_labels = set(labels) - {-1} # Remove noise (-1) from DBSCAN 51 | 52 | for label in unique_labels: 53 | cluster_points = black_pixels[labels == label] # Get points belonging to the cluster 54 | 55 | if len(cluster_points) < min_samples: 56 | continue # Skip tiny clusters 57 | 58 | # Fit a minimum enclosing circle 59 | (x, y), radius = cv2.minEnclosingCircle(cluster_points.astype(np.float32)) 60 | clusters.append((int(x), int(y), int(radius))) 61 | 62 | return clusters 63 | 64 | def isObstacle(world_x, world_y): 65 | for obs_x, obs_y, obs_r in obstacles: 66 | if (world_x - obs_x)**2 + (world_y - obs_y)**2 <= obs_r**2: 67 | return True 68 | return False 69 | 70 | # Bresenham's Line Algorithm 71 | def bresenham(x1, y1, x2, y2): 72 | points = [] 73 | dx = abs(x2 - x1) 74 | dy = abs(y2 - y1) 75 | sx = 1 if x1 < x2 else -1 76 | sy = 1 if y1 < y2 else -1 77 | err = dx - dy 78 | 79 | while True: 80 | points.append((x1, y1)) 81 | if x1 == x2 and y1 == y2: 82 | break 83 | e2 = 2 * err 84 | if e2 > -dy: 85 | err -= dy 86 | x1 += sx 87 | if e2 < dx: 88 | err += dx 89 | y1 += sy 90 | 91 | return points 92 | 93 | def simulate_lidar(x, y, theta): 94 | # Initialize sensor window with resolution 95 | grid_size = int(6/resolution) 96 | display_window = np.zeros((grid_size, grid_size)) 97 | 98 | # Calculate center of grid (robot position) 99 | grid_center_x = grid_size // 2 100 | grid_center_y = grid_size // 2 101 | 102 | # Fill with background color (gray) 103 | display_window.fill(0.5) 104 | 105 | # For each ray in the field of view 106 | half_fov = fov / 2 107 | angle_increment = fov / (num_ray-1) 108 | 109 | for i in range(num_ray): 110 | # Calculate ray angle 111 | ray_angle = theta - np.deg2rad(half_fov) + np.deg2rad(i * angle_increment) 112 | 113 | # Calculate endpoint of ray based on sensor range (in grid coordinates) 114 | end_x = int(grid_center_x + (sensor_range/resolution) * np.cos(ray_angle)) 115 | end_y = int(grid_center_y + (sensor_range/resolution) * np.sin(ray_angle)) 116 | 117 | # Clamp to grid boundaries 118 | end_x = max(0, min(end_x, grid_size-1)) 119 | end_y = max(0, min(end_y, grid_size-1)) 120 | 121 | # Use Bresenham's algorithm to get all points along the ray 122 | ray_points = bresenham(grid_center_x, grid_center_y, end_x, end_y) 123 | 124 | # Mark ray points in the display window 125 | hit_obstacle = False 126 | for point_x, point_y in ray_points: 127 | if 0 <= point_x < grid_size and 0 <= point_y < grid_size: 128 | # Convert grid coordinates to world coordinates 129 | world_x = x + (point_x - grid_center_x) * resolution 130 | world_y = y + (point_y - grid_center_y) * resolution 131 | 132 | if hit_obstacle: 133 | # After hitting an obstacle, mark the rest of the ray black 134 | display_window[point_y, point_x] = 0.0 135 | else: 136 | # Check if point is an obstacle 137 | if isObstacle(world_x, world_y): 138 | display_window[point_y, point_x] = 0.0 # Mark obstacle as black 139 | hit_obstacle = True # Set flag but don't break 140 | else: 141 | display_window[point_y, point_x] = 1.0 # Mark ray path as white 142 | 143 | # Mark robot position 144 | display_window[grid_center_y, grid_center_x] = 1.0 # Robot position as white 145 | 146 | # Convert to 0-255 range for display 147 | display_window = (display_window * 255).astype(np.uint8) 148 | 149 | # Convert to RGB 150 | display_window = np.stack([display_window]*3, axis=-1) 151 | 152 | # mask black pixels 153 | mask = np.all(display_window == [0, 0, 0], axis=-1) 154 | plain_white = np.ones((grid_size, grid_size)) 155 | plain_white = np.stack([plain_white]*3, axis=-1) 156 | plain_white[mask] = [0, 0, 0] 157 | # cluster maksed image 158 | clusters_pixel = cluster_black_pixels(plain_white, method="dbscan", eps=5, min_samples=5) 159 | # cluster using kmeans 160 | # clusters_pixel = cluster_black_pixels(plain_white, method="kmeans", n_clusters=2) 161 | clusters = [None] * len(clusters_pixel) 162 | # convert pixel coordinates to world coordinates and store in clusters 163 | for i, (pixel_x, pixel_y, pixel_radius) in enumerate(clusters_pixel): 164 | world_x = x + (pixel_x - grid_center_x) * resolution 165 | world_y = y + (pixel_y - grid_center_y) * resolution 166 | world_radius = pixel_radius * resolution # Scale the radius 167 | clusters[i] = (world_x, world_y, world_radius) 168 | print(clusters) 169 | 170 | return display_window, clusters_pixel 171 | 172 | 173 | def solve_mpc(x0, y0, theta0): 174 | opti = ca.Opti() 175 | X = opti.variable(N+1) 176 | Y = opti.variable(N+1) 177 | Theta = opti.variable(N+1) 178 | V = opti.variable(N) 179 | Omega = opti.variable(N) 180 | 181 | # System dynamics constraints 182 | for k in range(N): 183 | opti.subject_to(X[k+1] == X[k] + V[k] * ca.cos(Theta[k]) * T) 184 | opti.subject_to(Y[k+1] == Y[k] + V[k] * ca.sin(Theta[k]) * T) 185 | opti.subject_to(Theta[k+1] == Theta[k] + Omega[k] * T) 186 | 187 | # Initial conditions 188 | opti.subject_to(X[0] == x0) 189 | opti.subject_to(Y[0] == y0) 190 | opti.subject_to(Theta[0] == theta0) 191 | 192 | # Obstacle avoidance constraints with safety margin 193 | for k in range(N+1): 194 | opti.subject_to((X[k] - obs_x)**2 + (Y[k] - obs_y)**2 >= (obs_r + safety_margin)**2) 195 | opti.subject_to((X[k] - obs2_x)**2 + (Y[k] - obs2_y)**2 >= (obs2_r + safety_margin)**2) 196 | 197 | # Control constraints 198 | opti.subject_to(opti.bounded(0, V, 2)) # 0 ≤ v ≤ 2 199 | opti.subject_to(opti.bounded(-1, Omega, 1)) # -1 ≤ ω ≤ 1 200 | 201 | # Cost function: Higher weight on reaching goal 202 | goal_weight = 150 # Increased goal weight 203 | effort_weight = 0.5 # Control effort weight 204 | cost = effort_weight * (ca.sumsqr(V) + ca.sumsqr(Omega)) + goal_weight * ((X[N] - goal_x)**2 + (Y[N] - goal_y)**2) 205 | opti.minimize(cost) 206 | 207 | # Solve 208 | opti.solver('ipopt') 209 | sol = opti.solve() 210 | return sol.value(V[0]), sol.value(Omega[0]) 211 | 212 | # Simulation setup 213 | x_traj, y_traj, theta_traj = [], [], [] 214 | x, y, theta = 0, 0, 0 # Initial state 215 | 216 | # Run MPC loop 217 | for _ in range(max_iters): 218 | v_opt, omega_opt = solve_mpc(x, y, theta) # Solve MPC 219 | 220 | # # Apply first control input with small perturbation if near obstacle 221 | # if np.linalg.norm([x - obs_x, y - obs_y]) < obs_r + safety_margin + 0.2 or \ 222 | # np.linalg.norm([x - obs2_x, y - obs2_y]) < obs2_r + safety_margin + 0.2: 223 | # omega_opt += np.random.uniform(-0.1, 0.1) # Small perturbation to escape local minima 224 | 225 | x += v_opt * np.cos(theta) * T 226 | y += v_opt * np.sin(theta) * T 227 | theta += omega_opt * T 228 | 229 | # Store trajectory 230 | x_traj.append(x) 231 | y_traj.append(y) 232 | theta_traj.append(theta) 233 | 234 | # Stop if close to goal 235 | if np.linalg.norm([x - goal_x, y - goal_y]) < reached_goal_threshold: 236 | break 237 | 238 | # Plot results 239 | fig, (ax, ax2) = plt.subplots(1, 2, figsize=(8, 4)) 240 | # fig, ax = plt.subplots(figsize=(6, 6)) 241 | ax2.set_title("Sensor Display") 242 | ax.scatter([0, goal_x], [0, goal_y], color='red', label="Start/Goal") 243 | ax.scatter(obs_x, obs_y, color='black', marker='x', label="Obstacle") 244 | ax.scatter(obs2_x, obs2_y, color='black', marker='x', label="Obstacle") 245 | # Plot circles for all obstacles 246 | for obstacle in obstacles: 247 | circle = plt.Circle((obstacle[0], obstacle[1]), obstacle[2], color='gray', alpha=0.5) 248 | ax.add_patch(circle) 249 | ax.set_xlim(-1, 6) 250 | ax.set_ylim(-1, 6) 251 | ax.set_xlabel("X Position") 252 | ax.set_ylabel("Y Position") 253 | ax.legend() 254 | ax.grid() 255 | ax.set_title("MPC for Unicycle Model with Obstacles") 256 | 257 | # Plot robot trajectory 258 | robot, = ax.plot([], [], 'bo-', markersize=5, label="Robot") 259 | arrow = ax.quiver([], [], [], [], scale=10, color='blue') 260 | 261 | def update(frame): 262 | robot.set_data(x_traj[:frame+1], y_traj[:frame+1]) 263 | arrow.set_offsets([[x_traj[frame], y_traj[frame]]]) 264 | arrow.set_UVC([np.cos(theta_traj[frame])], [np.sin(theta_traj[frame])]) 265 | 266 | # calling the sensor simulation 267 | sensor_output, obstacle_clusters = simulate_lidar(x_traj[frame], y_traj[frame], theta_traj[frame]) 268 | ax2.clear() 269 | # plot red dot at x_traj[frame], y_traj[frame] 270 | ax2.scatter(sensor_output.shape[1]//2, sensor_output.shape[0]//2, color='red') 271 | # plot clusters 272 | for cluster in obstacle_clusters: 273 | center_x, center_y, radius = cluster 274 | circle = plt.Circle((center_x, center_y), radius, color='red', alpha=0.5) 275 | ax2.add_patch(circle) 276 | ax2.set_title("Sensor Display") 277 | ax2.imshow(sensor_output, cmap='binary') 278 | return robot, arrow 279 | 280 | ani = FuncAnimation(fig, update, frames=len(x_traj), interval=200, blit=False, repeat=False) 281 | # Save the animation as a video file (MP4 format) 282 | ani.save("mpc_dc_simulation.mp4", writer='ffmpeg', fps=5) 283 | plt.show() 284 | -------------------------------------------------------------------------------- /results/astar_pid_res.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/RahulHKumar/Robot-Planning-and-Control-Tutorial/65139b7eecf738d1e13d50d883a5e2c25bb8a804/results/astar_pid_res.png -------------------------------------------------------------------------------- /results/mpc_dc_simulation.gif: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/RahulHKumar/Robot-Planning-and-Control-Tutorial/65139b7eecf738d1e13d50d883a5e2c25bb8a804/results/mpc_dc_simulation.gif -------------------------------------------------------------------------------- /results/mpccbfres.jpg: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/RahulHKumar/Robot-Planning-and-Control-Tutorial/65139b7eecf738d1e13d50d883a5e2c25bb8a804/results/mpccbfres.jpg -------------------------------------------------------------------------------- /results/sbmp.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/RahulHKumar/Robot-Planning-and-Control-Tutorial/65139b7eecf738d1e13d50d883a5e2c25bb8a804/results/sbmp.png -------------------------------------------------------------------------------- /results/state_estimation.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/RahulHKumar/Robot-Planning-and-Control-Tutorial/65139b7eecf738d1e13d50d883a5e2c25bb8a804/results/state_estimation.png -------------------------------------------------------------------------------- /sampling_planner.py: -------------------------------------------------------------------------------- 1 | import numpy as np 2 | import matplotlib.pyplot as plt 3 | import networkx as nx 4 | import cvxpy as cp 5 | import scipy.linalg 6 | 7 | # Define the Node class for RRT and RRT* 8 | class Node: 9 | def __init__(self, x, y, vx=0, vy=0, parent=None, cost=0.0): 10 | self.x = x 11 | self.y = y 12 | self.vx = vx 13 | self.vy = vy 14 | self.parent = parent 15 | self.cost = cost 16 | 17 | class KinodynamicNode: 18 | def __init__(self, x, y, theta, parent=None, cost=0.0): 19 | self.x = x 20 | self.y = y 21 | self.theta = theta 22 | self.parent = parent 23 | self.cost = cost 24 | 25 | # Simulate unicycle motion 26 | def simulate_unicycle(node, v_x, omega, dt=0.05, steps=10): 27 | x, y, theta = node.x, node.y, node.theta 28 | for _ in range(steps): 29 | x += v_x * np.cos(theta) * dt 30 | y += v_x * np.sin(theta) * dt 31 | theta += omega * dt 32 | return KinodynamicNode(x, y, theta, parent=node, cost=node.cost + steps * dt) 33 | 34 | # Compute Euclidean distance 35 | def distance(n1, n2): 36 | return np.sqrt((n1.x - n2.x)**2 + (n1.y - n2.y)**2) 37 | 38 | # Check collision (simple circle) 39 | def is_collision(node, obstacle_center, obstacle_radius): 40 | return distance(node, Node(*obstacle_center)) <= obstacle_radius 41 | 42 | # Check collision between two points (used in PRM) 43 | def edge_collision(x1, y1, x2, y2, obstacle_center, obstacle_radius): 44 | steps = 10 45 | for i in range(steps + 1): 46 | t = i / steps 47 | x = x1 * (1 - t) + x2 * t 48 | y = y1 * (1 - t) + y2 * t 49 | if distance(Node(x, y), Node(*obstacle_center)) <= obstacle_radius: 50 | return True 51 | return False 52 | 53 | # Extract path 54 | def extract_path(goal): 55 | path = [] 56 | node = goal 57 | while node is not None: 58 | path.append((node.x, node.y)) 59 | node = node.parent 60 | return path[::-1] 61 | 62 | # PRM 63 | def prm(start, goal, n_samples=100, k=3, obstacle_center=(5,5), obstacle_radius=1.0): 64 | samples = [(start.x, start.y), (goal.x, goal.y)] 65 | while len(samples) < n_samples: 66 | x, y = np.random.uniform(0, 10), np.random.uniform(0, 10) 67 | if not is_collision(Node(x, y), obstacle_center, obstacle_radius): 68 | samples.append((x, y)) 69 | G = nx.Graph() 70 | for i, p1 in enumerate(samples): 71 | G.add_node(i, pos=p1) 72 | dists = [((p2[0]-p1[0])**2 + (p2[1]-p1[1])**2, j) for j, p2 in enumerate(samples) if j != i] 73 | dists.sort() 74 | for _, j in dists[:k]: 75 | p2 = samples[j] 76 | if not edge_collision(p1[0], p1[1], p2[0], p2[1], obstacle_center, obstacle_radius): 77 | G.add_edge(i, j, weight=np.linalg.norm(np.array(p1) - np.array(p2))) 78 | try: 79 | path_indices = nx.shortest_path(G, source=0, target=1, weight='weight') 80 | path = [samples[i] for i in path_indices] 81 | except nx.NetworkXNoPath: 82 | print("No path found with PRM") 83 | path = [] 84 | return samples, G, path 85 | 86 | # PRM* 87 | def prm_star(start, goal, n_samples=100, obstacle_center=(5,5), obstacle_radius=1.0): 88 | samples = [(start.x, start.y), (goal.x, goal.y)] 89 | while len(samples) < n_samples: 90 | x, y = np.random.uniform(0, 10), np.random.uniform(0, 10) 91 | if not is_collision(Node(x, y), obstacle_center, obstacle_radius): 92 | samples.append((x, y)) 93 | G = nx.Graph() 94 | n = len(samples) 95 | r = 1.0 * np.sqrt((np.log(n)/n)) * 10 96 | for i, p1 in enumerate(samples): 97 | G.add_node(i, pos=p1) 98 | for j, p2 in enumerate(samples): 99 | if i != j and np.linalg.norm(np.array(p1)-np.array(p2)) <= r: 100 | if not edge_collision(p1[0], p1[1], p2[0], p2[1], obstacle_center, obstacle_radius): 101 | G.add_edge(i, j, weight=np.linalg.norm(np.array(p1) - np.array(p2))) 102 | try: 103 | path_indices = nx.shortest_path(G, source=0, target=1, weight='weight') 104 | path = [samples[i] for i in path_indices] 105 | except nx.NetworkXNoPath: 106 | print("No path found with PRM*") 107 | path = [] 108 | return samples, G, path 109 | 110 | 111 | # RRT algorithm 112 | def rrt(start, goal, max_iters=500, step_size=0.5, obstacle_center=(5,5), obstacle_radius=1.0): 113 | nodes = [start] 114 | for _ in range(max_iters): 115 | rand_x, rand_y = np.random.uniform(0, 10), np.random.uniform(0, 10) 116 | random_node = Node(rand_x, rand_y) 117 | nearest = min(nodes, key=lambda node: distance(node, random_node)) 118 | direction = np.array([random_node.x - nearest.x, random_node.y - nearest.y]) 119 | length = np.linalg.norm(direction) 120 | if length == 0: 121 | continue 122 | direction = (direction / length) * min(step_size, length) 123 | new_x, new_y = nearest.x + direction[0], nearest.y + direction[1] 124 | new_node = Node(new_x, new_y, parent=nearest) 125 | if is_collision(new_node, obstacle_center, obstacle_radius): 126 | continue 127 | nodes.append(new_node) 128 | if distance(new_node, goal) < step_size: 129 | goal.parent = new_node 130 | nodes.append(goal) 131 | break 132 | return nodes 133 | 134 | # RRT* algorithm 135 | def rrt_star(start, goal, max_iters=500, step_size=0.5, search_radius=1.5, obstacle_center=(5,5), obstacle_radius=1.0): 136 | nodes = [start] 137 | for _ in range(max_iters): 138 | rand_x, rand_y = np.random.uniform(0, 10), np.random.uniform(0, 10) 139 | random_node = Node(rand_x, rand_y) 140 | nearest = min(nodes, key=lambda node: distance(node, random_node)) 141 | direction = np.array([random_node.x - nearest.x, random_node.y - nearest.y]) 142 | length = np.linalg.norm(direction) 143 | if length == 0: 144 | continue 145 | direction = (direction / length) * min(step_size, length) 146 | new_x, new_y = nearest.x + direction[0], nearest.y + direction[1] 147 | new_node = Node(new_x, new_y) 148 | 149 | if is_collision(new_node, obstacle_center, obstacle_radius): 150 | continue 151 | 152 | near_nodes = [node for node in nodes if distance(node, new_node) <= search_radius] 153 | min_cost = nearest.cost + distance(nearest, new_node) 154 | best_parent = nearest 155 | 156 | for node in near_nodes: 157 | temp_cost = node.cost + distance(node, new_node) 158 | if temp_cost < min_cost: 159 | min_cost = temp_cost 160 | best_parent = node 161 | 162 | new_node.cost = min_cost 163 | new_node.parent = best_parent 164 | nodes.append(new_node) 165 | 166 | for node in near_nodes: 167 | if new_node.cost + distance(new_node, node) < node.cost: 168 | node.parent = new_node 169 | node.cost = new_node.cost + distance(new_node, node) 170 | 171 | if distance(new_node, goal) < step_size: 172 | goal.parent = new_node 173 | goal.cost = new_node.cost + distance(new_node, goal) 174 | nodes.append(goal) 175 | break 176 | 177 | return nodes 178 | 179 | # Kinodynamic RRT* 180 | # Using unicycle model (Non-linear system) 181 | def kinodynamic_rrt_star(start, goal, max_iters=500, step_size=0.5, obstacle_center=(5,5), obstacle_radius=1.0, 182 | v_range=(0.0, 1.5), omega_range=(-1.0, 1.0)): 183 | nodes = [start] 184 | for _ in range(max_iters): 185 | rand_x, rand_y = np.random.uniform(0, 10), np.random.uniform(0, 10) 186 | rand_theta = np.random.uniform(-np.pi, np.pi) 187 | random_node = KinodynamicNode(rand_x, rand_y, rand_theta) 188 | nearest = min(nodes, key=lambda node: distance(node, random_node)) 189 | v_x = np.random.uniform(v_range[0], v_range[1]) 190 | omega = np.random.uniform(omega_range[0], omega_range[1]) 191 | new_node = simulate_unicycle(nearest, v_x, omega) 192 | if is_collision(new_node, obstacle_center, obstacle_radius): 193 | continue 194 | nodes.append(new_node) 195 | if distance(new_node, goal) < step_size: 196 | goal.parent = new_node 197 | nodes.append(goal) 198 | break 199 | return nodes 200 | 201 | # LQR-RRT* using double integrator model (Linear system) 202 | A = np.array([[1, 0, 0.05, 0], [0, 1, 0, 0.05], [0, 0, 1, 0], [0, 0, 0, 1]]) 203 | B = np.array([[0.00125, 0], [0, 0.00125], [0.05, 0], [0, 0.05]]) 204 | Q = np.diag([5, 5, 0.1, 0.1]) 205 | R = np.diag([0.7, 0.7]) 206 | 207 | def simulate_lqr_riccati(from_node, to_node, dt=0.05, horizon=10): 208 | x0 = np.array([from_node.x, from_node.y, from_node.vx, from_node.vy]) 209 | xg = np.array([to_node.x, to_node.y, to_node.vx, to_node.vy]) 210 | P = scipy.linalg.solve_discrete_are(A, B, Q, R) 211 | K = np.linalg.inv(B.T @ P @ B + R) @ (B.T @ P @ A) 212 | 213 | traj = [] 214 | x = x0.copy() 215 | for _ in range(horizon): 216 | u = -K @ (x - xg) 217 | x = A @ x + B @ u 218 | traj.append(Node(x[0], x[1], x[2], x[3])) 219 | 220 | final_node = traj[-1] 221 | final_node.parent = from_node 222 | return final_node, traj 223 | 224 | def simulate_lqr_cvxpy(from_node, to_node, horizon=10): 225 | x0 = np.array([from_node.x, from_node.y, from_node.vx, from_node.vy]) 226 | xg = np.array([to_node.x, to_node.y, to_node.vx, to_node.vy]) 227 | x = cp.Variable((4, horizon+1)) 228 | u = cp.Variable((2, horizon)) 229 | cost = 0 230 | constraints = [x[:, 0] == x0] 231 | for t in range(horizon): 232 | cost += cp.quad_form(x[:, t] - xg, Q) + cp.quad_form(u[:, t], R) 233 | constraints += [x[:, t+1] == A @ x[:, t] + B @ u[:, t]] 234 | cost += cp.quad_form(x[:, horizon] - xg, Q) 235 | prob = cp.Problem(cp.Minimize(cost), constraints) 236 | prob.solve() 237 | if prob.status != cp.OPTIMAL: 238 | return None, [] 239 | final_state = x.value[:, -1] 240 | final_node = Node(final_state[0], final_state[1], final_state[2], final_state[3], parent=from_node) 241 | traj = [Node(*x.value[:, t][:4]) for t in range(horizon + 1)] 242 | return final_node, traj 243 | 244 | 245 | def lqr_rrt_star(start, goal, max_iters=500, obstacle_center=(5,5), obstacle_radius=1.0, max_dist=0.5, mode='riccati'): 246 | nodes = [start] 247 | for _ in range(max_iters): 248 | rand = Node(np.random.uniform(0, 10), np.random.uniform(0, 10), np.random.uniform(-1, 1), np.random.uniform(-1, 1)) 249 | nearest = min(nodes, key=lambda node: distance(node, rand)) 250 | 251 | dist_to_rand = distance(nearest, rand) 252 | if dist_to_rand > max_dist: 253 | direction = np.array([rand.x - nearest.x, rand.y - nearest.y]) 254 | direction /= np.linalg.norm(direction) 255 | scaled_x = nearest.x + direction[0] * max_dist 256 | scaled_y = nearest.y + direction[1] * max_dist 257 | rand = Node(scaled_x, scaled_y, rand.vx, rand.vy) 258 | 259 | if mode == 'cvxpy': 260 | result = simulate_lqr_cvxpy(nearest, rand) 261 | elif mode == 'riccati': 262 | result = simulate_lqr_riccati(nearest, rand) 263 | else: 264 | raise ValueError("Unknown LQR mode: choose 'cvxpy' or 'riccati'") 265 | 266 | if result is None: 267 | continue 268 | 269 | new_node, _ = result 270 | if distance(new_node, goal) < max_dist: 271 | goal.parent = new_node 272 | nodes.append(goal) 273 | break 274 | if is_collision(new_node, obstacle_center, obstacle_radius): 275 | continue 276 | new_node.parent = nearest 277 | nodes.append(new_node) 278 | return nodes 279 | 280 | # Set the algorithm flag 281 | flag = 'lqr_rrt_star' # Choose: 'rrt', 'rrt_star', 'kinodynamic_rrt_star', 'prm', 'prm_star', 'lqr_rrt_star' 282 | 283 | # Initialize 284 | if flag == 'kinodynamic_rrt_star': 285 | start_node = KinodynamicNode(1, 1, 0) 286 | goal_node = KinodynamicNode(9, 9, 0) 287 | else: 288 | start_node = Node(1, 1, 0, 0) 289 | goal_node = Node(9, 9, 0, 0) 290 | 291 | obstacle_center = (5, 5) 292 | obstacle_radius = 1.0 293 | 294 | # Algorithm dispatch 295 | if flag == 'rrt': 296 | nodes = rrt(start_node, goal_node, obstacle_center=obstacle_center, obstacle_radius=obstacle_radius) 297 | path = extract_path(goal_node) 298 | color = 'c'; label = 'RRT' 299 | elif flag == 'rrt_star': 300 | nodes = rrt_star(start_node, goal_node, obstacle_center=obstacle_center, obstacle_radius=obstacle_radius) 301 | path = extract_path(goal_node) 302 | color = 'b'; label = 'RRT*' 303 | elif flag == 'kinodynamic_rrt_star': 304 | nodes = kinodynamic_rrt_star(start_node, goal_node, obstacle_center=obstacle_center, obstacle_radius=obstacle_radius) 305 | path = extract_path(goal_node) 306 | color = 'm'; label = 'Kinodynamic RRT*' 307 | elif flag == 'prm': 308 | samples, graph, path = prm(start_node, goal_node, obstacle_center=obstacle_center, obstacle_radius=obstacle_radius) 309 | color = 'g'; label = 'PRM' 310 | elif flag == 'prm_star': 311 | samples, graph, path = prm_star(start_node, goal_node, obstacle_center=obstacle_center, obstacle_radius=obstacle_radius) 312 | color = 'brown'; label = 'PRM*' 313 | elif flag == 'lqr_rrt_star': 314 | mode = 'cvxpy' # Choose: 'riccati' or 'cvxpy' 315 | nodes = lqr_rrt_star(start_node, goal_node, obstacle_center=obstacle_center, obstacle_radius=obstacle_radius, mode=mode) 316 | path = extract_path(goal_node) 317 | color = 'orange'; label = 'LQR-RRT*' 318 | 319 | # Plot 320 | plt.figure(figsize=(6, 6)) 321 | plt.xlim(0, 10); plt.ylim(0, 10); plt.grid(True) 322 | plt.gca().add_patch(plt.Circle(obstacle_center, obstacle_radius, color='black', alpha=0.5)) 323 | 324 | if flag in ['rrt', 'rrt_star', 'kinodynamic_rrt_star', 'lqr_rrt_star']: 325 | for node in nodes: 326 | if node.parent: 327 | plt.plot([node.x, node.parent.x], [node.y, node.parent.y], color=color, alpha=0.2) 328 | elif flag in ['prm', 'prm_star']: 329 | pos = nx.get_node_attributes(graph, 'pos') 330 | for edge in graph.edges: 331 | p1, p2 = pos[edge[0]], pos[edge[1]] 332 | plt.plot([p1[0], p2[0]], [p1[1], p2[1]], color, alpha=0.2) 333 | for p in pos.values(): 334 | plt.plot(p[0], p[1], 'go', markersize=2) 335 | 336 | if path: 337 | plt.plot(*zip(*path), color=color, linewidth=2, label=label, marker='*') 338 | 339 | plt.plot(1, 1, 'bo', markersize=8, label='Start') 340 | plt.plot(9, 9, 'ro', markersize=8, label='Goal') 341 | plt.legend() 342 | plt.title(f"Path Planning using {label}") 343 | if flag == 'lqr_rrt_star': 344 | plt.title(f"Path Planning using {label} ({mode})") 345 | plt.show() 346 | -------------------------------------------------------------------------------- /state_estimation.py: -------------------------------------------------------------------------------- 1 | import numpy as np 2 | import matplotlib.pyplot as plt 3 | from matplotlib.animation import FuncAnimation 4 | from IPython import embed 5 | 6 | class Robot: 7 | def __init__(self, radius, mu0, Sigma0, proc_noise_std, obs_noise_std): 8 | # Just for the looks 9 | self.radius = radius 10 | 11 | # Defining parameters for KF algorithm 12 | self.mu0 = mu0 13 | self.Sigma0 = Sigma0 14 | self.proc_noise_std = proc_noise_std 15 | self.obs_noise_std = obs_noise_std 16 | 17 | # Process noise covariance (R) 18 | self.R = np.diag(self.proc_noise_std ** 2) # process noise covariance 19 | 20 | # Observation model (C) 21 | self.C = np.eye(3) 22 | 23 | # Standard deviations for the noise in x, y, and theta (observation or sensor model noise) 24 | # Observation noise covariance (Q) 25 | self.Q = np.diag(self.obs_noise_std ** 2) 26 | 27 | def action_model(self, At, Bt, ut, xt_1): 28 | # xt = At*xt-1 + Bt*ut + wt 29 | # wt is gaussian noise with covariance R (process noise/ actuators noise) 30 | wt = np.random.multivariate_normal(np.zeros(3), self.R) 31 | xt = At @ xt_1 + Bt @ ut + wt 32 | return xt 33 | 34 | def sensor_model(self, Ct, xt): 35 | # zt = Ct*xt + vt 36 | # vt is gaussian noise with covariance Q (sensor noise) 37 | vt = np.random.multivariate_normal(np.zeros(3), self.Q) 38 | zt = Ct @ xt + vt 39 | return zt 40 | 41 | def dynamic_model(self, theta): 42 | # Input: mu_t-1 43 | # At = np.eye(3) 44 | # Bt = np.array([[np.cos(self.mu[2]), 0], [np.sin(self.mu[2]), 0], [0, 1]]) 45 | At = np.eye(3) 46 | Bt = np.array([ 47 | [np.cos(theta), 0], 48 | [np.sin(theta), 0], 49 | [0, 1] 50 | ]) 51 | return At, Bt 52 | 53 | def g_nonlin_motion(self, mu, u): 54 | # mu, u are np vectors of size 3x1 and 2x1 respectively 55 | x, y, theta = mu 56 | v, w = u 57 | x_new = x + v*np.cos(theta) 58 | y_new = y + v*np.sin(theta) 59 | theta_new = theta + w 60 | return np.array([x_new, y_new, theta_new]) 61 | 62 | def G_nonlin_motion(self, mu, u): 63 | # mu, u are np vectors of size 3x1 and 2x1 respectively 64 | x, y, theta = mu 65 | v, w = u 66 | G = np.array([ 67 | [1, 0, -v*np.sin(theta)], 68 | [0, 1, v*np.cos(theta)], 69 | [0, 0, 1] 70 | ]) 71 | return G 72 | 73 | def h_observation(self, mu): 74 | # mu is np vector of size 3x1 75 | return mu 76 | 77 | def H_observation(self): 78 | return np.eye(3) 79 | 80 | def kalman_filter(self, mu, cov, u_t, z_t, At, Bt): 81 | # Prediction step 82 | mu_t_bar = At @ mu + Bt @ u_t 83 | sigma_t_bar = At @ cov @ At.T + self.R 84 | 85 | # Kalman Gain calculation 86 | K = sigma_t_bar @ self.C.T @ np.linalg.inv(self.C @ sigma_t_bar @ self.C.T + self.Q) 87 | 88 | # Update step 89 | mu = mu_t_bar + K @ (z_t - self.C @ mu_t_bar) 90 | sigma = (np.eye(3) - K @ self.C) @ sigma_t_bar 91 | 92 | return mu, sigma 93 | 94 | def extended_kalman_filter(self, mu, cov, u_t, z_t): 95 | # Prediction step 96 | mu_t_bar = self.g_nonlin_motion(mu, u_t) 97 | G = self.G_nonlin_motion(mu, u_t) 98 | sigma_t_bar = G @ cov @ G.T + self.R 99 | 100 | # Observation prediction 101 | h = self.h_observation(mu_t_bar) 102 | H = self.H_observation() 103 | 104 | # Kalman Gain calculation 105 | K = sigma_t_bar @ H.T @ np.linalg.inv(H @ sigma_t_bar @ H.T + self.Q) 106 | 107 | # Update step 108 | mu = mu_t_bar + K @ (z_t - h) 109 | sigma = (np.eye(3) - K @ H) @ sigma_t_bar 110 | 111 | return mu, sigma 112 | 113 | def gnd_truth_dynamic(self, state_gnd, u_t): 114 | # This is only for visualization 115 | At, Bt = self.dynamic_model(state_gnd[2]) 116 | # embed() 117 | xt = At@state_gnd + Bt@u_t 118 | # print(xt) 119 | return xt 120 | 121 | # Defining pre computed trajectory for u=[v,w]'=[linear velocity, angular velocity]' 122 | # such that the robot moves in a circular 123 | def get_trajectory_offline(radius, steps): 124 | u = np.zeros((2, steps)) 125 | for i in range(steps): 126 | u[0, i] = 1.0 # linear velocity 127 | u[1, i] = 1.0 / radius # angular velocity 128 | 129 | return np.array(u) 130 | 131 | def main(): 132 | # Create robot instance 133 | radius_robot = 0.2 134 | initial_state = np.array([0, 0, 0]) # x, y, theta 135 | mu0 = initial_state 136 | sigma0 = np.eye(3) 137 | proc_noise_std = np.array([0.02, 0.02, 0.01]) 138 | obs_noise_std = np.array([0.5, 0.5, 0.2]) 139 | radius_circle = 5.0 140 | robot = Robot(radius_robot, mu0, sigma0, proc_noise_std, obs_noise_std) 141 | 142 | # Gnd state for visualization (not used in KF) (not a distribution) 143 | state_gnd = np.array([0, 0, 0]) 144 | 145 | # Number of steps for animation 146 | steps = 35 147 | 148 | # # Collect positions over time 149 | gnd_truth_x_positions = [] 150 | gnd_truth_y_positions = [] 151 | noisy_x_positions = [] 152 | noisy_y_positions = [] 153 | filtered_x_positions = [] 154 | filtered_y_positions = [] 155 | ekf_filtered_x_positions = [] 156 | ekf_filtered_y_positions = [] 157 | 158 | ctrl_inputs = get_trajectory_offline(radius_circle, steps) 159 | 160 | # Starting Kamaan Filter code 161 | state = initial_state 162 | mu = mu0 163 | mu_ekf = mu0 164 | cov = sigma0 165 | cov_ekf = sigma0 166 | # Simulate for a full circle 167 | for i in range(steps): 168 | u_present = np.transpose(ctrl_inputs[:, i]) 169 | # Get the ground truth state (for visualization) 170 | state_gnd = robot.gnd_truth_dynamic(np.transpose(state_gnd), u_present) 171 | gnd_truth_x, ground_truth_y, _ = state_gnd 172 | gnd_truth_x_positions.append(gnd_truth_x) 173 | gnd_truth_y_positions.append(ground_truth_y) 174 | 175 | # Now, call action model to get the next state 176 | At, Bt = robot.dynamic_model(state[2]) 177 | state = robot.action_model(At, Bt, u_present, state) 178 | # Gettting the sensor reading 179 | z_t = robot.sensor_model(robot.C, state) 180 | x_noisy, y_noisy, _ = z_t 181 | noisy_x_positions.append(x_noisy) 182 | noisy_y_positions.append(y_noisy) 183 | 184 | # Now, call Kalman Filter 185 | mu, cov = robot.kalman_filter(mu, cov, u_present, z_t, At, Bt) 186 | filtered_x, filtered_y, _ = mu 187 | # print("filtered_x, filtered_y at step ", i, " : ", filtered_x, filtered_y) 188 | filtered_x_positions.append(filtered_x) 189 | filtered_y_positions.append(filtered_y) 190 | 191 | # Similarly, let's implement EKF 192 | mu_ekf, cov_ekf = robot.extended_kalman_filter(mu_ekf, cov_ekf, u_present, z_t) 193 | ekf_filtered_x, ekf_filtered_y, _ = mu_ekf 194 | ekf_filtered_x_positions.append(ekf_filtered_x) 195 | ekf_filtered_y_positions.append(ekf_filtered_y) 196 | 197 | 198 | # Create the figure and axis 199 | fig, ax = plt.subplots(figsize=(5, 5)) 200 | ax.set_xlim(-10, 10) 201 | ax.set_ylim(-5, 15) 202 | ax.set_title('Robot Circular Motion Simulation') 203 | ax.set_xlabel('X Position') 204 | ax.set_ylabel('Y Position') 205 | ax.grid(True) 206 | ax.axis('equal') 207 | 208 | # Create line and scatter plot objects 209 | ideal_line, = ax.plot([], [], 'b-', label='Ideal Path') 210 | noisy_scatter, = ax.plot([], [], 'r-', label='Noisy Sensor Readings') 211 | filtered_line, = ax.plot([], [], 'g-', label='KF Filtered Path') 212 | ekf_filtered_line, = ax.plot([], [], 'm', label='EKF Filtered Path') 213 | ax.legend() 214 | 215 | # Animation update function 216 | def update(frame): 217 | # Update ideal path 218 | ideal_line.set_data(gnd_truth_x_positions[:frame+1], gnd_truth_y_positions[:frame+1]) 219 | 220 | # Update noisy scatter points 221 | noisy_scatter.set_data(noisy_x_positions[:frame+1], noisy_y_positions[:frame+1]) 222 | 223 | # Update filtered path 224 | filtered_line.set_data(filtered_x_positions[:frame+1], filtered_y_positions[:frame+1]) 225 | 226 | # Update ekf filtered path 227 | ekf_filtered_line.set_data(ekf_filtered_x_positions[:frame+1], ekf_filtered_y_positions[:frame+1]) 228 | 229 | return ideal_line, noisy_scatter, filtered_line, ekf_filtered_line 230 | 231 | # Create animation 232 | anim = FuncAnimation(fig, update, frames=steps, 233 | interval=200, # 1 s between frames 234 | blit=True, 235 | repeat=False) 236 | 237 | plt.show() 238 | 239 | # Run the main function 240 | if __name__ == "__main__": 241 | main() --------------------------------------------------------------------------------