This function implements the basic Kalman Filter algorithm
        These notations for the State-Space model are assumed:
            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 filter distributions x_{k} ~ N(m_{k}, P(k))

        Current Features:
        ----------------------------------------
        1) The function generaly do not modify the passed parameters. If
        it happens then it is an error. There are several exeprions: scalars
        can be modified into a matrix, in some rare cases shapes of
        the derivatives matrices may be changed, it is ignored for now.

        2) Copies of p_A, p_Q, index are created in memory to be used later
        in smoother. References to copies are kept in "matrs_for_smoother"
        return parameter.

        3) 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.

        4) Calculation of Grad_log_likelihood is not supported if matrices A,Q,
        H, or R changes over time. (later may be changed)

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

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

        p_A: scalar, square matrix, 3D array
            A_{k} in the model. If matrix then A_{k} = A - constant.
            If it is 3D array then A_{k} = p_A[:,:, index[0,k]]

        p_Q: scalar, square symmetric matrix, 3D array
            Q_{k-1} in the model. If matrix then Q_{k-1} = Q - constant.
            If it is 3D array then Q_{k-1} = p_Q[:,:, index[1,k]]

        p_H: scalar, matrix (measurement_dim, state_dim) , 3D array
            H_{k} in the model. If matrix then H_{k} = H - constant.
            If it is 3D array then H_{k} = p_Q[:,:, index[2,k]]

        p_R: scalar, square symmetric matrix, 3D array
            R_{k} in the model. If matrix then R_{k} = R - constant.
            If it is 3D array then R_{k} = p_R[:,:, index[3,k]]

        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".

        index: vector
            Which indices (on 3-rd dimension) from arrays p_A, p_Q,p_H, p_R to use
            on every time step. If this parameter is None then it is assumed
            that p_A, p_Q, p_H, p_R do not change over time and indices are not needed.
            index[0,:] - correspond to A, index[1,:] - correspond to Q
            index[2,:] - correspond to H, index[3,:] - correspond to R.
            If index.shape[0] == 1, it is assumed that indides for all matrices
            are the same.

        m_init: vector or matrix
            Initial distribution mean. If None it is assumed to be zero.
            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: square symmetric matrix or scalar
            Initial covariance of the states. If the parameter is scalar
            then it is assumed that initial covariance matrix is unit matrix
            multiplied by this scalar. If None the unit matrix is used instead.
            "multiple time series mode" does not affect it, since it does not
            affect anything related to state variaces.

        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 "grad_calc_params" parameter must
            provide the extra parameters for gradient calculation.

        grad_params_no: int
            If previous parameter is true, then this parameters gives the
            total number of parameters in the gradient.

        grad_calc_params: dictionary
            Dictionary with derivatives of model matrices with respect
            to parameters "dA", "dQ", "dH", "dR", "dm_init", "dP_init".
            They can be None, in this case zero matrices (no dependence on parameters)
            is assumed. If there is only one parameter then third dimension is
            automatically added.

        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.

        matrs_for_smoother: dict
            Dictionary with model functions for smoother. The intrinsic model
            functions are computed in this functions and they are returned to
            use in smoother for convenience. They are: 'p_a', 'p_f_A', 'p_f_Q'
            The dictionary contains the same fields.

def kalman_filter(cls, p_A, p_Q, p_H, p_R, Y, index=None, m_init=None,
                      P_init=None, p_kalman_filter_type='regular',
                      calc_log_likelihood=False,
                      calc_grad_log_likelihood=False, grad_params_no=None,
                      grad_calc_params=None):
        """
        This function implements the basic Kalman Filter algorithm
        These notations for the State-Space model are assumed:
            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 filter distributions x_{k} ~ N(m_{k}, P(k))

        Current Features:
        ----------------------------------------
        1) The function generaly do not modify the passed parameters. If
        it happens then it is an error. There are several exeprions: scalars
        can be modified into a matrix, in some rare cases shapes of
        the derivatives matrices may be changed, it is ignored for now.

        2) Copies of p_A, p_Q, index are created in memory to be used later
        in smoother. References to copies are kept in "matrs_for_smoother"
        return parameter.

        3) 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.

        4) Calculation of Grad_log_likelihood is not supported if matrices A,Q,
        H, or R changes over time. (later may be changed)

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

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

        p_A: scalar, square matrix, 3D array
            A_{k} in the model. If matrix then A_{k} = A - constant.
            If it is 3D array then A_{k} = p_A[:,:, index[0,k]]

        p_Q: scalar, square symmetric matrix, 3D array
            Q_{k-1} in the model. If matrix then Q_{k-1} = Q - constant.
            If it is 3D array then Q_{k-1} = p_Q[:,:, index[1,k]]

        p_H: scalar, matrix (measurement_dim, state_dim) , 3D array
            H_{k} in the model. If matrix then H_{k} = H - constant.
            If it is 3D array then H_{k} = p_Q[:,:, index[2,k]]

        p_R: scalar, square symmetric matrix, 3D array
            R_{k} in the model. If matrix then R_{k} = R - constant.
            If it is 3D array then R_{k} = p_R[:,:, index[3,k]]

        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".

        index: vector
            Which indices (on 3-rd dimension) from arrays p_A, p_Q,p_H, p_R to use
            on every time step. If this parameter is None then it is assumed
            that p_A, p_Q, p_H, p_R do not change over time and indices are not needed.
            index[0,:] - correspond to A, index[1,:] - correspond to Q
            index[2,:] - correspond to H, index[3,:] - correspond to R.
            If index.shape[0] == 1, it is assumed that indides for all matrices
            are the same.

        m_init: vector or matrix
            Initial distribution mean. If None it is assumed to be zero.
            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: square symmetric matrix or scalar
            Initial covariance of the states. If the parameter is scalar
            then it is assumed that initial covariance matrix is unit matrix
            multiplied by this scalar. If None the unit matrix is used instead.
            "multiple time series mode" does not affect it, since it does not
            affect anything related to state variaces.

        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 "grad_calc_params" parameter must
            provide the extra parameters for gradient calculation.

        grad_params_no: int
            If previous parameter is true, then this parameters gives the
            total number of parameters in the gradient.

        grad_calc_params: dictionary
            Dictionary with derivatives of model matrices with respect
            to parameters "dA", "dQ", "dH", "dR", "dm_init", "dP_init".
            They can be None, in this case zero matrices (no dependence on parameters)
            is assumed. If there is only one parameter then third dimension is
            automatically added.

        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.

        matrs_for_smoother: dict
            Dictionary with model functions for smoother. The intrinsic model
            functions are computed in this functions and they are returned to
            use in smoother for convenience. They are: 'p_a', 'p_f_A', 'p_f_Q'
            The dictionary contains the same fields.
        """

        #import pdb; pdb.set_trace()

        # Parameters checking ->
        # index
        p_A = np.atleast_1d(p_A)
        p_Q = np.atleast_1d(p_Q)
        p_H = np.atleast_1d(p_H)
        p_R = np.atleast_1d(p_R)

        # Reshape and check measurements:
        Y.shape, old_Y_shape  = cls._reshape_input_data(Y.shape)
        measurement_dim = Y.shape[1]
        time_series_no = Y.shape[2] # multiple time series mode

        if ((len(p_A.shape) == 3) and (len(p_A.shape[2]) != 1)) or\
            ((len(p_Q.shape) == 3) and (len(p_Q.shape[2]) != 1)) or\
            ((len(p_H.shape) == 3) and (len(p_H.shape[2]) != 1)) or\
            ((len(p_R.shape) == 3) and (len(p_R.shape[2]) != 1)):
            model_matrices_chage_with_time = True
        else:
            model_matrices_chage_with_time = False

        # Check index
        old_index_shape = None
        if index is None:
            if (len(p_A.shape) == 3) or (len(p_Q.shape) == 3) or\
                (len(p_H.shape) == 3) or (len(p_R.shape) == 3):
                raise ValueError("Parameter index can not be None for time varying matrices (third dimension is present)")
            else: # matrices do not change in time, so form dummy zero indices.
                index = np.zeros((1,Y.shape[0]))
        else:
            if len(index.shape) == 1:
                index.shape = (1,index.shape[0])
                old_index_shape = (index.shape[0],)

            if (index.shape[1] != Y.shape[0]):
                raise ValueError("Number of measurements must be equal the number of A_{k}, Q_{k}, H_{k}, R_{k}")

        if (index.shape[0] == 1):
            A_time_var_index = 0; Q_time_var_index = 0
            H_time_var_index = 0; R_time_var_index = 0
        elif (index.shape[0] == 4):
            A_time_var_index = 0; Q_time_var_index = 1
            H_time_var_index = 2; R_time_var_index = 3
        else:
            raise ValueError("First Dimension of index must be either 1 or 4.")

        state_dim = p_A.shape[0]
        # Check and make right shape for model matrices. On exit they all are 3 dimensional. Last dimension
        # correspond to change in time.
        (p_A, old_A_shape) = cls._check_SS_matrix(p_A, state_dim, measurement_dim, which='A')
        (p_Q, old_Q_shape) = cls._check_SS_matrix(p_Q, state_dim, measurement_dim, which='Q')
        (p_H, old_H_shape) = cls._check_SS_matrix(p_H, state_dim, measurement_dim, which='H')
        (p_R, old_R_shape) = cls._check_SS_matrix(p_R, state_dim, measurement_dim, which='R')

        # m_init
        if m_init is None:
            m_init = np.zeros((state_dim, time_series_no))
        else:
            m_init = np.atleast_2d(m_init).T

        # P_init
        if P_init is None:
            P_init = np.eye(state_dim)
        elif not isinstance(P_init, collections.Iterable): #scalar
            P_init = P_init*np.eye(state_dim)

        if p_kalman_filter_type not in ('regular', 'svd'):
            raise ValueError("Kalman filer type neither 'regular nor 'svd'.")

        # Functions to pass to the kalman_filter algorithm:
        # Parameters:
        # k - number of Kalman filter iteration
        # m - vector for calculating matrices. Required for EKF. Not used here.

        c_p_A = p_A.copy() # create a copy because this object is passed to the smoother
        c_p_Q = p_Q.copy() # create a copy because this object is passed to the smoother
        c_index = index.copy() # create a copy because this object is passed to the smoother

        if calc_grad_log_likelihood:
            if model_matrices_chage_with_time:
                raise ValueError("When computing likelihood gradient A and Q can not change over time.")

            dA = cls._check_grad_state_matrices(grad_calc_params.get('dA'), state_dim, grad_params_no, which = 'dA')
            dQ = cls._check_grad_state_matrices(grad_calc_params.get('dQ'), state_dim, grad_params_no, which = 'dQ')
            dH = cls._check_grad_measurement_matrices(grad_calc_params.get('dH'), state_dim, grad_params_no, measurement_dim, which = 'dH')
            dR = cls._check_grad_measurement_matrices(grad_calc_params.get('dR'), state_dim, grad_params_no, measurement_dim, which = 'dR')

            dm_init = grad_calc_params.get('dm_init')
            if dm_init is None:
                 # multiple time series mode. Keep grad_params always as a last dimension
                dm_init = np.zeros((state_dim, time_series_no, grad_params_no))

            dP_init = grad_calc_params.get('dP_init')
            if dP_init is None:
                dP_init = np.zeros((state_dim,state_dim,grad_params_no))
        else:
            dA = None
            dQ = None
            dH = None
            dR = None
            dm_init = None
            dP_init = None

        dynamic_callables = Std_Dynamic_Callables_Class(c_p_A, A_time_var_index, c_p_Q, c_index, Q_time_var_index, 20, dA, dQ)
        measurement_callables = Std_Measurement_Callables_Class(p_H, H_time_var_index, p_R, index, R_time_var_index, 20, dH, dR)

        (M, P,log_likelihood, grad_log_likelihood, dynamic_callables) = \
            cls._kalman_algorithm_raw(state_dim, dynamic_callables,
                                    measurement_callables, Y, m_init,
                                    P_init, p_kalman_filter_type = p_kalman_filter_type,
                                    calc_log_likelihood=calc_log_likelihood,
                                    calc_grad_log_likelihood=calc_grad_log_likelihood,
                                    grad_params_no=grad_params_no,
                                    dm_init=dm_init, dP_init=dP_init)

        # restore shapes so that input parameters are unchenged
        if old_index_shape is not None:
            index.shape = old_index_shape

        if old_Y_shape is not None:
            Y.shape = old_Y_shape

        if old_A_shape is not None:
            p_A.shape = old_A_shape

        if old_Q_shape is not None:
            p_Q.shape = old_Q_shape

        if old_H_shape is not None:
            p_H.shape = old_H_shape

        if old_R_shape is not None:
            p_R.shape = old_R_shape
        # Return values

        return (M, P,log_likelihood, grad_log_likelihood, dynamic_callables)