Inertial Navigation and Position Uncertainty during a Blind Safe Stop of an Autonomous Vehicle

—This work considers the problem of position and position-uncertainty estimation for atonomous vehicles during power black-out, where it cannot be assumed that any position data is accessible. To tackle this problem, the position estimation will instead be performed using power separated and independent measurement devices, including one inertial 6 Degrees of Freedom (DOF)measurementunit,fourangularwheelspeedsensorsandonepinionanglesensor.Themeasurementunit’ssensorsareinitially characterizedinordertounderstandconceptuallimitationsoftheinertialnavigationandalsotobeusedinaﬁlteringprocess. MeasurementmodelsarethenfusedtogetherwithvehicledynamicsprocessmodelsusingthearchitectureofanExtendedKalman Filter (EKF). Two different EKF ﬁlter concepts are developed to estimate the vehicle position during a safe stop; one simpler ﬁlter for smooth manoeuvres and a complex ﬁlter for aggressive manoeuvres. Both ﬁlter designs are tested and evaluated with data gathered from an experimental vehicle for selected manoeuvres of developed safe-stop scenarios. The experimental results from a set of use-case manoeuvres show a trend where the size of the position estimation errors signiﬁcantly grows above an initial vehicle speed of 70 km/h. This paper contributes to develop vehicle dynamics models for the purpose of a blind safe stop.


Inertial Navigation and Position Uncertainty During a Blind Safe Stop of an Autonomous Vehicle
Mats Jonasson , Åsa Rogenfelt, Charlotte Lanfelt, Jonas Fredriksson , and Martin Hassel Abstract-This work considers the problem of position and position-uncertainty estimation for atonomous vehicles during power black-out, where it cannot be assumed that any position data is accessible.To tackle this problem, the position estimation will instead be performed using power separated and independent measurement devices, including one inertial 6 Degrees of Freedom (DOF) measurement unit, four angular wheel speed sensors and one pinion angle sensor.The measurement unit's sensors are initially characterized in order to understand conceptual limitations of the inertial navigation and also to be used in a filtering process.Measurement models are then fused together with vehicle dynamics process models using the architecture of an Extended Kalman Filter (EKF).Two different EKF filter concepts are developed to estimate the vehicle position during a safe stop; one simpler filter for smooth manoeuvres and a complex filter for aggressive manoeuvres.Both filter designs are tested and evaluated with data gathered from an experimental vehicle for selected manoeuvres of developed safe-stop scenarios.The experimental results from a set of use-case manoeuvres show a trend where the size of the position estimation errors significantly grows above an initial vehicle speed of 70 km/h.This paper contributes to develop vehicle dynamics models for the purpose of a blind safe stop.
Index Terms-Inertial navigation, autonomous vehicle, allan variance, EKF, inertia measurement unit, blind safe stop, vehicle dynamics models.

I. INTRODUCTION
T HE development of autonomous vehicles without interac- tion by a human driver is prioritized in the society, see for example [1]- [3].To reach acceptance, self-driving vehicles need to be capable of handling nearly all possible situations and circumstances.During severe failures, such as electro-magnetic disturbance, communication errors, black-out due to disturbance in the power supply etc., the vehicle must quickly be transitioned to a safe state, which in many cases is a complete stop.The control to a complete stop must be able to perform blindly without access to environment sensors such as lidar, Global Positioning System (GPS) or other equipment that provides feedback of the vehicle's position [4].
In case of a failure, the strategy could be to follow the latest known buffered path reference.Since there is no guarantee that the position measurement then is available, the control to a complete stop is suggested to be based on the estimated position from sensors protected from electro-magnetic disturbance and equipped with redundant power supply and communication abilities.These sensors typically include a inertial measurement unit (IMU) measuring acceleration and angular speed of the vehicle body.
This type of navigation without position sensors that estimates the actual position is referred to as dead reckoning or inertial navigation in the case where inertial forces due to acceleration is used.
Inertial navigation has been applied in many fields.For example for positioning of humans inside buildings [5], [6], mobile telephones [7], underwater vehicles [8], aircraft [9], [10] and land-based vehicles [11]- [16].Dead reckoning is often used at seas or by aircraft where GPS signal reception is limited.Vehicular navigation experiments have been performed at places where GPS tracking is not possible, or limited as for example in tunnels or narrow streets surrounded by skyscrapers, see [17] and [18].This kind of navigation has, however, only been practised during very limited time sequences since without exact measurements the position estimates easily drift due to sensor biases.As reported in [18] the error grows almost exponentially with the duration of the dead reckoning.Dead reckoning has also been applied together with odometry, i.e. integration of angular wheel speeds for localization purposes for robots or vehicles, which is used in e.g.[19].Odometry together with other sensor sources such as maps and/or GPS has been studied in [20]- [22] together with Kalman or other integration filters, but kinetic models for vehicles are typically not used in that field.When no feedback of position is present, the position estimation/odometry is expected to be improved by putting more effort into modelling of the vehicle.
The main objective of this work is to accurately estimate the position and its uncertainty of the vehicle using inertial navigation during a safe stop.During a safe stop, the position estimation must be accurate enough not to create any hazards such as road departure or collision with other objects.In addition to the estimated position, it is imperative to get information of This work is licensed under a Creative Commons Attribution 4.0 License.For more information, see https://creativecommons.org/licenses/by/4.0/ the position uncertainty.In case of a large position uncertainty, relative to road edge and surrounding objects, the automated driving function could reduce the vehicle's speed or prepare for a crash in an earlier stage by engaging pre-crash functions such as belt pre-tension, seat adjustments, etc.The estimator is designed using model-based techniques, and two different Kalman filters are developed and evaluated.The vehicle model used in the filters include both lateral dynamics and roll dynamics to increase its applicability.In this paper, there are three different types of sensor systems which are all assumed to have full availability and are fail-safe: 1) One six degrees of freedom Micro Electro Mechanical System (MEMS) Inertial Measurement System (IMU), which outputs acceleration in m/s 2 and angular speed in rad/s.2) Four wheel tachometers, each of them gives output of the individual wheel's angular speed in rad/s.3) One pinion angle sensor which outputs the steering angle of the front axle in rad.The filters in this work are tailor-made for dead reckoning only.Since the requirements for positioning errors are strict, especially during automated driving and blind safe stop, the objective is to quantify the position estimate errors and their uncertainties for different driving conditions.
The work is this paper has emerged from [23] written by some of the authors, but this paper includes a comprehensive description of the state-of-the-art within the inertial navigation and most references are new.The analyses are deeper e.g. the difference of the results using the two filter concepts.This paper also includes new parts, e.g. a section about odometry.New figures and tables are added e.g.Fig. 16 and Table X.
The main contribution of this paper is the development and evaluation of two different estimators and the quantification of the positioning errors.The work attempts to answer the question of what position accuracy that is possible to achieve.Note that it is the analysis of the safe stop use-case that is in focus and not the development of well-known Kalman filters.The result from this work could be used to understand if it is a realistic strategy to perform the safe stop based on dead reckoning.As far as the authors know, there are no publications yet on this topic.Having said that, the authors have identified two gaps from previous work.The first gap is the development of vehicle dynamics models used for a blind safe stop.The models used in this paper include kinetic and kinematic algorithms for roll and pitch motion, lateral speed as well as a steering system.The second gap is the analysis and evaluation of a safe stop.
The paper starts in Section II with characterization of the IMU sensor's performance and analysis of the sensor data based on the Allan variance.Next, the angular wheel speed and pinion angle sensors are characterized.After characterizing the sensors, an introduction into odometry is presented in III.Further on, vehicle kinematics and dynamics are derived in Section IV.With the known sensor characteristics and the vehicle dynamics models, two filters are designed in Section V for filtering the sensor data in order to obtain the position estimates and their uncertainties.Real world experiments in a test vehicle are described in Section VI together with an analysis of the two filter concepts

II. SENSORS
To increase the applicability of inertial navigation the sensors need to be characterized and dynamics of the object needs to be described.In this section, the sensors used during the dead reckoning process are presented, it also describes how they are characterized for the development of a filter application.

A. Inertial Measurement Unit
An IMU is a device reporting acceleration (a x,y,z ) and angular speed (ω x,y,z ) along and around orthogonal axes.The technique for MEMS IMU is based on micro mechanics that are affected by inertial forces.When three accelerations and three angular speeds are reported, the IMU is referred to as a 6-DOF IMU.
Since position estimation implies an integration of a noisy signal, there is a need for understanding the disturbance characteristics of the IMU.The type of disturbances from the accelerometers and gyroscope are similar.The dominating disturbances are a constant bias and a thermo-mechanical noise.The latter fluctuates fast and typically faster than the sampling rate and causes white noise.In addition, there is a random flicker noise from components that results in bias drift.The error due to integration of a noisy signal is referred to as Random Walk (RW) and drift of the bias is mentioned Bias Instability (BI).
In order to find the IMU characteristics, data had to be collected in an experiment during a period of approximately 1 hour with a sampling rate of 100 Hz.The IMU was placed on a flat surface and was not exposed to any force disturbances except from gravity.It should be mentioned that specific standardized test for measuring IMU performance, such as IEEE STD 952 [24], has been developed.The IMU test in this work has been inspired by the standard but doesn't follow it.
The gathered data was then analyzed to be able to find characteristics for each axis of each sensor (see Tables II and III).Comparisons with the given data sheets for the experimental vehicle, see [25], were also made to find out if the results were reasonable and within the specifications given by the distributors, see Table I.Note that the actual test IMU has no pitch rate sensor, consequently pitch rate could not be measured.To scrutinize the IMU characteristics, Allan variance and plots were produced for each axis of the IMU.The resulting plots with the Allan deviation calculated for two independent sets of measurements are shown in Figs. 1 and 2. From the Allan deviation plots it is observed that the random walk starts to influence the variance when the window sizes exceed 30 s for the accelerometers and 80 s for the gyroscopes.Since the duration of a safe stop doesn't exceed approximately 30 seconds, the random walk will likely not have an effect on the bias in this application.Another interpretation of the Allan deviation plots is that offset compensation needs to be done at a 30/80 s interval for the accelerometers/gyroscopes (with 100 Hz sampling).Table IV shows the RW and BI data based on the Allan deviation plots.

B. Angular Wheel Speed Sensors
Wheel individual speeds are measured by tachometers located at each wheel.The tachometer works by measuring the time to pass a segment of a rotating ring, the segments commonly being referred to as teeth.From the measured time and the number of teeth of the ring, the wheel angular velocities are obtained.By multiplying the rotational velocities with the mean radius of the wheels, the forward velocity is derived in m/s.The radius of the wheel is assumed to be continuously estimated in the self-driving vehicles during normal conditions.During a safe stop the last estimated wheel radius before the severe failure will be used during the manoeuvre.
1) Characterization of Angular Wheel Speed Sensors: By using the difference between the left and right angular wheel speed at the front axle, the 2-σ variance of the angular wheel speeds are calculated without being influenced by possible changes in vehicle speed.Since the front left and front right angular wheel speed sensors are independent, the variance of the difference of the two random variables is equal to the sum of each of their variances.The histogram in Fig. 3 shows that the angular wheel speed difference resembles a Gaussian distribution.The angular wheel speed signals also have a clear quantization of 0.0078 rad/s.
The estimated variance of the front angular wheel speed sensors was estimated to approximately 6.3 • 10 −4 rad/s.It is assumed that all four angular wheel speed sensors have similar variance.

C. Pinion Angle Sensor
The steering pinion is a component in the steering system below the steering column, and the pinion angle is approximately equal to the steering-wheel angle.Since the steering torsion bar has a low torsional stiffness, the pinion angle sensor is more accurate than the steering-wheel angle sensor.

1) Characterization of the Pinion Angle Sensor:
The mechanical gain between front steering angle and pinion angle is seen in Fig. 4. As long as the front steering angles are small, here approximately within the range [−15 • , 15 • ], the gain is constant and has a value of 16.75.This means that the pinion angle is 16.75 times larger than the front steering angle.To determine the noise of the pinion-angle sensor the vehicle was traveling straight ahead with a constant speed while logging data from the pinion-angle sensor.The pinion angle sensor variance was determined to 9.4 • 10 −5 rad 2 and is quantized with a quantization step size of 9.8 • 10 −4 rad.

III. ODOMETRY
Odometry is the method where angular wheel speed sensors are used solely to compute position relative to a starting position, see for example [19], [26], [27].Measuring the rear left and rear right side angular wheel speed, ω RL,k and ω RR,k respectively at a time instant k with a sampling time of T s , the longitudinal vehicle speed v x,k is expressed as where R whl is the tyre radius.The vehicle yaw rate ω z,k is expressed as where w is the vehicle track width.The vehicle yaw angle ψ k is further obtained where the vehicle longitudinal and lateral position, X k and Y k respectively, are computed as To illustrate the odometry principle, angular wheel speed sensor data has been logged to determine the vehicle position by using ( 1), ( 2), ( 3), ( 4), ( 5) and vehicle parameters according to Table V.The result from a manoeuvre is seen in Fig. 5.
To get an understanding of the accuracy of the odometry, a reference system RT3000 Inertial and GPS Navigation System from Oxford Technical Solutions Limited, referred to as ref, has also been added in Fig. 5.The example illustrates that the odometry gives an unacceptable large drift in position.As a comparison the vehicle's inbuilt standard IMU has been used to compare the yaw rate accuracy between measured yaw rate from IMU and estimated from the odometry.The results clearly shows, for this specific example, that yaw rate from IMU is more accurate than from the odometry.
The odometry suffers from systematic errors, such us tyre radius uncertainty, giving bias.There are also random errors, from angular wheel speed sensor noise, which worsen the precision.In addition, there are physics that are not modelled, where two dominant mechanisms are tyre longitudinal slip and body sideslip.The first mechanism, gives error when wheels are disturbed by torque or force and the second mechanism gives error due to the vehicle lateral speed/drift not is taken into account.Having said that, the odometry has potential to be improved when the physics is modelled, which will be scrutinized in Section IV.

IV. VEHICLE DYNAMICS MODELING
In this section the vehicle motion is modeled in order to describe the kinematics and kinetics of the real world vehicle to a sufficiently high level of detail.Kinematics is the branch of physics that studies the relation between motion variables such as position, velocities, angular rates, etc., while kinetics is the study of forces that cause motion e.g.Newtons second law.
The reference body-fixed coordinate system, from now referred to as the vehicle coordinate system, is defined in the vehicle with the reference axes and rotation angles as shown in Fig. 6.

A. Rotational Motion
One way to describe an object's orientation in space is by using Euler angles [28].The Euler angles are in this paper used to define the orientation of a body relative to a non-moving inertial coordinate system.Note that among different fields different conventions may exist.In e.g.robotics the North-East-Down (NED) reference frame is used, which differs from the convention commonly used in the vehicle dynamics field.The coordinates of the inertial system and vehicle system are denoted X, Y, Z and x, y, z respectively and follows the ISO 8855 standard [29].Any objects orientation can be described by a composition of rotations in a specific order around three axes.Rotations around the x, y and z axis are called roll (ϕ), pitch (θ) and yaw (ψ).The reference direction of the inertial-fixed coordinate system is selected such that the X-Y plane is parallel  to the sea surface and the Z axis is parallel to the gravity field and has a positive reference direction upwards.The IMU is in this work assumed to be perfectly aligned during installation, and in turn, aligned with the coordinate system of the vehicle.
The model for the accelerometers [30] are expressed as Where a x,y,z is the measured accelerations from the accelerometers.The time derivatives of the velocities in the vehicle coordinate system is denoted vx,y,z .

B. Angular Vehicle Velocity to Euler Angular Velocity
The relation between angular velocities, ω x,y,z , in the vehicle's coordinate system and the Euler angular velocities φ, θ, ψ is given, as in [31], by

C. Position in the Inertial System
Using the vehicle's lateral and longitudinal velocities, v x , v y , combined with the vehicle's heading, ψ, the positions X and Y are found in the inertial coordinate system as In robotics and air craft industry, the inertial frame has commonly reference directions such that one of the axis is pointed to north.In the application of a safe stop such a definition is not necessary.Instead the origin and reference directions are determined when the dead-reckoning starts.When it starts, the origin and orientation of the inertial coordinate system is defined as follows: the vehicle Center of Gravity (COG) defines the origin and X-axis is aligned along the vehicle center line and forward directed and the Y -axis directed to the left of the vehicle.

D. Vehicle Body Roll Angle
Due to the vehicle's suspension system, the roll and pitch body angles are generated due to forces acting on the body.As an example when the vehicle is driving on a road with a lateral banking (ϕ r ), the gravitational force will yield the body a roll angle (ϕ b ) as seen in Fig. 7.Note that the Euler angle is given by ϕ = ϕ r + ϕ b .In the same way an acceleration or deceleration of the vehicle will yield the body a pitch angle.
The IMU accelerometer values in (6) will have gravitational components with contributions from both the road angle and the body angle.When performing an IMU based dead reckoning, only the road angle is desired and the extra influence of the body angle needs to be handled.
Vehicle experiments with a high fidelity vehicle model used in the pre-development phase were performed to evaluate how large the body pitch and roll angle were exhibited during accelerations, (±5 m/s 2 in x-direction and ±2 m/s 2 in y-direction).In Fig. 8 the steady-state relation between body roll-angle and lateral acceleration is illustrated.As seen, the body roll-angle does not exceed ±0.8 • for normal lateral acceleration levels and the relation could be considered as linear.

E. Lateral Bicycle Models
The bicycle model is a well established simplified vehicle model which assumes that the dynamics and kinematics of the vehicle can be described by a bike model [32], i.e. having only one front and one rear wheel, as shown in Fig. 9.
An accurate dynamical model also requires a description of the relation between the lateral tire force and the side slip.This relation is illustrated in Fig. 10, which shows a simulation of the lateral tire force as a function of the side slip angle for the rear wheel with tyre data from the a tyre supplier.As shown, the relation is linear for small slip angles, and is described by for the front and rear axles, where the cornering stiffness for each tyre is denoted C f and C r and the factor 2 in ( 9) is due to the presence of two wheels at each axle.
Using the bicycle model and the lateral tire force model approach it is possible to derive the two different dynamical models used in this work, as described in this section.
1) Steering Angle Based Bicycle Model: According to the bicycle model concept [32], the lateral motion is expressed as where vy and ψv x are the transverse and centripetal components of the lateral acceleration.The front and rear lateral axle forces are denoted F yf and F yr respectively and the force from road banking is denoted F bank .
The yaw moment equlibrium around the z-axis in the vehicle yields the following equation where l f and l r are the distances from the vehicle COG to the front and rear wheel axles, respectively.The vehicle's yaw inertia is denoted I z .
As discussed, the tire forces F yf and F yr are at small slip angles described by (9).If the vehicle is front wheel steered the expressions for the front and rear slip angles are given by where the steering angle δ is the angle between the wheel hub and the center line of the vehicle.The front and rear body slip angles θ V f and θ V r are calculated according to One concern with the steering angle based bicycle model is the singularity due to v x in ( 14), which will cause the terms to go towards infinity as v x → 0. To overcome this problem a constant velocity model is applied when |v x | < 1 m/s.Another risk using the steering angle based bicycle model is the assumption of linear tire dynamics in (9).When performing sharp turns, it might be possible to saturate the lateral tire force and enter the nonlinear slip region.In addition to an absolute error, this will also result in a phase error of the output estimations (linear acceleration and yaw acceleration), see [33].This phase shift can result in a model response for turning manoeuvres that is too fast compared to reality.
2) Acceleration Based Bicycle Model: An alternative to the steering angle based bicycle model is a model based on lateral accelerometer input instead of front steering angle, which can be derived from [32].The tire slip angle at the rear axle is given Fig. 8. Vehicle body roll and pitch angle due to longitudinal and lateral accelerations.as where the lateral velocity of the rear tires is denoted v yr .By using ( 9) and ( 15) result in an expression for the lateral velocity at the rear axle as The total expression for the lateral velocity, but transferred to COG by adjusting for yaw rate, is now The only unknown in this equation is the rear axle force F yr , which can be expressed with help from Newton's second law and the accelerometer data in y-direction, a y .Force equilibrium along the y-axis and moment equilibrium around the z-axis are formulated according to By using the small angle approximation for the steering angle, the following expression for the rear axle force is solved as where all variables are known except ψ which can be obtained by differentiating the yaw rate, ψ.Combining with (17) results in an explicit solution for the lateral velocity according to that easily can be calculated, given the longitudinal velocity and the IMU data.Compared to the steering angle based bicycle model, this model has the advantage of not exhibiting the aforementioned phase shift compared with the ground truth lateral velocity.Equation (20) has been used in e.g.[34] for the purpose of vehicle side-slip estimation.

V. FILTERING DESIGN
This section connects the previous sections together by utilizing the work with sensor characteristics and vehicle dynamics models to design a filter.Since the process model is nonlinear, it has been necessary to use a filter that can handle nonlinearities.The Extended Kalman Filter (EKF) is used, since it is one of the most commonly used and popular nonlinear filters, see [35].
The section first gives some background theory on Bayesian filtering which is used to fuse all the data together.Next, the development of two different filter concepts are then introduced.The two concepts use the sensor data in different ways and apply the different dynamical vehicle models derived in Section IV.

A. Extended Kalman Filter
The Extended Kalman Filter (EKF) is a further development of the Kalman filter used for non-linear systems.Since the EKF is relatively simple to implement it is widely used for non-linear problems.The same equation as for the KF are used, but the process and measurement models are linearized.The linearization can be done, according to e.g.[35], by using a first order Taylor series expansion around the previous updated state prediction xk−1|k−1 .However an optimal filter is then no longer maintained as described in e.g.[36].
When Gaussian and additive process and measurement noise are assumed, the EKF model can be expressed as where f k−1 (•) constitutes the process model function and is the Gaussian process noise, y k R m are the measurements and r k ∼ N (0, R k ) is the Gaussian measurement noise.
The model prediction step for the EKF is given by where is the linearized process model at xk−1|k−1 .The measurement update step is expressed as where the linerization here is done at xk|k−1 instead.

B. Selection of Filter Design
Two different filter designs C1 and C2 are here developed.Filter C1 is less complex than C2.The C2 filter includes the steering angle based bicycle model e.g.steering is modelled.The state vector in the C2 filter does also include the time derivatives of both velocities and Euler angles, which is expected to give more accurate results for aggressive manoeuvres.For example aggressive steering in a demanding lateral manoeuvre is expected to result in large yaw acceleration.Since the C2 filter contains more physics capturing e.g.yaw acceleration, the model uncertainty could reduce the expected improvement in accuracy.To understand if it is worth to include the higher complexity of the C2 filter, both filters are developed and, later on, tested and validated.

C. Filtering Concept 1
In the first filter, Concept 1 (C1), the accelerations and angular velocities from the IMU are used as input signals to the process model.The measurement model uses the angular wheel speed sensors along with the acceleration based bicycle model described in Section IV-E2 as measurements.The state vector is then given as The filter inputs are given by and the measurements as The superscript "accelerometer based" used in the measurement vector has here been added to address that the accelerometer based model, described in (20), has been applied to calculate the lateral speed.
The derivative of the state vector is obtained by using the kinematic equations ( 8), ( 6) and ( 7) described in Section IV-C, which yields the following equations A forward Euler method is used to discretize the model with a sampling time of T s , which results in the discrete process model equations according to Due to the non-linear equations, a Jacobian has to be calculated to perform the filtering process.The calculated Jacobian for this concept is presented in [23].
The measurements has a linear relation to the state vector in (24) and the following matrix is given: The parameters in the process matrix Q k are tuned by using trial and error, while the parameters in the measurement noise matrix R k are retrieved from the noise measurements described in Section II.

D. Filtering Concept 2
In the second filter, here referred to as Concept 2 (C2), the state vector is augmented to be given as comprising states for inertial longitudinal-and lateral-positions respectively, velocity and acceleration as well as the roll-, pitchyaw-angle and their time derivatives.The input to the process model is the pinion angle sensor measurement δ, which is applied in the process equations together with the steering angle based bicycle model, described in Section IV-E1, This concept uses measurements from the IMU and angular wheel speed sensors for the measurement vector according to In addition to the lateral bicycle model which describes the vehicle's lateral velocity and heading, two other models are used in the process equations to describe the longitudinal and lateral movement and the evolution of roll and pitch; the constant velocity and constant acceleration models.For the constant velocity model the velocity is assumed to be constant ignoring noise.When assuming the state vector is formed as x = [p v] T , where p is the position in one dimension and v is the velocity for the constant velocity model, results in In the constant acceleration model the acceleration a is assumed to be constant instead.Forming a state vector as x = [p v a] T , it can be described as For both models q is assumed to be a Gaussian noise that acts on the velocity in the constant velocity model and on the acceleration in the constant acceleration model.Given these models, the continuous state evolution process equations can be set up as follows To use this time domain state space representation in the filtering process the equations have to be discretized.The resulting discrete state process equations are Also in this case the process equations are nonlinear and the resulting Jacobian needed for this concept is presented in [23].
The measurement model matrix is formed using the kinematic relations described in Section IV.The measurement from the angular wheel speed sensors is used by dividing longitudinal velocity by the wheel radius R whl .
Note that the time index k is left out to get a shorter expression.When using this concept there is a need for a Jacobian due to the matrix non-linearity.The parameters in the process matrix Q k are tuned by using trial and error, while the parameters in the measurement noise matrix R k are retrieved from the noise measurements described in Section II.

E. Treatment of Sensor Data
The knowledge that the vehicle will decelerate during a safe stop was used when treating the angular wheel speed data.When a vehicle brakes the wheels might lock but will never rotate faster than the actual velocity.This fact made it favourable to use sensor data from the wheels that had the highest rotational velocity.To also take into consideration that during a turn the inner wheels rotate slower than the outer wheels, the mean of the left and right hand side of the wheels with highest rotational speed was used as the speed signal in the filter process.The following expression, that has been developed in this paper, explains how the angular wheel speed is selected among the wheels such that where the subscript of ω is either F L = front left, F R = front right, RL = rear left, RR = rear right.Note that the longitudinal slip is neglected, e.g. during braking the angular wheel speed becomes lower.The reason for neglecting the longitudinal slip is the tire uncertainty since slip is dependent on the tire's longitudinal stiffness.Adding slip in the measurements models would mean an increased dependence on tire parameters.

VI. EXPERIMENTAL TESTS
In order to evaluate the two filter concepts, data is logged at a proving ground using a real world vehicle.In this work a Volvo XC90, henceforth only referred to as the vehicle, will be used for experiments as well as for developing vehicle dynamics models.

A. Measurement Setup
A reference system RT3000 Inertial and GPS Navigation System from Oxford Technical Solutions Limited, mentioned to as RT3000, has been used for ground truth measurements.The RT3000 includes a 6-degree of freedom IMU and a highprecision differential GPS.The measurement data from the reference system is post processed in order to provide data at the COG position.
Raw data from the IMU, angular wheel speed sensors and pinion angle sensor are logged from the vehicle with 10 ms sampling time and not pre-processed in any way before the filtering process.Sensor data is for example not bias compensated.It should also be mentioned that all sensors are suffering from a non-deterministic transport dead time due to communication delay in the order of 20-50 ms in the vehicle's Controller Area Network (CAN) buses.Since the test vehicle is factory equipped with a 5-degrees of freedom instead of a 6-degrees of freedom IMU, the RT3000 is used to generate the missing 6th-degree (pitch-rate).
The vehicle's inbuilt GPS has also been logged as a reference for position.It is not used as input to the filters, but it is here used to demonstrate the accuracy of a standard single GPS receiver.Note that this GPS has a 1 s sampling time.
The Kalman filters are both executed with a 10 ms sampling time (T s ) and the vehicle parameters are set according to Table V.The identification of the vehicle parameters has been done by using data from the tyre supplier (tyre cornering stiffness and wheel radius), vehicle Computer-Aided Design (CAD) data (distances, inertia) and weight measurements (vehicle mass).The intention has been to set filter parameters as close as possible to the real nominal vehicle parameters.Vehicle parameters will vary depending on for example vehicle load, ambient temperature, etc., and in turn, a deviation of filter accuracy could be expected.To increase robustness, most parameters could to some extent be estimated online, see for example [37].The robustness issue is however out of scope in this paper.

VII. RESULTS
In this section the evaluation of the two estimator concepts are presented and discussed.

A. Definition of Estimation Errors
The two filter concepts and tuning parameters are compared for the different driving scenarios via the longitudinal, lateral, and heading errors, e lon , e lat , and e ψ defined according to Fig. 11.
The errors e lon and e lat are thus given by the coordinates of the actual position (X, Y ) in the coordinate system of the estimated position ( X, Ŷ ), rotated by the angle ψ such that

B. Example Manoeuvre: Cornering
In this case the vehicle is manually steered to follow a curved road initially traveling at 90 km/h or 120 km/h and braking to full stop in the curve.The estimation errors using the RT3000 reference for the two filter concept are shown in Table VI.
In Fig. 12 the position estimates are shown and also the 2-σ standard deviation ellipses (from the process noise) for each filter concept at the full stop for the case when driving initially at 120 km/h.The ellipse represents the area associated with a likelihood of 95% staying within it at full stop.The most trustworthy position reference is the RT3000 instrument "Reference (RT3000 GPS)".As a second reference, the speed and angular data from RT3000 is integrated by (27) to get the reference position "Reference (RT3000 Integrated)".As can be seen from Table VI, C2 has less lateral position error than C1 (C2 has 15% less lateral position error than C1).From Fig. 12(b) it is seen that the 2-σ standard deviation ellipse of the C2 filter is larger than the one from C1. Noteworthy, the lateral position errors and also the uncertainties are large compared with the lane width of a conventional highway.The large uncertainty indicates an unacceptable low precision jeopardizing vehicle safety.The Xand Y -position estimates are separately shown in Fig. 13 together with their respective 2-σ standard deviation as time evolves.As seen the standard deviation keeps growing throughout the whole estimation process which indicates that the duration time of the safe stop has a significant effect on the position uncertainty.
The estimated states for speeds are shown in Fig. 14.It is noticeable that the lateral speeds differ between the two filters, where C2 is more correct.For the longitudinal velocity they seem to perform similar.Estimated states for the Euler angles and their rates are shown in Fig. 15.The states follow the reference rather well, it is just for the roll-and pitch angle that the estimates wander with time.As seen from the experiment, the standard inbuilt GPS receiver "Reference (1 Hz GPS)" performs worse than the dead reckoning filters.This indicates that the fusion with a standard GPS will not be sufficient for the dead reckoning problem.

C. Estimation Errors From Experiments
In Section VII-B, a cornering manoeuvre was tested in an experiment.In this section, the experiment is extended to also include a safe stop manoeuvre when driving straight ahead, with and without road inclination, and also a single lane change.Together with the cornering manoeuvre, these maneuvers constitute a condensed set of a many common driving conditions.In particular the single lane change is important when the safe stop is intended to move the vehicle to a safe lateral position such as a road shoulder.In all manoeuvres, the deceleration was controlled to approximately 5 m/s 2 until the vehicle was at standstill.The road surface was asphalt with high friction.
The results for the different manoeuvres are presented in Tables VII, VIII and IX.
For all driving scenarios, it is observed that the longitudinal errors are negative, which means that there is a systematic   overestimation of the longitudinal stopping distance.This behaviour could readily be explained by an inaccurate value for the wheel radius or a constant bias in the accelerometer input.The latter effect could be minimized using an efficient offset compensation.
A couple of observations can be made from the results for the straight forward driving cases.The results clearly show that position errors increase with increased speed.Another observation is that the overestimation of the stopping distance increases in the uphill slope.This behaviour can most likely be explained by longitudinal tire slip, which has not been taken into account in this work.
To illustrate all lateral position errors, Fig. 16 shows their magnitude for all test in this paper.As seen, a general trend is that the errors are speed dependent.As long as the initial speed is kept below 70 km/h, the lateral errors are less than 1 meter.Above 70 km/h we observe large errors of a couple of meters.
The 2-σ lateral position uncertainty are summarized in Table X.The lateral manoeuvres give larger uncertainties than  From the experiments it is also obvious that the cornering and lane change manoeuvre are more demanding and give lager lateral position errors than straight ahead driving independent of road inclination.The lateral position errors are however less when using the C2 filter than the C1, which strengthens the hypothesis that including the time derivatives of velocities and Euler angles in the state vector is beneficial for demanding lateral manoeuvres.The cornering manoeuvres excites lateral speed and body roll angle, and as seen from Fig. 14(b) and Fig. 15(a), those states are hard to estimate correctly.In particular the lateral speed, which is tire dependent.Noteworthy, the C1 filter exhibits large lateral speed errors.
One concern in this paper is the validity of the experimental results due to too few tests.In [23] the C1 and C2 filters were tested in a virtual vehicle environment.Some of the driving scenarios, e.g. the straight forward scenario, was repeated 1000 times in order to investigate the distribution of estimation errors.The result from the repeated tests demonstrated that the accuracy and uncertainty in general were better for the C2 filter than the C1 filter.

VIII. CONCLUSION AND FUTURE WORK
This work has proposed designs for and evaluated two different extended Kalman filter concepts for inertial navigation with different complexities.Both filters estimate vehicle position and position uncertainty.The concepts have however a similar performance.The most complex filter C2 is developed to tackle aggressive manoeuvres which contain driving with higher order derivatives of motion states.
The main conclusion in this paper is that it is possible to use dead reckoning without any GPS or similar absolute position reference for position estimation of a vehicle during a limited time span under a safe stop.Reasonable position estimation errors for the dead reckoning process, when the vehicle has come to a full stop, are in the order of 3 m longitudinally and 0.75 m laterally.The results from the experiments show that all driving with initial speeds at 70 km/h or below fulfill those fictive requirements.Larger initial speeds would increase the time for the safe stop and thereby increase position errors.Since the 2σ lateral position uncertainty is in the order of a lane width, the precision could be deemed as low.
Low offset bias of IMU is important to reach high precision of the position estimates.This work has shown that the sensor bias of the IMU will, however, not wander significantly during the time of the safe stop which makes it unnecessary to bias compensate during that time.However, if the methods are to be used for longer time spans, bias compensation of the sensors are necessary, which is a suggested future work.
The work has shown that the C2 filter concept is beneficial for lateral manoeuvres such as cornering and lane change.As a whole, the C2 filter is preferable to C1 due to its better performance.Since the vehicle parameters were known in this work, it is recommended as a future work to study the robustness of the concepts for uncertain vehicle parameters.

Fig. 4 .
Fig.4.The actual ratio between the pinion angle and the front wheel angle (blue solid line) compared to a linear approximation (red dotted line).

Fig. 5 .
Fig. 5. Odometry using angular wheel speed sensors solely for the purpose to compute position (odo), together with yaw rate from the vehicle's inbuilt IMU (IMU) and a reference RT3000 instrumentation (ref).

Fig. 7 .
Fig. 7. Lateral acceleration resulting in a vehicle body roll angle.

Fig. 9 .
Fig. 9.A visualization of the conceptual bicycle model.

Fig. 10 .
Fig. 10.The lateral force-slip characteristics at the rear axle.

eFig. 11 .
Fig. 11.The defined estimations errors together with the trigonometry to calculate the errors.

Fig. 12 .
Fig. 12.The estimated Xand Y -positions at 120 km/h for filter C1 and C2 including three different references.(a) The estimated path.(b) 2-σ standard deviation ellipses of position.

Fig. 13 .Fig. 14 .
Fig.13.The estimated Xand Y -positions with standard deviations at 120 km/h.Note that the 2-σ standard deviation is plotted with positive as well as negative sign, which explains there are six curves but only four legends.

Fig. 16 .
Fig. 16.An overview of the lateral estimation errors for different vehicle speeds for all manoeuvres in the experiments.

TABLE III MEASURED
MEAN AND VARIANCE OF THE IMU Fig. 1.The Allan deviation plot from two sets of independent measurements of accelerometers.

TABLE VI ESTIMATION
ERRORS (ACCORDING TO SECTION VII-A) FROM FILTER CONCEPT C1 AND C2 WHEN PERFORMING A TURN

TABLE VII ESTIMATION
ERRORS (ACCORDING TO SECTION VII-A) FROM GATHERED DATA WHEN TRAVELING STRAIGHT FORWARD

TABLE VIII ESTIMATION
ERRORS (ACCORDING TO SECTION VII-A) FROM GATHERED DATA WHEN TRAVELING UP-AND DOWNHILL

TABLE X ESTIMATED
LATERAL POSITION UNCERTAINTY AT FULL STOP straight ahead driving.The average uncertainty for the manoeuvres in Table X is ±3.4 m for the C1 filter and ±3.6 m for the C2 filter.The uncertainty from both filters are in the order of a lane width.