Python scipy.matrix函数代码示例

def initialize_sequential(X_bar0, P_bar0, x_bar0):
    """
    Generate t=0 values for a new iteration from an initial state, covariance
    and a-priori estimate.
    """
    # Get initial state and STM and initialize integrator
    X_bar0_list = X_bar0.T.tolist()[0]
    stm0 = sp.matrix(sp.eye(18))
    stm0_list = sp.eye(18).reshape(1,324).tolist()[0]

    eom = ode(Udot).set_integrator('dop853', atol=1.0E-10, rtol=1.0E-9)
    eom.set_initial_value(X_bar0_list + stm0_list, 0)

    # Perform measurement update for t=0 observation
    obs0 = OBS[0]
    stn0 = obs0[0]
    comp0, Htilda0 = Htilda_matrix(X_bar0_list, 0, stn0)
    resid0 = [ obs0[1] - float(comp0[0]),
               obs0[2] - float(comp0[1]) ]
    y0 = sp.matrix([resid0]).T
    K0 = P_bar0 * Htilda0.T * (Htilda0 * P_bar0 * Htilda0.T + W.I).I
    x_hat0 = x_bar0 + K0 * (y0 - Htilda0 * x_bar0)
    P0 = (I - K0 * Htilda0) * P_bar0
    #P0 = (I - K0 * Htilda0) * P_bar0 * (I - K0 * Htilda0).T + K0 * W.I * K0.T

    return [stm0, comp0, resid0, Htilda0, x_hat0, P0, eom]

    def compute_kernel_matrix(self):
        """Compute the whole kernel matrix (see 2.1 from the SVM doc)"""
        print "Computing kernel matrix..."
        n = self.n
        X = self.X
        tau = self.tau

        # 1. compute d
        xxt = X * X.transpose()
        d = s.diag(xxt)
        d = s.matrix(d).transpose()

        # 2. compute A
        ones = s.matrix(s.ones(n))
        A = 0.5 * d * ones
        A += 0.5 * ones.transpose() * d.transpose()
        A -= xxt

        # 3. compute K with Gaussian kernel
        A = -tau*A
        K = s.exp(A)
        assert K.shape == (n,n), "Invalid shape of kernel matrix"
        self.K = K
        print "Finished computing kernel matrix."
        return

def GetPrincipalAxes(Angle1,Angle2,Angle3):
    "Input: the three euler angles from Tipsy. Output: the three axes..."

    pi = 3.14159265359
    phi =  Angle1 * pi/180.0
    theta = Angle2  * pi/180.0
    psi =  Angle3 * pi/180.0

    a11 = cos(psi) * cos(phi)  - cos(theta) * sin(phi) * sin(psi)   
    a12 = cos(psi) * sin(phi)  + cos(theta) * cos(phi) * sin(psi)   
    a13 = sin(psi) * sin(theta)     
    a21 = -sin(psi) * cos(phi)  - cos(theta) * sin(phi) *  cos(psi)     
    a22 = -sin(psi) * sin(phi)  + cos(theta) * cos(phi) * cos(psi)  
    a23 = cos(psi) * sin(theta)     
    a31 = sin(theta) * sin(phi)     
    a32 = -sin(theta) * cos(phi)    
    a33 = cos(theta)  

    a=scipy.matrix( [[a11,a12,a13],[a21,a22,a23],[ a31,a32,a33]])
    x=scipy.matrix( [[1.0],[0.0],[0.0]])
    y=scipy.matrix( [[0.0],[1.0],[0.0]])
    z=scipy.matrix( [[0.0],[0.0],[1.0]])

#    print a*x
#    print ''
#    print a*y
#    print ''
#    print a*z
    return a*x,a*y,a*z

def stream2xyz (u, v, w, mu, r, theta, phi, wedge, nu=0.0):
    """ Converts to galactic x,y,z from custom stream coordinates u,v,w;
    ACCEPTS ONLY 1 POINT AT A TIME - don't know what will happen if arrays are passed in
    stream is aligned along w-axis;  rotation is theta about y-axis, then phi about z-axis
    (See Nathan Cole's thesis, page 17)"""
    theta, phi = (theta*rad), (phi*rad)  #THETA, PHI INPUT SHOULD BE IN DEGREES!!
    # Get uvw origin in xyz
    ra, dec = GCToEq(mu, nu, wedge)
    l, b = EqTolb(ra, dec)
    xyz0 = lbr2xyz(l,b,r)
    # Rotate uvw into xyz
    R_M = sc.matrix([
        [(sc.cos(phi)*sc.cos(theta)), (-1.0*sc.sin(phi)), (sc.cos(phi)*sc.sin(theta))],
        [(sc.sin(phi)*sc.cos(theta)),  (sc.cos(phi)),     (sc.sin(phi)*sc.sin(theta))],
        [(-1.0*sc.sin(theta)),         (0.0),             (sc.cos(theta))]
        ])
    """R_inv = sc.matrix([
        [(sc.sin(theta)*sc.cos(phi)), (-1.0*sc.sin(theta)*sc.sin(phi)), (-1.0*sc.cos(theta))],
        [(sc.sin(phi)),               (sc.cos(phi)),                    (0.0)],
        [(sc.cos(theta)*sc.cos(phi)), (-1.0*sc.cos(theta)*sc.sin(phi)), (sc.sin(theta))]
        ])  OLD CRAP"""
    uvw_M = sc.matrix([u,v,w])
    xyz_M = R_M*uvw_M.T
    xyzR = sc.array(xyz_M)
    # Translate rotated values
    x = xyzR[0] + xyz0[0]
    y = xyzR[1] + xyz0[1]
    z = xyzR[2] + xyz0[2]
    return x[0],y[0],z[0]

def linsys(xdot, x, u, y, x0=None, u0=None):

    # y is required for linsys, but not linearize
    # apparently 'ss' does not support multiple outputs; linearize does
    
    
    As,Bs,Cs,Ds,F0,G0 = linearize(xdot, x, u, y, x0, u0)
    
    sumF0 = 0
    for i in F0:
        sumF0 += i
    if sumF0 > 0.001:
        print('Warning: The system was not linearized about an equilibrium point!')
        print
        print 'xdot at x0 = ', F0
        
        
    if Cs.shape[0] > 1:
        raise ValueError, "C matrix cannot have more than one row; system must be SISO"
        

    A = scipy.matrix(As).astype(np.float)
    B = scipy.matrix(Bs).astype(np.float)
    C = scipy.matrix(Cs).astype(np.float)
    D = scipy.matrix(Ds).astype(np.float)
    
    
    
    sys = 0 #ss(A,B,C,D)
    
    return sys

    def __init__(self, x, y, z, a, g, h):
        """
		Construct a Scatterer object, encapsulating the shape and material
		properties of a deformed-cylindrical object with sound speed and
		density similar to the surrounding fluid medium.

		Parameters
		----------
		x, y, z : array-like
			Posiions delimiting the central axis of the scatterer.
		a : array-like
			Array of radii along the centerline of the scatterer.
		g : array-like
			Array of sound speed contrasts (sound speed inside the scatterer
			divided by sound speed in the surrounding medium)
		h : array-like
			Array of density contrasts (density inside the scatterer
			divided by density in the surrounding medium)

		"""
        super(Scatterer, self).__init__()
        self.r = sp.matrix([x, y, z])
        self.a = sp.array(a)
        self.g = sp.array(g)
        self.h = sp.array(h)
        self.cum_rotation = sp.matrix(sp.eye(3))

        def problem_params(lr, gam, memories, inpst, neurons):
            """
            Return the lowest eigenvector of the classical Hamiltonian
            constructed by the learning rule, gamma, memories, and input.
            """
            # Bias Hamiltonian
            alpha = gam * np.array(inpst)

            # Memory Hamiltonian
            beta = np.zeros((qubits, qubits))
            if lr == "hebb":
                # Hebb rule
                memMat = sp.matrix(memories).T
                beta = sp.triu(memMat * memMat.T) / float(neurons)
            elif lr == "stork":
                # Storkey rule
                Wm = sp.zeros((neurons, neurons))
                for m, mem in enumerate(memories):
                    Am = sp.outer(mem, mem) - sp.eye(neurons)
                    Wm += (Am - Am * Wm - Wm * Am) / float(neurons)
                beta = sp.triu(Wm)
            elif lr == "proj":
                # Moore-Penrose pseudoinverse rule
                memMat = sp.matrix(memories).T
                beta = sp.triu(memMat * sp.linalg.pinv(memMat))

            # Find the eigenvectors
            evals, evecs = sp.linalg.eig(np.diag(alpha) + beta)
            idx = evals.argsort()

            return evals[idx], evecs[:, idx], np.diag(alpha), beta

	def plotm(self):
		N=24*60
		Lambda = sp.matrix(map(self.T,[i*60 for i in range(N)])).T
		Load = sp.matrix(sp.zeros([N,1]))
		Fs = sp.matrix(sp.zeros([N,1]))
		for i in range(self.ts/60,(self.ts+self.ld)/60):
			Load[i,0]=self.lv
		for i in range(self.ts/60,self.tf/60):
			Fs[i,0]=1

		plt.figure(figsize=(18,12))
		ax1 = plt.subplot2grid((3,5),(0,0),rowspan=1,colspan=5)
		ax1.set_ylabel("Load (W)")
		ax1.plot(sp.array(Load))
		ax1.axis([0,N,0,1])

		ax2 = plt.subplot2grid((3,5),(1,0),rowspan=1,colspan=5)
		ax2.set_ylabel("Feasible")
		ax2.plot(sp.array(Fs))
		ax2.axis([0,N,0,2])
		plt.draw()

		ax3 = plt.subplot2grid((3,5),(2,0),rowspan=1,colspan=5)
		ax3.set_ylabel("Tariff")
		ax3.plot(sp.array(Lambda))
		ax3.axis([0,N,0,40])
		plt.draw()
		return

	def computeVarianceContributions( self, firstDerivatives ) :

		qlist = []
		i = 0
		while i < self.dim :
			#print "Compute var %d" % i
			# derivatives of the eigenvalue for this state
			s = matrix( firstDerivatives[ i,0: ], float64 ).T
			##print s.shape

			# cross probability matrix for this state
			Pi = matrix( self.dirmat[ i,0: ], float64 ).T
			##print Pi.shape
			part1 = diag( arr2lst( Pi.T ) )
			part1 = matrix(part1, float64)
			##print part1.shape
			##print common_type(part1)
			Cp = matrix( part1 - Pi * Pi.T )
			##print common_type(Cp)
			##print Cp.shape
			del part1

			# degree of sensitivity for this state
			q = float( abs( s.T * Cp * s ) )
			del s
			del Pi
			del Cp

			qlist.append( q )

			i += 1

		return matrix( qlist )	

def initialize_batch(X_bar0, P_bar0, x_bar0):
    """
    Generate t=0 values for a new iteration from an initial state, covariance
    and a-priori estimate.
    """
    # Get initial state and STM and initialize integrator
    X_bar0_list = X_bar0.T.tolist()[0]
    stm0 = sp.matrix(sp.eye(18))
    stm0_list = sp.eye(18).reshape(1,324).tolist()[0]

    eom = ode(Udot).set_integrator('dop853', atol=1.0E-10, rtol=1.0E-9)
    eom.set_initial_value(X_bar0_list + stm0_list, 0)

    # Accumulate measurement at t=0
    obs0 = OBS[0]
    stn0 = obs0[0]
    comp0, Htilda0 = Htilda_matrix(X_bar0_list, 0, stn0)
    resid0 = [ obs0[1] - float(comp0[0]),
               obs0[2] - float(comp0[1]) ]
    y0 = sp.matrix([resid0]).T
    H0 = Htilda0 * stm0

    L0 = P_bar0.I + H0.T * W * H0
    N0 = P_bar0.I * x_bar0 + H0.T * W * y0

    return [stm0, comp0, resid0, Htilda0, H0, L0, N0, eom]

    def process_collision_geometry_for_table(self, firsttable, additional_tables = []):

        table_object = CollisionObject()
        table_object.operation.operation = CollisionObjectOperation.ADD
        table_object.header.frame_id = firsttable.pose.header.frame_id
        table_object.header.stamp = rospy.Time.now()

        #create a box for each table
        for table in [firsttable,]+additional_tables:
            object = Shape()
            object.type = Shape.BOX;
            object.dimensions.append(math.fabs(table.x_max-table.x_min))
            object.dimensions.append(math.fabs(table.y_max-table.y_min))
            object.dimensions.append(0.01)
            table_object.shapes.append(object)
  
        #set the origin of the table object in the middle of the firsttable
        table_mat = self.pose_to_mat(firsttable.pose.pose)
        table_offset = scipy.matrix([(firsttable.x_min + firsttable.x_max)/2.0, (firsttable.y_min + firsttable.y_max)/2.0, 0.0]).T
        table_offset_mat = scipy.matrix(scipy.identity(4))
        table_offset_mat[0:3,3] = table_offset
        table_center = table_mat * table_offset_mat
        origin_pose = self.mat_to_pose(table_center)
        table_object.poses.append(origin_pose)

        table_object.id = "table"
        self.object_in_map_pub.publish(table_object)

    def test_poisson3d_7pt(self):
        stencil = array([[[0, 0, 0],
                          [0, -1, 0],
                          [0, 0, 0]],
                         [[0, -1, 0],
                          [-1, 6, -1],
                          [0, -1, 0]],
                         [[0, 0, 0],
                          [0, -1, 0],
                          [0, 0, 0]]])

        cases = []
        cases.append(((1, 1, 1), matrix([[6]])))
        cases.append(((2, 1, 1), matrix([[6, -1],
                                        [-1, 6]])))
        cases.append(((2, 2, 1), matrix([[6, -1, -1, 0],
                                        [-1, 6, 0, -1],
                                        [-1, 0, 6, -1],
                                        [0, -1, -1, 6]])))
        cases.append(((2, 2, 2), matrix([[6, -1, -1, 0, -1, 0, 0, 0],
                                        [-1, 6, 0, -1, 0, -1, 0, 0],
                                        [-1, 0, 6, -1, 0, 0, -1, 0],
                                        [0, -1, -1, 6, 0, 0, 0, -1],
                                        [-1, 0, 0, 0, 6, -1, -1, 0],
                                        [0, -1, 0, 0, -1, 6, 0, -1],
                                        [0, 0, -1, 0, -1, 0, 6, -1],
                                        [0, 0, 0, -1, 0, -1, -1, 6]])))

        for grid, expected in cases:
            result = stencil_grid(stencil, grid).todense()
            assert_equal(result, expected)

def poly_fit(x, y, sig, order, verbose=1):
    n_params = order + 1
    #initialize matrices as arrays
    beta = sc.zeros(n_params, float)
    solution = sc.zeros(n_params, float)
    alpha = sc.zeros( (n_params,n_params), float)
    #Fill Matrices
    for k in range(n_params):
        # Fill beta
        for i in range(len(x)):
            holder = ( y[i]*poly(x[i], k) ) / (sig[i]*sig[i])
            beta[k] = beta[k] + holder
        # Fill alpha
        for l in range(n_params):
            for i in range(len(x)):
                holder = (poly(x[i],l)*poly(x[i],k)) / (sig[i]*sig[i])
                alpha[l,k] = alpha[l,k] + holder
    # Re-type matrices
    beta_m = sc.matrix(beta)
    alpha_m = sc.matrix(alpha)
    #Invert alpha,, then multiply beta on the right by the new matrix
    #epsilon_m = alpha_m.I
    a_m = beta_m * alpha_m.I
    if verbose==1:
        print "beta:\n", beta_m
        print "alpha:\n", alpha_m
        print "best-fit parameter matrix:\n", a_m
    return sc.array(a_m)[0,:]

def static():
    tmp = scipy.zeros((3, 3), float)

    for i in range(0, len(l)):
        L1 = L0[0, 0] / Lk[1, 1]
        L2 = L0[1, 1] / Lk[1, 1]
        D = scipy.matrix([[math.cos(fi[i]), - math.sin(fi[i]), 0.0],
                          [math.sin(fi[i]),   math.cos(fi[i]), 0.0],
                          [0.0,               0.0,             1.0]])
        B = scipy.matrix([[math.cos(fi[i]),        math.sin(fi[i]),      0.0],
                          [- L1 * math.sin(fi[i]), L2 * math.cos(fi[i]), 0.0],
                          [0.0,                    0.0,                  1.0]])
        tmp += l[i] * D * B

    E = S0 / S * I + tmp * d / S
    E = scipy.matrix(E).I

    tmp = scipy.zeros((3, 3), float)

    for i in range(0, len(l)):
        L1 = L0[0, 0] / Lk[1, 1]
        L2 = L0[1, 1] / Lk[1, 1]
        B = scipy.matrix([[math.cos(fi[i]),        math.sin(fi[i]),      0.0],
                          [- L1 * math.sin(fi[i]), L2 * math.cos(fi[i]), 0.0],
                          [0.0,                    0.0,                  1.0]])
        tmp += l[i] * B.T * Lk * B

    L = E.T * (S0 / S * L0 + d / S * tmp) * E

    print L[0, 0], L[1, 1], L[2, 2]

    def __init__(self, respond = None, regressors = None, intercept = False, D = None, d = None, G = None, a = None, b = None, **args):
        """Input: paras where they are expected to be tuple or dictionary"""
        ECRegression.__init__(self,respond, regressors, intercept, D, d, **args)

        if self.intercept and G != None:
            self.G = scipy.zeros((self.n, self.n))
            self.G[1:, 1:] = G
        elif self.intercept and G == None :
            self.G = scipy.identity(self.n)
            self.G[0, 0] = 0.0
        elif not self.intercept and G != None:
            self.G = G
        else:
            self.G = scipy.identity(self.n)
            
        if self.intercept:
            self.a = scipy.zeros((self.n, 1))
            self.a[1:] = a            
            self.b = scipy.zeros((self.n, 1))
            self.b[1:] = b
        else:
            if a is None:
                self.a = scipy.matrix( scipy.zeros((self.n,1)))
            else: self.a = a
            if b is None:
                self.b = scipy.matrix( scipy.ones((self.n,1)))
            else: self.b = b