卡尔曼滤波

注意

该笔记仅在旧笔记的基础上进行归类, 尚未对格式进行修复

有关教程

用途

1. 从存在噪音/漂移的多个测量值中得到准确值
2. 从间接测量量得到目标测量量

模型

1. 输入变量 $u$ 如速度/力
2. 输出变量 $y$ 如传感器读数
3. 估计状态 $x$ 如实际位置/速度
4. 误差 $e_{obs}=x-\hat{x}$
5. 比例矩阵 $C$ 反应 $x$ 与 $y$ 之间的关系
   1. 如果 $x$ 与 $y$ 为单变量, 则 $C$ 为系数
   2. 如果 $x$ 与 $y$ 为同一变量, 则 $C=1$
6. 当变量 $x$ 上有标记 $\hat{x}$, 表明 $x$ 为一个估计状态

误差方程

$$\frac{dx}{dt}=Ax+Bu$$

$$y=Cx$$

$$\frac{d\hat{x}}{dt}=A\hat{x}+Bu+K(y-\hat{y})$$

$$\hat{y}=C\hat{x}$$

整理得到

$$e_{obs}=e^{(A-CK)t}e_{obs}(0)$$

通过选择合理的 $k$, 可以使误差最终收敛于 0

实际误差

x_k=Ax_{k-1}+Bu_k+w_k$$ 其中有误差 $w_k\sim N(0,Q)$ 2. $$y_k=Cx_k+v_k$$ 其中有误差 $v_k\sim N(0,R)$ 3. 使用概率密度的方式表示 $\hat{x_k}$ 与 $y_k$, 得到如图  结合 $\hat{x_k^-}$ 与 $y_k$, 可以得到一个方差更小, 更准确的 $\hat{x_k}$ ## 计算过程 ### 预测部分 1. $\hat{x_k^-}$ 表示变量 $x$ 的预估计值 2. $P_k^-$ 表示预估计值的方差 3. $$\hat{x_k^-}=A\hat{x_{k-1}}+Bu_k

$$P_k^-=AP_{k-1}A^T+Q$$

更新部分

卡尔曼修正值

$$K_k=\frac{P_k^-C^T}{CP_k^-C^T+R}$$

通过比较 $\hat{x_k^-}$ 与 $y_k$ 的方差计算两者对于准确估计值的权重, 称为卡尔曼修正值 $K$

$$\hat{x_k}=\hat{x_k^-}+K_k(y_k-C\hat{x_k^-})$$

$$P_k=(I-K_kC)P_k^-$$

一维卡尔曼滤波示例

1. 一维下没有矩阵, 则将转置运算视为倒数, 单位阵 $I$ 视为 1
2. 测量量即估计量, 取 $C=1$
3. 希望估计量保持稳定, 取 $A=1$
   • 用于温度, 气压等变化缓慢的量
   • 对于复杂运动关系需要查找对应的 $A$
4. 参数 R 表示观测值的方差, 应尽可能准确, 可根据传感器的误差统计
   • 增加Q, 增益增加, 即观测值在状态更新方程中的权重变大, 滤波器更加灵敏, 反之亦然
   • 增加R, 增益减小, 即观测值在状态更新方程中的权重变小, 滤波器反应迟钝, 反之亦然
5. 参数 Q 的取值在 R/9 - R/4 较合适
6. 一般不考虑 $B$ 与 $u$

参考资料

1. Kalman滤波器参数如何选取
2. 卡尔曼滤波的理解以及参数调整