In [4]:

    

import numpy as np
import sympy as sy
import control.matlab as cm
sy.init_printing()


    

In [17]:

    

h = 0.14 # Sampling period
z = sy.symbols('z', real=False)
r1,r2, s0,s1,s2 = sy.symbols('r1,r2,s0,s1,s2', real=True) # Second order controller
pc1 = -1-1j
pc2 = np.conjugate(pc1)
pc3 = -1
pd1 = np.exp(h*pc1)
pd2 = np.exp(h*pc2)
pd3 = np.exp(h*pc3)

# The desired closed-loop poles
Ac = sy.poly(sy.simplify((z-pd1)*(z-pd2))*(z-pd3), z)
Ao = sy.poly(z**2, z)

# The plant
Bp = sy.poly(h**3/6.0 * (z**2 + 4*z + 1), z)
Ap = sy.poly((z-1)**3, z)

# The controller
Rp = sy.poly(z**2 + r1*z + r2, z)
Sp = sy.poly(s0*z**2 + s1*z + s2, z)
LHS = Ap*Rp + Bp*Sp
RHS = Ac*Ao
dioph=(LHS-RHS).all_coeffs()


    

In [18]:

    

sy.simplify((z-pd1)*(z-pd2))


    

    
Out[18]:





$$1.0 z^{2} - 1.72170486226218 z + 0.755783741455725$$

In [19]:

    

RHS.all_coeffs()


    

    
Out[19]:





$$\left [ 1.0, \quad -2.59106309766098, \quad 2.25256204238952, \quad -0.657046819815057, \quad 0.0, \quad 0.0\right ]$$

In [7]:

    




    

    
Out[7]:





$$\left [ 1.0, \quad 1.0 r_{1} + 0.000457333333333333 s_{0} - 3.0, \quad - 3.0 r_{1} + 1.0 r_{2} + 0.00182933333333333 s_{0} + 0.000457333333333333 s_{1} + 3.0, \quad 3.0 r_{1} - 3.0 r_{2} + 0.000457333333333333 s_{0} + 0.00182933333333333 s_{1} + 0.000457333333333333 s_{2} - 1.0, \quad - 1.0 r_{1} + 3.0 r_{2} + 0.000457333333333333 s_{1} + 0.00182933333333333 s_{2}, \quad - 1.0 r_{2} + 0.000457333333333333 s_{2}\right ]$$

In [20]:

    

sol=sy.solve(dioph, (r1,r2,s0,s1, s2))
print sol[r1]
print sol[r2]
print sol[s0]
print sol[s1]
print sol[s2]

t0 = Ac.evalf(subs={z:1})/Bp.evalf(subs={z:1,})
print t0


    

    




0.329165114241075
0.0680324802533507
174.428108085880
-321.564681193252
148.759067609367
1.62249450199559

In [32]:

    

type(t0)


    

    
Out[32]:





sympy.core.numbers.Float

In [34]:

    

G = cm.tf([1], [1, 0, 0,0])
Gd = cm.c2d(G, h)
Fb = cm.tf([float(sol[s0]), float(sol[s1]), float(sol[s2])], 
           [1., float(sol[r1]), float(sol[r2])], h)
Ff = cm.tf([float(t0), 0, 0], [1., float(sol[r1]), float(sol[r2])], h)

Gc = Ff * cm.feedback(Gd, Fb)
cm.pole(Gc)


    

    
Out[34]:





array([  8.69358235e-01 +0.00000000e+00j,
         8.60852431e-01 +1.21312956e-01j,
         8.60852431e-01 -1.21312956e-01j,
        -1.64582557e-01 +2.02348862e-01j,
        -1.64582557e-01 -2.02348862e-01j,
         5.95862269e-14 +4.54381191e-07j,   5.95862269e-14 -4.54381191e-07j])

In [33]:

    

print A2p
print A2p.evalf(subs={z:1})
print Bp
print Bp.evalf(subs={z:1})


    

    




Poly(z**2 - 1.58555290309441*z - (-0.792776451547206 + 0.168974310731771*I)*(0.792776451547206 + 0.168974310731771*I), z, domain='EX')
0.0714939167206446
Poly(0.00133942860759726*z + 0.00124712240506047, z, domain='RR')
0.00258655101265772

In [4]:

    

0.3/(5*np.sqrt(2))


    

    
Out[4]:





0.042426406871192847

In [6]:

    

np.exp(-0.21)*np.sin(0.21)


    

    
Out[6]:





0.16897431073177133

In [7]:

    

np.exp(0.03*(-14))


    

    
Out[7]:





0.65704681981505675

In [ ]: