<filename>EKF/tools.py
import numpy as np
from math import sin, cos, sqrt


def cartesian_to_polar(x, y, vx, vy, THRESH=0.0001):
    """
    Converts 2d cartesian position and velocity coordinates to polar coordinates

    Args:
      x, y, vx, vy : floats - position and velocity components in cartesian respectively
      THRESH : float - minimum value of rho to return non-zero values

    Returns:
      rho, drho : floats - radius and velocity magnitude respectively
      phi : float - angle in radians
    """

    rho = sqrt(x * x + y * y)
    phi = np.arctan2(y, x)

    if rho < THRESH:
        print("WARNING: in cartesian_to_polar(): d_squared < THRESH")
        rho, phi, drho = 0, 0, 0
    else:
        drho = (x * vx + y * vy) / rho

    return rho, phi, drho


def polar_to_cartesian(rho, phi, drho):
    """
    Converts 2D polar coordinates into cartesian coordinates

    Args:
      rho. drho : floats - radius magnitude and velocity magnitudes respectively
      phi : float - angle in radians

    Returns:
      x, y, vx, vy : floats - position and velocity components in cartesian respectively
    """

    x, y = rho * cos(phi), rho * sin(phi)
    vx, vy = drho * cos(phi), drho * sin(phi)

    return x, y, vx, vy


def time_difference(t1, t2):
    """
    Computes the time difference in microseconds (type: float) of two epoch times values in seconds (type: int)

    Args:
      t1 : int - previous epoch time in seconds
      t2 : int - current epoch time in seconds

    Returns: a float - the time difference in seconds
    """
    return (t2 - t1) / 1000000.0


def get_RMSE(predictions, truths):
    """
    Computes the root mean square errors (RMSE) of the attributes of two lists of DataPoint() instances

    Args:
      predictions - a list of DataPoint() instances
      truths - a list of DataPoint() instances

    Returns:
      px, py, vx, vy - The RMSE of each respective DataPoint() attributes (type: float)
    """
    pxs, pys, vxs, vys = [], [], [], []

    for p, t in zip(predictions, truths):
        ppx, ppy, pvx, pvy = p.get()
        tpx, tpy, tvx, tvy = t.get()

        pxs += [(ppx - tpx) * (ppx - tpx)]
        pys += [(ppy - tpy) * (ppy - tpy)]
        vxs += [(pvx - tvx) * (pvx - tvx)]
        vys += [(pvy - tvy) * (pvy - tvy)]

    px, py = sqrt(np.mean(pxs)), sqrt(np.mean(pys))
    vx, vy = sqrt(np.mean(vxs)), sqrt(np.mean(vys))

    return px, py, vx, vy


def calculate_jacobian(px, py, vx, vy, THRESH=0.0001, ZERO_REPLACEMENT=0.0001):
    """
      Calculates the Jacobian given for four state variables

      Args:
        px, py, vx, vy : floats - four state variables in the system
        THRESH - minimum value of squared distance to return a non-zero matrix
        ZERO_REPLACEMENT - value to replace zero to avoid division by zero error

      Returns:
        H : the jacobian matrix expressed as a 4 x 4 numpy matrix with float values
    """

    d_squared = px * px + py * py
    d = sqrt(d_squared)
    d_cubed = d_squared * d

    if d_squared < THRESH:

        print("WARNING: in calculate_jacobian(): d_squared < THRESH")
        H = np.matrix(np.zeros([3, 4]))

    else:

        r11 = px / d
        r12 = py / d
        r21 = -py / d_squared
        r22 = px / d_squared
        r31 = py * (vx * py - vy * px) / d_cubed
        r32 = px * (vy * px - vx * py) / d_cubed

        H = np.matrix([[r11, r12, 0, 0],
                       [r21, r22, 0, 0],
                       [r31, r32, r11, r12]])

    return H