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
