def get_closed_loop_gain(self, p=None):
"""
Get the closed loop gain for the specified poles.
:param p: pole list, defaults to self.p
:type p: [type], optional
:return: [description]
:rtype: [type]
"""
if p is None:
p = self.poles
A = np.array(self.A).tolist()
B = np.array(self.B).tolist()
L = place(A, B, p)
# Set L as ca.DM
self.L = ca.DM.zeros(1, 4)
self.L[0, 0] = L[0, 0]
self.L[0, 1] = L[0, 1]
self.L[0, 2] = L[0, 2]
self.L[0, 3] = L[0, 3]
return self.L
def e_12():
print('\n--E--\n')
bnb = Sim.BallAndBeam()
max_force = 15
damping_ratio = 1 / (2**(1 / 2))
# Tuning variables
rise_time_theta = .5
bandwidth_separation = 2.4
integrator_pole = 2.
natural_frequency_theta = np.pi / (2 * rise_time_theta *
(1 - damping_ratio**2)**(1 / 2))
rise_time_z = rise_time_theta * bandwidth_separation
natural_frequency_z = np.pi / (2 * rise_time_z *
(1 - damping_ratio**2)**(1 / 2))
coefs_theta = [
1, 2 * damping_ratio * natural_frequency_theta,
natural_frequency_theta**2
]
coefs_z = [
1, 2 * damping_ratio * natural_frequency_z, natural_frequency_z**2
]
coefs_I = [1, integrator_pole]
roots = np.roots(np.convolve(coefs_I, np.convolve(coefs_theta, coefs_z)))
A = bnb.dynamics.A
B = bnb.dynamics.B
C = bnb.dynamics.C
A_I, B_I = augment_matrices(A, B, C[0:1, :])
gains = ctrl.place(A_I, B_I, roots)
dim = A.shape[1]
feedback_gain = gains[0:1, 0:dim]
integrator_gain = gains[0, dim]
print(
f'Feedback Gain: {feedback_gain}\nIntegrator Gain: {integrator_gain}')
bnb.add_controller(Control.BallAndBeam,
feedback_gain,
integrator_gain,
max_force=max_force)
requests = sg.generator(sg.square,
amplitude=0.15,
y_offset=0.25,
frequency=0.1,
t_step=bnb.seconds_per_sim_step,
t_final=90)
handle = bnb.view_animation(requests)
Sim.Animations.plt.waitforbuttonpress()
Sim.Animations.plt.close()
return handle
def d_11():
print('\n--D--\n')
msd = Sim.MassSpringDamper()
max_force = 2.0
damping_ratio = 1 / (2**(1 / 2))
rise_time = 2.0
# mass = msd.dynamics.mass
# spring_const = msd.dynamics.spring_const
# natural_frequency = ((spring_const + max_force) / mass) ** (1 / 2)
natural_frequency = np.pi / (2 * rise_time *
(1 - damping_ratio**2)**(1 / 2))
# Part A
coefs = [1, 2 * damping_ratio * natural_frequency,
natural_frequency**2] # Small error
roots = np.roots(coefs)
print(f"Roots: {roots}")
# Part B
A = msd.dynamics.A
B = msd.dynamics.B
C = msd.dynamics.C
# Part C
control_mat = ctrl.ctrb(A, B)
rank = np.linalg.matrix_rank(control_mat)
order = A.shape[0]
print(f"controllability matrix rank: {rank}")
print(f"System order: {order}")
print(f"{'Controllable' if rank == order else 'Not Controllable'}")
# Part D
feedback_gain = ctrl.place(A, B, roots)
print(f'Feedback Gain: {feedback_gain}')
reference_gain = -1 / (C @ np.linalg.inv(A - B @ feedback_gain) @ B)
print(f"Reference gain: {reference_gain}")
# Part E
msd.add_controller(SSController.MassSpringDamper, feedback_gain,
reference_gain, max_force)
request = sg.generator(sg.constant,
amplitude=2 / 3,
t_step=msd.seconds_per_sim_step,
t_final=20)
handle = msd.view_animation(request)
Sim.Animations.plt.waitforbuttonpress()
Sim.Animations.plt.close()
return handle
def d_12():
print('\n--D--\n')
msd = Sim.MassSpringDamper()
max_force = 2.0
damping_ratio = 1 / (2**(1 / 2))
rise_time = 2.0
# mass = msd.dynamics.mass
# spring_const = msd.dynamics.spring_const
# natural_frequency = ((spring_const + max_force) / mass) ** (1 / 2)
natural_frequency = np.pi / (2 * rise_time *
(1 - damping_ratio**2)**(1 / 2))
coefs = [1, 2 * damping_ratio * natural_frequency, natural_frequency**2]
integrator_root = 1.0
roots = np.roots(np.convolve(coefs, [1, integrator_root]))
print(f"Roots: {roots}")
# Part B
A = msd.dynamics.A
B = msd.dynamics.B
C = msd.dynamics.C
A_I, B_I = augment_matrices(A, B, C)
gains = ctrl.place(A_I, B_I, roots)
feedback_gain = gains[:, 0:2]
integrator_gain = gains[0, 2]
print(
f'Feedback Gain: {feedback_gain}\nIntegrator Gain: {integrator_gain}')
msd.add_controller(Control.MassSpringDamper, feedback_gain,
integrator_gain, max_force)
request = sg.generator(sg.constant,
amplitude=2 / 3,
t_step=msd.seconds_per_sim_step,
t_final=20)
handle = msd.view_animation(request)
Sim.Animations.plt.waitforbuttonpress()
Sim.Animations.plt.close()
return handle
def f_12():
print('\n--F--\n')
pvt = Sim.PlanarVTOL()
max_force = 10
damping_ratio = 1 / (2**(1 / 2)) + 0.2
# Tuning variable
rise_time_h = 2.5
rise_time_theta = 0.1
bandwidth_separation = 7
integrator_pole_lat = 1.5
integrator_pole_lon = 1
natural_frequency_h = np.pi / (2 * rise_time_h *
(1 - damping_ratio**2)**(1 / 2))
natural_frequency_theta = np.pi / (2 * rise_time_theta *
(1 - damping_ratio**2)**(1 / 2))
rise_time_z = bandwidth_separation * rise_time_theta
natural_frequency_z = np.pi / (2 * rise_time_z *
(1 - damping_ratio**2)**(1 / 2))
coefs_theta = [
1, 2 * damping_ratio * natural_frequency_theta,
natural_frequency_theta**2
]
coefs_z = [
1, 2 * damping_ratio * natural_frequency_z, natural_frequency_z**2
]
coefs_I_lat = [1, integrator_pole_lat]
roots_lat = np.roots(
np.convolve(coefs_I_lat, np.convolve(coefs_theta, coefs_z)))
print(f"Latitudinal roots: {roots_lat}")
coefs_h = [
1, 2 * damping_ratio * natural_frequency_h, natural_frequency_h * 2
]
coefs_I_lon = [1, integrator_pole_lon]
roots_lon = np.roots(np.convolve(coefs_h, coefs_I_lon))
print(f"Longitudinal roots: {roots_lon}")
# Part B
A_lat = pvt.dynamics.A_lat
B_lat = pvt.dynamics.B_lat
C_lat = pvt.dynamics.C_lat
A_I_lat, B_I_lat = augment_matrices(A_lat, B_lat, C_lat[0:1, :])
A_lon = pvt.dynamics.A_lon
B_lon = pvt.dynamics.B_lon
C_lon = pvt.dynamics.C_lon
A_I_lon, B_I_lon = augment_matrices(A_lon, B_lon, C_lon)
gains_lat = ctrl.place(A_I_lat, B_I_lat, roots_lat)
dim_lat = A_lat.shape[1]
feedback_gain_lat = gains_lat[0:1, 0:dim_lat]
int_gain_lat = gains_lat[0, dim_lat]
print(
f'Latitudinal feedback Gain: {feedback_gain_lat}\nLatitudinal integrator gain: {int_gain_lat}'
)
gains_lon = ctrl.place(A_I_lon, B_I_lon, roots_lon)
dim_lon = A_lon.shape[1]
feedback_gain_lon = gains_lon[0:1, 0:dim_lon]
int_gain_lon = gains_lon[0, dim_lon]
print(
f'Longitudinal feedback Gain: {feedback_gain_lon}\nLongitudinal integrator gain: {int_gain_lon}'
)
request = zip(
sg.generator(sg.constant,
y_offset=1,
t_step=pvt.seconds_per_sim_step,
t_final=20),
sg.generator(sg.sin,
frequency=0.08,
y_offset=0,
amplitude=2.5,
t_step=pvt.seconds_per_sim_step,
t_final=20),
)
pvt.add_controller(Control.PlanarVTOL, feedback_gain_lat, int_gain_lat,
feedback_gain_lon, int_gain_lon, max_force)
handel = pvt.view_animation(request)
Sim.Animations.plt.waitforbuttonpress()
Sim.Animations.plt.close()
return handel
def testPlace(self, siso):
"""Call place()"""
place(siso.ss1.A, siso.ss1.B, [-2, -2.5])
def main():
# 物理パラメータの定義 -------------
M = 5
m = 0.4
l = 0.3
D_theta = 0.001
D_x = 0.002
g = 9.80665
N = (4. * M + m) * l / 3.
# ----------------------------
# システム行列の定義 --------------
A = np.array(
[[0., 0., 1., 0.], [0., 0., 0., 1.],
[0., -m * g * l / N, -4 * l * D_x / (3 * N), D_theta / N],
[0., (M + m) * g / N, D_x / N, -(M + m) * D_theta / (N * m * l)]])
B = np.array([[0.], [0.], [4. * l / (3. * N)], [-1. / N]])
#C = np.eye(2, 4)
C = np.array([[0., 0., 0., 1.]])
D = np.array([[0]])
# -----------------------------
# 最適レギュレータ重みの定義 --------------
R = np.array([[1]])
Q = np.diag([0.0, 0.0, 0.0, 1.0])
#Q = np.diag([1.0, 10000.0, 1.0])
#Q = np.sqrt(C.T @ C)
# -----------------------------------
# オブザーバシステム行列の定義 ------------
p_obs = np.array([-10, -20, -10, -10]) # 2次元配列にするとplaceでエラー出る
#p_obs = np.array([-30, -25, -20]) # 2次元配列にするとplaceでエラー出る
K_obs = place(A.T, C.T, p_obs)
A_obs = A - K_obs.T @ C
B_obs = np.c_[B, K_obs.T]
C_obs = np.eye(4, 4)
# -----------------------------------
"""
A = np.array([[1.1, 2.0],
[0.0, 0.95]])
B = np.array([[0],
[0.0787]])
C = np.array([[-2, 1]])
D = np.array([[0]])
#Q = np.diag([5, 3])
Q = C.T @ C
R = np.array([[1]])
"""
print("A: ")
print(A)
print("B: ")
print(B)
print("C: ")
print(C)
print("Q: ")
print(Q)
print("R: ")
print(R)
print("p_obs: ")
print(p_obs)
print("K_obs: ")
print(K_obs)
# システムが可制御・可観測でなければ終了
if check_ctrb(A, B) == -1:
print("システムが可制御でないので終了")
return 0
#if check_obsv(A, C) == -1 :
# print("システムが可観測でないので終了")
# return 0
# 最適レギュレータの設計
K, P, e = _clqr(A, B, Q, R)
# 結果表示
print("リカッチ方程式の解:\n", P)
print("状態フィードバックゲイン:\n", K)
print("閉ループ系の固有値:\n", e)
dt = 0.001
simtime = 10
time = 0.0
x = np.array([[0.0], [0.1], [0.0], [0.0]])
u = np.zeros([B.shape[1]])
y = np.zeros([C.shape[0], 1])
t_history = []
dx_history = np.zeros([0, len(x)])
x_history = np.zeros([0, len(x)])
y_history = np.zeros([0, len(y)])
u_history = np.zeros([0, len(u)])
x_ = np.array([[0.0], [0.0], [0.0], [0.0]])
y_ = np.zeros([C.shape[0], 1])
dx__history = np.zeros([0, len(x)])
x_history = np.zeros([0, len(x)])
#y__history = np.zeros([0, len(y_)])
#u__history = np.zeros([0, len(u_)])
while (time <= simtime):
# プラント側の計算 ------------------
u = -K @ x_ # 最適ゲインによる状態フィードバック
dx = A @ x + B @ u # 状態微分
x = x + dx * dt #+ 0.01*np.random.randn(4, 1) # 状態遷移(オイラー積分)
y = C @ x #+ D @ u # 状態観測
# -------------------------------
# オブザーバ側の計算 ----------------
dx_ = A @ x_ + B @ u + K_obs.T @ (y - y_)
x_ = x_ + dx_ * dt
y_ = C @ x_
# -------------------------------
dx_history = np.r_[dx_history, dx.T]
x_history = np.r_[x_history, x.T]
y_history = np.r_[y_history, y.T]
u_history = np.r_[u_history, u.T]
t_history.append(time)
time += dt
plt.figure()
plt.subplot(211)
plt.plot(t_history, u_history, color="blue", label=u"$\ u $")
plt.ylabel(u"$u(t)$", fontsize=16)
plt.grid(True)
plt.legend()
plt.tight_layout()
plt.subplot(212)
plt.plot(t_history, x_history[:, 0], label=u"$\ x $")
plt.plot(t_history, x_history[:, 1], label=u"$\ θ $")
plt.plot(t_history, x_history[:, 2], label=u"$\ \.{x} $")
plt.plot(t_history, x_history[:, 3], label=u"$\ \.{θ} $")
plt.xlabel(u"$t [sec]$", fontsize=16)
plt.ylabel(u"$x(t)$", fontsize=16)
plt.grid(True)
plt.legend()
plt.tight_layout()
plt.show()
def __init__(self):
try:
param_namespace = '/whirlybird'
self.param = rospy.get_param(param_namespace)
except KeyError:
rospy.logfatal('Parameters not set in ~/whirlybird namespace')
rospy.signal_shutdown('Parameters not set')
g = self.param['g']
l1 = self.param['l1']
l2 = self.param['l2']
m1 = self.param['m1']
m2 = self.param['m2']
d = self.param['d']
h = self.param['h']
r = self.param['r']
Jx = self.param['Jx']
Jy = self.param['Jy']
Jz = self.param['Jz']
km = self.param['km']
self.r_in = 0.0
self.l_in = 0.0
self.theta_e = 0
self.Fe = 0.85 * (m1 * l1 - m2 * l2) * (g / l1)
# initialize xhat for lat and lon
# specify rise time, damping, omega, and observer omega for theta, phi and psi
tr_phi = 0.3 / 5.0 # 5 times faster than controller rise times
tr_psi = tr_phi * 9.0
tr_theta = 1.0 / 5.5
zeta = 0.707
wn_phi = 2.2 / tr_phi
wn_psi = 2.2 / tr_psi
wn_theta = 2.2 / tr_theta
theta = 0.0
# Create A, B, C and L matrices for lateral and Longitude
self.Alon = np.matrix([[0, 1],
[(m1 * l1 - m2 * l2) * g * sin(theta) /
(m1 * l1**2 + m2 * l2**2 + Jy), 0]])
self.Blon = np.matrix([[0], [l1 / (m1 * l1**2 + m2 * l2**2 + Jy)]])
self.Clon = np.matrix([[1, 0]])
desiredpoles_lon = np.roots([1.0, 2.0 * wn_theta * zeta, wn_theta**2])
self.Llon = ctrl.place(self.Alon.T, self.Clon.T, desiredpoles_lon)
self.Llon = self.Llon.T
# what the poles should be
#self.Llon = np.matrix([[17.1094],
# [146.4]])
# poles are currently [[17.1],[0.49]]
print 'Llon', self.Llon
self.Alat = np.matrix(
[[0, 0, 1, 0], [0, 0, 0, 1], [0, 0, 0, 0],
[l1 * self.Fe / (m1 * l1**2 + m2 * l2**2 + Jz), 0, 0, 0]])
self.Blat = np.matrix([[0], [0], [1 / Jx], [0]])
self.Clat = np.matrix([[1, 0, 0, 0], [0, 1, 0, 0]])
desiredpoles_lat = np.roots(
np.convolve([1, 2 * wn_psi * zeta, wn_psi**2],
[1, 2 * wn_phi * zeta, wn_phi**2]))
self.Llat = ctrl.place(self.Alat.T, self.Clat.T, desiredpoles_lat)
self.Llat = self.Llat.T
self.xhat_lon = np.matrix([[0.], [0.]])
self.xhat_lat = np.matrix([[0.], [0.], [0.], [0.]])
print 'Llat', self.Llat
self.prev_time = rospy.Time.now()
self._sub_ = rospy.Subscriber('whirlybird',
Whirlybird,
self.whirlybirdCallback,
queue_size=5)
self.command_sub_ = rospy.Subscriber('command',
Command,
self.commandCallback,
queue_size=5)
self._pub_ = rospy.Publisher('estimator', Twist, queue_size=5)
while not rospy.is_shutdown():
# wait for new messages and call the callback when they arrive
rospy.spin()
# 最適レギュレータの設計
K, P, e = _clqr(A, B, Q, R)
# 結果表示
print("リカッチ方程式の解:\n", P)
print("状態フィードバックゲイン:\n", K)
print("閉ループ系の固有値:\n", e)
dt = 0.015
simtime = 10
time = 0.0
# オブザーバシステム行列の定義 ------------
p_obs = np.array([-17.1+0j, -17.3-0j, -17.0+0j, -17.5-0j]) # 2次元配列にするとplaceでエラー出る
#p_obs = 1 * e.real # 2次元配列にするとplaceでエラー出る
K_obs = place(A.T, C.T, p_obs)
A_obs = A - K_obs.T @ C
B_obs = np.c_[B, K_obs.T]
C_obs = np.eye(4, 4)
print("A_obs:\n", A_obs)
print("B_obs:\n", B_obs)
eval_obs, evec_obs = la.eig(A_obs)
print("オブザーバの固有値:\n", eval_obs)
print("オブザーバの状態フィードバックゲイン:\n", K_obs)
# -----------------------------------
"""
# カルマンフィルタ型 オブザーバの重みの定義 ------
#V = np.array([[0.3, 0.5, 0.002, 0.1]])
def e_11():
print('\n--E--\n')
bnb = Sim.BallAndBeam()
max_force = 15
damping_ratio = 1 / (2**(1 / 2))
# Tuning variables
rise_time_theta = .5
bandwidth_separation = 2.4
natural_frequency_theta = np.pi / (2 * rise_time_theta *
(1 - damping_ratio**2)**(1 / 2))
rise_time_z = rise_time_theta * bandwidth_separation
natural_frequency_z = np.pi / (2 * rise_time_z *
(1 - damping_ratio**2)**(1 / 2))
# Part A
coefs_theta = [
1, 2 * damping_ratio * natural_frequency_theta,
natural_frequency_theta**2
]
coefs_z = [
1, 2 * damping_ratio * natural_frequency_z, natural_frequency_z**2
]
roots = np.concatenate((
np.roots(coefs_theta),
np.roots(coefs_z),
))
# Part B
A = bnb.dynamics.A
B = bnb.dynamics.B
C = bnb.dynamics.C
# Part C
control_mat = ctrl.ctrb(A, B)
rank = np.linalg.matrix_rank(control_mat)
order = A.shape[0]
print(f"controllability matrix rank: {rank}")
print(f"System order: {order}")
print(f"{'Controllable' if rank == order else 'Not Controllable'}")
# Part D
feedback_gain = ctrl.place(A, B, roots)
print(f'Feedback Gain: {feedback_gain}')
reference_gain = -1 / (C[0] @ np.linalg.inv(A - B @ feedback_gain) @ B)
print(f"Reference gain: {reference_gain}")
# Part E
bnb.add_controller(SSController.BallAndBeam, feedback_gain, reference_gain,
max_force)
requests = sg.generator(sg.square,
amplitude=0.15,
y_offset=0.25,
frequency=0.05,
t_step=bnb.seconds_per_sim_step,
t_final=20)
handle = bnb.view_animation(requests)
Sim.Animations.plt.waitforbuttonpress()
Sim.Animations.plt.close()
return handle
def f_11():
print('\n--F--\n')
pvt = Sim.PlanarVTOL()
max_force = 10
damping_ratio = 1 / (2**(1 / 2)) + 0.2
# Tuning variable
rise_time_h = 2.5
rise_time_theta = 0.1
bandwidth_separation = 7
natural_frequency_h = np.pi / (2 * rise_time_h *
(1 - damping_ratio**2)**(1 / 2))
natural_frequency_theta = np.pi / (2 * rise_time_theta *
(1 - damping_ratio**2)**(1 / 2))
rise_time_z = bandwidth_separation * rise_time_theta
natural_frequency_z = np.pi / (2 * rise_time_z *
(1 - damping_ratio**2)**(1 / 2))
# Part A
coefs_theta = [
1, 2 * damping_ratio * natural_frequency_theta,
natural_frequency_theta**2
]
coefs_z = [
1, 2 * damping_ratio * natural_frequency_z, natural_frequency_z**2
]
roots_lat = np.concatenate((
np.roots(coefs_theta),
np.roots(coefs_z),
))
print(f"Latitudinal roots: {roots_lat}")
coefs_h = [
1, 2 * damping_ratio * natural_frequency_h, natural_frequency_h * 2
]
roots_lon = np.roots(coefs_h)
print(f"Longitudinal roots: {roots_lon}")
# Part B
A_lat = pvt.dynamics.A_lat
B_lat = pvt.dynamics.B_lat
C_lat = pvt.dynamics.C_lat
A_lon = pvt.dynamics.A_lon
B_lon = pvt.dynamics.B_lon
C_lon = pvt.dynamics.C_lon
# Part C
control_mat_lat = ctrl.ctrb(A_lat, B_lat)
rank = np.linalg.matrix_rank(control_mat_lat)
order = A_lat.shape[0]
print(f"Latitudinal controllability matrix rank: {rank}")
print(f"Latitudinal system order: {order}")
print(f"{'Controllable' if rank == order else 'Not Controllable'}")
control_mat_lon = ctrl.ctrb(A_lon, B_lon)
rank = np.linalg.matrix_rank(control_mat_lon)
order = A_lon.shape[0]
print(f"Longitudinal Controllability matrix rank: {rank}")
print(f"Longitudinal system order: {order}")
print(f"{'Controllable' if rank == order else 'Not Controllable'}")
# Part D
feedback_gain_lat = ctrl.place(A_lat, B_lat, roots_lat)
print(f'Latitudinal feedback Gain: {feedback_gain_lat}')
reference_gain_lat = -1 / (
C_lat[0] @ np.linalg.inv(A_lat - B_lat @ feedback_gain_lat) @ B_lat)
print(f"Latitudinal reference gain: {reference_gain_lat}")
feedback_gain_lon = ctrl.place(A_lon, B_lon, roots_lon)
print(f'Longitudinal feedback Gain: {feedback_gain_lon}')
reference_gain_lon = -1 / (
C_lon @ np.linalg.inv(A_lon - B_lon @ feedback_gain_lon) @ B_lon)
print(f"Longitudinal reference gain: {reference_gain_lon}")
# Part E
request = zip(
sg.generator(sg.constant,
y_offset=1,
t_step=pvt.seconds_per_sim_step,
t_final=20),
sg.generator(sg.sin,
frequency=0.08,
y_offset=0,
amplitude=2.5,
t_step=pvt.seconds_per_sim_step,
t_final=20),
)
pvt.add_controller(SSController.PlanarVTOL, feedback_gain_lat,
reference_gain_lat, feedback_gain_lon,
reference_gain_lon, max_force)
handel = pvt.view_animation(request)
Sim.Animations.plt.waitforbuttonpress()
Sim.Animations.plt.close()
return handel
Cr = np.array([[0, 0, 1, 0]])
# confirm that the system in controllable
C_AB = ctl.ctrb(A, B)
if np.linalg.matrix_rank(C_AB) != 4:
print('not controllable')
# two rise times (one set slightly higher than the other)
tr1 = 0.5 # rise time 1
zeta = 0.707 # damping
print('tr1', tr1)
wn1 = np.pi / 2.0 / tr1 / np.sqrt(1 - zeta**2)
tr2 = 0.1 # rise time 2
zeta = 0.707 # damping
print('tr2', tr2)
wn2 = np.pi / 2.0 / tr2 / np.sqrt(1 - zeta**2)
# find desired poles based on the rise times and damping ratios
poly = np.convolve([1, 2 * zeta * wn1, wn1**2], [1, 2 * zeta * wn2, wn2**2])
p = np.roots(poly)
print('desired poles', p)
# place the poles at the desired locations
K = ctl.place(A, B, p)
print('K', K)
# find kr gain to ensure the DC gain is zero
kr = -1.0 / (np.dot(Cr, np.dot(np.linalg.inv(A - B * K), B)))
print('kr', kr)
Clon = np.matrix([[1,0]])
Dlon = np.matrix([[0]])
C_ABlon = ctl.ctrb(Alon,Blon)
## altitude rise time
tr_h = 1.25
zeta = .707
wn = np.pi/2/tr_h/np.sqrt(1-zeta**2)
p = np.roots([1,2*zeta*wn,wn**2])
Klon = ctl.place(Alon,Blon,p)
krlon = -1/(Clon*np.linalg.inv(Alon-Blon*Klon)*Blon)
F = mtot*g
Alat = np.matrix([[0,0,1,0],
[0,0,0,1],
[0,-F/mtot,-mu/mtot,0],
[0,0,0,0]])
Blat = np.matrix([[0],
[0],
[0],
[1/(Jc+2*mr*d**2)]])
def LTI_CLQE(system,saveComputation):
A = system.A
B = system.B
C = system.C
G = system.G
g = system.g
tol = system.tol
size_x = A.shape[1]
size_u = B.shape[1]
size_y = C.shape[0]
size_l = G.shape[0]
G = svd_suit(G, tol)
N = G.null
R = G.row_space #E = [N, R]
##################################################################
# Derivative-State Constraints and Effective States
#D1*dx/dt + D2*x = 0
dof = int(size_x/2)
D1 = np.hstack([np.eye(dof), np.zeros((dof,dof))])
D2 = np.hstack([np.zeros((dof,dof)), np.eye(dof)])
GG = vertcat(horzcat(G.M, np.zeros((size_l, size_x))),horzcat(D1, D2))
GG = svd_suit(GG, tol)
NA = svd_suit(N.T@A,tol)
temp = svd_suit(np.hstack([NA.null,N,GG.row_space[size_x:,:]]),tol)
R_used = temp.left_null
size_z = N.shape[1]
size_zeta = R_used.shape[1]
size_chi = size_z+size_zeta
E = np.hstack([N, R_used])
N1 = np.hstack([N, np.zeros((size_x, size_zeta))])
##################################################################
class OutStruct:
def __init__(self):
self.matrix_chi = None
self.matrix_u= None
self.matrix_y= None
self.vector = None
self.map= None
class Output:
def __init__(self):
self.sizes = None
self.initialConditions = None
self.desired = None
self.closed_loop = None
self.matrices = None
self.observer = OutStruct()
self.controller = OutStruct()
output = Output()
output.sizes = {'size_x': size_x, 'size_u': size_u, 'size_y': size_y, 'size_l': size_l,
'size_z': size_z, 'size_zeta': size_zeta, 'size_xi': size_chi}
##################################################################
if system.controllerSettings is not None:
if system.controllerSettings.Q.shape[1] == 1:
costQ = np.diag(system.controllerSettings.Q[:size_z,:].squeeze())
else:
costQ = np.diag(system.controllerSettings.Q)
if system.controllerSettings.R.shape[1] == 1:
costR = np.diag(system.controllerSettings.R[:size_z].squeeze())
else:
costR = np.diag(system.controllerSettings.R)
Kz = lqr(N.T@A@N, N.T@B, costQ, costR)[0]
else:
p = system.controllerSettings.poles[:N.shape[1]]
Kz = place(N.T@A@N, N.T@B, p)
if system.observerSettings is not None:
if system.observerSettings.Q.shape[1] == 1:
costQ = np.diag(system.observerSettings.Q[:size_z].squeeze())
else:
costQ = np.diag(system.observerSettings.Q)
if system.observerSettings.R.shape[1] == 1:
costR = np.diag(system.observerSettings.R[:size_z].squeeze())
else:
costR = np.diag(system.observerSettings.R)
L = lqr((N1.T@A@E).T, [email protected], costQ, costR)[0]
else:
p = system.observerSettings.poles[:E.shape[1]]
L = place((N1.T@A@E).T, [email protected], p)
L = L.T
Kzeta = np.linalg.pinv(N.T@B) @ N.T@A@R_used
K = np.hstack((Kz, Kzeta))
##################################################################
#observer structure
output.controller.matrix_chi = K
output.observer.matrix_chi = N1.T@A@E - L@C@E
output.observer.matrix_u = N1.T@B
output.observer.matrix_y = L
output.observer.vector = N1.T@g
output.observer.map = E
output.matrices = {'G': G, 'N': N, 'R': R, 'R_used': R_used, 'E': E,
'Kz': Kz, 'Kzeta': Kzeta, 'K': K, 'L': L,
'N1': N1}
if saveComputation==2:
return output
##################################################################
### desired
class Desired:
class Derivation:
def __init__(self):
self.NB = None
self.NB_projector= None
self.M = None
self.zdz_corrected = None
def __init__(self):
self.x = None
self.dx = None
self.z = None
self.dz = None
self.z_corrected= None
self.dz_corrected = None
self.u_corrected = None
self.derivation = Desired.Derivation()
desired = Desired()
desired.x = system.x_desired
desired.dx = system.dx_desired
desired.z = [email protected]
desired.dz = [email protected]
desired.derivation.NB = svd_suit(N.T@B,tol)
desired.derivation.NB_projector = desired.derivation.NB.left_null @ desired.derivation.NB.left_null.T
desired.derivation.M = svd_suit(np.hstack([[email protected]@A@N, desired.derivation.NB_projector]), tol)
desired.derivation.zdz_corrected = [email protected][email protected]@g + [email protected] @ np.vstack([desired.z, desired.dz])
desired.z_corrected = desired.derivation.zdz_corrected[:size_z]
desired.dz_corrected = desired.derivation.zdz_corrected[size_z:]
if np.linalg.norm( [email protected]_null.T@
(desired.dz_corrected - N.T@A@[email protected]_corrected - N.T@g) ) < tol:
desired.u_corrected = desired.derivation.NB.pinv @ (desired.dz_corrected - N.T@A@[email protected]_corrected - N.T@g)
output.controller.vector = desired.u_corrected
else:
raise Warning('cannot create a node')
if saveComputation == 1:
output.desired = desired
return output
##################################################################
### IC
class InitialConditions:
def __init__(self):
self.x
self.z
self.zeta
self.chiEstimateRandom
x_initial = system.x_initial
initialConditions = InitialConditions()
initialConditions.x = x_initial
initialConditions.z = N.T@x_initial
initialConditions.zeta = R_used.T@x_initial
InitialConditions.chi_estimate_random = 0.01*np.random.randn(size_chi, 1)
output.initialConditions = initialConditions
zeta = initialConditions.zeta
u_des = desired.u_corrected
z_des = desired.z_corrected
x_des = N@z_des + R_used@zeta
desired.zeta_corrected = zeta
desired.x_corrected = x_des
output.desired = desired
##################################################################
### Projected LTI in z-chi coordinates
class ClosedLoop:
class Coords:
def __init__(self):
self.matrix = None
self.vector = None
self.ode_func = None
self.Y0 = None
self.M = None
def __init__(self):
self.z_chi = ClosedLoop.Coords()
self.x_chi = ClosedLoop.Coords()
closed_loop = ClosedLoop()
closed_loop.z_chi.matrix = np.vstack(np.hstack([N.T@A@N, -N.T@B@K]),
np.hstack([L@C@N, (N1.T@A@E - N1.T@B@K - L@C@E)]))
closed_loop.z_chi.vector = np.vstack([N.T@A@R_used@zeta + N.T @B@Kz@z_des + N.T @B@u_des + N.T @g,
L@C@R_used@zeta + N1.T@B@Kz@z_des + N1.T@B@u_des + N1.T@g])
closed_loop.z_chi.ode_fnc = lambda y: closed_loop.z_chi.matrix @ y + closed_loop.z_xi.vector
closed_loop.z_chi.Y0 = np.hstack([initialConditions.z,initialConditions.chi_estimate_random])
##################################################################
### LTI in x-chi coordinates
closed_loop.x_chi.M = np.vstack([
np.hstack([np.eye(size_x), np.zeros((size_x, size_chi)), -R]),
np.hstack([np.zeros((size_chi, size_x)), np.eye((size_chi,size_chi)),np.zeros((size_chi, size_l))]),
np.hstack([G.M,np.zeros((size_l, size_chi)),np.zeros((size_l, size_l))])
])
iM = np.linalg.pinv(closed_loop.x_chi.M)
iM11 = iM[:(size_x+size_chi), :(size_x+size_chi)]
closed_loop.x_chi.matrix = [email protected]([np.hstack([A, -B@K]),
np.hstack([L@C, (N1.T@A@E - N1.T@B@K - L@C@E)])])
closed_loop.x_chi.vector = [email protected]([B@Kz@z_des + B@u_des+ g,N1.T@B@Kz@z_des + N1.T@B@u_des + N1.T@g])
closed_loop.x_chi.ode_fnc = lambda y: closed_loop.x_chi.matrix @ y + closed_loop.x_xi.vector
closed_loop.x_chi.Y0 = np.vstack([initialConditions.x, initialConditions.chi_estimate_random])
output.closed_loop = closed_loop
return output