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 ******************************************************************************/