卡尔曼家族从零解剖-(06) 一维卡尔曼滤波编程(c++)实践、透彻理解公式结果

讲解关于slam一系列文章汇总链接:史上最全slam从零开始，针对于本栏目讲解的卡尔曼家族从零解剖链接 :卡尔曼家族从零解剖-(00)目录最新无死角讲解：https://blog.csdn.net/weixin_43013761/article/details/133846882

${\color{blue}{文末正下方中心}提供了本人 \color{red} 联系方式，\color{blue}点击本人照片即可显示WX→官方认证}$

一、前言

通过五个章节的分析，目前对于一维卡尔曼滤波有了一定层次的理解，这里先给出上篇博客推导出来的结论(卡尔曼五大公式)：
$\color{red} ①：\tag{01}\check x_{k}= f\hat x_{k-1}~~~~~~~~~~~~~~~②：\sigma^{-}_{X_{k}}=f^2\sigma_{X_{k-1}}^{+}+\sigma_{Q_{k-1}}$ $\color{red} \tag{02}③：k_k=\frac{h \sigma_{X_k}^{-} }{h^{2} \sigma_{X_k}^{-} +\sigma_{R_k}}$ $\color{red} \tag{03} ④：\hat x_{k}=k_k(y_k-h\check x)+\check x~~~~~~~~~~~~~~~~~~⑤：\sigma^+_{X_{k}}=(1-hk_k) \sigma_{X_k}^{-}$
上面的五个式子很明显是递推的若假设已知 $\hat x_0$ 、 $\sigma_{X_{0}}^+$ 、以及各个时刻观测 $y_k$ ，则可推导出出 $\hat x_k$ 、 $\sigma_{X_{k}}^+$ ，如下：
$\color{Green} \tag{04}【\hat x_0,\sigma_{X_{0}}^+,y_1】→【\hat x_1,\sigma_{X_{1}}^+,y_2】→\cdots→【\hat x_k,\sigma_{X_{k}}^+】$ 该篇本博客主要是进行编程实践，为了公式与源码更好的对应起来，对上述公式公式进行改写，因为编程中通常需要进行模块下，所以代码中会实现一个函数，该函数只完成一次递推，故上5式符号简写为：
$\color{red} ①：\tag{05} x_ {minus}= f x_{plus}~~~~~~~~~~~~~~~②：\sigma_{minus}=f^2\sigma_{plus}+q$ $\color{red} \tag{06}③：k=\frac{h \sigma_{minus} }{h^{2} \sigma_{minus} +r}$ $\color{red} \tag{07} ④： x_{plus}=k(y-h x_{minus})+x_{minus}~~~~~~~~~~~~~~~~~~⑤：\sigma_{plus}=(1-hk) \sigma_{minus}$ 上式中的 $r=\sigma_{R_k}$ (预测过程标准差，主要影响收敛速度)， $q=\sigma_{R_k}$ (观测过程标准差，理解为传感器精度，可以通过实验获得)，这两个值都是固定值，迭代过程中通常不会改变。由于是编程，(05) 式中的 $x_ {minus}$ 最终会被 (07) 式中的 $x_{plus}$ 覆盖，同理 $\sigma_{plus}$ 也会被覆盖。每次计算出来的 $x_{plus}$ 与 $\sigma_{plus}$ 又会作为下一次的 $x_{minus}$ 与 $\sigma_{minus}$ 进行输入。

二、C++一维示例

这是一个c++的示例程序，其主要演示如何使用一维卡尔曼滤波对一个线性方程进行预测，该线性方程建模十分简单，即为 $ax^2$ 的形式，容易看出这是一个二次函数，对应代码如下：

	float observe = a * std::pow(x, 2) + normal(mt);  // 生成 ax*x + b 加上一个高斯噪声的观测值

这里生成的是观测数据，另外一个重要的地方就是关于卡尔曼滤波初始化过程：

    // 创建一个卡尔曼滤波,分别设置参数 f, h, q, r; 这些数值迭代过程中是不会改变的
    KalmanFilter1D kalman_filter(1.0, 1.0, 1, 10);
    // 设置初始状态的均值与方差，后续迭代过程中会改变
    float x_plus = 100, sigma_plus = 0.0;

有兴趣的朋友可以调整一个参数，看下输出数据的变化，代码会把数据进行保存，通过 file_path 参数可以修改该保存路径，曲线图查看工具本人使用的是 plotjuggler，有兴趣的朋友可以自行百度以下使用方式，下面是整体代码(含注释)：

/*
 * @Author: WenhaiZhu 944284742@qq.com
 * @Date: 2023-11-13 14:22:04
 * @LastEditors: WenhaiZhu 944284742@qq.com
 * @LastEditTime: 2023-11-14 06:28:20
 * @FilePath: /slam_in_autonomous_driving/src/kalman_filter/kalman_filter1_zwh.cc
 * @Description: 这是默认设置,请设置`customMade`, 打开koroFileHeader查看配置 进行设置: https://github.com/OBKoro1/koro1FileHeader/wiki/%E9%85%8D%E7%BD%AE
 */

#include <gflags/gflags.h>
#include <glog/logging.h>
#include <pangolin/pangolin.h>
#include <fstream>
#include <iostream>
#include <random>

class KalmanFilter1D {
   public:
    KalmanFilter1D(float f, float h, float q, float r) : f_(f), h_(h), q_(q), r_(r) {}
    ~KalmanFilter1D() {}

    void operator()(float x_plus, float sigma_plus, float observe) {
        float x_minus = f_ * x_plus + q_;                                // 求先验均值
        float sigma_minus = std::pow(f_, 2) * sigma_plus + q_;           // 求先验方差
        k_ = (h_ * sigma_minus) / (std::pow(h_, 2) * sigma_minus + r_);  // 求卡尔曼增益系数
        x_plus_ = x_minus + k_ * (observe - h_ * x_minus);               // 对先验均值更新,得到后验均值
        sigma_plus_ = (1 - h_ * k_) * sigma_minus;                       // 对先验更新,获得后验方标准差
    }

    float GetXMinus() { return x_plus_; }
    float GetSigmaMinus() { return sigma_plus_; }
    float Get_k() {}

   private:
    float f_;
    float h_;
    float q_;
    float r_;

    float k_ = 0;
    float x_plus_ = 0;
    float sigma_plus_ = 0;
};

int main(int argc, char* argv[]) {
    // 预备工作,可以忽略
    google::InitGoogleLogging(argv[0]);
    FLAGS_stderrthreshold = google::INFO;
    FLAGS_colorlogtostderr = true;
    google::ParseCommandLineFlags(&argc, &argv, true);

    // 创建一个卡尔曼滤波,分别设置参数 f, h, q, r; 这些数值迭代过程中是不会改变的
    KalmanFilter1D kalman_filter(1.0, 1.0, 1, 10);
    // 设置初始状态的均值与方差，后续迭代过程中会改变
    float x_plus = 100, sigma_plus = 0.0;  

    // 创建文件,用于保存数据,方便可视化
    std::string file_path = "./samples_zwh.csv";
    std::ofstream fd(file_path);
    if (!fd.is_open()) {
        LOG(ERROR) << "Unable to open file: " << file_path;
    }
    fd << "t,"
       << "x_plus,"
       << "observe," << std::endl;

    std::mt19937 mt(0);                            // 随机种子
    std::normal_distribution<> normal(0.0, 20.0);  // 用于随机生成符合高斯分布的数据
    float a = 0.1, b = 0.2;                        // 真实模型为 ax*x + b
    float x = 0.0, dt = 0.1;                       // x标识x轴数值,后面会刷新,dt标识采样间隔

    for (int i = 0; i < 1000; ++i) {
        float observe = a * std::pow(x, 2) + normal(mt);  // 生成 ax*x + b 加上一个高斯噪声的观测值

        kalman_filter(x_plus, sigma_plus, observe);  // 进行卡尔曼滤波，observe 等于观测方程中的 y

        x_plus = kalman_filter.GetXMinus();          // 获取滤波之后的均值
        sigma_plus = kalman_filter.GetSigmaMinus();  // 获取滤波之后的标准差

        x += dt;                                                         // 采样更新
        fd << x << "," << x_plus << "," << observe << "," << std::endl;  // 保存数据用于可视化
    }
    fd.close();
    return 0;
}

下面曲线是本人通过 plotjuggler 绘画结果，红色为 observe(y)，绿色为为 x_plus，可以很明显看出 x_plus 的抖动小了很多，这就是滤波的效果。下图中，并没有把真值刻画出来，因为实际在应用的时候，也是没有办法知道真值的，如果是兴趣的朋友，可以简单修改一下源码把代码中 a * std::pow(x, 2) 结果保存下来即可。
在这里插入图片描述

三、透彻理解公式

1.先验分析

本博客的最开始，已经把公式给出，在前面文章中虽然大致给出了推导过程，但是并没有深入的探讨过这个公式，先来说一下 (05) 式，如下：
$\color{Green} ①：\tag{08} x_ {minus}= f x_{plus}~~~~~~~~~~~~~~~②：\sigma_{minus}=f^2\sigma_{plus}+q$ 上式中的 $q$ 表示符合噪声高斯噪声 $N (0, q)$ 的标准差，可以看出，每次迭代，预测状态 $x_ {minus}$ 对应的标准差 $\sigma_{minus}$ 都会在原来的基础( $\sigma_{plus}$ )上叠加一个 $q$ ，假如没后续没有观测方程对齐进行修正，可想而知， $\sigma_{minus}$ 会约来越打，即 $x_{minus}$ 的可信度越来越低，其是没有办法当作一个可信的结果输出的。

另外，为了方便讲解，对状态方程 $x_{plus} = f(x_ {minus})$ 进行了简化，直接变成了 $x_{plus} = f*x_ {minus}$ 形式，其实这里的 $f (x)$ 是一个广义的概念，并不局限于 $x$ 一个参数，回忆一下前面的博客卡尔曼家族从零解剖-(03)贝叶斯滤波→公式推导与示例中 (5) 式： $x_{k}=f(x_{k-1},v_{k-1})+q_k$ ，可以知道其接收两个参数，其实不止，只要保证 $f (x)$ 是线性的，完全可以接收更多的参数。如书写成 $x_{k}=f(x_{k-1},v_{k-1},u_{k-1},t_{k-1},)+q_k$ 等。值得 $\color{red} 注意:$ 只有 $x$ 才是由卡尔曼递推的，其他的只能类似于 $y$ 通过观测或者测量或者。且 $f(x_{k-1},v_{k-1},u_{k-1},t_{k-1},)+q_k$ 的结果只能为状态 $x$ 而不是 $v$ 、 $u$ 等。

2.卡尔曼增益

卡尔曼增益就是上面的 (06)，英文通常称为 Kalman Gain，如下：
$\color{Green} \tag{09}③：k=\frac{h \sigma_{minus} }{h^{2} \sigma_{minus} +r}$ 其就是一个比值，由于 $h$ 是一个常数，所以下面的讨论忽略他，单独来分析 $\sigma_{minus}$ 与 $r$ ，前者是预测出来的标准差，值越小则表示预测的状态 $x_{minus}$ 可信度越高，这里假设他无穷小，也就是说预测出来的结果无线接近真实值，根据上式，其结果 $k$ 就无限趋近于 0。再反过来，如果 $x_{minus}$ 无限趋向于无穷大，易知结果 $k$ 趋向于 1(前面提到不考虑 $h$ )。

那么总结起来， $k$ 越大，表示预测结果 $x_{minus}$ 越不可信， $k$ 越小表示预测结果 $x_{minus}$ 越可信。下面就是看如何把这个 $k$ 用起来。

3.状态更新

其实观测方程也是一个广义的概念，其并不局限于一个观测值 $y$ ，也就是说在 $k$ 时刻状态 $x_k$ 下，你可以观测到 $y_{k1}、y_{k2}、y_{k3}、\cdots、y_{kj}$ 。这里举一个例子，算法估算出温度是 $35$ (先验)，但是你可以用 $j$ 个体温计进行测量，获得 $j$ 个观测值，也就是说观测方程可以书写成 $(y_{k1},y_{k1},\cdots, y_{kj})=f(x_k)$ ，使用逆函数变换一下就成了 $x_k=f^{-1}(_{k1},y_{k1},\cdots, y_{kj})$ ，这里看起来可能更加合理一点，后续讲解矩阵形式的卡尔曼滤波，或许会为大家示例一下，这里就暂且略过了，回到 (07) 式： $\color{Green} \tag{07} ④： x_{plus}=k(y-h x_{minus})+x_{minus}~~~~~~~~~~~~~~~~~~⑤：\sigma_{plus}=(1-hk) \sigma_{minus}$
根据前面的结论， $k$ 越大，表示预测结果 $x_{minus}$ 越不可信， $k$ 越小表示预测结果 $x_{minus}$ 越可信。来看看是不是这么一回事，