## 2 The Attitude Algorithm
### Attitude Algorithm Software Implementation and Experimental Testing  
The software implementation of the algorithm (Figure 3a) was made by using the Matlab/Simulink software package, which allowed us to test it through numerical simulation, but also by using experimentally acquired data. To develop the software associated with the mathematics of the previously exposed algorithm, an S-function was programmed, simultaneously implementing the ortho-normalization procedure but also the mechanism applied for the algorithm initialization.  
A graphical user interface (GUI) (Figure 3d) masks the Simulink block \"Attitude\" in Figure 3a, which implements the attitude algorithm. The dynamic fields in the GUI facilitate the human user to initialize the algorithm by setting the initial values of the attitude angles and the integration step under the form of the sample time. At the same time, the selection of the method order is possible by using the interface, which means the changing of the \(C_{m}\) and \(S_{m}\) coefficients evaluation formulas (\(m=1\) to \(6\)). The block in Figure 3a, called \"Attitude\", is the first layer block in the software implementation, and it has as inputs the gyro readings (\(\omega_{x}\), \(\omega_{y}\), \(\omega_{z}\)) expressed in degrees/s in this implementation. The block outputs are the attitude angles and are provided in degrees. In the implementation, the angular speeds were labeled with \"\(p\), \(q\), \(r\)\", while the attitude angles were labeled with \"_Euler angles_\". The second layer block (Figure 3b) in the backside of \"Attitude\" includes the  
Figure 3: Software implementation of the attitude algorithm: (**a**) Simulink block “Attitude”; (**b**) “Attitude L2” block; (**c**) layer accessing algorithm S-function; (**d**) graphical user interface (GUI).  
block called \"Attitude L2\", which implements in an S-function (Figure 3c) the algorithm for attitude determination.  
In the evaluation phase, the software model was firstly numerical simulated, and subsequently experimentally tested on a bench in the lab and by using some experimentally acquired data [83]. The experimental model used for the testing at the lab level contained a strap-down inertial platform equipped with a gyro triad based on classical electro-mechanical gyro sensors, able to measure angular speeds of a maximum of \(15^{\circ}\)/s. The calibration of the gyro sensors has been performed by using a rotating platform, accurately controlled in angular speed. The numerical simulation analysis and the bench test results proved that the software model developed for the attitude algorithm works well, validating its operation in this way. The next tests were based on experimental data provided by a gyro triad with MEMS sensors, equipping the inertial measurement unit of the SDINS/GPS navigator. As a reference for the developed attitude algorithm evaluation, we used the attitude component (roll, pitch and yaw angles) of the solution of navigation offered by the SDINS/GPS integrated navigator. An example of such testing results is shown in Figure 4, where Figure 4 presents the measurements provided by the gyro triad (\(\omega_{x}\), \(\omega_{y}\), \(\omega_{z}\)) and Figure 4 presents the graphical characteristics of the attitude angles provided by the algorithm and by the reference navigator. For the exposed example, the numerical integration method was used with the truncation order \(m=6\). The characteristics in Figure 4 show that the attitude solutions provided by the reference navigator and by the developed algorithm are very close. The biases of the MEMS gyros were estimated and corrected by using an offline method, involving the determination of the mean values of the gyro signals in static tests, while the SDINS/GPS reference navigator data processing algorithm, based on a Kalman filter, estimated and corrected the sensors biases in real time [83].