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 #include <hector_pose_estimation/filter/set_filter.h>
00032
00033 namespace hector_pose_estimation {
00034
00035 template class Measurement_<GravityModel>;
00036
00037 GravityModel::GravityModel()
00038 : gravity_(0.0)
00039 {
00040 parameters().add("stddev", stddev_, 10.0);
00041 }
00042
00043 GravityModel::~GravityModel() {}
00044
00045 bool GravityModel::init(PoseEstimation &estimator, State &state) {
00046 setGravity(estimator.parameters().getAs<double>("gravity_magnitude"));
00047 return true;
00048 }
00049
00050 void GravityModel::getMeasurementNoise(NoiseVariance& R, const State&, bool init)
00051 {
00052 if (init) {
00053 R(0,0) = R(1,1) = R(2,2) = pow(stddev_, 2);
00054 }
00055 }
00056
00057 void GravityModel::getExpectedValue(MeasurementVector& y_pred, const State& state)
00058 {
00059 State::ConstOrientationType q(state.getOrientation());
00060
00061
00062 y_pred(0) = -gravity_.z() * (2*q.x()*q.z() - 2*q.w()*q.y());
00063 y_pred(1) = -gravity_.z() * (2*q.w()*q.x() + 2*q.y()*q.z());
00064 y_pred(2) = -gravity_.z() * (q.w()*q.w() - q.x()*q.x() - q.y()*q.y() + q.z()*q.z());
00065 }
00066
00067 void GravityModel::getStateJacobian(MeasurementMatrix& C, const State& state, bool)
00068 {
00069 State::ConstOrientationType q(state.getOrientation());
00070
00071 if (state.getOrientationIndex() >= 0) {
00072 C(0,State::QUATERNION_W) = gravity_.z()*2*q.y();
00073 C(0,State::QUATERNION_X) = -gravity_.z()*2*q.z();
00074 C(0,State::QUATERNION_Y) = gravity_.z()*2*q.w();
00075 C(0,State::QUATERNION_Z) = -gravity_.z()*2*q.x();
00076 C(1,State::QUATERNION_W) = -gravity_.z()*2*q.x();
00077 C(1,State::QUATERNION_X) = -gravity_.z()*2*q.w();
00078 C(1,State::QUATERNION_Y) = -gravity_.z()*2*q.z();
00079 C(1,State::QUATERNION_Z) = -gravity_.z()*2*q.y();
00080 C(2,State::QUATERNION_W) = -gravity_.z()*2*q.w();
00081 C(2,State::QUATERNION_X) = gravity_.z()*2*q.x();
00082 C(2,State::QUATERNION_Y) = gravity_.z()*2*q.y();
00083 C(2,State::QUATERNION_Z) = -gravity_.z()*2*q.z();
00084 }
00085 }
00086
00087 }