机动目标跟踪_张泽兵_05040056
一种精确跟踪机动目标的滤波算法的研究
一种精确跟踪机动目标的滤波算法的研究近年来，机动目标跟踪技术受到越来越多的关注。
准确地跟踪机动目标可以在自动控制、安全监控、机器视觉、生物机器人等领域发挥重要作用。
然而，在复杂环境中，滤波算法仍然存在一些问题，例如误差的累积和高精度处理的困难。
为了解决这些问题，本文提出了一种新的滤波算法，它可以有效地跟踪机动目标。
首先，本文介绍了有关机动目标跟踪技术的相关背景知识，例如常用的滤波算法，如卡尔曼滤波和粒子滤波等。
其次，基于这些基本概念，本文提出了一种新型的滤波算法，它考虑了位置空间模型、速度模型和动力学模型，能够有效地减少模型量和估计误差。
该方法在模拟实验中得到了验证，证明其有效性。
本文还分析了该算法在复杂环境中的表现，包括较小的静态干扰、较大的动态干扰以及传感器偏置等情况。
实验结果表明，本文提出的滤波算法在复杂环境中可以有效地跟踪机动目标，且可以较好地抵消外部的扰动。
综上所述，本文提出的一种滤波算法可以有效地跟踪机动目标，且在复杂环境中具有较高的精度。
因此，本研究侧重于研究机动目标跟踪技术，对今后的发展具有重要意义。
近年来，随着人工智能、模式识别和计算机视觉等技术的发展，跟踪机动目标的技术也受到了越来越多的关注。
特别是在安全监控、自动控制、机器视觉、生物机器人等领域，准确跟踪机动目标的技术显得尤为重要。
然而，在现实环境中，机动目标跟踪仍然存在一些挑战，例如跟踪数据的不确定性以及外部扰动的影响等。
为了解决这些问题，本文提出了一种新的滤波算法，它可以有效地跟踪机动目标，而且在复杂环境中能够较好地抵消外部的扰动。
本文提出的滤波算法利用了位置空间模型、速度模型和动力学模型，能够有效地减少模型量和估计误差。
与传统滤波算法相比，新方法具有低延迟、高精度和高可靠性等优点。
在模拟实验中，该算法也得到了验证，并有效地减少了误差。
综上所述，本文提出的一种滤波算法可以有效地跟踪机动目标，且在复杂环境中具有较高的精度。
一种跟踪机动目标的新方法
一种跟踪机动目标的新方法
张伯彦
【期刊名称】《系统工程与电子技术》
【年(卷),期】1990(000)001
【总页数】1页(P45)
【作者】张伯彦
【作者单位】不详;不详
【正文语种】中文
【中图分类】TN953
【相关文献】
1.一种新的基于机动检测的机动目标跟踪算法 [J], 关成斌;王国宏;王晓博
2.一种基于跟踪微分器的机动目标跟踪算法 [J], 王晓卫;周启煌;王仲
3.一种改进机动目标跟踪算法跟踪精度分析 [J], 王勇;张逊;梁二伟
4.机动目标跟踪中一种机动频率和方差自适应滤波算法 [J], 钱广华;李颖;骆荣剑
5.跟踪机动目标的一种新方法的研究 [J], 张伯彦;袁曾任
因版权原因，仅展示原文概要，查看原文内容请购买。
一种精确跟踪机动目标的滤波算法的研究
一种精确跟踪机动目标的滤波算法的研究近年来，机动目标滤波技术受到了越来越多的关注，其中，滤波技术是一种有效的方法，用于精确跟踪机动目标。
滤波技术不仅能在机动目标的运动轨迹上建立连续的位置估计，还能处理机动目标的噪声和冗余信息，用以提高跟踪精度。
因此，本文旨在探索一种精确跟踪机动目标的滤波算法，以提高跟踪性能。
首先，本文分析了机动目标滤波技术的原理和步骤，结合实际应用场景，提出一种新型精确跟踪机动目标的滤波算法。
该算法将机动目标运动动态模型与观测量模型结合起来，实现鲁棒性较强的机动目标的运动估计。
其次，通过对滤波算法的数学模型描述，把跟踪过程抽象为状态估计问题，用最小二乘法等算法解决状态估计问题，在一定程度上实现了精准跟踪。
此外，本文还提出了多传感器融合的技术，用一种复合传感器的方法来改进精度，从而实现更佳的机动目标跟踪效果。
此外，本文还提出了一种用于评价机动目标跟踪性能的指标体系，包括跟踪准确度指标和可靠度指标。
首先，利用精度与准确度指标，测量滤波算法的跟踪精度；其次，通过可靠度指标，衡量滤波算法的可用性，以评价机动目标跟踪的性能。
最后，本文以一个设备跟踪仿真实验为例，测试提出的滤波算法对机动目标运动跟踪的性能。
实验结果表明，本文提出的滤波算法能够有效地跟踪机动目标，滤波算法的精度和可靠性也得到了有效的改善。
因此，本文提出的滤波算法在机动目标跟踪中具有很好的应用前景。
综上所述，本文研究了一种精确跟踪机动目标的滤波算法，通过模型建立、状态估计和多传感器融合等技术，实现了鲁棒的机动目标跟踪性能。
实验结果表明，本文提出的滤波算法在机动目标跟踪中具有良好的应用前景。
同时，本文也为今后更好地改进滤波算法提供了理论参考。
一种精确跟踪机动目标的滤波算法的研究
一种精确跟踪机动目标的滤波算法的研究近年来，随着信息技术的发展，跟踪机动目标的技术受到了越来越多的关注和重视。
机动目标跟踪技术对于导航、恒定行驶、探测、跟踪、地面机动目标的检测等领域都有重要的意义，但仍面临着诸多技术问题。
滤波一直是解决跟踪机动目标技术中最核心的技术，目前国内外在机动目标跟踪领域开展了大量的研究，但仍然存在很多不可解决的技术问题，因此研究一种精确跟踪机动目标的滤波算法具有重要的现实意义。
机动目标跟踪的滤波算法是机动目标跟踪中最为重要的技术之一，一般采用Kalman滤波算法来计算机动目标的状态信息。
但由于测量噪声与运动噪声的影响，使得机动目标跟踪的滤波精度受到了影响，容易出现漂移、抖动等现象。
因此，研究一种精确跟踪机动目标的滤波算法，具有重要的意义。
为了减少测量噪声和运动噪声对机动目标滤波算法的影响，我们提出了一种新的滤波算法，该算法考虑了机动目标的运动特性，并采用卡尔曼滤波的基本流程，实现了精确跟踪机动目标的功能。
该滤波器首先根据机动目标的运动特性建立机动目标状态方程，然后计算卡尔曼增益，最后将测量值按照线性矩阵形式融入滤波算法中。
实验结果表明，该滤波器平均偏离率较低，可有效降低测量噪声和运动噪声的影响，从而提高机动目标滤波的精度。
该滤波算法可有效提高机动目标跟踪的精度，为无人机的自主定位和航空航行提供了可靠的参考。
此外，该算法可以应用于其他有机动目标跟踪需求的领域，如无人车辆、自动化机器人等。
综上所述，本文研究了一种精确跟踪机动目标的滤波算法，该算法考虑了机动目标的运动特性，可以有效抑制其受到的测量噪声和运动噪声的影响，从而提高机动目标滤波的精度。
该滤波算法可以为机动目标跟踪技术提供可靠的参考，也可以应用于其他有机动目标跟踪需求的领域。
一种精确跟踪机动目标的滤波算法的研究
一种精确跟踪机动目标的滤波算法的研究自从出现了遥控技术以来，随着机器人研究的进展，越来越多的工作需要遥控。
所谓机器人，就是可以代替人的操作完成各种工作的机器。
由于其外观酷似人类，因此人们把它们称之为“仿人机器人”。
其中有一类是具有自主导航能力的机器人，这类机器人往往带有武器系统，有的甚至可以使用威力强大的核弹头。
那么，就目前的研究水平而言，自主导航无疑是对机器人最基本的要求。
目标机动是指载体在空间或时间上的移动，即载体沿[gPARAGRAPH3]线上的滑动；目标机动目标也可理解为自动导引下的动态目标跟踪。
目标机动不但要求载体机动性能好，还要求目标机动性能好，这两者必须同时达到最佳才行。
目标机动具有动态和非动态两个特征，通常使用的跟踪算法都是针对非动态目标机动，采用的方法有：（ 1）我们可以用一个示意图来说明这个问题：如果假设A为已知的运动目标， B为未知的运动目标。
当A运动时， A与B之间存在一个时间差T，这样载体的位置就变化为： A →B，载体的运动距离为：dt。
如果B始终跟踪A，则A →B→T为最优解。
然而，载体实际上并不总能精确地瞄准A，常会发生较大的误差，即载体可能偏离A的位置，造成目标机动。
为此，就要求载体机动和目标机动都要小。
目标机动小，也就是说载体到目标的直线距离短，显然不利于精确定位；载体机动小，也就是说载体的运动速度低，在这种情况下，导引头将难以跟踪。
目标机动小的解决办法是利用载体运动的惯性和旋转对其施加控制，其方法是：设载体在给定方向上的位移为P，且P>0，载体沿给定方向的运动速度为u，设载体的实际速度为v。
只要求得载体的加速度，则可解出其速度，也就是P和v。
如果目标是一辆汽车，就可以设计汽车的运动过程为：车由静止开始加速，驶过某点后停止，再经过另一点继续加速，又返回原来的起点。
2、分类问题方法（ 1）数学模型方法根据目标机动状态的复杂性及影响机动效果的诸多因素，建立合适的数学模型，将目标机动问题归结为一些简单规则的组合问题，从而找到相应的滤波方法。
《野外环境下机动目标识别与跟踪技术的研究》
《野外环境下机动目标识别与跟踪技术的研究》一、引言随着现代社会对于实时性及高效率要求的提升，野外环境下的机动目标识别与跟踪技术在众多领域内均发挥了重要作用，如军事侦察、无人驾驶、野生动物研究等。
本文旨在探讨野外环境下机动目标识别与跟踪技术的现状、挑战及未来发展趋势。
二、机动目标识别与跟踪技术的重要性机动目标识别与跟踪技术在多个领域均有广泛的应用。
在军事上，能实现实时侦察与监视，增强战场感知能力。
在民用方面，则用于无人驾驶车辆的路径规划、野生动物的行为研究等。
而在野外复杂环境下，此项技术的可靠性及准确性的重要性不言而喻。
三、野外环境的挑战野外环境多变，包括气候条件、地形地貌、植被覆盖等多种因素，这些因素给机动目标的识别与跟踪带来了极大的挑战。
例如，在恶劣的气候条件下，设备的稳定性和运行可靠性是识别与跟踪技术的关键因素；而在复杂的山地和丛林环境中，目标的隐匿性以及障碍物的影响增加了识别与跟踪的难度。
四、机动目标识别技术机动目标识别技术主要包括基于图像处理的目标检测、基于雷达的目标识别以及基于多传感器融合的识别方法。
在野外环境下，多传感器融合的识别方法更具优势，它可以通过整合不同传感器的信息，提高在复杂环境下的目标识别能力。
五、机动目标跟踪技术机动目标跟踪技术主要依赖于各种算法和模型，如卡尔曼滤波、粒子滤波以及深度学习模型等。
这些技术能够根据目标的运动轨迹和历史信息，预测目标的未来位置，并实现持续的跟踪。
在野外环境下，由于环境的复杂性和目标的多样性，需要结合多种算法和技术手段来提高跟踪的准确性和稳定性。
六、技术难点与解决方案在野外环境下进行机动目标的识别与跟踪时，存在诸多技术难点。
如传感器易受环境影响导致数据失真、目标运动的不确定性以及多目标间的相互干扰等。
针对这些问题，研究者们提出了多种解决方案。
例如，通过优化传感器设计和数据处理算法来提高数据的准确性和可靠性；通过引入更先进的算法模型来提高目标预测和跟踪的准确性；以及通过多目标轨迹融合技术来减少多目标间的相互干扰等。
一种精确跟踪机动目标的滤波算法的研究
一种精确跟踪机动目标的滤波算法的研究近年来，随着智能技术的发展，机动目标的跟踪受到了越来越多的关注。
但是，在现实世界中，机动目标的跟踪是一个非常困难的问题。
这是因为随着机动目标的移动，有关的数据很容易受到外部干扰，而且大部分的数据都是噪声。
此外，机动目标的跟踪还需要考虑到物理模型，因为机动目标的运动和物理环境息息相关，比如机动目标受到重力，受到风力的影响等等。
因此，跟踪机动目标的算法必须考虑到这些因素，以便获得更好的跟踪精度。
基于上述的关于机动目标的跟踪的问题，滤波算法是一种有效的解决方法。
它可以有效地通过分析多个测量值，把噪声影响过滤掉，然后生成更准确的估计值。
而且，滤波算法可以利用前一时刻机动目标的状态，从而准确地预测下一时刻机动目标的状态。
因此，滤波算法是非常有效的机动目标跟踪技术。
本文的目的是研究一种新的滤波算法，以精确跟踪机动目标。
首先，给出了一个基于Kalman滤波的机动目标跟踪模型，其核心部分是基于状态估计的滤波算法。
然后，详细分析了滤波算法的原理，以及如何利用状态估计变换技术在机动目标跟踪问题中进行测量值的改进，最后给出了一个实验结果，证明这种新的滤波算法在实际机动目标跟踪问题中具有较高的精度。
总的来说，文中提出的滤波算法可以有效地跟踪机动目标，并获得更高的精度。
它可以有效地过滤噪声，利用前一时刻机动目标的状态，准确地预测下一时刻机动目标的状态，从而获得更准确的估计值。
实验结果也证明，这种新的滤波算法在实际机动目标跟踪问题中具有较高的精度。
因此，在跟踪机动目标方面，本研究提出的滤波算法可以有效地改善跟踪精度，并且可以在实际应用中发挥重要作用。
研究者将继续改进和优化滤波算法，以获得更精确的跟踪结果。
机动目标跟踪的一种防发散RBUKF算法
第39卷第1期2017年2月指輝控制与仿真Command Control &SimulationYol. 39 No. 1Feb.2017文章编号：1673-3819( 2017) 01-0041-03机动目标跟踪的一种防发散R B U K F算法张园，钟志通，刘淑波，初俊博(海军大连舰艇学院，辽宁大连116018)摘要：针对观测方程为非线性，状态方程为线性，且噪声为加性情况下的机动目标跟踪问题，应用Rao- Blackwellked UKF (R B U K F)算法滤波并对其进行了防发散处理，得到机动目标跟踪的一种基于防发散RBUKF(A D-R B U K F)算法。
对二维机动目标跟踪的仿真结果显示，本算法的跟踪效果明显优于其它两种常用的非线性滤波方法——基本的U K F算法和S P P F算法，仿真结果表明该算法是一种跟踪精度高，且适合工程应用的非线性滤波 方法。
关键词：机动目标跟踪；Rao-Blackw— d UKF ( R B U K F);防发散中图分类号:TJ765. 1;E917文献标志码：A D0I:10.3969/j.iw n.1673-3819.2017.01.009A kind of Anti-divergent RBUKF Algorithm of Maneuvering Target TrackingZHANG Yuan，ZHONG Zhi-tong，LIU Su-bo,CHU Jun-bo(Dalian Naval Academy，Dalian 116018，China)Abstract ：Devoted to the problem of maneuvering target tracking under nonlinear obser^^ation，linear state and added noise，a kind of algorithm based on anti-divergent RBUKF ( AD-RBU KF) of maneuvering target tracking is developed，which usesRao-Blackw-ellised UKF ( R B U K F) for filter and anti-divergent work. T w o -dimensional maneuvering target tracking simulation results show that，the tracking effect of this algorithm is clearly better than the other two commonly used nonlinear filtering method of the basic UKF algorithm and the SPPF algorithm. And the simulation results show that this algorithm is of high tracking accuracy，and it is suitable for nonlinear filtering method for engineering application.Key words：maneuvering target tracking；Rao-Blackwellised UKF (R B U K F) ；anti-divergent机动目标跟踪是军事和民用领域的一个基本问 题，可用于导弹的精确定位打击、卫星监控系统、防卫 系统、海岸监视系统、核武器运载系统以及海上或空中 交通管制等。
一种机动目标状态方程的多项式预测模型及跟踪方法
一种机动目标状态方程的多项式预测模型及跟踪方法
近年来，互联网技术发展迅速，广泛应用于多个领域，在特定应用领域中也取得了极大的成功。
其中，一种称为“机动目标状态方程的多项式预测模型及跟踪方法”的有效技术，可以有效地追踪人物、机器人和其他移动目标物体，使运动过程可被快速掌控。
这种模型可以有效地使用多项式预测方程来跟踪物体的运动状态，以精确地控制和优化实时跟踪的效果和准确性。
使用此模型的多种关键部件，能够有效地捕捉物体运动状态之间的关联性，进而精准预测物体运动的走向，从而大大提高物体追踪的速度和效率。
在使用此预测模型时，可以采用基于模式识别的优化技术，有效地识别出目标物体的开始及结束位置，并对移动目标物体的行驶状态进行实时监控。
此外，随着强大数据处理功能的不断完善，兼顾更多跟踪状态的精细特性，将能够更加准确地估计物体的位置和移动状态。
“机动目标状态方程的多项式预测模型及跟踪方法”是目前技术发展程度最高的跟踪技术，它将有效简化许多复杂而又繁琐的工作，为后续更先进的智能追踪技术提供可靠的基础平台。
相信未来，此预测模型将会更加智能化，加快训练曲线，以提升跟踪模型的准确性和可靠性，实现实现更高层次的动态追踪管控。
- 1、下载文档前请自行甄别文档内容的完整性，平台不提供额外的编辑、内容补充、找答案等附加服务。
- 2、"仅部分预览"的文档,不可在线预览部分如存在完整性等问题,可反馈申请退款(可完整预览的文档不适用该条件!)。
- 3、如文档侵犯您的权益，请联系客服反馈,我们会尽快为您处理(人工客服工作时间：9:00-18:30)。
(1) 算法描述在该问题中，机动目标经历三个阶段：初始匀速直线阶段、匀速圆周运动阶段、返回匀速直线阶段。
在此过程中线速度大小v 保持不变。
图1如图1所示：θ为轨迹切线与横轴正向夹角。
在初始匀速阶段和返回匀速直线阶段θ分别为0，π。
在匀速圆周运动阶段θ从0到π均匀变化。
由2/mv r ma =及/w v r =得/w a v =（w 为角速度）所以容易得到：/wt vt r θ==cos x v v θ=sin y v v θ=-状态变量[,,,,]T x y s r r v a θ=状态方程为：[][1][1]cos x x x x r n r n v T r n v T θ=-+=-+[][1][1]sin y y y x r n r n v T r n v T θ=-+=--[][1][1]an n wT n T vθθθ=-+=-+[][1][]v v n v n u n =-+[][1][]a a n a n u n =-+即[]([1])[]s n a s n u n =-+,其中[][0,0,0,[],[]]'v a u n u n u n =所以状态转换矩阵为211,0,sin ,cos ,0210,1,cos ,sin ,020,0,1,,/0,0,0,1,00,0,0,0,1v T T v T T a a A T T vs v θθθθ⎡⎤-⎢⎥⎢⎥⎢⎥--⎢⎥⎢⎥∂⎢⎥==-∂⎢⎥⎢⎥⎢⎥⎢⎥⎢⎥⎢⎥⎣⎦假设[]v u n 和[]a u n 不相关，方差分别为2v σ、2aσ，因此得驱动噪声220,0,0,0,00,0,0,0,00,0,0,,00,0,0,0,v a Q σσ⎡⎤⎢⎥⎢⎥=⎢⎥⎢⎥⎢⎥⎣⎦观测矢量[][][][]x y r n x n w n r n ⎡⎤=+⎢⎥⎣⎦其中[][][]x y u n w n u n ⎡⎤=⎢⎥⎣⎦所以观测矩阵1,0,0,0,00,1,0,0,0H ⎡⎤=⎢⎥⎣⎦，假设[]x u n 、[]y u n 不相关且方差分别为2x σ和2y σ观测噪声22,00,x y C σσ⎡⎤=⎢⎥⎢⎥⎣⎦总结上述式子，得到这个问题的扩展卡尔曼滤波方程为ˆˆ[|1]([1|1])sn n a s n n -=--[|1][1|1]T M n n AM n n A Q -=--+ 1[][|1]([|1])T T K n M n n H C HM n n H -=-+-ˆˆˆ[|][|1][]([][|1])sn n s n n K n x n Hs n n =-+-- [|]([])[|1]M n n I K n H M n n =--(2) 滤波初始化由问题描述，x σ=y σ=100，初始位置为[-20000;0] ,也可以有一定的偏离，经过一段时间后依然能够收敛。
不妨取[-10000;2000].初始速度v=300m/s ，θ=0，a=0 所以初始状态为[1][10000,0,0,300,0]T s -=-对驱动噪声，取v σ=30；a σ=2； (3) 仿真分析采用MATLAB 编写仿真程序，利用蒙特卡罗方法对跟踪滤波器进行仿真分析，次数为10次。
以下给出仿真图和结果分析。
滤波轨迹和滤波均值轨迹： 见图2。
图2 而当M=1时可以看出性能比之M=10时较差，尤其表现在拐弯处。
X、Y方向滤波估计误差均值及误差标准差：见图3、4。
图3图4 附Matlab源代码：function main()%%@project:飞行器跟踪模拟%@author:fantasy%@date:2006.5.10%@descripition:主函数%产生观测数据total=3*60;%总的时间长度global T;%采样周期T=1;N=total/T;%数据长度a=20;var_rx=100;var_ry=100;X=[];%观测数据X_ideal=[];%理想数据for i=1:N[rx,ry]=track(i*T,20);X_ideal=[X_ideal,[rx;ry]];rx=rx+var_rx*randn(1,1);ry=ry+var_ry*rand(1,1);X=[X,[rx;ry]];endX_filter=zeros(size(X));%滤波后数据X_mean=X_filter;%蒙特卡洛平均数据Error_var=zeros(size(X));M=10;%蒙特卡洛仿真次数for iCount=1:MX_filter=Trace(X);X_mean=X_mean+X_filter;Error_var=Error_var+(X_ideal-X_filter).^2;endX_mean=X_mean/M;Error_var=Error_var/M;Error_mean=X_ideal-X_mean;%误差均值Error_var=sqrt(Error_var-Error_mean.^2);plot(X_ideal(1,:),X_ideal(2,:),X(1,:),X(2,:),X_mean(1,:),X_mean(2,:)); axis equal;legend('理想轨迹','观测轨迹','滤波轨迹');figure;k=1:N;subplot(2,1,1),plot(k,Error_mean(1,:));title('x方向误差均值');xlabel('采样次数'),ylabel('误差均值(米)');subplot(2,1,2),plot(k,Error_var(1,:));title('x方向误差标准值');xlabel('采样次数'),ylabel('误差标准值值(米)');figure;subplot(2,1,1),plot(k,Error_mean(2,:));title('y方向误差均值');xlabel('采样次数'),ylabel('误差均值(米)');subplot(2,1,2),plot(k,Error_var(2,:));title('y方向误差标准值');xlabel('采样次数'),ylabel('误差标准值值(米)');%@subfunction%理想航迹方程function [x,y]=track(t,a)%parameter:% t:时间% x:横轴位移% y:纵轴位移% a:转弯处加速度% r:初始位置% v:初始速度r=[-20000,0]';v=300;w=a/v;%角速度t1=-r(1)/v;t2=t1+pi/w;D=v^2/a*2;%圆周运动直径if t<=0[x,y]=r0';elseif t>0&&t<=t1x=r(1)+v*t;y=r(2);elseif t>t1&&t<=t2angel=(t-t1)*w;x=D/2*sin(angel);y=-D*(sin(angel/2))^2; elsex=-v*(t-t2);y=-D;endfunction R=Trace(X)%@project:飞行器跟踪模拟%@author:fantasy%@date:2006.5.10%@parameter:% X:观测数据% R:输出坐标%观测时间间隔global T;%观测矩阵H=[1,0,0,0,0;...0,1,0,0,0];%位移测量误差var_rx=100;var_ry=100;var_rx2=var_rx^2;var_ry2=var_ry^2;%观测噪声协方差矩阵C=[var_rx2,0;...0,var_ry2];%驱动噪声协方差矩阵var_v=30;var_a=5;var_v2=var_v^2;var_a2=var_a^2;Q=zeros(5,5);Q(4,4)=var_v2;Q(5,5)=var_a2;%初始状态s0=[-10000,2000,0,300,0]';%Kalman滤波跟踪N=size(X,2);%观测数据长度s=s0;a=@traverse;M=Q;Xplus=[];%修正后的航迹for icurrent=1:N[s,M]=Karlman(s,M,X(:,icurrent),a,Q,C,H);Xplus=[Xplus;(s(1:2))'];end%可视化数据% plot(X(1,:),X(2,:),'r.');% axis('equal');% hold on;% plot(Xplus(:,1),Xplus(:,2));R=Xplus';function s_estimate=traverse(s)%状态方程%s=[rx,ry,theta,v,a]global T;s_estimate=zeros(5,1);s_estimate(1)=s(1)+s(4)*cos(s(3))*T;s_estimate(2)=s(2)-s(4)*sin(s(3))*T;s_estimate(3)=s(3)+(s(5)/s(4))*T;s_estimate(4)=s(4);s_estimate(5)=s(5);function [s,M]=Karlman(s_forward,M_forward,X,a,Q,C,H) %卡尔曼滤波%@author:fantasy%@date:2006.5.15%参数说明% X--观测数据矢量% A--状态矩阵% Q--驱动噪声协方差% C--观测噪声协方差% h--观测方程句柄% s--输出数据矢量% s_foward--前次输出矢量% M--前次预测矩阵global T;%预测s=feval(a,s_forward);%状态转换矩阵% A=[1,0,-s(4)/2*sin(s(3)/2)*T,cos(s(3)/2)*T,0;... % 0,1,-s(4)/2*cos(s(3)/2)*T,-sin(s(3)/2)*T,0;... % 0,0,1,-s(5)*T/(s(4))^2,T/s(4);...% 0,0,0,1,0;...% 0,0,0,0,1];A=[1,0,-s(4)*sin(s(3))*T,cos(s(3))*T,0;...0,1,-s(4)*cos(s(3))*T,-sin(s(3))*T,0;...0,0,1,-s(5)*T/(s(4))^2,T/s(4);...0,0,0,1,0;...0,0,0,0,1];%最小预测MSE矩阵M=M_forward;M=A*M*A'+Q;%卡尔曼增益矩阵K=M*H'*inv(C+H*M*H');%修正s=s+K*(X-H*s);%最小MSE矩阵M=M-K*H*M;。