A new polar alignment algorithm based on the Huber estimation filter with the aid of BeiDou Navigation Satellite System

For aircrafts equipped with BeiDou Navigation Satellite System/Strapdown Inertial Navigation System integrated navigation system, BeiDou Navigation Satellite System information can be used to achieve autonomous alignment. However, due to the complex polar environment and multipath effect, BeiDou Navigation Satellite System measurement noise often exhibits a non-Gaussian distribution that will severely degrade the estimation accuracy of standard Kalman filter. To address this problem, a new polar alignment algorithm based on the Huber estimation filter is proposed in this article. Considering the special geographical conditions in the polar regions, the dynamic model and the measurement model of BeiDou Navigation Satellite System/Strapdown Inertial Navigation System integrated alignment system in the grid frame are derived in this article. The BeiDou Navigation Satellite System measurement noise characteristics in the polar regions are analyzed and heavy-tailed characteristics are simulated, respectively. Since the estimation accuracy of standard Kalman filter can be severely degraded under non-Gaussian noise, a Kalman filter based on the Huber estimation is designed combining grid navigation system and generalized maximum likelihood estimation. The simulation and experiment results demonstrate that the proposed algorithm has better robustness under non-Gaussian noise, and it is effective in the polar regions. By employing the proposed algorithm, the rapidity and accuracy of the alignment process can be improved.


Introduction
The polar regions are becoming more and more important for all mankind, and the polar flight ability is of great significance. [1][2][3] The lack of high-precision navigation is one of the most prominent problems in polar navigation. As the most important autonomous navigation equipment, Strapdown Inertial Navigation System (SINS) can provide all kinds of high-frequency navigation information while maintaining good concealment and real-time performance. Compared with other navigation systems, SINS has better navigation performance in high latitude areas and is the optimal choice 1 College of Automation Engineering, Nanjing University of Aeronautics and Astronautics, Nanjing, China for polar navigation. 4,5 Accelerometers and gyroscopes are employed to measure the motion parameters of the aircraft. Under precise initial conditions, the navigation results can be obtained by recursive calculation. [7][8][9] The performance of initial alignment will affect the navigation accuracy. Two major issues need to be addressed for initial alignment in the polar regions. One is that the traditional north-oriented frame will lose effectiveness because of the rapid convergence of meridians. 10 The other is that the stationary base alignment cannot be achieved due to the decrease of the horizontal component of rotational angular velocity of the earth.
The traditional local-level north-oriented frame is widely used in the non-polar regions. Its x-axis and yaxis are both in the local level while z-axis coincides with geography up. The y-axis always points to true north as the heading reference. However, due to the rapid convergence of meridians in the polar regions, the traditional north-oriented frame cannot work properly. Although the local-level wander-azimuth frame and the local-level free-azimuth frame are proposed, there are singular values when extracting the wanderazimuth angle and longitude from the position direction cosine matrix. 11 In order to solve this problem, transversal SINS mechanization and grid SINS mechanization are proposed. In the transversal SINS mechanization, there is principle error when using the earth standard sphere model. However, the calculation is more complicated when using the earth ellipsoid model. 10 Therefore, we adopt the grid SINS mechanization for polar alignment.
In the polar regions, the horizontal component of rotational angular velocity of the earth decreases as latitude increases. Therefore, stationary base alignment cannot be achieved. It is necessary to use external information to assist the moving base alignment. A new polar alignment algorithm with the aid of star sensor is proposed and the accurate information provided by star sensor is employed as the measurement to improve the performance of initial alignment. 12 The OCTANS composed of three fiber-optic gyroscopes and three quartz accelerometers can provide the attitude information of UUV (Unmanned Underwater Vehicles) quickly and accurately, and it is used to improve the alignment accuracy of polar UUV. 13 An SINS/radar alignment algorithm is proposed to improve the attitude estimation accuracy. 1 The output of DVL (Doppler Velocity Log) is employed as the measurement in the polar transversal initial alignment algorithm. 14 The transfer alignment using high-precision master inertial navigation system (INS) output to estimate the states is researched in the polar regions. 15,16 The attitude initialization with the aid of GNSS (Global Navigation Satellite System) in the earth frame could be safely performed in the polar regions. 17 However, it only studies the coarse alignment method, and it does not consider the influence of the complex polar environment on GNSS performance. Research works have shown that the multipath effect is obvious in high latitude areas and will affect the measurement noise characteristics of GNSS. [14][15][16]18 The global networking of BeiDou Navigation Satellite System (BDS) was completed on 23 June 2020. It is of great significance to study the global applications of BDS. BDS consists of GEO (Geostationary Earth Orbit) satellites, IGSO (Inclined Geosynchronous Orbit) satellites, and MEO (Medium Earth Orbit) satellites. Compared with MEO satellites, IGSO satellites have longer visibility in high latitude areas. BDS can provide navigation services independently in the polar regions. 19 In recent years, BDS/SINS integrated navigation system is increasingly used in many fields due to their integrated performance outweighs the drawbacks of the individual systems. 6,20,21 For aircrafts equipped with BDS/SINS integrated navigation system, BDS information can be used to initialize SINS and achieve autonomous alignment. However, BDS also has multipath effect in the polar regions. Furthermore, the performance of standard Kalman filter for BDS/SINS integrated alignment will be degraded. Particle filter and Gaussian-sum filter are introduced to overcome the limitation of standard Kalman filter on noise characteristics. However, high computational complexity and memory burden are also mentioned. 22 A new polar alignment algorithm based on the Huber estimation filter is proposed in this article. The main contribution of this article is to analyze the BDS measurement noise characteristics in the polar regions and to propose a Kalman filter based on the Huber estimation combining grid navigation system and generalized maximum likelihood estimation. The remaining sections are arranged as follows.

SINS mechanization and navigation error equation in the grid frame
The Greenwich meridian is selected as the heading reference to define the grid frame, and the problem caused by meridians convergence in the polar regions can be avoided.

The grid frame
The definition of G frame (the grid frame) is shown in Figure 1. L and l are the latitude and the longitude of the aircraft, respectively. Point P represents the location of the aircraft. The plane passing through point P and parallel to the Greenwich meridian is defined as grid plane. The horizontal plane of point P is defined as tangent plane. The intersecting line of grid plane and tan-gent plane is defined as grid north. The angle between grid north and true north is defined as grid angle s. Grid up coincides with geography up, and grid east makes up the right-handed Cartesian coordinate with grid north in the tangent plane. The transform relationship among e frame (the earth centered earth fixed frame-x-axis intersects the Greenwich meridian in the equatorial plane, y-axis is perpendicular to x-axis in the equatorial plane, and z-axis coincides with the earth axis), g frame (the geography frame, east-north-up), and G frame is shown in Figure 1. C g e represents the direction cosine matrix from e frame to g frame, and C G g represents the direction cosine matrix from g frame to G frame C g e = À sin l cos l 0 À sin L cos l À sin L sin l cos L cos L cos l cos L sin l sin L Thus, the direction cosine matrix from e frame to G frame will be C G e = C G g C g e = À cos s sin l + sin s cos l sin L cos s cos l + sin s sin l sin L À sin s sin L Àsins sin l À cos s cos l sin L sin s cos l À cos s sin l sin L cos s cos L cos L cos l cos L sin l sin L The axial unit vectors for G frame are denoted as (e G E , e G N , e G U ), (e E , e N , e U ) for g frame, and (e X , e Y , e Z ) for e frame. It can be seen from the definition of the angle s and Figure 1 Projecting e G N and e Y to the g frame, hence Substituting equations (5) and (6) into equation (4), we can obtain sin s cos l À cos s sin L sin l = 0 ð7Þ Then sin s = sin L sin l ffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffi 1 À cos 2 Lsin 2 l p ð8Þ cos s = cos l ffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffi 1 À cos 2 Lsin 2 l p ð9Þ Figure 1. The description of the grid frame.
Substituting equations (8) and (9) into equations (2) and (3), the direction cosine matrices C G g and C G e can be calculated.

Grid SINS mechanization
The mechanization of grid SINS is where C G b is the direction cosine matrix from b frame (the body frame, Right-Front-Up) to G frame and _ C G b represents the derivation of C G b ; v Gb is the angular velocity of b frame relative to G frame, and v b Gb is the projection of v Gb on b frame; v ie is rotational angular velocity of the earth, and v G ie is the projection of v ie on G frame; v eG is the angular velocity of G frame relative to e frame, and v G eG is the projection of v eG on G frame; v G is the velocity of SINS in G frame and _ v G represents the derivation of v G ; f b is the specific force in b frame measured by SINS; g G is the projection of gravitational acceleration on G frame; P e is the position coordinates of the aircraft in e frame and _ P e represents the derivation of P e ; C e G is the direction cosine matrix from G frame to e frame; v b Gb 3 À Á represents the skewsymmetric matrix of v b Gb , and it can be defined as: where R Mh and R Nh are the radius of curvature in meridian and prime vertical, respectively. v b Gb can be obtained as follows where C b G is the direction cosine matrix from G frame to b frame, v ib is the angular velocity of b frame relative to i frame (earth centered inertial), and v b ib is the projection of v ib on b frame which could be measured by gyroscopes.

Navigation error equation in the grid frame
The attitude error equation and velocity error equation in G frame can be expressed as where f G is the misalignment angle in G frame and _ f G represents the derivation of f G ; dv G is the velocity error in G frame and d _ v G represents the derivation of dv G ; v iG is the angular velocity of G frame relative to i frame, and v G iG is the projection of v iG on G frame; C v can be expressed as where e b b , e b r , e b w are the random constant, the first-order Markov drift, and the white noise of the gyroscopes, respectively; r b a , r b w are the first-order Markov drift and the white noise of the accelerometers, respectively.

BDS/SINS integrated alignment state equation
The misalignment angle f G , the velocity error dv G , the random constant and first-order Markov drift of the gyroscopes e b b , e b r , and the first-order Markov drift of the accelerometers r b a are chosen as the states of alignment filter According to equations (14) and (15), the system state equation can be rewritten as where X(t) is the state vector and _ X(t) represents the derivation of X(t), A(t) is the state transition matrix, G(t) is the system noise coefficient matrix, and W(t) is the system noise vector. A(t) can be obtained as follows

BDS/SINS integrated alignment measurement equation
The velocity difference between SINS and BDS is chosen as the measurement. The velocity of SINS and BDS in G frame can be expressed as v G , v G B , and the velocity measurement equation can be defined as where Y v (t) is the measurement vector, H v (t) = ½ 0 3 3 3 diag½ 1 1 1 0 3 3 9 is the measurement matrix, and V v (t) is the measurement noise vector.
The BDS measurement noise characteristics in the polar regions and the design of Kalman filter based on the Huber estimation The standard Kalman filter is a minimum l 2 norm estimator. It requires accurately known system model and Gaussian noise. In fact, BDS measurement noise exhibits a non-Gaussian distribution in the complex polar environment, and the estimation accuracy of standard Kalman filter will be severely degraded. It is necessary to analyze the BDS noise characteristics in the polar regions and research the applicable filter.

Analysis of the BDS measurement noise characteristics in the polar regions
The sky plot of BDS satellites observed from the North Pole is shown in Figure 2. 23 None of the BDS satellites passes through the polar regions. As a result, the satellite altitude angle in high latitude regions is significantly lower than in other regions. This section will provide a detailed analysis. GEO satellites become invisible and ineffective above 65°north latitude. The maximum altitude angle of MEO and IGSO satellites decreases gradually as latitude increases. Each IGSO satellite has an 8-shaped ground track, which is distributed in the latitude range of about 6 55°. Compared with MEO satellites, IGSO satellites have longer visibility and larger maximum altitude angle in high latitude areas. Therefore, IGSO satellite is taken as an example to analyze the satellite altitude angle in the polar regions.
The schematic diagram of IGSO satellite altitude angle in the polar regions is shown in Figure 3 The maximum altitude angle b of BDS satellites in the polar regions is about 50°, which is much lower than that in the middle and low latitude areas. Due to the influence of lower satellite altitude angle and variability of surface materials, the multipath effect of BDS is obvious in high latitude areas, [14][15][16]18 which will result in measurement noise with non-Gaussian distributions. 24,25 Heavy-tailed distribution imitated by compound Gaussian distribution is one of the typical non-Gaussian distributions, and it is widely used in research works. [25][26][27] Therefore, we employ the heavytail distribution imitated by compound Gaussian distribution to study the polar alignment. Its probability density function is where s 1 , s 2 are the standard deviations of two Gaussian noise. The former term in equation (21) represents the contaminated Gaussian distribution, and the latter term is the contamination distribution. The parameter e represents the contamination level, whose value is in [0,1]. We take s 1 = 1, s 2 = 3:5, e 1 = 0:1, and e 2 = 0:3 as an example, and simulate with 5000 random points. The probability density curves of noises with different e are depicted in Figure 4. As shown in Figure 4, compared with standard Gaussian noise, the probability density curve of compound Gaussian noise is flatter and the probability of distribution at the tail is larger. This means that the probability of noise distribution beyond 3s and occurrence of outliers will increase in the polar regions, which will degrade the estimation accuracy of standard Kalman filter.

The design of Kalman filter based on the Huber estimation
Huber estimation filter 28,29 is a robust filter method based on the Huber estimation, and it is a generalized maximum likelihood estimation algorithm. It is a minimum l 1 =l 2 mixed norm estimator, while minimizing the maximum asymptotic estimation variance under  compound Gaussian noise, the estimation efficiency of l 2 norm under Gaussian noise is also maintained. It can be used for state estimation under non-Gaussian noise. In this section, a Kalman filter based on the Huber estimation is designed for non-Gaussian measurement noise in polar BDS/SINS integrated alignment.
From the equations (17) and (19), the discrete dynamic model can be obtained The generalized maximum likelihood estimation proposed by Huber requires that the residuals are independent. In order to combine the Kalman filter with Huber's robust estimation strategy, the measurement update should be recast as a linear regression problem between the measurement and the state prediction. If the true value of the state is X k and the one-step prediction of the state is X k, kÀ1 , the state prediction error can be written as d k = X k À X k, kÀ1 . Then, the system model can be written as where I is the identity matrix. Defining the decoupling matrix where R k , P k, kÀ1 represent the measurement noise covariance matrix and the one-step prediction variance matrix, respectively. In order to solve the linear regression problem via maximum likelihood estimation, the residuals' vector need to be decoupled. Multiply both sides of equation (23) by S À1=2 k , the linear regression problem is transformed to where The covariance of j k is the identity matrix, which means the transformed residuals are independent and consistent with the requirement of Huber's estimation. Therefore, the generalized maximum likelihood estimation proposed by Huber can be employed to solve the problem.
The measurement update of Kalman filter based on the Huber estimation can be solved by minimizing the cost function where z i refers to the ith component of the residuals' vector, and the score function r is defined as where r( Á ) is the l 1 =l 2 mixed norm, and g is a tuning parameter which was set to 1.345 in practice. 30 By adopting the cost function given above, the state estimation can minimize the maximum asymptotic estimation variance for the measurement noise with non-Gaussian characteristics. The solution of the linear regression problem is determined from the derivative of the cost function Define the weight function Define the weight matrix and substitute it into equation (31). Then, the modified one-step prediction variance matrixP k, kÀ1 and the measurement noise covarianceR k in Kalman filter based on the Huber estimation can be obtained

Simulation and experiment results
A new polar alignment algorithm based on the Huber estimation filter is proposed in this article. Simulation and experiment are conducted to verify the effectiveness of our approach.

Simulation results and analyses
The latitude and the longitude of the initial location are set as 87°and 126°. The simulation time and the filter period are 200 and 1 s, respectively. The aircraft flies straight in the north direction. The initial velocity is set as 100 m/s and the acceleration is set as 3 m/s 2 in 10-50 s. The experimental trajectory is shown in Figure 5.
The IMU (Inertial Measurement Unit) of SINS consists of three-axis gyroscopes and accelerometers and it is fixed directly to the aircraft. The working environment is harsh. Therefore, the first-order Markov drift of the gyroscopes and accelerometers should be considered. The other parameters for simulation can be set as listed in Table 1.
To compare the performance of standard Kalman filter and Kalman filter based on the Huber estimation  under non-Gaussian noise, assuming that BDS velocity measurement noise distribution follows the compound Gaussian distribution analyzed in section ''Analysis of the BDS measurement noise characteristics in the polar regions''. Take s 1 = 0:2, s 2 = 20, and e = 0:3 The polar alignment algorithm based on the Huber estimation filter with the aid of BDS proposed in this article is called the Proposed Algorithm. The polar alignment algorithm based on the standard Kalman filter with the aid of BDS is called the Conventional Algorithm. The attitude error between the corrected attitude and the true attitude is analyzed. The simulation results are expressed for comparison shown as Figure 6.
The histograms of the attitude error mean and standard deviation of the two alignment methods over a period of 150-200 s are shown in Figure 7.
As shown in Figure 6, the alignment results of conventional algorithm are as follows: the roll error remains at less than 6# after 150 s, the pitch error remains at less than 10# after 150 s, and the yaw error remains at less than 30# after 150 s. And the alignment results of proposed algorithm are as follows: the roll error remains at less than 6# after 70 s, the pitch error remains at less than 10# after 80 s, and the yaw error remains at less than 10# after 85 s. It takes less time to achieve the same alignment accuracy using proposed algorithm than using conventional algorithm.
Combining the mean and standard deviation of the attitude error given in Figure 7, in the same period of time (150-200 s), proposed algorithm achieves better alignment accuracy. The mean of roll error, pitch error, and yaw error of proposed algorithm respectively reduces to about 79%, 15%, and 27% compared with conventional algorithm.
Based on the results shown in Figures 6 and 7, the following conclusions about the simulation results can be made. Under heavy-tailed measurement noise, the performance of proposed algorithm is significantly better than conventional algorithm. The standard Kalman filter is sensitive to heavy-tailed noise and the alignment performance is severely degraded. Proposed algorithm achieves better state estimation, illustrating its robustness to non-Gaussian noise.

Experiment results and analyses
Considering the geography restrictions, the experiment is conducted in the form of semi-physical simulation.
The IMU information includev b ib andf b .v b ib is the practical angular velocity measured by gyroscopes, and it is composed of the true angular velocity v b ib and the gyro drift dv b ib .f b is the practical specific force where the superscript b represents the b frame of SINS.
In the simulation and the experiment, v b ib and f b can be gained by simulation in the polar regions once the attitude variations and maneuvers of the aircraft are confirmed. 12,14,15 The gyro drift and the accelerometer drift would not change in different regions because they are inherent to SINS. Therefore, the gyro drift dv b ib and the accelerometer drift df b for polar experiment can be extracted from actual measurement data in the nonpolar regions. The schematic diagram of data composition for polar experiment is illustrated in Figure 8.
The actual measurement data can be obtained through the experiment conducted in a laboratory in the non-polar regions (31.937°N, 118.798°E). The SINS is installed on a turntable, which can provide a high-precision three-axis rotary movement. The parameters of SINS used in the experiment are listed in Table 2.
According to the calculation process of gyro random walk and bias stability performance, the random walk index is only related to white noise, and the bias stability index is related to white noise and first-order Markov drift. 31 Thus, BDS/SINS integrated alignment system state equation is the same as that in section ''BDS/SINS integrated alignment state equation.'' The experiment results are expressed for comparison shown as Figure 9.
The histograms of the attitude error mean and standard deviation of the two alignment methods over a period of 150-200 s are shown in Figure 10.
As shown in Figure 9, the alignment results of conventional algorithm are as follows: the roll error remains at less than 6# after 150 s, the pitch error remains at less than 10# after 150 s, and the yaw error remains at less than 50# after 150 s. And the alignment results of proposed algorithm are as follows: the roll error remains at less than 6# after 115 s, the pitch error remains at less than 10# after 90 s, and the   yaw error remains at less than 10# after 110 s. It takes less time to achieve the same alignment accuracy using proposed algorithm than using conventional algorithm.
Combining the mean and standard deviation of the attitude error given in Figure 10, in the same period of time (150-200 s), proposed algorithm achieves better alignment accuracy. The mean of roll error, pitch error, and yaw error of proposed algorithm respectively reduces to about 60%, 27%, and 13% compared with conventional algorithm.
As shown in Figures 9 and 10, the semi-physical simulation results further validate that proposed algorithm has better performance than conventional algorithm. The semi-physical simulation results coincide with the simulation results.

Discussion
As shown in the simulation and experiment results, the new polar alignment algorithm based on the Huber estimation filter proposed in this article is much superior to that based on the Kalman filter. The main contributions and the proposed algorithm are discussed as follows: (1) Since the traditional north-oriented frame loses its effectiveness for polar navigation, this article employs the grid SINS mechanization for aircrafts. SINS mechanization and navigation error equation in the grid frame are represented. Furthermore, the dynamic model and the measurement model are established.
(2) For aircrafts equipped with BDS/SINS integrated navigation system, BDS information can be used to achieve autonomous alignment. However, due to the complex polar environment, the BDS noise characteristics are abnormal. Therefore, the BDS noise characteristics in the polar regions are analyzed and heavy-tailed characteristics are simulated, respectively. (3) Since BDS measurement noise exhibits a non-Gaussian distribution in the complex polar environment, the estimation accuracy of standard Kalman filter is severely degraded. Kalman filter based on the Huber estimation is a minimum l 1 =l 2 mixed norm estimator, while minimizing the maximum asymptotic estimation variance under compound Gaussian distribution, the estimation efficiency of the l 2 norm under Gaussian noise is maintained. Therefore, the proposed algorithm suppresses the outliers significantly better than the one based on the standard Kalman filter and has better robustness under non-Gaussian noise.
The above analysis indicates that the polar alignment algorithm based on the Huber estimation filter is effective, which takes full account of the characteristics of BDS and ensures the accuracy and robustness of the polar alignment.

Conclusion
BDS measurement noise often exhibits a non-Gaussian distribution due to the complex polar environment and multipath effect. To improve the estimation accuracy, a new polar alignment algorithm based on the Huber estimation filter is proposed. The dynamic model and the measurement model of BDS/SINS integrated alignment system in the grid frame are derived. The BDS measurement noise characteristics in the polar regions are analyzed and heavy-tailed characteristics are simulated, respectively. Since the estimation accuracy of standard Kalman filter can be severely degraded under non-Gaussian noise, a Kalman filter based on the Huber estimation is designed. The simulation and experiment results demonstrate that the proposed algorithm has better robustness under non-Gaussian noise, and it is effective in the polar regions. By employing the proposed algorithm, the rapidity and accuracy of the alignment can be improved. This method can be easily extended to other GNSS.