粒子滤波MATLAB源代码示例与状态估计

x = 0; % 初始状态 
R = input('请输入过程噪声方差R的值: ');; % 测量噪声协方差  
Q = input('请输入观测噪声方差Q的值: '); % 过程状态协方差 
tf = 100; % 模拟长度 
N = 100; % 粒子滤波器中的粒子个数 
xhat = x; 
P = 2; 
xhatPart = x; 
  
%粒子滤波器初期 
for i = 1 : N 
    xpart(i) = x + sqrt(P) * randn; 
end 
xArr = [x]; 
yArr = [x^2 / 20 + sqrt(R) * randn]; 
xhatArr = [x]; 
PArr = [P]; 
xhatPartArr = [xhatPart]; 
close all; 
  
for k = 1 : tf 
    % 模拟系统 
    x = 0.5 * x + 25 * x / (1 + x^2) + 8 * cos(1.2*(k-1)) + sqrt(Q) * randn; 
    y = x^2 / 20 + sqrt(R) * randn; 
   % 粒子滤波器 
    for i = 1 : N 
        xpartminus(i) = 0.5 * xpart(i) + 25 * xpart(i) / (1 + xpart(i)^2) + 8 * cos(1.2*(k-1)) + sqrt(Q) * randn; 
        ypart = xpartminus(i)^2 / 20; 
        vhat = y - ypart;% 观测和预测的差 
        q(i) = (1 / sqrt(R) / sqrt(2*pi)) * exp(-vhat^2 / 2 / R);