def debias_nz(prob, reg=1e-3):
    (w, X, y, lam, a_i, c_i, ipro) = prob
    nd = X.shape[0]
    #This is a fancy way of pruning zeros. You could also do this with loops without too much worry. 
    #You could also use numpy.nonzero to do this.
    stn = array([0] + filter(lambda x : x != 0, list((w[1:] != 0) * range(1, len(w)))), dtype=int)
    Hn = ipro[:, stn]
    Hn = Hn[stn, :].copy()
    #
    nz = len(stn)
    #
    Xty = zeros(nz)
    Xty[0] = y.sum()
    for i in range(1, nz):
        if sp.issparse(X):
            Xty[i] = X[:, stn[i] - 1].T.dot(y)[0]
        else:
            Xty[i] = X[:, stn[i] - 1].dot(y)
    Xty *= 2
    #
    try:
        wdb = la.solve(Hn, Xty, sym_pos=True)
    except la.LinAlgError:
        print("Oh no! Matrix is Singular. Trying again using regularization.")
        Hn[range(nz), range(nz)] += 2 * reg
        wdb = la.solve(Hn, Xty, sym_pos=True)
    
    #Update c_i
    c_i -= ipro[:, stn].dot(wdb - w[stn])
    c_i[stn] += a_i[stn] * (wdb - w[stn])

    w[stn] = wdb

    return

 def planeintersect(self, other):
     """ returns line of intersection of this plane and another
         None is returned if planes are parallel
         20110207 NEW VERSION: M. Krauss
     """
     N1 = self.N
     N2 = other.N
     D1 = self.D
     D2 = other.D
     if (N1.cross(N2)).abs() == 0:
         # Planes are parallel
         return None
     else:
         v = N1.cross(N2)
         b = np.array([[D1],[D2]])
         p = Point(0,0,0)
         try:
             # intersection with the plane x=0
             A = np.array([[N1.y , N1.z],[N2.y , N2.z]])
             x = la.solve(A,b)
             p = Point(0,x[0],x[1])
             return Line(p, v)
         except:
             try:
                 # intersection with the plane y=0
                 A = np.array([[N1.x , N1.z],[N2.x , N2.z]])
                 x = la.solve(A,b)
                 p = Point(x[0],0,x[1])
                 return Line(p, v)
             except:
                 # intersection with the plane z=0
                 A = np.array([[N1.x , N1.y],[N2.x , N2.y]])
                 x = la.solve(A,b)
                 p = Point(x[0],x[1],0)
                 return Line(p, v)

    def test_dare(self):
        A = matrix([[-0.6, 0],[-0.1, -0.4]])
        Q = matrix([[2, 1],[1, 0]])
        B = matrix([[2, 1],[0, 1]])
        R = matrix([[1, 0],[0, 1]])

        X,L,G = dare(A,B,Q,R)
        # print("The solution obtained is", X)
        assert_array_almost_equal(
            A.T * X * A - X -
            A.T * X * B * solve(B.T * X * B + R, B.T * X * A) + Q, zeros((2,2)))
        assert_array_almost_equal(solve(B.T * X * B + R, B.T * X * A), G)
        # check for stable closed loop
        lam = eigvals(A - B * G)
        assert_array_less(abs(lam), 1.0)

        A = matrix([[1, 0],[-1, 1]])
        Q = matrix([[0, 1],[1, 1]])
        B = matrix([[1],[0]])
        R = 2

        X,L,G = dare(A,B,Q,R)
        # print("The solution obtained is", X)
        assert_array_almost_equal(
            A.T * X * A - X -
            A.T * X * B * solve(B.T *  X * B + R, B.T * X * A) + Q, zeros((2,2)))
        assert_array_almost_equal(B.T * X * A / (B.T * X * B + R), G)
        # check for stable closed loop
        lam = eigvals(A - B * G)
        assert_array_less(abs(lam), 1.0)

def _pade(A, m):
    n = np.shape(A)[0]
    c = _padecoeff(m)
    if m != 13:
        apows = [[] for jj in range(int(np.ceil((m + 1) / 2)))]
        apows[0] = sp.eye(n, n, format='csr')
        apows[1] = A * A
        for jj in range(2, int(np.ceil((m + 1) / 2))):
            apows[jj] = apows[jj - 1] * apows[1]
        U = sp.lil_matrix((n, n)).tocsr()
        V = sp.lil_matrix((n, n)).tocsr()
        for jj in range(m, 0, -2):
            U = U + c[jj] * apows[jj // 2]
        U = A * U
        for jj in range(m - 1, -1, -2):
            V = V + c[jj] * apows[(jj + 1) // 2]
        F = la.solve((-U + V).todense(), (U + V).todense())
        return sp.lil_matrix(F).tocsr()
    elif m == 13:
        A2 = A * A
        A4 = A2 * A2
        A6 = A2 * A4
        U = A * (A6 * (c[13] * A6 + c[11] * A4 + c[9] * A2) +
                 c[7] * A6 + c[5] * A4 + c[3] * A2 +
                 c[1] * sp.eye(n, n).tocsr())
        V = A6 * (c[12] * A6 + c[10] * A4 + c[8] * A2) + c[6] * A6 + c[4] * \
            A4 + c[2] * A2 + c[0] * sp.eye(n, n).tocsr()
        F = la.solve((-U + V).todense(), (U + V).todense())
        return sp.csr_matrix(F)

   def conditional(self,xb):
      """         conditional(self,xb)  
         Calculates the mean and covariance of the conditional distribution
         when the variables xb are observed.
         Input: xb - The observed variables. It is assumed that the observed variables occupy the 
                     last positions in the array of random variables, i.e. if x is the random variable
                     associated with the object, then it is partioned as x = [xa,xb]^T.
         Output: mean_a_given_b - mean of the conditional distribution
                 cov_a_given_b  - covariance of the conditional distribution
      """
      xb         = np.array(xb,ndmin=1)
      nb         = len(xb)
      n_rand_var = len(self.mean)
      if nb >= n_rand_var:
         raise ValueError('The conditional vector should be smaller than the random variable!')
      mean = self.mean
      cov  = self.cov          

      # Partition the mean and covariance  
      na     = n_rand_var - nb
      mean_a = self.mean[:na]
      mean_b = self.mean[na:]
      cov_a  = self.cov[:na,:na]
      cov_b  = self.cov[na:,na:]
      cov_ab = self.cov[:na,na:]
      
      #Calculate the conditional mean and covariance
      mean_a_given_b = mean_a.flatten() + np.dot(cov_ab,solve(cov_b,xb.flatten()-mean_b.flatten()))
      cov_a_given_b  = cov_a - np.dot(cov_ab,solve(cov_b,cov_ab.transpose()))

      return mean_a_given_b, cov_a_given_b

    def test_care_g(self):
        A = matrix([[-2, -1],[-1, -1]])
        Q = matrix([[0, 0],[0, 1]])
        B = matrix([[1, 0],[0, 4]])
        R = matrix([[2, 0],[0, 1]])
        S = matrix([[0, 0],[0, 0]])
        E = matrix([[2, 1],[1, 2]])

        X,L,G = care(A,B,Q,R,S,E)
        # print("The solution obtained is", X)
        assert_array_almost_equal(
            A.T * X * E + E.T * X * A -
            (E.T * X * B + S) * solve(R, B.T * X * E + S.T)  + Q, zeros((2,2)))
        assert_array_almost_equal(solve(R, B.T * X * E + S.T), G)

        A = matrix([[-2, -1],[-1, -1]])
        Q = matrix([[0, 0],[0, 1]])
        B = matrix([[1],[0]])
        R = 1
        S = matrix([[1],[0]])
        E = matrix([[2, 1],[1, 2]])

        X,L,G = care(A,B,Q,R,S,E)
        # print("The solution obtained is", X)
        assert_array_almost_equal(
            A.T * X * E + E.T * X * A -
            (E.T * X * B + S) / R * (B.T * X * E + S.T) + Q , zeros((2,2)))
        assert_array_almost_equal(dot( 1/R , dot(B.T,dot(X,E)) + S.T) , G)

	def computeProjectionVectors( self, P, L, U ) :	
		eK = matrix( identity( self.dim, float64 )[ 0: ,( self.dim - 1 ) ] ).T
		U = matrix(U, float64)
		U[ self.dim - 1, self.dim - 1 ] = 1.0
		# Sergio: I added this exception because in rare cases, the matrix
		# U is singular, which gives rise to a LinAlgError.
		try: 
			x1 = matrix( solve( U, eK ), float64 )
		except LinAlgError:
			print "Matrix U was singular, so we input a fake x1\n"
			print "U: ", U
			x1 = matrix(ones(self.dim))

		#print "x1", x1
		del U

		LT = matrix( L, float64, copy=False ).T
		PT = matrix( P, float64, copy=False ).T

		x2 = matrix( solve( LT*PT, eK ), float64 )
		del L
		del P
		del LT
		del PT
		del eK

		return ( x1, x2 )

    def predict(self, X_p):
        '''
        Predict the values of the GP at each position x_p
        
        From Rasmussen & Williams "Gaussian Processes for Machine Learning",
        pg. 19, algorithm 2.1
        '''
        (X, Y) = (self.X, self.Y)
        sigma_n = self.hyper_params[0]
        K = self.kernel.value(self.hyper_params[1:], X, X)
        K_p = self.kernel.value(self.hyper_params[1:], X, X_p)
        K_p_p = self.kernel.value(self.hyper_params[1:], X_p, X_p)

        (self.K, self.K_p, self.K_p_p) = K, K_p, K_p_p

        # since the kernel is (should be) pos. def. and symmetric,
        # we can solve in a quick/stable way using the cholesky decomposition
        L = np.matrix(linalg.cholesky(K + sigma_n**2 * np.eye(len(X)),
                                      lower = True))
        alpha = np.matrix(linalg.solve(L.T, linalg.solve(L, Y)))
        f_p_mean = K_p.T * alpha.T
        v = np.matrix(linalg.solve(L, K_p))
        f_p_covariance = K_p_p - v.T*v
        log_marginal = (-0.5*np.matrix(Y)*alpha.T - sum(np.log(np.diag(L))) -
                        len(X)/2.0*np.log(2*np.pi))

        return (np.array(f_p_mean).flatten(),
                f_p_covariance,
                np.array(log_marginal).flatten())

def invert_low_rank(Ainv, U, C, V, diag=False):
    """
    Invert the matrix (A+UCV) where A^{-1} is known and C is lower rank than A

    Let N be rank of A and K be rank of C where K << N

    Then we can write the inverse,
    (A+UCV)^{-1} = A^{-1} - A^{-1}U (C^{-1}+VA^{-1}U)^{-1} VA^{-1}

    :param Ainv: NxN matrix A^{-1}
    :param U: NxK matrix
    :param C: KxK invertible matrix
    :param V: KxN matrix
    :return:
    """
    N,K = U.shape
    Cinv = inv(C)
    if diag:
        assert Ainv.shape == (N,)
        tmp1 = einsum('ij,j,jk->ik', V, Ainv, U)
        tmp2 = einsum('ij,j->ij', V, Ainv)
        tmp3 = solve(Cinv + tmp1, tmp2)
        # tmp4 = -U.dot(tmp3)
        tmp4 = -einsum('ij,jk->ik', U, tmp3)
        tmp4[diag_indices(N)] += 1
        return einsum('i,ij->ij', Ainv, tmp4)

    else:
        tmp = solve(Cinv + V.dot(Ainv).dot(U), V.dot(Ainv))
        return Ainv - Ainv.dot(U).dot(tmp)

def matrix_normal_density(X, M, U, V):
    """Sample from a matrix normal distribution"""
    norm = - 0.5*np.log(la.det(2*np.pi*U)) - 0.5*np.log(la.det(2*np.pi*V))
    XM = X-M
    pptn = -0.5*np.trace( np.dot(la.solve(U,XM),la.solve(V,XM.T)) )
    pdf = norm + pptn
    return pdf

    def _H(self):
        r"""Continuous-time linear time invariant system.

        This method is used to create a Continuous-time linear
        time invariant system for the mdof system.
        From this system we can obtain poles, impulse response,
        generate a bode, etc.


        """
        Z = np.zeros((self.n, self.n))
        I = np.eye(self.n)

        # x' = Ax + Bu
        B2 = I
        A = self.A()
        B = np.vstack([Z, la.solve(self.M, B2)])

        # y = Cx + Du
        # Observation matrices
        Cd = I
        Cv = Z
        Ca = Z

        C = np.hstack((Cd - Ca @ la.solve(self.M, self.K), Cv - Ca @ la.solve(self.M, self.C)))
        D = Ca @ la.solve(self.M, B2)

        sys = signal.lti(A, B, C, D)

        return sys

    def A(self):
        """State space matrix"""
        Z = np.zeros((self.n, self.n))
        I = np.eye(self.n)

        A = np.vstack([np.hstack([Z, I]), np.hstack([la.solve(-self.M, self.K), la.solve(-self.M, self.C)])])
        return A

def interpolation(interP,knots=None):
    """
    Interpolates the given points and returns an object of the Spline class 
    Arguments:
    
        * interP: interpolation points, (L x 2) matrix
        * knotP: knot points, (L+4 x 1) matrix
        
            * default: equidistant on [0,1]
                
    """
    nip=len(interP)
    ctrlP=np.zeros((nip,2))
    if knots != None:
            knots = np.array(knots,dtype='float')
            if len(ctrlP) + 4 != len(knots):
                raise ValueError('Knots is of the wrong size')

    else:
        knots = np.hstack((np.zeros(3), np.linspace(0,1,len(ctrlP)-2),
                           np.ones(3)))
    xi=(knots[1:-3]+knots[2:-2]+knots[3:-1])/3
    nMatrix=np.zeros((len(xi),len(xi)))
    for i in xrange(len(xi)):
        fun=basisFunction(i,knots)
        nMatrix[:,i]=fun(xi,3)

    ctrlP[:,0]=sl.solve(nMatrix,interP[:,0])
    ctrlP[:,1]=sl.solve(nMatrix,interP[:,1])
    
    return Spline(ctrlP)

    def computeHgg(self,f,g,Rg,Rgy,Rgyz):
        # set up self.Hgg with correspond matrix
        # also should give other two derivatives
        D = np.exp(g)*self.D0
        gp = np.gradient(g,self.dy)
        jumpsq = np.zeros(self.bins)
        driftsq = np.zeros(self.bins)
        driftsum = np.zeros(self.bins)
        for k in np.arange(self.bins):
            jumpsq[k] = sum(self.jumps[self.binpos==k]**2)
            driftsq[k] = sum( (D[k]*(f[k]+self.m[self.binpos==k])+gp[k]*D[k])**2)*self.dt*self.dt
            driftsum[k] = sum( D[k]*(f[k]+self.m[self.binpos==k])+gp[k]*D[k])*self.dt
        a1 = (jumpsq + driftsq)/D/4.0/self.dt
        a2 = driftsum/2.
        a4 = D*self.dt/2.
        A1 = np.tile(a1,(self.bins,1))*Rg+np.tile(a2,(self.bins,1))*-Rgy
        A2 = np.tile(a2,(self.bins,1))*Rg+np.tile(a4,(self.bins,1))*-Rgy
        A3 = np.tile(a1,(self.bins,1))*Rgy+np.tile(a2,(self.bins,1))*Rgyz
        A4 = np.tile(a2,(self.bins,1))*Rgy+np.tile(a4,(self.bins,1))*Rgyz

        A5 = np.tile(a1,(self.bins,1))*Rgy+np.tile(a2,(self.bins,1))*Rgyz
        A6 = np.tile(a2,(self.bins,1))*Rgy+np.tile(a4,(self.bins,1))*Rgyz
        M = np.vstack((Rg,Rgy))
        A = np.vstack((np.hstack((A1,A2)) ,np.hstack((A3,A4)) ))
        Lambda = solve(np.eye(self.bins*2)+A.astype(np.float128),M)
        Hggy = Lambda[self.bins:,:]
        Hgg = Lambda[:self.bins,:]

        Hggyz = solve(np.eye(self.bins)+A6,Rgyz-A5.dot(Hggy.T))
        return 0.5*(Hgg+Hgg.T),Hggy,0.5*(Hggyz+Hggyz.T) # first and third should be symmetric. get rid of small errors

def matlabLDiv(a, b):
    '''Implements the matlab \ operator on arrays (maybe works
    on matrices also).
    Solves the "left divide" equation: a * x = b for x
    Inputs:
    a,b arrays
    Returns: a \ b
    Results depend on dimensions of matrices. See documentation for
    matlab's MLDIVIDE operator \ .
    Theory is on the Numpy for Matlab user's page.
    http://wiki.scipy.org/NumPy_for_Matlab_Users
    See also this site, but beware of caveats
    http://stackoverflow.com/questions/1001634/array-division-translating-from-matlab-to-python
    '''
    import scipy.linalg as LA
    # Check if a is square.
    try:
        (r,c) = np.shape(a)
    except ValueError: # In case a is of dim=1, cannot unpack two vals.
        return LA.solve(a,b)
    else:
        if (r == c): # Square
            return LA.solve(a,b)
        else:
            return LA.lstsq(a,b)

    def fit(self, X, y, **params):
        """
        Fit Ridge regression model

        Parameters
        ----------
        X : numpy array of shape [n_samples,n_features]
            Training data
        y : numpy array of shape [n_samples]
            Target values

        Returns
        -------
        self : returns an instance of self.
        """
        self._set_params(**params)

        X = np.asanyarray(X, dtype=np.float)
        y = np.asanyarray(y, dtype=np.float)

        n_samples, n_features = X.shape

        X, y, Xmean, ymean = self._center_data(X, y)

        if n_samples > n_features:
            # w = inv(X^t X + alpha*Id) * X.T y
            self.coef_ = linalg.solve(np.dot(X.T, X) + self.alpha * np.eye(n_features), np.dot(X.T, y))
        else:
            # w = X.T * inv(X X^t + alpha*Id) y
            self.coef_ = np.dot(X.T, linalg.solve(np.dot(X, X.T) + self.alpha * np.eye(n_samples), y))

        self._set_intercept(Xmean, ymean)
        return self

    def b_operator(self, P):
        r"""
        The B operator, mapping P into

        .. math::

            B(P) := R - beta^2 A'PB(Q + beta B'PB)^{-1}B'PA + beta A'PA

        and also returning

        .. math::

            F := (Q + beta B'PB)^{-1} beta B'PA

        Parameters
        ----------
        P : array_like(float, ndim=2)
            A matrix that should be n x n

        Returns
        -------
        F : array_like(float, ndim=2)
            The F matrix as defined above
        new_p : array_like(float, ndim=2)
            The matrix P after applying the B operator

        """
        A, B, Q, R, beta = self.A, self.B, self.Q, self.R, self.beta
        S1 = Q + beta * dot(B.T, dot(P, B))
        S2 = beta * dot(B.T, dot(P, A))
        S3 = beta * dot(A.T, dot(P, A))
        F = solve(S1, S2)
        new_P = R - dot(S2.T, solve(S1, S2)) + S3

        return F, new_P

def get_inner_circle(A, B, C):
	xa, ya = A[0], A[1]
	xb, yb = B[0], B[1]
	xc, yc = C[0], C[1]

	ka = (yb - ya) / (xb - xa) if xb != xa else None
	kb = (yc - yb) / (xc - xb) if xc != xb else None
	
	alpha = np.arctan(ka) if ka != None else np.pi / 2
	beta  = np.arctan(kb) if kb != None else np.pi / 2

	a = np.sqrt((xb - xc)**2 + (yb - yc)**2)
	b = np.sqrt((xa - xc)**2 + (ya - yc)**2)
	c = np.sqrt((xa - xb)**2 + (ya - yb)**2)

	ang_a = np.arccos((b**2 + c**2 - a**2) / (2 * b * c))
	ang_b = np.arccos((a**2 + c**2 - b**2) / (2 * a * c))

	# 两条角平分线的斜率
	k1 = np.tan(alpha + ang_a / 2)
	k2 = np.tan(beta + ang_b / 2)
	kv = np.tan(alpha + np.pi / 2)
	
	# 求圆心
	y, x = solve([[1.0, -k1], [1.0, -k2]], [ya - k1 * xa, yb - k2 * xb])
	ym, xm = solve([[1.0, -ka], [1.0, -kv]], [ya - ka * xa, y - kv * x])
	r1 = np.sqrt((x - xm)**2 + (y - ym)**2)

	return(x, y, r1)

    def update_values(self):
        """
        This method is for updating in the finite horizon case.  It shifts the
        current value function 

            V_t(x) = x' P_t x + d_t
        
        and the optimal policy F_t one step *back* in time, replacing the pair
        P_t and d_t with P_{t-1} and d_{t-1}, and F_t with F_{t-1}  
        """
        # === Simplify notation === #
        Q, R, A, B, C = self.Q, self.R, self.A, self.B, self.C
        P, d = self.P, self.d
        # == Some useful matrices == #
        S1 = Q + self.beta * dot(B.T, dot(P, B))   
        S2 = self.beta * dot(B.T, dot(P, A))
        S3 = self.beta * dot(A.T, dot(P, A))
        # == Compute F as (Q + B'PB)^{-1} (beta B'PA) == #
        self.F = solve(S1, S2)  
        # === Shift P back in time one step == #
        new_P = R - dot(S2.T, solve(S1, S2)) + S3  
        # == Recalling that trace(AB) = trace(BA) == #
        new_d = self.beta * (d + np.trace(dot(P, dot(C, C.T))))  
        # == Set new state == #
        self.P, self.d = new_P, new_d

def _solve_cholesky(X, y, alpha, sample_weight=None):
    # w = inv(X^t X + alpha*Id) * X.T y
    n_samples, n_features = X.shape
    n_targets = y.shape[1]

    has_sw = sample_weight is not None

    if has_sw:
        sample_weight = sample_weight * np.ones(n_samples)
        sample_weight_matrix = sparse.dia_matrix((sample_weight, 0),
            shape=(n_samples, n_samples))
        weighted_X = safe_sparse_dot(sample_weight_matrix, X)
        A = safe_sparse_dot(weighted_X.T, X, dense_output=True)
        Xy = safe_sparse_dot(weighted_X.T, y, dense_output=True)
    else:
        A = safe_sparse_dot(X.T, X, dense_output=True)
        Xy = safe_sparse_dot(X.T, y, dense_output=True)

    one_alpha = np.array_equal(alpha, len(alpha) * [alpha[0]])

    if one_alpha:
        A.flat[::n_features + 1] += alpha[0]
        return linalg.solve(A, Xy, sym_pos=True,
                            overwrite_a=True).T
    else:
        coefs = np.empty([n_targets, n_features])
        for coef, target, current_alpha in zip(coefs, Xy.T, alpha):
            A.flat[::n_features + 1] += current_alpha
            coef[:] = linalg.solve(A, target, sym_pos=True,
                                   overwrite_a=False).ravel()
            A.flat[::n_features + 1] -= current_alpha
        return coefs