【视觉SLAM入门】7.1. 快速从0上手卡尔曼滤波，二维原理及代码(C++,Eigen库，Cmake工程实现，全部源码)

终问鼎

已于 2023-09-06 17:12:48 修改

阅读量853

点赞数 3

分类专栏：自动驾驶-SLAM # VSLAM 文章标签：矩阵 SLAM 滤波目标跟踪算法

于 2023-08-31 22:41:43 首次发布

本文链接：https://blog.csdn.net/Eric_Sober/article/details/132611068

版权

自动驾驶-SLAM 同时被 2 个专栏收录

37 篇文章 23 订阅

订阅专栏

VSLAM

16 篇文章 1 订阅

订阅专栏

"录诗误字，啮臂流血"

0. 简单认识
1. 公式解读
2. 应用举例
3.解决方案(公式--代码对应 )
4. 源码

前置事项： 原理部分大佬讲的很详细原理。

0. 简单认识

想象一个场景：实际场景中，假设有一个小车在做匀速直线运动，你能读到他的速度和位置。
信息1：虽然但是实际中读到的速度和位置传感器的数值带有误差；
信息2：下一秒的位置理论上可以通过 $s_{pre}+v\Delta t$ 计算出来，但是实际中肯定不会这么完美。
$如何充分利用这两个信息，以得到尽可能准确的速度和位置信息？$
卡尔曼滤波出来了，信息1我们称作观测信息；信息2称作预测的信息。通过给这两个信息权重计算出最终信息。
此之(#`O′)—卡尔曼滤波

1. 公式解读

(这一部分用来劝退，不能看懂记住如下5个公式就好，其实很简单)

有人说5个公式，有人说7个，实际上是一样的，只是7个的为了简化将一部分重新命名了而已。这里以5个为例，上边两个是预测，下边三个是更新

根据上一时刻信息预测此时的信息()： $\hat{x}_{\bar{k}} = A_k\hat{x}_{{k-1}} + B_ku_k$
根据上一刻信息预测此刻该有的协方差矩阵： $P_{\bar k} = A_k P_{k-1} A_k^T + Q$
求卡尔曼增益： $K_t = \frac{P_{\bar k} H^T_k} {H_k P_{\bar k} H_k^T + R}$
用观测数据更新在步骤1中得到状态： $\hat x_k = \hat x_{\bar k} + K_t(z_k - H_k \hat x _{\bar k})$
用卡尔曼增益更新步骤2中得到的协方差矩阵： $P_k = (I - K_t H_k) P_{\bar k}$

解释：

步骤1中： $A_k$ 称为状态转移矩阵， $B_k$ 叫做控制矩阵。相应的 $\hat{x}_{{k-1}}$ 是上一时刻的状态，而 $u_k$ 是控制输入，实际上这里用 $u_{k-1}$ 较为妥当；
；
步骤2中： $P$ 是协方差矩阵，就是状态向量x中的向量两两作协方差，通俗的讲就是用来衡量系统不确定程度。表示系统对状态转移矩阵中某个量更可信则对应的数值低，不可信则大，一开始会比较大，但数据越累越多该值越来越低； $Q$ 是无法用状态转移矩阵衡量的误差，称为过程噪声
；
步骤3中， $H$ 是测量矩阵，它的意义，比如我们状态向量中设置的是速度即 $v_x, v_y]$ ，但是我们测量是用不同传感器测量的，如果用陀螺仪则我们得到的是三个轴的速度 $v_x, v_y, v_z]$ ，为了和状态向量对齐，此时我们的测量矩阵应该是 $[1, 1, 0]$ ，说白了就是实际和理论对齐的东西；这里的 $R$ 是测量噪声矩阵，表示测量值和真实值的误差，一般厂家会给定。
；
步骤4，再更新状态量，新的状态量由预测和观测和权值(卡尔曼增益 $K_t$ )共同决定；
；
步骤5，重新更新系统不确定度，新的系统不确定度随着数据的输入而变小。

2. 应用举例

假设场景：

一块静止的单线激光雷达暴露在室内，室内有一个在运动的物体。现在可以拿到激光雷达数据 $[x, y]$ ，该物体的x 和 y 坐标，由于传感器误差，测得的值有一定误差；此外物体做匀速运动，但是由于地面摩擦存在，它的运动也非绝对匀速运动。现在要比较精确的求物体的坐标 $x_{re}, y_{re}]$ 。

下边利用以上5个公式和C++实现用KF来求解这个问题。

3.解决方案(公式–代码对应 )

首先写出系统的观测量，根据题设，可以观测到的量如下：
$\begin{bmatrix}x \\y \end{bmatrix}$
写出系统的观测量，为了更好利用所有以上信息，设状态量如下：
$\begin{bmatrix}x \\y \\v_x \\v_y \end{bmatrix}$

3.1 初始化

xXd表示非定长，为了其他函数也能调用。

class KalmanFilter
{       
    public:
        KalmanFilter(){
            is_initialized_ = false ;
        };
        ~KalmanFilter();

        void Initialization(Eigen::VectorXd x_in){
            x_ = x_in ;
        }
        bool IsInitialized(){
            return this->is_initialized_ ;
        }
private:
        bool is_initialized_ ;    // flag of initialized
        Eigen::VectorXd x_ ;      // state vector
}

3.2 卡尔曼滤波过程

3.2.1. 预测—状态量

该物体的理想状态向量应该由速度和位置组成，不妨设理想的计算值为 $x = [x, y, v_x, v_y]^T$
而对于匀速运动模型有：
$\hat{x}_{\bar{k}} = \begin{bmatrix} \hat{x}_{{k-1}} + v_{\hat{x}_{{k-1}}} \cdot \Delta t \\ \hat{y}_{{k-1}} + v_{\hat{y}_{{k-1}}} \cdot \Delta t \\ v_{\hat{x}_{{k-1}}} \\ v_{\hat{y}_{{k-1}}} \end{bmatrix}$
匀速运动 : $u = 0$ ，这里的u代表的加速减速作用

故按照上述推理，根据 $\hat{x}_{\bar{k}} = A_k\hat{x}_{{k-1}} + B_ku_k$ 进行展开，有

$\begin{bmatrix} \hat{x}_{{k-1}} + v_{\hat{x}_{{k-1}}} \cdot \Delta t \\ \hat{y}_{{k-1}} + v_{\hat{y}_{{k-1}}} \cdot \Delta t \\ v_{\hat{x}_{{k-1}}} \\ v_{\hat{y}_{{k-1}}} \end{bmatrix} = \begin{bmatrix} 1 & 0 & \Delta t & 0 \\ 0 & 1 & 0 & \Delta t \\ 0 & 0 & 1 & 0 \\ 0 & 0 & 0 & 1 \end{bmatrix} \cdot \begin{bmatrix} \hat{x}_{{k-1}} \\ \hat{y}_{{k-1}} \\ v_{\hat{x}_{{k-1}}} \\ v_{\hat{y}_{{k-1}}} \end{bmatrix} + \begin{bmatrix} 0 \\ 0 \\ 0 \\ 0 \end{bmatrix}$

所以这里的状态转移矩阵 $F$ 为上边第二个矩阵，但是由于 $\Delta t$ 大小不定，因此手动输入：

// state transistion matrix for predict x'
        void SetF(Eigen::MatrixXd F_in){
            F_ = F_in ;
        }

3.2.2 预测—系统协方差

根据
$P_{\bar k} = A_k P_{k-1} A_k^T + Q$
这里的 $P$ 是系统的不确定度，我们使用的是激光雷达，由于要运算的状态量是 $x, y, v_x, v_y]^T$ ，对激光雷达系统而言，对位置的信任度(确定度)显然较高，而激光雷达不具有测量速度能力，对速度确定度低。因此可以设置P为(后边会不断更新)：
$\begin{bmatrix} 1 & 0 & 0 & 0 \\ 0 & 1 & 0 & 0 \\ 0 & 0 & 100 & 0 \\ 0 & 0 & 0 & 100 \end{bmatrix}$
$Q$ 是这里的过程噪声，不能用状态转移矩阵表示，比如由于该物体运动时地面摩擦导致的随机噪声，工程上一般设置为单位阵。
$\begin{bmatrix} 1 & 0 & 0 & 0 \\ 0 & 1 & 0 & 0 \\ 0 & 0 & 1 & 0 \\ 0 & 0 & 0 & 1 \end{bmatrix}$
可以写出本部分代码：

        /**********calculate / predict x'************/
        // state transistion matrix for predict x'
        void SetF(Eigen::MatrixXd F_in){
            F_ = F_in ;
        }

        /**********calculate / predict P'************/
        // state covariance matrix for predict x'
        void SetP(Eigen::MatrixXd P_in){
            P_ = P_in ;
        }
        // process covariance matrix for predict x'
        void SetQ(Eigen::MatrixXd Q_in){
            Q_ = Q_in ;
        }
        void Prediction(){
            x_ = F_ * x_ ;
            Eigen::MatrixXd Ft = F_.transpose() ;
            P_ = F_ * P_ * Ft + Q_ ;
        }
    private:
        bool is_initialized_ ;    // flag of initialized
        Eigen::VectorXd x_ ;      // state vector
        Eigen::MatrixXd F_ ;      // state transistion matrix
        Eigen::MatrixXd P_ ;      // state covariance matrix
        Eigen::MatrixXd Q_ ;      // process covariance matrix

3.2.3 更新—H测量矩阵

上边卡尔曼第4个公式，中有一段：

$z_k - H_k \hat x _{\bar k}$

这里的 $z_k$ 实际上是测量值 $z_k = [x_{mk}, y_{mk}]^T$ ，而 $_{\bar k} = [x, y, v_x, v_y]^T$ ，为了使测量数据能够和状态量进行运算，所以引入了 $H$ 矩阵，也称测量矩阵。显然这里应该给：
$\begin{bmatrix} 1 & 0 & 0 & 0 \\ 0 & 1 & 0 & 0 \\ \end{bmatrix}$

只有这样才能计算
$\begin{bmatrix} x_{mk} \\ y_{mk}\end{bmatrix} - \begin{bmatrix} 1 & 0 & 0 & 0 \\ 0 & 1 & 0 & 0 \\ \end{bmatrix}\begin{bmatrix} \hat{x}_{{k-1}} + v_{\hat{x}_{{k-1}}} \cdot \Delta t \\ \hat{y}_{{k-1}} + v_{\hat{y}_{{k-1}}} \cdot \Delta t \\ v_{\hat{x}_{{k-1}}} \\ v_{\hat{y}_{{k-1}}} \end{bmatrix}$

3.2.3 更新—卡尔曼增益K

根据：
$K_t = \frac{P_{\bar k} H^T_k} {H_k P_{\bar k} H_k^T + R}$

这里还差一个 $R$ 矩阵未知，它实际上是这里激光雷达的测量噪声矩阵，表示测量值和真实值之间的差值，一般情况，传感器的厂家会提供该值，比如我这里的：
$\begin{bmatrix} 0.225 & 0 \\ 0 & 0.225 \end{bmatrix}$

卡尔曼增益可得

3.2.4 再更新状态量和系统协方差矩阵

$\hat x_k = \hat x_{\bar k} + K_t(z_k - H_k \hat x _{\bar k}) \\ \;\\ P_k = (I - K_t H_k) P_{\bar k}$

这里再根据卡尔曼增益和测量值更新一下预测值就行了。
以上三节的代码如下

        void MeasurementUpdate(const Eigen::VectorXd &z){
            Eigen::VectorXd y = z - H_ * x_ ;
            Eigen::MatrixXd S = H_ * P_ * H_.transpose() + R_ ;
            Eigen::MatrixXd K = P_ * H_.transpose() * S.inverse() ;
            x_ = x_ + (K * y) ;
            int size = x_.size() ;
            Eigen::MatrixXd I = Eigen::MatrixXd::Identity(size,size) ;
            P_ = (I - K*H_) * P_ ;
        }

至此，我们的卡尔曼滤波函数就写好了。最终在main函数中调用即可。

4. 源码

4.1 KalmanFilter.hpp 库

#ifndef KALMAN_FILTER_H
#define KALMAN_FILTER_H
#include "Eigen/Dense"

class KalmanFilter
{       
    public:
        KalmanFilter(){
            is_initialized_ = false ;
        };
        ~KalmanFilter();

        void Initialization(Eigen::VectorXd x_in){
            x_ = x_in ;
        }

        bool IsInitialized(){
            return this->is_initialized_ ;
        }

        /**********calculate / predict x'************/
        // state transistion matrix for predict x'
        void SetF(Eigen::MatrixXd F_in){
            F_ = F_in ;
        }

        /**********calculate / predict P'************/
        // state covariance matrix for predict x'
        void SetP(Eigen::MatrixXd P_in){
            P_ = P_in ;
        }
        // process covariance matrix for predict x'
        void SetQ(Eigen::MatrixXd Q_in){
            Q_ = Q_in ;
        }

        void SetH(Eigen::MatrixXd H_in){
            H_ = H_in ;
        }
        void SetR(Eigen::MatrixXd R_in){
            R_ = R_in ;
        }

        void Prediction(){
            x_ = F_ * x_ ;
            Eigen::MatrixXd Ft = F_.transpose() ;
            P_ = F_ * P_ * Ft + Q_ ;
        }

        void MeasurementUpdate(const Eigen::VectorXd &z){
            Eigen::VectorXd y = z - H_ * x_ ;
            Eigen::MatrixXd S = H_ * P_ * H_.transpose() + R_ ;
            Eigen::MatrixXd K = P_ * H_.transpose() * S.inverse() ;
            x_ = x_ + (K * y) ;
            int size = x_.size() ;
            Eigen::MatrixXd I = Eigen::MatrixXd::Identity(size,size) ;
            P_ = (I - K*H_) * P_ ;
        }

        Eigen::VectorXd GetX(){
            return x_ ;
        }

    private:
        bool is_initialized_ ;    // flag of initialized
        Eigen::VectorXd x_ ;      // state vector
        Eigen::MatrixXd F_ ;      // state transistion matrix
        Eigen::MatrixXd P_ ;      // state covariance matrix
        Eigen::MatrixXd Q_ ;      // process covariance matrix
        Eigen::MatrixXd H_ ;      // mesurement matrix
        Eigen::MatrixXd R_ ;      // mesurement covariance matrix

};
KalmanFilter::~KalmanFilter()
{
}

#endif

4.2 main.cpp 使用卡尔曼滤波

#include "kalman_filter.hpp"
#include <iostream>
#include <fstream>
#include <vector>

namespace plt = matplotlibcpp;

//read the data be created in ./data_script
bool GetLidarData(double& x, double& y, double& time){
    static bool is_read_file_finished = false ;
    static std::vector<double> file_x, file_y, file_time;
    static std::string line, value;

    if(!is_read_file_finished){
        is_read_file_finished = true ;
        std::string filename = "../data_script/file.txt";
        std::ifstream file(filename);

        while (std::getline(file, line)) {
                std::stringstream ss(line);
                std::getline(ss, value, ',');
                file_x.push_back(std::stod(value));
                std::getline(ss, value, ',');
                file_y.push_back(std::stod(value));
                std::getline(ss, value, ',');
                file_time.push_back(std::stod(value));
        }
    }
    if (file_time.size()>0){
                x = file_x.front() ;
                file_x.erase(file_x.begin()) ;

                y = file_y.front() ;
                file_y.erase(file_y.begin()) ;

                time = file_time.front() ;
                file_time.erase(file_time.begin()) ;
                return true ;
        } else {
                return false ;
        }  
}

int main(int argc, const char** argv) {
    double m_x = 0.0, m_y = 0.0 ;
    double last_timestamp = 0.0, now_timestamp = 0.0 ;
    KalmanFilter KF ;
    while (GetLidarData(m_x, m_y, now_timestamp))
    // Initialize kalman filter
    {

        // Initialize P,Q,H,R
        if (!KF.IsInitialized())
        {
            last_timestamp = now_timestamp ;
            Eigen::VectorXd x_in(4,1) ;
            x_in << m_x, m_y, 0.0, 0.0 ;
            KF.Initialization(x_in) ;
            // state convarince matrix
            Eigen::MatrixXd P_in(4, 4) ;
            P_in << 1.0,   0.0,   0.0,   0.0, 
                    0.0,   1.0,   0.0,   0.0, 
                    0.0,   0.0,   100.0, 0.0, 
                    0.0,   0.0,   0.0,   100.0 ;

            KF.SetP(P_in) ;

            // process convariance matrix
            Eigen::MatrixXd Q_in(4, 4) ;
            Q_in << 1.0,  0.0,  0.0,  0.0, 
                    0.0,  1.0,  0.0,  0.0, 
                    0.0,  0.0,  1.0,  0.0, 
                    0.0,  0.0,  0.0,  1.0 ;
            KF.SetQ(Q_in) ;

            // measurement matrix
            Eigen::MatrixXd H_in(2, 4) ;
            H_in << 1.0, 0.0, 0.0, 0.0, 
                    0.0, 1.0, 0.0, 0.0 ;
            KF.SetH(H_in) ;

            // measurement convarince matrix
            //R is provided by Sensor producer, in their datasheet
            Eigen::MatrixXd R_in(2, 2) ;
            R_in << 0.0225, 0.0, 
                    0.0,    0.0225 ;
            KF.SetR(R_in) ;
        }
        
        //state transition matrix
        double delta_t = now_timestamp - last_timestamp ;
        last_timestamp = now_timestamp ;
        Eigen::MatrixXd F_in(4, 4) ;
        F_in << 1.0,    0.0,    delta_t, 0.0, 
                0.0,    1.0,    0.0,     delta_t, 
                0.0,    0.0,    1.0,     0.0, 
                0.0,    0.0,    0.0,     1.0 ;
        KF.SetF(F_in) ;

        KF.Prediction() ;
        // measurement value
        Eigen::VectorXd z(2, 1) ;
        z << m_x, m_y ;
        KF.MeasurementUpdate(z) ;

        //result
        Eigen::VectorXd x_out = KF.GetX() ;
        std::cout << "original output x : " << m_x <<
                                    " y: " << m_y << std::endl ;
        std::cout << "kalman output x : " << x_out(0) <<
                                    " y: " << x_out(1) << std::endl ;

    }


    return 0;
}

这里激光雷达存放在file.txt文件中，数据格式是 x, y, time ,逐行

4.3 CMakeLists.txt文件构建cmake工程

# CMakeLists.txt  
  
# 设置构建类型为 Release  
cmake_minimum_required(VERSION 3.10)  
project(common_kf)  
  
set(CMAKE_CXX_STANDARD 14) # 使用 C++14 标准  
  

# 添加当前目录到 include 路径  
include_directories(${PROJECT_SOURCE_DIR}/include)  

# 添加源文件到编译  
add_executable(KalDemo main.cpp)

终问鼎

关注

3
点赞
踩
10

收藏

觉得还不错? 一键收藏
0
评论
【视觉SLAM入门】7.1. 快速从0上手卡尔曼滤波，二维原理及代码(C++,Eigen库，Cmake工程实现，全部源码)

经典卡尔曼滤波算法的认识，理解，从理论到落地，通俗易懂，从原理到实现，举了二维的滤波代码为例，从0开始对应公式用C++最终实现卡尔曼滤波，构建CMAKE工程，全部源码公开，逐行解释。
复制链接

扫一扫

专栏目录