An Open-Source Test Environment for Effective Development of MARG-Based Algorithms

Abstract

This paper presents an open-source environment for development, tuning, and performance evaluation of magnetic, angular rate, and gravity-based (MARG-based) filters, such as pose estimators and classification algorithms. The environment is available in both ROS/Gazebo and MATLAB/Simulink, and it contains a six-degrees of freedom (6 DOF) test bench, which simultaneously moves and rotates an MARG unit in the three-dimensional (3D) space. As the quality of MARG-based estimation becomes crucial in dynamic situations, the proposed test platform intends to simulate different accelerating and vibrating circumstances, along with realistic magnetic perturbation events. Moreover, the simultaneous acquisition of both the real pose states (ground truth) and raw sensor data is supported during these simulated system behaviors. As a result, the test environment executes the desired mixture of static and dynamic system conditions, and the provided database fosters the effective analysis of sensor fusion algorithms. The paper systematically describes the structure of the proposed test platform, from mechanical properties, over mathematical modeling and joint controller synthesis, to implementation results. Additionally, a case study is presented of the tuning of popular attitude estimation algorithms to highlight the advantages of the developed open-source environment.

1. Introduction

1.1. Relative Localization

Providing accurate pose estimates (i.e., position and attitude results) is a crucial task in the control of agile mobile systems, such as robots. Since the controller and estimator algorithms are linked in closed-loop, therefore, the estimator algorithm should meet important design requirements. Such requirements ensure that the controller algorithm successfully stabilizes the system in the close vicinity of the desired state based on the estimation results [1,2,3]. If these conditions are not satisfied, then the system can easily be driven to unwanted states, which may eventually damage the hardware [4,5,6,7]. As a result, the estimator algorithm should be both analyzed carefully during dynamic conditions and tuned properly in order to provide accurate and robust results [8,9,10,11].

The relative localization problem is solved with microelectromechanical systems-based (MEMS-based) sensors, such as accelerometers, magnetometers, and gyroscopes, in today’s embedded systems. These sensors form the inertial measurement unit (IMU), where a microcontroller processes the sensor data, executes filtering algorithms and functions as an attitude and heading reference system (AHRS) [12]. At the output of the IMU, the attitude is usually provided in Euler angles (roll, pitch, and yaw angles) or quaternion representation of orientation. The filtering algorithm (attitude/pose estimator) is designed such a way to both handle external disturbances effectively and provide properly smoothed signals. There are three types of disturbances that cause radical decrease in attitude estimation quality. On one hand, external accelerations executed during different translational motions prevent the use of the pure gravity vector in attitude calculation; moreover, vibrations occurring during the control of unstable systems also prohibit the obtainment of reliable attitude results [13]. On the other hand, magnetic perturbations are among the key disturbances that make attitude estimation difficult, since ferromagnetic materials alter the local magnetic field in the sensor frame, thereby resulting in inaccurate attitude realizations. Attitude estimator algorithms address the aforementioned issues and execute stochastic-based information fusion or frequency domain processing of MEMS sensor data to provide reliable results. These algorithms are categorized into two main groups, namely Kalman filter (KF) and complementary filter-based (CF-based) approaches, which are prevalent solutions for relative localization problems; usually, both techniques are augmented with deterministic attitude realization methods.

Deterministic approaches use solely gravity and magnetic field observations to solve the Wahba’s problem and provide attitude estimation [14,15]. Among the techniques, the three-axis attitude determination (TRIAD) and QUaternion ESTimator (QUEST) constitute the fundamental approaches; moreover, recent advances offer enhanced solutions, which provide higher reliability [16,17,18]. These approaches work well during static system conditions; however, external disturbances can reduce their performance significantly. Therefore, the high frequency attitude realization of gyroscope signals is incorporated in filter structures to obtain higher robustness in attitude estimation. Gyroscopes are not sensitive to external disturbances, but the numerical integration of angular rate data results in unbounded drift, i.e., only short term accuracy is ensured with angular rate sensors. This short term accuracy is advantageously utilized in both KF and CF structures. CFs use frequency domain information and fuse sensor data by combining the low frequency attitude realization of accelerometer and magnetometer with high frequency attitude results of gyroscope signals via low-pass and high-pass filters, respectively. CFs are characterized by simple structure and easy implementation; therefore, these algorithms are extensively applied in control systems. Among the techniques, the gradient descent algorithm-based (GDA-based) attitude estimator and the explicit complementary filter (ECF) have become popular choices, and their performance is usually considered as benchmark filters in comparative analysis [19,20,21]. Recent approaches in the realm of CFs augment these benchmark filters with adaptive strategies to both obtain higher reliability and handle external disturbances more effectively [22,23,24]. KFs constitute the core algorithms for state estimation of Gaussian stochastic systems. These Bayesian estimators operate on state space models (which describe the system dynamics) and use statistical information to provide state estimates with minimum variance. The magnetic, angular rate, and gravity (MARG) system fits well into stochastic state space models, where the numerically integrated angular rate measurements describe the state propagation, while the accelerometer and magnetometer data are employed in the attitude update (correct) equations. It is common to employ quaternion representation of orientation and describe the system dynamics with a 7-dimensional state space vector, where the temperature dependent gyroscope bias is also involved in the state propagation [25]. Additionally, the update equations are formulated with the application of the rotation matrix (which describes the relationship between the sensor and navigation frames) or the orientation observation is obtained with deterministic approaches [4]. Recent advances extend the basic filter structure with adaptive techniques, in which novel measurement methods, adaptation laws and inference mechanisms, both detect and incorporate the external disturbances into the filter structure, thereby preventing the quality decrease of estimation performance [5,26,27,28]. A recent survey on attitude estimation techniques is provided in Reference [1].

1.2. Contribution of the Paper

The performance of each technique discussed in the previous subsection heavily depends on the core parameters that characterize the filter structure. These parameters should be selected carefully in order to both meet the design requirements and provide accurate and robust attitude estimates in the intended application. If the intended application is expected to contain different system conditions (e.g., static states, slow/fast motions and agile movements), then the problem of selecting the proper parameters becomes even more crucial, since the system dynamics influences significantly the MARG-based state estimation process. Usually, it is difficult to derive the appropriate filter parameters (e.g., sensor noise power); moreover, the determination process requires particular equipment and measurement methods. As a result, engineering intuition-based filter tuning is usually performed, which yields only a compromise solution between filter dynamics and accuracy. However, it is also common to use optimization-based techniques to maximize the filter performance [2,25,27,29].

Both approaches discussed above require an environment, which enables the evaluation of filter performances. Namely, the true system states need to be obtained simultaneously with the estimation results in order to determine the estimation quality. The literature offers various solutions for such environments, from camera-based techniques [20,30], over industrial robotic manipulators-based approaches [31,32,33], to experimental apparatuses [2]. These environments enable the researcher to analyze the filter convergence, tune the parameters, observe the issues, and incorporate the observations into the development. Moreover, the optimization of these filter structures can be outlined effectively to obtain maximized filter convergence. These advantages emphasize the need for an universally applicable test environment that enables the execution of the aforementioned tasks.

As a result of the investigation, this paper proposes a novel test environment for effective development of MARG-based algorithms. This test environment includes a six-degrees of freedom (6 DOF) mechanism to execute various external accelerations and vibrations, thereby simulating different system conditions. An MARG unit is attached to the end of the defined kinematic chain of joints; therefore, the environment simultaneously supplies the raw MARG sensor data, along with the true pose (position and orientation) states, during the motion of the system. Moreover, the test environment includes an artificial magnetic perturbation algorithm to generate realistic magnetic disturbance effects during the execution of different test scenarios. This complete environment allows to simulate various real word scenarios and thereby enables both the development and testing of any filter structure. Additionally, the tuning of filter parameters can easily be performed, since the true states of the system are provided in the environment, and even numerical optimization can be employed to obtain maximized filter performance. The author made both the ROS/Gazebo and MATLAB/Simulink implementations publicly available in the Supplementary Online Material [34] in order to help other lab teams in the development of MARG-based algorithms. The proposed test environment has been employed during the development of an attitude estimator algorithm in an earlier work [1]. However, the main components (i.e., the structure of the environment, test bench properties, derivation of models, perturbation algorithms, and applied controllers) which form this comprehensive environment have not been published yet.

The remainder of the paper is organized as follows. Section 2 describes the complete elaboration of the test environment, from system equations, over control synthesis and MARG unit modeling, to implementation results. Section 3 gives a case study on the effective evaluation of attitude filters with the help of the proposed test platform. Finally, Section 4 provides the conclusions and recommendations for future studies.

2. Test Environment

To be able to effectively develop, test, and evaluate different relative localization-type algorithms, a test environment is required to be designed. This test environment should both allow to simulate various realistic system behaviors (i.e., static behaviors, dynamic conditions, and external perturbations) and contain realistic sensor models which include manufacturing errors and noise sources. Additionally, the test environment should provide the real system states (ground truth position and orientation), along with the raw, noisy, uncalibrated sensor data, during the execution of different system conditions. These features both enable the evaluation of filter performances and foster the development of novel techniques.

A test environment that satisfies the aforementioned requirements is depicted in Figure 1. This environment contains a 6 DOF platform that alters the pose (both position and orientation) of an MARG unit in the 3D space. The closed-loop structure simulates dynamic circumstances, i.e., the 6 DOF platform executes the desired mixture of static and dynamic system behaviors based on the supplied reference signals. Since both the system dynamics and sensor models are included in this environment; therefore, the acquisition of true system states (true position and orientation of the MARG frame), along with raw sensor data, is supported. These measurements contributes to post-processing, such as the analysis of dynamic effect on estimation process, filter performance quantification (i.e., state estimation error determination), and optimization of filter parameters. These attributes form the main advantages of the proposed environment, which has been implemented in both ROS/Gazebo and MATLAB/Simulink. Moreover, video demonstrations of the closed-loop dynamics, along with the ROS/MATLAB-based program packages, have been made publicly available on the author’s website [34].

Figure 1.

Structure of the test environment in ROS/Gazebo.

The 6 DOF test bench consists of three prismatic joints and three revolute joints. The prismatic joints maintain the desired spatial coordinates of the MARG unit. Moreover, by sliding back and forth, up, and down the sensor frame in the 3D space different accelerating system conditions are simulated via these joints. The revolute joints control the orientation (instantaneous Euler angles) of the sensor frame. These joints can execute both fast and slow rotation motion to simulate vibration and oscillation effects. A plate is attached to the end of this kinematic chain which contains the MARG unit (see the small orange sensor block in Figure 1). Therefore, this 6 DOF platform enables the control and measurement of the pose of sensor frame; moreover, the implemented sensor models provide the instantaneous measurements (raw sensor data) related to the system dynamics simultaneously. This comprehensive framework supplies the necessary environment and data to both generate various system conditions (dynamic spatial motion, along with external accelerations and sensor frame vibrations) and access the raw sensor measurements, along with the true system states, for post-processing and analysis. In the first row of Figure 2, three snapshots of the 6 DOF platform are depicted during the motion in the 3D space; moreover, the generated instantaneous accelerometer, gyroscope, and magnetometer measurements are highlighted in the second row.

Figure 2.

(a–c) Snapshots during the motion of the 6 six-degrees of freedom (6 DOF) platform; (d–f) raw measurements of the attached magnetic, angular rate, and gravity (MARG) unit.

2.1. Model Equations

Table 1 describes the main parameters of the model. The system dynamics is obtained with the help of the Lagrange-equations [35]:

$$\frac{d}{dt}\frac{\partial \mathcal{L}}{\partial \dot{q}}-\frac{\partial \mathcal{L}}{\partial q}=\tau ,$$ (1)

where q denotes the vector of generalized coordinates, and $\mathcal{L}$ defines the Lagrange function. The Lagrange function is defined as the difference of kinetic and potential energies, i.e., $\mathcal{L}=K-P$. The total kinetic energy of the system is composed of the kinetic energy resulting from translational motions ($T_t$, executed by the prismatic joints) and rotational energy ($T_r$, generated by the revolute joints) resulting from the oscillation of the body plate, i.e.,

$$\begin{array}{cc}\hfill T_t& =\frac{1}{2}(\left(m_{j,1}+m_b\right){\dot{x}}_b^2+\left(m_{j,2}+m_b\right){\dot{y}}_b^2+\left(m_{j,3}+m_b\right){\dot{z}}_b^2),\hfill \\ \hfill T_r& =\frac{1}{2}\left(J_{b,\varphi }{\dot{\varphi }}^2+J_{b,\theta }{\dot{\theta }}^2+J_{b,\psi }{\dot{\psi }}^2\right).\hfill \end{array}$$ (2)

Table 1.

Notation of basic test bench parameters.

Parameter	Symbol (Unit)	Value
Mass of link 1 and 2	$m_{j,1}$, $m_{j,2}$ (kg)	500
Mass of link 3	$m_{j,3}$ (kg)	200
Mass of the body plate	$m_b$ (g)	200
Inertia of the body plate ($\varphi$, $\theta$)	$J_{b,\varphi }$, $J_{b,\theta }$ (kg${\mathrm{mm}}^2$)	60
Inertia of the body plate ($\psi$)	$J_{b,\psi }$ (kg${\mathrm{mm}}^2$)	40
Viscous friction coefficient (joint 1–3)	$f_1$, $f_2$, $f_3$ (Ns/m)	100
Viscous friction coefficient (joint 4–6)	$f_4$, $f_5$, $f_6$ (Nms/rad)	$0.001$
Static friction (joint 1–3)	${\tau }_{s,1}$, ${\tau }_{s,2}$, ${\tau }_{s,3}$ (N)	0
Static friction (joint 4–6)	${\tau }_{s,4}$, ${\tau }_{s,5}$, ${\tau }_{s,6}$ (Nm)	0

By introducing the vector of generalized coordinates as $q={\left(x_b,y_b,z_b,\varphi ,\theta ,\psi \right)}^T$, the total kinetic energy is formulated in a compact form as:

$$\begin{array}{cc}\hfill T& =T_t+T_r=\frac{1}{2}{\dot{q}}^T{M}_{\mathrm{mass}}\dot{q},\hfill \\ \hfill M_{\mathrm{mass}}& =diag\left({\left(m_j^T+I_{1\times 3}{m}_b,J_b^T\right)}^T\right),\hfill \end{array}$$ (3)

where I denotes the identity matrix of size given in subscript, and $m_j={\left(m_{j,1},m_{j,2},m_{j,3}\right)}^T$, $m_{j,i}$ is the mass of the ith link, $i=\{1,2,3\}$, while $m_b$ and $J_b={\left(J_{b,\varphi },J_{b,\theta },J_{b,\psi }\right)}^T$ indicate the mass and moment of inertia of the body plate, respectively. The potential energy stored in the system is:

$$\begin{array}{cc}\hfill P=& \left(m_b+\sum _i^3{m}_{j,i}\right)g{h}_0+\left(m_b+m_{j,3}\right)g{z}_b-m_{j,3}g{h}_1,\hfill \end{array}$$ (4)

where $h_0$ is the base height, and $h_1$ denotes the distance between the body plate and third prismatic joint (see Figure 1). Based on Equations (3) and (4), the Lagrange function of the system $\mathcal{L}$ is derived.

The vector of generalized external forces ($\tau$ in Equation (1)) is defined as $\tau ={\left({\tau }_1,\cdots ,{\tau }_6\right)}^T$, where ${\tau }_i$ denotes the generalized force acting on the ith joint, $i=1,\dots ,6$. The generalized external force on each joint consists of the external torque ${\tau }_{a,i}$ produced by the joint driver (motor) and the effect of friction ${\tau }_{f,i}$, i.e., ${\tau }_i={\tau }_{a,i}-{\tau }_{f,i}$. Each joint is characterized by both static and viscous (damping) frictions; therefore, the friction is modeled with sliding and stuck state transitions as follows. Initially, the friction state is set to the stuck condition, and state transition occurs at the ith joint only if the transmitted motor torque is bigger than the static torque ${\tau }_{s,i}$, i.e.,

$$\left|{\tau }_{a,i}\right|>{\tau }_{s,i}.$$ (5)

If the above condition is satisfied, then the sliding mode is activated and viscous friction acts on the joints, i.e.,

$${\tau }_f=diag\left(f_1,\cdots ,f_6\right)\dot{q},$$ (6)

where $f_i$ denotes the viscous friction coefficient for both prismatic and revolute joints (see Table 1). If the ith motor torque becomes smaller than the static friction, moreover, the linear velocity (prismatic joints) or angular velocity (revolute joints) is zero, i.e.,

$$\left|{\tau }_{a,i}\right|\le {\tau }_{s,i}\wedge {\dot{q}}_i=0,$$ (7)

then the model switches back to stick condition and static friction is activated at the ith joint as:

$${\tau }_{f,i}=sign\left({\tau }_{a,i}\right)·min\left(\left|{\tau }_{a,i}\right|,{\tau }_{s,i}\right).$$ (8)

The evaluation of Equation (1) results in the motion equations of the system in the following form:

$$M\left(q\right)\ddot{q}+V(q,\dot{q})={\tau }_a-{\tau }_f,$$ (9)

where $M\left(q\right)=M_{\mathrm{mass}}$ denotes the 6-by 6 symmetric and positive definite inertia matrix given in Equation (3), while $V(q,\dot{q})={\left(0_{1\times 2},(m_b+m_{j,3})g,0_{1\times 3}\right)}^T$ is the 6-dimensional vector including the potential (gravity) force terms, where 0 denotes the zero vector of the given size. Based on Equation (9), the state-space representation $\dot{x}\left(t\right)=h\left(x,u\right)$ of the plant is obtained. The state vector is $x_{12\times 1}={(q,\dot{q})}^T$ and the state-space equation is given as:

$$\begin{array}{cc}\hfill \dot{x}\left(t\right)& =\left[\begin{array}{c}\dot{q}\\ M{\left(q\right)}^{-1}\left({\tau }_a-{\tau }_f-V(q,\dot{q})\right)\end{array}\right],\hfill \\ \hfill y\left(t\right)& =x\left(t\right).\hfill \end{array}$$ (10)

2.2. Control Synthesis

Proportional-integral-derivative (PID) controllers maintain the position regulation of the 6 DOF test bench. The prismatic joints are actuated with force inputs, while torque signals are supplied to the revolute joints. As a result, six force/torque action-type controllers are applied to perform both translational and rotation motion with the test bench in the 3D space. The control requirements are summarized in Table 2. These requirements ensure the execution of extensive dynamic motions, which inherently results in the simulation of external disturbances in the following ranges: $\pm 30\mathrm{g}$ for external accelerations in the 3D space and $\pm 40\mathrm{rad}/\mathrm{s}$ range for angular rates to simulate extensive body plate oscillations. The external accelerations are generated by the prismatic joints during the translational motion in the 3D space. Since these joints maintain the position regulation of the MARG unit, therefore, such settling time and overshoot requirements were selected for these joints that generate multiple external acceleration peaks during the position tracking. The body plate oscillations are executed by revolute joints. These joints maintain the angular position of the MARG unit (roll, pitch, and yaw angles). Since the test bench intends to simulate oscillations and vibrations, therefore, both a magnitude smaller settling time is specified and bigger overshoot is allowed for these joints. The control task was to design such position controllers, which satisfy ${lim}_{t\to T_s}q\left(t\right)=q_d$ at given maximum overshoot ($\Delta v$), where $q_d$ denotes the desired position, and $T_s$ is the settling time.

Table 2.

Control requirements related to the desired system dynamics.

Parameter	Symbol (Unit)	Value
External acceleration executed by joint 1–3	${\alpha }_{ext,1,2,3}$ (g)	30
Oscillation rate executed by joint 4–6	${\Omega }_{4,5,6}$ (rad/s)	40
Overshoot for joint 1–3	$\Delta v_{1,2,3}$ (%)	20
Settling time for joint 1–3	$T_s$ (s)	1
Overshoot for joint 4–6	$\Delta v_{4,5,6}$ (%)	40
Settling time for joint 4–6	$T_s$ (s)	$0.3$

Since the 6 DOF test platform is characterized by fully decoupled dynamics, therefore, each joint controller can be designed separately. The design of these position controllers is based on a third-order reference system ($H_d$), which describes the desired dynamics from the input (reference joint coordinate $q_d$) to the output (realized joint coordinate q) in closed-loop. First, the damping ratio ($\xi$) and natural frequency (${\omega }_n$) of a second-order reference system is obtained based on the defined overshoot and settling time values. The dynamics in time domain is given as:

$$\begin{array}{cc}\hfill \ddot{q}& +2\xi {\omega }_n\dot{q}+{\omega }_n^{2}q={\omega }_n^2{q}_d,\hfill \\ \hfill \Delta v& =e^{-\frac{\pi \xi }{\sqrt{1-{\xi }^2}}},T_s\approx \frac{4}{\xi {\omega }_n}.\hfill \end{array}$$ (11)

Applying the Laplace transformation, the transfer function of the second order system is obtained:

$$H=\frac{{\omega }_n^2}{s^2+2\xi {\omega }_{n}s+{\omega }_n^2},$$ (12)

where s is the Laplace variable. This second order system is extended with a first order dynamics, thereby forming the third-order reference system for the ith joint in the following form [36,37]:

$$\begin{array}{c}\hfill H_{d,i}=\frac{C_i{G}_{p,i}}{1+C_i{G}_{p,i}}=\frac{Q_i\left(s\right)}{Q_{d,i}\left(s\right)}=\frac{{\omega }_n^2}{\left(1+Ts\right)\left(s^2+2\xi {\omega }_{n}s+{\omega }_n^2\right)},\end{array}$$ (13)

where $C_i$ and $G_{p,i}$ denote the controller and plant transfer functions, respectively. For the position control problem, PID-type controllers are applied with feed forward-based gravitation acceleration compensation. Namely, for the ith joint:

$$\begin{array}{cc}\hfill {\tau }_{a,i}=& K_{P,i}\left(q_{d,i}-q_i\right)+K_{I,i}{\int }_0^t\left(q_{d,i}-q_i\right)d\xi -K_{D,i}{\dot{q}}_i-V\left(q_i,{\dot{q}}_i\right).\hfill \end{array}$$ (14)

As a result, the closed-loop dynamics of the ith joint is given as:

$$\begin{array}{cc}\hfill & M_i{\ddot{q}}_i+f_i{\dot{q}}_i=K_{P,i}\left(q_{d,i}-q_i\right)+K_{I,i}{\int }_0^t\left(q_{d,i}-q_i\right)d\xi -K_{D,i}{\dot{q}}_i,\hfill \end{array}$$ (15)

where $M_i$ denotes the ith element of the mass matrix in Equation (3), and $q_{d,i}$ indicates the desired position for the ith joint. By differentiating Equation (15), the equivalent third-order system is obtained, and the PID controller parameters can be derived:

$$\begin{array}{cc}\hfill q_{d,i}& =\frac{M_i}{K_{I,i}}{\stackrel{⃛}{q}}_i+\frac{f_i+K_{D,i}}{K_{I,i}}{\ddot{q}}_i+\frac{K_{P,i}}{K_{I,i}}{\dot{q}}_i+q_i,\hfill \\ \hfill H_{r,i}& =\frac{Q_i\left(s\right)}{Q_{d,i}\left(s\right)}=\frac{K_{I,i}}{M_i{s}^3+\left(f_i+K_{D,i}\right)s^2+K_{P,i}s+K_{I,i}}.\hfill \end{array}$$ (16)

The derivation of $K_{P,i}$, $K_{I,i}$, and $K_{D,i}$ parameters for the ith PID controller is straightforward. Based on the comparison of the reference system $H_{d,i}$ in Equation (13) and realized system $H_{r,i}$ in Equation (16), one obtains:

$$\begin{array}{cc}\hfill K_{I,i}& =\frac{{\omega }_n^2}{T}{M}_i,\hfill \\ \hfill K_{P,i}& =\left({\omega }_n^2+\frac{2\xi {\omega }_n}{T}\right)M_i,\hfill \\ \hfill K_{D,i}& =\left(2\xi {\omega }_n+\frac{1}{T}\right)M_i-f_i.\hfill \end{array}$$ (17)

Since the extra time constant in the third-order reference system ($H_d$) results in slower closed-loop dynamics than the desired response, therefore, the damping ratio ($\xi$) and natural frequency (${\omega }_n$) parameters may require slight tuning after implementation.

Figure 3 highlights the closed loop dynamics achieved with the derived controllers. In the first row, the reference tracking performance is highlighted for both the prismatic and revolute joints. It can be observed that the prismatic joints maintain the desired $x_{b,d}$, $y_{b,d}$, and $z_{b,d}$ spatial coordinates and fulfill the control requirements. Namely, the settling time for each actuated joint is less than 1 s; moreover, the reference values are tracked with reasonable overshoot (the overshoot is a must have requirement, since it results in additional external acceleration, which can be incorporated into the quality verification of estimation algorithms). In the experimental results, the reference values are supplied with approximately 1-s period for the prismatic joints, while the revolute joints are required to track $2\mathrm{Hz}$, $0.5\mathrm{Hz}$, and $5\mathrm{Hz}$ sinusoidal signals with different amplitudes to maintain the desired roll, pitch, and yaw angles.

Figure 3.

Reference tracking performance of the 6 DOF test bench in closed-loop.

The right side of the first row highlights the performance of actuated revolute joints. Since these joints intend to simulate different oscillation magnitudes; therefore, the joints are characterized by faster dynamics. Based on the figure, it can be concluded that both high and low frequency oscillations are successfully realized by these joints. The reference joint coordinates (desired roll ${\varphi }_d$, pitch ${\theta }_d$, and yaw ${\psi }_d$ angles) are tracked with acceptable performance; therefore, the derived PID controllers successfully satisfy the control requirements. In the second row of Figure 3, the generated external accelerations and oscillation rates are highlighted during the motion of the test bench. The external accelerations are in the $\pm 60\mathrm{g}$ range, while the angular rates appear in the $\pm 10\mathrm{rad}/\mathrm{s}$ range in this experiment. These values cover an extensive range of dynamic motions, from slow human motion tracking problems to the dynamics of agile mechatronic systems characterized by vibrations, fast maneuvers, and a variety of working conditions. These experimental results prove that the proposed test environment is capable of simulating various system behaviors, which enables the evaluation of different real world scenarios.

Figure 4 depicts the spatial motion of the linear joints during the aforementioned experiment. The blue curve highlights the reference trajectory ($x_{b,d}$, $y_{b,d}$ and $z_{b,d}$), while the red curve shows the realized trajectory during the 3 s-long experiment. An apparent difference can be observed between the blue and red curves, due to the fact that the new desired spatial coordinates have been supplied earlier than the settling of the prismatic joints controllers. Moreover, the desired spatial coordinates are supplied in different time instances; therefore, even if one prismatic joint controller settles, the other two controllers are either in the rising time phase or overshoot state, thereby resulting in discrepancy in either one or two coordinates. However, this discrepancy in trajectory tracking does not influence the task and goal of the environment at any manner, since every control requirement is satisfied; moreover, the test environment enables the generation of different dynamic conditions in the 3D space.

Figure 4.

Reference and realized trajectories of the 6 DOF test bench in closed-loop.

2.3. MARG System

The 6 DOF test bench performs various dynamic motions, where the pose of its body plate is altered continuously to simulate both oscillations and external acceleration events. An MARG unit model is attached to the body plate, which provides raw, noisy sensor data during the motion of the system.

The model of MARG unit provides the raw gyroscope ${\Omega }_k$, accelerometer $A_k$, and magnetometer $H_k$ measurements in each epoch k according to the following equations [38]:

$$\begin{array}{cc}\hfill {\Omega }_k& =\left(I+\Delta S_{\Omega }\right)M_{\Omega }{\omega }_k+{\overline{\omega }}_k+{\mu }_k,\hfill \\ \hfill A_k& =\left(I+\Delta S_A\right)M_A\left({\alpha }_k+g_k\right)+a_0+{\nu }_k,\hfill \\ \hfill H_k& =\left(I+\Delta S_H\right)M_H\left(B_{si}{h}_k+b_{hi}\right)+h_0+{ϵ}_k.\hfill \end{array}$$ (18)

Each sensor model in Equation (18) is characterized by scale factor and misalignment errors. These errors represent the imperfection of manufacturing by the corresponding matrices $\Delta S$ and M. Moreover, each sensor model provides the measurements as the sum of the real physical quantity, bias term (${\overline{\omega }}_k$, $a_0$ or $h_0$), and Gaussian additive measurement noise (${\mu }_k$, ${\nu }_k$ or ${ϵ}_k$). In case of the gyroscope sensor, the real physical quantity is the angular velocity ${\omega }_k$ of the body plate, while the sum of gravitational $g_k$ and external acceleration ${\alpha }_k$ constitute the physical quantity of the accelerometer sensor. Additionally, the physical quantity of the magnetometer model is the local magnetic field vector $h_k$ disturbed by magnetic soft iron $B_{si}$ and hard iron $b_{hi}$ errors. These magnetometer errors are compensated for in self-calibration procedures [39,40,41].

The aforementioned MARG model enables the generation of realistic raw MEMS sensor data during the motion of the 6 DOF test bench. Therefore, the effects of both external accelerations and sensor frame oscillations are appropriately represented by ${\Omega }_k$, $A_k$, and $H_k$ sensor outputs.

2.4. Magnetic Perturbation Generator

Magnetic fluctuations cannot be generated by Equation (18) in the simulation environment. However, random magnetic perturbations commonly occur in real world scenarios, where the magnetometer output gets disturbed by ferromagnetic objects [42]. Therefore, an essential requirement is to simulate random magnetic fluctuations in the proposed test environment.

An additional algorithm is utilized to generate these artificial magnetic perturbations. The algorithm calculates the output (i.e., the magnetic perturbation) in three steps as follows.

$$\begin{array}{cc}\hfill m_k& =r_1,k=s_1,\dots ,s_1+L_w,\hfill \\ \hfill m_k& =\frac{r_2}{L_w}\left(\left(k-s_2\right)mod{L}_w\right),k=s_2,\dots ,s_2+L_w,\hfill \\ \hfill m_k& =r_{3}sin\left(2\pi f_1\frac{k-s_3}{L_w}\right),k=s_3,\dots ,s_3+L_w,\hfill \\ \hfill m_k& =r_{4}sin\left(2\pi f_2\frac{k-s_4}{L_w}\right),k=s_4,\dots ,s_4+L_w,\hfill \\ \hfill m_k& =\frac{r_5}{\pi }{sin}^{-1}\left(sin\left(\pi \frac{k-s_5}{L_w}\right)\right),k=s_5,\dots ,s_5+L_w,\hfill \end{array}$$ (19)

$$\begin{array}{cc}\hfill {\mathcal{M}}_l& =\sum _{k=1}^{L_m}{m}_k{e}^{-j\left(\frac{2\pi l\left(k-1\right)}{L_m}\right)},l=1,\dots ,L_m,\hfill \\ \hfill h_l& =1,l=1,\left(\frac{L_m}{2}+1\right),\mathrm{and}{h}_l=2,l=2,\dots ,\frac{L_m}{2},\hfill \\ \hfill m_{a,k}& =m_{r,k}+j{m}_{i,k}=\frac{1}{L_m}\sum _{l=1}^{L_m}{\mathcal{M}}_l{h}_l{e}^{j\left(\frac{2\pi k\left(l-1\right)}{L_m}\right)},k=1,\dots ,L_m.\hfill \end{array}$$ (20)

1. Generate an artificial signal by combining five fundamental waveforms. In this algorithm, a square signal, a sawtooth wave, two sinusoidal signals and a triangle wave are combined to generate the artificial signal m of length $L_m=\Delta t{f}_s$, where $\Delta t$ denotes the time window, in which magnetic perturbation is to be generated, while $f_s$ is the sampling frequency. Namely, initialize $m_k=0$ for $k=1,\dots ,L_m$, and then let $L_w$ denote the length of each waveform; moreover, a vector $s=\left(s_1,\dots ,s_5\right)$ containing the start locations of each waveform is generated via a random permutation. As a result, the artificial signal (m) is produced with Equation (19), where $r_i$ is the random amplitude of the ith waveform, $i=1,\dots ,5$, while $f_1$ and $f_2$ denote the random frequencies of the sinusoidal signals in $\left[0.5,2.5\right]\mathrm{Hz}$ range. The first row of Figure 5 shows the generated artificial signal, where $\Delta t=2$ s, $f_s=200\mathrm{Hz}$ and $L_w=L_m/10$.

2. Use the artificial signal (m) and calculate the analytic signal $m_{a,k}=m_{r,k}+j{m}_{i,k}$, $k=1,\dots ,L_m$, where $m_{r,k}$ denotes the real part and is the original signal, while $m_{i,k}$ represents the imaginary part, and is the original signal with $\pi /2$ phase shift, obtained via Hilbert transformation. In this step, the fast Fourier transform (FFT) $\mathcal{M}$ of m is calculated, a zero vector h of length $L_m$ is created and offset (scaling) values are applied, and, finally, the inverse FFT of the element-wise product of m and h is obtained [43]; see Equation (20). Then, the artificial perturbation $m_p$ is generated as the sum of the imaginary part $m_i$ and absolute value of the analytic signal $m_a$, where the sequence of absolute values is reversed in time (see the outcome of this step in the second row of Figure 5):

   $$m_{p,k}=m_{i,k}+\left|m_{a,l}\right|,$$ (21)
3. Extract the continuous linear trend of the artificial perturbation signal by subtracting a straight line ${\widehat{m}}_p$ from $m_p$, where ${\widehat{m}}_p$ denotes the best linear fit to $m_p$ in the least squares sense:

   $$\Delta m_p=m_p-{\widehat{m}}_p,\sum _{k=1}^{L_m}{\left(m_{p,k}-{\widehat{m}}_{p,k}\right)}^2\to \min .$$ (22)
   Then, apply low-pass filtration to the detrended signal ($\Delta m_p$) via a second order Butterworth infinite impulse response (IIR) filter to obtain the output ($\tilde{m}$):

   $${\tilde{m}}_k=\sum _{i=1}^2{a}_i{\tilde{m}}_{k-i}+\sum _{j=0}^2{b}_j\Delta m_{p,k-j},$$ (23)

   where $k=1,\dots ,L_m$; moreover, $a_i$ and $b_i$ filter coefficients are obtained based on the cutoff frequency of the filter. In this algorithm, the cutoff frequency was selected to be $5\mathrm{Hz}$. The output of the algorithm is shown in the third row of Figure 5.

Figure 5.

Artificial magnetic perturbation algorithm.

Figure 5 highlights that the aforementioned algorithm produces realistic magnetic perturbation signals; therefore, it can be effectively used in the test environment to generate various real world scenarios, where the magnetometer data is disturbed due to environmental changes, such as the effect of ferromagnetic materials. Figure 6 illustrates the application of the proposed algorithm on raw magnetometer data. The blue curves indicate the raw undisturbed magnetometer measurements, while the red curves indicate particular sections, where the measurement was disturbed by the proposed algorithm. It can be seen that the algorithm altered the raw sensor signals three times during the 15 s-long measurement. In the last row of Figure 6 the magnitude of magnetometer data is shown, where the red curve highlights the effect of the perturbation algorithm (i.e., the norm of measurements has increased to approximately five times of the raw data norm during the perturbation events). These figures demonstrate that the algorithm provides useful output for simulating random realistic magnetic perturbations.

Figure 6.

Magnetometer data before (blue curves) and after (red curves) the artificial magnetic perturbations.

2.5. Implementation Results

The proposed 6-DOF test bench, along with its closed-loop architecture, MARG unit model, and magnetic perturbation algorithm, as a complete simulation environment, has been implemented in both MATLAB/Simulink and ROS/Gazebo. These implementations have been made publicly available in the Supplementary Online Material [34] with the aim to share the test environment with other researchers who’s work is focused on MEMS MARG unit-based algorithms. Appendix A and Appendix B describe the complete procedure of the usage of the proposed test environment.

Regarding the MATLAB-based implementation, the state-space dynamics of the 6 DOF test platform (Equation (10)) has been implemented with a Simulink S-function block. Each joint is controller in closed-loop; therefore, six position input effort output-type controllers have been implemented (Equation (14)) to maintain the desired joint coordinates. The PID parameters have been obtained using Equation (17). The MARG unit is connected to the end of the kinematic chain and is composed of accelerometer and gyroscope Simulink blocks, along with a custom-made magnetometer model (see Equation (18)). In the current implementation, it is assumed that the sensor models are calibrated (bias terms are zero vectors); moreover, zero scaling and no misalignment are considered in Equation (18). However, these values can easily be modified in the corresponding Simulink sensor blocks, if more advanced problems need to be examined. Figure 7 shows the implemented model in MATLAB/Simulink.

Figure 7.

The test environment in MATLAB/Simulink.

In ROS/Gazebo environment, the dynamics of the 6 DOF test bench is implemented in a Universal Robotic Description Format (URDF) file [44]. This XML file describes all elements of the test environment, from kinematic and dynamic properties (mass, inertia, friction parameters, and joint ranges), over collisions to frame positions and velocities. The derived PID controllers have been implemented and loaded via the controller manager ROS package. Effort action-based controllers are defined to drive the joints in closed-loop. The ros controllers meta-package provides the environment to implement these low level controllers. As it is given in Equation (14), these controllers are driven with desired position inputs and supply torque outputs to the joints. The state vector $x={\left(q,\dot{q}\right)}^T$ is obtained by the joint state controller, which is a sensor controller and supplies the true states of the system continuously (true joint positions, velocities, and joint efforts without measurement noise). The MARG unit is connected to the body plate of the test platform and is implemented with Gazebo sensor plugins [45]. The positions, frames, and noise properties of these sensors are defined in the aforementioned URDF file. Figure 1 shows the implemented model in ROS/Gazebo environment.

3. Case Study: Evaluation and Tuning of Attitude Filters

This section intends to show that the proposed environment fosters the effective analysis of MARG-based algorithms. Namely, the test platform enables (i) to observe the impact of different dynamic conditions on the estimation performance, (ii) the tuning of filter parameters, (iii) to detect robustness issues, and, finally, (iv) the complete development and evaluation MARG-based algorithms.

The case study demonstrates the performance evaluation and tuning of attitude filters in the following subsections. As it was discussed in the introduction, the task of attitude estimators is to provide both accurate and robust estimation results even if external disturbances (external acceleration, vibration, and magnetic perturbation) occur during the motion of the dynamical system. Therefore, the performance of these filters can be tested in such environment where the aforementioned disturbances commonly appear with different magnitudes and frequencies. The proposed test environment is completely suitable for the simulation of these system conditions.

3.1. Algorithms

Three popular attitude filters are discussed in the analysis, namely the ECF [19], GDA [20], and standard extended Kalman filter (EKF) [46] algorithms are evaluated with the help of the test environment. The implementations of these algorithms are widely used and available in both ROS/Gazebo [22,47,48] and MATLAB/Simulink environments [49,50]. Since the algorithms have been described in detail in earlier works [1,2,4,5,32,51,52], therefore, only the key steps of the algorithms are introduced as follows.

3.1.1. Explicit Complementary Filter

The algorithm in Reference [19] derives a quaternion-based CF, in which the error between the measured and estimated directions of gravity and magnetic field vectors is calculated. Then, a PI controller is used to correct the gyroscope signal, which finally drives the quaternion propagation:

$$\begin{array}{cc}\hfill e_k& ={}^S{A}_k\times {}^S\widehat{A}\left({\widehat{q}}_k\right)+{}^S{H}_k\times {}^S\widehat{H}\left({\widehat{q}}_k\right),\hfill \\ \hfill {\widehat{\Omega }}_k& ={\Omega }_k+K_p{e}_k+K_i\sum _{i=1}^k{e}_i{T}_s,\hfill \\ \hfill {\widehat{q}}_{k+1}& ={\widehat{q}}_k+\frac{T_s}{2}Q\left({\widehat{q}}_k\right)\left[\begin{array}{c}0\\ {\widehat{\Omega }}_k\end{array}\right],\hfill \end{array}$$ (24)

where $T_s$ is the sampling period, ${}^S{A}_k$ and ${}^S{H}_k$ are the measurements in the sensor frame, and ${}^S{\widehat{A}}_k={}_{\mathcal{E}}^S{\widehat{q}}_k\otimes {}^{\mathcal{E}}g\otimes {}_{\mathcal{E}}^S{{\widehat{q}}_k}^{*}$ and ${}^S{\widehat{H}}_k={}_{\mathcal{E}}^S{\widehat{q}}_k\otimes {}^{\mathcal{E}}h\otimes {{}_{\mathcal{E}}^S{\widehat{q}}_k}^{*}$ denote the estimated vectors, while $Q\left({\widehat{q}}_k\right)$ indicates the quaternion matrix [53]. The derivation of these quaternion-based expressions have been described in detail in an earlier work [1]. The filter is characterized by two parameters, these are the PI controller gains $K_p$ and $K_i$.

3.1.2. Gradient Descent-Based Attitude Filter

The algorithm in Reference [20] also uses quaternion representation of orientation. The accelerometer and magnetometer measurements are employed in a gradient descent algorithm to obtain the quaternion derivative related to the gyroscope measurement error. As a result, a drift corrective step is executed to maintain the gyroscope-based quaternion propagation:

$$\begin{array}{cc}\hfill f_k& =\left[\begin{array}{c}{}^S\widehat{A}\left({\widehat{q}}_k\right)-{}^S{A}_k\\ {}^S\widehat{H}\left({\widehat{q}}_k\right)-{}^S{H}_k\end{array}\right],\hfill \\ \hfill ▿f_k& =J_k^T{f}_k,\hfill \\ \hfill {\widehat{q}}_{k+1}& ={\widehat{q}}_k+T_s\left(\frac{1}{2}Q\left({\widehat{q}}_k\right)\left[\begin{array}{c}0\\ {\Omega }_k\end{array}\right]-\beta \frac{▿f_k}{\parallel f_k\parallel }\right),\hfill \end{array}$$ (25)

where $J_k$ is the Jacobian matrix of the objective function $f_k$, and $\beta$ denotes the learning rate. This algorithm is a single-gain ($\beta$) attitude filter.

3.1.3. Attitude Estimation with Extended Kalman Filter (EKF)

The EKF employs the gyroscope-based quaternion propagation to obtain the a priori state estimate ${\widehat{x}}_k^{-}={\left({\widehat{q}}_k^{-},{\widehat{\overline{\omega }}}_k^{-}\right)}^T$ in the so-called predict phase, and then these estimation results are updated with the accelerometer and magnetometer readings $z_k={\left({}^S{A}_k,{}^S{H}_k\right)}^T$ in the measurement update phase of the algorithm. The state propagation includes both the quaternion ($q_k$) and random walk process of gyro-bias (${\overline{\omega }}_k$), while the update equations are based on the relationship between the reference vectors and sensor measurements:

$$\begin{array}{cc}\hfill {\widehat{q}}_k^{-}& ={\widehat{q}}_{k-1}+\frac{T_s}{2}Q\left({\widehat{q}}_{k-1}\right)\left[\begin{array}{c}0\\ {\Omega }_k-{\widehat{\overline{\omega }}}_{k-1}\end{array}\right],\hfill \\ \hfill P_k^{-}& =\Phi P_{k-1}{\Phi }^T+Q,\hfill \\ \hfill G_k& =P_k^{-}{H}^T{\left(H{P}_k^{-}{H}^T+R\right)}^{-1},\hfill \\ \hfill {\widehat{z}}_k^{-}& ={\left({}^S\widehat{A}\left({\widehat{q}}_k^{-}\right),{}^S\widehat{H}\left({\widehat{q}}_k^{-}\right)\right)}^T,\hfill \\ \hfill {\widehat{x}}_k& ={\widehat{x}}_k^{-}+G_k\left(z_k-{\widehat{z}}_k^{-}\right),\hfill \\ \hfill P_k& =\left(I-G_{k}H\right)P_k^{-}.\hfill \end{array}$$ (26)

In Equation (26), $\widehat{x}$ is the state estimate, and $\Phi$ and H denote the Jacobians of state dynamics and measurement update equations, respectively, while P is the error covariance and G is the Kalman gain. Moreover, Q and R denote the state and measurement noise covariance matrices, which mainly determine the performance of the EKF.

3.2. Experimental Results

In the conducted experiments, the 6 DOF test platform executed various system behaviors in order to evaluate the filter convergences in different scenarios. External accelerations and sensor frame oscillations have been generated by supplying random desired spatial coordinates to the PID controllers of the three prismatic joints. Simultaneously, diverse sinusoidal signals as reference joint coordinates were supplied to the PID controllers of the three revolute joints to maintain a variety of sensor frame oscillations, where both the amplitude and frequency coefficients were randomly generated. As a result, the closed-loop system of the 6 DOF mechanism executed an extensive range of dynamic motions in the 3D space. Moreover, the proposed magnetic perturbation algorithm generated random pose-based disturbance events, as well. During these system motions, the test environment recorded the true system states (ground truth position and orientation of the sensor frame), along with the raw, noisy MARG sensor data. Therefore, the collected database enabled to both evaluate and tune the attitude estimator algorithms.

Figure 8, Figure 9 and Figure 10 show a sample measurement set obtained in the simulation environment. The sampling frequency was set to $f_s=200\mathrm{Hz}$; in the MARG model (see Equation (18)), the standard deviations were defined as follows: $\mu =0.0224\left(\mathrm{rad}/\mathrm{s}\right)/\sqrt{\mathrm{Hz}}$ for the gyroscope model, $\nu =0.0224\left(\mathrm{m}/{\mathrm{s}}^2\right)/\sqrt{\mathrm{Hz}}$ for the accelerometer output, and $ϵ=0.0022\mathrm{nu}/\sqrt{\mathrm{Hz}}$ for the magnetometer readings (normalized unit, nu). The magnetic perturbation algorithm was executed for random $\Delta t=2$ s time windows. The 6 DOF platform performed both static and dynamic motions in the following ranges: oscillations were generated with revolute joints in the $\pm 50\mathrm{rad}/\mathrm{s}$ angular velocity range, $\pm 4\mathrm{g}$ external accelerations were executed by the prismatic joints, and magnetic perturbations were applied in the 0–4 nu disturbance range.

Figure 8.

Initial and tuned performance of the gradient descent-based attitude filter.

Figure 9.

Initial and tuned performance of the explicit complementary filter.

Figure 10.

Initial and tuned performance of the standard extended Kalman filter (EKF).

The MARG sensor data (the output of the simulation environment) drove the attitude filter algorithms (Equations (24)–(26). The outputs of attitude filters were observed; moreover, the tuning of each filter was performed based on the heuristic techniques developed in References [1,2,54]. Figure 8 shows the GDA output, and Figure 9 highlights the performance of the ECF algorithm, while Figure 10 illustrates the output of the EKF algorithm. The first three rows of each figure highlight the ground truth Euler angles (true roll $\varphi$, pitch $\theta$, and yaw $\psi$ angles are indicated with blue curves); moreover, the initial (pink curves) and tuned (red curves) filter performances are shown in these subplots. The fourth row of each figure depicts the magnitude of accelerometer readings; therefore, both the static and dynamic system behaviors can be identified. Additionally, the norm of magnetometer data is depicted in the fifth row, which enables the identification of magnetic disturbance events.

It can be observed that the test environment effectively fostered the tuning procedure of each filter algorithm. The initial filter outputs (pink curves) contained notable estimation errors; therefore, the default filter parameters did not contribute to an acceptable filter performance during the executed dynamic motions. However, the tuning of filter parameters could easily be performed in the environment, which resulted in improved filter performances. The effects of external accelerations, vibrations, and magnetic perturbations can also be observed in the measurement results. These external disturbances significantly decrease the performance of each filter during the first 12 s in the experiment. These effects can be handled more effectively in adaptive filter structures. The analysis of adaptive filters exceeds the scope of this article, but, in fact, the test environment aims to effectively support the development and evaluation of such novel techniques.

The test environment also enables the evaluation of filter convergence in terms of mean squared error (MSE) and standard deviation (STD) of attitude estimation errors ($e_{\varphi ,k}={\varphi }_k-{\widehat{\varphi }}_k$, $e_{\theta ,k}={\theta }_k-{\widehat{\theta }}_k$, and $e_{\psi ,k}={\psi }_k-{\widehat{\psi }}_k$, where ${\widehat{\varphi }}_k$, ${\widehat{\theta }}_k$, and ${\widehat{\varphi }}_k$ denote the outputs, estimated Euler angles, of the implemented filter algorithms in epoch k). Moreover, a cumulated performance index can be defined with Equation (27) to quantify the overall performance (N is the length of the measurement). The evaluation of these performance indexes is summarized for each filter in Table 3. Based on the results, it can be observed that each filter provides significantly enhanced performance with the tuned parameters.

$$\begin{array}{c}\hfill F=\sqrt[3]{\left({\displaystyle \frac{{\sum }_{k=1}^N{e}_{\varphi ,k}^2}{N}}\right)\left({\displaystyle \frac{{\sum }_{k=1}^N{e}_{\theta ,k}^2}{N}}\right)\left({\displaystyle \frac{{\sum }_{k=1}^N{e}_{\psi ,k}^2}{N}}\right)}\end{array}.$$ (27)

Table 3.

Outcome of filter analysis. The bold values indicate the tuned filter performances.

	ECF		GDA		EKF
	Initial	Tuned	Initial	Tuned	Initial	Tuned
F	0.5237	0.1471	1.1950	0.1812	0.2891	0.1033
MSE ($\varphi$)	0.4405	0.1831	1.2361	0.2349	0.2351	0.0856
STD ($\varphi$)	0.6307	0.4275	0.9103	0.4835	0.4848	0.2881
MSE ($\theta$)	0.3713	0.0773	0.5610	0.0977	0.1068	0.0676
STD ($\theta$)	0.5996	0.2756	0.7410	0.3069	0.3184	0.2593
MSE ($\psi$)	0.8784	0.2248	2.4607	0.2589	0.9618	0.1906
STD ($\psi$)	0.6384	0.4476	1.0554	0.4871	0.9774	0.4115

Among the algorithms, the GDA was characterized by the largest performance index ($F=0.1812$) in the experiment (the smaller the value, the better the performance). This outcome can be related to the single (constant) gain property of the algorithm. The dynamic circumstances made the GDA provide large estimation errors around 7 s in Figure 8, which resulted in the biggest performance index in the analysis. The ECF showed a more robust filter performance with $F=0.1471$ performance index. The tuned PI controller enabled the filter to overcome the impact of external disturbances more effectively; however, notable estimation errors can be observed during the dynamic behavior of the system. The EKF algorithm yielded the most robust filter performance during the experiment ($F=0.1033$). This algorithm was characterized by four parameters (i.e., noise statistics related to quaternion propagation, bias walk process, accelerometer observations, and magnetometer measurements); therefore, its flexibility allowed to both effectively suppress the effects of disturbances and provide superior filter performance. However, the performance of EKF algorithm also decreased during the dynamic motions, as can be observed during the first 10-s period. The effective handling of these disturbances is performed by adaptive sensor fusion structures.

This subsection demonstrated the use of the proposed test environment for the analysis and tuning of common attitude filters. The test environment can be universally applied for any type of MARG-based algorithm, where external accelerations, vibrations, and magnetic perturbations commonly occur, and their effect should be carefully addressed (and incorporated) in the development of novel algorithms.

4. Conclusions

This paper presented an universally applicable test environment for development, testing, and performance evaluation of MARG-based filtering techniques, such as pose estimators and classification algorithms. The test environment contains a closed-loop controlled 6 DOF dynamical system to alter the pose of an MARG unit in the 3D space and thereby simulate various system behaviors. Effort action-based joint controllers are employed to execute realistic external accelerations and vibrations by the 6 DOF platform; moreover, an artificial magnetic perturbation algorithm is applied to generate realistic magnetic disturbances. The simulation environment executes the desired dynamical motions and simultaneously supplies the true system states (ground truth values), along with the raw sensor measurements. As a result, the proposed environment provides effective help in the development of filtering techniques, since a big variety of system conditions can be both simulated and evaluated. The environment has been implemented in both ROS/Gazebo and MATLAB/Simulink and is publicly available online. Future work will include the analysis of advanced conditions, where the impact of static friction, sensors frame misalignment, and scaling errors will also be evaluated during the state estimation procedures.

Supplementary Materials

The complete project is available online at http://appl-dsp.com/faekf.

Appendix A. Usage of the package in MATLAB/Simulink

After downloading the package from Reference [34], the start MATLAB file should be executed in the simulink folder. This script loads the system parameters (e.g., inertia and mass properties of the joints, friction coefficients, and PID controller parameters) and opens the Simulink simulation environment (see Figure 7). The researcher can freely change the length of simulation (tsim), sampling time (Tsampling), and time window for magnetic perturbation (dt) parameters to evaluate different dynamic scenarios, as well. Then, the desired position and orientation values should be supplied to the 6 DOF model (see the left side of Figure 7); these reference values will be realized by the dynamic system with the help of the closed-loop driven linear and revolute joints. The desired values can be supplied by different Simulink blocks, i.e., from simple Repeating Sequence Generator block, over Signal Generator block, to custom made signal waves (From Workspace block) can be utilized for the generation of reference values. The properties of MARG system can also be modified by editing the parameters of Gyroscope, Accelerometer, and Magnetometer blocks (see the right side of Figure 7). Namely, the natural frequency and damping ratio parameters, scaling, and misalignment errors, as well as the noise power for each axis (see Equation (18)), can be freely modified for each sensor block to observe different system scenarios. Once the parameters and input reference values are set up, the simulation environment can be executed. The outputs of the simulation are the true system states (position in posXYZ, orientation in angleXYZ, angular velocity in oscRate, linear acceleration in extAccel, and angular acceleration in extAngularAccel) and the realistic MARG sensor data (accelerometer in aMeas, gyroscope in gMeas, and magnetometer in mMeas). These outputs can be used to both effectively evaluate filter algorithms and tune filter parameters.

Appendix B. Usage of the package in ROS/Gazebo

The package [34] should be cloned and built in the catkin workspace. The 6 DOF test environment node is started with the spawn testenv launch file. This launch file uses the URDF file to construct both the kinematic chain and dynamics of the 6 DOF system (see the description in Section 2.5). The researcher can freely change the parameters of the test environment (i.e., from dimension of links, over mass parameters and joint limits, to MARG errors, noise powers, and sampling time) in the testenv xacro file in the urdf folder. Therefore, the aforementioned launch file spawns the dynamic system in Gazebo with the selected parameters; moreover, it adds a PID controller to each joint with the help of the controller manager package. The PID controller parameters can also be altered in the testenv trajectory control yaml file in the config folder. Once the spawning of the test environment is finished, the Gazebo simulator utilizes its physics engines to continuously update the instantaneous system states via ROS topics. The researcher can observe and monitor these instantaneous values in the rqt graphical user interface by subscribing to the joint states, imu raw, and magnetic ROS topics (see both Figure 1 and video demonstrations in Reference [34]). Moreover, the researcher can generate the database of both real system states and raw MARG data with the help of the rosbag tool. Finally, the exec traj node is utilized to supply the desired position and orientation values to the closed-loop architecture of the 6 DOF system. This node creates a ROS client to the follow joint trajectory topic for moving simultaneously the linear and revolute joints according to the selected reference values. In this node, the researcher can freely select the way points for each joint to be realized, thereby defining the custom spatial movements to be executed by the dynamic system. As a result, the PID controllers are supplied by the reference values, and the Gazebo simulator simulates the intended system behavior. The instantaneous measurements are collected in the database during the spatial movements by the rosbag tool; therefore, both the filter evaluation and parameter tuning can be effectively realized.

Funding

This research was funded by the EFOP-3.6.1-16-2016-00003 project, which is co-financed by the European Union. The author was also partially supported by the 2020-4.1.1-TKP2020 program.

Institutional Review Board Statement

Not applicable.

Informed Consent Statement

Not applicable.

Data Availability Statement

Not applicable.

Conflicts of Interest

The author declares no conflict of interest.