粒子滤波用于无人机跟踪小车的代码

这里是一个使用粒子滤波算法实现无人机跟踪小车的 Python 代码示例：

import numpy as np
import matplotlib.pyplot as plt

# 粒子滤波器类
class ParticleFilter:
    def __init__(self, num_particles, init_pose, motion_model, observation_model):
        self.num_particles = num_particles
        self.motion_model = motion_model
        self.observation_model = observation_model
        self.particles = np.tile(init_pose, (num_particles, 1))
        self.weights = np.ones(num_particles) / num_particles
        
    def predict(self, u):
        # 根据运动模型进行粒子预测
        self.particles = self.motion_model(self.particles, u)
        
    def update(self, z):
        # 根据观测模型进行粒子权重更新
        self.weights *= self.observation_model(z, self.particles)
        self.weights /= np.sum(self.weights)
        
    def resample(self):
        # 重采样
        new_particles = np.zeros_like(self.particles)
        cumulative_weights = np.cumsum(self.weights)
        step = cumulative_weights[-1] / self.num_particles
        r = np.random.uniform(0, step)
        j = 0
        for i in range(self.num_particles):
            while r > cumulative_weights[j]:
                j += 1
            new_particles[i] = self.particles[j]
            r += step
        self.particles = new_particles
        self.weights = np.ones(self.num_particles) / self.num_particles
        
    def get_pose(self):
        # 计算粒子加权平均值，得到估计的位置
        return np.average(self.particles, weights=self.weights, axis=0)

# 运动模型
def motion_model(particles, u):
    # 为每个粒子添加高斯噪声
    std = 0.1
    noise = np.random.normal(0, std, particles.shape)
    particles = particles + u + noise
    return particles

# 观测模型
def observation_model(z, particles):
    # 计算每个粒子的权重
    std = 0.1
    weights = np.exp(-np.sum((particles - z)**2, axis=1) / (2*std**2))
    return weights

# 初始化粒子滤波器
init_pose = np.array([0, 0])
pf = ParticleFilter(num_particles=100, init_pose=init_pose, motion_model=motion_model, observation_model=observation_model)

# 生成轨迹和观测数据
dt = 0.1
T = 10
u = np.array([1, 0])
x = np.zeros((T, 2))
z = np.zeros((T, 2))
for t in range(T):
    x[t] = init_pose + t*dt*u
    z[t] = x[t] + np.random.normal(0, 0.1, 2)

# 运行粒子滤波
for t in range(T):
    pf.predict(u)
    pf.update(z[t])
    pf.resample()
    print("时刻{}，估计位置为{}".format(t, pf.get_pose()))

# 绘制结果
plt.plot(x[:,0], x[:,1], 'r-', label='True Trajectory')
plt.scatter(z[:,0], z[:,1], color='b', marker='o', label='Observation')
plt.scatter(pf.particles[:,0], pf.particles[:,1], color='g', marker='x', label='Particles')
plt.scatter(pf.get_pose()[0], pf.get_pose()[1], color='m', marker='*', label='Estimation')
plt.legend()
plt.show()

在这个示例中，我们模拟了一个小车沿着一条直线行驶的场景，并使用无人机上的相机观测小车的位置。运动模型和观测模型都采用高斯噪声模型，粒子数为100。在每个时间步中，我们先根据运动模型对粒子进行预测，然后根据观测模型对粒子的权重进行更新，最后进行重采样得到下一时刻的粒子集合和权重。最终，我们计算粒子的加权平均值作为估计的小车位置，并绘制了真实轨迹、观测数据、粒子集合和估计位置的图像。