Patent Application
20080077325
Embodiments of 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 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 that form a part hereof, and in which is shown by way of specific illustrative embodiments in which the invention 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 scope of the present invention. The following detailed description is, therefore, not to be taken in a limiting sense.
In the embodiment of
As would be appreciated by one of ordinary skill in the art upon studying this specification, a Kalman filter calculates updates to a state vector based on measurement data updates, estimated sensor noise associated with those measurement data updates, estimates of process noise, and an error model that represent how the errors associated with measurement data change over time. The new state vector is calculated by multiplying a state transition matrix by a previous state vector. Thus, the state transition matrix determines how state vectors are updated between measurement data updates. In the past, inertial measurement data was considered the best data to use for determining the elements of a navigation Kalman filter's state transition matrix, because inertial measurement data demonstrates a relatively small drift error over short periods of time (i.e., in the order of seconds) and also because inertial sensor data is reliably available from inertial sensors. Thus, the state vector estimates and incremental corrections provided by such a Kalman filter were considered reliable estimates of “truth” (i.e. the true state of the system).
With the introduction of lower costs inertial sensors, the assumption that using inertial measurement data always provides the best estimate of “truth” over short periods of time is no longer valid. When certain environmental conditions (such as high vibration, for example) are present, other navigation aids may provide a better data for calculating the state transition matrix. Embodiments of the present invention selectively calculate specified elements in a state transition matrix (such as a state transition matrix for filter algorithm 130, for example) using data from navigation aids rather than data from inertial sensors during predefined events or conditions. Those conditions are defined by the known error characteristics of the inertial sensors, as described in greater detail below. Using a state transition matrix as improved by embodiments of the present invention, the filter algorithm 130 updates state vectors to more closely approximate “truth” than when compared to results based on inertial sensor data.
As one example, under ideal flight conditions state vector estimates calculated by a Kalman filter based on navigation signals from a Global Positioning System (GPS) satellite exhibit a greater measurement data error at 1 second than estimates calculated by a Kalman filter based on inertial sensors. GPS navigation signals also fail to provide certain data useful for updating elements of the transition matrix, such as vehicle 101 's attitude, for example. However, when low cost inertial measurement units are used and high vibrations are present, GPS signals will provide a better reference for updating the transition matrix elements than inertial data. Under such circumstances, a Kalman filter will produce state vector updates that more accurately reflect “truth” by incorporating GPS navigation data into the state transition matrix than with inertial measurement data.
To enable the incorporation of navigation aid data into the calculation of filter algorithm 130's state transition matrix, navigation system 100 provides the output of navigation aid 114 to processor 110. In one implementation of navigation system 100, navigation aid 114 is a global navigation satellite system (GNSS) receiver that produces position and velocity measurement data based on navigation signals from one or more GNSS satellites (not shown). Examples of a GNSS include, but are not limited to, the global positioning system (GPS), and the Galileo satellite system. In alternate embodiments and implementations, navigation aid 114 may include one or more other aids, such as, but not limited to a barometric altimeter, or a radar velocity sensor.
In operation, filter algorithm 130 receives inertial measurement data from inertial sensor 112, navigation aid data from navigation aid 114 as well as navigation data (i.e., position, velocity and attitude data) from navigation algorithm 140. Filter algorithm 130 is programmed to calculate a new state vector based on a previous state vector multiplied by the state transition matrix. Filter algorithm 130 outputs either all or part of the new state vector to navigation algorithm 140, including information that provides incremental corrections to the inertial measurement data provided by inertial sensor 112.
X[k+1]=Φ[k]X[k]
where X[k] (shown generally at 210) is an n×1 vector representing the state of the system at time index k, X[k+1] (shown generally at 230) is an n×1 vector representing the state of the system at time index k+1, and Φ[k is an n×n matrix representing the state transition matrix (shown at 220), as it exists at time index k. In one embodiment of the present invention, filter algorithm 130 is implemented using the state transition matrix 220 shown in
As previously discussed, under nominal operating conditions, a state transition matrix 220 having elements calculated from inertial sensor measurement data provides a state vector that most closely represent “truth” for use by navigation algorithm 140 in calculating a navigation solution. When certain anomalous operating conditions arise (such as high vibration, for example) the inertial sensor measurement data available to filter algorithm 130 degrades. If this degradation reaches a certain threshold, a state transition matrix 220 calculated from the inertial data will produce an erroneous state vector 230, causing navigation algorithm 140 to output an incorrect navigation solution. Embodiments of the present invention enable a navigation system to monitor for anomalous operating conditions and selectively calculate elements of the state transition matrix 220 using data from navigation aids or inertial sensor data, when the anomalous operating conditions reach predetermined thresholds. For example, in one embodiment, state transition matrix 220 element φi,j (shown generally at 222) is calculated using a first algorithm that incorporates inertial measurement data from inertial sensor 112. Filter algorithm 130 monitors for anomalous operating conditions for which element φi,j is known to be sensitive, based on the error characteristics of inertial sensor 112. When such an anomalous operating conditions arises and exceeds a pre-determine threshold, filter algorithm 130 begins calculating state transition matrix 220 element φi,j using a second algorithm that incorporates data from navigation aid 114. In one embodiment, an anomalous operating condition is said to exceed the pre-determined threshold when one or more parameters no longer fall within a pre-defined operating range. For example, in an implementation where the effects of high vibration levels are a concern, the filter algorithm 130 selectively switches from the first algorithm to the second algorithm based on factors such as, but not limited to, the root-mean-square (RMS) amplitude of a vibration, the frequency of a vibration, or a change in the vibration over a period of time. In another implementation, filter algorithm 130 selectively switches from a first algorithm to a second algorithm for calculating element φi,j based on the value of another element of state transition matrix 220, such as element φk,l (shown generally at 224) for example. Such an implementation is useful when the value of φk,l is indicative of a condition known to cause an increase in error from the inertial sensor. In another implementation, filter algorithm 130 can selectively switch from a first algorithm to a second algorithm based on the value of an element of a state vector, for the same reason described above.
For any of the elements of state transition matrix 220 a determination of whether to enable switching between an inertial data based algorithm and a navigation aid based algorithm is made during the design stage of system 100. During the design state, a system designer considers the error characteristics of the particular inertial sensors used in system 100. The system designer further considers the sensitivity of individual elements of state transition matrix 220 to those error characteristics. The system designer also considers the data available from any navigation aid 114 available to system 100, the error characteristics of that particular navigation aid, and then considers the sensitivity of individual elements of state transition matrix 220 to those error characteristics. The system designed would then select under what operating conditions a state transition matrix 220 element based on the navigation aid 114 produces a result that more closely approximates “truth” than one based on inertial sensor 112. The system designer would further determine what thresholds (if any) are appropriate for switching between an inertial data based algorithm and a navigation aid based algorithm for any particular element of state transition matrix 220. During real time operation, the filter algorithm 130 is enabled to switch between the algorithms based on the thresholds determined by the system designer.
As would be appreciated by one of ordinary skill in the art, different thresholds, based on different error characteristics, can be established for different elements of state transition matrix 220. Further, in other embodiments, filter algorithm 130 can utilize data from one or more of a plurality of different navigation aids to calculate different state transition matrix elements. In still other embodiments, elements of a state transition matrix can be calculated based on a combination of navigation aids and inertial sensors.
The method proceeds to 340 with monitoring for the existence of one or more operating conditions based on the known error characteristics of the at least one inertial sensor. Known error characteristics of the inertial sensor describe the inertial sensor's error response to various environmental conditions including factors such as, but not limited to, vibration, temperature and pressure. For example, in one embodiment where the inertial sensor's error characteristics indicate a sensitivity to vibrations, monitoring for the existence of one or more operating conditions further comprises monitoring for a predetermined level of vibration, monitoring for a predetermined frequency range of vibrations, or monitoring for a degree of change in a vibration over time. In one embodiment, the method further includes monitoring for the existence of one or more operating conditions based on known error characteristics of the at least one navigation aid. In another embodiment, monitoring for the existence of one or more operating conditions includes monitoring the value of an element of the state transition matrix that is indicative of a condition known to cause an increase in error from the inertial sensor, or monitoring the value of an element of a state vector for the same reason. When the one or more operating conditions do not exist (checked at 350), the method proceeds to 360 and the one or more elements of the state transition matrix are calculated based on the inertial measurement data from the inertial sensors. When the one or more operating conditions do exist (checked at 350), the method proceeds to 370 and the one or more elements of the state transition matrix are calculated based on the navigational aid data. In one embodiment, the method loops to repeat blocks 320 to 350 and recalculate the state transition matrix (at blocks 360, 370) based on the latest data available.
In one embodiment, the method further includes calculating a navigation solution, using a navigation algorithm, based on the inertial measurement data from the at least one inertial sensor. In one such embodiment, the filter algorithm calculates one or more incremental corrections to the inertial measurement data and provides the corrections to the navigation algorithm. In another such embodiment, the filter algorithm calculates one or more incremental corrections to the navigation data and provides the navigation data corrections to the navigation algorithm. Calculating a navigation solution comprises calculating navigation data such as a position, a velocity, and an attitude of a vehicle.
Although this specification has largely focused on the calculation of a state transition matrix for the purpose of calculating updates to the state vector, embodiments of the present invention are not so limited but include other applications of the state transition matrix. For example, a person of ordinary skill in the art would appreciate that updates to a Kalman filter error covariance matrix, P (a measure of the estimated accuracy of the state estimate) are also calculated with the state transition matrix using the formula:
P[k+1 =Φ[k]P[k]Φ[k]T+Q[k]
where P[k] represents the error covariance at time index k, P[k+1] represents the error covariance at time index k+1, Φ[k] represents the state transition matrix as it exists at time index k, and Q[k] represents the covariance of the process noise at time index k+1.
Several means are available to implement the systems and methods of the current invention as discussed in this specification. These means include, but are not limited to, digital computer systems, microprocessors, programmable controllers and field programmable gate arrays. Therefore other embodiments of the present invention are program instructions resident on computer readable media which when implemented by such controllers, enable the controllers to implement embodiments of the present invention. Computer readable media include any form of computer memory, including but not limited to punch cards, magnetic disk or tape, any optical data storage system, flash read only memory (ROM), non-volatile ROM, programmable ROM (PROM), erasable-programmable ROM (E-PROM), random access memory (RAM), or any other form of permanent, semi-permanent, or temporary memory storage system or device. Program instructions include, but are not limited to computer-executable instructions executed by computer system processors and hardware description languages such as Very High Speed Integrated Circuit (VHSIC) Hardware Description Language (VHDL).
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.
