Ts = 1.0 / 40
# initial condition
x0 = np.matrix([[-6.0], [0.0]])
# define system
di = DoubleIntegrator(Ts=Ts, x0=x0)
# define controller
controller = LQController(di, Q=1 * np.eye(2), R=0.1 * np.eye(1))
# create simulator
sim = SimEnv(di, controller)
# run simulation
res = sim.simulate(6)
# plot results
subplot(311)
plot(res.t, res.x[0], '-')
ylabel('x [m]')
grid()
subplot(312, sharex=gca())
plot(res.t, res.x[1])
ylabel('v [m/s]')
grid()
subplot(313, sharex=gca())
plot(res.t, res.u[0])
xlabel('t [s]')
Ts = 1.0 / 40
# initial condition
x0 = np.matrix([[-6.0], [0.0]])
# define system
di = DoubleIntegrator(Ts=Ts, x0=x0)
# define controller
controller = LQController(di, Q=1 * np.eye(2), R=0.1 * np.eye(1))
# create simulator
sim = SimEnv(di, controller)
# run simulation
res = sim.simulate(6)
# plot results
subplot(311)
plot(res.t, res.x[0], "-")
ylabel("x [m]")
grid()
subplot(312, sharex=gca())
plot(res.t, res.x[1])
ylabel("v [m/s]")
grid()
subplot(313, sharex=gca())
plot(res.t, res.u[0])
xlabel("t [s]")
car = Car(Ts=Ts, Sw=Sw, Sv=Sv, x0=np.matrix([[0.0], [0.0]]))
# define controller. We want a constant value of input. Remember to
# put a value for the accelleration in m/s^2, and not g!
throttle = Throttle(value=9.81 * 1)
# create kalman state observer object
kalman_observer = KalmanStateObserver(car)
# Create simulator.
# This is our simulation obejct; it is responsible of making all
# the parts communicating togheter.
sim = SimEnv(car, throttle, kalman_observer)
# run simulation for 10 seconds
res = sim.simulate(10)
# now do plots
from pylab import *
subplot(311)
plot(res.t, res.y[0], 'b.', label='Position measured')
plot(res.t, res.x[0], 'r-', lw=2, label='Position estimated')
legend(loc=2, numpoints=1)
ylabel('x [m]')
grid()
subplot(312, sharex=gca())
plot(res.t, res.x[1], 'r-', label='Velocity estimated')
ylabel('v [m/s]')
legend(loc=2, numpoints=1)
car = Car( Ts=Ts, Sw=Sw, Sv=Sv, x0 = np.matrix( [[0.0],[0.0]] ) )
# define controller. We want a constant value of input. Remember to
# put a value for the accelleration in m/s^2, and not g!
throttle = Throttle( value=9.81*1 )
# create kalman state observer object
kalman_observer = KalmanStateObserver( car )
# Create simulator.
# This is our simulation obejct; it is responsible of making all
# the parts communicating togheter.
sim = SimEnv( car, throttle, kalman_observer )
# run simulation for 10 seconds
res = sim.simulate( 10 )
# now do plots
from pylab import *
subplot(311)
plot ( res.t, res.y[0], 'b.', label='Position measured' )
plot ( res.t, res.x[0], 'r-', lw=2, label='Position estimated' )
legend(loc=2, numpoints=1)
ylabel( 'x [m]' )
grid()
subplot(312, sharex=gca())
plot ( res.t, res.x[1], 'r-', label='Velocity estimated' )
ylabel( 'v [m/s]' )
legend(loc=2, numpoints=1)
NoisyDtLTISystem.__init__( self, A, B, C, D, Ts, Sw, Sv, x0 )
if __name__ == '__main__':
# define system
single_integ = SingleIntegrator( Ts=10e-3, w0=1e-4, x0=np.matrix([[5]]) )
# define non-linear controller subject to saturation
nl_controller = NonLinearController( K = 50, alpha=0.9, saturation=(-4, 0.5) )
# create simulator
sim = SimEnv( single_integ, controller=nl_controller )
# simulate system for one second
res = sim.simulate( 5.0 )
# do plots
from pylab import *
subplot(211)
plot ( res.t, res.x[0] )
ylabel('e [m]')
grid()
subplot(212, sharex=gca())
plot ( res.t, res.u[0] )
ylabel('u')
xlabel('t [s]')
grid()
NoisyDtLTISystem.__init__(self, A, B, C, D, Ts, Sw, Sv, x0)
if __name__ == '__main__':
# define system
single_integ = SingleIntegrator(Ts=10e-3, w0=1e-4, x0=np.matrix([[5]]))
# define non-linear controller subject to saturation
nl_controller = NonLinearController(K=50, alpha=0.9, saturation=(-4, 0.5))
# create simulator
sim = SimEnv(single_integ, controller=nl_controller)
# simulate system for one second
res = sim.simulate(5.0)
# do plots
from pylab import *
subplot(211)
plot(res.t, res.x[0])
ylabel('e [m]')
grid()
subplot(212, sharex=gca())
plot(res.t, res.u[0])
ylabel('u')
xlabel('t [s]')
grid()