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/system/imu_model.h> 00030 #include <hector_pose_estimation/pose_estimation.h> 00031 #include <hector_pose_estimation/filter/set_filter.h> 00032 00033 namespace hector_pose_estimation { 00034 00035 template class System_<GyroModel>; 00036 template class System_<AccelerometerModel>; 00037 00038 static const Matrix3 MinusIdentity = -Matrix3::Identity(); 00039 00040 GyroModel::GyroModel() 00041 { 00042 rate_stddev_ = 1.0 * M_PI/180.0; 00043 rate_drift_ = 1.0e-1 * M_PI/180.0; 00044 parameters().add("stddev", rate_stddev_); 00045 parameters().add("drift", rate_drift_); 00046 } 00047 00048 GyroModel::~GyroModel() 00049 {} 00050 00051 bool GyroModel::init(PoseEstimation& estimator, System &system, State& state) 00052 { 00053 bias_ = state.addSubState<3,3>(this, system.getName() + "_bias"); 00054 return bias_; 00055 } 00056 00057 void GyroModel::getPrior(State &state) 00058 { 00059 bias_->block(state.P()) = 3600./2. * pow(rate_drift_, 2) * SymmetricMatrix3::Identity(); 00060 } 00061 00062 void GyroModel::getSystemNoise(NoiseVariance& Q, const State& state, bool init) 00063 { 00064 if (!init) return; 00065 bias_->block(Q)(X,X) = bias_->block(Q)(Y,Y) = pow(rate_drift_, 2); 00066 bias_->block(Q)(Z,Z) = pow(rate_drift_, 2); 00067 } 00068 00069 ColumnVector3 GyroModel::getRate(const ImuInput::RateType& imu_rate, const State& state) const 00070 { 00071 return imu_rate - bias_->getVector(); 00072 } 00073 00074 void GyroModel::getRateJacobian(SystemMatrixBlock& C, const State& state, bool init) 00075 { 00076 if (!init) return; 00077 bias_->cols(C) = MinusIdentity; 00078 } 00079 00080 void GyroModel::getRateNoise(CovarianceBlock Q, const State &, bool init) 00081 { 00082 if (!init) return; 00083 Q(X,X) = Q(Y,Y) = Q(Z,Z) = pow(rate_stddev_, 2); 00084 } 00085 00086 //void GyroModel::getDerivative(StateVector &x_dot, const State &state) 00087 //{ 00088 // x_dot.setZero(); 00089 // if (state.orientation() && !state.rate()) { 00090 // state.orientation()->segment(x_dot).head(3) = state.R() * bias_->vector(); 00091 // } 00092 //} 00093 00094 //void GyroModel::getStateJacobian(SystemMatrix& A, const State& state) 00095 //{ 00096 // A.setZero(); 00097 // if (state.orientation() && !state.rate()) { 00098 // state.orientation()->block(A, *bias_) = state.R(); 00099 // } 00100 //} 00101 00102 AccelerometerModel::AccelerometerModel() 00103 { 00104 acceleration_stddev_ = 1.0e-2; 00105 acceleration_drift_ = 1.0e-2; 00106 parameters().add("stddev", acceleration_stddev_); 00107 parameters().add("drift", acceleration_drift_); 00108 } 00109 00110 AccelerometerModel::~AccelerometerModel() 00111 {} 00112 00113 bool AccelerometerModel::init(PoseEstimation& estimator, System &system, State& state) 00114 { 00115 bias_ = state.addSubState<3,3>(this, system.getName() + "_bias"); 00116 return bias_; 00117 } 00118 00119 void AccelerometerModel::getPrior(State &state) 00120 { 00121 bias_->block(state.P()) = 3600./2. * pow(acceleration_drift_, 2) * SymmetricMatrix3::Identity(); 00122 } 00123 00124 void AccelerometerModel::getSystemNoise(NoiseVariance& Q, const State&, bool init) 00125 { 00126 if (!init) return; 00127 bias_->block(Q)(X,X) = bias_->block(Q)(Y,Y) = pow(acceleration_drift_, 2); 00128 bias_->block(Q)(Z,Z) = pow(acceleration_drift_, 2); 00129 } 00130 00131 ColumnVector3 AccelerometerModel::getAcceleration(const ImuInput::AccelerationType& imu_acceleration, const State& state) const 00132 { 00133 return imu_acceleration - bias_->getVector(); 00134 } 00135 00136 void AccelerometerModel::getAccelerationJacobian(SystemMatrixBlock& C, const State&, bool init) 00137 { 00138 if (!init) return; 00139 bias_->cols(C) = MinusIdentity; 00140 } 00141 00142 void AccelerometerModel::getAccelerationNoise(CovarianceBlock Q, const State &, bool init) 00143 { 00144 if (!init) return; 00145 Q(X,X) = Q(Y,Y) = Q(Z,Z) = pow(acceleration_stddev_, 2); 00146 } 00147 00148 //void AccelerometerModel::getDerivative(StateVector &x_dot, const State &state) 00149 //{ 00150 // x_dot.setZero(); 00151 // if (state.velocity() && !state.acceleration()) { 00152 // if (state.getSystemStatus() & STATE_VELOCITY_XY) { 00153 // state.velocity()->segment(x_dot)(X) = bias_nav_.x(); 00154 // state.velocity()->segment(x_dot)(Y) = bias_nav_.y(); 00155 // } 00156 // if (state.getSystemStatus() & STATE_VELOCITY_Z) { 00157 // state.velocity()->segment(x_dot)(Z) = bias_nav_.z(); 00158 // } 00159 // } 00160 //} 00161 00162 //void AccelerometerModel::getStateJacobian(SystemMatrixBlock& A, const State& state) 00163 //{ 00164 // A.setZero(); 00165 // if (state.velocity() && !state.acceleration()) { 00166 // const State::RotationMatrix &R = state.R(); 00167 00168 // if (state.getSystemStatus() & STATE_VELOCITY_XY) { 00169 // state.velocity()->block(A, *bias_).row(X) = R.row(X); 00170 // state.velocity()->block(A, *bias_).row(Y) = R.row(Y); 00171 // } 00172 // if (state.getSystemStatus() & STATE_VELOCITY_Z) { 00173 // state.velocity()->block(A, *bias_).row(Z) = R.row(Z); 00174 // } 00175 00176 // if (state.getSystemStatus() & STATE_VELOCITY_XY) { 00177 // state.velocity()->block(A, *state.orientation())(X,X) = 0.0; 00178 // state.velocity()->block(A, *state.orientation())(X,Y) = bias_nav_.z(); 00179 // state.velocity()->block(A, *state.orientation())(X,Z) = -bias_nav_.y(); 00180 00181 // state.velocity()->block(A, *state.orientation())(Y,X) = -bias_nav_.z(); 00182 // state.velocity()->block(A, *state.orientation())(Y,Y) = 0.0; 00183 // state.velocity()->block(A, *state.orientation())(Y,Z) = bias_nav_.x(); 00184 // } 00185 00186 // if (state.getSystemStatus() & STATE_VELOCITY_Z) { 00187 // state.velocity()->block(A, *state.orientation())(Z,X) = bias_nav_.y(); 00188 // state.velocity()->block(A, *state.orientation())(Z,Y) = -bias_nav_.x(); 00189 // state.velocity()->block(A, *state.orientation())(Z,Z) = 0.0; 00190 // } 00191 // } 00192 //} 00193 00194 } // namespace hector_pose_estimation