EE16B:Homework9

In[2]: #Run these to import all of the necessary files

import numpy as np
import scipy as sp
import scipy.io as spio
import scipy.signal as sig
import scipy.io.wavfile as wf
import matplotlib as plt
from pylab import *
import os
import re

# Plots graphs in the browser window

%matplotlib inline

Problem1:SimulatingStateSpaceModels
Part1b:StableSystems
Simulatethegivensystemasstatedintheproblem.
In[3]: # YOUR CODE HERE #
A = np.matrix('0.7 0.2; 0.8 0.35')
print(np.linalg.eigvals(A))
def fx(k):
x0 = []
x1 = []
for i in k:
x = (A**i)*np.matrix('1; 0.2')
x0.append(np.matrix.item(x[0]))
x1.append(np.matrix.item(x[1]))
return x0, x1
def x_0(k):
return fx(k)[0]
def x_1(k):
return fx(k)[1]

k = np.arange(21)
plt.figure()
plt.plot(k,x_0(k))
plt.plot(k,x_1(k))
[ 0.96160623 0.08839377]
Out[3]: [<matplotlib.lines.Line2D at 0x7f2a6b275240>]

Part1d:UnstableSystems
Simulatethegivensystemasstatedintheproblem.

In[4]: # YOUR CODE HERE #

A_p = np.matrix('0 1; -1 0')
def f_px(k):
x0 = []
x1 = []
for i in k:
x = (A_p**i)*np.matrix('1; 0.2')
x0.append(np.matrix.item(x[0]))
x1.append(np.matrix.item(x[1]))
return x0, x1
def xp_0(k):
return f_px(k)[0]
def xp_1(k):
return f_px(k)[1]
k = np.arange(21)
plt.figure()
plt.plot(k,xp_0(k))
plt.plot(k,xp_1(k))
Out[4]: [<matplotlib.lines.Line2D at 0x7f2a6b285c18>]

Problem2:OpenLoopUAVControl
Basedontheparametersgivenfortheproblem,writecodebelowtosimulatetheUAVinresponsetoa(noisy)stepinputof4.9N.

In[5]: def step_uav(A,B,u_eff,x0):

''' Steps the state of the UAV by one sample.
Inputs:
A: A 2x2 numpy array representing the update matrix A of the UAV state.
B: A 2x1 numpy array representing the update matrix B of the UAV state.
u_eff: A value representing the net force on the UAV (including gravity) for this
timestep.
x0: A 2x1 numpy array representing the initial state of the UAV. The first elemen
t
is position y, and the second elemement is velocity v.
Returns:
x1: A 2x1 numpy array representing the updated state of the UAV. The first elemen
t
is position y, and the second elemement is velocity v.
'''
# YOUR CODE HERE #
return A*x0 + B*u_eff
def sim_uav(A,B,u_eff,x0,n):
''' Iterates the state of the UAV and measures its position and velocity.
Inputs:
A: A 2x2 numpy array representing the update matrix A of the UAV state.
B: A 2x1 numpy array representing the update matrix B of the UAV state.
u_eff: A numpy array representing the net force on the UAV (including gravity)
for each timestep. u_eff must be at least length n.
x0: A 2x1 numpy array representing the initial state of the UAV. The first elemen
t
is position y, and the second elemement is velocity v.
n: The desired number of iterations.
Returns:
x: A 2xn numpy array representing time-series values of the UAV states.
'''
# YOUR CODE HERE #
init = step_uav(A,B,u_eff[0], x0)
init_l = np.ndarray.flatten(np.array(init))
x = [init_l]
curr_val = init
for i in range(1, n-1):
next_val = step_uav(A,B,u_eff[i], curr_val)
next_l = np.ndarray.flatten(np.array(next_val))
x.append(next_l)
curr_val = next_val
return np.matrix(x).T

NowcallyourfunctionsandplottheresultstosimulatethebehavioroftheUAV.

In[6]: iter = 10000

init = np.matrix([[1],[0]])
city
u = np.full(iter,4.9) + np.random.randn(iter)
u_eff = u - 4.9
Ts = 0.001

# Simulate for 10 seconds

# Inital conditions: 1m height, 0m/s velo
# Input: 4.9N, plus some unwanted noise
# Subtract gravity

# Define the A and B matrices

# YOUR CODE HERE #
A = np.matrix('1 0.001; 0 1')
B = np.matrix('0.0000005; 0.001')
# Call your function
# YOUR CODE HERE #
x = np.zeros((2,iter+1))
x[0][0] = 1
x[1][0] = 0
sim = sim_uav(A,B,u_eff, init, iter+1)
x[0,1:] = sim[0,:]
x[1,1:] = sim[1,:]
# # Plot the result
t = np.linspace(0.0,iter*Ts,num=iter+1)
plt.figure(1)
plt.subplot(211)
plt.plot(t,x[0,:])
plt.xlabel('Time (s)'); plt.ylabel('Height (m)')
plt.subplot(212)
plt.plot(t,x[1,:])
plt.xlabel('Time (s)'); plt.ylabel('Velocity (m/s)')
Out[6]: <matplotlib.text.Text at 0x7f2a6b0f7208>

HowwelldoestheUAVmaintainitsinitialposition?
Notverywell.

Problem3:ClosedLoopUAVControl
Basedontheparametersgivenfortheproblem,writecodebelowtosimulatetheUAVinclosedloopconfiguration.

In[7]: def step_uav_CL(A_CL,B_CL,u,x0):

''' Steps the state of the UAV by one sample.
Inputs:
A_CL: A 2x2 numpy array representing the update matrix A of the UAV state.
B_CL: A 2x1 numpy array representing the update matrix B of the UAV state.
u: A 2x1 numpy array representing the input for this timestep. The first element
is
desired velocity v_d, and the second is the uncontrolled force mg (plus any no
ise).
x0: A 2x1 numpy array representing the initial state of the UAV. The first elemen
t
is position y, and the second elemement is velocity v.
Returns:
x1: A 2x1 numpy array representing the updated state of the UAV. The first elemen
t
is position y, and the second elemement is velocity v.
'''
# YOUR CODE HERE #
return A_CL*x0 + B_CL*u
def sim_uav_CL(A_CL,B_CL,u,x0,n):
''' Iterates the state of the UAV and measures its position and velocity.
Inputs:
A_CL: A 2x2 numpy array representing the update matrix A of the UAV state.
B_CL: A 2x1 numpy array representing the update matrix B of the UAV state.
u: A 2-row numpy array representing the inputs for each timestep. u must be at le
ast length n.
x0: A 2x1 numpy array representing the initial state of the UAV. The first elemen
t
is position y, and the second elemement is velocity v.
n: The desired number of iterations.
Returns:
x: A 2xn numpy array representing time-series values of the UAV states.
'''
# YOUR CODE HERE #
init = step_uav_CL(A_CL,B_CL,u[0], x0)
init_l = np.ndarray.flatten(np.array(init)).tolist()
x = [init_l]
curr_val = init
for i in range(1, n-1):
next_val = step_uav_CL(A_CL,B_CL,u[i], curr_val)
next_l = np.ndarray.flatten(np.array(next_val)).tolist()
x.append(next_l)
curr_val = next_val
return np.matrix(x).T

NowcallyourfunctionsandplottheresultstosimulatethebehavioroftheUAV.

In[10]: phase1 = 5000; phase2 = 200; phase3 = 4800

# Define step function for input vd
vd = np.concatenate((np.zeros(phase1),np.full(phase2,0.2),np.zeros(phase3)))
init = np.array([[1],[0]])
ity
iter = phase1+phase2+phase3

# Inital conditions: 1m height, 0m/s veloc

# Simulate for 10 seconds

mg = np.full(iter,-4.9) + np.random.randn(iter) # Uncontrolled force: -4.9N, plus some unw

anted noise
# u = np.concatenate((vd,mg))
# The input vector u is made up of vd an
d mg
u = np.array([[[x[0]], [x[1]]] for x in zip(vd, mg)])
Ts = 0.001

# Define the A_CL and B_CL matrices; use h = 200 N/(m/s)

# YOUR CODE HERE #
h = 200
m = 0.5
A_CL = np.matrix(np.array([[1, Ts-h*(Ts**2)/(2*m)], [0, 1-h*Ts/m]]))
B_CL = np.matrix(np.array([[(Ts**2)/(2*m)], [Ts/m]]*np.array([h, 1])))
# Call your function
# YOUR CODE HERE #
x = np.zeros((2,iter+1))
x[0][0] = 1
x[1][0] = 0
sim = sim_uav_CL(A_CL,B_CL,u, init, iter+1)
x[0,1:] = sim[0,:]
x[1,1:] = sim[1,:]
#Plot the result
t = np.linspace(0.0,iter*Ts,num=iter+1)
plt.figure(1,figsize=(6,6))
plt.subplot(311)
plt.plot(t[0:iter],vd)
plt.xlabel('Time (s)'); plt.ylabel('Desired Velocity (m/s)')
plt.subplot(312)
plt.plot(t,x[0,:])
plt.xlabel('Time (s)'); plt.ylabel('Height (m)')
plt.subplot(313)
plt.plot(t,x[1,:])
plt.xlabel('Time (s)'); plt.ylabel('Velocity (m/s)')
Out[10]: <matplotlib.text.Text at 0x7f2a6ae60588>

Nowrepeattheexperiment,butuseadifferentvalueofh todefinenewstatematrices.

In[13]: # Define the new A_CL and B_CL matrices; use h = 800 N/(m/s)
# YOUR CODE HERE #
h = 800
m = 0.5
A_CL = np.matrix(np.array([[1, Ts-h*(Ts**2)/(2*m)], [0, 1-h*Ts/m]]))
B_CL = np.matrix(np.array([[(Ts**2)/(2*m)], [Ts/m]]*np.array([h, 1])))

# Call your function

# YOUR CODE HERE #
x = np.zeros((2,iter+1))
x = np.zeros((2,iter+1))
x[0][0] = 1
x[1][0] = 0
sim = sim_uav_CL(A_CL,B_CL,u, init, iter+1)
x[0,1:] = sim[0,:]
x[1,1:] = sim[1,:]
# Plot the result
t = np.linspace(0.0,iter*Ts,num=iter+1)
plt.figure(1,figsize=(6,6))
plt.subplot(311)
plt.plot(t[0:iter],vd)
plt.xlabel('Time (s)'); plt.ylabel('Desired Velocity (m/s)')
plt.subplot(312)
plt.plot(t,x[0,:])
plt.xlabel('Time (s)'); plt.ylabel('Height (m)')
plt.subplot(313)
plt.plot(t,x[1,:])
plt.xlabel('Time (s)'); plt.ylabel('Velocity (m/s)')
Out[13]: <matplotlib.text.Text at 0x7f2a6aa10c88>

Forthefirstfiveseconds,howwelldoestheUAVrespondtothenoiseinthesystem?Doesitremainatthedesiredheight?
Itovershootsthedesiredheight.
Afterthestepchangeinvd ,howdoestheUAVrespondtothechangeindesiredvelocity?
Itrespondsbyflyingup.
HowdoestheUAVbehaviorchangewithdifferentvaluesofh ?
Lowervaluesofhaffectstheheightless.