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

讲解关于slam一系列文章汇总链接:史上最全slam从零开始，针对于本栏目讲解的 卡尔曼家族从零解剖 链接 :卡尔曼家族从零解剖-(00)目录最新无死角讲解：https://blog.csdn.net/weixin_43013761/article/details/133846882
 
$文末正下方中心提供了本人联系方式，点击本人照片即可显示WX\to 官方认证$

$郑重声明：该系列博客为本人(WenhaiZhu)独家私有,禁止转载与抄袭,首次举报有谢!$

一、前言

通过五个章节的分析，目前对于一维卡尔曼滤波有了一定层次的理解，这里先给出上篇博客推导出来的结论(卡尔曼五大公式)：
$$①：{\check{x}}_k=f{\hat{x}}_{k-1}②：{\sigma }_{X_k}^{-}=f^2{\sigma }_{X_{k-1}}^{+}+{\sigma }_{Q_{k-1}}\text{\hspace{1em}(01)}$$ $$③：k_k=\frac{h{\sigma }_{X_k}^{-}}{h^2{\sigma }_{X_k}^{-}+{\sigma }_{R_k}}\text{\hspace{1em}(02)}$$$$④：{\hat{x}}_k=k_k(y_k-h\check{x})+\check{x}⑤：{\sigma }_{X_k}^{+}=(1-h{k}_k){\sigma }_{X_k}^{-}\text{\hspace{1em}(03)}$$
上面的五个式子很明显是递推的若假设已知 ${\hat{x}}_0$、${\sigma }_{X_0}^{+}$、以及各个时刻观测 $y_k$，则可推导出出 ${\hat{x}}_k$、${\sigma }_{X_k}^{+}$，如下：
$$【{\hat{x}}_0,{\sigma }_{X_0}^{+},y_1】\to 【{\hat{x}}_1,{\sigma }_{X_1}^{+},y_2】\to \cdots \to 【{\hat{x}}_k,{\sigma }_{X_k}^{+}】\text{\hspace{1em}(04)}$$该篇本博客主要是进行编程实践，为了公式与源码更好的对应起来，对上述公式公式进行改写，因为编程中通常需要进行模块下，所以代码中会实现一个函数，该函数只完成一次递推，故上5式符号简写为：
$$①：x_{minus}=f{x}_{plus}②：{\sigma }_{minus}=f^2{\sigma }_{plus}+q\text{\hspace{1em}(05)}$$ $$③：k=\frac{h{\sigma }_{minus}}{h^2{\sigma }_{minus}+r}\text{\hspace{1em}(06)}$$$$④：x_{plus}=k(y-h{x}_{minus})+x_{minus}⑤：{\sigma }_{plus}=(1-hk){\sigma }_{minus}\text{\hspace{1em}(07)}$$上式中的 $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++的示例程序，其主要演示如何使用一维卡尔曼滤波对一个线性方程进行预测，该线性方程建模十分简单，即为 $a{x}^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; 这些数值迭代过程中是不会改变的
    // q越大收敛速度越快，r越大越倾向于预测结果, 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) 式，如下：
$$①：x_{minus}=f{x}_{plus}②：{\sigma }_{minus}=f^2{\sigma }_{plus}+q\text{\hspace{1em}(08)}$$上式中的 $q$ 表示符合噪声高斯噪声 $N(0,q)$ 的标准差，可以看出，每次迭代，预测状态 $x_{minus}$ 对应的标准差 ${\sigma }_{minus}$ 都会在原来的基础(${\sigma }_{plus}$)上叠加一个 $q$，假如没后续没有观测方程对齐进行修正，可想而知，${\sigma }_{minus}$ 会约来越打，即 $x_{minus}$ 的可信度越来越低，其是没有办法当作一个可信的结果输出的。

另外，为了方便讲解，对 状态方程 $x_{plus}=f(x_{minus})$ 进行了简化，直接变成了 $x_{plus}=f\ast 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$ 等。值得 $注意:$ 只有 $x$ 才是由卡尔曼递推的，其他的只能类似于 $y$ 通过观测或者测量或者。且 $f(x_{k-1},v_{k-1},u_{k-1},t_{k-1},)+q_k$ 的结果只能为状态 $x$ 而不是 $v$、$u$ 等。

2.卡尔曼增益

卡尔曼增益就是上面的 (06)，英文通常称为 Kalman Gain，如下：
$$③：k=\frac{h{\sigma }_{minus}}{h^2{\sigma }_{minus}+r}\text{\hspace{1em}(09)}$$ 其就是一个比值，由于 $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) 式： $$④：x_{plus}=k(y-h{x}_{minus})+x_{minus}⑤：{\sigma }_{plus}=(1-hk){\sigma }_{minus}\text{\hspace{1em}(10)}$$
根据前面的结论，$k$ 越大，表示预测结果 $x_{minus}$ 越不可信，$k$ 越小表示预测 结果 $x_{minus}$ 越可信。来看看是不是这么一回事，

首先看 ④，$y-h{x}_{minus}$ 表示观测误差，如果观测误差比较大，那么说明观测结果不可信，应该相信预测结果，即前面因子 $k$ 应该越小越好。与前面【卡尔曼增益】的逻辑吻合。

再来看⑤，如果 ${\sigma }_{minus}$ 较大，说明预测结果不可信，那么因子 $1-hk$ 的结果越小越好，即 $k$ 越大越好。与前面【卡尔曼增益】的逻辑吻合。

4.思维拓展

$思考:$ 不知道有没有细心的朋友发现到示例程序中，每次递推都对应一个观测。其实呢，在实际应用过程中，通常不会这样的。好比如温度的测量，通常间隔 1 个小时，或者 3 个小时测量一次都是可以的，没有必要时时刻刻进行观测，这样太浪费资源了。

除了浪费资源，有的时候可能因为条件的限制，没有办法进行观测，就拿 GPS 来说，有的时候信号不好可能就收不到数据，或者受到无效数据，这个时候都类似于没有观测。但是，即使没有观测，卡尔曼滤波也是可以进行递推的，只进行 (08) 式的操作，放弃 (09) (10) 式。

这样很显然会存在一个缺点，那就是每次递推都会叠加一个 $q$ 的噪声，所以长时间没有观测，随着累计误差(噪声)的增加，算法就失效了，所以，间隔时间不能太长了。

那么问题又来了，多长时间算长，多短时间算短呢？实际上没有很固定的答案，其主要与你的观测有关，如果你的观测很准确，那么不观测的时间就可以相对延长，等到一次观测，则可以把跑偏的算法拉回正轨。如果你的观测本省就不太精准，那么就不能间隔时间太长不进行观测，容易跑飞。

有兴趣的朋友可以简单修改一下代码，看看调整一下观测频率，再对比一下曲线图，

5.深度解剖

$重点:$ 卡尔曼滤波递推公式虽然是线性的，但是这并不意味着其只能应用于线性变换的场景。来看 (5) 式 $$①：x_{minus}=f{x}_{plus}②：{\sigma }_{minus}=f^2{\sigma }_{plus}+q\text{\hspace{1em}(11)}$$ 其线性，表示的是 $x_{minus}$ 与 $x_{plus}$ 之间的为线性变换，并不是说再整个时间轴(举例)，状态 $x$ 需要呈现为线性变换。那上面例子来说，其是一个二次函数，但是卡尔曼滤波预测效果还是挺好的。

其实卡尔曼滤波，只要你观测足够密集，基本可以预测任意复杂的曲线，两点之间无线接近，可以当作直线来对待。但是如果两时刻状态发生的变换剧烈了，其预测效果与 $q$ 的关系比较大，因为变换太剧烈了，使用通用的直线去进行拟合(如预先设定的 ax)，是十分困难的，这个时候就需要依靠噪声 $q$ 去弥补，因为根据前面的推导，如果 $q$ 比较大，则预测结果 ${\hat{x}}_k$ 才允许距离 ${\hat{x}}_{k-1}$ 较远，从而实现剧烈变化的吻合。

这里就可以看出卡尔曼一个明显的弊端，其不适合预测高次函数，因为存在两点之间的特别剧烈变化的情况，除非把 $q$ 设置为无穷大，不过这样就没有意义了，等价于直接选择观测值了。

6.上帝视角

其实个人查阅过很多博客或教学视频，可能就是看多了反而更加杂乱，到最后发现自己连离散与连续都云里雾里了，明明前面理解的是离散，突然视角一转，又变变成连续了。这里主要的原因是大部分博客，或者 $up$ 没有从一个出血者的角度去考虑，先回顾一下 卡尔曼家族从零解剖-(03) 贝叶斯滤波→公式推导与示例 这篇博客中，的如下图像：
先从宏观的角度来看，对 $k_0$、$k_1$、$\cdots$、$k_k$ 时刻的预测与观测，不用多说把，肯定是离散的，对吧。所以对于贝叶斯滤波经常可以看到公式 $$P\left(X\mid Y_k\right)=\frac{P\left(Y_k\mid X\right)P\left(X\right)}{P(Y_k)}=\eta P\left(Y_k\mid X\right)P\left(X\right)\text{\hspace{1em}(12)}$$不用质疑，这肯定是正确的，因为这就是离散的贝叶斯公式。$坑点:$ 离散概率分布可以通过概率密度积分获得，这个时候容易进入的一个误区是，以为对状态轴 $x$ 进行积分就可以了如下：

假设状态分随机变量 $X$ 符合高斯分布，那么也就是如下所示($dt$移动了一下)：

每个状态 $x_k$ 都符合高斯分布，那么对高斯分布进行 $\mathrm{d}t$ f范围的积分不久可以了吗？但是这样积分是不行的，因为你积分出来有什么意义？既然是高斯分布，那么他的期望不就是他的均值吗？，结果不依旧是 $x_0$ 到 $x_k$ 吗？为什么，因为你本质上只使用到了递推公式 $f(x)$，少了观测的参与，下面的图示，更加符合实际：

因为观测 $y$ 的存在，每个状态分布都会进行调整，调整之后再进行积分。为了简单方便积分，所以即使通过观测 $y$ 调整之后得分布依旧为高斯分布。$Y$ 轴观测对状态的修正结果通常类似于下图(两个高斯函数的乘积)：

其中红色部分就是修正之后的结果，可以看出其是介于预测状态与观测状态两者之间的，后续会详细推导这个过程。

也就是是说，对于卡尔曼滤波来说，直观看到状态轴(图示x)，其应该是离散的，所以使用离散的方式进行建模，如 $x_k=f{x}_{k-1}$，由这个时刻的状态递推到下个时刻(因为通常来说，$k$ 不是连续的)。但是再处理每个时刻观测的时候，通常是需要连续建模的，如高斯概率密度函数就是连续，上图就是一个示例。

$总结:$ 通常来说，实际应用的卡尔曼滤波(或贝叶斯等)，从状态轴 $x$ 来看，通常都是离散的。但是对于每个状态处理时，若有观测 $y$ 参与，则通常会涉及到连续处理。$仅供参考，结论：$ 卡尔曼滤波即包含了离散，也融入了连续。这就是我的个人体会。所以，那些博客或者视频教材等，说是连续或离散都是正确的，或者说，都是错误的。

四、总结

到目前为止，对于卡尔曼滤波的了解应该是比较深入了，虽然仅仅局限于一维。且前面还有太多的疑惑没有具体分析，如：

①高斯分布分布负无穷到正无穷的积分为什么是1？
② 高斯分布经过线性变换为什么还是高斯分布？
③ 两个高斯分布函数的乘积为什么依旧是高斯分布？
④ 多维(多元)卡尔曼滤波应该如何推导与编程？
…

如果再加上后面还要分析扩展卡尔曼滤波(EKF)、误差状态卡尔曼滤波 (ESKF)、粒子滤波、迭代卡尔曼滤波等。任务量，确实比较庞大，不过没有关系，踏踏实实走好每一步，总会到终点的。