Implementation of an FPGA-based Aided IMU on a

Implementation of an FPGA-based Aided IMU on a Low-Cost Autonomous Outdoor Robot Michael Jew, Aly El-Osery

Stephen Bruder

Electrical Engineering Department New Mexico Tech Socorro, New Mexico Email: [email protected]

Applied Technology Associates Albuquerque, New Mexico Email: [email protected]

Abstract—This paper presents the implementation of real-time algorithms for an aided Inertial Navigation System (INS) on a fabric processor based field programmable gate array (FPGA). Such an approach allows for the development of a hard realtime computational architecture tailored to the specific INS requirements while still preserving flexibility. The development of a realistic simulator proved to be an important stepping stone in the deployment of the INS algorithms in addition to providing useful insight into overall system operation. The simulation supports modular code verification and validation for the embedded target. Specifically, we focus on the integration of an inertial navigation systems (INS) with a man-portable tracked MATILDA robot manufactured by Mesa Robotics. This platform has seen service in both Afghanistan and Iraq, and divides into three pieces for transportation where each piece weighs about 20 pounds. Keywords-component; Inertial navigation; inertial measurement units; FPGA, Kalman filter; robotics.

Fig. 1.

Modified Matilda robot

I. I NTRODUCTION Determination of an object’s position and orientation is of great value to many applications. Global Navigation Satellite Systems (GNSS) offer a widely available means of determining position with considerable accuracy; however, this method has considerable line of sight limitations. As an example, an urban area with many high buildings will suffer from multipath interference compounded by potential geometric dilution of precision resulting in compromised accuracy due to obstruction of the satellite signals [1]. Consequently, the need to provide a more robust approach is highly desirable. Inertial Navigation Systems (INS), consisting of accelerometers, gyroscopes, algorithms and a computational platform, provide complementary capabilities and is a mature technology [2], [3]. Employing Newton’s law of motion and assuming knowledge of the initial position, velocity, and attitude, these parameters can be propagated over time. Furthermore, technology advances in low-cost MEMS inertial measurements units [4] have opened the door to the development of low-cost INS solutions for a variety of applications, such as, unmanned autonomous vehicles. Employing an IMU containing gyroscopes and accelerometers, provides accelerations and angular rates required in determining the navigation solution. Unfortunately, low-cost inertial measurement units suffer from large uncertainties due This work is support in part by Applied Technology Associates.

to high intrinsic noise levels and instability in both the bias and scale factor. The inclusion of positional aiding sensors (e.g. GPS) and/or attitude aiding sensors (e.g. magnetometer and inclinometer) offers means to estimate and compensate for some of these uncertainties. In this paper we present a low-cost complete solution for an inertial navigation system, in addition to a simulation environment, which is suitable for an outdoor unmanned autonomous vehicle. This solution uses a GPS, inclinometer, and three axis magnetometer as aiding sensors. We also present a framework for designing inertial navigation systems on FPGA based processors. Furthermore, challenges in implementing the system on an FPGA and techniques to meet them will be presented. Results will include a comparison in simulation between generated trajectories and that estimated by the system, as well as comparison between real data from an outdoor test and that estimated by the system implemented on the FPGA. Although all the development in this paper is suitable for a variety of applications, our goal is to implement the system on a robotic platform. The chosen platform is a man-portable tracked MATILDA robot manufactured by Mesa Robotics which has been completely redesigned by the robotics group at New Mexico Tech, as shown in Fig. 1. This platform has seen service in both Afghanistan and Iraq, and it separates into three pieces for transportation where each piece weighs about 20 pounds.

~ze ≡ ~zi

the z-axis pointed in the direction providing the right-handed orthogonal coordinates (as shown in Fig. 1). A. ECEF Mechanization Governing Equations

N ωe ~ye

θlon θlat

wich een Gr

NMT

~xe ~xi Vernal Equinox

~yi

Fig. 2.

Coordinate frames:i-frame and e-frame

The basic principle is to use the measured forces from the accelerometers and Newton’s first and second laws to determine the position. However, in order to resolve the forces acting on the body correctly to determine the position in a specific coordinate frame, it is first necessary to determine the attitude of the body, hence, the need for gyros. The full governing equations for the ECEF mechanization, expressed in vector form, follow the development presented in [5] as     ~v ee,b ~r˙ ee b  ˙ e   C ef ω ei,e × ~vee,b + ~g el  (1) ,  ~v e,b  =  b ~ i,b − 2~ ˙e ~b × C Ce ω b

A. Notation I n×m n × m identity matrix 0n×m n × m Null matrix E(·) Expectation ~q ba /C ba Orientation of the a-frame wrt to the b-frame as a quaternion/rotation-matrix (~ ω ×) Skew symmetric matrix ~v ab,c Vector ~v of c with respect to b coordinatized in the a ∆t Sampling time − ˆ ~ at time step k ~x (k) A priori estimate of x II. NAVIGATION E QUATIONS In this section, the navigation equations used to compute position, velocity, and attitude are presented. It is important to first define the coordinate frames that are used. IMUs provide inertial measurements; therefore, they measure motion with respect to the fixed inertial frame (i-frame). On the other hand, for terrestrial navigation purposes, it is desirable to express the navigation solution in terms of an earth-fixed frame, namely, the Earth Centered Earth Fixed (ECEF) frame (e-frame) or local tangential frame (l-frame). Fig. 2 illustrates the orientation of the i-frame and the e-frame in relation to each other (offset defined via the Greenwich Mean Sidereal Time), wherein the i-frame is fixed and the e-frame is rotating about the common z-axis at approximately 15o /hr (ωe ). The local frame is a fixed reference frame with respect to earth in which the vehicle’s position and attitude are expressed as the vehicle is moving near the earth’s surface. Due to the constrained nature of the trajectories being considered herein the l-frame will remain time-invariant (wrt e-frame) during any given test. The x-axis is pointed north, the yaxis is pointed east, and the z-axis completes a right-handed orthogonal coordinate system (i.e. north-east-down (NED)). The body frame (b-frame) represents the orientation of the strapdown IMU which is rigidly mounted to the vehicle. The origin of the system is defined as the center of the IMU. The x-axis is pointed in the forward direction of motion of the vehicle, the y- axis is pointed out the right-hand side, and

b

e,b

where the local gravity, ~g el , represents the effect of the earth’s gravity on the body, and is calculated as ~ ei,e × ω ~g el = C eb~g − ω ~ ei,e × ~r ee . (2)

The second term in (2) represents the centripetal force experience by the body due to the rotation of earth. The angular rates ~ bi,b , the angular rate of the body measured by the IMU are ω with respect to the inertial frame. Computing the angular rate ~ ee,b , requires cancellation of the earth’s in the earth’s frame, ω rotational effect, therefore, ~ ei,e ~ bi,b − C be ω ~ be,b = ω ω

(3)

The magnitude of the earth’s rotation, ωe , is given as 7.292115 × 10−5 radians per second (WGS-84). The angular velocity of the e-frame with respect to the i-frame coordanitized in the e-frame is T ~ ii,e = 0 0 ωe ~ ei,e = Cie ω . (4) ω

B. ECEF Mechanization Algorithm

The basic steps for the mechanization algorithm include: correction of raw data (temperature compensation of scale factors and biases), attitude update, specific force transformation to ECEF, velocity update, and position update, as illustrated in Fig. 3. The various steps in the mechanization algorithm are described below. 1) Correction of Raw Data: The correction of raw data consists of two major steps: application of scale factors and biases (temperature compensated) and numerical integration. Since the IMU outputs angular rates, ω ~˜ bi,b , and specific forces, ˜b ~f i,b , are in terms of the internal analog-to-digital converter values, application of the appropriate scale factors and biases is necessary to convert the raw values into the equivalent angular rates and specific forces. The following calculations are used to apply the scale factors and bias values to the raw IMU outputs   Sgx 0 0 b ~ i,b (k) =  0 Sgy 0 ω ~˜ bi,b (k) − ~bg (5) ω 0 0 Sgz

~abi,b (k)

accels b ~ωi,b (k)

b ∆~vi,b (k)

Correction of Raw IMU Data

Specific Force Transformation to ECEF

b ∆θ~i,b (k)

gyros Attitude Update

 Sax b ~ (k) =  0 f i,b 0

(6)

tk−1

∆~v bi,b (k) =

Z

tk

~ b (k)dt. f i,b

(8)

tk−1

b This results in values for the incremental angle, ∆~θ i,b , and incremental linear velocity, ∆~v bi,b . 2) Attitude Update: Referring back to (1), propagation of the body attitude is necessary. First, the incremental angular position is expressed with respect to earth as: T b ∆~θ e,b (k) = ∆θx ∆θy ∆θz (9) b ~ ei,e ∆t. = ∆~θ i,b (k) − C be ω

Propagation of the direction cosine matrix, C eb , through time is achieved using through quaternion update followed by conversion of quaternion to DCM. Assuming small changes, the attitude is updated as follows ~q eb (k) = ~q eb (k − 1) · d~q

(10)

where d~q =

h

b k∆~ θ e,b (k)k ) cos( 2

T

(~e)

b k∆~ θ e,b (k)k sin( ) 2

and the instantaneous axis of rotation ~e =

b ∆~θ e,b (k) b k∆~θ e,b (k)k

iT

,

~ree (k)

e ~ve,b (k)

4) Velocity Update: In order to update the cumulative velocity of the vehicle, the incremental velocity is computed with respect to the earth as ∆~v ee,b (k) = ∆~v ei,b (k) − 2(~ ω ei,e × ∆~v ee,b (k − 1))∆t + ~g el ∆t. (12) Now that the incremental velocity is expressed e-frame and with respect to the earth, the cumulative velocity can be updated as ~v ee,b (k) = ~v ee,b (k − 1) + ∆~v ee,b (k).

(13)

5) Position Update: The updated cumulative velocity is used to update the position as ~r ee (k) = ~r ee (k − 1) + ~v ee,b (k)∆t

(14)

C. Sensor Error Models An error model is required for the analysis of accelerometer and gyroscopes errors. The accelerometer error model is given as ~ = (S A + M A ) f ~ + δ~bA + δ nl ~ A+n ~ A, δf (15) ~ A and n ~ A represent the accelerometer bias where δ~bA , δ nl errors, non-linearity and measurement noise, respectively. Similarly, the gyroscope error model is given as ~ G+n ~ + δ~bG + δ nl ~ G, δ~ ω = (S G + M G ) ω

(16)

~ G and n ~ G represent the gyroscope bias errors, where δ~bG , δ nl g-sensitivity errors and measurement noise, respectively. The errors in the position, velocity and attitude may then be represented as e T e δ~ x(t) = (δ~r e (t))T (δ~v e,b (t))T (δ~q (t))T (17) ˆ (t), ~ true (t) − ~x =x and similarly, the errors in the measured may be represented as h iT ˆ (t), (18) ~ (t))T (δ~ ~ true (t) − ~u δ~ u(t) = (δ f =u ω (t))T

.

3) Specific Forces Transformation to ECEF: The updated C eb matrix is used to transform the incremental inertial velocity to express it in terms of the earth as ∆~v ei,b (k) = C eb ∆~v bi,b (k).

Position Update

ECEF mechanization block diagram

where the Sg and Sa values are the scale factors and the ~bg and ~ba values are the biases for the gyroscopes and accelerometers, respectively. Next, a numerical integration step is required as shown below: Z tk b ~ ~ bi,b (k)dt, ω (7) ∆θ i,b (k) = and

e ~ve,b (k)

Ceb (k)

 0 ˜ 0  ~f bi,b (k) − ~ba , Saz

0 Say 0

Velocity Update

Ceb (k)

Fig. 3.

and

e ∆~vi,b (k)

(11)

ˆ (t) and ~u ˆ (t) are the estimate of the INS states and where ~x inputs, respectively. ˆ (t) + δ~ ~ true (t) = ~x Through substitution of x x(t) and ˆ (t) + δ~ ~ true (t) = ~u u u(t) into (1), using Taylor’s series expansion, and ignoring high order terms, the error equation can be written as [6]

˙ (t) = F (t)δ~ δ~x x(t) + G(t)δ~ u(t),

(19)

where  03×3  F (t) = 03×3 04×3

and

 03×4 ˆ  ~ ei,e ×) D(~ˆq , ~f )  , −2(w 04×3 W (ω ~ˆ , t) I 3×3



03×3 G(t) =  03×3 ∂W q (t) ~ ~ ∂ω  0 −ωx ωx 0 W (ω, t) =  ωy −ωz ωz ωy  q2 ∂W 1 q1  ~q (t) =  ~ ∂ω 2 −q4 −q3

 03×3 C eb (t) , 04×3

−ωy ωz 0 −ωx −q1 q2 q3 −q4

 −ωz −ωy  , ωx  0  q4 −q3  , q2  −q1

Fig. 4.

b ∆~θ e,b . ∆t In this paper, we only consider the measurement noise and bias terms. The other terms are not reliably observable through the error dynamic model. Nonetheless, these sensors may be characterized and compensated for through laboratory based sensor characterization (see Fig. 4).

~ (t) = ω

The augmented state space error model, including the IMU measurement noise assumed white Gaussian, is now ˙ (t) = A(t)δ~ ~ δ~x x(t) + B(t)w(t)



A(t) = 

Representing the biases as first order Markov processes [7] gives rise to.

and

1 ˙ ~ bA (t), δ~bA (t) = − I3×3 δ~bA (t) + w τA



(21)

q1 f1 − q4 f2 + q3 f3  q2 f1 + q3 f2 + q4 f3 ˆ T D(~ˆq , ~f ) = 2  −q3 f1 + q2 f2 + q1 f3 −q4 f1 − q1 f2 + q2 f3

F (t) 06×6 

and

~ = (~ nG )T w

(~ nA )T



G(t) − τ1G I 3×3 03×3

03×3  03×3 B(t) =   ∂W ~q (t) ~ ∂ω 06×3

(20)

~ bG (t) and where τG and τA are correlation times, and w ~ bA (t) are white noise processes of the gyroscopes and w accelerometers, respectively. The state space error model can now be augmented to include the errors in the bias resulting in the following modified state vector h iT δ~ x = (δ~r ee )T (δ~v ee,b )T (δ~q )T δ~bG ))T (δ~bA )T . (22)

(23)

where

III. AUGMENTED S TATE S PACE E RROR M ODEL

1 ˙ ~ bG (t) δ~bG (t) = − I3×3 δ~bG (t) + w τG

IMU calibration in ATA’s inertial laboratory

, 03×3 − τ1A I 3×3

03×3 Cbe (t) 04×3 06×3

(~ nq )T

 03×6 03×6  , 04×6  I 6×6

~ bG )T (w

~ bA )T , (w

~ w(τ ~ )T ] = Q(t)δ(t − τ ). E[w(t) A. Discretization The discrete process model for this state space realization is given by ~ δ~ x(k + 1) = Φ(k)δ~ x(k) + w(k)

(24)

δ~z (k) = Hδ~ x(k) + ~v (k),

(25)

q4 f1 + q1 f2 − q2 f3 q3 f1 − q2 f2 − q1 f3 q2 f1 + q3 f2 + q4 f3 q1 f1 − q4 f2 + q3 f3

 −q3 f1 + q2 f2 + q1 f3 q4 f1 + q1 f2 − q2 f3   −q1 f1 + q4 f2 − q3 f3  q2 f1 + q3 f2 + q4 f3

IMU ~abi,b (k)

or Trajectory generator

~ bi,b (k) ω

e ~ree (k), ~ve,b (k), ~qeb (k)

ECEF Mechanization

−

ˆ e (k), ~q ˆ e (k) ~ˆree (k), ~v e,b b

− Extended Kalman Filter

e δ~ree (k), δ~ve,b (k), δ~qeb (k)

Aiding sensors Fig. 5.

Block diagram of the navigation states estimation.

where the states used in this process model are iT h δ~ x = (δ~r ee )T (δ~v ee,b )T (δ~q )T (δ~bG )T (δ~bA )T ,

and the measurements used in this process model are e T e δ~z (k) = (δ~r e (k))T (δ~v e,b (k))T (δ~q (k))T ,

where

H=

I 10×10

010×6

,

T

P (k) = (I − K(k)H) P − (k) (I − K(k)H) + K(k)R(k)K t (k).

and the discrete transition matrix is approximated as

(33)

Φ(k) = eA(tk )∆t ≈ I + A(tk )∆t Assuming the process and measurement noise are uncorrelated implies that ( Q(k) , i = j T ~ w(j) ~ E[w(i) ]= (26) 0 , otherwise, ( R(k) , i = j T E[~v (i)~v (j) ] = (27) 0 , otherwise, and the noise covariance matrix Q can be computed as [2] 1 Φ(k)B(tk )Q(tk )BT (tk )ΦT (k)+ Q(k) ≈ 2 (28) B(tk+1 )Q(tk+1 )BT (tk+1 ) ∆t

B. Error State Estimation

Estimation of the navigation states is done using a loosely coupled configuration as shown in Fig. 5. The error estimates, obtained via a Kalman filter, are used to correct the INS output. The linearized extended Kalman filter equations that will be implemented are presented below [8]: 1) Using the state transition matrix to project the state estimate and the error covariance ˆ − (k) = Φ(k)~x ˆ (k − 1), ~x P − (k) = Φ(k)P (k − 1)ΦT (k) + Q(k).

2) Compute the Kalman gain h i−1 K(k) = P − (k)H T HP − (k)H T (k) + R(k) . (31) 3) Use current measurement to update the state estimates and the error covariance matrix ˆ (k) = ~x ˆ − (k) + K(k) ~z (k) − H~ x− (k) , (32) ~x

(29) (30)

The error covariance update (33) is implemented using the Joseph form which provides a more stable solution due to the guaranteed symmetry. C. Observability Analysis Currently, the Kalman filter contains 16 states which could be easily increased as more errors are modeled. However, unless these states are observable through the measurements, the utility of the corresponding estimates becomes questionable. In many cases, the observability of a system can be determined by computing the rank of the observability matrix, given in [5] as   H(k − m + 1)  H(k − m − 2)Φ(k − m + 1)    O(k) =  (34)  ..   . H(k)Φ(k − 1) . . . Φ(k − m + 1)

The rank of O is a binary indicator and does not provide a measure of how close the system is to being unobservable, hence, is prone to numerical ill-conditioning [9]. Consequently, in addition to the computation of the rank of O(k), we compute the Singular Value Decomposition (SVD) of O(k) as O = U ΣV ∗ and observe the diagonal values of the matrix. Using this approach it is possible to monitor the variations in the system observability due to changes in position, velocity and/or attitude, in the presence or lack of aiding measurements.

Digital Compass

GPS

IMU

• •

Xilinx ML507

Fig. 6.

Main system components

of rotation for each gyroscope providing a fully integrated six degree-of-freedom IMU. The IMU uses the Serial Peripheral Interface (SPI) and a simple register structure to provide easy access to the data and configuration. Also included in the IMU is an embedded controller which dynamically compensates for thermal effects on the MEMS sensors to provide more accurate sensor outputs. The WASS enabled single-frequency GPS receiver is a Thales Navigation DG16. The Honeywell HMR3000 is a tri-axial compass with inclinometer that provides heading, pitch, and roll measurements for navigation. Three of Honeywell’s magnetoresistive sensors are placed in orthogonal triad along with an electrolytic fluid-filled dual-axis tilt sensor to measure the vector components of the earth’s magnetic field and provides a gravitational reference. V. FPGA I MPLEMENTATION

IV. S YSTEM D ESIGN The previous section addressed the required foundation for computing a navigation solution using an INS as well as correcting its error using aiding sensors. Next, it is important to consider the practical implementations of such systems. Predicated upon the assertion that low-cost INS systems need to be complemented with aiding sensors in order to provide reasonable accuracies, it is essential to employ a suitable platform which allows ease of integration of those additional sensors. Utilizing a fixed computational architecture to meet the hard real-time processing requirements is challenging and requires considerable interfacing circuits. Fortunately, Field Programmable Gate Arrays (FPGA) offers a highly desirable alternative. Programmable logic blocks and interconnects available on an FPGA provide a highly configurable computational architecture that can easily be used to realize both interfacing modules and processing hardware running in parallel all within the same chip. The reconfigurability of FPGAs offers a highly customizable solution that can be enhanced, modified, and easily adapted to accommodate future changes. Highly specialized digital signal interfaces can be provided within the FPGA via custom fabric-IP blocks and custom sensor pre-processing can be performed with hardware determinism in parallel. The FPGA has been successfully employed as the central component in our computational architecture and provides a reliable interface (and pre-processing) to the MEMS IMU, GPS and compass/inclinometer (see Fig. 6): • The FPGA board is a Xilinx ML507 evaluation board which includes a Virtex-5 FX70T FPGA and the necessary interface chips with a variety of I/O connectors. The Virtex-5 FX70T FPGA includes a hard-core PowerPC440 processing unit surrounded by ”fabric” (i.e. logic) that is used to implement the necessary modules to interface the PowerPC core with the external peripherals. • The Analog Devices ADIS16355 IMU includes a triad of both accelerometers and gyroscopes with temperature sensing. The accelerometers are oriented along with axis

Using the Xilinx Embedded Development Kit (EDK), which provides an integrated software development environment, along with the Xilinx Integrated Software Environment (ISE) hardware description language (HDL) development tools, a full navigation system was implemented via a mix of HDL and Ccode. This system includes processing of the IMU and aiding sensor measurements, implementation of the mechnization equations, error estimation using extended Kalman filter, and the computation of the corrected navigation solution. A high-level description of the communication pathways for the different FPGA components is presented in Fig. 7 wherein the blue blocks represent components external to the FPGA board, the yellow block is the ”hard-core1 ” processing block of the FPGA, and the pink blocks represent the different interface modules that are realized (via HDL blocks) within the FPGA fabric. The ECEF mechanization and Kalman filtering algorithms are written in the C programming language and compiled using the compiler included with the Xilinx Embedded Development Kit. By using the included compiler, the C code is compiled into code that can be run on the PowerPC 440 processing unit within the Xilinx Virtex-5 FPGA. The code written for the FPGA closely matches the algorithm presented above with the notable exception that the C programming language does not have built in matrix and vector processing capabilities which are implemented in C. Currently, the matrix and vector mathematics are implemented using statically sized data due to the inherent complexity of dealing with dynamically sized data. This limits the code to only processing vectors and matrices of a known size. Eventually, these routines will be transitioned to the fabric. Specialized capabilities such as matrix inversion, which are required for the Kalman filter implementation, can also be implemented within the fabric. Non-volatile data collection for the FPGA implementation is achieved via the use of a compact flash card. 1 Not

an HDL module

Xilinx DP−FPU

TABLE III C HOSEN C LOCK F REQUENCIES FOR FPGA S YSTEM

Block RAM

FPGA Module DDR RAM

Xilinx Memory Comtroller

Compact Flash Card

Xilinx SystemACE Controller

Interrupt Controller

PowerPC 440

Fig. 7.

375 187.5 125 128 0.977

Serial Port RS232

SPI Interface

Analog Devices ADIS16355 IMU

Fixed Interval Timer

Operating Clock Frequency (MHz)

PowerPC DP-FPU PLB IMU’s SPI Clock Divider IMU’s SPI Clock

chip memory and connects to the PLB as well as directly to the PowerPC through its memory port. The fixed interval timer’s connection to the interrupt controller is not shown. The clock generator generates the clocks required for all FPGA modules including the PowerPC processor from an off-chip clock source and thus connects almost every other module in the system.

Honeywell HMR3000 Compass

GPS

FPGA based embedded processing

TABLE I FPGA M ODULE M AXIMUM C LOCK F REQUENCIES FPGA Module

Maximum Clock Frequency (MHz)

PowerPC DP-FPU PLB IMU’s SPI

550 210 150 1

TABLE II P OSSIBLE C LOCK F REQUENCY C OMBINATIONS IN MH Z PLB Clock

SPI Clock Divider

SPI Clock

125 150 150

128 256 128

0.977 0.586 1.172

Timing the different components of the system is another key and important aspect of the design. Every FPGA has a reference clock that is an input from an external source. The current input clock frequency of the FPGA is 100 MHz which the FPGA’s logic can increase or decrease as required. While, some of the different components have very specific clocking requirements others are dependent only on what speed the FPGA fabric can operate at. Since there are multiple clocks tied together via a series of clock generators, they need to be considered as a group when considering what frequency to operate the different modules. Table I describes the maximum operating clock frequencies for several of the FPGA modules. Table II shows several different clock frequency combinations to meet the IMU’s SPI clock frequency requirement of 1 MHz. Table III shows the clock frequencies used by the implemented system. Figs. 8, 9, and 10 show the connections between the different FPGA modules as implemented using the Xilinx EDK tool. Fig. 8 shows the PowerPC processing core and the FPGA modules connecting directly to it with connection to the Processor Local Bus (PLB) at the bottom. Fig. 9 shows all the devices that use the PLB to connect to the processor. The BRAM (block RAM) module is the PowerPC’s source of on-

Fig. 8.

PowerPC FPGA module connections

Fig. 9.

PLB slave module

X−Y Position 80

70

Fig. 10.

North−South Position (meters)

60

Key symbols X−Y Position

120

50

40

30 Mechanization Output Kalman Corrected Output GPS Measurements

20

10

100


0

Mechanization Output Kalman Corrected Output GPS Measurements

80

−10 −60

−50

−40

−30 −20 East−West Position (meters)

−10

0

10

60

Fig. 12.

FPGA output using a clockwise motion

40

Heading 400

20 350

0

300

20

Fig. 11.

40 60 East−West Position (meters)

80

250

100

Simulation output using a square path

Angle (degrees)

0

200

150

VI. M ODELING AND S IMULATION 100

The design of the simulation environment allows for driving the simulation with either synthesized or real-data. This allows for a robust opportunity to validate the algorithms first with simulated (i.e. synthesized) data and then verify with real sensor data. A detailed presentation of the simulation environment was previously presented in [10]. Fig. 11 shows a sample of the results obtained assuming a square path. In addition, GPS outage was also simulated during straight path segments and during turns. As shown in Fig. 11, the mechanization is drifting as expected and the output of the coupled system tracks the path accurately. Results from this simulation environment were then used to validate the algorithms implemented on the FPGA. VII. I MPLEMENTATION R ESULTS Several tests were performed to validate the system implementation. These tests include a variety of scenarios some of which are presented herein. Fig. 12 shows the results obtained by moving the system in a clockwise motion in an approximately square area of about 8 meters. As demonstrated in the figure, the mechanization suffers from high drift. This drift is attributed mainly to the inaccuracies of estimating the biases in the accelerometers and the gyroscopes. On the other hand, the corrected solution using the loosely-coupled Kalman

50

0

0

5

Fig. 13.

10

15

20 Time (seconds)

25

30

35

40

FPGA output heading for clockwise motion

configuration is able to estimate the system states including the biases and correctly tracks the path of the system. By computing the heading generated by the mechanization (see Fig. 13) the clockwise path with 90deg turns can readily be recognized. Fig. 14 illustrates the results of further testing of the system obtained by placing the system in a car and traveling in an L-shaped path. Once again, as shown in Fig. 14, the drift in the mechanization solution is clear. The derived heading from the mechanization output is provided in Fig. 15.

X−Y Position

Heading

150

200

150 100

50

50

Angle (degrees)


100

0

0

−50

−100 −50

−150

Mechanization Output Kalman Corrected Output GPS Measurements −100 −60

−40

−20

Fig. 14.

0

20 40 60 East−West Position (meters)

80

100

120

FPGA output moving in the parking lot

−200

0

10

Fig. 15.

20

30 Time (seconds)

40

50

60

FPGA heading for parking lot scenario

VIII. C ONCLUSION

R EFERENCES

Computing a navigation solution using low-cost IMUs is a challenging task and benefits from the use of additional aiding sensors. Most research efforts have focused on development of the required algorithms to enhance the performance of low-cost IMU based inertial navigation systems, however, less effort has been directed towards the development of a practical implementation suitable for small platforms. In this paper we have presented the implementation of an aided-INS system on an FPGA, hosted by a small mobile robot. This approach provides a highly configurable solution that can easily be adapted to a variety of applications and utilized an assortment of aiding sensors. Even though the overall implementation was successful, there are several areas that require improvements. Specifically, an automated approach for initializing the system is highly desirable. This remains an open challenge when employing low-performance IMUs. An approach based on pre-programmed motion scenarios is being investigated. As the mobile platform executes these scenarios, data is collected and used to initialize the system.

[1] R. B. Langley. Dilution of Precision. GPS World, pages 52–59, 1999. [2] E-H. Shin. Estimation Techniques for Low-Cost Inertial Navigation. PhD thesis, University of Calgary, 2005. [3] O. J. Woodman. An Introduction to Inertial Navigation. Technical Report 696, University of Cambridge, August 2007. [4] T. P. Crain II, R. H. Bishop, and T. Brady. Shifting the Inertial Navigation Paradigm with the MEMS Technology. In 33rd Annual AAS Guidance and Control Conference, Breckenridge, CO, United States, 6-10 Feburary 2010. [5] D. H. Titterton and J. L. Weston. Strapdown Inertial Navigation Technology, volume 207 of Progress in Astronautics and Aeronautics. AIAA, 2 edition, 2004. [6] D. E. Gaylor. Integrated GPS/INS Navigation System Design for Autonomous Spacecraft Rendezvous. PhD thesis, The University of Texax at Austin, 2003. [7] M. El-Diasty and S. Pagiatakis. A Rigorous Temperature-Dependent Stochastic Modelling and Testing for MEMS-Based Inertial Sensor Errors. Sensors, 9:8473–8489, 2009. [8] R. G. Brown and P. Y. C Hwang. Introduction to Random Signals and Applied Kalman Filtering. Wiley, 3rd edition, 1997. [9] S. Hong, H-H Chun, S-H Kwon, and M. H. Hyung. Observability Measures and Their Application to GPS/INS. IEEE Transactions on Vehicular Technology, 57:97–106, 2008. [10] A. El-Osery, M. Jew, and S. Bruder. An Inertial Navigation System for Autonomous Outdoor Robots. In ION 2010 International Technical Meeting, Palm Springs, CA, 25-27 January 2010.

ACKNOWLEDGMENT This work was funded by Applied Technology Associates (ATA) and partially supported by the Space Microsystems Innovations & Applications Center (COSMIAC), both located in Albuquerque, NM. The authors would also like to thank the staff of ATA for their support and feedback regarding the content of this work.