卡尔曼滤波

感谢大佬：421施公队

一、适用范围：

线性高斯系统
线性： 叠加性、齐次性

• 叠加性： $a\ast f(t)+b\ast g(t)=a\ast F(t)+b\ast G(t)$小写对应输入，大写对应输出。或者说：对于同一个系统，输入（a+b）的输出 等于 分别输入a和b的输出的和。
• 齐次性：$f(k\ast x)=k\ast f(x)$，即输入增大k倍，输出增大k倍。

高斯： 噪声满足高斯（正态）分布

宏观意义： 滤波即加权。理想状态中，信号权重为1，噪声为0，此即为低通滤波。卡尔曼滤波有两个值：估计值，观测值，两者融合即得修正值。

二、进阶

1.状态方程表达式

状态方程： $X_k=A{X}_{k-1}+B\ast u_k+w_k$
$X_k$：当前状态的当前值
$X_{k-1}$：上一时刻状态的值
$u_k$：当前输入
$w_k$：过程噪声，也可以视为误差修正
$A$：上一时刻与当前状态的状态转移矩阵
$B$：输入控制矩阵

观测方差：$y_k=C{X}_k+V_k$
$y_k$：观测量
$X_k$：当前状态的当前值
$V_k$：观测噪声，与观测器误差有关
$C$：观测系数

2. 高斯分布

1） 参数分析：
两个噪声：
$w_k\in N(0,Q_k)$，$V_k\in N(0,R_k)$，其中0为均值，亦即数学期望。
vk是测量误差，wk是系统受到的实际扰动
2）超参
Q、R

带箭头是估计值，没箭头是观测值。没有横杠是最优估计也叫做修正值/后验估计值，有横杠是先验估计值。纵轴是概率值，所以重合部分的概率会变高变大。

一句话总结：当前的最有估计${\hat{x}}_k$是由先验估计${\hat{x}}_k^{-}$和当前观测值$y_k$取公有部分得到最优估计值，其中，先验估计${\hat{x}}_k^{-}$为基于${\hat{x}}_{k-1}$估计出的当前估计值，$y_k$为当前时刻的观测估计值。

三、放弃

1.卡尔曼公式理解

宏观过程： 使用上一次的最优结果预测当前值（即先验估计），同时用观测值修正当前值（即修正预测值），得到最优结果。
公式如下：

---

1） ${\hat{x}}_t^{-}=F{\hat{x}}_{t-1}+B{u}_{t-1}$

基于之前的最优估计来推出当前的先验估计
${\hat{x}}_t^{-}$：当前的先验估计
${\hat{x}}_{t-1}$：前一步的最优估计
$F$：状态转移矩阵
$B$：控制输入的控制矩阵
$u_{t-1}$：输入

---

2）$P_t^{-}=F{P}_{t-1}{F}^T+Q$

先验估计协方差
针对第一步的先验估计，求出对应的协方差矩阵$P_t^{-}$
$F$：状态转移矩阵
$Q$：系统的运动噪声的方差
$P_{t-1}$:后验估计协方差，前一步的步骤6得到的

---

3） 测量：$Z_t=H{x}_t+v$

$Z_t$：当前的测量值，其维度不一定与${\hat{x}}_t$相同
$x_t$：当前的观测值
$H$：观测矩阵
$v$：观测噪声

---

4)更新卡尔曼增益（与Q、R有关）

$K_t=\frac{P_t^{-}{H}^T}{H{P}_t^{-}{H}^T+R}$
卡尔曼增益就是过程方差和观测方差的比例
$K_t$：卡尔曼增益
$P_t^{-}$：协方差矩阵，即第二步结果
$H$：观测矩阵
$R$：观测噪声的方差

所以可以理解成所以调节时一般都是调节Q和R这两个超参数

5）修正估计(最终滤波结果)

${\hat{x}}_t={\hat{x}}_t^{-}+K_t(z_t-H{\hat{x}}_t^{-})$
${\hat{x}}_t$: 卡尔曼最终结果
${\hat{x}}_t^{-}$：先验估计值，即第一步结果。
$K_t$：卡尔曼增益，即第四步结果
$z_t$：测量值，即第三步结果。
$H$：观测矩阵

6）更新后验估计协方差

$P_t=(I-K^{t}H)P_t^{-}$
即步骤2的$P_{t-1}$
$P_t$：后验估计协方差
$H$：观测矩阵
$K_t$：卡尔曼增益，即第四步结果
$P_t^{-}$：协方差矩阵，即第二步结果
$I$：单位矩阵

2.调节超参数

1)Q与R的取值

例如： 因为实际中不同传感器精度不同昂贵的高精度传感器就可以更信任一些Ｒ可以小一些。 或者我建立的模型很优秀误差极小就可以更信任模型Ｑ可以小一些

2）$P_0、\widehat{x_0}$的取值

习惯上$\widehat{x_0}$q取0，
P往小的取，一般为1，不可为0。
随着迭代次数的增加会快速收敛，所以初始值影响不大。

3.卡尔曼滤波的使用

1） 选择状态量、观测量，
2）构建方差，$设定F、H$
3）初始化参数，$P_0、\widehat{x_0}，Q，R$
4）带入公式迭代
5）调节超参，$Q，R$

四、精通

1.卡尔曼公式回顾

2.