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

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

数值算法/人工智能

开发平台：

Matlab

Ex321.asv：源码内容
  1. function Particle
  3. % Particle filter 
  5. x = 0; % 初始状态
  6. Q = 1; % 过程噪声协方差
  7. R = 1; % 测量噪声协方差
  8. tf = 50; % 仿真长度
  10. N = 100; % 粒子滤波器粒子数
  12. xhat = x;
  13. P = 2;
  14. xhatPart = x;
  16. % 初始化粒子过滤器
  17. N1=50;
  18. for i = 1 : N1
  19.     xpart(i) = x + sqrt(P) * randn;
  20. end
  22. xArr = [x];
  23. yArr = [x + sqrt(R) * randn];
  24. xhatArr = [x];
  25. PArr = [P];
  26. xhatPartArr = [xhatPart];
  28. close all;
  30. for k = 1 : tf
  31.     % 系统仿真
  32.     x = 3*x + sqrt(Q) * randn;%状态方程
  33.     y = x + sqrt(R) * randn;%观测方程
  34.      %  卡尔曼滤波
  35.     F = 3;
  36.     P = F * P * F' + Q;
  37.     H = xhat ;
  38.     K = P * H' * inv(H * P * H' + R);
  39.     xhat = 3 * xhat ;%预测
  40.     xhat = xhat + K * (y - xhat);%更新
  41.     P = (1 - K * H) * P;
  42.    
  43.     for i = 1 : N1
  44.         xpartminus(i) = 3 * xpart(i)  + sqrt(Q) * randn;
  45.         ypart = xpartminus(i);
  46.         vhat = y - ypart;%观测和预测的差
  47.         q(i) = (1 / sqrt(R) / sqrt(2*pi)) * exp(-vhat^2 / 2 / R);
  48.     end
  49.     %正常化的可能性，每个先验估计
  50.     qsum = sum(q);
  51.     for i = 1 : N1
  52.         q(i) = q(i) / qsum;%归一化权重
  53.     end
  54.     % 重采样
  55.     for i = 1 : N1
  56.         u = rand; % 均匀随机数介于0和1
  57.         qtempsum = 0;
  58.         for j = 1 : N1
  59.             qtempsum = qtempsum + q(j);
  60.             if qtempsum >= u
  61.                 xpart(i) = xpartminus(j);
  62.                 break;
  63.             end
  64.         end
  65.     end
  66.     xhatPart = mean(xpart);
  67.     xArr = [xArr x];
  68.     yArr = [yArr y];
  69.     xhatArr = [xhatArr xhat];
  70.     PArr = [PArr P];
  71.     xhatPartArr = [xhatPartArr xhatPart];
  72.     
  73.     x0=25;
  74.     xhat1 = x0;
  75.     xhatPart1 = x0;  
  76.     % 初始化粒子过滤器
  77. for i = 1 : N1
  78.     xpart1(i) = x0 + sqrt(P) * randn;
  79. end
  81. xArr1 = [x0];
  82. yArr1 = [x0 + sqrt(R) * randn];
  83. xhatArr1 = [x0];
  84. xhatPartArr1 = [xhatPart1];
  86. close all;
  87.     % 系统仿真
  88.     x1 =  -3*x0  + sqrt(Q) * randn;%状态方程
  89.     y1 = x1+ sqrt(R) * randn;%观测方程
  90.      %  卡尔曼滤波
  91.     F1 = -3 ;
  92.     P1 = F1 * P * F1' + Q;
  93.     H1 = xhat;
  94.     K1 = P1* H1' * inv(H1 * P1 * H1' + R);
  95.     xhat1 = -3 * xhat1 ;%预测
  96.     xhat1 = xhat1 + K1 * (y1 - xhat1);%更新
  97.     P1 = (1 - K1 * H1) * P1;
  98.    
  99.     for i = 1 : N
  100.         xpartminus1(i) =  xpart1(i)  + sqrt(Q) * randn;
  101.         ypart1 = xpartminus1(i);
  102.         vhat1 = y1 - ypart1;%观测和预测的差
  103.         vhat00=sqrt(y1.^2-ypart1.^2);
  104.         q1(i) = (1 / sqrt(R) / sqrt(2*pi)) * exp(-vhat1^2 / 2 / R);
  105.     end
  106.     %正常化的可能性，每个先验估计
  107.     qsum = sum(q1);
  108.     for i = 1 : N
  109.         q1(i) = q1(i) / qsum;%归一化权重
  110.     end
  111.     % 重采样
  112.     for i = 1 : N
  113.         u = rand; % 均匀随机数介于0和1
  114.         qtempsum = 0;
  115.         for j = 1 : N
  116.             qtempsum = qtempsum + q(j);
  117.             if qtempsum >= u
  118.                 xpart1(i) = xpartminus1(j);
  119.                 break;
  120.             end
  121.         end
  122.     xhatPart1 = mean(xpart1);
  123.     xArr1 = [xArr1 x1];
  124.     yArr1 = [yArr1 y1];
  125.     xhatArr1 = [xhatArr1 xhat1];
  126.     PArr = [PArr P];
  127.     xhatPartArr1 = [xhatPartArr1 xhatPart1];
  128.     
  129. t = 0 : tf;
  130.     if k == 20   
  131.     end
  132. end
  134. figure;
  135. plot(t, xArr, 'b.', t,xhatArr,'r',t, xhatPartArr, 'k-');
  136. xlabel('time step'); ylabel('state');
  137. legend('True state','KF', 'Particle filter estimate');