基于抗差估计的快速动态寻北算法*

蒋庆仙，白云超，马小辉，王成宾

(西安测绘研究所地理信息工程国家重点实验室，西安710054)

As a device acquiring orientation information fast and accurately，gyro north seeker has been widely applied in fields such as vehicle positioning and navigation，ocean prospecting，oil drilling and tunnel cutting[1－3].With improvement of industrial mechanization and automation，requiremens of precision and efficiency in measurement engineering are increasing.Therefor，north seekers need to be more precise，efficient，reliable and capable of self-determination.

Many north seeking methods are adopted in gyro north seeker according to sampling and calculation schemes.A rotary modulation north seeking scheme can be used to calculate original azimuth of gyro sensitive axis by sampling outputs of the gyro on condition that IMU，consisting of a gyro and two accelerometers，continuously rotates around its vertical rotor in process of north seeking.Its outstanding characteristics are that constant drift and random drift of gyro are periodically modulated by continuous rotating gyro and eliminated by integrating gyro signal，thus precision of north seeking is obviously enhanced[4－6].

Although many data processing algorithms can be applied to gyro north seeker，distinct algorithms have remarkable differences in theory and calculation，and obtain different effects.These algorithms mainly determine precision of north seeking on given hardware.The least squares estimation is usually adopted in north seeking calculation.When observations don’t follow normal distribution or observations are contaminated by outliers，LS estimator is not robust at all and a single outlier can possibly spoil the solution[7－8].Random drifts of gyro are primary factors to degrade north seeking precision.Kalman Filtering can perfectly estimate and compensate gyro drift，nevertheless Kalman filtering results are divergent when functional model is not accurate or observational noise and functional modelnoise variance matrix can’treflect practical noises[9].Kalman Filtering is incapable of resisting observation outliers and intolerant to abrupt malfunction of sensors.Although wavelet threshold de-noising can eliminate white noise and broad band noise，it is incompetent to low frequency colored noise and pulse noise[10－11].Robust estimation deals with suspect observations by descending weights of observations.On the one hand，it limitedly utilizes usable data;on the other hand，it obviates bad data by zero weights so as to ensure efficiency of observations and robustness of parameter estimate.

Adopting the robust estimation principle，a mixed iterative algorithm based on high breakdown point initial value and assisted by IGGⅢscheme is constructed in this paper.The in run bais stability of dynamically tuned gyro adopted in the north seeker is less than 0.03°/h.The tests show that，precision of north seeker applying robust estimation excels 1.5'and that of applying least squares estimation is 3'in 4 minutes.The calculations indicate new algorithm can effectively control influence of observation outliers on precision of north seeker and ensure efficiency of remained observations when abnormal disturbances are eliminated.

1 The Principle of Rotary Modulation North Seeking

As showed Fig.1，a dynamically tuned gyro is vertically fixed to a turn platform and gyro sensitive axes parallel with the platform.The rotating angular velocity of motor is controlled by a steady-speed circuit and the platform rotates with constant velocity Ω.The initial azimuth ψ between geographic north and gyro sensitive axis can be calculated by sampling position signals of a photoelectric encoder and outputs of gyro.

Fig.1 the principle of north seeking

Assume heading，roll and pitch of carrier are ψ、θ and γ，respectively(the direction of counter-clockwise is positive).Reference frames are constructed as follow:(1)geography reference frame OXnYnZn，of which directions are east，north and up direction;(2)turn platform reference frame OXbYbZb，of which Xb axis points to the initial position of raster，Yb axis points to the direction perpendicular to the initial position of raster，Zb axis points to the circumrotate axis.(3)gyro reference frame OXmYmZm，of which Xm and Ym axis coincide with X and Y sensitive axis，respectively，Zm axis points to the circumrotate axis.

The transformation matrix between OXnYnZn and OXbYbZb is expressed as[12]

The components of the earth rotating velocity in OXnYnZn are

Where，geography latitude of the carrier is φ;earth rotating velocity is ωieand ωie=15.041 08°/h.

When IMU rotates with constant rate Ω together with the turn platform，current angle between X sensitive axis and initial position of photoelectric encoder is αi= Ωti.The transformation matrix from OXbYbZb to OXmYmZm is

The projections of earth rotating velocity on OXmYmZm are

The angular velocity of X sensitive axis on OXmYmZm is expressed as

The dynamic model of gyro can write as

Where，K is a scale factor and ε(t)is gyro drift.

When random drift of gyro is eliminated by filtering and constant drift ε0is only considered，the output of gyro is expressed as

The equation(7)is combined as:

Let parameter X=[x1x2x3]，the output of gyro is written as

The equation(9)is linearized with respect to X and the approximations of parameter are set X0=[K0cosψ0K0sinψ0ε0]T.

The result can be obtained as follow[13]

The vectors of equation(10)write as

The estimate is expressed as

The estimates of ψ，K and ε0are obtained as follow:

The quadrant of gyro sensitive axis is determined according to signs of.The initial azimuth between geographic north and the gyro sensitive axis can be solely calculated.

The pitch angles of gyro sensitive axes are detected by two accelerometers and sensitive axes of accelerometers strictly parallel to sensitive axes of gyro.When the gyro along with the turn platform rotates with constant rate Ω，outputs of accelerometers are isochronously sampled.The components of the acceleration of gravity on OXbYbZb are expressed as[12]

Assume outputs of two accelerometers areand，respectively.After from OXmYmZm transforming to OXbYbZb，outputs on Xb and Yb are

The attitude angles(θ、γ)are

2 The Dynamic North-Seeking Formulas Based on Robust Estimation

Robust estimation can avoid the estimate of unknown parameter being affected by outlier and obtain an optimal estimate by adopting suitable estimation method on condition that outliers can’t be ignored.In order to assess robust ability，high breakdown point is introduced in this paper[7－8].The high breakdown point is a measure of whole robustness and is the smallest ratio of outliers completely making the estimate in control.That is to say，the high breakdown point demarcates a distance of model deflection and contaminated distribution exceeding the distance will destroy parameter estimate.

A setofindependentobservations{Li}(i=1，2，…，n)exist in the rotary modulation north seeking scheme.The weight matrix of observations is P=diag[p1，p2，…，pn]and the estimated vector of unknown parameter is.

The corresponding error equation is given by

Where，A denotes a n × 3 observation matrix，see equation(11);V is a n×1 residual vector.

When outliers exist in observations，equivalent weight function is introduced in calculations and the robust estimate of parameter X reads:

The variance-covariance matrix ofis

Robustness of robust estimation mostly depends on equivalent weight function.The key problem is to look for suitable equivalent weight functions to ensure robustness and efficiency of parameter estimate[14－15].Weight functions should be composed of two aspects:(1)dividing observations;(2)evaluating observations by alterative weight.Various equivalent weight functions are chosen in calculations，such as Denmark weight function，IGGⅢ weight function and Turkey weight functionandsoon.isadiagonalif observations are mutually independent.IGGⅢ weight function in common use is[7－8]:

Where，piis the primal weight of observation;is standard residual ofandis the estimate of standard deviation calculated by taking the median of residuals of the first epoch robust estimation[14].k0and k1are critical values of IGGⅢweight function，usually being chosen as k0∈[1.0～1.5]and k1∈[2.5 ～3.0]，respectively.

The solution of equation(12)is currently calculated by using iteration procedures.If initial value and equivalent weight function are reasonably chosen，the reliable estimate of parameter can be obtained by finite iterative calculations.On precondition of eliminating white noise，period noise and high frequency noise，the median of observations is served as iterative initial value so as to ensure initial value with 50%breakdown point.In other words，credible initial value can still be obtained even if 50%observations are contaminated by outliers[16－17].

Two models are adopted to evaluate precision of gyro north seeker.The mean square error of azimuth is

The root mean square(RMS)of azimuth is

3 North Seeking Calculations and Analysis

The computation example comes from a dynamically tuned gyro north seeker and in run bias stability of the dynamically tuned gyro is less than 0.03°/h.The rotating velocity of the turn platform is set as 5 r/min.The relative stability of rotating velocity that is controlled by a precise steady-speed circuit attains 10－4and position precision excels 1″.The highspeed data collection and calculation unit is made up of DSP and FPGA.Outputs of X sensitive axis and Y sensitive axis of gyro are respectively collected and sampling frequency is 25 kHz.Because frequency of gyro signal is lower than 30 Hz，Butterworth filter is selected to compose low pass filtering circuit in order not to attenuate low frequency signal.In ideal case，output of dynamically tuned gyro in rotary modulation north seeker is a standard sinusoid.The low pass filtered output is given in Fig.2.

Fig.2 output of gyro

Wavelet threshold de-noising isn’t applied to data processing because it is incompetent to low frequency noise and its calculations are very large.The reliable Kalman filtering needs reliable functional model，stochastic modeland reasonable estimation method.Because accurate functional model is difficultly constructed，Kalman filtering is also not adopted in calculations.Three schemes are carried out as follows，of which results are listed in Table 1.Weighs of observations reflect contribution of each calculated azimuth to the estimate of azimuth.The condition of iteration convergence is.The reason adopting mean square error of azimuths to evaluate the precision of gyro north seeker is that actual value of measured azimuth hasn’t been attained.

Scheme 1 Least squares estimation

Scheme 2 Robust estimation taking the mean of entire observations as starting value

Scheme 3 Robust estimation taking the median of entire observations as starting value

Table 1 The Results of North Seeking(°)

From the testcomputation，the following conclusions can be stated.

(1)Least squares estimation is not robust and large disturbances have significant effect on the LS estimate.In the computation example，calculation results of the first and the second epoch deviate from the azimuth.They result in a small estimate and a big least squares error because their weighs are individually 0.733 2 and 0.538 5.

(2)Schemes of 2 and 3 have robustness to degrade effect of abnormal disturbances on parameter estimate.In the computation example，although calculation results of the first and the second epoch deviate from the azimuth，they have little contribution to azimuth estimate because their weighs are individually 0.007 8 and 0.007 9 by adopting robust estimation to degrade their weigh.

(3)The robust initial value makes a great impact on parameter estimate.The estimate is credible and precise if the initial value is credible;vice versa.The median initial value is more credible than LS initial value.When the sample is not too large，scheme 3 serving the median of observations as initial value acquires a more credible estimate than scheme 2.

(4)The standard deviation factor directly relates to outliers identification and affects parameter estimate.The estimate of standard deviation factor calculated by taking the median of residuals of the first epoch robust estimation is reliable and the azimuth estimate is reliable in the computation example.

4 Conclusions

The least square estimation is usually adopted to calculate geography azimuth of gyro sensitive axis in gyro north seeker.The least squares estimate won’t be extremely reliable if observations are contaminated by outliers.Robust estimation can not only degrade the effect of abnormal disturbances on parameter estimate，but also ensure efficiency of remained observations when abnormal observations are eliminated.The mixed iterative algorithm based on high breakdown point initial value and assisted by IGGⅢscheme can effectively degrade abnormal disturbances.The tests show that，adopting the dynamically tuned gyro of 0.03°/h，the precision of north seeker attains 1.5'in 4 minutes and the performance of north seeker is extremely improved.

[1]Peng Zhihong，Zhang Le，Wang Hui.Brief Introduction to North Seeking Technologies in-Country and Abroad[J].Navigation and Control，2003，2(2):77－78.

[2]Han S，Wang J.A Novel Method to Integrate IMU and Magnetometers in Attitude and Heading Reference Systems[J].The Journal of Navigation，2011，64，727－738.

[3]Sung-Jin Kim，Sang-Sik Lee，Yong-Soo Kwon.Dynamic North-Finding Scheme Based on a Fiber Optic Gyro[J].Proceeding of SPIE，1997，3087:126－136.

[4]ZOU Xiangyang，SUN Qian，CHEN Jiabin.A North-Finder Algorithm and Its Signal Processing of Continuous Rotary North-Finder[J].Transactions of Beijing Institute of Technology，2004，24(9):804－807.

[5]XU Jianhua，XIE Ling，GAO Yanan.Study of Filter Used in Revolution Modulation North Finder[J].Transactions of Beijing Institute of Technology，2005，25(8):718－721.

[6]ZHANG Sijiang，QIN Shiqiao，WANG Xingshu.A North-Finder Based on Continuous Rotary Laser Gyro[J].Aero Weaponry，2006，1:12－15.

[7]YANG Y.Robust Estimation of Geodetic Datum Transformation[J].Journal of Geodesy，1999，73(5):268－269，271.

[8]YANG Y.Robust Estimation of Systematic Errors of Satellite Laser Range[J].Journal of Geodesy，1999，73(7):345－349.

[9]ZHOU Yueqing，GAO Ning，LIU Luyuan.Fault-Tolerant Adaptive Kalman Filter and its Application in Gyro Signal Processing[J].Journal of Tianjin University，2004，37(9):815－817.

[10]WANG Shulin.Application of Wavelet Transform in Signal Denoise[J].Transaction of Missile，Control and Guide，2006，26(4):294－296.

[11]HUO Ju，WANG Shijing，YANG Ming.Noise Processing of FOG Signal based on Wavelet Threshold-Value[J].Journal of Chinese Inertial Technology，2008，16(3):343－347.

[12]JIANG Qingxian，MA Xiaohui，CHEN Xiaobi，et al.North Determining Scheme by Two-Position for a FOG North Seeker[J].Journal of Chinese Inertial Technology，2006，14(3):1－5.

[13]Fei Yetai.Error Theory and Data Processing[M].Beijing，China:Mechanics Industry Publishing House，2005.

[14]YANG Yuanxi，WU Fumei.Modified Equivalent Weight Function with Variable Criterion for Robust Estimation[J].Journal of Zhengzhou Institute of Surveying and Mapping，2006，23(5):317－320.

[15]CHEN Ke，GUI Qingmin，LIU Li，et al.Robust t-Type Estimation in Gauss-Markov Model[J].Acta Geodaetica et Cartographica Sinica，2008，37(3):280－285.

[16]WANG Zhenjie，OU Jikun，QU Guoqing.Choice of Initial Value for Robust Estimation[J].Crustal Deformation and Earthquake，2001，21(3):32－35.

[17]WANG Chengbin，JIANG Qingxian，WU Fumei，et al.Method to Dispose the FOG Signals in the Two-Position North Seeker[J].Geomatic Science and Engineering，2008，28(4):43－47.