二维运动粒子滤波
对二维运动粒子滤波方法进行仿真研究。
参考了博主白巧克力亦唯心的微博，推导十分清晰详细，满满崇拜！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; %刚好