udacity Markov定位 - python版本（1）

udacity Markov定位程序，实现移动模式定位。python版本，根据网上的C++程序改写，方便大家参考。

参考代码：

（1）https://github.com/informramiz?page=5&tab=repositories

这个工程项目里有代码以及数据。感谢作者的无私分享。

参考文章：

（1）自动驾驶定位算法(九)-直方图滤波定位-腾讯云开发者社区-腾讯云

这篇文章对于理解Bayes滤波很有帮助。

参考公式：

一、移动模式

本程序实现Markov定位中的移动模式。

二、车辆移动方式

车辆移动过程中，根据概率，车辆可以移动到任意到任意一个位置。

如下图示：

图1：车辆位置移动方式（1）

T+1时刻，车辆位置变化来源示意如下图示：

图2：车辆位置移动方式（2）

三、概率转移

已知T时刻车辆位置概率分布，如下图示：

图3：T时刻车辆位置概率分布

求T+1时刻，车辆移动后，车辆位置概率分布，如下图示：

图4：T+1时刻车辆位置概率分布（待求解）

四、卷积计算

T+1时刻，车辆位置概率由两部分决定，T时刻当前车辆位置概率，T+1时刻，车辆移动到下一个位置的转移概率；如下图示：

图5：T+1时刻车辆位置概率求解示意图

五、Markov定位程序-移动模式

import numpy as np
import matplotlib.pyplot as plt
import math

b_initialized = 0
map_1d = [[1,9],[2,15],[3,25],[4,31],[5,59],[6,77]] #index,xf
controls_delta_x_f = [1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1]
bel_x_init = np.zeros(100)
bel_x = np.zeros(100)

M_PI = 3.1415926
ONE_OVER_SQRT_2PI = 1 / np.sqrt(2 * M_PI)
control_std = 1.0

time = 0

def square(x):
    return x * x

def normpdf(x, mu, std):
    return (ONE_OVER_SQRT_2PI / std) * np.exp(-0.5 * square((x - mu) / std))

def process_measurement(delta_x_f):
    global b_initialized
    global bel_x_init
    global bel_x
    if(b_initialized == 0):
        b_initialized = 1
        for l in range(6):
            landmark_temp = map_1d[l]
            if(landmark_temp[1] > 0 and landmark_temp[1] < len(bel_x_init)):
                position_x = landmark_temp[1]
                bel_x_init[position_x] = 1.0
                bel_x_init[position_x - 1] = 1.0
                bel_x_init[position_x + 1] = 1.0

        #normalize bel_x_init
        sum = np.sum(bel_x_init)
        bel_x_init = bel_x_init / sum

    for i in range(len(bel_x)):
        pos_i = float(i)
        posterior_motion = 0.0
        for j in range(len(bel_x)):
            pos_j = float(j)
            distance_ij = pos_i - pos_j
            transition_prob = normpdf(distance_ij, delta_x_f, control_std)
            posterior_motion = posterior_motion + transition_prob * bel_x_init[j]

        bel_x[i] = posterior_motion

    #normalize bel_x
    sum = np.sum(bel_x)
    bel_x = bel_x / sum
    bel_x_init = bel_x.copy()

def display_map(grid, bar_width=1):
    if(len(grid) > 0):
        x_labels = range(len(grid))
        plt.bar(x_labels, height=grid, width=bar_width-0.2, color='b')
        plt.xlabel('Grid Cell')
        plt.ylabel('Probability')
        plt.ylim(0, 1) # range of 0-1 for probability values
        plt.title('Probability of the robot being at each cell in the grid')
        plt.xticks(np.arange(min(x_labels), max(x_labels)+1, 3))
        plt.show()
    else:
        print('Grid is empty')

if __name__ == '__main__':
    for t in range(len(controls_delta_x_f)):
        time = t
        process_measurement(controls_delta_x_f[t])
        display_map(bel_x)

六、结果展示

（1）初始概率分布

（2）移动14次后，车辆位置概率分布