generic_quaternion_system_model.cpp
Go to the documentation of this file.
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
 All Classes Namespaces Files Functions Variables Typedefs Enumerations Enumerator Properties Friends

hector_pose_estimation_core
Author(s): Johannes Meyer
autogenerated on Mon Jul 15 2013 16:48:43