chapter2_3

2.5卡尔曼(Kalman)滤波本节讨论的主要问题及方法卡尔曼滤波的状态方程和量测方程卡尔曼滤波的递推算法1、本节讨论的主要问题及方法讨论的主要问题：本节将主要讨论卡尔曼滤波的状态方程和量测方程，及其递推算法。解决方法：利用状态方程和递推方法寻找最小均方误差下状态变量的估计值，即kxˆkxˆkkkxxxˆminTkkkExxx2、卡尔曼滤波的状态方程和量测方程假设某系统k时刻的状态变量为xk，状态方程和量测方程（也称为输出方程）表示为11kkkkxAxwkkkkvxCyAk为状态转移矩阵，描述系统状态由时间k-1的状态到时间k的状态之间的转移；Ck为量测矩阵，描述状态经其作用，变成可量测或可观测的；xk为状态向量，是不可观测的；yk为观测向量；wk为过程噪声；vk为量测噪声。图2.5.1卡尔曼滤波器的信号模型z-1AkCkk-1xk-1xkvkykkskx假设状态变量的增益矩阵A不随时间发生变化，wk,vk都是零均值白噪声，方差分别是Qk和Rk，并且初始状态x0与wk,vk都不相关，且噪声向量wk,vk也互不相关，即2200:[]0,,:[]0,,,0;,0[]0TkkwkkjkkjTkkvkkjkkjkkTkkwEwQEwwQvEvREvvRCovxwCovxvEwv其中jkjkkj013、卡尔曼滤波的递推算法基本思想：先不考虑输入信号ωk和观测噪声vk的影响，得到状态变量和输出信号（即观测数据）的估计值和再用输出信号的估计误差加权后校正状态变量的估计值，使状态变量估计误差的均方值最小。ˆ'kxˆ'kykyˆkxkx~'minˆˆˆ''ˆˆˆkkkkkkkkkTkkkxyyyyxxxxExxx量测方程校正当不考虑观测噪声和输入信号时，状态方程和量测方程为：1'1ˆ'ˆˆˆ'ˆkkkkkkkkkxACxCyxAx输出信号的估计误差为：'ˆ~kkkyyy为了提高状态估计的质量，用输出信号的估计误差来校正状态变量ky~)ˆ(ˆ)ˆ(ˆˆ11'1kkkkkkkkkkkkkxACyHxAyyHxAx其中，Hk为增益矩阵，实质是一加权矩阵。校正后状态变量的估计误差及其均方值分别为：ˆkkkxxxTˆˆ[()()]TkkkkkkkPExxExxxx未经校正的状态变量估计误差的均方值为：'''Tˆˆ[()()]kkkkkPExxxx卡尔曼滤波要求状态变量的估计误差的均方值Pk为最小，因此卡尔曼滤波的关键就是要得到Pk与Hk的关系式，即通过选择合适的Hk,使Pk取得最小值。ˆminkkkPHx'1111111ˆˆˆ()ˆˆ()ˆ()ˆ()()kkkkkkkkkkkkkkkkkkkkkkkkkkkkkkkkkkkxAxHyyAxHCxvCAxIHCAxHCxHvIHCAxHCAxHv递推步骤：1.计算状态变量的估计值：ˆkx2.计算状态变量的估计误差：x~11111111111111111ˆˆ{()()}ˆˆ()()ˆ()()()ˆ()()kkkkkkkkkkkkkkkkkkkkkkkkkkkkkkkkkkkkkkkkkkkkkkkkkkxxxAxIHCAxHCAxHvAxxHCAxxHCAHvIHCAxxIHCHvIHCAxxHv11kkkkxAx由上式可以看出，状态变量的估计误差由三部分组成，可记为x~cbax~其中kkkkkkkkkkvHcCHIbxxACHIa111)()ˆ()(TTTT11TTT1Tˆ()()()kkkkkkkkTTkkaxxAIHCbIHCcvH3.计算状态变量估计误差的均方值Pk：TTTTTTTTTTT[][()()][]kkkPExxEabcabcEaabbccabacbabccacb其中，T1111TTTTT1[][()()ˆˆ)()()(()kkkkkkkkkkkkkkkkkEaaEIHCAAIHCIHCAAIHxxxPCxTTTT111[][()()]()()kkkkkkkkkkkEbbEIHCIHCQIHCIHCTT[][]kkkTkkkkvvEccEHHRHHT1TTTTTTTTT1TT111TTTT11T111[]()]0[](()]0[][()ˆ(][])(()][]()()][)ˆ()ˆ])((kkkkkkkkkkkkkkkkkkkkkkkkkkkkkkkkkkvvxxIHCEbcEIHCHEcbEHIHCEabEIHCAEacEIHCAHEbaEIHCAIHCEcaEvHxxxxvT11TTˆ)])((kkkkkkAIHCxx根据假设的条件，状态变量的增益矩阵A不随时间发生变化，起始时刻为0，则10022110012121001100kkkjkjjkkkjkjjxAxxAxAxAxAxAxAxAxk-1仅依赖于x0,ω0,ω1,…,ωk-2,与ωk-1不相关，即TT1111[][]0kkkkExEx又'112111121111112ˆˆˆ()ˆˆ()kkkkkkkkkkkkkkkxAxHyyAxHCxvCAx仅依赖于xk-1,vk-1，而与vk不相关，即1ˆkx0])ˆ([])ˆ[(0])ˆ([])ˆ[(T111T111T11T11kkkkkkkkkkkkxxExxExxvEvxxE由此可知，TTTT[]0;[]0;[]0;[]0EabEacEbaEca也就是说，Pk仅有其中的三项不为零，化简成TTTT1T11T1[][][]()()()()()()kTkkkkkkkTkkkkkkkkTkkkkkkTkkkkkPEaaEbbEccIHCAPAIHCIHCQIHCHRHAPAIHCICHRHQH4.计算未经误差校正的状态变量估计误差的均方值Pk′：'''TT111111T111111TTT111T11111ˆˆdef[()()]ˆˆ[()()]ˆˆ[(()][(()]ˆˆ[()()][]kkkkkkkkkkkkkkkkkkkkkkkkkkkkkkkkkkkPExxxxEAxAxAxAxEAxxAAPxxAExxxEAQxA由上面推导结果可以看出，Pk′是一对称矩阵，满足Pk′=(Pk′)T。把Pk′代入Pk,'TT'''T'TT'TT''''TTTT'T1T1TT1T'T(()()[()][()]()()())[kkkkkkkkkTTkkkkkkkkkkkkkkkkkTkkkTkkkkkkkkkkkkkkkkkkCPPCCPCRPIHCPIHCHRHPHCPPCHHCPCHHRHPHHHHPHHHHPHSUUSSUSHSUSUSSUPHSUS1T1T'T'T1'][()]()kkkkkkkkkHSUSPCCPCPCP5.计算增益矩阵Hk分析上式，可以发现第一项和第三项均与Hk无关，第二项为一半正定阵，因此使Pk最小的Hk应满足T1T11T1opt''T1()0()()()kTkkkkkkHSUSHUSSUSSPCCPCR6.计算状态变量估计误差的最小均方误差矩阵''T'T1''''()()kkkkkkkkkkkkkkkkkPPPCCPCRCPPHCPIHCP7.卡尔曼递推公式总结如下：'1T1'1T'T'11)()()ˆ(ˆˆkkkkkkkkkkkkkkkkkkkkkkkkPCHIPQAPAPRCPCCPHxACyHxAx假设初始条件Ak,Ck,Qk,Rk,yk,xk-1,Pk-1已知，其中x0=E［x0］,P0=var［x0］,那么，递推流程见图2.5.2。^^''0011111''22222''ˆˆˆ,,,ˆˆ,,ˆˆ,,kkkkkxPxPHxPxPHxPxPHxP图2.5.3求的卡尔曼滤波一步递推算法1'111ˆˆ'ˆˆˆ'ˆˆˆ()kkkkkkkkkkkkkkkkkxAxyCxCAxxAxHyCAxˆkx卡尔曼滤波的特点：采用递推的方式，不要求存储全部的观测数据，便于实时计算；Hk,Pk,Pk′与观测数据yk无关，可以事先计算好并存储；Pk与Qk，Rk是紧密相关的：Rk增大时，Hk变小；（量测噪声大时，增益应取小些，以便减弱量测噪声的影响）P0减小或Qk－1变小或两者都变小时，Pk′变小，Pk变小，Hk变小；（P0减小说明初始估计较好，Qk－1变小表示状态转移的随机波动小，故新观测值对状态预测的校正影响减弱，增益应取小些）例x(t)是一个时不变的标量随机变量，y(t)=x(t)+v(t)是观测数据，其中v(t)为白噪声。若用Kalman滤波器自适应估计x(t)，试设计Kalman滤波器。构造状态空间方程设计x(t)的更新公式0101dxtxtxnxndtxnxnynxnvn状态方程量测方程