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/heading.h>
00030 #include <hector_pose_estimation/filter/set_filter.h>
00031
00032 namespace hector_pose_estimation {
00033
00034 template class Measurement_<HeadingModel>;
00035
00036 HeadingModel::HeadingModel()
00037 {
00038 parameters().add("stddev", stddev_, 10.0*M_PI/180.0);
00039 }
00040
00041 HeadingModel::~HeadingModel() {}
00042
00043 void HeadingModel::getMeasurementNoise(NoiseVariance& R, const State&, bool init)
00044 {
00045 if (init) {
00046 R(0,0) = pow(stddev_, 2);
00047 }
00048 }
00049
00050 void HeadingModel::getExpectedValue(MeasurementVector& y_pred, const State& state)
00051 {
00052 State::ConstOrientationType q(state.getOrientation());
00053 y_pred(0) = atan2(2*(q.x()*q.y() + q.w()*q.z()), q.w()*q.w() + q.x()*q.x() - q.y()*q.y() - q.z()*q.z());
00054 }
00055
00056 void HeadingModel::getStateJacobian(MeasurementMatrix& C, const State& state, bool)
00057 {
00058 State::ConstOrientationType q(state.getOrientation());
00059 const double t1 = q.w()*q.w() + q.x()*q.x() - q.y()*q.y() - q.z()*q.z();
00060 const double t2 = 2*(q.x()*q.y() + q.w()*q.z());
00061 const double t3 = 1.0 / (t1*t1 + t2*t2);
00062
00063 if (state.getOrientationIndex() >= 0) {
00064 C(0,State::QUATERNION_W) = 2.0 * t3 * (q.z() * t1 - q.w() * t2);
00065 C(0,State::QUATERNION_X) = 2.0 * t3 * (q.y() * t1 - q.x() * t2);
00066 C(0,State::QUATERNION_Y) = 2.0 * t3 * (q.x() * t1 + q.y() * t2);
00067 C(0,State::QUATERNION_Z) = 2.0 * t3 * (q.w() * t1 + q.z() * t2);
00068 }
00069 }
00070
00071 void HeadingModel::limitError(MeasurementVector &error) {
00072 error(0) = remainder(error(0), 2 * M_PI);
00073 }
00074
00075 }