def correction(self, H, h, measurements, estimated_cov_matrix):
H_hat = multiply(H, estimated_cov_matrix, transpose(H))
K = multiply(estimated_cov_matrix, transpose(H), invert(H_hat + self.Ez))
self.estimated_position += multiply(K, (measurements - h))
self.cov_matrix = multiply((np.eye(N=4) - multiply(K, H)), estimated_cov_matrix)
self.prediction_sequence.append(transpose(self.estimated_position))
def selectBestPositions(self, estimated_cov_matrix, estimated_position):
estimated_cov_matrix_inv = invert(estimated_cov_matrix)
distances = np.ones(self.sensor_size) * -1
for i in range(self.sensor_size):
extended_basestation_pos = np.append(self.basestations[i].position, np.array([0, 0]))
difference = transpose(estimated_position) - extended_basestation_pos
distances[i] = multiply(difference, estimated_cov_matrix_inv, transpose(difference))
valid_distances = self.sortWithIndeces(distances)
return [valid_distances[i][0] for i in range(0, min(3, len(valid_distances)))]
