基于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之编写类之一创建实例
Oct 11 Python
深入学习python的yield和generator
Mar 10 Python
Python深入06——python的内存管理详解
Dec 07 Python
使用Python快速搭建HTTP服务和文件共享服务的实例讲解
Jun 04 Python
python实现顺序表的简单代码
Sep 28 Python
pyqt5 comboBox获得下标、文本和事件选中函数的方法
Jun 14 Python
浅谈Python2之汉字编码为unicode的问题(即类似\xc3\xa4)
Aug 12 Python
PyCharm搭建Spark开发环境的实现步骤
Sep 05 Python
Pytorch 保存模型生成图片方式
Jan 10 Python
python实现可下载音乐的音乐播放器
Feb 25 Python
python实现程序重启和系统重启方式
Apr 16 Python
Django serializer优化类视图的实现示例
Jul 16 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
一个简洁的多级别论坛
2006/10/09 PHP
第三章 php操作符与控制结构代码
2011/12/30 PHP
PHP SOCKET编程详解
2015/05/22 PHP
PHP中set error handler函数用法小结
2015/11/11 PHP
Laravel创建数据库表结构的例子
2019/10/09 PHP
JavaScript 密码强度判断代码
2009/09/05 Javascript
jquery ui resizable bug解决方法
2010/10/26 Javascript
最短的IE判断代码
2011/03/13 Javascript
多种方式实现JS调用后台方法进行数据交互
2013/08/20 Javascript
将HTML格式的String转化为HTMLElement的实现方法
2014/08/07 Javascript
JavaScript学习笔记之内置对象
2015/01/22 Javascript
原生js获取元素样式的简单方法
2016/08/06 Javascript
详解js中常规日期格式处理、月历渲染和倒计时函数
2016/12/28 Javascript
JS表单数据验证的正则表达式(常用)
2017/02/18 Javascript
JavaScript正则替换HTML标签功能示例
2017/03/02 Javascript
JavaScript创建对象的七种方式(推荐)
2017/06/26 Javascript
在webstorm开发微信小程序之使用阿里自定义字体图标的方法
2018/11/15 Javascript
vue学习笔记之Vue中css动画原理简单示例
2020/02/29 Javascript
深入理解python中的select模块
2017/04/23 Python
Python画柱状统计图操作示例【基于matplotlib库】
2018/07/04 Python
python3 实现对图片进行局部切割的方法
2018/12/05 Python
简单了解python的内存管理机制
2019/07/08 Python
利用python中集合的唯一性实现去重
2020/02/11 Python
Proenza Schouler官方网站:纽约女装和配饰品牌
2019/01/03 全球购物
最新销售员个人自荐信
2013/09/21 职场文书
2014信息技术专业毕业生自我评价
2014/01/17 职场文书
购房意向书
2014/04/01 职场文书
说明书范文
2014/05/07 职场文书
公司授权委托书范文
2014/08/02 职场文书
公司员工安全协议书
2014/11/21 职场文书
2014年绿化工作总结
2014/12/09 职场文书
幼儿园新学期开学寄语
2015/05/27 职场文书
2015年“我们的节日·中秋节”活动总结
2015/07/30 职场文书
学校2016年圣诞节活动总结
2016/03/31 职场文书
解析CSS 提取图片主题色功能(小技巧)
2021/05/12 HTML / CSS
详解Oracle块修改跟踪功能
2021/11/07 Oracle