带非线性等式约束无迹卡尔曼滤波方法

汤 启，何腊梅

(四川大学 数学学院，成都610064)

(*通信作者电子邮箱helm111@163．com)

0 引言

动态随机系统的状态估计在包括自动化控制、信号处理、航行、目标追踪及计量经济学等领域中极其重要［1］。状态估计的目的在于依据由状态转移方程及由观测方程描述出的状态空间模型，找到最优或次优的估计。在线性系统且噪声是高斯分布的情形时，Kalman滤波是最优的状态估计器，而对于其他的情形，最优的状态估计器却不容易构造得到。对于非线性的系统，许多次优的估计技术被提出，如扩展卡尔曼滤波(Extended Kalman Filter，EKF)、无迹卡尔曼滤波(Unscented Kalman Filter，UKF)、滚动时域估计(Moving Horizon Estimation，MHE)及粒子滤波(Particle Filter，PF)等。

实际问题中，很多动态系统都满足额外的条件信息，即状态要满足一些约束条件，这些约束源自于物理规律、数学条件及一些实际限制条件等，并能用一系列线性或非线性的等式或不等式予以描述。正确地吸纳约束信息到滤波过程中，有助于提高状态估计的质量和精确度;然而，直接处理约束，尤其是非线性情形下的非线性约束，却并不是十分简单。

对于线性约束情形下的状态估计，有大量现有的方法可以使用，如投影法、伪观测法等。对于带约束的非线性系统状态估计问题，一般都是基于前面提到的非线性滤波方法并结合这些约束处理手段构造约束估计算法。

UKF因其自身的简单性及有效性，自从被提出后就被广泛地研究和应用，关于这方面的研究成果颇丰。经过20多年的发展，UKF方法日趋完善，当前关于UKF的研究方向主要在于sigma点结构调整和构建更高阶或更稳健UKF两方面，旨在提升UKF的估计质量和计算效率:前者主要通过调整sigma点的权值从而构造非对称的sigma散布［2－3］以及通过优化手段调整无迹变换(Unscented Transform，UT)中的参数与协差阵分解方式以构建自适应的UKF［4－5］;后者包括通过再引入自由参数以构建更高阶形式的UKF［6］和通过结合其他一些手段(如二次规划、非线性规划等优化方法，粒子滤波)以消解UKF的缺点从而构建更稳健的滤波［7－8］。在UKF框架下，结合一些约束手段，如别针法、优化法、约束同化法、软约束近似等，各种各样的约束无迹卡尔曼滤波算法被提出［9－11］，这些约束UKF滤波算法都通过对UKF估计进行再处理以获得关于状态的约束估计;不少学者也提出在无迹变换阶段时，就将约束信息考虑进UKF算法过程从而直接得到约束估计，例如，针对区间约束的无迹变换形式(Interval Constrained Unscented Transform，ICUT)［12］，针对线性不等式约束的无迹变换(Linear Constrained Unscented Transform，LCUT)［2，13］以及基于内点法构造的无迹卡尔曼滤波［14］。

伪观测法把约束视作额外的观测，因其简便直接而成为处理带等式约束状态估计问题的主要方法之一。批处理方法和序贯处理方法是伪观测法中常见的手段，这两种方法都能用于构造线性最小均方差(Linear Minimum Mean-Squared Error，LMMSE)估计器［15］，前者直接利用约束向量对观测进行扩维，而后者通过对观测及约束的两次更新阶段而实现对约束以及观测的吸纳。然而，批处理LMMSE估计器在实际应用中可能遭遇奇异性以及高计算载荷的问题。序贯处理作为一种经典的手段能够避免奇异性的问题，因其递归地调用序贯LMMSE估计器从而能够减轻计算负担。在序贯处理方法中，考虑观测与约束的处理次序问题，文献［15］给出两种形式的序贯LMMSE估计器。在线性情形下，这两种形式的估计器有相同的计算复杂度且都等价于批处理LMMSE估计器。但是，在非线性情形下，观测与约束的处理次序会影响到估计效果，两种方法的等价性不再成立。接下来，很自然地就会考虑在序贯方法中如何去决定处理次序以求获得较好的估计结果的问题。

在序贯处理中，约束与观测孰先孰后进行处理的问题一直莫衷一是，有学者提出通过度量观测方程和约束方程的非线性程度来决定优先性［16－17］。但从估计融合的角度来看，观测和约束可看作是由两个传感器观测得到，序贯处理过程实际是传感器估计结果的融合过程，而传感器之间的传递方向会影响融合后的估计结果［18］，低信噪比的传感器向较高信噪比传感器的传输方向相较于与之相对的传输方向会得到更好的估计结果，而这可能与前面依据非线性度量来确定次序的方法产生不一致的结论。为了解决这一问题，提出另外一种基于UKF的序贯估计方法。在该方法中，整个系统被虚构地分离为两套子系统，而两套子系统中的观测方程分别是实际的观测方程及约束方程，状态估计的结果则由交互并行使用这两个子系统而形成的两条滤波链接替给出。由于避免去事先确定二者的处理优先次序，且获得的估计效果优于序贯估计器，所以能解决序贯估计方法中处理次序对估计结果的影响这一问题。

1 动态系统及约束描述

一般形式的离散时间非线性动态系统为:

其中:xk∈Rnx表示系统状态(向量)，yk∈Rny表示带噪声的观测(向量)，nx、ny分别表示状态及观测向量的维数;wk∈Rnx是过程噪声，vk∈Rny是观测噪声，协差阵分别是Qk和Rk，f(·)和 h(·)都是非线性函数。非线性约束方程为:

其中:c(·)是非线性的约束函数，dk是约束向量。此外，约束也

可以通过可行域的形式给出:

2 无迹卡尔曼滤波

2．1 无迹变换

不同于在EKF中对式(1)及式(2)进行线性化而后再使用标准Kalman滤波公式，UKF利用无迹变换来获得对状态的估计。在已知随机变量sk的估计^sk与估计误差协差阵协差阵Pk，以及非线性变换zk=g(sk)时，利用无迹变换可以得到关于随机变量zk的均值及协差阵的近似，即^zk和Pzk。

无迹变换是建立在一系列确定选择出的sigma点 χk，i(i=0，1，2，…，2nx) 之上的，这些 sigma 点满足如下条件:其中:仿真实验中无迹变换里面的参数α、β、κ的取值参照其他相关文献的数值设定，设置为α=1，β=2，κ=0，后面不再赘述。

对得到的各个 sigma点应用非线性变换 g(·)，得到γk，i=g(χk，i)(i=0，1，2，…，2nx)，而关于随机变量 zk的估计结果即是:

2．2 无迹卡尔曼滤波算法

类似于Kalman滤波过程，UKF也是一种两阶段估计器，即过程更新及观测更新两阶段。过程更新阶段的步骤包括:

其中: χk－1|k－1指先验 sigma 矩阵，χk－1|k－1，i指先验 sigma 点，χk|k－1，i是衍化 sigma 点，^xk|k－1与 Pk|k－1分别是先验估计、先验估计误差协差阵，γk|k－1，i是观测衍化 sigma 点，^yk|k－1是观测预测值，Pykyk是观测变量协差阵，Pxkyk是交互协差阵。

观测更新阶段的过程步骤有

其中:Kk表示增益矩阵，^xk|k与Pk|k分别指后验估计、后验估计误差协差阵。

值得注意的是式(14)未必一定需要，由式(14)得到的衍化sigma点可以由式(11)中的先验sigma点予以取代，这可以节省计算量但代价是牺牲一定程度的估计性能［20］。

此外，还有另一种等价形式的观测更新算法步骤过程在文献［21］中给出，被称作校正数据同化阶段，其计算过程如下:

其中，χk|k，i'是校正 sigma 点，而 χk|k－1，i、Kk、γk|k－1，i分别由式(11)或式(14)、式(19)、式(15)给出。

2．3 约束处理方式

如前所述，许多线性情形下所用到的约束处理手段可以通过与UKF或者UT结合构造出不同种类的约束无迹卡尔曼滤波算法。下面将介绍基于UKF及伪观测法、约束同化法构建的约束算法。

2．3．1 伪观测法

伪观测法适用于等式约束的情形，也被称作是完美观测(Perfect Measurement，PM)法，其把约束看作是额外的无噪声的观测，从而可以仿照观测更新步骤吸纳约束信息，所以各种各样的带有预测校正形式的滤波方法，如EKF、UKF等，都能与这种方法相结合从而构造出状态约束滤波算法。批处理与序贯处理是伪观测法中常用的用以构造LMMSE估计器的手段，在批处理LMMSE估计器中，将约束与观测进行合并扩维，扩维后的观测向量形式为:

以式(25)重新描述观测方程，得到新的系统模型:

注意到重写过后的系统方程不再带有约束，直接应用前面介绍的UKF算法过程就可以得到约束状态估计与，只需要将式(10) 中的分别替换成

尽管批处理LMMSE估计器直观且简洁，但由于扩维后的观测噪声协差阵是奇异的，在求式(19)中的增益矩阵时可能遭遇数值问题，而序贯处理是应对这一问题的一种方式。

在使用批处理手段时，除了上面的结合UKF的方法之外，也可以使用线性化的方法，即对重构出的系统，即式(26)与式(27)，使用EKF算法过程即能构造出批处理EKF算法。

2．3．2 约束同化法

在约束同化法［22］中，约束被当作观测来进行处理，并通过另外的类似于观测更新阶段的过程，即约束同化阶段，吸纳约束信息。约束同化阶段的算法步骤如下，

其中:ζk，i是约束衍化sigma点，^xk|k与Pk|k由UKF算法过程得到，而d^

k，Pdkdk及Pxkdk由下式得到:

其中，式(32)中加入正定矩阵δInd是用来应对矩阵Pdkdk可能奇异时的情形，10－15≤δ≤10－9。而约束增益矩阵，约束估计x^ck|k及约束估计误差协差阵Pck|k的计算式分别为:

得到的约束估计接下来参与到下一时刻k+1的过程更新阶段式(10)中，即构成带反馈的滤波过程。

观测更新阶段与约束同化阶段各自得到的估计结果之间不同的传递方向，即产生了文献［15］中所给出的两种不同形式的序贯LMMSE估计器方法，即Form 1及Form 2两种序贯形式的LMMSE估计器。对于观测及约束的先后处理次序的不同，会造成Form 1及Form 2估计结果有差异。关于这二者处理优先性的观点一直没有定论。接下来，基于UKF算法，给出一种新的方法，该方法无需事先确定处理次序，但相对于Form 1与Form 2形式的UKF算法能获得更好的估计结果。

3 并行无迹滤波

3．1 子系统方程

关于动态系统及约束方程已在式(1)～(3)中进行了定义，在提出的并行无迹滤波(Sub-system Parallel Unscented Kalman Filter，SPUKF)中，将原动态系统虚拟地分离成两个子系统，分别记作System 1和 System 2，它们分别由过程方程各自与观测方程以及约束方程组合而得到，即:

3．2 SPUKF 算法过程

SPUKF算法由两套滤波更新过程 (即滤波链)所组成，分别记作成Process One和 Process Two。这两条滤波过程是由System 1及System 2不同的先后调用次序而得到，即两条滤波过程各自对应于序贯LMMSE估计器的两套形式，Form 1与Form 2，而每套滤波过程得到的是间隔两个采样时间间隙2T的时刻的状态估计。Process One和Process Two所获得的估计相组合即得到SPUKF方法的估计结果，显然地，只考虑其中某一套过程链不能得到状态在各个时刻的估计结果，所以需要将该两种滤波链进行齿合才能得到完整的各个时刻的估计，就像用拉链将两条链带衔接起来一样，SPUKF算法流程图如图1所示。3．2．1 Process One

图1 SPUKF算法流程Fig．1 Flow chart of SPUKF

在Process One中，System 2先于System 1被调用，即通过System 2得到的更新过后的结果将会作为下一时刻的先验估计向System 1传递，之后接着将得到的估计返送回System 2，设定System 1更新完成后的结果作为Process One的估计，并设定 Process One 的初始值为 x0、P0。令 k=1，2，3，…，Process One的具体算法步骤如下:

对于 k=1，2，…

1)生成sigma点。利用System 1执行完成过后的估计即 Process One 估计，进行 UT 变换生成先验sigma点:

2)处理System 2时的更新步骤。接下来处理System 2，以获得该系统在时刻k的状态估计珘xk|k及珘Pk|k:

类似地，式(46)～(48)也可以做这样的替换。

从上面的计算过程可以看到，在Process One滤波链中，由k－1时刻的估计推更新得到了 k+1时刻的估计，又因为初始值设置为x0与P0，这说明利用Process One滤波链能得到状态向量在偶数时刻的估计。为了补齐状态在奇数时刻的估计，再构造另一套滤波链过程——Process Two。

3．2．2 Process Two

在本节中，呈现另一套滤波过程，Process Two，该过程与Process One并行运行，但对于System 1和System 2的调用与Process One相反，在Process Two中System 1先于System 2被调用。令Process Two得到的状态在k时刻的估计为与也即是System 2处理完毕后得到的估计;且设定由x0，P0经过System 2更新完毕后得到的估计与作为Process Two的初始值。Process Two的算法过程与 Process One相仿，只是先对System 1进行处理，得到的估计再参与到System 2的处理中，处理完毕后得到Process Two的估计，即SPUKF估计。该算法由k时刻的估计经过System 1处理过程过程(49)～(60)及再经过System 2处理过程(37)～(48)后得到Process Two在k+2时刻的估计。由于设置的初始值是，Process Two得到的实际是状态在奇数时刻的估计，从而就能通过结合滤波链Process One及Process Two得到状态在各个时刻的估计。

4 模拟仿真

钟摆运动是一个典型的带非线性等式约束的动态系统范例，文献［9］、文献［10］、文献［22］等都将其作为实例进行仿真以展现约束滤波算法的效果。同样地，为了展现所提出的滤波方法的估计效果，本文也采用了该动态模型并在Matlab 2013a软件上进行了实验仿真。

4．1 RMSE

选取M次蒙特卡罗(Monte Carlo)模拟仿真结果的均方根误差(Root Mean Square Error，RMSE)作为比较各种滤波方法估计性能的参考指标，为避免状态向量各分量的量纲对RMSE的影响，所以只考虑状态各个分量的RMSE结果，状态的第j个分量的均方根误差RMSEj定义为:

其中:j=1，2，…，nx，i=1，2，…，N，这里N表示以T为采样间隔仿真时间段内的时刻总数，x^jk|k，i表示第i次蒙特卡罗模拟中状态的第j个分量在k时刻的状态估计，而表示状态分量的真实值。类似地，也可以构建出状态的第j个分量在k时刻的均方根误差

它能用于评估该种算法的估计稳定性。

为了验证得到的估计对于约束的满足程度，定义的约束均方根误差RMSEc为:

同时，为了能更清楚地给出约束估计算法相对于标准UKF算法对估计效果的提升效果，引入了误差改善比，约束算法的误差改善比计算为:

状态各个分量的误差改善比也可以由上面的式子进行计算。

4．2 钟摆运动仿真结果

定义钟摆在k时刻的状态向量为xk=［θkωk］T，其中θk表示该时刻钟摆所处的角度，ωk表示该时刻钟摆的角速度，则关于钟摆运动状态模型的过程方程及观测方程为:

其中:g是重力加速度，L指钟摆的长度，T表示离散化的时间步长，并假定过程噪声wk及观测噪声vk服从均值为0，协差 阵 分 别 为 Qk= diag(［0．0072，0．0072］) 和 Rk=diag(［0．12，0．12］) 的高斯分布。由机械能守恒定律知，该钟摆系统应满足约束方程:

其中:C是机械能能量常数，其他变量的值设定为L=1 m，T=0．05 s，钟摆质量m=1 kg，g=9．81 m/s2。给定系统状态的初始值为x0=［π/4，π/50］T与P0=diag(［1，1］)，利用约束方程(65)可以计算出机械能常数C。仿真模拟数值结果见表 1、表 2。

表1 几种约束UKF方法均方根误差结果Tab．1 RMSE results of several constrained UKFs

表1给出了不带约束无迹卡尔曼滤波(UKF)和并行无迹滤波(SPUKF)，分别基于扩展卡尔曼滤波及无迹卡尔曼滤波两种形式的批处理EKF估计器(Batch Processing Extended Kalman Filter，BchEKF)与批处理 UKF估计器(Batch Unscented Kalman Filter，Batch)，以及基于无迹卡尔曼滤波构造的两种形式的序贯LMMSE估计器［15，23］(分别记作Form 1和Form 2，其中Form 1表示观测先于约束的处理次序的序贯LMMSE估计器形式，Form 2表示与之相对的另一种处理次序的序贯LMMSE估计器)等方法在100次蒙特卡罗模拟中的均方根误差的结果。由表中给出的结果可以看到:SPUKF与Batch的估计效果相当，且明显好于两种基于无迹卡尔曼滤波的序贯估计器Form1、Form 2以及BchEKF，且SPUKF的估计效果也好于标准UKF估计，表明SPUKF这种约束算法有助于提升估计性能;SPUKF与批次形式及序贯形式的无迹卡尔曼滤波估计器的比较结果，也说明了不考虑观测与约束处理次序的SPUKF方法的有效性。由约束均方根误差的结果看到，由SPUKF得到的估计约束误差较小，相对于 BchEKF、UKF、Form 1及Form 2状态估计更满足约束条件。此外，批次形式下的LMMSE估计方法与序贯形式下的LMMSE估计方法的巨大差异，也印证了前文指出的非线性约束情形下两种形式的伪观测法等价性失效的结论［15］。

表2给出了以上各种估计方法在使用Matlab仿真时的算法运行时间情况以及 SPUKF、BchEKF、Batch、Form 1、Form 2等约束算法相对于标准UKF估计的均方根误差改善百分比。由表中数据可以看到SPUKF的运行时间与两种序贯形式下的UKF算法Form 1、Form 2的运行时间相差不大;由于中间进行无迹变换的次数多于UKF，所以SPUKF的运算时间要长些;从位置的估计误差改善比来看，SPUKF估计算法相对于标准UKF得到的估计，估计精度将提升22%左右，略低于Batch算法的33%;表中BchEKF，Form 1及Form 2的误差改善比为负数，表明这些算法在钟摆运动的实例中估计误差很大，远高于SPUKF、Batch。

表2 几种约束UKF方法运行时间及估计效果Tab．2 Running time and estimate quality of several constrained UKFs

图2及图3分别给出了100次蒙特卡罗仿真结果中各类约束UKF方法在各个时刻的钟摆位置及角速度的均方根误差结果。

由于BchEKF、Form 1及Form 2这三种估计方法的估计效果比较不佳，故在图像中不予呈现。从图中均方根误差曲线可以看到，在稳定阶段时，SPUKF估计方法的误差曲线基本与批处理无迹卡尔曼滤波方法Batch曲线相重合，且低于不带约束的UKF误差曲线，表明SPUKF的效果与批处理形式的无迹卡尔曼滤波算法估计效果相当，也再次表明了文中所提的UKF方法的有效性。

图2 各约束UKF算法钟摆位置均方误差结果Fig．2 RMSE results of pendulum position in constrained UKFs

5 结语

本文中提出了一种新形式的基于无迹卡尔曼滤波的伪观测法估计器，用以处理带非线性等式约束情形下的状态估计问题，且在序贯处理观测及约束时，无需确定二者的处理次序，故能解决序贯处理算法中关于观测及约束处理的次序优先性的问题，而且该种方法能获得与批处理方法相当的估计效果，并通过对钟摆运动的蒙特卡罗仿真模拟予以证实。构造分离并行子系统以及双滤波链形式，是关于带等式约束估计问题的有益尝试。关于SPUKF在其他方面的应用，如线性约束条件、状态模型不精确情形等，该种方法与其他约束滤波算法估计性能比较，以及将双滤波链形式交叉融合这种形式应用到其他滤波算法情形从而构造关于带约束状态的估计方法还需要后续不断的研究。

图3 各约束UKF算法钟摆角速度均方误差结果Fig．3 RMSE results of pendulum velocity in constrained UKFs