BFL ekf imu and odom

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_; 
	}
}