ros bfl
// refer to : https://blog.csdn.net/zhxue_11/article/details/83822462
#include <filter/extendendkalmanfilter.h>
#include <model/linearanalyticsystemmodel_gaussianuncertainty.h>
#include <model/linearanalyticmeasurementmodel_gaussianuncertainty.h>
#include <pdf/linearanalyticsystemmodel_gaussianuncertainty.h>
#include <pdf/linearanalyticmeasurementmodel_gaussianuncertainty.h>
#include "../mobile_robot.h"
#include "../mobile_robot_wall_cts.h"
using namespace MatrixWrapper;
using namespace BFL;
using namespace std;
// sys model : x(k) = A*x(k-1) + B*u(k-1)
Matrix A(2,2);
A(1,1) = 1.0;
A(1,2) = 0.0;
A(2,1) = 0.0;
A(2,2) = 1.0;
Matrix B(2,2);
B(1,1) = cos(0.8);
B(1,2) = 0.0;
B(2,1) = sin(0.8);
B(2,2) = 0.0;
vector<Matrix> AB(2);
AB[0] = A;
AB[1] = B;
ColumnVector sysNoise_Mu(2);
sysNoise_Mu(1) = MU_SYSTEM_NOISE_X;
sysNoise_Mu(2) = MU_SYSTEM_NOISE_Y;
SymmetricMatrix sysNoise_Cov(2);
sysNoise_Cov = 0.0;
sysNoise_Cov(1,1) = SIGMA_SYSTEM_NOISE_X;
sysNoise_Cov(1,2) = 0.0;
sysNoise_Cov(2,1) = 0.0;
sysNoise_Cov(2,2) = SIGMA_SYSTEM_NOISE_Y;
Gaussian system_Uncertainty(sysNoise_Mu, sysNoise_Cov);
LinearAnalyticConditionalGaussian sys_pdf(AB, system_Uncertainty);
LinearAnalyticSystemModelGaussianUncertainty sys_model(&sys_pdf);
// measure model z(k+1) = H*x(k+1)
Matrix H(1,2);
double wall_ct = 2/(sqrt(pow(RICO_WALL,2.0) + 1));
H = 0.0;
H(1,1) = wall_ct * RICO_WALL;
H(1,2) = 0 - wall_ct;
ColumnVector measNoise_Mu(1);
measNoise_Mu(1) = MU_MEAS_NOISE; //噪音均值
SymmetricMatrix measNoise_Cov(1);
measNoise_Cov(1,1) = SIGMA_MEAS_NOISE; //噪音协方差矩阵（σ^2）
Gaussian measurement_Uncertainty(measNoise_Mu, measNoise_Cov);
LinearAnalyticConditionalGaussian meas_pdf(H, measurement_Uncertainty);
LinearAnalyticMeasurementModelGaussianUncertainty meas_model(&meas_pdf);
// build the filter
ColumnVector prior_Mu(2);
prior_Mu(1) = PRIOR_MU_X;
prior_Mu(2) = PRIOR_MU_Y;
SymmetricMatrix prior_Cov(2);
prior_Cov(1,1) = PRIOR_COV_X;
prior_Cov(1,2) = 0.0;
prior_Cov(2,1) = 0.0;
prior_Cov(2,2) = PRIOR_COV_Y;
Gaussian prior(prior_Mu, prior_Cov);// very like system model construction;
ExtendedKalmanFilter filter(&prior_cont);
// update the filter with input and measure
MobileRobot mobile_robot;
mobile_robot.Move( input );
ColumnVector measurement = mobile_robot.Measure();
filter->Update(&sys_model,input,&meas_model, measurement);
// get the result
Pdf<ColumnVector> * posterior = filter->PostGet();
posterior->ExpectedValueGet();
posterior->CovarianceGet();
BFL particle
http://wiki.ros.org/bfl/Tutorials/Example of using a particle filter for localization by bfl library
ros robot_pose_ekf code
//crreate sys model and odom imu model
OdomEstimation::OdomEstimation():
{
ColumnVector sysNoise_Mu(6); sysNoise_Mu = 0;
SymmetricMatrix sysNoise_Cov(6); sysNoise_Cov = 0;
for (unsigned int i=1; i<=6; i++) sysNoise_Cov(i,i) = pow(1000.0,2);
Gaussian system_Uncertainty(sysNoise_Mu, sysNoise_Cov);
sys_pdf_ = new NonLinearAnalyticConditionalGaussianOdo(system_Uncertainty);
sys_model_ = new AnalyticSystemModelGaussianUncertainty(sys_pdf_);
// create MEASUREMENT MODEL ODOM
ColumnVector measNoiseOdom_Mu(6); measNoiseOdom_Mu = 0;
SymmetricMatrix measNoiseOdom_Cov(6); measNoiseOdom_Cov = 0;
for (unsigned int i=1; i<=6; i++) measNoiseOdom_Cov(i,i) = 1;
Gaussian measurement_Uncertainty_Odom(measNoiseOdom_Mu, measNoiseOdom_Cov);
Matrix Hodom(6,6); Hodom = 0;
Hodom(1,1) = 1; Hodom(2,2) = 1; Hodom(6,6) = 1;
odom_meas_pdf_ = new LinearAnalyticConditionalGaussian(Hodom, measurement_Uncertainty_Odom);
odom_meas_model_ = new LinearAnalyticMeasurementModelGaussianUncertainty(odom_meas_pdf_);
// create MEASUREMENT MODEL IMU
ColumnVector measNoiseImu_Mu(3); measNoiseImu_Mu = 0;
SymmetricMatrix measNoiseImu_Cov(3); measNoiseImu_Cov = 0;
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;
Himu(1,4) = 1; Himu(2,5) = 1; Himu(3,6) = 1;
imu_meas_pdf_ = new LinearAnalyticConditionalGaussian(Himu, measurement_Uncertainty_Imu);
imu_meas_model_ = new LinearAnalyticMeasurementModelGaussianUncertainty(imu_meas_pdf_);
}
// callback function for odom data
void OdomEstimationNode::odomCallback(const OdomConstPtr& odom)
{
// receive data
odom_stamp_ = odom->header.stamp;
odom_time_ = Time::now();
Quaternion q;
tf::quaternionMsgToTF(odom->pose.pose.orientation, q);
odom_meas_ = Transform(q, Vector3(odom->pose.pose.position.x, odom->pose.pose.position.y, 0));
for (unsigned int i=0; i<6; i++)
for (unsigned int j=0; j<6; j++)
odom_covariance_(i+1, j+1) = odom->pose.covariance[6*i+j];
my_filter_.addMeasurement(StampedTransform(odom_meas_.inverse(), odom_stamp_, base_footprint_frame_, "wheelodom"), odom_covariance_);
};
// callback function for imu data
void OdomEstimationNode::imuCallback(const ImuConstPtr& imu)
{
imu_callback_counter_++;
assert(imu_used_);
// receive data
imu_stamp_ = imu->header.stamp;
tf::Quaternion orientation;
quaternionMsgToTF(imu->orientation, orientation); // only use imu orientation and all the acc is ingnored !
imu_meas_ = tf::Transform(orientation, tf::Vector3(0,0,0));
for (unsigned int i=0; i<3; i++)
for (unsigned int j=0; j<3; j++)
imu_covariance_(i+1, j+1) = imu->orientation_covariance[3*i+j];// and alos use orientation_covariance
StampedTransform base_imu_offset;
robot_state_.lookupTransform(base_footprint_frame_, imu->header.frame_id, imu_stamp_, base_imu_offset);
imu_meas_ = imu_meas_ * base_imu_offset;
imu_time_ = Time::now();
// manually set covariance untile imu sends covariance
if (imu_covariance_(1,1) == 0.0){
SymmetricMatrix measNoiseImu_Cov(3); measNoiseImu_Cov = 0;
measNoiseImu_Cov(1,1) = pow(0.00017,2); // = 0.01 degrees / sec
measNoiseImu_Cov(2,2) = pow(0.00017,2); // = 0.01 degrees / sec
measNoiseImu_Cov(3,3) = pow(0.00017,2); // = 0.01 degrees / sec
imu_covariance_ = measNoiseImu_Cov;
}
my_filter_.addMeasurement(StampedTransform(imu_meas_.inverse(), imu_stamp_, base_footprint_frame_, "imu"), imu_covariance_); // send_imu_transform
};
// update filter
bool OdomEstimation::update(bool odom_active, bool imu_active, bool gps_active, bool vo_active, const Time& filter_time, bool& diagnostics_res)
{
// system update filter
// --------------------
// for now only add system noise
ColumnVector vel_desi(2); vel_desi = 0;
filter_->Update(sys_model_, vel_desi);
// process odom measurement
if (odom_active){
transformer_.lookupTransform("wheelodom", base_footprint_frame_, filter_time, odom_meas_);
if (odom_initialized_){
// convert absolute odom measurements to relative odom measurements in horizontal plane
Transform odom_rel_frame = Transform(tf::createQuaternionFromYaw(filter_estimate_old_vec_(6)),
filter_estimate_old_.getOrigin()) * odom_meas_old_.inverse() * odom_meas_;
ColumnVector odom_rel(6);
decomposeTransform(odom_rel_frame, odom_rel(1), odom_rel(2), odom_rel(3), odom_rel(4), odom_rel(5), odom_rel(6));
angleOverflowCorrect(odom_rel(6), filter_estimate_old_vec_(6));
// update filter
odom_meas_pdf_->AdditiveNoiseSigmaSet(odom_covariance_ * pow(dt,2));
ROS_DEBUG("Update filter with odom measurement %f %f %f %f %f %f",
odom_rel(1), odom_rel(2), odom_rel(3), odom_rel(4), odom_rel(5), odom_rel(6));
filter_->Update(odom_meas_model_, odom_rel);
diagnostics_odom_rot_rel_ = odom_rel(6);
}
else{
odom_initialized_ = true;
diagnostics_odom_rot_rel_ = 0;
}
odom_meas_old_ = odom_meas_;
}
// process imu measurement
// -----------------------
if (imu_active){
if (!transformer_.canTransform(base_footprint_frame_,"imu", filter_time)){
ROS_ERROR("filter time older than imu message buffer");
return false;
}
transformer_.lookupTransform("imu", base_footprint_frame_, filter_time, imu_meas_);
if (imu_initialized_){
// convert absolute imu yaw measurement to relative imu yaw measurement
Transform imu_rel_frame = filter_estimate_old_ * imu_meas_old_.inverse() * imu_meas_;
ColumnVector imu_rel(3); double tmp;
decomposeTransform(imu_rel_frame, tmp, tmp, tmp, tmp, tmp, imu_rel(3));
decomposeTransform(imu_meas_, tmp, tmp, tmp, imu_rel(1), imu_rel(2), tmp);
angleOverflowCorrect(imu_rel(3), filter_estimate_old_vec_(6));
diagnostics_imu_rot_rel_ = imu_rel(3);
// update filter
imu_meas_pdf_->AdditiveNoiseSigmaSet(imu_covariance_ * pow(dt,2));
filter_->Update(imu_meas_model_, imu_rel);
}
else{
imu_initialized_ = true;
diagnostics_imu_rot_rel_ = 0;
}
imu_meas_old_ = imu_meas_;
}
}