B1 = np.concatenate((B, np.zeros((Cr.shape[0], B.shape[1]))), 0)
int_pole = -4.0 * wn_th
cl_poles.append(int_pole)
K1 = ctrl.acker(A1, B1, cl_poles)
ki = K1.take([-1]).item(0)
K1 = K1.take(range(0, K1.shape[1] - 1))
# -- OBSERVER -- #
zeta_o = 0.707
wn_o_z = 4.0 * wn_z
wn_o_th = 4.0 * wn_th
obs_poles = list(np.roots([1, 2 * zeta_o * wn_o_z, wn_o_z**2])) + list(
np.roots([1, 2 * zeta_o * wn_o_th, wn_o_th**2]))
L = place(A.T, C.T, obs_poles).gain_matrix.T
# Add physical parameter error
input_disturbance = 0.5
uncertainty = 0.0
m1 += m1 * uncertainty * (2 * random() - 1)
m2 += m2 * uncertainty * (2 * random() - 1)
l += l * uncertainty * (2 * random() - 1)
if __name__ == '__main__':
print '-- PID --'
print ' Z:'
print ' kp = %f' % kp_z
print ' kd = %f' % kd_z
print ' ki = %f' % ki_z
print ' Theta:'
print('K1_long: ', K1_long)
print('K_long: ', K_long)
print('ki_long: ', ki_long)
###### OBSERVER #############
# Observer design
# obs_th_wn = 5.0*th_wn
obs_th_wn = 8.0 * th_wn
obs_des_char_poly_long = [1, 2.0 * th_zeta * obs_th_wn, obs_th_wn**2]
obs_des_poles_long = np.roots(obs_des_char_poly_long)
if np.linalg.matrix_rank(cnt.obsv(A_long, C_long)) != 2:
print("The longitudinal system is not Observable")
else:
print 'Long Observable'
L_long = place(A_long.T, C_long.T, obs_des_poles_long).gain_matrix.T
print('L_long:', L_long)
# K1_long = cnt.acker(A1_long,B1_long,des_poles_long)
# K_long = K1_long[0,0:2]
# ki_long = K1_long[0,2]
# print('K1_long: ', K1_long)
# print('K_long: ', K_long)
# print('ki_long: ', ki_long)
###############################################
############ Lateral ##########################
###############################################
A_lat = np.matrix(
[[0.0, 0.0, 1.0, 0.0], [0.0, 0.0, 0.0, 1.0], [0.0, 0.0, 0.0, 0.0],
[l1 * F_eq / (m1 * (l1**2) + m2 * (l2**2) + Jz), 0.0, 0.0, 0.0]])
K1_lat = ctrl.acker(A1_lat, B1_lat, cl_poles_lat)
K_lat = K1_lat[0,:4]
ki_lat = K1_lat[0,4]
# kr_lat = -1.0 / (Cr_lat * (np.linalg.inv(A_lat - B_lat*K_lat) * B_lat)).item(0)
# -------------------------------------------------------- #
# OBSERVER
# -------------------------------------------------------- #
zeta_o = 0.707
wn_oth = 5.0*wn_th
wn_ophi= 5.0*wn_phi
wn_opsi= 5.0*wn_psi
obs_poles_lat = list(np.roots([1,2*zeta_o*wn_ophi,wn_ophi**2])) \
+ list(np.roots([1,2*zeta_o*wn_opsi,wn_opsi**2]))
L_lat = place(A_lat.T, C_lat.T, obs_poles_lat).gain_matrix.T
obs_poles_lon = list(np.roots([1,2*zeta_o*wn_oth,wn_oth**2]))
L_lon = place(A_lon.T, C_lon.T, obs_poles_lon).gain_matrix.T
if __name__ == '__main__':
print '-- PID --'
print ' Theta: kp = %s, kd = %s' % (kp_th, kd_th)
print ' Phi: kp = %s, kd = %s' % (kp_phi, kd_phi)
print ' Psi: kp = %s, kd = %s' % (kp_psi, kd_psi)
print '-- FULL STATE --'
print ' theta poles = {:.3f}, {:.3f}'.format(*cl_poles_lon)
print ' K_lon = %s' % K_lon
print ' ki_lon = %s' % ki_lon
print ' phi poles = {:.3f}, {:.3f}'.format(*cl_poles_lat[:2])
print ' psi poles = {:.3f}, {:.3f}'.format(*cl_poles_lat[2:])
while np.sum(abs(state)) > 0.1:
#Define desired vehicle response model
xd = r * np.sin(rate * t) #desired x position
yd = r - r * np.cos(rate * t) #desired y position
dxd = r * rate * np.cos(rate * t) #desired x velo
dyd = r * rate * np.sin(rate * t) #desired y velo
ad = atan2(dyd, dxd) #desired angular position
#State-variable form after linearization about an equilibrium point (i forgot which one):
#The equilibrium point would likely be at 0 error and 0 input.
A = np.array(
[[0, 0, -vd * np.sin(ad)], [0, 0, vd * np.cos(ad)], [0, 0, 0]],
dtype=float)
B = np.array([[np.cos(ad), 0], [np.sin(ad), 0], [0, 1]], dtype=float)
poles = [p * rate for p in [1, 0.6, 0.8]]
K = place(A, B, poles, method='KNV0').gain_matrix
u = np.matmul(K, state)
#Kalman a-priori estimate
dstate = np.matmul(A, state) + np.matmul(B, u)
state = state + dstate
P = np.matmul(A, np.matmul(P,
np.transpose(A))) + N #covariance of the system
#Kalman a-posteriori estimate
noise = np.array([np.random.normal(0, [nx, ny, na])], dtype=float).T
sensor = np.matmul(H, state) + noise #sensor measurement
Q1 = np.matmul(H, np.matmul(P, np.transpose(H))) + N
Q2 = np.matmul(P, np.matmul(np.transpose(H),
np.linalg.inv(Q1))) #placeholder calculations