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)
