1、matlab 粒子滤波中，请问状态方程的 x(t)和观测方程的 y(t)表达什么意思?x(t)=f(x(t-1),u(t),w(t) (1) 状态转移方程，u(t) 为控制量，w(t) 为模型噪声 y(t)=h(x(t),e(t) (2) 观测方程，e(t)为观测噪声举个比较简单的例子，粒子滤波主要用来解决非线性非高斯问题，方法是通过蒙特卡罗方法生成具有一定概率密度分布的粒子，假设为 N 个，通过对粒子后验概率的求解，得到目标状态的最优估计，假设现在粒子初始状态为 X(0)=1，于是在 X(0)附近用正态分布生成 N 个粒子，生成的 N 个粒子可能是 1，2，0，3，1 ，1，这些粒子中肯定
2、1 出现的概率是最大的,对每个粒子初始权值 1/N; 假设现有一个状态转移方程 X(t)=X(t-1)+1+W,于是将这 N 个粒子通过状态转移方程求得了 X(1)时刻的粒子状态，2，3，1，4，2 ， 2(这里没有加上噪声 W，为了看着方便，W 一般是高斯噪声) 得到状态后通过观测方程求每个粒子的权值 w(1)i，一般实际中用似然 度函数，比如对于一个图像人头通过颜色直方图进行跟踪，那这个观测方程就是颜色直方图的似然度函数，于是得到了每个粒子的似然度匹配值，假设 X(1)i(i 表示 N 个粒子中的第 i 个粒子)的似然度值为 wp(1)i，用 X(0)i 的粒子权值w(0)i*wp(1)i
3、，得到了未归一化的权值，再对其进行归一化就得到了 w(1)i,而 X 在 T=1 时刻的估计值就可以用 X(1)=SUM(X(1)i*w(1)i); i=1N 来求得，或者也可以取 w 最大的一点的 X 值 也就是说在粒子滤波器中状态转移方程求的是粒子在下一个时刻的状态，观测方程是对粒子在这一状态的评价，即这个状态与最优的状态相比好不好，好，则这一点所占的权重就大，不好，则占的权重就小粒子滤波算法源于 Montecarlo 的思想，即以某事件出现的频率来指代该事件的概率。因此在滤波过程中，需要用到概率如 P(x)的地方，一概对变量 x 采样，以大量采样的分布近似来表示 P(x)。因此，采用此一
4、思想，在滤波过程中粒子滤波可以处理任意形式的概率，而不像 Kalman 滤波只能处理高斯分布的概率问题。他的一大优势也在于此。 再来看对任意如下的状态方程 x(t)=f(x(t-1),u(t),w(t) (1)y(t)=h(x(t),e(t) (2)其中的 x(t)为 t 时刻状态，u(t) 为控制量，w(t) 和 e(t)分别为模型噪声和观测噪声。(1)当然是状态转移方程，(2) 是观测方程。那么对于这么一个问题粒子滤波怎么来从观测 y(t)，和 x(t-1),u(t) 滤出真实状态 x(t)呢？看看滤波的预估阶段：粒子滤波首先根据 x(t-1) 和他的概率分布生成大量的采样，这些采样就称之
5、为粒子。那么这些采样在状态空间中的分布实际上就是x(t-1) 的概率分布了。好，接下来依据状态转移方程加上控制量可以对每一粒子得到一个预测粒子。所有的预测粒子就代表了涉及哪些参数化的东西）。进入校正阶段来：有了预测粒子，当然不是所有的预测粒子都能得到我们的时间观测值 y，越是接近真实状态的粒子，当然获得越有可能获得观测值 y。于是对所有的粒子得有个评价了，这个评价就是一个条件概率 P(y|xi),直白的说，这个条件概率代表了假设真实状态 x(t)取第 i 个粒子 xi 时获得观测 y 的概率。令这个条件概率为第 i 个粒子的权重。对所有粒子都进行这么一个评价，那么越有可能获得观测 y 的粒子，
6、当然获得的权重越高。好了预测信息融合在粒子的分布中，观测信息又融合在了每一粒子的权重中。最后采用重采样算法，去除低权值的粒子，复制高权值的粒子。所得当然是需要的真实状态 x(t)了，而这些重采样后的粒子，就代表了真实状态的概率分布了。下一轮滤波，再将重采样过后的粒子集输入到状态转移方程中，直接就能够获得预测粒子了。初始状态的问题：可以认为 x(0)在全状态空间内平均分布。于是初始采样就平均分布在整个状态空间中。然后将所有采样输入状态转移方程，得到预测粒子。再评价下所有预测粒子的权重，当然我们在整个状态空间中只有部分粒子能够获的高权值。重采样算法去除低权值的，将下一轮滤波的考虑重点缩小到高权值粒
7、子附近。下面是我最后改写和精简的一个粒子滤波 Matlab 算法。x = 0.1; % initial stateQ = 1; % process noise covarianceR = 1; % measurement noise covariancetf = 50; % simulation lengthN = 100; % number of particles in the particle filterxhat = x;P = 2;xhatPart = x;% Initialize the particle filter. 初始化粒子滤波，xpart 值用来在不同时刻生成粒子for
8、i = 1 : Nxpart(i) = x + sqrt(P) * randn;endxArr = x;xhatPartArr = xhatPart;close all;for k = 1 : tf %tf 为时间长度，k 可以理解为时间轴上的 k 时刻% System simulation% x 数据为时刻 k 的真实状态值x = 0.5 * x + 25 * x / (1 + x2) + 8 * cos(1.2*(k-1) + sqrt(Q) * randn; %状态方程(1)y = x2 / 20 + sqrt(R) * randn;%观测方程(2)% Particle filter 生
9、成 100 个粒子并根据预测和观测值差值计算各个粒子的权重for i = 1 : Nxpartminus(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(-vhat2 / 2 / R); %根据差值给出权重end% Normalize the likelihood of eac
10、h a priori estimate.qsum = sum(q);for i = 1 : Nq(i) = q(i) / qsum;%归一化权重end% Resample.for i = 1 : Nu = rand; % uniform random number between 0 and 1qtempsum = 0;for j = 1 : Nqtempsum = qtempsum + q(j);if qtempsum = u%重采样对低权重进行剔除，同时保留高权重，防止退化的办法xpart(i) = xpartminus(j);break;endendend% The particle f
11、ilter estimate is the mean of the particles.xhatPart = mean(xpart); %经过粒子滤波处理后的均值% Plot the estimated pdfs at a specific time.if k = 20% Particle filter pdfpdf = zeros(81,1);for m = -40 : 40for i = 1 : Nif (m = xpart(i) endendendfigure;m = -40 : 40;%此图 1 绘制 k=20 时刻 xpart(i)区间分布密度plot(m, pdf / N, r);
12、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 plottingxArr = xArr x; xhatPartArr = xhatPartArr xhatPart;endt = 0 : tf;figure;plot(t, xArr, b., t, xhatPartArr, g); %此图 2 对应 xArr 为真值，xhatPartArr 为粒子滤波值xlabel(time step); ylabel(state);legend(True state, Particle filter estimate);
