ROS中EKF（扩展卡尔曼跟踪）的使用
2024-05-28 08:40:20
0.简版（更新于2020年最后一天）
#include <bfl/wrappers/matrix/matrix_wrapper.h>
#include <bfl/filter/extendedkalmanfilter.h>
#include <bfl/model/linearanalyticsystemmodel_gaussianuncertainty.h>
#include <bfl/model/linearanalyticmeasurementmodel_gaussianuncertainty.h>
#include <bfl/pdf/linearanalyticconditionalgaussian.h>
#include <fstream>
#include <iostream>using namespace std;
using namespace BFL;
using namespace MatrixWrapper;class MYEKF
{public:BFL::Gaussian prior_;BFL::ExtendedKalmanFilter *filter_;BFL::LinearAnalyticConditionalGaussian *sys_pdf_;BFL::LinearAnalyticSystemModelGaussianUncertainty *sys_model_;BFL::LinearAnalyticConditionalGaussian *meas_pdf_;BFL::LinearAnalyticMeasurementModelGaussianUncertainty *meas_model_;MatrixWrapper::Matrix sys_matrix_;MatrixWrapper::SymmetricMatrix sys_sigma_;float time_;MYEKF() : filter_(NULL),sys_pdf_(NULL),sys_model_(NULL),meas_pdf_(NULL),meas_model_(NULL),sys_matrix_(6, 6) {}void Init(float x, float y, float t){sys_matrix_ = 0;for (unsigned int i = 1; i <= 3; i++){sys_matrix_(i, i) = 1;sys_matrix_(i + 3, i + 3) = 0.9;}// 噪声的u和sigma，sys_noise即QColumnVector sys_mu(6);sys_mu = 0;sys_sigma_ = SymmetricMatrix(6);sys_sigma_ = 0;for (unsigned int i = 0; i < 3; i++){sys_sigma_(i + 1, i + 1) = pow(0.05, 2);sys_sigma_(i + 4, i + 4) = pow(1.0, 2);}Gaussian sys_noise(sys_mu, sys_sigma_);//sys_pdf_ = new LinearAnalyticConditionalGaussian(sys_matrix_, sys_noise);sys_model_ = new LinearAnalyticSystemModelGaussianUncertainty(sys_pdf_);// =============================create meas model// meas_matrix即HMatrix meas_matrix(3, 6);meas_matrix = 0;for (unsigned int i = 1; i <= 3; i++)meas_matrix(i, i) = 1;// 噪声的u和sigma，meas_noise即RColumnVector meas_mu(3);meas_mu = 0;SymmetricMatrix meas_sigma(3);meas_sigma = 0;for (unsigned int i = 0; i < 3; i++)meas_sigma(i + 1, i + 1) = 0;Gaussian meas_noise(meas_mu, meas_sigma);//meas_pdf_ = new LinearAnalyticConditionalGaussian(meas_matrix, meas_noise);meas_model_ = new LinearAnalyticMeasurementModelGaussianUncertainty(meas_pdf_);// first pointColumnVector mu_vec(6);SymmetricMatrix sigma_vec(6);sigma_vec = 0;mu_vec(1) = x;mu_vec(2) = y;mu_vec(3) = 0;for (unsigned int i = 0; i < 3; i++){// mu_vec(i + 1) = pos[i];mu_vec(i + 4) = 0.0;sigma_vec(i + 1, i + 1) = pow(0.1, 2);sigma_vec(i + 4, i + 4) = pow(0.0000001, 2);}prior_ = Gaussian(mu_vec, sigma_vec);filter_ = new ExtendedKalmanFilter(&prior_);time_ = t;}bool Predict(float t){// set Ffor (unsigned int i = 1; i <= 3; i++)sys_matrix_(i, i + 3) = t - time_;sys_pdf_->MatrixSet(0, sys_matrix_);// scale system noise for dtsys_pdf_->AdditiveNoiseSigmaSet(sys_sigma_ * pow(t - time_, 2));time_ = t;// update filterreturn filter_->Update(sys_model_);}bool Update(float x, float y){ColumnVector meas_vec(3);// for (unsigned int i = 0; i < 3; i++)// meas_vec(i + 1) = p[i];meas_vec(1) = x;meas_vec(2) = y;meas_vec(3) = 0;SymmetricMatrix cov(3);cov = 0.0;cov(1, 1) = 0.0025;cov(2, 2) = 0.0025;cov(3, 3) = 0.0025;((LinearAnalyticConditionalGaussian *)(meas_model_->MeasurementPdfGet()))->AdditiveNoiseSigmaSet(cov);// update filterreturn filter_->Update(meas_model_, meas_vec);}void getEstimate(float &x, float &y){ColumnVector tmp = filter_->PostGet()->ExpectedValueGet();x = tmp(1);y = tmp(2);}
};int main()
{ifstream data("/home/lwd/data/track.txt");MYEKF *ekf = new MYEKF();float mt, mx, my;for (int i = 0; i < 74; ++i){if (i == 0){data >> mt >> mx >> my;ekf->Init(mx, my, mt);continue;}data >> mt >> mx >> my;ekf->Predict(mt);float tx, ty;ekf->getEstimate(tx, ty);cout << i + 1 << "\t" << tx - mx << "\t" << ty - my << endl;ekf->Update(mx, my);}return 0;
}
- CMakeLists
cmake_minimum_required(VERSION 2.8.3)
project(ekf)add_compile_options(-std=c++11)find_package(catkin REQUIRED COMPONENTSroscpp
)find_package(PkgConfig REQUIRED)
pkg_check_modules(BFL REQUIRED orocos-bfl)
include_directories(${BFL_INCLUDE_DIRS})
link_directories(${BFL_LIBRARY_DIRS})
message(${BFL_INCLUDE_DIRS})
message(${BFL_LIBRARY_DIRS})catkin_package(
# INCLUDE_DIRS include
# LIBRARIES ekf
# CATKIN_DEPENDS roscpp
# DEPENDS system_lib
)include_directories(include${catkin_INCLUDE_DIRS}
)add_executable(${PROJECT_NAME} src/ekf.cpp)
target_link_libraries(${PROJECT_NAME}${catkin_LIBRARIES}${BFL_LIBRARIES}
)1.代码
- 位置，速度类：state_pos_vel.h
#ifndef STATE_POS_VEL_H
#define STATE_POS_VEL_H#include <tf/tf.h>namespace BFL
{/// Class representing state with pos and vel
class StatePosVel
{public:tf::Vector3 pos_, vel_;/// ConstructorStatePosVel(const tf::Vector3& pos = tf::Vector3(0, 0, 0),const tf::Vector3& vel = tf::Vector3(0, 0, 0)): pos_(pos), vel_(vel) {};/// Destructor~StatePosVel() {};/// operator +=StatePosVel& operator += (const StatePosVel& s){this->pos_ += s.pos_;this->vel_ += s.vel_;return *this;}/// operator +StatePosVel operator + (const StatePosVel& s){StatePosVel res;res.pos_ = this->pos_ + s.pos_;res.vel_ = this->vel_ + s.vel_;return res;}/// output stream for StatePosVelfriend std::ostream& operator<< (std::ostream& os, const StatePosVel& s){os << "(" << s.pos_[0] << ", " << s.pos_[1] << ", " << s.pos_[2] << ")--("<< "(" << s.vel_[0] << ", " << s.vel_[1] << ", " << s.vel_[2] << ") ";return os;};
};
} // end namespace
#endif
- 跟踪基类：tracker.h
#ifndef __TRACKER__
#define __TRACKER__#include "state_pos_vel.h"
#include <bfl/wrappers/matrix/matrix_wrapper.h>
#include <string>namespace estimation
{class Tracker
{public:/// constructorTracker(const std::string& name): name_(name) {};/// destructorvirtual ~Tracker() {};/// return the name of the trackerconst std::string& getName() const{return name_;};/// initialize trackervirtual void initialize(const BFL::StatePosVel& mu, const BFL::StatePosVel& sigma, const double time) = 0;/// return if tracker was initializedvirtual bool isInitialized() const = 0;/// return measure for tracker quality: 0=bad 1=goodvirtual double getQuality() const = 0;/// return the lifetime of the trackervirtual double getLifetime() const = 0;/// return the time of the trackervirtual double getTime() const = 0;/// update trackervirtual bool updatePrediction(const double time) = 0;virtual bool updateCorrection(const tf::Vector3& meas,const MatrixWrapper::SymmetricMatrix& cov) = 0;/// get filter posteriorvirtual void getEstimate(BFL::StatePosVel& est) const = 0;
private:std::string name_;
}; // class
}; // namespace#endif
- 卡尔曼跟踪类：tracker_kalman.h
#ifndef __TRACKER_KALMAN__
#define __TRACKER_KALMAN__#include "tracker.h"
// bayesian filtering
#include <bfl/filter/extendedkalmanfilter.h>
#include <bfl/model/linearanalyticsystemmodel_gaussianuncertainty.h>
#include <bfl/model/linearanalyticmeasurementmodel_gaussianuncertainty.h>
#include <bfl/pdf/linearanalyticconditionalgaussian.h>
#include "state_pos_vel.h"
// TF
#include <tf/tf.h>
// log files
#include <fstream>namespace estimation
{class TrackerKalman: public Tracker
{public:/// constructorTrackerKalman(const std::string& name, const BFL::StatePosVel& sysnoise);/// destructorvirtual ~TrackerKalman();/// initialize trackervirtual void initialize(const BFL::StatePosVel& mu, const BFL::StatePosVel& sigma, const double time);/// return if tracker was initializedvirtual bool isInitialized() const{return tracker_initialized_;};/// return measure for tracker quality: 0=bad 1=goodvirtual double getQuality() const{return quality_;};/// return the lifetime of the trackervirtual double getLifetime() const;/// return the time of the trackervirtual double getTime() const;/// update trackervirtual bool updatePrediction(const double time);virtual bool updateCorrection(const tf::Vector3& meas,const MatrixWrapper::SymmetricMatrix& cov);/// get filter posteriorvirtual void getEstimate(BFL::StatePosVel& est) const;
private:// pdf / model / filterBFL::Gaussian prior_;BFL::ExtendedKalmanFilter* filter_;BFL::LinearAnalyticConditionalGaussian* sys_pdf_;BFL::LinearAnalyticSystemModelGaussianUncertainty* sys_model_;BFL::LinearAnalyticConditionalGaussian* meas_pdf_;BFL::LinearAnalyticMeasurementModelGaussianUncertainty* meas_model_;MatrixWrapper::Matrix sys_matrix_;MatrixWrapper::SymmetricMatrix sys_sigma_;double calculateQuality();// varsbool tracker_initialized_;double init_time_, filter_time_, quality_;
}; // class
}; // namespace
#endif
tracker_kalman.cpp
#include "tracker_kalman.h"
#include <iostream>using namespace MatrixWrapper;
using namespace BFL;
using namespace tf;
using namespace std;
using namespace ros;const static double damping_velocity = 0.9;namespace estimation
{// constructor
TrackerKalman::TrackerKalman(const string& name, const StatePosVel& sysnoise):Tracker(name),filter_(NULL),sys_pdf_(NULL),sys_model_(NULL),meas_pdf_(NULL),meas_model_(NULL),sys_matrix_(6, 6),tracker_initialized_(false)
{// create sys modelsys_matrix_ = 0; // Ffor (unsigned int i = 1; i <= 3; i++){sys_matrix_(i, i) = 1;sys_matrix_(i + 3, i + 3) = damping_velocity;}// QColumnVector sys_mu(6);sys_mu = 0;sys_sigma_ = SymmetricMatrix(6);sys_sigma_ = 0;for (unsigned int i = 0; i < 3; i++){sys_sigma_(i + 1, i + 1) = pow(sysnoise.pos_[i], 2);sys_sigma_(i + 4, i + 4) = pow(sysnoise.vel_[i], 2);}Gaussian sys_noise(sys_mu, sys_sigma_);sys_pdf_ = new LinearAnalyticConditionalGaussian(sys_matrix_, sys_noise);sys_model_ = new LinearAnalyticSystemModelGaussianUncertainty(sys_pdf_);// create meas model// HMatrix meas_matrix(3, 6);meas_matrix = 0;for (unsigned int i = 1; i <= 3; i++)meas_matrix(i, i) = 1;// RColumnVector meas_mu(3);meas_mu = 0;SymmetricMatrix meas_sigma(3);meas_sigma = 0;for (unsigned int i = 0; i < 3; i++)meas_sigma(i + 1, i + 1) = 0;Gaussian meas_noise(meas_mu, meas_sigma);meas_pdf_ = new LinearAnalyticConditionalGaussian(meas_matrix, meas_noise);meas_model_ = new LinearAnalyticMeasurementModelGaussianUncertainty(meas_pdf_);
};// destructor
TrackerKalman::~TrackerKalman()
{if (filter_) delete filter_;if (sys_pdf_) delete sys_pdf_;if (sys_model_) delete sys_model_;if (meas_pdf_) delete meas_pdf_;if (meas_model_) delete meas_model_;
};// initialize prior density of filter
void TrackerKalman::initialize(const StatePosVel& mu, const StatePosVel& sigma, const double time)
{ColumnVector mu_vec(6);SymmetricMatrix sigma_vec(6);sigma_vec = 0;for (unsigned int i = 0; i < 3; i++){mu_vec(i + 1) = mu.pos_[i];mu_vec(i + 4) = mu.vel_[i];sigma_vec(i + 1, i + 1) = pow(sigma.pos_[i], 2);sigma_vec(i + 4, i + 4) = pow(sigma.vel_[i], 2);}prior_ = Gaussian(mu_vec, sigma_vec);filter_ = new ExtendedKalmanFilter(&prior_);// tracker initializedtracker_initialized_ = true;quality_ = 1;filter_time_ = time;init_time_ = time;
}// update filter prediction
bool TrackerKalman::updatePrediction(const double time)
{bool res = true;if (time > filter_time_){// set dt in sys modelfor (unsigned int i = 1; i <= 3; i++)sys_matrix_(i, i + 3) = time - filter_time_;sys_pdf_->MatrixSet(0, sys_matrix_);// scale system noise for dtsys_pdf_->AdditiveNoiseSigmaSet(sys_sigma_ * pow(time - filter_time_, 2));filter_time_ = time;// update filterres = filter_->Update(sys_model_);if (!res) quality_ = 0;else quality_ = calculateQuality();}return res;
};// update filter correction
bool TrackerKalman::updateCorrection(const tf::Vector3& meas, const MatrixWrapper::SymmetricMatrix& cov)
{assert(cov.columns() == 3);// copy measurementColumnVector meas_vec(3);for (unsigned int i = 0; i < 3; i++)meas_vec(i + 1) = meas[i];// set covariance((LinearAnalyticConditionalGaussian*)(meas_model_->MeasurementPdfGet()))->AdditiveNoiseSigmaSet(cov);// update filterbool res = filter_->Update(meas_model_, meas_vec);if (!res) quality_ = 0;else quality_ = calculateQuality();return res;
};void TrackerKalman::getEstimate(StatePosVel& est) const
{ColumnVector tmp = filter_->PostGet()->ExpectedValueGet();for (unsigned int i = 0; i < 3; i++){est.pos_[i] = tmp(i + 1);est.vel_[i] = tmp(i + 4);}
};double TrackerKalman::calculateQuality()
{double sigma_max = 0;SymmetricMatrix cov = filter_->PostGet()->CovarianceGet();for (unsigned int i = 1; i <= 2; i++)sigma_max = max(sigma_max, sqrt(cov(i, i)));return 1.0 - min(1.0, sigma_max / 1.5);
}double TrackerKalman::getLifetime() const
{if (tracker_initialized_)return filter_time_ - init_time_;elsereturn 0;
}double TrackerKalman::getTime() const
{if (tracker_initialized_)return filter_time_;elsereturn 0;
}}; // namespace
- 主文件：main.cpp
#include <iostream>
#include "tracker_kalman.h"using namespace MatrixWrapper;
using namespace estimation;
using namespace BFL;
using namespace tf;
using namespace std;int main() {// system noiseBFL::StatePosVel sys_sigma_(Vector3(0.05, 0.05, 0.05),Vector3(1.0, 1.0, 1.0));TrackerKalman filter("tracker_name", sys_sigma_);// EKF prior densityStatePosVel prior_sigma(Vector3(0.1, 0.1, 0.1),Vector3(0.0000001, 0.0000001, 0.0000001));StatePosVel mu(Vector3(0, 0, 0), Vector3(0.5, 0.5, 0));filter.initialize(mu, prior_sigma, 0);SymmetricMatrix cov(3);cov = 0.0;cov(1, 1) = 0.0025;cov(2, 2) = 0.0025;cov(3, 3) = 0.0025;for (int i = 1; i < 10; ++i) {filter.updatePrediction(1 * i);filter.updateCorrection(Vector3(0.22*i, 0.09*i, 0), cov);StatePosVel est;filter.getEstimate(est);cout << est << endl;}return 0;
}
2.当TrackerKalman作为类的成员变量时
#include <iostream>
#include <list>
#include "tracker_kalman.h"using namespace MatrixWrapper;
using namespace estimation;
using namespace BFL;
using namespace tf;
using namespace std;class Test {public:TrackerKalman filter;StatePosVel sys_sigma_;int test;Test(int test): sys_sigma_(Vector3(0.05, 0.05, 0.05), Vector3(1.0, 1.0, 1.0)),filter("tracker_name", sys_sigma_) {StatePosVel prior_sigma(Vector3(0.1, 0.1, 0.1),Vector3(0.0000001, 0.0000001, 0.0000001));StatePosVel mu(Vector3(0, 0, 0), Vector3(0.5, 0.5, 0));filter.initialize(mu, prior_sigma, 0);this->test = test;}~Test() {}void Propagate(float time) {filter.updatePrediction(time);}void update(Vector3 meas, SymmetricMatrix cov) {filter.updateCorrection(meas, cov);}void getState(StatePosVel &est) {filter.getEstimate(est);}
};int main() { list<Test> l;l.push_back(Test(1));list<Test>::iterator i = l.begin();cout << i->test << endl;// i->Propagate(1);Test tt(6);tt.Propagate(0.6);Test *t = new Test(6);SymmetricMatrix cov(3);cov = 0.0;cov(1, 1) = 0.0025;cov(2, 2) = 0.0025;cov(3, 3) = 0.0025;for (int i = 1; i < 10; ++i) {t->Propagate(1 * i);t->update(Vector3(0.22 * i, 0.09 * i, 0), cov);StatePosVel est;t->getState(est);cout << est << endl;}return 0;
}
- main函数中
i->Propagate(1);不注释，会有段错误，注释则会在循环之后出现段错误。所以要使用指针，即list<Test*> l;，原因不明。
ROS中EKF（扩展卡尔曼跟踪）的使用相关推荐
- 在ROS平台使用扩展卡尔曼融合里程计与IMU传感器
里程计的问题 里程计在转弯的时候, 能容易发生飘移现象,而且精度也不高, 容易产生累积误差 因此, 笔者想到了能不能通过imu来提高机器人的定位效果, 本文采用扩展卡尔曼滤波, ros上的功能包为ro ...
- C++中贝叶斯滤波器包bfl的使用(2)-预测模型非线性的扩展卡尔曼滤波器
摘要 线性卡尔曼滤波器的教程已经翻译完.可是在实际生产生活中,线性状态转移和测量是很少见的,此时就需要扩展卡尔曼滤波器(EKF)登场了.同时在robot_pose_ekf包中使用的也是EKF. 接下来 ...
- 五.卡尔曼滤波器(EKF)开发实践之五: 编写自己的EKF替换robot_pose_ekf中EKF滤波器
本系列文章主要介绍如何在工程实践中使用卡尔曼滤波器,分七个小节介绍: 一.卡尔曼滤波器开发实践之一: 五大公式 二.卡尔曼滤波器开发实践之二: 一个简单的位置估计卡尔曼滤波器 三.卡尔曼滤波器(EK ...
- 【滤波】扩展卡尔曼滤波器（EKF）
%matplotlib inline #format the book import book_format book_format.set_style() 我们发展了线性卡尔曼滤波器的理论.然后在上 ...
- ## ***电池SOC仿真系列-基于扩展卡尔曼(EKF)算法的SOC估计（内含代码等资料）***
## ***电池SOC仿真系列-基于扩展卡尔曼(EKF)算法的SOC估计(内含代码等资料)*** ## 1 研究背景 电池的荷电状态(SOC)代表的是电池当前的剩余容量,数值定义是电池剩余电量与电池额 ...
- robot_localization中EKF源码介绍
robot_localization中EKF源码介绍 /** Copyright (c) 2014, 2015, 2016 Charles River Analytics, Inc.* Copyrig ...
- 【信号处理】基于扩展卡尔曼滤波器和无迹卡尔曼滤波器的窄带信号时变频率估计（Matlab代码实现）
目录 1 概述 2 数学模型 3 运行结果 4 结论 5 参考文献 6 Matlab代码实现 1 概述 本文讲解和比较了基于卡尔曼滤波器的频率跟踪方法的能力,例如扩展卡尔曼滤波器 (EKF) 和无味卡 ...
- 说不尽的卡尔曼 | 详解扩展卡尔曼滤波器
作者 | 火山 编辑 | 空中机器人前沿 点击下方卡片,关注"自动驾驶之心"公众号 ADAS巨卷干货,即可获取 点击进入→自动驾驶之心[目标跟踪]技术交流群 后台回复[卡尔曼滤波] ...
- 【UWB】Kalman filter, KF卡尔曼滤波, EKF 扩展卡尔曼滤波
文章目录 卡尔曼滤波器 扩展卡尔曼滤波器 协方差 Ref: 卡尔曼滤波器 首先从工程上看卡尔曼滤波算法. 引入一个离散控制过程的系统,该系统可用一个线性随机微分方程(linear stochastic ...
最新文章
- Spring中利用applicationContext.xml文件实例化对象和调用方法
- SpringBoot高级-任务-定时任务
- 如何优雅地实现判断一个值是否在一个集合中?
- php中写salt,请快速检查这个PHP+SALT实现-不工作?
- 高通做服务器芯片有优势吗,为什么高通海思联发科不把芯片面积做的和苹果a系列一样大？性能不就赶上了吗？...
- 查找字符串中元素出现的次数
- 【C/C++学院】0805-语音识别控制QQ/语音控制游戏
- 机器人机构学的数学基础——绪论
- 信息系统项目管理师证书有什么用？
- Java语言的发展简史
- The7th Zhejiang Provincial Collegiate Programming Contest-Problem A：A - Who is Older?
- 冶金物理化学复习【6】吉布斯自由能的变化
- SpringBoot整合华为云OBS
- ubuntu设置文件夹共享
- 我开始搞研发管理和项目管理了，发现最难管理的还是人
- 计算机网络实验华中科技大学,华中科技大学计算机网络实验报告.pdf
- OpenJudge NOI 1.8 20:反反复复
- 【精选】一文搞懂css三大特性
- java算法 例 百度云_Java版数据结构与算法(20集版)视频教程百度云下载
- 人工智能在教育领域的应用行业调研报告 - 市场现状分析与发展前景预测