1、58第五章 被动测量条件下的非线性跟踪滤波器5.1 引 言为了实现一些较复杂的制导律，满足突防并精确命中目标等要求，需要已知剩余时间、相对距离和相对速度等物理量，有时还需要已知目标加速度。许多型号的导弹安装红外、电视、被动雷达等被动式导引头，这些导引头只能测量出角度或角速度信息。即使装有主动雷达的导弹，在电子对抗条件下，也无法测得距离和速度信息，而只有方位信息可以利用 1。我们不妨称仅有角度测量信息的制导问题为“被动制导”问题。在被动制导情况下，欲实现复杂的制导律，就必须利用仅有的角度测量信息实时估计出相对距离、相对速度和目标加速度。这种估计问题又可以称作目标的“被动跟踪”问题。在海洋环境中利
2、用被动声纳对潜艇或舰船进行定位也属于被动跟踪问题。被动跟踪问题是一个非线性估计问题，因为无论是在惯性直角坐标系中，还是在极坐标系中建立描述该问题的数学模型，所得到的结果都是非线性的。在惯性直角坐标系中，若假设目标加速度模型是线性的，则动态方程为线性，而测量方程则是非线性的；在极坐标系中，动态方程是非线性的，测量方程是线性的。由于测量信息中不可避免地含有测量噪声，被动跟踪问题需要用非线性随机系统来描述。上述仅有角度测量的实时估计实际上是一个非线性滤波问题。因此，必须研究稳定性好，收敛速度快，估计精度高的非线性滤波方法。5.2 非线性滤波概述非线性滤波方法大致可以分为两种类型, 即统计方法和概率方
3、法。在统计方法中，一个基本的思想是对非线性方程进行线性化，然后应用 Kalman-Bucy 滤波原理。所谓线性化就是把非线性项关于给定的参考轨迹或滤波器的当前估计值展成 Taylor 级数，取其一次项得到线性化方程。前者得到标称状态线性化滤波 2,3，后者则得到著名的推广 Kalman 滤波(EKF) 2,46。标称状态线性化滤波要求参考轨迹接近于实际轨迹，在实际当中这很难实现。EKF 忽略了线性化模型误差，影响滤波性能。EKF 中，滤波器参数是状态估计的函数，状态估计误差影响滤波器的增益，因此容易导致滤波器有偏，甚至发散。EKF 要求先验的噪声统计，然而实际上它们常常是未知的。用错误的噪声统
4、计会产生滤波误差，甚至使滤波发散。由于以上原因，EKF 有较大的局限性。EKF 又有许多改进版本，例如迭代推广 Kalman 滤波，二阶滤波等 6。二阶滤波器在精度上高于一阶滤波器，但是它的算法相当复杂，实际应用有较大困难。此外, 还有精度更高但算法也更复杂的三阶滤波器 7。近期, 又出现了 Lyapunov 基随机系统观测器 8, 协方差上限分配估计器 9, 依状态 Riccati 方程估计器 10等新的 EKF 改进算法。文献11对这三种新滤波器与 EKF 进行了比较研究，得出的结论是三种新滤波器在收敛性、对初始误差的鲁棒性等方面均好于 EKF，但当系统维数较高时，实现起来有较大难度或所要
5、求的滤波器增益计算时间过长。3 259利用虚拟噪声技术，非线性系统的线性化误差在一定程度上可以归为线性系统模型中的一种噪声。因此，用自适应滤波在线地估计虚拟噪声的统计特性，可以降低线性化误差，提高非线性滤波的精度，这也是一种 EKF 的改进算法 12, 而且计算量与 EKF 相差不大。这种方法的关键是找到性能稳定的噪声统计估值器，这样才能保持自适应滤波的稳定。当然它对线性化误差的补偿能力是有限的。一般来讲，对非线性滤波器难以进行稳定性分析。而对文献13提出的一种针对一类特定非线性系统的修正增益推广 Kalman 滤波器（MGEKF ） ，则可以利用 Lyapunov 第二法求得其稳定的充分条件
6、。这种所谓的特定非线性系统是由线性动态模型和非线性测量模型所构成的，而且测量模型中的非线性函数是“可修正的” 。MGEKF 的估计精度较 EKF 有明显提高，但设计难度较高。文献14认为，当模型线性化误差造成滤波器的状态估计值偏离系统状态时，必然会在输出残差序列幅值上表现出来，这时只要在线适当调整增益阵，令残差序列仍相互保持正交，则可强迫滤波器保持对实际系统状态的跟踪，从而设计出一种强跟踪滤波器。当遭遇模型跳变等情况时，这种滤波器具有快速跟踪能力。针对由线性动态模型和非线性测量模型所构成的非线性系统，文献15提出了一种两步滤波算法。所谓的两步滤波是指，首先定义一组新的状态(它是系统真实状态的非
7、线性函数) ，使得系统测量值是这组新状态的线性函数。在第一步滤波中，应用 Kalman 滤波器得到新状态的最优估计值。在第二步滤波中，把新状态的估计值作为测量，并应用 Gauss-Newton 迭代算法求出系统真实状态的最优解。文献15给出的两步滤波器虽然是一阶滤波器，但精度相当高。文献 16则给出了二阶两步滤波器及其 U-D 分解算法。在许多实际情况下，测量噪声统计特性是时变的, 无法验前已知，这时应用噪声统计估值器可以在线地确定测量噪声的均值和方差。作者把改进的 Sage-Husa 时变测量噪声统计估值器与两步滤波器相结合，得到一种性能优良的自适应两步滤波器 17，不仅收敛快，准确性高，而
8、且计算量较 EKF 等其它递推型滤波器并没有明显的增加。由于解决了实时估计中测量噪声协方差矩阵的正定性问题，这种自适应滤波有很好的稳定性。进一步的研究表明，两步滤波仍然存在需要改进之处。比如，在定义新状态的过程中，为了使系统测量成为新状态的线性函数，一般来讲新状态的维数大于系统真实状态的维数。这样，在一阶两步滤波器的第一步滤波中，新状态协方差矩阵容易接近病态，而文献15提出的该矩阵的时间修正方程是一个存在矩阵减法运算的近似方程，这样受各种误差的影响就容易求出非正定的协方差矩阵来，而这是不符合物理意义的，必然会造成滤波的失败 18。对此，文献19推导出一种新的近似算法，避免了减法运算的出现，但算
9、法比较复杂。寻找更简洁且稳定性好的协方差阵算法是有实际意义的。非线性预测滤波器适合于补偿明显的、非高斯分布的模型误差，而且这种滤波器的稳定性和鲁棒性得到了证明 20，这就为我们研究动态模型误差补偿方法提供了借鉴。非线性预测滤波用一步时间超前方法估计动态模型确定性误差和随机误差的均值, 但对随机误差方差的实时变化没有估计和补偿能力。文献21,22给出的动态噪声协方差阵的估计算法值得借鉴。以上所述基于统计方法的非线性滤波均是线性化方法的改进。前面提到，另外一类非线性滤波方法是概率方法。众所周知，随机动态系统状态的条件概率密度函数代表了非线性滤波问题的完全解。这是因为，有了条件概率密度函数就可以计算
10、出状态的最优估计 23。因此，人们自然想到通过求取条件概率密度函数得到精确的非线性滤波器。60概率方法之一是假设概率密度满足一类特定的函数，例如，高斯函数或指数函数，或一些函数的组合 24。实际上，EKF 也是一种最简单的设定密度（正态分布）滤波器，此外，还有高斯混合滤波器 24和投影滤波器 25等。在更有普遍意义的概率方法中，一般的过程为确定条件概率密度函数的进化方程，进而得到条件矩的方程，然后采用一定的假设和截断策略，得到无限维滤波器的有限维近似。这类滤波方法通常要求系统中的非线性项具有可微性和平滑性。对连续离散滤波模型（动态方程是连续的，而测量方程是离散的）而言，在两次观测之间，条件概率
11、密度函数满足 Fokker-Planck 偏微分方程（或称作 Kolmogorov 前向方程） ，在观测时刻，则满足 Bayes 公式 4；对连续时间滤波模型（动态方程和测量方程都是连续的）而言，条件概率密度函数则满足 Zakai 方程。由于连续离散滤波模型在实际应用中更具有普遍意义，人们重点研究了 Fokker-Planck 偏微分方程（FPE）的解法 2628。实际应用中，FPE 的解析解无法得到，只能求得其有限维近似。通常，基于下列方法求取近似解：（1）Taylor 级数法 26；（ 2）高斯和法 29；（3）网格法 30；（4）Fourier 级数法 31。若所研究的问题具有好的信噪比
12、和验前信息，以及多项式型非线性，则比较适合应用 Taylor 级数法或高斯和法。网格法所需要的计算量随着系统维数的增加而指数上升，因此这种方法难以满足导弹制导对滤波器实时性的要求。Fourier 级数法要求构造一组正交多项式，这也有一定的难度。Galerkin法是一种求解偏微分方程的有效方法，将 Galerkin 法与 Fourier 级数法相结合，可以得到一种快速非线性滤波算法 32。Daum 非线性滤波 33也是一种基于求解 FPE 的方法。Daum 考虑了这样一种情况，即 FPE 的一个解存在并满足一系列辅助条件，而同时条件概率密度函数 能以一个标量 乘上一pxt(,)(,)xt0个指数
13、型函数的形式表示。这个指数型函数以向量 m 和矩阵 P 作为随时间变化的参数。Daum 证明了，在整个时间区间上， 都可以写成 与该指数型函数的乘积，还推导出了 m 和pxt(,)(,)xt0P 必须满足的耦合微分方程组。参数 m 和 P 可以被实时修正。Daum 非线性滤波适用于由连续非线性动态模型和离散线性测量模型所构成的系统。非线性系统 Bayes 滤波不失为一种严格的滤波方法，在理论分析中有较高价值。当然，由于Bayes 滤波中递推求解过程的每一步都要计算概率密度函数(PDF)的积分，这些积分一般都比较复杂，无法求得解析解。事实上，只有对线性、高斯系统，PDF 的积分才有解析解，这意味
14、着 Bayes滤波的实际应用有较大难度。文献34基于 Bayes 滤波理论提出了自举滤波器。该文献把 PDF 用一组随机样本来描述，而不是把它作为整个状态空间的函数。当随机样本数目 N 充分大时，它们就可以精确、等价地描述所要求的 PDF。作为 PDF 函数的矩（例如均值和协方差）的估计可以直接从样本得到。如果有必要，还可以由样本构造 PDF 的函数估计。自举滤波器是一种递推算法，对离散时间问题，它可以传播和修正这些样本。由于这种滤波算法的关键修正阶段（Bayes 规则）是利用一种加权自举来实现的，故称为自举滤波器。自举滤波器的一个优点是适合任何函数非线性和任意分布的系统动态和测量噪声，可见此
15、滤波器的适用范围很宽。近期, 自举滤波又被推广到多传感器多目标跟踪领域, 产生了一些与数据融合技术相结合的新方法 35, 36。只有 N 充分大时，才能保证样本逼近 PDF。在自举滤波器中，每一步滤波样本数 N 都是固定不变的，这样从总体上考虑，N 势必要选得很大，从而带来很大的计算量。文献37基于变样本数的思想，提出了一种快速自举滤波器，主要方法是根据每一个验前样本和样本数 N 的似然函数确61定标准化权值，而验后样本数则根据此权值确定。这种方法不能令样本数明显减少，有时甚至需要增加样本数，而且在估计精度方面无任何改进。作者认为，用区域寻求技术 38自适应地优选出感兴趣的样本区，可以显著减少
16、计算量，同时还可以提高收敛速度和估计精度。应该指出，基于概率方法的滤波器的计算负担要远大于基于统计方法的递推型滤波器，按目前弹上计算机的水平，实时实现有一定困难。但随着滤波算法的改善以及计算技术的新成果，如并行计算技术，在航天领域得到应用，相信这种更精确的滤波算法可以得到实现。粒子滤波 39也是近年来新出现的一种非线性滤波方法，它与自举滤波相似, 基本思想是通过在状态空间中探测出适当的随机粒子，然后在测量的基础上, 对粒子的权用 Bayes 方法修正, 接着由粒子和修正后的权计算出条件均值作为状态估计。这种滤波方法同样也适用于各种非线性、非高斯系统, 不过具体实现起来要比自举滤波复杂。除了以上
17、所介绍的方法，还有统计线性化滤波 1，最小二乘非线性滤波 2， 非线性滤波 40H等非线性滤波方法, 这里不作展开讨论。总的来说，多数非线性滤波方法并不是对每种非线性均等适用。在解决具体非线性系统的滤波问题时，可以考虑建立特殊滤波方法。另外，求解非线性系统滤波估值的计算量较大。因此，对非线性系统的实时控制而言，必须在滤波估值的精度与计算量之间进行权衡。本章将结合仅有角度测量的制导问题，介绍几种新型非线性滤波方法，它们可以较好地克服EKF 的缺点，而且算法比较简单，易于工程实现。5.3 非线性滤波在制导和跟踪中的应用这里, 我们重点讨论单传感器仅有角度测量情况下的制导和跟踪问题, 对其它一些情况
18、仅作简单叙述。描述“被动跟踪”和“被动制导”问题常用的坐标系是直角坐标系或修正极坐标系。无论在哪一种坐标系中系统模型都是非线性的。直角坐标系中，若目标加速度可以用线性的 Markov 随机模型 41来描述，则动态模型是线性的，而测量模型是非线性的；修正极坐标系中，动态模型是非线性的，而测量模型是线性的。早期研究二维被动跟踪问题时，人们的研究工作主要集中在两个方面：(1) 坐标系的选择；(2)滤波算法。文献42指出了直角坐标系内的 EKF 可能表现出不稳定行为，并详细分析了产生这一现象的原因。为了克服这一不足，文献42,43建议用伪线性滤波器，这种滤波器算法稳定，计算简单，且易于实现，但遗憾的是
19、存在着有偏估计性质。进一步的研究结果表明，修正极坐标系是一种可取的坐标系，因为这一坐标系内的 EKF 具有较好的稳定性44,45。近期，文献46,47 分别把极大似然估计方法和非线性最小二乘法应用于被动声纳测量目标跟踪。文献48则把广义最小二乘法结合序列均匀设计优化法用于水下目标定位。在研究三维被动制导问题时，人们还是喜欢采用直角坐标系 13,4952，因为在这一坐标系内系统动态模型是线性的，滤波器的计算量明显减小。也有人在修正极坐标系内研究被动制导问题，并用仿真结果证明了修正极坐标 EKF 略优于直角坐标 EKF45。 文献53 将坐标转换滤波器应用于被动制导问题，即在直角坐标系内利用线性动
20、态方程进行状态传播计算，而在测量时刻，把状态转62换到一个特定的极坐标系内，在这一极坐标系内，测量是转换后的状态的线性函数，然后利用Kalman 滤波方法对转换后的状态进行修正。数值计算结果表明坐标转换滤波器优于直角坐标EKF。文献50 提出一种设定密度滤波器，并将其应用于被动制导。这种滤波方法实际上是一种极大似然估计算法，它适用于动态模型是线性，而测量模型是非线性的系统。仿真结果表明设定密度滤波器优于直角坐标 EKF。文献13,54将 MGEKF 应用于寻的导弹被动制导，收到的效果好于上述几种滤波器。作者把 MGEKF 推广到时变测量噪声情形, 得到自适应 MGEKF55,56，应用效果好于
21、 MGEKF。近期 , 文献57在把 MGEKF 应用于被动跟踪问题的过程中给出了一种更简单的可修正函数。将直角坐标系中非线性测量模型的线性化截断误差当作虚拟测量噪声，作者用测量噪声统计估值器估计出虚拟噪声的均值和方差代入 EKF 算法补偿线性化误差的影响。这种基于虚拟噪声技术的自适应 EKF 应用于被动制导问题要好于直接应用 EKF51。另外, 作者还研究了应用标称状态线性化滤波实现被动跟踪, 结果表明当滤波器初值较准确时, 这种方法明显优于 EKF58。作者提出的自适应两步滤波器应用于被动制导问题，则滤波收敛速度和估计精度均优于MGEKF17, 59，它解决了测量噪声统计特性未知情况下的滤
22、波问题，具有很好的稳定性。文献60用 Daum 非线性滤波理论在修正极坐标系中导出了一种可以实际应用的 Daum 非线性滤波器，估计效果比自适应 Kalman 滤波器要好，但改进的效果并不很明显。这是因为该文献为了推导出简洁的计算形式，作了较多的近似和假设，Daum 非线性滤波理论上的优势并没有被充分发挥出来。文献52在直角坐标系中研究自举滤波器在被动制导中的应用，取得的仿真结果明显好于EKF，MGEKF 和混合滤波器 61。当然，仿真时要花费比较多的时间。对自举滤波器进行改进后，一方面节省时间，另一方面加快收敛速度，提高精度，更适合应用于被动制导。另外，文献62讨论了粒子滤波方法在被动跟踪中
23、的应用，需要解决的也是计算的实时性问题。5.4 推广 KALMAN 滤波(EKF)这里，我们只对 EKF 作简单的回顾：考虑非线性系统(5.1)(),()(1kkxhzu其中， 是 维状态向量， 是 维观测向量， 是 维控制向量， 是)(kx1nm1l维可微向量函数， 是 维可微向量函数， 是 维矩阵， 和 分别是n1hn)(k维和 维独立的高斯白噪声，噪声统计为mkjTjkEk)()(,)( 11Qq Rr系统 0 的 EKF 为(5.2)()(),/()/( 1kkk quxx3 663(5.3)()/()/()/()/1( 1kkkkTQxPxP(5.4) )()/(/1)/()/()1
24、( 1TT kk RhhhK (5.5) 1),(1kkkrz(5.6)/ Kxx(5.7)/()/()()(k PhIP(5.8)00,0/PE此滤波器要求 和 已知。)()(11rQq、 1kR5.5 自适应推广 KALMAN 滤波(AEKF)5.5.1 SAGE-HUSA 噪声统计估值器我们从介绍线性系统的 Saga-Husa 噪声统计估值器出发，逐步引出 AEKF 算法。考虑线性动态系统(5.9)()( )1(1)1() kkkxHzux其中， 是 维状态向量， 是 维观测向量， 是 维控制向量，)(kx1nml、 和 为已知的矩阵， 和 是相互独立的正态白噪声，噪声统计为)(Hkjj
25、E1T1)(, QqkkRr假设噪声均值 、 和协方差 、 是未知的定常向量或矩阵，自适应滤波问题就是基于观1qr1QR测求噪声统计和状态 。)(kx噪声统计已知时的 Kalman 滤波器为(5.10)()/(/ kkKx(5.11)1/)( rHz(5.12)1)(1/ qukk(5.13)/()(/ RHPPKTT(5.14)/( Qkk(5.15)1/I(5.16)00)(,(0xxE当 未知时，连同状态 的极大后验(MAP)估计 可以用11,RQrq ), )/(,11kjxrq极大化如下条件概率密度求得： )(|,(11* kkpJZRrQqX其中， 。2),(,),0(k zzZx
26、xX64由 Bayes 公式 )()(,11*kpJZRrQqX而 与最优化无关，因而问题转化为求如下无条件概率密度的极大值)(kpZ ,|)(,),(| 111111 RrQqrqrqRQX ppkkJ其中，假设 和 相互独立且均服从均匀分布。11rq、由正态性假设易知 )1()1()()0(21exp,|,|)( 1 2212/01 11 10 kjkkj jjc jpp QP quxxQP类似地有 )()(21exp,|,),(| 2121 11kjk kjjc jpRrxHzRzRrQqXZ于是 )()(21)(1)(1)(0( 21121 10 kjkj k jjjjjcJ RQP
27、rxHzquxxQ而 const)()(21 )(1)(2lnlln1 211 kj kj jjJR QrxHz quxQ注意， 有相同的极点。暂且设 已知，则令J与 ln/kj, , , 0q1lnJQ1lJ0r1lJR1lnJ可得噪声统计的 MAP 估值器为(5.17) kj jjkjj11 )()/()/()( uxx(5.18)kj jj T1)()/1()/( q(5.19)kj kj11 /)(xHzr(5.20)kj kjjT11 )/()()()( rxHzrR651. 次优 MAP 估值器在式 00 中，以滤波估值 或预报估值 近似代替计算复杂的平滑估值)/(jx)1/(jx
28、，即可得次优 MAP 估值器：)/(kjx(5.21) kj jjjj11 )1()/()/()( uq(5.22)kj jjj T1)()1/()/( qxxQ(5.23)kj11 )(Hzr(5.24)kj jjjjj T11 )/()()/()( rxHzrR2. 次优无偏 MAP 估值器由式 0、0 和 0 有(5.25)111 )()( qKqkjjE又由式 0 和 0 有(5.26)111)()( rrkjj可见, 式 0 和 0 给出的估计是无偏的。注意到(5.27)(/()/()() 11 rxHrxHz kkkk式中， 。由式 0、0、00 以及矩阵 P 的对称性，有/1/(
29、kx 11 T1T)/()1()/1()/)()/()( QPPKQkjkjkjj jjjjjjjjjEE由此可得 的次优无偏 MAP 估值器为)(1kQ(5.28)()/()/()()( TT jjjjjjj K又由式 0、0 和 0 有 kjkj jjjEE1 1T1T1 )(/()()( RHPR由此可得 的次优无偏估值器为k(5.29)kj jjjj1 TT1 )(/()()(66由式 0、0、0 和 0 引出 Sage-Husa 的递推次优无偏 MAP 估值器为(5.30)()/()1/()1)(1 kkkkk uxxqq (5.31)(/1/1TT kPPKKQ(5.32)/()(
30、)(11 kkkkHzrr(5.33)()/(1TT kHRR5.5.2 SAGE-HUSA 时变噪声统计估值器基于 Sage-Husa 噪声统计估值器，即式 00，可以用指数加权方法联立给出模型噪声和观测噪声的未知时变均值和协方差估值器。可以证明 Sage-Husa 时变噪声统计估值器在一定意义下对时变噪声统计的跟踪是无偏的。考虑线性动态系统 )()()(1( kkkuxxHz噪声统计为 kjjEE)()(,)( 1T1QqkkRr由 5.5.1 中的结果，Sage-Husa 次优无偏 MAP 噪声统计估值器的非递推形式为(5.34) kj jjjk01 )()/()/()( uxxq(5.
31、35)(/)(1/11 T1 T jjjkj PPKKQ (5.36)kj jk01 )/()()( xHzr(5.37)kj jj TT )1()/(11 HR从统计观点看，Sage-Husa 噪声统计估值器是算术平均，和式中每项的系数均为 ，1/()k但对时变噪声统计而言，应该强调新近数据的作用，对过于陈旧的数据应逐渐遗忘。这可以用渐消记忆方法实现。即在和式中每项乘以不同的加权系数，而且按指数加权法，应选取加权系数使之满足j(5.38)1,0,0ff1kjjjj b这引出(5.39)kdbkkjj ,2,)(), 1fff 式中， 叫做遗忘因子。在式 00 中每项乘以 代替原来的加权系数
32、，便可得到时fb j 1/()变噪声统计估值器。易导出其递推算法为67(5.40)()/()1/()(1()1 kkkdkk uxxqq (5.41)(/1/() TT kPPKKQ(5.42)/(11 kk Hzrr(5.43)()/()()() Tkkk HRR假设噪声统计是慢时变的，在 时刻，未知的噪声统计的一种合理的规定是用指数加权平1均定义： kjjbd01f1 )()(qqkjjfQQkjjbd01f1)()(rrkjjfRR这种定义近似地代表了 时刻噪声统计的真实值。在上述意义下，平行于证明 Sage-Husa 噪声k统计估值器无偏性时的推导，可以证明 Sage-Husa 时变噪
33、声统计估值器具有跟踪的无偏性，即 )1()(,)1()(1 kEkEQqrr5.5.3 改进的 SAGE-HUSA 观测噪声统计估值器对于带定常噪声统计 的系统，Sage-Husa 给出的观测噪声统计 的 MAP 估值11,RQq 1,Rr器为(5.44) kj kjjk01 )/1()()( xHzr(5.45) j kjk0 T111 )/(/1)( rzrxzRSage-Husa 用 近似代替 来推导观测噪声统计估值器。如果用)/x)(kj近似代替 将会提高估值器精度, 这引出新的次优 MAP 均值估计器：)/(j )/(5.46)kjj01 )1/(r式中， 是 的滤波估值，由式 0
34、有)1/(j)(j(5.47)/()1()/jj xHz易知(5.48)1()1(1(/()/(1 jjjjjj KIr 由式 0，改进的 估值器为1R68(5.49) kjkj jjjjjjjjk0 TT111 )1()()( /(1)( KHIKHI rrR估值器 0 的均值为(5.50) kjkj jjjjEjjjjj jjkE0 TTT0 TT1 )1()1()1( )()1()1()( HKKHI IR注意到 1T/)( RPjjjjjET)()()(/1 PKj /11T jjjjj P以及矩阵 和 具有对称性，由式 0 可得1R(5.51)kj jjkE0 T1 )1()/()(
35、 HH由式 0 和 0 可得次优无偏 MAP 估值器为(5.52) kj jjj jj0 T T1 )1()/() )()( PKIKI又易知估值器 0 也是无偏的。如果 和 是时变的，那么由式 0 和 0 利用指数加权法可得改进的 Sage-Husa 时变观测噪声1rR统计估值器：(5.53)1/()1)()()(1 kkdkk xHzr(5.54)/ )1(TTT1 kPHKHIKI式中，(5.55)1()ffkkbd为遗忘因子。10fb5.5.4 AEKF 算法仍然考虑非线性系统(5.56)(),()(1kkxhzux噪声统计为 kjjEE, 1T1Qqkk)()()(Rr69假设 和
36、都是未知的。如果在测量时刻 k 已经得到状态的滤波估值)()(11kkrQq、 ）（ 1R，把系统 0 第一式中的 围绕 展开成 Taylor 级数/(x,x)/(k(5.57)TOH.)/()/(, xx式中，H.O.T 代表 Taylor 展开式中的所有高阶项，则该式可以写作(5.58)()(/)1( kkkcx式中，(5.59)()/(/),/()( kuxc (5.60)TOH.)我们称 为虚拟动态噪声，它带有未知的时变统计)(k, )(2kEq )(2kEQ同理，将 在 处 Taylor 展开可得线性化的观测模型,zh1/(x(5.61)/() dxhz 式中，(5.62)1/()/
37、(,1/)( kkkk xhxd(5.63)TOH.)(我们称 为虚拟观测噪声，它带有未知的时变统计)(k, )(2kEr )(2kER由式 0 和 0 可见，虚拟噪声 补偿了线性化模型误差，有利于滤波性能的改善。)和如果利用 Sage-Husa 时变噪声统计估值器对 和 进行估计，那么)22rQq、(5.64)(,/()1/()1()22 kdkk uxxqq (5.65) TT22 )/()/()/( )1/1kk kkk PPKKQ(5.66),1)(1()22 dkkk xhzrr(5.67) TT )/1()/()/()(1 kkxhR式中， , 。)()1ffkkbd0f把式 00
38、 中的 和 用 和 来替换，再加(1rQq、 )1kR)(22rQq、 2R上式 00，就构成了一种自适应推广 Kalman 滤波器。其初值为 0),0(;/),/Px适当选取初值和遗忘因子，进行自适应推广 Kalman 滤波，可得非线性系统状态的自适应估计。5.5.5 AEKF 在仅有角度测量的寻的导弹制导中的应用70寻的制导是一个三维空间中的运动问题，如果在直角坐标系中描述该问题（如图 5.1 所示） ，而且目标加速度用线性模型描述，则系统的动态方程是简单的线性方程，滤波器的计算量较小。图 5.1 中, el 和 az 分别代表视线倾角和视线偏角，d 2 代表目标与导弹之间的斜距，d 1
39、代表该斜距在水平面上的投影。 Y 导 弹 目 标 d2 el az d1 xr X Z zrOy图 5.1 直角坐标系中的目标导弹相对运动描述仿真时，目标和导弹都可以用质点来表示。目标的加速度用 Gauss-Markov 随机过程描述。系统的动态模型为(5.68)()(tttwBuAx式中， 。 和 Tttt)()()( aavvtrtr zyxzyxzyx )(tryx、代表目标与导弹之间的相对位置在直角坐标系中的三个分量； 和 代表目标trz tvyx、 z与导弹之间的相对速度在直角坐标系中的三个分量；而 和 代表目标加速度在)(ttyx、 tz直角坐标系中的三个分量。 t0IA3其中的每
40、一部分代表一个 33 矩阵。 由下式给出t0t其中， 代表机动时间常数的倒数，即机动频率。30IB为导弹加速度，Tmm)()()(tattat zyxu为高斯白噪声向量， 、 和 彼此相互独立。Tttt00zyxww xwtytzt71把式 0 离散化后可得(5.69)()()1( kkkuxx式中， ， Tttt )()() kaavvrkr zyxzyzyx, (5.70)33 323)1(I0IItttet 32/(0It表示测量周期。t动态噪声向量 为)(k(5.71)Tttt00)( zyx而且, (5.72)19)(qkE 32631T)( I0QkE是高斯型白色随机向量序列。)(
41、k观测模型为(5.73)(),()(kkxhz式中，(5.74)T1221 )(tan)(tan),( krkrxzzxyxh为测量噪声，它也是高斯型白色随机向量序列, 而且)(k, (5.75)12)(0rkE1TER与 相互独立。下面，结合式 00 讨论 AEKF 的设计问题。AEKF 可以对四个统计量 、)(22kQq、和 同时进行估计。但是在实际应用中，如果初值 和 选择不)(2kr2R 0)(02r、当，滤波器容易发散。为了增强 AEKF 的鲁棒性，在实际条件允许的情况下，应该尽可能地只对四个统计值的一部分进行实时估计。在上述问题中，系统的动态模型是线性的，因此不存在动态模型线性化误
42、差。动态噪声 的均值和方差可以验前确定其界，所以这里不考虑 和)(k)(2kq的实时估计问题。因为系统的观测模型是非线性的，应用推广 Kalman 滤波原理时存在线性)(2kQ化误差，所以我们希望实时地估计虚拟观测噪声的统计特性。由于只对 和 进行估计，)(2krR我们以 代替 , 把改进的 Sage-Husa 时变观测噪声统计估值器推广到非线)/1(xh)1(H性系统，得到(5.76)1),/()22 kkkdkk xhzrr(5.77) TT22 )/1()/()/1( )1(/()(1)( kkk kk xhPx KxhIKIR72式中，(5.78)1()ffkkbd为遗忘因子。fb为了
43、便于说明改进的 Sage-Husa 时变观测噪声统计估值器的优点，我们将 Sage-Husa 时变噪声统计估值器中 和 的估计算法重写作：)(2kr2R(5.79)1),/()1()(12 kkdxhz(5.80) TT2 )/1()/(/() kkk xhP 比较两种测量噪声统计估值器可以发现，式 0 利用 时刻的滤波估值 对k进行估计，而式 0 是利用 时刻的预测估值 对 进行估计，可见前)1(2kr 1)/1(kx2r者有更高的估计精度。更重要的是式 0 对 估值方法的改进。在该式中，正定矩阵之间只)(2R进行加法运算，因此无论发生何种情况， 的正定性都能得到保证。而在式 0 中，正定矩
44、阵之间要进行减法运算。当遇到滤波器初值选择不当，或系统的非线性程度较高等情况时，0 就有可能求出负定的 阵，这种现象很容易造成滤波器的发散。所以说，式 0 比式 0 更具有鲁)1(2kR棒性。为通过比较说明AEKF的有效性，在数字仿真时，我们同时应用AEKF和EKF来研究“被动跟踪”问题。设制导初始时刻目标与导弹之间相对运动关系为 2t2t2t m/s10)(,/s10)(,m/s10)( 55,35zyx zyx aaavvvrrr动态噪声 的统计特性 , 测量噪声 的均值为零，方差阵为)(k42.kT1kDR式中，, 21.0I )()(00)()()( 222222 krkrr zyxz
45、yxDAEKF和EKF 中，状态估计的初始值均为 2t2t2t m/s0)/(,/s0)/(,m/s0)/( /11958/,/,3/ zyx zyx aaavvvrr状态方差阵的初始估计值为 326341)/(IP此外，AEKF中虚拟观测噪声统计特性的初始估计值为, T20)(r )0()0(T2DR式中，73 )0()0(0)()()0( 2222 zyxzyx rrrD遗忘因子 。985.fb系统动力学模型中 ，采样周期 , 导弹的制导指令由线性二次型最优制导律1sec1.t63给出，其表达式为(5.81)zzzz yyyxxxaCvrat321mt式中，各项增益分别为 ， ， ，而各2go1tNCgot2gogo)(1)ep(ttN增益中 ， ， 为末制导结束时刻。该仿真实例中，制导过程持续3.7sec。3Ntfgof实际应用中，只能利用状态的估计值来实现制导律，但在仿真时可以利用状态的真实值来实现制导律。这里，我们只是利用导引规律建立一定的运动
