General nonlinear filtering algorithm for inference in the state-space
        model:

        x_{k} =  f_a(k, x_{k-1}, A_{k}) + q_{k-1};       q_{k-1} ~ N(0, Q_{k-1})
        y_{k} =  f_h(k, x_{k}, H_{k}) + r_{k};           r_{k-1} ~ N(0, R_{k})

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

        Current Features:
        ----------------------------------------

        1) Function support "multiple time series mode" which means that exactly
        the same State-Space model is used to filter several sets of measurements.
        In this case third dimension of Y should include these state-space measurements
        Log_likelihood and Grad_log_likelihood have the corresponding dimensions then.

        2) Measurement may include missing values. In this case update step is
        not done for this measurement. (later may be changed)

        Input:
        -----------------
        state_dim: int
            Demensionality of the states

        p_a: function (k, x_{k-1}, A_{k}). Dynamic function.
            k (iteration number),
            x_{k-1}
            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),
            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).

        p_h: function (k, x_{k}, H_{k}). Measurement function.
            k (iteration number),
            x_{k}
            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 dynamic function, it is
            passed into p_h.
            k (iteration number),
            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).

        Y: matrix or vector or 3D array
            Data. If Y is matrix then samples are along 0-th dimension and
            features along the 1-st. If 3D array then third dimension
            correspond to "multiple time series mode".

        m_init: vector or matrix
            Initial distribution mean. For "multiple time series mode"
            it is matrix, second dimension of which correspond to different
            time series. In regular case ("one time series mode") it is a
            vector.

        P_init: matrix or scalar
            Initial covariance of the states. Must be not None
            "multiple time series mode" does not affect it, since it does not
            affect anything related to state variaces.

        p_kalman_filter_type: string


        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.

        grad_calc_params: dictionary
            Dictionary with derivatives of model matrices with respect
            to parameters "dA", "dQ", "dH", "dR", "dm_init", "dP_init".

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

        M: (no_steps+1,state_dim) matrix or (no_steps+1,state_dim, time_series_no) 3D array
            Filter estimates of the state means. In the extra step the initial
            value is included. In the "multiple time series mode" third dimension
            correspond to different timeseries.

        P: (no_steps+1, state_dim, state_dim) 3D array
            Filter estimates of the state covariances. In the extra step the initial
            value is included.

        log_likelihood: double or (1, time_series_no) 3D array.
            If the parameter calc_log_likelihood was set to true, return
            logarithm of marginal likelihood of the state-space model. If
            the parameter was false, return None. In the "multiple time series mode" it is a vector
            providing log_likelihood for each time series.

        grad_log_likelihood: column vector or (grad_params_no, time_series_no) matrix
            If calc_grad_log_likelihood is true, return gradient of log likelihood
            with respect to parameters. It returns it column wise, so in
            "multiple time series mode" gradients for each time series is in the
            corresponding column.

def _kalman_algorithm_raw(cls,state_dim, p_dynamic_callables, p_measurement_callables, Y, m_init,
                          P_init, p_kalman_filter_type='regular',
                          calc_log_likelihood=False,
                          calc_grad_log_likelihood=False, grad_params_no=None,
                          dm_init=None, dP_init=None):
        """
        General nonlinear filtering algorithm for inference in the state-space
        model:

        x_{k} =  f_a(k, x_{k-1}, A_{k}) + q_{k-1};       q_{k-1} ~ N(0, Q_{k-1})
        y_{k} =  f_h(k, x_{k}, H_{k}) + r_{k};           r_{k-1} ~ N(0, R_{k})

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

        Current Features:
        ----------------------------------------

        1) Function support "multiple time series mode" which means that exactly
        the same State-Space model is used to filter several sets of measurements.
        In this case third dimension of Y should include these state-space measurements
        Log_likelihood and Grad_log_likelihood have the corresponding dimensions then.

        2) Measurement may include missing values. In this case update step is
        not done for this measurement. (later may be changed)

        Input:
        -----------------
        state_dim: int
            Demensionality of the states

        p_a: function (k, x_{k-1}, A_{k}). Dynamic function.
            k (iteration number),
            x_{k-1}
            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),
            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).

        p_h: function (k, x_{k}, H_{k}). Measurement function.
            k (iteration number),
            x_{k}
            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 dynamic function, it is
            passed into p_h.
            k (iteration number),
            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).

        Y: matrix or vector or 3D array
            Data. If Y is matrix then samples are along 0-th dimension and
            features along the 1-st. If 3D array then third dimension
            correspond to "multiple time series mode".

        m_init: vector or matrix
            Initial distribution mean. For "multiple time series mode"
            it is matrix, second dimension of which correspond to different
            time series. In regular case ("one time series mode") it is a
            vector.

        P_init: matrix or scalar
            Initial covariance of the states. Must be not None
            "multiple time series mode" does not affect it, since it does not
            affect anything related to state variaces.

        p_kalman_filter_type: string


        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.

        grad_calc_params: dictionary
            Dictionary with derivatives of model matrices with respect
            to parameters "dA", "dQ", "dH", "dR", "dm_init", "dP_init".

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

        M: (no_steps+1,state_dim) matrix or (no_steps+1,state_dim, time_series_no) 3D array
            Filter estimates of the state means. In the extra step the initial
            value is included. In the "multiple time series mode" third dimension
            correspond to different timeseries.

        P: (no_steps+1, state_dim, state_dim) 3D array
            Filter estimates of the state covariances. In the extra step the initial
            value is included.

        log_likelihood: double or (1, time_series_no) 3D array.
            If the parameter calc_log_likelihood was set to true, return
            logarithm of marginal likelihood of the state-space model. If
            the parameter was false, return None. In the "multiple time series mode" it is a vector
            providing log_likelihood for each time series.

        grad_log_likelihood: column vector or (grad_params_no, time_series_no) matrix
            If calc_grad_log_likelihood is true, return gradient of log likelihood
            with respect to parameters. It returns it column wise, so in
            "multiple time series mode" gradients for each time series is in the
            corresponding column.

        """

        steps_no = Y.shape[0] # number of steps in the Kalman Filter
        time_series_no = Y.shape[2] # multiple time series mode

        # Allocate space for results
        # Mean estimations. Initial values will be included
        M = np.empty(((steps_no+1),state_dim,time_series_no))
        M[0,:,:] = m_init # Initialize mean values
        # Variance estimations. Initial values will be included
        P = np.empty(((steps_no+1),state_dim,state_dim))
        P_init = 0.5*( P_init + P_init.T) # symmetrize initial covariance. In some ustable cases this is uiseful
        P[0,:,:] = P_init # Initialize initial covariance matrix

        if p_kalman_filter_type == 'svd':
            (U,S,Vh) = sp.linalg.svd( P_init,full_matrices=False, compute_uv=True,
                      overwrite_a=False,check_finite=True)
            S[ (S==0) ] = 1e-17 # allows to run algorithm for singular initial variance
            P_upd = (P_init, S,U)

        log_likelihood = 0 if calc_log_likelihood else None
        grad_log_likelihood = 0 if calc_grad_log_likelihood else None

        #setting initial values for derivatives update
        dm_upd = dm_init
        dP_upd = dP_init
        # Main loop of the Kalman filter
        for k in range(0,steps_no):
            # In this loop index for new estimations is (k+1), old - (k)
            # This happened because initial values are stored at 0-th index.

            prev_mean = M[k,:,:] # mean from the previous step

            if p_kalman_filter_type == 'svd':
                m_pred, P_pred, dm_pred, dP_pred = \
                cls._kalman_prediction_step_SVD(k, prev_mean ,P_upd, p_dynamic_callables,
                    calc_grad_log_likelihood=calc_grad_log_likelihood,
                    p_dm = dm_upd, p_dP = dP_upd)
            else:
                m_pred, P_pred, dm_pred, dP_pred = \
                cls._kalman_prediction_step(k, prev_mean ,P[k,:,:], p_dynamic_callables,
                    calc_grad_log_likelihood=calc_grad_log_likelihood,
                    p_dm = dm_upd, p_dP = dP_upd )

            k_measurment = Y[k,:,:]

            if (np.any(np.isnan(k_measurment)) == False):
                if p_kalman_filter_type == 'svd':
                    m_upd, P_upd, log_likelihood_update, dm_upd, dP_upd, d_log_likelihood_update = \
                    cls._kalman_update_step_SVD(k,  m_pred , P_pred, p_measurement_callables,
                            k_measurment, calc_log_likelihood=calc_log_likelihood,
                            calc_grad_log_likelihood=calc_grad_log_likelihood,
                            p_dm = dm_pred, p_dP = dP_pred )


    #                m_upd, P_upd, log_likelihood_update, dm_upd, dP_upd, d_log_likelihood_update = \
    #                cls._kalman_update_step(k,  m_pred , P_pred[0], f_h, f_H, p_R.f_R, k_measurment,
    #                        calc_log_likelihood=calc_log_likelihood,
    #                        calc_grad_log_likelihood=calc_grad_log_likelihood,
    #                        p_dm = dm_pred, p_dP = dP_pred, grad_calc_params_2 = (dH, dR))
    #
    #                (U,S,Vh) = sp.linalg.svd( P_upd,full_matrices=False, compute_uv=True,
    #                      overwrite_a=False,check_finite=True)
    #                P_upd = (P_upd, S,U)
                else:
                    m_upd, P_upd, log_likelihood_update, dm_upd, dP_upd, d_log_likelihood_update = \
                    cls._kalman_update_step(k,  m_pred , P_pred, p_measurement_callables, k_measurment,
                            calc_log_likelihood=calc_log_likelihood,
                            calc_grad_log_likelihood=calc_grad_log_likelihood,
                            p_dm = dm_pred, p_dP = dP_pred )

            else:
#                if k_measurment.shape != (1,1):
#                    raise ValueError("Nan measurements are currently not supported for \
#                                     multidimensional output and multiple time series.")
#                else:
#                    m_upd = m_pred; P_upd = P_pred; dm_upd = dm_pred; dP_upd = dP_pred
#                    log_likelihood_update = 0.0;
#                    d_log_likelihood_update = 0.0;

                if not np.all(np.isnan(k_measurment)):
                    raise ValueError("""Nan measurements are currently not supported if
                                     they are intermixed with not NaN measurements""")
                else:
                    m_upd = m_pred; P_upd = P_pred; dm_upd = dm_pred; dP_upd = dP_pred
                    if calc_log_likelihood:
                        log_likelihood_update = np.zeros((time_series_no,))
                    if calc_grad_log_likelihood:
                        d_log_likelihood_update = np.zeros((grad_params_no,time_series_no))


            if calc_log_likelihood:
                log_likelihood += log_likelihood_update

            if calc_grad_log_likelihood:
                grad_log_likelihood += d_log_likelihood_update

            M[k+1,:,:] = m_upd # separate mean value for each time series

            if p_kalman_filter_type == 'svd':
                P[k+1,:,:] = P_upd[0]
            else:
                P[k+1,:,:] = P_upd

        # !!!Print statistics! Print sizes of matrices
        # !!!Print statistics! Print iteration time base on another boolean variable
        return (M, P, log_likelihood, grad_log_likelihood, p_dynamic_callables.reset(False))