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 main(): A = build_matrix() U, Es = gauss(A) E = multiply(multiply(Es[2], Es[1]), Es[0]) L = gauss_jordan(E) T = transpose(A) D, new_U = factorize_D(U) assert multiply(E, A) == U assert multiply(E, A) != multiply(A, E) assert multiply(L, U) == A assert multiply(multiply(L, D), new_U) == A assert transpose(T) == A
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)))]
def prediction(self): self.estimated_position = multiply(self.F, self.estimated_position) estimated_cov_matrix = multiply(self.F, self.cov_matrix, transpose(self.F)) + self.Ex measurements = self.selectiveMeasurements(estimated_cov_matrix) h = np.zeros(self.sensor_size) for i in range(0, self.sensor_size): if measurements[i] != 0: h[i] = self.model.spaceToValue(self.estimated_position[0:2] - self.basestations[i].position) H = np.empty((0, 4)) for i in range(0, len(measurements)): if measurements[i] != 0: dh_dx, dh_dy = self.model.derivative(self.estimated_position[0:2] - self.basestations[i].position) else: dh_dx, dh_dy = 0.0, 0.0 H = np.append(H, np.array([[dh_dx, dh_dy, 0.0, 0.0]]), axis=0) return H, h, measurements, estimated_cov_matrix