2D Histogram filter localization example

histogram_filter_2d_localization.py

# -*- coding: utf-8 -*-
# @Author: insaneyilin
# Python version 2.7
# Adapted from
#     https://github.com/AtsushiSakai/PythonRobotics/tree/master/Localization/histogram_filter 

import copy
import math

import matplotlib.pyplot as plt
import numpy as np
from scipy.ndimage import gaussian_filter
from scipy.stats import norm


SIM_TIME = 40.0  # simulation time [s]
DT = 0.25  # time tick [s]
MAX_MEASURE_RANGE = 20.0
RANGE_STD = 2.0
MOTION_STD= 3.0

show_animation = True

class HistogramFilter2DLocator(object):
    def __init__(self, min_x, min_y, max_x, max_y, map_reso):
        self.min_x = min_x
        self.min_y = min_y
        self.max_x = max_x
        self.max_y = max_y
        self.map_reso = map_reso
        self.grid_map_x_size = int(round((self.max_x - self.min_x) / self.map_reso))
        self.grid_map_y_size = int(round((self.max_y - self.min_y) / self.map_reso))
        self.grid_map_dx = 0
        self.grid_map_dy = 0
        self.grid_map_data = [[1.0 for _ in range(self.grid_map_y_size)] \
                                                               for _ in range(self.grid_map_x_size)]
        self.grid_map_data = self.normalize_probability(self.grid_map_data)

    def update_localization(self, measurements, range_std, vel, dt, motion_std):
        self.grid_map_data = self.motion_update(self.grid_map_data, vel, dt, motion_std)
        self.grid_map_data = self.measurements_update(self.grid_map_data, measurements, range_std)
        return

    def motion_update(self, gmap, vel, dt, motion_std):
        self.grid_map_dx += dt * vel[0, 0]
        self.grid_map_dy += dt * vel[1, 0]
        x_shift = self.grid_map_dx // self.map_reso
        y_shift = self.grid_map_dy // self.map_reso
        # map should be shifted
        if abs(x_shift) >= 1.0 or abs(y_shift) >= 1.0:
            gmap = self.map_shift(gmap, int(x_shift), int(y_shift))
            self.grid_map_dx -= x_shift * self.map_reso
            self.grid_map_dy -= y_shift * self.map_reso
        gmap = gaussian_filter(gmap, sigma=motion_std)
        return gmap

    def measurements_update(self, gmap, measurements, std):
        for i in range(measurements.shape[0]):
            for ix in range(self.grid_map_x_size):
                for iy in range(self.grid_map_y_size):
                    gmap[ix][iy] *= self.calc_gaussian_observation_pdf(measurements, i, \
                                                                                                                               ix, iy, std)
        gmap = self.normalize_probability(gmap)
        return gmap

    def map_shift(self, gmap, x_shift, y_shift):
        tmp_gmap = copy.deepcopy(gmap)
        for ix in range(self.grid_map_x_size):
            for iy in range(self.grid_map_y_size):
                nix = ix + x_shift
                niy = iy + y_shift
                if nix in range(self.grid_map_x_size) and niy in range(self.grid_map_y_size):
                    gmap[ix + x_shift][iy + y_shift] = tmp_gmap[ix][iy]
        return gmap

    def normalize_probability(self, gmap):
        prob_sum = sum([sum(igmap) for igmap in gmap])
        for ix in range(self.grid_map_x_size):
            for iy in range(self.grid_map_y_size):
                gmap[ix][iy] /= prob_sum
        return gmap

    def calc_gaussian_observation_pdf(self, z, iz, ix, iy, std):
        # predicted range
        x = ix * self.map_reso + self.min_x
        y = iy * self.map_reso + self.min_y
        d = math.sqrt((x - z[iz, 1]) ** 2 + (y - z[iz, 2]) ** 2)
        # likelihood
        pdf = (1.0 - norm.cdf(abs(d - z[iz, 0]), 0.0, std))
        return pdf

def get_landmark_range_measurements_with_noise(landmarks_gt, position_gt, \
            max_range=50.0, range_noise=2.0):
    z = np.zeros((0, 3))
    for i in range(len(landmarks_gt[:, 0])):
        diff_x = position_gt[0, 0] - landmarks_gt[i, 0]
        diff_y = position_gt[1, 0] - landmarks_gt[i, 1]
        dist = math.hypot(diff_x, diff_y)
        if dist <= max_range:
            zi = np.array([dist + 2 * range_noise * np.random.random() - range_noise,
                    landmarks_gt[i, 0] + 2 * range_noise * np.random.random() - range_noise,
                    landmarks_gt[i, 1] + 2 * range_noise * np.random.random() - range_noise])
            z = np.vstack((z, zi))
    return z


def calc_grid_index(hf):
    mx, my = np.mgrid[slice(hf.min_x - hf.map_reso / 2.0, hf.max_x + hf.map_reso / 2.0, hf.map_reso),
                      slice(hf.min_y - hf.map_reso / 2.0, hf.max_y + hf.map_reso / 2.0, hf.map_reso)]
    return mx, my

def draw_heat_map(hf, mx, my):
    data = hf.grid_map_data
    maxp = np.amax(data)
    max_p_ix, max_p_iy = np.where(data == maxp)
    max_p_x = max_p_ix[0] * hf.map_reso + hf.min_x
    max_p_y = max_p_iy[0] * hf.map_reso + hf.min_y
    plt.plot(max_p_x, max_p_y, "og", label='maxp_pos')
    plt.pcolor(mx, my, data, vmax=maxp, cmap=plt.cm.get_cmap("Blues"))
    plt.axis("equal")


if __name__ == '__main__':
    LANDMARKS_GT = np.array([[-12.0, 15.0],
            [0.0, -10.0],
            [5.0, 0.0],
            [16.0, 20.0]])
    time = 0.0

    position_gt = np.zeros((2, 1))  # x, y
    position_gt[0, 0] = -20.0
    position_gt[1, 0] = -20.0
    vel_gt = np.ones((2, 1))  # vx, vy

    hf = HistogramFilter2DLocator(min_x=-25.0, min_y=-25.0, \
                                                                    max_x=25.0, max_y=25.0,map_reso=1.0)
    mx, my = calc_grid_index(hf)  # for grid map visualization

    while time < SIM_TIME:
        time += DT

        if np.random.randint(2) > 0:
            vel_gt[0, 0] = 0.5
            vel_gt[1, 0] = 0.5
        else:
            vel_gt[0, 0] = 1.5
            vel_gt[1, 0] = 1.5
        # linear motion
        position_gt += vel_gt * DT
        # get landmarks measurements with noise
        landmarks_measurements = \
                get_landmark_range_measurements_with_noise(LANDMARKS_GT, position_gt, \
                        MAX_MEASURE_RANGE, RANGE_STD)
        hf.update_localization(landmarks_measurements, RANGE_STD, vel_gt, DT, MOTION_STD)
        if show_animation:
            plt.cla()
            draw_heat_map(hf, mx, my)
            # draw position gt
            plt.plot(position_gt[0, :], position_gt[1, :], "xr", label='position_gt')
            # draw ground truth landmarks
            plt.plot(LANDMARKS_GT[:, 0], LANDMARKS_GT[:, 1], ".k", label='landmarks_gt')
            # draw landmarks measurements
            plt.plot(landmarks_measurements[:, 1], landmarks_measurements[:, 2], "bo", \
                            label='landmarks_measurements')
            plt.title("Time[s]:" + str(time)[0: 4])
            plt.legend()
            plt.pause(0.1)
    print('Done.')