卡尔曼滤波（二）——三种数学思想

最新推荐文章于 2024-04-30 15:33:40 发布

Patrick_zl

最新推荐文章于 2024-04-30 15:33:40 发布

阅读量563

点赞数

分类专栏：卡尔曼滤波文章标签：线性代数矩阵卡尔曼滤波算法

本文链接：https://blog.csdn.net/patrick_zl/article/details/121276887

版权

卡尔曼滤波专栏收录该内容

2 篇文章 0 订阅

订阅专栏

本文为卡尔曼滤波个人学习文档，主要资料来源B站UP主DR_CAN相关视频（【卡尔曼滤波器】2_数学基础_数据融合_协方差矩阵_状态空间方程_观测器问题_哔哩哔哩_bilibili），欢迎各路大佬交流指正。

本节并没有直接讨论卡尔曼滤波，而是为卡尔曼滤波器的推导进行一些有关于数学基础的铺垫，包括以下三个方面：

1、数据融合（Data Fusion）

2、协方差矩阵（Covariance Matrix）

3、状态空间方程以及观测器（State Space, Observation）

1、数据融合（Data Fusion）

数据融合指利用对以同一目标为对象的多个来源的数据进行整合，提取有用信息的一系列方法，这里只讲比较简单的数据融合方法。

假设有两个传感器，其测量数值符合正态分布（Normal distribution, 也成为Gaussion distribution），可将其表达为：

$Z_{1}\sim N(\mu _{1},\sigma ^{2}_{1})$ ， $Z_{2}\sim N(\mu _{2},\sigma ^{2}_{2})$

因为是两个不同的传感器，因此其测量出的数据相对独立。结合上一届的卡尔曼滤波思想，对于在条件仅限于这两个传感器的情况下，我们可将最后的测量结果表达为：

$\hat{Z}=Z_{1}+K(Z_{2}-Z_{1})$

可看出，在 $K=0$ 时，有 $\hat{Z}=Z_{1}$ ；在 $K=1$ 时，有 $\hat{Z}=Z_{2}$ 。因此我们可通过调节 $K$ 的取值来实现对两个传感器置信程度的控制。根据上式可看出 $\hat{Z}$ 依旧符合正态分布：

$\hat{Z}\sim N(\hat{\mu },\hat{\sigma }^{2})$

在实际的应用中，我们总希望最后的估计值较为接近真实值。如下图所示在方差越小，估计值落在真实值附近的可能性越大。因此我们需要寻找 $K$ 值，使 $\hat{\sigma}^{2}=Var(\hat{Z})$ 最小。

根据以上公式，我们可以推出：

$\hat{\sigma}^{2}=Var(\hat{Z})=Var(Z_{1}+K(Z_{2}-Z_{1}))$

$=Var(Z_{1}+KZ_{2}-KZ_{1})=Var((1-K)Z_{1}+KZ_{2})$

由 $Z_{1}$ ， $Z_{2}$ 独立得：

$\hat{\sigma}^{2}=Var((1-K)Z_{1})+Var(KZ_{2})=(1-K)^{2}Var(Z_{1})+K^{2}Var(Z_{2})$

最后得：

$\hat{\sigma}^{2}=(1-K)^{2}\sigma_{1}^{2}+K^{2}\sigma_{2}^{2}$

由上式可知当 $\frac{d\hat{\sigma}^{2}}{dK}=0$ 时， $\hat{\sigma}^{2}$ 最小，因此有：

$\frac{d\hat{\sigma}^{2}}{dK}=2(K-1)\sigma^{2}_{1}+2K\sigma^{2}_{2}=0$

最后解得：

$K=\frac{\sigma^{2}_{1}}{\sigma^{2}_{1}+\sigma^{2}_{2}}$

表示当 $K$ 取此值时，估计值 $\hat{Z}$ 最小。至此可以看到卡尔曼增益的雏形了。

2、协方差矩阵（Covariance Matrix）

协方差描述的是两个变量之间的相关性，对于两个变量 $x$ ， $y$ 可将二者的协方差表达为：

$\sigma_{x}\sigma_{y}=\sigma_{y}\sigma_{x}=\frac{1}{n}\sum_{i=1}^{n}(x_{i}-\bar{x})(y_{i}-\bar{y})$

上式可理解为当 $x_{i}$ 高于平均值时，如果相应的每个 $y_{i}$ 也高于平均值，则 $\sigma_{x}\sigma_{y}>0$ ，表示二者成正相关；反之当 $x_{i}$ 高于平均值时，相应的每个 $y_{i}$ 低于平均值，则 $\sigma_{x}\sigma_{y}<0$ ,表示二者负相关。

并且我们将变量 $x$ 的方差表示为 $\sigma^{2}_{x}$ 。因此规定对于两个变量 $x$ ， $y$ 可将二者的协方差表达为：

$P=\begin{bmatrix} \sigma^{2}_{x} & \sigma_{x}\sigma_{y}\\ \sigma_{y}\sigma_{x} & \sigma^{2}_{y} \end{bmatrix}$

对于三个变量 $x$ ， $y$ ， $z$ 的协方差矩阵则表示为：

$P=\begin{bmatrix} \sigma^{2}_{x} & \sigma_{x}\sigma_{y} & \sigma_{x}\sigma_{z}\\ \sigma_{y}\sigma_{x} & \sigma^{2}_{y} & \sigma_{y}\sigma_{z}\\ \sigma_{z}\sigma_{x} & \sigma_{z}\sigma_{y} & \sigma_{z}^{2} \end{bmatrix}$

对于任意变量的协方差矩阵也可以此类推。

对于协方差矩阵的求取可用如下方法（以求取拥有 $n$ 个样本的三个变量 $x$ ， $y$ ， $z$ 的协方差矩阵为例）：

$Step1$ ：设样本矩阵 $s$ ：

$s= \begin{bmatrix} x_{1}& y_{1}&z_{1}\\ x_{2}& y_{2}&z_{2}\\ ...&...&...\\ x_{n}& y_{n}&z_{n} \end{bmatrix}$

$Step2$ ：求过度矩阵 $t$ （注意式中第二项为元素全为1的方阵，而不是单位阵）：

$t=s-\frac{1}{n} \begin{bmatrix} 1&1&...&1\\ 1&1&...&1\\ ...&...&...&...\\1&1&...&1 \end{bmatrix}_{n\times n}s$

$Step3$ ：最后求协方差矩阵 $P$ ：

$P=\frac{1}{n}t^{T}t$

3、状态空间方程以及观测器（State Space, Observation）

现代控制理论中，除了有传统的输入输出作为描述系统的概念意外，还引入状态变量这一概念标识系统的状态。由输入、输出和状态标量组成，描述系统变化的方程称为状态空间方程（也叫状态空间表达式）。状态空间方程由状态方程和输出方程组成。对于离散系统和连续系统，状态空间方程有着各自的表达方法。由于卡尔曼滤波较多适用于离散系统，所以本节主要讲解离散系统的状态空间方程。

假设地面上有一辆车向前行驶，因为人类都是控制油门前进的，因此将加速度来设为输入量 $u$ 对这辆车进行控制。我们可以把这辆车的位移 $l$ ，速度 $v$ 和加速度 $a$ 设置为状态变量，则由其三者组成的状态空间向量为：

$X= \begin{bmatrix} l&v&a \end{bmatrix}^{T}$

每隔一段时间 $t$ ，空中的卫星就会对小车的速度 $l$ 和加速度 $a$ 进行一次测量，对于 $K$ 时刻来说，本时刻的状态变量：位移 $l_{K}$ ，速度 $v_{K}$ 和加速度 $a_{K}$ 与上一时刻的状态变量有如下关系：

$l_{K}=l_{K-1}+tv_{k-1}+t^{2}a_{k-1}$

$v_{K}=v_{k-1}+ta_{k-1}$

$a_{K}=u_{K-1}$

因此我们可将上述公式总结成状态转移方程和测量：

$X_{K}=AX_{K-1}+Bu_{K-1}$ $X_{K}=AX_{K-1}+Bu_{K-1}+w_{K-1}$

$Z_{K}=HX_{K}$

其中：

$A=\begin{bmatrix} 1&t&t^{2}\\ 0&1&t\\ 0&0&0 \end{bmatrix}$ ， $B=\begin{bmatrix} 0\\0\\1\end{bmatrix}$ ， $H=\begin{bmatrix} 0&1&0\\0&0&1 \end{bmatrix}$

上式组成离散系统的状态空间方程。

对于上式来说有两点值得讨论：

1、在测量和计算的过程中都会有各种各样的误差。比如状态转移方程中是假设汽车在采样时间 $t$ 中一直以加速度 $u_{K-1}$ 在运动，但实际可能不是这样的；又或者对于卫星传感器来说测得的观测值 $Z_{K}$ 可能带有一定的误差。因此我们以 $w_{K-1}$ 表示过程噪音加在状态转移方程的后面，以 $v_{K}$ 代表测量噪音加在测量方程后面：

$X_{K}=AX_{K-1}+Bu_{K-1}+w_{K-1}$

$Z_{K}=HX_{K}+v_{K}$

后面的卡尔曼滤波算法会对上式的两个噪音进行处理，达到滤波的效果。

2、对于状态空间方程两个式子中的两个 $X_{K}$ 的取值是有冲突的：状态转移方程中的 $X_{K}$ 是我们根据系统计算出来的；而测量方程中的 $X_{K}$ 其实属于未知的，因为在测量方程中我们先得到的是带有噪音的测量值 $Z_{K}$ 。因此对于连个 $X_{K}$ 的置信问题就需要我们用到第一节提到的数据融合思想了。这也是卡尔曼滤波解决的问题。

具体的卡尔曼滤波的推导将在下一节中给出。感谢阅读，学海无涯，希望你我共同进步。

Patrick_zl

关注

0
点赞
踩
3

收藏

觉得还不错? 一键收藏
0
评论
卡尔曼滤波（二）——三种数学思想

本文为卡尔曼滤波个人学习文档，主要资料来源B站UP主DR_CAN相关视频（【卡尔曼滤波器】2_数学基础_数据融合_协方差矩阵_状态空间方程_观测器问题_哔哩哔哩_bilibili），欢迎各路大佬交流指正。本节并没有直接讨论卡尔曼滤波，而是为卡尔曼滤波器的推导进行一些有关于数学基础的铺垫，包括以下三个方面：1、数据融合（Data Fusion）2、协方差矩阵（Covariance Matrix）3、状态空间方程以及观测器（）...
复制链接

扫一扫