卡尔曼滤波器详解（二）：数学基础

最新推荐文章于 2023-09-25 20:46:07 发布

@清欢_

最新推荐文章于 2023-09-25 20:46:07 发布

阅读量2.6k

点赞数 8

分类专栏： VSLAM 文章标签：算法协方差统计学

本文链接：https://blog.csdn.net/weixin_46581517/article/details/107696653

版权

本文深入探讨了卡尔曼滤波器的基础数学概念，包括数据融合的原理，如何通过卡尔曼增益找到最优估计，并详细介绍了协方差矩阵的定义及其在统计学中的应用。此外，还讲解了状态空间方程在二阶控制系统和二维旋转中的应用，阐述了噪声影响下的状态更新与误差处理。

摘要由CSDN通过智能技术生成

文章目录

卡尔曼滤波器：数学基础

文章讨论三个方面：数据融合、协方差矩阵、状态空间方程。

卡尔曼滤波器：数学基础

一、数据融合

假设存在两次测量，两次测量均存在误差且误差服从正态分布。
第一次测量的测量值为 $z_1$ ，标准差为 $\sigma_1$ ；第二次测量的测量值为 $z_2$ ，标准差为 $\sigma_2$ 。

1. 结论

估计真实值在两次测量值之间，且更接近标准差小的测量值。

2. 推导

已知（详见上期公式（5））：
$\hat{z} = z_1 + K(z_2 - z_1) \tag{1}$
式中： $K$ ——卡尔曼增益/因数，且 $K\in[0,1]$ 。

$K = 0$ 时， $\hat{z} = z_1$ ； $K = 1$ 时， $\hat{z} = z_2$ 。

我们的目的是求 $K$ 使得 $\sigma_{\hat{z}}$ 最小，即求 $K$ 使得方差 $Var(\hat{z})$ 最小。
$\sigma_{\hat{z}}^2 = \bm{V}ar(z_1 + K(z_2 - z_1)) \\ \ \\ \quad \ \ = \bm{V}ar(z_1 - Kz_1 + Kz_2) \\ \ \\ \quad \ \ \ = \bm{V}ar((1-K)z_1 + Kz_2)$
两次测量结果相互独立，可得：
$\sigma_{\hat{z}}^2 = \bm{V}ar((1-K)z_1 + Kz_2) \\ \ \\ \qquad \qquad = \bm{V}ar((1-K)z_1) + \bm{V}ar(Kz_2) \\ \ \\ \qquad \qquad \quad \ \ = (1-K)^2\bm{V}ar(z_1)+K^2\bm{V}ar(z_2)\ \ \ \\ \ \\ = (1-K)^2\sigma_1^2 + K^2\sigma_2^2 \ \ \tag{2}$
当方差最小时，方差 $\sigma_{\hat{z}}^2$ 对 $K$ 的导数为零，即：
$\frac{\partial{\sigma_{\hat{z}}^2}}{\partial{K}} = 0 \Rightarrow -2(1-K)\sigma_1^2 + 2K\sigma_2^2=0 \\ \ \\ \qquad \ \ \Rightarrow -\sigma_1 + K\sigma_1^2 + K\sigma_2^2 = 0 \\ \ \\ \Rightarrow K(\sigma_1^2 + \sigma_2^2) = \sigma_1^2 \ \\ \ \\ \Rightarrow K = \frac{\sigma_1^2}{\sigma_1^2 + \sigma_2^2} \quad \ \ \ \ \tag{3}$
将式（3）带入式（1）中，可得：