基于Python实现粒子滤波效果


Posted in Python onDecember 01, 2020

1、建立仿真模型

(1)假设有一辆小车在一平面运动,起始坐标为[0,0],运动速度为1m/s,加速度为0.1 m / s 2 m/s^2 m/s2,则可以建立如下的状态方程:
Y = A ∗ X + B ∗ U Y=A*X+B*U Y=A∗X+B∗U
U为速度和加速度的的矩阵
U = [ 1 0.1 ] U= \begin{bmatrix} 1 \\ 0.1\\ \end{bmatrix} U=[10.1​]
X为当前时刻的坐标,速度,加速度
X = [ x y y a w V ] X= \begin{bmatrix} x \\ y \\ yaw \\ V \end{bmatrix} X=⎣⎢⎢⎡​xyyawV​⎦⎥⎥⎤​
Y为下一时刻的状态
则观察矩阵A为:
A = [ 1 0 0 0 0 1 0 0 0 0 1 0 0 0 0 0 ] A= \begin{bmatrix} 1&0 & 0 &0 \\ 0 & 1 & 0&0 \\ 0 & 0 &1 &0 \\ 0&0 & 0 &0 \end{bmatrix} A=⎣⎢⎢⎡​1000​0100​0010​0000​⎦⎥⎥⎤​
矩阵B则决定小车的运动规矩,这里取B为:
B = [ c o s ( x ) ∗ t 0 s i n ( x ) ∗ t 0 0 t 1 0 ] B= \begin{bmatrix} cos(x)*t &0\\ sin(x)*t &0\\ 0&t\\ 1&0 \end{bmatrix} B=⎣⎢⎢⎡​cos(x)∗tsin(x)∗t01​00t0​⎦⎥⎥⎤​
python编程实现小车的运动轨迹:

"""

Particle Filter localization sample

author: Atsushi Sakai (@Atsushi_twi)

"""

import math

import matplotlib.pyplot as plt
import numpy as np
from scipy.spatial.transform import Rotation as Rot


DT = 0.1 # time tick [s]
SIM_TIME = 50.0 # simulation time [s]
MAX_RANGE = 20.0 # maximum observation range

# Particle filter parameter
NP = 100 # Number of Particle
NTh = NP / 2.0 # Number of particle for re-sampling

def calc_input():
  v = 1.0 # [m/s]
  yaw_rate = 0.1 # [rad/s]
  u = np.array([[v, yaw_rate]]).T
  return u

def motion_model(x, u):
  F = np.array([[1.0, 0, 0, 0],
         [0, 1.0, 0, 0],
         [0, 0, 1.0, 0],
         [0, 0, 0, 0]])

  B = np.array([[DT * math.cos(x[2, 0]), 0],
         [DT * math.sin(x[2, 0]), 0],
         [0.0, DT],
         [1.0, 0.0]])

  x = F.dot(x) + B.dot(u)

  return x

def main():
  print(__file__ + " start!!")

  time = 0.0
  # State Vector [x y yaw v]'
  x_true = np.zeros((4, 1))
  
  x = []
  y = []

  while SIM_TIME >= time:
    time += DT
    u = calc_input()

    x_true = motion_model(x_true, u)
    
    x.append(x_true[0])
    y.append(x_true[1])
    
  plt.plot(x,y, "-b")
    
if __name__ == '__main__':
  main()

运行结果:

基于Python实现粒子滤波效果

2、生成观测数据

实际运用中,我们需要对小车的位置进行定位,假设坐标系上有4个观测点,在小车运动过程中,需要定时将小车距离这4个观测点的位置距离记录下来,这样,当小车下一次寻迹时就有了参考点;

def observation(x_true, xd, u, rf_id):
  x_true = motion_model(x_true, u)

  # add noise to gps x-y
  z = np.zeros((0, 3))

  for i in range(len(rf_id[:, 0])):

    dx = x_true[0, 0] - rf_id[i, 0]
    dy = x_true[1, 0] - rf_id[i, 1]
    d = math.hypot(dx, dy)
    if d <= MAX_RANGE:
      dn = d + np.random.randn() * Q_sim[0, 0] ** 0.5 # add noise
      zi = np.array([[dn, rf_id[i, 0], rf_id[i, 1]]])
      z = np.vstack((z, zi))

  # add noise to input
  ud1 = u[0, 0] + np.random.randn() * R_sim[0, 0] ** 0.5
  ud2 = u[1, 0] + np.random.randn() * R_sim[1, 1] ** 0.5
  ud = np.array([[ud1, ud2]]).T

  xd = motion_model(xd, ud)

  return x_true, z, xd, ud

3、实现粒子滤波

#
def gauss_likelihood(x, sigma):
  p = 1.0 / math.sqrt(2.0 * math.pi * sigma ** 2) * \
    math.exp(-x ** 2 / (2 * sigma ** 2))

  return p

def pf_localization(px, pw, z, u):
  """
  Localization with Particle filter
  """

  for ip in range(NP):
    x = np.array([px[:, ip]]).T
    w = pw[0, ip]

    # 预测输入
    ud1 = u[0, 0] + np.random.randn() * R[0, 0] ** 0.5
    ud2 = u[1, 0] + np.random.randn() * R[1, 1] ** 0.5
    ud = np.array([[ud1, ud2]]).T
    x = motion_model(x, ud)

    # 计算权重
    for i in range(len(z[:, 0])):
      dx = x[0, 0] - z[i, 1]
      dy = x[1, 0] - z[i, 2]
      pre_z = math.hypot(dx, dy)
      dz = pre_z - z[i, 0]
      w = w * gauss_likelihood(dz, math.sqrt(Q[0, 0]))

    px[:, ip] = x[:, 0]
    pw[0, ip] = w

  pw = pw / pw.sum() # 归一化

  x_est = px.dot(pw.T)
  p_est = calc_covariance(x_est, px, pw)
  #计算有效粒子数
  N_eff = 1.0 / (pw.dot(pw.T))[0, 0] 
  #重采样
  if N_eff < NTh:
    px, pw = re_sampling(px, pw)
  return x_est, p_est, px, pw


def re_sampling(px, pw):
  """
  low variance re-sampling
  """

  w_cum = np.cumsum(pw)
  base = np.arange(0.0, 1.0, 1 / NP)
  re_sample_id = base + np.random.uniform(0, 1 / NP)
  indexes = []
  ind = 0
  for ip in range(NP):
    while re_sample_id[ip] > w_cum[ind]:
      ind += 1
    indexes.append(ind)

  px = px[:, indexes]
  pw = np.zeros((1, NP)) + 1.0 / NP # init weight

  return px, pw

4、完整源码

该代码来源于https://github.com/AtsushiSakai/PythonRobotics

"""

Particle Filter localization sample

author: Atsushi Sakai (@Atsushi_twi)

"""

import math

import matplotlib.pyplot as plt
import numpy as np
from scipy.spatial.transform import Rotation as Rot

# Estimation parameter of PF
Q = np.diag([0.2]) ** 2 # range error
R = np.diag([2.0, np.deg2rad(40.0)]) ** 2 # input error

# Simulation parameter
Q_sim = np.diag([0.2]) ** 2
R_sim = np.diag([1.0, np.deg2rad(30.0)]) ** 2

DT = 0.1 # time tick [s]
SIM_TIME = 50.0 # simulation time [s]
MAX_RANGE = 20.0 # maximum observation range

# Particle filter parameter
NP = 100 # Number of Particle
NTh = NP / 2.0 # Number of particle for re-sampling

show_animation = True


def calc_input():
  v = 1.0 # [m/s]
  yaw_rate = 0.1 # [rad/s]
  u = np.array([[v, yaw_rate]]).T
  return u


def observation(x_true, xd, u, rf_id):
  x_true = motion_model(x_true, u)

  # add noise to gps x-y
  z = np.zeros((0, 3))

  for i in range(len(rf_id[:, 0])):

    dx = x_true[0, 0] - rf_id[i, 0]
    dy = x_true[1, 0] - rf_id[i, 1]
    d = math.hypot(dx, dy)
    if d <= MAX_RANGE:
      dn = d + np.random.randn() * Q_sim[0, 0] ** 0.5 # add noise
      zi = np.array([[dn, rf_id[i, 0], rf_id[i, 1]]])
      z = np.vstack((z, zi))

  # add noise to input
  ud1 = u[0, 0] + np.random.randn() * R_sim[0, 0] ** 0.5
  ud2 = u[1, 0] + np.random.randn() * R_sim[1, 1] ** 0.5
  ud = np.array([[ud1, ud2]]).T

  xd = motion_model(xd, ud)

  return x_true, z, xd, ud


def motion_model(x, u):
  F = np.array([[1.0, 0, 0, 0],
         [0, 1.0, 0, 0],
         [0, 0, 1.0, 0],
         [0, 0, 0, 0]])

  B = np.array([[DT * math.cos(x[2, 0]), 0],
         [DT * math.sin(x[2, 0]), 0],
         [0.0, DT],
         [1.0, 0.0]])

  x = F.dot(x) + B.dot(u)

  return x


def gauss_likelihood(x, sigma):
  p = 1.0 / math.sqrt(2.0 * math.pi * sigma ** 2) * \
    math.exp(-x ** 2 / (2 * sigma ** 2))

  return p


def calc_covariance(x_est, px, pw):
  """
  calculate covariance matrix
  see ipynb doc
  """
  cov = np.zeros((3, 3))
  n_particle = px.shape[1]
  for i in range(n_particle):
    dx = (px[:, i:i + 1] - x_est)[0:3]
    cov += pw[0, i] * dx @ dx.T
  cov *= 1.0 / (1.0 - pw @ pw.T)

  return cov


def pf_localization(px, pw, z, u):
  """
  Localization with Particle filter
  """

  for ip in range(NP):
    x = np.array([px[:, ip]]).T
    w = pw[0, ip]

    # Predict with random input sampling
    ud1 = u[0, 0] + np.random.randn() * R[0, 0] ** 0.5
    ud2 = u[1, 0] + np.random.randn() * R[1, 1] ** 0.5
    ud = np.array([[ud1, ud2]]).T
    x = motion_model(x, ud)

    # Calc Importance Weight
    for i in range(len(z[:, 0])):
      dx = x[0, 0] - z[i, 1]
      dy = x[1, 0] - z[i, 2]
      pre_z = math.hypot(dx, dy)
      dz = pre_z - z[i, 0]
      w = w * gauss_likelihood(dz, math.sqrt(Q[0, 0]))

    px[:, ip] = x[:, 0]
    pw[0, ip] = w

  pw = pw / pw.sum() # normalize

  x_est = px.dot(pw.T)
  p_est = calc_covariance(x_est, px, pw)

  N_eff = 1.0 / (pw.dot(pw.T))[0, 0] # Effective particle number
  if N_eff < NTh:
    px, pw = re_sampling(px, pw)
  return x_est, p_est, px, pw


def re_sampling(px, pw):
  """
  low variance re-sampling
  """

  w_cum = np.cumsum(pw)
  base = np.arange(0.0, 1.0, 1 / NP)
  re_sample_id = base + np.random.uniform(0, 1 / NP)
  indexes = []
  ind = 0
  for ip in range(NP):
    while re_sample_id[ip] > w_cum[ind]:
      ind += 1
    indexes.append(ind)

  px = px[:, indexes]
  pw = np.zeros((1, NP)) + 1.0 / NP # init weight

  return px, pw


def plot_covariance_ellipse(x_est, p_est): # pragma: no cover
  p_xy = p_est[0:2, 0:2]
  eig_val, eig_vec = np.linalg.eig(p_xy)

  if eig_val[0] >= eig_val[1]:
    big_ind = 0
    small_ind = 1
  else:
    big_ind = 1
    small_ind = 0

  t = np.arange(0, 2 * math.pi + 0.1, 0.1)

  # eig_val[big_ind] or eiq_val[small_ind] were occasionally negative
  # numbers extremely close to 0 (~10^-20), catch these cases and set the
  # respective variable to 0
  try:
    a = math.sqrt(eig_val[big_ind])
  except ValueError:
    a = 0

  try:
    b = math.sqrt(eig_val[small_ind])
  except ValueError:
    b = 0

  x = [a * math.cos(it) for it in t]
  y = [b * math.sin(it) for it in t]
  angle = math.atan2(eig_vec[1, big_ind], eig_vec[0, big_ind])
  rot = Rot.from_euler('z', angle).as_matrix()[0:2, 0:2]
  fx = rot.dot(np.array([[x, y]]))
  px = np.array(fx[0, :] + x_est[0, 0]).flatten()
  py = np.array(fx[1, :] + x_est[1, 0]).flatten()
  plt.plot(px, py, "--r")


def main():
  print(__file__ + " start!!")

  time = 0.0

  # RF_ID positions [x, y]
  rf_id = np.array([[10.0, 0.0],
           [10.0, 10.0],
           [0.0, 15.0],
           [-5.0, 20.0]])

  # State Vector [x y yaw v]'
  x_est = np.zeros((4, 1))
  x_true = np.zeros((4, 1))

  px = np.zeros((4, NP)) # Particle store
  pw = np.zeros((1, NP)) + 1.0 / NP # Particle weight
  x_dr = np.zeros((4, 1)) # Dead reckoning

  # history
  h_x_est = x_est
  h_x_true = x_true
  h_x_dr = x_true

  while SIM_TIME >= time:
    time += DT
    u = calc_input()

    x_true, z, x_dr, ud = observation(x_true, x_dr, u, rf_id)

    x_est, PEst, px, pw = pf_localization(px, pw, z, ud)

    # store data history
    h_x_est = np.hstack((h_x_est, x_est))
    h_x_dr = np.hstack((h_x_dr, x_dr))
    h_x_true = np.hstack((h_x_true, x_true))

    if show_animation:
      plt.cla()
      # for stopping simulation with the esc key.
      plt.gcf().canvas.mpl_connect(
        'key_release_event',
        lambda event: [exit(0) if event.key == 'escape' else None])

      for i in range(len(z[:, 0])):
        plt.plot([x_true[0, 0], z[i, 1]], [x_true[1, 0], z[i, 2]], "-k")
      plt.plot(rf_id[:, 0], rf_id[:, 1], "*k")
      plt.plot(px[0, :], px[1, :], ".r")
      plt.plot(np.array(h_x_true[0, :]).flatten(),
           np.array(h_x_true[1, :]).flatten(), "-b")
      plt.plot(np.array(h_x_dr[0, :]).flatten(),
           np.array(h_x_dr[1, :]).flatten(), "-k")
      plt.plot(np.array(h_x_est[0, :]).flatten(),
           np.array(h_x_est[1, :]).flatten(), "-r")
      plot_covariance_ellipse(x_est, PEst)
      plt.axis("equal")
      plt.grid(True)
      plt.pause(0.001)


if __name__ == '__main__':
  main()

到此这篇关于基于Python实现粒子滤波的文章就介绍到这了,更多相关Python实现粒子滤波内容请搜索三水点靠木以前的文章或继续浏览下面的相关文章希望大家以后多多支持三水点靠木!

Python 相关文章推荐
python使用新浪微博api上传图片到微博示例
Jan 10 Python
Python抓取电影天堂电影信息的代码
Apr 07 Python
Python注释详解
Jun 01 Python
CentOS 6.X系统下升级Python2.6到Python2.7 的方法
Oct 12 Python
Python数据操作方法封装类实例
Jun 23 Python
Python文件监听工具pyinotify与watchdog实例
Oct 15 Python
Python3爬虫全国地址信息
Jan 05 Python
利用OpenCV和Python实现查找图片差异
Dec 19 Python
Django实现从数据库中获取到的数据转换为dict
Mar 27 Python
如何基于python实现不邻接植花
May 01 Python
python自动化测试三部曲之request+django实现接口测试
Oct 07 Python
Python中的socket网络模块介绍
Jul 23 Python
Django集成MongoDB实现过程解析
Dec 01 #Python
基于Django快速集成Echarts代码示例
Dec 01 #Python
Python更改pip镜像源的方法示例
Dec 01 #Python
Python读取图像并显示灰度图的实现
Dec 01 #Python
Python性能测试工具Locust安装及使用
Dec 01 #Python
python爬虫中抓取指数的实例讲解
Dec 01 #Python
OpenCV灰度化之后图片为绿色的解决
Dec 01 #Python
You might like
php定时执行任务设置详解
2015/02/06 PHP
Aster vs KG BO3 第三场2.19
2021/03/10 DOTA
图片上传即时显示缩略图的js代码
2009/05/27 Javascript
window.onload 加载完毕的问题及解决方案(下)
2009/07/09 Javascript
javascript编写贪吃蛇游戏
2015/07/07 Javascript
js仿百度登录页实现拖动窗口效果
2016/03/11 Javascript
论Bootstrap3和Foundation5网格系统的异同
2016/05/16 Javascript
js实现精确到秒的倒计时效果
2016/05/29 Javascript
Js类的静态方法与实例方法区分及jQuery拓展的两种方法
2016/06/03 Javascript
jQuery基于toggle实现click触发DIV的显示与隐藏问题分析
2016/06/12 Javascript
详解layui中的树形关于取值传值问题
2018/01/16 Javascript
JavaScript实现获取两个排序数组的中位数算法示例
2019/02/26 Javascript
纯 JS 实现放大缩小拖拽功能(完整代码)
2019/11/25 Javascript
[03:44]2014DOTA2国际邀请赛 71专访:DK战队赛前讨论视频遭泄露
2014/07/13 DOTA
浅析python中SQLAlchemy排序的一个坑
2017/02/24 Python
解决phantomjs截图失败,phantom.exit位置的问题
2018/05/17 Python
pandas通过loc生成新的列方法
2018/11/28 Python
python实现坦克大战游戏 附详细注释
2020/03/27 Python
通过celery异步处理一个查询任务的完整代码
2019/11/19 Python
python GUI库图形界面开发之PyQt5窗口控件QWidget详细使用方法
2020/02/26 Python
django Layui界面点击弹出对话框并请求逻辑生成分页的动态表格实例
2020/05/12 Python
Python库安装速度过慢解决方案
2020/07/14 Python
python如何爬取动态网站
2020/09/09 Python
用canvas显示验证码的实现
2020/04/10 HTML / CSS
欧舒丹比利时官网:L’OCCITANE比利时
2017/04/25 全球购物
Kathmandu新西兰官网:新西兰户外运动品牌
2019/07/27 全球购物
C#笔试题
2015/07/14 面试题
应届毕业生求职信范文
2013/12/18 职场文书
职业生涯规划书前言
2014/04/15 职场文书
篮球兴趣小组活动总结
2014/07/07 职场文书
2014年环卫工作总结
2014/11/22 职场文书
工作感想范文
2015/08/07 职场文书
聘任合同书
2015/09/21 职场文书
css3中transform属性实现的4种功能
2021/08/07 HTML / CSS
Nginx配置文件详解以及优化建议指南
2021/09/15 Servers
HttpClient实现文件上传功能
2022/08/14 Java/Android