jeljaik / orocos-bfl-berdy Goto Github PK

As done in EKF/UKF Toolbox.

Summary in these slides: http://ags.cs.uni-kl.de/fileadmin/inf_ags/opt-ss12/lec10_opt.pdf

A method that returns the 3-dim euler angles equivalent to a specific quaternion is missing.

E.g. Change this line into something like:

class Matrix : protected EigenMatrix, public Matrix_Wrapper {...}

Hello,

I'm trying to get the probability of a new measurement: meas belonging to a given measurement model, given the current state of the system (the prediction of the filter).

MatrixWrapper::ColumnVector post = kalman_filter_->PostGet()->ExpectedValueGet();

[...]

proba = measurement_models_[MeasurementType::FACE]->ProbabilityGet(meas, post);

However this probability is often greater than 1, for example:

post = [10](4.66914e-310,30,0.5,0.587388,1.18765,1.07375,0,0,0,0) / meas = [3](0.57807,1.23188,1.07589) / proba: 2.32501

Am I doing something wrong?

Cheers,

Ugo

This should be the actual wrapper for quaternions. What I had written before was just the particular implementation for Boost whithin vector_BOOST.h

There's no specialization for a single Quaternion state. This might become handy for a distributed orientation filter or for more specialized orientation filters such as the multiplicative one, where a specialized class derived from AnalyticConditionalGaussianAdditiveNoise might not be enough!