[转]粒子滤波的Matlab仿真算法实现
2013-01-30 23:59
465 查看
粒子滤波算法源于Monte carlo的思想,即以某事件出现的频率来指代该事件的概率。因此在滤波过程中,需要用到概率如P(x)的地方,一概对变量x采样,以大量采样的分布近似来表示P(x)。因此,采用此一思想,在滤波过程中粒子滤波可以处理任意形式的概率,而不像Kalman滤波只能处理高斯分布的概率问题。
对任意如下的状态方程:
\[x(t)=f[x(t-1),u(t),w(t)]\]
\[y(t)=h[x(t),e(t)]\]
其中的x(t)为t时刻状态,u(t)为控制量,w(t) 和e(t)分别为模型噪声和观测噪声。(1)当然是状态转移方程,(2)是观测方程。那么对于这么一个问题粒子滤波怎么来从观测y(t),和x(t-1),u(t) 滤出真实状态x(t)呢?
看看滤波的预估阶段:粒子滤波首先根据x(t-1) 和他的概率分布生成大量的采样,这些采样就称之为粒子。那么这些采样在状态空间中的分布实际上就是x(t-1) 的概率分布了。好,接下来依据状态转移方程加上控制量可以对每一粒子得到一个预测粒子。所有的预测粒子就代表了涉及哪些参数化的东西。
进入校正阶段来:有了预测粒子,当然不是所有的预测粒子都能得到我们的时间观测值y,越是接近真实状态的粒子,当然获得越有可能获得观测值y。于是对所有的粒子得有个评价了,这个评价就是一个条件概率P(y|xi),直白的说,这个条件概率代表了假设真实状态x(t)取第i个粒子xi时获得观测y的概率。令这个条件概率为第i个粒子的权重。对所有粒子都进行这么一个评价,那么越有可能获得观测y的粒子,当然获得的权重越高。好了预测信息融合在粒子的分布中,观测信息又融合在了每一粒子的权重中。
最后采用重采样算法,去除低权值的粒子,复制高权值的粒子。所得当然是需要的真实状态x(t)了,而这些重采样后的粒子,就代表了真实状态的概率分布了。
下一轮滤波,再将重采样过后的粒子集输入到状态转移方程中,直接就能够获得预测粒子了。
初始状态的问题:可以认为x(0)在全状态空间内平均分布。于是初始采样就平均分布在整个状态空间中。然后将所有采样输入状态转移方程,得到预测粒子。再评价下所有预测粒子的权重,当然我们在整个状态空间中只有部分粒子能够获的高权值。重采样算法去除低权值的,将下一轮滤波的考虑重点缩小到高权值粒子附近。
下面是我最后改写和精简的一个粒子滤波Matlab算法。
对任意如下的状态方程:
\[x(t)=f[x(t-1),u(t),w(t)]\]
\[y(t)=h[x(t),e(t)]\]
其中的x(t)为t时刻状态,u(t)为控制量,w(t) 和e(t)分别为模型噪声和观测噪声。(1)当然是状态转移方程,(2)是观测方程。那么对于这么一个问题粒子滤波怎么来从观测y(t),和x(t-1),u(t) 滤出真实状态x(t)呢?
看看滤波的预估阶段:粒子滤波首先根据x(t-1) 和他的概率分布生成大量的采样,这些采样就称之为粒子。那么这些采样在状态空间中的分布实际上就是x(t-1) 的概率分布了。好,接下来依据状态转移方程加上控制量可以对每一粒子得到一个预测粒子。所有的预测粒子就代表了涉及哪些参数化的东西。
进入校正阶段来:有了预测粒子,当然不是所有的预测粒子都能得到我们的时间观测值y,越是接近真实状态的粒子,当然获得越有可能获得观测值y。于是对所有的粒子得有个评价了,这个评价就是一个条件概率P(y|xi),直白的说,这个条件概率代表了假设真实状态x(t)取第i个粒子xi时获得观测y的概率。令这个条件概率为第i个粒子的权重。对所有粒子都进行这么一个评价,那么越有可能获得观测y的粒子,当然获得的权重越高。好了预测信息融合在粒子的分布中,观测信息又融合在了每一粒子的权重中。
最后采用重采样算法,去除低权值的粒子,复制高权值的粒子。所得当然是需要的真实状态x(t)了,而这些重采样后的粒子,就代表了真实状态的概率分布了。
下一轮滤波,再将重采样过后的粒子集输入到状态转移方程中,直接就能够获得预测粒子了。
初始状态的问题:可以认为x(0)在全状态空间内平均分布。于是初始采样就平均分布在整个状态空间中。然后将所有采样输入状态转移方程,得到预测粒子。再评价下所有预测粒子的权重,当然我们在整个状态空间中只有部分粒子能够获的高权值。重采样算法去除低权值的,将下一轮滤波的考虑重点缩小到高权值粒子附近。
下面是我最后改写和精简的一个粒子滤波Matlab算法。
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. 初始化粒子滤波,xpart值用来在不同时刻生成粒子 for i = 1 : N xpart(i) = x + sqrt(P) * randn; end xArr = [x]; xhatPartArr = [xhatPart]; close all; for k = 1 : tf %tf为时间长度,k可以理解为时间轴上的k时刻 % System simulation % x数据为时刻k的真实状态值 x = 0.5 * x + 25 * x / (1 + x^2) + 8 * cos(1.2*(k-1)) + sqrt(Q) * randn; %状态方程(1) y = x^2 / 20 + sqrt(R) * randn;%观测方程(2) % Particle filter 生成100个粒子并根据预测和观测值差值计算各个粒子的权重 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); 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为概率密度函数,这里是xpart(i)值落在[m, m+1)上的次数 pdf(m+41) = pdf(m+41) + 1; end end end figure; m = -40 : 40; %此图1绘制k==20时刻xpart(i)区间分布密度 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))]); end % Save data in arrays for later plotting xArr = [xArr x]; xhatPartArr = [xhatPartArr xhatPart]; end t = 0 : tf; figure; plot(t, xArr, 'b.', t, xhatPartArr, 'g'); %此图2对应xArr为真值,xhatPartArr为粒子滤波值 xlabel('time step'); ylabel('state'); legend('True state', 'Particle filter estimate');
相关文章推荐
- 第2章 单层前向网络及LMS学习算法仿真实例 Matlab 实现
- 粒子滤波(Particle filter)算法简介及MATLAB实现
- 内积之比法相位提取算法(RIP)---MATLAB实现
- 【算法导论】八皇后问题的算法实现(C、MATLAB、Python版)
- 利用梯度下降法实现线性回归的算法及matlab实现
- 通过文件读写方式实现Matlab和Modelsim的联合仿真
- 极限学习机(ELM) 算法及MATLAB程序实现
- PCA降维算法总结以及matlab实现PCA(个人的一点理解)
- 【学习笔记】matlab算法实现贝叶斯判别classify函数
- 机器学习笔记-----AP(affinity propagat)算法讲解及matlab实现
- 最小二乘解包裹算法----MATLAB实现
- 《合成孔径雷达成像——算法与实现》之【10】仿真图3.13
- matlab实现ICP(3D迭代最近点算法)
- 区域生长算法原理以及Matlab实现
- 几种简单常用的镜头边缘检测算法(matlab实现)
- 匈牙利算法的matlab实现
- Yale人脸数据库、PCA算法实现Matlab中特征脸生成和简单的人脸识别
- 基于C均值算法简单分类(matlab实现)
- 数字图像去噪典型算法及matlab实现
- Fisher线性判别与感知器算法Matlab实现