EKF在跟踪制导中的应用

EKF在跟踪制导中的应用2015552065李鑫摘要：介绍了Kalman滤波理论的背景，概括Kalman滤波理论与Wiener滤波理论的特点和Kalman滤波理论的优势。分析了Kalman滤波理论基本原理，并在线性Kalman滤波的基础上针对非线性系统，阐述扩展Kalman滤波（即EKF）的原理。结合军事领域中寻的制导系统，以目标的位移、速度、加速度为状态量，目标的方位角为观测量，构建系统状态方程和观测方程，完成了EKF在目标跟踪制导算法，并在MATLAB环境下仿真，仿真结果表明，滤波估计较好的跟踪了目标，轨迹趋于一致。关键词Kalman滤波扩展Kalman滤波跟踪制导1、引言滤波理论就是在对系统可观测信号进行测量的基础上，根据一定的滤波准则，采取某种统计量最优方法，对系统状态进行估计的理论和方法。所谓最优滤波或最优估计是指在最小方差意义下的最优滤波或估计，经典的滤波理论包括Wiener滤波理论和Kalman滤波理论。Wiener滤波方法是由控制论创始人N.Wiener在20世纪40年代初由于研究炮火控制系统的需要提出的。它是一种频域滤波方法，它的基本工具是平稳随机谱的分解。其缺点和局限性是要求信号为平稳随机过程，要求存储全部历史数据。滤波器是非递推的，计算量和存储量大，难以在工程上实现。采用频域设计法是造成Wiener滤波器设计困难的根本原因。因此人们逐渐转向寻求在时域内直接设计最优滤波器的方法。Kalman在20世纪60年代提出了Kalman滤波方法。Kalman滤波方法是一种时域方法，它把状态空间的概念引入随机估计理论中，把信号过程视为白噪声作用下的一个线性系统输出，用状态方程来描述这种输入-输出关系，估计过程中利用系统状态方程、观测方程、和白噪声激励，即系统过程噪声和观测噪声，它们的统计特性形成滤波算法。2、Kalman滤波算法基本原理考虑用如下状态空间模型描述的动态系统𝑿(𝑘)=𝜱𝑿(𝑘−1)+𝜞𝑾(𝑘)𝒁(𝑘)=𝑯𝑿(𝑘)+𝑽(𝑘)式中，𝑘为离散时间，系统在时刻𝑘的状态为𝑿(𝑘)∈𝑅𝑛；𝒁(𝑘)∈𝑅𝑚为对应状态的观测信号；𝑾(𝑘)∈𝑅𝑟为输入的白噪声；𝑽(𝑘)∈𝑅𝑚为观测噪声。称式第一个式子为状态方程，称第二个式子为观测方程。称𝜱为状态转移矩阵，𝜞为噪声驱动矩阵，𝑯为观测矩阵。假设𝑾(𝑘)和𝑽(𝑘)是均值为零、方差阵各为𝑸和𝑹的不相关白噪声以及初始状态𝑿(0)不相关于𝑾(𝑘)和𝑽(𝑘)。Kalman滤波问题是：基于观测信号{𝒁(1)，𝒁(2)，𝒁(3)，⋯，𝒁(𝑘)}，求状态𝑿(𝑗)的线性最小方差估计值𝑿̂(𝑗|𝑘)，它极小化性能指标𝐽=E[(𝑿(𝑗)−𝑿̂(𝑗|𝑘)T)(𝑿(𝑗)−𝑿̂(𝑗|𝑘))]下面直接给出Kalman滤波器递推公式状态一步预测：𝑿̂(𝑘|𝑘−1)=𝜱𝑿̂(𝑘−1)一步预测协方差：𝑷(𝑘|𝑘−1)=Φ𝑷(𝑘−1)𝜱T+𝜞𝑸𝜞T滤波增益：K(𝑘)=𝑷(𝑘|𝑘−1)𝑯T(𝑯𝑷(𝑘|𝑘−1)𝑯T+𝑅)−1状态更新：𝑿̂(𝑘)=𝑿̂(𝑘|𝑘−1)+𝑲(𝑘)(𝒁(𝑘)−𝑯𝑿̂(𝑘|𝑘−1))协方差阵更新：𝑷(𝑘)=(𝑰−𝑲(𝑘)𝑯)𝑷(𝑘|𝑘−1)在一个滤波周期内，从Kalman滤波在使用系统信息和观测信息来看，Kalman滤波具有两个明显的信息更新过程：时间更新和观测更新。从时间推移来看，状态一步预测方程和一步预测协方差方程将时间从𝑘−1时刻推进至𝑘时刻，描述了Kalman滤波的时间更新过程，其余各式用来计算对时间更新值的修正量。3、扩展Kalman滤波前文给出的Kalman滤波能够在线性高斯模型的条件下，对能够获得较好的滤波效果。但是，实际系统总是存在着不同程度的非线性，典型的非线性函数关系包括平方关系、对数关系、指数关系、三角函数关系等。并且其非线性因素不能忽略，必须建立适用于非线性系统的滤波算法。扩展Kalman滤波建立在线性Kalman滤波的基础上，其核心思想是，将非线性函数展开成Taylor级数并略去二阶及以上的项，得到一个近似线性化的模型，然后应用Kalman滤波完成对目标的估计处理。将离散非线性系统表示为𝑿(𝑘)=𝒇[𝑘−1，𝑿(𝑘−1)]+𝜞𝑾(𝑘)𝒁(𝑘)=𝒉[𝑘，𝑿(𝑘)]+𝑽(𝑘)同Kalman滤波基本方程相比，可将非线性函数𝒇和𝒉围绕滤波值𝑿̂(𝑘)做一阶Taylor展开，即在线性化的方程中，状态转移矩阵𝜱和观测矩阵𝑯由𝒇和𝒉的雅克比矩阵代替𝜱=𝜕𝒇𝜕𝑿=[𝜕𝑓1𝜕𝑥1𝜕𝑓1𝜕𝑥2⋯𝜕𝑓1𝜕𝑥𝑛𝜕𝑓2𝜕𝑥1𝜕𝑓2𝜕𝑥2⋯𝜕𝑓2𝜕𝑥𝑛⋮⋮⋮⋮𝜕𝑓𝑛𝜕𝑥1𝜕𝑓𝑛𝜕𝑥2⋯𝜕𝑓𝑛𝜕𝑥𝑛]𝑯=𝜕𝒉𝜕𝑿=[𝜕ℎ1𝜕𝑥1𝜕ℎ1𝜕𝑥2⋯𝜕ℎ1𝜕𝑥𝑛𝜕ℎ2𝜕𝑥1𝜕ℎ2𝜕𝑥2⋯𝜕ℎ2𝜕𝑥𝑛⋮⋮⋮⋮𝜕ℎ𝑛𝜕𝑥1𝜕ℎ𝑛𝜕𝑥2⋯𝜕ℎ𝑛𝜕𝑥𝑛]4、跟踪制导的EKF模型建立三维平面𝑥−𝑦−𝑧内运动的导弹M，其在某一时刻k的位置、速度、和加速度用矢量表示为𝑿(𝑘)=[𝑟𝑥(𝑘)𝑟𝑦(𝑘)𝑟𝑧(𝑘)𝑣𝑥(𝑘)𝑣𝑦(𝑘)𝑣𝑧(𝑘)𝑎𝑥(𝑘)𝑎𝑦(𝑘)𝑎𝑧(𝑘)]T同时假设三个𝑥−𝑦−𝑧方向上具有加性系统噪声𝑾(𝑘)，则在笛卡尔坐标系下导弹的运动方程为𝑿(𝑘)=𝒇𝒌(𝑿(𝑘−1)，𝑾(𝑘))通常情况下，上述方程为线性的，即能表示为以下形式。𝑿(𝑘)=𝜱𝑿(𝑘−1)+𝜞𝑼(𝑘)+𝑾(𝑘)式中,𝜱=[𝑰3∆𝑡𝑰31𝜆2(𝑒−𝜆∆𝑡+𝜆∆𝑡−1)𝟎3𝑰31𝜆(1−𝑒−𝜆∆𝑡)𝟎3𝟎3𝑒−𝜆∆𝑡𝑰3]𝜞=[−(∆𝑡2/2)−∆𝑡𝑰3𝟎3]考虑带有观测器的导弹M，对移动目标进行观测，导弹和目标的相对位置如图4.1所示。图4.1导弹与目标的相对位置示意图导弹对目标采用方位角观测，测量量为俯仰角和水平方向偏角，在笛卡尔坐标系下，观测方程为𝒁(𝑘)=𝒉[𝑿(𝑘)]+𝑽(𝑘)式中，𝒉[𝑿(𝑘)]=[tan−1𝑟𝑦(𝑘)√𝑟𝑥𝟐(𝑘)+𝑟𝑧𝟐(𝑘)tan−1−𝑟𝑥(𝑘)𝑟𝑧(𝑘)]因观测方程非线性，对其关于𝑥(𝑘)求偏导，得到雅克比矩阵𝑯=𝜕𝒉[𝑿(𝑘)]𝜕𝑿5、跟踪制导的EKF算法仿真根据导弹跟踪的特点，在MATLAB环境下设计EKF算法。设定采样时间，仿真时长分别为∆𝑡=0.01𝑠，𝑡=3.7s设定导弹的初始状态𝒙(0)=[350015001000−1100−150−50101010]T设定EKF滤波器估计的初始化状态𝒆𝒙(0)=[30001200800−950−100−100000]T设定过程噪声方差𝜎2=0.1𝑸=[104×𝑰6𝟎6×3𝟎3×6𝜎2×𝑰3×3]设定EKF滤波器初始协方差阵：𝑷0=[104×𝑰6𝟎6×3𝟎3×6102×𝑰3]运行程序，得到目标的跟踪轨迹如图5.1所示。跟踪结果表明，滤波估计状态较好地跟踪了目标，轨迹趋于一致。图5.1导弹EKF跟踪轨迹与真实轨迹同样计算EKF滤波后的状态值与目标真实值状态之间的偏差，可以得到位置偏差图，速度偏差图和加速度偏差图，如图5.2~图5.4所示。位移偏差收敛性较好，速度偏差收敛性较差，加速度则稳定在特定的值内。图5.2导弹EKF跟踪位移偏差图5.3导弹EKF跟踪速度偏差图5.4导弹EKF跟踪加速度偏差6、参考文献[1]李刚,孙江,樊浩.寻的制导综述[J].飞航导弹,2007,12:44-47.[2]李全运,宋建梅.捷联寻的制导系统滤波器设计[J].兵工学报,2005,02:215-219.[3]鲍虎,刘隆和.寻的制导体制及其对抗方法[J].航天电子对抗,1999,01:7-11.[4]沈晔青,龚华军,熊琰.精确制导系统中的目标跟踪与卡尔曼滤波[J].红外与激光工程,2006,S1:105-110.[5]杨丹.卡尔曼滤波器设计及其应用研究[D].湘潭大学,2014.[6]彭丁聪.卡尔曼滤波的基本原理及应用[J].软件导刊,2009,11:32-34.[7]杨宏,李亚安,李国辉.一种改进扩展卡尔曼滤波新方法[J].计算机工程与应用,2010,19:18-20.