Continuous rotation north-finding algorithm based on constrained adaptive Kalman filter

GUO Fengzhi，LI Xingfei，LI Jingxian

(State Key Laboratory of Precision Measurement Technology and Instruments，Tianjin University，Tianjin 300072，China)

Abstract：As an orientation measurement system,north-finder has been playing a significant role in both military and civilian fields of orientation and control.In this paper,to deal with drawbacks in the conventional north-finding systems,a dynamic strategy based on continuous rotation modulation to measure the rotational angular velocity of the earth is proposed.By modeling the dynamic error,optimizing the process constraint and estimating dynamic noise,a method combining delay compensation and hardware adjustment,and a constrained adaptive Kalman filter (CAKF)algorithm are designed for the north-finding strategy.According to simulation and experiments,the proposed algorithm can achieve the high-precision north-finding with robust and anti-noise performance.

Key words：north-finding；constrained adaptive Kalman filter (CAKF)；continuous rotation modulation；dynamic error；tilted base

0 Introduction

The north-finding technique has been widely applied to both military areas such as missile launch and civilian areas such as mining,mapping,navigation and astronomy.The main north-finding principles based on the Earth’s magnetic field,the global navigation satellite system (GNSS),the celestial body detection and the gyrocompass commonly have crucial disadvantages.The magnetic compass may be interfered by local magnetic field anomalies,the GNSS depends on the magnitude of the signals from several satellites at the receiver location,and the celestial body detection needs access to the clear open sky,they can be greatly affected when the application environment becomes degraded or is just indoor scene.Moreover,it takes a long time for the gyrocompass to find north[1-3].In contrast,the north-finding method in which the gyro is utilized to measure the rotational angular velocity of the earth is not restricted by the application environment and can achieve high-precision azimuth in a relatively short period.

According to the north-finding process,there are two main strategies,the stationary multi-position method and the dynamic continuous rotation method.The former requires the sensor to target multiple directions but stay stationary while measuring at each direction for a period of time,which commonly includes two-position,three-position,four-position and multi-position gyro north-finding methods[4-5].In contrast,the latter requires the measurement system to rotate continuously at the same speed.

Both north-finding strategies involve sensor error,turntable error and installation error[6-9].Li et al.[10]adequately analyzed the propagation of the gyroscope error including the bias,random walk and bias instability based on Allan variance.Jun et al.[4]built the gyro misalignment and the shaft misalignment model to calculate the installation error and turntable error.Sun et al.[11]used translocation to eliminate constant drift and scale factor error of the fiber-optic gyroscope.To decrease the azimuth error,filtering algorithms such as the least-square approximation method,Kalman filter (KF),particle filter (PF),lock-in amplifier technology and empirical mode decomposition (EMD)have been applied[12-15].Yu et al.[16]utilized the accurate measurement method to add the linear equation constraint into the conventional KF algorithm to decrease the concerned error.Zhang et al.[14]utilized the weight function obtained by the threshold and the measurement standard deviation in the filtering algorithms to achieve the robust result.Xing et al.[15]utilized optimal weighted fusion of multiple accelerometers to decrease the error of the tilt angles which may also affect the accuracy of azimuth.

For the stationary north-finding method,its system is relatively simple and low-cost,which only needs a manual turntable for changing the measurement angle,but requires calibration to eliminate the effects of error such as 1/fnoise and temporary drift[13].Compared with the stationary method,the continuous rotation method not only utilizes the rotation modulation to eliminate the static noise such as 1/fnoise,rate random walk (RRW)and additive bias,but also automatically finds north in a shorter time[17-18].Researchers have done a lot of work.Prikhodko et al.[13]analyzed the relationship between the magnitude of the error propagation and the azimuth.Zhang et al.[19]analyzed the influence of varied rotation speed and sampling frequency.

However,although the abovementioned static error can be eliminated by carousel,continuous rotation also results in the dynamic error mainly caused by the comprehensive rotational interference which may greatly affect the accuracy and repeatability of the north-finding result,while it has not been analyzed adequately.Even for the precise turntable,the velocity instability always exists and its value is larger as the velocity decreases,which can directly affect the sensors on the turntable by adding unbiased and even biased noise.Wang et al.[12]analyzed part of the impact of the velocity instability,but not entirely,because it may cause error based on some integrated effect.Apart from the noise caused by the turntable,the measurement,phase and transmission delays also lead to a crucial problem for the synchronous measurement of multiple sensors.In order to decrease the dynamic error,the proper structural design and delay compensation should be developed.

Compared with other algorithms,KF is the minimum-variance linear state estimator for linear dynamic systems with Gaussian or non-Gaussian process and measurement noise,which provides a real-time method for the estimation of azimuth.Nevertheless,its performance is greatly sensitive to the accuracy of the filter parameters,so the appropriate parameters must be given to keep the north-finding system robust enough.For the north-finding model based on the Kalman filter,the system noise is close to zero,because when the north-finding system works in the normal condition,the azimuth hardly changes.However,the measurement noise may be varied and its relative parameter is usually set at a constant which is acquired by repeatedly testing for a specific system.When the control and measurement mechanisms of the north-finding system or application condition change,the former parameter relative to the measurement noise does not meet the new measurement requirement.Especially,when the noise is dynamic and varying,the constant parameter may not make the KF work normally.Besides,with no constraint between the state estimations,the error probably occurs when the noise increases,which means degradation in performance.

In this paper,a novel dynamic north-finding algorithm based on constrained adaptive Kalman filter (CAKF)is proposed to prevent accuracy and repeatability from being affected by the dynamic noise.This algorithm combines the constrained KF to guarantee the validity of the estimation and the adaptive KF based on Allan variance to estimate the real-time measurement noise.In the special circumstance in which the measurement noise keeps constant,the CAKF is simplified as constrained KF.In the following passages,the North is defined as the geographic North pole,which is the point where the Earth’s axis of rotation intersects its surface in the Northern Hemisphere,instead of the geomagnetic North pole.

The remainder of this paper is as follows:Section 1 introduces the method of the north-finding model on the tilted base and analyzes dynamic error.Section 2 derives the constrained adaptive KF algorithm.Section 3 discusses the performance of different KF algorithms by simulation.Section 4 discusses the results of the experiments.Section 5 draws a conclusion.

1 North-finding model

The north-finding system includes one gyro,one accelerometer and a uniaxial turntable with the slip ring and the grating ruler.In the measurement frame or m-frame,the sensitive axes of the accelerometer and the gyro and the rotation axis are relatively on thex-axis,y-axis andz-axis.The coordinate frame of the north-finding system constituted by the gyro,the accelerometer and the turntable is named as the body frame or b-frame.The navigation frame or n-frame is denoted as the local East-North-Up (ENU)Cartesian coordinate system.The aim of north-finding is equivalent to finding the transformation relation between the m-frame and the n-frame.

The north-finding system is commonly placed on the horizontal plane,but that is not necessary,and the horizontal condition can hardly be guaranteed in the real application environment.In this way,the north-finding model is built on the tilted base.

1.1 Tilted base

In the tilted condition,the accelerometer is used to calculate the tilt angles.In order to derive the output function of the gyro and the accelerometer,the rotational angular velocity of the earth and the gravitational acceleration are projected into the sensitive direction of sensor by the direction cosine matrix (DCM).The azimuthφ,that the deflection angle from the heading direction to the true north with respect to the horizon,can be expressed as

(1)

In consideration of the tilted base of the system,two angles of pitchθand rollγare introduced to separately define the tilt angles as follows

(2)

(3)

The rotation matrix from the n-frame to the b-frame is

(4)

The turntable drives the measurement system to rotate,and the rotation angleα,that represents the transformation from the b-frame to the m-frame,is defined as

(5)

Thus,the DCM from the n-frame to the m-frame is expressed as

(6)

Furthermore,in order to calculate the heading angle,it is necessary to establish the relationship between the output of sensors and the angular velocity vector and the gravity vector of the earth rotation.The sensitive axes of the gyro and the accelerometer are respectively on they-axis andx-axis in the m-frame.The rotation vector of the earth in the n-frame is defined as

(7)

whereLis the latitude.The three-axis projection of the rotational angular velocity of the earth in the m-frame is calculated as

(8)

The direction vector of the gravitational acceleration in the n-frame is defined as

(9)

The three-axis projection of the gravitational acceleration in the m-frame is calculated as

(10)

cosL(cosφ(cosαcosθ-sinαsinγsinθ)-cosγsinαsinφ).

(11)

The measurement function can be defined as

Zg=HgXg,

(12)

where

(13)

(14)

(15)

andqgrepresents the measurement bias of the gyro.Assuming that the sensitive direction of the accelerometer is going along the positive direction of thex-axis in the m-frame,the projection offmon thex-axis is expressed as

fmx=-g(sinαsinθ-cosαcosθsinγ).

(16)

After normalization of Eq.(16),the measurement function can be defined as

Za=HaXa,

(17)

where

(18)

Ha=[sinαcosα1],

(19)

Za=[fmx],

(20)

andqais the measurement bias of the accelerometer.

1.2 System error

1.2.1 Data alignment and delay elimination

Compared with the stationary north-finding method,the continuous rotation method has strict requirements on the measurement delay,the phase delay and the transmission delay of the data of sensors including the gyro,the accelerometer and the grating ruler.The measurement delay is the difference in measurement time between different sensors;the phase delay is the time delay of the phase relative to the amplitude envelope;the transmission delay is the time delay from the sensor output to the processor input.With such delays,the data from different sensors may not be aligned correctly in the timeline.To solve this problem,adequate steps should be taken.First,all sensors should be sampled synchronously to prevent varying and unknown measurement delays.Second,the transmission delay needs to be compensated and the phase delay needs to be calibrated according to the frequency,because the phase delay may vary with the frequency of continuous rotation modulation.When the sampling synchronization is guaranteed,considering the error caused by the phase delay and transmission delay,Eqs.(14),(15)and (19)are separately redefined as

(21)

sin(α+Δ1)cosθsinγ)],

(22)

Ha=[sin(α+Δ2) cos(α+Δ2) 1],

(23)

whereΔ1andΔ2are the sum of the phase and transmission delay of the gyro and the accelerometer relative to the grating ruler.When the tilt angles are close to zero,error caused by the phase delay of the accelerometer can be ignored,but the phase difference between the gyro and the grating ruler is transmitted to the heading error in equal proportion.To solve the problem due to the difference of phase delay between the gyro and the grating ruler,the calibrated phase delay needs to be acquired and the original Eqs.(14)and (15)are replaced by Eqs.(21)and (22)when calculating the azimuth.

1.2.2 Angular velocity instability

Even for the precise turntable,the rotation velocity can not keep the same as the input order because of velocity fluctuation which may be affected by the mechanical structure,control algorithm,etc.Usually,the fluctuation frequency is greatly related to the rotation velocity and the portion with the same frequency cannot be entirely eliminated even after band-pass filtering.As a result,the rotation velocity with the velocity fluctuation can be defined as

ωturn(t)=ωin+ωf(t)=ωin+ωf0sin(2πft+ζ0),

(24)

whereωinis the input velocity andζ0is the phase of velocity fluctuation.Considering the angle of the sensitive axis of the gyro and the rotation axis of the turntable can not be perfectly perpendicular,the velocity fluctuation will be coupled onto the input of the gyro,which is expressed as

(25)

where

k1=cosξ,k2=sinξ,

(26)

ξis the angle between the ideal sensitive axis and the real sensitive axis.Besides,according to the angle transformation (AT),the Eq.(11)can be denoted as

(27)

whereα0andα(t)represent the new amplitude and the angle after the AT.

According to Eqs.(24)and (27),the Eq.(25)can be denoted as

ωg(t)=k1Aysin(2πft+α0)+k2ωf0sin(2πft+ζ0)+

k2ωin=Agsin(2πft+σ)+k2ωin,

(28)

whereσis the new phase after the AT.Considering the tilt angles are usually small,which means the absolute value ofθandγare both less than 10°,theAyis close to 1 andα0can be approximately considered as the azimuth.So when theξis much less than 90° and theωf0regarded as the amplitude of the velocity fluctuation is large,the calculated azimuth will be significantly affected by the velocity instability and change fromα0toσ.Similarly,when the tilt angles are large,the accuracy of azimuth is also greatly affected by the velocity fluctuation.Therefore,besides selecting a well-performed turntable,the sensitive axis of the gyro must be adjusted to the ideal orthogonal direction to avoid the interference of the velocity instability.To show the coupled condition,the coupled angle between the axes of the gyro and the turntable can be measured by swinging the turntable.

2 KF algorithm

KF is a method for real-time estimation of the azimuth.Although some researchers have proposed some effective KF algorithms to decrease noise and extract the targeted signal from the outputs of the gyro and the accelerometer,the constraints of the state variables may not be considered and the measurement noise is only regarded as a constant which needs to be modified manually.Given this,a novel KF algorithm,CAKF that combines the adaptive Kalman filter (AKF)and the constrained Kalman filter (CKF)has been developed.

2.1 Conventional KF

The conventional form of the KF is given below.The time update equation is defined as

(29)

(30)

whereKis the gain matrix;His the measurement coefficient matrix;Ris the measurement noise matrix;zis the measurement vector of the system;Iis the unit matrix.

2.2 CKF

The KF aims to minimize the variance of the estimated error.Whenx0,the process noiseωkand the measurement noisevkare Gaussian,the KF estimate can be given by

(31)

(32)

such that

(33)

(34)

(35)

which is a 1×1 vector,andx(i)denotes theith element of the vectorx.

(36)

Thus,the first-order expansion of the linear equation constraint can be written as

(37)

Note that the nonlinear equality constraint is transformed into a quadratic programming problem expressed as

(38)

where

(39)

(40)

(41)

(42)

where

(43)

2.3 AKF

The performance of KF greatly depends on the process noise covariancesQkand the measurement noise covariancesRk.As mentioned above,theQkis close to zero and can be set as a very small constant.ForRk,although the adapted value can be acquired by manually testing repeatedly for a specific system,it may dramatically change when the application conditions and environments change.To ensure the robust performance,the AKF based on Allan variance was introduced[22].

(44)

(45)

where

(46)

2.4 CAKF

By the integration of the aforementioned algorithms,the CAKF has been developed.The new time update equation is defined as

(47)

The new measurement update step is defined as

(48)

3 Simulation

To manifest the performance of the new KF algorithm,the simulation data of the gyro,the accelerometer and the grating ruler are generated.The Gaussian white noise is also added to those data.

3.1 Unconstrained and constrained estimation

Concerning the geographical information,the azimuth is set as 30° and the latitude is 40° N.As for the north-finding system,sensors are sampled synchronously as the turntable rotates every 0.1°.The Gaussian white noise is added to the data as interference.The filtering results of the unconstrained KF are shown in Figs.1 and 2.

(a)KF with no noise

(a)KF with white Gaussian noise added

To eliminate the error,the CKF is applied and its filtering result is shown in Fig.3.As Fig.3 shown,the curve becomes much smoother,the constraint is satisfied,and the azimuth error is significantly reduced by utilizing the nonlinear constrained KF.

(a)Nonlinear constrained KF with white Gaussian noise

Although in Figs.4(a)and 4(b),the constraint is not satisfied in the beginning,after successive iterations of linear constrained KF,it is satisfied and the result is not affected.Besides,the computation load is greatly reduced by linearization.

(a)Linear constrained KF with white Gaussian noise

3.2 Effect of noise

When noise is kept at the same level,the CKF may have a good performance with the adequateRkas shown in Figs.3 and 4.However,with noise varying,the outlier may come out with stronger intensity and higher probability which greatly contaminate the desired signal.In order to prevent the effect of the varying noise,the AKF is introduced.By estimating the measurement noise based on the Allan variance,the filter can automatically adjust the gain matrix to control the convergence speed and avoid divergence.

To simulate the condition in which the noise is changing,a segmented Gaussian white noise is added into the output of the gyro.The amplitude of the input noise and detected noise calculated based on the Allan variance adaptive method is illustrated in Fig.5.

Fig.5 Logarithmic amplitude of input noise and estimated noise based on Allan variance adaptive method

The ratio of the noise amplitude is 1∶10∶2.The azimuth and its error of the CKF and CAKF are separately shown in Figs.6 and 7.

Fig.6 Azimuth of CAKF,CKF (Rk=1)and CKF (Rk=10)

Fig.7 Error of azimuth of CAKF,CKF (Rk=1)and CKF (Rk=10)

In this condition,the CKF needs a largerRkto converge more quickly and a smallerRkto get a more accurate azimuth.

WhenRkis large,the filter can become more robust but comes at the cost of speed,whenRkis small,the convergence speed increases with the loss of accuracy.Even worse,when theRkis much smaller than the trueRk,the filter may diverge,shown as the broken curve and gray curve (which is partly covered by the broken curve)from 480° to 960° in Fig.7.In contrast,CAKF can change theRkaccording to the Allan variance to make full use of the data.Based on the Monte Carlo method,the performances of CKF and CAKF according to 50 simulations are shown in Table 1.

Table 1 50 simulation error

In the 50 simulations,Rkof the CKF is separately set as 100,10,1,and 0.1 to test whetherRkcan set as a constant value to decrease the estimation error of azimuth,but all attempts fail to improve performance unlessRkis also changed manually into accurate value according to the noise.In contrast,both the nonlinear and linear CAKF are quite robust to calculate the estimation of azimuth with less mean error and standard deviation.Moreover,the noise amplitude may change more dynamically in more complex conditions,and the outputs of different algorithms are compared in the experiment section.

4 Experiments

In the experiment,the fiber-optic gyroscope (FOG)and the quartz accelerometer are utilized.The synchronous sampling is achieved by using the external trigger signal to eliminate the measurement delay.The latitude is 39.43° N.

4.1 Phase measurement

To gain the phase and transmission delay between the FOG and the grating ruler,the FOG is put on the turntable and the sensitive axis is parallel to the axis of the turntable and vertical downwards.The turntable oscillates at the sinusoidal angular velocity with frequency changing from small to large gradually,and the FOG and the grating ruler are simultaneously sampled.By comparing the phase of two signals,the angular velocity measured by the FOG and the grating ruler,the difference of the phase and transmission delay between the FOG and the grating ruler with varying measurement frequency is illustrated in Fig.8.

Fig.8 Phase difference between grating ruler and FOG

When the rotation speed is lower than 360 °/s or 1 Hz,the phase difference is less than 0.04°.To achieve high-precision azimuth,the error caused by the phase difference must be compensated after calculating the azimuth.

4.2 Rate instability analysis

To reduce the effect of the velocity instability,the sensitive axis of the FOG should be as perpendicular to the rotation axis as possible.Although the clamp has been precisely machined,the component of the sensitive axis of the FOG along the rotation axis still needs to be measured to analyze the effect of the rate instability.Fig.9 shows the output of the FOG when the turntable is oscillating.

Fig.9 Angular velocity of turntable input and angular velocity measured by FOG

The tilt angle of the FOG is 2.16′ calculated according to Fig.9.When the rotation speed is 36 °/s and the rate stability is 1×10-5,the azimuth error is less than 0.2″.As a consequence,in this experiment,rate instability makes little difference in the north-finding result.

4.3 North-finding

In this section,four algorithms,the KF,the CKF,the AKF and the CAKF are utilized to seek north.According to the different test conditions,the stable condition and the dynamic noise condition,the estimation results of four algorithms are separately contrasted in the following two sections.

4.3.1 Stable condition

The north-finding algorithm consists of two main parts,tilt estimation and azimuth estimation.

The rotation rate is set at 36 °/s in this section to compare four KF algorithms.The error of experiments is shown in Table 2.Meanwhile,one typical filtering result is shown in Fig.10 for analysis.

Table 2 North-finding error in stable condition

(a)KF

In Fig.10,the convergence curves clearly show the performance of the different algorithms.In the first few seconds,thex(1)andx(2)are out of the range of [-1,1],so their relative angles are set as 90° and 0° separately to represent the condition instead of the true value of angles.For the KF or CKF,Rkis set at the appropriate value.For the original KF and AKF,the estimated azimuth angles,which are calculated based onx(1)andx(2),respectively,are close but different due to no constraint set,which causes the estimation error.

For the CKF and CAKF,the estimated azimuth angles do not have this problem because of the constraint of Eq.(35).Constrained estimation significantly has less error than unconstrained estimation.In this stable condition,without dynamic noise,the estimation results of CKF and CAKF are close to each other and better than the results of KF and AKF,which is also verified in Table 2.

Although the CKF seems to work well enough and even a little better than the CAKF in this stable condition and the KF has similar circumstance relative to the AKF,we must realize that the measurement noise is nearly constant andRkof the CKF or KF has been set as a quite accurate value which is close to theRkestimated by the Eq.(45).That means when the measurement noise hardly changes and the exact value ofRkis known,the CAKF or KF can be degraded to the CKF or KF and the CKF or KF is accurate enough in this circumstance.When the measurement noise changes dynamically,however,the performance of CKF or KF can be greatly impaired while the CAKF or AKF is still robust enough,which is shown in the following section.

4.3.2 Dynamic noise effect

Although in the stable condition,the conventional KF can still perform well when the parameter is set as a relative accuracy value,the measurement noise matrixRkis usually not a priori information to a new north-finding system,and sometimes may change because of the comprehensive application environments.The performance of the KF or CKF may be greatly restricted in the tough and dynamic environments.In the following experiments,the data from sensors are contaminated by extra unbiased noise to compare the performance between different algorithms.

Fig.11 shows the filtering results of different algorithms when the signal of the FOG is greatly affected by constantly changing noise.No matter how to adjust the parameters in the filter,the KF and CKF can not get a satisfying result.That is because a constantRkcan make the filter work well only in a part of the time and badly during others when measurement noise is changing.To deal with the constantly changing noise,theRkpresented in Fig.12 is calculated by the method of the MAA.The noise seems to increase and decrease periodically,which leads to a more complicated curve ofRkcompared with that in the simulation section.

(a)KF

Fig.12 Logarithm of Rk calculated by the MAA

Table 3 shows the azimuth results based on four KF algorithms.The AKF performs better than the KF algorithm because of the accurate estimation forRk,while ignoring the constraint betweenx(1)andx(2)leads to extra error.Similarly,the CKF takes the latter into consideration but utilizes the inappropriate constant value ofRk,which decreases the performance of the filter.At last,with the advantages of both CKF and AKF,the CAKF has the best performance on the accuracy in the four algorithms when the noise changes dramatically.

Table 3 North-finding error with dynamic noise

5 Conclusions

In this paper,the north-finding strategy based on continuous rotation modulation to measure the rotational angular velocity of the earth is proposed.To get the output functions of the gyro and the accelerometer concerning,the rotational angular velocity of the earth and the gravity,respectively,the basic north-finding model on the tilted base is built up.The dynamic error including the measurement,phase and transmission delay,and turntable rate instability are analyzed,and the effective method of delay compensation and hardware amendment are provided to decrease the dynamic error.Besides,to deal with the effect of the dynamically varying noise,the AKF based on the fading memory method of the Allan variance is derived.And the CKF is applied to calculate the accuracy constrained posterior estimate from the unconstrained posterior estimate.To decrease the computation load,the nonlinear constraint can also be substituted by the linear constraint which can keep the performance of constrained estimation.Combining both algorithms,the CAKF is derived.According to the simulation and experiments,the performances of the four algorithms are compared.Noise can greatly affect the performance of the conventional KF because of failing to meet the constrained requirement but the performance degradation can be avoided by the CKF.When noise is constantly varying,the AKF can efficiently utilize new data from sensors and prevent results from diverging.All the results manifest that the CAKF has robust and anti-noise performance for achieving the high-precision north-finding.