Первые три уравнения -- законы поступательного движения (вторые законы Ньютона). Последние три уравнения -- законы вращательного движения.
где
Current coordinates: x, y, z
Desired coordinates: xd, yd, zd
Current angles: φ, θ, ψ
Let desired angles be zeros: φd = θd = ψd = 0
genotype = {KxP, KyP, KzP, KφP, KθP, KψP, KxD, KyD, KzD, KφD, KθD, KψD, KxDD, KyDD, KzDD}
|x - x_a| + |y - y_a| + |z - z_a| -> min
x_a(t) = kt if t <= T else kT
where k = x_d / T, T -- flight time