Rauch–Tung–Striebel(RTS) update step

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

        p_m: matrix of size (state_dim, time_series_no)
             Filter mean on step k

        p_P:  matrix of size (state_dim,state_dim)
             Filter Covariance on step k

        p_m_pred: matrix of size (state_dim, time_series_no)
             Means from the smoother prediction step.

        p_P_pred:
             Covariance from the smoother prediction step.

        p_m_prev_step
            Smoother mean from the previous step.

        p_P_prev_step:
            Smoother covariance from the previous step.

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

def _rts_smoother_update_step(k, p_m , p_P, p_m_pred, p_P_pred, p_m_prev_step,
                                  p_P_prev_step, p_dynamic_callables):
        """
        Rauch–Tung–Striebel(RTS) update step

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

        p_m: matrix of size (state_dim, time_series_no)
             Filter mean on step k

        p_P:  matrix of size (state_dim,state_dim)
             Filter Covariance on step k

        p_m_pred: matrix of size (state_dim, time_series_no)
             Means from the smoother prediction step.

        p_P_pred:
             Covariance from the smoother prediction step.

        p_m_prev_step
            Smoother mean from the previous step.

        p_P_prev_step:
            Smoother covariance from the previous step.

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

        """

        A = p_dynamic_callables.Ak(k,p_m,p_P) # state transition matrix (or Jacobian)

        tmp = np.dot( A, p_P.T)
        if A.shape[0] == 1: # 1D states
            G = tmp.T / p_P_pred # P[:,:,k] is symmetric
        else:
            try:
                LL,islower = linalg.cho_factor(p_P_pred)
                G = linalg.cho_solve((LL,islower),tmp).T
            except:
                # It happende that p_P_pred has several near zero eigenvalues
                # hence the Cholesky method does not work.
                res = sp.linalg.lstsq(p_P_pred, tmp)
                G = res[0].T

        m_upd = p_m + G.dot( p_m_prev_step-p_m_pred )
        P_upd = p_P + G.dot( p_P_prev_step-p_P_pred).dot(G.T)

        P_upd = 0.5*(P_upd + P_upd.T)

        return m_upd, P_upd, G