h = lambda b, M: rho(L(beta(b)),M)
# State function and output function
sys.f(Matrix(
[x[1],
9.81 - 2 * f(x[0], x[2])/M_p - k_f * x[1] / M_p,
A * r * (rho_e * u[0] - h(x[0], x[2]) * u[1] )
])
)
sys.h(Matrix([x[0]]))
# Linearization!!!
A,B,C = sys.linearize(Matrix([0.5,0,0.01417]), Matrix([5, 5.7691]))
print "A=\n", A
print "B=\n", B
print "C=\n", C
K1, K2, K3 = luenberger(A,C,[
[-0.1 + 0.2*I, -0.1 - 0.2*I, -0.5],
[-0.5 + 1.0*I, -0.5 - 1.0*I, -2.5],
[-2.5 + 5.0*I, -2.5 - 5.0*I, -12.5]])
print "K1=\n",K1
print "K2=\n",K2
print "K3=\n",K3
sys.f(
Matrix(
[x[1],
(l_1_ * f(x[0],x[2]) - l_1_ * m * g * cos(x[0] + theta_) - (
k_f * x[1])) / (I + l_1_**2 * m_1),
A * r * (rho_e * u[0] - h(x[0],x[2]) * u[1])
])
)
sys.h(Matrix([x[0]]))
# Linearization!!!
A,B,C = sys.linearize(Matrix([0.02824, 0, 0.13233]), Matrix([5.0, 4.46]))
print "A=\n", A
print "B=\n", B
print "C=\n", C
from sympy import I
K = luenberger(A, C, [-10 + 4 * I, -10 - 4 * I, -10])
print "K = \n", K
V_2 = Matrix([1e-2])
V_1 = kalman(A, C, K, V_2)
print "V_1 = \n", V_1
print "V_2 = \n", V_2
Raices = [] Raices.append([-0.5 + 1.0*I, -0.5 - 1.0*I, -2.5]) Raices.append([-2.5 + 5.0*I, -2.5 - 5.0*I, -12.5]) Raices.append([-12.5 + 25.0*I, -12.5 - 25.0*I, -62.5]) # for Raiz in Raices: # L = calcular_L(A,C,Raiz) # print "********************* R **********************" # print Raiz # print "********************* L **********************" # print latex(L.evalf()) # print "********************* A **********************" # print A.evalf() # print "********************* B **********************" # print B.row_join(L).evalf() # print "********************* C **********************" # print C.evalf() # print "******************** SCICOS *********************" from luenberger import luenberger print luenberger(A, C, Raices)
# print latex(L.evalf())
# print "********************* A **********************"
# print A.evalf()
# print "********************* B **********************"
# print B.row_join(L).evalf()
# print "********************* C **********************"
# print C.evalf()
# print "******************** SCICOS *********************"
from luenberger import luenberger
for K in [luenberger(A, C, r) for r in raices]:
print "K = \n[%s];"%(" ; ".join([str(k) for k in K]))
P_sym = Symbolic_Matrix("p",(3,3))
V2 = matrices.Matrix([1e-2])
res = solvers.solve(tuple((P_sym * C.transpose() * V2.inv() - K).mat), P_sym.mat)
P = zeros(P_sym.shape)
P.mat = [p if not p in res else res[p] for p in P_sym.mat]
V1 = - (P * A.transpose() + A * P - P * C.transpose() * V2.inv() * C * P)
res = solvers.solve(tuple([V1[i,j] - V1[j,i] for i in xrange(3) for j in xrange(3) if i < j] + [V1[0,1], V1[0,2], V1[1,2]]), P_sym.mat)
Raices = [] Raices.append([-0.5 + 1.0*I, -0.5 - 1.0*I, -2.5]) Raices.append([-2.5 + 5.0*I, -2.5 - 5.0*I, -12.5]) Raices.append([-12.5 + 25.0*I, -12.5 - 25.0*I, -62.5]) # for Raiz in Raices: # L = calcular_L(A,C,Raiz) # print "********************* R **********************" # print Raiz # print "********************* L **********************" # print latex(L.evalf()) # print "********************* A **********************" # print A.evalf() # print "********************* B **********************" # print B.row_join(L).evalf() # print "********************* C **********************" # print C.evalf() # print "******************** SCICOS *********************" from luenberger import luenberger print "\n".join([";".join([str(l) for l in L]) for L in luenberger(A, C, Raices)])
