def _EM_gradient(A,Q,H,R,m_init,P_init,measurements, M, P, G, dA, dQ, dH, dR, dm_init, dP_init):
        """
        Gradient computation with the EM algorithm.

        Input:
        -----------------

        M: Means from the smoother
        P: Variances from the smoother
        G: Gains? from the smoother
        """
        import pdb; pdb.set_trace();

        param_number = dA.shape[-1]
        d_log_likelihood_update = np.empty((param_number,1))

        sample_no = measurements.shape[0]
        P_1 = P[1:,:,:] # remove 0-th step
        P_2 = P[0:-1,:,:] # remove 0-th step

        M_1 = M[1:,:] # remove 0-th step
        M_2 = M[0:-1,:] # remove the last step

        Sigma = np.mean(P_1,axis=0) + np.dot(M_1.T, M_1) / sample_no #
        Phi =   np.mean(P_2,axis=0) + np.dot(M_2.T, M_2) / sample_no #

        B = np.dot( measurements.T, M_1 )/ sample_no
        C =   (sp.einsum( 'ijk,ikl', P_1, G) + np.dot(M_1.T, M_2)) / sample_no #

#        C1 = np.zeros( (P_1.shape[1],P_1.shape[1]) )
#        for k in range(P_1.shape[0]):
#            C1 += np.dot(P_1[k,:,:],G[k,:,:]) + sp.outer( M_1[k,:], M_2[k,:] )
#        C1 = C1 / sample_no

        D = np.dot( measurements.T, measurements ) / sample_no

        try:
            P_init_inv = sp.linalg.inv(P_init)

            if np.max( np.abs(P_init_inv)) > 10e13:
                compute_P_init_terms = False
            else:
                compute_P_init_terms = True
        except np.linalg.LinAlgError:
            compute_P_init_terms = False

        try:
            Q_inv = sp.linalg.inv(Q)

            if np.max( np.abs(Q_inv)) > 10e13:
                compute_Q_terms = False
            else:
                compute_Q_terms = True
        except np.linalg.LinAlgError:
            compute_Q_terms = False

        try:
            R_inv = sp.linalg.inv(R)

            if np.max( np.abs(R_inv)) > 10e13:
                compute_R_terms = False
            else:
                compute_R_terms = True
        except np.linalg.LinAlgError:
            compute_R_terms = False


        d_log_likelihood_update = np.zeros((param_number,1))
        for j in range(param_number):
            if compute_P_init_terms:
                d_log_likelihood_update[j,:] -= 0.5 * np.sum(P_init_inv* dP_init[:,:,j].T ) #p #m

                M0_smoothed = M[0]; M0_smoothed.shape = (M0_smoothed.shape[0],1)
                tmp1 = np.dot( dP_init[:,:,j], np.dot( P_init_inv, (P[0,:,:] + sp.outer( (M0_smoothed - m_init), (M0_smoothed - m_init) )) )  ) #p #m
                d_log_likelihood_update[j,:] += 0.5 * np.sum(P_init_inv* tmp1.T )

                tmp2 = sp.outer( dm_init[:,j], M0_smoothed )
                tmp2 += tmp2.T
                d_log_likelihood_update[j,:] += 0.5 * np.sum(P_init_inv* tmp2.T )

            if compute_Q_terms:

                d_log_likelihood_update[j,:] -=  sample_no/2.0 * np.sum(Q_inv* dQ[:,:,j].T ) #m

                tmp1 = np.dot(C,A.T); tmp1 += tmp1.T; tmp1 = Sigma - tmp1 + np.dot(A, np.dot(Phi,A.T)) #m
                tmp1 = np.dot( dQ[:,:,j], np.dot( Q_inv, tmp1) )
                d_log_likelihood_update[j,:] += sample_no/2.0 * np.sum(Q_inv * tmp1.T)

                tmp2 = np.dot( dA[:,:,j], C.T); tmp2 += tmp2.T;
                tmp3 = np.dot(dA[:,:,j], np.dot(Phi,A.T)); tmp3 += tmp3.T
                d_log_likelihood_update[j,:] -= sample_no/2.0 * np.sum(Q_inv.T * (tmp3 - tmp2) )

            if compute_R_terms:
                d_log_likelihood_update[j,:] -=  sample_no/2.0 * np.sum(R_inv* dR[:,:,j].T )

                tmp1 = np.dot(B,H.T); tmp1 += tmp1.T; tmp1 = D - tmp1 + np.dot(H, np.dot(Sigma,H.T))
                tmp1 = np.dot( dR[:,:,j], np.dot( R_inv, tmp1) )
                d_log_likelihood_update[j,:] += sample_no/2.0 * np.sum(R_inv * tmp1.T)

                tmp2 = np.dot( dH[:,:,j], B.T); tmp2 += tmp2.T;
                tmp3 = np.dot(dH[:,:,j], np.dot(Sigma,H.T)); tmp3 += tmp3.T
                d_log_likelihood_update[j,:] -= sample_no/2.0 * np.sum(R_inv.T * (tmp3 - tmp2) )

        return d_log_likelihood_update
def _EM_gradient ( A , Q , H , R , m_init , P_init , measurements , M , P , G , dA , dQ , dH , dR , dm_init , dP_init ) : import pdb ; pdb . set_trace ( ) ; param_number = dA . shape [ - 1 ] d_log_likelihood_update = np . empty ( ( param_number , 1 ) ) sample_no = measurements . shape [ 0 ] P_1 = P [ 1 : , : , : ] P_2 = P [ 0 : - 1 , : , : ] M_1 = M [ 1 : , : ] M_2 = M [ 0 : - 1 , : ] Sigma = np . mean ( P_1 , axis = 0 ) + np . dot ( M_1 . T , M_1 ) / sample_no Phi = np . mean ( P_2 , axis = 0 ) + np . dot ( M_2 . T , M_2 ) / sample_no B = np . dot ( measurements . T , M_1 ) / sample_no C = ( sp . einsum ( 'ijk,ikl' , P_1 , G ) + np . dot ( M_1 . T , M_2 ) ) / sample_no D = np . dot ( measurements . T , measurements ) / sample_no try : P_init_inv = sp . linalg . inv ( P_init ) if np . max ( np . abs ( P_init_inv ) ) > 10e13 : compute_P_init_terms = False else : compute_P_init_terms = True except np . linalg . LinAlgError : compute_P_init_terms = False try : Q_inv = sp . linalg . inv ( Q ) if np . max ( np . abs ( Q_inv ) ) > 10e13 : compute_Q_terms = False else : compute_Q_terms = True except np . linalg . LinAlgError : compute_Q_terms = False try : R_inv = sp . linalg . inv ( R ) if np . max ( np . abs ( R_inv ) ) > 10e13 : compute_R_terms = False else : compute_R_terms = True except np . linalg . LinAlgError : compute_R_terms = False d_log_likelihood_update = np . zeros ( ( param_number , 1 ) ) for j in range ( param_number ) : if compute_P_init_terms : d_log_likelihood_update [ j , : ] -= 0.5 * np . sum ( P_init_inv * dP_init [ : , : , j ] . T ) M0_smoothed = M [ 0 ] ; M0_smoothed . shape = ( M0_smoothed . shape [ 0 ] , 1 ) tmp1 = np . dot ( dP_init [ : , : , j ] , np . dot ( P_init_inv , ( P [ 0 , : , : ] + sp . outer ( ( M0_smoothed - m_init ) , ( M0_smoothed - m_init ) ) ) ) ) d_log_likelihood_update [ j , : ] += 0.5 * np . sum ( P_init_inv * tmp1 . T ) tmp2 = sp . outer ( dm_init [ : , j ] , M0_smoothed ) tmp2 += tmp2 . T d_log_likelihood_update [ j , : ] += 0.5 * np . sum ( P_init_inv * tmp2 . T ) if compute_Q_terms : d_log_likelihood_update [ j , : ] -= sample_no / 2.0 * np . sum ( Q_inv * dQ [ : , : , j ] . T ) tmp1 = np . dot ( C , A . T ) ; tmp1 += tmp1 . T ; tmp1 = Sigma - tmp1 + np . dot ( A , np . dot ( Phi , A . T ) ) tmp1 = np . dot ( dQ [ : , : , j ] , np . dot ( Q_inv , tmp1 ) ) d_log_likelihood_update [ j , : ] += sample_no / 2.0 * np . sum ( Q_inv * tmp1 . T ) tmp2 = np . dot ( dA [ : , : , j ] , C . T ) ; tmp2 += tmp2 . T ; tmp3 = np . dot ( dA [ : , : , j ] , np . dot ( Phi , A . T ) ) ; tmp3 += tmp3 . T d_log_likelihood_update [ j , : ] -= sample_no / 2.0 * np . sum ( Q_inv . T * ( tmp3 - tmp2 ) ) if compute_R_terms : d_log_likelihood_update [ j , : ] -= sample_no / 2.0 * np . sum ( R_inv * dR [ : , : , j ] . T ) tmp1 = np . dot ( B , H . T ) ; tmp1 += tmp1 . T ; tmp1 = D - tmp1 + np . dot ( H , np . dot ( Sigma , H . T ) ) tmp1 = np . dot ( dR [ : , : , j ] , np . dot ( R_inv , tmp1 ) ) d_log_likelihood_update [ j , : ] += sample_no / 2.0 * np . sum ( R_inv * tmp1 . T ) tmp2 = np . dot ( dH [ : , : , j ] , B . T ) ; tmp2 += tmp2 . T ; tmp3 = np . dot ( dH [ : , : , j ] , np . dot ( Sigma , H . T ) ) ; tmp3 += tmp3 . T d_log_likelihood_update [ j , : ] -= sample_no / 2.0 * np . sum ( R_inv . T * ( tmp3 - tmp2 ) ) return d_log_likelihood_update
Gradient computation with the EM algorithm.

        Input:
        -----------------

        M: Means from the smoother
        P: Variances from the smoother
        G: Gains? from the smoother