卡尔曼滤波器推导

注：受控制领域大牛CAN博士启发，受益匪浅，作此文以为笔记。

简介

  设

  卡尔曼滤波器是从测量值$Z$_k的平均数开始的。开始推导：

由上式可知

  也就是说随着$k$的增大，测量结果Z_k不在重要，因为已经获得了足够多的测量值，此时的估计值已经很贴近了实际值了。我们令K_k=$1/k$，即

可知，K_k在$[0,1]$之间，当K_k $=0$时，估计值等于上一次计算的估计值，当K_k $=1$时，估计值等于本次测量值，这时引入两个参数e_EST，e_MEA,令

有

其中，e_MEA是测量误差，是测量工具自身的属性，是不变的，e_EST是估计误差，会受历史数据的影响，即

由上述几个式子便可使用卡尔曼滤波器来解决实际的问题了。步骤如下：
第一步

第二步

第三步

例
  有一个质量为$50g$的物体，但我们此时并不知道该物体质量是多少，先估计其有$46g$，估计误差为$5g$，将其放在称上称得质量为

该称的测量误差为$3g$,将所有数据放在Excel里进行计算

其中蓝色线条表示测量值，红色线条表示估计值，从图中可以看出，尽管测量值起伏较大，但估计值整体趋势很平缓，不断向实际值靠拢且十分接近实际值。

数据融合

  从一个例子入手，设某物体质量为$m$,分别用标准差为σ₁ $=2g$和σ₂ $=4g$得称来称该物体，称得质量分别为Z₁ $=30g$和Z₂ $=33g$,求出最优估计值。
  从上述中可得式

此时引入标准差，即估计值的标准差，当标准差越小时，即方差越小，估计值的波动越小，也就越趋于真实值。如下：

由上式可知，估计值的方差是关于K_k的函数，使估计值方差对K_k求导，即

将σ₁ $=2g$和σ₂ $=4g$代入上式中，K_k $=0.2$,得

协方差矩阵

  有以下$3$组数据

平均值

方差

协方差

协方差矩阵$P$

为方便编程计算，引入一个过渡矩阵$A$

则

注： 式中的$3$是指矩阵得维数。
  在$matlab$中验证一下

与计算得结果一致。

状态空间表达式

  有如下系统

  该系统中，物块质量为$M$,弹簧弹力系数为$k$,阻尼系数为$B$,系统输入为拉力$F$。于是有

状态变量

得

测量量

状态空间表达式

化为离散形式

  由于系统存在各种不确定性，需要加入过程噪声$W$和测量噪声$V$,即

  $W$服从正态分布，期望为$0$，协方差矩阵为$Q$,即$P(W)-N(0,Q)$。$V$也服从正态分布，期望为$0$，协方差矩阵为$R$，即$P(V)-N(0,R)$。其中$Q=E[W{W}^T]$,推导如下：

同理，$R=E[V{V}^T]$。

卡尔曼增益推导

  由于过程噪声是不确定的，于是状态估计值先验为

根据先验估计和测量估计可得出后验估计

令 $G=$ K_k$H$,则

  我们的目标是求得合理的K_k值使得估计误差最小，有

同理

  当后验估计值越接近真实值 X_k, 则说明 e_k 的方差越小，即 e_k 越接近于期望值$0$。于是有

接着推导

有

先验误差协方差矩阵

e_k的协方差矩阵P_k

  由之前的推导可得

总结