The present invention relates generally to navigation systems and in particular to a navigation system with minimal on-board processing.
Navigation systems used by vehicles such as aircraft, spacecraft, orbital vehicles, ships, motor vehicles and the like are used to determine the location or position of the vehicle with reference to the earth. A common navigational system uses global positioning satellites (GPS) that provides signals upon which a vehicle uses to estimate position and velocity. For some applications the determined GPS position and GPS velocity estimates do not provide the accuracy required due to noise factors in the GPS signals. Kalman filters have been used to increase the accuracy of the position and velocity estimates. A Kalman filter system is a computational solution for tracking a time-dependant state vector with noisy equations of motion in real time by a least square method. Kalman filters are used to separate signal from noise to optimally predict a modeled system with time. Typical Kalman filter systems are relatively complex systems that require a relatively large amount of processing power. In applications where a non-complex system is needed or where the processing power is limited, a typical Kalman filter system is not a viable option.
For the reasons stated above and for other reasons stated below which will become apparent to those skilled in the art upon reading and understanding the present specification, there is a need in the art for a relatively simple velocity and position estimate that requires relatively small processing power.
The above-mentioned problems of current systems are addressed by embodiments of the present invention and will be understood by reading and studying the following specification.
In one embodiment, a method of using a complementary filter is provided. The method includes integrating an acceleration estimate to determine an integrated acceleration estimate. Subtracting a GPS velocity from the integrated acceleration estimate to determine a velocity noise error. Filtering the velocity noise error and adding the filtered velocity noise error to the integrated acceleration estimate to obtain a velocity estimate.
In another embodiment, a method of determining a position estimate of an object is provided. The method comprises integrating an acceleration estimate in each of three dimensions to determine an integrated acceleration estimate for each of the dimensions. Subtracting a GPS velocity in each of the dimensions from the integrated acceleration estimate for each of the dimensions to determine a velocity noise error in each dimension. Filtering the velocity noise error in each dimension. Adding the filtered velocity noise error to the integrated acceleration estimate in each dimension to obtain a velocity estimate in each dimension. Integrating the velocity estimate in each dimension to determine an integrated velocity estimate in each dimension. Subtracting a GPS position in each dimension from the integrated velocity estimate in each dimension to determine a position noise error in each dimension. Filtering the position noise error in each dimension and adding the filtered position noise error in each dimension to the integrated velocity estimate to obtain a position estimate in each dimension.
In yet another embodiment, a machine readable medium having instructions stored thereon for generating a position estimate of an object in three dimensions is provided. The method of the machine readable instructions for each dimension includes integrating an acceleration estimate to determine an integrated acceleration estimate. Subtracting a GPS velocity from the integrated acceleration estimate to determine a velocity noise error. Filtering the velocity noise error. Adding the filtered velocity noise error to the integrated acceleration estimate to obtain a velocity estimate. Integrating the velocity estimate to determine a position. Subtracting a GPS position from the integrated velocity estimate to determine a position noise error. Filtering the position noise error and adding the filtered position noise error to the integrated velocity estimate to obtain a position estimate.
In still another embodiment, a position determining device to estimate the position of an object in three dimensions is provided. The determination of the position estimate in each dimension includes a means to determine an acceleration estimate. A means of integrating the acceleration estimate to determine an integrated acceleration estimate. A means of subtracting a GPS velocity from the integrated acceleration estimate to determine a velocity noise error. A means of filtering the velocity noise error. A means of adding the filtered velocity noise error to the integrated acceleration estimate to obtain a velocity estimate. A means of integrating the velocity estimate to determine an integrated velocity estimate. A means of subtracting a GPS position from the integrated velocity estimate to determine a position noise error. A means of filtering the position noise error and a means of adding the filtered position noise error to the integrated velocity estimate to obtain a position estimate.
In still yet another embodiment, a complementary filter position determining device is provided. The device includes a first filter circuit and a second filter circuit. The first filter circuit includes a first integrator, a first subtractor, a first low pass filter and a first adder. The first integrator is adapted to integrate an acceleration estimate to determine an integrated acceleration. The first subtractor is adapted to subtract a GPS velocity from the determined integrated acceleration estimate to determine a velocity noise error. The first low pass filter is adapted to filter the velocity noise error and the adder is adapted to add the filtered velocity noise error to the integrated acceleration estimate to determine a velocity estimate. The second filter circuit includes a second integrator, a second subtractor, a second low pass filter and a second adder. The second integrator is adapted to integrate the velocity estimate to determine an integrated velocity estimate. The second subtractor is adapted to subtract a GPS position from the determined integrated velocity estimate to determine a position noise error. The second low pass filter adapted to filter the position noise error and the second adder is adapted to add the filtered position noise error to the integrated velocity estimate to determine a position estimate.
In finally still another embodiment, a position estimate system is provided the position estimate system includes an acceleration determining function, an x filter circuit, a y filter circuit and a z filter circuit. The acceleration determining function is adapted to determine an acceleration estimate of an object in a first, second and third dimension. The x filter circuit is adapted to determine a position estimate in the first dimension based at least in part on the acceleration estimate in the first dimension and a GPS acceleration in the first dimension. The y filter circuit is adapted to determine a position estimate in the second dimension based at least in part on the acceleration estimate in the second dimension and a GPS acceleration in the second dimension and the z filter circuit adapted to determine a position estimate in the third dimension based at least in part on the acceleration estimate in the third dimension and a GPS acceleration in the third dimension.
The present invention can be more easily understood and further advantages and uses thereof more readily apparent, when considered in view of the description of the preferred embodiments and the following figures in which:
In accordance with common practice, the various described features are not drawn to scale but are drawn to emphasize specific features relevant to the present invention. Reference characters denote like elements throughout Figures and text.
In the following detailed description, reference is made to the accompanying drawings, which form a part hereof, and in which is shown by way of illustration specific embodiments in which the inventions may be practiced. These embodiments are described in sufficient detail to enable those skilled in the art to practice the invention, and it is to be understood that other embodiments may be utilized and that logical, mechanical and electrical changes may be made without departing from the spirit and scope of the present invention. The following detailed description is, therefore, not to be taken in a limiting sense, and the scope of the present invention is defined only by the claims and equivalents thereof.
Embodiments of the present invention provide a navigation system that estimates the position of an object in a fast and efficient manner. In particular, in embodiments of the present invention, a complementary filter system is used to determine the position of the object. The complementary filter system provides a relatively simple system that requires relatively less processing power than a typical Kalman filter system. This provides benefits over a typical Kalman filter system such as less software to code, less software to test, smaller on board processor required, less weight, and less power used.
Referring to
The velocity estimate is used by the position estimator 203/second filter circuit to determine position. The position estimator 203/second filter circuit includes a position (or second) integrator 210. The position integrator 210 integrates the velocity estimate into a position (or integrated velocity estimate). A position subtractor 212 is used to subtract the integrated velocity estimate from a GPS position to determine the position noise error. A position low pass filter 214 filters the position noise error. A position adder 216 is used to add the position noise error to the integrated velocity estimate to determine the position estimate. In embodiments of the present invention, position estimate is determined for each of the three dimensions.
A flow diagram 400 of the implementation of a position determining device, similar to that illustrated in
The velocity estimate is also integrated with a second integrator to determine a position (or integrated velocity estimate) (412). The integrated velocity estimate is supplied to a position subtractor and a position adder. A GPS position is also supplied to the position subtractor (407). The position subtractor subtracts the integrated velocity estimate from the GPS position to determine a position noise error (414). The position noise error is then filtered (416). The position adder then adds the integrated velocity estimate to the filtered noise error to determine the position estimate (420). The position estimate is then outputted (409).
As described above, the position determining devices of the present invention use an acceleration estimate that is input in to the device. An example of an acceleration estimate that can be used in embodiments of the present invention is provided below in Equation 1. Equation 1 estimates the acceleration of a vehicle in space such as a vehicle in orbit of the earth.
In equation 1, R is the radial distance from the vehicle to the center of the earth, Re (6378.137 km) is the equatorial radius of the earth, μ is the gravitation constant (0.3986×106 km2/sec2), Ωe (7.292115147×10−5 rad/sec) is the rotation rate of the earth and J2 (1.0826299×10−3) is the zonal harmonics that describe the earth's gravity.
As illustrated in
is multiplied by the output of block (510) to form
which is then negated by block (514) to form
R is inverted and multiplied by the radius of the earth Re in block (520) to form Re/R which is then squared by block (522) to form Re2/R2. The output of block (522) is multiplied by J2 (524) and then multiplied by 1.5 (526) to form the term
The z component of the position estimate {circumflex over (z)} is divided by R the result of which is squared and multiplied by 5 in block (534) to form
In block (548) the output of block 534 is subtracted from 3 (546) to form
In block (536) the output of block (534) is subtracted from 1 (538) to form
The outputs of block 536 and 548 are put into vector form and multiplied by the output of block (536) and (548) are put into vector form and multiplied (528) by the output of block (526) to form
The output of block (528) is summed (530) with 1 (532) to form the vector
The output of (514) is multiplied (516) by the output of (530) to form the vector
Ωe at block (540) is squared at (542) and multiplied (544) by the x and y components of the position estimate (501) to form the terms Ωe2{circumflex over (x)} and Ωe2ŷ. Ωe at block (540) is also multiplied by 2 (55) and −2 (552) to form the terms 2Ωe and −2 Ωe respectively. The outputs of (550) and (552) are multiplied (554) by the y and z components of the velocity estimate (502) to form the terms 2Ωe{circumflex over (ν)}y and 2Ωe{circumflex over (ν)}x. The outputs of (544) and (554) are summed together (545) and along with the 0 output of (556) are used to form the vector
which is input to (518). The inputs to (518) are summed to form the acceleration estimate (505) defined by equation 1.
The filter rate used in embodiments of the present invention to filter the velocity noise error and the position noise error will affect the performance of the position determining devices of embodiments of the present invention. For example, referring to
Embodiments of devices that make up the navigation systems of the present invention may be implemented in digital electronic circuitry, or with a programmable processor (for example, a special-purpose processor or a general-purpose process such as a computer firmware, software, or in combinations of them). Apparatus embodying these techniques may include appropriate input and output devices, a programmable processor, and a storage medium tangibly embodying program instructions for execution by the programmable processor. A process embodying these techniques may be performed by a programmable processor executing a program of instructions to perform desired functions by operating on input data and generating appropriate output. The techniques may be implemented in one or more programs that are executable on a programmable system including at least one programmable processor coupled to receive data and instructions from, and to transmit data and instructions to, a data storage system, at least one input device, and at least one output device. Generally, a processor will receive instructions and data from a read-only memory and/or a random access memory. Storage devices suitable for tangibly embodying computer program instructions and data include all forms of non-volatile memory, including by way of example semiconductor memory devices, such as EPROM, EEPROM, and flash memory devices; magnetic disks such as internal hard disks and removable disks; magneto-optical disks; and CD-ROM disks. Any of the foregoing may be supplemented by, or incorporated in, specially-designed application-specific integrated circuits (ASICs).
Although specific embodiments have been illustrated and described herein, it will be appreciated by those of ordinary skill in the art that any arrangement, which is calculated to achieve the same purpose, may be substituted for the specific embodiment shown. This application is intended to cover any adaptations or variations of the present invention. Therefore, it is manifestly intended that this invention be limited only by the claims and the equivalents thereof.
Number | Name | Date | Kind |
---|---|---|---|
5030958 | Schmalenberger et al. | Jul 1991 | A |
5184304 | Huddle | Feb 1993 | A |
5339684 | Jircitano et al. | Aug 1994 | A |
5359889 | Jircitano et al. | Nov 1994 | A |
5784029 | Geier | Jul 1998 | A |
6039028 | Stuntz et al. | Mar 2000 | A |
6145378 | McRobbie et al. | Nov 2000 | A |
6175806 | Thuente | Jan 2001 | B1 |
6246960 | Lin | Jun 2001 | B1 |
6292750 | Lin | Sep 2001 | B1 |
6298316 | Diesel | Oct 2001 | B1 |
6408245 | An et al. | Jun 2002 | B1 |
6417802 | Diesel | Jul 2002 | B1 |
6560535 | Levy et al. | May 2003 | B2 |
6631323 | Tucker et al. | Oct 2003 | B2 |
6859170 | Devereux et al. | Feb 2005 | B2 |
6900760 | Groves | May 2005 | B2 |
7043364 | Scherzinger | May 2006 | B2 |
20010020216 | Lin | Sep 2001 | A1 |
20020111717 | Scherzinger et al. | Aug 2002 | A1 |
20020143491 | Scherzinger | Oct 2002 | A1 |
20030055562 | Levy et al. | Mar 2003 | A1 |
20050065727 | Hu et al. | Mar 2005 | A1 |
20050125142 | Yamane | Jun 2005 | A1 |
20050203701 | Scherzinger | Sep 2005 | A1 |
20070016371 | Waid et al. | Jan 2007 | A1 |
20070156338 | Coatantiec et al. | Jul 2007 | A1 |
20090198444 | Takanashi et al. | Aug 2009 | A1 |
Number | Date | Country | |
---|---|---|---|
20070100546 A1 | May 2007 | US |