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/system_model.h>
00030
00031 namespace hector_pose_estimation {
00032
00033 SystemModel::SystemModel()
00034 : BFL::AnalyticConditionalGaussianAdditiveNoise(StateDimension, 2)
00035 , BFL::AnalyticSystemModelGaussianUncertainty(this)
00036 , dt_(0.0)
00037 , x_(static_cast<const StateVector&>(ConditionalArgumentGet(0)))
00038 , u_(static_cast<const ColumnVector&>(ConditionalArgumentGet(1)))
00039 , x_pred_(StateDimension)
00040 , A_(StateDimension,StateDimension)
00041 , measurement_status_(0)
00042 {
00043 A_ = 0.0;
00044 for(unsigned int i = 1; i <= StateDimension; i++) A_(i,i) = 1.0;
00045 AdditiveNoiseMuSet(StateVector(0.0));
00046 AdditiveNoiseSigmaSet(SymmetricMatrix_<StateDimension>(0.0));
00047 }
00048
00049 SystemModel::~SystemModel()
00050 {
00051 }
00052
00053 void SystemModel::getPrior(BFL::Gaussian &prior) const {
00054 StateVector mu = 0;
00055 mu(QUATERNION_W) = 1.0;
00056 StateCovariance cov = 0;
00057 cov(QUATERNION_W,QUATERNION_W) = 0.25 * 1.0;
00058 cov(QUATERNION_X,QUATERNION_X) = 0.25 * 1.0;
00059 cov(QUATERNION_Y,QUATERNION_Y) = 0.25 * 1.0;
00060 cov(QUATERNION_Z,QUATERNION_Z) = 0.25 * 1.0;
00061 #ifdef USE_RATE_SYSTEM_MODEL
00062 cov(RATE_X,RATE_X) = pow(0.0 * M_PI/180.0, 2);
00063 cov(RATE_Y,RATE_Y) = pow(0.0 * M_PI/180.0, 2);
00064 cov(RATE_Z,RATE_Z) = pow(0.0 * M_PI/180.0, 2);
00065 #endif // USE_RATE_SYSTEM_MODEL
00066 cov(POSITION_X,POSITION_X) = 0.0;
00067 cov(POSITION_Y,POSITION_Y) = 0.0;
00068 cov(POSITION_Z,POSITION_Z) = 0.0;
00069 cov(VELOCITY_X,VELOCITY_X) = 0.0;
00070 cov(VELOCITY_Y,VELOCITY_Y) = 0.0;
00071 cov(VELOCITY_Z,VELOCITY_Z) = 0.0;
00072 cov(BIAS_ACCEL_X,BIAS_ACCEL_X) = 0.0;
00073 cov(BIAS_ACCEL_Y,BIAS_ACCEL_Y) = 0.0;
00074 cov(BIAS_ACCEL_Z,BIAS_ACCEL_Z) = 0.0;
00075 cov(BIAS_GYRO_X,BIAS_GYRO_X) = pow(5.0 * M_PI/180.0, 2);
00076 cov(BIAS_GYRO_Y,BIAS_GYRO_Y) = pow(5.0 * M_PI/180.0, 2);
00077 cov(BIAS_GYRO_Z,BIAS_GYRO_Z) = pow(5.0 * M_PI/180.0, 2);
00078
00079 prior.ExpectedValueSet(mu);
00080 prior.CovarianceSet(cov);
00081 }
00082
00083 SymmetricMatrix SystemModel::CovarianceGet(double dt) const
00084 {
00085
00086 return this->AdditiveNoiseSigmaGet() * dt;
00087 }
00088
00089 }