zoukankan      html  css  js  c++  java
  • 二维粒子滤波纯代码

    % 参数设置
    N = 100;   %粒子总数
    Q = 5;      %过程噪声
    R = 5;      %测量噪声
    T = 20;     %测量时间
    theta = pi/T;       %旋转角度
    distance = 80/T;    %每次走的距离
    WorldSize = 100;    %世界大小
    X = zeros(2, T);    %存储系统状态
    Z = zeros(2, T);    %存储系统的观测状态
    P = zeros(2, N);    %建立粒子群
    PCenter = zeros(2, T);  %所有粒子的中心位置
    w = zeros(N, 1);         %每个粒子的权重
    err = zeros(1,T);     %误差
    X(:, 1) = [50; 20];     %初始系统状态
    %wgn(m,n,p)产生一个m行n列的高斯白噪声的矩阵,p以dBW为单位指定输出噪声的强度。
    Z(:, 1) = [50; 20] + wgn(2, 1, 10*log10(R));    %初始系统的观测状态
    
    %初始化粒子群
    for i = 1 : N
        P(:, i) = [WorldSize*rand; WorldSize*rand];%在worldSize区域内随机生成N个粒子
        dist = norm(P(:, i)-Z(:, 1));     %与测量位置相差的距离,用于估算该粒子的权重
        %由于上面已经随机生成了N个粒子,现在将其与真实的测量值z进行比较,越接近则权重越大,或者说差值越小权重越大
        %这里的权重计算是关于p(z/x)的分布,即观测方程的分布,假设观测噪声满足高斯分布,那么w(i)=p(z/x)
        w(i) = (1 / sqrt(R) / sqrt(2 * pi)) * exp(-(dist)^2 / 2 / R);   %求权重
    end
    PCenter(:, 1) = sum(P, 2) / N;      %所有粒子的几何中心位置
    
    %%
    err(1) = norm(X(:, 1) - PCenter(:, 1));     %粒子几何中心与系统真实状态的误差
    figure(1);
    %利用set(gca,'propertyname','propertyvalue'......)命令可以调整图形的坐标属性。
    %propertyname和property value分别为坐标属性名称(不区分字母大小写)及对应的属性值。
    set(gca,'FontSize',10);
    hold on%使当前及图形保持而不被刷新,准备接受此后将绘制的新曲线
    plot(X(1, 1), X(2, 1), 'r.', 'markersize',30)   %系统状态位置
    %axis[xmin,xmax,ymin,ymax]设定坐标范围,必须满足xmin<xmax,后面同理
    axis([0 100 0 100]);
    plot(P(1, :), P(2, :), 'kx', 'markersize',5);   %绘出各个粒子位置
    plot(PCenter(1, 1), PCenter(2, 1), 'b.', 'markersize',25); %所有粒子的几何中心位置
    %图形标注函数legend(string1,string2,string3...),在当前图中添加图例
    
    legend('True State', 'Particles', 'The Center of Particles');
    %添加图形标题命令title('string'),在当前坐标系的顶部加一个文本串string,作为该图形的标题
    title('Initial State');
    %使当前轴及图形不再具备不被刷新的性质
    hold off
    
    %%
    %开始运动
    for k = 2 : T
    
        %模拟一个弧线运动的状态
        %wgn(m,n,p)产生一个m行n列的高斯白噪声的矩阵,p以dBW为单位指定输出噪声的强度。
        X(:, k) = X(:, k-1) + distance * [(-cos(k * theta)); sin(k * theta)] + wgn(2, 1, 10*log10(Q));     %状态方程
        Z(:, k) = X(:, k) + wgn(2, 1, 10*log10(R));     %观测方程 
    
        %粒子滤波
        %预测
        for i = 1 : N
            %将之前生成的粒子带入到状态方程,进行下一步状态的预测
            P(:, i) = P(:, i) + distance * [-cos(k * theta); sin(k * theta)] + wgn(2, 1, 10*log10(Q));
            dist = norm(P(:, i)-Z(:, k));     %与测量位置相差的距离,用于估算该粒子的权重
            %由于上面已经随机生成了N个粒子,现在将其与真实的测量值z进行比较,越接近则权重越大,或者说差值越小权重越大
            %这里的权重计算是关于p(z/x)的分布,即观测方程的分布,假设观测噪声满足高斯分布,那么w(i)=p(z/x)
            w(i) = (1 / sqrt(R) / sqrt(2 * pi)) * exp(-(dist)^2 / 2 / R);   %求权重
        end
    %归一化权重
        wsum = sum(w);
        for i = 1 : N
            w(i) = w(i) / wsum;
        end
    
        %重采样(更新)
        for i = 1 : N
            wmax = 2 * max(w) * rand;  %另一种重采样规则,获取权重当中的最大值与均匀分布在0-1之间数的乘积再乘以2作为后面判断选出大权重粒子的依据
            %randi()函数生成均匀分布的伪随机整数,范围为imin--imax,如果没指定imin,则默认为1
            %r = randi([imin,imax],...)返回一个在[imin,imax]范围内的伪随机整数
            index = randi(N, 1);
            %在这里随机产生N个粒子当中的某个粒子,然后选择该粒子的权值,判断是否是大的粒子权重,是的话就把该粒子选出来
            while(wmax > w(index))
                %使vmax递减,不然的话单个粒子的权重是不可能大于vmax的值的
                wmax = wmax - w(index);
                index = index + 1;
                %如果从某个随机的粒子开始找,没找到从该粒子开始到最后的粒子权值总和大于vmax,那么就重新从第一个粒子开始查找
                if index > N
                    index = 1;
                end          
            end
            P(:, i) = P(:, index);     %从上面的index中得到新粒子
        end
        %sum(X,2)表示把X按行求和;如果是sum(X),那就是按列求和
        PCenter(:, k) = sum(P, 2) / N;      %所有粒子的几何中心位置
    
        %计算误差
        err(k) = norm(X(:, k) - PCenter(:, k));     %粒子几何中心与系统真实状态的误差
    
        figure(2);
        set(gca,'FontSize',12);
        %clf; 用来清除图形的命令。一般在画图之前用
        clf;
        hold on
        plot(X(1, k), X(2, k), 'r.', 'markersize',50);  %系统状态位置
        axis([0 100 0 100]);
        plot(P(1, :), P(2, :), 'k.', 'markersize',5);   %各个粒子位置
        plot(PCenter(1, k), PCenter(2, k), 'b.', 'markersize',25); %所有粒子的中心位置
        legend('True State', 'Particle', 'The Center of Particles');
        hold off
        pause(0.1);
    end
    
    %%
    figure(3);
    set(gca,'FontSize',12);
    plot(X(1,:), X(2,:), 'r', Z(1,:), Z(2,:), 'g', PCenter(1,:), PCenter(2,:), 'b-');
    axis([0 100 0 100]);
    legend('True State', 'Measurement', 'Particle Filter');
    xlabel('x', 'FontSize', 20); ylabel('y', 'FontSize', 20);
    
    %%
    figure(4);
    set(gca,'FontSize',12);
    plot(err,'.-');
    xlabel('t', 'FontSize', 20);
    title('The err');
  • 相关阅读:
    虚函数******
    C++ 中的运算符重载********
    继承
    静态成员和成员函数
    类指针
    内联函数
    k8基础架构
    cobbler自动装机
    ens33-eth0
    PXE-自动装机配置
  • 原文地址:https://www.cnblogs.com/gary-guo/p/6806091.html
Copyright © 2011-2022 走看看