卡尔曼滤波(Kalman Filter)状态估计、协方差矩阵、数据融合，看这一文即可，【非常详细推导，认真推理，包你看得懂】 上

本文是对卡尔曼滤波学习的记录，主要参照了DR_CAN老师的视频进行学习，文章末尾有链接！

卡尔曼滤波

卡尔曼滤波（Kalman Filtering） 是一种用于估计系统状态的数学方法，它结合了传感器测量和系统模型的信息，以获得对系统状态的最佳估计。卡尔曼滤波最初是由Rudolf E. Kálmán在1960年代提出的，用于航空航天领域的导航和控制问题。自那时以来，它已经在许多领域得到了广泛应用，包括工程、经济学、生物学等。

卡尔曼滤波的主要思想是通过融合系统的动态模型（通常是线性系统）和实际测量值来估计系统的状态。它基于两个主要假设：
(1) 系统的动态行为可以用线性动态方程描述。
(2) 测量噪声和过程噪声都是高斯分布的，并且彼此独立。

卡尔曼滤波的过程可以概括为以下几个步骤：
(1) 预测（Predict）：根据系统的动态模型，使用先前的状态估计来预测下一个时刻的状态，并估计状态的不确定性。
(2) 更新（Update）：将新的测量值与预测值进行比较，根据它们的不确定性，更新状态的估计值和不确定性。这一步使用贝叶斯推断来融合测量和预测信息。

1. 数据融合

在学习卡尔曼滤波（Kalman Filtering）之前首先要对数据融合（对于理解卡尔曼滤波很重要）有一定的概念（卡尔曼滤波就是先验数据和后验数据的数据融合）：
例如：你用两个称，去称物体，得到两个重量和标准差(符合正态分布)分别是：
$$\begin{array}{c}\begin{array}{ll}{z}_1=30\mathrm{g}& {\sigma }_1=2\mathrm{g}\\ z_2=32\mathrm{g}& {\sigma }_2=4\mathrm{g}\end{array}>\begin{array}{c}\text{ 正态分布 }\end{array}\end{array}$$

如图所示，如何去估算的物体真实重量（如绿虚线所示）？凭感觉肯定在30~32g之间，而如何利用数学去找到最优的估计值呢？就要用到数据融合的思想。

那么，定义估计的真实重量 $\hat{z}=z_1+k\left(z_2-z_1\right)$ ，其中 $k\in [0,1]$ 就是卡尔曼增益（Kalman Gain），当 $k=0$ 时， $\hat{z}=z_1$; 当 $k=1$ 时， $\hat{z}=z_2$. 通过调节 $k$ 的大小，可以表征估计值更接近哪个数据。为了获得最优的估计值，那么我们希望估计出来的值方差小，表明这个估计值较为准确，所以我们需要求得一个合适的 $k$ 值，使得 $\hat{z}$ 的方差 $Var(\hat{z})$ 或者标准差 ${\sigma }_{\hat{z}}$最小.

$$\begin{array}{c}\begin{array}{rl}{\sigma }_{\hat{z}}^2=Var(\hat{z})& =Var(z_1+k\left(z_2-z_1\right))\\ & =Var((1-k)z_1+k{z}_2)\\ & =(1-k{)}^{2}Var(z_1)+k^{2}Var(z_2)\\ & =(1-k{)}^2{\sigma }_1^2+k^2{\sigma }_2^2\end{array}\end{array}$$
为了使 $Var(\hat{z})$ 最小，对 $k$ 求导等于0得：
$$\begin{array}{c}-2(1-k){\sigma }_1^2+2k{\sigma }_2^2=0\end{array}$$
整理得：$$\begin{array}{c}k=\frac{{\sigma }_1^2}{{\sigma }_1^2+{\sigma }_2^2}=\frac{2^2}{2^2+4^2}=0.2\end{array}$$
因此，通过数据融合得到的最优估计值为：
$$\begin{array}{c}\hat{z}=z_1+k\left(z_2-z_1\right)=30+0.2(32-30)=30.4\end{array}$$

2. 协方差矩阵

接下来介绍一下协方差矩阵的概念，协方差矩阵 是将方差和协方差在一个矩阵中表现出来，表征的是个变量间的联动关系，为了更好的理解其含义，
例：

球员	身高 $x$	体重 $y$	年龄 $z$
A	179	74	33
B	187	80	31
C	175	71	28
平均	180.3	75	30.7

那么，我们可以计算身高、体重、年龄的方差和协方差分别为：
$$\begin{gathered} \text{方差}：\begin{array}{rl}& {\sigma }_x^2=\frac{1}{3}\left((179-180.3{)}^2+(187-180.3{)}^2+(175-180.3{)}^2\right)=24.89\\ & {\sigma }_y^2=14\\ & {\sigma }_z^2=4.2\end{array} \\ \text{协方差}：\begin{array}{rl}& {\sigma }_x{\sigma }_y=\frac{1}{3}((179-180.3)(74-75)+(187-180.3)(80-75)+(175-180.3)(71-75))=18.7={\sigma }_y{\sigma }_x\\ & {\sigma }_x{\sigma }_z=4.4={\sigma }_z{\sigma }_x\\ & {\sigma }_y{\sigma }_z=3.3={\sigma }_z{\sigma }_y\end{array} \end{gathered}$$
那么协方差矩阵 $P$ 为（注意：${\sigma }_x{\sigma }_y$ 不是表示二者相乘而是协方差 Cov$({\sigma }_x,{\sigma }_y)$，下文也是如此:
$$\begin{array}{c}P=\left[\begin{array}{ccc}{\sigma }_x^2& {\sigma }_x{\sigma }_y& {\sigma }_x{\sigma }_z\\ {\sigma }_{yx}& {\sigma }_y^2& {\sigma }_y{\sigma }_z\\ {\sigma }_{zx}& {\sigma }_z{\sigma }_y& {\sigma }_z^2\end{array}\right]=\left[\begin{array}{ccc}24.89& 18.7& 4.4\\ 18.7& 14& 3.3\\ 4.4& 3.3& 4.22\end{array}\right]\end{array}$$
这边解释一下协方差各个数的含义：首先是对角线，24.89、14、4.22分别体现了身高、体重、年龄的跨度，越大表示跨度越大；${\sigma }_x{\sigma }_y=18.7$ 表示了身高和体重的相关性，是比较相关的；再看${\sigma }_x{\sigma }_z=4.4$ 和${\sigma }_y{\sigma }_z=3.3$表示身高和年龄以及体重和年龄的关联性就不是很强。
方便编程运算协方差矩阵 $P$ 可通过矩阵计算：$$\begin{array}{c}\begin{array}{rl}& a=\left[\begin{array}{lll}{x}_1& y_1& z_1\\ x_2& y_2& z_2\\ x_3& y_3& z_3\end{array}\right]-\frac{1}{3}\left[\begin{array}{lll}1& 1& 1\\ 1& 1& 1\\ 1& 1& 1\end{array}\right]\left[\begin{array}{lll}{x}_1& y_1& z_1\\ x_2& y_2& z_2\\ x_3& y_3& z_3\end{array}\right]\\ & P=\frac{1}{3}{a}^{T}a\\ & \end{array}\end{array}$$

3. 卡尔曼滤波器的推导

假设系统的状态 $X_k=[x_1{x}_2{]}^T$ , 输出为 $Z_k$且状态空间方程为：
$$\begin{array}{c}\begin{array}{rl}& X_k=A{X}_{k-1}+B{U}_{k-1}+W_{k-1}\\ & Z_k=H{X}_k+V_k\end{array}\end{array}$$其中， $W_{k-1}=[w_1{w}_2{]}^T$ 为系统的过程噪声，$V_k$ 为测量噪声, 二者都是一种不确定性的表现，在自然界中，一般可以将它们假设符合正态分布，也就是其概率分布符合期望为 0，协方差矩阵为 $Q$，即 $P(W)\sim (0,Q)$ 。其中,
$$\begin{array}{c}Q=E[W{W}^T]=E\left[\left[\begin{array}{l}{w}_1\\ w_2\end{array}\right]\left[\begin{array}{ll}{w}_1& w_2\end{array}\right]\right]=\left[\begin{array}{l}{\sigma }_{w_1}^2{\sigma }_{w_1}{\sigma }_{w_2}\\ {\sigma }_{w_2}{\sigma }_{w_1}{\sigma }_{w_2}^2\end{array}\right]\end{array}$$
另外，同上 $P(V)\sim (0,R)$。

3.1 先验估计

由于我们不确定噪声量的大小，所以只能得到 $X_k$ 的先验估计值 $\widehat{X_k^{-}}$ （^表示估计值的意思，-表示先验），也就是不考虑状态方程中的过程噪声计算出来的结果称为先验估计值，也就是: $$\begin{array}{c}\widehat{X_k^{-}}=A{\hat{X}}_{k-1}+B{U}_{k-1}\text{ （算出来的） }\end{array}$$
而后，我们可以测量测出 $Z_k$ ,不考虑噪声前提下，根据 $Z_k=H{X}_k$ 反求状态得：$$\widehat{X_{k_{mea}}}=H^{-1}{Z}_k\text{ （测出来的） }$$
所以，无论是算出来的还是测出来的的数据都不包含噪声，所以我们才需要用卡尔曼滤波去估计真实值（这时候就是体现出上一章所讲的数据融合概念的意义）根据数据融合可得后验估计值 $\widehat{X_k}$ 为(当 G=0 表示完全相信算出来的数据，G=1表示完全相信测出来的数据)：
$$\begin{array}{c}\begin{array}{l}{\hat{X}}_k={\hat{X}}_k^{-}+G\left(H^{-1}{Z}_k-{\hat{X}}_k^{-}\right)\end{array}\end{array}$$
为了方便计算，我们可定义 $G=k_{k}H$带入得卡尔曼滤波器的重要公式( $k_k\in [0,H^{-1}]$ )：$$\begin{array}{c}\begin{array}{l}{\hat{X}}_k={\hat{X}}_k^{-}+k_k(Z_k-H{\hat{X}}_k^{-})\end{array}\end{array}$$

3.2 求解最优卡尔曼增益

此时，需要去寻找一个最优的 $k_k$ 使得估计值 ${\hat{X}}_k$ 与真实值 $X_k$ 误差最小, 引入误差 $e_k=X_k-{\hat{X}}_k$ , 将估计值(12)代入误差公式中去，得：
$$\begin{array}{c}\begin{array}{ll}{e}_k=X_k-{\hat{X}}_k& =X_k-({\hat{X}}_k^{-}+k_k(Z_k-H{\hat{X}}_k^{-}))\\ (化简得)& =(I-k_{k}H)(X_k-{\hat{X}}_k^{-})-k_k{V}_k\\ & =(I-k_{k}H)e_k^{-}-k_k{V}_k（定义误差先验为e_k^{-}=X_k-{\hat{X}}_k^{-}）\end{array}\end{array}$$

根据此前定义 $P(e_k)\sim (0,P_k)$ ，那么协方差矩阵 $P_k$(推导较为复杂，懒得推可直接看结果)：
$$\begin{array}{c}\begin{array}{ll}{P}_k& =E[e{e}^T]=\left[\begin{array}{l}{\sigma }_{e_1}^2{\sigma }_{e_1}{\sigma }_{e_2}\\ {\sigma }_{e_2}{\sigma }_{e_1}{\sigma }_{e_2}^2\end{array}\right]注：(AB{)}^T=B^T{A}^T;(A+B{)}^T=A^T+B^T\\ & =E\left[[I-k_{k}H)e_k^{-}-k_k{V}_k][(I-k_{k}H)e_k^{-}-k_k{V}_k{]}^T\right]\\ & =E[[I-k_{k}H)e_k^{-}-k_k{V}_k][{e_k^{-}}^T(I-k_{k}H{)}^T-V_k^T{k}_k^T]]\\ & =E[(I-k_{k}H)e_k^{-}{e_k^{-}}^T(I-k_{k}H{)}^T]+E[k_k{V}_k{V}_k^T{k}_k^T]\\ & 注：E[(I-k_{k}H)e_k^{-}{V}_k^T{k}_k^T]=(I-k_{k}H)E[e_k^{-}]E[V_k^T]k_k^T=0由于E[e_k^{-}]=0.\\ & =(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\\ & =(I-k_{k}H)P_k^{-}(I-k_{k}H{)}^T+k_{k}R{k}_k^T\\ & =(P_k^{-}-k_{k}H{P}_k^{-})(I^T-H^T{k}_k^T)+k_{k}R{k}_k^T\\ & =P_k^{-}-k_{k}H{P}_k^{-}-P_k^{-}{H}^T{k}_k^T+k_{k}H{P}_k^{-}{H}^T{k}_k^T+k_{k}R{k}_k^T注：P^{-}为先验协方差\end{array}\end{array}$$

此时，引入迹的概念：$tr(P_k)={\sigma }_{e_1}^2+{\sigma }_{e_2}^2$ ，为对角线方差相加，我们希望能取到最优 $k_k$ 使方差最小，也就是迹最小，使误差 $e_k$ 接近期望值 0。
$$\begin{array}{c}\begin{array}{l}tr(P_k)=tr(P_k^{-})-2tr(k_{k}H{P}_k^{-})+tr(k_{k}H{P}_k^{-}{H}^T{k}_k^T)+tr(k_{k}R{k}_k^T)\\ 注：k_{k}H{P}_k^{-}与P_k^{-}{H}^T{k}_k^T互为转至，所以迹是一样的\end{array}\end{array}$$
为了取最优的卡尔曼增益 $k_k$ 使得迹最小，所以迹对 $k_k$ 求导等于0 得 (注：$\frac{\partial tr(AB)}{\partial A}=B^T;\frac{\partial tr(AB{A}^T)}{\partial A}=2AB$)：
$$\begin{array}{c}\begin{array}{rl}\frac{\partial tr(P_k)}{\partial k_k}=-2(H{P}_k^{-}{)}^T+2k_{k}H{P}_k^{-}{H}^T+2k_{k}R& =0\\ -P_k^{-}{H}^T+k_k(H{P}_k^{-}{H}^T+R)& =0\\ k_k(H{P}_k^{-}{H}^T+R)& =P_k^{-}{H}^T\end{array}\\ 因此：k_k=\frac{P_k^{-}{H}^T}{H{P}_k^{-}{H}^T+R}\end{array}$$
因此，公式17可以理解为：当测量噪声协方差 $R$ 无穷大时，$k_k=0$，状态估计值：${\hat{X}}_k={\hat{X}}_k^{-}+k_k(Z_k-H{\hat{X}}_k^{-})={\hat{X}}_k^{-}$，选择相信计算值；反之，测量噪声协方差等于 0 时，$k_k=H^{-1}$, 状态估计值：${\hat{X}}_k=H^{-1}{Z}_k$，选择相信测量值。

至此，卡尔曼滤波的数学推导就记录到这里了，下一章将具体讲解卡尔曼滤波的5个重要公式以及其应用！请各位移步下文。
敲公式不易，麻烦各位看官一键三连！感谢！欢迎收藏以便后续用到的时候查公式！

视频专栏链接：DR_CAN卡尔曼滤波视频专栏：密码：6.66

链接: DR_CAN卡尔曼滤波视频专栏