def update_ekf(self, z):
"""Updates the state by using Extended Kalman Filter equations
Args:
z (:obj:`Matrix`): The measurement at k+1
"""
"""
Todo:
* update the state by using Extended Kalman Filter equations
"""
#Lesson 5 Section 14
px = self._x.value[0][0]
py = self._x.value[1][0]
vx = self._x.value[2][0]
vy = self._x.value[3][0]
rho = sqrt(px * px + py * py)
theta = atan2(py, px)
ro_dot = (px * vx + py * vy) / rho
z_pred = Matrix([[rho], [theta], [ro_dot]])
y = z - z_pred
if (y.value[1][0] > pi):
y.value[1][0] -= 2 * pi
elif (y.value[1][0] < (-pi)):
y.value[1][0] += 2 * pi
#Lesson 5 Section 7
Ht = self._H.transpose()
S = self._H * self._P * Ht + self._R
Si = S.inverse()
PHt = self._P * Ht
K = PHt * Si
#new estimate
self._x = self._x + (K * y)
x_size = self._x.dimx
I = Matrix([[]])
I.identity(x_size)
self._P = (I - K * self._H) * self._P
def update(self, z):
"""Updates the state by using standard Kalman Filter equations
Args:
z (:obj:`Matrix`): The measurement at k+1
"""
"""
Todo:
* update the state by using Kalman Filter equations
"""
z_pred = self._H * self._x
y = z - z_pred
Ht = self._H.transpose()
S = self._H * self._P * Ht + self._R
Si = S.inverse()
PHt = self._P * Ht
K = PHt * Si
#new estimate
self._x = self._x + (K * y)
x_size = self._x.dimx
I = Matrix([[]])
I.identity(x_size)
self._P = (I - K * self._H) * self._P