00001 //================================================================================================= 00002 // Copyright (c) 2013, Johannes Meyer, TU Darmstadt 00003 // All rights reserved. 00004 00005 // Redistribution and use in source and binary forms, with or without 00006 // modification, are permitted provided that the following conditions are met: 00007 // * Redistributions of source code must retain the above copyright 00008 // notice, this list of conditions and the following disclaimer. 00009 // * Redistributions in binary form must reproduce the above copyright 00010 // notice, this list of conditions and the following disclaimer in the 00011 // documentation and/or other materials provided with the distribution. 00012 // * Neither the name of the Flight Systems and Automatic Control group, 00013 // TU Darmstadt, nor the names of its contributors may be used to 00014 // endorse or promote products derived from this software without 00015 // specific prior written permission. 00016 00017 // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND 00018 // ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED 00019 // WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE 00020 // DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER BE LIABLE FOR ANY 00021 // DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES 00022 // (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 00023 // LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND 00024 // ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT 00025 // (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS 00026 // SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 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()); 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 00085 #ifdef USE_HECTOR_TIMING 00086 } 00087 { hector_diagnostics::TimingSection section("predict.ekf.state"); 00088 #endif 00089 state().update(x_diff); 00090 00091 #ifdef USE_HECTOR_TIMING 00092 } 00093 #endif 00094 00095 ROS_DEBUG_STREAM_NAMED("ekf.prediction", "x_pred = [" << state().getVector().transpose() << "]"); 00096 ROS_DEBUG_STREAM_NAMED("ekf.prediction", "P_pred = [" << std::endl << state().getCovariance() << "]"); 00097 00098 Filter::doPredict(dt); 00099 return true; 00100 } 00101 00102 } // namespace filter 00103 } // namespace hector_pose_estimation