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)])