System and method for real-time guidance and mapping of a tunnel boring machine and tunnel

Abstract

A system and methods are disclosed for providing the location of a tunnel boring machine (TBM) by establishing of a plurality of known locations or “monuments”; from these monuments located at least on, over or within the TBM's start point, known in the art as a “pit”. The present invention provides among other things an integrated navigation system that provides real-time parametric guidance information to the TBM, relative to the tunnel origin, past course and current trajectory, while simultaneously employing a non-contact measuring system in concert with said origin and course information for the final provision of an as-built map of tunnel dimensions and centerline.

Claims

1. A dual purpose geo-location and dynamic mapping system for a tunnel boring machine (TBM), the system comprising: a TBM; a first Inertial Navigation System (INS) mounted on a vehicle, wherein the vehicle is separate from the TBM; a position determining system for determining an accurate position of the vehicle; a first computer located on the vehicle, wherein the first computer is configured to collect data from a ranging device as the vehicle traverses the tunnel and store the data in a first memory; a second INS located on the TBM connected to a second computer and a second memory, wherein the second INS position data is updated by the first INS.

2. The system of claim 1, wherein the ranging device is mounted on the vehicle to make measurements orthogonal to the geometric centerline of the tunnel.

3. The system of claim 1, wherein the first memory is located on the vehicle.

4. The system of claim 1, wherein the stored data from the first memory is transmitted to the second memory when the vehicle arrives at a known and predetermined location with the TBM.

5. The system of claim 1, wherein the first INS is connected to the first computer.

6. The system of claim 1, wherein the second computer is configured to receive the stored data from the first computer.

7. The system of claim 1, wherein the second computer is configured to receive an updated position of the first INS.

8. The system of claim 1, wherein the second computer is configured to translate the first INS position to the second INS as a position update.

9. The system of claim 1, wherein the second computer is configured to determine real-time parametric guidance information for the TBM.

10. The system of claim 9, wherein the real-time parametric guidance information causes the TBM to at least one of maintain a current path and course correct to the design centerline of the tunnel.

11. A dual purpose geo-location and dynamic mapping method for a tunnel boring machine (TBM), the method comprising: operating a TBM; operating a first Inertial Navigation System (INS) mounted on a vehicle, wherein the vehicle is separate from the TBM; using a position determining system to determine an accurate position of the vehicle; operating a first computer located on the vehicle, wherein the first computer is configured to collect data from a ranging device as the vehicle traverses the tunnel and store the data in a first memory; using a second INS located on the TBM connected to a second computer and a second memory, wherein the second INS position data is updated by the first INS.

12. The method of claim 11, wherein the ranging device is mounted on the vehicle to make measurements orthogonal to the geometric centerline of the tunnel.

13. The method of claim 11, wherein the first memory is located on the vehicle.

14. The method of claim 11, wherein the stored data from the first memory is transmitted to the second memory when the vehicle arrives at a known and predetermined location with the TBM.

15. The method of claim 11, wherein the first INS is connected to the first computer.

16. The method of claim 11, wherein the second computer is configured to receive the stored data from the first computer.

17. The method of claim 11, wherein the second computer is configured to receive an updated position of the first INS.

18. The method of claim 11, wherein the second computer is configured to translate the first INS position to the second INS as a position update.

19. The method of claim 11, wherein the second computer is configured to determine real-time parametric guidance information for the TBM.

20. The method of claim 19, wherein the real-time parametric guidance information causes the TBM to at least one of maintain a current path and course correct to the design centerline of the tunnel.

Description

BRIEF DESCRIPTION OF THE DRAWINGS

[0031] A more complete understanding of the present invention may be derived by referring to the detailed description when considered in connection with the following illustrative figures. In the figures, like-reference numbers refer to like-elements or acts throughout the figures. The presently preferred embodiments of the invention are illustrated in the accompanying drawings, in which:

[0032] FIG. 1 is a perspective view of the locomotive in the launch pit according to the preferred embodiment of the present invention, depicting the physical method for the accumulation of locomotive initial geo-location data at the launch pit and the retransmission of said data to the locomotive.

[0033] FIG. 2 is a block diagram of the components contained within FIG. 1.

[0034] FIG. 3 depicts an alternate embodiment using a robotic total survey station mounted in the launch pit and a system of survey reflectors.

[0035] FIG. 4 is a block diagram depicting the components within FIG. 3 which provide for the accumulation and transmission of initial geo-location data.

[0036] FIG. 5 depicts an alternate embodiment using a robotic total survey station mounted on the locomotive and a system of survey reflectors in the launch pit.

[0037] FIG. 6 is a block diagram depicting the components contained within FIG. 5 which provide for the accumulation and transmission of initial geo-location data.

[0038] FIG. 7 depicts an alternate embodiment using a docking station and alignment marks at surveyed locations within the launch pit.

[0039] FIG. 8 is a block diagram depicting the components contained within FIG. 7 which provide for the accumulation and transmission of initial geo-location data.

[0040] FIG. 9 is a block diagram of the components contained within the locomotive.

[0041] FIG. 10 is a block diagram illustrating the components contained within the TBM.

[0042] FIG. 11 depicts the process elements associated with the utilization of geo-location input data.

[0043] FIG. 12 depicts the process elements associated with the utilization of updated geo-location information.

[0044] FIG. 13 is a block diagram illustrating the components contained within the fault tolerant inertial navigation system.

[0045] FIG. 14 depicts the process elements accomplished within the fault tolerant inertial navigation system componentry.

[0046] FIG. 15 is a block diagram illustrating the components utilized for the self-contained mapping system.

[0047] FIG. 16 depicts the process elements accomplished by the self-contained mapping system.

[0048] FIG. 17 is a block diagram of the SACore IMM for Automatic Guidance of a TBM.

DETAILED DESCRIPTION

[0049] In the following description, and for the purposes of explanation, numerous specific details are set forth in order to provide a thorough understanding of the various aspects of the invention. It will be understood, however, by those skilled in the relevant arts, that the present invention may be practiced without these specific details. In other instances, known structures and devices are shown or discussed more generally in order to avoid obscuring the invention. In many cases, a description of the operation is sufficient to enable one to implement the various forms of the invention, particularly when the operation is to be implemented in software. It should be noted that there are many different and alternative configurations, devices and technologies to which the disclosed inventions may be applied. The full scope of the inventions is not limited to the examples that are described below.

[0050] In the following examples of the illustrated embodiments, references are made to the accompanying drawings which form a part hereof, and in which is shown by way of illustration various embodiments in which the invention may be practiced. It is to be understood that other embodiments may be utilized and structural and functional changes may be made without departing from the scope of the invention.

[0051] FIG. 1 illustrates the preferred embodiment of the physical method for the accumulation of locomotive initial geo-location data within the launch pit 105 and the retransmission of said data to the locomotive 100. The locomotive 100 starts and ends each travel cycle through the tunnel 110 in the launch pit 105. Surrounding the launch pit 105 are three or more geo-location and retransmission devices 200. A locomotive mounted transceiver 115 receives and transmits the position data 225 (FIG. 2).

[0052] FIG. 2 describes the components illustrated in FIG. 1. The geo-location and retransmission devices 200 each contain at least one global positioning system (GPS) receiver 205, microprocessor 210, and transmitter 215. These devices 200, well known in the art of position determination and land survey techniques, are designed to receive a GPS signal 220 by which position data 225 is determined and relayed to a locomotive mounted transceiver 115 (FIG. 1).

[0053] These commercially available systems utilize two positioning and navigation systems in a single unit, the first is used within sight of earth-orbiting Global Navigation Satellite System (GNSS) satellites and the second in less than optimal GNSS locations. The locomotive 100 (FIG. 1) in the launch pit 105 (FIG. 1) will generally not be in line-of-sight of the earth-orbiting satellites. The locomotive mounted transceiver 115 (FIG. 1) receives transmissions from a group of at least three ground-based beacons stationed at precise known locations, each of which transmits a distinct signal, including location information. The means of data transmission may include wireless protocols such as 802.11g, Bluetooth, time modulated ultra-wide band (TM-UWB) or other such wireless methods as may arise with developing technologies.

[0054] Another embodiment of the present invention, illustrated in FIG. 3 and described by FIG. 4, uses a pit mounted robotic total survey station 400 comprising a robotic mount 405, a controller 410, and a total station 415 (similar in method and construction to the Trimble Series 6), and a survey station transceiver 420. The pit mounted robotic total survey station 400 is mounted along the perimeter of the launch pit 105 where it utilizes reflective monuments 310 outside of the launch pit 105 to determine its geo-location. A locomotive mounted reflective monument 305 is used to determine the location of the locomotive 100 relative to the known reflective monuments 310. The survey station transceiver 420 contained within the pit mounted robotic total survey station 400 transmits position data 225 to the survey station transceiver 420 on the locomotive 100.

[0055] According to yet another embodiment of the present invention, illustrated in FIG. 5 and described by FIG. 6, a locomotive mounted robotic total survey station 600, comprising a robotic mount 405, a controller 410, and total station 415. Within the method described by this embodiment, the locomotive mounted robotic total survey station 600 obtains the position of the locomotive 100 by triangulation with at least three reflective monuments 310 at surveyed locations within the launch pit 105. As depicted in FIG. 9, the locomotive mounted transceiver 115 is bypassed with this embodiment and position data 225 is direct fed to self-contained mapping system (SCMS) 900 (FIG. 9) onboard the locomotive 100.

[0056] A less technical yet viable alternative embodiment of the present invention is illustrated in FIG. 7 and described by FIG. 8, wherein the locomotive 100 stops at a fixed docking station 700 installed at the terminal point of the track in the pit 105. Alignment marks 705 may also be utilized in this embodiment either independently or in concert with the fixed docking station 700 to establish position data 225. The surveyed origin point established by the docking station 700 is a direct input to central processing unit one (CPU1) 1520 (FIG. 15) onboard the SCMS 900 (FIG. 9), offsets to the inertial navigation system (INS) are calculated and the INS is updated to the current position and time of position.

[0057] Referring now to FIG. 9, there is illustrated an embodiment of the present invention describing the componentry mounted on and within the body of the locomotive 100 for the gathering and storage of information related to the guidance of the TBM 1000 (FIG. 10) and the mapping of the tunnel 110 (FIGS. 1, 3, 5, 7). Within the preferred embodiment of FIG. 1 and the alternate embodiment of FIG. 3, a locomotive mounted transceiver 115 establishes a link by which position data 225 (FIGS. 2, 4, 6, and 8) is transferred to and received by CPU1 1520 (FIG. 15). In the case of the alternate embodiments of FIGS. 5 and 7, initial position data is directly provided to CPU1 1520 (FIG. 15). In each of the described embodiments, the position data 225 (FIGS. 2, 4, 6, and 8) from the SCMS 900 is utilized to either initialize or recalibrate fault tolerant inertial navigation system one (FTINS1) 1515 (FIG. 15) such that a point of origin, also known as initial position, within the launch pit (Position 1) is calculated. Data is transmitted from CPU1 920 via SCMS transmitter 1505 (FIG. 15) to the host PC 910 and to the TBM 1000 (FIG. 10). The data transmitted to the host PC 910 may be stored on internet based storage.

[0058] Referring now to FIG. 10, the docking process 1135 (FIG. 11) within an embodiment of the present invention establishes the actual position 1020 relative to the locomotive 100 (FIGS. 1, 2, 3, and 7), which is both necessary and sufficient to characterize the actual underground location of the TBM 1000. Central processing unit 2 (CPU2) 1010 receives updated position information (Position 2) 1015 from CPU1 1520 (FIG. 15) through TBM transceiver 1005. The TBM 1000 obtains the presumed position (Position 3) 1025 from FTINS2 1030 based on movement of the TBM 1000 (element 1200). CPU2 1010 calculates the difference between the updated position information (Position 2) 1015 and the presumed position (Position 3) 1025 (element 1205), and the difference is applied as a correction to the Position 3 1025 data in FTINS2 1030 (element 1210).

[0059] FIG. 11 depicts the process elements associated with the utilization of position input data provided by the componentry depicted in FIG. 1, the interaction of componentry depicted in FIG. 9 as the locomotive 100 transits the curvilinear tunnel path 1125, and the transmission of accumulated data to the TBM 1000 componentry of FIG. 10. Initial Position 0 is established at the geo-location and retransmission devices 200 (element 1100). The time modulated signal with Position 0 data is then transmitted to CPU1 1520 (FIG. 15) onboard the locomotive 100 (element 1105). CPU1 1520 (FIG. 15) calculates initial position of the locomotive 100 (Position 1 1400) (element 1110). The Position 1 1400 data is then provided to FTINS1 1515 (FIG. 15) (element 1115). FTINS1 1515 (FIG. 15) is then updated to Position 1 1400 (element 1120).

[0060] Two processes occur as the locomotive 100 (FIGS. 1, 3, 5, and 7) transits a curvilinear path 1120 through the tunnel 110 (FIGS. 1, 3, 5, and 7) from the launch pit 105 (FIGS. 1, 3, 5, and 7) to the TBM 1000 (FIG. 10). The first process is the establishment of guidance information for the TBM 1000 (FIG. 10) by the delivery of a position update to the FTINS2 1030 (FIG. 10). The second process is the active mapping of the tunnel 110 (FIGS. 1, 3, 5, and 7) as-built, accomplished by the measurement of distance to the tunnel walls 1130 by a SCMS 900 (FIGS. 9 and 15). These processes may be accomplished simultaneously during the transit from launch pit 105 (FIGS. 1, 3, 5, and 7) to docking with the TBM at Position 2 1135 or separately so as to focus on delivery of position data to the TBM 1000 (FIG. 10) on the incoming trip and to focus on tunnel mapping on the outgoing trip.

[0061] Within these two processes, whether simultaneous or separate, information from FTINS1 1515 (FIG. 15) and the SCMS 900 (FIGS. 9 and 14) is provided to CPU1 1520 (FIG. 15), which compiles the aforementioned data with the initial position data 1120. Upon docking 1135 with the TBM 1000 (FIG. 10), the FTINS microprocessor 1320 (FIG. 13) calculates the actual position and provides this data to the onboard CPU1 1520 (FIG. 15) (element 1140). The docking process 1135 establishes actual position relative to the locomotive 100 (FIGS. 1, 3, 5, and 7) and the TBM 1000 (FIG. 10). All information relative to travel and tunnel measurement from the SCMS 900 (FIG. 9) is retained onboard the locomotive 100 (FIGS. 1, 3, 5, and 7) until its return to the launch pit 105 (FIGS. 1, 3, 5, and 7), where data collected in terms of TBM 1000 (FIG. 10) position and tunnel measurements are uploaded to the host PC 910 (FIG. 9) and may be backed up to internet-based data storage.

[0062] Referring now to FIG. 12, the TBM 1000 (FIG. 10) obtains the presumed position (Position 3) 1025 (FIG. 10) from FTINS2 1030 (FIG. 10) based on movement of the TBM 1000 (FIG. 10) (element 1200). CPU2 1010 (FIG. 10) calculates the difference between the updated position information (Position 2) 1015 (FIG. 10) and the presumed position (Position 3) 1025 (FIG. 10) (element 1205), and the difference is applied as a correction to the Position 3 1025 (FIG. 10) data in FTINS2 1030 (FIG. 10) (element 1210). There is programmed within CPU2 1010 (FIG. 10) a known offset distance 1215 from FTINS2 1030 (FIG. 10) to the steering control point of the TBM 1000 (FIG. 10), and CPU2 1010 (FIG. 10) applies the aforementioned known offset to the Position 3 1025 (FIG. 10) data (element 1220) contained within FTINS2 1030 (FIG. 10). The designed tunnel route 1225 programmed within the read-only memory of CPU2 1010 (FIG. 10), and the as-designed tunnel route 1225 is now compared to the current position 1230. CPU2 1010 (FIG. 10) on the TBM 1000 (FIG. 10) now establishes a new immediate heading 1235 for the TBM 1000 (FIG. 10) to follow. This new heading is applied to a steering correction 1240 to the TBM 1000 (FIG. 10) for either a manual or automatic pilot to follow.

[0063] FIG. 13 depicts the components in the fault tolerant inertial navigation system (FTINS) 1300. Both the locomotive 100 (FIGS. 1, 3, 5, and 7) and the TBM 1000 (FIG. 10) are equipped with two inertial measurement units (IMUs) 1305 which include one or more angular rate sensors (gyroscopes) 1310 and one or more accelerometers 1315 which provide information to the FTINS microprocessor 1320. The output of the FTINS microprocessor 1320 describes the physical location of the locomotive 100 (FIGS. 1, 3, 5, and 7) relative to the known initial position data 225 (FIGS. 2, 4, 6, and 8), and the physical location of the TBM 1000 (FIG. 10) relative to its last position update.

[0064] FIG. 14 depicts the process elements accomplished within the FTINS 1300 (FIG. 13) componentry. The gyroscopes provide (ω.sub.x, ω.sub.y, ω.sub.z) 1405 and the accelerometers provide ({umlaut over (x)}, ÿ, {umlaut over (z)}) 1410 data. The FTINS microprocessor 1320 (FIG. 13) calculates change in position by integrating the acceleration data 1415. The FTINS microprocessor 1320 (FIG. 13) then receives the initialized position (Position 1) 1400 and compares it to the change in position 1420. The FTINS microprocessor 1320 (FIG. 13) finally calculates the current position 1425.

[0065] Referring now to FIG. 15, there is illustrated an embodiment of the present invention describing the self-contained mapping system (SCMS) 900 mounted within the locomotive 100 (FIGS. 1, 3, 5, and 7). The SCMS 900 uses a vibration isolation device 1510 and reflective monuments 310 installed on, or cast into, the tunnel walls to map the tunnels. The vibration isolation device 1510 comprises: FTINS1 1515; a contact-free 3D scanner 1525, such as a Light Detection and Ranging or Laser Imaging Detection and Ranging (LiDAR); CPU1 1520; a data storage unit 1530, such as a hard disk drive or a flash memory device; and a wired or wireless transmitter 1505. The SCMS 900 is capable of generating a 3D map of the tunnel 110 (FIGS. 1, 3, 5, and 7) during and after the tunnel boring process, generating an accurate measurement of the tunnel centerline, and, through the use of the reflective monuments 310 (FIGS. 3, 4, 5, and 6), the SCMS 900 may be used to observe the movement of fixed points on the tunnel wall during and after the tunnel boring process to determine change in tunnel geometry over time.

[0066] FIG. 16 depicts the process elements accomplished by the SCMS 900 (FIGS. 9 and 15). The SCMS 900 (FIGS. 9 and 15) applies a position offset to the FTINS1 1515 (FIG. 15) measurements in order to match the FTINS1 1515 (FIG. 15) reference frame with that of the 3D scanner 935 (FIGS. 9 and 15). The SCMS 900 (FIGS. 9 and 15) then measure the distance to the tunnel wall relative to the locomotive 100 (FIGS. 1, 3, 5, and 7) and geometric tunnel center 1600. The FTINS1 1515 (FIG. 15) estimates the absolute position of the locomotive 100 (FIGS. 1, 3, 5, and 7) (element 1605). Timestamps 1610, 1615 are applied to the two measurements and the data is stored. A position offset to the FTINS1 1515 (FIG. 15) is applied to the laser reference frame 1625. The data resulting from 1610 and 1625 is then correlated 1620 and used to: generate the geometric centerline of the tunnel 1630, extract reflective monument measurements 1635, and generate a 3D mesh of the tunnel wall 1640. The position of the reflective monuments 310 (FIGS. 3, 4, 5, and 6) can be extracted 1635 by isolating measurements from the 3D scanner 1525 (FIG. 15) which indicate a higher reflectivity, and individual reflective monuments can be identified by their specific reflectivity. Upon completing the trip through the tunnel, the SCMS 900 (FIGS. 9 and 15) uploads the measurements from the FTINS1 1515 (FIG. 15) and 3D scanner 1525 (FIG. 15) to an external computer via wired or wireless link 1645 for analysis of the as-built tunnel geometry.

Multi-Sensor Data Fusion:

[0067] Those skilled in the art of state estimation, robotics, and advanced defense avionics understand academically that sensor-fusion is the art of combining sensory data or data derived from disparate sources such that the resulting information is in some sense “better” than would be possible when these sources were used individually. This process is predicated on the covariance (or the measure of how much two variables vary together) of non-independent sources. The term “better” in the case above can mean more accurate, more complete, more dependable, or refer to the result of an emerging view or state estimation.

[0068] The data sources for a fusion process are not specified to originate from identical sources or sensors which may or may not be spatially and temporally aligned. Further one can distinguish direct fusion, indirect fusion, and fusion of the outputs of the former two. Direct fusion is the fusion of sensor data from a set of heterogeneous or homogeneous sensors, soft sensors, and history values of sensor data, while indirect fusion uses information sources like a prior knowledge about the environment and human input. Sensor fusion is also known as “multi-sensor data fusion” and is a subset of information fusion through an implementation of the probability theory.

[0069] Probability theory is the mathematical study of phenomena characterized by randomness or uncertainty. More precisely, probability is used for modeling situations when the result of a measurement, realized under the same circumstances, produces different results. Mathematicians and actuaries think of probabilities as numbers in the closed interval from 0 to 1 assigned to “events” whose occurrence or failure to occur is random. Two crucial concepts in the theory of probability are those of a random variable and of the probability distribution of a random variable.

[0070] Implementing the features described above with affordable instruments requires reliable real-time estimates of system state. Unfortunately, the complete state is not always observable. State Estimation takes all the data obtained and uses it to determine the underlying behavior of the system at any point in time. It includes fault detection, isolation and continuous system state estimation.

[0071] There are two parts to state estimation: modeling and algorithms. The overall approach is to use a model to predict the behavior of the system in a particular state, and then compare that behavior with the actual measurements from the instruments to determine which state or states is the most likely to produce the observed system behavior.

[0072] This is not well understood or currently implemented in the construction industry; the approach understood and practiced is logical decisions in linear and deterministic systems. If use cases require higher confidences in measurements, instrument specifications are tightened resulting in the undesired effect of cost and schedule increases. The environment we live and operate in is neither linear nor deterministic; use cases are infinite; and the perverse variability of the systems and potential errors cannot be modeled. The variability of the problem identified above includes aspects other than just spatial (i.e. precise location of the tunnel boring machine); temporal relationships are part of the fundamental intellectual structure (together with space and number) within which events must be sequenced, quantify the duration of events, quantify the intervals between them, and compare the kinematics of objects.

[0073] In any of the embodiments listed above; the use of Fusion Engine (FE) and Kalman filters in the guidance system of the TBM, will greatly improve position accuracy and reduce instrument costs. The FE continuously receives measurements from multiple sources and generates a state estimate and covariance (confidence) of the current position of the TBM; all updated position data measurements received are used to ensure the measurement data is within the FE estimates.

[0074] In order to continuously and accurately estimate the position of the TBM the Kalman filters in the preferred embodiment are implemented as an asynchronous n-scalable Interacting Multiple Model (IMM) estimation Filter. The IMM comprises multiple models of drift from position in order to accurately match the maneuvering and drift expectations.

[0075] Since the drift or progression of the gyros in either FTINS is not known ahead of time, an accurate model cannot be designed, so errors in the position estimation will occur. Adding process noise to model the TBM maneuvers or using a maneuver detector to adapt the filter has been used in the art, but detection delays and large estimation errors during maneuvers are still a problem. It is generally accepted that the Interacting Multiple Model (IMM) estimator provides superior tracking performance compared to a single Kalman Filter.

[0076] The IMM is based on using several models in parallel to estimate the maneuvering TBM's states. Each Kalman Filter, uses a different model for each maneuver, one models a constant state of the TBM, another models a position change in the longitudinal axis while another models a position change in the lateral axis and vertical axis. Switching between these models during each sample period is determined probabilistically. Unlike maneuver detection systems where only one filter model is used at a time, the IMM uses all filters. The overall state estimate output is a weighted combination of the estimates from the individual filters. The weighting is based on the likelihood that a filter model is the correct maneuvering TBM model.

[0077] The IMM estimator is a state estimation algorithm that uses Markovian switching coefficients. A system with these coefficients is described by r models, M.sup.1, M.sup.2, . . . , M.sup.r, and given probabilities of switching between these models. M.sup.j(k) denotes that model j (M.sup.j) is in effect during the sampling period ending at time t.sub.k, [t.sub.k-1, t.sub.k]. The dynamics and measurement for a linear system are given by

x(k)=Φ.sup.j(k,k−1)x(k−1)+G.sup.j(k,k−1)w.sup.j(k−1),  (1)

and

z(k)=H.sup.j(k)x(k)+v.sup.j(k),  (2)

[0078] where x(k) is the system state at time t.sub.k, z(k) is the measurement vector at time t.sub.k, Φ.sup.j(k,k−1) is the state-transition matrix from time t.sub.k-1 to time t.sub.k for M.sup.j(k), G.sup.j(k,k−1) is the noise input matrix, and H.sup.j(k) is the observation matrix for M.sup.j(k). The process noise vector w.sup.j(k−1) and the measurement noise vector v.sup.j(k) are mutually uncorrelated zero-mean white Gaussian processes with covariance matrices Q.sup.j(k−1) and R.sup.j(k) respectively.

[0079] The initial conditions for the system state under each model j are Gaussian random variables with mean x.sup.j(0) and covariance P.sup.j(0). These prior statistics are assumed known, as also is μ.sup.j(0)=Pr{M.sup.j (0)}, which is the initial probability of model j at t.sub.0.

[0080] The model switching is governed by a finite-state Markov chain according to the probability π.sub.ij=Pr{M.sup.j(k)|M.sup.i(k−1)} of switching from M.sup.i(k−1) to M.sup.j(k). The model switching probabilities, π.sub.ij, are assumed known and an example is

[00001]$\begin{array}{cc}{\pi }_{\mathrm{ij}}=\left[\begin{array}{cc}\mathrm{.95}& \mathrm{.05}\\ \mathrm{.05}& \mathrm{.95}\end{array}\right].& \left(3\right)\end{array}$

[0081] A block diagram of the IMM estimator with only two models, for simplicity, is shown in FIG. 17.

[0082] The inputs to the IMM estimator are {circumflex over (x)}.sup.1(k−1|k−1), {circumflex over (x)}.sup.2(k−1|k−1), P.sup.1(k−1|k−1), P.sup.2(k−1|k−1), and μ.sup.i|j(k−1|k−1), all from the sampling period ending at t.sub.k-1. Where {circumflex over (x)}.sup.1(k−1|k−1) is the state estimate from filter 1 at time t.sub.k-1 using measurements from time t.sub.k-1 and P.sup.1(k−1|k−1) is the corresponding state covariance matrix. Each of the filters use a different mixture of {circumflex over (x)}.sup.1(k−1|k−1) and {circumflex over (x)}.sup.2(k−1|k−1) for their input, For r models, this mixing allows the model-conditioned estimates in the current cycle to be computed using r filters rather than r.sup.2 filters, which greatly decreases the computational burden. The inputs to the filters, {circumflex over (x)}.sup.01(k−1|k−1), {circumflex over (x)}.sup.02(k−1|k−1), and the corresponding covariance matrices are computed in the Interaction (Mixing) block.

[0083] For the filter matched to M.sup.j(k), the inputs are

[00002]$\begin{array}{cc}.Math.{\hat{x}}^{0.Math.j}\left(k-1k-1\right)=\underset{i=1}{\overset{r}{.Math.}}.Math.{\mu }^{i|j}\left(k-1k-1\right).Math.{\hat{x}}^i\left(k-1k-1\right)& \left(4\right)\\ P^{0.Math.j}\left(k-1k-1\right)=\underset{i=1}{\overset{r}{.Math.}}.Math.{\mu }^{ij}\left(k-1k-1\right).Math.\left\{P^i\left(k-1k-1\right)+\left[{\hat{x}}^i\left(k-1k-1\right)-{\hat{x}}^{0.Math.j}\left(k-1k-1\right)\right]\star {\left[{\hat{x}}^i\left(k-1k-1\right)-{\hat{x}}^{0.Math.j}\left(k-1k-1\right)\right]}^T\right\},& \left(5\right)\end{array}$

where the conditional model probability is

[00003]$\begin{array}{cc}\begin{array}{c}{\mu }^{ij}\left(k-1k-1\right)=.Math.\mathrm{Pr}.Math.\left\{M^i\left(k-1\right)M^j\left(k\right),Z_1^{k-1}\right\}\\ =.Math.\frac{1}{{\mu }^j\left(kk-1\right)}.Math.{\pi }_{\mathrm{ij}}.Math.{\mu }^i\left(k-1k-1\right),\end{array}& \left(6\right)\end{array}$

and the predicted model probability is

[00004]$\begin{array}{cc}\begin{array}{c}{\mu }^j\left(kk-1\right)=.Math.\mathrm{Pr}.Math.\left\{M^j\left(k\right)Z_1^{k-1}\right\}\\ =.Math.\underset{i=1}{\overset{r}{.Math.}}.Math.{\pi }_{\mathrm{ij}}.Math.{\mu }^i\left(k-1k-1\right).\end{array}& \left(7\right)\end{array}$

[0084] Using the measurements, z(k), for the filter matched to M.sup.j(k), the updates are computed using the familiar Kalman Filter equations

{circumflex over (x)}.sup.j(k|k−1)=Φ.sup.j(k,k−1){circumflex over (x)}.sup.01(k|k−1),  (8)

P.sup.j(k|k−1)=Φ(k,k−1)P.sup.0j(k|k−1)[Φ.sup.j(k,k−1)].sup.T+G.sup.j(k,k−1)Q.sup.j(k−1)[G.sup.j(k,k−1)].sup.T  (9)

v.sup.j(k)=z(k)−H(k){circumflex over (x)}.sup.j(k|k−1),  (10)

S.sup.j(k)=H.sup.j(k)P.sup.j(k|k−1)[H.sup.j(k)].sup.T+R.sup.j(k),  (11)

K.sup.j(k)=P.sup.j(k|k−1)[H.sup.j(k)].sup.T[S.sup.j(k)].sup.−1,  (12)

{circumflex over (x)}.sup.j(k|k)={circumflex over (x)}.sup.j(k|k−1)+K.sup.j(k)v.sup.j(k),  (13)

P.sup.j(k|k)=[I−K.sup.j(k)H.sup.j(k)]P.sup.j(k|k−1),  (14)

where {circumflex over (x)}.sup.j(k|k−1) is the predicted state estimate under M.sup.j(k), P.sup.j(k|k−1) is the corresponding prediction covariance, v.sup.j(k) is the residual, S.sup.j(k) is the residual covariance matrix, K.sup.j(k) is the Kalman gain matrix, {circumflex over (x)}.sup.j(k|k) is the updated state estimate under M.sup.j(k), and P.sup.j(k|k) is the updated covariance matrix.

[0085] The likelihood of the filter matched to M.sup.j(k) is defined by Λ.sup.j(k)=f[z(k)|M.sup.j(k), Z.sub.1.sup.k-1], where f[|] denotes a conditional density. Using the assumption of Gaussian statistics, the filter residual and the residual covariance, the likelihood is

[00005]$\begin{array}{cc}{\Lambda }^j\left(k\right)=\frac{1}{\sqrt{\det .Math.\left[2.Math..Math.\pi .Math..Math.S^j\left(k\right)\right]}}.Math.\exp .Math.\left\{-{{\frac{1}{2}\left[v^j\left(k\right)\right]}^T\left[S^j\left(k\right)\right]}^{-1}.Math.v^j\left(k\right)\right\}.& \left(15\right)\end{array}$

The probability for M.sup.j(k) is

[00006]$\begin{array}{cc}{\mu }^j\left(kk\right)=\mathrm{Pr}.Math.\left\{M^j\left(k\right)Z_1^k\right\}=\frac{1}{c}.Math.{\mu }^j\left(kk-1\right).Math.{\Lambda }^j\left(k\right),& \left(16\right)\end{array}$

where the normalization factor c is

[00007]$\begin{array}{cc}c=\underset{j=1}{\overset{r}{.Math.}}.Math.{\mu }^i\left(kk-1\right).Math.{\Lambda }^i\left(k\right).& \left(17\right)\end{array}$

[0086] These computations are performed in the Model Probability Update block. Finally the combined state estimate {circumflex over (x)}(k|k) and the corresponding state error covariance for the IMM are given by

[00008]$\begin{array}{cc}.Math.\hat{x}\left(kk\right)=\underset{j=1}{\overset{r}{.Math.}}.Math.{\mu }^j\left(kk\right).Math.{\hat{x}}^j\left(kk\right),& \left(18\right)\\ P\left(kk\right)=\underset{j=1}{\overset{r}{.Math.}}.Math.{\mu }^j\left(kk\right).Math.\left\{P^j\left(kk\right)+{\left[{\hat{x}}^j\left(kk\right)-\hat{x}\left(kk\right)\right]\left[{\hat{x}}^j\left(kk\right)-\hat{x}\left(kk\right)\right]}^T\right\}.& \left(19\right)\end{array}$

[0087] The final state estimate, {circumflex over (x)}(k|k), is the best estimate of the TBM state and P(k|k) is the error covariance matrix for this optimal state estimate.

[0088] For the sake of convenience, the operations are described as various interconnected functional blocks or distinct software modules. This is not necessary, however, and there may be cases where these functional blocks or modules are equivalently aggregated into a single logic device, program or operation with unclear boundaries. In any event, the functional blocks and software modules or described features can be implemented by themselves, or in combination with other operations in either hardware or software.

[0089] Having described and illustrated the principles of the invention in a preferred embodiment thereof, it should be apparent that the invention may be modified in arrangement and detail without departing from such principles. Claim is made to all modifications and variation coming within the spirit and scope of the following claims.