《目标跟踪系统中的滤波方法》课件第4章

第4章等式状态约束条件下的滤波方法4.1引言4.2线性状态约束方法4.3非线性状态约束方法4.4线性等式状态约束条件下的粒子滤波算法4.5非线性等式状态约束条件下的滤波算法4.6小结

4.1引言

在状态估计理论的实际应用中，状态向量可能受到某些线性或者非线性函数的约束。例如：地面目标跟踪系统中，目标运动轨迹可能会受道路二维形状的约束；船舶沿海岸线航行过程中会受海岸线物理位置的约束；钟摆在摆动过程中遵循机械能守恒定律等。如果能够将这些固有的状态约束条件有效地施加到滤波过程中，则从理论上可以获得更高的滤波精度。尽管卡尔曼滤波是最小方差意义下的最优滤波器，然而，如果系统是非线性的，则由卡尔曼滤波衍生出来的算法并不能得到最优解。此外，即便系统是线性的，通过施加约束，可以缩小算法中所使用的系统模型和实际系统之间的差异。因此，针对状态约束下估计算法的研究是很有意义的[1-3]。

4.2线性状态约束方法

考虑系统模型

xk+1=Fxk+ωk

(4-1)

yk=Hxk+υk

(4-2)

其中，xk是k时刻的状态值，yk是k时刻的量测值，ωk和υk是均值为零协方差分别为Q和R的过程噪声和量测噪声，F和H是状态转移矩阵和量测矩阵。Kalman滤波器是上世纪中叶由几个不同研究者分别独立提出的[11]。

Kalman滤波方程如下:

Pk|k－1=FPk－1|k－1FT+Q

(4-3)

Kk=Pk|k－1HT(HPk|k－1HT+R)－1

(4-4)(4-5)(4-6)(4-7)当ωk和υk为不相关的高斯白噪声序列时，Kalman滤波器是最小方差滤波器，而且每一时刻估计误差协方差的迹也是最小的。但当ωk和υk是非高斯时，Kalman滤波器是最小方差线性滤波器，也存在一些其它的非线性滤波器会表现出的更好的性能，例如文献[12]针对量测噪声为均匀分布时所提出的方法。当ωk和υk是相关或有色的噪声序列时，式(4-3)~式(4-7)可以被修正，并以此来获得最小方差滤波器[2]。现在假定系统满足等式约束

Dxk=d

(4-8)或者不等式约束

Dxk≤d

(4-9)

其中D是已知的矩阵，d是已知的向量。在这种情况下想要获得满足约束的状态估计xk，即状态估计值应满足

^(4-10)或(4-11)4.2.1模型降阶

在式(4-8)中的等式约束可以用降低系统模型参数的阶数来解决[13]。举一个例子，考虑系统(4-12)(4-13)假设有约束(4-14)如果将xk(3)=－xk(1)代入到式(4-12)和式(4-13)，就能得到(4-15)(4-16)(4-17)式(4-15)~式(4-17)可以被写为(4-18)(4-19)该例说明如何将一个等式约束滤波问题转化为一个等价的但无约束的滤波问题。对于无约束系统，Kalman滤波器是最优线性估计，同时它也是最初的受约束系统的最优线性估计。降阶后模型的维数低于最初模型，因此减少了Kalman滤波器的计算量。这个方法的一个缺点是状态变量的物理意义会丢失，另一个缺点是它不能直接地应用于式(4-9)中的不等式约束。4.2.2最佳量测

状态等式约束可以作为具有零量测噪声的最佳量测来对待[14,15]。如果这个约束通过式(4-8)给定，此处D是一个s×n维矩阵，其中s<n，然后将约束分量扩维为式(4-2)的s个最佳量测，扩维后的方程为(4-20)状态等式(4-1)不改变，但是量测等式会增大。实际上，量测等式的最后s个分量是没有噪声的，这意味着状态的后验滤波估计和s个量测值是一致的。也就是说Kalman滤波估计满足。这个方法在数学上等价于模型降阶方法。4.2.3估计投影

另一种约束滤波方法是将Kalman滤波的无约束估计投影到约束面[5]。约束估计为(4-21)使得

Dx=d

(4-22)其中W是一个正定权值矩阵。该问题的解为(4-23)如果过程和量测噪声是高斯的且设W=(Pk|k)－1，则得到服从状态约束的最大概率估计。如果设W=I，则得到服从状态约束的最小二乘估计。该方法类似于参考文献[16]中使用的输入信号估计方法。可以证明式(4-23)的约束状态估计值是无偏的，即(4-24)令W=(Pk|k)－1，可得到最小方差滤波。即如果W=(Pk|k)－1，那么(4-25)满足所有的。设W=I会产生一个约束估计，它比非约束估计更接近于真实状态。即：对于任意时刻k，若W=I，则(4-26)在文献[17]中，根据一些附加的性质用另一种途径得到式(4-23)。假定约束先验估计是基于无约束估计的，约束滤波方程为(4-27)(4-28)(4-29)如果约束先验估计是基于约束估计的，那么约束滤波为(4-30)(4-31)(4-32)4.2.4具有不等式约束的估计投影

约束滤波器的估计投影方法有这样一个优点，即：它能扩展到式(4-11)中的不等式约束。如果有约束条件，那么约束估计能够通过修正式(4-21)和式(4-22)得到。即要解下列问题：(4-33)(4-34)使得如果主动约束的集合是一个已知的先验值，那么式(4-33)和式(4-34)的解也是等式约束问题的一个解，即(4-35)使得(4-36)式(4-33)和式(4-34)的不等式约束问题等价于式(4-35)和式(4-36)的等式约束问题。因此等式约束状态估计的所有性质也适用于不等式约束状态估计。4.2.5增益投影

标准Kalman滤波可以通过求解下列问题来推导[2]：(4-37)该问题的解给出了最优Kalman增益(4-38)(4-39)状态估计和量测更新方程为(4-40)(4-41)如果将约束增加到该问题中，那么式(4-37)的最小化问题可以写为(4-42)使得(4-43)该约束问题的解为(4-44)当把的值代入式(4-41)中的Kk时，所得结果就是约束状态估计(4-45)这个估计类似于式(4-23)在W=I的情况下的估计值。4.2.6概率密度函数截断

在概率密度函数(ProbabilityDensityFunction，PDF)截断方法中，通过Kalman滤波计算状态估计的PDF，假定它是高斯的，而且在约束边界截断PDF，该约束状态估计等价于截断的PDF的均值。设计该方法的最初目的是解决不等式约束，对其进行稍加改变后可以应用到等式约束问题中。

当状态向量维数大于一维时，该方法变得复杂。在这种情况下，对状态估计进行归一化，使得其分量相互独立。然后，将归一化的约束一次应用于一个分量。当所有的约束使用完之后，使用归一化的逆过程来得到约束估计状态。详细过程可查看文献[2,18]。4.2.7系统投影

状态约束意味着过程噪声也是受约束的。这种认识引导我们可以先修改初始估计误差协方差和过程噪声，然后再执行标准的卡尔曼滤波方程[19]。假定受约束系统方程为

xk+1=Fxk+ωk

(4-46)

Dxk=d

(4-47)

同样可以假设无噪声系统也满足上式约束条件，即DFxk=0。但是这种结果意味着Dωk=0。如果没有满足上述方程，那么噪声与状态就是相关的，这是与系统特性的典型假设相违背的。如果Dωk=0，则

DωkωTkDT=0

(4-48)

E(DωkωTkDT)=0

(4-49)

DQDT=0

(4-50)上述方程意味着Q必须是奇异矩阵，假设Q是行满秩的。举一个简单例子，考虑式(4-12)和式(4-13)中的三态系统。由式(4-12)可得

x1,k+1+x3,k+1=5x1,k+5x3,k+ω1,k+ω3,k

(4-51)

将式(4-51)与式(4-14)结合，有

ω1,k+ω3,k=0

(4-52)

这意味着为了保证该约束系统的一致性，协方差矩阵Q必须是奇异的。要保证Dωk=0，就要使式(4-50)成立。

如果过程噪声协方差Q没有满足式(4-50)，为了保证系统一致性，将Q投影到一个能够满足约束条件的修正协方差Q上，然后用Q代替卡尔曼滤波中的Q。为达此目的，首先计算DT的奇异值分解~~

这里，Sr是一个r×r的矩阵，r是D的秩。下一步我们计算N=U2UT2和Q=NQN。Q的取值满足(4-53)~~(4-54)4.2.8软约束

软约束与硬约束相对，它的约束仅仅只需要近似满足而不是严格满足。通常，在约束是启发式而不是严格的，或者在约束函数具有一定的不确定性和模糊性的情况下，我们希望实施软约束。例如，假设在二维坐标系中有一个车辆导航系统，x(1)指示北方向，x(2)指示东方向。已知车辆在x(1)=mx(2)+b这样的直线道路上行驶，其中m和b为已知的常量。但是由于道路也有一个未知的非零宽度，因此，状态约束也可以被写为x(1)≈mx(2)+b。在这种情况下获得了一个近似等式约束，可称之为软约束。应该说，绝大多数实际工程系统中应该使用软约束而不是硬约束。在卡尔曼滤波中软约束可以用不同的方法来实现。首先，在最佳量测方法中通过加入少量的非零量测噪声到最佳量测项中，将其扩展为软约束。其次，软约束可以通过在标准卡尔曼滤波中加入正则化项的方法来实现。第三，软约束的执行可以通过将无约束估计投影到约束方向而不是精确到约束表面来实现。4.2.9仿真实验

考虑一个导航问题。首先，系统的两个状态分量分别表示地面车辆的向北方向和向东方向的位置，另外的两个分量是向北方向和向东方向的角速度。车辆的角速度θ是顺时针从东方向为起点测量得到的。使用某种测位置设备，获得了车辆北方向和东方向的位置测量，这些测量值是含噪声的。该系统方程表示如下:(4-55)(4-56)其中，T是离散的时间步长，uk是一个加速度输入。过程噪声协方差及量测噪声协方差分别为Q=diag([4，4，1，1])和R=diag([900，900])。初始估计误差协方差为P0=diag([900，900，4，4])。如果我们已知车辆在道路上是以航向θ在行驶，则有(4-57)将约束条件写为Dixk=0，使用两个矩阵，记为(4-58)(4-59)D1直接制约速度和位置。D2根据速度决定位置，即速度约束隐式地对位置进行约束。值得注意的是，此处我们不能使用约束D=[1－tanθ00]。如果我们使用该式，那么位置将受约束，而速度却未受到约束。但是速度是通过系统方程来决定位置的。因此D的这种特性是不符合状态方程的，特别是它违反了文献[17]中的DF=d条件。此时，我们可以采取一些方法来实现状态估计。例如，我们可以使用Q和P0来运行标准的无约束卡尔曼滤波，并且忽视那些约束条件，或运行最佳量测滤波算法，或将无约束估计投影到约束面，或使用概率假设密度滤波截断算法，或使用约束滑动水平估计(MHE)。另外，我们可以利用被预测的Q和P0|0，然后使用标准卡尔曼滤波。由于Q

和P0|0在约束方面具有一致性，如果初始估计x0|0满足了约束，那么状态估计对于所有的时间点都能满足约束。这种方法便是系统投影法。我们注意到，在该情况下，不论是最佳量测滤波器、估计投影滤波器、PDF截断滤波器，还是MHE，都不会改变估计值。因为无约束估计潜在地也受系统投影约束。除此之外，可以选择使用式(4-58)或者式(4-59)中的D1、D2矩阵来约束系统。~~~^~使用MATLAB软件对上述系统进行仿真，一共仿真150秒，每隔3秒观测一次[1]。使用的状态向量初始值为x0=[0010tanθ10]T，并在估计过程中将其作为初始估计。采用蒙特卡罗方法仿真100次。表4.1显示的是两个位置状态分量的平均均方根误差和约束误差的均方根误差。仿真结果表明所有约束滤波算法的约束误差为0。当D1用来作为约束矩阵时，所有约束滤波器的执行结果都是相同的。然而，当D2用来作为约束矩阵时，最佳量测与系统投影方法得到的结果是最好的。

4.3非线性状态约束方法

若状态约束是非线性的，则与Dxk=d相对应，非线性约束方程写为

g(xk)=b

(4-60)

围绕着约束方程中的xk|k－1进行泰勒级数展开，得^(4-61)其中，s是g(x)的维数，ei是自然数基向量的第i个分量。下面给出了n×n矩阵gi″(x)的第p行q列上的元素(4-62)忽略二阶项，则有(4-63)若令(4-64)(4-65)式(4-63)与线性约束Dxk=d相同。因此，在约束线性化后，所有线性约束方法都可以用来解决非线性约束。为了进一步提高算法性能，下面介绍几种其它的非线性约束滤波算法。4.3.1二阶项展开

若在式(4-61)中保留二阶项，那么约束估计可以近似为(4-66)使得(4-67)其中，W是一个加权矩阵，Mi，mi和μi都可以从式(4-61)获得，即有(4-68)(4-69)(4-70)该思路和二阶扩展卡尔曼滤波方法相似，依据系统方程和量测方程的线性化，为了提高性能而保留二阶项[2]。式(4-66)和式(4-67)中的优化问题可以通过数值化方法得到解决。文献[9]中，假定s=1和M是正定时，采用拉格朗日乘子法来求解上述优化问题。4.3.2平滑约束卡尔曼滤波

另外一种处理非线性等式约束的方法是平滑约束卡尔曼滤波(SCKF)[10]。这种方法首先通过线性化方法处理非线性约束，之后将其当作最佳量测来执行。然而，该估计结果只能近似满足非线性约束。如果在每一个量测时刻多次应用线性化约束，那么估计结果将随着迭代次数的增加而不断地接近于约束条件。该思路和无约束估计中的迭代卡尔曼滤波方法类似。在处理非线性系统的迭代卡尔曼滤波算法中，非线性化系统在每个测量时刻重复进行线性化。在平滑约束卡尔曼滤波中，非线性系统在每个时刻进行线性化，并将其作为测量值重复进行滤波，以提高精确性。举例来说，在滤波过程中使用方差为1的量测值，相当于使用方差为10的量测值重复滤波10次。依据SCKF的方法，非线性约束g(x)=h通过下面的算法来实现，该算法在每次测量更新后被执行。

Step1：初始化i为1，该值为约束的个数。

Step2：设R0′=αGPGT，其中G=g′(x)为1×n的雅可比矩阵。R0′是初始化方差，系统约束将其作为量测值纳入到状态估计中。注意到GPGT是g(x)的近似线性化方差，因此，R0′是这个方差的一部分，并作为量测值纳入到系统约束中。α是一个调整参数，通常在0.01和0.1之间。

Step3：设R0′=R0′exp(－i)。该方程被用于逐步降低量测方差，而此量测方差是适用于约束的。

Step4：设。Si是一个与约束信息相关的归一化形式。当Si超出阈值Smax时，迭代终止。Smax的一个典型值为100。或者预先设定一个迭代次数imax，当迭代次数等于该值时，停止迭代。这样做的原因是因为尚无法证明SCKF的收敛性。在迭代终止之后设xk|k=x及Pk|k=P。^^^

Step5：将约束合并入量测向量中，并应用下列公式：(4-71)(4-72)(4-73)这些方程用于量测更新，与标准的卡尔曼滤波方程相同。但是我们合并的量测分量并不是非常准确的约束量测。

Step6：计算更新后的雅可比矩阵G=g′(x)。将i值加1后，回到Step3继续迭代。^4.3.3水平滑动估计

水平滑动估计(MovingHorizonEstimation，MHE)[4,20]用来在卡尔曼滤波算法的基础上，解决下列优化问题：(4-74)上述讨论引出一种针对一般非线性约束估计的相似方法。给定如下非线性系统和非线性约束：

xk+1=f(xk)+ωk

(4-75)

yk=h(xk)+υk

(4-76)

g(xk)=0

(4-77)解优化问题(4-78)使得

g({xk})=0

(4-79)

此处为了符号互用，用g({xk})代替g(xk)，k=1，…，N。这种受约束的非线性优化问题可以通过多种方法解决，因此应用到特定优化算法中的所有理论也可以应用到约束MHE中。该方法的缺点在于问题的维数随着求解步数的增加而不断增大。每得到一个量测值，独立变量就会增加n个，其中n是状态变量的个数。

为了减少计算量，将式(4-78)近似为(4-80)使得g({xk})=0

(4-81)其中{xk}是集合{xM，…，xN}，N－M+1是滑动的尺寸。该问题的维数是n(N－M+1)，滑动尺寸的选择要在估计精度与计算工作量之间进行折中。估计误差协方差PM|M是从标准的EKF递归中获得的。计算公式如下：(4-82)(4-83)(4-84)(4-85)(4-86)文献[21]给出了有关MHE稳定性的一些结果。MHE在其公式的通用性方面有优势，但是即使选择很小的滑动尺寸，与其它类型的具有约束的算法相比，其运算复杂度也很大。

在MHE中，另一个难点是式(4-74)与式(4-78)中对矩阵P0可逆的假设，以及式(4-80)中对矩阵PM|M可逆的假设。一个约束系统的估计误差协方差通常是奇异的[5]，如同在式(4-86)中那样，可以使用无约束滤波算法中的协方差矩阵绕开该问题，但是这种处理方法使得MHE算法变成次优的，即使对于线性系统也是如此。解决该问题的另一种途径是利用文献[5]中的奇异约束协方差，且使其降低到一个对角阵形式。这将导致状态估计进行相应转换，一些转换状态估计的方差将会变成0，意味着式(4-80)中的这些估计值将不会随着时刻的增加而改变。该途径使MHE的结果更优，但同样额外增加了时间复杂度。

递归的非线性动态数据调和联合预测更新最优化是和MHE相似的约束状态估计的另一种方法。这些方法本质上都是一个滑动尺寸为1的水平滑动估计，然而该类方法的最终目标是数据调和(即为输出估计)而不是状态估计，并且该类方法也包括参数估计。4.3.4不敏卡尔曼滤波

不敏卡尔曼滤波(UnscentedKalmanFilter，UKF)适用于非线性系统，基于以下两个基本原则。首先，虽然它很难实现一个概率密度函数的非线性变换，但是它对向量易于实现非线性变换。其次，不难在状态空间中找到一组向量，其采样概率密度函数近似于给定的概率密度函数。UKF通过产生一组称为西格玛点的向量进行运算。UKF使用的西格玛点数目为n+1至2n+1，其中n为状态向量的维数。西格玛点以一种特殊的方式变换和合并，以得到一个状态估计和状态估计误差协方差。约束可以作为量测向量的一部分被合并入量测向量中，将其看做是最佳量测方法进行处理。下面讨论的多种方法都可以实现。一种可行的方法是在前一步无约束的UKF后验估计的基础上，进行先验状态估计。该方法相对于约束UKF，无约束UKF独立运行，在每一个量测时刻，无约束UKF的状态估计与被作为最佳量测的约束值相结合，以获得一个约束的后验UKF估计值。这种滤波器被称为UKF投影，对于线性系统与约束条件来说，类似于式(4-27)~式(4-29)，注意非线性约束可以通过多种方式合并为最佳量测，例如线性化、二阶扩张[9]、不敏变换或者是SCKF，这些途径是一个开放的问题，可以作进一步研究。

另一种方法是在前一步约束的UKF后验估计的基础上，进行先验状态估计[17]，在每一个量测时刻，无约束UKF的状态估计与被作为最佳量测的约束值相结合，以获得一个约束的后验UKF估计值。这种约束的后验估计被作为下一时刻更新的初始条件，该滤波器被称为等式约束UKF。该方法也等价于文献[17]中的量测扩维UKF。对于线性系统和约束，等式约束UKF类似于式(4-30)~式(4-32)。文献[22]提出了一种类似的滤波器，该文认为约束估计协方差将比无约束估计协方差大，因为无约束估计近似于最小方差估计。二步式UKF(2UKF)[22]将每一个后验西格玛点投影到约束面以获得受约束的西格玛点，状态估计是通过以常规的方式对西格玛点进行加权平均获得的，由此产生的估计随后投影到约束表面。注意到受约束的西格玛点的均值并不要求一定满足非线性约束。2UKF很特别，在应用约束之后，其估计误差协方差将增大，其增大的原因是无约束估计是最小方差估计，所以想通过应用约束来改变估计就要增大协方差。此外，如果协方差随着约束的应用而降低，那么协方差矩阵可能会变为奇异的，这可能导致不敏变换中求矩阵平方根的数值计算问题。4.3.5内点方法

一种具有不等式约束状态估计的方法称为内点似然最大化算法

[23]。该算法基于内点方法，但是在约束的实施方面，内点方法从根本上不同于活动集方法。对于不等式约束，正如本章前面所讨论的，活动集方法是通过解决等式约束的子问题进行处理的，然后检查最初的约束是否满足。活动集的一个难点是随着约束数量的增加，计算工作量呈指数增长。利用内点法解决不等式约束问题时，使用牛顿法进行迭代。它也有缺陷，即随着时步数的增长，问题呈线性增长。然而，与MHE类似，该问题可以通过限制滑动尺寸的大小来缓解。4.3.6粒子滤波方法

粒子滤波方法通过传播许多状态估计进行操作，之所以称之为粒子，是因为这些估计值服从真实状态的概率密度函数分布。广义地说，一个UKF可以称为一种粒子滤波器，但是UKF在几个基本方面不同于粒子滤波器。首先，粒子滤波器的时间更新包括了噪声的随机产生，噪声的产生是根据已知过程噪声的概率密度函数产生的；而UKF的时间更新是确定的。其次，UKF中西格玛点的数目是确定的，通常选择n+1个、2n个或者2n+1个，其中n是状态向量的维数；而一个粒子滤波器的粒子数目没有上限，但通常随着n的增加呈指数增加。第三，UKF中状态向量的估计值和协方差的精度达到三阶；粒子滤波器不直接估计均值和协方差，而是估计状态的概率密度函数，当粒子数目接近无穷大时，概率密度估计值收敛于真实的概率密度。正如UKF可以被认为是一个推广的卡尔曼滤波器一样，粒子滤波器也可以被认为是一个推广的UKF。给定足够多的粒子，粒子滤波器总是表现得比UKF好，但是粒子滤波算法的计算复杂度很高。状态约束粒子滤波已经通过多种方法得以解决，例如根据粒子满足约束的程度或者是否形成确保传播粒子满足约束条件的过程噪声来修改粒子的似然函数[24]，而且已经讨论的许多方法也可能适用于约束粒子滤波，例如投影、概率密度函数截断或者SCKF。这些方法可以应用到每个粒子或者只适用于每一次的状态估计，从而可以获得多种约束粒子滤波算法。4.4线性等式状态约束条件下的粒子滤波算法

4.4.1问题描述

假定离散系统的动态方程和量测方程分别为

xk=f(xk－1)+vk－1

(4-87)

yk=h(xk)+nk

(4-88)

其中，f和h为状态转移函数和量测函数，vk和nk分别为过程噪声和量测噪声，它们都是零均值高斯噪声，方差分别为Qk－1和Rk，并且过程噪声和量测噪声互不相关。假定状态向量xk存在如下线性等式约束条件：

Dxk=dk

(4-89)

其中，D为已知状态约束矩阵，dk为已知向量。从贝叶斯滤波角度来看，我们的目标是进行如下递推计算并获得各个时刻的状态向量估计。首先根据0~k时刻观测量y0:k计算状态向量的概率密度分布，即(4-90)为了使p(xk|y0:k)满足式(4-89)，通过对式(4-90)修正的方法实现。使用文献[2]~[5]中的方法都可以实现对式(4-90)的修正，并能取得不错的效果。其中文献[5]的方法在状态向量为高斯分布的条件下，计算过程简单，运算量小，而且精确程度较高，因此本节选用文献[5]中的方法对状态向量进行修正。接下来根据状态转移方程(4-87)计算k+1时刻的一步预测分布(4-91)4.4.2算法描述及步骤

粒子滤波算法采用大量的粒子及其权值来表示状态向量的后验分布，当粒子数目无限大时，可以无限逼近状态向量的后验分布。令{xi0:k，wik}Ni=1表示对后验概率密度p(x0:k|y0:k)进行了N次随机抽样，其中{xi0:k，i=1，…，N}表示k时刻粒子集中具有的N个粒子，wik表示第i个粒子的权值。此时，k时刻的后验概率密度可以近似为(4-92)其中，δ是DiracDelta函数，wik是经过归一化处理的权值。粒子权值要根据重要性抽样原理计算获得。下面给出粒子权值的计算过程。假定p(x)∝π(x)是一个难于采样的概率密度，粒子可以通过一个易于采样的提议函数(Proposalfunction)q产生，即xi~q(x)，i=1，…,N。概率密度函数p(x)通过下式加权近似：(4-93)其中，是第i个粒子的归一化权值。式(4-92)中权值计算为(4-94)由于(4-95)将式(4-95)代入式(4-94)，得(4-96)如果只考虑k时刻的状态估计和量测，并忽略掉k时刻以前所有的状态估计和量测对k时刻粒子权值的影响，可得(4-97)最后还需要对权值进行归一化处理，即(4-98)若状态向量服从高斯分布，则k时刻的状态估计及其协方差为(4-99)下面考虑约束条件式(4-89)对状态向量的影响。如果x表示利用约束条件修正之后的状态估计，W表示任意对称的正定矩阵，x表示没有考虑约束条件之前的状态估计，则具有状态约束的估计问题就转化为下列优化问题:~^(4-100)采用拉格朗日乘子法求解式(4-100)。首先构建式子(4-101)对式(4-101)求偏导数，并令其对各变量的偏导数等于0，即(4-102)求解方程组(4-102)，得(4-103)这种修正状态估计的方法通常称为投影方法，也就是把当前状态估计向约束子空间投影。在具有状态约束的粒子滤波算法中使用投影方法修正状态向量估计值，可以直接利用式(4-103)修正式(4-99)，也可以直接对每一个带有权值的粒子利用式(4-103)进行修正。因此我们获得了两种基于投影的具有状态约束的粒子滤波方法。如果状态向量服从高斯分布，则在最大化后验概率密度函数p(x|y)意义下有W=P－1，在最小化均方误差意义下有W=I[5]。在估计过程中，可能会出现粒子退化(DegeneracyProblem)现象。粒子退化是指经过多次递推计算后，大部分粒子权值都小到可以忽略不计的程度，只有少数几个粒子权值较大。由于那些权值很小(近似为0)的粒子在滤波过程中几乎起不到作用，粒子多样性特征得不到保证，会严重影响滤波精度。为了解决该问题，通常的做法是计算等效粒子数Neff，如果等效粒子数小于某个阈值，则采用重抽样算法对粒子重抽样形成新的粒子集[6]。其中等效粒子数的计算公式为(4-104)

MethodA算法步骤如下：

输入：xik－1，wik－1，Pk－1，yk；

输出：xk，xik，wi，Pk；

Step1：Fors=1，…，N

(1)使用式(4-103)计算第s个粒子修正之后的值xsk；

(2)使用式(4-87)计算第s个粒子下一时刻的状态预测值xsk+1|k；

(3)使用式(4-97)计算粒子权值；

EndFor

Step2：使用式(4-98)将权值归一化；

Step3：使用式(4-99)计算k时刻的状态估计值xk及其方差Pk；~^

Step4：使用式(4-104)计算有效粒子数，如果有效粒子数小于某个阈值，则进行重抽样。

MethodB算法步骤如下：

输入：xik－1，wik－1，Pk－1，yk；

输出：xk，xik，wi，Pk；

Step1：Fors=1，…，N

(1)使用式(4-87)计算第s个粒子下一时刻的状态预测值xsk+1|k；

(2)使用式(4-97)计算粒子权值；

EndFor

Step2:使用式(4-98)将权值归一化；

Step3:使用式(4-99)计算k时刻的状态估计值xk及其方差Pk;~^

Step4：使用式(4-103)计算状态估计值xk的修正值xk；

Step5：使用式(4-104)计算有效粒子数，如果有效粒子数小于某个阈值，则进行重抽样。

在上述算法中只考虑了状态约束为线性的情况，对于非线性状态约束，可以采用泰勒级数展开[5,9]，从而将非线性问题转化为线性问题，然后再利用上述算法加以解决。^~4.4.3仿真实验及结果分析

假设目标在x－y平面内以原点为圆心，r=100m为半径做圆周运动。目标运动的角速度θ=5.7deg/s，目标初始状态向量xk=0=[ξ

ξ

ζ

ζ]T=[100

0

0

10]T。有一个传感器对目标进行量测跟踪，传感器的位置为(xs，ys)=(0，0)，采样间隔T=1s，量测方程为(4-105)···(4-106)跟踪采用的目标状态转移方程为(4-107)其中，过程噪声nk和量测噪声

都服从零均值高斯分布，方差分别为Q=diag([2

2])和R=diag([720.012])，并且两者不相关。状态向量初始值x0=x0，相应协方差为P0=diag([52

12

52

12])。假定在跟踪过程中有如下位置约束：

g(xk)=ξ2k+ζ2k=1002

(4-108)

仿真步数为120步，目标绕原点旋转两周。

由于式(4-108)是非线性约束，需要对其进行线性化处理。也就是首先计算g(xk)的雅可比矩阵，使其符合式(4-89)的形式，然后再使用式(4-103)计算状态估计值的修正值。^实验一误差性能分析

分别采用MethodA、MethodB、GeneralPF以及文献[5]中的EstimateProjectionEKF方法实现目标跟踪。其中，参与粒子滤波的粒子数为1000，MonteCarlo仿真50次。计算不同时刻的均方根误差(RMS)。其中位置分量的RMS计算公式为

(4-109)图4.1(a)和图4.1(b)表明，算法的误差性能由高到低依次是MethodA、MethodB、EstimateProjectionEKF、GeneralPF。本节算法与普通粒子滤波算法的误差性能相比有较大幅度提高，这是因为在滤波过程中加入了状态约束条件，缩小了状态估计的空间，从而提高了滤波精度；本节算法与EstimateProjectionEKF算法的误差性能相比也有不同程度提高，这是因为本节算法本质上还是粒子滤波算法，而粒子滤波是一种比EKF精度更高的估计方法，在使用投影方法对粒子滤波状态向量施加有效约束后，能够取得较好的滤波精度。由量测模型易知，量测误差具有周期性；另一方面，跟踪所使用的约束条件的位置分量具有周期性，从而使图4.1中滤波误差变化呈现周期性。针对这种周期性和滤波结果误差大小的互补性，可以融合MethodA和MethodB以获得更好的误差性能，然而这种融合策略需要视具体问题而定，不具有一般性。此外，图4.1表明，几种方法的误差变化周期有所不同，原因如下：由于状态约束只和目标位置相关，因此各种方法的速度分量的误差周期相同，并且也和常规PF算法的位置误差的周期相同；而在位置分量上施加状态约束后，由于约束分量的周期性，使位置分量的误差周期发生改变。MethodB和EstimateProjectionEKF方法对状态向量估计值的修正时机相同，因此其位置分量的滤波误差周期相同；而MethodA针对每个粒子进行修正，因此该方法与其它方法的位置分量误差周期不相同。图4.1不同时刻的均方根误差实验二算法性能受粒子数影响分析

为了考察粒子数对各种粒子滤波算法误差性能的影响，将粒子数作为粒子滤波算法的参数，粒子数依次选取为{50，100，150，200，300，…，1000}。使用不同的粒子数作为参数值参与运算，仿真120步，采用MonteCarlo方法重复执行50次，计算使用不同粒子数后不同方法估计结果的均方根误差(RMS)。此时，位置RMS定义为(4-110)其中，N表示仿真步数，其它参数含义与式(4-109)中的参数含义相同。实验结果如图4.2所示。由于EstimateProjectionEKF方法的滤波过程与粒子数的多少无关，所以图中显示的误差结果近似为一条直线。图4.2(a)和图4.2(b)表明，尽管粒子数不相同，但是各种算法的误差性能的优劣和实验一结果一致。并且各种粒子滤波算法随着粒子数的不断增大，其误差不断减小。但是当粒子数增加到200以上时，各种方法本身的误差大小基本保持一致，也就是说当粒子数大到一定程度后算法本身的误差性能改善很微弱，使用较多的粒子已经很难有效改善滤波性能了。在粒子数小于200的情况下，注意到MethodA比其它方法表现更出色，尤其是在粒子数选取特别少(小于50)的情况下，此时其它算法的状态估计结果可能已经发散，而MethodA仍然能够将误差保持在较小的范围内。这是因为MethodA在使用状态转移方程对粒子进行传播之前，使用状态约束函数将各个粒子投影到约束子空间中，粒子集在较小的空间中表征向量分布，因此在状态转移之后能够更好地逼近状态分布的后验概率。图4.2不同粒子数下各种粒子滤波算法的均方根误差为了考察新算法的时间复杂度，对实验执行时间进行统计。结果表明。采用1000个粒子，MethodA执行120步需要37秒，MethodB完成同样的过程需要32秒，而常规粒子滤波算法需要31秒。其中仿真使用的计算机CPU(双核)主频为1.6GHz，内存为1GB。可以看出本节中的两种算法与常规粒子滤波算法相比，时间复杂度增加幅度不大。对比两种新算法的时间复杂度，结果表明，MethodA较高的滤波精度需要较高的时间复杂度作为代价。

4.5非线性等式状态约束条件下的滤波算法

4.5.1问题描述

离散系统的动态方程和量测方程分别如式(4-87)和式(4-88)所示。假定状态向量xk存在如下非线性约束条件：

g(xk)=dk

(4-111)

其中，g为已知非线性状态约束函数，dk是已知向量。具有状态约束的估计问题就是在式(4-111)条件下求解式(4-87)和式(4-88)所示系统中各个时刻的状态估计。对于非线性状态约束问题，采用基于泰勒级数展开的线性化的方法存在如下问题[25,26]：①如果非线性方程的局部线性特征不满足，则会出现较大的线性化误差，会对估计结果产生较大影响；②线性化的过程需要计算非线性约束方程的雅可比矩阵，然而并不是所有非线性约束函数的雅可比矩阵都存在；③某些非线性系统的雅可比矩阵求解困难，容易引入计算误差。在非线性滤波问题中，能够有效避免上述问题的算法就是不敏卡尔曼滤波(UKF)。不敏卡尔曼滤波是一种基于UT变换的滤波算法。本节就是通过使用UT变换来计算状态向量在非线性约束条件下的状态估计及协方差，从而避免求解非线性状态约束函数的雅可比矩阵，也就避免了上述问题。4.5.2基于UT变换的最佳量测值方法

UT变换在计算过程中使用状态向量的二阶统计量，是一种更为直接的非线性估计方法。一般来说，对概率密度函数做近似比对非线性函数或者变换做近似容易。UT变换采用一系列点来近似非线性函数的概率密度分布，并通过点的非线性变换来近似概率密度函数的变换。UT变换的过程如下[25-27]：首先确定若干个点(Sigmapoints，西格玛点)来近似状态向量的均值和方差；然后计算每一个Sigma点的非线性变换；最后根据变换后的Sigma点计算变换后的均值和方差。假定一个n维的状态向量xk，其方差为Pk，现在要计算该状态向量在非线性方程(4-111)作用下的变换均值及其协方差。首先使用下式计算状态向量的Sigma点及其权值：^其中，上标m表示w为状态向量的权值，上标c表示w为协方差阵的权值，i=1，…，n，λ=α2(n+κ)－n，参数α，β，κ是事先指定的参数，它们通常取值为0.5，2，3－n；(P)i表示求矩阵P第i列的值，并将其作为列向量。然后计算每个Sigma点的非线性变换，即

ζi,k=g(χi，k)，i=0，1,…，2n

(4-118)

最后计算非线性变换后的均值及相关的协方差矩阵，即本方法将非线性约束作为量测噪声为0的最佳量测值(PerfectMeasurement，PM)看待[2,8]，并将其应用到基于UT变换的估计过程中。即若表示量测噪声方差，则=0。根据线性最小方差估计准则[28]将式(4-119)、式(4-121)、式(4-122)代入式(4-123)和式(4-124)，得到非线性状态约束条件下的估计值xk及其协方差Pk。~~4.5.3基点误差降低方法

由于UT变换的误差受状态向量误差的影响，其状态估计误差性能下降。具体表现为：如果基点误差大，则状态估计误差大；如果基点误差小，则状态估计误差小。为了降低基点误差对估计结果的影响，在SCKF算法[10]思想的启发下，给状态约束施加一系列量测噪声方差，以此来表示不同强度的非线性状态约束。然后依次针对不同强度的非线性状态约束下的状态向量实施UT变换，并采用线性最小方差估计准则计算状态估计值及其协方差矩阵。

由式(4-111)可知，如果状态向量xk存在误差，则计算出的非线性约束与dk之间会存在差异，为了表示这种约束计算过程中的差异，将约束值dk表达为

dk=g(xk)+μ

(4-125)为了确定弱约束方差序列的大小，本节采用和SCKF算法类似的方法。首先用UT变换所得的非线性约束值的方差乘以一个常数a，并将其作为方差初值，其中a>0，在使用中取经验值。在以后的迭代过程中，方差以指数形式递减。4.5.4仿真实验及结果分析

使用钟摆运动估计例子[1]。已知钟摆的状态向量xk=[θk

ωk]T，θk表示k时刻钟摆所处的角度，ωk表示k时刻钟摆的角速度。则系统的动态方程和量测方程为其中，T表示离散化的步长，L表示钟摆的长度，g表示重力加速度，并假定过程噪声[vθ，k

vω,k]T和量测噪声nk服从均值为0、方差分别为Q和R的高斯分布，并且过程噪声和量测噪声相互独立。此外，由机械能守恒定律可知系统中存在如下非线性状态约束:

(4-129)其中C为钟摆系统具有的机械能能量常数。实验中参数设置如下：L=1，T=0.05，g=9.81，Q=diag([0.0072

0.0072])，R=diag([0