def plot_full(x_all, z_all, b_all):
fig, ax = plt.subplots()
fig.tight_layout()
handles = Plotter.plot_trajectory(ax, x_all)
handles.extend(Plotter.plot_observed_ball_trajectory(ax, z_all))
handles.extend(Plotter.plot_filtered_trajectory(ax, b_all))
ax.legend(handles=handles, loc='upper left')
ax.set_aspect('equal')
plt.show()
# u_all = model.u.repeated(ca.DMatrix.zeros(model.nu, 15)) # u_all[:, 'v'] = 5 # Initial state is drawn from N(m0, S0) # model.init_x0() # Simulate x_all = Simulator.simulate_trajectory(model, u_all) z_all = Simulator.simulate_observed_trajectory(model, x_all) b_all = Simulator.filter_observed_trajectory(model, z_all, u_all) # Plot 2D fig, ax = plt.subplots(figsize=(10, 10)) fig.tight_layout() Plotter.plot_trajectory(ax, x_all) Plotter.plot_observed_ball_trajectory(ax, z_all) Plotter.plot_filtered_trajectory(ax, b_all) # Plot 3D # fig_3D = plt.figure(figsize=(10, 10)) # ax_3D = fig_3D.add_subplot(111, projection='3d') # Plotter.plot_trajectory_3D(ax_3D, x_all) # # plt.show() # ============================================================================ # Model predictive control # ============================================================================ # ----------------------------- Simulation --------------------------------- # # Create models for simulation and planning