trajectory registration of IMU and USBL data

Posted on Edited on In
Symbols count in article: 11k Reading time ≈ 32 mins.
Trajectory registration of IMU and USBL data, from WGS84 to ENU

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.

Fig.1 Trajectory diagram without excluding USBL abnormal data

 

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.

Fig.2 Trajectory of IMU

 

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:

Fig.3 Trajectories of IMU and USBL

  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}\]

Fig.4 ENU coordinate and USBL coordinate

  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.\]

  1. 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.

Fig.5 IMU and USBL tracks after registration

 

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.

Fig.6 Trajectory matching satellite image result

 

Code

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
import math
import numpy as np
import pymap3d as pm
import xlrd                              #xlrd==1.2.0
from matplotlib import pyplot as plt

# Location of USBL
usbl_lonO = 111.111111
usbl_latO = 88.188888889
usbl_hO = -0

# Read Excel data into matrix functions
def excel2m(path):
    data = xlrd.open_workbook(path)
    table = data.sheets()[0]             # Get the first sheet in Excel
    nrows = table.nrows
    ncols = table.ncols
    datamatrix = np.zeros((nrows, ncols))
    for x in range(ncols):
        cols = table.col_values(x)
        cols1 = np.matrix(cols)           # Convert list to matrix for matrix manipulation
        datamatrix[:, x] = cols1          # Store the data
    return datamatrix

excel_imu = excel2m("C:\\Users\\imu.xlsx")
excel_usbl = excel2m("C:\\Users\\usbl1.xlsx")

# The local coordinate origin (original point of ENU)
lat0 = 111             # deg
lon0 = 88             # deg
h0 = 0                         # meters

# Convert imu(GPS) data into ENU coordinate
lat = np.array(np.zeros((excel_imu.shape[0],1)))
lon = np.array(np.zeros((excel_imu.shape[0],1)))
h = np.array(np.zeros((excel_imu.shape[0],1)))
e = []
n = []
u = []
for i in range(excel_imu.shape[0]):
    lat[i] = excel_imu[i,1]
    lon[i] = excel_imu[i,0]
    h[i] = excel_imu[i,2]
    (xi,yi,zi) = pm.geodetic2enu(lat[i,0], lon[i,0], h[i,0], lat0, lon0, h0)
    e.append(xi)
    n.append(yi)
    u.append(zi)

print('e =',e)
print('n =',n)
print('u =',u)

# The coordinates of the USBL origin projected in the ENU coordinate system(point O)
(usbl_eO,usbl_nO,usbl_uO) = pm.geodetic2enu(usbl_latO, usbl_lonO , usbl_hO, lat0, lon0, h0)
print('usbl_eO =',usbl_eO)
print('usbl_nO =',usbl_nO)
print('usbl_uO =',usbl_uO)

# choose one reliable point a of usbl(in USBL coordinate)
usbl_a_y = excel_usbl[163,1] # 163th does not match 225th as follows, because the same timestamp of point a in imu.xlsx and usbl.xlsx are not the same serial number
usbl_a_z = excel_usbl[163,2]
print('usbl_a_y =',usbl_a_y)
print('usbl_a_z =',usbl_a_z)

# point a in ENU coordinate
(enu_a_e,enu_a_n,enu_a_u) = pm.geodetic2enu(excel_imu[225,1], excel_imu[225,0], excel_imu[225,2], lat0, lon0, h0)
print('enu_a_e =',enu_a_e)
print('enu_a_n =',enu_a_n)

# The Angle difference of the same line in different plane coordinates is equal to the rotation Angle of the coordinate system
# Use the line Oa(point a is the origin of the USBL coordinate)
k_enu = (enu_a_n - usbl_nO) / (enu_a_e - usbl_eO)         # Slope of line Oa in ENU coordinate
print(k_enu)
angle_enu = math.atan(k_enu)                              # Angle of line Oa in ENU(radian)
print(angle_enu*180/(math.pi))
k_usbl = usbl_a_z / usbl_a_y                              # Slope of line Oa in USBL coordinate
print(k_usbl)
angle_usbl = math.atan(k_usbl)                            # Angle of line Oa in USBL(radian)
print(angle_usbl*180/(math.pi))
angle_diff = angle_usbl - angle_enu
print('angle_diff =',angle_diff*180/(math.pi))                          # The rotation Angle value of two coordinate systems

# registration process
x = np.array(np.zeros((excel_usbl.shape[0],1)))
y = np.array(np.zeros((excel_usbl.shape[0],1)))
z = np.array(np.zeros((excel_usbl.shape[0],1)))
usbl_x = []
usbl_y = []
usbl_z = []
for i in range(excel_usbl.shape[0]):
    x[i,0] = excel_usbl[i,0]
    # y[i, 0] = excel_usbl[i, 1]
    # z[i, 0] = excel_usbl[i, 2]
    y[i,0] = excel_usbl[i,1] * math.cos(angle_diff) + excel_usbl[i,2] * math.sin(angle_diff) + usbl_eO
    z[i,0] = excel_usbl[i,2] * math.cos(angle_diff) - excel_usbl[i,1] * math.sin(angle_diff) + usbl_nO + 6
    usbl_x.append(x[i,0])
    usbl_y.append(y[i,0])
    usbl_z.append(z[i,0])

print('usbl_x =',usbl_x)
print('usbl_y =',usbl_y)
print('usbl_z =',usbl_z)

fig = plt.figure()
fig = plt.plot(e,n)
plt.scatter(usbl_y,usbl_z,c='r',s=5)
plt.xlabel('East(m)')
plt.ylabel('North(m)')
plt.title('IMU and USBL trajectories in ENU coordinate',size=16,loc = 'center')
plt.legend(['IMU','USBL'])
plt.axis('equal')
plt.show()