Heading detecting method

Abstract: A heading detecting apparatus which calculates a Kalman filter gain by individually analyzing and evaluating error factors contained in the output data of turning angular velocity and magnetic sensors, determines the rate of use of the turning angular velocity and magnetic sensors on the basis of the Kalman filter gain and estimates a current heading of a moving body.

Description: FIELD OF THE INVENTION The present invention relates to heading (azimuth) detecting method, and more particularly to such a method which detects the heading of the yawing direction of a moving body by the use of a magnetic sensor and a turning angular velocity sensor (eg optical fiber gyro, mechanical type gyro, vibration gyro, gas rate gyro) for sensing the turning angular velocity of a moving body. DESCRIPTION OF THE PRIOR ART As a method for providing information about the actual location of a vehicle traveling streets, aircraft navigating air routes or ship navigating sea routes, there is known "dead reckoning," in which a distance sensor, a heading sensor (magnetic sensor or turning angular velocity sensor) and a processing unit (e.g., computer) for processing distance and heading data obtained from the distance and heading sensors are employed and the current location data of a moving body are obtained by using an amount of distance change δ1 and a heading ϑ (case of the magnetic sensor) or amount of heading change δϑ (case of the turning angular velocity sensor). A description of the "vehicle" will hereinafter be given, and in a case where "travel" of the vehicle and "navigation" of the aircraft or ship are used together, the "travel" is used. In the dead reckoning method, the east-west directional component δx (= δ1 x cos ϑ) and south-north directional component δy (= δ1 x sin ϑ) of the distance change amount δ1 that occurs as the vehicle moves along a street are calculated, and current location output data (Px, Py) are obtained by adding the calculated components δx and δy to the previous location output data (Px′, Py′). However, conventional systems using dead reckoning have their disadvantages in that the accumulation of error occurs due to inherent limitations on the achievable accuracy of the heading sensor. That is to say, when the heading sensor is a magnetic sensor which senses earth magnetism for obtaining the absolute heading of a moving body, the magnetic sensor senses the feeble intensity of the Earth's magnetic field. Therefore, if the moving body is magnetized, an error will occur in the output data from the magnetic sensor. In order to compensate for this error, the initialization of the magnetic sensor is performed. However, when the moving body passes through regions including magnetic disturbance, such as railroad crossings, places wherein power cables are buried, iron bridges, highways with sound insulating walls and high buildings, the moving body is subjected to the influence of the strong electromagnetic field and therefore the amount of the magnetization of the moving body varies. For this reason, sometimes errors occur again during traveling. Therefore, unless the magnetic sensor output data containing such magnetic disturbance is detected with accuracy and removed, an accurate heading of the moving body cannot be obtained. When, on the other hand, the turning angular velocity sensor is employed, it is known that errors in the sensor output data will appear frequently at the time that the heading change has become more than a predetermined value, at the time that the power source is turned on, at the time that the vehicle travels at very low speeds or at the time that it is detected that the vehicle is traveling on rough roads such as mountain roads. Unless compensation for that errors is made, the dead reckoned positions will become increasingly imprecise or inaccurate. Then, it has been proposed that both the turning angular velocity sensor and the magnetic sensor are used. If either the turning angular velocity sensor output data or the magnetic sensor output data is reduced in reliability, one data can be compensated for by the other data. Such a scheme is, for example, disclosed in GB-A- 2 216 272. That is to say, in a heading detecting apparatus, in which a current location of a moving body is obtained by reading and storing output data of the turning angular velocity and magnetic sensors and calculating the current heading of the moving body from those sensor output data and from the previous estimated heading, a current estimated heading of the moving body can be obtained by calculating Kalman filter gain in consideration of characteristic errors inherently contained in the output data from the turning angular velocity and magnetic sensors, and by processing the magnetic sensor heading data and the heading data calculated from the turning angular velocity sensor output, with a weight processing method based on the calculated Kalman filter gain. However, in this method, it is important how the characteristic error components contained in the output data from the turning angular velocity and magnetic sensors are evaluated. That is to say, the individual error components are evaluated by some method and if these are set to constant values, the processing can be most easily performed. However, setting to the constant values is insufficient because the error component in the output of the magnetic sensor is sometimes increased rapidly due to the changes in the magnetized amount during traveling, and it is desirable to evaluate the error components accurately at real time by some method. In addition, since a bias value that is contained in the output data from the turning angular velocity sensor varies with time, it is necessary to take into consideration the error in the turning angular velocity sensor data resulting from that variation. Furthermore, it is also necessary to take the scale factor (output gain) of the turning angular velocity sensor into consideration because sometimes the scale factor departs from a standard value. The inventor of the present invention has proposed a heading detecting apparatus (Japanese Patent Application No. 1-329851), which measures the output data from the turning angular velocity and magnetic sensors and processes them at real time and is capable of accurately estimating the current heading of a moving body with the aid of the data higher in reliability among the both output data from the turning angular velocity and magnetic sensors. In this apparatus, only dispersion values that are contained in the final output data from the turning angular velocity and magnetic sensors are measured and processing is performed in accordance with the measured dispersion values. Therefore, this apparatus does not take into consideration individual error factors contained in the output data of the turning angular velocity and magnetic sensors. It is, accordingly, an object of the present invention to provide a heading detecting apparatus which is capable of estimating a current heading a moving body accurately by individually analysing and evaluating the error factors contained in the heading data of a magnetic sensor and in the angular velocity data of the turning angular velocity sensor and by determining the rate of use of the output data of the turning anqular velocity and magnetic sensors. Reference is made to EP-A-0451988 (corresponding to Japanese application No. 2-81582) which relates to a similar system of heading detection. This document falls within the terms of Article 54(3) of the EPC. In this system, current heading derives from a weighted average of the magnetic sensor and gyro outputs using Kalman filtering. The weights are based on estimates of the errors in each output, but do not take account of the error in gyro heading output due to the gyro's scale factor (see p.21 ff.). SUMMARY OF THE INVENTION According to the present invention there is provided a method of detecting heading of a moving body from outputs of a magnetic sensor (42) and gyro (43), characterised by comprising the steps of: (a) sampling the output of said gyro (43) at regular intervals and estimating bias values based on the gyro outputs sampled while said moving body is in its stopped state; (b) calculating a dispersion value of said bias values; (c) calculating a current error that is contained in the output of said gyro (43), from said dispersion value of said bias values a previously calculated change rate of time of said dispersion value of said bias values multiplied by an elapsed time from the travel start of said moving body up to the present time, and the output of said gyro (43) multiplied by a scale factor corresponding to a known ratio of a turning angle measured from the output of said gyro (43) and an actual turning angle; (d) calculating a degree of magnetisation of said moving body on the basis of the outputs of said magnetic sensor (42) and said gyro (43) each time said moving body turns, and calculating a dispersion value of said degrees of magnetisation; (e) calculating a change between the latest calculated degree of magnetisation of said moving body and the previously calculated degree of magnetisation of said moving body each time said moving body turns, and calculating the dispersion value of the calculated changes in said degree of magnetisation; (f) estimating a current degree of magnetisation of said moving body and an error of said current degree of magnetisation, from said degree of magnetisation of said moving body and said dispersion value of said degree of magnetisation calculated at the step (d), and from said change in said degree of magnetisation of said moving body and said dispersion value of said change in said degree of magnetisation calculated at the step (e); (g) calculating a change in a difference between the heading data of said magnetic sensor (42) and heading data obtained from the output of said gyro (43) and its dispersions; (h) calculating an error that is contained in heading data output of said magnetic sensor (42), from said dispersion in said difference between the heading data of said magnetic sensor (42) and the heading data obtained from the output of said gyro (43), and from said error of the current degree of magnetisation of the moving body; (i) calculating a Kalman filter gain from said error in the output of said gyro (43) calculated at the step (c) and from said error of said heading data output of said magnetic sensor (42) calculated at the step (h) said errors representing the reliability of the respective output data and; (j) calculating a current estimated heading of said moving body by processing the heading data calculated from the magnetic sensor output and the heading data calculated from the gyro output with weight processing based upon said Kalman filter gain. Preferably, said moving body is a vehicle. First, the sampling of the output of the gyro (43) at regular intervals to estimate the bias values based on the gyro outputs while the moving body is in its stopped state is calculated by the step (a). The reason why the data from the gyro as the moving body is in its stopped state is sampled is that the gyro output during vehicle's stop contains the bias value only. Next, based on the dispersion value of said bias value calculated by step (a) that is multiplied by an elapsed time after the moving body moves, a change rate of time of the dispersion value of the bias value multiplied by the elapsed time, and the output of the gyro multiplied by a previously calculated rate of a turning angle of the gyro (43), a current error contained in the output of the gyro (43) is calculated by step (c). Next, by step (d), a degree of magnetisation of the moving body and the error thereof are calculated under a predetermined condition as the moving body is in its travelling state, and by step (e), a change in the degree of magnetisation of the moving body and a dispersion value are calculated. The "under a predetermined condition" are conditions such as curve travel and the like in which the degree of magnetisation of the moving body and error thereof can be calculated from the heading data of the magnetic sensor. The reason why the change in the degree of magnetisation of the moving body is calculated by step (e) is that the degree of magnetisation changes due to the above described factors (railroad crossings, etc). Next, by step (f), based on the degree of magnetisation of the moving body and the error of the degree of magnetisation that were calculated by step (d) and based on the change in the degree of magnetisation of the moving body and the error of the change that were calculated by step (e), a current degree of magnetisation of the moving body and an error of the current degree of magnetisation are calculated. And, by step (g), a change in a difference between the heading data of the magnetic sensor (42) and heading data obtained from the output of the gyro (43) is calculated. Sometimes a difference occurs between the heading data from the gyro and the heading data calculated from the magnetic sensor output. However, for a short period of time, that difference is caused by the error in the heading data of the magnetic sensor rather than by the error in the heading data calculated from the gyro output, so the difference is thought of as an error in the heading data of the magnetic sensor. In step (h), based on the error of the heading data of the magnetic sensor (42) calculated by step (g) and on the error of the degree of magnetisation calculated by step (f), an error that is contained in heading data output of the magnetic sensor (42) is calculated. And, with step (i), a reliability in the output data of each sensor is calculated from the error the output of the gyro calculated by step (c) and from the error of the heading data output of the magnetic sensor calculated by step (h), and then a Kalman filter gain is calculated. By step (j), by processing the heading data calculated from the magnetic sensor output and the heading data calculated from gyro output with weight processing based upon the Kalman filter gain, a current estimated heading of the moving body is calculated. Therefore, the current heading of the moving body can be estimated accurately by individually analysing and evaluating the error factors contained in the output data of the magnetic sensor and gyro and by determining the rate of use of the output data of the magnetic sensor and gyro. Particularly, by taking the error of the gyro scale factor into consideration, the error of the gyro output can be evaluated accurately. Therefore, the Kalman gain, which is the rate of use of the output data of the magnetic and turning angular velocity sensors, can be set at a suitable value, so that the vehicle heading can be sensed more accurately. Of course, in addition to the errors described above, various errors, such as an error of quantization at the time of A/D conversion, can be taken into consideration. BRIEF DESCRIPTION OF THE DRAWINGS The above and other objects and advantages will become apparent from the following detailed description when read in conjunction with the accompanying drawings wherein: Fig 1 is a block diagram of a heading detecting apparatus incorporating the principles of this invention; Fig 2 is a block diagram illustrating one embodiment of the heading detecting apparatus; Fig 3 is a flow chart illustrating a heading detecting sequence; Fig 4 is a diagram showing the travel track of a vehicle obtained with the aid of the heading detecting apparatus of the present invention; FIG. 5 is a graph illustrating a change in the heading during travel as the error of a gyro scale factor is taken into consideration and illustrating the rate of use of magnetic sensor data; and FIG. 6 is a graph illustrating a change in the heading during travel as the error of a gyro scale factor is not taken into consideration and illustrating the rate of use of magnetic sensor data. DESCRIPTION OF THE PREFERRED EMBODIMENTS FIG. 2 illustrates a preferred embodiment of a heading detecting apparatus of the present invention which may be employed in a vehicle navigational system. The heading detecting apparatus comprises a wheel sensor 41 which senses the number of rotations of the left and right wheels (not shown) respectively of a vehicle (this sensor is used as a distance sensor), a magnetic sensor 42 for sensing the heading of a vehicle, a first A/D (analog-to-digital) converter 42a connected to the magnetic sensor 42, a gyro 43, and a second A/D converter 43a connected to the gyro 43. The gyro 43 is selected from among an optical fiber gyro which reads a turning angular velocity as a phase change of interference light, a vibration gyro which senses a turning angular velocity with the aid of a cantilever vibration technique of a piezoelectric element, and a mechanical type gyro. The gyro 43 is used as a turning angular velocity sensor which senses the heading of a vehicle. The heading detecting apparatus further comprises a road map memory 2 for storing road map data, a locator 1 which calculates an estimated heading of a vehicle in accordance with the output data sensed by the gyro 43 and magnetic sensor 42 and also calculates the location of the vehicle with the aid of the data of the wheel sensor 41, a buffer memory 3 to which the location and heading of the vehicle are inputted, a data memory 6 connected to the locator 1 for storing magnetized amount data that are contained in the data of the magnetic sensor 42 and a dispersion of the magnetized amount data and for storing a gyro bias value that is contained in the data of the gyro and a dispersion of the gyro bias value, a navigation controller 5, a display 7 connected to the navigation controller 5 for displaying on the map the vehicle current location read out of the buffer memory 3, and a keyboard connected to the navigation controller 5. In the locator 1 described above, the number of rotations of the wheel is obtained by counting the number of the pulses outputted from the wheel sensor 41 with a counter (not shown), and travel distance output data per unit time are calculated by multiplying the count data of the counter by a predetermined constant number indicative of a distance per one count. Also, a relative change in the vehicle heading is obtained from the gyro 43. Then, based on the relative change and the absolute heading output data of the magnetic sensor 42, the locator 1 calculates the heading output data of the vehicle, as will be described below. The above described road map memory 2 stores road map data of a given area in advance and comprises a semiconductor memory, cassette tape, CD-ROM, IC memory, DAT or the like. The above described display 7 comprises a CRT display, crystalline liquid display or the like and displays a road map that the vehicle is travelling and a current location of the vehicle. The above described navigation controller 5 is constituted by a figure processor, an image processing memory and the like, and generates instructions to perform a retrieval of the map on the display 7, switching of scale, scrolling, a display of the vehicle current location and the like. The above described memory 6 stores a gyro bias value Bo, a gyro bias correction error qo, an estimated change rate ε of the gyro bias error, a noise component N that is contained in the gyro output, a scale factor error A of the gyro (rate of a turning angle measured from the gyro output and an actual turning angle), a magnetized amount Mn of the vehicle, a dispersion value Xn² of the magnetized amount Mn, a magnetized amount change δMn, and a dispersion value Yn² of the magnetized amount change δMn. These values are calculated as follows. The output data δϑG of the gyro 43 and the output data ϑH of the magnetic sensor 42 are sampled every a constant time. If it is assumed that the time to the current process from the previous process is δt, the number of samplings will be proportional to the time δt. The output data δϑG of the gyro as the vehicle is in its stopped state is normally zero, but it will appear if a bias occurs in the gyro. In estimating this gyro bias value Bo, the value, which has been used during the travel before the vehicle stops, is used as it is. Of course, the gyro output data during vehicle's stop can also be integrated and averaged. The error qo of the gyro bias value represents what extent of fluctuation the gyro bias value Bo has, and is obtained by reading a plurality of the gyro output data during stop and calculating a dispersion that is contained in that gyro output data. The estimated change rate ε of the gyro bias error is a value that is obtained from experience as a function of temperature, etc. The magnetized amount Mn can be obtained by calculating the center of a geomagnetism heading circle from a heading change amount as the vehicle turns and from the output data of the gyro 43 before and after the vehicle turns (see Japanese patent "kokai" publication No. 63-128222). The dispersion value Xn² of the magnetized amount Mn is a dispersion value that is contained in the magnetized amount Mn calculated each time the vehicle turns. The magnetized amount change δMn is a change in the magnetized amount (the moving direction and magnitude of the center of the geomagnetism heading circle) as the vehicle is subjected to a great change of the magnetic field during travel, and Yn² is a dispersion value of the magnetized amount change δMn. If it is assumed that the previous magnetized amount is M′ and the current magnetized amount is M, the magnetized amount change δMn will be calculated by the following equation: δMn = k1 + k2M′ + k3M + k4M′M + k5M′²M² - M where k1 to k5 are coefficients, respectively. The coefficients k1 to k5 are calculated based on the data as a specific vehicle was actually magnetized. The dispersion value Yn² is the remainder as the coefficients k1 to k5 were determined by the above method. The vehicle heading detecting sequence by the apparatus constructed as described above will hereinafter be described in detail. During travel, the vehicle location and map are displayed on the display 7 in accordance with the individual sensor output data read and stored in the locator 1. Also, during the display, the data (the number of breakins is indicated by a subscript n) of the magnetized amount Mn, the dispersion value Xn², the magnetized amount change δMn, and the dispersion value Yn² are read every a constant time by break-in, and then the vehicle heading is updated. The vehicle heading detecting flow at the time of this breakin is shown in FIG. 3. It is noted that the break-in may also be made every a constant distance that is obtained based on the output data indicative of the distances traveled by the vehicle. The above described constant time or distance is set, depending upon the type of the turning angular velocity sensor and the functional performance of the magnetic sensor. In step (1), the output data δϑG of the gyro 43 and the output data ϑH of the magnetic sensor 42 are read. Next, in step (2), the gyro bias value Bo, gyro bias error qo, change rate ε of the gyro bias error, noise component N, and the gyro scale factor error A are read out of the data memory 6. In step (3), the magnetized amount Mn, the dispersion value Xn², the magnetized amount change δMn, and the dispersion value Yn² are read out of the data memory 6. The magnetized amount Mn and the dispersion value Xn² are obtained only under specific conditions such as curve travel, so the latest values are not always obtained during travel. Therefore, in step (4), based on the magnetized amount change δMn and the dispersion value Yn², a Kalman filer coefficient an representing a weight ratio of a current estimated amount and the previous estimated amount, a current estimated magnetized amount µn and the dispersion value µvn² of µn are obtained as follows: µn = αnMn + (1 - αn) (Mn-1 + δMn) αn = (Yn² + µvn-1²)/(Xn² + Yn² + µvn-1²) µvn² = αnXn² Then, based on each data that were read out, the current estimated heading of a vehicle is obtained. In order to obtain the current estimated heading, in step (5) the dispersion qn² of the output data δϑH of the gyro 43 is first calculated by the following equation (I): qn² = (qo + εT)²δt² + N²δt + Aδϑ where T is the time that has elapsed from the previous vehicle's stop, qo is a gyro bias error (containing an error of quantization) which is a constant, εT is equal to the change rate of the gyro bias error multiplied by the elapsed time T and is an error resulting from a change (drift) of the gyro bias, and N² is a dispersion caused by noises. The reason that the errors qo and εT are added up is that these errors are not considered to be an independent phenomenon. If these errors are considered to be an independent phenomenon, then second power of qo and second power of εT will be added up (japanese application No. 2-81582). The reason that δt is not squared is that the noise error N is proportional to 1/2 power of the number of additions. δϑ is a change between the previous turning angle and the current turning angle, and Aδϑ is a dispersion of the turning angle caused by the error of the scale factor. The value of A depends on the surrounding temperature and is determined every a certain temperature range as a gyro standard. Therefore, the value of A is used as it is. Next, the dispersion rn² of the output data ϑH of the magnetic sensor 42 will be calculated by the following equation: rn² = rD² + rQ² + rL² + (τn² - σn-1² ) + rµn² where rD = constant part of an error that is contained in the output data ϑH of the magnetic sensor 42, rQ = quantization error = 1/(2 x geomagnetism level), rL = level error = 0.7 x | geomagnetism level difference |/estimated value of geomagnetism level, τn = difference between the heading obtained from the output of the gyro 43 and the heading of the magnetic sensor 42 = (ϑn-1 + δϑn) - ϑHn, σn-1² = dispersion of the previous estimated heading ϑn-1, τn² - σn-1² = dispersion contained in τn - dispersion of the previous estimated heading ϑn-1 = dispersion that is contained for the first time in the estimated heading, and rµn² = dispersion of the heading data of the magnetic sensor resulting from the dispersion of the magnetized amount. It is noted that it is assumed that the various errors described above arise independently of one another, and the dispersion rn² is obtained by the sum of dispersion values based on the individual factors. In step (6), by using qn² and rn², an estimated heading ϑ in which errors are taken into consideration is calculated by the following equation: ϑn = βnϑHn + (1 - βn)(ϑn-1 + δϑGn) where ϑ is a current heading, ϑn-1 is the previous heading, δϑGn and ϑHn are sensor output data that are used as the current heading is calculated, and βn is a Kalman gain which is a variable of 0 < β < 1. By using the previous Kalman gain βn-1, βn is obtained by the following equation: βn = (qn² + σn-1²)/(qn² + rn² + σn-1²) The dispersion of the estimated heading is calculated by the following equation: σn² = βnτn² As described above, the mean and dispersion of the gyro bias estimated value, the error of the change rate of that estimated value, the noise component, the error of the gyro scale factor, the mean and dispersion of the magnetized amount contained in the magnetic sensor output, and the mean and dispersion of the change amount of that magnetized amount have been calculated and stored. Then, when calculating the estimated heading of a vehicle, the dispersions that are contained in the output data of the gyro and magnetic sensor are respectively calculated from the aforesaid stored data, and an estimated heading can be obtained based on the data that have been weighted. From this estimated heading and the distance data of the wheel sensor 41, the estimated location of a vehicle can be calculated. At this point, it is a matter of course that a map matching method may be used which compares an estimated location with road map, evaluates a degree of correlation with respect to road map data, corrects the estimated location and displays the current location of a vehicle on roads (Japanese patent "kokai" publication Nos. 63-148115 and 64-53112). FIG. 4 illustrates the travel track of a vehicle that was obtained using the heading detecting apparatus described above. The vehicle starts from an A point and passes through railroad crossings B1, B2 and through corners D, E and returns back to the A point. Before and after the vehicle passes through an iron bridge C, it travels loops C1 and C2. The actual roads on the map are indicated by the solid line L of FIG. 4. The travel track obtained by the heading detecting apparatus of the present invention is indicated by the broken line L1. The travel track obtained by the following equation (II) in which the error A of the gyro scale factor is not taken into consideration is indicated by the one-dot chain line L2 (it is noted that EP-A-0 451 988) discloses equation in which the noise component N is ignored and the errors qo and εT are independent from each other, but it is essentially identical to equation (II)). qn² = (qo + εT)²δt² + N²δt As shown in FIG. 4, the travel track is departed from the actual road at the railroad crossing B1, but this is due to an error in the initial heading. FIG. 5 illustrates the estimated heading ϑ obtained by the above described equation (I) in which the error A of the gyro scale factor is taken into consideration, the heading based on the magnetic sensor, and the rate of use βn (Kalman gain) of the magnetic sensor data. FIG. 6 illustrates the estimated heading ϑ obtained by the above described equation (II) in which the error A of the gyro scale factor is not taken into consideration, the heading based on the magnetic sensor, and the rate of use βn (Kalman gain) of the magnetic sensor data. The abscissas of FIG. 5 represents the distances traveled along the D, E and A points of FIG. 4 by the vehicle. Likewise, the abscissas of FIG. 6 represents the distances traveled along the D, E and A points. The travel of the vehicle along the track L1 of FIG. 4 is first explained in conjunction with FIG. 5. When the vehicle travels over the loop C1 after it traveled the point D, the estimated heading ϑ repeats a change to 180° from -180° two times. That is, the estimated heading ϑ changes 360° along the loop C1. And, when the vehicle travels the iron bridge C, the noise component is slightly increased. When the vehicle travels over the next loop C2, the estimated heading ϑ repeats a change to 180° from -180° two times. At the F point, the vehicle undergoes extrinsic noises. After the vehicle travels the corner E, the direction of the vehicle changes to about right angles, so a constant change has arisen in the estimated heading ϑ. The difference between the tracks L1 and L2 will hereinafter be explained in conjunction with FIG. 6. As compared with FIG. 5, the difference is that the estimated heading after the E point is different, the rate of use βn of the magnetic sensor is low, and a difference between the heading of the magnetic sensor and the estimated heading is large. The reason that the rate of use βn is low can be considered as follows. When equation (II) is used, the error A of the gyro scale factor is not taken into consideration, so the errors contained in the gyro output are evaluated to be smaller than actual errors. That is, since the reliability in the gyro output is evaluated to be higher than actual reliability, so that the rate of use of the magnetic sensor output is reduced. For this reason, the difference between the heading of the magnetic sensor and the estimated heading becomes large. However, when equation (I) is used, the error A of the gyro scale factor is taken into consideration, so the reliability in the gyro output is not overestimated. Therefore, the rate of use βn of the magnetic sensor becomes higher. And, as a result of the reliability in the gyro output being evaluated correctly, the difference between the heading of the magnetic sensor and the estimated heading becomes smaller, and the estimated heading of the vehicle after the E point is more accurate than that obtained by equation (II). This is clear by the fact that in FIG. 4, the track L1 between the E and A points is parallel to the actual road L. Accordingly, when the reliability in the gyro output is evaluated, the accuracy in the detected heading can be considerably increased by taking the gyro scale factor error A into consideration. While the subject invention has been described with relation to the preferred embodiment, various modifications and adaptations thereof will now be apparent to those skilled in the art. For example, the noise component contained in equation (I) can be ignored.

Claims: A method of detecting heading of a moving body from outputs of a magnetic sensor (42) and a gyro (43), comprising the steps of: (a) sampling the output (δϑ G ) of said gyro (43) at regular intervals and estimating bias values (Bo) based on the gyro outputs sampled while said moving body is in its stopped state (b) calculating a dispersion value (qo) of said bias values (Bo); (c) calculating a current error (qn²) that is contained in the output of said gyro (43), from said dispersion value (qo) of said bias values, a previously calculated change rate (ε) of time of said dispersion value of said bias values multiplied by an elapsed time (T) from the travel start of said moving body up to the present time, and the output (δϑ G ) of said gyro (43) multiplied by a scale factor (A) corresponding to a known ratio of a turning angle measured from the output of said gyro (43) and an actual turning angle; (d) calculating a degree of magnetisation (Mn) of said moving body on the basis of the outputs of said magnetic sensor (42) and said gyro (43) each time said moving body turns, and calculating a dispersion value (Xn²) of said degrees of magnetisation; (e) calculating a change (δMn) between the latest calculated degree of magnetisation of said moving body and the previously calculated degree of magnetisation of said moving body, and calculating the dispersion value (Yn²) of the calculated changes in said degree of magnetisation; (f) estimating a current degree of magnetisation (µn) of said moving body and an error (µvn²) of said current degree of magnetisation, from said degree of magnetisation (Mn) of said moving body and said dispersion value (Xn²) of said degree of magnetisation calculated at the step (d), and from said change (δMn) in said degree of magnetisation of said moving body and said dispersion (Yn²) value of said change in said degree of magnetisation calculated at the step (e); (g) calculating a difference (τn) between the heading data (ϑHn) of said magnetic sensor (42) and heading data obtained from the output of said gyro (43) and its dispersion (τn²) (h) calculating an error (rn²) that is contained in heading data output (ϑH) of said magnetic sensor (42), from said dispersion (τn²) in said difference between the heading data of said magnetic sensor (42) and the heading data obtained from the output of said gyro (43), and from said error (µvn²) of the current degree of magnetisation of the moving body; (i) calculating a Kalman filter gain (βn) from said error (qn²) in the output of said gyro (43) calculated at the step (c) and from said error (rn²) of said heading data output of said magnetic sensor (42) calculated at the step (h), said errors representing the reliability of the respective output data; and (j) calculating a current estimated heading (ϑn) of said moving body by processing the heading data calculated from the gyro output with weight processing based upon said Kalman filter gain (βn). A heading calculation method as set forth in Claim 1, wherein said moving body is a vehicle.

Inventors: MATSUZAKI SHIN-ICHI OSAKA WORK, MATSUZAKI, SHIN-ICHI, OSAKA WORKS OF
Applicants: SUMITOMO ELECTRIC INDUSTRIES, SUMITOMO ELECTRIC INDUSTRIES, LIMITED
Application year: 1992
Main classification: G01C21
IPC codes: G01C21, G01C19, G01C17