Input:

        k: int
              Iteration No. Starts at 0. Total number of iterations equal to the
              number of measurements.

        m_P: matrix of size (state_dim, time_series_no)
             Mean value from the previous step. For "multiple time series mode"
                it is matrix, second dimension of which correspond to different
                time series.

        p_P: tuple (P_pred, S, V)
             Covariance matrix from the prediction step and its SVD decomposition.
             P_pred = V * S * V.T The tuple is (P_pred, S, V)

        p_h: function (k, x_{k}, H_{k}). Measurement function.
            k (iteration number), starts at 0
            x_{k} state
            H_{k} Jacobian matrices of f_h. In the linear case it is exactly H_{k}.

        p_f_H: function (k, m, P) return Jacobian of measurement function, it is
            passed into p_h.
            k (iteration number), starts at 0
            m: point where Jacobian is evaluated
            P: parameter for Jacobian, usually covariance matrix.

        p_f_R: function (k). Returns noise matrix of measurement equation
            on iteration k.
            k (iteration number). starts at 0

        p_f_iRsr: function (k). Returns the square root of the noise matrix of
            measurement equation on iteration k.
            k (iteration number). starts at 0

        measurement: (measurement_dim, time_series_no) matrix
            One measurement used on the current update step. For
            "multiple time series mode" it is matrix, second dimension of
            which correspond to different time series.

        calc_log_likelihood: boolean
            Whether to calculate marginal likelihood of the state-space model.

        calc_grad_log_likelihood: boolean
                Whether to calculate gradient of the marginal likelihood
                of the state-space model. If true then the next parameter must
                provide the extra parameters for gradient calculation.

        p_dm: 3D array (state_dim, time_series_no, parameters_no)
                Mean derivatives from the prediction step. For "multiple time series mode"
                it is 3D array, second dimension of which correspond to different
                time series.

        p_dP: array
            Covariance derivatives from the prediction step.

        grad_calc_params_2: List or None
            List with derivatives. The first component is 'f_dH' - function(k)
            which returns the derivative of H. The second element is 'f_dR'
             - function(k). Function which returns the derivative of R.

        Output:
        ----------------------------
        m_upd, P_upd, dm_upd, dP_upd: metrices, 3D objects
            Results of the prediction steps.

        log_likelihood_update: double or 1D array
            Update to the log_likelihood from this step

        d_log_likelihood_update: (grad_params_no, time_series_no) matrix
            Update to the gradient of log_likelihood, "multiple time series mode"
            adds extra columns to the gradient.

def _kalman_update_step_SVD(k, p_m , p_P, p_meas_model_callable, measurement, calc_log_likelihood= False,
                            calc_grad_log_likelihood=False, p_dm = None, p_dP = None):
        """
        Input:

        k: int
              Iteration No. Starts at 0. Total number of iterations equal to the
              number of measurements.

        m_P: matrix of size (state_dim, time_series_no)
             Mean value from the previous step. For "multiple time series mode"
                it is matrix, second dimension of which correspond to different
                time series.

        p_P: tuple (P_pred, S, V)
             Covariance matrix from the prediction step and its SVD decomposition.
             P_pred = V * S * V.T The tuple is (P_pred, S, V)

        p_h: function (k, x_{k}, H_{k}). Measurement function.
            k (iteration number), starts at 0
            x_{k} state
            H_{k} Jacobian matrices of f_h. In the linear case it is exactly H_{k}.

        p_f_H: function (k, m, P) return Jacobian of measurement function, it is
            passed into p_h.
            k (iteration number), starts at 0
            m: point where Jacobian is evaluated
            P: parameter for Jacobian, usually covariance matrix.

        p_f_R: function (k). Returns noise matrix of measurement equation
            on iteration k.
            k (iteration number). starts at 0

        p_f_iRsr: function (k). Returns the square root of the noise matrix of
            measurement equation on iteration k.
            k (iteration number). starts at 0

        measurement: (measurement_dim, time_series_no) matrix
            One measurement used on the current update step. For
            "multiple time series mode" it is matrix, second dimension of
            which correspond to different time series.

        calc_log_likelihood: boolean
            Whether to calculate marginal likelihood of the state-space model.

        calc_grad_log_likelihood: boolean
                Whether to calculate gradient of the marginal likelihood
                of the state-space model. If true then the next parameter must
                provide the extra parameters for gradient calculation.

        p_dm: 3D array (state_dim, time_series_no, parameters_no)
                Mean derivatives from the prediction step. For "multiple time series mode"
                it is 3D array, second dimension of which correspond to different
                time series.

        p_dP: array
            Covariance derivatives from the prediction step.

        grad_calc_params_2: List or None
            List with derivatives. The first component is 'f_dH' - function(k)
            which returns the derivative of H. The second element is 'f_dR'
             - function(k). Function which returns the derivative of R.

        Output:
        ----------------------------
        m_upd, P_upd, dm_upd, dP_upd: metrices, 3D objects
            Results of the prediction steps.

        log_likelihood_update: double or 1D array
            Update to the log_likelihood from this step

        d_log_likelihood_update: (grad_params_no, time_series_no) matrix
            Update to the gradient of log_likelihood, "multiple time series mode"
            adds extra columns to the gradient.

        """

        #import pdb; pdb.set_trace()

        m_pred = p_m # from prediction step
        P_pred,S_pred,V_pred = p_P # from prediction step

        H = p_meas_model_callable.Hk(k, m_pred, P_pred)
        R = p_meas_model_callable.Rk(k)
        R_isr = p_meas_model_callable.R_isrk(k) # square root of the inverse of R matrix

        time_series_no = p_m.shape[1] # number of time serieses

        log_likelihood_update=None; dm_upd=None; dP_upd=None; d_log_likelihood_update=None
        # Update step (only if there is data)
        #if not np.any(np.isnan(measurement)): # TODO: if some dimensions are missing, do properly computations for other.
        v = measurement-p_meas_model_callable.f_h(k, m_pred, H)

        svd_2_matr = np.vstack( ( np.dot( R_isr.T, np.dot(H, V_pred)) , np.diag( 1.0/np.sqrt(S_pred) ) ) )

        (U,S,Vh) = sp.linalg.svd( svd_2_matr,full_matrices=False, compute_uv=True,
                     overwrite_a=False,check_finite=True)

         # P_upd = U_upd S_upd**2 U_upd.T
        U_upd = np.dot(V_pred, Vh.T)
        S_upd = (1.0/S)**2

        P_upd = np.dot(U_upd * S_upd, U_upd.T) # update covariance
        P_upd = (P_upd,S_upd,U_upd) # tuple to pass to the next step

         # stil need to compute S and K for derivative computation
        S = H.dot(P_pred).dot(H.T) + R
        if measurement.shape[0]==1: # measurements are one dimensional
            if (S < 0):
                raise ValueError("Kalman Filter Update: S is negative step %i" % k )
                 #import pdb; pdb.set_trace()

            K = P_pred.dot(H.T) / S
            if calc_log_likelihood:
                log_likelihood_update = -0.5 * ( np.log(2*np.pi) + np.log(S) +
                                    v*v / S)
                #log_likelihood_update = log_likelihood_update[0,0] # to make int
                if np.any(np.isnan(log_likelihood_update)): # some member in P_pred is None.
                    raise ValueError("Nan values in likelihood update!")
            LL = None; islower = None
        else:
            LL,islower = linalg.cho_factor(S)
            K = linalg.cho_solve((LL,islower), H.dot(P_pred.T)).T

            if calc_log_likelihood:
                log_likelihood_update = -0.5 * ( v.shape[0]*np.log(2*np.pi) +
                    2*np.sum( np.log(np.diag(LL)) ) +\
                        np.sum((linalg.cho_solve((LL,islower),v)) * v, axis = 0) ) # diagonal of v.T*S^{-1}*v


        # Old  method of computing updated covariance (for testing) ->
        #P_upd_tst = K.dot(S).dot(K.T)
        #P_upd_tst = 0.5*(P_upd_tst + P_upd_tst.T)
        #P_upd_tst =  P_pred - P_upd_tst# this update matrix is symmetric
        # Old  method of computing updated covariance (for testing) <-

        if calc_grad_log_likelihood:
            dm_pred_all_params = p_dm # derivativas of the prediction phase
            dP_pred_all_params = p_dP

            param_number = p_dP.shape[2]

            dH_all_params = p_meas_model_callable.dHk(k)
            dR_all_params = p_meas_model_callable.dRk(k)

            dm_upd = np.empty(dm_pred_all_params.shape)
            dP_upd = np.empty(dP_pred_all_params.shape)

             # firts dimension parameter_no, second - time series number
            d_log_likelihood_update = np.empty((param_number,time_series_no))
            for param in range(param_number):

               dH = dH_all_params[:,:,param]
               dR = dR_all_params[:,:,param]

               dm_pred = dm_pred_all_params[:,:,param]
               dP_pred = dP_pred_all_params[:,:,param]

                # Terms in the likelihood derivatives
               dv = - np.dot( dH, m_pred) -  np.dot( H, dm_pred)
               dS = np.dot(dH, np.dot( P_pred, H.T))
               dS += dS.T
               dS += np.dot(H, np.dot( dP_pred, H.T)) + dR

                # TODO: maybe symmetrize dS

                #dm and dP for the next stem
               if LL is not None: # the state vector is not a scalar
                   tmp1 = linalg.cho_solve((LL,islower), H).T
                   tmp2 = linalg.cho_solve((LL,islower), dH).T
                   tmp3 = linalg.cho_solve((LL,islower), dS).T
               else: # the state vector is a scalar
                   tmp1 = H.T / S
                   tmp2 = dH.T / S
                   tmp3 = dS.T / S

               dK = np.dot( dP_pred, tmp1) + np.dot( P_pred, tmp2) - \
                    np.dot( P_pred, np.dot( tmp1, tmp3 ) )

               # terms required for the next step, save this for each parameter
               dm_upd[:,:,param] = dm_pred + np.dot(dK, v) + np.dot(K, dv)

               dP_upd[:,:,param] = -np.dot(dK, np.dot(S, K.T))
               dP_upd[:,:,param] += dP_upd[:,:,param].T
               dP_upd[:,:,param] += dP_pred - np.dot(K , np.dot( dS, K.T))

               dP_upd[:,:,param] = 0.5*(dP_upd[:,:,param] + dP_upd[:,:,param].T) #symmetrize
               # computing the likelihood change for each parameter:
               if LL is not None: # the state vector is not 1D
                   tmp5 = linalg.cho_solve((LL,islower), v)
               else: # the state vector is a scalar
                   tmp5 = v / S


               d_log_likelihood_update[param,:] = -(0.5*np.sum(np.diag(tmp3)) + \
                   np.sum(tmp5*dv, axis=0) - 0.5 * np.sum(tmp5 * np.dot(dS, tmp5), axis=0) )
                # Before
                #d_log_likelihood_update[param,0] = -(0.5*np.sum(np.diag(tmp3)) + \
                #np.dot(tmp5.T, dv) - 0.5 * np.dot(tmp5.T ,np.dot(dS, tmp5)) )

        # Compute the actual updates for mean of the states. Variance update
        # is computed earlier.
        m_upd = m_pred + K.dot( v )

        return m_upd, P_upd, log_likelihood_update, dm_upd, dP_upd, d_log_likelihood_update