def main():
# 制御対象を定義(今回はモータを想定)
J = 0.6 # 慣性
D = 0.2 # 粘性
Kt = 0.15 # トルク定数
P = crl.tf([Kt], [J, D, 0]) # モータ角度の電流制御
# 参照モデルの定義
M = crl.tf([50], [1, 50])
# シミュレーション条件を定義
Ts = 0.001 # サンプリング時間
# 制御器構造の定義
beta = crl.ss("0", "1", "0; 1", "1; 0") # PI制御器
print(crl.tf(beta))
beta = crl.c2d(beta, Ts)
# 制御対象と参照モデルのゲイン線図
plt.figure(1)
crl.bode([P, M])
plt.legend(["Plant", "Ref. model"])
# prbsを生成する
n = 12 # m系列信号の次数
Tp = 5 # 何周期印加するか？
Ap = 0.5 # 振幅 [A]
u0 = 2 * prbs(n)
u0 = Ap * (u0 - np.average(u0))
u0 = np.tile(u0, Tp)
t = np.linspace(0, Ts * u0.shape[0], u0.shape[0])
plt.figure(2)
plt.plot(t, u0)
plt.xlabel("Time [s]")
plt.ylabel("Current [A]")
# 実験データの取得
print("=== Start Experiment ===")
Pd = crl.c2d(P, Ts, "tustin") # 双一次変換で離散時間モデルを作成
t = np.arange(start=0, stop=Tp * Ts * (2**n - 1), step=Ts)
(y0, t0, xout) = crl.lsim(Pd, u0[:], t[:]) # 応答を取得
plt.figure(3)
plt.plot(y0)
# VRFTを実行する
print("=== Start VRFT ===")
W = crl.tf([50], [1, 50]) # 周波数重み
L = crl.minreal((1 - M) * M * W) # プレフィルタ
Ld = crl.c2d(L, Ts, "tustin")
(ul, tl, xlout) = crl.lsim(Ld, u0, t)
(phi_l, tl, xlout) = crl.lsim(beta, y0[:])
print(phi_l)
rho = np.linalg.solve(phi_l, ul)
print(rho)
plt.show()
def __init__(self):
# define parameters
self.mc = 1.0
self.mr = 0.25
self.g = 9.81
self.Fe = (self.mc + 2 * self.mr) * self.g
self.mu = 0.1
self.Jc = 0.0042
self.d = 0.3
self.H = 8 # number of points in the horizon
# define the state space equations of the form xdot = Ax + Bu
self.A_long = np.array([[0.0, 1.0], [0.0, 0.0]])
self.B_long = np.array([[0], [1 / (self.mc + 2 * self.mr)]])
self.C_long = np.array([[1, 0], [0, 1]])
self.D_long = np.array([[0], [0]])
self.A_lat = np.array([[0, 0, 1, 0], [0, 0, 0, 1],
[
0, -(self.Fe) / (self.mc + 2 * self.mr),
(-self.mu) / (self.mc + 2 * self.mr), 0
], [0, 0, 0, 0]])
self.B_lat = np.array([[0], [0], [0],
[1 / (self.Jc + 2 * self.mr * self.d**2)]])
self.C_lat = np.array([[1, 0, 0, 0], [0, 1, 0, 0]])
self.D_lat = np.array([[0], [0]])
# convert the continuous state space system to a discrete time system
self.statespace_long = StateSpace(self.A_long, self.B_long,
self.C_long, self.D_long)
self.statespace_lat = StateSpace(self.A_lat, self.B_lat, self.C_lat,
self.D_lat)
# The interval of time between each discrete interval
self.Ts = 0.5
# creates an object which contains the discretized version of A, B, C, and D as well as a dt
self.discrete_long = c2d(self.statespace_long,
self.Ts,
method='zoh',
prewarp_frequency=None)
self.discrete_lat = c2d(self.statespace_lat,
self.Ts,
method='zoh',
prewarp_frequency=None)
self.objhist_h = np.ones(self.H)
self.objhist_z = np.ones(self.H)
self.ulast_long = []
self.flast_long = []
self.glast_long = []
self.ulast_lat = []
self.flast_lat = []
self.glast_lat = []
def __init__(self, sysc=sysc_default, Ts=dt_default, epsilon=1e-7):
self.sysd = c2d(sysc, Ts)
self.Ts = Ts
self.sysc = sysc
self.epsilon = epsilon
self.est = Estimator(self.sysd, 500, self.epsilon)
self.y_index = None
self.worst_case_control = 'current'
self.k = None
self.y_up = None
self.y_lo = None
# diy
self.p = 0.5
self.i = 7
self.d = 0
self.total = 10
self.slot = int(self.total / Ts)
self.t_arr = linspace(0, self.total, self.slot + 1)
self.ref = [0] * 251 + [1] * 250
self.thres = 5
self.drift = 0.12
self.x_0 = [0]
self.y_real_arr = []
self.s = 0
self.att = 0
self.cin = 0
self.ymeasure = 0
self.yreal = 0
self.score = []
self.place = 350
self.maxc = 5
# --------------------------------------
self.safeset = {'lo': [-2.7], 'up': [2.7]}
def __init__(self, sysc=sysc_default, Ts=dt_default, epsilon=1e-7):
self.sysd = c2d(sysc, Ts)
self.Ts = Ts
self.sysc = sysc
self.epsilon = epsilon
self.est = Estimator(self.sysd, 500, self.epsilon)
self.y_index = None
self.worst_case_control = 'current'
self.k = None
self.y_up = None
self.y_lo = None
# diy
self.p = 0.1
self.i = 0
self.d = 0.6
self.total = 30
self.slot = int(self.total / Ts)
self.t_arr = linspace(0, self.total, self.slot + 1)
self.ref = [2] * 601 + [4] * 600 + [2] * 300
self.thres = 3
self.drift = 1
self.x_0 = [0]
self.y_real_arr = []
self.s = 0
self.att = 0
self.cin = 0
self.ymeasure = 0
self.yreal = 0
self.score = []
self.place = 700
self.maxc = 50
self.xmeasure = [[0], [0], [0], [0], [0], [0], [0], [0], [0]]
self.xreal = [[0], [0], [0], [0], [0], [0], [0], [0], [0]]
self.safeset = {'lo': [-10000] * 8 + [-1], 'up': [10000] * 8 + [8]}
def __init__(self, dt):
num = np.array([0.01, 0.01, 0.02])
den = np.array([0.001, 1, 0])
sys_tf = ctrl.tf(num, den)
self.A, self.B, self.C, self.D = ctrl.ssdata(
ctrl.c2d(sys_tf, dt, 'foh'))
self.X = np.zeros((self.A.shape[0]))
def __init__(self, sysc=sysc_default, Ts=dt_default, epsilon=1e-7):
self.sysd = c2d(sysc, Ts)
self.Ts = Ts
self.sysc = sysc
self.epsilon = epsilon
self.est = Estimator(self.sysd, 500, self.epsilon)
self.y_index = None
self.worst_case_control = 'current'
self.k = None
self.y_up = None
self.y_lo = None
# diy
self.p = 14
self.i = 0.8
self.d = 5.7
self.total = 10
self.slot = int(self.total / Ts)
self.t_arr = linspace(0, self.total, self.slot + 1)
self.ref = [0.5] * 201 + [0.7] * 200 + [0.5] * 100
self.thres = 10
self.drift = 1
self.x_0 = [0]
self.y_real_arr = []
self.s = 0
self.att = 0
self.cin = 0
self.ymeasure = 0
self.yreal = 0
self.score = []
self.place = 300
self.maxc = 20
self.xmeasure = [[0], [0], [0]]
self.xreal = [[0], [0], [0]]
def __init__(self, sysc, Ts, epsilon=1e-7):
self.sysd = c2d(sysc, Ts)
self.Ts = Ts
self.sysc = sysc
self.epsilon = epsilon
self.est = Estimator(self.sysd, 150, self.epsilon)
self.y_index = None
self.worst_case_control = 'current'
self.k = None
self.y_up = None
self.y_lo = None
def testSS2cont(self):
"""Test c2d()"""
sys = ss(np.array([[-3, 4, 2], [-1, -3, 0], [2, 5, 3]]),
np.array([[1, 4], [-3, -3], [-2, 1]]),
np.array([[4, 2, -3], [1, 4, 3]]), np.array([[-2, 4], [0,
1]]))
sysd = c2d(sys, 0.1)
np.testing.assert_array_almost_equal(
np.array(
[[0.742840837331905, 0.342242024293711, 0.203124211149560],
[-0.074130792143890, 0.724553295044645, -0.009143771143630],
[0.180264783290485, 0.544385612448419, 1.370501013067845]]),
sysd.A)
np.testing.assert_array_almost_equal(
np.array([[0.012362066084719, 0.301932197918268],
[-0.260952977031384, -0.274201791021713],
[-0.304617775734327, 0.075182622718853]]), sysd.B)
def __init__(self, sysc=sysc_default, Ts=dt_default, epsilon=1e-7):
self.sysd = c2d(sysc, Ts)
self.Ts = Ts
self.sysc = sysc
self.epsilon = epsilon
self.est = Estimator(self.sysd, 500, self.epsilon)
self.y_index = None
self.worst_case_control = 'current'
self.k = None
self.y_up = None
self.y_lo = None
# diy
self.p = 11
self.i = 0
self.d = 5
self.total = 24
self.slot = int(self.total / Ts)
self.t_arr = linspace(0, self.total, self.slot + 1)
self.ref = [math.pi / 2] * 71 + [math.pi / 2] * 50
self.thres = 0.2
self.drift = 0.01
self.x_0 = [0]
self.y_real_arr = []
self.s = 0
self.att = 0
self.cin = 0
self.ymeasure = 0
self.yreal = 0
self.score = []
self.place = 20
self.maxc = 20
self.xmeasure = [[0], [0], [0]]
self.xreal = [[0], [0], [0]]
self.safeset = {
'lo': [-4, -1000000000, -100000000],
'up': [4, 1000000000, 100000000]
}
# Ganancia del mixer. MixerGain = 8 lpfoutput = (lpfoutputI + 1j * lpfoutputQ) # there are L Nyquist periods in one period of p(t) (for Tropp's analysis # Tx=Tp or L=N). L = N # 1-st order RC low-pass filter # En este fragmento de codigo se hallo la respuesta al impulso del filtro RC # Implementado analogamente. tau = 1 / (2 * pi * fc) B = 1 A = [tau, 1] lpf = tf(B, A) lpf_d = c2d(lpf, 1 / (float(W)), 'tustin') [[Bd]], [[Ad]] = tfdata(lpf_d) original = x ### Simulando la funcion impz de matlab. x = zeros(25) x[0] = 1 h = lfilter(Bd, Ad, x) # Finds closest time value to perform sampling i_tstart = argmin(abs(tspice - Td)) lpfoutput = lpfoutput[i_tstart:-1] tspice = tspice[i_tstart:-1] lpfoutput = lpfoutput - mean(lpfoutput) lpfoutput = lpfoutput / MixerGain
def runOptimization():
m = 5
k = 3
b = 0.5
H = 10 # number of points in the horizon
# define the state space equations of the form xdot = Ax + Bu
A = np.array([[0, 1], [-k / m, -b / m]])
B = np.array([[0], [1 / m]])
C = np.array([[1, 0], [0, 1]])
D = np.array([[0], [0]])
# convert the continuous state space system to a discrete time system
statespace = StateSpace(A, B, C, D)
# The interval of time between each discrete interval
Ts = 0.2
# creates an object which contains the discretized version of A, B, C, and D as well as a dt
discrete = c2d(statespace, Ts, method='zoh', prewarp_frequency=None)
objhist = np.ones(H)
def objcon(u):
f = 0 # the objective f to minimize is the difference between the reference x and the actual x, summed across each point in the time horizon
z = 0.0 # initial position of the mass
zdot = 0.0 # initial velocity of the mass
zref = 1.0 # the commanded position for the mass
x = np.array([[z], [zdot]]) # vectorize the state variables
g = np.array([0])
for i in range(len(u)):
objhist[i] = z
if i != 0:
g = np.append(g, 100 - (u[i] - u[i - 1]) / Ts)
g = np.append(g, 100 + (u[i] - u[i - 1]) / Ts)
f = f + abs(zref - z)
x_next = np.matmul(discrete.A, x) + np.matmul(discrete.B, np.array([[u[i]]]))
z = x_next[0]
x = x_next
return f[0, 0], g
def penalty(x, mu):
f, g = objcon(x)
sum = 0
for i in range(len(g)):
sum = sum + min(0, g[i])**2
return f + (mu/2 * sum)
xlast = np.array([])
flast = np.array([])
glast = np.array([])
dflast = np.array([])
dglast = np.array([])
def obj(x):
nonlocal xlast, flast, glast, dflast, dglast
if not np.array_equal(x, xlast):
flast, glast = objcon(x)
xlast = x
return flast
def con(x):
nonlocal xlast, flast, glast, dflast, dglast
if not np.array_equal(x, xlast):
flast, glast = objcon(x)
xlast = x
return glast
def constrained_optimizer(fun, x):
mu = 1.0
rho = 1.1
k = 0
convergence = 1e-2
f_next = 100
f = 0
options = {'disp': True}
while f_next-f > convergence:
f = f_next
res = minimize(fun, x, args=mu, bounds=Bounds(-10.0, 10.0), options=options, method='slsqp')
mu = rho * mu
x = res.x
f_next = res.fun
print('The forces are: ', x)
print('mu = ', mu)
k += 1
print('k = ', k)
print('f = ', f)
print('f_next = ', f_next)
return x
x0 = np.ones(H)
t0 = time.time()
x = constrained_optimizer(penalty, x0)
t1 = time.time() - t0
print('Elapsed time: ', t1)
f = obj(x)
print('x = ', x)
print('f = ', f)
t = Ts * np.array(range(H))
plt.plot(t, objhist)
plt.xlabel('Time (s)')
plt.ylabel('Position z (m)')
plt.show()
import numpy as np from control.matlab import c2d, StateSpace m = 5 k = 3 b = 0.5 A = np.array([[0, 1], [-k / m, -b / m]]) B = np.array([[0], [1 / m]]) C = np.array([[1, 0], [0, 1]]) D = np.array([[0], [0]]) statespace = StateSpace(A, B, C, D) Ts = 0.2 discrete = c2d(statespace, Ts, method='zoh', prewarp_frequency=None) print(discrete)
# z^2 - 1.98 z + 0.9802 # # Sample time: 0.01 seconds # Discrete-time transfer function. #Test if we generated a system with the same behaviour: # MATLAB: # [output, t] = step(syst_fake) # plot(t, output) # Python # output, t = step([syst_fake.num[0][0][0], syst_fake.den[0][0]]) # plot(output, t) # MATLAB wants the inverse order to plot # show() syst_fake_dis = c2d(syst_fake, 0.01, method='zoh') print syst_fake_dis # OLD scipy.signal.cont2discrete way! #syst_fake_dis = c2d([syst_fake.num[0][0][0], syst_fake.den[0][0]], 0.01) #print_c2d_matlablike(syst_fake_dis) # Python: # zero order hold by default # 4.96670866519e-05 z 4.93370716897e-05 # ----------------------- # z^2 1.0 z^2 -1.97990166083 z 0.980198673307 # # Sample time: 0.01 seconds # Discrete-time transfer function. # MATLAB:
""" Codigo para obter o equivalente digital para C(s) """ import control.matlab as control # Parametros calculados do compensador a = 10.5 b = 1.42 # Definindo FT do compensador C = control.tf([1, a], [1, b]) # Intervalo de amostragem Ta = 2e-3 # s. # Obtendo o equivalente de discreto pelo metodo de Tustin Cd = control.c2d(C, Ta, 'tustin') # Imprimindo valores print(Cd)
"""
import control as ct
import numpy as np
import matplotlib.pyplot as plt
import Plotter as mp
import control.matlab as cm
#Define the transfer function
num= np.array([2000*np.pi])
den= np.array([1,2000*np.pi])
sys1 = ct.tf(num, den)
sys2=cm.c2d(sys1,1/8000,method='tustin')
#print tf
print(sys1)
print(sys2)
#generate Bode
mag, phase, omega = cm.bode(sys1,dB=True,Plot=False)
mag2, phase2, omega2 = cm.bode(sys2,dB=True,Plot=False)
fig,axes=plt.subplots(2)
fig2,axes2=plt.subplots(2)
mp.myPlotterBode(axes[0],axes[1],omega,mag,phase,param_dict={'color':'blue'})
cls()
Kt = 1.0
Jt = 1.0 # [?]
u1 = tf([Kt], [1])
u2 = tf([1 / Jt], [1])
# fixme: как добавить сигнал шума?
u12 = series(u1, u2)
u3 = tf([1], [1, 0])
u4 = copy.deepcopy(u3)
u34 = series(u3, u4)
# похоже нельзя соединить аналоговую и дискретную
u34_d0 = c2d(u3, Ts=0.5)
u34_d = c2d(u3, Ts=0.5)
print series(u34_d0, u34_d)
u14 = series(u12, u34)
print u14
ts, xs = step_response(u14)
# print (c2d(u14, Ts=0.5))
plot(ts, xs)
grid()
show()
from control import matlab import matplotlib.pyplot as plt import numpy as np Gs = matlab.tf(0.5, [1, 0.5]) print(Gs) T = 0.1 Gz = matlab.c2d(Gs, T, method='zoh') #'zoh'0次ホールド print(Gz)
K = c.place(A, B, P) # Place new poles
Acl = np.array(A - B * K) # Create a closed loop A matrix
Ecl = LA.eigvals(Acl) # Calculate new eigenvalues (this evaluates to the poles that I placed!)
syscl = c.ss(Acl, B, C, D) # Closed loop state space model
step = c.step_response(syscl)
Kdc = c.dcgain(syscl) # Calculating a dc gain in order to reach intended output goal
Kr = 1 / Kdc
sysclNew = c.ss(Acl, B * Kr, C, D)
sysclNewDiscrete = matlab.c2d(sysclNew, 0.01)
stepNew = c.step_response(sysclNew)
stepDiscrete = c.step_response(sysclNewDiscrete)
plt.figure(1)
yout, T = stepDiscrete
plt.plot(yout.T, T.T)
plt.xlabel("Time (Seconds)")
plt.ylabel("Amplitude")
plt.show() # Graph step response with time
# plt.figure(2)
# yout, T = stepNew
# plt.plot(yout.T, T.T)
# plt.xlabel("Time (Seconds)")
def main():
dt = 0.05
simulation_time = 10 # in seconds
iteration_num = int(simulation_time / dt)
# you must be care about this matrix
# these A and B are for continuos system if you want to use discret system matrix please skip this step
# lineared car system
V = 5.0
A = np.array([[0., V], [0., 0.]])
B = np.array([[0.], [1.]])
C = np.eye(2)
D = np.zeros((2, 1))
# make simulator with coninuous matrix
init_xs_lead = np.array([5., 0., 0.])
init_xs_follow = np.array([0., 0., 0.])
lead_car = TwoWheeledSystem(init_states=init_xs_lead)
follow_car = TwoWheeledSystem(init_states=init_xs_follow)
# create system
sysc = matlab.ss(A, B, C, D)
# discrete system
sysd = matlab.c2d(sysc, dt)
Ad = sysd.A
Bd = sysd.B
# evaluation function weight
Q = np.diag([1., 1.])
R = np.diag([5.])
pre_step = 15
# make controller with discreted matrix
# please check the solver, if you want to use the scipy, set the MpcController_scipy
lead_controller = MpcController_cvxopt(Ad,
Bd,
Q,
R,
pre_step,
dt_input_upper=np.array([30 * dt]),
dt_input_lower=np.array([-30 * dt]),
input_upper=np.array([30.]),
input_lower=np.array([-30.]))
follow_controller = MpcController_cvxopt(
Ad,
Bd,
Q,
R,
pre_step,
dt_input_upper=np.array([30 * dt]),
dt_input_lower=np.array([-30 * dt]),
input_upper=np.array([30.]),
input_lower=np.array([-30.]))
lead_controller.initialize_controller()
follow_controller.initialize_controller()
# reference
lead_reference = np.array([[0., 0.] for _ in range(pre_step)]).flatten()
for i in range(iteration_num):
print("simulation time = {0}".format(i))
# make lead car's move
if i > int(iteration_num / 3):
lead_reference = np.array([[4., 0.]
for _ in range(pre_step)]).flatten()
lead_states = lead_car.xs
lead_opt_u = lead_controller.calc_input(lead_states[1:],
lead_reference)
lead_opt_u = np.hstack((np.array([V]), lead_opt_u))
# make follow car
follow_reference = np.array([lead_states[1:]
for _ in range(pre_step)]).flatten()
follow_states = follow_car.xs
follow_opt_u = follow_controller.calc_input(follow_states[1:],
follow_reference)
follow_opt_u = np.hstack((np.array([V]), follow_opt_u))
lead_car.update_state(lead_opt_u, dt=dt)
follow_car.update_state(follow_opt_u, dt=dt)
# figures and animation
lead_history_states = np.array(lead_car.history_xs)
follow_history_states = np.array(follow_car.history_xs)
time_history_fig = plt.figure()
x_fig = time_history_fig.add_subplot(311)
y_fig = time_history_fig.add_subplot(312)
theta_fig = time_history_fig.add_subplot(313)
car_traj_fig = plt.figure()
traj_fig = car_traj_fig.add_subplot(111)
traj_fig.set_aspect('equal')
x_fig.plot(np.arange(0, simulation_time + 0.01, dt),
lead_history_states[:, 0],
label="lead")
x_fig.plot(np.arange(0, simulation_time + 0.01, dt),
follow_history_states[:, 0],
label="follow")
x_fig.set_xlabel("time [s]")
x_fig.set_ylabel("x")
x_fig.legend()
y_fig.plot(np.arange(0, simulation_time + 0.01, dt),
lead_history_states[:, 1],
label="lead")
y_fig.plot(np.arange(0, simulation_time + 0.01, dt),
follow_history_states[:, 1],
label="follow")
y_fig.plot(np.arange(0, simulation_time + 0.01, dt),
[4. for _ in range(iteration_num + 1)],
linestyle="dashed")
y_fig.set_xlabel("time [s]")
y_fig.set_ylabel("y")
y_fig.legend()
theta_fig.plot(np.arange(0, simulation_time + 0.01, dt),
lead_history_states[:, 2],
label="lead")
theta_fig.plot(np.arange(0, simulation_time + 0.01, dt),
follow_history_states[:, 2],
label="follow")
theta_fig.plot(np.arange(0, simulation_time + 0.01, dt),
[0. for _ in range(iteration_num + 1)],
linestyle="dashed")
theta_fig.set_xlabel("time [s]")
theta_fig.set_ylabel("theta")
theta_fig.legend()
time_history_fig.tight_layout()
traj_fig.plot(lead_history_states[:, 0],
lead_history_states[:, 1],
label="lead")
traj_fig.plot(follow_history_states[:, 0],
follow_history_states[:, 1],
label="follow")
traj_fig.set_xlabel("x")
traj_fig.set_ylabel("y")
traj_fig.legend()
plt.show()
lead_history_us = np.array(lead_controller.history_us)
follow_history_us = np.array(follow_controller.history_us)
input_history_fig = plt.figure()
u_1_fig = input_history_fig.add_subplot(111)
u_1_fig.plot(np.arange(0, simulation_time + 0.01, dt),
lead_history_us[:, 0],
label="lead")
u_1_fig.plot(np.arange(0, simulation_time + 0.01, dt),
follow_history_us[:, 0],
label="follow")
u_1_fig.set_xlabel("time [s]")
u_1_fig.set_ylabel("u_omega")
input_history_fig.tight_layout()
plt.show()
animdrawer = AnimDrawer([lead_history_states, follow_history_states])
animdrawer.draw_anim()
Uc = matlab.ctrb(A, b)
print(Uc)
rank = np.linalg.matrix_rank(Uc)
print(rank)
#可観測性の検証
Uo = matlab.obsv(A, C)
print(Uo) #出力に三点リーダーが出てくるのはいっぱいあるのを省略している
rank = np.linalg.matrix_rank(Uo)
print(rank)
#離散時間系でもやってみましょう！
print("ここからは離散時間系での実行")
S = matlab.ss(A, b, C, d)
P = matlab.c2d(S, T, method='zoh') #continuous to discrete
#科制御性の検証
Uc = matlab.ctrb(P.A, P.B)
rank = np.linalg.matrix_rank(Uc)
print(rank)
#可観測性の検証
Uo = matlab.obsv(P.A, P.C)
rank = np.linalg.matrix_rank(Uo)
print(rank)
'''
中島「磯野～！安定判別しようぜ～！」
カツオ「ええ～！？そんな面倒くさいことしたくないよ～」
中島「そこでコンピュータの力を借りるのさ、固有値問題なんてすぐ解けるよ」
'''
def runoptimization():
# define parameters
mc = 1.0
mr = 0.25
g = 9.81
Fe = (mc + 2 * mr) * g
mu = 0.1
Jc = 0.0042
d = 0.3
H = 7 # number of points in the horizon
# define the state space equations of the form xdot = Ax + Bu
A_long = np.array([[0.0, 1.0], [0.0, 0.0]])
B_long = np.array([[0], [1 / (mc + 2 * mr)]])
C_long = np.array([[1, 0], [0, 1]])
D_long = np.array([[0], [0]])
A_lat = np.array([[0, 0, 1, 0],
[0, 0, 0, 1],
[0, -(Fe) / (mc + 2 * mr), (-mu) / (mc + 2 * mr), 0],
[0, 0, 0, 0]])
B_lat = np.array([[0],
[0],
[0],
[1 / (Jc + 2 * mr * d ** 2)]])
C_lat = np.array([[1, 0, 0, 0],
[0, 1, 0, 0]])
D_lat = np.array([[0],
[0]])
# convert the continuous state space system to a discrete time system
statespace_long = StateSpace(A_long, B_long, C_long, D_long)
statespace_lat = StateSpace(A_lat, B_lat, C_lat, D_lat)
# The interval of time between each discrete interval
Ts = 0.2
# creates an object which contains the discretized version of A, B, C, and D as well as a dt
discrete_long = c2d(statespace_long, Ts, method='zoh', prewarp_frequency=None)
discrete_lat = c2d(statespace_lat, Ts, method='zoh', prewarp_frequency=None)
objhist_h = np.ones(H)
objhist_z = np.ones(H)
def objcon_long(u, discrete_long):
f = 0 # the objective f to minimize is the difference between the reference x and the actual x, summed across each point in the time horizon
h = 0.0 # initial position of the mass
hdot = 0.0 # initial velocity of the mass
href = 1.0 # the commanded longitudinal position for the VTOL
x_long = np.array([[h], [hdot]]) # vectorize the state variables
# g = np.array([0])
for i in range(len(u)):
objhist_h[i] = h
# if i != 0:
# g = np.append(g, 100 - (u[i] - u[i-1])/Ts)
# g = np.append(g, 100 + (u[i] - u[i-1])/Ts)
f = f + abs(href - h)
x_long_next = np.matmul(discrete_long.A, x_long) + np.matmul(discrete_long.B, np.array([[u[i]]]))
h = x_long_next[0]
x_long = x_long_next
return f[0, 0]
def objcon_lat(u, discrete_lat):
f = 0 # the objective f to minimize is the difference between the reference x and the actual x, summed across each point in the time horizon
z = 0.0
theta = 0.0
zdot = 0.0
thetadot = 0.0
zref = 2.0 # the commanded lateral position for the VTOL
x_lat = np.array([[z], [theta], [zdot], [thetadot]])
# g = np.array([0])
for i in range(len(u)):
objhist_z[i] = z
# if i != 0:
# g = np.append(g, 100 - (u[i] - u[i-1])/Ts)
# g = np.append(g, 100 + (u[i] - u[i-1])/Ts)
f = f + abs(zref - z)
x_lat_next = np.matmul(discrete_lat.A, x_lat) + np.matmul(discrete_lat.B, np.array([[u[i]]]))
z = x_lat_next[0]
x_lat = x_lat_next
return f[0, 0]
ulast_long = []
flast_long = []
glast_long = []
ulast_lat = []
flast_lat = []
glast_lat = []
def obj_long(u, discrete_long):
nonlocal ulast_long, flast_long, glast_long
if not np.array_equal(u, ulast_long):
flast_long = objcon_long(u, discrete_long)
ulast_long = u
return flast_long
def con_long(u):
nonlocal ulast_long, flast_long, glast_long
if not np.array_equal(u, ulast_long):
flast_long = objcon_long(u, discrete_long)
ulast_long = u
return glast_long
def obj_lat(u, discrete_lat):
nonlocal ulast_lat, flast_lat, glast_lat
if not np.array_equal(u, ulast_lat):
flast_lat = objcon_lat(u, discrete_lat)
ulast_lat = u
return flast_lat
def con_lat(u, discrete_lat):
nonlocal ulast_lat, flast_lat, glast_lat
if not np.array_equal(u, ulast_lat):
flast_lat = objcon_lat(u, discrete_lat)
ulast_lat = u
return glast_lat
def nelder_mead(fun, discrete, x, tau_x, tau_f):
# initialize variables
n = len(x)
simplex = np.zeros((n, n+1))
simplex[:, 0] = x
s = np.zeros(n)
l = 0.0001
def gradx(simplex0, n):
sum = 0
for i in range(n):
sum += np.linalg.norm(simplex0[:, i] - simplex0[:, n])
return sum
def gradf(fun, simplex0, n):
sum = 0
for i in range(n):
sum += (fun(simplex0[:, i], discrete) - fun(simplex0[:, n], discrete))**2
return np.sqrt(sum/(n+1))
for j in range(n):
for i in range(n):
if j == i:
s[i] = (l/(n*np.sqrt(2)))*(np.sqrt(n+1)-1) + (1/np.sqrt(2))
else:
s[i] = (l/(n*np.sqrt(2)))*(np.sqrt(n+1)-1)
simplex[:, j+1] = x + s
f = np.zeros(n+1)
while gradx(simplex, n) > tau_x or gradf(fun, simplex, n) > tau_f:
for i in range(n+1):
f[i] = fun(simplex[:, i], discrete)
simplex = simplex[:, np.argsort(f)]
xc = (1/n) * (simplex.sum(axis=1)-simplex[:, -1])
xr = xc + (xc - simplex[:, -1])
if fun(xr, discrete) < fun(simplex[:, 0], discrete):
xe = xc + 2*(xc - simplex[:, -1])
if fun(xe, discrete) < fun(simplex[:, 0], discrete):
simplex[:, -1] = xe
else:
simplex[:, -1] = xr
elif fun(xr, discrete) <= fun(simplex[:, -2], discrete):
simplex[:, -1] = xr
else:
if fun(xr, discrete) > fun(simplex[:, -1], discrete):
xic = xc - 0.5 * (xc - simplex[:, -1])
if fun(xic, discrete) < fun(simplex[:, -1], discrete):
simplex[:, -1] = xic
else:
for i in range(n):
simplex[:, i+1] = simplex[:, 0] + 0.5 * (simplex[:, i+1]-simplex[:, 0])
else:
xoc = xc + 0.5 * (xc - simplex[:, -1])
if fun(xoc, discrete) < fun(xr, discrete):
simplex[:, -1] = xoc
else:
for i in range(n):
simplex[:, i+1] = simplex[:, 0] + 0.5 * (simplex[:, i+1] - simplex[:, 0])
return simplex[:,0]
u0_long = 0.5 * np.ones(H)
u0_lat = 0.5 * np.ones(H)
constraints = {'type': 'ineq', 'fun': con_long}
options = {'disp': False}
x = nelder_mead(obj_long, discrete_long, u0_long, 1e-6, 1e-8)
print('Optimized x: ', x)
plt.plot(objhist_h)
plt.xlabel('Time (s)')
plt.ylabel('Position z (m)')
lb = -100.0
ub = 100.0
bounds = Bounds([lb, lb, lb, lb, lb, lb, lb], [ub, ub, ub, ub, ub, ub, ub])
res_long = differential_evolution(obj_long, bounds=bounds, args=(discrete_long,), strategy='best1bin', maxiter=1000, popsize=15, tol=0.01, mutation=0.5, recombination=0.7, disp=True, polish=True, init='latinhypercube', atol=0)
res_lat = differential_evolution(obj_lat, bounds=bounds, args=(discrete_lat,), strategy='best1bin', maxiter=1000, popsize=15, tol=0.01, mutation=0.5, recombination=0.7, disp=True, polish=True, init='latinhypercube', atol=0)
print('Optimized x_gen: ', res_long.x)
print('Optimized tau_gen: ', res_lat.x)
plt.plot(objhist_h)
# plt.plot(objhist_z)
plt.xlabel('Time (s)')
plt.ylabel('Position (m)')
plt.show()
# ----------------------- # z^2 - 1.98 z + 0.9802 # # Sample time: 0.01 seconds # Discrete-time transfer function. #Test if we generated a system with the same behaviour: # MATLAB: # [output, t] = step(syst_fake) # plot(t, output) # Python # output, t = step([syst_fake.num[0][0][0], syst_fake.den[0][0]]) # plot(output, t) # MATLAB wants the inverse order to plot # show() syst_fake_dis = c2d(syst_fake, 0.01, method='zoh') print syst_fake_dis # OLD scipy.signal.cont2discrete way! #syst_fake_dis = c2d([syst_fake.num[0][0][0], syst_fake.den[0][0]], 0.01) #print_c2d_matlablike(syst_fake_dis) # Python: # zero order hold by default # 4.96670866519e-05 z 4.93370716897e-05 # ----------------------- # z^2 1.0 z^2 -1.97990166083 z 0.980198673307 # # Sample time: 0.01 seconds # Discrete-time transfer function. # MATLAB:
[0, 0, 0, 1],
[-h12 * mp * g * Lp / DLT, h12 * myup / DLT, 0, -h11 * myum / DLT]
])
b = np.array([[0], [-h12 * Km / DLT], [0], [h11 * Km / DLT]])
C = np.array([[1, 0, 0, 0], [0, 0, 1, 0]])
d = np.array([[0], [0]])
Uc = matlab.ctrb(A, b) #連続系可制御性行列作成
print(Uc) #その表示
rank = np.linalg.matrix_rank(Uc) #そのランク調査
print(rank) #ランクが4なら可制御
Ev = np.linalg.eigvals(A) #固有値=特性根計算
print(Ev) #左半面なら安定
S = matlab.ss(A, b, C, d) #システム行列の設定
P = matlab.c2d(S, T, method='zoh') #離散系へ変換
Uc = matlab.ctrb(P.A, P.B) #離散系での可制御性行列
print(Uc)
rank = np.linalg.matrix_rank(Uc) #ランク調査
print(rank) #ランクが4なら可制御
Ev = np.linalg.eigvals(P.A) #固有値=特性根計算
print(Ev) #単位円内なら安定
Uo = matlab.obsv(P.A, P.C) #可観測性行列作成
print(Uo)
rank = np.linalg.matrix_rank(Uo) #ランク調査
print(rank) #ランクが4なら可観測
x0 = np.array([[0.1], [0.1], [0.1], [.1]])
N = 200
t = np.arange(0, N * T + T, T)
def runoptimization():
# define parameters
mc = 1.0
mr = 0.25
g = 9.81
Fe = (mc + 2*mr)*g
mu = 0.1
Jc = 0.0042
d = 0.3
H = 50 # number of points in the horizon
# define the state space equations of the form xdot = Ax + Bu
A_long = np.array([[0.0, 1.0], [0.0, 0.0]])
B_long = np.array([[0], [1 / (mc + 2*mr)]])
C_long = np.array([[1, 0], [0, 1]])
D_long = np.array([[0], [0]])
A_lat = np.array([[0, 0, 1, 0],
[0, 0, 0, 1],
[0, -(Fe)/(mc+2*mr), (-mu)/(mc+2*mr), 0],
[0, 0, 0, 0]])
B_lat = np.array([[0],
[0],
[0],
[1/(Jc+2*mr*d**2)]])
C_lat = np.array([[1, 0, 0, 0],
[0, 1, 0, 0]])
D_lat = np.array([[0],
[0]])
# convert the continuous state space system to a discrete time system
statespace_long = StateSpace(A_long, B_long, C_long, D_long)
statespace_lat = StateSpace(A_lat, B_lat, C_lat, D_lat)
# The interval of time between each discrete interval
Ts = 0.05
# creates an object which contains the discretized version of A, B, C, and D as well as a dt
discrete_long = c2d(statespace_long, Ts, method='zoh', prewarp_frequency=None)
discrete_lat = c2d(statespace_lat, Ts, method='zoh', prewarp_frequency=None)
objhist_h = np.ones(H)
objhist_z = np.ones(H)
def objcon_long(u, discrete_long):
f = 0 # the objective f to minimize is the difference between the reference x and the actual x, summed across each point in the time horizon
h = 0.0 # initial position of the mass
hdot = 0.0 # initial velocity of the mass
href = 1.0 # the commanded longitudinal position for the VTOL
x_long = np.array([[h], [hdot]]) # vectorize the state variables
# g = np.array([0])
for i in range(len(u)):
objhist_h[i] = h
# if i != 0:
# g = np.append(g, 100 - (u[i] - u[i-1])/Ts)
# g = np.append(g, 100 + (u[i] - u[i-1])/Ts)
f = f + abs(href - h)
x_long_next = np.matmul(discrete_long.A, x_long) + np.matmul(discrete_long.B, np.array([[u[i]]]))
h = x_long_next[0]
x_long = x_long_next
return f[0, 0]
def objcon_lat(u, discrete_lat):
f = 0 # the objective f to minimize is the difference between the reference x and the actual x, summed across each point in the time horizon
z = 0.0
theta = 0.0
zdot = 0.0
thetadot = 0.0
zref = 1.0 # the commanded lateral position for the VTOL
x_lat = np.array([[z], [theta], [zdot], [thetadot]])
# g = np.array([0])
for i in range(len(u)):
objhist_z[i] = z
# if i != 0:
# g = np.append(g, 100 - (u[i] - u[i-1])/Ts)
# g = np.append(g, 100 + (u[i] - u[i-1])/Ts)
f = f + abs(zref - z)
x_lat_next = np.matmul(discrete_lat.A, x_lat) + np.matmul(discrete_lat.B, np.array([[u[i]]]))
z = x_lat_next[0]
x_lat = x_lat_next
return f[0, 0]
ulast_long = []
flast_long = []
glast_long = []
ulast_lat = []
flast_lat = []
glast_lat = []
def obj_long(u, discrete_long):
nonlocal ulast_long, flast_long, glast_long
if not np.array_equal(u, ulast_long):
flast_long = objcon_long(u, discrete_long)
ulast_long = u
return flast_long
def con_long(u):
nonlocal ulast_long, flast_long, glast_long
if not np.array_equal(u, ulast_long):
flast_long = objcon_long(u, discrete_long)
ulast_long = u
return glast_long
def obj_lat(u, discrete_lat):
nonlocal ulast_lat, flast_lat, glast_lat
if not np.array_equal(u, ulast_lat):
flast_lat = objcon_lat(u, discrete_lat)
ulast_lat = u
return flast_lat
def con_lat(u, discrete_lat):
nonlocal ulast_lat, flast_lat, glast_lat
if not np.array_equal(u, ulast_lat):
flast_lat = objcon_lat(u, discrete_lat)
ulast_lat = u
return glast_lat
u0_long = 0.5 * np.ones(H)
u0_lat = 0.5 * np.ones(H)
constraints = {'type': 'ineq', 'fun': con_long}
options = {'disp': False}
t0 = time.time()
res_f = minimize(obj_long, u0_long, args=discrete_long, bounds=Bounds(-10.0, 10.0), options=options, method='slsqp')
res_tau = minimize(obj_lat, u0_lat, args=discrete_lat, bounds=Bounds(-10.0, 10.0), options=options, method='slsqp')
t1 = time.time() - t0
print('Time elapsed:', t1)
# print("x = ", res.x)
# print('f = ', res.fun)
# print(res.success)
# print(res.message)
x_long_star = res_f.x
x_lat_star = res_tau.x
return x_long_star, x_lat_star, objhist_h, objhist_z
def main():
dt = 0.05
simulation_time = 30 # in seconds
iteration_num = int(simulation_time / dt)
# you must be care about this matrix
# these A and B are for continuos system if you want to use discret system matrix please skip this step
tau = 0.63
A = np.array([[-1. / tau, 0., 0., 0.], [0., -1. / tau, 0., 0.],
[1., 0., 0., 0.], [0., 1., 0., 0.]])
B = np.array([[1. / tau, 0.], [0., 1. / tau], [0., 0.], [0., 0.]])
C = np.eye(4)
D = np.zeros((4, 2))
# make simulator with coninuous matrix
init_xs = np.array([0., 0., 0., 0.])
plant = FirstOrderSystem(A, B, C, init_states=init_xs)
# create system
sysc = matlab.ss(A, B, C, D)
# discrete system
sysd = matlab.c2d(sysc, dt)
Ad = sysd.A
Bd = sysd.B
# evaluation function weight
Q = np.diag([1., 1., 1., 1.])
R = np.diag([1., 1.])
pre_step = 10
# make controller with discreted matrix
# please check the solver, if you want to use the scipy, set the MpcController_scipy
controller = MpcController_cvxopt(
Ad,
Bd,
Q,
R,
pre_step,
dt_input_upper=np.array([0.25 * dt, 0.25 * dt]),
dt_input_lower=np.array([-0.5 * dt, -0.5 * dt]),
input_upper=np.array([1., 3.]),
input_lower=np.array([-1., -3.]))
controller.initialize_controller()
for i in range(iteration_num):
print("simulation time = {0}".format(i))
reference = np.array([[0., 0., -5., 7.5]
for _ in range(pre_step)]).flatten()
states = plant.xs
opt_u = controller.calc_input(states, reference)
plant.update_state(opt_u, dt=dt)
history_states = np.array(plant.history_xs)
time_history_fig = plt.figure()
x_fig = time_history_fig.add_subplot(411)
y_fig = time_history_fig.add_subplot(412)
v_x_fig = time_history_fig.add_subplot(413)
v_y_fig = time_history_fig.add_subplot(414)
v_x_fig.plot(np.arange(0, simulation_time + 0.01, dt), history_states[:,
0])
v_x_fig.plot(np.arange(0, simulation_time + 0.01, dt),
[0. for _ in range(iteration_num + 1)],
linestyle="dashed")
v_x_fig.set_xlabel("time [s]")
v_x_fig.set_ylabel("v_x")
v_y_fig.plot(np.arange(0, simulation_time + 0.01, dt), history_states[:,
1])
v_y_fig.plot(np.arange(0, simulation_time + 0.01, dt),
[0. for _ in range(iteration_num + 1)],
linestyle="dashed")
v_y_fig.set_xlabel("time [s]")
v_y_fig.set_ylabel("v_y")
x_fig.plot(np.arange(0, simulation_time + 0.01, dt), history_states[:, 2])
x_fig.plot(np.arange(0, simulation_time + 0.01, dt),
[-5. for _ in range(iteration_num + 1)],
linestyle="dashed")
x_fig.set_xlabel("time [s]")
x_fig.set_ylabel("x")
y_fig.plot(np.arange(0, simulation_time + 0.01, dt), history_states[:, 3])
y_fig.plot(np.arange(0, simulation_time + 0.01, dt),
[7.5 for _ in range(iteration_num + 1)],
linestyle="dashed")
y_fig.set_xlabel("time [s]")
y_fig.set_ylabel("y")
time_history_fig.tight_layout()
plt.show()
history_us = np.array(controller.history_us)
input_history_fig = plt.figure()
u_1_fig = input_history_fig.add_subplot(211)
u_2_fig = input_history_fig.add_subplot(212)
u_1_fig.plot(np.arange(0, simulation_time + 0.01, dt), history_us[:, 0])
u_1_fig.set_xlabel("time [s]")
u_1_fig.set_ylabel("u_x")
u_2_fig.plot(np.arange(0, simulation_time + 0.01, dt), history_us[:, 1])
u_2_fig.set_xlabel("time [s]")
u_2_fig.set_ylabel("u_y")
input_history_fig.tight_layout()
plt.show()
[0, 0, (m * g) / (M * l) + (g / l), -0.25]
])
B = np.array([[
0,
], [
1 / M,
], [
0,
], [
1 / (M * l),
]])
C = np.array([[1., 0., 0., 0.], [0, 0, 1, 0]])
D = 0
sys_Cont = ct.StateSpace(A, B, C, D)
sys_Dis = cm.c2d(sys_Cont, dt)
Ad = sys_Dis.A
Bd = sys_Dis.B
Cd = sys_Dis.C
# Controlability test
Controlable = ct.ctrb(Ad, Bd)
# print(np.linalg.matrix_rank(Controlable)) # full rank - therefore controllable
# Observability Test:
Observable = ct.obsv(Ad, Cd)
# print(np.linalg.matrix_rank(Observable)) # full rank - therefore observable
def pendControl(sys, sysC, x0, x0_est):
def runoptimization():
# define parameters
m = 5
k = 3
b = 0.5
H = 10 # number of points in the horizon
# define the state space equations of the form xdot = Ax + Bu
A = np.array([[0, 1], [-k / m, -b / m]])
B = np.array([[0], [1 / m]])
C = np.array([[1, 0], [0, 1]])
D = np.array([[0], [0]])
# convert the continuous state space system to a discrete time system
statespace = StateSpace(A, B, C, D)
# The interval of time between each discrete interval
Ts = 0.2
# creates an object which contains the discretized version of A, B, C, and D as well as a dt
discrete = c2d(statespace, Ts, method='zoh', prewarp_frequency=None)
objhist = np.ones(H)
def objcon(u, discrete):
f = 0 # the objective f to minimize is the difference between the reference x and the actual x, summed across each point in the time horizon
z = 0.0 # initial position of the mass
zdot = 0.0 # initial velocity of the mass
zref = 1.0 # the commanded position for the mass
x = np.array([[z], [zdot]]) # vectorize the state variables
g = np.array([0])
for i in range(len(u)):
objhist[i] = z
if i != 0:
g = np.append(g, 100 - (u[i] - u[i - 1]) / Ts)
g = np.append(g, 100 + (u[i] - u[i - 1]) / Ts)
f = f + abs(zref - z)
x_next = np.matmul(discrete.A, x) + np.matmul(
discrete.B, np.array([[u[i]]]))
z = x_next[0]
x = x_next
return f[0, 0], g
ulast = []
flast = []
glast = []
def obj(u, discrete):
nonlocal ulast, flast, glast
if not np.array_equal(u, ulast):
flast, glast = objcon(u, discrete)
ulast = u
return flast
def con(u):
nonlocal ulast, flast, glast
if not np.array_equal(u, ulast):
flast, glast = objcon(u, discrete)
ulast = u
return glast
u0 = 0.5 * np.ones(H)
constraints = {'type': 'ineq', 'fun': con}
options = {'disp': False}
t0 = time.time()
res = minimize(obj,
u0,
args=discrete,
bounds=Bounds(-10.0, 10.0),
constraints=constraints,
options=options,
method='slsqp')
t1 = time.time() - t0
print('Time elapsed:', t1)
# print("x = ", res.x)
# print('f = ', res.fun)
# print(res.success)
# print(res.message)
x_star = res.x
return x_star, objhist
plt.plot(tt, temp, label='temp')
plt.plot(tt, ff_temp, label='ff_temp')
plt.legend()
plt.grid()
tsp = 200
sim_wfs = np.zeros(tt.size)
sim_temp = np.zeros(tt.size)
fplant = weld1.filter1.Filter1((0,0.002985),(1., -0.99004983))
ctrl2 = 0.2 * control.tf((1,43), (1,0)) * control.tf((1,7), (1/10,1))
# ctrl2 = 2 * control.tf((1/1,1), (1,0)) * control.tf((0/1,1), (0/2,1))
ctrl2 = 0.25 * control.tf((1,20), (1,0)) * control.tf((1,10), (1/15,1))
ctrl2 = control.tf(3, 1) + control.tf(2, (1,0)) + control.tf((0,0), 1)
ctrl2 = control.tf(7.5, 1) + control.tf(50, (1,0)) + control.tf((0.25,0), (0,1))
ctrl2_d = mt.c2d(ctrl2, .001)
fctrl = weld1.filter1.Filter1(ctrl2_d.num[0][0],ctrl2_d.den[0][0])
b, a = signal.butter(3, 100/500, 'low')
ftemp = weld1.filter1.Filter1(b, a)
for i in range(1,tt.size):
tsp = max(temp[i] - 1100, 0)
# err = tsp - ftemp.step(sim_temp[i-1])
err = tsp - sim_temp[i - 1] # ftemp.step(sim_temp[i-1])
sim_wfs[i] = fctrl.step(err)
sim_temp[i] = fplant.step(sim_wfs[i])
plt.figure('Simulation 10')
plt.plot(tt, temp-1100, label='temp')
plt.plot(tt, sim_wfs, label='sim_wfs')
plt.plot(tt, sim_temp, label='sim_temp')
plt.legend()
