Sensor fusion system and method for estimating position, speed and orientation of a vehicle, in particular an aircraft

• US 7,970,500 B2
• Filed: 02/25/2005
• Issued: 06/28/2011
• Est. Priority Date: 02/27/2004
• Status: Expired due to Fees

• Current Assignee: Instituto Nacional de Técnica Aeroespacial
• Original Assignee: Instituto Nacional de Técnica Aeroespacial
• Inventors: Parra Carqué, Sergio De La
• Primary Examiner(s): NGUYEN, TAN QUANG
• Application Number:
  US10/590,578
• Publication Number:
  US 20070213889A1
• Time in Patent Office: 2,314 Days
• Field of Search: 701/77, 701/3, 701/4, 701/7, 701/214, 701/220, 342/357.02, 342/357.2
• US Class Current: 701/7
• CPC Class Codes: G01C 21/16 by integrating acceleration or speed, i.e. inertial navigation
• CPC Class Codes: G01C 3/08 Use of electric radiation detectors
• CPC Class Codes: G05D 1/101 specially adapted for aircraft

Abstract

This invention relates to a system for estimating the position, velocity and orientation of a vehicle, by determining the components of two noncollinear constant unit vectors _b,_b according to vehicle body axes; and determining the components of the noncollinear constant unit vectors {right arrow over (g)}_t,{right arrow over (e)}_t according to Earth'"'"'s axes. The system further determines the three components of angular velocity _b of the vehicle in body axes; corrects the angular velocity _b with a correction u_ω and obtains a corrected angular velocity {circumflex over (ω)}_b=_b+u_ω; a control module implementing a control law to calculate the correction u_ω, where the control law is:
u_ω=σ(_b×ĝ_b+_b×ê_b)  [1]
where σ is a positive scalar,

such that upon using the corrected angular velocity {circumflex over (ω)}_b=_b+u_ω as input to a module for integrating the kinematic equations, the latter are stable in the ISS sense and the error in the estimation of the direction cosine matrix {circumflex over (B)} and of the Euler angles {circumflex over (φ)} is bounded.

Claims

1. A system for estimating the position, velocity and orientation of a vehicle, comprising:

an Inertial Measurement Unit (IMU) including a group of at least three gyroscopes for measuring the angular velocity _b(t) of the vehicle in body axes and at least three accelerometers located along the vehicle body axes to provide the specific force _b in body axes;

a magnetometer able to measure the Earth'"'"'s magnetic field according to the vehicle body axes;

static pressure and differential pressure sensors;

two vanes to measure the angles of attack and side slip;

an angular velocity acquisition and processing module configured to acquire the angular velocity _b(t) and delay it to obtain _b(t−

τ

);

a data acquisition and processing module configured to acquire the specific force _b(t) measured by the accelerometers, the static pressure _s(t) measured in sensor, the differential pressure _d(t) measured in sensor, the angle of attack (t) measured in sensor, the angle of sideslip (t) measured in sensor and the value of the Earth'"'"'s magnetic field _b(t) measured in the magnetometer, delay them and process them to calculate the true airspeed (t−

τ

), the air velocity in body axes _b(t−

τ

) as follows;

2. The system according to claim 1, wherein said noncollinear unit vectors {right arrow over (g)},{right arrow over (e)} are local gravity {right arrow over (g)} and projection of the magnetic field on the plane perpendicular to gravity {right arrow over (e)}.

3. The system according to claim 1, further comprising a Savitzky-Golay filter where _b, numerical derivative of _b , is calculated.

4. The system according to claim 1, further comprising:

a navigation module where the navigation equations of the vehicle are integrated from the specific force _b and the direction cosine matrix {circumflex over (B)} to obtain calculated position and velocity in local axes and corrected in a Kalman filter to obtain estimated position and velocity in local axes.

5. A method for estimating the position, velocity and orientation of a vehicle comprising:

calculating the components of two noncollinear constant unit vectors _b,_b according to vehicle body axes from measurements of sensors located in the vehicle according to the body axes of the latter, said calculation comprising;

measuring specific force _b(t) in body axes, static pressure _s(t), differential pressure _d(t), angle of attack (t), angle of sideslip (t) and the value of the Earth'"'"'s magnetic field _b(t);

calculating the true airspeed (t) from the differential pressure _d(t) and static pressure _s(t) measurements and from knowing the outside temperature at the initial moment T₀;

calculating the air velocity in body axes as follows;

6. The method according to claim 5, wherein said noncollinear unit vectors {right arrow over (g)},{right arrow over (e)} are local gravity {right arrow over (g)} and projection of the magnetic field on the plane perpendicular to gravity {right arrow over (e)}.

7. The method according to claim 5, wherein _b, the numerical derivative of _b, is calculated in a Savitzky-Golay filter.

8. A method according to claim 5 including:

integrating the navigation equations of the vehicle according to the specific force _b and the direction cosine matrix {circumflex over (B)} to obtain the calculated position and velocity in local axes and they are corrected in a Kalman filter to obtain estimated position and velocity in local axes.