How to get the complete state of an object (e.g. velocity), not just the measurement dimensions

[autonomobil]

Hey, what if I want to know (not only the position but also) the velocity of an object in xyz when I get the worldstate message? It should be given somewhere in the state matrix (because of linear motion model, right?). Is there a possibility to get the complete state (`G`?), which is used in the KalmanFilter?

Right now we are only getting G_small_ via const pbl::Gaussian &KalmanFilter::getGaussian() , correct? If I change return G_small_ to return G_, the wire core doesn't work anymore. Sure thing I have to change something somewhere else, maybe in the function pbl::PDF& PositionFilter::getValue() in PositionFilter.cpp ?

Is this sequence for position estimation correct? WorldModelROS::publish -> WorldModelROS::hypothesisToMsg -> WorldModelROS::objectToMsg -> Property::getValue -> MultiModelFilter::getValue -> PositionFilter::getValue -> KalmanFilter::getGaussian

If yes, where do I need to change something to be able to return G_ instead of G_small_?

[jelfring]

Indeed, in WorldModelROS::publish() the MAP hypothesis is taken and converted to a ROS message via WorldModelROS::hypothesisToMsg. This function loops over all objects and converts them to a message via WorldModelROS::objectToMsg. In there, each of the properties are converted. The line:

pbl::PDFtoMsg(prop->getValue(), prop_msg.pdf);

converts a pdf associated with a property to a message. prop->getValue() internally calls 'getValue()' on the estimator that you use. In most example, this indeed is the position filter. That function eventually calls:

double KalmanFilter::getLikelihood(const pbl::Gaussian& z) const {
    return z.getDensity(G_small_);
}

This is where G_small_ is returned. The problem with changing this function is that is also used, e.g., when computing the likelihood and returning the full pdf in that case will indeed lead to problems due to incompatible sizes.

The question is what is the alternative?

A possible solution would be to loop over all properties as is done above. Then instead of calling getValue() on each of the properties you call getEstimator() (in your case this will return the PositionFilter). If you add a pure virtual functiongetState() to the IStateEstimator, you can use that function to get the full state.

For the PositionFilter, the implementation of this virtual function can be a slightly modified version of the getValue() function: you can for example call KalmanFilter::getState() and KalmanFilter::getStateCoveriance() and then reconstruct the full G_.

You will need to implement this getState function for all estimators you are using, however, for the other ones a quick solution could be to call getValue().

[autonomobil]

Hey, I found a solution, I implemented it in my forked branch. It is essentially what you suggested, a bunch of new functions which get the complete State instead of G_small.

I also included my code for deleting old objects.