def kalmd(sys, Q, R):
"""Solves for the steady state kalman gain and error covariance matrices.
Keyword arguments:
sys -- discrete state-space model
Q -- process noise covariance matrix
R -- measurement noise covariance matrix
Returns:
Kalman gain, error covariance matrix.
"""
m = sys.A.shape[0]
observability_rank = np.linalg.matrix_rank(cnt.obsv(sys.A, sys.C))
if observability_rank != m:
print("Warning: Observability of %d != %d, unobservable state",
observability_rank, m)
# Compute the steady state covariance matrix
P_prior = sp.linalg.solve_discrete_are(a=sys.A.T, b=sys.C.T, q=Q, r=R)
S = sys.C * P_prior * sys.C.T + R
K = P_prior * sys.C.T * np.linalg.inv(S)
P = (np.eye(m) - K * sys.C) * P_prior
return K, P
def _reduce_state_space(self):
x_al = self.fsf_dict['x_al']
x_sp_al = self.fsf_dict['x_sp_al']
x_er_al = self.fsf_dict['x_er_al']
u_al = self.fsf_dict['u_al']
u_sp_al = self.fsf_dict['u_sp_al']
u_er_al = self.fsf_dict['u_er_al']
al_sys = self.fsf_dict['al_sys']
#Construct reduced state space
x_ral = x_al[7:]
x_sp_ral = x_sp_al[7:]
x_er_ral = x_er_al[7:]
u_ral = u_al[np.ix_([0,1,3,4])]
u_sp_ral = u_sp_al[np.ix_([0,1,3,4])]
u_er_ral = u_er_al[np.ix_([0,1,3,4])]
A = al_sys.A
B = al_sys.B
C = al_sys.C
D = al_sys.D
A_ral = A[np.ix_([7,8,9,10,11,12],[7,8,9,10,11,12])]
B_ral = B[np.ix_([7,8,9,10,11,12],[0,1,3,4])]
C_ral = C[np.ix_([7,8,9,10,11,12],[7,8,9,10,11,12])]
D_ral = D[np.ix_([7,8,9,10,11,12],[0,1,3,4])]
#Lateral and longitudinal state spaces
ral_sys = control.StateSpace(A_ral, B_ral, C_ral, D_ral)
ral_ctrb = control.ctrb(ral_sys.A, ral_sys.B)
ral_obsv = control.obsv(ral_sys.A, ral_sys.C)
#Update FSF dictionary
self.fsf_dict.update(x_ral = x_ral)
self.fsf_dict.update(x_sp_ral = x_sp_ral)
self.fsf_dict.update(x_er_ral = x_er_ral)
self.fsf_dict.update(u_ral = u_ral)
self.fsf_dict.update(u_sp_ral = u_sp_ral)
self.fsf_dict.update(u_er_ral = u_er_ral)
self.fsf_dict.update(ral_sys = ral_sys)
def kalmd(A, C, Q, R): """Solves for the steady state kalman gain and error covariance matrices. Keyword arguments: sys -- discrete state-space model Q -- process noise covariance matrix R -- measurement noise covariance matrix Returns: Kalman gain, error covariance matrix. """ m = A.shape[0] observability_rank = np.linalg.matrix_rank(cnt.obsv(A, C)) if observability_rank != m: print( "Warning: Observability of %d != %d, unobservable state" % (observability_rank, m) ) # Compute the steady state covariance matrix # P_prior = sp.linalg.solve_discrete_are(a=A.T, b=C.T, q=Q, r=R) P_prior = np.array([[R]]) S = C * P_prior * C.T + R K = P_prior * C.T * np.linalg.inv(S) P = (np.eye(m) - K * C) * P_prior print(str(P_prior[0, 0]) + " " + str(S[0, 0]) + " " + str(P[0, 0]) + " " + str(K[0, 0])) return K, P
def regulator_with_observer(sys,
poles=None,
po=None,
ts=None,
extra_poles=[],
ck=[]):
final_poles = []
if poles is not None:
final_poles = poles
if po is not None and ts is not None:
log_po = np.log(100 / po)
psi = log_po / np.sqrt(np.pi**2 + log_po**2)
wn = 4 / psi / ts
s1 = -psi * wn + 1j * wn * np.sqrt(1 - psi**2)
s2 = -psi * wn - 1j * wn * np.sqrt(1 - psi**2)
final_poles.append(s1)
final_poles.append(s2)
for p in extra_poles:
final_poles.append(p)
rank = np.linalg.matrix_rank(ct.ctrb(sys.A, sys.B))
total_poles = len(final_poles)
if total_poles < rank:
raise BaseException(
f"you have {total_poles} but you need {rank} to implement a FSFB")
k = ct.acker(sys.A, sys.B, final_poles)
mo = ct.obsv(sys.A, sys.C)
obs_rank = np.linalg.matrix_rank(mo)
print("obs_rank=", obs_rank)
final_poles_obs = 5 * final_poles
L = ct.acker(sys.A.T, sys.C.T, final_poles_obs)
L = L.T
print("")
print("L=")
print(L)
pa = np.concatenate([sys.A, -sys.B * k], axis=1)
pb = np.concatenate([L * sys.C, sys.A - sys.B * k - L * sys.C], axis=1)
alc = np.concatenate([pa, pb], axis=0)
blc = np.concatenate(
[np.zeros((sys.A.shape[0], 1)),
np.zeros((sys.A.shape[0], 1))])
clc = np.concatenate([sys.C, -sys.D * k], axis=1)
dlc = np.array([[0]])
return ct.ss(alc, blc, clc, dlc)
def __isObservable(self, th=None):
if th is None:
th = self.__th
F = np.array(self.__F(th))
C = np.array(self.__C(th))
n = self.__n
obsv_matrix = control.obsv(F, C)
rank = np.linalg.matrix_rank(obsv_matrix)
return rank == n
def full_observer(system, poles_o, poles_s, debug=False) -> ObserverResult:
"""implementation of the complete observer for an autonomous system
:param system : tuple (a, b, c) of system matrices
:param poles_o: tuple of complex poles/eigenvalues of the observer
dynamics
:param poles_s: tuple of complex poles/eigenvalues of the closed
system
:param debug: output control for debugging in unittest(False:normal
output,True: output local variables and normal output)
:return: dataclass ObserverResult (controller function, feedback matrix, observer_gain, and pre-filter)
"""
# systemically relevant matrices
a = system[0]
b = system[1]
c = system[2]
d = system[3]
# ignore the PendingDeprecationWarning for built-in packet control
warnings.filterwarnings('ignore', category=PendingDeprecationWarning)
ctr_matrix = ctr.ctrb(a, b) # controllabilty matrix
assert np.linalg.det(ctr_matrix) != 0, 'this system is not controllable'
# State feedback
f_t = ctr.acker(a, b, poles_s)
# pre-filter
a1 = a - b * f_t
v = (-1 * (c * a1**(-1) * b)**(-1)) # pre-filter
obs_matrix = ctr.obsv(a, c) # observability matrix
assert np.linalg.det(obs_matrix) != 0, 'this system is unobservable'
# calculate observer gain
l_v = ctr.acker(a.T, c.T, poles_o).T
# controller function
# in this case controller function is: -1 * feedback * states + pre-filter * reference output
# disturbance value ist not considered
# t = sp.Symbol('t')
# sys_input = -1 * (f_t * sys_state)[0]
# input_func = sp.lambdify((sys_state, t), sys_input, modules='numpy')
# this method returns controller function, feedback matrix, observer gain and pre-filter
result = ObserverResult(f_t, l_v, v, None)
# return internal variables for unittest
if debug:
c_locals = Container(fetch_locals=True)
result.debug = c_locals
return result
def feed_forward(self, A, B, C, D, K):
"""creates the feed forward matrix F to compute
control inputs using a desired state"""
F = np.linalg.pinv(-C * np.linalg.inv(A - B * K) * B)
if np.linalg.matrix_rank(control.ctrb(A - B * K, B * F)) < A.shape[0]:
print "System is not fully controllable"
else:
print "System is fully controllable"
if np.linalg.matrix_rank(control.obsv(A - B * K,
C - D * K)) < A.shape[0]:
print "System is not fully observable"
else:
print "System is fully observable"
return F
def kalmd(sys, Q, R):
"""Solves for the steady-state kalman gain matrix.
Keyword arguments:
sys -- discrete state-space model
Q -- process noise covariance matrix
R -- measurement noise covariance matrix
Returns:
K -- numpy.array(states x outputs), Kalman gain matrix.
"""
if np.linalg.matrix_rank(ct.obsv(sys.A, sys.C)) < sys.A.shape[0]:
print(
f"Warning: The system is unobservable\n\nA = {sys.A}\nC = {sys.C}\n"
)
P = sp.linalg.solve_discrete_are(a=sys.A.T, b=sys.C.T, q=Q, r=R)
S = sys.C @ P @ sys.C.T + R
return np.linalg.solve(S.T, sys.C @ P.T).T
def design_regredob(sys_c_ol,
sampling_interval,
desired_settling_time,
desired_observer_settling_time=None,
spoles=None,
sopoles=None,
disp=True):
""" Design a digital reduced order observer regulator with the desired settling time.
Args:
sys_c_ol (StateSpace): The continouous plant model
sampling_interval: The sampling interval for the digital control system in seconds.
desired_settling_time: The desired settling time in seconds
desired_observer_settling_time (optional): The desired observer settling time
in seconds. If not provided the observer settling time will be 4 times faster
than the overall settling time. Default is None.
spoles (optional): The desired closed loop poles. If not supplied, then optimal
poles will try to be used. Default is None.
sopoles (optional): The desired observer poles. If not supplied, then optimal
poles will try to be used. Default is None.
disp: Print debugging output. Default is True.
Returns:
tuple: (sys_d_ol, L1, L2, K, F, G, H) Where sys_d_ol is the discrete plant, L is the stablizing
gain matrix, and K is the observer gain matrix.
"""
# Make sure the system is in fact continuous and not discrete
if sys_c_ol.dt != None:
print("Error: Function expects continuous plant")
return None
A = sys_c_ol.A
B = sys_c_ol.B
C = sys_c_ol.C
D = sys_c_ol.D
num_states = A.shape[0]
num_inputs = B.shape[1]
num_outputs = C.shape[0]
num_measured_states = num_outputs
num_unmeasured_states = num_states - num_measured_states
# Convert to discrete system using zero order hold method
sys_d_ol = sys_c_ol.to_discrete(sampling_interval, method="zoh")
phi = sys_d_ol.A
gamma = sys_d_ol.B
# Check controlability of the discrete system
controllability_mat = control.ctrb(phi, gamma)
rank = LA.matrix_rank(controllability_mat)
if rank != num_states:
print(rank, num_states)
print("Error: System is not controlable")
return None
# Check observability of the discrete system
observability_mat = control.obsv(phi, C)
rank = LA.matrix_rank(observability_mat)
if rank != num_states:
print("Error: System is not observable")
return None
# Choose poles if none were given
if spoles is None:
spoles = find_candadate_spoles(sys_c_ol, desired_settling_time, disp)
num_spoles_left = num_states - len(spoles)
if num_spoles_left > 0:
# Use normalized bessel poles for the rest
spoles.extend(
control_poles.bessel_spoles(num_spoles_left,
desired_settling_time))
zpoles = control_poles.spoles_to_zpoles(spoles, sampling_interval)
if disp:
print("spoles = ", spoles)
print("zpoles = ", zpoles)
# place the poles such that eig(phi - gamma*L) are inside the unit circle
full_state_feedback = signal.place_poles(phi, gamma, zpoles)
# Check the poles for stability just in case
for zpole in full_state_feedback.computed_poles:
if abs(zpole) >= 1:
print("Computed pole is not stable")
return None
L = full_state_feedback.gain_matrix
# Choose poles if none were given
if sopoles is None:
sopoles = []
if desired_observer_settling_time == None:
desired_observer_settling_time = desired_settling_time / 4
# TODO: Find existing poles based on the rules. For now just use bessel
num_sopoles_left = num_unmeasured_states - len(sopoles)
if num_sopoles_left > 0:
# Use normalized bessel poles for the rest
sopoles.extend(
control_poles.bessel_spoles(num_sopoles_left,
desired_observer_settling_time))
if disp:
print("Using normalized bessel for the remaining",
num_sopoles_left, "sopoles")
zopoles = control_poles.spoles_to_zpoles(sopoles, sampling_interval)
if disp:
print("sopoles = ", sopoles)
print("zopoles = ", zopoles)
# partition out the phi and gamma matrix
phi11 = phi[:num_measured_states, :num_measured_states]
phi12 = phi[:num_measured_states, num_measured_states:]
phi21 = phi[num_measured_states, :num_measured_states]
phi22 = phi[num_measured_states:, num_measured_states:]
gamma1 = gamma[:num_measured_states]
gamma2 = gamma[num_measured_states:]
C1 = C[:num_measured_states, :num_measured_states]
if num_measured_states >= num_states / 2 and LA.matrix_rank(
phi12) == num_unmeasured_states:
# case 1
if num_unmeasured_states % 2 == 0:
F = np.matrix([[zopoles[0].real, zopoles[0].imag],
[zopoles[1].imag, zopoles[1].real]])
else:
# We only support 1 real pole
F = np.matrix([zopoles[0].real])
cp = C1 * phi12
cp_t = np.transpose(cp)
K = (phi22 - F) * np.linalg.inv(cp_t * cp) * cp_t
elif num_measured_states == 1:
# case 2 (unsupported)
print("unsupported design with measured states = 1")
np.poly(np.eig(phi22))
else:
full_state_feedback = signal.place_poles(np.transpose(phi22),
np.transpose(C1 * phi12),
zopoles)
K = np.transpose(full_state_feedback.gain_matrix)
F = phi22 - K * C1 * phi12
H = gamma2 - K * C1 * gamma1
G = (phi21 - K * C1 * phi11) * np.linalg.inv(C1) + (F * K)
# Check the poles for stability just in case
for zopole in full_state_feedback.computed_poles:
if abs(zopole) > 1:
print("Computed observer pole is not stable")
return None
return (sys_d_ol, np.matrix(L), np.matrix(K), np.matrix(F), np.matrix(G),
np.matrix(H))
def design_regob(sys_c_ol,
sampling_interval,
desired_settling_time,
desired_observer_settling_time=None,
spoles=None,
sopoles=None,
disp=True):
""" Design a digital full order observer regulator with the desired settling time.
Args:
sys_c_ol (StateSpace): The continouous plant model
sampling_interval: The sampling interval for the digital control system in seconds.
desired_settling_time: The desired settling time in seconds
desired_observer_settling_time (optional): The desired observer settling time
in seconds. If not provided the observer settling time will be 4 times faster
than the overall settling time. Default is None.
spoles (optional): The desired closed loop poles. If not supplied, then optimal
poles will try to be used. Default is None.
sopoles (optional): The desired observer poles. If not supplied, then optimal
poles will try to be used. Default is None.
disp: Print debugging output. Default is True.
Returns:
tuple: (sys_d_ol, L, K) Where sys_d_ol is the discrete plant, L is the stablizing
gain matrix, and K is the observer gain matrix.
"""
# Make sure the system is in fact continuous and not discrete
if sys_c_ol.dt != None:
print("Error: Function expects continuous plant")
return None
A = sys_c_ol.A
B = sys_c_ol.B
C = sys_c_ol.C
D = sys_c_ol.D
num_states = A.shape[0]
num_inputs = B.shape[1]
num_outputs = C.shape[0]
# Convert to discrete system using zero order hold method
sys_d_ol = sys_c_ol.to_discrete(sampling_interval, method="zoh")
phi = sys_d_ol.A
gamma = sys_d_ol.B
# Check controlability of the discrete system
controllability_mat = control.ctrb(phi, gamma)
rank = LA.matrix_rank(controllability_mat)
if rank != num_states:
print(rank, num_states)
print("Error: System is not controlable")
return None
# Check observability of the discrete system
observability_mat = control.obsv(phi, C)
rank = LA.matrix_rank(observability_mat)
if rank != num_states:
print("Error: System is not observable")
return None
# Choose poles if none were given
if spoles is None:
spoles = find_candadate_spoles(sys_c_ol, desired_settling_time, disp)
num_spoles_left = num_states - len(spoles)
if num_spoles_left > 0:
# Use normalized bessel poles for the rest
spoles.extend(
control_poles.bessel_spoles(num_spoles_left,
desired_settling_time))
zpoles = control_poles.spoles_to_zpoles(spoles, sampling_interval)
if disp:
print("spoles = ", spoles)
print("zpoles = ", zpoles)
# place the poles such that eig(phi - gamma*L) are inside the unit circle
full_state_feedback = signal.place_poles(phi, gamma, zpoles)
# Check the poles for stability just in case
for zpole in full_state_feedback.computed_poles:
if abs(zpole) >= 1:
print("Computed pole is not stable")
return None
L = full_state_feedback.gain_matrix
# Choose poles if none were given
if sopoles is None:
sopoles = []
if desired_observer_settling_time == None:
desired_observer_settling_time = desired_settling_time / 4
# TODO: Find existing poles based on the rules. For now just use bessel
num_sopoles_left = num_states - len(sopoles)
if num_sopoles_left > 0:
# Use normalized bessel poles for the rest
sopoles.extend(
control_poles.bessel_spoles(num_sopoles_left,
desired_observer_settling_time))
if disp:
print("Using normalized bessel for the remaining",
num_sopoles_left, "sopoles")
zopoles = control_poles.spoles_to_zpoles(sopoles, sampling_interval)
if disp:
print("sopoles = ", sopoles)
print("zopoles = ", zopoles)
# Find K such that eig(phi - KC) are inside the unit circle
full_state_feedback = signal.place_poles(np.transpose(phi),
np.transpose(C), zopoles)
# Check the poles for stability just in case
for zopole in full_state_feedback.computed_poles:
if abs(zopole) > 1:
print("Computed observer pole is not stable")
return None
K = np.transpose(full_state_feedback.gain_matrix)
return (sys_d_ol, np.matrix(L), np.matrix(K))
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):
A = sys.A
B = sys.B
C = sys.C
dt = sys.dt
sim_Time = 8
t = np.linspace(0, sim_Time, sim_Time / dt)
Overshoot_LQR = performance_specs[0]
settle_LQR = performance_specs[1]
Overshoot_KF = performance_specs[2]
print('K: ', K)
print('ki: ', ki)
####################################################
# Observer
####################################################
# Observer design
obs_th_wn = 5.0*th_wn
obs_z_wn = 5.0*z_wn
obs_des_char_poly = np.convolve([1,2.0*z_zeta*obs_z_wn,obs_z_wn**2],
[1,2.0*th_zeta*obs_th_wn,obs_th_wn**2])
obs_des_poles = np.roots(obs_des_char_poly)
if np.linalg.matrix_rank(cnt.obsv(A,C))!=4:
print('System Not Observable')
else:
L = place(A.T,C.T,obs_des_poles).gain_matrix.T
print('L:', L)
#################################################
# Uncertainty Parameters
#################################################
UNCERTAINTY_PARAMETERS = True
if UNCERTAINTY_PARAMETERS:
alpha = 0.0 # Uncertainty parameter
m1 = m1*(1+2*alpha*np.random.rand()-alpha) # Mass of the pendulum, kg
m2 = m2*(1+2*alpha*np.random.rand()-alpha) # Length of the rod, m
l = l*(1+2*alpha*np.random.rand()-alpha) # Damping coefficient, Ns
def testConvert(self):
"""Test state space to transfer function conversion."""
verbose = self.debug
from control.statesp import _mimo2siso
#print __doc__
# Machine precision for floats.
eps = np.finfo(float).eps
for states in range(1, self.maxStates):
for inputs in range(1, self.maxIO):
for outputs in range(1, self.maxIO):
# start with a random SS system and transform to TF then
# back to SS, check that the matrices are the same.
ssOriginal = matlab.rss(states, outputs, inputs)
if (verbose):
self.printSys(ssOriginal, 1)
# Make sure the system is not degenerate
Cmat = control.ctrb(ssOriginal.A, ssOriginal.B)
if (np.linalg.matrix_rank(Cmat) != states):
if (verbose):
print(" skipping (not reachable)")
continue
Omat = control.obsv(ssOriginal.A, ssOriginal.C)
if (np.linalg.matrix_rank(Omat) != states):
if (verbose):
print(" skipping (not observable)")
continue
tfOriginal = matlab.tf(ssOriginal)
if (verbose):
self.printSys(tfOriginal, 2)
ssTransformed = matlab.ss(tfOriginal)
if (verbose):
self.printSys(ssTransformed, 3)
tfTransformed = matlab.tf(ssTransformed)
if (verbose):
self.printSys(tfTransformed, 4)
# Check to see if the state space systems have same dim
if (ssOriginal.states != ssTransformed.states):
print("WARNING: state space dimension mismatch: " + \
"%d versus %d" % \
(ssOriginal.states, ssTransformed.states))
# Now make sure the frequency responses match
# Since bode() only handles SISO, go through each I/O pair
# For phase, take sine and cosine to avoid +/- 360 offset
for inputNum in range(inputs):
for outputNum in range(outputs):
if (verbose):
print("Checking input %d, output %d" \
% (inputNum, outputNum))
ssorig_mag, ssorig_phase, ssorig_omega = \
control.bode(_mimo2siso(ssOriginal, \
inputNum, outputNum), \
deg=False, Plot=False)
ssorig_real = ssorig_mag * np.cos(ssorig_phase)
ssorig_imag = ssorig_mag * np.sin(ssorig_phase)
#
# Make sure TF has same frequency response
#
num = tfOriginal.num[outputNum][inputNum]
den = tfOriginal.den[outputNum][inputNum]
tforig = control.tf(num, den)
tforig_mag, tforig_phase, tforig_omega = \
control.bode(tforig, ssorig_omega, \
deg=False, Plot=False)
tforig_real = tforig_mag * np.cos(tforig_phase)
tforig_imag = tforig_mag * np.sin(tforig_phase)
np.testing.assert_array_almost_equal( \
ssorig_real, tforig_real)
np.testing.assert_array_almost_equal( \
ssorig_imag, tforig_imag)
#
# Make sure xform'd SS has same frequency response
#
ssxfrm_mag, ssxfrm_phase, ssxfrm_omega = \
control.bode(_mimo2siso(ssTransformed, \
inputNum, outputNum), \
ssorig_omega, \
deg=False, Plot=False)
ssxfrm_real = ssxfrm_mag * np.cos(ssxfrm_phase)
ssxfrm_imag = ssxfrm_mag * np.sin(ssxfrm_phase)
np.testing.assert_array_almost_equal( \
ssorig_real, ssxfrm_real)
np.testing.assert_array_almost_equal( \
ssorig_imag, ssxfrm_imag)
#
# Make sure xform'd TF has same frequency response
#
num = tfTransformed.num[outputNum][inputNum]
den = tfTransformed.den[outputNum][inputNum]
tfxfrm = control.tf(num, den)
tfxfrm_mag, tfxfrm_phase, tfxfrm_omega = \
control.bode(tfxfrm, ssorig_omega, \
deg=False, Plot=False)
tfxfrm_real = tfxfrm_mag * np.cos(tfxfrm_phase)
tfxfrm_imag = tfxfrm_mag * np.sin(tfxfrm_phase)
np.testing.assert_array_almost_equal( \
ssorig_real, tfxfrm_real)
np.testing.assert_array_almost_equal( \
ssorig_imag, tfxfrm_imag)
def obs(self):
# whether the system is observable
import control
obs = control.obsv(self.A, self.C)
rank = np.linalg.matrix_rank(obs)
print("rank", rank, "n", self.n)
def observable(self):
O_mat = control.obsv(self.F, self.H)
d = numpy.linalg.matrix_rank(O_mat)
return d
g = 9.8 # gravity
d = 4.0 # dampping term
#############################
# Linearized system and place pole
A = np.array([[0, 1, 0, 0], [0, -d, -(g * m) / M, 0], [0, 0, 0, 1],
[0, 0, (g * m + M * g) / (L * M), -0.1 * d]],
dtype=np.float)
B = np.array([[0], [1.0 / M], [0], [-1.0 / (L * M)]], dtype=np.float)
C = np.array(
[1, 0, 0, 0], dtype=np.float
) # only observable if x measured... because x can't be reconstructed
O = control.obsv(A, C)
print("Obsv A,C: ", O)
print("rank(obsv): ", np.linalg.matrix_rank(O))
print('----------')
print('Which measurements are best if we omit "x"')
A = A[1:, 1:]
B = B[1:]
C = np.array([1, 0, 0], dtype=np.float)
print('C: ', C)
O = control.obsv(A, C)
print("Obsv A,C: ", O)
print("rank(obsv): ", np.linalg.matrix_rank(O))
C = np.array([0, 1, 0], dtype=np.float)
def check_ctrb_obsv(A, B, C):
ctrb_matrix = ctrb(A, B.T)
obsv_matrix = obsv(A, C)
print("Controllability matrix rank: %d" % matrix_rank(ctrb_matrix))
print("Observability matrix rank: %d" % matrix_rank(obsv_matrix))
print np.matrix(B) A_eig = list(A.eigenvals().keys()) for i in range(0, len(A_eig)): A_eig[i]=sp.N(A_eig[i]) print "A_eig:=" print np.matrix(A_eig) # Controllability control_matrix = co.ctrb(A, B) control_matrix = sp.Matrix(control_matrix) # Observability: observe_matrix = co.obsv(A, C) observe_matrix = sp.Matrix(observe_matrix) print "Rank of control_matrix: "+str(control_matrix.rank()) print "Rank of observe_matrix: "+str(observe_matrix.rank()) ## tODO this is a problem!? # pole wish position: poles_wish = [-6, -6, -6, -6, -6, -6, -6, -6] A = np.matrix(A) B = np.matrix(B) C = np.matrix(C) A = A.astype(float) B = B.astype(float) C = C.astype(float)
import numpy as np
from numpy.linalg import matrix_rank
from control import ctrb, obsv
g = 9.8
L = 0.43 # pendulum length
d = 0.01 # friction coefficent
A = np.matrix([[.0, 1., .0, .0], [-g / L, -d / L, .0, .0], [.0, .0, .0, 1.],
[.0, .0, .0, .0]])
B = np.matrix([[.0, 1. / L, .0, 1.]])
C = np.matrix([[1.0, .0, 1., .0]])
ctrb_matrix = ctrb(A, np.transpose(B))
obsv_matrix = obsv(A, C)
print "Ctrb matrix rank: %d" % matrix_rank(ctrb_matrix)
print "Obsv matrix rank: %d" % matrix_rank(obsv_matrix)
def linearize_matricies(self, x, u, dt):
"""creates discritized linearized F,G,H & M matricies
Inputs:
x (array_like): linearization point-states
u (array_like): linearization point-inputs
dt (float): discrete interval
"""
dxdotdot_dxdot = -(self.A_x * self.Cd_x * self.rho * x[1]) / self.m
dxdotdot_dtheta = -self.g * cos(x[10]) * cos(x[8]) + (
(self.Fb * cos(x[10]) * cos(x[8])) / self.m)
dxdotdot_dpsi = self.g * sin(x[10]) * sin(x[8]) - (
(self.Fb * sin(x[10]) * sin(x[8])) / self.m)
dxdotdot_dTL = 1 / self.m
dxdotdot_dTR = 1 / self.m
dydotdot_dydot = -(self.A_y * self.Cd_y * self.rho * x[3]) / self.m
dydotdot_dphi = self.g * cos(x[6]) * cos(x[10]) - (
(self.Fb * cos(x[10]) * cos(x[6])) / self.m)
dydotdot_dpsi = -self.g * sin(x[6]) * sin(x[10]) + (
(self.Fb * sin(x[10]) * sin(x[6])) / self.m)
dydotdot_dTF = 1 / self.m
dydotdot_dTB = 1 / self.m
dzdotdot_dzdot = -(self.A_z * self.Cd_z * self.rho * x[5]) / self.m
dzdotdot_dphi = -self.g * cos(x[8]) * sin(x[6]) + (
(self.Fb * cos(x[8]) * sin(x[6])) / self.m)
dzdotdot_dtheta = -self.g * cos(x[6]) * sin(x[8]) + (
(self.Fb * cos(x[6]) * sin(x[8])) / self.m)
dzdotdot_dTFL = 1 / self.m
dzdotdot_dTFR = 1 / self.m
dzdotdot_dTBL = 1 / self.m
dzdotdot_dTBR = 1 / self.m
dphidotdot_dydot=-.5*self.Ixx*self.rho*(2*self.A_yB*self.Cd_yB*self.dz_yB* \
(self.dz_yB*x[7]+x[3])+2*self.A_yT*self.Cd_yT*self.dz_yT*(self.dz_yT*x[7]+x[3]))
dphidotdot_dzdot=-.5*self.Ixx*self.rho*(2*self.A_zL*self.Cd_zL*self.dy_zL* \
(self.dy_zL*x[7]+x[5])+2*self.A_zR*self.Cd_zR*self.dy_zR*(self.dy_zR*x[7]+x[5]))
dphidotdot_dphi = (self.Fb /
self.Ixx) * (self.phi_bz * cos(x[6]) * cos(x[8]) +
self.phi_by * cos(x[8]) * sin(x[6]))
dphidotdot_dphidot=-.5*self.Ixx*self.rho*(2*self.A_yB*self.Cd_yB*self.dz_yB**2* \
(self.dz_yB*x[7]+x[3])+2*self.A_yT*self.Cd_yT*self.dz_yT**2*(self.dz_yT*x[7]+x[3])+ \
2*self.A_zL*self.Cd_zL*self.dy_zL**2*(self.dy_zL*x[7]+x[5])+2*self.A_zR*self.Cd_zR* \
self.dy_zR**2*(self.dy_zR*x[7]+x[5]))
dphidotdot_dtheta = (self.Fb /
self.Ixx) * (self.phi_by * cos(x[6]) * sin(x[8]) -
self.phi_bz * sin(x[6]) * sin(x[8]))
dphidotdot_dTFL = -self.phi_L / self.Ixx
dphidotdot_dTFR = self.phi_R / self.Ixx
dphidotdot_dTBL = -self.phi_L / self.Ixx
dphidotdot_dTBR = self.phi_R / self.Ixx
dthetadotdot_dxdot=-.5*self.Iyy*self.rho*(2*self.A_xB*self.Cd_xB*self.dz_xB* \
(self.dz_xB*x[9]+x[1])+2*self.A_xT*self.Cd_xT*self.dz_xT*(self.dz_xT*x[9]+x[1]))
dthetadotdot_dzdot=-.5*self.Iyy*self.rho*(2*self.A_zB*self.Cd_zB*self.dx_zB* \
(self.dx_zB*x[9]+x[5])+2*self.A_zF*self.Cd_zF*self.dx_zF*(self.dx_zF*x[9]+x[5]))
dthetadotdot_dphi = (-self.Fb / self.Iyy) * (self.theta_bx *
cos(x[8]) * sin(x[6]))
dthetadotdot_dtheta = (self.Fb / self.Iyy) * (
self.theta_bz * cos(x[8]) - self.theta_bx * cos(x[6]) * sin(x[8]))
dthetadotdot_dthetadot=-.5*self.Iyy*self.rho*(2*self.A_xB*self.Cd_xB*self.dz_xB**2* \
(self.dz_xB*x[9]+x[1])+2*self.A_xT*self.Cd_xT*self.dz_xT**2*(self.dz_xT*x[9]+x[1])+ \
2*self.A_zB*self.Cd_zB*self.dx_zB**2*(self.dx_zB*x[9]+x[5])+2*self.A_zF*self.Cd_zF* \
self.dx_zF**2*(self.dx_zF*x[9]+x[5]))
dthetadotdot_dTFL = self.theta_F / self.Iyy
dthetadotdot_dTFR = self.theta_F / self.Iyy
dthetadotdot_dTBL = -self.theta_B / self.Iyy
dthetadotdot_dTBR = -self.theta_B / self.Iyy
dpsidotdot_dxdot=-.5*self.Izz*self.rho*(2*self.A_xL*self.Cd_xL*self.dy_xL* \
(self.dy_xL*x[11]+x[1])+2*self.A_xR*self.Cd_xR*self.dy_xR*(self.dy_xR*x[11]+x[1]))
dpsidotdot_dydot=-.5*self.Izz*self.rho*(2*self.A_yBa*self.Cd_yBa*self.dx_yBa* \
(self.dx_yBa*x[11]+x[3])+2*self.A_yF*self.Cd_yF*self.dx_yF*(self.dx_yF*x[11]+x[3]))
dpsidotdot_dphi = (self.Fb / self.Izz) * (self.psi_bx * cos(x[8]) *
sin(x[6]))
dpsidotdot_dtheta = (self.Fb / self.Izz) * (
-self.psi_by * cos(x[8]) - self.psi_bx * sin(x[6]) * sin(x[8]))
dpsidotdot_dpsidot=-.5*self.Izz*self.rho*(2*self.A_xL*self.Cd_xL*self.dy_xL**2* \
(self.dy_xL*x[11]+x[1])+2*self.A_xR*self.Cd_xR*self.dy_xR**2*(self.dy_xR*x[11]+x[1])+ \
2*self.A_yBa*self.Cd_yBa*self.dx_yBa**2*(self.dx_yBa*x[11]+x[3])+2*self.A_yF*self.Cd_yF* \
self.dx_yF**2*(self.dx_yF*x[11]+x[3]))
dpsidotdot_dTL = self.psi_L / self.Izz
dpsidotdot_dTR = -self.psi_R / self.Izz
dpsidotdot_dTF = self.psi_F / self.Izz
dpsidotdot_dTB = -self.psi_B / self.Izz
# A should be a 12x12
# B should be a 12x8
# C should be a 10x12 (12x12 with DVL)
# D should be a 10x8 (12x8 with DVL)
A = np.array([[0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0],
[
0, dxdotdot_dxdot, 0, 0, 0, 0, 0, 0, dxdotdot_dtheta,
0, dxdotdot_dpsi, 0
], [0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0],
[
0, 0, 0, dydotdot_dydot, 0, 0, dydotdot_dphi, 0, 0,
0, dydotdot_dpsi, 0
], [0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0],
[
0, 0, 0, 0, 0, dzdotdot_dzdot, dzdotdot_dphi, 0,
dzdotdot_dtheta, 0, 0, 0
], [0, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0],
[
0, 0, 0, dphidotdot_dydot, 0, dphidotdot_dzdot,
dphidotdot_dphi, dphidotdot_dphidot,
dphidotdot_dtheta, 0, 0, 0
], [0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0],
[
0, dthetadotdot_dxdot, 0, 0, 0, dthetadotdot_dzdot,
dthetadotdot_dphi, 0, dthetadotdot_dtheta,
dthetadotdot_dthetadot, 0, 0
], [0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1],
[
0, dpsidotdot_dxdot, 0, dpsidotdot_dydot, 0, 0,
dpsidotdot_dphi, 0, dpsidotdot_dtheta, 0,
dxdotdot_dpsi, dpsidotdot_dpsidot
]])
B = np.array([[0, 0, 0, 0, 0, 0, 0, 0],
[dxdotdot_dTL, dxdotdot_dTR, 0, 0, 0, 0, 0, 0],
[0, 0, 0, 0, 0, 0, 0, 0],
[0, 0, dydotdot_dTF, dydotdot_dTB, 0, 0, 0, 0],
[0, 0, 0, 0, 0, 0, 0, 0],
[
0, 0, 0, 0, dzdotdot_dTFL, dzdotdot_dTFR,
dzdotdot_dTBL, dzdotdot_dTBR
], [0, 0, 0, 0, 0, 0, 0, 0],
[
0, 0, 0, 0, dphidotdot_dTFL, dphidotdot_dTFR,
dphidotdot_dTBL, dphidotdot_dTBR
], [0, 0, 0, 0, 0, 0, 0, 0],
[
0, 0, 0, 0, dthetadotdot_dTFL, dthetadotdot_dTFR,
dthetadotdot_dTBL, dthetadotdot_dTBR
], [0, 0, 0, 0, 0, 0, 0, 0],
[
dpsidotdot_dTL, dpsidotdot_dTR, dpsidotdot_dTF,
dpsidotdot_dTB, 0, 0, 0, 0
]])
C = np.array([[
0, dxdotdot_dxdot, 0, 0, 0, 0, 0, 0, dxdotdot_dtheta, 0,
dxdotdot_dpsi, 0
],
[
0, 0, 0, dydotdot_dydot, 0, 0, dydotdot_dphi, 0, 0,
0, dydotdot_dpsi, 0
],
[
0, 0, 0, 0, 0, dzdotdot_dzdot, dzdotdot_dphi, 0,
dzdotdot_dtheta, 0, 0, 0
], [0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0],
[0, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0],
[0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0],
[0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0],
[0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 0],
[0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1],
[0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0]])
D = np.array([[dxdotdot_dTL, dxdotdot_dTR, 0, 0, 0, 0, 0, 0],
[0, 0, dydotdot_dTF, dydotdot_dTB, 0, 0, 0, 0],
[
0, 0, 0, 0, dzdotdot_dTFL, dzdotdot_dTFR,
dzdotdot_dTBL, dzdotdot_dTBR
], [0, 0, 0, 0, 0, 0, 0, 0], [0, 0, 0, 0, 0, 0, 0, 0],
[0, 0, 0, 0, 0, 0, 0, 0], [0, 0, 0, 0, 0, 0, 0, 0],
[0, 0, 0, 0, 0, 0, 0, 0], [0, 0, 0, 0, 0, 0, 0, 0],
[0, 0, 0, 0, 0, 0, 0, 0]])
if np.linalg.matrix_rank(control.ctrb(A, B)) < A.shape[0]:
print "System is not fully controllable"
else:
print "System is fully controllable"
# print np.sum(control.obsv(A,C),axis=1)
if np.linalg.matrix_rank(control.obsv(A, C)) < A.shape[0]:
print "System is not fully observable"
else:
print "System is fully observable"
state_space = StateSpace(A, B, C, D, dt)
return state_space
####################################################
# Observer
####################################################
# Observer design
obs_th_wn = 5.0 * th_wn
obs_z_wn = 5.0 * z_wn
obs_h_wn = 5.0 * h_wn
obs_des_char_poly_lat = np.convolve(
[1, 2.0 * z_zeta * obs_z_wn, obs_z_wn**2],
[1, 2.0 * th_zeta * obs_th_wn, obs_th_wn**2])
obs_des_char_poly_lon = [1, 2.0 * h_zeta * obs_h_wn, obs_h_wn**2]
obs_des_poles_lat = np.roots(obs_des_char_poly_lat)
obs_des_poles_lon = np.roots(obs_des_char_poly_lon)
if np.linalg.matrix_rank(cnt.obsv(A_lat, C_lat)) != 4:
print('Latitudinal System Not Observable')
else:
L_lat = place(A_lat.T, C_lat.T, obs_des_poles_lat).gain_matrix.T
print('L_lat:', L_lat)
if np.linalg.matrix_rank(cnt.obsv(A_lon, C_lon)) != 2:
print('Longitudinal System Not Observable')
else:
L_lon = place(A_lon.T, C_lon.T, obs_des_poles_lon).gain_matrix.T
print('L_lon:', L_lon)
#################################################
# Uncertainty Parameters
#################################################
UNCERTAINTY_PARAMETERS = True
def design_tsob(sys_c_ol,
Ca,
sampling_interval,
desired_settling_time,
desired_observer_settling_time=None,
spoles=None,
sopoles=None,
sapoles=None,
disp=True):
""" Design a digital full order observer tracking system with the desired settling time.
Args:
sys_c_ol (StateSpace): The continouous plant model
sampling_interval: The sampling interval for the digital control system in seconds.
desired_settling_time: The desired settling time in seconds
desired_observer_settling_time (optional): The desired observer settling time
in seconds. If not provided the observer settling time will be 4 times faster
than the overall settling time. Default is None.
spoles (optional): The desired closed loop poles. If not supplied, then optimal
poles will try to be used. Default is None.
sopoles (optional): The desired observer poles. If not supplied, then optimal
poles will try to be used. Default is None.
sapoles (optional): The poles of the reference input and disturbance vectors.
If not supplied the reference input is assumed to be a step. Default is None.
disp: Print debugging output. Default is True.
Returns:
tuple: (sys_d_ol, phia, gammaa, L1, L2, K) Where sys_d_ol is the discrete plant,
phia is the discrete additional dynamics A matrix, gammaa is the discrete
additional dynamics B matrix, L1 is the plant gain matrix, L2 is the
additional gain matrix, and K is the observer gain matrix.
"""
if disp:
print("Designing a tracking system with full order observer.")
# Make sure the system is in fact continuous and not discrete
if sys_c_ol.dt != None:
print("Error: Function expects continuous plant")
return None
A = sys_c_ol.A
B = sys_c_ol.B
C = sys_c_ol.C
D = sys_c_ol.D
num_states = A.shape[0]
num_inputs = B.shape[1]
num_outputs = C.shape[0]
num_tracked = Ca.shape[0]
# Convert to discrete system using zero order hold method
sys_d_ol = sys_c_ol.to_discrete(sampling_interval, method="zoh")
phi = sys_d_ol.A
gamma = sys_d_ol.B
# Check controlability of the discrete system
controllability_mat = control.ctrb(phi, gamma)
rank = LA.matrix_rank(controllability_mat)
if rank != num_states:
print(rank, num_states)
print("Error: System is not controlable")
return None
# Check observability of the discrete system
observability_mat = control.obsv(phi, C)
rank = LA.matrix_rank(observability_mat)
if rank != num_states:
print("Error: System is not observable")
return None
# Create the design model with additional dynamics
if sapoles is None:
# assume tracking a step input (s=0, z=1)
sapoles = [0]
zapoles = [
-p for p in np.poly(
control_poles.spoles_to_zpoles(sapoles, sampling_interval))
]
zapoles = np.delete(zapoles, 0) # the first coefficient isn't important
gammaa = np.transpose(np.matrix(zapoles))
q = gammaa.shape[0]
phia_left = np.matrix(gammaa)
phia_right = np.concatenate((np.eye(q - 1), np.zeros((1, q - 1))), axis=0)
phia = np.concatenate((phia_left, phia_right), axis=1)
if num_tracked > 1:
# replicate the additional dynamics
phia = np.kron(np.eye(num_tracked), phia)
gammaa = np.kron(np.eye(num_tracked), gammaa)
# Form the design matrix
phid_top_row = np.concatenate((phi, np.zeros(
(num_states, q * num_tracked))),
axis=1)
phid_bot_row = np.concatenate((gammaa * Ca, phia), axis=1)
phid = np.concatenate((phid_top_row, phid_bot_row), axis=0)
gammad = np.concatenate((gamma, np.zeros((gammaa.shape[0], num_tracked))),
axis=0)
# Choose poles if none were given
if spoles is None:
spoles = find_candadate_spoles(sys_c_ol, desired_settling_time, disp)
num_spoles_left = num_states - len(spoles)
if num_spoles_left > 0:
# Use normalized bessel poles for the rest
spoles.extend(
control_poles.bessel_spoles(num_spoles_left,
desired_settling_time))
zpoles = control_poles.spoles_to_zpoles(spoles, sampling_interval)
if disp:
print("spoles = ", spoles)
print("zpoles = ", zpoles)
# place the poles such that eig(phi - gamma*L) are inside the unit circle
full_state_feedback = signal.place_poles(phid, gammad, zpoles)
# Check the poles for stability just in case
for zpole in full_state_feedback.computed_poles:
if abs(zpole) >= 1:
print("Computed pole is not stable", zpole)
return None
L = full_state_feedback.gain_matrix
L1 = L[:, 0:num_states]
L2 = L[:, num_states:]
# Choose poles if none were given
if sopoles is None:
sopoles = []
if desired_observer_settling_time == None:
desired_observer_settling_time = desired_settling_time / 4
# TODO: Find existing poles based on the rules. For now just use bessel
num_sopoles_left = num_states - len(sopoles)
if num_sopoles_left > 0:
# Use normalized bessel poles for the rest
sopoles.extend(
control_poles.bessel_spoles(num_sopoles_left,
desired_observer_settling_time))
zopoles = control_poles.spoles_to_zpoles(sopoles, sampling_interval)
if disp:
print("sopoles = ", sopoles)
print("zopoles = ", zopoles)
# Find K such that eig(phi - KC) are inside the unit circle
full_state_feedback = signal.place_poles(np.transpose(phi),
np.transpose(C), zopoles)
# Check the poles for stability just in case
for zopole in full_state_feedback.computed_poles:
if abs(zopole) > 1:
print("Computed observer pole is not stable", zopole)
return None
K = np.transpose(full_state_feedback.gain_matrix)
return (sys_d_ol, phia, gammaa, np.matrix(L1), np.matrix(L2), np.matrix(K))
def design_regsf(sys_c_ol,
sampling_interval,
desired_settling_time,
spoles=None,
disp=True):
""" Design a digital full state feedback regulator with the desired settling time.
Args:
sys_c_ol (StateSpace): The continouous plant model
sampling_interval: The sampling interval for the digital control system in seconds.
desired_settling_time: The desired settling time in seconds
spoles (optional): The desired closed loop poles. If not supplied, then optimal
poles will try to be used. Default is None.
Returns:
tuple: (sys_d_ol, L) Where sys_d_ol is the discrete plant and L is the stablizing
gain matrix.
"""
# Make sure the system is in fact continuous and not discrete
if sys_c_ol.dt != None:
print("Error: Function expects continuous plant")
return None
A = sys_c_ol.A
B = sys_c_ol.B
C = sys_c_ol.C
D = sys_c_ol.D
num_states = A.shape[0]
num_inputs = B.shape[1]
num_outputs = C.shape[0]
# Convert to discrete system using zero order hold method
sys_d_ol = sys_c_ol.to_discrete(sampling_interval, method="zoh")
phi = sys_d_ol.A
gamma = sys_d_ol.B
# Check controlability of the discrete system
controllability_mat = control.ctrb(phi, gamma)
rank = LA.matrix_rank(controllability_mat)
if rank != num_states:
print("Error: System is not controlable")
return None
# Check observability of the discrete system
observability_mat = control.obsv(phi, C)
rank = LA.matrix_rank(observability_mat)
if rank != num_states:
print("Error: System is not observable")
return None
# Choose poles if none were given
if spoles is None:
spoles = find_candadate_spoles(sys_c_ol, desired_settling_time, disp)
num_spoles_left = num_states - len(spoles)
if num_spoles_left > 0:
# Use normalized bessel poles for the rest
spoles.extend(
control_poles.bessel_spoles(num_spoles_left,
desired_settling_time))
zpoles = control_poles.spoles_to_zpoles(spoles, sampling_interval)
if disp:
print("spoles = ", spoles)
print("zpoles = ", zpoles)
# place the poles such that ...
full_state_feedback = signal.place_poles(phi, gamma, zpoles)
# Check the poles for stability
for zpole in full_state_feedback.computed_poles:
if abs(zpole) > 1:
print("Computed pole is not stable")
return None
L = full_state_feedback.gain_matrix
return (sys_d_ol, np.matrix(L))
def reduced_observer(system, poles_o, poles_s, x0, tt, debug=False):
"""
:param system: tuple (A, B, C) of system matrices
:param poles_o: tuple of complex poles/eigenvalues of the
observer dynamics
:param poles_s: tuple of complex poles/eigenvalues of the
closed system
:param x0: initial condition
:param tt: vector for the time axis
:param debug: output control for debugging in unittest
(False:normal output,True: output local variables
and normal output)
:return yy: output of the system
:return xx: states of the system
:return tt: time of the simulation
"""
# renumber the matrices to construct the matrices
# in the form of the reduced observer
a, b, c, d = ob_func.transformation(system, debug=False)
# row rank of the output matrix
rank = np.linalg.matrix_rank(c[0])
# submatrices of the system matrix A
# and the input matrix
a_11 = a[0:rank, 0:rank]
a_12 = a[0:rank, rank:a.shape[1]]
a_21 = a[rank:a.shape[1], 0:rank]
a_22 = a[rank:a.shape[1], rank:a.shape[1]]
b_1 = b[0:rank, :]
b_2 = b[rank:b.shape[0], :]
# construct the observability matrix
# q_1 = a_12
# q_2 = a_12 * a_22
# q = np.vstack([q_1, q_2])
obs_matrix = ctr.obsv(a_22, a_12) # observability matrix
rank_q = np.linalg.matrix_rank(obs_matrix)
assert np.linalg.det(obs_matrix) != 0, 'this system is unobservable'
# ignore the PendingDeprecationWarning for built-in packet control
warnings.filterwarnings('ignore', category=PendingDeprecationWarning)
# state feedback
f_t = ctr.acker(a, b, poles_s)
c_ = c - d * f_t
f_1 = f_t[:, 0:1]
f_2 = f_t[:, 1:]
l_ = ctr.acker(a_22.T, a_12.T, poles_o).T
# State representation of the entire system
# Original system and observer error system,
# Dimension: 4 x 4)
sys = ob_func.state_space_func(a, a_11, a_12, a_21, a_22, b, b_1, b_2, c_,
l_, f_1, f_2)
# simulation for the proper movement
yy, tt, xx = ctr.matlab.initial(sys, tt, x0, return_x=True)
result = yy, xx, tt, sys
# return internal variables for unittest
if debug:
c_locals = Container(fetch_locals=True)
return result, c_locals
return result
def main():
init_data = None
with open('/vagrant/client/init_data.json') as f:
init_data = json.load(f)
num_states = init_data['num_states']
if type(num_states) is not int:
raise TypeError("num_states must be int")
num_inputs = init_data['num_inputs']
if type(num_inputs) is not int:
raise TypeError("num_inputs must be int")
delay = init_data['delay']
if type(delay) is not int:
raise TypeError("delay must be int")
nonlinearity = init_data['nonlinearity']
if type(nonlinearity) is not str:
raise TypeError("nonlinearity must be str")
error_dist_mu = init_data['error_dist_mu']
if type(error_dist_mu) is not float:
raise TypeError("error_dist_mu must be float")
error_dist_sigma = init_data['error_dist_sigma']
if type(error_dist_sigma) is not float:
raise TypeError("error_dist_sigma must be float")
feed_forward = init_data['feed_forward']
if type(feed_forward) is not list:
raise TypeError("feed_forward must be list")
A = init_data['A']
if type(A) is not list:
raise TypeError("A must be list")
B = init_data['B']
if type(B) is not list:
raise TypeError("B must be list")
C = init_data['C']
if type(C) is not list:
raise TypeError("C must be list")
D = init_data['D']
if type(D) is not list:
raise TypeError("D must be list")
controller = Controller(num_states, num_inputs)
if not controller.set_matrix('A', A):
sys.exit()
if not controller.set_matrix('B', B):
sys.exit()
if not controller.set_matrix('C', C):
sys.exit()
if not controller.set_matrix('D', D):
sys.exit()
controller.calculate_observer_controller()
dyn_process_session = DynamicProcessSession()
dyn_process_session.set_num_states(json.dumps(num_states))
dyn_process_session.set_num_outputs(json.dumps(num_inputs))
dyn_process_session.set_delay(json.dumps(delay))
dyn_process_session.set_nonlinearity(nonlinearity)
dyn_process_session.set_error_dist_mu(error_dist_mu)
dyn_process_session.set_error_dist_sigma(error_dist_sigma)
dyn_process_session.set_coefficient('A', json.dumps(A))
dyn_process_session.set_coefficient('B', json.dumps(B))
dyn_process_session.set_coefficient('C', json.dumps(C))
W_c = control.ctrb(controller.A, controller.B)
W_o = control.obsv(controller.A, controller.C)
print('The rank of controlability matrix is: {}'.format(
np.linalg.matrix_rank(W_c)))
print('The rank of observability matrix is: {}'.format(
np.linalg.matrix_rank(W_o)))
queue_ = queue.Queue()
commands = Commands(logger, queue_)
commands_thread = CommandsThread(commands)
commands_thread.start()
t0 = time.perf_counter()
freq_counter = 0
freq = 0
while(True):
freq_counter += 1
t = time.perf_counter()
if (t - t0) > 1:
freq = freq_counter
freq_counter = 0
t0 = t
logger.info("freq: {}".format(freq))
logger.info('------------------')
y = dyn_process_session.get_output()
y = json_to_np(y)
logger.info('y: {}'.format(y))
u = controller.get_control_signal(y, feed_forward)
dyn_process_session.set_input(np_to_json(u))
time.sleep(0.1)
logger.info("====================================")
try:
[action, matrix_name, values] = queue_.get_nowait()
if action is 'exit':
sys.exit()
getattr(controller, action)(matrix_name, values)
except queue.Empty:
pass
Q = np.eye(4,4)
R = np.eye(2,2)
#S1 = sp.linalg.solve_continuous_are(A, B, Q, R)
#K1 = np.linalg.inv(R).dot(B.T).dot(S1)
#E1 = np.linalg.eigvals(A-B.dot(K1))
#print("S1 =\n", S1)
#print("K1 =\n", K1)
#print("E1 =\n", E1)
S2, E2, K2 = ct.care(Ap, Bp, Q, R)
#print("\nS2 =\n", S2)
#print("K2 =\n", K2)
#print("E2 =\n", E2)
print "ctrb = "+ str(np.linalg.matrix_rank(ct.ctrb(Ap,Bp)))
print "obsv = " + str(np.linalg.matrix_rank(ct.obsv(Ap,Cp)))
#K3, S3, E3 = ct.matlab.lqr(Ap, Bp, Q, R)
#print("\nS3 =\n", S3)
#print("K3 =\n", K3)
#print("E3 =\n", E3)
ss_P = ct.ss(Ap,Bp,Cp,Dp)
print ss_P
t = np.linspace(0, 3, 1000)
stepy = ct.step(ss_P,t)
#plt.plot(t,stepy)
#ct.impulse(ss_P)
ct.nyquist(ss_P)
#bode plot only implemented for SISO system.omg
#ct.bode(ss_P)
Aa = np.array([[A[0, 0], A[0, 1], 0], [A[1, 0], A[1, 1], 0],
[np.dot(C, A)[0], np.dot(C, A)[1], 1]])
Ba = np.array([[B[0, 0], B[0, 1]], [B[1, 0], B[1, 1]],
[np.dot(C, B)[0], np.dot(C, B)[1]]])
Ca = np.array([0, 0, 1])
Da = 0
# %% Controllability and Observability analysis
Co = control.ctrb(Aa, Ba)
if matrix_rank(Co) == len(Aa):
print('The augmented system is controllable.')
Ob = control.obsv(Aa, Ca)
if matrix_rank(Ob) == len(Aa):
print('The augmented system is observable.')
# %% LQR controller
# y | Delta*x1 | Delta*x2
Qlqr = np.diag([1, 100, 1])
Rlqr = 1 * np.eye(2)
# K, X, eigVals = dlqr(Aa, Ba, Qlqr, Rlqr)
# Recursive Riccati Difference Equation (RDE)
P = 100 * np.eye(3)
for k in range(300):
P = np.dot(np.dot(Aa.T, P), Aa) - np.dot(
C, Qe, Re = model.compute_noise_cov(sys_i, sens)
print('\n Sensors: ', sens)
print('\nQe = ')
print(Qe)
print('eig(Qe)')
print(np.linalg.eig(Qe)[0])
print('\nRe = ')
print(Re)
print('eig(Re)')
print(np.linalg.eig(Re)[0])
print('\nRank of observability matrix: %d of %d' %
(np.linalg.matrix_rank(control.obsv(model.rsys[sys_i].A, C)), nx))
Ts = 5
print('Mode frequencies:')
print(np.abs(np.angle(np.diag(model.sys_eig[sys_i]))) / (2.0 * np.pi * Ts))
"""
Save full model & final model
"""
pickle.dump(model, open('data/model_full.p', 'wb'))
final_model = [model.rsys[sys_i], C, Qe, Re, sens]
pickle.dump(final_model, open('data/model_sparse.p', 'wb'))
"""
Plot results
"""
Q = np.eye(4, 4)
R = np.eye(2, 2)
#S1 = sp.linalg.solve_continuous_are(A, B, Q, R)
#K1 = np.linalg.inv(R).dot(B.T).dot(S1)
#E1 = np.linalg.eigvals(A-B.dot(K1))
#print("S1 =\n", S1)
#print("K1 =\n", K1)
#print("E1 =\n", E1)
S2, E2, K2 = ct.care(Ap, Bp, Q, R)
#print("\nS2 =\n", S2)
#print("K2 =\n", K2)
#print("E2 =\n", E2)
print "ctrb = " + str(np.linalg.matrix_rank(ct.ctrb(Ap, Bp)))
print "obsv = " + str(np.linalg.matrix_rank(ct.obsv(Ap, Cp)))
#K3, S3, E3 = ct.matlab.lqr(Ap, Bp, Q, R)
#print("\nS3 =\n", S3)
#print("K3 =\n", K3)
#print("E3 =\n", E3)
ss_P = ct.ss(Ap, Bp, Cp, Dp)
print ss_P
t = np.linspace(0, 3, 1000)
stepy = ct.step(ss_P, t)
#plt.plot(t,stepy)
#ct.impulse(ss_P)
ct.nyquist(ss_P)
#bode plot only implemented for SISO system.omg
#ct.bode(ss_P)
import sys
sys.path.append('..') # add parent directory
import matplotlib.pyplot as plt
import control
import pendulumParam as P
from kalmanFilter import kalmanFilter, kalmanFilterEC
from pendulumNonlinearDynamics import Pendulum
from pendulumAnimation import pendulumAn
from plotDataZ import plotData
from signalGenerator import signalGen
#Compute controller
dpoles = np.array([-1.1, -1.2, -1.3, -1.4])
Kr = control.place(P.A * 1.01, P.B * 1.01, dpoles)
print(control.obsv(P.A, P.C)) # which states of A, C are observable?
print(np.linalg.matrix_rank(control.obsv(
P.A, P.C))) # expect a rank of 0 if observable
#Initialize and rename for convenience
ref = signalGen(amplitude=.5, frequency=0.05, y_offset=0)
pendulum = Pendulum(param=P)
states = [np.array([[P.z0], [P.zdot0], [P.theta0], [P.thetadot0]])]
states_est = [states[0]]
mu = np.array([[P.z0 + 0.1], [P.zdot0 + 0.1], [P.theta0 + 0.1],
[P.thetadot0 + 0.1]])
sigma = np.eye(4) * 0.00001
u = 0
#performs very simple first order integration
# _p = [-3.0, -3.1, -3.2, -3.3] # aggressive
# _p = [-3.5, -3.6, -3.7, -3.8] # breaks
# _p = [-10,-10.1,-10.2,-10.3] # breaks
# _p = [-10,-10.1,-10.2,-10.3] # breaks
p = np.array(_p)
x0 = np.array([0, 0, -np.pi + 0.1, 1]) # intial x
Q = np.array([[10, 0, 0, 0], [0, 1, 0, 0], [0, 0, 2, 0], [0, 0, 0, 2]])
R = 0.1
# K = control.place(A, B, p)
K = control.lqr(A, B, Q, R)[0]
# controllability = control.ctrb(A, B)
observability = control.obsv(A, C)
print(observability)
sys = control.ss(A, B, C, D)
def xDot(t, y):
# print(t)
error = y - [0, 0, -np.pi, 0]
u = -K * ((error)[np.newaxis].T)
return cartpend(y, m, M, L, g, d, u[0][0])
t0 = 0
t1 = 2
dt = 0.02
def dare(F, G1, G2, H):
"""Solves the discrete-time algebraic Riccati equation
0 = F ^ T * X * F
- X - F ^ T * X * G1 * (G2 + G1 ^ T * X * G1) ^ -1 * G1 ^ T * X * F + H
Under the assumption that X ^ -1 exists, this equation is equivalent to
0 = F ^ T * (X ^ -1 + G1 * G2 ^ -1 * G1 ^ T) ^ -1 * F - X + H
Parameters
==========
Inputs are real matrices:
F : n x n
G1 : n x m
G2 : m x m, symmetric, positive definite
H : n x n, symmetric, positive semi-definite
Assumptions
===========
(F, G1) is a stabilizable pair
(C, F) is a detectable pair (where C is full rank factorization of H, i.e.,
C ^ T * C = H and rank(C) = rank(H).
F is invertible
Returns
=======
Unique nonnegative definite solution of discrete Algrebraic Ricatti
equation.
Notes
=====
This is an implementation of the Schur method for solving algebraic Riccati
eqautions as described in dx.doi.org/10.1109/TAC.1979.1102178
"""
# Verify that F is non-singular
u, s, v = la.svd(F)
assert(np.all(s > 0.0))
# Verify that (F, G1) controllable
C = ctrb(F, G1)
u, s, v = la.svd(C)
assert(np.all(s > 0.0))
# Verify that (H**.5, F) is observable
O = obsv(H**.5, F)
u, s, v = la.svd(O)
assert(np.all(s > 0.0))
n = F.shape[0]
m = G2.shape[0]
G = np.dot(G1, np.dot(inv(G2), G1.T))
Finv = inv(F)
Finvt = Finv.T
# Form symplectic matrix
Z = empty((2*n, 2*n))
Z[:n, :n] = F + np.dot(G, np.dot(Finvt, H))
Z[:n, n:] = -np.dot(G, Finvt)
Z[n:, :n] = -np.dot(Finvt, H)
Z[n:, n:] = Finvt
S, U, sdim = schur(Z, sort='iuc')
# Verify that the n eigenvalues of the upper left block stable
assert(sdim == n)
U11 = U[:n, :n]
U21 = U[n:, :n]
return solve(U[:n, :n].T, U[n:, :n].T).T