gtsam/cpp/Simulated3D.cpp

/**
* @file   Simulated3D.cpp
* @brief  measurement functions and derivatives for simulated 3D robot
* @author Alex Cunningham
**/

#include "Simulated3D.h"

namespace gtsam {

Vector prior_3D (const Vector& x)
{
	return x;
}

Matrix Dprior_3D(const Vector& x)
{
	Matrix H = eye((int) x.size());
	return H;
}

Vector odo_3D(const Vector& x1, const Vector& x2)
{
	return x2 - x1;
}

Matrix Dodo1_3D(const Vector& x1, const Vector& x2)
{
	Matrix H = -1 * eye((int) x1.size());
	return H;
}

Matrix Dodo2_3D(const Vector& x1, const Vector& x2)
{
	Matrix H = eye((int) x1.size());
	return H;
}


Vector mea_3D(const Vector& x,  const Vector& l)
{
	return l - x;
}

Matrix Dmea1_3D(const Vector& x, const Vector& l)
{
	Matrix H = -1 * eye((int) x.size());
	return H;
}

Matrix Dmea2_3D(const Vector& x, const Vector& l)
{
	Matrix H = eye((int) x.size());
	return H;
}

} // namespace gtsam