Kalman滤波——初阶入门

2023-11-03

概要

kalman滤波在机器人控制、数字图像等领域应用非常广泛的一种方法，很多人对其名字不能理解，因为kalman滤波在大多数时候表现出来都是将多个数据进行融合，为什么不叫kalman融合呢？如果你有这个疑问，那就说明你对kalman滤波理解不够，任何的数据融合都是为了将多种途径的数据中的噪声滤波，以达到尽可能接近真实值的目的，从这个角度理解，其融合只是表象，滤除了信号中的噪声才是本质。接下来我将从最简单的角度带领大家入门kalman滤波，保证没有任何基础的人也可以理解什么是卡尔曼滤波。先从原理开始，后面会有C语言的实现代码演示。

思路讲解

首先大家设想一下如下场景：在一个封闭房间里，已知当前的温度，如何尽可能准确的获取下一时刻的温度呢？方法无外乎两种，推断or测量。

1. 根据当前时刻的温度，用经验估计下一时刻的温度。
2. 使用温度计测量下一时刻的温度。

当然，任何估计或者测量，都是有误差的，有没有办法用一种方法，将上述两种方法的值进行融合，很好，看到这里，你已经看到了kalman滤波的关键所在了。在kalman滤波里面，有一个卡尔曼增益(Kalman Gain)或卡尔曼系数来表达两个数据格子的权重，并且有方法让这个Kg根据每次的情况进行迭代，达到无限循环的目的。

核心公式

x^−t=Fx^t−1+But−1 x ^ t − = F x ^ t − 1 + B u t − 1 —— 状态预测方程
1. F —— 状态转换矩阵，如何从上一时刻状态推测当前状态
2. B —— 控制矩阵，系统控制量如何作用当前状态
3. ut−1 u t − 1 系统控制量（系统输入量）
P−t=FPt−1FT+Q P t − = F P t − 1 F T + Q —— 协方差传递方程
1. P P —— 预测状态x^−t ${\hat{x}}_{t}^{-}$ 的协方差
2. Q Q —— 上述预测模型的协方差矩阵
Kt=P−tHTHP−tHT+R $K_{t} = \frac{P_{t}^{-} H_{T}}{H P_{t}^{-} H_{T} + R}$ —— Kg K g 的求解
1. zt=Hxt+v z t = H x t + v —— 观察矩阵
  1. H H —— 本身状态和观测状态的转换关系
  2. v $v$ —— 观测噪声
  3. R R —— 观测噪声的协方差
x^t=x^−t+Kt(zt−Hx^−t) ${\hat{x}}_{t} = {\hat{x}}_{t}^{-} + K_{t} (z_{t} - H {\hat{x}}_{t}^{-})$ —— 当前状态更新
Pt=(I−KtH)P−t P t = ( I − K t H ) P t − —— 对预测状态协方差的更新

公式分析

不像别的教程，上来就进行step-by-step的分析。我觉得先将核心的五个公式放上来，让大家先过目一遍，带着问题思考，有助于理解。

我们的分析还是从经典的例子——小车模型开始。

状态预测模型

现在我们假设有一辆小车在二维平面上直线运动如下，

这里写图片描述

如果其当前状态为 P P 、V $V$ 、 U U ，其中P $P$ 为距离， V V 为速度，U $U$ 位加速度，那么我们可以得到如下方程组:

Pt=Pt−1+Vt−1∗δt+12utδt2 P t = P t − 1 + V t − 1 ∗ δ t + 1 2 u t δ t 2

Vt=Vt−1+ut∗δt V t = V t − 1 + u t ∗ δ t

因为卡尔曼滤波只能应用在线性滤波的场景下，所以我们当然认为上述方程组是线性方程，那么其矩阵形式为这里写图片描述

如果我们令 F F = 10δt1 $\begin{matrix} 1 & δ t \\ 0 & 1 \end{matrix}$ 、 B B = δt22δt $\begin{matrix} \frac{δ t^{2}}{2} \\ δ t \end{matrix}$

F F 为状态转换矩阵，表示如何从上一时刻状态推测当前状态

B $B$ 为控制矩阵，表示系统控制量如何作用当前状态

则上述公式可以写为 x^−t=Fx^t−1+But−1 x ^ t − = F x ^ t − 1 + B u t − 1 ①

这就是卡尔曼滤波的第一个方程——状态预测方程。这里的 x^t−1 x ^ t − 1 代表上一时刻估计的最佳状态（当然不是真实值，真实值我们永远也无法准确获取）， x^−t x ^ t − 代表根据上一时刻对当前时刻的推测值，减号代表还未经过kalman修正。

我们知道，任何的状态估计都是有噪声的，在这里我们用协方差矩阵 P P 来表示状态预测模型的噪声大小。但是仅表示当前的噪声大小还不能让整个系统迭代下去，但是因为有上述的公式，我们根据协方差的定义，可以根据当前时刻的协方差来表示下一时刻的协方差：

cov(Ax,Bx)=Acov(x,x)BT $c o v (A x, B x) = A c o v (x, x) B^{T}$

Pt=FPt−1FT P t = F P t − 1 F T 其中 FT F T 表示状态转换矩阵 F F 的转置。

尽管如此，我们的预测模型也不可能完全准确，它还是会有噪声的存在，同样，这里我们用协方差矩阵Q $Q$ 来表示其噪声大小。所以上述公式的完全版：

Pt=FPt−1FT+Q P t = F P t − 1 F T + Q ②

观测模型

我们当然还会有观测，在上述的小车模型里，我们可以观测到距离和速度，或者二者之一即可，令我们观测到的状态为 Zt Z t ，则小车的当前 Xt X t 到观测状态 Zt Z t 的表达关系为：

Zt=HXt+v Z t = H X t + v ③

Xt X t —— PtVt P t V t

H H —— 本身状态和观测状态的转换关系

v $v$ —— 观测噪声

因为我们这里我们可以观测到一维或多维数据，例如在小车模型里，我们可以观测到速度或者距离或者二者集合，所以这里的矩阵 H H 可以是10 $\begin{matrix} 1 \\ 0 \end{matrix}$ ，也可以是 11 1 1

在这里还有一点，我们同样要用一个协方差矩阵 R R 来表示观测噪声v $v$ 的波动大小。

状态更新

上述两点，我们讲到了预测模型和观测模型，接下来就是kalman滤波的核心，如何根据这两个模型值来得到一个更接近真实值的修正值。

x^t=x^−t+Kt(zt−Hx^−t) x ^ t = x ^ t − + K t ( z t − H x ^ t − ) ④

这里括号里面的是观测值和预测值的残差，其实就是公式③中的观测噪声 v v ，

Kt $K_{t}$ 代表 t t 时刻的卡尔曼增益

根据对残差乘上Kt $K_{t}$ ，则表达了观测模型和预测模型各自的权重。

Kg K g 求解

那么如何求解 Kg K g 呢，这里推导比较复杂，后续的文章再做详细说明，今天就列出公式：

Kt=P−tHTHP−tHT+R K t = P t − H T H P t − H T + R ⑤

Kt K t 除了上述说的衡量两个模型的权重功能外，还能够将数值从 zt z t 转换到 xt x t 。

什么意思？前面说了，在小车模型中状态预测值 x^t x ^ t 是速度和距离的矩阵集合，而观测值可以是单纯的速度或距离或二者集合。所以他们 xt x t 和 zt z t 两者可能单位都不同，这个时候就要 Kg K g 来进行状态转换。

协方差 P P 更新

为了使得整个系统能够迭代运算下去，我们当然需要对状态预测模型的协方差P $P$ 进行更新

Pt=(I−KtH)P−t P t = ( I − K t H ) P t − ⑥

代码实例

下面我们用C代码来实现以下上述小车模型中的一维kalman滤波

我是在win+Qt5的环境下实现的。
std::vector<int> result, measure;

/* 1 Dimension */
typedef struct {
    float x; //-- @Zuo 状态值
    float F; //-- @Zuo x(n)=F*x(n-1)+u(n),u(n)~N(0,Q)
    float H; //-- @Zuo z(n)=H*x(n)+w(n),w(n)~N(0,R)
    float Q; //-- @Zuo 协方差P传递方程的协方差
    float R; //-- @Zuo 观测噪声协方差
    float P; //-- @Zuo 预测模型的协方差
    float gain; //-- @Zuo 卡尔曼增益
} kalman1_state;

void kalman1_init(kalman1_state *state, float init_x, float init_P)
{
    state->x = init_x;
    state->P = init_P;
    state->F = 1;
    state->H = 1;
    state->Q = 2e2;
    state->R = 5e2;
}

float kalman1_filter(kalman1_state *state, float z_measure)
{
    /* Predict */
    state->x = state->F * state->x;
    state->P = state->F * state->F * state->P + state->Q;

    /* Measurement */
    state->gain = state->P * state->H / (state->P * state->H * state->H + state->R);
    state->x = state->x + state->gain * (z_measure - state->H * state->x);
    state->P = (1 - state->gain * state->H) * state->P;

    return state->x;
}

MainWindow::MainWindow(QWidget *parent) :
    QMainWindow(parent),
    ui(new Ui::MainWindow)
{
    ui->setupUi(this);

    kalman1_state ks;
    kalman1_init(&ks, 0, 1);
    for(int i=0; i<100; i++){
        float m = 40;
        float noise = (std::rand()%200)/10.0;

        if(i==50) noise = -40;
        if(i==70) noise = 35;

        m += noise;

        kalman1_filter(&ks, m);
        result.push_back(ks.x);
        measure.push_back(m);
    }
}

void MainWindow::paintEvent(QPaintEvent *event)
{
    Q_UNUSED(event);

    QPainter painter(this);

    QFont font;
    font.setFamily("Microsoft YaHei");
    font.setPointSize(50);
    font.setItalic(true);
    painter.setFont(font);

    painter.setPen(QColor(255, 0, 0));
    painter.drawLine(QPoint(0,500), QPoint(2000,500));

    for(int i=0; i<measure.size()-1; i++){
        painter.setPen(QColor(0, 255, 0));
        painter.drawLine(QPoint(i*10,result[i]*10), QPoint((i+1)*10,result[i+1]*10));
        painter.setPen(QColor(0, 0, 255));
        painter.drawLine(QPoint(i*10,measure[i]*10), QPoint((i+1)*10,measure[i+1]*10));
    }
}
下面是结果图，其中红色线代表真实值，当然越接近代表滤波效果越好。绿色和蓝色分别代表kalman滤波的输出值和测量值。我给测量值增加了一个20以内的随机误差，但是可以看到我稍微皮了一下，在i=50和i=70的时候让误差大幅度的波动了一下，但是看到kalman滤波还是能够较好的将结果往回拉。

参考

B站视频讲解

Kalman滤波器从原理到实现

PS

觉得本文有帮助的可以左上角点赞，或者留言互动。

本文内容由网友自发贡献，版权归原作者所有，本站不承担相应法律责任。如您发现有涉嫌抄袭侵权的内容，请联系:hwhale#tublm.com(使用前将#替换为@)