卡尔曼滤波卡尔曼滤波 Theory of Kalman filter Theory of Kalman filter Theory of Kalman filter Theory of Kalman filter 1 1 卡尔曼滤波与最优估计卡尔曼滤波与最优估计卡尔曼滤波是一种最优估计技术卡尔曼滤波是一种最优估计技术 ！它能将仅与部分状态有关的测量值进行处理，得它能将仅与部分状态有关的测量值进行处理，得出从某种统计意义上讲估计误差最小的更多的状出从某种统计意义上讲估计误差最小的更多的状态的估计值。态的估计值。 估计误差最小的标准称为估计准则。估计误差最小的标准称为估计准则。 根据不同的估计准则和估计计算方法，有各种不根据不同的估计准则和估计计算方法，有各种不同的最优估计。同的最优估计。 卡尔曼滤波是一种递推线性最小方差估计。卡尔曼滤波是一种递推线性最小方差估计。最小方差最小方差最小方差最小方差估计估计估计估计线性最小线性最小线性最小线性最小方差估计方差估计方差估计方差估计递推线性最小递推线性最小递推线性最小递推线性最小方差估计方差估计方差估计方差估计1 1.1.1最小方差估计最小方差估计最小方差估计的估计准则是估计的均方误差最小，即：最小方差估计的估计准则是估计的均方误差最小，即：系统的系统的系统的系统的n n n n维随维随维随维随机向量机向量机向量机向量Z Z Z Z是是是是m m m m维随机量维随机量维随机量维随机量测向量测向量测向量测向量利用利用利用利用Z Z Z Z计算得到的计算得到的计算得到的计算得到的X X X X的最小方差估值的最小方差估值的最小方差估值的最小方差估值估计的误差估计的误差估计的误差估计的误差估计均方差阵估计均方差阵估计均方差阵估计均方差阵最小方差估计的误差小于等于其他估计的均方误差最小方差估计的误差小于等于其他估计的均方误差! !估计的均方误差就是估计误差的方差，即：估计的均方误差就是估计误差的方差，即：最小方差估计具有无偏性质，即它的估计误差（亦可最小方差估计具有无偏性质，即它的估计误差（亦可用用 表示）的均值为零。即：表示）的均值为零。即：因此，最小方差估计不但使估值因此，最小方差估计不但使估值 的均方误差的均方误差最小，而且这种最小的均方误差就是估计的误差方差最小，而且这种最小的均方误差就是估计的误差方差 1.21.2线性最小方差估计线性最小方差估计如果将估值如果将估值 规定为量测矢量规定为量测矢量Z Z的线性函数，即的线性函数，即使得下述指标满足使得下述指标满足式中式中A A和和b b分别是（分别是（nmnm）阶和）阶和n n维的矩阵和矢量。这维的矩阵和矢量。这 样样的估计方法称为的估计方法称为线性最小方差估计线性最小方差估计，有时用符号，有时用符号E*X/ZE*X/Z表示。表示。 有关量测量有关量测量Z Z的线性函数有无穷多个，但能使的线性函数有无穷多个，但能使X X具有最具有最小方差估计的线性函数只有一个，记为小方差估计的线性函数只有一个，记为利用上述的指标我们可以得出利用上述的指标我们可以得出A0A0和和b0b0， 因此因此X X在在Z Z上的线性最小方差估计为上的线性最小方差估计为线性最小方差估计的均方误差为线性最小方差估计的均方误差为1.2.11.2.1线性最小方差估计线性最小方差估计的性质的性质性质性质1 1 无偏性无偏性 线性最小方差估计是线性最小方差估计是X X在在Z Z上的无偏估计，即上的无偏估计，即性质性质2 2 线性线性1 1 线性最小方差估计是具有线性性质，即若线性最小方差估计是具有线性性质，即若X X的线的线性最小方差估计为性最小方差估计为 ，则，则 的线性最小方差估计为的线性最小方差估计为其中其中F F为确定性矩阵，为确定性矩阵，e e为确定性向量。为确定性向量。性质性质3 3 线性线性2 2 若若Y Y与与Z Z不相关，则不相关，则1.31.3递推线性最小方差估计递推线性最小方差估计卡尔曼滤波卡尔曼滤波 卡尔曼滤波的准则与线性最小方差估计相同卡尔曼滤波的准则与线性最小方差估计相同估值同样是量测值的线性函数估值同样是量测值的线性函数只要包括初始值在内的滤波器初值选择正确，它的估只要包括初始值在内的滤波器初值选择正确，它的估值也是无偏的值也是无偏的 计算方法计算方法递推形式递推形式在在k k时刻以前估值的基础上，根据时刻以前估值的基础上，根据k k时刻的量测值时刻的量测值Z Zk k, ,递推得到递推得到k k时刻的状态估计值时刻的状态估计值 ：根据根据k-1k-1时刻以前时刻以前所有的量测值得到所有的量测值得到 X X（k k）也可以说是综合利用）也可以说是综合利用k k时刻以前的所有量测值得到时刻以前的所有量测值得到 的的一次仅处理一个量测量一次仅处理一个量测量计算量大大减小计算量大大减小 主要适用于线性动态系统！主要适用于线性动态系统！主要适用于线性动态系统！主要适用于线性动态系统！ 2 2 卡尔曼滤波方程卡尔曼滤波方程2.12.1离散系统的数学描述离散系统的数学描述设离散化后的系统状态方程和量测方程分别为：设离散化后的系统状态方程和量测方程分别为：X Xk k为为k k时刻的时刻的n n维状态向量维状态向量（被估计量）（被估计量）Z Zk k为为k k时刻的时刻的m m维量测向量维量测向量k-1k-1到到k k时刻的系统一步状时刻的系统一步状态转移矩阵（态转移矩阵（nnnn阶）阶）W Wk-1k-1为为k-1k-1时刻的系统噪声时刻的系统噪声（r r维）维）k-1k-1为系统噪声矩阵为系统噪声矩阵（nrnr阶）阶）H Hk k为为k k时刻系统量测矩阵时刻系统量测矩阵（mnmn阶）阶）V Vk k为为k k时刻时刻m m维量测噪声维量测噪声Q Qk k和和R Rk k分别称为系统噪声和量测噪声的方差矩阵，在分别称为系统噪声和量测噪声的方差矩阵，在卡尔曼滤波中要求它们分别是已知值的非负定阵和正卡尔曼滤波中要求它们分别是已知值的非负定阵和正定阵；定阵； k jk j是是Kronecker Kronecker 函数，即：函数，即：卡尔曼滤波要求卡尔曼滤波要求WWk k 和和VVk k 是互不相关的零均值的白是互不相关的零均值的白噪声序列，有：噪声序列，有： Var Var 为对为对求方差的符号求方差的符号卡尔曼滤波要求卡尔曼滤波要求m mx0x0和和C Cx0x0为已知量，为已知量，初始状态的初始状态的 一、二阶统计特性为：一、二阶统计特性为：且要求且要求X X0 0与与WWk k 和和VVk k 都不相关都不相关2 2.2 .2 离散卡尔曼滤波方程离散卡尔曼滤波方程 或 状态一步预测方程状态一步预测方程状态估值计算方程状态估值计算方程滤波增益方程滤波增益方程一步预测均方差方程一步预测均方差方程估计均方差方程估计均方差方程2 2.2 .2 离散卡尔曼滤波方程离散卡尔曼滤波方程 时间修正时间修正方程方程量测修正量测修正方程方程（1 1）状态一步预测方程）状态一步预测方程各滤波方程的物理意义：各滤波方程的物理意义：X Xk-1k-1的卡尔曼滤波估值的卡尔曼滤波估值利用利用X Xk-1k-1计算得到的一步预测计算得到的一步预测 也可以认为是利用也可以认为是利用也可以认为是利用也可以认为是利用k-1k-1k-1k-1时刻和以前时时刻和以前时时刻和以前时时刻和以前时刻的量测值得到的刻的量测值得到的刻的量测值得到的刻的量测值得到的X X X Xk k k k的一步预测的一步预测的一步预测的一步预测 上式就是通过上式就是通过 计算新息，把计算新息，把 估计出估计出来，并左乘一个系数矩阵来，并左乘一个系数矩阵 加到加到 中，从而得到中，从而得到 估值估值 和，和， 称为滤波增称为滤波增益矩阵益矩阵（2 2）状态估值计算方程）状态估值计算方程 计算估值计算估值计算估值计算估值X X X Xk k k k的方程。它是在一步预测的方程。它是在一步预测的方程。它是在一步预测的方程。它是在一步预测X X X Xk/k-1k/k-1k/k-1k/k-1的基础上，根据量测值的基础上，根据量测值的基础上，根据量测值的基础上，根据量测值Z Z Z Zk k k k计算出来的计算出来的计算出来的计算出来的一步预测误差一步预测误差若把若把 看作是量测看作是量测 的一的一步预测，则步预测，则 就是就是量测的一步预测误差量测的一步预测误差由两部分组成：由两部分组成： 和和 ， 正是在正是在 基础上估计基础上估计 所需信息，所需信息，因此因此又称又称 为新息为新息由于由于 也具有无偏性，即也具有无偏性，即 的均值的均值为零，所以为零，所以 也称为一步预测误差方差阵。上式中的也称为一步预测误差方差阵。上式中的 和和 分别就是新息中的两部分内容分别就是新息中的两部分内容（3 3）滤波增益方程）滤波增益方程一步预测均方差阵，即：一步预测均方差阵，即：K Kk k选取的标准就是卡尔曼滤波的估计准则，也就是使选取的标准就是卡尔曼滤波的估计准则，也就是使得得 均方误差阵最小：均方误差阵最小：如果如果R Rk k大，大，K Kk k就小就小R Rk k小，小，K Kk k就大就大（4 4）一步预测均方误差方程）一步预测均方误差方程从下式可以看出，求从下式可以看出，求K Kk k必须先求出必须先求出P Pk/k-1k/k-1式中式中 ，为，为 的估计误差，可以看出一步预测均方误差阵的估计误差，可以看出一步预测均方误差阵P Pk/k-1k/k-1是是从估计均方误差阵从估计均方误差阵P Pk-1k-1转移过来的，并且再加上系统转移过来的，并且再加上系统噪声方差的影响。噪声方差的影响。 的均方误差阵，的均方误差阵，即：即：（5 5）估计均方误差方程）估计均方误差方程或 计算量小，但在计算机有舍入误计算量小，但在计算机有舍入误计算量小，但在计算机有舍入误计算量小，但在计算机有舍入误差的条件下，不能始终保证算出差的条件下，不能始终保证算出差的条件下，不能始终保证算出差的条件下，不能始终保证算出的的的的P P P Pk k k k是对称的是对称的是对称的是对称的 （6 6）卡尔曼滤波的计算流程）卡尔曼滤波的计算流程滤波计算回路滤波计算回路滤波计算回路滤波计算回路增益计算回路增益计算回路增益计算回路增益计算回路2.32.3离散卡尔曼滤波基本方程使用要点离散卡尔曼滤波基本方程使用要点在滤波开始时，必须有初始值在滤波开始时，必须有初始值 和和 才才能进行能进行为了保证估值的无偏性，应选择：为了保证估值的无偏性，应选择：这样才能保证估计均方差阵这样才能保证估计均方差阵P Pk k始终最小。始终最小。1.1.滤波初值的选取滤波初值的选取有上述的卡尔曼滤波基本方程中的均方误差的公式有上述的卡尔曼滤波基本方程中的均方误差的公式2.2.估计均方误差阵的等价形式及选用估计均方误差阵的等价形式及选用 （a）（b）（c）由卡尔曼滤波方程的推导得知，基本方程只适用于系由卡尔曼滤波方程的推导得知，基本方程只适用于系统方程和量测方程都是离散型的情况。但实际的物理统方程和量测方程都是离散型的情况。但实际的物理系统一般都是连续的，动力学特性用连续的微分方程系统一般都是连续的，动力学特性用连续的微分方程来描述。所以在使用卡尔曼基本方程之前，必须对系来描述。所以在使用卡尔曼基本方程之前，必须对系统方程和量测方程进行离散化处理。统方程和量测方程进行离散化处理。 设描述物理系统动力特性的系统方程为设描述物理系统动力特性的系统方程为3.3.一步转移阵和等效离散系统噪声方差阵的计一步转移阵和等效离散系统噪声方差阵的计算算其中系统驱动源其中系统驱动源W W（t t）为白噪声过程，即）为白噪声过程，即q q为为w w（t t）的方差强度矩阵。）的方差强度矩阵。根据线性系统理论，系统方程的离散化形式为根据线性系统理论，系统方程的离散化形式为其中，其中， 满足方程满足方程对该方程求解并进行约等变化，得对该方程求解并进行约等变化，得式中式中 T T为滤波周期。为滤波周期。 这个式子即为一步转移矩阵的实时计算这个式子即为一步转移矩阵的实时计算公式。公式。同样，通过系统的离散化处理，得出等效离散系统噪声同样，通过系统的离散化处理，得出等效离散系统噪声方差阵方差阵一步预测方程改为：一步预测方程改为：状态估计方程改为：状态估计方程改为：其他滤波方程不变其他滤波方程不变4.4.系统有确定性控制时的滤波基本方程系统有确定性控制时的滤波基本方程设系统除了白噪声外，还有确定性驱动项设系统除了白噪声外，还有确定性驱动项5.5.一步预测基本方程一步预测基本方程令令一步预测基本方程式指利用一步预测基本方程式指利用 递推计算递推计算 的的 全套方程。全套方程。 根据基本方程的一步预测方程得根据基本方程的一步预测方程得 则则从而也可以得到均方误差从而也可以得到均方误差例例1 1 设有线定常系统设有线定常系统 式中状态变量式中状态变量XkXk和量测和量测ZkZk均为标量，均为标量，为常数。为常数。WkWk和和VkVk为零均值白噪声序列，分别具有协方差为零均值白噪声序列，分别具有协方差并且并且WkWk，VkVk，X0X0三者互不相关。求三者互不相关。求 的递推方程。的递推方程。例例2 -2 -滤波滤波设运动体沿某一直线运动，设运动体沿某一直线运动，tktk时刻的位移、速度、加时刻的位移、速度、加速度、加加速度分别为速度、加加速度分别为sksk，vkvk，akak，jkjk，只对运动体，只对运动体的位置做测量，测量值为的位置做测量，测量值为Zk=sk+VkZk=sk+Vk，若，若量测量的采样周期为量测量的采样周期为T,T,求求sksk，vkvk，akak的估计。的估计。设系统方程和量测方程的一般形式为设系统方程和量测方程的一般形式为2.42.4白噪声条件下离散卡尔曼滤波方程的一般形式白噪声条件下离散卡尔曼滤波方程的一般形式