卡尔曼滤波器推导

本文已参与「新人创作礼」活动，一起开启掘金创作之路

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

简介

  设

  卡尔曼滤波器是从测量值$Z$k的平均数开始的。开始推导： 由上式可知   也就是说随着$k$的增大，测量结果Zk不在重要，因为已经获得了足够多的测量值，此时的估计值已经很贴近了实际值了。我们令Kk=$1/k$，即 可知，Kk在$[0,1]$之间，当Kk $=0$时，估计值等于上一次计算的估计值，当Kk $=1$时，估计值等于本次测量值，这时引入两个参数eEST，eMEA,令 有 其中，eMEA是测量误差，是测量工具自身的属性，是不变的，eEST是估计误差，会受历史数据的影响，即 由上述几个式子便可使用卡尔曼滤波器来解决实际的问题了。步骤如下： 第一步 第二步 第三步 例   有一个质量为$50g$的物体，但我们此时并不知道该物体质量是多少，先估计其有$46g$，估计误差为$5g$，将其放在称上称得质量为 该称的测量误差为$3g$,将所有数据放在Excel里进行计算 其中蓝色线条表示测量值，红色线条表示估计值，从图中可以看出，尽管测量值起伏较大，但估计值整体趋势很平缓，不断向实际值靠拢且十分接近实际值。

数据融合

  从一个例子入手，设某物体质量为$m$,分别用标准差为σ1 $=2g$和σ2 $=4g$得称来称该物体，称得质量分别为Z1 $=30g$和Z2 $=33g$,求出最优估计值。   从上述中可得式 此时引入标准差，即估计值的标准差，当标准差越小时，即方差越小，估计值的波动越小，也就越趋于真实值。如下： 由上式可知，估计值的方差是关于Kk的函数，使估计值方差对Kk求导，即 将σ1 $=2g$和σ2 $=4g$代入上式中，Kk $=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[WW^T]$,推导如下： 同理，$R=E[VV^T]$。

卡尔曼增益推导

  由于过程噪声是不确定的，于是状态估计值先验为 根据先验估计和测量估计可得出后验估计 令 $G=$ Kk$H$,则   我们的目标是求得合理的Kk值使得估计误差最小，有 同理   当后验估计值越接近真实值 Xk, 则说明 ek 的方差越小，即 ek 越接近于期望值$0$。于是有 接着推导

有

先验误差协方差矩阵

ek的协方差矩阵Pk

  由之前的推导可得

总结