# fig_3D = plt.figure(figsize=(10, 10)) # ax_3D = fig_3D.add_subplot(111, projection='3d') # Plotter.plot_trajectory_3D(ax_3D, x_all) # ============================================================================ # Simulate trajectory and observations # ============================================================================ # Nominal controls for simulation # 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) #
