随机输入粒子滤波算法matlab代码

以下是随机输入粒子滤波（RPF）的 MATLAB 代码实现：

function x_hat = rpf(y, f, h, Q, R, N, M)
% 随机输入粒子滤波算法
% Args:
%     y: 观测序列
%     f: 状态转移函数
%     h: 观测函数
%     Q: 状态转移噪声协方差矩阵
%     R: 观测噪声协方差矩阵
%     N: 粒子数
%     M: 随机输入噪声数
% Returns:
%     x_hat: 状态估计值

% 初始化粒子的权重和状态
w = ones(N, M) / (N * M);
x = zeros(N, M);

% 生成初试粒子
x(:, 1) = randn(N, 1);

% 递推计算粒子权重和状态
for i = 2:length(y)
    % 重采样
    idx = randsample(N, N, true, w(:, i-1));
    x(:, i) = x(idx, i-1);
    
    % 添加随机输入噪声
    xi = randn(N, M) .* sqrt(Q);
    x(:, i) = x(:, i) + xi(:, i);
    
    % 计算粒子的权重
    for j = 1:N
        w(j, i) = w(idx(j), i-1) * exp(-0.5 * (y(i) - h(x(j, i)))^2 / R);
    end
    
    % 归一化权重
    w(:, i) = w(:, i) / sum(w(:, i));
end

% 计算状态估计值
x_hat = sum(w .* x, 1);

end

其中，`y`为观测序列，`f`为状态转移函数，`h`为观测函数，`Q`为状态转移噪声协方差矩阵，`R`为观测噪声协方差矩阵，`N`为粒子数，`M`为随机输入噪声数。函数返回状态估计值 `x_hat`。