离散卡尔曼滤波

卡尔曼滤波TheoryofKalmanfilter1卡尔曼滤波与最优估计•卡尔曼滤波是一种最优估计技术！•它能将仅与部分状态有关的测量值进行处理，得出从某种统计意义上讲估计误差最小的更多的状态的估计值。•估计误差最小的标准称为估计准则。•根据不同的估计准则和估计计算方法，有各种不同的最优估计。•卡尔曼滤波是一种递推线性最小方差估计。最小方差估计线性最小方差估计递推线性最小方差估计1.1最小方差估计•最小方差估计的估计准则是估计的均方误差最小，即：[()][()]|minLTXXZEXXZXXZ系统的n维随机向量Z是m维随机量测向量利用Z计算得到的X的最小方差估值估计的误差估计均方差阵•最小方差估计的误差小于等于其他估计的均方误差!•估计的均方误差就是估计误差的方差，即：0~)]}({[XEZXXETTXEXXEXEXXE]~~][~~[~~•最小方差估计具有无偏性质，即它的估计误差（亦可用表示）的均值为零。即：X~•因此，最小方差估计不但使估值的均方误差最小，而且这种最小的均方误差就是估计的误差方差)(ZX1.2线性最小方差估计•如果将估值规定为量测矢量Z的线性函数，即X•使得下述指标满足•式中A和b分别是（n×m）阶和n维的矩阵和矢量。这样的估计方法称为线性最小方差估计，有时用符号E*[X/Z]表示。LXZAZb[()][()]|minLTXXZEXXZXXZ•有关量测量Z的线性函数有无穷多个，但能使X具有最小方差估计的线性函数只有一个，记为00LXZAZb•利用上述的指标我们可以得出A0和b0，0101XZZXXZZZACCbmCCm•因此X在Z上的线性最小方差估计为1ˆLXXZZZXZmCCZm•线性最小方差估计的均方误差为1XXZZZXPCCCC1.2.1线性最小方差估计的性质•性质1无偏性线性最小方差估计是X在Z上的无偏估计，即0~)]}({[XEZXXE•性质2线性1线性最小方差估计是具有线性性质，即若X的线性最小方差估计为，则的线性最小方差估计为/EXZFXe其中F为确定性矩阵，e为确定性向量。//EFXeZFEXZe•性质3线性2若Y与Z不相关，则/,//EXYZEXYEXZEX1.3递推线性最小方差估计——卡尔曼滤波•卡尔曼滤波的准则与线性最小方差估计相同•估值同样是量测值的线性函数•只要包括初始值在内的滤波器初值选择正确，它的估值也是无偏的})]()][({[})]()][({[TTZXZXEZXXZXXEbAZX0~)]}({[XEZXXE•计算方法——递推形式•在k时刻以前估值的基础上，根据k时刻的量测值Zk,递推得到k时刻的状态估计值：根据k-1时刻以前所有的量测值得到1kXkZkX)(ˆtXX（k）也可以说是综合利用k时刻以前的所有量测值得到的一次仅处理一个量测量计算量大大减小主要适用于线性动态系统！2卡尔曼滤波方程2.1离散系统的数学描述•设离散化后的系统状态方程和量测方程分别为：kkkkkkkkkkVXHZWXX1111,Xk为k时刻的n维状态向量（被估计量）Zk为k时刻的m维量测向量k-1到k时刻的系统一步状态转移矩阵（n×n阶）Wk-1为k-1时刻的系统噪声（r维）Γk-1为系统噪声矩阵（n×r阶）Hk为k时刻系统量测矩阵（m×n阶）Vk为k时刻m维量测噪声•Qk和Rk分别称为系统噪声和量测噪声的方差矩阵，在卡尔曼滤波中要求它们分别是已知值的非负定阵和正定阵；•δkj是Kroneckerδ函数，即：)(1)(0jkjkkj•卡尔曼滤波要求{Wk}和{Vk}是互不相关的零均值的白噪声序列，有：kjkTjkkjkTjkRVVEQWWE•Var{·}为对{·}求方差的符号•卡尔曼滤波要求mx0和Cx0为已知量，•初始状态的一、二阶统计特性为：00xmXE00xCXVar•且要求X0与{Wk}和{Vk}都不相关2.2离散卡尔曼滤波方程1/)(kkkkkPHKIP或11,1/kkkkkXX•状态一步预测方程)(1/1/kkkkkkkkXHZKXX•状态估值计算方程11/1/)(kTkkkkTkkkkRHPHHPK•滤波增益方程TkkkTkkkkkkkQPP1111,11,1/•一步预测均方差方程TkkkTkkkkkkkKRKHKIPHKIP)()(1/•估计均方差方程2.2离散卡尔曼滤波方程11,1/kkkkkXX)(1/1/kkkkkkkkXHZKXX11/1/)(kTkkkkTkkkkRHPHHPKTkkkTkkkkkkkQPP1111,11,1/TkkkTkkkkkkkKRKHKIPHKIP)()(1/时间修正方程量测修正方程（1）状态一步预测方程•各滤波方程的物理意义：1ˆkXXk-1的卡尔曼滤波估值1/ˆkkX利用Xk-1计算得到的一步预测也可以认为是利用k-1时刻和以前时刻的量测值得到的Xk的一步预测上式就是通过计算新息，把估计出来，并左乘一个系数矩阵加到中，从而得到估值和，称为滤波增益矩阵1/kkX1/~kkXkXˆkKkK（2）状态估值计算方程计算估值Xk的方程。它是在一步预测Xk/k-1的基础上，根据量测值Zk计算出来的)(1/1/kkkkkkkkXHZKXXkkkkkkkkkkkkkkVXHXHVXHXHZ1/1/1/~一步预测误差1/1/~kkkkkXXX若把看作是量测的一步预测，则就是量测的一步预测误差1/kkkXH)(1/kkkkXHZkZ由两部分组成：和，正是在基础上估计所需信息，因此又称为新息1/kkXkV1/~kkX1/~kkXkX)(1/kkkkXHZ•由于也具有无偏性，即的均值为零，所以也称为一步预测误差方差阵。上式中的和分别就是新息中的两部分内容1/ˆkkX1/~kkX1/kkPTkkkkHPH1/kR（3）滤波增益方程一步预测均方差阵，即：/1/1/1,TkkkkkkkkZHPEXX11/1/)(kTkkkkTkkkkRHPHHPK•Kk选取的标准就是卡尔曼滤波的估计准则，也就是使得均方误差阵最小：kXˆ如果Rk大，Kk就小Rk小，Kk就大（4）一步预测均方误差方程11/1/)(kTkkkkTkkkkRHPHHPK•从下式可以看出，求Kk必须先求出Pk/k-1•式中，为的估计误差，可以看出一步预测均方误差阵Pk/k-1是从估计均方误差阵Pk-1转移过来的，并且再加上系统噪声方差的影响。111ˆ~kkkXXX1ˆkXTkkkTkkkkkkkQPP1111,11,1/的均方误差阵，即：TkkkXXEP111~,~1ˆkX（5）估计均方误差方程TkkkTkkkkkkkKRKHKIPHKIP)()(1/1/)(kkkkkPHKIP或计算量小，但在计算机有舍入误差的条件下，不能始终保证算出的Pk是对称的（6）卡尔曼滤波的计算流程1/kk11,1/kkkkkXX)(1/1/kkkkkkkkXHZKXX11/1/)(kTkkkkTkkkkRHPHHPKTkkkTkkkkkkkQPP1111,11,1/TkkkTkkkkkkkKRKHKIPHKIP)()(1/1ˆkX1kkkXˆ1kPkP1kk1,kkTkkkQ111kRkHkRkH1/kkPkKkZ滤波计算回路增益计算回路2.3离散卡尔曼滤波基本方程使用要点•在滤波开始时，必须有初始值和才能进行0ˆX0P•为了保证估值的无偏性，应选择：000ˆxmXEXTxxTmXmXEXXXXEP))(()ˆ)(ˆ(00000000000xCXVar•这样才能保证估计均方差阵Pk始终最小。1.滤波初值的选取•有上述的卡尔曼滤波基本方程中的均方误差的公式2.估计均方误差阵的等价形式及选用/1()()TTkkkkkkkkkkPIKHPIKHKRK/1()kkkkkPIKHP111/1TkkkkkkPPHRH（a）（b）（c）•由卡尔曼滤波方程的推导得知，基本方程只适用于系统方程和量测方程都是离散型的情况。但实际的物理系统一般都是连续的，动力学特性用连续的微分方程来描述。所以在使用卡尔曼基本方程之前，必须对系统方程和量测方程进行离散化处理。•设描述物理系统动力特性的系统方程为3.一步转移阵和等效离散系统噪声方差阵的计算()()()()()XtFtXtGtWt其中系统驱动源W（t）为白噪声过程，即[]0[]TEwtEwtwqtq为w（t）的方差强度矩阵。根据线性系统理论，系统方程的离散化形式为1111(),,kktkkkkktXtttXttGwd其中，满足方程1,kktt,,,kkkkttFtttttI23231,...2!3!kkkkkTTITFFF对该方程求解并进行约等变化，得式中T为滤波周期。这个式子即为一步转移矩阵的实时计算公式。,kkFFt同样，通过系统的离散化处理，得出等效离散系统噪声方差阵1!ikiiTQMi•一步预测方程改为：,111111kkkkkkkkkkkkkXXWBUZHXVY•状态估计方程改为：1111,ˆˆ1kkkkkUBXXkk)ˆ(ˆˆ11kkkkXHYZKXXkkkkk•其他滤波方程不变4.系统有确定性控制时的滤波基本方程•设系统除了白噪声外，还有确定性驱动项5.一步预测基本方程/1ˆkkkZX和+1/ˆkkX令•一步预测基本方程式指利用递推计算的全套方程。根据基本方程的一步预测方程得+1/k+1,kk+1,k/1/1ˆˆˆˆ=kkkkkkkkkkXXXKZHXk+1,kkKK则+1/k+1,k/1/1ˆˆˆ=kkkkkkkkXXKZHX•从而也可以得到均方误差1,k+1,k,1k+1,kTTkkkkkkkkPKHPQ•例1设有线定常系统11kkkkkkXXWZXV式中状态变量Xk和量测Zk均为标量，Φ为常数。Wk和Vk为零均值白噪声序列，分别具有协方差,kjkjkjkjEWWQEVVR并且Wk，Vk，X0三者互不相关。求的递推方程。ˆkX•例2α-β-γ滤波•设运动体沿某一直线运动，tk时刻的位移、速度、加速度、加加速度分别为sk，vk，ak，jk，只对运动体的位置做测量，测量值为Zk=sk+Vk，若•量测量的采样周期为T,求sk，vk，ak的估计。0,0,kklkjkklkjEjEjjqEVEVVr•设系统方程和量测方程的一般形式为2.4白噪声条件下离散卡尔曼滤波方程的一般形式,111111kkkkkkkkkkkkkXXWBUZHXVY