Python 做粒子滤波

本文介绍了使用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
评论
添加红包

请填写红包祝福语或标题

红包个数最小为10个

红包金额最低5元

当前余额3.43前往充值 >
需支付:10.00
成就一亿技术人!
领取后你会自动成为博主和红包主的粉丝 规则
hope_wisdom
发出的红包
实付
使用余额支付
点击重新获取
扫码支付
钱包余额 0

抵扣说明:

1.余额是钱包充值的虚拟货币,按照1:1的比例进行支付金额的抵扣。
2.余额无法直接购买下载,可以购买VIP、付费专栏及课程。

余额充值