# Modifications to ArduSub That Improve BlueROV SITL Accuracy and Design of Hybrid Autopilot
## 4 Designing Autopilot Performance  
Appendix B provides a methodology for calculating the gains of a critically damped closed-loop linear system. In this section, the specifications of the PD control law of this particular ROV are described. Because the proposed error feedback control only applies to angular states, the generic variable state variables \(\gamma_{d}\) and \(\gamma\) used to derive control gains in Appendix B are replaced with the specific state vectors \(\eta_{2d}\) and \(\eta_{2}\). Desired angular states are designated in the inertial frame and passed into the autopilot as \(\eta_{2d}\). The rate of change of the desired state \(\dot{\eta}_{2d}\) is calculated as a moving average from \(\eta_{2d}\), because the pilot input for the desired state is provided in real time rather than as a pre-defined trajectory. The FMU tracks the vehicle's velocity in the body frame \(\
u_{2}\) and this is transformed into the inertial frame with the transformation matrix  
\[\mathbf{J}_{2,\eta_{2}}=\begin{pmatrix}1&\sin(\phi)\tan(\theta)&\cos(\phi)\tan( \theta)\\ 0&\cos(\phi)&-\sin(\phi)\\ 0&\sin(\phi)/\cos(\theta)&\cos(\phi)/\cos(\theta)\end{pmatrix}, \tag{15}\]  
by left multiplying to result in  
\[\dot{\eta}_{2}=\mathbf{J}_{2,\eta_{2}}\
u_{2}. \tag{16}\]  
These velocity state vectors are compared to determine the derivative error  
\[\dot{\mathbf{\eta}}_{2}=\dot{\eta}_{2d}-\dot{\eta}_{2}. \tag{17}\]  
Proportional gains \(\mathbf{k}_{p}\) and derivative gains \(\mathbf{k}_{d}\) are calculated in Equation (A17) and Equation (A18), respectively, as diagonal matrices and are applied to the errors through the control law as seen in Equation (A5). For the proposed autopilot for AR pilots, only angular torques are generated; hence, the specific control law is  
\[\mathbf{\tau}_{4:6}=\mathbf{k}_{p}\mathbf{J}_{2,\eta_{2}}^{-1}\ddot{\eta}_{2}+\mathbf{k}_{d} \mathbf{J}_{2,\eta_{2}}^{-1}\dot{\mathbf{\eta}}_{2}+\left(\mathbf{r}_{\mathbf{B}}\times\mathbf{J}_{ 1,\eta_{2d}}^{-1}\mathbf{F}_{\mathbf{B}}\right), \tag{18}\]  
where the inertial-to-body frame transformation matrix of angular components in the current angular state is  
\[\mathbf{J}_{2,\eta_{2}}^{-1}=\begin{pmatrix}1&0&-\sin(\theta)\\ 0&\cos(\phi)&\sin(\phi)\cos(\theta)\\ 0&-\sin(\phi)&\cos(\phi)\cos(\theta)\end{pmatrix}, \tag{19}\]  
where \(\mathbf{r}_{\mathbf{B}}\) is the constant vector from the center of gravity to the center of buoyancy in the body frame, \(\mathbf{F}_{\mathbf{B}}\) is the constant buoyancy force of the completely submerged vehicle in the inertial frame, and \(\mathbf{J}_{1,\eta_{2d}}^{-1}\) is the inertial-to-body frame transformation matrix for linear components at the _desired_ angular state (not shown here in its entirety due to space limitations, please see [32] for the full form).  
The cross-product term in Equation (18) not present in Equation (A5) is a feed-forward term that accounts for the hydrostatic righting moment \(\tau_{\mathbf{b},\mathbf{4:6}}\). Without the feed-forward term, the angular control law converges to a steady-state with an offset error resulting from the unaccounted for torque bias. In Appendix B, it is claimed that buoyancy effects can be ignored during calculations of PD gains. This is allowable because when the desired state is achieved, i.e., \(\mathbf{\eta}_{2}=\mathbf{\eta}_{2d}\), the feed-forward term exactly cancels out the hydrostatic righting moment such that  
\[\mathbf{\tau}_{\mathbf{b},\mathbf{4:6}}=\left(\mathbf{r}_{\mathbf{B}}\times\mathbf{J}_{1,\eta_{2}}^{-1} \mathbf{F}_{\mathbf{B}}\right)=\left(\mathbf{r}_{\mathbf{B}}\times\mathbf{J}_{1,\eta_{2d}}^{-1}\bm {F}_{\mathbf{B}}\right). \tag{20}\]Thus, when equating the control forces from Equation (18) with the hydrodynamic model Equation (A6) while calculating gains, the formula reduces to the same form as Equation (A13), meaning that the calculations that follow in Equations (A17) and (A18) are unchanged.