# An Improved Strong Tracking Kalman Filter Algorithm for the Initial Alignment of the Shearer

Authors: Yuming Chen; Wei Li; Gaifang Xin; Hai Yang; Ting Xia
Journal: Complexity
 (2019)

---

## Abstract

The strap-down inertial navigation system (SINS) is a commonly used sensor for autonomous underground navigation, which can be used for shearer positioning under a coal mine. During the process of initial alignment, inaccurate or time-varying noise covariance matrices will significantly degrade the accuracy of the initial alignment of the shearer. To overcome the performance degradation of the existing initial alignment algorithm under complex underground environment, a novel adaptive filtering algorithm is proposed by the integration of the strong tracking Kalman filter and the sequential filter for the initial alignment of the shearer with complex underground environment. Compared with the traditional multiple fading factor strong tracking Kalman filter (MSTKF) method, the proposed MSTSKF algorithm integrates the advantage of strong tracking Kalman filter and sequential filter, and multiple fading factor and forgetting factor for east and north velocity measurement are designed in the algorithm, respectively, which can effectively weaken the coupling relationship between the different states and increase strong robustness against process uncertainties. The simulation and experiment results show that the proposed MSTSKF method has better initial alignment accuracy and robustness than existing strong tracking Kalman filter algorithm.


---

## Body

## 1. Introduction

Coal is the most widely distributed and abundant energy resources in the world, and it has become one dominant energy in the world energy architecture [1]. However, the complexity of working environment in coal mines has brought great difficulties for the mining of coal mine and the safety problems of coal mines have received growing considerable attention in recent years. In order to tackle the problems of accidents and casualties and simultaneously realize high automation in an underground environment, it is desirable to implement unmanned mining. Moreover, as one of the most important technologies, navigation and localization of the shearer has become the key to mine automation [2].In view of the special environment in coal mine, radio navigation [3], satellite positioning [4], and astronomical navigation [5] are typically rely on external reference information source so that their practical applications are restricted [6]. The positioning technology of the shearer based on inertial navigation is an autonomous navigation system which neither requires external information nor radiates external energy; therefore, strap-down inertial navigation system (SINS) is more applicable to navigating and localizing the shearer in the coal mine environment [7, 8]. SINS can track the position and orientation of the movement of the carrier in real time based on the accelerometer and gyroscope inherent in SINS. The process of initial alignment prior to the navigation operation is of vital importance for SINS, the accuracy of subsequent dead-reckoning algorithm relies heavily on the initial attitude matrix, and a poor initial attitude matrix will result in much degradation and divergence for subsequent navigation [9, 10].Determination of the attitude matrix between the body frame and the reference navigation frame is the core issue for initial alignment. The process of initial alignment is generally divided into two consecutive phases: coarse alignment and fine alignment [11, 12]. The purpose of the coarse alignment method is to provide a roughly known initial attitude to guarantee the validity of the linear filtering model for subsequent fine alignment phases. Analytic method [13, 14] and the optimization-based alignment (OBA) method [15, 16] are the most common means for coarse alignment. Generally, the results of coarse alignment still have significant differences relative to the true value, and subsequent fine alignment stage intends to further enhance the accuracy of the coarse alignment by implementing state estimation methods [17].The Kalman filter (KF) is a powerful and most common technique to estimate the unknown state of the system, and it has been widely used for the initial alignment of the SINS [18, 19]. The performance of traditional KF is heavily depends on precise prior knowledge of the process noise covariance matrix (Q) and the measurement noise covariance matrix (R). Unfortunately, due to the complicated environment in coal mine, accurate noise statistics information is difficult to be determined and the use of inaccurate or time-varying noise statistics may result in substantial estimation errors or even filtering divergence [20, 21]. Moreover, precise knowledge of both the system and measurement models is fundamental for KF; conventional initial alignment models are valid only under the condition that initial misalignment angles are small angles. A modeling error may occur when the initial misalignment angels are large angles, which will also decline the accuracy of KF. For the initial alignment of the shearer in the underground environment, accurate noise statistics information and dynamic process models are difficult or impossible to be determined, and the noise statistics information may be time-varying since the performance of SINS may degrade with the change of the complicated environment. Therefore, the conventional KF method is not suitable for initial alignment of the shearer.In recent years, numerous studies have been devoted to solve the problem of the unknown noise statistical information inherent in the initial alignment of the shearer. The adaptive KF (AKF) has drawn increasing attention and become an effective method to tackle those troublesome issues [22]. Mehra et al. [23] classified the AKF into the following categories: Bayesian [24], maximum-likelihood [25], and covariance matching [26]. Based on the Bayesian framework, Huang et al. [27] proposed a robust Gaussian approximate fixed interval smoother aiming at nonlinear systems with heavy-tailed process and measurement noises. Paper [28] proposed a new outlier-robust Student’s t based Gaussian approximate filter aiming at the outlier measurements of velocity and range in cooperative localization of autonomous underwater vehicles. However, the computation load of the Bayesian and maximum-likelihood methods is so complex that their practical applications are dramatically constrained. The technique that makes the filter residuals consistent with their theoretical covariance is called covariance matching. Strong tracking Kalman filter (STKF) is an AKF that aiming at the unknown statistical characteristics of noise based on the covariance matching technique [29, 30]. This filter is introducing a time-varying suboptimal fading factor into the prediction covariance to enhance the state estimation robustness and smoothness of the KF based on the innovation sequence orthogonality principle. The STKF has been approved as a promising substitute for the KF method due to its antidisturbance and tracking ability of mutation status and widely used in the areas of navigation integration and signal processing. Table 1 summarizes the different improved forms of strong tracking Kalman filter. Conventional single fading factor STKF regulates the prediction covariance matrix by introducing a scalar fading factor. However, it is difficult to deal with high-dimensional filter problem since STKF cannot guarantee the effectiveness of each state, and even leads to the divergence of the filter. To solve this problem, multiple fading factor strong tracking Kalman filter (MSTKF) is proposed to improve the filter performance for the high-dimensional system. Xiong et al. [29] presented a novel robust single position algorithm based on dead-reckoning and MSTKF method, focusing on the condition where precise knowledge of the system models is not available. Hua Liu et al. [31] introduced STF method into spherical simplex-radial cubature Kalman filter to improve the accuracy and robustness of target tracking with severe maneuvering. While MSTKF method introduces multiple fading factors to make different channel have different fading rate, MSTKF method takes no account of the coupling relationship between each state, which will degrade the performance of MSTKF method during the strong coupling relationship between each state.Table 1
Summarizing table about the different versions of KF.
Name Abbreviation Kalman filter KF Adaptive Kalman filter AKF Strong tracking Kalman filter STKF Multiple Fading factor strong tracking Kalman filter MSTKF Multiple fading factor strong tracking Sequential Kalman filter MSTSKFIn view of the above problems, this paper presents a new strong tracking Kalman filter based on the concept of sequential filter to degrade the coupling relationship between each state, while improving the accuracy and robustness of the initial alignment under complex environment. The rest of this paper is structured as follows. Section2 provides a brief review of the existing STKF and MSTKF method and analyzes the drawback of those methods. Then, an improved MSTKF algorithm is elaborately designed for initial alignment of the shearer. In Sections 3 and 4, simulations and field experiments are designed to verify the effectiveness of the proposed method. Concluding remarks are provided in Section 5.


## 2. Initial Alignment of the Shearer Based on Improved STKF Algorithm

In this section, the fine alignment equation of the SINS is firstly introduced based on the assumption of small misalignment angles. Secondly, aiming at the problem of that unknown noise statistical information inherent in the initial alignment of the shearer, Sections2.2 and 2.3 briefly describe the STKF and MSTKF methods. Finally, Section 2.3 presents the drawback of STKF and MSTKF methods and elaborately describes the proposed MSTSKF method in detail.


### 2.1. The Fine Alignment Equation of the SINS

The attitude and velocity error equations of the strap-down system are as follows:(1)ϕ˙=ϕ×ωien+ωenn+δωinn-δωibn(2)δv˙n=fsfn×ϕ+vn×2δωien+δωenn-2ωien+ωenn×δvn+δfsfn+δgnwhere ϕ and δvn are the velocity and attitude error vectors, respectively, ωien denotes the Earth’s rotation rate, fsfn is the specific force measured by an accelerometer in the navigation frame, εn and ∇n are the gyro drift error vector and accelerometer bias error vector, respectively, and δ in (2) denotes error vector variable.The rough initial attitude matrix is obtained by the coarse alignment without any prior knowledge, and coarse alignment provides a fairly good initial condition for the fine alignment since the fine alignment requires prior initialization information to guarantee that the nonlinear error models of initial alignment can be transformed into the linear error models, The shearer is in a static state when the initial alignment is carried out;ωenn and (2ωien+ωenn)×δvn, induced by the linear motion, can be approximated as zero. Besides, accelerometer and gyroscope errors can be equivalent to accelerometer constant drift and gyroscope constant drift. Therefore, the linear error models of initial alignment under small misalignment angles are as follows:(3)ϕ˙=ϕ×ωien-εn(4)δv˙n=fsfn×ϕ+∇nFor the initial alignment of the shearer, the initial attitude angles errors and the sensor bias errors make the differences between the real velocity and open-loop calculated velocity. During the process of initial alignment, the real velocity is zero since the shearer remains stationary. Therefore, the observation equation in fine alignment is constructed by the velocity difference between the velocity information obtained by solving SINS navigation equations and the real velocity(5)Z=VINS-Vreal=HkXk+Rkwhere Rk is measured noise and the observation matrix Hk is denoted as(6)Hk=02×3E2×202×2E 2 × 2 in (6) denotes 2D identity matrix, (3) ~ (6) constitute the state equations and the observation equations of the KF, and the optimal estimation can be achieved when the statistical parameters of the noise are consistent with the theoretical value. However, in practical engineering applications, the statistical parameters of the noise may be inaccurate or unknown; it is necessary to introduce STKF dealing with the problem that inaccurate or unknown noise inherent in the system.


### 2.2. Strong Tracking Kalman Filter

First, the state space model and observation model can be described as(7)Xk=ϕk/k-1Xk-1+Γk-1Wk-1Zk=HkXk+Vkwhere Xk and Zk denote the state vector and measurement vector in the dynamic system at discrete time k, ϕk/(k-1) and Hk are the system transition matrix and observation matrix, respectively, Γk-1 denotes the system noise assignment matrix at the k-1 time, Wk-1 and Vk are the Gaussian white noise sequence with zero-mean and independent for each other, and the statistical characteristics of Vk and Wk are shown as(8)EWk=0EWkWjT=QkδkjEVk=0EVkVjT=RkδkjEWkVjT=0where the symbol of δ in (8) denotes unit sampling signal ant it is defined as follows:(9)δkj=1k=j0k≠jThe equation of innovation sequence in KF is defined as follows:(10)εk/k-1=Zk-Z^k/k-1When establishing model without any model errors and with precise description of the noise, the innovation sequence of the standard KF equation should be a zero-mean Gaussian white noise sequence [32], which can be shown as (11)Eεk/k-1εk/k-1T=Hkϕk/k-1Pk-1ϕk/k-1THkT+HkΓk-1Qk-1Γk-1THkT+Rk=HkPk/k-1HkT+RkIf there are no significant uncertainties in the system process models and noise statistics information, the KF will converge with time going. However, in a various complex practical circumstances, the availability of precisely known model is unrealistic and the statistics characteristics of the noise may vary significantly due to hostile environment. Therefore, the balance of (11) will be destroyed since the parameter of system architecture and the description of noise show mismatch; this phenomenon can be viewed as innovation mismatch. This deficiency severely limits the applications of the traditional KF method.The basic idea and concept of STKF are to make the errors induced by the inaccurate system model or noise parameters equivalent to the filter estimation errors. The fading factor is incorporated into the process of the KF, which will adjust the predicted error covariance matrix in real time so that the orthogonal principle of the innovation sequence can be satisfied.(12)Exk-x~kxk-x~kT=min(13)Eεk+jεkT=0where (12) denotes the performance evaluation indicator for the filter, and (13) denote that the innovation sequence must meet the requirement orthogonal principle.The predicted error covariance matrix has been continually adjusted artificially, and (11) can be rewritten as(14)Eεk/k-1εk/k-1T-Rk=HkλPk/k-1HkTCalculating the trace on both sides of (14) and taking into account the fact that value of the fading factor should be greater than 1, one can obtain(15)λk=max⁡1,trNktrMk(16)Nk=Vk-Rk(17)Mk=HkPk/k-1HkT(18)Vk=ε1ε1Tk=1βVk-1+εkεkT1+β,k≥2where tr is the trace operation, β is the forgetting factor and its value is in the range from 0 to 1, and the complete STKF algorithm is as follows:(19)X^k/k-1=ϕk/k-1X^k-1Pk/k-1=ϕk/k-1Pk-1ϕk/k-1T+Γk-1Qk-1Γk-1TPk/k-1=λkPk/k-1Kk=Pk/k-1HkTHkPk/k-1HkT+Rk-1X^k=X^k/k-1+KkZk-HkX^k/k-1Pk=E-KkHkPk/k-1


### 2.3. Multiple Fading Factor Strong Tracking Kalman Filter

Conventional STKF regulates the prediction covariance matrix by introducing a scalar fading factor. Although the algorithm can be easily realized, it makes each channel with the same fading rate, which will degrade the estimation performance of the filter to a certain extent. Generally speaking, the effect of model uncertainties on each state part may be different; the accuracy of filter will be lost when a significant uncertainty exists in the model, which becomes a fatal deficiency for STKF method. Therefore, based on the aforementioned consideration, a straightforward way to attenuate the negative effect of the STKF method is to adopt multiple fading factors to make different channels have different fading rate; (14) can be rewritten as follows: (20)Eεk/k-1εk/k-1T=HkΛkPk/k-1ΛkTHkT+Rk(21)Λk=Λ1,1Λ2,2⋱Λn,n=λkσk(22)σk=σ1,1σ2,2⋱σn,nwhere multiple fading factors Λk are defined as diagonal matrix and σk is predetermined coefficients aiming at each state variables.The multiple fading factor can also be determine by calculating the trace on both sides of (20) in the same way, and the results are shown as(23)Λk=max⁡1,trNktrMkσk(24)Nk=Vk-Rk(25)Mk=HkσkPk/k-1σkTHkT(26)Vk=ε1ε1Tk=1βVk-1+εkεkT1+β,k≥2where tr is the trace operation, β is the forgetting factor, and its value is in the range from 0 to 1.


### 2.4. Multiple Fading Factor Strong Tracking Sequential Kalman Filter

For MSTKF, although predetermined coefficientsσk is utilized to overcome the issue that each channel with the same fading rate, Nk and Mk is one-multidimensional matrix, from the viewpoint of algebraic derivation of (23), the operation of calculating the trace takes into account the influence of all innovation sequences as a whole, which makes the component of mutation state affect the component of unmutated state and will have an adverse effect on the precision of the MSTKF. Therefore, in order to weaken the coupling relationship between the different states and further refine the result of MSTKF method, the concept of sequential filter has been incorporated into MSTKF method to perform MSTKF method separately aiming at each measurement component.Based on the idea of progressive measurement update, sequential filtering decomposes the high-dimensional measurement updating into several low-dimensional measurement updating methods, which can effectively reduce the computational complexity and further improve the real-time performance. Although the sequential filtering can reduce the computational burden of KF, it cannot reduce the coupling relationship between each state. However, the idea of using a single measurement state at each iteration in sequential filtering is incorporated into the STKF, and selection of fading factor based on the relationship between single measurement values and system of each state can effectively weaken the relationship between each state.For the initial alignment of the shearer, the east velocity error is mainly induced by the north misalignment angles and the east gyroscope bias; therefore, the designer can design fading factor with only consideration of the north misalignment angle and east gyroscope bias, which is equivalent to designing the different STFK aiming at the east velocity error and the north velocity error.Sequential filtering technique makesNk and Mk be one-dimensional vectors; hence, the operation of calculating the trace will not affect another state and the coupling relationship of each state can be weakened. The design procedure of the MSTSKF algorithm is as follows.The measurement equations at k moment are divided into N groups(27)Zk1Zk2⋮ZkN=Hk1Hk2⋮HkNXk+Vk1Vk2⋮VkNThe measurement noise variance matrix can also be described in the form of diagonal blocks(28)Rk=Rk1Rk2⋱RkNThe i-th measurement component of the M-dimensional measurement equation can be expressed as(29)Zki=HkiXk+VkiEquation (20) can be rewritten as(30)Eεk/k-1iεk/k-1iT=HkiΛkiPk/k-1iΛkiTHk/k-1iT+Rki(31)Λki=Λ1,1iΛ2,2i⋱Λn,ni=λkiσki(32)σki=σ1,1iσ2,2i⋱σn,niwhere multiple fading factors Λk(i) are defined as diagonal matrix and σk(i) is predetermined coefficients aiming at each measurement.The multiple fading factorΛk(i) can also be determine by calculating the trace on both sides of (30) in the same way, and the results are shown as(33)Λki=max⁡1,trNkitrMkiσki(34)Nki=Vki-Rkii(35)Mki=HkiσkiPk/k-1i-1σkiTHkiT(36)Vki=ε1iε1iTk=1βiVk-1i+εkiεkiT1+βi,k≥2wheretr is the trace operation, β(i) is the forgetting factor and its value is in the range from 0 to 1, and the proposed MSTSKF method is shown in Algorithm 1.Algorithm 1:Multiple fading factor strong tracking sequential Kalman filter.
1: Input:X^k-1, Pk-1, Φk/k-1, Qk-1, Rk-1, Hk, Zk, β(i), σ(i) Time update 2:Xk/k-1=Φk/k-1X^k-1 3:Pk/k-1=Φk/k-1Pk-1Φk/k-1T+Qk-1 Measurement update: 4: Initialization:Pk/k-1(0)=Pk/k-1 5:  for i=1  : N 6:    Calculate innovation sequenceVk(i) using (36) 7:    Update Multiple fading factorΛk(i) using using (33)~(35) 8:Pk/k-1(i-1)=Λk(i)Pk/k-1(i-1)(Λk(i))T Sequential filtering update: 9:    UpdateKk(i), X^k(i) and Pk/k-1(i): 10:Kk(i)=Pk/k-1(i-1)(Hki)T(HkiPk/k-1(i-1)(Hki)T+Rk(ii))-1 11:X^k(i)=X^k(i-1)+Kki(Zki-HkiX^k(i-1)) 12:Pk/k-1(i)=(E-Kk(i)Hki)Pk/k-1(i) End for 13: Outputs:X^k=X^k(i),Pk-1=Pk/k-1(i)


## 2.1. The Fine Alignment Equation of the SINS

The attitude and velocity error equations of the strap-down system are as follows:(1)ϕ˙=ϕ×ωien+ωenn+δωinn-δωibn(2)δv˙n=fsfn×ϕ+vn×2δωien+δωenn-2ωien+ωenn×δvn+δfsfn+δgnwhere ϕ and δvn are the velocity and attitude error vectors, respectively, ωien denotes the Earth’s rotation rate, fsfn is the specific force measured by an accelerometer in the navigation frame, εn and ∇n are the gyro drift error vector and accelerometer bias error vector, respectively, and δ in (2) denotes error vector variable.The rough initial attitude matrix is obtained by the coarse alignment without any prior knowledge, and coarse alignment provides a fairly good initial condition for the fine alignment since the fine alignment requires prior initialization information to guarantee that the nonlinear error models of initial alignment can be transformed into the linear error models, The shearer is in a static state when the initial alignment is carried out;ωenn and (2ωien+ωenn)×δvn, induced by the linear motion, can be approximated as zero. Besides, accelerometer and gyroscope errors can be equivalent to accelerometer constant drift and gyroscope constant drift. Therefore, the linear error models of initial alignment under small misalignment angles are as follows:(3)ϕ˙=ϕ×ωien-εn(4)δv˙n=fsfn×ϕ+∇nFor the initial alignment of the shearer, the initial attitude angles errors and the sensor bias errors make the differences between the real velocity and open-loop calculated velocity. During the process of initial alignment, the real velocity is zero since the shearer remains stationary. Therefore, the observation equation in fine alignment is constructed by the velocity difference between the velocity information obtained by solving SINS navigation equations and the real velocity(5)Z=VINS-Vreal=HkXk+Rkwhere Rk is measured noise and the observation matrix Hk is denoted as(6)Hk=02×3E2×202×2E 2 × 2 in (6) denotes 2D identity matrix, (3) ~ (6) constitute the state equations and the observation equations of the KF, and the optimal estimation can be achieved when the statistical parameters of the noise are consistent with the theoretical value. However, in practical engineering applications, the statistical parameters of the noise may be inaccurate or unknown; it is necessary to introduce STKF dealing with the problem that inaccurate or unknown noise inherent in the system.


## 2.2. Strong Tracking Kalman Filter

First, the state space model and observation model can be described as(7)Xk=ϕk/k-1Xk-1+Γk-1Wk-1Zk=HkXk+Vkwhere Xk and Zk denote the state vector and measurement vector in the dynamic system at discrete time k, ϕk/(k-1) and Hk are the system transition matrix and observation matrix, respectively, Γk-1 denotes the system noise assignment matrix at the k-1 time, Wk-1 and Vk are the Gaussian white noise sequence with zero-mean and independent for each other, and the statistical characteristics of Vk and Wk are shown as(8)EWk=0EWkWjT=QkδkjEVk=0EVkVjT=RkδkjEWkVjT=0where the symbol of δ in (8) denotes unit sampling signal ant it is defined as follows:(9)δkj=1k=j0k≠jThe equation of innovation sequence in KF is defined as follows:(10)εk/k-1=Zk-Z^k/k-1When establishing model without any model errors and with precise description of the noise, the innovation sequence of the standard KF equation should be a zero-mean Gaussian white noise sequence [32], which can be shown as (11)Eεk/k-1εk/k-1T=Hkϕk/k-1Pk-1ϕk/k-1THkT+HkΓk-1Qk-1Γk-1THkT+Rk=HkPk/k-1HkT+RkIf there are no significant uncertainties in the system process models and noise statistics information, the KF will converge with time going. However, in a various complex practical circumstances, the availability of precisely known model is unrealistic and the statistics characteristics of the noise may vary significantly due to hostile environment. Therefore, the balance of (11) will be destroyed since the parameter of system architecture and the description of noise show mismatch; this phenomenon can be viewed as innovation mismatch. This deficiency severely limits the applications of the traditional KF method.The basic idea and concept of STKF are to make the errors induced by the inaccurate system model or noise parameters equivalent to the filter estimation errors. The fading factor is incorporated into the process of the KF, which will adjust the predicted error covariance matrix in real time so that the orthogonal principle of the innovation sequence can be satisfied.(12)Exk-x~kxk-x~kT=min(13)Eεk+jεkT=0where (12) denotes the performance evaluation indicator for the filter, and (13) denote that the innovation sequence must meet the requirement orthogonal principle.The predicted error covariance matrix has been continually adjusted artificially, and (11) can be rewritten as(14)Eεk/k-1εk/k-1T-Rk=HkλPk/k-1HkTCalculating the trace on both sides of (14) and taking into account the fact that value of the fading factor should be greater than 1, one can obtain(15)λk=max⁡1,trNktrMk(16)Nk=Vk-Rk(17)Mk=HkPk/k-1HkT(18)Vk=ε1ε1Tk=1βVk-1+εkεkT1+β,k≥2where tr is the trace operation, β is the forgetting factor and its value is in the range from 0 to 1, and the complete STKF algorithm is as follows:(19)X^k/k-1=ϕk/k-1X^k-1Pk/k-1=ϕk/k-1Pk-1ϕk/k-1T+Γk-1Qk-1Γk-1TPk/k-1=λkPk/k-1Kk=Pk/k-1HkTHkPk/k-1HkT+Rk-1X^k=X^k/k-1+KkZk-HkX^k/k-1Pk=E-KkHkPk/k-1


## 2.3. Multiple Fading Factor Strong Tracking Kalman Filter

Conventional STKF regulates the prediction covariance matrix by introducing a scalar fading factor. Although the algorithm can be easily realized, it makes each channel with the same fading rate, which will degrade the estimation performance of the filter to a certain extent. Generally speaking, the effect of model uncertainties on each state part may be different; the accuracy of filter will be lost when a significant uncertainty exists in the model, which becomes a fatal deficiency for STKF method. Therefore, based on the aforementioned consideration, a straightforward way to attenuate the negative effect of the STKF method is to adopt multiple fading factors to make different channels have different fading rate; (14) can be rewritten as follows: (20)Eεk/k-1εk/k-1T=HkΛkPk/k-1ΛkTHkT+Rk(21)Λk=Λ1,1Λ2,2⋱Λn,n=λkσk(22)σk=σ1,1σ2,2⋱σn,nwhere multiple fading factors Λk are defined as diagonal matrix and σk is predetermined coefficients aiming at each state variables.The multiple fading factor can also be determine by calculating the trace on both sides of (20) in the same way, and the results are shown as(23)Λk=max⁡1,trNktrMkσk(24)Nk=Vk-Rk(25)Mk=HkσkPk/k-1σkTHkT(26)Vk=ε1ε1Tk=1βVk-1+εkεkT1+β,k≥2where tr is the trace operation, β is the forgetting factor, and its value is in the range from 0 to 1.


## 2.4. Multiple Fading Factor Strong Tracking Sequential Kalman Filter

For MSTKF, although predetermined coefficientsσk is utilized to overcome the issue that each channel with the same fading rate, Nk and Mk is one-multidimensional matrix, from the viewpoint of algebraic derivation of (23), the operation of calculating the trace takes into account the influence of all innovation sequences as a whole, which makes the component of mutation state affect the component of unmutated state and will have an adverse effect on the precision of the MSTKF. Therefore, in order to weaken the coupling relationship between the different states and further refine the result of MSTKF method, the concept of sequential filter has been incorporated into MSTKF method to perform MSTKF method separately aiming at each measurement component.Based on the idea of progressive measurement update, sequential filtering decomposes the high-dimensional measurement updating into several low-dimensional measurement updating methods, which can effectively reduce the computational complexity and further improve the real-time performance. Although the sequential filtering can reduce the computational burden of KF, it cannot reduce the coupling relationship between each state. However, the idea of using a single measurement state at each iteration in sequential filtering is incorporated into the STKF, and selection of fading factor based on the relationship between single measurement values and system of each state can effectively weaken the relationship between each state.For the initial alignment of the shearer, the east velocity error is mainly induced by the north misalignment angles and the east gyroscope bias; therefore, the designer can design fading factor with only consideration of the north misalignment angle and east gyroscope bias, which is equivalent to designing the different STFK aiming at the east velocity error and the north velocity error.Sequential filtering technique makesNk and Mk be one-dimensional vectors; hence, the operation of calculating the trace will not affect another state and the coupling relationship of each state can be weakened. The design procedure of the MSTSKF algorithm is as follows.The measurement equations at k moment are divided into N groups(27)Zk1Zk2⋮ZkN=Hk1Hk2⋮HkNXk+Vk1Vk2⋮VkNThe measurement noise variance matrix can also be described in the form of diagonal blocks(28)Rk=Rk1Rk2⋱RkNThe i-th measurement component of the M-dimensional measurement equation can be expressed as(29)Zki=HkiXk+VkiEquation (20) can be rewritten as(30)Eεk/k-1iεk/k-1iT=HkiΛkiPk/k-1iΛkiTHk/k-1iT+Rki(31)Λki=Λ1,1iΛ2,2i⋱Λn,ni=λkiσki(32)σki=σ1,1iσ2,2i⋱σn,niwhere multiple fading factors Λk(i) are defined as diagonal matrix and σk(i) is predetermined coefficients aiming at each measurement.The multiple fading factorΛk(i) can also be determine by calculating the trace on both sides of (30) in the same way, and the results are shown as(33)Λki=max⁡1,trNkitrMkiσki(34)Nki=Vki-Rkii(35)Mki=HkiσkiPk/k-1i-1σkiTHkiT(36)Vki=ε1iε1iTk=1βiVk-1i+εkiεkiT1+βi,k≥2wheretr is the trace operation, β(i) is the forgetting factor and its value is in the range from 0 to 1, and the proposed MSTSKF method is shown in Algorithm 1.Algorithm 1:Multiple fading factor strong tracking sequential Kalman filter.
1: Input:X^k-1, Pk-1, Φk/k-1, Qk-1, Rk-1, Hk, Zk, β(i), σ(i) Time update 2:Xk/k-1=Φk/k-1X^k-1 3:Pk/k-1=Φk/k-1Pk-1Φk/k-1T+Qk-1 Measurement update: 4: Initialization:Pk/k-1(0)=Pk/k-1 5:  for i=1  : N 6:    Calculate innovation sequenceVk(i) using (36) 7:    Update Multiple fading factorΛk(i) using using (33)~(35) 8:Pk/k-1(i-1)=Λk(i)Pk/k-1(i-1)(Λk(i))T Sequential filtering update: 9:    UpdateKk(i), X^k(i) and Pk/k-1(i): 10:Kk(i)=Pk/k-1(i-1)(Hki)T(HkiPk/k-1(i-1)(Hki)T+Rk(ii))-1 11:X^k(i)=X^k(i-1)+Kki(Zki-HkiX^k(i-1)) 12:Pk/k-1(i)=(E-Kk(i)Hki)Pk/k-1(i) End for 13: Outputs:X^k=X^k(i),Pk-1=Pk/k-1(i)


## 3. Performance Evaluation

In this section, simulations are presented to verify the superiority and effectiveness of the proposed algorithm in the initial alignment of the shearer. Middle precision inertial measurement modules are selected in this simulation, and its performance indexes of employed sensors are listed in Table2.Table 2
Initial parameters.
Parameter Value Gyroscope drift 0.01 ° / h Gyroscope noise 0.005 ° / h Accelerometer bias 10 - 5 g Accelerometer noise 10 - 4 g / H z Initial longitude 117 ° E Initial latitude 34 ° NIn the simulation, in order to better compare the performance of the above-mentioned filtering algorithms, the proposed MSTSKF and existing filter algorithms are applied in the initial alignment of the shearer with inaccurate and time-varying noise covariance matrix. In this simulation, the sampling frequency is 100 Hz, the forgetting factor is 0.95 in both MSTKF method and MSTSKF method,σk is diag (5,10,5,30,30) in the MSTKF method, σk(1) is diag (1,10,1,30,30), and σk(2) is diag (10,1,1,30,30) in the MSTSKF method. The simulation lasts about 600 s and the alignment results of KF, MSTKF, and MSTSKF are discussed and comprehensively evaluated. Note that the STKF was found to diverge in the process of simulation; thus its results are not shown in the following comparisons.


### 3.1. The System Noise Parameter Mismatch

The standard KF can obtain the optimal estimation of the state only under the limited condition that the structural parameters of the dynamic system and the statistical parameters of the noises are accurately known. However, in many practical applications, the determination of the statistical characteristics of system noise is commonly depending on the precision of inertial sensors or personal experience, which leads to the unavailable for the accurate noise statistical parameters and the inconsistency between the setup noise covariance matrix and the true noise covariance matrix. In this simulation, the real system noise covariance matrix is magnified 50 times to validate the stability and accuracy of the various filtering algorithms from the perspective of the mismatch of the statistical characteristics of the noise.As observed from Figure1, when the system noise covariance matrix is inconsistent with the theoretical value, the conventional KF produces a dramatic fluctuation phenomenon during the filtering process for the roll angle and pitch angle, and its filtering accuracy is also slightly poorer compared with the MSTKF or MSTSKF method. From the heading angle error curve, it is observed that the inaccurate noise covariance matrix will result in divergence of the conventional KF. Although the heading attitude errors under the MSTKF and MSTSKF methods converge with time, the proposed MSTSKF method can achieve a more satisfactory performance in the accuracy of the heading attitude; Figure 1 clearly exhibits that the steady-state errors of heading angle under the MSTKF and MSTSKF methods at the final time are 0.2003° and 0.0073°, respectively.Figure 1
Attitude angle error of the simulation when the system noise covariance matrix is magnified 50 times.


### 3.2. The System Noise Parameter Abrupt Change

The process of initial alignment for shearer is prone to interference from surrounding hostile environments such as vibration of the shearer, and the corresponding performance of gyroscope and accelerometer will be compromised and voluntary to divergence; thus, the statistical characteristics of system noise will be inevitably changed with time going. In this simulation, there is a drastic change of system noise during the time periods of 300 s to 400 s, the true system noise covariance matrix expands 50 times compared with the set value, and the simulation results are shown in Figure2.Figure 2
Attitude angle error of the simulation when the system noise has a sudden change.From Figure2, it is observed that all the methods have the equal convergence rates and filtering accuracy when the system noise covariance matrix is not mutated. However, when the system noise covariance matrix varies significantly induced by the performance degradation of the sensor or unstable external environment, the conventional KF has a sharp spike and the heading attitude error diverges to a certain extent. Both the MSTKF and MSTSKF methods possess the advantage of robustness and reliability for the mutation of the system noise. However, the proposed MSTSKF method is more robust against noise covariance matrix mutation compared with the MSTKF method.


### 3.3. The System Noise of Z-Axis Accelerometer Is Mismatch

The above initial alignment simulation investigates the situation where the covariance matrix of the noise is mismatch due to the unstable external environment. However, for practical application, the effect of the environment on different state components makes a certain differences. The vibration of shearer body induced by external hostile environment is easier to degrade the performance of Z-axis accelerometer in the entire initial alignment process. Therefore, the following simulation increases the noise of the Z-axis accelerometer only to 50 times as much as the original set value.It can be seen from Figure3 that the proposed MSTSKF method can not only align the attitude within a very short period of time, but also converge to a satisfactory estimation accuracy. However, the MSTKF method has a drastic fluctuation and sometimes even inferior to the KF method. The corresponding reasons are as follows: the coupling relationship among each state makes the easily mutated states produce an effect on the nonmutated states, which may cause the fluctuation phenomenon as mentioned above. To address this issue, the proposed MSTSKF method designs the fading factors aiming at each measurement value, respectively. This method can reduce the coupling between states and greatly improve the performance and stability of the filter.Figure 3
Attitude angle error of the simulation when the noise of Z-axis accelerometer increases to 50 times.


### 3.4. Initial Misalignment Angles with Large Angles

The precise alignment equation of the shearer can be approximated as linear error models based on the assumption that the initial misalignment angle is constrained in several degrees. Therefore, the above linear KF initial alignment equations are valid only when the misalignment angles are limited to a few degrees. When the initial misalignment angles are large angles, the initial alignment equation will become nonlinear equations, which will inevitably lead to a model mismatch and high nonlinearities introduced into the process model will increase the uncertainty of system noise. Aiming at this issue, large initial misalignment angles are selected to verify whether the proposed algorithm exhibits a robust for uncertainty of the model under large misalignment angles, the initial misalignment angles are set as10°, 10°, and 50°, respectively, and the simulation results are shown in Figure 4.Figure 4
Attitude angle error of the simulation when the initial misalignment angles are large angles.The simulation shows that the performance of the MSTKF and MSTSKF methods is superior to the conventional KF method when large initial misalignment angle is present. For the pitch angle and roll angle, MSTSKF and MSTKF methods have a comparable performance from the perspective of the convergence rate and accuracy, but both slightly higher than KF method. For the heading angle, both the MSTKF and MSTSKF exhibit fast convergence speed and filter accuracy compared with the KF method. The errors of the heading angle can reach ultimate precision under the MSTKF and MSTSKF method at the end of the simulation. However, the error of the heading angle calculated by the KF method is0.4768° at the end of the simulation; it is clearly shown that the heading error cannot guarantee less than 0.1° within 600 s when the initial misalignment angles are unknown. Therefore, MSTKF and MSTSKF method are more suitable for the initial alignment of the shearer under large initial misalignment angles.


## 3.1. The System Noise Parameter Mismatch

The standard KF can obtain the optimal estimation of the state only under the limited condition that the structural parameters of the dynamic system and the statistical parameters of the noises are accurately known. However, in many practical applications, the determination of the statistical characteristics of system noise is commonly depending on the precision of inertial sensors or personal experience, which leads to the unavailable for the accurate noise statistical parameters and the inconsistency between the setup noise covariance matrix and the true noise covariance matrix. In this simulation, the real system noise covariance matrix is magnified 50 times to validate the stability and accuracy of the various filtering algorithms from the perspective of the mismatch of the statistical characteristics of the noise.As observed from Figure1, when the system noise covariance matrix is inconsistent with the theoretical value, the conventional KF produces a dramatic fluctuation phenomenon during the filtering process for the roll angle and pitch angle, and its filtering accuracy is also slightly poorer compared with the MSTKF or MSTSKF method. From the heading angle error curve, it is observed that the inaccurate noise covariance matrix will result in divergence of the conventional KF. Although the heading attitude errors under the MSTKF and MSTSKF methods converge with time, the proposed MSTSKF method can achieve a more satisfactory performance in the accuracy of the heading attitude; Figure 1 clearly exhibits that the steady-state errors of heading angle under the MSTKF and MSTSKF methods at the final time are 0.2003° and 0.0073°, respectively.Figure 1
Attitude angle error of the simulation when the system noise covariance matrix is magnified 50 times.


## 3.2. The System Noise Parameter Abrupt Change

The process of initial alignment for shearer is prone to interference from surrounding hostile environments such as vibration of the shearer, and the corresponding performance of gyroscope and accelerometer will be compromised and voluntary to divergence; thus, the statistical characteristics of system noise will be inevitably changed with time going. In this simulation, there is a drastic change of system noise during the time periods of 300 s to 400 s, the true system noise covariance matrix expands 50 times compared with the set value, and the simulation results are shown in Figure2.Figure 2
Attitude angle error of the simulation when the system noise has a sudden change.From Figure2, it is observed that all the methods have the equal convergence rates and filtering accuracy when the system noise covariance matrix is not mutated. However, when the system noise covariance matrix varies significantly induced by the performance degradation of the sensor or unstable external environment, the conventional KF has a sharp spike and the heading attitude error diverges to a certain extent. Both the MSTKF and MSTSKF methods possess the advantage of robustness and reliability for the mutation of the system noise. However, the proposed MSTSKF method is more robust against noise covariance matrix mutation compared with the MSTKF method.


## 3.3. The System Noise of Z-Axis Accelerometer Is Mismatch

The above initial alignment simulation investigates the situation where the covariance matrix of the noise is mismatch due to the unstable external environment. However, for practical application, the effect of the environment on different state components makes a certain differences. The vibration of shearer body induced by external hostile environment is easier to degrade the performance of Z-axis accelerometer in the entire initial alignment process. Therefore, the following simulation increases the noise of the Z-axis accelerometer only to 50 times as much as the original set value.It can be seen from Figure3 that the proposed MSTSKF method can not only align the attitude within a very short period of time, but also converge to a satisfactory estimation accuracy. However, the MSTKF method has a drastic fluctuation and sometimes even inferior to the KF method. The corresponding reasons are as follows: the coupling relationship among each state makes the easily mutated states produce an effect on the nonmutated states, which may cause the fluctuation phenomenon as mentioned above. To address this issue, the proposed MSTSKF method designs the fading factors aiming at each measurement value, respectively. This method can reduce the coupling between states and greatly improve the performance and stability of the filter.Figure 3
Attitude angle error of the simulation when the noise of Z-axis accelerometer increases to 50 times.


## 3.4. Initial Misalignment Angles with Large Angles

The precise alignment equation of the shearer can be approximated as linear error models based on the assumption that the initial misalignment angle is constrained in several degrees. Therefore, the above linear KF initial alignment equations are valid only when the misalignment angles are limited to a few degrees. When the initial misalignment angles are large angles, the initial alignment equation will become nonlinear equations, which will inevitably lead to a model mismatch and high nonlinearities introduced into the process model will increase the uncertainty of system noise. Aiming at this issue, large initial misalignment angles are selected to verify whether the proposed algorithm exhibits a robust for uncertainty of the model under large misalignment angles, the initial misalignment angles are set as10°, 10°, and 50°, respectively, and the simulation results are shown in Figure 4.Figure 4
Attitude angle error of the simulation when the initial misalignment angles are large angles.The simulation shows that the performance of the MSTKF and MSTSKF methods is superior to the conventional KF method when large initial misalignment angle is present. For the pitch angle and roll angle, MSTSKF and MSTKF methods have a comparable performance from the perspective of the convergence rate and accuracy, but both slightly higher than KF method. For the heading angle, both the MSTKF and MSTSKF exhibit fast convergence speed and filter accuracy compared with the KF method. The errors of the heading angle can reach ultimate precision under the MSTKF and MSTSKF method at the end of the simulation. However, the error of the heading angle calculated by the KF method is0.4768° at the end of the simulation; it is clearly shown that the heading error cannot guarantee less than 0.1° within 600 s when the initial misalignment angles are unknown. Therefore, MSTKF and MSTSKF method are more suitable for the initial alignment of the shearer under large initial misalignment angles.


## 4. Experimental Verification

In this section, field experiments were established to illustrate the effectiveness and superiority of the proposed algorithm; the experimental data of the shearer is collected to evaluate the performance of the proposed algorithm. As shown in Figure5, the experimental platform consists of the self-made fiber-optic SINS, MG150/345-WDK type shearer, navigation computer, and power supply. The fiber-optic SINS is composed of three-axis accelerometers and three-axis gyroscopes. Gyroscope constant bias is 0.01°/h, gyroscope angle random walk error is 0.005°/h, accelerometer constant bias is 10-5g, accelerometer random walk error is 10-4g/Hz, initial longitude is 117°E, and initial latitude is 34°N. The angular rate and specific force of the shearer body are measured by the self-made fiber-optic gyroscope and accelerometer with a 100 Hz sampling frequency. The proposed algorithm and existing algorithms are coded with MATLAB and the experiments are run on a computer with Intel Core i7-4720HQ CPU at 2.60 GHz.Figure 5
Shearer mounted strap-down inertial navigation system in this experiment.In the experiment test, fiber-optic SINS is fixedly equipped on the shearer as shown in Figure5, after preheating and stabilizing of the SINS, the shearer firstly holds in the stationary for about 600 s, and the aforementioned total 600 s accelerometer data and gyroscope dates are saved for postmortem high accuracy of the initial alignment. The obtained initial alignment results are considered as the comparison metrics to evaluating the performance of the aforementioned initial alignment algorithm. Next, the operator walked and spoke loudly around the shearer and simultaneously started the shearer to left the drum idling. The field experimental data is collected to evaluate the performance of the proposed MSTSKF method in large initial misalignment angles and mismatch of noise covariance matrix cases. In this experiment, the forgetting factor is 0.95 in both MSTKF method and MSTSKF method, the σk is diag(5,15,15, 30,30) in the MSTKF method, σk(1) is diag(1,10,1,30,30), and σk(2) is diag (10,1,1,30,30) in the MSTSKF method.When the initial misalignment angles are set as10°, 10°, and 50°, respectively, Figure 6 shows the result of the initial alignment errors under the large misalignment angles. The test result is identical to the simulation result; the proposed MSTSKF algorithm achieves almost optimal performance in accuracy and convergence rate. Different from the simulation result is that the convergence rate of the heading angle is slightly slower than simulation results; the reasons are as follows: in this experimental, the determination of the statistical parameters of the noises depends on the performance of sensors. However, the performance of the sensor has a degradation since the unstable external environment makes the differences between the set values and the actual values of the noise parameters; this critical factor leads to the convergence rate of the heading angle which is slightly slower than simulation results.Figure 6
Attitude angle error of the experiment when the initial misalignment angles are large angles.The experimental parameters are set as follows: initial misalignment angles are set as1°, 1°, and 5°, respectively, and the system noise covariance matrix is 1/20 of the true noise covariance matrix (the true noise covariance matrix refers to the noise covariance matrix according to the performance of the sensor). It is observed from Figure 7 that all the filter methods can converge with time, but the proposed MSTSKF method provides a strong robustness against the change of the environment compared with KF and MSTKF methods. In addition, the convergence accuracy of the proposed MSTSKF method also achieved satisfactory result. The final convergence accuracy of the heading angle under the three algorithms is 0.0139°, -0.1176°, and 0.0056°, respectively. Therefore, the proposed MSTSKF method is more applicable for the rapid and precise initial alignment of the shearer.Figure 7
Attitude angle error of the experiment when the system noise covariance matrix is magnified 50 times.


## 5. Conclusions

This paper investigates the initial alignment for the SINS under the complex environment of the mechanized mining face. Aiming at the problems of slow convergence and poor accuracy for the existing KF algorithm under the complex environment of the mechanized mining face, this paper systematically proposes MSTSKF method to suppress inaccurate or time-varying noise statistics induced by harsh environment. The proposed MSTSKF method incorporates the sequential filter into MSTKF method, based on which multiple fading factor and forgetting factor for east and north velocity measurement are designed, respectively. MSTSKF method can effectively suppress the coupling between each state and significantly improve the filtering performance under complicated environment. Simulation and filed tests demonstrate that the proposed MSTSKF method is superior to KF and MSTKF method in dealing with the model uncertainty, such as the parameter mismatch, state mutation, large initial misalignment angles, and statistic errors of process noise. Therefore, the proposed MSTSKF method has a wide prospect in initial alignment of the shearer.



---

*Source: 3172501-2019-03-18.xml*