$search
00001 //================================================================================================= 00002 // Copyright (c) 2011, Johannes Meyer and Martin Nowara, 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/generic_quaternion_system_model.h> 00030 00031 namespace hector_pose_estimation { 00032 00033 static const double GRAVITY = -9.8065; 00034 00035 GenericQuaternionSystemModel::GenericQuaternionSystemModel() 00036 { 00037 gravity_ = GRAVITY; 00038 #ifdef USE_RATE_SYSTEM_MODEL 00039 rate_stddev_ = 0.0; 00040 angular_acceleration_stddev_ = 10.0 * M_PI/180.0; 00041 #else // USE_RATE_SYSTEM_MODEL 00042 rate_stddev_ = 1.0 * M_PI/180.0; 00043 #endif // USE_RATE_SYSTEM_MODEL 00044 acceleration_stddev_ = 1.0e-2; 00045 velocity_stddev_ = 0.0; 00046 acceleration_drift_ = 1.0e-6; 00047 rate_drift_ = 1.0e-2 * M_PI/180.0; 00048 parameters().add("gravity", gravity_); 00049 parameters().add("rate_stddev", rate_stddev_); 00050 #ifdef USE_RATE_SYSTEM_MODEL 00051 parameters().add("angular_acceleration_stddev", angular_acceleration_stddev_); 00052 #endif // USE_RATE_SYSTEM_MODEL 00053 parameters().add("acceleration_stddev", acceleration_stddev_); 00054 parameters().add("velocity_stddev", velocity_stddev_); 00055 parameters().add("acceleration_drift", acceleration_drift_); 00056 parameters().add("rate_drift", rate_drift_); 00057 } 00058 00059 bool GenericQuaternionSystemModel::init() 00060 { 00061 noise_ = 0.0; 00062 noise_(QUATERNION_W,QUATERNION_W) = noise_(QUATERNION_X,QUATERNION_X) = noise_(QUATERNION_Y,QUATERNION_Y) = noise_(QUATERNION_Z,QUATERNION_Z) = pow(0.5 * rate_stddev_, 2); // will be overridden in CovarianceGet() ! 00063 #ifdef USE_RATE_SYSTEM_MODEL 00064 noise_(RATE_X,RATE_X) = noise_(RATE_Y,RATE_Y) = noise_(RATE_Z,RATE_Z) = pow(angular_acceleration_stddev_, 2); 00065 #endif // USE_RATE_SYSTEM_MODEL 00066 noise_(POSITION_X,POSITION_X) = noise_(POSITION_Y,POSITION_Y) = noise_(POSITION_Z,POSITION_Z) = pow(velocity_stddev_, 2); 00067 noise_(VELOCITY_X,VELOCITY_X) = noise_(VELOCITY_Y,VELOCITY_Y) = noise_(VELOCITY_Z,VELOCITY_Z) = pow(acceleration_stddev_, 2); 00068 noise_(BIAS_ACCEL_X,BIAS_ACCEL_X) = noise_(BIAS_ACCEL_Y,BIAS_ACCEL_Y) = pow(acceleration_drift_, 2); 00069 noise_(BIAS_ACCEL_Z,BIAS_ACCEL_Z) = pow(acceleration_drift_, 2); 00070 noise_(BIAS_GYRO_X,BIAS_GYRO_X) = noise_(BIAS_GYRO_Y,BIAS_GYRO_Y) = noise_(BIAS_GYRO_Z,BIAS_GYRO_Z) = pow(rate_drift_, 2); 00071 this->AdditiveNoiseSigmaSet(noise_); 00072 return true; 00073 } 00074 00075 GenericQuaternionSystemModel::~GenericQuaternionSystemModel() 00076 { 00077 } 00078 00079 SystemStatus GenericQuaternionSystemModel::getStatusFlags() const 00080 { 00081 SystemStatus flags = measurement_status_; 00082 // flags |= STATE_XY_POSITION | STATE_Z_POSITION; 00083 if (flags & STATE_XY_POSITION) flags |= STATE_XY_VELOCITY; 00084 if (flags & STATE_Z_POSITION) flags |= STATE_Z_VELOCITY; 00085 if (flags & STATE_XY_VELOCITY) flags |= STATE_ROLLPITCH; 00086 return flags; 00087 } 00088 00089 //--> System equation of this model xpred = x_(k+1) = f(x,u) 00090 ColumnVector GenericQuaternionSystemModel::ExpectedValueGet(double dt) const 00091 { 00092 x_pred_ = x_; 00093 00094 //--> Enhance readability 00095 //---------------------------------------------------------- 00096 double abx = u_(ImuInput::ACCEL_X) + x_(BIAS_ACCEL_X); 00097 double aby = u_(ImuInput::ACCEL_Y) + x_(BIAS_ACCEL_Y); 00098 double abz = u_(ImuInput::ACCEL_Z) + x_(BIAS_ACCEL_Z); 00099 #ifdef USE_RATE_SYSTEM_MODEL 00100 double wnx = x_(RATE_X); 00101 double wny = x_(RATE_Y); 00102 double wnz = x_(RATE_Z); 00103 #else // USE_RATE_SYSTEM_MODEL 00104 double wbx = u_(ImuInput::GYRO_X) + x_(BIAS_GYRO_X); 00105 double wby = u_(ImuInput::GYRO_Y) + x_(BIAS_GYRO_Y); 00106 double wbz = u_(ImuInput::GYRO_Z) + x_(BIAS_GYRO_Z); 00107 #endif // USE_RATE_SYSTEM_MODEL 00108 00109 q0 = x_(QUATERNION_W); 00110 q1 = x_(QUATERNION_X); 00111 q2 = x_(QUATERNION_Y); 00112 q3 = x_(QUATERNION_Z); 00113 double p_x = x_(POSITION_X); 00114 double p_y = x_(POSITION_Y); 00115 double p_z = x_(POSITION_Z); 00116 double v_x = x_(VELOCITY_X); 00117 double v_y = x_(VELOCITY_Y); 00118 double v_z = x_(VELOCITY_Z); 00119 //---------------------------------------------------------- 00120 00121 //--> Attitude 00122 //---------------------------------------------------------- 00123 #ifdef USE_RATE_SYSTEM_MODEL 00124 x_pred_(QUATERNION_W) = q0 + dt*0.5*( (-wnx)*q1+(-wny)*q2+(-wnz)*q3); 00125 x_pred_(QUATERNION_X) = q1 + dt*0.5*(( wnx)*q0 +(-wnz)*q2+( wny)*q3); 00126 x_pred_(QUATERNION_Y) = q2 + dt*0.5*(( wny)*q0+( wnz)*q1 +(-wnx)*q3); 00127 x_pred_(QUATERNION_Z) = q3 + dt*0.5*(( wnz)*q0+(-wny)*q1+( wnx)*q2 ); 00128 #else // USE_RATE_SYSTEM_MODEL 00129 x_pred_(QUATERNION_W) = q0 + dt*0.5*( (-wbx)*q1+(-wby)*q2+(-wbz)*q3); 00130 x_pred_(QUATERNION_X) = q1 + dt*0.5*(( wbx)*q0 +( wbz)*q2+(-wby)*q3); 00131 x_pred_(QUATERNION_Y) = q2 + dt*0.5*(( wby)*q0+(-wbz)*q1 +( wbx)*q3); 00132 x_pred_(QUATERNION_Z) = q3 + dt*0.5*(( wbz)*q0+( wby)*q1+(-wbx)*q2 ); 00133 #endif // USE_RATE_SYSTEM_MODEL 00134 //---------------------------------------------------------- 00135 00136 //--> Velocity (without coriolis forces) and Position 00137 //---------------------------------------------------------- 00138 if (getStatusFlags() & STATE_XY_VELOCITY) { 00139 x_pred_(VELOCITY_X) = v_x + dt*((q0*q0+q1*q1-q2*q2-q3*q3)*abx + (2.0*q1*q2-2.0*q0*q3) *aby + (2.0*q1*q3+2.0*q0*q2) *abz); 00140 x_pred_(VELOCITY_Y) = v_y + dt*((2.0*q1*q2+2.0*q0*q3) *abx + (q0*q0-q1*q1+q2*q2-q3*q3)*aby + (2.0*q2*q3-2.0*q0*q1) *abz); 00141 } 00142 if (getStatusFlags() & STATE_Z_VELOCITY) { 00143 x_pred_(VELOCITY_Z) = v_z + dt*((2.0*q1*q3-2.0*q0*q2) *abx + (2.0*q2*q3+2.0*q0*q1) *aby + (q0*q0-q1*q1-q2*q2+q3*q3)*abz + gravity_); 00144 } 00145 00146 if (getStatusFlags() & STATE_XY_POSITION) { 00147 x_pred_(POSITION_X) = p_x + dt*(v_x); 00148 x_pred_(POSITION_Y) = p_y + dt*(v_y); 00149 } 00150 if (getStatusFlags() & STATE_Z_POSITION) { 00151 x_pred_(POSITION_Z) = p_z + dt*(v_z); 00152 } 00153 //---------------------------------------------------------- 00154 00155 return x_pred_ + AdditiveNoiseMuGet(); 00156 } 00157 00158 //--> Covariance 00159 // Warning: CovarianceGet() must be called AFTER ExpectedValueGet(...) or dfGet(...) 00160 // unfortunately MatrixWrapper::SymmetricMatrix CovarianceGet(const MatrixWrapper::ColumnVector& u, const MatrixWrapper::ColumnVector& x) cannot be overridden 00161 SymmetricMatrix GenericQuaternionSystemModel::CovarianceGet(double dt) const 00162 { 00163 double rate_variance_4 = 0.25 * pow(rate_stddev_, 2); 00164 noise_(QUATERNION_W,QUATERNION_W) = rate_variance_4 * (q1*q1+q2*q2+q3*q3); 00165 noise_(QUATERNION_X,QUATERNION_X) = rate_variance_4 * (q0*q0+q2*q2+q3*q3); 00166 noise_(QUATERNION_Y,QUATERNION_Y) = rate_variance_4 * (q0*q0+q1*q1+q3*q3); 00167 noise_(QUATERNION_Z,QUATERNION_Z) = rate_variance_4 * (q0*q0+q1*q1+q2*q2); 00168 // return noise_ * (dt*dt); 00169 return noise_ * dt; 00170 } 00171 00172 //--> Jacobian matrix A 00173 Matrix GenericQuaternionSystemModel::dfGet(unsigned int i, double dt) const 00174 { 00175 if (i != 0) return Matrix(); 00176 00177 //--> Enhance readability 00178 //---------------------------------------------------------- 00179 double abx = u_(ImuInput::ACCEL_X) + x_(BIAS_ACCEL_X); 00180 double aby = u_(ImuInput::ACCEL_Y) + x_(BIAS_ACCEL_Y); 00181 double abz = u_(ImuInput::ACCEL_Z) + x_(BIAS_ACCEL_Z); 00182 #ifdef USE_RATE_SYSTEM_MODEL 00183 double wnx = x_(RATE_X); 00184 double wny = x_(RATE_Y); 00185 double wnz = x_(RATE_Z); 00186 #else // USE_RATE_SYSTEM_MODEL 00187 double wbx = u_(ImuInput::GYRO_X) + x_(BIAS_GYRO_X); 00188 double wby = u_(ImuInput::GYRO_Y) + x_(BIAS_GYRO_Y); 00189 double wbz = u_(ImuInput::GYRO_Z) + x_(BIAS_GYRO_Z); 00190 #endif // USE_RATE_SYSTEM_MODEL 00191 00192 q0 = x_(QUATERNION_W); 00193 q1 = x_(QUATERNION_X); 00194 q2 = x_(QUATERNION_Y); 00195 q3 = x_(QUATERNION_Z); 00196 //---------------------------------------------------------- 00197 00198 //--> Set A-Matrix 00199 //---------------------------------------------------------- 00200 #ifdef USE_RATE_SYSTEM_MODEL 00201 A_(QUATERNION_W,QUATERNION_X) = dt*(-0.5*wnx); 00202 A_(QUATERNION_W,QUATERNION_Y) = dt*(-0.5*wny); 00203 A_(QUATERNION_W,QUATERNION_Z) = dt*(-0.5*wnz); 00204 A_(QUATERNION_W,RATE_X) = -0.5*dt*q1; 00205 A_(QUATERNION_W,RATE_Y) = -0.5*dt*q2; 00206 A_(QUATERNION_W,RATE_Z) = -0.5*dt*q3; 00207 00208 A_(QUATERNION_X,QUATERNION_W) = dt*( 0.5*wnx); 00209 A_(QUATERNION_X,QUATERNION_Y) = dt*(-0.5*wnz); 00210 A_(QUATERNION_X,QUATERNION_Z) = dt*(+0.5*wny); 00211 A_(QUATERNION_X,RATE_X) = 0.5*dt*q0; 00212 A_(QUATERNION_X,RATE_Y) = 0.5*dt*q3; 00213 A_(QUATERNION_X,RATE_Z) = -0.5*dt*q2; 00214 00215 A_(QUATERNION_Y,QUATERNION_W) = dt*( 0.5*wny); 00216 A_(QUATERNION_Y,QUATERNION_X) = dt*( 0.5*wnz); 00217 A_(QUATERNION_Y,QUATERNION_Z) = dt*(-0.5*wnx); 00218 A_(QUATERNION_Y,RATE_X) = -0.5*dt*q3; 00219 A_(QUATERNION_Y,RATE_Y) = 0.5*dt*q0; 00220 A_(QUATERNION_Y,RATE_Z) = 0.5*dt*q1; 00221 00222 A_(QUATERNION_Z,QUATERNION_W) = dt*( 0.5*wnz); 00223 A_(QUATERNION_Z,QUATERNION_X) = dt*(-0.5*wny); 00224 A_(QUATERNION_Z,QUATERNION_Y) = dt*(+0.5*wnx); 00225 A_(QUATERNION_Z,RATE_X) = 0.5*dt*q2; 00226 A_(QUATERNION_Z,RATE_Y) = -0.5*dt*q1; 00227 A_(QUATERNION_Z,RATE_Z) = 0.5*dt*q0; 00228 #else // USE_RATE_SYSTEM_MODEL 00229 A_(QUATERNION_W,QUATERNION_X) = dt*(-0.5*wbx); 00230 A_(QUATERNION_W,QUATERNION_Y) = dt*(-0.5*wby); 00231 A_(QUATERNION_W,QUATERNION_Z) = dt*(-0.5*wbz); 00232 A_(QUATERNION_W,BIAS_GYRO_X) = -0.5*dt*q1; 00233 A_(QUATERNION_W,BIAS_GYRO_Y) = -0.5*dt*q2; 00234 A_(QUATERNION_W,BIAS_GYRO_Z) = -0.5*dt*q3; 00235 00236 A_(QUATERNION_X,QUATERNION_W) = dt*( 0.5*wbx); 00237 A_(QUATERNION_X,QUATERNION_Y) = dt*( 0.5*wbz); 00238 A_(QUATERNION_X,QUATERNION_Z) = dt*(-0.5*wby); 00239 A_(QUATERNION_X,BIAS_GYRO_X) = 0.5*dt*q0; 00240 A_(QUATERNION_X,BIAS_GYRO_Y) = -0.5*dt*q3; 00241 A_(QUATERNION_X,BIAS_GYRO_Z) = 0.5*dt*q2; 00242 00243 A_(QUATERNION_Y,QUATERNION_W) = dt*( 0.5*wby); 00244 A_(QUATERNION_Y,QUATERNION_X) = dt*(-0.5*wbz); 00245 A_(QUATERNION_Y,QUATERNION_Z) = dt*( 0.5*wbx); 00246 A_(QUATERNION_Y,BIAS_GYRO_X) = 0.5*dt*q3; 00247 A_(QUATERNION_Y,BIAS_GYRO_Y) = 0.5*dt*q0; 00248 A_(QUATERNION_Y,BIAS_GYRO_Z) = -0.5*dt*q1; 00249 00250 A_(QUATERNION_Z,QUATERNION_W) = dt*( 0.5*wbz); 00251 A_(QUATERNION_Z,QUATERNION_X) = dt*( 0.5*wby); 00252 A_(QUATERNION_Z,QUATERNION_Y) = dt*(-0.5*wbx); 00253 A_(QUATERNION_Z,BIAS_GYRO_X) = -0.5*dt*q2; 00254 A_(QUATERNION_Z,BIAS_GYRO_Y) = 0.5*dt*q1; 00255 A_(QUATERNION_Z,BIAS_GYRO_Z) = 0.5*dt*q0; 00256 #endif // USE_RATE_SYSTEM_MODEL 00257 00258 if (getStatusFlags() & STATE_XY_VELOCITY) { 00259 A_(VELOCITY_X,QUATERNION_W) = dt*(-2.0*q3*aby+2.0*q2*abz+2.0*q0*abx); 00260 A_(VELOCITY_X,QUATERNION_X) = dt*( 2.0*q2*aby+2.0*q3*abz+2.0*q1*abx); 00261 A_(VELOCITY_X,QUATERNION_Y) = dt*(-2.0*q2*abx+2.0*q1*aby+2.0*q0*abz); 00262 A_(VELOCITY_X,QUATERNION_Z) = dt*(-2.0*q3*abx-2.0*q0*aby+2.0*q1*abz); 00263 A_(VELOCITY_X,BIAS_ACCEL_X) = dt*(q0*q0+q1*q1-q2*q2-q3*q3); 00264 A_(VELOCITY_X,BIAS_ACCEL_Y) = dt*(2.0*q1*q2-2.0*q0*q3); 00265 A_(VELOCITY_X,BIAS_ACCEL_Z) = dt*(2.0*q1*q3+2.0*q0*q2); 00266 00267 A_(VELOCITY_Y,QUATERNION_W) = dt*(2.0*q3*abx-2.0*q1*abz+2.0*q0*aby); 00268 A_(VELOCITY_Y,QUATERNION_X) = dt*(2.0*q2*abx-2.0*q1*aby-2.0*q0*abz); 00269 A_(VELOCITY_Y,QUATERNION_Y) = dt*(2.0*q1*abx+2.0*q3*abz+2.0*q2*aby); 00270 A_(VELOCITY_Y,QUATERNION_Z) = dt*(2.0*q0*abx-2.0*q3*aby+2.0*q2*abz); 00271 A_(VELOCITY_Y,BIAS_ACCEL_X) = dt*(2.0*q1*q2+2.0*q0*q3); 00272 A_(VELOCITY_Y,BIAS_ACCEL_Y) = dt*(q0*q0-q1*q1+q2*q2-q3*q3); 00273 A_(VELOCITY_Y,BIAS_ACCEL_Z) = dt*(2.0*q2*q3-2.0*q0*q1); 00274 00275 } else { 00276 A_(VELOCITY_X,QUATERNION_W) = 0.0; 00277 A_(VELOCITY_X,QUATERNION_X) = 0.0; 00278 A_(VELOCITY_X,QUATERNION_Y) = 0.0; 00279 A_(VELOCITY_X,QUATERNION_Z) = 0.0; 00280 A_(VELOCITY_X,BIAS_ACCEL_X) = 0.0; 00281 A_(VELOCITY_X,BIAS_ACCEL_Y) = 0.0; 00282 A_(VELOCITY_X,BIAS_ACCEL_Z) = 0.0; 00283 00284 A_(VELOCITY_Y,QUATERNION_W) = 0.0; 00285 A_(VELOCITY_Y,QUATERNION_X) = 0.0; 00286 A_(VELOCITY_Y,QUATERNION_Y) = 0.0; 00287 A_(VELOCITY_Y,QUATERNION_Z) = 0.0; 00288 A_(VELOCITY_Y,BIAS_ACCEL_X) = 0.0; 00289 A_(VELOCITY_Y,BIAS_ACCEL_Y) = 0.0; 00290 A_(VELOCITY_Y,BIAS_ACCEL_Z) = 0.0; 00291 } 00292 00293 if (getStatusFlags() & STATE_Z_VELOCITY) { 00294 A_(VELOCITY_Z,QUATERNION_W) = dt*(-2.0*q2*abx+2.0*q1*aby+2.0*q0*abz); 00295 A_(VELOCITY_Z,QUATERNION_X) = dt*( 2.0*q3*abx+2.0*q0*aby-2.0*q1*abz); 00296 A_(VELOCITY_Z,QUATERNION_Y) = dt*(-2.0*q0*abx+2.0*q3*aby-2.0*q2*abz); 00297 A_(VELOCITY_Z,QUATERNION_Z) = dt*( 2.0*q1*abx+2.0*q2*aby+2.0*q3*abz); 00298 A_(VELOCITY_Z,BIAS_ACCEL_X) = dt*( 2.0*q1*q3-2.0*q0*q2); 00299 A_(VELOCITY_Z,BIAS_ACCEL_Y) = dt*( 2.0*q2*q3+2.0*q0*q1); 00300 A_(VELOCITY_Z,BIAS_ACCEL_Z) = dt*(q0*q0-q1*q1-q2*q2+q3*q3); 00301 00302 } else { 00303 A_(VELOCITY_Z,QUATERNION_W) = 0.0; 00304 A_(VELOCITY_Z,QUATERNION_X) = 0.0; 00305 A_(VELOCITY_Z,QUATERNION_Y) = 0.0; 00306 A_(VELOCITY_Z,QUATERNION_Z) = 0.0; 00307 A_(VELOCITY_Z,BIAS_ACCEL_X) = 0.0; 00308 A_(VELOCITY_Z,BIAS_ACCEL_Y) = 0.0; 00309 A_(VELOCITY_Z,BIAS_ACCEL_Z) = 0.0; 00310 } 00311 00312 if (getStatusFlags() & STATE_XY_POSITION) { 00313 A_(POSITION_X,VELOCITY_X) = dt; 00314 A_(POSITION_Y,VELOCITY_Y) = dt; 00315 } else { 00316 A_(POSITION_X,VELOCITY_X) = 0.0; 00317 A_(POSITION_Y,VELOCITY_Y) = 0.0; 00318 } 00319 00320 if (getStatusFlags() & STATE_Z_POSITION) { 00321 A_(POSITION_Z,VELOCITY_Z) = dt; 00322 } else { 00323 A_(POSITION_Z,VELOCITY_Z) = 0.0; 00324 } 00325 //---------------------------------------------------------- 00326 00327 return A_; 00328 } 00329 00330 void GenericQuaternionSystemModel::Limit(StateVector& x) const { 00331 normalize(x); 00332 } 00333 00334 void GenericQuaternionSystemModel::normalize(StateVector& x) { 00335 double s = 1.0/sqrt(x(QUATERNION_W)*x(QUATERNION_W)+x(QUATERNION_X)*x(QUATERNION_X)+x(QUATERNION_Y)*x(QUATERNION_Y)+x(QUATERNION_Z)*x(QUATERNION_Z)); 00336 x(QUATERNION_W) *= s; 00337 x(QUATERNION_X) *= s; 00338 x(QUATERNION_Y) *= s; 00339 x(QUATERNION_Z) *= s; 00340 } 00341 00342 } // namespace hector_pose_estimation