卡尔曼滤波笔记

最新推荐文章于 2025-03-18 22:42:19 发布

原创最新推荐文章于 2025-03-18 22:42:19 发布 · 346 阅读

0 ·

CC 4.0 BY-SA版权

学习笔记专栏收录该内容

3 篇文章

订阅专栏

本文详细介绍了卡尔曼滤波的基本原理及其应用。首先定义了线性系统的状态方程和观测方程，接着阐述了卡尔曼滤波的核心递推过程，包括预测与更新两个阶段，并给出了具体的数学表达式。

卡尔曼滤波

一.卡尔曼滤波使用

1.线性系统模型：
$\left\{ \begin{aligned} &状态方程：X(i)=FX(i-1)+BU(i-1)+W(i)\\ &观测方程： Y(i)=HX(i)+V(i) \end{aligned} \right.$
$F$ 为状态转移矩阵，B为控制矩阵，H为观测矩阵，W为输入的白噪声，且其方差为Q，V为观测噪声，且其方差为R。

2.卡尔曼滤波递推过程

核心：预测+更新

变量名称：
${状态变量先验估计值：X^(i∣i−1)=X^i−协方差变量先验估计值：P(i∣i−1)=Pi−卡尔曼增益：K(i)状态变量最优估计值：X^(i∣i)=X^i协方差变量最优估计值：P(i∣i)=Pi \left\{ \begin{aligned} &状态变量先验估计值：\hat{X}(i|i-1)=\hat{X}^-_i\\ &协方差变量先验估计值：P(i|i-1)=P^-_i\\ &卡尔曼增益：K(i)\\ &状态变量最优估计值：\hat{X}(i|i)=\hat{X}_i\\ &协方差变量最优估计值：P(i|i)=P_i \end{aligned} \right.$
(1)预测过程：利用上一次的最优结果预测当前的状态值：
${X^(i∣i−1)=FX^(i−1∣i−1)+BU(i−1)P(i∣i−1)=FP(i−1∣i−1)FT+Q \left\{ \begin{aligned} &\hat{X}(i|i-1)=F\hat{X}(i-1|i-1)+BU(i-1)\\ &P(i|i-1)=FP(i-1|i-1)F^T+Q \end{aligned} \right.$
(2)更新过程：利用观测值和预测值更新得到当前的最优状态值（利用观测值修正预测值）：
${K(i)=Pi(i∣i−1)HT(HPi(i∣i−1)HT+R)−1X^(i)=X^(i∣i−1)+K(i)(Y(i)−HX^(i∣i−1))P(i)=(I−K(i)H)P \left\{ \begin{aligned} &K(i)=P_i(i|i-1)H^T(HP_i(i|i-1)H^T+R)^{-1}\\ &\hat{X}(i)=\hat{X}(i|i-1)+K(i)(Y(i)-H\hat{X}(i|i-1))\\ &P(i)=(I-K(i)H)P \end{aligned} \right.$