卡尔曼滤波推导
我们先给出离散时间现行系统的状态、观测方程：
\begin{array}{l}
x_{k}=A * x_{k-1}+B * u_{k}+w_{k-1} \\
z_{k}=H * x_{k}+v_{k}
\end{array}
其中，x_k 表示状态矩阵，z_k 表示状态矩阵的观测量，A 表示状态转移矩阵，B 表示输入矩阵，其具体影响状态矩阵，H 表示观测矩阵，\omega_{k-1} 表示过程噪声，v_k表示测量噪声。
测量噪声很容易理解，数据采集中都包含噪声，这个噪声就是测量噪声。对于过程噪声，大部分线性时变系统并不严格，具有一定不确定性，会导致状态矩阵有一定偏差，因而存在\omega_{k+1} 。两种噪声均建模为高斯白噪声，服从p(\omega) \in N(0, Q),p(v) \in N(0, R) ，为对应的协方差矩阵。Q,R 在此，定义三种状态矩阵的表示方式：
- x_k 表示第k次状态真实值；$
- \hat{x}_{k \mid k-1} 表示第k-1次测量后对第k次的估计值，即先验状态估计值；
- \hat{x}_{k \mid k} 表示第k次测量后对第k次的估计值，即后验状态估计值；
先验状态估计值由状态预测方程得到：\hat{x}_{k \mid k-1}=A*\hat{x}_{k-1 \mid k-1}+B*u_k 后验状态估计值由状态更新方程得到（第二种加法表达式）：\hat{x}_{k \mid k}=\hat{x}_{k \mid k-1}+K(z_k-H*\hat{x}_{k \mid k-1}) 误差e_{k \mid k-1} ：
\begin{aligned}
e_{k \mid k-1} &=x_{k}-\hat{x}_{k \mid k-1} \\
&=\left(A x_{k-1}+B u_{k-1}+\omega_{k-1}\right)-\left(A \hat{x}_{k-1 \mid k-1}+B u_{k-1}\right) \\
&=A\left(x_{k-1}-\hat{x}_{k-1 \mid k-1}\right)+\omega_{k-1}
\end{aligned}
误差e_{k \mid k} ：
\begin{aligned}
e_{k \mid k} &=x_{k}-\hat{x}_{k \mid k} \\
&=x_{k}-\left(\hat{x}_{k \mid k-1}+K\left(z_{k}-H \hat{x}_{k \mid k-1}\right)\right) \\
&=x_{k}-\left[\hat{x}_{k \mid k-1}+K\left(H x_{k}+v_{k}-H \hat{x}_{k \mid k-1}\right)\right] \\
&=x_{k}-\hat{x}_{k \mid k-1}-K H\left(x_{k}-\hat{x}_{k \mid k-1}\right)-K v_{k} \\
&=\left(I-K H\right) e_{k \mid k-1}-K v_{k}
\end{aligned}
先验协方差：
\begin{aligned}
P_{k \mid k-1}=E\left(e_{k \mid k-1} e_{k \mid k-1}^{T}\right)
&=E\left[\left(x_{k}-\hat{x}_{k \mid k-1}\right)\left(x_{k}-\hat{x}_{k \mid k-1}\right)^{T}\right] \\
&=E\left\{\left[A\left(x_{k-1}-\hat{x}_{k-1 \mid k-1}\right)+\omega_{k-1}\right]\left[A\left(x_{k-1}-\hat{x}_{k-1 \mid k-1}\right)+\omega_{k-1}\right]^{T}\right\} \\
&=A E\left[\left(x_{k-1}-\hat{x}_{k-1 \mid k-1}\right)\left(x_{k-1}-\hat{x}_{k-1 \mid k-1}\right)^{T}\right] A^{T}+E\left[\omega_{k-1} \omega_{k-1}^{T}\right] \\
&=A P_{k-1 \mid k-1} A^{T}+Q
\end{aligned}
后验协方差：
\begin{aligned}
P_{k \mid k} &=E\left[e_{k \mid k} e_{k \mid k}^{T}\right] \\
&=E\left\{\left[\left(I-KH\right) e_{k \mid k-1}-K v_{k}\right]\left[\left(I-K H\right) e_{k \mid k-1}-K v_{k}\right]^{T}\right\} \\
&=E\left\{\left[\left(I-KH\right) e_{k \mid k-1}-K v_{k}\right]\left[e_{k \mid k-1}^{T}\left(I-K H\right)^{T}-v_{k}^{T} K^{T}\right]\right\} \\
&=E\left[\left(I-K H\right) e_{k \mid k-1} e_{k \mid k-1}^{T}\left(I-KH\right)^{T}+K v_{k} v_{k}^{T} K^{T}\right] \\
&=\left(I-K H\right) P_{k \mid k-1}\left(I-KH\right)^{T}+KR K^{T} \\
&=P_{k \mid k-1}-K H P_{k \mid k-1}-P_{k \mid k-1} H^{T} K^{T}+K H P_{k \mid k-1} H^{T} K^{T}+K R K^{T}
\end{aligned}
卡尔曼代价函数为P_{k \mid k} ，即：J=\sum_{\min } P_{k} ，对K 求导：\frac{\partial P_{k \mid k}}{\partial K}=-2\left(P_{k \mid k-1} H^{T}\right)+2 K\left(H P_{k \mid k-1} H^{T}+R\right) 令导数为0，可以得卡尔曼增益K 为：K=P_{k \mid k-1}H^T(HP_{k \mid k-1}H^T+R)^{-1} 将其代入公式7中，可得：P_{k \mid k}=\left(I-K H\right) P_{k \mid k-1} 至此，完成所有公式的推导，预测即先验，更新即后验。
