def dynamics(self, x, u, w):
        """
        Dynamics
        x(k+1) = A x(k) + B u(k) + w(k)

        E(ww^T) = Q

        Parameters
        ----------
        x : The current state.
        u : The current input.
        w : The current process noise.

        Return
        ------
        x(k+1) : The next state.

        """
        x = pl.matrix(x)
        u = pl.matrix(u)
        w = pl.matrix(w)
        assert x.shape[1] == 1
        assert u.shape[1] == 1
        assert w.shape[1] == 1
        return self.A*x + self.B*u + w

def project_perp(A):
    """
    Creates a projection matrix onto the space perpendicular to the
    rowspace of A.
    """
    A = pl.matrix(A)
    I = pl.matrix(pl.eye(A.shape[1]))
    P = project(A)
    return  I - P

 def __init_prob_map(width,height,L):
     if type(L).__name__ == "list":
         _init = SimpleProbStrategy.__init_prob_map
         return reduce(__add__,[_init(width,height,l) for l in L])
     else:
         my_x = arange(1,width+1)
         my_y = arange(1,height+1)
         my_rows = matrix([minimum(L,minimum((width+1)-my_x, my_x))] * width)
         my_cols = matrix([minimum(L,minimum((height+1)-my_y,my_y))] * height).transpose()
         return my_rows + my_cols

 def calc_f_matrix(self):
     '''Calculate the F-matrix for cubic spline iCSD method'''
     el_len = self.coord_electrode.size
     z_js = pl.zeros(el_len+2)
     z_js[1:-1] = self.coord_electrode
     z_js[-1] = z_js[-2] + pl.diff(self.coord_electrode).mean()
     
     # Define integration matrixes
     f_mat0 = pl.matrix(pl.zeros((el_len, el_len+1)))
     f_mat1 = pl.matrix(pl.zeros((el_len, el_len+1)))
     f_mat2 = pl.matrix(pl.zeros((el_len, el_len+1)))
     f_mat3 = pl.matrix(pl.zeros((el_len, el_len+1)))
     
     # Calc. elements
     for j in xrange(el_len):
         for i in xrange(el_len):
             f_mat0[j, i] = si.quad(self.f_mat0, a=z_js[i], b=z_js[i+1], \
                 args=(z_js[j+1]), epsabs=self.tol)[0]
             f_mat1[j, i] = si.quad(self.f_mat1, a=z_js[i], b=z_js[i+1], \
                                args=(z_js[j+1], z_js[i]), \
                                     epsabs=self.tol)[0]
             f_mat2[j, i] = si.quad(self.f_mat2, a=z_js[i], b=z_js[i+1], \
                                args=(z_js[j+1], z_js[i]), \
                                     epsabs=self.tol)[0]
             f_mat3[j, i] = si.quad(self.f_mat3, a=z_js[i], b=z_js[i+1], \
                                args=(z_js[j+1], z_js[i]), \
                                     epsabs=self.tol)[0]
             # image technique if conductivity not constant:
             if self.cond != self.cond_top:    
                 f_mat0[j, i] = f_mat0[j, i] + (self.cond-self.cond_top) / \
                     (self.cond + self.cond_top) * \
                         si.quad(self.f_mat0, a=z_js[i], b=z_js[i+1], \
                             args=(-z_js[j+1]), \
                                 epsabs=self.tol)[0]
                 f_mat1[j, i] = f_mat1[j, i] + (self.cond-self.cond_top) / \
                     (self.cond + self.cond_top) * \
                         si.quad(self.f_mat1, a=z_js[i], b=z_js[i+1], \
                             args=(-z_js[j+1], z_js[i]), epsabs=self.tol)[0]
                 f_mat2[j, i] = f_mat2[j, i] + (self.cond-self.cond_top) / \
                     (self.cond + self.cond_top) * \
                         si.quad(self.f_mat2, a=z_js[i], b=z_js[i+1], \
                             args=(-z_js[j+1], z_js[i]), epsabs=self.tol)[0]
                 f_mat3[j, i] = f_mat3[j, i] + (self.cond-self.cond_top) / \
                     (self.cond + self.cond_top) * \
                         si.quad(self.f_mat3, a=z_js[i], b=z_js[i+1], \
                             args=(-z_js[j+1], z_js[i]), epsabs=self.tol)[0]
     
     e_mat0, e_mat1, e_mat2, e_mat3 = self.calc_e_matrices()
     
     # Calculate the F-matrix
     self.f_matrix = pl.matrix(pl.zeros((el_len+2, el_len+2)))
     self.f_matrix[1:-1, :] = f_mat0*e_mat0 + f_mat1*e_mat1 + \
                             f_mat2*e_mat2 + f_mat3*e_mat3
     self.f_matrix[0, 0] = 1
     self.f_matrix[-1, -1] = 1

    def __init__(self, t, x, y, u):

        self.t = pl.matrix(t)
        self.x = pl.matrix(x)
        self.y = pl.matrix(y)
        self.u = pl.matrix(u)

        assert self.t.shape[0] == 1
        assert self.x.shape[0] < self.x.shape[1]
        assert self.y.shape[0] < self.y.shape[1]
        assert self.u.shape[0] < self.u.shape[1]

def block_hankel(data, f):
    """
    Create a block hankel matrix.
    f : number of rows
    """
    data = pl.matrix(data)
    assert len(data.shape) == 2
    n = data.shape[1] - f
    return pl.matrix(pl.hstack([
        pl.vstack([data[:, i+j] for i in range(f)])
        for j in range(n)]))

 def _bodyState2LidarState(self, bodyState):
     '''
     transform body 6-dim vector state to lidar matrix state
     return w_R_L(3*3 matrix), w_T_L(3*1 matrix)
     '''
     bodyState[3:]*=DEG2RAD;
     roll=bodyState[3];pitch=bodyState[4];yaw=bodyState[5];
     w_R_b=pl.matrix([[ cos(pitch)*cos(yaw),-cos(roll)*sin(yaw)+sin(roll)*sin(pitch)*cos(yaw), sin(roll)*sin(yaw)+cos(roll)*sin(pitch)*cos(yaw)],
                      [ cos(pitch)*sin(yaw), cos(roll)*cos(yaw)+sin(roll)*sin(pitch)*sin(yaw),-sin(roll)*cos(yaw)+cos(roll)*sin(pitch)*sin(yaw)],
                      [-sin(pitch),           sin(roll)*cos(pitch),                                  cos(roll)*cos(pitch)]])
     w_R_L = w_R_b * LidarFrame.b_R_L 
     w_T_b = pl.matrix(bodyState[:3]).T
     w_T_L = w_T_b + w_R_b * LidarFrame.b_T_L
     return w_R_L, w_T_L

    def simulate(self, f_u, x0, tf):
        """
        Simulate the system.

        Parameters
        ----------
        f_u: The input function  f_u(t, x, i)
        x0: The initial state.
        tf: The final time.

        Return
        ------
        data : A StateSpaceDataArray object.

        """
        #pylint: disable=too-many-locals, no-member
        x0 = pl.matrix(x0)
        assert x0.shape[1] == 1
        t = 0
        x = x0
        dt = self.dt
        data = StateSpaceDataList([], [], [], [])
        i = 0
        n_x = self.A.shape[0]
        n_y = self.C.shape[0]
        assert pl.matrix(f_u(0, x0, 0)).shape[1] == 1
        assert pl.matrix(f_u(0, x0, 0)).shape[0] == n_y

        # take square root of noise cov to prepare for noise sim
        if pl.norm(self.Q) > 0:
            sqrtQ = scipy.linalg.sqrtm(self.Q)
        else:
            sqrtQ = self.Q

        if pl.norm(self.R) > 0:
            sqrtR = scipy.linalg.sqrtm(self.R)
        else:
            sqrtR = self.R

        # main simulation loop
        while t + dt < tf:
            u = f_u(t, x, i)
            v = sqrtR.dot(pl.randn(n_y, 1))
            y = self.measurement(x, u, v)
            data.append(t, x, y, u)
            w = sqrtQ.dot(pl.randn(n_x, 1))
            x = self.dynamics(x, u, w)
            t += dt
            i += 1
        return data.to_StateSpaceDataArray()

    def test_subspace_det_algo1_siso(self):
        """
        Subspace deterministic algorithm (SISO).
        """
        ss1 = sysid.StateSpaceDiscreteLinear(
            A=0.9, B=0.5, C=1, D=0, Q=0.01, R=0.01, dt=0.1)

        pl.seed(1234)
        prbs1 = sysid.prbs(1000)
        def f_prbs(t, x, i):
            "input function"
            #pylint: disable=unused-argument, unused-variable
            return prbs1[i]

        tf = 10
        data = ss1.simulate(f_u=f_prbs, x0=pl.matrix(0), tf=tf)
        ss1_id = sysid.subspace_det_algo1(
            y=data.y, u=data.u,
            f=5, p=5, s_tol=1e-1, dt=ss1.dt)
        data_id = ss1_id.simulate(f_u=f_prbs, x0=0, tf=tf)
        nrms = sysid.subspace.nrms(data_id.y, data.y)
        self.assertGreater(nrms, 0.9)

        if ENABLE_PLOTTING:
            pl.plot(data_id.t.T, data_id.x.T, label='id')
            pl.plot(data.t.T, data.x.T, label='true')
            pl.legend()
            pl.grid()

	def sigma_vectors(self,x,P):

		"""
		generates sigma vectors

		Arguments
		----------
		x : matrix
			state at time instant t
		P:  matrix
			state covariance matrix at time instant t

		Returns
		----------
		Chi : matrix
			matrix of sigma points
		"""
		State_covariance_cholesky=sp.linalg.cholesky(P).T
		State_covariance_cholesky_product=self.gamma_sigma_points*State_covariance_cholesky		
		chi_plus=[]
		chi_minus=[]
		for i in range(self.L):
			chi_plus.append(x+State_covariance_cholesky_product[:,i].reshape(self.L,1)) 
			chi_minus.append(x-State_covariance_cholesky_product[:,i].reshape(self.L,1)) 

		Chi=pb.hstack((x,pb.hstack((pb.hstack(chi_plus),pb.hstack(chi_minus))))) 
		return pb.matrix(Chi)

def log_inv(X): # inverts a 3x3 matrix given by the logscale values
    if (X.shape[0] != X.shape[1]):
        raise Exception("X is not a square matrix and cannot be inverted")
    
    if (X.shape[0] == 1):
        return matrix((-X[0,0]))
    
    ldet = log_det(X)
    if (ldet == nan):
        raise Exception("The determinant of X is 0, cannot calculate the inverse")
     
    if (X.shape[0] == 2): # X is a 2x2 matrix
        I = (-log_det(X)) * ones((2,2))
        I[0,0] += X[1,1]
        I[0,1] += X[0,1] + complex(0, pi)
        I[1,0] += X[1,0] + complex(0, pi)
        I[1,1] += X[0,0]
        return I
    
    if (X.shape[0] == 3): # X is a 3x3 matrix
        I = (-log_det(X)) * ones((3,3))
        I[0,0] += log_subt_exp(X[1,1]+X[2,2], X[1,2]+X[2,1])
        I[0,1] += log_subt_exp(X[0,2]+X[2,1], X[0,1]+X[2,2])
        I[0,2] += log_subt_exp(X[0,1]+X[1,2], X[0,2]+X[1,1])
        I[1,0] += log_subt_exp(X[2,0]+X[1,2], X[1,0]+X[2,2])
        I[1,1] += log_subt_exp(X[0,0]+X[2,2], X[0,2]+X[2,0])
        I[1,2] += log_subt_exp(X[0,2]+X[1,0], X[0,0]+X[1,2])
        I[2,0] += log_subt_exp(X[1,0]+X[2,1], X[2,0]+X[1,1])
        I[2,1] += log_subt_exp(X[2,0]+X[0,1], X[0,0]+X[2,1])
        I[2,2] += log_subt_exp(X[0,0]+X[1,1], X[0,1]+X[1,0])
        return I
    
    raise Exception("log_inv is only implemented for matrices of size < 4")

def find_pCr(S, dG0_f, c_mid=1e-3, ratio=3.0, T=default_T, bounds=None, log_stream=None):
    """
        Compute the feasibility of a given set of reactions
    
        input: S = stoichiometric matrix (reactions x compounds)
               dG0_f = deltaG0'-formation values for all compounds (in kJ/mol) (1 x compounds)
               c_mid = the default concentration
               ratio = the ratio between the distance of the upper bound from c_mid and the lower bound from c_mid (in logarithmic scale)
        
        output: (concentrations, margin)
    """
    Nc = S.shape[1]
    cpl = make_pCr_problem(S, dG0_f, c_mid, ratio, T, bounds, log_stream)
    
    # Objective: minimize the pC variable.
    cpl.objective.set_sense(cpl.objective.sense.minimize)
    cpl.objective.set_linear([("pC", 1)])

    #cpl.write("../res/test_PCR.lp", "lp")
    cpl.solve()
    if cpl.solution.get_status() != cplex.callbacks.SolveCallback.status.optimal:
        raise LinProgNoSolutionException("")
    dG_f = pylab.matrix(cpl.solution.get_values(["c%d" % c for c in xrange(Nc)])).T
    concentrations = pylab.exp((dG_f-dG0_f)/(R*T))
    pCr = cpl.solution.get_values(["pC"])[0]

    return dG_f, concentrations, pCr

def find_mtdf(S, dG0_f, c_range=(1e-6, 1e-2), T=default_T, bounds=None, log_stream=None):
    """
        Find a distribution of concentration that will satisfy the 'relaxed' thermodynamic constraints.
        The 'relaxation' means that there is a slack variable 'B' where all dG_r are constrained to be < B.
        Note that B can also be negative, which will happen when the pathway is feasible.
        MTDF (Maximal Thermodynamic Driving Force) is defined as the minimal B, note that it is a function of the concentration bounds.
    """
    Nr, Nc = S.shape
    
    # compute right hand-side vector - r,
    # i.e. the deltaG0' of the reactions divided by -RT
    if (S.shape[1] != dG0_f.shape[0]):
        raise Exception("The S matrix has %d columns, while the dG0_f vector has %d" % (S.shape[1], dG0_f.shape[0]))
    
    cpl = create_cplex(S, dG0_f, log_stream)
    add_thermodynamic_constraints(cpl, dG0_f, c_range=c_range, bounds=bounds)

    # Define the MTDF variable and use it relax the thermodynamic constraints on each reaction
    cpl.variables.add(names=["B"], lb=[-1e6], ub=[1e6])
    for r in xrange(Nr):
        cpl.linear_constraints.set_coefficients("r%d" % r, "B", -1)

    # Set 'B' as the objective
    cpl.objective.set_linear([("B", 1)])
    
    #cpl.write("../res/test_MTDF.lp", "lp")
    cpl.solve()
    if (cpl.solution.get_status() != cplex.callbacks.SolveCallback.status.optimal):
        raise LinProgNoSolutionException("")

    dG_f = pylab.matrix(cpl.solution.get_values(["c%d" % c for c in xrange(Nc)])).T
    concentrations = pylab.exp((dG_f-dG0_f)/(R*T))
    MTDF = cpl.solution.get_values(["B"])[0]

    return dG_f, concentrations, MTDF

 def _transformPoints(self, points, w_R_old, w_T_old, w_R_new, w_T_new):
     '''
     transform lidar points from old state to new state
     Input:
     new/old R/L are both Lidar state
     
     points are n*3-dim array
     return n*3-dim array
     '''
     
     #verify rotation matrix
     #print pl.norm(w_R_old[0],2), pl.norm(w_R_old[1],2), pl.norm(w_R_old[2],2)
     #print pl.norm(w_R_new[0],2), pl.norm(w_R_new[1],2), pl.norm(w_R_new[2],2)
     
     new_R_old = w_R_new.T * w_R_old
     new_T_old = w_R_new.T * (w_T_old - w_T_new)
     
     #pdb.set_trace()
     
     #old points first transform to body frame
     old_P = pl.matrix(points).T
     new_P = new_R_old * old_P + new_T_old
     
     #print new_P.T
     #pdb.set_trace()
     
     return pl.array(new_P.T)

	def __init__(self,A,B,C,Sw,Sv,x0,Pi0):
		
		ny, nx = C.shape
		nu = B.shape[1]	
		
		if type(A) is list:
			for Ai in A:
				assert Ai.shape == (nx, nx)
		else:
			assert A.shape == (nx, nx), A.shape
		assert B.shape == (nx, nu), B.shape
		assert Sw.shape == (nx, nx), Sw.shape
		assert Sv.shape == (ny, ny), Sv.shape
		assert x0.shape == (nx,1), x0.shape
		
		self.A = A
		self.B = B
		self.C = C
		self.Sw = Sw
		self.Sv = Sv
		self.ny = ny
		self.nx = nx
		self.nu = nu
		self.x0 = x0
		self.Pi0 = Pi0
		
		# initial condition
		# TODO - not sure about this as a prior covariance...
		self.P0 = 40000 * pb.matrix(pb.ones((self.nx,self.nx)))
		self.log = logging.getLogger('LDS')
		# check for stability
		self.is_stable()
		self.log.info('initialised state space model')

 def _phase_map(self):
     self.dphi = (2*P.pi * self.nhop * P.arange(self.nfft/2+1)) / self.nfft
     A = P.diff(P.angle(self.STFT),1) # Complete Phase Map
     U = P.c_[P.angle(self.STFT[:,0]), A - P.matrix(self.dphi).T ]
     U = U - P.np.round(U/(2*P.pi))*2*P.pi
     self.dPhi = U
     return U

    def simulate(self, T):

        """Simulates the full neural field model

		Arguments
		----------

		T: ndarray
				simulation time instants
		Returns
		----------
		V: list of matrix
			each matrix is the neural field at a time instant

		Y: list of matrix
			each matrix is the observation vector corrupted with noise at a time instant
		"""

        Y = []
        V = []

        spatial_location_num = (len(self.field_space)) ** 2
        sim_field_space_len = len(self.field_space)

        # initial field
        v0 = self.Sigma_e_c * pb.matrix(np.random.randn(spatial_location_num, 1))
        v_membrane = pb.reshape(v0, (sim_field_space_len, sim_field_space_len))
        firing_rate = self.act_fun.fmax / (1.0 + pb.exp(self.act_fun.varsigma * (self.act_fun.v0 - v_membrane)))

        for t in T[1:]:

            v = self.Sigma_varepsilon_c * pb.matrix(np.random.randn(len(self.obs_locns), 1))
            w = pb.reshape(
                self.Sigma_e_c * pb.matrix(np.random.randn(spatial_location_num, 1)),
                (sim_field_space_len, sim_field_space_len),
            )

            print "simulation at time", t
            g = signal.fftconvolve(self.K, firing_rate, mode="same")
            g *= self.spacestep ** 2
            v_membrane = self.Ts * pb.matrix(g) + self.xi * v_membrane + w
            firing_rate = self.act_fun.fmax / (1.0 + pb.exp(self.act_fun.varsigma * (self.act_fun.v0 - v_membrane)))
            # Observation
            Y.append((self.spacestep ** 2) * (self.C * pb.reshape(v_membrane, (sim_field_space_len ** 2, 1))) + v)
            V.append(v_membrane)

        return V, Y

 def Sun(self, year):
     """ Returns a row vector for the position of the sun relative to the surface of the earth
     Parameters
     ----------
     year: float
           Angle of the earth relative ot the sun depending on its rotation around the sun
     """
     return matrix([[1,0,0]]) * self.Rot_z(year)

 def WriteReactionsToHtml(self, S, rids, fluxes, cids, show_cids=True):
     self.thermo.pH = 7
     dG0_r = self.thermo.GetTransfromedReactionEnergies(S, cids)
     
     self.html_writer.write("<li>Reactions:</br><ul>\n")
     
     for r in range(S.shape[0]):
         self.html_writer.write('<li><a href=' + self.kegg.rid2link(rids[r]) + '>%s ' % rids[r] + '</a>')
         self.html_writer.write('[x%g, &Delta;G<sub>r</sub>&deg; = %.1f] : ' % (fluxes[r], dG0_r[r, 0]))
         self.html_writer.write(self.kegg.vector_to_hypertext(S[r, :].flat, cids, show_cids=show_cids))
         self.html_writer.write('</li>\n')
     
     v_total = pylab.dot(pylab.matrix(fluxes), S).flat
     dG0_total = pylab.dot(pylab.matrix(fluxes), dG0_r)[0,0]
     self.html_writer.write('<li><b>Total </b>')
     self.html_writer.write('[&Delta;G<sub>r</sub>&deg; = %.1f kJ/mol] : \n' % dG0_total)
     self.html_writer.write(self.kegg.vector_to_hypertext(v_total, cids, show_cids=show_cids))
     self.html_writer.write("</li></ul></li>\n")

    def calc_e_matrices(self):
        '''Calculate the E-matrices used by cubic spline iCSD method'''
        el_len = self.coord_electrode.size
        ## expanding electrode grid
        z_js = pl.zeros(el_len+2)
        z_js[1:-1] = self.coord_electrode
        z_js[-1] = self.coord_electrode[-1] + \
            pl.diff(self.coord_electrode).mean()
        
        ## Define transformation matrices
        c_mat3 = pl.matrix(pl.zeros((el_len+1, el_len+1)))
        
        for i in xrange(el_len+1):
            for j in xrange(el_len+1):
                if i == j:
                    c_mat3[i, j] = 1./pl.diff(z_js)[i]

        # Get K-matrix
        k_matrix = self.calc_k_matrix()
        
        # Define matrixes for C to A transformation:
        # identity matrix except that it cuts off last element:
        tja = pl.matrix(pl.zeros((el_len+1, el_len+2)))
        # converting k_j to k_j+1 and cutting off last element:
        tjp1a = pl.matrix(pl.zeros((el_len+1, el_len+2))) 

        # C to A
        for i in xrange(el_len+1):
            for j in xrange(el_len+2):
                if i == j-1:
                    tjp1a[i, j] = 1
                elif i == j:
                    tja[i, j] = 1
        
        # Define spline coeffiscients
        e_mat0 = tja    
        e_mat1 = tja*k_matrix
        e_mat2 = 3 * c_mat3**2 * (tjp1a-tja) - c_mat3 * \
                (tjp1a + 2 * tja) * k_matrix
        e_mat3 = 2 * c_mat3**3 * (tja-tjp1a) + c_mat3**2 * \
                (tjp1a + tja) * k_matrix
        
        return e_mat0, e_mat1, e_mat2, e_mat3