Drone Flight - Coupled Stochastic Lorenz

turtles-own [
  aX ; drift factor for Lorenz Coupled SDE for the X vector value
  aY ; drift factor for Lorenz Coupled SDE for the Y vector value
  aZ ; drift factor for Lorenx Coupled SDE for the Z vector value
  X ; X position
  Y ; Y position
  Z ; Z position

  ; Auxiliary variables for SDE calculation:
  k1x
  k1y
  k1z
  k2x
  k2y
  k2z
  Xa
  Ya
  Za
  dWx
  dWy
  dWz
  Sx
  Sy
  Sz

]

to setup
  ca
  reset-ticks
  crt ndrones
  
  let x0 random-float 20 - 10
  let y0 random-float 20 - 10
  let z0 random-float 20
  
  
  
  ask turtles [
    set X x0 + 2 * (1 - random-float 1.000)
    set Y x0 + 2 * (1 - random-float 1.000)
    set Z z0 + 2 * (1 - random-float 1.000) ]
end 

to go
  ; Calculate the nonlinear coupled SDE
  ask turtles[update-drift]
  ask turtles[update-k1]
  ask turtles[get-auxiliary]
  ask turtles[update-new-drift]
  ask turtles[update-displacement]
  
  ; Update the position
  update-position
end 

to update-position
  ask turtles [ if draw_trajectory [pen-down]
                facexyz X Y Z
                setxyz X Y Z ]
end 

;;;;;;;;;;;;;;;;;;;;;;
;;; SDE Procedures ;;;
;;;;;;;;;;;;;;;;;;;;;;

; The initial drift is updated in accordance with the Lorenz equations 
; with global mean field coupling

to update-drift  
  set aX (1 - coupling) * (sigma * (Y - X)) + coupling * mean [X] of turtles
  set aY (1 - coupling) * (rho * X - Y - X * Z) + coupling * mean [Y] of turtles
  set aZ (1 - coupling) * (X * Y - beta * Z) + coupling * mean [Z] of turtles  
end 

; First update of the k1 component for each field mode following
; Roberts' (2012) modified Runge-Kutta algorithm using the output
;from the first drift update obtained from the Lorenz system's equations

to update-k1
  ; Sx, Sy and Sz are randombly selected between -1 and 1 with equal
  ; probability, this is used in Itô calculus
  set Sx get_S
  set Sy get_S
  set Sz get_S
  ; The Wiener dW terms are selected for each field component each
  ; dW component is independently selected with Gaussian distribution with a
  ; zero mean and standard deviation given by the square root of the integration step dt
  set dWx (sqrt dt) * random-normal 0 1
  set dWy (sqrt dt) * random-normal 0 1
  set dWz (sqrt dt) * random-normal 0 1

  ; The k1 component of the algorithm is obtained using the three
  ; inputs as per Robert's (2012) algorithm
  set k1x get_k1 aX Sx dWx
  set k1y get_k1 aY Sy dWy
  set k1z get_k1 aZ Sz dWz
end 

; Auxliliary variables used for the computation of the displaced values for X, Y and Z
; displaces as X + k1x, Y + k1y and Z + k1z, this is a necessary step for the calculation of
; k2 which requires the calculation of the new drift using the displaced coordinates

to get-auxiliary
  set Xa X + k1x
  set Ya Y + k1y
  set Za Z + k1z
end 

; The new drift is now calculated by using the displaced coordinates using the Lorenz
; system's equations

to update-new-drift
  set aX (1 - coupling) * (sigma * (Ya - Xa)) + coupling * mean [Xa] of turtles
  set aY (1 - coupling) * (rho * Xa - Ya - Xa * Za) + coupling * mean [Ya] of turtles
  set aZ (1 - coupling) * (Xa * Ya - beta * Za) + coupling * mean [Za] of turtles
end 


; The new field value variables are obtained by calculating the displacement

to update-displacement
  ; First calculate k2 using the new drift
  set k2x get_k2 aX Sx dWx
  set k2y get_k2 aY Sy dWy
  set k2z get_k2 aZ Sz dWz

  ; The SDE procedure calculates the displacement is calculated using k1 and k2
  let dX_value SDE k1x k2x
  let dY_value SDE k1y k2y
  let dZ_value SDE k1z k2z

  ; The new variables are calculated
  set X X + dX_value
  set Y Y + dY_value
  set Z Z + dZ_value
end 

; The auxiliary variable S is calculated with equal probabilities between
; -1 and 1 for the numeric integration to approximate Itô integral

to-report get_S
  let S 0
  ifelse random-float 1 < 0.5  [set S -1] [set S 1]
  report S
end 

; k1 is given by the drift a which is multiplied by dt and corresponds
; in our case to the Lorenz system's equations
; and it is added by a term that depends upon the stochastic component
; following Robert's scheme we have b which in our case is a parameter that
; controls the noise level multiplied by the the Wiener innovation dW subtracted
; by S multiplied by the square root of the time step is

to-report get_k1 [a S dW]
  let k1 dt * a + b * (dW - S * sqrt(dt))
  report k1
end 

; k2 is calculated in the same way as k1 with the exception that it uses the
; displaced drift and S is multiplied by the square root of dt.

to-report get_k2 [a S dW]
  let k2 dt * a + b * (dW + S * sqrt(dt))
  report k2
end 

; The displacement for the SDE is obtained by taking the mean value of k1 and k2

to-report SDE [k1 k2]
  let displacement 0.5 * (k1 + k2)
  report displacement
end