t_elapse = 0.1 # Simulation time elapsed between each iteration t_pause = 0.01 # Pause between each iteration user_input = Sliders() # Instantiate Sliders class simAnimation = Animation() # Instantiate Animate class dynam = Dynamics() # Instantiate Dynamics class t = t_start # Declare time variable to keep track of simulation time elapsed while t < t_end: plt.ion() # Make plots interactive plt.figure( user_input.fig.number) # Switch current figure to user_input figure plt.pause(0.001) # Pause the simulation to detect user input # The dynamics of the model will be propagated in time by t_elapse # at intervals of t_Ts. t_temp = t + t_elapse while t < t_temp: dynam.propagateDynamics( # Propagate the dynamics of the model in time user_input.getInputValues()) t += t_Ts # Update time elapsed plt.figure( simAnimation.fig.number) # Switch current figure to animation figure simAnimation.drawSystem( # Update animation with current user input dynam.Outputs()) # time.sleep(t_pause)
t = t_start # Declare time variable to keep track of simulation time elapsed while t < t_end: ref_input = sig_gen.getRefInputs(t) # The dynamics of the model will be propagated in time by t_elapse # at intervals of t_Ts. t_temp = t + t_elapse while t < t_temp: states = dynam.States() # Get current states u = ctrl.getForces(ref_input, states) # Calculate the forces xhat = ctrl.getObsStates() # Get xhat dynam.propagateDynamics( u) # Propagate the dynamics of the model in time t = round(t + t_Ts, 2) # Update time elapsed # plt.figure(simAnimation.fig.number) # Switch current figure to animation figure # simAnimation.drawSystem( # Update animation with current user input # dynam.Outputs()) # plt.pause(0.0001) # Organizes the new data to be passed to plotGen new_data = [[ref_input[0], states[0], xhat[0][0]], [states[0] - xhat[0][0]], [states[2], xhat[2][0]], [states[2] - xhat[2][0]], [states[1], xhat[1][0]], [states[1] - xhat[1][0]], [states[3], xhat[3][0]], [states[3] - xhat[3][0]], [u[0]]] plotGen.updateDataHistory(t, new_data)
actual = np.array([]) t = P.t_start while t < P.t_end: t_next_plot = t + P.t_plot while t < t_next_plot: t = t + P.Ts vc = 1.25 + 0.5 * np.cos(2 * np.pi * (0.2) * t) omegac = -0.5 + 0.2 * np.cos(2 * np.pi * (0.6) * t) noise_v = vc + np.random.normal( 0, np.sqrt(P.alpha1 * vc**2 + P.alpha2 * omegac**2)) noise_omega = omegac + np.random.normal( 0, np.sqrt(P.alpha3 * vc**2 + P.alpha4 * omegac**2)) u = [noise_v, noise_omega] dynamics.propagateDynamics(u) ekf_slam.run(dynamics.states(), u) animation.draw(dynamics.states(), ekf_slam) if estimate.size == 0: estimate = np.array(ekf_slam.get_mu()) actual = np.array(dynamics.states()) else: estimate = np.vstack((estimate, ekf_slam.get_mu())) actual = np.vstack((actual, dynamics.states())) dataPlot.update(t, dynamics.states(), ekf_slam.get_mu(), ekf_slam.get_sig()) plt.pause(0.001)
while t < t_end: # Get referenced inputs from signal generators ref_input = sig_gen.getRefInputs(t) ref_input[0] = ref_input[0] + 3 ref_input[1] = ref_input[1] + 3 # The dynamics of the model will be propagated in time by t_elapse # at intervals of t_Ts. t_temp = t + t_elapse while t < t_temp: states = dynam.States() # Get current states u = ctrl.getForces(ref_input, states) # Calculate the forces dynam.propagateDynamics( ref_input) # Propagate the dynamics of the model in time t = round(t + t_Ts, 2) # Update time elapsed # plt.figure(simAnimation.fig.number) # Switch current figure to animation figure # simAnimation.drawSystem( # Update animation with current user input # dynam.Outputs()) # plt.pause(0.00001) # Organizes the new data to be passed to plotGen new_data = [[ref_input[1], states[1]], [ref_input[0], states[0]], [u[2], states[2]], [u[0], u[1]]] plotGen.updateDataHistory(t, new_data) # plt.figure(plotGen.fig.number) # Switch current figure to plotGen figure # plotGen.update_plots() # Update the plot # plt.pause(0.0001)