Gradient computation with the EM algorithm.

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

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

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