# Patent

## Title
Bias and misalignment compensation for 6-DOF IMU using GNSS/INS data

## Abstract

A system and method for correcting bias and angle misalignment errors in the angle rate and acceleration outputs from a 6-DOF IMU mounted to a vehicle. The method includes providing velocity and estimation attitude data in an inertial frame from, for example, a GNSS/INS, and determining an ideal acceleration estimation and an ideal rate estimation in a vehicle frame using the velocity and attitude data. The method then determines the IMU bias error and misalignment error using the ideal acceleration and rate estimations and the angle rate and acceleration outputs in an IMU body frame from the IMU.

## Background
BACKGROUND OF THE INVENTION
Field of the Invention
This invention relates generally to system and method for determining bias and misalignment errors in the outputs from a 6-degree of freedom (DOF) inertial measurement unit (IMU) and, more particularly, to a system and method for determining bias and misalignment errors in the outputs from a 6-DOF IMU on a vehicle using velocity and attitude data from a global navigation satellite system (GNSS) and/or an inertial navigation system (INS), where the method includes determining an ideal acceleration estimation and an ideal angle rate estimation in a vehicle frame using the velocity and attitude data, and determining an IMU bias error and a misalignment error using the ideal acceleration and angular rate estimation and the measured angle rate and acceleration outputs from the IMU.
Discussion of the Related Art
The operation of modern vehicles is becoming more autonomous, i.e., vehicles are able to provide driving control with less driver intervention. Cruise control systems have been on vehicles for a number of years where the vehicle operator can set a particular speed of the vehicle, and the vehicle will maintain that speed without the driver operating the throttle. Adaptive cruise control systems have been recently developed in the art where not only does the system maintain the set speed, but also will automatically slow the vehicle down in the event that a slower moving preceding vehicle is detected using various sensors, such as radar and cameras. Certain modern vehicles also provide autonomous parking where the vehicle will automatically provide the steering control for parking the vehicle. Some vehicle systems providing automatic braking without driver intervention to avoid rear-end collisions. As vehicle systems improve, they will become more autonomous with the goal being a completely autonomously driven vehicle. For example, future vehicles probably will employ autonomous systems for lane changing, passing, turns away from traffic, turns into traffic, etc.
Various active safety control systems, driver assist systems and autonomous driving operations on vehicles, such as systems providing electronic stability control (ECS), adaptive cruise control (ACC), lane keeping (LK), lane changing (LC), etc., require highly robust and precise modules for estimating various vehicle dynamics. Such modules are necessary to provide knowledge of the vehicle position and velocity to control the vehicle.
Active safety control for the systems discussed above, and others, rely on accurate vehicle speed estimation for proper performance. Currently, these types of proposed systems rely on wheel speed sensors and other vehicle kinematic inputs to provide the vehicle speed estimation. However, sometimes sensors and control modules that determine vehicle speed fail or operate improperly, where loss of vehicle speed could be serious. Certain automotive safety requirements, such as ASIL-D, require redundant vehicle speed estimation processes in the event the primary estimation processor fails. For example, for those systems that require active control, the control systems are required to provide accurate speed estimation for five seconds after the failure event so as to give the driver time to take control of the vehicle.
It is known in the art to provide a 6-DOF IMU on a vehicle that provides six rate of change measurements, particularly, angular rate measurements of vehicle roll, pitch and yaw, and acceleration measurements of longitudinal acceleration, lateral acceleration and up/down acceleration. A typical IMU provided on a vehicle will include three gyros that provide the angular rate measurements and three accelerometers that provide the acceleration measurements.
It has been proposed in the art to use the angular rate and acceleration measurements from an IMU on a vehicle to provide signals that can be used for vehicle speed estimation. U.S. patent application Ser. No. 14/680,894, filed Apr. 7, 2015, titled, Fail Operational Vehicle Speed Estimation Through Data Fusion of 6-DOF, IMU, GPS and Radar, discloses a system and method for providing redundant vehicle speed estimation that determines whether one or more primary sensors that provide vehicle speed has failed, and if so, estimates the vehicle speed in a secondary module using a buffered vehicle speed value and inertial measurement signals from an IMU. The method also uses GPS signal data and/or velocity data provided by range sensors from static objects to improve the estimated vehicle speed if they are available.
In the process discussed in the '894 application, the accuracy of the secondary vehicle speed estimation largely depends on the accuracy of the IMU acceleration and angular rate measurements. However, the measurements of acceleration and angular rates provided by the IMU on a vehicle for this purpose usually deviate from the actual acceleration and angular rate values due to several error sources. The two major sources of error for the IMU measurements are accelerometer and gyro bias components and frame misalignment corresponding to mounting the IMU to the vehicle. There is a bias term on each axis, which corresponds to six bias error terms in addition to three misalignment errors. Although it is desirable to perfectly align the IMU with the vehicle frame, there still are mounting errors that even if they are small, could provide significant misalignment errors in the calculations. Further, the bias and misalignment errors can be improved during manufacture of the vehicle by moving the vehicle and identifying the errors. However, such error corrections of bias and misalignment are also difficult to accurately obtain. Further, these error corrections for bias and misalignment often require special vehicle maneuvers during the manufacturing stage of the vehicle, and further bias terms are not fixed and may change in time due to different road, vehicle and environment conditions.
SUMMARY OF THE INVENTION
The present disclosure describes a system and method for correcting bias and angle misalignment errors in the angle rate and acceleration outputs from a 6-DOF IMU mounted to a vehicle. The method includes providing accurate velocity and estimation attitude data in an inertial frame from, for example, a GNSS/INS, and determining an ideal acceleration estimation and an ideal rate estimation in a vehicle frame using the velocity and attitude data. The method then determines the IMU bias error and misalignment error using the ideal acceleration and rate estimations and the angle rate and acceleration outputs in an IMU body frame from the IMU.
Additional features of the present invention will become apparent from the following description and appended claims, taken in conjunction with the accompanying drawings.



BRIEF DESCRIPTION OF THE DRAWINGS
FIG. 1 is an isometric illustration of a vehicle showing a vehicle reference frame and an IMU body frame;
FIG. 2 is a block diagram of an IMU error estimation system; and
FIG. 3 is a flow diagram showing a process for providing IMU error estimations.



DETAILED DESCRIPTION OF THE EMBODIMENTS
The following discussion of the embodiments of the invention directed to a system and method for identifying IMU bias and misalignment errors in a 6-DOF IMU on a vehicle is merely exemplary in nature, and is in no way intended to limit the invention or its applications or uses. For example, the system and method has particular application for secondary vehicle speed estimation. However, as will be appreciated by those skilled in the art, the system and method may have other applications and for other mobile platforms, such as on trains, machines, tractors, boats, recreation vehicles, etc.
FIG. 1 is an isometric view of a vehicle 10 including a GNSS/INS unit 12 that provides accurate vehicle velocity and attitude data including roll, yaw and pitch angles in a known manner. Further, the vehicle 10 includes sensors 14 intended to represent one or more of wheel speed sensors, steering wheel angle sensors, yaw rate sensors, longitudinal and lateral acceleration sensors, etc. as required by the discussion herein. The vehicle 10 also includes a 6-DOF IMU 16 that provides the acceleration and angular rate measurements as also discussed herein. As mentioned, the IMU 16 is a known device that includes three accelerometers and three gyros. When the vehicle speed measurement data is unavailable, the measurements from the IMU 16 can be employed to provide vehicle speed estimations as discussed, for example, in the '894 application.
The discussion herein will refer to three coordinate frames including an inertial frame (N-frame), which is a fixed reference frame on the ground with the X-axis directed to the east, the Y-axis directed to the north and the Z-axis directed upward. A vehicle frame 20 (V-frame) is a body-fixed frame located at the center of gravity of the vehicle 10 with the X-axis directed forward, the Y-axis directed to the left and the Z-axis directed upward, as shown in FIG. 1. A body frame 22 (b-frame) is a body-fixed frame located at the center of the IMU 16, where the X-Y-Z axes are aligned with the X-Y-Z axes of the accelerometers and gyros in the IMU 16. The IMU errors include the six components of bias for each accelerometer and gyro along the X-Y-Z axes of the body frame 22 and three misalignment angles corresponding to the rotation angles between the body frame 22 and the vehicle frame 20. For each axis of the body frame 22, there are two bias terms, specifically, one for the acceleration and one for the angular rate.
FIG. 2 is a block diagram of a system 30 that employs an IMU error estimation model to correct for the bias and misalignment errors referred to above. The system 30 includes an IMU error estimation module 32 that estimates the IMU bias and misalignment errors and a state estimation module 34 that corrects the raw measurements from the IMU 16 and utilizes them in the velocity and attitude estimation models. In the error estimation module 32, the accurate vehicle velocity and attitude are provided at box 36 using, for example, the GNSS/INS data. Using the vehicle velocity and attitude data from the box 36, the module 32 determines an ideal acceleration and angular rate in the vehicle frame 20 at box 38.
The discussion below details how the ideal acceleration and angular rate are determined. The following equations define the velocities and angle rates in the vehicle frame 20.

{dot over (V)}v=−ΩvVv+Rnvgn+fv,  (1)

{dot over (R)}nv=−ΩvRnv,  (2)

where {dot over (V)}v is the time derivate of the vehicle velocity in the vehicle frame 20, gn=[0,0, −9.80665]t is the vector of gravitation acceleration in the inertial frame, fv is the acceleration vector in the vehicle frame 20, and Rnv is a transformation matrix from the inertial frame to the vehicle frame 20 corresponding to the Euler angle rotations of roll φ, pitch θ and yaw ψ, where:




  
    
      
        
          
            R
            n
            v
          
          =
          
            [
            
              
            
            ⁢
            
              
                
                  
                    cos
                    ⁢
                    
                      
                    
                    ⁢
                    θ
                    ⁢
                    
                      
                    
                    ⁢
                    cos
                    ⁢
                    
                      
                    
                    ⁢
                    ψ
                  
                
                
                  
                    cos
                    ⁢
                    
                      
                    
                    ⁢
                    θ
                    ⁢
                    
                      
                    
                    ⁢
                    sin
                    ⁢
                    
                      
                    
                    ⁢
                    ψ
                  
                
                
                  
                    
                      -
                      sin
                    
                    ⁢
                    
                      
                    
                    ⁢
                    θ
                  
                
              
              
                
                  
                    
                      cos
                      ⁢
                      
                        
                      
                      ⁢
                      ψ
                      ⁢
                      
                        
                      
                      ⁢
                      sin
                      ⁢
                      
                        
                      
                      ⁢
                      φ
                      ⁢
                      
                        
                      
                      ⁢
                      sin
                      ⁢
                      
                        
                      
                      ⁢
                      θ
                    
                    -
                    
                      cos
                      ⁢
                      
                        
                      
                      ⁢
                      φ
                      ⁢
                      
                        
                      
                      ⁢
                      sin
                      ⁢
                      
                        
                      
                      ⁢
                      ψ
                    
                  
                
                
                  
                    
                      sin
                      ⁢
                      
                        
                      
                      ⁢
                      φ
                      ⁢
                      
                        
                      
                      ⁢
                      sin
                      ⁢
                      
                        
                      
                      ⁢
                      θ
                      ⁢
                      
                        
                      
                      ⁢
                      sin
                      ⁢
                      
                        
                      
                      ⁢
                      ψ
                    
                    +
                    
                      cos
                      ⁢
                      
                        
                      
                      ⁢
                      φ
                      ⁢
                      
                        
                      
                      ⁢
                      cos
                      ⁢
                      
                        
                      
                      ⁢
                      ψ
                    
                  
                
                
                  
                    cos
                    ⁢
                    
                      
                    
                    ⁢
                    θ
                    ⁢
                    
                      
                    
                    ⁢
                    sin
                    ⁢
                    
                      
                    
                    ⁢
                    φ
                  
                
              
              
                
                  
                    
                      cos
                      ⁢
                      
                        
                      
                      ⁢
                      ψ
                      ⁢
                      
                        
                      
                      ⁢
                      cos
                      ⁢
                      
                        
                      
                      ⁢
                      φ
                      ⁢
                      
                        
                      
                      ⁢
                      cos
                      ⁢
                      
                        
                      
                      ⁢
                      θ
                    
                    +
                    
                      sin
                      ⁢
                      
                        
                      
                      ⁢
                      φ
                      ⁢
                      
                        
                      
                      ⁢
                      sin
                      ⁢
                      
                        
                      
                      ⁢
                      ψ
                    
                  
                
                
                  
                    
                      cos
                      ⁢
                      
                        
                      
                      ⁢
                      φ
                      ⁢
                      
                        
                      
                      ⁢
                      sin
                      ⁢
                      
                        
                      
                      ⁢
                      θ
                      ⁢
                      
                        
                      
                      ⁢
                      sin
                      ⁢
                      
                        
                      
                      ⁢
                      ψ
                    
                    -
                    
                      sin
                      ⁢
                      
                        
                      
                      ⁢
                      φ
                      ⁢
                      
                        
                      
                      ⁢
                      cos
                      ⁢
                      
                        
                      
                      ⁢
                      ψ
                    
                  
                
                
                  
                    cos
                    ⁢
                    
                      
                    
                    ⁢
                    θ
                    ⁢
                    
                      
                    
                    ⁢
                    cos
                    ⁢
                    
                      
                    
                    ⁢
                    φ
                  
                
              
            
            ⁢
            
              
            
            ]
          
        
        ⁢
        
          
        
        ,
      
    
    
      
        (
        3
        )
      
    
  




and where Ωv=[(ωvx] is the skew-symmetric matrix of the IMU's vehicle angular rates ωv as:





  
    
      
        
          Ω
          v
        
        =
        
          
            [
            
              
                
                  0
                
                
                  
                    -
                    
                      ω
                      ψ
                    
                  
                
                
                  
                    ω
                    θ
                  
                
              
              
                
                  
                    ω
                    ψ
                  
                
                
                  0
                
                
                  
                    -
                    
                      ω
                      φ
                    
                  
                
              
              
                
                  
                    -
                    
                      ω
                      θ
                    
                  
                
                
                  
                    ω
                    φ
                  
                
                
                  0
                
              
            
            ]
          
          .
        
      
    
    
      
        (
        4
        )
      
    
  




Assuming that the vehicle velocity Vv and the roll angle φ, pitch angle θ and yaw angle ψ are known from the GNSS/INS data, by using equation (1) the ideal IMU acceleration vector fv and the angular rate measurements Ωv in the vehicle frame 20 can be found as:

Ωv=−{dot over (R)}nv(Rnv)T,  (5)

fv={dot over (V)}v+ΩvVv−Rnvgn,  (6)

where {dot over (V)}v is obtained using the numerical derivatives of the velocity data and {dot over (R)}nv is obtained using equation (3) as:





  
    
      
        
          
            
              R
              .
            
            n
            v
          
          =
          
            
              
                
                  ∂
                  
                    R
                    n
                    v
                  
                
                φ
              
              ⁢
              
                φ
                .
              
            
            +
            
              
                
                  ∂
                  
                    R
                    n
                    v
                  
                
                θ
              
              ⁢
              
                θ
                .
              
            
            +
            
              
                
                  ∂
                  
                    R
                    n
                    v
                  
                
                ψ
              
              ⁢
              
                ψ
                .
              
            
          
        
        ,
      
    
    
      
        (
        7
        )
      
    
  




where {dot over (φ)}, {dot over (θ)} and {dot over (ψ)} are calculated numerically.

The ideal IMU acceleration vector fv and the ideal angular rate Ωv from the box 38 are sent to an IMU bias and misalignment estimation box 40 that compares the ideal acceleration vector fv and the ideal angular rate Ωv estimation values to the angle rates ωb in the body frame 22 from the gyros in the IMU 16 provided at box 42 and the acceleration values fb in the body frame 22 from the accelerometers in the IMU 16 provided at box 44 to calculate the bias and misalignment estimation errors. As long as the GNSS/INS date provides accurate information to determine the vehicle velocity and attitude at the box 36, then the bias and misalignment errors for each accelerometer on the X-Y-Z axes of the body frame 22 and the three misalignment angle errors can be accurately obtained at the box 40.
In order to calculate the bias and misalignment estimation errors, a relationship between the ideal and the actual measurements is obtained. The IMU measurements in equation (5) are ideal measurements in the sense that they do not include any IMU errors. Considering bias and misalignment as two significant errors, the ideal values and the actual IMU measurements are related as:

fb=Rvbfv+fbias,  (8)

ωb=Rvbωv+ωbias,  (9)

where fb and ωb are the actual IMU measurements in the body frame 22, fbias and ωbias are the bias vectors of the accelerometers and gyros, respectively, and Rvb is the transformation matrix from the vehicle frame 20 to the body frame 22 corresponding to the IMU misalignment angles as:





  
    
      
        
          
            R
            v
            b
          
          =
          
            [
            
              
            
            ⁢
            
              
                
                  
                    cos
                    ⁢
                    
                      
                    
                    ⁢
                    β
                    ⁢
                    
                      
                    
                    ⁢
                    cos
                    ⁢
                    
                      
                    
                    ⁢
                    γ
                  
                
                
                  
                    cos
                    ⁢
                    
                      
                    
                    ⁢
                    θ
                    ⁢
                    
                      
                    
                    ⁢
                    sin
                    ⁢
                    
                      
                    
                    ⁢
                    γ
                  
                
                
                  
                    
                      -
                      sin
                    
                    ⁢
                    
                      
                    
                    ⁢
                    β
                  
                
              
              
                
                  
                    
                      cos
                      ⁢
                      
                        
                      
                      ⁢
                      γ
                      ⁢
                      
                        
                      
                      ⁢
                      sin
                      ⁢
                      
                        
                      
                      ⁢
                      α
                      ⁢
                      
                        
                      
                      ⁢
                      sin
                      ⁢
                      
                        
                      
                      ⁢
                      β
                    
                    -
                    
                      cos
                      ⁢
                      
                        
                      
                      ⁢
                      α
                      ⁢
                      
                        
                      
                      ⁢
                      sin
                      ⁢
                      
                        
                      
                      ⁢
                      γ
                    
                  
                
                
                  
                    
                      sin
                      ⁢
                      
                        
                      
                      ⁢
                      γ
                      ⁢
                      
                        
                      
                      ⁢
                      sin
                      ⁢
                      
                        
                      
                      ⁢
                      α
                      ⁢
                      
                        
                      
                      ⁢
                      sin
                      ⁢
                      
                        
                      
                      ⁢
                      β
                    
                    +
                    
                      cos
                      ⁢
                      
                        
                      
                      ⁢
                      α
                      ⁢
                      
                        
                      
                      ⁢
                      cos
                      ⁢
                      
                        
                      
                      ⁢
                      γ
                    
                  
                
                
                  
                    cos
                    ⁢
                    
                      
                    
                    ⁢
                    β
                    ⁢
                    
                      
                    
                    ⁢
                    sin
                    ⁢
                    
                      
                    
                    ⁢
                    α
                  
                
              
              
                
                  
                    
                      cos
                      ⁢
                      
                        
                      
                      ⁢
                      γ
                      ⁢
                      
                        
                      
                      ⁢
                      cos
                      ⁢
                      
                        
                      
                      ⁢
                      α
                      ⁢
                      
                        
                      
                      ⁢
                      sin
                      ⁢
                      
                        
                      
                      ⁢
                      β
                    
                    +
                    
                      sin
                      ⁢
                      
                        
                      
                      ⁢
                      α
                      ⁢
                      
                        
                      
                      ⁢
                      sin
                      ⁢
                      
                        
                      
                      ⁢
                      γ
                    
                  
                
                
                  
                    
                      sin
                      ⁢
                      
                        
                      
                      ⁢
                      γ
                      ⁢
                      
                        
                      
                      ⁢
                      cos
                      ⁢
                      
                        
                      
                      ⁢
                      α
                      ⁢
                      
                        
                      
                      ⁢
                      sin
                      ⁢
                      
                        
                      
                      ⁢
                      β
                    
                    -
                    
                      sin
                      ⁢
                      
                        
                      
                      ⁢
                      α
                      ⁢
                      
                        
                      
                      ⁢
                      cos
                      ⁢
                      
                        
                      
                      ⁢
                      γ
                    
                  
                
                
                  
                    cos
                    ⁢
                    
                      
                    
                    ⁢
                    β
                    ⁢
                    
                      
                    
                    ⁢
                    cos
                    ⁢
                    
                      
                    
                    ⁢
                    α
                  
                
              
            
            ⁢
            
              
            
            ]
          
        
        ⁢
        
          
        
        ,
      
    
    
      
        (
        10
        )
      
    
  




where α, β and γ are the misalignment Euler angles about the X, Y and Z axes, respectively.

Equations (8) and (9) include six equations with nine unknowns to be found. To find a unique solution to the equations, three equations related to the acceleration measurements from a different time instant are necessary.
In the box 40, the three misalignment angles α, β and γ are first found using the ideal acceleration measurements at two different time instants in the following two cases. The first case varies the longitudinal acceleration with constant lateral and vertical accelerations. In this case, the following conditions hold for the ideal acceleration measurements of the IMU 16 at times t1 and t2 as:

fxv(t1)≠fxv(t2),  (11)

fyv(t1)=fyv(t2),  (12)

fzv(t1)=fzv(t2).  (13)

Assuming that the bias terms remain constant at times t1 and t2 and using equations (8)-(13), the difference between the actual IMU measurements at times t1 and t2 is found as:




  
    
      
        {
        
          
            
              
                
                  
                    Δ
                    ⁢
                    
                      
                    
                    ⁢
                    
                      f
                      x
                      b
                    
                  
                  =
                  
                    cos
                    ⁢
                    
                      
                    
                    ⁢
                    β
                    ⁢
                    
                      
                    
                    ⁢
                    cos
                    ⁢
                    
                      
                    
                    ⁢
                    Δ
                    ⁢
                    
                      
                    
                    ⁢
                    
                      f
                      x
                      v
                    
                  
                
              
            
            
              
                
                  
                    Δ
                    ⁢
                    
                      
                    
                    ⁢
                    
                      f
                      y
                      b
                    
                  
                  =
                  
                    
                      (
                      
                        
                          cos
                          ⁢
                          
                            
                          
                          ⁢
                          γ
                          ⁢
                          
                            
                          
                          ⁢
                          sin
                          ⁢
                          
                            
                          
                          ⁢
                          α
                          ⁢
                          
                            
                          
                          ⁢
                          sin
                          ⁢
                          
                            
                          
                          ⁢
                          β
                        
                        -
                        
                          cos
                          ⁢
                          
                            
                          
                          ⁢
                          α
                          ⁢
                          
                            
                          
                          ⁢
                          sin
                          ⁢
                          
                            
                          
                          ⁢
                          γ
                        
                      
                      )
                    
                    ⁢
                    Δ
                    ⁢
                    
                      
                    
                    ⁢
                    
                      f
                      x
                      v
                    
                  
                
              
            
            
              
                
                  
                    Δ
                    ⁢
                    
                      
                    
                    ⁢
                    
                      f
                      z
                      b
                    
                  
                  =
                  
                    
                      (
                      
                        
                          cos
                          ⁢
                          
                            
                          
                          ⁢
                          γ
                          ⁢
                          
                            
                          
                          ⁢
                          cos
                          ⁢
                          
                            
                          
                          ⁢
                          α
                          ⁢
                          
                            
                          
                          ⁢
                          sin
                          ⁢
                          
                            
                          
                          ⁢
                          β
                        
                        +
                        
                          sin
                          ⁢
                          
                            
                          
                          ⁢
                          α
                          ⁢
                          
                            
                          
                          ⁢
                          sin
                          ⁢
                          
                            
                          
                          ⁢
                          γ
                        
                      
                      )
                    
                    ⁢
                    Δ
                    ⁢
                    
                      
                    
                    ⁢
                    
                      f
                      x
                      v
                    
                  
                
              
            
          
          ,
        
      
    
    
      
        (
        14
        )
      
    
  




where Δf=f(t2)−f(t1), and where the constant lateral and vertical acceleration terms in the vehicle frame 20 as well as the bias terms are canceled out in the subtraction.

Linearizing equation (14) in the neighborhood of (α, β, γ)=(0,0,0,) yields:




  
    
      
        {
        
          
            
              
                
                  
                    Δ
                    ⁢
                    
                      
                    
                    ⁢
                    
                      f
                      x
                      b
                    
                  
                  =
                  
                    Δ
                    ⁢
                    
                      
                    
                    ⁢
                    
                      f
                      y
                      v
                    
                  
                
              
            
            
              
                
                  
                    Δ
                    ⁢
                    
                      
                    
                    ⁢
                    
                      f
                      y
                      b
                    
                  
                  =
                  
                    
                      -
                      γ
                    
                    ⁢
                    
                      
                    
                    ⁢
                    Δ
                    ⁢
                    
                      
                    
                    ⁢
                    
                      f
                      x
                      v
                    
                  
                
              
            
            
              
                
                  
                    Δ
                    ⁢
                    
                      
                    
                    ⁢
                    
                      f
                      z
                      b
                    
                  
                  =
                  
                    β
                    ⁢
                    
                      
                    
                    ⁢
                    Δ
                    ⁢
                    
                      
                    
                    ⁢
                    
                      f
                      x
                      v
                    
                  
                
              
            
          
          ,
        
      
    
    
      
        (
        15
        )
      
    
  




which results in:





  
    
      
        
          β
          =
          
            
              Δ
              ⁢
              
                
              
              ⁢
              
                f
                z
                b
              
            
            
              Δ
              ⁢
              
                
              
              ⁢
              
                f
                x
                v
              
            
          
        
        ,
      
    
    
      
        (
        16
        )
      
    
  








  
    
      
        γ
        =
        
          -
          
            
              
                Δ
                ⁢
                
                  
                
                ⁢
                
                  f
                  y
                  b
                
              
              
                Δ
                ⁢
                
                  
                
                ⁢
                
                  f
                  x
                  v
                
              
            
            .
          
        
      
    
    
      
        (
        17
        )
      
    
  




If β≠0 or γ≠0, then the misalignment angle α is found by substituting equations (16) and (17) into equation (14) as:




  
    
      
        
          α
          =
          
            
              sin
              
                -
                1
              
            
            ⁡
            
              (
              
                
                  
                    
                      c
                      1
                    
                    ⁢
                    Δ
                    ⁢
                    
                      
                    
                    ⁢
                    
                      f
                      y
                      b
                    
                  
                  +
                  
                    
                      c
                      2
                    
                    ⁢
                    Δ
                    ⁢
                    
                      
                    
                    ⁢
                    
                      f
                      z
                      b
                    
                  
                
                
                  
                    c
                    1
                    2
                  
                  +
                  
                    c
                    2
                    2
                  
                
              
              )
            
          
        
        ,
      
    
    
      
        (
        18
        )
      
    
  




where:

c1=sin β cos γΔfxv,  (19)

c2=sin γfxv.  (20)

The second case varies the longitudinal and vertical accelerations with a constant lateral acceleration. In this case, the following conditions are provided.

fxv(t1)≠fxv(t2),  (21)

fyv(t1)=fyv(t2),  (22)

fzv(t1)≠fzv(t2).  (23)

Assuming that the bias terms remain constant at times t1 and t2 and using equations (8), (9), (21), (22) and (23), the difference between the actual IMU measurements at times t1 and t2 is found as:




  
    
      
        {
        
          
            
              
                
                  
                    Δ
                    ⁢
                    
                      
                    
                    ⁢
                    
                      f
                      x
                      b
                    
                  
                  =
                  
                    
                      cos
                      ⁢
                      
                        
                      
                      ⁢
                      β
                      ⁢
                      
                        
                      
                      ⁢
                      cos
                      ⁢
                      
                        
                      
                      ⁢
                      γ
                      ⁢
                      
                        
                      
                      ⁢
                      Δ
                      ⁢
                      
                        
                      
                      ⁢
                      
                        f
                        x
                        v
                      
                    
                    -
                    
                      sin
                      ⁢
                      
                        
                      
                      ⁢
                      β
                      ⁢
                      
                        
                      
                      ⁢
                      
                        f
                        z
                        b
                      
                    
                  
                
              
            
            
              
                
                  
                    Δ
                    ⁢
                    
                      
                    
                    ⁢
                    
                      f
                      y
                      b
                    
                  
                  =
                  
                    
                      
                        (
                        
                          
                            cos
                            ⁢
                            
                              
                            
                            ⁢
                            γ
                            ⁢
                            
                              
                            
                            ⁢
                            sin
                            ⁢
                            
                              
                            
                            ⁢
                            α
                            ⁢
                            
                              
                            
                            ⁢
                            sin
                            ⁢
                            
                              
                            
                            ⁢
                            β
                          
                          -
                          
                            cos
                            ⁢
                            
                              
                            
                            ⁢
                            α
                            ⁢
                            
                              
                            
                            ⁢
                            sin
                            ⁢
                            
                              
                            
                            ⁢
                            γ
                          
                        
                        )
                      
                      ⁢
                      Δ
                      ⁢
                      
                        
                      
                      ⁢
                      
                        f
                        x
                        v
                      
                    
                    +
                    
                      sin
                      ⁢
                      
                        
                      
                      ⁢
                      α
                      ⁢
                      
                        
                      
                      ⁢
                      cos
                      ⁢
                      
                        
                      
                      ⁢
                      β
                      ⁢
                      
                        
                      
                      ⁢
                      
                        f
                        z
                        v
                      
                    
                  
                
              
            
            
              
                
                  
                    Δ
                    ⁢
                    
                      
                    
                    ⁢
                    
                      f
                      z
                      b
                    
                  
                  =
                  
                    
                      
                        (
                        
                          
                            cos
                            ⁢
                            
                              
                            
                            ⁢
                            γ
                            ⁢
                            
                              
                            
                            ⁢
                            cos
                            ⁢
                            
                              
                            
                            ⁢
                            α
                            ⁢
                            
                              
                            
                            ⁢
                            sin
                            ⁢
                            
                              
                            
                            ⁢
                            β
                          
                          -
                          
                            sin
                            ⁢
                            
                              
                            
                            ⁢
                            α
                            ⁢
                            
                              
                            
                            ⁢
                            sin
                            ⁢
                            
                              
                            
                            ⁢
                            γ
                          
                        
                        )
                      
                      ⁢
                      Δ
                      ⁢
                      
                        
                      
                      ⁢
                      
                        f
                        x
                        v
                      
                    
                    +
                    
                      cos
                      ⁢
                      
                        
                      
                      ⁢
                      α
                      ⁢
                      
                        
                      
                      ⁢
                      cos
                      ⁢
                      
                        
                      
                      ⁢
                      β
                      ⁢
                      
                        
                      
                      ⁢
                      
                        f
                        z
                        v
                      
                    
                  
                
              
            
          
          ,
        
      
    
    
      
        (
        24
        )
      
    
  




where the constant lateral acceleration terms and the bias terms in the vehicle frame 20 are cancelled out in the subtraction.

Linearizing equation (24) in the neighborhood of (α, β, γ)=(0,0,0,) yields:




  
    
      
        {
        
          
            
              
                
                  
                    Δ
                    ⁢
                    
                      
                    
                    ⁢
                    
                      f
                      x
                      b
                    
                  
                  =
                  
                    
                      Δ
                      ⁢
                      
                        
                      
                      ⁢
                      
                        f
                        x
                        v
                      
                    
                    -
                    
                      βΔ
                      ⁢
                      
                        
                      
                      ⁢
                      
                        f
                        z
                        v
                      
                    
                  
                
              
            
            
              
                
                  
                    Δ
                    ⁢
                    
                      
                    
                    ⁢
                    
                      f
                      y
                      b
                    
                  
                  =
                  
                    
                      
                        -
                        γ
                      
                      ⁢
                      
                        
                      
                      ⁢
                      Δ
                      ⁢
                      
                        
                      
                      ⁢
                      
                        f
                        x
                        v
                      
                    
                    +
                    
                      αΔ
                      ⁢
                      
                        
                      
                      ⁢
                      
                        f
                        z
                        v
                      
                    
                  
                
              
            
            
              
                
                  
                    Δ
                    ⁢
                    
                      
                    
                    ⁢
                    
                      f
                      z
                      b
                    
                  
                  =
                  
                    
                      β
                      ⁢
                      
                        
                      
                      ⁢
                      Δ
                      ⁢
                      
                        
                      
                      ⁢
                      
                        f
                        x
                        v
                      
                    
                    +
                    
                      Δ
                      ⁢
                      
                        
                      
                      ⁢
                      
                        f
                        z
                        v
                      
                    
                  
                
              
            
          
          .
        
      
    
    
      
        (
        25
        )
      
    
  




The misalignment angle is found from equation (25) as:




  
    
      
        β
        =
        
          
            
              
                Δ
                ⁢
                
                  
                
                ⁢
                
                  f
                  z
                  b
                
              
              -
              
                Δ
                ⁢
                
                  
                
                ⁢
                
                  f
                  z
                  v
                
              
            
            
              Δ
              ⁢
              
                
              
              ⁢
              
                f
                x
                v
              
            
          
          .
        
      
    
    
      
        (
        26
        )
      
    
  




If an estimation of the misalignment angle γ is available from previous times, then:




  
    
      
        α
        =
        
          
            
              
                Δ
                ⁢
                
                  
                
                ⁢
                
                  f
                  y
                  b
                
              
              -
              
                Δ
                ⁢
                
                  
                
                ⁢
                
                  f
                  x
                  v
                
              
            
            
              Δ
              ⁢
              
                
              
              ⁢
              
                f
                z
                v
              
            
          
          .
        
      
    
    
      
        (
        27
        )
      
    
  




For implementing the misalignment estimation algorithm, the ideal IMU measurements in equations (5) and (6) are buffered in the module 32 for a short period of time (e.g., 1 second). The time t2 is put equal to the current time, and time t1 is found by searching the buffered data, which satisfy the conditions in the first case or the second case. Since the misalignment angles are constant, they are updated as the average of the current and previous values at each time instant.
Once the misalignment angles α, β and γ are calculated, the bias components for the accelerometers and gyros can be obtained using equations (8), (9) and (10) as:

fbias=fb−Rvbfv,  (28)

ωbias=ωb−Rvbωv.  (29)

FIG. 3 is a flowchart diagram 60 showing a process for providing the IMU error estimation calculations in the boxes 38 and 40 as discussed above. Vehicle attitude measurements of roll φ, pitch θ and yaw ψ are provided at box 62 from the GNSS/INS data to box 64 that transforms the vehicle attitude measurements from the inertial frame to the vehicle frame 20 to obtain the transformation matrixes Rnv and {dot over (R)}nv as provided by equations (2) and (3). The transformation matrixes Rnv and {dot over (R)}nv are then sent to box 66 to provide compensated rates in the vehicle frame 20 including the angular rates ωv and the skew-symmetric matrix Ωv provided by equation (5). The vehicle velocity and acceleration values Vv and {dot over (V)}v from the GNSS/INS data are provided at box 68 and are compensated for acceleration in the vehicle frame 20 at box 70 using the skew-symmetric matrix Ωv provided by equation (5) from the box 66 to obtain the measured acceleration vector fv in the vehicle frame 20 provided by equation (6). The algorithm uses the vector fv from the box 70 and the IMU acceleration measurement values fb from box 74 to calculate the misalignment angles α, β and γ using either the first case through equations (16), (17) and (18) or the second case through equations (26) and (27). The algorithm generates the transformation matrix Rvb for transformation from the vehicle frame 20 to the body frame 22 using equation (10) at box 76. The transformation matrix Rvb is then provided to box 78 along with the angle rates ωb in the body frame 22 from the IMU 16 at box 80 and the acceleration rates fb in the body frame 22 from the box 74 to generate the bias components fbias and ωbias using equations (28) and (29), where the results obtained from equations (16), (17) and (18) or equations (26) and (27), and equations (28) and (29) give the nine values.
The nine values identifying the bias and misalignment errors from the box 40 are provided to the module 34 so that if the GNSS/INS data is not available at any particular point in time, then the previous calculated errors can be used to identify the vehicle velocity and attitude in case those values are needed if the vehicle velocity sensors fail. Therefore, if the IMU data is needed to estimate the vehicle velocity as in the '894 application, those values are adjusted for the errors prior to being used for those calculations. The errors from the box 40 are applied to an IMU bias and misalignment compensation box 46 in the module 34 along with IMU angle rates ωb at box 48 and the IMU acceleration values fb at box 50 to be corrected as follows.

fv=(Rvb)T(fb−fbias),  (30)

ωv=(Rvb)T(ωb−ωbias).  (31)

Once the IMU angle rates and acceleration values are corrected at the box 46, those corrected values can be used to determine the vehicle velocity and attitude at box 52 as described, for example, in the '894 application.
As will be well understood by those skilled in the art, the several and various steps and processes discussed herein to describe the invention may be referring to operations performed by a computer, a processor or other electronic calculating device that manipulate and/or transform data using electrical phenomenon. Those computers and electronic devices may employ various volatile and/or non-volatile memories including non-transitory computer-readable medium with an executable program stored thereon including various code or executable instructions able to be performed by the computer or processor, where the memory and/or computer-readable medium may include all forms and types of memory and other computer-readable media.
The foregoing discussion discloses and describes merely exemplary embodiments of the present invention. One skilled in the art will readily recognize from such discussion and from the accompanying drawings and claims that various changes, modifications and variations can be made therein without departing from the spirit and scope of the invention as defined in the following claims.

## Claims

1. A method for correcting bias and angle misalignment errors in angle rate and acceleration outputs from a six-degree of freedom (DOF) inertial measurement unit (IMU), said IMU being mounted on a platform, said method comprising:
obtaining velocity and estimation attitude data in an inertial frame;
determining an acceleration estimation and a rate estimation in a platform frame using the velocity and attitude data; and
determining an IMU bias error and a misalignment error using the acceleration and rate estimations and the angle rate and acceleration outputs from the IMU.

2. The method according to claim 1 wherein obtaining velocity and attitude data includes obtaining velocity and attitude data from a global navigation satellite system (GNSS) and/or an inertial navigation system (INS).

3. The method according to claim 1 wherein determining an acceleration estimation and a rate estimation includes transforming velocity, roll angle, pitch angle and yaw angle of the platform in the inertial frame to the platform frame.

4. The method according to claim 3 wherein transforming velocity and roll, pitch and yaw includes using the equations:


{dot over (V)}v=−ΩvVv+Rnvgn+fv 



{dot over (R)}nv=−ΩvRnv 


where {dot over (V)}v is a time derivate of the velocity in the platform frame, gn=[0,0, −9.80665]t is a vector of gravitation acceleration in the inertial frame, f′′ is an acceleration vector in the platform frame, and Rnv is a transformation matrix from the inertial frame to the platform frame corresponding to angle rotations of roll φ, pitch θ and yaw ψ, where:




  
    
      
        
          
            R
            n
            v
          
          =
          
            [
            
              
            
            ⁢
            
              
                
                  
                    cos
                    ⁢
                    
                      
                    
                    ⁢
                    θcos
                    ⁢
                    
                      
                    
                    ⁢
                    ψ
                  
                
                
                  
                    cos
                    ⁢
                    
                      
                    
                    ⁢
                    θ
                    ⁢
                    
                      
                    
                    ⁢
                    sin
                    ⁢
                    
                      
                    
                    ⁢
                    ψ
                  
                
                
                  
                    
                      -
                      sin
                    
                    ⁢
                    
                      
                    
                    ⁢
                    θ
                  
                
              
              
                
                  
                    
                      cos
                      ⁢
                      
                        
                      
                      ⁢
                      ψsin
                      ⁢
                      
                        
                      
                      ⁢
                      φ
                      ⁢
                      
                        
                      
                      ⁢
                      sin
                      ⁢
                      
                        
                      
                      ⁢
                      θ
                    
                    -
                    
                      cos
                      ⁢
                      
                        
                      
                      ⁢
                      φ
                      ⁢
                      
                        
                      
                      ⁢
                      sin
                      ⁢
                      
                        
                      
                      ⁢
                      ψ
                    
                  
                
                
                  
                    
                      sin
                      ⁢
                      
                        
                      
                      ⁢
                      φ
                      ⁢
                      
                        
                      
                      ⁢
                      sin
                      ⁢
                      
                        
                      
                      ⁢
                      θ
                      ⁢
                      
                        
                      
                      ⁢
                      sin
                      ⁢
                      
                        
                      
                      ⁢
                      ψ
                    
                    +
                    
                      cos
                      ⁢
                      
                        
                      
                      ⁢
                      φ
                      ⁢
                      
                        
                      
                      ⁢
                      cos
                      ⁢
                      
                        
                      
                      ⁢
                      ψ
                    
                  
                
                
                  
                    cos
                    ⁢
                    
                      
                    
                    ⁢
                    θ
                    ⁢
                    
                      
                    
                    ⁢
                    sin
                    ⁢
                    
                      
                    
                    ⁢
                    φ
                  
                
              
              
                
                  
                    
                      cos
                      ⁢
                      
                        
                      
                      ⁢
                      ψ
                      ⁢
                      
                        
                      
                      ⁢
                      cos
                      ⁢
                      
                        
                      
                      ⁢
                      φcosθ
                    
                    +
                    
                      sin
                      ⁢
                      
                        
                      
                      ⁢
                      φ
                      ⁢
                      
                        
                      
                      ⁢
                      sin
                      ⁢
                      
                        
                      
                      ⁢
                      ψ
                    
                  
                
                
                  
                    
                      cos
                      ⁢
                      
                        
                      
                      ⁢
                      φsinθ
                      ⁢
                      
                        
                      
                      ⁢
                      sin
                      ⁢
                      
                        
                      
                      ⁢
                      ψ
                    
                    -
                    
                      sin
                      ⁢
                      
                        
                      
                      ⁢
                      θ
                      ⁢
                      
                        
                      
                      ⁢
                      cos
                      ⁢
                      
                        
                      
                      ⁢
                      ψ
                    
                  
                
                
                  
                    cos
                    ⁢
                    
                      
                    
                    ⁢
                    θ
                    ⁢
                    
                      
                    
                    ⁢
                    cos
                    ⁢
                    
                      
                    
                    ⁢
                    φ
                  
                
              
            
            ⁢
            
              
            
            ]
          
        
        ⁢
        
          
        
      
    
    
      
        
      
    
  




and where Ωv=[ωvx] is a skew-symmetric matrix of the IMU angular rate outputs as:




  
    
      
        
          Ω
          v
        
        =
        
          
            [
            
              
                
                  0
                
                
                  
                    -
                    
                      ω
                      ψ
                    
                  
                
                
                  
                    ω
                    θ
                  
                
              
              
                
                  
                    ω
                    ψ
                  
                
                
                  0
                
                
                  
                    -
                    
                      ω
                      φ
                    
                  
                
              
              
                
                  
                    -
                    
                      ω
                      θ
                    
                  
                
                
                  
                    ω
                    φ
                  
                
                
                  0
                
              
            
            ]
          
          .
        
      
    
    
      
        
      
    
  




Ωb=−{dot over (R)}nv(Rnv)T 



fv={dot over (V)}v+ΩvVv−Rnvgn 


where Ω is the angular rate, {dot over (V)}v is obtained using numerical derivatives of the velocity data, and {dot over (R)}nv is obtained as:




  
    
      R
      .
    
    n
    v
  
  =
  
    
      
        
          ∂
          
            R
            n
            v
          
        
        φ
      
      ⁢
      
        φ
        .
      
    
    +
    
      
        
          ∂
          
            R
            n
            v
          
        
        θ
      
      ⁢
      
        θ
        .
      
    
    +
    
      
        
          ∂
          
            R
            n
            v
          
        
        ψ
      
      ⁢
      
        ψ
        .
      
    
  




where {dot over (φ)}, {dot over (θ)} and {dot over (ψ)} are calculated numerically.

5. The method according to claim 1 wherein determining an IMU bias error and a misalignment error includes comparing the acceleration estimation and the ideal rate estimation to the angle rate and acceleration outputs from the IMU.

6. The method according to claim 5 wherein comparing the acceleration estimation and the rate estimation includes transforming the acceleration and angular rate estimations from the platform frame to an IMU body frame.

7. The method according to claim 6 wherein determining the IMU bias error and misalignment error includes identifying three misalignment angles using the acceleration estimations at two different sample time.

8. The method according to claim 7 wherein determining the misalignment angles includes varying a longitudinal acceleration of the platform with a constant lateral and vertical acceleration.

9. The method according to claim 7 wherein determining the misalignment angles includes varying the longitudinal and vertical accelerations of the platform with a constant lateral acceleration.

10. The method according to claim 7 wherein determining the IMU bias error and misalignment error includes using equations:


fbias=fb−Rvbfv,



ωbias=ωb−Rvbωv.

11. The method according to claim 1 further comprising using the IMU bias error and misalignment error to determine platform velocity in the event that providing velocity and estimation attitude data is unavailable.

12. The method according to claim 1 wherein the platform is a vehicle and the IMU is mounted to a vehicle structure.

13. A method for correcting bias and angle misalignment errors in angle rate and acceleration outputs from a six-degree of freedom (DOF) inertial measurement unit (IMU) mounted on a vehicle, said method comprising:
providing velocity and estimation attitude data in an inertial frame from a global navigation satellite system (GNSS) and/or an inertial navigation system (INS);
determining an acceleration estimation and a rate estimation in a vehicle frame using the velocity and attitude data that includes transforming velocity, roll angle, pitch angle and yaw angle of the vehicle in the inertial frame to the vehicle frame; and
determining an IMU bias error and a misalignment error using the acceleration and rate estimations and the angle rate and acceleration outputs from the IMU that includes comparing the acceleration estimation and the rate estimation to the angle rate and acceleration outputs from the IMU and transforming the acceleration and angular rate estimations from the vehicle frame to an IMU body frame.

14. The method according to claim 13 wherein determining the IMU bias error and misalignment error includes identifying three misalignment angles using the ideal acceleration estimations at two different sample time.

15. The method according to claim 14 wherein determining the misalignment angles includes varying a longitudinal acceleration of the vehicle with a constant lateral and vertical acceleration.

16. The method according to claim 14 wherein determining the misalignment angles includes varying the longitudinal and vertical accelerations of the vehicle with a constant lateral acceleration.

17. The method according to claim 14 wherein determining the IMU bias error and misalignment error includes using equations:


fbias=fb−Rvbfv,



ωbias=ωb−Rvbωv,


where fbias is the misalignment error, fb is acceleration in the body frame, fv is acceleration in the vehicle frame, ωbias is the bias error, ωb is angular rate in the body frame, ωv is angular rate in the vehicle frame, and Rvb is a transformational matrix between the vehicle frame and the body frame.

18. The method according to claim 13 further comprising using the IMU bias error and misalignment error to determine vehicle velocity in the event that providing velocity and estimation attitude data is unavailable.

19. A system for correcting bias and angle misalignment errors in angle rate and acceleration outputs from a six-degree of freedom (DOF) inertial measurement unit (IMU), said IMU being mounted on a platform, said system comprising:
means for providing velocity and estimation attitude data in an inertial frame;
means for determining an ideal acceleration estimation and a rate estimation in a platform frame using the velocity and attitude data; and
means for determining an IMU bias error and a misalignment error using the acceleration and rate estimations and the angle rate and acceleration outputs from the IMU.

20. The system according to claim 19 wherein the means for providing velocity and attitude data provides the velocity and attitude data from a global navigation satellite system (GNSS) and/or an inertial navigation system (INS).