二维粒子滤波python

二维粒子滤波是一种基于蒙特卡罗方法的状态估计算法，常用于机器人自定位和目标跟踪等领域。在二维粒子滤波中，机器人的状态被表示为一个二维向量，包括机器人的位置和方向。粒子滤波的核心思想是通过一系列粒子来近似表示机器人的状态分布，然后根据测量数据和运动模型对粒子进行更新和重采样，从而实现对机器人状态的估计。

在Python中，可以使用numpy和matplotlib等库来实现二维粒子滤波算法。具体实现过程包括初始化粒子、运动模型、测量模型、重采样等步骤。其中，权值的计算是粒子滤波器的关键一步，它决定了一次迭代中哪些粒子能够存活。在每次迭代中，粒子的权值会根据测量数据和运动模型进行更新，然后根据权值进行重采样，从而得到下一次迭代的粒子集合。

相关问题

蒙特卡洛粒子滤波定位算法源代码

以下是一个简单的Python实现蒙特卡洛粒子滤波定位算法的源代码，供您参考：

import numpy as np

# 运动模型
def motion_model(x, u):
    # 假设机器人在x位置，执行u运动后到达x_prime位置
    x_prime = np.array([x[0] + u[0]*np.cos(x[2]), 
                        x[1] + u[0]*np.sin(x[2]), 
                        x[2] + u[1]])
    return x_prime

# 传感器模型
def sensor_model(x, z, sensor_std):
    # 计算机器人在x位置，测量z的概率
    z_true = np.array([np.sqrt((landmark[0] - x[0])**2 + (landmark[1] - x[1])**2) for landmark in landmarks])
    prob = np.exp(-0.5*(z - z_true)**2/sensor_std**2)
    return np.prod(prob)

# 初始化粒子权重
def initialize_particles(num_particles):
    particles = np.zeros((num_particles, 3)) # 每个粒子包含(x, y, theta)三个状态
    particles[:, :2] = np.random.uniform(low=[0, 0], high=[10, 10], size=(num_particles, 2)) # x, y坐标随机初始化
    particles[:, 2] = np.random.uniform(low=-np.pi, high=np.pi, size=num_particles) # theta方向随机初始化
    weights = np.ones(num_particles)/num_particles # 初始权重均为1/N
    return particles, weights

# 粒子更新
def update_particles(particles, u, sensor_std):
    for i in range(particles.shape[0]):
        # 运动模型更新
        particles[i] = motion_model(particles[i], u)
        # 传感器模型更新
        weights[i] *= sensor_model(particles[i], z, sensor_std)
    weights /= np.sum(weights) # 归一化权重
    return particles, weights

# 粒子重采样
def resample_particles(particles, weights):
    indices = np.random.choice(particles.shape[0], size=particles.shape[0], replace=True, p=weights)
    return particles[indices], np.ones(particles.shape[0])/particles.shape[0]

# 主函数
if __name__ == '__main__':
    num_particles = 1000 # 粒子数
    landmarks = np.array([[2, 2], [8, 8]]) # 地标坐标
    sensor_std = 0.1 # 传感器噪声方差
    x_true = np.array([2, 2, np.pi/4]) # 真实初始位置
    z = np.array([np.sqrt((landmark[0] - x_true[0])**2 + (landmark[1] - x_true[1])**2) for landmark in landmarks]) # 测量结果
    particles, weights = initialize_particles(num_particles) # 初始化粒子
    for t in range(100):
        u = np.array([1, np.pi/10]) # 机器人运动控制量
        particles, weights = update_particles(particles, u, sensor_std) # 粒子更新
        if np.sum(weights**2)/np.sum(weights) < num_particles/2: # 如果权重方差过小，进行重采样
            particles, weights = resample_particles(particles, weights)
        x_est = np.average(particles, weights=weights, axis=0)
        print(f"t={t}, x_true={x_true}, x_est={x_est}")

上述代码实现了一个简单的二维机器人在已知两个地标的环境中进行定位的过程。您可以根据实际应用场景进行相应的修改和优化。

给我一个2-D 系统融合滤波代码示例

二维系统融合滤波，通常是指卡尔曼滤波(Kalman Filter)在多传感器数据融合中的应用。这里给出一个简单的2D（x, y）位置估计的粒子滤波（Particle Filter）代码示例，这个例子假设我们有来自多个传感器（如GPS、惯导等）的数据输入：

import numpy as np

class ParticleFilter:
    def __init__(self, num_particles=100, std_position=1.0, std_velocity=0.5):
        self.num_particles = num_particles
        self.position_particles = np.random.multivariate_normal([0, 0], [[std_position**2, 0], [0, std_position**2]], size=num_particles)
        self.velocity_particles = np.random.multivariate_normal([0, 0], [[std_velocity**2, 0], [0, std_velocity**2]], size=num_particles)
        self.weights = np.ones(num_particles) / num_particles

    def predict(self, dt, motion_model):
        # 运动模型：x_next = x_current + v*dt
        self.position_particles += self.velocity_particles * dt
        self.velocity_particles += motion_model(dt)

    def update(self, measurements, measurement_model):
        # 更新权重
        weights = np.zeros(self.num_particles)
        for i in range(self.num_particles):
            predicted_measurement = measurement_model(self.position_particles[i])
            likelihood = np.exp(-np.linalg.norm(measurements - predicted_measurement)**2 / 2)
            weights[i] = self.weights[i] * likelihood
        self.weights = weights / weights.sum()

    def get_estimated_position(self):
        return np.mean(self.position_particles, axis=0), np.cov(self.position_particles.T)


# 使用示例
motion_model = lambda dt: np.array([[1, dt, 0], [0, 1, dt], [0, 0, 1]])  # 假设匀速直线运动
measurement_model = lambda pos: pos + np.random.normal(size=(2,))  # 假设加噪声的测量

pf = ParticleFilter()
for _ in range(10):  # 模拟10次传感器读数更新
    pf.predict(1.0, motion_model)
    pf.update(np.random.randn(2), measurement_model)

estimated_position, covariance = pf.get_estimated_position()
print(f"Estimated position: {estimated_position}")

注意：这只是一个简化版本，实际应用中可能需要处理更多复杂的因素，如传感器不确定性、协方差矩阵更新等。