def update_plot(i):
global image, image2, particles, galaxy2, count
t = i * dt
particles_x = np.array([])
particles_y = np.array([])
for i, particle in enumerate(particles):
particles[i] = rk4(t, particle, F, dt)
particles_x = np.append(particles_x, [particle[0]])
particles_y = np.append(particles_y, [particle[2]])
galaxy2 = rk4(t, galaxy2, F2, dt)
if image is not None:
image.remove()
if image2 is not None:
image2.remove()
image = plot.scatter(particles_x, particles_y)
image2 = plot.scatter(galaxy2[0], galaxy2[2], s=200, c='r')
count += 1
if count == N - 1:
count_d = 0
count_h = 0
for i, particle in enumerate(particles):
(x, vx, y, vy) = particle
r = np.sqrt(x**2 + y**2)
if r < Rd:
count_d += 1
if r < Rh:
count_h += 1
print('Stars inside disk: %d' % count_d)
print('Stars inside halo: %d' % count_h)
def rka(x,t,tau,err,derivsRK,param):
"""Adaptive Runge-Kutta routine
Inputs
x Current value of the dependent variable
t Independent variable (usually time)
tau Step size (usually time step)
err Desired fractional local truncation error
derivsRK Right hand side of the ODE; derivsRK is the
name of the function which returns dx/dt
Calling format derivsRK (x,t,param).
param Extra parameters passed to derivsRK
Outputs
xSmall New value of the dependent variable
t New value of the independent variable
tau Suggested step size for next call to rka
"""
#* Set initial variables
tSave, xSave = t, x # Save initial values
safe1, safe2 = 0.9, 4.0 # Safety factors
eps = 1.e-15
#* Loop over maximum number of attempts to satisfy error bound
xTemp = np.empty(len(x))
xSmall = np.empty(len(x)); xBig = np.empty(len(x))
maxTry = 100
for iTry in range(maxTry):
#* Take the two small time steps
half_tau = 0.5 * tau
xTemp = rk4(xSave,tSave,half_tau,derivsRK,param)
t = tSave + half_tau
xSmall = rk4(xTemp,t,half_tau,derivsRK,param)
#* Take the single big time step
t = tSave + tau
xBig = rk4(xSave,tSave,tau,derivsRK,param)
#* Compute the estimated truncation error
scale = err * (abs(xSmall) + abs(xBig))/2.
xDiff = xSmall - xBig
errorRatio = np.max( np.absolute(xDiff) / (scale + eps) )
#* Estimate new tau value (including safety factors)
tau_old = tau
tau = safe1*tau_old*errorRatio**(-0.20)
tau = max(tau, tau_old/safe2)
tau = min(tau, safe2*tau_old)
#* If error is acceptable, return computed values
if errorRatio < 1 :
return np.array([xSmall, t, tau])
#* Issue error message if error bound never satisfied
print 'ERROR: Adaptive Runge-Kutta routine failed'
return np.array([xSmall, t, tau])
def calc_for_plot(t_max, h, x0, y0, b, w0, f0, w):
t = np.arange(0, t_max, h)
# h = 0.0001
# t_max = 5
#
# b =1
# w0 = 5
#
# x0 = 1
# y0 = 1
#
# f0 =1
# w = 4.795
def f_out(t, x, y):
return f0*np.cos(w*t) - 2*b*y - w0**2*x
def f_in(x, y):
return -2*b*y - w0*w0*x
def f(t, x):
return t
x = np.zeros(len(t), dtype = float)
y = np.zeros(len(t), dtype = float)
x[0] = x0
y[0] = y0
# phi = atan(-y0/(w0*x0))
#A0 = x0/(cos(atan(phi)))
#x_real = A0*np.exp(-b*t)*np.cos(t*np.sqrt(w0*w0-b*b) + phi)
for i in range(1, len(t)):
#y[i] = rk4.rk4(x[i-1], y[i-1], f_in, h)
y[i] = rk4.rk4(x[i-1], y[i-1], lambda x, y: f_out(t[i-1], x, y), h)
x[i] = rk4.rk4(y[i-1], x[i-1], f, h)
#print(np.linalg.norm(x_real - x), len(t), len(x))
# plt.subplot(311)
# plt.plot(t, x_real)
# plt.xlabel(r'$t$')
# plt.ylabel(r'$x$')
# plt.grid(True)
omega = np.arange(0, 50, h)
if f0 == 0 :
AChH = omega*0
else:
AChH = f0/np.sqrt((w0**2-omega**2)**2+4*(b*omega)**2)
#A = f0/np.sqrt((w0**2- w**2)**2+4*(b*w)**2)
return (t, x), (x,y), (omega, AChH)
def solve(l, Xl, Yl, px, py):
Z = np.zeros([l, l])
#phase = np.zeros([l, l])
ln = line()
line_points = ln.points()
XX = [line_points[i][0][0] for i in range(0, len(line_points))]
YY = [line_points[i][0][1] for i in range(0, len(line_points))]
LL = [line_points[i][1] for i in range(0, len(line_points))]
# Plot dell'antenna
plt.plot(YY, XX)
intg = rk4(1 + 0 * 1j, line_points)
for x in Xl:
for y in Yl:
intg.compute(x / 2, y / 2)
Ax = intg.A_old[0]
Ay = intg.A_old[1] # NULLO
Z[int(x + px)][int(y + py)] = abs(Ax) #cm.phase(Ax))
return Z
def get_data_hybrid1(data_size, remove_transient=False, normalized=False):
shift_markers = []
if remove_transient:
start = int(data_size / 6)
else:
start = 1
[t, data] = get_lorenz(n=data_size, tmax=500)
series = normalize(data[start:, 0], normalized)
end_point = data[-1, :2]
plt.figure()
plt.plot(data[start:, 0], data[start:, 1], '.')
plt.savefig('hybrid1_lorenz1.png')
np.savetxt('hybrid1_lorenz1.txt', data[start:, 0])
shift_markers.append(len(series))
A_list = np.arange(0.86, 0.87, 0.02)
for A in A_list:
print("A=", A)
changeA(A)
[data1, t] = rk4(derivative, end_point, 0, 0.005, data_size)
plt.figure()
plt.plot(data1[start:, 0], data1[start:, 1], '.')
plt.savefig('hybrid1_pendulum.png')
np.savetxt('hybrid1_pendulum.txt', data[:, 0])
series1 = normalize(data1[start:, 0], normalized)
series = np.concatenate((series, series1), axis=0)
shift_markers.append(len(series))
end_point = data1[-1, :]
[t, data] = get_lorenz(n=data_size,
tmax=500,
init=[end_point[0], end_point[1], 0])
series1 = normalize(data[start:, 0], normalized)
series = np.concatenate((series, series1), axis=0)
plt.figure()
plt.plot(data[start:, 0], data[start:, 1], '.')
plt.savefig('hybrid1_lorenz2.png')
np.savetxt('hybrid1_lorenz2.txt', data[start:, 0])
end_point = data1[-1, :]
shift_markers.append(len(series))
[t, data] = get_rossler(n=data_size,
tmax=500,
start=[end_point[0], end_point[1], 0])
plt.figure()
plt.plot(data[start:, 0], data[start:, 1], '.')
plt.savefig('hybrid1_rossler_2d.png')
np.savetxt('hybrid1_rossler.txt', data[start:, 0])
series1 = normalize(data[start:, 0], normalized)
series = np.concatenate((series, series1), axis=0)
return [shift_markers, series]
def plot(h = 0.1,start=(0.1,0),n=10,color='b'):
pts = [start]
for i in range(int(2*n*math.pi/h)+1):
pts.append(rk4.rk4(h,pts[-1],f))
plt.scatter([r*math.cos(theta) for (r,theta) in pts],
[r*math.sin(theta) for (r,theta) in pts],
c=color,
s=1)
def step(self, t, dt):
"""
Advance the system one step in time using fourth-order Runge-Kutta.
"""
xvec = [self.earth.x, self.earth.y, self.earth.vx, self.earth.vy,
self.moon.x, self.moon.y, self.moon.vx, self.moon.vy,
self.lander.x, self.lander.y, self.lander.vx, self.lander.vy]
dxdt = self._deriv(t, xvec)
self._unpack_and_advance(rk4(xvec, dxdt, t, dt, self._deriv))
self._check_for_collisions()
def plot_stability(f=lambda x, y: (x, y),
fixed_points=[(0, 0)],
R=1,
cs=['r', 'b', 'g', 'm', 'c', 'y', 'k'],
linestyles=['-', '--', '-.', ':'],
Limit=1.0E12,
N=1000,
step=0.1,
S=1,
s=10,
K=1,
legend=True,
accept=lambda _: True,
eps=0.1):
starts0 = []
starts1 = []
for fixed_point in fixed_points:
for i in range(K * len(cs) * len(linestyles)):
offset = tuple(R * z for z in utilities.direct_surface(d=2))
while not accept(offset):
offset = tuple(R * z for z in utilities.direct_surface(d=2))
xys = [tuple(x + y for x, y in zip(fixed_point, offset))]
for j in range(N):
(x, y) = rk4.rk4(0.1, xys[-1], adapt(f=f))
if abs(x) < 1.5 * Limit and abs(y) < 1.5 * Limit:
xys.append((x, y))
else:
break
if abs(xys[-1][0]) > Limit or abs(xys[-1][1]) > Limit:
plt.plot([z[0] for z in xys], [z[1] for z in xys],
c=cs[i % len(cs)],
linestyle=linestyles[(i // len(cs)) %
len(linestyles)],
linewidth=3)
starts1.append((xys[0]))
else:
if abs(xys[-1][0] - xys[0][0]) < eps and abs(xys[-1][1] -
xys[0][1]) < eps:
starts0.append((xys[0]))
plt.scatter([S * x for (x, _) in starts0], [S * y for (_, y) in starts0],
c='b',
marker='*',
s=s,
label='Stable')
plt.scatter([S * x for (x, _) in starts1], [S * y for (_, y) in starts1],
c='r',
marker='+',
s=s,
label='Unstable')
plt.legend(title='Starting points, scaled by {0:3}'.format(S),
loc='best').set_draggable(True)
def get_data_pendulum(data_size, remove_transient=False, normalized=False):
if remove_transient:
start = int(data_size / 6)
else:
start = 1
shift_markers = []
end_point = [3.14, 50]
changeA(0.91)
[data, t] = rk4(derivative, end_point, 0, 0.005, data_size)
series = normalize(data[start:, 0], normalized)
end_point = data[-1, :]
shift_markers.append(len(series))
plt.figure()
plt.plot(data[start:, 0], data[start:, 1], '.')
plt.savefig('pendulum_A_{}.png'.format(0.91))
A_list = [.6, 0.886]
for A in A_list:
print("A=", A)
changeA(A)
[data, t] = rk4(derivative, end_point, t[-1], 0.005, data_size)
plt.figure()
plt.plot(data[start:, 0], data[start:, 1], '.')
plt.savefig('pendulum_A_{}.png'.format(A))
series1 = normalize(data[start:, 0], normalized)
series = np.concatenate((series, series1), axis=0)
shift_markers.append(len(series))
end_point = data[-1, :]
np.savetxt('pendulum_hybrid.txt', series)
return [shift_markers, series]
def update_plot(i):
global image, particles
t = i * dt
particles_x = np.array([])
particles_y = np.array([])
for i, particle in enumerate(particles):
particles[i] = rk4(t, particle, F, dt)
particles_x = np.append(particles_x, [particle[0]])
particles_y = np.append(particles_y, [particle[2]])
if image is not None:
image.remove()
image = plot.scatter(particles_x, particles_y)
def get_data_hybrid2(data_size, remove_transient=False, normalized=False):
shift_markers = []
end_point = [1, 3.14]
if remove_transient:
start = int(data_size / 6)
else:
start = 1
changeA(0.47)
[data, t] = rk4(derivative, end_point, 0, 0.005, data_size)
series = normalize(data[start:, 0], normalized)
shift_markers.append(len(series))
end_point = data[-1, :]
plt.figure()
plt.plot(data[start:, 0], data[start:, 1], '.')
plt.savefig('pendulum_A_{}.png'.format(0.47))
A_list = [0.6, 0.86, 0.89, 0.92]
for A in A_list:
print("A=", A)
changeA(A)
[data, t] = rk4(derivative, end_point, t[-1], 0.005, data_size)
series1 = normalize(data[start:, 0], normalized)
series = np.concatenate((series, series1), axis=0)
shift_markers.append(len(series))
end_point = data[-1, :]
plt.figure()
plt.plot(data[start:, 0], data[start:, 1], '.')
plt.savefig('pendulum_A_{}.png'.format(A))
#changeA(0.86);
#[data,t] = rk4(derivative, [3.14, 50], 0, 0.005, data_size);
#series = np.concatenate((series, data[:,0]), axis=0);
#series = data[:,0]
return [shift_markers, series]
def dinamico(self, tf, **kwargs):
h = kwargs.get('h', 0.001)
N = int((tf) / h)
self.t = np.linspace(0, tf, N)
ceros = matrix(0.0, (N,1))
ceros_n = matrix(0.0, (N, 8))
ceros_n_2 = matrix(0.0, (N, 6))
t_eventos = self.crea_tiempo_eventos(self.t)
self.X = copy(ceros_n)
self.data = {'V': copy(ceros_n_2),
'I': copy(ceros_n_2),
'Te': copy(ceros),
'Tm': copy(ceros),
'w': copy(ceros),
'd': copy(ceros)}
self.dx = matrix(0.0, (1,8))
self.X[0, :] = self.x0
self.data['w'][0] = self.x0[6] #* 120 / (2*np.pi*self.pp)
self.data['d'][0] = self.x0[7] #* 180 / np.pi
for p in xrange(1, N):
#print 'Iteracion %d'%p
self.p = p
if t_eventos[p-1]:
for evento in t_eventos[p-1]:
print evento
self.aplicar_evento(evento[0], evento[1])
self.X[p, :] = rk4(self.ecuaciones, self.X[p-1, :], self.t[p], h)
# Se almacenan variables
self.data['V'][p, :] = self.V.T
self.data['w'][p] = self.X[p, 6]
self.data['d'][p] = self.X[p, 7]
self.data['I'][p, :] = self.I.T
self.data['Te'][p] = self.te
self.data['Tm'][p] = self.tm
sys.path.append('../')
import matplotlib.pyplot as plt,matplotlib.colors as colors,phase,numpy as np,rk4
X,Y,U,V,fixed=phase.generate(f=lambda x,y:(x-y,x*x-4))
phase.plot_phase_portrait(X,Y,U,V,fixed,title=r'$\dot{x}=x-y,\dot{y}=x^2-4$',suptitle='Example 6.3.1')
plt.figure()
import utilities,rk4
def f(x,y,direction=1):
return (direction*(y*y*y-4*x),direction*(y*y*y-y-3*x))
X,Y,U,V,fixed=phase.generate(f=f,nx=256, ny = 256,xmin=-100,xmax=100,ymin=-100,ymax=100)
phase.plot_phase_portrait(X,Y,U,V,fixed,title='$\dot{x}=y^3-4x,\dot{y}=y^3-y-3x$',suptitle='Example 6.3.9')
cs = ['r','b','g','m','c','y']
starts=[ (0,25*i) for i in range(-5,6)]
for xy0,i in zip(starts,range(len(starts))):
xy=[xy0]
for j in range(100000):
xy.append(rk4.rk4(0.0001,xy[-1],phase.adapt(f=lambda x,y:f(x,y,-1))))
plt.plot([z[0] for z in xy],
[z[1] for z in xy],
c=cs[i%len(cs)],
label='({0:.3f},{1:.3f})'.format(xy0[0],xy0[1]),linewidth=3)
leg=plt.legend(loc='best')
plt.show()
ymax=3.5)
phase.plot_phase_portrait(X,
Y,
U,
V,
fixed,
title=r'$\dot{x}=x(3-2x-y),\dot{y}=y(2-x-y)$',
suptitle='Example 6.4.2')
cs = ['r', 'b', 'g', 'm', 'c', 'y']
starts = [(0.01, 0.1), (1.05, 0.1), (0.1, 2.1), (1.1, 0.1), (10, 10),
(0.1, 1.9)]
for xy0, i in zip(starts, range(len(starts))):
xy = [xy0]
for j in range(1000000):
xy.append(rk4.rk4(0.0001, xy[-1], phase.adapt(f=f)))
plt.plot([z[0] for z in xy], [z[1] for z in xy],
c=cs[i % len(cs)],
label='({0:.3f},{1:.3f})'.format(xy0[0], xy0[1]),
linewidth=3)
leg = plt.legend(loc='best')
plt.figure()
X, Y, U, V, fixed = phase.generate(f=lambda x, y: (x * (3 - 2 * x - 2 * y), y *
(2 - x - y)),
nx=256,
ny=256,
xmin=-0.5,
xmax=3.5,
'gR': gR,
'pR': pR,
'mW': mW,
'kPa': kPa,
'g': g,
'p': p,
'm': m,
'A': A,
'mu': mu,
'Cd': Cd,
'Crr': Crr
}
# Creates ODE function call
ode = lambda t, y: train_motion(t, y, params_dict)
# Utilizes Runge Kutta to solve the ODEs
N = 0.04
tspan = np.arange(0.0, 0.95, N)
y0 = np.array([0.0, 0.0])
tr, yr = rk4(ode, tspan, y0)
plt.figure()
plt.plot(tr, yr[:, 0], 'r-')
plt.title('Position Over Time')
plt.xlabel('time (secs)')
plt.figure()
plt.plot(tr, yr[:, 1], 'g-')
plt.title('Velocity Over Time')
plt.xlabel('time (secs)')
## Import all of my methods
import euler
import rk2
import rk4
import se1
import se2
import sho1
import numpy as np
import matplotlib.pyplot as plt
## Execute the calculations of each one
euler.euler(euler.x, euler.v)
rk2.rk2(rk2.x, rk2.v)
rk4.rk4(rk4.x, rk4.v)
se1.se1(se1.x, se1.p)
se2.se2(se2.x, se2.p)
methods = [euler, rk2, rk4, se1, se2]
colors = ["r-", "y-", "g-", "b-", "c-"]
## Graphing area
for i, method in enumerate(methods):
plt.plot(method.t_points, method.e_points, colors[i], label=method.name)
plt.legend()
plt.xlabel("t")
plt.ylabel("total energy")
plt.title("Energy Conservation")
plt.plot(sho1.t_points, sho1.e_points, "r--", label=sho1.name)
plt.show()
w0 = np.array([0.1, 0.1, 0.1]) #initial w_actual components
state = np.zeros(
(N + 1, 7)) #array of state vectors at different time instants
state[0][0:4] = qp(qinv(qd(0)), q0)
state[0][4:] = w0 - np.dot(rm(state[0][0:4]), fw(0))
tau = np.zeros((N + 1, 3)) #array of torque applied at different time instants
for i in range(N): #propagating state
time = i * h
w = state[i][4:] + np.dot(rm(state[i][0:4]), fw(time))
delw = state[i][4:]
q_error = state[i][0:4]
state[i + 1] = rk4.rk4(state[i], y2dot, h, time)
state[i + 1][0:4] = state[i + 1][0:4] / np.linalg.norm(state[i + 1][0:4])
tau[i + 1] = np.cross(w, np.dot(j, w)) + np.dot(
j, -np.dot(cpm(delw), np.dot(rm(q_error), fw(time))) +
np.dot(rm(q_error), wddot(time))) - kp * q_error[1:] - kv * delw
dw = np.zeros((N + 1, 3))
for i in range(N + 1):
dw[i] = state[i][4:]
print(dw[90000])
plt.figure(0)
plt.plot(t1, tau)
plt.savefig('torque_q_BO.png')
plt.figure(1)
sdeltarn)
# Angular Rate of Body - Target (i.e. The desired angular rate)
wbr = wbn - np.dot(np.asarray(br), wrn)
## Runge-Kutta goes here
# Time inputs for Runge-Kutta
a = time[ii]
bb = time[ii] + 1
# Number of Steps for the Runge-Kutta to run through
m = 1
# Outputs from Runge-Kutta
x = rk4(a, bb, x, m, sbr, wbr, K, P)
# Update all previous values for next iteration
sigsat = x[0:3]
bn = MRP2C(sigsat)
rn = rnnew
srn = C2MRP(rn)
nr = nrnew
br = bn * nr
wbn = x[3:6]
sbr = C2MRP(br)
s = norm(sbr)
# Check the magnitude of the Body-Target MRPs and switch to shadow set if necessary
def AttExec(K_Gain, P_Gain, Position_of_Satellite, Velocity_of_Satellite, Attitude_of_Target, Attitude_of_Satellite,
Position_of_Target, Velocity_of_Target, Angular_Rate_of_Spacecraft,
Time_to_Sweep_and_Observe, Tolerance):
def norm(input):
norm = np.sqrt(sum(np.square(input)))
return norm
K = K_Gain
P = P_Gain
sBN = Attitude_of_Satellite
wBN = Angular_Rate_of_Spacecraft
wbn = wBN
rsat = Position_of_Satellite
vsat = Velocity_of_Satellite
control_torque = np.zeros([len(Position_of_Target), Time_to_Sweep_and_Observe, 3])
Observe = np.zeros([len(Position_of_Target), Time_to_Sweep_and_Observe])
sigsat = sBN
x = np.concatenate((sigsat, wbn))
# Time for the spacecraft to sweep to desired attitude (seconds)
t = Time_to_Sweep_and_Observe
# Increment Time step (seconds)
dt = 1
time = range(0, t)
for jj in range(len(Position_of_Target)):
robj = Position_of_Target[jj, 0]
vobj = Velocity_of_Target[jj, 1]
sobj = Attitude_of_Target[jj]
# Body-Inertial Frame Rotation Matrix
bn = MRP2C(sigsat)
# Hill-Inertial Frame Rotation Matrix
hn1 = MRP2C(sigsat)
hn2 = MRP2C(sobj)
# Inertial Position and Velocity of the Satellite
rsatin = hn1.transpose().dot(rsat)
vsatin = hn1.transpose().dot(vsat)
# Inertial Position and Velocity of the Object
robjin = hn2.transpose().dot(robj)
vobjin = hn2.transpose().dot(vobj)
# Inertial Position and Velocity from Satellite to Object
rtargetin = robjin - rsatin
vtargetin = vobjin - vsatin
# Target unit vectors
r1 = rtargetin/norm(rtargetin)
r3 = np.cross(rtargetin, vtargetin)/norm(np.cross(rtargetin, vtargetin))
r2 = np.cross(r3, r1)
# Inertial - Target Rotation Matrix
nr = np.matrix([r1.transpose(), r2.transpose(), r3.transpose()])
rn = nr.transpose()
# MRP of Inertial-Target
srn = C2MRP(rn)
# Check magnitude and switch to Shadow MRPs if necessary
if norm(srn) > 1:
srn = -srn/(norm(srn)**2)
# Body - Target rotation Matrix
br = bn*rn.transpose()
# MRPs for Body-Target frame
sbr = C2MRP(br)
# Checking magnitude and switching to shadow set MRPs if necessary
if norm(sbr) > 1:
sbr = -sbr/(norm(sbr)**2)
for ii in xrange(t):
# New Hill-Inertial MRPs
shnnew1 = sigsat
shnnew2 = sobj
# New Hill-Inertial Rotation Matrices
hnnew1 = MRP2C(shnnew1)
hnnew2 = MRP2C(shnnew2)
# Updated position and velocity for Spacecraft
rsinnew = hnnew1.transpose().dot(rsat)
vsinnew = hnnew1.transpose().dot(vsat)
# Updated position and velocity for Object
roinnew = hnnew2.transpose().dot(robj)
voinnew = hnnew2.transpose().dot(vobj)
# Updated position and velocity for Spacecraft - Target
rtinnew = roinnew - rsinnew
vtinnew = voinnew - vsinnew
# Updating the target unit vectors
r1 = rtinnew / norm(rtinnew)
r3 = np.cross(rtinnew, vtinnew) / norm(np.cross(rtinnew, vtinnew))
r2 = np.cross(r3, r1)
# Update for Inertial - Target Rotation Matrix
nrnew = np.matrix([r1.transpose(), r2.transpose(), r3.transpose()])
rnnew = nrnew.transpose()
# Update for Inertial - Target MRPs
srnnew = C2MRP(rnnew)
# MRP for Inertial - Target rate of change
sdeltarn = (srnnew - srn)/dt
BMatrix = BmatMRP(srn)
# Angular Rate of Inertial - Target
wrn = (4/(1+np.dot(srn, srn))**2)*np.dot(BMatrix.transpose(), sdeltarn)
# Angular Rate of Body - Target (i.e. The desired angular rate)
wbr = wbn - np.dot(np.asarray(br), wrn)
## Runge-Kutta goes here
# Time inputs for Runge-Kutta
a = time[ii]
bb = time[ii] + 1
# Number of Steps for the Runge-Kutta to run through
m = 1
# Outputs from Runge-Kutta
x = rk4(a, bb, x, m, sbr, wbr, K, P)
# Update all previous values for next iteration
sigsat = x[0:3]
bn = MRP2C(sigsat)
rn = rnnew
srn = C2MRP(rn)
nr = nrnew
br = bn*nr
wbn = x[3:6]
sbr = C2MRP(br)
s = norm(sbr)
# Check the magnitude of the Body-Target MRPs and switch to shadow set if necessary
if s > 1:
sbr = - sbr/(s**2)
if s < Tolerance:
observation = 1
else:
observation = 0
# "Output" Values for the Executive Branch
control_torque[jj, ii, ...] = (-K*sbr - P*sbr)
Observe[jj, ii] = observation
control_torque.tolist()
Observe.tolist()
return control_torque, Observe
# control: U = optivar(N,1,'U') # a variable to denote the final time tf = optivar(1,1,'tf') tf.setInit(1) # Construct list of all constraints g = [] for k in range(N): xk = vertcat(S[k], P[k] ) xk_plus = vertcat(S[k+1], P[k+1]) # shooting constraint xf = rk4(lambda x,u: tf*ode(x,u),1.0/N,xk,U[k]) g.append(xk_plus==xf) # path constraint constr = lambda P: 1-sin(2*pi*P)/2 g.append(S <= constr(P)) # Initialize with speed 1. S.setInit(1) U.setLb(0) U.setUb(1) g+= [S[0]==0, P[0]==0, P[-1]==1]
a1 = es.compute(11,0.2) at1 = a1[0] ax1 = a1[1] # h = 0.1, actual a2 = es.compute(21,0.1) at2 = a2[0] ax2 = a2[1] # h = 0.05, actual a3 = es.compute(41,0.05) at3 = a3[0] ax3 = a3[1] # h = 0.2, rk4 h1 = rk.rk4(False,10.0) t1 = h1[0] x1 = h1[1] # h = 0.1, rk4 h2 = rk.rk4(False,20.0) t2 = h2[0] x2 = h2[1] # h = 0.05, rk4 h3 = rk.rk4(False,40.0) t3 = h3[0] x3 = h3[1] # h = 0.2, euler's method h4 = em1.EulersMethod(False,10.0)
def update(self, sim, parent_mu, parent_radius, time, parent_omega):
# update vehicle atitude
self.attitude = self.attitude + self.delta_attitude
# reentry dynamics
# if dyanmic pressure is greater than x go to trim angle of attack
#if (self.q_aero > 1000 and not(self.thrusting)):
# self.attitude = -90.0 + math.atan2(self.v_parent[1],self.v_parent[0]) * 180/math.pi
# Update forces
if (self.thrusting):
# thrust is in Newtons and vehicle mass is in kg
self.a_mag = self.thrust / (self.dry_mass + self.dv_propellant_mass
+ self.attitude_propellant_mass)
self.dv_ideal = self.dv_ideal + self.a_mag * sim.dt
dprop = sim.dt * (self.thrust / self.isp)
# give infinite propellant
if (self.dv_propellant_mass > 0):
self.dv_propellant_mass = self.dv_propellant_mass - dprop
else:
self.a_mag = 0
self.a[0] = self.a_mag * math.sin(self.attitude * math.pi / 180)
self.a[1] = self.a_mag * math.cos(self.attitude * math.pi / 180)
# update aerodynamic forces
rho = 0
if (numpy.linalg.norm(self.r_parent) - parent_radius > 200000):
rho = 0
elif (numpy.linalg.norm(self.r_parent) - parent_radius < 0):
rho = atmosphere.get_density(0)
else:
rho = atmosphere.get_density(
numpy.linalg.norm(self.r_parent) - parent_radius)
# compute aerodynamic drag
self.q_aero = 0.5 * rho * numpy.linalg.norm(
self.v_parent) * numpy.linalg.norm(self.v_parent)
drag_aero = 0.1 * 10 * self.q_aero
#lift_aero = 0.1 * 10 * self.q_aero * math.sin(self.fin_delta_attitude*math.pi/180)
lift_aero = 0.0
a_drag_aero = drag_aero / (self.dry_mass + self.dv_propellant_mass +
self.attitude_propellant_mass)
a_lift_aero = lift_aero / (self.dry_mass + self.dv_propellant_mass +
self.attitude_propellant_mass)
if (numpy.linalg.norm(self.v_parent) > 5e-14):
self.a[0] = self.a[0] - a_drag_aero * self.v_parent[
0] / numpy.linalg.norm(self.v_parent)
self.a[1] = self.a[1] - a_drag_aero * self.v_parent[
1] / numpy.linalg.norm(self.v_parent)
self.a[0] = self.a[0] + a_lift_aero * self.v_parent[
1] / numpy.linalg.norm(self.v_parent)
self.a[1] = self.a[1] - a_lift_aero * self.v_parent[
0] / numpy.linalg.norm(self.v_parent)
self.a_mag = numpy.linalg.norm(self.a)
# propagate the vehicle state
prev_r = self.r_parent
prev_v = self.v_parent
state = rk4(self.r_parent, self.v_parent, self.a, sim.dt, parent_mu)
self.r_parent = state[0]
self.v_parent = state[1]
# compute planet relative velocity and position
self.vr_parent[0] = self.v_parent[0] - parent_omega * self.r_parent[1]
self.vr_parent[1] = self.v_parent[1] + parent_omega * self.r_parent[0]
self.altitude = numpy.linalg.norm(self.r_parent) - parent_radius
if (self.altitude <= 0 and not (self.thrusting)):
self.r_parent[0] = parent_radius * self.r_parent[
0] / numpy.linalg.norm(self.r_parent)
self.r_parent[1] = parent_radius * self.r_parent[
1] / numpy.linalg.norm(self.r_parent)
self.v_parent = numpy.array([0.0, 0.0])
self.longitude = math.atan2(self.r_parent[1],
self.r_parent[0]) + parent_omega * time
self.horizontal_position = self.longitude * parent_radius
# compute the orbital elements
# this should be vehicle.state.compute_orbital_elements
self.oe = rv2oe(self.r_parent, self.v_parent, parent_mu)
self.ra = self.oe[2]
self.rp = self.oe[3]
self.altitude = numpy.linalg.norm(self.r_parent) - parent_radius
self.perigee = self.rp - parent_radius
self.apogee = self.ra - parent_radius
self.semjax = self.oe[0]
self.semrax = self.oe[5]
self.argp = self.oe[4] * 180 / math.pi
self.period = self.oe[6]
def AttExec(K_Gain, P_Gain, Position_of_Satellite, Velocity_of_Satellite,
Attitude_of_Target, Attitude_of_Satellite, Position_of_Target,
Velocity_of_Target, Angular_Rate_of_Spacecraft,
Time_to_Sweep_and_Observe, Tolerance):
def norm(input):
norm = np.sqrt(sum(np.square(input)))
return norm
K = K_Gain
P = P_Gain
sBN = Attitude_of_Satellite
wBN = Angular_Rate_of_Spacecraft
wbn = wBN
rsat = Position_of_Satellite
vsat = Velocity_of_Satellite
control_torque = np.zeros(
[len(Position_of_Target), Time_to_Sweep_and_Observe, 3])
Observe = np.zeros([len(Position_of_Target), Time_to_Sweep_and_Observe])
sigsat = sBN
x = np.concatenate((sigsat, wbn))
# Time for the spacecraft to sweep to desired attitude (seconds)
t = Time_to_Sweep_and_Observe
# Increment Time step (seconds)
dt = 1
time = range(0, t)
for jj in range(len(Position_of_Target)):
robj = Position_of_Target[jj, 0]
vobj = Velocity_of_Target[jj, 1]
sobj = Attitude_of_Target[jj]
# Body-Inertial Frame Rotation Matrix
bn = MRP2C(sigsat)
# Hill-Inertial Frame Rotation Matrix
hn1 = MRP2C(sigsat)
hn2 = MRP2C(sobj)
# Inertial Position and Velocity of the Satellite
rsatin = hn1.transpose().dot(rsat)
vsatin = hn1.transpose().dot(vsat)
# Inertial Position and Velocity of the Object
robjin = hn2.transpose().dot(robj)
vobjin = hn2.transpose().dot(vobj)
# Inertial Position and Velocity from Satellite to Object
rtargetin = robjin - rsatin
vtargetin = vobjin - vsatin
# Target unit vectors
r1 = rtargetin / norm(rtargetin)
r3 = np.cross(rtargetin, vtargetin) / norm(
np.cross(rtargetin, vtargetin))
r2 = np.cross(r3, r1)
# Inertial - Target Rotation Matrix
nr = np.matrix([r1.transpose(), r2.transpose(), r3.transpose()])
rn = nr.transpose()
# MRP of Inertial-Target
srn = C2MRP(rn)
# Check magnitude and switch to Shadow MRPs if necessary
if norm(srn) > 1:
srn = -srn / (norm(srn)**2)
# Body - Target rotation Matrix
br = bn * rn.transpose()
# MRPs for Body-Target frame
sbr = C2MRP(br)
# Checking magnitude and switching to shadow set MRPs if necessary
if norm(sbr) > 1:
sbr = -sbr / (norm(sbr)**2)
for ii in xrange(t):
# New Hill-Inertial MRPs
shnnew1 = sigsat
shnnew2 = sobj
# New Hill-Inertial Rotation Matrices
hnnew1 = MRP2C(shnnew1)
hnnew2 = MRP2C(shnnew2)
# Updated position and velocity for Spacecraft
rsinnew = hnnew1.transpose().dot(rsat)
vsinnew = hnnew1.transpose().dot(vsat)
# Updated position and velocity for Object
roinnew = hnnew2.transpose().dot(robj)
voinnew = hnnew2.transpose().dot(vobj)
# Updated position and velocity for Spacecraft - Target
rtinnew = roinnew - rsinnew
vtinnew = voinnew - vsinnew
# Updating the target unit vectors
r1 = rtinnew / norm(rtinnew)
r3 = np.cross(rtinnew, vtinnew) / norm(np.cross(rtinnew, vtinnew))
r2 = np.cross(r3, r1)
# Update for Inertial - Target Rotation Matrix
nrnew = np.matrix([r1.transpose(), r2.transpose(), r3.transpose()])
rnnew = nrnew.transpose()
# Update for Inertial - Target MRPs
srnnew = C2MRP(rnnew)
# MRP for Inertial - Target rate of change
sdeltarn = (srnnew - srn) / dt
BMatrix = BmatMRP(srn)
# Angular Rate of Inertial - Target
wrn = (4 / (1 + np.dot(srn, srn))**2) * np.dot(
BMatrix.transpose(), sdeltarn)
# Angular Rate of Body - Target (i.e. The desired angular rate)
wbr = wbn - np.dot(np.asarray(br), wrn)
## Runge-Kutta goes here
# Time inputs for Runge-Kutta
a = time[ii]
bb = time[ii] + 1
# Number of Steps for the Runge-Kutta to run through
m = 1
# Outputs from Runge-Kutta
x = rk4(a, bb, x, m, sbr, wbr, K, P)
# Update all previous values for next iteration
sigsat = x[0:3]
bn = MRP2C(sigsat)
rn = rnnew
srn = C2MRP(rn)
nr = nrnew
br = bn * nr
wbn = x[3:6]
sbr = C2MRP(br)
s = norm(sbr)
# Check the magnitude of the Body-Target MRPs and switch to shadow set if necessary
if s > 1:
sbr = -sbr / (s**2)
if s < Tolerance:
observation = 1
else:
observation = 0
# "Output" Values for the Executive Branch
control_torque[jj, ii, ...] = (-K * sbr - P * sbr)
Observe[jj, ii] = observation
control_torque.tolist()
Observe.tolist()
return control_torque, Observe
te = args
m_magen, c_bak = y
dm_magen = I(t,te)-kappa*m_magen
dc_bak = alpha*m_magen
if c_bak > 0:
dc_bak -= gamma
return np.array((dm_magen, dc_bak))
data = []
plots = []
for te in (1,2,3,4,5):
y = rk4(dgl, np.array((0,0)), 0, 14, 1500, args=te)
data.append([])
for i,elem in enumerate(y):
t,cbak = y[i][0],y[i][1][1]
if cbak < 0:
cbak = 0
data[-1].append((t,cbak))
plots.append(graph.data.points(data[-1], x=1, y=2, title=r"$t_e=%d$" % te))
g = graph.graphxy(
key=graph.key.key(pos="tr", dist=0.1),
width = 10,
x = graph.axis.lin(title=r"$t$", max=10),
y = graph.axis.lin(title=r"$c_\mathrm{BAK}$ in $\permil$")
from rk4 import rk4
x = lambda t: 2 * math.pow(math.e, -t) + t - 1
f = lambda t, x: -float(x) + t
t_0 = 0.0
x_0 = 1.0
eps = 1e-4
h = 0.1
n = 10
print("runge-kutta with h")
print("v" * 30 + "\n\n")
x_h = rk4(f, t_0, x_0, h, n)
print("-" * 30 + "\n\n")
print("runge-kutta with h/2")
print("v" * 30 + "\n\n")
x_h2 = rk4(f, t_0, x_0, 0.5 * h, 2 * n)
print("-" * 30 + "\n\n")
x_precise = x(t_0 + h * n)
err_runge = math.fabs(x_h[-1] - x_h2[-1])
err_precise = math.fabs(x_precise - x_h2[-1])
print("=" * 30)
print("x_h = %.15f" % x_h[-1])
print("x_[h/2] = %.15f" % x_h2[-1])
def update_guidance(self, mass, r, v, period, perigee, altitude, scale,
center_x, center_y, viewing_angle, radius, mu, g0):
# determine dt based on a period
points = 200
v_mag = numpy.linalg.norm(v)
fpa = math.atan2(v[1], v[0])
dt = period / points
if (dt < 0.01):
dt = 0.01
if dt > self.traj_display_dt_max:
dt = self.traj_display_dt_max
r_list = list()
v_list = list()
a = numpy.array([0, 0])
r_display = numpy.array([0, 0])
append_to_list = True
fpa_prev = math.atan2(v[1], v[0])
dt_small = 0.0001
dt_prev = 0.0001
for i in range(points):
# check to see if you should use parent or sibling for propagation
parent_R = math.sqrt(r[0]**2 + r[1]**2)
R = math.sqrt(r[0]**2 + r[1]**2)
# update aerodynamic forces
a = numpy.array([0, 0])
if (R - radius > 200000):
rho = 0
elif (R - radius < 0):
rho = atmosphere.get_density(0)
dt = 3
else:
rho = atmosphere.get_density(R - radius)
dt = 3
# compute aerodynamic drag
q_aero = 0.5 * rho * numpy.linalg.norm(v) * numpy.linalg.norm(v)
drag_aero = 0.1 * 10 * q_aero
lift_aero = 0.1 * 10 * q_aero * math.sin(0 / 180)
a_drag_aero = drag_aero / (mass)
a_lift_aero = lift_aero / (mass)
if (numpy.linalg.norm(v) > 5e-14):
a[0] = a[0] - a_drag_aero * v[0] / numpy.linalg.norm(v)
a[1] = a[1] - a_drag_aero * v[1] / numpy.linalg.norm(v)
a[0] = a[0] + a_lift_aero * v[1] / numpy.linalg.norm(v)
a[1] = a[1] - a_lift_aero * v[0] / numpy.linalg.norm(v)
a_mag = numpy.linalg.norm(a)
if (append_to_list):
r_display[0] = r[0] * math.cos(
viewing_angle) - r[1] * math.sin(viewing_angle)
r_display[1] = r[0] * math.sin(
viewing_angle) + r[1] * math.cos(viewing_angle)
r_list.append((r_display[0] / scale + center_x,
r_display[1] / scale + center_y))
if (i <= 2):
v_list.append((r_display[0] / scale + center_x,
r_display[1] / scale + center_y))
# stop appending
if (R < radius and i > 0):
append_to_list = False
v_mag = numpy.linalg.norm(v)
r_mag = numpy.linalg.norm(r)
angle_rate_max = 1000 * math.pi / 180
fpa = math.atan2(v[1], v[0])
if (i == 0):
fpa_rate = abs(fpa - fpa_prev) / dt_small
else:
fpa_rate = abs(fpa - fpa_prev) / dt
if (i == 0):
state = rk4(r, v, a, dt_small, mu)
elif (fpa_rate > angle_rate_max):
state = rk4(r, v, a, dt * angle_rate_max / fpa_rate, mu)
else:
state = rk4(r, v, a, dt, mu)
fpa_prev = fpa
r = state[0]
v = state[1]
return r_list, v_list, r, v
wrn = (4/(1+np.dot(srn, srn))**2)*np.dot(BMatrix.transpose(), sdeltarn)
# Angular Rate of Body - Target (i.e. The desired angular rate)
wbr = wbn - np.dot(np.asarray(br), wrn)
## Runge-Kutta goes here
# Time inputs for Runge-Kutta
a = time[ii]
bb = time[ii] + 1
# Number of Steps for the Runge-Kutta to run through
m = 1
# Outputs from Runge-Kutta
x = rk4(a, bb, x, m, sbr, wbr, K, P)
# Update all previous values for next iteration
sigsat = x[0:3]
bn = MRP2C(sigsat)
rn = rnnew
srn = C2MRP(rn)
nr = nrnew
br = bn*nr
wbn = x[3:6]
sbr = C2MRP(br)
s = norm(sbr)
# Check the magnitude of the Body-Target MRPs and switch to shadow set if necessary
X=np.array([0.0,0.0,0.0],dtype=float) #where we will store the data at each timestep for euler / rk4
sampleTimes=np.asarray(range(int(niter)+1))*dz
result=np.asarray([x0]) #where we will store the data for plotting
X[0]=x0
X[1]=xp0
X[2]=xpp0
z=0
for titer in range(int(niter)):
X=rk4(X,z,dz,deriv,[])
result=np.append(result,[X[0]]) #store the value of theta we saw
z=z+dz
plt.scatter(sampleTimes,result,)
plt.xlabel('z')
plt.ylabel('v(z)')
plt.title('3d')
plt.savefig('3d.eps',format='eps')
# Runge Kutta for set of first order differential equations # PROGRAM oscillator # IMPLICIT none # # declarations # N:number of equations, nsteps:number of steps, tstep:length of steps # y(1): initial position, y(2):initial velocity y=np.zeros(2,'d') N=2 nsteps=300 tstep=0.1 t=0.0 y[0]=1.0 y[1]=0.0 x = np.zeros(1,'d') tim = np.zeros(1,'d') x[0] = y[0] tim[0] = t # do loop n steps of Runga-Kutta algorithm for j in range(0,nsteps): t=j*tstep rk4.rk4(t, tstep, y, N) x = np.append(x, y[0]) tim = np.append(tim, t) print t print tim pl.plot(tim,x)
pid_w = pid.PID(Kp_w, Ki_w, Kd_w, dt, w_max, w_min)
x_target = np.array([-2.0, -2.0, 0.0])
w_target_prev = 0.0
for k in xrange(Nsim):
w_target = np.arctan2((x_target[1] - xk[1, k]), (x_target[0] - xk[0, k]))
w_target = np.unwrap([w_target_prev, w_target])[-1]
d_target = np.linalg.norm(x_target[:2] - xk[:2, k])
uk[0, k] = pid_v.get_output(d_target, 0.0)
uk[1, k] = pid_w.get_output(w_target, xk[2, k])
print model_lego.vw2vlvr(*uk[:, k])
xk[:, k + 1] = rk4.rk4(model_lego.differential_drive, xk[:, k], [uk[:, k]],
dt, 50)
w_target_prev = w_target
plt_titles = ("$x [m]$", "$y [m]$", "$\psi [rad]$", "$v$", "$\omega$")
f, axarr = plt.subplots(Nx + Nu, 1)
for i in xrange(Nx):
axarr[i].plot(tsim, xk[i, :-1], ".-", color='k')
axarr[i].set_title(plt_titles[i])
axarr[i].grid()
for i in xrange(Nu):
axarr[Nx + i].step(tsim, uk[i, :], "k.-", where='post')
axarr[Nx + i].set_title(plt_titles[Nx + i])
axarr[Nx + i].grid()
# Velocity control
Kp = 0.5
Ki = 20.0
Kd = 0.0
pid_vel = pid.PID(Kp, Ki, Kd, dt, u_max, u_min)
reference = 5.0 # rad/seg
prev_pos = 0.0
for k in xrange(Nsim):
measurement = dcmotor_measurement(xk[:, k])
omega = (measurement - prev_pos) / dt
prev_pos = measurement
uk[:, k] = pid_vel.get_output(reference, omega)
xk[:, k + 1] = rk4.rk4(dcmotor_model, xk[:, k], [uk[:, k], Vb0], dt, 50)
f, axarr = plt.subplots(4, 1)
# First, we plot the reference.
# Then the state evolution.
# And at last, the controls.
axarr[1].plot(tsim, reference * np.ones((Nsim, )), 'r', linewidth=4)
for i in xrange(3):
axarr[i].plot(tsim, xk[i, :-1], ".-", color='k')
axarr[i].set_title(plt_titles[i])
axarr[i].grid()
axarr[3].step(tsim, uk.T, "k.-", where='post')
axarr[3].set_title('$u_k$')
axarr[3].grid()
plt.tight_layout()
verletResult = np.asarray([theta])
X[0] = theta
X[1] = omega
t = 0
for titer in range(int(niter)):
X = euler(X, t, dt, deriv, [])
eulerResult = np.append(eulerResult,
[X[0]]) #store the value of theta we saw
t = t + dt
X[0] = theta
X[1] = omega
t = 0
for titer in range(int(niter)):
X = rk4(X, t, dt, deriv, [])
rk4Result = np.append(rk4Result, [X[0]]) #store the value of theta we saw
t = t + dt
t = 0
X[1] = theta #initial value of the angle
X[0] = theta + omega * dt + dt * dt * (
-np.sin(theta)) / 2 #next value of the angle (startup)
np.append(verletResult, [X[0]])
for titer in range(int(niter)):
xcurr = X[0] #current value of x
X[0] = 2 * X[0] - X[1] + dt * dt * (-np.sin(xcurr)
) #update the value of the current x
X[1] = xcurr #update the value of the old x
verletResult = np.append(verletResult,
[X[0]]) #store the value of theta we saw
import test import rk4 y = rk4.rk4(test.f,11,0.0,0.1,1.0) print y
x0 = np.array([0])
dist = lambda x1, x2: x2 - x1
def f(y, x):
out = np.zeros(y.shape)
for idx in range(y.shape[0]):
if (idx % 2 == 0):
for i in range(y.shape[0] / 2):
if (idx != i * 2):
out[idx +
1] = out[idx + 1] + G * M * dist(y[idx], y[
i * 2]) / np.linalg.norm(dist(y[idx], y[i * 2]))**3
if (idx % 2 == 1):
out[idx - 1] = out[idx]
return out
y, x = rk4(y0, x0, f, h, n)
plt.figure(figsize=(16, 4))
plt.subplot(131)
plt.title("Numerical and Analytical Solution")
plt.plot(x, y)
plt.savefig("fig2.pdf")
def iterate(self, t_n, x_n, u_n, n=5):
for i in range(n - 1):
x_n = rk4.rk4(self.f, t_n, x_n, u_n, 0.005 / n)
return rk4.rk4(self.f, t_n, x_n, u_n, 0.005 / n)
from euler import eulerMethod
from rk2 import rk2
from rk4 import rk4
from rrz2 import rrz2
if __name__ == "__main__":
eulerMethod()
rk2()
rk4()
rrz2()
def update_simulation(self):
self.ui_pendulum.delete_pendulum()
self.double_pendulum = rk4.rk4(self.double_pendulum)
self.ui_pendulum.draw_pendulum(self.double_pendulum)
import numpy as np
from constants import T, h
import rk4
from wdesired import fw
def qddot(t, qd):
qdv = qd[1:]
qdo = qd[0]
f = np.zeros(4)
f[0] = -0.5 * np.dot(qdv, fw(t))
f[1:] = 0.5 * (qdo * fw(t) + np.cross(qdv, fw(t)))
return f
qdes = np.zeros(
(int(T / h + 1), 4)) #array of q_desired at different time instants
qdes[0][:] = np.array([1, 0, 0, 0])
for i in range(int(T / h)): #propagating q_desired
tn = i * h
qdes[i + 1][:] = rk4.rk4(qdes[i], qddot, h, tn)
qdes[i + 1][:] = qdes[i + 1][:] / np.linalg.norm(qdes[i + 1][:])
def qd(n):
return qdes[n]
xhat[1, k + 1] = xhat[1, k] + 0.5 * (travelled_distance[0] + travelled_distance[1]) * np.sin(xhat[2, k])
xhat[2, k + 1] = xhat[2, k] + (travelled_distance[1] - travelled_distance[0]) / model_lego.wheel_distance
w_target = np.arctan2((x_target[1] - xk[1, k]), (x_target[0] - xk[0, k]))
w_target = np.unwrap([w_target_prev, w_target])[-1]
d_target = np.linalg.norm(x_target[:2]- xk[:2, k])
_vk = pid_v.get_output(d_target, 0.0)
_wk = pid_w.get_output(w_target, xk[2, k])
_vlvr = model_lego.vw2vlvr_1(_vk, _wk)
uk[:,k] = model_lego.v2pwm(_vlvr)
print uk[:,k]
xk[:, k+1] = rk4.rk4(model_lego.f, xk[:, k], [uk[:, k]], dt, 50)
w_target_prev = w_target
measurement_prev = measurement
plt_titles = ("$x [m]$","$y [m]$","$\psi [rad]$","$u_l$", "$u_r$")
Nx = 3
f, axarr = plt.subplots(Nx+Nu,1)
for i in xrange(Nx):
axarr[i].plot(tsim,xk[i,:-1],".-", color='k')
axarr[i].plot(tsim, xhat[i, :-1], "o-", color='r')
axarr[i].set_title(plt_titles[i])
axarr[i].grid()
for i in xrange(Nu):
axarr[Nx+i].step(tsim,uk[i,:],"k.-", where='post')
