#include <observation.h>
Public Types | |
typedef Eigen::Matrix< double, Dimension, Dimension > | CovMat |
typedef Eigen::Matrix< double, Dimension, ImuState::Dimension > | JacMat |
typedef Eigen::Matrix< double, Dimension, 1 > | Vector |
Public Member Functions | |
const Eigen::Affine3d & | body2ImuTransform () const |
Retrieve the transformation from target frame to IMU frame. | |
const CovMat & | covariance () const |
access to the observation covariance matrix | |
const JacMat & | jacobian () const |
access to the observation jacobian matrix | |
const Eigen::Affine3d & | lastWorld2Imu () const |
get back the last stored IMU pose estimate | |
Observation () | |
void | set (const Vector &v) |
set the current observation in vector form | |
void | setBody2ImuTransform (const Eigen::Quaterniond &q, const Eigen::Vector3d &t) |
Set the transformation from the target frame to the IMU. | |
void | setBody2ImuTransform (const Eigen::Affine3d &T) |
Set the transformation from the target frame to the IMU. | |
void | setCovariance (const CovMat &covariance) |
set the observation covariance matrix | |
void | setCovarianceFromStdDev (const Vector &stdDevs) |
set the diagonal of the observation covariance from std. deviations | |
virtual void | updateJacobian (const ImuState &state)=0 |
pure virtual function, that should update the observation jacobian according to the current state | |
void | updateLastStateInformation (const ImuState &state) |
remember this IMU state information to create a pseudo measurement later on | |
const Vector & | vector () const |
get the current observation in vector form | |
virtual | ~Observation () |
Static Public Attributes | |
static const size_t | Dimension = 7 |
Protected Member Functions | |
void | computeDeltaFromAbsolute (const Eigen::Affine3d ¤tObservation) |
void | updateObservationFromDelta () |
Protected Attributes | |
Eigen::Affine3d | body2ImuTransform_ |
Eigen::Affine3d | deltaTransform_ |
Eigen::Affine3d | lastBodyTransform_ |
Eigen::Affine3d | lastWorld2Imu_ |
CovMat | measCov_ |
JacMat | measJacobian_ |
Vector | observation_ |
Definition at line 48 of file observation.h.
typedef Eigen::Matrix<double, Dimension, Dimension> imu_filter::Observation::CovMat |
Definition at line 53 of file observation.h.
typedef Eigen::Matrix<double, Dimension, ImuState::Dimension> imu_filter::Observation::JacMat |
Definition at line 54 of file observation.h.
typedef Eigen::Matrix<double, Dimension, 1> imu_filter::Observation::Vector |
Definition at line 52 of file observation.h.
imu_filter::Observation::Observation | ( | ) | [inline] |
Definition at line 56 of file observation.h.
virtual imu_filter::Observation::~Observation | ( | ) | [inline, virtual] |
Definition at line 67 of file observation.h.
const Eigen::Affine3d& imu_filter::Observation::body2ImuTransform | ( | ) | const [inline] |
Retrieve the transformation from target frame to IMU frame.
Definition at line 130 of file observation.h.
void imu_filter::Observation::computeDeltaFromAbsolute | ( | const Eigen::Affine3d & | currentObservation | ) | [inline, protected] |
Definition at line 171 of file observation.h.
const CovMat& imu_filter::Observation::covariance | ( | ) | const [inline] |
access to the observation covariance matrix
Definition at line 102 of file observation.h.
const JacMat& imu_filter::Observation::jacobian | ( | ) | const [inline] |
access to the observation jacobian matrix
Definition at line 107 of file observation.h.
const Eigen::Affine3d& imu_filter::Observation::lastWorld2Imu | ( | ) | const [inline] |
get back the last stored IMU pose estimate
Definition at line 148 of file observation.h.
void imu_filter::Observation::set | ( | const Vector & | v | ) | [inline] |
set the current observation in vector form
v | the current observation |
Definition at line 75 of file observation.h.
void imu_filter::Observation::setBody2ImuTransform | ( | const Eigen::Quaterniond & | q, |
const Eigen::Vector3d & | t | ||
) | [inline] |
Set the transformation from the target frame to the IMU.
q | rotation from target frame to IMU frame |
t | translation from target frame to IMU frame |
Definition at line 115 of file observation.h.
void imu_filter::Observation::setBody2ImuTransform | ( | const Eigen::Affine3d & | T | ) | [inline] |
Set the transformation from the target frame to the IMU.
T | transformation from target frame to IMU frame |
Definition at line 125 of file observation.h.
void imu_filter::Observation::setCovariance | ( | const CovMat & | covariance | ) | [inline] |
set the observation covariance matrix
covariance | observation covariance |
Definition at line 86 of file observation.h.
void imu_filter::Observation::setCovarianceFromStdDev | ( | const Vector & | stdDevs | ) | [inline] |
set the diagonal of the observation covariance from std. deviations
stdDevs | standard deviations of the observations |
Definition at line 92 of file observation.h.
virtual void imu_filter::Observation::updateJacobian | ( | const ImuState & | state | ) | [pure virtual] |
pure virtual function, that should update the observation jacobian according to the current state
Implemented in imu_filter::VOObservation.
void imu_filter::Observation::updateLastStateInformation | ( | const ImuState & | state | ) | [inline] |
remember this IMU state information to create a pseudo measurement later on
The orientation and position of the IMU state will be stored, in order to create a relative transformation estimate later on.
state | the IMU state we want to remember |
Definition at line 139 of file observation.h.
void imu_filter::Observation::updateObservationFromDelta | ( | ) | [inline, protected] |
Definition at line 177 of file observation.h.
const Vector& imu_filter::Observation::vector | ( | ) | const [inline] |
get the current observation in vector form
Definition at line 80 of file observation.h.
Eigen::Affine3d imu_filter::Observation::body2ImuTransform_ [protected] |
Definition at line 160 of file observation.h.
Eigen::Affine3d imu_filter::Observation::deltaTransform_ [protected] |
Definition at line 168 of file observation.h.
const size_t imu_filter::Observation::Dimension = 7 [static] |
Definition at line 51 of file observation.h.
Eigen::Affine3d imu_filter::Observation::lastBodyTransform_ [protected] |
Definition at line 162 of file observation.h.
Eigen::Affine3d imu_filter::Observation::lastWorld2Imu_ [protected] |
Definition at line 165 of file observation.h.
CovMat imu_filter::Observation::measCov_ [protected] |
Definition at line 157 of file observation.h.
JacMat imu_filter::Observation::measJacobian_ [protected] |
Definition at line 158 of file observation.h.
Vector imu_filter::Observation::observation_ [protected] |
Definition at line 156 of file observation.h.