www.redpel.com +917620593389

Low-Cost. High-Accuracy, State Estimation for Vehicle Collision Prevention System

Kamal Saadeddin

Mamoun F. Abdel-Hafez

Mechatronics Engineering Graduate Program American University of Sharjah Sharjah, UAE [email protected]

Department of Mechanical Engineering American University of Sharjah Sharjah, UAE [email protected]

Mohammad Amin Jarrah

Department of Mechanical Engineering American University of Sharjah Sharjah, UAE [email protected] Abstract-In this paper, a low-cost navigation system with high integrity and reliability is proposed for enhancing highway traffic safety in adverse weather situations, such as a foggy or rainy weather.

A

high-integrity,

IS-state,

estimation

filter is

proposed to obtain a high accuracy state estimate. The filter utilizes a vehicle velocity constraint measurement to enhance the accuracy of the estimate. Two estimations filters, the Kalman filter

(KF)

compared

and the information filter to

obtain

the

estimate

of

(IF) , the

are designed and vehicle

state.

An

instrumentation system that consists of a microcontroller, a GPS receiver,

an

IMU,

encoders,

and

a

Zigbee

transceiver

is

constructed. The microcontroller provides a vehicle navigation solution at 50 Hz by fusing the measurements of the IMU, the

GPS receiver and the digital compass using the proposed filter design.

Experimental

tests,

using

the

proposed

system,

are

conducted and the obtained results are presented. These results are processed with and without the velocity constraints. The estimation accuracy improvement with the addition of the velocity constraints is demonstrated. Also, results of a static test are used to assess the accuracy of the low-cost navigation systems against a commercial solution.

L Keywords-component; Collision Prevention Sytem; GPS; IMU; State Estimation

II.

INTRODUCTION

Prediction of vehicle's dangerous driving conditions or maneuvers can help preventing accidents from taking place. One of the major causes of traffic accidents is a result of keeping no safe distance from neighboring vehicles while driving. If a system that warns drivers if they are heading too close and dangerously towards other vehicles is available, that would help reducing vehicles' accidents significantly. This system would be particularly useful in low visibility conditions, like fog and rain, which are also a cause of a large number of accidents. In this system, each vehicle can calculate its relative position and velocity to other vehicles when the transmitted position and velocity of nearby vehicles are available. So, even

if the driver has low visibility of other vehicles or if he loses concentration, then the system can issue a warning to alert the driver of possible unsafe driving conditions. In this paper, a navigation system that calculates the state of each vehicle at 50 Hz rate is proposed. Using Zigbee transceivers, this state is continuously transmitted to neighboring vehicles. In the proposed navigation system, a low-cost GPS receiver measures the position of the vehicle at a frequency of 5 Hz. Three-access accelerometers and gyroscopes measure the linear acceleration and angular velocity of the vehicle at 50 Hz. Also, encoders are used to measure forward velocity of the vehicle. Using measurements from the GPS receiver and the forward velocity constraint from encoders, the three-axes acceleration and angular velocity measurements are integrated to obtain the position and velocity of the vehicle at 50 Hz [1-5]. The position and velocity along with ID of the vehicle is then transmitted to neighboring vehicles using a Zigbee transceiver [6-8]. Fig. 1 shows a schematic diagram of the proposed system. There have been a number of studies pursued to reduce vehicles' accidents using different approaches and navigation algorithms. The use of encoders and GPS receivers along with a digital map to estimate a vehicle's state is proposed in [9]. However, such a system would be too expensive to implement and may not be applicable if a precise digital map is not available. Some researchers suggest using radar in addition to vision based systems to predict traffic accidents [10]. Such a system has the advantage of preventing collisions with other objects, like walls, pavements and pedestrians, as well as other vehicles. However, unlike the proposed solution, using a radio communication system, these systems do not provide a 360 degree field of view. In another work presented in [11], the implementation of car to infrastructure communication is suggested. This

978-1-4673-0862-5/12/$31.00 ©2012 IEEE

www.redpel.com +9176205933891

infrastructure communication can provide vehicle state. This approach is suitable for applications aided with real-time traffic data or internet connectivity. However, collision prediction communication is only required between vehicles present within the vicinity of each other. Vehicle to Vehicle communication has the advantage of not requiring an infrastructure. So, it can work in any area where all vehicles are provided with a collision avoidance system along with a compatible communication protocol. Different issues faced with the implementation of this system are discussed in [12].

In this paper, the suggested system setup is described. The fusion algorithm of the low-cost GPS, velocity constraints, and three-axis gyroscopes and accelerometers are shown. The experimental results of the estimation algorithm are verified and compared with a standalone commercial solution. Static and dynamic one vehicle experimental results are shown.

In [13], measurements are obtained using a gyroscope, GPS and a speedometer. In the solution proposed in [13], a nonlinear observer which consists of three subfilters is used in developing a vehicle navigation solution. One of the subfilters has two gains and estimates yaw angle and gyro bias. Another subfilter has one gain and is responsible for estimating the speedometer's scaling. The third one also has one gain and gives estimates of the vehicle's velocity and position. In the filtering stage proposed in [13], nonholomonic constraints are taken into consideration. When tested, the algorithm gave reliable results. However, the algorithm does not take into consideration the accelerometer bias and it also does not use quaternion approach which is known for its stability in fmding a vehicle's attitude.

Fig. 2 shows the components of the proposed system. The system is composed of an IMU/GPS unit, an encoder, a Zigbee transceiver, and a Microcontroller. The measurements of the IMU and GPS are fused to obtain an estimate of the vehicle state. The Encoder is used to measure the body frame velocity of the vehicle. The Microcontroller estimates the state of each vehicle at 50 Hz rate by using the algorithm proposed in this paper. Then, the Zigbee transceiver is responsible of communicating the position, velocity, attitude and vehicle ID to other vehicles in the vicinity through a network of Zigbee routers a Zigbee coordinator.

Also, several navigation techniques were developed recently for navigation using artificial intelligence (AI) rather than using Kalman filter to avoid inadequacies present in the approach ofKF [14-18]. Some of the main inadequacies in KF approach for GPSIINS integration are a stochastic error model for every sensor used should be predefined, and values like correlation time, variance and covariance should be known accurately. However, these AI-based techniques relate INS errors to INS output at specific time intervals only. They do not take into consideration the error dependency on past INS readings. To overcome this problem, a study proposes the use of Input-Delayed Neural Networks (IDNN) to model the INS errors based on current and some of the past INS data [19].

In the next section, the proposed system's setup is first illustrated. III.

SYSTEM SETUP

The system proposed in this work is a non-centralized system. It works when all vehicles present in surrounding area are equipped with the proposed safety system. The system should estimate an accurate measurement of car's position, velocity and attitude, and should transmit these measurements to nearby vehicles through an RF module. It should process the data received to be able to predict dangerous situations and warn the driver if such a situation arises. In the next subsections, more details are explained about the system components.

Therefore, due to the inadequacies of using ordinary Kalman filtering for fusing GPS and INS only, in this work, vehicle constraints are also fused with the GPS/INS to enhance the error estimation. Also, other than Kalman filter, an Information filter, which has a lighter computational load compared toKF, is going to be demonstrated. Figure 2: System Components

A.

Microcontroller

For development purposes, PCI04 computer is used whereas for production, use of a low cost microcontroller such as PIC is proposed. The microcontroller is capable of interfacing with a ZigBee communication module, 3-axis accelerometer, 3-axis gyroscope, encoder (or odometer) and a GPS unit. Fig. 3 shows the input and output ports of the PC104 microcontroller. Figure I: Each vehicle broadcasts its position,velocity and attitude to nearby vehicles

www.redpel.com +9176205933892

with a white Gaussian noise. These constraints are known as nonholomonic velocity constraints. Before fusing them with the GPS measurements, the measurements are rotated to the earth centered, earth fixed (ECEF) frame using the transformation matrix C:. In the sections below, coordinate transformation matrices and the dynamics of the vehicle are shown. In the equations to follow, .(1 represents the skew matrix of angular velocity of earth, P is the vehicle's position, V is the vehicle's velocity, and G is local gravitation vector. The superscript e is used to denote the vectors' representation in the ECEF frame. Figure 3: Microcontroller inputs and outputs

B.

Communication System

As mesh networking is essential for creating a non­ centralized system, ZigBee is a good choice as it has good support for mesh networking for RF communication. It consumes low power and is a relatively cheap solution for wireless mesh networking. It also has low latency, which makes it suitable for this application that is time-critical. As in this application, each vehicle only needs to know telemetry nearby vehicles; Zigbee's low range becomes an advantage. ZigBee transceivers also take care of packet collision on their own. Zigbee can support a maximum of 65000 nodes. This is enough for the low range covered by these devices. C.

Inertial Measurement

To fmd the position and velocity, GPS measurements and velocity constraints are fused with inertial sensors. Measurements from a three-axis accelerometer and a three­ axis gyroscope are integrated to find position, velocity and heading of the vehicle. This provides a good result in the short run. However, due to errors accumulation in measurement, the results soon drift away from their actual values. The GPS unit is used to correct this error using a Kalman filter. Also, an information filter is used for sensor fusion. Although the GPS unit provides a more accurate measurement of position and velocity, it provides these readings at a slower rate (5 Hz) and is not available in certain circumstances such as in tunnels and in condense urban canyon environments. Also, low-cost GPS receivers don't provide velocity measurements. Also, velocity constraints from odometers are fused with the inertial measurements suing the same filters. These constraints improve the results by constraining the velocities in two axes, y and z, in the body frame to zero. This is because in land vehicle applications, velocities in these two directions are usually insignificant. The next section of the paper talks about the algorithms used in the proposed solution. IV.

INERTIAL MEASUREMENT UNIT

Using accelerometers and gyroscopes to find velocity and position is illustrated in [1-5]. The accelerometer measures the vehicle's specific force Fb in three directions in the body frame. The gyroscope provides the vehicle's angular velocity b w . Here, the subscript b is used to denote the vectors' representation in the body frame. The encoder measures the body frame velocity in the x direction. Also, body frame velocities in y and z directions are constrained to zero mean

A.

Coordinate Transformations

The transformation from ECEF to navigation frame is given below. Note that and are the geodetic latitude and geodetic longitude, respectively.

II.

cf

=

[-Sin COSII. -sinll. -sincosll.

-sinsinll. cosll. -cossinll.

cos 1 -s�n

(1)

The transformation from body to navigation frame is given below. Note that
=

[

]

cosecostIJ sin",sinecos>/J - cos",sin>/J cos",sinecos>/J + sin",sin>/J cosesin>/J sin",sinesin>/J + cos"'cos>/J cos",sinesin>/J - sin",cos>/J (2) -sine sin",cose cos"'cose

So, after obtaining the transformation from body to navigation frame and the transformation from navigation to ECEF frame, the transformation from body to ECEF frame is calculated as:

C:

=

C�C:

(3)

These transformation matrices are used in converting the sensors' outputs to be processed ECEF frame. In the next section, the vehicle dynamics are shown. B.

Velocity and Position

The vehicle's dynamics are obtained from the measurements of accelerometers and gyroscopes. The velocity time rate of change in ECEF is given as [5]:

To find the velocity in the ECEF frame, (4) is integrated. Here, 2.(1Ve is the coriolis acceleration and .(12 pe is the Centrifugal acceleration. Ce is the gravity matrix. When the velocity is found, it can be integrated in order to fmd position using the equation below. pe

=

Ve

(5)

Due to the bias error in the accelerometers' specific force the velocity and position of the vehicle will diverge quickly from the correct position and velocity. b F ,

www.redpel.com +9176205933893

C.

Attitude

The attitude of the vehicle can be represented in terms of quaternions. These quaternions can be initialized from the transformation matrix cff as follows. Let Cij denote the i-th row and j-th column entry of this matrix. The attitude of the vehicle is initialized as [5]:

, - r:,

-wx -wy °

Wz

D.b e - Wy -wz ° Wz Wy -wx To get the Euler angles from the quaternions, the following relations are used. The matrix C is the same as the matrix cff. (13a) sine

=

-C31 (13b) (13c)

where 1> is the roll, e is the pitch and 1/1 is the yaw. In the next section, the fusion of velocity constraints is explained. D. Velocity Constraints

Then, these are assembled in one matrix as follows.

In this application, since we are dealing with land vehicles, the body-frame velocity in the y and z axes, v; and v2, are constrained to zero with white noises due to drifts. The fusion of these velocity constraints helps in improving the results of the estimated states. The body frame velocity in the x axis v� is measured using wheel encoders.

(7)

The time rate of change of the attitude of the vehicle is given in the following equation as:

where

n:b

is skew symmetric matrix of the angular

velocity of the body frame relative to the ECEF frame represented in the body frame. The quaternions are updated as follows.

z�elocitY(k)

H�elocity (k)

=

where qe is an estimated quaternion and qe is a quaternion obtained from a measured value.

This equation is represented as:

R�elocity (k)

+ "l y

(14)

"lz

[� a

=

I

[0 2

°

°

0]

(16)

°

y

a

2

(17)

°

To eliminate the diverging of pOSItion, velocity and attitude, a KF and IF are used to fuse the measurements of the GPS and the a priori vehicle's estimates obtained from the gyroscopes and accelerometers, along with velocity constraints. This is described in the next section. V.

(11)

V�(k) v;(k) v2(k)

The error observation vector between the INS readings and the velocity constraints is

(9)

The gyroscope provides angular velocity of the vehicle wib relative to the inertial frame. The angular velocity of the earth relative to the inertial frame is subtracted from the angular velocity of the body frame relative to the inertial frame to get the angular velocity of the body relative to the ECEF frame as

[ l 0 []

=

MEASUREMENTS FUSION ALGORITHM USING KF

The Kalman filter is used in order to get a better estimate of vehicle's position, velocity and attitude by using readings obtained from GPS to fix errors in the inertial estimates. The states vector used in this work is the following.

www.redpel.com +9176205933894

cl

x(klk -1)

=

Ax(k -11k -1) + B(k)w(k) (19)

Assuming all errors are removed in initial calibration of the INS, x(110) is set to zero. However, due to drift in INS, the covariance matrix, P(klk -1) changes every time step due to growth in uncertainties. Q(k) is the process noise.

P(klk -1)

=

A(k)P(k -11k -I)AT(k) + Q(k) (20)

where cj is the error in assuming small error, as

=

cj ci

(27a)

ci computation and is represented,

where 8qe is the attitude between represented as

and E frame and

E

IS

So, the error observation is updated using KF and then sent to INS. So, (21)

Using a KF the error estimate is updated every time step through the following equation.

x(klk)

x(klk -1) + W(k)v(k)

=

(22)

W(k) is a Kalman gain matrix and v(k) is known as the innovation matrix. W(k) v(k)

=

=

P(klk -I)HT(k)S-l(k)

(23)

z(k) -H(k)x(klk -1)

(24)

The matrix, S(k) , which is known as the innovation covariance is shown below.

S(k)

=

(27d) Q is the process covariance and R is the covariance of the measurements. VI.

MEASUREMENTS FUSION ALGORITHM USING

IF

Theoretically, the information filter should give the same result mathematically as that of a Kalman filter. However, it is easier to apply in cases where measurements from mUltiple sensors are fused. This is because it uses information space rather than state space. The main components of the information filter are the information state matrix and the information state vector. The information state matrix is the inverse of the covariance matrix, that is

Y(k)

H(k)P(klk -I)HT(k) + R(k) (25)

=

P-l(k) (28a)

and the information state vector, that is covariance

Then, the update of the following.

matrix is the

P(klk) (1 -W(k)H(k))P(klk -1)(1 -W(k)H(k)f (26a) W(k)R(k)WT(k)

H(k)

RGPS(k)

where 0 is a matrix.

[�

=

I

[a"�

I

PCPS

=

3 x 3

To update DCM,

0

0

0

0

0

0

0 0

+

(26b)

(28b)

Y(klk -1)

=

[A(k)Y(k -11k -I)AT(k) + Q(k)r1

(29a)

y(klk -1)

=

P-l(k -11k -I)A(k)P(k -11k -l)y(k 11k -1) (29b)

Then, the information observation vector, i(k) , that represents the contribution of the observation, is

0 0

(26c)

2

(J e vvelocity

matrix of zeros and J is a

cl becomes

Y(k)x(k)

while the predicted information vector is evaluated as

�l

2 (J e VCPS

=

The predicted information state matrix is

=

0

y(k)

3 x 3

identity

and the information observation matrix, J(k), that represents the certainty, is

J(k)

=

HT(k)R-l(k)H(k)

www.redpel.com +9176205933895

(30b)

The information observation vector and the information observation matrix are evaluated when an observation z(k) is made. After obtaining the observations, the estimates are updated

�

Tirre[s]

as

y(klk) Y(klk)

=

=

y(klk Y(klk

-

-

1) + i(k)

(3Ia)

1) + /(k)

(3Ib)

Tirre[s]

For multiple sensors, we can easily update the estimates by adding the summation of the information observation vectors and matrices of the multiple sensors used, that is

y(klk) Y(klk)

=

=

y(klk Y(klk

-

-

1) + L i(k)

(32a)

1) + L /(k)

(32b)

Tirre[s]

A. Static Test using KF without Velocity Constraints

In Fig. 4 and 5, the position and velocity, respectively, of a static test are shown. The data was treated using a Kalman filter for INS and GPS fusion only. So, in these figures, the results of the solution are not constrained with nonholomonic velocity constraints and only INS and GPS are fused together.

120

140

100

120

140

�'

100

� �

4.7541 ' 47541 I V & 47541 ,... 47541

EXPERIMENTAL TESTING

The algorithm for INS and Kalman filter and information filter were implemented in code and tested. Multiple experiments were conducted to test the performance of the proposed algorithm. First, a static test was performed. Results of this experiment should give zero velocity and constant position. This can be used for validating the accuracy of our sensors and the proposed estimation filter as a partial truth state, zero velocity and constant position, is known. The second set of data is from a dynamic vehicle experiment. The vehicle was driven around a path to get realistic data for a road vehicle. Results obtained from these experiments are shown below.

=

100

Figure 4: Velocity Estimate. Static Test without Velocity Constraints UsingKF

By fusing the dynamics of the vehicle measured by the accelerometers and gyroscopes with the GPS measurements and the velocity constraints, the errors of the position, velocity and attitude are reduced which leads to an accurate estimate. Experimental results are given the next section to verify the accuracy of the algorithm. VII.

80

475410

r=ooJ

�

20

40

60

O' 2.7104 '1

I 27104 g

� 27104

271040

Tirre[s]

80

f\

20

100

�

40

60

Tirre[s]

80

100

120

°a

=::

140

-MIOG

-solution

120

140

Figure 5: Position Estimate,Static Test without Velocity Constraints Using KF

In the two figures above, we can see that oscillations in y and z directions of velocities away from zero are present. As a matter of fact, the velocities in all directions are supposed to be equal to zero. These oscillations also reduce accuracy of position readings. Position is given in ECEF, while velocity is given in body frame. B.

Static Test using KF with Velocity Constraints

In Fig. 6 and 7, the position and velocity of the same static test are shown respectively. The data was also treated using the same Kalman filter. However, in these figures, the results of the solution are constrained with nonholomonic velocity constraints, in contrary to the previous data set, where only INS and GPS are fused together. The forward velocity is measured by an encoder and is fused to the solution. In these figures, position is given in ECEF, while velocity is given in body frame.

www.redpel.com +9176205933896

Time [51

Time [s1

j-MI -WIO�G"" ' o" : �� � _

_______

o

�

�

M

Time [51

80

100

1�

1�

���--� M --� 80----�lOO�--'�20 20----� 40'---'� 1 --�'40 Tirre[s]

Figure 8: Velocity Estimate,Static Test without Velocity Constraints Using IF Figure 6: Velocity Estimate,Static Test with Velocity Constraints Using KF

� --- - ------ =-MIOG

' 3.2685 '1O _3.2685 fo. _

f 3.2685 i

o

)( 3.2685 3.2685

v,_____.

�V\

20

-solution

�------------.. �

40

M

'

Tirre[s]

t m

M

m

47541 V I 47541 & ,... 47541

-/.JV

�...-Aj"'J'/' -/ -

'------.....

___

�

�

FMiOOl �

475410ic------,'n -Mtn-------t, , .;,00 ;;;---<1�20-----20----� 40,---' 80---140 --' Tirre[s] O 1 2.7104 , . I 2.7104 & "

��

-solution

��

2.7104� �---'1�20--�, 40 M ------:0 0 ---"' 2O----� 40.--- '� 80----�l OO Tirre[s]

Figure 7: Position Estimate,Static Test with Velocity Constraints Using KF

From Fig. 6, we notice that the estimate solution is much closer to zero than that of MIDG in x, y and z directions, since the encoder and velocity constraints are zero. C.

-'-� == � _""'�

';

3.26850

�

� � � -MIOG

4.7541 '

t 3268' 3.2685

Static Test using IF without Velocity Constraints

Here, in the following Fig. 8 and 9, the position and velocities of the same static test are shown respectively. However, here the data was treated using the information filter described in section 5 for INS and GPS fusion only. So, in these figures, the results of the solution are not constrained with nonholomonic velocity constraints and only INS and GPS are fused together. So, we can see that oscillations in y and z directions of velocities away from zero are present. This also reduces accuracy of position readings. In these figures, position is given in ECEF, while velocity is given in body frame.

20

40

60

t 47541

�� 4.7541o

Time [51

80

100

120

140

-.oc

�

�

�

M

Time [51

80

t::::( �� �

100

�1�

----"'''''"

[

1�

��"""'"=-<� 27104 2.71040!-------:O � 40�--- 6�O ----�80----�1O-0 ----�, 20--� 140 20 Time [51

Figure 9: Position Estimate,Static Test without Velocity Constraints Using IF

In Fig. 8 and 9, it can be seen that velocities in y and z directions oscillate away from zero. This also reduces accuracy of position readings. In these figures, position is given in ECEF, while velocity is given in body frame. D. Static Test using IF with Velocity Constraints

The static test data is now fused with the GPS and encoder data using the information filter described in this paper. The estimation error resulting from this integration should be bounded by the GPS measurement error and by the fact that our encoder velocity reading with nonholonomic constraints are equal to zero.

www.redpel.com +9176205933897

2

o

-20

-MIDG

0.4417

20

40

60

80

Time [s)

100

120

!E>N

140

'00.4417 � 0.4417

;

-MIDG

20

40

60

� � E====:

80

Time [51

100

120

0.4417

140

� ====,J==- o

- 0'

20

40

6�

Time [s)

' 80

100

� �

a..

120

. ' 1O. _ 47541 '-----oS "::---� 47541

40

60

Time [s)

� 20 40 60

�N 2.7104

80

� ,

2.7 '04 '0 "

Time [51

100

140

� 120

- '-____.

20

30

40

50 Time[s[

60

70

80

90

-150

10

20

30

40

50 Time[s]

60

70

80

90

100

Figure 13: Velocity Estimate, Dynamic Test without Velocity Constraints Using KF

Time [s)

� � :� !: 3.2689 '1O'

---solltlon

&. 32687

32686 326850

x

From Fig. 10, we notice that the estimate solution of velocity is much closer to zero than that of MIDG in all directions, because both encoder and velocity constraints are at zero as the unit is static_

I

::

10

20

30

40

50 Time[s]

60

70

80

'lO �::

>-4.7541 4.75410

90

100

-MIDG

---sohAion

&. 4.7542

Single Vehicle Dynamic Test using KF without Velocity Constraints

Fig. 12 through 15, show the results of a single vehicle dynamic test, where the vehicle is driven around a three quarters complete roundabout almost between the thirtieth and the fortieth second_ The vehicle had a 90 degrees right turn at after 60 seconds. Finally the car enters a parking lot by turning 90 degrees to the right. The results in the following four figures were treated using KF with no velocity constraints. As seen, overall the results from solution closely follows the results from MIDG but since no velocity constraints are implemented here, they also follow velocities deviations of MIDG from zero in y and z directions.

-MIOG

:[32688

Figure I I: Position Estimate,Static Test with Velocity Constraints Using IF

E.

10

140

r-IV'v'---'-"..A

r--�F

a

2°0

_

� =o.-,�--= """"--..." 20 80 40 60 100 120

2.71040

,''',''

.� L:'� •

'DG "='-" ---- ,,',,""

__

-MIDG

>

M'DG ---- solution

120

1�O

�--�-�--�---

_

/

__

475410 E

80

a 9685 09685 09685 09685

40 �=.20

�

_----=------------

�.--�----

09685 09685 09685 09685 a 9685 a 9685 Lamtda [rod]

Figure 12: Path Estimate of the vehicle,Dynamic Test without Velocity Constraints Using KF

Figure 10: Velocity Estimate,Static Test with Velocity Constraints Using IF

" 3.2685 ' 1O _ i 3 2685 ",32685 326850 20

�

Direction of motion

'b � 684

o4 '

_____

;:;:

� Roundabout

0.4417 0.4417

'�

2 ' 0

----r-==;=-,

0.4418 0.4418

_

0 11 �-20

90 degreesturn ___

0.4418

�

-

__----

10

20

30

10

20

30

--

50 Time[s]

� 2.71

2.70980

40

50 Time[s]

60

70

80

Figure 14: Position Estimate,Dynamic Test without Velocity Constraints Using KF

(

1000

�

�.

t

J fA � 4_ ----� _2 ====� �8�-�_6�-� O ml��=---�--

500

i 100" 1 500 [ �

xPerror[m]

�

i

" � � .? 5--_7-= --�-�17 ' �� � � . 5-�-�27 . 5-�

] �::t �

ac'liiII t

" %�--�-a �2�

3

4

zp error[m]

� ....����-

Figure 15: Position Error in body frame,Dynamic Test without Velocity Constraints Using KF

www.redpel.com +9176205933898

In the figures above, velocity constraints were not implemented. Therefore, especially when looking at Fig. 14, it can be noticed that the y and z components of the velocity estimate follow those of the MIDG, which are not very close to zero. Fig. 15 illustrates a normal Gaussian distribution of position error in body frame of the car. This is the difference of positions from MIDG and the estimated solution. From Fig. 15, it can be noticed that the position errors in the body frame are, on the average, less than 2 meter for x direction, and -0.5 to 0.5 meters for y direction. F.

��

3.2689 ' 1O 32688 :[ � 32687 ><32686 326850

[ 200 ilOOO'1[

�

0.4417 5" 0.4417 � .; 0.4417

Directionofmotion

'b � 684

04 '

0.9685

0.9685

0.9685

Roundabout

•

� 0.9685 0.9685 09 . 685 Lambda [rad]

0.9685

0.9685

09 . 685

� '�=�.I

60

-MIDG

.....-""

=.20 > , 0 -2°0

10

20

-solution

30

40

50

Tirre[s]

60

70

80

100

-MIDG

0

=. -5 > >-·10 -150 5

90

10

20

30

40

50

Tirre[s]

60

70

80

90

100

�

� Figure 17: Velocity Estimate,Dynamic Test with Velocity Constraints Using KF

50

Timers]

60

70

80

90

100

:40

--solution

_________

10

20

30

50

Time[s]

60

70

80

90

100

10

20

30

40

50

Timo[s]

60

70

Figure 18: Position Estimate,Dynamic Test with Velocity Constraints Using KF

�

�7�-6_ �-�5_ --�7--_73--72_ -�1_ �'��O--�--� xPerror[m]

l

.

2. 5

Y Perror[m]

��

__

__

zp error[m]

Figure 19: Position Error in body frame,Dynamic Test with Velocity Constraints Using KF

0.9685

Figure 16: Path Estimate of the vehicle,Dynamic Test with Velocity Constraints Using KF

� 40

40

1 1.5 05 00 !4 �200 .... 7R �db"� I _� �.���1 0 5 i ';4 q

0.4418

0.4417

30

�

2.70980

0.4418

0.4417

20

� 2 71

0.4418

____

10

-----­ l::r�

In Fig. 16 and 19, the velocity (in body frame) and position (in ECEF) estimates of the car are shown and compared against the MIDG solution using an IF with velocity constraints. It can be seen that the velocity estimate in x direction closely matches the MIDG solution. Also Fig. 18 shows that the change in position of the vehicle from its initial location closely matches the results reported by the MIDG unit.

0.4417

�

"O

>-4.7541 4.75410

Single Vehicle Dynamic Test using KF with Velocity Constraints

�

:;:::

I

� 4.7542

FMiOOl

'

As noticed from the figures above, we see that implementing the velocity constraints had improved the estimation accuracy. Given the application in consideration, it is known that a vehicle driving on road would have zero body­ frame velocities in y and z directions. Therefore, especially when looking at Fig. 18, it can be noticed that the y and z components of the velocity estimate are closer to zero than those of MIDG. From Fig. 19, it can be noticed that most position errors in body frame are less than 1 meter for x direction, and less than 0.5 meters for y direction. G.

Single Vehicle Dynamic Test using IF without Velocity Constraints

Fig. 20 through 22 show the results of the same single vehicle dynamic test. The results in the following four figures were treated using IF with no velocity constraints.

�

90 d greesturn 0.4418 -MIDG . 0.4418 ----- solLJlOn 0.4418 0.4417 90 greesturn 0'0.4417 � ;0.4417 0.4417 ____ Roundabout 0.4417 0.4417 Direction of motion 0.41r�684 0.9685 0.9685 0.9685 0.9685 0.9685 0.9685 0.9685 0.9685 0.9685 0.9685

I

�

� Lamtxla [rad]

Figure 20: Path Estimate of the vehicle,Dynamic Test without Velocity Constraints Using IF

www.redpel.com +9176205933899

50

t o�

-�oo

'�'"

_

-5°0

10

20

30

40

60

50

Time [51

80

70

90

t :�

_

1 -2°0

t :�

�

10

20

20

30

40

30

40

60

50

Time [51

60

50

Time [51

80

70

-�OO

80

70

90

_

_

a..

-------

oo

10

4.7543)( 106 47542 � 4.7541 4.7540 10

20

30

40

� � �

�::;.r· � 271 2.70980

50

Time [51

60

80

70

90

100

a.4.7541 4.7540

�

20

30

----___-

40

50

Time [51

60

80

70

90

10

20

30

40

50

Time [51

60

70

10

30

20

8�

90

100

70

80

90

100

40

50

Time [51

60

70

80

90

100

-�oo

10

30

20

40

50

Time [51

60

70

80

90

[

100

oc

'�,"

-

10

20

30

40

� :;:� � 27102 . � 271 2.70980

50

Time [51

60

70

80

90

100

___ ________

10

20

30

40

� �

:=,

60

'"� '�

2.7104X 106

______

50

Time [51

l� �

�O

1

40

Figure 24: Velocity Estimate,Dynamic Test with Velocity Constraints Using IF

- ' f3.2688 a.3.2686 3.26840

'�,"

_

30

20

-�oo

0 :;: -5 -10o

1

100

l� �

f3.2688 3.2686 3.26840

10

0

-2°0

Figure 2 1: Velocity Estimate,Dynamic Test without Velocity Constraints Using IF

- '

'�,"

1� 10

100

90

-MIDG

-5°0

100

"�= 10

100 5

50

Time [51

60

70

80

90

100

80

90

100

_____ 10

20

30

40

50

Time [51

60

70

Figure 22: Position Estimate,Dynamic Test without Velocity Constraints Using IF

Figure 25: Position Estimate, Dynamic Test with Velocity Constraints Using IF

As seen, like in KF, overall the results from solution closely follows the results from MIDG but since no velocity constraints are implemented here, they also follow velocities deviations of MIDG from zero in y and z directions.

It can be seen from Fig. 24 that since the velocity constraints are applied, velocities closer to zero are obtained from the proposed solution than those obtained from MIDG. It can be also seen that the estimate of positions in ECEF in x direction in body frame obtained from the algoritiun with the used low-cost solution closely matches the results obtained from the commercial standalone MIDG unit.

H.

Single Vehicle Dynamic Test using IF with Velocity Constraints

The same dynamic test data is now fused with the GPS and encoder data using the information filter described in section 5. The estimation error resulting from this integration should be bounded by the GPS measurement error and the fact that our nonholonomic constraints are zero. 90 degrees turn

04418 04418 0.4418 04417 :00.4417 � '; 0.4417

�

0.4417 0.4417

Directionofmotion

0.4417

'b . � 684

04 '

0.9685

Roundabout

/,.

09 . 685 0.9685 0.9685 0.9685 09 . 685 0.9685 0.9685 09 . 685 0.9685 LamlXla [rad]

Figure 23: Path Estimate of the vehicle,Dynamic Test with Velocity Constraints Using IF

VIII. CONCLUSION In this paper, a low-cost system is proposed for highway traffic safety assurance. The system uses low-cost three-axis accelerometers and gyroscopes to measure the dynamics of a vehicle. A GPS receiver is also used to reduce the drift in the estimate obtained due to the bias errors in the sensors' measurements. Velocity measurement from odometers and nonholomonic constraints are fused to improve the accuracy of the estimate. As shown in the experimental results, whenever the velocity constraints are used, the estimation accuracy is enhanced. This is especially seen in the body-frame y and z velocity estimates as they converge to zero, unlike results obtained without using the constraints which are shown to oscillate away from zero. A Kalman filter and an information filter were used in the fusion process. Both the Kalman and the information filters gave close results. However, implementing the IF is easier than the KF when using multiple sensors and is computationally less expensive. Once the state of each vehicle

www.redpel.com +91762059338910

is estimated, the estimates are transmitted to all neighboring vehicles using a Zigbee transceiver. Therefore, each vehicle calculates its position and velocity relative to other neighboring vehicles. This allows the vehicle to issue an alarm whenever dangerous situations are perceived. Experimental results were shown to verify the accuracy of the estimation algorithm. The results obtained were checked against a commercial standalone solution. The proposed solution is vital to ensure highway traffic safety in adverse environments such as a foggy or rainy weather. REFERENCES [I]

L. R. Sahawneh, M. A. AI-Jarrah,K. Assaleh, and M. F. Abdel-Hafez, "Real-Time Implementation of GPS Aided Low Cost Strapdown Inertial Navigation System," Journal of Intelligent & Robotic Systems, Vol. 6 1, No. 1-4,2011,pp. 527-544.

[2]

S. Sukkarieh, "Low Cost, High Integrity, Aided Inertial Navigation Systems for Autonomous Land Vehicles," Ph.D. dissertation, Mechanical and Mechatronic Engineering, Australian Centre for Field Robotics,The University of Sydney,Sydney,Australia,March,2000.

[3]

M. F. Abdel-Hafez. "The Autocovariance Least Squares Technique for GPS Measurement Noise Estimation," in IEEE Transactions on Vehicular Technology, Vol. 59,No. 2,January 2010, pp. 574-588.

[4]

A. Noureldin, T. B. Karamat, M. D. Eberts and A. EI-Shafie, "Performance Enhancement of MEMS-Based lNS/GPS Integration for Low-Cost Navigation Applications," IEEE Transactions on Vehicular Technology,Vo1.58,issue 3,March,2009,pp.I077 - 1096.

[5]

R. Schelling, "A Low-Cost Angular Rate Sensor for Automotive Applications in Surface Micromachining Technology," Third Annual International Conference on Advanced Microsystems for Automotive Applications Proceedings,March, 1999.

[6]

P. Belanovie, D. Valerio, A. Paier, T. Zemen, F. Ricciato, and C. F. Mecklenbrauker. "On Wireless Links for Vehicle-to-Infrastructure Communications," in IEEE Transactions on Vehicular Technology, 59(1),20 10,269-282.

[7]

P. Kinney, "ZigBee Technology: Wireless Control that Simply Works," Communications Design Conference, October,2003.

[8]

S. Biswas, R. Tatchikou and F. Dion. "Vehicle-to-Vehicle Wireless Communication Protocols for Enhancing Highway Traffic Safety," in IEEE Communications Magazine, 44(1),2006,74-82.

[9]

A. Lahrech, C. Boucher and J.-C. Noyer, "Accurate vehicle positioning in urban areas," IMTC 2007 - IEEE Instrumentation and Measurement Technology Conference, Warsaw,Poland,May 1-3,2007.

[10] L. Che-Chung, L. Chi-Wei, H. Dau-Chen & C. Yung-Hsin , "Design a Support Vector Machine-based Intelligent System for Vehicle Driving Safety Warning," in Intelligent Transportation Systems, 2008. lTSC 2008. lith International IEEE Conference on , vol., no., pp.938-943, 12-15 Oct. 2008. [II] D. Hightower. "Wireless Technology Advances Crash Avoidance," in Microwaves & RF, 2010, pp. 22 [12] J. Santa, R. Toledo-Moreo, M.A. Zamora-Izquierdo, B. Ubeda, A.F. Gomez-Skarmeta "An analysis of communication and navigation issues in collision avoidance support systems," in Transportation Research Part C: Emerging Technologies, 18 (3), 2010, pp. 351-366. [13] S. Bonnabel and E. Salaiin, "Design and prototyping of a low-cost vehicle localization system with guaranteed convergence properties," Control Engineering Practice, vol. 19,pp. 591-601,2011.

[14]

S. Sadhu, M. Srinivasan, S. Bhaumik, T.K. Ghoshal, Central difference formulation of risk-sensitive filter, IEEE Signal Processing Letters 14 (6) (2007) 421-424.

[15] P. Setoodeh, A.R. Khayatian, E. Farjah, Attitude estimation by divided difference filter-based sensor fusion, The Joumal of Navigation (60) (2007) 119-128. [16] J. Rezaie, B. Moshiri, B.N. Araabi, A. Asadian, GPS/lNS Integration using Nonlinear Blending Filters, SICE Annual Conference, 17-20 September 2007,Kagawa University,Japan,pp. 1674-1680. [17] Y.L. Zhang,F. Gao, L. Tian,lNS/GPS integrated navigation for wheeled agricultural robot based on sigma-point Kalman filter, in: 7th International Conference on System Simulation and Scientific Computing, 10-12 October,2008,pp. 1425-1431. [18] J.S. Kim, S.K. Yoon, D.R. Shin, A state-space approach to multiuser parameter estimation using central difference filter for CDMA systems, Wireless Personal Communication 40 (2007) 457-478. [19] A. Noureldin, A. EI-Shafie, and M. Bayoumi, "GPS/lNS integration utilizing dynamic neural networks for vehicular navigation," Information Fusion, vol. 12,pp. 48-57,20II.

www.redpel.com +91762059338911