-
Notifications
You must be signed in to change notification settings - Fork 0
/
backward_euler.py
72 lines (57 loc) · 2.25 KB
/
backward_euler.py
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
# -*- coding: utf-8 -*-
"""
Created on Wed Dec 12 22:30:56 2018
@author: Aleksandar Haber
Simulation of the continuous-time state-space model using the backward Euler method.
"""
import numpy as np
import matplotlib.pyplot as plt
# define the continuous-time system matrices
A=np.matrix([[0, 1],[- 0.1, -0.05]])
B=np.matrix([[0],[1]])
C=np.matrix([[1, 0]])
#define an initial state for simulation
x0=np.random.rand(2,1)
#define the number of time-samples used for the simulation and the sampling time for the discretization
time=300
sampling=0.5
#define an input sequence for the simulation
#input_seq=np.random.rand(time,1)
input_seq=np.ones(time)
#plt.plot(input_sequence)
# the following function simulates the state-space model using the backward Euler method
# the input parameters are:
# -- A,B,C - continuous time system matrices
# -- initial_state - the initial state of the system
# -- time_steps - the total number of simulation time steps
# -- sampling_perios - the sampling period for the backward Euler discretization
# this function returns the state sequence and the output sequence
# they are stored in the vectors Xd and Yd respectively
def simulate(A,B,C,initial_state,input_sequence, time_steps,sampling_period):
from numpy.linalg import inv
I=np.identity(A.shape[0]) # this is an identity matrix
Ad=inv(I-sampling_period*A)
Bd=Ad*sampling_period*B
Xd=np.zeros(shape=(A.shape[0],time_steps+1))
Yd=np.zeros(shape=(C.shape[0],time_steps+1))
for i in range(0,time_steps):
if i==0:
Xd[:,[i]]=initial_state
Yd[:,[i]]=C*initial_state
x=Ad*initial_state+Bd*input_sequence[i]
else:
Xd[:,[i]]=x
Yd[:,[i]]=C*x
x=Ad*x+Bd*input_sequence[i]
Xd[:,[-1]]=x
Yd[:,[-1]]=C*x
return Xd, Yd
state,output=simulate(A,B,C,x0,input_seq, time ,sampling)
plt.plot(output[0,:])
plt.xlabel('Discrete time instant-k')
plt.ylabel('Position- d')
plt.title('System step response')
plt.savefig('step_response.png')
###############################################################################
# end of model definition and simulation
###############################################################################