卡尔曼滤波的具体推导过程

jrrz0828

已于 2024-08-19 14:56:45 修改

阅读量123

点赞数 3

文章标签：自动驾驶

于 2024-08-14 17:31:54 首次发布

本文链接：https://blog.csdn.net/jrrz0828/article/details/141196348

版权

1.卡尔曼滤波简介

卡尔曼滤波器通过将多个传感器或测量器的数据进行融合，利用概率论和线性系统理论对状态进行估计和预测，从而有效地解决了不确定性带来的问题。它能够根据先验知识和测量信息来递归地更新状态估计，并提供对未来状态的最优预测。

卡尔曼滤波器的应用范围非常广泛，主要是因为我们的生活和工作中存在着大量的不确定性，这些不确定性主要体现在三个方面：

现实世界中的物理系统往往很难建立完美的数学模型
系统的扰动常常是不可控的，也很难建模（这种扰动可能来自环境的不确定性，也可能是系统自身的不确定性）
传感器本身存在的误差

2.递归算法与数据融合

卡尔曼滤波器结合了递归算法和数据融合的思想，下面举一例子进行说明。

例如在一台无人机上使用传感器测量当前高度，通过多次测量取平均值得到估计的结果

方法一：常见形式，每次都做k次加法和1次乘法

${{\hat z}_{[k]}}$ 为k次测量后的估计值：

（2-1）

方法二：递归算法，记录上一次的估计值 ${{\hat z}_{[k - 1]}}$ 和k，只需2次加法和1次乘法

（2-2）

当测量次数k很大时，新的测量结果对估计值的影响就变得很小了，通过调整k的大小，将测量值与上一次的估计值融合在一起，即体现了数据融合的思想。

3.概率论基础知识

3.1期望与方差

3.1.1期望

对于连续性随机变量 $X$ ， ${f_X}(x)$ 代表其概率密度函数，对其取定积分可得到随机变量 $X$ 在某一区间内的概率。

（3-1）

随机变量 $X$ 的期望定义为

（3-2）

期望的性质有

3.1.2方差

随机变量 $X$ 的方差定义为

（3-3）

（3-4）

（3-5）

方差的性质有

3.1.3标准差

标准差是方差的算数平方根

（3-6）

3.2正态分布

正态分布也成为高斯分布，是自然界中普遍存在的一种分布方式，大部分随机过程中产生的误差都服从或接近于正态分布。

如果一个随机变量 $X$ 服从正态分布，可以表达为 $X \sim N(\mu ,{\sigma ^2})$ ，其中 $\mu = E(X)$ 代表期望， ${\sigma ^2} = Var(X)$ 代表方差，其概率密度呈一条钟形曲线，如下图所示。

其概率密度为：

（3-7）

3.3测量误差融合举例

在工程中，测量误差会被考虑为服从正态分布。基于这一思想，将多个带有测量误差的测量数据融合到一起，得到一个相对更为准确的测量值。

以无人机测量高度为例，使用气压计和超声波传感器对高度进行测量，通过将二者的测量数据进行融合，得到一个理论上的最优估计值。

对两个传感器进行定义

参数	气压计	超声波传感器
传感器误差	${e_1} = {z_a} - {z_1}$ ， ${z_a}$ 为真实值， ${z_1}$ 为测量值	${e_2} = {z_a} - {z_2}$ ， ${z_a}$ 为真实值， ${z_2}$ 为测量值
误差分布	${e_1} \sim N(0 ,\sigma _{{e_1}}^2)$	${e_2} \sim N(0 ,\sigma _{{e_2}}^2)$
期望	$E({e_1}) = 0$	$E({e_2}) = 0$
标准差	${\sigma _{{e_1}}} = 1$	${\sigma _{{e_2}}} = 0.5$

数据融合算法可以写成：

（3-8）

为求解最优的参数K，定义估计误差为

（3-9）

（3-10）

其期望为

（3-11）

估计误差的方差为

（3-12）

由于两次测量时相互独立的，因此

（3-13）

令 $\frac{{d\sigma _{\hat e}^2}}{{dK}} = 0$ ，即

（3-14）

（3-15）

将 ${\sigma _{{e_1}}} = 1$ 和 ${\sigma _{{e_2}}} = 0.5$ 代入可得

（3-16）

二阶导数为

（3-17）

因此，当 $K = {K^*}$ 时， $\sigma _{\hat e}^2$ 为最小值，最优估计误差的标准差为

（3-18）

这一标准差小于任何一个传感器单独的测量的误差的标准差（ ${\sigma _{\hat e}} < {\sigma _{{e_2}}} < {\sigma _{{e_1}}}$ ），数据融合结果如下图所示

由上述分析可知，通过数据融合得到的最优估计值位于两个测量值之间，并更接近于测量误差标准差更小的一侧（即 ${z_2}$ 一侧），其概率分布钟形曲线更为细长。

这意味着我们使用两个“不太准确”的测量值，通过数据融合得到了一个相对准确的估计值。这一思想在工程中运用十分广泛，使用多传感器融合可以有效提高精度和可靠性。

3.4协方差与协方差矩阵

在上一节中，两个传感器之间是相互独立的（即传感器之间的测量结果互不影响），因此数据融合时考虑它们各自的方差即可。

当两个信号之间存在关联时，在进行融合时就需要将其纳入考虑范围之中。

两组信号之间的关系使用协方差表示，多组信号之间的关系使用协方差矩阵表示。

考虑两个随机变量 ${X_1}$ 和 ${X_2}$ ，它们之间的协方差定义为

（3-19）

随机变量 ${X_1}$ 和 ${X_2}$ 组成的协方差矩阵可写成如下形式

（3-20）

写成向量形式

（3-21）

（3-22）

将其推广到一般形式，考虑一个有n个随机变量组成的n×1向量，且向量中的每一个随机变量都服从正态分布，即 ${X_i} \sim N({\mu _{{X_i}}},\sigma _{{X_i}}^2)$ ，则向量 $X$ 的期望和协方差矩阵分别为：

（3-23）

（3-24）

我们称这个由随机变量组成的向量服从正态分布 $X \sim N({\mu _X},C(X))$ ，特别的，当向量服从正态分布 $X \sim N(0,C(X))$ ，其协方差矩阵可以写成

（3-25）

注：上述推导过程中，请注意随机变量和向量的不同写法，向量为粗体

4.矩阵求导公式

5.线性卡尔曼滤波器推导

${x_{[k]}}$	n×1状态量
A	n×n状态矩阵
${u_{[k-1]}}$	p×1输入向量
B	n×p输入矩阵
${w_{[k-1]}}$	过程噪声，服从正态分布 $w \sim N(0,{Q_c})$
${z_{[k]}}$	n×1观测向量
${H_{m}}$	n×n观测矩阵
${v_{[k]}}$	测量噪声，服从正态分布 $v \sim N(0,{R_c})$
$\hat x_{[k]}^ -$	k时刻的先验状态估计，是根据系统模型“估计”的结果 “^”代表估计，“-”代表先验
${{\hat x}_{[k - 1]}}$	k-1时刻的后验状态估计
${{\hat x}_{me{a_{[k]}}}}$	是根据测量值“估计”的结果（由于测量噪声的存在）
${K_{[k]}}$	卡尔曼增益， ${K_{[k]}} \in [0,H_m^{ - 1}]$
$e_{[k]}^ -$	先验状态估计误差
${e_{[k]}}$	后验状态估计误差
$P_{[k]}^ -$	先验状态估计误差的协方差矩阵
${P_{[k]}}$	后验状态估计误差的协方差矩阵

5.1卡尔曼滤波器研究模型

考虑一个带有过程噪声的离散系统的状态空间方程

（5-1）

其中过程噪声 ${w_{[k-1]}}$ 服从正态分布 $w \sim N(0,{Q_c})$ ，协方差矩阵为

（5-2）

如果过程噪声是相互独立的，则 ${\sigma _{{w_i}{w_j}}} = 0$ ，即

（5-3）

定义观测向量为

（5-4）

其中测量噪声 ${v_{[k]}}$ 服从正态分布 $v \sim N(0,{R_c})$ ，协方差矩阵为

（5-5）

如果测量噪声相互独立，则 ${\sigma _{{v_i}{v_j}}} = 0$ ，即

（5-6）

分析这个系统：

由于过程噪声w的存在，无法通过式（5-1）计算得到精确的状态向量 ${x_{[k]}}$ ；同时，由于测量噪声v的存在，也无法通过式（5-4）推导出精确的状态向量 ${x_{[k]}}$ 。通过设计卡尔曼滤波器将这两个式子中的结果进行数据融合，可以得到一个相对准确的估计值。

注：上述分析中假定系统为时不变系统，时变系统暂时不在讨论范围之内

定义先验状态估计

（5-7）

引入测量估计 ${{\hat x}_{me{a_{[k]}}}}$ ，将式（5-4）写成

（5-8）

如式（5-8）所示，如果没有测量噪声，即可通过该式求得精确的状态变量。而由于测量噪声的存在，求得的 ${{\hat x}_{me{a_{[k]}}}}$ 是根据测量值“估计”的结果，可得

（5-9）

通过式（5-7）和式（5-9）从不同的渠道得到了两个估计值，其中 $\hat x_{[k]}^ -$ 是通过状态空间方程“计算”得到的估计值； ${{\hat x}_{me{a_{[k]}}}}$ 是通过传感器“测量”得到的估计值

将式（5-7）和式（5-9）融合，定义k时刻的后验状态估计 ${{\hat x}_{[k]}}$ 为

（5-10）

将式（5-9）代入（5-10）可得

（5-11）

（5-12）

式（5-12）即为卡尔曼滤波的一般表达形式，其中 ${K_{[k]}}$ 被称为卡尔曼增益， ${K_{[k]}} \in [0,H_m^{ - 1}]$ ，通过它将先验状态估计 $\hat x_{[k]}^ -$ 与测量值 ${{z}_{[k]}}$ 融合在一起得到更加准确的后验状态估计 ${{\hat x}_{[k]}}$ 。

${K_{[k]}} \to 0 \Rightarrow {{\hat x}_{[k]}} \to \hat x_{[k]}^ -$	测量噪声远大于过程噪声，系统更相信先验状态估计
${K_{[k]}} \to H_m^{ - 1} \Rightarrow {{\hat x}_{[k]}} \to H_m^{ - 1}{z_{[k]}}$	测量误差很小，系统更相信测量值