Kalman滤波——初阶入门

版权声明：本文为博主原创文章，未经博主允许不得转载。 https://blog.csdn.net/u012423865/article/details/80517834

概要

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

思路讲解

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

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

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

核心公式

1. $\hat x_t^- = F\hat x_{t-1} + Bu_{t-1}$ —— 状态预测方程
   
   1. F —— 状态转换矩阵，如何从上一时刻状态推测当前状态
   2. B —— 控制矩阵，系统控制量如何作用当前状态
   3. $u_{t-1}$系统控制量（系统输入量）
2. $P_t^- = FP_{t-1}F^T + Q$ —— 协方差传递方程
   
   1. $P$ —— 预测状态$\hat x_t^-$的协方差
   2. $Q$ —— 上述预测模型的协方差矩阵
3. $K_t = \frac{P_t^-H_T}{HP_t^-H_T + R}$ —— $K_g$的求解
   
   1. $z_t = Hx_t + v$ —— 观察矩阵
      
      1. $H$ —— 本身状态和观测状态的转换关系
      2. $v$ —— 观测噪声
      3. $R$ —— 观测噪声的协方差
4. $\hat x_t = \hat x_t^- + K_t(z_t - H\hat x_t^-)$ —— 当前状态更新
5. $P_t = (I - K_tH)P_t^-$ —— 对预测状态协方差的更新

公式分析

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

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

状态预测模型

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

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

$P_t = P_{t-1} + V_{t-1}*\delta t + \frac{1}{2}u_t\delta t^2$

$V_t = V_{t-1} + u_t*\delta t$

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

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

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

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

则上述公式可以写为$\hat x_t^- = F\hat x_{t-1} + Bu_{t-1}$ ①

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

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

$cov(Ax,Bx) = Acov(x,x)B^T$

​ $P_t = FP_{t-1}F^T$ 其中$F^T$表示状态转换矩阵$F$的转置。

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

  $P_t = FP_{t-1}F^T + Q$ ②

观测模型

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

​ $Z_t = HX_t + v$ ③

1. $X_t$ —— $\begin{matrix} P_t \\ V_t \end{matrix}$

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

3. $v$ —— 观测噪声

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

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

状态更新

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

​ $\hat x_t = \hat x_t^- + K_t(z_t - H\hat x_t^-)$ ④

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

​ $K_t$代表$t$时刻的卡尔曼增益

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

$K_g$求解

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

​ $K_t = \frac{P_t^-H_T}{HP_t^-H_T + R}$ ⑤

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

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

协方差$P$更新

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

​ $P_t = (I - K_tH)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滤波还是能够较好的将结果往回拉。

参考

1. B站视频讲解
2. Kalman滤波器从原理到实现

PS

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