# Modifications to ArduSub That Improve BlueROV SITL Accuracy and Design of Hybrid Autopilot
## Appendix C Frequency Response Testing  
After achieving effectively critically damped step responses, the next sequence of testing for characterizing the performance of the yaw PD control law was closed-loop frequency response testing with SITL. Observations were made of the vehicle's ability to follow a \(\pm 30^{\circ}\) desired yaw heading sine wave. A set of desired frequency yaw trajectories were generated from \(0.05\,\mathrm{Hz}\) to \(1.97\,\mathrm{Hz}\) in \(0.04\,\mathrm{Hz}\) increments and saved as csv files. These trajectories were fed into the ROS csv_reader node which read the csv line-by-line in sync with the simulation such that the system had no knowledge of future headings (similar to the AR pilot commands). A separate ROS control_law node monitored vehicle state and calculated the force outputs according to the modified PD control law as described in Section 4 with the addition of \(\delta k_{d}\), as discussed in the previous section. The resulting headings were recorded to csv files with a ROS logger node. Three desired closed-loop natural frequency settings were tested: \(\omega_{d}=\{2.0,3.0,4.0\}\,\mathrm{rad/s}\).  
These simulation results map the nonlinear frequency response \(H_{NL}\) using the tuned yaw controller of Equation (24) with \(\kappa=20.0\). The ratio of the amplitude of the output (actual) to input (desired) is known as the gain \(|H|\) or amplitudes of the system. Amplitudes were calculated by taking half the difference of the maximum and minimum heading from respective output and input data, and the gain is shown on the Bode gain plot. The amount that the output lags behind the control input is known as the phase shift. To quantify phase shift, a peak finding function was used on the respective output and input data to store the times of local maxima into vectors. This analysis was performed for the three \(\omega_{d}\) and displayed in Figure 11, with the frequency presented in units of rad/s.  
The theoretical curves for the linear transfer function \(H_{\mathrm{L}}\) shown on the Bode plot were calculated using the same transfer function as Equation (22), solving for the frequency response by replacing \(s:=j\omega\), where \(j\) is an imaginary number and \(\omega\) is frequency, which can be expressed as  
\[|H_{\mathrm{L}}|=\left|\frac{\Psi(j\omega)}{\overline{\Psi}_{d}(j\omega)} \right|. \tag{19}\]  
The theoretical phase shift is the angle of Equation (22) in the complex plane, which can be expressed as  
\[\angle H_{\mathrm{L}}=\arctan\biggl{(}\frac{Im(H_{\mathrm{L}})}{Re(H_{\mathrm{ L}})}\biggr{)}, \tag{20}\]  
which is also a function of \(\omega\).  
One common metric of assessing a closed-loop controller's response is characterizing attenuation of the frequency response with control bandwidth. The definition given by [32] states that the \(-3\,\mathrm{dB}\) cutoff is  
\[|H(j\omega)|_{\omega=\omega_{b}}=\frac{\sqrt{2}}{2} \tag{21}\]  
where \(|H(j\omega)|\) is the gain of the frequency response of the transfer function, \(\omega\) is the frequency of the desired heading, \(\omega_{n}\) is the closed-loop natural frequency which is inter changeable with \(\omega_{d}\) as per Equation (A15), and \(\omega_{b}\) is the control bandwidth which marks the highest frequency the system can keep up with before the gain falls below \(-3\,\mathrm{dB}\). In other words, the frequency \(\omega\) where the gain falls to \(|H|=\sqrt{2}/2\approx 0.71\) is the so-called control bandwidth \(\omega_{b}\). Looking at Figure A1, in the region spanning \(0.71\leq|H|\leq 1.0\), the slope is constant for all three tested \(\omega_{d}\) settings, causing an apparent linear relationship. Furthermore, unity gains \(|H|=1.0\) are reached at frequencies \(\omega=\omega_{d}\), which could serve as a custom definition of \"bandwidths\" for this particular nonlinear system, which could assist pilots in understanding how the choice of \(\omega_{d}\) can be expected to facilitate BROV2 performance.