123 navigation

Track 1: AUTOMATIC CONTROL

International Symposium on Electrical & Electronics Engineering 2007 - Oct 24, 25 2007 - HCM City, Vietnam

-1-

FEEDFORWARD STRUCTURE OF KALMAN FILTERS FOR LOW

COST NAVIGATION

T. D. Tana, L.M.Ha

a, N. T. Long

a, H.H.Tue

a, c, N. P. Thuy

a, b

a Faculty of Electronics and Telecommunication, College of Technology, VNU, Hanoi

b International Training Institute for Materials Science (ITIMS), HUT, Hanoi c Laval University, Canada

ABSTRACT

Nowadays, navigation and guidance are very important problems for marine, aeronautics and space

technology. In such systems, Inertial Measurement Units (IMUs) are widely used as the core of the

Inertial Navigation Systems. However, there are existing errors in the accelerometer and gyroscope

signals that cause unacceptable drifts. To eliminate the deterministic errors, we can specify them

quantitatively by calibrating the device. It is, however, more complex in determination of the

stochastic errors. Thus, an optimal filter such as Kalman one is often used to minimize these errors on

the INS system. In this paper, we present a feed forward Kalman Filter (KF) of which the noisy INS

output is combined with the noisy GPS output in improving the quality of the navigation and guidance

systems.

1. INTRODUCTION

The demand of navigation and guidance has

been urgent for many years [1]. In fact, GPS

have been employed widely in many

applications while INS is daily used in

controlling flight dynamics. In principle, an

IMU consists of gyroscopes and accelerometers

that measure angular velocities and accelerations

in three dimensions. Recently, thank to the

development of MEMS technology, the IMUs

become smaller, cheaper and more precise.

However, there are still problems with MEMS

based the IMUs which are necessary to be solved. The position error of an INS increases

rapidly with navigation due to the integration of

measurement errors in the gyroscopes and

accelerometers. In order to make the corrections,

there appears a new trend in navigation and

guidance domain [2]: it consists of the

integration of INS and GPS altogether.

Integrating these two methods can improve the

performance of the system and reduce

concurrently the disadvantages of both INS and

GPS.

2. INS AND GPS

2.1. Inertial Navigation System (INS)

An INS often consists of three accelerometers

and three gyroscopes in order to measure the

accelerations in three dimensions and the

rotation rates around three axes [3]. There are

two typical INS systems: gimble system and

strapdown system. The strapdown INS system

has been used more popular than gimble system.

The strapdown system is mostly based on the

MEMS technology that is relatively inexpensive

and compact. In strapdown system,

accelerometers and gyroscopes are fixed to body

frame of the aircraft. Signals from these sensors

are processed in order to obtain three Euler angles. The results are corrected by gravity

acceleration and Earth rotation velocity.

Three accelerations (ax, ay and az) along

three axes of body frame relate to three

velocities (U, V and W) in the Earth fixed frame

by following equation:

φθ

φθ

θ

.cosg.cos-W.p.

.sing.cos-W.p.

g.sinW.q.

.

.

.

+−=

+−=

+−+=

qUaW

rUaV

rVaU

z

y

x

(2)

Where p, q, and r are velocities of roll, pitch,

and yaw, respectively.



-2-

To convert the movement from body frame to

navigation frame (see Fig. 1), we could use

Direct Cosine Matrix (DCM):

=

=

W

V

U

.

.

.

T

D

E

N

DCM

V

V

V

Z

Y

X

(3)

Then, we can obtain the latitude, longitude and

height of the aircraft by following equations:

D

earth

E

earth

N VHR

V

R

V−=== ��

λµλ

cos (4)

Figure 1. Navigation frame

There are many kinds of errors in the INS. Most

of the errors in the INS are caused by sensor

imperfections that are necessary to be solved.

The position error of an INS increases rapidly

with navigation due to the integration of

measurement errors in the gyroscopes and

accelerometers. In order to make the corrections,

the errors are often classified into deterministic

errors and stochastic errors. We have succeeded

in specifying the parameters of the IMU errors,

which is a necessary step when applying error-processing algorithms for the INS. A discrete

INS algorithm was developed in our lab in order

to develop a completed Inertial Measurement

Unit (IMU) (see Fig. 2). The detail of this

discrete algorithm will not be presented in this

paper.

Figure 2. The discrete INS algorithm

2.2. Global Positioning System (GPS)

The GPS consists of 24 satellites that fly above

the surface of the Earth at the height 19.200 km

in order to acquire the position of the aircraft

(latitude, longitude and height) [4]. Radio

signals often hardly transmit over solid

buildings, tunnels…To get the correct position

of the aircraft; it requires at least four satellites.

They can separate the GPS into three segments

(see Fig. 3):

• Space segment: consists of 24 satellites.

• Control segment: ground control

• User segment: receiver

Figure 3. Structure of the GPS

Errors in the GPS mostly caused by six

following factors (not including Selective

Availability error):

• Ephemeris data

• Satellite clock

• Multipath reflection

• Atmospheric delay

• Random measurement noise

• Receiver (include software)



-3-

3. INTEGRATION OF INS AND GPS

USING KALMAN FILTERING

The INS system has two main advantages when comparing with other navigation system: self-

contained ability and high accuracy for short-

term navigation. In long-term navigation

applications, the INS should works with the aid

of the GPS system. The integration of INS and

GPS is considered an optimal combination. The

heart of integrated system is Kalman algorithm

[5].

Fig. 4 illustrates a feedforward

configuration. Its advantage is that provides a

rapid filter response. Alternatively, the

configuration in Fig. 5 is a feedback one. This

configuration is more complex than the open loop one but it can provide better performance

in the exist of nonlinear effects. In our project,

the feedforward configuration is chosen in order

to provide fast performance [6, 7].

Figure 4. Feedforward configuration

Figure 5. Feedback configuration

The Kalman filter is a multiple input, multiple

output digital filter that can optimally estimate

in real time the states of the system based on its

noisy outputs. These states are velocity errors

(eVN, eVE, eVD), drift terms (GBx, Gby, GBz) and

attitude errors (Tn, Te). The GPS output is used

as a tool to estimate the error in the INS and to

correct the error as much as possible. We call

this is the GPS-aided INS system configuration.

Looking at the discrete model:

kkkk wxx +Φ=+1 (5)

Where Φk is the state transition matrix, wk is the

driven response at tk+1 due to the presence of

input white noise during time interval (tk, tk+1).

Because this time interval is short (it means

that the update rate of the INS is high), we can

approximate Φk as:

tFIe tFk ∆+≈=Φ ∆ )( (6)

and the covariance matrix associated with wk is

[ ] tGQGwwEQ Tk

Tk

Tkkk ∆ΦΦ≈= (7)

Where

[ ]222222xxxazayaxdiagQ ωωω σσσσσσ=

Looking at the observation equation:

zk = Hk xk + vk (8)

where

−

−

−

=

GPSINS

GPSINS

GPSINS

k

VdVd

VeVe

VnVn

z , ( )5333 0 ××= IH k and

[ ] [ ]222

VdVeVn

T

kkk diagvvER σσσ== is

the covariance matrix for vk.

In this system, we assume that the process

noise wk and the measurement noise vk are

uncorrelated.

Figure 6 shows the more detail of the

feedforward KF implementation.

Figure 6. The 8 states feedforward structure



-4-

4. EXPERRIMETAL RESULTS

In this study, we used the MICRO-ISU BP3010

which consists of three ADXRS300 gyros and

three heat compensated ADXL210E

accelerometers. The measurements are

synthesized by IMU’s micro-controllers and

transmitted out via RS232 interface. The unit

transmits output data as angular incremental and

velocity incremental data in serial frames of 16

bytes at 64 Hz.

Figure 7. The BP3010 – A MEMS unit.

After a calibration procedure, the experiment system is placed on precise rate table which

contains sequence of different rates for each

dimension has been made use. The IMU is

initially positioned in center of rate table and

each rate is run approximately for 10 minutes.

As mentioned above, eight states were

considered and Kalman filter was used to

estimate the INS errors. The INS can give out

correct values of the velocities by subtracting

the noisy INS to the estimation of the INS

errors. The standard deviation of the sensors

chosen was 30 mGal. The standard deviation of

the GPS was 20 m.

To point out the usefulness of the KF, look

at the Fig. 7 and Fig. 8 that are illustrate the

heading of INS while standing still in the case

without KF and with KF.

Figure 7. The drift of heading (without KF)

Figure 8. The heading with the present of KF

While the INS is in rotation at the rate 10 o/s, the

integration system with KF can provide the

exactly the heading as shown in Fig. 9.

Figure 9. The heading with the present of KF at

the rate 10 o/s



-5-

Figure 10. The north velocity of the stand still

IMU in two cases: with and without KF

Come back to stand still case and look at the

north velocities in two cases: with and without

KF. The graphs for velocity computed and

corrected by the Kalman filter are given in Fig.

10. We can see that the un-aided INS deviates

from the ideal velocity by a large quantity. If the

integration system is supported by KF, the

output Vn is around 0 m/s. It is mean that our KF

could give the exact correction in feedforward

configuration. The update from the INS was taken every

0.015625s, the GPS update was taken every 1s

and the KF was run every 0.5s to achieve better

accuracy. Every alternate 0.5s instant, when the

GPS update is not available; we have to predict

the error state by using the most recent GPS

update as the measurement, i.e. the GPS update

is taken constant for that whole one second. This

also comes in use when there are GPS outages.

For the experiment of the IMU on road [8],

GPS and the data acquisition system were

installed in a vehicle. The vehicle was driven for

12 minutes in a 5 km trajectory. Initially the

vehicle was at rest, with the engine on, for about

45 seconds. This stationary data was used for

calibration and alignment purposes.

Fig. 11 and 12 present the velocity of KF

compared with the values measured with the

GPS unit. It can be seen outputs of KF, black,

follows the GPS velocities with small error for

approximately 15 minutes.

Figure 11. Comparison of East velocities

Figure 12. Comparison of North velocities

The 2-D trajectory is presented in Fig. 13.

This figure shows the position of the vehicle

along North and East direction on the Earth

instead of the latitude and the longitude. The

reason is that we can prevent numerical

instabilities in calculating the Kalman gain. It

can be seen the INS/GPS trajectory supported by

KF follows the GPS one (dot curve) with small

error for a quite long trip.



-6-

Figure 13. Comparison of trajectory

In land vehicle applications the errors in roll and

pitch can be catastrophic since they are mainly

responsible for the gravity vector compensation.

The selection of the roll and pitch gyros should

contemplate the terrain type since it will be

responsible for the maximum rate measured by

these gyros.

5. CONCLUSIONS

In this paper, an eight state Kalman filter

was proposed to be used in order to enhance the

quality of a combined GPS and INS system. The

experimental results have shown that the initial

calibration and alignment is accurate enough to

allow navigation with IMU sensors for extended period of time with low dead reckoning errors.

In fact, the Kalman filter tremendously improves

accuracies compared to the GPS and INS when

operating alone as individual navigation

systems. The accuracy of estimated parameters

is improved by increasing the dimension of the

systems’ states space. Obviously, by its simplicity, this model can be embedded easily

into a real time system. Future work will

investigate in-flight calibration and alignment

algorithms extending the error models of the

INS system.

Acknowledgements: This work was supported

by the QGTĐ0509 project.

REFERENCES

1. Collinson, R.P.G., “Introduction to

Avionics”, Chapman and Hall, London, 1996.

2. Omerbashich, “Integrated INS/GPS

Navigation from a Popular Perspective",

Journal of Air Transportation, Vol. 7, No. 1

(2002), pp. 103-119.

3. Randle, S.J., Horton, M.A., “Low Cost

Navigation Using Micro – Machined

Technology”, IEEE Intelligent

Transportation Systems Conference, 1997.

4. Parkinson, B.W., and Spilker, J.J.Jr.,

“Global Positioning System: Theory and

Applications”, Vol. 1(1996), AIAA,

Washington DC. 5. Grewal, M.S., Weill, L.R., and Andrews,

A.P., “Kalman Filtering: Theory and

Practice using MATLAB”, John Wiley and

Sons, New York, (2001).

6. Wolf, R., Eissfeller, B., Hein, G.W., “A

Kalman Filter for the Integration of a Low

Cost INS and an attitude GPS”, Institute of Geodesy and Navigation, Munich,

Germany.

7. Tran Duc Tan, Huynh Huu Tue, Nguyen

Thang Long, Nguyen Phu Thuy, Nguyen

Van Chuc, Designing Kalman Filters for

Integration of Inertial Navigation System

and Global Positioning System, The 10th biennial Vietnam Conference on Radio &

Electronics, REV-2006, (2006), pp. 76-80.

8. Panzieri, S., Pascucci, F., Ulivi, G., “An

Outdoor navigation system using GPS and

Inertial Platform”, IEEE ASME

Transactions on Mechatronics, Vol.

7.(2002).

123 navigation

Documents

Transcript of 123 navigation