MIPMstudent.py

import numpy as np
import scipy.sparse, scipy.optimize, scipy.linalg

class MIPM(object):
    def __init__(self, A, B):
        self.A = A
        self.B = B
        
        # Initial scaling factor for the Lagrange multipliers (0 leads to a singular KKT matrix)
        self.epsilon = 1e3
        
        # Full search direction step scaling parameter (0, 1]. Closer to 1 is more aggressive.
        # eta < 1 is recommended (Dorado p. 40)
        self.eta = 0.9

        # Kreisselmeier-Steinhauser function soft constraint penalty
        # parameter.
        self.rho = 100
        
    @property
    def H(self):
        return self.diagonalBlockMatrix([self.R, self.Q]*(self.T-1) + [self.R, self.P])
    
    @property
    def F(self):
        return self.diagonalBlockMatrix([self.Fu, self.Fx]*(self.T-1) + [self.Fu, self.Ft])

    @property
    def Ftilde(self):
        return self.diagonalBlockMatrix([self.Futilde, self.Fxtilde]*(self.T-1) + [self.Futilde, self.Fttilde])
    
    @property
    def C(self):
        T = self.T
        n = self.n
        m = self.m
        A = self.A
        B = self.B
        
        ncols = T*(n + m)
        # nrows = T*n
        
        return np.vstack([np.hstack([B, -np.eye(n), np.zeros([n, ncols - m - n])])]+[np.hstack([np.zeros([n, m+i*(n+m)]), A, B, -np.eye(n), np.zeros([n, ncols - m-i*(n+m) - n - m - n])]) for i in range(T-1)])
    
    @property
    def f(self):
        return np.hstack([self.fu, self.fx]*(self.T-1) + [self.fu, self.ft])

    @property
    def ftilde(self):
        return np.hstack([self.futilde, self.fxtilde]*(self.T-1) + [self.futilde, self.fttilde])
    
    @property
    def d(self):
        return np.hstack([-self.A.dot(self.x0), np.zeros((self.T-1)*self.n)])
    
    @property
    def n(self):
        return self.A.shape[0]
    
    @property
    def m(self):
        return self.B.shape[1]
    
    @property
    def ku(self):
        return self.Fu.shape[0]
    
    @property
    def kx(self):
        return self.Fx.shape[0]
    
    @property
    def kt(self):
        return self.Ft.shape[0]
    
    @property
    def K(self):
        return self.T*self.Fu.shape[0] + (self.T-1)*self.Fx.shape[0] + self.Ft.shape[0]
    
    @property
    def S(self):
        return np.diag(self.s)
    
    @property
    def Lam(self):
        return np.diag(self.lam)

    @property
    def dtilde(self):
        dtilde_l = []

        for i in range(len(self.ftilde)):
            ei = self.expSoftConstraint(i)
            dtilde_l.append(ei/(1+ei))

        return np.asarray(dtilde_l)

    @property
    def dhat(self):
        dhat_l = []

        for i in range(len(self.ftilde)):
            ei = self.expSoftConstraint(i)
            den = (1+ei*ei)
            dhat_l.append(ei/(den*den))

        return np.asarray(dhat_l)

    def expSoftConstraint(self, i):
        return np.exp(self.rho*(self.Ftilde[i,:].dot(self.z) - self.ftilde[i]))
    
    @property
    def xi(self):
        return np.hstack([self.z, self.nu, self.lam, self.s])
    
    @property
    def U(self):
        T = self.T
        n = self.n
        m = self.m
        z = self.z
        
        return np.asarray([z[i*m+i*n:(i+1)*m+i*n] for i in range(T)])
    
    @property
    def X(self):
        T = self.T
        n = self.n
        m = self.m
        z = self.z
        
        return np.asarray([z[(i+1)*m+i*n:(i+1)*m+(i+1)*n] for i in range(T)])

    @property
    def Phi(self):
        H = self.H
        F = self.F
        Sinv = np.diag(1/self.s)
        Lam = self.Lam

        return H + F.T.dot(Sinv.dot(Lam.dot(F)))

    @property
    def Phitilde(self):
        Ftilde = self.Ftilde
        dhat = self.dhat

        return self.Phi + self.rho*Ftilde.T.dot(np.diag(dhat).dot(Ftilde))
    
    @property
    def KKTMatrix(self):
        """
        Constructs the KKT matrix.
        """
        T = self.T
        n = self.n
        m = self.m
        K = self.K
        
        H = self.H
        F = self.F
        C = self.C
        
        # np.block
        # scipy.sparse.bmat
        return scipy.sparse.bmat([
            [H                       , C.T                 , F.T               , np.zeros([T*(n+m), K])],
            [C                       , np.zeros([T*n, T*n]), np.zeros([T*n, K]), np.zeros([T*n, K])],
            [F                       , np.zeros([K, T*n])  , np.zeros([K, K])  , np.eye(K)],
            [np.zeros([K, T*(n + m)]), np.zeros([K, T*n])  , self.S            , self.Lam],
        ])
    
    @property
    def augKKTMatrix(self):
        """
        Constructs the augmented KKT matrix with Schur complement
        structure.
        """
        Phi = self.Phi
        C = self.C

        T = self.T
        n = self.n

        return np.vstack([
            np.hstack([Phi, C.T]),
            np.hstack([C, np.zeros([n*T, n*T])])
        ])

    @property
    def augKKTMatrixtilde(self):
        """
        Constructs the augmented KKT matrix with Schur complement
        structure and soft constraints.
        """
        Phitilde = self.Phitilde
        C = self.C

        T = self.T
        n = self.n

        return np.vstack([
            np.hstack([Phitilde, C.T]),
            np.hstack([C, np.zeros([n*T, n*T])])
        ])

    @property
    def affineResidual(self):
        """
        Constructs the affine step residual.
        """
        # H = self.H
        # F = self.F
        # C = self.C
        
        # z = self.z
        # nu = self.nu
        # lam = self.lam
        # s = self.s
        
        # rc = H.dot(z) + F.T.dot(lam) + C.T.dot(nu)
        # re = C.dot(z) - self.d
        # ri = F.dot(z) + s - self.f
        # rs = self.S.dot(
        #         self.Lam.dot(
        #             np.ones(self.K)))
        
        return -1*np.hstack([self.rc, self.re, self.ri, self.rs])

    @property
    def augAffineResidual(self):
        """
        Constructs the augmented affine step residual.
        """
        # H = self.H
        # F = self.F
        # C = self.C
        
        # z = self.z
        # nu = self.nu
        # lam = self.lam
        # s = self.s

        # Sinv = np.diag(1/s)
        # Lam = self.Lam
        
        # rc = H.dot(z) + F.T.dot(lam) + C.T.dot(nu)
        # re = C.dot(z) - self.d
        # ri = F.dot(z) + s - self.f
        # rs = self.S.dot(
        #         self.Lam.dot(
        #             np.ones(self.K)))

        # rd = rc + F.T.dot(Sinv.dot(Lam.dot(ri) - rs))

        return -1*np.hstack([self.rd, self.re])

    @property
    def rc(self):
        H = self.H
        F = self.F
        C = self.C
        
        z = self.z
        nu = self.nu
        lam = self.lam
        
        return H.dot(z) + F.T.dot(lam) + C.T.dot(nu)

    @property
    def re(self):
        C = self.C

        z = self.z

        d = self.d
        
        return C.dot(z) - d

    @property
    def ri(self):
        F = self.F
        
        z = self.z
        s = self.s
        
        f = self.f

        return F.dot(z) + s - f

    @property
    def rd(self):
        F = self.F

        s = self.s

        Lam = self.Lam
        Sinv = np.diag(1/s)

        return self.rc + F.T.dot(Sinv.dot(Lam.dot(self.ri) - self.rs))

    @property
    def rdtilde(self):
        Ftilde = self.Ftilde
        dtilde = self.dtilde

        return self.rd + Ftilde.T.dot(dtilde)

    @property
    def rs(self):
        S = self.S
        Lam = self.Lam

        K = self.K
        
        return S.dot(Lam.dot(np.ones(K)))
    
    def ccResidual(self, dlam, ds, mu_aff):
        """
        Constructs the combined centering and correction residual.
        """
        T = self.T
        n = self.n
        m = self.m
        K = self.K
        
        sigma = self.computeCenteringParameter(mu_aff)
        dLam = np.diag(dlam)
        dS = np.diag(ds)
        e = np.ones(K)
        
        # return np.hstack([np.zeros(T*(n+m)), np.zeros(T*n), np.zeros(K), sigma*mu_aff*e - dLam.dot(dS.dot(e))])
        return np.hstack([np.zeros(T*(n+m)), np.zeros(T*n), np.zeros(K), -sigma*self.dualityGap*e - dLam.dot(dS.dot(e))])
    
    def initKKT(self):
        """
        Initializes the solution variables (xi).
        
        Note: epsilon should be chosen sufficiently large
        so as to aid in convergence.
        """
        T = self.T
        n = self.n
        m = self.m
        K = self.K
        
        # Iteration step
        self.k = 1
        
        # State evolution and control inputs are assumed zero;
        # can be improved by initializing with past values
        # (aka warm start)
        self.z = np.zeros(T*(n+m))
        
        # Lagrange multipliers
        self.nu = np.ones(n*T)*self.epsilon
        self.lam = np.ones(K)*self.epsilon
        self.s = np.ones(K)*self.epsilon
        
    def decomposeResidual(self, xi):
        """
        Returns a 4-tuple of the form (z, nu, lam, s).
        """
        T = self.T
        n = self.n
        m = self.m
        K = self.K
        
        u0 = T*(n+m)
        u1 = u0 + n*T
        u2 = u1 + K
        
        return (xi[0:u0], xi[u0:u1], xi[u1:u2], xi[u2:])
    
    @property
    def dualityGap(self):
        """
        Computes the duality gap from the current Lagrange
        multipliers.
        """
        return self.lam.dot(self.s)/self.K
    
    def computeAffineDualityGap(self, dlam, ds, a):
        """
        Computes the affine duality gap given the Lagrange
        multiplier deltas and step size.
        """
        return (self.lam + a*dlam).dot(self.s + a*ds)/self.K
    
    def computeCenteringParameter(self, mu_aff):
        return np.power(mu_aff/self.dualityGap, 3)

    def searchStepSizeWright(self, dlam, ds):
        """
        Computes the search step length in the sense of Wright.
        
        @see: J. Nocedal and S. J. Wright (2006), Numerical Optimization (2 ed.), Sec. 14.2, Eq. 14.36, p. 409
        """
        x  = np.hstack([self.lam, self.s])
        dx = np.hstack([dlam, ds])
        
        try:
            x  = x[dx < 0]
            dx = dx[dx < 0]
            
            return min([-x[i]/dx[i] for i in range(len(x))])
        except:
            return 0.0
        
    def execNaiveIteration(self, verbose=False):
        """
        Performs a primal-dual interior point iteration in the sense
        of Mehrotra (1992).
        
        This method is dubbed "naive" since its matrix inversion step
        is predicated on full Gaussian elimination of the original KKT
        matrix, as opposed to some compacted version thereof.
        """
        # NOTE: Once you're done, remove this exception
        raise NotImplementedError
        
        # Factor KKT matrix
        KKTinv = np.linalg.inv(self.KKTMatrix.toarray())

        # Compute affine Lagrange delta
        dxi_aff = KKTinv.dot(self.affineResidual)
        dz_aff, dnu_aff, dlam_aff, ds_aff = self.decomposeResidual(dxi_aff)
        lam = self.lam
        s = self.s

        # Compute affine search step length
        # NOTE: Implement using self.searchStepSizeWright
        if verbose:
            print("***")
            print(f"ITER {self.k}")
            print("LAGRANGE MULT. CONSTRAINTS:")
            print(f"lam: {all(lam + a_aff*dlam_aff >= 0)}")
            print(f"s  : {all(s + a_aff*ds_aff >= 0)}")
            
        # Compute affine duality gap and centering parameter

        # Compute combined correction and centering Lagrange delta
        # NOTE: Use self.ccResidual

        # Compute total Lagrange delta
        dxi = dxi_aff + dxi_cc
        _, _, dlam, ds = self.decomposeResidual(dxi)

        # Compute total search step length
        if verbose:
            print("DUALITY GAP AND STEP SIZE/ADAPTATION:")
            print(f"a_aff {a_aff:.5}; a {a:.5}; sigma {sigma:.5}; mu_aff {mu_aff:.5}; mu {self.dualityGap:.5}")

        # Update KKT system solution
        xi_upd = self.xi + a*dxi
        self.z, self.nu, self.lam, self.s = self.decomposeResidual(xi_upd)
        
        self.k += 1

    def execAugmentedIteration(self, verbose=False):
        """
        Performs a primal-dual interior point iteration in the sense
        of Mehrotra (1992).
        
        This method is dubbed "augmented" since its matrix inversion
        is predicated on Gaussian elimination of the augmented KKT
        matrix, as opposed to the original Hessian. Additionally, this
        function implements soft constraints as well.
        """
        # NOTE: Once you're done, remove this exception
        raise NotImplementedError

        T = self.T
        n = self.n
        m = self.m
        K = self.K


        # Factor augmented KKT matrix
        augKKTinv = np.linalg.inv(self.augKKTMatrix)

        F = self.F

        lam = self.lam
        s = self.s

        S = np.diag(s)
        Sinv = np.diag(1/s)
        Lam = np.diag(lam)
        Laminv = np.diag(1/lam)

        re = self.re
        # rd = self.rd
        rd = self.rdtilde
        ri = self.ri
        rs = self.rs

        d_aff = augKKTinv.dot(-1*np.hstack([rd, re]))
        dz_aff = d_aff[:T*(n+m)]
        dnu_aff = d_aff[T*(n+m):]
        dlam_aff = Sinv.dot(Lam.dot(F.dot(dz_aff) + ri - Laminv.dot(rs)))
        ds_aff = -Laminv.dot(rs + S.dot(dlam_aff))

        dxi_aff = np.hstack([dz_aff, dnu_aff, dlam_aff, ds_aff])

        # Compute affine search step length
        if verbose:
            print("***")
            print(f"ITER {self.k}")
            print("LAGRANGE MULT. CONSTRAINTS:")
            print(f"lam: {all(lam + a_aff*dlam_aff >= 0)}")
            print(f"s  : {all(s + a_aff*ds_aff >= 0)}")
            
        # Compute affine duality gap and centering parameter

        # Compute combined correction and centering Lagrange delta
        # NOTE: Do NOT use self.ccResidual. Instead, change the
        # definitions of re, rc, ri, rs to reflect the cc-system, and
        # copy the code given above for the affine step.

        # Compute total Lagrange delta
        dxi = dxi_aff + dxi_cc
        _, _, dlam, ds = self.decomposeResidual(dxi)

        # Compute total search step length
        if verbose:
            print("DUALITY GAP AND STEP SIZE/ADAPTATION:")
            print(f"a_aff {a_aff:.5}; a {a:.5}; sigma {sigma:.5}; mu_aff {mu_aff:.5}; mu {self.dualityGap:.5}")

        # Update KKT system solution
        xi_upd = self.xi + a*dxi
        self.z, self.nu, self.lam, self.s = self.decomposeResidual(xi_upd)
        
        self.k += 1
    
    def iterateResidualConvergenceCrit(self, threshold=1e-9, max_iter=50, verbose=False, softconstraints=True):
        for _ in range(max_iter):
            res = np.linalg.norm(self.affineResidual)
            if verbose:
                print(f"RESIDUAL NORM: {res:.5}")
                    
            if (res > threshold):    
                if softconstraints:
                    self.execAugmentedIteration(verbose)
                else:
                    self.execNaiveIteration(verbose)
            else:
                break
                
    def unitTests(self):
        def bool2passfail(b):
            if (b):
                return 'PASS'
            else:
                return 'FAIL'

        print('{0:.<60} : {1: <4}'.format("H.shape                    = (T*(n + m), T*(n + m)) ", bool2passfail(self.H.shape == (self.T*(self.n + self.m), self.T*(self.n + self.m)))))
        print('{0:.<60} : {1: <4}'.format("K                          = T*ku + (T-1)*kx + kt ", bool2passfail(self.K == self.T*self.ku + (self.T-1)*self.kx + self.kt)))
        print('{0:.<60} : {1: <4}'.format("F.shape                    = (K, T*(n + m)) ", bool2passfail(self.F.shape == (self.K, self.T*(self.n + self.m)))))
        print('{0:.<60} : {1: <4}'.format("C.shape                    = (T*n, T*(n + m)) ", bool2passfail(self.C.shape == (self.T*self.n, self.T*(self.n + self.m)))))
        print('{0:.<60} : {1: <4}'.format("Phi.shape                  = (T*(n + m), T*(n + m)) ", bool2passfail(self.Phi.shape == (self.T*(self.n + self.m), self.T*(self.n + self.m)))))
        print('{0:.<60} : {1: <4}'.format("f.shape                    = K ", bool2passfail(self.f.shape[0] == self.K)))
        print('{0:.<60} : {1: <4}'.format("d.shape                    = T*n ", bool2passfail(self.d.shape[0] == self.T*self.n)))
        # print('{0:.<60} : {1: <4}'.format("KKTMatrix.shape   = (T**3)*(2*n+m) + 2*K ", bool2passfail(self.KKTMatrix.shape[0] == (self.T**3)*(2*self.n + self.m) + 2*self.K)))
        print('{0:.<60} : {1: <4}'.format("KKTMatrix.shape            = square ", bool2passfail(self.KKTMatrix.shape[0] == self.KKTMatrix.shape[1])))
        print('{0:.<60} : {1: <4}'.format("affineResidual.shape[0]    = KKTMatrix.shape[1]", bool2passfail(self.affineResidual.shape[0] == self.KKTMatrix.shape[1])))
        print('{0:.<60} : {1: <4}'.format("augKKTMatrix.shape         = (T*(2*n + m), T*(2*n + m)) ", bool2passfail(self.augKKTMatrix.shape == (self.T*(2*self.n + self.m), self.T*(2*self.n + self.m)))))
        print('{0:.<60} : {1: <4}'.format("augAffineResidual.shape[0] = augKKTMatrix.shape[1]", bool2passfail(self.augAffineResidual.shape[0] == self.augKKTMatrix.shape[1])))
    
    @staticmethod
    def diagonalBlockMatrix(M_l):
        """
        Creates a diagonal block matrix from a list of matrices.
        """
        w = sum([M.shape[1] for M in M_l])
        Mnew_l = []
        w_offset = 0
        
        for i in range(len(M_l)):
            M = M_l[i]
            hi, wi = M.shape
            Mnew_l.append([
                np.zeros((hi, w_offset)),
                M,
                np.zeros((hi, w - wi - w_offset))
            ])
            w_offset += wi
            
        return np.asarray(np.bmat(Mnew_l))

def mipmExample(constr=[50, 0], verbose=True):
    # Ex. 9.5.3 of Franklin et al.

    A = np.array([[ 0.9607, 0.0196,  0.1776],
              [-0.0098, 0.9999, -0.0009],
              [ 0     , 0     ,  0.8187]])

    B = np.array([[ 0.0185, 0.1974],
                [-0.0001, 0.1390],
                [ 0.1813, 0.0000]])

    mipm = MIPM(A, B)

    # Initial state
    mipm.x0 = np.array([20, 20, 20])

    # Prediction horizon
    mipm.T = 30

    # State penalty matrix
    mipm.Q = np.array([[0.25, 0, 0],
                    [0   , 1, 0],
                    [0   , 0, 0]])

    # Input penalty matrix
    mipm.R = np.array([[0.04, 0   ],
                    [0   , 0.01]])

    # Terminal state penalty matrix
    mipm.P = np.eye(3)

    # State constraint matrix
    mipm.Fx = np.array([[ 1,  0, 0],
                        [-1,  0, 0],
                        [ 0,  1, 0],
                        [ 0, -1, 0]])

    # State constraint vector
    mipm.fx = np.array([constr[0], constr[1], 25, 10])

    # Input constraint matrix
    mipm.Fu = np.array([[ 1,  0],
                        [-1,  0],
                        [ 0,  1],
                        [ 0, -1]])

    # Input constraint vector
    mipm.fu = np.array([10, 10, 10, 10])

    # Terminal state constraint matrix
    mipm.Ft = np.array([[ 1,  0, 0],
                        [-1,  0, 0],
                        [ 0,  1, 0],
                        [ 0, -1, 0]])

    # Terminal state constraint vector
    mipm.ft = np.array([constr[0], constr[1], 25, 10])

    mipm.epsilon = 1e3

    # Initialize KKT variables
    mipm.initKKT()

    mipm.unitTests()

    print()

    mipm.iterateResidualConvergenceCrit(verbose=verbose, softconstraints=False)

    return mipm

def mipmExampleSoft(constr=[50, 0], rho=100, verbose=True):
    # Ex. 9.5.3 of Franklin et al.

    A = np.array([[ 0.9607, 0.0196,  0.1776],
              [-0.0098, 0.9999, -0.0009],
              [ 0     , 0     ,  0.8187]])

    B = np.array([[ 0.0185, 0.1974],
                [-0.0001, 0.1390],
                [ 0.1813, 0.0000]])

    mipm = MIPM(A, B)

    # Initial state
    mipm.x0 = np.array([20, 20, 20])

    # Prediction horizon
    mipm.T = 30

    # State penalty matrix
    mipm.Q = np.array([[0.25, 0, 0],
                    [0   , 1, 0],
                    [0   , 0, 0]])

    # Input penalty matrix
    mipm.R = np.array([[0.04, 0   ],
                    [0   , 0.01]])

    # Terminal state penalty matrix
    mipm.P = np.eye(3)

    # State constraint matrix
    mipm.Fx = np.array([[ 0,  1, 0],
                        [ 0, -1, 0]])

    # State constraint vector
    mipm.fx = np.array([25, 10])

    # Input constraint matrix
    mipm.Fu = np.array([[ 1,  0],
                        [-1,  0],
                        [ 0,  1],
                        [ 0, -1]])

    # Input constraint vector
    mipm.fu = np.array([10, 10, 10, 10])

    # Terminal state constraint matrix
    mipm.Ft = np.array([[ 0,  1, 0],
                        [ 0, -1, 0]])

    # Terminal state constraint vector
    mipm.ft = np.array([25, 10])

    # SOFT CONSTRAINTS

    # State constraint matrix
    mipm.Fxtilde = np.array([[ 1,  0, 0],
                             [-1,  0, 0]])

    # State constraint vector
    mipm.fxtilde = np.array([constr[0], constr[1]])

    # Input constraint matrix
    mipm.Futilde = np.array([[ 0,  0]])

    # Input constraint vector
    mipm.futilde = np.array([1])

    # Terminal state constraint matrix
    mipm.Fttilde = np.array([[ 1,  0, 0],
                             [-1,  0, 0]])

    # Terminal state constraint vector
    mipm.fttilde = np.array([constr[0], constr[1]])

    mipm.epsilon = 1e3
    mipm.rho = rho

    # Initialize KKT variables
    mipm.initKKT()

    mipm.unitTests()

    print()

    mipm.iterateResidualConvergenceCrit(verbose=verbose)

    return mipm

if __name__ == "__main__":
    import matplotlib as mpl
    import matplotlib.pyplot as plt
    mpl.rcParams['figure.figsize'] = 12, 5


    mipm = mipmExample([50, 0], verbose=True)
    mipmSoft = mipmExampleSoft([50, 0], rho=1e2, verbose=True)

    # States
    X = mipm.X
    Xsoft = mipmSoft.X

    tab10 = plt.get_cmap("tab10")

    for i in range(mipm.n):
        plt.plot(np.hstack([mipm.x0[i], X[:,i]]), label='$x_'+str(i)+'$', color=tab10(i))
        plt.plot(np.hstack([mipmSoft.x0[i], Xsoft[:,i]]), label='$x_'+str(i)+'$ soft', color=tab10(i), linestyle='--')

    plt.xticks(np.arange(0, mipm.T+1, 1.0))
    plt.xlim([0, mipm.T])
    plt.xlabel('Discrete time step')
    plt.ylabel('State magnitude')
    plt.title('Dorado/Mehrotra LTI-MPC State Evolution')

    plt.minorticks_on()
    plt.grid(which='minor', color='k', alpha=0.25, linestyle=':')
    plt.grid(which='major', color='k', alpha=0.5)
    plt.legend()
    plt.show()

    # Control input
    U = mipm.U
    Usoft = mipmSoft.U

    for i in range(mipm.m):
        plt.plot(np.hstack([0, U[:,i]]), label='$u_'+str(i)+'$', color=tab10(i))
        plt.plot(np.hstack([0, Usoft[:,i]]), label='$u_'+str(i)+'$ soft', color=tab10(i), linestyle='--')

    plt.xticks(np.arange(0, mipm.T+1, 1.0))
    plt.xlim([0, mipm.T])
    plt.xlabel('Discrete time step')
    plt.ylabel('State magnitude')
    plt.title('Dorado/Mehrotra LTI-MPC Control History')

    plt.minorticks_on()
    plt.grid(which='minor', color='k', alpha=0.25, linestyle=':')
    plt.grid(which='major', color='k', alpha=0.5)
    plt.legend()
    plt.show()