粒子滤波程序
粒子滤波通俗讲解
粒子滤波通俗讲解粒子滤波是一种用于估计系统状态的方法，它基于随机过程的思想，通过将系统状态表示为一组粒子的集合，利用粒子的权重来对系统状态进行估计和更新。
在传统的滤波方法中，通常假设系统的状态和观测是连续的，并且满足一定的数学模型。
然而，在实际应用中，很多系统的状态和观测都是离散的或者具有非线性特性，这就给滤波问题带来了挑战。
粒子滤波的核心思想是通过一组粒子来近似表示系统的状态分布。
每个粒子代表了系统的一个可能状态，并且具有一定的权重。
在滤波过程中，通过对粒子的权重进行更新和重采样，可以逐步逼近真实的状态分布。
具体来说，粒子滤波包括以下几个步骤：1. 初始化：根据系统的先验信息，生成一组初始粒子。
初始粒子的状态可以根据先验分布随机生成，或者通过观测值进行初始化。
2. 预测：根据系统的动力学模型，对每个粒子进行状态预测。
预测的方法可以根据具体的系统模型进行选择，例如使用运动学方程进行预测。
3. 权重更新：根据观测值和粒子的预测状态，计算每个粒子的权重。
权重的计算可以通过比较观测值和预测状态之间的差异来进行，差异越小，权重越大。
4. 重采样：根据粒子的权重，进行重采样操作。
重采样的目的是根据粒子的权重，增加高权重粒子的数量，减少低权重粒子的数量。
重采样方法可以采用多种方式，例如轮盘赌法或者系统化重采样。
5. 状态估计：根据重采样后的粒子，计算系统的状态估计值。
常见的估计方法包括计算粒子的加权平均值或者加权中位数。
通过不断迭代上述步骤，粒子滤波可以逐步逼近真实的系统状态分布。
与传统的滤波方法相比，粒子滤波具有以下优点：1. 粒子滤波可以处理非线性和非高斯的系统模型。
由于粒子滤波是基于粒子的近似表示，因此可以灵活地应对系统模型的复杂性。
2. 粒子滤波可以处理观测缺失和非完全观测的情况。
由于粒子滤波是基于观测值和粒子状态的比较来更新权重，因此可以有效地处理观测值不完全的情况。
3. 粒子滤波可以处理非线性的测量方程。
粒子滤波C++程序
void CFastTrackingDlg::OnBnClickedButton4(){// TODO: Add your control notification handler code here//1.condensation setupconst int stateNum=4;const int measureNum=2;const int sampleNum=2000;CvConDensation* condens = cvCreateConDensation(stateNum,measureNum,sampleNum);CvMat* lowerBound;CvMat* upperBound;lowerBound = cvCreateMat(stateNum, 1, CV_32F);upperBound = cvCreateMat(stateNum, 1, CV_32F);cvmSet(lowerBound,0,0,0.0 );cvmSet(upperBound,0,0,winWidth );cvmSet(lowerBound,1,0,0.0 );cvmSet(upperBound,1,0,winHeight );cvmSet(lowerBound,2,0,0.0 );cvmSet(upperBound,2,0,0.0 );cvmSet(lowerBound,3,0,0.0 );cvmSet(upperBound,3,0,0.0 );float A[stateNum][stateNum] ={1,0,1,0,0,1,0,1,0,0,1,0,0,0,0,1};memcpy(condens->DynamMatr,A,sizeof(A));cvConDensInitSampleSet(condens, lowerBound, upperBound);CvRNG rng_state = cvRNG(0xffffffff);for(int i=0; i < sampleNum; i++){condens->flSamples[i][0] = float(cvRandInt( &rng_state ) % winWidth); //widthcondens->flSamples[i][1] = float(cvRandInt( &rng_state ) % winHeight);//height }CvFont font;cvInitFont(&font,CV_FONT_HERSHEY_SCRIPT_COMPLEX,1,1);char* winName="condensation";cvNamedWindow(winName);cvSetMouseCallback(winName,mouseEvent);IplImage* img=cvCreateImage(cvSize(winWidth,winHeight),8,3);bool isPredictOnly=false;//trigger for prediction only,press SPACEBARwhile (1){//2.condensation predictionCvPoint predict_pt=cvPoint((int)condens->State[0],(int)condens->State[1]);float variance[measureNum]={0};//get variance/standard deviation of each statefor (int i=0;i<measureNum;i++){//sumfloat sumState=0;for (int j=0;j<condens->SamplesNum;j++){sumState+=condens->flSamples[j][i];}//averagesumState/=sampleNum;//variancefor (int j=0;j<condens->SamplesNum;j++){variance[i]+=(condens->flSamples[j][i]-sumState)*(condens->flSamples[j][i]-sumState);}variance[i]/=sampleNum-1;}//3.update particals confidenceCvPoint pt;if (isPredictOnly){pt=predict_pt;}else{pt=mousePosition;}for (int i=0;i<condens->SamplesNum;i++){float probX=(float)exp(-1*(pt.x-condens->flSamples[i][0])*(pt.x-condens->flSamples[i][0])/(2*variance[0]));float probY=(float)exp(-1*(pt.y-condens->flSamples[i][1])*(pt.y-condens->flSamples[i][1])/(2*variance[1]));condens->flConfidence[i]=probX*probY;}//4.update condensationcvConDensUpdateByTime(condens);//drawcvSet(img,cvScalar(255,255,255,0));cvCircle(img,predict_pt,5,CV_RGB(0,255,0),3);//predicted point with greenchar buf[256];sprintf_s(buf,256,"predicted position:(%3d,%3d)",predict_pt.x,predict_pt.y);cvPutText(img,buf,cvPoint(10,30),&font,CV_RGB(0,0,0));if (!isPredictOnly){cvCircle(img,mousePosition,5,CV_RGB(255,0,0),3);//current position with redsprintf_s(buf,256,"real position :(%3d,%3d)",mousePosition.x,mousePosition.y);cvPutText(img,buf,cvPoint(10,60),&font,CV_RGB(0,0,0));}cvShowImage(winName, img);int key=cvWaitKey(30);if (key==27){//escbreak;}else if (key==' '){//trigger for prediction//isPredict=!isPredict;if (isPredictOnly){isPredictOnly=false;}else{isPredictOnly=true;}}}cvReleaseImage(&img);cvReleaseConDensation(&condens);}。
matlab 粒子滤波代码
以下是一个简单的 MATLAB 粒子滤波器的代码示例：```matlab% 初始化参数N = 100; % 粒子数量dt = 0.1; % 时间步长x = [0 0]; % 初始位置P = eye(2); % 初始协方差矩阵Q = eye(2); % 过程噪声协方差矩阵R = eye(2); % 观测噪声协方差矩阵G = [0.9 0.1; 0.1 0.9]; % 转换矩阵N_particles = size(Q,1); % 粒子数量particles = zeros(N_particles,2); % 初始化粒子particles(:,1) = x(1); % 设置粒子的 x 分量particles(:,2) = x(2); % 设置粒子的 y 分量weights = ones(N_particles,1) / N_particles; % 初始化权重% 模拟观测数据z = [1.2 0.5]; % 观测位置R_inv = inv(R); % 观测噪声协方差矩阵的逆H = [z(1) -z(2); z(2) z(1)]; % 观测矩阵y = H * x; % 预测的观测值% 粒子滤波步骤for t = 1:100% 重采样步骤weights = weights / sum(weights);index = randsample(1:N_particles, N, true, weights); particles = particles(index,:);% 预测步骤x_pred = particles;P_pred = Q;x_pred = G * x_pred;P_pred = P_pred + dt * G * P_pred;P_pred = P_pred + P_pred * G' + R;% 更新步骤y_pred = H * x_pred;S = H * P_pred * H' + R_inv;K = P_pred * H' * inv(S);x = x_pred + K * (z - y_pred);P = P_pred - P_pred * K * H';end```在这个代码示例中，我们使用了两个步骤：重采样步骤和预测/更新步骤。
粒子滤波算法综述
粒子滤波算法综述作者：李孟敏来源：《中国新通信》2015年第10期【摘要】对粒子滤波算法的原理、发展历史以及应用领域进行综述，首先针对非线性非高斯系统的状态滤波问题阐述粒子滤波的原理，而后讨论粒子滤波算法存在的主要问题和改进手段，最后阐明其在多个研究领域中的应用现状。
【关键字】非线性滤波概率密度重采样粒子退化一、引言粒子滤波（PF）是一种在处理非线性非高斯系统状态估计问题时具有较好估计效果的方法，其原理是通过非参数蒙特卡洛方法实现贝叶斯滤波。
其最早起源于Hammersley等人在20实际50年代末提出的顺序重要性采样（SIS）滤波思想。
但由于上述方法存在严重的样本权值退化从而导致的粒子数匮乏现象，直到1993年Gordon等人将重采样技术引入蒙特卡洛重要性采样过程，提出一种Bootstrap滤波方法，从而奠定了粒子滤波算法的基础。
二、基本粒子滤波算法三、粒子滤波算法存在的主要问题及改进对于SIS算法来说，容易出现粒子的退化问题，目前存在的诸多对SIS算法的改进中，能够降低该现象影响的有效方法是选择合适的重要性函数和采用重采样方法。
针对状态空间模型的改进算法，如辅助变量粒子滤波算法（APF），局部线性化方法，代表的算法主要有EKF，UKF等。
针对重采样改进方法，文献通过将遗传算法和进化算法引入粒子滤波算法中，增加重采样过程中粒子的多样性。
然APF算法在过程噪声较小时，可获得比标准粒子滤波更高的滤波精度，在过程噪声较大时，其效果则大大降低。
采用局部线性化的方法EKF，UKF都是针对非线性系统的线性卡尔曼滤波方法的变形和改进，因此受到线性卡尔曼滤波算法的条件制约，而对于非高斯分布的状态模型，其滤波性能变差。
将遗传算法和进化算法与粒子滤波结合的改进粒子滤波算法，虽取得了较好的滤波效果，然而是以消耗过多计算资源为代价的。
四、粒子滤波的应用4.1 目标跟踪对目标进行定位和跟踪是典型的动态系统状态估计问题，在诸如纯角度跟踪的运动模型中，采用粒子滤波方法进行实现目标跟踪已获得了较好的跟踪精度，文献研究了多目标跟踪与数据融合问题，文献给出了基于粒子滤波的群目标跟踪算法。
粒子滤波算法
粒⼦滤波算法
上学的时候每次遇到“粒⼦滤波”那⼀堆符号，我就晕菜。
今天闲来⽆事，搜了⼀些⽂章看，终于算是理解了。
下⾯⽤⽩话记⼀下我的理解。
问题表述：
某年⽉，警⽅（跟踪程序）要在某个城市的茫茫⼈海（采样空间）中跟踪寻找⼀个罪犯（⽬标），警⽅采⽤了粒⼦滤波的⽅法。
1. 初始化：
警⽅找来了⼀批警⽝（粒⼦），并且让每个警⽝预先都闻了罪犯留下来的⾐服的味道（为每个粒⼦初始化状态向量S0），然后将警⽝均匀布置到城市的各个区（均匀分布是初始化粒⼦的⼀种⽅法，另外还有诸如⾼斯分布，即：将警⽝以罪犯留⾐服的那个区为中⼼来扩展分布开来）。
2. 搜索：
每个警⽝都闻⼀闻⾃⼰位置的⼈的味道（粒⼦状态向量Si），并且确定这个味道跟预先闻过的味道的相似度（计算特征向量的相似性），这个相似度的计算最简单的⽅法就是计算⼀个欧式距离（每个粒⼦i对应⼀个相似度Di），然后做归⼀化（即：保证所有粒⼦的相似度之和为1）。
3. 决策：
总部根据警⽝们发来的味道相似度确定罪犯出现的位置（概率上最⼤的⽬标）：最简单的决策⽅法为哪个味道的相似度最⾼，那个警⽝处的⼈就是⽬标。
4. 重采样：
总部根据上⼀次的决策结果，重新布置下⼀轮警⽝分布（重采样过程）。
最简单的⽅法为：把相似度⽐较⼩的地区的警⽝抽调到相似度⾼的地区。
上述，2,3,4过程重复进⾏，就完成了粒⼦滤波跟踪算法的全过程。
一种基于神经网络的粒子滤波算法设计
一种基于神经网络的粒子滤波算法设计谢世龙;周玉国;刘真【期刊名称】《自动化技术与应用》【年(卷),期】2017(036)011【摘要】粒子退化现象是粒子滤波算法应用中的一个主要问题,标准重采样粒子滤波算法虽然可以有效的解决粒子退化现象,改善粒子滤波性能,但同时也会出现其他问题,如粒子样本的多样性缺失、具有较高权值的粒子多次被统计等等.对此,提出一种基于神经网络的粒子滤波算法研究,将BP神经网络算法和标准粒子滤波算法相结合,调整较小权值的粒子,分裂较大权值的粒子,仿真结果表明,提出的算法是可行的.%Degeneracy phenomenon is a main disadvantage to particle filter application,common re-sampling methods can resolve degeneracy phenomenon,improve the performance of particle filter,but it also has other problems,such as the sample impoverishment is deduced and particles with high weights repeatedly statistics.Therefore,a particle filter algorithm based on neural network is bine neural network with particle filter algorithm,the degeneracy phenomenon is relieved by weight adjustment and weight division.Simulation results show the feasibility of the proposed algorithm.【总页数】4页(P1-4)【作者】谢世龙;周玉国;刘真【作者单位】青岛理工大学自动化学院,山东青岛266520;青岛理工大学自动化学院,山东青岛266520;青岛理工大学自动化学院,山东青岛266520【正文语种】中文【中图分类】TP273+.4【相关文献】1.基于System Generator的简化粒子滤波算法设计及硬件实现 [J], 王佳辉;王义平;薛雅丽2.基于优化粒子滤波器的体育视频目标跟踪算法设计 [J], 王俊鹏;侯小毛3.一种基于卷积神经网络的立体匹配算法设计 [J], 鲁志敏; 袁勋; 陈松4.一种基于卷积神经网络的立体匹配算法设计 [J], 鲁志敏; 袁勋; 陈松5.基于粒子滤波的LAI时间序列重构算法设计与实现 [J], 李曼曼;刘峻明;王鹏新因版权原因，仅展示原文概要，查看原文内容请购买。
粒子滤波原理
粒子滤波原理粒子滤波（Particle Filter）是一种非参数实时滤波方法，用于估计目标的状态。
它适用于非线性和非高斯问题，并被广泛应用于机器人感知、目标跟踪、信号处理等领域。
本文将介绍粒子滤波的基本原理、流程和应用。
1. 基本原理粒子滤波的基本原理是根据贝叶斯定理，通过推断目标状态的后验分布来预测目标状态。
具体来说，粒子滤波将目标状态表示为一组粒子，每个粒子代表一种可能的状态。
粒子的数量越多，则对目标后验分布的估计就越准确。
粒子滤波算法的流程如下：（1）初始化粒子集合，即根据先验信息生成一组随机的粒子，并赋予它们相应的权重；（2）接收观测数据，并对每个粒子进行状态转移和权重更新。
状态转移是根据系统模型进行的，对于机器人定位问题，状态转移可以使用运动学方程描述机器人在环境中的运动；权重更新是根据观测模型计算得到的，对于机器人定位问题，权重可以用激光传感器的测量值和地图进行匹配计算；（3）根据粒子的权重进行重采样，生成新的粒子集合。
重采样的目的是为了减小样本的方差，并确保样本的代表性。
（4）重复步骤（2）、（3），直到目标状态的后验分布收敛，或达到设定的迭代次数。
2. 算法改进粒子滤波算法在实际应用中存在一些问题，例如样本退化和计算复杂度高等。
为了解决这些问题，学者们提出了一系列改进算法，主要包括以下几种：串行粒子滤波（Sequential Monte Carlo, SMC）、粒子群优化算法（Particle Swarm Optimization, PSO）、希尔伯特-黄变换粒子滤波（Hilbert-Huang Transform Particle Filter, HHTPF）和变分粒子群优化算法（Variational Particle Swarm Optimization, VPSO）等。
串行粒子滤波算法是一种常用的改进算法，它将原始粒子集合分为若干个子集，在每个子集上执行滤波过程。
通过这种方式，可以减少不必要的计算，提高算法的效率。
matlab 粒子滤波重采样
粒子滤波（Particle Filtering）是一种基于蒙特卡洛方法的贝叶斯估计算法，广泛应用于非线性非高斯系统的状态估计和参数估计。
在MATLAB中，可以通过编程实现粒子滤波算法，并对其进行重采样。
MATLAB实现粒子滤波的基本步骤如下：
1. 初始化粒子：根据系统的状态空间模型，生成一组随机粒子，并赋予初状态。
2. 预测：根据系统的动态模型和观测模型，对粒子的状态进行预测，得到预测的粒子集合。
3. 更新：利用观测值更新粒子的状态，通过加权卡尔曼滤波或其他贝叶斯方法更新粒子的权重。
4. 重采样：根据粒子的权重进行重采样，生成新的粒子集合。
5. 输出：根据粒子集合的均值和方差，估计系统的状态和参数。
在MATLAB中实现粒子滤波重采样，可以参考以下步骤：
1. 定义粒子滤波相关的函数，如初始化粒子、预测、更新、重采样等。
2. 根据问题的具体形式，编写主函数，包括粒子滤波的各个步骤。
3. 调用相关函数，实现粒子滤波算法。
- 1、下载文档前请自行甄别文档内容的完整性，平台不提供额外的编辑、内容补充、找答案等附加服务。
- 2、"仅部分预览"的文档,不可在线预览部分如存在完整性等问题,可反馈申请退款(可完整预览的文档不适用该条件!)。
- 3、如文档侵犯您的权益，请联系客服反馈,我们会尽快为您处理(人工客服工作时间：9:00-18:30)。
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 : Np_xpart(i) = p_x_estimate + sqrt(pf_P) * randn;endfor 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; %观测值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 : Np_xpart(i) = p_x_estimate + sqrt(pf_P) * randn;endfor 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 : Np_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 : Np_q(i) = p_q(i) / p_qsum;%各个粒子进行权值归一化end% 重采样权重大的粒子多采点，权重小的粒子少采点, 相当于每一次都进行重采样；for i = 1 : Np_u = rand;p_qtempsum = 0;for j = 1 : Np_qtempsum = p_qtempsum + p_q(j);if p_qtempsum >= p_up_xpart(i) = p_xpartminus(j); %在这里xpart(i) 实现循环赋值；终于找到了这里！！！break;endendendp_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);endfor i = (u_symmetry_number+2) : u_total_numberu_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_numberu_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_numberu_x_next = u_x_next + u_w_N1 * u_x_par(i);endu_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_numberu_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_numberu_y_2obser(i,:) = u_x_par(i);end%通过观测方程得到一系列的粒子for i = 1: u_total_numberu_y_2obser(i) = u_y_2obser(i)^2/20;end%通过观测方程后的均值y_obseu_y_obse = u_w_1 * u_y_2obser(1);for i = 2 : u_total_numberu_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_numberu_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_numberu_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));endt = 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;。