# Modifications to ArduSub That Improve BlueROV SITL Accuracy and Design of Hybrid Autopilot
## 5 Controller Tuning Methodology  
In this section, the procedure followed for tuning one degree of freedom, specifically the yaw direction, shall be reviewed. The equation for control forces in the yaw direction is  
\[N_{C}=k_{p}(\omega_{d})[\psi_{d}-\psi]+k_{d}(\omega_{d})[\dot{\psi}_{d}-\dot{ \psi}], \tag{21}\]  
which is the third element of the vector represented in Equation (18). Notice that there are no buoyant torques in the yaw direction, which simplifies calculations. In the case of control forces in the other directions, an additional feed-forward term would appear in \(K_{C}\) and \(M_{C}\), as seen in Equation (20).  
To give a high-level overview of how all the parts covered previously go together, Figure 11 shows a block diagram of the information flow throughout the portion of the hybrid autopilot that controls angular DOF. \"Pilot Input\" during the tests discussed in this section originated from prerecorded csvs outputting desired angular states \(\mathbf{\eta}_{\mathbf{2d}}\). These desired states are input into the \"Control Law\", which uses gains calculated in Appendix B and produces the desired angular body torques \(\mathbf{\tau}_{\mathbf{46}}\). These torques are parsed with the \"Thruster Allocation\" block, implementing logic described in Section 2.2.1 and Algorithm 2 to determine thruster forces \(\mathbf{m}\) that achieve the desired body torques. The collective effect of the eight thruster forces drive the \"State/SITL\" block, outputting the angular orientations \(\mathbf{\eta}_{\mathbf{2}}\) of the actual/virtual vehicle, respectively. Ultimately, the angular states are fed back to the \"Control Law\" block, establishing the closed loop of the angular controller.  
In Appendix B, the BROV2 is approximated as an LTI system in order to calculate control gains as a function of desired closed-loop natural frequency \(\omega_{d}\), but in actuality drag behaves nonlinearly, scaling quadratically with velocity as explained in Section 2.2. Therefore, an effective linear drag value \(\tilde{N}_{r}\) was determined with trim conditions, which was used to set the linear PD controller. The step response of the nonlinear system under Equation (21) was tested in SITL to verify behavior similar to a critically damped system. It was observed that the step response exhibited underdamped behavior, where the level of oscillation was inversely proportional to \(\omega_{d}^{2}\), so the controller was modified to include an additional damping term inversely proportional to \(\omega_{d}^{2}\). With the additional damping term, the step response achieved critically or close-to-critically damped behavior for a wide range of desired closed-loop natural frequencies. Using the yaw PD control law tuned in SITL, the closed-loop step response was determined through water tank testing,  
Figure 11: A block diagram of the information flow throughout the closed-loop portion of the hybrid autopilot that controls the angular degrees of freedom; arrows are labeled with the important vectors that pass between the nodes.  
demonstrating agreement with simulation results. Following the step response tuning, a series of frequency response tests documented in Appendix C were performed in SITL for low (\(\omega_{d}=2.0\,\mathrm{rad/s}\)), medium (\(\omega_{d}=3.0\,\mathrm{rad/s}\)), and high (\(\omega_{d}=4.0\,\mathrm{rad/s}\)) desired closed-loop natural frequency settings to produce a Bode plot. The final sequence of testing generalized the yaw control law across all angular degrees of freedom and activated the open-loop linear controls to test the performance of the total hybrid autopilot in SITL given a simulated dive involving all six degrees of freedom.