Closed vesor closed 3 years ago
Hi @vesor, thanks for reaching out. Iโm out of office at the moment and Iโll have a look at your repo as soon as possible! ๐ We definitely have plan to add some โstandardโ non-linear measurement models and a way to ease the use of multiple sensors. We have plan to do quite big changes soon and release a new version of the library. Any suggestion are welcome! If you would like to see some nonlinear models or sensors just let us know and we will try to add them ๐
The code is in https://github.com/vesor/bayes-filters-lib/blob/vdev/src/BayesFilters/src/NonLinearScalarModel.cpp
void NonLinearScalarModel::propagate(const Eigen::Ref<const Eigen::MatrixXd>& cur_states, Eigen::Ref<Eigen::MatrixXd> pred_states)
{
// see example 15.1 in book Optimal State Estimation Kalman, H-infinity, and Nonlinear Approaches
pred_states = (0.5 * cur_states.array() + 25 * cur_states.array() * (1 + cur_states.array().pow(2)).inverse()).matrix();
}
And I modified the test_SIS and test_UKF to replace WhiteNoiseAcceleration with NonLinearScalarModel. I use plot.py to visualize the result. Black dot is target, light cross is UKF and dark cross is SIS.
After I make noise smaller by change
double tilde_q = 10.0f; to 1.0f
and replace utils::make_unique
Another thing is current LinearModel hardcode for measurement of size 2.
H_.resize(2, 4);
H_ << 1.0, 0.0, 0.0, 0.0,
0.0, 0.0, 1.0, 0.0;
Maybe you can change it for more general sizes?
I also tried the UKF sample from Opencv (with some modifications such as change measurement to linear). https://github.com/vesor/bayes-filters-lib/blob/vdev/test/test_UKF_opencv/main.cpp The result looks fine: But after I change the code to remove the 8cos( 1.2(n-1) ) item in double x1 = 0.5x + 25( x/(xx + 1) ) + 8cos( 1.2*(n-1) ) + q; (I want to make it the same as I used in bfl, which dosen't have this item.) The result becomes bad:
I am quite new to Bayes filters and I don't know why 8cos( 1.2(n-1) ) item matters.
Hi @vesor,
I had a look at your code and I will try to help you with the nonlinear
model from example 15.1
taken from Optimal State Estimation Kalman, H-infinity, and Nonlinear Approaches
.
8 cos (1.2 * (k -1))
First of all, if you wish to add the final term of the model equation it is pretty easy. You just need to add a member k_
to the class NonLinearScalarModel
, initialize it to 0
and then use it as follows within the method NonLinearScalarModel::propagate
Eigen::ArrayXd cos_input(1, cur_states.cols());
cos_input = 1.2 * k_;
pred_states = (0.5 * cur_states.array() + 25 * cur_states.array() * (1 + cur_states.array().pow(2)).inverse() + 8 * cos_input.cos()).matrix();
k_++;
According to the book, the process noise should be zero mean Gaussian
with assigned scalar variance.
For this reason you don't need the term 1.0/3.0 * std::pow(T_, 3.0)
, the sample time T
and the evaluation of the square root of the matrix (that you probably found in the WhiteNoiseAcceleration
class). In the end it suffices to have:
1x1
Q_
matrix initialized, in the constructor, to some variance
that you can take from the constructor
getNoiseSample
, i.e.
MatrixXd rand_vectors(getOutputSize().first, num);
for (int i = 0; i < rand_vectors.size(); i++)
*(rand_vectors.data() + i) = gauss_rnd_sample_();
return std::sqrt(Q_(0, 0)) * rand_vectors;
LinearModel
As you are probably aware of, according to your previous comments, the class LinearModel
assumes a state vector of the form x, x_dot, y, y_dot
, i.e. a 2D target with velocities and extracts the components x
and y
.
This class is mainly used for library tests
and probably will be extended in order to deal with a variable state size and shape. However, what we usually do, while developing a filter for a specific application, is to write a specific measurement
class for the problem at hand.
If you wish to test the scalar
model taken from the book you should adapt the class LinearModel
, or write a new one, in order to:
H_
matrix, i.e. to be 1x1
matrix with value 1.0
R_
matrix., i.e. to be a 1x1
matrix with value sigma_x ^ 2
(you can instead ignore the parameter sigma_y
)With these changes, you should be able to execute the test_UKF_nonlinear
properly. You will find a figure with the results at the end of this comment.
For the particle filter
implementation, your implementation in test_SIS_nonlinear
is almost ready except for the initialization part. In fact the class InitSurveillanceAreaGrid
, again used only for library tests
, assumes a 2D
state vector with velocities.
In your case, you should provide a class that initializes a 1D
state vector using, e.g., some initial state. This class inherits from ParticleSetInitialization
. There is only one method to be implemented, namely the method initialize
. In this method, you can access to the state of each particle using particles.state()
, a Eigen::VectorXd
. An example follows:
class ParticlesInitialization : public ParticleSetInitialization
{
public:
ParticlesInitialization(const double& initial_state)
{
initial_state_.resize(1);
initial_state_(0) = initial_state;
}
~ParticlesInitialization()
{ };
protected:
bool initialize(ParticleSet& particles) override
{
// ParticleSet.components contains the number of particles
for (std::size_t i = 0; i < particles.components; i++)
particles.state() = initial_state_;
return true;
}
VectorXd initial_state_;
};
After this change, you should be able to obtain good results also from test_SIS_nonlinear
.
This figure was obtained using your script plot.py
. The outcome of the UKF
is represented in red, while the outcome of the particle filter
is represented in green
.
If you need some code to start with, you can find an example, obtained from your code plus the changes that I have suggested, in this branch.
If you want to test the complete Example 15.1
, i.e. also taking into account the nonlinear measurement model y = x ^ 2 + r
, you will need to implement a class inheriting from AdditiveMeasurementModel
, i.e. a class describing a measurement model, not necessarily linear, with a Gaussian additive noise
component.
Let me know in case of further doubts :smile:
Outcomes from this issue:
LinearModel
class, see #56.InitSurveillanceAreaGrid
class, see #57.Thanks for the detailed help. I downloaded your code from impl/vesor branch, build and run. But the test_SIS_nonlinear just crash and testSIS_xxx.txt files are empty. test_UKF_nonlinear can run but the result still bad.
Do you have time to make impl/vesor branch work? I have a quick look at all your changes, but I can't find the root cause.
First, there is a small bug in your branch, std::pair<bool, MatrixXd> LinearModel::getNoiseSample(const int num) const { MatrixXd rand_vectors(2, num);
the size should be 1 instead of 2, when using scalar model.
Second, all your code changes seems to be code clean up which won't affect the final result. The only thing which matters is the 8cos( 1.2(n-1) ) item. After I added this item, I can get a good result as you have. (code with minimal change )
So bfl looks fine and the question become why the filter works bad if remove 8cos( 1.2(n-1) ) item. That may be out of bfl's scope but I appreciate if you can share your thoughts.
Thanks.
Hi @vesor, thanks for your feedback. We are substantially upgrading the library for the next release and we are including your feedback. Thanks about that ๐
So bfl looks fine and the question become why the filter works bad if remove 8cos( 1.2(n-1) ) item. That may be out of bfl's scope but I appreciate if you can share your thoughts.
It is indeed something related to the filtering problem at hand and not about the library. I don't have the book and hence cannot read the problem at this very moment, but I will discuss it with @xEnVrE and let you know our opinion ๐
If you have any suggestion for us to improve our work, let us know!
Closing, feel free to re-open.
Seems only WhiteNoiseAcceleration used in tests. I tried to add an non linear model but seems the result is very bad.
https://github.com/vesor/bayes-filters-lib/tree/vdev (You need to modify path in CMakeLists.txt in tests folder and the path in plot.py.)
Besides, any plan for non linear measurement model?
Thanks