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