Returns log-likelihood of the measurement z given the Gaussian
    posterior (x, P) using measurement function H and measurement
    covariance error R

def log_likelihood(z, x, P, H, R):
    """
    Returns log-likelihood of the measurement z given the Gaussian
    posterior (x, P) using measurement function H and measurement
    covariance error R
    """
    S = np.dot(H, np.dot(P, H.T)) + R
    return logpdf(z, np.dot(H, x), S)