Observability and Performance Analysis of Velocity Measurements with Lever Arm Aided INS †

In most autonomous vehicles the navigation subsystem is based on Inertial Navigation System (INS). Regardless of the INS grade, its navigation solution drifts in time. To avoid such a drift, the INS is fused with external sensor measurements. Recent publications show that the leverarm, the relative position between the INS and aiding sensor, has influence on the navigation performance. Most published research in this field is focused on INS/GNSS fusion with GNSS position updates only where performance and analytical observability analysis were made to examine the consequence of vehicle maneuvers on the estimation of the lever-arm states. Yet, besides position updates, a variety of sensors measuring the vehicle velocity vector are available including GNSS and a Doppler velocity log. As in position measurements, when performing INS/velocity measurements fusion, the lever-arm must be taken account for. In this paper, performance analysis for velocity measurements with lever-arm aided INS is made for different maneuvers. Two error-states models are used in the analysis. Simulation results show the sensitivity of the error-states to lever arm and vehicle maneuver.


Introduction
Navigating air, sea and land vehicles using multi-sensor navigation systems has been subject to increasing interest in the literature.From the overgrowing need for high precision and reliable navigation systems and through the diverse potential for independence and automotive platforms, navigation based on multi-sensor fusion will allow to utilize the advantages of each individual sensor to optimize the process performance [1][2][3][4].
When performing this multi-sensor integration, one must take into account the effect of leverarm, which is usually referred to the relative position between the sensors mounted on the vehicle.Publications show that the lever-arm has influence on the navigation performance and observability, of the integrated navigation system [5][6][7].
So far, however, there has been no discussion about the lever-arm effect in the case of velocity measurements update.Nowadays, a variety of velocity measurements sources are available and may be used to enhance navigation, especially in areas were GPS measurements are absence [5].In this paper, the observability of lever-arm aided INS with velocity measurement is tested.Software simulations conducted to find the observable and unobservable subspaces for different maneuvers.Such a derivation is made for the 12 errors state model, where the position and lever-arm errors are not included in the error state, and additionally to the 15 errors state model, where the lever-arm error is included with three more error-state.

Kinematic Equation of Motion
In this section the navigation equations of motion and error-state model used in this study are introduced.In both models we omit the position states since they are not observable from velocity measurements [8].For convenience, the local navigation reference frame was chosen to represent the velocity and attitude misalignment while the accelerometer and gyro biases are represented in the body reference frame.The navigation equations in the navigation frame [9,10]: where and are the velocity and gravitation vectors represented in the navigation frame, is the transformation matrix from body to navigation frame, and Ω are the specific force and the skew-symmetric matrix of the angular velocity vector, respectively represented in the body frame.The notations used for reference frames are: i-frame (inertial), e-frame (earth fixed), b-frame (body) and n-frame (navigation).

Error States Models
The errors mechanization model for the navigation equations was considered as follow: where and are the velocity errors and attitude misalignment vectors respectively, × is the skew-symmetric matrix of , and are the specific force and gyro measurements, and are the accelerometer bias and random noise, and are the gyro bias and random noise, and is the lever-arm elements expressed in the body frame.The system dynamics can be written as where is the error-state vector, is the system matrix, is the system noise vector and G is the system noise distribution matrix.Two error state models are implemented and compared as addressed in the following sections.

12 Error State Model
The 12 error state model consists of the velocity, attitude, accelerometer bias and gyro bias error states.In the 12 error state model the lever-arm error is omitted and considered to be known and constant.This model is suitable when lever-arm elements estimation is done with a high confidence.In that case, the expected STD for the other elements in the vector should decrease.When assuming constant biases, the corresponding linear model is given by [11]: = 0.
The error-state vector is then and the system dynamics in (8) can represent by

15 Error State Model
The 15 error state model contains all errors from the 12 error state model augmented with the lever-arm error-states, (which is now not considered to be constant).This model is suitable for situations where lever-arm elements are hard to evaluate.The model shares the same navigation equations and dynamic linear model as in the 12 error state model, with the addition of terms regarding the lever-arm error.
Let the additional lever-arm elements states modeled as constants Thus, the error-state vector is then and the system dynamics in (8) can represent by

Velocity Aiding
Consider a single level-armed velocity measurement sensor to an INS system as shown in Figure 1, were is the lever-arm.Those measurements can be modeled as: where represent the velocity measurement vector with lever-arm and is the angular velocity vector.Let the measurement residual, the difference between the INS-based velocity and measurement vecloity, be then, using expressions ( 3)-( 7) and (20), we obtain After rearranging and eliminating 2nd order error elements (22) reduces to Let = × and Ω = × , thus the velocity measurement estimation error can be written as: The measurement model can be written as where is the velocity measurement residual vector, is the measurements design matrix and is the measurements noise.

12 Error State Model
Let a 12 error-state vector in (25), then the measurements matrix according to (24), after omitting lever-arm error coefficients, is

.2. 15 Error State Model
Let a 15 error-state vector in (25), then the measurements matrix according to (24) is

Results and Discussion
To test the observability properties of the models a variance software simulation was conducted.A low-grade IMU sensor was simulate at 100 Hz.The INS solution was corrected with EKF (Extended Kalman Filter) at 1 Hz using the velocity measurements.All measurements were assumed to be Gaussian white.The STD of the velocity measurements noise was set to 0.1 m/s, with lever arm of [1 0 0] in meters in the body frame.The accelerometer bias and noise STD were set to [−0.05 0.05 −0.1] and 0.01 in m/s 2 , respectively.The gyro bias and noise STD were set to [−0.1 0.05 0.1] and 0.1 in °/s 2 , respectively.
The simulation complete time of 120 s and tested 4 maneuvers-in the first 30 seconds the system was motionless, in 30-60 the system establish 1 complete round around Z axis, in 60-90 the system establish 1 complete round around X axis, and in 90-120 the system accelerated in X.

12 Error State Model
Figure 2a shows the STD simulation results for the 12 error-state model.According to the attitude STD graph, both roll and pitch STD's decreases from the beginning while the yaw STD decreases only when the system rotated around X. Similarly, the gyro bias STD is decreases rapidly for the horizontal components while the 'Down' component is significantly improve only when the systems experiencing changes in the vertical specific force while rotating around X.This behavior is opposite for the accelerometer bias STD when the 'Down' component start decreasing immediately while the horizontal components began to be reduced only when the system is experiencing changes in the rotation rate.
The findings here are equivalent to the description in [6], while here this results are related to the velocity measurement scenario, instead of the position, and with a comparison between the 12 and 15 error state models.

Figure
Figure2bshows the STD simulation results for the 15 error-state model.The STD values are decreasing slow and not as low as the 12 error state model due to the uncertainty in the lever-arm elements.This weaken the solution of the 15 error state model, especially in rotation maneuvers when the lever-arm is expressed in the measurements model.Using the 15 error state model though, the extraction of the lever arm elements is possible.As shown in Figure2b, errors in lever-arm estimation reduced only when the system rotate, and the direction of the improvement is orthogonal to the rotation axis.

Figure 2 .
Figure 2. STD results from the software simulation.(a) STD values of the 12 error-state model; (b) STD values of the 15 error state model