39 using namespace MatrixWrapper;
49 OdomEstimation::OdomEstimation():
52 filter_initialized_(false),
53 odom_initialized_(false),
54 imu_initialized_(false),
55 vo_initialized_(false),
56 gps_initialized_(false),
57 output_frame_(
std::string(
"odom_combined")),
58 base_footprint_frame_(
std::string(
"base_footprint"))
61 ColumnVector sysNoise_Mu(6); sysNoise_Mu = 0;
62 SymmetricMatrix sysNoise_Cov(6); sysNoise_Cov = 0;
63 for (
unsigned int i=1; i<=6; i++) sysNoise_Cov(i,i) = pow(1000.0,2);
64 Gaussian system_Uncertainty(sysNoise_Mu, sysNoise_Cov);
69 ColumnVector measNoiseOdom_Mu(6); measNoiseOdom_Mu = 0;
70 SymmetricMatrix measNoiseOdom_Cov(6); measNoiseOdom_Cov = 0;
71 for (
unsigned int i=1; i<=6; i++) measNoiseOdom_Cov(i,i) = 1;
72 Gaussian measurement_Uncertainty_Odom(measNoiseOdom_Mu, measNoiseOdom_Cov);
73 Matrix Hodom(6,6); Hodom = 0;
74 Hodom(1,1) = 1; Hodom(2,2) = 1; Hodom(6,6) = 1;
75 odom_meas_pdf_ =
new LinearAnalyticConditionalGaussian(Hodom, measurement_Uncertainty_Odom);
79 ColumnVector measNoiseImu_Mu(3); measNoiseImu_Mu = 0;
80 SymmetricMatrix measNoiseImu_Cov(3); measNoiseImu_Cov = 0;
81 for (
unsigned int i=1; i<=3; i++) measNoiseImu_Cov(i,i) = 1;
82 Gaussian measurement_Uncertainty_Imu(measNoiseImu_Mu, measNoiseImu_Cov);
83 Matrix Himu(3,6); Himu = 0;
84 Himu(1,4) = 1; Himu(2,5) = 1; Himu(3,6) = 1;
85 imu_meas_pdf_ =
new LinearAnalyticConditionalGaussian(Himu, measurement_Uncertainty_Imu);
89 ColumnVector measNoiseVo_Mu(6); measNoiseVo_Mu = 0;
90 SymmetricMatrix measNoiseVo_Cov(6); measNoiseVo_Cov = 0;
91 for (
unsigned int i=1; i<=6; i++) measNoiseVo_Cov(i,i) = 1;
92 Gaussian measurement_Uncertainty_Vo(measNoiseVo_Mu, measNoiseVo_Cov);
93 Matrix Hvo(6,6); Hvo = 0;
94 Hvo(1,1) = 1; Hvo(2,2) = 1; Hvo(3,3) = 1; Hvo(4,4) = 1; Hvo(5,5) = 1; Hvo(6,6) = 1;
95 vo_meas_pdf_ =
new LinearAnalyticConditionalGaussian(Hvo, measurement_Uncertainty_Vo);
99 ColumnVector measNoiseGps_Mu(3); measNoiseGps_Mu = 0;
100 SymmetricMatrix measNoiseGps_Cov(3); measNoiseGps_Cov = 0;
101 for (
unsigned int i=1; i<=3; i++) measNoiseGps_Cov(i,i) = 1;
102 Gaussian measurement_Uncertainty_GPS(measNoiseGps_Mu, measNoiseGps_Cov);
103 Matrix Hgps(3,6); Hgps = 0;
104 Hgps(1,1) = 1; Hgps(2,2) = 1; Hgps(3,3) = 1;
105 gps_meas_pdf_ =
new LinearAnalyticConditionalGaussian(Hgps, measurement_Uncertainty_GPS);
132 ColumnVector prior_Mu(6);
133 decomposeTransform(prior, prior_Mu(1), prior_Mu(2), prior_Mu(3), prior_Mu(4), prior_Mu(5), prior_Mu(6));
134 SymmetricMatrix prior_Cov(6);
135 for (
unsigned int i=1; i<=6; i++) {
136 for (
unsigned int j=1; j<=6; j++){
137 if (i==j) prior_Cov(i,j) = pow(0.001,2);
138 else prior_Cov(i,j) = 0;
141 prior_ =
new Gaussian(prior_Mu,prior_Cov);
159 bool OdomEstimation::update(
bool odom_active,
bool imu_active,
bool gps_active,
bool vo_active,
const Time& filter_time,
bool& diagnostics_res)
163 ROS_INFO(
"Cannot update filter when filter was not initialized first.");
169 if (dt == 0)
return false;
171 ROS_INFO(
"Will not update robot pose with time %f sec in the past.", dt);
174 ROS_DEBUG(
"Update filter at time %f with dt %f", filter_time.
toSec(), dt);
180 ColumnVector vel_desi(2); vel_desi = 0;
189 ROS_ERROR(
"filter time older than odom message buffer");
197 ColumnVector odom_rel(6);
198 decomposeTransform(odom_rel_frame, odom_rel(1), odom_rel(2), odom_rel(3), odom_rel(4), odom_rel(5), odom_rel(6));
203 ROS_DEBUG(
"Update filter with odom measurement %f %f %f %f %f %f",
204 odom_rel(1), odom_rel(2), odom_rel(3), odom_rel(4), odom_rel(5), odom_rel(6));
222 ROS_ERROR(
"filter time older than imu message buffer");
229 ColumnVector imu_rel(3);
double tmp;
253 ROS_ERROR(
"filter time older than vo message buffer");
260 ColumnVector vo_rel(6);
261 decomposeTransform(vo_rel_frame, vo_rel(1), vo_rel(2), vo_rel(3), vo_rel(4), vo_rel(5), vo_rel(6));
279 ROS_ERROR(
"filter time older than gps message buffer");
285 ColumnVector gps_vec(3);
311 diagnostics_res =
true;
312 if (odom_active && imu_active){
314 if (diagnostics > 0.3 && dt > 0.01){
315 diagnostics_res =
false;
324 ROS_DEBUG(
"AddMeasurement from %s to %s: (%f, %f, %f) (%f, %f, %f, %f)",
335 for (
unsigned int i=0; i<covar.rows(); i++){
336 if (covar(i+1,i+1) == 0){
385 ROS_ERROR(
"Cannot get transform at time %f", 0.0);
389 poseTFToMsg(tmp, estimate.pose.pose);
392 estimate.header.stamp = tmp.
stamp_;
396 SymmetricMatrix covar =
filter_->PostGet()->CovarianceGet();
397 for (
unsigned int i=0; i<6; i++)
398 for (
unsigned int j=0; j<6; j++)
399 estimate.pose.covariance[6*i+j] = covar(i+1,j+1);
405 while ((a-ref) > M_PI) a -= 2*M_PI;
406 while ((a-ref) < -M_PI) a += 2*M_PI;
411 double& x,
double& y,
double&z,
double&Rx,
double& Ry,
double& Rz){
420 double& x,
double& y,
double&z,
double&Rx,
double& Ry,
double& Rz){