runaway_robot_noisy_filter.py

from math import atan2, cos, sin, sqrt, pi

'''
pr1.2
Created on May 26, 2017

@author: john jimenez (jjimenez36)
'''

def estimate_next_pos(m_i, other = None):
    """Estimate the next (x, y) position of the wandering Traxbot
    based on noisy (x, y) measurements."""
        
    def d():
        ms = other[0]
        return distance(m_i, ms[1])
    
    def d_mu():
        n_d, old_d_mu = other[1:3]
        return mean(old_d_mu, n_d, d())

    def theta2(): return theta(other[0][1], m_i)
    
    def delta_theta():
        m0, m1 = other[0]
        return theta2() - theta(m0, m1)
    
    def delta_theta_mu(): 
        n_theta = other[1] - 1
        delta_t = delta_theta()
        return angle_mean(other[3], n_theta, delta_t) if n_theta > 0 else delta_t

    def calc_next_pos():
        delta_t_mu = delta_theta_mu()
        t3 = theta2() + delta_t_mu
        return add(m_i, prod(d_mu(), (cos(t3), sin(t3))))
    
    def can_calc_theta(): return other and len(other[0]) != 1
    
    def new_other():
        if other:
            ms = other[0]
            n_ms = len(ms)
            new_n_d = other[1] + 1
            new_ms = ms[n_ms - 1:] + (m_i,)
            new_d_mu, new_delta_t_mu = (distance(m_i, ms[0]), None) if n_ms == 1 else (d_mu(), delta_theta_mu())
            return (new_ms, new_n_d, new_d_mu, new_delta_t_mu)
        else: return ((m_i,), 0)
    
    next_pos_est = calc_next_pos() if can_calc_theta() else m_i
    return next_pos_est, new_other()

def add((v0_x, v0_y), (v1_x, v1_y)):
    return v0_x + v1_x, v0_y + v1_y

def prod (coeff, (v_x, v_y)):
    return coeff * v_x, coeff * v_y

def theta((v0_x, v0_y), (v1_x, v1_y)):
    return atan2(v1_y - v0_y, v1_x - v0_x)

def mean(oldmean, n, x):
    return (oldmean * n + x) / (n + 1)

def angle_mean(oldmean, n, a):
    delta = a - oldmean
    # handles -pi/pi boundary
    norm_a = ((-2 * pi) if delta > pi else ((2 * pi) if delta < -pi else 0)) + a
    
    return mean(oldmean, n, norm_a)

# A helper function you may find useful.
def distance(point1, point2):
    """Computes distance between point1 and point2. Points are (x, y) pairs."""
    x1, y1 = point1
    x2, y2 = point2
    return sqrt((x2 - x1) ** 2 + (y2 - y1) ** 2)