卡尔曼滤波器公式

最新推荐文章于 2023-05-29 14:18:51 发布

澄CKAI

最新推荐文章于 2023-05-29 14:18:51 发布

阅读量237

点赞数

分类专栏：卡尔曼文章标签： matlab

版权声明：本文为博主原创文章，遵循 CC 4.0 BY-SA 版权协议，转载请附上原文出处链接和本声明。

本文链接：https://blog.csdn.net/CK__CSDN/article/details/127692780

版权

卡尔曼专栏收录该内容

2 篇文章 0 订阅

订阅专栏

1系统方程：

状态方程： $x_k=Ax_{k-1}+Bu_k+w_k$

测量方程： $y_k=Hx_{k-1}+v_k$

注： $w_k$ 为协方差为 $Q$ 的噪声； $v_k$ 为协方差为 $\theta$ 的噪声；

2最优协方差

已知若有两正态分布:

正态分布 $N_1$ ：均值： $u_1$ 方差： $\sigma_1^2$

$N_1=\frac{1}{\sigma_1\sqrt{2\pi }}e^-{\frac{(x-u_1)^2}{2\sigma_1^2}}$

正态分布 $N_2$ ：均值： $u_2$ 方差： $\sigma_2^2$

$N_2=\frac{1}{\sigma_2\sqrt{2\pi }}e^-{\frac{(x-u_2)^2}{2\sigma_2^2}}$

两种相乘得到正态分布N：均值： $u$ 方差： $\sigma^2$

$N=N_1\times N_2=\frac{1}{\sigma\sqrt{2\pi }}e^-{\frac{(x-u)^2}{2\sigma^2}}$

有：

$u=u_1-\frac{\sigma _1^2(u_1-u_0)}{\sigma _1^2+\sigma _2^2}$

$\sigma ^2=\sigma_1 ^2-\frac{\sigma_1 ^4}{\sigma_1 ^2+\sigma_2 ^2}$

3卡尔曼滤波

(1)预测：

状态预测： $\bg_green x_k^{'}=Ax_{k-1}+Bu_k$ $x_k^{'}$ 协方差： $\bg_green P_k^{'}=AP_{k-1}A^T+Q$

输出预测： $y_k^{'}=Hx_k^{'}$ $y_k^{'}$ 协方差： $R_k^{'}=HP_k^{'}H^T$

(2)测量：

输出测量： $y_k^{''}=Hx_k^{''}=z_k$ $y_k^{''}$ 协方差： $R_k^{''}=HP_k^{''}H^T=\theta _k$

(3)更新：

$y_k^{'}$ 可以认为是均值为 $y_k^{'}$ ，协方差为 $R_k^{'}$ 的正态分布，

$y_k^{''}$ 可以认为是均值为 $y_k^{''}$ ，协方差为 $R_k^{''}$ 的正态分布，

利用两者可以得到：均值为 $y_k$ ，协方差为 $R_k$ 的正态分布，

① 求 $y_k$ ，进而求得 $x_k$

$y_k=y_k^{'}+\frac{R_k^{'}}{R_k^{'}+R_k^{''}}(y_k^{''}-y_k^{'})$

$Hx_k=Hx_k^{'}+\frac{HP_k^{'}H^T}{HP_k^{'}H^T+\theta}(z_k-Hx_k^{'})$

$x_k=x_k^{'}+\frac{P_k^{'}H^T}{HP_k^{'}H^T+\theta}(z_k-Hx_k^{'})$

② 求 $R_k$ ，进而求得 $P_k$

$R_k=R_k^{'}-\frac{R_k^{'}}{R_k^{'}+R_k^{''}}R_k^{'}$

$HP_kH^T=HP_k^{'}H^T-\frac{HP_k^{'}H^T}{HP_k^{'}H^T+\theta }HP_k^{'}H^T$

$P_k=P_k^{'}-\frac{P_k^{'}H^T}{HP_k^{'}H^T+\theta }HP_k^{'}$

③令：

$\bg_green K_k=P_k^{'}H^T(HP_k^{'}H^T+\theta )^{-1}$

则有：

$\bg_green x_k=x_k^{'}+K_k(z_k-Hx_k^{'})$

$\bg_green P_k=P_k^{'}-K_kHP_k^{'}$

4卡尔曼滤波的五条公式

预测：

$x_k^{'}=Ax_{k-1}+Bu_k$

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

更新：

$K_k=P_k^{'}H^T(HP_k^{'}H^T+\theta )^{-1}$

$x_k=x_k^{'}+K_k(z_k-Hx_k^{'})$

$P_k=P_k^{'}-K_kHP_k^{'}$

关注

0
点赞
踩
0

收藏

觉得还不错? 一键收藏
0
评论
复制链接

分享到 QQ

分享到新浪微博

扫一扫

专栏目录

评论

被折叠的条评论为什么被折叠?

到【灌水乐园】发言

查看更多评论

添加红包

成就一亿技术人!

hope_wisdom

发出的红包

实付元

使用余额支付

点击重新获取

扫码支付

钱包余额 0

抵扣说明：

1.余额是钱包充值的虚拟货币，按照1:1的比例进行支付金额的抵扣。
2.余额无法直接购买下载，可以购买VIP、付费专栏及课程。