Main Page | Modules | Namespace List | Class Hierarchy | Alphabetical List | Class List | Directories | File List | Namespace Members | Class Members | File Members | Related Pages | Examples

Observer.cpp

Go to the documentation of this file.
00001 //////////////////////////////////////////////////////////////////////////////////////////////////
00002 /*! \file Observer.cpp
00003  *  \brief The observer class provides the interface between the individual estimators and the
00004  *  rest of the Whorl software
00005  *  \author     $Author: jayhawk_hokie $
00006  *  \version $Revision: 1.1 $
00007  *  \date    $Date: 2007/03/27 19:15:05 $
00008 *//////////////////////////////////////////////////////////////////////////////////////////////////
00009 /*
00010 */
00011 //////////////////////////////////////////////////////////////////////////////////////////////////
00012 
00013 
00014 #include"Observer.h"
00015 
00016 /*! \brief Correction of the accelerometer reading, so that it reads acceleration
00017  *         due to gravity only. In order to do this we calculate the acceleration
00018  *         due to rotation from the rate gyros adn subtract from the acceleration
00019  *         measurement.
00020  * 
00021  *   \f[\ddot{ \vec{r}}_{\text{Gravity Vector}}  = \ddot{\vec{r}}_{\text{Measurement}} - \ddot{\vec{r}}_{\text{offset}} \f]
00022  *
00023  *   \f[\ddot{\vec{r}}_{\text{offset}} = \dot{\vec{\omega}}^x \vec{r} + 2 \vec{\omega}^x \vec{\omega}^x \vec{r} \f]
00024  *
00025  *  @param _angularAcceleration is the angular acceleration in the body frame of the
00026  *         simulator with units of rad/s^2.
00027  *  @param _angularRates is the angular rates in the body frame of the simulator
00028  *         with units of rad/s.
00029  *  @param _accelerationMeasurement is the measurement of the acceleration from
00030  *         the MotionPak before being corrected due to offset with units of m/s^2.
00031  *  @param _vectorCM is the position of the sensor with respect to teh center of 
00032  *         rotation of the air-bearing platform with units of m.
00033  */
00034 Vector Observer::AccelerationCorrection( Vector _angularAcceleration, Vector _angularRates, 
00035         Vector _accelerationMeasurement, Vector _vectorCM )
00036 {
00037         Vector correctedAcceleration(3);
00038         Vector offsetAcceleration(3);
00039 
00040         offsetAcceleration = skew( _angularAcceleration )*_vectorCM 
00041                 + 2*skew( _angularRates )*skew( _angularRates )*_vectorCM; // [m/s^2]
00042 
00043         correctedAcceleration = _accelerationMeasurement - offsetAcceleration; // [m/s^2]
00044 
00045         return( correctedAcceleration );
00046 }
00047 
00048 
00049 // Do not change the comments below - they will be added automatically by CVS
00050 /*****************************************************************************
00051 *       $Log: Observer.cpp,v $
00052 *       Revision 1.1  2007/03/27 19:15:05  jayhawk_hokie
00053 *       Initial Submission.
00054 *
00055 *       Revision 1.8  2005/03/16 18:08:15  cakinli
00056 *
00057 ******************************************************************************/

Generated on Wed Sep 5 12:54:23 2007 for DSACSS Operational Code by  doxygen 1.3.9.1