卡尔曼滤波

一、简介

上世纪60年代卡尔曼为了克服维纳滤波必须用到无限过去数据的缺点，把状态空间模型引入滤波理论，并导出了一套递推估计算法，后人称之为卡尔曼滤波理论。

卡尔曼滤波是以最小均方误差为估计的最佳准则，来寻求一套递推估计的算法，其基本思想是：采用信号与噪声的状态空间模型，利用前一时刻地估计值和现时刻的观测值来更新对状态变量的估计，求出现时刻的估计值。它适合于实时处理和计算机运算。卡尔曼滤波的实质是由测量值重构系统的状态向量。它以“预测-实测-修正”的顺序递推，根据系统的量测值来消除随机干扰，再现系统的状态，或根据系统的量测值从被污染的系统中恢复系统的本来面目。

二、计算方法

卡尔曼滤波是基于线性动力学系统。为了用卡尔曼滤波器基于有噪音的观测值序列估计一个过程的内部状态，则需要用卡尔曼滤波器的框架对该过程建模。

卡尔曼滤波模型假定k时间点的真实状态从k-1时刻的状态演化而来，x_k=F_kx_k-1+B_ku_k+w_k, 其中F_k为状态转移矩阵，B_k为控制输入变量系数矩阵，u_k为控制输入变量，w_k为过程噪音向量，是均值为0的多变量正态分布，对应协方差矩阵为Q_k。

在时刻k状态x_k的观测值为z_k，且满足z_k=H_kx_k+v_k, 其中H_k为观测值映射矩阵，v_k为观测噪音，假设其是均值为0的多变量正态分布，对应协方差矩阵为R_k。在这里假定各种噪音变量间为彼此独立的。

卡尔曼滤波是一种迭代估计法。这意味着估计当前时刻的状态仅需要前一时刻估计的状态和当前时刻的观测值，不需要之前的观测值和估计状态的序列。滤波得到的状态由两个变量来表示，k时刻的状态估计值_k和k时刻的后验误差协方差矩阵P_k。

卡尔曼滤波由两步完成，即“预测”和“更新”。“预测”通过上一时刻得到的估计状态来产生当前时刻的估计状态。该预测状态被称为先验状态估计，是因为当前时刻的状态估计没有包含当前时刻的观测信息。在“更新”步骤中，需要结合观测信息来对先验状态估计进行进一步修正，得到后验状态估计。这两个步骤交替进行，不断得出最新时刻的最优后验状态估计。

“预测”：

先验状态估计    $\widehat{\mathbf{X}}_{k|k-1}=\mathbf{F}_{k}\widehat{\mathbf{X}}_{k-1}+\mathbf{B}_{k}\mathbf{u}_{k}$  

先验误差协方差    $\widehat{\mathbf{P}}_{k|k-1}=\mathbf{F}_{k}\mathbf{P}_{k-1}\mathbf{F}_{k}^{T}+\mathbf{Q}_{k}$

“更新”：

根据最小误差求解得到卡尔曼增益

 $\mathbf{K}_{k}=\mathbf{P}_{k|k-1}\mathbf{H}_{k}^{T}(\mathbf{R}_{k}+\mathbf{H}_{k}\mathbf{P}_{k|k-1}\mathbf{H}_{k}^{T})^{-1}$

利用观测结果对估计的先验状态进行修正，得到后验状态估计

 $\widehat{\mathbf{X}}_{k}=\widehat{\mathbf{X}}_{k|k-1}+\mathbf{K}_{k}(\mathbf{z}_{k}-\mathbf{H}_{k}\widehat{\mathbf{X}}_{k|k-1})$

计算后验误差协方差

 $\mathbf{P}_{k}=(\mathbf{I}-\mathbf{K}_{k}\mathbf{H}_{k})\mathbf{P}_{k|k-1}(\mathbf{I}-\mathbf{K}_{k}\mathbf{H}_{k})^{T}+\mathbf{K}_{k}\mathbf{R}_{k}\mathbf{K}_{k}^{T}$