Go to the documentation of this file.00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
00022
00023
00024
00025
00026
00027
00028
00029 #include <hector_pose_estimation/filter/ekf.h>
00030 #include <hector_pose_estimation/system.h>
00031
00032 #include <boost/pointer_cast.hpp>
00033
00034 #ifdef USE_HECTOR_TIMING
00035 #include <hector_diagnostics/timing.h>
00036 #endif
00037
00038 namespace hector_pose_estimation {
00039 namespace filter {
00040
00041 EKF::EKF(State &state)
00042 : Filter(state)
00043 {}
00044
00045 EKF::~EKF()
00046 {}
00047
00048 bool EKF::init(PoseEstimation &estimator)
00049 {
00050 x_diff = State::Vector(state_.getVectorDimension());
00051 A = State::SystemMatrix(state_.getCovarianceDimension(), state_.getCovarianceDimension());
00052 Q = State::Covariance(state_.getCovarianceDimension(), state_.getCovarianceDimension());
00053 return true;
00054 }
00055
00056 bool EKF::preparePredict(double dt)
00057 {
00058 x_diff.setZero();
00059 A.setIdentity();
00060 Q.setZero();
00061 return Filter::preparePredict(dt);
00062 }
00063
00064 bool EKF::predict(const SystemPtr& system, double dt)
00065 {
00066 if (!Filter::predict(system, dt)) return false;
00067 EKF::Predictor *predictor = boost::dynamic_pointer_cast<EKF::Predictor>(system->predictor());
00068 x_diff += predictor->x_diff;
00069 A += predictor->A;
00070 Q += predictor->Q;
00071 return true;
00072 }
00073
00074 bool EKF::doPredict(double dt) {
00075 ROS_DEBUG_NAMED("ekf.prediction", "EKF prediction (dt = %f):", dt);
00076
00077 ROS_DEBUG_STREAM_NAMED("ekf.prediction", "A = [" << std::endl << A << "]");
00078 ROS_DEBUG_STREAM_NAMED("ekf.prediction", "Q = [" << std::endl << Q << "]");
00079
00080 #ifdef USE_HECTOR_TIMING
00081 { hector_diagnostics::TimingSection section("predict.ekf.covariance");
00082 #endif
00083 state().P() = A * state().P() * A.transpose() + Q;
00084 state().P().assertSymmetric();
00085
00086 #ifdef USE_HECTOR_TIMING
00087 }
00088 { hector_diagnostics::TimingSection section("predict.ekf.state");
00089 #endif
00090 state().update(x_diff);
00091
00092 #ifdef USE_HECTOR_TIMING
00093 }
00094 #endif
00095
00096 ROS_DEBUG_STREAM_NAMED("ekf.prediction", "x_pred = [" << state().getVector().transpose() << "]");
00097 ROS_DEBUG_STREAM_NAMED("ekf.prediction", "P_pred = [" << std::endl << state().getCovariance() << "]");
00098
00099 Filter::doPredict(dt);
00100 return true;
00101 }
00102
00103 }
00104 }