物理量解释
- $z_k$ 第k次测量结果
- $K_k$ 卡尔曼增益/因数
- $e_{EST}$ 估计误差
- $e_{MEA}$ 测量误差
简单描述
当对一个目标进行测量时，假设测量k次，第k次的预测值为 \(x_k = x_{k-1} + K_k(z_k - x_{k-1})\) 其中，$K_k$满足一个核心公式$K_k = \frac{e_{EST_{k-1}}}{e_{EST_{k-1}} + e_{MEA_k}}$
在k时刻：
- ${e_{EST_{k-1}}} \gg {e_{MEA_k}}$, $K_k -> 1$, $x_k=x_{k-1}+z_n-x_{k-1}=z_k$
- ${e_{EST_{k-1}}} \ll {e_{MEA_k}}$ $K_k -> 0$,$x_k = x_{k-1}$
随着测量进行，预测值需要依据如下步骤进行：
- 计算$K_k$
- 计算$x_k$
- 更新$e_{EST_k}$
一个简单的例子：随着递归的计算，估计值正越来越贴近真实值
数据融合
求$K_k$使得$\sigma_z$最小=>方差$Var(\hat z)$最小
过程推导
首先通过状态空间方程
\(x_k = Ax_{k-1} + Bu_{k-1} + w_{k-1}\) \(z_k = Hx_k + v_k\)
其中，$x_K$是状态变量，A是状态矩阵，B是控制矩阵，$w_k$是过程噪声，$z_k$是测量值，$v_k$是测量噪声。
过程噪声符合正态分布：
\[w \sim N(0,Q)\]Q是协方差矩阵，0指的是期望为0
\[Q = E[ww^T]\]由于过程噪声的存在，状态空间可由估计值表示：
\[\hat{x_k^-} = A\hat{x_{k-1}^-} + Bu_{k-1}\] \[z_k = H\hat{x_{k_{mea}}^-}\]含有-表示先验的值,不含表示后验，相当于经过数据融合之后的值。数据融合表示将计算出来的估计的值和测量值进行计算得到一个更贴近真实值的值，方式如下
\[\hat{x_k} = \hat{x_k^-} + G(H^-z_k - \hat{x_k^-})\]当G = 0时，$\hat{x_k} = \hat{x_k^-}$估计值等于算出来的结果，当G = 1时，$\hat{x_k} = H^-z_k$等于测出来的结果
将$G = K_kH$代入得
\[\hat{x_k} = \hat{x_k^-} + K_k(z_k - Hx_k^-) ,K_k\in[0,H^-]\]下面就需要寻找一个合适的$K_k$来使$\hat{x_k}$更贴近真实值$x_k$
首先考虑估计误差
\[e_k = x_k - \hat{x_k}\]误差符合正态分布
\[e_k \sim (0,P)\]P为误差的协方差矩阵$P = E[e_ke_k^T]$
我们寻找合适的$K_k$来使协方差矩阵P的迹最小
\[\begin{aligned} P &= E[ee^T]\\ &= E[(x_k-\hat{x_k})(x_k-\hat{x_k})^T]\\ &= E[(x_k-\hat{x_k^-}-K_Kz_k+K_kH\hat{x_k^-})(x_k-\hat{x_k^-}-K_Kz_k+K_kH\hat{x_k^-})^T]\\ &= E[(x_k-\hat{x_k^-}-K_K(Hx_k+v_k)+K_kH\hat{x_k^-})(x_k-\hat{x_k^-}-K_K(Hx_k+v_k)+K_kH\hat{x_k^-})^T]\\ &= E[(x_k-\hat{x_k^-}-K_kH(x_k-\hat{x_k^-})-K_kv_k)(x_k-\hat{x_k^-}-K_kH(x_k-\hat{x_k^-})-K_kv_k)^T]\\ &= E[[(I-K_kH)e_k^--K_kv_k][(I-K_kH)e_k^--K_kv_k]^T]\\ &= E[[(I-K_kH)e_k^--K_kv_k][e_k^{-T}(I-K_kH)^T-v_k^TK_k^T]] \end{aligned}\]打开括号，由于测量误差与估计误差独立，最终整理得
\[P_k = P_k^--K_kHP_k^--P_k^-H^TK_k^T+K_kHP_k^-H^TK_k^T+K_kRK_k^T\]求这个矩阵的迹
\[tr(P_k) = tr(P_k^-)-rtr(K_kHP_k^-)+tr(K_kHP_k^-H^TK_k^T)+tr(K_kRK_k^T)\]将其对$K_k$求导并令导数等于0
\[\frac{dtr(P_k)}{dK_k} = 0\] \[\frac{dtr(P_k)}{dK_k} = 0-2(HP_k^-)^T+2K_kHP_k^-H^T+2K_kR = 0\] \[K_k(HP_k^-H^T+R) = P_k^-H^T\] \[K_k = \frac{P_k^-H^T}{HP_k^-H^T+R}\]当R（测量噪声）特别大时，预测值会倾向于估计结果，当比较小时，会倾向于测量结果
误差协方差矩阵的推导
\(P_k^-=E[e_k^-e_k^{-T}]\)
\[\begin{aligned} e_k^- &=x_k-\hat{x^-_k}\\ &=Ax_{k-1}+Bu_{k-1}+w_{k-1}-A\hat{x_{k-1}}+Bu_{k-1}\\ &=A(x_{k-1}-\hat{x_{k-1}})+w_{k-1}\\ &=Ae_{k-1}+w_{k-1} \end{aligned}\] \[\begin{aligned} P_k^-&=E[e_k^-e_k^{-T}]\\ &=e[(Ae_{k-1}+w_{k-1})(Ae_{k-1}+w_{k-1})^T]\\ &=e[(Ae_{k-1}+w_{k-1})(e_{k-1}^TA^T+w_{k-1}^T)]\\ &=AE[e_{k-1}e_{k-1}^T]A^T+E[w_{k-1}w_{k-1}^T]\\ &=AP_{k-1}a^t+Q \end{aligned}\]用卡尔曼滤波进行真实值的预测过程为：先预测再校正，具体过程如下：
Matlab Simulink仿真案例
用simulink中自带的卡尔曼滤波模块搭建一个最简单的案例。为了简化案例，就用这个模块对一个测量进行滤波，测量的值就用两个阶跃和一点噪音代替。
这里面由于测量的值只有一个，因此所有的矩阵都是一个元素。调整Q、R、N矩阵，从而调整滤波器对于测量、预测的权重。Q越大对测量的权重越大，预测权重越小，R则相反。
运行一下效果如下：
