# trajectory registration of IMU and USBL data

## Data processing

Inertial navigationa data(IMU) is relatively continuous and
fluctuates less, which can be directly used for trajectory
transformation. Due to the influence of USBL equipment, USBL data has
many anomalies,such as large jump in time series, data not updated,
numerical anomaly and not continuous enough. Therefore, USBL data need
to be screened to eliminate outliers before they can be used for
trajectory description.

The USBL data screening scheme is as follows:

1. Delete the data that XYZ is 0 at the initial moment, during which
the USBL device does not normally send/receive valid data.

2. The X-axis of USBL points upward horizontally, which can be
understood as depth data. Since the relative variation range of depth
value is very small, data points with X value greater than 10 can be
deleted based on this standard.

3. The direction of the Y-axis of the USBL is roughly parallel to the
short edge of the experimental water area. With the USBL equipment as
the center, the range of the short edge of the experimental water area
is limited to ±50m.

4. Since the motion state of the object changes continuously and the
motion speed is relatively slow, the changes of data with similar time
stamps are generally monotonous and small, which can be used as the
standard to eliminate data points with large jumps.

The following figure shows the track of unexcluded USBL abnormal data. The green scatter points represent USBL data points, and the existence of many abnormal points can be observed from the figure.

## IMU data calculation and trajectory drawing

Since the IMU data initialization comes from GPS calibration, the
data values after IMU are also longitude and latitude values, and the
coordinate system is WGS84 geodetic coordinate system. In order to draw
the trajectory conveniently, the longitude and latitude values under
WGS84 need to be converted into the northeast Sky (ENU) coordinate
system with the origin of the first point measured by IMU. ENU
coordinate system is also called station center coordinate system. The
coordinate system takes the user's position as the origin, with X axis
pointing east, Y axis pointing north and Z axis pointing to the
zenith.

The transformation of data point coordinates from WGS84 to ENU
coordinate system consists of two steps:

1. Transfer the ENU coordinate system to the ECEF (Earth-centered
solid Right Angle) coordinate system

The ORIGIN of the ECEF coordinate system is the earth's center of mass, the X-axis extends through the intersection of the prime meridian (0 degrees longitude) and the equator (0 degrees latitude), and the Z-axis extends through the North Pole (that is, coincides with the earth's rotation axis). The Y-axis follows the right-handed coordinate system, crossing the equator and 90 degrees longitude. (Lon, Lat, Alt) in the WGS84 coordinate system is converted to points (X,Y,Z) in the ECEF coordinate system:

\[\left\{\begin{array}{l}X=(N+a l t) \cos (l a t) \cos (l o n) \\ Y=(N+a l t) \cos (l a t) \sin (l o n) \\ Z=\left(N\left(1-e^{2}\right)+a l t\right) \sin (l a t)\end{array}\right.\]

Where e is the eccentricity of the ellipsoid and N is the radius of curvature of the reference ellipsoid.

\[\left\{\begin{array}{l}e^{2}=\frac{a^{2}-b^{2}}{a^{2}} \\ N=\frac{a}{\sqrt{1-e^{2} \sin ^{2} l a t}}\end{array}\right.\]

Since polar flattening \(f=\frac{a-b}{a}\) under WGS-84, the relationship between eccentricity e and polar flattening f is as follows:

\[e^{2}=f(2-f)\]

So N could also be written as

\[N=\frac{a}{\sqrt{1-f(2-f) \sin ^{2} \operatorname{lat}}}\]

2. Switch the ECEF coordinate system to the ENU coordinate system

The user's coordinate origin \(P_{0}=\left(x_{0}, y_{0}, z_{0}\right)\), calculate point \(P=(x, y, z)\) at the position of ENU coordinate system (e,n,u) with point \(P_{0}\) as the coordinate origin, the data of WGS84 coordinate system are needed here, \(L L A_{0}=\left(\right.\) lon\(_{0}\), lat\(_{0}\), alt\(\left._{0}\right)\)

\[\left[\begin{array}{l}\Delta x \\ \Delta y \\ \Delta z\end{array}\right]=\left[\begin{array}{l}x \\ y \\ z\end{array}\right]-\left[\begin{array}{l}x_{0} \\ y_{0} \\ z_{0}\end{array}\right]\]

\[\left[\begin{array}{l}e \\ n \\ u\end{array}\right]=S \cdot\left[\begin{array}{l}\Delta x \\ \Delta y \\ \Delta z\end{array}\right]=\left[\begin{array}{ccc}-\sin \left(\operatorname{lon}_{0}\right) & \cos \left(\operatorname{lon}_{0}\right) & 0 \\ -\sin \left(\operatorname{lat}_{0}\right) \cos \left(\operatorname{lon}_{0}\right) & -\sin \left(\operatorname{lat}_{0}\right) \sin \left(\operatorname{lon}_{0}\right) & \cos \left(\operatorname{lat}_{0}\right) \\ \cos \left(\operatorname{lat}_{0}\right) \cos \left(\operatorname{lon}_{0}\right) & \cos \left(\operatorname{lat}_{0}\right) \sin \left(\operatorname{lon}_{0}\right) & \sin \left(\operatorname{lat}_{0}\right)\end{array}\right] \cdot\left[\begin{array}{l}\Delta x \\ \Delta y \\ \Delta z\end{array}\right]\]

Namely, coordinate transformation matrix:

\[S=\left[\begin{array}{ccc}-\sin \left(\operatorname{lon}_{0}\right) & \cos \left(\operatorname{lon}_{0}\right) & 0 \\ -\sin \left(\operatorname{lat}_{0}\right) \cos \left(\operatorname{lon}_{0}\right) & -\sin \left(\operatorname{lat}_{0}\right) \sin \left(\operatorname{lon}_{0}\right) & \cos \left(\operatorname{lat}_{0}\right) \\ \cos \left(\operatorname{lat}_{0}\right) \cos \left(\operatorname{lon}_{0}\right) & \cos \left(\operatorname{lat}_{0}\right) \sin \left(\operatorname{lon}_{0}\right) & \sin \left(\operatorname{lat}_{0}\right)\end{array}\right]\]

After all IMU data points have been converted to ENU coordinates, the trajectory recorded by the IMU is plotted.

## USBL data calculation and trajectory drawing

For the USBL data, the data took the USBL device as the origin and the direction defined by the USBL device as the coordinate system. The values were given. The Y-axis of the USBL coordinate system was closer to the E-axis of ENU, and the Z-axis of the USBL coordinate system was closer to the axis of ENU. The trajectory directly drawn is as follows:

It can be intuitively observed from the figure that due to the Angle difference and position difference between USBL coordinate system and ENU coordinate system, the trajectory angles of the two coordinate systems differ by one Angle and their positions differ. Therefore, data registration of the two coordinate systems is required.

## IMU and USBL data registration

Since there is an Angle difference between USBL and ENU coordinates (e-n and Y-Z planes), we need to eliminate this Angle difference. Because the Angle difference of the slope of the same line in different coordinate systems on the same plane is equal to the Angle difference of the coordinate axes. To calculate the Angle difference, A relatively reliable data point A (the 225th timestamp data point) was selected to calculate the coordinates of point A in ENU coordinate system (EN plane) (-14.88295364,-35.75550466) and point A in USBL coordinate system (YZ plane) (-38.0,-57.7). Then, according to the latitude and longitude value of the USBL device coordinate point O, the coordinates of point O in ENU coordinate system (13.16433724, 36.59235146) and the coordinates of point O in USBL coordinate system (0,0) are calculated. After obtaining the above four coordinates, the slope and Angle α of line OA in ENU coordinate system and the slope and Angle β of line OA in USBL coordinate system can be obtained, and then the Angle difference of the two coordinate systems γ can be obtained:

\[\gamma=\alpha-\beta=56.63194470^{\circ}-43.54190515^{\circ}=13.09003955^{\circ}\]

USBL data points can be registered into ENU coordinate system after
the Angle difference and position difference of the two coordinate axes
are known. The specific steps are as follows:

1. Rotate the USBL coordinate system clockwise by the Angle γ The
rotation transformation formula of the coordinate axis is:

\[\left\{\begin{array}{l}x_{1}=x \cos \theta+y \sin \theta \\ y_{1}=y \cos \theta-x \sin \theta\end{array}\right.\]

- Align the data coordinates obtained in Step 1 with the ENU coordinate system

\[\left\{\begin{array}{l}x_{2}=x_{1}+O_{E} \\ y_{2}=y_{1}+O_{N}\end{array}\right.\]

After obtaining the USBL data points after registration, the IMU and USBL data tracks after registration are plotted.

## Trajectory matching

Using satellite photos and relevant GPS coordinates, the obtained trajectory is superimposed with the actual field, and the following results are obtained. The comparison between the figure and the actual environment shows that the start point and the docking dock are consistent with the actual situation, which proves the validity of data processing and the authenticity of the trajectory from the side.

## Code

1 | import math |