kalman滤波公式直观推导

卡尔曼滤波公式的直观推导过程随机线性离散系统卡尔曼滤波基本方程不考虑控制作用，设随机线性离散系统的方程为：（1）式中：是系统的n维状态序列；是系统的m维观测序列；是p维系统的过程噪声序列；是m维观测噪声序列；是系统的n*n维状态转移矩阵；是n*p维噪声输入矩阵；是m*n维观测矩阵kkkkZHxv,11,11kkkkkkkxxwkxkZ1kwkv,1kk,1kkkH如果被估计状态和它的观测值满足式（1）的约束，k时刻的观测值为，且已获得k-1时刻的的最优估计状态，则的估计可按下面的滤波方程求解：状态一步预测：（2）状态估计：（3）滤波增益矩阵：（4）一步预测误差方差矩阵：(5)估计误差方差矩阵：（6）kxkZkZ1kx1ˆkxkxˆkx,1,11ˆˆkkkkkxx,1,1ˆˆˆ()kkkkkkkkxxKzHx1,1,1()TTkkkkkkkkkKPHHPHR1,1,1,1,1,1TTkkkkkkkkkkkkPPQ,1()kkkkkPIKHP直观推导简单直观的想法是用（7）作为的预测估计，对于k时刻系统观测值的预测估计为（8）当在k时刻获得观测值时，它与预测估计有一定的误差定义为（9）可以利用误差来修正原来的状态估计，于是有（10）记：（11）（12）它们分别为获得观测值前后对的估计误差,1,11ˆˆkkkkkxxkxkZ,1,1ˆˆkkkkkZHxkZ,1ˆkkZ,1,1,1ˆˆkkkkkkkkkZZZZHx%,1kkZ%,1ˆkkx,1,1ˆˆˆ()kkkkkkkkxxKzHxˆkkkxxx%kx,1,1ˆkkkkkxxx%接下来是根据目标函数(13)最小的要求来确定最优滤波增益矩阵根据式（1），式（12），式（9）有(14)滤波方差矩阵为：(15)[]TkkJExx%%kKˆkkkxxx%,1,1ˆˆ()kkkkkkkkxxKzHx,1,1ˆ()kkkkkkkkkxKHxvHx%,1,1,1[][]kkkkkkkkkkkkkxKHxvIKHxKv%%%[]TkkkPExx%%,1[][]TTkkkkkkkkkIKHPIKHKRK将上式展开，并同时加上或减去一下项（16）再将有关的项合并到一起，即式中前两项不含因子，为使滤波误差方差矩阵极小，只需选择（17）于是得到而此时误差方差矩阵为（18）1,1,1,1[]TTkkkkkkkkkkkPHHPHRHPkK1,1,1,1,111,1,1,1,1,1[]()[]()TTkkkkkkkkkkkkkkTTTTTTkkkkkkkkkkkkkkkkkkkkkkkPPPHHPHRHPKPHHPHRHPHRKPHHPHRgkK1,1,1()0TTkkkkkkkkkKPHHPHR1,1,1()TTkkkkkkkkkKPHHPHR1,1,1,1,1[]TTkkkkkkkkkkkkkkPPPHHPHRHP,1()kkkkIKHP进一步有：（19）于是有：,1,1ˆkkkkkxxx%,11,11,11ˆkkkkkkkkkxwx,11,11kkkkkkxw%,1,1,1[]TkkkkkkPExx%%1,1,1,1,1TTkkkkkkkkkkPQ