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.h> 00030 #include <hector_pose_estimation/pose_estimation.h> 00031 00032 #ifdef USE_HECTOR_TIMING 00033 #include <hector_diagnostics/timing.h> 00034 #endif 00035 00036 namespace hector_pose_estimation { 00037 00038 Filter::Filter(State &state) 00039 : state_(state) 00040 { 00041 } 00042 00043 Filter::~Filter() 00044 { 00045 } 00046 00047 bool Filter::init(PoseEstimation& estimator) 00048 { 00049 return true; 00050 } 00051 00052 void Filter::cleanup() 00053 { 00054 } 00055 00056 void Filter::reset() 00057 { 00058 state_.reset(); 00059 } 00060 00061 bool Filter::preparePredict(double) 00062 { 00063 return true; 00064 } 00065 00066 bool Filter::predict(const Systems& systems, double dt) { 00067 bool result = true; 00068 00069 #ifdef USE_HECTOR_TIMING 00070 hector_diagnostics::TimingSection section("predict"); 00071 #endif 00072 00073 if (!preparePredict(dt)) return false; 00074 00075 // Iterate through system models. For an EKF, this will populate the x_diff vector, A and Q matrices. 00076 for(Systems::iterator it = systems.begin(); it != systems.end(); it++) { 00077 const SystemPtr& system = *it; 00078 result &= predict(system, dt); 00079 } 00080 00081 // Call the filter's global predict method. This will actually calculate the updated state vector and variance. 00082 result &= doPredict(dt); 00083 00084 return result; 00085 } 00086 00087 bool Filter::predict(const SystemPtr& system, double dt) { 00088 #ifdef USE_HECTOR_TIMING 00089 hector_diagnostics::TimingSection section("predict." + system->getName()); 00090 #endif 00091 return system->update(dt); 00092 } 00093 00094 bool Filter::doPredict(double dt) { 00095 // already done in System::update() 00096 // state_.updated(); 00097 return true; 00098 } 00099 00100 bool Filter::prepareCorrect() 00101 { 00102 return true; 00103 } 00104 00105 bool Filter::correct(const Measurements& measurements) { 00106 bool result = true; 00107 00108 #ifdef USE_HECTOR_TIMING 00109 hector_diagnostics::TimingSection section("correct"); 00110 #endif 00111 00112 if (!prepareCorrect()) return false; 00113 00114 // Iterate through measurement models. This will process the correction step directly. 00115 for(Measurements::iterator it = measurements.begin(); it != measurements.end(); it++) { 00116 const MeasurementPtr& measurement = *it; 00117 result &= correct(measurement); 00118 } 00119 00120 // Call the filter's global correct method. No-op for EKF. 00121 result &= doCorrect(); 00122 00123 return result; 00124 } 00125 00126 bool Filter::correct(const MeasurementPtr& measurement) { 00127 #ifdef USE_HECTOR_TIMING 00128 hector_diagnostics::TimingSection section("correct." + measurement->getName()); 00129 #endif 00130 return measurement->process(); 00131 } 00132 00133 bool Filter::doCorrect() { 00134 // already done in Measurement::update() 00135 // state_.updated(); 00136 return true; 00137 } 00138 00139 } // namespace hector_pose_estimation