三轴加速度计 卡尔曼滤波 python 卡尔曼滤波 推导

三、卡尔曼五大公式推导

相信大家已经看了我的上一篇文章吧，其实上一篇通过例子讲了卡尔曼滤波思想，卡尔曼滤波又称最优递归数字处理算法，他就是一处理数据的算法，目的是获取更为准确的数据。接下来，我要开始超级硬核的推导，推导非常详细，而且容易立即额，希望你们可以和我一起推导

相信大家都了解过状态方程与观测器吧，现代控制理论比较基础的内容，我举一个简单的例子，就是控制中最常见的弹簧阻尼系统，如图1

该系统的力平衡方程可以用以下式子表示(不明白的可以去https://wenku.baidu.com/view/f800e6e1ddccda38376bafa0.html看看，有详细推导过程)$\frac{m}{k}\ddot{y}+\frac{f}{k}\dot{y}+y=\frac{1}{k}F$

在状态方程中，我们一般习惯用x表示，做一个简单的变量替换，用x代替y，然后让两边同时乘以k，可以得到$m\ddot{x}+f\dot{x}+kx=F=u$

在弹簧阻尼系统中，这个外力F实际就是我们的输入，可以将其表示为u。我们令$x_{1}=x,x_{2}=\dot{x}$,所以上式我们可以改写为$\left\{\begin{matrix} \dot{x_{1}}= x_{2}\\ \dot{x_{2}}= -\frac{k}{m}x_{1}-\frac{f}{m}x_{2}+\frac{1}{m}u \end{matrix}\right.$将其改写为矩阵形式$\begin{bmatrix} \dot{x_{1}}\\ \dot{x_{2}} \end{bmatrix}=\begin{bmatrix} 0 &1 \\ -\frac{k}{m} & -\frac{f}{m} \end{bmatrix}\begin{bmatrix} x_{1}\\ x_{2} \end{bmatrix}+\begin{bmatrix} 0\\ \frac{1}{m} \end{bmatrix}u$假设我们有一位置观测器，用于观测$x_{1},x_{2}$的值，我们观测值分别为$z_{1},z_{2}$,用数学式表达如下$\begin{bmatrix} z_{1}\\ z_{2} \end{bmatrix}=\begin{bmatrix} 1 & 0\\ 0 & 1 \end{bmatrix}\begin{bmatrix} x_{1}\\ x_{2} \end{bmatrix}$这两个式子就是我们的状态方程与观测方程，这是一个简单的例子，推广为一般形式，连续系统的表达式为$X=AX+Bu$$Z=HX$其离散形式$X_{k}=AX_{k-1}+Bu_{k-1}$$Z_{k}=HX_{k}$由此来看，我们可以从状态方程算出系统状态，但是这是处于理想条件下的情况，往往现实中状态方程和观测方程不会这么理想，会有各种各样的干扰，那么系统的状态方程可以写成下面的形式$X_{k}=AX_{k-1}+Bu_{k-1}+w_{k-1}$$Z_{k}=HX_{k}+v_{k}$其中$w_{k-1}$为过程噪声，$v_{k}$是测量噪声，它们是服从正态分布的而且是获取不了的，这时我们怎么得到更加接近真实值的$X_{k}$?

还记得上一篇文章我讲了一个用两个有误差的传感器获取更加准确的硬币直径的例子吗？我们现在的思路和那个例子一样，我们使用状态方程计算出来的值就可以看成传感器1测出来的值，用观测器得到的值就可以看为传感器2获得的值，按照那个例子的思路，我们开始推导

首先我们假设过程噪声w它是满足$P(w)\sim (0,Q)$即w满足正态分布，其均值为0，协方差矩阵为Q（统计概率方面的知识），为什么用协方差而不是方差，这是因为我们过程噪声$w_{k}$并不是标量，它是一个向量；同理，测量噪声同样满足正态分布，其数学表达式如下$P(v)\sim (0,R)$在实际中，我们往往得不到w和v的值，于是我们采用状态方程已知的部分来计算Xk，其数学表达式如下：${\color{Red} \hat{X_{k}}^{-}=A\hat{X_{k-1}}+Bu_{k-1}}$大家仔细观察一下，我这个式子出现了一些新的符号，$\hat{X_{k}}^{-}$这个符号我们一般将其称为第k次的先验估计，$\hat{X_{k-1}}$这个符号代表第k-1次的估计值。现在我来解释一下这两个参数的意义，$\hat{X_{k}}^{-}$是通过一个有误差的状态方程计算出来的值，后面我们会使用观测方程得到的值对这个值进行修正，即进行数据融合，因此我们称其为先验估计。$\hat{X_{k-1}}$则是上一次我们估计的值，这个值是在第k-1次计算中最接近真实值的值，所以我们在第k次计算中使用，以减弱干扰。

同理，在不考虑噪声时，通过观测方程我们也可以得到一个$\hat{X_{k}}$，其数学表达式如下：$\hat{X_{k_{mea}}}=H^{-1}Z_{k}$为了和先验估计$\hat{X_{k}}^{-}$区别，我们用$\hat{X_{k_{mea}}}$表示，叫做测量估计，mea代表measure（测量）。由上一篇数据融合的思想，我们第k次的估计值$\hat{X_{k}}$可以由先验估计和测量估计表示，表达式如下：$\hat{X_{k}}=\hat{X_{k}}^{-}+G(\hat{X_{k_{mea}}}-\hat{X_{k}}^{-})$$\hat{X_{k}}=\hat{X_{k}}^{-}+G(H^{-1}Z_{k}-\hat{X_{k}}^{-})$式中，G是一个系数，当G=0，$\hat{X_{k}}=\hat{X_{k}}^{-}$;当G=1时，$\hat{X_{k}}=\hat{X_{k_{mea}}}=H^{-1}Z_{k}$。

做一个简单的数学变换，令$G=K_{k}H$,则上式变换如下：${\color{Red} \hat{X_{k}}=\hat{X_{k}}^{-}+K_{k}(Z_{k}-H\hat{X_{k}}^{-})},(K_{k}\in [0,H^{-1}])$这个式子眼熟不？这个公式就和上篇文章数据融合的公式形式一致，按照数据融合的思想，我们现在就要找到一个参数$K_{k}$使得我们的估计值$\hat{X_{k}}$最接近真实值$X_{k}$,接下来要开始推导了，建议拿起笔一起计算，过程不难的。

首先定义$e_{k}=X_{k}-\hat{X_{k}}$,从状态方程和观测方程可知，真实值$X_{k}$与估计值$\hat{X_{k}}$只相差$w_{k}$与$v_{k}$的线性组合，而$w_{k}$与$v_{k}$都服从正态分布，因此$e_{k}$也服从正态分布。现在假设$e_{k}$服从正态分布$P(e_{k})\sim (0,P_{k})$，$P_{k}$是$e_{k}$的协方差矩阵，按照概率论的知识我们可以知道$P_{k}=E(e_{k}\cdot e_{k}^{T})$E代表求数学期望，$e_{k}$是列向量,其数学期望为0，满足正态分布（关于协方差不太懂可以转到）。为了使$\hat{X_{k}}$更加接近真实值$X_{k}$，我们需要选择合适的$K_{k}$使$e_{k}$的方差最小，即$P_{k}$的对角线之和最小，所以接下来化简$P_{k}$

$\begin{aligned} P_{k}&=E(e_{k}\cdot e_{k}^{T})\\ &=E[(X_{k}-\hat{X_{k}})(X_{k}-\hat{X_{k}})^{T}](1) \end{aligned}$将$\hat{X_{k}}=\hat{X_{k}}^{-}+K_{k}(Z_{k}-H\hat{X_{k}}^{-})$带入$(X_{k}-\hat{X_{k}})$,可得$X_{k}-\hat{X_{k}}=X_{k}-\hat{X_{k}}^{-}-K_{k}Z_{k}+K_{k}H\hat{X_{k}}^{-}（2）$将$Z_{k}=HX_{k}+v_{k}$代入（2），得到$\begin{aligned} (2)&=X_{k}-\hat{X_{k}}^{-}-K_{k}HX_{k}-K_{k}v_{k}+K_{k}H\hat{X_{k}}^{-}\\ &=(X_{k}-\hat{X_{k}}^{-})-K_{k}H(X_{k}-\hat{X_{k}}^{-})-K_{k}v_{k}\\ &=(I-K_{k}H)(X_{k}-\hat{X_{k}}^{-})-K_{k}v_{k}（3） \end{aligned}$仿照$e_{k}$的定义，我们定义$e_{k}^{-}=X_{k}-\hat{X_{k}^{-}}$将（3）式和（4）式代入（1），并化简$P_{k}=E[((I-K_{k}H)e_{k}^{-}-K_{k}v_{k})((I-K_{k}H)e_{k}^{-}-K_{k}v_{k})^{T}]$因为$(AB)^{T}=B^{T}A^{T}$;$(A+B)^{T}=A^{T}+B^{T}$,所以$\begin{aligned} P_{k}&=E[(I-K_{k}H)e_{k}^{-}(e_{k}^{-})^{T}(I-K_{k}H)^{T}-(I-K_{k}H)e_{k}^{-}v_{k}^{T}K_{k}^{T}-K_{k}v_{k}(e_{k}^{-})^{T}(I-K_{k}H)^{T}+K_{k}v_{k}v_{k}^{T}K_{k}^{T}]\\ &=E[(I-K_{k}H)e_{k}^{-}(e_{k}^{-})^{T}(I-K_{k}H)^{T}]-E[(I-K_{k}H)e_{k}^{-}v_{k}^{T}K_{k}^{T}]-E[K_{k}v_{k}(e_{k}^{-})^{T}(I-K_{k}H)^{T}]+E[K_{k}v_{k}v_{k}^{T}K_{k}^{T}] \end{aligned}$因为$(I-K_{k}H)$和$K_{k}$都是常数，所以

$E[(I-K_{k}H)e_{k}^{-}(e_{k}^{-})^{T}(I-K_{k}H)^{T}]=(I-K_{k}H)E[e_{k}^{-}(e_{k}^{-})^{T}](I-K_{k}H)^{T}$

$E[(I-K_{k}H)e_{k}^{-}v_{k}^{T}K_{k}^{T}]=(I-K_{k}H)E[e_{k}^{-}v_{k}^{T}]K_{k}^{T}$

$E[K_{k}v_{k}(e_{k}^{-})^{T}(I-K_{k}H)^{T}]=K_{k}E[v_{k}(e_{k}^{-})^{T}](I-K_{k}H)^{T}$

$E[K_{k}v_{k}v_{k}^{T}K_{k}^{T}]=K_{k}E[v_{k}v_{k}^{T}]K_{k}^{T}$

由概率论知识可知，当A与B相互独立，E（AB）=E（A）E（B），所以$E[v_{k}(e_{k}^{-})^{T}]=E[v_{k}]E[(e_{k}^{-})^{T}]$;因为$e_{k}^{-}=X_{k}-\hat{X_{k}^{-}}$,而$v_{k}$是测量噪声，两者之间相互独立，又因为$v_{k}$满足正态分布，所以$E[v_{k}(e_{k}^{-})^{T}]=E[v_{k}]E[(e_{k}^{-})^{T}]=0$，因此，我们可以得到以下式子：

$P_{k}=(I-K_{k}H)E[e_{k}^{-}(e_{k}^{-})^{T}](I-K_{k}H)^{T}+K_{k}E[v_{k}v_{k}^{T}]K_{k}^{T}$

仔细观察这个式子，出现了$E[e_{k}^{-}(e_{k}^{-})^{T}]$,参考前面$e_{k}$的协方差矩阵的计算公式$P_{k}=E(e_{k}\cdot e_{k}^{T})$,我们可以将$E[e_{k}^{-}(e_{k}^{-})^{T}]$定义为$P_{k}^{-}$,即先验误差的协方差矩阵，即$P_{k}^{-}=E[e_{k}^{-}(e_{k}^{-})^{T}]$又因为测量噪声$v_{k}$满足$P(v)\sim (0,R)$,所以$R=E[v_{k}v_{k}^{T}]$

此时$P_{k}$的表达式为

$\begin{aligned} P_{k}&=(I-K_{k}H)P_{k}^{-}(I-K_{k}H)^{T}+K_{k}E[v_{k}v_{k}^{T}]K_{k}^{T}\\ &=(P_{k}^{-}-K_{k}HP_{k}^{-})(I-K_{k}H)^{T}+K_{k}RK_{k}^{T}\\ &=P_{k}^{-}-K_{k}HP_{k}^{-}-P_{k}^{-}H^{T}K_{k}^{T}+K_{k}HP_{k}^{-}H^{T}K_{k}^{T}+K_{k}RK_{k}^{T}\\ &=P_{k}^{-}-K_{k}HP_{k}^{-}-(K_{k}HP_{k}^{-})^{T}+K_{k}HP_{k}^{-}H^{T}K_{k}^{T}+K_{k}RK_{k}^{T} \end{aligned}$此时$P_{k}$化简完毕，对$P_{k}$矩阵取迹，即$P_{k}$对角线元素之和，对$P_{k}$矩阵取迹写作$tr(P_{k})$所以$\begin{aligned} tr(P_{k})&=tr(P_{k}^{-})-tr(K_{k}HP_{k}^{-})-tr((K_{k}HP_{k}^{-})^{T})+tr(K_{k}HP_{k}^{-}H^{T}K_{k}^{T})+tr(K_{k}RK_{k}^{T})\\ &=tr(P_{k}^{-})-2tr(K_{k}HP_{k}^{-})+tr(K_{k}HP_{k}^{-}H^{T}K_{k}^{T})+tr(K_{k}RK_{k}^{T}) \end{aligned}$矩阵转置并不会改变主对角线上元素，所以$tr(K_{k}HP_{k}^{-})=tr((K_{k}HP_{k}^{-})^{T})$

为了求取参数$K_{k}$的值，我们令$\frac{dtr(P_{k})}{dK_{k}}=0$

补充两个数学公式$\frac{dtr(AB)}{dA}=B^{T}$$\frac{dtr(ABA^{T})}{dA}=2AB$

将这两个公式运用于$tr(P_{k})$对$K_{k}$求导，推导过程如下：$\frac{dtr(P_{k})}{dK_{k}}=0-2(HP_{k}^{-})^T+2K_{k}HP_{k}^{-}H^{T}+2K_{k}R=0$

$-(P_{k}^{-})^TH^{T}+K_{k}HP_{k}^{-}H^{T}+K_{k}R=0$又因为$P_{k}^{-}$是协方差矩阵，所以$(P_{k}^{-})^T=P_{k}^{-}$,即$-P_{k}^{-}H^{T}+K_{k}HP_{k}^{-}H^{T}+K_{k}R=0$

$K_{k}HP_{k}^{-}H^{T}+K_{k}R=P_{k}^{-}H^{T}$

$K_{k}(HP_{k}^{-}H^{T}+R)=P_{k}^{-}H^{T}$

${\color{Red} K_{k}=\frac{P_{k}^{-}H^{T}}{HP_{k}^{-}H^{T}+R}}$$K_{k}$的值推导完毕，结合后验估计公式${\color{Red} \hat{X_{k}}=\hat{X_{k}}^{-}+K_{k}(Z_{k}-H\hat{X_{k}}^{-})}$来分析一下式子

(1)、当R大时，代表测量噪声大，$K_{k}$小，表示我们更愿意使用先验估计，即计算值

(2)、当R小时，代表测量噪声小，$K_{k}$大，表示我们更愿意相信测量值

其实这篇文章标红的公式便是卡尔曼滤波的五大公式，在本文中我们完成了最困难的3个公式的推导，下一篇将完成卡尔曼滤波的五大公式推导