Sw = 1e1 * np.matrix([[0.25 * Ts**4, 0.5 * Ts**3], [0.5 * Ts**3, Ts**2]])
# Ok, now define the system, giving zero initial conditions.
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())
if __name__ == '__main__':
# this is the sampling time
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()
Sw = np.matrix([[w0]])
Sv = np.matrix([[1e-9]])
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')
