卡尔曼滤波推导.pdfVIP

卡尔曼滤波推导 网上搜了好久，也没有看到像样点的卡尔曼滤波器的推导方法，唯有那几个步骤，实在让人 不解。最近看了好久，也有点心得，就写写吧，也好总结一下。 一、基于Bayes 理论 卡尔曼滤波理论首先基于Bayes 公式： p (z / x ) p (x ) p (x / z ) （1） 该方程为基本的Bayes 公式，p (z / x ) 为似然函数，即在 p (z ) 已知x 的情况下取观测量z 的可能性，p (x ) 为x 的先验密度，p (x / z ) 是我们要求的后验 密度。p (z ) 为一个与x 无关的概率密度，。因此对于x 的后验密度p (x / z ) 来说，p (z ) 可 以写作常量  ，得到p (x / z ) p (z / x )p (x ) （1.1）。 下面假设x 表示t 时刻的状态，z 表示t 时刻的观测量，u 表示t 时刻的控制量。 t t t 对于先验密度p (xt | z1:t 1) p (xt | xt 1)p (xt 1 | zt1)dxt 1 （2 ），这其实是一个全概率公式。 从上述公式（2 ）和（1.1）可知，要获得后验密度p (x / z ) 可分为两步： 1、根据前一刻的状态分布以及状态转移概率预测获得先验密度，p (x t | z 1:t 1) 。 2 、通过似然函数p (z / x ) 和先验密度p (x t | z 1:t 1) 更新获得后验密度p (x / z ) 。这里对于暂 不解释，后面会发现不必在乎。 二、卡尔曼滤波适用范围 卡尔曼滤波器适用于线性高斯系统，即状态方程和测量方程是线性的，加性噪声是的高 斯的。 线性状态方程：x A x B u  ，A 是状态转移矩阵，B 是控制输入矩阵， 是 0 t t t 1 t t t t t t 均值，协方差矩阵为R 的高斯噪声。 t 线性测量方程：z C x  ，C 是测量矩阵， 是 0 均值，协方差矩阵为Q 的高斯噪 t t t t t t t 声。 这里需要假设初始状态的概率密度服从N ( ,  ) 即 0 0 1 2 2 1 T 1 ( ) det(2 ) exp( ( ) ( )) p x   x   x  0 0 0 0 0 0 0 2 对于高斯变量的线性变换，仍然服从高斯分布。 三、卡尔曼预测 对于先验密度， p (xt | xt 1, z1:t1)  p (xt | xt 1) p (xt 1 | z1:t1)dxt 1 ，由线性状态转   x ~N ( A x B u ,R ) x ~N ( , )