五.卡尔曼滤波器(EKF)开发实践之五: 编写自己的EKF替换robot_pose_ekf中EKF滤波器

本系列文章主要介绍如何在工程实践中使用卡尔曼滤波器,分七个小节介绍:

一.卡尔曼滤波器开发实践之一: 五大公式

二.卡尔曼滤波器开发实践之二:  一个简单的位置估计卡尔曼滤波器    

三.卡尔曼滤波器(EKF)开发实践之三:  基于三个传感器的海拔高度数据融合  

四.卡尔曼滤波器(EKF)开发实践之四:  ROS系统位姿估计包robot_pose_ekf详解

五.卡尔曼滤波器(EKF)开发实践之五:  编写自己的EKF替换robot_pose_ekf中EKF滤波器  也就是本文

六.卡尔曼滤波器(UKF)开发实践之六: 无损卡尔曼滤波器(UKF)进阶-白话讲解篇

七.卡尔曼滤波器(UKF)开发实践之七: 无损卡尔曼滤波器(UKF)进阶-实例篇

--------------------------------正文开始------------------------------------------

        有了前四节的介绍,我们对扩展的卡尔曼滤波器(EKF)的实践应用也有了进一步的认识,学以致用, 我们的最终目的是应用到我们的工作实践中. 到了用自己的EKF知识去做点实际的工作的时候了.        

        废话不多说了,我们在ROS系统中,新建一个命名为my_robot_pose_ekf的包,有关ros的入口程序完全使用原robot_pose_ekf的入口代码. 重点是我们去除原包里所有BFL相关代码.  并直接替换我们自己的EKF滤波器代码,然后编译运行,看结果.

        这里直接贴代码和运行效果图:

一. C++版的TinyEKF核心基类:

#ifndef __TINY_EKF_INCLUDE__
#define __TINY_EKF_INCLUDE__

#include <bfl/wrappers/matrix/matrix_wrapper.h>

using namespace MatrixWrapper;

namespace ekf
{

    class TinyEKF
    {
    public:
        TinyEKF(int numOfState, int numOfMeasurement, float pVal, float qVal, float rVal);
        ~TinyEKF();

        void updateRk(SymmetricMatrix &newRk);
        void setAdditiveMeasureNoiseRk(SymmetricMatrix &noiseMatrix);
        void getAdditiveMeasureNoiseRk(SymmetricMatrix &noiseMatrix);

        void setInitState(ColumnVector& _x_k, SymmetricMatrix& p_current);

        void getPostCovariance(SymmetricMatrix &matrix);

        //输入当前测量值,  返回新的最优评估值
        void doStep(ColumnVector &z, ColumnVector &x_k);

        /*
        根据系统运动学方程或几何学方程,更新预测状态向量(nx1)
        # (1). X_k = F_k * X_k-1 + B_k * u_k
        # F_k: 根据系统模型设计的预测矩阵或状态变量转移矩阵nxn
        # B_k: 系统外部控制矩阵
        # u_k: 系统外部控制向量,例如加速度,转向等.
        # 这里注意: B_k * u_k的结果维数要保持和X_k同样的维数,以便做矩阵相加
            
        参数x是含有n个元素的当前(k-1时刻)状态值(Current state)向量,
        返回:
        X_k: 依据方程(1)计算的含有n个元素的新的(k)预测状态向量(nx1)
        F_k: 一个依据系统运动学方程或几何学方程设计的预测状态变换模型(n*n矩阵)
        */
        virtual bool stateTransitionFunction(ColumnVector &x, ColumnVector &x_new, SymmetricMatrix &f_k)
        {
            return false;
        }

        /*
        预测值状态变换函数(即测量值转状态值得反函数),把预测值状态向量转换为与测量值向量同样的单位或度量,
        必要时需要非线性函数线性化(返回雅各比矩阵)
        方程(4). X^_k = X_k + K_k * (z_k - zz)     # zz = H_k * X_k
        
        参数x是含有n个元素的当前(k-1时刻)状态值(Current state)向量,
        返回:
        1. 预估测量值向量ZZ_K: 一个m个元素的预估测量值向量: 经 经雅各比矩阵 * 当前状态值向量 得到的 "预估测量值向量"
        2. H_k: 一个m*n雅各比矩阵H_k,矩阵的元素为: 状态值转换为观测值的非线性函数的一阶导数,或有限差分, 或连续差分的比值
        m为测量值个数, n为状态量个数, 用处1: H_k(mxn) * X(nx1) = ZZ_k(mx1),以便下一步方程(4)执行: Z_k - ZZ_K
        例如，您的函数可能包含一个将气压转换为以米为单位的海拔高度的组件,或者需要非线性函数线性化。
        */
        virtual bool stateToMeasurementTransitionFunction(ColumnVector &x, ColumnVector &zz_k, Matrix &h_k)
        {
            return false;
        }

    private:
        int numOfState;       //状态向量个数
        int numOfMeasurement; //测量值向量个数
        ColumnVector X_k;          //上一时刻(k-1)或当前时刻k的状态向量:  n个元素向量,或nx1矩阵
        SymmetricMatrix Q_k;       //各状态变量的预测噪声协方差矩阵nxn
        SymmetricMatrix P_current; //前一时刻(k-1)预测协方差矩阵: 预测P_k
        SymmetricMatrix P_result;  //最优P_k: 当前时刻最优估计协方差矩阵

        /*
        For update
        # 传感器测量值向量与预测值向量之间的非线性转换矩阵
        */
        SymmetricMatrix R_k;

        //单位矩阵I, 这里当数字1使用.  P_k = P_k - K_k*H_k*P_k = (I - G_k*H_k)*P_k
        //Identity matrix(单位矩阵) will be useful later
        SymmetricMatrix Identity;
    };

}
#endif

#include "TinyEKF.h"

using namespace std;

namespace ekf
{
    // constructor
    TinyEKF::TinyEKF(int _numOfState, int _numOfMeasurement, float pVal = 0.1, float qVal = 1e-4, float rVal = 0.1) : numOfState(_numOfState),
                                                                                                                      numOfMeasurement(_numOfMeasurement)
    {
        X_k.resize(_numOfState);
        X_k = 0;

        Q_k.resize(_numOfState, true);
        Q_k = 0;
        for (unsigned int i = 1; i <= _numOfState; i++)
            Q_k(i, i) = 1;
        Q_k *= qVal;

        P_current.resize(_numOfState, true);
        P_current = 0;

        P_result.resize(_numOfState, true);
        P_result = 0;
        for (unsigned int i = 1; i <= _numOfState; i++)
            P_result(i, i) = 1;
        P_result *= pVal;

        R_k.resize(_numOfMeasurement, true);
        R_k = 0;
        for (unsigned int i = 1; i <= _numOfMeasurement; i++)
            R_k(i, i) = 1;
        R_k *= rVal;

        Identity.resize(_numOfState, true);
        Identity = 0;
        for (unsigned int i = 1; i <= _numOfState; i++)
            Identity(i, i) = 1;
    }

    // destructor
    TinyEKF::~TinyEKF()
    {
    }

    void TinyEKF::setInitState(ColumnVector& _x_k, SymmetricMatrix& _p_k)
    {
        for (unsigned int i = 1; i <= numOfState; i++)
            X_k(i) = _x_k(i);
        for (unsigned int i = 1; i <= 6; i++)
            for (unsigned int j = 1; j <= 6; j++)
                P_result(i, j) = _p_k(i, j);
    }

    void TinyEKF::updateRk(SymmetricMatrix &newRk)
    {
        for (unsigned int i = 1; i <= numOfMeasurement; i++)
            for (unsigned int j = 1; j <= numOfMeasurement; j++)
                R_k(i, j) = newRk(i, j);
    }

    void TinyEKF::setAdditiveMeasureNoiseRk(SymmetricMatrix &noiseMatrix /*measurementCount * measurementCount*/)
    {
        for (unsigned int i = 1; i <= numOfMeasurement; i++)
            for (unsigned int j = 1; j <= numOfMeasurement; j++)
                R_k(i, j) = noiseMatrix(i, j);
    }

    void TinyEKF::getAdditiveMeasureNoiseRk(SymmetricMatrix &noiseMatrix /*measurementCount * measurementCount*/)
    {
        for (unsigned int i = 1; i <= numOfMeasurement; i++)
            for (unsigned int j = 1; j <= numOfMeasurement; j++)
                noiseMatrix(i, j) = R_k(i, j);
    }

    void TinyEKF::getPostCovariance(SymmetricMatrix &matrix)
    {
        for (unsigned int i = 1; i <= 6; i++)
            for (unsigned int j = 1; j <= 6; j++)
                matrix(i, j) = P_result(i, j);
    }

    void TinyEKF::doStep(ColumnVector &z, ColumnVector &x_k)
    {
        //Predict ----------------------------------------------------

        /*  根据系统运动学方程或几何学方程,更新预测状态
    参数: 
        x 前一时刻(k-1)状态向量
    返回:
        x: 新时刻(k)状态向量
        F_K:一个针对新状态的雅各比行列式(n*n矩阵),矩阵的元素为: 状态值和观测值之间非线性函数的一阶导数,或有限差分, 或连续差分的比值
    */
        /*# 预测状态方程
    # (1). X_k = F_k * X_k-1 + B_k * u_k
    # F_k: 根据系统模型设计的预测矩阵或状态变量转移矩阵nxn
    # B_k: 系统外部控制矩阵 维数确保可以和前面F_k相乘
    # u_k: 系统外部控制向量,例如加速度,转向等.  维数确保可以和前面F_k相乘
    */
        ColumnVector x_k_new(numOfState);
        SymmetricMatrix F_k(numOfState);
        stateTransitionFunction(X_k, x_k_new, F_k);

        /*# 预测协方差矩阵
    # (2). P_k = F_k * P_k-1 * F_k^T + Q_k
    # Q_k: 各状态量对应的噪音协方差矩阵nxn
    */
        P_current = (SymmetricMatrix)(((SymmetricMatrix)(F_k * P_result)) * F_k.transpose() + Q_k); //P_k

        //Update -----------------------------------------------------
        /*# 预测值状态转测量值变换函数(即测量值转状态值得反函数)矩阵,把预测值状态向量转换为与测量值向量同样的单位或度量,
    # 必要时需要非线性函数线性化(返回雅各比矩阵)
    # 1. 一个m个元素的预估测量值向量: 经经雅各比矩阵 * 当前状态值向量得到的 "预估测量值向量"
    # H_k: 预测值状态转测量值变换函数的雅各比矩阵
    */
        ColumnVector zz_k(numOfMeasurement);
        Matrix H_k(numOfMeasurement, numOfState);
        stateToMeasurementTransitionFunction(x_k_new, zz_k, H_k);

        /* 卡尔曼增益: K_k
    # (3). K_k = P_k * H_k^T * (H_k * P_k * H_k^T + R_k)^-1
    # R_k: 传感器噪声(不确定性)协方差矩阵mxm
    */
        //Note:  这里建议都采用Matrix类型，因为掺杂着SymmetricMatrix类型会使对矩阵就逆矩阵（inverse()）时，运算结果不正确
        Matrix K_k;
        Matrix tmp1 = (Matrix)((Matrix)P_current * H_k.transpose());
        Matrix tmp2 = (Matrix)(H_k * (Matrix)P_current);
        Matrix tmp3 = (Matrix)(tmp2 * H_k.transpose());
#if true //分步计算
        Matrix tmp31 = (Matrix)(tmp3 + R_k);
        Matrix tmp4 = tmp31.inverse();
        K_k = tmp1 * (Matrix)tmp4;
#else
        K_k = (Matrix)(tmp1 * (Matrix)(((Matrix)(tmp3 + R_k)).inverse()));
#endif

        /*# 最终,最优预测状态向量值
    # z_k: 传感器读数或均值
    # (4). X^_k = X_k + K_k * (z_k - H_k * X_k)
    # self.x += np.dot(K_k, (np.array(z) - H_k.dot(self.x)))
    */

        /*# zz_k: 由状态转测量值函数stateToMeasurementTransitionFunction返回的值
    # (4). X^_k = X_k + K_k * (z_k - zz_k)    # Note:  此处的zz_k = H_k * X_k)
    */
        X_k = x_k_new + (K_k * (z - zz_k));

        /*# 最后,最优预测协方差矩阵
    # (5). P^_k = P_k - K_k * H_k * P_k = (I - K_k * H_k) * P_k
    */
        P_result = (SymmetricMatrix)(((SymmetricMatrix)(Identity - (K_k * H_k))) * P_current);

        x_k = X_k;
    }

}

二.  基于TinyEKF的工作类: OdomEstimationEKF

#ifndef __ODOM_ESTIMATION_EKF_INCLUDE__
#define __ODOM_ESTIMATION_EKF_INCLUDE__

#include "TinyEKF.h"

namespace ekf
{

    class OdomEstimationEKF : public TinyEKF
    {
    private:
        int stateCount = 1;
        int measurementCount = 1;

        SymmetricMatrix F_k;
        Matrix H_k;

    public:
        OdomEstimationEKF(int _numOfState, int _numOfMeasurement, float pVal, float qVal, float rVal);
        ~OdomEstimationEKF(){};

        bool stateTransitionFunction(ColumnVector &x, ColumnVector &x_new, SymmetricMatrix &f_k);
        bool stateToMeasurementTransitionFunction(ColumnVector &x, ColumnVector &zz_k, Matrix &h_k);
    };

}
#endif

#include <bfl/wrappers/matrix/matrix_wrapper.h>
#include "OdomEstimationEKF.h"
#include <math.h>

using namespace std;

namespace ekf
{

    OdomEstimationEKF::OdomEstimationEKF(int _numOfState = 6, int _numOfMeasurement = 9, float pVal = 0.1, float qVal = 1e-4, float rVal = 0.1)
        : TinyEKF(_numOfState, _numOfMeasurement, pVal, qVal, rVal)
    {
        stateCount = _numOfState; 
        measurementCount = _numOfMeasurement;
        
        /*
        Note:  在robot_pose_ekf中，F_k的设置采用了非线性方程线性化矩阵，即雅各比矩阵。对输入控制求偏导函数
        1。  状态转换方程描述为：
        //进行非线性计算,更新状态
        //近似执行        X_k = F_k * X_k-1 + B_k * u_k
	    //查看系统预测模型更新(update)时,控制向量输入都为方向为vel(1,2)均为0
        //state(1) += cos(state(6)/theta/) * vel(1)/v/; //根据速度v更新x轴位移
        //state(2) += sin(state(6) /theta/) * vel(1) /v/; //根据速度v更新y轴位移
        //state(6) += vel(2);                                 //更新theta,即方向.
        /*2。雅各比矩阵F_k :
        //密度函数矩阵,雅各比矩阵F_k.  对参数1即状态向量X_k(3)求雅各比矩阵
        for (unsigned int i = 1; i <= 6; i++){
            for (unsigned int j = 1; j <= 6; j++){
                if (i == j)  df(i, j) = 1;
                else  df(i, j) = 0;
            }
        }
        df(1, 6) = -vel_trans * sin(yaw); //求x关于yaw的偏导:             x =  vel(1)*cos(state(3))   x` = -vel(1)*sin(state(3))
        df(2, 6) = vel_trans * cos(yaw);  //求y关于yaw的偏导              y =  vel(1)*sin(state(3))   y` = vel(1)*cos(state(3))

        3。查看robot_pose_ekf系统预测模型更新(update)时,控制向量输入都为0,所以这里用对角线为1方阵即可！
        */

        //状态转换矩阵设为I   只是进行odom融合,且没有输入控制u_k（0，0），因为k-1状态到K状态之间没有明确的转换关系, 我们假设为恒等关系
        F_k.resize(stateCount); //矩阵nxn
        F_k = 0;
        for (unsigned int i = 1; i <= stateCount; i++)
            F_k(i, i) = 1;

        H_k.resize(measurementCount, stateCount); //测量值个数为9*6
        H_k = 0;

        /*
        hk = [
            (1, 0, 0, 0, 0, 0),  p_x
            (0, 1, 0, 0, 0, 0),  p_y
            (0, 0, 0, 0, 0, 0),  p_z = 0
            (0, 0, 0, 0, 0, 0),  d_roll = 0
            (0, 0, 0, 0, 0, 0),  d_pitch = 0
            (0, 0, 0, 0, 0, 1),  d_yaw
            (0, 0, 0, 0, 0, 0),  d_roll = 0
            (0, 0, 0, 0, 0, 0),  d_pitch = 0
            (0, 0, 0, 0, 0, 1)   d_yae
            ] */
        
        H_k(1, 1) = 1;
        H_k(2, 2) = 1;
        H_k(6, 6) = 1;
        H_k(9, 6) = 1;
    }

    /*
    根据系统运动学方程或几何学方程,更新预测状态向量(nx1)
    # (1). X_k = F_k * X_k-1 + B_k * u_k
    # F_k: 根据系统模型设计的预测矩阵或状态变量转移矩阵nxn
    # B_k: 系统外部控制矩阵
    # u_k: 系统外部控制向量,例如加速度,转向等.
    # 这里注意: B_k * u_k的结果维数要保持和X_k同样的维数,以便做矩阵相加
        
    参数x是含有n个元素的当前(k-1时刻)状态值(Current state)向量,
    返回:
    X_k: 依据方程(1)计算的含有n个元素的新的(k)预测状态向量(nx1)
    F_k: 一个依据系统运动学方程或几何学方程设计的预测状态变换模型(n*n矩阵)
    */
    bool OdomEstimationEKF::stateTransitionFunction(ColumnVector &x, ColumnVector &x_new, SymmetricMatrix &f_k)
    {
        //状态转换矩阵设为I   只是进行odom融合,且没有输入控制u_k（0，0），因为k-1状态到K状态之间没有明确的转换关系, 我们假设为恒等关系
        /*SymmetricMatrix _f_k(stateCount); //矩阵nxn
        _f_k = 0;
        for (unsigned int i = 1; i <= stateCount; i++)
            _f_k(i, i) = 1;*/

        f_k = F_k; // 返回一个单位矩阵.

        x_new = F_k * x; // 这里直接返回了当前状态值得相同值

        //cout<<"stateTransitionFunction:   "<<"f_k="<<f_k<<",  x_new="<<x_new<<"\n";

        return true;
    }

    /*
    预测值状态变换函数(即测量值转状态值得反函数),把预测值状态向量转换为与测量值向量同样的单位或度量,
    必要时需要非线性函数线性化(返回雅各比矩阵)
    方程(4). X^_k = X_k + K_k * (z_k - zz)     # zz = H_k * X_k
    
    参数x是含有n个元素的当前(k-1时刻)状态值(Current state)向量,
    返回:
    1. 预估测量值向量ZZ_K: 一个m个元素的预估测量值向量: 经 经雅各比矩阵 * 当前状态值向量 得到的 "预估测量值向量"
    2. H_k: 一个m*n雅各比矩阵H_k,矩阵的元素为: 状态值转换为观测值的非线性函数的一阶导数,或有限差分, 或连续差分的比值
    m为测量值个数, n为状态量个数, 用处1: H_k(mxn) * X(nx1) = ZZ_k(mx1),以便下一步方程(4)执行: Z_k - ZZ_K
    例如，您的函数可能包含一个将气压转换为以米为单位的海拔高度的组件,或者需要非线性函数线性化。
    
    Note: 
    1.一般情况下,如果不涉及到单位,度量或线性变换. 直接返回(m,n)矩阵: 对角线的地方为1，其余的地方为0.
    2.需要做基本的单位,度量或线性变换的,返回一个固定的(m,n)变换矩阵
    3.特殊情况,如果需要做非线性函数线性化的,需要返回一个元素为连续差分((z_k2 - z_k1) / (x_k2-x_k1))的(m,n)变换矩阵
    */
    bool OdomEstimationEKF::stateToMeasurementTransitionFunction(ColumnVector &x, ColumnVector &zz_k, Matrix &h_k)
    {
        //Matrix _h_k(measurementCount,stateCount); //测量值个数为9*6
        //_h_k = 0;
        /*
        hk = [
            (1, 0, 0, 0, 0, 0),  p_x
            (0, 1, 0, 0, 0, 0),  p_y
            (0, 0, 0, 0, 0, 0),  p_z = 0
            (0, 0, 0, 0, 0, 0),  d_roll = 0
            (0, 0, 0, 0, 0, 0),  d_pitch = 0
            (0, 0, 0, 0, 0, 1),  d_yaw
            (0, 0, 0, 0, 0, 0),  d_roll = 0
            (0, 0, 0, 0, 0, 0),  d_pitch = 0
            (0, 0, 0, 0, 0, 1)   d_yae
            ] */
        /*for (unsigned int i=1; i<=stateCount; i++)
            _h_k(i,i) =1;*/
        /*
        _h_k(1,1) = 1; 
        _h_k(2,2) = 1; 
        _h_k(6,6) = 1;
        _h_k(7,4) = 1; 
        _h_k(8,5) = 1; 
        _h_k(9,6) = 1;*/

        //返回经状态转换函数变换后的测量值zz_k: H_k(mxn) * X(nx1) = ZZ_k(mx1)
        zz_k = H_k * x;
        h_k = H_k;

        return true;
    }
}

以上就是全部的EKF相关代码,  因为前面四个小节已经说明的很清楚,再加上代码中有详细的中文注释. 所以代码实现不在做具体分析.

在此,仅给除和上一节robot_pose_ekf中定义不同的,,,,: 

根据上一节的分析,我们知道输入控制向量u_k里(v, θ)都是0, 故不再需要B_k和u_k, 状态方程简化为前后状态呈恒等关系:

得:

odom和imu传感器测量值列向量:

所有传感器的噪音协方差矩阵初始化为一个较小值:

运行中,动态使用odom和imu传感器数据带来的噪音协方差矩阵,对应元素替换:

三.  在我的智能小车上,启动小车的SLAM功能,运行效果图

(1). 准备就绪后,同时启动ROS系统原robot_pose_ekf包和我的my_robot_pose_ekf包. 从打印log信息做对比:

新启动初始状态:

 小车运行一段后:

两幅截图中, 上半部分为ROS原robot_pose_ekf包运行效果;  下半部分为我的my_robot_pose_ekf效果.

(2). 开启SLAM在室内,创建地图效果:

ROS系统原robot_pose_ekf建图效果:

我的my_robot_pose_pose建图效果1:

 我的my_robot_pose_pose建图效果2:

综上,可以看出. 自己的my_robot_pose_ekf运用到实际工程中,车辆位姿状态估计准确可靠. 效果图上除了因建图算法和每次行走路径差异引起少许的地图区别外,地图规整清晰可用, 证明自己的EKF代码稳定可用. 

四.robot_pose_ekf滤波器的松耦合数据融合算法架构图:

 我的ekf 滤波器的高内聚的数据融合算法架构图:

五.  松耦合的robot_pose_ekf和高内聚的my_robot_pose_ekf执行效率对比

robot_pose_ekf每次预测/更新平均]耗时:

my_robot_pose_ekf 每次预测/更新平均]耗时:

 看来,时间效率大致相当. 略好一点点.

最后:

本小节,以及前面四个小节涉及的完整代码,请参见:

1.gitee: 

studyEKF: 学习和实践EKF

2,github:

GitHub - pigknight/studyEKF: EKF 扩展的卡尔曼滤波器  (网络不好,我总打不开)