线性系统估计————卡尔曼滤波

前提：线性时变系统（F、G、H不变）；

          v(k)是具有协方差矩阵Q(k)的零均值，白色高斯过程噪声，w(k)是具有协方差矩阵R(k)的零均值，白色高斯过程噪声;

          初始状态x(0)是高斯的，具有均值和协方差P(0|0);

          且已知量测值

动态（系统）方程：

量测（观测）方程：

卡尔曼滤波算法：

状态一步预测：

 一步预测斜方差：

              预测的量测值：

量测的预测协方差：

滤波增益：

               量测残差：

估计值：

状态协方差：