JP4657050B2 - Speed vector determination system - Google Patents

Description

  The present invention relates to a speed vector determination system that is mounted on a moving body such as a vehicle and obtains the speed vector of the moving body with high accuracy by using only satellite signals such as GPS.

  In a mobile object such as a car, it is desired to use a satellite signal such as GPS to obtain the moving speed and direction, that is, the velocity vector with high accuracy along with its position.

  In order to obtain the velocity vector with high accuracy, it is desirable to use the Doppler frequency effectively, and one of the satellite signals used for the calculation should use a satellite signal from a satellite with a relatively low elevation angle. Is good.

  However, on roads in urban buildings, satellite signals from high-elevation satellites can be obtained directly, but satellite signals from low-elevation satellites can be obtained after being blocked by buildings and reflected. Will increase. That is, the received satellite signal often includes a satellite signal affected by multipath.

  Since the velocity vector VF obtained including the satellite signal affected by the multipath includes an error component, the velocity vector cannot be obtained with high accuracy.

In order to reduce the influence of such multipath, it is possible to obtain an estimated speed vector based on the direction obtained from the direction sensor and the speed obtained from the vehicle speed sensor using the direction sensor and the vehicle speed sensor. 1. In Patent Document 1, a positioning position and a velocity vector are obtained by using the azimuth sensor and the vehicle speed sensor.
JP 2001-264409 A

  As in the conventional method disclosed in Patent Document 1, a navigation apparatus including a speed vector determination system is generally based on an estimated speed vector method using a geomagnetic sensor or an angular velocity sensor as an azimuth sensor in order to be inexpensively configured. Is.

  However, when a geomagnetic sensor is used, a large error occurs in a place where there are many magnetic materials such as an iron bridge.

  In addition, when an angular velocity sensor is used, only a relative direction can be obtained. Therefore, it is important to obtain a correct absolute direction from other means (for example, a satellite speed calculation device). It is a problem that a correct velocity vector is often not obtained as shown in FIG.

  Therefore, in the case of using the estimated speed vector method, it is necessary to use an expensive gyroscope with good azimuth accuracy. In this case, there is a problem that the navigation device becomes expensive.

  The present invention is mounted on a moving body such as a vehicle, uses only satellite signals such as GPS, does not require other dead reckoning sensors that increase costs, and is highly susceptible to multipath such as high-rise buildings. However, it is an object of the present invention to provide a speed vector determination system that can determine an optimal speed vector of a moving object.

The speed vector determination system according to claim 1 is a speed vector determination system which is mounted on a mobile body and obtains the speed vector of the mobile body using satellite signals from a plurality of satellites,
A satellite signal tracking device that tracks each satellite signal and sends tracking information;
Of the plurality of satellites tracked by the satellite signal tracking device, using satellite elevation angle information based on satellite orbit information, at least one satellite is commonly included and other satellites are included. A satellite combination determination device for obtaining a plurality of combinations of satellite signals;
A velocity vector solution candidate calculation device for calculating a velocity vector solution for each of a plurality of combinations determined by the satellite combination determination device to obtain a velocity vector solution candidate;
It has an optimum solution selection device for obtaining an optimum velocity vector based on the degree of coincidence between the velocity vector solution candidates with respect to the velocity vector solution candidates obtained by the velocity vector solution candidate calculation device.

The speed vector determination system according to claim 2 is a speed vector determination system which is mounted on a mobile body and obtains the speed vector of the mobile body using satellite signals from a plurality of satellites,
A satellite signal tracking device that tracks each satellite signal and sends tracking information;
Of the plurality of satellites tracked by the satellite signal tracking device, using satellite elevation angle information based on satellite orbit information, at least one satellite is commonly included and other satellites are included. A satellite combination determination device for obtaining a plurality of combinations of satellite signals;
A velocity vector solution candidate calculation device for calculating a velocity vector solution for each of a plurality of combinations determined by the satellite combination determination device to obtain a velocity vector solution candidate;
It has an optimum solution selection device for obtaining an optimum velocity vector based on the degree of coincidence with the previous velocity vector regarding the velocity vector solution candidate obtained by the velocity vector solution candidate computing device.

The speed vector determination system according to claim 3 is a speed vector determination system which is mounted on a mobile body and obtains the speed vector of the mobile body using satellite signals from a plurality of satellites.
A satellite signal tracking device that tracks each satellite signal and sends tracking information;
Of the plurality of satellites tracked by the satellite signal tracking device, using satellite elevation angle information based on satellite orbit information, at least one satellite is commonly included and other satellites are included. A satellite combination determination device for obtaining a plurality of combinations of satellite signals;
A velocity vector solution candidate calculation device for calculating a velocity vector solution for each of a plurality of combinations determined by the satellite combination determination device to obtain a velocity vector solution candidate;
With respect to the velocity vector solution candidates obtained by the velocity vector solution candidate calculation device, the minimum vector difference between the velocity vector solution candidates is compared with a predetermined threshold, and when the minimum vector difference is larger than the predetermined threshold, An optimum solution selection device that obtains an optimum velocity vector based on the degree of coincidence and obtains an optimum velocity vector based on the degree of coincidence between the respective velocity vector solution candidates when the minimum vector difference is not greater than the predetermined threshold. It is characterized by.

  The velocity vector determination system according to claim 4 is the velocity vector determination system according to any one of claims 1 to 3, wherein the satellite combination determination device determines a satellite elevation angle based on the satellite orbit information when obtaining the plurality of combinations. Signal level information based on the tracking information is also used together with the information.

  A speed vector determination system according to a fifth aspect is the speed vector determination system according to any one of the first to fourth aspects, wherein in the plurality of combinations in the satellite combination determination device, the commonly included satellites have a predetermined elevation angle. It is a satellite having a high elevation angle, and the other satellite is a satellite having a low elevation angle lower than the predetermined elevation angle.

  A speed vector determination system according to a sixth aspect is the speed vector determination system according to the fifth aspect, wherein the number of the high elevation satellites is two.

  The speed vector determination system according to claim 7 is the speed vector determination system according to any one of claims 1 to 6, wherein the optimum solution selection device uses the optimum speed vector and an approximate speed obtained using all satellites. When the optimum speed vector is different from the approximate speed vector by a predetermined threshold or more, a final solution is obtained based on the approximate speed vector.

  The speed vector determination system according to claim 8 is the speed vector determination system according to any one of claims 1 to 6, wherein the optimal solution selection device compares the direction of the optimal speed vector with the previous direction, When the direction of the optimum speed vector is different from the previous direction by a predetermined threshold or more, the speed is the speed of the optimum speed vector and the direction is close to the previous direction and close to the direction of the optimum speed vector by a predetermined angle. A final solution is obtained based on the speed vector obtained by addition or subtraction.

  According to the speed vector determination system of the present invention, only a satellite signal such as GPS is used, and no other dead reckoning navigation sensor that increases the cost is required. The optimum velocity vector of the moving object can be obtained.

  In addition, at least one satellite is included in common by using satellite elevation angle information based on satellite orbit information and signal level information based on tracking information from a plurality of satellites tracked by the satellite signal tracking device. Satellites are included, a plurality of combinations of three or more satellite signals are obtained, velocity vector solutions for each of the combinations are calculated to obtain velocity vector solution candidates, and the degree of coincidence between these velocity vector solution candidates Based on the above, the optimum velocity vector is obtained. As a result, it can be expected that an optimum velocity vector of the moving object is obtained even in an environment where the influence of multipath is large in a state in which the relative relationship between the moving object and the skyscraper is unknown.

  Further, since the optimum speed vector is obtained based on the degree of coincidence with the previous speed vector for the speed vector solution candidate obtained by the speed vector solution candidate calculation device, even if the speed vector solution candidates greatly differ from each other, the previous speed vector It can be expected that the optimum velocity vector of the moving object is obtained even in an environment where the influence of multipath is large.

  Further, regarding the speed vector solution candidate obtained by the speed vector solution candidate calculation device, the minimum vector difference between the respective speed vector solution candidates is compared with a predetermined threshold, and when the minimum vector difference is larger than the predetermined threshold, An optimum speed vector is obtained based on the degree of coincidence, and an optimum speed vector is obtained based on the degree of coincidence between the respective velocity vector solution candidates when the minimum vector difference is not greater than the predetermined threshold. Thereby, the optimum vector can be automatically selected and determined according to the magnitude of the minimum vector difference.

  In addition, since there are two high elevation satellites and the velocity vector is obtained in combination with other satellites, it can be expected that the optimum velocity vector of the moving object is obtained.

  FIG. 1 is a diagram showing a configuration of an embodiment of a velocity vector determination system using satellite signals according to the present invention.

  The antenna 1 receives satellite signals from a plurality of satellites such as GPS and supplies them to the satellite signal tracking device 11.

  The satellite signal tracking device 11 tracks the received satellite signal. For satellites that have been successfully tracked, tracking information such as satellite tracking status, satellite signal level, pseudorange data used for position measurement, and carrier phase data used for speed measurement is output.

  The positioning position calculation device 12 calculates the positioning position based on the pseudo distance data from the satellite signal tracking device 11 and the satellite orbit information supplied from the satellite orbit calculation device included in the control device 100, and the obtained positioning is performed. The position Pout is output to the control device 100. Also, an approximate velocity vector VF is obtained and output to the optimum solution selection device 15.

  The satellite combination determination device 13 obtains a plurality of combinations of three or more satellite signals from the plurality of satellites tracked by the satellite signal tracking device 11. In a moving body such as a car moving on a plane, a combination of three satellite signals may be used, and a combination of four or more satellites may be used as necessary.

  The plurality of combinations may include at least one, usually two, satellites located at a high elevation angle in common, and may include satellites with different low elevation angles in each combination.

  Here, the high elevation angle preferably has an elevation angle in the range of 40 ° to 90 °, and in high-rise buildings and the like, the elevation angle in the range of 50 ° to 90 ° is desirable. As long as the elevation angle is within this range, there is almost no influence of multipath due to reflected waves. The other satellites may be satellites located at an elevation angle lower than the high elevation angle, that is, satellites having an elevation angle of 40 ° or 50 ° or less.

  FIG. 2 is a view of the satellite reception situation in the high-rise building district as seen from the rear of the vehicle. A car 21 is traveling on a road between the high-rise building 22 and the high-rise building 23. A satellite signal is transmitted as a direct wave A1 from the high elevation satellite 24 to the car 21. Further, from the low elevation satellite 25, the direct wave B1 is blocked by the high-rise building 22 and does not reach the vehicle 21, and the reflected wave B2 reflected by the high-rise building 23 is transmitted to the vehicle 21. Since the satellite signal from the high elevation satellite 24 is a direct wave, it is not affected by multipath. However, since only the high elevation satellites have a small horizontal component of the Doppler frequency for obtaining the velocity, it is difficult to obtain the velocity vector in the horizontal direction with high accuracy.

  FIG. 3 is a view of the satellite reception situation in the high-rise building area as seen from above the vehicle. A car 21 is traveling on a road between the high-rise building 22 and the high-rise building 23. A satellite signal is transmitted from the traveling direction satellite 26 to the car 21 as a direct wave C1. Further, from the cross direction satellite 27, the direct wave D <b> 1 is blocked by the high-rise building 22 and does not reach the vehicle 21, and the reflected wave D <b> 2 reflected by the high-rise building 23 is transmitted to the vehicle 21. In a high-rise building area, a direct wave C1 is also obtained from a satellite signal from a satellite located in the traveling direction of the road or in the opposite direction. When the satellite in the traveling direction is a low elevation satellite, the horizontal component of the Doppler frequency is large, so that an accurate horizontal velocity vector can be obtained. However, it is impossible to determine which satellite signal is a direct wave under a situation where the satellite signal receiver cannot use high-rise building map information or the like.

  In FIG. 2, one high elevation satellite 24 and one low elevation satellite 25 are shown, and in FIG. 3, one traveling direction satellite 26 and one cross direction satellite 27 are shown. It may be considered that there are always 5 or more satellites including the high elevation satellite 24, the low elevation satellite 25, the traveling direction satellite (including the opposite direction), and the cross direction satellite.

FIG. 4 is a diagram illustrating an example of a combination of a plurality of satellites for obtaining a velocity vector solution.
This combination example includes a combination of satellites that commonly include high elevation satellites (satellite A and satellite B indicated by black stars) and low elevation satellites (satellite C, satellite D, and satellite E indicated by white stars). Show. The first combination is a combination of satellite A, satellite B, and satellite C, the second combination is a combination of satellite A, satellite B, and satellite D, and the third combination is satellite A, satellite B, and Combination with satellite E. For this combination, satellite orbit information from the control device 100 is used.

  Further, using the satellite signal level, instead of simply selecting in order of elevation angle, a satellite having a high elevation angle and a high satellite signal level may be preferentially selected.

  Returning to FIG. 1, the velocity vector solution candidate calculation device 14 calculates velocity vector solutions for each of the plurality of combinations determined by the satellite combination determination device 13 to obtain a plurality of velocity vector solution candidates.

  A plurality of velocity vector solution candidates in the velocity vector solution candidate calculation device 14 are a velocity vector V1 obtained by the first combination of the satellite A, the satellite B, and the satellite C in FIG. The velocity vector V2 obtained by the second combination consisting of and the velocity vector V3 obtained by the third combination consisting of the satellite A, the satellite B, and the satellite E are candidate 1, candidate 2, and candidate 3. If more combinations are taken, more candidates are obtained.

  The optimum solution selection device 15 obtains an optimum velocity vector Vs based on a predetermined selection condition with respect to the velocity vector solution candidates “candidate 1, candidate 2 and candidate 3” obtained by the velocity vector solution candidate calculation device 14, and performs control. Output to the device 100.

  The first selection condition in the optimum solution selection device 15 is that the speed vector solution candidates “candidate 1, candidate 2 and candidate 3” obtained by the velocity vector solution candidate calculation device 14 are determined according to the degree of coincidence between the velocity vector solution candidates. Based on this, an optimum speed vector Vs is obtained.

  An example of this is shown in FIG. In FIG. 5, candidate 1 and candidate 2 show substantially the same velocity vector, and candidate 3 is greatly different from them. In this case, either candidate 1 or candidate 2 is selected as the optimum speed vector Vs. In this case, either may be used, but to be more specific, among candidates 1 and 2, the one that matches the previous optimum speed vector is selected. However, in special cases, such as when the high elevation satellite is affected by multipaths due to some influence, the degree of coincidence may be high even with an abnormal solution. Therefore, it is better to determine that the solution is not abnormal.

  Note that the degree of coincidence between the velocity vector solution candidates is obtained by the absolute value of the difference in latitude direction component of the velocity vector of each candidate + the absolute value of the difference in longitude direction component. The smaller this sum, the higher the degree of coincidence.

  The second selection condition in the optimum solution selection device 15 is based on the degree of coincidence with the previous velocity vector regarding the velocity vector solution candidates “candidate 1, candidate 2 and candidate 3” obtained by the velocity vector solution candidate calculation device 14. The optimum speed vector Vs is obtained.

  An example of this is shown in FIG. In FIG. 6, when the number of satellites in the traveling direction is small, each candidate becomes a disjoint velocity vector, but since candidate 1 has the highest degree of coincidence with the previous optimum velocity vector, candidate 1 is selected as the optimum velocity vector Vs. To the control device 100.

  Assuming that there is no candidate 1 in FIG. 6, the candidate 2 having the next highest matching score is selected. In this case, since the difference from the previous orientation is large, the orientation is important in order to emphasize the continuity of the orientation. It is preferable to limit the change so as to limit the azimuth change within a predetermined angle change range.

  In addition, when the azimuth is actually being changed, it is preferable to check the difference from the approximate velocity vector obtained using the satellite signals of all satellites so that the azimuth is not accidentally fixed to the previous azimuth.

  The above operation will be described with reference to the flowchart of FIG.

  In step 101, an approximate velocity vector VF is obtained using satellite signals from all satellites. Some satellite signals from all satellites are reflected waves that have undergone multipath in a high-rise building or the like, but some are direct waves, so an approximate velocity vector VF can be obtained. The approximate velocity vector VF cannot be expected to be highly accurate, but can be used as a rough guide. Further, the satellite signals from all the satellites may be satellite signals that can be tracked by the satellite signal tracking device 11.

  Next, in step 102, all combinations of two high elevation satellites and other satellites are calculated, and velocity vector candidates V1, V2, V3,.

  The combination of the satellites is preferably a combination of three satellites, two high elevation satellites and one low elevation satellite. In order to combine these three satellites, we want to determine the horizontal speed of a moving object such as a car. The altitude speed approximates zero, and the three variables of latitude speed, longitude speed and internal oscillator frequency error. It can be expected that better accuracy can be obtained by including one low elevation satellite. If altitude velocity is also required, a combination of two satellites with a high elevation angle and two other satellites, or a combination of three satellites with a high elevation angle and another satellite, and the velocity vector may be solved.

  Next, in step 103, differences D1, D2, D3,... Dn between the velocity vector candidates are obtained, and the minimum value is set as the minimum vector difference Dmin.

  Next, in step 104, the minimum vector difference Dmin is compared with a predetermined threshold value. If the minimum vector difference Dmin is smaller than the predetermined threshold value, the process proceeds to step 105, and the candidate closest to the previous direction among the two speed vector candidates forming the minimum vector difference Dmin is set as the most likely candidate speed vector Vmost.

  On the other hand, if the minimum vector difference Dmin is not smaller than the predetermined threshold value in step 104, the process proceeds to step 106, and the candidate closest to the previous direction among the velocity vector candidates V1, V2, V3,. The most likely candidate velocity vector Vmost is used.

  Next, in step 107, the most probable candidate velocity vector Vmost obtained in step 105 or step 106 is compared with the approximate velocity vector VF obtained in step 101, and it is determined whether or not the difference is smaller than a predetermined threshold value. . When the difference between the most probable candidate velocity vector Vmost and the approximate velocity vector VF is larger than a predetermined threshold value, the routine proceeds to step 108 and the approximate velocity vector VF is made the final solution.

  On the other hand, if the difference between the most probable candidate velocity vector Vmost and the approximate velocity vector VF is smaller than the predetermined threshold value, the routine proceeds to step 109.

  In step 109, the azimuth difference between the azimuth of the most likely candidate velocity vector Vmost and the azimuth of the previous velocity vector is compared with a predetermined threshold (predetermined angle). If the difference in orientation is greater than a predetermined threshold (predetermined angle), the process proceeds to step 110.

  In step 110, the speed remains the speed of the most likely candidate speed vector Vmost, and a predetermined threshold (predetermined angle) is added to or subtracted from the direction of the previous most likely speed vector Vmost so as to approach the direction of the most likely candidate speed vector Vmost. The final direction is a velocity vector having the final direction and the speed of the most likely candidate velocity vector Vmost.

  On the other hand, if the heading difference is smaller than the predetermined threshold (predetermined angle), the routine proceeds to step 111, where the most probable candidate velocity vector Vmost is taken as the final solution.

  In step 112, the final direction and speed are determined according to the information from step 110 or step 111, the optimum speed vector Vs is obtained, and output to the control device 100.

  It should be noted that the approximate velocity vector VF may be changed to be given to step 109 without making the output from step 108 the final solution. This state is indicated by a broken line in FIG. In this modified example, the approximate speed vector VF is also added to the determination target in step 109 in addition to the most likely candidate speed vector Vmost.

  If the azimuth is obtained, the variable becomes two variables of speed and internal oscillator frequency error. Therefore, the speed vector can be corrected by obtaining the speed again with two satellites having a high elevation angle after the azimuth is determined.

For example, in the case of 3 satellites, if H is a direction cosine matrix,
[H] [Vn, Ve, ΔT] = [ρ1, ρ2, ρ3]
To obtain the latitude direction velocity component Vn and the longitude direction velocity component Ve, and obtain the azimuth Θ by tan ⁻¹ (Ve / Vn) or the like. Here, H is a 3 × 3 matrix of H11 to H33, ΔT is an internal oscillator frequency error, and ρ1, ρ2, and ρ3 are velocity error components of each satellite. After obtaining the azimuth Θ, for the two satellites with high elevation angle with few multipaths,
Since the accuracy of the speed is improved by obtaining the speed again by [H ′] [V, ΔT] = [ρ1, ρ2] etc., the final result may be closer to the true speed solution than by solving with three satellites. H ′ is a matrix with 2 rows and 2 columns composed of H11 ′ to H22 ′, and V = (Vn ² + Ve ² ) ^1/2 .

  In the above description, the satellite signal system alone is used. However, when a roughly correct estimated speed vector can be obtained from other than the satellite signal system, the speed vector candidates V1, V2 are used by using the difference from the estimated vector. , V3,..., Vn candidates may be limited before obtaining an optimal solution.

The figure which shows the structure of the velocity vector determination system which concerns on the Example of this invention. A view of satellite reception in a skyscraper from the back of the car A view of the satellite reception situation in a skyscraper from the top of the car The figure which shows the example of the combination of plural satellites in order to obtain the solution of velocity vector The figure explaining the selection conditions in the optimal solution selection apparatus Another figure explaining selection conditions in the optimal solution selection device The flowchart explaining operation | movement of the speed vector determination system of this invention.

Explanation of symbols

DESCRIPTION OF SYMBOLS 1 Antenna 11 Satellite signal tracking device 12 Positioning position calculating device 13 Satellite combination determining device 14 Speed vector solution candidate calculating device 15 Optimal solution selecting device 100 Control device

Claims (8)

A speed vector determination system which is mounted on a mobile body and obtains the speed vector of the mobile body using satellite signals from a plurality of satellites,
A satellite signal tracking device that tracks each satellite signal and sends tracking information;
Of the plurality of satellites tracked by the satellite signal tracking device, using satellite elevation angle information based on satellite orbit information, at least one satellite is commonly included and other satellites are included. A satellite combination determination device for obtaining a plurality of combinations of satellite signals;
A velocity vector solution candidate calculation device for calculating a velocity vector solution for each of a plurality of combinations determined by the satellite combination determination device to obtain a velocity vector solution candidate;
A speed vector having a speed vector solution candidate obtained by the speed vector solution candidate calculation device, and an optimum solution selection device for obtaining an optimum speed vector based on a degree of coincidence between the speed vector solution candidates. Decision system. A speed vector determination system which is mounted on a mobile body and obtains the speed vector of the mobile body using satellite signals from a plurality of satellites,
A satellite signal tracking device that tracks each satellite signal and sends tracking information;
Of the plurality of satellites tracked by the satellite signal tracking device, using satellite elevation angle information based on satellite orbit information, at least one satellite is commonly included and other satellites are included. A satellite combination determination device for obtaining a plurality of combinations of satellite signals;
A velocity vector solution candidate calculation device for calculating a velocity vector solution for each of a plurality of combinations determined by the satellite combination determination device to obtain a velocity vector solution candidate;
A speed vector determination system, comprising: an optimum solution selection device that obtains an optimum velocity vector based on a degree of coincidence with a previous velocity vector with respect to a velocity vector solution candidate obtained by the velocity vector solution candidate calculation device. . A speed vector determination system which is mounted on a mobile body and obtains the speed vector of the mobile body using satellite signals from a plurality of satellites,
A satellite signal tracking device that tracks each satellite signal and sends tracking information;
Of the plurality of satellites tracked by the satellite signal tracking device, using satellite elevation angle information based on satellite orbit information, at least one satellite is commonly included and other satellites are included. A satellite combination determination device for obtaining a plurality of combinations of satellite signals;
A velocity vector solution candidate calculation device for calculating a velocity vector solution for each of a plurality of combinations determined by the satellite combination determination device to obtain a velocity vector solution candidate;
With respect to the velocity vector solution candidates obtained by the velocity vector solution candidate calculation device, the minimum vector difference between the velocity vector solution candidates is compared with a predetermined threshold, and when the minimum vector difference is larger than the predetermined threshold, An optimum solution selection device that obtains an optimum velocity vector based on the degree of coincidence and obtains an optimum velocity vector based on the degree of coincidence between the respective velocity vector solution candidates when the minimum vector difference is not greater than the predetermined threshold. A velocity vector determination system characterized by   The satellite combination determination apparatus uses signal level information based on the tracking information as well as satellite elevation angle information based on the satellite orbit information when obtaining the plurality of combinations. The velocity vector determination system according to any one of the above.   In the plurality of combinations in the satellite combination determination device, the commonly included satellites are satellites having a higher elevation angle than a predetermined elevation angle, and the other satellites are satellites having a low elevation angle lower than the predetermined elevation angle. The speed vector determination system according to claim 1, wherein the speed vector determination system is characterized in that   6. The velocity vector determination system according to claim 5, wherein there are two high elevation satellites.   The optimum solution selection device compares the optimum speed vector with an approximate speed vector obtained using all satellites, and if the optimum speed vector differs from the approximate speed vector by a predetermined threshold or more, the approximate speed vector The speed vector determination system according to claim 1, wherein a final solution is obtained based on the vector. The optimum solution selection device compares the direction of the optimum speed vector with the previous direction, and when the direction of the optimum speed vector differs from the previous direction by a predetermined threshold or more, the speed is the speed of the optimum speed vector. The final solution is obtained on the basis of a velocity vector obtained by adding or subtracting a predetermined angle so that the azimuth is closer to the azimuth of the optimum velocity vector than the previous azimuth. The velocity vector determination system described in 1.

Images