CN108009358A - 基于imm_ukf的三维制导律辨识滤波方法 - Google Patents

基于imm_ukf的三维制导律辨识滤波方法 Download PDF

Info

Publication number
CN108009358A
CN108009358A CN201711251352.4A CN201711251352A CN108009358A CN 108009358 A CN108009358 A CN 108009358A CN 201711251352 A CN201711251352 A CN 201711251352A CN 108009358 A CN108009358 A CN 108009358A
Authority
CN
China
Prior art keywords
mrow
msub
mtd
msubsup
mover
Prior art date
Legal status (The legal status is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the status listed.)
Granted
Application number
CN201711251352.4A
Other languages
English (en)
Other versions
CN108009358B (zh
Inventor
邹昕光
周荻
Current Assignee (The listed assignees may be inaccurate. Google has not performed a legal analysis and makes no representation or warranty as to the accuracy of the list.)
Harbin Institute of Technology
Original Assignee
Harbin Institute of Technology
Priority date (The priority date is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the date listed.)
Filing date
Publication date
Application filed by Harbin Institute of Technology filed Critical Harbin Institute of Technology
Priority to CN201711251352.4A priority Critical patent/CN108009358B/zh
Publication of CN108009358A publication Critical patent/CN108009358A/zh
Application granted granted Critical
Publication of CN108009358B publication Critical patent/CN108009358B/zh
Active legal-status Critical Current
Anticipated expiration legal-status Critical

Links

Classifications

    • G—PHYSICS
    • G06—COMPUTING; CALCULATING OR COUNTING
    • G06F—ELECTRIC DIGITAL DATA PROCESSING
    • G06F30/00—Computer-aided design [CAD]
    • G06F30/20—Design optimisation, verification or simulation

Landscapes

  • Engineering & Computer Science (AREA)
  • Physics & Mathematics (AREA)
  • Theoretical Computer Science (AREA)
  • Computer Hardware Design (AREA)
  • Evolutionary Computation (AREA)
  • Geometry (AREA)
  • General Engineering & Computer Science (AREA)
  • General Physics & Mathematics (AREA)
  • Pharmaceuticals Containing Other Organic And Inorganic Compounds (AREA)
  • Navigation (AREA)

Abstract

基于IMM_UKF的三维制导律辨识滤波方法，本发明涉及基于IMM_UKF的制导律辨识滤波方法。本发明的目的是为了解决现有技术中敌方拦截导弹pursuer会在不同的制导阶段采用不同的导航常数，导致运动模型发生变化，使MMAE滤波方法在这种情况下制导精度降低的问题。过程为：一、建立evader和pursuer的相对运动方程；二、基于步骤一建立的evader和pursuer的相对运动方程，建立相对evader的pursuer运动模型；三、基于步骤二建立的相对evader的pursuer运动模型，设计evader上的IMM滤波器。本发明用于导弹主动防御制导率涉及领域。

Description

基于IMM_UKF的三维制导律辨识滤波方法
技术领域
本发明涉及基于IMM_UKF的制导律辨识滤波方法。
背景技术
飞行器在飞行过程中会遭遇拦截导弹的拦截。为了有效的逃避拦截，飞行器需要估计拦截导弹的运动状态，例如两者之间的相对位置，相对速度，拦截导弹的加速度以及其制导律相关的信息。利用这些信息来确定自己逃逸的策略。
为了估计拦截导弹的运动状态，首先需要对其运动进行数学建模，然后设计Kalman滤波器去跟踪它的状态。对于Kalman滤波器而言，模型的准确性十分关键，因为滤波器对模型误差比较敏感。在拦截导弹机动未知的情况下，对其运动进行精确建模很困难。一般采用一些通用模型，例如常速模型CV，常加速模型CA，singer模型和当前统计模型来建模。这些模型一般将拦截导弹的加速度或者加速度的导数看作是一个随机变量。但是对于运动受制导律约束的拦截导弹而言，其加速度很难被看作是一个随机变量，有必要在对拦截导弹运动建模的过程中考虑其制导律信息。一般而言，大多数制导律都可以采用PN制导律来近似，因此我们在拦截导弹的跟踪问题中引入了PN制导律信息。假设拦截导弹采用PN制导律，但是PN制导律的导航比未知而且可能发生变化，我们只知道它的取值范围。多模型滤波器技术适用于解决该问题。例如我们可以使用一个可以覆盖PN制导律导航比可能取值范围的模型集来构造一个多模型滤波器。
在制导律辨识滤波研究中，传统方法多使用多模型自适应估计器(MMAE)对拦截导弹的状态进行估计，但是MMAE是一个静态的多模型滤波器，不能随着拦截导弹运动模型的跳变而自适应改变模型的概率权值，其性能在拦截导弹的运动模型发生变化的时候性能将会受到严重影响。
在很多实际问题中，PN制导律的导航比在整个制导期间会发生变化。例如拦截导弹可能会根据距离其目标的远近程度使用不同的导航比。当拦截导弹和目标之间的距离较远时，其导引头的热噪声较大，因此导航比适合设置为稍小的数值，这样避免噪声引起制导指令发生大的波动。当相对距离较近时热噪声较小，此时导航比适合设置为较大的值，加速视线角速度的收敛，达到成功拦截目标的目的。导航比发生变化后，整个的拦截导弹的运动模型也将发生变化，导致模型失配，此时无论是Kalman滤波器还是MMAE滤波器，其性能都将下降。
在PN制导律约束下的拦截导弹的运动模型是一个非线性模型。可以采用UKF等滤波器来对非线性系统进行近似。EKF滤波器需要计算Jacobians矩阵，在一些场合中Jacobian矩阵计算比较复杂。
发明内容
本发明的目的是为了解决现有技术中敌方拦截导弹pursuer会在不同的制导阶段采用不同的导航常数，导致运动模型发生变化，使MMAE滤波方法在这种情况下制导精度降低的问题，而提出基于IMM_UKF的三维制导律辨识滤波方法。
基于IMM_UKF的三维制导律辨识滤波方法具体过程为：
步骤一、建立evader和pursuer的相对运动方程；
所述pursuer为敌方拦截导弹，evader为目标飞行器；
步骤二、基于步骤一建立的evader和pursuer的相对运动方程，建立相对evader的pursuer运动模型；
步骤三、基于步骤二建立的相对evader的pursuer运动模型，设计evader上的IMM滤波器；
所述IMM滤波器为交互式多模型滤波器。
本发明的有益效果为：本发明采用基于IMM_UKF的三维制导律辨识滤波方法，对pursuer采用PN制导律的运动模型进行了更为准确的建模。使用IMM滤波器可以很好的解决pursuer在制导过程中导航常数变化，导致运动模型发生变化的情况。另外本发明采用三维制导律辨识滤波，进一步提高了制导精度。
IMM滤波器中的元滤波器彼此之间会互相影响，最终所有元滤波器的滤波结果被融合为目标的状态估计值和状态协方差矩阵。在PN制导律约束下的拦截导弹的运动模型是一个非线性模型。UKF滤波器更为简单，滤波的性能更好。UKF不需要计算Jacobians矩阵，它使用被称为unscented transform的方法对状态的一阶和二阶矩近似。在高度非线性的情况下，UKF的性能会高于EKF。本发明使用UKF作为IMM滤波器的元滤波器。
pursuer相对于evader的距离向量在场景惯性坐标系下沿x轴的分量rx估计误差的均值最大为0.21638m，均值最小为-1.06749m；
pursuer相对于evader的相对速度向量在场景惯性坐标系下沿x轴的分量vx估计误差的均值最大为0.272416m/s，均值最小为-0.0938242m/s；
pursuer相对于evader在场景惯性坐标系x轴下的加速度apx估计误差的均值最大为0.196056m/s2，均值最小为-0.0113468m/s2；
pursuer相对于evader的距离向量在场景惯性坐标系下沿y轴的分量ry估计误差的均值最大为1.23704m，均值最小为-0.536286m；
pursuer相对于evader的相对速度向量在场景惯性坐标系下沿y轴的分量vy估计误差的均值最大为3.54621m/s，均值最小为-3.98085m/s；
pursuer相对于evader在场景惯性坐标系y轴下的加速度apy估计误差的均值最大为10.6247m/s2，均值最小为-26.4115m/s2；
pursuer相对于evader的距离向量在场景惯性坐标系下沿z轴的分量rz估计误差的均值最大为0.982833m，均值最小为-1.59334m；
pursuer相对于evader的相对速度向量在场景惯性坐标系下沿z轴的分量vz估计误差的均值最大为3.62914m/s，均值最小为-6.31078m/s；
pursuer相对于evader在场景惯性坐标系z轴下的加速度apz的均值估计误差最大为9.56203m/s2，均值最小为-25.0015m/s2；
如图4所示，pursuer相对于evader的距离向量在场景惯性坐标系下沿x轴的分量rx的估计均方根误差最大为3.59922m，最小为0.16909m；
如图5所示，pursuer相对于evader的相对速度向量在场景惯性坐标系下沿x轴的分量vx的估计均方根误差最大为10.3354m/s，最小为0.117284m/s；
如图6所示，pursuer相对于evader在场景惯性坐标系x轴下的加速度apx的估计均方根误差最大为0.284444m/s2，最小为0.00585257m/s2；
如图7所示，pursuer相对于evader的距离向量在场景惯性坐标系下沿y轴的分量ry的估计均方根误差最大为3.56157m，最小为0.152854m；
如图8所示，pursuer相对于evaderr的相对速度向量在场景惯性坐标系下沿y轴的分量vy的估计均方根误差最大为10.1596m/s，最小为0.135485m/s；
如图9所示，pursuer相对于evader在场景惯性坐标系y轴下的加速度apy的估计均方根误差最大为26.4168m/s2，最小为0.16814m/s2；
如图10所示，pursuer相对于evader的距离向量在场景惯性坐标系下沿z轴的分量rz的估计均方根误差最大为3.92254m，最小为0.0898603m；
如图11所示，pursuer相对于evader的相对速度向量在场景惯性坐标系下沿z轴的分量vz的估计均方根误差最大为9.60577m/s，最小为0.106509m/s；
如图12所示，pursuer相对于evader在场景惯性坐标系z轴下的加速度apz的估计均方根误差最大为25.0086m/s2，最小为0.132461m/s2。
附图说明
图1为evader和pursuer的相对运动关系图，pursuer为敌方拦截导弹，evader为目标飞行器；
图2为模型概率(测量无噪声evader执行正弦机动)图；
图3为模型概率(测量有噪声evader执行正弦机动)图；
图4为IMM滤波器对位置x轴分量的估计均方根误差(evader执行正弦机动)图；
图5为IMM滤波器对速度x轴分量的估计均方根误差(evader执行正弦机动)图；
图6为IMM滤波器对加速度x轴分量的估计均方根误差(evader执行正弦机动)图；
图7为IMM滤波器对位置y轴分量的估计均方根误差(evader执行正弦机动)图；
图8为IMM滤波器对速度y轴分量的估计均方根误差(evader执行正弦机动)图；
图9为IMM滤波器对加速度y轴分量的估计均方根误差(evader执行正弦机动)图；
图10为IMM滤波器对位置z轴分量的估计均方根误差(evader执行正弦机动)图；
图11为IMM滤波器对速度z轴分量的估计均方根误差(evader执行正弦机动)图；
图12为IMM滤波器对加速度z轴分量的估计均方根误差(evader执行正弦机动)图。
具体实施方式
具体实施方式一：本实施方式的基于IMM_UKF的三维制导律辨识滤波方法具体过程为：
步骤一、建立evader和pursuer的相对运动方程；
所述pursuer为敌方拦截导弹，evader为目标飞行器；
步骤二、基于步骤一建立的evader和pursuer的相对运动方程，建立相对evader的pursuer运动模型；
步骤三、基于步骤二建立的相对evader的pursuer运动模型，设计evader上的IMM滤波器；
所述IMM滤波器为交互式多模型滤波器。
具体实施方式二：本实施方式与具体实施方式一不同的是：所述步骤一中建立evader和pursuer的相对运动方程；具体过程为：
场景惯性坐标系OXY定义为目标飞行器evader的初始视线坐标系；图1显示了evader和pursuer在平面上的相对运动关系。
目标飞行器evader和敌方拦截导弹pursuer分别用e和p表示；
evader惯性坐标系与场景惯性坐标系重合；pursuer惯性坐标系原点和y轴与场景坐标系重合，pursuer惯性坐标系x轴方向和场景惯性坐标系x轴方向相反；
Ve为evader的速度向量，ae为evader的加速度向量，qep,ε和qep,β分别为evader到pursuer的视线倾角和偏角；类似的，Vp为pursuer的速度向量，ap为pursuer的加速度向量，qpe,ε和qpe,β为pursuer到evader的视线倾角和偏角；rpe为pursuer到evader的距离；
建立evader和pursuer解耦和的相对运动方程为：
其中qpe,ε和qpe,β是pursuer对evader的视线倾角和偏角；和是pursuer对evader的视线倾角速度和偏角速度；为pursuer对evader的视线倾角加速度，为pursuer对evader的视线偏角加速度，aeε为evader的加速度在pursuer到evader视线坐标系下沿y轴的分量，aeβ为evader的加速度在pursuer到evader视线坐标系下沿z轴的分量，rpe为pursuer到evader的距离，为pursuer和evader之间的接近速度；apε和apβ分别为pursuer加速度ap在pursuer对evader的视线坐标系y轴和z轴的投影。
其它步骤及参数与具体实施方式一相同。
具体实施方式三：本实施方式与具体实施方式一或二不同的是：所述步骤二中基于步骤一建立的evader和pursuer的相对运动方程，建立相对evader的pursuer运动模型；具体过程为：
假设pursuer采用PN制导律拦截evader。称在这种情况下pursuer的运动为PN运动模型。设相对于evader的pursuer状态向量为
x＝[rx,ry,rz,vx,vy,vz,apx,apy,apz]T (3)
其中rx为pursuer相对于evader的距离向量在场景惯性坐标系下沿x轴的分量，ry为pursuer相对于evader的距离向量在场景惯性坐标系下沿y轴的分量，rz为pursuer相对于evader的距离向量在场景惯性坐标系下沿z轴的分量；vx为pursuer相对于evader的相对速度向量在场景惯性坐标系下沿x轴的分量，vy为pursuer相对于evader的相对速度向量在场景惯性坐标系下沿y轴的分量，vz为pursuer相对于evader的相对速度向量在场景惯性坐标系下沿z轴的分量；apx为pursuer在场景惯性坐标系x轴下的加速度，apy为pursuer在场景惯性坐标系y轴下的加速度，apz为pursuer在场景惯性坐标系z轴下的加速度，T为转置；
即
rx＝xp-xe ry＝yp-ye rz＝zp-ze (4)
vx＝vpx-vex vy＝vpy-vey vz＝vpz-vez (5)
其中向量[xp,yp,zp]T和[xe,ye,ze]T分别为pursuer和evader在场景惯性坐标系下的位置，向量[vpx,vpy,vpz]T和[vex,vey,vez]T分别为pursuer和evader在场景惯性坐标系下的速度。最后；
相对于evader的pursuer运动模型的一般形式为
其中为rx的一阶导数，为ry的一阶导数，为rz的一阶导数，为vx的一阶导数，为vy的一阶导数，为vz的一阶导数，为apx的一阶导数，为apy的一阶导数，为apz的一阶导数；其中向量[aex,aey,aez]T为evader在场景惯性坐标系下的加速度；向量[ap4x,ap4y,ap4z]T为pursuer制导指令在场景惯性下的投影；参数τ为时间常数；
设ap4为pursuer在其视线系下的制导指令。考虑到在末制导阶段，导弹的推力发动机关闭，因此导弹沿视线系纵向加速度为零。当pursuer使用PN制导律时，ap4的表达式为
其中常数Nε和Nβ为导航常数；
向量ap4和[ap4x,ap4y,ap4z]T的关系为
其中矩阵C40为坐标转换矩阵，将pursuer视线坐标系的向量转换为pursuer惯性坐标系下分量，值为
矩阵Cps为坐标系转换矩阵，将pursuer惯性坐标系转换为场景惯性坐标系；根据之前提到的pursuer惯性坐标系和场景惯性坐标系的关系，Cps值为
pursuer和evader的视线角和视线角速度关系为
其中qep,ε和qep,β为evader到pursuer的视线倾角和偏角；和为evader到pursuer的视线倾角角速度和偏角速度；
另外为evader相对于pursuer的接近速度，为pursuer相对于evader的接近速度，两者相等，即
视线角和距离为
分别对式(13)和(14)取时间的导数，得
另外
为evader和pursuer之间的距离；ρ为中间变量；
其中
将式(17)和式(18)代入式(9)，得
将式(11)-(18)代入式(8)，得
其中
η＝(rxvx+ryvy+rzvz)(-rxvz+rzvx) (23)
为中间变量；
将式(21)代入式(6)，得
式(24)就是从evader角度看关于pursuer的PN运动模型；
假设evader可以测量pursuer在场景惯性坐标系下相对自己的位置，则式(24)的测量矩阵为
具体实施方式四：本实施方式与具体实施方式一至三之一不同的是：所述时间常数τ值为0.1。
具体实施方式五：本实施方式与具体实施方式一至四之一不同的是：所述步骤三中基于步骤二建立的相对evader的pursuer运动模型，设计evader上的IMM滤波器；具体过程为：
滤波器设计
多模型滤波器适合解决目标运动模式未知或变化的状态估计问题。其中IMM滤波器和固定多模型滤波器(例如MMAE)相比可以解决模型切换问题。考虑到pursuer在制导过程中会切换导航常数，导致制导模式发生变化。因此IMM滤波器适合本发明所设计的问题。先介绍evader上的IMM滤波器设计，IMM滤波器中的元滤波器采用UKF。
evader上的IMM滤波器设计
IMM滤波器具有能估计变结构系统状态的能力，是一种最常使用的多模型滤波器。和固定结构的多模型滤波器不同，在每个滤波迭代处理中，元滤波器的输入进行混合。和其他多模型滤波器相同，IMM滤波器有一个模型集，包含了被跟踪目标所有可能的运动模型。每个运动模型对应于一个滤波器(例如KF，EKF或UKF等)，该滤波器称为元滤波器。因此滤波器的模型集对应一组元滤波器。IMM滤波器的估计为其元滤波器估计值的模型概率加权和。模型概率表征各个模型和目标实际运动模型的匹配程度。
假设IMM滤波器的模型集为M＝{M1,…,Mn}，其中Mj代表第j个pursuer运动模型；模型集中的模型都是PN运动模型，其导航常数各不相同(见式(24))；模型Mj的先验概率为 代表初始时刻pursuer运动模型为Mj的事件；1≤j≤n；n取值为正整数，为模型集中模型的数量；
设为已知的pursuer运动模型转换概率；pursuer运动模型Mj对应的状态方程和测量方程为
其中fj(k,xk-1)，hj(k,xk)，和分别为pursuer运动模型Mj在k时刻下的状态转换函数，测量函数，过程噪声和测量噪声；为在k时刻pursuer运动模型为Mj的事件；为在k-1时刻pursuer运动模型为Mi的事件；1≤i≤n；
IMM滤波算法是一个迭代式的算法，每次迭代处理可以分为三个步骤：混合，滤波和融合。现对k时刻的迭代处理描述如下
1)、求解k时刻IMM滤波算法混合输入和混合协方差矩阵
2)、对1)得到的和进行滤波，得到pursuer运动模型Mj在k时刻的似然函数(滤波)；
3)、基于2)得到k时刻IMM滤波算法最终状态估计和协方差矩阵(融合)。
具体实施方式六：本实施方式与具体实施方式一至五之一不同的是：所述1)中求解k时刻IMM滤波算法混合输入和混合协方差矩阵；具体过程为
第一步，混合
混合是指在一次迭代中每个元滤波器根据上一次迭代的估计状态和协方差矩阵来计算当前的元滤波器的初始状态和协方差矩阵。用表示pursuer运动模型Mi到Mj的转换概率，其计算方法为
其中为k-1时刻pursuer运动模型为Mi的概率，是归一化因子，Mi代表第i个pursuer运动模型，1≤i≤n；
最终k时刻IMM滤波算法混合输入和混合协方差矩阵计算公式为
其中和分别代表混合输入和混合协方差矩阵。为pursuer运动模型Mi对应的Kalman滤波器在k-1时刻的状态估计，为pursuer运动模型Mi对应的Kalman滤波器在k-1时刻的状态估计协方差矩阵。
具体实施方式七：本实施方式与具体实施方式一至六之一不同的是：所述2)中对1)得到的和进行滤波，得到pursuer运动模型Mj在k时刻的似然函数(滤波)；公式为：
讨论的IMM滤波器采用UKF滤波器作为其元滤波器。采用1)得到的和作为输入，每个UKF元滤波器都执行一次滤波迭代。滤波处理又可以分为两个子步骤：状态预测和状态更新
其中函数UKFp(·)为状态预测函数，UKFu(·)为状态更新函数；为pursuer运动模型Mj在k时刻的状态预测，为pursuer运动模型Mj对应的Kalman滤波器在k时刻的状态预测协方差矩阵，为pursuer运动模型Mj对应的Kalman滤波器在k-1时刻的系统(公式26和公式18)过程噪声矩阵，为pursuer运动模型Mj对应的Kalman滤波器在k时刻的状态估计，为pursuer运动模型Mj对应的Kalman滤波器在k时刻的状态估计协方差矩阵，为pursuer运动模型Mj对应的Kalman滤波器在k时刻的新息向量，为pursuer运动模型Mj对应的Kalman滤波器在k时刻的新息的协方差矩阵，zk为k时刻的测量值，为pursuer运动模型Mj在k时刻的测量噪声矩阵；
模型Mj对应的Kalman滤波器在k时刻的似然函数为
其中为pursuer运动模型Mj对应的Kalman滤波器在k时刻下的新息向量，为新息的协方差矩阵；为正态分布概率密度函数。
具体实施方式八：本实施方式与具体实施方式一至七之一不同的是：所述3)中基于2)得到k时刻IMM滤波算法最终状态估计和协方差矩阵(融合)；
最终状态估计和协方差矩阵为
为模型Mj对应的Kalman滤波器在k时刻的状态估计，为模型Mj对应的Kalman滤波器在k时刻的状态估计协方差矩阵；为IMM滤波器的状态估计；Pk,kIMM滤波器在k时刻的状态估计协方差矩阵；
为k时刻pursuer运动模型为模型Mj的概率，表达式如下
c为归一化常数。
实施例一：
本实施例基于IMM_UKF的三维制导律辨识滤波方法具体过程为：
仿真结果及讨论
仿真场景中包含两个飞行器evader和pursuer。我们首先给出了仿真环境配置和初始条件。接着对本文提出的基于IMM滤波器的制导律行为辨识方法性能进行仿真和评估。我们在evader不机动或执行正弦机动两种场景下进行仿真。
仿真环境配置及初始条件
evader和pursuer的初始距离为100km。将evader的初始视线坐标系作为场景惯性坐标系。在场景惯性系下，evader初始位置为[0,0,0]Tm，pursuer的初始位置为[105,0,0]Tm。在Monte Carlo仿真中，pursuer每次仿真中具体的初始位置设置为上述位置的每一个分量加上一个在[-1000,1000]Tm范围均匀分布的随机偏移。pursuer的初始速度为2500m/s。evader的初始飞行倾角γe为-5deg，飞行偏角为5deg。evader和pursuer的最大加速度分别为2g和6g。假设evader能测量pursuer的相对位置。测量噪声为零均值方差为25m2的高斯白噪声。
pursuer采用PN制导律拦截evader，由于之前提到的原因，导航常数会随着距离而切换。初始的导航函数设置为3，当pursuer和evader的相对距离小于初始距离的一半时，导航函数切换为5。
滤波器性能
假设pursuer使用PN制导律来拦截evader，并且这个信息为evader所知。但是evader不知道PN制导律的导航常数。针对这个问题，evader采用了所提的IMM滤波器来估计pursuer的状态。该滤波器包含5个PN模型：PN3，PN3.5，PN4，PN4.5和PN5，模型导航常数分别是3，3.5，4，4.5和5。该IMM滤波器对pursuer的相对位置，相对速度和加速度进行估计。
IMM滤波器状态及其元滤波器状态中相对位置分量设置为当前的测量值。初始相对速度分量可由相对位置的微分求得。然而通过已经是估计值的速度的微分来求pursuer的加速度，其精度无法满足需要，因此pursuer的初始加速度的估计值设置为0，也就是意味着evader对pursuer初始的加速度没有先验值。
IMM滤波器中的元滤波器过程噪声矩阵Qe设置为
以上全部滤波器的测量噪声矩阵R初始值设置为
随着距离的变化，测量噪声也会随之变化，仿真会根据测量噪声的变化动态调整测量噪声矩阵。
由于evader对pursuer的导航常数没有先验信息，因此IMM滤波器的模型先验概率设置为
μ＝[0.2 0.2 0.2 0.2 0.2] (35)
图2、图3显示了IMM滤波器模型概率在evader执行正弦机动时的结果。之前提到在pursuer与evader之间距离达到初始距离的一半时，pursuer会将导航常数从3切换到5。在仿真算例中，导航常数切换的时间约为6秒。模型PN3的概率很快收敛到1，这说明IMM滤波器在很短的时间里有效的辨识出了pursuer的导航常数。在约为6秒时，pursuer突然切换其导航常数。由于滤波器的暂态效应，之后大约1秒钟IMM滤波器对该事件开始响应并变化模型概率。为了进一步分析IMM滤波器的特性，我们在理想测量环境下重复了上述场景的仿真。由于摒除了噪声的影响，IMM滤波器本身的特性能得到更好的展示。在理想情况下，pursuer切换其导航常数一秒后，模型PN3的概率快速下降。而现在pursuer的导航常数变为5，模型PN5的概率应该很快收敛到1。模型PN5的概率确实上升了，但是其他一些模型的概率也在上升(模型PN4和PN4.5)。对于图2、图3大约2秒后，模型PN5的概率开始占据主导。
IMM滤波器是估计pursuer状态不可或缺的工具。为了进一步验证滤波器的性能，下面针对evader的IMM滤波器进行了Monte Carlo仿真验证。图4-图12显示了在evader执行正弦机动情况下，evader和pursuer的相对位置，相对速度和pursuer加速度估计误差。仿真结果是从200轮的Monte Carlo仿真中得到。除了初始阶段，各个状态的估计误差均方根误差都十分接近于0。之前提到，IMM滤波器的初始状态和真值有相当的误差，滤波器使这些误差快速收敛到0值附近。在制导中段，pursuer切换导航常数导致IMM滤波器产生模型不匹配。由于暂态效应，滤波器需要一段时间选出匹配的模型。因此，在这段时间内，误差偏离零点。但由此产生的误差在IMM选择出匹配模型后快速收敛到0。由此看来，似乎pursuer反evader跟踪的一个策略是将导航常数切换的时机变为在制导后期。这个看似有效的策略其实帮不了pursuer。在制导后期，pursuer对evader的视线角速度已经减少为一个很接近零的值，根据PN制导律公式(24)，其加速度也变得几乎可以忽略不计。这意味着此时导航常数的变化对加速度几乎没有影响。因此，IMM滤波器可以有效地对pursuer状态进行估计。
针对制导律辨识问题提出基于IMM滤波器的制导律辨识方法。该方法可以有效的对PN制导律进行辨识。在导航常数变化的场景中，IMM滤波器也可以很好的适应。仿真结果证实了方法的有效性。
本发明还可有其它多种实施例，在不背离本发明精神及其实质的情况下，本领域技术人员当可根据本发明作出各种相应的改变和变形，但这些相应的改变和变形都应属于本发明所附的权利要求的保护范围。

Claims (8)

1.基于IMM_UKF的三维制导律辨识滤波方法，其特征在于：所述方法具体过程为：
步骤一、建立evader和pursuer的相对运动方程；
所述pursuer为敌方拦截导弹，evader为目标飞行器；
步骤二、基于步骤一建立的evader和pursuer的相对运动方程，建立相对evader的pursuer运动模型；
步骤三、基于步骤二建立的相对evader的pursuer运动模型，设计evader上的IMM滤波器；
所述IMM滤波器为交互式多模型滤波器。
2.根据权利要求1所述基于IMM_UKF的三维制导律辨识滤波方法，其特征在于：所述步骤一中建立evader和pursuer的相对运动方程；具体过程为：
场景惯性坐标系OXY定义为目标飞行器evader的初始视线坐标系；
evader惯性坐标系与场景惯性坐标系重合；pursuer惯性坐标系原点和y轴与场景坐标系重合，pursuer惯性坐标系x轴方向和场景惯性坐标系x轴方向相反；
建立evader和pursuer解耦和的相对运动方程为：
<mrow> <msub> <mover> <mi>q</mi> <mo>&amp;CenterDot;&amp;CenterDot;</mo> </mover> <mrow> <mi>p</mi> <mi>e</mi> <mo>,</mo> <mi>&amp;epsiv;</mi> </mrow> </msub> <mo>=</mo> <mo>-</mo> <mn>2</mn> <mfrac> <msub> <mover> <mi>r</mi> <mo>&amp;CenterDot;</mo> </mover> <mrow> <mi>p</mi> <mi>e</mi> </mrow> </msub> <msub> <mi>r</mi> <mrow> <mi>p</mi> <mi>e</mi> </mrow> </msub> </mfrac> <msub> <mover> <mi>q</mi> <mo>&amp;CenterDot;</mo> </mover> <mrow> <mi>p</mi> <mi>e</mi> <mo>,</mo> <mi>&amp;epsiv;</mi> </mrow> </msub> <mo>+</mo> <mfrac> <mn>1</mn> <msub> <mi>r</mi> <mrow> <mi>p</mi> <mi>e</mi> </mrow> </msub> </mfrac> <msub> <mi>a</mi> <mrow> <mi>e</mi> <mi>&amp;epsiv;</mi> </mrow> </msub> <mo>-</mo> <mfrac> <mn>1</mn> <msub> <mi>r</mi> <mrow> <mi>p</mi> <mi>e</mi> </mrow> </msub> </mfrac> <msub> <mi>a</mi> <mrow> <mi>p</mi> <mi>&amp;epsiv;</mi> </mrow> </msub> <mo>-</mo> <mo>-</mo> <mo>-</mo> <mrow> <mo>(</mo> <mn>1</mn> <mo>)</mo> </mrow> </mrow>
<mrow> <msub> <mover> <mi>q</mi> <mo>&amp;CenterDot;&amp;CenterDot;</mo> </mover> <mrow> <mi>p</mi> <mi>e</mi> <mo>,</mo> <mi>&amp;beta;</mi> </mrow> </msub> <mo>=</mo> <mo>-</mo> <mn>2</mn> <mfrac> <msub> <mover> <mi>r</mi> <mo>&amp;CenterDot;</mo> </mover> <mrow> <mi>p</mi> <mi>e</mi> </mrow> </msub> <msub> <mi>r</mi> <mrow> <mi>p</mi> <mi>e</mi> </mrow> </msub> </mfrac> <msub> <mover> <mi>q</mi> <mo>&amp;CenterDot;</mo> </mover> <mrow> <mi>p</mi> <mi>e</mi> <mo>,</mo> <mi>&amp;beta;</mi> </mrow> </msub> <mo>-</mo> <mfrac> <mn>1</mn> <msub> <mi>r</mi> <mrow> <mi>p</mi> <mi>e</mi> </mrow> </msub> </mfrac> <msub> <mi>a</mi> <mrow> <mi>e</mi> <mi>&amp;beta;</mi> </mrow> </msub> <mo>+</mo> <mfrac> <mn>1</mn> <msub> <mi>r</mi> <mrow> <mi>p</mi> <mi>e</mi> </mrow> </msub> </mfrac> <msub> <mi>a</mi> <mrow> <mi>p</mi> <mi>&amp;beta;</mi> </mrow> </msub> <mo>-</mo> <mo>-</mo> <mo>-</mo> <mrow> <mo>(</mo> <mn>2</mn> <mo>)</mo> </mrow> </mrow>
其中qpe,ε和qpe,β是pursuer对evader的视线倾角和偏角；和是pursuer对evader的视线倾角速度和偏角速度；为pursuer对evader的视线倾角加速度，为pursuer对evader的视线偏角加速度，aeε为evader的加速度在pursuer到evader视线坐标系下沿y轴的分量，aeβ为evader的加速度在pursuer到evader视线坐标系下沿z轴的分量，rpe为pursuer到evader的距离，为pursuer和evader之间的速度；apε和apβ分别为pursuer加速度ap在pursuer对evader的视线坐标系y轴和z轴的投影。
3.根据权利要求2所述基于IMM_UKF的三维制导律辨识滤波方法，其特征在于：所述步骤二中基于步骤一建立的evader和pursuer的相对运动方程，建立相对evader的pursuer运动模型；具体过程为：
设相对于evader的pursuer状态向量为
x＝[rx,ry,rz,vx,vy,vz,apx,apy,apz]T (3)
其中rx为pursuer相对于evader的距离向量在场景惯性坐标系下沿x轴的分量，ry为pursuer相对于evader的距离向量在场景惯性坐标系下沿y轴的分量，rz为pursuer相对于evader的距离向量在场景惯性坐标系下沿z轴的分量；vx为pursuer相对于evader的相对速度向量在场景惯性坐标系下沿x轴的分量，vy为pursuer相对于evader的相对速度向量在场景惯性坐标系下沿y轴的分量，vz为pursuer相对于evader的相对速度向量在场景惯性坐标系下沿z轴的分量；apx为pursuer在场景惯性坐标系x轴下的加速度，apy为pursuer在场景惯性坐标系y轴下的加速度，apz为pursuer在场景惯性坐标系z轴下的加速度，T为转置；
即
rx＝xp-xe ry＝yp-ye rz＝zp-ze (4)
vx＝vpx-vex vy＝vpy-vey vz＝vpz-vez (5)
其中向量[xp,yp,zp]T和[xe,ye,ze]T分别为pursuer和evader在场景惯性坐标系下的位置，向量[vpx,vpy,vpz]T和[vex,vey,vez]T分别为pursuer和evader在场景惯性坐标系下的速度；
相对于evader的pursuer运动模型的一般形式为
<mrow> <mtable> <mtr> <mtd> <mrow> <msub> <mover> <mi>r</mi> <mo>&amp;CenterDot;</mo> </mover> <mi>x</mi> </msub> <mo>=</mo> <msub> <mi>v</mi> <mi>x</mi> </msub> </mrow> </mtd> </mtr> <mtr> <mtd> <mrow> <msub> <mover> <mi>r</mi> <mo>&amp;CenterDot;</mo> </mover> <mi>y</mi> </msub> <mo>=</mo> <msub> <mi>v</mi> <mi>y</mi> </msub> </mrow> </mtd> </mtr> <mtr> <mtd> <mrow> <msub> <mover> <mi>r</mi> <mo>&amp;CenterDot;</mo> </mover> <mi>z</mi> </msub> <mo>=</mo> <msub> <mi>v</mi> <mi>z</mi> </msub> </mrow> </mtd> </mtr> <mtr> <mtd> <mrow> <msub> <mover> <mi>v</mi> <mo>&amp;CenterDot;</mo> </mover> <mi>x</mi> </msub> <mo>=</mo> <msub> <mi>a</mi> <mrow> <mi>p</mi> <mi>x</mi> </mrow> </msub> <mo>-</mo> <msub> <mi>a</mi> <mrow> <mi>e</mi> <mi>x</mi> </mrow> </msub> </mrow> </mtd> </mtr> <mtr> <mtd> <mrow> <msub> <mover> <mi>v</mi> <mo>&amp;CenterDot;</mo> </mover> <mi>y</mi> </msub> <mo>=</mo> <msub> <mi>a</mi> <mrow> <mi>p</mi> <mi>y</mi> </mrow> </msub> <mo>-</mo> <msub> <mi>a</mi> <mrow> <mi>e</mi> <mi>y</mi> </mrow> </msub> </mrow> </mtd> </mtr> <mtr> <mtd> <mrow> <msub> <mover> <mi>v</mi> <mo>&amp;CenterDot;</mo> </mover> <mi>z</mi> </msub> <mo>=</mo> <msub> <mi>a</mi> <mrow> <mi>p</mi> <mi>z</mi> </mrow> </msub> <mo>-</mo> <msub> <mi>a</mi> <mrow> <mi>e</mi> <mi>z</mi> </mrow> </msub> </mrow> </mtd> </mtr> <mtr> <mtd> <mrow> <msub> <mover> <mi>a</mi> <mo>&amp;CenterDot;</mo> </mover> <mrow> <mi>p</mi> <mi>x</mi> </mrow> </msub> <mo>=</mo> <mrow> <mo>(</mo> <msub> <mi>a</mi> <mrow> <mi>p</mi> <mn>4</mn> <mi>x</mi> </mrow> </msub> <mo>-</mo> <msub> <mi>a</mi> <mrow> <mi>p</mi> <mi>x</mi> </mrow> </msub> <mo>)</mo> </mrow> <mo>/</mo> <mi>&amp;tau;</mi> </mrow> </mtd> </mtr> <mtr> <mtd> <mrow> <msub> <mover> <mi>a</mi> <mo>&amp;CenterDot;</mo> </mover> <mrow> <mi>p</mi> <mi>y</mi> </mrow> </msub> <mo>=</mo> <mrow> <mo>(</mo> <msub> <mi>a</mi> <mrow> <mi>p</mi> <mn>4</mn> <mi>y</mi> </mrow> </msub> <mo>-</mo> <msub> <mi>a</mi> <mrow> <mi>p</mi> <mi>y</mi> </mrow> </msub> <mo>)</mo> </mrow> <mo>/</mo> <mi>&amp;tau;</mi> </mrow> </mtd> </mtr> <mtr> <mtd> <mrow> <msub> <mover> <mi>a</mi> <mo>&amp;CenterDot;</mo> </mover> <mrow> <mi>p</mi> <mi>z</mi> </mrow> </msub> <mo>=</mo> <mrow> <mo>(</mo> <msub> <mi>a</mi> <mrow> <mi>p</mi> <mn>4</mn> <mi>z</mi> </mrow> </msub> <mo>-</mo> <msub> <mi>a</mi> <mrow> <mi>p</mi> <mi>z</mi> </mrow> </msub> <mo>)</mo> </mrow> <mo>/</mo> <mi>&amp;tau;</mi> </mrow> </mtd> </mtr> </mtable> <mo>-</mo> <mo>-</mo> <mo>-</mo> <mrow> <mo>(</mo> <mn>6</mn> <mo>)</mo> </mrow> </mrow>
其中为rx的一阶导数，为ry的一阶导数，为rz的一阶导数，为vx的一阶导数，为vy的一阶导数，为vz的一阶导数，为apx的一阶导数，为apy的一阶导数，为apz的一阶导数；其中向量[aex,aey,aez]T为evader在场景惯性坐标系下的加速度；向量[ap4x,ap4y,ap4z]T为pursuer制导指令在场景惯性下的投影；参数τ为时间常数；
设ap4为pursuer在其视线系下的制导指令；
当pursuer使用PN制导律时，ap4的表达式为
<mrow> <msub> <mi>a</mi> <mrow> <mi>p</mi> <mn>4</mn> </mrow> </msub> <mo>=</mo> <mfenced open = "[" close = "]"> <mtable> <mtr> <mtd> <mn>0</mn> </mtd> </mtr> <mtr> <mtd> <mo>-</mo> <msub> <mi>N</mi> <mi>&amp;epsiv;</mi> </msub> <msub> <mover> <mi>r</mi> <mo>&amp;CenterDot;</mo> </mover> <mrow> <mi>p</mi> <mi>e</mi> </mrow> </msub> <msub> <mover> <mi>q</mi> <mo>&amp;CenterDot;</mo> </mover> <mrow> <mi>p</mi> <mi>e</mi> <mo>,</mo> <mi>&amp;epsiv;</mi> </mrow> </msub> </mtd> </mtr> <mtr> <mtd> <mrow> <msub> <mi>N</mi> <mi>&amp;beta;</mi> </msub> <msub> <mover> <mi>r</mi> <mo>&amp;CenterDot;</mo> </mover> <mrow> <mi>p</mi> <mi>e</mi> </mrow> </msub> <msub> <mover> <mi>q</mi> <mo>&amp;CenterDot;</mo> </mover> <mrow> <mi>p</mi> <mi>e</mi> <mo>,</mo> <mi>&amp;beta;</mi> </mrow> </msub> </mrow> </mtd> </mtr> </mtable> </mfenced> <mo>-</mo> <mo>-</mo> <mo>-</mo> <mrow> <mo>(</mo> <mn>7</mn> <mo>)</mo> </mrow> </mrow>
其中常数Nε和Nβ为导航常数，向量ap4和[ap4x,ap4y,ap4z]T的关系为
<mrow> <mfenced open = "[" close = "]"> <mtable> <mtr> <mtd> <msub> <mi>a</mi> <mrow> <mi>p</mi> <mn>4</mn> <mi>x</mi> </mrow> </msub> </mtd> </mtr> <mtr> <mtd> <msub> <mi>a</mi> <mrow> <mi>p</mi> <mn>4</mn> <mi>y</mi> </mrow> </msub> </mtd> </mtr> <mtr> <mtd> <msub> <mi>a</mi> <mrow> <mi>p</mi> <mn>4</mn> <mi>z</mi> </mrow> </msub> </mtd> </mtr> </mtable> </mfenced> <mo>=</mo> <msub> <mi>C</mi> <mrow> <mi>p</mi> <mi>s</mi> </mrow> </msub> <msub> <mi>C</mi> <mn>40</mn> </msub> <msub> <mi>a</mi> <mrow> <mi>p</mi> <mn>4</mn> </mrow> </msub> <mo>-</mo> <mo>-</mo> <mo>-</mo> <mrow> <mo>(</mo> <mn>8</mn> <mo>)</mo> </mrow> </mrow>
其中矩阵C40为坐标转换矩阵，将pursuer视线坐标系的向量转换为pursuer惯性坐标系下分量，值为
<mrow> <msub> <mi>C</mi> <mn>40</mn> </msub> <mo>=</mo> <mfenced open = "[" close = "]"> <mtable> <mtr> <mtd> <mrow> <mi>cos</mi> <mi> </mi> <msub> <mi>q</mi> <mrow> <mi>p</mi> <mi>e</mi> <mo>,</mo> <mi>&amp;epsiv;</mi> </mrow> </msub> <mi>cos</mi> <mi> </mi> <msub> <mi>q</mi> <mrow> <mi>p</mi> <mi>e</mi> <mo>,</mo> <mi>&amp;beta;</mi> </mrow> </msub> </mrow> </mtd> <mtd> <mrow> <mo>-</mo> <mi>sin</mi> <mi> </mi> <msub> <mi>q</mi> <mrow> <mi>p</mi> <mi>e</mi> <mo>,</mo> <mi>&amp;epsiv;</mi> </mrow> </msub> <mi>cos</mi> <mi> </mi> <msub> <mi>q</mi> <mrow> <mi>p</mi> <mi>e</mi> <mo>,</mo> <mi>&amp;beta;</mi> </mrow> </msub> </mrow> </mtd> <mtd> <mrow> <mi>sin</mi> <mi> </mi> <msub> <mi>q</mi> <mrow> <mi>p</mi> <mi>e</mi> <mo>,</mo> <mi>&amp;beta;</mi> </mrow> </msub> </mrow> </mtd> </mtr> <mtr> <mtd> <mrow> <mi>sin</mi> <mi> </mi> <msub> <mi>q</mi> <mrow> <mi>p</mi> <mi>e</mi> <mo>,</mo> <mi>&amp;epsiv;</mi> </mrow> </msub> </mrow> </mtd> <mtd> <mrow> <mi>cos</mi> <mi> </mi> <msub> <mi>q</mi> <mrow> <mi>p</mi> <mi>e</mi> <mo>,</mo> <mi>&amp;epsiv;</mi> </mrow> </msub> </mrow> </mtd> <mtd> <mn>0</mn> </mtd> </mtr> <mtr> <mtd> <mrow> <mo>-</mo> <mi>sin</mi> <mi> </mi> <msub> <mi>q</mi> <mrow> <mi>p</mi> <mi>e</mi> <mo>,</mo> <mi>&amp;beta;</mi> </mrow> </msub> <mi>cos</mi> <mi> </mi> <msub> <mi>q</mi> <mrow> <mi>p</mi> <mi>e</mi> <mo>,</mo> <mi>&amp;epsiv;</mi> </mrow> </msub> </mrow> </mtd> <mtd> <mrow> <mi>sin</mi> <mi> </mi> <msub> <mi>q</mi> <mrow> <mi>p</mi> <mi>e</mi> <mo>,</mo> <mi>&amp;epsiv;</mi> </mrow> </msub> <mi>sin</mi> <mi> </mi> <msub> <mi>q</mi> <mrow> <mi>p</mi> <mi>e</mi> <mo>,</mo> <mi>&amp;beta;</mi> </mrow> </msub> </mrow> </mtd> <mtd> <mrow> <mi>cos</mi> <mi> </mi> <msub> <mi>q</mi> <mrow> <mi>p</mi> <mi>e</mi> <mo>,</mo> <mi>&amp;beta;</mi> </mrow> </msub> </mrow> </mtd> </mtr> </mtable> </mfenced> <mo>-</mo> <mo>-</mo> <mo>-</mo> <mrow> <mo>(</mo> <mn>9</mn> <mo>)</mo> </mrow> </mrow>
矩阵Cps为坐标系转换矩阵，将pursuer惯性坐标系转换为场景惯性坐标系；Cps值为
<mrow> <msub> <mi>C</mi> <mrow> <mi>p</mi> <mi>s</mi> </mrow> </msub> <mo>=</mo> <mfenced open = "[" close = "]"> <mtable> <mtr> <mtd> <mrow> <mo>-</mo> <mn>1</mn> </mrow> </mtd> <mtd> <mn>0</mn> </mtd> <mtd> <mn>0</mn> </mtd> </mtr> <mtr> <mtd> <mn>0</mn> </mtd> <mtd> <mn>1</mn> </mtd> <mtd> <mn>0</mn> </mtd> </mtr> <mtr> <mtd> <mn>0</mn> </mtd> <mtd> <mn>0</mn> </mtd> <mtd> <mrow> <mo>-</mo> <mn>1</mn> </mrow> </mtd> </mtr> </mtable> </mfenced> <mo>-</mo> <mo>-</mo> <mo>-</mo> <mrow> <mo>(</mo> <mn>10</mn> <mo>)</mo> </mrow> </mrow>
pursuer和evader的视线角和视线角速度关系为
<mrow> <mtable> <mtr> <mtd> <mrow> <msub> <mi>q</mi> <mrow> <mi>p</mi> <mi>e</mi> <mo>,</mo> <mi>&amp;epsiv;</mi> </mrow> </msub> <mo>=</mo> <mo>-</mo> <msub> <mi>q</mi> <mrow> <mi>p</mi> <mi>e</mi> <mo>,</mo> <mi>&amp;epsiv;</mi> </mrow> </msub> </mrow> </mtd> <mtd> <mrow> <msub> <mover> <mi>q</mi> <mo>&amp;CenterDot;</mo> </mover> <mrow> <mi>p</mi> <mi>e</mi> <mo>,</mo> <mi>&amp;epsiv;</mi> </mrow> </msub> <mo>=</mo> <mo>-</mo> <msub> <mover> <mi>q</mi> <mo>&amp;CenterDot;</mo> </mover> <mrow> <mi>p</mi> <mi>e</mi> <mo>,</mo> <mi>&amp;epsiv;</mi> </mrow> </msub> </mrow> </mtd> </mtr> <mtr> <mtd> <mrow> <msub> <mi>q</mi> <mrow> <mi>e</mi> <mi>p</mi> <mo>,</mo> <mi>&amp;beta;</mi> </mrow> </msub> <mo>=</mo> <msub> <mi>q</mi> <mrow> <mi>p</mi> <mi>e</mi> <mo>,</mo> <mi>&amp;beta;</mi> </mrow> </msub> </mrow> </mtd> <mtd> <mrow> <msub> <mover> <mi>q</mi> <mo>&amp;CenterDot;</mo> </mover> <mrow> <mi>e</mi> <mi>p</mi> <mo>,</mo> <mi>&amp;beta;</mi> </mrow> </msub> <mo>=</mo> <msub> <mover> <mi>q</mi> <mo>&amp;CenterDot;</mo> </mover> <mrow> <mi>p</mi> <mi>e</mi> <mo>,</mo> <mi>&amp;beta;</mi> </mrow> </msub> </mrow> </mtd> </mtr> </mtable> <mo>-</mo> <mo>-</mo> <mo>-</mo> <mrow> <mo>(</mo> <mn>11</mn> <mo>)</mo> </mrow> </mrow>
其中qep,ε和qep,β为evader到pursuer的视线倾角和偏角；和为evader到pursuer的视线倾角角速度和偏角速度；
另外为evader相对于pursuer的速度，为pursuer相对于evader的速度，两者相等，即
<mrow> <msub> <mover> <mi>r</mi> <mo>&amp;CenterDot;</mo> </mover> <mrow> <mi>e</mi> <mi>p</mi> </mrow> </msub> <mo>=</mo> <msub> <mover> <mi>r</mi> <mo>&amp;CenterDot;</mo> </mover> <mrow> <mi>p</mi> <mi>e</mi> </mrow> </msub> <mo>-</mo> <mo>-</mo> <mo>-</mo> <mrow> <mo>(</mo> <mn>12</mn> <mo>)</mo> </mrow> </mrow>
视线角和距离为
<mrow> <mtable> <mtr> <mtd> <mrow> <msub> <mi>q</mi> <mrow> <mi>e</mi> <mi>p</mi> <mo>,</mo> <mi>&amp;epsiv;</mi> </mrow> </msub> <mo>=</mo> <mi>a</mi> <mi>r</mi> <mi>c</mi> <mi>t</mi> <mi>a</mi> <mi>n</mi> <mfrac> <msub> <mi>r</mi> <mi>y</mi> </msub> <msub> <mi>r</mi> <mi>x</mi> </msub> </mfrac> </mrow> </mtd> <mtd> <mrow> <msub> <mi>q</mi> <mrow> <mi>e</mi> <mi>p</mi> <mo>,</mo> <mi>&amp;beta;</mi> </mrow> </msub> <mo>=</mo> <mo>-</mo> <mi>a</mi> <mi>r</mi> <mi>c</mi> <mi>t</mi> <mi>a</mi> <mi>n</mi> <mfrac> <msub> <mi>r</mi> <mi>z</mi> </msub> <msub> <mi>r</mi> <mi>x</mi> </msub> </mfrac> </mrow> </mtd> </mtr> </mtable> <mo>-</mo> <mo>-</mo> <mo>-</mo> <mrow> <mo>(</mo> <mn>13</mn> <mo>)</mo> </mrow> </mrow>
<mrow> <msub> <mi>r</mi> <mrow> <mi>e</mi> <mi>p</mi> </mrow> </msub> <mo>=</mo> <msqrt> <mrow> <msubsup> <mi>r</mi> <mi>x</mi> <mn>2</mn> </msubsup> <mo>+</mo> <msubsup> <mi>r</mi> <mi>y</mi> <mn>2</mn> </msubsup> <mo>+</mo> <msubsup> <mi>r</mi> <mi>z</mi> <mn>2</mn> </msubsup> </mrow> </msqrt> <mo>-</mo> <mo>-</mo> <mo>-</mo> <mrow> <mo>(</mo> <mn>14</mn> <mo>)</mo> </mrow> </mrow>
分别对式(13)和(14)取时间的导数，得
<mrow> <mtable> <mtr> <mtd> <mrow> <msub> <mover> <mi>q</mi> <mo>&amp;CenterDot;</mo> </mover> <mrow> <mi>e</mi> <mi>p</mi> <mo>,</mo> <mi>&amp;epsiv;</mi> </mrow> </msub> <mo>=</mo> <mfrac> <mrow> <msub> <mi>v</mi> <mi>y</mi> </msub> <mrow> <mo>(</mo> <msubsup> <mi>r</mi> <mi>x</mi> <mn>2</mn> </msubsup> <mo>+</mo> <msubsup> <mi>r</mi> <mi>z</mi> <mn>2</mn> </msubsup> <mo>)</mo> </mrow> <mo>-</mo> <msub> <mi>r</mi> <mi>y</mi> </msub> <mrow> <mo>(</mo> <msub> <mi>r</mi> <mi>x</mi> </msub> <msub> <mi>v</mi> <mi>x</mi> </msub> <mo>+</mo> <msub> <mi>r</mi> <mi>z</mi> </msub> <msub> <mi>v</mi> <mi>z</mi> </msub> <mo>)</mo> </mrow> </mrow> <mrow> <msqrt> <mrow> <msubsup> <mi>r</mi> <mi>x</mi> <mn>2</mn> </msubsup> <mo>+</mo> <msubsup> <mi>r</mi> <mi>z</mi> <mn>2</mn> </msubsup> </mrow> </msqrt> <mrow> <mo>(</mo> <msubsup> <mi>r</mi> <mi>x</mi> <mn>2</mn> </msubsup> <mo>+</mo> <msubsup> <mi>r</mi> <mi>y</mi> <mn>2</mn> </msubsup> <mo>+</mo> <msubsup> <mi>r</mi> <mi>z</mi> <mn>2</mn> </msubsup> <mo>)</mo> </mrow> </mrow> </mfrac> </mrow> </mtd> <mtd> <mrow> <msub> <mover> <mi>q</mi> <mo>&amp;CenterDot;</mo> </mover> <mrow> <mi>e</mi> <mi>p</mi> <mo>,</mo> <mi>&amp;beta;</mi> </mrow> </msub> <mo>=</mo> <mfrac> <mrow> <msub> <mi>r</mi> <mi>z</mi> </msub> <msub> <mi>v</mi> <mi>x</mi> </msub> <mo>-</mo> <msub> <mi>r</mi> <mi>x</mi> </msub> <msub> <mi>v</mi> <mi>z</mi> </msub> </mrow> <mrow> <msubsup> <mi>r</mi> <mi>x</mi> <mn>2</mn> </msubsup> <mo>+</mo> <msubsup> <mi>r</mi> <mi>z</mi> <mn>2</mn> </msubsup> </mrow> </mfrac> </mrow> </mtd> </mtr> </mtable> <mo>-</mo> <mo>-</mo> <mo>-</mo> <mrow> <mo>(</mo> <mn>15</mn> <mo>)</mo> </mrow> </mrow>
<mrow> <msub> <mover> <mi>r</mi> <mo>&amp;CenterDot;</mo> </mover> <mrow> <mi>e</mi> <mi>p</mi> </mrow> </msub> <mo>=</mo> <mfrac> <mrow> <msub> <mi>r</mi> <mi>x</mi> </msub> <msub> <mi>v</mi> <mi>x</mi> </msub> <mo>+</mo> <msub> <mi>r</mi> <mi>y</mi> </msub> <msub> <mi>v</mi> <mi>y</mi> </msub> <mo>+</mo> <msub> <mi>r</mi> <mi>z</mi> </msub> <msub> <mi>v</mi> <mi>z</mi> </msub> </mrow> <msqrt> <mrow> <msubsup> <mi>r</mi> <mi>x</mi> <mn>2</mn> </msubsup> <mo>+</mo> <msubsup> <mi>r</mi> <mi>y</mi> <mn>2</mn> </msubsup> <mo>+</mo> <msubsup> <mi>r</mi> <mi>z</mi> <mn>2</mn> </msubsup> </mrow> </msqrt> </mfrac> <mo>-</mo> <mo>-</mo> <mo>-</mo> <mrow> <mo>(</mo> <mn>16</mn> <mo>)</mo> </mrow> </mrow>
另外
<mrow> <mtable> <mtr> <mtd> <mrow> <mi>sin</mi> <mi> </mi> <msub> <mi>q</mi> <mrow> <mi>e</mi> <mi>p</mi> <mo>,</mo> <mi>&amp;epsiv;</mi> </mrow> </msub> <mo>=</mo> <mfrac> <msub> <mi>r</mi> <mi>y</mi> </msub> <msub> <mi>r</mi> <mrow> <mi>e</mi> <mi>p</mi> </mrow> </msub> </mfrac> </mrow> </mtd> <mtd> <mrow> <mi>cos</mi> <mi> </mi> <msub> <mi>q</mi> <mrow> <mi>e</mi> <mi>p</mi> <mo>,</mo> <mi>&amp;epsiv;</mi> </mrow> </msub> <mo>=</mo> <mfrac> <mi>&amp;rho;</mi> <msub> <mi>r</mi> <mrow> <mi>e</mi> <mi>p</mi> </mrow> </msub> </mfrac> </mrow> </mtd> </mtr> </mtable> <mo>-</mo> <mo>-</mo> <mo>-</mo> <mrow> <mo>(</mo> <mn>17</mn> <mo>)</mo> </mrow> </mrow>
<mrow> <mtable> <mtr> <mtd> <mrow> <mi>sin</mi> <mi> </mi> <msub> <mi>q</mi> <mrow> <mi>e</mi> <mi>p</mi> <mo>,</mo> <mi>&amp;beta;</mi> </mrow> </msub> <mo>=</mo> <mo>-</mo> <mfrac> <msub> <mi>r</mi> <mi>z</mi> </msub> <mi>&amp;rho;</mi> </mfrac> </mrow> </mtd> <mtd> <mrow> <mi>cos</mi> <mi> </mi> <msub> <mi>q</mi> <mrow> <mi>e</mi> <mi>p</mi> <mo>,</mo> <mi>&amp;beta;</mi> </mrow> </msub> <mo>=</mo> <mfrac> <msub> <mi>r</mi> <mi>x</mi> </msub> <mi>&amp;rho;</mi> </mfrac> </mrow> </mtd> </mtr> </mtable> <mo>-</mo> <mo>-</mo> <mo>-</mo> <mrow> <mo>(</mo> <mn>18</mn> <mo>)</mo> </mrow> </mrow>
为evader和pursuer之间的距离；ρ为中间变量；
其中
<mrow> <mi>&amp;rho;</mi> <mo>=</mo> <msqrt> <mrow> <msubsup> <mi>r</mi> <mi>x</mi> <mn>2</mn> </msubsup> <mo>+</mo> <msubsup> <mi>r</mi> <mi>z</mi> <mn>2</mn> </msubsup> </mrow> </msqrt> <mo>-</mo> <mo>-</mo> <mo>-</mo> <mrow> <mo>(</mo> <mn>19</mn> <mo>)</mo> </mrow> </mrow>
将式(17)和式(18)代入式(9)，得
<mrow> <msub> <mi>C</mi> <mn>40</mn> </msub> <mo>=</mo> <mfenced open = "[" close = "]"> <mtable> <mtr> <mtd> <mfrac> <msub> <mi>r</mi> <mi>x</mi> </msub> <msub> <mi>r</mi> <mrow> <mi>e</mi> <mi>p</mi> </mrow> </msub> </mfrac> </mtd> <mtd> <mfrac> <mrow> <msub> <mi>r</mi> <mi>x</mi> </msub> <msub> <mi>r</mi> <mi>y</mi> </msub> </mrow> <mrow> <msub> <mi>&amp;rho;r</mi> <mrow> <mi>e</mi> <mi>p</mi> </mrow> </msub> </mrow> </mfrac> </mtd> <mtd> <mrow> <mo>-</mo> <mfrac> <msub> <mi>r</mi> <mi>z</mi> </msub> <mi>&amp;rho;</mi> </mfrac> </mrow> </mtd> </mtr> <mtr> <mtd> <mrow> <mo>-</mo> <mfrac> <msub> <mi>r</mi> <mi>y</mi> </msub> <msub> <mi>r</mi> <mrow> <mi>e</mi> <mi>p</mi> </mrow> </msub> </mfrac> </mrow> </mtd> <mtd> <mfrac> <mi>&amp;rho;</mi> <msub> <mi>r</mi> <mrow> <mi>e</mi> <mi>p</mi> </mrow> </msub> </mfrac> </mtd> <mtd> <mn>0</mn> </mtd> </mtr> <mtr> <mtd> <mfrac> <msub> <mi>r</mi> <mi>z</mi> </msub> <msub> <mi>r</mi> <mrow> <mi>e</mi> <mi>p</mi> </mrow> </msub> </mfrac> </mtd> <mtd> <mfrac> <mrow> <msub> <mi>r</mi> <mi>y</mi> </msub> <msub> <mi>r</mi> <mi>z</mi> </msub> </mrow> <mrow> <msub> <mi>&amp;rho;r</mi> <mrow> <mi>e</mi> <mi>p</mi> </mrow> </msub> </mrow> </mfrac> </mtd> <mtd> <mfrac> <msub> <mi>r</mi> <mi>x</mi> </msub> <mi>&amp;rho;</mi> </mfrac> </mtd> </mtr> </mtable> </mfenced> <mo>-</mo> <mo>-</mo> <mo>-</mo> <mrow> <mo>(</mo> <mn>20</mn> <mo>)</mo> </mrow> </mrow>
将式(11)-(18)代入式(8)，得
其中
η＝(rxvx+ryvy+rzvz)(-rxvz+rzvx) (23)
为中间变量；
将式(21)代入式(6)，得
<mrow> <msub> <mover> <mi>r</mi> <mo>&amp;CenterDot;</mo> </mover> <mi>x</mi> </msub> <mo>=</mo> <msub> <mi>v</mi> <mi>x</mi> </msub> </mrow>
<mrow> <msub> <mover> <mi>r</mi> <mo>&amp;CenterDot;</mo> </mover> <mi>y</mi> </msub> <mo>=</mo> <msub> <mi>v</mi> <mi>y</mi> </msub> </mrow>
<mrow> <msub> <mover> <mi>r</mi> <mo>&amp;CenterDot;</mo> </mover> <mi>z</mi> </msub> <mo>=</mo> <msub> <mi>v</mi> <mi>z</mi> </msub> </mrow>
<mrow> <msub> <mover> <mi>v</mi> <mo>&amp;CenterDot;</mo> </mover> <mi>x</mi> </msub> <mo>=</mo> <msub> <mi>a</mi> <mrow> <mi>p</mi> <mi>x</mi> </mrow> </msub> <mo>-</mo> <msub> <mi>a</mi> <mrow> <mi>e</mi> <mi>x</mi> </mrow> </msub> </mrow>
<mrow> <msub> <mover> <mi>v</mi> <mo>&amp;CenterDot;</mo> </mover> <mi>y</mi> </msub> <mo>=</mo> <msub> <mi>a</mi> <mrow> <mi>p</mi> <mi>y</mi> </mrow> </msub> <mo>-</mo> <msub> <mi>a</mi> <mrow> <mi>e</mi> <mi>y</mi> </mrow> </msub> </mrow>
<mrow> <msub> <mover> <mi>v</mi> <mo>&amp;CenterDot;</mo> </mover> <mi>z</mi> </msub> <mo>=</mo> <msub> <mi>a</mi> <mrow> <mi>p</mi> <mi>z</mi> </mrow> </msub> <mo>-</mo> <msub> <mi>a</mi> <mrow> <mi>e</mi> <mi>z</mi> </mrow> </msub> </mrow>
式(24)就是从evader角度看关于pursuer的PN运动模型；
假设evader测量pursuer在场景惯性坐标系下相对自己的位置，则式(24)的测量矩阵为
<mrow> <msub> <mi>H</mi> <mi>e</mi> </msub> <mo>=</mo> <mfenced open = "[" close = "]"> <mtable> <mtr> <mtd> <mn>1</mn> </mtd> <mtd> <mn>0</mn> </mtd> <mtd> <mn>0</mn> </mtd> <mtd> <mn>0</mn> </mtd> <mtd> <mn>0</mn> </mtd> <mtd> <mn>0</mn> </mtd> <mtd> <mn>0</mn> </mtd> <mtd> <mn>0</mn> </mtd> <mtd> <mn>0</mn> </mtd> </mtr> <mtr> <mtd> <mn>0</mn> </mtd> <mtd> <mn>1</mn> </mtd> <mtd> <mn>0</mn> </mtd> <mtd> <mn>0</mn> </mtd> <mtd> <mn>0</mn> </mtd> <mtd> <mn>0</mn> </mtd> <mtd> <mn>0</mn> </mtd> <mtd> <mn>0</mn> </mtd> <mtd> <mn>0</mn> </mtd> </mtr> <mtr> <mtd> <mn>0</mn> </mtd> <mtd> <mn>0</mn> </mtd> <mtd> <mn>1</mn> </mtd> <mtd> <mn>0</mn> </mtd> <mtd> <mn>0</mn> </mtd> <mtd> <mn>0</mn> </mtd> <mtd> <mn>0</mn> </mtd> <mtd> <mn>0</mn> </mtd> <mtd> <mn>0</mn> </mtd> </mtr> </mtable> </mfenced> <mo>-</mo> <mo>-</mo> <mo>-</mo> <mrow> <mo>(</mo> <mn>25</mn> <mo>)</mo> </mrow> <mo>.</mo> </mrow>
4.根据权利要求3所述基于IMM_UKF的三维制导律辨识滤波方法，其特征在于：所述时间常数τ值为0.1。
5.根据权利要求4所述基于IMM_UKF的三维制导律辨识滤波方法，其特征在于：所述步骤三中基于步骤二建立的相对evader的pursuer运动模型，设计evader上的IMM滤波器；具体过程为：
假设IMM滤波器的模型集为M＝{M1,…,Mn}，其中Mj代表第j个pursuer运动模型；模型Mj的先验概率为 代表初始时刻pursuer运动模型为Mj的事件；1≤j≤n；n取值为正整数，为模型集中模型的数量；
设为已知的pursuer运动模型转换概率；pursuer运动模型Mj对应的状态方程和测量方程为
<mrow> <mtable> <mtr> <mtd> <mrow> <msub> <mi>x</mi> <mi>k</mi> </msub> <mo>=</mo> <msup> <mi>f</mi> <mi>j</mi> </msup> <mrow> <mo>(</mo> <mi>k</mi> <mo>,</mo> <msub> <mi>x</mi> <mrow> <mi>k</mi> <mo>-</mo> <mn>1</mn> </mrow> </msub> <mo>)</mo> </mrow> <mo>+</mo> <msubsup> <mi>w</mi> <mrow> <mi>k</mi> <mo>-</mo> <mn>1</mn> </mrow> <mi>j</mi> </msubsup> </mrow> </mtd> </mtr> <mtr> <mtd> <mrow> <msub> <mi>z</mi> <mi>k</mi> </msub> <mo>=</mo> <msup> <mi>h</mi> <mi>j</mi> </msup> <mrow> <mo>(</mo> <mi>k</mi> <mo>,</mo> <msub> <mi>x</mi> <mi>k</mi> </msub> <mo>)</mo> </mrow> <mo>+</mo> <msubsup> <mi>v</mi> <mi>k</mi> <mi>j</mi> </msubsup> </mrow> </mtd> </mtr> </mtable> <mo>-</mo> <mo>-</mo> <mo>-</mo> <mrow> <mo>(</mo> <mn>26</mn> <mo>)</mo> </mrow> </mrow>
其中fj(k,xk-1)，hj(k,xk)，和分别为pursuer运动模型Mj在k时刻下的状态转换函数，测量函数，过程噪声和测量噪声；为在k时刻pursuer运动模型为Mj的事件；为在k-1时刻pursuer运动模型为Mi的事件；1≤i≤n；
1)、求解k时刻IMM滤波算法混合输入和混合协方差矩阵
2)、对1)得到的和进行滤波，得到pursuer运动模型Mj在k时刻的似然函数；
3)、基于2)得到k时刻IMM滤波算法最终状态估计和协方差矩阵。
6.根据权利要求5所述基于IMM_UKF的三维制导律辨识滤波方法，其特征在于：所述1)中求解k时刻IMM滤波算法混合输入和混合协方差矩阵；具体过程为
用表示pursuer运动模型Mi到Mj的转换概率，其计算方法为
<mrow> <mtable> <mtr> <mtd> <mrow> <msub> <mover> <mi>c</mi> <mo>&amp;OverBar;</mo> </mover> <mi>j</mi> </msub> <mo>=</mo> <munderover> <mo>&amp;Sigma;</mo> <mrow> <mi>i</mi> <mo>=</mo> <mn>1</mn> </mrow> <mi>n</mi> </munderover> <msub> <mi>p</mi> <mrow> <mi>i</mi> <mi>j</mi> </mrow> </msub> <msubsup> <mi>&amp;mu;</mi> <mrow> <mi>k</mi> <mo>-</mo> <mn>1</mn> </mrow> <mi>i</mi> </msubsup> </mrow> </mtd> </mtr> <mtr> <mtd> <mrow> <msubsup> <mi>&amp;mu;</mi> <mi>k</mi> <mrow> <mi>i</mi> <mo>|</mo> <mi>j</mi> </mrow> </msubsup> <mo>=</mo> <mfrac> <mn>1</mn> <msub> <mover> <mi>c</mi> <mo>&amp;OverBar;</mo> </mover> <mi>j</mi> </msub> </mfrac> <msub> <mi>p</mi> <mrow> <mi>i</mi> <mi>j</mi> </mrow> </msub> <msubsup> <mi>&amp;mu;</mi> <mrow> <mi>k</mi> <mo>-</mo> <mn>1</mn> </mrow> <mi>i</mi> </msubsup> </mrow> </mtd> </mtr> </mtable> <mo>-</mo> <mo>-</mo> <mo>-</mo> <mrow> <mo>(</mo> <mn>27</mn> <mo>)</mo> </mrow> </mrow>
其中为k-1时刻pursuer运动模型为Mi的概率，是归一化因子，Mi代表第i个pursuer运动模型，1≤i≤n；
最终k时刻IMM滤波算法混合输入和混合协方差矩阵计算公式为
<mrow> <mtable> <mtr> <mtd> <mrow> <msubsup> <mi>x</mi> <mrow> <mi>k</mi> <mo>-</mo> <mn>1</mn> </mrow> <mrow> <mo>(</mo> <mi>j</mi> <mo>,</mo> <mn>0</mn> <mo>)</mo> </mrow> </msubsup> <mo>=</mo> <munderover> <mi>&amp;Sigma;</mi> <mrow> <mi>i</mi> <mo>=</mo> <mn>1</mn> </mrow> <mi>n</mi> </munderover> <msubsup> <mi>&amp;mu;</mi> <mi>k</mi> <mrow> <mi>i</mi> <mo>|</mo> <mi>j</mi> </mrow> </msubsup> <msubsup> <mover> <mi>x</mi> <mo>^</mo> </mover> <mrow> <mi>k</mi> <mo>-</mo> <mn>1</mn> <mo>,</mo> <mi>k</mi> <mo>-</mo> <mn>1</mn> </mrow> <mi>i</mi> </msubsup> </mrow> </mtd> </mtr> <mtr> <mtd> <mrow> <msubsup> <mi>P</mi> <mrow> <mi>k</mi> <mo>-</mo> <mn>1</mn> </mrow> <mrow> <mo>(</mo> <mi>j</mi> <mo>,</mo> <mn>0</mn> <mo>)</mo> </mrow> </msubsup> <mo>=</mo> <munderover> <mi>&amp;Sigma;</mi> <mrow> <mi>i</mi> <mo>=</mo> <mn>1</mn> </mrow> <mi>n</mi> </munderover> <msubsup> <mi>&amp;mu;</mi> <mi>k</mi> <mrow> <mi>i</mi> <mo>|</mo> <mi>j</mi> </mrow> </msubsup> <mo>&amp;lsqb;</mo> <msubsup> <mi>P</mi> <mrow> <mi>k</mi> <mo>-</mo> <mn>1</mn> <mo>,</mo> <mi>k</mi> <mo>-</mo> <mn>1</mn> </mrow> <mi>i</mi> </msubsup> <mo>+</mo> <mrow> <mo>(</mo> <msubsup> <mover> <mi>x</mi> <mo>^</mo> </mover> <mrow> <mi>k</mi> <mo>-</mo> <mn>1</mn> <mo>,</mo> <mi>k</mi> <mo>-</mo> <mn>1</mn> </mrow> <mi>i</mi> </msubsup> <mo>-</mo> <msubsup> <mi>x</mi> <mrow> <mi>k</mi> <mo>-</mo> <mn>1</mn> </mrow> <mrow> <mo>(</mo> <mi>j</mi> <mo>,</mo> <mn>0</mn> <mo>)</mo> </mrow> </msubsup> <mo>)</mo> </mrow> <msup> <mrow> <mo>(</mo> <msubsup> <mover> <mi>x</mi> <mo>^</mo> </mover> <mrow> <mi>k</mi> <mo>-</mo> <mn>1</mn> <mo>,</mo> <mi>k</mi> <mo>-</mo> <mn>1</mn> </mrow> <mi>i</mi> </msubsup> <mo>-</mo> <msubsup> <mi>x</mi> <mrow> <mi>k</mi> <mo>-</mo> <mn>1</mn> </mrow> <mrow> <mo>(</mo> <mi>j</mi> <mo>,</mo> <mn>0</mn> <mo>)</mo> </mrow> </msubsup> <mo>)</mo> </mrow> <mi>T</mi> </msup> <mo>&amp;rsqb;</mo> </mrow> </mtd> </mtr> </mtable> <mo>-</mo> <mo>-</mo> <mo>-</mo> <mrow> <mo>(</mo> <mn>28</mn> <mo>)</mo> </mrow> </mrow>
其中和分别代表混合输入和混合协方差矩阵；为pursuer运动模型Mi对应的Kalman滤波器在k-1时刻的状态估计，为pursuer运动模型Mi对应的Kalman滤波器在k-1时刻的状态估计协方差矩阵。
7.根据权利要求6所述基于IMM_UKF的三维制导律辨识滤波方法，其特征在于：所述2)中对1)得到的和进行滤波，得到pursuer运动模型Mj在k时刻的似然函数；公式为：
<mrow> <mtable> <mtr> <mtd> <mrow> <mo>&amp;lsqb;</mo> <msubsup> <mover> <mi>x</mi> <mo>^</mo> </mover> <mrow> <mi>k</mi> <mo>,</mo> <mi>k</mi> <mo>-</mo> <mn>1</mn> </mrow> <mi>j</mi> </msubsup> <mo>,</mo> <msubsup> <mi>P</mi> <mrow> <mi>k</mi> <mo>,</mo> <mi>k</mi> <mo>-</mo> <mn>1</mn> </mrow> <mi>j</mi> </msubsup> <mo>&amp;rsqb;</mo> <mo>=</mo> <msub> <mi>UKF</mi> <mi>p</mi> </msub> <mrow> <mo>(</mo> <msubsup> <mi>x</mi> <mrow> <mi>k</mi> <mo>-</mo> <mn>1</mn> </mrow> <mrow> <mo>(</mo> <mi>j</mi> <mo>,</mo> <mn>0</mn> <mo>)</mo> </mrow> </msubsup> <mo>,</mo> <msubsup> <mi>P</mi> <mrow> <mi>k</mi> <mo>-</mo> <mn>1</mn> </mrow> <mrow> <mo>(</mo> <mi>j</mi> <mo>,</mo> <mn>0</mn> <mo>)</mo> </mrow> </msubsup> <mo>,</mo> <msup> <mi>f</mi> <mi>j</mi> </msup> <mo>(</mo> <mrow> <mi>k</mi> <mo>,</mo> <msubsup> <mi>x</mi> <mrow> <mi>k</mi> <mo>-</mo> <mn>1</mn> </mrow> <mrow> <mo>(</mo> <mi>j</mi> <mo>,</mo> <mn>0</mn> <mo>)</mo> </mrow> </msubsup> </mrow> <mo>)</mo> <mo>,</mo> <msubsup> <mi>Q</mi> <mrow> <mi>k</mi> <mo>-</mo> <mn>1</mn> </mrow> <mi>j</mi> </msubsup> <mo>)</mo> </mrow> </mrow> </mtd> </mtr> <mtr> <mtd> <mrow> <mo>&amp;lsqb;</mo> <msubsup> <mover> <mi>x</mi> <mo>^</mo> </mover> <mrow> <mi>k</mi> <mo>,</mo> <mi>k</mi> </mrow> <mi>j</mi> </msubsup> <mo>,</mo> <msubsup> <mi>P</mi> <mrow> <mi>k</mi> <mo>,</mo> <mi>k</mi> </mrow> <mi>j</mi> </msubsup> <mo>,</mo> <msubsup> <mi>v</mi> <mrow> <mi>k</mi> <mo>,</mo> <mi>k</mi> </mrow> <mi>j</mi> </msubsup> <mo>,</mo> <msubsup> <mi>S</mi> <mi>k</mi> <mi>j</mi> </msubsup> <mo>&amp;rsqb;</mo> <mo>=</mo> <msub> <mi>UKF</mi> <mi>u</mi> </msub> <mrow> <mo>(</mo> <msubsup> <mover> <mi>x</mi> <mo>^</mo> </mover> <mrow> <mi>k</mi> <mo>,</mo> <mi>k</mi> <mo>-</mo> <mn>1</mn> </mrow> <mi>j</mi> </msubsup> <mo>,</mo> <msubsup> <mi>P</mi> <mrow> <mi>k</mi> <mo>,</mo> <mi>k</mi> <mo>-</mo> <mn>1</mn> </mrow> <mi>j</mi> </msubsup> <mo>,</mo> <msub> <mi>z</mi> <mi>k</mi> </msub> <mo>,</mo> <msup> <mi>h</mi> <mi>j</mi> </msup> <mo>(</mo> <mrow> <mi>k</mi> <mo>,</mo> <msubsup> <mover> <mi>x</mi> <mo>^</mo> </mover> <mrow> <mi>k</mi> <mo>,</mo> <mi>k</mi> <mo>-</mo> <mn>1</mn> </mrow> <mi>j</mi> </msubsup> </mrow> <mo>)</mo> <mo>,</mo> <msubsup> <mi>R</mi> <mi>k</mi> <mi>j</mi> </msubsup> <mo>)</mo> </mrow> </mrow> </mtd> </mtr> </mtable> <mo>-</mo> <mo>-</mo> <mo>-</mo> <mrow> <mo>(</mo> <mn>29</mn> <mo>)</mo> </mrow> </mrow>
其中函数UKFp(·)为状态预测函数，UKFu(·)为状态更新函数；为pursuer运动模型Mj在k时刻的状态预测，为pursuer运动模型Mj对应的Kalman滤波器在k时刻的状态预测协方差矩阵，为pursuer运动模型Mj对应的Kalman滤波器在k-1时刻的系统过程噪声矩阵，为pursuer运动模型Mj对应的Kalman滤波器在k时刻的状态估计，为pursuer运动模型Mj对应的Kalman滤波器在k时刻的状态估计协方差矩阵，为pursuer运动模型Mj对应的Kalman滤波器在k时刻的新息向量，为pursuer运动模型Mj对应的Kalman滤波器在k时刻的新息的协方差矩阵，zk为k时刻的测量值，为pursuer运动模型Mj在k时刻的测量噪声矩阵；
模型Mj对应的Kalman滤波器在k时刻的似然函数为
其中为pursuer运动模型Mj对应的Kalman滤波器在k时刻下的新息向量，为新息的协方差矩阵；为正态分布概率密度函数。
8.根据权利要求7所述基于IMM_UKF的三维制导律辨识滤波方法，其特征在于：所述3)中基于2)得到k时刻IMM滤波算法最终状态估计和协方差矩阵；
最终状态估计和协方差矩阵为
<mrow> <mtable> <mtr> <mtd> <mrow> <msub> <mover> <mi>x</mi> <mo>^</mo> </mover> <mrow> <mi>k</mi> <mo>,</mo> <mi>k</mi> </mrow> </msub> <mo>=</mo> <munderover> <mi>&amp;Sigma;</mi> <mrow> <mi>j</mi> <mo>=</mo> <mn>1</mn> </mrow> <mi>n</mi> </munderover> <msubsup> <mi>&amp;mu;</mi> <mi>k</mi> <mi>j</mi> </msubsup> <msubsup> <mover> <mi>x</mi> <mo>^</mo> </mover> <mrow> <mi>k</mi> <mo>,</mo> <mi>k</mi> </mrow> <mi>j</mi> </msubsup> </mrow> </mtd> </mtr> <mtr> <mtd> <mrow> <msub> <mi>P</mi> <mrow> <mi>k</mi> <mo>,</mo> <mi>k</mi> </mrow> </msub> <mo>=</mo> <munderover> <mi>&amp;Sigma;</mi> <mrow> <mi>j</mi> <mo>=</mo> <mn>1</mn> </mrow> <mi>n</mi> </munderover> <msubsup> <mi>&amp;mu;</mi> <mi>k</mi> <mi>j</mi> </msubsup> <mo>&amp;lsqb;</mo> <msubsup> <mi>P</mi> <mrow> <mi>k</mi> <mo>,</mo> <mi>k</mi> </mrow> <mi>j</mi> </msubsup> <mo>+</mo> <mrow> <mo>(</mo> <msubsup> <mover> <mi>x</mi> <mo>^</mo> </mover> <mrow> <mi>k</mi> <mo>,</mo> <mi>k</mi> </mrow> <mi>j</mi> </msubsup> <mo>-</mo> <msub> <mover> <mi>x</mi> <mo>^</mo> </mover> <mrow> <mi>k</mi> <mo>,</mo> <mi>k</mi> </mrow> </msub> <mo>)</mo> </mrow> <msup> <mrow> <mo>(</mo> <msubsup> <mover> <mi>x</mi> <mo>^</mo> </mover> <mrow> <mi>k</mi> <mo>,</mo> <mi>k</mi> </mrow> <mi>j</mi> </msubsup> <mo>-</mo> <msub> <mover> <mi>x</mi> <mo>^</mo> </mover> <mrow> <mi>k</mi> <mo>,</mo> <mi>k</mi> </mrow> </msub> <mo>)</mo> </mrow> <mi>T</mi> </msup> <mo>&amp;rsqb;</mo> </mrow> </mtd> </mtr> </mtable> <mo>-</mo> <mo>-</mo> <mo>-</mo> <mrow> <mo>(</mo> <mn>31</mn> <mo>)</mo> </mrow> </mrow>
为模型Mj对应的Kalman滤波器在k时刻的状态估计，为模型Mj对应的Kalman滤波器在k时刻的状态估计协方差矩阵；为IMM滤波器的状态估计；Pk,kIMM滤波器在k时刻的状态估计协方差矩阵；
为k时刻pursuer运动模型为模型Mj的概率，表达式如下
<mrow> <mtable> <mtr> <mtd> <mrow> <mi>c</mi> <mo>=</mo> <munderover> <mo>&amp;Sigma;</mo> <mrow> <mi>j</mi> <mo>=</mo> <mn>1</mn> </mrow> <mi>n</mi> </munderover> <msubsup> <mi>&amp;Lambda;</mi> <mi>k</mi> <mi>j</mi> </msubsup> <msub> <mover> <mi>c</mi> <mo>&amp;OverBar;</mo> </mover> <mi>j</mi> </msub> </mrow> </mtd> </mtr> <mtr> <mtd> <mrow> <msubsup> <mi>&amp;mu;</mi> <mi>k</mi> <mi>j</mi> </msubsup> <mo>=</mo> <mfrac> <mn>1</mn> <mi>c</mi> </mfrac> <msubsup> <mi>&amp;Lambda;</mi> <mi>k</mi> <mi>j</mi> </msubsup> <msub> <mover> <mi>c</mi> <mo>&amp;OverBar;</mo> </mover> <mi>j</mi> </msub> </mrow> </mtd> </mtr> </mtable> <mo>-</mo> <mo>-</mo> <mo>-</mo> <mrow> <mo>(</mo> <mn>32</mn> <mo>)</mo> </mrow> </mrow>
c为归一化常数。
CN201711251352.4A 2017-12-01 2017-12-01 基于imm_ukf的三维制导律辨识滤波方法 Active CN108009358B (zh)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN201711251352.4A CN108009358B (zh) 2017-12-01 2017-12-01 基于imm_ukf的三维制导律辨识滤波方法

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN201711251352.4A CN108009358B (zh) 2017-12-01 2017-12-01 基于imm_ukf的三维制导律辨识滤波方法

Publications (2)

Publication Number Publication Date
CN108009358A true CN108009358A (zh) 2018-05-08
CN108009358B CN108009358B (zh) 2021-06-15

Family

ID=62056305

Family Applications (1)

Application Number Title Priority Date Filing Date
CN201711251352.4A Active CN108009358B (zh) 2017-12-01 2017-12-01 基于imm_ukf的三维制导律辨识滤波方法

Country Status (1)

Country Link
CN (1) CN108009358B (zh)

Cited By (2)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN111125926A (zh) * 2019-12-30 2020-05-08 哈尔滨工业大学 一种基于变结构多模型的拦截飞行器状态估计方法
CN112577489A (zh) * 2020-12-08 2021-03-30 北京电子工程总体研究所 一种基于交互多模型滤波的导引头视线转率提取方法

Citations (2)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN105446352A (zh) * 2015-11-23 2016-03-30 哈尔滨工业大学 一种比例导引制导律辨识滤波方法
CN105510882A (zh) * 2015-11-27 2016-04-20 电子科技大学 基于目标机动参数估计的快速自适应采样周期跟踪方法

Patent Citations (2)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN105446352A (zh) * 2015-11-23 2016-03-30 哈尔滨工业大学 一种比例导引制导律辨识滤波方法
CN105510882A (zh) * 2015-11-27 2016-04-20 电子科技大学 基于目标机动参数估计的快速自适应采样周期跟踪方法

Non-Patent Citations (4)

* Cited by examiner, † Cited by third party
Title
JOONGSUP YUN等: "Missile Guidance Law Estimation Using Modified Interactive Multiple Model Filter", 《JOURNAL OF GUIDANCE, CONTROL, AND DYNAMICS》 *
李士勇等: "《智能制导：寻的导弹智能自适应导引律》", 31 December 2011, 哈尔滨工业大学出版社 *
邹昕光等: "PN 制导律多模型自适应辨识滤波方法", 《宇航学报》 *
邹昕光等: "主动防御非奇异终端滑模协同制导律", 《兵工学报》 *

Cited By (4)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN111125926A (zh) * 2019-12-30 2020-05-08 哈尔滨工业大学 一种基于变结构多模型的拦截飞行器状态估计方法
CN111125926B (zh) * 2019-12-30 2024-09-20 哈尔滨工业大学 一种基于变结构多模型的拦截飞行器状态估计方法
CN112577489A (zh) * 2020-12-08 2021-03-30 北京电子工程总体研究所 一种基于交互多模型滤波的导引头视线转率提取方法
CN112577489B (zh) * 2020-12-08 2024-05-07 北京电子工程总体研究所 一种基于交互多模型滤波的导引头视线转率提取方法

Also Published As

Publication number Publication date
CN108009358B (zh) 2021-06-15

Similar Documents

Publication Publication Date Title
Xiong et al. A robust single GPS navigation and positioning algorithm based on strong tracking filtering
CN105371870B (zh) 一种基于星图数据的星敏感器在轨精度测量方法
CN107908895A (zh) 一种基于imm滤波器主动防御视线策略协同制导律设计方法
CN104061932B (zh) 一种利用引力矢量和梯度张量进行导航定位的方法
Xu et al. Indoor INS/UWB-based human localization with missing data utilizing predictive UFIR filtering
Konovalenko et al. UAV Navigation On The Basis Of The Feature Points Detection On Underlying Surface.
Chirikjian et al. Gaussian approximation of non-linear measurement models on Lie groups
CN107144278B (zh) 一种基于多源特征的着陆器视觉导航方法
CN107633256A (zh) 一种多源测距下联合目标定位与传感器配准方法
CN108009358A (zh) 基于imm_ukf的三维制导律辨识滤波方法
CN106802143B (zh) 一种基于惯性仪器和迭代滤波算法的船体形变角测量方法
CN113189541B (zh) 一种定位方法、装置及设备
Xue et al. A Gaussian-generalized-inverse-Gaussian joint-distribution-based adaptive MSCKF for visual-inertial odometry navigation
Xu et al. 3D pseudolinear Kalman filter with own-ship path optimization for AOA target tracking
CN105333873B (zh) 一种着陆点在线选取的行星安全着陆制导方法
Tang et al. SLAM with improved schmidt orthogonal unscented Kalman filter
Pak Gaussian sum FIR filtering for 2D target tracking
Liu et al. An UWB/PDR fusion algorithm based on improved square root unscented Kalman filter
Jung et al. Gaussian Mixture Midway-Merge for Object SLAM With Pose Ambiguity
Xu et al. Learning-based adaptive estimation for aoa target tracking with non-gaussian white noise
Manchester et al. Method for optical-flow-based precision missile guidance
Olson Landmark selection for terrain matching
CN112346032A (zh) 基于一致性扩展卡尔曼滤波的单红外传感器目标定轨方法
CN105890589A (zh) 一种水下机器人单目视觉定位方法
Han-hai et al. Research on slam algorithm of iterated extended Kalman filtering for multi-sensor fusion

Legal Events

Date Code Title Description
PB01 Publication
PB01 Publication
SE01 Entry into force of request for substantive examination
SE01 Entry into force of request for substantive examination
GR01 Patent grant
GR01 Patent grant