Wolfram Language Paclet Repository
Community-contributed installable additions to the Wolfram Language
ZigangPan`NonlinearCholeskyFactorization`
backsteppinglocaloptimalmatchingglobalinverseoptimal  |
| {V,α,qcheck,rcheck}=backsteppinglocaloptimalmatchingglobalinverseoptimal[f,g,h,q,r,γ,xc,m] m xc'[t]=f[xc]+g[xc]u+h[xc]w t ∫ 0 f[xc]={a1[x1]x2+f1[x1],a2[x1,x2]x3+f2[x1,x2],...fn[x1,...,xn]} g[xc]={{0},{0},...,{b[xc]}} h[xc]={h1[x1],h2[x1,x2],...hn[x1,...,xn]} f[0]=0 a1[x1]≠0 a2[x1,x2]≠0 b[xc]≠0 q[xc] r[xc] γ xc={x1,x2,...,xn}, V α qcheck rcheck | |
Examples
(1)
Basic Examples
(1)
In[1]:=
xc={x1,x2};
In[2]:=
$Assumptions=((x1|x2)∈Reals);
In[3]:=
fe[x1_,x2_]:={x2+x1^2,0}
In[4]:=
ge[x1_,x2_]:={{0},{1}}
In[5]:=
he[x1_,x2_]:={{1},{0}}
In[6]:=
qe[x1_,x2_]:=x1^2+x2^2
In[7]:=
γ=4;
In[8]:=
f=Apply[fe,xc];
In[9]:=
g=Apply[ge,xc];
In[10]:=
h=Apply[he,xc];
In[11]:=
r={{1}};
In[12]:=
q=Apply[qe,xc];
In[13]:=
s=(g.Inverse[r].Transpose[g]-h.Transpose[h]/γ^2)/4;
In[14]:=
Va2=
[f,s,q,xc,2]
 | approximateHJIequation |
Out[14]=
1.8717007154895465283161760209428058465234317466386+2.2081249720274031853679747747788583689816484446750x1x2+1.8122664250438800684138278298792941843986802781164
2
x1
2
x2
In[15]:=
Va3=
[f,s,q,xc,3]
| approximateHJIequation |
Out[15]=
1.8717007154895465283161760209428058465234317466386+2.9823690239504571962366257455014334394864580442933+2.2081249720274031853679747747788583689816484446750x1x2+4.338563859297960000755192639715877875550556325191x2+1.8122664250438800684138278298792941843986802781164+2.2304196799928599937349927998131939360983499770050x1+0.4385534995646503487291876620616039596984551071135
2
x1
3
x1
2
x1
2
x2
2
x2
3
x2
In[16]:=
ans=
[f,g,h,q,r,γ,xc,3];
 | backsteppinglocaloptimalmatchingglobalinverseoptimal |
In[17]:=
Vx=D[ans〚1〛,{xc}];
In[18]:=
Plot3D[ans〚1〛,{x1,-2,2},{x2,-5,5},PlotRange{0,3}]
Out[18]=
In[19]:=
Plot3D[ans〚4〛,{x1,-2,2},{x2,-5,5},PlotRange{0,2}]
Out[19]=
In[20]:=
Plot3D[ans〚3〛,{x1,-2,2},{x2,-5,5},PlotRange{0,20}]
Out[20]=
In[21]:=
Plot3D[ans〚2〛,{x1,-2,2},{x2,-5,5},PlotRange{-60,10}]
Out[21]=
In[22]:=
Plot3D[Vx.f-Vx.(g.Inverse[ans〚4〛].Transpose[g]-1/γ^2h.Transpose[h]).Vx/4+ans〚3〛,{x1,-2,2},{x2,-5,5},PlotRangeAll]
Out[22]=
In[23]:=
closedloopdynamics=FormulaToFunction[xc,f+g.ans〚2〛+h.{0}];
In[24]:=
sol=RungeKutta45[closedloopdynamics,0,{0.4,0.5},20,1/100];
10% completed.
20% completed.
30% completed.
40% completed.
50% completed.
60% completed.
70% completed.
80% completed.
90% completed.
100% completed.
In[25]:=
Plot[sol[t],{t,0,20},PlotRangeAll]
Out[25]=
