def get_observer(self, stepSize, noise_settings):
""":arg timeStep: step size of simulation
:return observer object """
input_matrix_variance, output_matrix_variance, input_noise_variance, output_noise_variance, bdisable_input_noise, bdisable_output_noise, bdisable_N_matrix, bdisable_W_matrix = noise_settings
combo_idx = self.observer_combo.currentIndex()
# the chosen combo entry defines the type of planner that is returned
if combo_idx == 0:
print("Linear Kalman Filter")
# 1. Operational Point
# lamb_op = self.linear_kalman_lamb_op.value() * np.pi / 180
e_op = self.linear_kalman_e_op.value() * np.pi / 180
# 2. Initial values
p_init = self.linear_kalman_init_p.value() * np.pi / 180
e_init = self.linear_kalman_init_e.value() * np.pi / 180
lamb_init = self.linear_kalman_init_lamb.value() * np.pi / 180
dp_init = self.linear_kalman_init_dp.value() * np.pi / 180
de_init = self.linear_kalman_init_de.value() * np.pi / 180
dlamb_init = self.linear_kalman_init_dlamb.value() * np.pi / 180
f_init = self.linear_kalman_init_f.value()
b_init = self.linear_kalman_init_b.value()
# 3. Initial covariance
p_cov_init = self.linear_kalman_init_cov_p.value() * (np.pi / 180)**2
e_cov_init = self.linear_kalman_init_cov_e.value() * (np.pi / 180)**2
lamb_cov_init = self.linear_kalman_init_cov_lamb.value() * (np.pi / 180)**2
dp_cov_init = self.linear_kalman_init_cov_dp.value() * (np.pi / 180)**2
de_cov_init = self.linear_kalman_init_cov_de.value() * (np.pi / 180)**2
dlamb_cov_init = self.linear_kalman_init_cov_dlamb.value() * (np.pi / 180)**2
f_cov_init = self.linear_kalman_init_cov_f.value()
b_cov_init = self.linear_kalman_init_cov_b.value()
# 4. Model Type
model_name = self.linear_kalman_model_type.currentText()
if model_name == "simple":
model_type = ModelType.EASY
elif model_name == "gyromoment":
model_type = ModelType.GYROMOMENT
else:
raise NotImplementedError("Combo element not implemented yet")
# 5. Number of Outputs
nOutputs_text = self.linear_kalman_nOutputs_combo.currentText()
if nOutputs_text == "p, e, lambda":
nOutputs = 3
elif nOutputs_text == "p, e, lambda, f, b":
nOutputs = 5
else:
raise NotImplementedError("Combo element not implemented yet")
# 6. should check limits
should_check_limits = self.linear_kalman_should_limit_checkbox.checkState() == 2
# 7. dynamic inertia
dynamic_inertia = self.linear_kalman_dynamic_inertia_checkbox.checkState() == 2
if model_type == ModelType.GYROMOMENT:
init_state_vector = np.array([p_init, e_init, lamb_init, dp_init, de_init, dlamb_init, f_init, b_init])
init_cov_matrix = np.diagflat([p_cov_init, e_cov_init, lamb_cov_init,
dp_cov_init, de_cov_init, dlamb_cov_init,
f_cov_init, b_cov_init])
else:
init_state_vector = np.array([p_init, e_init, lamb_init, dp_init, de_init, dlamb_init])
init_cov_matrix = np.diagflat([p_cov_init, e_cov_init, lamb_cov_init,
dp_cov_init, de_cov_init, dlamb_cov_init])
observer = Observer.LinearKalmanFilterWithLimits(init_state_vector, init_cov_matrix, model_type, e_op,
nOutputs, stepSize, input_matrix_variance, output_matrix_variance,
input_noise_variance, output_noise_variance)
observer.set_should_limit(should_check_limits)
observer.set_dynamic_inertia(dynamic_inertia)
elif combo_idx == 1:
print("Extended Kalman Filter")
# 2. Initial values
p_init = self.ext_kalman_init_p.value() * np.pi / 180
e_init = self.ext_kalman_init_e.value() * np.pi / 180
lamb_init = self.ext_kalman_init_lamb.value() * np.pi / 180
dp_init = self.ext_kalman_init_dp.value() * np.pi / 180
de_init = self.ext_kalman_init_de.value() * np.pi / 180
dlamb_init = self.ext_kalman_init_dlamb.value() * np.pi / 180
f_init = self.ext_kalman_init_f.value()
b_init = self.ext_kalman_init_b.value()
# 3. Initial covariance
p_cov_init = self.ext_kalman_init_cov_p.value() * (np.pi / 180)**2
e_cov_init = self.ext_kalman_init_cov_e.value() * (np.pi / 180)**2
lamb_cov_init = self.ext_kalman_init_cov_lamb.value() * (np.pi / 180)**2
dp_cov_init = self.ext_kalman_init_cov_dp.value() * (np.pi / 180)**2
de_cov_init = self.ext_kalman_init_cov_de.value() * (np.pi / 180)**2
dlamb_cov_init = self.ext_kalman_init_cov_dlamb.value() * (np.pi / 180)**2
f_cov_init = self.ext_kalman_init_cov_f.value()
b_cov_init = self.ext_kalman_init_cov_b.value()
# 4. Model Type
model_name = self.ext_kalman_model_type.currentText()
if model_name == "simple":
model_type = ModelType.EASY
elif model_name == "gyromoment":
model_type = ModelType.GYROMOMENT
else:
raise NotImplementedError("Combo element not implemented yet")
# 5. Number of Outputs
nOutputs_text = self.ext_kalman_nOutputs_combo.currentText()
if nOutputs_text == "p, e, lambda":
nOutputs = 3
elif nOutputs_text == "p, e, lambda, f, b":
nOutputs = 5
else:
raise NotImplementedError("Combo element not implemented yet")
# 6. should check limits
should_check_limits = self.ext_kalman_should_limit_checkbox.checkState() == 2
# 7. dynamic inertia
dynamic_inertia = self.ext_kalman_dynamic_inertia_checkbox.checkState() == 2
init_state_vector = np.array([p_init, e_init, lamb_init, dp_init, de_init, dlamb_init, f_init, b_init])
init_cov_matrix = np.diagflat([p_cov_init, e_cov_init, lamb_cov_init,
dp_cov_init, de_cov_init, dlamb_cov_init,
f_cov_init, b_cov_init])
if model_type == ModelType.GYROMOMENT:
if nOutputs == 5:
observer = Observer.ExtKalmanFilterGyroModelLimits(init_state_vector, init_cov_matrix,
stepSize, input_matrix_variance,
output_matrix_variance,
input_noise_variance, output_noise_variance)
elif nOutputs == 3:
observer = Observer.ExtKalmanFilterGyroModelLimitsOnly3(init_state_vector, init_cov_matrix,
stepSize, input_matrix_variance,
output_matrix_variance,
input_noise_variance, output_noise_variance)
else:
observer = Observer.ExtKalmanFilterEasyModelLimits(init_state_vector, init_cov_matrix,
stepSize, input_matrix_variance,
output_matrix_variance,
input_noise_variance, output_noise_variance)
observer.set_should_limit(should_check_limits)
observer.set_dynamic_inertia(dynamic_inertia)
elif combo_idx == 2:
observer = Observer.NoKalmanFilter(np.zeros(6), np.diag(np.zeros(6)))
else:
raise NotImplementedError("This option of the combo box is not implemented yet.")
# disable noise or enable it
if bdisable_input_noise:
observer.toggle_input_noise(False)
if bdisable_output_noise:
observer.toggle_output_noise(False)
if bdisable_N_matrix:
observer.toggle_input_variance(False)
if bdisable_W_matrix:
observer.toggle_output_variance(False)
return observer
