def predict(x,u,A,B,P,Q):
    """
    Prediction step of the Kalman filter
    
    Input:
    x - state vector
    u - input vector
    A - process matrix
    B - input-to-state matrix
    P - state covariance matrix
    Q - evolution noise
    
    Returns:
    x - predicted state
    P - predicted covariance
    """
    xpls = A.dot(x) + B.dot(u)
    P = A.dot(P).dot(A.T) + Q
    return xpls, P