资讯

历史

科技

环境与自然

成长

游戏

财经

文学与艺术

美食

健康

家居

文化

情感

汽车

三农

军事

旅行

运动

教育

生活

星座命理

状态估计问题-卡尔曼滤波

创作时间:

作者:

@小白创作中心

状态估计问题-卡尔曼滤波

引用

CSDN

https://blog.csdn.net/weixin_49401384/article/details/136969771

卡尔曼滤波是一种广泛应用于机器人、自动驾驶和飞行控制等领域的数据滤波算法。它通过线性系统状态方程，利用系统输入观测数据，对系统状态进行最优估计，以去除噪声并还原真实数据。本文将从基础概念出发，详细推导卡尔曼滤波的五大经典公式，并从概率分布和最小化误差两个角度深入理解其原理。

卡尔曼滤波-五大经典公式

这里还是引用网上卡尔曼滤波介绍文章中经典的工作流程图。不难看出，卡尔曼滤波器工作流程是一个不断迭代的过程，单个周期内分为预测（Predict）和更新（Update）两部分。这里引入几个关于系统状态量的概念：

$x_k$：表示系统状态的真实值（通常无法直接得到）；
$\bar{x_k}$：表示系统状态的预测值（通常根据上一时刻系统状态的估计值预测得到，也叫先验状态）；
$\hat{x_k}$：表示系统状态的估计值（通常是预测值加入观测信息后修正得到，也叫后验状态）；

预测：

输入：过去的最优状态$(\hat{x}_{k-1}, \hat{P_k})$，外界对过程的影响$u_k$，环境的不确定度$Q_k$；
输出：预测的当前时刻系统状态$(\bar{x}_{k}, \bar{P_k})$；
其他：对过程的描述转换$(F_k, B_k)$，与时间有关；

$$
\bar{x_k} = F_k \hat{x}_{k-1} +B_ku_k \tag{1}
$$

$$
\bar{P_k} = F_k\hat{P}_{k-1}F_k^T+Q_k \tag{2}
$$

更新：

输入：预测的当前时刻系统状态$(\bar{x}_{k}, \bar{P_k})$，观测值的状态$(z_k, R_k)$，状态量到观测量维度的变换矩阵$H_k$；
输出：经过观测值修正后的最优估计状态$(\hat{x}_{k}, \hat{P_k})$；

$$
K = \bar{P_k}H_k^T(H_k\bar{P_k}H_k^T+R_k)^{-1}
$$

$$
\hat{x_k} = \bar{x_k} + K(z_k-H_k\bar{x_k}) \tag{4}
$$

$$
\hat{P_k} = \bar{P_k}-KH_k\bar{P_k}=(I-KH_k)\bar{P_k} \tag{5}
$$

卡尔曼滤波-推导过程

系统状态估计描述的是已知前一时刻的系统状态$x_{k-1}$和当前时刻的系统观测量$z_k$，如果获取当前时刻系统最可能的状态信息。通常需要构建状态方程和观测方程来进行系统状态的建模：

状态方程：

描述的是前一时刻的系统状态$x_{k-1}$与当前时刻系统状态$x_k$的关系；

$$
x_k = F_k x_{k-1} +B_ku_k+w_k
$$

其中：

$F_k$为状态转移矩阵，$B_ku_k$为外部输入，$w_k$为过程噪声，一般满足高斯分布$w_k\sim N(0, Q_k)$。

测量方程：

描述的是当前时刻的观测量$z_k$与当前时刻系统状态$x_k$的关系；

$$
z_k=H_kx_k+v_k
$$

其中：

$H_k$为测量矩阵，$v_k$为测量噪声，一般满足高斯分布$v_k\sim N(0, R_k)$。

我们都知道卡尔曼滤波是线性状态估计问题中最为直接的一种处理方法，那么如何理解卡尔曼滤波呢？可以从如下两个角度进行理解：一种是从概率分布的角度，一种是从最小化误差的角度。

1. 从概率分布的角度

卡尔曼滤波将状态方程中的过程噪声假设成均值为0的高斯噪声，使得系统状态向量也可以看做是一个符合高斯分布的随机向量。同时对于测量方程中的测量噪声也假设成均值为0的高斯噪声，同样通过测量方程可以将系统状态向量转换到观测域，此时观测域的状态向量同样满足高斯分布。这样就可以得到两个关于系统状态向量的高斯分布（观测域下）：

系统状态（预测值）满足$N ( H_kx_k ，H_kP_kH^{T}_k)$
系统状态（观测值）满足$N ( z_k ，R_k)$

其中预测的方差（多维称协方差）是根据系统状态$x_k$的协方差$P_k$及协方差传播定律（已知向量$x$的协方差为$\Sigma$，协方差满足$Cov(Ax)=A\Sigma A^{T}$）得到的。

概率论中提到两个高斯分布的联合概率分布依旧保持高斯特性，简单以下图为例，绿色和红色表示不同的高斯分布函数，蓝色为两个高斯分布的乘积，可以大概看出蓝色部分相较于原来的两个高斯分布，分布更为集中，形态上像是一个幅值被压缩的高斯分布。那么如果这个性质能够应用到前面提到的系统状态向量的两个高斯分布上，是不是就说明可以通过两个高斯分布的乘积来对系统状态进行更加可靠的估计呢？

两个高斯分布的乘积推导：

假设两个高斯分布满足$N_1 \sim (\mu_1,\sigma^{2}_1)$，$N_2 \sim (\mu_2,\sigma^{2}_1)$，现在需要计算两者相乘后新的概率分布$N$：

$$
f(x) =f(x_1) \cdot f(x_2) =\frac{1}{\sqrt{2\pi}\sigma_1}exp(-\frac{(x-\mu_1)^{2}}{2\sigma^{2}{1}}) \times \frac{1}{\sqrt{2\pi}\sigma_2}exp(-\frac{(x-\mu_2)^{2}}{2\sigma^{2}{2}}) = \frac{1}{2\pi \sigma_1 \sigma_2} exp(-\frac{(x-\mu_1)^{2}}{2\sigma^{2}{1}}-\frac{(x-\mu_2)^{2}}{2\sigma^{2}{2}})
$$

单独取出$f(x)$的指数部分，即

$$
\frac{(x-\mu_1)^{2}}{2\sigma^{2}{1}}+\frac{(x-\mu_2)^{2}}{2\sigma^{2}{2}} = \frac{\sigma^{2}_2(x^{2} - 2 \mu_1x +\mu^{2}_1) + \sigma^{2}_1(x^{2} - 2 \mu_2 x +\mu^{2}2)}{2\sigma^{2}{1} \sigma^{2}_2} = \frac{(\sigma^{2}_1 + \sigma^{2}2)x^{2} - 2x(\mu_1 \sigma^2_2 + \mu_2 \sigma^2_1)+(\mu^2_1 \sigma^2_2 +\mu^2_2 \sigma^2_1)}{2\sigma^{2}{1} \sigma^{2}_2} = \frac{x^2 - 2x \frac{\mu_1 \sigma^2_2 + \mu_2 \sigma^2_1}{\sigma^{2}_1 + \sigma^{2}_2} + \frac{\mu^2_1 \sigma^2_2 +\mu^2_2 \sigma^2_1}{\sigma^{2}_1 + \sigma^{2}_2}}{\frac{2\sigma^2_1\sigma^2_2}{\sigma^{2}_1 + \sigma^{2}_2}} = \frac{(x - \frac{\mu_1 \sigma^2_2 + \mu_2 \sigma^2_1}{\sigma^{2}_1 + \sigma^{2}_2})^2 + \frac{\mu^2_1 \sigma^2_2 +\mu^2_2 \sigma^2_1}{\sigma^{2}_1 + \sigma^{2}_2} - (\frac{\mu^2_1 \sigma^2_2 +\mu^2_2 \sigma^2_1}{\sigma^{2}_1 + \sigma^{2}_2})^2}{\frac{2\sigma^2_1\sigma^2_2}{\sigma^{2}_1 + \sigma^{2}_2}} = \frac{(x - \frac{\mu_1 \sigma^2_2 + \mu_2 \sigma^2_1}{\sigma^{2}_1 + \sigma^{2}_2})^2 }{\frac{2\sigma^2_1\sigma^2_2}{\sigma^{2}_1 + \sigma^{2}_2}} + \frac{ \frac{\mu^2_1 \sigma^2_2 +\mu^2_2 \sigma^2_1}{\sigma^{2}_1 + \sigma^{2}_2} - (\frac{\mu^2_1 \sigma^2_2 +\mu^2_2 \sigma^2_1}{\sigma^{2}_1 + \sigma^{2}_2})^2}{\frac{2\sigma^2_1\sigma^2_2}{\sigma^{2}_1 + \sigma^{2}_2}}
$$

前面已经变成了$\frac{(x-\mu)^{2}}{2\sigma^{2}}$的形式，其中$\mu = \frac{\mu_1 \sigma^2_2 + \mu_2 \sigma^2_1}{\sigma^{2}_1 + \sigma^{2}_2},\sigma^2 = \frac{\sigma^2_1\sigma^2_2}{\sigma^{2}_1 + \sigma^{2}_2}$。继续对（1-2）式后续的常数部分进行简化，即

$$
\frac{ \frac{\mu^2_1 \sigma^2_2 +\mu^2_2 \sigma^2_1}{\sigma^{2}_1 + \sigma^{2}_2} - (\frac{\mu^2_1 \sigma^2_2 +\mu^2_2 \sigma^2_1}{\sigma^{2}_1 + \sigma^{2}_2})^2}{\frac{2\sigma^2_1\sigma^2_2}{\sigma^{2}_1 + \sigma^{2}_2}} = \frac{(\mu^2_1 \sigma^2_2 +\mu^2_2 \sigma^2_1)(\sigma^{2}_1 + \sigma^{2}_2) -(\mu^2_1 \sigma^2_2 +\mu^2_2 \sigma^2_1)^2 }{2\sigma^2_1\sigma^2_2 (\sigma^{2}_1 + \sigma^{2}_2)} = \frac{(\mu_1 - \mu_2)^2}{2(\sigma^{2}_1 + \sigma^{2}_2)}
$$

因此，两个高斯概率密度函数的乘积可以写成

其中：

$\mu = \frac{\mu_1 \sigma^2_2 + \mu_2 \sigma^2_1}{\sigma^{2}_1 + \sigma^{2}_2} = \mu_1 +\frac{\sigma^2_1(\mu_2 - \mu_1)}{\sigma^{2}_1 + \sigma^{2}_2}$
$\sigma^2 = \frac{\sigma^2_1\sigma^2_2}{\sigma^{2}_1 + \sigma^{2}_2} = \sigma_1^2 - \frac{\sigma_1^4}{\sigma^{2}_1 + \sigma^{2}_2}$

因此两个高斯分布的乘积仍为高斯分布，且均值为$\mu$，方差为$\sigma^2$，$S_g$被称为缩放因子，表示相乘后的高斯分布函数相比于标准高斯分布是一个被压缩或者放大的高斯分布。为了方便表达，我们令$K = \frac{\sigma_1^2}{\sigma_1^2 + \sigma_2^2}$，这样两个高斯分布相乘后新的高斯分布的均值和方差可以表示为：