matlab粒子滤波算法

1. 救命啊！！关于改进粒子滤波算法问题

粒子滤波算法受到许多领域的研究人员的重视,该算法的主要思想是使用一个带有权值的粒子集合来表示系统的后验概率密度.在扩展卡尔曼滤波和Unscented卡尔曼滤波算法的基础上,该文提出一种新型粒子滤波算法.首先用Unscented卡尔曼滤波器产生系统的状态估计,然后用扩展卡尔曼滤波器重复这一过程并产生系统在k时刻的最终状态估计.在实验中,针对非线性程度不同的两种系统,分别采用5种粒子滤波算法进行实验

2. 粒子滤波算法中粒子分布图怎么画呀

matlab中画
如果每次迭代都画出来比较麻烦

3. 粒子滤波算法的具体流程是怎样的

粒子滤波(PF: Particle Filter)算法起源于20世纪50年代Poor Man's Monte Carlo问题的研究，但第一个具有应用性的粒子滤波算法于1993年由Gordon等提出（“A novel Approach to nonlinear/non-Gaussian Bayesian State estimation”）。它是利用粒子集来表示概率，可以用在任何形式的状态空间模型上。其核心思想是通过从后验概率中抽取的随机状态粒子来表示其分布情况，是一种顺序重要性采样法(Sequential Importance Sampling)。

粒子滤波的应用非常广泛，尤其是在目标跟踪（“A probabilistic framework for matching temporal trajectories”）等视觉任务方面。粒子滤波算法有许多不同的改进方式。针对不同的问题，PF算法被改造以适应更好的问题。本文主要侧重于目标跟踪方面的应用。以人脸跟踪为例，下图展示了粒子滤波的跟踪结果。下面介绍下粒子滤波的基本过程：初始化、概率转移、权重重计算和重采样四个阶段。

1.初始化阶段

跟踪区域初始化。在使用粒子滤波算法进行目标跟踪前需要选择要跟踪的目标物体。这个过程可以用人工划定方法和自动识别方法。使用人工的方法可以通过鼠标在图像区域标记出一个感兴趣矩形；使用自动的方法就是利用自动的目标检测技术，初步检测出图像中要跟踪物体的大致位置。以人脸跟踪为例，人工方法就是鼠标划定视频第一帧中人脸的区域；自动方法就是可以使用人脸检测算法检测出人脸的初始位置。

粒子初始化。对于本文人脸检测的示例，粒子就是图像中的矩形区域，主要由矩形中心(x,y)和宽高(w,h)四个变量表示。粒子初始化的步骤，就是在图像中选择指定数量的粒子（矩形），比如N=100个粒子。粒子初始化过程就是在图像中随机或指定方式放粒子。比如说，我们可以指定100个粒子初始状态和跟踪区域一致，即粒子参数和跟踪区域的(x,y,w,h)相等。

2.状态转移阶段

使用粒子滤波算法来对目标进行跟踪，即是通过前一次的先验概率来估算出当前环境下的后验概率密度，这个过程也是由粒子来完成的。具体来说，即根据上一帧中粒子的状态(x,y,w,h)t-1，来估计出本帧中各个粒子的状态(x,y,w,h)t。从上一帧图像的粒子状态转变为当前帧粒子的状态，这个变异过程就叫作转移（transmission）。粒子滤波的转移方程跟Kalman滤波的差不多：

上面的是状态转移方程，下面的为观测方程，wk和vk是高斯噪声。在本文示例中，xk=(x,y,w,h)t。变量x,y,w,h可以依据公式（1）分别更新。在不同的算法中，f采用的函数也不相同。如果xk=xk-1+wk，则状态转移方程其实是随机游走过程；如果xk=Axk-1+wk，状态转移方程则为一阶自回归方程；如果xk=A1xk-1+A2xk-2+wk，则状态转移方程为二阶自回归方程。

3.权重重计算阶段

转移阶段将上一帧中粒子的位置进行了转移，得到当前帧中新的位置。但并不是所有粒子的作用都有用。也就是有些粒子并不是跟踪区域所要所移动的位置。因此，在此阶段，粒子滤波算法将对每个粒子进行打分，将得分较低的粒子删除，将得分多的粒子生成更多的粒子（重采样过程完成）。具体打分的方法根据不同的需求会不同，例如人脸跟踪方法中使用距离作为衡量的标准。将每个粒子与跟踪区域进行相似度计算（在这里，分别提取粒子和跟踪区域的视觉特征进行计算，比如颜色直方图），使用相似度作为相应粒子的权重。每一个粒子都需要计算其权重，并且需要将其归一化。该阶段其实也是后验概率进行更新的过程。

4.重采样阶段

粒子滤波算法会淘汰权值低的粒子，让权值高的粒子来产生出更多的粒子，这就使得算法朝着权值高的地方收敛。假设有100个粒子，1号粒子的权重为0.02而2号粒子的权重为0.003。于是在重采样阶段，1号粒子生孩子的指标是0.02×100=2，2号粒子的指标是0.003×100=0.3，可以发现，1号粒子除了刚产生的粒子外还要再额外的产生一个粒子，而2号粒子就被铲除了。如此，最后得到的100个粒子即为所求，然后取个加权平均就得到了目标的状态值。

4. 有人做过用粒子滤波来跟踪目标的吗我想问一下，我怎么知道系统状态方程看很多matlab的仿真中用的状态方

clear;

% tic;
x = 0.1; % 初始状态
x_estimate = 1;%状态的估计
e_x_estimate = x_estimate; %EKF的初始估计
u_x_estimate = x_estimate; %UKF的初始估计
p_x_estimate = x_estimate; %PF的初始估计
Q = 10;%input('请输入过程噪声方差Q的值: '); % 过程状态协方差
R = 1;%input('请输入测量噪声方差R的值: '); % 测量噪声协方差
P =5;%初始估计方差
e_P = P; %UKF方差
u_P = P;%UKF方差
pf_P = P;%PF方差
tf = 50; % 模拟长度
x_array = [x];%真实值数组
e_x_estimate_array = [e_x_estimate];%EKF最优估计值数组
u_x_estimate_array = [u_x_estimate];%UKF最优估计值数组
p_x_estimate_array = [p_x_estimate];%PF最优估计值数组
u_k = 1; %微调参数
u_symmetry_number = 4; % 对称的点的个数
u_total_number = 2 * u_symmetry_number + 1; %总的采样点的个数
linear = 0.5;
N = 500; %粒子滤波的粒子数
close all;
%粒子滤波初始 N 个粒子
for i = 1 : N
p_xpart(i) = p_x_estimate + sqrt(pf_P) * randn;
end
for k = 1 : tf
% 模拟系统
x = linear * x + (25 * x / (1 + x^2)) + 8 * cos(1.2*(k-1)) + sqrt(Q) * randn; %状态值
y = (x^2 / 20) + sqrt(R) * randn; %观测值

%扩展卡尔曼滤波器
%进行估计 第一阶段的估计
e_x_estimate_1 = linear * e_x_estimate + 25 * e_x_estimate /(1+e_x_estimate^2) + 8 * cos(1.2*(k-1));
e_y_estimate = (e_x_estimate_1)^2/20; %这是根据k=1时估计值为1得到的观测值；只是这个由我估计得到的 第24行的y也是观测值 不过是由加了噪声的真实值得到的
%相关矩阵
e_A = linear + 25 * (1-e_x_estimate^2)/((1+e_x_estimate^2)^2);%传递矩阵
e_H = e_x_estimate_1/10; %观测矩阵
%估计的误差
e_p_estimate = e_A * e_P * e_A' + Q;
%扩展卡尔曼增益
e_K = e_p_estimate * e_H'/(e_H * e_p_estimate * e_H' + R);
%进行估计值的更新 第二阶段
e_x_estimate_2 = e_x_estimate_1 + e_K * (y - e_y_estimate);
%更新后的估计值的误差
e_p_estimate_update = e_p_estimate - e_K * e_H * e_p_estimate;
%进入下一次迭代的参数变化
e_P = e_p_estimate_update;
e_x_estimate = e_x_estimate_2;

% 粒子滤波器
% 粒子滤波器
for i = 1 : N
p_xpartminus(i) = 0.5 * p_xpart(i) + 25 * p_xpart(i) / (1 + p_xpart(i)^2) + 8 * cos(1.2*(k-1)) + sqrt(Q) * randn; %这个式子比下面一行的效果好
% xpartminus(i) = 0.5 * xpart(i) + 25 * xpart(i) / (1 + xpart(i)^2) + 8 * cos(1.2*(k-1));
p_ypart = p_xpartminus(i)^2 / 20; %预测值
p_vhat = y - p_ypart;% 观测和预测的差
p_q(i) = (1 / sqrt(R) / sqrt(2*pi)) * exp(-p_vhat^2 / 2 / R); %各个粒子的权值
end
% 平均每一个估计的可能性
p_qsum = sum(p_q);
for i = 1 : N
p_q(i) = p_q(i) / p_qsum;%各个粒子进行权值归一化
end
% 重采样 权重大的粒子多采点，权重小的粒子少采点, 相当于每一次都进行重采样；
for i = 1 : N
p_u = rand;
p_qtempsum = 0;
for j = 1 : N
p_qtempsum = p_qtempsum + p_q(j);
if p_qtempsum >= p_u
p_xpart(i) = p_xpartminus(j); %在这里 xpart(i) 实现循环赋值；终于找到了这里！！！
break;
end
end
end

p_x_estimate = mean(p_xpart);
% p_x_estimate = 0;
% for i = 1 : N
% p_x_estimate =p_x_estimate + p_q(i)*p_xpart(i);
% end

%不敏卡尔曼滤波器

%采样点的选取 存在x(i)
u_x_par = u_x_estimate;
for i = 2 : (u_symmetry_number+1)
u_x_par(i,:) = u_x_estimate + sqrt((u_symmetry_number+u_k) * u_P);
end
for i = (u_symmetry_number+2) : u_total_number
u_x_par(i,:) = u_x_estimate - sqrt((u_symmetry_number+u_k) * u_P);
end
%计算权值
u_w_1 = u_k/(u_symmetry_number+u_k);
u_w_N1 = 1/(2 * (u_symmetry_number+u_k));

%把这些粒子通过传递方程 得到下一个状态
for i = 1: u_total_number
u_x_par(i) = 0.5 * u_x_par(i) + 25 * u_x_par(i)/(1+u_x_par(i)^2) + 8 * cos(1.2*(k-1));
end
%传递后的均值和方差
u_x_next = u_w_1 * u_x_par(1);
for i = 2 : u_total_number
u_x_next = u_x_next + u_w_N1 * u_x_par(i);
end
u_p_next = Q + u_w_1 * (u_x_par(1)-u_x_next) * (u_x_par(1)-u_x_next)';
for i = 2 : u_total_number
u_p_next = u_p_next + u_w_N1 * (u_x_par(i)-u_x_next) * (u_x_par(i)-u_x_next)';
end

% %对传递后的均值和方差进行采样 产生粒子 存在y(i)
% u_y_2obser(1) = u_x_next;
% for i = 2 : (u_symmetry_number+1)
% u_y_2obser(i,:) = u_x_next + sqrt((u_symmetry_number+k) * u_p_next);
% end
% for i = (u_symmetry_number + 2) : u_total_number
% u_y_2obser(i,:) = u_x_next - sqrt((u_symmetry_number+u_k) * u_p_next);
% end
%另外存在y_2obser(i) 中；
for i = 1 :u_total_number
u_y_2obser(i,:) = u_x_par(i);
end

%通过观测方程 得到一系列的粒子
for i = 1: u_total_number
u_y_2obser(i) = u_y_2obser(i)^2/20;
end
%通过观测方程后的均值 y_obse
u_y_obse = u_w_1 * u_y_2obser(1);
for i = 2 : u_total_number
u_y_obse = u_y_obse + u_w_N1 * u_y_2obser(i);
end
%Pzz测量方差矩阵
u_pzz = R + u_w_1 * (u_y_2obser(1)-u_y_obse)*(u_y_2obser(1)-u_y_obse)';
for i = 2 : u_total_number
u_pzz = u_pzz + u_w_N1 * (u_y_2obser(i) - u_y_obse)*(u_y_2obser(i) - u_y_obse)';
end
%Pxz状态向量与测量值的协方差矩阵
u_pxz = u_w_1 * (u_x_par(1) - u_x_next)* (u_y_2obser(1)-u_y_obse)';
for i = 2 : u_total_number
u_pxz = u_pxz + u_w_N1 * (u_x_par(i) - u_x_next) * (u_y_2obser(i)- u_y_obse)';
end

%卡尔曼增益
u_K = u_pxz/u_pzz;

%估计量的更新
u_x_next_optimal = u_x_next + u_K * (y - u_y_obse);%第一步的估计值 + 修正值；
u_x_estimate = u_x_next_optimal;
%方差的更新
u_p_next_update = u_p_next - u_K * u_pzz * u_K';
u_P = u_p_next_update;

%进行画图程序
x_array = [x_array,x];
e_x_estimate_array = [e_x_estimate_array,e_x_estimate];
p_x_estimate_array = [p_x_estimate_array,p_x_estimate];
u_x_estimate_array = [u_x_estimate_array,u_x_estimate];

e_error(k,:) = abs(x_array(k)-e_x_estimate_array(k));
p_error(k,:) = abs(x_array(k)-p_x_estimate_array(k));
u_error(k,:) = abs(x_array(k)-u_x_estimate_array(k));

end
t = 0 : tf;
figure;
plot(t,x_array,'k.',t,e_x_estimate_array,'r-',t,p_x_estimate_array,'g--',t,u_x_estimate_array,'b:');
set(gca,'FontSize',10);
set(gcf,'color','White');
xlabel('时间步长');% lable --->label 我的神
ylabel('状态');
legend('真实值','EKF估计值','PF估计值','UKF估计值');
figure;
plot(t,x_array,'k.',t,p_x_estimate_array,'g--', t, p_x_estimate_array-1.96*sqrt(P), 'r:', t, p_x_estimate_array+1.96*sqrt(P), 'r:');
set(gca,'FontSize',10);
set(gcf,'color','White');
xlabel('时间步长');% lable --->label 我的神
ylabel('状态');
legend('真实值','PF估计值', '95% 置信区间');
%root mean square 平均值的平方根
e_xrms = sqrt((norm(x_array-e_x_estimate_array)^2)/tf);
disp(['EKF估计误差均方值=',num2str(e_xrms)]);
p_xrms = sqrt((norm(x_array-p_x_estimate_array)^2)/tf);
disp(['PF估计误差均方值=',num2str(p_xrms)]);
u_xrms = sqrt((norm(x_array-u_x_estimate_array)^2)/tf);
disp(['UKF估计误差均方值=',num2str(u_xrms)]);
% plot(t,e_error,'r-',t,p_error,'g--',t,u_error,'b:');
% legend('EKF估计值误差','PF估计值误差','UKF估计值误差');
t = 1 : tf;
figure;
plot(t,e_error,'r-',t,p_error,'g--',t,u_error,'b:');
set(gca,'FontSize',10);
set(gcf,'color','White');
xlabel('时间步长');% lable --->label 我的神
ylabel('状态');
legend('EKF估计值误差','PF估计值误差','UKF估计值误差');
% toc;

5. 如何将下面的离散化公式用粒子（群）滤波算法估计Qo和po（matlab程序实现）.

这个确实有难度啊

6. 粒子滤波算法是什么时间，由谁最先提出来的

我通俗解释一下,粒子滤波（PF）的应用大致这样：（其实目标跟踪的理论就是对状态向量的实时估值）

设有一堆样本,假设有N个,初始给他们同样的权值1/N.

这个系统状态转移方程,一般是非线性的,我们只需要知道怎么做才能把这时刻的状态值传播到下一个时刻.具体做法,N个样本值通过状态转移得下一时刻的样本预测值,包含过程噪声因素.d

系统还有一个非线性的观测方程,通过它得到真正的观测值Z.这时候,把N个样本预测值带进去获得Z‘.

根据Z’和Z相差的程度,决定对这个样本的可信程度,当然越接近的越好,然后把这些可信程度进行权值归一化.

重采样环节,把这些样本按照权值进行随机采样（权值越高的,当然越容易被抽中.比如说,下一时刻的值,有四个样本说等于1,有两个样本说等于1.5,那么有2/3概率认为等于1.这个解释起来真的有够复杂的,一般做起来200~300个样本获得的值都接近一样了,还要设个2/3n的阈值防止粒子匮乏,也就是防止所有样本得到相同的后验估计结果）,获得的值尽可能接近真实发生的情况.

循环2~5

7. 基于颜色的粒子滤波目标跟踪算法MATLAB仿真与改进

我这里有一个粒子群的完整范例：functionmain()clc;clearall;closeall;tic;%程序运行计时E0=0.001;%允许误差MaxNum=100;%粒子最大迭代次数narvs=1;%目标函数的自变量个数particlesize=30;%粒子群规模c1=2;%每个粒子的个体学习因子，也称为加速常数c2=2;%每个粒子的社会学习因子，也称为加速常数w=0.6;%惯性因子vmax=0.8;%粒子的最大飞翔速度x=-5+10*rand(particlesize,narvs);%粒子所在的位置v=2*rand(particlesize,narvs);%粒子的飞翔速度%用inline定义适应度函数以便将子函数文件与主程序文件放在一起，%目标函数是：y=1+(2.1*(1-x+2*x.^2).*exp(-x.^2/2))%inline命令定义适应度函数如下：fitness=inline('1/(1+(2.1*(1-x+2*x.^2).*exp(-x.^2/2)))','x');%inline定义的适应度函数会使程序运行速度大大降低fori=1:particlesizeforj=1:narvsf(i)=fitness(x(i,j));endendpersonalbest_x=x;personalbest_faval=f;[globalbest_favali]=min(personalbest_faval);globalbest_x=personalbest_x(i,:);k=1;whilek<=MaxNumfori=1:particlesizeforj=1:narvsf(i)=fitness(x(i,j));endiff(i)vmax;v(i,j)=vmax;elseifv(i,j)<-vmax;v(i,j)=-vmax;endendx(i,:)=x(i,:)+v(i,:);endifabs(globalbest_faval)<E0,break,endk=k+1;endValue1=1/globalbest_faval-1;Value1=num2str(Value1);%strcat指令可以实现字符的组合输出disp(strcat('themaximumvalue','=',Value1));%输出最大值所在的横坐标位置Value2=globalbest_x;Value2=num2str(Value2);disp(strcat('thecorrespondingcoordinate','=',Value2));x=-5:0.01:5;y=2.1*(1-x+2*x.^2).*exp(-x.^2/2);plot(x,y,'m-','linewidth',3);holdon;plot(globalbest_x,1/globalbest_faval-1,'kp','linewidth',4);legend('目标函数','搜索到的最大值');xlabel('x');ylabel('y');gridon;toc;

8. 谁有无迹kalman粒子滤波的matlab程序能否分享一下。

clear;clc;
A = [1.1269 -0.4940 0.1129
1.0000 0 0
0 1.0000 0];
B = [-0.3832
0.5919
0.5191];
C = [1 0 0];
Plant = ss(A,[B B],C,0,-1,'inputname',{'u' 'w'},'outputname','y');
Q = 1; R = 1;
[kalmf,L,P,M] = kalman(Plant,Q,R);

a = A;
b = [B B 0*B];
c = [C;C];
d = [0 0 0;0 0 1];
P = ss(a,b,c,d,-1,'inputname',{'u' 'w' 'v'},'outputname',{'y' 'yv'});
sys = parallel(P,kalmf,1,1,[],[])
% Close loop around input #4 and output #2
SimModel = feedback(sys,1,4,2,1)
% Delete yv from I/O list
SimModel = SimModel([1 3],[1 2 3])

t = [0:100]';
u = sin(t/5);

n = length(t)
randn('seed',0)
w = sqrt(Q)*randn(n,1);
v = sqrt(R)*randn(n,1);

[out,x] = lsim(SimModel,[w,v,u]);

y = out(:,1); % true response
ye = out(:,2); % filtered response
yv = y + v; % measured response

subplot(211), plot(t,y,'--',t,ye,'-'),
xlabel('No. of samples'), ylabel('Output')
title('Kalman filter response')
subplot(212), plot(t,y-yv,'-.',t,y-ye,'-'),
xlabel('No. of samples'), ylabel('Error')

MeasErr = y-yv;
MeasErrCov = sum(MeasErr.*MeasErr)/length(MeasErr);
EstErr = y-ye;
EstErrCov = sum(EstErr.*EstErr)/length(EstErr);