Suppose that we have a state-space represented differential equation like : $$ \frac{d \mathbf x}{dt} = \mathbf A \mathbf x(t) + \mathbf b \mathbf u(t) $$
Using euler's algorithm, the update equation from $\mathbf x(t)$ to $\mathbf x(t+\Delta t)$ is written as : $$ \mathbf x(t+\Delta t) = \mathbf x(t) + \frac{d \mathbf x}{dt} \Delta t = \mathbf x(t) + (\mathbf A \mathbf x(t) + \mathbf b \mathbf u) \Delta t $$ where $\Delta t$ is the simulation time-step.
The following script is an example of the algorithm for simulating a one-dimentional spring-mass-damper model.
In [1]:
import numpy as np
import matplotlib.pyplot as plt
%matplotlib inline
In [2]:
m = 10
k = 50
c = 10
T = 10
dt = 1e-3
d = 2
# Initial State
x0 = 0
v0 = 0
A = np.asarray([ [0, 1], [-k/m, -c/m]])
b = np.asarray([0,1/m])
In [3]:
N = int(T/dt)
# Initialize all data
t_list = np.arange(N) * dt
u_list = np.zeros_like(t_list)
u_list[ np.logical_and(1 <= t_list, t_list <= 2) ] = 10
x_list = np.zeros((d, N))
x = x_list[:,0] = np.asarray([x0,v0])
for n in range(1, N):
u = u_list[n]
dxdt = np.dot(A, x) + np.dot(b, u)
x = x + dxdt * dt
x_list[:,n] = x
(fig, axs) = plt.subplots(3, 1, figsize=(12, 5), sharex=True)
ax = axs[0]
ax.plot(t_list, x_list[0])
ax.set_ylabel('Pos (m)')
ax = axs[1]
ax.plot(t_list, x_list[1])
ax.set_ylabel('Velocity (m/s)')
ax = axs[2]
ax.plot(t_list, u_list)
ax.set_ylabel('Input (N)')
ax.set_xlabel('Time (s)')
for ax in axs:
ax.grid()