**2.1. System dynamics**

The equations of motion used to simulate the dynamic behavior of the autonomous submersible vehicle in a horizontal plane are listed in Eqs. (1)–(4). All variables in these equations are assumed to be in nondimensional form with respect to the vehicle length (7.3<sup>0</sup> ) and constant forward speed (�3 ft./s). The vehicle weighs 435 lbs. and is neutrally buoyant. Time is nondimensionalized such that 1 s represents the time it takes to travel one vehicle length (**Figure 2**).

**Figure 2.** Vehicle geometry and reference axes: (a) *Phoenix* in open ocean [1] and (b) vehicle geometry and reference axis.

Autonomous Underwater Vehicle Guidance, Navigation, and Control http://dx.doi.org/10.5772/intechopen.80316 5

$$(m - Y\_{\circ})\circ - (Y\_{\circ} - m\mathbf{x}\_{G})\circ = Y\_{\circ}\nu + (Y\_{r} - m)r + Y\_{\delta\_{\circ}}\delta\_{\circ} + Y\_{\delta\_{\circ}}\delta\_{\circ} \tag{1}$$

$$(m\mathbf{x}\_{\rm G} - \mathbf{N}\_{\dot{r}})\dot{\mathbf{v}} - (\mathbf{N}\_{\dot{r}} - I\_z)\dot{r} = \mathbf{N}\_{\dot{v}}\mathbf{v} + (\mathbf{N}\_{\dot{r}} - m\mathbf{x}\_{\rm G})\mathbf{r} + \mathbf{N}\_{\dot{\delta}\_b}\boldsymbol{\delta}\_s + \mathbf{N}\_{\delta\_b}\boldsymbol{\delta}\_b\tag{2}$$

$$
\dot{\psi} = r \tag{3}
$$

$$
\dot{y} = \sin\psi + \nu\cos\psi \tag{4}
$$

In addition to the following dependent equation *x* ¼ *cosψ* � *νsinψ* (5)

where


The constant definitions in the mass *m*, mass moment of inertia with respect to a vertical axis that passes through the vehicle's geometric center (amidships) *Iz*, position of the vehicle's center of gravity (measured positive forward of amidships) *xG*, with the remaining terms referred to as the hydrodynamic coefficients. These constants are all presented in nondimensional form.

Defining the state vector f g*x* � f g *ν r ψ y <sup>T</sup>* and the control f g*<sup>u</sup>* � f g *<sup>δ</sup><sup>s</sup> <sup>δ</sup><sup>b</sup> <sup>T</sup>* and assuming small angles, the dynamics expressed in Eqs. (1)–(4) may be expressed in state space form as f*x*g ¼ ½ � *A* f g*x* þ ½ � *B* ½ � *u* where

$$[A] = \begin{bmatrix} -1.4776 & -0.3083 & 0 & 0\\ -1.8673 & -1.2682 & 0 & 0\\ 0 & 1 & 0 & 0\\ 1 & 0 & 1 & 0 \end{bmatrix} \quad [B] = \begin{bmatrix} 0.2271 & 0.1454\\ -1.9159 & 1.2112\\ 0 & 0\\ 0 & 0 \end{bmatrix} \tag{6}$$

The system may also be expressed in a transfer function ratio of outputs divided by inputs in Laplace form using Eq. (7) where observer matrix [*C*] is merely a proper identity matrix to this point of the manuscript. Eq. (7) yields two transfer function relationships between each of the two possible rudder inputs as seen in Eqs. (8) and (9). Notice that both transfer functions have poles and zeros at the origin, while pole-zero cancelation is possible in the case of the stern rudder. On the other hand, even after pole-zero cancelation in the bow rudder Eq. (9), there remains an open loop pole at the origin that must be dealt with during control design, since it represents a potentially unstable element (at the very least, in the instance where the estimated constants are exactly correct, and these equations of motion exactly describe the system, an oscillatory element exists that will not decay). Nonetheless, the dynamics accord to nature. Consider trying to steer a row-boat using the rear rudder. It is much more stable than trying to steer the rowboat using a rudder in the front. This analogy applies to the submersible vehicle and is verified in these results.

$$\mathbf{G}(\mathbf{S}) = \left[\mathbf{C}\right] \left(\mathbf{s}[I] - \left[A\right]\right)^{-1} \left[B\right] \tag{7}$$

$$G(S)|\_{\delta\_b} \equiv \frac{Y(s)}{\delta\_b(s)} = \frac{0.2271 \text{s}^3 + 0.875 \text{s}^2}{\text{s}^4 + 2.746 \text{s}^3 + 1.298 \text{s}^2} = \frac{\text{s}^2(0.2271 \text{s} + 0.875)}{\text{s}^2(\text{s}^2 + 2.746 \text{s} + 1.298)}\tag{8}$$

$$G(S)|\_{\delta\_b} \equiv \frac{Y(\mathbf{s})}{\delta\_b(\mathbf{s})} = \frac{1.211\mathbf{s}^2 + 1.518\mathbf{s}}{\mathbf{s}^4 + 2.746\mathbf{s}^3 + 1.298\mathbf{s}^2} = \frac{s(1.211\mathbf{s} + 1.518)}{\mathbf{s}^2(\mathbf{s}^2 + 2.746\mathbf{s} + 1.298)}\tag{9}$$

In **Figure 3**, the uncontrolled system is analyzed by merely performing a circular turn with each (and then both) rudders. The bow and stern rudders alone are each compared to the combined use of both bow and stern rudders. The bow rudder was deflected +15� for about 21 s, while the stern rudder was deflected for �15� for about 11 s. When both rudders were deflected the maneuver was completed in roughly 8 seconds. Two initial conditions for the sway velocity were investigated (*ν*ð Þ¼ <sup>0</sup> 0 and then *<sup>ν</sup>*ð Þ¼ <sup>0</sup> ffiffiffi 8 <sup>p</sup> ). In all cases, the bow rudder alone performed the poorest, with the stern rudder alone performing the turn in a smaller radius and shorter time. Furthermore, the combined use of both rudders resulting in tightest maneuver.

Two simulation methodologies were used to investigate sensitivities to integration method. MATLAB was used with Euler integration, while SIMULINK was used with Runge-Kutta integration with identical timesteps, Δt = 0.1 s. The results were nearly negligible and are displayed in **Table 1**, from which insensitivity to integration approach is established.

**Figure 3.** Analysis of uncontrolled system: comparison of rudder performance: (a) counter-clockwise turn, *ν*ð Þ¼ 0 0 and (b) initial sway velocity *<sup>ν</sup>*ð Þ¼ <sup>0</sup> ffiffiffiffi <sup>8</sup>*:* <sup>p</sup>


**Table 1.** Comparison of simulation integration methodology.

#### **2.2. Control law design**

In the system analysis, the optimal rudder implementation scheme was determined to be the application of both rudders, where the rudders were slaved to the same maneuver angle magnitude with the opposite sign, i.e. a "scissored-pair" per Eq. (10). In the case where only variable *y* is to be measured, the new state space formulation of the system equation components are in Eq. (11). Under the assumption of rudders constrained to behave as a scissoredpair the transfer function from rudder input to output *y* is given by Eq. (12) whose poles and zeros are listed in Eq. (13), with Eq. (14) revealing the system's eigenvalues, noting the values are identical to the location of the poles in accordance with theory. The controllability and observability matrices ([*CO*] and [*OB*] respectively) are listed in Eq. (15) (whose matrix product [*OC*] is in Eq. (16)) verifying these system equations are both controllable and observable, since these matrices are full rank, while the determinant of the controllability matrix is 63.1778, a large value with a small value of the matrix condition number, 13.4513. The nonzero determinant of the controllability matrix proves controllability, but to see how close the system is to being uncontrollable, the matrix condition number proves more useful. These two figures of merit indicate the system equations are highly controllable, and accordingly this manuscript will investigate and compare several options for navigation control: pole placement, linear quadratic optimal control, linear quadratic Gaussian, and time optimal control. The same holds true for observability, and thus linear quadratic Gaussian. The matrix product [*OC*] is the same for every definition of state variables for the given system.

$$
\delta\flat\_{\mathbb{b}} = -\delta\_{\mathbb{s}} \tag{10}
$$

$$\begin{aligned} [A] = \begin{bmatrix} -1.4776 & -0.3083 & 0 & 0\\ -1.8673 & -1.2682 & 0 & 0\\ 0 & 1 & 0 & 0\\ 1 & 0 & 1 & 0 \end{bmatrix}, [B] = \begin{bmatrix} 0.0816\\ -3.1271\\ 0\\ 0 \end{bmatrix}, \mathbf{C} = [0 \ 0 \ 0 \ 1], \mathbf{D} = [0] \end{aligned} \tag{11}$$

$$|G(S)|\_\delta \equiv \frac{Y(s)}{\delta(s)} = \frac{0.08164s^2 - 2.06s - 4.773}{s^4 + 2.746s^3 + 1.298s^2} \tag{12}$$

$$\text{poles}: \text{s} = 0, 0, -0.6070, -2.1388; \text{zeros}\\ \text{at}: \text{s} = -6.1279e^{13}, \text{near} - 0, \text{near} - 0 \tag{13}$$

*eig A*ð Þ¼ *λ* ¼ 0*,* 0*,* � 0*:*6070*,* � 2*:*1388 (14)

$$
\begin{aligned}
\begin{bmatrix}
\text{CO}\end{bmatrix} &= \begin{bmatrix}
0.0816 & 0.8433 & -2.4216 & 5.5544 \\
0 & -3.1271 & 3.8132 & -6.4105 \\
0 & 0.0816 & -2.2838 & 1.3916
\end{bmatrix} \text{[}\text{O}\text{]} = \begin{bmatrix}
0 & 0 & 0 & 1 \\
1 & 0 & 1 & 0 \\
0.8917 & -0.4217 & 0 & 0
\end{bmatrix} \text{(15)} \\
\\ &\text{[OC]} = \begin{bmatrix}
0 & 0.0816 & -2.838 & 1.3916 \\
0.0816 & -2.2838 & 1.3916 & -0.8561 \\
1.3916 & -0.8561 & 0.5441 & -0.3825
\end{bmatrix} \end{aligned}
$$

Diagonalizing the original system [*A*] matrix, the spectral decomposition ½ � *T* ½ �¼ *Λ* ½ � *A* ½ �! *T* ½ � *Λ* <sup>¼</sup> ½ � *<sup>T</sup>* �<sup>1</sup> ½ � *A* ½ � *T* in Eq. (17) may be used to verify a diagonal matrix of eigenvalues [Λ], and then write the system of equations in *normal-coordinate form* �*x*0g ¼ *<sup>A</sup>*<sup>0</sup> � � *<sup>x</sup>*<sup>0</sup> f g <sup>þ</sup> *<sup>B</sup>*<sup>0</sup> ½ �½ � *<sup>u</sup> ; y*<sup>0</sup> f g <sup>¼</sup> *<sup>C</sup>*<sup>0</sup> ½ � *<sup>x</sup>*<sup>0</sup> ½ � � using the following transformation: *<sup>A</sup>*<sup>0</sup> � � <sup>¼</sup> ½ �¼ *<sup>Λ</sup>* ½ � *<sup>T</sup>* �<sup>1</sup> ½ � *<sup>A</sup>* ½ � *<sup>T</sup>* , *<sup>B</sup>*<sup>0</sup> ½ �¼ ½ � *<sup>T</sup>* �<sup>1</sup> ½ � *B* , and *C*<sup>0</sup> ½ �¼ ½ � *C* ½ � *T* whose results are in Eq. (18).

$$[A] = \underbrace{\begin{bmatrix} 0.4663 & -0.1074 & 0 & 0 \\ 1 & 0.3033 & 0 & 0 \\ -0.4676 & -0.4996 & 0 & 0 \\ 0.0006 & 1 & 1 & -1 \end{bmatrix}}\_{\overline{T}^{-1}} \begin{bmatrix} -1.4776 & -0.3083 & 0 & 0 \\ -1.8673 & -1.2682 & 0 & 0 \\ 0 & 1 & 0 & 0 \\ 1 & 0 & 1 & 0 \end{bmatrix} \begin{bmatrix} 0.4663 & -0.1074 & 0 & 0 \\ 1 & 0.3033 & 0 & 0 \\ -0.4676 & -0.4996 & 0 & 0 \\ 0.0006 & 1 & 1 & -1 \end{bmatrix} \tag{17}$$

$$[A'] = \begin{bmatrix} -2.1388 & 0 & 0 & 0\\ 0 & -0.6070 & 0 & 0\\ 0 & 0 & 0 & 0\\ 0 & 0 & 0 & 0 \end{bmatrix}, [B'] = \begin{Bmatrix} -1.2502028\\ -6.1888806\\ -8.4625e^{7}\\ -8.4625e^{7} \end{Bmatrix}, [C] = \{0.0006 \quad 1 \quad 1 \quad -1 \quad \{18\}$$

$$\{\mu\}\_{\text{baseline}} = \{\delta\} = -K\_v \upsilon - K\_r r - K\_\psi \psi - K\_y y \tag{19}$$

For the pole placement proportional-derivative (PD) controller articulated in Eq. (19), the poles are set to have roughly the same time constant, while avoiding exactly coincident poles. Gains are iterated for various time constants as displayed in **Figure 4**, but the following rule of thumb is asserted as well to quickly achieve performance that closely mimics the performance of linear-quadratic optimal (LQR) gains where the control effort and tracking error are equally weighted in the cost function of the optimization.

*RULE OF THUMB: Select unity time-constant tc to roughly locate closed-loop poles per Eq.* (20)*. Then place other poles at slightly different locations (e.g. sp* ¼ *s*<sup>1</sup> � 0*:*01∀*p)*

$$Pole : s\_1 = \frac{1}{t\_c} \tag{20}$$

Autonomous Underwater Vehicle Guidance, Navigation, and Control http://dx.doi.org/10.5772/intechopen.80316 9

**Figure 4.** Gain values for each state iterated for various time constants.

The gains achieved using the rule of thumb KR.O.T. = {0.5070 –0.3687 -0.7157 -0.1972} have quite different values compared to the gains calculated through the matrix Riccati equation in the linear-quadratic optimization KLQR = {0.0939 –1.2043 -2.2138 -1}, but nonetheless the resulting behaviors are indeed very similar.

Next, the initial feedback control design was evaluated in simulations where the ship is initially located off the desired track by one ship's length port side with zero heading, and rudder deflection was limited to 0.4 radians (23). Next, another simulation was performed to test an initial heading angle of 30 starboard where the initial *y*(0) = 0. The results are displayed in **Figure 5(a)** and **(b)** respectively. All state variations were plotted in **Figure 4**, highlighting the fact that *y* converges to zero along with the other states. Furthermore, the results of rudderlimited simulations are displayed in **Figure 6** and **Figure 7** for both scenarios (**Table 2**).

**Figure 5.** Simulations testing the initial baseline feedback controller in two scenarios: (a) initially one ship's length port side and (b) initial heading 30o starboard.

**Figure 6.** State variations for both scenarios simulated using pole-placement gains via *rule of thumb*: (a) initially one ship's length port side and (b) initial heading 30<sup>o</sup> starboard.


**Table 2.** Gains for various time constants and also solution to linear quadratic optimization.

#### **2.3. Observer design**

To design a state observer, the system must be observable [4], verifiable through examination of the observability matrix [*OB*] per Eq. (21), where [*C*]=[*ν r ψ y*] = [0 1 1 1]. The condition of the observability matrix reveals the degree of observability, and it is defined by the ratio of maximum to minimal singular values.

$$[\text{OB}] = \begin{bmatrix} \text{C} \\ \text{CA} \\ \text{CA}^2 \\ \vdots \\ \text{CA}^{n-1} \end{bmatrix} \tag{21}$$

*2.3.1. Full-order observer design*

$$\left\{ \dot{\hat{\mathbf{x}}} \right\} = [A]\{\hat{\mathbf{x}}\} + [B][\boldsymbol{u}] + [L](\{\boldsymbol{y}\} - [\mathbb{C}]\{\hat{\mathbf{x}}\}) \tag{22}$$

$$\left\{\dot{\mathbf{x}}\right\}-\left\{\dot{\hat{\mathbf{x}}}\right\}=[A]\{\mathbf{x}\}-[A]\{\hat{\mathbf{x}}\}-[L]([\mathbf{C}]\{\mathbf{x}\}-[\mathbf{C}]\{\hat{\mathbf{x}}\})\tag{23}$$

Autonomous Underwater Vehicle Guidance, Navigation, and Control http://dx.doi.org/10.5772/intechopen.80316 11

**Figure 7.** Rudder-limited trajectory track using pole-placement gains via *rule of thumb* and LQR: (a) initially one ship's length port side and (b) initial heading 30o starboard.

$$\left\{\dot{\hat{e}}\right\} \equiv \left\{\dot{\hat{x}}\right\} - \left\{\dot{\hat{x}}\right\} = \left( [A] - [L][\mathbb{C}] \right) (\left\{\mathbf{x}\right\} - \left\{\dot{\hat{x}}\right\}) \tag{24}$$

$$\{\acute{e}\} = ([A] - [L][\mathbb{C}])\{e\} \tag{25}$$

Assuming that only *ν* measurements are available, a mathematical model of the estimated system is in Eq. (22) with a full order observer design using the observer error Eq. (23) leading to the error vector in Eq. (24) allowing the re-expression of Eq. (22) as Eq. (25), where the dynamic behavior of the error vector is determined by the eigenvalues of matrix½ �� *A* ½ � *L* ½ � *C* , where ½ � *L* gains of the observer may be chosen as desired for systems that prove observable, such that the error vector will converge to zero for any stable ½ �� *A Ke* ½ �½ � *C* . In the following paragraphs, ½ � *L* is designed by solving the matrix Ricatti equation leading to linear quadratic optimal gains, and also by solving the *rule of thumb* relationship between gains and time constant as done for the controller gains (**Table 3**).

**Figure 8** displays the results of simulations revealing the accuracy of state estimation when ½ � *L* is calculated by the *rule of thumb*, where the time constant is chosen to be half (*tc* = 1/2) the time constant of the controller (*tc* = 1) and the simulation is initialized with the heading angle 30� off, while **Figure 9** displays the simulation initialized one boat-length starboard position.


**Table 3.** Full-order observer gains designed by *rule of thumb* for various time constants as multiple of controller time constant, *tc* .

**Figure 8.** Simulations starting 30� off heading gains via *rule of thumb* state observer gains, (a) true and estimated sway velocity, *ν(t)*, (b) true and estimated turning rate, *r(t)*, (c) true and estimated heading angle, *ψ(t)*, (d) true and estimated cross track, *y(t).*

#### *2.3.2. Reduced-order observer design*

Assuming that some measurements are available from sensors, this paragraph describes the possible iterations and reveals states that are relatively more important to measure with sensors. Four possible output matrices are used to investigate observability. Four options for output matrices ½ � *C <sup>i</sup>* for i = 1…4 result in four reduced-order observers ½ � *OB <sup>i</sup>* for i = 1…4 are detailed in Eqs. (26)–(29). Output matrix ½ � *C* <sup>1</sup> produces an observability matrix ½ � *OB* <sup>1</sup> with rank = 4 (observable) and determinant not nearly equal to zero. Output matrix ½ � *C* <sup>2</sup> produces an observability matrix ½ � *OB* <sup>2</sup> with rank = 4 (observable) and determinant not nearly equal to zero. Output matrix ½ � *C* <sup>3</sup> produces an observability matrix ½ � *OB* <sup>3</sup>with rank = 4 (observable) and determinant nearly equal to zero. The matrix condition number is very high indicating the system is barely observable. Output matrix ½ � *C* <sup>4</sup> produces an observability matrix ½ � *OB* <sup>4</sup> with rank = 3 (not observable) and determinant equal to zero with a matrix condition number equal to infinity. This means if all other states are measured by sensors, it is not possible to use an observer (even an optimal observer) to determine lateral deviation (cross-track error), *y*. It is a Autonomous Underwater Vehicle Guidance, Navigation, and Control http://dx.doi.org/10.5772/intechopen.80316 

**Figure 9.** Simulations starting 1 boat-length starboard with gains via *rule of thumb*, (a) true and estimated sway velocity, *ν(t)*, (b) true and estimated turning rate, *r(t)*, (c) true and estimated heading angle, *ψ(t)*, (d) true and estimated cross track, *y(t).*

key state to measure with sensors. The sensor combinations that include *y* are observable. Using every other sensor, (except *y)* results in a system that is not observable. Furthermore, measuring *y* alone results in a barely observable system.

$$[\mathbf{C}]\_1 = \begin{Bmatrix} \boldsymbol{\nu} \\ \boldsymbol{r} \\ \boldsymbol{\psi} \\ \boldsymbol{y} \end{Bmatrix} = \begin{Bmatrix} \boldsymbol{0} \\ \boldsymbol{1} \\ \boldsymbol{1} \\ \boldsymbol{1} \end{Bmatrix} \rightarrow [\mathbf{O}\boldsymbol{\mathcal{B}}]\_1 = \begin{bmatrix} \mathbf{C} \\ \mathbf{CA} \\ \mathbf{CA}^2 \\ \vdots \\ \mathbf{CA}^{n-1} \end{bmatrix} = \begin{bmatrix} \boldsymbol{0} & \boldsymbol{1} & \boldsymbol{1} & \boldsymbol{1} \\ -\boldsymbol{0.8673} & -\boldsymbol{0.2682} & \boldsymbol{1} & \boldsymbol{0} \\ \boldsymbol{1.7823} & \boldsymbol{1.6074} & \boldsymbol{0} & \boldsymbol{0} \\ -\boldsymbol{5.6352} & -\boldsymbol{2.5879} & \boldsymbol{0} & \boldsymbol{0} \end{bmatrix} \tag{26}$$

$$[\mathbf{C}]\_2 = \begin{Bmatrix} \boldsymbol{\nu} \\ \boldsymbol{\nu} \\ \boldsymbol{\psi} \\ \boldsymbol{y} \end{Bmatrix} = \begin{Bmatrix} \boldsymbol{0} \\ \mathbf{1} \\ \mathbf{0} \\ \mathbf{1} \end{Bmatrix} \rightarrow [\mathbf{O}\mathbf{B}]\_2 = \begin{bmatrix} \mathbf{C} \\ \mathbf{C}\mathbf{A} \\ \mathbf{C}\mathbf{A}^2 \\ \vdots \\ \mathbf{C}\mathbf{A}^{\text{v}-1} \end{bmatrix} = \begin{bmatrix} \boldsymbol{0} & \boldsymbol{1} & \boldsymbol{0} & \boldsymbol{1} \\ -\boldsymbol{0}.8673 & -\boldsymbol{1}.2682 & \boldsymbol{1} & \boldsymbol{0} \\ \boldsymbol{3}.6496 & \boldsymbol{2.8756} & \boldsymbol{0} & \boldsymbol{0} \\ -\boldsymbol{10}.7624 & -\boldsymbol{4}.7717 & \boldsymbol{0} & \boldsymbol{0} \end{bmatrix} \tag{27}$$

$$[\mathbf{C}]\_2 = \begin{Bmatrix} \mathbf{v} \\ r \\ \psi \\ y \end{Bmatrix} = \begin{Bmatrix} \mathbf{0} \\ \mathbf{0} \\ \mathbf{0} \\ 1 \end{Bmatrix} \rightarrow [\mathbf{O}\mathbf{B}]\_3 = \begin{bmatrix} \mathbf{C} \\ \mathbf{CA} \\ \mathbf{CA}^2 \\ \vdots \\ \mathbf{CA}^{n-1} \end{bmatrix} = \begin{bmatrix} \mathbf{0} & \mathbf{0} & \mathbf{0} & \mathbf{1} \\ \mathbf{1} & \mathbf{0} & \mathbf{1} & \mathbf{0} \\ -\mathbf{1}\mathbf{4}776 & \mathbf{0}\mathbf{1}\mathbf{7} & \mathbf{0} & \mathbf{0} \\ \mathbf{0}.8917 & -\mathbf{0}\mathbf{4}217 & \mathbf{0} & \mathbf{0} \end{bmatrix} \tag{28}$$

$$\begin{Bmatrix} \mathbf{v} \\ \mathbf{v} \\ \end{Bmatrix} \rightarrow \begin{Bmatrix} \mathbf{1} \\ \mathbf{1} \end{Bmatrix} \qquad \qquad \qquad \begin{Bmatrix} \mathbf{C} \\ \mathbf{C} \\ \mathbf{A} \end{Bmatrix} \rightarrow \begin{Bmatrix} \mathbf{1} & \mathbf{1} & \mathbf{1} & \mathbf{0} \\ -\mathbf{1}\mathbf{4}776 & \mathbf{0} & \mathbf{1}\mathbf{0} \end{Bmatrix}$$

$$[\mathbf{C}]\_2 = \left\{ \begin{array}{c} \cdot \\ \cdot \\ \lor \\ \lor \\ \lor \end{array} \right\} = \left\{ \begin{array}{c} 1 \\ 1 \\ 1 \\ 0 \end{array} \right\} \rightarrow [\text{OB}]\_4 = \left\{ \begin{array}{c} \text{CA} \\ \text{CA}^2 \\ \text{CA}^2 \\ \vdots \\ \text{CA}^{\text{n-1}} \end{array} \right\} = \begin{array}{c} -3.3450 & -0.5764 & 1 & 0 \\ 6.90190 & 1.7621 & 0 & 0 \\ -12.1843 & -4.0900 & 0 & 0 \end{array} \right\} \tag{29}$$

Assuming *y* is to be measured by a sensor, **Table 4** reveals that measuring *ν* in addition to y produces the most observable system, and is recommended for designing reduced-order observers. The drawback is measuring ν requires a Doppler sonar, which may not always be available. If all states are measureable except ν the resulting reduced-order observer merely estimates *ν* using gains on the measureable states displayed in **Table 5**. **Figure 10** reveals very good estimation of *ν* when all other states are sensed, and this estimated value of *ν* was fed to the motion controller in addition to the measured states (the poorly estimated states were neglected instead favoring the more-accurate measurements). State convergence to zero is achieved in the instance of state initialization 30� off-heading. **Figure 11** displays similar results for the instance of state initialization one boat-length starboard.


1 Reminder: high condition number is less observable system.

**Table 4.** Observability matrix condition number for options to supplement *y* measurement.


1 Relatively faster <sup>1</sup> <sup>10</sup> *tc* is used in subsequent simulations.

**Table 5.** Reduced-order observer gains designed by *rule of thumb* for various time constants as multiple of controller time constant, *tc* .

Autonomous Underwater Vehicle Guidance, Navigation, and Control http://dx.doi.org/10.5772/intechopen.80316 15

**Figure 10.** Simulations starting 30� off heading gains via *rule of thumb* reduced-order state observer gains: (a) true and estimated sway velocity, *ν(t)* versus time (seconds), (b) true and estimated turning rate, *r(t)* versus time (seconds), (c) true and estimated heading angle, *ψ(t)* versus time (seconds), (d) true and estimated cross track, *y(t)* versus time (seconds).

#### *2.3.3. Gain margin and phase margin*

**Figure 12** compares the loop gains of the system with and without a compensator via gain margin and phase margin with full-state feedback, while **Figure 13** displays the loop gains when output-feedback via observers is used. Each has relative strengths. Full state (theoretical) feedback yields infinite gain margin, yet relatively lower phase margin (usually consider more important of the two), while output feedback (real-world) yields good (but lesser) gain margin with increased phase margin.

### **2.4. Tracking systems and feedforward control in the presence of constant disturbance currents**

This section evolves the earlier developed system equations and performance analysis by adding non-quiescent conditions, in particular introduction of a lateral underwater ocean current with an absolute velocity, *υ*0, requiring a modification of the system equations to add the lateral current to Eq. (4) resulting in Eq. (30).

$$\dot{y} = \sin\psi + \nu\cos\psi + \nu\_0\tag{30}$$

**Figure 11.** Simulations starting one boat-length starboard with gains via *rule of thumb* reduced-order state observer gains: (a) true and estimated sway velocity, *ν(t)* versus time (seconds), (b) true and estimated turning rate, *r(t)* versus time (seconds), (c) true and estimated heading angle, *ψ(t)* versus time (seconds), (d) true and estimated cross track, *y(t)* versus time (seconds).

#### *2.4.1. Analysis of disturbed system in ocean currents via state equations and simulations*

Using the controller (Eq. (19)) and the modified system equations where Eq. (4) is replaced by Eq. (30), and applying the final value theorem: *f t*ð Þ*<sup>t</sup>*!<sup>∞</sup>*sF s*ð Þ*<sup>s</sup>*!<sup>0</sup>, a steady state value 1/<sup>ω</sup> + 1 has some variable quantity added to unity for various *υ*0. Thus, steady-state errors exist in all cases with such disturbances, which are verified by simulations depicted in **Figure 14** using gain values from the *rule of thumb* (ROT) for unity time constant. The steady-state errors are directly proportional to the disturbance magnitude. **Figure 15** displays max rudder deflection for the maximal lateral ocean current in the study (to verify the control design continues to remain less than 0.4 radians) where we learned any current greater than 0.4 cannot be eliminated; therefore we next investigate feedforward control and integral control.

#### *2.4.2. Elimination of steady-state error using feedforward control*

Modify the control law to f g*u feedforward* ¼ f g*δ* ¼ �*K*1*υ* � *K*2*r* � *K*3*ψ* � *K*4*y* � *K*<sup>0</sup> in order to eliminate the steady-state error, where *K*<sup>0</sup> is chosen to insure zero steady-state error, where the feedback gains are chosen by the *rule of thumb* (**Figures 16** and **17**).

Autonomous Underwater Vehicle Guidance, Navigation, and Control http://dx.doi.org/10.5772/intechopen.80316 17

**Figure 12.** Infinite gain margin and 80.2o phase margin using full state feedback via full-ordered observer with rule of thumb controller gains: (a) root locus real Axis, (b) bode plot frequency (rad/sec).

**Figure 13.** 61.4 gain margin and 145 phase margin using reduced-order observer (both rule of thumb gains for halfcontroller tc = 0.5, and compensator with rule of thumb gains (tc = 1), (a) root locus, real axis, (b) bode plot, frequency (rad/sec).

**Figure 14.** Steady-state position error for various lateral underwater ocean currents.

**Figure 15.** Feedback alone unable to counter constant lateral underwater ocean currents, (a) rudder deflection, *υ*<sup>0</sup> ¼ 0*:*5, (b) steady state error vs. *υ*0.

**Figure 16.** Feedforward element included to counter constant lateral underwater ocean currents, (a) rudder deflection, *υ*<sup>0</sup> ¼ 0*:*5, (b) all states when *υ*<sup>0</sup> ¼ 0*:*5.

**Figure 17.** Comparison: feedback control with and without feedforward (*υ*<sup>0</sup> ¼ 0*:*5).

#### **2.5. Disturbance estimation with reduced-order observer and integral control**

Section 2.4 demonstrated feedforward control effectively countered the disturbance currents, but the current was presumed to be known. In to truly be effective, the reduced order observer is next augmented to include estimation of the unknown disturbance current velocity *ν* ^*c*, where the observer now estimates the disturbance current velocity, the lateral sway velocity, *ν*, the lateral deviation (cross-track error), *y,* and the heading angle *ψ*. **Figure 18a** and **b** display the estimates of the unknown current for two current velocity conditions: *ν* ^*<sup>c</sup>*<sup>1</sup> ¼ *νest*<sup>1</sup> ¼ 0*:*1and *ν* ^*<sup>c</sup>*<sup>2</sup> ¼ *νest*<sup>2</sup> ¼ 0*:*5 respectively, while **Figure 18c** and **d** display the *y* and *ψ* states for each current velocity conditions. Notice how large rudder deflections modify the heading angle to the command-tracking value which counters the disturbance current (sometimes referred to as "crabbing"), and after establishing the crab heading angle, the rudder deflection goes towards zero, illustrating the effectiveness of command tracking.

**Figure 19** displays all the states versus time in seconds and also the trajectory when a worstcase unknown disturbance current *ν<sup>c</sup>* ¼ �0*:*5 is applied and estimated by the reduced-order observer where the observer gains are solutions to the linear quadratic Gaussian optimization. Meanwhile **Figure 20** displays the results in cases utilizing command tracking with reduced order observer and with command: ψ = �0.5 and sinusoidal disturbance current *υ<sup>c</sup>*<sup>0</sup> = Asin(0.1 t) but no disturbance estimation or feedforward, while **Figure 21** uses disturbance estimation and feedforward and rule of thumb gains. Lastly, **Figure 22** displays the performance of reduced-order observers, which is especially useful in instances of limited at-sea computational capabilities.

#### **2.6. Waypoint guidance**

A simple line-of-sight guidance routine was employed based on fixing waypoints through a minefield in order to navigate to a specified point and safely return home. The coordinates are

**Figure 18.** Reduced-order observer state estimates versus time (seconds) for two disturbance currents *υ<sup>c</sup>*<sup>0</sup> ¼ ½ � 0*:*1 0*:*5 , where *Δ* is the rudder deflection using these estimates when the worst-case disturbance current is applied. (a) Sway velocity, (b) disturbance current, (c) lateral deviation (cross-track error), (d) heading angle.

**Figure 19.** Performance with disturbance estimation and command tracking using LQR and *rule of thumb* gains in reduced order observer, and command tracking to *ψ* ¼ �0*:*5 amidst constant disturbance current *υ<sup>c</sup>* ¼ 0*:*5., (a) states, (b) trajectory.

Autonomous Underwater Vehicle Guidance, Navigation, and Control http://dx.doi.org/10.5772/intechopen.80316 21

**Figure 20.** Utilization of command tracking with reduced order observer, with command: *ψ* ¼ �0*:*5 and sinusoidal disturbance current *ν<sup>c</sup>*<sup>0</sup> ¼ *Asin*ð Þ 0*:*1*t* but no disturbance estimation or feedforward, (a) all states vs. time (seconds), (b) trajectory.

**Figure 21.** Utilization of command tracking with reduced order observer, with command: ψ = �0.5 and sinusoidal disturbance current *ν<sup>c</sup>*<sup>0</sup> ¼ *Asin*ð Þ 0*:*1*t* and disturbance estimation and feedforward and rule of thumb gains, (a), (b).

fed to a logic determining when to turn per Eq. (31), where *d* is the distance to the waypoint, and the heading command was autonomously calculated per Eq. (32).

$$\text{Turnu} \mathbf{i} : \sqrt{(\mathbf{x}\_c - \mathbf{x})^2 + \left(y\_c - y\right)^2} \le d \tag{31}$$

$$
\psi\_{command} = K \tan^{-1} \left( \frac{y\_c - y}{x\_c - x} \right) \tag{32}
$$

Particular attention is brought to the inverse tangent calculation, since quadrant must be preserved in the calculation, since the vehicle will navigate in 360�.

**Figure 22.** Utilization of command tracking with reduced order observer, with command: ψ = �0.5 and sinusoidal disturbance current *ν<sup>c</sup>*<sup>0</sup> ¼ *Asin*ð Þ 0*:*1*t* , (a) with disturbance estimation (and feedforward), reduced order observer, (b) with integral control but no disturbance estimation or feedforward.
