kalman filter

k时刻的位置和速度
$$x_k=\left[\begin{array}{c}{p}_k\\ v_k\end{array}\right]$$

运动方程
$$\begin{gathered} x_k=F_{k-1}{x}_{k-1}+G_{k-1}{u}_{k-1}+w_{k-1} \\ =\left[\begin{array}{cc}1& \Delta t\\ 0& 1\end{array}\right]x_{k-1}+\left[\begin{array}{c}\frac{1}{2}\Delta t^2\\ \Delta t\end{array}\right]a_{k-1}+w_{k-1} \end{gathered}$$

$F_k$叫状态转移矩阵， $G_k$叫控制输入矩阵, 噪声$w_k\sim N(0,Q_k)$

测量方程
$z_k=H_k{x}_k+v_k$

$H_k$为状态观测矩阵。 注意这里的$v_k$是噪声不是速度， 后面我们只用$x_k$来代表运动状态， 而不用$p_k,v_k$来举例了， 所以后面的$v_k$都是噪声$v_k\sim N(0,R_k)$

然后就可以进行我们的prediction 和 correction

我们用$x^k$表示真实状态, ${\hat{x}}_k$表示通过运动方程预测的值（先验估计值a prior state estimate），${\check{x}}_k$表示最优估计值（后验估计值a posteriori state estimate）

${\check{e}}_k=x_k-{\check{x}}_k$真实值与先验的误差
${\hat{e}}_k=x_k-{\hat{x}}_k$真实值与后验的误差

${\check{P}}_k=cov({\check{e}}_k)=E({\check{e}}_k\ast {\check{e}}_k^T)$真实值与先验的协方差, 注意${\check{e}}_k$为列向量， ${\check{e}}_k\ast {\check{e}}_k^T$为矩阵
${\hat{P}}_k=cov({\hat{e}}_k)=E({\hat{e}}_k\ast {\hat{e}}_k^T)$ 真实值与后验的协方差

推导一下后面那个等式
$cov({\check{e}}_k)=E({\check{e}}_k\ast {\check{e}}_k^T)-E({\check{e}}_k)E({\check{e}}_k^T)$
其中$E({\check{e}}_k)=E(x_k-{\check{x}}_k)=-E({\check{x}}_k)=0$ 先验均值为0

prediction

更新先验

更新运动状态
${\check{\mathbf{x}}}_{\mathbf{k}}={\mathbf{F}}_{\mathbf{k}-1}{\hat{\mathbf{x}}}_{\mathbf{k}-1}+{\mathbf{G}}_{\mathbf{k}-1}{\mathbf{u}}_{\mathbf{k}-1}+{\mathbf{w}}_{\mathbf{k}-1}$ ， 这个很直观， 根据运动方程得到

更新协方差

${\check{P}}_k=cov({\check{e}}_k)=cov({\check{e}}_k,{\check{e}}_k)=cov({\check{x}}_k)=F_{k-1}cov({\hat{x}}_{k-1})F_{k-1}^T+cov(w_{k-1})=F_{k-1}{\hat{P}}_{k-1}{F}_{k-1}^T+Q_{k-1}$

得到
${\check{\mathbf{P}}}_{\mathbf{k}}={\mathbf{F}}_{\mathbf{k}-1}{\hat{\mathbf{P}}}_{\mathbf{k}-1}{\mathbf{F}}_{\mathbf{k}-1}^{\mathbf{T}}+{\mathbf{Q}}_{\mathbf{k}-1}$

稍微推导一下
$cov({\check{e}}_k,{\check{e}}_k)=cov(x_k-{\check{x}}_k,x_k-{\check{x}}_k)=cov(-{\check{x}}_k,-{\check{x}}_k)=cov({\check{x}}_k,{\check{x}}_k)$
常数与常数，变量与常数($x_k$)的协方差为0， 协方差也满足负负得正

Correction

先算optimal gain
${\mathbf{K}}_{\mathbf{k}}={\check{\mathbf{P}}}_{\mathbf{k}}{\mathbf{H}}_{\mathbf{k}}^{\mathbf{T}}({\mathbf{H}}_{\mathbf{k}}{\check{\mathbf{P}}}_{\mathbf{k}}{\mathbf{H}}_{\mathbf{k}}^{\mathbf{T}}+{\mathbf{R}}_{\mathbf{k}}{)}^{-1}$

这个$K$怎么来的呢？
我们希望后验误差${\hat{e}}_k^2$尽可能小，又由于${\hat{x}}_k$是随机变量，可以对后验误差求期望，这样把随机变量出现的概率也考虑进去了（outlier 概率小）。所以最终我们希望最小化$E({\hat{e}}_k^2)$， 而$E({\hat{e}}_k^2)=trace(E({\hat{e}}_k\ast {\hat{e}}_k^T))=trace({\hat{P}}_k)$, 也就是要最小化后验协方差${\hat{P}}_k$的trace

$$\begin{array}{rl}{\hat{P}}_k& =cov({\hat{e}}_k)=cov(x_k-{\hat{x}}_k)=cov(x_k-({\check{x}}_k+K_k(z_k-H_k{\check{x}}_k)))\\ & =cov(x_k-({\check{x}}_k+K_k(H_k{x}_k+v_k-H_k{\check{x}}_k)))\\ & =cov((I-K_k{H}_k)(x_k-{\check{x}}_k)-K_k{v}_k)\\ & =(I-K_k{H}_k){\check{P}}_k(I-K_k{H}_k{)}^T+K_k{R}_k{K}_k^T\\ & ={\check{P}}_k-K_k{H}_k{\check{P}}_k-{\check{P}}_k{H}_k^T{K}_k^T+K_k(H_k{\check{P}}_k{H}_k^T+R_k)K_k^T\end{array}$$

由于$\frac{d(tr(BAC))}{dA}=B^T{C}^T$, 所以
$$\begin{array}{r}\frac{d(tr({\hat{P}}_k))}{d{K}_k}=-2(H_k{\check{P}}_k{)}^T+2K_k(H_k{\check{P}}_k{H}_k^T+R_k)\end{array}$$

令其导数为0， 就得到$K_k$的推导， 注意${\check{P}}_k$是对称矩阵， 所以转置也是自己

$K$的直观理解

直观但不严谨的表示$K_k=({\hat{x}}_k-{\check{x}}_k)/(z_k-H_k{\check{x}}_k)$
$({\hat{x}}_k-{\check{x}}_k)$可以理解为predicted error
$(z_k-H_k{\check{x}}_k)=(z_k-H_k{\hat{x}}_k)+(H_k{\hat{x}}_k-H_k{\check{x}}_k)$,其中 $(z_k-H_k{\hat{x}}_k)$ 可以理解为measurement error, $(H_k{\hat{x}}_k-H_k{\check{x}}_k)$为predicted error

然后更新后验
${\hat{\mathbf{x}}}_{\mathbf{k}}={\check{\mathbf{x}}}_{\mathbf{k}}+{\mathbf{K}}_{\mathbf{k}}({\mathbf{z}}_{\mathbf{k}}-{\mathbf{H}}_{\mathbf{k}}{\check{\mathbf{x}}}_{\mathbf{k}})$
${\hat{\mathbf{P}}}_{\mathbf{k}}=(1-{\mathbf{K}}_{\mathbf{k}}{\mathbf{H}}_{\mathbf{k}}){\check{\mathbf{P}}}_{\mathbf{k}}$

第二个式， 对第一个式两边求cov即可得

https://zhuanlan.zhihu.com/p/48876718

https://zhuanlan.zhihu.com/p/134595781

https://www.youtube.com/watch?v=LioOvUZ1MiM