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

QuaternionEKFObserver Class Reference

#include <QuaternionEKFObserver.h>

Inheritance diagram for QuaternionEKFObserver:

Inheritance graph
[legend]
Collaboration diagram for QuaternionEKFObserver:

Collaboration graph
[legend]
List of all members.

Public Member Functions

 QuaternionEKFObserver ()
 Default Constructor.
 QuaternionEKFObserver (TiXmlHandle handle, Whorl *ptr_whorl)
 Create a Quaternion Extended Kalman Filter Observer from XML handle and whorl object.
 ~QuaternionEKFObserver ()
 Default Deconstructor.
int Initialize ()
 Initialize the observer.
int Deinitialize ()
 Deinitialize the observer.
int Run ()
void StartEKF ()
 Start the Quaternion Extended Kalman Filter routine in a separate thread.
void StopEKF ()
 Stop the Quaternion Extended Kalman Filter routine. This function does not need to be called a t the end of the program. The thread will be closed automatically when the program ends.
void InitializeFilter ()
void MurrellAlgorithm ()
Vector SingleMeasurement (Vector _inertial, Vector _measurement, Vector _dXhatMinus, Matrix _estimatedAttitude, Matrix _sensorNoise)
void MeasurementUpdate ()
void StateCovarianceUpdate ()
void StateCovarianceUpdate (Vector dXhatPlus)
void StateTransitionMatrix ()
void Parse (TiXmlHandle handle)
 Parse inertial measurement vectors from configuration file.
void GetCurrentMeasurements ()
 Get Current Measurements from Accelerometers, Angular Rates Gyros, and Magnetometer. The values are stored in member variables.
void GetInitialGuessTriad ()
void CheckMagnetometerMeasurement ()
Vector AccelerationCorrection (Vector _angularAcceleration, Vector _angularRates, Vector _accelerationMeasurement, Vector _vectorCM)
 Correction of the accelerometer reading, so that it reads acceleration due to gravity only. In order to do this we calculate the acceleration due to rotation from the rate gyros adn subtract from the acceleration measurement.

Private Attributes

Whorl * m_whorl
 Member variable of Whorl type.
Vector m_magnetometerInertial
Vector m_accelerometerInertial
Vector m_measurementsBody
Vector m_angularRates
Vector m_rsensor
ssfTime m_currentMeasurementTime
ssfTime m_previousMeasurementTime
ssfTime m_stepTime
 Time step inside EKF.
ssfTime m_durationTime
 Duration Time inside EKF.
Vector m_BM
Matrix m_R
double m_sigmav
double m_sigmau
double m_P0a
double m_P0b
Vector m_outputState
 state vector comprised of quaternion and angular rates
Matrix m_K
 Kalman Gain.
Vector m_qminus
Vector m_qplus
 attitude estimate
Matrix m_H
Vector m_h
Vector m_angularRatehat
Vector m_previousMeasurement
Matrix m_Pminus
 State Estimate Error Covariance Matrix.
Matrix m_Pplus
 Measurement Update Estimate Covariance Matrix.
Matrix m_phi
int m_magnetometerCondition
int m_initialRange
int m_finalRange
pthread_t m_EKFThreadID
 Member pthread for splitting off EKF routine.
pthread_mutex_t m_Mutex
 Memeber mutex lock variable.
int m_exitCondition

Friends

void * QuaternionEKF (void *arg)
 Friend function split in a new thread that runs the Quaternion Extended Kalman Filter routine.
void * QuaternionEKFOriginal (void *arg)

Constructor & Destructor Documentation

QuaternionEKFObserver::QuaternionEKFObserver (   ) 
 

Default Constructor.

Definition at line 16 of file QuaternionEKFObserver.cpp.

QuaternionEKFObserver::QuaternionEKFObserver (  TiXmlHandle  handle,
Whorl *  ptr_whorl
) 
 

Create a Quaternion Extended Kalman Filter Observer from XML handle and whorl object.

Parameters:
handle XML handle with current whorl as starting node
ptr_whorl Pointer to a whorl object

Definition at line 19 of file QuaternionEKFObserver.cpp.

References Initialize(), CAMdoubleMatrix::initialize(), CAMdoubleVector::initialize(), m_accelerometerInertial, m_angularRatehat, m_angularRates, m_BM, m_currentMeasurementTime, m_durationTime, m_exitCondition, m_finalRange, m_h, m_H, m_initialRange, m_K, m_magnetometerCondition, m_magnetometerInertial, m_measurementsBody, m_Mutex, m_outputState, m_P0a, m_P0b, m_phi, m_Pminus, m_Pplus, m_previousMeasurement, m_previousMeasurementTime, m_qminus, m_qplus, m_R, m_rsensor, m_sigmau, m_sigmav, m_stepTime, m_whorl, and Parse().

QuaternionEKFObserver::~QuaternionEKFObserver (   ) 
 

Default Deconstructor.

Definition at line 69 of file QuaternionEKFObserver.cpp.

References m_Mutex, and StopEKF().

Member Function Documentation

Vector Observer::AccelerationCorrection (  Vector  _angularAcceleration,
Vector  _angularRates,
Vector  _accelerationMeasurement,
Vector  _vectorCM
)  [inherited]
 

Correction of the accelerometer reading, so that it reads acceleration due to gravity only. In order to do this we calculate the acceleration due to rotation from the rate gyros adn subtract from the acceleration measurement.

\[\ddot{ \vec{r}}_{\text{Gravity Vector}} = \ddot{\vec{r}}_{\text{Measurement}} - \ddot{\vec{r}}_{\text{offset}} \]

\[\ddot{\vec{r}}_{\text{offset}} = \dot{\vec{\omega}}^x \vec{r} + 2 \vec{\omega}^x \vec{\omega}^x \vec{r} \]

Parameters:
_angularAcceleration is the angular acceleration in the body frame of the simulator with units of rad/s^2.
_angularRates is the angular rates in the body frame of the simulator with units of rad/s.
_accelerationMeasurement is the measurement of the acceleration from the MotionPak before being corrected due to offset with units of m/s^2.
_vectorCM is the position of the sensor with respect to teh center of rotation of the air-bearing platform with units of m.

Definition at line 34 of file Observer.cpp.

References O_SESSAME::skew(), and O_SESSAME::Vector.

Referenced by triadObserver::Run().

void QuaternionEKFObserver::CheckMagnetometerMeasurement (   ) 
 

Definition at line 301 of file QuaternionEKFObserver.cpp.

References _, m_finalRange, m_initialRange, m_magnetometerCondition, m_measurementsBody, m_previousMeasurement, and O_SESSAME::norm2().

Referenced by QuaternionEKF(), and QuaternionEKFOriginal().

int QuaternionEKFObserver::Deinitialize (   ) 
 

Deinitialize the observer.

void QuaternionEKFObserver::GetCurrentMeasurements (   ) 
 

Get Current Measurements from Accelerometers, Angular Rates Gyros, and Magnetometer. The values are stored in member variables.

Definition at line 266 of file QuaternionEKFObserver.cpp.

References _, Whorl::GetAccelerometer(), Whorl::GetMagnetometer(), Whorl::GetRateGyro(), m_currentMeasurementTime, and m_whorl.

Referenced by QuaternionEKF(), and QuaternionEKFOriginal().

void QuaternionEKFObserver::GetInitialGuessTriad (   ) 
 

Definition at line 321 of file QuaternionEKFObserver.cpp.

References _, triadObserver::GetAttitude(), O_SESSAME::DirectionCosineMatrix::GetQuaternion(), m_accelerometerInertial, m_magnetometerInertial, m_measurementsBody, m_qminus, triadObserver::SetAccelerometerBodyVector(), triadObserver::SetAccelerometerInertialVector(), triadObserver::SetMagnetometerBodyVector(), and triadObserver::SetMagnetometerInertialVector().

Referenced by QuaternionEKF(), and QuaternionEKFOriginal().

int QuaternionEKFObserver::Initialize (   )  [virtual]
 

Initialize the observer.

Implements Observer.

Definition at line 82 of file QuaternionEKFObserver.cpp.

References StartEKF().

Referenced by QuaternionEKFObserver().

void QuaternionEKFObserver::InitializeFilter (   ) 
 

Definition at line 252 of file QuaternionEKFObserver.cpp.

Referenced by QuaternionEKF(), and QuaternionEKFOriginal().

void QuaternionEKFObserver::MeasurementUpdate (   ) 
 

Definition at line 663 of file QuaternionEKFObserver.cpp.

References _, CAMmatrixBase::inverse(), m_accelerometerInertial, m_finalRange, m_h, m_initialRange, m_magnetometerInertial, m_qminus, O_SESSAME::Matrix, and O_SESSAME::skew().

Referenced by QuaternionEKFOriginal().

void QuaternionEKFObserver::MurrellAlgorithm (   ) 
 

Definition at line 336 of file QuaternionEKFObserver.cpp.

References _, m_accelerometerInertial, m_magnetometerInertial, m_measurementsBody, m_qminus, m_R, O_SESSAME::Matrix, SingleMeasurement(), StateCovarianceUpdate(), and O_SESSAME::Vector.

Referenced by QuaternionEKF().

void QuaternionEKFObserver::Parse (  TiXmlHandle  handle  ) 
 

Parse inertial measurement vectors from configuration file.

Parameters:
handle XML handle with current whorl as starting node

Definition at line 97 of file QuaternionEKFObserver.cpp.

References checkResponse(), TiXmlHandle::Child(), TiXmlHandle::Element(), TiXmlHandle::FirstChild(), Whorl::GetSimulatorName(), m_accelerometerInertial, m_magnetometerInertial, m_P0a, m_P0b, m_rsensor, m_sigmau, m_sigmav, m_whorl, and O_SESSAME::normalize().

Referenced by QuaternionEKFObserver().

int QuaternionEKFObserver::Run (   )  [virtual]
 

Run is the funtion that implements the estimator in the derived class Filter.

Implements Observer.

Definition at line 516 of file QuaternionEKFObserver.cpp.

References m_Mutex, m_outputState, m_whorl, and Whorl::SetState().

Vector QuaternionEKFObserver::SingleMeasurement (  Vector  _inertial,
Vector  _measurement,
Vector  _dXhatMinus,
Matrix  _estimatedAttitude,
Matrix  _sensorNoise
) 
 

Definition at line 369 of file QuaternionEKFObserver.cpp.

References _, O_SESSAME::eye(), m_Pplus, O_SESSAME::Matrix, matrixInv3x3(), O_SESSAME::skew(), and O_SESSAME::Vector.

Referenced by MurrellAlgorithm().

void QuaternionEKFObserver::StartEKF (   ) 
 

Start the Quaternion Extended Kalman Filter routine in a separate thread.

Definition at line 167 of file QuaternionEKFObserver.cpp.

References m_EKFThreadID, and QuaternionEKFOriginal.

Referenced by Initialize().

void QuaternionEKFObserver::StateCovarianceUpdate (  Vector  dXhatPlus  ) 
 

Definition at line 403 of file QuaternionEKFObserver.cpp.

References _, O_SESSAME::ssfTime::GetSeconds(), CAMdoubleMatrix::identity(), m_angularRatehat, m_angularRates, m_BM, m_Mutex, m_phi, m_Pminus, m_qminus, m_qplus, m_sigmau, m_sigmav, m_stepTime, O_SESSAME::Matrix, O_SESSAME::normalize(), O_SESSAME::skew(), StateTransitionMatrix(), and O_SESSAME::Vector.

void QuaternionEKFObserver::StateCovarianceUpdate (   ) 
 

Definition at line 606 of file QuaternionEKFObserver.cpp.

References _, O_SESSAME::ssfTime::GetSeconds(), CAMdoubleMatrix::identity(), m_angularRatehat, m_angularRates, m_BM, m_finalRange, m_initialRange, m_Mutex, m_phi, m_Pminus, m_Pplus, m_qminus, m_qplus, m_sigmau, m_sigmav, m_stepTime, O_SESSAME::Matrix, O_SESSAME::normalize(), O_SESSAME::skew(), StateTransitionMatrix(), and O_SESSAME::Vector.

Referenced by MurrellAlgorithm(), and QuaternionEKFOriginal().

void QuaternionEKFObserver::StateTransitionMatrix (   ) 
 

Definition at line 458 of file QuaternionEKFObserver.cpp.

References _, O_SESSAME::ssfTime::GetSeconds(), CAMdoubleMatrix::identity(), m_angularRatehat, m_qminus, m_stepTime, O_SESSAME::Matrix, O_SESSAME::norm2(), O_SESSAME::skew(), and O_SESSAME::Vector.

Referenced by StateCovarianceUpdate().

void QuaternionEKFObserver::StopEKF (   ) 
 

Stop the Quaternion Extended Kalman Filter routine. This function does not need to be called a t the end of the program. The thread will be closed automatically when the program ends.

Definition at line 239 of file QuaternionEKFObserver.cpp.

References m_EKFThreadID, m_exitCondition, and m_Mutex.

Referenced by ~QuaternionEKFObserver().

Friends And Related Function Documentation

void* QuaternionEKF (  void *  arg  )  [friend]
 

Friend function split in a new thread that runs the Quaternion Extended Kalman Filter routine.

Parameters:
arg the argument is set to be a "this" pointer cast as a void pointer

Definition at line 173 of file QuaternionEKFObserver.cpp.

void* QuaternionEKFOriginal (  void *  arg  )  [friend]
 

Definition at line 534 of file QuaternionEKFObserver.cpp.

Referenced by StartEKF().

Member Data Documentation

Vector QuaternionEKFObserver::m_accelerometerInertial [private]
 

Definition at line 139 of file QuaternionEKFObserver.h.

Referenced by GetInitialGuessTriad(), MeasurementUpdate(), MurrellAlgorithm(), Parse(), and QuaternionEKFObserver().

Vector QuaternionEKFObserver::m_angularRatehat [private]
 

Definition at line 181 of file QuaternionEKFObserver.h.

Referenced by QuaternionEKFObserver(), StateCovarianceUpdate(), and StateTransitionMatrix().

Vector QuaternionEKFObserver::m_angularRates [private]
 

Definition at line 145 of file QuaternionEKFObserver.h.

Referenced by QuaternionEKFObserver(), and StateCovarianceUpdate().

Vector QuaternionEKFObserver::m_BM [private]
 

Definition at line 157 of file QuaternionEKFObserver.h.

Referenced by QuaternionEKFObserver(), and StateCovarianceUpdate().

ssfTime QuaternionEKFObserver::m_currentMeasurementTime [private]
 

Definition at line 149 of file QuaternionEKFObserver.h.

Referenced by GetCurrentMeasurements(), QuaternionEKF(), QuaternionEKFObserver(), and QuaternionEKFOriginal().

ssfTime QuaternionEKFObserver::m_durationTime [private]
 

Duration Time inside EKF.

Definition at line 155 of file QuaternionEKFObserver.h.

Referenced by QuaternionEKF(), QuaternionEKFObserver(), and QuaternionEKFOriginal().

pthread_t QuaternionEKFObserver::m_EKFThreadID [private]
 

Member pthread for splitting off EKF routine.

Definition at line 203 of file QuaternionEKFObserver.h.

Referenced by StartEKF(), and StopEKF().

int QuaternionEKFObserver::m_exitCondition [private]
 

Definition at line 208 of file QuaternionEKFObserver.h.

Referenced by QuaternionEKF(), QuaternionEKFObserver(), QuaternionEKFOriginal(), and StopEKF().

int QuaternionEKFObserver::m_finalRange [private]
 

Definition at line 196 of file QuaternionEKFObserver.h.

Referenced by CheckMagnetometerMeasurement(), MeasurementUpdate(), QuaternionEKFObserver(), and StateCovarianceUpdate().

Vector QuaternionEKFObserver::m_h [private]
 

Definition at line 179 of file QuaternionEKFObserver.h.

Referenced by MeasurementUpdate(), and QuaternionEKFObserver().

Matrix QuaternionEKFObserver::m_H [private]
 

Definition at line 177 of file QuaternionEKFObserver.h.

Referenced by QuaternionEKFObserver().

int QuaternionEKFObserver::m_initialRange [private]
 

Definition at line 195 of file QuaternionEKFObserver.h.

Referenced by CheckMagnetometerMeasurement(), MeasurementUpdate(), QuaternionEKFObserver(), and StateCovarianceUpdate().

Matrix QuaternionEKFObserver::m_K [private]
 

Kalman Gain.

Definition at line 169 of file QuaternionEKFObserver.h.

Referenced by QuaternionEKFObserver().

int QuaternionEKFObserver::m_magnetometerCondition [private]
 

Definition at line 193 of file QuaternionEKFObserver.h.

Referenced by CheckMagnetometerMeasurement(), and QuaternionEKFObserver().

Vector QuaternionEKFObserver::m_magnetometerInertial [private]
 

Definition at line 138 of file QuaternionEKFObserver.h.

Referenced by GetInitialGuessTriad(), MeasurementUpdate(), MurrellAlgorithm(), Parse(), and QuaternionEKFObserver().

Vector QuaternionEKFObserver::m_measurementsBody [private]
 

Definition at line 142 of file QuaternionEKFObserver.h.

Referenced by CheckMagnetometerMeasurement(), GetInitialGuessTriad(), MurrellAlgorithm(), QuaternionEKF(), QuaternionEKFObserver(), and QuaternionEKFOriginal().

pthread_mutex_t QuaternionEKFObserver::m_Mutex [private]
 

Memeber mutex lock variable.

Definition at line 206 of file QuaternionEKFObserver.h.

Referenced by QuaternionEKF(), QuaternionEKFObserver(), QuaternionEKFOriginal(), Run(), StateCovarianceUpdate(), StopEKF(), and ~QuaternionEKFObserver().

Vector QuaternionEKFObserver::m_outputState [private]
 

state vector comprised of quaternion and angular rates

Definition at line 166 of file QuaternionEKFObserver.h.

Referenced by QuaternionEKFObserver(), and Run().

double QuaternionEKFObserver::m_P0a [private]
 

Definition at line 162 of file QuaternionEKFObserver.h.

Referenced by Parse(), and QuaternionEKFObserver().

double QuaternionEKFObserver::m_P0b [private]
 

Definition at line 163 of file QuaternionEKFObserver.h.

Referenced by Parse(), and QuaternionEKFObserver().

Matrix QuaternionEKFObserver::m_phi [private]
 

Definition at line 191 of file QuaternionEKFObserver.h.

Referenced by QuaternionEKFObserver(), and StateCovarianceUpdate().

Matrix QuaternionEKFObserver::m_Pminus [private]
 

State Estimate Error Covariance Matrix.

Definition at line 186 of file QuaternionEKFObserver.h.

Referenced by QuaternionEKFObserver(), and StateCovarianceUpdate().

Matrix QuaternionEKFObserver::m_Pplus [private]
 

Measurement Update Estimate Covariance Matrix.

Definition at line 189 of file QuaternionEKFObserver.h.

Referenced by QuaternionEKFObserver(), SingleMeasurement(), and StateCovarianceUpdate().

Vector QuaternionEKFObserver::m_previousMeasurement [private]
 

Definition at line 183 of file QuaternionEKFObserver.h.

Referenced by CheckMagnetometerMeasurement(), QuaternionEKF(), QuaternionEKFObserver(), and QuaternionEKFOriginal().

ssfTime QuaternionEKFObserver::m_previousMeasurementTime [private]
 

Definition at line 150 of file QuaternionEKFObserver.h.

Referenced by QuaternionEKF(), QuaternionEKFObserver(), and QuaternionEKFOriginal().

Vector QuaternionEKFObserver::m_qminus [private]
 

Definition at line 172 of file QuaternionEKFObserver.h.

Referenced by GetInitialGuessTriad(), MeasurementUpdate(), MurrellAlgorithm(), QuaternionEKFObserver(), StateCovarianceUpdate(), and StateTransitionMatrix().

Vector QuaternionEKFObserver::m_qplus [private]
 

attitude estimate

Definition at line 175 of file QuaternionEKFObserver.h.

Referenced by QuaternionEKFObserver(), and StateCovarianceUpdate().

Matrix QuaternionEKFObserver::m_R [private]
 

Definition at line 159 of file QuaternionEKFObserver.h.

Referenced by MurrellAlgorithm(), and QuaternionEKFObserver().

Vector QuaternionEKFObserver::m_rsensor [private]
 

Definition at line 147 of file QuaternionEKFObserver.h.

Referenced by Parse(), and QuaternionEKFObserver().

double QuaternionEKFObserver::m_sigmau [private]
 

Definition at line 161 of file QuaternionEKFObserver.h.

Referenced by Parse(), QuaternionEKFObserver(), and StateCovarianceUpdate().

double QuaternionEKFObserver::m_sigmav [private]
 

Definition at line 160 of file QuaternionEKFObserver.h.

Referenced by Parse(), QuaternionEKFObserver(), and StateCovarianceUpdate().

ssfTime QuaternionEKFObserver::m_stepTime [private]
 

Time step inside EKF.

Definition at line 153 of file QuaternionEKFObserver.h.

Referenced by QuaternionEKF(), QuaternionEKFObserver(), QuaternionEKFOriginal(), StateCovarianceUpdate(), and StateTransitionMatrix().

Whorl* QuaternionEKFObserver::m_whorl [private]
 

Member variable of Whorl type.

Definition at line 135 of file QuaternionEKFObserver.h.

Referenced by GetCurrentMeasurements(), Parse(), QuaternionEKFObserver(), and Run().

The documentation for this class was generated from the following files:
Generated on Wed Sep 5 12:54:42 2007 for DSACSS Operational Code by  doxygen 1.3.9.1