Kalman滤波——初阶入门 - 好文

概要

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

思路讲解

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

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

核心公式

* x^−t=Fx^t−1+But−1x^t−=Fx^t−1+But−1 —— 状态预测方程

* F —— 状态转换矩阵，如何从上一时刻状态推测当前状态
* B —— 控制矩阵，系统控制量如何作用当前状态
* ut−1ut−1系统控制量（系统输入量）
* P−t=FPt−1FT+QPt−=FPt−1FT+Q —— 协方差传递方程

* PP —— 预测状态x^−tx^t−的协方差
* QQ —— 上述预测模型的协方差矩阵
* Kt=P−tHTHP−tHT+RKt=Pt−HTHPt−HT+R —— KgKg的求解

* zt=Hxt+vzt=Hxt+v —— 观察矩阵

* HH —— 本身状态和观测状态的转换关系
* vv —— 观测噪声
* RR —— 观测噪声的协方差
* x^t=x^−t+Kt(zt−Hx^−t)x^t=x^t−+Kt(zt−Hx^t−) —— 当前状态更新
* Pt=(I−KtH)P−tPt=(I−KtH)Pt− —— 对预测状态协方差的更新
公式分析

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

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

状态预测模型

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

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

Pt=Pt−1+Vt−1∗δt+12utδt2Pt=Pt−1+Vt−1∗δt+12utδt2

Vt=Vt−1+ut∗δtVt=Vt−1+ut∗δt

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

如果我们令FF = 10δt11δt01、BB = δt22δtδt22δt

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

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

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

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

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

Pt=FPt−1FTPt=FPt−1FT 其中FTFT表示状态转换矩阵FF的转置。

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

Pt=FPt−1FT+QPt=FPt−1FT+Q ②

观测模型

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

Zt=HXt+vZt=HXt+v ③

*
XtXt —— PtVtPtVt

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

*
vv —— 观测噪声

因为我们这里我们可以观测到一维或多维数据，例如在小车模型里，我们可以观测到速度或者距离或者二者集合，所以这里的矩阵HH可以是1010，也可以是1111

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

状态更新

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

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

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

KtKt代表tt时刻的卡尔曼增益

根据对残差乘上KtKt，则表达了观测模型和预测模型各自的权重。

KgKg求解

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

Kt=P−tHTHP−tHT+RKt=Pt−HTHPt−HT+R ⑤

KtKt除了上述说的衡量两个模型的权重功能外，还能够将数值从ztzt转换到xtxt。

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

协方差PP更新

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

Pt=(I−KtH)P−tPt=(I−KtH)Pt− ⑥

代码实例

下面我们用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站视频讲解 <https://www.bilibili.com/video/av4356232/>
* Kalman滤波器从原理到实现
<https://blog.csdn.net/xiahouzuoxin/article/details/39582483>
PS

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

热门工具换一换