卡尔曼滤波 Kalman filter

一、概述

卡尔曼滤波是综合系统预测值和观测值，以得到更准确的估计值。与频域上的高通、低通、带通的滤波概念不同，卡尔曼滤波是依据已有的状态转移矩阵（对动力系统的知识），再加上过程噪声和观测噪声的概率分布，进而对观测值进行最优估计。

卡尔曼滤波就是“猜-测”的过程，先“猜”，然后根据“测”的结果矫正“猜”的结果， 从而获得比“猜”和“测”都更准确的结果。大概过程如下：

• Predict过程： 在预测过程中，利用状态转移矩阵从上一时刻的状态预测当前的状态，得到先验估计；
• Correct过程：利用状态转移矩阵、预测值的协方差、观测矩阵、观测值的协方差计算卡尔曼增益(用来决定更相信预测值还是更相信观测值多一些），并综合当前的预测值和当前的观测值，更新预测值，测到后验估计。

二、动力系统

假设线性动力系统满足：

                       （1） 

                                         （2）

式中，

表示k时刻的系统状态。

是状态转移矩阵，由上一时刻的状态推导下一时刻的状态；

是控制矩阵，是控制向量（若没有控制，则无此项）；

是观测矩阵，由状态推导观测量；

过程噪音，假设满足均值为0的正态分布，其中为协方差。

是观测噪音，假设满足均值为0的正态分布，其中为协方差。

三、算法

迭代过程：a) Predict过程： 利用状态转移矩阵（先验）预测-> b) Correct过程：计算卡尔曼增益-> 结合观测值修正预测值。

卡尔曼滤波5个核心方程

a) Predict过程（先验预测）：

根据状态转移矩阵估计状态

          （1）