System and Method of Navigation based on State Estimation Using a Stepped Filter

• US 20110202204A1
• Filed: 04/30/2009
• Published: 08/18/2011
• Est. Priority Date: 05/13/2008
• Status: Active Grant

• Current Assignee: Department of Health and Human Services (State of Victoria)
• Original Assignee: The Government of The US As Represented By The Secretary of The Navy
• Inventors: Kahn, Aaron D.
• Granted Patent: US 8,560,234 B2
• Time in Patent Office: Days
• US Class Current: 701/3
• CPC Class Codes: G01C 21/165 combined with non-inertial navigation instruments
• CPC Class Codes: G01S 19/49 whereby the further system is an inertial position system, e.g. loosely-coupled

Abstract

Embodiments include systems and methods of navigation. In on embodiment, a plurality of position and motion states of a vehicle are estimated. The states may be estimated based on information received from a satellite receiver and an inertial measurement sensor. Estimating the states comprises performing one or more of a plurality of update steps at the rate that information is received from the satellite receiver. The states are estimated at a rate greater than the rate at which the update steps are performed. In one embodiment, the states are estimated using a stepped extended Kalman filter.

Claims

1. A method of estimating a plurality of position and motion states of a vehicle, the method comprising:

receiving by a processor first data indicative of acceleration and angular rate of the vehicle from an inertial measurement sensor disposed proximal to the vehicle;

receiving by the processor second data indicative of position and velocity of the vehicle based on satellite data received by a receiver disposed proximal to the vehicle,

wherein the second data indicative of position and velocity of the vehicle is received; and

estimating the plurality of position and motion states of the vehicle based at least in part on the first and second data

wherein estimating the states comprises performing one of a plurality of update steps so as to estimate the states based on the plurality of update steps at a first rate and

wherein the states are estimated based on an inertial estimate at a second rate that is greater than the first rate.

2. The method of claim 1, wherein estimating the states comprises:

determining an estimate of a velocity error between a measured velocity of the vehicle and an estimated velocity state, wherein the estimated velocity state is one of the plurality of states and wherein the measured velocity is based at least in part on the second data; and

determining an estimate of a position error between a measured position of the vehicle and an estimated position state, wherein the estimated position state is one of the plurality of states and wherein the measured position is based at least in part on the second data, and

wherein estimating the states is based at least in part on the estimate of velocity error and the estimate of position error.

3. The method of claim 2, wherein the update steps comprise one of:

at least partially computing a state transition matrix;

at least partially computing a covariance matrix indicative of the statistical variance of the states with respect to each of the other ones of the states;

at least partially updating the plurality of position and motion states based on the estimate of the determined velocity error; and

at least partially updating the plurality of position and motion states based on the estimate of the determined position error.

4. The method of claim 1, wherein at least two of the plurality of steps comprise the step of at least partially computing a state transition matrix and whereby the state transition matrix is updated by performing the at least two of the plurality of steps.

5. The method of claim 1, wherein at least two of the plurality of steps comprise the step of at least partially computing a covariance matrix and whereby the covariance matrix is updated by performing the at least two of the plurality of steps.

6. The method of claim 1, wherein at least two of the plurality of steps comprise the step of at least partially updating the plurality of position and motion states based on the estimate of the determined velocity error and whereby the plurality of position and motion states is updated by performing the at least two of the plurality of steps.

7. The method of claim 1, wherein at least two of the plurality of steps comprise the step of at least partially updating the plurality of position and motion states based on the estimate of the determined position error and whereby the plurality of position and motion states is updated by performing the at least two of the plurality of steps.

8. The method of claim 1, further comprising outputting data indicative of at least a portion of the states of the vehicle to a control unit configured to control motion of the vehicle.

9. The method of claim 8, further comprising outputting a control signal based at least in part on the data indicative of at least a portion of the states of the vehicle.

10. The method of claim 1, wherein estimating the states comprises performing an extended Kalman filter.

11. The method of claim 1, wherein the vehicle comprises an aircraft.

12. The method of claim 1, wherein the plurality of states comprises each of position in Earth-Centered Earth-Fixed coordinates, each of north, east, and down velocity in tangent frame, attitude quaternion elements, rate gyroscope bias, and gravity.

13. A system estimating a plurality of position and motion states of a vehicle, the system comprising:

a memory configured to store data associated with a plurality of position and motion states of a vehicle;

a processor configured to;

receive first data indicative of acceleration and angular rate of the vehicle from an inertial measurement sensor disposed proximal to the vehicle;

receive second data indicative of position and velocity of the vehicle based on satellite data received by a receiver disposed proximal to the vehicle,

wherein the second data indicative of position and velocity of the vehicle is received; and

estimate the plurality of position and motion states of the vehicle based at least in part on the first and second data

wherein estimating the states comprises performing one of a plurality of update steps so as to estimate the states based on the plurality of update steps at a first rate and

wherein the states are estimated based on an inertial estimate at a second rate that is greater than the first rate.

14. The system of claim 13, wherein estimating the states comprises:

determining an estimate of a velocity error between a measured velocity of the vehicle and an estimated velocity state, wherein the estimated velocity state is one of the plurality of states and wherein the measured velocity is based at least in part on the second data; and

determining an estimate of a position error between a measured position of the vehicle and an estimated position state, wherein the estimated position state is one of the plurality of states and wherein the measured position is based at least in part on the second data, and

wherein estimating the states is based at least in part on the estimate of velocity error and the estimate of position error.

15. The system of claim 13, wherein the update steps comprise one of:

at least partially computing a state transition matrix;

at least partially computing a covariance matrix indicative of the statistical variance of the states with respect to each of the other ones of the states;

at least partially updating the plurality of position and motion states based on the estimate of the determined velocity error; and

at least partially updating the plurality of position and motion states based on the estimate of the determined position error.

16. The system of claim 13, wherein the processor is further configured to output data indicative of at least a portion of the states of the vehicle to a control unit configured to control motion of the vehicle.

17. The system of claim 16, wherein the processor is further configured to output a control signal based at least in part on the data indicative of at least a portion of the states of the vehicle.

18. The system of claim 13, wherein estimating the states comprises performing an extended Kalman filter.

19. The system of claim 13, wherein the vehicle comprises an aircraft.

20. The system of claim 13, wherein the plurality of states comprises each of position in Earth-Centered Earth-Fixed coordinates, each of north, east, and down velocity in tangent frame, attitude quaternion elements, rate gyroscope bias, and gravity.

21. A computer-program product for estimating a plurality of position and motion states of a vehicle:

a computer-readable medium having stored thereon codes executable by at least one processor to perform the method comprising;

receiving by a processor first data indicative of acceleration and angular rate of the vehicle from an inertial measurement sensor disposed proximal to the vehicle;

receiving by the processor second data indicative of position and velocity of the vehicle based on satellite data received by a receiver disposed proximal to the vehicle,

wherein the second data indicative of position and velocity of the vehicle is received; and

estimating the plurality of position and motion states of the vehicle based at least in part on the first and second data

wherein estimating the states comprises performing one of a plurality of update steps so as to estimate the states based on the plurality of update steps at a first rate and

wherein the states are estimated based on an inertial estimate at a second rate that is greater than the first rate.

22. The product of claim 21, wherein estimating the states comprises:

determining an estimate of a velocity error between a measured velocity of the vehicle and an estimated velocity state, wherein the estimated velocity state is one of the plurality of states and wherein the measured velocity is based at least in part on the second data; and

determining an estimate of a position error between a measured position of the vehicle and an estimated position state, wherein the estimated position state is one of the plurality of states and wherein the measured position is based at least in part on the second data, and

wherein estimating the states is based at least in part on the estimate of velocity error and the estimate of position error.

23. The product of claim 21, wherein the update steps comprise one of:

at least partially computing a state transition matrix;

at least partially computing a covariance matrix indicative of the statistical variance of the states with respect to each of the other ones of the states;

at least partially updating the plurality of position and motion states based on the estimate of the determined velocity error; and

at least partially updating the plurality of position and motion states based on the estimate of the determined position error.

24. The product of claim 21, wherein the method further comprises outputting data indicative of at least a portion of the states of the vehicle to a control unit configured to control motion of the vehicle.

25. The product of claim 24, wherein the method further comprises outputting a control signal based at least in part on the data indicative of at least a portion of the states of the vehicle.

26. The product of claim 21, wherein estimating the states comprises performing an extended Kalman filter.

27. The product of claim 21, wherein the vehicle comprises an aircraft.

28. The product of claim 21, wherein the plurality of states comprises each of position in Earth-Centered Earth-Fixed coordinates, each of north, east, and down velocity in tangent frame, attitude quaternion elements, rate gyroscope bias, and gravity.

29. A system for estimating a plurality of position and motion states of a vehicle, the system comprising:

means for storing data associated with a plurality of position and motion states of a vehicle;

means for processing configured to;

receive by first data indicative of acceleration and angular rate of the vehicle from an inertial measurement sensor disposed proximal to the vehicle;

receive second data indicative of position and velocity of the vehicle based on satellite data received by a receiver disposed proximal to the vehicle,

wherein the second data indicative of position and velocity of the vehicle is received; and

estimate the plurality of position and motion states of the vehicle based at least in part on the first and second data

wherein estimating the states comprises performing one of a plurality of update steps so as to estimate the states based on the plurality of update steps at a first rate and

wherein the states are estimated based on an inertial estimate at a second rate that is greater than the first rate.

30. The system of claim 29, wherein estimating the states comprises:

determining an estimate of a velocity error between a measured velocity of the vehicle and an estimated velocity state, wherein the estimated velocity state is one of the plurality of states and wherein the measured velocity is based at least in part on the second data; and

determining an estimate of a position error between a measured position of the vehicle and an estimated position state, wherein the estimated position state is one of the plurality of states and wherein the measured position is based at least in part on the second data, and

wherein estimating the states is based at least in part on the estimate of velocity error and the estimate of position error.

31. The system of claim 29, wherein the update steps comprise one of:

at least partially computing a state transition matrix;

at least partially computing a covariance matrix indicative of the statistical variance of the states with respect to each of the other ones of the states;

at least partially updating the plurality of position and motion states based on the estimate of the determined velocity error; and

at least partially updating the plurality of position and motion states based on the estimate of the determined position error.

32. The system of claim 29, wherein means for processing further comprise outputting data indicative of at least a portion of the states of the vehicle to a control unit configured to control motion of the vehicle.

33. The system of claim 32, wherein the means for processing further comprise outputting a control signal based at least in part on the data indicative of at least a portion of the states of the vehicle.

34. The system of claim 29, wherein estimating the states comprises performing an extended Kalman filter.

35. The system of claim 29, wherein the vehicle comprises an aircraft.

36. The system of claim 29, wherein the plurality of states comprises each of position in Earth-Centered Earth-Fixed coordinates, each of north, east, and down velocity in tangent frame, attitude quaternion elements, rate gyroscope bias, and gravity.