├── .gitignore ├── LICENSE ├── MPC_linear_tracking.py ├── PF_range.py ├── README.md ├── UKF_range_bearing.py ├── UT_example.py ├── ackermann_kinematic.py ├── control_approx_linearization.py ├── dead_recknoning_cart.py ├── diffdrive_GNSS_EKF.py ├── diffdrive_control_mpc.py ├── diffdrive_control_mpc_alternate.py ├── diffdrive_kinematic.py ├── dynamic_extension_tracking.py ├── fws_beacons_ekf.py ├── fws_beacons_observer.py ├── fws_kinematic.py ├── kalman_filter_cart.py ├── mobotpy ├── __init__.py ├── graphics.py ├── integration.py └── models.py ├── oneD_combined_control.py ├── oneD_discrete_control.py ├── oneD_dynamic.py ├── oneD_dynamic_control.py ├── oneD_dynamic_observer.py ├── oneD_integral_control.py ├── oneD_kinematic.py ├── oneD_kinematic_control.py ├── recursive_least_squares.py ├── tricycle_kinematic.py ├── unicycle_dynamic.py └── vanilla_SLAM.py /.gitignore: -------------------------------------------------------------------------------- 1 | .DS_Store 2 | *.code-workspace 3 | *.pyc 4 | *.venv 5 | *.vscode -------------------------------------------------------------------------------- /LICENSE: -------------------------------------------------------------------------------- 1 | MIT License 2 | 3 | Copyright (c) 2021 Joshua A. Marshall 4 | 5 | Permission is hereby granted, free of charge, to any person obtaining a copy 6 | of this software and associated documentation files (the "Software"), to deal 7 | in the Software without restriction, including without limitation the rights 8 | to use, copy, modify, merge, publish, distribute, sublicense, and/or sell 9 | copies of the Software, and to permit persons to whom the Software is 10 | furnished to do so, subject to the following conditions: 11 | 12 | The above copyright notice and this permission notice shall be included in all 13 | copies or substantial portions of the Software. 14 | 15 | THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 16 | IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 17 | FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE 18 | AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER 19 | LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, 20 | OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE 21 | SOFTWARE. 22 | -------------------------------------------------------------------------------- /MPC_linear_tracking.py: -------------------------------------------------------------------------------- 1 | """ 2 | Example MPC_linear_tracking.py 3 | Author: Joshua A. Marshall 4 | GitHub: https://github.com/botprof/agv-examples 5 | """ 6 | 7 | # %% 8 | # SIMULATION SETUP 9 | 10 | import numpy as np 11 | import matplotlib.pyplot as plt 12 | from scipy import signal 13 | from numpy.linalg import matrix_power 14 | from numpy.linalg import inv 15 | 16 | # Set the simulation time [s] and the sample period [s] 17 | SIM_TIME = 30.0 18 | T = 0.1 19 | 20 | # Create an array of time values [s] 21 | t = np.arange(0.0, SIM_TIME, T) 22 | N = np.size(t) 23 | 24 | # %% 25 | # VEHICLE MODELS 26 | 27 | # Vehicle mass [kg] 28 | m = 1.0 29 | 30 | # Discrete time vehicle model 31 | F = np.array([[1, T], [0, 1]]) 32 | G = np.array([[0], [T / m]]) 33 | n = G.shape[0] 34 | 35 | # Continuous time vehicle model (for full-state feedback) 36 | A = np.array([[0, 1], [0, 0]]) 37 | B = np.array([[0], [1 / m]]) 38 | 39 | # %% 40 | # UNCONSTRAINED MPC CONTROLLER DESIGN 41 | 42 | # Lookahead time steps 43 | p = 100 44 | 45 | # Construct the matrices M and L 46 | L = np.zeros((n * p, n)) 47 | M = np.zeros((n * p, p)) 48 | for i in range(0, p): 49 | L[n * i : n * i + n, 0:n] = matrix_power(F, i + 1) 50 | for j in range(0, p - i): 51 | M[n * (p - i) - n : n * (p - i), j : j + 1] = matrix_power(F, p - i - j - 1) @ G 52 | 53 | # Decide on state and input cost matrices 54 | smallQ = np.array([[2.0, 0.0], [0.0, 1.0]]) 55 | Q = 1.0 * np.kron(np.eye(p), smallQ) 56 | R = 0.1 * np.eye(p) 57 | 58 | # Because the system is LTI we can pre-compute the gain matrix 59 | K_MPC = inv(M.T @ Q @ M + R) @ M.T @ Q 60 | 61 | # %% 62 | # FULL-STATE FEEDBACK CONTROLLER DESIGN 63 | 64 | # Choose some poles for the FSF controller 65 | poles = np.array([-1.0, -2.0]) 66 | 67 | # Find the controller gain to place the poles at our chosen locations 68 | K_FSF = signal.place_poles(A, B, poles) 69 | 70 | # %% 71 | # SIMULATE THE CLOSED-LOOP SYSTEMS 72 | 73 | # Set the desired trajectory to take a step mid way through the trajectory 74 | x_d = np.zeros((2, N + p)) 75 | for k in range(int(N / 2), N + p): 76 | x_d[0, k] = 10 77 | x_d[1, k] = 0 78 | 79 | # Set up some more arrays for MPC and FSF 80 | x_MPC = np.zeros((n, N)) 81 | u_MPC = np.zeros((1, N)) 82 | xi_d = np.zeros(n * p) 83 | x_FSF = np.zeros((n, N)) 84 | u_FSF = np.zeros((1, N)) 85 | 86 | # Set the initial conditions 87 | x_MPC[0, 0] = 3 88 | x_MPC[1, 0] = 0 89 | x_FSF[0, 0] = x_MPC[0, 0] 90 | x_FSF[1, 0] = x_MPC[1, 0] 91 | 92 | # Simulate the the closed-loop system with MPC 93 | for k in range(1, N): 94 | x_MPC[:, k] = F @ x_MPC[:, k - 1] + G @ u_MPC[:, k - 1] 95 | for j in range(0, p): 96 | xi_d[n * j : n * j + n] = x_d[:, k + j] 97 | u = K_MPC @ (xi_d - L @ x_MPC[:, k - 1]) 98 | u_MPC[:, k] = u[0] 99 | 100 | # Simulate the closed-loop system with FSF 101 | for k in range(1, N): 102 | x_FSF[:, k] = F @ x_FSF[:, k - 1] + G @ u_FSF[:, k - 1] 103 | u_FSF[:, k] = -K_FSF.gain_matrix @ (x_FSF[:, k - 1] - x_d[:, k - 1]) 104 | 105 | # %% 106 | # PLOT THE RESULTS 107 | 108 | # Change some plot settings (optional) 109 | plt.rc("text", usetex=True) 110 | plt.rc("text.latex", preamble=r"\usepackage{cmbright,amsmath}") 111 | plt.rc("savefig", format="pdf") 112 | plt.rc("savefig", bbox="tight") 113 | 114 | fig1 = plt.figure(1) 115 | ax1a = plt.subplot(311) 116 | plt.plot(t, x_d[0, 0:N], "C2--") 117 | plt.plot(t, x_MPC[0, :], "C0") 118 | plt.plot(t, x_FSF[0, :], "C1") 119 | plt.grid(color="0.95") 120 | plt.ylabel(r"$x_1$ [m]") 121 | plt.legend(["Desired", "MPC", "Full-state feedback"], loc="lower right") 122 | plt.setp(ax1a, xticklabels=[]) 123 | ax1b = plt.subplot(312) 124 | plt.plot(t, x_d[1, 0:N], "C2--") 125 | plt.plot(t, x_MPC[1, :], "C0") 126 | plt.plot(t, x_FSF[1, :], "C1") 127 | plt.grid(color="0.95") 128 | plt.ylabel(r"$x_2$ [m/s]") 129 | plt.setp(ax1b, xticklabels=[]) 130 | ax1c = plt.subplot(313) 131 | plt.plot(t, u_MPC[0, :], "C0") 132 | plt.plot(t, u_FSF[0, :], "C1") 133 | plt.grid(color="0.95") 134 | plt.ylabel(r"$u$ [N]") 135 | plt.xlabel(r"$t$ [s]") 136 | 137 | # Save the plot 138 | # plt.savefig("../agv-book/figs/ch4/MPC_linear_tracking_fig1.pdf") 139 | 140 | # Show the plots 141 | plt.show() 142 | -------------------------------------------------------------------------------- /PF_range.py: -------------------------------------------------------------------------------- 1 | """ 2 | Example PF_range.py 3 | Author: Joshua A. Marshall 4 | GitHub: https://github.com/botprof/agv-examples 5 | """ 6 | 7 | # %% 8 | # SIMULATION SETUP 9 | 10 | import numpy as np 11 | import matplotlib.pyplot as plt 12 | from mobotpy.integration import rk_four 13 | from mobotpy.models import DiffDrive 14 | from scipy.stats import chi2 15 | 16 | # Set the simulation time [s] and the sample period [s] 17 | SIM_TIME = 30.0 18 | T = 0.5 19 | 20 | # Create an array of time values [s] 21 | t = np.arange(0.0, SIM_TIME, T) 22 | N = np.size(t) 23 | 24 | # %% 25 | # VEHICLE SETUP 26 | 27 | # Set the track length of the vehicle [m] 28 | ELL = 1.0 29 | 30 | # Create a vehicle object of type DiffDrive 31 | vehicle = DiffDrive(ELL) 32 | 33 | # Uncertainty in wheel speed measurements [m/s] 34 | SIGMA_SPEED = 0.01 35 | 36 | # Set the range [m] sensor uncertainty 37 | SIGMA_RANGE = 0.3 38 | 39 | # %% 40 | # DEAD RECKONING EXAMPLE 41 | 42 | # Set the number of particles to use 43 | M = 1000 44 | 45 | # Create an array of particles for each time index 46 | x_pf = np.zeros((3, M, N)) 47 | 48 | # Set the covariance matrices 49 | Q = np.diag([SIGMA_SPEED**2, SIGMA_SPEED**2]) 50 | 51 | # Initialize the vehicle's true state 52 | x = np.zeros((3, N)) 53 | 54 | # Initialize a estimated pose estimate 55 | x_hat = np.zeros((3, N)) 56 | 57 | # Initialize a covariance matrix 58 | P_hat = np.zeros((3, 3, N)) 59 | 60 | # Set the initial process covariance 61 | P_hat[:, :, 0] = np.diag(np.square([0.1, 0.1, 0.01])) 62 | 63 | # Initialize the first particles on the basis of the initial uncertainty 64 | for i in range(1, M): 65 | x_pf[:, i, 0] = x_hat[:, 0] + np.sqrt(P_hat[:, :, 0]) @ np.random.standard_normal(3) 66 | 67 | # Initialize the first particles on a uniform distribution over the space 68 | # for i in range(1, M): 69 | # x_pf[:, i, 0] = 100 * np.random.uniform(-1, 1, 3) 70 | 71 | for i in range(1, N): 72 | 73 | # Compute some inputs (i.e., drive around) 74 | v = np.array([3.95, 4.05]) 75 | 76 | # Run the vehicle motion model 77 | x[:, i] = rk_four(vehicle.f, x[:, i - 1], v, T) 78 | 79 | # Propagate each particle through the motion model 80 | for j in range(0, M): 81 | 82 | # Model the proprioceptive sensors (i.e., speed and turning rate) 83 | v_m = v + np.sqrt(Q) @ np.random.standard_normal(2) 84 | 85 | # Propagate each particle 86 | x_pf[:, j, i] = rk_four(vehicle.f, x_pf[:, j, i - 1], v_m, T) 87 | 88 | # Plot the results of the dead reckoning example 89 | plt.figure(1) 90 | plt.plot(x_pf[0, :, 0], x_pf[1, :, 0], ".", label="Particles", alpha=0.2) 91 | for k in range(1, N, 1): 92 | plt.plot(x_pf[0, :, k], x_pf[1, :, k], ".", alpha=0.2) 93 | plt.plot(x[0, :], x[1, :], "C0", label="Actual path") 94 | plt.axis("equal") 95 | plt.xlabel("$x$ [m]") 96 | plt.ylabel("$y$ [m]") 97 | plt.legend() 98 | plt.show() 99 | 100 | # %% 101 | # CREATE A MAP OF FEATURES 102 | 103 | # Set the number of features in the map 104 | m = 50 105 | 106 | # Set the size [m] of a square map 107 | D_MAP = 100 108 | 109 | # Create a map of randomly placed feature locations 110 | f_map = np.zeros((2, m)) 111 | for i in range(0, m): 112 | f_map[:, i] = D_MAP * (2.0 * np.random.rand(2) - 1.0) 113 | 114 | plt.figure(2) 115 | plt.plot(f_map[0, :], f_map[1, :], "C2*", label="Feature") 116 | plt.axis("equal") 117 | plt.xlabel("$x$ [m]") 118 | plt.ylabel("$y$ [m]") 119 | plt.legend() 120 | plt.show() 121 | 122 | # %% 123 | # FUNCTION TO MODEL RANGE TO FEATURES 124 | 125 | 126 | def range_sensor(x, f_map, R, R_MAX, R_MIN): 127 | """Function to model the range sensor.""" 128 | 129 | # Find the indices of features that are within range (R_MIN, R_MAX) 130 | a = np.array([]) 131 | for i in range(0, m): 132 | r = np.sqrt((f_map[0, i] - x[0]) ** 2 + (f_map[1, i] - x[1]) ** 2) 133 | if np.all([r < R_MAX, r > R_MIN]): 134 | a = np.append(a, i) 135 | 136 | # Simulate for each time 137 | if np.shape(a)[0] > 0: 138 | 139 | # Assign the size of the output 140 | k = np.shape(a)[0] 141 | y = np.zeros(k) 142 | 143 | # Compute the range and bearing to all features (plus sensor noise) 144 | for i in range(0, k): 145 | 146 | # Range measurement [m] 147 | y[i] = ( 148 | np.sqrt( 149 | (f_map[0, int(a[i])] - x[0]) ** 2 150 | + (f_map[1, int(a[i])] - x[1]) ** 2 151 | ) 152 | + np.sqrt(R[0, 0]) * np.random.standard_normal() 153 | ) 154 | 155 | else: 156 | 157 | # No features were found within the sensing range 158 | y = np.array([]) 159 | 160 | # Return the measurements and indices map 161 | return y, a 162 | 163 | 164 | # %% 165 | # PARTICLE FILTER FUNCTIONS 166 | 167 | 168 | def pf_resample(x_pf, x_likelihood): 169 | """Function to resample particles.""" 170 | 171 | # Initialize a set of output particles 172 | x_pf_resampled = np.zeros((3, M)) 173 | 174 | # Do the resampling (one way) 175 | indices = np.searchsorted(np.cumsum(x_likelihood), np.random.random_sample(M)) 176 | for j in range(0, M): 177 | x_pf_resampled[:, j] = x_pf[:, indices[j]] 178 | 179 | # Return the resampled particles 180 | return x_pf_resampled 181 | 182 | 183 | def diffdrive_pf(x_pf, v, y, a, f_map, Q, R, T): 184 | """Particle filter for differential drive vehicle function.""" 185 | 186 | # Find the number of observed features 187 | m_k = a.shape[0] 188 | 189 | # Initialize the output 190 | x_pf_new = np.zeros((3, M)) 191 | 192 | # Propagate the particles through the vehicle model (i.e., a priori step) 193 | for j in range(0, M): 194 | 195 | # Model the wheel speed measurements 196 | v_m = v + np.sqrt(Q) @ np.random.standard_normal(2) 197 | 198 | # Propagate each particle 199 | x_pf_new[:, j] = rk_four(vehicle.f, x_pf[:, j], v_m, T) 200 | 201 | # Set likelihoods all equal to start the a posteriori step 202 | x_likelihood = 1.0 / M * np.ones(M) 203 | 204 | # Compute the relative likelihood 205 | if m_k > 1: 206 | 207 | # Set up some arrays 208 | y_hat = np.zeros((m_k, M)) 209 | y_dif = np.zeros(m_k) 210 | 211 | # Compute some needed matrices 212 | R_inv = np.linalg.inv(np.kron(np.identity(m_k), R)) 213 | R_det = np.linalg.det(np.kron(np.identity(m_k), R)) 214 | 215 | for j in range(0, M): 216 | 217 | # For each visible beacon find the expected measurement 218 | for i in range(0, m_k): 219 | y_hat[i, j] = np.sqrt( 220 | (f_map[0, int(a[i])] - x_pf_new[0, j]) ** 2 221 | + (f_map[1, int(a[i])] - x_pf_new[1, j]) ** 2 222 | ) 223 | 224 | # Compute the relative likelihoods 225 | y_dif = y - y_hat[:, j] 226 | x_likelihood[j] = ( 227 | 1.0 228 | / ((2.0 * np.pi) ** (k / 2) * np.sqrt(R_det)) 229 | * np.exp(-0.5 * y_dif.T @ R_inv @ y_dif) 230 | ) 231 | 232 | # Normalize the likelihoods 233 | x_likelihood /= np.sum(x_likelihood) 234 | 235 | # Generate a set of a posteriori particles by re-sampling on the basis of the likelihoods 236 | x_pf_new = pf_resample(x_pf_new, x_likelihood) 237 | 238 | return x_pf_new 239 | 240 | 241 | # %% 242 | # RUN THE PARTICLE FILTER SIMULATION 243 | 244 | # Initialize some arrays 245 | x_pf = np.zeros((3, M, N)) 246 | x_hat = np.zeros((3, N)) 247 | P_hat = np.zeros((3, 3, N)) 248 | 249 | # Initialize the initial guess to a location different from the actual location 250 | x_hat[:, 0] = x[:, 0] + np.array([0, 5.0, 0.1]) 251 | 252 | # Set some initial conditions 253 | P_hat[:, :, 0] = np.diag(np.square([5.0, 5.0, 0.1])) 254 | 255 | # Set the covariance matrices 256 | Q = np.diag([SIGMA_SPEED**2, SIGMA_SPEED**2]) 257 | 258 | # Set sensor range 259 | R_MAX = 25.0 260 | R_MIN = 1.0 261 | 262 | # Set the range and bearing covariance 263 | R = np.diag([SIGMA_RANGE**2]) 264 | 265 | # Initialize the first particles on the basis of the initial uncertainty 266 | for i in range(1, M): 267 | x_pf[:, i, 0] = x_hat[:, 0] + np.sqrt(P_hat[:, :, 0]) @ np.random.standard_normal(3) 268 | 269 | # Initialize the first particles on the basis of the initial uncertainty 270 | # for i in range(1, M): 271 | # x_pf[:, i, 0] = 100 * np.random.uniform(-1, 1, 3) 272 | 273 | # Simulate for each time 274 | for i in range(1, N): 275 | 276 | # Compute some inputs (i.e., drive around) 277 | v = np.array([3.95, 4.05]) 278 | 279 | # Run the vehicle motion model 280 | x[:, i] = rk_four(vehicle.f, x[:, i - 1], v, T) 281 | 282 | # Run the range and bearing sensor model 283 | y_m, a = range_sensor(x[:, i], f_map, R, R_MAX, R_MIN) 284 | 285 | # Run the particle filter 286 | x_pf[:, :, i] = diffdrive_pf(x_pf[:, :, i - 1], v, y_m, a, f_map, Q, R, T) 287 | 288 | # %% 289 | # PLOT THE SIMULATION OUTPUTS 290 | 291 | # Change some plot settings (optional) 292 | plt.rc("text", usetex=True) 293 | plt.rc("text.latex", preamble=r"\usepackage{cmbright,amsmath,bm}") 294 | plt.rc("savefig", format="pdf") 295 | plt.rc("savefig", bbox="tight") 296 | 297 | # Plot the results of the particle filter simulation 298 | plt.figure(3) 299 | plt.plot(x_pf[0, :, 0], x_pf[1, :, 0], ".", label="Particles", alpha=0.2) 300 | for k in range(1, N, 1): 301 | plt.plot(x_pf[0, :, k], x_pf[1, :, k], ".", alpha=0.2) 302 | plt.plot(x[0, :], x[1, :], "C0", label="Actual path") 303 | plt.plot(f_map[0, :], f_map[1, :], "C2*", label="Feature") 304 | plt.axis("equal") 305 | plt.xlabel("$x$ [m]") 306 | plt.ylabel("$y$ [m]") 307 | plt.legend() 308 | plt.show() 309 | 310 | # Compute the mean errors and estimated covariance bounds 311 | for i in range(0, N): 312 | x_hat[0, i] = np.mean(x_pf[0, :, i]) 313 | x_hat[1, i] = np.mean(x_pf[1, :, i]) 314 | x_hat[2, i] = np.mean(x_pf[2, :, i]) 315 | 316 | for i in range(0, N): 317 | P_hat[:, :, i] = np.cov(x_pf[:, :, i]) 318 | 319 | # Find the scaling factors for plotting covariance bounds 320 | ALPHA = 0.01 321 | s1 = chi2.isf(ALPHA, 1) 322 | s2 = chi2.isf(ALPHA, 2) 323 | 324 | fig5 = plt.figure(4) 325 | sigma = np.zeros((3, N)) 326 | ax1 = plt.subplot(311) 327 | sigma[0, :] = np.sqrt(s1 * P_hat[0, 0, :]) 328 | plt.fill_between(t, -sigma[0, :], sigma[0, :], color="C1", alpha=0.2) 329 | plt.plot(t, x[0, :] - x_hat[0, :], "C0") 330 | plt.ylabel(r"$e_1$ [m]") 331 | plt.setp(ax1, xticklabels=[]) 332 | ax2 = plt.subplot(312) 333 | sigma[1, :] = np.sqrt(s1 * P_hat[1, 1, :]) 334 | plt.fill_between(t, -sigma[1, :], sigma[1, :], color="C1", alpha=0.2) 335 | plt.plot(t, x[1, :] - x_hat[1, :], "C0") 336 | plt.ylabel(r"$e_2$ [m]") 337 | plt.setp(ax2, xticklabels=[]) 338 | ax3 = plt.subplot(313) 339 | sigma[2, :] = np.sqrt(s1 * P_hat[2, 2, :]) 340 | plt.fill_between(t, -sigma[2, :], sigma[2, :], color="C1", alpha=0.2) 341 | plt.plot(t, x[2, :] - x_hat[2, :], "C0") 342 | plt.ylabel(r"$e_3$ [rad]") 343 | plt.xlabel(r"$t$ [s]") 344 | plt.show() 345 | 346 | # %% 347 | -------------------------------------------------------------------------------- /README.md: -------------------------------------------------------------------------------- 1 | # Autonomous Ground Vehicle Navigation & Control Simulation Examples in Python 2 | 3 | __This project remains a work in progress.__ 4 | 5 | This is a repository of introductory autonomous ground vehicle (i.e., wheeled mobile robot) simulation examples in Python. The purpose of these examples is to provide easy-to-follow code that is illustrative of a number of fundamental mobile robot modelling, control, and navigation (localization, mapping) concepts. At present, motion planning problems lie beyond the scope of this example set. The focus here is on ground vehicles, although the presented techniques are also applicable more broadly. This code was initially developed to supplement topics covered in robotics courses offered in the [Department of Electrical & Computer Engineering](https://smithengineering.queensu.ca/ece/) at [Queen's University](https://www.queensu.ca). 6 | 7 | ## Requirements 8 | 9 | The examples in this repository were created for use with [Python](https://www.python.org) 3.9.X or later. The following packages are required in some or all of the examples in this repository: 10 | * [NumPy](https://numpy.org) 1.22.X or later 11 | ```pip install numpy``` or ```conda install numpy``` 12 | * [SciPy](https://scipy.org) 1.7.X or later 13 | ```pip install scipy``` or ```conda install scipy``` 14 | * [Matplotlib](https://matplotlib.org) 3.5.X or later 15 | ``` pip install matplotlib``` or ```conda install matplotlib``` 16 | 17 | The plotting routines also use [LaTeX](https://www.latex-project.org) for maths. If you don't want to install LaTeX then you will have to comment out and edit those parts in the plotting routines. However, using LaTeX is encouraged (and useful). 18 | 19 | ## MoBotPy Package 20 | 21 | The repository also includes a supporting Python package MoBotPy (`mobotpy`) that contains some code that is used repeatedly. Code developed in the worked examples is subsequently added to MoBotPy. 22 | 23 | Module Filename | Description 24 | --------------- | ----------- 25 | [integration.py](mobotpy/integration.py) | Provides basic Runge-Kutta and Euler integration functions. 26 | [models.py](mobotpy/models.py) | Provides standard vehicle models, plotting, and animation methods. 27 | [graphics.py](mobotpy/models.py) | Provides some basic shape plotting functions (used by [models.py](mobotpy/models.py)). 28 | 29 | ## Tables of Examples in this Repository 30 | 31 | This section provides a list of the examples in this repository. 32 | 33 | ### Introductory Linear Systems Examples 34 | 35 | These examples provide a review of basic concepts from control systems engineering in preparation for more advanced methods. 36 | 37 | Filename | Description 38 | -------- | ----------- 39 | [oneD_kinematic.py](oneD_kinematic.py) | Simulation of a linear 1D kinematic vehicle. 40 | [oneD_dynamic.py](oneD_dynamic.py) | Simulation of a linear 1D dynamic vehicle. 41 | [oneD_kinematic_control.py](oneD_kinematic_control.py) | Point stabilization of a linear 1D kinematic vehicle. 42 | [oneD_dynamic_control.py](oneD_kinematic_control.py) | Point stabilization of a linear 1D dynamic vehicle. 43 | [oneD_discrete_control.py](oneD_discrete_control.py) | Point stabilization of a linear 1D dynamic vehicle in discrete time. 44 | [oneD_integral_control.py](oneD_integral_control.py) | Example illustrating integral action for disturbance rejection. 45 | [oneD_dynamic_observer.py](oneD_dynamic_observer.py) | State estimation for a linear 1D dynamic vehicle. 46 | [oneD_combined_control.py](oneD_combined_control.py) | Example illustrating control combined with a state estimator. 47 | 48 | ### Vehicle Modelling Examples 49 | 50 | These examples provide simple models for a variety of commonly used wheeled vehicles. 51 | 52 | Filename | Description 53 | -------- | ----------- 54 | [diffdrive_kinematic.py](diffdrive_kinematic.py) | Simulation of a differential drive vehicle's kinematics. 55 | [tricycle_kinematic.py](tricycle_kinematic.py) | Simulation of a tricycle vehicle's kinematics. 56 | [ackermann_kinematic.py](ackermann_kinematic.py) | Simulation of an Ackermann steered (car-like) vehicle's kinematics. 57 | [unicycle_dynamic.py](unicycle_dynamic.py) | Simulation of a dynamic unicycle (i.e, single wheel) illustrating wheel slip. 58 | 59 | ### Vehicle Control Examples 60 | 61 | Filename | Description 62 | -------- | ----------- 63 | [control_approx_linearization.py](control_approx_linearization.py) | Trajectory tracking for a differential drive vehicle using control by approximate linearization. 64 | [dynamic_extension_tracking.py](dynamic_extension_tracking.py) | Trajectory tracking for a differential drive vehicle using feedback linearization with dynamic extension. 65 | [MPC_linear_tracking.py](MPC_linear_tracking.py) | Trajectory tracking for a 1D dynamic vehicle using unconstrained model predictive control (MPC). 66 | [diffdrive_control_mpc.py](diffdrive_control_mpc.py) | MPC-based trajectory tracking for a differential-drive vehicle using [CVXPY](https://www.cvxpy.org) convex optimizer. 67 | 68 | ### Vehicle Navigation Examples 69 | 70 | Filename | Description 71 | -------- | ----------- 72 | [fws_beacons_observer.py](fws_beacons_observer.py) | Luenberger-like observer design for a four-wheel steered vehicle with range-only beacons. 73 | [diffdrive_GNSS_EKF.py](diffdrive_GNSS_EKF.py) | Simple EKF implementation for a differential drive vehicle with wheel encoders, an angular rate gyro, and GNSS. 74 | [UT_example.py](UT_example.py) | Introductory problem illustrating a basic unscented transform (UT) of statistics for Gaussian inputs, after [Julier and Uhlmann (2004)](https://doi.org/10.1109/JPROC.2003.823141). 75 | [UKF_range_bearing.py](UKF_range_bearing.py) | Example implementation of a UKF for vehicle navigation by using odometry together with a range and bearing sensor, similar to the example on p. 290 of the book [Principles of Robot Motion: Theory, Algorithms, and Implementations (2005)](https://mitpress.mit.edu/books/principles-robot-motion). 76 | [PF_range.py](PF_range.py) | Example implementation of a particle filter (PF) for vehicle navigation by using odometry together with a range sensor. The example starts by showing particle clusters that grow with only dead reckoning, followed by a range-only sensor example with basic resampling. 77 | [vanilla_SLAM.py](vanilla_SLAM.py) | Simple 2D SLAM example illustrating the basic notion of including feature measurements as part of the KF state. 78 | 79 | ## Cite this Work 80 | 81 | You may wish to cite this work in your publications. Use the appropriate [release version vX.X.X](https://github.com/botprof/agv-examples/releases) in your reference. 82 | 83 | > Joshua A. Marshall, Autonomous Ground Vehicle Navigation and Control Simulation Examples in Python, vX.X.X, 2023, URL: [https://github.com/botprof/agv-examples](https://github.com/botprof/agv-examples). 84 | 85 | You might also use the BibTeX entry below. 86 | 87 | ```latex 88 | @misc{Marshall2023, 89 | author = {Marshall, Joshua A.}, 90 | title = {Autonomous Ground Vehicle Navigation and Control Simulation Examples in Python, vX.X.X}, 91 | year = {2023}, 92 | howpublished = {\url{https://github.com/botprof/agv-examples}} 93 | } 94 | ``` 95 | ## Contact the Author 96 | 97 | [Joshua A. Marshall](https://offroad.engineering.queensu.ca/people/joshua-marshall/), PhD, PEng 98 | [Department of Electrical & Computer Engineering](https://www.ece.queensu.ca) 99 | [Queen's University](http://www.queensu.ca) 100 | Kingston, ON K7L 3N6 Canada 101 | +1 (613) 533-2921 102 | [joshua.marshall@queensu.ca](mailto:joshua.marshall@queensu.ca) 103 | 104 | ## License 105 | 106 | Source code examples in this notebook are subject to an [MIT License](LICENSE). 107 | -------------------------------------------------------------------------------- /UKF_range_bearing.py: -------------------------------------------------------------------------------- 1 | """ 2 | Example UKF_range_bearing.py 3 | Author: Joshua A. Marshall 4 | GitHub: https://github.com/botprof/agv-examples 5 | """ 6 | 7 | # %% 8 | # SIMULATION SETUP 9 | 10 | import numpy as np 11 | import matplotlib.pyplot as plt 12 | from matplotlib.patches import Circle 13 | from scipy.stats import chi2 14 | from scipy.linalg import block_diag 15 | from numpy.linalg import inv 16 | from mobotpy.integration import rk_four 17 | from mobotpy.models import DiffDrive 18 | 19 | # Set the simulation time [s] and the sample period [s] 20 | SIM_TIME = 30.0 21 | T = 0.04 22 | 23 | # Create an array of time values [s] 24 | t = np.arange(0.0, SIM_TIME, T) 25 | N = np.size(t) 26 | 27 | # %% 28 | # VEHICLE SETUP 29 | 30 | # Set the track length of the vehicle [m] 31 | ELL = 1.0 32 | 33 | # Create a vehicle object of type DiffDrive 34 | vehicle = DiffDrive(ELL) 35 | 36 | # %% 37 | # BUILD A MAP OF FEATURES IN THE VEHICLE'S ENVIRONMENT 38 | 39 | # Number of features 40 | M = 150 41 | 42 | # Map size [m] 43 | D_MAP = 100 44 | 45 | # Randomly place features in the map 46 | f_map = np.zeros((2, M)) 47 | for i in range(0, M): 48 | f_map[:, i] = D_MAP * (2.0 * np.random.rand(2) - 1.0) 49 | 50 | # %% 51 | # SENSOR MODELS 52 | 53 | # Uncertainty in wheel speeds [m/s] 54 | SIGMA_SPEED = 0.1 55 | 56 | # Max and min sensor ranges 57 | R_MAX = 15 58 | R_MIN = 1 59 | 60 | # Set the range [m] and bearing [rad] uncertainty 61 | SIGMA_RANGE = 0.3 62 | SIGMA_BEARING = 15 * np.pi / 180 63 | 64 | # Create a range and bearing sensor model 65 | def RandB_sensor(x, f_map, R): 66 | 67 | # Define how many total features are available 68 | m = np.shape(f_map)[1] 69 | 70 | # Find the indices of features that are within range [r_min, r_max] 71 | a = np.array([]) 72 | for i in range(0, m): 73 | r = np.sqrt((f_map[0, i] - x[0]) ** 2 + (f_map[1, i] - x[1]) ** 2) 74 | if np.all( 75 | [ 76 | r < R_MAX, 77 | r > R_MIN, 78 | ] 79 | ): 80 | a = np.append(a, i) 81 | 82 | # Compute the range and bearing to all features within range 83 | if np.shape(a)[0] > 0: 84 | # Specify the size of the output 85 | m_k = np.shape(a)[0] 86 | y = np.zeros(2 * m_k) 87 | 88 | # Compute the range and bearing to all features (including sensor noise) 89 | for i in range(0, m_k): 90 | # Range measurement [m] 91 | y[2 * i] = ( 92 | np.sqrt( 93 | (f_map[0, int(a[i])] - x[0]) ** 2 94 | + (f_map[1, int(a[i])] - x[1]) ** 2 95 | ) 96 | + np.sqrt(R[0, 0]) * np.random.randn(1)[0] 97 | ) 98 | # Bearing measurement [rad] 99 | y[2 * i + 1] = ( 100 | np.unwrap( 101 | np.array( 102 | [ 103 | np.arctan2( 104 | f_map[1, int(a[i])] - x[1], f_map[0, int(a[i])] - x[0] 105 | ) 106 | - x[2] 107 | ] 108 | ) 109 | )[0] 110 | - np.pi 111 | + np.sqrt(R[1, 1]) * np.random.randn(1)[0] 112 | ) 113 | else: 114 | # No features were found within the sensing range 115 | y = np.array([]) 116 | 117 | # Return the range and bearing to features in y with indices in a 118 | return y, a 119 | 120 | 121 | # %% 122 | # CREATE A UKF-BASED ESTIMATOR 123 | 124 | 125 | def UKF(x, P, v_m, y_m, a, f_map, Q, R, kappa): 126 | 127 | # Set the augmented state and covariance 128 | xi = np.append(x, v_m) 129 | n_x = np.shape(x)[0] 130 | n_xi = np.shape(xi)[0] 131 | P_xi = block_diag(P, Q) 132 | 133 | # Define a set of sigma points for for the a priori estimate 134 | xi_sig = np.zeros((n_xi, 2 * n_xi + 1)) 135 | P_xi_sig = np.linalg.cholesky((n_xi + kappa) * P_xi) 136 | xi_sig[:, 0] = xi 137 | for i in range(0, n_xi): 138 | xi_sig[:, i + 1] = xi + P_xi_sig[:, i] 139 | xi_sig[:, n_xi + i + 1] = xi - P_xi_sig[:, i] 140 | 141 | # Propagate each sigma point through the vehicle's model 142 | xi_sig_hat = np.zeros((n_xi, 2 * n_xi + 1)) 143 | for i in range(0, 2 * n_xi + 1): 144 | xi_sig_hat[0:n_x, i] = rk_four( 145 | vehicle.f, xi_sig[0:n_x, i], xi_sig[n_x:n_xi, i], T 146 | ) 147 | 148 | # Compute the mean and covariance from the transformed sigma points 149 | w_xi = 0.5 / (n_xi + kappa) * np.ones(2 * n_xi + 1) 150 | w_xi[0] = 2 * kappa * w_xi[0] 151 | xi = np.average(xi_sig_hat, axis=1, weights=w_xi) 152 | P_xi = np.cov(xi_sig_hat, ddof=0, aweights=w_xi) 153 | 154 | # Help to keep the covariance matrix symmetrical 155 | P_xi = (P_xi + np.transpose(P_xi)) / 2 156 | 157 | # Set the vehicle state estimates 158 | x_hat = xi[0:n_x] 159 | P_hat = P_xi[0:n_x, 0:n_x] 160 | 161 | # Find the number of observed features 162 | m_k = np.shape(a)[0] 163 | 164 | # Compute the a posteriori estimate if there are visible features 165 | if m_k > 0: 166 | 167 | # Compute a new set of sigma points using the latest x_hat and P_hat 168 | x_sig = np.zeros((n_x, 2 * n_x + 1)) 169 | P_sig = np.linalg.cholesky((n_x + kappa) * P_hat) 170 | x_sig[:, 0] = x_hat 171 | for i in range(0, n_x): 172 | x_sig[:, i + 1] = x_hat + P_sig[:, i] 173 | x_sig[:, n_x + i + 1] = x_hat - P_sig[:, i] 174 | 175 | # Find the expected measurement corresponding to each sigma point 176 | y_hat_sig = np.zeros((2 * m_k, 2 * n_x + 1)) 177 | for j in range(0, 2 * n_x + 1): 178 | # Compute the expected measurements 179 | for i in range(0, m_k): 180 | y_hat_sig[2 * i, j] = np.sqrt( 181 | (f_map[0, int(a[i])] - x_sig[0, j]) ** 2 182 | + (f_map[1, int(a[i])] - x_sig[1, j]) ** 2 183 | ) 184 | y_hat_sig[2 * i + 1, j] = ( 185 | np.unwrap( 186 | [ 187 | np.arctan2( 188 | f_map[1, int(a[i])] - x_sig[1, j], 189 | f_map[0, int(a[i])] - x_sig[0, j], 190 | ) 191 | - x_sig[2, j] 192 | ] 193 | )[0] 194 | - np.pi 195 | ) 196 | 197 | # Recombine the sigma points 198 | w_x = 0.5 / (n_x + kappa) * np.ones(2 * n_x + 1) 199 | w_x[0] = 2 * kappa * w_x[0] 200 | y_hat = np.average(y_hat_sig, axis=1, weights=w_x) 201 | P_y = np.zeros((2 * m_k, 2 * m_k)) 202 | P_xy = np.zeros((n_x, 2 * m_k)) 203 | for i in range(0, 2 * n_x + 1): 204 | y_diff = y_hat_sig[:, i] - y_hat 205 | x_diff = x_sig[:, i] - x_hat 206 | P_y = P_y + w_x[i] * (y_diff.reshape((2 * m_k, 1))) @ np.transpose( 207 | y_diff.reshape((2 * m_k, 1)) 208 | ) 209 | P_xy = P_xy + w_x[i] * (x_diff.reshape((n_x, 1))) @ np.transpose( 210 | y_diff.reshape((2 * m_k, 1)) 211 | ) 212 | P_y = P_y + np.kron(np.identity(m_k), R) 213 | 214 | # Help to keep the covariance matrix symmetrical 215 | P_y = (P_y + np.transpose(P_y)) / 2 216 | 217 | # Update the estimate 218 | K = P_xy @ inv(P_y) 219 | x_hat = x_hat + K @ (y_m - y_hat) 220 | P_hat = P_hat - K @ P_y @ np.transpose(K) 221 | 222 | # Help keep the covariance matrix symmetric 223 | P_hat = (P_hat + np.transpose(P_hat)) / 2 224 | 225 | return x_hat, P_hat 226 | 227 | 228 | # %% 229 | # SIMULATE THE SYSTEM 230 | 231 | # Set the covariance matrices 232 | Q = np.diag([SIGMA_SPEED**2, SIGMA_SPEED**2]) 233 | R = np.diag([SIGMA_RANGE**2, SIGMA_BEARING**2]) 234 | 235 | # Initialize state, input, and estimator variables 236 | x = np.zeros((3, N)) 237 | v_m = np.zeros((2, N)) 238 | x_hat_UKF = np.zeros((3, N)) 239 | P_hat_UKF = np.zeros((3, 3, N)) 240 | 241 | # Initialize the state 242 | x_init = np.zeros(3) 243 | 244 | # Set the initial guess of the estimator 245 | x_guess = x_init + np.array([5.0, -5.0, 0.1]) 246 | P_guess = np.diag(np.square([5.0, -5.0, 0.1])) 247 | 248 | # Set the initial conditions 249 | x[:, 0] = x_init 250 | v_m[:, 0] = np.zeros(2) 251 | x_hat_UKF[:, 0] = x_guess 252 | P_hat_UKF[:, :, 0] = P_guess 253 | 254 | KAPPA = 3 - np.shape(x)[0] 255 | 256 | for i in range(1, N): 257 | 258 | # Compute some inputs (i.e., drive around) 259 | v = np.array([2.05, 1.95]) 260 | 261 | # Run the vehicle motion model 262 | x[:, i] = rk_four(vehicle.f, x[:, i - 1], v, T) 263 | 264 | # Model the rate sensors 265 | v_m[0, i] = v[0] + np.sqrt(Q[0, 0]) * np.random.randn(1)[0] 266 | v_m[1, i] = v[1] + np.sqrt(Q[1, 1]) * np.random.randn(1)[0] 267 | 268 | # Run the measurement model 269 | y_m, a = RandB_sensor(x[:, i], f_map, R) 270 | 271 | # Run the UKF estimator 272 | x_hat_UKF[:, i], P_hat_UKF[:, :, i] = UKF( 273 | x_hat_UKF[:, i - 1], 274 | P_hat_UKF[:, :, i - 1], 275 | v_m[:, i - 1], 276 | y_m, 277 | a, 278 | f_map, 279 | Q, 280 | R, 281 | KAPPA, 282 | ) 283 | 284 | # %% 285 | # PLOT THE SIMULATION OUTPUTS 286 | 287 | # Change some plot settings (optional) 288 | plt.rc("text", usetex=True) 289 | plt.rc("text.latex", preamble=r"\usepackage{cmbright,amsmath,bm}") 290 | plt.rc("savefig", format="pdf") 291 | plt.rc("savefig", bbox="tight") 292 | 293 | # Find the scaling factors for covariance bounds 294 | ALPHA = 0.01 295 | s1 = chi2.isf(ALPHA, 1) 296 | s2 = chi2.isf(ALPHA, 2) 297 | 298 | # Set some plot limits for better viewing 299 | X_RANGE = 0.5 300 | Y_RANGE = 0.5 301 | THETA_RANGE = 0.1 302 | PHI_RANGE = 0.1 303 | 304 | # Plot the errors with covariance bounds 305 | sigma = np.zeros((3, N)) 306 | fig1 = plt.figure(1) 307 | ax1 = plt.subplot(311) 308 | sigma[0, :] = np.sqrt(s1 * P_hat_UKF[0, 0, :]) 309 | plt.fill_between(t, -sigma[0, :], sigma[0, :], color="C0", alpha=0.2) 310 | plt.plot(t, x[0, :] - x_hat_UKF[0, :], "C0") 311 | plt.ylabel(r"$e_1$ [m]") 312 | plt.setp(ax1, xticklabels=[]) 313 | ax1.set_ylim([-X_RANGE, X_RANGE]) 314 | plt.grid(color="0.95") 315 | ax2 = plt.subplot(312) 316 | sigma[1, :] = np.sqrt(s1 * P_hat_UKF[1, 1, :]) 317 | plt.fill_between(t, -sigma[1, :], sigma[1, :], color="C0", alpha=0.2) 318 | plt.plot(t, x[1, :] - x_hat_UKF[1, :], "C0") 319 | plt.ylabel(r"$e_2$ [m]") 320 | plt.setp(ax2, xticklabels=[]) 321 | ax2.set_ylim([-Y_RANGE, Y_RANGE]) 322 | plt.grid(color="0.95") 323 | ax3 = plt.subplot(313) 324 | sigma[2, :] = np.sqrt(s1 * P_hat_UKF[2, 2, :]) 325 | plt.fill_between(t, -sigma[2, :], sigma[2, :], color="C0", alpha=0.2) 326 | plt.plot(t, x[2, :] - x_hat_UKF[2, :], "C0") 327 | plt.ylabel(r"$e_3$ [rad]") 328 | plt.setp(ax3, xticklabels=[]) 329 | ax3.set_ylim([-THETA_RANGE, THETA_RANGE]) 330 | plt.xlabel(r"$t$ [s]") 331 | plt.grid(color="0.95") 332 | 333 | # Plot the actual versus estimated positions on the map 334 | fig2, ax = plt.subplots() 335 | circle = Circle(x[0:2, 0], radius=R_MAX, alpha=0.2, color="C0", label="Sensing range") 336 | ax.add_artist(circle) 337 | plt.plot(x[0, :], x[1, :], "C0", label="Actual") 338 | plt.plot(x_hat_UKF[0, :], x_hat_UKF[1, :], "C1--", label="Estimated") 339 | plt.plot(f_map[0, :], f_map[1, :], "C2*", label="Transmitter") 340 | plt.axis("equal") 341 | ax.set_xlim([np.min(x_hat_UKF[0, :]) - 10, np.max(x_hat_UKF[0, :]) + 10]) 342 | ax.set_ylim([np.min(x_hat_UKF[1, :]) - 10, np.max(x_hat_UKF[1, :]) + 10]) 343 | plt.xlabel(r"$x_1$ [m]") 344 | plt.ylabel(r"$x_2$ [m]") 345 | plt.grid(color="0.95") 346 | plt.legend() 347 | 348 | # Show the plot to the screen 349 | plt.show() 350 | -------------------------------------------------------------------------------- /UT_example.py: -------------------------------------------------------------------------------- 1 | """ 2 | Example UT_example.py 3 | Author: Joshua A. Marshall 4 | GitHub: https://github.com/botprof/agv-examples 5 | """ 6 | 7 | # %% 8 | # SIMULATION SETUP 9 | 10 | import numpy as np 11 | import matplotlib.pyplot as plt 12 | from scipy.stats import chi2 13 | from matplotlib import patches 14 | 15 | # Set the number of points to simulate for ground truth 16 | N = 100000 17 | 18 | # Initialized input and output arrays 19 | x = np.zeros((2, N)) 20 | y = np.zeros((2, N)) 21 | 22 | # Set the mean and covariance values for the inputs 23 | R_BAR = 1.0 24 | THETA_BAR = np.pi / 2.0 25 | SIGMA_R = 0.02 26 | SIGMA_THETA = 15.0 27 | 28 | # Generate random inputs for ground truth 29 | x[0, :] = R_BAR + SIGMA_R * np.random.randn(1, N) 30 | x[1, :] = THETA_BAR + SIGMA_THETA * np.pi / 180.0 * np.random.randn(1, N) 31 | 32 | # %% 33 | # FUNCTION DEFINITIONS 34 | 35 | # The nonlinear polar to rectilinear transformation 36 | def h(x): 37 | y = np.zeros(2) 38 | y[0] = x[0] * np.cos(x[1]) 39 | y[1] = x[0] * np.sin(x[1]) 40 | return y 41 | 42 | 43 | # Function that implements the unscented transformation 44 | def unscented_transform(f, x, P_x, kappa): 45 | """Unscented transform of statistics.""" 46 | 47 | # Get the dimension of the random variable 48 | n = np.shape(x)[0] 49 | 50 | # Create array for sigma points 51 | x_sig = np.zeros((n, 2 * n + 1)) 52 | 53 | # Find matrix square root 54 | nP_sig = np.linalg.cholesky((n + kappa) * P_x) 55 | 56 | # Generate the sigma points 57 | x_sig[:, 0] = x 58 | for i in range(0, n): 59 | x_sig[:, i + 1] = x + nP_sig[:, i] 60 | x_sig[:, n + i + 1] = x - nP_sig[:, i] 61 | 62 | # Pass sigma points through the system model 63 | y_sig = np.zeros((n, 2 * n + 1)) 64 | for i in range(0, 2 * n + 1): 65 | y_sig[:, i] = f(x_sig[:, i]) 66 | 67 | # Compute weighted mean and covariance from the transformed sigma points 68 | w = 0.5 / (n + kappa) * np.ones(2 * n + 1) 69 | w[0] = 2 * kappa * w[0] 70 | y = np.average(y_sig, axis=1, weights=w) 71 | P_y = np.cov(y_sig, ddof=0, aweights=w) 72 | 73 | # Help to keep the covariance matrix symmetrical 74 | P_y = (P_y + np.transpose(P_y)) / 2 75 | 76 | # Return the output mean an covariance 77 | return y, P_y 78 | 79 | 80 | # %% 81 | # COMPUTE AND PLOT GROUND TRUTH 82 | 83 | # Compute outputs by passing each point through the nonlinear transformation 84 | for i in range(0, N): 85 | y[:, i] = h(x[:, i]) 86 | 87 | # Compute the output statistics by brute force (using NumPy functions) 88 | y_bar = np.mean(y, axis=1) 89 | P_y = np.cov(y, ddof=0) 90 | 91 | # Change some plot settings (optional) 92 | plt.rc("text", usetex=True) 93 | plt.rc("text.latex", preamble=r"\usepackage{cmbright,amsmath,bm}") 94 | plt.rc("savefig", format="pdf") 95 | plt.rc("savefig", bbox="tight") 96 | 97 | # Plot the output 98 | fig1, ax1 = plt.subplots() 99 | plt.plot(y[0, :], y[1, :], "C0+", alpha=0.2) 100 | plt.xlabel(r"$y_1$") 101 | plt.ylabel(r"$y_2$") 102 | plt.grid(color="0.95") 103 | OUTPUT_STATS = ( 104 | r"\begin{align*}" 105 | + r"N &=" 106 | + str(N) 107 | + r"\\" 108 | + r"\bar{y}_1 &=" 109 | + str(np.around(y_bar[0], decimals=3)) 110 | + r"\\" 111 | + r"\bar{y}_2 &=" 112 | + str(np.around(y_bar[1], decimals=3)) 113 | + r"\\" 114 | + r"\end{align*}" 115 | ) 116 | plt.text(0.05, 0.9, OUTPUT_STATS, transform=ax1.transAxes) 117 | 118 | # Save the plot 119 | plt.savefig("../agv-book/figs/ch5/UT_example_fig1.pdf") 120 | 121 | # Show the plot to the screen 122 | plt.show() 123 | 124 | # %% 125 | # COMPUTE THE OUTPUT STATISTICS BY LINEAR APPROXIMATION 126 | 127 | # Find the scaling factor for plotting covariance bounds 128 | ALPHA = 0.05 129 | s2 = chi2.isf(ALPHA, 2) 130 | 131 | # Create the plot and axes 132 | fig2, ax2 = plt.subplots() 133 | plt.xlabel(r"$y_1$") 134 | plt.ylabel(r"$y_2$") 135 | plt.grid(color="0.95") 136 | 137 | # Create the 95 % confidence ellipse for the actual statistics 138 | ell_actual = patches.Ellipse( 139 | (y_bar[0], y_bar[1]), 140 | 2 * np.sqrt(s2 * P_y[0, 0]), 141 | 2 * np.sqrt(s2 * P_y[1, 1]), 142 | angle=0, 143 | alpha=0.2, 144 | color="C0", 145 | ) 146 | ax2.add_artist(ell_actual) 147 | 148 | # Create the 95 % confidence ellipse for the linearized estimate 149 | ell_linear = patches.Ellipse( 150 | (0, 1), 151 | 2 * np.sqrt(s2) * 15.0 * np.pi / 180.0, 152 | 2 * np.sqrt(s2) * 0.02, 153 | angle=0, 154 | alpha=0.2, 155 | color="C1", 156 | ) 157 | ax2.add_artist(ell_linear) 158 | 159 | # Plot location of means 160 | plt.plot(y_bar[0], y_bar[1], "C0+") 161 | plt.plot(0, 1, "C1+") 162 | 163 | # Set the axis limits based on the actual covariance 164 | ax2.set_xlim(y_bar[0] - np.sqrt(s2 * P_y[0, 0]), y_bar[0] + np.sqrt(s2 * P_y[0, 0])) 165 | ax2.set_ylim(y_bar[1] - np.sqrt(s2 * P_y[1, 1]), y_bar[1] + np.sqrt(s2 * P_y[1, 1])) 166 | 167 | # Add a legend and show the plot 168 | plt.legend(["Actual", "Linearized"]) 169 | 170 | # Save the plot 171 | plt.savefig("../agv-book/figs/ch5/UT_example_fig2.pdf") 172 | 173 | # Show the plot to the screen 174 | plt.show() 175 | 176 | # %% 177 | # COMPUTE THE OUTPUT STATISTICS BY THE UT 178 | 179 | KAPPA = 3 - np.shape(x)[0] 180 | 181 | y_u, P_u = unscented_transform( 182 | h, 183 | np.array([R_BAR, THETA_BAR]), 184 | np.diag([SIGMA_R**2, (SIGMA_THETA * np.pi / 180.0) ** 2]), 185 | KAPPA, 186 | ) 187 | 188 | # Create the plot and axes 189 | fig3, ax3 = plt.subplots() 190 | plt.xlabel(r"$y_1$") 191 | plt.ylabel(r"$y_2$") 192 | plt.grid(color="0.95") 193 | 194 | # Create a covariance ellipse for the actual statistics 195 | ell_actual = patches.Ellipse( 196 | (y_bar[0], y_bar[1]), 197 | 2 * np.sqrt(s2 * P_y[0, 0]), 198 | 2 * np.sqrt(s2 * P_y[1, 1]), 199 | angle=0, 200 | alpha=0.2, 201 | color="C0", 202 | ) 203 | ax3.add_artist(ell_actual) 204 | 205 | # Create the 95 % confidence ellipse for the linearized estimate 206 | ell_linear = patches.Ellipse( 207 | (0, 1), 208 | 2 * np.sqrt(s2) * 15.0 * np.pi / 180.0, 209 | 2 * np.sqrt(s2) * 0.02, 210 | angle=0, 211 | alpha=0.2, 212 | color="C1", 213 | ) 214 | ax3.add_artist(ell_linear) 215 | 216 | # Create a covariance ellipse for the unscented transform of statistics 217 | ell_UT = patches.Ellipse( 218 | (y_u[0], y_u[1]), 219 | 2 * np.sqrt(s2 * P_u[0, 0]), 220 | 2 * np.sqrt(s2 * P_u[1, 1]), 221 | angle=0, 222 | alpha=0.2, 223 | color="C2", 224 | ) 225 | ax3.add_artist(ell_UT) 226 | 227 | # Plot location of means 228 | plt.plot(y_bar[0], y_bar[1], "C0+") 229 | plt.plot(0, 1, "C1+") 230 | plt.plot(y_u[0], y_u[1], "C2+") 231 | 232 | # Set the axis limits based on the actual covariance 233 | ax3.set_xlim(y_bar[0] - np.sqrt(s2 * P_y[0, 0]), y_bar[0] + np.sqrt(s2 * P_y[0, 0])) 234 | ax3.set_ylim(y_bar[1] - np.sqrt(s2 * P_y[1, 1]), y_bar[1] + np.sqrt(s2 * P_y[1, 1])) 235 | 236 | # Add a legend and show the figure 237 | plt.legend(["Actual", "Linearized", "Unscented"]) 238 | 239 | # Save the plot 240 | plt.savefig("../agv-book/figs/ch5/UT_example_fig3.pdf") 241 | 242 | # Show the plot to the screen 243 | plt.show() 244 | -------------------------------------------------------------------------------- /ackermann_kinematic.py: -------------------------------------------------------------------------------- 1 | """ 2 | Example ackermann_kinematic.py 3 | Author: Joshua A. Marshall 4 | GitHub: https://github.com/botprof/agv-examples 5 | """ 6 | 7 | # %% 8 | # SIMULATION SETUP 9 | 10 | import numpy as np 11 | import matplotlib.pyplot as plt 12 | from mobotpy.integration import rk_four 13 | from mobotpy.models import Ackermann 14 | 15 | # Set the simulation time [s] and the sample period [s] 16 | SIM_TIME = 15.0 17 | T = 0.1 18 | 19 | # Create an array of time values [s] 20 | t = np.arange(0.0, SIM_TIME, T) 21 | N = np.size(t) 22 | 23 | # %% 24 | # RUN SIMULATION 25 | 26 | # Initialize arrays that will be populated with our inputs and states 27 | x = np.zeros((4, N)) 28 | u = np.zeros((2, N)) 29 | phi_L = np.zeros(N) 30 | phi_R = np.zeros(N) 31 | 32 | # Set the wheelbase and track of the vehicle [m] 33 | ELL_W = 2.50 34 | ELL_T = 1.75 35 | 36 | # Set the initial pose [m, m, rad, rad], velocities [m/s, rad/s] 37 | x[0, 0] = 0.0 38 | x[1, 0] = 0.0 39 | x[2, 0] = np.pi / 2.0 40 | x[3, 0] = 0.0 41 | u[0, 0] = 5.0 42 | u[1, 0] = 0 43 | 44 | # Let's now use the class Ackermann for plotting 45 | vehicle = Ackermann(ELL_W, ELL_T) 46 | 47 | # Run the simulation 48 | for k in range(1, N): 49 | x[:, k] = rk_four(vehicle.f, x[:, k - 1], u[:, k - 1], T) 50 | phi_L[k] = np.arctan( 51 | 2 * ELL_W * np.tan(x[3, k]) / (2 * ELL_W - ELL_T * np.tan(x[3, k])) 52 | ) 53 | phi_R[k] = np.arctan( 54 | 2 * ELL_W * np.tan(x[3, k]) / (2 * ELL_W + ELL_T * np.tan(x[3, k])) 55 | ) 56 | u[0, k] = 5.0 57 | u[1, k] = -0.25 * np.sin(2.0 * t[k]) 58 | 59 | # %% 60 | # MAKE SOME PLOTS 61 | 62 | # Change some plot settings (optional) 63 | plt.rc("text", usetex=True) 64 | plt.rc("text.latex", preamble=r"\usepackage{cmbright,amsmath,bm}") 65 | plt.rc("savefig", format="pdf") 66 | plt.rc("savefig", bbox="tight") 67 | 68 | # Plot the states as a function of time 69 | fig1 = plt.figure(1) 70 | fig1.set_figheight(6.4) 71 | ax1a = plt.subplot(411) 72 | plt.plot(t, x[0, :]) 73 | plt.grid(color="0.95") 74 | plt.ylabel(r"$x$ [m]") 75 | plt.setp(ax1a, xticklabels=[]) 76 | ax1b = plt.subplot(412) 77 | plt.plot(t, x[1, :]) 78 | plt.grid(color="0.95") 79 | plt.ylabel(r"$y$ [m]") 80 | plt.setp(ax1b, xticklabels=[]) 81 | ax1c = plt.subplot(413) 82 | plt.plot(t, x[2, :] * 180.0 / np.pi) 83 | plt.grid(color="0.95") 84 | plt.ylabel(r"$\theta$ [deg]") 85 | plt.setp(ax1c, xticklabels=[]) 86 | ax1c = plt.subplot(414) 87 | plt.plot(t, phi_L * 180.0 / np.pi, "C1", label=r"$\phi_L$") 88 | plt.plot(t, phi_R * 180.0 / np.pi, "C2", label=r"$\phi_R$") 89 | plt.grid(color="0.95") 90 | plt.ylabel(r"$\phi_L,\phi_R$ [deg]") 91 | plt.xlabel(r"$t$ [s]") 92 | plt.legend() 93 | 94 | # Save the plot 95 | plt.savefig("../agv-book/figs/ch3/ackermann_kinematic_fig1.pdf") 96 | 97 | # Plot the position of the vehicle in the plane 98 | fig2 = plt.figure(2) 99 | plt.plot(x[0, :], x[1, :]) 100 | plt.axis("equal") 101 | X_BL, Y_BL, X_BR, Y_BR, X_FL, Y_FL, X_FR, Y_FR, X_BD, Y_BD = vehicle.draw( 102 | x[0, 0], x[1, 0], x[2, 0], phi_L[0], phi_R[0] 103 | ) 104 | plt.fill(X_BL, Y_BL, "k") 105 | plt.fill(X_BR, Y_BR, "k") 106 | plt.fill(X_FR, Y_FR, "k") 107 | plt.fill(X_FL, Y_FL, "k") 108 | plt.fill(X_BD, Y_BD, "C2", alpha=0.5, label="Start") 109 | X_BL, Y_BL, X_BR, Y_BR, X_FL, Y_FL, X_FR, Y_FR, X_BD, Y_BD = vehicle.draw( 110 | x[0, N - 1], x[1, N - 1], x[2, N - 1], phi_L[N - 1], phi_R[N - 1] 111 | ) 112 | plt.fill(X_BL, Y_BL, "k") 113 | plt.fill(X_BR, Y_BR, "k") 114 | plt.fill(X_FR, Y_FR, "k") 115 | plt.fill(X_FL, Y_FL, "k") 116 | plt.fill(X_BD, Y_BD, "C3", alpha=0.5, label="End") 117 | plt.xlabel(r"$x$ [m]") 118 | plt.ylabel(r"$y$ [m]") 119 | plt.legend() 120 | 121 | # Save the plot 122 | plt.savefig("../agv-book/figs/ch3/ackermann_kinematic_fig2.pdf") 123 | 124 | # Show all the plots to the screen 125 | plt.show() 126 | 127 | # %% 128 | # MAKE AN ANIMATION 129 | 130 | # Create and save the animation 131 | ani = vehicle.animate( 132 | x, 133 | T, 134 | phi_L, 135 | phi_R, 136 | True, 137 | "../agv-book/gifs/ch3/ackermann_kinematic.gif", 138 | ) 139 | 140 | # Show the movie to the screen 141 | plt.show() 142 | 143 | # # Show animation in HTML output if you are using IPython or Jupyter notebooks 144 | # plt.rc('animation', html='jshtml') 145 | # display(ani) 146 | # plt.close() 147 | -------------------------------------------------------------------------------- /control_approx_linearization.py: -------------------------------------------------------------------------------- 1 | """ 2 | Example control_approx_linearization.py 3 | Author: Joshua A. Marshall 4 | GitHub: https://github.com/botprof/agv-examples 5 | """ 6 | 7 | # %% 8 | # SIMULATION SETUP 9 | 10 | import numpy as np 11 | import matplotlib.pyplot as plt 12 | from scipy import signal 13 | from mobotpy.models import DiffDrive 14 | from mobotpy.integration import rk_four 15 | 16 | # Set the simulation time [s] and the sample period [s] 17 | SIM_TIME = 20.0 18 | T = 0.04 19 | 20 | # Create an array of time values [s] 21 | t = np.arange(0.0, SIM_TIME, T) 22 | N = np.size(t) 23 | 24 | # %% 25 | # COMPUTE THE REFERENCE TRAJECTORY 26 | 27 | # Radius of the circle [m] 28 | R = 10 29 | 30 | # Angular rate [rad/s] at which to traverse the circle 31 | OMEGA = 0.1 32 | 33 | # Pre-compute the desired trajectory 34 | x_d = np.zeros((3, N)) 35 | u_d = np.zeros((2, N)) 36 | for k in range(0, N): 37 | x_d[0, k] = R * np.sin(OMEGA * t[k]) 38 | x_d[1, k] = R * (1 - np.cos(OMEGA * t[k])) 39 | x_d[2, k] = OMEGA * t[k] 40 | u_d[0, k] = R * OMEGA 41 | u_d[1, k] = OMEGA 42 | 43 | # %% 44 | # VEHICLE SETUP 45 | 46 | # Set the track length of the vehicle [m] 47 | ELL = 1.0 48 | 49 | # Create a vehicle object of type DiffDrive 50 | vehicle = DiffDrive(ELL) 51 | 52 | # %% 53 | # SIMULATE THE CLOSED-LOOP SYSTEM 54 | 55 | # Initial conditions 56 | x_init = np.zeros(3) 57 | x_init[0] = 0.0 58 | x_init[1] = 5.0 59 | x_init[2] = 0.0 60 | 61 | # Setup some arrays 62 | x = np.zeros((3, N)) 63 | u = np.zeros((2, N)) 64 | x[:, 0] = x_init 65 | 66 | for k in range(1, N): 67 | 68 | # Simulate the differential drive vehicle motion 69 | x[:, k] = rk_four(vehicle.f, x[:, k - 1], u[:, k - 1], T) 70 | 71 | # Compute the approximate linearization 72 | A = np.array( 73 | [ 74 | [0, 0, -u_d[0, k - 1] * np.sin(x_d[2, k - 1])], 75 | [0, 0, u_d[0, k - 1] * np.cos(x_d[2, k - 1])], 76 | [0, 0, 0], 77 | ] 78 | ) 79 | B = np.array([[np.cos(x_d[2, k - 1]), 0], [np.sin(x_d[2, k - 1]), 0], [0, 1]]) 80 | 81 | # Compute the gain matrix to place poles of (A - BK) at p 82 | p = np.array([-1.0, -2.0, -0.5]) 83 | K = signal.place_poles(A, B, p) 84 | 85 | # Compute the controls (v, omega) and convert to wheel speeds (v_L, v_R) 86 | u_unicycle = -K.gain_matrix @ (x[:, k - 1] - x_d[:, k - 1]) + u_d[:, k] 87 | u[:, k] = vehicle.uni2diff(u_unicycle) 88 | 89 | # %% 90 | # MAKE PLOTS 91 | 92 | # Change some plot settings (optional) 93 | plt.rc("text", usetex=True) 94 | plt.rc("text.latex", preamble=r"\usepackage{cmbright,amsmath,bm}") 95 | plt.rc("savefig", format="pdf") 96 | plt.rc("savefig", bbox="tight") 97 | 98 | # Plot the states as a function of time 99 | fig1 = plt.figure(1) 100 | fig1.set_figheight(6.4) 101 | ax1a = plt.subplot(411) 102 | plt.plot(t, x_d[0, :], "C1--") 103 | plt.plot(t, x[0, :], "C0") 104 | plt.grid(color="0.95") 105 | plt.ylabel(r"$x$ [m]") 106 | plt.setp(ax1a, xticklabels=[]) 107 | plt.legend(["Desired", "Actual"]) 108 | ax1b = plt.subplot(412) 109 | plt.plot(t, x_d[1, :], "C1--") 110 | plt.plot(t, x[1, :], "C0") 111 | plt.grid(color="0.95") 112 | plt.ylabel(r"$y$ [m]") 113 | plt.setp(ax1b, xticklabels=[]) 114 | ax1c = plt.subplot(413) 115 | plt.plot(t, x_d[2, :] * 180.0 / np.pi, "C1--") 116 | plt.plot(t, x[2, :] * 180.0 / np.pi, "C0") 117 | plt.grid(color="0.95") 118 | plt.ylabel(r"$\theta$ [deg]") 119 | plt.setp(ax1c, xticklabels=[]) 120 | ax1d = plt.subplot(414) 121 | plt.step(t, u[0, :], "C2", where="post", label="$v_L$") 122 | plt.step(t, u[1, :], "C3", where="post", label="$v_R$") 123 | plt.grid(color="0.95") 124 | plt.ylabel(r"$\bm{u}$ [m/s]") 125 | plt.xlabel(r"$t$ [s]") 126 | plt.legend() 127 | 128 | # Save the plot 129 | # plt.savefig("../agv-book/figs/ch4/control_approx_linearization_fig1.pdf") 130 | 131 | # Plot the position of the vehicle in the plane 132 | fig2 = plt.figure(2) 133 | plt.plot(x_d[0, :], x_d[1, :], "C1--", label="Desired") 134 | plt.plot(x[0, :], x[1, :], "C0", label="Actual") 135 | plt.axis("equal") 136 | X_L, Y_L, X_R, Y_R, X_B, Y_B, X_C, Y_C = vehicle.draw(x[0, 0], x[1, 0], x[2, 0]) 137 | plt.fill(X_L, Y_L, "k") 138 | plt.fill(X_R, Y_R, "k") 139 | plt.fill(X_C, Y_C, "k") 140 | plt.fill(X_B, Y_B, "C2", alpha=0.5, label="Start") 141 | X_L, Y_L, X_R, Y_R, X_B, Y_B, X_C, Y_C = vehicle.draw( 142 | x[0, N - 1], x[1, N - 1], x[2, N - 1] 143 | ) 144 | plt.fill(X_L, Y_L, "k") 145 | plt.fill(X_R, Y_R, "k") 146 | plt.fill(X_C, Y_C, "k") 147 | plt.fill(X_B, Y_B, "C3", alpha=0.5, label="End") 148 | plt.xlabel(r"$x$ [m]") 149 | plt.ylabel(r"$y$ [m]") 150 | plt.legend() 151 | 152 | # Save the plot 153 | # plt.savefig("../agv-book/figs/ch4/control_approx_linearization_fig2.pdf") 154 | 155 | # Show the plots to the screen 156 | # plt.show() 157 | 158 | # %% 159 | # MAKE AN ANIMATION 160 | 161 | # Create the animation 162 | ani = vehicle.animate_trajectory(x, x_d, T) 163 | 164 | # Create and save the animation 165 | # ani = vehicle.animate_trajectory( 166 | # x, x_d, T, True, "../agv-book/gifs/ch4/control_approx_linearization.gif" 167 | # ) 168 | 169 | # Show all the plots to the screen 170 | plt.show() 171 | 172 | # Show animation in HTML output if you are using IPython or Jupyter notebooks 173 | # from IPython.display import display 174 | 175 | # plt.rc("animation", html="jshtml") 176 | # display(ani) 177 | # plt.close() 178 | -------------------------------------------------------------------------------- /dead_recknoning_cart.py: -------------------------------------------------------------------------------- 1 | """ 2 | Example dead_reckoning_cart.py 3 | Author: Joshua A. Marshall 4 | GitHub: https://github.com/botprof/agv-examples 5 | """ 6 | 7 | # %% 8 | # SIMULATION SETUP 9 | 10 | import numpy as np 11 | import matplotlib.pyplot as plt 12 | from scipy.stats import chi2 13 | 14 | # Set the simulation time [s] and the sample period [s] 15 | SIM_TIME = 5.0 16 | T = 0.1 17 | 18 | # Create an array of time values [s] 19 | t = np.arange(0.0, SIM_TIME, T) 20 | N = np.size(t) 21 | 22 | # %% 23 | # DEFINE THE ROBOT MODEL 24 | 25 | # Mobile robot model 26 | F = np.array([[1, T], [0, 1]]) 27 | G = np.array([0.5 * T**2, T]) 28 | 29 | # Input noise covariance matrix 30 | Q = 1.0**2 31 | 32 | # %% 33 | # SET UP INITIAL CONDITIONS 34 | 35 | # Set up arrays for a two-dimensional state 36 | x_hat = np.zeros((2, N)) 37 | P_hat = np.zeros((2, 2, N)) 38 | 39 | # Robot state (actual) 40 | x = np.zeros([2, N]) 41 | 42 | # Initial estimate of the robot's location 43 | x_hat[:, 0] = x[:, 0] 44 | # Initial covariance matrix 45 | P_hat[:, :, 0] = 0.0 * np.eye(2) 46 | 47 | # Initial inputs 48 | u = np.zeros(1) 49 | u_measured = np.zeros(1) 50 | 51 | # %% 52 | # RUN THE SIMULATION 53 | 54 | for k in range(1, N): 55 | # Simulate the actual vehicle 56 | x[:, k] = F @ x[:, k - 1] + G * u 57 | 58 | # Run the estimator (dead reckoning) 59 | x_hat[:, k] = F @ x_hat[:, k - 1] + G * u_measured 60 | P_hat[:, :, k] = F @ P_hat[:, :, k - 1] @ F.T + G * Q @ G.T 61 | # Help the covariance matrix stay symmetric 62 | P_hat[:, :, k] = 0.5 * (P_hat[:, :, k] + P_hat[:, :, k].T) 63 | 64 | # Control input (actual) 65 | u = 10 * np.sin(2 * t[k]) 66 | 67 | # Measured input (with zero-mean Gaussian noise) 68 | u_measured = u + np.sqrt(Q) * np.random.randn() 69 | 70 | # %% 71 | # PLOT THE RESULTS 72 | 73 | # Change some plot settings (optional) 74 | plt.rc("text", usetex=True) 75 | plt.rc("text.latex", preamble=r"\usepackage{cmbright,amsmath,bm}") 76 | plt.rc("savefig", format="pdf") 77 | plt.rc("savefig", bbox="tight") 78 | 79 | # Find the scaling factor for plotting covariance bounds 80 | ALPHA = 0.01 81 | s1 = chi2.isf(ALPHA, 1) 82 | s2 = chi2.isf(ALPHA, 2) 83 | 84 | # Plot the state and the estimate 85 | sigma = np.zeros((2, N)) 86 | plt.figure() 87 | ax1 = plt.subplot(211) 88 | sigma[0, :] = np.sqrt(s1 * P_hat[0, 0, :]) 89 | plt.fill_between( 90 | t, 91 | x_hat[0, :] - sigma[0, :], 92 | x_hat[0, :] + sigma[0, :], 93 | color="C0", 94 | alpha=0.2, 95 | label=str(100 * (1 - ALPHA)) + r" \% Confidence", 96 | ) 97 | plt.plot(t, x[0, :], "C0", label="Actual") 98 | plt.plot(t, x_hat[0, :], "C1", label="Estimate") 99 | plt.ylabel(r"$x_1$ [m]") 100 | plt.setp(ax1, xticklabels=[]) 101 | plt.grid(color="0.95") 102 | plt.legend() 103 | ax2 = plt.subplot(212) 104 | sigma[1, :] = np.sqrt(s1 * P_hat[1, 1, :]) 105 | plt.fill_between( 106 | t, 107 | x_hat[1, :] - sigma[1, :], 108 | x_hat[1, :] + sigma[1, :], 109 | color="C0", 110 | alpha=0.2, 111 | label=str(100 * (1 - ALPHA)) + r" \% Confidence", 112 | ) 113 | plt.plot(t, x[1, :], "C0", label="Actual") 114 | plt.plot(t, x_hat[1, :], "C1", label="Estimate") 115 | plt.ylabel(r"$x_2$ [m]") 116 | plt.grid(color="0.95") 117 | plt.xlabel(r"$t$ [s]") 118 | plt.show() 119 | 120 | # Plot the error in the estimate 121 | sigma = np.zeros((2, N)) 122 | plt.figure() 123 | ax1 = plt.subplot(211) 124 | sigma[0, :] = np.sqrt(s1 * P_hat[0, 0, :]) 125 | plt.fill_between( 126 | t, 127 | -sigma[0, :], 128 | sigma[0, :], 129 | color="C0", 130 | alpha=0.2, 131 | label=str(100 * (1 - ALPHA)) + r" \% Confidence", 132 | ) 133 | plt.plot(t, x[0, :] - x_hat[0, :], "C0", label="Estimator Error") 134 | plt.ylabel(r"$e_1$ [m]") 135 | plt.setp(ax1, xticklabels=[]) 136 | plt.grid(color="0.95") 137 | plt.legend() 138 | ax2 = plt.subplot(212) 139 | sigma[1, :] = np.sqrt(s1 * P_hat[1, 1, :]) 140 | plt.fill_between( 141 | t, 142 | -sigma[1, :], 143 | sigma[1, :], 144 | color="C0", 145 | alpha=0.2, 146 | label=str(100 * (1 - ALPHA)) + r" \% Confidence", 147 | ) 148 | plt.plot(t, x[1, :] - x_hat[1, :], "C0", label="Estimator Error") 149 | plt.ylabel(r"$e_2$ [m]") 150 | plt.grid(color="0.95") 151 | plt.xlabel(r"$t$ [s]") 152 | plt.show() 153 | -------------------------------------------------------------------------------- /diffdrive_GNSS_EKF.py: -------------------------------------------------------------------------------- 1 | """ 2 | Example diffdrive_GNSS_EKF.py 3 | Author: Joshua A. Marshall 4 | GitHub: https://github.com/botprof/agv-examples 5 | """ 6 | 7 | # %% 8 | # SIMULATION SETUP 9 | 10 | import numpy as np 11 | import matplotlib.pyplot as plt 12 | from mobotpy.models import DiffDrive 13 | from mobotpy.integration import rk_four 14 | from scipy.stats import chi2 15 | from numpy.linalg import inv 16 | 17 | # Set the simulation time [s] and the sample period [s] 18 | SIM_TIME = 30.0 19 | T = 0.01 20 | 21 | # Create an array of time values [s] 22 | t = np.arange(0.0, SIM_TIME, T) 23 | N = np.size(t) 24 | 25 | # %% 26 | # VEHICLE SETUP 27 | 28 | # Set the track length of the vehicle [m] 29 | ELL = 1.0 30 | 31 | # Create a vehicle object of type DiffDrive 32 | vehicle = DiffDrive(ELL) 33 | 34 | # %% 35 | # FUNCTION DEFINITIONS 36 | 37 | 38 | def unicycle_GNSS_ekf(u_m, y, Q, R, x, P, T): 39 | 40 | # Define some matrices for the a priori step 41 | G = np.array([[np.cos(x[2]), 0], [np.sin(x[2]), 0], [0, 1]]) 42 | F = np.identity(3) + T * np.array( 43 | [[0, 0, -u_m[0] * np.sin(x[2])], [0, 0, u_m[0] * np.cos(x[2])], [0, 0, 0]] 44 | ) 45 | L = T * G 46 | 47 | # Compute a priori estimate 48 | x_new = x + T * G @ u_m 49 | P_new = F @ P @ np.transpose(F) + L @ Q @ np.transpose(L) 50 | 51 | # Numerically help the covariance matrix stay symmetric 52 | P_new = (P_new + np.transpose(P_new)) / 2 53 | 54 | # Define some matrices for the a posteriori step 55 | C = np.array([[1, 0, 0], [0, 1, 0]]) 56 | H = C 57 | M = np.identity(2) 58 | 59 | # Compute the a posteriori estimate 60 | K = ( 61 | P_new 62 | @ np.transpose(H) 63 | @ inv(H @ P_new @ np.transpose(H) + M @ R @ np.transpose(M)) 64 | ) 65 | x_new = x_new + K @ (y - C @ x_new) 66 | P_new = (np.identity(3) - K @ H) @ P_new 67 | 68 | # Numerically help the covariance matrix stay symmetric 69 | P_new = (P_new + np.transpose(P_new)) / 2 70 | 71 | # Define the function output 72 | return x_new, P_new 73 | 74 | 75 | # %% 76 | # SET UP INITIAL CONDITIONS AND ESTIMATOR PARAMETERS 77 | 78 | # Initial conditions 79 | x_init = np.zeros(3) 80 | x_init[0] = 0.0 81 | x_init[1] = 0.0 82 | x_init[2] = 0.0 83 | 84 | # Setup some arrays for the actual vehicle 85 | x = np.zeros((3, N)) 86 | u = np.zeros((2, N)) 87 | x[:, 0] = x_init 88 | 89 | # Set the initial guess for the estimator 90 | x_guess = x_init + np.array([5.0, -5.0, 0.1]) 91 | 92 | # Set the initial pose covariance estimate as a diagonal matrix 93 | P_guess = np.diag(np.square([5.0, -5.0, 0.1])) 94 | 95 | # Set the true process and measurement noise covariances 96 | Q = np.diag( 97 | [ 98 | 1.0 / (np.power(T, 2)) * np.power(0.2 * np.pi / ((2 ** 12) * 3), 2), 99 | np.power(0.001, 2), 100 | ] 101 | ) 102 | R = np.power(5.0, 2) * np.identity(2) 103 | 104 | # Initialized estimator arrays 105 | x_hat = np.zeros((3, N)) 106 | x_hat[:, 0] = x_guess 107 | 108 | # Measured odometry (speed and angular rate) and GNSS (x, y) signals 109 | u_m = np.zeros((2, N)) 110 | y = np.zeros((2, N)) 111 | 112 | # Covariance of the estimate 113 | P_hat = np.zeros((3, 3, N)) 114 | P_hat[:, :, 0] = P_guess 115 | 116 | # Compute some inputs to just drive around 117 | for k in range(1, N): 118 | # Compute some inputs to steer the unicycle around 119 | u_unicycle = np.array([2.0, np.sin(0.005 * T * k)]) 120 | 121 | # %% 122 | # SIMULATE AND PLOT WITHOUT GNSS 123 | 124 | # Set the process and measurement noise covariances to ignore GNSS 125 | Q_hat = Q 126 | R_hat = 1e10 * R 127 | 128 | for k in range(1, N): 129 | 130 | # Simulate the differential drive vehicle's motion 131 | u[:, k] = vehicle.uni2diff(u_unicycle) 132 | x[:, k] = rk_four(vehicle.f, x[:, k - 1], u[:, k - 1], T) 133 | 134 | # Simulate the vehicle speed estimate 135 | u_m[0, k] = u_unicycle[0] + np.power(Q[0, 0], 0.5) * np.random.randn(1)[0] 136 | 137 | # Simulate the angular rate gyroscope measurement 138 | u_m[1, k] = u_unicycle[1] + np.power(Q[1, 1], 0.5) * np.random.randn(1)[0] 139 | 140 | # Simulate the GNSS measurement 141 | y[0, k] = x[0, k] + np.power(R[0, 0], 0.5) * np.random.randn(1)[0] 142 | y[1, k] = x[1, k] + np.power(R[1, 1], 0.5) * np.random.randn(1)[0] 143 | 144 | # Run the EKF estimator 145 | x_hat[:, k], P_hat[:, :, k] = unicycle_GNSS_ekf( 146 | u_m[:, k], y[:, k], Q_hat, R_hat, x_hat[:, k - 1], P_hat[:, :, k - 1], T 147 | ) 148 | 149 | # Change some plot settings (optional) 150 | plt.rc("text", usetex=True) 151 | plt.rc("text.latex", preamble=r"\usepackage{cmbright,amsmath,bm}") 152 | plt.rc("savefig", format="pdf") 153 | plt.rc("savefig", bbox="tight") 154 | 155 | # Plot results without GNSS 156 | fig1 = plt.figure(1) 157 | ax1 = plt.subplot(311) 158 | plt.setp(ax1, xticklabels=[]) 159 | plt.plot(t, x[0, :], "C0", label="Actual") 160 | plt.plot(t, x_hat[0, :], "C1--", label="Estimated") 161 | plt.ylabel(r"$x_1$ [m]") 162 | plt.grid(color="0.95") 163 | plt.legend() 164 | ax2 = plt.subplot(312) 165 | plt.setp(ax2, xticklabels=[]) 166 | plt.plot(t, x[1, :], "C0") 167 | plt.plot(t, x_hat[1, :], "C1--") 168 | plt.ylabel(r"$x_2$ [m]") 169 | plt.grid(color="0.95") 170 | ax3 = plt.subplot(313) 171 | plt.plot(t, x[2, :], "C0") 172 | plt.plot(t, x_hat[2, :], "C1--") 173 | plt.xlabel(r"$t$ [s]") 174 | plt.ylabel(r"$x_3$ [rad]") 175 | plt.grid(color="0.95") 176 | 177 | # Save the plot 178 | plt.savefig("../agv-book/figs/ch5/diffdrive_GNSS_EKF_fig1.pdf") 179 | 180 | # Find the scaling factor for plotting 99% covariance bounds 181 | alpha = 0.01 182 | s1 = chi2.isf(alpha, 1) 183 | s2 = chi2.isf(alpha, 2) 184 | 185 | # Plot the estimation error with covariance bounds 186 | sigma = np.zeros((3, N)) 187 | fig2 = plt.figure(2) 188 | ax1 = plt.subplot(311) 189 | sigma[0, :] = np.sqrt(s1 * P_hat[0, 0, :]) 190 | plt.fill_between( 191 | t, 192 | -sigma[0, :], 193 | sigma[0, :], 194 | color="C0", 195 | alpha=0.2, 196 | label=str(100 * (1 - alpha)) + " \% Confidence", 197 | ) 198 | plt.plot(t, x[0, :] - x_hat[0, :], "C0", label="Error") 199 | plt.ylabel(r"$e_1$ [m]") 200 | plt.setp(ax1, xticklabels=[]) 201 | plt.grid(color="0.95") 202 | plt.legend() 203 | ax2 = plt.subplot(312) 204 | sigma[1, :] = np.sqrt(s1 * P_hat[1, 1, :]) 205 | plt.fill_between( 206 | t, 207 | -sigma[1, :], 208 | sigma[1, :], 209 | color="C0", 210 | alpha=0.2, 211 | label=str(100 * (1 - alpha)) + " \% Confidence", 212 | ) 213 | plt.plot(t, x[1, :] - x_hat[1, :], "C0", label="Error") 214 | plt.ylabel(r"$e_2$ [m]") 215 | plt.setp(ax2, xticklabels=[]) 216 | plt.grid(color="0.95") 217 | ax3 = plt.subplot(313) 218 | sigma[2, :] = np.sqrt(s1 * P_hat[2, 2, :]) 219 | plt.fill_between( 220 | t, 221 | -sigma[2, :], 222 | sigma[2, :], 223 | color="C0", 224 | alpha=0.2, 225 | label=str(100 * (1 - alpha)) + " \% Confidence", 226 | ) 227 | plt.plot(t, x[2, :] - x_hat[2, :], "C0", label="Error") 228 | plt.ylabel(r"$e_3$ [rad]") 229 | plt.xlabel(r"$t$ [s]") 230 | plt.grid(color="0.95") 231 | 232 | # Save the plot 233 | plt.savefig("../agv-book/figs/ch5/diffdrive_GNSS_EKF_fig2.pdf") 234 | 235 | # Show the plots to the screen 236 | plt.show() 237 | 238 | # %% 239 | # PLOT THE NOISY GNSS DATA 240 | 241 | fig3 = plt.figure(3) 242 | ax1 = plt.subplot(211) 243 | plt.plot(t, y[0, :], "C1", label="GNSS measurement") 244 | plt.plot(t, x[0, :], "C0", label="Actual") 245 | plt.ylabel(r"$x_1$ [m]") 246 | plt.grid(color="0.95") 247 | plt.setp(ax1, xticklabels=[]) 248 | plt.legend() 249 | ax2 = plt.subplot(212) 250 | plt.plot(t, y[1, :], "C1", label="GNSS measurement") 251 | plt.plot(t, x[1, :], "C0", label="Actual") 252 | plt.ylabel(r"$x_2$ [m]") 253 | plt.xlabel(r"$t$ [s]") 254 | plt.grid(color="0.95") 255 | 256 | # Save the plot 257 | plt.savefig("../agv-book/figs/ch5/diffdrive_GNSS_EKF_fig3.pdf") 258 | 259 | # Show the plots to the screen 260 | plt.show() 261 | 262 | # %% 263 | # SIMULATE AND PLOT WITH GNSS + ODOMETRY FUSION 264 | 265 | # Find the scaling factor for plotting covariance bounds 266 | alpha = 0.01 267 | s1 = chi2.isf(alpha, 1) 268 | s2 = chi2.isf(alpha, 2) 269 | 270 | # Estimate the process and measurement noise covariances 271 | Q_hat = Q 272 | R_hat = R 273 | 274 | for k in range(1, N): 275 | 276 | # Simulate the differential drive vehicle's motion 277 | u[:, k] = vehicle.uni2diff(u_unicycle) 278 | x[:, k] = rk_four(vehicle.f, x[:, k - 1], u[:, k - 1], T) 279 | 280 | # Simulate the vehicle speed estimate 281 | u_m[0, k] = u_unicycle[0] + np.power(Q[0, 0], 0.5) * np.random.randn(1)[0] 282 | 283 | # Simulate the angular rate gyroscope measurement 284 | u_m[1, k] = u_unicycle[1] + np.power(Q[1, 1], 0.5) * np.random.randn(1)[0] 285 | 286 | # Simulate the GNSS measurement 287 | y[0, k] = x[0, k] + np.power(R[0, 0], 0.5) * np.random.randn(1)[0] 288 | y[1, k] = x[1, k] + np.power(R[1, 1], 0.5) * np.random.randn(1)[0] 289 | 290 | # Run the EKF estimator 291 | x_hat[:, k], P_hat[:, :, k] = unicycle_GNSS_ekf( 292 | u_m[:, k], y[:, k], Q_hat, R_hat, x_hat[:, k - 1], P_hat[:, :, k - 1], T 293 | ) 294 | 295 | # Set the ranges for error uncertainty axes 296 | x_range = 2.0 297 | y_range = 2.0 298 | theta_range = 0.2 299 | 300 | # Plot the estimation error with covariance bounds 301 | sigma = np.zeros((3, N)) 302 | fig4 = plt.figure(4) 303 | ax1 = plt.subplot(311) 304 | sigma[0, :] = np.sqrt(s1 * P_hat[0, 0, :]) 305 | plt.fill_between( 306 | t, 307 | -sigma[0, :], 308 | sigma[0, :], 309 | color="C0", 310 | alpha=0.2, 311 | label=str(100 * (1 - alpha)) + " \% Confidence", 312 | ) 313 | plt.plot(t, x[0, :] - x_hat[0, :], "C0", label="Error") 314 | plt.ylabel(r"$e_1$ [m]") 315 | plt.setp(ax1, xticklabels=[]) 316 | ax1.set_ylim([-x_range, x_range]) 317 | plt.grid(color="0.95") 318 | plt.legend() 319 | ax2 = plt.subplot(312) 320 | sigma[1, :] = np.sqrt(s1 * P_hat[1, 1, :]) 321 | plt.fill_between(t, -sigma[1, :], sigma[1, :], color="C0", alpha=0.2) 322 | plt.plot(t, x[1, :] - x_hat[1, :], "C0") 323 | plt.ylabel(r"$e_2$ [m]") 324 | plt.setp(ax2, xticklabels=[]) 325 | ax2.set_ylim([-y_range, y_range]) 326 | plt.grid(color="0.95") 327 | ax3 = plt.subplot(313) 328 | sigma[2, :] = np.sqrt(s1 * P_hat[2, 2, :]) 329 | plt.fill_between( 330 | t, 331 | -sigma[2, :], 332 | sigma[2, :], 333 | color="C0", 334 | alpha=0.2, 335 | label=str(100 * (1 - alpha)) + " \% Confidence", 336 | ) 337 | plt.plot(t, x[2, :] - x_hat[2, :], "C0", label="Error") 338 | ax3.set_ylim([-theta_range, theta_range]) 339 | plt.ylabel(r"$e_3$ [rad]") 340 | plt.xlabel(r"$t$ [s]") 341 | plt.grid(color="0.95") 342 | 343 | # Save the plot 344 | plt.savefig("../agv-book/figs/ch5/diffdrive_GNSS_EKF_fig4.pdf") 345 | 346 | # Show the plots to the screen 347 | plt.show() 348 | 349 | # %% 350 | # MAKE AN ANIMATION 351 | 352 | # Create and save the animation 353 | ani = vehicle.animate_estimation( 354 | x, x_hat, P_hat, alpha, T, True, "../agv-book/gifs/ch5/diffdrive_GNSS_EKF.gif" 355 | ) 356 | 357 | # Show the movie to the screen 358 | plt.show() 359 | 360 | # # Show animation in HTML output if you are using IPython or Jupyter notebooks 361 | # plt.rc('animation', html='jshtml') 362 | # display(ani) 363 | # plt.close() 364 | -------------------------------------------------------------------------------- /diffdrive_control_mpc.py: -------------------------------------------------------------------------------- 1 | """ 2 | Example diffdrive_control_mpc.py 3 | Author: Joshua A. Marshall 4 | GitHub: https://github.com/botprof/agv-examples 5 | """ 6 | 7 | # %% 8 | # SIMULATION SETUP 9 | 10 | import numpy as np 11 | import matplotlib.pyplot as plt 12 | import cvxpy as cp 13 | from mobotpy.models import DiffDrive 14 | from mobotpy.integration import rk_four 15 | 16 | # Set the simulation time [s] and the sample period [s] 17 | SIM_TIME = 30.0 18 | T = 0.1 19 | 20 | # Create an array of time values [s] 21 | t = np.arange(0.0, SIM_TIME, T) 22 | N = np.size(t) 23 | 24 | # %% 25 | # UNCONSTRAINED MPC CONTROLLER DESIGN 26 | 27 | # Lookahead time steps 28 | P = 50 29 | 30 | # Decide on state and input cost matrices 31 | smallQ = np.diag([1.0, 1.0, 2.0]) 32 | smallR = np.diag([1.0, 1.0]) 33 | 34 | # Create a new desired trajectory time array with sufficient time for the MPC 35 | t_d = np.arange(0.0, SIM_TIME + P * T, T) 36 | 37 | # %% 38 | # VEHICLE SETUP 39 | 40 | # Set the track length of the vehicle [m] 41 | ELL = 1.0 42 | 43 | # Create a vehicle object of type DiffDrive 44 | vehicle = DiffDrive(ELL) 45 | 46 | # %% 47 | # COMPUTE THE REFERENCE TRAJECTORY 48 | 49 | # Radius of the circle [m] 50 | R = 10.0 51 | 52 | # Angular rate [rad/s] at which to traverse the circle 53 | OMEGA = 0.1 54 | 55 | # Pre-compute the desired trajectory 56 | x_d = np.zeros((3, N + P)) 57 | u_d = np.zeros((2, N + P)) 58 | for k in range(0, int(N / 2)): 59 | x_d[0, k] = R * np.sin(OMEGA * t_d[k]) 60 | x_d[1, k] = R * (1 - np.cos(OMEGA * t_d[k])) 61 | x_d[2, k] = OMEGA * t_d[k] 62 | u_d[:, k] = vehicle.uni2diff(np.array([R * OMEGA, OMEGA])) 63 | 64 | for k in range(int(N / 2), N + P): 65 | x_d[0, k] = x_d[0, k - 1] + R * OMEGA * T 66 | x_d[1, k] = x_d[1, k - 1] 67 | x_d[2, k] = 0 68 | u_d[:, k] = vehicle.uni2diff(np.array([R * OMEGA, 0])) 69 | 70 | # %% 71 | # SIMULATE THE CLOSED-LOOP SYSTEM 72 | 73 | # Initial conditions 74 | x_init = np.zeros(3) 75 | x_init[0] = 0.0 76 | x_init[1] = 3.0 77 | x_init[2] = 0.0 78 | 79 | # Setup some arrays 80 | x = np.zeros((3, N)) 81 | u = np.zeros((2, N)) 82 | x[:, 0] = x_init 83 | 84 | for k in range(1, N): 85 | 86 | # Simulate the differential drive vehicle motion 87 | x[:, k] = rk_four(vehicle.f, x[:, k - 1], u[:, k - 1], T) 88 | 89 | # Set vectors for optimization 90 | x_MPC = cp.Variable((3, P)) 91 | u_MPC = cp.Variable((2, P)) 92 | 93 | # Initialize the cost function and constraints 94 | J = 0 95 | constraints = [] 96 | 97 | # For each lookahead step 98 | for j in range(0, P): 99 | 100 | # Compute the approximate linearization 101 | F = np.array( 102 | [ 103 | [ 104 | 1, 105 | 0, 106 | -T * 0.5 107 | * (u_d[0, k + j - 1] + u_d[1, k + j - 1]) 108 | * np.sin(x_d[2, k + j - 1]), 109 | ], 110 | [ 111 | 0, 112 | 1, 113 | T * 0.5 114 | * (u_d[0, k + j - 1] + u_d[1, k + j - 1]) 115 | * np.cos(x_d[2, k + j - 1]), 116 | ], 117 | [0, 0, 1], 118 | ] 119 | ) 120 | G = T * np.array( 121 | [ 122 | [0.5 * np.cos(x_d[2, k + j - 1]), 0.5 * np.cos(x_d[2, k + j - 1])], 123 | [0.5 * np.sin(x_d[2, k + j - 1]), 0.5 * np.sin(x_d[2, k + j - 1])], 124 | [-1 / ELL, 1 / ELL], 125 | ] 126 | ) 127 | 128 | # Increment the cost function 129 | J += cp.quad_form(x_MPC[:, j] - x_d[:, k + j], smallQ) + cp.quad_form( 130 | u_MPC[:, j], smallR 131 | ) 132 | 133 | # Enter the "subject to" constraints 134 | constraints += [ 135 | x_MPC[:, j] 136 | == x_d[:, k + j] 137 | + F @ (x_MPC[:, j - 1] - x_d[:, k + j - 1]) 138 | + G @ (u_MPC[:, j - 1] - u_d[:, k + j - 1]) 139 | ] 140 | constraints += [x_MPC[:, 0] == x[:, k]] 141 | # constraints += [u_MPC[:, j] <= 1.5 * np.ones(2)] 142 | # constraints += [u_MPC[:, j] >= -1.5 * np.ones(2)] 143 | 144 | # Solve the optimization problem 145 | problem = cp.Problem(cp.Minimize(J), constraints) 146 | problem.solve(verbose=False) 147 | 148 | # Set the control input to the first element of the solution 149 | u[:, k] = u_MPC[:, 0].value 150 | 151 | # %% 152 | # MAKE PLOTS 153 | 154 | # Change some plot settings (optional) 155 | plt.rc("text", usetex=True) 156 | plt.rc("text.latex", preamble=r"\usepackage{cmbright,amsmath,bm}") 157 | plt.rc("savefig", format="pdf") 158 | plt.rc("savefig", bbox="tight") 159 | 160 | # Plot the states as a function of time 161 | fig1 = plt.figure(1) 162 | fig1.set_figheight(6.4) 163 | ax1a = plt.subplot(411) 164 | plt.plot(t, x_d[0, 0:N], "C1--") 165 | plt.plot(t, x[0, :], "C0") 166 | plt.grid(color="0.95") 167 | plt.ylabel(r"$x$ [m]") 168 | plt.setp(ax1a, xticklabels=[]) 169 | plt.legend(["Desired", "Actual"]) 170 | ax1b = plt.subplot(412) 171 | plt.plot(t, x_d[1, 0:N], "C1--") 172 | plt.plot(t, x[1, :], "C0") 173 | plt.grid(color="0.95") 174 | plt.ylabel(r"$y$ [m]") 175 | plt.setp(ax1b, xticklabels=[]) 176 | ax1c = plt.subplot(413) 177 | plt.plot(t, x_d[2, 0:N] * 180.0 / np.pi, "C1--") 178 | plt.plot(t, x[2, :] * 180.0 / np.pi, "C0") 179 | plt.grid(color="0.95") 180 | plt.ylabel(r"$\theta$ [deg]") 181 | plt.setp(ax1c, xticklabels=[]) 182 | ax1d = plt.subplot(414) 183 | plt.step(t, u[0, :], "C2", where="post", label="$v_L$") 184 | plt.step(t, u[1, :], "C3", where="post", label="$v_R$") 185 | plt.grid(color="0.95") 186 | plt.ylabel(r"$\bm{u}$ [m/s]") 187 | plt.xlabel(r"$t$ [s]") 188 | plt.legend() 189 | 190 | # Save the plot 191 | # plt.savefig("../agv-book/figs/ch4/control_approx_linearization_fig1.pdf") 192 | 193 | # Plot the position of the vehicle in the plane 194 | fig2 = plt.figure(2) 195 | plt.plot(x_d[0, 0:N], x_d[1, 0:N], "C1--", label="Desired") 196 | plt.plot(x[0, :], x[1, :], "C0", label="Actual") 197 | plt.axis("equal") 198 | X_L, Y_L, X_R, Y_R, X_B, Y_B, X_C, Y_C = vehicle.draw(x[0, 0], x[1, 0], x[2, 0]) 199 | plt.fill(X_L, Y_L, "k") 200 | plt.fill(X_R, Y_R, "k") 201 | plt.fill(X_C, Y_C, "k") 202 | plt.fill(X_B, Y_B, "C2", alpha=0.5, label="Start") 203 | X_L, Y_L, X_R, Y_R, X_B, Y_B, X_C, Y_C = vehicle.draw( 204 | x[0, N - 1], x[1, N - 1], x[2, N - 1] 205 | ) 206 | plt.fill(X_L, Y_L, "k") 207 | plt.fill(X_R, Y_R, "k") 208 | plt.fill(X_C, Y_C, "k") 209 | plt.fill(X_B, Y_B, "C3", alpha=0.5, label="End") 210 | plt.xlabel(r"$x$ [m]") 211 | plt.ylabel(r"$y$ [m]") 212 | plt.legend() 213 | 214 | # Save the plot 215 | # plt.savefig("../agv-book/figs/ch4/control_approx_linearization_fig2.pdf") 216 | 217 | # Show all the plots to the screen 218 | plt.show() 219 | 220 | # %% 221 | # MAKE AN ANIMATION 222 | 223 | # Create the animation 224 | ani = vehicle.animate_trajectory(x, x_d, T) 225 | 226 | # Create and save the animation 227 | # ani = vehicle.animate_trajectory( 228 | # x, x_d, T, True, "../agv-book/gifs/ch4/control_approx_linearization.gif" 229 | # ) 230 | 231 | # Show the movie to the screen 232 | plt.show() 233 | 234 | # Show animation in HTML output if you are using IPython or Jupyter notebooks 235 | # from IPython.display import display 236 | 237 | # plt.rc("animation", html="jshtml") 238 | # display(ani) 239 | # plt.close() 240 | -------------------------------------------------------------------------------- /diffdrive_control_mpc_alternate.py: -------------------------------------------------------------------------------- 1 | """ 2 | Example diffdrive_control_mpc.py 3 | Author: Joshua A. Marshall 4 | GitHub: https://github.com/botprof/agv-examples 5 | """ 6 | 7 | # %% 8 | # SIMULATION SETUP 9 | 10 | import numpy as np 11 | import matplotlib.pyplot as plt 12 | import cvxpy as cp 13 | from scipy import signal 14 | from mobotpy.models import DiffDrive 15 | from mobotpy.integration import rk_four 16 | 17 | # Set the simulation time [s] and the sample period [s] 18 | SIM_TIME = 30.0 19 | T = 0.1 20 | 21 | # Create an array of time values [s] 22 | t = np.arange(0.0, SIM_TIME, T) 23 | N = np.size(t) 24 | 25 | # %% 26 | # UNCONSTRAINED MPC CONTROLLER DESIGN 27 | 28 | # Lookahead time steps 29 | P = 50 30 | 31 | # Decide on state and input cost matrices 32 | smallQ = np.diag([1.0, 1.0, 2.0]) 33 | smallR = np.diag([1.0, 1.0]) 34 | 35 | # Create a new desired trajectory time array with sufficient time for the MPC 36 | t_d = np.arange(0.0, SIM_TIME + P * T, T) 37 | 38 | # %% 39 | # VEHICLE SETUP 40 | 41 | # Set the track length of the vehicle [m] 42 | ELL = 1.0 43 | 44 | # Create a vehicle object of type DiffDrive 45 | vehicle = DiffDrive(ELL) 46 | 47 | # %% 48 | # COMPUTE THE REFERENCE TRAJECTORY 49 | 50 | # Radius of the circle [m] 51 | R = 10.0 52 | 53 | # Angular rate [rad/s] at which to traverse the circle 54 | OMEGA = 0.1 55 | 56 | # Pre-compute the desired trajectory 57 | x_d = np.zeros((3, N + P)) 58 | u_d = np.zeros((2, N + P)) 59 | for k in range(0, int(N / 2)): 60 | x_d[0, k] = R * np.sin(OMEGA * t_d[k]) 61 | x_d[1, k] = R * (1 - np.cos(OMEGA * t_d[k])) 62 | x_d[2, k] = OMEGA * t_d[k] 63 | u_d[:, k] = vehicle.uni2diff(np.array([R * OMEGA, OMEGA])) 64 | 65 | for k in range(int(N / 2), N + P): 66 | x_d[0, k] = x_d[0, k - 1] + R * OMEGA * T 67 | x_d[1, k] = x_d[1, k - 1] 68 | x_d[2, k] = 0 69 | u_d[:, k] = vehicle.uni2diff(np.array([R * OMEGA, 0])) 70 | 71 | # %% 72 | # SIMULATE THE CLOSED-LOOP SYSTEM 73 | 74 | # Initial conditions 75 | x_init = np.zeros(3) 76 | x_init[0] = 0.0 77 | x_init[1] = 3.0 78 | x_init[2] = 0.0 79 | 80 | # Setup some arrays 81 | x = np.zeros((3, N)) 82 | u = np.zeros((2, N)) 83 | x[:, 0] = x_init 84 | 85 | for k in range(1, N): 86 | 87 | # Simulate the differential drive vehicle motion 88 | x[:, k] = rk_four(vehicle.f, x[:, k - 1], u[:, k - 1], T) 89 | 90 | # Set vectors for optimization 91 | x_MPC = cp.Variable((3, P)) 92 | u_MPC = cp.Variable((2, P)) 93 | 94 | # Initialize the cost function and constraints 95 | J = 0 96 | constraints = [] 97 | 98 | # For each lookahead step 99 | for j in range(0, P): 100 | 101 | # Compute the approximate linearization 102 | A = np.array( 103 | [ 104 | [ 105 | 0, 106 | 0, 107 | -0.5 108 | * (u_d[0, k + j - 1] + u_d[1, k + j - 1]) 109 | * np.sin(x_d[2, k + j - 1]), 110 | ], 111 | [ 112 | 0, 113 | 0, 114 | 0.5 115 | * (u_d[0, k + j - 1] + u_d[1, k + j - 1]) 116 | * np.cos(x_d[2, k + j - 1]), 117 | ], 118 | [0, 0, 0], 119 | ] 120 | ) 121 | B = np.array( 122 | [ 123 | [0.5 * np.cos(x_d[2, k + j - 1]), 0.5 * np.cos(x_d[2, k + j - 1])], 124 | [0.5 * np.sin(x_d[2, k + j - 1]), 0.5 * np.sin(x_d[2, k + j - 1])], 125 | [-1 / ELL, 1 / ELL], 126 | ] 127 | ) 128 | 129 | # Find a discrete time model of the system using zero-order hold 130 | d_model = signal.cont2discrete( 131 | (A, B, np.eye(3), np.zeros((3, 2))), T, method="zoh" 132 | ) 133 | F = d_model[0] 134 | G = d_model[1] 135 | 136 | # Increment the cost function 137 | J += cp.quad_form(x_MPC[:, j] - x_d[:, k + j], smallQ) + cp.quad_form( 138 | u_MPC[:, j], smallR 139 | ) 140 | 141 | # Enter the "subject to" constraints 142 | constraints += [ 143 | x_MPC[:, j] 144 | == x_d[:, k + j] 145 | + F @ (x_MPC[:, j - 1] - x_d[:, k + j - 1]) 146 | + G @ (u_MPC[:, j - 1] - u_d[:, k + j - 1]) 147 | ] 148 | constraints += [x_MPC[:, 0] == x[:, k]] 149 | # constraints += [u_MPC[:, j] <= 1.5 * np.ones(2)] 150 | # constraints += [u_MPC[:, j] >= -1.5 * np.ones(2)] 151 | 152 | # Solve the optimization problem 153 | problem = cp.Problem(cp.Minimize(J), constraints) 154 | problem.solve(verbose=False) 155 | 156 | # Set the control input to the first element of the solution 157 | u[:, k] = u_MPC[:, 0].value 158 | 159 | # %% 160 | # MAKE PLOTS 161 | 162 | # Change some plot settings (optional) 163 | plt.rc("text", usetex=True) 164 | plt.rc("text.latex", preamble=r"\usepackage{cmbright,amsmath,bm}") 165 | plt.rc("savefig", format="pdf") 166 | plt.rc("savefig", bbox="tight") 167 | 168 | # Plot the states as a function of time 169 | fig1 = plt.figure(1) 170 | fig1.set_figheight(6.4) 171 | ax1a = plt.subplot(411) 172 | plt.plot(t, x_d[0, 0:N], "C1--") 173 | plt.plot(t, x[0, :], "C0") 174 | plt.grid(color="0.95") 175 | plt.ylabel(r"$x$ [m]") 176 | plt.setp(ax1a, xticklabels=[]) 177 | plt.legend(["Desired", "Actual"]) 178 | ax1b = plt.subplot(412) 179 | plt.plot(t, x_d[1, 0:N], "C1--") 180 | plt.plot(t, x[1, :], "C0") 181 | plt.grid(color="0.95") 182 | plt.ylabel(r"$y$ [m]") 183 | plt.setp(ax1b, xticklabels=[]) 184 | ax1c = plt.subplot(413) 185 | plt.plot(t, x_d[2, 0:N] * 180.0 / np.pi, "C1--") 186 | plt.plot(t, x[2, :] * 180.0 / np.pi, "C0") 187 | plt.grid(color="0.95") 188 | plt.ylabel(r"$\theta$ [deg]") 189 | plt.setp(ax1c, xticklabels=[]) 190 | ax1d = plt.subplot(414) 191 | plt.step(t, u[0, :], "C2", where="post", label="$v_L$") 192 | plt.step(t, u[1, :], "C3", where="post", label="$v_R$") 193 | plt.grid(color="0.95") 194 | plt.ylabel(r"$\bm{u}$ [m/s]") 195 | plt.xlabel(r"$t$ [s]") 196 | plt.legend() 197 | 198 | # Save the plot 199 | # plt.savefig("../agv-book/figs/ch4/control_approx_linearization_fig1.pdf") 200 | 201 | # Plot the position of the vehicle in the plane 202 | fig2 = plt.figure(2) 203 | plt.plot(x_d[0, 0:N], x_d[1, 0:N], "C1--", label="Desired") 204 | plt.plot(x[0, :], x[1, :], "C0", label="Actual") 205 | plt.axis("equal") 206 | X_L, Y_L, X_R, Y_R, X_B, Y_B, X_C, Y_C = vehicle.draw(x[0, 0], x[1, 0], x[2, 0]) 207 | plt.fill(X_L, Y_L, "k") 208 | plt.fill(X_R, Y_R, "k") 209 | plt.fill(X_C, Y_C, "k") 210 | plt.fill(X_B, Y_B, "C2", alpha=0.5, label="Start") 211 | X_L, Y_L, X_R, Y_R, X_B, Y_B, X_C, Y_C = vehicle.draw( 212 | x[0, N - 1], x[1, N - 1], x[2, N - 1] 213 | ) 214 | plt.fill(X_L, Y_L, "k") 215 | plt.fill(X_R, Y_R, "k") 216 | plt.fill(X_C, Y_C, "k") 217 | plt.fill(X_B, Y_B, "C3", alpha=0.5, label="End") 218 | plt.xlabel(r"$x$ [m]") 219 | plt.ylabel(r"$y$ [m]") 220 | plt.legend() 221 | 222 | # Save the plot 223 | # plt.savefig("../agv-book/figs/ch4/control_approx_linearization_fig2.pdf") 224 | 225 | # Show all the plots to the screen 226 | plt.show() 227 | 228 | # %% 229 | # MAKE AN ANIMATION 230 | 231 | # Create the animation 232 | ani = vehicle.animate_trajectory(x, x_d, T) 233 | 234 | # Create and save the animation 235 | # ani = vehicle.animate_trajectory( 236 | # x, x_d, T, True, "../agv-book/gifs/ch4/control_approx_linearization.gif" 237 | # ) 238 | 239 | # Show the movie to the screen 240 | # plt.show() 241 | 242 | # Show animation in HTML output if you are using IPython or Jupyter notebooks 243 | from IPython.display import display 244 | 245 | plt.rc("animation", html="jshtml") 246 | display(ani) 247 | plt.close() 248 | -------------------------------------------------------------------------------- /diffdrive_kinematic.py: -------------------------------------------------------------------------------- 1 | """ 2 | Example diffdrive_kinematic.py 3 | Author: Joshua A. Marshall 4 | GitHub: https://github.com/botprof/agv-examples 5 | """ 6 | 7 | # %% 8 | # SIMULATION SETUP 9 | 10 | import numpy as np 11 | import matplotlib.pyplot as plt 12 | from mobotpy.models import DiffDrive 13 | 14 | # Set the simulation time [s] and the sample period [s] 15 | SIM_TIME = 30.0 16 | T = 0.04 17 | 18 | # Create an array of time values [s] 19 | t = np.arange(0.0, SIM_TIME, T) 20 | N = np.size(t) 21 | 22 | # Set the track of the vehicle [m] 23 | ELL = 0.35 24 | 25 | # %% 26 | # FUNCTION DEFINITIONS 27 | 28 | 29 | def rk_four(f, x, u, T): 30 | """Fourth-order Runge-Kutta numerical integration.""" 31 | k_1 = f(x, u) 32 | k_2 = f(x + T * k_1 / 2.0, u) 33 | k_3 = f(x + T * k_2 / 2.0, u) 34 | k_4 = f(x + T * k_3, u) 35 | x_new = x + T / 6.0 * (k_1 + 2.0 * k_2 + 2.0 * k_3 + k_4) 36 | return x_new 37 | 38 | 39 | def diffdrive_f(x, u): 40 | """Differential drive kinematic vehicle model.""" 41 | f = np.zeros(3) 42 | f[0] = 0.5 * (u[0] + u[1]) * np.cos(x[2]) 43 | f[1] = 0.5 * (u[0] + u[1]) * np.sin(x[2]) 44 | f[2] = 1.0 / ELL * (u[1] - u[0]) 45 | return f 46 | 47 | 48 | def uni2diff(u): 49 | """Convert speed and angular rate to wheel speeds.""" 50 | v = u[0] 51 | omega = u[1] 52 | v_L = v - ELL / 2 * omega 53 | v_R = v + ELL / 2 * omega 54 | return np.array([v_L, v_R]) 55 | 56 | 57 | def openloop(t): 58 | """Specify open loop speed and angular rate inputs.""" 59 | v = 0.5 60 | omega = 0.5 * np.sin(10 * t * np.pi / 180.0) 61 | return np.array([v, omega]) 62 | 63 | 64 | # %% 65 | # RUN SIMULATION 66 | 67 | # Initialize arrays that will be populated with our inputs and states 68 | x = np.zeros((3, N)) 69 | u = np.zeros((2, N)) 70 | 71 | # Set the initial pose [m, m, rad], velocities [m/s, m/s] 72 | x[0, 0] = 0.0 73 | x[1, 0] = 0.0 74 | x[2, 0] = np.pi / 2.0 75 | u[:, 0] = uni2diff(openloop(t[0])) 76 | 77 | # Run the simulation 78 | for k in range(1, N): 79 | x[:, k] = rk_four(diffdrive_f, x[:, k - 1], u[:, k - 1], T) 80 | u[:, k] = uni2diff(openloop(t[k])) 81 | 82 | # %% 83 | # MAKE PLOTS 84 | 85 | # Change some plot settings (optional) 86 | plt.rc("text", usetex=True) 87 | plt.rc("text.latex", preamble=r"\usepackage{cmbright,amsmath,bm}") 88 | plt.rc("savefig", format="pdf") 89 | plt.rc("savefig", bbox="tight") 90 | 91 | # Plot the states as a function of time 92 | fig1 = plt.figure(1) 93 | fig1.set_figheight(6.4) 94 | ax1a = plt.subplot(411) 95 | plt.plot(t, x[0, :]) 96 | plt.grid(color="0.95") 97 | plt.ylabel(r"$x$ [m]") 98 | plt.setp(ax1a, xticklabels=[]) 99 | ax1b = plt.subplot(412) 100 | plt.plot(t, x[1, :]) 101 | plt.grid(color="0.95") 102 | plt.ylabel(r"$y$ [m]") 103 | plt.setp(ax1b, xticklabels=[]) 104 | ax1c = plt.subplot(413) 105 | plt.plot(t, x[2, :] * 180.0 / np.pi) 106 | plt.grid(color="0.95") 107 | plt.ylabel(r"$\theta$ [deg]") 108 | plt.setp(ax1c, xticklabels=[]) 109 | ax1d = plt.subplot(414) 110 | plt.step(t, u[0, :], "C1", where="post", label="$v_L$") 111 | plt.step(t, u[1, :], "C2", where="post", label="$v_R$") 112 | plt.grid(color="0.95") 113 | plt.ylabel(r"$\bm{u}$ [m/s]") 114 | plt.xlabel(r"$t$ [s]") 115 | plt.legend() 116 | 117 | # Save the plot 118 | plt.savefig("../agv-book/figs/ch3/diffdrive_kinematic_fig1.pdf") 119 | 120 | # Let's now use the class DiffDrive for plotting 121 | vehicle = DiffDrive(ELL) 122 | 123 | # Plot the position of the vehicle in the plane 124 | fig2 = plt.figure(2) 125 | plt.plot(x[0, :], x[1, :], "C0") 126 | plt.axis("equal") 127 | X_L, Y_L, X_R, Y_R, X_B, Y_B, X_C, Y_C = vehicle.draw(x[0, 0], x[1, 0], x[2, 0]) 128 | plt.fill(X_L, Y_L, "k") 129 | plt.fill(X_R, Y_R, "k") 130 | plt.fill(X_C, Y_C, "k") 131 | plt.fill(X_B, Y_B, "C2", alpha=0.5, label="Start") 132 | X_L, Y_L, X_R, Y_R, X_B, Y_B, X_C, Y_C = vehicle.draw( 133 | x[0, N - 1], x[1, N - 1], x[2, N - 1] 134 | ) 135 | plt.fill(X_L, Y_L, "k") 136 | plt.fill(X_R, Y_R, "k") 137 | plt.fill(X_C, Y_C, "k") 138 | plt.fill(X_B, Y_B, "C3", alpha=0.5, label="End") 139 | plt.xlabel(r"$x$ [m]") 140 | plt.ylabel(r"$y$ [m]") 141 | plt.legend() 142 | 143 | # Save the plot 144 | # plt.savefig("../agv-book/figs/ch3/diffdrive_kinematic_fig2.pdf") 145 | 146 | # Show all the plots to the screen 147 | # plt.show() 148 | 149 | # %% 150 | # MAKE AN ANIMATION 151 | 152 | # Create and save the animation 153 | ani = vehicle.animate(x, T, True, "../agv-book/gifs/ch3/diffdrive_kinematic.gif") 154 | 155 | # Show the movie to the screen 156 | plt.show() 157 | 158 | # Show animation in HTML output if you are using IPython or Jupyter notebooks 159 | # from IPython.display import display 160 | 161 | # plt.rc("animation", html="jshtml") 162 | # display(ani) 163 | # plt.close() 164 | -------------------------------------------------------------------------------- /dynamic_extension_tracking.py: -------------------------------------------------------------------------------- 1 | """ 2 | Example dynamic_extension_tracking.py 3 | Author: Joshua A. Marshall 4 | GitHub: https://github.com/botprof/agv-examples 5 | """ 6 | 7 | # %% 8 | # SIMULATION SETUP 9 | 10 | import numpy as np 11 | import matplotlib.pyplot as plt 12 | from mobotpy.models import DiffDrive 13 | from mobotpy.integration import rk_four 14 | from scipy import signal 15 | 16 | # Set the simulation time [s] and the sample period [s] 17 | SIM_TIME = 15.0 18 | T = 0.04 19 | 20 | # Create an array of time values [s] 21 | t = np.arange(0.0, SIM_TIME, T) 22 | N = np.size(t) 23 | 24 | # %% 25 | # COMPUTE THE REFERENCE TRAJECTORY 26 | 27 | # Radius of the circle [m] 28 | R = 10 29 | 30 | # Angular rate [rad/s] at which to traverse the circle 31 | OMEGA = 0.1 32 | 33 | # Pre-compute the desired trajectory 34 | x_d = np.zeros((3, N)) 35 | u_d = np.zeros((2, N)) 36 | xi_d = np.zeros((4, N)) 37 | ddz_d = np.zeros((2, N)) 38 | for k in range(0, N): 39 | x_d[0, k] = R * np.sin(OMEGA * t[k]) 40 | x_d[1, k] = R * (1 - np.cos(OMEGA * t[k])) 41 | x_d[2, k] = OMEGA * t[k] 42 | u_d[0, k] = R * OMEGA 43 | u_d[1, k] = OMEGA 44 | 45 | # Pre-compute the extended system reference trajectory 46 | for k in range(0, N): 47 | xi_d[0, k] = x_d[0, k] 48 | xi_d[1, k] = x_d[1, k] 49 | xi_d[2, k] = u_d[0, k] * np.cos(x_d[2, k]) 50 | xi_d[3, k] = u_d[0, k] * np.sin(x_d[2, k]) 51 | 52 | # Pre-compute the extended system reference acceleration 53 | for k in range(0, N): 54 | ddz_d[0, k] = -u_d[0, k] * u_d[1, k] * np.sin(x_d[2, k]) 55 | ddz_d[1, k] = u_d[0, k] * u_d[1, k] * np.cos(x_d[2, k]) 56 | 57 | # %% 58 | # VEHICLE SETUP 59 | 60 | # Set the track length of the vehicle [m] 61 | ELL = 1.0 62 | 63 | # Create a vehicle object of type DiffDrive 64 | vehicle = DiffDrive(ELL) 65 | 66 | # %% 67 | # SIMULATE THE CLOSED-LOOP SYSTEM 68 | 69 | # Initial conditions 70 | x_init = np.zeros(3) 71 | x_init[0] = 0.0 72 | x_init[1] = 10.0 73 | x_init[2] = 0.0 74 | 75 | # Setup some arrays 76 | x = np.zeros((3, N)) 77 | xi = np.zeros((4, N)) 78 | u = np.zeros((2, N)) 79 | x[:, 0] = x_init 80 | 81 | # Set the initial speed [m/s] to be non-zero to avoid singularity 82 | w = np.zeros(2) 83 | u_unicycle = np.zeros(2) 84 | u_unicycle[0] = u_d[0, 0] 85 | 86 | # Initial extended state 87 | xi[0, 0] = x_init[0] 88 | xi[1, 0] = x_init[1] 89 | xi[2, 0] = u_d[0, 0] * np.cos(x_init[2]) 90 | xi[3, 0] = u_d[0, 0] * np.sin(x_init[2]) 91 | 92 | # Defined feedback linearized state matrices 93 | A = np.array([[0, 0, 1, 0], [0, 0, 0, 1], [0, 0, 0, 0], [0, 0, 0, 0]]) 94 | B = np.array([[0, 0], [0, 0], [1, 0], [0, 1]]) 95 | 96 | # Choose pole locations for closed-loop linear system 97 | p = np.array([-1.0, -2.0, -2.5, -1.5]) 98 | K = signal.place_poles(A, B, p) 99 | 100 | for k in range(1, N): 101 | 102 | # Simulate the vehicle motion 103 | x[:, k] = rk_four(vehicle.f, x[:, k - 1], u[:, k - 1], T) 104 | 105 | # Update the extended system states 106 | xi[0, k] = x[0, k] 107 | xi[1, k] = x[1, k] 108 | xi[2, k] = u_unicycle[0] * np.cos(x[2, k]) 109 | xi[3, k] = u_unicycle[0] * np.sin(x[2, k]) 110 | 111 | # Compute the extended linear system input control signals 112 | eta = K.gain_matrix @ (xi_d[:, k - 1] - xi[:, k - 1]) + ddz_d[:, k - 1] 113 | 114 | # Compute the new (unicycle) vehicle inputs 115 | B_inv = np.array( 116 | [ 117 | [np.cos(x[2, k - 1]), np.sin(x[2, k - 1])], 118 | [-np.sin(x[2, k - 1]) / u_unicycle[0], np.cos(x[2, k - 1]) / u_unicycle[0]], 119 | ] 120 | ) 121 | w = B_inv @ eta 122 | u_unicycle[0] = u_unicycle[0] + T * w[0] 123 | u_unicycle[1] = w[1] 124 | 125 | # Convert unicycle inputs to differential drive wheel speeds 126 | u[:, k] = vehicle.uni2diff(u_unicycle) 127 | 128 | # %% 129 | # MAKE PLOTS 130 | 131 | # Change some plot settings (optional) 132 | plt.rc("text", usetex=True) 133 | plt.rc("text.latex", preamble=r"\usepackage{cmbright,amsmath,bm}") 134 | plt.rc("savefig", format="pdf") 135 | plt.rc("savefig", bbox="tight") 136 | 137 | # Plot the states as a function of time 138 | fig1 = plt.figure(1) 139 | fig1.set_figheight(6.4) 140 | ax1a = plt.subplot(411) 141 | plt.plot(t, x_d[0, :], "C1--") 142 | plt.plot(t, x[0, :], "C0") 143 | plt.grid(color="0.95") 144 | plt.ylabel(r"$x$ [m]") 145 | plt.setp(ax1a, xticklabels=[]) 146 | plt.legend(["Desired", "Actual"]) 147 | ax1b = plt.subplot(412) 148 | plt.plot(t, x_d[1, :], "C1--") 149 | plt.plot(t, x[1, :], "C0") 150 | plt.grid(color="0.95") 151 | plt.ylabel(r"$y$ [m]") 152 | plt.setp(ax1b, xticklabels=[]) 153 | ax1c = plt.subplot(413) 154 | plt.plot(t, x_d[2, :] * 180.0 / np.pi, "C1--") 155 | plt.plot(t, x[2, :] * 180.0 / np.pi, "C0") 156 | plt.grid(color="0.95") 157 | plt.ylabel(r"$\theta$ [deg]") 158 | plt.setp(ax1c, xticklabels=[]) 159 | ax1d = plt.subplot(414) 160 | plt.step(t, u[0, :], "C2", where="post", label="$v_L$") 161 | plt.step(t, u[1, :], "C3", where="post", label="$v_R$") 162 | plt.grid(color="0.95") 163 | plt.ylabel(r"$\bm{u}$ [m/s]") 164 | plt.xlabel(r"$t$ [s]") 165 | plt.legend() 166 | 167 | # Save the plot 168 | plt.savefig("../agv-book/figs/ch4/dynamic_extension_tracking_fig1.pdf") 169 | 170 | # Plot the position of the vehicle in the plane 171 | fig2 = plt.figure(2) 172 | plt.plot(x_d[0, :], x_d[1, :], "C1--", label="Desired") 173 | plt.plot(x[0, :], x[1, :], "C0", label="Actual") 174 | plt.axis("equal") 175 | X_L, Y_L, X_R, Y_R, X_B, Y_B, X_C, Y_C = vehicle.draw(x[0, 0], x[1, 0], x[2, 0]) 176 | plt.fill(X_L, Y_L, "k") 177 | plt.fill(X_R, Y_R, "k") 178 | plt.fill(X_C, Y_C, "k") 179 | plt.fill(X_B, Y_B, "C2", alpha=0.5, label="Start") 180 | X_L, Y_L, X_R, Y_R, X_B, Y_B, X_C, Y_C = vehicle.draw( 181 | x[0, N - 1], x[1, N - 1], x[2, N - 1] 182 | ) 183 | plt.fill(X_L, Y_L, "k") 184 | plt.fill(X_R, Y_R, "k") 185 | plt.fill(X_C, Y_C, "k") 186 | plt.fill(X_B, Y_B, "C3", alpha=0.5, label="End") 187 | plt.xlabel(r"$x$ [m]") 188 | plt.ylabel(r"$y$ [m]") 189 | plt.legend() 190 | 191 | # Save the plot 192 | plt.savefig("../agv-book/figs/ch4/dynamic_extension_tracking_fig2.pdf") 193 | 194 | # Show all the plots to the screen 195 | plt.show() 196 | 197 | # %% 198 | # MAKE AN ANIMATION 199 | 200 | # Create and save the animation 201 | ani = vehicle.animate_trajectory( 202 | x, x_d, T, True, "../agv-book/gifs/ch4/dynamic_extension_tracking.gif" 203 | ) 204 | 205 | # Show the movie to the screen 206 | plt.show() 207 | 208 | # # Show animation in HTML output if you are using IPython or Jupyter notebooks 209 | # plt.rc('animation', html='jshtml') 210 | # display(ani) 211 | # plt.close() 212 | -------------------------------------------------------------------------------- /fws_beacons_ekf.py: -------------------------------------------------------------------------------- 1 | """ 2 | Example fws_beacons_ekf.py 3 | Author: Joshua A. Marshall 4 | GitHub: https://github.com/botprof/agv-examples 5 | """ 6 | 7 | # %% 8 | # SIMULATION SETUP 9 | 10 | import numpy as np 11 | import matplotlib.pyplot as plt 12 | from numpy.linalg import inv 13 | from scipy.stats import chi2 14 | from mobotpy.integration import rk_four 15 | from mobotpy.models import FourWheelSteered 16 | 17 | # Set the simulation time [s] and the sample period [s] 18 | SIM_TIME = 10.0 19 | T = 0.04 20 | 21 | # Create an array of time values [s] 22 | t = np.arange(0.0, SIM_TIME, T) 23 | N = np.size(t) 24 | 25 | # %% 26 | # MOBILE ROBOT SETUP 27 | 28 | # Set the wheelbase and track of the vehicle [m] 29 | ELL_W = 2.50 30 | ELL_T = 1.75 31 | 32 | # Let's now use the class Ackermann for plotting 33 | vehicle = FourWheelSteered(ELL_W, ELL_T) 34 | 35 | # %% 36 | # CREATE A MAP OF FEATURES 37 | 38 | # Set a minimum number of features in the map that achieves observability 39 | N_FEATURES = 5 40 | 41 | # Set the size [m] of a square map 42 | D_MAP = 50.0 43 | 44 | # Create a map of randomly placed feature locations 45 | f_map = np.zeros((2, N_FEATURES)) 46 | for i in range(0, N_FEATURES): 47 | f_map[:, i] = D_MAP * (np.random.rand(2) - 0.5) 48 | 49 | # %% 50 | # SET UP THE NOISE COVARIANCE MATRICES 51 | 52 | # Range sensor noise standard deviation (each feature has the same noise) [m] 53 | SIGMA_W1 = 1.0 54 | 55 | # Steering angle noise standard deviation [rad] 56 | SIGMA_W2 = 0.05 57 | 58 | # Sensor noise covariance matrix 59 | R = np.zeros((N_FEATURES + 1, N_FEATURES + 1)) 60 | R[0:N_FEATURES, 0:N_FEATURES] = np.diag([SIGMA_W1**2] * N_FEATURES) 61 | R[N_FEATURES, N_FEATURES] = SIGMA_W2**2 62 | 63 | # Speed noise standard deviation [m/s] 64 | SIGMA_U1 = 0.1 65 | 66 | # Steering rate noise standard deviation [rad/s] 67 | SIGMA_U2 = 0.2 68 | 69 | # Process noise covariance matrix 70 | Q = np.diag([SIGMA_U1**2, SIGMA_U2**2]) 71 | 72 | # %% 73 | # FUNCTION TO MODEL RANGE TO FEATURES 74 | 75 | 76 | def range_sensor(x, sigma_w, f_map): 77 | """ 78 | Function to model the range sensor. 79 | 80 | Parameters 81 | ---------- 82 | x : ndarray 83 | An array of length 2 representing the robot's position. 84 | sigma_w : float 85 | The range sensor noise standard deviation. 86 | f_map : ndarray 87 | An array of size (2, N_FEATURES) containing map feature locations. 88 | 89 | Returns 90 | ------- 91 | ndarray 92 | The range (including noise) to each feature in the map. 93 | """ 94 | 95 | # Compute the measured range to each feature from the current robot position 96 | z = np.zeros(N_FEATURES) 97 | for j in range(0, N_FEATURES): 98 | z[j] = ( 99 | np.sqrt((f_map[0, j] - x[0]) ** 2 + (f_map[1, j] - x[1]) ** 2) 100 | + sigma_w * np.random.randn() 101 | ) 102 | 103 | # Return the array of noisy measurements 104 | return z 105 | 106 | 107 | # %% 108 | # FUNCTION TO IMPLEMENT THE EKF FOR THE FWS MOBILE ROBOT 109 | 110 | 111 | def fws_ekf(q, P, u, z, Q, R, f_map): 112 | """ 113 | Function to implement an observer for the robot's pose. 114 | 115 | Parameters 116 | ---------- 117 | q : ndarray 118 | An array of length 4 representing the robot's pose. 119 | P : ndarray 120 | The state covariance matrix. 121 | u : ndarray 122 | An array of length 2 representing the robot's inputs. 123 | z : ndarray 124 | The range measurements to the features. 125 | Q : ndarray 126 | The process noise covariance matrix. 127 | R : ndarray 128 | The sensor noise covariance matrix. 129 | f_map : ndarray 130 | An array of size (2, N_FEATURES) containing map feature locations. 131 | 132 | Returns 133 | ------- 134 | ndarray 135 | The estimated state of the robot. 136 | ndarray 137 | The state covariance matrix. 138 | """ 139 | 140 | # Compute the a priori estimate 141 | # (a) Compute the Jacobian matrices (i.e., linearize about current estimate) 142 | F = np.zeros((4, 4)) 143 | F = np.eye(4) + T * np.array( 144 | [ 145 | [ 146 | 0, 147 | 0, 148 | -u[0] * np.sin(q[2]) * np.cos(q[3]), 149 | -u[0] * np.cos(q[2]) * np.sin(q[3]), 150 | ], 151 | [ 152 | 0, 153 | 0, 154 | u[0] * np.cos(q[2]) * np.cos(q[3]), 155 | -u[0] * np.sin(q[2]) * np.sin(q[3]), 156 | ], 157 | [0, 0, 0, u[0] * 1.0 / (0.5 * ELL_W) * np.cos(q[3])], 158 | [0, 0, 0, 0], 159 | ] 160 | ) 161 | G = np.zeros((4, 2)) 162 | G = T * np.array( 163 | [ 164 | [np.cos(q[2]) * np.cos(q[3]), 0], 165 | [np.sin(q[2]) * np.cos(q[3]), 0], 166 | [1.0 / (0.5 * ELL_W) * np.sin(q[3]), 0], 167 | [0, 1], 168 | ] 169 | ) 170 | 171 | # (b) Compute the state covariance matrix and state update 172 | P_new = F @ P @ F.T + G @ Q @ G.T 173 | P_new = 0.5 * (P_new + P_new.T) 174 | q_new = q + T * vehicle.f(q, u) 175 | 176 | # Compute the a posteriori estimate 177 | # (a) Compute the Jacobian matrices (i.e., linearize about current estimate) 178 | H = np.zeros((N_FEATURES + 1, 4)) 179 | for j in range(0, N_FEATURES): 180 | H[j, :] = np.array( 181 | [ 182 | -(f_map[0, j] - q[0]) / range_sensor(q, 0, f_map)[j], 183 | -(f_map[1, j] - q[1]) / range_sensor(q, 0, f_map)[j], 184 | 0, 185 | 0, 186 | ] 187 | ) 188 | # Add a measurement for the steering angle 189 | H[N_FEATURES, :] = np.array([0, 0, 0, 1]) 190 | 191 | # Check the observability of this system 192 | observability_matrix = H 193 | for j in range(1, 4): 194 | observability_matrix = np.concatenate( 195 | (observability_matrix, H @ np.linalg.matrix_power(F, j)), axis=0 196 | ) 197 | if np.linalg.matrix_rank(observability_matrix) < 4: 198 | raise ValueError("System is not observable!") 199 | 200 | # (b) Compute the Kalman gain 201 | K = P_new @ H.T @ inv(H @ P_new @ H.T + R) 202 | 203 | # (c) Compute the state update 204 | z_hat = np.zeros(N_FEATURES + 1) 205 | z_hat[0:N_FEATURES] = range_sensor(q_new, 0, f_map) 206 | z_hat[N_FEATURES] = q_new[3] 207 | q_new = q_new + K @ (z - z_hat) 208 | 209 | # (d) Compute the covariance update 210 | P_new = (np.eye(4) - K @ H) @ P_new @ (np.eye(4) - K @ H).T + K @ R @ K.T 211 | P_new = 0.5 * (P_new + P_new.T) 212 | 213 | # Return the estimated state 214 | return q_new, P_new 215 | 216 | 217 | # %% 218 | # RUN SIMULATION 219 | 220 | # Initialize arrays that will be populated with our inputs and states 221 | x = np.zeros((4, N)) 222 | u = np.zeros((2, N)) 223 | x_hat = np.zeros((4, N)) 224 | P_hat = np.zeros((4, 4, N)) 225 | 226 | # Set the initial pose [m, m, rad, rad], velocities [m/s, rad/s] 227 | x[0, 0] = -2.0 228 | x[1, 0] = 3.0 229 | x[2, 0] = np.pi/8.0 230 | x[3, 0] = 0.0 231 | u[0, 0] = 2.0 232 | u[1, 0] = 0 233 | 234 | # Initialize the state estimate 235 | x_hat[:, 0] = np.array([0.0, 0.0, 0.0, 0.0]) 236 | P_hat[:, :, 0] = np.diag([5.0**2, 5.0**2, (np.pi/4)**2, 0.05**2]) 237 | 238 | # Just drive around and try to localize! 239 | for k in range(1, N): 240 | # Simulate the robot's motion 241 | x[:, k] = rk_four(vehicle.f, x[:, k - 1], u[:, k - 1], T) 242 | # Measure the actual range to each feature 243 | z = np.zeros(N_FEATURES + 1) 244 | z[0:N_FEATURES] = range_sensor(x[:, k], SIGMA_W1, f_map) 245 | z[N_FEATURES] = x[3, k] + SIGMA_W2 * np.random.randn() 246 | 247 | # Use the range measurements to estimate the robot's state 248 | x_hat[:, k], P_hat[:, :, k] = fws_ekf( 249 | x_hat[:, k - 1], P_hat[:, :, k - 1], u[:, k - 1], z, Q, R, f_map 250 | ) 251 | # Choose some new inputs 252 | # Choose some new inputs 253 | u[0, k] = 2.0 254 | u[1, k] = -0.1 * np.sin(1 * t[k]) 255 | 256 | # %% 257 | # MAKE SOME PLOTS 258 | 259 | 260 | # Function to wrap angles to [-pi, pi] 261 | def wrap_to_pi(angle): 262 | """Wrap angles to the range [-pi, pi].""" 263 | return (angle + np.pi) % (2 * np.pi) - np.pi 264 | 265 | 266 | # Change some plot settings (optional) 267 | plt.rc("text", usetex=True) 268 | plt.rc("text.latex", preamble=r"\usepackage{cmbright,amsmath,bm}") 269 | plt.rc("savefig", format="pdf") 270 | plt.rc("savefig", bbox="tight") 271 | 272 | # Find the scaling factor for plotting covariance bounds 273 | ALPHA = 0.01 274 | s1 = chi2.isf(ALPHA, 1) 275 | s2 = chi2.isf(ALPHA, 2) 276 | 277 | # Plot the position of the vehicle in the plane 278 | fig1 = plt.figure(1) 279 | plt.plot(f_map[0, :], f_map[1, :], "C4*", label="Feature") 280 | plt.plot(x[0, :], x[1, :], "C0", label="Actual") 281 | plt.plot(x_hat[0, :], x_hat[1, :], "C1--", label="Estimated") 282 | plt.axis("equal") 283 | X_BL, Y_BL, X_BR, Y_BR, X_FL, Y_FL, X_FR, Y_FR, X_BD, Y_BD = vehicle.draw(x[:, 0]) 284 | plt.fill(X_BL, Y_BL, "k") 285 | plt.fill(X_BR, Y_BR, "k") 286 | plt.fill(X_FR, Y_FR, "k") 287 | plt.fill(X_FL, Y_FL, "k") 288 | plt.fill(X_BD, Y_BD, "C2", alpha=0.5, label="Start") 289 | X_BL, Y_BL, X_BR, Y_BR, X_FL, Y_FL, X_FR, Y_FR, X_BD, Y_BD = vehicle.draw(x[:, N - 1]) 290 | plt.fill(X_BL, Y_BL, "k") 291 | plt.fill(X_BR, Y_BR, "k") 292 | plt.fill(X_FR, Y_FR, "k") 293 | plt.fill(X_FL, Y_FL, "k") 294 | plt.fill(X_BD, Y_BD, "C3", alpha=0.5, label="End") 295 | plt.xlabel(r"$x$ [m]") 296 | plt.ylabel(r"$y$ [m]") 297 | plt.legend() 298 | 299 | # Plot the states as a function of time 300 | fig2 = plt.figure(2) 301 | fig2.set_figheight(6.4) 302 | ax2a = plt.subplot(411) 303 | plt.plot(t, x[0, :], "C0", label="Actual") 304 | plt.plot(t, x_hat[0, :], "C1--", label="Estimated") 305 | plt.grid(color="0.95") 306 | plt.ylabel(r"$x$ [m]") 307 | plt.setp(ax2a, xticklabels=[]) 308 | plt.legend() 309 | ax2b = plt.subplot(412) 310 | plt.plot(t, x[1, :], "C0", label="Actual") 311 | plt.plot(t, x_hat[1, :], "C1--", label="Estimated") 312 | plt.grid(color="0.95") 313 | plt.ylabel(r"$y$ [m]") 314 | plt.setp(ax2b, xticklabels=[]) 315 | ax2c = plt.subplot(413) 316 | plt.plot(t, wrap_to_pi(x[2, :]) * 180.0 / np.pi, "C0", label="Actual") 317 | plt.plot(t, wrap_to_pi(x_hat[2, :]) * 180.0 / np.pi, "C1--", label="Estimated") 318 | plt.ylabel(r"$\theta$ [deg]") 319 | plt.grid(color="0.95") 320 | plt.setp(ax2c, xticklabels=[]) 321 | ax2d = plt.subplot(414) 322 | plt.plot(t, wrap_to_pi(x[3, :]) * 180.0 / np.pi, "C0", label="Actual") 323 | plt.plot(t, wrap_to_pi(x_hat[3, :]) * 180.0 / np.pi, "C1--", label="Estimated") 324 | plt.ylabel(r"$\phi$ [deg]") 325 | plt.grid(color="0.95") 326 | plt.xlabel(r"$t$ [s]") 327 | 328 | # Plot the estimator errors as a function of time 329 | sigma = np.zeros((4, N)) 330 | fig3 = plt.figure(3) 331 | fig3.set_figheight(6.4) 332 | ax3a = plt.subplot(411) 333 | sigma[0, :] = np.sqrt(s1 * P_hat[0, 0, :]) 334 | plt.fill_between( 335 | t, 336 | -sigma[0, :], 337 | sigma[0, :], 338 | color="C0", 339 | alpha=0.2, 340 | label=str(100 * (1 - ALPHA)) + r" \% Confidence", 341 | ) 342 | plt.plot(t, x[0, :] - x_hat[0, :], "C0", label="Error") 343 | plt.grid(color="0.95") 344 | plt.ylabel(r"$e_x$ [m]") 345 | plt.setp(ax2a, xticklabels=[]) 346 | plt.legend() 347 | ax3b = plt.subplot(412) 348 | sigma[1, :] = np.sqrt(s1 * P_hat[1, 1, :]) 349 | plt.fill_between( 350 | t, 351 | -sigma[1, :], 352 | sigma[1, :], 353 | color="C0", 354 | alpha=0.2, 355 | label=str(100 * (1 - ALPHA)) + r" \% Confidence", 356 | ) 357 | plt.plot(t, x[1, :] - x_hat[1, :], "C0", label="Error") 358 | plt.grid(color="0.95") 359 | plt.ylabel(r"$e_y$ [m]") 360 | plt.setp(ax2b, xticklabels=[]) 361 | ax3c = plt.subplot(413) 362 | sigma[2, :] = np.sqrt(s1 * P_hat[2, 2, :]) 363 | plt.fill_between( 364 | t, 365 | -sigma[2, :] * 180.0 / np.pi, 366 | sigma[2, :] * 180.0 / np.pi, 367 | color="C0", 368 | alpha=0.2, 369 | label=str(100 * (1 - ALPHA)) + r" \% Confidence", 370 | ) 371 | plt.plot(t, (x[2, :] - x_hat[2, :]) * 180.0 / np.pi, "C0", label="Error") 372 | plt.ylabel(r"$e_\theta$ [deg]") 373 | plt.grid(color="0.95") 374 | plt.setp(ax2c, xticklabels=[]) 375 | ax3d = plt.subplot(414) 376 | sigma[3, :] = np.sqrt(s1 * P_hat[3, 3, :]) 377 | plt.fill_between( 378 | t, 379 | -sigma[3, :] * 180.0 / np.pi, 380 | sigma[3, :] * 180.0 / np.pi, 381 | color="C0", 382 | alpha=0.2, 383 | label=str(100 * (1 - ALPHA)) + r" \% Confidence", 384 | ) 385 | plt.plot(t, (x[3, :] - x_hat[3, :]) * 180 / np.pi, "C0", label="Error") 386 | plt.ylabel(r"$e_\phi$ [deg]") 387 | plt.grid(color="0.95") 388 | plt.xlabel(r"$t$ [s]") 389 | 390 | # Show all the plots to the screen 391 | plt.show() 392 | 393 | # %% 394 | -------------------------------------------------------------------------------- /fws_beacons_observer.py: -------------------------------------------------------------------------------- 1 | """ 2 | Example fws_beacons_observer.py 3 | Author: Joshua A. Marshall 4 | GitHub: https://github.com/botprof/agv-examples 5 | """ 6 | 7 | # %% 8 | # SIMULATION SETUP 9 | 10 | import numpy as np 11 | import matplotlib.pyplot as plt 12 | from scipy import signal 13 | from mobotpy.integration import rk_four 14 | from mobotpy.models import FourWheelSteered 15 | 16 | # Set the simulation time [s] and the sample period [s] 17 | SIM_TIME = 20.0 18 | T = 0.1 19 | 20 | # Create an array of time values [s] 21 | t = np.arange(0.0, SIM_TIME, T) 22 | N = np.size(t) 23 | 24 | # %% 25 | # VEHICLE SETUP 26 | 27 | # Set the wheelbase and track of the vehicle [m] 28 | ELL_W = 2.50 29 | ELL_T = 1.75 30 | 31 | # Let's now use the class Ackermann for plotting 32 | vehicle = FourWheelSteered(ELL_W, ELL_T) 33 | 34 | # %% 35 | # CREATE A MAP OF FEATURES 36 | 37 | # Set the minimum number of features in the map that achieves observability 38 | N_FEATURES = 2 39 | 40 | # Set the size [m] of a square map 41 | D_MAP = 30.0 42 | 43 | # Create a map of randomly placed feature locations 44 | f_map = np.zeros((2, N_FEATURES)) 45 | for i in range(0, N_FEATURES): 46 | f_map[:, i] = D_MAP * (np.random.rand(2) - 0.5) 47 | 48 | # %% 49 | # FUNCTION TO MODEL RANGE TO FEATURES 50 | 51 | 52 | def range_sensor(x, f_map): 53 | """ 54 | Function to model the range sensor. 55 | 56 | Parameters 57 | ---------- 58 | x : ndarray 59 | An array of length 2 representing the robot's position. 60 | f_map : ndarray 61 | An array of size (2, N_FEATURES) containing map feature locations. 62 | 63 | Returns 64 | ------- 65 | ndarray 66 | The range to each feature in the map. 67 | """ 68 | 69 | # Compute the range to each feature from the current robot position 70 | r = np.zeros(N_FEATURES) 71 | for j in range(0, N_FEATURES): 72 | r[j] = np.sqrt((f_map[0, j] - x[0]) ** 2 + (f_map[1, j] - x[1]) ** 2) 73 | 74 | # Return the array of measurements 75 | return r 76 | 77 | 78 | # %% 79 | # FUNCTION TO IMPLEMENT THE OBSERVER 80 | 81 | 82 | def fws_observer(q, u, r, f_map): 83 | """ 84 | Function to implement an observer for the robot's pose. 85 | 86 | Parameters 87 | ---------- 88 | q : ndarray 89 | An array of length 4 representing the (last) robot's pose. 90 | u : ndarray 91 | An array of length 2 representing the robot's inputs. 92 | r : ndarray 93 | An array of length N_FEATURES representing the range to each feature. 94 | f_map : ndarray 95 | An array of size (2, N_FEATURES) containing map feature locations. 96 | 97 | Returns 98 | ------- 99 | ndarray 100 | The estimated pose of the robot. 101 | """ 102 | 103 | # Compute the Jacobian matrices (i.e., linearize about current estimate) 104 | F = np.zeros((4, 4)) 105 | F = np.eye(4) + T * np.array( 106 | [ 107 | [ 108 | 0, 109 | 0, 110 | -u[0] * np.sin(q[2]) * np.cos(q[3]), 111 | -u[0] * np.cos(q[2]) * np.sin(q[3]), 112 | ], 113 | [ 114 | 0, 115 | 0, 116 | u[0] * np.cos(q[2]) * np.cos(q[3]), 117 | -u[0] * np.sin(q[2]) * np.sin(q[3]), 118 | ], 119 | [0, 0, 0, u[0] * 1.0 / (0.5 * ELL_W) * np.cos(q[3])], 120 | [0, 0, 0, 0], 121 | ] 122 | ) 123 | H = np.zeros((N_FEATURES, 4)) 124 | for j in range(0, N_FEATURES): 125 | H[j, :] = np.array( 126 | [ 127 | -(f_map[0, j] - q[0]) / range_sensor(q, f_map)[j], 128 | -(f_map[1, j] - q[1]) / range_sensor(q, f_map)[j], 129 | 0, 130 | 0, 131 | ] 132 | ) 133 | 134 | # Check the observability of this system 135 | observability_matrix = H 136 | for j in range(1, 4): 137 | observability_matrix = np.concatenate( 138 | (observability_matrix, H @ np.linalg.matrix_power(F, j)), axis=0 139 | ) 140 | if np.linalg.matrix_rank(observability_matrix) < 4: 141 | raise ValueError("System is not observable!") 142 | 143 | # Set the desired poles at lambda_z (change these as desired) 144 | lambda_z = np.array([0.8, 0.7, 0.6, 0.5]) 145 | # Compute the observer gain 146 | L = signal.place_poles(F.T, H.T, lambda_z).gain_matrix.T 147 | # Use the pseudo-inverse to compute the observer gain (when overdetermined) 148 | # L = signal.place_poles(F.T, np.eye(4), lambda_z).gain_matrix @ np.linalg.pinv(H) 149 | 150 | # Predict the state using the inputs and the robot's kinematic model 151 | q_new = q + T * vehicle.f(q, u) 152 | # Correct the state using the range measurements 153 | q_new = q_new + L @ (r - range_sensor(q, f_map)) 154 | 155 | # Return the estimated state 156 | return q_new 157 | 158 | 159 | # %% 160 | # RUN SIMULATION 161 | 162 | # Initialize arrays that will be populated with our inputs and states 163 | x = np.zeros((4, N)) 164 | u = np.zeros((2, N)) 165 | x_hat = np.zeros((4, N)) 166 | 167 | # Set the initial pose [m, m, rad, rad], velocities [m/s, rad/s] 168 | x[0, 0] = -5.0 169 | x[1, 0] = -3.0 170 | x[2, 0] = np.pi / 2.0 171 | x[3, 0] = 0.0 172 | u[0, 0] = 5.0 173 | u[1, 0] = 0 174 | 175 | # Just drive around and try to localize! 176 | for k in range(1, N): 177 | # Measure the actual range to each feature 178 | r = range_sensor(x[:, k - 1], f_map) 179 | # Use the range measurements to estimate the robot's state 180 | x_hat[:, k] = fws_observer(x_hat[:, k - 1], u[:, k - 1], r, f_map) 181 | # Choose some new inputs 182 | u[0, k] = 2.0 183 | u[1, k] = -0.1 * np.sin(1 * t[k]) 184 | # Simulate the robot's motion 185 | x[:, k] = rk_four(vehicle.f, x[:, k - 1], u[:, k - 1], T) 186 | 187 | # %% 188 | # MAKE SOME PLOTS 189 | 190 | 191 | # Function to wrap angles to [-pi, pi] 192 | def wrap_to_pi(angle): 193 | """Wrap angles to the range [-pi, pi].""" 194 | return (angle + np.pi) % (2 * np.pi) - np.pi 195 | 196 | 197 | # Change some plot settings (optional) 198 | plt.rc("text", usetex=True) 199 | plt.rc("text.latex", preamble=r"\usepackage{cmbright,amsmath,bm}") 200 | plt.rc("savefig", format="pdf") 201 | plt.rc("savefig", bbox="tight") 202 | 203 | # Plot the position of the vehicle in the plane 204 | fig1 = plt.figure(1) 205 | plt.plot(f_map[0, :], f_map[1, :], "C4*", label="Feature") 206 | plt.plot(x[0, :], x[1, :]) 207 | plt.axis("equal") 208 | X_BL, Y_BL, X_BR, Y_BR, X_FL, Y_FL, X_FR, Y_FR, X_BD, Y_BD = vehicle.draw(x[:, 0]) 209 | plt.fill(X_BL, Y_BL, "k") 210 | plt.fill(X_BR, Y_BR, "k") 211 | plt.fill(X_FR, Y_FR, "k") 212 | plt.fill(X_FL, Y_FL, "k") 213 | plt.fill(X_BD, Y_BD, "C2", alpha=0.5, label="Start") 214 | X_BL, Y_BL, X_BR, Y_BR, X_FL, Y_FL, X_FR, Y_FR, X_BD, Y_BD = vehicle.draw(x[:, N - 1]) 215 | plt.fill(X_BL, Y_BL, "k") 216 | plt.fill(X_BR, Y_BR, "k") 217 | plt.fill(X_FR, Y_FR, "k") 218 | plt.fill(X_FL, Y_FL, "k") 219 | plt.fill(X_BD, Y_BD, "C3", alpha=0.5, label="End") 220 | plt.xlabel(r"$x$ [m]") 221 | plt.ylabel(r"$y$ [m]") 222 | plt.legend() 223 | 224 | # Plot the states as a function of time 225 | fig2 = plt.figure(2) 226 | fig2.set_figheight(6.4) 227 | ax2a = plt.subplot(411) 228 | plt.plot(t, x[0, :], "C0", label="Actual") 229 | plt.plot(t, x_hat[0, :], "C1--", label="Estimated") 230 | plt.grid(color="0.95") 231 | plt.ylabel(r"$x$ [m]") 232 | plt.setp(ax2a, xticklabels=[]) 233 | plt.legend() 234 | ax2b = plt.subplot(412) 235 | plt.plot(t, x[1, :], "C0", label="Actual") 236 | plt.plot(t, x_hat[1, :], "C1--", label="Estimated") 237 | plt.grid(color="0.95") 238 | plt.ylabel(r"$y$ [m]") 239 | plt.setp(ax2b, xticklabels=[]) 240 | ax2c = plt.subplot(413) 241 | plt.plot(t, wrap_to_pi(x[2, :]) * 180.0 / np.pi, "C0", label="Actual") 242 | plt.plot(t, wrap_to_pi(x_hat[2, :]) * 180.0 / np.pi, "C1--", label="Estimated") 243 | plt.ylabel(r"$\theta$ [deg]") 244 | plt.grid(color="0.95") 245 | plt.setp(ax2c, xticklabels=[]) 246 | ax2d = plt.subplot(414) 247 | plt.plot(t, wrap_to_pi(x[3, :]) * 180.0 / np.pi, "C0", label="Actual") 248 | plt.plot(t, wrap_to_pi(x_hat[3, :]) * 180.0 / np.pi, "C1--", label="Estimated") 249 | plt.ylabel(r"$\phi$ [deg]") 250 | plt.grid(color="0.95") 251 | plt.xlabel(r"$t$ [s]") 252 | 253 | # Show all the plots to the screen 254 | plt.show() 255 | -------------------------------------------------------------------------------- /fws_kinematic.py: -------------------------------------------------------------------------------- 1 | """ 2 | Example fws_beacons_observer.py 3 | Author: Joshua A. Marshall 4 | GitHub: https://github.com/botprof/agv-examples 5 | """ 6 | 7 | # %% 8 | # SIMULATION SETUP 9 | 10 | import numpy as np 11 | import matplotlib.pyplot as plt 12 | from mobotpy.integration import rk_four 13 | from mobotpy.models import FourWheelSteered 14 | 15 | # Set the simulation time [s] and the sample period [s] 16 | SIM_TIME = 30.0 17 | T = 0.1 18 | 19 | # Create an array of time values [s] 20 | t = np.arange(0.0, SIM_TIME, T) 21 | N = np.size(t) 22 | 23 | # %% 24 | # VEHICLE SETUP 25 | 26 | # Set the wheelbase and track of the vehicle [m] 27 | ELL_W = 2.50 28 | ELL_T = 1.75 29 | 30 | # Let's now use the class Ackermann for plotting 31 | vehicle = FourWheelSteered(ELL_W, ELL_T) 32 | 33 | # %% 34 | # RUN SIMULATION 35 | 36 | # Initialize arrays that will be populated with our inputs and states 37 | x = np.zeros((4, N)) 38 | u = np.zeros((2, N)) 39 | 40 | # Set the initial pose [m, m, rad, rad], velocities [m/s, rad/s] 41 | x[0, 0] = 0.0 42 | x[1, 0] = 0.0 43 | x[2, 0] = np.pi / 2.0 44 | x[3, 0] = 0.0 45 | u[0, 0] = 5.0 46 | u[1, 0] = 0.0 47 | 48 | # Run the simulation 49 | for k in range(1, N): 50 | x[:, k] = rk_four(vehicle.f, x[:, k - 1], u[:, k - 1], T) 51 | u[0, k] = 5.0 52 | u[1, k] = -0.5 * np.sin(1.0 * t[k]) 53 | 54 | # %% 55 | # MAKE SOME PLOTS 56 | 57 | # Change some plot settings (optional) 58 | plt.rc("text", usetex=True) 59 | plt.rc("text.latex", preamble=r"\usepackage{cmbright,amsmath,bm}") 60 | plt.rc("savefig", format="pdf") 61 | plt.rc("savefig", bbox="tight") 62 | 63 | # Plot the states as a function of time 64 | fig1 = plt.figure(1) 65 | fig1.set_figheight(6.4) 66 | ax1a = plt.subplot(311) 67 | plt.plot(t, x[0, :]) 68 | plt.grid(color="0.95") 69 | plt.ylabel(r"$x$ [m]") 70 | plt.setp(ax1a, xticklabels=[]) 71 | ax1b = plt.subplot(312) 72 | plt.plot(t, x[1, :]) 73 | plt.grid(color="0.95") 74 | plt.ylabel(r"$y$ [m]") 75 | plt.setp(ax1b, xticklabels=[]) 76 | ax1c = plt.subplot(313) 77 | plt.plot(t, x[2, :] * 180.0 / np.pi) 78 | plt.grid(color="0.95") 79 | plt.ylabel(r"$\theta$ [deg]") 80 | plt.xlabel(r"$t$ [s]") 81 | plt.legend() 82 | 83 | # Save the plot 84 | # plt.savefig("../agv-book/figs/ch3/ackermann_kinematic_fig1.pdf") 85 | 86 | # Plot the position of the vehicle in the plane 87 | fig2 = plt.figure(2) 88 | plt.plot(x[0, :], x[1, :]) 89 | plt.axis("equal") 90 | X_BL, Y_BL, X_BR, Y_BR, X_FL, Y_FL, X_FR, Y_FR, X_BD, Y_BD = vehicle.draw(x[:, 0]) 91 | plt.fill(X_BL, Y_BL, "k") 92 | plt.fill(X_BR, Y_BR, "k") 93 | plt.fill(X_FR, Y_FR, "k") 94 | plt.fill(X_FL, Y_FL, "k") 95 | plt.fill(X_BD, Y_BD, "C2", alpha=0.5, label="Start") 96 | X_BL, Y_BL, X_BR, Y_BR, X_FL, Y_FL, X_FR, Y_FR, X_BD, Y_BD = vehicle.draw(x[:, N - 1]) 97 | plt.fill(X_BL, Y_BL, "k") 98 | plt.fill(X_BR, Y_BR, "k") 99 | plt.fill(X_FR, Y_FR, "k") 100 | plt.fill(X_FL, Y_FL, "k") 101 | plt.fill(X_BD, Y_BD, "C3", alpha=0.5, label="End") 102 | plt.xlabel(r"$x$ [m]") 103 | plt.ylabel(r"$y$ [m]") 104 | plt.legend() 105 | 106 | # Save the plot 107 | # plt.savefig("../agv-book/figs/ch3/ackermann_kinematic_fig2.pdf") 108 | 109 | # Show all the plots to the screen 110 | plt.show() 111 | 112 | # %% 113 | # MAKE AN ANIMATION 114 | 115 | # Create the animation 116 | ani = vehicle.animate( 117 | x, 118 | T, 119 | ) 120 | 121 | # Create and save the animation 122 | # ani = vehicle.animate( 123 | # x, 124 | # T, 125 | # True, 126 | # "../agv-book/gifs/ch3/ackermann_kinematic.gif", 127 | # ) 128 | 129 | # Show the movie to the screen 130 | plt.show() 131 | 132 | # # Show animation in HTML output if you are using IPython or Jupyter notebooks 133 | # plt.rc('animation', html='jshtml') 134 | # display(ani) 135 | # plt.close() 136 | -------------------------------------------------------------------------------- /kalman_filter_cart.py: -------------------------------------------------------------------------------- 1 | """ 2 | Example kalman_filter_cart.py 3 | Author: Joshua A. Marshall 4 | GitHub: https://github.com/botprof/agv-examples 5 | """ 6 | 7 | # %% 8 | # SIMULATION SETUP 9 | 10 | import numpy as np 11 | import matplotlib.pyplot as plt 12 | from scipy.stats import chi2 13 | 14 | # Set the simulation time [s] and the sample period [s] 15 | SIM_TIME = 10.0 16 | T = 0.1 17 | 18 | # Create an array of time values [s] 19 | t = np.arange(0.0, SIM_TIME, T) 20 | N = np.size(t) 21 | 22 | # %% 23 | # DEFINE THE ROBOT AND SENSOR MODELS 24 | 25 | # Mobile robot model 26 | M = 1.0 27 | F = np.array([[1, T], [0, 1]]) 28 | G = np.array([0.5 * T**2 / M, T / M]) 29 | 30 | # Sensor model (GPS-like sensor) 31 | H = np.array([1, 0]) 32 | 33 | # Model noise covariance 34 | Q = np.diag([0.1**2, 0.01**2]) 35 | 36 | # GPS sensor noise covariance 37 | R = 2.5**2 38 | 39 | # %% 40 | # SET UP INITIAL CONDITIONS 41 | 42 | # Set up arrays for a two-dimensional state 43 | x_hat = np.zeros((2, N)) 44 | P_hat = np.zeros((2, 2, N)) 45 | 46 | # Robot state (actual) 47 | x = np.zeros([2, N]) 48 | x[:, 0] = np.array([3.0, 0.0]) 49 | 50 | # Set an array for the sensor (GPS) measurements 51 | y = np.zeros(N) 52 | 53 | # Initial estimate of the robot's location 54 | x_hat[:, 0] = x[:, 0] + np.array([2.0, 0.0]) 55 | # Initial covariance matrix 56 | P_hat[:, :, 0] = np.diag([2.0**2, 0.2**2]) 57 | 58 | # Initial inputs 59 | u = np.zeros([N]) 60 | 61 | # %% 62 | # RUN THE SIMULATION 63 | 64 | for k in range(1, N): 65 | # Simulate the actual vehicle 66 | x[:, k] = F @ x[:, k - 1] + G * u[k - 1] 67 | 68 | # Run the a priori estimation step 69 | x_hat[:, k] = F @ x_hat[:, k - 1] + G * (u[k - 1] + np.sqrt(Q) @ np.random.standard_normal(2)) 70 | P_hat[:, :, k] = F @ P_hat[:, :, k - 1] @ F.T + Q 71 | # Help the covariance matrix stay symmetric 72 | P_hat[:, :, k] = 0.5 * (P_hat[:, :, k] + P_hat[:, :, k].T) 73 | 74 | # Take a sensor measurement (with zero-mean Gaussian noise) 75 | y[k] = H @ x[:, k] + np.sqrt(R) * np.random.standard_normal() 76 | 77 | # Run the a posteriori estimation step 78 | K = P_hat[:, :, k] @ H.T / (H @ P_hat[:, :, k] @ H.T + R) 79 | x_hat[:, k] = x_hat[:, k] + K * (y[k] - H @ x_hat[:, k]) 80 | 81 | P_hat[:, :, k] = (np.eye(2) - K @ H) @ P_hat[:, :, k] @ ( 82 | np.eye(2) - K @ H 83 | ).T + K @ K.T * R 84 | # Help the covariance matrix stay symmetric 85 | P_hat[:, :, k] = 0.5 * (P_hat[:, :, k] + P_hat[:, :, k].T) 86 | 87 | # Control input (actual) 88 | u[k] = 10 * np.sin(2 * t[k]) 89 | 90 | # %% 91 | # PLOT THE RESULTS 92 | 93 | # Change some plot settings (optional) 94 | plt.rc("text", usetex=True) 95 | plt.rc("text.latex", preamble=r"\usepackage{cmbright,amsmath,bm}") 96 | plt.rc("savefig", format="pdf") 97 | plt.rc("savefig", bbox="tight") 98 | 99 | # Find the scaling factor for plotting covariance bounds 100 | ALPHA = 0.01 101 | s1 = chi2.isf(ALPHA, 1) 102 | s2 = chi2.isf(ALPHA, 2) 103 | 104 | # Plot the true position and noisy sensor (GPS) measurements 105 | plt.figure() 106 | plt.plot(t, y, "C2", label="Sensor") 107 | plt.plot(t, x[0, :], "C0", label="Actual") 108 | plt.xlabel(r"$t$ [s]") 109 | plt.ylabel(r"$x_1$ [m]") 110 | plt.grid(color="0.95") 111 | plt.legend() 112 | 113 | # Plot the state and the estimate 114 | sigma = np.zeros((2, N)) 115 | plt.figure() 116 | ax1 = plt.subplot(211) 117 | sigma[0, :] = np.sqrt(s1 * P_hat[0, 0, :]) 118 | plt.fill_between( 119 | t, 120 | x_hat[0, :] - sigma[0, :], 121 | x_hat[0, :] + sigma[0, :], 122 | color="C0", 123 | alpha=0.2, 124 | label=str(100 * (1 - ALPHA)) + r" \% Confidence", 125 | ) 126 | plt.plot(t, x[0, :], "C0", label="Actual") 127 | plt.plot(t, x_hat[0, :], "C1", label="Estimate") 128 | plt.plot(t, y, "C2", label="Sensor", alpha=0.2) 129 | plt.ylabel(r"$x_1$ [m]") 130 | plt.setp(ax1, xticklabels=[]) 131 | plt.grid(color="0.95") 132 | plt.legend() 133 | ax2 = plt.subplot(212) 134 | sigma[1, :] = np.sqrt(s1 * P_hat[1, 1, :]) 135 | plt.fill_between( 136 | t, 137 | x_hat[1, :] - sigma[1, :], 138 | x_hat[1, :] + sigma[1, :], 139 | color="C0", 140 | alpha=0.2, 141 | label=str(100 * (1 - ALPHA)) + r" \% Confidence", 142 | ) 143 | plt.plot(t, x[1, :], "C0", label="Actual") 144 | plt.plot(t, x_hat[1, :], "C1", label="Estimate") 145 | plt.ylabel(r"$x_2$ [m]") 146 | plt.grid(color="0.95") 147 | plt.xlabel(r"$t$ [s]") 148 | plt.show() 149 | 150 | # Plot the error in the estimate 151 | sigma = np.zeros((2, N)) 152 | plt.figure() 153 | ax1 = plt.subplot(211) 154 | sigma[0, :] = np.sqrt(s1 * P_hat[0, 0, :]) 155 | plt.fill_between( 156 | t, 157 | -sigma[0, :], 158 | sigma[0, :], 159 | color="C0", 160 | alpha=0.2, 161 | label=str(100 * (1 - ALPHA)) + r" \% Confidence", 162 | ) 163 | plt.plot(t, x[0, :] - x_hat[0, :], "C0", label="Estimator error") 164 | plt.plot(t, x[0, :] - y, "C2", label="GPS-like sensor error", alpha=0.2) 165 | plt.ylabel(r"$e_1$ [m]") 166 | plt.setp(ax1, xticklabels=[]) 167 | plt.grid(color="0.95") 168 | plt.legend() 169 | ax2 = plt.subplot(212) 170 | sigma[1, :] = np.sqrt(s1 * P_hat[1, 1, :]) 171 | plt.fill_between( 172 | t, 173 | -sigma[1, :], 174 | sigma[1, :], 175 | color="C0", 176 | alpha=0.2, 177 | label=str(100 * (1 - ALPHA)) + r" \% Confidence", 178 | ) 179 | plt.plot(t, x[1, :] - x_hat[1, :], "C0", label="Estimator Error") 180 | plt.ylabel(r"$e_2$ [m]") 181 | plt.grid(color="0.95") 182 | plt.xlabel(r"$t$ [s]") 183 | plt.show() 184 | -------------------------------------------------------------------------------- /mobotpy/__init__.py: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/botprof/agv-examples/1eda886ed1e4681dfca121cd1de6fd08843392b7/mobotpy/__init__.py -------------------------------------------------------------------------------- /mobotpy/graphics.py: -------------------------------------------------------------------------------- 1 | """ 2 | Python module graphics.py for drawing basic shapes. 3 | Author: Joshua A. Marshall 4 | GitHub: https://github.com/botprof/agv-examples 5 | """ 6 | 7 | import numpy as np 8 | 9 | """SHAPE FUNCTIONS""" 10 | 11 | 12 | def draw_circle(x, y, radius, increment=10): 13 | """Finds points that approximate a circle as a regular polygon. 14 | 15 | The regular polygon has centre (x, y), a radius, and the function computes 16 | a point every angular increment [deg] (default 10). 17 | """ 18 | angles = np.deg2rad(np.arange(0.0, 360.0, increment)) 19 | X = radius * np.cos(angles) 20 | Y = radius * np.sin(angles) 21 | X += x 22 | Y += y 23 | return X, Y 24 | 25 | 26 | def draw_rectangle(x, y, length, width, angle): 27 | """Finds points that draw a rectangle. 28 | 29 | The rectangle has centre (x, y), a length, width, and angle [rad]. 30 | """ 31 | V = np.zeros((2, 5)) 32 | l = 0.5 * length 33 | w = 0.5 * width 34 | V = np.array([[-l, -l, l, l, -l], [-w, w, w, -w, -w]]) 35 | R = np.array([[np.cos(angle), np.sin(-angle)], [np.sin(angle), np.cos(angle)]]) 36 | V = R @ V 37 | X = V[0, :] + x 38 | Y = V[1, :] + y 39 | return X, Y 40 | -------------------------------------------------------------------------------- /mobotpy/integration.py: -------------------------------------------------------------------------------- 1 | """ 2 | Python module integration.py for numerical integration routines. 3 | Author: Joshua A. Marshall 4 | GitHub: https://github.com/botprof/agv-examples 5 | """ 6 | 7 | 8 | def rk_four(f, x, u, T): 9 | """ 10 | Perform fourth-order Runge-Kutta numerical integration. 11 | 12 | The function to integrate is f(x, u, params), where the state variables are 13 | collected in the variable x, we assume a constant input vector u over time 14 | interval T > 0, and params is an array of the system's parameters. 15 | """ 16 | k_1 = f(x, u) 17 | k_2 = f(x + T * k_1 / 2.0, u) 18 | k_3 = f(x + T * k_2 / 2.0, u) 19 | k_4 = f(x + T * k_3, u) 20 | x_new = x + T / 6.0 * (k_1 + 2.0 * k_2 + 2.0 * k_3 + k_4) 21 | return x_new 22 | 23 | 24 | def euler_int(f, x, u, T): 25 | """ 26 | Perform Euler (trapezoidal) numerical integration. 27 | 28 | The function to integrate is f(x, u, params), where the state variables are 29 | collected in the variable x, we assume a constant input vector u over time 30 | interval T > 0, and params is an array of the system's parameters. 31 | """ 32 | x_new = x + T * f(x, u) 33 | return x_new 34 | -------------------------------------------------------------------------------- /mobotpy/models.py: -------------------------------------------------------------------------------- 1 | """ 2 | Python module models.py for various vehicle models. 3 | Author: Joshua A. Marshall 4 | GitHub: https://github.com/botprof/agv-examples 5 | 6 | This module contains classes for various vehicle models, including: 7 | * 1D vehicle class (i.e., a simple cart) [Cart] 8 | * Differential-drive vehicle class [DiffDrive] 9 | * Tricycle or planar bicycle vehicle class [Tricycle] 10 | * Ackermann steered vehicle class [Ackermann] 11 | * Four-wheel steering vehicle class [FourWheelSteering] 12 | """ 13 | 14 | import numpy as np 15 | from mobotpy import graphics 16 | import matplotlib.pyplot as plt 17 | import matplotlib.animation as animation 18 | from scipy.stats import chi2 19 | from matplotlib import patches 20 | 21 | 22 | class Cart: 23 | """1D vehicle class (i.e., a simple cart). 24 | 25 | Parameters 26 | ---------- 27 | length : float 28 | Length of the cart [m]. 29 | """ 30 | 31 | def __init__(self, length): 32 | """Constructor method.""" 33 | self.d = length 34 | 35 | def draw(self, x): 36 | """Finds the points to draw simple rectangular cart. 37 | 38 | The cart has position x and length d. The resulting cart has a height 39 | that is half the length. 40 | """ 41 | X = np.array(5) 42 | Y = np.array(5) 43 | X = [ 44 | x - self.d / 2, 45 | x - self.d / 2, 46 | x + self.d / 2, 47 | x + self.d / 2, 48 | x - self.d / 2, 49 | ] 50 | Y = [-self.d / 4, self.d / 4, self.d / 4, -self.d / 4, -self.d / 4] 51 | return X, Y 52 | 53 | def animate(self, x, T, save_ani=False, filename="animate_cart.gif"): 54 | """Create an animation of a simple 1D cart. 55 | 56 | Returns animation object for array of 1D cart positions x with time 57 | increments T [s], cart width d [m]. 58 | 59 | To save the animation to a GIF file, set save_ani to True and give a 60 | filename (default filename is 'animate_cart.gif'). 61 | """ 62 | fig, ax = plt.subplots() 63 | plt.plot([np.min(x) - self.d, np.max(x) + self.d], [0, 0], "k--") 64 | plt.xlabel(r"$x$ [m]") 65 | ax.set_xlim([np.min(x) - self.d, np.max(x) + self.d]) 66 | plt.yticks([]) 67 | plt.axis("equal") 68 | (polygon,) = ax.fill([], [], "C0", alpha=0.5) 69 | (line,) = plt.plot([], [], "ko") 70 | time_text = ax.text(0.05, 0.9, "", transform=ax.transAxes) 71 | 72 | # Initialization function 73 | 74 | def init(): 75 | polygon.set_xy(np.empty([5, 2])) 76 | line.set_data([], []) 77 | time_text.set_text("") 78 | return polygon, line, time_text 79 | 80 | # Function to draw cart 81 | 82 | def movie(k): 83 | X, Y = self.draw(x[k]) 84 | a = [X, Y] 85 | polygon.set_xy(np.transpose(a)) 86 | line.set_data([x[k]], [0]) 87 | time_text.set_text(r"$t$ = %.1f s" % (k * T)) 88 | return polygon, line, time_text 89 | 90 | # Create the animation 91 | ani = animation.FuncAnimation( 92 | fig, 93 | movie, 94 | np.arange(1, len(x), max(1, int(1 / T / 10))), 95 | init_func=init, 96 | interval=T * 1000, 97 | blit=True, 98 | repeat=False, 99 | ) 100 | # Save to a file if requested 101 | if save_ani is True: 102 | ani.save(filename, fps=min(1 / T, 10)) 103 | # Return the figure object 104 | return ani 105 | 106 | 107 | class DiffDrive: 108 | """Differential-drive vehicle class. 109 | 110 | Parameters 111 | ---------- 112 | ell : float 113 | The track length of the vehicle [m]. 114 | """ 115 | 116 | def __init__(self, ell): 117 | """Constructor method.""" 118 | self.ell = ell 119 | 120 | def f(self, x, u): 121 | """Differential drive kinematic vehicle kinematic model. 122 | 123 | Parameters 124 | ---------- 125 | x : ndarray of length 3 126 | The vehicle's state (x, y, theta). 127 | u : ndarray of length 2 128 | The left and right wheel speeds (v_L, v_R). 129 | 130 | Returns 131 | ------- 132 | f : ndarray of length 3 133 | The rate of change of the vehicle states. 134 | """ 135 | f = np.zeros(3) 136 | f[0] = 0.5 * (u[0] + u[1]) * np.cos(x[2]) 137 | f[1] = 0.5 * (u[0] + u[1]) * np.sin(x[2]) 138 | f[2] = 1.0 / self.ell * (u[1] - u[0]) 139 | return f 140 | 141 | def uni2diff(self, u_in): 142 | """ 143 | Convert speed and angular rate inputs to differential drive wheel speeds. 144 | 145 | Parameters 146 | ---------- 147 | u_in : ndarray of length 2 148 | The speed and turning rate of the vehicle (v, omega). 149 | 150 | Returns 151 | ------- 152 | u_out : ndarray of length 2 153 | The left and right wheel speeds (v_L, v_R). 154 | """ 155 | v = u_in[0] 156 | omega = u_in[1] 157 | v_L = v - self.ell / 2 * omega 158 | v_R = v + self.ell / 2 * omega 159 | u_out = np.array([v_L, v_R]) 160 | return u_out 161 | 162 | def draw(self, x, y, theta): 163 | """ 164 | Finds points that draw a differential drive vehicle. 165 | 166 | The centre of the wheel axle is (x, y), the vehicle has orientation 167 | theta, and the vehicle's track length is ell. 168 | 169 | Returns X_L, Y_L, X_R, Y_R, X_BD, Y_BD, X_C, Y_C, where L is for the 170 | left wheel, R for the right wheel, B for the body, and C for the caster. 171 | """ 172 | # Left and right wheels 173 | X_L, Y_L = graphics.draw_rectangle( 174 | x - 0.5 * self.ell * np.sin(theta), 175 | y + 0.5 * self.ell * np.cos(theta), 176 | 0.5 * self.ell, 177 | 0.25 * self.ell, 178 | theta, 179 | ) 180 | X_R, Y_R = graphics.draw_rectangle( 181 | x + 0.5 * self.ell * np.sin(theta), 182 | y - 0.5 * self.ell * np.cos(theta), 183 | 0.5 * self.ell, 184 | 0.25 * self.ell, 185 | theta, 186 | ) 187 | # Body 188 | X_BD, Y_BD = graphics.draw_circle(x, y, self.ell) 189 | # Caster 190 | X_C, Y_C = graphics.draw_circle( 191 | x + 0.5 * self.ell * np.cos(theta), 192 | y + 0.5 * self.ell * np.sin(theta), 193 | 0.125 * self.ell, 194 | ) 195 | # Return the arrays of points 196 | return X_L, Y_L, X_R, Y_R, X_BD, Y_BD, X_C, Y_C 197 | 198 | def animate(self, x, T, save_ani=False, filename="animate_diffdrive.gif"): 199 | """Create an animation of a differential drive vehicle. 200 | 201 | Returns animation object for array of vehicle positions x with time 202 | increments T [s], track ell [m]. 203 | 204 | To save the animation to a GIF file, set save_ani to True and provide a 205 | filename (default 'animate_diffdrive.gif'). 206 | """ 207 | fig, ax = plt.subplots() 208 | plt.xlabel(r"$x$ [m]") 209 | plt.ylabel(r"$y$ [m]") 210 | plt.axis("equal") 211 | (line,) = ax.plot([], [], "C0") 212 | (leftwheel,) = ax.fill([], [], color="k") 213 | (rightwheel,) = ax.fill([], [], color="k") 214 | (body,) = ax.fill([], [], color="C0", alpha=0.5) 215 | (castor,) = ax.fill([], [], color="k") 216 | time_text = ax.text(0.05, 0.9, "", transform=ax.transAxes) 217 | 218 | def init(): 219 | """Function that initializes the animation.""" 220 | line.set_data([], []) 221 | leftwheel.set_xy(np.empty([5, 2])) 222 | rightwheel.set_xy(np.empty([5, 2])) 223 | body.set_xy(np.empty([36, 2])) 224 | castor.set_xy(np.empty([36, 2])) 225 | time_text.set_text("") 226 | return line, leftwheel, rightwheel, body, castor, time_text 227 | 228 | def movie(k): 229 | """Function called at each step of the animation.""" 230 | # Draw the path followed by the vehicle 231 | line.set_data(x[0, 0 : k + 1], x[1, 0 : k + 1]) 232 | # Draw the differential drive vehicle 233 | X_L, Y_L, X_R, Y_R, X_B, Y_B, X_C, Y_C = self.draw( 234 | x[0, k], x[1, k], x[2, k] 235 | ) 236 | leftwheel.set_xy(np.transpose([X_L, Y_L])) 237 | rightwheel.set_xy(np.transpose([X_R, Y_R])) 238 | body.set_xy(np.transpose([X_B, Y_B])) 239 | castor.set_xy(np.transpose([X_C, Y_C])) 240 | # Add the simulation time 241 | time_text.set_text(r"$t$ = %.1f s" % (k * T)) 242 | # Dynamically set the axis limits 243 | ax.set_xlim(x[0, k] - 10 * self.ell, x[0, k] + 10 * self.ell) 244 | ax.set_ylim(x[1, k] - 10 * self.ell, x[1, k] + 10 * self.ell) 245 | ax.figure.canvas.draw() 246 | # Return the objects to animate 247 | return line, leftwheel, rightwheel, body, castor, time_text 248 | 249 | # Create the animation 250 | ani = animation.FuncAnimation( 251 | fig, 252 | movie, 253 | np.arange(1, len(x[0, :]), max(1, int(1 / T / 10))), 254 | init_func=init, 255 | interval=T * 1000, 256 | blit=True, 257 | repeat=False, 258 | ) 259 | if save_ani is True: 260 | ani.save(filename, fps=min(1 / T, 10)) 261 | # Return the figure object 262 | return ani 263 | 264 | def animate_trajectory( 265 | self, x, xd, T, save_ani=False, filename="animate_diffdrive.gif" 266 | ): 267 | """Create an animation of a differential drive vehicle with plots of 268 | actual and desired trajectories. 269 | 270 | Returns animation object for array of vehicle positions and desired 271 | positions x with time increments T [s], track ell [m]. 272 | 273 | To save the animation to a GIF file, set save_ani to True and provide a 274 | filename (default 'animate_diffdrive.gif'). 275 | """ 276 | fig, ax = plt.subplots() 277 | plt.xlabel(r"$x$ [m]") 278 | plt.ylabel(r"$y$ [m]") 279 | plt.axis("equal") 280 | (desired,) = ax.plot([], [], "--C1") 281 | (line,) = ax.plot([], [], "C0") 282 | (leftwheel,) = ax.fill([], [], color="k") 283 | (rightwheel,) = ax.fill([], [], color="k") 284 | (body,) = ax.fill([], [], color="C0", alpha=0.5) 285 | (castor,) = ax.fill([], [], color="k") 286 | time_text = ax.text(0.05, 0.9, "", transform=ax.transAxes) 287 | 288 | def init(): 289 | """Function that initializes the animation.""" 290 | desired.set_data([], []) 291 | line.set_data([], []) 292 | leftwheel.set_xy(np.empty([5, 2])) 293 | rightwheel.set_xy(np.empty([5, 2])) 294 | body.set_xy(np.empty([36, 2])) 295 | castor.set_xy(np.empty([36, 2])) 296 | time_text.set_text("") 297 | return desired, line, leftwheel, rightwheel, body, castor, time_text 298 | 299 | def movie(k): 300 | """Function called at each step of the animation.""" 301 | # Draw the desired trajectory 302 | desired.set_data(xd[0, 0 : k + 1], xd[1, 0 : k + 1]) 303 | # Draw the path followed by the vehicle 304 | line.set_data(x[0, 0 : k + 1], x[1, 0 : k + 1]) 305 | # Draw the differential drive vehicle 306 | X_L, Y_L, X_R, Y_R, X_B, Y_B, X_C, Y_C = self.draw( 307 | x[0, k], x[1, k], x[2, k] 308 | ) 309 | leftwheel.set_xy(np.transpose([X_L, Y_L])) 310 | rightwheel.set_xy(np.transpose([X_R, Y_R])) 311 | body.set_xy(np.transpose([X_B, Y_B])) 312 | castor.set_xy(np.transpose([X_C, Y_C])) 313 | # Add the simulation time 314 | time_text.set_text(r"$t$ = %.1f s" % (k * T)) 315 | # Dynamically set the axis limits 316 | ax.set_xlim(x[0, k] - 10 * self.ell, x[0, k] + 10 * self.ell) 317 | ax.set_ylim(x[1, k] - 10 * self.ell, x[1, k] + 10 * self.ell) 318 | ax.figure.canvas.draw() 319 | # Return the objects to animate 320 | return desired, line, leftwheel, rightwheel, body, castor, time_text 321 | 322 | # Create the animation 323 | ani = animation.FuncAnimation( 324 | fig, 325 | movie, 326 | np.arange(1, len(x[0, :]), max(1, int(1 / T / 10))), 327 | init_func=init, 328 | interval=T * 1000, 329 | blit=True, 330 | repeat=False, 331 | ) 332 | if save_ani is True: 333 | ani.save(filename, fps=min(1 / T, 10)) 334 | # Return the figure object 335 | return ani 336 | 337 | def animate_estimation( 338 | self, 339 | x, 340 | x_hat, 341 | P_hat, 342 | alpha, 343 | T, 344 | save_ani=False, 345 | filename="animate_diffdrive.gif", 346 | ): 347 | """Create an animation of a differential drive vehicle with plots of 348 | estimation uncertainty. 349 | 350 | Returns animation object for array of vehicle positions x with time 351 | increments T [s], track ell [m]. 352 | 353 | To save the animation to a GIF file, set save_ani to True and provide a 354 | filename (default 'animate_diffdrive.gif'). 355 | """ 356 | fig, ax = plt.subplots() 357 | plt.xlabel(r"$x$ [m]") 358 | plt.ylabel(r"$y$ [m]") 359 | plt.axis("equal") 360 | (estimated,) = ax.plot([], [], "--C1") 361 | (line,) = ax.plot([], [], "C0") 362 | (leftwheel,) = ax.fill([], [], color="k") 363 | (rightwheel,) = ax.fill([], [], color="k") 364 | (body,) = ax.fill([], [], color="C0", alpha=0.5) 365 | (castor,) = ax.fill([], [], color="k") 366 | time_text = ax.text(0.05, 0.9, "", transform=ax.transAxes) 367 | s2 = chi2.isf(alpha, 2) 368 | 369 | def init(): 370 | """Function that initializes the animation.""" 371 | estimated.set_data([], []) 372 | line.set_data([], []) 373 | leftwheel.set_xy(np.empty([5, 2])) 374 | rightwheel.set_xy(np.empty([5, 2])) 375 | body.set_xy(np.empty([36, 2])) 376 | castor.set_xy(np.empty([36, 2])) 377 | time_text.set_text("") 378 | return estimated, line, leftwheel, rightwheel, body, castor, time_text 379 | 380 | def movie(k): 381 | """Function called at each step of the animation.""" 382 | # Draw the desired trajectory 383 | estimated.set_data(x_hat[0, 0 : k + 1], x_hat[1, 0 : k + 1]) 384 | # Draw the path followed by the vehicle 385 | line.set_data(x[0, 0 : k + 1], x[1, 0 : k + 1]) 386 | # Draw the differential drive vehicle 387 | X_L, Y_L, X_R, Y_R, X_B, Y_B, X_C, Y_C = self.draw( 388 | x[0, k], x[1, k], x[2, k] 389 | ) 390 | leftwheel.set_xy(np.transpose([X_L, Y_L])) 391 | rightwheel.set_xy(np.transpose([X_R, Y_R])) 392 | body.set_xy(np.transpose([X_B, Y_B])) 393 | castor.set_xy(np.transpose([X_C, Y_C])) 394 | # Compute eigenvalues and eigenvectors to find axes for covariance ellipse 395 | W, V = np.linalg.eig(P_hat[0:2, 0:2, k]) 396 | # Find the index of the largest and smallest eigenvalues 397 | j_max = np.argmax(W) 398 | j_min = np.argmin(W) 399 | ell = patches.Ellipse( 400 | (x_hat[0, k], x_hat[1, k]), 401 | 2 * np.sqrt(s2 * W[j_max]), 402 | 2 * np.sqrt(s2 * W[j_min]), 403 | angle=np.arctan2(V[j_max, 1], V[j_max, 0]) * 180 / np.pi, 404 | alpha=0.2, 405 | color="C1", 406 | ) 407 | ax.add_artist(ell) 408 | # Add the simulation time 409 | time_text.set_text(r"$t$ = %.1f s" % (k * T)) 410 | # Dynamically set the axis limits 411 | ax.set_xlim(x[0, k] - 10 * self.ell, x[0, k] + 10 * self.ell) 412 | ax.set_ylim(x[1, k] - 10 * self.ell, x[1, k] + 10 * self.ell) 413 | ax.figure.canvas.draw() 414 | # Return the objects to animate 415 | return estimated, line, leftwheel, rightwheel, body, castor, time_text 416 | 417 | # Create the animation 418 | ani = animation.FuncAnimation( 419 | fig, 420 | movie, 421 | np.arange(1, len(x[0, :]), max(1, int(1 / T / 10))), 422 | init_func=init, 423 | interval=T * 1000, 424 | blit=True, 425 | repeat=False, 426 | ) 427 | if save_ani is True: 428 | ani.save(filename, fps=min(1 / T, 10)) 429 | # Return the figure object 430 | return ani 431 | 432 | 433 | class Tricycle: 434 | """Tricycle or planar bicycle vehicle class. 435 | 436 | Parameters 437 | ---------- 438 | ell_W : float 439 | The wheelbase of the vehicle [m]. 440 | ell_T : float 441 | The vehicle's track length [m]. 442 | """ 443 | 444 | def __init__(self, ell_W, ell_T): 445 | """Constructor method.""" 446 | self.ell_W = ell_W 447 | self.ell_T = ell_T 448 | 449 | def f(self, x, u): 450 | """Tricycle or planar bicycle vehicle kinematic model. 451 | 452 | Parameters 453 | ---------- 454 | x : ndarray of length 4 455 | The vehicle's state (x, y, theta, phi). 456 | u : ndarray of length 2 457 | 458 | Returns 459 | ------- 460 | f : ndarray of length 4 461 | The rate of change of the vehicle states. 462 | """ 463 | f = np.zeros(4) 464 | f[0] = u[0] * np.cos(x[2]) 465 | f[1] = u[0] * np.sin(x[2]) 466 | f[2] = u[0] * 1.0 / self.ell_W * np.tan(x[3]) 467 | f[3] = u[1] 468 | return f 469 | 470 | def draw(self, x, y, theta, phi): 471 | """Finds points that draw a tricycle vehicle. 472 | 473 | The centre of the rear wheel axle is (x, y), the body has orientation 474 | theta, steering angle phi, wheelbase ell_W and track length ell_T. 475 | 476 | Returns X_L, Y_L, X_R, Y_R, X_F, Y_F, X_B, Y_B, where L is for the left 477 | wheel, R is for the right wheel, F is for the single front wheel, and 478 | BD is for the vehicle's body. 479 | """ 480 | # Left and right back wheels 481 | X_L, Y_L = graphics.draw_rectangle( 482 | x - 0.5 * self.ell_T * np.sin(theta), 483 | y + 0.5 * self.ell_T * np.cos(theta), 484 | 0.5 * self.ell_T, 485 | 0.25 * self.ell_T, 486 | theta, 487 | ) 488 | X_R, Y_R = graphics.draw_rectangle( 489 | x + 0.5 * self.ell_T * np.sin(theta), 490 | y - 0.5 * self.ell_T * np.cos(theta), 491 | 0.5 * self.ell_T, 492 | 0.25 * self.ell_T, 493 | theta, 494 | ) 495 | # Front wheel 496 | X_F, Y_F = graphics.draw_rectangle( 497 | x + self.ell_W * np.cos(theta), 498 | y + self.ell_W * np.sin(theta), 499 | 0.5 * self.ell_T, 500 | 0.25 * self.ell_T, 501 | theta + phi, 502 | ) 503 | # Body 504 | X_BD, Y_BD = graphics.draw_rectangle( 505 | x + self.ell_W / 2.0 * np.cos(theta), 506 | y + self.ell_W / 2.0 * np.sin(theta), 507 | 2.0 * self.ell_W, 508 | 2.0 * self.ell_T, 509 | theta, 510 | ) 511 | # Return the arrays of points 512 | return X_L, Y_L, X_R, Y_R, X_F, Y_F, X_BD, Y_BD 513 | 514 | def animate( 515 | self, 516 | x, 517 | T, 518 | save_ani=False, 519 | filename="animate_tricycle.gif", 520 | ): 521 | """Create an animation of a tricycle vehicle. 522 | 523 | Returns animation object for array of vehicle positions x with time 524 | increments T [s], wheelbase ell_W [m], and track ell_T [m]. 525 | 526 | To save the animation to a GIF file, set save_ani to True and give a 527 | filename (default 'animate_tricycle.gif'). 528 | """ 529 | fig, ax = plt.subplots() 530 | plt.xlabel(r"$x$ [m]") 531 | plt.ylabel(r"$y$ [m]") 532 | plt.axis("equal") 533 | (line,) = ax.plot([], [], "C0") 534 | (leftwheel,) = ax.fill([], [], color="k") 535 | (rightwheel,) = ax.fill([], [], color="k") 536 | (frontwheel,) = ax.fill([], [], color="k") 537 | (body,) = ax.fill([], [], color="C0", alpha=0.5) 538 | time_text = ax.text(0.05, 0.9, "", transform=ax.transAxes) 539 | 540 | def init(): 541 | """A function that initializes the animation.""" 542 | line.set_data([], []) 543 | leftwheel.set_xy(np.empty([5, 2])) 544 | rightwheel.set_xy(np.empty([5, 2])) 545 | frontwheel.set_xy(np.empty([5, 2])) 546 | body.set_xy(np.empty([5, 2])) 547 | time_text.set_text("") 548 | return line, leftwheel, rightwheel, frontwheel, body, time_text 549 | 550 | def movie(k): 551 | """The function called at each step of the animation.""" 552 | # Draw the path followed by the vehicle 553 | line.set_data(x[0, 0 : k + 1], x[1, 0 : k + 1]) 554 | # Draw the tricycle vehicle 555 | X_L, Y_L, X_R, Y_R, X_F, Y_F, X_B, Y_B = self.draw( 556 | x[0, k], x[1, k], x[2, k], x[3, k] 557 | ) 558 | leftwheel.set_xy(np.transpose([X_L, Y_L])) 559 | rightwheel.set_xy(np.transpose([X_R, Y_R])) 560 | frontwheel.set_xy(np.transpose([X_F, Y_F])) 561 | body.set_xy(np.transpose([X_B, Y_B])) 562 | # Add the simulation time 563 | time_text.set_text(r"$t$ = %.1f s" % (k * T)) 564 | # Dynamically set the axis limits 565 | ax.set_xlim(x[0, k] - 10 * self.ell_W, x[0, k] + 10 * self.ell_W) 566 | ax.set_ylim(x[1, k] - 10 * self.ell_W, x[1, k] + 10 * self.ell_W) 567 | ax.figure.canvas.draw() 568 | # Return the objects to animate 569 | return line, leftwheel, rightwheel, frontwheel, body, time_text 570 | 571 | # Create the animation 572 | ani = animation.FuncAnimation( 573 | fig, 574 | movie, 575 | np.arange(1, len(x[0, :]), max(1, int(1 / T / 10))), 576 | init_func=init, 577 | interval=T * 1000, 578 | blit=True, 579 | repeat=False, 580 | ) 581 | if save_ani is True: 582 | ani.save(filename, fps=min(1 / T, 10)) 583 | # Return the figure object 584 | return ani 585 | 586 | 587 | class Ackermann: 588 | """Ackermann steered vehicle class. 589 | 590 | Parameters 591 | ---------- 592 | ell_W : float 593 | The wheelbase of the vehicle [m]. 594 | ell_T : float 595 | The vehicle's track length [m]. 596 | """ 597 | 598 | def __init__(self, ell_W, ell_T): 599 | """Constructor method.""" 600 | self.ell_W = ell_W 601 | self.ell_T = ell_T 602 | 603 | def f(self, x, u): 604 | """Ackermann steered vehicle kinematic model. 605 | 606 | Parameters 607 | ---------- 608 | x : ndarray of length 4 609 | The vehicle's state (x, y, theta, phi). 610 | u : ndarray of length 2 611 | The vehicle's speed and steering angle rate. 612 | 613 | Returns 614 | ------- 615 | f : ndarray of length 4 616 | The rate of change of the vehicle states. 617 | """ 618 | f = np.zeros(4) 619 | f[0] = u[0] * np.cos(x[2]) 620 | f[1] = u[0] * np.sin(x[2]) 621 | f[2] = u[0] * 1.0 / self.ell_W * np.tan(x[3]) 622 | f[3] = u[1] 623 | return f 624 | 625 | def ackermann(self, x): 626 | """Computes the Ackermann steering angles. 627 | 628 | Parameters 629 | ---------- 630 | x : ndarray of length 4 631 | The vehicle's state (x, y, theta, phi). 632 | 633 | Returns 634 | ------- 635 | ackermann_angles : ndarray of length 2 636 | The left and right wheel angles (phi_L, phi_R). 637 | """ 638 | phi_L = np.arctan( 639 | 2 * self.ell_W * np.tan(x[3]) / (2 * self.ell_W - self.ell_T * np.tan(x[3])) 640 | ) 641 | phi_R = np.arctan( 642 | 2 * self.ell_W * np.tan(x[3]) / (2 * self.ell_W + self.ell_T * np.tan(x[3])) 643 | ) 644 | ackermann_angles = np.array([phi_L, phi_R]) 645 | return ackermann_angles 646 | 647 | def draw(self, x, y, theta, phi_L, phi_R): 648 | """Finds points that draw an Ackermann steered (car-like) vehicle. 649 | 650 | The centre of the rear wheel axle is (x, y), the body has orientation 651 | theta, effective steering angle phi, wheelbase ell_W and track length 652 | ell_T. 653 | 654 | Returns X_BL, Y_BL, X_BR, Y_BR, X_FL, Y_FL, X_FR, Y_FR, X_BD, Y_BD, 655 | where L denotes left, R denotes right, B denotes back, F denotes front, 656 | and BD denotes the vehicle's body. 657 | """ 658 | # Left and right back wheels 659 | X_BL, Y_BL = graphics.draw_rectangle( 660 | x - 0.5 * self.ell_T * np.sin(theta), 661 | y + 0.5 * self.ell_T * np.cos(theta), 662 | 0.5 * self.ell_T, 663 | 0.25 * self.ell_T, 664 | theta, 665 | ) 666 | X_BR, Y_BR = graphics.draw_rectangle( 667 | x + 0.5 * self.ell_T * np.sin(theta), 668 | y - 0.5 * self.ell_T * np.cos(theta), 669 | 0.5 * self.ell_T, 670 | 0.25 * self.ell_T, 671 | theta, 672 | ) 673 | # Left and right front wheels 674 | X_FL, Y_FL = graphics.draw_rectangle( 675 | x + self.ell_W * np.cos(theta) - self.ell_T / 2 * np.sin(theta), 676 | y + self.ell_W * np.sin(theta) + self.ell_T / 2 * np.cos(theta), 677 | 0.5 * self.ell_T, 678 | 0.25 * self.ell_T, 679 | theta + phi_L, 680 | ) 681 | X_FR, Y_FR = graphics.draw_rectangle( 682 | x + self.ell_W * np.cos(theta) + self.ell_T / 2 * np.sin(theta), 683 | y + self.ell_W * np.sin(theta) - self.ell_T / 2 * np.cos(theta), 684 | 0.5 * self.ell_T, 685 | 0.25 * self.ell_T, 686 | theta + phi_R, 687 | ) 688 | # Body 689 | X_BD, Y_BD = graphics.draw_rectangle( 690 | x + self.ell_W / 2.0 * np.cos(theta), 691 | y + self.ell_W / 2.0 * np.sin(theta), 692 | 2.0 * self.ell_W, 693 | 2.0 * self.ell_T, 694 | theta, 695 | ) 696 | # Return the arrays of points 697 | return X_BL, Y_BL, X_BR, Y_BR, X_FL, Y_FL, X_FR, Y_FR, X_BD, Y_BD 698 | 699 | def animate( 700 | self, 701 | x, 702 | T, 703 | phi_L, 704 | phi_R, 705 | save_ani=False, 706 | filename="animate_ackermann.gif", 707 | ): 708 | """Create an animation of an Ackermann steered (car-like) vehicle. 709 | 710 | Returns animation object for array of vehicle positions x with time 711 | increments T [s], wheelbase ell_W [m], and track ell_T [m]. 712 | 713 | To save the animation to a GIF file, set save_ani to True and give a 714 | filename (default 'animate_ackermann.gif'). 715 | """ 716 | fig, ax = plt.subplots() 717 | plt.xlabel(r"$x$ [m]") 718 | plt.ylabel(r"$y$ [m]") 719 | plt.axis("equal") 720 | (line,) = ax.plot([], [], "C0") 721 | (BLwheel,) = ax.fill([], [], color="k") 722 | (BRwheel,) = ax.fill([], [], color="k") 723 | (FLwheel,) = ax.fill([], [], color="k") 724 | (FRwheel,) = ax.fill([], [], color="k") 725 | (body,) = ax.fill([], [], color="C0", alpha=0.5) 726 | time_text = ax.text(0.05, 0.9, "", transform=ax.transAxes) 727 | 728 | def init(): 729 | """A function that initializes the animation.""" 730 | line.set_data([], []) 731 | BLwheel.set_xy(np.empty([5, 2])) 732 | BRwheel.set_xy(np.empty([5, 2])) 733 | FLwheel.set_xy(np.empty([5, 2])) 734 | FRwheel.set_xy(np.empty([5, 2])) 735 | body.set_xy(np.empty([5, 2])) 736 | time_text.set_text("") 737 | return line, BLwheel, BRwheel, FLwheel, FRwheel, body, time_text 738 | 739 | def movie(k): 740 | """The function called at each step of the animation.""" 741 | # Draw the path followed by the vehicle 742 | line.set_data(x[0, 0 : k + 1], x[1, 0 : k + 1]) 743 | # Draw the Ackermann steered drive vehicle 744 | X_BL, Y_BL, X_BR, Y_BR, X_FL, Y_FL, X_FR, Y_FR, X_BD, Y_BD = self.draw( 745 | x[0, k], x[1, k], x[2, k], phi_L[k], phi_R[k] 746 | ) 747 | BLwheel.set_xy(np.transpose([X_BL, Y_BL])) 748 | BRwheel.set_xy(np.transpose([X_BR, Y_BR])) 749 | FLwheel.set_xy(np.transpose([X_FL, Y_FL])) 750 | FRwheel.set_xy(np.transpose([X_FR, Y_FR])) 751 | body.set_xy(np.transpose([X_BD, Y_BD])) 752 | # Add the simulation time 753 | time_text.set_text(r"$t$ = %.1f s" % (k * T)) 754 | # Dynamically set the axis limits 755 | ax.set_xlim(x[0, k] - 10 * self.ell_W, x[0, k] + 10 * self.ell_W) 756 | ax.set_ylim(x[1, k] - 10 * self.ell_W, x[1, k] + 10 * self.ell_W) 757 | ax.figure.canvas.draw() 758 | # Return the objects to animate 759 | return line, BLwheel, BRwheel, FLwheel, FRwheel, body, time_text 760 | 761 | # Create the animation 762 | ani = animation.FuncAnimation( 763 | fig, 764 | movie, 765 | np.arange(1, len(x[0, :]), max(1, int(1 / T / 10))), 766 | init_func=init, 767 | interval=T * 1000, 768 | blit=True, 769 | repeat=False, 770 | ) 771 | if save_ani is True: 772 | ani.save(filename, fps=min(1 / T, 10)) 773 | # Return the figure object 774 | return ani 775 | 776 | 777 | class FourWheelSteered: 778 | """Four-wheel steered vehicle class. 779 | 780 | Parameters 781 | ---------- 782 | ell_W : float 783 | The wheelbase of the vehicle [m]. 784 | ell_T : float 785 | The vehicle's track length [m]. 786 | """ 787 | 788 | def __init__(self, ell_W, ell_T): 789 | """Constructor method.""" 790 | self.ell_W = ell_W 791 | self.ell_T = ell_T 792 | 793 | def f(self, x, u): 794 | """Four-wheel steered vehicle kinematic model. 795 | 796 | Parameters 797 | ---------- 798 | x : ndarray of length 4 799 | The vehicle's state (x, y, theta, phi). 800 | u : ndarray of length 2 801 | The vehicle's front wheel speed and steering angle rate. 802 | 803 | Returns 804 | ------- 805 | f : ndarray of length 4 806 | The rate of change of the vehicle states. 807 | """ 808 | f = np.zeros(4) 809 | f[0] = u[0] * np.cos(x[2]) * np.cos(x[3]) 810 | f[1] = u[0] * np.sin(x[2]) * np.cos(x[3]) 811 | f[2] = u[0] * 1.0 / (0.5 * self.ell_W) * np.sin(x[3]) 812 | f[3] = u[1] 813 | return f 814 | 815 | def ackermann(self, x): 816 | """Computes the Ackermann steering angles. 817 | 818 | Parameters 819 | ---------- 820 | x : ndarray of length 4 821 | The vehicle's state (x, y, theta, phi). 822 | 823 | Returns 824 | ------- 825 | ackermann_angles : ndarray of length 2 826 | The left and right wheel angles (phi_L, phi_R). 827 | """ 828 | phi_L = np.arctan( 829 | 2 * self.ell_W * np.tan(x[3]) / (2 * self.ell_W - self.ell_T * np.tan(x[3])) 830 | ) 831 | phi_R = np.arctan( 832 | 2 * self.ell_W * np.tan(x[3]) / (2 * self.ell_W + self.ell_T * np.tan(x[3])) 833 | ) 834 | ackermann_angles = np.array([phi_L, phi_R]) 835 | return ackermann_angles 836 | 837 | def draw(self, x): 838 | """Finds points that draw a four-wheel steered vehicle. 839 | 840 | Parameters 841 | ---------- 842 | x : ndarray of length 4 843 | The vehicle's state (x, y, theta, phi). 844 | 845 | The geometric centre of the vehicle is (x, y), the body has orientation 846 | theta, effective steering angle phi, wheelbase ell_W and track length 847 | ell_T. 848 | 849 | Returns X_BL, Y_BL, X_BR, Y_BR, X_FL, Y_FL, X_FR, Y_FR, X_BD, Y_BD, 850 | where L denotes left, R denotes right, B denotes back, F denotes front, 851 | and BD denotes the vehicle's body. 852 | """ 853 | # Left and right back wheels 854 | X_BL, Y_BL = graphics.draw_rectangle( 855 | x[0] - 0.5 * self.ell_W * np.cos(x[2]) - 0.5 * self.ell_T * np.sin(x[2]), 856 | x[1] - 0.5 * self.ell_W * np.sin(x[2]) + 0.5 * self.ell_T * np.cos(x[2]), 857 | 0.5 * self.ell_T, 858 | 0.25 * self.ell_T, 859 | x[2] - self.ackermann(x)[0], 860 | ) 861 | X_BR, Y_BR = graphics.draw_rectangle( 862 | x[0] - 0.5 * self.ell_W * np.cos(x[2]) + 0.5 * self.ell_T * np.sin(x[2]), 863 | x[1] - 0.5 * self.ell_W * np.sin(x[2]) - 0.5 * self.ell_T * np.cos(x[2]), 864 | 0.5 * self.ell_T, 865 | 0.25 * self.ell_T, 866 | x[2] - self.ackermann(x)[1], 867 | ) 868 | # Left and right front wheels 869 | X_FL, Y_FL = graphics.draw_rectangle( 870 | x[0] + 0.5 * self.ell_W * np.cos(x[2]) - self.ell_T / 2 * np.sin(x[2]), 871 | x[1] + 0.5 * self.ell_W * np.sin(x[2]) + self.ell_T / 2 * np.cos(x[2]), 872 | 0.5 * self.ell_T, 873 | 0.25 * self.ell_T, 874 | x[2] + self.ackermann(x)[0], 875 | ) 876 | X_FR, Y_FR = graphics.draw_rectangle( 877 | x[0] + 0.5 * self.ell_W * np.cos(x[2]) + self.ell_T / 2 * np.sin(x[2]), 878 | x[1] + 0.5 * self.ell_W * np.sin(x[2]) - self.ell_T / 2 * np.cos(x[2]), 879 | 0.5 * self.ell_T, 880 | 0.25 * self.ell_T, 881 | x[2] + self.ackermann(x)[1], 882 | ) 883 | # Body 884 | X_BD, Y_BD = graphics.draw_rectangle( 885 | x[0], 886 | x[1], 887 | 2.0 * self.ell_W, 888 | 2.0 * self.ell_T, 889 | x[2], 890 | ) 891 | # Return the arrays of points 892 | return X_BL, Y_BL, X_BR, Y_BR, X_FL, Y_FL, X_FR, Y_FR, X_BD, Y_BD 893 | 894 | def animate( 895 | self, 896 | x, 897 | T, 898 | save_ani=False, 899 | filename="animate_fourwheelsteered.gif", 900 | ): 901 | """Create an animation of an Ackermann steered (car-like) vehicle. 902 | 903 | Returns animation object for array of vehicle positions x with time 904 | increments T [s], wheelbase ell_W [m], and track ell_T [m]. 905 | 906 | To save the animation to a GIF file, set save_ani to True and give a 907 | filename (default 'animate_ackermann.gif'). 908 | """ 909 | fig, ax = plt.subplots() 910 | plt.xlabel(r"$x$ [m]") 911 | plt.ylabel(r"$y$ [m]") 912 | plt.axis("equal") 913 | (line,) = ax.plot([], [], "C0") 914 | (BLwheel,) = ax.fill([], [], color="k") 915 | (BRwheel,) = ax.fill([], [], color="k") 916 | (FLwheel,) = ax.fill([], [], color="k") 917 | (FRwheel,) = ax.fill([], [], color="k") 918 | (body,) = ax.fill([], [], color="C0", alpha=0.5) 919 | time_text = ax.text(0.05, 0.9, "", transform=ax.transAxes) 920 | 921 | def init(): 922 | """A function that initializes the animation.""" 923 | line.set_data([], []) 924 | BLwheel.set_xy(np.empty([5, 2])) 925 | BRwheel.set_xy(np.empty([5, 2])) 926 | FLwheel.set_xy(np.empty([5, 2])) 927 | FRwheel.set_xy(np.empty([5, 2])) 928 | body.set_xy(np.empty([5, 2])) 929 | time_text.set_text("") 930 | return line, BLwheel, BRwheel, FLwheel, FRwheel, body, time_text 931 | 932 | def movie(k): 933 | """The function called at each step of the animation.""" 934 | # Draw the path followed by the vehicle 935 | line.set_data(x[0, 0 : k + 1], x[1, 0 : k + 1]) 936 | # Draw the Ackermann steered drive vehicle 937 | X_BL, Y_BL, X_BR, Y_BR, X_FL, Y_FL, X_FR, Y_FR, X_BD, Y_BD = self.draw( 938 | x[:, k] 939 | ) 940 | BLwheel.set_xy(np.transpose([X_BL, Y_BL])) 941 | BRwheel.set_xy(np.transpose([X_BR, Y_BR])) 942 | FLwheel.set_xy(np.transpose([X_FL, Y_FL])) 943 | FRwheel.set_xy(np.transpose([X_FR, Y_FR])) 944 | body.set_xy(np.transpose([X_BD, Y_BD])) 945 | # Add the simulation time 946 | time_text.set_text(r"$t$ = %.1f s" % (k * T)) 947 | # Dynamically set the axis limits 948 | ax.set_xlim(x[0, k] - 10 * self.ell_W, x[0, k] + 10 * self.ell_W) 949 | ax.set_ylim(x[1, k] - 10 * self.ell_W, x[1, k] + 10 * self.ell_W) 950 | ax.figure.canvas.draw() 951 | # Return the objects to animate 952 | return line, BLwheel, BRwheel, FLwheel, FRwheel, body, time_text 953 | 954 | # Create the animation 955 | ani = animation.FuncAnimation( 956 | fig, 957 | movie, 958 | np.arange(1, len(x[0, :]), max(1, int(1 / T / 10))), 959 | init_func=init, 960 | interval=T * 1000, 961 | blit=True, 962 | repeat=False, 963 | ) 964 | if save_ani is True: 965 | ani.save(filename, fps=min(1 / T, 10)) 966 | # Return the figure object 967 | return ani 968 | -------------------------------------------------------------------------------- /oneD_combined_control.py: -------------------------------------------------------------------------------- 1 | """ 2 | Example oneD_combined_control.py 3 | Author: Joshua A. Marshall 4 | GitHub: https://github.com/botprof/agv-examples 5 | """ 6 | 7 | # %% 8 | # SIMULATION SETUP 9 | 10 | from scipy import signal 11 | import numpy as np 12 | import matplotlib.pyplot as plt 13 | from mobotpy.models import Cart 14 | 15 | # Set some variables that describe the desired behaviour 16 | ZETA = 1.1 17 | OMEGA_N = np.sqrt(0.3) 18 | 19 | # Define the sample time [s] 20 | T = 0.5 21 | 22 | # Define the vehicle mass [kg] 23 | M = 10.0 24 | 25 | # Compute the controller pole locations 26 | lambda_sc = np.roots([1, 2 * ZETA * OMEGA_N, OMEGA_N**2]) 27 | lambda_zc = np.exp(lambda_sc * T) 28 | 29 | # %% 30 | # PARAMETERS 31 | 32 | # Function that models the vehicle and sensor(s) in discrete time 33 | F = np.array([[1, T], [0, 1]]) 34 | G = np.array([[T**2 / (2 * M)], [T / M]]) 35 | H = np.array([[1, 0]]) 36 | 37 | # Find gain matrix K that places the poles at lambda_zc 38 | K = signal.place_poles(F, G, lambda_zc) 39 | 40 | # Choose estimator gains for stability 41 | lambda_so = 2.0 * lambda_sc 42 | lambda_zo = np.exp(lambda_so * T) 43 | LT = signal.place_poles(F.T, H.T, lambda_zo) 44 | 45 | # %% 46 | # FUNCTION DEFINITIONS 47 | 48 | 49 | def vehicle(x, u, F, G): 50 | """Discrete-time 1D dynamic vehicle model.""" 51 | x_new = F @ x + G @ [u] 52 | return x_new 53 | 54 | 55 | def controller(x, K): 56 | """Full state feedback controller.""" 57 | u = -K @ x 58 | return u 59 | 60 | 61 | def observer(x_hat, u, y, F, G, H, L): 62 | """Observer-based state estimator.""" 63 | x_hat_new = F @ x_hat + G @ [u] + L @ ([y] - H @ x_hat) 64 | return x_hat_new 65 | 66 | 67 | # %% 68 | # RUN SIMULATION 69 | 70 | # Create an array of time values [s] 71 | SIM_TIME = 20.0 72 | t = np.arange(0, SIM_TIME, T) 73 | N = np.size(t) 74 | 75 | # Initialize arrays that will be populated with our inputs and states 76 | x = np.zeros((2, N)) 77 | u = np.zeros(N) 78 | x_hat = np.zeros((2, N)) 79 | 80 | # Set the initial position [m], velocity [m/s], and force input [N] 81 | x[0, 0] = 1.0 82 | x[1, 0] = 0.0 83 | u[0] = 0.0 84 | 85 | # Set the initial estimated position (different from the actual position) 86 | x_hat[0, 0] = 0.0 87 | x_hat[0, 0] = 0.0 88 | 89 | # Run the simulation 90 | for k in range(1, N): 91 | y = x[0, k - 1] 92 | x_hat[:, k] = observer(x_hat[:, k - 1], u[k - 1], y, F, G, H, LT.gain_matrix.T) 93 | x[:, k] = vehicle(x[:, k - 1], u[k - 1], F, G) 94 | u[k] = controller(x_hat[:, k], K.gain_matrix)[0] 95 | # %% 96 | # MAKE A PLOT 97 | 98 | # Change some plot settings (optional) 99 | plt.rc("text", usetex=True) 100 | plt.rc("text.latex", preamble=r"\usepackage{cmbright,amsmath}") 101 | plt.rc("savefig", format="pdf") 102 | plt.rc("savefig", bbox="tight") 103 | 104 | # Plot the states (x) and input (u) vs time (t) 105 | fig1 = plt.figure(1) 106 | ax1a = plt.subplot(311) 107 | plt.plot(t, x[0, :], "C0", label="Actual") 108 | plt.step(t, x_hat[0, :], "C1--", where="post", label="Estimated") 109 | plt.grid(color="0.95") 110 | plt.ylabel(r"$x_1$ [m]") 111 | plt.setp(ax1a, xticklabels=[]) 112 | plt.legend() 113 | ax1b = plt.subplot(312) 114 | plt.plot(t, x[1, :], "C0") 115 | plt.step(t, x_hat[1, :], "C1--", where="post") 116 | plt.grid(color="0.95") 117 | plt.ylabel(r"$x_2$ [m/s]") 118 | plt.setp(ax1b, xticklabels=[]) 119 | ax1c = plt.subplot(313) 120 | plt.step(t, u, "C1", where="post") 121 | plt.ylabel(r"$u$ [N]") 122 | plt.xlabel(r"$t$ [s]") 123 | plt.grid(color="0.95") 124 | 125 | # Show all the plots to the screen 126 | # plt.show() 127 | 128 | # Save the plot 129 | # plt.savefig("../agv-book/figs/ch2/oneD_combined_control_fig1.pdf") 130 | 131 | # %% 132 | # MAKE AN ANIMATION 133 | 134 | # Set the side length of the vehicle [m] 135 | LENGTH = 1.0 136 | 137 | # Let's use the Cart class to create an animation 138 | vehicle = Cart(LENGTH) 139 | 140 | # Create and save the animation 141 | ani = vehicle.animate(x[0, :], T) 142 | 143 | # Create and save the animation 144 | # ani = vehicle.animate( 145 | # x[0, :], T, True, "../agv-book/gifs/ch2/oneD_combined_control.gif" 146 | # ) 147 | 148 | # Show all the plots to the screen 149 | plt.show() 150 | 151 | # Show animation in HTML output if you are using IPython or Jupyter notebooks 152 | # from IPython.display import display 153 | 154 | # plt.rc("animation", html="jshtml") 155 | # display(ani) 156 | # plt.close() 157 | -------------------------------------------------------------------------------- /oneD_discrete_control.py: -------------------------------------------------------------------------------- 1 | """ 2 | Example oneD_discrete_control.py 3 | Author: Joshua A. Marshall 4 | GitHub: https://github.com/botprof/agv-examples 5 | """ 6 | 7 | # %% 8 | # SIMULATION SETUP 9 | 10 | from scipy import signal 11 | import numpy as np 12 | import matplotlib.pyplot as plt 13 | from mobotpy.models import Cart 14 | 15 | # %% 16 | # PARAMETERS 17 | 18 | # Set some parameters that describe the desired behaviour 19 | ZETA = 1.1 20 | OMEGA_N = np.sqrt(0.3) 21 | 22 | # Define the sample time [s] 23 | T = 0.5 24 | 25 | # Compute the pole locations 26 | lambda_s = np.roots([1, 2 * ZETA * OMEGA_N, OMEGA_N ** 2]) 27 | lambda_z = np.exp(lambda_s * T) 28 | 29 | # Define the vehicle mass [kg] 30 | M = 10.0 31 | 32 | # Define the system matrices 33 | F = np.array([[1, T], [0, 1]]) 34 | G = np.array([[T ** 2 / (2 * M)], [T / M]]) 35 | 36 | # Find gain matrix K that places the poles at lambda_z 37 | K = signal.place_poles(F, G, lambda_z) 38 | 39 | # %% 40 | # FUNCTION DEFINITIONS 41 | 42 | 43 | def vehicle(x, u, F, G): 44 | """Discrete-time 1D dynamic vehicle model.""" 45 | x_new = F @ x + G @ [u] 46 | return x_new 47 | 48 | 49 | def controller(x, K): 50 | """Proportional controller.""" 51 | u = -K @ x 52 | return u 53 | 54 | 55 | # %% 56 | # RUN SIMULATION 57 | 58 | # Create an array of time values [s] 59 | SIM_TIME = 30.0 60 | t = np.arange(0, SIM_TIME, T) 61 | N = np.size(t) 62 | 63 | # Initialize arrays that will be populated with our inputs and states 64 | x = np.zeros((2, N)) 65 | u = np.zeros(N) 66 | 67 | # Set the initial position [m], velocity [m/s], and force input [N] 68 | x[0, 0] = 1.0 69 | x[1, 0] = 0.0 70 | u[0] = 0.0 71 | 72 | # Run the simulation 73 | for k in range(1, N): 74 | x[:, k] = vehicle(x[:, k - 1], u[k - 1], F, G) 75 | u[k] = controller(x[:, k], K.gain_matrix) 76 | 77 | # %% 78 | # MAKE A PLOT 79 | 80 | # Change some plot settings (optional) 81 | plt.rc("text", usetex=True) 82 | plt.rc("text.latex", preamble=r"\usepackage{cmbright,amsmath}") 83 | plt.rc("savefig", format="pdf") 84 | plt.rc("savefig", bbox="tight") 85 | 86 | # Plot the states (x) and input (u) vs time (t) 87 | fig1 = plt.figure(1) 88 | ax1a = plt.subplot(311) 89 | plt.plot(t, x[0, :], "C0") 90 | plt.grid(color="0.95") 91 | plt.ylabel(r"$x_1$ [m]") 92 | plt.setp(ax1a, xticklabels=[]) 93 | ax1b = plt.subplot(312) 94 | plt.plot(t, x[1, :], "C0") 95 | plt.grid(color="0.95") 96 | plt.ylabel(r"$x_2$ [m/s]") 97 | plt.setp(ax1b, xticklabels=[]) 98 | ax1c = plt.subplot(313) 99 | plt.step(t, u, "C1", where="post") 100 | plt.grid(color="0.95") 101 | plt.ylabel(r"$u$ [N]") 102 | plt.xlabel(r"$t$ [s]") 103 | 104 | # Save the plot 105 | plt.savefig("../agv-book/figs/ch2/oneD_discrete_control_fig1.pdf") 106 | 107 | # %% 108 | # MAKE AN ANIMATION 109 | 110 | # Set the side length of the vehicle [m] 111 | LENGTH = 1.0 112 | 113 | # Let's use the Cart class to create an animation 114 | vehicle = Cart(LENGTH) 115 | 116 | # Create and save the animation 117 | ani = vehicle.animate( 118 | x[0, :], T, True, "../agv-book/gifs/ch2/oneD_discrete_control.gif" 119 | ) 120 | 121 | # %% 122 | 123 | # Show all the plots to the screen 124 | plt.show() 125 | -------------------------------------------------------------------------------- /oneD_dynamic.py: -------------------------------------------------------------------------------- 1 | """ 2 | Example oneD_dynamic.py 3 | Author: Joshua A. Marshall 4 | GitHub: https://github.com/botprof/agv-examples 5 | """ 6 | 7 | # %% 8 | # SIMULATION SETUP 9 | 10 | import numpy as np 11 | import matplotlib.pyplot as plt 12 | from mobotpy.models import Cart 13 | 14 | # Set the simulation time [s] and the sample period [s] 15 | SIM_TIME = 30.0 16 | T = 0.04 17 | 18 | # Create an array of time values [s] 19 | t = np.arange(0, SIM_TIME, T) 20 | N = np.size(t) 21 | 22 | # %% 23 | # FUNCTION DEFINITIONS 24 | 25 | # Set the mass of the vehicle [kg] 26 | M = 10.0 27 | 28 | # Define the vehicle model matrices 29 | F = np.array([[1, T], [0, 1]]) 30 | G = np.array([[T ** 2 / (2 * M)], [T / M]]) 31 | 32 | 33 | def vehicle(x, u, F, G): 34 | """Discrete-time 1D dynamic vehicle model.""" 35 | x_new = F @ x + G @ [u] 36 | return x_new 37 | 38 | 39 | # %% 40 | # RUN SIMULATION 41 | 42 | # Initialize arrays that will be populated with our inputs and states 43 | x = np.zeros((2, N)) 44 | u = np.zeros(N) 45 | 46 | # Set the initial position [m], velocity [m/s], and force input [N] 47 | x[0, 0] = 1.0 48 | x[1, 0] = 0.0 49 | u[0] = 0.0 50 | 51 | # Run the simulation 52 | for k in range(1, N): 53 | x[:, k] = vehicle(x[:, k - 1], u[k - 1], F, G) 54 | u[k] = np.sin(k * T) 55 | 56 | # %% 57 | # MAKE A PLOT 58 | 59 | # Change some plot settings (optional) 60 | plt.rc("text", usetex=True) 61 | plt.rc("text.latex", preamble=r"\usepackage{cmbright,amsmath}") 62 | plt.rc("savefig", format="pdf") 63 | plt.rc("savefig", bbox="tight") 64 | 65 | # Plot the states (x) and input (u) vs time (t) 66 | fig1 = plt.figure(1) 67 | ax1a = plt.subplot(311) 68 | plt.plot(t, x[0, :], "C0") 69 | plt.grid(color="0.95") 70 | plt.ylabel(r"$x_1$ [m]") 71 | plt.setp(ax1a, xticklabels=[]) 72 | ax1b = plt.subplot(312) 73 | plt.plot(t, x[1, :], "C0") 74 | plt.grid(color="0.95") 75 | plt.ylabel(r"$x_2$ [m/s]") 76 | plt.setp(ax1b, xticklabels=[]) 77 | ax1c = plt.subplot(313) 78 | plt.step(t, u, "C1", where="post") 79 | plt.grid(color="0.95") 80 | plt.ylabel(r"$u$ [N]") 81 | plt.xlabel(r"$t$ [s]") 82 | 83 | # Save the plot 84 | plt.savefig("../agv-book/figs/ch2/oneD_dynamic_fig1.pdf") 85 | 86 | # %% 87 | # MAKE AN ANIMATION 88 | 89 | # Set the side length of the vehicle [m] 90 | LENGTH = 1.0 91 | 92 | # Let's use the Cart class to create an animation 93 | vehicle = Cart(LENGTH) 94 | 95 | # Create and save the animation 96 | ani = vehicle.animate(x[0, :], T, True, "../agv-book/gifs/ch2/oneD_dynamic.gif") 97 | 98 | # %% 99 | 100 | # Show all the plots to the screen 101 | plt.show() 102 | 103 | # Show animation in HTML output if you are using IPython or Jupyter notebooks 104 | # plt.rc('animation', html='jshtml') 105 | # display(ani) 106 | # plt.close() 107 | -------------------------------------------------------------------------------- /oneD_dynamic_control.py: -------------------------------------------------------------------------------- 1 | """ 2 | Example oneD_dynamic_control.py 3 | Author: Joshua A. Marshall 4 | GitHub: https://github.com/botprof/agv-examples 5 | """ 6 | 7 | # %% 8 | # SIMULATION SETUP 9 | 10 | import numpy as np 11 | import matplotlib.pyplot as plt 12 | from mobotpy.models import Cart 13 | 14 | # Set the simulation time [s] and the sample period [s] 15 | SIM_TIME = 30.0 16 | T = 0.04 17 | 18 | # Create an array of time values [s] 19 | t = np.arange(0, SIM_TIME, T) 20 | N = np.size(t) 21 | 22 | # %% 23 | # FUNCTION DEFINITIONS 24 | 25 | # Set the mass of the vehicle [kg] 26 | m = 10.0 27 | 28 | # Define the vehicle model matrices 29 | F = np.array([[1, T], [0, 1]]) 30 | G = np.array([[T**2 / (2 * m)], [T / m]]) 31 | 32 | 33 | def vehicle(x, u, F, G): 34 | """Discrete-time 1D dynamic vehicle model.""" 35 | x_new = F @ x + G @ [u] 36 | return x_new 37 | 38 | 39 | def controller(x, K): 40 | """Proportional controller.""" 41 | u_new = -K @ x 42 | return u_new 43 | 44 | 45 | # %% 46 | # RUN SIMULATION 47 | 48 | # Initialize arrays that will be populated with our inputs and states 49 | x = np.zeros((2, N)) 50 | u = np.zeros(N) 51 | 52 | # Choose the controller gains 53 | K = np.array([3.0, 4.0]) 54 | 55 | # Set the initial position [m], velocity [m/s], and force input [N] 56 | x[0, 0] = 1.0 57 | x[1, 0] = 0.0 58 | u[0] = 0.0 59 | 60 | # Run the simulation 61 | for k in range(1, N): 62 | x[:, k] = vehicle(x[:, k - 1], u[k - 1], F, G) 63 | u[k] = controller(x[:, k], K) 64 | 65 | # %% 66 | # MAKE A PLOT 67 | 68 | # Change some plot settings (optional) 69 | plt.rc("text", usetex=True) 70 | plt.rc("text.latex", preamble=r"\usepackage{cmbright,amsmath}") 71 | plt.rc("savefig", format="pdf") 72 | plt.rc("savefig", bbox="tight") 73 | 74 | # Plot the states (x) and input (u) vs time (t) 75 | fig1 = plt.figure(1) 76 | ax1a = plt.subplot(311) 77 | plt.plot(t, x[0, :], "C0") 78 | plt.grid(color="0.95") 79 | plt.ylabel(r"$x_1$ [m]") 80 | plt.setp(ax1a, xticklabels=[]) 81 | ax1b = plt.subplot(312) 82 | plt.plot(t, x[1, :], "C0") 83 | plt.grid(color="0.95") 84 | plt.ylabel(r"$x_2$ [m/s]") 85 | plt.setp(ax1b, xticklabels=[]) 86 | ax1c = plt.subplot(313) 87 | plt.step(t, u, "C1", where="post") 88 | plt.grid(color="0.95") 89 | plt.ylabel(r"$u$ [N]") 90 | plt.xlabel(r"$t$ [s]") 91 | 92 | # Save the plot 93 | plt.savefig("../agv-book/figs/ch2/oneD_dynamic_control_fig1.pdf") 94 | 95 | # %% 96 | # MAKE AN ANIMATION 97 | 98 | # Set the side length of the vehicle [m] 99 | LENGTH = 1.0 100 | 101 | # Let's use the Cart class to create an animation 102 | vehicle = Cart(LENGTH) 103 | 104 | # Create and save the animation 105 | ani = vehicle.animate(x[0, :], T, True, "../agv-book/gifs/ch2/oneD_dynamic_control.gif") 106 | 107 | # %% 108 | 109 | # Show all the plots to the screen 110 | plt.show() 111 | 112 | # Show animation in HTML output if you are using IPython or Jupyter notebooks 113 | # plt.rc('animation', html='jshtml') 114 | # display(ani) 115 | # plt.close() 116 | -------------------------------------------------------------------------------- /oneD_dynamic_observer.py: -------------------------------------------------------------------------------- 1 | """ 2 | Example oneD_dynamic_observer.py 3 | Author: Joshua A. Marshall 4 | GitHub: https://github.com/botprof/agv-examples 5 | """ 6 | 7 | # %% 8 | # SIMULATION SETUP 9 | 10 | from scipy import signal 11 | import numpy as np 12 | import matplotlib.pyplot as plt 13 | 14 | # Set the simulation time [s] and the sample period [s] 15 | SIM_TIME = 5.0 16 | T = 0.1 17 | 18 | # Create an array of time values [s] 19 | t = np.arange(0, SIM_TIME, T) 20 | N = np.size(t) 21 | 22 | # %% 23 | # VEHICLE MODEL DEFINITION 24 | 25 | # Set the mass of the vehicle [kg] 26 | M = 10.0 27 | 28 | # Function that models the vehicle and sensor(s) in discrete time 29 | F = np.array([[1, T], [0, 1]]) 30 | G = np.array([[T**2 / (2 * M)], [T / M]]) 31 | H = np.array([[1, 0]]) 32 | 33 | 34 | def vehicle(x, u, F, G): 35 | """Discrete-time 1D dynamic vehicle model.""" 36 | x_new = F @ x + G @ [u] 37 | return x_new 38 | 39 | 40 | # %% 41 | # OBSERVER DEFINITION 42 | 43 | # Choose estimator gains for stability 44 | lambda_z = np.array([0.5, 0.4]) 45 | L = signal.place_poles(F.T, H.T, lambda_z).gain_matrix.T 46 | 47 | 48 | def observer(x_hat, u, y, F, G, H, L): 49 | """Observer-based state estimator.""" 50 | x_hat_new = F @ x_hat + G @ [u] + L @ ([y] - H @ x_hat) 51 | return x_hat_new 52 | 53 | 54 | # %% 55 | # RUN SIMULATION 56 | 57 | # Initialize arrays that will be populated with our inputs and states 58 | x = np.zeros((2, N)) 59 | u = np.zeros(N) 60 | x_hat = np.zeros((2, N)) 61 | 62 | # Set the initial position [m], velocity [m/s], and force input [N] 63 | x[0, 0] = 1.0 64 | x[1, 0] = 0.0 65 | u[0] = 0.0 66 | 67 | # Set the initial estimated position (different from the actual position) 68 | x_hat[0, 0] = 0.0 69 | x_hat[0, 0] = 0.0 70 | 71 | # Run the simulation for time step k 72 | for k in range(1, N): 73 | y = x[0, k - 1] 74 | x_hat[:, k] = observer(x_hat[:, k - 1], u[k - 1], y, F, G, H, L) 75 | x[:, k] = vehicle(x[:, k - 1], u[k - 1], F, G) 76 | u[k] = 2.0 * np.sin(k * T) 77 | 78 | # %% 79 | # MAKE A PLOT 80 | 81 | # Change some plot settings (optional) 82 | plt.rc("text", usetex=True) 83 | plt.rc("text.latex", preamble=r"\usepackage{cmbright,amsmath}") 84 | plt.rc("savefig", format="pdf") 85 | plt.rc("savefig", bbox="tight") 86 | 87 | # Plot the states (x) and input (u) vs time (t) 88 | fig1 = plt.figure(1) 89 | ax1a = plt.subplot(211) 90 | plt.plot(t, x[0, :], "C0") 91 | plt.step(t, x_hat[0, :], "C1--", where="post") 92 | plt.grid(color="0.95") 93 | plt.ylabel(r"$x_1$ [m]") 94 | plt.setp(ax1a, xticklabels=[]) 95 | plt.legend(["Actual", "Estimated"]) 96 | ax1b = plt.subplot(212) 97 | plt.plot(t, x[1, :], "C0") 98 | plt.step(t, x_hat[1, :], "C1--", where="post") 99 | plt.grid(color="0.95") 100 | plt.ylabel(r"$x_2$ [m/s]") 101 | plt.xlabel(r"$t$ [s]") 102 | 103 | # Save the plot 104 | # plt.savefig("../agv-book/figs/ch2/oneD_dynamic_observer_fig1.pdf") 105 | 106 | # Show all the plots to the screen 107 | plt.show() 108 | -------------------------------------------------------------------------------- /oneD_integral_control.py: -------------------------------------------------------------------------------- 1 | """ 2 | Example oneD_integral_control.py 3 | Author: Joshua A. Marshall 4 | GitHub: https://github.com/botprof/agv-examples 5 | """ 6 | 7 | # %% 8 | # SIMULATION SETUP 9 | 10 | from scipy import signal 11 | import numpy as np 12 | import matplotlib.pyplot as plt 13 | from mobotpy.models import Cart 14 | 15 | # %% 16 | # PARAMETERS 17 | 18 | # Set some variables that describe the desired behaviour 19 | ZETA = 1.1 20 | OMEGA_N = np.sqrt(0.3) 21 | 22 | # Define the sample time [s] 23 | T = 0.1 24 | 25 | # Compute the pole locations 26 | lambda_s = np.roots([1, 2 * ZETA * OMEGA_N, OMEGA_N ** 2]) 27 | lambda_z = np.hstack((np.exp(lambda_s * T), 0.5)) 28 | 29 | # Define the vehicle mass [kg] 30 | M = 10.0 31 | 32 | # Define the system matrices 33 | F = np.array([[1, T], [0, 1]]) 34 | G = np.array([[T ** 2 / (2 * M)], [T / M]]) 35 | H = np.array([[-1, 0]]) 36 | 37 | # Augmented state system matrices 38 | A = np.hstack((np.vstack((F, H)), np.array([[0], [0], [1]]))) 39 | B = np.vstack((G, 0)) 40 | 41 | # Find gain matrix K that places the poles inside the unit disk 42 | K = signal.place_poles(A, B, lambda_z) 43 | 44 | # %% 45 | # FUNCTION DEFINITIONS 46 | 47 | 48 | def vehicle(x, u, F, G): 49 | """Discrete-time 1D vehicle model on a slope.""" 50 | x_new = F @ x + G @ [u] - G @ [M * 9.81 * np.sin(np.pi / 30)] 51 | return x_new 52 | 53 | 54 | def integrator(x, xi): 55 | """Augmented state integrator.""" 56 | xi_new = xi - x[0] 57 | return xi_new 58 | 59 | 60 | def controller(x, xi, K): 61 | """State feedback controller with integral action.""" 62 | u = -K @ np.hstack((x, xi)) 63 | return u 64 | 65 | 66 | # %% 67 | # RUN SIMULATION 68 | 69 | # Create an array of time values [s] 70 | SIM_TIME = 15.0 71 | t = np.arange(0, SIM_TIME, T) 72 | N = np.size(t) 73 | 74 | # Initialize arrays that will be populated with our inputs and states 75 | x = np.zeros((2, N)) 76 | xi = np.zeros(N) 77 | u = np.zeros(N) 78 | 79 | # Set the initial position [m], velocity [m/s], and force input [N] 80 | x[0, 0] = 1.0 81 | x[1, 0] = 0.0 82 | u[0] = 0.0 83 | 84 | # Run the simulation 85 | for k in range(1, N): 86 | x[:, k] = vehicle(x[:, k - 1], u[k - 1], F, G) 87 | xi[k] = integrator(x[:, k - 1], xi[k - 1]) 88 | u[k] = controller(x[:, k], xi[k], K.gain_matrix) 89 | 90 | # %% 91 | # MAKE A PLOT 92 | 93 | # Change some plot settings (optional) 94 | plt.rc("text", usetex=True) 95 | plt.rc("text.latex", preamble=r"\usepackage{cmbright,amsmath}") 96 | plt.rc("savefig", format="pdf") 97 | plt.rc("savefig", bbox="tight") 98 | 99 | # Plot the states (x) and input (u) vs time (t) 100 | fig1 = plt.figure(1) 101 | ax1a = plt.subplot(311) 102 | plt.plot(t, x[0, :], "C0") 103 | plt.grid(color="0.95") 104 | plt.ylabel(r"$x_1$ [m]") 105 | plt.setp(ax1a, xticklabels=[]) 106 | ax1b = plt.subplot(312) 107 | plt.plot(t, x[1, :], "C0") 108 | plt.grid(color="0.95") 109 | plt.ylabel(r"$x_2$ [m/s]") 110 | plt.setp(ax1b, xticklabels=[]) 111 | ax1c = plt.subplot(313) 112 | plt.step(t, u, "C1", where="post") 113 | plt.grid(color="0.95") 114 | plt.ylabel(r"$u$ [N]") 115 | plt.xlabel(r"$t$ [s]") 116 | 117 | # Save the plot 118 | plt.savefig("../agv-book/figs/ch2/oneD_integral_control_fig1.pdf") 119 | 120 | # %% 121 | # MAKE AN ANIMATION 122 | 123 | # Set the side length of the vehicle [m] 124 | LENGTH = 1.0 125 | 126 | # Let's use the Cart class to create an animation 127 | vehicle = Cart(LENGTH) 128 | 129 | # Create and save the animation 130 | ani = vehicle.animate( 131 | x[0, :], T, True, "../agv-book/gifs/ch2/oneD_integral_control.gif" 132 | ) 133 | 134 | # %% 135 | 136 | # Show all the plots to the screen 137 | plt.show() 138 | -------------------------------------------------------------------------------- /oneD_kinematic.py: -------------------------------------------------------------------------------- 1 | """ 2 | Example oneD_kinematic.py 3 | Author: Joshua A. Marshall 4 | GitHub: https://github.com/botprof/agv-examples 5 | """ 6 | 7 | # %% 8 | # SIMULATION SETUP 9 | 10 | import matplotlib.pyplot as plt 11 | import numpy as np 12 | from mobotpy.models import Cart 13 | 14 | # Set the simulation time [s] and the sample period [s] 15 | SIM_TIME = 30.0 16 | T = 0.4 17 | 18 | # Create an array of time values [s] 19 | t = np.arange(0, SIM_TIME, T) 20 | N = np.size(t) 21 | 22 | # %% 23 | # FUNCTION DEFINITIONS 24 | 25 | 26 | def vehicle(x, u, T): 27 | """Discrete-time 1D kinematic vehicle model.""" 28 | x_new = x + T * u 29 | return x_new 30 | 31 | 32 | # %% 33 | # RUN SIMULATION 34 | 35 | # Initialize arrays that will be populated with our inputs and states 36 | x = np.zeros(N) 37 | u = np.zeros(N) 38 | 39 | # Set the initial position [m] and input [m/s] and run the simulation 40 | x[0] = 1.0 41 | u[0] = 0.0 42 | for k in range(1, N): 43 | x[k] = vehicle(x[k - 1], u[k - 1], T) 44 | u[k] = np.sin(k * T) 45 | 46 | # %% 47 | # MAKE A PLOT 48 | 49 | # Change some plot settings (optional) 50 | plt.rc("text", usetex=True) 51 | plt.rc("text.latex", preamble=r"\usepackage{cmbright,amsmath}") 52 | plt.rc("savefig", format="pdf") 53 | plt.rc("savefig", bbox="tight") 54 | 55 | # Change these to the locations where you wish to store outputs 56 | PDF_PATH = "../agv-book/figs/ch2/" 57 | GIF_PATH = "../agv-book/gifs/ch2/" 58 | 59 | # Plot the state (x) and input (u) vs time (t) 60 | fig1 = plt.figure(1) 61 | ax1a = plt.subplot(211) 62 | plt.plot(t, x, "C0") 63 | plt.grid(color="0.95") 64 | plt.ylabel(r"$x$ [m]") 65 | plt.setp(ax1a, xticklabels=[]) 66 | ax1b = plt.subplot(212) 67 | plt.step(t, u, "C1", where="post") 68 | plt.grid(color="0.95") 69 | plt.ylabel(r"$u$ [m/s]") 70 | plt.xlabel(r"$t$ [s]") 71 | 72 | # Save the plot 73 | plt.savefig(PDF_PATH + "oneD_kinematic_fig1.pdf") 74 | 75 | # %% 76 | # MAKE AN ANIMATION 77 | 78 | # Set the side length of the vehicle [m] 79 | LENGTH = 1.0 80 | 81 | # Let's use the Cart class to create an animation 82 | vehicle = Cart(LENGTH) 83 | 84 | # Create and save the animation 85 | ani = vehicle.animate(x, T, True, GIF_PATH + "oneD_kinematic.gif") 86 | 87 | # %% 88 | # DISPLAY PLOTS 89 | 90 | # Show all the plots to the screen 91 | plt.show() 92 | 93 | # Show animation in HTML output if you are using IPython or Jupyter notebooks 94 | # plt.rc('animation', html='jshtml') 95 | # display(ani) 96 | # plt.close() 97 | -------------------------------------------------------------------------------- /oneD_kinematic_control.py: -------------------------------------------------------------------------------- 1 | """ 2 | Example oneD_kinematic_control.py 3 | Author: Joshua A. Marshall 4 | GitHub: https://github.com/botprof/agv-examples 5 | """ 6 | 7 | # %% 8 | # SIMULATION SETUP 9 | 10 | import numpy as np 11 | import matplotlib.pyplot as plt 12 | from mobotpy.models import Cart 13 | 14 | # Set the simulation time [s] and the sample period [s] 15 | SIM_TIME = 10.0 16 | T = 0.04 17 | 18 | # Create an array of time values [s] 19 | t = np.arange(0, SIM_TIME, T) 20 | N = np.size(t) 21 | 22 | # %% 23 | # FUNCTION DEFINITIONS 24 | 25 | 26 | def vehicle(x, u, T): 27 | """Discrete-time 1D kinematic vehicle model.""" 28 | x_new = x + T * u 29 | return x_new 30 | 31 | 32 | # %% 33 | # RUN SIMULATION 34 | 35 | # Initialize arrays that will be populated with our inputs and states 36 | x = np.zeros(N) 37 | u = np.zeros(N) 38 | 39 | # Set the initial position [m] and initial input [m/s] 40 | x[0] = 1.0 41 | u[0] = 0.0 42 | 43 | # Set the desired position [m] and controller gain 44 | x_d = 4.0 45 | k_P = 1.0 46 | 47 | # Run the simulation 48 | for k in range(1, N): 49 | x[k] = vehicle(x[k - 1], u[k - 1], T) 50 | u[k] = k_P * (x_d - x[k]) 51 | 52 | # %% 53 | # MAKE A PLOT 54 | 55 | # Change some plot settings (optional) 56 | plt.rc("text", usetex=True) 57 | plt.rc("text.latex", preamble=r"\usepackage{cmbright,amsmath}") 58 | plt.rc("savefig", format="pdf") 59 | plt.rc("savefig", bbox="tight") 60 | 61 | # Plot the state (x) and input (u) vs time (t) 62 | fig1 = plt.figure(1) 63 | ax1a = plt.subplot(211) 64 | plt.plot(t, x, "C0") 65 | plt.grid(color="0.95") 66 | plt.ylabel(r"$x$ [m]") 67 | plt.setp(ax1a, xticklabels=[]) 68 | ax1b = plt.subplot(212) 69 | plt.step(t, u, "C1", where="post") 70 | plt.grid(color="0.95") 71 | plt.ylabel(r"$u$ [m/s]") 72 | plt.xlabel(r"$t$ [s]") 73 | 74 | # Save the plot 75 | plt.savefig("../agv-book/figs/ch2/oneD_kinematic_control_fig1.pdf") 76 | 77 | # %% 78 | # MAKE AN ANIMATION 79 | 80 | # Set the side length of the vehicle [m] 81 | LENGTH = 1.0 82 | 83 | # Let's use the Cart class to create an animation 84 | vehicle = Cart(LENGTH) 85 | 86 | # Create and save the animation 87 | ani = vehicle.animate(x, T, True, "../agv-book/gifs/ch2/oneD_kinematic_control.gif") 88 | 89 | # %% 90 | 91 | # Show all the plots to the screen 92 | plt.show() 93 | 94 | # Show animation in HTML output if you are using IPython or Jupyter notebooks 95 | # plt.rc('animation', html='jshtml') 96 | # display(ani) 97 | # plt.close() 98 | -------------------------------------------------------------------------------- /recursive_least_squares.py: -------------------------------------------------------------------------------- 1 | """ 2 | Example recursive_least_squares.py 3 | Author: Joshua A. Marshall 4 | GitHub: https://github.com/botprof/agv-examples 5 | """ 6 | 7 | # %% 8 | # SIMULATION SETUP 9 | 10 | import numpy as np 11 | from numpy.linalg import inv 12 | import matplotlib.pyplot as plt 13 | from scipy.stats import chi2 14 | 15 | # Set the simulation time [s] and the sample period [s] 16 | SIM_TIME = 2.0 17 | T = 0.1 18 | 19 | # Create an array of time values [s] 20 | t = np.arange(0.0, SIM_TIME, T) 21 | N = np.size(t) 22 | 23 | # %% 24 | # FUNCTION DEFINITIONS 25 | 26 | 27 | def gps_sensor(x, R, H): 28 | """Simulated GPS sensor.""" 29 | y = H @ x + R @ np.random.randn(np.size(x)) 30 | return y 31 | 32 | 33 | def rls_estimator(x, P, y, R, H): 34 | n = np.size(x) 35 | K = P @ H.T @ inv(H @ P @ H.T + R) 36 | x_next = x + K @ (y - H @ x) 37 | P_next = (np.eye(n) - K @ H) @ P @ (np.eye(n) - K @ H).T + K @ R @ K.T 38 | return x_next, P_next 39 | 40 | 41 | # %% 42 | # SET UP INITIAL CONDITIONS 43 | 44 | # Set up arrays for a two-dimensional GPS measurements problem 45 | x_hat = np.zeros((2, N)) 46 | P_hat = np.zeros((2, 2, N)) 47 | y = np.zeros((2, N)) 48 | 49 | # Robot location (actual) 50 | x = np.array([2.0, 4.0]) 51 | # Initial estimate of the robot's location 52 | x_hat[:, 0] = np.array([0.0, 0.0]) 53 | # Initial covariance matrix 54 | P_hat[:, :, 0] = 10.0 * np.eye(2) 55 | # Measurement noise covariance matrix 56 | SIGMA = 1.0 57 | R = SIGMA**2 * np.eye(2) 58 | # Measurement matrix (i.e., GPS coordinates) 59 | H = np.eye(2) 60 | 61 | # %% 62 | # RUN THE SIMULATION 63 | 64 | for k in range(1, N): 65 | # Simulate the GPS sensor 66 | y[:, k] = gps_sensor(x, R, H) 67 | # Update the estimate using the recursive least squares estimator 68 | x_hat[:, k], P_hat[:, :, k] = rls_estimator( 69 | x_hat[:, k - 1], P_hat[:, :, k - 1], y[:, k], R, H 70 | ) 71 | 72 | # %% 73 | # PLOT THE RESULTS 74 | 75 | # Change some plot settings (optional) 76 | plt.rc("text", usetex=True) 77 | plt.rc("text.latex", preamble=r"\usepackage{cmbright,amsmath,bm}") 78 | plt.rc("savefig", format="pdf") 79 | plt.rc("savefig", bbox="tight") 80 | 81 | # Plot the true and estimated robot location 82 | plt.figure() 83 | plt.plot(x[0], x[1], "C4o", label="True") 84 | plt.plot(x_hat[0, :], x_hat[1, :], "C0-", label="Estimated") 85 | plt.xlabel(r"$x_1$ [m]") 86 | plt.ylabel(r"$x_2$ [m]") 87 | plt.legend() 88 | plt.grid(color="0.95") 89 | plt.axis("equal") 90 | 91 | # Find the scaling factor for plotting covariance bounds 92 | ALPHA = 0.01 93 | s1 = chi2.isf(ALPHA, 1) 94 | s2 = chi2.isf(ALPHA, 2) 95 | 96 | # Plot the error in the estimate 97 | sigma = np.zeros((2, N)) 98 | plt.figure() 99 | ax1 = plt.subplot(211) 100 | sigma[0, :] = np.sqrt(s1 * P_hat[0, 0, :]) 101 | plt.fill_between( 102 | t, 103 | -sigma[0, :], 104 | sigma[0, :], 105 | color="C0", 106 | alpha=0.2, 107 | label=str(100 * (1 - ALPHA)) + r" \% Confidence", 108 | ) 109 | plt.plot(t, x[0] - x_hat[0, :], "C0", label="Estimator Error") 110 | plt.plot(t, x[0] - y[0, :], "C1", label="Raw Measurement Error") 111 | plt.ylabel(r"$e_1$ [m]") 112 | plt.setp(ax1, xticklabels=[]) 113 | plt.grid(color="0.95") 114 | plt.legend() 115 | ax2 = plt.subplot(212) 116 | sigma[1, :] = np.sqrt(s1 * P_hat[1, 1, :]) 117 | plt.fill_between( 118 | t, 119 | -sigma[1, :], 120 | sigma[1, :], 121 | color="C0", 122 | alpha=0.2, 123 | label=str(100 * (1 - ALPHA)) + r" \% Confidence", 124 | ) 125 | plt.plot(t, x[1] - x_hat[1, :], "C0", label="Estimator Error") 126 | plt.plot(t, x[1] - y[1, :], "C1", label="Raw Measurement Error") 127 | plt.ylabel(r"$e_2$ [m]") 128 | plt.grid(color="0.95") 129 | plt.xlabel(r"$t$ [s]") 130 | plt.show() 131 | 132 | # %% 133 | -------------------------------------------------------------------------------- /tricycle_kinematic.py: -------------------------------------------------------------------------------- 1 | """ 2 | Example tricycle_kinematic.py 3 | Author: Joshua A. Marshall 4 | GitHub: https://github.com/botprof/agv-examples 5 | """ 6 | 7 | # %% 8 | # SIMULATION SETUP 9 | 10 | import numpy as np 11 | import matplotlib.pyplot as plt 12 | from mobotpy.models import Tricycle 13 | from mobotpy.integration import rk_four 14 | 15 | # Set the simulation time [s] and the sample period [s] 16 | SIM_TIME = 15.0 17 | T = 0.1 18 | 19 | # Create an array of time values [s] 20 | t = np.arange(0.0, SIM_TIME, T) 21 | N = np.size(t) 22 | 23 | # Set the wheelbase and track of the vehicle [m] 24 | ELL_W = 2.50 25 | ELL_T = 1.75 26 | 27 | # %% 28 | # MODEL DEFINITION 29 | 30 | 31 | def tricycle_f(x, u): 32 | """Tricycle vehicle kinematic model.""" 33 | f = np.zeros(4) 34 | f[0] = u[0] * np.cos(x[2]) 35 | f[1] = u[0] * np.sin(x[2]) 36 | f[2] = u[0] * 1.0 / ELL_W * np.tan(x[3]) 37 | f[3] = u[1] 38 | return f 39 | 40 | 41 | # %% 42 | # RUN SIMULATION 43 | 44 | # Initialize arrays that will be populated with our inputs and states 45 | x = np.zeros((4, N)) 46 | u = np.zeros((2, N)) 47 | 48 | # Set the initial pose [m, m, rad, rad], velocities [m/s, rad/s] 49 | x[0, 0] = 0.0 50 | x[1, 0] = 0.0 51 | x[2, 0] = np.pi / 2.0 52 | x[3, 0] = 0.0 53 | u[0, 0] = 5.0 54 | u[1, 0] = 0 55 | 56 | # Run the simulation 57 | for k in range(1, N): 58 | x[:, k] = rk_four(tricycle_f, x[:, k - 1], u[:, k - 1], T) 59 | u[0, k] = 5.0 60 | u[1, k] = 0.25 * np.sin(2.0 * t[k]) 61 | 62 | # %% 63 | # MAKE SOME PLOTS 64 | 65 | # Change some plot settings (optional) 66 | plt.rc("text", usetex=True) 67 | plt.rc("text.latex", preamble=r"\usepackage{cmbright,amsmath,bm}") 68 | plt.rc("savefig", format="pdf") 69 | plt.rc("savefig", bbox="tight") 70 | 71 | # Plot the states as a function of time 72 | fig1 = plt.figure(1) 73 | fig1.set_figheight(6.4) 74 | ax1a = plt.subplot(611) 75 | plt.plot(t, x[0, :]) 76 | plt.grid(color="0.95") 77 | plt.ylabel(r"$x$ [m]") 78 | plt.setp(ax1a, xticklabels=[]) 79 | ax1b = plt.subplot(612) 80 | plt.plot(t, x[1, :]) 81 | plt.grid(color="0.95") 82 | plt.ylabel(r"$y$ [m]") 83 | plt.setp(ax1b, xticklabels=[]) 84 | ax1c = plt.subplot(613) 85 | plt.plot(t, x[2, :] * 180.0 / np.pi) 86 | plt.grid(color="0.95") 87 | plt.ylabel(r"$\theta$ [deg]") 88 | plt.setp(ax1c, xticklabels=[]) 89 | ax1c = plt.subplot(614) 90 | plt.plot(t, x[3, :] * 180.0 / np.pi) 91 | plt.grid(color="0.95") 92 | plt.ylabel(r"$\phi$ [deg]") 93 | plt.setp(ax1c, xticklabels=[]) 94 | ax1c = plt.subplot(615) 95 | plt.step(t, u[0, :], "C1", where="post") 96 | plt.grid(color="0.95") 97 | plt.ylabel(r"$v_1$ [m/s]") 98 | plt.setp(ax1c, xticklabels=[]) 99 | ax1d = plt.subplot(616) 100 | plt.step(t, u[1, :], "C1", where="post") 101 | plt.grid(color="0.95") 102 | plt.ylabel(r"$v_2$ [deg/s]") 103 | plt.xlabel(r"$t$ [s]") 104 | 105 | # Save the plot 106 | # plt.savefig("../agv-book/figs/ch3/tricycle_kinematic_fig1.pdf") 107 | 108 | # Let's now use the class Tricycle for plotting 109 | vehicle = Tricycle(ELL_W, ELL_T) 110 | 111 | # Plot the position of the vehicle in the plane 112 | fig2 = plt.figure(2) 113 | plt.plot(x[0, :], x[1, :]) 114 | plt.axis("equal") 115 | X_L, Y_L, X_R, Y_R, X_F, Y_F, X_B, Y_B = vehicle.draw( 116 | x[0, 0], x[1, 0], x[2, 0], x[3, 0] 117 | ) 118 | plt.fill(X_L, Y_L, "k") 119 | plt.fill(X_R, Y_R, "k") 120 | plt.fill(X_F, Y_F, "k") 121 | plt.fill(X_B, Y_B, "C2", alpha=0.5, label="Start") 122 | X_L, Y_L, X_R, Y_R, X_F, Y_F, X_B, Y_B = vehicle.draw( 123 | x[0, N - 1], x[1, N - 1], x[2, N - 1], x[3, N - 1] 124 | ) 125 | plt.fill(X_L, Y_L, "k") 126 | plt.fill(X_R, Y_R, "k") 127 | plt.fill(X_F, Y_F, "k") 128 | plt.fill(X_B, Y_B, "C3", alpha=0.5, label="End") 129 | plt.xlabel(r"$x$ [m]") 130 | plt.ylabel(r"$y$ [m]") 131 | plt.legend() 132 | 133 | # Save the plot 134 | # plt.savefig("../agv-book/figs/ch3/tricycle_kinematic_fig2.pdf") 135 | 136 | # Show the plots to the screen 137 | # plt.show() 138 | 139 | # %% 140 | # MAKE AN ANIMATION 141 | 142 | # Create and save the animation 143 | ani = vehicle.animate(x, T) 144 | 145 | # Create and save the animation 146 | # ani = vehicle.animate(x, T, True, "../agv-book/gifs/ch3/tricycle_kinematic.gif") 147 | 148 | # Show all the plots to the screen 149 | plt.show() 150 | 151 | # Show animation in HTML output if you are using IPython or Jupyter notebooks 152 | # from IPython.display import display 153 | # plt.rc('animation', html='jshtml') 154 | # display(ani) 155 | # plt.close() 156 | -------------------------------------------------------------------------------- /unicycle_dynamic.py: -------------------------------------------------------------------------------- 1 | """ 2 | Example unicycle_dynamic.py 3 | Author: Joshua A. Marshall 4 | GitHub: https://github.com/botprof/agv-examples 5 | """ 6 | 7 | # %% 8 | # SIMULATION SETUP 9 | 10 | from IPython.display import display 11 | import numpy as np 12 | import matplotlib.pyplot as plt 13 | from mobotpy.integration import rk_four 14 | from mobotpy.models import DiffDrive 15 | 16 | # Set the simulation time [s] and the sample period [s] 17 | SIM_TIME = 60.0 18 | T = 0.04 19 | 20 | # Create an array of time values [s] 21 | t = np.arange(0.0, SIM_TIME, T) 22 | N = np.size(t) 23 | 24 | # Set the vehicle's mass and moment of inertia 25 | M = 1.0 # kg 26 | I = 1.0 # kg m^2 27 | 28 | # Set the maximum lateral tire force [N] 29 | LAMBDA_MAX = 0.1 30 | 31 | # %% 32 | # FUNCTION DEFINITIONS 33 | 34 | 35 | def unicycle_f_dyn(x, u): 36 | """Unicycle dynamic vehicle model. 37 | 38 | Parameters 39 | ---------- 40 | x : ndarray of length 6 41 | The vehicle's state (x, y, theta, v_x, v_y, v_2). 42 | u : ndarray of length 3 43 | Force and torque applied to the wheel (f, tau, lambda_f). 44 | 45 | Returns 46 | ------- 47 | f_dyn : ndarray of length 6 48 | The rate of change of the vehicle states. 49 | """ 50 | f_dyn = np.zeros(6) 51 | f_dyn[0] = x[3] 52 | f_dyn[1] = x[4] 53 | f_dyn[2] = x[5] 54 | f_dyn[3] = 1.0 / M * (u[0] * np.cos(x[2]) - u[2] * np.sin(x[2])) 55 | f_dyn[4] = 1.0 / M * (u[0] * np.sin(x[2]) + u[2] * np.cos(x[2])) 56 | f_dyn[5] = 1.0 / I * u[1] 57 | return f_dyn 58 | 59 | 60 | def lateral_force(x): 61 | """Computes the lateral tire force for a single wheel. 62 | 63 | Parameters 64 | ---------- 65 | x : ndarray of length 6 66 | The vehicle's state (x, y, theta, v_x, v_y, v_2). 67 | 68 | Returns 69 | ------- 70 | lambda_f : float 71 | The computed lateral tire force [N]. 72 | old_x : ndarray of length 6 73 | The vehicle's state with or without slip. 74 | """ 75 | 76 | # Compute lateral force 77 | lambda_f = M * x[5] * (x[3] * np.cos(x[2]) + x[4] * np.sin(x[2])) 78 | 79 | # Check if the required lateral force is bigger than the tire can handle 80 | if np.abs(lambda_f) > LAMBDA_MAX: 81 | # Wheel slips 82 | lambda_f = LAMBDA_MAX * np.sign(lambda_f) 83 | old_vx = x[3] 84 | old_vy = x[4] 85 | else: 86 | # Wheel doesn't slip, so enforce velocity to be in the wheel's direction 87 | old_vx = (x[3] * np.cos(x[2]) + x[4] * np.sin(x[2])) * np.cos(x[2]) 88 | old_vy = (x[3] * np.cos(x[2]) + x[4] * np.sin(x[2])) * np.sin(x[2]) 89 | 90 | # Assign the new state 91 | old_x = np.array([x[0], x[1], x[2], old_vx, old_vy, x[5]]) 92 | 93 | # Return the output 94 | return lambda_f, old_x 95 | 96 | 97 | # %% 98 | # RUN SIMULATION 99 | 100 | # Initialize arrays that will be populated with our inputs and states 101 | x = np.zeros((6, N)) 102 | u = np.zeros((3, N)) 103 | 104 | # Set the initial conditions 105 | x_init = np.zeros(6) 106 | x_init[0] = 0.0 107 | x_init[1] = 3.0 108 | x_init[2] = 0.0 109 | x_init[3] = 0.0 110 | x_init[4] = 0.0 111 | x_init[5] = 0.0 112 | x[:, 0] = x_init 113 | 114 | # Run the simulation 115 | for k in range(1, N): 116 | 117 | # Make some force and torque inputs to steer the vehicle around 118 | if k < round(N / 6): 119 | u[0, k - 1] = 0.1 # Apply "large" force 120 | u[1, k - 1] = 0.0 121 | elif k < round(3 * N / 6): 122 | u[0, k - 1] = 0.0 # Stop accelerating 123 | u[1, k - 1] = -0.01 # Start turning 124 | elif k < round(4 * N / 6): 125 | u[0, k - 1] = 0.0 126 | u[1, k - 1] = 0.02 # Turn the other way 127 | else: 128 | u[0, k - 1] = 0.0 129 | u[1, k - 1] = 0.0 130 | 131 | # Compute the lateral force applied to the vehicle's wheel 132 | u[2, k - 1], x[:, k - 1] = lateral_force(x[:, k - 1]) 133 | 134 | # Update the motion of the vehicle 135 | x[:, k] = rk_four(unicycle_f_dyn, x[:, k - 1], u[:, k - 1], T) 136 | 137 | # %% 138 | # MAKE PLOTS 139 | 140 | # Change some plot settings (optional) 141 | plt.rc("text", usetex=True) 142 | plt.rc("text.latex", preamble=r"\usepackage{cmbright,amsmath,bm}") 143 | plt.rc("savefig", format="pdf") 144 | plt.rc("savefig", bbox="tight") 145 | 146 | # Plot the states as a function of time 147 | fig1 = plt.figure(1) 148 | ax1 = plt.subplot(311) 149 | plt.setp(ax1, xticklabels=[]) 150 | plt.plot(t, x[0, :], "C0") 151 | plt.ylabel(r"$x$ [m]") 152 | plt.grid(color="0.95") 153 | ax2 = plt.subplot(312) 154 | plt.setp(ax2, xticklabels=[]) 155 | plt.plot(t, x[1, :], "C0") 156 | plt.ylabel(r"$y$ [m]") 157 | plt.grid(color="0.95") 158 | ax3 = plt.subplot(313) 159 | plt.plot(t, x[2, :], "C0") 160 | plt.xlabel(r"$t$ [s]") 161 | plt.ylabel(r"$\theta$ [rad]") 162 | plt.grid(color="0.95") 163 | 164 | # Save the plot 165 | plt.savefig("../agv-book/figs/ch3/unicycle_dynamic_fig1.pdf") 166 | 167 | # Plot the lateral tire force 168 | fig2 = plt.figure(2) 169 | plt.plot(t[0 : N - 1], u[2, 0 : N - 1], "C0") 170 | plt.xlabel(r"$t$ [s]") 171 | plt.ylabel(r"$\lambda$ [N]") 172 | plt.grid(color="0.95") 173 | 174 | # Save the plot 175 | plt.savefig("../agv-book/figs/ch3/unicycle_dynamic_fig2.pdf") 176 | 177 | """ 178 | To keep thing simple, we plot the unicycle as a differential drive vehicle, 179 | because the differential drive vehicle has the same nonholonomic constraints. 180 | """ 181 | 182 | # Set the track of the vehicle [m] 183 | ELL = 1.0 184 | 185 | # Let's now use the class DiffDrive for plotting 186 | vehicle = DiffDrive(ELL) 187 | 188 | # Plot the position of the vehicle in the plane 189 | fig3 = plt.figure(3) 190 | plt.plot(x[0, :], x[1, :], "C0") 191 | plt.axis("equal") 192 | X_L, Y_L, X_R, Y_R, X_B, Y_B, X_C, Y_C = vehicle.draw(x[0, 0], x[1, 0], x[2, 0]) 193 | plt.fill(X_L, Y_L, "k") 194 | plt.fill(X_R, Y_R, "k") 195 | plt.fill(X_C, Y_C, "k") 196 | plt.fill(X_B, Y_B, "C2", alpha=0.5, label="Start") 197 | X_L, Y_L, X_R, Y_R, X_B, Y_B, X_C, Y_C = vehicle.draw( 198 | x[0, N - 1], x[1, N - 1], x[2, N - 1] 199 | ) 200 | plt.fill(X_L, Y_L, "k") 201 | plt.fill(X_R, Y_R, "k") 202 | plt.fill(X_C, Y_C, "k") 203 | plt.fill(X_B, Y_B, "C3", alpha=0.5, label="End") 204 | plt.xlabel(r"$x$ [m]") 205 | plt.ylabel(r"$y$ [m]") 206 | plt.legend() 207 | 208 | # Save the plot 209 | plt.savefig("../agv-book/figs/ch3/unicycle_dynamic_fig3.pdf") 210 | 211 | # Show all the plots to the screen 212 | plt.show() 213 | 214 | # %% 215 | # MAKE AN ANIMATION 216 | 217 | # Create and save the animation 218 | ani = vehicle.animate(x, T, True, "../agv-book/gifs/ch3/unicycle_dynamic.gif") 219 | 220 | # Show the movie to the screen 221 | plt.show() 222 | 223 | # # Show animation in HTML output if you are using IPython or Jupyter notebooks 224 | # plt.rc('animation', html='jshtml') 225 | # display(ani) 226 | # plt.close() 227 | -------------------------------------------------------------------------------- /vanilla_SLAM.py: -------------------------------------------------------------------------------- 1 | """ 2 | Example vanilla_SLAM.py 3 | Author: Joshua A. Marshall 4 | GitHub: https://github.com/botprof/agv-examples 5 | """ 6 | 7 | # %% 8 | # SIMULATION SETUP 9 | import numpy as np 10 | import matplotlib.pyplot as plt 11 | from scipy.stats import chi2 12 | from matplotlib import patches 13 | 14 | # Uncomment for interactive plots in Jupyter 15 | # %matplotlib ipympl 16 | 17 | # Set the simulation time [s] and the sample period [s] 18 | SIM_TIME = 60.0 19 | T = 0.1 20 | 21 | # Create an array of time values [s] 22 | t = np.arange(0.0, SIM_TIME, T) 23 | N = np.size(t) 24 | 25 | # %% 26 | # CREATE A MAP OF FEATURES TO BE MAPPED AND USED FOR LOCALIZATION 27 | 28 | # Set the map expanse (square) [m] 29 | D_MAP = 20 30 | 31 | # Set the number of features 32 | m = 20 33 | 34 | # Create the map of features 35 | f_map = np.zeros((2, m)) 36 | for i in range(1, m): 37 | f_map[:, i] = D_MAP * np.random.uniform(0, 1, 2) 38 | 39 | # %% 40 | # VEHICLE, SENSOR MODELS AND KALMAN FILTER FUNCTIONS 41 | 42 | 43 | # Discrete-time omnidirectional vehicle model 44 | def vehicle(x, u, T): 45 | x_new = x + T * u 46 | return x_new 47 | 48 | 49 | # Function to measure relative position to features in map 50 | def f_sensor(x, f_map, R): 51 | 52 | # Find the number of features available 53 | m = np.shape(f_map)[1] 54 | 55 | # Find the relative position measurement with measurement noise 56 | y_new = np.zeros(2 * m) 57 | for i in range(0, m): 58 | y_new[2 * i] = ( 59 | f_map[0, i] - x[0] + np.sqrt(R[0, 0]) * np.random.normal(0, 1, 1)[0] 60 | ) 61 | y_new[2 * i + 1] = ( 62 | f_map[1, i] - x[1] + np.sqrt(R[1, 1]) * np.random.normal(0, 1, 1)[0] 63 | ) 64 | 65 | # Return the measurement array 66 | return y_new 67 | 68 | 69 | # Prediction step for a KF with process noise covariance Q 70 | def KF_predict(x, u_m, F, Q, P, T): 71 | x_new = vehicle(x, u_m, T) 72 | P_new = F @ P @ np.transpose(F) + Q 73 | return x_new, P_new 74 | 75 | 76 | # Correction step for a KF with measurement noise covariance R 77 | def KF_correct(x, y, H, R, P): 78 | K = P @ np.transpose(H) @ np.linalg.inv(H @ P @ np.transpose(H) + R) 79 | x_new = x + K @ (y - H @ x) 80 | P_new = (np.identity(np.shape(x)[0]) - K @ H) @ P 81 | return x_new, P_new 82 | 83 | 84 | # %% 85 | # SET UP AND RUN THE SLAM PROBLEM 86 | 87 | # Initial robot position [m, m] 88 | x_init = np.zeros(2) 89 | 90 | # Initialize the states and covariance arrays 91 | x_veh = np.zeros((2, N)) 92 | x_hat = np.zeros((2 * m, N)) 93 | P_hat = np.zeros((2 * m, 2 * m, N)) 94 | 95 | # Set the process and measurement noise covariances 96 | Q_x = np.diag(np.square([0.05, 0.05])) 97 | R_y = np.diag(np.square([5.0, 5.0])) 98 | 99 | # Choose a location where the vehicle starts (relative to map origin) 100 | x_veh[:, 0] = -f_map[:, 0] + np.array([5, 5]) 101 | 102 | # Set up the vehicle model 103 | F = np.eye(2) 104 | 105 | # Set up the measurement model 106 | H = np.zeros((2 * m, 2 * m)) 107 | H[0:2, :] = np.hstack((-np.identity(2), np.zeros((2, 2 * m - 2)))) 108 | H[2 : 2 * m, :] = np.hstack( 109 | (np.kron(np.ones((m - 1, 1)), -np.identity(2)), np.identity(2 * m - 2)) 110 | ) 111 | R = np.kron(np.identity(m), R_y) 112 | 113 | # Simulation the SLAM solution 114 | for i in range(0, N): 115 | 116 | # Initialize using the first feature as a point of reference 117 | if i == 0: 118 | 119 | # Feature observations made by the vehicle 120 | y = f_sensor(x_veh[:, i], f_map, R_y) 121 | 122 | # Initial state estimates 123 | x_hat[0:2, i] = -y[0:2] 124 | P_hat[0:2, 0:2, i] = Q_x + R_y 125 | for j in range(1, m): 126 | x_hat[2 * j, i] = x_hat[0, i] + y[2 * j] 127 | x_hat[2 * j + 1, i] = x_hat[1, i] + y[2 * j + 1] 128 | P_hat[2 * j : 2 * j + 2, 2 * j : 2 * j + 2, i] = P_hat[0:2, 0:2, i] + R_y 129 | 130 | else: 131 | 132 | # Compute some inputs (i.e., drive the vehicle around a square) 133 | if i < 0.25 * N: 134 | u = np.array([1, 0]) 135 | elif i < 0.5 * N: 136 | u = np.array([0, 1]) 137 | elif i < 0.75 * N: 138 | u = np.array([-1, 0]) 139 | else: 140 | u = np.array([0, -1]) 141 | 142 | # Update the vehicle motion 143 | x_veh[:, i] = vehicle(x_veh[:, i - 1], u, T) 144 | 145 | # Feature measurements/observations made by the vehicle 146 | y = f_sensor(x_veh[:, i], f_map, R_y) 147 | 148 | # Run the KF prediction step 149 | x_hat[0:2, i], P_hat[0:2, 0:2, i] = KF_predict( 150 | x_hat[0:2, i - 1], 151 | u + np.sqrt(Q_x) @ np.random.normal(0, 1, 2), 152 | F, 153 | Q_x, 154 | P_hat[0:2, 0:2, i - 1], 155 | T, 156 | ) 157 | x_hat[2 : 2 * m, i] = x_hat[2 : 2 * m, i - 1] 158 | P_hat[2 : 2 * m, 0 : 2 * m, i] = P_hat[2 : 2 * m, 0 : 2 * m, i - 1] 159 | 160 | # Run the KF correction step (try commenting out and see what happens) 161 | x_hat[:, i], P_hat[:, :, i] = KF_correct(x_hat[:, i], y, H, R, P_hat[:, :, i]) 162 | 163 | # %% 164 | # PLOT THE SIMULATION OUTPUTS 165 | 166 | # Change some plot settings (optional) 167 | plt.rc("text", usetex=True) 168 | plt.rc("text.latex", preamble=r"\usepackage{cmbright,amsmath,bm}") 169 | plt.rc("savefig", format="pdf") 170 | plt.rc("savefig", bbox="tight") 171 | 172 | # Find the scaling factor for plotting covariance bounds 173 | ALPHA = 0.01 174 | s1 = chi2.isf(ALPHA, 1) 175 | s2 = chi2.isf(ALPHA, 2) 176 | 177 | # Plot the errors and covariance bounds for the vehicle state 178 | sigma = np.zeros((2, N)) 179 | fig1 = plt.figure(1) 180 | ax1 = plt.subplot(211) 181 | plt.plot(t, x_veh[0, :] - x_hat[0, :], "C0", label="Error") 182 | sigma[0, :] = np.sqrt(s1 * P_hat[0, 0, :]) 183 | plt.fill_between( 184 | t, 185 | -sigma[0, :], 186 | sigma[0, :], 187 | color="C1", 188 | alpha=0.2, 189 | label=str(100 * (1 - ALPHA)) + "% confidence", 190 | ) 191 | plt.ylabel(r"$e_1$ [m]") 192 | plt.setp(ax1, xticklabels=[]) 193 | ax1.set_ylim([-2.5, 2.5]) 194 | plt.legend() 195 | plt.grid(color="0.95") 196 | ax2 = plt.subplot(212) 197 | plt.plot(t, x_veh[1, :] - x_hat[1, :], "C0") 198 | sigma[1, :] = np.sqrt(s1 * P_hat[1, 1, :]) 199 | plt.fill_between(t, -sigma[1, :], sigma[1, :], color="C1", alpha=0.2) 200 | plt.ylabel(r"$e_2$ [m]") 201 | plt.xlabel(r"$t$ [s]") 202 | ax2.set_ylim([-2.5, 2.5]) 203 | plt.grid(color="0.95") 204 | 205 | # Plot the actual and estimate vehicle poses 206 | fig2, ax = plt.subplots() 207 | plt.plot(x_veh[0, :], x_veh[1, :], "C0", label="Actual") 208 | plt.plot(x_hat[0, :], x_hat[1, :], "C1", label="Estimated") 209 | 210 | # Plot each feature's estimated location and covariance 211 | for i in range(1, m): 212 | plt.plot(x_hat[2 * i, N - 1], x_hat[2 * i + 1, N - 1], "C1*") 213 | W, V = np.linalg.eig(P_hat[2 * i : 2 * i + 2, 2 * i : 2 * i + 2, N - 1]) 214 | j_max = np.argmax(W) 215 | j_min = np.argmin(W) 216 | ell = patches.Ellipse( 217 | (x_hat[2 * i, N - 1], x_hat[2 * i + 1, N - 1]), 218 | 2.0 * np.sqrt(s2 * W[j_max]), 219 | 2.0 * np.sqrt(s2 * W[j_min]), 220 | angle=np.arctan2(V[j_max, 1], V[j_max, 0]), 221 | alpha=0.1, 222 | color="C1", 223 | ) 224 | ax.add_artist(ell) 225 | 226 | # Plot the actual feature locations 227 | plt.plot(f_map[0, :], f_map[1, :], "C0*", label="Map feature") 228 | plt.xlabel(r"$x_1$ [m]") 229 | plt.ylabel(r"$x_2$ [m]") 230 | plt.legend() 231 | plt.grid(color="0.95") 232 | plt.axis("equal") 233 | 234 | # Show the plots to the screen 235 | plt.show() 236 | --------------------------------------------------------------------------------