'V' Formation Aircraft¶

https://www.ridwanreza.com/2022/05/v-formation-aircraft.html

In [1]:

import autograd.numpy as np
import math
from autograd import grad, jacobian, hessian
import matplotlib.pyplot as plt

In [2]:

# 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

In [3]:

#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.

In [4]:

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.

In [5]:

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. ]

In [6]:

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

In [7]:

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

In [8]:

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

In [9]:

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

In [10]:

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

In [11]:

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

In [12]:

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

In [13]:

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

In [14]:

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

In [15]:

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

In [16]:

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

In [17]:

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

In [18]:

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

In [19]:

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

In [20]:

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

In [21]:

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

In [22]:

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

In [23]:

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

In [24]:

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

In [25]:

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

In [26]:

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

In [27]:

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

In [28]:

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

In [29]:

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

In [30]:

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

In [31]:

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

In [32]:

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

In [33]:

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

In [34]:

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

In [35]:

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

In [36]:

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

In [37]:

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

In [38]:

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

In [39]:

#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

In [40]:

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

In [41]:

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

In [42]:

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)¶

In [43]:

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 )

In [44]:

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]

In [45]:

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

In [46]:

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

In [47]:

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

In [48]:

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

In [49]:

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

In [50]:

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

In [51]:

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

In [52]:

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

In [53]:

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

In [54]:

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

In [55]:

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

In [56]:

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

In [57]:

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

In [58]:

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

In [59]:

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

In [60]:

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

In [61]:

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

In [62]:

<