*2.10. Numerical Simulation in MATLAB/SIMULINK*

Validating simulations were performed in MATLAB/SIMULINK Release R2021a with Euler integration solver (ode1) and a fixed time step of 0.01 s, whose results are presented in Section 3, while this subsection displays the SIMULINK models permitting the reader to duplicate the results presented here. Sensor noise was added per Section 2.8. The classical feedback subsystem is displayed in Figure 4a. The optimal open loop subsystem implements Equation (31), and is elaborated in Figure 4b,c. The real time optimal subsystem implements Equations (42) and (31) augmented by feedback decoupling as in Equation (42). The "switch to open loop" subsystem switches when the matrix inverted in Equation (39) is singular indicated by a zero valued determinant and is elaborated in Figure 4d. The quadratic cost calculation computes Equation (3) and is elaborated in Figure 4b, while the cross-product motion feedback implements the cross product of Equation (42). The P + V subsystem and PD/PI/PID subsystems depicted in Figure 4a implement classical methods not re-derived here, but whose computer code is presented in Appendix B, Algorithms A1 and A2.

Figure 5 displays the SIMULINK subsystems used to implement the three instantiations of real-time optimization (labeled RTOC from provenance in optimal control) where the switching displayed in Figure 3b permits identical simulation experiments to be performed with all conditions fixed, varying only the proposed implementation. The subsystems execute Equation (39) with three variations of matrix inversion: (1) MATLAB's backslash "\", (2) Moore-Penrose pseudoinverse (*pinv*), (3) *LU-inverse*.

**Figure 5.** SIMULINK subsystems; (**a**) real-time optimal decision switching topology; (**b**) real-time optimal calculation of Equation (39) using *backslash*\*inverse*; (**c**) real-time optimal calculation of Equation (39) using *pinv inverse*; (**d**) real-time optimal calculation of Equation (39) using *LU inverse*.

Section 2.10 presented SIMULINK subsystems used to implement the equations derived in the section. Table 1 displays the software configuration used to simulate the equations leading to the results presented immediately afterwards in Section 3.

**Table 1.** Software configuration for simulations reported in Section 3.


#### **3. Results**

Section 2 derived several options for estimating state, rate, and control simultaneously as outputs of Pontryagin's treatment of the problem formulated as Hamiltonian systems. Section 2.9 described implementation of sensor noise narratively, while Figure 3 illustrated the topological elaboration using SIMULINK including state and rate sensor with added Gaussian noise whose noise power was set in accordance with Section 2.9. SIMULINK subsystems were presented to aid repeatability (with callback codes in the Appendix B). Those subsystems were used to run more than ten-thousand simulations: a nominal simulation run for each technique with the remainder utilized to evaluate vulnerability to variations as described in Section 2.9. In Section 3.1, benchmarks of performance are established using classical methods for state and rate errors and optimum cost calculated in Section 2. Sections 3.2–3.4 describe real-time optimal utilization of feedback to establish online estimates of the solution of the modified boundary value problem described in Section 2. Each section respectively evaluates the three methods compared: *backslash*\*inverse*, *pinv inverse*, and *LU inverse*.

General lessons from the results include:


#### *3.1. Benchmark Classical Methods*

Classical methods as presented in [41,42] with nonlinear decoupling loops as proposed in Equation (42) depicted in Figure 3b were implemented in SIMULINK according to Figure 4a. Computer code implementing these classical methods is presented in Appendix B, Algorithm 1. Estimation was executed by feedback of proportional plus integral plus derivative (PID), proportional plus derivative (PD), and also proportional plus velocity, and the results displayed in Figure 6, establishing the benchmark for state and rate tracking. Table 2 displays quantitative data corresponding to Figure 6's qualitative displays. Notice the optimal estimation of state, rate, and decision criteria is also included in Figure 6 and Table 2, since the optimal cost benchmark is established by Pontryagin's treatment in Equation (31).

**Figure 6.** Comparison of classical methods with scaled time on the abscissae and respective ordinates titled in the subplot captions: thinner solid blue line is manually tuned PID, dashed line is linear quadratic optimal PD, dotted line is proportional plus velocity tuned to performance specification, thicker solid black line is open loop optimal per Pontryagin (Equation (31) provided for anticipated comparison). (**a**) States, (**b**) rates, and (**c**) decision criteria). Notice the solid black line representing the optimal open loop solution in subfigure (**c**) is initially positive to create movement in the desired direction, and the control switches exactly at the halfway point to a negative input, slowing progress towards the final desired end states (position and rate states). The ranges of the zoomed view in the inset are indicated by the respective scales.



#### *3.2. Real-Time Optimal Methods with Backslash*

This section displays the results of real-time optimal estimation using the *backslash*\*inverse* depicted in Figure 5b with and without singular switching displayed in Figure 4d to inverse the [*T*] in Equation (39). The results are compared to open loop optimal results per Equation (31) displayed in Figure 4c. Figure 7 reveals real-time optimal state estimation performs relatively poorly using the MATLAB *backslash*\*inverse*, but performance is restored to nearoptimal performance when augmented with singular switching. State and rate errors are restored to essentially optimal values, while cost is restored to very near the optimal case as evidenced by the quantitative results displayed in Table 3.



**Figure 7.** Comparison of real-time optimal methods with scaled time on the abscissae and respective ordinates titled in the subplot captions: solid line is open loop optimal (benchmark), dashed line is real-time optimal using *backslash*, thick dotted line is real-time optimal using *backslash* with singular switching. (**a**) States, (**b**) rates. Notice the solid black line representing the optimal open loop solution in subfigure (**c**) is initially positive to create movement in the desired direction, and the control switches exactly at the halfway point to a negative input, slowing progress towards the final desired end states (position and rate states). The ranges of the zoomed view in the inset are indicated by the respective scales.

Figure 8 and Table 4 reveal the estimation performance of the constants of integration solving the modified two-point boundary value problem (BVP) using state and rate feedback to reset the initial conditions of the BVP. Oddly, despite relatively superior performance estimating the state and rates when using singular switching augmentation, parameter estimation is far inferior.

**Figure 8.** Comparison of real-time optimal methods with scaled time on the abscissae and respective ordinates titled in the subplot captions: dashed line is real-time optimal using *backslash*, dotted line is real-time optimal using *backslash* with singular switching. (**a**) Estimates of *a*ˆ, (**b**) estimates of ˆ *b*. The ranges of the zoomed view in the inset are indicated by the respective scales.

**Table 4.** Comparison of real-time optimal decision methods using *backslash* matrix inversion.


Section 3.1 presented the results of classical and optimal methods as benchmarks for performance. Meanwhile, Section 3.2 presented the results of implementing real-time optimal estimation with *backslash*\*inverse* with and without singular switching compared to the optimal benchmark. Next, Section 3.3 presents results using *pinv inverse* with and without singular switching.

#### *3.3. Real-Time Optimal Methods with Pinv*

Inversion of the [*T*] matrix in Equation (39) was also accomplished by the Moore– Penrose pseudoinverse equation: [*T*] <sup>−</sup><sup>1</sup> ∼= [*T*] † ≡ [*T*] *<sup>T</sup>*[*T*] −<sup>1</sup> [*T*] *<sup>T</sup>*. All other facets of the problem are left identical, while only the method of matrix inversion is modified resulting in state and rates estimates and comparison of control in Figure 9 with corresponding quantitative results in Table 5. Parameter estimation accuracy is displayed in Figure 10 and Table 6.

**Figure 9.** Comparison of real-time optimal methods with scaled time on the abscissae and respective ordinates titled in the subplot captions: solid line is open loop optimal (benchmark), dashed line is real-time optimal using backslash, thick dotted line is real-time optimal using *pinv* with singular switching. (**a**) States, (**b**) rates, and (**c**) decision criteria or control. The ranges of the zoomed view in the inset are indicated by the respective scales.

**Table 5.** Comparison of real-time optimal decision methods using *pinv* matrix inversion.


**Figure 10.** Comparison of real-time optimal methods with scaled time on the abscissae and respective ordinates titled in the subplot captions: dashed line is real-time optimal using backslash, dotted line is real-time optimal using *pinv* with singular switching. (**a**) Estimates of *a*ˆ, (**b**) estimates of ˆ *b*. The ranges of the zoomed view in the inset are indicated by the respective scales.


**Table 6.** Comparison of real-time optimal decision methods using *p-inv* matrix inversion.

#### *3.4. Real-Time Optimal Methods with Lu-Inverse*

Inversion of the [T] matrix in Equation (39) was also accomplished by the *LU-inverse,* which first creates a pivoted version *Tp* and then inverts the product of a lower triangular matrix, [*L*] and an upper triangular matrix [*U*] based on [*T*]: [*T*] <sup>−</sup><sup>1</sup> ∼= *Tp* −<sup>1</sup> <sup>≡</sup> ([*L*][*U*])−<sup>1</sup> . All other facets of the problem are left identical, while only the method of matrix inversion is modified resulting in state and rates estimates and comparison of control in Figure 11 with corresponding quantitative results in Table 7. Parameter estimation accuracy is displayed in Figure 12 and Table 8.

**Figure 11.** Comparison of real-time optimal methods with scaled time on the abscissae and respective ordinates titled in the subplot captions: solid line is open loop optimal (benchmark), dashed line is real-time optimal using backslash, thick dotted line is real-time optimal using *LU-inverse* with singular switching. (**a**) States, (**b**) rates, and (**c**) decision criteria or control. The ranges of the zoomed view in the inset are indicated by the respective scales.

**Table 7.** Comparison of real-time optimal decision methods using *LU-inverse* matrix inversion.


**Table 8.** Comparison of real-time optimal decision methods using *LU-inverse* matrix inversion.


**Figure 12.** Comparison of real-time optimal methods with scaled time on the abscissae and respective ordinates titled in the subplot captions: dashed line is real-time optimal using backslash, dotted line is real-time optimal using *LU-inverse* with singular switching. (**a**) Estimates of *a*ˆ, (**b**) estimates of ˆ *b*. The ranges of the zoomed view in the inset are indicated by the respective scales.

### *3.5. Monte Carlo Analysis Using Pinv (with Singular Switching) and Open Loop Optimal with Cross-Product Deoupling*

Over ten-thousand simulation runs were performed with 10% uniformly random variations in plant parameters (mass and mass moment of inertia). Noise was added to state and rate sensors with zero mean and standard deviation 0.01, and the results are displayed in the "scatter" plots in Figure 13 with corresponding quantitative results displayed in Table 9. Feedback implemented by resetting the initial condition of the reformulated boundary value problem (when implemented with singular switching) yielded optimal results when augmented with cross-product decoupling.

**Figure 13.** Comparison of the impacts of system variations on real-time optimal using (**a**) open loop optimal, (**b**) *pinv* without switching, (**c**) *pinv* with switching. Scaled state on the abscissae and scaled rate on the ordinates.

**Table 9.** Impact of variations with real-time optimal decision methods using *pinv* inversion.


#### *3.6. Comparison of Results*

Section 3.1 presented the benchmark results produced by classical methods and open loop optimal mathematical solutions. Section 3.2 presented results utilizing MATLAB's *backslash*\*inversion*, while Section 3.3 included results using Moore–Penrose pseudoinverse, *pinv*. Section 3.4 presented results using *LU-inverse*. Section 3.5 revealed robustness to variations in plan parameters with both state and rate sensor noise. This section consolidates the results into a single table of raw data depicted in Table 10. This data will be used to produce percent performance improvement as figures of merit in the Discussion (Section 4).


**Table 10.** Comparison of real-time optimal decision methods.

#### **4. Discussion**

State and rate estimation algorithms fused with noisy sensor measurements using several of the proposed methodologies achieve state-of-the art accuracies with optimality that is analytic and deterministic rather than stochastic, and therefore use very simple equations with necessarily low computational burdens. Simple relationships with small numbers of multiplications and additions are required to be comparable to the simplicity of classical methods, but optimum results are produced that exceed the modern notion of linear-quadratic optimal estimation. Implementation of non-standard feedback achieves robustness with the additional computational cost of a matrix inverse, and therefore three optional inversion methods were compared. General lessons taken with manually tuned PID as a benchmark for state and rate estimation errors, while optimal loop optimal cost is the benchmark for the cost of utilization of state estimations for guidance and control:

	- a. Linear-quadratic optimal estimation achieved 87% better state estimates, but over 400% poorer rate estimates compared to classical PID with costs over 2000% open-loop optimal costs.
	- b. Classical position plus velocity estimation achieved 90% improved state estimation with over 30% better rate estimation, but cost of implementation remains high (over 400% higher than the optimal benchmark).
	- a. Singular switching with *backslash*\*inverse* produced 69% improvement in state estimation and 29% improvement in rate estimation with roughly optimal costs.
	- b. Singular switching with *LU-inverse* produced 72% improvement in state estimation and 33% improvement in rate estimation with roughly optimal costs (3% better than optimal . . . a numerical curiosity).
	- c. Singular switching with *pinv inverse* produced 69% improvement in state estimation and 29% improvement in rate estimation with roughly optimal costs

(approximately identical improvement percentages to *LU-inverse* with singular switching).


#### *4.1. Notes on Percentages of Performance Improvements*

The choice of benchmarks for establishing percentage performance improvements leads to seemingly exaggerated numbers. The current selection of benchmarks emphasizes the strengths of the respective methods: classical feedback estimation methods are designable to achieve high accuracy but suffer from high effort by the decision criteria associated with their use. Optimal methods as instantiated here emphasize minimization of decision effort, so the benchmark for control effort is selected as optimal open loop rather than classical feedback (e.g., manually tuned PID). Percent degradation over thirty thousand percent results when compared to the optimal value (of six) as a benchmark. If the calculation had instead used the manually tuned classical PID as a benchmark, the optimal effort would exhibit an improvement over ninety-nine percent.

The final line in Table 11 illustrates the extreme penalty of not using feedback decoupling of the vector cross-products in Equation (1) representing translation due to the rotating reference. The penalty embodies the deleterious effects of neglecting treatment of the nonlinear, coupled, full six-degree-of-freedom system of equations.



### *4.2. Notes on Executability*

Table 12 displays simulation run time for the eleven methods compared in the manuscript. Run-time was established by establishing the run start-time with the *tic* command, while simulation stop-time was the very first command (*toc*) following each simulation run. Simply neglecting the cross-product terms results in the fastest calculation. All the methods evaluated are of the same order of magnitude of computational burden. Simulations are depicted graphically in Figures 2–5 and alphanumerically in Appendix B.


**Table 12.** Simulation run-time comparison.

#### *4.3. Future Research*

Notice in Figure 3c sinusoidal wave input is coded using the identical time-index of the rest of the simulation. The next stages of future research will utilize this identical simulation to investigate efficacy of the proposed virtual sensoring amidst unknown wave actions. Secondly, hardware validation of key facets of this research is a logical next step.
