Dynamic Matrix Controller

gistfile1.py

import scipy
import numpy
import scipy.signal
import matplotlib.pyplot as plt
from scipy.linalg import toeplitz
from scipy.linalg import hankel
from numpy.linalg import inv

numpy.set_printoptions(linewidth=500)

class DMC(object):
   # P: Prediction Horizon
   # M: Control Horizon
   def __init__(self, sr, K, P, M):
      if P >= sr.size:
         raise Exception('prediction horizon must be less than size of unit step response') 

      self.K = K
      self.P = P
      self.y = 0
      self.sr = sr

      self.u = 0

      # storage for past input
      self.numpast = sr.size-P
      self.pastinput = numpy.zeros(self.numpast)

      # matrix to calculate free response from past input
      self.free = numpy.subtract(hankel(sr[1:P+1], sr[P:sr.size]), numpy.tile(sr[0:sr.size-P].T, (P, 1)))

      # set up Gain Matrix
      tmp = numpy.zeros(M)
      tmp[0] = sr[0]

      B = toeplitz(sr[0:P], tmp)
      I = numpy.identity(M)*(K)

      # gain matrix
      self.G = inv(numpy.add(B.T.dot(B), I)).dot(B.T)[0]

        
   # y: process output
   def calc(self, sp, y):

      f = numpy.add(numpy.dot(self.free, self.pastinput), y)

      ref = numpy.zeros(self.P)
      ref.fill(sp)
      
      du = numpy.dot(self.G, numpy.subtract(ref, f))

      self.pastinput = numpy.concatenate([[du], self.pastinput[0:self.pastinput.size-1]])

      self.u += du

      return self.u, du
      

                          
#sys_ss = scipy.signal.tf2ss([0.2713],[1,-0.8351])
#sysd_ss = scipy.signal.cont2discrete(sys_ss,1)
#t, y = scipy.signal.dstep(sysd_ss)  
sr = numpy.array([0,0,0.271,0.498,0.687,0.845,0.977,1.087,1.179,1.256,1.32,1.374,1.419,1.456,1.487,1.513,1.535,1.553,1.565,1.581, 1.592,1.6,1.608,1.614,1.619,1.632,1.627,1.63,1.633,1.635])

K = 5
P = 10
M = 5

sp = 1
y = 0

steps = 200

Y = numpy.zeros(steps)
U = numpy.zeros(steps)
S = numpy.zeros(steps)

delay = numpy.zeros(3);

d = DMC(sr, K, P, M)

for i in range(0,steps):
   if i == 100:
      sp = 0.5

   S[i] = sp
   
   (u, du) = d.calc(sp, y)
   U[i] = u
   Y[i] = y
   delay = numpy.concatenate([delay[1:3], [u]])
   y = 0.8351*y+0.2713*delay[0] #plant output


plt.plot(Y)
plt.plot(U, drawstyle='steps-post')
plt.plot(S, drawstyle='steps-post')
plt.show()