Python 做粒子滤波

最新推荐文章于 2024-04-16 16:29:22 发布

三川小哥知乎

最新推荐文章于 2024-04-16 16:29:22 发布

阅读量3.8k

点赞数 2

本文链接：https://blog.csdn.net/qq_27262241/article/details/85121326

版权

本文介绍了使用Python进行粒子滤波的方法。每个粒子代表机器人位置的估计，通过运动模型更新粒子位置，并结合地图信息计算权重。根据权重，对粒子进行重采样以优化位置估计，最终的加权平均位置即为当前位置的估计。

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

简述：

每一个粒子代表一个机器人当前位置的一个估计值，走一步后，运动u使每个粒子得到自己的新预测位置，每个预测位置与已知地图的点构建预测观察值，预测观察值和实际观察值的误差值 在上一步计算的协方差中就得到了这个粒子的权重。所有粒子位置的加权平均就是当前位置估计值，协方差的更新是通过对所有粒子与估计位置的加权离差平方和SSD

：

根据权重再重新采样?思路就是把权重大的粒子切分成很多个最小单位的粒子

#!/usr/bin/env python3
# -*- coding: utf-8 -*-
"""
Created on Wed Dec 19 10:30:20 2018

@author: sc
"""
import numpy as np
import math
import matplotlib.pyplot as plt
dt=0.1
NP=100
NTh=NP/2
Q=np.diag([0.1])**2
Qsim=np.diag([0.2])**2
MAX_RANGE=20
def motion_model(x,u):
    B=np.matrix([[dt*math.cos(x[2,0]),0.0],
                  [dt*math.sin(x[2,0]),0.0],
                  [0.0,dt],
                  [1.0,0.0]])
    x=x+B.dot(u)
    return x
def pf_filter(xEst,covEst,px,pw,z,u):
    for i in range(NP):
        pf=motion