control formulation testing

control_problem/algorithms.py

+from numpy import array,isnan,eye, hstack,vstack,shape,zeros,dot
+from scipy import linalg
+
+
+def Newton_Scalar(f,x,fprime,args):
+    glag = True
+    count = 1
+    while glag and (count<100):
+        xn = x - f(x,*args)/fprime(x,*args)  #Newton iterate
+        if abs(xn-x)<1e-14:
+            glag = False
+        else:
+            x = xn                      #Next iterate
+            count = count + 1
+
+
+    #Sanity check for non-convergent cases.
+    if (count>100) or isnan(x):
+        print("Not converged")
+        print("code not generated")
+        exit()
+    
+    return x
+
+    
+    
+def RK4(fun,y0,t0,tfinal,dt,args):
+    
+    y = y0
+    
+    t = t0
+    Time = [t0,]
+    Solution = [y,]
+    flag = True
+    
+    while flag==True:
+        k1 = fun(y,t,*args)
+        k2 = fun(y+0.5*dt*k1,t+0.5*dt,*args)
+        k3 = fun(y+0.5*dt*k2,t+0.5*dt,*args)    
+        k4 = fun(y+dt*k3,t+dt,*args)
+    
+        y = y + (k1 + 2*k2 + 2*k3 + k4)/6.*dt
+        Solution.append(y)
+        
+        t = t + dt
+        Time.append(t)
+        
+        if abs(t-tfinal)<dt/10:
+            flag = False
+        elif any(isnan(y)):
+            flag = False
+
+    Time = array(Time)
+    Solution = array(Solution)
+    return Time,Solution
+    
+
+    
+def BDF_N(fun,Previous_States,t0,tfinal,dt,args,Explicit_Time=False):
+    #This code only works for linear ODEs of the type dy/dt=A(t)y
+    
+    #The integrator also works for linear PDEs where the fun should return relevant boundary conditions as well
+    #fun is a python function which evaluates the RHS of the PDE and then spits out this along with the boundary conditions.
+    
+    #alpha is the operator acting on the unknown function coefficients: eg (-1)**n, (1)**n, etc.
+    #beta is the value of the boundary operator. So for homogeneous problems, beta = 0.
+    
+    # alpha acting on y evaluates to beta at each time t.
+
+    #The type of the BDF algorithm is implicitly decided by length of the previous states 
+    #All Previous_States should be same length. This decides the size of the matrices in time step
+    
+    flag = True
+    N = len(Previous_States[0])
+    for term in Previous_States[1:]:
+        if len(term)==N:
+            flag = True
+        else:
+            flag = False
+            break
+    
+    if flag == False:
+        print("Previous States are not all same length. Integrator aborted")
+        return None
+
+    BDF_Type = len(Previous_States)
+    
+    if BDF_Type == 1:
+        State_Weights = array([1,dt])
+    elif BDF_Type == 2:
+        State_Weights = array([-1./3, 4./3, 2./3*dt])
+    elif BDF_Type == 3:
+        State_Weights = array([2./11, -9./11, 18./11, 6./11*dt ])
+    elif BDF_Type == 4:
+        State_Weights = array([-3./25, 16./25, -36./25, 48./25, 12./25*dt])
+    
+    #Function fun(N,t,args) should return the right-hand side matrix A(t), boundary operator alpha(t) and boundary data beta(t)
+    
+    #Check to see if fun depends explicitly on time. If not, relevant matrices can be precomputed.
+    if Explicit_Time==False:
+        A, alpha, beta = fun(N,*args)
+        
+        I = eye(N)
+
+        Number_of_BCs = shape(alpha)[0] #Number of rows of boundary operator
+    
+        if Number_of_BCs!=len(beta):
+            print("Number of boundary conditions don't match the shape of the boundary operator. Integrator aborted")
+            return None
+
+        A = A[:-Number_of_BCs,:]
+        I = I[:-Number_of_BCs,:]
+        print(shape(A),shape(alpha),shape(beta),shape(I))
+
+        A = I - State_Weights[-1]*A
+        
+        A = vstack((A,alpha))
+        I = vstack((I,zeros([Number_of_BCs,N])))
+        beta = hstack([zeros(N-Number_of_BCs),beta])
+    else:
+        I = eye(N)
+        print("Sorry, explicit time dependent rhs not currently implemented. Integrator aborted")
+        return None
+        
+    
+    Time = [t0,]
+    t = t0
+    Solution = [Previous_States[0],]
+    for term in Previous_States[1:]:
+        Solution.append(term)
+        
+    flag = True
+    
+    while flag==True:
+        b = 0
+        for coeff, term in zip(State_Weights[:-1],Previous_States):
+            b = coeff*term + b
+        
+        b = dot(I,b) + beta
+        
+        New_State = linalg.solve(A,b)
+        
+        Previous_States = Previous_States[1:]
+        Previous_States.append(New_State)
+        
+        Solution.append(New_State)
+        t = t + dt
+        Time.append(t)
+        
+        if abs(t-tfinal)<dt/10:
+            flag = False
+        elif any(isnan(New_State)):
+            flag = False
+            
+    Time = array(Time)
+    Solution = array(Solution)
+    return Time,Solution
+    

control_problem/boundary_control_full_nonlinear_problem.py

+import numpy as np
+import matplotlib.pyplot as plt
+import scipy
+import math
+import sympy
+
+N = 100
+sigma0 = -1
+l0 = 20
+
+# t array
+T = 100
+dt = 0.01
+times = np.arange(0, T, dt)
+
+def hermite_function(order, arg):
+    return ((2**order * math.factorial(order) * np.sqrt(np.pi))**(-0.5) 
+            * scipy.special.eval_hermite(order, arg) * np.exp(-arg**2/2) 
+            )
+
+# vector of hermite functions
+vector_of_hermite_polynomials = np.zeros(N, dtype = 'complex_')
+for r in range(0, N):
+    vector_of_hermite_polynomials[r] = hermite_function(r, 0) * (-1j)**(r) 
+
+# adjacency matrix
+a = np.zeros((N, N), dtype='complex_')
+for n in range(0, N):
+    for m in range(0, N):
+        if m == n:
+            a[m][n] = (n + 1/2)
+        elif m == n - 2:
+            a[m][n] = np.sqrt(n * (n - 1))/2
+        elif m == n + 2:
+           a[m][n] = np.sqrt((n + 1) * (n + 2))/2
+
+# a1
+a1 = np.zeros((N, N), dtype='complex_')
+for nn in range(0, N):
+    for mm in range(0, N):
+        if mm == nn-1:
+            a1[nn][mm] = np.sqrt(nn/2)
+        if mm == nn + 1:
+            a1[nn][mm] = np.sqrt((nn + 1)/2)
+
+# q1
+def q(length):
+    q1_array = np.zeros(N, dtype='complex_')
+    q1_array[0] = np.sqrt(1/2) * (-1j) * (hermite_function(1, 0) + hermite_function(1, -2 * length))
+    for p1 in range(1, N):
+        q1_array[p1] = ( np.sqrt(p1/2) * (-1j)**(p1-1) * (hermite_function(p1 - 1, 0) + hermite_function(p1 - 1, -2 * length))
+                        + np.sqrt((p1 + 1)/2) * (-1j)**(p1+1) * (hermite_function(p1 + 1, 0) + hermite_function(p1 + 1, -2 * length))    
+                                )
+    return q1_array
+
+# b 
+b = np.zeros((len(times), N), dtype='complex_')
+b[0] = 0
+
+length = np.zeros(len(times))
+length[0] = l0
+
+def func1(b, length):
+    return ((1j/np.pi) * np.dot(vector_of_hermite_polynomials, b) * np.dot(a1, b)
+            - np.dot(a, b) + 1j * q(length) * (sigma0 + ((1-sigma0)/np.pi )* np.dot(vector_of_hermite_polynomials, b))
+    )
+
+def func2(b):
+    return (1/np.pi) * np.dot(vector_of_hermite_polynomials, b)
+
+
+# euler
+for l in range(0, len(times)-1):
+    b[l+1] = b[l] + dt * func1(b[l], length[l])
+    length[l+1] = length[l] + dt * func2(b[l])
+
+print(length[-1])
+plt.plot(times, length)
+plt.xlabel('time')
+plt.ylabel('length')
+plt.title('Boundary growth, N=%i' %N)
+plt.show()
\ No newline at end of file

control_problem/cheb.py

+from numpy import *
+from numpy.fft import fft,ifft
+
+def cheb(y):
+    '''Chebyshev transform. Finds Chebyshev coefficients given y evaluated on
+    Chebyshev grid'''
+    N = len(y) - 1
+    yt = real(fft(r_[y, y[-2:0:-1]]))
+    yt = yt/(2*N)
+    yt = r_[yt[0],yt[1:N]+yt[-1:N:-1],yt[N]]
+    return yt
+
+def icheb(c):
+    '''Inverse Chebyshev transform. Evaluates Chebyshev series at the Chebyshev 
+    grid points given Chebyshev coefficients.'''
+    N = len(c) - 1
+    y = r_[c[0],0.5*c[1:N],c[N],0.5*c[-1:N:-1]]
+    y = y*2*N
+    y = real(ifft(r_[y,y[-2:0:-1]]))[:N+1]
+    return y
+
+def Dcheb(y,interval):
+    '''Chebyshev derivative of y evaluated on Chebyshev grid in interval [a,b]'''
+    N = len(y) - 1
+    a,b = interval
+    x = 0.5*(b-a)*(cos(r_[0:N+1]*pi/N) + 1) + a
+
+    k = r_[0:N]
+
+    A = real(fft(r_[y,y[-2:0:-1]]))
+    yy = real(ifft(1j*r_[k,0,-k[-1:0:-1]]*A))
+
+    fact = 2.*(x-a)/(b-a)-1
+    fact = fact[1:-1]
+    yprime = -2./(b-a)*yy[1:N]/sqrt(1-fact**2)
+
+    A = A/(2*N)
+    A = r_[A[0],A[1:N]+A[-1:N:-1],A[N]]
+    k = r_[0:N+1]
+
+    yprime1 = sum(k**2*A)*2./(b-a)
+    yprimeN = sum((-1)**(k+1)*k**2*A)*2./(b-a)
+
+    return r_[yprime1,yprime,yprimeN]
+
+def regrid(y,M):
+    N = len(y) - 1
+    a = cheb(y)
+    if M==N:
+        return y
+    if M>N:
+        a = r_[a,zeros(M-N)]
+        return icheb(a)
+    if M<N:
+        a = a[:M+1]
+        return icheb(a)
+
+
+
+def clenshaw(x,c):
+    '''Clenshaw algorithm to evaluate Chebyshev series at x
+    assumes x is in [-1,1]'''
+    N = len(c) - 1
+    b = zeros(N+2)
+    b[-1] = 0
+    b[-2] = c[-1]
+    for r in r_[N-1:0:-1]:
+        b[r] = 2*x*b[r+1] - b[r+2] + c[r]
+    s = x*b[1] - b[2] + c[0]
+
+    return s
+
+
+def clenshaw2(x,c,change_grid = True):
+    '''Vectorized version of Clenshaw algorithm
+    Use this for Chebyshev polynomial evaluation'''
+    if change_grid:
+    	if (min(x)!=-1) or (max(x)!=1):
+        	x = 2*(x-min(x))/(max(x)-min(x)) - 1
+    N = len(c) - 1
+    b = zeros([N+2,len(x)])
+    b[-1,:] = 0
+    b[-2,:] = c[-1]
+    for r in r_[N-1:0:-1]:
+        b[r,:] = 2*x*b[r+1,:] - b[r+2,:] + c[r]
+    s = x*b[1,:] - b[2,:] + c[0]
+
+    return s
+
+
+
+def chebD(c,interval):
+    '''Finds derivative of Chebyshev series in spectral space
+    i.e. maps c_n--->d_n where c_n,d_n are Chebyshev coefficients
+    of f(x) and f'(x) in the interval [a,b].'''
+    N = len(c) - 1
+    a,b = interval
+    if (a!=-1.) or (b!=1.):
+        factor = 2./(b-a)
+    else: 
+        factor = 1.
+
+    b = c*r_[0:N+1]
+
+
+    cp = zeros_like(b)
+
+    cp[0] = sum(b[1::2])
+
+    evens = b[2::2]
+    odds = b[1::2]
+
+    cp[1:N+1-(N%2):2] = 2*cumsum(evens[-1::-1])[-1::-1]
+    cp[2:N+1-((N+1)%2):2] = 2*cumsum(odds[-1::-1])[-2::-1]
+
+    cp = cp*factor
+
+    return cp
+
+
+def chebD_semiinf(c):
+    '''Finds the derivative of Chebyshev series in spectral space
+    i.e. maps c_n --> d_n where c_n, d_n are Chebyshev coefficients
+    of f(x) and f'(x) in the interval [0,oo)'''
+    '''To be used only for the positive half-line'''
+    
+    N = len(c) - 1
+    
+    b = c*r_[0:N+1]
+
+
+    cp = zeros_like(b)
+
+    cp[0] = sum(b[1::2])
+
+    evens = b[2::2]
+    odds = b[1::2]
+
+    cp[1:N+1-(N%2):2] = 2*cumsum(evens[-1::-1])[-1::-1]
+    cp[2:N+1-((N+1)%2):2] = 2*cumsum(odds[-1::-1])[-2::-1]
+
+    d0 = 3./4*cp[0] - cp[1]/2. + cp[2]/8.
+    d1 = -cp[0] + 7./8*cp[1] - cp[2]/2. + cp[3]/8.
+    d2 = cp[0]/4. - cp[1]/2. + 3./4*cp[2] - cp[3]/2. + cp[4]/8.
+    d3 = cp[1]/8. - cp[2]/2. + 3./4*cp[3] - cp[4]/2. + cp[5]/8.
+    
+    dn = [ cp[i-2]/8. - cp[i-1]/2. + 3./4*cp[i] - cp[i+1]/2. + cp[i+2]/8.  for i in range(4,N-1)]
+
+    dn1 = cp[N-1-2]/8. - cp[N-1-1]/2. + 3./4*cp[N-1] - cp[N-1+1]/2.
+    
+    dn2 = cp[N-2]/8. - cp[N-1]/2. + 3./4*cp[N]
+    
+    dn = r_[d0,d1,d2,d3,dn,dn1,dn2]
+
+    return dn
+
+
+def cheb2zD_semiinf(c):
+    '''Finds the Chebyshev coefficients of the operator 2z df/dz when
+    f has a series in Chebyshev rational functions Rn(z) = Tn((z-1)/(z+1)). Input
+    is the coefficients of f.'''
+    
+    N = len(c) - 1
+    
+    b = c*r_[0:N+1]
+
+
+    cp = zeros_like(b)
+
+    cp[0] = sum(b[1::2])
+
+    evens = b[2::2]
+    odds = b[1::2]
+
+    cp[1:N+1-(N%2):2] = 2*cumsum(evens[-1::-1])[-1::-1]
+    cp[2:N+1-((N+1)%2):2] = 2*cumsum(odds[-1::-1])[-2::-1]
+
+    d0 = -cp[2]/4. + cp[0]/2.
+    d1 = cp[1]/4. - cp[3]/4.
+    d2 = -cp[0]/2. + cp[2]/2. - cp[4]/4.
+    
+    dn = [ -cp[n-2]/4.  + cp[n]/2. - cp[n+2]/4 for n in range(3,N-1)] 
+    
+    dn1 = -cp[N-3]/4. + cp[N-1]/2
+    dn2 = -cp[N-2]/4. + cp[N]/2
+    
+    dn = r_[d0,d1,d2,dn,dn1,dn2]
+    
+    return dn
+
+
+
+def Intcheb(y,interval):
+    '''Clenshaw-Curtis to find definite integral of function y(x) given at
+    Chebyshev grid points in some interval [a,b]'''
+    fact = 0.5*(interval[1]-interval[0])
+    b = cheb(y)
+    N = len(y) - 1
+    if N%2 == 0:
+        w = array([ 2./(-(2*k)**2+1) for k in r_[0:N/2+1]])
+    else:
+        w = array([ 2./(-(2*k)**2+1) for k in r_[0:(N-1)/2+1]])
+    return dot(b[::2],w)*fact
+
+
+def chebI(c,interval,x0=None,f0=None):
+    if x0==None:
+        x0=interval[0]
+
+    N = len(c) - 1
+    I = diag(1./(2*r_[0.5,r_[2:N+1]]),-1) -diag(1./(2*r_[1,r_[1:N]]),1)
+    I[0,1] = 0
+    
+    factor = (interval[1]-interval[0])/2.
+    
+    ci = dot(I,c)*factor
+    x = 2*(x0-interval[0])/(interval[1]-interval[0]) - 1
+    
+    if x==-1 and f0==None:
+        ci[0] = -sum((-1)**r_[1:N+1]*ci[1:])
+    else:
+        ci[0] = f0 - clenshaw(x,ci)
+    return ci
+
+
+
+def cheb_convolve(a,b):
+    '''Finds the product of two functions whose Chebyshev coefficients are 
+    given by a and b. Output is the coefficiets of the product.'''
+
+    M = len(b)
+    N = len(a)
+    
+    if N>M:
+        b = r_[b,zeros(N-M)]
+        N = N - 1
+    elif M>N:
+        a = r_[a,zeros(M-N)]
+        N = M - 1
+    else:
+        N = N - 1
+    
+    a[0] = a[0]*2.
+    b[0] = b[0]*2.
+        
+    c0 = a[0]*b[0] + 2*dot(a[1:],b[1:])
+    
+    c1 = [ dot(a[0:k+1][::-1],b[0:k+1]) + dot(a[1:N-k+1],b[k+1:N+1]) + dot(a[k+1:N+1],b[1:N-k+1])  for k in range(1,N) ]
+
+    c2 = [ dot(a[k-N:N+1][::-1],b[k-N:N+1])  for k in range(N,2*N+1)]
+    
+    c = r_[c0/2,c1,c2]/2.
+    
+    return c[:N+1]
+    
+def cosT(d,inverse=False):
+    '''Finds the cosine transform of a given sequence'''
+    b = []
+    N = len(d)-1
+    for n in r_[0:N+1]:
+        b.append(sum(d*cos(n*r_[0:N+1]*pi/N)))
+    b = array(b)
+    if inverse:
+        return b
+    else:
+        b[0] = b[0]/(N)
+        b[1:] = b[1:]*2/(N)
+        return b
+
+
+

control_problem/full_nonlinear_problem.py

+import numpy as np
+import matplotlib.pyplot as plt
+import scipy
+import math
+import sympy
+
+N = 100
+sigma0 = -1
+l0 = 2
+
+# t array
+T = 400
+dt = 0.01
+times = np.arange(0, T, dt)
+
+def hermite_function(order, arg):
+    return ((2**order * math.factorial(order) * np.sqrt(np.pi))**(-0.5) 
+            * scipy.special.eval_hermite(order, arg) * np.exp(-arg**2/2) 
+            )
+
+# vector of hermite functions
+vector_of_hermite_polynomials = np.zeros(N, dtype = 'complex_')
+for q in range(0, N):
+    vector_of_hermite_polynomials[q] = hermite_function(q, 0) * (-1j)**(q) 
+
+# adjacency matrix
+a = np.zeros((N, N), dtype='complex_')
+for n in range(0, N):
+    for m in range(0, N):
+        if m == n:
+            a[m][n] = (n + 1/2)
+        elif m == n - 2:
+            a[m][n] = np.sqrt(n * (n - 1))/2
+        elif m == n + 2:
+           a[m][n] = np.sqrt((n + 1) * (n + 2))/2
+
+# a1
+a1 = np.zeros((N, N), dtype='complex_')
+for nn in range(0, N):
+    for mm in range(0, N):
+        if mm == nn-1:
+            a1[nn][mm] = np.sqrt(nn/2)
+        if mm == nn + 1:
+            a1[nn][mm] = np.sqrt((nn + 1)/2)
+
+# q1
+def q1(length):
+    q1_array = np.zeros(N, dtype='complex_')
+    q1_array[0] = np.sqrt(1/2) * (-1j) * (hermite_function(1, 0) + hermite_function(1, -2 * length))
+    for p1 in range(1, N):
+        q1_array[p1] = ( np.sqrt(p1/2) * (-1j)**(p1-1) * (hermite_function(p1 - 1, 0) + hermite_function(p1 - 1, -2 * length))
+                        + np.sqrt((p1 + 1)/2) * (-1j)**(p1+1) * (hermite_function(p1 + 1, 0) + hermite_function(p1 + 1, -2 * length))    
+                                )
+    return q1_array
+
+# q2
+def q2(length):
+    q2_array = np.zeros(N, dtype='complex_')
+    for p2 in range(0, 2):
+        q2_array[p2] = (1/2j) * (     ((2 * p2 + 1)/2)         * (-1j)**p2 *     (hermite_function(p2, length + l0/2) - hermite_function(p2, length-l0/2))
+                                    + np.sqrt((p2+1)*(p2+2))/2 * (-1j)**(p2+2) * (hermite_function(p2+2, length + l0/2) - hermite_function(p2+2, length - l0/2))
+                                )
+    for p3 in range(2, N):
+        q2_array[p3] = (1/2j) * (   ((2 * p3 + 1)/2)           * (-1j)**p3     * (hermite_function(p3, length+l0/2) - hermite_function(p3, length-l0/2))
+                                    + np.sqrt((p3+1)*(p3+2))/2 * (-1j)**(p3+2) * (hermite_function(p3+2, length+l0/2) - hermite_function(p3+2, length-l0/2))
+                                    + np.sqrt(p3*(p3-1))/2     * (-1j)**(p3-2) * (hermite_function(p3-2, length+l0/2) - hermite_function(p3-2, length-l0/2))
+                                )
+    return q2_array
+
+# b 
+b = np.zeros((len(times), N), dtype='complex_')
+b[0] = 0
+
+length = np.zeros(len(times))
+length[0] = l0
+
+def func1(b, length):
+    return ((1/np.pi) * np.dot(vector_of_hermite_polynomials, b) * (1j * q1(length) + np.dot(a1, b))
+            - np.dot(a, b) - 2j * sigma0 * q2(length)
+           )
+
+def func2(b):
+    return (1/np.pi) * np.dot(vector_of_hermite_polynomials, b)
+
+
+# euler
+for l in range(0, len(times)-1):
+    b[l+1] = b[l] + dt * func1(b[l], length[l])
+    length[l+1] = length[l] + dt * func2(b[l])
+
+print(length[-1])
+plt.plot(times, length)
+plt.xlabel('time')
+plt.ylabel('length')
+plt.title('N=%i' %N)
+plt.show()
\ No newline at end of file

control_problem/integro_diff_linear_control_problem.py

+import numpy as np
+import matplotlib.pyplot as plt
+import scipy
+from scipy.integrate import quad 
+
+from math import exp, sqrt, pi
+
+def k_integral(l, l0, s, t):
+    return (np.exp((4 * l**2 + l0**2)/(8 * (s - t))) /(np.sqrt(np.pi) * 16 * (t - s)**(5/2)) *
+                ( - ( (l0 - 2*l)**2 + 8 * (s - t)) * np.exp( (l0 + 2 * l)**2/(16 * (t - s)))
+                  + ( (l0 + 2*l)**2 + 8 * (s - t)) * np.exp( (l0 - 2 * l)**2/(16 * (t - s)))
+                )
+            )
+
+def total_function(tt, l0, l):
+    ss = np.arange(0, tt-h/10, h)
+    index = np.arange(len(ss))
+    # print(l[index])
+    return np.trapz(k_integral(l[index], l0, ss[index], tt), ss[index])
+    
+# Explicit Euler Method
+h = 0.01
+t = np.arange(0, 50, h)
+
+length = np.zeros(len(t))
+l0 = 1
+length[0] = l0
+
+for i in range(0, len(t)-1):
+    print(i)
+    length[i+1] = length[i] + h * (1/np.pi) * total_function(t[i], l0, length)    
+
+fig1, ax1 = plt.subplots(1, 1) 
+ax1.plot(t, length) 
+ax1.set_ylabel("$L(t)$") 
+ax1.set_xlabel("$t$") 
+
+# # fig2, ax2 = plt.subplots(1, 1) 
+# # 
+# ax2.loglog(t, length) 
+# ax2.set_ylabel("$\log L(t)$") 
+# ax2.set_xlabel("$\log t$") 
+
+# fig3, ax3 = plt.subplots(1, 1) 
+
+# ax3.semilogy(t, length) 
+# ax3.set_ylabel("$\log L(t)$") 
+# ax3.set_xlabel("$t$") 
+ 
+plt.show() 
+print(length[-1])
\ No newline at end of file

control_problem/zeroth_control_problem_analytical.py

+import numpy as np
+import matplotlib.pyplot as plt
+import scipy
+import math
+import sympy
+
+N = 100
+sigma0 = -1
+l0 = 1
+
+# t array
+T = 100
+dt = 0.01
+times = np.arange(0, T, dt)
+
+def hermite_function(order, arg):
+    return ((2**order * math.factorial(order) * np.sqrt(np.pi))**(-0.5) 
+            * scipy.special.eval_hermite(order, arg) * np.exp(-arg**2/2) 
+            )
+
+# vector of hermite functions
+vector_of_hermite_functions = np.zeros(N, dtype = 'complex_')
+for q in range(0, N):
+    vector_of_hermite_functions[q] = hermite_function(q, 0) * (-1j)**(q) 
+# print(vector_of_hermite_functions)
+
+# adjacency matrix
+a = np.zeros((N, N), dtype='complex_')
+for n in range(0, N):
+    for m in range(0, N):
+        if m == n:
+            a[m][n] = (n + 1/2)
+        elif m == n - 2:
+            a[m][n] = np.sqrt(n * (n - 1))/2
+        elif m == n + 2:
+           a[m][n] = np.sqrt((n + 1) * (n + 2))/2
+# q1
+q1_array = np.zeros(N, dtype='complex_')
+q1_array[0] = (1/2j) * np.sqrt(1/2) * (-1j) * (hermite_function(1, -2 * l0) - hermite_function(1, 2 * l0))
+for p1 in range(1, N):
+    q1_array[p1] = (1/2j) * ( np.sqrt(p1/2) * (-1j)**(p1-1) * (hermite_function(p1 - 1, -2 * l0) - hermite_function(p1 - 1, 2 * l0))
+                            + np.sqrt((p1 + 1)/2) * (-1j)**(p1+1) * (hermite_function(p1 + 1, -2 * l0) - hermite_function(p1 + 1, 2 * l0))    
+                            )
+# q2
+q2_array = np.zeros(N, dtype='complex_')
+for p2 in range(0, 2):
+    q2_array[p2] = (-1/4) * (     ((2 * p2 + 1)/2)         * (-1j)**p2 *     (hermite_function(p2, -3*l0/2) - hermite_function(p2, -l0/2) - hermite_function(p2, l0/2) + hermite_function(p2, 3*l0/2))
+                                + np.sqrt((p2+1)*(p2+2))/2 * (-1j)**(p2+2) * (hermite_function(p2+2, -3*l0/2) - hermite_function(p2+2, -l0/2) - hermite_function(p2+2, l0/2) + hermite_function(p2+2, 3*l0/2))
+                            )
+for p3 in range(2, N):
+    q2_array[p3] = (-1/4) * (   ((2 * p3 + 1)/2)           * (-1j)**p3     * (hermite_function(p3, -3*l0/2) - hermite_function(p3, -l0/2) - hermite_function(p3, l0/2) + hermite_function(p3, 3*l0/2))
+                                + np.sqrt((p3+1)*(p3+2))/2 * (-1j)**(p3+2) * (hermite_function(p3+2, -3*l0/2) - hermite_function(p3+2, -l0/2) - hermite_function(p3+2, l0/2) + hermite_function(p3+2, 3*l0/2))
+                                + np.sqrt(p3*(p3-1))/2     * (-1j)**(p3-2) * (hermite_function(p3-2, -3*l0/2) - hermite_function(p3-2, -l0/2) - hermite_function(p3-2, l0/2) + hermite_function(p3-2, 3*l0/2))
+                            )
+#  matrix
+matrix = a + (1/np.pi) * np.dot(q1_array, vector_of_hermite_functions)
+
+# discard imaginary parts of matrix if they are < machine precision
+index = np.where(np.imag(matrix) < 1e-10)
+matrix[index] = np.real(matrix[index])
+# print('M=', matrix)
+
+# diagonal matrix
+evalues_m, evectors_m = np.linalg.eig(matrix)
+print(np.max(evalues_m), np.min(evalues_m))
+diagonal_matrix = np.diag(evalues_m)
+# print('D=', diagonal_matrix)
+
+d_inv = np.linalg.inv(diagonal_matrix)
+# print('$D^{-1}=$', d_inv)
+
+q2_hat_array = np.dot(np.linalg.inv(evectors_m), q2_array)
+
+# exp of  diag matrix
+exp_diag_matrix = np.zeros((len(times), N, N))
+for tt in range(0, len(times)):
+    for nn in range(0, N):
+        # if (-times[tt] * evalues_m[nn]) < 2e01:
+        exp_diag_matrix[tt][nn][nn] = np.exp(- times[tt] * evalues_m[nn])
+
+print(np.max(exp_diag_matrix), np.min(exp_diag_matrix))
+# length
+length = np.zeros(len(times))
+for l in range(0, len(times)):
+    length[l] = l0 + (2 * sigma0/np.pi) * (np.linalg.multi_dot([vector_of_hermite_functions, evectors_m, 
+                                                              np.identity(N)*times[l], d_inv, q2_hat_array])
+                                            + np.linalg.multi_dot([vector_of_hermite_functions, evectors_m, exp_diag_matrix[l], 
+                                                              d_inv, d_inv, q2_hat_array])
+                                            - np.linalg.multi_dot([vector_of_hermite_functions, evectors_m, 
+                                                              d_inv, d_inv, q2_hat_array])
+                                )
+
+plt.plot(times, length)
+plt.title('Zeroth order control problem')
+plt.show()
+# print(length[-1])
\ No newline at end of file