# Quadrotor Formation Strategies Based on Distributed Consensus and Model Predictive Controls
## 5 Formation Strategies
### Formation Strategy: Follower
#### 5.2.1 Formation Strategy in the Horizontal Motion  
In the horizontal motion, the formation strategy is carried out by the consensus control and MPC. As mentioned in Section 2, the desired formation trajectories possess only the information of the desired position for the followers over the time horizon. Therefore, an algorithm of the finite difference method, the block FDM Algorithm in Figure 5, is added to the formation strategy in the horizontal motion to generate the required information for the consensus control. Consider point \(\zeta_{i}\) on given function \(f(\zeta)\), the first to third derivative of \(\zeta_{i}\) given by the finite difference method are shown as follows:  
\[ \begin{array}{rcl}f^{\prime}(\zeta_{i})&=&\frac{-f(\zeta_{i+2})+8f(\zeta_{i+1 })-8f(\zeta_{i-1})+f(\zeta_{i-2})}{12h},\\ f^{\prime\prime}(\zeta_{i})&=&\frac{-f(\zeta_{i+2})+16f(\zeta_{i+1})-30f(\zeta_ {i})+16f(\zeta_{i-1})-f(\zeta_{i-2})}{12h^{2}},\\ f^{\prime\prime\prime}(\zeta_{i})&=&\frac{-f(\zeta_{i+3})+8f(\zeta_{i+2})-13f( \zeta_{i+1})+13f(\zeta_{i-1})-8f(\zeta_{i-2})+f(\zeta_{i-3})}{8h^{3}},\end{array}  \tag{40}\]  
where \(h\) is the time interval between each point. As shown in Equation (40), computing the third derivative of \(f(\zeta)\) at \(\zeta_{i}\) requires the points three steps prior to, and three steps after, \(\zeta_{i}\). This would be a problem when dealing with the first three points and the last three points of the desired formation trajectory, since we don't have knowledge of the points prior to the first point and after the last point of the desired formation trajectory. However, these points can be acquired by applying linear extrapolation backward in time to the first point and forward in time to the last point of the desired formation trajectory. Once the complete desired trajectory is generated, consensus control engages. The consensus control algorithm computes the flight paths of the followers that track the corresponding desired formation trajectories using the control input given in Equation (15). Here, we rearrange the states at time step \(k\) of the i-th follower as follows:  
\[ \begin{array}{l}\mathbf{s}_{i}^{(1)}(k)=\left[\begin{array}{c}\Delta x(k)\\ \Delta y(k)\end{array}\right],\quad\mathbf{s}_{i}^{(2)}(k)=\left[\begin{array}{c} \Delta u(k)\\ \Delta v(k)\end{array}\right],\quad\mathbf{s}_{i}^{(3)}(k)=\left[\begin{array}{c} \Delta\theta(k)\\ \Delta\phi(k)\end{array}\right],\quad\mathbf{s}_{i}^{(4)}(k)=\left[\begin{array}[ ]{c}\Delta q(k)\\ \Delta p(k)\end{array}\right].\end{array}  \tag{41}\]  
The corresponding desired formation trajectory of each modified state \(\hat{s}_{i}^{(ks)}(k)\)\((ks=1,2,3,4)\) at time step \(k\) is defined as \(\mathbf{d}_{i}^{(1)}(k)\), \(\mathbf{d}_{i}^{(2)}(k)\), \(\mathbf{d}_{i}^{(3)}(k)\) and \(\mathbf{d}_{i}^{(4)}(k)\). The error between the modified state and the corresponding desired formation trajectory at time step \(k\) can be written as:  
\[\hat{\mathbf{s}}_{i}^{(ks)}(k)=\mathbf{s}_{i}^{(ks)}(k)-\mathbf{d}_{i}^{(ks)}(k),\quad k =1,2,3,4. \tag{42}\]  
Figure 5: Follower’s control strategy. The consensus control input at time step \(k\) can be computed by using the modified states and the control gains \(\beta_{ks}\) (\(ks=1,2,3,4\)). The control input will result in a two-dimensional column matrix. The first and second elements of the control input are the pitch and roll control commands, respectively. The next step is to apply the consensus control input to the discrete state-space model of (13). Repeating the steps above until the flight path over the predicted time horizon is obtained. After the flight path of the follower under consideration is generated, it then serves as the reference trajectory for the MPC. The MPC obtains the optimal control sequence using Equation (29). First, rearrange the initial condition and the reference trajectory in the form of \(\bar{x}\) described in Equation (27).