本系列文章主要介绍了如何在工程实践中使用卡尔曼滤波器。
一.开发卡尔曼滤波器的实践之一： 五大公式
二.卡尔曼滤波器开发实践之二: 一个简单的位置估计卡尔曼滤波器
三.卡尔曼滤波器(EKF)开发实践三： 基于三个传感器的海拔高度数据集成
四.卡尔曼滤波器(EKF)开发实践四： ROS系统位置估计包robot_pose_ekf详解 也就是本文
五.卡尔曼滤波器(EKF)开发实践五： 编写自己的EKF替换robot_pose_ekf中EKF滤波器
六.卡尔曼滤波器(UKF)开发实践六： 卡尔曼滤波器无损(UKF)高级-白话讲解
七.卡尔曼滤波器(UKF)开发实践七： 卡尔曼滤波器无损(UKF)进阶-实例篇
--------------------------------正文开始------------------------------------------ 在我的工作学习中,只要涉及到智能汽车,智能机器人,经常会接触到基于多路传感器的位姿估计,比如ROS该系统用于评估机器人 3D 位置扩展卡尔曼滤波算法包：
该包用于评估机器人 3D 使用来自不同源的位置测量信息，可使用带有 6D（3D position and 3D orientation）卡尔曼滤波器从轮子里程计集成了模型信息的扩展(odom)， 惯性传感器(IMU),视觉里程计(VO),以及GPS定位数据信息。 根据ROS官方文介绍： 。
接下来，我将扩展卡尔曼滤波器(ekf)五大公式角度，对robot_pose_ekf做详细分析.
我打算把这个包全部放在里EKF部分,对于这个包的基于ROS系统的入口代码和各种入口代码topic订阅和发布，TF如何从传感器处理监控和发布的位置信息，本文不介绍.此外，此包支持odom.imu,vo,gps四路传感器位置信息集成，这里我只用odom和imu介绍两路位置融合.
odom传感器数据格式： odom( x, y, z, roll,pitch,yaw): 其中,x,y作为智能车在平面地图上的x和y坐标;z坐标忽略恒等于0； roll(车身纵向翻滚角)和pitch(车身俯仰角)不考虑恒等于0. yaw(偏航角)作为智能车在平面地图上左右转向方向. 即:odom( x, y, 0, 0,0,yaw),共6个测量值数据. 此外odom传感器还提供自己的噪声协方差矩阵
,为一个 6x6矩阵.
imu惯性传感器数据格式： imu(roll,pitch,yaw): 其中roll(车身纵向翻滚角)和pitch(车身俯仰角)不考虑恒等于0. yaw(偏航角)作为智能车在平面地图上左右转向方向.即:imu( 0,0,yaw),测量值数据共3个. 此外imu传感器还提供自己的噪声协方差矩阵,为一个 3x3矩阵.
系统状态列向量：X_k=( x, y, z, roll,pitch,yaw), 实际有效值为：X_k=( x, y, 0, 0,0,yaw). 理由同上odom.
先讲BFL(贝叶斯滤波器库)，这是一个包含开源贝叶斯滤波器库的开源贝叶斯滤波器库(). 根据包据介绍，包 注意这个,这与我在前一篇文章中介绍的不同。方式.
下面我结合关键代码介绍一下是如何采用的方式使用(详细robot_pose_ekf您可以在线下载代码：
源码位置:odom_estimation.cpp
所在方法: OdomEstimation::initialize(const Transform& prior, const Time& time)
// set prior of filter ColumnVector prior_Mu(6); //--->X_k-1 状态向量(6)先验(初始)估计值(x,y,z,roll,pitch,yaw) decomposeTransform(prior, prior_Mu(1), prior_Mu(2), prior_Mu(3), prior_Mu(4), prior_Mu(5), prior_Mu(6)); SymmetricMatrix prior_Cov(6); //--->P_k-1 矩阵6的初始预测x6, for (unsigned int i=1; i<=6; i ) { for (unsigned int j=1; j<=6; j ){ if (i==j) prior_Cov(i,j) = pow(0.001,2); else prior_Cov(i,j) = 0; } } prior_ = new Gaussian(prior_Mu,prior_Cov); ///关于先验状态值的二维高斯分布 filter_ = new ExtendedKalmanFilter(prior_); //Construction of the Filter // remember prior addMeasurement(StampedTransform(prior, time, output_frame_, base_footprint_frame_));如代码和注释所示，此处定义：
- 先验状态列向量： (x,y,z,roll,pitch,yaw), 即态值个数: n = 6.
- 先验不确定性协方差矩阵
- 先验估计高斯函数: prior_ = new Gaussia(prior_Mu,prior_Cov), 注: 最后,ekf的最优估计值和最优协方差矩阵也将基于此高斯分布获取:
filter_->PostGet()->ExpectedValueGet();
filter_->PostGet()->CovarianceGet();
- 滤波器对象: filter_ = new ExtendedKalmanFilter(prior_);
- 最后,保存第一个odom位姿数据,等待imu数据到来,才开始数据融合.
Note: 整个过滤器的先验估计值和后验(最优)估计值,先验不确定性协方差矩阵和后验(最优)不确定性协方差矩阵,都保存在EKF实例对象 filter_中.
源码位置: odom_estimation.cpp
所在方法: OdomEstimation::OdomEstimation()
系统预测模型
// create SYSTEM MODEL
ColumnVector sysNoise_Mu(6); sysNoise_Mu = 0;//预测模型状态向量X_k(6)
SymmetricMatrix sysNoise_Cov(6); sysNoise_Cov = 0;//预测噪音协方差矩阵Q_k(6x6)
for (unsigned int i=1; i<=6; i++) sysNoise_Cov(i,i) = pow(1000.0,2);
Gaussian system_Uncertainty(sysNoise_Mu/*均值:预测向量nx1*/, sysNoise_Cov/*预测噪音协方差矩阵nxn*/);//预测二维高斯分布
//非线性模型没有F_k,B_k. 但求得的雅各比矩阵, 相当于F_k和B_k的结合
sys_pdf_ = new NonLinearAnalyticConditionalGaussianOdo(system_Uncertainty/*指定预测模型高斯分布函数*/);//非线性概率密度函数
sys_model_ = new AnalyticSystemModelGaussianUncertainty(sys_pdf_);//整个系统的预测模型- 系统预测模型列向量并初始化全0: x_k(6) = (0,0,0,0,0,0), 对应(x,y,z,roll,pitch,yaw)
- 系统预测模型噪音协方差矩阵:
- 系统模型非线性概率密度函数(PDF)和系统模型: sys_pdf_ 和 sys_model_
这一步sys_model_定义,其实对应:
和
Note:
(1).现在只是定义了模型和必要的矩阵和向量, 真正的执行,要等到针对sys_model_做更新(update)时才执行.
(2).本人也有个疑问: 的对角线有效值,为什么都设置这么大?
上面的矩阵和向量定义是不是差点什么? 对,还不见和
其实,预测矩阵和控制矩阵就在 sys_pdf_ 里面以形式被定义,而做为控制向量,在OdomEstimation::update()即ekf针对sys_model_更新时才提供,现在可以先剧透:为2维列向量u_k=(v, θ), 其中v是小车速度,θ为偏航角度. 还可以剧透的是, v和θ在整个生命周期全是0.即:u_k=(0, 0).
- 现在来看上面说的是如何定义和的:
我们注意到这里的sys_pdf_ 的类名为: NonLinearAnalyticConditionalGaussianOdo.即这是一个非线性概率密度函数.
看到NonLinearAnalyticConditionalGaussianOdo类的定义,其有一个私有矩阵成员变量: MatrixWrapper::Matrix df;
再看NonLinearAnalyticConditionalGaussianOdo的实现代码:
NonLinearAnalyticConditionalGaussianOdo::NonLinearAnalyticConditionalGaussianOdo(const Gaussian& additiveNoise)
: AnalyticConditionalGaussianAdditiveNoise(additiveNoise,NUMCONDARGUMENTS_MOBILE),
df(6,6)
{
// initialize df matrix
//密度函数矩阵,雅各比矩阵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;
}
}
}
ColumnVector NonLinearAnalyticConditionalGaussianOdo::ExpectedValueGet() const
{
//条件概率密度的条件参数，状态与输入都是条件概率密度的私有变量。
ColumnVector state = ConditionalArgumentGet(0); //获取状态向量X_k(3)
ColumnVector vel = ConditionalArgumentGet(1); //获取输入向量z_k(2) ,这里是速度和方向
//进行非线性计算,更新状态
//近似执行 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,即方向.
// Get the mean Value of the Additive Gaussian uncertainty,即:_additiveNoise_Mu: X_k-1(6)
return state + AdditiveNoiseMuGet();
}
Matrix NonLinearAnalyticConditionalGaussianOdo::dfGet(unsigned int i) const
{
if (i==0)//derivative to the first conditional argument (x) 状态向量X_k(6)
{
double vel_trans = ConditionalArgumentGet(1)(1);//输入向量的速度值
double yaw = ConditionalArgumentGet(0)(6);//状态向量的偏航角theta
df(1,3)=-vel_trans*sin(yaw); //求x关于yaw的偏导: x = vel(1)*cos(state(3)) x` = -vel(1)*sin(state(3))
df(2,3)= vel_trans*cos(yaw); //求y关于yaw的偏导 y = vel(1)*sin(state(3)) y` = vel(1)*cos(state(3))
return df;
}
else
{
/*...这里代码省略*/
}
}
上述代码完成以下工作:
(1). 构造函数中初始化矩阵df为:
(2).在ExpectedValueGet()方法中,完成了
的完整计算. 具体怎么解释呢? 这里就要提到在本系列文章的第一节中介绍的非线性函数线性化问题.
根据初中物理运动学公式: St = S0 + v * t 得系统运动学方程:
x轴方向新位置x: x_k = x_k-1 + 0 + 0 + 0 + 0 + cos(θ) * v_k-1 * Δt
y轴方向新位置y: y_k = 0 + y_k-1 + 0 + 0 + 0 + sin(θ) * v_k-1 * Δt
直线运动速度v: θ_k = 0 + 0 + 0 + 0 + 0 + θ_k-1
即:
大家注意我这里写的是[FB_k], 即这个矩阵是 和的结合矩阵.
(注: 这里可以看出,系统控制向量里: 第一个元素是速度:v 第二个元素是方向:θ 且θ为方向偏移量. 而且代码中并没有考虑Δt,我的分析里添加了对Δt引入.)
(3). dfGet(unsigned int i) 方法中有对上面动态生成的做了非线性函数线性化处理.
因为我们知道这上面运动方程明显是非线性的,且 cos(θ)和sin(θ) 明显是非线性曲线函数. 我们必须使用 cos(θ)和sin(θ)偏导函数来代替,由第一节的导函数定义引用知:
于是上面系统运动学方程线性化后为:
x轴方向新位置x: x_k = x_k-1 + 0 + 0 + 0 + 0 - sin(θ) * v_k-1 * Δt
y轴方向新位置y: y_k = 0 + y_k-1 + 0 + 0 + 0 + cos(θ) * v_k-1 * Δt
直线运动速度v: θ_k = 0 + 0 + 0 + 0 + 0 + θ_k-1
得:
注: 的:
df(1,3) = -vel_trans*sin(yaw); df(2,3) = vel_trans*cos(yaw);
代码中应为, 应为. 之所以错误的代码还能正常工作. 是因为工作时,系统控制例向量( v, θ),始终是(0 , 0 ). 其中原因就在上面的[FB_k]矩阵里.
odom测量模型源码:
/*********************************
* Initialise measurement(update) model *
********************************/
// create MEASUREMENT MODEL ODOM
ColumnVector measNoiseOdom_Mu(6); measNoiseOdom_Mu = 0;//--->z_k(6) 6个测量值(x,y,z,roll,pitch,yaw)
SymmetricMatrix measNoiseOdom_Cov(6); measNoiseOdom_Cov = 0; //--->R_k 传感器噪音协方差矩阵6x6
for (unsigned int i=1; i<=6; i++) measNoiseOdom_Cov(i,i) = 1;
Gaussian measurement_Uncertainty_Odom(measNoiseOdom_Mu, measNoiseOdom_Cov);//关于测量值的二维高斯分布
//状态向量向测量值向量的转换矩阵(针对非线性为雅各比矩阵)mxn. m:测量值个数6, n:状态值个数6
Matrix Hodom(6,6); Hodom = 0;
Hodom(1,1) = 1; Hodom(2,2) = 1; Hodom(6,6) = 1;
odom_meas_pdf_ = new LinearAnalyticConditionalGaussian(Hodom/*状态值转测量值雅各比矩阵6x6*/, measurement_Uncertainty_Odom);
odom_meas_model_ = new LinearAnalyticMeasurementModelGaussianUncertainty(odom_meas_pdf_);
上述代码主要定义针对odom测量值的H_k, R_k, z_k:
- 定义odom测量值列向量: measNoiseOdom_Mu(6); 即odom传感器的 6个测量值(x,y,z,roll,pitch,yaw)
- 定义odom传感器噪音协方差矩阵: measNoiseOdom_Cov; 并初始化对角线为1; 注: 后续此odom噪音协方差会用从odom传感器odom测量值一起而来odom噪音协方差矩阵替换.
- 定义状态值转测量值(转同尺度,度量等)转换矩阵: Hodom(6x6), 因为都是线性恒等关系,得:
- 定义odom测量模型概率密度函数(PDF): odom_meas_pdf_ 和 odom_meas_model_
imu测量模型源码:
ColumnVector measNoiseImu_Mu(3); measNoiseImu_Mu = 0; //--->z_k(3) 3个测量值(roll,pitch,yaw)
SymmetricMatrix measNoiseImu_Cov(3); measNoiseImu_Cov = 0;//--->R_k 传感器噪音协方差矩阵3x3
for (unsigned int i=1; i<=3; i++) measNoiseImu_Cov(i,i) = 1;
Gaussian measurement_Uncertainty_Imu(measNoiseImu_Mu, measNoiseImu_Cov);//关于测量值的二维高斯分布
Matrix Himu(3,6); Himu = 0;//状态向量向测量值向量的转换矩阵(针对非线性为雅各比矩阵)mxn. m:测量值个数3, n:状态值个数6
Himu(1,4) = 1; Himu(2,5) = 1; Himu(3,6) = 1;
imu_meas_pdf_ = new LinearAnalyticConditionalGaussian(Himu/*状态值转测量值雅各比矩阵3x6*/, measurement_Uncertainty_Imu);
imu_meas_model_ = new LinearAnalyticMeasurementModelGaussianUncertainty(imu_meas_pdf_);
上述代码主要定义针对imu测量值的H_k, R_k, z_k:
- 定义imu测量值列向量: measNoiseImu_Mu (3); 即imu传感器的 3个测量值(roll,pitch,yaw)
- 定义imu传感器噪音协方差矩阵: measNoiseImu_Cov; 并初始化对角线为1; 注: 后续此imu噪音协方差会用从imu传感器imu测量值一起而来imu噪音协方差矩阵替换.
- 定义状态值转测量值(转同尺度,度量等)转换矩阵: Himu(3x6), 因为都是线性恒等关系,得:
- 定义imu测量模型概率密度函数(PDF): imu_meas_pdf_ 和 imu_meas_model_
此外, robot_pose_ekf包还支持vo(视觉odom),gps. 只是配置它们不启用. Vo和odom的代码实现和odom和imu十分类似,此处不在阐述.
源码位置: odom_estimation.cpp
所在方法: OdomEstimation:: update (……)
// system update filter
ColumnVector vel_desi(2); vel_desi = 0;
filter_->Update(sys_model_, vel_desi/*控制向量:u_k*/); //分步更新系统预测模型
以上代码:
- 定义系统预测模型的控制向量u_k(v, θ),两个元素v和θ恒等以0. 这里也就解释了为什么前面sys_model_定义时,提供的FB_k联合矩阵有问题,但仍影响运行结果了.
- 使用sys_model_以vel_desi为控制向量执行卡尔曼滤波器公式(1): 和公式(2) .
- 和保存到ExtendedKalmanFilter实例filter_中.
<2>测量模型更新: odom模型odom_meas_model_和imu模型imu_meas_model_. 其他vo_meas_model_和gps_meas_model_类似不在阐述.
odom测量模型源码:
// update filter
odom_meas_pdf_->AdditiveNoiseSigmaSet(odom_covariance_ * pow(dt,2));//更新odom协方差,即更新前面measNoiseOdom_Cov
filter_->Update(odom_meas_model_, odom_rel/*测量向量:z_k*/); //分步更新odom测量模型
以上odom更新代码:
- 把随同odom传感器测量值一起的odom传感器噪音协方差矩阵乘以pow(dt,2)后,设置到初始化阶段定义的odom概率密度函数odom_meas_pdf_, 即动态更新了odom的.
- 使用odom_meas_model_以odom_rel为测量值列向量执行卡尔曼滤波器公式(4): 和公式(5) .
- 从filter_中取出和,参与公式(4)和公式(5)计算后,把最终结果:后验估计值(或最优估计值)保存到实例filter_的; 后验协方差矩阵(或最优协方差矩阵)保存到实例filter_的;
imu测量模型源码: 大致和odom过程一样, 略.
<3> 准备下次更新(update)准备工作, 包含系统预测模型更新和测量值模型更行
把后验估计值(或最优估计值)设置到;
把后验协方差矩阵(或最优协方差矩阵) 设置到;
<4> 获取最优估计
主程序通过调用filter_->PostGet()->ExpectedValueGet();获取当前最优估计,作为输出发布给其他程序使用.
4. robot_pose_ekf功能包中EKF整体的预测/更新架构图:
最后,发张本人制作的robot_pose_ekf中ekf代码架构图. 直观地看看何为"".