*3.3. Kinematics of the Irregular Track*

Figure 3 shows the displacement of the rail heads due to irregularity in a cross-section of the track (*Y<sup>t</sup>* − *<sup>Z</sup><sup>t</sup>* plane). The irregularity vectors*rlir* (*lir*, *l*eft rail *ir*regularity) and*rrir*

(*rir*, *r*ight rail *ir*regularity) describe the displacement of the rail centerlines with respect to their design positions. The components of these vectors in the TF are functions of *s*, given by:

$$\mathbf{r}^{lr} = \begin{bmatrix} 0\\y^{lr} \\ z^{lr} \end{bmatrix}, \quad \mathbf{r}^{ir} = \begin{bmatrix} 0\\y^{ir}\\z^{ir} \end{bmatrix} \tag{6}$$

In the railway industry, the following four combinations of the rail head centerlines irregularities are measured:

$$\begin{array}{ll}\text{Alignment} & \mathfrak{f}\_{al}^{\scriptscriptstyle{\tiny}} = \left(y^{lir} + y^{rir}\right)/2\\\text{Vertical profile:} & \mathfrak{f}\_{vp}^{\scriptscriptstyle{\tiny}} = \left(z^{lir} + z^{rir}\right)/2\\\text{Gauge variation:} & \mathfrak{f}\_{\xi v}^{\scriptscriptstyle{\tiny}} = y^{lir} - y^{rir}\\\text{Cross level:} & \mathfrak{f}\_{\xi l}^{\scriptscriptstyle{\tiny}} = z^{lir} - z^{rir} \end{array}$$

The orientation of the rail head frames with respect to the TF is given by the following rotation matrices:

$$\begin{aligned} \mathbf{A}^{t,lrp} &= \begin{bmatrix} 1 & 0 & 0 \\ 0 & \cos(\beta + \delta) & -\sin(\beta + \delta) \\ 0 & \sin(\beta + \delta) & \cos(\beta + \delta) \end{bmatrix}, \\\ \mathbf{A}^{t,rrp} &= \begin{bmatrix} 1 & 0 & 0 \\ 0 & \cos(-\beta + \delta) & -\sin(-\beta + \delta) \\ 0 & \sin(-\beta + \delta) & \cos(-\beta + \delta) \end{bmatrix}, \end{aligned} \tag{7}$$

where *β* is the orientation angle of the rail profiles, *δ* = *<sup>z</sup>lir* <sup>−</sup> *<sup>z</sup>rir*2*Lr* is the linearized rotation angle due to the irregularity, and *Lr* is the distance from the track centerline to the rail profile frames. These angles and distance can be observed in Figure 3.

The absolute position vectors of two points, *P* and *Q*, defined in the right and left rail heads, respectively, are given by:

$$\begin{aligned} \mathbf{R}\_P^{rrp} &= \mathbf{R}^t + \mathbf{A}^t \left( \mathbf{F}^{rp} + \mathbf{F}^{ir} + \mathbf{A}^{t, rp} \hat{\mathbf{u}}\_P^{rrp} \right) \\\\ \mathbf{R}\_Q^{lrp} &= \mathbf{R}^t + \mathbf{A}^t \left( \mathbf{F}^{lrp} + \mathbf{F}^{lr} + \mathbf{A}^{t, lrp} \hat{\mathbf{u}}\_Q^{lrp} \right) \end{aligned} \tag{8}$$

where **uˆ** *rrp <sup>P</sup>* and **uˆ** *lrp <sup>Q</sup>* contain the components of the position vector of points *P* and *Q* in the rail head profiles as shown in Figure 3. These vectors are parameterized following the rail head profile geometry:

$$\begin{aligned} \mathfrak{d}\_P^{rrp} = \begin{bmatrix} 0\\ s\_2^{rr} \\ h^r(s\_2^{rr}) \end{bmatrix}, \quad \mathfrak{d}\_Q^{lrp} = \begin{bmatrix} 0\\ s\_2^{lr} \\ h^r(s\_2^{lr}) \end{bmatrix} \end{aligned} \tag{9}$$

where *lr* and *rr* stand for "left rail" and "right rail", and *h<sup>r</sup>* is the function that defines the rail head profile.

### *3.4. Kinematics of a Body Moving along the Track*

The coordinates used to describe the position and orientation of an arbitrary body *i*, as shown in Figure 2, or the TGMS shown in Figure 1, moving along the track are:

$$\mathbf{q}^i = \begin{bmatrix} s^i & r^i\_y & r^i\_z & \varrho^i & \theta^i & \psi^i \end{bmatrix}^T \tag{10}$$

where *s<sup>i</sup>* is the arc-length along the track of the position of the body, *r<sup>i</sup> <sup>y</sup>* and *r<sup>i</sup> <sup>z</sup>* are the non-zero components of the position vector *r<sup>i</sup>* of the BF with respect to the TF, this is **r¯***<sup>i</sup>* = 0 *r<sup>i</sup> <sup>y</sup> r<sup>i</sup> z T* , and *ϕ<sup>i</sup>* , *θ<sup>i</sup>* and *ψ<sup>i</sup>* are three Euler angles (roll, pitch and yaw, respectively) that define the orientation of the BF with respect to the TF. Note that, out of the six coordinates used for the kinematic description of a body, only the arc-length coordinate *s<sup>i</sup>* is an absolute coordinate, being the other 5 track-relative coordinates.

The three Euler angles are assumed to be small, such that the following kinematic linearization is used:

$$\mathbf{A}^{t,i} \simeq \begin{bmatrix} 1 & -\psi^i & \theta^i \\ \psi^i & 1 & -q^i \\ -\theta^i & q^i & 1 \end{bmatrix} \tag{11}$$

The absolute position vector of point *P* that belongs to body *i*, as shown in Figure 2, is given by:

$$\mathbf{R}\_P^i = \mathbf{R}^t + \mathbf{A}^t \left(\overline{\mathbf{r}}^i + \mathbf{A}^{t,i} \hat{\mathbf{u}}\_P^i\right) \tag{12}$$

Using basic rigid body kinematics (see details in [17]), the absolute velocity and acceleration of point *P* are given by:

$$\ddot{\mathbf{R}}\_P^i = \ddot{\mathbf{R}}^t + \dot{\mathbf{r}}^i + \tilde{\boldsymbol{\omega}}^t \ddot{\mathbf{r}}^i + \mathbf{A}^{t,i} \left(\tilde{\boldsymbol{\omega}}^i \dot{\mathbf{u}}\_P^i\right) \tag{13}$$

$$\bar{\mathbf{R}}\_P^i = \bar{\mathbf{R}}^t + \mathbb{H}^i + \left(\mathbb{\tilde{a}}^t + \mathfrak{a}^t \mathfrak{a}^t\right)\mathbf{\dot{r}}^i + 2\mathfrak{a}^t\mathbf{\dot{r}}^i + \mathbf{A}^{t,i}\left(\mathbb{\tilde{a}}^i + \tilde{\mathfrak{a}}^i \tilde{\mathfrak{a}}^i\right)\mathbf{\dot{u}}\_{P}^i \tag{14}$$

where these vectors are projected to the track frame. In order to compute Equations (13) and (14), the orientation matrices, angular velocities and angular accelerations of the different frames need to be computed as a function of the generalized coordinates and velocities. The angular velocity *ω*¯ *<sup>t</sup>* and acceleration *α*¯*<sup>t</sup>* vectors of the TF are given in Equation (5). The absolute angular velocity of body *i* is obtained as:

$$
\hat{\omega}^{i} = \hat{\omega}^{t} + \hat{\omega}^{t,i} = \left(\mathbf{A}^{t,i}\right)^{T}\hat{\omega}^{t} + \hat{\omega}^{t,i} \tag{15}
$$

The relative angular velocity of body *i* with respect to the TF, under the small-angles assumption, is given by:

$$
\boldsymbol{\hat{\phi}}^{t,i} = \begin{bmatrix} \boldsymbol{\phi}^{i} \\ \boldsymbol{\theta}^{i} \\ \boldsymbol{\psi}^{i} \end{bmatrix}, \quad \boldsymbol{\tilde{\omega}}^{t,i} = \mathbf{A}^{t,i} \boldsymbol{\hat{\phi}}^{t,i} = \begin{bmatrix} 1 & \boldsymbol{\psi}^{i} & -\boldsymbol{\theta}^{i} \\ -\boldsymbol{\psi}^{i} & 1 & \boldsymbol{\varrho}^{i} \\ \boldsymbol{\theta}^{i} & -\boldsymbol{\varphi}^{i} & 1 \end{bmatrix} \begin{bmatrix} \boldsymbol{\phi}^{i} \\ \boldsymbol{\theta}^{i} \\ \boldsymbol{\psi}^{i} \end{bmatrix} \tag{16}
$$

The absolute angular acceleration of body *i*, *α*ˆ*<sup>t</sup>* is simply calculated as the timederivative of Equation (15).

#### **4. Kinematics of the Computer Vision**

Using the pin-hole model of the camera, Figure 4 shows the relation between the position vector *n im <sup>P</sup>* of an arbitrary point *<sup>P</sup>* in the camera frame <sup>&</sup>lt; *<sup>X</sup>cam*,*Ycam*, *<sup>Z</sup>cam* <sup>&</sup>gt; (in our problem it can be left cam *lcam* or right cam *rcam*) and the position vector of the recorded point *P'* in the image plane < *Xim*,*Yim* >.

**Figure 4.** Kinematics of the computer vision.

Figure 5 shows the location of the camera in the TGMS and the relation between the position vector *v cam <sup>P</sup>* of the arbitrary point *P* in the camera frame and its position vector *u tgms <sup>P</sup>* in the TGMS frame.

**Figure 5.** TGMS frame and camera frame.

The components of vectors *n im <sup>P</sup>* and *u tgms <sup>P</sup>* are related through the equation [18,19]:

$$c\begin{bmatrix} \mathbf{n}\_{P'}^{im} \\ 1 \end{bmatrix} = \mathbf{M}^{int} \mathbf{M}^{ext} \begin{bmatrix} \hat{\mathbf{u}}\_{P}^{tgsus} \\ 1 \end{bmatrix} \tag{17}$$

where *c* is an unknown *scale f actor*. The matrix product on the right-hand side of this equation is called projection matrix **P** = **M***int***M***ext*. The column matrix **n***im <sup>P</sup>* is 2 × 1 (image is planar), and its components are given in pixel units (dimensionless) while the column matrix **uˆ** *tgms <sup>P</sup>* is 3 × 1, and its components are given in meters. Due to these dimensions, the projection matrix **<sup>P</sup>** is 3 × 4. Matrix **<sup>M</sup>***int* is 3 × 3 and it is called matrix of intrinsic parameters of the camera, and matrix **<sup>M</sup>***ext* is 3 × 4, it is called matrix of extrinsic parameters of the camera, and it is given by:

$$\mathbf{M}^{\text{ext}} = \begin{bmatrix} \left(\mathbf{A}^{\text{t}\text{gms}, \text{cam}}\right)^{T} & -\left(\mathbf{A}^{\text{t}\text{gms}, \text{cam}}\right)^{T}\mathbf{\hat{u}}\_{\text{cam}}^{\text{t}\text{gms}} \end{bmatrix} \tag{18}$$

Matrices of intrinsic and extrinsic parameters can be experimentally obtained, for example, using the Zhang calibration method [18]. In this investigation, the calibration method is based on Zhang method but adapted to the application at hand. This method is fully detailed in [19].

Just using the 3 scalar equations in Equation (17), the position vector **uˆ** *tgms <sup>P</sup>* of the point *P* cannot be obtained using the values of **n***im <sup>P</sup>* because there are 4 unknowns (three components of **uˆ** *tgms <sup>P</sup>* and the scale factor *c*). One exception occurs when the point *P* moves on a surface whose equation is known in the TGMS frame. This is the case at hand if *P* belongs to the plane highlighted by the laser projector. In this case, the following system of equations can be solved to find **uˆ** *tgms <sup>P</sup>* :

$$\begin{cases} \begin{bmatrix} \mathbf{n}\_{P'}^{im} \\ 1 \end{bmatrix} = \mathbf{M}^{int} \mathbf{M}^{ext} \begin{bmatrix} \hat{\mathbf{u}}\_{P}^{tgss} \\ 1 \end{bmatrix} \\\ A^{las} \begin{bmatrix} u\_{P}^{tgsm} \\ u\_{P}^{tgsm} \end{bmatrix}\_{x} + B^{las} \begin{bmatrix} u\_{P}^{tgsm} \\ u\_{P}^{tgsm} \end{bmatrix}\_{y} + C^{las} \begin{bmatrix} u\_{P}^{tgsm} \\ u\_{P}^{tgsm} \end{bmatrix}\_{z} + D^{las} = 0 \end{cases} \tag{19}$$

where *Alas*, *Blas*, *Clas* and *Dlas* are the constants that define the laser plane (left laser, *llas*, or right laser, *rlas*, in our case), and *utgms P k k* = *x*, *y*, *z* means the component *k* of the vector *u tgms <sup>P</sup>* in the TGMS frame. The constants that define the laser planes have to be experimentally obtained in the TGMS computer vision-calibration process [19]. Equation (19) is a system of 4 algebraic equations with 4 unknowns that can be used to find vector components **uˆ** *tgms <sup>P</sup>* using as input data the vector components **<sup>n</sup>***im <sup>P</sup>* and the parameters of the cameras and the lasers.

#### **5. Detecting the Rail Cross-Section from a Camera Frame**

As a result of the solution of Equation (19) for all highlighted pixels in the image frames, a cloud of points *Pi* in the right rail and a cloud of points *Qi* in the left rail, with position vectors **uˆ** *tgms <sup>P</sup>* and **uˆ** *tgms <sup>Q</sup>* , respectively, that belong to the rails cross-sections can be identified. In fact, the points do not really belong to cross-sections, just to sections, because the laser planes are not necessarily perpendicular to the rails center line. However, because the relative angles of the TGMS with respect to the TF are very small, the irregularities are also small, and the lasers are set to project the light plane at right angles with respect to the rails, it will be assumed in this paper that the highlighted sections are actually cross-sections.

Figure 6a shows a sketch of the cloud of points and, in dashed line, the theoretical rail-head profile. The theoretical rail-head profiles, when they are new, not worn, have a known geometry that is made of circular and straight segments. An example is the UIC 54 E1 rail-head profile shown in Figure 6b. Detecting the rail cross-section from a camera frame can be solved with the well-developed computer vision algorithms of feature detection or feature tracking [20]. However, in this investigation, because the feature to be detected can be represented analytically, an optimization approach has been developed. The optimization problem consists of finding the position **uˆ** *tgms Orp* and angle *<sup>ϕ</sup>tgms*,*rp* of the rail profile that better fits the cloud of points. This will be the assumed position and orientation of the rail head profile (*lrp* or *rrp*) in the TGMS frame when the vehicle is moving. The objective function to minimize is the sum of the squares of the distance of the points in the cloud to the theoretical profile curve shown in Figure 6b.

**Figure 6.** Cloud of points detected with computer vision (**a**). UIC 54 E1 rail-head profile (**b**).

#### **6. Equations for Geometry Measurement**

The equations that can be used to measure the track irregularities are easily deduced with the help of Figure 7. In this figure, vectors **uˆ** *tgms Olrp* and **uˆ** *tgms Orrp* are output data of the computer vision algorithm explained in previous section. The following equalities can be easily identified with the help of the figure:

$$\begin{aligned} \vec{r}^{t\text{gs}} &+ \vec{u}^{t\text{gs}}\_{Olrp} = \vec{r}^{lrp} + \vec{r}^{lir} \\ \vec{r}^{t\text{gs}} &+ \vec{u}^{t\text{gs}}\_{Orrp} = \vec{r}^{rrp} + \vec{r}^{rir} \end{aligned} \tag{20}$$

**Figure 7.** Planar view of the TGMS.

Subtracting these vector equations, one gets:

$$
\vec{u}\_{Olrp}^{t\text{gms}} - \vec{u}\_{Ormp}^{t\text{gms}} = \vec{r}^{lrp} + \vec{r}^{lir} - \left(\vec{r}^{rrp} + \vec{r}^{rir}\right) \tag{21}
$$

In this equation, the position vector*rtgms* of the TGMS does not appear. This vector equation can be projected in the TF, as follows:

$$\mathbf{A}^{t,t \text{gms}} \left( \hat{\mathbf{u}}\_{\text{Olrp}}^{t \text{gms}} - \hat{\mathbf{u}}\_{\text{Orr}}^{t \text{gms}} \right) = \mathbf{\tilde{r}}^{lr} - \mathbf{\tilde{r}}^{rrp} + \mathbf{\tilde{r}}^{ir} - \mathbf{\tilde{r}}^{ir} \tag{22}$$

Using again the small-angles assumption, the *Y*, *Z* components of this equation are given by:

$$
\begin{bmatrix} 1 & -q^{tgsms} \\ q^{tgsms} & 1 \end{bmatrix} \begin{bmatrix} \left[\dot{u}\_{Olrp}^{tgsm}\right]\_y - \left[\dot{u}\_{Orp}^{tgsm}\right]\_y \\ \left[\dot{u}\_{Olrp}^{tgsm}\right]\_z - \left[\dot{u}\_{Orp}^{tgsm}\right]\_z \end{bmatrix} = \begin{bmatrix} 2L^r \\ 0 \end{bmatrix} + \begin{bmatrix} r\_y^{ir} - r\_y^{ir} \\ r\_z^{ir} - r\_z^{ir} \end{bmatrix} \tag{23}
$$

where *L<sup>r</sup>* is half the distance between the rail-head profiles without irregularities. In this equation, the result **r¯***lrp* <sup>−</sup> **r¯***rrp* <sup>=</sup> <sup>2</sup>*L<sup>r</sup>* <sup>0</sup> *<sup>T</sup>* has been used. According to the definition given in Section 3.3, the components of the last column matrix of Equation (23) are the gauge variation (*gv*) and the cross-level (*cl*). Therefore, rearranging Equation (23) yields:

$$\begin{aligned} \prescript{}{\xi}{\xi}\_{\mathcal{V}} &= \left( \left[ \dot{\boldsymbol{\mu}}\_{\mathrm{Orlp}}^{\mathrm{fgsm}} \right]\_{\boldsymbol{y}} - \left[ \dot{\boldsymbol{\mu}}\_{\mathrm{Orrp}}^{\mathrm{fgsm}} \right]\_{\boldsymbol{y}} \right) - \boldsymbol{\mu}^{\mathrm{fgsm}} \left( \left[ \dot{\boldsymbol{\mu}}\_{\mathrm{Orlp}}^{\mathrm{fgsm}} \right]\_{\boldsymbol{z}} - \left[ \dot{\boldsymbol{\mu}}\_{\mathrm{Orrp}}^{\mathrm{fgsm}} \right]\_{\boldsymbol{z}} \right) - 2L^{r} \\ \prescript{}{\xi}{\boldsymbol{z}}\_{\mathrm{cl}} &= \boldsymbol{\mu}^{\mathrm{fgsm}} \left( \left[ \dot{\boldsymbol{\mu}}\_{\mathrm{Orrp}}^{\mathrm{fgsm}} \right]\_{\boldsymbol{y}} - \left[ \dot{\boldsymbol{\mu}}\_{\mathrm{Orrp}}^{\mathrm{fgsm}} \right]\_{\boldsymbol{y}} \right) + \left( \left[ \dot{\boldsymbol{\mu}}\_{\mathrm{Orrp}}^{\mathrm{fgsm}} \right]\_{\boldsymbol{z}} - \left[ \dot{\boldsymbol{\mu}}\_{\mathrm{Orrp}}^{\mathrm{fgsm}} \right]\_{\boldsymbol{z}} \right) \end{aligned} \tag{24}$$

Adding the vector Equation (20), one gets:

$$2\vec{r}^{t\text{gms}} + \vec{u}\_Q^{t\text{gms}} + \vec{u}\_P^{t\text{gms}} = \vec{r}^{l\text{ir}} + \vec{r}^{r\text{ir}} \tag{25}$$

where the fact that*rlrp* +*rrrp* =-0 has been used. Using again the small-angles assumption, the *Y*, *Z* components of this equation are given by:

$$-2\left[\begin{array}{c}r\_y^{\text{tyms}}\\r\_z^{\text{tyms}}\end{array}\right] + \left[\begin{array}{c}1\\qr^{\text{tyms}}\end{array}\right] + \left[\begin{array}{c}-q^{\text{tyms}}\\q^{\text{tyms}}\end{array}\right] \left[\begin{array}{c}\left[\begin{matrix}\mathfrak{u}\_{\text{Olr}r}^{\text{tyms}}\end{matrix}\right]\_y + \left[\begin{matrix}\mathfrak{u}\_{\text{Ovr}r}^{\text{tyms}}\\\\\mathfrak{u}\_{\text{Olr}r}^{\text{tyms}}\end{matrix}\right]\_z\right] = \left[\begin{array}{c}r\_y^{\text{Ir}} + r\_y^{\text{ir}}\\r\_z^{\text{Ir}} + r\_z^{\text{ir}}\end{array}\right] \tag{26}$$

According to the definition given in Section 3.3, the components of the last column matrix of Equation (26) are twice the alignment irregularity (*ξal*) and twice the vertical profile (*ξvp*). Therefore, rearranging Equation (26) yields:

$$\begin{aligned} \mathbf{x}\_{al}^{x} &= \frac{1}{2} \left( \left[ \hat{\mathbf{u}}\_{Olrp}^{t\text{yms}} \right]\_{y} + \left[ \hat{\mathbf{u}}\_{Orrp}^{t\text{yms}} \right]\_{y} \right) - \frac{q^{t\text{yms}}}{2} \left( \left[ \hat{\mathbf{u}}\_{Olrp}^{t\text{yms}} \right]\_{z} + \left[ \hat{\mathbf{u}}\_{Orrp}^{t\text{yms}} \right]\_{z} \right) + r\_{y}^{t\text{yms}}\\ \mathbf{x}\_{vp}^{x} &= \frac{q^{t\text{yms}}}{2} \left( \left[ \hat{\mathbf{u}}\_{Olrp}^{t\text{ym}} \right]\_{y} + \left[ \hat{\mathbf{u}}\_{Orrp}^{t\text{ym}} \right]\_{y} \right) + \frac{1}{2} \left( \left[ \hat{\mathbf{u}}\_{Orrp}^{t\text{ym}} \right]\_{z} + \left[ \hat{\mathbf{u}}\_{Orrp}^{t\text{ym}} \right]\_{z} \right) + r\_{z}^{t\text{yms}} \end{aligned} \tag{27}$$

Therefore, Equations (24) and (27) can be used to find all track irregularities. The following conclusions are highlighted:


The next section is devoted to the calculation of the TGMS to TF relative orientation, that includes the calculation of *ϕtgms*. Section 9 is devoted to the calculation of the relative trajectory **r¯***tgms* of the TGMS with respect to the TF.

#### **7. Measurement of TGMS to TF Relative Orientation**

The calculation of the relative rotation of the TGMS with respect to the TF requires the calculation of the absolute orientation of the TGMS and the absolute orientation of the TF. As given in Equations (2) and (3), finding the orientation of the TF just requires the instantaneous value of the arc-length *s*, because the track design geometry is known. The value of *s* has to be known accurately. The odometry algorithm explained in the next section has been developed to this end.

The calculation of the absolute orientation of the TGMS is obtained with the IMU sensor. Most IMU sensors come with their own internal algorithm to calculate the orientation angles or quaternions. However, these algorithms are not accurate in the application at hand. They are based on the sensor fusion of the data of the gyroscope, the accelerometer and the magnetometer, as follows:

1. The gyroscope provides three signals that are proportional to the components of the angular velocity vector in the sensor frame (the sensor frame is assumed to be parallel to the TGMS frame), as follows:

$$
\omega^{\rm inuu} = \hat{\omega}^{\rm gms}\_{\rm abs} \tag{28}
$$

These components are non-linearly related to the coordinates that define the TGMS orientation and their time-derivatives, as shown later.

2. The accelerometer signals are proportional to the components of the absolute acceleration in the local frame plus the absolute gravity vector field, as follows:

$$\mathbf{a}^{imu} = \hat{\mathbf{R}}^{t \text{g} \text{us}} + \left(\mathbf{A}^{t \text{g} \text{us}}\right)^{T} \begin{bmatrix} 0 & 0 & \text{g} \end{bmatrix}^{T} \tag{29}$$

This is the absolute acceleration in the sensor frame, plus the gravitational constant *g*, that is assumed to act in the absolute *Z* direction.

3. The magnetometer signals are proportional to the components of the Earth's magnetic field in the local frame. This information can be used to find the direction of the Earth's magnetic north.

In the algorithm developed in this investigation to find the TGMS absolute orientation, the magnetometer's signals are not used. This is mainly because out of the three Euler angles needed to define the orientation, the yaw angle lacks interest in our application. It is mainly the yaw angle which can be identified with the help of the magnetometer.

Following an Euler's angle sequence yaw-pitch-roll, as done with the TF to get the rotation matrix given in Equation (2), the absolute angular velocity of the TGMS that appear in Equation (28) can be obtained as follows:

$$
\begin{split}
\boldsymbol{\dot{\phi}}\_{\text{abs}}^{\text{tym}} &= \begin{bmatrix}
\cos\theta\_{\text{abs}}^{\text{tym}} \sin\varphi
\end{bmatrix} \boldsymbol{\dot{\phi}}\_{\text{abs}}^{\text{tym}} + \begin{bmatrix}
0 \\
\cos\varphi\_{\text{abs}}^{\text{tym}} \\
\cos\theta\_{\text{abs}}^{\text{tym}} \cos\varphi
\end{bmatrix} \boldsymbol{\dot{\phi}}\_{\text{abs}}^{\text{tym}} + \begin{bmatrix}
0 \\
\cos\varphi\_{\text{abs}}^{\text{tym}} \\
\end{bmatrix} \boldsymbol{\dot{\theta}}\_{\text{abs}}^{\text{tym}} + \begin{bmatrix}
1 \\
0 \\
0
\end{bmatrix} \boldsymbol{\dot{\phi}}\_{\text{abs}}^{\text{tym}} = \\
&= \begin{bmatrix}
1 & 0 & -\sin\varphi\_{\text{abs}}^{\text{tym}} \\
0 & \cos\varphi\_{\text{abs}}^{\text{tym}} & \cos\varphi\_{\text{abs}}^{\text{tym}} \\
0 & -\sin\varphi\_{\text{abs}}^{\text{tym}} & \cos\varphi\_{\text{abs}}^{\text{tym}} \\
\end{bmatrix} \boldsymbol{\begin{bmatrix}
\dot{\phi}\_{\text{abs}}^{\text{tym}} \\
\dot{\theta}\_{\text{abs}}^{\text{tym}} \\
\dot{\psi}\_{\text{abs}}^{\text{tym}}
\end{bmatrix} = \boldsymbol{\mathcal{G}}\_{\text{tym}}^{\text{tym}} \boldsymbol{\Phi}\_{\text{abs}}^{\text{tym}}
\end{split}
\tag{30}
$$

where **Φ***tgms abs* = [ *<sup>ϕ</sup>tgms abs θ tgms abs <sup>ψ</sup>tgms abs* ] *<sup>T</sup>* is the set of absolute angles (with respect to the global frame) of the TGMS. This expression is non-linear in terms of the TGMS absolute angles, but linear in term of their time-derivative. This equation can be inverted to isolate the time-derivatives of the angles, as follows:

$$
\begin{bmatrix} \dot{\boldsymbol{\theta}} \\ \dot{\boldsymbol{\theta}} \\ \dot{\boldsymbol{\psi}} \end{bmatrix} = \left(\dot{\mathbf{G}}\right)^{-1} \boldsymbol{\hat{\omega}} = \begin{bmatrix} 1 & \frac{\sin\boldsymbol{\varrho}\sin\theta}{\cos\boldsymbol{\theta}} & \frac{\cos\boldsymbol{\varrho}\sin\theta}{\cos\boldsymbol{\theta}} \\ 0 & \cos\boldsymbol{\varrho} & -\sin\boldsymbol{\varrho} \\ 0 & \frac{\sin\boldsymbol{\varrho}}{\cos\boldsymbol{\theta}} & \frac{\cos\boldsymbol{\varrho}}{\cos\boldsymbol{\theta}} \end{bmatrix} \begin{bmatrix} \dot{\boldsymbol{\omega}}\_{x} \\ \dot{\boldsymbol{\omega}}\_{y} \\ \dot{\boldsymbol{\omega}}\_{z} \end{bmatrix} \tag{31}$$

where, as done in the rest of this section, the superscript *tgms* and subscript *abs* have been eliminated in the symbols for simplicity. Using again the small-angles assumption of the roll and pitch angles, the following linearized formulas result:

$$\begin{array}{l} \dot{\boldsymbol{\varphi}} = \boldsymbol{\hat{\omega}}\_{x} + \frac{\sin\varrho\sin\theta}{\cos\theta}\boldsymbol{\hat{\omega}}\_{y} + \frac{\cos\varrho\sin\theta}{\cos\theta}\boldsymbol{\hat{\omega}}\_{z} \simeq \boldsymbol{\hat{\omega}}\_{x} + \theta\boldsymbol{\hat{\omega}}\_{z} \\\dot{\theta} = \cos\varrho\boldsymbol{\hat{\omega}}\_{y} - \sin\varrho\boldsymbol{\hat{\omega}}\_{z} \simeq \boldsymbol{\hat{\omega}}\_{y} - \varrho\boldsymbol{\hat{\omega}}\_{z} \\\dot{\boldsymbol{\psi}} = \frac{\sin\varrho}{\cos\theta}\boldsymbol{\hat{\omega}}\_{y} + \frac{\cos\varrho}{\cos\theta}\boldsymbol{\hat{\omega}}\_{z} \simeq \boldsymbol{\varrho}\boldsymbol{\hat{\omega}}\_{y} + \boldsymbol{\hat{\omega}}\_{z} \end{array} \tag{32}$$

For the accelerometer signals, Equation (29) can be linearized as follows:

$$\mathbf{a}^{imu} \simeq \hat{\mathbf{K}}^{tgsms} + \mathcal{g} \begin{bmatrix} -\theta \\ \varphi \\ 1 \end{bmatrix} \tag{33}$$

where:

$$\hat{\mathbf{R}}^{\text{tgsm}} = \left[\mathbf{A}^{\text{tgsm}}\right]^{\text{T}} \hat{\mathbf{R}}^{\text{tgsm}}, \quad \mathbf{A}^{\text{t,tgsm}} \simeq \begin{bmatrix} 1 & -\boldsymbol{\psi}^{\text{tgsm}} & \boldsymbol{\theta}^{\text{tgsm}} \\ \boldsymbol{\psi}^{\text{tgsm}} & 1 & -\boldsymbol{\varphi}^{\text{tgsm}} \\ -\boldsymbol{\theta}^{\text{tgsm}} & \boldsymbol{\eta}^{\text{tgsm}} & 1 \end{bmatrix} \tag{34}$$

Again, because of the small-angles assumption and for the sake of simplicity, the following approximation is used:

$$
\hat{\mathbf{K}}^{\text{tgsus}} \simeq \hat{\mathbf{K}}^{\text{tgsus}} \tag{35}
$$

Using the result of Equation (14), the absolute acceleration of the TGMS is given by:

$$\dot{\mathbf{R}}^{\dagger \text{gms}} = \dot{\mathbf{R}}^{\dagger} + \ddot{\mathbf{F}}^{\dagger \text{gms}} + \left(\tilde{\mathbf{a}}^{\dagger} + \tilde{\boldsymbol{\omega}}^{\dagger} \tilde{\boldsymbol{\omega}}^{\dagger}\right) \mathbf{\tilde{r}}^{\dagger \text{gms}} + 2\tilde{\boldsymbol{\omega}}^{\dagger} \dot{\mathbf{F}}^{\dagger \text{gms}} \tag{36}$$

In this equation, the last three terms on the right-hand side are unknown. These are the TGMS to TF relative acceleration, the tangential and centripetal relative accelerations and the Coriolis acceleration, respectively. In order to calculate these terms, the value of the relative position vector **r**¯*tgms*, velocity **r**¯˙*tgms* and acceleration **r**¨¯*tgms* of the TGMS with respect to the TF are needed. As it can be observed in Figure 8, the trajectory followed by the TGMS when the vehicle is moving is a 3D curve that slightly differs with respect to the track centerline. In fact, the difference between these 3D curves is needed to measure the absolute track irregularities. However, in order to find the TGMS to TF relative orientation, it is going to be assumed that the acceleration equals the one of a particle moving along the track centerline, this is:

$$
\vec{\mathbf{R}}^{\text{tyrs}} \simeq \vec{\mathbf{R}}^{t} = \begin{bmatrix}
\vec{V} \\
\rho\_{h} V^{2} \\
\end{bmatrix} \tag{37}
$$

Therefore, the influence of the last three terms in Equation (36) has been neglected.

**Figure 8.** TGMS trajectory.

The sequence of approximations that has been assumed to calculate the acceleration of the TGMS may look rough for some readers. However, it has to be taken into account that in most algorithms used to find orientation from IMU's signals [21,22] the true acceleration measured by the sensor is neglected in comparison with the gravity term in Equation (33). In the application at hand, this approximation is not accurate and the "compensation" introduced with Equation (36) is much better than nothing.

Finally, the following measurement equation is used to account for the accelerometer signals:

$$\mathbf{a}\_{corr}^{imu} = \mathbf{a}\_{filt}^{imu} - \begin{bmatrix} \dot{V} \\ \rho\_h V^2 \\ -\rho\_{\mathcal{V}} V^2 \end{bmatrix} \approx \mathbf{g} \begin{bmatrix} -\theta \\ \theta \\ 1 \end{bmatrix} \tag{38}$$

where **a***imu corr* is the "corrected" accelerometer signal and **a***imu filt* is the low-pass filtered acceleration signal. The reason for low-pass filtering the acceleration signals is that the effect of the TGMS to TF relative accelerations that have been neglected in Equation (36) contribute to relatively high-frequencies to the signals. This way their contribution is, in part, filtered out of the signals.

Using the two first gyroscope equations given in Equation (32) and the last two accelerometer equations given in Equation (38), the following linear dynamic system can be formulated in state-space form:

$$\begin{aligned} \dot{\mathbf{x}} &= \begin{bmatrix} \dot{\boldsymbol{\theta}} \\ \dot{\boldsymbol{\theta}} \end{bmatrix} = \begin{bmatrix} 0 & \hat{\boldsymbol{\alpha}}\_{z} \\ -\hat{\boldsymbol{\alpha}}\_{z} & 0 \end{bmatrix} \begin{bmatrix} \boldsymbol{\theta} \\ \boldsymbol{\theta} \end{bmatrix} + \begin{bmatrix} \hat{\boldsymbol{\alpha}}\_{x} \\ \hat{\boldsymbol{\alpha}}\_{y} \end{bmatrix} = \mathbf{F} \mathbf{x} + \mathbf{u} \\\ \mathbf{z} &= \begin{bmatrix} \begin{pmatrix} a^{imu}\_{corr} \\ a^{imu}\_{corr} \end{pmatrix}\_{y} \end{bmatrix} = \begin{bmatrix} 0 & -\mathbf{g} \\ \mathbf{g} & 0 \end{bmatrix} \begin{bmatrix} \boldsymbol{\theta} \\ \boldsymbol{\theta} \end{bmatrix} = \mathbf{H} \mathbf{x} \end{aligned} \tag{39}$$

where the state vector **x** = *ϕ θ <sup>T</sup>* just includes the roll and pitch angles, the state transition matrix **F** is not constant but depends on the measured vertical angular velocity, the measurement matrix **H** is constant, the measurement vector **z** includes the *x* and *y* measured-corrected accelerations and the input vector **u** includes the *x* and *y* measured angular velocities. The linear system shown in (39) is a very simple set of equations that can be used with a standard Kalman filter algorithm to find the TGMS to TF relative angles *ϕ* and *θ*.

#### **8. Odometry Algorithm**

The odometry algorithm presented here can be used when the TGMS has no access to the data of a precise odometer of the vehicle and/or a GNSS cannot be used, for example, as it happens in underground trains. Underground trains use to be metropolitan. Being

metropolitan, there used to be many narrow curved sections. Curved sections facilitate the method presented next.

As shown in Figure 9, the design geometry of a railway track (horizontal profile, as explained in Section 3.2) is a succession of segments of three types: straight (*s* in the figure) with zero curvature, circular (*c* in the figure) with constant curvature and transitions (*t* in the figure) with linearly varying curvature. The curvature function can be decomposed into a set of zero segments (straight segments) plus a set of curvature functions that have trapezoidal shape (normal curve) or double-trapezoidal shape (*S-curve*). The location of the curvature functions (start and end points) is exactly identified along the track using the design geometry provided by the track preprocessor.

**Figure 9.** Design horizontal curvature of a railway track.

The curvature of the track can be experimentally approximated in the TGMS with the installed sensors. The curvature of the trajectory followed by the TGMS can be obtained as:

$$
\rho\_h^{\text{exp}} \simeq \frac{\hat{\omega}\_z^{t\_{\text{S}^{ms}}}}{V} \tag{40}
$$

Of course, this approximate measure is a noisy version of the track horizontal curvature. However, experimental measures show that the overall shape of the curvature functions can be clearly obtained with this approximation.

The concept of the odometry algorithm is to monitor the experimental curvature during the ride of the train using Equation (40) and to store the data together with the approximate coordinate *s tgms app* obtained with the help of the installed encoder and the assumption of rolling-without-slipping of the wheel. The plot of the experimental curvature looks like the plot at the top of Figure 10. Using the track preprocessor, the design value of the curvature of the track *ρ design <sup>h</sup>* in the area where the train is located, may look like the lower plot in Figure 10. As shown in the figure, this information can be used to correct the value of *s tgms app* at points 1, 2, 3 and 4 located at the entry or exit of the curves. Measures of *s tgms app* between these corrected points are also corrected using a linear mapping, as shown in Figure 11. Where, black arrows mean odometry corrections due to the algorithm output. Coloured arrows mean odometry correction using linear interpolation of the algorithm output. Different colours are just used to distinguish the curvature function sections

**Figure 10.** Odometry algorithm.

The problem is how to detect the entry and exit of the curves using the functions *ρ* exp *h s tgms app* and *ρ design h stgms* . In fact, it is the exit of the curves that is detected first. Once the TGMS leaves a curve, the shape of the curvature function that the TGMS has ahead is known. Therefore, when the measured curvature *ρ* exp *h s tgms app* "looks similar" to the expected curvature function *ρ design h stgms* , the exit of the curve has been reached. This similarity is computed by calculating at each instant the squared-error of the experimentally measured curvature and the expected curvature function, as follows:

$$e2(s) = \int\_{\mathbb{S} = s - \Delta \mathfrak{s}}^{\mathbb{S} = \mathfrak{s}} \left[ \rho\_h^{\text{exp}}(\mathfrak{s}) - \rho\_h^{\text{desig}}(\mathfrak{s} - s + s\_{\text{exit}}) \right]^2 d\mathfrak{s} \tag{41}$$

where *e*2(*s*) is the squared error (*s* substitute *s tgms app* for simplicity in the formula), Δ*s* is the length of the expected curvature function and *sexit* is the location of the exit of the curve in the design geometry. For a better accuracy, the value of the squared error is normalized for each curvature function using the following factor:

$$I2 = \int\_{\mathbf{s}=0}^{\mathbf{s}=\Delta\mathbf{s}} \left[ \rho\_h^{design}(\mathbf{s}) \right]^2 d\mathbf{s} \tag{42}$$

**Figure 11.** Correction of *tgms*.

The normalization factors, that are different for each curvature function, are of course computed in a preprocesing stage. The normalized squared-error is given by:

$$mc2(s) = \frac{\epsilon 2(s)}{l2} \tag{43}$$

Thanks to the normalization, the value of *ne*2 varies between approximately 1, in straight track sections, and 0 when there is a perfect matching between *ρ* exp *h s tgms app* and *ρ design h stgms* . A typical plot of the function is observed in Figure 12. This function used to be smooth, such that detecting the local minimum that indicates the detection of the exit of the curve is a very easy task. Once the exit of the curve is detected, the expected curvature function is substituted by the next curve ahead along the track. It can be shown that this method can be run in real-time. The main computational cost is the one associated with the calculation of the integral given in Equation (41).

**Figure 12.** Normalized squared-error.

#### **9. Measurement of TGMS to TF Relative Motion**

The final and most difficult step needed to find the absolute irregularities of the track using Equation (27) is to obtain the relative trajectory **r¯***tgms* of the TGMS with respect to the TF shown in Figure 8. Equation (36) can be treated as a second order differential equation in terms of **r¯***tgms*. This equation has the following scalar components:

$$\begin{aligned} \ddot{\mathbf{R}}^{t\text{Sym}} &= \begin{bmatrix} V \\ \rho\_h V^2 \\ -\rho\_v V^2 \end{bmatrix} + \begin{bmatrix} 0 \\ \dot{r}\_y^{t\text{sym}} \\ \dot{r}\_z^{t\text{sym}} \end{bmatrix} + \\ \begin{bmatrix} -r\_y^{t\text{sym}} \left( V\rho\_h + V^2 (\rho\_h^\prime - \rho\_{\text{tr}}\rho\_v) \right) + r\_z^{t\text{sym}} \left( V^2 \rho\_{\text{tr}}\rho\_h + V\rho\_v \right) \\ -r\_y^{t\text{sym}} \left( V^2 (\rho\_{\text{tr}}^2 + \rho\_h^2) \right) - r\_z^{t\text{sym}} \left( -V^2 \rho\_v \rho\_h + V\rho\_{\text{tr}} \right) \\ r\_y^{t\text{sym}} \left( V^2 \rho\_v \rho\_h + V\rho\_{\text{tr}} \right) - r\_z^{t\text{sym}} \left( V^2 (\rho\_{\text{tr}}^2 + \rho\_v^2) \right) \end{bmatrix} + \\ + 2 \begin{bmatrix} V r\_z^{t\text{sym}} \rho\_v - V r\_y^{t\text{sym}} \rho\_h \\ -V r\_z^{t\text{sym}} \rho\_{\text{tr}} \\ \dot{r} r\_y^{t\text{sym}} \end{bmatrix} \end{aligned} (44)$$

Alternatively, the absolute acceleration of the TGMS can be obtained using the measurement of the accelerometer given in Equation (29). If the vectors in Equation (29) are projected to the TF, it yields:

$$\begin{aligned} \dot{\mathbf{k}}^{\text{tgs}} &= \mathbf{A}^{\text{t}} \boldsymbol{t}^{\text{tgs}} \mathbf{a}^{\text{imu}} - \left(\mathbf{A}^{\text{t}}\right)^{T} \begin{bmatrix} 0 & 0 & \mathbf{g} \end{bmatrix}^{T} = \\\begin{bmatrix} a\_{x}^{\text{imu}} - a\_{y}^{\text{imu}} \boldsymbol{\Psi}^{\text{tgs}} + a\_{z}^{\text{imu}} \boldsymbol{\theta}^{\text{tgs}} \mathbf{m}^{\text{gs}} \\\ a\_{y}^{\text{imu}} + a\_{x}^{\text{imu}} \boldsymbol{\Psi}^{\text{tgs}} - a\_{z}^{\text{imu}} \boldsymbol{\Psi}^{\text{tgs}} \\\ a\_{z}^{\text{imu}} - a\_{x}^{\text{imu}} \boldsymbol{\theta}^{\text{tgs}} + a\_{y}^{\text{imu}} \boldsymbol{\Psi}^{\text{tgs}} \end{bmatrix} + \begin{bmatrix} \mathbf{g}^{\text{t}} \\\ -\mathbf{g} \mathbf{g}^{\text{t}} \\\ -\mathbf{g} \end{bmatrix} \end{aligned} \tag{45}$$

where the small-angles assumption has been used again. The left-hand sides of Equations (44) and (45) represent the same physical magnitudes, therefore, the right hand sides of these equations can be equated. Using just the second and third components of the right hand sides and rearranging yields:

$$
\begin{bmatrix}
\dot{r}\_y^{\text{gms}} \\
\dot{r}\_z^{\text{gms}}
\end{bmatrix} + \begin{bmatrix}
0 & -2V\rho\_{tw} \\
2V\rho\_{tw} & 0
\end{bmatrix} \begin{bmatrix}
\dot{r}\_y^{\text{gms}} \\
\dot{r}\_z^{\text{gms}}
\end{bmatrix} + \\
\begin{bmatrix}
V^2 \rho\_v \rho\_h + V\rho\_{tw} & -V^2 \left(\rho\_{tw}^2 + \rho\_v^2\right)
\end{bmatrix} \begin{bmatrix}
r\_y^{\text{gms}} \\
r\_z^{\text{gms}}
\end{bmatrix} = \\
\begin{bmatrix}
a\_y^{\text{imu}} + a\_x^{\text{imu}} \Psi^{\text{gms}} - a\_x^{\text{imu}} \Psi^{\text{gms}} - \mathcal{g} \,\mathbf{g}^{\text{f}} - \rho\_h V^2 \\
a\_z^{\text{imu}} - a\_x^{\text{imu}} \Psi^{\text{gms}} + a\_y^{\text{imu}} \Psi^{\text{gms}} - \mathcal{g} + \rho\_v V^2
\end{bmatrix}
\end{bmatrix} \tag{46}
$$

This is a 2*nd* order linear system of ordinary differential equations (ODE) with timevariant coefficients (linear time-varying system, LTV). This ODE has to be integrated forward in time to find the TGMS to TF relative trajectory (*r tgms <sup>y</sup>* (*t*) and *r tgms <sup>z</sup>* (*t*)). The inputs of these equations are all calculated or measured so far. These inputs are:


In the case of a tangent (straight) track, where all track curvatures are zero, Equation (46) reduces to:

$$\begin{bmatrix} \dot{r}\_y^{t\text{gms}}\\ \dot{r}\_z^{t\text{gms}} \end{bmatrix} = \begin{bmatrix} a\_y^{imu} + a\_x^{imu} \psi^{t\text{gms}} - a\_z^{imu} \phi^{t\text{gms}}\\ a\_z^{imu} - a\_x^{imu} \theta^{t\text{gms}} + a\_y^{imu} \phi^{t\text{gms}} - \mathcal{g} \end{bmatrix} \tag{47}$$

Numerical integration of Equation (46) can be used to find the TGMS to TF relative motion. However, a close look to Equation (46) shows that these differential equations are equivalent to the equations of motion of a linear 2-degrees of freedom system with negative-definite stiffness and damping matrices. It is a highly unstable system. Therefore, serious problems of drift of the resulting displacements are expected. As a result that the track irregularity is a relatively-high frequency component of the track geometry (being the design geometry the low-frequency component), the drift of the solution can be solved using several methods, like these two:


The second method is used in this investigation to get the results presented in Section 11.

#### **10. Experimental Setup**

A scale track has been built at the rooftop of the School of Engineering at the University of Seville. Figure 13 on the left shows an aerial view of the 90 m-track. The scale is approximately 1:10 (5-inch gauge). The track includes a tangent section (next to end A), a 26 m-curved section and a 12 m-curved section, among other features. The track is supported on a set of mechanisms that can be observed on the right of the figure that are separated 10 cm. These mechanisms can be used to create any irregularity profile. Details of the track design can be found in [24].

**Figure 13.** Scaled track at the School of Engineering, University of Seville. (**a**) Aerial view, (**b**) detail of track supports.

Figure 14 shows the scale vehicle that has been used to install the TGMS. The vehicle has a classical architecture of 4 wheelsets, 2 bogies and 1 car-body. The TGMS is attached to the car-body. The right picture shows a detail of the vehicle where the video cameras and the laser beam can be observed. The vehicle drive is an electric motor with a chain transmission. Details of the vehicle design can be found in [24]. The TGMS is equipped with two video cameras Ximea MQ003CG-CM. The VGA cameras have a resolution of 648 × 488 pixels. They include a CMOS RGB bayer matrix sensor. Their maximum frame acquisition rate reaches 500 fps. The camera has a C-mount lens FUJINN 2/3" 12.5 F1.2– F1.6 MI E1.5MP MV that allows the adjustment of focus and exposure. The projection lasers have a power of 10 mW and they illuminate in red. The IMU is a LORD Microstrain 3DM-GX5-25 AHRS, an triaxial accelerometer, gyroscope and magnetometer industrial sensor fully calibrated and temperature compensate. It has a maximum sampling rate of 1 kH. The accelerometer has two different measuring ranges <sup>±</sup>8 g (25 <sup>μ</sup>g/√Hz) or <sup>±</sup>20 g (80 <sup>μ</sup>g/√Hz). The gyroscope has an angle random walk of 0.3◦/ √ h. The traveled distance by the vehicle along the track is obtained using a precision digital encoder Kubler 2400 series. It is a two channel encoder with up to 1440 pulses per revolution.

In order to have a reference value of the track irregularities, an accurate track geometry measurement has been performed. The relative irregularities have been measured with the instrument that can be observed in the right of Figure 15. This instrument slides along the track. It includes a LVDT to measure the gauge variation and an inclinometer to measure the cross-level. In order to measure the absolute irregularities, alignment and longitudinal profile, the total station shown in the left hand side of the figure has been used. Details of the track geometry measurement can be found in [24].

D E

**Figure 14.** Scale vehicle, (**a**) global view of instrumented scale vehicle, (**b**) detail showing video cameras and laser beam.

**Figure 15.** Irregularity measurement. (**a**) Robotic total station, (**b**) LVDT and inclinometer.
