00001 //================================================================================================= 00002 // Copyright (c) 2011, 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/measurements/rate.h> 00030 #include <hector_pose_estimation/filter/set_filter.h> 00031 00032 namespace hector_pose_estimation { 00033 00034 template class Measurement_<RateModel>; 00035 00036 RateModel::RateModel() 00037 { 00038 parameters().add("stddev", stddev_, 1.0*M_PI/180.0); 00039 } 00040 00041 RateModel::~RateModel() {} 00042 00043 bool RateModel::init(PoseEstimation &estimator, State &state) 00044 { 00045 gyro_drift_ = state.addSubState<3>(this, "gyro"); 00046 return true; 00047 } 00048 00049 void RateModel::getMeasurementNoise(NoiseVariance& R, const State&, bool init) 00050 { 00051 if (init) { 00052 R(0,0) = R(1,1) = R(2,2) = pow(stddev_, 2); 00053 } 00054 } 00055 00056 void RateModel::getExpectedValue(MeasurementVector& y_pred, const State& state) 00057 { 00058 // const State::OrientationType& q = state.getOrientation(); 00059 // const State::RateType& rate = state.getRate(); 00060 00061 // y_pred(0) = (q.w()*q.w()+q.x()*q.x()-q.y()*q.y()-q.z()*q.z()) * rate.x() + (2.0*q.x()*q.y()+2.0*q.w()*q.z()) * rate.y() + (2.0*q.x()*q.z()-2.0*q.w()*q.y()) * rate.z(); 00062 // y_pred(1) = (2.0*q.x()*q.y()-2.0*q.w()*q.z()) * rate.x() + (q.w()*q.w()-q.x()*q.x()+q.y()*q.y()-q.z()*q.z()) * rate.y() + (2.0*q.y()*q.z()+2.0*q.w()*q.x()) * rate.z(); 00063 // y_pred(2) = (2.0*q.x()*q.z()+2.0*q.w()*q.y()) * rate.x() + (2.0*q.y()*q.z()-2.0*q.w()*q.x()) * rate.y() + (q.w()*q.w()-q.x()*q.x()-q.y()*q.y()+q.z()*q.z()) * rate.z(); 00064 00065 y_pred = state.getRate(); 00066 00067 if (gyro_drift_) { 00068 y_pred += gyro_drift_->getVector(); 00069 } 00070 } 00071 00072 void RateModel::getStateJacobian(MeasurementMatrix &C0, SubMeasurementMatrix &C1, const State &state, bool init) 00073 { 00074 // const State::OrientationType& q = state.getOrientation(); 00075 // const State::RateType& rate = state.getRate(); 00076 00077 // if (state.getOrientationIndex() >= 0) { 00078 // C(0,State::QUATERNION_W) = 2.0*q.w() * rate.x() + 2.0*q.z() * rate.y() - 2.0*q.y() * rate.z(); 00079 // C(0,State::QUATERNION_X) = 2.0*q.x() * rate.x() + 2.0*q.y() * rate.y() + 2.0*q.z() * rate.z(); 00080 // C(0,State::QUATERNION_Y) = -2.0*q.y() * rate.x() + 2.0*q.x() * rate.y() - 2.0*q.w() * rate.z(); 00081 // C(0,State::QUATERNION_Z) = -2.0*q.z() * rate.x() + 2.0*q.w() * rate.y() + 2.0*q.x() * rate.z(); 00082 // C(1,State::QUATERNION_W) = -2.0*q.z() * rate.x() + 2.0*q.w() * rate.y() + 2.0*q.x() * rate.z(); 00083 // C(1,State::QUATERNION_X) = 2.0*q.y() * rate.x() - 2.0*q.x() * rate.y() + 2.0*q.w() * rate.z(); 00084 // C(1,State::QUATERNION_Y) = 2.0*q.x() * rate.x() + 2.0*q.y() * rate.y() + 2.0*q.z() * rate.z(); 00085 // C(1,State::QUATERNION_Z) = -2.0*q.w() * rate.x() - 2.0*q.z() * rate.y() + 2.0*q.y() * rate.z(); 00086 // C(2,State::QUATERNION_W) = 2.0*q.y() * rate.x() - 2.0*q.x() * rate.y() + 2.0*q.w() * rate.z(); 00087 // C(2,State::QUATERNION_X) = 2.0*q.z() * rate.x() - 2.0*q.w() * rate.y() - 2.0*q.x() * rate.z(); 00088 // C(2,State::QUATERNION_Y) = 2.0*q.w() * rate.x() + 2.0*q.z() * rate.y() - 2.0*q.y() * rate.z(); 00089 // C(2,State::QUATERNION_Z) = 2.0*q.x() * rate.x() + 2.0*q.y() * rate.y() + 2.0*q.z() * rate.z(); 00090 // } 00091 00092 // if (state.getRateIndex() >= 0) { 00093 // C(0,State::RATE_X) = (q.w()*q.w()+q.x()*q.x()-q.y()*q.y()-q.z()*q.z()); 00094 // C(0,State::RATE_Y) = (2.0*q.x()*q.y()+2.0*q.w()*q.z()); 00095 // C(0,State::RATE_Z) = (2.0*q.x()*q.z()-2.0*q.w()*q.y()); 00096 // C(1,State::RATE_X) = (2.0*q.x()*q.y()-2.0*q.w()*q.z()); 00097 // C(1,State::RATE_Y) = (q.w()*q.w()-q.x()*q.x()+q.y()*q.y()-q.z()*q.z()); 00098 // C(1,State::RATE_Z) = (2.0*q.y()*q.z()+2.0*q.w()*q.x()); 00099 // C(2,State::RATE_X) = (2.0*q.x()*q.z()+2.0*q.w()*q.y()); 00100 // C(2,State::RATE_Y) = (2.0*q.y()*q.z()-2.0*q.w()*q.x()); 00101 // C(2,State::RATE_Z) = (q.w()*q.w()-q.x()*q.x()-q.y()*q.y()+q.z()*q.z()); 00102 // } 00103 00104 if (!init) return; 00105 00106 if (state.getRateIndex() >= 0) { 00107 C0(0,State::RATE_X) = 1.0; 00108 C0(1,State::RATE_Y) = 1.0; 00109 C0(2,State::RATE_Z) = 1.0; 00110 } 00111 00112 if (gyro_drift_) { 00113 C1.setIdentity(); 00114 } 00115 } 00116 00117 } // namespace hector_pose_estimation