Particle Filter算法

最新推荐文章于 2023-08-12 02:44:39 发布

hepansls

最新推荐文章于 2023-08-12 02:44:39 发布

阅读量617

点赞数 1

文章标签： python 自动驾驶

本文链接：https://blog.csdn.net/qq_16737417/article/details/104612794

版权

本文介绍了基于Python的Particle Filter算法在自动驾驶中的应用，详细解析了如何计算车辆（robot）位置，并通过添加传感器噪声和随机误差来分析算法的准确性。讨论了权重分布的改变（如从正态分布到帕累托分布）对定位结果的影响，以及通过多次采样减少随机噪声提高定位精度的方法。

摘要由CSDN通过智能技术生成

本次基于Paricles Filter的开源python代码，用几种方法对最终的mouse位置进行计算
源代码详情见：
代码及说明链接：
http://ros-developer.com/2019/04/10/parcticle-filter-explained-with-python-code-from-scratch/
里面包含上述问题的原理讲解视频及相应源代码（鼠标坐标即robot）。

import numpy as np
import scipy
from numpy.random import uniform
import scipy.stats
import heapq
np.set_printoptions(threshold=3)
np.set_printoptions(suppress=True)#压缩显示数据
import cv2
from sklearn.cluster import MeanShift, estimate_bandwidth
#pip install sklearn -i https://pypi.tuna.tsinghua.edu.cn/simple
import random
def drawLines(img, points, r, g, b):
    cv2.polylines(img, [np.int32(points)], isClosed=False, color=(r, g, b))


def drawCross(img, center, r, g, b):
    d = 5
    t = 2
    if cv2.__version__[0]=='4':
        LINE_AA = cv2.LINE_AA
    else:
        LINE_AA=cv2.CV_AA
    # LINE_AA = cv2.LINE_AA if cv2.__version__ == '3' else cv2.CV_AA
    #这里需要修改一下'3'，根据cv版本的不同，里面的宏定义不同
    color = (r, g, b)
    ctrx = center[0, 0]
    ctry = center[0, 1]
    cv2.line(img, (ctrx - d, ctry - d), (ctrx + d, ctry + d), color, t, LINE_AA)
    cv2.line(img, (ctrx + d, ctry - d), (ctrx - d, ctry + d), color, t, LINE_AA)


def mouseCallback(event, x, y, flags, null):#代表鼠标位于窗口的（x，y）坐标位置，即Point(x,y);
    global center
    global trajectory
    global previous_x
    global previous_y
    global zs

    center = np.array([[x, y]])
    #print(center, end='\n')
    trajectory = np.vstack((trajectory, np.array([x, y]))) #将两个数组按行放到一起
    #print(trajectory,end='\n')
    # gauss_noise=sensorSigma * np.random.randn(1,2) + sensorMu
    # gauss_noise = np.array([0.] * NL)
    # for i in range(0, NL):
    #     gauss_noise[i] += (random.gauss(gauss_mu, gauss_sigm) * 5)
    if previous_x > 0:
        heading = np.arctan2(np.array([y - previous_y]), np.array([previous_x - x]))

        if heading > 0:
            heading = -(heading - np.pi)
        else:
            heading = -(np.pi + heading)
        # print(heading)
        # 求范数，默认ord=2为求二范数，也就是距离，axis=1表示按行向量处理，求多个行向量的范数
        distance = np.linalg.norm(np.array([[previous_x, previous_y]]) - np.array([[x, y]]), axis=1)
        # print(distance,end='\n')两次移动之间的距离
        std = np.array([2, 4])
        u = np.array([heading, distance])
        # 原始粒子得到预测粒子
        predict(particles, u, std, dt=1.)
        # 计算鼠标与landmark之间的距离z[i]
        # zs = np.linalg.norm(landmarks - center, axis=1)
        # zs = (np.linalg.norm(landmarks - center, axis=1) + (np.random.randn(NL) * sensor_std_err))
        # #更新权重
        # zs += gauss_noise
        number_measurement=10
        zs = np.array([0.]*NL)
        for times in range(0,number_measurement):
            gauss_noise = np.array([0.] * NL)
            for i in range(0, NL):
                gauss_noise[i] += (random.gauss(gauss_mu, gauss_sigm) * 5)
            zs += (np.linalg.norm(landmarks - center, axis=1) + (np.random.randn(NL) * sensor_std_err))+gauss_noise
        zs /=number_measurement
        update(particles, weights, z=zs, R=50, landmarks=landmarks)
        # print(weights,end='\n')
        indexes = systematic_resample(weights)
        # print(indexes,end='\n')
        resample_from_index(particles, weights, indexes