## 3 LQ Controller
### Control: Closed Loop System  
The longitudinal dynamics of the follower vehicles are controlled in a closed loop using the LQC technique. The latter is based on the minimization of a quadratic functional and has the advantage of ensuring the asymptotic stability of the controlled system [23]. Since the LQC control law is a linear combination of all the states \(\{\delta e\}\), the equations are rearranged to have the errors of inter-vehicle distance \(\delta e_{d_{i,i+1}}\) and vehicle velocity \(\delta e_{v,i}\) as states:  
\[\{\delta e\}=\{\delta z\}-\left\{\delta z_{ref}\right\} \tag{16}\]  
where \(\left\{\delta z_{ref}\right\}\) is the reference vector of the desired speeds and desired inter-vehicle distances (normalised w.r.t. the nominal values and variational). Moreover, an augmented state \(\eta_{i}\), that is, the integral of the distance error \(\left(\dot{\eta}_{i}=\delta e_{d_{i,i+1}}\right)\), is added as the last element of the state vector to include an integral term in the full-state feedback control law, compensating for the steady state errors.  
Therefore, the equations are entirely re-written by substituting the expression (19) in (15), leading to:  
\[\left\{\delta\dot{e}\right\}=[A]\{\delta e\}+[B]\{\delta u\}+[A]\left\{\delta z _{ref}\right\}-\left\{\delta\dot{z}_{ref}\right\}+[H]\delta p+[C]\delta z_{1,v} \tag{17}\]  
The control law \(\{\delta u\}\) is here defined as:  
\[\{\delta u\}=\{\delta u_{FB}\}+\{\delta u_{FF}\} \tag{18}\]  
where \(\delta u_{FB}\) represents the feedback control term, whereas \(\delta u_{FF}\) represents the feedforward control term. The feedback control law is full-state feedback:  
\[\{\delta u_{FB}\}=-[L]\{\delta e\} \tag{19}\]  
where \([L]\) is the control gain matrix, computed by solving the steady-state Riccati equation. This solution guarantees the minimization of the cost function \(J(\{\delta u\})\), defined by the LQC problem:  
\[J(\{\delta u\})=\int_{0}^{\infty}\Big{(}\{\delta e\}^{T}[Q]\{\delta e\}+\{ \delta u\}^{T}[R]\{\delta u\}\Big{)}dt \tag{20}\]  
where \([Q]\) and \([R]\) are diagonal weight matrices that are tuned based on the desired system performance. More specifically, \([Q]\) controls the deviation of the states from equilibrium, whereas \([R]\) controls the input effort. In the next section, the tuning process for the matrices will be presented.  
The feedforward contribution \(\delta u_{FF}\) is designed to compensate for the terms of Equation (16) that are not a linear combination of the states:  
\[\{\delta u_{FF}\}=[B]^{+}\Big{(}-[A]\Big{\{}\delta z_{ref}\Big{\}}+\Big{\{} \delta\dot{z}_{ref}\Big{\}}-[H]\delta p-[C]\delta z_{1,p}\Big{)} \tag{21}\]  
Since the input matrix \([B]\) is not square and hence not invertible, \([B]^{+}\) indicates the pseudoinverse (or generalised inverse) matrix, computed with the Moore-Penrose method [25]. The existence and uniqueness of the pseudoinverse of the \([B]\) matrix have been verified by checking the four Penrose conditions applied to the \([B]\) matrix (see Appendix B for more details). Figure 4 shows the block diagram of the controlled platooning system. On the left are the input/disturbances applied to the dynamic system, i.e., the road slope, the velocity of the lead vehicle, and the reference speed and distance of each vehicle. The plant with the two controllers (feedback and feedforward) is shown in the central part and on the right of the figure.