卡尔曼滤波简介

最新推荐文章于 2024-06-16 23:54:11 发布

Young_Gy

最新推荐文章于 2024-06-16 23:54:11 发布

阅读量1.3w

点赞数 56

文章标签：卡尔曼滤波状态估计

本文链接：https://blog.csdn.net/young_gy/article/details/78177291

版权

卡尔曼滤波是一种在不确定状况下组合多源信息得到所需状态最优估计的一种方法。本文将简要介绍卡尔曼滤波的原理及推导。

这里写图片描述

什么是卡尔曼滤波

首先定义问题：对于某一系统，知道当前状态 $X_t$ ，存在以下两个问题：

经过时间 $\triangle t$ 后，下个状态 $X_{t+1}$ 如何求出？
假定已求出 $X_{t+1}$ ，在 $t+1$ 时刻收到传感器的非直接信息 $Z_{t+1}$ ，如何对状态 $X_{t+1}$ 进行更正？

这两个问题正是卡尔曼滤波要解决的问题，形式化两个问题如下：

预测未来
修正当下

下面，将以机器人导航为例，从预测未来和修正当下两个角度介绍卡尔曼滤波器。

卡尔曼滤波的原理

问题场景如下：一个机器人，我们想知道它实时的状态 $\vec{x}$ ，同时也想做到预测未来和修正当下这两件事。

其状态 $x$ 表示为一维大小为2的向量，元素分别表示位置信息与速度信息：

x ⃗ = [p v]

$\vec{x} = \begin{bmatrix} p\\ v \end{bmatrix}$

可是状态 $x$ 不一定是精准的，其不确定性用协方差表示：

P k = [Σ p p Σ v p Σ p v Σ v v]

$P_k = \begin{bmatrix} \Sigma_{pp} & \Sigma_{pv} \\ \Sigma_{vp} & \Sigma_{vv} \\ \end{bmatrix}$

预测未来

只考虑自身状态

只考虑自身状态的情况下，根据物理公式，可得：

p k v k = p k - 1 + Δ t = v k - 1 v k - 1

$\begin{split} \color{deeppink}{p_k} &= \color{royalblue}{p_{k-1}} + \Delta t &\color{royalblue}{v_{k-1}} \\ \color{deeppink}{v_k} &= &\color{royalblue}{v_{k-1}} \end{split}$

用矩阵表示如下：

x ̂ k = [10 Δ t 1] x ̂ k - 1 = F k x ̂ k - 1

$\begin{align} \color{deeppink}{\mathbf{\hat{x}}_k} &= \begin{bmatrix} 1 & \Delta t \\ 0 & 1 \end{bmatrix} \color{royalblue}{\mathbf{\hat{x}}_{k-1}} \\ &= \mathbf{F}_k \color{royalblue}{\mathbf{\hat{x}}_{k-1}} \end{align}$

在状态变化的过程中引入了新的不确定性，根据协方差的乘积公式可得：

C o v (x) C o v (A x) = Σ = A Σ A T

$\begin{equation} \begin{split} Cov(x) &= \Sigma\\ Cov(\color{firebrick}{\mathbf{A}}x) &= \color{firebrick}{\mathbf{A}} \Sigma \color{firebrick}{\mathbf{A}}^T \end{split} \label{covident} \end{equation}$

x ̂ k P k = F k x ̂ k - 1 = F k P k - 1 F T k

$\begin{equation} \begin{split} \color{deeppink}{\mathbf{\hat{x}}_k} &= \mathbf{F}_k \color{royalblue}{\mathbf{\hat{x}}_{k-1}} \\ \color{deeppink}{\mathbf{P}_k} &= \mathbf{F_k} \color{royalblue}{\mathbf{P}_{k-1}} \mathbf{F}_k^T \end{split} \end{equation}$

考虑外部状态

外部状态，这里以加速度为例，引入变量 $\color{darkorange}{a}$ ( $\color{darkorange}{\vec{\mathbf{u}_k}}$ )。

p k v k = p k - 1 + Δ t = v k - 1 + v k - 1 + 1 2 a Δ t 2 a Δ t

$\begin{split} \color{deeppink}{p_k} &= \color{royalblue}{p_{k-1}} + {\Delta t} &\color{royalblue}{v_{k-1}} + &\frac{1}{2} \color{darkorange}{a} {\Delta t}^2 \\ \color{deeppink}{v_k} &= &\color{royalblue}{v_{k-1}} + & \color{darkorange}{a} {\Delta t} \end{split}$

x ̂ k = F k x ̂ k - 1 + ⎡ ⎣ ⎢ ⎢ Δ t 2 2 Δ t ⎤ ⎦ ⎥ ⎥ a = F k x ̂ k - 1 + B k u k \to

$\begin{equation} \begin{split} \color{deeppink}{\mathbf{\hat{x}}_k} &= \mathbf{F}_k \color{royalblue}{\mathbf{\hat{x}}_{k-1}} + \begin{bmatrix} \frac{\Delta t^2}{2} \\ \Delta t \end{bmatrix} \color{darkorange}{a} \\ &= \mathbf{F}_k \color{royalblue}{\mathbf{\hat{x}}_{k-1}} + \mathbf{B}_k \color{darkorange}{\vec{\mathbf{u}_k}} \end{split} \end{equation}$

同时，环境仍然存在我们无法刻画的误差，以 ${\color{mediumaquamarine}{\mathbf{Q}_k}}$ 表示，最终的预测公式如下：

x ̂ k P k = F k x ̂ k - 1 + B k u k \to = F k P k - 1 F T k + Q k

$\begin{equation} \begin{split} \color{deeppink}{\mathbf{\hat{x}}_k} &= \mathbf{F}_k \color{royalblue}{\mathbf{\hat{x}}_{k-1}} + \mathbf{B}_k \color{darkorange}{\vec{\mathbf{u}_k}} \\ \color{deeppink}{\mathbf{P}_k} &= \mathbf{F_k} \color{royalblue}{\mathbf{P}_{k-1}} \mathbf{F}_k^T + \color{mediumaquamarine}{\mathbf{Q}_k} \end{split} \end{equation}$

从上述式子可见：

$\color{deeppink}{\textbf{新的最优估计}}$ 是 $\color{royalblue} {\textbf{之前最优估计}}$ 的预测加上 $\color{darkorange}{\textbf{已知的外界影响}}$ 的修正。
$\color{deeppink}{\textbf{新的不确定度}}$ 是 $\color{royalblue} {\textbf{预测的不确定度}}$ 加上 $\color{mediumaquamarine}{\textbf{环境的不确定度}}$ 。

修正当下

我们已得到 $\color{deeppink}{\mathbf{\hat{x}}_k, P_k}$ ，下面要通过观测到的测量值 $\color{yellowgreen}{\vec{\mathbf{z}_k}}$ 对 ${\mathbf{\hat{x}}_k, P_k}$ 进行更新。

因为 $\color{deeppink}{\mathbf{\hat{x}}_k, P_k}$ 和 $\color{yellowgreen}{\vec{\mathbf{z}_k}}$ 的数据尺度不一定相同，例如 $\color{deeppink}{\mathbf{\hat{x}}_k, P_k}$ 包含了 $\color{deeppink}{\textbf{笛卡尔}}$ 的坐标信息，使用radar得到的 $\color{yellowgreen}{\vec{\mathbf{z}_k}}$ 则包含 $\color{yellowgreen}{\textbf{极坐标}}$ 信息。所以首先应该把两者放在相同的尺度下去比较，尺度转换使用 $\mathbf{H}_k$ 将预测信息转化为测量信息的尺度。

μ ⃗ expected Σ expected = H k x ̂ k = H k P k H T k

$\begin{equation} \begin{aligned} \vec{\mu}_{\text{expected}} &= \mathbf{H}_k \color{deeppink}{\mathbf{\hat{x}}_k} \\ \mathbf{\Sigma}_{\text{expected}} &= \mathbf{H}_k \color{deeppink}{\mathbf{P}_k} \mathbf{H}_k^T \end{aligned} \end{equation}$

这样一来，便得到测量尺度上的两个分布：

测量值的分布 $\mathcal{N}(x, \color{yellowgreen}{\mu_1}, \color{mediumaquamarine}{\sigma_1})$
预测值变换后的分布 $\mathcal{N}(x, \color{fuchsia}{\mu_0}, \color{deeppink}{\sigma_0})$

下面一个问题就是如何用这个两个分布组成新的分布。

 (x, μ 0, σ 0) \cdot  (x, μ 1, σ 1) = ?  (x, μ', σ')

$\begin{equation} \mathcal{N}(x, \color{fuchsia}{\mu_0}, \color{deeppink}{\sigma_0}) \cdot \mathcal{N}(x, \color{yellowgreen}{\mu_1}, \color{mediumaquamarine}{\sigma_1}) \stackrel{?}{=} \mathcal{N}(x, \color{royalblue}{\mu’}, \color{mediumblue}{\sigma’}) \end{equation}$

这里写图片描述

简单的一维情况如下：

k = σ 2 0 σ 2 0 + σ 2 1

$\begin{equation} \color{purple}{\mathbf{k}} = \frac{\sigma_0^2}{\sigma_0^2 + \sigma_1^2} \end{equation}$

高维情况下，针对测量值分布 $(\color{yellowgreen}{\mu_1}, \color{mediumaquamarine}{\Sigma_1}) = (\color{yellowgreen}{\vec{\mathbf{z}_k}}, \color{mediumaquamarine}{\mathbf{R}_k})$ 与预测值的变化分布 $(\color{fuchsia}{\mu_0}, \color{deeppink}{\Sigma_0}) = (\color{fuchsia}{\mathbf{H}_k \mathbf{\hat{x}}_k}, \color{deeppink}{\mathbf{H}_k \mathbf{P}_k \mathbf{H}_k^T})$ 组合的高斯分布如下：

x ̂' k P' k = x ̂ k = P k + - K' (z k \to - H k x ̂ k) K' H k P k

$\begin{equation} \begin{split} \color{royalblue}{\mathbf{\hat{x}}_k’} &= \color{fuchsia}{\mathbf{\hat{x}}_k} & + & \color{purple}{\mathbf{K}’} ( \color{yellowgreen}{\vec{\mathbf{z}_k}} – \color{fuchsia}{\mathbf{H}_k \mathbf{\hat{x}}_k} ) \\ \color{royalblue}{\mathbf{P}_k’} &= \color{deeppink}{\mathbf{P}_k} & – & \color{purple}{\mathbf{K}’} \color{deeppink}{\mathbf{H}_k \mathbf{P}_k} \end{split} \end{equation}$

K' = P k H T k (H k P k H T k + R k) - 1

$\begin{equation} \color{purple}{\mathbf{K}’} = \color{deeppink}{\mathbf{P}_k \mathbf{H}_k^T} ( \color{deeppink}{\mathbf{H}_k \mathbf{P}_k \mathbf{H}_k^T} + \color{mediumaquamarine}{\mathbf{R}_k})^{-1} \end{equation}$

总结

预测未来

输入：过去的最优状态（ $\color{royalblue}{\mathbf{\hat{x}}_{k-1}}$ ， $\color{royalblue}{\mathbf{P}_{k-1}}$ ）、外界对过程的影响 $\color{darkorange}{\vec{\mathbf{u}_k}}$ ，环境的不确定度 $\color{mediumaquamarine}{\mathbf{Q}_k}$ 。
输出：预测的最优状态（ $\color{deeppink}{\mathbf{\hat{x}}_{k}}$ ， $\color{deeppink}{\mathbf{P}_{k}}$ ）。
其他：对过程的描述（ $\mathbf{F_k}$ ， $\mathbf{B_k}$ ）跟时间有关。

x ̂ k P k = F k x ̂ k - 1 + B k u k \to = F k P k - 1 F T k + Q k

修正当下

输入：预测的最优状态（ $\color{deeppink}{\mathbf{\hat{x}}_{k}}$ ， $\color{deeppink}{\mathbf{P}_{k}}$ ），测量的状态分布 $(\color{yellowgreen}{\vec{\mathbf{z}_k}}, \color{mediumaquamarine}{\mathbf{R}_k})$ ，预测到测量的变换矩阵 $\color{deeppink}{\mathbf{H}_k}$ 。
输出：经过测量修正的最优状态（ $\color{royalblue}{\mathbf{\hat{x}}_{k}'}$ ， $\color{royalblue}{\mathbf{P}_{k}'}$ ）。