二维运动粒子滤波

对二维运动粒子滤波方法进行仿真研究。

参考了博主白巧克力亦唯心的微博，推导十分清晰详细，满满崇拜！http://blog.csdn.net/heyijia0327 

 过程中遇到两个问题：
1.采样方式的问题：在随机变量分布P(xk|xk-1)中采样效果不理想，范围较小，无法覆盖需要的取值范围，个人认为原因可能是观测量中没有与速度值直接相关的项，导致速度无法覆盖需要的范围。修改为在重采样的粒子中采样，达到更广泛的范围。

2.权值大小的问题：采样粒子对应的距离值，在随机变量距离的概率密度函数上，所取得概率密度值较小，解决方法为增大随机变量距离的方差，扩大随机变量的取值范围，即使得距离的概率密度的曲线变胖；而采样粒子对应的角度值，在随机变量角度的概率密度函数上，不同粒子所取得的概率密度值相差过小（如60度权重为1.2，90度为1.5），解决方法为改变方差变小，是概率密度函数变瘦，斜率变大，相同自变量对应的因变量差值变大。

顺便提一下编程需要模块：

1.系统的状态方程和观测方程标明（注释模块）

2.（1）初值选取（如初始状态，粒子数）/仿真条件确定（如真次数，时间间隔）

   （2）存储区

   （3）算法模块

3.绘图

粒子滤波流程：

1.取初始粒子

2.在重要性概率密度函数中进行重要性采样，得到预采样粒子

3.将每个粒子带入状态方程（预测），求得预测值

4.对每个预测值带入观测方程，与实际观测值做差，带入观测量的概率密度函数，求得对应概率密度，作为粒子权值

5.对粒子权值进行归一化

6.对粒子进行重采样

7.取重采样粒子的均值，得到状态估计值

贴代码：

%% 二维运动粒子滤波
clear all;
close all;
clc
%% 系统状态方程与量测方程  T=1
%    s_x(k)=s_x(k-1)+v_x(k-1)+noise_s_x(k-1)  x方向位置
%    v_x(k)=v_x(k-1)+noise_v_x(k-1)  x方向速度
%    s_y(k)=s_y(k-1)+v_y(k-1)+noise_s_y(k-1)  y方向位置
%    v_y(k)=v_y(k-1)+noise_v_y(k-1)  y方向速度

%    distance(k)=sqrt(s_x(k)^2+s_y(k)^2)+noise_distance(k)  测量距离
%    theta(k)=atan(s_y(k)/s_x(k))+noise_theta(k)  测量角度
%% 初始状态定义
%初始速度和位置
initail_s_x=480;
initail_v_x=40;
initail_s_y=400;
initail_v_y=-30;
%粒子数及仿真次数
N=300;%粒子数
M=200;%仿真次数
%噪声分布，设定系数，之后用randn从N~(0,1)正太分布中取样，设定的即为方差
noise_s_x=10;
noise_v_x=5;
noise_s_y=10;
noise_v_y=5;
noise_distance=20;
noise_theta=3/180*pi;

%% 存储空间定义-真实状态（位置/速度），粒子滤波状态（位置/速度），观测值（距离/角度）
s_x = zeros(1,M); %真实状态
v_x = zeros(1,M);
s_y = zeros(1,M);
v_y = zeros(1,M);
distance = zeros(1,M); %真实观测距离
theta = zeros(1,M); %真实观测角度
pf_s_x = zeros(1,M); %粒子滤波估计值
pf_v_x = zeros(1,M);
pf_s_y = zeros(1,M);
pf_v_y = zeros(1,M);
pre_pf_s_x=zeros(N,M); %预测采样粒子
pre_pf_v_x=zeros(N,M);
pre_pf_s_y=zeros(N,M);
pre_pf_v_y=zeros(N,M);
resample_pf_s_x = zeros(N,M); %重采样粒子
resample_pf_v_x = zeros(N,M);
resample_pf_s_y = zeros(N,M);
resample_pf_v_y = zeros(N,M);
distance_particle=zeros(N,M); %粒子预测观测距离
theta_particle=zeros(N,M); %粒子预测观测角度
weight_particle=zeros(N,M); %粒子权重
weight_particle1=zeros(N,M);
weight_particle2=zeros(N,M);

%% 真实状态
%赋初值
s_x(1)=initail_s_x;
v_x(1)=initail_v_x;
s_y(1)=initail_s_y;
v_y(1)=initail_v_y;
for k=2:M
    s_x(k)=s_x(k-1)+v_x(k-1)+noise_s_x*randn(1);
    v_x(k)=v_x(k-1)+noise_v_x*randn(1);
    s_y(k)=s_y(k-1)+v_y(k-1)+noise_s_y*randn(1);
    v_y(k)=v_y(k-1)+noise_v_y*randn(1);
end

%% 测量值
for k=1:M
    distance(k)=sqrt(s_x(k)^2+s_y(k)^2)+noise_distance*randn(1);
    theta(k)=atan(s_y(k)/s_x(k))+noise_theta*randn(1);
end

%% 粒子滤波
%赋初值
pf_s_x(1)=initail_s_x;
pf_v_x(1)=initail_v_x;
pf_s_y(1)=initail_s_y;
pf_v_y(1)=initail_v_y;

%初始粒子值选取
for n=1:N
    pre_pf_s_x(n,1)=pf_s_x(1)+noise_s_x*randn(1);
    pre_pf_v_x(n,1)=pf_v_x(1)+noise_v_x*randn(1);
    pre_pf_s_y(n,1)=pf_s_y(1)+noise_s_y*randn(1);
    pre_pf_v_y(n,1)=pf_v_y(1)+noise_v_y*randn(1);
    resample_pf_s_x(n,1)=initail_s_x+noise_s_x*randn(1);
    resample_pf_v_x(n,1)=initail_v_x+noise_v_x*randn(1);
    resample_pf_s_y(n,1)=initail_s_y+noise_s_y*randn(1);
    resample_pf_v_y(n,1)=initail_v_y+noise_v_y*randn(1);
end
for k=2:M
    for n=1:N
        
%采样方式1*****************************************************************************************************
%         pre_pf_s_x(n,k)=pf_s_x(k-1)+pf_v_x(k-1)+noise_s_x*randn(1);
%         pre_pf_v_x(n,k)=pf_v_x(k-1)+noise_v_x*randn(1);
%         pre_pf_s_y(n,k)=pf_s_y(k-1)+pf_v_y(k-1)+noise_s_y*randn(1);
%         pre_pf_v_y(n,k)=pf_v_y(k-1)+noise_v_y*randn(1);
%采样方式1*****************************************************************************************************

%采样方式2*****************************************************************************************************
        pre_pf_s_x(n,k)=resample_pf_s_x(n,k-1)+resample_pf_v_x(n,k-1)+noise_s_x*randn(1);
        pre_pf_v_x(n,k)=resample_pf_v_x(n,k-1)+noise_v_x*randn(1);
        pre_pf_s_y(n,k)=resample_pf_s_y(n,k-1)+resample_pf_v_y(n,k-1)+noise_s_y*randn(1);
        pre_pf_v_y(n,k)=resample_pf_v_y(n,k-1)+noise_v_y*randn(1);
%采样方式2*****************************************************************************************************
        
        distance_particle(n,k)=sqrt(pre_pf_s_x(n,k)^2+pre_pf_s_y(n,k)^2);
        err_distance = distance(k)-distance_particle(n,k);
        theta_particle(n,k)=atan(pre_pf_s_y(n,k)/pre_pf_s_x(n,k));
        err_theta = theta(k)-theta_particle(n,k);
%         weight_particle(n,k)=(1/sqrt(2*pi*noise_distance*50))*exp(-(err_distance)^2/(2*noise_distance*50));
        weight_particle1(n,k)=(1/sqrt(2*pi*noise_distance*50))*exp(-err_distance^2/(2*noise_distance*50)); 
        weight_particle2(n,k)=(1/sqrt(2*pi)/noise_theta)*exp(-err_theta^2/2/noise_theta^2);
        weight_particle(n,k)=weight_particle1(n,k)*weight_particle2(n,k)+1e-99;
%         weight_particle2(n,k)=(1/sqrt(2*pi*noise_theta))*exp(-err_theta^2/(2*noise_theta));
%         weight_particle1(n,k)=(1/sqrt(2*pi)/noise_distance)*exp(-err_distance^2/2/noise_distance);               
    end
    %权重归一化
    weight_particle(:,k)=weight_particle(:,k)/sum(weight_particle(:,k));
    %重采样
    part_weight=cumsum(weight_particle(:,k));%权重分区
%（1）************************************************************************************************************    
    ut(1)=rand(1)/N;
    kk = 1; %刚好<ut(n)的元素的下标
    hp = zeros(N,1);
    for n = 1:N
        ut(n)=ut(1)+(n-1)/N;
        while(part_weight(kk)<ut(n));
            kk = kk + 1;
        end;
        hp(n) = kk;
        weight_particle(n,k)=1/N;
    end;    
    for n = 1:N
        resample_pf_s_x(n,k) = pre_pf_s_x(hp(n),k);
        resample_pf_s_y(n,k) = pre_pf_s_y(hp(n),k);       % The new particles    
        resample_pf_v_x(n,k) = pre_pf_v_x(hp(n),k);
        resample_pf_v_y(n,k) = pre_pf_v_y(hp(n),k);
    end
%************************************************************************************************************

%（2）************************************************************************************************************
%     for n=1:N        
%         resample_pf_s_x(n,k)=pre_pf_s_x(find(rand<=part_weight,1),k); %rand从[0.1]区间的均匀分布上采样
%         resample_pf_v_x(n,k)=pre_pf_v_x(find(rand<=part_weight,1),k); %find 返回第一个满足要求元素的下标
%         resample_pf_s_y(n,k)=pre_pf_s_y(find(rand<=part_weight,1),k);
%         resample_pf_v_y(n,k)=pre_pf_v_y(find(rand<=part_weight,1),k);
%     end
%************************************************************************************************************
    pf_s_x(k)=mean(resample_pf_s_x(:,k));
    pf_v_x(k)=mean(resample_pf_v_x(:,k));
    pf_s_y(k)=mean(resample_pf_s_y(:,k));
    pf_v_y(k)=mean(resample_pf_v_y(:,k));
end
%% 绘图
t=1:M;
figure(1);%
clf
%plot(t,s_x,'-b',t,pf_s_x,'-.r');
plot(s_x,s_y,'-b',pf_s_x,pf_s_y,'-.r');
legend('actual state','pf');
xlabel('x state'); ylabel('y state');

figure(2);
clf
plot(t,s_x,'-b',t,pf_s_x,'-.r');
legend('actual state','pf');
xlabel('t'); ylabel('x state');

figure(3);
clf
plot(t,s_y,'-b',t,pf_s_y,'-.r');
legend('actual state','pf');
xlabel('t'); ylabel('y state');

figure(4);
clf
plot(t,v_x,'-b',t,pf_v_x,'-.r');
legend('actual state','pf');
xlabel('t'); ylabel('x speed');

figure(5);
clf
plot(t,v_y,'-b',t,pf_v_y,'-.r');
legend('actual state','pf');
xlabel('t'); ylabel('y speed');
%(1.采样方式问题，2.取值的概率分布问题)