卡尔曼滤波器第四章

《卡尔曼滤波器第四章》由会员分享，可在线阅读，更多相关《卡尔曼滤波器第四章（27页珍藏版）》请在装配图网上搜索。

1、第四章 卡尔曼滤波器的设计和实现4.1 kalman filter 模型误差和发散在前几章我们讨论的状态方程和测量方程可以认为是实际物理系统的一种数 学描述。状态方程则是我们用以设计滤波器的依据。由于技术上的原因，数学模 型总是不准确的，我们讨论由此引起的误差。设模型为X = AX + BU + GWk+1kkk（41）Z = HX + VkkkU 为控制项， W 、 V 是白噪声且互不相关，同 X 也不相关，即有kkk0W （0, Q ） , V （0, R ） , X （E x 1 P ）kk000根据以上条件，设计 Kalman 滤波器。时间修正过程：P (k + 1 k )= AP (

2、k. k )At + GQG t（42）文(+ 1 k)= AX Q：k)+ BU k（43）测量修正过程：K (k. k)= P(k； k 1 )H t Cp (k； k 1)H t + R 1（44）P (k : k )：=I K Ct k )H p Ct k 1 )I K 4 k )H T + K 4 k )RK t Q . k )（45）X dfk)= Af (k.k 1)+ KChk)Z - Hf (/k - 1) k（46）注：P 写成（45）式的形式，可以保证 P 分量在计算中的对称性，在编制kk软件时用此式。以下为了方便，设X ( + 1 k )- X- ，X )- X_kP(

3、k + 1 k)= P -， P(k； k)= P ， K (k k)= Kk预测误差协方差(a prior error covariance) 是p - - e v x -丿 X x -虫kkkk现在假定对象以X /状态描写，其动态方程和测量方程为 kX - AX + BU + G W k + 1kkkZ - HX +Vkkk4-7)4-8)V 是白噪声且互不相关，与X 也不相关，即有 k0W 6, Q ),V G, R ),X (E x 1 P )kk000方程式（4T）和（4-8）有同样的n阶，矩阵A、B、G、H、能与前面给的值不等。因此，将依据（4-1 ）式设计的滤波器用于（4-8）式

4、，就要导致很严重的后果，所以就不能说成是最优的。面讨论模型和实际系统不相符合时，产生误差的理论分析方法。以验前（priori）和验后（posteriori）两个误差来说明。它们分别表示估值X -、X与实际系统状态的误差值，即kk验前误差：二 X - X -kk4-9)验后误差：-X - Xkk4-10)它们和滤波器设计中出现的预测误差（predicted） X - -不相等，X - -kkkk的方差为P-，以（4-7）式表示，相对（4-1）（4-6）式它应该是最小的，但实k际确做不到。如果滤波器从理论和实际上完全正确，那么，实际上的误差值X -和 kX 在执行上应该是可接受的小。 k面， 探讨

5、如何 寻找实际 的误差 协方差( actual error covariances )I -(X -)和 E 卜 Xt k k k k首先，考虑时间修正k+1k+ik+i(4一11)=AX+ B U+ GW-AX - BUkkkkk定义模型不匹配的系统矩阵为A A = A - A(4-12)上式可以写为 X - = AX - + A AX + BU BU + GW (4-13)k + 1kkkkk现在，附加误差到系统的实际状态，形成了一个增广状态(an augmented state)A0X k+BAAAX -B0UGk+W BUkGkk(4-14)X k+1X -k+1定义相关矩阵简单起见，

6、UkV-kUkVk(V JkS-kX-kX，TkX -k4-15)VT4-14)式代入( 4-15)式，4-16)kuW -力一 A 0 一UV T A T AATG 一k +1k+1=kk+V -S -A A AVS0ATGk +1k +1j-1kk一11则得到时间的修正方程AUA T + G QGT4-17)V -= AA U A T + A V At + GQGTk+1kkS-k +1=A S A T + G QG T + A A U A AT + (AV A AT + A A V TAT )kkkk4-18)(4-19)在上述三式中，我们感兴趣的是S -，它是实际误差X- = X -

7、X -的相关k + 1kkk阵。U是实际系统状态的相关阵EL dx小。当没有模型不匹配误差时，即 kk kA A = 0、G = G、Q = Q 时，（4-19）式等同于（4-2 ）式，即有 S -= P -。k + 1k + 1所以， UkV - （实际系统状态和误差互相关阵）只是影响计算环节，我们最终k要求的是s-，s-小就说明我们的估值X-和实际系统状态X；相近。k +1k + 1kk面考虑测量修正方程X = X； - X = X； - X - - K Y； - HX -丿kkkkkk kk4-20)上式中，用到Z；，是因为输出值Z的测量只能是实际系统的输出，所以Zkkk定义模型不匹配的

8、输出矩阵为4-21)X = (I - K H )X - - K A HX - K Vkkkkkk k再定义一个增广状态矢量，有4-22)X ；kXkX ；kX-k0+VK kk4-23)类似上边的方法，两边求相关，有 UV T I0 _U(v JIAH t KtKK=KK(I - K H VSK A HI K HV -S -0KKKKKK1-K0R+4-24)则可产生测量修正方程式kt kU = Ukk4-25)4-26)4-27)V = (/ K H)V K A H Uk k k k k+ K RKT + K A H U A H 丁不 tk k k k kS = (I - K H)S-(I

9、- K Hk k k kX/ K H)V-A HTKT + K A H &-(I K Hk k k k k k如果AH = 0、R = R ,即没有模型误差，（4-27）式将变为（4-5）式，即P。k为区别起见，在这里，将P称为预测误差协方差（predicted error covariance）。 k同样，U、V只是为了推导S有用。k k k尽管模型有误差，设估值和状态的初始条件是相同的，即S = V = U = P0 0 0 0结合（ 4-13）式和（ 4-22）式，有X -= A （I - K H ）X - + （A A - AK A H）X + BU BU + GW AK VK + 1

10、KKKKKKKKK（ 4-28）上式为实际误差的系统误差方程式，其意义为：（ 1）如果模型是正确的，它简化为X - = A （I - K H ）X - + GW - AK VK +1KKKK K（2）如果A A丰0或A H丰0，误差系统被实际系统状态X 驱动，除非X KK 是有界的，误差特性将增长。因此，在存在模型不确切情况下，在大多数情况下 实际系统必须是稳定的，避免引起无边界估值误差。3）从（4-28）式可以看出，确定性输入不准确时，即控制项不准确，误差 系统被B UBU驱动，能够引起无边界误差。kk（4）即使A（I K H ）对大的k值是稳定的，它由Kalman filter稳态性质k所

11、保证，模型的不准确性也能导致实际估值误差不收敛和无边界。5）求（4-28）式的数学期望：E t - L A(I K H )E t +(A A - AK A H )E tx 订 + BU BUk+1kkkkkk（ 4-29） 在存在模型误差时，甚至如果E t - = 0，也不能保证E k - = 0，所以，U、0kkV 、S 不能被认为是协方差，只能是相关阵。模型不正确能够导致偏值估值（即 kk估值是有偏的）。我们称以前给定的P为预测误差协方差阵（prediceted error covariance）, kS 为实际误差相关阵(actual error correlation)。 k例 4-1

12、】设模型是一个标量系统f x = xj k+1kI z = x + vkkk式中， v -(0, r), x - (m , P )。k000解：1+k(P r)0按上述模型设计滤波器，有状态估值误差协方差阵和滤波器增益矩阵P jrK k 1 + k Ip r )0设P l、r 10，P曲线如图4 T所示。当有更多测量进行时，P 0，0kk增益也趋于零。这意味着，测量次数越多，估值在理论上变得越可靠，后面测量的影响越来越小。设实际系统为w 0,q ，v kk0,r ,x0xk1zkxkxkwkvkm , P ，可见是一个维纳过程(Wiener process)，00但上面却将其看作为一个随机常值

13、(random constant)。有测量修正方程式对于研究实际误差关系xkx ，我们需要前面推导的公式。有 A 0、 kH 0,所以不必计算U、V、Vkk，就可以直接计算实际误差相关阵S 。 k因为时间修正为：Sqk测量修正为：K 2Skk1 k 1K 2rkP r1 k P r0P 2 r01 k P r 20利用上述公式可以得到：时间修正：测量修正：时间修正测量修正一般形式SPq P 2rP1 P rq000011P r21 P r 200SP 1P rq 1P r 200P r 20021SP 102 P0rq 11 P r 2012 P r 20P10k1q1i-e1 k P r 2

14、0设 P1 ，r 10 ，q00 .01 ，S 曲线如 4-1 中所示， k可看出完全不同于 P 。k求 S 的极限： kk i k 1ki 2 i0k 1 k 1 k 2k 1i26i0所以i2 P r 20w1 2k P r k2 P r 20 0r1kq P230q k2 P rk3 3 k3 2 P 2 r200k2 P 2 r20可见，S是随着k值（测量次数）增加而增加。在仿真计算时，利用公式:kPk1P2kPrkS =（1 - K）2 S - + K 2 rk k k k来进行，可画出图 4-1。【例 4-2】在降阶滤波器中模型不匹配。 设实际系统的方程式为（4-8）式，分析设计时

15、，采用（4-1）式的模型方程式，往往是模型的阶数n小于实际系统的阶数n。在实际工程中，如果阶数nm p m 取值相当，它仍能给出我们所要求的主要信息，否则将产生误差。解：设T G Rnm.xnp是状态转移矩阵，它使实际系统的状态X 转移到模型状态 kX，即X = TX 。设T有满秩的n行，由于是降阶，所以有m n。 kkkm定义所选模型不匹配矩阵为A A = A T + ATA H = H HT式中T +是T的伪逆，因为T + T = I,有满秩的n行，所以T +为右逆矩阵 m实际的误差为 n 个矢量pX - = X - T + X - kkkX = X - T + Xkkk结论：（1）对于一

16、个降阶滤波器，当考虑它由于没有估计到全部状态而引起的实际误差相关阵S -时，以可按（4-17）式至（4-19）式计算。其中，A用T + AT置k换并按（4-25）式至（4-27）式计算，且H和K 7要用HT和T + K置换。kk为简单记，上述说明设，U = 0，U = 0。kk2）对降阶滤波器，当考虑实际误差时，要采用（4-28）式，必须做如上相同置换，还要B j T +B。【例 4-3】当系统的不稳定状态未能模型化时。先说明 Kalman filter 的发散（divergence）。当实际误差相关阵 S 没有边界，它的期望值偏离预测误差协方差阵 P 时， kkKalman filter 发

17、散，又叫真发散（ true divergence。如果S保留一定边界，但它的值比P有明显大的值，称其为视在发散 kk（ apparent divergence 。，如图 4-2 所示。上述情况的出现，是由于滤波器不稳定或者实际系统不稳定状态没有被模型化。在例4-1中，C,G （：Q ）是不可达的，给出一个滤波器不稳定的例子（S g ）。k这个例子，将说明实际系统的不稳定状态没有被模型化而造成S发散。k 设系统状态是标量随机偏置（ scalar random bias） x = x k+1k（4-30）I z = x + vkkkv （0, r）, x （x , P ）。k000从例4-1可有：

18、预测（估值。误差协方差（对于x - x。是kkPP /r匚1 + k（P rY K k1 + k （P r）0 0将基于(4-30)设计的 Kalman filter 用于(4-31)，有P r ( .)x= x +0z - x 丿k+1k 1 + (k + 1 )(P r丿 k+1k0可见，当k T时，P和K T 0。kk现在假设实际模型为1 tX =Vk +10 1z = bk0 x kX k+ vk4-31)v(0, r), x(x, P )，式中t为采样周期。设X ax心)x (k )t，可得 k000k =12零输入时的响应在图4-3中，可以看出有一个随机斜坡(random ramp

19、)。绘制图 4-3 时，考虑-x G + 1)1 t-x G )x (k + 1)20 1x1 ()2f x (k+ 1)=| x (k+ 1)=2x(k)+ tx (k)x1 (k)22所以，根据初始条件，有f x (k + 1)=x (k + 1)=2x(k) + tx(0)x1 (0)2对x(k)的估值也如上式。可见，在设计Kalman filter时，所拟的模型缺少一项1随机斜坡项。因此，新的模型比实际系统的模型降阶一次。根据上一节讨论的情 况，有 x = T X ，即kkx (k ) =-xx1 (k )2由于x (k)没有被建模，所以2T = Il 0 T += Tt (TT t

20、)-1 = h 0t是T的右逆矩阵1 0T + AT =0 0不匹配模型的矩阵为- 0A A = A T + AT =0T,A H = H HT = 01T+KkP r1 + k (P r )0 0所以，实际误差系统为(据(4-28)式)Xk+1=A (I - K H )X - + (A A - AK A H)X + BU - BU +kk将有关T + AT t AHT t H ，T +K tK，0 ，则X1+k (P /r)10XOf +o0 P /r X +k1 + k Cp / r )00T1vk4-32)1+所以1 + ( - 1)(P r)1 + k Cp r )0 .0由上式可知，

21、误差和X :有关，由于x，(k )是常值Lx do )，使X - T (随K T k22k 十 1以t斜率上升)，当k很大时，Kalman filter增益t 0，上式变为 1 0 0 tX -=X-+X k+10 0k0 1k可见，误差没有边界的增长，其原因就是因为实际系统的不稳定部分没有被模型 考虑。4.2 防止预测误差协方差阵发散的方法我们讨论两种防止预测误差协方差阵发散的方法：虚拟过程噪声注入和指数数据加权法。这两种方法均可做到当k T时，阻止P t 0，即也包括增 益kK t 0。k下面以两个例子来说明这两种方法。【例 4-4】采用过程噪声注入防止发散。设实际系统的模型如例 4-3所

22、给，是一个没有过程噪声的随机斜坡偏置，即1 t X 01k(4-33)z、110X f + vkkk而用于 KF 设计的状态模型，是一个没有过程噪声的随机偏置，即x = x k+1k(4-34)I z = x + vkkk由于系统状态模型中有不稳定部分未被模型化，所以实际误差系统的方程式 由(4-32)式给出。当k t g时，得下式1 00 tX -=X - +X k+10 0k0 1k4-35)由X的常值性，可以预见实际误差相关阵S-将随着k T而增加。类似地推导, kk也可以得到当k T8时，S T，所以KF发散。k下面以如下状态方程来设计 KF：k+1=X + wkkX +vkk4-36

23、)w (0, q)， v (0, r)kkx (x , P )。0 0 0按 KF 公式，有P- = P + q k +1k4-37)P-1 +k4-38)可见，只需q丰0，就不难发现,P值随k T就不趋于零。图4-4是P = 10， k0r = 10，q = 0.1时，经计算后得到的P k图。当k T时，P不趋于零。 kk当k T时P- = P- AP-(4-39)k+1k +1 有P - =+ q，所以(-)2 - qP - - qr = 0，方程的正解为r+ P-4-40)P 的稳态值为k11 + 一 一 14-41)所以，KF的增益为4-42)有非零的稳态值4-43)图4-5中绘出了稳

24、态增益和q/r之间的关系。图 4-5现在，假设实际系统是随机斜坡，如（4-33）式所示，其中v（0, r），x k0(X，, p00）。应用上述KF对这个实际系统,存在模型不匹配误差，结果如例4-3 的（4-32）式。由于其K趋于常值，所以有k实际误差系统为X -=1 Kk0_X +0TX +K 一kk +100k01k0vk4-44)式中， K 由（4-42）式给定，（4-45） k式和（4-32）式的差别是，在这个例子中，虚拟过程噪声已经被加到模型中，则 K 不趋近于零。 k这意味着，当 k 变大以后，测量值是起作用的。（4-44）式趋近于稳态误差系统X -=_1 - K 0_X- +0T

25、X +K 一k+10 0k01k0vk4-45)这有两点应该提醒：（1）如果q丰0，实际系统的测量噪声V当测量次数k T 8时，不断激发误差kX -，就是在x (k )= 0时，也使壬-(k )工0，注入噪声过程使滤波器有次 k +121最优的特性。(2)如果正确选择q，在注入噪声后，即使存在模型不匹配问题(本例是有部分 模型未被模型化)，误差仍然是有界的。因为，模型 (A,H )是可检测的且 (A,GQ )是可达的，所以A(I - KH )= 1 - K总是稳定的，即1- K 1 , Q、R为常数矩阵。a 1意图 4-6 00 r0 -V=，S=kk t1k00S 是有界的。 k例 4-5

26、】指数数据加权。 设模型协方差矩阵为R = R a - 2 (k+1) kQ = Q a - 2 (k+1) k味着，随着时间k增加，过程噪声和测量噪声协方差阵衰减，其结果是由于以指 数规律随k T衰减噪声，这就使我们更加确信当前的数据。采用一组新形式的滤波公式，简单的推导如下。P =- 1 + HT R-1 H 】1(447)k+1k +1k+1 k +1 k +1利用矩阵逆公式，有P = P - P - H T (H P - H T + R y1H P -(448)k +1k +1k +1k +1k +1k +1k +1k +1k +1k +1P -= A P At + G Q GTk +

27、1k k kk k k将( 448)式代入( 449)式P-k+1=A P - - P - H tkk(HP - H Tk+R -1HP-kA T + GQG类似求法，将代入 -，经整理可得到k +1k + 1X -= A(I - K H- + BU + AK Zk + 1kkkk kK = P-HT(HP-HT + R)-1kkk449)450)451)452)450)、(451)、(452)三式为 KF 的一个验前递推公式(将时间修正和测量修正结合）。将（4-46）式代入（4-50）式，有AkP-HT-HTkk+ R a - 2 (k+i)-1 HP -kA T + GQGP- ak+12

28、 (k+1) = a 2 aP -a 2k kP -a 2kH t HP -a 2kH t-1HP -ak2k定义P 辿P -a 2kk k上式成为R )1Pa=a 2 AP a -PaH THP a H T+HP ak+1kkka 2丿kA T +Po0GQGTa -2(k+1)(4-53)AT + GQG T(4-54)(4-55)初始条件是Pa0KF 增益矩阵为P-H T(HPk- H T + Ra -2(k +1) -1 k估值修正方程式为k+1P a HT HP a HT-14-56)二 AX - + BU +kkAKkhX-)kk=A(I K H )X - + BU +kkkAK

29、Zkk4-57)上述KF有时变噪声协方差Q和R。 kk考虑如下方程式W (0, Q )，Vkkkf X= a AX + BU + GWk + 1kkkI Z = HX + Vkk C, R a 2),计算 P a 和 K 。 kk4-58)P a在形式上同（4-53）式, k计算（4-58）式fR )1P- =a 2AP - P - HTHP - Ht +HP -k + 1kk1 k a 2 丿kAt + GQG t(4-59)这是（4-58）式的误差协方差，过程和测量噪声是不变的。当噪声是时变的，则 是 Pa。考虑式(4-54) Pa AP- a 2k，由于a 1，当k T 时，Pa p-，

30、可k +1k 十1 = k +1k +1k +1认为P a不趋于零，所以，K也不趋于零，这样对于大的k，测量数据也不会不 k + 1k起作用了。下面给出一个具体推导的例子。例 4-6 】设系统模型为4-60)4-61)f x = xI k + 1kI z = x + vkkkv (0, r), x (x , P )。k000在以前的计算中，当k T8时，P T 0，所以K T 0。kkP a - kk我们假设 KF 有指数数据加权，将有关数据代入（4-55）式P a (r： a 2)、k: a 2P a + (ra 2);v k丿P a P ak +1k-2k Li=0-2k L-2kL1 时

31、，有解-2 (k-i-1-2(k-1)a 2ii=0-2 (k-1) ( 1- a 2ka2所以= a -2 k L1(1-a -2k1-将P a代入上式，有k11 a -2 kP a =a - 2 k P -1 + k0r1 - a - 2 丿4-62)或PakPa 2k0+ (P r )x 2 k 14 - a - 2 k)C - a -204-63)4-62）式可见，当稳态时，( 1 )P a = r1 -I a 2丿Pa图 4-7Ka2所以，如果a 1 ,当k T g时，Pa与K不趋于零。P = 10 , r = 10 , a = 1. 1 时，误差协方差的变化曲线如图 4-7所示。4

32、.3 滤波的稳定性由于研究随机过程估值X （t）渐近性态有困难，因此，在讨论随机系统的稳定性时，则主要研究方差方程，计算的方差值是确定性的，而估值则做不到这一点 对于KF,当所用动态噪声和量测噪声以及初始先验值是正确的，则KF是无偏估 计，因此，估计误差的均值是零。这样，只要研究误差的方差，就相当于已经掌 握估计量的一阶及二阶矩的信息（如果分布是高斯的，则等于已经掌握了过程的 概率分布函数）。滤波稳定性：在设计滤波器时，由于对初始状态的统计特性了解的不够确切，因此，初始值X和P的给定就不一定准确。如果，经过一个充分长的计算时间 00后，初值的影响可以忽略，这个滤波器是稳定的。 为了讨论滤波器的

33、稳定性，先给出稳定性的定义。 设系统为X = A X + U（4-64）kk , k -1 k -1k -1式中， A 为可逆转移矩阵。k,k-1定义：（1）如果存在常数c 0，使得对所有k 0，有A II 0 , c 0，使得对所有k l 0时，有12IIa 0，c 0，使得当k l 0时 12|a 0k,i i-1i-1i-1 k,ii 二 k - N +14-74)S AT H T R -1H A j , kjj = k - N +10j j ,k4-75)如果存在正整数N和a 0、 0，使所有k N时，有a I 0、卩0，使所有k N时，有a I S At htR -1H A N，使(

34、4-74)、(4-75)式都存在一致的有限上界和正定下界。对于常值系统，( 4-74)至( 4-77)式成为S A lGG T C) 04-78)S(A)H T HA l 04-79)式中，n 为状态维数，且在上述两式中，设 Q 0 ， R 0 。在满足上述基本定理情况下，有三个推理。1对于一致完全能控制和一致完全能观测的线性系统，当时间充分长之后， 它的最优线性滤波值将（渐近地）不依赖滤波初值的选取，而且有界的量 测输入将导致有界的滤波输出。2.根据以上基本定理情况，如果P（0）、P（0）是两个不同的初始误差方差阵,12并用P（k）、P（k）分别表示从它们出发按KF公式算出的第k个时刻的滤波12误差方差阵，则存在常数c3 0和c40，使当所有k工1工0时，则P (k ) P (k )c e c34(k1) P (/) - P (/)2 1此式表明，对于一致完全能控制和一致完全能观测的线性系统，当k f 0 时|p （k） P （k）| 0、R 0时，其一致性总是满足的），存在一个唯一的正定阵P，如果从任意的初始方差阵P出发，当k f 0时，恒有P f P。0k