uwsim: InertialMeasurementUnit.cpp Source File

Go to the documentation of this file.
00001 /* 
00002  * Copyright (c) 2013 University of Jaume-I.
00003  * All rights reserved. This program and the accompanying materials
00004  * are made available under the terms of the GNU Public License v3.0
00005  * which accompanies this distribution, and is available at
00006  * http://www.gnu.org/licenses/gpl.html
00007  * 
00008  * Contributors:
00009  *     Mario Prats
00010  *     Javier Perez
00011  */
00012 
00013 #include <uwsim/UWSimUtils.h>
00014 #include <uwsim/InertialMeasurementUnit.h>
00015 
00016 osg::Quat InertialMeasurementUnit::getMeasurement()
00017 {
00018   //Should get world coords and then transform to the localizedWorld
00019   boost::shared_ptr<osg::Matrix> rMi = getWorldCoords(imu_node_);
00020   osg::Matrixd lMi = *rMi * osg::Matrixd::inverse(rMl_);
00021 
00022   //Now add some gaussian noise
00023   static boost::normal_distribution<> normal(0, std_);
00024   static boost::variate_generator<boost::mt19937&, boost::normal_distribution<> > var_nor(rng_, normal);
00025 
00026   return lMi.getRotate()
00027       * osg::Quat(var_nor(), osg::Vec3d(1, 0, 0), var_nor(), osg::Vec3d(0, 1, 0), var_nor(), osg::Vec3d(0, 0, 1));
00028 }