# SINS/OD Integrated Navigation Algorithm Based on Body Frame Position Increment for Land Vehicles

Authors: He Chen; Zhili Zhang; Zhaofa Zhou; Pengpeng Liu; Qi Guo
Journal: Mathematical Problems in Engineering
 (2018)

---

## Abstract

It is a challenge to achieve high accuracy navigation for land vehicles without the aid of global navigation satellite systems (GNSS). Inertial measurement unit (IMU) and odometer (OD) are widely deployed due to their complementary properties. In this paper, SINS/OD integrated navigation algorithm based on body frame position increment is studied to improve the navigation performance. Taking the calibration errors of odometer scale factor, IMU installation angle, and lever arm into consideration, the odometer measurement model is derived. Then measurement equations based on body frame position increment are proposed to overcome the amplified random errors in the traditional velocity observation approach. Odometer fault detection and exception are conducted based on residualχ2 detection method, with the nonholonomic constraints of land vehicles applied to mitigate the standalone SINS error drift. Long distance real test is carried out using laser gyro SINS to assess the proposed algorithm, which shows that navigation performance can be effectively improved.


---

## Body

## 1. Introduction

Land vehicle navigation has been widely used in areas such as mining, agriculture, cargo handling, and borehole surveying [1]. Strapdown inertial navigation system (SINS) is always employed in these applications because it can provide all 3D attitude, velocity, and position information of the vehicles with high bandwidth and high reliability. As is known, the error drift of SINS accumulates rapidly with no bound due to the integration of noise-contaminated inertial measurements, so auxiliary information from other sensors is always introduced to mitigate the error drift. Nowadays, global navigation satellite system (GNSS) receivers are the most commonly used sensors because of their high accuracy and low cost [2–4]. The performance of SINS/GNSS integrated navigation system relies heavily on the information of GNSS. The GNSS signal, however, is prone to blockage due to shelters such as buildings, tunnels, and trees, as well as to jamming and spoofing around critical sites or in hostile environment [5–9]. In these GNSS denied scenarios, the navigation performance decays rapidly. What is worse, the position given by the system may be totally wrong when the received GNSS signal is jammed, which is disastrous for special applications like military vehicles.Several approaches have been developed for the control of standalone SINS navigation error drift during GNSS outages, for example, zero-velocity updates (ZUPT) [10–12] and nonholonomic constraints (NHC) of land vehicles [1, 13–15]. The ZUPT approach, if frequently implemented, will restrict the manoeuvrability of vehicles; if not, satisfactory performance cannot be reached. The NHC method takes advantage of the a priori information that velocity components in the upward and transverse axes are zero for land vehicles and can always be implemented on condition that the vehicle runs normally. The upsetting fact is that the observability of NHC method is limited due to lack of forward velocity measurement [16, 17]. Above all, long distance/time self-contained high-precision navigation for land vehicles without the aid of GNSS is still a challenge up to now.Relative sensors such us odometers (OD) and Doppler velocity log (DVL) can be used to measure the forward speed of vehicles. DVL is often used in marine vehicles to measure velocity relative to sea bottom or ocean current [18–20], while OD is widely equipped in modern land vehicles [8, 16, 17, 21, 22]. OD is cost-effective and the accuracy of velocity, different from SINS, does not degrade with time. The integration of SINS and OD is totally self-contained for both SINS and odometer neither receiving nor emitting signals, which contributes a lot to the anti-interference and concealment ability of vehicles and is especially preferred in military applications. In [8], OD and NHC are combined to provide 3D velocity for the control of SINS error drift during short-term GPS outages. In [21], a versatile strategy, comprising two coupled subproblems (i.e., self-calibration and positioning, in-motion alignment, and positioning), is suggested for SINS/OD integrated navigation. The pulse output of OD is used to calculate the forward speed of land vehicles by dividing the sampling interval. Since the sampling interval is very small, the velocity calculation process significantly amplifies random errors. Furthermore, large OD measurement errors may occur due to skid caused by bad road conditions such as soft or icy land surfaces. To solve these problems, a novel scheme based on body frame position increment is recommended in this paper. The contributions are twofold. Firstly, measurement equations based on position increment are derived, which can overcome the drawback of amplified random errors in velocity measurements. Secondly, a χ2 distribution based fault detection and exclusion (FDE) method is introduced to test and isolate large OD errors mentioned beforehand, and NHC method is applied to limit the SINS error drift during OD unavailable periods. The proposed algorithm is evaluated by a long distance field test.The rest contents are arranged as follows. OD velocity measurement model is derived in Section2 considering the calibration errors of OD scale factor, IMU installation angles, and OD lever arm, and the random error amplification effect is illustrated. Section 3 deals with the proposed integration algorithm for SINS and OD, where the measurement equations based on body frame position increment are constructed and residual chi-square detection method is adopted for FDE. Field test results are presented in Section 4 to verify the recommended algorithm and conclusions are drawn in Section 5.


## 2. Velocity Measurement Model of Odometer

Denote bys frame the IMU coordinate system, its origin at the center of IMU, and its three axes pointing to the right, forward, and upward of IMU, respectively. The vehicle body frame is referred to as b frame, whose three axes point to the right, forward, and upward, respectively. Choose geographical coordinate system (also called East-North-Up frame) as the navigation frame (denoted by n frame). The configuration of IMU and OD on a land vehicle is shown in Figure 1, where O is the IMU center, P represents the OD measurement point, and vector L stands for the lever arm of point P relative to point O (called OD lever arm afterwards).Figure 1
Installation configuration of SINS and OD on a land vehicle.According to the working principle of the OD applied, it outputs digital pulses proportional to the mileage increment within the sampling interval. Denoting the forward speed of pointP by v, we have(1)ΔS=KodN,(2)v=ΔSΔt,where Kod is OD scale factor, N is OD output, ΔS refers to the mileage increment, and Δt denotes the sampling interval.Denote the ground velocity of pointP by VOD. When a vehicle maneuvers normally, its skyward and lateral velocities are zero, which gives(3)VODxb=0,VODzb=0,where the superscript b refers to coordinate components expressed in the b frame.Combing (2) and (3) yields velocity of point P expressed in b frame:(4)VODb=0v0=ΔSbΔt,where ΔSb=0ΔS0T and can be seen as augmented position increment measured by OD.Considering the error in OD scale factor, we have(5)ΔS^b=1+δKDΔSb,where δKD refers to the OD scale factor error.Substituting (5) into (4), we obtain(6)V^ODb=ΔS^bΔt=VODb+δKDVODb.Denoting the ground speed vector of pointO as VSINS, according to Figure 1, the relationship between VSINS and VOD satisfies(7)VOD=VSINS+ωeb×L=VSINS+VL,where ωeb is the rotation rate of b frame with respect to Earth frame (e frame) and VL is the velocity caused by OD lever arm L.It can be obtained from (7) that(8)V^DRb=V^ODb-C^sbω^ebs×L^s,where V^DRb refers to the ground speed of point O calculated from OD outputs; C^sb is the calibrated value of direct cosine matrix (DCM) from s frame to b frame, which, determined by the installation configuration of IMU, can be referred to as IMU installation matrix; L^s is the calibrated value of OD lever arm expressed in s frame; ω^ebs is the angular rate of b frame with respect to e frame expressed in s frame and is determined with SINS outputs.Denote byδα the misalignment angle between Csb and C^sb; δα is defined to meet(9)C^sb=I-δα×Csb.Substituting (6) and (9) into (8) yields(10)V^DRb=VODb+δKDVODb-I-δα×Csbωebs+δωebs×Ls+δLs≈VODb-VLb+δKDVODb-VLb×δα-Csbωebs×δLs+CsbLs×δωebs=VSINSb+VODbδKD-VLb×δα-Csbωebs×δLs+CsbLs×δωebs,where δLs is OD lever arm error and δω^ebs is the error of ω^ebs.Considering the calculation process ofω^ebs, we obtain(11)ω^ebs=ω~ibs-ω^ies=ωibs+εs-C^nsC^enωiee,where ω~ibs is the rate of b frame with respect to inertial frame measured by the gyro assembly in IMU, εs is the gyro drifts expressed in s frame, C^ns=C^snT and C^sn is the DCM from s frame to n frame determined from SINS outputs, ωiee=00ωieT and refers to the rate of Earth expressed in e frame, and ωie is the self-rotation rate of Earth.According to the relationship betweenn frame and e frame, it yields(12)C^en=-sin⁡λ^cos⁡λ^0-sin⁡L^cos⁡λ^-sin⁡L^sin⁡λ^cos⁡L^cos⁡L^cos⁡λ^cos⁡L^sin⁡λ^sin⁡L^=I-δθ×Cen,where δθ=-δLδλcos⁡Lδλsin⁡L=-1000cos⁡L00sin⁡L0δLδλδh=M1δp.Considering the definition of attitude misalignment angle in SINS, we get(13)C^ns=CnsI+ϕ×,where ϕ stands for the attitude misalignment angle of SINS.Substituting (12) and (13) while retaining the first-order approximation only, (11) can be rewritten as(14)δωebs=εs+Cnsωien×ϕ-Cnsωien×M1δp.Then OD measurement model can be obtained by substituting (14) into (10), that is,(15)V^DRb=VSINSb+CsbLs×Cnsωien×ϕ-CsbLs×Cnsωien×M1δp+CsbLs×εs+VODbδKD-VLb×δα-Csbωebs×δLs.Conventionally, measurement equations are established by taking the difference betweenV^SINS and V^DR. In this scheme, the calibration errors δKD, δα, and δLs can be augmented as system states to be estimated online. However, it will amplify the random error in the process of calculating VODb. To illustrate this phenomenon, a set of ten minutes forward speed data obtained from (2) is illustrated in Figure 2, the corresponding mileage increment obtained from (1) is shown in Figure 3, and the sampling interval is 5 ms.Figure 2
Forward speed calculated from OD outputs.Figure 3
Mileage increment obtained from OD outputs.It can be clearly seen from Figures2 and 3 that the random variation amplitude of forward speed velocity is about 1 m/s while that of mileage increment is about 0.005 m. In a word, the measurement noise is enlarged as expected. The enlarged random noise deteriorates the quality of observations and thus degrades the performance of Kalman filter. Noting that the noise amplification effect originates from the division operation in (2), a natural thought is to construct measurement equations directly from (1). Section 3 deals with this approach in detail.


## 3. SINS/OD Integrated Navigation Algorithm Design

### 3.1. Measurement Equations Based on Body Frame Position Increment

The velocity output of SINS is the calculated values ofn frame coordinated VSINS, namely, V^SINSn. Converting it into b frame, we have(16)V^SINSb=C^sbC^nsV^SINSn=I-δα×CsbCnsI+ϕ×VSINSn+δVSINSn≈VSINSb-CsbCnsVSINSn×ϕ+CsbCnsδVSINSn+VSINSb×δα.Integrating (16) from tk-1 to tk yields(17)ΔPSINSb=∫tk-1tkV^SINSbdτ≈∫tk-1tkVSINSbdτ-∫tk-1tkCsbCnsVSINSn×dτϕ+∫tk-1tkCsbCnsdτδVSINSn+∫tk-1tkVSINSb×dτδα,where ΔPSINSb is the body frame position increment calculated from SINS outputs.Similarly, we can obtain the body frame position incrementΔPDRb calculated from OD outputs by integrating (15) from tk-1 to tk, that is,(18)ΔPDRb=∫tk-1tkV^DRbdτ≈∫tk-1tkVSINSbdτ+∫tk-1tkCsbLs×Cnsωien×dτϕ-∫tk-1tkCsbLs×Cnsωien×M1dτδp+∫tk-1tkCsbLs×dτεs+ΔSkbδKD-∫tk-1tkVLb×dτδα-∫tk-1tkCsbωebs×dτδLs.Defining observation variables as the difference betweenΔPSINSb and ΔPDRb, we have(19)Z=ΔPSINSb-ΔPDRb.Substituting (17) and (18), (19) can be rewritten as(20)Z=Hϕϕ+HδvδVSINSn+Hpδp+Hεεs+HKδKD+Hδαδα+HδLδLswith(21)Hϕ=-∫tk-1tkCsbCnsVSINSn×+CsbLs×Cnsωien×dτ,Hδv=∫tk-1tkCsbCnsdτ,Hp=∫tk-1tkCsbLs×Cnsωien×M1dτ,Hε=-∫tk-1tkCsbLs×dτ,HK=-ΔSkb,Hδα=ΔSkb×,HδL=∫tk-1tkCsbωebs×dτ.Noting that(22)HKδKD+Hδαδα=0-SkδKD0+00Sk000-Sk00δαxδαyδαz=Skδαz-SkδKD-Skδαx=00Sk-Sk000-Sk0δKDδαxδαz=M2δα1.Equation (22) shows that measurement variable Z is irrelevant to the y-axis component of IMU installation matrix error, so we omit it for brevity.Substituting (22) into (20), we obtain(23)Z=Hϕϕ+HδvδVSINSn+Hpδp+Hεεs+Hδα1δα1+HδLδLs,where Hδα1=M2, δα1=δKDδαxδαzT.Equation (23) is the new measurement equations derived. It can be seen that there is no need to calculate the forward velocity from OD outputs any more. Here, the b frame expressed position increment rather than the n frame expressed counterpart is applied for convenience of OD FDE, which is carefully investigated in Section 3.3.


### 3.2. Kalman Filter for Integrated Navigation

The error dynamic equations of SINS can be expressed as follows [23]:(24)ϕ˙=Mϕϕϕ+MϕvδVSINSn+Mϕpδp-Csnεs-Csnwgs,δV˙SINSn=Mvϕϕ+MvvδVSINSn+Mvpδp+Csn∇s+Csnwas,δp˙=MpvδVSINSn+Mppδp,where ϕ is the attitude misalignment angle of SINS, δVSINSn the velocity error, δp the position error, εs the gyro constant drift expressed in s frame, ∇s the accelerometer zero deviation expressed in s frame, wgs the gyro noise expressed in s frame, and was the accelerometer noise expressed in s frame; the derivation and expressions of matrices in (24) can be found in [23].Error termsεs, ∇s, δα1, and δLs can be treated as random constant states, that is,(25)ε˙s=0,∇˙s=0,δα˙1=0,δL˙s=0.Combining (24) and (25), we can obtain the 21-dimensional augmented state equations of Kalman filter, with (23) being the measurement equations. The Kalman filter equations can be rewritten as follows:(26)X˙t=AtXt+Btwt,Zt=HtXt+Vt,where Xt is the state vector, Zt is the measurement vector, wt~0,q is the process noise, and Vt~0,R is the measurement noise.Equation (26) is the continuous form of the system equations. To conduct the Kalman filter calculation procedures, the discrete form of (26) should be derived. Let the update interval of Kalman filter be ΔT; we have the following [24]:(27)Xk+1=Φk+1,kXk+Wk,Zk+1=Hk+1Xk+1+Vk+1,where the subscript k denotes values at tk=kΔT, Φk+1,k=I+AkΔT, Wk~N0,Qk, Qk=BkqBkTΔT, Vk~N0,Rk, EWkWjT=Qkδkj, EVkVjT=Rkδkj, EWkVjT=0.Note that the calibration errors (δα1 and δLs) are included in the system states and can be estimated online. This is especially important considering the fact that the calibration of IMU installation matrix and OD lever arm is not an easy thing in most applications; that is, it is not convenient to obtain C^sb and L^s before the integrated navigation implementation, if not impossible. In these cases, the IMU can always be mounted to keep the misalignment between s frame and b frame a small quantity, as a result, C^sb can be treated as a unit matrix directly without the calibration procedure. As for OD lever arm, it is usually adequate to view L^s as zero. Furthermore, even if C^sb and δKD can be preobtained in some way, the corresponding true values may vary significantly due to load changing and ambient temperature, which necessitates the online estimation ability, too.


### 3.3. OD Fault Detection and Exception

The measurement model of OD is established on the basis that OD works normally and the vehicle runs without skidding or jumping. On condition that the above-mentioned faults occur, the established OD measurement model cannot correctly reflect the true speed of the vehicle. The error-contaminated speed measurements will pollute the whole system and reduce the precision. So it necessitates the function of FDE in the integrated system. Conventionally, the IMU is of high reliability and considered with no failure in the navigation process. The fault of OD is mainly caused by harsh road conditions. For example, if a car runs on a muddy, snowy, or icy road, forward and (or) side slip may occur, making the measured forward speed larger or lesser than the real value and the lateral zero-velocity assumption incorrect. In addition, the upward zero-velocity constraint may not be satisfied on rugged roads.To sum up, the failures can be categorized into two types, namely, the forward speed measurement error (Type I) and the invalidation of nonholonomic constraints (Type II). If only Type I fault occurs, NHC can still be used to mitigate the SINS error drift. So we adopt a two-stage fault detection scheme here. Firstly, calculate a fault detection function with all the filter residuals to determine if some fault exists; if so, transfer to the second stage by dealing with a new function with the NHC related residuals to determine if the motion constraints are met. Once Type II fault is detected in the second stage, the measurement update procedure of Kalman filter should be avoided; otherwise, the measurement update procedure can be implemented based on NHC information.Among the many fault detection methods, residualχ2 detection method has been widely used due to its low calculation cost and high sensitivity to residual change [24, 25]. To perform the χ2 detection method, a detection function should be formed using filter residual information. Define the detection function for stage one as(28)λk=rkTPrk-1rk,where rk=Zk-HkX^k,k-1 is the filter residual and Prk=HkPk,k-1HkT+Rk is the covariance matrix of the filter residual rk.When the system is faultless,rk~N0,Prk. Once some failure appears, the statistic characteristic of rk no longer holds. So a binary hypothesis can be made for rk:(29)H0:Erk=0,ErkrkT=Prk,H1:Erk=μ,ErkrkT=Prkμ≠0,where H0 is the null hypothesis and denotes that the system is faultless and H1 is the alternative hypothesis and indicates that certain fault occurs.IfH0 is true, λk obeys the χ2 distribution of 3 degrees of freedom, namely, λk~χ2(3). Then the fault detection criteria can be set as(30)ifλk>TD,H1istrueifλk≤TD,H0istrue,where the detection threshold TD is determined by the false alarm rate η.Onceη is chosen, TD can be worked out by solving the corresponding upper quintile of χ2(3). Under normal working conditions, the probability that λk>TD is less than η, which can be regarded as a small probability event. On the other hand, if λk>TD should happen, it indicates that the system is working abnormally.IfH1 is true, we introduce a new detection function λk′ to determine whether the vehicle motion constraints are satisfied (stage two). The detection function λk′ is defined as(31)λk′=rk′TPrk′-1rk′,where rk′=rk1rk3T and Prk′ is the covariance matrix of rk′ and can be obtained by deleting the second row and the second column of Prk.Similar to the first stage, the detection thresholdTD′ is the upper quintile of χ2(2). If λk′>TD′, it indicates that the motion constraint condition of vehicles is not satisfied, then the measurement update of Kalman filter should be avoided. If λk′≤TD′, it indicates that the constraint condition of vehicles is satisfied, then let Z′=Z1Z3T, H′=H1,:H3,:, and the system is put into the motion constrained navigation mode. It should be noted that once the system is transferred into vehicle motion constraint aided navigation mode, Si cannot be obtained due to OD failure, making it impossible to determine Hδα1 with (22). The solution of this problem is to calculate Si using the following:(32)Si=∫tk-1tkVSINSb+VLbdτ.A summary of the two-stage FDE is illustrated in Figure4.Figure 4
Procedures of the two-stage FDE method.It is clear that the faults of odometer can be easily isolated just by avoiding measurement update of the Kalman filter. The significant error increase during the fault-existing period can be mitigated by taking advantage of the NHC information. When the faults disappear, the integration of SINS and OD can be successfully implemented with new measurements.


## 3.1. Measurement Equations Based on Body Frame Position Increment

The velocity output of SINS is the calculated values ofn frame coordinated VSINS, namely, V^SINSn. Converting it into b frame, we have(16)V^SINSb=C^sbC^nsV^SINSn=I-δα×CsbCnsI+ϕ×VSINSn+δVSINSn≈VSINSb-CsbCnsVSINSn×ϕ+CsbCnsδVSINSn+VSINSb×δα.Integrating (16) from tk-1 to tk yields(17)ΔPSINSb=∫tk-1tkV^SINSbdτ≈∫tk-1tkVSINSbdτ-∫tk-1tkCsbCnsVSINSn×dτϕ+∫tk-1tkCsbCnsdτδVSINSn+∫tk-1tkVSINSb×dτδα,where ΔPSINSb is the body frame position increment calculated from SINS outputs.Similarly, we can obtain the body frame position incrementΔPDRb calculated from OD outputs by integrating (15) from tk-1 to tk, that is,(18)ΔPDRb=∫tk-1tkV^DRbdτ≈∫tk-1tkVSINSbdτ+∫tk-1tkCsbLs×Cnsωien×dτϕ-∫tk-1tkCsbLs×Cnsωien×M1dτδp+∫tk-1tkCsbLs×dτεs+ΔSkbδKD-∫tk-1tkVLb×dτδα-∫tk-1tkCsbωebs×dτδLs.Defining observation variables as the difference betweenΔPSINSb and ΔPDRb, we have(19)Z=ΔPSINSb-ΔPDRb.Substituting (17) and (18), (19) can be rewritten as(20)Z=Hϕϕ+HδvδVSINSn+Hpδp+Hεεs+HKδKD+Hδαδα+HδLδLswith(21)Hϕ=-∫tk-1tkCsbCnsVSINSn×+CsbLs×Cnsωien×dτ,Hδv=∫tk-1tkCsbCnsdτ,Hp=∫tk-1tkCsbLs×Cnsωien×M1dτ,Hε=-∫tk-1tkCsbLs×dτ,HK=-ΔSkb,Hδα=ΔSkb×,HδL=∫tk-1tkCsbωebs×dτ.Noting that(22)HKδKD+Hδαδα=0-SkδKD0+00Sk000-Sk00δαxδαyδαz=Skδαz-SkδKD-Skδαx=00Sk-Sk000-Sk0δKDδαxδαz=M2δα1.Equation (22) shows that measurement variable Z is irrelevant to the y-axis component of IMU installation matrix error, so we omit it for brevity.Substituting (22) into (20), we obtain(23)Z=Hϕϕ+HδvδVSINSn+Hpδp+Hεεs+Hδα1δα1+HδLδLs,where Hδα1=M2, δα1=δKDδαxδαzT.Equation (23) is the new measurement equations derived. It can be seen that there is no need to calculate the forward velocity from OD outputs any more. Here, the b frame expressed position increment rather than the n frame expressed counterpart is applied for convenience of OD FDE, which is carefully investigated in Section 3.3.


## 3.2. Kalman Filter for Integrated Navigation

The error dynamic equations of SINS can be expressed as follows [23]:(24)ϕ˙=Mϕϕϕ+MϕvδVSINSn+Mϕpδp-Csnεs-Csnwgs,δV˙SINSn=Mvϕϕ+MvvδVSINSn+Mvpδp+Csn∇s+Csnwas,δp˙=MpvδVSINSn+Mppδp,where ϕ is the attitude misalignment angle of SINS, δVSINSn the velocity error, δp the position error, εs the gyro constant drift expressed in s frame, ∇s the accelerometer zero deviation expressed in s frame, wgs the gyro noise expressed in s frame, and was the accelerometer noise expressed in s frame; the derivation and expressions of matrices in (24) can be found in [23].Error termsεs, ∇s, δα1, and δLs can be treated as random constant states, that is,(25)ε˙s=0,∇˙s=0,δα˙1=0,δL˙s=0.Combining (24) and (25), we can obtain the 21-dimensional augmented state equations of Kalman filter, with (23) being the measurement equations. The Kalman filter equations can be rewritten as follows:(26)X˙t=AtXt+Btwt,Zt=HtXt+Vt,where Xt is the state vector, Zt is the measurement vector, wt~0,q is the process noise, and Vt~0,R is the measurement noise.Equation (26) is the continuous form of the system equations. To conduct the Kalman filter calculation procedures, the discrete form of (26) should be derived. Let the update interval of Kalman filter be ΔT; we have the following [24]:(27)Xk+1=Φk+1,kXk+Wk,Zk+1=Hk+1Xk+1+Vk+1,where the subscript k denotes values at tk=kΔT, Φk+1,k=I+AkΔT, Wk~N0,Qk, Qk=BkqBkTΔT, Vk~N0,Rk, EWkWjT=Qkδkj, EVkVjT=Rkδkj, EWkVjT=0.Note that the calibration errors (δα1 and δLs) are included in the system states and can be estimated online. This is especially important considering the fact that the calibration of IMU installation matrix and OD lever arm is not an easy thing in most applications; that is, it is not convenient to obtain C^sb and L^s before the integrated navigation implementation, if not impossible. In these cases, the IMU can always be mounted to keep the misalignment between s frame and b frame a small quantity, as a result, C^sb can be treated as a unit matrix directly without the calibration procedure. As for OD lever arm, it is usually adequate to view L^s as zero. Furthermore, even if C^sb and δKD can be preobtained in some way, the corresponding true values may vary significantly due to load changing and ambient temperature, which necessitates the online estimation ability, too.


## 3.3. OD Fault Detection and Exception

The measurement model of OD is established on the basis that OD works normally and the vehicle runs without skidding or jumping. On condition that the above-mentioned faults occur, the established OD measurement model cannot correctly reflect the true speed of the vehicle. The error-contaminated speed measurements will pollute the whole system and reduce the precision. So it necessitates the function of FDE in the integrated system. Conventionally, the IMU is of high reliability and considered with no failure in the navigation process. The fault of OD is mainly caused by harsh road conditions. For example, if a car runs on a muddy, snowy, or icy road, forward and (or) side slip may occur, making the measured forward speed larger or lesser than the real value and the lateral zero-velocity assumption incorrect. In addition, the upward zero-velocity constraint may not be satisfied on rugged roads.To sum up, the failures can be categorized into two types, namely, the forward speed measurement error (Type I) and the invalidation of nonholonomic constraints (Type II). If only Type I fault occurs, NHC can still be used to mitigate the SINS error drift. So we adopt a two-stage fault detection scheme here. Firstly, calculate a fault detection function with all the filter residuals to determine if some fault exists; if so, transfer to the second stage by dealing with a new function with the NHC related residuals to determine if the motion constraints are met. Once Type II fault is detected in the second stage, the measurement update procedure of Kalman filter should be avoided; otherwise, the measurement update procedure can be implemented based on NHC information.Among the many fault detection methods, residualχ2 detection method has been widely used due to its low calculation cost and high sensitivity to residual change [24, 25]. To perform the χ2 detection method, a detection function should be formed using filter residual information. Define the detection function for stage one as(28)λk=rkTPrk-1rk,where rk=Zk-HkX^k,k-1 is the filter residual and Prk=HkPk,k-1HkT+Rk is the covariance matrix of the filter residual rk.When the system is faultless,rk~N0,Prk. Once some failure appears, the statistic characteristic of rk no longer holds. So a binary hypothesis can be made for rk:(29)H0:Erk=0,ErkrkT=Prk,H1:Erk=μ,ErkrkT=Prkμ≠0,where H0 is the null hypothesis and denotes that the system is faultless and H1 is the alternative hypothesis and indicates that certain fault occurs.IfH0 is true, λk obeys the χ2 distribution of 3 degrees of freedom, namely, λk~χ2(3). Then the fault detection criteria can be set as(30)ifλk>TD,H1istrueifλk≤TD,H0istrue,where the detection threshold TD is determined by the false alarm rate η.Onceη is chosen, TD can be worked out by solving the corresponding upper quintile of χ2(3). Under normal working conditions, the probability that λk>TD is less than η, which can be regarded as a small probability event. On the other hand, if λk>TD should happen, it indicates that the system is working abnormally.IfH1 is true, we introduce a new detection function λk′ to determine whether the vehicle motion constraints are satisfied (stage two). The detection function λk′ is defined as(31)λk′=rk′TPrk′-1rk′,where rk′=rk1rk3T and Prk′ is the covariance matrix of rk′ and can be obtained by deleting the second row and the second column of Prk.Similar to the first stage, the detection thresholdTD′ is the upper quintile of χ2(2). If λk′>TD′, it indicates that the motion constraint condition of vehicles is not satisfied, then the measurement update of Kalman filter should be avoided. If λk′≤TD′, it indicates that the constraint condition of vehicles is satisfied, then let Z′=Z1Z3T, H′=H1,:H3,:, and the system is put into the motion constrained navigation mode. It should be noted that once the system is transferred into vehicle motion constraint aided navigation mode, Si cannot be obtained due to OD failure, making it impossible to determine Hδα1 with (22). The solution of this problem is to calculate Si using the following:(32)Si=∫tk-1tkVSINSb+VLbdτ.A summary of the two-stage FDE is illustrated in Figure4.Figure 4
Procedures of the two-stage FDE method.It is clear that the faults of odometer can be easily isolated just by avoiding measurement update of the Kalman filter. The significant error increase during the fault-existing period can be mitigated by taking advantage of the NHC information. When the faults disappear, the integration of SINS and OD can be successfully implemented with new measurements.


## 4. Field Test Results

To evaluate the proposed algorithm, a field test is carried out using the laser gyroscope SINS. The test system is composed of a car, an IMU consisting of a laser gyro triad and an accelerator triad, an odometer, and a GPS receiver. The bias stability of the laser gyro is 0.005°/h; the zero offset of the accelerometer is 50μg; the positioning accuracy of the GPS receiver is 5 m. The SINS and GPS outputs are loosely integrated to supply the reference for measuring the errors of the SINS/OD integrated navigation system. The trajectory of the vehicle is shown in Figure 5, where the red point represents the start point and the red triangle stands for the end point; the entire test period lasts 153 min, including 10 min initial alignment and 143 min maneuver, and the total travel mileage is about 210 km.Figure 5
Trajectory of the long distance vehicle test.Figures6–8 present the navigation errors of the traditional velocity measurement approach and the body frame position increment measurement approach proposed in this paper. It can be seen from Figure 6 that the attitude errors of the proposed method converge faster than the traditional method and the accuracy is higher. Also, the new method is less prone to external disturbances. Similar conclusions can be made for velocity (see Figure 7) and position errors (see Figure 8). From Figures 7 and 8 it can be found that the performance improvement in the height channel is not significant comparing to that in the horizontal channel. The reason is that the pitch and roll angle of the vehicle is always small (see Figure 9) so random error of velocity in the height channel is minor as it is mainly determined by the upward zero-speed condition of the vehicle, while the velocity in horizontal channel is mainly affected by the speed measured by OD, which, as mentioned above, amplifies the effects of random fluctuations. In addition, the velocity errors do not increase with time because of the drift mitigation effect of OD measurements, while the position errors still grow slowly with time due to the lack of absolute position measurements. To better reflect the performance of SINS/OD integration, the position errors with respect to the travel mileage is presented in Figure 10. Once again, it is clearly seen that the proposed position increments observation method significantly reduces the horizontal position error compared with the speed observation method, yet the improvement of height error is not obvious, just as expected. The values of fault detection function λk are shown in Figure 11, wherein the false alarm rate η is equal to 0.01 and the corresponding detection threshold is 11.34. It can be seen that, during the entire test process, λk is less thanTD, indicating that the system works properly with no OD measurement failure, so the second stage of FDE is not implemented and the values of λk′ are not shown here.Figure 6
Attitude errors with velocity and position increment observations.Figure 7
Velocity errors with velocity and position increment observations.Figure 8
Position errors with velocity and position increment observations.Figure 9
Attitude angles of the land vehicle output by SINS/GPS integration.Figure 10
Horizontal and vertical position errors relative to distance.Figure 11
Values of fault detection functionλk.To further verify the FDE scheme, OD failures are intentionally added. The OD outputs during 1000 s~1005 s are increased while the OD outputs from 4000 s~4010 s decreased to simulate different kinds of vehicle slipping. When the OD faults are inserted, the values of fault detection function are shown in Figure12. It can be noted that λk is larger than TD during the fault-existing periods, which means that the faults are correctly detected. During the periods when OD faults exist, the statistic value λk′ is always less than the threshold TD′ (9.21), indicating the availability of vehicle motion constraint information. The navigation results are shown in Figures 13–16, where “FDE applied” denotes the results from the proposed navigation method while “no FDE” denotes the results from the position increment measurement approach without FDE. In Figure 13, the attitude errors suffer from an obvious increase during 1000 s~1005 s and 4000 s~4010 s in the “no FDE” method, while there is no such an error increase in the “FDE applied” way; that is, the OD faults have been successfully detected and isolated. Similar remarks can be made on the velocity errors (see Figure 14) and position errors (see Figures 15 and 16), too. From Figures 14 and 15, we can see a convergence of the attitude and velocity errors when OD faults disappear since they are observable in the position increments observation approach. The position errors, however, diverge with time for they are not observable. Moreover, the effect of OD faults on height errors is not significant. The reason is that the projection of forward speed on the upward is limited due to the small pitch and roll angles, just as mentioned above.Figure 12
Values of fault detection function with intentionally added OD failures.Figure 13
Attitude errors when OD failures are manually added.Figure 14
Velocity errors when OD failures are manually added.Figure 15
Position errors when OD failures are manually added.Figure 16
Position errors relative to distance when OD failures are manually added.


## 5. Conclusions

Traditionally, the outputs of SINS and OD are integrated in velocity observation approach, in which the OD outputs, together with the NHC information, are utilized to provide velocity measurements. To overcome adverse effect caused by the amplified random errors, a novel approach based on body frame position increment is presented. Measurement equations are derived with the consideration of the calibration errors of OD scale factor, IMU installation angles, and IMU/OD lever arm. To relieve the effect of OD failures due to severe road conditions, FDE of the integrated system, which has not been seriously treated in the current literatures, is discussed and a novel two-stage FDE method is introduced based on the newly deduced measurement equations. It is shown that not only can the drawback of the traditional method be avoided, but also the FDE procedures can be implemented conveniently. The effectiveness of the proposed algorithms is verified through the results of a long distance field test. Despite the advantages of the proposed method, the position errors still increase slowly with time/distance, which is inevitable due to the lack of absolute position sources. Consequently, combination of the current system with other sensors, such as electronic maps and altimeters is a topic deserving further investigation.



---

*Source: 5719472-2018-08-13.xml*