JPH06341848A - Navigation apparatus - Google Patents

Abstract

PURPOSE:To provide a navigation apparatus where a sensor noise is estimated properly and quickly and high-accuracy positional information is obtained by performing a modeling operation which is specific to various kinds of sensors. CONSTITUTION:Sensor signals from direction sensors 11, 12 which detect the direction of a moving body are inputted to a filter processing means 1, an estimated direction 5 is found, and the running position of the moving body is found together with velocity data by a velocity sensor 15. At this time, any of direction data, velocity data and a filter-output estimated value 7 is used as an input, errors of the individual direction sensors are evaluated by a sensor- error evaluation means 3, and a filter gain is computed by a filter-gain computation means 2 on the basis of its output. Thereby, the accuracy of the estimated direction 5 which is output finally is enhanced.

Description

Detailed Description of the Invention

[0001]

BACKGROUND OF THE INVENTION 1. Field of the Invention The present invention detects the motion of a moving body by various sensors provided in the moving body, combines the detection signals thereof, and performs a filtering process to obtain accuracy of position information of the moving body. The present invention relates to a navigation device that improves the performance.

[0002]

2. Description of the Related Art Conventionally, as a method of detecting the position of a vehicle traveling in an arbitrary place of a road traffic network, processing is performed by using a distance sensor, a direction sensor, and output signals from both sensors. There is a dead reckoning navigation system that includes a processing device and that obtains current position data of a vehicle while adding up a distance change amount and an azimuth change amount that accompany traveling of the vehicle.

However, in this dead reckoning, the errors that the distance sensor and the direction sensor inevitably have are accumulated with the traveling distance. Therefore, there is a problem that errors included in the obtained current position data are also accumulated.

In particular, the azimuth sensor has conventionally been used by combining a geomagnetic sensor capable of detecting the absolute azimuth of the vehicle and a gyro sensor capable of detecting a relative azimuth change of the vehicle. The traveling direction of is detected.

However, each sensor has an error characteristic peculiar to each sensor. The geomagnetic sensor generates short-term noise in a place where the magnetic field environment is bad, which is affected by high-voltage lines and buildings, but outputs a relatively correct direction in the long-term. On the other hand, the gyro sensor is not affected by the magnetic field environment at all, and outputs correct relative direction change in the short term, but drifts in the long term (variation of angular velocity bias).
Due to the influence of, the azimuth error is accumulated over time.

As one method for solving such a problem, there is a method disclosed in Japanese Patent Laid-Open No. 1-219610, in which a sensor signal is filtered. This is a method to detect the situation of the magnetic field environment from the difference in the direction change amount between the geomagnetic sensor and the gyro sensor per unit time, and adjust the magnitude of the filter gain in order to approach the true geomagnetic direction accordingly. .

As a method similar to this, there is an example shown in Japanese Patent Laid-Open No. 3-188316 in which Kalman filter processing is introduced in the calculation of the filter gain. In this method, for example, from the dispersion value of the orientation data of the geomagnetic sensor and the gyro sensor when traveling straight,
Determine the noise component of each sensor. After that, based on the value of the noise component, by the Kalman filter algorithm,
The filter gain weighted in the direction determined by the sensor with less noise is calculated.

Qualitatively, both of these conventional systems place weight on the gyro sensor in the short term and gradually shift the weight to the geomagnetic sensor in the long term because of the characteristics of the direction sensors described above. Is. Then, the speed of convergence to the geomagnetic azimuth (filter gain) is determined from the difference in the azimuth change amount of each sensor, and the latter is determined from the variance value of each sensor data during straight running. That is, the speed of convergence to the geomagnetic direction is determined only from the output data of the geomagnetic sensor and the gyro sensor.

[0009]

However, in the navigation device, the speed of convergence to the geomagnetic direction is closely related to the vehicle speed data. That is, when considering the convergence to the geomagnetic azimuth in a navigation device that constantly obtains the traveling position from the traveling distance integrated with the vehicle speed and the estimated azimuth, not the convergence with respect to the passage of time but the convergence with respect to the traveling distance. Should be considered. That is, as the vehicle speed increases and the traveling distance increases, a faster and more accurate traveling direction is required.

On the other hand, in the conventional example, to give an extreme example, even when the vehicle is stopped (a heading error does not lead to a position error), it is running at a high speed (a slight heading). Error causes a large position error)
As long as the noise characteristics of the geomagnetic sensor and the gyro are the same, there is a problem that the convergence speed to the geomagnetic direction is the same.

In the former conventional method described above, the physical basis of the equation for calculating the filter gain from the difference in the azimuth change amount of both sensors is poor, and the azimuth error due to this is unavoidable.

On the other hand, in the latter method, the Kalman filter algorithm is adopted as a rational framework for determining the filter gain. However, on the other hand, the opportunity to change the filter gain is only when the data during straight running is accumulated and statistical processing becomes possible. In other words, the frequency of filter gain determination is extremely low,
It's hard to say real time. Conversely, until statistical processing becomes possible and an appropriate filter gain is determined, an inappropriate gain will continue to be used, leading to deterioration in position detection accuracy. .

There is also a problem in the statistical processing method for estimating this sensor noise. The sensor noise is unique to each sensor. For example, the geomagnetic sensor has magnetizing noise, and a large offset error occurs in the sensor output in an instant. In the case of a gyro sensor, azimuth errors in one direction gradually accumulate.

Furthermore, in the above conventional example, there is no modeling of these noises. In the prior art, the raw azimuth data output from the sensor is statistically processed, the variance value is calculated, and the filter gain is obtained from the calculated variance value. In this method, when the above-mentioned noise peculiar to each sensor occurs, an appropriate countermeasure cannot be taken, and as a result, a large azimuth error may occur.

The problems of the conventional example in the filter processing applied to the direction estimation have been described above. However, this is not limited to bearing estimation. Similarly, using a sensor,
This is a common problem in estimating a specific physical quantity related to navigation, for example, in the case of position estimation.

The present invention has been made in view of the above problems, and a navigation device for estimating the sensor noise properly and quickly by performing modeling peculiar to various sensors to obtain more accurate position information. Is intended to provide.

[0017]

The above object is to provide a speed sensor for detecting the speed of a moving body, one or more sensors for detecting a physical quantity other than the speed, which indicates the movement of the moving body, and the sensors and the speed sensor. In the navigation device that obtains the traveling position of the moving body using the output from the sensor group, the output from the sensor group is subjected to filter processing having a predetermined filter characteristic, and the processing result is used to estimate. One or more sensors using at least one of a filter processing unit for calculating a value, outputs from one or more sensors other than the speed sensor, and an estimated value output from the filter processing unit. Sensor error evaluation means for evaluating the output error from the sensor error evaluation means, and adjusting the filter characteristics of the filter processing means based on the output of the sensor error evaluation means
It can be achieved by a navigation device, characterized in that it has a filter gain calculation means for calculating a filter gain.

The above-described object is also to provide a sensor group including a plurality of sensors for detecting a physical quantity representing the motion of the moving body and an output of the sensor group to obtain the position of the moving body. Filtering means for applying a filtering process having a predetermined characteristic to a plurality of outputs, and using the processing result to calculate an estimated value of a physical quantity related to the motion of at least one moving body,
A sensor error evaluation unit that evaluates the error of one or more sensors by inputting one or more of the plurality of outputs from the sensor group and the estimated value output from the filter processing unit, and the sensor error evaluation unit. Can be used to determine a window characteristic, and window processing means for eliminating an abnormal value in the output from one or more sensors depending on the characteristic can be achieved.

[0019]

According to the present invention, a speed sensor for detecting the speed of a moving body, one or more sensors for detecting a physical quantity other than the speed, which represents the motion of the moving body, and outputs from a sensor group including these sensors and the speed sensor. In the navigation device that obtains the traveling position of the moving body using, the filter processing means performs filter processing having a predetermined filter characteristic on the output from these sensor groups, and uses the processing result to determine the speed. An estimated value relating to a physical quantity representing the motion of a moving body other than is calculated.

The sensor error evaluation means uses one or more of the estimated value output from the filter processing means and the output from one or more sensors other than the speed sensor, The output error from the sensor can be properly evaluated.

Based on the output of this sensor error evaluation means,
The filter gain calculation means calculates a filter gain for adjusting the filter characteristic of the filter processing means.

As described above, by using the calculated filter gain, the characteristics of the filter processing means are properly adjusted, and the accuracy of the finally output estimated value is improved.
Therefore, using the estimates thus determined,
It is possible to determine the position or orientation of the moving body with high accuracy.

According to the present invention, the filter processing means is used in the navigation device for determining the position of the moving body by using a sensor group including a plurality of sensors for detecting a physical quantity representing the motion of the moving body and an output from the sensor group. Performs a filter process having a predetermined characteristic on a plurality of outputs from the sensor group, and uses the process result to calculate an estimated value of a physical quantity related to the motion of at least one moving body.

Next, the sensor error evaluation means receives one or more of a plurality of outputs from these sensor groups and an estimated value output from the filter processing means as an input, and receives one or more sensors. Evaluate the error of.

Finally, the window processing means determines the window characteristic by using the output of the sensor error evaluation means,
The characteristic excludes outliers in the output from one or more sensors.

As described above, the accuracy of the estimated value output from the filter processing means is finally improved by adopting the window processing means for excluding the abnormal value by using the result of the error evaluation of the sensor. You can Therefore, by using the estimated value determined in this way, the position or orientation of the moving body can be determined with high accuracy.

[0027]

Embodiments of the apparatus according to the present invention will be described below with reference to the drawings. FIG. 18 shows an example of a configuration diagram of a vehicle navigation device to which the present invention is applied. In the present embodiment, a vehicle speed sensor 23 that measures the speed of a moving body, a geomagnetic sensor 21 and a turning angular velocity sensor 22 that measure the azimuth of the moving body, and signals from these sensors are used to implement processing means described below. It has a controller 20 for calculating the operation position. Further, in the present embodiment, the map memory 25 for storing map data, the display device 26 for displaying the position of the moving body calculated by the controller 20 on the map read from the map memory 25, the initial position, etc. And an initial position input device 27 for inputting.

Here, the geomagnetic sensor 21 and the turning angular velocity sensor 22 are used as direction sensors, and the vehicle speed sensor 23 is used as a speed sensor.

The azimuth sensor is typically a geomagnetic sensor 21 that detects the earth's magnetic field (geomagnetism) to obtain the absolute azimuth. However, the GPS positioning device can also be used as an absolute azimuth sensor that obtains the traveling azimuth from the time change of the position obtained thereby.

The turning angular velocity sensor 22 measures the turning angular velocity of the moving body. Specifically, there are mechanical gyros, vibration gyros, optical fiber gyros, etc., which are used in navigation devices for aircraft, spacecraft, and automobiles. These can also be used as an orientation sensor if a reference orientation is given.

The vehicle speed sensor 23 is a photoelectric or electromagnetic speed sensor that outputs a pulse signal according to the wheel rotation speed in a vehicle such as an automobile or an electric train. Further, in aircraft, there is a speed sensor that utilizes a change in atmospheric pressure applied to a moving body.

The map memory 25 stores road map data covering a predetermined range in advance. Specifically, semiconductor memory, cassette tape, CD-ROM,
IC memory, DAT, etc. can be used.

The display device 26 displays a road map and a vehicle position while the vehicle is traveling by using a CRT, a liquid crystal display or the like.

The controller 20 calculates the estimated azimuth of the vehicle based on the data detected by the geomagnetic sensor 21 and the turning angular velocity sensor 22, and calculates the position of the vehicle together with the data of the vehicle speed sensor 23. Further, a graphic processor, an image processing memory, etc. can be provided, and it is possible to perform map search, scale reduction, scroll, display of the vehicle position of the above calculation results, etc. on the display device 26.

The initial position input device 27 is capable of inputting the vehicle position, such as a keyboard and a joystick.

The operation of this embodiment will be described with reference to the flowchart of FIG. When the system of this embodiment is started, first, initial processing is performed (step 200).
Here, when Kalman filter processing is applied to the filter processing means, initial processing of Kalman filter processing described later is also performed. When the current position is set by the initial position input device 27 such as key input (step 20)
2) A map of the current location of the vehicle and its surroundings is displayed on the display device 26 (step 204).

Next, in step 206, step 2
Interrupts 08 and below are permitted and the following main loop is entered. In this main loop, the current vehicle position and the map are updated by integrating and adding the traveling amounts for each unit traveling distance.

Travel direction calculation interrupt processing (step 208)
By the travel vector product division processing (step 210), a new vehicle position is calculated, and when it is determined that the vehicle position has moved (Yes in step 212), the current position and the surrounding map are updated (step 214).

Travel direction calculation interrupt processing (step 208)
Then, the sensors 21 and 22 for measuring the azimuth are read, and the azimuth is calculated by filtering. The specific operation of step 208 will be described in the first embodiment in which Kalman filter processing is used as the subsequent filter processing means.

Running vector product division processing (step 21)
The processing procedure of 0) will be described with reference to FIG. This process is executed every time the vehicle travels the predetermined distance ΔD.
That is, the vehicle direction θ is read in step 400. This vehicle direction θ is calculated and stored in the process of step 208 of FIG.

Using this vehicle direction, the x-direction component and the y-direction component of the distance ΔD are obtained (step 402). Finally, the obtained component is added to the x-direction integrated distance and the y-direction integrated distance to update the integrated distance (step 4).
04).

When the above processing is performed, the coordinate position calculated by the latest x direction integration processing and y direction integration processing in the processing of FIG. 15 is determined as the current traveling position of the vehicle.

The basic features of the navigation device according to the present invention will be described with reference to FIGS. In the first and subsequent embodiments shown below, a configuration combining these basic features will be described.

FIG. 1 shows a characteristic part of an example of a navigation device to which the present invention is applied. Here, the basic configuration of the present invention has a configuration as shown in FIG. 18 described above. Here, the controller 20 of the navigation device
In FIG. 18, a filter processing unit 1 that outputs an estimated azimuth 5 and an estimated value 7 including the azimuth, a filter gain calculation unit 2 that determines a gain of the filter processing unit 1, and an azimuth shown in FIG. Sensors 11, 12 and speed sensor 1
The sensor error evaluation means 3 for evaluating the error 5 is included. Here, the direction sensors 11 and 12 correspond to the geomagnetic sensor 21 and the turning angular velocity sensor 22 in FIG. 18, and the velocity sensor 15 corresponds to the vehicle speed sensor 23.

In this example, the filter processing means 1 obtains the estimated azimuth 5 by using the sensor signals from the plurality of azimuth sensors 11 and 12 which measure the azimuth and the rotation angle of the moving body. In synchronization with the output from the filter processing means 1, the traveling position of the moving body is obtained together with the speed data obtained from the speed sensor 15.

Here, one or more of the direction data, the velocity data, and the estimated value 7 of the filter output is input,
The sensor error evaluation means 3 evaluates the measurement error of the azimuth sensors 11 and 12. Furthermore, the accuracy of the finally output estimated azimuth 5 is improved by calculating the filter gain calculation means 2 based on the output.

The characteristic portion of another example of the navigation device to which the present invention is applied is shown in FIG. Here, as shown in FIG. 1, not only the direction sensor but also general sensors 13 and 14 are used as the sensor to measure the physical quantity representing the motion of the moving body.

The overall structure of this example is the same as the example of FIG. However, in this example, the filter processing means 1 outputs the estimated position 6 of the moving body by using the measured physical quantity data and the speed data of the speed sensor 15 as inputs.

In this example, one or more of the data from each of the sensors 13 and 14, the speed data and the estimated value 7 are input, and the error of each of the sensors 13 and 14 is calculated as follows.
The sensor error evaluation means 3 evaluates. Based on that output
The accuracy of the estimated position 6 to be finally output is improved by calculating the filter gain of the filter processing means 1 by the filter gain calculation means 2.

FIG. 3 shows a characteristic part in another example of the navigation device to which the present invention is applied. This means
Any one or more of general sensors 13 and 14, a filter processing unit 1 that obtains an estimated value 7 such as an azimuth and a position using output signals thereof, and data and estimated value 7 from each sensor. Is input to the sensor error evaluation means 3 for evaluating each sensor error, and the window processing means 4 for excluding an abnormal value of the sensor signal based on the output thereof. With this configuration, the accuracy of the finally output estimated value 7 is improved.

According to the present invention, due to the above features, the error in the measurement signal itself by the sensor can be reduced. Therefore, unlike the final stage correction such as the error correction of the map matching method in the vehicle navigation device, for example, a similar error does not occur immediately after the correction. Therefore, the present invention is equivalent to using a highly accurate sensor and can improve system reliability.

Examples of the sensors 13 and 14 in the respective means shown in FIGS. 2 and 3 include the direction sensor and the speed sensor described above, and the position detection sensor and the range detection sensor in addition to them.

The position detecting sensor calculates the current position of the moving body from the signal transmitted from a transmitting station such as a GPS satellite or a ground station. Specifically, GPS,
GLONASS, beacons, etc., aircraft, spacecraft,
It is used in navigation devices such as automobiles.

The range detection sensor measures the distance (range) between the transmitting station and the receiver mounted on the moving body, or its rate of change (range rate). If the position of the transmitter is known, the position is detected. It is something to send. Currently available are GPS, GLONASS, LORAN-C,
There are beacons, etc.

GPS is used as a navigation device for aircrafts, spacecrafts, automobiles and the like. GLONAS
S is managed and operated by the former Soviet Union and is a satellite ranging system similar to GPS. The LORAN-C is often installed in ships and the like. Beacons are so-called radio lighthouses installed on the ground. This is to transmit the beacon position locally, and by receiving the signal, it can be known that the mobile body is located near the beacon.
Mainly used for position detection of automobiles and aircraft.

[Embodiment 1] This embodiment has a configuration shown in FIG. This combines the basic features of the invention shown in FIGS.
Is a navigation device that realizes Kalman filter processing means.

The hardware configuration of this embodiment is the configuration of FIG. 18 described above. Here, the geomagnetic sensor 21
The turning angular velocity sensor 22 is used as a direction sensor, and the vehicle speed sensor 23 is used as a speed sensor.

In the filter processing means 1 (see FIG. 4) using the Kalman filter processing in the controller 20, the time change of the estimated quantity (state quantity), the state quantity, and the quantity measurable as a sensor output (observed quantity). The relationship with and needs to be modeled.

In the following description, in the Kalman filter processing in this embodiment, first, the driving direction and the error factors of the direction sensor used, which are important in car navigation, are used as the state quantities to be estimated, and the change over time is modeled. To do. Next, the relationship between the observed quantity and the state quantity is represented by a model.

The main error factors of the geomagnetic sensor 21 are the magnetization and the μ effect of the vehicle body (hereinafter referred to as the vehicle body effect). Magnetization is a phenomenon in which the vehicle body to which the geomagnetic sensor 21 is attached is magnetized to give a large azimuth error. This corresponds to the movement of the center of the azimuth circle drawn by the output of the geomagnetic sensor 21. The vehicle body effect is an effect that a vehicle body, which is a magnetic body, exerts on the earth's magnetic field, and is a phenomenon in which an azimuth circle is transformed into an ellipse.

The magnetizing component and the vehicle body effect can each be described by two parameters, and can be modeled as follows assuming that the parameters do not change with time.

[0062]

[Equation 1]

[0063]

[Equation 2]

[0064]

[Equation 3]

[0065]

[Equation 4]

This model represents the behavior of error factors (magnetization, vehicle body effect) of the geomagnetic sensor 21, and will be referred to as a geomagnetic sensor error model hereinafter.

One of the error factors of the turning angular velocity sensor 22 is the angular velocity bias. Bias is a short time
Can be assumed to be almost constant,

[0068]

[Equation 5]

Was modeled as This model describes the error factors of the turning angular velocity sensor 22, and
It is called a gyro error model.

Two Kalman filter models (state equation and observation equation) will be described. The state quantity x of the orientation estimation Kalman filter is

[0071]

[Equation 6]

It is assumed that

The equation of state is

[0074]

[Equation 7]

It was set as The system noise v (k) is a modeling error according to the state quantity equation 7.

The observation equation is

[0077]

[Equation 8]

[0078]

[Equation 9]

[0079]

[Equation 10]

Next to this, when summarized,

[0081]

[Equation 11]

It becomes

Here, the observed amount y is

[0084]

[Equation 12]

And the measurement noise w is

[0086]

[Equation 13]

It is

Here, when the effect of the vehicle body effect is small as an error factor of the geomagnetic sensor 21, Rx representing the vehicle body effect,
A simple method of expressing Ry by one state quantity (estimated quantity) R may be used.

A Kalman filter is calculated for the above state equation and the observation equation to estimate the traveling direction of the automobile. Since the obtained observation equation is non-linear, the algorithm of the extended Kalman filter is applied. The extended Kalman filter has the following recurrence formula,

[0090]

[Equation 14]

[0091]

[Equation 15]

[0092]

[Equation 16]

[0093]

[Equation 17]

[0094]

[Equation 18]

[0095]

[Formula 19]

Is given by

In the Kalman filter, the optimum estimated value x (k | k) of the state quantity x (k), the predicted value x (k | k−1),
The estimation error covariance matrix P (k | k), P corresponding to these
Calculate (k | k-1). V is a covariance matrix of system noise v, and W is a covariance matrix of observation noise w. H is the Jacobian at the predicted value x (k | k−1) of the function h. The iterative calculation of the Kalman filter has a form of recurrence formula and can be realized by software of a computer.

The azimuth is estimated by using the above-described extended Kalman filter algorithm. This processing procedure will be described with reference to the block diagram of FIG. 4 and the flowchart of FIG. The position determination according to this embodiment is performed according to the flow of FIG. 15 described above.

First, initial processing (step 20 in FIG. 15) is performed.
In 0), the prediction value and the initial value of the error covariance matrix are set as shown in Expression 14. After that, a traveling azimuth calculation interrupt process for calculating the traveling azimuth at regular intervals (step 20 in FIG.
8) Enter.

This step 208 uses the functional means shown in FIG. 4 to determine the azimuth according to the flow shown in FIG.

First, sensor data (observation amount) is read (step 300). Next, the predicted value of the state quantity is substituted into the observation equation, the predicted value of the observed quantity is calculated by the observed quantity predicted value calculation means 31 (step 302), and the observed quantity is predicted from the difference between the predicted value and the observed quantity. The error is the observed amount prediction error calculation means 3
2 (step 304).

After that, the sensor error evaluation means 3 described below is used by using the observed amount prediction error, the predicted value, the vehicle speed data and the like.
In step 306, the sensor error is evaluated. here,
The observed amount itself may be used instead of the observed amount prediction error, and the estimated value may be used instead of the predicted value.

As an example of the sensor error evaluation means 3, FIG.
An example in which the fuzzy inference processing means 50 is used as the error evaluation means 3 of the geomagnetic sensor 21 as shown in FIG.

The fuzzy inference processing means 50 receives the observation amount prediction error Imag51 of the geomagnetic sensor 21 which is the output of the observation amount prediction error calculating means 32 and the vehicle speed sensor 2 as inputs.
The output data Vel52 of No. 3 is used. As the output to the filter gain calculation means 34, the covariance Wmag 53 of the observation noise of the geomagnetic sensor 21 is taken. In this case, the input value may be a statistic amount of each data itself in a certain period, for example, an average value.

The qualitative relationship between the magnitudes of the input values 51 and 52 and the magnitude of the output 53 corresponding to the magnitudes thereof is represented by using fuzzy theory.

It is expected that the larger the input Imag 51 is, the larger the noise of the geomagnetic sensor 21 is. Therefore, the output Wmag 51 is increased.
53 needs to be relatively large. On the contrary, vehicle speed V
Geomagnetic sensor 2 that converges eventually as el52 increases
It is necessary to increase the speed of convergence to the direction indicated by 1. Therefore, the output Wmag53 needs to be relatively small.

This qualitative relationship is expressed as a fuzzy rule using five levels of fuzzy labels as shown in FIG. As the fuzzy inference method, for example, when the max-mini composite centroid method is used, each input value 51,
For 52, the output value 53 can be obtained as the center of gravity position of the hatched area as shown in the lower part of FIG. 7. However, in order to save the calculation amount, the above calculation is performed in advance,
The fuzzy inference may be realized by a table lookup method in which the output value corresponding to the input value is read from the memory by writing it in the memory of the microcomputer.

Further, in the example of the fuzzy processing means 50, the fuzzy variables are used for both the antecedent part and the consequent part of the fuzzy rule. However, in order to reduce the calculation load such as the calculation of the center of gravity, fuzzy inference may be realized by the inference method of Sugano et al., Which applies a linear form without using fuzzy variables only in the consequent part.

Further, in the sensor error processing means 3,
It is also possible to realize the sensor error evaluation by simply expressing the above qualitative relationship quantitatively by a certain functional expression without using the fuzzy theory. Examples of using other means for the sensor error evaluation means 3 will be described in Examples 2 to 7 below.

Next, using the output from the sensor error evaluation means 3 and the predicted value, the Kalman filter gain is obtained by the filter gain calculation means 2 (step 322, equation 15). Using the obtained gain, the estimated value of the state quantity and the error covariance is calculated by the estimated value calculation means 35 (step 324, equations 16 and 17). The traveling direction to be obtained is calculated and output as one of the estimated values (estimated direction 5).

The output from the sensor error evaluation means 3 is also used as a criterion for the window processing means 4.
For example, it is possible to remove noise in the geomagnetic sensor 21 above a certain level.

The calculated estimated value and the turning angular velocity sensor 2
Based on the output data from 2, the predicted value (state amount, error covariance) after the next fixed time is calculated by the predicted value calculation means 36 (step 326, Formula 18, Formula 19), and the interrupt process is ended. To do.

Further, in the present embodiment, as shown in FIG. 5, the output of the turning angular velocity sensor 22 is not used as the observed amount,
The predicted value calculation means 36 may be configured to use only as the azimuth change amount when calculating the predicted value after a fixed time.

By the processing procedure as described above, the error of the geomagnetic sensor 21 can be properly evaluated. further,
Since Kalman filtering can be reasonably adapted to the evaluation result, highly accurate direction estimation can be performed.

[Embodiment 2] This embodiment has the same hardware configuration as that of Embodiment 1 (see FIG. 18) and includes a turning angular velocity sensor 22 as a target sensor in the sensor error evaluation means 3. The configuration will be described.

This embodiment has the structure of the functional means shown in FIG. FIG. 8 shows the part related to the input / output of the sensor error evaluation means 3 in this embodiment. Here, fuzzy inference is applied to the error evaluation as in the first embodiment.

The input of the sensor error processing means 3 is the output of the observed amount prediction error calculating means 32, which is the turning angular velocity sensor 2.
The observation amount prediction error Igyro54 of 2 and the output data 52 of the vehicle speed sensor 23 are taken, and the covariance Wgyro5 of the observation noise of the geomagnetic sensor is output as the output to the filter gain calculation means 34.
Take 5 In this case, the input values 52 and 54 may be statistics of each data itself in a certain period, for example, an average value.

A qualitative relationship between the magnitudes of the input values 54 and 52 and the magnitude of the output 55 corresponding to the magnitudes of the input values 54 and 52 is expressed by using fuzzy theory. It is expected that the larger the input Igyro 54 is, the larger the noise of the turning angular velocity sensor 22 is. Therefore, the output Wgyro 55 needs to be relatively large.
On the contrary, as the vehicle speed data 52 increases, the convergence speed to the direction indicated by the geomagnetic sensor 22 that converges finally needs to be increased, and thus the output Wgyro 55 needs to be relatively large.

This qualitative relationship is expressed as a fuzzy rule (output 5 for vehicle speed data 52 as in FIG. 7).
The relationship of 5 is the reverse of that of FIG. 7), and if it is quantified using the membership function, the filter gain adapted to the actual turning angular velocity sensor error can be selected.
Therefore, the accuracy of the estimated azimuth accuracy can be improved.

[Embodiment 3] This embodiment has the same hardware configuration (see FIG. 18) and functional means configuration as that of the first embodiment (see FIG. 4). However, the sensor error evaluation means 3
In FIG. 9, fuzzy inference 50 is used for error evaluation of the geomagnetic sensor 21, and means for calculating geomagnetic azimuth radius error is used as shown in FIG.

The input / output of the sensor error evaluation means 3 of this embodiment has a structure as shown in FIG. here,
As the inputs, the observation amounts Ex and Ey61 of the geomagnetic sensor that are input to the observation amount prediction error calculating unit 32 are output as they are, and the magnetization components Mx and My of the outputs of the prediction value calculating unit 36. , Vehicle body effects Rx and Ry 62, and output data 52 of the vehicle speed sensor 23 are used. Further, as an output of the sensor error evaluation unit 3 to the filter gain calculation unit 34, the covariance Wmag5 of the observation noise of the geomagnetic sensor 21 is obtained.
Take 3. In this case, the input values 61, 62, and 52 may be statistics of each data itself for a certain period, for example, an average value.

In this embodiment, by using the input observed amount 61 and predicted value 62,

[0123]

[Equation 20]

[0124]

[Equation 21]

Thus, the geomagnetic azimuth circle radius error calculating means 6
A geomagnetic azimuth circle radius error 63 is calculated from 0. Here, the equation 20 calculates the error based on the geomagnetic azimuth circle radius estimated by the Kalman filter. Also, the number 21
Is based on, for example, the geomagnetic horizontal component (30 microtesla) in Japan.

The error 63 thus calculated and the vehicle speed data 52 constitute the fuzzy inference as shown in the first embodiment.

As in the first embodiment, the larger the geomagnetic azimuth circle radius error 63 thus obtained is, the larger the noise of the geomagnetic sensor 21 is expected to be. Therefore, the output Wmag53 needs to be relatively large. On the contrary, as the vehicle speed 52 increases, the convergence speed in the direction indicated by the geomagnetic sensor 21 that finally converges needs to be increased. Therefore, the output Wmag53
Must be relatively small. By expressing this qualitative relationship as a fuzzy rule as in the case of FIG. 7 and quantifying it using a membership function, it is possible to realize a highly accurate estimated azimuth accuracy.

Further, a fuzzy theory which inputs the geomagnetic sensor observation amount prediction error Imag51 and the geomagnetic azimuth circle radius error 63 of FIG. 6 can be constructed.

[Embodiment 4] In this embodiment, as the sensor error evaluation means 3, the same hardware configuration (see FIG. 18) and functional means configuration (see FIG. 4) for performing the magnetization correction of the geomagnetic sensor 21 are used. (See reference).

Sensor error evaluation means 3 in this embodiment.
FIG. 10 shows the input / output relationship of the. Here, the input to the magnetization correction means 70 as the sensor error evaluation means 3 is the geomagnetic sensor observation amount prediction error Imag51 of the output of the observation amount prediction error calculation means 32. Further, in the output to the filter gain calculation means 34, when the magnetization amount changes significantly, the diagonal terms Pmx, my corresponding to the magnetization amount of the estimation error covariance matrix included in the equations 14 to 19 are initially set. It is reset to the value and output.

Magnetization is a phenomenon in which the vehicle body to which the geomagnetic sensor is attached is magnetized and gives a large heading error. This corresponds to the movement of the center of the azimuth circle drawn by the output of the geomagnetic sensor 21. In such a case, a large error constantly occurs in the observed amount prediction error Imag51 that is input. Therefore, the observation amount prediction error Imag51 is detected to determine whether or not the magnetization correction is necessary. That is, the magnetization correction may be performed by detecting that the error of a certain level or more continues for a certain period.

In the Kalman filter processing of the first embodiment, a large change in the magnetization amount causes a large estimation error in the magnetization components Mx and My estimated as the state quantity. When it is determined that the magnetization correction is necessary, in order to restore the correct estimation of the magnetization amount, here, as shown in the output, the error covariance Pmx, my of the magnetization component is set to a large value (for example, an initial value). )
You can reset it to.

Even if a large change in the amount of magnetization occurs at a railroad crossing or the like, the above-described magnetization correction means 70 can instantly detect it. Further, since the estimation of the magnetization amount can be corrected properly by the above method, it is possible to maintain the highly accurate direction estimation.

Further, not only the diagonal terms Pmx and my corresponding to the magnetization amount of the estimation error covariance matrix, but also the diagonal terms corresponding to all the estimation amounts are small in size below a certain level. Or, it is desirable to reset to a value larger than the value at that time (for example, an initial value) after a certain period of time has elapsed.

[Embodiment 5] This embodiment is a navigation apparatus having a sensor error evaluation means 3 having an input / output relationship as shown in FIG. This is the same as Example 4 (FIG.
0) is another example for performing the magnetization correction as the sensor error evaluation means 3, and the other functional means configuration and hardware configuration are the same as in the first embodiment (see FIGS. 4 and 18).

The difference between the present embodiment and the fourth embodiment is that the change in the magnetization amount is detected. In the present embodiment, the geomagnetic azimuth circle radius error calculating means 60 used in FIG. 9 is applied as a method for detecting a change in the amount of magnetization.

Due to the large change in the magnetization amount, the magnetization component M
x and My will have a large estimation error. Therefore, a large error will occur in the geomagnetic azimuth circle radius calculated using the magnetization component. Based on the radius error, as in the fourth embodiment, it is detected that the error of a certain level or more continues for a certain period, and the magnetization correction is performed. As a method of obtaining the geomagnetic azimuth circle radius error 63, either of the above equations 20 and 21 may be used.
In this embodiment, the same effect as that of the fourth embodiment can be expected.

[Embodiment 6] This embodiment corresponds to Embodiment 1 (see FIG.
The window processing means 4 is controlled as the sensor error evaluation means 3 (see). The hardware and functional means configuration of this embodiment are the same as those of the first embodiment. FIG. 12 shows the input / output relationship in the sensor error evaluation means 3 of this embodiment.

Here, in order to remove the burst noise of the geomagnetic sensor, the geomagnetic azimuth circle radius error calculating means 60 used in FIGS. 9 and 11 is applied, and data in which the error 63 becomes a certain level or more is obtained. , And is removed by the window processing means 4. As a method of obtaining the geomagnetic azimuth circle radius error 63, either of the above equations 20 and 21 may be used.

By the window processing means 4 in this embodiment, it is possible to instantaneously judge and remove the burst noise of the geomagnetic sensor 21 or the like, so that it is possible to improve the accuracy of the estimated bearing.

[Embodiment 7] Each of the sensor error evaluation means 3 of Embodiments 1 to 6 described above can be configured independently, or may be configured in combination. Rather, it can be adapted to various sensor errors, so that further improvement in azimuth accuracy can be expected. In the present embodiment, an example in which a combination of the constituent elements of the sensor evaluation means 3 described in the above embodiments is used as a sensor error evaluation means will be described.

The hardware and functional means configuration of this embodiment is the same as that of the first embodiment (see FIG. 4), and the input / output relationship in the sensor error evaluation means 3 is shown in FIG.

Here, the sensor error evaluation means 3 comprises the fuzzy inference processing means 50, the magnetization correction means 70, and the geomagnetic azimuth circle error calculation means 60 described above.

The geomagnetic sensor 2 which is the output of the observed amount prediction error calculation means 32 is input to the fuzzy inference processing means 50.
The observed amount prediction error Imag51 of 1, the observed amount prediction error Igyro54 of the turning angular velocity sensor 22, and the output data Vel52 of the vehicle speed sensor 23 are used. The output thereof is the covariance Wgyro55 of the observation noise of the turning angular velocity sensor and the covariance Wmag53 of the observation noise of the geomagnetic sensor 21. In this case, the input value may be a statistic amount of each data itself in a certain period, for example, an average value.

Qualitative relationship between the magnitudes of the output values 53 corresponding to the respective magnitudes of the input values 51 and 52 and the qualitative relations of the magnitude of the output value 55 corresponding to the respective magnitudes of the input values 52 and 54. The relationship is expressed by using fuzzy theory, as in Examples 1 and 2, respectively.

The geomagnetic sensor observation amount prediction error Imag51 of the output of the observation amount prediction error calculation unit 32 is taken as an input to the magnetization correction unit 70. With respect to the output to the filter gain calculation means 34, when the magnetization amount changes significantly, the diagonal terms Pmx, my corresponding to the magnetization amount of the estimation error covariance matrix included in the equations 14 to 19 are set to initial values. It is reset and output. As a result, similarly to the fourth embodiment, even if a large change in the amount of magnetization occurs at a railroad crossing or the like, it can be instantly detected. Further, the estimation of the magnetization amount can be corrected properly by the above method.

The input of the geomagnetic azimuth circle radius error calculating means 60 is the same as the output of the observed quantities Ex and Ey61 of the geomagnetic sensor input to the observed quantity prediction error calculating means 32, and the predicted value calculating means 36. Magnetization components Mx, My, and vehicle body effects Rx, Ry62 of the outputs of the above are used. A geomagnetic azimuth circle radius error 63 is used for the output, and data in which the error 63 becomes a certain level or higher is removed by the window processing means 4. As in the sixth embodiment, since the geomagnetic azimuth circle radius error calculation method 60 and the window processing means 4 are used, it is possible to instantaneously determine and remove the burst noise of the geomagnetic sensor 21 or the like.

The processing procedure of the sensor error evaluation means 3 in this embodiment will be described with reference to the flowchart of FIG.

First, step 3 of the Kalman filtering process.
Example 4 (Fig. 1) using the observed amount prediction error obtained in
The magnetization correction process shown in 0) is performed. That is, in step 306, the above-mentioned magnetization correction is determined. Here, only in the case of Yes, in step 308, the error covariance Pmx, my of the magnetization amount is reset to the initial value. After that, in step 310, the geomagnetic azimuth circle radius error 63 is calculated by the equation 20 or the equation 21 (when the magnetization correction means of the fifth embodiment is executed, step 310 is executed first and then step 306 is executed). You can move it).

Next, the calculated geomagnetic azimuth circle radius error 63
Example 6 (FIG. 12) in step 312 using
The window processing shown in is performed. That is, as described above, when the magnitude of the geomagnetic azimuth circle radius error 63 exceeds a certain level, the filter gain calculation (step 3
22) and the estimated value calculation (step 324) are omitted, and the process jumps to step 326.

If the above conditions are not met, step 31
4, the covariance of observation noise is controlled by fuzzy inference as shown in Examples 1 to 3 (FIGS. 6, 8 and 9). When the input to the above-mentioned fuzzy inference processing means 50 is a statistic, only when the input values are complete, the process proceeds to the calculation of the membership value (step 316), the center of gravity value is calculated in step 318, and the center of gravity value is calculated. In step 320, the observed noise covariance value is set.

In the above processing procedure, when the error covariance of the magnetization amount (step 308) or the covariance of the observation noise (step 320) is changed, the step based on the changed value is used. At 322, the Kalman filter gain is calculated. Here, since the influence of the sensor error is added, the accuracy of the orientation estimation and other estimations can be improved.

[Embodiment 8] FIG. 13 shows the configuration of the filter processing means 1, the sensor error evaluation means 3, the window processing means 4 and the sensors 21, 22, 23 and 24 in this embodiment.

In this embodiment, all the sensor data including the vehicle speed sensor data shown in FIG. 2 is used to perform the above-mentioned Kalman filter processing to obtain the estimated position to be obtained. Here, a GPS 24 is newly added as a sensor. The GPS 24 can also be used as an absolute position detection sensor, an absolute azimuth detection sensor, or a range detection sensor.

Therefore, the state equation and the observation equation can be created using the sensor output model, the sensor error model, and the vehicle motion model in the same format as shown in the first to seventh embodiments.

[Embodiment 9] In this embodiment, in the example disclosed in Japanese Patent Laid-Open No. 1-219610 described in the section of the conventional example, the magnetic field environment detecting means corresponding to the sensor error evaluating means 3 is simply provided with vehicle speed information. This is an example showing that the problem of the conventional example can be improved by adding only.

That is, in contrast to the conventional method in which the gain of the low-pass filter processing means 80 is calculated only from the difference in the azimuth change amount between the geomagnetic sensor 21 and the turning angular velocity sensor 22, the filter gain is relatively set when the vehicle speed is high. The processing may be added so as to increase the speed of the magnetic field in order to accelerate the convergence to the geomagnetic direction.

[0158]

According to the navigation device of the present invention,
Kalman filtering that models the error characteristics of the sensor is used to calculate the estimated amount, and the sensor error is evaluated using the sensor error parameter obtained during the filtering, so the error can be calculated more quickly and more accurately. It becomes possible to capture. Further, since the Kalman filter gain is changed using the evaluation result, an appropriate gain can be calculated in real time, and more accurate position information and the like can be obtained.

Further, according to the present invention, the gain of the filter for calculating the estimated amount is determined in consideration of the vehicle speed data as well, so that the traveling position must be obtained at any time as the traveling distance increased by integrating the vehicle speed. , Which is peculiar to the navigation device.

[0160]

[Brief description of drawings]

FIG. 1 is a configuration diagram illustrating basic features of a navigation device of the present invention.

FIG. 2 is a configuration diagram illustrating basic features of the navigation device of the present invention.

FIG. 3 is a configuration diagram illustrating basic features of the navigation device of the present invention.

FIG. 4 is a configuration diagram showing an embodiment of the present invention in which a Kalman filter process is applied to the detection of the traveling azimuth according to a configuration in which FIGS. 1 and 3 are combined.

5 is a configuration diagram showing an embodiment in which the configuration of FIG. 4 is partially modified.

6 is a block diagram showing an embodiment of the sensor error evaluation means of FIG.

7 is a diagram for explaining the principle of fuzzy inference applied to the sensor error evaluation means of FIG.

8 is a configuration diagram showing an embodiment of the sensor error evaluation means of FIG.

9 is a configuration diagram showing an embodiment of the sensor error evaluation means in FIG.

10 is a configuration diagram showing an embodiment of the sensor error evaluation means in FIG.

11 is a configuration diagram showing an embodiment of the sensor error evaluation means in FIG.

12 is a configuration diagram showing an embodiment of the sensor error evaluation means of FIG.

13 is a configuration diagram showing an embodiment of the present invention in which a Kalman filter process is applied to the traveling position detection according to the configuration of FIG.

FIG. 14 is a configuration diagram showing an embodiment in which the effect of vehicle speed data of the present invention is added to the conventional low-pass filter system.

FIG. 15 is a diagram illustrating a general flow of a vehicle navigation device to which the present invention has been applied.

16 is a flowchart of a traveling azimuth calculation interrupt process in FIG.

FIG. 17 is a flowchart of a traveling vector product division inclusion process in FIG.

FIG. 18 is a block diagram showing a hardware configuration of a navigation device to which the present invention is applied.

FIG. 19 is a configuration diagram showing an embodiment of the sensor error evaluation means in FIG.

20 is a flowchart of a traveling azimuth calculation interrupt process in FIG.

[Explanation of symbols]

DESCRIPTION OF SYMBOLS 1 ... Filter processing means, 2 ... Filter gain calculation means, 3 ... Sensor error evaluation means, 4 ... Window processing means, 5 ... Estimated direction, 6 ... Estimated position, 7 ... Estimated value, 11, 12 ... Direction sensor, 13, 14 ... Sensor, 15 ... Speed sensor, 20 ... Controller, 2
1 ... Geomagnetic sensor, 22 ... Turning angular velocity sensor, 23
… Vehicle speed sensor, 24… GPS, 25… Map memory,
26 ... Display device, 27 ... Initial position input means, 31
... observed amount predicted value calculating means, 32 ... observed amount predicted error calculating means, 35 ... estimated value calculating means, 36 ... predicted value calculating means, 50 ... fuzzy inference processing means, 60 ... geomagnetic azimuth circle radius error calculating means, 70 ... Magnetization correction means, 80
... Low-pass filter processing means.

 ─────────────────────────────────────────────────── ─── Continuation of the front page (72) Masatoshi Hoshino 7-1-1, Omika-cho, Hitachi-shi, Ibaraki Hitachi Ltd. Hitachi Research Laboratory (72) Inventor Koji Kuroda 7-chome, Omika-cho, Hitachi-shi, Ibaraki No. 1 Incorporated company Hitachi Ltd. Hitachi Research Laboratory (72) Inventor Yoshimasa Nagashima 2-4991 Hironodai, Zama City, Kanagawa Prefecture In-house company Xanavi Informatics (72) Inventor Kenji Takano 2 Hironodai, Zama City, Kanagawa Prefecture Chome 4991 Address Incorporated Company Xanavie Informatics (72) Inventor Mikihiko Taisei 2-4991 Hironodai, Zama City, Kanagawa Prefecture Incorporated Company Xanavie Informatics

Claims (18)

[Claims] 1. A speed sensor for detecting the speed of a moving body, one or more sensors for detecting a physical quantity other than the speed, which indicates a motion of the moving body, and outputs from a sensor group including the sensor and the speed sensor. Then, in a navigation device for obtaining the traveling position of a moving body, a filter processing means for performing filter processing having a predetermined filter characteristic on the output from the sensor group and calculating an estimated value using the processing result. A sensor that evaluates an output error from one or more sensors by using one or more of outputs from one or more sensors other than the speed sensor and an estimated value output from the filter processing means. A filter gain calculator that adjusts the filter characteristics of the filter processing unit based on the output of the error evaluation unit and the sensor error evaluation unit and calculates the filter gain. Navigation apparatus characterized by having and. 2. The navigation device according to claim 1, wherein the sensor is a plurality of sensors that measure the orientation of a moving body by different methods. 3. The geomagnetic sensor for detecting the earth's magnetic field, the turning angular velocity sensor for detecting the turning angular velocity of a moving body, and the absolute position or absolute azimuth on the earth using satellite signals as claimed in claim 1. Navigation device, characterized in that it includes at least one of the GPS receiving devices, and the filter processing means calculates at least the position of the moving body as one of the estimated values. 4. The filter processing means according to claim 2, wherein the sensor includes at least one of a geomagnetic sensor that detects a geomagnetic field and a turning angular velocity sensor that detects a turning angular velocity of a moving body. Is a navigation device, wherein at least the traveling direction of the moving body is calculated as one of the estimated values. 5. The sensor error evaluation means according to any one of claims 1 to 4, further using an output from the speed sensor, an estimated value output from the filter processing means being a predetermined value. A navigation device having a convergence speed adjusting function for adjusting the speed of convergence to the navigation device. 6. The navigation device according to claim 1, wherein the filter processing means includes Kalman filter processing using at least an error factor of the sensor as a state quantity to be estimated. 7. The estimated value output from the filter processing means, which is input to the sensor error evaluation means, is an estimated value in the Kalman filter processing, and a next time step calculated from the estimated value. And a prediction error of the observed amount calculated from the predicted value and the sensor output, a navigation device. 8. The output of the sensor error evaluation means according to claim 6, which is at least one of a covariance value of an observation error of the sensor and an error covariance value of an estimated value in Kalman filtering. A navigation device characterized by the above. 9. The sensor error evaluation means according to claim 6, wherein the input is a prediction error of the observed amount and the output of the speed sensor, and the output is a covariance value of the observation error of the sensor, A navigation device further having a function of adjusting an output value according to each magnitude of an input value. 10. The Kalman filter according to claim 1, wherein one of the sensors is a geomagnetic sensor, and the filter processing unit uses at least a factor of error of the geomagnetic sensor as a state quantity to be estimated. The processing, the sensor error evaluation means, in order to correct the change in the magnetization amount of the geomagnetic sensor, using the prediction error of the observed amount of the geomagnetic sensor in the Kalman filter processing as its input, the error covariance value of the magnetization amount as an output The navigation device further comprising a function of setting the output to a predetermined value when the input value changes rapidly using the. 11. The variation of magnetization of the geomagnetic sensor according to claim 10, further comprising using a geomagnetic azimuth radius value calculated from a predicted value and an observed amount in Kalman filtering. A navigation device having a function of detecting a change in the amount of magnetization. 12. A navigation device for determining the position of a moving body using a sensor group including a plurality of sensors for detecting a physical quantity representing the motion of the moving body, and an output from the sensor group. Filtering means for applying a filtering process having a predetermined characteristic to the output and using the processing result to calculate an estimated value of a physical quantity related to the motion of at least one moving body, and a plurality of sensor groups from the sensor group. Of the estimated values output and output from the filter processing means, one or more of the estimated values are input to the sensor error evaluation means for evaluating the error of one or more sensors, and the output of the sensor error evaluation means is used. A window processing means for determining an characteristic and eliminating an abnormal value in the output from one or more sensors according to the characteristic. Navigation device. 13. The sensor according to claim 12, wherein the sensor is a geomagnetic sensor for detecting a magnetic field of the earth, a turning angular velocity sensor for detecting a turning angular velocity of a moving body, and a GPS for detecting an absolute position or an absolute azimuth on the earth using a satellite signal. A navigation device comprising at least one of a receiving device and a speed sensor for detecting the speed of a moving body. 14. The navigation apparatus according to claim 12, wherein the filter processing means is Kalman filter processing using at least an error factor of the sensor as a state quantity to be estimated. 15. The navigation system according to claim 12, wherein one of the sensors is a geomagnetic sensor that detects a geomagnetic field, and the window processing unit is a unit that excludes an abnormal value of the geomagnetic sensor. apparatus. 16. The Kalman filter process according to claim 15, wherein the filter processing unit uses at least an error factor of the sensor as a state quantity to be estimated, and the sensor error evaluation unit receives a prediction in the Kalman filter process as an input. Using the value and the observed amount, a geomagnetic azimuth circle radius error is calculated as an output, and the window processing means uses the output of the sensor error evaluation means to exclude an abnormal value of the geomagnetic sensor. apparatus. 17. The navigation device according to claim 1, wherein the sensor error evaluation means further includes a fuzzy inference processing function in consideration of the ambiguity between the input value and the output value. 18. The sensor error evaluation means according to claim 6 or 14, wherein when the magnitude of the diagonal term of the estimation error covariance matrix is equal to or less than a specific value, the diagonal term is increased to the specific value or more. A navigation device characterized in that it is increased or all diagonal terms are set to have a larger value than a previous value at the time of system startup and at each elapse of a certain specific time.