1、一、 无轨卡尔曼滤波（UKF）前面已经提到，广义卡尔曼滤波（EKF）是一种应用最为广泛的非线性系统的状态估计算法。然而，由于 EKF 需要通过线性化来传递状态量的均值和方差，如果系统的非线性非常严重，其估计结果就会变得不可靠。本部分讨论的无轨卡尔曼滤波（UKF ） ，是卡尔曼滤波的又一推广形式，其相对于 EKF 能有效减少线性化误差，提升滤波性能。当随机变量通过非线性函数后，EKF 只用了均值和方差真实表达式展开序列的第一项作为近似值。当状态方程和量测方程的非线性非常严重时，取这样的一阶线性化近似值容易导致均值和方差在传递上的显著误差。而无轨转换正是能有效减少这种误差的一种传递方式。本部分首先
2、分析在无轨转换下，随机变量通过一个非线性函数后的均值和方差。接着，我们将利用无轨转换讨论的结果给出 UKF 的滤波基本方程，并且指出其相对于 EKF 是如何减少线性化误差的。3.1 无轨转换非线性系统的问题在于难以用一个通用的非线性函数来传递概率密度函数。EKF 遵循的原则是用均值和方差的一阶线性转换来近似真实的非线性转换，但是这种近似许多时候是不能满足要求的。无轨转换遵循两个基本原则。第一，它对单个的点很容易的进行非线性转换（而不是对整个概率密度函数） 。第二，在状态空间能找到一组相互独立的点，这一组点的采样概率密度函数近似于状态向量的真实概率密度函数。基于这两个原则，假设我们知道向量 的均
3、值为 ，方差为 ，并且找到xP一组其整体均值和方差与 和 都相等的确定向量，我们把这样的向量称为xPsigma 点。接着对每一确定向量用非线性函数 进行转换得到转换后的hy=一组向量，转换后的向量要能很好的估计出 的真实均值和方差。这就是无轨转换的关键所在。举例来说，假设 是一个 维的向量，选择如下 个 sigma 点：x1n2n（3.1） 1, ,2Tiiniii n Px+其中 是 矩阵平方根所以有 ，并且 是nPTPinP的第 行。接下来我们将给出上述的 sigma 点是怎样用来近似非线性转换nPi后的真实均值和方差的。3.1.1 均值的近似假设有一个向量 ，其均值为 ，方差为 ，以及一
4、个非线性函数xxP，我们想要估算 的均值。将式（ 3.1）中每个独立的 sigma 点 通hy=xy ix过非线性函数 得到 ，并对转换后的结果通过加权来估算 的真实均值 。i yy用 sigma 点得到的近似均值用 表示：uy（3.2）21niuiWy加权系数 的定义如下：iW ,2iin因而式（3.2）可以写为：（3.3）21niuiy现在让我们来计算 的值，来看看它是如何相当好的接近 的真实均值。uy y将式（3.3 ）中的每个 围绕 进行泰勒展开。ix（3.4）2211!i ii inuiihDhxxyx注意到对任意整数 ，我们有0k（3.5） 212211212121210i knn
5、ijii jknijij jknijji jDhxhxh x xxx这是因为 。因此式（3.4）中所有的奇次项都为 0，1,inix从而我们得到：（式（3.5）中展开时好像有问题，就是求偏导时忽略了比如 ，21kiinx等一些交叉项，最后一步的结果是对的，因为对 和对 求偏211kiinx mi导得到的形式是一样的，然后代入相同的值 ，所以后面这一项x相同） 。21kjhxx(3.6)22412461! !i iii inuiinihDhhxxxxyx对于上面等式右边的第二项展开得：（3.7） 22211221, 2,1!42inniki iniklilkliklkliklniklkliklD
6、xhxh x xxx其中再次用到了式（3.1）中条件 。将式（3.1）中,inx和 的表达式代入上式得到ikxil（3.8 22 ,1 ,12,12,12n nikl ikilkli klikl klnkll lnklll hxhnxhx x xxxPP）这样的话，式（3.6）就可以写为（3.9）2,12461 !i inukll lni hhxD xxyP将 的真实均值重新列写在下面：y（3.10）241!hEhxxy上式的第二项可以展开写为：（3.11）2212,12,12,11!ninijijijnijijijnijijijDxhExhx x=x=x=x=P因而从式（3.10）出发 可以
7、写为：y（3.12）2,146! nijijijhhxED=x将上式与式（3.9）相比较，可以看出 和 可以匹配到三阶，而线性化只uy能匹配到一阶。所以如果我们用式（3.1）和（3.3）来计算 ，那么它与 的uyy真实均值就能匹配到三阶。这个算法最大的问题是在式（3.1）中需要求取矩阵平方根。但是无轨转换的好处是不用计算线性化矩阵 ，因此也可以减少一部H分计算量。当然，无轨转换（相对于线性化）最大的优点是可以提高均值转换的精度。3.1.2 方差的近似我们将估算方差表示为 ，用下面的表达式表示之uP（3.13）21212322411!i i ii in Tiiiuuuiiiin Ti iuuii
8、 TnjWhhDDhhh xxxxxPyyyyx将上式乘开得到 2 21 002 2002222 14!114i i ii i jj i jTTnTui TTTj TTj jDhhDnDhhDn xxxxxxxxxP3!i i T xx由于 ，上式中其中某些项为。因此估算方差可以写1,iinx为（3.14）21HOTinuiDhxP其中 HOT 表示高阶项（指四次幂或者更高的项） ，忽略高阶项并将式（3.14 ）展开得到（3.15）21, Tniiuklil lhxxP重新调用 和 ，可以将估算方差写成iinkkxiinll,(3.16),1,1 Tniiukll lTnkllklThxxPH
9、将与 的这个等式与 的真实方差相比较，式（3.13）中的近似方差能够与uPy真实方差匹配到三阶。虽然此处近似的阶数和线性化方法中是一样的，但是我们应该注意到无轨逼近的误差幅值要小于线性逼近的误差幅值，因为至少无轨逼近的四阶或者更高阶项含有可以用来相互抵消的符号项，而线性逼近除了项外，再无其他项。THP将无轨逼近总结如下：1、 假设一个均值为 ，方差为 的 维向量 ，并且已知非线性函数xPnx，要估算 的均值和方差，将其表示为 和 。()hy=yuyP2、 构建如下 个 sigma 点向量2n()i（3.17） 1, ,2Tiiniii n xP+其中 是 矩阵平方根所以有 ，并且 是nPTnP
10、inP的第 行。i3、将 sigma 点进行如下转换：（3.18） 1,2iihnyx4、 用下式来估计 的真实均值和方差（3.19）21niui TiiuuiPyy3.3 无轨卡尔曼滤波上一部分给出的无轨转换能够进一步应用到无轨卡尔曼滤波。毕竟卡尔曼滤波算法正是需要利用时间更新和量测更新来传递系统的均值和方差。如果系统是线性的，那么均值和方差就能进行准确无误的更新，但如果系统是非线性的，那么均值和方差就只能进行近似的更新。与基于线性化的 EKF 相比，无轨转换能够进行更加精确的均值和方差的传递。因此，我们用无轨转换来替换EKF 方程来求取 UKF 的运算法则。将 UKF 算法概括如下：无轨卡
11、尔曼滤波1. 维离散非线性系统的状态方程和量测方程n（3.20）111,0,kkkkkftht:XU+WZVWQR2. 给定初值（3.21）()000TEXPX=-3. 时间更新（a ）由第 步到第 步转移，当前对 均值和方差的最佳估计值为()1k-kk和 ，按照式（3.13 ）给出的，先选择 时刻的 sigma 点 。1kX-kP- ()1-()1ikX-（3.22）11 , 2Tikini iikkn XP+（b）将 代入非线性的系统函数 ，得到 时刻的一步预测 sigma 点1ik ()fk/1ikX（3.23）/11,iikkkftXU（c）结合所有的 求得 时刻的一步预测值/1ik
12、/1X-（3.24）()2/1/1nikki- -=（d）同样，按照式（3.13）求取一步预测均方误差阵。不过在这儿需要将过程噪声的方差阵考虑进来。（3.25）2/1/1/1/1/11n Ti ikkkkkki P=XXQ4. 量测更新（a）根据当前 均值和方差的最优预测值 和 ，选择 时刻k /1k-/1k-P的 sigma 点 iX（3.26）/1/1 , 2Tikini iikkn P+X如果愿意为了节省计算量，而牺牲部分性能，就可以不生成新的 sigma 点而直接利用时间更新里产生的 sigma 点，这一步就可以忽略掉。（b）将上述的 sigma 点 代入已知的非线性量测函数 ，得到预
13、测量ik ()h测的 sigma 点 ikZ（3.27）,iikkhtZX（c）结合所有的 求出 时刻的预测量测ik（3.28）21nikki（d）按照式（3.13）估算预测量测的方差阵，在这儿需要将量测噪声方差阵考虑进去（3.29）()()21n Tiikkki=-+zPZZR（e）按照式（3.13）估算一步预测值 和预测量测 之间的互协方差/1k-Xk（3.30）()()2/1/11n Ti ikkki-=xz（上式中是取 还是 ，文中符号没表示清楚，可以查一下其他有关()/1ik-X()ikUKF 的资料）（f）用常规卡尔曼滤波的基本方程完成状态量的量测更新（3.31）()1/1kkkk
14、Tk-xzzK=PX+ZK上面的算法是考虑系统的过程噪声和量测噪声以一种加性噪声的形式进入系统，但是通常情况下应该是以一种非线性的形式加入到系统的状态方程和量测方程中。即：（3.32）11,kkkfthtXUWZV这样的话上述给出的 UKF 算法就不是一个严谨的算法。为了处理这种情况，就需要作状态扩增，把过程噪声和量测噪声也作为状态量来处理。即： ()kakk=XWV然后用 UKF 来估计扩增后的状态 ，UKF 初始化时， 和 按下式取()ak 0aP值 ()00000 0a TaE=-XOOPQR这样我们就能利用上面给出的 UKF 来估计扩增后的状态量，除了在式（3.25 ）和是（3.29）中不用再加入 和 。1k-Qk
