# Integrated Navigation Fusion Strategy of INS/UWB for Indoor Carrier Attitude Angle and Position Synchronous Tracking

Authors: Qigao Fan; Yaheng Wu; Jing Hui; Lei Wu; Zhenzhong Yu; Lijuan Zhou
Journal: The Scientific World Journal
 (2014)

---

## Abstract

In some GPS failure conditions, positioning for mobile target is difficult. This paper proposed a new method based on INS/UWB for attitude angle and position synchronous tracking of indoor carrier. Firstly, error model of INS/UWB integrated system is built, including error equation of INS and UWB. And combined filtering model of INS/UWB is researched. Simulation results show that the two subsystems are complementary. Secondly, integrated navigation data fusion strategy of INS/UWB based on Kalman filtering theory is proposed. Simulation results show that FAKF method is better than the conventional Kalman filtering. Finally, an indoor experiment platform is established to verify the integrated navigation theory of INS/UWB, which is geared to the needs of coal mine working environment. Static and dynamic positioning results show that the INS/UWB integrated navigation system is stable and real-time, positioning precision meets the requirements of working condition and is better than any independent subsystem.


---

## Body

## 1. Introduction

For many years, positioning technology is developing rapidly. On the ground and in the skies, positioning service can be provided by global positioning systems (GPS) stably and reliably [1, 2]. But in some places, such as coal mine, urban canyons and indoors, due to satellite signal blockage, GPS cannot provide a solution with consistent and long-term stable accuracy. In view of this, indoor positioning becomes a research hotspot.Whether in the case of indoors or outdoors, inertial navigation system (INS) can output acceleration and attitude angle of carrier synchronously [3, 4]. Yet, it has inherent defects when positioning independence for a long time. Positioning accuracy of INS decreased as the drift error [5]. Generally, GPS is used to provide compensation for INS. But we know that GPS signal is disabled in the case of indoor. Based on the above analysis, we researched ultra wide band (UWB) positioning technique, a kind of effective indoor wireless sensor positioning strategy, as compensation for INS.UWB is a new advanced and promising positioning technology with centimeter level ranging accuracy and high speed of data transmission characteristics, especially for indoor applications. But in some complicated cases, UWB signal is under the influence of the multipath effect and non-line-of-sight conditions [6, 7]. Ascher et al. [8] presented a tightly coupled UWB/INS system for pedestrian indoor applications and analyzed the influence of integrity monitoring algorithms. Xu et al. [9] researched a new approach using least squares support vector machine and H∞ filter for integration of INS/WSN and the analysis method is worth using for reference. de Angelis et al. [10] proposed an indoor positioning system based on INS and UWB and described a system solution briefly. Zwirello et al. [11] presented a simulation-based feasibility study on tightly coupled model of UWB and inertial data integration. This approach gives the possibility to profit from UWB measurements even if no direct TDOA solution would be available and step-length-update to smooth the calculated trajectory between consecutive UWB updates. Hol et al. [12] designed a six-DOF tracking system combining UWB measurements with low-cost MEMS inertial measurements. The tracking system not only estimates the position of the sensor unit, but also provides velocity estimates. Evennou and Marx [13] presented an aided dead-reckoning navigation structure and signal processing algorithms for self-localization of an autonomous mobile device by fusing pedestrian dead reckoning and WiFi signal strength measurements, and the system accuracy can be further improved. Tanigawa et al. [14] designed an experimental system of GPS/INS/UWB and studied a multisensor fusion algorithm. Yet, it is only for outdoor use.From the above, INS/UWB integrated system is a very promising positioning technology. The aim of this paper is to present our work towards closed-loop solution of general indoor positioning based on a combination of INS and UWB distance measurement system. The modeling of INS/UWB integrated system includes system error equation, filtering model, optimal comprehensive strategy, and system experiment. Research results will be used to realize carrier attitude angle and position tracking in indoor environment.


## 2. Error Model of INS/UWB Integrated System

In order to establish the coupling model of INS/UWB, system error needs to be analyzed. Precondition for building the error model of INS/UWB integrated system is to know the error source and transfer rule.As shown in Figure1, INS error source mainly includes inertial instrument error (gyroscope drift and accelerometer zero bias and measurement noise, etc.), inertial instrument of installation error, system error of initial condition (initial speed of the coal mining machine and position error), system calculation error (integral phase eliminating the high order), and kinds of interference caused by the error. And UWB positioning system error source mainly includes wireless measurement error (multipath diffraction, time delay, and context switching between non-line-of-sight and stadias, etc.), the anchor node coordinate drift, wireless positioning decoding error (nonlinear equations), and node fault. Error transfer rule of INS/UWB is as follows.(i)
Attitude angle error is produced by angular velocity error through once integral and initial deflection, combined with the state of carrier space error, which forms error anglesΔΦE, ΔΦ, and ΔΦU. On this basis, coupling the error of acceleration measurement and zero offset, acceleration errors ΔaE and ΔaN can be got.(ii)
Further through the integration of the primary and secondary, and combined with the initial position and velocity deviation, we can get velocity errorsΔVE and ΔVN and the navigation position errors Δλ and ΔL.(iii)
On the one side of the UWB, through the error model, we can obtain the vector errorΔPUWB between the reference node and mobile node. On the basis of comprehensive analysis and considering system error transfer relationship, the error equation on all phases is established and the optimal filtering strategy is used; then the correction of inertial attitude, position, and UWB ranging error is completed under the collaborative positioning system.Figure 1
Program plan of multiparameter error propagation and compensation.


### 2.1. Error Model of INS

INS error state equation has 13 dimensions, which includes three gyroscope drift errors, three random errors of liner accelerometer, three attitude angle errors, two velocity errors, and two position error states. Selecting ENU coordinate system, we set the gyroscope random errorΔε=[εE,εN,εU]T and geographic coordinate of three axial line accelerometer random error ∇i=[∇E,∇N,∇U]T; according to the basic error equation of INS [15, 16], we can get the following.(1)
Attitude angle error equation is(1)ΔΦ˙E=-δVNR1+(VER2tanL+ωesinL)ΔΦN-(VER+ωecos⁡L)ΔΦU+εE,ΔΦ˙N=δVER2-ωesinL-(VER2tanL+ωesinL)ΔΦE-VNR1ΔΦU+εN,ΔΦ˙U=δVER2tanL+(VER2sec2L+ωecos⁡L)ΔL+(VER2+ωecos⁡L)ΔΦE+VNR1ΔΦN+εU.(2)
Velocity error equation is(2)ΔV˙E=fNΔΦU-fUΔΦN+VNR2ΔVEtanL+(VER2tanL+2ωesinL)ΔΦN-(2ωecos⁡LVE+VEVNR2sec2L)ΔL+∇E,ΔV˙N=fUΔΦN-fEΔΦU-(2ωesinL+VER2ΔVEtanL)ΔΦU-(VE2R2sec2L+2ωecos⁡LVN)ΔL+∇N.(3)
Position error equation is(3)ΔL˙=ΔVNR1,ΔV˙N=secLR2ΔVE+VER2ΔLsecLtanL.Through the error equation, we can analyze the response form of the specific error amount to the particular error factor, and then propagation characteristics of inertial navigation error can be analyzed.Desirable state vector is(4)XI=[ΔΦE,ΔΦN,ΔΦU,ΔVE,ΔVN,ΔL,Δλ,εE,εN,εU,∇E,∇N,∇U]T.INS system error state equation is(5)X˙I(t)=FI(t)XI(t)+WI(t),
where  WI(t) is the system noise matrix.F
I
(
t
) is the system coefficient matrix, which can be represented as follows:
(6)FI(t)=(F1(t)F2(t)0F3(t))13×13,
where F1(t) is a system matrix which includes 7 related navigation parameters of carrier positioning, and the dimension is 7 × 7. F1(t)=[F1(t)7×4,F1(t)7×3]:(7)F1(t)7×4=(VNtanLR2-VUR22ωesinL+VEtanLR20-fU-(2VEtanLR2+2ωesinL)0fU00-1R10ωesinL+VEtanLR21R20-ωesinL-VEtanLR20tanLR20ωecos⁡L+VER2VNR101R100secLR2000),F1(t)7×3=(fN2ωeVNcos⁡L+VEVNR2sec2L0-fE-VE(VER2sec2L+2ωecos⁡L)0-ωecos⁡L-VER200-VNR1-ωesinL00ωecos⁡L+VER2sec2L00000VER2secLtanL0),where F2(t) is the transformation matrix of random error between the gyroscope and accelerometer, and the dimension is 7 × 6:
(8)F2(t)=(O2×3CbnCbnO2×3O2×3O2×3).C
b
n is transformation matrix from carrier coordinate system to navigation coordinate system:(9)Cbn=(cos⁡φcos⁡γ+sinφsinγ-sinφcos⁡γ+cos⁡φsinθsinγ-cos⁡θsinγsinφcos⁡θcos⁡φcos⁡θsinθcos⁡φsinγ-sinφsinθcos⁡γ-sinφsinγ-cos⁡φsinθcos⁡γcos⁡θcos⁡γ).Andφ, θ, and γ are carrier attitude angles.WhereF3(t) is the system matrix between the gyroscope and the accelerometer random error and the dimension is 6 × 6, Tg and Ta are time constant of gyroscope and accelerometer error, respectively:
(10)F3(t)=diag⁡[-1Tgx,-1Tgy,-1Tgz,-1Tax,-1Tay,-1Taz].We carried on the simulation about three main factors affecting inertial navigation positioning, which are the acceleration drift, the gyroscope drift, and the environmental noise, respectively. We added the sensor error to the coordinate positioning system of carrier, as shown in Table1.Table 1
Drift simulation conditions of inertial drift of the inertial navigation system.
Types of errors
Case  1
Case  2
Case  3
x-axis accelerometer drift (ug)
0
10
50
y-axis accelerometer drift (ug)
0
10
50
z-axis accelerometer drift (ug)
0
10
50
x-axis gyroscope drift (deg/h)
0
0.01
0.05
y-axis gyroscope drift (deg/h)
0
0.01
0.05
z-axis gyroscope drift (deg/h)
0
0.01
0.05
White noise coefficient of deviation
0
0.01
0.05Simulation results were compared and analyzed, as shown in Figure2 to Figure 7.Figure 2
Pitch angle errors with different sensor error.The simulation results show that when we consider the drift of inertial device in SINS, corresponding system error increases. According to the attitude angle error results (Figures2, 3, and 4), we know that, with the inertial device drift and random noise increasing, the attitude angle error of carrier amplitude increases, but it does not show the tendency of divergence. Overall angle error is controlled within ±2° and the maximum of yaw angle error is −1.491°. Because of acceleration and gyroscope drift, the velocity error amplitude increases rapidly. As shown in Figures 5 and 6, the maximum value of northern velocity is −98.3 m/s, and, according to the speed integral of the carrier, we know that there is divergence feature for positioning error obviously, as shown in Figure 7, which embodies the imperfection of using the pure inertial navigation system for carrier location; namely, due to the inertial device drift and random error, location information will cause serious losing of the reliability within a short time and will be not suitable for the carrier positioning.Figure 3
Roll angle error with different sensor error.Figure 4
Yaw angle error with different sensor error.Figure 5
North velocity error with different sensor error.Figure 6
East velocity error with different sensor error.Figure 7
Position error with different sensor error.


### 2.2. Error Model of UWB

In many of the enclosed environment applications, UWB positioning is two-dimensional plane, and one example is the UWB positioning model shown in Figure8.Figure 8
Node arrangement based on UWB in the enclosed environment.Precondition of the error analysis for UWB positioning system is establishing the relationship between the arrival time TOA error and circle location line error. As shown in Figure9, given the position line equation of UWB:
(11)RA=(x-x1)2+(y-y1)2,RB=(x-x2)2+(y-y2)2,(12)RA=cToA,RB=cToB,
where ToA and ToB are time differences of arrival of A(x1,y1) and B(x2,y2), respectively, mobile node of carrier P(x,y) can be got by (11).Figure 9
Positioning model of the carrier based on UWB.Definition of position line error is the vertical distance between the line of real position and the line of measuring position. Based on this, relationship between positioning error parameterΔu and position line error ΔR can be described as follows:
(13)ΔR=Δu(∂u/∂x)2+(∂u/∂y)2=cΔt,
where Δt is TOA arrival time error. Combined with (11), the location of the line equation is obtained, so, through the differential calculation with x,y,RA,RB, and RC, we get
(14)RAΔRA=(x-x1)Δx+(y-y1)Δy,RBΔRB=(x-x2)Δx+(y-y2)Δy,Δx=(y-y1)RAΔRA-(y-y1)RBΔRB(x-x1)(y-y2)-(x-x2)(y-y1),Δy=(x-x1)RAΔRA-(x-x1)RBΔRB(x-x2)(y-y1)-(x-x1)(y-y2).In the process of positioning, airborne movement node can receive signal, which is more than three reference nodes. IfΔt is zero, N equations of position line will meet a point, at the same time, according to observation equations determined in the abovementioned model, and then we can obtain no fuzzy solution of shearer location coordinates. But error is inevitable in practice system; the line of circle position formed by these reference points cannot meet a point; the position to solve the problem is evolved into overdetermined equations to solve the problem. Caffery positioning method [17, 18] is used in the paper. This paper will transform nonlinear equations of round position line into linear equations and then using the least-square method estimate airborne movement node location.WhenN airborne mobile node signals are received and N nonlinear equations on carrier position are given, by subtracting the N− 1 equation from the N equation,
(15)12(R12-R22+x22-x12+y22-y12)=(x2-x1)x+(y2-y1)y,12(R22-R32+x32-x22+y32-y22)=(x3-x2)x+(y3-y2)y,⋮12(RN-12-RN2+xN2-xN-12+yN2-yN-12)=(xN-xN-1)x+(yN-yN-1)y.After the transformation, we can make theN circle position line equations into N − 1 and then make the N nonlinear equations into N − 1. By selecting suitable reference node, further least-square solutions are got:
(16)x=(ATA)-1ATb,
where
(17)A=(x2-x1y2-y1x3-x2y3-y2⋮⋮xN-xN-1yN-yN-1),b=12(R12-R22+x22-x12+y22-y12R22-R32+x32-x22+y32-y12⋮RN-12-RN2+xN2-xN-12+yN2-yN-12),x=(xy).When parameter measurement error of UWB occurred, according to the abovementioned equations, goal error can be described as(18)Δx=(ΔxΔy)=(ATA)-1ATBR∘ΔR,
where ∘ is Schur product, B is coefficient matrix of the position line,  R is the distance between mobile node and reference node, and ΔR is the distance error. The parameter of each reference node is independent. Parameter equation is δR2. Then we can determine the positioning covariance as follows:
(19)Pd=δR2[(ATA)-1ATB]D[(ATA)-1ATB]T,
where D=diag⁡(R12,R22,…,RN2) and D is given by the coordinate estimation.


### 2.3. Combined Filtering Model of INS/UWB

INS and UWB are two separate systems and the coupling model is built in order to make the complementary advantages, as shown in Figure10. According to the coupling model, INS/UWB integrated navigation is realized by using feedback correction method in this paper. On the one hand, attitude and velocity error of INS are input to the Kalman filter. On the other hand, position error can be got by difference of output parameters between INS and UWB. Based on the estimates value of attitude error, velocity error, and position error, output correction is made for the navigation parameters.Figure 10
Coupled model diagram of INS and UWB.In the INS/UWB integrated navigation system, the real-time location given by INS is geographic longitude and latitude, while UWB is relative positioning information. DefiningLINS as the latitude output by INS and λINS as the longitude, L and λ are the real value, respectively. Then, location information can be represented as
(20)LINS=L+ΔL,λINS=λ+Δλ.At the same time,ΔRx and ΔRy are eastern and northern measurement distance error of UWB, respectively, after coordinate transformation, longitude, and latitude output of UWB are LUWB and λUWB, respectively:
(21)LUWB=L-ΔRyR1,λUWB=λ-ΔRxR2cos⁡L,
where R1 and R2 are the earth ellipsoid local meridian and the local prime vertical curvature radius, respectively. Taking location difference as observation measurement, then location observation equation can be represented:
(22)Zk(t)=[(LINS-LUWB)R1(λINS-λUWB)R2cos⁡L]=[R1ΔL+ΔRyR2Δλcos⁡L+ΔRx].We can get(23)Zk(t)=Jk(t)Xk(t)+Vk(t),
where
(24)Jk(t)=[O2×5,diag⁡[R1,R2cos⁡L],O2×3],Vk(t)=[ΔRy,ΔRx].As shown in (20), we can get the measurement equation of integrated system. Then, in this paper, optimal filtering strategy of INS/UWB will be researched.


## 2.1. Error Model of INS

INS error state equation has 13 dimensions, which includes three gyroscope drift errors, three random errors of liner accelerometer, three attitude angle errors, two velocity errors, and two position error states. Selecting ENU coordinate system, we set the gyroscope random errorΔε=[εE,εN,εU]T and geographic coordinate of three axial line accelerometer random error ∇i=[∇E,∇N,∇U]T; according to the basic error equation of INS [15, 16], we can get the following.(1)
Attitude angle error equation is(1)ΔΦ˙E=-δVNR1+(VER2tanL+ωesinL)ΔΦN-(VER+ωecos⁡L)ΔΦU+εE,ΔΦ˙N=δVER2-ωesinL-(VER2tanL+ωesinL)ΔΦE-VNR1ΔΦU+εN,ΔΦ˙U=δVER2tanL+(VER2sec2L+ωecos⁡L)ΔL+(VER2+ωecos⁡L)ΔΦE+VNR1ΔΦN+εU.(2)
Velocity error equation is(2)ΔV˙E=fNΔΦU-fUΔΦN+VNR2ΔVEtanL+(VER2tanL+2ωesinL)ΔΦN-(2ωecos⁡LVE+VEVNR2sec2L)ΔL+∇E,ΔV˙N=fUΔΦN-fEΔΦU-(2ωesinL+VER2ΔVEtanL)ΔΦU-(VE2R2sec2L+2ωecos⁡LVN)ΔL+∇N.(3)
Position error equation is(3)ΔL˙=ΔVNR1,ΔV˙N=secLR2ΔVE+VER2ΔLsecLtanL.Through the error equation, we can analyze the response form of the specific error amount to the particular error factor, and then propagation characteristics of inertial navigation error can be analyzed.Desirable state vector is(4)XI=[ΔΦE,ΔΦN,ΔΦU,ΔVE,ΔVN,ΔL,Δλ,εE,εN,εU,∇E,∇N,∇U]T.INS system error state equation is(5)X˙I(t)=FI(t)XI(t)+WI(t),
where  WI(t) is the system noise matrix.F
I
(
t
) is the system coefficient matrix, which can be represented as follows:
(6)FI(t)=(F1(t)F2(t)0F3(t))13×13,
where F1(t) is a system matrix which includes 7 related navigation parameters of carrier positioning, and the dimension is 7 × 7. F1(t)=[F1(t)7×4,F1(t)7×3]:(7)F1(t)7×4=(VNtanLR2-VUR22ωesinL+VEtanLR20-fU-(2VEtanLR2+2ωesinL)0fU00-1R10ωesinL+VEtanLR21R20-ωesinL-VEtanLR20tanLR20ωecos⁡L+VER2VNR101R100secLR2000),F1(t)7×3=(fN2ωeVNcos⁡L+VEVNR2sec2L0-fE-VE(VER2sec2L+2ωecos⁡L)0-ωecos⁡L-VER200-VNR1-ωesinL00ωecos⁡L+VER2sec2L00000VER2secLtanL0),where F2(t) is the transformation matrix of random error between the gyroscope and accelerometer, and the dimension is 7 × 6:
(8)F2(t)=(O2×3CbnCbnO2×3O2×3O2×3).C
b
n is transformation matrix from carrier coordinate system to navigation coordinate system:(9)Cbn=(cos⁡φcos⁡γ+sinφsinγ-sinφcos⁡γ+cos⁡φsinθsinγ-cos⁡θsinγsinφcos⁡θcos⁡φcos⁡θsinθcos⁡φsinγ-sinφsinθcos⁡γ-sinφsinγ-cos⁡φsinθcos⁡γcos⁡θcos⁡γ).Andφ, θ, and γ are carrier attitude angles.WhereF3(t) is the system matrix between the gyroscope and the accelerometer random error and the dimension is 6 × 6, Tg and Ta are time constant of gyroscope and accelerometer error, respectively:
(10)F3(t)=diag⁡[-1Tgx,-1Tgy,-1Tgz,-1Tax,-1Tay,-1Taz].We carried on the simulation about three main factors affecting inertial navigation positioning, which are the acceleration drift, the gyroscope drift, and the environmental noise, respectively. We added the sensor error to the coordinate positioning system of carrier, as shown in Table1.Table 1
Drift simulation conditions of inertial drift of the inertial navigation system.
Types of errors
Case  1
Case  2
Case  3
x-axis accelerometer drift (ug)
0
10
50
y-axis accelerometer drift (ug)
0
10
50
z-axis accelerometer drift (ug)
0
10
50
x-axis gyroscope drift (deg/h)
0
0.01
0.05
y-axis gyroscope drift (deg/h)
0
0.01
0.05
z-axis gyroscope drift (deg/h)
0
0.01
0.05
White noise coefficient of deviation
0
0.01
0.05Simulation results were compared and analyzed, as shown in Figure2 to Figure 7.Figure 2
Pitch angle errors with different sensor error.The simulation results show that when we consider the drift of inertial device in SINS, corresponding system error increases. According to the attitude angle error results (Figures2, 3, and 4), we know that, with the inertial device drift and random noise increasing, the attitude angle error of carrier amplitude increases, but it does not show the tendency of divergence. Overall angle error is controlled within ±2° and the maximum of yaw angle error is −1.491°. Because of acceleration and gyroscope drift, the velocity error amplitude increases rapidly. As shown in Figures 5 and 6, the maximum value of northern velocity is −98.3 m/s, and, according to the speed integral of the carrier, we know that there is divergence feature for positioning error obviously, as shown in Figure 7, which embodies the imperfection of using the pure inertial navigation system for carrier location; namely, due to the inertial device drift and random error, location information will cause serious losing of the reliability within a short time and will be not suitable for the carrier positioning.Figure 3
Roll angle error with different sensor error.Figure 4
Yaw angle error with different sensor error.Figure 5
North velocity error with different sensor error.Figure 6
East velocity error with different sensor error.Figure 7
Position error with different sensor error.


## 2.2. Error Model of UWB

In many of the enclosed environment applications, UWB positioning is two-dimensional plane, and one example is the UWB positioning model shown in Figure8.Figure 8
Node arrangement based on UWB in the enclosed environment.Precondition of the error analysis for UWB positioning system is establishing the relationship between the arrival time TOA error and circle location line error. As shown in Figure9, given the position line equation of UWB:
(11)RA=(x-x1)2+(y-y1)2,RB=(x-x2)2+(y-y2)2,(12)RA=cToA,RB=cToB,
where ToA and ToB are time differences of arrival of A(x1,y1) and B(x2,y2), respectively, mobile node of carrier P(x,y) can be got by (11).Figure 9
Positioning model of the carrier based on UWB.Definition of position line error is the vertical distance between the line of real position and the line of measuring position. Based on this, relationship between positioning error parameterΔu and position line error ΔR can be described as follows:
(13)ΔR=Δu(∂u/∂x)2+(∂u/∂y)2=cΔt,
where Δt is TOA arrival time error. Combined with (11), the location of the line equation is obtained, so, through the differential calculation with x,y,RA,RB, and RC, we get
(14)RAΔRA=(x-x1)Δx+(y-y1)Δy,RBΔRB=(x-x2)Δx+(y-y2)Δy,Δx=(y-y1)RAΔRA-(y-y1)RBΔRB(x-x1)(y-y2)-(x-x2)(y-y1),Δy=(x-x1)RAΔRA-(x-x1)RBΔRB(x-x2)(y-y1)-(x-x1)(y-y2).In the process of positioning, airborne movement node can receive signal, which is more than three reference nodes. IfΔt is zero, N equations of position line will meet a point, at the same time, according to observation equations determined in the abovementioned model, and then we can obtain no fuzzy solution of shearer location coordinates. But error is inevitable in practice system; the line of circle position formed by these reference points cannot meet a point; the position to solve the problem is evolved into overdetermined equations to solve the problem. Caffery positioning method [17, 18] is used in the paper. This paper will transform nonlinear equations of round position line into linear equations and then using the least-square method estimate airborne movement node location.WhenN airborne mobile node signals are received and N nonlinear equations on carrier position are given, by subtracting the N− 1 equation from the N equation,
(15)12(R12-R22+x22-x12+y22-y12)=(x2-x1)x+(y2-y1)y,12(R22-R32+x32-x22+y32-y22)=(x3-x2)x+(y3-y2)y,⋮12(RN-12-RN2+xN2-xN-12+yN2-yN-12)=(xN-xN-1)x+(yN-yN-1)y.After the transformation, we can make theN circle position line equations into N − 1 and then make the N nonlinear equations into N − 1. By selecting suitable reference node, further least-square solutions are got:
(16)x=(ATA)-1ATb,
where
(17)A=(x2-x1y2-y1x3-x2y3-y2⋮⋮xN-xN-1yN-yN-1),b=12(R12-R22+x22-x12+y22-y12R22-R32+x32-x22+y32-y12⋮RN-12-RN2+xN2-xN-12+yN2-yN-12),x=(xy).When parameter measurement error of UWB occurred, according to the abovementioned equations, goal error can be described as(18)Δx=(ΔxΔy)=(ATA)-1ATBR∘ΔR,
where ∘ is Schur product, B is coefficient matrix of the position line,  R is the distance between mobile node and reference node, and ΔR is the distance error. The parameter of each reference node is independent. Parameter equation is δR2. Then we can determine the positioning covariance as follows:
(19)Pd=δR2[(ATA)-1ATB]D[(ATA)-1ATB]T,
where D=diag⁡(R12,R22,…,RN2) and D is given by the coordinate estimation.


## 2.3. Combined Filtering Model of INS/UWB

INS and UWB are two separate systems and the coupling model is built in order to make the complementary advantages, as shown in Figure10. According to the coupling model, INS/UWB integrated navigation is realized by using feedback correction method in this paper. On the one hand, attitude and velocity error of INS are input to the Kalman filter. On the other hand, position error can be got by difference of output parameters between INS and UWB. Based on the estimates value of attitude error, velocity error, and position error, output correction is made for the navigation parameters.Figure 10
Coupled model diagram of INS and UWB.In the INS/UWB integrated navigation system, the real-time location given by INS is geographic longitude and latitude, while UWB is relative positioning information. DefiningLINS as the latitude output by INS and λINS as the longitude, L and λ are the real value, respectively. Then, location information can be represented as
(20)LINS=L+ΔL,λINS=λ+Δλ.At the same time,ΔRx and ΔRy are eastern and northern measurement distance error of UWB, respectively, after coordinate transformation, longitude, and latitude output of UWB are LUWB and λUWB, respectively:
(21)LUWB=L-ΔRyR1,λUWB=λ-ΔRxR2cos⁡L,
where R1 and R2 are the earth ellipsoid local meridian and the local prime vertical curvature radius, respectively. Taking location difference as observation measurement, then location observation equation can be represented:
(22)Zk(t)=[(LINS-LUWB)R1(λINS-λUWB)R2cos⁡L]=[R1ΔL+ΔRyR2Δλcos⁡L+ΔRx].We can get(23)Zk(t)=Jk(t)Xk(t)+Vk(t),
where
(24)Jk(t)=[O2×5,diag⁡[R1,R2cos⁡L],O2×3],Vk(t)=[ΔRy,ΔRx].As shown in (20), we can get the measurement equation of integrated system. Then, in this paper, optimal filtering strategy of INS/UWB will be researched.


## 3. Optimal Comprehensive and Filtering Strategy of INS/UWB

In view of the closed environment, INS/UWB integrated navigation strategy is adopted to synchronous detection of the carrier, which is a typical multisensor information fusion problem. The core is integrated navigation data filtering fusion. In this field, the most widely used and most successful information fusion technology is Kalman filter [19]. In this paper, integrated navigation data fusion strategy of INS/UWB based on Kalman filtering theory was researched.


### 3.1. Information Fusion Method

Traditional Kalman filter (KF) is an optimal estimation algorithm which is linear, unbiased, and taking minimum error variance for estimation criterion [20]. When the system equation of INS/UWB is known, at the same time, on the condition of the system noise and measurement noise statistical properties known, using linear Kalman filtering technology, the optimal estimate can be realized. But dynamic positioning system in closed environment has time-varying characteristics and the state statistical feature of measurement noise is unknown. If traditional linear Kalman filtering strategy is used directly, filtering precision will be down rapidly, even divergence. In order to solve this problem, fuzzy adaptive Kalman filtering (FAKF) is proposed in this paper, and its purpose is to ignore the accurate measurement noise prior data in the process of filtering. On the basis of the classical Kalman recursive equations, add the measurement noise regulation equation:
(25)Mk=ckdMk-1,
where  Mk is the k step measurement noise estimation, ckd is the adjustment coefficient of measurement noise, and d has a great influence for ckd. When d=0, at this point the measurement noise is not needed to be adjusted; when d<1, the adjustment range is small and the cycle is longer, but the process is stable; when d>1, the adjustment range is larger and cycle is shorter, but it is easier to generate oscillation. ck can be obtained by fuzzy inference system (FIS) [21] and input reference of FIS is got by the difference between residual observed value and estimate value with INS/UWB measurement model. Defining r as the measurement residual, Tr as measurement variance, and Vr as estimating equations, combined with (23), we can get(26)rk=Zk(t)-Z^k(t)=Zk(t)-HkX^k∣k-1,(27)Tr=1N∑i=i0kririT,(28)Vr=Mk-1+Hk(Φk,k-1Pk-1Φk,k-1T+Q)HkT.As shown in (26), where  Zk(t) is practical measurement value of INS/UWB system, Z^k(t) is measurement estimate value. In (27), i0=k-N+1. Defining PR as the difference value between residual measured variance and estimated variance, there is
(29)PR(k)=Tr(k)-Vr(k).On the condition of constructing accurately system model,PR(k) should be zero; namely, the residual actual variance and theoretical variance are equal. If system noise increases, Tr will increase, PR(k)>0, at this time, and Mk-1 will increase, which makes PR(k) close to zero. If system noise reduces, Tr will decrease, and PR(k)<0, at this time, and Mk-1 will reduce; then PR(k) will be close to zero. As shown in (25), if ck>1, Mk-1 will increase; if ck<1, Mk-1 will reduce; if ck = 1, Mk-1 will not be changed. Further, by setting the fuzzy rules, PR(k) and ck will be given, as shown in Figures 11 and 12.Figure 11
The membership function of the FIS inputPRk.Figure 12
The membership function of the FIS inputck.


### 3.2. Simulation Research of INS/UWB Based on Fuzzy Adaptive Kalman Filtering

Based on the INS/UWB coupling mechanism, the state of integrated navigation system is(30)X=[ΔΦE,ΔΦN,ΔΦU,ΔVE,ΔVN,ΔX,ΔY]T,
where ΔΦE, ΔΦN, and ΔΦU are the imbalance angle of east, north, and up direction, respectively, where ΔVE and ΔVN are the velocity error of east and north direction, respectively, and where ΔX and ΔY are latitude error and longitude error, respectively, by coordinate transformation; then the measure vector of integrated system is
(31)Z=[ΔVE,ΔVN,ΔX,ΔY].As shown in Figure13, we set up simulation test scenarios. Simulation trajectory contains straight line and broken line. The simulation time is 200 s and the initial position is PO=(0,0). In the first stage, the initial acceleration on the X direction is 0.01 m/s2 and the y-axis direction is zero. After 90 seconds into the second stage, at point A, acceleration on the X direction keeps at 0.01 m/s2 and on the Y direction becomes 0.0025 m/s2; lasting time is 60 s to 110 s; this process is at the end of the point B. Then entering the final stage, the moving target makes a linear motion along the x-axis and the lasting time is 90 s. Simulation results are shown in Figures 14, 15, 16, 17, and 18. Initial system noise is Q0 = diag[2×10-3,2×10-3,2×10-3,1×10-3,1×10-3,1.5×10-3,1.5×10-3] and the initial measurement noise is R0 = diag[1×10-4,1×10-4,2×10-4,2×10-4].Figure 13
Simulation trajectory schematic diagram.Figure 14
Trajectory tracking effect comparison between FAKF and KF.Figure 15
Velocity error inx-axis direction.Figure 16
Velocity error iny-axis direction.Figure 17
Position error inx-axis direction.Figure 18
Position error iny-axis direction.By the results, we can know that in the period of 70 s~140 s measurement error is larger than other periods and the tracking error of KF increases obviously. Yet, FAKF is able to adjust the measurement noise and reduce the tracking error. We make a comparison result, as shown in Table2. Results show that the adaptive fuzzy Kalman filter method is better than the conventional Kalman filtering, because of smaller tracking error, which is suitable for INS/UWB integrated navigation data fusion.Table 2
Performance comparison between FAKF and KF.
KF
FAKF
Velocity error range inx (m/s)
−0.08~0.05
−0.03~0.02
Velocity error range iny (m/s)
−0.12~0.12
−0.07~0.07
Velocity variance inx
3.3
×
10
-
3
8.3
×
10
-
4
Velocity variance iny
3.1
×
10
-
3
6.1
×
10
-
4
Position error range inx (m)
−0.85~0.5
−0.1~0.3
Position error range iny (m)
−0.06~0.19
−0.07~0.05
Position variance inx
0.0974
0.0048
Position variance iny
3.5
×
10
-
3
3.8
×
10
-
4


## 3.1. Information Fusion Method

Traditional Kalman filter (KF) is an optimal estimation algorithm which is linear, unbiased, and taking minimum error variance for estimation criterion [20]. When the system equation of INS/UWB is known, at the same time, on the condition of the system noise and measurement noise statistical properties known, using linear Kalman filtering technology, the optimal estimate can be realized. But dynamic positioning system in closed environment has time-varying characteristics and the state statistical feature of measurement noise is unknown. If traditional linear Kalman filtering strategy is used directly, filtering precision will be down rapidly, even divergence. In order to solve this problem, fuzzy adaptive Kalman filtering (FAKF) is proposed in this paper, and its purpose is to ignore the accurate measurement noise prior data in the process of filtering. On the basis of the classical Kalman recursive equations, add the measurement noise regulation equation:
(25)Mk=ckdMk-1,
where  Mk is the k step measurement noise estimation, ckd is the adjustment coefficient of measurement noise, and d has a great influence for ckd. When d=0, at this point the measurement noise is not needed to be adjusted; when d<1, the adjustment range is small and the cycle is longer, but the process is stable; when d>1, the adjustment range is larger and cycle is shorter, but it is easier to generate oscillation. ck can be obtained by fuzzy inference system (FIS) [21] and input reference of FIS is got by the difference between residual observed value and estimate value with INS/UWB measurement model. Defining r as the measurement residual, Tr as measurement variance, and Vr as estimating equations, combined with (23), we can get(26)rk=Zk(t)-Z^k(t)=Zk(t)-HkX^k∣k-1,(27)Tr=1N∑i=i0kririT,(28)Vr=Mk-1+Hk(Φk,k-1Pk-1Φk,k-1T+Q)HkT.As shown in (26), where  Zk(t) is practical measurement value of INS/UWB system, Z^k(t) is measurement estimate value. In (27), i0=k-N+1. Defining PR as the difference value between residual measured variance and estimated variance, there is
(29)PR(k)=Tr(k)-Vr(k).On the condition of constructing accurately system model,PR(k) should be zero; namely, the residual actual variance and theoretical variance are equal. If system noise increases, Tr will increase, PR(k)>0, at this time, and Mk-1 will increase, which makes PR(k) close to zero. If system noise reduces, Tr will decrease, and PR(k)<0, at this time, and Mk-1 will reduce; then PR(k) will be close to zero. As shown in (25), if ck>1, Mk-1 will increase; if ck<1, Mk-1 will reduce; if ck = 1, Mk-1 will not be changed. Further, by setting the fuzzy rules, PR(k) and ck will be given, as shown in Figures 11 and 12.Figure 11
The membership function of the FIS inputPRk.Figure 12
The membership function of the FIS inputck.


## 3.2. Simulation Research of INS/UWB Based on Fuzzy Adaptive Kalman Filtering

Based on the INS/UWB coupling mechanism, the state of integrated navigation system is(30)X=[ΔΦE,ΔΦN,ΔΦU,ΔVE,ΔVN,ΔX,ΔY]T,
where ΔΦE, ΔΦN, and ΔΦU are the imbalance angle of east, north, and up direction, respectively, where ΔVE and ΔVN are the velocity error of east and north direction, respectively, and where ΔX and ΔY are latitude error and longitude error, respectively, by coordinate transformation; then the measure vector of integrated system is
(31)Z=[ΔVE,ΔVN,ΔX,ΔY].As shown in Figure13, we set up simulation test scenarios. Simulation trajectory contains straight line and broken line. The simulation time is 200 s and the initial position is PO=(0,0). In the first stage, the initial acceleration on the X direction is 0.01 m/s2 and the y-axis direction is zero. After 90 seconds into the second stage, at point A, acceleration on the X direction keeps at 0.01 m/s2 and on the Y direction becomes 0.0025 m/s2; lasting time is 60 s to 110 s; this process is at the end of the point B. Then entering the final stage, the moving target makes a linear motion along the x-axis and the lasting time is 90 s. Simulation results are shown in Figures 14, 15, 16, 17, and 18. Initial system noise is Q0 = diag[2×10-3,2×10-3,2×10-3,1×10-3,1×10-3,1.5×10-3,1.5×10-3] and the initial measurement noise is R0 = diag[1×10-4,1×10-4,2×10-4,2×10-4].Figure 13
Simulation trajectory schematic diagram.Figure 14
Trajectory tracking effect comparison between FAKF and KF.Figure 15
Velocity error inx-axis direction.Figure 16
Velocity error iny-axis direction.Figure 17
Position error inx-axis direction.Figure 18
Position error iny-axis direction.By the results, we can know that in the period of 70 s~140 s measurement error is larger than other periods and the tracking error of KF increases obviously. Yet, FAKF is able to adjust the measurement noise and reduce the tracking error. We make a comparison result, as shown in Table2. Results show that the adaptive fuzzy Kalman filter method is better than the conventional Kalman filtering, because of smaller tracking error, which is suitable for INS/UWB integrated navigation data fusion.Table 2
Performance comparison between FAKF and KF.
KF
FAKF
Velocity error range inx (m/s)
−0.08~0.05
−0.03~0.02
Velocity error range iny (m/s)
−0.12~0.12
−0.07~0.07
Velocity variance inx
3.3
×
10
-
3
8.3
×
10
-
4
Velocity variance iny
3.1
×
10
-
3
6.1
×
10
-
4
Position error range inx (m)
−0.85~0.5
−0.1~0.3
Position error range iny (m)
−0.06~0.19
−0.07~0.05
Position variance inx
0.0974
0.0048
Position variance iny
3.5
×
10
-
3
3.8
×
10
-
4


## 4. Experimental Researches

An indoor experiment platform is established in order to verify theoretical model. The experiment platform is geared to the need of coal mine environment, which is built by the ratio of 1 : 3 scales relative to the actual working condition. Composition mainly includes shearer, hydraulic support, scraper conveyor, INS module, and UWB module, as shown in Figure19. Then, as shown in Figure 20, two reference node coordinates are set: (x1, y1) = (2, 0) and (x2, y2) = (4, 0); Y axis is y0 = 1. Based on the integrated model of INS/UWB, the static and dynamic positioning tests are researched.Figure 19
The positioning test platform of combined navigation with INS and UWB.Figure 20
Coordinate orientation diagram.


### 4.1. Static Positioning Test

As shown in Figure21, static reference point is set in interval of 1 m, a total of 20 basis points. Results are shown in Table 3; xR and yR are X and Y position of static reference point, respectively, xUWB and yUWB are output by UWB; xINS/UWB and yINS/UWB are output by INS/UWB.Table 3
Static positioning test data.
x
R (m)
y
R (m)
x
UWB (m)
y
UWB (m)
x
INS/UWB (m)
y
INS/UWB (m)
1
1
2.24
1.58
1.52
1.13
2
1
2.92
1.31
2.43
1.08
3
1
3.8
1.18
3.31
1.03
4
1
4.74
0.89
4.15
0.94
5
1
5.67
0.91
5.06
0.97
6
1
6.59
1.17
6.08
1.04
7
1
7.43
1.14
6.94
1.08
8
1
8.38
1.14
7.94
1.05
9
1
9.62
0.95
9.1
0.96
10
1
10.77
0.93
10.06
0.97
11
1
11.42
0.9
10.96
0.94
12
1
12.39
1.13
11.98
1.04
13
1
13.67
1.1
13.24
1.03
14
1
14.61
1.09
14.29
1.05
15
1
15.73
1.15
15.29
1.02
16
1
16.81
1.08
16.35
0.97
17
1
17.93
0.94
17.56
0.99
18
1
18.97
1.23
18.58
1.06
19
1
20.05
1.17
19.63
1.08Figure 21
Reference point layout for static positioning test.According to the measuring data, we can get the track renderings ofX and Y direction, respectively, as shown in Figures 22, 23, 24, and 25.Figure 22
Static tracking effect ofX coordinate.Figure 23
Static tracking error ofX coordinate.Figure 24
Static tracking effect ofY coordinate.Figure 25
Static tracking error ofY coordinate.


### 4.2. Dynamic Positioning Test

As the signal frequency of INS is100Hz, and UWB is 10 Hz, system fusion frequency is selected for 10 Hz, so filtering cycle is 0.1 s, and test time is set to 250 s. As shown in Figure20,Y coordinate is set to 1, the initial position P0 = (0, 1), and the termination position P1 = (20, 1). The results are shown in Figures 26 and 27.Figure 26
Dynamic tracking performance of position.Figure 27
Dynamic tracking error of position.


### 4.3. Result Analysis

We conduct the analysis of experimental results.(i)
In the process of static positioning test, as shown in Figures22 and 23, tracking error range of x-axis on the condition of UWB is 0.38 m~1.24 m, the average residual rate is 10.7%, and the confidence level is 89.3%. Then, under the same conditions, tracking error range of INS/UWB is 0.06 m~0.64 m, the average residual rate is 4.6%, and the confidence level is 95.4%. As shown in Figures 24 and 25, tracking error range of  y-axis on the condition of UWB is 0.12 m~0.58 m, the average residual rate is 13.2%, and the confidence level is 86.8%. Then, tracking error range of y-axis on the condition of INS/UWB is 0.06 m~0.13 m, the average residual rate is 5.1%, and the confidence level is 94.9%.(ii)
In the process of dynamic positioning test, we have designed a set of computer software and PC interface in the experimental process is shown in Figure29. Dynamic tracking results of attitude angle and position are shown in Figures 26, 27, and 28. Positioning error is ΔPUWB = (−0.18 m~0.56 m), ΔPINS/UWB = (−0.17 m~0.1 m), the average residual rate is RUWB = 7.3%, RINS/UWB = 4.8%, and the confidence coefficients are CUWB = 92.7% and CINS/UWB = 95.2%, respectively.Figure 28
Dynamic tracking performance of attitude angle.Figure 29
PC interface in the experimental process.Results show that integrated navigation system is stable and divergence problem does not exist. At the same time, integrated navigation precision of INS/UWB is better than any independent subsystem.


## 4.1. Static Positioning Test

As shown in Figure21, static reference point is set in interval of 1 m, a total of 20 basis points. Results are shown in Table 3; xR and yR are X and Y position of static reference point, respectively, xUWB and yUWB are output by UWB; xINS/UWB and yINS/UWB are output by INS/UWB.Table 3
Static positioning test data.
x
R (m)
y
R (m)
x
UWB (m)
y
UWB (m)
x
INS/UWB (m)
y
INS/UWB (m)
1
1
2.24
1.58
1.52
1.13
2
1
2.92
1.31
2.43
1.08
3
1
3.8
1.18
3.31
1.03
4
1
4.74
0.89
4.15
0.94
5
1
5.67
0.91
5.06
0.97
6
1
6.59
1.17
6.08
1.04
7
1
7.43
1.14
6.94
1.08
8
1
8.38
1.14
7.94
1.05
9
1
9.62
0.95
9.1
0.96
10
1
10.77
0.93
10.06
0.97
11
1
11.42
0.9
10.96
0.94
12
1
12.39
1.13
11.98
1.04
13
1
13.67
1.1
13.24
1.03
14
1
14.61
1.09
14.29
1.05
15
1
15.73
1.15
15.29
1.02
16
1
16.81
1.08
16.35
0.97
17
1
17.93
0.94
17.56
0.99
18
1
18.97
1.23
18.58
1.06
19
1
20.05
1.17
19.63
1.08Figure 21
Reference point layout for static positioning test.According to the measuring data, we can get the track renderings ofX and Y direction, respectively, as shown in Figures 22, 23, 24, and 25.Figure 22
Static tracking effect ofX coordinate.Figure 23
Static tracking error ofX coordinate.Figure 24
Static tracking effect ofY coordinate.Figure 25
Static tracking error ofY coordinate.


## 4.2. Dynamic Positioning Test

As the signal frequency of INS is100Hz, and UWB is 10 Hz, system fusion frequency is selected for 10 Hz, so filtering cycle is 0.1 s, and test time is set to 250 s. As shown in Figure20,Y coordinate is set to 1, the initial position P0 = (0, 1), and the termination position P1 = (20, 1). The results are shown in Figures 26 and 27.Figure 26
Dynamic tracking performance of position.Figure 27
Dynamic tracking error of position.


## 4.3. Result Analysis

We conduct the analysis of experimental results.(i)
In the process of static positioning test, as shown in Figures22 and 23, tracking error range of x-axis on the condition of UWB is 0.38 m~1.24 m, the average residual rate is 10.7%, and the confidence level is 89.3%. Then, under the same conditions, tracking error range of INS/UWB is 0.06 m~0.64 m, the average residual rate is 4.6%, and the confidence level is 95.4%. As shown in Figures 24 and 25, tracking error range of  y-axis on the condition of UWB is 0.12 m~0.58 m, the average residual rate is 13.2%, and the confidence level is 86.8%. Then, tracking error range of y-axis on the condition of INS/UWB is 0.06 m~0.13 m, the average residual rate is 5.1%, and the confidence level is 94.9%.(ii)
In the process of dynamic positioning test, we have designed a set of computer software and PC interface in the experimental process is shown in Figure29. Dynamic tracking results of attitude angle and position are shown in Figures 26, 27, and 28. Positioning error is ΔPUWB = (−0.18 m~0.56 m), ΔPINS/UWB = (−0.17 m~0.1 m), the average residual rate is RUWB = 7.3%, RINS/UWB = 4.8%, and the confidence coefficients are CUWB = 92.7% and CINS/UWB = 95.2%, respectively.Figure 28
Dynamic tracking performance of attitude angle.Figure 29
PC interface in the experimental process.Results show that integrated navigation system is stable and divergence problem does not exist. At the same time, integrated navigation precision of INS/UWB is better than any independent subsystem.


## 5. Conclusions

(i)
Using INS for positioning independently in a closed environment, the performance of attitude angle tracking is good, but the position error is divergent. This paper proposed a combined method of navigation based on INS/UWB to solve this problem. By analyzing error equation of INS and UWB, respectively, coupled model of INS/UWB was established to realize the fusion of two subsystems.
(ii)
On the basis of the coupled model, optimal comprehensive and filtering strategy based on FAKF were proposed. Simulation results show that FAKF has smaller tracking error, which is suitable for INS/UWB integrated navigation data fusion.
(iii)
Integrated positioning experiment platform was built according to coal mines closed environment; static and dynamic positioning results show that integrated navigation system based on INS/UWB could track the position and attitude angle of the mobile carrier in real time; and positioning accuracy satisfies the requirement of working condition.



---

*Source: 215303-2014-07-08.xml*