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

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

数值算法/人工智能

开发平台：

Matlab

Ex3210.asv：源码内容
  1. function Particle
  3. % Particle filter 
  5. x = 0; % 初始状态
  6. Q = 1; % 过程噪声协方差
  7. R = 1; % 测量噪声协方差
  8. tf = 180; % 仿真长度
  10. N = 100; % 粒子滤波器粒子数
  12. xhat = x;
  13. P = 2;
  14. xhatPart = x;
  16. % 初始化粒子过滤器
  17. for i = 1 : N
  18.     xpart(i) = x + sqrt(P) * randn;
  19. end
  21. xArr = [x];
  22. yArr = [x + sqrt(R) * randn];
  23. xhatArr = [x];
  24. PArr = [P];
  25. xhatPartArr = [xhatPart];
  27. close all;
  29. for k = 1 : tf
  30.     % 系统仿真
  31.     t=0:180;
  32.     t1=20000/300;
  33.     t2=t1+200*pi/20;
  34.     if  t>=0&&t<=t1
  35.         x=0;
  36. elseif t>t1&&t<=t2
  37.     x=sqrt(562.5^2-(x+4500)^2)+ sqrt(Q) * randn;
  38. else
  39.     x=-9000;
  40. end
  41.     y = x + sqrt(R) * randn;%观测方程
  42.      %  卡尔曼滤波
  43.      if t>t1&&t<=t2
  44.     F = (x+4500)/sqrt(562.5^2-(x+4500)^2);
  45.     P = F * P * F' + Q;
  46.     H = xhat / 10;
  47.     K = P * H' * inv(H * P * H' + R);
  48.     xhat = sqrt(562.5^2-(xhat+4500)^2);%预测
  49.     xhat = xhat + K * (y - xhat);%更新
  50.     P = (1 - K * H) * P;
  51. else F=0;
  52.     P = F * P * F' + Q;
  53.     H = xhat / 10;
  54.     K = P * H' * inv(H * P * H' + R);
  55.     xhat = sqrt(562.5^2-(xhat+4500)^2);%预测
  56.     xhat = xhat + K * (y - xhat);%更新
  57.     P = (1 - K * H) * P;
  58.    
  59.     for i = 1 : N
  60.         xpartminus(i) = sqrt(562.5^2-(xpartminus+4500)^2) + sqrt(Q) * randn;
  61.         ypart = xpartminus(i);
  62.         vhat = y - ypart;%观测和预测的差
  63.         q(i) = (1 / sqrt(R) / sqrt(2*pi)) * exp(-vhat^2 / 2 / R);
  64.     end
  65.     %正常化的可能性，每个先验估计
  66.     qsum = sum(q);
  67.     for i = 1 : N
  68.         q(i) = q(i) / qsum;%归一化权重
  69.     end
  70.     % 重采样
  71.     for i = 1 : N
  72.         u = rand; % 均匀随机数介于0和1
  73.         qtempsum = 0;
  74.         for j = 1 : N
  75.             qtempsum = qtempsum + q(j);
  76.             if qtempsum >= u
  77.                 xpart(i) = xpartminus(j);
  78.                 break;
  79.             end
  80.         end
  81.     end
  82.     xhatPart = mean(xpart);
  83.     xArr = [xArr x];
  84.     yArr = [yArr y];
  85.     xhatArr = [xhatArr xhat];
  86.     PArr = [PArr P];
  87.     xhatPartArr = [xhatPartArr xhatPart];
  88.     
  89. t = 0 : tf;
  90.     if k == 20   
  91.     end
  92. end
  94. figure;
  95. plot(t, xArr, 'b.', t,xhatArr,'r',t, xhatPartArr, 'k-');
  96. xlabel('time step'); ylabel('state');
  97. legend('True state','KF', 'Particle filter estimate');