离散卡尔曼滤波ppt课件.ppt

我吓了一跳，蝎子是多么丑恶和恐怖的东西，为什么把它放在这样一个美丽的世界里呢？但是我也感到愉快，证实我的猜测没有错：表里边有一个活的生物1 1 卡尔曼滤波与最优估计卡尔曼滤波与最优估计 卡尔曼滤波是一种最优估计技术卡尔曼滤波是一种最优估计技术 ！ 它能将仅与部分状态有关的测量值进行处理，得它能将仅与部分状态有关的测量值进行处理，得出从某种统计意义上讲估计误差最小的更多的状出从某种统计意义上讲估计误差最小的更多的状态的估计值。态的估计值。 估计误差最小的标准称为估计准则。估计误差最小的标准称为估计准则。 根据不同的估计准则和估计计算方法，有各种不根据不同的估计准则和估计计算方法，有各种不同的最优估计。同的最优估计。 卡尔曼滤波是一种递推线性最小方差估计。卡尔曼滤波是一种递推线性最小方差估计。我吓了一跳，蝎子是多么丑恶和恐怖的东西，为什么把它放在这样一个美丽的世界里呢？但是我也感到愉快，证实我的猜测没有错：表里边有一个活的生物1 1.1 .1最小方差估计最小方差估计 最小方差估计的估计准则是估计的均方误差最小，即：最小方差估计的估计准则是估计的均方误差最小，即： ( )( ) |minLTXXZE XX ZXX Z 最小方差估计的误差小于等于其他估计的均方误差最小方差估计的误差小于等于其他估计的均方误差! !我吓了一跳，蝎子是多么丑恶和恐怖的东西，为什么把它放在这样一个美丽的世界里呢？但是我也感到愉快，证实我的猜测没有错：表里边有一个活的生物 估计的均方误差就是估计误差的方差，即：估计的均方误差就是估计误差的方差，即： 0)(XEZXXE TTXEXXEXEXXE 最小方差估计具有无偏性质，即它的估计误差（亦可最小方差估计具有无偏性质，即它的估计误差（亦可用用 表示）的均值为零。即：表示）的均值为零。即：X 因此，最小方差估计不但使估值因此，最小方差估计不但使估值 的均方误差最的均方误差最小，而且这种最小的均方误差就是估计的误差方差小，而且这种最小的均方误差就是估计的误差方差 )(ZX我吓了一跳，蝎子是多么丑恶和恐怖的东西，为什么把它放在这样一个美丽的世界里呢？但是我也感到愉快，证实我的猜测没有错：表里边有一个活的生物1.21.2线性最小方差估计线性最小方差估计 如果将估值如果将估值 规定为量测矢量规定为量测矢量Z Z的线性函数，即的线性函数，即X 使得下述指标满足使得下述指标满足 式中式中A A和和b b分别是（分别是（n nm m）阶和）阶和n n维的矩阵和矢量。这维的矩阵和矢量。这 样的估计方法称为样的估计方法称为线性最小方差估计线性最小方差估计，有时用符号，有时用符号E E* *X/ZX/Z表示。表示。 LXZAZb ( )( ) |minLTX XZE X X ZX X Z我吓了一跳，蝎子是多么丑恶和恐怖的东西，为什么把它放在这样一个美丽的世界里呢？但是我也感到愉快，证实我的猜测没有错：表里边有一个活的生物 有关量测量有关量测量Z Z的线性函数有无穷多个，但能使的线性函数有无穷多个，但能使X X具有具有最小方差估计的线性函数只有一个，记为最小方差估计的线性函数只有一个，记为 00LXZA Zb 利用上述的指标我们可以得出利用上述的指标我们可以得出A0A0和和b0b0， 0101XZZXXZZZACCbmCCm我吓了一跳，蝎子是多么丑恶和恐怖的东西，为什么把它放在这样一个美丽的世界里呢？但是我也感到愉快，证实我的猜测没有错：表里边有一个活的生物 因此因此X X在在Z Z上的线性最小方差估计为上的线性最小方差估计为 1LXXZZZX ZmC CZ m 线性最小方差估计的均方误差为线性最小方差估计的均方误差为1XXZZZXPCCCC我吓了一跳，蝎子是多么丑恶和恐怖的东西，为什么把它放在这样一个美丽的世界里呢？但是我也感到愉快，证实我的猜测没有错：表里边有一个活的生物1.2.11.2.1线性最小方差估计线性最小方差估计的性质的性质 性质性质1 1 无偏性无偏性 线性最小方差估计是线性最小方差估计是X X在在Z Z上的无偏估计，即上的无偏估计，即 0)(XEZXXE 性质性质2 2 线性线性1 1 线性最小方差估计是具有线性性质，即若线性最小方差估计是具有线性性质，即若X X的线性的线性最小方差估计为最小方差估计为 ，则，则 的线性最小的线性最小方差估计为方差估计为/EXZF Xe我吓了一跳，蝎子是多么丑恶和恐怖的东西，为什么把它放在这样一个美丽的世界里呢？但是我也感到愉快，证实我的猜测没有错：表里边有一个活的生物其中其中F F为确定性矩阵，为确定性矩阵，e e为确定性向量。为确定性向量。/E FX e ZFE X Ze 性质性质3 3 线性线性2 2 若若Y Y与与Z Z不相关，则不相关，则/ ,/E X Y ZE X YE X ZEX我吓了一跳，蝎子是多么丑恶和恐怖的东西，为什么把它放在这样一个美丽的世界里呢？但是我也感到愉快，证实我的猜测没有错：表里边有一个活的生物1.31.3递推线性最小方差估计递推线性最小方差估计卡尔曼滤波卡尔曼滤波 卡尔曼滤波的准则与线性最小方差估计相同卡尔曼滤波的准则与线性最小方差估计相同 估值同样是量测值的线性函数估值同样是量测值的线性函数 只要包括初始值在内的滤波器初值选择正确，它的估只要包括初始值在内的滤波器初值选择正确，它的估值也是无偏的值也是无偏的 )()()()(TTZXZXEZXXZXXEbAZX 0)(XEZXXE 计算方法计算方法递推形式递推形式我吓了一跳，蝎子是多么丑恶和恐怖的东西，为什么把它放在这样一个美丽的世界里呢？但是我也感到愉快，证实我的猜测没有错：表里边有一个活的生物 在在k k时刻以前估值的基础上，根据时刻以前估值的基础上，根据k k时刻的量测值时刻的量测值Z Zk k, ,递推得到递推得到k k时刻的状态估计值时刻的状态估计值 ：根据根据k-1k-1时刻以前时刻以前所有的量测值得到所有的量测值得到 1kXkZkX)(tXX X（k k）也可以说是综合利用）也可以说是综合利用k k时刻以前的所有量测值得到时刻以前的所有量测值得到 的的一次仅处理一个量测量一次仅处理一个量测量计算量大大减小计算量大大减小我吓了一跳，蝎子是多么丑恶和恐怖的东西，为什么把它放在这样一个美丽的世界里呢？但是我也感到愉快，证实我的猜测没有错：表里边有一个活的生物2 2 卡尔曼滤波方程卡尔曼滤波方程2.12.1离散系统的数学描述离散系统的数学描述 设离散化后的系统状态方程和量测方程分别为：设离散化后的系统状态方程和量测方程分别为：kkkkkkkkkkVXHZWXX1111,X Xk k为为k k时刻的时刻的n n维状态向量维状态向量（被估计量）（被估计量）Z Zk k为为k k时刻的时刻的m m维量测向量维量测向量k-1k-1到到k k时刻的系统一步状态时刻的系统一步状态转移矩阵（转移矩阵（n nn n阶）阶）WWk-1k-1为为k-1k-1时刻的系统噪声时刻的系统噪声（r r维）维） k-1k-1为系统噪声矩阵为系统噪声矩阵（n nr r阶）阶）H Hk k为为k k时刻系统量测矩阵时刻系统量测矩阵（m mn n阶）阶）V Vk k为为k k时刻时刻m m维量测噪声维量测噪声我吓了一跳，蝎子是多么丑恶和恐怖的东西，为什么把它放在这样一个美丽的世界里呢？但是我也感到愉快，证实我的猜测没有错：表里边有一个活的生物 Q Qk k和和R Rk k分别称为系统噪声和量测噪声的方差矩阵，分别称为系统噪声和量测噪声的方差矩阵，在卡尔曼滤波中要求它们分别是已知值的非负定阵和在卡尔曼滤波中要求它们分别是已知值的非负定阵和正定阵；正定阵； k jk j是是Kronecker Kronecker 函数，即：函数，即：)(1)(0jkjkkj 卡尔曼滤波要求卡尔曼滤波要求WWk k 和和VVk k 是互不相关的零均值的是互不相关的零均值的白噪声序列，有：白噪声序列，有：kjkTjkkjkTjkRVVEQWWE我吓了一跳，蝎子是多么丑恶和恐怖的东西，为什么把它放在这样一个美丽的世界里呢？但是我也感到愉快，证实我的猜测没有错：表里边有一个活的生物 Var Var 为对为对求方差的符号求方差的符号 卡尔曼滤波要求卡尔曼滤波要求m mx0 x0和和C Cx0 x0为已知量，为已知量， 初始状态的初始状态的 一、二阶统计特性为：一、二阶统计特性为：00 xmXE00 xCXVar 且要求且要求X X0 0与与WWk k 和和VVk k 都不相关都不相关我吓了一跳，蝎子是多么丑恶和恐怖的东西，为什么把它放在这样一个美丽的世界里呢？但是我也感到愉快，证实我的猜测没有错：表里边有一个活的生物2 2.2 .2 离散卡尔曼滤波方程离散卡尔曼滤波方程 1/)(kkkkkPHKIP或 11,1/kkkkkXX 状态一步预测方程状态一步预测方程)(1/1/kkkkkkkkXHZKXX 状态估值计算方程状态估值计算方程11/1/)(kTkkkkTkkkkRHPHHPK 滤波增益方程滤波增益方程TkkkTkkkkkkkQPP1111,11,1/ 一步预测均方差方程一步预测均方差方程TkkkTkkkkkkkKRKHKIPHKIP)()(1/ 估计均方差方程估计均方差方程我吓了一跳，蝎子是多么丑恶和恐怖的东西，为什么把它放在这样一个美丽的世界里呢？但是我也感到愉快，证实我的猜测没有错：表里边有一个活的生物2 2.2 .2 离散卡尔曼滤波方程离散卡尔曼滤波方程 11,1/kkkkkXX)(1/1/kkkkkkkkXHZKXX11/1/)(kTkkkkTkkkkRHPHHPKTkkkTkkkkkkkQPP1111,11,1/TkkkTkkkkkkkKRKHKIPHKIP)()(1/时间修正时间修正方程方程量测修正量测修正方程方程我吓了一跳，蝎子是多么丑恶和恐怖的东西，为什么把它放在这样一个美丽的世界里呢？但是我也感到愉快，证实我的猜测没有错：表里边有一个活的生物（1 1）状态一步预测方程）状态一步预测方程 各滤波方程的物理意义：各滤波方程的物理意义：1kXX Xk-1k-1的卡尔曼滤波估值的卡尔曼滤波估值1/kkX利用利用X Xk-1k-1计算得到的一步预测计算得到的一步预测 我吓了一跳，蝎子是多么丑恶和恐怖的东西，为什么把它放在这样一个美丽的世界里呢？但是我也感到愉快，证实我的猜测没有错：表里边有一个活的生物 上式就是通过上式就是通过 计算新息，把计算新息，把 估计出来，并估计出来，并左乘一个系数矩阵左乘一个系数矩阵 加到加到 中，从而得到中，从而得到 估值估值 和，和， 称为滤波增益矩阵称为滤波增益矩阵1/ kkX1/kkXkXkKkK（2 2）状态估值计算方程）状态估值计算方程)(1/1/kkkkkkkkXHZKXXkkkkkkkkkkkkkkVXHXHVXHXHZ1/1/1/一步预测误差一步预测误差1/1/kkkkkXXX若把若把 看作是量测看作是量测 的一步预测，的一步预测，则则 就是量测的一步预测误差就是量测的一步预测误差1/ kkkXH)(1/ kkkkXHZkZ由两部分组成：由两部分组成： 和和 ， 正是在正是在 基础上估计基础上估计 所需信息，因此所需信息，因此又称又称 为新息为新息1/ kkXkV1/kkX1/kkXkX)(1/ kkkkXHZ我吓了一跳，蝎子是多么丑恶和恐怖的东西，为什么把它放在这样一个美丽的世界里呢？但是我也感到愉快，证实我的猜测没有错：表里边有一个活的生物 由于由于 也具有无偏性，即也具有无偏性，即 的均值为零，所的均值为零，所以以 也称为一步预测误差方差阵。上式中的也称为一步预测误差方差阵。上式中的 和和 分别就是新息中的两部分内容分别就是新息中的两部分内容1/kkX1/kkX1/ kkPTkkkkHPH1/ kR（3 3）滤波增益方程）滤波增益方程一步预测均方差阵，即：一步预测均方差阵，即：/1/1/1,Tkkk kk kk kZH PE XX11/1/)(kTkkkkTkkkkRHPHHPK K Kk k选取的标准就是卡尔曼滤波的估计准则，也就是选取的标准就是卡尔曼滤波的估计准则，也就是使得使得 均方误差阵最小：均方误差阵最小：kX如果如果R Rk k大，大，K Kk k就小就小R Rk k小，小，K Kk k就大就大我吓了一跳，蝎子是多么丑恶和恐怖的东西，为什么把它放在这样一个美丽的世界里呢？但是我也感到愉快，证实我的猜测没有错：表里边有一个活的生物（4 4）一步预测均方误差方程）一步预测均方误差方程11/1/)(kTkkkkTkkkkRHPHHPK 从下式可以看出，求从下式可以看出，求K Kk k必须先求出必须先求出P Pk/k-1k/k-1 式中式中 ，为，为 的估计误差，可以的估计误差，可以看出一步预测均方误差阵看出一步预测均方误差阵P Pk/k-1k/k-1是从估计均方误差阵是从估计均方误差阵P Pk-1k-1转移过来的，并且再加上系统噪声方差的影响。转移过来的，并且再加上系统噪声方差的影响。111kkkXXX1kXTkkkTkkkkkkkQPP1111,11,1/ 的均方误差阵，即：的均方误差阵，即：TkkkXXEP111,1kX我吓了一跳，蝎子是多么丑恶和恐怖的东西，为什么把它放在这样一个美丽的世界里呢？但是我也感到愉快，证实我的猜测没有错：表里边有一个活的生物（5 5）估计均方误差方程）估计均方误差方程TkkkTkkkkkkkKRKHKIPHKIP)()(1/1/)(kkkkkPHKIP或 我吓了一跳，蝎子是多么丑恶和恐怖的东西，为什么把它放在这样一个美丽的世界里呢？但是我也感到愉快，证实我的猜测没有错：表里边有一个活的生物（6 6）卡尔曼滤波的计算流程）卡尔曼滤波的计算流程1/ kk11,1/kkkkkXX)(1/1/kkkkkkkkXHZKXX11/1/)(kTkkkkTkkkkRHPHHPKTkkkTkkkkkkkQPP1111,11,1/TkkkTkkkkkkkKRKHKIPHKIP)()(1/1kX1kkkX1kPkP1kk1, kkTkkkQ111kRkHkRkH1/ kkPkKkZ我吓了一跳，蝎子是多么丑恶和恐怖的东西，为什么把它放在这样一个美丽的世界里呢？但是我也感到愉快，证实我的猜测没有错：表里边有一个活的生物2.32.3离散卡尔曼滤波基本方程使用要点离散卡尔曼滤波基本方程使用要点 在滤波开始时，必须有初始值在滤波开始时，必须有初始值 和和 才能进行才能进行0X0P 为了保证估值的无偏性，应选择：为了保证估值的无偏性，应选择：000 xmXEXTxxTmXmXEXXXXEP)()(00000000000 xCXVar 这样才能保证估计均方差阵这样才能保证估计均方差阵P Pk k始终最小。始终最小。1. 1.滤波初值的选取滤波初值的选取我吓了一跳，蝎子是多么丑恶和恐怖的东西，为什么把它放在这样一个美丽的世界里呢？但是我也感到愉快，证实我的猜测没有错：表里边有一个活的生物 有上述的卡尔曼滤波基本方程中的均方误差的公式有上述的卡尔曼滤波基本方程中的均方误差的公式2. 2.估计均方误差阵的等价形式及选用估计均方误差阵的等价形式及选用/1()()TTkkkk kkkkkkPIK H PIK HK R K/1()kkkk kPIK HP 111/1Tkk kkkkPPH R H（a）（b）（c）我吓了一跳，蝎子是多么丑恶和恐怖的东西，为什么把它放在这样一个美丽的世界里呢？但是我也感到愉快，证实我的猜测没有错：表里边有一个活的生物 由卡尔曼滤波方程的推导得知，基本方程只适用于系由卡尔曼滤波方程的推导得知，基本方程只适用于系统方程和量测方程都是离散型的情况。但实际的物理统方程和量测方程都是离散型的情况。但实际的物理系统一般都是连续的，动力学特性用连续的微分方程系统一般都是连续的，动力学特性用连续的微分方程来描述。所以在使用卡尔曼基本方程之前，必须对系来描述。所以在使用卡尔曼基本方程之前，必须对系统方程和量测方程进行离散化处理。统方程和量测方程进行离散化处理。 设描述物理系统动力特性的系统方程为设描述物理系统动力特性的系统方程为3. 3.一步转移阵和等效离散系统噪声方差阵的计算一步转移阵和等效离散系统噪声方差阵的计算( )( )( )( )( )X tF t X tG t W t其中系统驱动源其中系统驱动源WW（t t）为白噪声过程，即）为白噪声过程，即我吓了一跳，蝎子是多么丑恶和恐怖的东西，为什么把它放在这样一个美丽的世界里呢？但是我也感到愉快，证实我的猜测没有错：表里边有一个活的生物 0TE w tE w t wqtq q为为w w（t t）的方差强度矩阵。）的方差强度矩阵。根据线性系统理论，系统方程的离散化形式为根据线性系统理论，系统方程的离散化形式为 1111( ),kktkkkkktXttt X ttGw d 其中，其中， 满足方程满足方程1,kktt ,kkk kt tF tt tt tI我吓了一跳，蝎子是多么丑恶和恐怖的东西，为什么把它放在这样一个美丽的世界里呢？但是我也感到愉快，证实我的猜测没有错：表里边有一个活的生物23231,.2!3!kkkkkTTI TFFF 对该方程求解并进行约等变化，得对该方程求解并进行约等变化，得式中式中 T T为滤波周期。为滤波周期。 这个式子即为一步转移矩阵的实时计算公式。这个式子即为一步转移矩阵的实时计算公式。 ,kkFF t同样，通过系统的离散化处理，得出等效离散系统噪声同样，通过系统的离散化处理，得出等效离散系统噪声方差阵方差阵1!ikiiTQMi我吓了一跳，蝎子是多么丑恶和恐怖的东西，为什么把它放在这样一个美丽的世界里呢？但是我也感到愉快，证实我的猜测没有错：表里边有一个活的生物 一步预测方程改为：一步预测方程改为：,111111kk kkkkkkkkkkkXXWB UZH XVY 状态估计方程改为：状态估计方程改为：1111,1kkkkkUBXXkk)(11kkkkXHYZKXXkkkkk 其他滤波方程不变其他滤波方程不变4. 4.系统有确定性控制时的滤波基本方程系统有确定性控制时的滤波基本方程 设系统除了白噪声外，还有确定性驱动项设系统除了白噪声外，还有确定性驱动项我吓了一跳，蝎子是多么丑恶和恐怖的东西，为什么把它放在这样一个美丽的世界里呢？但是我也感到愉快，证实我的猜测没有错：表里边有一个活的生物5. 5.一步预测基本方程一步预测基本方程/1kk kZX和+1/kkX令令 一步预测基本方程式指利用一步预测基本方程式指利用 递推计算递推计算 的的 全套方程。全套方程。 根据基本方程的一步预测方程得根据基本方程的一步预测方程得 +1/k+1,kk+1,k/ 1/ 1=kkkk kkkkk kXXXK ZHXk+1,kkKK我吓了一跳，蝎子是多么丑恶和恐怖的东西，为什么把它放在这样一个美丽的世界里呢？但是我也感到愉快，证实我的猜测没有错：表里边有一个活的生物 则则+1/k+1,k/1/1=kkk kkkk kXXK ZH X 从而也可以得到均方误差从而也可以得到均方误差1,k+1,k, 1k+1,kTTkkkk kkkkPKH PQ 我吓了一跳，蝎子是多么丑恶和恐怖的东西，为什么把它放在这样一个美丽的世界里呢？但是我也感到愉快，证实我的猜测没有错：表里边有一个活的生物 例例1 1 设有线定常系统设有线定常系统11kkkkkkXXWZXV 式中状态变量式中状态变量XkXk和量测和量测ZkZk均为标量，均为标量，为常数。为常数。WkWk和和VkVk为零均值白噪声序列，分别具有协方差为零均值白噪声序列，分别具有协方差,kjkjkjkjE WWQE VVR并且并且WkWk，VkVk，X0X0三者互不相关。求三者互不相关。求 的递推方程。的递推方程。kX我吓了一跳，蝎子是多么丑恶和恐怖的东西，为什么把它放在这样一个美丽的世界里呢？但是我也感到愉快，证实我的猜测没有错：表里边有一个活的生物我吓了一跳，蝎子是多么丑恶和恐怖的东西，为什么把它放在这样一个美丽的世界里呢？但是我也感到愉快，证实我的猜测没有错：表里边有一个活的生物 例例2 -2 -滤波滤波 设运动体沿某一直线运动，设运动体沿某一直线运动，tk tk时刻的位移、速度、加时刻的位移、速度、加速度、加加速度分别为速度、加加速度分别为sksk，vkvk，akak，jk jk，只对运动体，只对运动体的位置做测量，测量值为的位置做测量，测量值为Zk=sk+VkZk=sk+Vk，若，若 量测量的采样周期为量测量的采样周期为T,T,求求sksk，vkvk，akak的估计。的估计。0,0,kklkjkklkjE jE j jqE VE V Vr我吓了一跳，蝎子是多么丑恶和恐怖的东西，为什么把它放在这样一个美丽的世界里呢？但是我也感到愉快，证实我的猜测没有错：表里边有一个活的生物我吓了一跳，蝎子是多么丑恶和恐怖的东西，为什么把它放在这样一个美丽的世界里呢？但是我也感到愉快，证实我的猜测没有错：表里边有一个活的生物我吓了一跳，蝎子是多么丑恶和恐怖的东西，为什么把它放在这样一个美丽的世界里呢？但是我也感到愉快，证实我的猜测没有错：表里边有一个活的生物我吓了一跳，蝎子是多么丑恶和恐怖的东西，为什么把它放在这样一个美丽的世界里呢？但是我也感到愉快，证实我的猜测没有错：表里边有一个活的生物 设系统方程和量测方程的一般形式为设系统方程和量测方程的一般形式为2.42.4白噪声条件下离散卡尔曼滤波方程的一般形式白噪声条件下离散卡尔曼滤波方程的一般形式,111111kk kkkkkkkkkkkXXWB UZH XVY我吓了一跳，蝎子是多么丑恶和恐怖的东西，为什么把它放在这样一个美丽的世界里呢？但是我也感到愉快，证实我的猜测没有错：表里边有一个活的生物