1、KalmanKalman滤波误差分析滤波误差分析1 1 矩阵矩阵RiccatiRiccati方程的解方程的解2 Kalman2 Kalman滤波误差方差阵的上、下界滤波误差方差阵的上、下界1 矩阵矩阵Riccati方程的解方程的解从前面Kalman滤波方程中，我们知道，对于如下系统：11()()()()()()()()()kkkkkkkkkkX tF t X tG t W tZ tH t X tV tXXWZH XV ，连续：离散：其滤波均方误差阵（无偏时即误差方差阵）为：1000()()()()()()()()()()()()()()()var()TTTXP tF t P tP t FtP
2、t Ht RtH t P tG t Q t GtP tP tX t111|1|1,11,111101|01,001,0000(0)Tkk kkkkTk kk kkk kkkkXTTPPHRHPPQPPPPQ 为了弄清楚误差的变化情况，最直接的办法当然是求解上述方程。为方便，下面以连续系统为例求解P(t)。从 的表达式可以看出，这是一个非线性矩阵微分方程。如将起展开则为一个非线性微分方程组，而非线性微分方程目前尚无一般解法，通常只能用数值解法，因此得不到封闭形式的解。但是，从形式上可以看出，它是一个矩阵Riccati方程，仿照在最佳控制理论中，Riccati方程的导出过程，可知，如设：则上述非线
3、性矩阵Riccati方程可分解为两个线性微分方程：()P t1()()()P tC t Dt要验证上述分解是不困难的，求导直接代入即可：为了简便，不写宗量，有：11000010000()()()()()()()()()()()()()()()()()()()(),(),()TTTXXC tF t C tG t Q t Gt D tD tHt Rt H t C tFt D tP tP tC tDtC tP tDtI D tI初条件为可让11111111111111111()()TTTTTTTTTP CDCDCDCD DDFC GQG D DCD H R HC F D DFCDGQG DDCD H
4、 R HCDCD F DDFP GQGPH R HP PF采用扩充状态变量（增广状态矢量）的办法，可以将两个线性D.E合并成一个：1()()()()()()()()()()()()TTTC tC tF tG t Q t GtD tHt Rt H tFtD t 00011012002102200.()()()(,)(,)(,)(,)(,)0(,)0XD EC tP tD tIt tt tt tt tt tIt tI这是一个齐次，其初条件为：假设其状态转移阵为：一般情况下，C(t)、D(t)相互交联在一起，求解过程比较麻烦，在特殊情况下，如Q(t)=0或R-1(t)=0，即没有系统噪声或量测噪声方
5、差阵非常大时（对量测噪声没有任何先验知识）问题可以简化。从动态方程亦可看到，这是增广系统阵为三角阵（上三角阵或下三角阵）。若Q(t)=0，则有 110120021022011001202100220111001202100220(,)(,)()()(,)(,)()()(,)()(,)()(,)()(,)()()()(,)()(,)(,)()(,XXXXXt tt tPtC tt tt tD tIC tt tPtt tD tt tPtt tP tC t Dtt tPtt tt tPtt t 1)100000()()()()()()()()()()()(),()()(,)()TTXXC tF t
6、C tD tHt Rt H t C tFt D tC tPtD tIC tt tPt 1111()()TTTTTTFF (,)t(,)Tt00001001100120121001000()(,)(,)()()()(,)()(,)(,),(,)0(,)(,)()()()(,)(,)(,)()()()(,)TtTTXttTTttTTTtD tt t ItHRHt P t dt tt tt tt ttHRHt dt tt HRHt d与前面比较，可得0000(,)(,)(,)(,)(,)1,TTTTtt ttt tt tT 两次转置，结果不动0100001000()(,)()(,)(,)()()()
7、(,)()(,)tTTTXtTXP tt t P tt tt HRHt d P tt t 0010001000(,)()(,)()(,)()()()(,)()(,)()0TtTTTXtTXt tP tt tPtItHRHt d Ptt tQ t 将内的提到外面来，注意求逆次序有：这就是在是的解。0110(,)tTTTXtt tIH R Hd P0100000000000()0,()()()()()()()()()()()()(),()()(,)()(,)()(,)()()()(,)(,)()(,)(TTXTtTTXtXRtR tIC tF t C tG t Q t Gt D tD tFt D
8、tC tP tD tID tt tIC tt tP ttGQGt dt tP tt tt 对于即的情况，有初条件同样，即001100,)()()()(,)(,)(,)ttTTGQGtdt tt t 比较一般式，可知012000021022000(,)(,)(,)()()()(,)(,)0(,)(,)(,)tTTtTTt tt ttGQGtdt tt tt tt t 1200(,)0t t显然0100000111|1|1,11,111101|01,001,0000()()()(,)()(,)()()()(,)(,)(0),XtTTtTkk kkkkTTk kk kkk kkkkTTXP tC t
9、 Dtt tP ttGQGtdt tKalmanPPH R HPPQPPPPQ 而对于离散系统，滤波均方误差阵为：初条件为1kk将替换第二式中的，再以第一式代入，得1|1,1,1111|1,|11,11|1|1|1|1|1|11|1,1,|1()()TTkkkkkkkkkkTTTkkkkk kkkkkkkkkkkkkkkkkkkkkTTTkkkkkkkkkkkkk kkPPQPPH R HQPCDCPDCQH R H CQ 这就是一步预测的方差阵方程。仿照连续系统的情况，设或分解，有1|111|1,|11,|1TTkkkkk kTTTkkkkkkkk kkkk kDDH R H CD，注意为原
10、系统的转移阵不是滤波系统转移阵1|01|01|01|01|01|011|,kkCP DDI CPD初条件要验证上述也不困难，但为了避免求，因此采用关系式：1|1|1|kkkkkkCPD1|1|1|11,1,|11,|1()kkkkkkTTTTTkkkkkkkkkkk kkkkkkk kCDPQH R HCQD 分别将，的表达式代入，即可求出的关系式，具体运算比较麻烦：1|kkP看是否正确？11|1,|11,|1|1|1|111,|1|11,|1|111|1,|1|11|1()()()TTTkkkkkkkk kkkk kk kk kk kTTTkkk kk kkkkkkkkkk kk kTTkk
11、kkkkkk kk kkkk kPH R H CDCPDPDQH R H PI DPH R H PI DPP 将代入，并整理可得：（两边乘逆阵）为非奇异1|111111|1,|1|11,1111,|1|11,1111,|1()()()()k kTTTkkkkk kkkkk kkkkkkTTTkkkkkk kk kkkkkkTTkkkkkk kkkkDPPH R H PIQH R H PI PQH R HPQ阵，可以消去，（或两边乘以）11|01|01|01,001,00001|01|01|011|1,1,1,11|1,1,TTTTTTTkkkkkkkkkkkkkkkkkTTkkkkkkkkkP
12、C DPQCPDICQH R HQDH R H 证实了上述分解的正确性。初条件为取同理，分解而得的两个差分方程可以采用扩充状态变量（或增广状态矢量）的办法合在一起：|1|1k kk kCD 1|01|01|011121,1,1,21221,1,kkkkkkkkkkCPDI初条件为与连续系统相仿，可令11121|11,1,21221|11,1,kkk kkkkkkkk kkkkkCCDD而将上述方程写成这亦是一个齐次方程，式中：1111,1,1,121,1,2111,1,221,1,11121|1|01,1,21221|1,1,TTTkkkkkkkkkkkkTTkkkkkkkTTkkkkkkkT
13、kkkkkkkkkkkkkkkkQH R HQH R HCPDI 上述方程的解为11|1|1|11|1,|111|1,|11,|11|1,1 1|01|1,|11,11,2200()0kkkkkkkkkkkkkk kTTTkkkkkkkk kkkk kkkkkkkkk kkkk kkkPCDRCCDH R H CDCPCC 不能运用上式迭代，即可求而同样，当或时，上述方程可以简化。时有出即,11|0P 11|1,11,11,1 1|0111,11,1 1|01kTTTTkkkkiiiiiiiikTTTkkiiiiiiDIH R HPIH R HP1,11,1,TTTkiki 注意下标关系，上下
14、标限11,1,1,1 1|0111|1|1|111,1 1|0,1,1 1|01,11111,1 1|0,1,1 1|01,111,11,0()(1,2,3)()()kTTTkiiiiiikkkkkkkTTTkiiiiikikTTTkiiiiikikIH R HPkkPCDPIH R HPPIH R HPP 上述结果直接用迭代法最方便。111,01,0,0,01,010,11,0111,0,0,01,01(0)(0)(0)(0)kTTTTTXiiiiiXiTTkkTTTkXiiiiiXkiIH R HPPIH R HP 1|01,01,001|1|1,1,11,1|1,11,1 1|0,1,1
15、 1|0,111,0,0,0(0),(0,0)0,()(0)(0)(0TXkkkkkTkkkkkkkTkkkkkkkkTTTkiTiiiikiTkTkXiiiiiXkkPPQQQPPPPPPPIH R HPPIH R HPQ 注意同时，也因为可以从求得：1,0111|11,1,1|11,),00kTkikkTTkkk kkkkkkkkTkkk kkkRI RCCQDD 而当时，有1|1,11|1,1 1|01,11,1111,1 1|01,111,1211|1|1|11,11|01,1111,1,121|1()0TkkkkTTkkkkiiiiiikTTkkiiiiiikkkkkkkTTTkii
16、iiikikkk kDICPQPQPCDPQRPP 当时，将上,11|01,1111,12()kTTTkkiiiiikiPPQ 述结果代入，得,1 1|0,1,111,21|01,01,0000,0,0,111,1,00,1110,011(0)(0)(0)0,kTTTkkk iiiik iiTTXkTTkkXkk iiiik iikTTkXiiiiikikkPQPPQPPQPQRRI 这就是当时的结果。方向：1.不断扩展，充分充要条件2.针对具体问题，讨论具体条件2 Kalman滤波误差方差阵的上、下界滤波误差方差阵的上、下界上面我们讨论了K.F.误差方差阵的解，可以看到，对于一个实际系统来说
17、，上述过程是相当麻烦的；另外，在工程上，我们更关心的往往是P(t)，PX的变化范围，即上下界，而不是具体的变化过程，因此就没有必要求出其解来。定理：定理：若系统,1111()()()()()()()()()kk kkkkkkkkXXWZH XVX tF t X tG t W tZ tH t X tV t（离散型）（连续型）是一致完全可控和一致完全可观测的，即有11221012021212000(1,)(1,)(,)(,)0,0,()kIW kNkIIM kNkIIW t tIIM t tINtkN ttPP P t （离散型）（连续性）式中为正整数任意而且假定则有一致的上下界，即2112*11
18、,22121(,11)1kkTTkj kjjjj k NkXnIPIXMk kNH R Zn 以离散型方程为例来证明：首先作 的无偏估计（参阅上一章可测性部分）11221212min()min()，可换成可换成()P t或111213121222323132333123nnnnnnnnaaaaaaaaaaaaaaaaijijijkN1kNk,1,111*11,1,11111,11,1,1,2(,1)()(,1)jjjjkjj kkj iiikkkjjkijkTTkj kjjjj kkj k Nkjj iiijijTTj kjiijj kijijZH XVXXWkijijjkXMk kNH RH
19、XHWVMk kNXXH RWHX 1kkj k N 111nnnijiijjijijbaab 11,1(1,1)iTTj kjjjj kj k NM ik NH R H 11,111111,1111,1111,11,2(,1)(,1)(,1)(,1)kkTTj kjjjj iiij k NijkTTj kjjjj k NkiTTkj kjjjj kijj k NTTk iiiijjkkjNMk kNH RHWMk kNH R VXMk kNH R HWMk kNH R 1kjj k NV*1,1111,1()(,1)(1,1)kkkkkk iiiijkTTj kjjjj kNXXXXMk k
20、NM ikNWH R V 于是，可求出来：*1,111,2()()(,1)(1,1)TkkkkkkTTk iiiik ii k NkPEXXXXMk kNM ikNPQ （无偏估计 即为方差阵）而11111,1(,1)2(1,1)(,1)(,1)(,1)0(),()TkTTj kjjjjjj kj k NM k k NMikNMk kNMk kNH R R R HMk kNMMtrM I MtrM M 根据矩阵的性质，对于一个任意非负定阵，有这一点不难证明:M两边乘？2222222220,?2 11 01 10,0,01 30 21 1551 04 50,05 100 45 62 12 13
21、30,0,1 62 64 4A BAB AAB ABAABA BA BABABABBAAAB 不完全对称12,0.065 均可223,0.09 即可23433.5,343.542.05113sylvastorABAA不定不满足条件（）2225.20255.054.20255.055.05105.0564.20255.0525.21525.502505.056AAB111111112,12122200,0,()()()TnnTnnnnnnTiji jTTTTTMC CccccMC CcccctrMtrMcMtrMMM trMtrMX MXX C CXCXCXCXX XX将表示成的形式有22212
22、222,222()()()()()()()()()()()()Tijiji jijTTTTTTTTTTTTTTCXCXCXCXCtr C CcCcCXtr C C X XX MXXtr C CIXMtr M IX M XXC C C C XCXCCCXX Ctr M I CXXtr MMXMtr MM根据范数的性质即同样2()()TTijijtr C Ctr CCc注意：221121(,1)(,1)(,1)(,1)IM k kNIIW k kNItr M k kNntr W k kNn由于1212121212,0,MMM MMMMM trMtrMtr MtrMtrM()()ijjiijijji
23、ijtr ABa btr BAb a12(,1)(1,1)(,1)IMk kNM ikNM k kN*12,111,11(,1)(1,1)()(1,1)(,1)(,1)kki k NTTTk iiiik iPMk kNM ikNtrQIMikNMk kNMk kN 代入有*121,1111*1,1111*2122()(1,1)()()(1)kTTkk iiiik ii k NkTTkk iiiik ii k NkPMtrQM ikNMIPMtrQtr MIIPn 11()()iitrMMtrM M(,1)W k kN221 nI12max(,)12min(,)221.0,0,0,00ABABA
24、 BABAA注意：对称不能保证对称，只有111111121111211112.,()()3.()()()()()()()()()4.0,00()(0TTTTTkiikkkkkkikkiiiiikkTkABABAtrA Itr AIM M tr M MM tr tr M M MM tr M MM tr tr MMtrMMtrM MtrM MAtrAXXA B 若则，主对角线元素均对于任意，可求1100)5.,TAXAB AB，主对角线元素必须,0,()0()0TAB ABABAB 不一定！但！成立！?既然上面已经证明了Pk的上界，显然若能将下界问题转化为上界问题，问题的证明将会比较方便。根据矩阵
25、理论，若 ，这启示我们要利用逆阵的关系，但实际计算时还有些技巧。实际运算表明，直接采用对应的“倒数”（逆矩阵）关系还是有点麻烦的。只要设法建立某种“倒数”关系，问题就可望解决。12121kPIn关于的证明11ABAB有11*,kkkkPP PP对偶“最优道系统”“对应道系统”11111|1|111|1|1111|1,11,1,11,11111;()TkkkkkkkkkkkkkkkTTkkk kkk kk kkk kTTkkkkkkkkkkPPH R HPPPPPPPPPPPH R HPPH R H（信息大，方差小）令则或|11|11,1,()kk kkkTkkkkkkPPPPP求出的相互关系；
26、求得即的上界的下界。11|11,1111,11111,1,1,1111,111|1,11,111111|111111|11()()()TTk kkk kkkkk kTTTkk kk kk kkkkk kTTkk kk kkk kkkkTk kkkkkk kkkPPHR HPHR HPPPQPQPPQ 11Tk|1,11,1111TTk kk kkk kkkkPPQ写成标准式：,1,11,11111111Tk kk kTTkk kkkkTkkkkHQRHRQ 对应地可作新系统111|1Tkk kkkkPPH R H为方便起见，合写在一起：|1,11,1111111|1TTk kk kkk kkk
27、kTkk kkkPPQPPH R H|1,1111,k kkkk kkkkkkkkPPXXWZH XV 于是可作新的系统：（对应于）,1,11,11,111,111111111(),;()()TTk kk kkkTTTTkk kkkkkkkTkkkkTkjkjkkjkTkkjkjkkjHHQRHRQE W WQRE V VRQ式中可以证明，上述系统是一致完全可控和一致完全可测的。,11,11111,1111,1,111,1111,1111,1111,11,(,1)kTTk iiik iii kNkTTTk ii iiiii ik ii kNkTTk iiiik ii kNkTTikiiiiki
28、 kNTTj kjjjj kj kW k kNQHR HHR HHR HH R H 可控阵1kN1,1,11,1,1()()()1,11TTk ii ik ii iTTTTi ik ik ii iij i k Nkj k Nk （转置求逆，次序不动）令(1,)0,1,1;(,1)M kkNkkkkkM k kN由于 的任意性 可令即为11,111,11,111,111(,1)(,1)0;0()kkTTTTj kjjjj kj kjjjj kj k Nj k NkTTk jjjjk jj k NM k kNH R HQQW k kNIWI 可观测阵M设某一线性预测估计为*111|1,1111,1
29、11(,1)(,1)kTTkkkki kiiii k NkTikki kiii k NXMk kNH R ZWk kNQZ 1,11,TTTkjjjjH11,1,11,1,11,1,1kkkiikjjjj ikkTTTTTTTiijjkikjjjji kj kjj ij iXXWXH WXH W*1|11|11,11,1111,1,111,1,1,1,1,111(,1)(,1)(,1)()(kkkkkTTkk kkkkik iiii k NkTTTTk kk iiiik ikki k NkTTTTiji kj kjk iiiij iXXXWk kNW k kNXQZWk kNQXH WQ 1)
30、kiii k NXV 1,1,111,11,1111,1,1,111(,1)()(,1)(,1)0(,1)(jkTTTk kk iiiik ij kNi kNkTTjij kjk iiii kNkTTTjk kj kjj kNkik iiii kNWk kNQH WQ VWk kNW j kNH WQ VW j kNW 同理有1,1)k kNI21*1|1|(,1)(,1)(,1)(,1)()TkkkkWj k NtrW j k NW j k Nn W k k NE XX于是有1,1,11,1,1(,1)(,1)(,1)(,1)(,1)kTTTk kj kjj k NTjjj kk kWk k
31、NW j kNHR HWj kNW k kNWk kN 11,1,121,111,1,11,1,121,11(,1)()(,1)(,1)(,1)()1(1)kTTTk kj kjjjj kj k Nk kkTTTk kj kjjjj kj k Nk kTk kk kWk kNtrH R HWj kNW k kNWWk kNtrH R HnInn 21Wn WMII（单位阵提出去）*1|1|2111121|1,1,1,1,1,1kkkkTTkkkkkkkkkkkPPnPP 显然，最优线性预测方差阵即2*1121|1,1,11()TkkkkkknP 即：211121,1,1221112121112121()01101TkkkkkkkknIPnnIPPIPIn 即()0TTTAAABAB 注意证明过程可见，对于一致完全可控，一致完全可观测的随机系统，其滤波误差方差阵有一致的上下界。即随着k或t，滤波误差不会无限，也不会无限0。11111111111111110,()()ABABABBABBB IBBBBB IBBB BA ABA BB说明：的证明不能为此证明。11()0B B BB
