def matmul(mat, vec, lenth):
matvec = [[vec[0][0], 0], [vec[1][0], 0], [vec[2][0], 0], [vec[3][0], 0],
[vec[4][0], 0]]
result = svd.matrixmultiply(mat, matvec)
if lenth == 5:
return [[result[0][0]], [result[1][0]], [result[2][0]], [result[3][0]],
[result[4][0]]]
elif lenth == 10:
return [[result[0][0]], [result[1][0]], [result[2][0]], [result[3][0]],
[result[4][0]], [result[5][0]], [result[6][0]], [result[7][0]],
[result[8][0]], [result[9][0]]]
def matmul(mat, vec, lenth=3):
matvec = [[vec[0][0], 0], [vec[1][0], 0], [vec[2][0], 0]]
result = svd.matrixmultiply(mat, matvec)
if lenth == 20:
return [[result[0][0]], [result[1][0]], [result[2][0]], [result[3][0]],
[result[4][0]], [result[5][0]], [result[6][0]], [result[7][0]],
[result[8][0]], [result[9][0]], [result[10][0]],
[result[11][0]], [result[12][0]], [result[13][0]],
[result[14][0]], [result[15][0]], [result[16][0]],
[result[17][0]], [result[18][0]], [result[19][0]]]
return [[result[0][0]], [result[1][0]], [result[2][0]]]
def matmul(mat,vec): matvec = [[vec[0][0],0],[vec[1][0],0],[vec[2][0],0],[vec[3][0],0],[vec[4][0],0],[vec[5][0],0],[vec[6][0],0],[vec[7][0],0],[vec[8][0],0],[vec[9][0],0],[vec[10][0],0],[vec[11][0],0],[vec[12][0],0],[vec[13][0],0],[vec[14][0],0],[vec[15][0],0],[vec[16][0],0],[vec[17][0],0],[vec[18][0],0],[vec[19][0],0],[vec[20][0],0],[vec[21][0],0],[vec[22][0],0],[vec[23][0],0],[vec[24][0],0],[vec[25][0],0],[vec[26][0],0],[vec[27][0],0],[vec[28][0],0],[vec[29][0],0]] result = svd.matrixmultiply(mat,matvec) return [[result[0][0]],[result[1][0]],[result[2][0]]]
C = [[1.,0.,0.],
[0.,1.,0.],
[0.,0.,1.]]
D = [[ 0., 0.],
[ 0., 0.],
[ 0., 0.]]
# initial condition: Xo = [1,1,0,0,pi/4]
# Uo = [1,1]
# final condition: Xf = [3,8,0,0,pi]
# Uf = [0,0]
print "(1) Observability check:"
CA = svd.matrixmultiply(C,A)
CA2 = svd.matrixmultiply(CA,A)
CA3 = svd.matrixmultiply(CA2,A)
CA4 = svd.matrixmultiply(CA3,A)
CA5 = svd.matrixmultiply(CA4,A)
CA6 = svd.matrixmultiply(CA5,A)
CA7 = svd.matrixmultiply(CA6,A)
CA8 = svd.matrixmultiply(CA7,A)
CA9 = svd.matrixmultiply(CA8,A)
Observe_matrix = np.concatenate((CA9,CA8,CA7,CA6,CA5,CA4,CA3,CA2,CA,C),axis=0) # 30*3
print Observe_matrix
# This is the observability matrix I get:
'''
[[ 1.04333183e+13 -2.10375831e+12 -7.59205100e+10]
[ 1.77767749e+12 -3.58447688e+11 -1.29355218e+10]
[0., 0., 0., 0., 0.]]
B = [[0., 0.], [0., 0.], [1. / math.sqrt(2), 0.], [1. / math.sqrt(2), 0.],
[0., 1.]]
C = [[1., 0., 0., 0., 0.], [0., 1., 0., 0., 0.], [0., 0., 0., 0., 1.]]
D = [[0., 0.], [0., 0.], [0., 0.]]
# initial condition: Xo = [1,1,0,0,pi/4]
# Uo = [1,1]
# final condition: Xf = [3,8,0,0,pi]
# Uf = [0,0]
print "(1) Controllability check:"
AB = svd.matrixmultiply(A, B)
A2B = svd.matrixmultiply(A, AB)
A3B = svd.matrixmultiply(A, A2B)
A4B = svd.matrixmultiply(A, A3B)
Control_matrix = np.concatenate((A4B, A3B, A2B, AB, B), axis=1)
print Control_matrix
# This is the controllability matrix I get:
#
# [[-0.70710678 -0.70710678 0.70710678 0.70710678 -0.70710678 -0.70710678 0.70710678 0. 0. 0. ]
# [-0.70710678 0.70710678 0.70710678 -0.70710678 -0.70710678 0.70710678 0.70710678 0. 0. 0. ]
# [ 0.70710678 0.70710678 -0.70710678 -0.70710678 0.70710678 0.70710678 -0.70710678 -0.70710678 0.70710678 0. ]
# [ 0.70710678 -0.70710678 -0.70710678 0.70710678 0.70710678 -0.70710678 -0.70710678 0.70710678 0.70710678 0. ]
# [ 0. 0. 0. 0. 0. 0. 0. 0. 0. 1. ]]
Control_rank = matrix_rank(Control_matrix)
A = [[28.7009, -5.6535, -0.0002], [0., 0., 1.], [132.5065, -8.3440, -0.0008]]
B = [[113.0690, 0.], [0., 0.], [166.8785, 1.]]
C = [[1., 0., 0.], [0., 1., 0.], [0., 0., 1.]]
D = [[0., 0.], [0., 0.], [0., 0.]]
# initial condition: Xo = [0,5*pi/180,0]
# Uo = [0,0]
# final condition: Xf = [0,0,0]
# Uf = [0,0]
print "(1) Controllability check:"
AB = svd.matrixmultiply(A, B)
A2B = svd.matrixmultiply(A, AB)
A3B = svd.matrixmultiply(A, A2B)
A4B = svd.matrixmultiply(A, A3B)
A5B = svd.matrixmultiply(A, A4B)
A6B = svd.matrixmultiply(A, A5B)
A7B = svd.matrixmultiply(A, A6B)
A8B = svd.matrixmultiply(A, A7B)
A9B = svd.matrixmultiply(A, A8B)
# try 10 steps to reach the goal:
Control_matrix = np.concatenate(
(A9B, A8B, A7B, A6B, A5B, A4B, A3B, A2B, AB, B), axis=1)
print Control_matrix
# This is the controllability matrix I get: ( 20 x 3 )