diff --git a/pynumdiff/utils/_pi_cruise_control.py b/pynumdiff/utils/_pi_cruise_control.py deleted file mode 100644 index 794aea8..0000000 --- a/pynumdiff/utils/_pi_cruise_control.py +++ /dev/null @@ -1,65 +0,0 @@ -import numpy as _np - -def run(timeseries_length=4, dt=0.01): - """ - Simulate proportional integral control of a car attempting to maintain constant velocity while going up and down hills. - This function is used for testing differentiation methods. - - This is a linear interpretation of something similar to what is described in Astrom and Murray 2008 Chapter 3. - - :param timeseries_length: number of seconds to simulate - :type timeseries_length: float - - :param dt: timestep in seconds - :type dt: float - - :return: a tuple consisting of arrays of size [N, M], where M is the number of time steps.: - - state_vals: state of the car, i.e. position and velocity as a function of time - - disturbances: disturbances, ie. hills, that the car is subjected to - - controls: control inputs applied by the car - :rtype: tuple -> (np.array, np.array, np.array) - """ - - t = _np.arange(0, timeseries_length+dt, dt) - - # disturbance - hills = _np.sin(2*_np.pi*t) + 0.3*_np.sin(4*2*_np.pi*t + 0.5) + 1.2*_np.sin(1.7*2*_np.pi*t + 0.5) - hills = 0.01*hills - - # parameters - mg = 10000 # mass*gravity - fr = 0.9 # friction - ki = 5/0.01*dt # integral control - kp = 25/0.01*dt # proportional control - vd = 0.5 # desired velocity - - A = _np.array([[1, dt, 0, 0, 0], - [0, 1, dt, 0, 0], - [0, -fr, 0, -mg, ki], - [0, 0, 0, 0, 0], - [0, 0, 0, 0, 1]]) - - B = _np.array([[0, 0], - [0, 0], - [0, kp], - [1, 0], - [0, 1]]) - - x0 = _np.array([0, 0, 0, hills[0], 0]).reshape(A.shape[0], 1) - - # run simulation - xs = [x0] - us = [_np.array([0, 0]).reshape([2,1])] - for i in range(1, len(hills)-1): - u = _np.array([hills[i], vd - xs[-1][1,0]]).reshape([2,1]) - xnew = A@xs[-1] + B@u - xs.append(xnew) - us.append(u) - - xs = _np.hstack(xs) - us = _np.hstack(us) - - if len(hills.shape) == 1: - hills = _np.reshape(hills, [1, len(hills)]) - - return xs, hills, us diff --git a/pynumdiff/utils/old_pi_cruise_control.py b/pynumdiff/utils/old_pi_cruise_control.py index 2cf2773..695182a 100644 --- a/pynumdiff/utils/old_pi_cruise_control.py +++ b/pynumdiff/utils/old_pi_cruise_control.py @@ -1,7 +1,4 @@ -""" -Simulation and Control of a cruise ? -""" - +"""Simulation and Control of cruise control, in all its nonlinear glory""" import numpy as np from pynumdiff.utils import utility @@ -21,18 +18,12 @@ def triangle(iterations, dt): - """ - Create sawtooth pattern of hills for the car to drive over. - - :param iterations: number of time points in time series - :type iterations: int + """Create sawtooth pattern of hills for the car to drive over. - :param dt: time step in seconds - :type dt: float - - :return: time series of hills (angle of road as a function of time) - :rtype: np.matrix + :param int iterations: number of time points in time series + :param float dt: time step in seconds + :return: (np.array) -- time series of hills (angle of road as a function of time) """ t = np.arange(0, iterations*dt, dt) continuous_x = np.sin(0.02*t*np.sqrt(t)) @@ -59,34 +50,25 @@ def triangle(iterations, dt): reversal_vals = np.array(reversal_vals)[idx] reversal_ts = t[reversal_idxs] - x = np.interp(t, reversal_ts, reversal_vals) - x = np.matrix(x) - - return x + return np.interp(t, reversal_ts, reversal_vals) def effective_wheel_radius(v=20): - """ - Allow effective wheel radius to be a function of velocity. + """Allow effective wheel radius to be a function of velocity. - :param v: ignored for now - :type v: + :param float v: velocity - :return: effective wheel radius, constant for now - :rtype: float + :return: (float) -- effective wheel radius, constant for now """ return v def torque(omega): - """ - Convert throttle input to Torque. See Astrom and Murray 2008 Chapter 3. + """Convert throttle input to Torque. See Astrom and Murray 2008 Chapter 3. - :param omega: throttle - :type omega: float + :param float omega: throttle - :return: motor torque - :rtype: float + :return: **torque** (float) -- motor torque """ omega_m = parameters['omega_m'] t_m = parameters['T_m'] @@ -96,25 +78,17 @@ def torque(omega): # pylint: disable-msg=too-many-locals def step_forward(state_vals, disturbances, desired_v, dt): - """ - One-step Euler integrator that takes the current state, disturbances, and desired velocity and calculates the subsequent state. - - :param state_vals: current state [position, velocity, road_angle] - :type state_vals: np.matrix - - :param disturbances: current hill angle - :type disturbances: np.matrix - - :param desired_velocity: current desired velocity - :type desired_velocity: np.matrix + """One-step Euler integrator that takes the current state, disturbances, and + desired velocity and calculates the subsequent state. - :param dt: time step (seconds) - :type dt: float + :param np.array state_vals: current state [position, velocity, road_angle] + :param np.array disturbances: current hill angle + :param np.array desired_v: current desired velocity + :param float dt: time step (seconds) - :return: a tuple consisting of: - - new_state: new state - - u: control inputs - :rtype: tuple -> (np.matrix, np.matrix) + :return: tuple[np.array, np.array] of\n + - **new_state** -- new state + - **u** -- control inputs """ p = state_vals[0, -1] v = state_vals[1, -1] @@ -125,9 +99,9 @@ def step_forward(state_vals, disturbances, desired_v, dt): rho = parameters['rho'] Cd = parameters['Cd'] A = parameters['A'] - v_r = desired_v[0, -1] + v_r = desired_v[-1] alpha_n = effective_wheel_radius(v) - z = np.sum(desired_v[0, :] - state_vals[1, :])*dt + z = np.sum(desired_v - state_vals[1, :])*dt k_p = parameters['k_p'] k_i = parameters['k_i'] u = k_p*(v_r-v) + k_i*z @@ -141,87 +115,63 @@ def step_forward(state_vals, disturbances, desired_v, dt): # driving force Fd = alpha_n*u*torque(alpha_n*v) vdot = 1/m*(Fd - (Fr + Fa + Fg)) - new_state = np.matrix([[p + dt*v], [v + vdot*dt], [theta]]) - return new_state, np.matrix(u) + new_state = np.array([[p + dt*v], [v + vdot*dt], [theta]]) + return new_state, u # disturbance def hills(iterations, dt, factor): - """ - Wrapper for creating a hill profile for the car to drive over that has an appropriate length and magnitude - - :param iterations: number of time points to simulate - :type iterations: int - - :param dt: timestep of simulation in seconds - :type dt: float + """Wrapper for creating a hill profile for the car to drive over that has an appropriate length and magnitude - :param factor: determines magnitude of the hills - :type factor: int + :param int iterations: number of time points to simulate + :param float dt: timestep of simulation in seconds + :param int factor: determines magnitude of the hills - - :return: hills, the output of the triangle function, a [1,M] matrix where M is the number of time points simulated - :rtype: np.matrix + :return: **hills** (np.array) -- the output of the triangle function, a shape (M,) array where M is the number + of time points simulated """ return triangle(iterations, dt)*0.3/factor # desired velocity def desired_velocity(n, factor): - """ - Wrapper for defining the desired velocity as a matrix with size [1, M], where M is the number of time points to simulate - See function "run" for how this function gets used. - - :param n: number of time points to simulate - :type n: int + """Wrapper for defining the desired velocity as an array of shape (M,), where M is the number of time points + to simulate. - :param factor: factor that determines the magnitude of the desired velocity - :type factor: float + :param int n: number of time points to simulate + :param float factor: factor that determines the magnitude of the desired velocity - :return: desired velocity as function of time, a [1,M] matrix, M is the number of time points to simulate - :rtype: np.matrix + :return: (np.array) -- desired velocity as function of time """ - return np.matrix([2/factor]*n) + return np.array([2/factor]*n) -def run(timeseries_length=4, dt=0.01): - """ - Simulate proportional integral control of a car attempting to maintain constant velocity while going up and down hills. - This function is used for testing differentiation methods. - - See Astrom and Murray 2008 Chapter 3. - - :param timeseries_length: number of seconds to simulate - :type timeseries_length: float +def run(duration=4, dt=0.01): + """Simulate proportional integral control of a car attempting to maintain constant velocity while going up + and down hills. This function can be used for testing differentiation methods. See Astrom and Murray 2008 + Chapter 3. - :param dt: timestep in seconds - :type dt: float + :param float duration: number of seconds to simulate + :param float dt: timestep in seconds - :return: a tuple consisting of arrays of size [N, M], where M is the number of time steps.: - - state_vals: state of the car, i.e. position and velocity as a function of time - - disturbances: disturbances, ie. hills, that the car is subjected to - - controls: control inputs applied by the car - :rtype: tuple -> (np.array, np.array, np.array) + :return: tuple[np.array, np.array, np.array] of\n + - **state_vals** -- state of the car, i.e. position and velocity as a function of time, shape (N,M) + - **disturbances** -- disturbances from hills that the car is subjected to, shape (M,) + - **controls** -- control inputs applied by the car, shape (M+1,) """ - t = np.arange(0, timeseries_length, dt) + t = np.arange(0, duration, dt) iterations = len(t) - # hills - disturbances = np.matrix(np.zeros([3, iterations+1])) - h = hills(iterations+1, dt, factor=0.5*timeseries_length/2) - disturbances[2, :] = h[:, 0:disturbances.shape[1]] - - # controls - controls = np.matrix([[0]]) - - # initial condition - state_vals = np.matrix([[0], [0], [0]]) + disturbances = np.array(np.zeros([3, iterations+1])) # disturbance from hills + h = hills(iterations+1, dt, factor=0.5*duration/2) + disturbances[2, :] = h[0:disturbances.shape[1]] - # desired vel - v_r = desired_velocity(iterations, factor=0.5*iterations*dt/2) + controls = np.array([0]) + state_vals = np.array([[0], [0], [0]]) # initial condition + v_r = desired_velocity(iterations, factor=0.5*iterations*dt/2) # desired, reference velocity for i in range(1, iterations+1): - new_state, u = step_forward(state_vals, disturbances[:, 0:i], v_r[:, 0:i], dt) + new_state, u = step_forward(state_vals, disturbances[:, 0:i], v_r[0:i], dt) state_vals = np.hstack((state_vals, new_state)) controls = np.hstack((controls, u)) diff --git a/pynumdiff/utils/simulate.py b/pynumdiff/utils/simulate.py index cdb45ce..c533062 100644 --- a/pynumdiff/utils/simulate.py +++ b/pynumdiff/utils/simulate.py @@ -6,7 +6,6 @@ # local imports from pynumdiff.utils.utility import peakdet -from pynumdiff.utils import _pi_cruise_control from pynumdiff.finite_difference import first_order as _finite_difference @@ -235,7 +234,7 @@ def pi_control(duration=4, noise_type='normal', noise_parameters=(0, 0.5), """ t = np.arange(0, duration, dt) - actual_vals, extra_measurements, controls = _pi_cruise_control.run(duration, dt) + actual_vals, extra_measurements, controls = _pi_cruise_control(duration, dt) x = np.ravel(actual_vals[0, :]) dxdt = np.ravel(actual_vals[1, :]) @@ -252,6 +251,66 @@ def pi_control(duration=4, noise_type='normal', noise_parameters=(0, 0.5), [np.array(extra_measurements), np.array(controls)] +def _pi_cruise_control(duration=4, dt=0.01): + """Simulate proportional integral control of a car attempting to maintain constant velocity while going + up and down hills. This function is used for testing differentiation methods. This is a linear + interpretation of something similar to what is described in Astrom and Murray 2008 Chapter 3. + + :param float duration: number of seconds to simulate + :param dt: timestep in seconds + + :return: tuple[np.array, np.array, np.array] of arrays of shape (N, M), where M is the + number of time steps\n + - **state_vals** -- state of the car, i.e. position and velocity as a function of time + - **disturbances** -- disturbances from hills that the car is subjected to + - **controls** -- control inputs applied by the car + """ + _np = np # for compatibility with original code + t = _np.arange(0, duration+dt, dt) + + # disturbance + hills = _np.sin(2*_np.pi*t) + 0.3*_np.sin(4*2*_np.pi*t + 0.5) + 1.2*_np.sin(1.7*2*_np.pi*t + 0.5) + hills = 0.01*hills + + # parameters + mg = 10000 # mass*gravity + fr = 0.9 # friction + ki = 5/0.01*dt # integral control + kp = 25/0.01*dt # proportional control + vd = 0.5 # desired velocity + + A = _np.array([[1, dt, 0, 0, 0], + [0, 1, dt, 0, 0], + [0, -fr, 0, -mg, ki], + [0, 0, 0, 0, 0], + [0, 0, 0, 0, 1]]) + + B = _np.array([[0, 0], + [0, 0], + [0, kp], + [1, 0], + [0, 1]]) + + x0 = _np.array([0, 0, 0, hills[0], 0]).reshape(A.shape[0], 1) + + # run simulation + xs = [x0] + us = [_np.array([0, 0]).reshape([2,1])] + for i in range(1, len(hills)-1): + u = _np.array([hills[i], vd - xs[-1][1,0]]).reshape([2,1]) + xnew = A@xs[-1] + B@u + xs.append(xnew) + us.append(u) + + xs = _np.hstack(xs) + us = _np.hstack(us) + + if len(hills.shape) == 1: + hills = _np.reshape(hills, [1, len(hills)]) + + return xs, hills, us + + def lorenz_x(duration=4, noise_type='normal', noise_parameters=(0, 0.5), random_seed=1, dt=0.01, simdt=0.0001): """Create toy example of x component from a lorenz attractor @@ -271,7 +330,7 @@ def lorenz_x(duration=4, noise_type='normal', noise_parameters=(0, 0.5), - **vel** -- a true derivative information of the time series; - None -- dummy output """ - noisy_measurements, actual_vals, _ = lorenz_xyz(duration, noise_type, noise_parameters, + noisy_measurements, actual_vals, _ = _lorenz_xyz(duration, noise_type, noise_parameters, random_seed, dt, simdt) noisy_pos = np.ravel(noisy_measurements[0, :]) @@ -281,7 +340,7 @@ def lorenz_x(duration=4, noise_type='normal', noise_parameters=(0, 0.5), return noisy_pos, pos, vel, None -def lorenz_xyz(duration=4, noise_type='normal', noise_parameters=(0, 0.5), random_seed=1, +def _lorenz_xyz(duration=4, noise_type='normal', noise_parameters=(0, 0.5), random_seed=1, dt=0.01, simdt=0.0001, x0=(5, 1, 3), normalize=True): """Simulation of Lorenz system with Eular method @@ -296,7 +355,7 @@ def lorenz_xyz(duration=4, noise_type='normal', noise_parameters=(0, 0.5), rando :param tuple x0: a tuple of initial state of the Lorenz system :param bool normalize: whether to roughly normalize the time series - :return: tuple[np.array, np.array, np.array, None] of\n + :return: tuple[np.array, np.array, None] of\n - **noisy_measurements** -- noisy time series from Lorenz system; - **actual_vals** -- noise-free time series from Lorenz system; - None -- dummy output @@ -354,7 +413,7 @@ def lorenz_xyz(duration=4, noise_type='normal', noise_parameters=(0, 0.5), rando return noisy_measurements[:, idx], actual_vals[:, idx], None -def rk4_lorenz_xyz(duration=4, noise_type='normal', noise_parameters=(0, 0.5), +def _rk4_lorenz_xyz(duration=4, noise_type='normal', noise_parameters=(0, 0.5), random_seed=1, dt=0.01, normalize=True): """ :param float duration: governs the length of the series, duration/dt @@ -389,7 +448,7 @@ def dxyz_dt(xyz): zdot = x*y - beta*z return [xdot, ydot, zdot] - ts = np.linspace(0, timeseries_length, timeseries_length/dt) + ts = np.linspace(0, duration, int(duration/dt)) xyz_0 = [5, 1, 3] vals, _ = odeint(dxyz_dt, xyz_0, ts, full_output=True)