Estimation device, control method, program and storage medium
Provided is an estimation device capable of estimating a current position with a high degree of accuracy. A driving support system includes: a vehicle mounted device 1 that is mounted on a vehicle and performs a control concerning vehicle driving support; a LIDAR 2 ; a gyroscope sensor 3 ; and a vehicle speed sensor 4 . The vehicle mounted device 1 acquires, from the LIDAR 2 , a measurement value z t k indicating a positional relationship between a reference landmark Lk of an index k and the own vehicle. The vehicle mounted device 1 calculates a post estimated value x{circumflex over ( )} t by correcting a prior estimated value x t estimated, on the basis of the measurement value z t k and a position vector m k of the reference landmark Lk included in a map DB 10 , from a moving speed measured by the vehicle speed sensor 4 and an angular rate measured by the gyroscope sensor 3.
1. An estimation device comprising:
a storage medium configured to store a program and map information; and
a central processing unit (CPU) coupled to the storage medium and configured to execute the program to:
acquire map information from the storage medium;
acquire first information, wherein the first information indicates a distance and an angle to an object situated within a first range at a reference time;
calculate a first estimated position, wherein the first estimated position is an estimated position of a moving body at the reference time;
determine second information based on the first estimated position and the acquired map information, wherein the second information indicates a positional relationship, at the reference time, between the first estimated position and a position of the object indicated by position information of the object included in the acquired map information;
determine a correction valued based on calculating the difference between the first information and the second information; and
update the first estimated position of the moving body based on adding the determined correction value to the first estimated value, wherein the updated first estimated position is an updated estimated position of the moving body at the reference time.
2. The estimation device according to claim 1 ,
wherein the CPU is further configured to execute the program to calculate the first estimated position at least based on a position of the moving body estimated a predetermined time before the reference time.
3. The estimation device according to claim 2 , wherein the CPU is further configured to execute the program to
acquire control information of the moving body; and
calculate the first estimated position based on the position estimated the predetermined time before the reference time and the control information of the moving body_.
4. The estimation device according to claim 2 ,
wherein a predicting step, in which the CPU executes the program to calculate the first estimated position, and an updating step, in which the CPU executes the program to update the first estimated position calculated at the predicting step based on the difference between the first information and the second information, are alternately executed, and
wherein at the predicting step, the CPU executes the program to calculate the first estimated position based on a prior first estimated position updated at a previous updating step prior to the predicting step.
5. The estimation device according to claim 1 ,
wherein the CPU is further configured to execute the program to acquire the first information from a measurement device which includes an emitting unit, a light receiving unit and an output unit, the emitting unit emitting laser light while changing outgoing direction of the laser light, the light receiving unit receiving the laser light reflected by the object, the output unit outputting the first information based on a light receiving signal outputted by the light receiving unit and the outgoing direction of the laser light received by the light receiving unit and a response delay time of the laser light.
6. The estimation device according to claim 1 ,
wherein the object is an artificial object.
7. The estimation device according to claim 6 ,
wherein the object is an artificial object periodically arranged.
8. A control method executed by an estimation device comprising:
acquiring map information;
calculating a first estimated position, wherein the first estimated position is an estimated position of a position of the moving body at a reference time;
acquiring first information, wherein the first information indicates a distance and an angle to an object situated within a first range at the reference time;
determining second information based on the first estimated position and the acquired map information, wherein the second information indicates a positional relationship, at the reference time, between the first estimated position and a position of the object indicated by position information of the object included in the acquired map information;
determining a correction valued based on calculating the difference between the first information and the second information; and
updated the first estimated position of the moving body based on adding the determined correction value to the first estimated value, wherein the updated first estimated position is an updated estimated position of the moving body at the reference time.
9. A non-transitory storage medium storing a program, the program, when executed by a computer, the program causing the computer to:
acquire map information;
calculate a first estimated position, wherein the first estimated position is an estimated position of a position of the moving body at a reference time;
acquire first information, wherein the first information indicates a distance and an angle to an object situated within a first range at the reference time;
determine second information based on the first estimated position and the acquired map information, wherein the second information indicates a positional relationship, at the reference time, between the first estimated position and a position of the object indicated by position information of the object included in the acquired map information;
determine a correction valued based on calculating the difference between the first information and the second information; and
update the first estimated position of the moving body based on adding the determined correction value to the first estimated value, wherein the updated first estimated position is an updated estimated position of the moving body at the reference time.