search

tue-robotics / wire

BSD 2-Clause "Simplified" License
21 stars 15 forks source link

Is it possible to implement a CTRV-motion model instead of the linear motion constant velocity model? Also UKF? #10

Closed autonomobil closed 5 years ago

autonomobil commented 5 years ago

I wondered if it is possible to implement a CTRV-motion model like here instead of the linear motion constant velocity model?

If yes where should I start?

Another upgrade would be to swap the normal Kalman Filter with a UKF, but that seems quite complicated isn't it?

jelfring commented 5 years ago

Yes, this is possible.

It is probably quickest to start with the PositionFilter that is used in most examples. If you look at the implementation of this file you can see it currently has a member variable KalmanFilter* kalman_filter_. Within the Kalman implementation, the state vector, propagation model and measurement models are hardcoded. You can introduce a new filter with different models but the same interface.

One point of attention is this function:

const pbl::Gaussian& KalmanFilter::getGaussian() const {
    return G_small_;
}

This function should return the 'measured' pdf which in case of this example is a 2D position. If you change the state, make sure you construct your own G_small_ by just taking the first and second element from the state vector and the upper-left 2x2 submatrix for covariance (assuming you use the state vector from the link you provided).

A more generic way is to implement a new estimator that derives from IStateEstimator and implements the pure virtual functions. In this case you need to make sure your measurements are compatible with the filter such that likelihood computations work out fine. More precisely, getValue() should return that part of the state estimate that is measured (in case of a linear Kalman filter this would for example be the pdf with mean H x_predicted and covariance H P_predicted * transpose(H), where H is the measurement model matrix).

Whether you use a linear, extended or unscented Kalman filter does not matter, in the end you represent the state with a Gaussian distribution hence the interface to other parts of the code will not be affected.

autonomobil commented 5 years ago

Ok thank you very much