NonLinearPrismaticMeasurementPdf.cpp
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) //derivative to the first conditional argument (x)
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 


joint_tracker
Author(s): Roberto Martín-Martín
autogenerated on Sat Jun 8 2019 18:26:46