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

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

function [estimated_state] = particle_filter(num_particles, state_dim, observation_dim, transition_fn, observation_fn, initial_state, observations, noise_std)
    weights = ones(num_particles, 1) / num_particles;
    particles = repmat(initial_state, num_particles, 1);
    for t = 1:length(observations)
        % Resampling
        if 1 / sum(weights .^ 2) < num_particles / 2
            indexes = randsample(num_particles, num_particles, true, weights);
            particles = particles(indexes, :);
            weights = ones(num_particles, 1) / num_particles;
        end
        % Particle filtering
        for i = 1:num_particles
            % State transition
            particles(i, :) = transition_fn(particles(i, :)) + normrnd(0, noise_std, 1, state_dim);
            % Observation update
            weights(i) = weights(i) * observation_fn(observations(t), particles(i, :), noise_std);
        end
        weights = weights / sum(weights);
    end
    estimated_state = weights' * particles;
end

% Example usage
num_particles = 1000;
state_dim = 2;
observation_dim = 1;
noise_std = 0.1;

initial_state = [0, 0];
observations = [1, 2, 3, 4, 5];

transition_fn = @(x) [x(1) + normrnd(0, noise_std), x(2) + normrnd(0, noise_std)];
observation_fn = @(y, x, noise_std) exp(-((y - x(1)) ^ 2) / (2 * noise_std ^ 2));

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

disp(['Estimated state: [' num2str(estimated_state) ']']);

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