# Performance Improvement of Inertial Navigation System by Using Magnetometer with Vehicle Dynamic Constraints

Authors: Daehee Won; Jongsun Ahn; Sangkyung Sung; Moonbeom Heo; Sung-Hyuck Im; Young Jae Lee
Journal: Journal of Sensors
 (2015)

---

## Abstract

A navigation algorithm is proposed to increase the inertial navigation performance of a ground vehicle using magnetic measurements and dynamic constraints. The navigation solutions are estimated based on inertial measurements such as acceleration and angular velocity measurements. To improve the inertial navigation performance, a three-axis magnetometer is used to provide the heading angle, and nonholonomic constraints (NHCs) are introduced to increase the correlation between the velocity and the attitude equation. The NHCs provide a velocity feedback to the attitude, which makes the navigation solution more robust. Additionally, an acceleration-based roll and pitch estimation is applied to decrease the drift when the acceleration is within certain boundaries. The magnetometer and NHCs are combined with an extended Kalman filter. An experimental test was conducted to verify the proposed method, and a comprehensive analysis of the performance in terms of the position, velocity, and attitude showed that the navigation performance could be improved by using the magnetometer and NHCs. Moreover, the proposed method could improve the estimation performance for the position, velocity, and attitude without any additional hardware except an inertial sensor and magnetometer. Therefore, this method would be effective for ground vehicles, indoor navigation, mobile robots, vehicle navigation in urban canyons, or navigation in any global navigation satellite system-denied environment.


---

## Body

## 1. Introduction

Navigation information represents some of the most important data for autonomous applications. Inertial navigation systems (INSs) have been used to provide standalone navigation solutions for a long time, including position, velocity, and attitude information. Almost all autonomous applications adapt INSs as the base system [1–4]. These can estimate a solution without any external data, but the accuracy decreases with an increase in the operating time. In particular, the heading estimation is relatively poor compared to the other attitude elements (roll and pitch angle), and it has poor observability. In terms of the attitude accuracy, the earth’s magnetic field is very helpful when estimating the heading angle. Because this magnetic field does not have drift, it can be used as a reference for the heading angle. In addition to using an inertial navigation system, various applications have adopted a magnetometer to compute headings, including mobile robots, unmanned aerial vehicles (UAVs), and missiles [5–8].Magnetometers have been used with inertial sensors (accelerometers and gyroscopes) to improve the attitude estimation performance. In addition to improving the attitude accuracy, a magnetometer also provides a slight improvement in the position accuracy. A navigation equation for the position and velocity includes the attitude elements, and the accuracy of the position depends on the attitude. However, the improvement is not significant because the position and velocity are highly dependent on the measured acceleration rather than the attitude. Moreover, information is only transferred from the attitude to velocity and not in the opposite direction for feedback. Therefore, it is difficult to improve the position and attitude at the same time. In this paper, we propose a method to improve the INS performance for a ground vehicle using a magnetometer and dynamic constraints.It is well known that the navigation solution provided by a conventional INS contains drift error [1, 2, 4, 9]. The drift in the roll and pitch can be reduced or corrected using a zero velocity update or acceleration-based attitude estimation. However, because these methods are not effective for heading estimation (yaw), a magnetometer has been used to resolve the problem. Various calibration methods [10–13] have been proposed for the accurate estimation of the heading angle. The sensor has been rotated to measure the data in all directions (360°). The scale factor and offset have been computed, after which compensated magnetic data were used to calculate the heading angle. Soken and Hajiyev [14] proposed online magnetometer calibration, and Alonso and Shuster [15] and Springmann and Cutler [16] showed an attitude estimation method without an inertial sensor. However, these previous studies only focused on improving the attitude estimation, without improving the position and velocity accuracy.Global navigation satellite systems (GNSSs) have been integrated with inertial sensors to obtain accurate position and velocity solutions. Loosely, tightly, and ultratightly coupled integration [2, 17, 18] are famous techniques for GNSS integration. However, GNSS integration always requires visible satellites, and the navigation solution also diverges if there is no visible satellite or sometimes even with less than four satellites. Recently, a vision sensor was introduced to inertial navigation to cope with the visible satellite problem [3, 19–21]. The tracked visual point method provides more robust navigation solutions than conventional INS. However, the vision sensor is too sensitive to the amount of light, and sometimes there are no available points to use. Moreover, it cannot remove the drift error without other absolute information [21, 22]. Both the GNSS and vision integration approaches need additional hardware, which increase the cost. Moreover, they are highly dependent on the surrounding environment.Vehicle dynamic constraints such as nonholonomic constraints (NHCs) have been introduced to improve the INS performance and could decrease the drift rate [9, 23–26]. The basic assumption for NHCs is that a ground vehicle is governed by two NHCs if it does not jump off or slide on the ground. In reality, these constraints are acceptable for general vehicle operation, even though they are sometimes violated as a result of the vibrations of the vehicle’s engine and suspension or by side-slip during cornering [24–27]. However, this method still has no feedback from the attitude and it is not accurate without external information. Only relative navigation is possible based on the previous information and current measurements.For the accurate estimation of navigation solutions, previous researchers only focused on improving the attitude using a magnetometer, or they used additional hardware and had operating environment limitations. In this paper, we propose a method to improve the estimation performance for the position, velocity, and attitude without any additional hardware except an inertial sensor and magnetometer. For robust estimation, we use a magnetic heading with NHCs. The heading is computed by using calibrated magnetic measurement. The NHCs are applied to increase the correlation of the velocity and attitude. This method can provide a velocity feedback to the attitude, which makes the navigation solution more robust than a conventional navigation equation that only transfers information from the attitude to velocity. The magnetic heading and NHCs are combined with an extended Kalman filter (EKF). An experimental test showed that the proposed method significantly improves the accuracy of the position, velocity, and attitude estimations. As a result, the method can increase the survival time of dead reckoning in GNSS-denied environments such as indoors or in urban canyons. Therefore, the proposed method could be effective for ground vehicles, indoor navigation, mobile robots, vehicle navigation in urban canyons, or navigation in any GNSS-denied environment.In Section2 of this paper, the basic navigation equation used to compute the navigation solution is introduced. The NHCs are described in Section 3, and then the estimation filter used to combine the magnetic heading and NHCs is discussed in Section 4. The simulation and experimental test results are presented in Section 5. The conclusions are presented in Section 6.


## 2. Basic Navigation Equation

Inertial measurements are used to compute basic navigation solutions. In addition to this, magnetic field measurement improves the heading accuracy. In this section, a navigation equation is introduced for both the inertial sensor and magnetometer.


### 2.1. Inertial Navigation System (INS)

An inertial measurement unit (IMU) provides the acceleration and angular velocity. These measurements can be used to compute a navigation solution, including the position, velocity, and attitude information. This technique is called inertial navigation. INS equations can be formulated in various coordinate systems, including north-east-down (NED) frame; Earth-Centered, Earth-Fixed (ECEF) frame; and longitude-latitude-height (LLH) frame. In this paper, we use equations based on the NED frame and the target IMU is a low-cost microelectromechanical systems (MEMS) IMU. The NED frame is simpler and more intuitive than other coordinate systems. The navigation solution is computed by using the acceleration and angular velocity [1, 2, 4]. The discrete navigation equation is as follows:(1)
p
n
t
+
1
v
n
t
+
1
ψ
n
t
+
1
=
p
n
t
+
v
n
t
Δ
t
v
n
t
+
C
b
n
t
f
b
t
+
g
n
Δ
t
ψ
n
t
+
E
b
n
t
ω
b
t
Δ
t
+
w
t
,where p
n is the position, v
n is the velocity, ψ
n is the attitude, C
b
n is the direction cosine matrix, E
b
n is the rotation rate transformation matrix from the body frame to the navigation frame, f
b is the specific force in the body frame, g
n is gravity, ω
b is the angular rate in the body frame, Δ
t is the time interval, and w is the process noise of the INS. The navigation solution is computed and updated by using (1) recursively. The IMU should be calibrated, and its initial biases should be compensated prior to computing the navigation solution. Fortunately, most commercial IMUs are calibrated in the factory, or there exist available calibration processes [1]. However, the initial state values remain unknown to process (1). Zero velocity update (ZUPT) and gyrocompassing are well-known methods for initializing an INS [1, 9, 23].With only an IMU, an INS does not have any correction process via external information, which makes the solution inaccurate. In this paper, we use the magnetic heading and NHCs to improve the accuracy of the navigation solution. The magnetic heading is presented in the subsequent section, and then the details of the NHCs are introduced in Section3.


### 2.2. Magnetic Heading

A magnetometer measures and detects both the sign and the magnitude of the earth’s magnetic field. Then, the horizontal components of the magnetic field are used to determine magnetic north, which differs from true north. The difference is called the declination angle, which should be corrected for accurate heading estimation. Moreover, the magnetic field is distorted by the effect of the vehicle or surrounding environment. Thus, three processes should be performed in advance of determining the heading:(
1
) tilt compensation to obtain the horizontal magnetic measurements, (
2
) declination angle compensation to adjust to true north, (
3
) and ferrous distortion compensation by adjusting the scale factor and offset of the measurements. These processes are calledmagnetometer calibration.First, a magnetometer attached to a vehicle or its surface is not always horizontal to the earth’s local plane. There exists tilt angle (roll and pitch) with respect to the local horizontal plane, which makes it difficult to compute the heading. Common sensors to measure this tilt angle include accelerometers, gyroscopes, gimbaled structures, and fluid sensors. In this paper, we use a three-axis accelerometer and gyroscope inside the IMU. Once the magnetic readings (m
x
b
,
m
y
b
,
a
n
d
m
z
b) are measured on the body frame, they are converted into a horizontal plane using a coordinate transformation matrix (C
b
h) with the roll (ϕ) and pitch (θ) angles in Figure 1. The transformation equation is shown below:(2)
m
x
h
m
y
h
=
C
b
h
m
x
b
m
y
b
m
z
b
,
C
b
h
=
cos
⁡
θ
sin
⁡
ϕ
sin
⁡
θ
cos
⁡
ϕ
sin
⁡
θ
0
cos
⁡
ϕ
-
sin
⁡
ϕ
,where the “b” and “h” superscripts represent the body frame and horizontal plane, respectively, and subscripts x, y, and z denote the axes.Figure 1
Magnetic vector presented on horizontal plane with declination angle.Next, we need to compensate for the declination angle. This is easily determined and compensated for using a lookup table based on the geographic location [10–13]. The National Oceanic and Atmosphere Administration (NOAA) provides magnetic declinations all over the world (http://www.ngdc.noaa.gov/geomag/declination.shtml). After the declination is determined, the compensation parameters can be computed by rotating magnetometer to measure all direction magnetic fields.In reality, a magnetic distortion exists because the magnetometer is mounted on a vehicle that includes ferrous materials nearby. The ferrous materials will distort, or bend, the magnetic field. In previous research [10–13], these magnetic distortions were divided into more detailed factors for higher accuracy. In this paper, the distortions are divided into two categories: hard- and soft-iron effects. First, the hard-iron effect comes from any nearby iron or steel on the vehicle. This effect produces a constant magnitude error in the magnetic reading along each axis. The soft-iron effect comes from the interaction of the earth’s magnetic field and any magnetically soft material. The amount of distortion from the soft-iron depends on the sensor orientation [10, 11].Those distortions can be accounted for and should be removed from the magnetic readings before computing the magnetic heading. The horizontal magnetic readings should make a circle for the 360° rotation without any distortions. In reality, it shows an ellipsoid pattern, as well as biases with respect to the origin as shown in Figure2. We can compensate for these distortions by determining the scale factor (s
x
,
s
y) and offset (o
x
,
o
y):(3)
m
-
x
h
m
-
y
h
=
s
x
·
m
x
h
s
y
·
m
y
h
+
o
x
o
y
.The scale factor changes the ellipsoid to a circle, and the offset moves the center of circle to (
0,0
). These two factors are calculated from the maximum and minimum values of the magnetic readings for the 360° rotation. The scale factor is determined by using the following equations:(4)
s
x
=
max
⁡
1
,
max
⁡
m
y
h
-
min
⁡
m
y
h
max
⁡
m
x
h
-
min
⁡
m
x
h
,
s
y
=
max
⁡
1
,
max
⁡
m
x
h
-
min
⁡
m
x
h
max
⁡
m
y
h
-
min
⁡
m
y
h
.And the offset is determined by using the scale factor minimum and maximum readings as follows:(5)
o
x
=
max
⁡
m
x
h
-
min
⁡
m
x
h
2
-
max
⁡
m
x
h
·
s
x
,
o
y
=
max
⁡
m
y
h
-
min
⁡
m
y
h
2
-
max
⁡
m
y
h
·
s
y
.Once the magnetic readings are compensated by using the tilt angle, declination, and distortions, then the magnetic heading can be computed by using an arctangent function as follows: (6)
ψ
m
a
g
=
tan
-
1
⁡
m
-
y
h
m
-
x
h
.Finally, we can obtain the magnetic heading, which will be combined with the NHCs using an EKF.Figure 2
Magnetic readings on horizontal plane: blue circle represents raw measurements and red circle shows values after compensation by calibration parameters.


## 2.1. Inertial Navigation System (INS)

An inertial measurement unit (IMU) provides the acceleration and angular velocity. These measurements can be used to compute a navigation solution, including the position, velocity, and attitude information. This technique is called inertial navigation. INS equations can be formulated in various coordinate systems, including north-east-down (NED) frame; Earth-Centered, Earth-Fixed (ECEF) frame; and longitude-latitude-height (LLH) frame. In this paper, we use equations based on the NED frame and the target IMU is a low-cost microelectromechanical systems (MEMS) IMU. The NED frame is simpler and more intuitive than other coordinate systems. The navigation solution is computed by using the acceleration and angular velocity [1, 2, 4]. The discrete navigation equation is as follows:(1)
p
n
t
+
1
v
n
t
+
1
ψ
n
t
+
1
=
p
n
t
+
v
n
t
Δ
t
v
n
t
+
C
b
n
t
f
b
t
+
g
n
Δ
t
ψ
n
t
+
E
b
n
t
ω
b
t
Δ
t
+
w
t
,where p
n is the position, v
n is the velocity, ψ
n is the attitude, C
b
n is the direction cosine matrix, E
b
n is the rotation rate transformation matrix from the body frame to the navigation frame, f
b is the specific force in the body frame, g
n is gravity, ω
b is the angular rate in the body frame, Δ
t is the time interval, and w is the process noise of the INS. The navigation solution is computed and updated by using (1) recursively. The IMU should be calibrated, and its initial biases should be compensated prior to computing the navigation solution. Fortunately, most commercial IMUs are calibrated in the factory, or there exist available calibration processes [1]. However, the initial state values remain unknown to process (1). Zero velocity update (ZUPT) and gyrocompassing are well-known methods for initializing an INS [1, 9, 23].With only an IMU, an INS does not have any correction process via external information, which makes the solution inaccurate. In this paper, we use the magnetic heading and NHCs to improve the accuracy of the navigation solution. The magnetic heading is presented in the subsequent section, and then the details of the NHCs are introduced in Section3.


## 2.2. Magnetic Heading

A magnetometer measures and detects both the sign and the magnitude of the earth’s magnetic field. Then, the horizontal components of the magnetic field are used to determine magnetic north, which differs from true north. The difference is called the declination angle, which should be corrected for accurate heading estimation. Moreover, the magnetic field is distorted by the effect of the vehicle or surrounding environment. Thus, three processes should be performed in advance of determining the heading:(
1
) tilt compensation to obtain the horizontal magnetic measurements, (
2
) declination angle compensation to adjust to true north, (
3
) and ferrous distortion compensation by adjusting the scale factor and offset of the measurements. These processes are calledmagnetometer calibration.First, a magnetometer attached to a vehicle or its surface is not always horizontal to the earth’s local plane. There exists tilt angle (roll and pitch) with respect to the local horizontal plane, which makes it difficult to compute the heading. Common sensors to measure this tilt angle include accelerometers, gyroscopes, gimbaled structures, and fluid sensors. In this paper, we use a three-axis accelerometer and gyroscope inside the IMU. Once the magnetic readings (m
x
b
,
m
y
b
,
a
n
d
m
z
b) are measured on the body frame, they are converted into a horizontal plane using a coordinate transformation matrix (C
b
h) with the roll (ϕ) and pitch (θ) angles in Figure 1. The transformation equation is shown below:(2)
m
x
h
m
y
h
=
C
b
h
m
x
b
m
y
b
m
z
b
,
C
b
h
=
cos
⁡
θ
sin
⁡
ϕ
sin
⁡
θ
cos
⁡
ϕ
sin
⁡
θ
0
cos
⁡
ϕ
-
sin
⁡
ϕ
,where the “b” and “h” superscripts represent the body frame and horizontal plane, respectively, and subscripts x, y, and z denote the axes.Figure 1
Magnetic vector presented on horizontal plane with declination angle.Next, we need to compensate for the declination angle. This is easily determined and compensated for using a lookup table based on the geographic location [10–13]. The National Oceanic and Atmosphere Administration (NOAA) provides magnetic declinations all over the world (http://www.ngdc.noaa.gov/geomag/declination.shtml). After the declination is determined, the compensation parameters can be computed by rotating magnetometer to measure all direction magnetic fields.In reality, a magnetic distortion exists because the magnetometer is mounted on a vehicle that includes ferrous materials nearby. The ferrous materials will distort, or bend, the magnetic field. In previous research [10–13], these magnetic distortions were divided into more detailed factors for higher accuracy. In this paper, the distortions are divided into two categories: hard- and soft-iron effects. First, the hard-iron effect comes from any nearby iron or steel on the vehicle. This effect produces a constant magnitude error in the magnetic reading along each axis. The soft-iron effect comes from the interaction of the earth’s magnetic field and any magnetically soft material. The amount of distortion from the soft-iron depends on the sensor orientation [10, 11].Those distortions can be accounted for and should be removed from the magnetic readings before computing the magnetic heading. The horizontal magnetic readings should make a circle for the 360° rotation without any distortions. In reality, it shows an ellipsoid pattern, as well as biases with respect to the origin as shown in Figure2. We can compensate for these distortions by determining the scale factor (s
x
,
s
y) and offset (o
x
,
o
y):(3)
m
-
x
h
m
-
y
h
=
s
x
·
m
x
h
s
y
·
m
y
h
+
o
x
o
y
.The scale factor changes the ellipsoid to a circle, and the offset moves the center of circle to (
0,0
). These two factors are calculated from the maximum and minimum values of the magnetic readings for the 360° rotation. The scale factor is determined by using the following equations:(4)
s
x
=
max
⁡
1
,
max
⁡
m
y
h
-
min
⁡
m
y
h
max
⁡
m
x
h
-
min
⁡
m
x
h
,
s
y
=
max
⁡
1
,
max
⁡
m
x
h
-
min
⁡
m
x
h
max
⁡
m
y
h
-
min
⁡
m
y
h
.And the offset is determined by using the scale factor minimum and maximum readings as follows:(5)
o
x
=
max
⁡
m
x
h
-
min
⁡
m
x
h
2
-
max
⁡
m
x
h
·
s
x
,
o
y
=
max
⁡
m
y
h
-
min
⁡
m
y
h
2
-
max
⁡
m
y
h
·
s
y
.Once the magnetic readings are compensated by using the tilt angle, declination, and distortions, then the magnetic heading can be computed by using an arctangent function as follows: (6)
ψ
m
a
g
=
tan
-
1
⁡
m
-
y
h
m
-
x
h
.Finally, we can obtain the magnetic heading, which will be combined with the NHCs using an EKF.Figure 2
Magnetic readings on horizontal plane: blue circle represents raw measurements and red circle shows values after compensation by calibration parameters.


## 3. Nonholonomic Constraints (NHCs)

An INS has inherent drift error in its navigation solutions unless correction information is provided by an external system. There are several techniques to reduce this error, such as improved navigation algorithms [24, 25, 28], a denoising IMU [29], and the use of vehicle kinematic constraints [9, 25–27]. This paper focuses on vehicle kinematic constraints, which are called NHCs.The basic assumption for NHCs is that a ground vehicle is governed by 2 NHCs if it does not jump off or slide on the ground. In reality, these constraints are acceptable for general vehicle operation, even though they are sometimes violated as a result of the vibrations of the vehicle’s engine and suspension and by side-slip during cornering [9, 23–27]. The constraints can be regarded as a zero velocity update (ZUPT) for the velocity of the vehicle in the plane perpendicular to the forward direction (x-axis). In other words, the velocities on the lateral and vertical axes (y and z velocities on body frame) in Figure 3 are almost zero, as shown below:(7)
v
y
b
≈
0
,
v
z
b
≈
0
.A coordinate transformation matrix (C
n
b) is used to convert the velocity from the navigation frame to the body frame, and it is expressed as follows:(8)
v
^
b
=
C
n
b
v
^
n
.The y- and z-axis velocities computed using (8) should be zero because of the NHCs in (7). However, they are not exactly zero because of the velocity and attitude estimation errors of the INS. These errors can be estimated and corrected by using a Kalman filter. Two velocity residuals from the NHCs are considered as measurements for the navigation filter. The equation for the residuals (r) is expressed as follows:(9)
r
=
δ
v
y
b
δ
v
z
b
=
v
^
y
b
-
v
y
b
v
^
z
b
-
v
z
b
=
v
^
y
b
v
^
z
b
.A linearized observation model can be obtained by applying perturbation in (8), which can be expressed as follows:(10)
δ
v
y
b
δ
v
z
b
=
H
N
H
C
,
v
H
N
H
C
,
ψ
δ
v
n
δ
ψ
n
,
H
N
H
C
,
v
=
C
12
C
22
C
32
C
13
C
23
C
33
,
H
N
H
C
,
ψ
=
-
v
d
C
22
+
v
e
C
32
v
d
C
12
-
v
n
C
32
-
v
e
C
12
+
v
n
C
22
-
v
d
C
23
+
v
e
C
33
v
d
C
13
-
v
n
C
33
-
v
e
C
13
+
v
n
C
23
,where C
i
j is the ith row and jth column of C
b
n and v
n, v
e, and v
d denote the north, east, and down velocities, respectively.Figure 3
Velocity presented on body frame with NHCs.The NHCs can improve the velocity estimation, which also improves the position estimation. However, because external and absolute information is still not available, it is only possible to obtain a relative navigation solution. Moreover, this technique is vulnerable to errors in the yaw estimation.


## 4. Estimation Filter

The equations for the magnetic heading and NHCs were presented in the previous section. The magnetic heading is computed from the corrected magnetic readings using an arc-tangent function. We can also obtain two additional measurement equations from the velocity constraints of the NHCs. This section will address the details of the filter structure used to implement the navigation algorithm with an IMU and a magnetometer.An EKF is used for the navigation filter. EKF has two distinct steps: predict and update. In predict step, the state vector is propagated from the current epoch into the next epoch by using a system dynamic model. Then, we can predict the state for the next time step. After the prediction, the predicted state vector is refined using a measurement, and the state vector is estimated, which minimizes the square of the error [30–33]. To implement the EKF, the state vector and system model should be defined for the prediction and observation model for the update.


### 4.1. System Model

First, the state vector is defined on the navigation frame using the position, velocity, and attitude. It is expressed as follows:(11)
x
=
p
n
v
n
ψ
n
T
.The system model for EKF should be linear because of the EKF constraints. Equation (1) can be linearized and expressed with respect to the error of the state vector in (11). The final system model with the error state vector (δ
x) can be written as follows:(12)
δ
x
k
+
1
=
F
k
δ
x
k
+
w
k
,
F
k
=
I
3
×
3
Δ
t
I
3
×
3
0
3
×
3
0
3
×
3
I
3
×
3
Δ
t
f
n
×
0
3
×
3
0
3
×
3
I
3
×
3
,where I is the identity matrix, 0 is a zero matrix, f
n
× is the skew-symmetric matrix of f
n, the subscript k denotes the time stamp, and the subscript a
×
b shows the number of matrix rows (a) and columns (b). The process noise (w
k) is assumed to be white noise. Equation (12) propagates the state vector into the next time step (k
+
1).


### 4.2. Observation Model

From (6) and (10), we can obtain the three observation equations for the magnetic heading and NHCs. In addition to these equations, the acceleration readings can be used as measurements to predict the roll and pitch angles if the accelerations are within reliable boundaries. The accelerometer can be regarded as a gravity sensor when the accelerations are so low that we can consider them to be equal to zero [34, 35]. The accelerometer reading should be f
b
≈
g under low accelerations. Then, the roll and pitch angles can be estimated by analyzing the magnitude of the acceleration reading on each axis. The acceleration range is determined as shown in [34], and it is set to 0.9
g
≤
f
b
≤
1.1
g in this paper. The relationship between the accelerometer reading and the attitude is written as follows:(13)
f
b
=
C
n
b
f
n
=
g
sin
⁡
θ
-
g
sin
⁡
ϕ
cos
⁡
θ
-
g
cos
⁡
ϕ
cos
⁡
θ
,
(14)
ϕ
=
tan
-
1
⁡
-
f
y
b
-
f
z
b
,
θ
=
tan
-
1
⁡
f
x
b
f
y
b
2
+
f
z
b
2
.Finally, we can determine the magnetic heading (ψ
m
a
g), y and z velocities on body frame (v
y
b
,
v
z
b), and roll and pitch angles based on the acceleration (ϕ
a
c
c
,
θ
a
c
c). The measurement order is slightly changed, and the observation mode can be expressed as follows:(15)
z
=
v
y
b
v
z
b
ϕ
a
c
c
θ
a
c
c
ψ
m
a
g
T
,
z
k
=
H
k
δ
x
k
+
υ
k
,
H
k
=
0
2
×
3
H
N
H
C
,
v
H
N
H
C
,
ψ
0
3
×
3
0
3
×
3
I
3
×
3
,where the measurement noise (υ
k) is assumed to be white noise. This model is only reliable under low accelerations. Therefore, we need additional observation model for high accelerations. The roll and pitch measurements are no longer reliable under a high acceleration. Thus, the observation model should be changed as follows:(16)
z
=
v
y
b
v
z
b
ψ
m
a
g
T
,
(17)
H
k
=
0
2
×
3
H
N
H
C
,
v
H
N
H
C
,
ψ
0
1
×
3
0
1
×
3
0
0
1
.


### 4.3. Filter Structure

We obtain the system model and observation model to implement the EKF. Figure4 shows a block diagram of the proposed method for estimating the navigation solution using a magnetometer and an IMU. Basically, the navigation solution is computed using the INS block, and the output is not compensated until the EKF result is fed back recursively. The heading measurement is calculated using the tilt-compensated magnetic reading and calibration data, and then it is sent to the EKF as a measurement. In addition, the INS output is used to generate the NHCs with (7) and (8), whereas the roll and pitch are computed based on the acceleration using (14). The estimation filter is constructed from (11) to (17), and then the navigation solution is estimated recursively.Figure 4
Filter structure used to estimate navigation solution with magnetometer and IMU.In this section, the navigation filter was formulated to implement the EKF. The next section will discuss the results of an experimental test of the proposed method.


## 4.1. System Model

First, the state vector is defined on the navigation frame using the position, velocity, and attitude. It is expressed as follows:(11)
x
=
p
n
v
n
ψ
n
T
.The system model for EKF should be linear because of the EKF constraints. Equation (1) can be linearized and expressed with respect to the error of the state vector in (11). The final system model with the error state vector (δ
x) can be written as follows:(12)
δ
x
k
+
1
=
F
k
δ
x
k
+
w
k
,
F
k
=
I
3
×
3
Δ
t
I
3
×
3
0
3
×
3
0
3
×
3
I
3
×
3
Δ
t
f
n
×
0
3
×
3
0
3
×
3
I
3
×
3
,where I is the identity matrix, 0 is a zero matrix, f
n
× is the skew-symmetric matrix of f
n, the subscript k denotes the time stamp, and the subscript a
×
b shows the number of matrix rows (a) and columns (b). The process noise (w
k) is assumed to be white noise. Equation (12) propagates the state vector into the next time step (k
+
1).


## 4.2. Observation Model

From (6) and (10), we can obtain the three observation equations for the magnetic heading and NHCs. In addition to these equations, the acceleration readings can be used as measurements to predict the roll and pitch angles if the accelerations are within reliable boundaries. The accelerometer can be regarded as a gravity sensor when the accelerations are so low that we can consider them to be equal to zero [34, 35]. The accelerometer reading should be f
b
≈
g under low accelerations. Then, the roll and pitch angles can be estimated by analyzing the magnitude of the acceleration reading on each axis. The acceleration range is determined as shown in [34], and it is set to 0.9
g
≤
f
b
≤
1.1
g in this paper. The relationship between the accelerometer reading and the attitude is written as follows:(13)
f
b
=
C
n
b
f
n
=
g
sin
⁡
θ
-
g
sin
⁡
ϕ
cos
⁡
θ
-
g
cos
⁡
ϕ
cos
⁡
θ
,
(14)
ϕ
=
tan
-
1
⁡
-
f
y
b
-
f
z
b
,
θ
=
tan
-
1
⁡
f
x
b
f
y
b
2
+
f
z
b
2
.Finally, we can determine the magnetic heading (ψ
m
a
g), y and z velocities on body frame (v
y
b
,
v
z
b), and roll and pitch angles based on the acceleration (ϕ
a
c
c
,
θ
a
c
c). The measurement order is slightly changed, and the observation mode can be expressed as follows:(15)
z
=
v
y
b
v
z
b
ϕ
a
c
c
θ
a
c
c
ψ
m
a
g
T
,
z
k
=
H
k
δ
x
k
+
υ
k
,
H
k
=
0
2
×
3
H
N
H
C
,
v
H
N
H
C
,
ψ
0
3
×
3
0
3
×
3
I
3
×
3
,where the measurement noise (υ
k) is assumed to be white noise. This model is only reliable under low accelerations. Therefore, we need additional observation model for high accelerations. The roll and pitch measurements are no longer reliable under a high acceleration. Thus, the observation model should be changed as follows:(16)
z
=
v
y
b
v
z
b
ψ
m
a
g
T
,
(17)
H
k
=
0
2
×
3
H
N
H
C
,
v
H
N
H
C
,
ψ
0
1
×
3
0
1
×
3
0
0
1
.


## 4.3. Filter Structure

We obtain the system model and observation model to implement the EKF. Figure4 shows a block diagram of the proposed method for estimating the navigation solution using a magnetometer and an IMU. Basically, the navigation solution is computed using the INS block, and the output is not compensated until the EKF result is fed back recursively. The heading measurement is calculated using the tilt-compensated magnetic reading and calibration data, and then it is sent to the EKF as a measurement. In addition, the INS output is used to generate the NHCs with (7) and (8), whereas the roll and pitch are computed based on the acceleration using (14). The estimation filter is constructed from (11) to (17), and then the navigation solution is estimated recursively.Figure 4
Filter structure used to estimate navigation solution with magnetometer and IMU.In this section, the navigation filter was formulated to implement the EKF. The next section will discuss the results of an experimental test of the proposed method.


## 5. Test and Analysis

### 5.1. Test Settings

An experimental test with a car was conducted to verify the proposed method. The data were collected at Konkuk University in Korea on August 13, 2013, and postprocessed using MATLAB. The target sensor was an Analog Devices ADIS16364, which included a three-axis accelerometer and gyroscope. The target magnetometer was a Honeywell HMC1053. The reference sensor was a NovAtel SPAN, which consisted of a ProPak-V3 GPS Receiver and Honeywell HG1700-AG11 IMU. A NovAtel DL-V3 Receiver was used for the reference station to compute precise and accurate solutions. The sensors are shown in Table1. Then, the reference position, velocity, and attitude were computed by using commercial software (NovAtel Inertial Explorer).Table 1
Sensors used in test.
Sensor
Model
Target sensor
IMU
Analog Devices ADIS16364
Magnetometer
Honeywell HMC1053
Reference system
Rover
IMU
Honeywell HG1700-AG11
GPS
NovAtel ProPak-3 Receiver
Reference station
NovAtel DL-V3 Receiver
Software
NovAtel Inertial ExplorerIn the test, the vehicle remained stationary for the first 200 s to obtain the zero velocity data. The initial attitude was obtained by using ZUPT with this stationary IMU data (gyrocompassing was not used because of low resolution of the target IMU). After that, the vehicle made three small circles to calibrate the magnetometer. Then, the vehicle followed an eight-shaped road three times as shown Figure5. Figure 6 shows the velocity and acceleration during the test. The proposed method (I
N
S
+
N
H
C
+
M
a
g) was analyzed in comparison to three conventional approaches, as well as the reference data from NovAtel SPAN. These three approaches included an INS, INS with magnetic heading information (I
N
S
+
M
a
g), and INS with NHCs (I
N
S
+
N
H
C).Figure 5
Test trajectory to collect the data.Figure 6
Reference velocity (a) and acceleration (b) during the test.
(a)
(b)All four navigation filters, including the proposed method, were implemented after the ZUPT, which was conducted by using the Kalman filter with a zero velocity condition. As a result, all of the methods started with the same initial values after 200 s. The magnetometer was assumed to be precalibrated prior to estimating the navigation solutions. The calibration was conducted by using the data collected from the circular track. The declination angle was determined from the NOAA lookup table to be 8.05° at the test site (Seoul, Korea). Finally, the calibration parameters were obtained as presented in Table2. The compensated magnetometer readings after the calibration are shown in Figure 2.Table 2
Magnetometer calibration results.
Axis
Parameter
Value
X
Scale factor
1.284
Offset
0.032
Y
Scale factor
1.000
Offset
0.029


### 5.2. Test Results

The estimated position results are presented in Figure7, and the position errors with respect to the reference are shown in Figure 8. Without NHCs, the position results were diverse in short time. When the magnetometer was integrated with the INS, the drift rate was slightly slower than that in the INS-only case. However, in both cases without NHCs (INS and “I
N
S
+
M
a
g”), the results were meaningless because they had significant errors and were difficult to use for position estimation. The position error for “I
N
S
+
N
H
C” was decreased dramatically compared to the error before the integration of NHCs. The trends of the estimated trajectory showed patterns similar to the reference, even though they had drift error. The proposed method (I
N
S
+
N
H
C
+
M
a
g), which integrated the magnetometer in addition to the NHCs, showed the best position estimation and was close to the reference trajectory. The error was four times better than that for “I
N
S
+
N
H
C.” The “I
N
S
+
N
H
C” results moved in the wrong direction after the initial circular turn, and the drift rate was higher than that of the proposed method.Figure 7
Estimated trajectory on horizontal plane (east and north axes).Figure 8
Estimated position error (norm) with respect to reference.Figure9 shows the estimated velocity error. Each error comes from taking the norm of the three-axis velocity errors. The navigation solution was not estimated until 200 s had passed because of the ZUPT initialization. The NHCs could keep the error from diverging, whereas both results without NHCs had significant errors. Their maximum errors were around 60 and 90 m/s (more than 200 km/s), even though the speed did not exceed 7 m/s during the test. Thus, these errors were not reasonable. In addition, both results from “I
N
S
+
N
H
C” and “I
N
S
+
N
H
C
+
M
a
g” were not diverse. This was because the NHCs used velocity constraints, which could prevent the velocity from drifting. As a result, the NHCs improved the position estimations.Figure 9
Estimated velocity error (norm) with respect to reference.The attitude estimation results are shown in Figure10 and listed in Table 3. The results were analyzed for each axis. Three parameters (mean, standard deviation (STD), and root mean square (RMS)) were introduced for a statistical analysis. In terms of the roll and pitch angles, the NHCs improved the estimation performance by approximately two times. For the yaw angle, the magnetometer definitely kept the error bound lower, whereas in other cases without the magnetometer, it continuously increased. “I
N
S
+
N
H
C” did not have reliable performance for the yaw estimation, although it could estimate the roll and pitch angles well. Therefore, it was determined that the proposed method showed the best attitude estimation performance for all the axes.Table 3
Attitude error analysis with respect to mean, STD, and RMS.
Mean
STD
RMS
Roll
Pitch
Yaw
Roll
Pitch
Yaw
Roll
Pitch
Yaw
INS
0.14
0.38
−48.60
1.00
1.06
32.02
1.01
1.12
58.19
INS + Mag
0.13
0.45
2.39
1.00
1.19
5.85
1.00
1.27
6.32
INS + NHC
0.08
−0.12
−47.70
0.26
0.47
30.80
0.27
0.48
56.77
INS + NHC + Mag
0.07
−0.23
−1.06
0.26
0.50
4.73
0.28
0.55
4.84Figure 10
Estimated attitude error on each axis with respect to reference.In this section, the estimation performance for a navigation solution was analyzed. A comprehensive analysis of the performance in terms of the position, velocity, and attitude showed that the proposed method had the best performance for all of the navigation information because of the magnetometer and NHCs.The position solution still had a little drift error because of the absence of external information such as that from a GPS. It would be possible to use a GPS to bound the position solution, even though the update rate is fairly low. Therefore, the proposed method could be used to obtain reliable estimation solutions for ground vehicle navigation.


## 5.1. Test Settings

An experimental test with a car was conducted to verify the proposed method. The data were collected at Konkuk University in Korea on August 13, 2013, and postprocessed using MATLAB. The target sensor was an Analog Devices ADIS16364, which included a three-axis accelerometer and gyroscope. The target magnetometer was a Honeywell HMC1053. The reference sensor was a NovAtel SPAN, which consisted of a ProPak-V3 GPS Receiver and Honeywell HG1700-AG11 IMU. A NovAtel DL-V3 Receiver was used for the reference station to compute precise and accurate solutions. The sensors are shown in Table1. Then, the reference position, velocity, and attitude were computed by using commercial software (NovAtel Inertial Explorer).Table 1
Sensors used in test.
Sensor
Model
Target sensor
IMU
Analog Devices ADIS16364
Magnetometer
Honeywell HMC1053
Reference system
Rover
IMU
Honeywell HG1700-AG11
GPS
NovAtel ProPak-3 Receiver
Reference station
NovAtel DL-V3 Receiver
Software
NovAtel Inertial ExplorerIn the test, the vehicle remained stationary for the first 200 s to obtain the zero velocity data. The initial attitude was obtained by using ZUPT with this stationary IMU data (gyrocompassing was not used because of low resolution of the target IMU). After that, the vehicle made three small circles to calibrate the magnetometer. Then, the vehicle followed an eight-shaped road three times as shown Figure5. Figure 6 shows the velocity and acceleration during the test. The proposed method (I
N
S
+
N
H
C
+
M
a
g) was analyzed in comparison to three conventional approaches, as well as the reference data from NovAtel SPAN. These three approaches included an INS, INS with magnetic heading information (I
N
S
+
M
a
g), and INS with NHCs (I
N
S
+
N
H
C).Figure 5
Test trajectory to collect the data.Figure 6
Reference velocity (a) and acceleration (b) during the test.
(a)
(b)All four navigation filters, including the proposed method, were implemented after the ZUPT, which was conducted by using the Kalman filter with a zero velocity condition. As a result, all of the methods started with the same initial values after 200 s. The magnetometer was assumed to be precalibrated prior to estimating the navigation solutions. The calibration was conducted by using the data collected from the circular track. The declination angle was determined from the NOAA lookup table to be 8.05° at the test site (Seoul, Korea). Finally, the calibration parameters were obtained as presented in Table2. The compensated magnetometer readings after the calibration are shown in Figure 2.Table 2
Magnetometer calibration results.
Axis
Parameter
Value
X
Scale factor
1.284
Offset
0.032
Y
Scale factor
1.000
Offset
0.029


## 5.2. Test Results

The estimated position results are presented in Figure7, and the position errors with respect to the reference are shown in Figure 8. Without NHCs, the position results were diverse in short time. When the magnetometer was integrated with the INS, the drift rate was slightly slower than that in the INS-only case. However, in both cases without NHCs (INS and “I
N
S
+
M
a
g”), the results were meaningless because they had significant errors and were difficult to use for position estimation. The position error for “I
N
S
+
N
H
C” was decreased dramatically compared to the error before the integration of NHCs. The trends of the estimated trajectory showed patterns similar to the reference, even though they had drift error. The proposed method (I
N
S
+
N
H
C
+
M
a
g), which integrated the magnetometer in addition to the NHCs, showed the best position estimation and was close to the reference trajectory. The error was four times better than that for “I
N
S
+
N
H
C.” The “I
N
S
+
N
H
C” results moved in the wrong direction after the initial circular turn, and the drift rate was higher than that of the proposed method.Figure 7
Estimated trajectory on horizontal plane (east and north axes).Figure 8
Estimated position error (norm) with respect to reference.Figure9 shows the estimated velocity error. Each error comes from taking the norm of the three-axis velocity errors. The navigation solution was not estimated until 200 s had passed because of the ZUPT initialization. The NHCs could keep the error from diverging, whereas both results without NHCs had significant errors. Their maximum errors were around 60 and 90 m/s (more than 200 km/s), even though the speed did not exceed 7 m/s during the test. Thus, these errors were not reasonable. In addition, both results from “I
N
S
+
N
H
C” and “I
N
S
+
N
H
C
+
M
a
g” were not diverse. This was because the NHCs used velocity constraints, which could prevent the velocity from drifting. As a result, the NHCs improved the position estimations.Figure 9
Estimated velocity error (norm) with respect to reference.The attitude estimation results are shown in Figure10 and listed in Table 3. The results were analyzed for each axis. Three parameters (mean, standard deviation (STD), and root mean square (RMS)) were introduced for a statistical analysis. In terms of the roll and pitch angles, the NHCs improved the estimation performance by approximately two times. For the yaw angle, the magnetometer definitely kept the error bound lower, whereas in other cases without the magnetometer, it continuously increased. “I
N
S
+
N
H
C” did not have reliable performance for the yaw estimation, although it could estimate the roll and pitch angles well. Therefore, it was determined that the proposed method showed the best attitude estimation performance for all the axes.Table 3
Attitude error analysis with respect to mean, STD, and RMS.
Mean
STD
RMS
Roll
Pitch
Yaw
Roll
Pitch
Yaw
Roll
Pitch
Yaw
INS
0.14
0.38
−48.60
1.00
1.06
32.02
1.01
1.12
58.19
INS + Mag
0.13
0.45
2.39
1.00
1.19
5.85
1.00
1.27
6.32
INS + NHC
0.08
−0.12
−47.70
0.26
0.47
30.80
0.27
0.48
56.77
INS + NHC + Mag
0.07
−0.23
−1.06
0.26
0.50
4.73
0.28
0.55
4.84Figure 10
Estimated attitude error on each axis with respect to reference.In this section, the estimation performance for a navigation solution was analyzed. A comprehensive analysis of the performance in terms of the position, velocity, and attitude showed that the proposed method had the best performance for all of the navigation information because of the magnetometer and NHCs.The position solution still had a little drift error because of the absence of external information such as that from a GPS. It would be possible to use a GPS to bound the position solution, even though the update rate is fairly low. Therefore, the proposed method could be used to obtain reliable estimation solutions for ground vehicle navigation.


## 6. Conclusion

A navigation algorithm was proposed to improve the inertial navigation performance of a ground vehicle using magnetic measurements and dynamic constraints. To improve the estimation performance, a three-axis magnetometer was used to estimate the heading angle and NHCs were introduced to increase the correlation between the velocity and the attitude equation. The NHCs could provide feedback from the velocity to the attitude, which made the navigation solution more robust compared to conventional inertial navigation equations that only transfer information in one direction, from the attitude to velocity. Additionally, acceleration-based roll and pitch estimations were applied to decrease the drift when the acceleration was within certain boundary. The magnetometer and NHCs were combined with an EKF. An experimental test was conducted to verify the proposed method. The position and velocity error was decreased dramatically when the NHCs were integrated, whereas the attitude errors were decreased by the magnetometer integration. The dynamic constraints could dramatically decrease the drift of the position and velocity errors, and the roll and pitch errors were decreased by approximately 50% compared with the cases without NHCs. However, the yaw angle still had drift. We conclude that the proposed method could improve all of the navigation performances as a result of using the magnetometer and dynamic constraints, based on a comprehensive analysis of the performance in terms of the position, velocity, and attitude.The proposed method could improve the estimation performances for the position, velocity, and attitude without additional hardware, except an inertial sensor and a magnetometer. Additionally, the method increased the accuracy compared with a conventional INS and provided accurate information for a longer time. As a result, the method could increase the survival time of dead reckoning in GNSS-denied environments such as indoors or in urban canyons. Therefore, the proposed method could be effective for ground vehicles, indoor navigation, mobile robots, vehicle navigation in urban canyons, and navigation in any GNSS-denied environment.



---

*Source: 435062-2015-08-10.xml*