✅作者简介热爱科研的Matlab仿真开发者擅长毕业设计辅导、数学建模、数据处理、算法改进、程序设计科研仿真。完整代码获取 定制创新 论文复现私信个人信条做科研博学之、审问之、慎思之、明辨之、笃行之是为博学慎思明辨笃行。1. 相关介绍在现代科技的众多领域中滤波与估计技术犹如精密的 “信息过滤器”从充满噪声的数据中提炼出关键信息对目标状态进行精准预测。无论是引导飞行器穿越复杂空域还是帮助机器人在未知环境中导航滤波与估计都发挥着举足轻重的作用。本文将深入探索扩展卡尔曼滤波EKF、交互式多模型IMM平滑器、Hermite 多项式计算、球径向积分法则以及无迹卡尔曼滤波UKF等核心技术揭示它们的奥秘与应用。扩展卡尔曼滤波EKF非线性世界的线性近似线性化的智慧扩展卡尔曼滤波EKF是应对非线性系统估计问题的常用方法。在现实世界中许多系统的动态模型和观测模型呈现非线性特性直接处理这些非线性关系极具挑战性。EKF 巧妙地通过线性化手段解决这一难题。它基于泰勒级数展开在当前估计点附近将非线性函数近似为线性函数。以一个简单的非线性系统f(x)为例通过泰勒展开f(x)≈f(x^)∂x∂f∣x^(x−x^)忽略高阶项后将其转化为近似线性形式。这样原本复杂的非线性系统就能够纳入卡尔曼滤波的框架利用卡尔曼滤波经典的预测和更新步骤进行状态估计。多领域的应用足迹EKF 广泛应用于各类实际场景。在全球定位系统GPS辅助的惯性导航系统中GPS 提供的位置信息和惯性传感器测量的加速度、角速度等数据都伴随着噪声且载体的运动模型是非线性的。EKF 能够有效融合这些不同来源的数据通过对系统状态如位置、速度、姿态的连续估计为载体提供精确的导航信息。例如在航空领域飞机在飞行过程中受到气流、重力等多种因素影响其运动呈现非线性EKF 可实时处理传感器数据帮助飞行员准确掌握飞机状态。优势与局限并存EKF 的优点显而易见。它为非线性系统的估计提供了一种相对简单且有效的解决方案计算量在可接受范围内能够快速给出状态估计结果。然而EKF 并非完美无缺。线性化过程不可避免地引入了误差尤其是在非线性较强的系统中这种近似误差可能会逐渐累积导致估计精度下降。同时EKF 对模型误差较为敏感如果系统模型与实际情况存在偏差其估计性能会受到显著影响。交互式多模型IMM平滑器应对不确定性的多面手多模型协作的艺术交互式多模型IMM平滑器基于一种创新的理念利用多个不同的模型并行工作每个模型对应目标可能的一种运动模式。在实际应用中目标的运动模式往往复杂多变单一模型难以准确描述。IMM 平滑器通过多个模型同时对目标状态进行估计模型之间通过交互作用动态调整各自的权重。例如在目标跟踪场景中一个模型可能假设目标做匀速直线运动另一个模型假设目标进行转弯运动。随着观测数据的不断输入IMM 平滑器依据数据与各模型的匹配程度调整每个模型的权重使得更符合实际运动模式的模型在最终估计中占据更大比重。此外IMM 平滑器还利用平滑过程不仅考虑当前观测数据还结合过去的观测信息进一步优化状态估计。空中追踪的实践范例在空中目标跟踪领域IMM 平滑器展现出卓越的性能。空中目标如飞机、无人机等其运动模式丰富多样可能在不同阶段进行匀速飞行、转弯、加速或减速等操作。IMM 平滑器通过多个模型并行跟踪能够快速适应目标运动模式的变化。例如当飞机从直线飞行转为转弯时负责转弯模式的模型权重会逐渐增加而匀速直线运动模型的权重相应减小从而准确跟踪目标的新运动状态提高跟踪精度。权衡中的性能特点IMM 平滑器的优势在于对目标运动模式不确定性的出色处理能力。它能够在多种可能的运动模式之间灵活切换确保对复杂运动目标的稳定跟踪。然而这种灵活性是以较高的计算复杂度为代价的。由于需要同时维护多个模型并在模型之间进行频繁的交互计算IMM 平滑器的计算量相对较大这在一些计算资源受限的场景中可能成为制约因素。球径向积分法则高维积分的高效之道高维积分的新视角球径向积分法则是一种用于高维空间积分计算的数值方法。在滤波与估计中常常涉及到对概率密度函数在高维空间的积分如计算后验概率、预测分布等。传统的积分方法在高维空间中计算量急剧增加效率低下。球径向积分法则提供了一种更高效的解决方案。它将积分区域分解为球坐标和径向坐标通过精心设计的采样点分布和权重分配对函数进行积分近似。例如在n维空间中将积分表示为球坐标下的积分形式通过在球面上和径向上选择特定的采样点并赋予相应权重计算积分的近似值。滤波估计的积分助力在滤波与估计领域球径向积分法则为处理高维积分问题提供了有力支持。以计算后验概率p(x∣y)为例通常需要对联合概率密度函数p(x,y)在y固定的情况下对x进行积分。球径向积分法则能够高效地处理这一高维积分过程准确计算后验概率为状态估计提供关键信息。在一些复杂的多传感器融合问题中涉及到对多个变量的联合概率密度函数积分球径向积分法则能够显著提高计算效率保证系统的实时性。应用中的权衡考量球径向积分法则具有计算效率高、精度较好的优点特别适用于高维空间积分问题。它能够在相对较少的采样点下获得较高的积分精度减少计算量。然而该法则对积分区域的形状有一定要求一般适用于具有球对称性或可转化为球对称形式的积分区域。此外采样点的选择和权重计算需要根据具体问题进行优化否则可能导致积分精度下降。无迹卡尔曼滤波UKF非线性处理的新突破Sigma 点的神奇力量无迹卡尔曼滤波UKF是一种针对非线性系统的高效滤波方法其核心在于通过选择一组 Sigma 点来近似非线性函数的概率分布。与 EKF 不同UKF 不依赖于线性化近似而是直接对非线性函数进行处理。具体来说UKF 根据状态变量的均值和协方差精心选择一组 Sigma 点这些点能够更准确地捕捉非线性函数的特性。然后将这些 Sigma 点通过非线性函数进行传播并对传播后的点进行加权计算从而得到状态估计和协方差更新。例如在一个二维非线性系统中UKF 通过选择合适的 Sigma 点能够更精确地描述系统状态的概率分布避免了 EKF 线性化带来的误差。与 EKF 的性能较量与 EKF 相比UKF 在处理非线性问题上具有显著优势。由于避免了线性化过程UKF 通常能够提供更高的估计精度尤其是在非线性较强的系统中。同时UKF 对模型误差具有更好的鲁棒性即使系统模型存在一定偏差仍能保持较好的估计性能。然而UKF 的计算量相对较大。在高维状态空间中Sigma 点的数量和计算量会迅速增加这对计算资源提出了较高要求。复杂场景的广泛应用UKF 在众多复杂场景中得到了广泛应用。在机器人视觉导航领域相机成像模型是非线性的UKF 能够有效处理图像信息与机器人位姿之间的非线性关系实现精确的定位和导航。在生物医学信号处理中生物系统的状态变化往往呈现非线性UKF 可用于对生物信号进行滤波和状态估计如对心脏跳动信号的分析帮助医生更准确地了解心脏状态。2. 运行效果展示3. 部分代码呈现%% This software is distributed under the GNU General Public% Licence (version 2 or later); please refer to the file% Licence.txt, included with the software, for details.keep_trajectory 0;silent 0;% Number of steps to advance at a time in animations.steps 2;% Handles to measurement model function and its derivativesh_func bot_h;dh_dx_func bot_dh_dx;d2h_dx2_func bot_d2h_dx2;% Create a bit curved trajectory and angle% measurements from two sensorsS1 [-1;-2];S2 [1;1];sd 0.05;dt 0.01;if ~keep_trajectory% Accelerations for the objecta zeros(1,500);a(1,50:100) pi/2/51/dt 0.01*randn(1,51);a(1,200:250) pi/2/51/dt 0.01*randn(1,51);a(1,350:400) pi/2/51/dt 0.01*randn(1,51);x [0;0;1;0];t 0;X [];Y [];T [];for i1:500F [0 0 1 0;...0 0 0 1;...0 0 0 a(i);...0 0 -a(i) 0];x expm(F*dt)*x;y1 atan2(x(2)-S1(2), x(1)-S1(1)) sd * randn;y2 atan2(x(2)-S2(2), x(1)-S2(1)) sd * randn;t t dt;X [X x];T [T t];Y [Y [y1;y2]];endend% Uncomment if you want to plot and print the measurements%plot(1:size(Y,2),Y(1,:),.,1:size(Y,2),Y(2,:),.);%legend(Sensor 1, Sensor 2);%title(Measurements from sensors in radians);%print -dpsc bot_demo_measurements.ps%% Initialize EKF to values%% x 0% y 0,% dx/dt 0% dy/dt 0%% with great uncertainty in velocity%M [0;0;0;0];P diag([4 4 4 4]);R sd^2;if ~silentder_check(h_func, dh_dx_func, 1, M, S1);der_check(h_func, dh_dx_func, 1, M, S2);endqx 0.1;qy 0.1;F [0 0 1 0;0 0 0 1;0 0 0 0;0 0 0 0];[A,Q] lti_disc(F,[],diag([0 0 qx qy]),dt);clc;clf;disp([In this demonstration we track a moving object with two sensors, ,...which gives only bearings of the object with respect to sensors position.,...The state of the system is estimated with EKF.])disp( );fprintf(Running EKF...)%% Track and animate%MM zeros(size(M,1),size(Y,2));PP zeros(size(M,1),size(M,1),size(Y,2));ME zeros(size(M,1),1);for k1:size(Y,2)% Track with EKF[M,P] ekf_predict1(M,P,A,Q);[M,P] ekf_update1(M,P,Y(:,k),dh_dx_func,R*eye(2),h_func,[],[S1 S2]);MM(:,k) M;PP(:,:,k) P;ME(k) P(1,1) P(2,2);endekf_rmse sqrt(mean((X(1,:)-MM(1,:)).^2(X(2,:)-MM(2,:)).^2));fprintf(Done!\n)disp( );disp([The filtering results are now displayed sequentially. ,...Notice how the estimates gets more accurate when the filter gets on the right track. ,...The green ellipse around the current estimate (little blue circle) reflects the filters ,...confidence intervals of the position estimate.]);disp( );disp(push any key to proceed);% Plot the filtered estimates sequentiallyif ~silentM MM(:,1);P PP(:,:,1);EST M;tt (0:0.01:1)*2*pi;cc repmat(M(1:2),1,length(tt)) ...2*chol(P(1:2,1:2))*[cos(tt);sin(tt)];h plot(X(1,:),X(2,:),r-,...M(1),M(2),bo,...EST(1,:),EST(2,:),b--,...cc(1,:),cc(2,:),g-,...S1(1),S1(2),k--,...S1(1),S1(2),k^,...S2(1),S2(2),k--,...S2(1),S2(2),k^);legend(Location,NorthWest,Real trajectory,Current estimate,Estimated trajectory,...Confidence interval,Measurements from sensors,Positions of the sensors);title(Bearings Only Tracking with EKF.)axis([-1.5 1.5 -2.5 1.5]);EST [];for k1:steps:size(Y,2)M MM(:,k);P PP(:,:,k);EST MM(:,1:k);% Confidence ellipsecc repmat(M(1:2),1,length(tt)) ...2*chol(P(1:2,1:2))*[cos(tt);sin(tt)];% Measurement directionslen 2.5;dx1 len*cos(Y(1,k));dy1 len*sin(Y(1,k));dx2 len*cos(Y(2,k));dy2 len*sin(Y(2,k));% Update graphicsset(h(2),xdata,M(1)); set(h(2),ydata,M(2));set(h(3),xdata,EST(1,:)); set(h(3),ydata,EST(2,:));set(h(4),xdata,cc(1,:)); set(h(4),ydata,cc(2,:));set(h(5),xdata,[S1(1);S1(1)dx1]); set(h(5),ydata,[S1(2);S1(2)dy1]);set(h(7),xdata,[S2(1);S2(1)dx2]); set(h(7),ydata,[S2(2);S2(2)dy2]);pauseendendclc;disp([In this demonstration we track a moving object with two sensors, ,...which gives only bearings of the object with respect to sensors position.,...The state of the system is estimated with EKF.])disp( );fprintf(Running EKF...Done!\n)disp( );disp([The filtering results are now displayed sequentially. ,...Notice how the estimates gets more accurate when the filter gets on the right track. ,...The green ellipse around the current estimate (little blue circle) reflects the filters ,...confidence intervals of the position estimate.]);disp( );disp(push any key to smooth the estimates with ERTS and ETF);if (~silent) pause; end;clc;fprintf(Smoothing with ERTS and ETF...);%% Smoother 1%[SM1,SP1] erts_smooth1(MM,PP,A,Q);eks_rmse1 sqrt(mean((X(1,:)-SM1(1,:)).^2(X(2,:)-SM1(2,:)).^2));ME1 squeeze(SP1(1,1,:)SP1(2,2,:));%% Smoother 2%[SM2,SP2] etf_smooth1(MM,PP,Y,A,Q,[],[],[],...dh_dx_func,R*eye(2),h_func,[],[S1 S2]);eks_rmse2 sqrt(mean((X(1,:)-SM2(1,:)).^2(X(2,:)-SM2(2,:)).^2));ME2 squeeze(SP2(1,1,:)SP2(2,2,:));fprintf(Done!\n)disp( );disp([Smoothing results of ERTS are now displayed sequentially. ,...Notice how the confidence ellipse gets even smaller now.]);disp( );disp(push any key to proceed);% Plot the smoothed estimates sequentiallyif ~silentM SM1(:,1);P SP1(:,:,1);EST M;cc repmat(M(1:2),1,length(tt)) ...2*chol(P(1:2,1:2))*[cos(tt);sin(tt)];h plot(X(1,:),X(2,:),r-,...M(1),M(2),o,...EST(1,:),EST(2,:),--,...cc(1,:),cc(2,:),g-,...MM(1,:),MM(2,:),b--,...S1(1),S1(2),k^,S2(1),S2(2),k^);legend(Location,NorthWest,Real trajectory,Current estimate,Smoothed trajectory,...Confidence interval,Filter estimate,Positions of the sensors);title(Bearings Only Tracking with ERTS.)axis([-1.5 1.5 -2.5 1.5]);EST [];for ksize(Y,2):-steps:1M SM1(:,k);P SP1(:,:,k);EST SM1(:,end:-1:k);% Confidence ellipsecc repmat(M(1:2),1,length(tt)) ...2*chol(P(1:2,1:2))*[cos(tt);sin(tt)];set(h(2),xdata,M(1)); set(h(2),ydata,M(2));set(h(3),xdata,EST(1,:)); set(h(3),ydata,EST(2,:));set(h(4),xdata,cc(1,:)); set(h(4),ydata,cc(2,:));pause;endenddisp( )disp(push any key to display all estimates together)if (~silent) pause; end;clc;disp(All estimates are now displayed.)%% Plot all the estimates together%if ~silentplot(X(1,:),X(2,:),k-,...MM(1,:),MM(2,:),b--,...SM1(1,:),SM1(2,:),r-.,...SM2(1,:),SM2(2,:),g-.,...S1(1),S1(2),k^,S2(1),S2(2),k^);axis([-1.5 1.5 -2.5 1]);legend(Real trajectory,...EKF1 estimate,...ERTS estimate,...ETF estimate,...Positions of sensors,...Location, NorthWest);title(Filtering and smoothing result with 1st order EKF);axis([-1.5 1.5 -2.5 1.5]);% Uncomment if you want to save an image%print -dpsc bot_demo_ekf1.psend% Print RMSEdisp( );disp(RMS errors:);fprintf(EKF-RMSE %.3f [%.3f]\n,ekf_rmse,sqrt(mean(ME)));fprintf(EKS-RMSE1 %.4f [%.4f]\n,eks_rmse1,sqrt(mean(ME1)));fprintf(EKS-RMSE2 %.4f [%.4f]\n,eks_rmse2,sqrt(mean(ME2)));4. 参考文献更多免费数学建模和仿真教程关注领取如果觉得内容不错那就请分享和点个“在看”呗
【滤波跟踪】扩展卡尔曼滤波EKF、交互式多模型IMM平滑器、Hermite 多项式计算、球径向积分法则以及无迹卡尔曼滤波UKF附matlab代码
✅作者简介热爱科研的Matlab仿真开发者擅长毕业设计辅导、数学建模、数据处理、算法改进、程序设计科研仿真。完整代码获取 定制创新 论文复现私信个人信条做科研博学之、审问之、慎思之、明辨之、笃行之是为博学慎思明辨笃行。1. 相关介绍在现代科技的众多领域中滤波与估计技术犹如精密的 “信息过滤器”从充满噪声的数据中提炼出关键信息对目标状态进行精准预测。无论是引导飞行器穿越复杂空域还是帮助机器人在未知环境中导航滤波与估计都发挥着举足轻重的作用。本文将深入探索扩展卡尔曼滤波EKF、交互式多模型IMM平滑器、Hermite 多项式计算、球径向积分法则以及无迹卡尔曼滤波UKF等核心技术揭示它们的奥秘与应用。扩展卡尔曼滤波EKF非线性世界的线性近似线性化的智慧扩展卡尔曼滤波EKF是应对非线性系统估计问题的常用方法。在现实世界中许多系统的动态模型和观测模型呈现非线性特性直接处理这些非线性关系极具挑战性。EKF 巧妙地通过线性化手段解决这一难题。它基于泰勒级数展开在当前估计点附近将非线性函数近似为线性函数。以一个简单的非线性系统f(x)为例通过泰勒展开f(x)≈f(x^)∂x∂f∣x^(x−x^)忽略高阶项后将其转化为近似线性形式。这样原本复杂的非线性系统就能够纳入卡尔曼滤波的框架利用卡尔曼滤波经典的预测和更新步骤进行状态估计。多领域的应用足迹EKF 广泛应用于各类实际场景。在全球定位系统GPS辅助的惯性导航系统中GPS 提供的位置信息和惯性传感器测量的加速度、角速度等数据都伴随着噪声且载体的运动模型是非线性的。EKF 能够有效融合这些不同来源的数据通过对系统状态如位置、速度、姿态的连续估计为载体提供精确的导航信息。例如在航空领域飞机在飞行过程中受到气流、重力等多种因素影响其运动呈现非线性EKF 可实时处理传感器数据帮助飞行员准确掌握飞机状态。优势与局限并存EKF 的优点显而易见。它为非线性系统的估计提供了一种相对简单且有效的解决方案计算量在可接受范围内能够快速给出状态估计结果。然而EKF 并非完美无缺。线性化过程不可避免地引入了误差尤其是在非线性较强的系统中这种近似误差可能会逐渐累积导致估计精度下降。同时EKF 对模型误差较为敏感如果系统模型与实际情况存在偏差其估计性能会受到显著影响。交互式多模型IMM平滑器应对不确定性的多面手多模型协作的艺术交互式多模型IMM平滑器基于一种创新的理念利用多个不同的模型并行工作每个模型对应目标可能的一种运动模式。在实际应用中目标的运动模式往往复杂多变单一模型难以准确描述。IMM 平滑器通过多个模型同时对目标状态进行估计模型之间通过交互作用动态调整各自的权重。例如在目标跟踪场景中一个模型可能假设目标做匀速直线运动另一个模型假设目标进行转弯运动。随着观测数据的不断输入IMM 平滑器依据数据与各模型的匹配程度调整每个模型的权重使得更符合实际运动模式的模型在最终估计中占据更大比重。此外IMM 平滑器还利用平滑过程不仅考虑当前观测数据还结合过去的观测信息进一步优化状态估计。空中追踪的实践范例在空中目标跟踪领域IMM 平滑器展现出卓越的性能。空中目标如飞机、无人机等其运动模式丰富多样可能在不同阶段进行匀速飞行、转弯、加速或减速等操作。IMM 平滑器通过多个模型并行跟踪能够快速适应目标运动模式的变化。例如当飞机从直线飞行转为转弯时负责转弯模式的模型权重会逐渐增加而匀速直线运动模型的权重相应减小从而准确跟踪目标的新运动状态提高跟踪精度。权衡中的性能特点IMM 平滑器的优势在于对目标运动模式不确定性的出色处理能力。它能够在多种可能的运动模式之间灵活切换确保对复杂运动目标的稳定跟踪。然而这种灵活性是以较高的计算复杂度为代价的。由于需要同时维护多个模型并在模型之间进行频繁的交互计算IMM 平滑器的计算量相对较大这在一些计算资源受限的场景中可能成为制约因素。球径向积分法则高维积分的高效之道高维积分的新视角球径向积分法则是一种用于高维空间积分计算的数值方法。在滤波与估计中常常涉及到对概率密度函数在高维空间的积分如计算后验概率、预测分布等。传统的积分方法在高维空间中计算量急剧增加效率低下。球径向积分法则提供了一种更高效的解决方案。它将积分区域分解为球坐标和径向坐标通过精心设计的采样点分布和权重分配对函数进行积分近似。例如在n维空间中将积分表示为球坐标下的积分形式通过在球面上和径向上选择特定的采样点并赋予相应权重计算积分的近似值。滤波估计的积分助力在滤波与估计领域球径向积分法则为处理高维积分问题提供了有力支持。以计算后验概率p(x∣y)为例通常需要对联合概率密度函数p(x,y)在y固定的情况下对x进行积分。球径向积分法则能够高效地处理这一高维积分过程准确计算后验概率为状态估计提供关键信息。在一些复杂的多传感器融合问题中涉及到对多个变量的联合概率密度函数积分球径向积分法则能够显著提高计算效率保证系统的实时性。应用中的权衡考量球径向积分法则具有计算效率高、精度较好的优点特别适用于高维空间积分问题。它能够在相对较少的采样点下获得较高的积分精度减少计算量。然而该法则对积分区域的形状有一定要求一般适用于具有球对称性或可转化为球对称形式的积分区域。此外采样点的选择和权重计算需要根据具体问题进行优化否则可能导致积分精度下降。无迹卡尔曼滤波UKF非线性处理的新突破Sigma 点的神奇力量无迹卡尔曼滤波UKF是一种针对非线性系统的高效滤波方法其核心在于通过选择一组 Sigma 点来近似非线性函数的概率分布。与 EKF 不同UKF 不依赖于线性化近似而是直接对非线性函数进行处理。具体来说UKF 根据状态变量的均值和协方差精心选择一组 Sigma 点这些点能够更准确地捕捉非线性函数的特性。然后将这些 Sigma 点通过非线性函数进行传播并对传播后的点进行加权计算从而得到状态估计和协方差更新。例如在一个二维非线性系统中UKF 通过选择合适的 Sigma 点能够更精确地描述系统状态的概率分布避免了 EKF 线性化带来的误差。与 EKF 的性能较量与 EKF 相比UKF 在处理非线性问题上具有显著优势。由于避免了线性化过程UKF 通常能够提供更高的估计精度尤其是在非线性较强的系统中。同时UKF 对模型误差具有更好的鲁棒性即使系统模型存在一定偏差仍能保持较好的估计性能。然而UKF 的计算量相对较大。在高维状态空间中Sigma 点的数量和计算量会迅速增加这对计算资源提出了较高要求。复杂场景的广泛应用UKF 在众多复杂场景中得到了广泛应用。在机器人视觉导航领域相机成像模型是非线性的UKF 能够有效处理图像信息与机器人位姿之间的非线性关系实现精确的定位和导航。在生物医学信号处理中生物系统的状态变化往往呈现非线性UKF 可用于对生物信号进行滤波和状态估计如对心脏跳动信号的分析帮助医生更准确地了解心脏状态。2. 运行效果展示3. 部分代码呈现%% This software is distributed under the GNU General Public% Licence (version 2 or later); please refer to the file% Licence.txt, included with the software, for details.keep_trajectory 0;silent 0;% Number of steps to advance at a time in animations.steps 2;% Handles to measurement model function and its derivativesh_func bot_h;dh_dx_func bot_dh_dx;d2h_dx2_func bot_d2h_dx2;% Create a bit curved trajectory and angle% measurements from two sensorsS1 [-1;-2];S2 [1;1];sd 0.05;dt 0.01;if ~keep_trajectory% Accelerations for the objecta zeros(1,500);a(1,50:100) pi/2/51/dt 0.01*randn(1,51);a(1,200:250) pi/2/51/dt 0.01*randn(1,51);a(1,350:400) pi/2/51/dt 0.01*randn(1,51);x [0;0;1;0];t 0;X [];Y [];T [];for i1:500F [0 0 1 0;...0 0 0 1;...0 0 0 a(i);...0 0 -a(i) 0];x expm(F*dt)*x;y1 atan2(x(2)-S1(2), x(1)-S1(1)) sd * randn;y2 atan2(x(2)-S2(2), x(1)-S2(1)) sd * randn;t t dt;X [X x];T [T t];Y [Y [y1;y2]];endend% Uncomment if you want to plot and print the measurements%plot(1:size(Y,2),Y(1,:),.,1:size(Y,2),Y(2,:),.);%legend(Sensor 1, Sensor 2);%title(Measurements from sensors in radians);%print -dpsc bot_demo_measurements.ps%% Initialize EKF to values%% x 0% y 0,% dx/dt 0% dy/dt 0%% with great uncertainty in velocity%M [0;0;0;0];P diag([4 4 4 4]);R sd^2;if ~silentder_check(h_func, dh_dx_func, 1, M, S1);der_check(h_func, dh_dx_func, 1, M, S2);endqx 0.1;qy 0.1;F [0 0 1 0;0 0 0 1;0 0 0 0;0 0 0 0];[A,Q] lti_disc(F,[],diag([0 0 qx qy]),dt);clc;clf;disp([In this demonstration we track a moving object with two sensors, ,...which gives only bearings of the object with respect to sensors position.,...The state of the system is estimated with EKF.])disp( );fprintf(Running EKF...)%% Track and animate%MM zeros(size(M,1),size(Y,2));PP zeros(size(M,1),size(M,1),size(Y,2));ME zeros(size(M,1),1);for k1:size(Y,2)% Track with EKF[M,P] ekf_predict1(M,P,A,Q);[M,P] ekf_update1(M,P,Y(:,k),dh_dx_func,R*eye(2),h_func,[],[S1 S2]);MM(:,k) M;PP(:,:,k) P;ME(k) P(1,1) P(2,2);endekf_rmse sqrt(mean((X(1,:)-MM(1,:)).^2(X(2,:)-MM(2,:)).^2));fprintf(Done!\n)disp( );disp([The filtering results are now displayed sequentially. ,...Notice how the estimates gets more accurate when the filter gets on the right track. ,...The green ellipse around the current estimate (little blue circle) reflects the filters ,...confidence intervals of the position estimate.]);disp( );disp(push any key to proceed);% Plot the filtered estimates sequentiallyif ~silentM MM(:,1);P PP(:,:,1);EST M;tt (0:0.01:1)*2*pi;cc repmat(M(1:2),1,length(tt)) ...2*chol(P(1:2,1:2))*[cos(tt);sin(tt)];h plot(X(1,:),X(2,:),r-,...M(1),M(2),bo,...EST(1,:),EST(2,:),b--,...cc(1,:),cc(2,:),g-,...S1(1),S1(2),k--,...S1(1),S1(2),k^,...S2(1),S2(2),k--,...S2(1),S2(2),k^);legend(Location,NorthWest,Real trajectory,Current estimate,Estimated trajectory,...Confidence interval,Measurements from sensors,Positions of the sensors);title(Bearings Only Tracking with EKF.)axis([-1.5 1.5 -2.5 1.5]);EST [];for k1:steps:size(Y,2)M MM(:,k);P PP(:,:,k);EST MM(:,1:k);% Confidence ellipsecc repmat(M(1:2),1,length(tt)) ...2*chol(P(1:2,1:2))*[cos(tt);sin(tt)];% Measurement directionslen 2.5;dx1 len*cos(Y(1,k));dy1 len*sin(Y(1,k));dx2 len*cos(Y(2,k));dy2 len*sin(Y(2,k));% Update graphicsset(h(2),xdata,M(1)); set(h(2),ydata,M(2));set(h(3),xdata,EST(1,:)); set(h(3),ydata,EST(2,:));set(h(4),xdata,cc(1,:)); set(h(4),ydata,cc(2,:));set(h(5),xdata,[S1(1);S1(1)dx1]); set(h(5),ydata,[S1(2);S1(2)dy1]);set(h(7),xdata,[S2(1);S2(1)dx2]); set(h(7),ydata,[S2(2);S2(2)dy2]);pauseendendclc;disp([In this demonstration we track a moving object with two sensors, ,...which gives only bearings of the object with respect to sensors position.,...The state of the system is estimated with EKF.])disp( );fprintf(Running EKF...Done!\n)disp( );disp([The filtering results are now displayed sequentially. ,...Notice how the estimates gets more accurate when the filter gets on the right track. ,...The green ellipse around the current estimate (little blue circle) reflects the filters ,...confidence intervals of the position estimate.]);disp( );disp(push any key to smooth the estimates with ERTS and ETF);if (~silent) pause; end;clc;fprintf(Smoothing with ERTS and ETF...);%% Smoother 1%[SM1,SP1] erts_smooth1(MM,PP,A,Q);eks_rmse1 sqrt(mean((X(1,:)-SM1(1,:)).^2(X(2,:)-SM1(2,:)).^2));ME1 squeeze(SP1(1,1,:)SP1(2,2,:));%% Smoother 2%[SM2,SP2] etf_smooth1(MM,PP,Y,A,Q,[],[],[],...dh_dx_func,R*eye(2),h_func,[],[S1 S2]);eks_rmse2 sqrt(mean((X(1,:)-SM2(1,:)).^2(X(2,:)-SM2(2,:)).^2));ME2 squeeze(SP2(1,1,:)SP2(2,2,:));fprintf(Done!\n)disp( );disp([Smoothing results of ERTS are now displayed sequentially. ,...Notice how the confidence ellipse gets even smaller now.]);disp( );disp(push any key to proceed);% Plot the smoothed estimates sequentiallyif ~silentM SM1(:,1);P SP1(:,:,1);EST M;cc repmat(M(1:2),1,length(tt)) ...2*chol(P(1:2,1:2))*[cos(tt);sin(tt)];h plot(X(1,:),X(2,:),r-,...M(1),M(2),o,...EST(1,:),EST(2,:),--,...cc(1,:),cc(2,:),g-,...MM(1,:),MM(2,:),b--,...S1(1),S1(2),k^,S2(1),S2(2),k^);legend(Location,NorthWest,Real trajectory,Current estimate,Smoothed trajectory,...Confidence interval,Filter estimate,Positions of the sensors);title(Bearings Only Tracking with ERTS.)axis([-1.5 1.5 -2.5 1.5]);EST [];for ksize(Y,2):-steps:1M SM1(:,k);P SP1(:,:,k);EST SM1(:,end:-1:k);% Confidence ellipsecc repmat(M(1:2),1,length(tt)) ...2*chol(P(1:2,1:2))*[cos(tt);sin(tt)];set(h(2),xdata,M(1)); set(h(2),ydata,M(2));set(h(3),xdata,EST(1,:)); set(h(3),ydata,EST(2,:));set(h(4),xdata,cc(1,:)); set(h(4),ydata,cc(2,:));pause;endenddisp( )disp(push any key to display all estimates together)if (~silent) pause; end;clc;disp(All estimates are now displayed.)%% Plot all the estimates together%if ~silentplot(X(1,:),X(2,:),k-,...MM(1,:),MM(2,:),b--,...SM1(1,:),SM1(2,:),r-.,...SM2(1,:),SM2(2,:),g-.,...S1(1),S1(2),k^,S2(1),S2(2),k^);axis([-1.5 1.5 -2.5 1]);legend(Real trajectory,...EKF1 estimate,...ERTS estimate,...ETF estimate,...Positions of sensors,...Location, NorthWest);title(Filtering and smoothing result with 1st order EKF);axis([-1.5 1.5 -2.5 1.5]);% Uncomment if you want to save an image%print -dpsc bot_demo_ekf1.psend% Print RMSEdisp( );disp(RMS errors:);fprintf(EKF-RMSE %.3f [%.3f]\n,ekf_rmse,sqrt(mean(ME)));fprintf(EKS-RMSE1 %.4f [%.4f]\n,eks_rmse1,sqrt(mean(ME1)));fprintf(EKS-RMSE2 %.4f [%.4f]\n,eks_rmse2,sqrt(mean(ME2)));4. 参考文献更多免费数学建模和仿真教程关注领取如果觉得内容不错那就请分享和点个“在看”呗
