<jupyter_start><jupyter_text>Krmiljenje lateralne pozicije kvadrotorjaLateralno hitrost kvadrotorja $u$ lahko krmilimo z nagibom vozila za kot okrog vzdolžne osi $\theta$. Predpostavimo, da je vozilo opremljeno z avtopilotom, ki krmili kot okrog vzodlžne osi $\theta$ do zahtevane vrednosti $\theta_r$, s čimer lahko zaprtozančno dinamiko okrog vzdolžne osi modeliramo kot odziv sistema drugega reda s poloma v $-20\pm25i$ in enotskim ojačanjem. Ko je vozilo nagnjeno za kot $\theta$, propelerji generirajo lateralno silo, ki je približno enaka $F_l = F\sin{\theta} \approx mg\theta$, kjer sta $m=1800$ g masa vozila, in $g = 9.81$ m/$\text{s}^2$ težnostni pospešek. Prisotnost vetra se izraža v obliki lateralne sile $D = -cu = -0.8u$. Kot okrog vzdolžne osi mora biti znotraj intervala vrednosti $\pm20$ stopinj skozi celotno obratovanje. Pozicija vozila je merjena z GPS.Razviti želimo regulator, katerega vhod je $\theta_r$, $s$ pa izhod ter izpolnjuje naslednje zahteve:- čas ustalitve krajši od 7 s;- brez prenihaja;- brez ali z minimalnim odstopkom v stacionarnem stanju v odzivu na koračno funkcijo pomika.Lateralno dinamiko lahko popišemo z naslednjima enačbama:$$m\dot{u} = -cu + mg\theta$$in$$\dot s = u$$Možna popis dinamike (gibanja) okrog vzdolžne osi je:\begin{cases}    \dot{z} = \begin{bmatrix} 0 & 1 \\ -\omega_n^2 & -2\xi\omega_n \end{bmatrix}z + \begin{bmatrix} 0 \\ 1 \end{bmatrix}\theta_r \\    \theta = \begin{bmatrix} \omega_n^2 & 0 \end{bmatrix}z,\end{cases}pri čemer velja $z = \begin{bmatrix} z_1 & z_2 \end{bmatrix}^T$, $\omega_n=\sqrt{(-20)^2+(25)^2} \approx 32$ in $\xi = \frac{20}{\omega_n} \approx 0.62$.Če združimo zgornji enačbi in definiramo vektor stanj $x = \begin{bmatrix} x_1 & x_2 & x_3 & x_4 \end{bmatrix}^T= \begin{bmatrix} s & u & z_1 & z_2 \end{bmatrix}^T$ lahko zapišemo:\begin{cases}    \dot{x} = \begin{bmatrix} 0 & 1 & 0 & 0 \\ 0 & -c/m & g\omega_n^2 & 0 \\ 0 & 0 & 0 & 1 \\ 0 & 0 & -\omega_n^2 & -2\xi\omega_n \end{bmatrix}x + \begin{bmatrix} 0 \\ 0 \\ 0 \\ 1 \end{bmatrix}\theta_r \\    y = \begin{bmatrix} 1 & 0 & 0 & 0 \end{bmatrix}x\end{cases} Načrtovanje regulatorja Načrtovanje krmilnikaDa zagotovimo praktično ničen odstopek v stacionarnem stanju, dodamo vnaprejšnje ojačanje, ki je enako inverzni vrednosti statičnega ojačanja zaprtozančnega sistema. Poli dinamike okrog vzdolžne osi so hitri; z namenom ohranitve vrednosti $\theta$ med prehodnim pojavom razporedimo pole zaprtozančnega sistema tako, da so 13-krat manjši od polov odprtozančnega sistema: ($-1.54\pm1.92i$). Glede preostalih polov pa naredimo naslednje: rahlo povečamo konvergenco pola $-c/m$ (=> $-0.7$), zadnji pol pa razporedimo v $-0.7$. Načrtovanje spoznavalnikaRazvijemo spoznavalnik vseh stanj razširjenega sistema s poli, ki ležijo blizu $-30$, s čimer zagotovimo razmeroma hitro konvergenco. Kako upravljati s tem interaktivnim primerom?- Testiraj sistem ob prisotnosti napak v začetnih ocenjenih stanjih in, če je potrebno, razporedi pole tako, da bo sistem ustrezal danim zahtevam.<jupyter_code># Preparatory cell

X0 = numpy.matrix('0.0; 0.0; 0.0; 0.0')
K = numpy.matrix([0,0,0,0])
L = numpy.matrix([[0],[0],[0],[0]])

X0w = matrixWidget(4,1)
X0w.setM(X0)
Kw = matrixWidget(1,4)
Kw.setM(K)
Lw = matrixWidget(4,1)
Lw.setM(L)


eig1c = matrixWidget(1,1)
eig2c = matrixWidget(2,1)
eig3c = matrixWidget(1,1)
eig4c = matrixWidget(2,1)
eig1c.setM(numpy.matrix([-0.7])) 
eig2c.setM(numpy.matrix([[-1.54],[-1.92]]))
eig3c.setM(numpy.matrix([-0.7]))
eig4c.setM(numpy.matrix([[-1.],[-1.]]))

eig1o = matrixWidget(1,1)
eig2o = matrixWidget(2,1)
eig3o = matrixWidget(1,1)
eig4o = matrixWidget(2,1)
eig1o.setM(numpy.matrix([-30.])) 
eig2o.setM(numpy.matrix([[-30.],[0.]]))
eig3o.setM(numpy.matrix([-30.1]))
eig4o.setM(numpy.matrix([[-30.2],[0.]]))
# Misc

#create dummy widget 
DW = widgets.FloatText(layout=widgets.Layout(width='0px', height='0px'))

#create button widget
START = widgets.Button(
    description='Test',
    disabled=False,
    button_style='', # 'success', 'info', 'warning', 'danger' or ''
    tooltip='Test',
    icon='check'
)
                       
def on_start_button_clicked(b):
    #This is a workaround to have intreactive_output call the callback:
    #   force the value of the dummy widget to change
    if DW.value> 0 :
        DW.value = -1
    else: 
        DW.value = 1
    pass
START.on_click(on_start_button_clicked)

# Define type of method 
selm = widgets.Dropdown(
    options= ['Nastavi K in L', 'Nastavi lastne vrednosti'],
    value= 'Nastavi lastne vrednosti',
    description='',
    disabled=False
)

# Define the number of complex eigenvalues
selec = widgets.Dropdown(
    options= ['brez kompleksnih lastnih vrednosti', 'dve kompleksni lastni vrednosti', 'štiri kompleksne lastne vrednosti'],
    value= 'dve kompleksni lastni vrednosti',
    description='Kompleksne lastne vrednosti krmilnika:',
    disabled=False
)
seleo = widgets.Dropdown(
    options= ['brez kompleksnih lastnih vrednosti', 'dve kompleksni lastni vrednosti'],
    value= 'brez kompleksnih lastnih vrednosti',
    description='Kompleksne lastne vrednosti spoznavalnika:',
    disabled=False
)

#define type of ipout 
selu = widgets.Dropdown(
    options=['impulzna funkcija', 'koračna funkcija', 'sinusoidna funkcija', 'kvadratni val'],
    value='koračna funkcija',
    description='Vhod:',
    style = {'description_width': 'initial'},
    disabled=False
)
# Define the values of the input
u = widgets.FloatSlider(
    value=10,
    min=0,
    max=20,
    step=1,
    description='',
    disabled=False,
    continuous_update=False,
    orientation='horizontal',
    readout=True,
    readout_format='.1f',
)
period = widgets.FloatSlider(
    value=0.5,
    min=0.001,
    max=10,
    step=0.001,
    description='Perioda: ',
    disabled=False,
    continuous_update=False,
    orientation='horizontal',
    readout=True,
    readout_format='.3f',
)

simTime = widgets.FloatText(
    value=10,
    description='',
    disabled=False
)
# Support functions

def eigen_choice(selec,seleo):
    if selec == 'brez kompleksnih lastnih vrednosti':
        eig1c.children[0].children[0].disabled = False
        eig2c.children[1].children[0].disabled = True
        eig3c.children[0].children[0].disabled = False
        eig4c.children[0].children[0].disabled = False
        eig4c.children[1].children[0].disabled = True
        eigc = 0
    if seleo == 'brez kompleksnih lastnih vrednosti':
        eig1o.children[0].children[0].disabled = False
        eig2o.children[1].children[0].disabled = True
        eig3o.children[0].children[0].disabled = False
        eig4o.children[0].children[0].disabled = False
        eig4o.children[1].children[0].disabled = True
        eigo = 0
    if selec == 'dve kompleksni lastni vrednosti':
        eig1c.children[0].children[0].disabled = False
        eig2c.children[1].children[0].disabled = False
        eig3c.children[0].children[0].disabled = False
        eig4c.children[0].children[0].disabled = True
        eig4c.children[1].children[0].disabled = True
        eigc = 2
    if seleo == 'dve kompleksni lastni vrednosti':
        eig1o.children[0].children[0].disabled = False
        eig2o.children[1].children[0].disabled = False
        eig3o.children[0].children[0].disabled = False
        eig4o.children[0].children[0].disabled = True
        eig4o.children[1].children[0].disabled = True
        eigo = 2
    if selec == 'štiri kompleksne lastne vrednosti':
        eig1c.children[0].children[0].disabled = True
        eig2c.children[1].children[0].disabled = False
        eig3c.children[0].children[0].disabled = True
        eig4c.children[0].children[0].disabled = False
        eig4c.children[1].children[0].disabled = False
        eigc = 4
    if seleo == 'štiri kompleksne lastne vrednosti':
        eig1o.children[0].children[0].disabled = True
        eig2o.children[1].children[0].disabled = False
        eig3o.children[0].children[0].disabled = True
        eig4o.children[0].children[0].disabled = False
        eig4o.children[1].children[0].disabled = False
        eigo = 4
    return eigc, eigo

def method_choice(selm):
    if selm == '<NAME>':
        method = 1
        selec.disabled = True
        seleo.disabled = True
    if selm == 'Nastavi lastne vrednosti':
        method = 2
        selec.disabled = False
        seleo.disabled = False
    return method
c = 0.8
m = 1.8
omega = 32
xi = 0.62
g = 9.81

A = numpy.matrix([[0, 1, 0, 0],
                  [0, -c/m, g*omega**2, 0],
                  [0, 0, 0, 1],
                  [0, 0, -omega**2, -2*xi*omega]])
B = numpy.matrix([[0],[0],[0],[1]])
C = numpy.matrix([[1,0,0,0]])
OLpoles, OLvectors = numpy.linalg.eig(A)

def main_callback2(X0w, K, L, eig1c, eig2c, eig3c, eig4c, eig1o, eig2o, eig3o, eig4o, u, period, selm, selec, seleo, selu, simTime, DW):
    eigc, eigo = eigen_choice(selec,seleo)
    method = method_choice(selm)
    
    if method == 1:
        solc = numpy.linalg.eig(A-B*K)
        solo = numpy.linalg.eig(A-L*C)
    if method == 2:
        #for bettere numerical stability of place
        if eig1c[0,0]==eig2c[0,0] or eig1c[0,0]==eig3c[0,0] or eig1c[0,0]==eig4c[0,0]:
            eig1c[0,0] *= 1.01
        if eig2c[0,0]==eig3c[0,0] or eig2c[0,0]==eig4c[0,0]:
            eig3c[0,0] *= 1.015
        if eig1o[0,0]==eig2o[0,0] or eig1o[0,0]==eig3o[0,0] or eig1o[0,0]==eig4o[0,0]:
            eig1o[0,0] *= 1.01
        if eig2o[0,0]==eig3o[0,0] or eig2o[0,0]==eig4o[0,0]:
            eig3o[0,0] *= 1.015
            
        if eigc == 0:
            K = control.acker(A, B, [eig1c[0,0], eig2c[0,0], eig3c[0,0], eig4c[0,0]])
            Kw.setM(K)
        if eigc == 2:
            K = control.acker(A, B, [eig3c[0,0],
                                      eig1c[0,0],
                                      numpy.complex(eig2c[0,0], eig2c[1,0]), 
                                      numpy.complex(eig2c[0,0],-eig2c[1,0])])
            Kw.setM(K)
        if eigc == 4:
            K = control.acker(A, B, [numpy.complex(eig4c[0,0], eig4c[1,0]), 
                                      numpy.complex(eig4c[0,0],-eig4c[1,0]),
                                      numpy.complex(eig2c[0,0], eig2c[1,0]), 
                                      numpy.complex(eig2c[0,0],-eig2c[1,0])])
            Kw.setM(K)
        if eigo == 0:
            L = control.place(A.T, C.T, [eig1o[0,0], eig2o[0,0], eig3o[0,0], eig4o[0,0]]).T
            Lw.setM(L)
        if eigo == 2:
            L = control.place(A.T, C.T, [eig3o[0,0],
                                         eig1o[0,0],
                                         numpy.complex(eig2o[0,0], eig2o[1,0]), 
                                         numpy.complex(eig2o[0,0],-eig2o[1,0])]).T
            Lw.setM(L)
        if eigo == 4:
            L = control.place(A.T, C.T, [numpy.complex(eig4o[0,0], eig4o[1,0]), 
                                      numpy.complex(eig4o[0,0],-eig4o[1,0]),
                                      numpy.complex(eig2o[0,0], eig2o[1,0]), 
                                      numpy.complex(eig2o[0,0],-eig2o[1,0])]).T
            Lw.setM(L)
            
    sys = sss(A,B,[[1,0,0,0],[0,0,0,0]],[[0],[1]])
    syse = sss(A-L*C,numpy.hstack((B,L)),numpy.eye(4),numpy.zeros((4,2)))
    sysc = sss(0,[0,0,0,0],0,-K)
    sys_append = control.append(sys,syse,sysc)
    try:
        sys_CL = control.connect(sys_append,
                                 [[1,7],[2,2],[3,1],[4,3],[5,4],[6,5],[7,6]],
                                 [1],
                                 [1,2])
    except:
        sys_CL = control.connect(sys_append,
                                 [[1,7],[2,2],[3,1],[4,3],[5,4],[6,5],[7,6]],
                                 [1],
                                 [1,2])

    X0w1 = numpy.zeros((8,1))
    X0w1[4,0] = X0w[0,0]
    X0w1[5,0] = X0w[1,0]
    X0w1[6,0] = X0w[2,0]
    X0w1[7,0] = X0w[3,0]
    
    u1 = u
    try:
        DCgain = control.dcgain(sys_CL[0,0])
        u = u/DCgain
    except:
        print("Napaka v izračunu DC ojačanja zaprtozančnega krmiljenega sistema. Vnaprejšnje ojačanje je nastavljeno na 1.")
        DCgain = 1
        
    if simTime != 0:
        T = numpy.linspace(0, simTime, 10000)
    else:
        T = numpy.linspace(0, 1, 10000)
      
    if selu == 'impulzna funkcija': #selu
        U = [0 for t in range(0,len(T))]
        U[0] = u
        U1 = [0 for t in range(0,len(T))]
        U1[0] = u1
        T, yout, xout = control.forced_response(sys_CL,T,U,X0w1)
    if selu == 'koračna funkcija':
        U = [u for t in range(0,len(T))]
        U1 = [u1 for t in range(0,len(T))]
        T, yout, xout = control.forced_response(sys_CL,T,U,X0w1)
    if selu == 'sinusoidna funkcija':
        U = u*numpy.sin(2*numpy.pi/period*T)
        U1 = u1*numpy.sin(2*numpy.pi/period*T)
        T, yout, xout = control.forced_response(sys_CL,T,U,X0w1)
    if selu == 'kvadratni val':
        U = u*numpy.sign(numpy.sin(2*numpy.pi/period*T))
        U1 = u1*numpy.sign(numpy.sin(2*numpy.pi/period*T))
        T, yout, xout = control.forced_response(sys_CL,T,U,X0w1)
    
    try:
        step_info_dict = control.step_info(sys_CL[0,0],SettlingTimeThreshold=0.05,T=T)
        print('Poli odprtozančnega sistema: ', numpy.round(OLpoles,3))
        print('Zaprtozančni sistem')
        print('Informacije o odzivu sistema: \n\tČas vzpona [s] =',step_info_dict['RiseTime'],'\n\tČas ustalitve (5%) [s] =',step_info_dict['SettlingTime'],'\n\tPrenihaj [%]=',step_info_dict['Overshoot'])
        print('')
        print('Maksimalna vrednost theta (delež od vrednosti 20°)=', max(abs(xout[2]*omega**2))/(numpy.pi/180*20)*100)
    except:
        print("Napaka v izračunu informacij o odzivu sistema.")
    print("Ojačanje zaprtozančnega sistema =",DCgain)
    
    fig = plt.figure(num='Simulacija 1', figsize=(14,12))
    
    fig.add_subplot(221)
    plt.title('Odziv sistema')
    plt.ylabel('Izhod')
    plt.plot(T,yout[0],T,U1,'r--')
    plt.xlabel('$t$ [s]')
    plt.axvline(x=0,color='black',linewidth=0.8)
    plt.axhline(y=0,color='black',linewidth=0.8)
    plt.legend(['$y$','Referenca'])
    plt.grid()
    
    fig.add_subplot(222)
    plt.title('Vhod')
    plt.ylabel('$u$')
    plt.plot(T,yout[1])
    plt.xlabel('$t$ [s]')
    plt.axvline(x=0,color='black',linewidth=0.8)
    plt.axhline(y=0,color='black',linewidth=0.8)
    plt.grid()
    
    fig.add_subplot(223)
    plt.title('Odziv tretjega stanja')
    plt.ylabel(r'$x_3$')
    plt.plot(T,xout[2],
             T,[20*numpy.pi/180/omega**2 for i in range(len(T))],'r--',
             T,[-20*numpy.pi/180/omega**2 for i in range(len(T))],'r--')
    plt.xlabel('$t$ [s]')
    plt.axvline(x=0,color='black',linewidth=0.8)
    plt.axhline(y=0,color='black',linewidth=0.8)
    plt.grid()
    
    fig.add_subplot(224)
    plt.title('Napaka ocene stanj')
    plt.ylabel('Napaka ocene stanj')
    plt.plot(T,xout[4]-xout[0])
    plt.plot(T,xout[5]-xout[1])
    plt.plot(T,xout[6]-xout[2])
    plt.plot(T,xout[7]-xout[3])
    plt.xlabel('$t$ [s]')
    plt.axvline(x=0,color='black',linewidth=0.8)
    plt.axhline(y=0,color='black',linewidth=0.8)
    plt.legend(['$e_{1}$','$e_{2}$','$e_{3}$','$e_{4}$'])
    plt.grid()
    #plt.tight_layout()
   
alltogether2 = widgets.VBox([widgets.HBox([selm, 
                                          selec,
                                          seleo,
                                          selu]),
                            widgets.Label(' ',border=3),
                            widgets.HBox([widgets.HBox([widgets.Label('K:',border=3), Kw, 
                                                        widgets.Label('Lastne vrednosti:',border=3),
                                                        widgets.HBox([eig1c, 
                                                                      eig2c, 
                                                                      eig3c,
                                                                      eig4c])])]),
                            widgets.Label(' ',border=3),
                            widgets.HBox([widgets.VBox([widgets.HBox([widgets.Label('L:',border=3), Lw, widgets.Label(' ',border=3),
                                          widgets.Label(' ',border=3),
                                          widgets.Label('Lastne vrednosti:',border=3), 
                                          eig1o, 
                                          eig2o,
                                          eig3o, 
                                          eig4o,
                                          widgets.Label(' ',border=3),
                                          widgets.Label(' ',border=3),
                                          widgets.Label('X0 est.:',border=3), X0w]),
                                          widgets.Label(' ',border=3),
                                                       widgets.HBox([
                                          widgets.VBox([widgets.Label('Simulacijski čas [s]:',border=3)]),
                                          widgets.VBox([simTime])])]),
                                          widgets.Label(' ',border=3)]),
                            widgets.Label(' ',border=3),
                            widgets.HBox([widgets.Label('Referenca [m]:',border=3),
                                          u,
                                          period, 
                                          START])])
out2 = widgets.interactive_output(main_callback2, {'X0w':X0w, 'K':Kw, 'L':Lw,
                                                   'eig1c':eig1c, 'eig2c':eig2c, 'eig3c':eig3c, 'eig4c':eig4c, 
                                                   'eig1o':eig1o, 'eig2o':eig2o, 'eig3o':eig3o, 'eig4o':eig4o,  
                                                   'u':u, 'period':period, 'selm':selm, 'selec':selec, 'seleo':seleo, 'selu':selu, 'simTime':simTime, 'DW':DW})
out2.layout.height = '900px'
display(out2, alltogether2)<jupyter_output><empty_output>
StarcoderdataJupyterStructuredCleanDedup