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

def make_S(A0, Ak, G0, Gk, phi):

    R = P = scipy.matrix(scipy.zeros((6,6)))

    for i in range(0, 3, 2):
        for j in range(3):
            R[i, j] = Ak[i, j]
    R[1, 1] = R[3, 3] = R[4, 4] = 1.0;
    R[5, 5] = Ak[5, 5]

    P[0, 0] = (A0[0, 0] * cos(phi)**2.0 + A0[1, 0] * sin(phi)**2.0)
    P[0, 1] = (A0[0, 1] * cos(phi)**2.0 + A0[1, 1] * sin(phi)**2.0)
    P[0, 2] = (A0[0, 2] * cos(phi)**2.0 + A0[1, 2] * sin(phi)**2.0)
    P[0, 3] = (A0[3, 3] * sin(2.0 * phi))
    P[1, 0] = sin(phi)**2.0
    P[1, 1] = cos(phi)**2.0
    P[1, 3] = -sin(2.0*phi)
    P[2, 0] = A0[2, 0]
    P[2, 1] = A0[2, 1]
    P[2, 2] = A0[2, 2]
    P[3, 0] = -0.5*sin(2.0*phi)
    P[3, 1] = 0.5*sin(2.0*phi)
    P[3, 3] = cos(2.0*phi)
    P[4, 4] = cos(phi)
    P[4, 5] = -sin(phi)
    P[5, 4] = A0[4, 4] * sin(phi)
    P[5, 5] = A0[5, 5] * cos(phi)

    scipy.savetxt("R", R)
    scipy.savetxt("P", P)
    return scipy.matrix(R.I) * scipy.matrix(P)

def marginalize(dist_vars,marg_vars):
    #Initialize marginal dict, same for all dists
    margdist_vars={}
    margdist_vars['dist']=dist_vars['dist']
    #Gaussian
    if dist_vars['dist']=='gaussian':
        N_k=len(dist_vars['w'])#Number of gaussians
        N_D=len(dist_vars['mu'][0])#Dim of orgiginal parameter space
        
        #Initialize remaining components of marg dict, before any marginalization        
        margdist_vars['mu']=dist_vars['mu'][:]
        margdist_vars['cov']=dist_vars['cov'][:]
        margdist_vars['w']=dist_vars['w'][:]
        margdist_vars['vars']=dist_vars['vars'][:]
        
        for marg_var in marg_vars:
            #Get indices of marginalized var in current gaussian
            i_m=margdist_vars['vars'].index(marg_var)
            #Create list of current indices
            i_old=list(range(N_D))
            #remove index of marg_var
            i_old.remove(i_m)
            
            
            #remove marg_var from list of vars
            margdist_vars['vars'].remove(marg_var)
        
            margdist_vars['mu']=[sp.delete(margdist_vars['mu'][i],i_m,0) for i in range(len(margdist_vars['w']))]
            
            #For testing
#            for i in range(N_k):
#                margdist_vars['w'][i]=dist_vars['w'][i]
#                margdist_vars['cov'][i]=sp.delete(sp.delete(margdist_vars['cov'][i],i_m,0),i_m,1)
            
            #Loop over components in mixture
            #marg cov:T_M=L_m-T_m
            #marg weight:w_m=sp.sqrt(2*pi/L_mm)
            for i in range(N_k):
                #invert original covariance matrix
                Lambda=inv(sp.matrix(margdist_vars['cov'][i]))
                #Store marg compononent of 
                L_mm=Lambda[i_m,i_m]
                #Remove marginal component from Lambda
                L_m=sp.delete(sp.delete(Lambda,i_m,0),i_m,1)
                #Construct skew matrix
                l_m=sp.matrix(Lambda[i_m,i_old]+Lambda[i_old,i_m])
                T_m=l_m.T*l_m/(4*L_mm)
                #Construct marginalized covariance matrix
                margdist_vars['cov'][i]=sp.asarray(inv(L_m-T_m))
                #Scale weight
                margdist_vars['w'][i]=sp.sqrt(2*sp.pi/L_mm)*dist_vars['w'][i]
            
            #Update dimensions of marginalized parameter space
            N_D=N_D-1
         
        return margdist_vars
            
                
                

def connect(sys, Q, inputv, outputv):
    '''
    Index-base interconnection of system

    The system sys is a system typically constructed with append, with
    multiple inputs and outputs. The inputs and outputs are connected
    according to the interconnection matrix Q, and then the final
    inputs and outputs are trimmed according to the inputs and outputs
    listed in inputv and outputv.

    Note: to have this work, inputs and outputs start counting at 1!!!!

    Parameters.
    -----------
    sys: StateSpace Transferfunction
        System to be connected
    Q: 2d array
        Interconnection matrix. First column gives the input to be connected
        second column gives the output to be fed into this input. Negative
        values for the second column mean the feedback is negative, 0 means
        no connection is made
    inputv: 1d array
        list of final external inputs
    outputv: 1d array
        list of final external outputs

    Returns
    -------
    sys: LTI system
        Connected and trimmed LTI system

    Examples
    --------
    >>> sys1 = ss("1. -2; 3. -4", "5.; 7", "6, 8", "9.")
    >>> sys2 = ss("-1.", "1.", "1.", "0.")
    >>> sys = append(sys1, sys2)
    >>> Q = sp.mat([ [ 1, 2], [2, -1] ]) # basically feedback, output 2 in 1
    >>> sysc = connect(sys, Q, [2], [1, 2])
    '''
    # first connect
    K = sp.zeros( (sys.inputs, sys.outputs) )
    for r in sp.array(Q).astype(int):
        inp = r[0]-1
        for outp in r[1:]:
            if outp > 0 and outp <= sys.outputs:
                K[inp,outp-1] = 1.
            elif outp < 0 and -outp >= -sys.outputs:
                K[inp,-outp-1] = -1.
    sys = sys.feedback(sp.matrix(K), sign=1)
    
    # now trim
    Ytrim = sp.zeros( (len(outputv), sys.outputs) )
    Utrim = sp.zeros( (sys.inputs, len(inputv)) )
    for i,u in enumerate(inputv):
        Utrim[u-1,i] = 1.
    for i,y in enumerate(outputv):
        Ytrim[i,y-1] = 1.
    return sp.matrix(Ytrim)*sys*sp.matrix(Utrim)  

def EigenVectors(A):
    data = eig(A)
    g0 = data[1][0]
    g1 = data[1][1]
    g0.shape = (2, 1)
    g1.shape = (2, 1)
    g0 = matrix(g0)
    g1 = matrix(g1)
    return g0, g1

def Pow(M, power = 1):
    """Calculates the principal power of a square Hermitian matrix"""
    assert isHermitian(M, tol = 1e-14)
    D, U = eig(M)
    U = matrix(U)
    E = [x ** power for x in D]
    E = matrix(diag(E))
    Mpow = U * E * inv(U)
    return Mpow