A method for performing
Simultaneous Localization And Mapping (SLAM) of the surroundings of an autonomously controlled moving platform (such as UAV or a vehicle), using
radar signals, comprising the following steps: receiving samples of the received IF
radar signals, from the DSP; receiving previous map from memory; receiving data regarding motions parameters of the moving platform from an
Inertial Navigation System (INS) module, containing
MEMS sensors data; grouping points to bodies using a clustering process; merging bodies that are marked by the clustering process as separate bodies, using prior knowledge; for each body, creating a local grid map around the body with a
mass function per entry of the grid map; matching between bodies from previous map and the new bodies; calculating the assumed new location of the moving platform for each on the mL particles using previous frame results and new INS data; for each calculated new location of the mL particles with normal distribution, sampling N assumed locations; for each body and each body particle from the previous map and for each location particle, calculating the
velocity vector and orientation of the body between previous and current map, with respect to the environment using the body velocity calculated in previous step and sampling N2 particles of the body, or using
image registration to create one particle where each particle contains the location and orientation of the body in the new map; for each particle from the collection of particles of the body: propagating the body grid according the chosen particle; calculating the Conflict of the new observed body and propagated body grid, using a fusion function on the parts of the grid that are matched; calculating Extra Conflict as a sum of the
mass of occupied new and old grid cells that do not have a match between the grids; calculating particle weight as an inverse weight of the combination between Conflict and Extra conflict; for each N1 location particle, calculating the weight as the sum of the best weight per body for that particle;
resampling mL particles for locations according to the location weight; for each body, choosing mB particles from the particles with location in one of the mL particles for the chosen location, and according to the particles weights; for each body and each one of the mB particles calculate the body velocity using motion model; and creating map for next step, with all the chosen particles and the
mass function of the grid around each body, for each body particle according to the fusion function.