Circuitry and method
A circuitry for simultaneous localization and mapping for a mobile platform, wherein the circuitry is configured to: estimate, based on obtained radar detection data, an ego-motion of the mobile platform; and update, based on the estimated ego-motion and the obtained radar detection data, a set of particles, wherein each particle of the set of particles includes a location and an orientation of the mobile platform and an occupancy grid map that represents an environment of the mobile platform, wherein the occupancy grid map includes a plurality of cells and each cell of the plurality of cells is assigned an occupation probability which indicates a probability that the cell is occupied by a target.
1. A circuitry for simultaneous localization and mapping for a mobile platform, the circuitry being configured to:
estimate, based on obtained radar detection data, an ego-motion of the mobile platform,
update, based on the estimated ego-motion and the obtained radar detection data, a set of particles, wherein each particle of the set of particles includes a location and an orientation of the mobile platform and an occupancy grid map that represents an environment of the mobile platform, wherein the occupancy grid map includes a plurality of cells and each cell of the plurality of cells is assigned an occupation probability which indicates a probability that the cell is occupied by a target, and
estimate, based on the obtained radar detection data, a location of the target, wherein the occupation probability is updated based on the estimated location of the target,
wherein the occupation probability is updated further based on a detection probability of the target, wherein the detection probability is based on a first probability distribution of a distance of the target to the mobile platform, a second probability distribution for a detection angle of the target to a radar sensor on the mobile platform, and a third probability distribution for a detection noise level of the target.
2. The circuitry according to claim 1 , wherein the circuitry is further configured to determine a particle weight for each particle of the set of particles, wherein the particle weight indicates a quality estimation of the particle.
3. The circuitry according to claim 2 , wherein the particle weight of each particle of the set of particles is determined based on the occupation probability of at least a part of the cells of the plurality of cells of the occupancy grid map of the particle.
4. The circuitry according to claim 3 , wherein the particle weight is determined based on a number of targets in the occupancy grid map.
5. The circuitry according to claim 3 , wherein the particle weight is determined based on an average probability of the occupancy grid map in a local update area corresponding to local grids of the radar sensors.
6. The circuitry according to claim 3 , wherein the particle weight is determined by multiplying a particle weight determined based on a number of targets in the occupancy grid map with a particle weight determined based on an average probability of the occupancy grid map in a local update area corresponding to local grids of the radar sensors.
7. The circuitry according to claim 2 , wherein the circuitry is further configured to perform particle resampling when the particle weight of a predetermined number of particles of the set of particles is below a predetermined threshold.
8. The circuitry according to claim 7 , wherein the particle resampling includes drawing with replacement from the set of particles, wherein a probability that a particle of the set of particles is drawn is based on the corresponding particle weight of the particle.
9. The circuitry according to claim 1 , wherein the circuitry is further configured to perform dead reckoning based on the estimated ego-motion, wherein the location and the orientation of the mobile platform of each particle of the set of particles are updated based on the dead reckoning.
10. The circuitry according to claim 1 , wherein the circuitry is further configured to perform scattering of at least one of the location and the orientation of the mobile platform, wherein at least one of the location and the orientation of the mobile platform is updated based on the scattering.
11. The circuitry according to claim 10 , wherein the circuitry is further configured to
determine a location of the target based on the scattered location and orientation of the mobile platform.
12. The circuitry according to claim 1 , wherein the circuitry is further configured to initialize the occupancy grid map of each particle of the set of particles based on a free-space model.
13. A method for simultaneous localization and mapping for a mobile platform, the method comprising:
estimating, based on obtained radar detection data, an ego-motion of the mobile platform;
updating, based on the estimated ego-motion and the obtained radar detection data, a set of particles, wherein each particle of the set of particles includes a location and an orientation of the mobile platform and an occupancy grid map that represents an environment of the mobile platform, wherein the occupancy grid map includes a plurality of cells and each cell of the plurality of cells is assigned an occupation probability which indicates a probability that the cell is occupied by a target; and
estimating, based on the obtained radar detection data, a location of the target, wherein the occupation probability is updated based on the estimated location of the target,
wherein the occupation probability is updated further based on a detection probability of the target, wherein the detection probability is based on a first probability distribution of a distance of the target to the mobile platform, a second probability distribution for a detection angle of the target to a radar sensor on the mobile platform, and a third probability distribution for a detection noise level of the target.
14. The method according to claim 13 , further comprising:
determining a particle weight for each particle of the set of particles, wherein the particle weight indicates a quality estimation of the particle.
15. The method according to claim 14 , wherein the particle weight of each particle of the set of particles is determined based on the occupation probability of at least a part of the cells of the plurality of cells of the occupancy grid map of the particle.
16. The method according to claim 14 , further comprising:
performing particle resampling when the particle weight of a predetermined number of particles of the set of particles is below a predetermined threshold.
17. The method according to claim 16 , wherein the particle resampling includes drawing with replacement from the set of particles, wherein a probability that a particle of the set of particles is drawn is based on the corresponding particle weight of the particle.
18. The method according to claim 13 , further comprising:
performing dead reckoning based on the estimated ego-motion, wherein the location and the orientation of the mobile platform of each particle of the set of particles are updated based on the dead reckoning.
19. The method according to claim 13 , further comprising:
performing scattering of at least one of the location and the orientation of the mobile platform, wherein at least one of the location and the orientation of the mobile platform is updated based on the scattering.
20. The method according to claim 13 , further comprising:
initializing the occupancy grid map of each particle of the set of particles based on a free-space model.