Go to the documentation of this file.00001 #include "joint_tracker/pdf/NonLinearPrismaticMeasurementPdf.h"
00002
00003 #include <ros/ros.h>
00004
00005 using namespace BFL;
00006
00007 #define PRISM_PDF_DIM 6
00008 #define PRISM_CONDITIONAL_VAR_DIM 4
00009 #define PRISM_NUM_CONDITIONAL_ARGS 2
00010
00011 NonLinearPrismaticMeasurementPdf::NonLinearPrismaticMeasurementPdf(const Gaussian& additiveNoise) :
00012 AnalyticConditionalGaussianAdditiveNoise(additiveNoise, PRISM_NUM_CONDITIONAL_ARGS),
00013 dfx(PRISM_PDF_DIM, PRISM_CONDITIONAL_VAR_DIM)
00014 {
00015
00016 }
00017
00018 NonLinearPrismaticMeasurementPdf::~NonLinearPrismaticMeasurementPdf()
00019 {
00020
00021 }
00022
00023 MatrixWrapper::ColumnVector NonLinearPrismaticMeasurementPdf::ExpectedValueGet() const
00024 {
00025 ColumnVector state = ConditionalArgumentGet(0);
00026
00027 double phi = state(1);
00028 double theta = state(2);
00029 double pv = state(3);
00030
00031 ColumnVector expected_pose(PRISM_PDF_DIM);
00032 expected_pose(1) = pv * cos(phi) * sin(theta);
00033 expected_pose(2) = pv * sin(phi) * sin(theta);
00034 expected_pose(3) = pv * cos(theta);
00035 expected_pose(4) = 0.;
00036 expected_pose(5) = 0.;
00037 expected_pose(6) = 0.;
00038
00039 return expected_pose + this->AdditiveNoiseMuGet();
00040 }
00041
00042 MatrixWrapper::Matrix NonLinearPrismaticMeasurementPdf::dfGet(unsigned int i) const
00043 {
00044 if (i == 0)
00045 {
00046 double phi = ConditionalArgumentGet(0)(1);
00047 double theta = ConditionalArgumentGet(0)(2);
00048 double pv = ConditionalArgumentGet(0)(3);
00049
00050 dfx = 0;
00051 dfx(1, 1) = -pv*sin(theta)*sin(phi);
00052 dfx(2, 1) = pv*sin(theta)*cos(phi);
00053 dfx(1, 2) = pv*cos(theta)*cos(phi);
00054 dfx(2, 2) = pv*cos(theta)*sin(phi);
00055 dfx(3, 2) = -pv*sin(theta);
00056 dfx(1, 3) = sin(theta)*cos(phi);
00057 dfx(2, 3) = sin(theta)*sin(phi);
00058 dfx(3, 3) = cos(theta);
00059 return dfx;
00060 }
00061 else
00062 {
00063 ROS_ERROR_STREAM_NAMED("NonLinearPrismaticMeasurementPdf::dfGet",
00064 "The derivative is not implemented for the " << i << "th conditional argument");
00065 exit(BFL_ERRMISUSE);
00066 }
00067 }
00068