基于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生成日历实例解析
Aug 21 Python
Python 查看文件的编码格式方法
Dec 21 Python
Python实现爬虫从网络上下载文档的实例代码
Jun 13 Python
Python实现将Excel转换成xml的方法示例
Aug 25 Python
Python实现爬取马云的微博功能示例
Feb 16 Python
Win10下Python3.7.3安装教程图解
Jul 08 Python
Django ORM 查询管理器源码解析
Aug 05 Python
解决jupyter notebook import error但是命令提示符import正常的问题
Apr 15 Python
判断Threading.start新线程是否执行完毕的实例
May 02 Python
Django Model中字段(field)的各种选项说明
May 19 Python
零基础学python应该从哪里入手
Aug 11 Python
全面介绍python中很常用的单元测试框架unitest
Dec 14 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
老照片 - 几十年前的收音机与人
2021/03/02 无线电
php checkbox 取值详细说明
2010/08/19 PHP
php 网上商城促销设计实例代码
2012/02/17 PHP
php实现xml转换数组的方法示例
2017/02/03 PHP
javascript实现的动态文字变换
2007/07/28 Javascript
基于jquery 的一个progressbar widge
2010/10/29 Javascript
js调用打印机打印网页字体总是缩小一号的解决方法
2014/01/24 Javascript
angularJS结合canvas画图例子
2015/02/09 Javascript
Javascript writable特性介绍
2015/02/27 Javascript
Angularjs全局变量被作用域监听的正确姿势
2016/02/06 Javascript
ClearTimeout消除闪动实例代码
2016/02/29 Javascript
深入理解JavaScript 函数
2016/06/06 Javascript
移动端触屏幻灯片图片切换插件idangerous swiper.js
2017/04/10 Javascript
值得分享和收藏的xmlplus组件学习教程
2017/05/05 Javascript
Angularjs 双向绑定时字符串的转换成数字类型的问题
2017/06/12 Javascript
JS简单实现滑动加载数据的方法示例
2017/10/18 Javascript
示例vue 的keep-alive缓存功能的实现
2018/12/13 Javascript
Python 实现网页自动截图的示例讲解
2018/05/17 Python
使用python语言,比较两个字符串是否相同的实例
2018/06/29 Python
Mac下Anaconda的安装和使用教程
2018/11/29 Python
Python综合应用名片管理系统案例详解
2020/01/03 Python
django实现日志按日期分割
2020/05/21 Python
Selenium执行完毕未关闭chromedriver/geckodriver进程的解决办法(java版+python版)
2020/12/07 Python
html5配合css3实现带提示文字的输入框(摆脱js)
2013/03/08 HTML / CSS
Html5 Canvas动画基础碰撞检测的实现
2018/12/06 HTML / CSS
Speedo速比涛中国官方网站:全球领先泳装运动品牌
2018/04/24 全球购物
汉语专业应届生求职信
2013/10/01 职场文书
数据管理员的自我评价分享
2013/11/15 职场文书
二年级学生评语大全
2014/04/23 职场文书
装饰技术负责人岗位职责
2015/04/13 职场文书
大学生入党群众意见书
2015/06/02 职场文书
Golang 实现超大文件读取的两种方法
2021/04/27 Golang
Go语言 go程释放操作(退出/销毁)
2021/04/30 Golang
通过Qt连接OpenGauss数据库的详细教程
2021/06/23 PostgreSQL
零基础学java之方法的定义与调用详解
2022/04/10 Java/Android
Win10服务全部禁用了怎么启动?Win10服务全部禁用解决方法
2022/09/23 数码科技