This function implements Rauch–Tung–Striebel(RTS) smoother algorithm
        based on the results of kalman_filter_raw.
        These notations are the same:
            x_{k} =  A_{k} * x_{k-1} + q_{k-1};       q_{k-1} ~ N(0, Q_{k-1})
            y_{k} = H_{k} * x_{k} + r_{k};            r_{k-1} ~ N(0, R_{k})

        Returns estimated smoother distributions x_{k} ~ N(m_{k}, P(k))

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

        p_a: function (k, x_{k-1}, A_{k}). Dynamic function.
                k (iteration number), starts at 0
                x_{k-1} State from the previous step
                A_{k} Jacobian matrices of f_a. In the linear case it is exactly A_{k}.

        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.

        p_f_Q: function (k). Returns noise matrix of dynamic model on iteration k.
            k (iteration number). starts at 0

        filter_means: (no_steps+1,state_dim) matrix or (no_steps+1,state_dim, time_series_no) 3D array
            Results of the Kalman Filter means estimation.

        filter_covars: (no_steps+1, state_dim, state_dim) 3D array
            Results of the Kalman Filter covariance estimation.

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

        M: (no_steps+1, state_dim) matrix
            Smoothed estimates of the state means

        P: (no_steps+1, state_dim, state_dim) 3D array
            Smoothed estimates of the state covariances

def rts_smoother(cls,state_dim, p_dynamic_callables, filter_means,
                          filter_covars):
        """
        This function implements Rauch–Tung–Striebel(RTS) smoother algorithm
        based on the results of kalman_filter_raw.
        These notations are the same:
            x_{k} =  A_{k} * x_{k-1} + q_{k-1};       q_{k-1} ~ N(0, Q_{k-1})
            y_{k} = H_{k} * x_{k} + r_{k};            r_{k-1} ~ N(0, R_{k})

        Returns estimated smoother distributions x_{k} ~ N(m_{k}, P(k))

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

        p_a: function (k, x_{k-1}, A_{k}). Dynamic function.
                k (iteration number), starts at 0
                x_{k-1} State from the previous step
                A_{k} Jacobian matrices of f_a. In the linear case it is exactly A_{k}.

        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.

        p_f_Q: function (k). Returns noise matrix of dynamic model on iteration k.
            k (iteration number). starts at 0

        filter_means: (no_steps+1,state_dim) matrix or (no_steps+1,state_dim, time_series_no) 3D array
            Results of the Kalman Filter means estimation.

        filter_covars: (no_steps+1, state_dim, state_dim) 3D array
            Results of the Kalman Filter covariance estimation.

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

        M: (no_steps+1, state_dim) matrix
            Smoothed estimates of the state means

        P: (no_steps+1, state_dim, state_dim) 3D array
            Smoothed estimates of the state covariances
        """

        no_steps = filter_covars.shape[0]-1# number of steps (minus initial covariance)

        M = np.empty(filter_means.shape) # smoothed means
        P = np.empty(filter_covars.shape) # smoothed covars
        #G = np.empty( (no_steps,state_dim,state_dim)  ) # G from the update step of the smoother

        M[-1,:] = filter_means[-1,:]
        P[-1,:,:] = filter_covars[-1,:,:]
        for k in range(no_steps-1,-1,-1):

            m_pred, P_pred, tmp1, tmp2 = \
                    cls._kalman_prediction_step(k, filter_means[k,:],
                                                filter_covars[k,:,:], p_dynamic_callables,
                                                calc_grad_log_likelihood=False)
            p_m = filter_means[k,:]
            if len(p_m.shape)<2:
                p_m.shape = (p_m.shape[0],1)

            p_m_prev_step = M[k+1,:]
            if len(p_m_prev_step.shape)<2:
                p_m_prev_step.shape = (p_m_prev_step.shape[0],1)

            m_upd, P_upd, G_tmp = cls._rts_smoother_update_step(k,
                            p_m ,filter_covars[k,:,:],
                            m_pred, P_pred, p_m_prev_step ,P[k+1,:,:], p_dynamic_callables)

            M[k,:] = m_upd#np.squeeze(m_upd)
            P[k,:,:] = P_upd
            #G[k,:,:] = G_upd.T # store transposed G.
        # Return values

        return (M, P)