Extended Kalman Filter

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

        p_state_dim: integer

        p_a: if None - the function from the linear model is assumed. No non-
             linearity in the dynamic is assumed.

             function (k, x_{k-1}, A_{k}). Dynamic function.
             k: (iteration number),
             x_{k-1}: (previous state)
             x_{k}: Jacobian matrices of f_a. In the linear case it is exactly A_{k}.

        p_f_A: matrix - in this case function which returns this matrix is assumed.
               Look at this parameter description in kalman_filter function.

               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: matrix. In this case function which returns this matrix is asumed.
               Look at this parameter description in kalman_filter function.

               function (k). Returns noise matrix of dynamic model on iteration k.
               k: (iteration number).

        p_h: if None - the function from the linear measurement model is assumed.
             No nonlinearity in the measurement is assumed.

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

        p_f_H: matrix - in this case function which returns this matrix is assumed.
               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: matrix. In this case function which returns this matrix is asumed.
               function (k). Returns noise matrix of measurement equation
               on iteration k.
               k: (iteration number).

        Y: matrix or vector
            Data. If Y is matrix then samples are along 0-th dimension and
            features along the 1-st. May have missing values.

        p_mean: vector
            Initial distribution mean. If None it is assumed to be zero

        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.

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

def extended_kalman_filter(cls,p_state_dim, p_a, p_f_A, p_f_Q, p_h, p_f_H, p_f_R, Y, m_init=None,
                          P_init=None,calc_log_likelihood=False):

        """
        Extended Kalman Filter

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

        p_state_dim: integer

        p_a: if None - the function from the linear model is assumed. No non-
             linearity in the dynamic is assumed.

             function (k, x_{k-1}, A_{k}). Dynamic function.
             k: (iteration number),
             x_{k-1}: (previous state)
             x_{k}: Jacobian matrices of f_a. In the linear case it is exactly A_{k}.

        p_f_A: matrix - in this case function which returns this matrix is assumed.
               Look at this parameter description in kalman_filter function.

               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: matrix. In this case function which returns this matrix is asumed.
               Look at this parameter description in kalman_filter function.

               function (k). Returns noise matrix of dynamic model on iteration k.
               k: (iteration number).

        p_h: if None - the function from the linear measurement model is assumed.
             No nonlinearity in the measurement is assumed.

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

        p_f_H: matrix - in this case function which returns this matrix is assumed.
               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: matrix. In this case function which returns this matrix is asumed.
               function (k). Returns noise matrix of measurement equation
               on iteration k.
               k: (iteration number).

        Y: matrix or vector
            Data. If Y is matrix then samples are along 0-th dimension and
            features along the 1-st. May have missing values.

        p_mean: vector
            Initial distribution mean. If None it is assumed to be zero

        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.

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

        # Y
        Y.shape, old_Y_shape  =  cls._reshape_input_data(Y.shape)

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

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

        if p_a is None:
            p_a = lambda k,m,A: np.dot(A, m)

        old_A_shape = None
        if not isinstance(p_f_A, types.FunctionType): # not a function but array
            p_f_A = np.atleast_1d(p_f_A)
            (p_A, old_A_shape) = cls._check_A_matrix(p_f_A)

            p_f_A = lambda k, m, P: p_A[:,:, 0] # make function
        else:
            if p_f_A(1, m_init, P_init).shape[0] != m_init.shape[0]:
                raise ValueError("p_f_A function returns matrix of wrong size")

        old_Q_shape = None
        if not isinstance(p_f_Q, types.FunctionType): # not a function but array
            p_f_Q = np.atleast_1d(p_f_Q)
            (p_Q, old_Q_shape) = cls._check_Q_matrix(p_f_Q)

            p_f_Q = lambda k: p_Q[:,:, 0] # make function
        else:
            if p_f_Q(1).shape[0] != m_init.shape[0]:
                raise ValueError("p_f_Q function returns matrix of wrong size")

        if p_h is None:
            lambda k,m,H: np.dot(H, m)

        old_H_shape = None
        if not isinstance(p_f_H, types.FunctionType): # not a function but array
            p_f_H = np.atleast_1d(p_f_H)
            (p_H, old_H_shape) = cls._check_H_matrix(p_f_H)

            p_f_H = lambda k, m, P: p_H # make function
        else:
            if p_f_H(1, m_init, P_init).shape[0] != Y.shape[1]:
                raise ValueError("p_f_H function returns matrix of wrong size")

        old_R_shape = None
        if not isinstance(p_f_R, types.FunctionType): # not a function but array
            p_f_R = np.atleast_1d(p_f_R)
            (p_R, old_R_shape) = cls._check_H_matrix(p_f_R)

            p_f_R = lambda k: p_R # make function
        else:
            if p_f_R(1).shape[0] != m_init.shape[0]:
                raise ValueError("p_f_R function returns matrix of wrong size")

#        class dynamic_callables_class(Dynamic_Model_Callables):
#
#            Ak =
#            Qk =


        class measurement_callables_class(R_handling_Class):
            def __init__(self,R, index, R_time_var_index, unique_R_number):
                super(measurement_callables_class,self).__init__(R, index, R_time_var_index, unique_R_number)

            Hk = AddMethodToClass(f_H)
            f_h = AddMethodToClass(f_hl)


        (M, P,log_likelihood, grad_log_likelihood)  = cls._kalman_algorithm_raw(p_state_dim, p_a, p_f_A, p_f_Q, p_h, p_f_H, p_f_R, Y, m_init,
                          P_init, calc_log_likelihood,
                          calc_grad_log_likelihood=False, grad_calc_params=None)

        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 (M, P)