卡尔曼滤波算法(1)

最新推荐文章于 2024-07-12 10:45:15 发布

Starrrr_

最新推荐文章于 2024-07-12 10:45:15 发布

阅读量438

点赞数

文章标签：算法概率论线性代数

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

本文链接：https://blog.csdn.net/Starrrr_/article/details/125187780

版权

一、离散情况

1.前提假设

系统的演化方程为：

$x_k=\Phi _{k,k-1}x_{k-1}+\Gamma _{k,k-1}w_{k-1}$

$z_k=H_kx_k+v_k$

其中 $\Phi _{k,k-1}$ 为一个方阵, $w_{k-1}$ 为过程噪声， $v_k$ 为观测噪声， $z_k$ 为观测值。

噪声都是零均值的，噪声 $w_{k-1},v_{k}$ 是线性无关的：

$E[w_{k-1}]=0,R_{uw}(k,j)=Q_k\delta _k$

$E[v_k]=0,R_w(k,j)=R_k\delta _{kj}$

$R_{uw}(k,j)=0$

初始状态 $x_0$ 的期望和方差为 $E[x_0]=\bar{x_0},Var(x_0)=P_0$ .

假定 $x_0$ 和 $w_k,v_k$ 均无关，即

$R_{xw}(0,k)=E(x_0w_k)^T=0,R_{xv}(0,k)=E(x_0v_k)^T=0$

2.直观法

假设在k时刻已经获得k次测量值 $z_1,z_2,...,z_{k-1},z_k$ ,并且求 $x_h$ 在 $z_1,z_2,...,z_{k-1},z_k$ 下的期望。

运用递推的办法，假设k-1时刻已经有了，即 $x_{k-1}$ 的最优线性估计 $\hat{x}_{k-1|k-1}$ ,前面的k-1表示时间，后面的k-1表示 $\hat{x}_{k-1|k-1}$ 是 $z_1,z_2,...z_{k-1}$ 的线性函数，下面对于 $x_k$ 来进行估计，可以利用前面的模型

$\hat{x}_{k|k-1}=\Phi _{k,k-1}\hat{x}_{k-1|k-1}$ 作为 $\hat{x}_{k|k-1}$ 的预测估计。

将估计值带入观测方程中，就可以得到 $z_k$ 的一个预测值，即

$\hat{z}_{k|k-1}=H_k\hat{x}_{k|k-1}$ .

得到的预测估计与测量值 $z_k$ 有一个误差，记作 $\tilde{z}_{k|k-1}=z_k-\hat{z}_{k|k-1}=z_k-H_k\hat{x}_{k|k-1}$ .

得到k时刻状态 $x_k$ 的滤波值，即

$\hat{x}_{k|k}=\hat{x}_{k|k-1}+K_k[z_k-H_k\hat{x}_{k|k-1}]$

其中 $K_k$ 为待定的增益矩阵。

下面的问题是如何按照目标函数 $J=E[\tilde{x}_{k|k}\tilde{x}^T_{k|k}]$ 最小的要求确定最优滤波增益矩阵。

定义

$\tilde{x}_{k|k-1}=x_k-\hat{x}_{k|k-1},\tilde{x}_{k|k}=x_k-\hat{x}_{k|k}$ ,

运用前面的式子计算得到

$\tilde{x}_{k|k}=x_k-\hat{x}_{k|k}=[I-K_kH_k]\tilde{x}_{k|k-1}-K_kv_k$

注意： $\tilde{x}_{k|k-1},v_k$ 是相互独立的。

再计算期望，最后得出协方差矩阵为：

$P_{k|k}=E[\tilde{x}_{k|k}\tilde{x}^T_{k|k}]=[I-K_kH_k]P_{k|k-1}[I-K_kH_k]^T+K_kR_kK^T_k$ .

$P_{k|k-1}=E[\Phi _{k,k-1}\tilde{x}_{k-1|k-1}+\Gamma _{k,k-1}w_{k-1}][\Phi_{k,k-1}\tilde{x}_{k-1|k-1}+\Gamma _{k,k-1}w_{k-1}]^T$

$=\Phi _{k,k-1}P_{k-1|k-1} \Phi ^T_{k,k-1}+\Gamma _{k,k-1}Q_{k-1}\Gamma ^T_{k,k-1}$

至此，离散卡尔曼滤波器的所有公式的推导完成了。

3.几何推导法

正交投影：设X和Z分别具有二阶矩阵的n维和m维随机向量，如果存在一个与X同维的随机向量 $\hat{X}$ ，满足下列三个条件：

1） $\hat{X}$ 可由Z线性表示

2） $\hat{X}$ 是无偏的，即 $\hat{X}$ 的方差和X的方差相同

3）X- $\hat{X}$ 与Z正交，即 $E(X\--\hat{X})Z^T=0$ .

二、连续情况

关注

0
点赞
踩
0

收藏

觉得还不错? 一键收藏
2
评论
卡尔曼滤波算法(1)

卡尔曼滤波（1）
复制链接

扫一扫

评论 2

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

到【灌水乐园】发言

查看更多评论

添加红包

成就一亿技术人!

hope_wisdom

发出的红包

实付元

使用余额支付

点击重新获取

扫码支付

钱包余额 0

抵扣说明：

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