measurement update with heading(yaw)

ekf_measure_update_heading.py

"""
tracker.py
"""
yaw_prev = self.tracks[idx_old_t].akf.state.item(3)
yaw = box[4]

while yaw_prev < 0:
yaw_prev += 2.0*np.pi

while yaw_prev > 2.0*np.pi:
yaw_prev -= 2.0*np.pi

assert yaw_prev >= 0.0 and yaw_prev <= 2.0 * np.pi

yaw_candidate = [yaw]
yaw_candidate.append(yaw + np.pi / 2.0)
yaw_candidate.append(yaw + np.pi)
yaw_candidate.append(yaw + 3.0 * np.pi / 2.0)

yaw_diff = 100.0                    
heading = yaw
for yaw_i in yaw_candidate:
diff = np.abs(yaw_prev - yaw_i)

if yaw_diff > diff:
    heading = yaw_i
    yaw_diff = diff

if heading > 2.0 * np.pi:
heading -= 2.0 * np.pi
measu = [x, y, heading]


"""
adaptive_kalman_filter.py
"""

if self.model == 1: # car model
    x = self.state
    P = self.covar

    z = np.matrix([[measu[0], measu[1], measu[2]]]).T # x, y, yaw

    rpos = 0.1**2 # Standard Deviation
    ryaw = 0.01**2 # Standard Deviation
    R = np.matrix([[rpos,  0.0,  0.0],
                   [0.0,  rpos,  0.0],
                   [0.0,   0.0, ryaw]])

    # Measurement Update (Correction)
    # ===============================
    # Measurement Function  
    JH = np.matrix([[1.0, 0.0, 0.0, 0.0, 0.0], # x
                    [0.0, 1.0, 0.0, 0.0, 0.0], # y
                    [0.0, 0.0, 0.0, 1.0, 0.0]])# yaw

    hx = JH * x

    S = JH * P * JH.T + R
    K = (P * JH.T) * np.linalg.inv(S)

    # Update the estimate via z
    y = z - (hx) # Innovation or Residual
    x = x + (K*y)

    # Update the error covariance
    I = np.eye(5)
    P = (I - (K*JH)) * P

    self.state = x
    self.covar = P