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