您现在的位置:首页 >> 技术文章 >> MATLAB技术 >> 内容

MATLAB代做|FPGA代做|Python代做--PSO粒子滤波

时间:2019-2-2 16:28:07 点击:

  核心提示:MATLAB代做|FPGA代做|Python代做--PSO粒子滤波...
function ParticleEx1

% Particle filter example, adapted from Gordon, Salmond, and Smith paper.

x = 0.1; % initial state
Q = 1; % process noise covariance
R = 1; % measurement noise covariance
tf = 50; % simulation length

N = 100; % number of particles in the particle filter

xhat = x;
P = 2;
xhatPart = x;

% Initialize the particle filter.
for i = 1 : N
    xpart(i) = x + sqrt(P) * randn;
end
jArr = [0];
xArr = [x];
yArr = [x^2 / 20 + sqrt(R) * randn];
xhatArr = [x];
PArr = [P];
xhatPartArr = [xhatPart];

close all;

for k = 1 : tf
    % System simulation
    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;%观测方程
    % Extended Kalman filter
    F = 0.5 + 25 * (1 - xhat^2) / (1 + xhat^2)^2;
    P = F * P * F' + Q;
    H = xhat / 10;
    K = P * H' * (H * P * H' + R)^(-1);
    xhat = 0.5 * xhat + 25 * xhat / (1 + xhat^2) + 8 * cos(1.2*(k-1));%预测
    xhat = xhat + K * (y - xhat^2 / 20);%更新
    P = (1 - K * H) * P;
    % Particle filter
    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);
    end
    % Normalize the likelihood of each a priori estimate.
    qsum = sum(q);
    for i = 1 : N
        q(i) = q(i) / qsum;%归一化权重
    end
    % Resample.重采样
    for i = 1 : N
        u = rand; % uniform random number between 0 and 1
        qtempsum = 0;
        for j = 1 : N
            qtempsum = qtempsum + q(j);
            if qtempsum >= u
                xpart(i) = xpartminus(j);
                if k == 20
                qArr=q;    
                jArr = [jArr j];
                end
                break;
            end
        end
    end
    % The particle filter estimate is the mean of the particles.
    xhatPart = mean(xpart);
    % Plot the estimated pdf's at a specific time.
    if k == 20
        % Particle filter pdf
        pdf = zeros(81,1);
        for m = -40 : 40
            for i = 1 : N
                if (m <= xpart(i)) && (xpart(i) < m+1)
                    pdf(m+41) = pdf(m+41) + 1;
                end
            end
        end
        figure;
        m = -40 : 40;
        plot(m, pdf / N, 'r');
        hold;
        title('Estimated pdf at k=20');
        disp(['min, max xpart(i) at k = 20: ', num2str(min(xpart)), ', ', num2str(max(xpart))]);
        % Kalman filter pdf
        pdf = (1 / sqrt(P) / sqrt(2*pi)) .* exp(-(m - xhat).^2 / 2 / P);
        plot(m, pdf, 'b');
        legend('Particle filter', 'Kalman filter');
        grid on;
    end
    % Save data in arrays for later plotting
    xArr = [xArr x];
    yArr = [yArr y];
    xhatArr = [xhatArr xhat];
    PArr = [PArr P];
    xhatPartArr = [xhatPartArr xhatPart];
end

t = 0 : tf;

%figure;
%plot(t, xArr);
%ylabel('true state');

figure;
plot(t, xArr, 'b.', t, xhatArr, 'g-.', t, xhatArr-2*sqrt(PArr), 'r:', t, xhatArr+2*sqrt(PArr), 'r:');
axis([0 tf -40 40]);
set(gca,'FontSize',12); set(gcf,'Color','White'); 
xlabel('time step'); ylabel('state');
legend('True state', 'EKF estimate', '95% confidence region'); 
grid on;

figure;
plot(t, xArr, 'b.', t, xhatPartArr, 'k-');
set(gca,'FontSize',12); set(gcf,'Color','White'); 
xlabel('time step'); ylabel('state');
legend('True state', 'Particle filter estimate'); 
grid on;

xhatRMS = sqrt((norm(xArr - xhatArr))^2 / tf);
xhatPartRMS = sqrt((norm(xArr - xhatPartArr))^2 / tf);
disp(['Kalman filter RMS error = ', num2str(xhatRMS)]);
disp(['Particle filter RMS error = ', num2str(xhatPartRMS)]);

%qArr
%tt=max(qArr)
%t=jArr 
%[m,n]=hist(jArr,100)
%m
%n
%sum(m)

联系:highspeedlogic

QQ :1224848052

微信:HuangL1121

邮箱:1224848052@qq.com

网站:http://www.mat7lab.com/

网站:http://www.hslogic.com/

微信扫一扫:

作者:-PSO粒子滤波 来源:-PSO粒子滤波
  • 您是如何找到本站的?
  • 百度搜索
  • Google搜索
  • 查阅资料过程中
  • 论坛发现
  • 百度贴吧发现
  • 朋友介绍
本站最新成功开发工程项目案例
相关文章
  • 没有相关文章
相关评论
发表我的评论
  • 大名:
  • 内容:
  • matlab代做|matlab专业代做|python代做|matlab淘宝代做|matlab代写(www.hslogic.com) © 2019 版权所有 All Rights Reserved.
  • Email:highspeed_logic@163.com 站长QQ: 1224848052

    专业代做/代写/承接、MATLAB、SIMULINK、FPGA项目、博士/硕士/本科毕业设计、课题设计、论文,毕业论文,Coursework、Eassy、Assignment