卡尔曼滤波算法原理

Rareay

已于 2022-12-04 19:58:38 修改

阅读量954

点赞数

CC 4.0 BY-SA版权

分类专栏：机器视觉文章标签：算法概率论

于 2022-12-04 19:55:04 首次发布

本文链接：https://blog.csdn.net/qq_33236581/article/details/128176344

机器视觉专栏收录该内容

1 篇文章

订阅专栏

1 公式

预测：
$X^k=AX^k−1+Bkuk→\hat{X}_k = A \hat{X}_{k-1} + B_{k} \overrightarrow{u_k}$

$P_k = A P_{k-1} A^T + Q$

修正：
$K_k'=P_kH^T(H P_k H^T + R)^{-1}$

$X^k′=Xk^+Kk′(zk−HkX^k)\hat{X}_k'=\hat{X_k}+K_k'(z_k-H_k\hat{X}_k)$

$P_k' = P_k - K_k' H P_k$

2 建模

假设一辆汽车在直路上行驶，车内可以通过 GPS 定位获取自己的位置 p（ $±10m\pm10m$ )，也可以获取车速 v（ $±1m/h\pm1m/h$ ），同时车里的人会随机加速或减速，也能获得加速度 a（ $±1m/s2\pm1m/s^2$ ），我们的目的是通过对这些测量值的滤波让位置 p 的误差控制在 $±1m\pm1m$ 内。（模型中的测量误差均成正太分布）

我们就用 kalman 的原理逐步推导，直观的了解其数学模型。首先，算法中最关键的其实是概率模型，也就是高斯分布（正太分布）： $N(x,μ,σ2)=1σ2πe−(x−μ)22σ2N(x,\mu,\sigma^2)=\frac{1}{\sigma \sqrt{2\pi}} e^{-\frac{(x-\mu)^2}{2 \sigma^2}}$ ， kalman filter 最终的估计值也就是一个概率分布，只是给外面使用的是其中的均值。在这个模型当中，我们假设估计值 $X^=[pv]\hat{X}= \begin{bmatrix}p \\v \end{bmatrix}$ （高斯分布的均值 $μ\mu$ ），对应的估计协方差为 $P=[ΣppΣpvΣvpΣvv]P=\begin{bmatrix}\Sigma_{pp} && \Sigma_{pv} \\ \Sigma_{vp} && \Sigma_{vv} \end{bmatrix}$ （高斯分布的方差 $σ2\sigma^2$ ）。

注意： $σ\sigma$ 是标准差， $Σ\Sigma$ 是方差， $Σ=σ2\Sigma = \sigma^2$ 。

预测

预测取决于实际的物理模型，这里用到的就是运动学方程：

$pk=pk−1+Δtvk−1p_k = p_{k-1} + \Delta{tv_{k-1}}$

$vk=vk−1v_k = \quad \quad \quad \quad v_{k-1}$

即：

$X^k=[1Δt01]X^k−1=AX^k−1\begin{aligned} \hat{X}_k &= \begin{bmatrix}1 && \Delta{t} \\ 0 && 1 \end{bmatrix} \hat{X}_{k-1} \\ &= A \hat{X}_{k-1} \end{aligned}$

所以这里的 $A$ 就是物理模型矩阵，用于预测出下一时刻的估计值。同样，协方差 $P$ 也要经过物理模型的转换，更新协方差的公式如下：

$\Sigma$

$A\Sigma A^T$

所以预测方程：

$X^k=AkX^k−1\hat{X}_k = A_k \hat{X}_{k-1}$

$P_k = AP_{k-1}A^T$

外部控制

上面的预测是在匀速的情况下，没有外部干扰，如果踩油门或刹车带来了加速减速，那么会引入加速度 $a$ ，同样可以根据运动学方程来描述：
$pk=pk−1+Δtvk−1+12aΔt2p_k = p_{k-1} + \Delta{tv_{k-1}} + \frac{1}{2}a \Delta{t^2}$

$vk=vk−1+aΔtv_k = \quad \quad \quad \quad v_{k-1} + a \Delta{t}$

即：

$X^k=AkX^k−1+[Δt22Δt]a=AkXk−1^+Bkuk\begin{aligned} \hat{X}_k &= A_k \hat{X}_{k-1} + \begin{bmatrix} \frac{\Delta t^2}{2} \\ \Delta t \end{bmatrix} a \\ &= A_k \hat{X_{k-1}} + B_k u_k \end{aligned}$

这里的 $B_k$ 就是外部控制矩阵，描述了外部控制量与估计值的物理关系， $u_k$ 就是外部控制量。同时，外部控制量带来的影响也是呈高斯分布，其协方差为 $Q$ ，需要叠加到已预测的协方差当中，所以最终的预测方程组为：
$X^k=AX^k−1+Bkuk\hat{X}_k = A \hat{X}_{k-1} + B_{k}u_k$

$P_k = A P_{k-1} A^T + Q$

修正

完成预测之后，我们需要通过传感器的测量值对其进行校正，由于预测值和测量值都是一个估计，都存在自己的 $μ\mu$ 和 $σ2\sigma^2$ ，修正这个环节就是融合两个估计值，得到新的 $μ′\mu'$ 和 $σ′2\sigma'^2$ ，即滤波器的最优估计： $X^x′\hat{X}_x'$ 、 $P_k'$ 。如何融合两个高斯分布？方法很简单，直接把他们相乘，做归一化（概率和为1）。

先使用一维高斯分布来分析，假设两个分布： $N(x,μ0,σ02)N(x,\mu_0,\sigma_0^2)$ 和 $N(x,μ1,σ12)N(x,\mu_1,\sigma_1^2)$ ，使：

$N(x,μ0,σ02)⋅N(x,μ1,σ12)=N(x,μ′,σ′2)N(x,\mu_0,\sigma_0^2) \cdot N(x,\mu_1,\sigma_1^2) = N(x,\mu',\sigma'^2)$

将 $N(x,μ,σ2)=1σ2πe−(x−μ)22σ2N(x,\mu,\sigma^2)=\frac{1}{\sigma \sqrt{2\pi}} e^{-\frac{(x-\mu)^2}{2 \sigma^2}}$ 代入上式且做归一化，得到：

$μ′=μ0+σ02(μ1−μ0)σ02+σ12\mu'=\mu_0 + \frac{\sigma_0^2 (\mu_1 - \mu_0)}{\sigma_0^2 + \sigma_1^2}$

$σ′2=σ02−σ04σ02+σ12\sigma'^2 = \sigma_0^2 - \frac{\sigma_0^4}{\sigma_0^2 + \sigma_1^2}$

把上式相同的部分提取出来，得到：

$\frac{\sigma_0^2}{\sigma_0^2 + \sigma_1^2}$

$μ′=μ0+k(μ1−μ0)\mu' = \mu_0 + k(\mu_1 - \mu_0)$

$σ′2=σ02−kσ02\sigma'^2 = \sigma_0^2 - k \sigma_0^2$

好，得到这个关系式基本就快结束了，我们再看需要融合的两个估计：

预测估计： $N(x,HX^k,HPkHT)N(x, H \hat{X}_k, HP_kH^T)$ ，即 $μ0=HkX^k\mu_0 = H_k \hat{X}_k$ ， $σ02=HPkHT\sigma_0^2 = HP_kH^T$
测量估计： $N(x, z_k, R)$ ，即 $μ1=zk\mu_1 = z_k$ ， $σ12=R\sigma_1^2 = R$

在上式中，你可能存在疑问，为什么预测估计中要加入 $H$ 转换矩阵？其实我们做融合的前提是两个估计处于同一尺度，例如一个估计是预测的路程，一个估计是测量的耗油量，它们在数值没有直接的关联，融合之后也不存在意义，所以要把路程换算成耗油量再去做融合才有意义，融合之后的值就是耗油量的最优估计，这里的转换就是 $H$ ，称之为测量转换矩阵，把预测值的尺度转换为测量值的尺度。你可能还会有疑问，我们建模中的预测和测量是同一尺度啊？是的，同一尺度给 $H$ 赋值为 1 就可以了。

我们把两个估计代入高斯融合的关系式，得到：

$\frac{HP_kH^T}{HP_kH^T + R}$