# Modifications to ArduSub That Improve BlueROV SITL Accuracy and Design of Hybrid Autopilot
## Appendix B Calculating Control Gains for a Critically Damped System  
A simple PD control law was carried out via a ROS layer. Given a generic state variable \(\gamma\) containing pose and \(\gamma_{d}\) containing desired pose, the difference between the two  
\[\tilde{\gamma}=\gamma_{d}-\gamma, \tag{10}\]  
is the proportional error. There is also an associated rate over time  
\[\hat{\gamma}=\frac{d\tilde{\gamma}}{dt}, \tag{11}\]  
known as the derivative error. Using the error terms, a corrective force \(\tau\) in the body-fixed frame is calculated by applying proportional gain \(k_{p}\) to \(\tilde{\gamma}\) and derivative gain \(k_{d}\) to \(\hat{\gamma}\),  
\[\tau=k_{p}\tilde{\gamma}+\gamma_{d}\hat{\gamma}. \tag{12}\]  
The full 6 DOF hydrodynamic model relates the system inertia matrix \(M\), added mass matrix \(M_{A}\), and acceleration \(\tilde{\gamma}\), as well as the damping matrix \(B\) and velocity \(\dot{\gamma}\), to produce the net external force  
\[\tau=(M+M_{A})\tilde{\gamma}+B\dot{\gamma}+\tau_{B}. \tag{13}\]  
The additional term \(\tau_{B}\) is forces/torques due to buoyancy that will not effect the PD gain calculation, which is addressed in Section 4. Simplified models for the vehicle parameters are used in which only diagonal elements are nonzero. The system inertia matrix takes the form  
\[M=diag(m,m,m,I_{xx},I_{yy},I_{zz}) \tag{14}\]  
where \(m\) is mass of the vehicle and \(I_{jj}\) is the moment of inertia about the j-axis. The added mass matrix also takes a simplified diagonal form  
\[M_{A}=diag(X_{\dot{\alpha}},Y_{\dot{\alpha}},Z_{\dot{\alpha}},K_{p},M_{\dot{ \alpha}},N_{t}). \tag{15}\]  
The damping matrix consists of  
\[B=B_{L}+B_{NL} \tag{16}\]  
where the linear terms are grouped in  
\[B_{L}=diag(X_{\dot{\alpha}},Y_{\dot{\alpha}},Z_{\dot{\alpha}},K_{p},M_{\dot{ \alpha}},N_{r}) \tag{17}\]  
and the nonlinear terms are grouped in  
\[B_{NL}=diag(X_{u|u|}|u|,Y_{\dot{\alpha}|\dot{\alpha}|}|v|,Z_{\dot{\alpha}|\dot {\alpha}|\dot{\alpha}|}|w|,K_{p|p|}|p|,M_{q|q|}|q|,N_{\dot{\alpha}|\dot{\alpha} |}|r|). \tag{18}\]  
The nonlinear terms present in Equation (18) are functions of the absolute values of the corresponding velocities. To proceed with calculating control gains, it is necessary to linearize the terms in \(B\); therefore, a new matrix  
\[\tilde{B}=diag(\tilde{X}_{\dot{\alpha}},\tilde{Y}_{\dot{\alpha}},\tilde{Z}_{ \dot{\alpha}},\tilde{K}_{p},\tilde{M}_{\dot{\alpha}},\tilde{N}_{r}), \tag{19}\]  
consisting of characteristic drag terms linearized with respect to expected operating conditions is introduced. In the following Section 5.1, the determination of the characteristic linear yaw drag term \(\tilde{N}_{r}\) used to calculate control gains is detailed, and the remaining degrees of freedom are calculated with a similar process. A feed-forward term appended to Equation (A5) that cancels out the \(\tau_{B}\) in Equation (A6) shall be discussed in the immediately following subsection, for the yaw axis \(\tau_{B}\) is non-existent. Substituting Equations (A3) and (A4) into Equation (A5), setting this equal to Equation (A6), and grouping like terms together the resulting equation is obtained  
\[\hat{\gamma}=\frac{d\tilde{\gamma}}{dt}, \tag{11}\]0 (A13)  
where the left-hand side is a particular function \(f(t)\) and the right-hand side is a second-order system of the standard form \(f(t)=k\mathbf{x}+d\mathbf{\dot{x}}+m\mathbf{\ddot{x}}\). This makes the parameters  
\[\hat{\gamma}=\frac{d\tilde{\gamma}}{dt}, \tag{11}\]1 (A14)  
The natural frequency of a second-order system is \(\omega_{n}=\sqrt{k/m}\), which can be reinterpreted to construct the diagonal matrix of closed-loop natural frequencies with  
\[\hat{\gamma}=\frac{d\tilde{\gamma}}{dt}, \tag{11}\]2 (A15)  
In this paper, the notation \((\cdot)_{ii}\) refers to the i-th element of a diagonal matrix.