def updateState(self, measurements, probabilities, p0):
x_p = self.x_p
p_p = self.p_p
if measurements.size > 0:
y_u, y_i = self.__getInnovation__(x_p, measurements, probabilities)
s_u = KalmanFilter.__getInnovationCovariance__(self, p_p)
k_u = KalmanFilter.__getKalmanGain__(self, s_u, p_p)
x_u = KalmanFilter.__getUpdatedState__(self, x_p, k_u, y_u)
p_u = self.__getStateCovariance__(k_u, p_p, s_u, y_u, y_i,
probabilities, p0)
else:
self.x_u = x_p
self.p_u = p_p
self.measurement = measurements
self.probabilities = probabilities
