随机输入噪声的粒子滤波代码

下面是一个简单的Python代码实现，用于演示随机输入噪声的粒子滤波：

import numpy as np

def particle_filter(num_particles, state_dim, observation_dim, transition_fn, observation_fn, initial_state, observations, noise_std):
    weights = np.ones(num_particles) / num_particles
    particles = np.tile(initial_state, (num_particles, 1))
    for t in range(len(observations)):
        # Resampling
        if 1 / np.sum(weights ** 2) < num_particles / 2:
            indexes = np.random.choice(num_particles, num_particles, p=weights)
            particles = particles[indexes]
            weights = np.ones(num_particles) / num_particles
        # Particle filtering
        for i in range(num_particles):
            # State transition
            particles[i] = transition_fn(particles[i]) + np.random.normal(0, noise_std, state_dim)
            # Observation update
            weights[i] *= observation_fn(observations[t], particles[i], noise_std)
        weights /= np.sum(weights)
    return np.dot(weights, particles)

# Example usage
num_particles = 1000
state_dim = 2
observation_dim = 1
noise_std = 0.1

def transition_fn(x):
    # Example state transition function
    return np.array([x[0] + np.random.normal(0, noise_std), x[1] + np.random.normal(0, noise_std)])

def observation_fn(y, x, noise_std):
    # Example observation function
    return np.exp(-((y - x[0]) ** 2) / (2 * noise_std ** 2))

initial_state = np.array([0, 0])
observations = np.array([1, 2, 3, 4, 5])

estimated_state = particle_filter(num_particles, state_dim, observation_dim, transition_fn, observation_fn, initial_state, observations, noise_std)

print('Estimated state:', estimated_state)

这个代码实现了一个简单的粒子滤波算法，用于估计一个具有两个状态维度和一个观测维度的非线性系统的状态。在此示例中，我们使用了一个简单的状态转移函数和观测函数，并为系统引入了高斯噪声。