卡尔曼滤波

1贝叶斯滤波

1.1理论基础

（1）本质：通过贝叶斯公式对随机信号处理，从而减小不确定度（即方差）。

（2）随机过程：x1,…,xn为随机变量，但不独立。
主观概率（先验概率）（实验前），
引入外部观测（证据、信息），
得到相对客观的概率（后验概率）（实验后）。

（3）先验概率、后验概率、似然概率：
离散举例：温度测量$T实际温度、T_m温度计测量温度$
a. 先验概率分布
$$\{\begin{array}{rl}& P(T=10)=0.8表示实际温度为10的概率为0.8\\ & P(T=11)=0.2\end{array}$$
b. 温度计测量值$T_m$
c. 后验概率分布：
$P(T=10|T_m=10.3)=\frac{P(T_m=10.3|T=10)\ast P(T=10)}{P(T_m=10.3)}$
表示在温度计显示为10.3度的条件下，实际温度为10度的概率。
其中， $P(T_m=10.3|T=10)$表示在实际温度为10度的情况下温度计测量为10.3度的概率。似然概率：观测精度/传感器精度；
$P(T_m=10.3)$看作常数$\eta$。

后验=$\eta \ast 似然\ast 先验$，其中$\eta =\frac{1}{\sum (似然\ast 先验)}$。

---

连续：
$f_{X|Y}(x|y)=\frac{f_{Y|X}(y|x)\ast f_X(x)}{f_Y(y)}=\eta \ast 似然\ast 后验$

定理：若$f_X(x)$~$N({\mu }_1,{\delta }_1^2)，f_{Y|X}(y|x)$~$N({\mu }_2,{\delta }_2^2)$，
则$f_{X|Y}(x|y)$~$N(\frac{{\delta }_2^2}{{\delta }_1^2+{\delta }_2^2}\ast {\mu }_1+\frac{{\delta }_1^2}{{\delta }_1^2+{\delta }_2^2}\ast {\mu }_2,\frac{{\delta }_1^2{\delta }_2^2}{{\delta }_1^2+{\delta }_2^2}).$ (本质：实现方差降低)。

|
|

1.2贝叶斯滤波算法

X-先验，Y-观测，$Q_k$-预测噪声，$R_k$-观测噪声
（1）过程

x1,…,xn由递推得：


（2）前提假设：
$$a.\{\begin{array}{rl}& 状态方程：X_k=f(X_{k-1})+Q_k\\ & 观测方程：Y_k=h(X_k)+R_k\end{array}$$
b. $X_k、X_{k-1}、Y_k、Q_k、R_k$均为随机变量；
$X_0、Q_1...Q_k、R_1...R_k$相互独；
$X_0$ ~ $f_0(x)，Q_k$ ~ $f_{Q_k}(x)，R_k$ ~ $f_{R_k}(x)$。

（3）预测步、更新步：

|
|
||||||||||||||||||||||||||||||||||||||||||||||||||||||||||||||||||||||||||||

2卡尔曼滤波

（1）前提假设：
a. $f(X_{k-1})=F\ast X_{k-1}，h(X_k)=H\ast X_k$，F、H为常数；
b. Q~ N(0, q), R~ N(0,r)。

（2）预测步、更新步

（3）五个公式：
$$\{\begin{array}{rl}& {\mu }_k^{-}=F\ast {\mu }_{k-1}^{+}\\ & {\delta }_k^{-}=F^2\ast {\delta }_{k-1}^{+}+q\\ & 卡尔曼增益K=\frac{H\ast {\delta }_k^{-}}{H^2\ast {\delta }_k^{-}+r}\\ & {\mu }_k^{+}=K\ast (y_k-H\ast {\mu }_k^{-})+{\mu }_k^{-}\\ & {\delta }_k^{+}=(1-H\ast K)\ast {\delta }_k^{-}\end{array}$$

（4）矩阵形式：${\mu }_k\to \overrightarrow{{\mu }_k}，{\delta }_k\to \overrightarrow{{\delta }_k}$，F、H均为矩阵。
$$\{\begin{array}{rl}& \overrightarrow{{\mu }_k^{-}}=F\ast \overrightarrow{{\mu }_{k-1}^{+}}\\ & \overrightarrow{{\delta }_k^{-}}=F\ast \overrightarrow{{\delta }_{k-1}^{+}}\ast F^T+q\\ & 卡尔曼增益K=\frac{H\ast \overrightarrow{{\delta }_k^{-}}}{H\ast \overrightarrow{{\delta }_k^{-}}\ast H^T+r}\\ & \overrightarrow{{\mu }_k^{+}}=K\ast (\overrightarrow{y_k}-H\ast \overrightarrow{{\mu }_k^{-}})+\overrightarrow{{\mu }_k^{-}}\\ & \overrightarrow{{\delta }_k^{+}}=(1-H\ast K)\ast \overrightarrow{{\delta }_k^{-}}\end{array}$$

学习来源：感谢up主：忠厚老实的老王