def _plot(file = '/home/anand/btpdata/case_7.npz'):
_file = load(file)
t = _file['t']
x = _file['x']
xd = _file['xd']
u = _file['u']
subplot(311)
plot(t, x[0])
plot(t, x[1])
plot(t, x[2])
grid()
legend(['Vx','Vy','Vz'])
subplot(312)
plot(t, x[9])
plot(t, x[10])
plot(t, x[11])
plot(t, arctan(x[2]/x[0]))
grid()
legend(['Phi','Theta','Psi','alpha'])
subplot(313)
plot(x[3],-x[5])
grid()
y = zeros_like(x[3])
for i in range(len(x[3])):
y[i] = stepprofile(x[3][i])
plot(x[3],y)
show()
figure()
plot(u[0])
plot(u[3])
show()
def control(self,t,x):
u = zeros([7])
self.h_req = stepprofile(x[3])
thr = self.heightcontrol(-x[5])
u[3] = max(0,min(1,thr))
print u[3]
return u
