'V' Formation Aircraft
'V' Formation Aircraft¶
https://www.ridwanreza.com/2022/05/v-formation-aircraft.html
import autograd.numpy as np
import math
from autograd import grad, jacobian, hessian
import matplotlib.pyplot as plt
# Function to calculate Steepest Descent and Newton Method
def nablaf(x,f):
j=jacobian(f)
return j(x)
def nabla2f(x,f):
j=hessian(f)
return j(x)
def Anablaf(x,f):
return nabla2f(x,f)@nablaf(x,f)
def theta(x,f):
return -(nablaf(x,f)@nablaf(x,f))/(nablaf(x,f)@Anablaf(x,f))
def steepest_descent(x,f,printt=False):
stop=False
iter=0; max_iter=250
error=0.1**8
while stop==False:
iter+=1
r=nablaf(x,f)
Ar=Anablaf(x,f)
t=theta(x,f)
x=x+t*r
#if iter%10==1 and printt==True:
# print('Iter =',iter) ; print('x =',x) #; print('r =',nablaf(x,f)) ; print('|r| =',np.linalg.norm(nablaf(x,f)))
if iter>=max_iter or np.linalg.norm(nablaf(x,f))<=error: stop = True
if printt==True: print('Iter =',iter) ; print('x =',x)
return x,iter
def newton(x,f,printt=False):
stop=False
iter=0; max_iter=250
error=0.1**8
while stop==False:
iter+=1
r=nablaf(x,f)
Ar=Anablaf(x,f)
t=theta(x,f)
x=x-np.linalg.inv(nabla2f(x,f))@r
if(printt==True): print('Iter =',iter); print('x =',x)
if iter>=max_iter or np.linalg.norm(nablaf(x,f))<=error: stop = True
return x,iter
#Function To Calculate Enhanced Steepest Descent (Riahi, 2016)
#https://www.ridwanreza.com/2022/04/sd-newton-enhanced-sd.html
def pin(x,n,n_hat):
xtemp= np.zeros(len(x))
m=len(x)
a=int((n-1)*(m/n_hat)+1)
b=int(n*(m/n_hat))
for i in range(a,b+1):
xtemp[i-1]=x[i-1]
return xtemp
def phi_nhat(r):
phi=np.zeros(len(r))
for i in range(len(r)):
phi[i]=r[i]@r[i]
return phi
def phi2_nhat(r,x,f):
phi2=np.zeros((len(r),len(r)))
nn=nabla2f(x,f)
for i in range(len(r)):
for j in range(len(r)):
phi2[i][j]=r[i]@(nn@r[j])
return phi2
def Theta_nhat(r,x,f):
try:
return -np.linalg.inv(phi2_nhat(r,x,f))@phi_nhat(r)
except:
print('======================The Phi^2 matrix is singular===========================')
print('Current x=',x)
print('Current Phi^2=',phi2_nhat(r,x,f))
def get_rn(x,f,n_hat):
x_tilda=[]
rn=[]
for n in range(n_hat):
x_tilda.append(pin(x,n+1,n_hat))
nn=nablaf(x,f)
rn.append(pin(nn,n+1,n_hat))
return rn
def update_x(x,rn,n_hat):
s=np.zeros(len(x))
for i in range(n_hat):
s+=Theta_nhat(rn,x,f)[i]*rn[i]
x=x+s
return x
def get_r(rn):
r= 0
for i in range(len(rn)):
r+=np.linalg.norm(rn[i])
return r
def enhanced_sd(x,f,n_hat):
stop=False
iter=0; max_iter=250
error=0.1**3
print("Initial x= ",x)
while stop==False:
iter+=1
rn=get_rn(x,f,n_hat)
try:
x=update_x(x,rn,n_hat)
except:
break
print('Iter =',iter) ; print('x =',x)
if iter>=max_iter or get_r(get_rn(x,f,n_hat))<=error: stop = True
return x,iter
Example 1¶
5 aircraft which will go in a V formation in a 2D plane $(x,y)$.
Private objective for each aircraft $i\in{0,1,2,3,4}$ $$f_1(\mathbb{x})=|\mathbb{A}_i\mathbb{x}_i-\mathbb{b}_i|_2^2$$
Common objective for each aircraft $i\in{0,1,2,3,4}$ $$f_2(\mathbb{x})=\sum_{j\in R(i)}|\mathbb{x}_i-\mathbb{x}_j-\Delta \mathbb{x}_{ij}|_2^2$$
with :
- $\mathbb{x}=\begin{bmatrix}x\\y\end{bmatrix}$
- $\mathbb{A}_i=\begin{bmatrix}0&0\\0&1\end{bmatrix}, \forall i\in{0,1,2,3,4}$
- $\mathbb{b}_i=\begin{bmatrix}0\\i+1\end{bmatrix}, \forall i\in{0,1,2,3,4}$
- $\Delta \mathbb{x}_{ij}=\begin{bmatrix}1\\-\end{bmatrix}, \forall i\in{0,1,2,3,4}, \forall j\in R(i)$
- The $y$-axis distance is ignored
Minimize : $$f(\mathbb{x})=\sum_{i=0}^4\left[|\mathbb{A}_i\mathbb{x}_i-\mathbb{b}_i|_2^2+\sum_{j\in R(i)}|\mathbb{x}_i-\mathbb{x}_j-\Delta \mathbb{x}_{ij}|_2^2\right]$$
Calculate the position of the formation using several method.
def fxy(x,b=np.array([1,2,3,4,5], dtype=float)):
return (x[5]-b[0])**2 + (x[6]-b[1])**2 + (x[7]-b[2])**2 +(x[8]-b[3])**2+(x[9]-b[4])**2+2*((x[1]-x[0]-1)**2+(x[2]-x[1]-1)**2+(x[2]-x[3]-1)**2+(x[3]-x[4]-1)**2)
Steepest Descent (example 1)¶
The problem directly solve by steepest descent.
Note : Newton Method and Enhanced Steepest Descent fail to solve because they face a singular matrix inside the calculation.
print('\n=======Steepest Descent========')
xx=np.array([0,0,0,0,0], dtype=float)
yy=np.array([0,0,0,0,0], dtype=float)
x=np.concatenate((xx,yy), axis=None)
x,iter=steepest_descent(x,fxy,printt=True)
=======Steepest Descent======== Iter = 72 x = [-0.8 0.2 1.2 0.2 -0.8 1. 2. 3. 4. 5. ]
xx=x[0:5] ; yy=x[5:10]
plt.plot([0,4],[5,5],"-.g", label='Expected Route') ; plt.plot([0,4],[4,4],"-.g") ; plt.plot([0,4],[3,3],"-.g") ; plt.plot([0,4],[2,2],"-.g") ; plt.plot([0,4],[1,1],"-.g")
plt.plot(xx+1.5,yy,'r', label='V Formation') ; plt.plot(xx+1.5,yy,'r>',label='Aircarft Position')
plt.plot(xx+2.5,yy,'b', label='V Formation (next)') ; plt.plot(xx+2.5,yy,'b>',label='Aircarft Position (next)')
plt.xlabel('x') ; plt.ylabel('y')
plt.title('Aircraft Position-Example 1')
plt.legend()
plt.show()
Steepest Descent with Decentralize Algorithm (example 1)¶
First parent iteration
xx=np.array([0,0,0,0,0], dtype=float)
yy=np.array([0,0,0,0,0], dtype=float)
x=np.concatenate((xx[0],yy[0]), axis=None)
x_tempt=x ; cumulative_iter=0
def fxy0(x):
return (x[1]-1)**2 + 2*(0-x[0]-1)**2
x,iter=newton(x,fxy0);cumulative_iter+=iter
xx[0]=x[0] ; yy[0]=x[1]
print('x=',xx) ; print('y=',yy) ; print('Cumulative Iteration=',cumulative_iter)
x= [-1. 0. 0. 0. 0.] y= [1. 0. 0. 0. 0.] Cumulative Iteration= 1
def fxy1(x):
return (x[1]-2)**2 +2*(0-x[0]-1)**2+ 2*(x[0]+1-1)**2
x,iter=newton(x,fxy1);cumulative_iter+=iter
xx[1]=x[0] ; yy[1]=x[1]
print('x=',xx) ; print('y=',yy) ; print('Cumulative Iteration=',cumulative_iter)
x= [-1. -0.5 0. 0. 0. ] y= [1. 2. 0. 0. 0.] Cumulative Iteration= 2
def fxy2(x):
return (x[1]-3)**2+2*((x[0]-0-1)**2+(x[0]+0.5-1)**2)
x,iter=newton(x,fxy2);cumulative_iter+=iter
xx[2]=x[0] ; yy[2]=x[1]
print('x=',xx) ; print('y=',yy) ; print('Cumulative Iteration=',cumulative_iter)
x= [-1. -0.5 0.75 0. 0. ] y= [1. 2. 3. 0. 0.] Cumulative Iteration= 3
def fxy3(x,b=np.array([1,2,3,4,5], dtype=float)):
return (x[1]-4)**2+2*((0.75-x[0]-1)**2+(x[0]-0-1)**2)
x,iter=newton(x,fxy3);cumulative_iter+=iter
xx[3]=x[0] ; yy[3]=x[1]
print('x=',xx) ; print('y=',yy) ; print('Cumulative Iteration=',cumulative_iter)
x= [-1. -0.5 0.75 0.375 0. ] y= [1. 2. 3. 4. 0.] Cumulative Iteration= 4
def fxy4(x):
return (x[1]-5)**2+2*(0.375-x[0]-1)**2
x,iter=newton(x,fxy4);cumulative_iter+=iter
xx[4]=x[0] ; yy[4]=x[1]
print('x=',xx) ; print('y=',yy) ; print('Cumulative Iteration=',cumulative_iter)
print(np.linalg.norm(x-x_tempt)) ;x_tempt=x
x= [-1. -0.5 0.75 0.375 -0.625] y= [1. 2. 3. 4. 5.] Cumulative Iteration= 5 5.038911092686593
Second parent iteration
def fxy0(x):
return (x[1]-1)**2 + 2*(-0.5-x[0]-1)**2
x,iter=newton(x,fxy0);cumulative_iter+=iter
xx[0]=x[0] ; yy[0]=x[1]
print('x=',xx) ; print('y=',yy) ; print('Cumulative Iteration=',cumulative_iter)
x= [-1.5 -0.5 0.75 0.375 -0.625] y= [1. 2. 3. 4. 5.] Cumulative Iteration= 6
def fxy1(x):
return (x[1]-2)**2 +2*(0.75-x[0]-1)**2+ 2*(x[0]+1.5-1)**2
x,iter=newton(x,fxy1);cumulative_iter+=iter
xx[1]=x[0] ; yy[1]=x[1]
print('x=',xx) ; print('y=',yy) ; print('Cumulative Iteration=',cumulative_iter)
x= [-1.5 -0.375 0.75 0.375 -0.625] y= [1. 2. 3. 4. 5.] Cumulative Iteration= 7
def fxy2(x):
return (x[1]-3)**2+2*((x[0]+0.375-1)**2+(x[0]-0.375-1)**2)
x,iter=newton(x,fxy2);cumulative_iter+=iter
xx[2]=x[0] ; yy[2]=x[1]
print('x=',xx) ; print('y=',yy) ; print('Cumulative Iteration=',cumulative_iter)
x= [-1.5 -0.375 1. 0.375 -0.625] y= [1. 2. 3. 4. 5.] Cumulative Iteration= 8
def fxy3(x,b=np.array([1,2,3,4,5], dtype=float)):
return (x[1]-4)**2+2*((1-x[0]-1)**2+(x[0]+0.625-1)**2)
x,iter=newton(x,fxy3);cumulative_iter+=iter
xx[3]=x[0] ; yy[3]=x[1]
print('x=',xx) ; print('y=',yy) ; print('Cumulative Iteration=',cumulative_iter)
x= [-1.5 -0.375 1. 0.1875 -0.625 ] y= [1. 2. 3. 4. 5.] Cumulative Iteration= 9
def fxy4(x):
return (x[1]-5)**2+2*(0.1875-x[0]-1)**2
x,iter=newton(x,fxy4);cumulative_iter+=iter
xx[4]=x[0] ; yy[4]=x[1]
print('x=',xx) ; print('y=',yy) ; print('Cumulative Iteration=',cumulative_iter)
print(np.linalg.norm(x-x_tempt)) ;x_tempt=x
x= [-1.5 -0.375 1. 0.1875 -0.8125] y= [1. 2. 3. 4. 5.] Cumulative Iteration= 10 0.1875
Third parent iteration
def fxy0(x):
return (x[1]-1)**2 + 2*(-0.375-x[0]-1)**2
x,iter=newton(x,fxy0);cumulative_iter+=iter
xx[0]=x[0] ; yy[0]=x[1]
print('x=',xx) ; print('y=',yy) ; print('Cumulative Iteration=',cumulative_iter)
x= [-1.375 -0.375 1. 0.1875 -0.8125] y= [1. 2. 3. 4. 5.] Cumulative Iteration= 11
def fxy1(x):
return (x[1]-2)**2 +2*(1-x[0]-1)**2+ 2*(x[0]+1.375-1)**2
x,iter=newton(x,fxy1);cumulative_iter+=iter
xx[1]=x[0] ; yy[1]=x[1]
print('x=',xx) ; print('y=',yy) ; print('Cumulative Iteration=',cumulative_iter)
x= [-1.375 -0.1875 1. 0.1875 -0.8125] y= [1. 2. 3. 4. 5.] Cumulative Iteration= 12
def fxy2(x):
return (x[1]-3)**2+2*((x[0]+0.1875-1)**2+(x[0]-0.1875-1)**2)
x,iter=newton(x,fxy2);cumulative_iter+=iter
xx[2]=x[0] ; yy[2]=x[1]
print('x=',xx) ; print('y=',yy) ; print('Cumulative Iteration=',cumulative_iter)
x= [-1.375 -0.1875 1. 0.1875 -0.8125] y= [1. 2. 3. 4. 5.] Cumulative Iteration= 13
def fxy3(x,b=np.array([1,2,3,4,5], dtype=float)):
return (x[1]-4)**2+2*((1-x[0]-1)**2+(x[0]+0.8125-1)**2)
x,iter=newton(x,fxy3);cumulative_iter+=iter
xx[3]=x[0] ; yy[3]=x[1]
print('x=',xx) ; print('y=',yy) ; print('Cumulative Iteration=',cumulative_iter)
x= [-1.375 -0.1875 1. 0.09375 -0.8125 ] y= [1. 2. 3. 4. 5.] Cumulative Iteration= 14
def fxy4(x):
return (x[1]-5)**2+2*(0.09375-x[0]-1)**2
x,iter=newton(x,fxy4);cumulative_iter+=iter
xx[4]=x[0] ; yy[4]=x[1]
print('x=',xx) ; print('y=',yy) ; print('Cumulative Iteration=',cumulative_iter)
print(np.linalg.norm(x-x_tempt)) ;x_tempt=x
x= [-1.375 -0.1875 1. 0.09375 -0.90625] y= [1. 2. 3. 4. 5.] Cumulative Iteration= 15 0.09375
Fourth parent iteration
def fxy0(x):
return (x[1]-1)**2 + 2*(-0.1875- x[0]-1)**2
x,iter=newton(x,fxy0);cumulative_iter+=iter
xx[0]=x[0] ; yy[0]=x[1]
print('x=',xx) ; print('y=',yy) ; print('Cumulative Iteration=',cumulative_iter)
x= [-1.1875 -0.1875 1. 0.09375 -0.90625] y= [1. 2. 3. 4. 5.] Cumulative Iteration= 16
def fxy1(x):
return (x[1]-2)**2 +2*(1-x[0]-1)**2+ 2*(x[0]+1.1875-1)**2
x,iter=newton(x,fxy1);cumulative_iter+=iter
xx[1]=x[0] ; yy[1]=x[1]
print('x=',xx) ; print('y=',yy) ; print('Cumulative Iteration=',cumulative_iter)
x= [-1.1875 -0.09375 1. 0.09375 -0.90625] y= [1. 2. 3. 4. 5.] Cumulative Iteration= 17
def fxy2(x):
return (x[1]-3)**2+2*((x[0]+0.09375-1)**2+(x[0]-0.90375-1)**2)
x,iter=newton(x,fxy2);cumulative_iter+=iter
xx[2]=x[0] ; yy[2]=x[1]
print('x=',xx) ; print('y=',yy) ; print('Cumulative Iteration=',cumulative_iter)
x= [-1.1875 -0.09375 1.405 0.09375 -0.90625] y= [1. 2. 3. 4. 5.] Cumulative Iteration= 18
def fxy3(x,b=np.array([1,2,3,4,5], dtype=float)):
return (x[1]-4)**2+2*((1.405-x[0]-1)**2+(x[0]+0.90625-1)**2)
x,iter=newton(x,fxy3);cumulative_iter+=iter
xx[3]=x[0] ; yy[3]=x[1]
print('x=',xx) ; print('y=',yy) ; print('Cumulative Iteration=',cumulative_iter)
x= [-1.1875 -0.09375 1.405 0.249375 -0.90625 ] y= [1. 2. 3. 4. 5.] Cumulative Iteration= 19
def fxy4(x):
return (x[1]-5)**2+2*(0.249375-x[0]-1)**2
x,iter=newton(x,fxy4);cumulative_iter+=iter
xx[4]=x[0] ; yy[4]=x[1]
print('x=',xx) ; print('y=',yy) ; print('Cumulative Iteration=',cumulative_iter)
print(np.linalg.norm(x-x_tempt)) ;x_tempt=x
x= [-1.1875 -0.09375 1.405 0.249375 -0.750625] y= [1. 2. 3. 4. 5.] Cumulative Iteration= 20 0.155625
Fith parent iteration
def fxy0(x):
return (x[1]-1)**2 + 2*(-0.09375- x[0]-1)**2
x,iter=newton(x,fxy0);cumulative_iter+=iter
xx[0]=x[0] ; yy[0]=x[1]
print('x=',xx) ; print('y=',yy) ; print('Cumulative Iteration=',cumulative_iter)
x= [-1.09375 -0.09375 1.405 0.249375 -0.750625] y= [1. 2. 3. 4. 5.] Cumulative Iteration= 21
def fxy1(x):
return (x[1]-2)**2 +2*(1.405 -x[0]-1)**2+ 2*(x[0]+1.09375-1)**2
x,iter=newton(x,fxy1);cumulative_iter+=iter
xx[1]=x[0] ; yy[1]=x[1]
print('x=',xx) ; print('y=',yy) ; print('Cumulative Iteration=',cumulative_iter)
x= [-1.09375 0.155625 1.405 0.249375 -0.750625] y= [1. 2. 3. 4. 5.] Cumulative Iteration= 22
def fxy2(x):
return (x[1]-3)**2+2*((x[0]-0.155625-1)**2+(x[0]-0.249375-1)**2)
x,iter=newton(x,fxy2);cumulative_iter+=iter
xx[2]=x[0] ; yy[2]=x[1]
print('x=',xx) ; print('y=',yy) ; print('Cumulative Iteration=',cumulative_iter)
x= [-1.09375 0.155625 1.2025 0.249375 -0.750625] y= [1. 2. 3. 4. 5.] Cumulative Iteration= 23
def fxy3(x,b=np.array([1,2,3,4,5], dtype=float)):
return (x[1]-4)**2+2*((1.2025-x[0]-1)**2+(x[0]+0.750625-1)**2)
x,iter=newton(x,fxy3);cumulative_iter+=iter
xx[3]=x[0] ; yy[3]=x[1]
print('x=',xx) ; print('y=',yy) ; print('Cumulative Iteration=',cumulative_iter)
x= [-1.09375 0.155625 1.2025 0.2259375 -0.750625 ] y= [1. 2. 3. 4. 5.] Cumulative Iteration= 24
def fxy4(x):
return (x[1]-5)**2+2*(0.2259375-x[0]-1)**2
x,iter=newton(x,fxy4);cumulative_iter+=iter
xx[4]=x[0] ; yy[4]=x[1]
print('x=',xx) ; print('y=',yy) ; print('Cumulative Iteration=',cumulative_iter)
print(np.linalg.norm(x-x_tempt)) ;x_tempt=x
x= [-1.09375 0.155625 1.2025 0.2259375 -0.7740625] y= [1. 2. 3. 4. 5.] Cumulative Iteration= 25 0.0234375
Sixth parent iteration
def fxy0(x):
return (x[1]-1)**2 + 2*(0.155625- x[0]-1)**2
x,iter=newton(x,fxy0);cumulative_iter+=iter
xx[0]=x[0] ; yy[0]=x[1]
print('x=',xx) ; print('y=',yy) ; print('Cumulative Iteration=',cumulative_iter)
x= [-0.844375 0.155625 1.2025 0.2259375 -0.7740625] y= [1. 2. 3. 4. 5.] Cumulative Iteration= 26
def fxy1(x):
return (x[1]-2)**2 +2*(1.2025-x[0]-1)**2+ 2*(x[0]-0.844375-1)**2
x,iter=newton(x,fxy1);cumulative_iter+=iter
xx[1]=x[0] ; yy[1]=x[1]
print('x=',xx) ; print('y=',yy) ; print('Cumulative Iteration=',cumulative_iter)
x= [-0.844375 1.0234375 1.2025 0.2259375 -0.7740625] y= [1. 2. 3. 4. 5.] Cumulative Iteration= 27
def fxy2(x):
return (x[1]-3)**2+2*((x[0]-1.0234375-1)**2+(x[0]-0.2259375-1)**2)
x,iter=newton(x,fxy2);cumulative_iter+=iter
xx[2]=x[0] ; yy[2]=x[1]
print('x=',xx) ; print('y=',yy) ; print('Cumulative Iteration=',cumulative_iter)
x= [-0.844375 1.0234375 1.6246875 0.2259375 -0.7740625] y= [1. 2. 3. 4. 5.] Cumulative Iteration= 28
def fxy3(x,b=np.array([1,2,3,4,5], dtype=float)):
return (x[1]-4)**2+2*((1.6246875-x[0]-1)**2+(x[0]+0.7740625-1)**2)
x,iter=newton(x,fxy3);cumulative_iter+=iter
xx[3]=x[0] ; yy[3]=x[1]
print('x=',xx) ; print('y=',yy) ; print('Cumulative Iteration=',cumulative_iter)
x= [-0.844375 1.0234375 1.6246875 0.4253125 -0.7740625] y= [1. 2. 3. 4. 5.] Cumulative Iteration= 29
def fxy4(x):
return (x[1]-5)**2+2*(0.4253125-x[0]-1)**2
x,iter=newton(x,fxy4);cumulative_iter+=iter
xx[4]=x[0] ; yy[4]=x[1]
print('x=',xx) ; print('y=',yy) ; print('Cumulative Iteration=',cumulative_iter)
print(np.linalg.norm(x-x_tempt)) ;x_tempt=x
x= [-0.844375 1.0234375 1.6246875 0.4253125 -0.5746875] y= [1. 2. 3. 4. 5.] Cumulative Iteration= 30 0.19937499999999997
Seventh parent iteration
def fxy0(x):
return (x[1]-1)**2 + 2*(0.0234375- x[0]-1)**2
x,iter=newton(x,fxy0);cumulative_iter+=iter
xx[0]=x[0] ; yy[0]=x[1]
print('x=',xx) ; print('y=',yy) ; print('Cumulative Iteration=',cumulative_iter)
x= [-0.9765625 1.0234375 1.6246875 0.4253125 -0.5746875] y= [1. 2. 3. 4. 5.] Cumulative Iteration= 31
def fxy1(x):
return (x[1]-2)**2 +2*(1.203125-x[0]-1)**2+ 2*(x[0]+1.15625-1)**2
x,iter=newton(x,fxy1);cumulative_iter+=iter
xx[1]=x[0] ; yy[1]=x[1]
print('x=',xx) ; print('y=',yy) ; print('Cumulative Iteration=',cumulative_iter)
x= [-0.9765625 0.0234375 1.6246875 0.4253125 -0.5746875] y= [1. 2. 3. 4. 5.] Cumulative Iteration= 32
#x=np.array([-1.1875 , -0.09375, 1. , 0.09375, -0.90625, 1. , 2. , 3. , 4. , 5.])
def fxy2(x):
return (x[1]-3)**2+2*((x[0]-0.0234375-1)**2+(x[0]-0.2265625-1)**2)
x,iter=newton(x,fxy2);cumulative_iter+=iter
xx[2]=x[0] ; yy[2]=x[1]
print('x=',xx) ; print('y=',yy) ; print('Cumulative Iteration=',cumulative_iter)
x= [-0.9765625 0.0234375 1.125 0.4253125 -0.5746875] y= [1. 2. 3. 4. 5.] Cumulative Iteration= 33
def fxy3(x,b=np.array([1,2,3,4,5], dtype=float)):
return (x[1]-4)**2+2*((1.125-x[0]-1)**2+(x[0]+0.7734375-1)**2)
x,iter=newton(x,fxy3);cumulative_iter+=iter
xx[3]=x[0] ; yy[3]=x[1]
print('x=',xx) ; print('y=',yy) ; print('Cumulative Iteration=',cumulative_iter)
x= [-0.9765625 0.0234375 1.125 0.17578125 -0.5746875 ] y= [1. 2. 3. 4. 5.] Cumulative Iteration= 34
def fxy4(x):
return (x[1]-5)**2+2*(0.17578125-x[0]-1)**2
x,iter=newton(x,fxy4);cumulative_iter+=iter
xx[4]=x[0] ; yy[4]=x[1]
print('x=',xx) ; print('y=',yy) ; print('Cumulative Iteration=',cumulative_iter)
print(np.linalg.norm(x-x_tempt)) ;x_tempt=x
x= [-0.9765625 0.0234375 1.125 0.17578125 -0.82421875] y= [1. 2. 3. 4. 5.] Cumulative Iteration= 35 0.24953124999999998
plt.plot([0,4],[5,5],"-.g", label='Expected Route') ; plt.plot([0,4],[4,4],"-.g") ; plt.plot([0,4],[3,3],"-.g") ; plt.plot([0,4],[2,2],"-.g") ; plt.plot([0,4],[1,1],"-.g")
plt.plot(xx+1.5,yy,'r', label='V Formation') ; plt.plot(xx+1.5,yy,'r>',label='Aircarft Position')
plt.plot(xx+2.5,yy,'b', label='V Formation (next)') ; plt.plot(xx+2.5,yy,'b>',label='Aircarft Position (next)')
plt.xlabel('x') ; plt.ylabel('y')
plt.title('Aircraft Position-Example 1-Decentralized Algorithm')
plt.legend()
plt.show()
Example 2¶
5 aircraft which will go in a V formation.
Private objective for each aircraft $i\in{0,1,2,3,4}$ $$f_1(\mathbb{x})=|\mathbb{A}_i\mathbb{x}_i-\mathbb{b}_i|_2^2$$
Common objective for each aircraft $i\in{0,1,2,3,4}$ $$f_2(\mathbb{x})=\sum_{j\in R(i)}|\mathbb{x}_i-\mathbb{x}_j-\Delta \mathbb{x}_{ij}|_2^2$$
with :
- $\mathbb{x}=\begin{bmatrix}x\\y\end{bmatrix}$
- $\mathbb{A}_i=\begin{bmatrix}1&0\\0&-1\end{bmatrix}, \forall i\in{0,1,2,3,4}$
- $\mathbb{b}=[2\ \ 1\ \ 0\ -1\ -2]^T$
- $\Delta \mathbb{x}_{ij}=\begin{bmatrix}1\\1\end{bmatrix}, \forall i\in{0,1,2,3,4}, \forall j\in R(i)$
Minimize : $$f(\mathbb{x})=\sum_{i=0}^4\left[|\mathbb{A}_i\mathbb{x}_i-\mathbb{b}_i|_2^2+\sum_{j\in R(i)}|\mathbb{x}_i-\mathbb{x}_j-\Delta \mathbb{x}_{ij}|_2^2\right]$$
Calculate the position of the formation using several method.
Steepest Descent (example 2)¶
def fxy(x,b=np.array([0,1,2,3,4], dtype=float)):
return (x[0]-x[5]+2)**2+(x[1]-x[6]+1)**2+(x[2]-x[7])**2+(x[3]-x[8]-1)**2+(x[4]-x[9]-2)**2+2*( (x[1]-x[0]-1)**2+(x[2]-x[1]-1)**2+(x[2]-x[3]-1)**2+(x[3]-x[4]-1)**2+(x[6]-x[5]-1)**2+(x[7]-x[6]-1)**2+(x[7]-x[8]-1)**2+(x[8]-x[9]-1)**2 )
print('\n=======Steepest Descent========')
xx=np.array([0,0,0,0,0], dtype=float)
yy=np.array([0,0,0,0,0], dtype=float)
x=np.concatenate((xx,yy), axis=None)
x,iter=steepest_descent(x,fxy,printt=True)
=======Steepest Descent======== Iter = 17 x = [-1.5 -0.2 1.2 0.6 -0.1 -0.1 0.6 1.2 -0.2 -1.5]
xx=x[0:5] ; yy=x[5:10]
plt.plot([-2,3],[-2,3],"-.g") ; plt.plot([-3,2],[-2,3],"-.g") ; plt.plot([-2,2],[0,4],"-.g")
plt.plot([-1,4],[-2,3],"-.g") ; plt.plot([-0,5],[-2,3],"-.g", label='Expected Route')
plt.plot(xx,yy,'r', label='V Formation'); plt.plot(xx,yy,'or',label='Aircarft Position')
plt.plot(xx+1,yy+1,'b', label='V Formation (next)') ; plt.plot(xx+1,yy+1,'ob',label='Aircarft Position (next)')
plt.xlabel('x') ; plt.ylabel('y')
plt.title('Aircraft Position-Example 2')
plt.xlim([-2, 5.5]) ; plt.ylim([-2, 3])
plt.legend(loc ="lower right")
plt.show()
Steepest Descent with Decentralize Algorithm (example 2)¶
Frst iteration
print('\n=======Steepest Descent with Descentralize Algorithm========')
xx=np.array([0,0,0,0,0], dtype=float)
yy=np.array([0,0,0,0,0], dtype=float)
x=np.concatenate((xx[0],yy[0]), axis=None)
x_tempt=x; cumulative_iter=0
def fxy0(x):
return (x[0]-x[1]+2)**2 + 2*(0-x[0]-1)**2 + 2*(0-x[1]-1)**2
x,iter=newton(x,fxy0);cumulative_iter+=iter
xx[0]=x[0] ; yy[0]=x[1]
print('x=',xx) ; print('y=',yy) ; print('Cumulative Iteration=',cumulative_iter)
=======Steepest Descent with Descentralize Algorithm======== x= [-1.5 0. 0. 0. 0. ] y= [-0.5 0. 0. 0. 0. ] Cumulative Iteration= 1
x=np.concatenate((xx[1],yy[1]), axis=None)
def fxy1(x):
return (x[0]-x[1]+1)**2 + 2*( (x[0]+1.5-1)**2 + (0-x[0]-1)**2 ) + 2*((x[1]+0.5-1)**2+(0+x[1]-1)**2)
x,iter=newton(x,fxy0);cumulative_iter+=iter
xx[1]=x[0] ; yy[1]=x[1]
print('x=',xx) ; print('y=',yy) ; print('Cumulative Iteration=',cumulative_iter)
x= [-1.5 -1.5 0. 0. 0. ] y= [-0.5 -0.5 0. 0. 0. ] Cumulative Iteration= 2
x=np.concatenate((xx[2],yy[2]), axis=None)
def fxy2(x):
return (x[0]-x[1])**2 + 2*( (x[0]+3/2-1)**2 + (x[0]-0-1)**2 ) + 2*((x[1]+1/2-1)**2+(x[1]-0-1)**2 )
x,iter=newton(x,fxy2);cumulative_iter+=iter
xx[2]=x[0] ; yy[2]=x[1]
print('x=',xx) ; print('y=',yy) ; print('Cumulative Iteration=',cumulative_iter)
x= [-1.5 -1.5 0.33333333 0. 0. ] y= [-0.5 -0.5 0.66666667 0. 0. ] Cumulative Iteration= 3
x=np.concatenate((xx[3],yy[3]), axis=None)
def fxy3(x):
return (x[0]-x[1]-1)**2 + 2*( (1/3-x[0]-1)**2 + (x[0]-0-1)**2 ) + 2*((2/3-x[1]-1)**2+(x[1]-0-1)**2 )
x,iter=newton(x,fxy3);cumulative_iter+=iter
xx[3]=x[0] ; yy[3]=x[1]
print('x=',xx) ; print('y=',yy) ; print('Cumulative Iteration=',cumulative_iter)
x= [-1.5 -1.5 0.33333333 0.36111111 0. ] y= [-0.5 -0.5 0.66666667 0.13888889 0. ] Cumulative Iteration= 4
x=np.concatenate((xx[4],yy[4]), axis=None)
def fxy4(x):
return (x[0]-x[1]-2)**2 + 2*(0.36111111-x[0]-1)**2 + 2*(0.13888889-x[1]-1)**2
x,iter=newton(x,fxy4);cumulative_iter+=iter
xx[4]=x[0] ; yy[4]=x[1]
print('x=',xx) ; print('y=',yy) ; print('Cumulative Iteration=',cumulative_iter)
print(np.linalg.norm(x-x_tempt)) ;x_tempt=x
x= [-1.5 -1.5 0.33333333 0.36111111 -0.19444445] y= [-0.5 -0.5 0.66666667 0.13888889 -1.30555556] Cumulative Iteration= 5 1.31995604070087
Second iteration
x=np.concatenate((xx[0],yy[0]), axis=None)
def fxy0(x):
return (x[0]-x[1]+2)**2 + 2*(-3/2-x[0]-1)**2 + 2*(-1/2-x[1]-1)**2
x,iter=newton(x,fxy0);cumulative_iter+=iter
xx[0]=x[0] ; yy[0]=x[1]
print('x=',xx) ; print('y=',yy) ; print('Cumulative Iteration=',cumulative_iter)
x= [-2.75 -1.5 0.33333333 0.36111111 -0.19444445] y= [-1.25 -0.5 0.66666667 0.13888889 -1.30555556] Cumulative Iteration= 6
x=np.concatenate((xx[1],yy[1]), axis=None)
def fxy1(x):
return (x[0]-x[1]+1)**2 + 2*( (x[0]+2.75 -1)**2 + (1/3-x[0]-1)**2 ) + 2*((x[1]+1.25 -1)**2+(2/3+x[1]-1)**2)
x,iter=newton(x,fxy1);cumulative_iter+=iter
xx[1]=x[0] ; yy[1]=x[1]
print('x=',xx) ; print('y=',yy) ; print('Cumulative Iteration=',cumulative_iter)
x= [-2.75 -1.16666667 0.33333333 0.36111111 -0.19444445] y= [-1.25000000e+00 1.11022302e-16 6.66666667e-01 1.38888889e-01 -1.30555556e+00] Cumulative Iteration= 7
x=np.concatenate((xx[2],yy[2]), axis=None)
def fxy2(x):
return (x[0]-x[1])**2 + 2*( (x[0]+1.16666667-1)**2 + (x[0]-0.36111111-1)**2 ) + 2*((x[1]-2.22044605e-16-1)**2+(x[1]-1.38888889e-01-1)**2 )
x,iter=newton(x,fxy2);cumulative_iter+=iter
xx[2]=x[0] ; yy[2]=x[1]
print('x=',xx) ; print('y=',yy) ; print('Cumulative Iteration=',cumulative_iter)
x= [-2.75 -1.16666667 0.67592592 0.36111111 -0.19444445] y= [-1.25000000e+00 1.11022302e-16 9.90740740e-01 1.38888889e-01 -1.30555556e+00] Cumulative Iteration= 8
x=np.concatenate((xx[3],yy[3]), axis=None)
def fxy3(x):
return (x[0]-x[1]-1)**2 + 2*( (0.67592592-x[0]-1)**2 + (x[0]-0.19444445-1)**2 ) + 2*((9.90740740e-01-x[1]-1)**2+(x[1]+1.30555555-1)**2 )
x,iter=newton(x,fxy3);cumulative_iter+=iter
xx[3]=x[0] ; yy[3]=x[1]
print('x=',xx) ; print('y=',yy) ; print('Cumulative Iteration=',cumulative_iter)
x= [-2.75 -1.16666667 0.67592592 0.50308642 -0.19444445] y= [-1.25000000e+00 1.11022302e-16 9.90740740e-01 -2.25308640e-01 -1.30555556e+00] Cumulative Iteration= 9
x=np.concatenate((xx[4],yy[4]), axis=None)
def fxy4(x):
return (x[0]-x[1]-2)**2 + 2*(0.50308642-x[0]-1)**2 + 2*(-2.25308640e-01-x[1]-1)**2
x,iter=newton(x,fxy4);cumulative_iter+=iter
xx[4]=x[0] ; yy[4]=x[1]
print('x=',xx) ; print('y=',yy) ; print('Cumulative Iteration=',cumulative_iter)
print(np.linalg.norm(x-x_tempt)) ;x_tempt=x
x= [-2.75 -1.16666667 0.67592592 0.50308642 -0.17901234] y= [-1.25000000e+00 1.11022302e-16 9.90740740e-01 -2.25308640e-01 -1.54320987e+00] Cumulative Iteration= 10 0.23815483519146158
Third iteration
x=np.concatenate((xx[0],yy[0]), axis=None)
def fxy0(x):
return (x[0]-x[1]+2)**2 + 2*(-1.16666667-x[0]-1)**2 + 2*(1.11022302e-16-x[1]-1)**2
x,iter=newton(x,fxy0);cumulative_iter+=iter
xx[0]=x[0] ; yy[0]=x[1]
print('x=',xx) ; print('y=',yy) ; print('Cumulative Iteration=',cumulative_iter)
x= [-2.375 -1.16666667 0.67592592 0.50308642 -0.17901234] y= [-7.91666667e-01 1.11022302e-16 9.90740740e-01 -2.25308640e-01 -1.54320987e+00] Cumulative Iteration= 11
def fxy1(x):
return (x[0]-x[1]+1)**2 + 2*( (x[0]+2.375 -1)**2 + (0.67592592-x[0]-1)**2 ) + 2*((x[1]+7.91666667e-01 -1)**2+(9.90740740e-01+x[1]-1)**2)
x,iter=newton(x,fxy1);cumulative_iter+=iter
xx[1]=x[0] ; yy[1]=x[1]
print('x=',xx) ; print('y=',yy) ; print('Cumulative Iteration=',cumulative_iter)
x= [-2.375 -0.85648148 0.67592592 0.50308642 -0.17901234] y= [-0.79166667 0.11574074 0.99074074 -0.22530864 -1.54320987] Cumulative Iteration= 12
def fxy2(x):
return (x[0]-x[1])**2 + 2*( (x[0]+0.85648148-1)**2 + (x[0]-0.50308642-1)**2 ) + 2*((x[1]-0.11574074-1)**2+(x[1]+0.22530864-1)**2 )
x,iter=newton(x,fxy2);cumulative_iter+=iter
xx[2]=x[0] ; yy[2]=x[1]
print('x=',xx) ; print('y=',yy) ; print('Cumulative Iteration=',cumulative_iter)
x= [-2.375 -0.85648148 0.8436214 0.50308642 -0.17901234] y= [-0.79166667 0.11574074 0.92489712 -0.22530864 -1.54320987] Cumulative Iteration= 13
def fxy3(x):
return (x[0]-x[1]-1)**2 + 2*( (0.8436214 -x[0]-1)**2 + (x[0]+0.17901234-1)**2 ) + 2*((0.92489712-x[1]-1)**2+(x[1]+1.54320987-1)**2 )
x,iter=newton(x,fxy3);cumulative_iter+=iter
xx[3]=x[0] ; yy[3]=x[1]
print('x=',xx) ; print('y=',yy) ; print('Cumulative Iteration=',cumulative_iter)
x= [-2.375 -0.85648148 0.8436214 0.39206105 -0.17901234] y= [-0.79166667 0.11574074 0.92489712 -0.36891289 -1.54320987] Cumulative Iteration= 14
def fxy4(x):
return (x[0]-x[1]-2)**2 + 2*(0.39206105 -x[0]-1)**2 + 2*(-0.36891289-x[1]-1)**2
x,iter=newton(x,fxy4);cumulative_iter+=iter
xx[4]=x[0] ; yy[4]=x[1]
print('x=',xx) ; print('y=',yy) ; print('Cumulative Iteration=',cumulative_iter)
print(np.linalg.norm(x-x_tempt)) ;x_tempt=x
x= [-2.375 -0.85648148 0.8436214 0.39206105 -0.29818243] y= [-0.79166667 0.11574074 0.92489712 -0.36891289 -1.6786694 ] Cumulative Iteration= 15 0.18041838769490495
Fourth iteration
def fxy0(x):
return (x[0]-x[1]+2)**2 + 2*(-0.85648148 -x[0]-1)**2 + 2*(0.11574074-x[1]-1)**2
x,iter=newton(x,fxy0);cumulative_iter+=iter
xx[0]=x[0] ; yy[0]=x[1]
print('x=',xx) ; print('y=',yy) ; print('Cumulative Iteration=',cumulative_iter)
x= [-2.11342593 -0.85648148 0.8436214 0.39206105 -0.29818243] y= [-0.62731482 0.11574074 0.92489712 -0.36891289 -1.6786694 ] Cumulative Iteration= 16