Go to the documentation of this file.00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
00022
00023
00024
00025
00026
00027
00028
00029 #include <hector_pose_estimation/measurements/gravity.h>
00030 #include <hector_pose_estimation/pose_estimation.h>
00031
00032 namespace hector_pose_estimation {
00033
00034 GravityModel::GravityModel()
00035 : MeasurementModel(MeasurementDimension)
00036 , gravity_(0.0)
00037 {
00038 parameters().add("stddev", stddev_, 10.0);
00039 }
00040
00041 bool GravityModel::init()
00042 {
00043 NoiseCovariance noise = 0.0;
00044 noise(1,1) = noise(2,2) = noise(3,3) = pow(stddev_, 2);
00045 this->AdditiveNoiseSigmaSet(noise);
00046 return true;
00047 }
00048
00049 GravityModel::~GravityModel() {}
00050
00051 bool GravityModel::applyStatusMask(const SystemStatus &status) const {
00052 if (status & STATE_ROLLPITCH) return false;
00053 return true;
00054 }
00055
00056 SystemStatus GravityModel::getStatusFlags() const {
00057 return STATE_ROLLPITCH;
00058 }
00059
00060 ColumnVector GravityModel::ExpectedValueGet() const {
00061 const double qw = x_(QUATERNION_W);
00062 const double qx = x_(QUATERNION_X);
00063 const double qy = x_(QUATERNION_Y);
00064 const double qz = x_(QUATERNION_Z);
00065
00066
00067 this->y_(1) = -gravity_ * (2*qx*qz - 2*qw*qy);
00068 this->y_(2) = -gravity_ * (2*qw*qx + 2*qy*qz);
00069 this->y_(3) = -gravity_ * (qw*qw - qx*qx - qy*qy + qz*qz);
00070
00071 return y_;
00072 }
00073
00074 Matrix GravityModel::dfGet(unsigned int i) const {
00075 if (i != 0) return Matrix();
00076
00077 const double qw = x_(QUATERNION_W);
00078 const double qx = x_(QUATERNION_X);
00079 const double qy = x_(QUATERNION_Y);
00080 const double qz = x_(QUATERNION_Z);
00081
00082 C_(1,QUATERNION_W) = gravity_*2*qy;
00083 C_(1,QUATERNION_X) = -gravity_*2*qz;
00084 C_(1,QUATERNION_Y) = gravity_*2*qw;
00085 C_(1,QUATERNION_Z) = -gravity_*2*qx;
00086 C_(2,QUATERNION_W) = -gravity_*2*qx;
00087 C_(2,QUATERNION_X) = -gravity_*2*qw;
00088 C_(2,QUATERNION_Y) = -gravity_*2*qz;
00089 C_(2,QUATERNION_Z) = -gravity_*2*qy;
00090 C_(3,QUATERNION_W) = -gravity_*2*qw;
00091 C_(3,QUATERNION_X) = gravity_*2*qx;
00092 C_(3,QUATERNION_Y) = gravity_*2*qy;
00093 C_(3,QUATERNION_Z) = -gravity_*2*qz;
00094
00095 return C_;
00096 }
00097
00098 bool Gravity::beforeUpdate(PoseEstimation &estimator, const Update &update) {
00099 model_->setGravity(estimator.getSystemModel()->getGravity());
00100 return true;
00101 }
00102
00103 }