Linear Time-Invariant Stochastic Differential Equation (LTI SDE):

            dx(t) = F x(t) dt + L d \beta  ,where

                x(t): (vector) stochastic process
                \beta: (vector) Brownian motion process
                F, L: (time invariant) matrices of corresponding dimensions
                Qc: covariance of noise.

        This function rewrites it into the corresponding state-space form:

            x_{k} =  A_{k} * x_{k-1} + q_{k-1};       q_{k-1} ~ N(0, Q_{k-1})

        TODO: this function can be redone to "preprocess dataset", when
        close time points are handeled properly (with rounding parameter) and
        values are averaged accordingly.
        
        Input:
        --------------
        F,L: LTI SDE matrices of corresponding dimensions

        Qc: matrix (n,n)
            Covarince between different dimensions of noise \beta.
            n is the dimensionality of the noise.

        dt: double or iterable
            Time difference used on this iteration.
            If dt is iterable, then A and Q_noise are computed for every
            unique dt

        compute_derivatives: boolean
            Whether derivatives of A and Q are required.

        grad_params_no: int
            Number of gradient parameters

        P_inf: (state_dim. state_dim) matrix

        dP_inf

        dF: 3D array
            Derivatives of F

        dQc: 3D array
            Derivatives of Qc

        dR: 3D array
            Derivatives of R

        Output:
        --------------

        A: matrix
            A_{k}. Because we have LTI SDE only dt can affect on matrix
            difference for different k.

        Q_noise: matrix
            Covariance matrix of (vector) q_{k-1}. Only dt can affect the
            matrix difference for different k.

        reconstruct_index: array
            If dt was iterable return three dimensinal arrays A and Q_noise.
            Third dimension of these arrays correspond to unique dt's.
            This reconstruct_index contain indices of the original dt's
            in the uninue dt sequence. A[:,:, reconstruct_index[5]]
            is matrix A of 6-th(indices start from zero) dt in the original
            sequence.
        dA: 3D array
            Derivatives of A

        dQ: 3D array
            Derivatives of Q

def lti_sde_to_descrete(F,L,Qc,dt,compute_derivatives=False,
                            grad_params_no=None, P_inf=None,
                            dP_inf=None, dF = None, dQc=None):
        """
        Linear Time-Invariant Stochastic Differential Equation (LTI SDE):

            dx(t) = F x(t) dt + L d \beta  ,where

                x(t): (vector) stochastic process
                \beta: (vector) Brownian motion process
                F, L: (time invariant) matrices of corresponding dimensions
                Qc: covariance of noise.

        This function rewrites it into the corresponding state-space form:

            x_{k} =  A_{k} * x_{k-1} + q_{k-1};       q_{k-1} ~ N(0, Q_{k-1})

        TODO: this function can be redone to "preprocess dataset", when
        close time points are handeled properly (with rounding parameter) and
        values are averaged accordingly.
        
        Input:
        --------------
        F,L: LTI SDE matrices of corresponding dimensions

        Qc: matrix (n,n)
            Covarince between different dimensions of noise \beta.
            n is the dimensionality of the noise.

        dt: double or iterable
            Time difference used on this iteration.
            If dt is iterable, then A and Q_noise are computed for every
            unique dt

        compute_derivatives: boolean
            Whether derivatives of A and Q are required.

        grad_params_no: int
            Number of gradient parameters

        P_inf: (state_dim. state_dim) matrix

        dP_inf

        dF: 3D array
            Derivatives of F

        dQc: 3D array
            Derivatives of Qc

        dR: 3D array
            Derivatives of R

        Output:
        --------------

        A: matrix
            A_{k}. Because we have LTI SDE only dt can affect on matrix
            difference for different k.

        Q_noise: matrix
            Covariance matrix of (vector) q_{k-1}. Only dt can affect the
            matrix difference for different k.

        reconstruct_index: array
            If dt was iterable return three dimensinal arrays A and Q_noise.
            Third dimension of these arrays correspond to unique dt's.
            This reconstruct_index contain indices of the original dt's
            in the uninue dt sequence. A[:,:, reconstruct_index[5]]
            is matrix A of 6-th(indices start from zero) dt in the original
            sequence.
        dA: 3D array
            Derivatives of A

        dQ: 3D array
            Derivatives of Q
        """
        # Dimensionality
        n = F.shape[0]

        if not isinstance(dt, collections.Iterable): # not iterable, scalar
            #import pdb; pdb.set_trace()
            # The dynamical model
            A  = matrix_exponent(F*dt)

            # The covariance matrix Q by matrix fraction decomposition ->
            Phi = np.zeros((2*n,2*n))
            Phi[:n,:n] = F
            Phi[:n,n:] = L.dot(Qc).dot(L.T)
            Phi[n:,n:] = -F.T
            AB = matrix_exponent(Phi*dt)
            AB = np.dot(AB, np.vstack((np.zeros((n,n)),np.eye(n))))

            Q_noise_1 = linalg.solve(AB[n:,:].T,AB[:n,:].T)
            Q_noise_2  = P_inf - A.dot(P_inf).dot(A.T)
            # The covariance matrix Q by matrix fraction decomposition <-

            if compute_derivatives:
                dA = np.zeros([n, n, grad_params_no])
                dQ = np.zeros([n, n, grad_params_no])

                #AA  = np.zeros([2*n, 2*n, nparam])
                FF  = np.zeros([2*n, 2*n])
                AA = np.zeros([2*n, 2*n, grad_params_no])

                for p in range(0, grad_params_no):

                    FF[:n,:n] = F
                    FF[n:,:n] = dF[:,:,p]
                    FF[n:,n:] = F

                    # Solve the matrix exponential
                    AA[:,:,p] = matrix_exponent(FF*dt)

                    # Solve the differential equation
                    #foo         = AA[:,:,p].dot(np.vstack([m, dm[:,p]]))
                    #mm          = foo[:n,:]
                    #dm[:,p] = foo[n:,:]

                    # The discrete-time dynamical model*
                    if p==0:
                        A  = AA[:n,:n,p]
                        Q_noise_3  = P_inf - A.dot(P_inf).dot(A.T)
                        Q_noise = Q_noise_3
                        #PP = A.dot(P).dot(A.T) + Q_noise_2

                    # The derivatives of A and Q
                    dA[:,:,p] = AA[n:,:n,p]
                    tmp = dA[:,:,p].dot(P_inf).dot(A.T)
                    dQ[:,:,p] = dP_inf[:,:,p] - tmp \
                       - A.dot(dP_inf[:,:,p]).dot(A.T) - tmp.T
                    
                    dQ[:,:,p] = 0.5*(dQ[:,:,p] + dQ[:,:,p].T) # Symmetrize
            else:
              dA = None
              dQ = None
              Q_noise = Q_noise_2
	      # Innacuracies have been observed when Q_noise_1 was used.
	
            #Q_noise = Q_noise_1

            Q_noise = 0.5*(Q_noise + Q_noise.T) # Symmetrize
            return A, Q_noise,None, dA, dQ

        else: # iterable, array

            # Time discretizations (round to 14 decimals to avoid problems)
            dt_unique, tmp, reconstruct_index = np.unique(np.round(dt,8),
                                        return_index=True,return_inverse=True)
            del tmp
            # Allocate space for A and Q
            A = np.empty((n,n,dt_unique.shape[0]))
            Q_noise = np.empty((n,n,dt_unique.shape[0]))

            if compute_derivatives:
                dA = np.empty((n,n,grad_params_no,dt_unique.shape[0]))
                dQ = np.empty((n,n,grad_params_no,dt_unique.shape[0]))
            else:
                dA = None
                dQ = None
            # Call this function for each unique dt
            for j in range(0,dt_unique.shape[0]):
                A[:,:,j], Q_noise[:,:,j], tmp1, dA_t, dQ_t = ContDescrStateSpace.lti_sde_to_descrete(F,L,Qc,dt_unique[j],
                    compute_derivatives=compute_derivatives, grad_params_no=grad_params_no, P_inf=P_inf, dP_inf=dP_inf, dF = dF, dQc=dQc)
                if compute_derivatives:
                    dA[:,:,:,j] = dA_t
                    dQ[:,:,:,j] = dQ_t

            # Return
            return A, Q_noise, reconstruct_index, dA, dQ