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)))]