卡尔曼估计两步法

养生的控制人

分类：建模仿真

发布时间 2022.03.14阅读数 3288 评论数 0

å¡å°æ¼ä¼°è®¡ä¸¤æ¥æ³

在上一篇文章中手把手推导了一遍卡尔曼增益，不熟悉的小伙伴可以看

养生的控制人：卡尔曼增益推导170 赞同 · 21 评论文章

这里再回顾一下重点。

问题重述

假设真实系统为

$x_{k+1}=Ax_{k}+Bu_{k}+w_{k}$

$y_k=Cx_k+v_k$

其中 $w\sim \mathcal{N}(0,Q),v\sim \mathcal{N}(0,R)$ 。

我们对系统状态的估计（数据融合）为

$\hat{x}_k = \hat{x}_{k}^{cal} + K_k(y_k-C\hat{x}_k^{cal})$

其中卡曼尔增益为

$K_k=\frac{\hat{P}_kC^T}{C\hat{P}_kC^T+R}$

我们可以看到卡尔曼增益中的估计协方差矩阵 $\hat{P}_k$ 还是未知的，因此我们需要把它表示出来。

估计协方差矩阵推导

根据定义 $\hat{P}_k=E[\hat{e}_k\hat{e}_k^T]=E[(x_k-\hat{x}_k^{cal})(x_k-\hat{x}_k^{cal})^T]$

带入真实系统模型和状态估计的模型 $\hat{x}_k^{cal}=A\hat{x}_{k-1}+Uu_{k-1}$

$\hat{e}_k=x_k-\hat{x}_k^{cal}=A(x_{k-1}-\hat{x}_{k-1})+w_{k-1}=Ae_{k-1}+w_{k-1}$

再带入估计协方差矩阵的表达式

$\hat{P}_{k}=E[(Ae_{k-1}+w_{k-1})(Ae_{k-1}+w_{k-1})^T]$

把转置放进去

$\hat{P}_{k}=E[(Ae_{k-1}+w_{k-1})(e_{k-1}^TA^T+w_{k-1}^T)]$

把括号打开

$\hat{P}_{k}=E[Ae_{k-1}e_{k-1}^TA^T+Ae_{k-1} w_{k-1}^T + w_{k-1}e_{k-1}^TA^T+w_{k-1}w_{k-1}^T]$

由于四项之间是线性相加的，可以把期望的运算放进去，等于每一项的期望。

注意到 $e_{k-1}=x_{k-1}-\hat{x}_{k-1}$ ，而 $w_{k-1}$ 是作用到 $x_k$ 上的，所以

$e_{k-1}$ 和 $w_{k-1}$ 是相互独立的。因为相互独立，所以相乘的期望等于期望的相乘

$E[e_{k-1}w_{k-1}^T]=E[e_{k-1}]E[w_{k-1}^T]=0$

因此估计的协方差矩阵剩下两项

$\hat{P}_k=AE[e_{k-1}e_{k-1}^T]A^T+E[w_{k-1}w_{k-1}^T]=AP_{k-1}A^T+Q$

有了这个表达式后，我们就可以用卡尔曼滤波器来估计状态了！

卡尔曼滤波器两步法

目前为止卡尔曼增益中各项都是已知的了，因此可以用于估计了，卡尔曼滤波分为两个步骤：预测和校正。

Step1. 预测

$\hat{x}_{k}^{cal}=Ax_{k-1}+Bu_{k-1}$ （1）利用系统模型对状态变量进行递推估计

$\hat{P}_k=AP_{k-1}A^T+Q$ （2）对协方差矩阵进行估计

Step2. 校正

$K_k=\frac{\hat{P}_kC^T}{C\hat{P}_kC^T+R}$ （3）计算卡尔曼增益

$\hat{x}_k = \hat{x}_{k}^{cal} + K_k(y_k-C\hat{x}_k^{cal})$ （4）数据融合即对状态的估计

$P_k=(I-K_kC)\hat{P}_k$ （5）更新协方差矩阵用于下一次的预测

至此，卡尔曼滤波（估计器）就推导完毕了，给定初值 $x_0,P_0$ ，它就能递推估计了。记住核心思想无非是：数据融合、预测校正。

公式（5）的推导

在上一篇文章中我们推导得到协方差矩阵的表达式为

$P_k=\hat{P}_k-K_kC\hat{P}_k-\hat{P}_kC^TK_k^T+K_kC\hat{P}_kC^TK_k^T+K_kRK_k^T$

合并同类项

$P_k=\hat{P}_k-K_kC\hat{P}_k-\hat{P}_kC^TK_k^T+K_k(C\hat{P}_kC^T+R)K_k^T$

带入卡尔曼增益

$P_k=\hat{P}_k-K_kC\hat{P}_k-\hat{P}_kC^TK_k^T+\frac{\hat{P}_kC^T}{C\hat{P}_kC^T+R}(C\hat{P}_kC^T+R)K_k^T$

$P_k=\hat{P}_k-K_kC\hat{P}_k-\hat{P}_kC^TK_k^T+\hat{P}_kC^TK_k^T$

$P_k=\hat{P}_k-K_kC\hat{P}_k=(I-K_kC)\hat{P}_k$

建模仿真卡尔曼滤波卡尔曼估算

转载原出处：https://zhuanlan.zhihu.com/p/166253882

打赏 0

点赞 0

收藏 0

分享

上一篇：卡尔曼增益推导

下一篇：机器学习与优化基础（Machine Learning and Optimization）

为你推荐

给作者打赏

您当前积分：0

想获取更多信息和操作，请移步电脑网页版