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

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

时间：2022-04-28 05:35:57

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

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

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

三.卡尔曼滤波器(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; //各状态变量的预测噪声协方差矩阵nxnSymmetricMatrix P_current; //前一时刻(k-1)预测协方差矩阵: 预测P_kSymmetricMatrix 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 laterSymmetricMatrix Identity;};}#endif

#include "TinyEKF.h"using namespace std;namespace ekf{// constructorTinyEKF::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;}// destructorTinyEKF::~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;#elseK_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); //矩阵nxnF_k = 0;for (unsigned int i = 1; i <= stateCount; i++)F_k(i, i) = 1;H_k.resize(measurementCount, stateCount); //测量值个数为9*6H_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效果.