卡尔曼滤波从理论到实践(&&C++代码)详细解析

文章目录

• • 概要
  • 一、卡尔曼滤波基本知识
  • 二、卡尔曼滤波基本模型
  • 三、卡尔曼滤波公式推导
  • 四、卡尔曼滤波的详细理解
  • 五、卡尔曼滤波的应用案例
  • 六、卡尔曼滤波C++代码实现
  • 七、总结

概要

出自：

自动驾驶感知融合-卡尔曼及扩展卡尔曼滤波(Lidar&Radar)-CSDN博客

【自动驾驶】学习卡尔曼滤波（一）——线性卡尔曼滤波-CSDN博客

1 卡尔曼滤波基本概念

        卡尔曼滤波(Kalman Filter)使用随时间推移观察到的一系列测量值（包含统计噪声和其他误差），生成未知变量的估计值，该估计值往往更多通过估计每个时间范围内变量的联合概率分布，结果因此比仅基于单个测量的结果更准确。简单来说说，卡尔曼滤波器是一种最优估计算法，它能够从一系列的不完全及包含噪声的测量中，估计动态系统的状态。

        在自动驾驶领域，卡尔曼滤波器广泛被用于通过特征级融合激光雷达和毫米波雷达的数据，估计汽车的位置和速度。不同的传感器提取得到的测量结果如距离，速度，角度，因为受到信号漂移或噪声的影响，往是不准确的。而卡尔曼滤波为我们提供了一种将来自不同传感器的测量结果与预测位置的数学模型相结合的方法。它根据我们对一个特定传感器或模型的信任程度，来更新测量值和预估值之间的可信权重，从而获得对准确位置的最佳估计。

2 关于KF EKF UKF的区别

        卡尔曼滤波（Kalman filtering, KF）是一种利用线性系统状态方程，通过系统输入输出观测数据，对系统状态进行最优估计的算法。
        卡尔曼滤波的一个典型实例是从一组有限的，对物体位置的，包含噪声的观察序列中预测出物体的坐标位置及速度。在很多工程应用（雷达、计算机视觉）中都可以找到它的身影。同时，卡尔曼滤波也是控制理论以及控制系统工程中的一个重要话题。
        为了解决普通KF仅能处理线性数据的问题，从而就有了用于解决非线性数据的基于KF的推展：拓展卡尔曼滤波（EKF）和无迹卡尔曼滤波（UKF）。KF主要应用于系统方程和量测方程为线性的的场景，EKF应用于轻度非线性场景，而UKF则在强非线性情况下有更好的表现。

3 卡尔曼滤波的步骤

        卡尔曼滤波器的操作包括两个阶段：预测与更新。在预测阶段，滤波器使用上一状态的估计结果，做出对当前状态的估计。在更新阶段，滤波器利用当前状态的观测值优化在预测阶段获得的预估值，以获得一个当前阶段更精确的新估计值。

        每当我们从传感器获取新数据时，我们都会重复这两个步骤，不断更新位置状态估计值和误差协方差矩阵P的准确性。

---

一、卡尔曼入门基本知识

出自：精通（教你从理论到实践）_哔哩哔哩_bilibili

1 图中说明：

• 目标距离：5m
• 期望数据：图中以黑色原点的直线，值均为5m
• 红色线数据代表: 超声波传过来的距离数据，有波动
• 滤波数据：紫色线代表滤波数据，使信号更趋于真实值，去除噪声

---

二、卡尔曼滤波基本模型

补充理解：出自：卡尔曼滤波（Kalman Filter）原理与公式推导 - 知乎

注意：

wk 为过程噪声，服从正态分布，均值为0，方差为Qk；

vk 为观测噪声，服从正态分布，均值为0，方差为Rk；

关于4卡尔曼直观图解可以看出：

橙色曲线的正态分布是先验估计，紫色曲线的正态分布是当前的观测值，红色曲线是当前的最优估计值，其是前两者公有的部分组成。

三、卡尔曼滤波公式推导

注意:

6中的后验估计协方差对应的是4中的修正估计的协方差

注意：

I 公式层面理解 关于 Q R的取值

假设F是一维，也即1，化简K可得

如果信任观测值，则K要增大，也即使得化简后K的分子大一点或者分母小一点，如把R调小，则K增大，信任观测值；

如果运动模型很理想，则可以调小K，R和Q可以调大，则K减小。

II 其他层面理解 Q R 取值

当模型较理想，过程噪声很小，Q可以取小点；否则，取大点；

当观测传感器精度较高，则R取小一点，否则取大点；

关于协方差的求解基础知识：

其中cov(Ax,Ax) = cov(Ax+K,Ax+K)

关于2式先验估计协方差怎么求得：

补充：

四、卡尔曼滤波的详细理解

补充：

这里需要注意的是：

公式1先验估计基于公式4的后验最优估计值，作为下一次先验预测的输入。

公式5更新误差协方差是为了求出当前后验的协方差Pk，以变作为下一次预测的公式2求先验的协方差的输入Pk-1。

其次，要给预测的1和2方程赋初始值x^0, P0

---

五、卡尔曼滤波应用案例

此处K是二维，因为其有两个状态

案例3：【卡尔曼滤波器】5_直观理解与二维实例【包含完整的EXCEL代码】_哔哩哔哩_bilibili

卡尔曼滤波核心公式：

python版代码实现参见：

GitHub - liuchangji/2D-Kalman-Filter-Example_Dr_CAN_in_python

卡尔曼增益K：负责融合估计值与测量值，谁方差小，就更相信谁一些。
状态估计协方差矩阵P：初始状态与过程噪声有关，并且由于每次K的迭代，状态估计协方差矩阵也在迭代（因为使用了上一次的结果作为下一次的初始状态，上一次的结果来自于卡尔曼增益K对观测与估计的融合，状态估计协方差会减小）
测量误差协方差矩阵R：来自于传感器误差（我猜是可以通过试验获得，比如测量100次，然后记录数据）
过程噪声协方差矩阵Q：来自于世界中的不确定性，这个值怎么选，我也不太清楚，不知道如何度量世界中的不确定性。
其余的就是要给一个目标的初始状态，然后根据观测，不断地更新估计，得到一个稳定的K。

# https://zhuanlan.zhihu.com/p/48876718
# https://www.bilibili.com/video/BV1dV411B7ME?share_source=copy_web
# DR_CAN例子的python实现
import numpy as np
import matplotlib.pyplot as plt


def gaussian_distribution_generator(var):
    return np.random.normal(loc=0.0, scale=var, size=None)


# 状态转移矩阵，上一时刻的状态转移到当前时刻
A = np.array([[1, 1],
              [0, 1]])

# 过程噪声协方差矩阵Q，p(w)~N(0,Q)，噪声来自真实世界中的不确定性
Q = np.array([[0.1, 0],
              [0, 0.1]])

# 观测噪声协方差矩阵R，p(v)~N(0,R)
R = np.array([[1, 0],
              [0, 1]])

# 状态观测矩阵
H = np.array([[1, 0],
              [0, 1]])

# 控制输入矩阵B
B = None
# 初始位置与速度
X0 = np.array([[0],
               [1]])

# 状态估计协方差矩阵P初始化
P = np.array([[1, 0],
              [0, 1]])

if __name__ == "__main__":
    # ---------------------------初始化-------------------------
    X_true = np.array(X0)  # 真实状态初始化
    X_posterior = np.array(X0)
    P_posterior = np.array(P)

    speed_true = []
    position_true = []

    speed_measure = []
    position_measure = []

    speed_prior_est = []
    position_prior_est = []

    speed_posterior_est = []
    position_posterior_est = []

    for i in range(30):
        # -----------------------生成真实值----------------------
        # 生成过程噪声
        w = np.array([[gaussian_distribution_generator(Q[0, 0])],
                      [gaussian_distribution_generator(Q[1, 1])]])
        X_true = np.dot(A, X_true) + w  # 得到当前时刻状态
        speed_true.append(X_true[1, 0])
        position_true.append(X_true[0, 0])
        # -----------------------生成观测值----------------------
        # 生成观测噪声
        v = np.array([[gaussian_distribution_generator(R[0, 0])],
                      [gaussian_distribution_generator(R[1, 1])]])

        Z_measure = np.dot(H, X_true) + v  # 生成观测值,H为单位阵E
        position_measure.append(Z_measure[0, 0])
        speed_measure.append(Z_measure[1, 0])
        # ----------------------进行先验估计---------------------
        X_prior = np.dot(A, X_posterior)
        position_prior_est.append(X_prior[0, 0])
        speed_prior_est.append(X_prior[1, 0])
        # 计算状态估计协方差矩阵P
        P_prior_1 = np.dot(A, P_posterior)
        P_prior = np.dot(P_prior_1, A.T) + Q
        # ----------------------计算卡尔曼增益,用numpy一步一步计算Prior and posterior
        k1 = np.dot(P_prior, H.T)
        k2 = np.dot(np.dot(H, P_prior), H.T) + R
        K = np.dot(k1, np.linalg.inv(k2))
        # ---------------------后验估计------------
        X_posterior_1 = Z_measure - np.dot(H, X_prior)
        X_posterior = X_prior + np.dot(K, X_posterior_1)
        position_posterior_est.append(X_posterior[0, 0])
        speed_posterior_est.append(X_posterior[1, 0])
        # 更新状态估计协方差矩阵P
        P_posterior_1 = np.eye(2) - np.dot(K, H)
        P_posterior = np.dot(P_posterior_1, P_prior)

       

    # 可视化显示
    if True:
        fig, axs = plt.subplots(1,2)
        axs[0].plot(speed_true, "-", label="speed_true", linewidth=1)  # Plot some data on the axes.
        axs[0].plot(speed_measure, "-", label="speed_measure", linewidth=1)  # Plot some data on the axes.
        axs[0].plot(speed_prior_est, "-", label="speed_prior_est", linewidth=1)  # Plot some data on the axes.
        axs[0].plot(speed_posterior_est, "-", label="speed_posterior_est", linewidth=1)  # Plot some data on the axes.
        axs[0].set_title("speed")
        axs[0].set_xlabel('k')  # Add an x-label to the axes.
        axs[0].legend()  # Add a legend.

        axs[1].plot(position_true, "-", label="position_true", linewidth=1)  # Plot some data on the axes.
        axs[1].plot(position_measure, "-", label="position_measure", linewidth=1)  # Plot some data on the axes.
        axs[1].plot(position_prior_est, "-", label="position_prior_est", linewidth=1)  # Plot some data on the axes.
        axs[1].plot(position_posterior_est, "-", label="position_posterior_est", linewidth=1)  # Plot some data on the axes.
        axs[1].set_title("position")
        axs[1].set_xlabel('k')  # Add an x-label to the axes.
        axs[1].legend()  # Add a legend.

        plt.show()

---

六、卡尔曼入滤波C++代码实现

案例背景：出自：图说卡尔曼滤波（C++实现）_c++卡尔曼滤波-CSDN博客

C++ 代码实现详细解析

/* #include "kalman_filter.hpp"

int main(){
    kalman_filter kal;
    kal.calu(5,6);
} */

#include <Eigen/Dense> //包含Eigen矩阵运算库，用于矩阵计算
#include <cmath>
#include <fstream>
#include <iostream>
#include <limits> //用于生成随机分布数列
#include <string>
#include <vector>

/* std::numeric_limits<double>::infinity();   	// 返回 double的正无穷大
std::numeric_limits<int>::min();			// 返回 int的最小值
std::numeric_limits<int>::max();			// 返回 int的最大值
std::numeric_limits<int>::is_signed;		// 判断类型有无符号，返回bool
std::numeric_limits<int>::has_infinity;		// 判断类型是否存在正无穷的表示
std::numeric_limits<double>::epsilon();		// 返回
1与可表示的大于1的最小值之间的差 近似的0 */

using namespace std;
using Eigen::MatrixXd;

int main() {
  ofstream fout("..//result.txt"); //""中是txt文件路径，注意：路径要用//隔开

  double generateGaussianNoise(double mu,
                               double sigma); //随机高斯分布数列生成器函数

  const double delta_t = 0.1; //控制周期，100ms  
  const int num = 100; //迭代次数   
  const double acc = 10; //加速度，ft/m

  MatrixXd A(2,2);//Eigen::MatrixXd matrix(rows, cols); // 创建一个指定大小的矩阵，所有元素初始化为0
  A(0,0) = 1;
  A(0,1) = delta_t;
  A(1,0) = 0;
  A(1,1) = 1;  

  MatrixXd B(2,1);
  B(0,0) = pow(delta_t,2)/2;
  B(1,0) = delta_t;

  MatrixXd H(1,2); //测量的是小车的位移，速度为0
  H(0,0) = 1;
  H(0,1) = 0;  

//过程激励噪声协方差，假设系统的噪声向量只存在速度分量上，且速度噪声的方差是一个常量0.01，位移分量上的系统噪声为0
  MatrixXd Q(2,2);
  Q(0,0) = 0;  //位移方差
  Q(0,1) = 0;
  Q(1,0) = 0;
  Q(1,1) = 0.01; 

//观测噪声协方差，测量值只有位移，它的协方差矩阵大小是1*1，就是测量噪声的方差本身
  MatrixXd R(1,1);
  R(0,0) = 10;

//time初始化，产生时间序列
 vector<double>time(100,0);  //指定值初始化，time被初始化为包含100个值为0的double

 //不论decltype后的“（）”中写的多么复杂，最后decltype()返回的结果必定只是一个类型而已。
//  decltype(10) a;            //好比书写int a;
//  decltype('Q') b = 'b';    //好比书写char b = 'b';

 for (decltype(time.size()) i = 0; i != num; ++i)
 {
     time[i] = i*delta_t;
 }

 MatrixXd X_real(2,1);
 vector<MatrixXd>x_real,rand; //类型是一个动态矩阵


//生成高斯分布的随机数
 for (int i = 0; i < 100; ++i) {
   MatrixXd a(1, 1);
   a(0, 0) = generateGaussianNoise(0, sqrt(10));
   rand.push_back(a); //100行1列的随机数

   //for (auto i:rand) {  //打印矩阵rand
    //  cout << i << endl;
    // cout<< rand.size() << endl;
   //}
 }


//生成真实的位移值
 for (int i = 0; i < 100; ++i) {
   X_real(0, 0) = 0.5 * acc * pow(time[i], 2);
   X_real(1, 0) = 0;
   x_real.push_back(X_real);  //将数组2x1的矩阵X_real逐次整体放入x_real中，最终x_real中有100个2x1的小矩阵
}

//变量定义，包括状态预测值，状态估计值，测量值，预测状态与真实状态的协方差矩阵，估计状态和真实状态的协方差矩阵，初始值均为零
 //MatrixXd::Constant(n, n, a):新建一个nxn常量矩阵，元素值用a去赋值；
 

//  MatrixXd X_evlt1 = MatrixXd::Constant(2,1,0); //新建一个2x1常量矩阵，元素值全为0
//  cout<< X_evlt1 << endl;

 MatrixXd X_evlt = MatrixXd::Constant(2, 1, 0),
          X_pdct = MatrixXd::Constant(2, 1, 0),
          Z_meas = MatrixXd::Constant(1, 1, 0),
          Pk = MatrixXd::Constant(2, 2, 0), 
          Pk_p = MatrixXd::Constant(2, 2, 0),
          K = MatrixXd::Constant(2, 1, 0); //卡尔曼增益 对应两个状态 2x1

//初始化矩阵参数
 vector<MatrixXd> x_evlt, x_pdct, z_meas, pk, pk_p, k;
 x_evlt.push_back(X_evlt); //后验最优估计初始值 2x1的矩阵，初始值为0

/*  for (auto i : x_evlt) {   //打印矩阵x_real
   cout << i << endl;
 } */
 x_pdct.push_back(X_pdct);//先验估计初始值
 z_meas.push_back(Z_meas);//测量初始值
 pk.push_back(Pk);//后验估计的协方差初始值
 pk_p.push_back(Pk_p);//先验估计协方差初始值
 k.push_back(K);


//开始迭代,卡尔曼滤波的核心五个公式
 for (int i = 1; i < num; ++i) {
   
   预测

   //预测值,也即先验估计,以上一时刻的后验证估计作为本次输入
   X_pdct = A * x_evlt[i - 1] + B * acc; //由状态方程可知，系统输入是加速度。
   x_pdct.push_back(X_pdct); //将预测值存到容器x_pdct

   //预测状态与真实状态的协方差矩阵，也即先验估计协方差，以上一时候的后验协方差作为本次输入，
   Pk_p = A * pk[i - 1] * A.transpose() + Q;
   pk_p.push_back(Pk_p);


   ///更新

   //更新卡尔曼增益
   MatrixXd tmp(1, 1);//一个值
   tmp = H * pk_p[i] * H.transpose() + R;
   K = pk_p[i]*H.transpose()*tmp.inverse();//逆
   k.push_back(K);

   //测量值z,只测量位置。
   Z_meas = H*x_real[i] + rand[i];  //真实值*H + 测量噪声
   z_meas.push_back(Z_meas);

   //后验估计值或修正估计
   X_evlt = x_pdct[i] + k[i]*(z_meas[i]-H*x_pdct[i]);
   x_evlt.push_back(X_evlt);  //循环结束，x_evlt矩阵容器(100x1)中存放100个的2x1的矩阵

//    for (auto i : x_evlt) { //打印矩阵x_evlt
//     //  cout << i << endl;
//     //  cout << x_evlt.size() << endl;
//    }

   //估计状态和真实状态的协方差矩阵，Pk,也即后验证协方差
   Pk = (MatrixXd::Identity(2,2)-k[i]*H)*pk_p[i];  //MatrixXd::Identity(rows,cols) // eye(rows,cols) 单位矩阵
   pk.push_back(Pk);
}


  cout<< " 含噪声的测量值 " <<  " 后验估计值 " << " 真值 " << endl;
  for (int i = 0; i < num; i++)
  {
      cout<< z_meas[i] << " " << x_evlt[i](0,0) << " " << x_real[i](0,0) << endl;  //x_evlt[i](0,0)为第i行的2x1矩阵的第一个位置值
      fout << z_meas[i] << " " << x_evlt[i](0,0) << " " << x_real[i](0,0) << endl; //将数据写入到文本中
  }
  
  fout.close();
  return 0;
}


double generateGaussianNoise(double mu, double sigma) {
  const double epsilon = std::numeric_limits<double>::min();
//   cout << "epsilon = " << epsilon << endl;
  const double two_pi = 2.0 * 3.1415926;

  static double z0, z1;
  static bool generate;
  generate = !generate;

  if (!generate) {
    return z1 * sigma + mu;
  }
  double u1, u2;
  // while循环和for循环都是先判断，如果条件满足就进入循环，执行循环语句，如果条件不满足就跳出循环。

  //而do
  // while循环则是先进入循环体，执行循环语句后再对判断while（）中表达式，如果表达式为真则继续进入循环体，表达式为假则循环结束。

  do {
    u1 = rand() * (1.0 / RAND_MAX);
    u2 = rand() * (1.0 / RAND_MAX);
  } while (u1 < epsilon);

  z0 = sqrt(-2.0 * log(u1)) * cos(two_pi * u2);
  z1 = sqrt(-2.0 * log(u1)) * sin(two_pi * u2);
  return z0 * sigma + mu;
}

使用python画图：参考：Python读取.csv/.txt文件并画图-CSDN博客

ax1.plot(data[:,0], color='green',  linewidth=2, label="z_meas")
ax1.plot(data[:,1],color='red',  linewidth=2,label="x_evlt")
ax1.plot(data[:,2],color='blue',  linewidth=2,label="x_real")

输出结果图：

调参经验

(a) 一开始预测不准，则更相信测量值，增大初始的Pk->增大Pk`->k->更相信测量值

(b) 状态方程建立不正确，相当于过程噪声增大了，所以这个时候应该增大过程噪声的方差Q

---

七、总结

        本文主要参考下述博客对卡尔曼滤波的原理，公式推导以及实践进行了整合，仅作为学习总结记录，切勿转载，详细理解请参考对应的参考博客！

参考

1 卡尔曼滤波（Kalman Filter）原理与公式推导 - 知乎

2 【自动驾驶】学习卡尔曼滤波（一）——线性卡尔曼滤波-CSDN博客

3  自动驾驶感知融合-卡尔曼及扩展卡尔曼滤波(Lidar&Radar)-CSDN博客

4  精通（教你从理论到实践）_哔哩哔哩_bilibili

5  【卡尔曼滤波器】5_直观理解与二维实例【包含完整的EXCEL代码】_哔哩哔哩_bilibili