开通博客赚积分
发布资源赚积分
分类

源码开发语言/平台
当前位置：首页 > 源码/资料 > 数值算法/人工智能 > 查看源码
Ex1111.m
资源名称：work.rar　[点击查看]
上传用户：shigeng
上传日期：2017-01-30
资源大小：122k
文件大小：3k
源码类别：

数值算法/人工智能

开发平台：

Matlab

Ex1111.m：源码内容
  1. function Particle
  3. % Particle filter 
  5. y = 0; % 初始状态
  6. Q = 1; % 过程噪声协方差
  7. R = 1; % 测量噪声协方差
  8. tf = 50; % 仿真长度
  10. N = 100; % 粒子滤波器粒子数
  12. yhat = y;
  13. P = 2;
  14. yhatPart = y;
  16. % 初始化粒子过滤器
  17. for i = 1 : N
  18.     ypart(i) = y + sqrt(P) * randn;
  19. end
  21. yArr = [y];
  22. rArr = [y + sqrt(R) * randn];
  23. yhatArr = [y];
  24. PArr = [P];
  25. yhatPartArr = [yhatPart];
  27. close all;
  29. for k = 1 : tf
  30.     % 系统仿真
  31. x1=0:10:900;
  32. y1=sqrt(-(x1-450).^2+450^2)+ sqrt(Q) * randn;%状态方程
  33. x2=900:10:1800;
  34. y2=-sqrt(-(x2-1350).^2+450^2)+ sqrt(Q) * randn;%状态方程
  35. x3=1800:2500;
  36. y3=sqrt(Q) * randn;%状态方程
  37.    r = y1 + sqrt(R) * randn;%观测方程
  38.  
  39.    
  40.      %  卡尔曼滤波
  41.    x1=0:10:900;
  42.    F1=(450-x1)/sqrt(-(x1-450).^2+450^2);
  43.    P1 = F1 * P * F1' + Q;
  44.    H1 = yhat / 10;
  45.    K1 = P1 * H1' * inv(H1 * P1 * H1' + R);
  46.    yhat1 = sqrt(-(yhat-450).^2+450^2);%预测
  47.    yhat1 = yhat1 + K1 * (r - yhat);%更新
  48.    P1 = (1 - K1 * H1) * P1;
  49.    
  50.    x2=900:10:1800;
  51.    F2=(x2-1350)/sqrt(-(x2-1350).^2+450^2);
  52.    P2 = F2 * P * F2' + Q;
  53.    H2 = yhat / 10;
  54.    K2 = P2 * H2' * inv(H2 * P2 * H2' + R);
  55.    yhat2 = -sqrt(-(yhat-1350).^2+450^2);%预测
  56.    yhat2 = yhat2 + K2 * (r - yhat);%更新
  57.    P2 = (1 - K2 * H2) * P2;
  58.     
  59.    x3=1800:2500;
  60.    F3=1;
  61.    P3 = F3 * P * F3' + Q;
  62.    H3 = yhat / 10;
  63.    K3 = P3 * H3' * inv(H3 * P3 * H3' + R);
  64.    yhat3 = 0;%预测
  65.    yhat3 = yhat3 + K3 * (r - yhat);%更新
  66.    P3 = (1 - K3 * H3) * P3;
  67.  end
  68. end   
  69.     for i = 1 : N
  70.        x1=0:10:900;
  71.        y1partminus(i)=sqrt(-(ypart(i)-450).^2+450^2)+ sqrt(Q) * randn;
  72.        x2=900:10:1800;
  73.        y2partminus(i)=-sqrt(-(ypart(i)-1350).^2+450^2)+ sqrt(Q) * randn;
  74.        x3=1800:2500;
  75.        y3partminus(i)=sqrt(Q) * randn;
  76.       end
  77.         r1part = y1partminus(i);
  78.         r2part = y2partminus(i);
  79.         r3part = y3partminus(i);
  80.         
  81.         vhat1 = r - r1part;%观测和预测的差
  82.         vhat2 = r - r2part;
  83.         vhat3 = r - r3part;
  84.         vhat = mean((vhat1+vhat2+vhat3)/3)
  85.         q(i) = (1 / sqrt(R) / sqrt(2*pi)) * exp(-vhat^2 / 2 / R);
  86.     end
  87.     %正常化的可能性，每个先验估计
  88.     qsum = sum(q);
  89.     for i = 1 : N
  90.         q(i) = q(i) / qsum;%归一化权重
  91.     end
  92.     % 重采样
  93.     for i = 1 : N
  94.         u = rand; % 均匀随机数介于0和1
  95.         qtempsum = 0;
  96.         for j = 1 : N
  97.             qtempsum = qtempsum + q(j);
  98.             if qtempsum >= u
  99.                 xpart(i) = xpartminus(j);
  100.                 break;
  101.             end
  102.         end
  103.     end
  104.     yhatPart = mean(ypart);
  105.     yArr = [yArr y];
  106.     rArr = [rArr r];
  107.     yhatArr1 = [y1];yhatArr2 = [y2];yhatArr3 = [y3];
  108.     yhatArr1 = [yhatArr1 yhat1];
  109.     yhatArr2 = [yhatArr2 yhat2];
  110.     yhatArr3 = [yhatArr3 yhat3];
  111.     PArr = [PArr P];
  112.     yhatPartArr = [yhatPartArr yhatPart];
  113.    
  114.     
  115.  t = 0 : tf;
  116.     if k == 20   
  117.     end
  118. end
  120. figure;
  121. plot(t, yArr, 'b.', t,yhatArr1,'r',t,yhatArr2,'r',t,yhatArr3,'r',t, yhatPartArr, 'k-');
  122. xlabel('time step'); ylabel('state');
  123. legend('True state','KF', 'Particle filter estimate');